diff --git a/CMakeLists.txt b/CMakeLists.txt index a8cd7ec..02a81e7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,7 @@ set(SOURCES src/Logger.cpp src/MacAddress.cpp src/DoIPDefaultConnection.cpp + src/uds/UdsMock.cpp ) diff --git a/examples/ExampleDoIPServerModel.h b/examples/ExampleDoIPServerModel.h index e22082d..9e4a0c2 100644 --- a/examples/ExampleDoIPServerModel.h +++ b/examples/ExampleDoIPServerModel.h @@ -7,35 +7,159 @@ #define EXAMPLEDOIPSERVERMODEL_H #include "DoIPServerModel.h" +#include "ThreadSafeQueue.h" +#include "uds/UdsMock.h" +#include "uds/UdsResponseCode.h" using namespace doip; class ExampleDoIPServerModel : public DoIPServerModel { public: ExampleDoIPServerModel() { - onCloseConnection = [](IConnectionContext &ctx, DoIPCloseReason reason) noexcept { + onOpenConnection = [this](IConnectionContext &ctx) noexcept { (void)ctx; + startWorker(); + }; + onCloseConnection = [this](IConnectionContext &ctx, DoIPCloseReason reason) noexcept { + (void)ctx; + stopWorker(); DOIP_LOG_WARN("Connection closed ({})", fmt::streamed(reason)); }; - onDiagnosticMessage = [](IConnectionContext &ctx, const DoIPMessage &msg) noexcept -> DoIPDiagnosticAck { + onDiagnosticMessage = [this](IConnectionContext &ctx, const DoIPMessage &msg) noexcept -> DoIPDiagnosticAck { (void)ctx; - DOIP_LOG_INFO("Received Diagnostic message (from ExampleDoIPServerModel)", fmt::streamed(msg)); + m_log->info("Received Diagnostic message (from ExampleDoIPServerModel)", fmt::streamed(msg)); // Example: Access payload using getPayload() - auto payload = msg.getDiagnosticMessagePayload(); - if (payload.second >= 3 && payload.first[0] == 0x22 && payload.first[1] == 0xF1 && payload.first[2] == 0x90) { - DOIP_LOG_INFO(" - Detected Read Data by Identifier for VIN (0xF190) -> send NACK"); - return DoIPNegativeDiagnosticAck::UnknownTargetAddress; - } + // auto payload = msg.getDiagnosticMessagePayload(); + // if (payload.second >= 3 && payload.first[0] == 0x22 && payload.first[1] == 0xF1 && payload.first[2] == 0x90) { + // m_log->info(" - Detected Read Data by Identifier for VIN (0xF190) -> send NACK"); + // return DoIPNegativeDiagnosticAck::UnknownTargetAddress; + // } return std::nullopt; }; - onDiagnosticNotification = [](IConnectionContext &ctx, DoIPDiagnosticAck ack) noexcept { + onDiagnosticNotification = [this](IConnectionContext &ctx, DoIPDiagnosticAck ack) noexcept { (void)ctx; - DOIP_LOG_INFO("Diagnostic ACK/NACK sent (from ExampleDoIPServerModel)", fmt::streamed(ack)); + m_log->info("Diagnostic ACK/NACK sent (from ExampleDoIPServerModel)", fmt::streamed(ack)); + }; + + onDownstreamRequest = [this](IConnectionContext &ctx, const DoIPMessage &msg, ServerModelDownstreamResponseHandler callback) noexcept { + (void)ctx; + (void)msg; + + m_log->info("Received downstream request (from ExampleDoIPServerModel)", fmt::streamed(msg)); + m_downstreamCallback = callback; + if (!m_downstreamCallback) { + m_log->error("onDownstreamRequest: No callback function passed"); + return DoIPDownstreamResult::Error; + } + + // Store message in send queue + auto [data, size] = msg.getDiagnosticMessagePayload(); + m_tx.push(ByteArray(data, size)); + m_log->info("Enqueued msg"); + return DoIPDownstreamResult::Pending; }; + + m_uds.registerDefaultServices(); + + m_uds.registerDiagnosticSessionControlHandler([this](uint8_t sessionType) { + m_loguds->info("Diagnostic Session Control requested, sessionType={:02X}", sessionType); + auto response = ByteArray{sessionType}; // Positive response SID = 0x50 + response.writeU16BE(m_p2_ms); + response.writeU16BE(m_p2star_10ms); + return std::make_pair(uds::UdsResponseCode::PositiveResponse, response); // Positive response + }); + + m_uds.registerECUResetHandler([this](uint8_t resetType) { + m_loguds->info("ECU Reset requested, resetType={:02X}", resetType); + return std::make_pair(uds::UdsResponseCode::PositiveResponse, ByteArray{resetType}); // Positive response SID = 0x61 + }); + + m_uds.registerReadDataByIdentifierHandler([this](uint16_t did) { + m_loguds->info("Read Data By Identifier requested, DID={:04X}", did); + if (did == 0xF190) { + // Return example VIN + ByteArray vinPayload = {'1', 'H', 'G', 'C', 'M', + '8', '2', '6', '3', '3', + 'A', '0', '0', '0', '0', '1', 'Z'}; + ByteArray response = {static_cast((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.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 + }); + } + + 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; + uds::UdsMock m_uds; + std::thread m_worker; + bool m_running = true; + uint16_t m_p2_ms = 1000; + uint16_t m_p2star_10ms = 200; + + + void startWorker() { + m_worker = std::thread([this] { + while (m_running) { + downstream_thread(); + } + }); + m_log->info("Started worker thread"); + } + void stopWorker() { + m_running = false; + if (m_worker.joinable()) + m_worker.join(); + m_log->info("Stopped worker thread"); + } + + /** + * @brief Thread simulating downstream communication (e. g. CAN). + */ + void downstream_thread() { + if (m_tx.size()) { + // simulate send. In a real environment we could send a CAN message + ByteArray req; + m_tx.pop(req); + m_log->info("Simulate send {}", fmt::streamed(req)); + // simulate some latency + std::this_thread::sleep_for(50ms); + // simulate receive + m_rx.push(m_uds.handleDiagnosticRequest(req)); + } + + if (m_rx.size()) { + ByteArray rsp; + m_rx.pop(rsp); + m_log->info("Simulate receive {}", fmt::streamed(rsp)); + if (m_downstreamCallback) { + m_downstreamCallback(rsp, DoIPDownstreamResult::Handled); + m_downstreamCallback = nullptr; + } + } + std::this_thread::sleep_for(10ms); } }; 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(); 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/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/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/inc/DoIPMessage.h b/inc/DoIPMessage.h index a141afb..8271a58 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,19 @@ 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::RoutingActivationResponse && 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 +297,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 +325,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 +368,7 @@ class DoIPMessage { return msg; } -protected: + protected: ByteArray m_data; ///< Complete message data (header + payload) /** @@ -384,7 +390,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()); @@ -427,201 +433,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.writeU16BE(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.writeU16BE(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.writeU16BE(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 +639,59 @@ 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(); + 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?"; + } else if (msg.getPayloadType() == DoIPPayloadType::AliveCheckResponse) { + auto sa = msg.getSourceAddress(); + 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(); + 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; 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/inc/DoIPServerModel.h b/inc/DoIPServerModel.h index 4041f69..f4d5899 100644 --- a/inc/DoIPServerModel.h +++ b/inc/DoIPServerModel.h @@ -23,6 +23,14 @@ 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 + */ +using ServerModelDownstreamResponseHandler = std::function; + /** * @brief Callback for downstream (subnet) request handling * @@ -37,25 +45,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,16 +90,8 @@ 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); + DoIPAddress serverAddress = DoIPAddress(0x0E00); /** * @brief Check if downstream forwarding is enabled @@ -145,18 +135,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/inc/uds/IUdsServiceHandler.h b/inc/uds/IUdsServiceHandler.h new file mode 100644 index 0000000..a1d912c --- /dev/null +++ b/inc/uds/IUdsServiceHandler.h @@ -0,0 +1,40 @@ +#ifndef IUDSSERVICEHANDLER_H +#define IUDSSERVICEHANDLER_H + +#include "UdsResponseCode.h" +#include "DoIPMessage.h" +#include + +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; + 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..a8dbbfe 100644 --- a/inc/uds/UdsMock.h +++ b/inc/uds/UdsMock.h @@ -1,97 +1,115 @@ #ifndef UDSMOCK_H #define UDSMOCK_H +#include #include +#include #include #include "DoIPMessage.h" +#include "IUdsServiceHandler.h" +#include "LambdaUdsHandler.h" #include "UdsResponseCode.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 - */ - ByteArray handleDiagnosticRequest(const ByteArray &request) { - // Simple echo behavior for testing - auto response = m_serviceHandler(request); - return makeResponse(request, response.first, response.second); + 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); + + // 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) const; + + // 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 = {}) { + 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(static_cast(responseCode)); // NRC + 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 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/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 3615ad9..3c2a47e 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,55 @@ 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) { + 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; +} } // namespace doip::uds 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 a6e4b00..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 { @@ -182,23 +181,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..be90ad6 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) { @@ -250,20 +250,33 @@ 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); - // } - // } + + // 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); + } 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 +301,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; } } @@ -298,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) { @@ -309,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: @@ -352,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) { @@ -388,6 +398,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,22 +428,28 @@ 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(); + 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 diff --git a/src/uds/UdsMock.cpp b/src/uds/UdsMock.cpp new file mode 100644 index 0000000..97402dd --- /dev/null +++ b/src/uds/UdsMock.cpp @@ -0,0 +1,78 @@ +#include "uds/UdsMock.h" +#include "DoIPMessage.h" + +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]; + return handler(sessionType); + }); +} + +void UdsMock::registerECUResetHandler(std::function handler) { + registerService(UdsService::ECUReset, [handler = std::move(handler)](const ByteArray &req) -> UdsResponse { + 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 { + 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 { + uint8_t subFunction = req[1]; + return handler(subFunction); + }); +} + +} // namespace doip::uds 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 diff --git a/test/scripts/change-vin-and-reset.py b/test/scripts/change-vin-and-reset.py index 1997e5a..a1cf291 100644 --- a/test/scripts/change-vin-and-reset.py +++ b/test/scripts/change-vin-and-reset.py @@ -4,18 +4,36 @@ 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(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.write_data_by_identifier(udsoncan.DataIdentifier.VIN, 'ABC123456789') # Standard ID for VIN is 0xF190. Codec is set in the client configuration + 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 + # Read VIN once and handle response type (udsoncan may return bytes or str) + vin_response = client.read_data_by_identifier(udsoncan.DataIdentifier.VIN) + 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 except NegativeResponseException as e: diff --git a/test/uds/UdsMock_Test.cpp b/test/uds/UdsMock_Test.cpp index 756d196..75bf34a 100644 --- a/test/uds/UdsMock_Test.cpp +++ b/test/uds/UdsMock_Test.cpp @@ -1,18 +1,19 @@ -#include #include "MacAddress.h" -#include -#include +#include #include +#include #include +#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") { - uds::UdsMock udsMock; + TEST_CASE("UdsMock default behavior returns ServiceNotSupported focus") { + UdsMock udsMock; ByteArray request = {0x10, 0x01}; // Example UDS request (Diagnostic Session Control) ByteArray response = udsMock.handleDiagnosticRequest(request); @@ -20,21 +21,61 @@ TEST_SUITE("UdsMock") { // Expected negative response: 0x7F, 0x10, NRC for ServiceNotSupported (0x11) ByteArray expectedResponse = {0x7F, 0x10, 0x11}; + INFO(response); CHECK(response == expectedResponse); } 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 - }); + 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], 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 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); + }); + 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}; + + 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); + } }