From bec98a8e2c8487f1027ab3896bb934d734f6f737 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Wed, 3 Dec 2025 20:48:41 +0100 Subject: [PATCH 01/17] feat: Implement first downstream handling --- examples/ExampleDoIPServerModel.h | 38 +++++++++++++++++++++ inc/DoIPConnection.h | 33 ------------------- inc/DoIPDefaultConnection.h | 11 ++----- inc/DoIPServerModel.h | 43 +++++++++--------------- inc/IConnectionContext.h | 13 +------- src/DoIPConnection.cpp | 19 ----------- src/DoIPDefaultConnection.cpp | 55 ++++++++++++++++++++----------- 7 files changed, 92 insertions(+), 120 deletions(-) diff --git a/examples/ExampleDoIPServerModel.h b/examples/ExampleDoIPServerModel.h index e22082d..17b6f9b 100644 --- a/examples/ExampleDoIPServerModel.h +++ b/examples/ExampleDoIPServerModel.h @@ -7,6 +7,8 @@ #define EXAMPLEDOIPSERVERMODEL_H #include "DoIPServerModel.h" +#include "ThreadSafeQueue.h" +#include "uds/UdsMock.h" using namespace doip; @@ -36,7 +38,43 @@ class ExampleDoIPServerModel : public DoIPServerModel { (void)ctx; DOIP_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; + // Store message in send queue + // auto [data, size] = msg.getDiagnosticMessagePayload(); + // m_tx.push(ByteArray(data, size)); + m_downstreamCallback = callback; + return DoIPDownstreamResult::Pending; + }; } + + private: + ServerModelDownstreamResponseHandler m_downstreamCallback = nullptr; + ThreadSafeQueue m_rx; + ThreadSafeQueue m_tx; + uds::UdsMock uds; + + + void downstream_thread() { + if (m_tx.size()) { + // simulate send + ByteArray req; + m_tx.pop(req); + sleep(1); + m_rx.push(uds.handleDiagnosticRequest(req)); + } + + if (m_rx.size()) { + ByteArray rsp; + m_rx.pop(rsp); + if (m_downstreamCallback) { + m_downstreamCallback(rsp, DoIPDownstreamResult::Handled); + } + } + std::this_thread::sleep_for(10ms); + } }; #endif /* EXAMPLEDOIPSERVERMODEL_H */ diff --git a/inc/DoIPConnection.h b/inc/DoIPConnection.h index 375024e..73ce0b0 100644 --- a/inc/DoIPConnection.h +++ b/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; diff --git a/inc/DoIPDefaultConnection.h b/inc/DoIPDefaultConnection.h index f260b04..3410ae5 100644 --- a/inc/DoIPDefaultConnection.h +++ b/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 diff --git a/inc/DoIPServerModel.h b/inc/DoIPServerModel.h index 4041f69..4418ce1 100644 --- a/inc/DoIPServerModel.h +++ b/inc/DoIPServerModel.h @@ -23,6 +23,16 @@ using ServerModelCloseHandler = std::function; using ServerModelDiagnosticNotificationHandler = std::function; +/** + * @brief Callback for downstream response notification + * + * @param response the downstream response (maybe empty) + * @param result the downstream result + * + * @param ctx The connection context + */ +using ServerModelDownstreamResponseHandler = std::function; + /** * @brief Callback for downstream (subnet) request handling * @@ -37,25 +47,15 @@ using ServerModelDiagnosticNotificationHandler = std::function; - -/** - * @brief Callback for downstream response notification - * - * Optional callback invoked when a downstream response is received and - * about to be sent back to the client. This allows the application to - * inspect or log the response. - * - * @param ctx The connection context - * @param request The original request that was sent downstream - * @param response The response received from downstream - */ -using ServerModelDownstreamResponseHandler = std::function; +using ServerModelDownstreamHandler = std::function; /** * @brief DoIP Server Model - Configuration and callbacks for a DoIP server connection @@ -92,14 +92,6 @@ struct DoIPServerModel { */ ServerModelDownstreamHandler onDownstreamRequest; - /** - * @brief Optional notification when downstream response is received - * - * Called before the response is sent back to the DoIP client. - * Useful for logging, metrics, or response modification. - */ - ServerModelDownstreamResponseHandler onDownstreamResponse; - /// The logical address of this DoIP server DoIPAddress serverAddress = DoIPAddress(0x0E, 0x00); @@ -145,18 +137,13 @@ struct DefaultDoIPServerModel : public DoIPServerModel { // Default no-op }; - - // Note: onDownstreamRequest is intentionally left as nullptr // This means downstream forwarding is disabled by default onDownstreamRequest = nullptr; - onDownstreamResponse = nullptr; } - ~DefaultDoIPServerModel() { - + ~DefaultDoIPServerModel() { } - }; } // namespace doip diff --git a/inc/IConnectionContext.h b/inc/IConnectionContext.h index 61bb750..bf29f82 100644 --- a/inc/IConnectionContext.h +++ b/inc/IConnectionContext.h @@ -175,18 +175,7 @@ class IConnectionContext { * * @param response The diagnostic response from downstream */ - virtual void receiveDownstreamResponse(const DoIPMessage &response) = 0; - - /** - * @brief Notify application that downstream response was received - * - * Optional callback invoked after a downstream response is received - * and before it is sent to the client. Allows for logging or inspection. - * - * @param request The original request that was sent downstream - * @param response The response received from downstream - */ - virtual void notifyDownstreamResponseReceived(const DoIPMessage &request, const DoIPMessage &response) = 0; + virtual void receiveDownstreamResponse(const ByteArray &response, DoIPDownstreamResult result) = 0; }; } // namespace doip diff --git a/src/DoIPConnection.cpp b/src/DoIPConnection.cpp index a6e4b00..fc11a76 100644 --- a/src/DoIPConnection.cpp +++ b/src/DoIPConnection.cpp @@ -182,23 +182,4 @@ bool DoIPConnection::hasDownstreamHandler() const { return m_serverModel->hasDownstreamHandler(); } -DoIPDownstreamResult DoIPConnection::notifyDownstreamRequest(const DoIPMessage &msg) { - if (m_serverModel->onDownstreamRequest) { - return m_serverModel->onDownstreamRequest(*this, msg); - } - return DoIPDownstreamResult::Error; -} - -void DoIPConnection::receiveDownstreamResponse(const DoIPMessage &response) { - // m_stateMachine.processEvent(DoIPServerEvent::DiagnosticMessageReceivedDownstream, response); - (void)response; - DOIP_LOG_ERROR("receiveDownstreamResponse not implemented yet"); -} - -void DoIPConnection::notifyDownstreamResponseReceived(const DoIPMessage &request, const DoIPMessage &response) { - if (m_serverModel->onDownstreamResponse) { - m_serverModel->onDownstreamResponse(*this, request, response); - } -} - } // namespace doip \ No newline at end of file diff --git a/src/DoIPDefaultConnection.cpp b/src/DoIPDefaultConnection.cpp index 423e05a..465f47a 100644 --- a/src/DoIPDefaultConnection.cpp +++ b/src/DoIPDefaultConnection.cpp @@ -250,20 +250,25 @@ void DoIPDefaultConnection::handleRoutingActivated(DoIPServerEvent event, OptDoI return; } - auto ack = notifyDiagnosticMessage(*msg); + auto ack = notifyDiagnosticMessage(message); sendDiagnosticMessageResponse(sourceAddress.value(), ack); // Reset general inactivity timer restartStateTimer(); - // if (m_context.hasDownstreamHandler()) { - // startDownstreamResponseTimer(); - // auto result = m_context.notifyDownstreamRequest(*msg); - // if (result == DoIPDownstreamResult::Pending) { - // transitionTo(DoIPServerState::WaitDownstreamResponse); - // } else if (result == DoIPDownstreamResult::Error) { - // sendDiagnosticMessageResponse(sourceAddress.value(), DoIPNegativeDiagnosticAck::TargetUnreachable); - // } - // } + if (hasDownstreamHandler()) { + auto result = notifyDownstreamRequest(message); + if (result == DoIPDownstreamResult::Pending) { + // wait for downstream response + transitionTo(DoIPServerState::WaitDownstreamResponse); + } else if (result == DoIPDownstreamResult::Handled) { + // no downstream response expected -> go back to "idle" + transitionTo(DoIPServerState::RoutingActivated); + } else if (result == DoIPDownstreamResult::Error) { + // request could not be handled -> issue error and back to idle + sendDiagnosticMessageResponse(sourceAddress.value(), DoIPNegativeDiagnosticAck::TargetUnreachable); + transitionTo(DoIPServerState::RoutingActivated); + } + } } void DoIPDefaultConnection::handleWaitAliveCheckResponse(DoIPServerEvent event, OptDoIPMessage msg) { @@ -288,7 +293,6 @@ void DoIPDefaultConnection::handleWaitAliveCheckResponse(DoIPServerEvent event, default: DOIP_LOG_WARN("Received unsupported message type {} in Wait Alive Check Response state", fmt::streamed(message.getPayloadType())); sendDiagnosticMessageResponse(DoIPAddress::ZeroAddress, DoIPNegativeDiagnosticAck::TransportProtocolError); - // closeConnection(DoIPCloseReason::InvalidMessage); return; } } @@ -388,6 +392,13 @@ ssize_t DoIPDefaultConnection::sendDiagnosticMessageResponse(const DoIPAddress & return sentBytes; } +ssize_t DoIPDefaultConnection::sendDownstreamResponse(const DoIPAddress &sourceAddress, const ByteArray& payload) { + DoIPAddress targetAddress = getServerAddress(); + DoIPMessage message = message::makeDiagnosticMessage(sourceAddress, targetAddress, payload); + + return sendProtocolMessage(message); +} + DoIPDiagnosticAck DoIPDefaultConnection::notifyDiagnosticMessage(const DoIPMessage &msg) { if (m_serverModel->onDiagnosticMessage) { return m_serverModel->onDiagnosticMessage(*this, msg); @@ -411,21 +422,25 @@ bool DoIPDefaultConnection::hasDownstreamHandler() const { return m_serverModel->hasDownstreamHandler(); } + + DoIPDownstreamResult DoIPDefaultConnection::notifyDownstreamRequest(const DoIPMessage &msg) { if (m_serverModel->onDownstreamRequest) { - return m_serverModel->onDownstreamRequest(*this, msg); + auto handler = [this](const ByteArray &response, DoIPDownstreamResult result) { + this->receiveDownstreamResponse(response, result); + }; + return m_serverModel->onDownstreamRequest(*this, msg, handler); } return DoIPDownstreamResult::Error; } -void DoIPDefaultConnection::receiveDownstreamResponse(const DoIPMessage &response) { - (void)response; - // TODO: Implement state machine event processing for downstream response -} - -void DoIPDefaultConnection::notifyDownstreamResponseReceived(const DoIPMessage &request, const DoIPMessage &response) { - if (m_serverModel->onDownstreamResponse) { - m_serverModel->onDownstreamResponse(*this, request, response); +void DoIPDefaultConnection::receiveDownstreamResponse(const ByteArray &response, DoIPDownstreamResult result) { + DoIPAddress sa = getServerAddress(); + DoIPAddress ta = getClientAddress(); + if (result == DoIPDownstreamResult::Handled) { + sendProtocolMessage(message::makeDiagnosticMessage(sa, ta, response)); + } else { + sendProtocolMessage(message::makeDiagnosticNegativeResponse(sa, ta, DoIPNegativeDiagnosticAck::TargetUnreachable, {})); } } From a3ac77c31281e7697db43e211f43df1c598bdf84 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Wed, 3 Dec 2025 21:06:08 +0100 Subject: [PATCH 02/17] fix: Fix review comments --- examples/ExampleDoIPServerModel.h | 69 +++++++++++++++++++++---------- 1 file changed, 48 insertions(+), 21 deletions(-) diff --git a/examples/ExampleDoIPServerModel.h b/examples/ExampleDoIPServerModel.h index 17b6f9b..5c8c95d 100644 --- a/examples/ExampleDoIPServerModel.h +++ b/examples/ExampleDoIPServerModel.h @@ -15,8 +15,9 @@ using namespace doip; class ExampleDoIPServerModel : public DoIPServerModel { public: ExampleDoIPServerModel() { - onCloseConnection = [](IConnectionContext &ctx, DoIPCloseReason reason) noexcept { + onCloseConnection = [this](IConnectionContext &ctx, DoIPCloseReason reason) noexcept { (void)ctx; + stopWorker(); DOIP_LOG_WARN("Connection closed ({})", fmt::streamed(reason)); }; @@ -46,35 +47,61 @@ class ExampleDoIPServerModel : public DoIPServerModel { // auto [data, size] = msg.getDiagnosticMessagePayload(); // m_tx.push(ByteArray(data, size)); m_downstreamCallback = callback; + if (!m_downstreamCallback) { + DOIP_LOG_ERROR("onDownstreamRequest: No callback function passed"); + return DoIPDownstreamResult::Error; + } return DoIPDownstreamResult::Pending; }; - } - private: - ServerModelDownstreamResponseHandler m_downstreamCallback = nullptr; - ThreadSafeQueue m_rx; - ThreadSafeQueue m_tx; - uds::UdsMock uds; + startWorker(); + } + private: + ServerModelDownstreamResponseHandler m_downstreamCallback = nullptr; + ThreadSafeQueue m_rx; + ThreadSafeQueue m_tx; + uds::UdsMock m_uds; + std::thread m_worker; + bool m_running = true; - void downstream_thread() { - if (m_tx.size()) { - // simulate send - ByteArray req; - m_tx.pop(req); - sleep(1); - m_rx.push(uds.handleDiagnosticRequest(req)); + void startWorker() { + m_worker = std::thread([this] { + while (m_running) { + downstream_thread(); } + }); + } + void stopWorker() { + m_running = false; + if (m_worker.joinable()) + m_worker.join(); + DOIP_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); + sleep(1); // wait for response + // simulate receive + m_rx.push(m_uds.handleDiagnosticRequest(req)); + } - if (m_rx.size()) { - ByteArray rsp; - m_rx.pop(rsp); - if (m_downstreamCallback) { - m_downstreamCallback(rsp, DoIPDownstreamResult::Handled); - } + if (m_rx.size()) { + ByteArray rsp; + m_rx.pop(rsp); + if (m_downstreamCallback) { + m_downstreamCallback(rsp, DoIPDownstreamResult::Handled); + m_downstreamCallback = nullptr; } - std::this_thread::sleep_for(10ms); } + std::this_thread::sleep_for(10ms); + } }; #endif /* EXAMPLEDOIPSERVERMODEL_H */ From 60cf4e93a5da614fac74b5e89cb6e032f6f9a394 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Wed, 3 Dec 2025 22:16:16 +0100 Subject: [PATCH 03/17] feat: Better rendering of some messages --- inc/DoIPMessage.h | 435 +++++++++++++++++++++++++--------------------- 1 file changed, 233 insertions(+), 202 deletions(-) diff --git a/inc/DoIPMessage.h b/inc/DoIPMessage.h index a141afb..df9ecf8 100644 --- a/inc/DoIPMessage.h +++ b/inc/DoIPMessage.h @@ -1,21 +1,21 @@ #ifndef DOIPMESSAGE_IMPROVED_H #define DOIPMESSAGE_IMPROVED_H -#include -#include -#include #include +#include #include +#include +#include #include "AnsiColors.h" #include "ByteArray.h" #include "DoIPAddress.h" -#include "DoIPRoutingActivationType.h" +#include "DoIPFurtherAction.h" +#include "DoIPIdentifiers.h" #include "DoIPNegativeAck.h" #include "DoIPNegativeDiagnosticAck.h" #include "DoIPPayloadType.h" -#include "DoIPIdentifiers.h" -#include "DoIPFurtherAction.h" +#include "DoIPRoutingActivationType.h" #include "DoIPSyncStatus.h" namespace doip { @@ -64,7 +64,6 @@ constexpr size_t DOIP_DIAG_HEADER_SIZE = DOIP_HEADER_SIZE + 4; class DoIPMessage; using OptDoIPMessage = std::optional; - /** * @brief Represents a complete DoIP message with internal ByteArray representation. * @@ -82,8 +81,7 @@ using OptDoIPMessage = std::optional; */ class DoIPMessage { - -public: + public: /** * @brief Default constructor - creates invalid empty message */ @@ -221,7 +219,7 @@ class DoIPMessage { * * @return const uint8_t* Pointer to the message data */ - const uint8_t* data() const { + const uint8_t *data() const { return m_data.data(); } @@ -239,7 +237,7 @@ class DoIPMessage { * * @return const ByteArray& Reference to the complete message data */ - const ByteArray& asByteArray() const { + const ByteArray &asByteArray() const { return m_data; } @@ -259,12 +257,16 @@ class DoIPMessage { */ std::optional getSourceAddress() const { auto payloadRef = getPayload(); + // todo: Simplify if (getPayloadType() == DoIPPayloadType::DiagnosticMessage && payloadRef.second >= 2) { return DoIPAddress(payloadRef.first, 0); } if (getPayloadType() == DoIPPayloadType::RoutingActivationRequest && payloadRef.second >= 2) { return DoIPAddress(payloadRef.first, 0); } + if (getPayloadType() == DoIPPayloadType::AliveCheckResponse && payloadRef.second >= 2) { + return DoIPAddress(payloadRef.first, 0); + } return std::nullopt; } @@ -292,14 +294,14 @@ class DoIPMessage { getPayloadSize() == getPayloadLengthFromHeader(); } - /** + /** * @brief Checks if the protocol version is valid. * @param data Pointer to the byte array * @param length Length of the data * @param offset Offset to start checking from (default: 0) * @return bool True if valid */ - static bool isValidProtocolVersion(const uint8_t *data, size_t length, size_t offset = 0) { + static bool isValidProtocolVersion(const uint8_t *data, size_t length, size_t offset = 0) { if (length < 2) { return false; } @@ -320,7 +322,8 @@ class DoIPMessage { * @return std::optional The payload type if valid */ static std::optional> tryParseHeader(const uint8_t *data, size_t length) { - if (!data || length < DOIP_HEADER_SIZE) return std::nullopt; + if (!data || length < DOIP_HEADER_SIZE) + return std::nullopt; if (!isValidProtocolVersion(data, length)) { return std::nullopt; @@ -362,7 +365,7 @@ class DoIPMessage { return msg; } -protected: + protected: ByteArray m_data; ///< Complete message data (header + payload) /** @@ -427,201 +430,200 @@ class DoIPMessage { */ namespace message { - /** - * @brief Creates a vehicle identification request message. - * - * @return DoIPMessage the vehicle identification request - */ - inline DoIPMessage makeVehicleIdentificationRequest() { - return DoIPMessage(DoIPPayloadType::VehicleIdentificationRequest, {}); - } +/** + * @brief Creates a vehicle identification request message. + * + * @return DoIPMessage the vehicle identification request + */ +inline DoIPMessage makeVehicleIdentificationRequest() { + return DoIPMessage(DoIPPayloadType::VehicleIdentificationRequest, {}); +} - /** - * @brief Creates a vehicle identification response message. - * - * @param vin the vehicle identification number (VIN) - * @param logicalAddress the logical address of the entity - * @param entityType the entity identifier (EID) - * @param groupId the group identifier (GID) - * @param furtherAction the further action code - * @param syncStatus the synchronization status - * @return DoIPMessage the vehicle identification response message - */ - inline DoIPMessage makeVehicleIdentificationResponse( - const DoIPVIN& vin, - const DoIPAddress& logicalAddress, - const DoIPEID& entityType, - const DoIPGID& groupId, - DoIPFurtherAction furtherAction = DoIPFurtherAction::NoFurtherAction, - DoIPSyncStatus syncStatus = DoIPSyncStatus::GidVinSynchronized) { - - ByteArray payload; - payload.reserve(vin.size() + logicalAddress.size() + entityType.size() + groupId.size() + 2); - - payload.insert(payload.end(), vin.begin(), vin.end()); - payload.writeU16(logicalAddress.toUint16()); - payload.insert(payload.end(), entityType.begin(), entityType.end()); - payload.insert(payload.end(), groupId.begin(), groupId.end()); - payload.writeEnum(furtherAction); - payload.writeEnum(syncStatus); - - - return DoIPMessage(DoIPPayloadType::VehicleIdentificationResponse, std::move(payload)); - } +/** + * @brief Creates a vehicle identification response message. + * + * @param vin the vehicle identification number (VIN) + * @param logicalAddress the logical address of the entity + * @param entityType the entity identifier (EID) + * @param groupId the group identifier (GID) + * @param furtherAction the further action code + * @param syncStatus the synchronization status + * @return DoIPMessage the vehicle identification response message + */ +inline DoIPMessage makeVehicleIdentificationResponse( + const DoIPVIN &vin, + const DoIPAddress &logicalAddress, + const DoIPEID &entityType, + const DoIPGID &groupId, + DoIPFurtherAction furtherAction = DoIPFurtherAction::NoFurtherAction, + DoIPSyncStatus syncStatus = DoIPSyncStatus::GidVinSynchronized) { + + ByteArray payload; + payload.reserve(vin.size() + logicalAddress.size() + entityType.size() + groupId.size() + 2); + + payload.insert(payload.end(), vin.begin(), vin.end()); + payload.writeU16(logicalAddress.toUint16()); + payload.insert(payload.end(), entityType.begin(), entityType.end()); + payload.insert(payload.end(), groupId.begin(), groupId.end()); + payload.writeEnum(furtherAction); + payload.writeEnum(syncStatus); + + return DoIPMessage(DoIPPayloadType::VehicleIdentificationResponse, std::move(payload)); +} - /** - * @brief Creates a generic DoIP negative response (NACK). - * - * @param nack the negative response code - * @return DoIPMessage the DoIP message - */ - inline DoIPMessage makeNegativeAckMessage(DoIPNegativeAck nack) { - return DoIPMessage(DoIPPayloadType::NegativeAck, {static_cast(nack)}); - } +/** + * @brief Creates a generic DoIP negative response (NACK). + * + * @param nack the negative response code + * @return DoIPMessage the DoIP message + */ +inline DoIPMessage makeNegativeAckMessage(DoIPNegativeAck nack) { + return DoIPMessage(DoIPPayloadType::NegativeAck, {static_cast(nack)}); +} - /** - * @brief Creates a diagnostic message. - * - * @param sa the source address - * @param ta the target address - * @param msg_payload the original diagnostic messages (e.g. UDS message) - * @return DoIPMessage the DoIP message - */ - inline DoIPMessage makeDiagnosticMessage( - const DoIPAddress &sa, - const DoIPAddress &ta, - const ByteArray &msg_payload) { +/** + * @brief Creates a diagnostic message. + * + * @param sa the source address + * @param ta the target address + * @param msg_payload the original diagnostic messages (e.g. UDS message) + * @return DoIPMessage the DoIP message + */ +inline DoIPMessage makeDiagnosticMessage( + const DoIPAddress &sa, + const DoIPAddress &ta, + const ByteArray &msg_payload) { - ByteArray payload; - payload.reserve(sa.size() + ta.size() + msg_payload.size()); + ByteArray payload; + payload.reserve(sa.size() + ta.size() + msg_payload.size()); - sa.appendTo(payload); - ta.appendTo(payload); - payload.insert(payload.end(), msg_payload.begin(), msg_payload.end()); + sa.appendTo(payload); + ta.appendTo(payload); + payload.insert(payload.end(), msg_payload.begin(), msg_payload.end()); - return DoIPMessage(DoIPPayloadType::DiagnosticMessage, std::move(payload)); - } + return DoIPMessage(DoIPPayloadType::DiagnosticMessage, std::move(payload)); +} - /** void b - * @brief Creates a diagnostic positive ACK message. - * - * @param sa the source address - * @param ta the target address - * @param msg_payload the original diagnostic messages (e.g. UDS message) - * @return DoIPMessage the DoIP message - */ - inline DoIPMessage makeDiagnosticPositiveResponse( - const DoIPAddress &sa, - const DoIPAddress &ta, - const ByteArray &msg_payload) { +/** void b + * @brief Creates a diagnostic positive ACK message. + * + * @param sa the source address + * @param ta the target address + * @param msg_payload the original diagnostic messages (e.g. UDS message) + * @return DoIPMessage the DoIP message + */ +inline DoIPMessage makeDiagnosticPositiveResponse( + const DoIPAddress &sa, + const DoIPAddress &ta, + const ByteArray &msg_payload) { - ByteArray payload; - payload.reserve(sa.size() + ta.size() + msg_payload.size() + 1); + ByteArray payload; + payload.reserve(sa.size() + ta.size() + msg_payload.size() + 1); - sa.appendTo(payload); - ta.appendTo(payload); - payload.emplace_back(DIAGNOSTIC_MESSAGE_ACK); - payload.insert(payload.end(), msg_payload.begin(), msg_payload.end()); + sa.appendTo(payload); + ta.appendTo(payload); + payload.emplace_back(DIAGNOSTIC_MESSAGE_ACK); + payload.insert(payload.end(), msg_payload.begin(), msg_payload.end()); - return DoIPMessage(DoIPPayloadType::DiagnosticMessageAck, std::move(payload)); - } + return DoIPMessage(DoIPPayloadType::DiagnosticMessageAck, std::move(payload)); +} - /** - * @brief Creates a diagnostic negative ACK message (NACK). - * - * @param sa the source address - * @param ta the target address - * @param nack the negative acknowledgment code - * @param msg_payload the original diagnostic messages (e.g. UDS message) - * @return DoIPMessage the DoIP message - */ - inline DoIPMessage makeDiagnosticNegativeResponse( - const DoIPAddress &sa, - const DoIPAddress &ta, - DoIPNegativeDiagnosticAck nack, - const ByteArray &msg_payload) { +/** + * @brief Creates a diagnostic negative ACK message (NACK). + * + * @param sa the source address + * @param ta the target address + * @param nack the negative acknowledgment code + * @param msg_payload the original diagnostic messages (e.g. UDS message) + * @return DoIPMessage the DoIP message + */ +inline DoIPMessage makeDiagnosticNegativeResponse( + const DoIPAddress &sa, + const DoIPAddress &ta, + DoIPNegativeDiagnosticAck nack, + const ByteArray &msg_payload) { - ByteArray payload; - payload.reserve(sa.size() + ta.size() + msg_payload.size() + 1); + ByteArray payload; + payload.reserve(sa.size() + ta.size() + msg_payload.size() + 1); - sa.appendTo(payload); - ta.appendTo(payload); - payload.emplace_back(static_cast(nack)); - payload.insert(payload.end(), msg_payload.begin(), msg_payload.end()); + sa.appendTo(payload); + ta.appendTo(payload); + payload.emplace_back(static_cast(nack)); + payload.insert(payload.end(), msg_payload.begin(), msg_payload.end()); - return DoIPMessage(DoIPPayloadType::DiagnosticMessageNegativeAck, std::move(payload)); - } + return DoIPMessage(DoIPPayloadType::DiagnosticMessageNegativeAck, std::move(payload)); +} - /** - * @brief Create an 'alive check' request - * - * @return DoIPMessage - */ - inline DoIPMessage makeAliveCheckRequest() { - return DoIPMessage(DoIPPayloadType::AliveCheckRequest, {}); - } +/** + * @brief Create an 'alive check' request + * + * @return DoIPMessage + */ +inline DoIPMessage makeAliveCheckRequest() { + return DoIPMessage(DoIPPayloadType::AliveCheckRequest, {}); +} - /** - * @brief Create an 'alive check' response - * - * @param sa the source address - * @return DoIPMessage - */ - inline DoIPMessage makeAliveCheckResponse(const DoIPAddress &sa) { - ByteArray payload; - payload.writeU16(sa.toUint16()); - return DoIPMessage(DoIPPayloadType::AliveCheckResponse, std::move(payload)); - } +/** + * @brief Create an 'alive check' response + * + * @param sa the source address + * @return DoIPMessage + */ +inline DoIPMessage makeAliveCheckResponse(const DoIPAddress &sa) { + ByteArray payload; + payload.writeU16(sa.toUint16()); + return DoIPMessage(DoIPPayloadType::AliveCheckResponse, std::move(payload)); +} - /** - * @brief Creates a routing activation request message. - * - * @param ea the entity address - * @param actType the activation type - * @return DoIPMessage the activation request message - */ - inline DoIPMessage makeRoutingActivationRequest( - const DoIPAddress &ea, - DoIPRoutingActivationType actType = DoIPRoutingActivationType::Default) { - - ByteArray payload; - payload.reserve(ea.size() + 1 + 4); - payload.writeU16(ea.toUint16()); - payload.writeEnum(actType); - // Reserved 4 bytes for future use - payload.insert(payload.end(), {0, 0, 0, 0}); - - return DoIPMessage(DoIPPayloadType::RoutingActivationRequest, std::move(payload)); - } +/** + * @brief Creates a routing activation request message. + * + * @param ea the entity address + * @param actType the activation type + * @return DoIPMessage the activation request message + */ +inline DoIPMessage makeRoutingActivationRequest( + const DoIPAddress &ea, + DoIPRoutingActivationType actType = DoIPRoutingActivationType::Default) { + + ByteArray payload; + payload.reserve(ea.size() + 1 + 4); + payload.writeU16(ea.toUint16()); + payload.writeEnum(actType); + // Reserved 4 bytes for future use + payload.insert(payload.end(), {0, 0, 0, 0}); + + return DoIPMessage(DoIPPayloadType::RoutingActivationRequest, std::move(payload)); +} - /** - * @brief Creates a routing activation response message. - * - * @param routingReq the routing request message - * @param ea the entity address - * @param actType the activation type - * @return DoIPMessage the activation response message - */ - inline DoIPMessage makeRoutingActivationResponse( - const DoIPMessage &routingReq, - const DoIPAddress &ea, - DoIPRoutingActivationType actType = DoIPRoutingActivationType::Default) { +/** + * @brief Creates a routing activation response message. + * + * @param routingReq the routing request message + * @param ea the entity address + * @param actType the activation type + * @return DoIPMessage the activation response message + */ +inline DoIPMessage makeRoutingActivationResponse( + const DoIPMessage &routingReq, + const DoIPAddress &ea, + DoIPRoutingActivationType actType = DoIPRoutingActivationType::Default) { - ByteArray payload; - payload.reserve(ea.size() + 1 + 4); + ByteArray payload; + payload.reserve(ea.size() + 1 + 4); - auto sourceAddr = routingReq.getSourceAddress(); - if (sourceAddr) { - sourceAddr.value().appendTo(payload); - } + auto sourceAddr = routingReq.getSourceAddress(); + if (sourceAddr) { + sourceAddr.value().appendTo(payload); + } - ea.appendTo(payload); - payload.emplace_back(static_cast(actType)); - // Reserved 4 bytes for future use - payload.insert(payload.end(), {0, 0, 0, 0}); + ea.appendTo(payload); + payload.emplace_back(static_cast(actType)); + // Reserved 4 bytes for future use + payload.insert(payload.end(), {0, 0, 0, 0}); - return DoIPMessage(DoIPPayloadType::RoutingActivationResponse, std::move(payload)); - } + return DoIPMessage(DoIPPayloadType::RoutingActivationResponse, std::move(payload)); +} } // namespace message @@ -634,20 +636,49 @@ namespace message { * @param msg DoIPMessage to print * @return std::ostream& Reference to the output stream */ -inline std::ostream& operator<<(std::ostream& os, const DoIPMessage& msg) { - os << ansi::dim << - "V" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') - << static_cast(PROTOCOL_VERSION) << std::dec << ansi::reset - << "|" << ansi::cyan << msg.getPayloadType() << ansi::reset - << "|L" << msg.getPayloadSize() - << "| Payload: "; - - auto payload = msg.getPayload(); - os << ansi::bold_white; - for (size_t i = 0; i < payload.second; ++i) { - if (i > 0) os << '.'; - os << std::hex << std::uppercase << std::setw(2) << std::setfill('0') - << static_cast(payload.first[i]); +inline std::ostream &operator<<(std::ostream &os, const DoIPMessage &msg) { + os << ansi::dim << "V" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(PROTOCOL_VERSION) << std::dec << ansi::reset; + + + if (msg.getPayloadType() == DoIPPayloadType::DiagnosticMessageNegativeAck) { + auto payload = msg.getDiagnosticMessagePayload(); + os << ansi::red << "|Diag NACK " << static_cast(payload.first[0]); + } else if (msg.getPayloadType() == DoIPPayloadType::AliveCheckRequest) { + os << ansi::yellow << "|Alive Check?"; + } else if (msg.getPayloadType() == DoIPPayloadType::AliveCheckResponse) { + auto sa = msg.getSourceAddress(); + os << ansi::green << "|Alive Check! " << sa.value(); + } else if (msg.getPayloadType() == DoIPPayloadType::DiagnosticMessage) { + auto payload = msg.getDiagnosticMessagePayload(); + auto sa = msg.getSourceAddress(); + auto ta = msg.getTargetAddress(); + os << "|Diag "; + os << ansi::bold_magenta << sa.value(); + os << ansi::reset << " -> "; + os << ansi::bold_magenta << ta.value(); + + os << ansi::reset << ": "; + os << ansi::bold_blue; + for (size_t i = 0; i < payload.second; ++i) { + if (i > 0) + os << '.'; + os << std::hex << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(payload.first[i]); + } + } else { + os << "|" << ansi::cyan << msg.getPayloadType() << ansi::reset; + auto payload = msg.getPayload(); + os << "|L" << msg.getPayloadSize() + << "| Payload: "; + + os << ansi::bold_white; + for (size_t i = 0; i < payload.second; ++i) { + if (i > 0) + os << '.'; + os << std::hex << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(payload.first[i]); + } } os << ansi::reset; os << std::dec; From 2d7fdcc1a038ad73a4d527c6b5721f7023a5e2e3 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Wed, 3 Dec 2025 22:43:13 +0100 Subject: [PATCH 04/17] client: Add some remarks --- examples/exampleDoIPClient.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/examples/exampleDoIPClient.cpp b/examples/exampleDoIPClient.cpp index f1bd26c..6955dc3 100644 --- a/examples/exampleDoIPClient.cpp +++ b/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(); From d727f8138d5c16c6c5514591b0f2349e6df70384 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Wed, 3 Dec 2025 22:49:08 +0100 Subject: [PATCH 05/17] feat: Continue impl of downstream handling --- examples/ExampleDoIPServerModel.h | 36 ++++++++++++++++++++----------- inc/DoIPDownstreamResult.h | 17 +++++++++++++++ src/DoIPClient.cpp | 3 +++ src/DoIPConnection.cpp | 5 ++--- src/DoIPDefaultConnection.cpp | 16 ++++++++++++-- 5 files changed, 60 insertions(+), 17 deletions(-) diff --git a/examples/ExampleDoIPServerModel.h b/examples/ExampleDoIPServerModel.h index 5c8c95d..7d5f3a2 100644 --- a/examples/ExampleDoIPServerModel.h +++ b/examples/ExampleDoIPServerModel.h @@ -15,49 +15,57 @@ using namespace doip; class ExampleDoIPServerModel : public DoIPServerModel { public: ExampleDoIPServerModel() { + 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"); + 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; - // Store message in send queue - // auto [data, size] = msg.getDiagnosticMessagePayload(); - // m_tx.push(ByteArray(data, size)); + + m_log->info("Received downstream request (from ExampleDoIPServerModel)", fmt::streamed(msg)); m_downstreamCallback = callback; if (!m_downstreamCallback) { - DOIP_LOG_ERROR("onDownstreamRequest: No callback function passed"); + 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; }; - startWorker(); } private: + std::shared_ptr m_log = Logger::get("smodel"); ServerModelDownstreamResponseHandler m_downstreamCallback = nullptr; ThreadSafeQueue m_rx; ThreadSafeQueue m_tx; @@ -71,12 +79,13 @@ class ExampleDoIPServerModel : public DoIPServerModel { downstream_thread(); } }); + m_log->info("Started worker thread"); } void stopWorker() { m_running = false; if (m_worker.joinable()) m_worker.join(); - DOIP_LOG_INFO("Stopped worker thread"); + m_log->info("Stopped worker thread"); } /** @@ -87,7 +96,9 @@ class ExampleDoIPServerModel : public DoIPServerModel { // simulate send. In a real environment we could send a CAN message ByteArray req; m_tx.pop(req); - sleep(1); // wait for response + 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)); } @@ -95,6 +106,7 @@ class ExampleDoIPServerModel : public DoIPServerModel { 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; diff --git a/inc/DoIPDownstreamResult.h b/inc/DoIPDownstreamResult.h index 431c880..ec24892 100644 --- a/inc/DoIPDownstreamResult.h +++ b/inc/DoIPDownstreamResult.h @@ -1,6 +1,8 @@ #ifndef DOIPDOWNSTREAMRESULT_H #define DOIPDOWNSTREAMRESULT_H +#include "AnsiColors.h" + namespace doip { /** @@ -12,6 +14,21 @@ enum class DoIPDownstreamResult { Error ///< Failed to initiate downstream request }; +inline std::ostream &operator<<(std::ostream &os, DoIPDownstreamResult result) { + switch(result) { + case DoIPDownstreamResult::Pending: + os << ansi::yellow << "Pending" << ansi::reset; + break; + case DoIPDownstreamResult::Handled: + os << ansi::green << "Handled" << ansi::reset; + break; + case DoIPDownstreamResult::Error: + os << ansi::red << "Error" << ansi::reset; + break; + } + return os; +} + } // namespace doip #endif /* DOIPDOWNSTREAMRESULT_H */ diff --git a/src/DoIPClient.cpp b/src/DoIPClient.cpp index 84cc406..1fb5ada 100644 --- a/src/DoIPClient.cpp +++ b/src/DoIPClient.cpp @@ -107,17 +107,20 @@ void DoIPClient::reconnectServer() { ssize_t DoIPClient::sendRoutingActivationRequest() { DoIPMessage routingActReq = message::makeRoutingActivationRequest(m_sourceAddress); + DOIP_LOG_INFO("TX: {}", fmt::streamed(routingActReq)); return write(_sockFd, routingActReq.data(), routingActReq.size()); } ssize_t DoIPClient::sendDiagnosticMessage(const ByteArray &payload) { DoIPMessage msg = message::makeDiagnosticMessage(m_sourceAddress, m_logicalAddress, payload); + DOIP_LOG_INFO("TX: {}", fmt::streamed(msg)); return write(_sockFd, msg.data(), msg.size()); } ssize_t DoIPClient::sendAliveCheckResponse() { DoIPMessage msg = message::makeAliveCheckResponse(m_sourceAddress); + DOIP_LOG_INFO("TX: {}", fmt::streamed(msg)); return write(_sockFd, msg.data(), msg.size()); } diff --git a/src/DoIPConnection.cpp b/src/DoIPConnection.cpp index fc11a76..89c3cc3 100644 --- a/src/DoIPConnection.cpp +++ b/src/DoIPConnection.cpp @@ -50,7 +50,7 @@ int DoIPConnection::receiveTcpMessage() { DOIP_LOG_DEBUG("Waiting for {} bytes of payload...", payloadLength); unsigned int receivedPayloadBytes = receiveFixedNumberOfBytesFromTCP(m_receiveBuf.data(), payloadLength); if (receivedPayloadBytes < payloadLength) { - DOIP_LOG_ERROR("DoIP message completely incomplete"); + DOIP_LOG_ERROR("DoIP message incomplete"); // m_stateMachine.processEvent(DoIPServerEvent::InvalidMessage); // todo: Notify application of invalid message? closeSocket(); @@ -62,8 +62,7 @@ int DoIPConnection::receiveTcpMessage() { } DoIPMessage message(plType, m_receiveBuf.data(), payloadLength); - // todo: process message in state machine - // m_stateMachine.processMessage(message); + handleMessage2(message); return 1; } else { diff --git a/src/DoIPDefaultConnection.cpp b/src/DoIPDefaultConnection.cpp index 465f47a..f583a16 100644 --- a/src/DoIPDefaultConnection.cpp +++ b/src/DoIPDefaultConnection.cpp @@ -111,7 +111,7 @@ void DoIPDefaultConnection::transitionTo(DoIPServerState newState) { return desc.state == newState; }); if (it != STATE_DESCRIPTORS.end()) { - DOIP_LOG_INFO("Transitioning from state {} to state {}", fmt::streamed(m_state->state), fmt::streamed(newState)); + DOIP_LOG_INFO("-> Transitioning from state {} to state {}", fmt::streamed(m_state->state), fmt::streamed(newState)); m_state = &(*it); startStateTimer(m_state); if (m_state->enterStateHandler) { @@ -255,8 +255,16 @@ void DoIPDefaultConnection::handleRoutingActivated(DoIPServerEvent event, OptDoI // Reset general inactivity timer restartStateTimer(); + + // nack -> stop here + if (ack.has_value()) { + transitionTo(DoIPServerState::RoutingActivated); + return; + } + if (hasDownstreamHandler()) { auto result = notifyDownstreamRequest(message); + DOIP_LOG_DEBUG("Downstream req -> {}", fmt::streamed(result)); if (result == DoIPDownstreamResult::Pending) { // wait for downstream response transitionTo(DoIPServerState::WaitDownstreamResponse); @@ -302,6 +310,8 @@ void DoIPDefaultConnection::handleWaitDownstreamResponse(DoIPServerEvent event, (void)msg; // Unused parameter // Implementation of handling wait downstream response would go here + DOIP_LOG_CRITICAL("handleWaitDownstreamResponse NOT IMPL"); + } void DoIPDefaultConnection::handleFinalize(DoIPServerEvent event, OptDoIPMessage msg) { @@ -313,7 +323,7 @@ void DoIPDefaultConnection::handleFinalize(DoIPServerEvent event, OptDoIPMessage } void DoIPDefaultConnection::handleTimeout(ConnectionTimers timer_id) { - DOIP_LOG_WARN("SM2 Timeout '{}'", fmt::streamed(timer_id)); + DOIP_LOG_WARN("Timeout '{}'", fmt::streamed(timer_id)); switch (timer_id) { case ConnectionTimers::InitialInactivity: @@ -437,11 +447,13 @@ DoIPDownstreamResult DoIPDefaultConnection::notifyDownstreamRequest(const DoIPMe void DoIPDefaultConnection::receiveDownstreamResponse(const ByteArray &response, DoIPDownstreamResult result) { DoIPAddress sa = getServerAddress(); DoIPAddress ta = getClientAddress(); + DOIP_LOG_INFO("Downstream rsp: {} ({})", fmt::streamed(response), fmt::streamed(result)); if (result == DoIPDownstreamResult::Handled) { sendProtocolMessage(message::makeDiagnosticMessage(sa, ta, response)); } else { sendProtocolMessage(message::makeDiagnosticNegativeResponse(sa, ta, DoIPNegativeDiagnosticAck::TargetUnreachable, {})); } + transitionTo(DoIPServerState::RoutingActivated); } } // namespace doip \ No newline at end of file From 039b6946b421de92221691549507593963b068c4 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Wed, 3 Dec 2025 22:49:36 +0100 Subject: [PATCH 06/17] test: Use default diag session --- test/scripts/change-vin-and-reset.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/scripts/change-vin-and-reset.py b/test/scripts/change-vin-and-reset.py index 1997e5a..88f3180 100644 --- a/test/scripts/change-vin-and-reset.py +++ b/test/scripts/change-vin-and-reset.py @@ -13,8 +13,8 @@ conn = DoIPClientUDSConnector(doip_client) with Client(conn, request_timeout=2) as client: try: - client.change_session(DiagnosticSessionControl.Session.extendedDiagnosticSession) # integer with value of 3 - client.unlock_security_access(MyCar.debug_level) # Fictive security level. Integer coming from fictive lib, let's say its value is 5 + client.change_session(1) # integer with value of 3 + #client.unlock_security_access(MyCar.debug_level) # Fictive security level. Integer coming from fictive lib, let's say its value is 5 client.write_data_by_identifier(udsoncan.DataIdentifier.VIN, 'ABC123456789') # Standard ID for VIN is 0xF190. Codec is set in the client configuration print('Vehicle Identification Number successfully changed.') client.ecu_reset(ECUReset.ResetType.hardReset) # HardReset = 0x01 From 5b281f6112c4c02e7222b0533856f5786d359c32 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Thu, 4 Dec 2025 18:29:01 +0100 Subject: [PATCH 07/17] feat: Begin impl of UDS services --- CMakeLists.txt | 1 + inc/uds/IUdsServiceHandler.h | 22 +++++ inc/uds/LambdaUdsHandler.h | 21 +++++ inc/uds/UdsMock.h | 154 +++++++++++++++++++++-------------- src/uds/UdsHelpers.cpp | 38 +++++++++ 5 files changed, 173 insertions(+), 63 deletions(-) create mode 100644 inc/uds/IUdsServiceHandler.h create mode 100644 inc/uds/LambdaUdsHandler.h create mode 100644 src/uds/UdsHelpers.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a8cd7ec..5ce2a63 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,7 @@ set(SOURCES src/Logger.cpp src/MacAddress.cpp src/DoIPDefaultConnection.cpp + src/uds/UdsHelpers.cpp ) diff --git a/inc/uds/IUdsServiceHandler.h b/inc/uds/IUdsServiceHandler.h new file mode 100644 index 0000000..d94c273 --- /dev/null +++ b/inc/uds/IUdsServiceHandler.h @@ -0,0 +1,22 @@ +#ifndef IUDSSERVICEHANDLER_H +#define IUDSSERVICEHANDLER_H + +#include "UdsResponseCode.h" +#include "DoIPMessage.h" +#include + +namespace doip::uds { + +using UdsResponse = std::pair; + +class IUdsServiceHandler { +public: + virtual ~IUdsServiceHandler() = default; + virtual UdsResponse handle(const ByteArray &request) = 0; +}; + +using IUdsServiceHandlerPtr = std::unique_ptr; + +} // namespace doip::uds + +#endif // IUDSSERVICEHANDLER_H diff --git a/inc/uds/LambdaUdsHandler.h b/inc/uds/LambdaUdsHandler.h new file mode 100644 index 0000000..ec97740 --- /dev/null +++ b/inc/uds/LambdaUdsHandler.h @@ -0,0 +1,21 @@ +#ifndef LAMBDAUDSHANDLER_H +#define LAMBDAUDSHANDLER_H + +#include "IUdsServiceHandler.h" +#include + +namespace doip::uds { + +class LambdaUdsHandler : public IUdsServiceHandler { +public: + using Fn = std::function; + explicit LambdaUdsHandler(Fn fn) : m_fn(std::move(fn)) {} + UdsResponse handle(const ByteArray &request) override { return m_fn(request); } + +private: + Fn m_fn; +}; + +} // namespace doip::uds + +#endif // LAMBDAUDSHANDLER_H diff --git a/inc/uds/UdsMock.h b/inc/uds/UdsMock.h index 9fa75bb..26819e2 100644 --- a/inc/uds/UdsMock.h +++ b/inc/uds/UdsMock.h @@ -3,95 +3,123 @@ #include #include +#include +#include #include "DoIPMessage.h" #include "UdsResponseCode.h" +#include "IUdsServiceHandler.h" +#include "LambdaUdsHandler.h" +#include "UdsServices.h" using namespace doip; namespace doip::uds { -using UdsResponse = std::pair; -using UdsServiceHandler = std::function; - -struct UdsServiceDescriptor { - uint8_t serviceId; - size_t requestMinLength; - size_t responseMinLength; - UdsServiceHandler handler; -}; - constexpr uint8_t UDS_POSITIVE_RESPONSE_OFFSET = 0x40; -// Lookup table -/* -const std::array UdsServiceLookupTable = {{ - {0x10, 2, 2, [](const ByteArray& request) { return handleDiagnosticSessionControl(request); }}, - {0x11, 2, 2, [](const ByteArray& request) { return handleECUReset(request); }}, - {0x27, 2, 2, [](const ByteArray& request) { return handleSecurityAccess(request); }}, - {0x28, 2, 2, [](const ByteArray& request) { return handleCommunicationControl(request); }}, - {0x3E, 1, 1, [](const ByteArray& request) { return handleTesterPresent(request); }}, - {0x83, 2, 2, [](const ByteArray& request) { return handleAccessTimingParameters(request); }}, - {0x84, 2, 2, [](const ByteArray& request) { return handleSecuredDataTransmission(request); }}, - {0x85, 2, 2, [](const ByteArray& request) { return handleControlDTCSetting(request); }}, - {0x86, 2, 2, [](const ByteArray& request) { return handleResponseOnEvent(request); }}, - {0x87, 2, 2, [](const ByteArray& request) { return handleLinkControl(request); }}, - {0x22, 3, 2, [](const ByteArray& request) { return handleReadDataByIdentifier(request); }}, - {0x23, 5, 2, [](const ByteArray& request) { return handleReadMemoryByAddress(request); }}, - {0x24, 3, 2, [](const ByteArray& request) { return handleReadScalingDataByIdentifier(request); }}, - {0x2A, 3, 2, [](const ByteArray& request) { return handleReadDataByPeriodicIdentifier(request); }}, - {0x2C, 3, 2, [](const ByteArray& request) { return handleDynamicallyDefineDataIdentifier(request); }}, - {0x2E, 3, 2, [](const ByteArray& request) { return handleWriteDataByIdentifier(request); }}, - {0x3D, 5, 2, [](const ByteArray& request) { return handleWriteMemoryByAddress(request); }}, - {0x14, 2, 2, [](const ByteArray& request) { return handleClearDiagnosticInformation(request); }}, - {0x19, 2, 2, [](const ByteArray& request) { return handleReadDTCInformation(request); }}, -}}; -*/ - -//static_assert(UdsServiceLookupTable.size() == 14, "Update the lookup table if new services are added!"); - -/** - * @brief Mock UDS service class for testing purposes - * - * This class simulates UDS service behavior for unit tests. - */ class UdsMock { - public: - explicit UdsMock(const UdsServiceHandler& handler = UdsMock::defaultHandler) : m_serviceHandler(handler) {} - - /** - * @brief Simulates handling a UDS diagnostic request - * @param request The incoming UDS request payload - * @return The simulated UDS response payload - */ +public: + UdsMock() = default; + + // Register a handler owning pointer + void registerService(UdsService serviceId, IUdsServiceHandlerPtr handler) { + m_handlers[static_cast(serviceId)] = std::move(handler); + } + + // Register a lambda/function + void registerService(UdsService serviceId, std::function fn) { + m_handlers[static_cast(serviceId)] = std::make_unique(std::move(fn)); + } + + // Unregister + void unregisterService(UdsService serviceId) { + m_handlers.erase(static_cast(serviceId)); + } + + // Convenience: clear all + void clear() { m_handlers.clear(); } + + // --- Typed registration helpers (convenience wrappers) --- + // Diagnostic Session Control (0x10): handler(sessionType) + void registerDiagnosticSessionControlHandler(std::function handler); + + // ECU Reset (0x11): handler(resetType) + void registerECUResetHandler(std::function handler); + + // Read Data By Identifier (0x22): handler(did, params) + void registerReadDataByIdentifierHandler(std::function handler); + + // Tester Present (0x3E): handler(subFunction) + void registerTesterPresentHandler(std::function handler); + ByteArray handleDiagnosticRequest(const ByteArray &request) { - // Simple echo behavior for testing - auto response = m_serviceHandler(request); - return makeResponse(request, response.first, response.second); + if (request.empty()) return {}; + uint8_t sid = request[0]; + UdsResponse resp = defaultResponse(request); + auto it = m_handlers.find(sid); + if (it != m_handlers.end() && it->second) { + resp = it->second->handle(request); + } + return makeResponse(request, resp.first, resp.second); + } + + // Register default handlers for all known services. + // By default these handlers simply return ServiceNotSupported. Tests + // can register custom handlers afterwards to override behavior. + void registerDefaultServices() { + const std::array services = { + UdsService::DiagnosticSessionControl, + UdsService::ECUReset, + UdsService::SecurityAccess, + UdsService::CommunicationControl, + UdsService::TesterPresent, + UdsService::AccessTimingParameters, + UdsService::SecuredDataTransmission, + UdsService::ControlDTCSetting, + UdsService::ResponseOnEvent, + UdsService::LinkControl, + UdsService::ReadDataByIdentifier, + UdsService::ReadMemoryByAddress, + UdsService::ReadScalingDataByIdentifier, + UdsService::ReadDataByPeriodicIdentifier, + UdsService::DynamicallyDefineDataIdentifier, + UdsService::WriteDataByIdentifier, + UdsService::WriteMemoryByAddress, + UdsService::ClearDiagnosticInformation, + UdsService::ReadDTCInformation, + }; + + for (auto s : services) { + registerService(s, [](const ByteArray &req) -> UdsResponse { + (void)req; + return {UdsResponseCode::ServiceNotSupported, {}}; + }); + } } - private: - ByteArray makeResponse(const ByteArray &request, UdsResponseCode responseCode = UdsResponseCode::OK, const ByteArray &extraData = {}) { +private: + static UdsResponse defaultResponse(const ByteArray &request) { + (void)request; + return {UdsResponseCode::ServiceNotSupported, {}}; + } + + static ByteArray makeResponse(const ByteArray &request, UdsResponseCode responseCode = UdsResponseCode::OK, const ByteArray &extraData = {}) { if (responseCode != UdsResponseCode::OK) { ByteArray negativeResponse; negativeResponse.emplace_back(0x7F); // Negative response indicator - negativeResponse.emplace_back(request[0]); // Original service ID + negativeResponse.emplace_back(request.empty() ? 0x00 : request[0]); // Original service ID or 0 negativeResponse.emplace_back(static_cast(responseCode)); // NRC return negativeResponse; } ByteArray positiveResponse; - positiveResponse.emplace_back(static_cast(request[0] + UDS_POSITIVE_RESPONSE_OFFSET)); // Positive response SID + positiveResponse.emplace_back(static_cast((request.empty() ? 0x00 : request[0]) + UDS_POSITIVE_RESPONSE_OFFSET)); // Positive response SID positiveResponse.insert(positiveResponse.end(), extraData.begin(), extraData.end()); return positiveResponse; } - static UdsResponse defaultHandler(const ByteArray &request) { - (void)request; // Unused - return {UdsResponseCode::ServiceNotSupported, {}}; - } - - UdsServiceHandler m_serviceHandler; + std::unordered_map m_handlers; }; } // namespace doip::uds diff --git a/src/uds/UdsHelpers.cpp b/src/uds/UdsHelpers.cpp new file mode 100644 index 0000000..7eea2f9 --- /dev/null +++ b/src/uds/UdsHelpers.cpp @@ -0,0 +1,38 @@ +#include "uds/UdsMock.h" +#include "DoIPMessage.h" + +namespace doip::uds { + +void UdsMock::registerDiagnosticSessionControlHandler(std::function handler) { + registerService(UdsService::DiagnosticSessionControl, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { + if (req.size() < 2) return {UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, {}}; + uint8_t sessionType = req[1]; + return handler(sessionType); + }); +} + +void UdsMock::registerECUResetHandler(std::function handler) { + registerService(UdsService::ECUReset, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { + if (req.size() < 2) return {UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, {}}; + uint8_t resetType = req[1]; + return handler(resetType); + }); +} + +void UdsMock::registerReadDataByIdentifierHandler(std::function handler) { + registerService(UdsService::ReadDataByIdentifier, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { + if (req.size() < 3) return {UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, {}}; + uint16_t did = (static_cast(req[1]) << 8) | req[2]; + return handler(did); + }); +} + +void UdsMock::registerTesterPresentHandler(std::function handler) { + registerService(UdsService::TesterPresent, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { + if (req.size() < 2) return {UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, {}}; + uint8_t subFunction = req[1]; + return handler(subFunction); + }); +} + +} // namespace doip::uds From e2d5ab53d11131546a8b792a6e392119c292d172 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Thu, 4 Dec 2025 21:19:00 +0100 Subject: [PATCH 08/17] WIP: fix test --- test/uds/UdsMock_Test.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/test/uds/UdsMock_Test.cpp b/test/uds/UdsMock_Test.cpp index 756d196..50a4b25 100644 --- a/test/uds/UdsMock_Test.cpp +++ b/test/uds/UdsMock_Test.cpp @@ -24,10 +24,13 @@ TEST_SUITE("UdsMock") { } TEST_CASE("UdsMock custom handler returns positive response") { - uds::UdsMock udsMock([](const ByteArray &request) { - // Custom handler that always returns OK - return std::make_pair(uds::UdsResponseCode::OK, ByteArray{request[1]}); // Positive response - }); + uds::UdsMock udsMock; + + udsMock.registerService(uds::UdsService::DiagnosticSessionControl, + [](const ByteArray &request) { + // Custom handler that returns positive response + return std::make_pair(uds::UdsResponseCode::OK, ByteArray{request[1]}); + }); ByteArray request = {0x10, 0x01}; // Example UDS request (Diagnostic Session Control) ByteArray response = udsMock.handleDiagnosticRequest(request); From d41859adb167495063d14fe69d5e688e19d9c0c1 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Thu, 4 Dec 2025 21:46:37 +0100 Subject: [PATCH 09/17] fix: DoIPAddress now accepts single uint16 --- examples/exampleDoIPServer.cpp | 2 +- examples/exampleDoIPServerSimple.cpp | 2 +- inc/ByteArray.h | 8 ++++---- inc/DoIPAddress.h | 5 ++--- inc/DoIPClient.h | 2 +- inc/DoIPMessage.h | 8 ++++---- inc/DoIPServerModel.h | 2 +- test/DoIPDefaultConnection_Test.cpp | 7 ++++--- test/DoIPMessage_Test.cpp | 12 ++++++------ 9 files changed, 24 insertions(+), 24 deletions(-) diff --git a/examples/exampleDoIPServer.cpp b/examples/exampleDoIPServer.cpp index 48a4668..582f964 100644 --- a/examples/exampleDoIPServer.cpp +++ b/examples/exampleDoIPServer.cpp @@ -14,7 +14,7 @@ using namespace doip; using namespace std; -static const DoIPAddress LOGICAL_ADDRESS(static_cast(0x0), static_cast(0x28)); +static const DoIPAddress LOGICAL_ADDRESS(0x0028); DoIPServer server; std::vector doipReceiver; diff --git a/examples/exampleDoIPServerSimple.cpp b/examples/exampleDoIPServerSimple.cpp index 83324bb..2ea9e0a 100644 --- a/examples/exampleDoIPServerSimple.cpp +++ b/examples/exampleDoIPServerSimple.cpp @@ -11,7 +11,7 @@ using namespace doip; using namespace std; -static const DoIPAddress LOGICAL_ADDRESS(static_cast(0x00), static_cast(0x28)); +static const DoIPAddress LOGICAL_ADDRESS(0x0028); static std::atomic g_shutdownRequested{false}; // Forward declarations diff --git a/inc/ByteArray.h b/inc/ByteArray.h index e2b6841..b67a672 100644 --- a/inc/ByteArray.h +++ b/inc/ByteArray.h @@ -107,7 +107,7 @@ struct ByteArray : std::vector { * * @param value The 16-bit value to append */ - void writeU16(uint16_t value) { + void writeU16BE(uint16_t value) { emplace_back(static_cast((value >> 8) & 0xFF)); emplace_back(static_cast(value & 0xFF)); } @@ -140,7 +140,7 @@ struct ByteArray : std::vector { * * @param value The 32-bit value to append */ - void writeU32(uint32_t value) { + void writeU32BE(uint32_t value) { emplace_back(static_cast((value >> 24) & 0xFF)); emplace_back(static_cast((value >> 16) & 0xFF)); emplace_back(static_cast((value >> 8) & 0xFF)); @@ -171,9 +171,9 @@ struct ByteArray : std::vector { if constexpr (sizeof(UnderlyingType) == 1) { emplace_back(static_cast(integral_value)); } else if constexpr (sizeof(UnderlyingType) == 2) { - writeU16(static_cast(integral_value)); + writeU16BE(static_cast(integral_value)); } else if constexpr (sizeof(UnderlyingType) == 4) { - writeU32(static_cast(integral_value)); + writeU32BE(static_cast(integral_value)); } else { static_assert(sizeof(UnderlyingType) <= 4, "Enum underlying type too large (max 32-bit supported)"); } diff --git a/inc/DoIPAddress.h b/inc/DoIPAddress.h index da0c061..fbad14f 100644 --- a/inc/DoIPAddress.h +++ b/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(address >> 8), static_cast(address & 0xff)} {} /** * @brief Constructs an DoIPAddress from a byte array starting at the specified offset. diff --git a/inc/DoIPClient.h b/inc/DoIPClient.h index 61b3665..a82f8e8 100644 --- a/inc/DoIPClient.h +++ b/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; diff --git a/inc/DoIPMessage.h b/inc/DoIPMessage.h index df9ecf8..896b30b 100644 --- a/inc/DoIPMessage.h +++ b/inc/DoIPMessage.h @@ -387,7 +387,7 @@ class DoIPMessage { // Payload length (big-endian uint32_t) uint32_t payloadLength = static_cast(payload.size()); - m_data.writeU32(payloadLength); + m_data.writeU32BE(payloadLength); // Payload data m_data.insert(m_data.end(), payload.begin(), payload.end()); @@ -462,7 +462,7 @@ inline DoIPMessage makeVehicleIdentificationResponse( payload.reserve(vin.size() + logicalAddress.size() + entityType.size() + groupId.size() + 2); payload.insert(payload.end(), vin.begin(), vin.end()); - payload.writeU16(logicalAddress.toUint16()); + payload.writeU16BE(logicalAddress.toUint16()); payload.insert(payload.end(), entityType.begin(), entityType.end()); payload.insert(payload.end(), groupId.begin(), groupId.end()); payload.writeEnum(furtherAction); @@ -571,7 +571,7 @@ inline DoIPMessage makeAliveCheckRequest() { */ inline DoIPMessage makeAliveCheckResponse(const DoIPAddress &sa) { ByteArray payload; - payload.writeU16(sa.toUint16()); + payload.writeU16BE(sa.toUint16()); return DoIPMessage(DoIPPayloadType::AliveCheckResponse, std::move(payload)); } @@ -588,7 +588,7 @@ inline DoIPMessage makeRoutingActivationRequest( ByteArray payload; payload.reserve(ea.size() + 1 + 4); - payload.writeU16(ea.toUint16()); + payload.writeU16BE(ea.toUint16()); payload.writeEnum(actType); // Reserved 4 bytes for future use payload.insert(payload.end(), {0, 0, 0, 0}); diff --git a/inc/DoIPServerModel.h b/inc/DoIPServerModel.h index 4418ce1..28777fc 100644 --- a/inc/DoIPServerModel.h +++ b/inc/DoIPServerModel.h @@ -93,7 +93,7 @@ struct DoIPServerModel { ServerModelDownstreamHandler onDownstreamRequest; /// The logical address of this DoIP server - DoIPAddress serverAddress = DoIPAddress(0x0E, 0x00); + DoIPAddress serverAddress = DoIPAddress(0x0E00); /** * @brief Check if downstream forwarding is enabled diff --git a/test/DoIPDefaultConnection_Test.cpp b/test/DoIPDefaultConnection_Test.cpp index 55b8424..cac4a35 100644 --- a/test/DoIPDefaultConnection_Test.cpp +++ b/test/DoIPDefaultConnection_Test.cpp @@ -23,7 +23,7 @@ TEST_SUITE("DoIPDefaultConnection") { struct DoIPDefaultConnectionTestFixture { std::unique_ptr connection; - DoIPAddress sa = DoIPAddress(0x0E, 0x00); + DoIPAddress sa = DoIPAddress(0x0E00); DoIPDefaultConnectionTestFixture() : connection(std::make_unique(std::make_unique())) {} @@ -31,12 +31,13 @@ TEST_SUITE("DoIPDefaultConnection") { TEST_CASE_FIXTURE(DoIPDefaultConnectionTestFixture, "DoIPDefaultConnection: Get Server Address") { DoIPAddress serverAddress = connection->getServerAddress(); + INFO("Server Address: " << serverAddress); CHECK(serverAddress.hsb() == 0x0E); CHECK(serverAddress.lsb() == 0x00); } TEST_CASE_FIXTURE(DoIPDefaultConnectionTestFixture, "DoIPDefaultConnection: Send Protocol Message") { - DoIPMessage message = message::makeDiagnosticMessage(DoIPAddress(0xCA, 0xFE), DoIPAddress(0xBA, 0xBE), {0xDE, 0xAD, 0xBE, 0xEF}); + DoIPMessage message = message::makeDiagnosticMessage(DoIPAddress(0xCAFE), DoIPAddress(0xBABE), {0xDE, 0xAD, 0xBE, 0xEF}); CHECK(connection->sendProtocolMessage(message) == 16); } @@ -99,7 +100,7 @@ TEST_SUITE("DoIPDefaultConnection") { CHECK(connection->getState() == DoIPServerState::WaitRoutingActivation); CHECK(connection->getCloseReason() == DoIPCloseReason::None); - connection->handleMessage2(message::makeRoutingActivationRequest(DoIPAddress(0x0E, 0x00))); + connection->handleMessage2(message::makeRoutingActivationRequest(DoIPAddress(0xE000))); CHECK(connection->getState() == DoIPServerState::RoutingActivated); WAIT_FOR_STATE(connection, DoIPServerState::WaitAliveCheckResponse, 100000); diff --git a/test/DoIPMessage_Test.cpp b/test/DoIPMessage_Test.cpp index 86e8e59..d861995 100644 --- a/test/DoIPMessage_Test.cpp +++ b/test/DoIPMessage_Test.cpp @@ -40,7 +40,7 @@ TEST_SUITE("DoIPMessage") { } TEST_CASE("Message factory - makeDiagnosticMessage") { - DoIPMessage msg = message::makeDiagnosticMessage(DoIPAddress(0xca, 0xfe), DoIPAddress(0xba, 0xbe), {0xde, 0xad, 0xbe, 0xef}); + DoIPMessage msg = message::makeDiagnosticMessage(DoIPAddress(0xcafe), DoIPAddress(0xbabe), {0xde, 0xad, 0xbe, 0xef}); ByteArray expected{ 0x04, 0xfb, // protocol version + inv 0x80, 0x01, // payload type @@ -61,10 +61,10 @@ TEST_SUITE("DoIPMessage") { } TEST_CASE("Message factory - makeDiagnosticPositiveResponse") { - DoIPMessage msg = message::makeDiagnosticPositiveResponse(DoIPAddress(0xca, 0xfe), DoIPAddress(0xba, 0xbe), {0xde, 0xad, 0xbe, 0xef}); + DoIPMessage msg = message::makeDiagnosticPositiveResponse(DoIPAddress(0xcafe), DoIPAddress(0xbabe), {0xde, 0xad, 0xbe, 0xef}); ByteArray expected{ 0x04, 0xfb, // protocol version + inv - 0x80, 0x02, // payload tyByteArray raw = message::makeAliveCheckResponse(DoIPAddress(0xa0, 0xb0));pe + 0x80, 0x02, // payload tyByteArray raw = message::makeAliveCheckResponse(DoIPAddress(0xa0b0));pe 0x00, 0x00, 0x00, 0x09, // payload length 0xca, 0xfe, // sa 0xba, 0xbe, // ta @@ -84,8 +84,8 @@ TEST_SUITE("DoIPMessage") { TEST_CASE("Message factory - makeDiagnosticNegativeResponse") { DoIPMessage msg = message::makeDiagnosticNegativeResponse( - DoIPAddress(0xca, 0xfe), - DoIPAddress(0xba, 0xbe), + DoIPAddress(0xcafe), + DoIPAddress(0xbabe), DoIPNegativeDiagnosticAck::TargetBusy, {0xde, 0xad, 0xbe, 0xef}); @@ -118,7 +118,7 @@ TEST_SUITE("DoIPMessage") { } TEST_CASE("Message factory - makeAliveCheckResponse") { - DoIPMessage msg = message::makeAliveCheckResponse(DoIPAddress(0xa0, 0xb0)); + DoIPMessage msg = message::makeAliveCheckResponse(DoIPAddress(0xa0b0)); ByteArray expected{ 0x04, 0xfb, // protocol version + inv 0x00, 0x08, // payload type From c1bd461822e08a2d842454d992910585139b1357 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Thu, 4 Dec 2025 22:22:19 +0100 Subject: [PATCH 10/17] WIP --- examples/ExampleDoIPServerModel.h | 20 ++++++++++++++++ test/uds/UdsMock_Test.cpp | 39 +++++++++++++++++++++++++++++-- 2 files changed, 57 insertions(+), 2 deletions(-) diff --git a/examples/ExampleDoIPServerModel.h b/examples/ExampleDoIPServerModel.h index 7d5f3a2..5e0f444 100644 --- a/examples/ExampleDoIPServerModel.h +++ b/examples/ExampleDoIPServerModel.h @@ -9,6 +9,7 @@ #include "DoIPServerModel.h" #include "ThreadSafeQueue.h" #include "uds/UdsMock.h" +#include "uds/UdsResponseCode.h" using namespace doip; @@ -62,10 +63,29 @@ class ExampleDoIPServerModel : public DoIPServerModel { return DoIPDownstreamResult::Pending; }; + m_uds.registerDefaultServices(); + + m_uds.registerDiagnosticSessionControlHandler([this](uint8_t sessionType) { + m_loguds->info("Diagnostic Session Control requested, sessionType={:02X}", sessionType); + return std::make_pair(uds::UdsResponseCode::PositiveResponse, ByteArray{0x50, sessionType}); // Positive response + }); + + 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'}; + ByteArray response = {0x62, static_cast((did >> 8) & 0xFF), static_cast(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 + }); } private: std::shared_ptr m_log = Logger::get("smodel"); + std::shared_ptr m_loguds = Logger::get("uds"); ServerModelDownstreamResponseHandler m_downstreamCallback = nullptr; ThreadSafeQueue m_rx; ThreadSafeQueue m_tx; diff --git a/test/uds/UdsMock_Test.cpp b/test/uds/UdsMock_Test.cpp index 50a4b25..bb55548 100644 --- a/test/uds/UdsMock_Test.cpp +++ b/test/uds/UdsMock_Test.cpp @@ -6,13 +6,15 @@ #include #include "uds/UdsMock.h" +#include "../doctest_aux.h" using namespace doip; +using namespace doip::uds; TEST_SUITE("UdsMock") { TEST_CASE("UdsMock default behavior returns ServiceNotSupported") { - uds::UdsMock udsMock; + UdsMock udsMock; ByteArray request = {0x10, 0x01}; // Example UDS request (Diagnostic Session Control) ByteArray response = udsMock.handleDiagnosticRequest(request); @@ -24,7 +26,7 @@ TEST_SUITE("UdsMock") { } TEST_CASE("UdsMock custom handler returns positive response") { - uds::UdsMock udsMock; + UdsMock udsMock; udsMock.registerService(uds::UdsService::DiagnosticSessionControl, [](const ByteArray &request) { @@ -40,4 +42,37 @@ TEST_SUITE("UdsMock") { CHECK(response == expectedResponse); } + + TEST_CASE("UdsMock custom RDBI handler returns positive response") { + UdsMock udsMock; + + udsMock.registerService(UdsService::ReadDataByIdentifier, + [](const ByteArray &request) { + // Extract DID from request + if (request.size() < 3) { + return std::make_pair(uds::UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, ByteArray{}); + } + uint16_t did = request.readU16BE(1); + // Custom handler that returns positive response with dummy data + ByteArray responseData; + responseData.writeU16BE(did); // Echo back the DID + responseData.push_back(0x12); // Dummy data byte 1 + responseData.push_back(0x34); // Dummy data byte 2 + return std::make_pair(uds::UdsResponseCode::OK, responseData); + }); + ByteArray request = {0x22, 0x01, 0x02}; // Example UDS request (Diagnostic Session Control) + ByteArray response = udsMock.handleDiagnosticRequest(request); + + // Expected positive response: 0x50, 0x01 + ByteArray expectedResponse = {0x62, 0x01, 0x02, 0x12, 0x34}; + + CHECK_BYTE_ARRAY_EQ(response , expectedResponse); + + // bad request + request = {0x22, 0x01}; // Example UDS request (Diagnostic Session Control) + response = udsMock.handleDiagnosticRequest(request); + expectedResponse = {0x7f, 0x22, 0x13}; + + CHECK_BYTE_ARRAY_EQ(response, expectedResponse); + } } From 8e63e295dee6bca79cde2603413cbca2dedb12a8 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Fri, 5 Dec 2025 22:32:05 +0100 Subject: [PATCH 11/17] feat: Improve message output --- inc/DoIPMessage.h | 13 +++++++++++-- inc/DoIPRoutingActivationResult.h | 29 +++++++++++++++++++++++++++++ src/DoIPDefaultConnection.cpp | 8 ++------ 3 files changed, 42 insertions(+), 8 deletions(-) diff --git a/inc/DoIPMessage.h b/inc/DoIPMessage.h index 896b30b..83126bb 100644 --- a/inc/DoIPMessage.h +++ b/inc/DoIPMessage.h @@ -264,6 +264,9 @@ class DoIPMessage { if (getPayloadType() == DoIPPayloadType::RoutingActivationRequest && payloadRef.second >= 2) { return DoIPAddress(payloadRef.first, 0); } + if (getPayloadType() == DoIPPayloadType::RoutingActivationResponse && payloadRef.second >= 2) { + return DoIPAddress(payloadRef.first, 0); + } if (getPayloadType() == DoIPPayloadType::AliveCheckResponse && payloadRef.second >= 2) { return DoIPAddress(payloadRef.first, 0); } @@ -640,7 +643,6 @@ inline std::ostream &operator<<(std::ostream &os, const DoIPMessage &msg) { os << ansi::dim << "V" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(PROTOCOL_VERSION) << std::dec << ansi::reset; - if (msg.getPayloadType() == DoIPPayloadType::DiagnosticMessageNegativeAck) { auto payload = msg.getDiagnosticMessagePayload(); os << ansi::red << "|Diag NACK " << static_cast(payload.first[0]); @@ -648,7 +650,14 @@ inline std::ostream &operator<<(std::ostream &os, const DoIPMessage &msg) { os << ansi::yellow << "|Alive Check?"; } else if (msg.getPayloadType() == DoIPPayloadType::AliveCheckResponse) { auto sa = msg.getSourceAddress(); - os << ansi::green << "|Alive Check! " << sa.value(); + os << ansi::green << "|Alive Check " << sa.value() << " ✓"; + } else if (msg.getPayloadType() == DoIPPayloadType::RoutingActivationRequest) { + auto sa = msg.getSourceAddress(); + os << ansi::yellow << "|Routing activation? " << sa.value(); + } else if (msg.getPayloadType() == DoIPPayloadType::RoutingActivationResponse) { + auto sa = msg.getSourceAddress(); + + os << ansi::green << "|Routing activation " << sa.value() << " ✓"; } else if (msg.getPayloadType() == DoIPPayloadType::DiagnosticMessage) { auto payload = msg.getDiagnosticMessagePayload(); auto sa = msg.getSourceAddress(); diff --git a/inc/DoIPRoutingActivationResult.h b/inc/DoIPRoutingActivationResult.h index a0fe1ab..0225c05 100644 --- a/inc/DoIPRoutingActivationResult.h +++ b/inc/DoIPRoutingActivationResult.h @@ -45,6 +45,35 @@ inline bool closeSocketOnRoutingActivationResult(DoIPRoutingActivationResult res } } +inline std::ostream& operator<<(std::ostream& os, DoIPRoutingActivationResult result) { + switch (result) { + case DoIPRoutingActivationResult::UnknownSourceAddress: + return os << "UnknownSourceAddress"; + case DoIPRoutingActivationResult::NoMoreRoutingSlotsAvailable: + return os << "NoMoreRoutingSlotsAvailable"; + case DoIPRoutingActivationResult::InvalidAddressOrRoutingType: + return os << "InvalidAddressOrRoutingType"; + case DoIPRoutingActivationResult::SourceAddressAlreadyRegistered: + return os << "SourceAddressAlreadyRegistered"; + case DoIPRoutingActivationResult::Unauthorized: + return os << "Unauthorized"; + case DoIPRoutingActivationResult::MissingConfirmation: + return os << "MissingConfirmation"; + case DoIPRoutingActivationResult::InvalidRoutingType: + return os << "InvalidRoutingType"; + case DoIPRoutingActivationResult::SecuredConnectionRequired: + return os << "SecuredConnectionRequired"; + case DoIPRoutingActivationResult::VehicleNotReadyForRouting: + return os << "VehicleNotReadyForRouting"; + case DoIPRoutingActivationResult::RouteActivated: + return os << "RouteActivated"; + case DoIPRoutingActivationResult::RouteActivatedConfirmationRequired: + return os << "RouteActivatedConfirmationRequired"; + default: + return os << "Unknown(" << static_cast(result) << ")"; + } +} + } // namespace doip #endif /* DOIPROUTINGACTIVATIONRESULT_H */ diff --git a/src/DoIPDefaultConnection.cpp b/src/DoIPDefaultConnection.cpp index f583a16..be90ad6 100644 --- a/src/DoIPDefaultConnection.cpp +++ b/src/DoIPDefaultConnection.cpp @@ -366,16 +366,12 @@ ssize_t DoIPDefaultConnection::sendRoutingActivationResponse(const DoIPAddress & payload.insert(payload.end(), {0x00, 0x00, 0x00, 0x00}); DoIPMessage response(DoIPPayloadType::RoutingActivationResponse, std::move(payload)); - auto sentBytes = sendProtocolMessage(response); - DOIP_LOG_INFO("Sent routing activation response: code=" + std::to_string(static_cast(response_code)) + " to address=" + std::to_string(static_cast(source_address.toUint16()))); - return sentBytes; + return sendProtocolMessage(response); } ssize_t DoIPDefaultConnection::sendAliveCheckRequest() { auto request = message::makeAliveCheckRequest(); - auto sentBytes = sendProtocolMessage(request); - DOIP_LOG_INFO("Sent alive check request"); - return sentBytes; + return sendProtocolMessage(request); } ssize_t DoIPDefaultConnection::sendDiagnosticMessageResponse(const DoIPAddress &sourceAddress, DoIPDiagnosticAck ack) { From f2b0c0087df5c9330c6e07f3889981c838656e8f Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Sat, 6 Dec 2025 21:26:36 +0100 Subject: [PATCH 12/17] feat: Implement UDS services 0x10, 0x2E and 0x3E --- examples/ExampleDoIPServerModel.h | 29 ++++++++++++++++++++-------- test/scripts/change-vin-and-reset.py | 17 ++++++++++++++-- 2 files changed, 36 insertions(+), 10 deletions(-) diff --git a/examples/ExampleDoIPServerModel.h b/examples/ExampleDoIPServerModel.h index 5e0f444..342cb30 100644 --- a/examples/ExampleDoIPServerModel.h +++ b/examples/ExampleDoIPServerModel.h @@ -31,11 +31,11 @@ class ExampleDoIPServerModel : public DoIPServerModel { 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) { - m_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; }; @@ -67,20 +67,30 @@ class ExampleDoIPServerModel : public DoIPServerModel { m_uds.registerDiagnosticSessionControlHandler([this](uint8_t sessionType) { m_loguds->info("Diagnostic Session Control requested, sessionType={:02X}", sessionType); - return std::make_pair(uds::UdsResponseCode::PositiveResponse, ByteArray{0x50, sessionType}); // Positive response + 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.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'}; - ByteArray response = {0x62, static_cast((did >> 8) & 0xFF), static_cast(did & 0xFF)}; + ByteArray vinPayload = {'1', 'H', 'G', 'C', 'M', + '8', '2', '6', '3', '3', + 'A', '0', '0', '0', '0', '1', 'Z'}; + ByteArray response = {static_cast((did >> 8) & 0xFF), static_cast(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.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: @@ -92,6 +102,9 @@ class ExampleDoIPServerModel : public DoIPServerModel { 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] { diff --git a/test/scripts/change-vin-and-reset.py b/test/scripts/change-vin-and-reset.py index 88f3180..7dfd827 100644 --- a/test/scripts/change-vin-and-reset.py +++ b/test/scripts/change-vin-and-reset.py @@ -4,18 +4,31 @@ from udsoncan.client import Client from udsoncan.exceptions import * from udsoncan.services import * +from udsoncan import DataIdentifier, AsciiCodec udsoncan.setup_logging() +# Add this config +config = { + 'data_identifiers': { + DataIdentifier.VIN: AsciiCodec(17) + } +} + ecu_ip = '127.0.0.1' ecu_logical_address = 0x00E0 doip_client = DoIPClient(ecu_ip, ecu_logical_address) conn = DoIPClientUDSConnector(doip_client) -with Client(conn, request_timeout=2) as client: +with Client(conn, request_timeout=2, config=config) as client: try: client.change_session(1) # integer with value of 3 #client.unlock_security_access(MyCar.debug_level) # Fictive security level. Integer coming from fictive lib, let's say its value is 5 - client.write_data_by_identifier(udsoncan.DataIdentifier.VIN, 'ABC123456789') # Standard ID for VIN is 0xF190. Codec is set in the client configuration + vin = client.read_data_by_identifier(udsoncan.DataIdentifier.VIN) + vin_response = client.read_data_by_identifier(udsoncan.DataIdentifier.VIN) + vin_bytes = vin_response.service_data.values[udsoncan.DataIdentifier.VIN] + print('Current Vehicle Identification Number is: %s' % vin_bytes.decode('ascii')) + + client.write_data_by_identifier(udsoncan.DataIdentifier.VIN, 'ABC123456789GHIJK') # Standard ID for VIN is 0xF190. Codec is set in the client configuration print('Vehicle Identification Number successfully changed.') client.ecu_reset(ECUReset.ResetType.hardReset) # HardReset = 0x01 except NegativeResponseException as e: From ac71daedc315c94300a3765c0c1e654b3fac9bc4 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Sat, 6 Dec 2025 22:23:13 +0100 Subject: [PATCH 13/17] feat: Implemented remaining UDS services so that script finishes successfully --- examples/ExampleDoIPServerModel.h | 14 +++++++++ inc/uds/UdsMock.h | 46 ++++++++++++++++++++++------ inc/uds/UdsServices.h | 44 ++++++++++++++++++++++++++ src/uds/UdsHelpers.cpp | 12 +++++--- test/scripts/change-vin-and-reset.py | 13 +++++--- 5 files changed, 112 insertions(+), 17 deletions(-) diff --git a/examples/ExampleDoIPServerModel.h b/examples/ExampleDoIPServerModel.h index 342cb30..9e4a0c2 100644 --- a/examples/ExampleDoIPServerModel.h +++ b/examples/ExampleDoIPServerModel.h @@ -73,6 +73,11 @@ class ExampleDoIPServerModel : public DoIPServerModel { 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) { @@ -87,6 +92,15 @@ class ExampleDoIPServerModel : public DoIPServerModel { 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((did >> 8) & 0xFF), static_cast(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 diff --git a/inc/uds/UdsMock.h b/inc/uds/UdsMock.h index 26819e2..6b0ea4d 100644 --- a/inc/uds/UdsMock.h +++ b/inc/uds/UdsMock.h @@ -1,15 +1,15 @@ #ifndef UDSMOCK_H #define UDSMOCK_H -#include -#include #include +#include #include +#include #include "DoIPMessage.h" -#include "UdsResponseCode.h" #include "IUdsServiceHandler.h" #include "LambdaUdsHandler.h" +#include "UdsResponseCode.h" #include "UdsServices.h" using namespace doip; @@ -19,7 +19,7 @@ namespace doip::uds { constexpr uint8_t UDS_POSITIVE_RESPONSE_OFFSET = 0x40; class UdsMock { -public: + public: UdsMock() = default; // Register a handler owning pointer @@ -28,7 +28,7 @@ class UdsMock { } // Register a lambda/function - void registerService(UdsService serviceId, std::function fn) { + void registerService(UdsService serviceId, std::function fn) { m_handlers[static_cast(serviceId)] = std::make_unique(std::move(fn)); } @@ -50,17 +50,45 @@ class UdsMock { // Read Data By Identifier (0x22): handler(did, params) void registerReadDataByIdentifierHandler(std::function handler); + // Write Data By Identifier (0x2E): handler(did, data) + void registerWriteDataByIdentifierHandler(std::function handler); + // Tester Present (0x3E): handler(subFunction) void registerTesterPresentHandler(std::function handler); + ByteArray handleDiagnosticRequest(const ByteArray &request) { - if (request.empty()) return {}; + if (request.empty()) + return {}; uint8_t sid = request[0]; + UdsService service = static_cast(sid); + + const UdsServiceDescriptor *desc = findServiceDescriptor(service); + if (!desc) { + return makeResponse(request, UdsResponseCode::ServiceNotSupported, {}); + } + + if (request.size() < desc->minReqLength || request.size() > desc->maxReqLength) { + std::cerr << "UdsMock: Request length " << request.size() + << " out of bounds for service 0x" << std::hex << static_cast(service) << std::dec + << " (expected " << desc->minReqLength << "-" << desc->maxReqLength << ")\n"; + return makeResponse(request, UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, {}); + } + UdsResponse resp = defaultResponse(request); auto it = m_handlers.find(sid); if (it != m_handlers.end() && it->second) { resp = it->second->handle(request); } + + auto rspSize = resp.second.size() + 1; // +1 for the SID + if (rspSize < desc->minRspLength || rspSize > desc->maxRspLength) { + std::cerr << "UdsMock: Response length " << resp.second.size() + << " out of bounds for service 0x" << std::hex << static_cast(service) << std::dec + << " (expected " << desc->minRspLength << "-" << desc->maxRspLength << ")\n"; + return makeResponse(request, UdsResponseCode::GeneralProgrammingFailure, {}); + } + return makeResponse(request, resp.first, resp.second); } @@ -98,7 +126,7 @@ class UdsMock { } } -private: + private: static UdsResponse defaultResponse(const ByteArray &request) { (void)request; return {UdsResponseCode::ServiceNotSupported, {}}; @@ -107,9 +135,9 @@ class UdsMock { static ByteArray makeResponse(const ByteArray &request, UdsResponseCode responseCode = UdsResponseCode::OK, const ByteArray &extraData = {}) { if (responseCode != UdsResponseCode::OK) { ByteArray negativeResponse; - negativeResponse.emplace_back(0x7F); // Negative response indicator + negativeResponse.emplace_back(0x7F); // Negative response indicator negativeResponse.emplace_back(request.empty() ? 0x00 : request[0]); // Original service ID or 0 - negativeResponse.emplace_back(static_cast(responseCode)); // NRC + negativeResponse.emplace_back(static_cast(responseCode)); // NRC return negativeResponse; } diff --git a/inc/uds/UdsServices.h b/inc/uds/UdsServices.h index 3615ad9..fca08ca 100644 --- a/inc/uds/UdsServices.h +++ b/inc/uds/UdsServices.h @@ -8,6 +8,7 @@ namespace doip::uds { + enum class UdsService : uint8_t { DiagnosticSessionControl = 0x10, ECUReset = 0x11, @@ -30,9 +31,52 @@ enum class UdsService : uint8_t { ReadDTCInformation = 0x19, }; +using uds_length = uint16_t; +struct UdsServiceDescriptor { + UdsService service; + uds_length minReqLength; + uds_length maxReqLength; + uds_length minRspLength; + uds_length maxRspLength; +}; +constexpr uds_length MAX_UDS_MESSAGE_LENGTH = 4095; +constexpr std::array UDS_SERVICE_DESCRIPTORS = {{ + { UdsService::DiagnosticSessionControl, 2, 2, 6, 6 }, + { UdsService::ECUReset, 2, 2, 2, 2 }, + { UdsService::SecurityAccess, 2, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::CommunicationControl, 2, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::TesterPresent, 2, 2, 2, 2 }, + { UdsService::AccessTimingParameters, 2, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::SecuredDataTransmission, 2, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::ControlDTCSetting, 2, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::ResponseOnEvent, 2, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::LinkControl, 2, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::ReadDataByIdentifier, 3, MAX_UDS_MESSAGE_LENGTH, 4, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::ReadMemoryByAddress, 4, MAX_UDS_MESSAGE_LENGTH, 4, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::ReadScalingDataByIdentifier, 3, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::ReadDataByPeriodicIdentifier, 3, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::DynamicallyDefineDataIdentifier, 3, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::WriteDataByIdentifier, 4, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::WriteMemoryByAddress, 4, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::ClearDiagnosticInformation, 3, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH }, + { UdsService::ReadDTCInformation, 2, MAX_UDS_MESSAGE_LENGTH, 3, MAX_UDS_MESSAGE_LENGTH } +}}; +/** + * @brief Find service descriptor by service ID + * + * @param sid the UDS service ID + * @return const UdsServiceDescriptor* the service descriptor or nullptr if not found + */ +inline const UdsServiceDescriptor* findServiceDescriptor(UdsService sid) { + for (const auto& desc : UDS_SERVICE_DESCRIPTORS) { + if (desc.service == sid) + return &desc; + } + return nullptr; +} } // namespace doip::uds diff --git a/src/uds/UdsHelpers.cpp b/src/uds/UdsHelpers.cpp index 7eea2f9..e1e6825 100644 --- a/src/uds/UdsHelpers.cpp +++ b/src/uds/UdsHelpers.cpp @@ -5,7 +5,6 @@ namespace doip::uds { void UdsMock::registerDiagnosticSessionControlHandler(std::function handler) { registerService(UdsService::DiagnosticSessionControl, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { - if (req.size() < 2) return {UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, {}}; uint8_t sessionType = req[1]; return handler(sessionType); }); @@ -13,7 +12,6 @@ void UdsMock::registerDiagnosticSessionControlHandler(std::function handler) { registerService(UdsService::ECUReset, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { - if (req.size() < 2) return {UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, {}}; uint8_t resetType = req[1]; return handler(resetType); }); @@ -21,15 +19,21 @@ void UdsMock::registerECUResetHandler(std::function handle void UdsMock::registerReadDataByIdentifierHandler(std::function handler) { registerService(UdsService::ReadDataByIdentifier, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { - if (req.size() < 3) return {UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, {}}; uint16_t did = (static_cast(req[1]) << 8) | req[2]; return handler(did); }); } +void UdsMock::registerWriteDataByIdentifierHandler(std::function handler) { + registerService(UdsService::WriteDataByIdentifier, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { + uint16_t did = (static_cast(req[1]) << 8) | req[2]; + return handler(did, ByteArray(req.data() + 3, req.size() - 3)); + }); +} + + void UdsMock::registerTesterPresentHandler(std::function handler) { registerService(UdsService::TesterPresent, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { - if (req.size() < 2) return {UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, {}}; uint8_t subFunction = req[1]; return handler(subFunction); }); diff --git a/test/scripts/change-vin-and-reset.py b/test/scripts/change-vin-and-reset.py index 7dfd827..a1cf291 100644 --- a/test/scripts/change-vin-and-reset.py +++ b/test/scripts/change-vin-and-reset.py @@ -23,11 +23,16 @@ try: client.change_session(1) # integer with value of 3 #client.unlock_security_access(MyCar.debug_level) # Fictive security level. Integer coming from fictive lib, let's say its value is 5 - vin = client.read_data_by_identifier(udsoncan.DataIdentifier.VIN) + # Read VIN once and handle response type (udsoncan may return bytes or str) vin_response = client.read_data_by_identifier(udsoncan.DataIdentifier.VIN) - vin_bytes = vin_response.service_data.values[udsoncan.DataIdentifier.VIN] - print('Current Vehicle Identification Number is: %s' % vin_bytes.decode('ascii')) - + vin_value = vin_response.service_data.values[udsoncan.DataIdentifier.VIN] + if isinstance(vin_value, bytes): + vin_str = vin_value.decode('ascii', errors='ignore') + else: + # already a str + vin_str = str(vin_value) + print('Current Vehicle Identification Number is: %s' % vin_str) + client.write_data_by_identifier(udsoncan.DataIdentifier.VIN, 'ABC123456789GHIJK') # Standard ID for VIN is 0xF190. Codec is set in the client configuration print('Vehicle Identification Number successfully changed.') client.ecu_reset(ECUReset.ResetType.hardReset) # HardReset = 0x01 From de1f8dabc0c7c33d54ed080fab00b916ffdd58bc Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Sun, 7 Dec 2025 10:24:38 +0100 Subject: [PATCH 14/17] feat: Rename UdsHelpers to UdsMock --- CMakeLists.txt | 2 +- src/uds/{UdsHelpers.cpp => UdsMock.cpp} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename src/uds/{UdsHelpers.cpp => UdsMock.cpp} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ce2a63..02a81e7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,7 +83,7 @@ set(SOURCES src/Logger.cpp src/MacAddress.cpp src/DoIPDefaultConnection.cpp - src/uds/UdsHelpers.cpp + src/uds/UdsMock.cpp ) diff --git a/src/uds/UdsHelpers.cpp b/src/uds/UdsMock.cpp similarity index 100% rename from src/uds/UdsHelpers.cpp rename to src/uds/UdsMock.cpp From 8c9cc6886ff932ad9858e3bf5a9104cfc16588fe Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Sun, 7 Dec 2025 10:52:53 +0100 Subject: [PATCH 15/17] feat: Enhance UdsMock and UdsResponseCode with improved handling and output formatting --- inc/uds/IUdsServiceHandler.h | 18 ++++ inc/uds/UdsMock.h | 40 +-------- inc/uds/UdsResponseCode.h | 157 +++++++++++++++++++++++++++++++++++ inc/uds/UdsServices.h | 9 +- src/uds/UdsMock.cpp | 38 ++++++++- test/uds/UdsMock_Test.cpp | 55 ++++++------ 6 files changed, 248 insertions(+), 69 deletions(-) diff --git a/inc/uds/IUdsServiceHandler.h b/inc/uds/IUdsServiceHandler.h index d94c273..a1d912c 100644 --- a/inc/uds/IUdsServiceHandler.h +++ b/inc/uds/IUdsServiceHandler.h @@ -9,6 +9,24 @@ namespace doip::uds { using UdsResponse = std::pair; +inline std::ostream &operator<<(std::ostream &os, const UdsResponse &response) { + std::ios_base::fmtflags flags(os.flags()); + + os << response.first << " ["; + os << std::hex << std::uppercase << std::setw(2) << std::setfill('0'); + + for (size_t i = 0; i < response.second.size(); ++i) { + if (i > 0) { + os << '.'; + } + os << std::hex << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(response.second[i]); + } + + os.flags(flags); + return os; +} + class IUdsServiceHandler { public: virtual ~IUdsServiceHandler() = default; diff --git a/inc/uds/UdsMock.h b/inc/uds/UdsMock.h index 6b0ea4d..a8dbbfe 100644 --- a/inc/uds/UdsMock.h +++ b/inc/uds/UdsMock.h @@ -56,41 +56,8 @@ class UdsMock { // Tester Present (0x3E): handler(subFunction) void registerTesterPresentHandler(std::function handler); - - ByteArray handleDiagnosticRequest(const ByteArray &request) { - if (request.empty()) - return {}; - uint8_t sid = request[0]; - UdsService service = static_cast(sid); - - const UdsServiceDescriptor *desc = findServiceDescriptor(service); - if (!desc) { - return makeResponse(request, UdsResponseCode::ServiceNotSupported, {}); - } - - if (request.size() < desc->minReqLength || request.size() > desc->maxReqLength) { - std::cerr << "UdsMock: Request length " << request.size() - << " out of bounds for service 0x" << std::hex << static_cast(service) << std::dec - << " (expected " << desc->minReqLength << "-" << desc->maxReqLength << ")\n"; - return makeResponse(request, UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, {}); - } - - UdsResponse resp = defaultResponse(request); - auto it = m_handlers.find(sid); - if (it != m_handlers.end() && it->second) { - resp = it->second->handle(request); - } - auto rspSize = resp.second.size() + 1; // +1 for the SID - if (rspSize < desc->minRspLength || rspSize > desc->maxRspLength) { - std::cerr << "UdsMock: Response length " << resp.second.size() - << " out of bounds for service 0x" << std::hex << static_cast(service) << std::dec - << " (expected " << desc->minRspLength << "-" << desc->maxRspLength << ")\n"; - return makeResponse(request, UdsResponseCode::GeneralProgrammingFailure, {}); - } - - return makeResponse(request, resp.first, resp.second); - } + ByteArray handleDiagnosticRequest(const ByteArray &request) const; // Register default handlers for all known services. // By default these handlers simply return ServiceNotSupported. Tests @@ -127,11 +94,6 @@ class UdsMock { } private: - static UdsResponse defaultResponse(const ByteArray &request) { - (void)request; - return {UdsResponseCode::ServiceNotSupported, {}}; - } - static ByteArray makeResponse(const ByteArray &request, UdsResponseCode responseCode = UdsResponseCode::OK, const ByteArray &extraData = {}) { if (responseCode != UdsResponseCode::OK) { ByteArray negativeResponse; diff --git a/inc/uds/UdsResponseCode.h b/inc/uds/UdsResponseCode.h index 14df411..60e0d6b 100644 --- a/inc/uds/UdsResponseCode.h +++ b/inc/uds/UdsResponseCode.h @@ -3,6 +3,10 @@ #include +#include "AnsiColors.h" +#include +#include + namespace doip::uds { enum class UdsResponseCode : uint8_t { OK = 0, // positive response @@ -79,6 +83,159 @@ namespace doip::uds { VoltageTooLow = 0x93, ResourceTemporarilyNotAvailable = 0x94, }; + + + inline std::ostream &operator<<(std::ostream &os, const UdsResponseCode &code) { + os << "UdsResponseCode(0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(code) << std::dec << ")"; + + if (code == UdsResponseCode::OK) { + os << " " << ansi::green << "OK" << ansi::reset; + } else { + os << " " << ansi::red << "NRC" << ansi::reset; + switch(code) { + case UdsResponseCode::GeneralReject: + os << " General Reject"; + break; + case UdsResponseCode::ServiceNotSupported: + os << " Service Not Supported"; + break; + case UdsResponseCode::SubFunctionNotSupported: + os << " SubFunction Not Supported"; + break; + case UdsResponseCode::IncorrectMessageLengthOrInvalidFormat: + os << " Incorrect Message Length Or Invalid Format"; + break; + case UdsResponseCode::ResponseTooLong: + os << " Response Too Long"; + break; + case UdsResponseCode::BusyRepeatRequest: + os << " Busy Repeat Request"; + break; + case UdsResponseCode::ConditionsNotCorrect: + os << " Conditions Not Correct"; + break; + case UdsResponseCode::RequestSequenceError: + os << " Request Sequence Error"; + break; + case UdsResponseCode::NoResponseFromSubnetComponent: + os << " No Response From Subnet Component"; + break; + case UdsResponseCode::FailurePreventsExecutionOfRequestedAction: + os << " Failure Prevents Execution Of Requested Action"; + break; + case UdsResponseCode::RequestOutOfRange: + os << " Request Out Of Range"; + break; + case UdsResponseCode::SecurityAccessDenied: + os << " Security Access Denied"; + break; + case UdsResponseCode::AuthenticationRequired: + os << " AuthenticationRequired"; + break; + case UdsResponseCode::InvalidKey: + os << " InvalidKey"; + break; + case UdsResponseCode::ExceedNumberOfAttempts: + os << " ExceedNumberOfAttempts"; + break; + case UdsResponseCode::RequiredTimeDelayNotExpired: + os << " RequiredTimeDelayNotExpired"; + break; + case UdsResponseCode::SecureDataTransmissionRequired: + os << " SecureDataTransmissionRequired"; + break; + case UdsResponseCode::SecureDataTransmissionNotAllowed: + os << " SecureDataTransmissionNotAllowed"; + break; + case UdsResponseCode::SecureDataVerificationFailed: + os << " SecureDataVerificationFailed"; + break; + case UdsResponseCode::UploadDownloadNotAccepted: + os << " UploadDownloadNotAccepted"; + break; + case UdsResponseCode::TransferDataSuspended: + os << " TransferDataSuspended"; + break; + case UdsResponseCode::GeneralProgrammingFailure: + os << " GeneralProgrammingFailure"; + break; + case UdsResponseCode::WrongBlockSequenceCounter: + os << " WrongBlockSequenceCounter"; + break; + case UdsResponseCode::RequestCorrectlyReceived_ResponsePending: + os << " RequestCorrectlyReceived_ResponsePending"; + break; + case UdsResponseCode::SubFunctionNotSupportedInActiveSession: + os << " SubFunctionNotSupportedInActiveSession"; + break; + case UdsResponseCode::ServiceNotSupportedInActiveSession: + os << " ServiceNotSupportedInActiveSession"; + break; + case UdsResponseCode::RpmTooHigh: + os << " RpmTooHigh"; + break; + case UdsResponseCode::RpmTooLow: + os << " RpmTooLow"; + break; + case UdsResponseCode::EngineIsRunning: + os << " EngineIsRunning"; + break; + case UdsResponseCode::EngineIsNotRunning: + os << " EngineIsNotRunning"; + break; + case UdsResponseCode::EngineRunTimeTooLow: + os << " EngineRunTimeTooLow"; + break; + case UdsResponseCode::TemperatureTooHigh: + os << " TemperatureTooHigh"; + break; + case UdsResponseCode::TemperatureTooLow: + os << " TemperatureTooLow"; + break; + case UdsResponseCode::VehicleSpeedTooHigh: + os << " VehicleSpeedTooHigh"; + break; + case UdsResponseCode::VehicleSpeedTooLow: + os << " VehicleSpeedTooLow"; + break; + case UdsResponseCode::ThrottlePedalTooHigh: + os << " ThrottlePedalTooHigh"; + break; + case UdsResponseCode::ThrottlePedalTooLow: + os << " ThrottlePedalTooLow"; + break; + case UdsResponseCode::TransmissionRangeNotInNeutral: + os << " TransmissionRangeNotInNeutral"; + break; + case UdsResponseCode::TransmissionRangeNotInGear: + os << " TransmissionRangeNotInGear"; + break; + case UdsResponseCode::BrakeSwitchNotClosed: + os << " BrakeSwitchNotClosed"; + break; + case UdsResponseCode::ShifterLeverNotInPark: + os << " ShifterLeverNotInPark"; + break; + case UdsResponseCode::TorqueConverterClutchLocked: + os << " TorqueConverterClutchLocked"; + break; + case UdsResponseCode::VoltageTooHigh: + os << " VoltageTooHigh"; + break; + case UdsResponseCode::VoltageTooLow: + os << " VoltageTooLow"; + break; + case UdsResponseCode::ResourceTemporarilyNotAvailable: + os << " ResourceTemporarilyNotAvailable"; + break; + default: + os << " UnknownNRC"; + break; + } + } + return os; + } } // namespace doip::uds #endif /* UDSRESPONSE_H */ diff --git a/inc/uds/UdsServices.h b/inc/uds/UdsServices.h index fca08ca..3c2a47e 100644 --- a/inc/uds/UdsServices.h +++ b/inc/uds/UdsServices.h @@ -71,9 +71,12 @@ constexpr std::array UDS_SERVICE_DESCRIPTORS = {{ * @return const UdsServiceDescriptor* the service descriptor or nullptr if not found */ inline const UdsServiceDescriptor* findServiceDescriptor(UdsService sid) { - for (const auto& desc : UDS_SERVICE_DESCRIPTORS) { - if (desc.service == sid) - return &desc; + auto it = std::find_if(UDS_SERVICE_DESCRIPTORS.begin(), UDS_SERVICE_DESCRIPTORS.end(), + [sid](const UdsServiceDescriptor& desc) { + return desc.service == sid; + }); + if (it != UDS_SERVICE_DESCRIPTORS.end()) { + return &(*it); } return nullptr; } diff --git a/src/uds/UdsMock.cpp b/src/uds/UdsMock.cpp index e1e6825..97402dd 100644 --- a/src/uds/UdsMock.cpp +++ b/src/uds/UdsMock.cpp @@ -3,6 +3,43 @@ namespace doip::uds { +ByteArray UdsMock::handleDiagnosticRequest(const ByteArray &request) const { + if (request.empty()) + return {}; + uint8_t sid = request[0]; + UdsService service = static_cast(sid); + + const UdsServiceDescriptor *desc = findServiceDescriptor(service); + if (!desc) { + return makeResponse(request, UdsResponseCode::ServiceNotSupported, {}); + } + + if (request.size() < desc->minReqLength || request.size() > desc->maxReqLength) { + std::cerr << "UdsMock: Request length " << request.size() + << " out of bounds for service 0x" << std::hex << static_cast(service) << std::dec + << " (expected " << desc->minReqLength << "-" << desc->maxReqLength << ")\n"; + return makeResponse(request, UdsResponseCode::IncorrectMessageLengthOrInvalidFormat); + } + + UdsResponse resp = {UdsResponseCode::ServiceNotSupported, {}}; + auto it = m_handlers.find(sid); + if (it != m_handlers.end() && it->second) { + resp = it->second->handle(request); + } else { + return makeResponse(request, UdsResponseCode::ServiceNotSupported); + } + + auto rspSize = resp.second.size() + 1; // +1 for the SID + if (rspSize < desc->minRspLength || rspSize > desc->maxRspLength) { + std::cerr << "UdsMock: Response length " << resp.second.size() + << " out of bounds for service 0x" << std::hex << static_cast(service) << std::dec + << " (expected " << desc->minRspLength << "-" << desc->maxRspLength << ")\n"; + return makeResponse(request, UdsResponseCode::GeneralProgrammingFailure, {}); + } + + return makeResponse(request, resp.first, resp.second); +} + void UdsMock::registerDiagnosticSessionControlHandler(std::function handler) { registerService(UdsService::DiagnosticSessionControl, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { uint8_t sessionType = req[1]; @@ -31,7 +68,6 @@ void UdsMock::registerWriteDataByIdentifierHandler(std::function handler) { registerService(UdsService::TesterPresent, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { uint8_t subFunction = req[1]; diff --git a/test/uds/UdsMock_Test.cpp b/test/uds/UdsMock_Test.cpp index bb55548..75bf34a 100644 --- a/test/uds/UdsMock_Test.cpp +++ b/test/uds/UdsMock_Test.cpp @@ -1,19 +1,18 @@ -#include #include "MacAddress.h" -#include -#include +#include #include +#include #include -#include "uds/UdsMock.h" #include "../doctest_aux.h" +#include "uds/UdsMock.h" using namespace doip; using namespace doip::uds; TEST_SUITE("UdsMock") { - TEST_CASE("UdsMock default behavior returns ServiceNotSupported") { + TEST_CASE("UdsMock default behavior returns ServiceNotSupported focus") { UdsMock udsMock; ByteArray request = {0x10, 0x01}; // Example UDS request (Diagnostic Session Control) @@ -22,6 +21,7 @@ TEST_SUITE("UdsMock") { // Expected negative response: 0x7F, 0x10, NRC for ServiceNotSupported (0x11) ByteArray expectedResponse = {0x7F, 0x10, 0x11}; + INFO(response); CHECK(response == expectedResponse); } @@ -29,50 +29,53 @@ TEST_SUITE("UdsMock") { UdsMock udsMock; udsMock.registerService(uds::UdsService::DiagnosticSessionControl, - [](const ByteArray &request) { - // Custom handler that returns positive response - return std::make_pair(uds::UdsResponseCode::OK, ByteArray{request[1]}); - }); + [](const ByteArray &request) { + // Custom handler that returns positive response + return std::make_pair(uds::UdsResponseCode::OK, ByteArray{request[1], 1, 2, 3, 4}); + }); ByteArray request = {0x10, 0x01}; // Example UDS request (Diagnostic Session Control) ByteArray response = udsMock.handleDiagnosticRequest(request); - // Expected positive response: 0x50, 0x01 - ByteArray expectedResponse = {0x50, 0x01}; + // Expected positive response: 0x50, 0x01, 0x01, 0x02, 0x03, 0x04 + ByteArray expectedResponse = {0x50, 0x01, 0x01, 0x02, 0x03, 0x04}; + INFO(response); CHECK(response == expectedResponse); } - TEST_CASE("UdsMock custom RDBI handler returns positive response") { + TEST_CASE("UdsMock custom RDBI handler returns positive response focus") { UdsMock udsMock; udsMock.registerService(UdsService::ReadDataByIdentifier, - [](const ByteArray &request) { - // Extract DID from request - if (request.size() < 3) { - return std::make_pair(uds::UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, ByteArray{}); - } - uint16_t did = request.readU16BE(1); - // Custom handler that returns positive response with dummy data - ByteArray responseData; - responseData.writeU16BE(did); // Echo back the DID - responseData.push_back(0x12); // Dummy data byte 1 - responseData.push_back(0x34); // Dummy data byte 2 - return std::make_pair(uds::UdsResponseCode::OK, responseData); - }); + [](const ByteArray &request) { + // Extract DID from request + if (request.size() < 3) { + return std::make_pair(uds::UdsResponseCode::IncorrectMessageLengthOrInvalidFormat, ByteArray{}); + } + uint16_t did = request.readU16BE(1); + // Custom handler that returns positive response with dummy data + ByteArray responseData; + responseData.writeU16BE(did); // Echo back the DID + responseData.push_back(0x12); // Dummy data byte 1 + responseData.push_back(0x34); // Dummy data byte 2 + return std::make_pair(uds::UdsResponseCode::OK, responseData); + }); ByteArray request = {0x22, 0x01, 0x02}; // Example UDS request (Diagnostic Session Control) ByteArray response = udsMock.handleDiagnosticRequest(request); // Expected positive response: 0x50, 0x01 ByteArray expectedResponse = {0x62, 0x01, 0x02, 0x12, 0x34}; - CHECK_BYTE_ARRAY_EQ(response , expectedResponse); + INFO(response); + CHECK_BYTE_ARRAY_EQ(response, expectedResponse); // bad request request = {0x22, 0x01}; // Example UDS request (Diagnostic Session Control) response = udsMock.handleDiagnosticRequest(request); expectedResponse = {0x7f, 0x22, 0x13}; + INFO(response); CHECK_BYTE_ARRAY_EQ(response, expectedResponse); } } From 74bec8076fcb5ae38fae8799f3e05bfbbd742b58 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Sun, 7 Dec 2025 11:03:19 +0100 Subject: [PATCH 16/17] fix: Check for nullptr in stream operator --- inc/DoIPMessage.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/inc/DoIPMessage.h b/inc/DoIPMessage.h index 83126bb..8271a58 100644 --- a/inc/DoIPMessage.h +++ b/inc/DoIPMessage.h @@ -645,6 +645,10 @@ inline std::ostream &operator<<(std::ostream &os, const DoIPMessage &msg) { if (msg.getPayloadType() == DoIPPayloadType::DiagnosticMessageNegativeAck) { auto payload = msg.getDiagnosticMessagePayload(); + if (payload.first == nullptr || payload.second < 1) { + os << ansi::red << "|Diag NACK "; + return os; + } os << ansi::red << "|Diag NACK " << static_cast(payload.first[0]); } else if (msg.getPayloadType() == DoIPPayloadType::AliveCheckRequest) { os << ansi::yellow << "|Alive Check?"; From bba891804a2405ce658dc826f537eba67947ec29 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Sun, 7 Dec 2025 11:05:07 +0100 Subject: [PATCH 17/17] refactor: Remove unused parameter documentation from ServerModelDownstreamResponseHandler --- inc/DoIPServerModel.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/inc/DoIPServerModel.h b/inc/DoIPServerModel.h index 28777fc..f4d5899 100644 --- a/inc/DoIPServerModel.h +++ b/inc/DoIPServerModel.h @@ -28,8 +28,6 @@ using ServerModelDiagnosticNotificationHandler = std::function;