Hands-On: Diagnostic Server Implementation

ara::diag::DiagnosticServer diagServer(
    ara::core::InstanceSpecifier{"DiagApp/DiagServer"});

// $10 DiagnosticSessionControl
diagServer.RegisterService(0x10, [&](auto req) {
    uint8_t session = req[1];
    if (session == 0x01 || session == 0x03) { // Default, Extended
        current_session = session;
        return ara::diag::ServiceDataContainer{0x50, session, 0x00, 0x19, 0x01, 0xF4};
    }
    return ara::diag::ServiceDataContainer{0x7F, 0x10, 0x12}; // NRC subFunctionNotSupported
});

// $19 ReadDTCByStatusMask
diagServer.RegisterService(0x19, [&](auto req) {
    if (req[1] == 0x02) { // subFunction 02
        uint8_t mask = req[2];
        auto dtcs = dtcStorage.GetDTCsByStatus(mask);
        ara::diag::ServiceDataContainer resp{0x59, 0x02, 0xFF}; // FF = DTCFormatIdentifier
        for (auto& dtc : dtcs) {
            resp.push_back((dtc.code >> 16) & 0xFF);
            resp.push_back((dtc.code >> 8) & 0xFF);
            resp.push_back(dtc.code & 0xFF);
            resp.push_back(dtc.statusByte);
        }
        return resp;
    }
    return ara::diag::ServiceDataContainer{0x7F, 0x19, 0x12};
});

{
  "doipConfig": {
    "logicalAddress": "0x0E80",
    "networkInterface": "eth0",
    "port": 13400,
    "maxPayloadSize": 65535
  }
}

# Test with Python doipclient
python3 -c "
import doipclient
client = doipclient.DoIPClient('192.168.0.10', 13400)
client.connect()
# Send $22 F190 (Read VIN)
response = client.send_doip(bytes([0x22, 0xF1, 0x90]))
print('VIN response:', response.hex())
client.disconnect()
" 

// Force fault condition for testing
dtcMonitor.ReportMonitorAction(ara::diag::MonitorAction::kFailed);

// Wait for DTC to be stored (debounce cycle)
std::this_thread::sleep_for(std::chrono::milliseconds(500));

// Verify via $19 02 response
// DTC status byte bit 0 (testFailed) should be set
// DTC status byte bit 3 (confirmedDTC) set after confirmation threshold

// Clear DTC with $14
diagServer.RegisterService(0x14, [&](auto req) {
    dtcStorage.ClearAll();
    return ara::diag::ServiceDataContainer{0x54};
});

// $27 01: Seed request (Level 1 access)
// $27 02: Key response
// $2E: WriteDID (protected by SecurityAccess)

diagServer.RegisterService(0x2E, [&](auto req) {
    if (!security_unlocked) {
        // NRC 0x33: SecurityAccessDenied
        return ara::diag::ServiceDataContainer{0x7F, 0x2E, 0x33};
    }
    uint16_t did = (req[1] << 8) | req[2];
    if (did == 0x0100) { // Calibration offset DID
        float offset;
        memcpy(&offset, &req[3], 4);
        kvs.SetValue("calibOffset", offset);
        kvs.SyncToStorage();
        return ara::diag::ServiceDataContainer{0x6E, req[1], req[2]};
    }
    return ara::diag::ServiceDataContainer{0x7F, 0x2E, 0x31};
});