Feature/integrate downstream 2 (#7)

* feat: Implement first downstream handling
* fix: Fix review comments
* feat: Better rendering of some messages
* client: Add some remarks
* feat: Continue impl of downstream handling
* test: Use default diag session
* feat: Begin impl of UDS services
* fix: DoIPAddress now accepts single uint16
* feat: Improve message output
* feat: Implement UDS services 0x10, 0x2E and 0x3E
* feat: Implemented remaining UDS services so that script finishes successfully
* feat: Rename UdsHelpers to UdsMock
* feat: Enhance UdsMock and UdsResponseCode with improved handling and output formatting
* fix: Check for nullptr in stream operator

Author: Magolves

CMakeLists.txt

@@ -83,6 +83,7 @@ set(SOURCES
     src/Logger.cpp
     src/MacAddress.cpp
     src/DoIPDefaultConnection.cpp
+    src/uds/UdsMock.cpp
 )
 
 

examples/ExampleDoIPServerModel.h

@@ -7,35 +7,159 @@
 #define EXAMPLEDOIPSERVERMODEL_H
 
 #include "DoIPServerModel.h"
+#include "ThreadSafeQueue.h"
+#include "uds/UdsMock.h"
+#include "uds/UdsResponseCode.h"
 
 using namespace doip;
 
 class ExampleDoIPServerModel : public DoIPServerModel {
   public:
     ExampleDoIPServerModel() {
-        onCloseConnection = [](IConnectionContext &ctx, DoIPCloseReason reason) noexcept {
+        onOpenConnection = [this](IConnectionContext &ctx) noexcept {
             (void)ctx;
+            startWorker();
+        };
+        onCloseConnection = [this](IConnectionContext &ctx, DoIPCloseReason reason) noexcept {
+            (void)ctx;
+            stopWorker();
             DOIP_LOG_WARN("Connection closed ({})", fmt::streamed(reason));
         };
 
-        onDiagnosticMessage = [](IConnectionContext &ctx, const DoIPMessage &msg) noexcept -> DoIPDiagnosticAck {
+        onDiagnosticMessage = [this](IConnectionContext &ctx, const DoIPMessage &msg) noexcept -> DoIPDiagnosticAck {
             (void)ctx;
-            DOIP_LOG_INFO("Received Diagnostic message (from ExampleDoIPServerModel)", fmt::streamed(msg));
+            m_log->info("Received Diagnostic message (from ExampleDoIPServerModel)", fmt::streamed(msg));
 
             // Example: Access payload using getPayload()
-            auto payload = msg.getDiagnosticMessagePayload();
-            if (payload.second >= 3 && payload.first[0] == 0x22 && payload.first[1] == 0xF1 && payload.first[2] == 0x90) {
-                DOIP_LOG_INFO(" - Detected Read Data by Identifier for VIN (0xF190) -> send NACK");
-                return DoIPNegativeDiagnosticAck::UnknownTargetAddress;
-            }
+            // auto payload = msg.getDiagnosticMessagePayload();
+            // if (payload.second >= 3 && payload.first[0] == 0x22 && payload.first[1] == 0xF1 && payload.first[2] == 0x90) {
+            //     m_log->info(" - Detected Read Data by Identifier for VIN (0xF190) -> send NACK");
+            //     return DoIPNegativeDiagnosticAck::UnknownTargetAddress;
+            // }
 
             return std::nullopt;
         };
 
-        onDiagnosticNotification = [](IConnectionContext &ctx, DoIPDiagnosticAck ack) noexcept {
+        onDiagnosticNotification = [this](IConnectionContext &ctx, DoIPDiagnosticAck ack) noexcept {
             (void)ctx;
-            DOIP_LOG_INFO("Diagnostic ACK/NACK sent (from ExampleDoIPServerModel)", fmt::streamed(ack));
+            m_log->info("Diagnostic ACK/NACK sent (from ExampleDoIPServerModel)", fmt::streamed(ack));
+        };
+
+        onDownstreamRequest = [this](IConnectionContext &ctx, const DoIPMessage &msg, ServerModelDownstreamResponseHandler callback) noexcept {
+            (void)ctx;
+            (void)msg;
+
+            m_log->info("Received downstream request (from ExampleDoIPServerModel)", fmt::streamed(msg));
+            m_downstreamCallback = callback;
+            if (!m_downstreamCallback) {
+                m_log->error("onDownstreamRequest: No callback function passed");
+                return DoIPDownstreamResult::Error;
+            }
+
+            // Store message in send queue
+            auto [data, size] = msg.getDiagnosticMessagePayload();
+            m_tx.push(ByteArray(data, size));
+            m_log->info("Enqueued msg");
+            return DoIPDownstreamResult::Pending;
         };
+
+        m_uds.registerDefaultServices();
+
+        m_uds.registerDiagnosticSessionControlHandler([this](uint8_t sessionType) {
+            m_loguds->info("Diagnostic Session Control requested, sessionType={:02X}", sessionType);
+            auto response = ByteArray{sessionType}; // Positive response SID = 0x50
+            response.writeU16BE(m_p2_ms);
+            response.writeU16BE(m_p2star_10ms);
+            return std::make_pair(uds::UdsResponseCode::PositiveResponse, response); // Positive response
+        });
+
+        m_uds.registerECUResetHandler([this](uint8_t resetType) {
+            m_loguds->info("ECU Reset requested, resetType={:02X}", resetType);
+            return std::make_pair(uds::UdsResponseCode::PositiveResponse, ByteArray{resetType}); // Positive response SID = 0x61
+        });
+
+        m_uds.registerReadDataByIdentifierHandler([this](uint16_t did) {
+            m_loguds->info("Read Data By Identifier requested, DID={:04X}", did);
+            if (did == 0xF190) {
+                // Return example VIN
+                ByteArray vinPayload = {'1', 'H', 'G', 'C', 'M',
+                                        '8', '2', '6', '3', '3',
+                                        'A', '0', '0', '0', '0', '1', 'Z'};
+                ByteArray response = {static_cast<uint8_t>((did >> 8) & 0xFF), static_cast<uint8_t>(did & 0xFF)};
+                response.insert(response.end(), vinPayload.begin(), vinPayload.end());
+                return std::make_pair(uds::UdsResponseCode::PositiveResponse, response); // Positive response
+            }
+            return std::make_pair(uds::UdsResponseCode::RequestOutOfRange, ByteArray{0x22}); // Positive response
+        });
+
+        m_uds.registerWriteDataByIdentifierHandler([this](uint16_t did, ByteArray value) {
+            m_loguds->info("Write Data By Identifier requested, DID={:04X}, value={}", did, fmt::streamed(value));
+            if (did == 0xF190) {
+                // Accept VIN write
+                return std::make_pair(uds::UdsResponseCode::PositiveResponse, ByteArray{static_cast<uint8_t>((did >> 8) & 0xFF), static_cast<uint8_t>(did & 0xFF)}); // Positive response
+            }
+            return std::make_pair(uds::UdsResponseCode::RequestOutOfRange, ByteArray{0x2E}); // NRC for WriteDataByIdentifier
+        });
+
+        m_uds.registerTesterPresentHandler([this](uint8_t subFunction) {
+            m_loguds->info("Tester Present requested, subFunction={:02X}", subFunction);
+            return std::make_pair(uds::UdsResponseCode::PositiveResponse, ByteArray{0x00}); // Positive response SID = 0x7E
+        });
+    }
+
+  private:
+    std::shared_ptr<spdlog::logger> m_log = Logger::get("smodel");
+    std::shared_ptr<spdlog::logger> m_loguds = Logger::get("uds");
+    ServerModelDownstreamResponseHandler m_downstreamCallback = nullptr;
+    ThreadSafeQueue<ByteArray> m_rx;
+    ThreadSafeQueue<ByteArray> m_tx;
+    uds::UdsMock m_uds;
+    std::thread m_worker;
+    bool m_running = true;
+    uint16_t m_p2_ms = 1000;
+    uint16_t m_p2star_10ms = 200;
+
+
+    void startWorker() {
+        m_worker = std::thread([this] {
+            while (m_running) {
+                downstream_thread();
+            }
+        });
+        m_log->info("Started worker thread");
+    }
+    void stopWorker() {
+        m_running = false;
+        if (m_worker.joinable())
+            m_worker.join();
+        m_log->info("Stopped worker thread");
+    }
+
+    /**
+     * @brief Thread simulating downstream communication (e. g. CAN).
+     */
+    void downstream_thread() {
+        if (m_tx.size()) {
+            // simulate send. In a real environment we could send a CAN message
+            ByteArray req;
+            m_tx.pop(req);
+            m_log->info("Simulate send {}", fmt::streamed(req));
+            // simulate some latency
+            std::this_thread::sleep_for(50ms);
+            // simulate receive
+            m_rx.push(m_uds.handleDiagnosticRequest(req));
+        }
+
+        if (m_rx.size()) {
+            ByteArray rsp;
+            m_rx.pop(rsp);
+            m_log->info("Simulate receive {}", fmt::streamed(rsp));
+            if (m_downstreamCallback) {
+                m_downstreamCallback(rsp, DoIPDownstreamResult::Handled);
+                m_downstreamCallback = nullptr;
+            }
+        }
+        std::this_thread::sleep_for(10ms);
     }
 };
 

examples/exampleDoIPClient.cpp

@@ -67,12 +67,19 @@ int main(int argc, char *argv[]) {
         exit(EXIT_FAILURE);
     }
 
+    // NEEDS REWORK! State handling required!
     client.receiveMessage();
 
     client.sendDiagnosticMessage({0x22, 0xF1, 0x90}); // Example: Read Data by Identifier (0xF190 = VIN)
-    client.receiveMessage();
+    client.receiveMessage(); // ack
+    std::this_thread::sleep_for(2s);
     client.sendDiagnosticMessage({0x22, 0xF2, 0x90}); // Example: Read Data by Identifier (0xF190 = VIN)
     client.receiveMessage();
+    std::this_thread::sleep_for(2s);
+    client.sendDiagnosticMessage({0x22, 0xF1, 0x90}); // Example: Read Data by Identifier (0xF190 = VIN)
+    client.receiveMessage(); // ack
+
+    std::this_thread::sleep_for(2s);
 
     client.closeTcpConnection();
     client.closeUdpConnection();

examples/exampleDoIPServer.cpp

@@ -14,7 +14,7 @@
 using namespace doip;
 using namespace std;
 
-static const DoIPAddress LOGICAL_ADDRESS(static_cast<uint8_t>(0x0), static_cast<uint8_t>(0x28));
+static const DoIPAddress LOGICAL_ADDRESS(0x0028);
 
 DoIPServer server;
 std::vector<std::thread> doipReceiver;

examples/exampleDoIPServerSimple.cpp

@@ -11,7 +11,7 @@
 using namespace doip;
 using namespace std;
 
-static const DoIPAddress LOGICAL_ADDRESS(static_cast<uint8_t>(0x00), static_cast<uint8_t>(0x28));
+static const DoIPAddress LOGICAL_ADDRESS(0x0028);
 static std::atomic<bool> g_shutdownRequested{false};
 
 // Forward declarations

inc/ByteArray.h

@@ -107,7 +107,7 @@ struct ByteArray : std::vector<uint8_t> {
      *
      * @param value The 16-bit value to append
      */
-    void writeU16(uint16_t value) {
+    void writeU16BE(uint16_t value) {
         emplace_back(static_cast<uint8_t>((value >> 8) & 0xFF));
         emplace_back(static_cast<uint8_t>(value & 0xFF));
     }
@@ -140,7 +140,7 @@ struct ByteArray : std::vector<uint8_t> {
      *
      * @param value The 32-bit value to append
      */
-    void writeU32(uint32_t value) {
+    void writeU32BE(uint32_t value) {
         emplace_back(static_cast<uint8_t>((value >> 24) & 0xFF));
         emplace_back(static_cast<uint8_t>((value >> 16) & 0xFF));
         emplace_back(static_cast<uint8_t>((value >> 8) & 0xFF));
@@ -171,9 +171,9 @@ struct ByteArray : std::vector<uint8_t> {
         if constexpr (sizeof(UnderlyingType) == 1) {
             emplace_back(static_cast<uint8_t>(integral_value));
         } else if constexpr (sizeof(UnderlyingType) == 2) {
-            writeU16(static_cast<uint16_t>(integral_value));
+            writeU16BE(static_cast<uint16_t>(integral_value));
         } else if constexpr (sizeof(UnderlyingType) == 4) {
-            writeU32(static_cast<uint32_t>(integral_value));
+            writeU32BE(static_cast<uint32_t>(integral_value));
         } else {
             static_assert(sizeof(UnderlyingType) <= 4, "Enum underlying type too large (max 32-bit supported)");
         }

inc/DoIPAddress.h

@@ -28,10 +28,9 @@ struct DoIPAddress {
     /**
      * @brief Constructs an DoIPAddress with specified high and low significant bytes.
      *
-     * @param hsb High significant byte (default: 0)
-     * @param lsb Low significant byte (default: 0)
+     * @param address the address
      */
-    constexpr explicit DoIPAddress(uint8_t hsb = 0, uint8_t lsb = 0) : m_bytes{{hsb, lsb}} {}
+    constexpr explicit DoIPAddress(uint16_t address = 0x0000) : m_bytes{static_cast<uint8_t>(address >> 8), static_cast<uint8_t>(address & 0xff)} {}
 
     /**
      * @brief Constructs an DoIPAddress from a byte array starting at the specified offset.

inc/DoIPClient.h

@@ -59,7 +59,7 @@ class DoIPClient {
     int _sockFd{-1}, _sockFd_udp{-1}, _sockFd_announcement{-1}, _connected{-1};
     int m_broadcast = 1;
     struct sockaddr_in _serverAddr, _clientAddr, _announcementAddr;
-    DoIPAddress m_sourceAddress = DoIPAddress(0xE0, 0x00); // HACK
+    DoIPAddress m_sourceAddress = DoIPAddress(0xE000);
 
     uint8_t VINResult[17] = {0};
     DoIPAddress m_logicalAddress = DoIPAddress::ZeroAddress;

inc/DoIPConnection.h

@@ -99,39 +99,6 @@ class DoIPConnection : public DoIPDefaultConnection {
      */
     bool hasDownstreamHandler() const override;
 
-    /**
-     * @brief Forward a diagnostic message to downstream
-     *
-     * Delegates to DoIPServerModel::onDownstreamRequest callback.
-     * The callback is responsible for sending the message via
-     * the appropriate transport (CAN, LIN, etc.).
-     *
-     * @param msg The diagnostic message to forward
-     * @return Result indicating if the request was initiated successfully
-     */
-    DoIPDownstreamResult notifyDownstreamRequest(const DoIPMessage &msg) override;
-
-    /**
-     * @brief Receive a response from downstream device
-     *
-     * Called by the application when a response arrives from downstream.
-     * This injects the response into the state machine via
-     * processEvent(DiagnosticMessageReceivedDownstream, response).
-     *
-     * @param response The diagnostic response from downstream
-     */
-    void receiveDownstreamResponse(const DoIPMessage &response) override;
-
-    /**
-     * @brief Notify application that downstream response was received
-     *
-     * Delegates to DoIPServerModel::onDownstreamResponse callback if set.
-     *
-     * @param request The original request that was sent downstream
-     * @param response The response received from downstream
-     */
-    void notifyDownstreamResponseReceived(const DoIPMessage &request, const DoIPMessage &response) override;
-
   private:
     DoIPAddress m_gatewayAddress;
 

inc/DoIPDefaultConnection.h

@@ -248,15 +248,9 @@ class DoIPDefaultConnection : public IConnectionContext {
     /**
      * @brief Receives a downstream response
      * @param response The downstream response message
+     * @param result the downstream result
      */
-    void receiveDownstreamResponse(const DoIPMessage &response) override;
-
-    /**
-     * @brief Notifies application that a downstream response was received
-     * @param request The original request
-     * @param response The response message
-     */
-    void notifyDownstreamResponseReceived(const DoIPMessage &request, const DoIPMessage &response) override;
+    void receiveDownstreamResponse(const ByteArray &response, DoIPDownstreamResult result) override;
 
     /**
      * @brief Gets the current state of the connection
@@ -327,6 +321,7 @@ class DoIPDefaultConnection : public IConnectionContext {
     ssize_t sendRoutingActivationResponse(const DoIPAddress &source_address, DoIPRoutingActivationResult response_code);
     ssize_t sendAliveCheckRequest();
     ssize_t sendDiagnosticMessageResponse(const DoIPAddress &sourceAddress, DoIPDiagnosticAck ack);
+    ssize_t sendDownstreamResponse(const DoIPAddress &sourceAddress, const ByteArray& payload);
 };
 
 } // namespace doip