diff --git a/.circleci/config.yml b/.circleci/config.yml index 14ccfa198..29f44677b 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -105,6 +105,12 @@ jobs: cd examples/linux_server_0x27 ./test.sh + - run: + name: Build and test DoIP client example + command: | + cd examples/doip_client + ./test.sh + workflows: version: 2 build: diff --git a/.gitignore b/.gitignore index 8674b9637..7e0355fc3 100644 --- a/.gitignore +++ b/.gitignore @@ -40,3 +40,6 @@ cppcheck_reports .venv tools/cppcheck/misra_c_2023__headlines_for_cppcheck.txt latex/ +# Ignore binaries of doip example +examples/doip_client/doip_server +examples/doip_client/doip_client diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 000000000..3bd3aeba0 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.15) +project(iso14229) + +# Options for each transport layer variant +option(BUILD_UDS_TP_ISOTP_C "Build ISO-TP C variant" OFF) +option(BUILD_UDS_TP_ISOTP_C_SOCKETCAN "Build ISO-TP C SocketCAN variant" OFF) +option(BUILD_UDS_TP_ISOTP_SOCK "Build ISO-TP socket variant" ON) +option(BUILD_UDS_TP_ISOTP_MOCK "Build ISO-TP mock variant" OFF) +option(BUILD_UDS_TP_DOIP "Build DoIP (ISO 13400) variant" OFF) + +# Populate VARIANT_LIST based on enabled options +set(VARIANT_LIST) +if(BUILD_UDS_TP_ISOTP_C) + list(APPEND VARIANT_LIST UDS_TP_ISOTP_C) +endif() +if(BUILD_UDS_TP_ISOTP_C_SOCKETCAN) + list(APPEND VARIANT_LIST UDS_TP_ISOTP_C_SOCKETCAN) +endif() +if(BUILD_UDS_TP_ISOTP_SOCK) + list(APPEND VARIANT_LIST UDS_TP_ISOTP_SOCK) +endif() +if(BUILD_UDS_TP_ISOTP_MOCK) + list(APPEND VARIANT_LIST UDS_TP_ISOTP_MOCK) +endif() +if(BUILD_UDS_TP_DOIP) + list(APPEND VARIANT_LIST UDS_TP_DOIP) +endif() + +# Create a library target for each enabled variant +foreach(VARIANT IN LISTS VARIANT_LIST) + # Remove UDS_TP_ prefix and convert to lowercase + string(REPLACE "UDS_TP_" "" VARIANT_SUFFIX ${VARIANT}) + string(TOLOWER ${VARIANT_SUFFIX} VARIANT_SUFFIX_LOWER) + + # Define target name + set(TARGET_NAME "iso14229_${VARIANT_SUFFIX_LOWER}") + + # Create library target + add_library(${TARGET_NAME} iso14229.c) + + # Set compile definition for this variant + target_compile_definitions(${TARGET_NAME} PRIVATE ${VARIANT}) + + # When building DoIP, also compile the transport instances (TCP/UDP) + if (VARIANT STREQUAL "UDS_TP_DOIP") + target_sources(${TARGET_NAME} PRIVATE + src/tp/doip/doip_tp_tcp.c + src/tp/doip/doip_tp_udp.c + ) + endif() + + message(STATUS "Configured target: ${TARGET_NAME} with variant: ${VARIANT}") +endforeach() + + diff --git a/docs/client_doip.md b/docs/client_doip.md new file mode 100644 index 000000000..87a2cb23c --- /dev/null +++ b/docs/client_doip.md @@ -0,0 +1,155 @@ +# DoIP Client {#client_doip} + +This document describes the DoIP (ISO 13400) client and transport usage in this codebase, focusing on TCP diagnostics, UDP vehicle discovery, and the DoIP transport layer. + +## Basic Usage + +### Discovery and Selection + +Use UDP discovery to locate vehicles and select one based on VIN or first responder. + +```c +#include "tp/doip/doip_client.h" + +DoIPClient_t tp; +UDSDoIPSetDiscoveryOptions(/*request_only=*/true, /*dump_raw=*/false); +UDSDoIPSetSelectionCallback(&tp, /*optional*/ NULL, NULL); + +// Loopback discovery (binds 13401, sends VI request to 13400) +int found = UDSDoIPDiscoverVehicles(&tp, /*timeout_ms=*/2000, /*loopback=*/true); +if (found <= 0 || tp.server_ip[0] == '\0') { + // handle no selection +} + +// Selected server is available in tp.server_ip and tp.server_port +``` + +To filter by VIN prefix: + +```c +static bool select_by_vin(const DoIPDiscoveryInfo *info, void *user) { + const char *prefix = (const char*)user; + if (!prefix || !*prefix || info->vin[0] == '\0') return false; + return strncmp(info->vin, prefix, strlen(prefix)) == 0; +} + +UDSDoIPSetSelectionCallback(&tp, select_by_vin, (void*)"WVWZZZ"); +int found = UDSDoIPDiscoverVehicles(&tp, 2000, true); +``` + +### Initialization and Routing Activation + +After discovery, initialize the TCP connection and activate routing: + +```c +// Provide source/target logical addresses +uint16_t SA = 0x0E00; // example tester address +uint16_t TA = 0x0E80; // example server address + +UDSErr_t rc = UDSDoIPInitClient(&tp, tp.server_ip, tp.server_port, SA, TA); +if (rc != UDS_OK) { + // handle error +} + +// If needed later: +rc = UDSDoIPActivateRouting(&tp); +``` + +### Sending and Receiving Diagnostic Messages + +Once routing is active, the higher-level UDS client can use the DoIP transport (via `UDSTp_t` inside `DoIPClient_t`) to send UDS requests and receive responses. See the generic client flow in [docs/client.md](client.md) and the DoIP transport integration in [src/tp/doip/doip_client.c](src/tp/doip/doip_client.c). + +## DoIP Client Structure + +`DoIPClient_t` holds TCP/UDP transports, state, addresses, buffers, and flags. Relevant fields: + +- `tcp`: DoIP TCP transport for diagnostics +- `udp`: DoIP UDP transport for discovery +- `source_address`, `target_address`: logical addresses +- `server_ip`, `server_port`: selected server endpoint +- `uds_response`/`uds_response_len`: buffered diagnostic response + +See [src/tp/doip/doip_client.h](src/tp/doip/doip_client.h). + +## Transport Layer (DoIP) + +The DoIP transport abstraction is defined in [src/tp/doip/doip_transport.h](src/tp/doip/doip_transport.h). + +- `DoIPTransport`: + - `fd`, `port`, `ip`, `is_udp`, `loopback` + - `connect_timeout_ms`, `send_timeout_ms` +- TCP helpers: + - `doip_tp_tcp_init()`, `doip_tp_tcp_connect()`, `doip_tp_tcp_send()`, `doip_tp_tcp_recv()`, `doip_tp_tcp_close()` +- UDP helpers: + - `doip_tp_udp_init()`, `doip_tp_udp_join_default_multicast()`, + `doip_tp_udp_recv()`, `doip_tp_udp_recvfrom()`, `doip_tp_udp_sendto()`, `doip_tp_udp_close()` + +### Non-Blocking Behavior + +- Sockets are set to non-blocking (`O_NONBLOCK`). +- `select()` is used for readiness and timeouts. +- TCP connect: non-blocking with `EINPROGRESS` handling (select on writable + `SO_ERROR`). See [src/tp/doip/doip_tp_tcp.c](src/tp/doip/doip_tp_tcp.c). +- TCP send: send-all loop with timeout (`MSG_NOSIGNAL` used when available). Partial writes are handled until the buffer is sent or timeout. See [src/tp/doip/doip_tp_tcp.c](src/tp/doip/doip_tp_tcp.c). +- TCP/UDP recv: `select()` for readability then a single `recv()`/`recvfrom()`. + +### Ports and Constants + +Defined in [src/tp/doip/doip_defines.h](src/tp/doip/doip_defines.h): + +- `DOIP_TCP_PORT` = 13400 +- `DOIP_UDP_DISCOVERY_PORT` = 13400 (vehicle side) +- `DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT` = 13401 (tester side) +- `DOIP_DEFAULT_TIMEOUT_MS` = 5000 + +UDP discovery binds to 13401 by default (tester request port). Active VI requests are sent to 13400. + +### Configuring Timeouts + +```c +DoIPTransport *tcp = &tp.tcp; +// Override defaults per transport +doip_tp_set_timeouts(tcp, /*connect_timeout_ms=*/2000, /*send_timeout_ms=*/1000); +``` + +## UDP Discovery Details + +Implemented in [src/tp/doip/doip_client.c](src/tp/doip/doip_client.c) and [src/tp/doip/doip_tp_udp.c](src/tp/doip/doip_tp_udp.c): + +- Bind (loopback): `127.0.0.1:13401` to receive announcements/responses. +- Bind (non-loopback): `INADDR_ANY:13401` with `SO_BROADCAST` enabled. +- Active VI Request: `doip_tp_udp_sendto()` sends a header-only VI request (payload type 0x0001, length 0) to `13400`. +- Optional multicast join (non-loopback): `224.224.224.224` when not in request-only mode. +- VIN/EID/GID parsing: best-effort extraction from vehicle identification payloads. + +## Example: Discovery CLI + +See the example in [examples/doip_discovery_example/doip_discover.c](examples/doip_discovery_example/doip_discover.c): + +- Loopback discover first responder: +```bash +(cd examples/doip_discovery_example && ./doip_discovery loopback) +``` +- Filter by VIN prefix: +```bash +(cd examples/doip_discovery_example && ./doip_discovery WVWZZZ loopback) +``` +- Options: + - `--request-only`: send VI requests, skip multicast + - `--raw`: dump raw frames via logger + +## Configuration {#doip_configuration} + +Compile-time and runtime: + +- `DOIP_DEFAULT_TIMEOUT_MS` (compile-time): select timeouts default +- `doip_tp_set_timeouts()` (runtime): per-transport connect/send timeouts (TCP) +- `UDSDoIPSetDiscoveryOptions(request_only, dump_raw)` (runtime): discovery behavior + +## See Also + +- DoIP client implementation: [src/tp/doip/doip_client.c](src/tp/doip/doip_client.c) +- DoIP transport API: [src/tp/doip/doip_transport.h](src/tp/doip/doip_transport.h) +- TCP transport: [src/tp/doip/doip_tp_tcp.c](src/tp/doip/doip_tp_tcp.c) +- UDP transport: [src/tp/doip/doip_tp_udp.c](src/tp/doip/doip_tp_udp.c) +- DoIP constants: [src/tp/doip/doip_defines.h](src/tp/doip/doip_defines.h) +- Generic client guide: [docs/client.md](client.md) diff --git a/docs/mainpage.md b/docs/mainpage.md index 0dbeacdba..90609313e 100644 --- a/docs/mainpage.md +++ b/docs/mainpage.md @@ -1,6 +1,6 @@ # iso14229 {#mainpage} -iso14229 is a UDS (ISO14229) library for writing servers and clients. +iso14229 is a UDS (ISO14229) library for writing servers and clients. **Source Code:** https://github.com/driftregion/iso14229 @@ -23,6 +23,7 @@ To access the examples, clone or download the repository from https://github.com | \ref examples/arduino_server/README.md "arduino_server" | Arduino server | | \ref examples/esp32_server/README.md "esp32_server" | ESP32 server | | \ref examples/s32k144_server/README.md "s32k144_server" | NXP S32K144 server | +| \ref examples/doip_client/README.md "doip_client" | DoIP client | --- @@ -44,6 +45,7 @@ Configure the library at compilation time with preprocessor defines: | **isotp_c_socketcan** | `-DUDS_TP_ISOTP_C_SOCKETCAN` | isotp-c over SocketCAN | Linux newer than 2.6.25 | \ref examples/linux_server_0x27/README.md "linux_server_0x27" | | **isotp_c** | `-DUDS_TP_ISOTP_C` | Software ISO-TP | Everything else | \ref examples/arduino_server/README.md "arduino_server" \ref examples/esp32_server/README.md "esp32_server" \ref examples/s32k144_server/README.md "s32k144_server" | | **isotp_mock** | `-DUDS_TP_ISOTP_MOCK` | In-memory transport for testing | platform-independent unit tests | see unit tests | +| **doip** | `-DUDS_TP_DOIP` | DoIP (iso 13400) client | platform-independent unit tests | \ref examples/linux_rdbi_wdbi/README.md | ### System Selection Override diff --git a/examples/doip_client/Makefile b/examples/doip_client/Makefile new file mode 100644 index 000000000..faf439f3f --- /dev/null +++ b/examples/doip_client/Makefile @@ -0,0 +1,21 @@ +CLI_SRCS += iso14229.c doip_test_client.c +CLI_HDRS += iso14229.h +CLI_TARGET = doip_client + +SVR_SRCS = doip_server.c doip_test_server.c +SVR_TARGET = doip_server + +# Use the verbose log level for debugging +# CFLAGS = -DUDS_TP_DOIP=1 -DUDS_CONFIG_LOG_COLORS=1 -DUDS_LOG_LEVEL=UDS_LOG_VERBOSE -g -Wall -Wpedantic -Wextra +CFLAGS = -DUDS_TP_DOIP=1 -DUDS_CONFIG_LOG_COLORS=1 -DUDS_LOG_LEVEL=UDS_LOG_INFO -g -Wall -Wpedantic -Wextra + +all: client server + +client: $(CLI_SRCS) $(CLI_HDRS) + $(CC) $(CFLAGS) $(CLI_SRCS) -o $(CLI_TARGET) + +server: $(SVR_SRCS) + $(CC) $(CFLAGS) $(SVR_SRCS) -o $(SVR_TARGET) + +clean: + rm -f $(CLI_TARGET) $(SVR_TARGET) diff --git a/examples/doip_client/README.md b/examples/doip_client/README.md new file mode 100644 index 000000000..086601e4b --- /dev/null +++ b/examples/doip_client/README.md @@ -0,0 +1,246 @@ +# DoIP Client/Server Example + +This example demonstrates the **DoIP (Diagnostic over IP - ISO 13400)** transport layer implementation for UDS (ISO 14229) using TCP. + +## Overview + +The example consists of: + +- **DoIP Server** (`doip_test_server.c`) - Receives UDS diagnostic requests via DoIP and sends responses +- **DoIP Client** (`doip_test_client.c`) - Sends UDS diagnostic requests via DoIP and receives responses +- **Test Script** (`test.sh`) - Automated integration test + +## Features + +### Client + +- Connects to DoIP server via TCP +- Performs routing activation +- Executes UDS diagnostic services (took example from `examples/linux_rdbi_wdbi`): + - **ReadDataByIdentifier (0x22)** - Reads DID 0xF190 + - **WriteDataByIdentifier (0x2E)** - Writes incremented value to DID 0xF190 +- Demonstrates async request/response handling + +### Server + +- Listens on TCP port 13400 (standard DoIP port) +- UDP discovery is not supported (and not needed) +- Handles routing activation requests +- Processes UDS diagnostic messages: + - ReadDataByIdentifier (0x22) + - WriteDataByIdentifier (0x2E) + - DiagnosticSessionControl (0x10) +- Sends DoIP diagnostic message ACK/NACK +- Responds to alive check requests + +## Building + +```bash +make +``` + +This builds two executables: + +- `doip_server` - DoIP server +- `doip_client` - DoIP client + +## Running + +### Manual Test + +Start the server in one terminal: + +```bash +./doip_server +``` + +Run the client in another terminal: + +```bash +./doip_client +``` + +The client will: + +1. Connect to server at `127.0.0.1:13400` +2. Activate routing +3. Read DID 0xF190 +4. Write DID 0xF190 with incremented value +5. Exit + +### Automated Test + +```bash +./test.sh +``` + +The test script: + +1. Builds both client and server +2. Starts server in background +3. Runs client and waits for completion +4. Verifies client exited successfully (exit code 0) +5. Verifies server is still running +6. Cleans up + +## Configuration + +Edit `Makefile` to adjust log level: + +```makefile +# Debug build with verbose logging +CFLAGS = -DUDS_TP_DOIP=1 -DUDS_LOG_LEVEL=UDS_LOG_VERBOSE + +# Release build with info logging +CFLAGS = -DUDS_TP_DOIP=1 -DUDS_LOG_LEVEL=UDS_LOG_INFO +``` + +## Network Details + +- **Protocol**: DoIP over TCP (ISO 13400) +- **Port**: 13400 +- **Server IP**: 127.0.0.1 (localhost) +- **Source Address**: 0x0E00 (client logical address) +- **Target Address**: 0x4001 (server logical address) + +## DoIP Message Flow + +```text +Client Server + | | + |------- TCP Connect ---------->| + | | + |--- Routing Activation Req --->| + |<-- Routing Activation Res ----| + | | + |--- Diagnostic Message ------->| + |<-- Diagnostic Message ACK ----| + |<-- Diagnostic Response -------| + | | + |--- Diagnostic Message ------->| + |<-- Diagnostic Message ACK ----| + |<-- Diagnostic Response -------| + | | + |------- TCP Close ------------>| +``` + +## Troubleshooting + +**Server won't start:** + +- Check if port 13400 is already in use: `netstat -tuln | grep 13400` +- Kill any existing server: `pkill doip_server` + +**Client can't connect:** + +- Verify server is running: `ps aux | grep doip_server` +- Check firewall settings +- Use `tcpdump` to inspect traffic: `sudo tcpdump -i lo port 13400 -X` + +**Test script fails:** + +- Check exit codes in test output +- Run with verbose logging (edit Makefile) +- Run server and client manually to isolate issues + +## References + +- ISO 13400: Road vehicles - Diagnostic communication over Internet Protocol (DoIP) +- ISO 14229: Unified Diagnostic Services (UDS) + +--- + +## New Modules and Functions (DoIP Transport & Discovery) + +This codebase now includes a small DoIP transport abstraction, a split TCP/UDP implementation, and basic UDP discovery with a selection callback. + +- Transport Abstraction + - File: [src/tp/doip/doip_transport.h](../../src/tp/doip/doip_transport.h) + - `DoIPTransport`: minimal socket wrapper used by the client (fields: `fd`, `port`, `ip`, `is_udp`, `loopback`). + - TCP Transport: [src/tp/doip/doip_tp_tcp.c](../../src/tp/doip/doip_tp_tcp.c) + - `doip_tp_tcp_init(t, ip, port)`: initialize transport with remote IP/port. + - `doip_tp_tcp_connect(t)`: connect to DoIP TCP server. + - `doip_tp_tcp_send(t, buf, len)`: send bytes. + - `doip_tp_tcp_recv(t, buf, len, timeout_ms)`: receive with timeout. + - `doip_tp_tcp_close(t)`: close socket. + - UDP Transport: [src/tp/doip/doip_tp_udp.c](../../src/tp/doip/doip_tp_udp.c) + - `doip_tp_udp_init(t, port, loopback)`: bind for discovery (loopback binds 127.0.0.1). + - `doip_tp_udp_join_default_multicast(t)`: join 224.224.224.224:13400. + - `doip_tp_udp_recv(t, buf, len, timeout_ms)`: receive datagram. + - `doip_tp_udp_recvfrom(t, buf, len, timeout_ms, src_ip, src_ip_sz, src_port)`: receive + source address. + - `doip_tp_udp_close(t)`: close socket. + +- DoIP Client Refactor + - Files: [src/tp/doip/doip_client.h](../../src/tp/doip/doip_client.h), [src/tp/doip/doip_client.c](../../src/tp/doip/doip_client.c) + - The client now embeds `DoIPTransport` for TCP (diagnostics) and UDP (discovery) and uses these for connect/send/recv/close. + - Field `udp_loopback` controls whether discovery uses loopback instead of multicast. + +- Discovery & Server Selection + - Types (in client header): + - `DoIPDiscoveryInfo`: `{ ip, remote_port, logical_address, vin[18], eid[13], gid[13] }`. + - `DoIPSelectServerFn`: `bool (*)(const DoIPDiscoveryInfo*, void*)` callback. + - APIs: + - `UDSDoIPSetSelectionCallback(tp, fn, user)`: register callback to choose a server from responders. + - `UDSDoIPDiscoverVehicles(tp, timeout_ms, loopback)`: listens for discovery frames (multicast or loopback), parses VIN/EID/GID when present, invokes selection callback (or defaults to first responder), and updates `tp->server_ip`/`tp->server_port` on selection. + - Notes: + - Default DoIP multicast group/port: 224.224.224.224:13400. + - Default selection without a callback picks the first responder; prefer a callback to filter by VIN/logical address/EID/GID. + +- Example + - File: [examples/doip_discovery_example/main.c](../doip_discovery_example/main.c) + - Demonstrates discovery and selection via VIN prefix. Prints the selected server IP. + +- Build Integration + - CMake: when `BUILD_UDS_TP_DOIP=ON`, the DoIP target includes the new TCP/UDP transport sources. + - Bazel: `src/BUILD` updated to include the new transport sources and headers. + + ### Quick Start: Discovery + Selection + + Minimal example of discovering DoIP responders and selecting one by VIN prefix: + + ```c + #include + #include + #include + #include + #include "tp/doip/doip_client.h" + + static bool select_by_vin(const DoIPDiscoveryInfo *info, void *user) { + const char *needle = (const char *)user; // VIN prefix or full VIN + if (!needle || !*needle) return false; + if (info->vin[0] == '\0') return false; + return strncmp(info->vin, needle, strlen(needle)) == 0; + } + + int main(int argc, char **argv) { + const char *vin_prefix = argc > 1 ? argv[1] : NULL; + bool loopback = argc > 2 ? (strcmp(argv[2], "loopback") == 0) : false; + + DoIPClient_t tp; + memset(&tp, 0, sizeof(tp)); + + UDSDoIPSetSelectionCallback(&tp, select_by_vin, (void*)vin_prefix); + + int count = UDSDoIPDiscoverVehicles(&tp, 2000, loopback); + printf("Discovered %d responders\n", count); + printf("Selected server: %s\n", tp.server_ip); + return 0; + } + ``` + + Build and run (ad hoc): + + ```bash + gcc -DUDS_TP_DOIP -Isrc -I. -o build/doip_discovery_example \ + src/tp/doip/doip_client.c src/tp/doip/doip_tp_udp.c src/tp/doip/doip_tp_tcp.c \ + examples/doip_discovery_example/main.c + + # Default multicast discovery (~2s), chooses first responder + ./build/doip_discovery_example + + # Filter by VIN prefix + ./build/doip_discovery_example WBA + + # Loopback discovery for local testing + ./build/doip_discovery_example "" loopback + ``` diff --git a/examples/doip_client/doip_server.c b/examples/doip_client/doip_server.c new file mode 100644 index 000000000..83449b589 --- /dev/null +++ b/examples/doip_client/doip_server.c @@ -0,0 +1,529 @@ +/** + * @file doip_server.c + * @brief ISO 13400 (DoIP) Transport Layer - Server Implementation + * @details DoIP TCP transport layer server for UDS (ISO 14229) + * + * This implementation provides a DoIP transport layer for the iso14229 library. + * It implements the minimal DoIP protocol features required for diagnostic communication: + * - TCP-based communication on port 13400 + * - Routing activation + * - Diagnostic message handling + * - Alive check + * + * @note This is a simplified implementation focusing on TCP diagnostic messages. + * UDP vehicle discovery and other advanced features are not included. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* DoIP Protocol Constants */ +#define DOIP_PROTOCOL_VERSION 0x03 +#define DOIP_PROTOCOL_VERSION_INV 0xFC +#define DOIP_TCP_PORT 13400 +#define DOIP_HEADER_SIZE 8 + +/* DoIP Payload Types */ +#define DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_REQ 0x0005 +#define DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_RES 0x0006 +#define DOIP_PAYLOAD_TYPE_ALIVE_CHECK_REQ 0x0007 +#define DOIP_PAYLOAD_TYPE_ALIVE_CHECK_RES 0x0008 +#define DOIP_PAYLOAD_TYPE_DIAG_MESSAGE 0x8001 +#define DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_POS_ACK 0x8002 +#define DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_NEG_ACK 0x8003 + +/* DoIP Routing Activation Response Codes */ +#define DOIP_ROUTING_ACTIVATION_RES_SUCCESS 0x10 +#define DOIP_ROUTING_ACTIVATION_RES_UNKNOWN_SA 0x00 +#define DOIP_ROUTING_ACTIVATION_RES_ALREADY_ACTIVE 0x01 + +/* DoIP Diagnostic Message NACK Codes */ +#define DOIP_DIAG_NACK_INVALID_SA 0x02 +#define DOIP_DIAG_NACK_UNKNOWN_TA 0x03 +#define DOIP_DIAG_NACK_MESSAGE_TOO_LARGE 0x04 +#define DOIP_DIAG_NACK_OUT_OF_MEMORY 0x05 +#define DOIP_DIAG_NACK_TARGET_UNREACHABLE 0x06 + +/* Configuration */ +#define DOIP_MAX_CLIENTS 8 +#define DOIP_BUFFER_SIZE 4096 +#define DOIP_LOGICAL_ADDRESS_SERVER 0x0001 +#define DOIP_ROUTING_ACTIVATION_TYPE 0x00 + +/* DoIP Header Structure */ +typedef struct { + uint8_t protocol_version; + uint8_t protocol_version_inv; + uint16_t payload_type; + uint32_t payload_length; +} __attribute__((packed)) DoIPHeader_t; + +/* Client Connection State */ +typedef struct { + int socket_fd; + bool active; + bool routing_activated; + uint16_t source_address; + uint8_t rx_buffer[DOIP_BUFFER_SIZE]; + size_t rx_offset; + uint8_t tx_buffer[DOIP_BUFFER_SIZE]; + size_t tx_offset; +} DoIPClientConnection_t; + +/* DoIP Server Context */ +typedef struct { + int listen_socket; + uint16_t logical_address; + DoIPClientConnection_t clients[DOIP_MAX_CLIENTS]; + + /* UDS integration callbacks */ + void (*on_diag_message)(uint16_t source_addr, const uint8_t *data, size_t len); + void *user_data; +} DoIPServer_t; + +/* Static server instance */ +static DoIPServer_t server = {0}; + +/** + * @brief Create and initialize DoIP header + */ +static void doip_header_init(DoIPHeader_t *header, uint16_t payload_type, uint32_t payload_length) { + header->protocol_version = DOIP_PROTOCOL_VERSION; + header->protocol_version_inv = DOIP_PROTOCOL_VERSION_INV; + header->payload_type = htons(payload_type); + header->payload_length = htonl(payload_length); +} + +/** + * @brief Parse DoIP header from buffer + */ +static bool doip_header_parse(const uint8_t *buffer, DoIPHeader_t *header) { + if (!buffer || !header) { + return false; + } + + memcpy(header, buffer, sizeof(DoIPHeader_t)); + header->payload_type = ntohs(header->payload_type); + header->payload_length = ntohl(header->payload_length); + + /* Validate protocol version */ + if (header->protocol_version != DOIP_PROTOCOL_VERSION || + header->protocol_version_inv != DOIP_PROTOCOL_VERSION_INV) { + return false; + } + + return true; +} + +/** + * @brief Send DoIP message to client + */ +static int doip_send_message(DoIPClientConnection_t *client, uint16_t payload_type, + const uint8_t *payload, uint32_t payload_len) { + uint8_t buffer[DOIP_BUFFER_SIZE]; + DoIPHeader_t *header = (DoIPHeader_t *)buffer; + + if (DOIP_HEADER_SIZE + payload_len > DOIP_BUFFER_SIZE) { + return -1; + } + + doip_header_init(header, payload_type, payload_len); + + if (payload && payload_len > 0) { + memcpy(buffer + DOIP_HEADER_SIZE, payload, payload_len); + } + + ssize_t sent = send(client->socket_fd, buffer, DOIP_HEADER_SIZE + payload_len, 0); + if (sent < 0) { + perror("send"); + return -1; + } + + return 0; +} + +/** + * @brief Handle routing activation request + */ +static void doip_handle_routing_activation(DoIPClientConnection_t *client, + const uint8_t *payload, uint32_t payload_len) { + if (payload_len < 7) { + return; + } + + uint16_t source_address = (payload[0] << 8) | payload[1]; + uint8_t activation_type = payload[2]; + (void)activation_type; /* Currently unused, reserved for future use */ + + /* Build routing activation response */ + uint8_t response[13]; + response[0] = (source_address >> 8) & 0xFF; /* Client source address */ + response[1] = source_address & 0xFF; + response[2] = (server.logical_address >> 8) & 0xFF; /* Server logical address */ + response[3] = server.logical_address & 0xFF; + response[4] = DOIP_ROUTING_ACTIVATION_RES_SUCCESS; /* Response code */ + response[5] = 0x00; /* Reserved */ + response[6] = 0x00; + response[7] = 0x00; + response[8] = 0x00; + + /* Optional: OEM specific data */ + response[9] = 0x00; + response[10] = 0x00; + response[11] = 0x00; + response[12] = 0x00; + + client->routing_activated = true; + client->source_address = source_address; + + doip_send_message(client, DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_RES, response, 13); + + printf("DoIP-server: Routing activated for SA=0x%04X\n", source_address); +} + +/** + * @brief Handle alive check request + */ +static void doip_handle_alive_check(DoIPClientConnection_t *client) { + uint8_t response[2]; + response[0] = (server.logical_address >> 8) & 0xFF; + response[1] = server.logical_address & 0xFF; + + doip_send_message(client, DOIP_PAYLOAD_TYPE_ALIVE_CHECK_RES, response, 2); +} + +/** + * @brief Handle diagnostic message + */ +static void doip_handle_diag_message(DoIPClientConnection_t *client, + const uint8_t *payload, uint32_t payload_len) { + if (!client->routing_activated) { + /* Send NACK - routing not activated */ + uint8_t nack[5] = {0}; + nack[0] = (client->source_address >> 8) & 0xFF; + nack[1] = client->source_address & 0xFF; + nack[2] = (server.logical_address >> 8) & 0xFF; + nack[3] = server.logical_address & 0xFF; + nack[4] = DOIP_DIAG_NACK_TARGET_UNREACHABLE; + doip_send_message(client, DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_NEG_ACK, nack, 5); + return; + } + + if (payload_len < 4) { + return; + } + + uint16_t source_address = (payload[0] << 8) | payload[1]; + uint16_t target_address = (payload[2] << 8) | payload[3]; + + /* Verify target address matches our logical address */ + if (target_address != server.logical_address) { + uint8_t nack[5]; + nack[0] = (source_address >> 8) & 0xFF; + nack[1] = source_address & 0xFF; + nack[2] = (target_address >> 8) & 0xFF; + nack[3] = target_address & 0xFF; + nack[4] = DOIP_DIAG_NACK_UNKNOWN_TA; + doip_send_message(client, DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_NEG_ACK, nack, 5); + printf("DoIP-server: Diagnostic message with unknown TA=0x%04X, expected 0x%04X\n", target_address, server.logical_address); + return; + } + + /* Send positive ACK */ + uint8_t ack[5]; + ack[0] = (source_address >> 8) & 0xFF; + ack[1] = source_address & 0xFF; + ack[2] = (target_address >> 8) & 0xFF; + ack[3] = target_address & 0xFF; + ack[4] = 0x00; /* ACK code */ + doip_send_message(client, DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_POS_ACK, ack, 5); + + /* Pass UDS data to application callback */ + if (server.on_diag_message && payload_len > 4) { + const uint8_t *uds_data = payload + 4; + size_t uds_len = payload_len - 4; + server.on_diag_message(source_address, uds_data, uds_len); + } +} + +/** + * @brief Process received DoIP message + */ +static void doip_process_message(DoIPClientConnection_t *client, + const DoIPHeader_t *header, + const uint8_t *payload) { + printf("DoIP-server: Received payload type 0x%04X\n", header->payload_type); + switch (header->payload_type) { + case DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_REQ: + doip_handle_routing_activation(client, payload, header->payload_length); + break; + + case DOIP_PAYLOAD_TYPE_ALIVE_CHECK_REQ: + doip_handle_alive_check(client); + break; + + case DOIP_PAYLOAD_TYPE_DIAG_MESSAGE: + doip_handle_diag_message(client, payload, header->payload_length); + break; + + default: + printf("DoIP-server: Unknown payload type 0x%04X\n", header->payload_type); + break; + } +} + +/** + * @brief Handle client data reception + */ +static void doip_handle_client_rx(DoIPClientConnection_t *client) { + ssize_t bytes_read = recv(client->socket_fd, + client->rx_buffer + client->rx_offset, + DOIP_BUFFER_SIZE - client->rx_offset, 0); + + if (bytes_read <= 0) { + if (bytes_read == 0) { + printf("DoIP-server: Client disconnected\n"); + } else { + perror("recv"); + } + close(client->socket_fd); + client->active = false; + client->routing_activated = false; + return; + } + + client->rx_offset += bytes_read; + + /* Process complete DoIP messages */ + while (client->rx_offset >= DOIP_HEADER_SIZE) { + DoIPHeader_t header; + if (!doip_header_parse(client->rx_buffer, &header)) { + printf("DoIP-server: Invalid header\n"); + close(client->socket_fd); + client->active = false; + return; + } + + size_t total_msg_size = DOIP_HEADER_SIZE + header.payload_length; + + if (client->rx_offset < total_msg_size) { + /* Wait for more data */ + break; + } + + /* Process message */ + const uint8_t *payload = client->rx_buffer + DOIP_HEADER_SIZE; + doip_process_message(client, &header, payload); + + /* Remove processed message from buffer */ + if (client->rx_offset > total_msg_size) { + memmove(client->rx_buffer, + client->rx_buffer + total_msg_size, + client->rx_offset - total_msg_size); + } + client->rx_offset -= total_msg_size; + } +} + +/** + * @brief Initialize DoIP server + */ +int doip_server_init(uint16_t logical_address, + void (*diag_msg_callback)(uint16_t, const uint8_t*, size_t)) { + memset(&server, 0, sizeof(DoIPServer_t)); + + server.logical_address = logical_address; + server.on_diag_message = diag_msg_callback; + + /* Create TCP socket */ + server.listen_socket = socket(AF_INET, SOCK_STREAM, 0); + if (server.listen_socket < 0) { + perror("socket"); + return -1; + } + + /* Set socket options */ + int opt = 1; + if (setsockopt(server.listen_socket, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) { + perror("setsockopt"); + close(server.listen_socket); + return -1; + } + + /* Bind to DoIP port */ + struct sockaddr_in addr; + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = INADDR_ANY; + addr.sin_port = htons(DOIP_TCP_PORT); + + if (bind(server.listen_socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind"); + close(server.listen_socket); + return -1; + } + + /* Listen for connections */ + if (listen(server.listen_socket, DOIP_MAX_CLIENTS) < 0) { + perror("listen"); + close(server.listen_socket); + return -1; + } + + printf("DoIP Server: Listening on port %d (LA=0x%04X)\n", + DOIP_TCP_PORT, logical_address); + + return 0; +} + +/** + * @brief Accept new client connection + */ +static void doip_accept_connection(void) { + struct sockaddr_in client_addr; + socklen_t addr_len = sizeof(client_addr); + + int client_fd = accept(server.listen_socket, + (struct sockaddr *)&client_addr, &addr_len); + if (client_fd < 0) { + perror("accept"); + return; + } + + /* Find free client slot */ + DoIPClientConnection_t *client = NULL; + for (int i = 0; i < DOIP_MAX_CLIENTS; i++) { + if (!server.clients[i].active) { + client = &server.clients[i]; + break; + } + } + + if (!client) { + printf("DoIP-server: Max clients reached, rejecting connection\n"); + close(client_fd); + return; + } + + /* Initialize client */ + memset(client, 0, sizeof(DoIPClientConnection_t)); + client->socket_fd = client_fd; + client->active = true; + client->routing_activated = false; + + printf("DoIP-server: Client connected from %s:%d\n", + inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port)); +} + +/** + * @brief Send diagnostic message response to client + */ +int doip_server_send_diag_response(uint16_t source_address, + const uint8_t *data, size_t len) { + /* Find client with matching source address */ + DoIPClientConnection_t *client = NULL; + for (int i = 0; i < DOIP_MAX_CLIENTS; i++) { + if (server.clients[i].active && + server.clients[i].routing_activated && + server.clients[i].source_address == source_address) { + client = &server.clients[i]; + break; + } + } + + if (!client) { + printf("DoIP-server: No active client with SA=0x%04X\n", source_address); + return -1; + } + + /* Build diagnostic message payload */ + uint8_t payload[DOIP_BUFFER_SIZE]; + if (len + 4 > DOIP_BUFFER_SIZE) { + return -1; + } + + payload[0] = (server.logical_address >> 8) & 0xFF; /* Source address (server) */ + payload[1] = server.logical_address & 0xFF; + payload[2] = (source_address >> 8) & 0xFF; /* Target address (client) */ + payload[3] = source_address & 0xFF; + memcpy(payload + 4, data, len); + + return doip_send_message(client, DOIP_PAYLOAD_TYPE_DIAG_MESSAGE, payload, len + 4); +} + +/** + * @brief Process DoIP server events (call periodically) + */ +void doip_server_process(int timeout_ms) { + fd_set readfds; + struct timeval tv; + int max_fd = server.listen_socket; + + FD_ZERO(&readfds); + FD_SET(server.listen_socket, &readfds); + + /* Add active client sockets */ + for (int i = 0; i < DOIP_MAX_CLIENTS; i++) { + if (server.clients[i].active) { + FD_SET(server.clients[i].socket_fd, &readfds); + if (server.clients[i].socket_fd > max_fd) { + max_fd = server.clients[i].socket_fd; + } + } + } + + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + int ret = select(max_fd + 1, &readfds, NULL, NULL, &tv); + if (ret < 0) { + perror("select"); + return; + } + + if (ret == 0) { + return; /* Timeout */ + } + + /* Check for new connections */ + if (FD_ISSET(server.listen_socket, &readfds)) { + doip_accept_connection(); + } + + /* Check client sockets */ + for (int i = 0; i < DOIP_MAX_CLIENTS; i++) { + if (server.clients[i].active && + FD_ISSET(server.clients[i].socket_fd, &readfds)) { + doip_handle_client_rx(&server.clients[i]); + } + } +} + +/** + * @brief Shutdown DoIP server + */ +void doip_server_shutdown(void) { + /* Close all client connections */ + for (int i = 0; i < DOIP_MAX_CLIENTS; i++) { + if (server.clients[i].active) { + close(server.clients[i].socket_fd); + } + } + + /* Close listen socket */ + if (server.listen_socket >= 0) { + close(server.listen_socket); + } + + printf("DoIP Server: Shutdown complete\n"); +} diff --git a/examples/doip_client/doip_server.h b/examples/doip_client/doip_server.h new file mode 100644 index 000000000..6697a3a96 --- /dev/null +++ b/examples/doip_client/doip_server.h @@ -0,0 +1,61 @@ +/** + * @file doip_server.h + * @brief ISO 13400 (DoIP) Transport Layer - Server API + */ + +#ifndef DOIP_SERVER_H +#define DOIP_SERVER_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Initialize DoIP server + * + * @param logical_address Server's DoIP logical address + * @param diag_msg_callback Callback function for received diagnostic messages + * @return 0 on success, -1 on error + */ +int doip_server_init(uint16_t logical_address, + void (*diag_msg_callback)(uint16_t source_addr, + const uint8_t *data, + size_t len)); + +/** + * @brief Send diagnostic message response to client + * + * @param source_address Client's source address (routing activated) + * @param data UDS response data + * @param len Length of response data + * @return 0 on success, -1 on error + */ +int doip_server_send_diag_response(uint16_t source_address, + const uint8_t *data, + size_t len); + +/** + * @brief Process DoIP server events + * + * This function should be called periodically to handle incoming connections + * and messages. It will block for up to timeout_ms waiting for events. + * + * @param timeout_ms Timeout in milliseconds + */ +void doip_server_process(int timeout_ms); + +/** + * @brief Shutdown DoIP server + * + * Closes all connections and releases resources. + */ +void doip_server_shutdown(void); + +#ifdef __cplusplus +} +#endif + +#endif /* DOIP_SERVER_H */ diff --git a/examples/doip_client/doip_test_client.c b/examples/doip_client/doip_test_client.c new file mode 100644 index 000000000..9337d068c --- /dev/null +++ b/examples/doip_client/doip_test_client.c @@ -0,0 +1,145 @@ +/** + * @file main.c + * @author Oliver Wieland (oliverwieland@web.de) + * @brief Example DoIP client using the iso14229 library + * @version 0.1 + * @date 2025-12-10 + * + * @copyright Copyright (c) 2025 + * + */ +#include "iso14229.h" + +#include +#include + +#if defined(UDS_TP_DOIP) + +#else +#error "no transport defined" +#endif + +typedef struct { + enum { + Step_0_RDBI_Send, + Step_1_RDBI_Recv, + Step_2_WDBI_Send, + Step_3_WDBI_Recv, + Step_DONE, + } step; + UDSErr_t err; + uint16_t rdbi_f190; +} SequenceContext_t; + +UDSErr_t fn(UDSClient_t *client, UDSEvent_t evt, void *ev_data) { + SequenceContext_t *c = (SequenceContext_t *)client->fn_data; + if (evt != UDS_EVT_Poll) { + UDS_LOGI(__FILE__, "%s (%d)", UDSEventToStr(evt), evt); + } + if (UDS_EVT_Err == evt) { + UDS_LOGE(__FILE__, "Exiting on step %d with error: %s", c->step, + UDSErrToStr(*(UDSErr_t *)ev_data)); + c->err = *(UDSErr_t *)ev_data; + c->step = Step_DONE; + } + switch (c->step) { + case Step_0_RDBI_Send: { + const uint16_t dids[] = {0xf190}; + c->err = UDSSendRDBI(client, dids, 1); + if (c->err) { + UDS_LOGE(__FILE__, "UDSSendRDBI failed with err: %d", c->err); + c->step = Step_DONE; + } + c->step = Step_1_RDBI_Recv; + break; + } + case Step_1_RDBI_Recv: { + UDSRDBIVar_t vars[] = { + {0xf190, 2, &(c->rdbi_f190), memmove}, + }; + if (UDS_EVT_ResponseReceived == evt) { + c->err = UDSUnpackRDBIResponse(client, vars, 1); + if (c->err) { + UDS_LOGE(__FILE__, "UDSUnpackRDBIResponse failed with err: %s", + UDSErrToStr(c->err)); + c->step = Step_DONE; + } + UDS_LOGI(__FILE__, "0xf190 has value %d", c->rdbi_f190); + c->step = Step_2_WDBI_Send; + } + break; + } + case Step_2_WDBI_Send: { + uint16_t val = c->rdbi_f190 + 1; + uint8_t data[2] = { + (val & 0xff00) >> 8, + val & 0x00ff, + }; + c->err = UDSSendWDBI(client, 0xf190, data, sizeof(data)); + if (c->err) { + UDS_LOGE(__FILE__, "UDSSendWDBI failed with err: %s", UDSErrToStr(c->err)); + c->step = Step_DONE; + } + c->step = Step_3_WDBI_Recv; + break; + } + case Step_3_WDBI_Recv: { + if (UDS_EVT_ResponseReceived == evt) { + UDS_LOGI(__FILE__, "WDBI response received"); + c->step = Step_DONE; + } + default: + break; + } + } + return UDS_OK; +} + + +int main(int ac, char **av) { + (void)ac; + (void)av; + UDSClient_t client; + DoIPClient_t tp; + + // Check for invalid IP address length + UDSErr_t result = UDSDoIPInitClient(&tp, "1234567890123456789012345678901234567890123456789012345678901234 ", 13400, 0x1234, 0x0001); + if (result == UDS_OK) { + UDS_LOGE(__FILE__, "DoIP Client: UDSDoIPInitClient should have failed with invalid IP address"); + UDSDoIPDeinit(&tp); + exit(-1); + } + + result = UDSDoIPInitClient(&tp, "127.0.0.1", 13400, 0x1234, 0x0001); + if (result != UDS_OK) { + UDS_LOGE(__FILE__, "DoIP Client: UDSDoIPInitClient failed with error %d", result); + UDSDoIPDeinit(&tp); + exit(-1); + } + + if (UDSClientInit(&client)) { + exit(-1); + } + + client.tp = (UDSTp_t *)&tp; + client.fn = fn; + + UDS_LOGI(__FILE__, "DoIP Client: UDSDoIPInitClient returned %d", result); + + SequenceContext_t ctx = {0}; + client.fn_data = &ctx; + + UDS_LOGI(__FILE__, "polling"); + while (ctx.step != Step_DONE) { + UDSClientPoll(&client); + } + +#ifdef KEEP_ALIVE_CHECK + // poll forver to cause keep-alive checks from server + while (true) { + UDSClientPoll(&client); + } +#endif + + return ctx.err; +} diff --git a/examples/doip_client/doip_test_server.c b/examples/doip_client/doip_test_server.c new file mode 100644 index 000000000..6fe4ade0a --- /dev/null +++ b/examples/doip_client/doip_test_server.c @@ -0,0 +1,150 @@ +/** + * @file doip_example.c + * @brief Example usage of DoIP transport layer with UDS + * + * This example demonstrates: + * 1. DoIP server receiving diagnostic requests + * 2. DoIP client sending diagnostic requests + * 3. Integration with UDS protocol (ISO 14229) + * + * To compile server: + * gcc -o doip_server_example doip_example.c doip_server.c -DSERVER_MODE + * + * To compile client: + * gcc -o doip_client_example doip_example.c doip_client.c -DCLIENT_MODE + */ + +#include +#include +#include +#include +#include + +#include "doip_server.h" + +static volatile int server_running = 1; + +void signal_handler(int signum) { + (void)signum; /* Unused parameter */ + printf("\nShutdown signal received\n"); + server_running = 0; +} + + +/** + * @brief Callback for received diagnostic messages + */ +void on_diagnostic_message(uint16_t source_addr, const uint8_t *data, size_t len) { + printf("\n=== Received UDS Request ===\n"); + printf("Source Address: 0x%04X\n", source_addr); + printf("UDS Data (%zu bytes): ", len); + for (size_t i = 0; i < len; i++) { + printf("%02X ", data[i]); + } + printf("\n"); + + /* Example: Handle ReadDataByIdentifier (0x22) */ + if (len >= 3 && data[0] == 0x22) { + uint16_t did = (data[1] << 8) | data[2]; + printf("Service: ReadDataByIdentifier (DID=0x%04X)\n", did); + + /* Build positive response */ + uint8_t response[10]; + response[0] = 0x62; /* Positive response (0x22 + 0x40) */ + response[1] = data[1]; + response[2] = data[2]; + + /* Example data for DID */ + response[3] = 0x12; + response[4] = 0x34; + response[5] = 0x56; + response[6] = 0x78; + + /* Send response */ + doip_server_send_diag_response(source_addr, response, 7); + printf("Sent positive response\n"); + } else + /* Example: Handle WriteDataByIdentifier (0x2E) */ + if (len >= 4 && data[0] == 0x2E) { + uint16_t did = (data[1] << 8) | data[2]; + printf("Service: WriteDataByIdentifier (DID=0x%04X)\n", did); + + /* Build positive response */ + uint8_t response[10]; + response[0] = 0x6E; /* Positive response (0x2E + 0x40) */ + response[1] = data[1]; + response[2] = data[2]; + + /* Send response */ + doip_server_send_diag_response(source_addr, response, 7); + printf("Sent positive response\n"); + } + + /* Example: Handle DiagnosticSessionControl (0x10) */ + else if (len >= 2 && data[0] == 0x10) { + uint8_t session_type = data[1]; + printf("Service: DiagnosticSessionControl (Session=0x%02X)\n", session_type); + + /* Build positive response */ + uint8_t response[6]; + response[0] = 0x50; /* Positive response */ + response[1] = session_type; + response[2] = 0x00; /* P2 high byte */ + response[3] = 0x32; /* P2 low byte (50ms) */ + response[4] = 0x01; /* P2* high byte */ + response[5] = 0xF4; /* P2* low byte (500ms) */ + + doip_server_send_diag_response(source_addr, response, 6); + printf("Sent positive response\n"); + } + /* Example: Handle TesterPresent (0x3E) */ + else if (len >= 2 && data[0] == 0x3E) { + printf("Service: TesterPresent\n"); + + uint8_t response[2]; + response[0] = 0x7E; /* Positive response */ + response[1] = data[1]; + + doip_server_send_diag_response(source_addr, response, 2); + printf("Sent positive response\n"); + } + else { + /* Unknown service - send negative response */ + uint8_t response[3]; + response[0] = 0x7F; /* Negative response */ + response[1] = data[0]; + response[2] = 0x11; /* ServiceNotSupported */ + + doip_server_send_diag_response(source_addr, response, 3); + printf("Sent negative response (ServiceNotSupported)\n"); + } + + printf("===========================\n\n"); +} + +int main(void) { + printf("DoIP Server Example\n"); + printf("===================\n\n"); + + signal(SIGINT, signal_handler); + signal(SIGTERM, signal_handler); + + /* Initialize DoIP server */ + uint16_t logical_address = 0x0001; + if (doip_server_init(logical_address, on_diagnostic_message) < 0) { + fprintf(stderr, "Failed to initialize DoIP server\n"); + return 1; + } + + printf("Server running. Press Ctrl+C to stop.\n\n"); + + /* Main loop */ + while (server_running) { + doip_server_process(100); /* 100ms timeout */ + } + + doip_server_shutdown(); + + printf("Server stopped.\n"); + return 0; +} diff --git a/examples/doip_client/iso14229.c b/examples/doip_client/iso14229.c new file mode 120000 index 000000000..d393690ec --- /dev/null +++ b/examples/doip_client/iso14229.c @@ -0,0 +1 @@ +../../iso14229.c \ No newline at end of file diff --git a/examples/doip_client/iso14229.h b/examples/doip_client/iso14229.h new file mode 120000 index 000000000..00d608599 --- /dev/null +++ b/examples/doip_client/iso14229.h @@ -0,0 +1 @@ +../../iso14229.h \ No newline at end of file diff --git a/examples/doip_client/test.sh b/examples/doip_client/test.sh new file mode 100755 index 000000000..be3a8e084 --- /dev/null +++ b/examples/doip_client/test.sh @@ -0,0 +1,65 @@ +#!/bin/bash + +set -euo pipefail + +cleanup() { + if [ -n "${SERVER_PID:-}" ] && kill -0 $SERVER_PID 2>/dev/null; then + echo "Cleaning up server (PID $SERVER_PID)..." + kill $SERVER_PID 2>/dev/null || true + wait $SERVER_PID 2>/dev/null || true + fi +} + +trap cleanup EXIT + +make + +# 1) Start doip_server in bg and store server PID +./doip_server & +SERVER_PID=$! +echo "Started server with PID $SERVER_PID" + +# Give server time to start +sleep 1 + +# 2) Run doip_client in bg +./doip_client & +CLIENT_PID=$! +echo "Started client with PID $CLIENT_PID" + +# 3) Wait 5s +sleep 5 + +# 4) Verify that client exited normally (return code 0) +if kill -0 $CLIENT_PID 2>/dev/null; then + echo "ERROR: Client is still running after 10 seconds" + kill $CLIENT_PID 2>/dev/null || true + exit 1 +fi + +wait $CLIENT_PID 2>/dev/null +CLIENT_EXIT=$? + +if [ $CLIENT_EXIT -ne 0 ]; then + echo "ERROR: Client exited with code $CLIENT_EXIT (expected 0)" + exit 1 +fi + +echo "Client exited normally with code 0" + +# 5) Verify that server is alive +if ! kill -0 $SERVER_PID 2>/dev/null; then + echo "ERROR: Server is not running" + exit 1 +fi + +echo "Server is still running (PID $SERVER_PID)" + +# 6) Kill the server +kill $SERVER_PID +wait $SERVER_PID 2>/dev/null || true +echo "Server stopped" + +echo "Test passed successfully" + +echo "Both client and server exited successfully" diff --git a/examples/doip_discovery_example/Makefile b/examples/doip_discovery_example/Makefile new file mode 100644 index 000000000..308e6ddbe --- /dev/null +++ b/examples/doip_discovery_example/Makefile @@ -0,0 +1,16 @@ +APP_SRCS += doip_discover.c \ + main/iso14229.c +APP_TARGET = doip_discovery + +# Use the verbose log level for debugging +# CFLAGS = -DUDS_TP_DOIP=1 -DUDS_CONFIG_LOG_COLORS=1 -DUDS_LOG_LEVEL=UDS_LOG_VERBOSE -g -Wall -Wpedantic -Wextra +CFLAGS = -DUDS_TP_DOIP=1 -DUDS_CONFIG_LOG_COLORS=1 -DUDS_LOG_LEVEL=UDS_LOG_INFO -g -Wall -Wpedantic -Wextra +INCLUDES = -I../../src -I../.. + +all: app + +app: $(APP_SRCS) + $(CC) $(CFLAGS) $(INCLUDES) $(APP_SRCS) -o $(APP_TARGET) + +clean: + rm -f $(APP_TARGET) diff --git a/examples/doip_discovery_example/doip_discover.c b/examples/doip_discovery_example/doip_discover.c new file mode 100644 index 000000000..243ae24ac --- /dev/null +++ b/examples/doip_discovery_example/doip_discover.c @@ -0,0 +1,78 @@ +#include +#include +#include +#include +#include +#include "tp/doip/doip_client.h" + +static bool select_by_vin(const DoIPDiscoveryInfo *info, void *user) { + const char *needle = (const char *)user; /* expected VIN prefix or full VIN */ + if (!needle || !*needle) return false; + if (info->vin[0] == '\0') return false; + bool match = strncmp(info->vin, needle, strlen(needle)) == 0; + printf("Discovered: VIN=%s IP=%s PORT=%u%s\n", info->vin, info->ip, info->remote_port, + match ? " [MATCH]" : ""); + return match; +} + +int main(int argc, char **argv) { + const char *vin_prefix = NULL; + bool loopback = false; + uint16_t udp_port = 0; /* 0=>default */ + bool request_only = false; + bool dump_raw = false; + + if (argc > 1) { + if (strcmp(argv[1], "loopback") == 0) { + loopback = true; + if (argc > 2) { + if (strcmp(argv[2], "--request-only") == 0) request_only = true; + else if (strcmp(argv[2], "--raw") == 0) dump_raw = true; + else udp_port = (uint16_t)atoi(argv[2]); + } + if (argc > 3) { + if (strcmp(argv[3], "--request-only") == 0) request_only = true; + else if (strcmp(argv[3], "--raw") == 0) dump_raw = true; + } + } else { + vin_prefix = argv[1]; + if (argc > 2 && strcmp(argv[2], "loopback") == 0) { + loopback = true; + if (argc > 3) { + if (strcmp(argv[3], "--request-only") == 0) request_only = true; + else if (strcmp(argv[3], "--raw") == 0) dump_raw = true; + else udp_port = (uint16_t)atoi(argv[3]); + } + if (argc > 4) { + if (strcmp(argv[4], "--request-only") == 0) request_only = true; + else if (strcmp(argv[4], "--raw") == 0) dump_raw = true; + } + } + } + } + + DoIPClient_t tp; + memset(&tp, 0, sizeof(tp)); + + /* Set a selection callback (optional): choose server whose VIN matches prefix */ + if (vin_prefix && *vin_prefix) { + UDSDoIPSetSelectionCallback(&tp, select_by_vin, (void*)vin_prefix); + } + + /* Discover vehicles (multicast by default; pass "loopback" to use 127.0.0.1; optional port) */ + UDSDoIPSetDiscoveryOptions(request_only, dump_raw); + int count = UDSDoIPDiscoverVehiclesEx(&tp, 2000, loopback, udp_port); + printf("Discovered %d responders\n", count); + + if (tp.server_ip[0] == '\0') { + printf("No server selected. Exiting.\n"); + return 0; + } + + /* Demonstration ends at discovery/selection. Connection can be done via UDSDoIPInitClient. */ + printf("Selected server: %s\n", tp.server_ip); + if (udp_port != 0) { + printf("(Discovery UDP port override: %u)\n", udp_port); + } + return 0; +} diff --git a/examples/doip_discovery_example/main/iso14229.c b/examples/doip_discovery_example/main/iso14229.c new file mode 120000 index 000000000..eb8faa831 --- /dev/null +++ b/examples/doip_discovery_example/main/iso14229.c @@ -0,0 +1 @@ +../../../iso14229.c \ No newline at end of file diff --git a/examples/doip_discovery_example/main/iso14229.h b/examples/doip_discovery_example/main/iso14229.h new file mode 120000 index 000000000..e6f020973 --- /dev/null +++ b/examples/doip_discovery_example/main/iso14229.h @@ -0,0 +1 @@ +../../../iso14229.h \ No newline at end of file diff --git a/iso14229.c b/iso14229.c index 53ff82439..54bbe8e9e 100644 --- a/iso14229.c +++ b/iso14229.c @@ -3716,6 +3716,1455 @@ void ISOTPMockFree(UDSTp_t *tp) { #endif + +#ifdef UDS_LINES +#line 1 "src/tp/doip/doip_client.c" +#endif +#if defined(UDS_TP_DOIP) +/** + * @file doip_client.c + * @brief ISO 13400 (DoIP) Transport Layer - Client Implementation + * @details DoIP TCP transport layer client for UDS (ISO 14229) + * + * + * @note This is a simplified implementation focusing on TCP diagnostic messages. + * UDP vehicle discovery is not included. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/* Macro to extract 16-bit BE DoIP address from buffer */ +#define DOIP_ADDRESS(a, off) ((uint16_t)(((a[(off)]) << 8) | (a[(off) + 1]))) + +/** + * @brief Initialize DoIP header + * @param header Pointer to DoIPHeader_t structure to initialize + * @param payload_type DoIP payload type + * @param payload_length Length of DoIP payload + */ +static void doip_header_init(DoIPHeader_t *header, uint16_t payload_type, uint32_t payload_length) { + header->protocol_version = DOIP_PROTOCOL_VERSION; + header->protocol_version_inv = DOIP_PROTOCOL_VERSION_INV; + header->payload_type = htons(payload_type); + header->payload_length = htonl(payload_length); +} + +/** + * @brief Convert DoIP client state to string. + * + * @param state the state to convert + * @return const char* the string representation of the state + */ +const char *doip_client_state_to_string(DoIPClientState_t state) { + switch (state) { + case DOIP_STATE_DISCONNECTED: + return "DISCONNECTED"; + case DOIP_STATE_CONNECTED: + return "CONNECTED"; + case DOIP_STATE_ROUTING_ACTIVATION_PENDING: + return "ROUTING_ACTIVATION_PENDING"; + case DOIP_STATE_READY_FOR_DIAG_REQUEST: + return "READY"; + case DOIP_STATE_DIAG_MESSAGE_ACK_PENDING: + return "ACK_PENDING"; + case DOIP_STATE_DIAG_MESSAGE_RESPONSE_PENDING: + return "RESPONSE_PENDING"; + case DOIP_STATE_ERROR: + return "ERROR"; + default: + return "UNKNOWN_STATE"; + } +} + +/** + * @brief Helper macro to change DoIP client state with logging. + */ +#define doip_change_state(t, s) \ + { \ + if (_doip_change_state((t), (s))) { \ + UDS_LOGV(__FILE__, "DoIP: State change to %s (line %d)", \ + doip_client_state_to_string(s), __LINE__); \ + } \ + } + +/** + * @brief Change DoIP client state + * @param tp DoIP client context + * @param new_state New state to set + * @return true if state changed, false if same state + */ +static bool _doip_change_state(DoIPClient_t *tp, DoIPClientState_t new_state) { + if (tp->state == new_state) + return false; + + // UDS_LOGI(__FILE__, "DoIP: State change %d -> %d", tp->state, new_state); + tp->state = new_state; + return true; +} + +/** + * @brief Parse DoIP header from buffer + * @param buffer Pointer to buffer containing DoIP header + * @param header Pointer to DoIPHeader_t structure to fill + */ +static bool doip_header_parse(const uint8_t *buffer, DoIPHeader_t *header) { + if (NULL == buffer || NULL == header) { + return false; + } + + header->protocol_version = buffer[0]; + header->protocol_version_inv = buffer[1]; + header->payload_type = buffer[2] << 8 | buffer[3]; + header->payload_length = buffer[4] << 24 | buffer[5] << 16 | buffer[6] << 8 | buffer[7]; + + /* Validate protocol version */ + if (header->protocol_version != DOIP_PROTOCOL_VERSION || + header->protocol_version_inv != DOIP_PROTOCOL_VERSION_INV) { + return false; + } + + return true; +} + +/** + * @brief Send DoIP message. + * @param tp DoIP client context + * @param payload_type DoIP payload type + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + * @return int Number of payload bytes sent, or -1 on error + */ +static int doip_send_message(const DoIPClient_t *tp, uint16_t payload_type, const uint8_t *payload, + uint32_t payload_len) { + uint8_t buffer[DOIP_BUFFER_SIZE]; + DoIPHeader_t *header = (DoIPHeader_t *)buffer; + + if (DOIP_HEADER_SIZE + payload_len > DOIP_BUFFER_SIZE) { + return -1; + } + + doip_header_init(header, payload_type, payload_len); + + if (payload && payload_len > 0) { + memcpy(buffer + DOIP_HEADER_SIZE, payload, payload_len); + } + + ssize_t sent = tp->tcp.send(&tp->tcp, buffer, DOIP_HEADER_SIZE + payload_len); + if (sent < 0) { + perror("tcp_send"); + return -1; + } + + /* Return number of UDS payload bytes sent (strip headers) */ + return sent - DOIP_HEADER_SIZE - DOIP_DIAG_HEADER_SIZE; +} + +/** + * @brief Stores UDS response data in DoIP client context + * + * @param tp DoIP client context + * @param data Pointer to DoIP response data. Assumes a diag message payload. + * @param len Length of DoIP response data + */ +void doip_store_uds_response(DoIPClient_t *tp, const uint8_t *data, size_t len) { + if (len > DOIP_BUFFER_SIZE) { + UDS_LOGE(__FILE__, "DoIP: Response too large to store (%zu bytes)", len); + return; + } + + // Store UDS response data in separate buffer for doip_tp_recv to retrieve + if (len > DOIP_DIAG_HEADER_SIZE) { + uint16_t sa = DOIP_ADDRESS(data, 0); + + // strip diag header (sa and ta) + const uint8_t *uds_data = data + DOIP_DIAG_HEADER_SIZE; + size_t uds_len = len - DOIP_DIAG_HEADER_SIZE; + + /* Copy UDS data to uds_response buffer */ + if (uds_len <= DOIP_BUFFER_SIZE) { + memcpy(tp->uds_response, uds_data, uds_len); + tp->uds_response_len = uds_len; + UDS_LOGI(__FILE__, "DoIP: Stored diagnostic response (%zu bytes) from SA=0x%04X", + uds_len, sa); + } else { + UDS_LOGE(__FILE__, "DoIP: Diagnostic response too large (%zu bytes)", uds_len); + } + } +} + +/** + * @brief Handle routing activation response + * @param tp DoIP client context + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + */ +static void doip_handle_routing_activation_response(DoIPClient_t *tp, const uint8_t *payload, + uint32_t payload_len) { + if (payload_len < 9) { + UDS_LOGI(__FILE__, "DoIP: Invalid routing activation response length"); + doip_change_state(tp, DOIP_STATE_ERROR); + return; + } + + uint16_t client_sa = DOIP_ADDRESS(payload, 0); + uint16_t server_sa = DOIP_ADDRESS(payload, 2); + uint8_t response_code = payload[4]; + (void)client_sa; /* Validated implicitly by successful response */ + + if (response_code == DOIP_ROUTING_ACTIVATION_RES_SUCCESS) { + doip_change_state(tp, DOIP_STATE_READY_FOR_DIAG_REQUEST); + UDS_LOGI(__FILE__, "DoIP: Routing activated (SA=0x%04X, TA=0x%04X)", tp->source_address, + server_sa); + } else { + UDS_LOGI(__FILE__, "DoIP: Routing activation failed (code=0x%02X)", response_code); + doip_change_state(tp, DOIP_STATE_ERROR); + } +} + +/** + * @brief Sends alive check response when (DoIP) server requests it. + * @param tp DoIP client context + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + */ +static void doip_handle_alive_check_request(const DoIPClient_t *tp, const uint8_t *payload, + uint32_t payload_len) { + + (void)tp; + (void)payload; + (void)payload_len; + + // alive check request payload is empty, the response contains the client's source address + uint8_t response[2]; + response[0] = (tp->source_address >> 8) & 0xFF; + response[1] = tp->source_address & 0xFF; + + UDS_LOGI(__FILE__, "DoIP: Alive check request -> response from 0x%04X", tp->source_address); + int sent = doip_send_message(tp, DOIP_PAYLOAD_TYPE_ALIVE_CHECK_RES, response, sizeof(response)); + if (sent < 0) { + UDS_LOGE(__FILE__, "DoIP: Failed to send alive check response"); + } else { + UDS_LOGI(__FILE__, "DoIP: Sent alive check response (%d bytes)", sent); + } +} + +/** + * @brief Handle diagnostic message positive ACK. + * @param tp DoIP client context + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + */ +static void doip_handle_diag_pos_ack(DoIPClient_t *tp, const uint8_t *payload, + uint32_t payload_len) { + if (payload_len < 5) { + return; + } + + uint16_t source_address = DOIP_ADDRESS(payload, 0); + uint16_t target_address = DOIP_ADDRESS(payload, 2); + uint8_t ack_code = payload[4]; + + tp->diag_ack_received = true; + + UDS_LOGI(__FILE__, "DoIP: Diagnostic message ACK (SA=0x%04X, TA=0x%04X, code=0x%02X)", + source_address, target_address, ack_code); +} + +/** + * @brief Handle diagnostic message negative ACK. + * @param tp DoIP client context + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + */ +static void doip_handle_diag_neg_ack(DoIPClient_t *tp, const uint8_t *payload, + uint32_t payload_len) { + if (payload_len < 5) { + return; + } + + uint16_t source_address = DOIP_ADDRESS(payload, 0); + uint16_t target_address = DOIP_ADDRESS(payload, 2); + uint8_t nack_code = payload[4]; + + tp->diag_nack_received = true; + tp->diag_nack_code = nack_code; + + UDS_LOGW(__FILE__, "DoIP: Diagnostic message NACK (SA=0x%04X, TA=0x%04X, code=0x%02X)", + source_address, target_address, nack_code); +} + +/** + * @brief Handle diagnostic message (response from server). + * @param tp DoIP client context + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + */ +static void doip_handle_diag_message(DoIPClient_t *tp, const uint8_t *payload, + uint32_t payload_len) { + if (payload_len < 4) { + return; + } + + uint16_t target_address = DOIP_ADDRESS(payload, 2); + + /* Verify target address matches our logical address */ + if (target_address != tp->source_address) { + UDS_LOGI(__FILE__, "DoIP: Received diagnostic message for different TA=0x%04X", + target_address); + return; + } + + /* Store UDS response data in separate buffer */ + doip_store_uds_response(tp, payload, payload_len); +} + +/**#ifdef DOIP_MOCK_TP + * @brief Process received DoIP message according to payload type. + * @param tp DoIP client context + * @param header Pointer to DoIP header + * @param payload Pointer to DoIP payload + */ +static void doip_process_message(DoIPClient_t *tp, const DoIPHeader_t *header, + const uint8_t *payload) { + switch (header->payload_type) { + case DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_RES: + doip_handle_routing_activation_response(tp, payload, header->payload_length); + break; + + case DOIP_PAYLOAD_TYPE_ALIVE_CHECK_REQ: + doip_handle_alive_check_request(tp, payload, header->payload_length); + break; + + case DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_POS_ACK: + doip_handle_diag_pos_ack(tp, payload, header->payload_length); + break; + + case DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_NEG_ACK: + doip_handle_diag_neg_ack(tp, payload, header->payload_length); + break; + + case DOIP_PAYLOAD_TYPE_DIAG_MESSAGE: + doip_handle_diag_message(tp, payload, header->payload_length); + break; + + default: + UDS_LOGI(__FILE__, "DoIP: Unknown payload type 0x%04X", header->payload_type); + break; + } +} + +/** + * @brief Receive and process data + * @param tp DoIP client context + * @param timeout_ms Timeout in milliseconds + */ +static ssize_t doip_receive_data(DoIPClient_t *tp, int timeout_ms) { + ssize_t bytes_read = tp->tcp.recv(&tp->tcp, tp->rx_buffer + tp->rx_offset, + DOIP_BUFFER_SIZE - tp->rx_offset, timeout_ms); + + if (bytes_read <= 0) { + if (bytes_read == 0) { + UDS_LOGE(__FILE__, "DoIP: Server disconnected"); + } else { + perror("tcp_recv"); + } + doip_change_state(tp, DOIP_STATE_DISCONNECTED); + return -1; + } + + UDS_LOGI(__FILE__, "DoIP: Received %zd bytes", bytes_read); + + tp->rx_offset += bytes_read; + /* Process complete DoIP messages */ + while (tp->rx_offset >= DOIP_HEADER_SIZE) { + DoIPHeader_t header; + if (!doip_header_parse(tp->rx_buffer, &header)) { + UDS_LOGE(__FILE__, "DoIP: Invalid header"); + doip_change_state(tp, DOIP_STATE_ERROR); + return -1; + } + + size_t total_msg_size = DOIP_HEADER_SIZE + header.payload_length; + + if (tp->rx_offset < total_msg_size) { + /* Wait for more data */ + break; + } + + /* Process message */ + const uint8_t *payload = tp->rx_buffer + DOIP_HEADER_SIZE; + UDS_LOGI(__FILE__, "DoIP: Processing message type 0x%04X, length %u", header.payload_type, + header.payload_length); + UDS_LOG_SDU(__FILE__, payload, header.payload_length, NULL); + doip_process_message(tp, &header, payload); + + /* Remove processed message from buffer */ + if (tp->rx_offset > total_msg_size) { + memmove(tp->rx_buffer, tp->rx_buffer + total_msg_size, tp->rx_offset - total_msg_size); + } + tp->rx_offset -= total_msg_size; + } + + return bytes_read; +} + +/** + * @brief Connect to DoIP server + * @param tp DoIP client context + */ +int doip_client_connect(DoIPClient_t *tp) { + if (tp->state != DOIP_STATE_DISCONNECTED) { + UDS_LOGE(__FILE__, "DoIP: Already connected or in error state"); + return -1; + } + + if (tp->tcp.init(&tp->tcp, tp->server_ip, tp->server_port ? tp->server_port : DOIP_TCP_PORT) < + 0) { + UDS_LOGE(__FILE__, "DoIP: TCP init failed"); + return -1; + } + if (tp->tcp.connect(&tp->tcp) < 0) { + UDS_LOGE(__FILE__, "DoIP: TCP connect error (%s:%d)", tp->server_ip, + tp->server_port ? tp->server_port : DOIP_TCP_PORT); + return -1; + } + + doip_change_state(tp, DOIP_STATE_CONNECTED); + UDS_LOGI(__FILE__, "DoIP Client: Connected to %s:%d", tp->server_ip, + tp->server_port ? tp->server_port : DOIP_TCP_PORT); + + return 0; +} + +/** + * @brief Activate routing. Sends a "routing activation" request to the server. + * @param tp DoIP client context + */ +int doip_client_activate_routing(DoIPClient_t *tp) { + if (tp->state != DOIP_STATE_CONNECTED) { + UDS_LOGE(__FILE__, "DoIP: Not connected"); + return -1; + } + + /* Build routing activation request */ + uint8_t payload[11]; + payload[0] = (tp->source_address >> 8) & 0xFF; + payload[1] = tp->source_address & 0xFF; + payload[2] = DOIP_ROUTING_ACTIVATION_TYPE; + payload[3] = 0x00; /* Reserved */ + payload[4] = 0x00; + payload[5] = 0x00; + payload[6] = 0x00; + + /* Optional: OEM specific */ + payload[7] = 0x00; + payload[8] = 0x00; + payload[9] = 0x00; + payload[10] = 0x00; + + if (doip_send_message(tp, DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_REQ, payload, 11) < 0) { + return -1; + } + + doip_change_state(tp, DOIP_STATE_ROUTING_ACTIVATION_PENDING); + + /* Wait for routing activation response */ + int timeout_ms = DOIP_DEFAULT_TIMEOUT_MS; + clock_t start = clock(); + + while (tp->state == DOIP_STATE_ROUTING_ACTIVATION_PENDING) { + clock_t elapsed_ticks = clock() - start; + clock_t elapsed_ms = (elapsed_ticks * 1000LL) / CLOCKS_PER_SEC; + int remaining_ms = timeout_ms - elapsed_ms; + + if (remaining_ms <= 0) { + UDS_LOGE(__FILE__, "DoIP: Routing activation timeout"); + doip_change_state(tp, DOIP_STATE_ERROR); + return -1; + } + + if (doip_receive_data(tp, remaining_ms) < 0) { + return -1; + } + } + + if (tp->state != DOIP_STATE_READY_FOR_DIAG_REQUEST) { + UDS_LOGE(__FILE__, "DoIP: Routing activation failed"); + return -1; + } + + return 0; +} + +/** + * @brief Send diagnostic (UDS) message via DoIP. The UDS message is wrapped in a DoIP diagnostic + * message, consisting of a DoIP header and a diagnostic message header (source and target + * addresses). + * @param tp DoIP client context + * @param data Pointer to diagnostic message data. This is the UDS payload. + * @param len Length of diagnostic message data (of UDS payload) + */ +ssize_t doip_client_send_diag_message(DoIPClient_t *tp, const uint8_t *data, size_t len) { + /* Use shared tx_buffer instead of stack allocation (embedded-friendly) */ + uint8_t *payload = tp->tx_buffer; + if (len + 4 > DOIP_BUFFER_SIZE) { + UDS_LOGE(__FILE__, "DoIP: Message too large: %zu bytes > %d", len + 4, DOIP_BUFFER_SIZE); + return -1; + } + + doip_change_state(tp, DOIP_STATE_DIAG_MESSAGE_SEND_PENDING); + + /* Add diagnostic message header (source and target addresses) */ + payload[0] = (tp->source_address >> 8) & 0xFF; + payload[1] = tp->source_address & 0xFF; + payload[2] = (tp->target_address >> 8) & 0xFF; + payload[3] = tp->target_address & 0xFF; + memcpy(payload + 4, data, len); + + /* Reset ACK/NACK flags */ + tp->diag_ack_received = false; + tp->diag_nack_received = false; + + int sent = doip_send_message(tp, DOIP_PAYLOAD_TYPE_DIAG_MESSAGE, payload, len + 4); + + if (sent < 0) { + return -1; + } + + /* Wait for ACK/NACK of DoIP server*/ + doip_change_state(tp, DOIP_STATE_DIAG_MESSAGE_ACK_PENDING); + int timeout_ms = DOIP_ACK_TIMEOUT_MS; /* 1 second for ACK */ + clock_t start = clock(); + + while (!tp->diag_ack_received && !tp->diag_nack_received) { + clock_t elapsed_ticks = clock() - start; + clock_t elapsed_ms = (elapsed_ticks * 1000LL) / CLOCKS_PER_SEC; + int remaining_ms = timeout_ms - elapsed_ms; + + if (remaining_ms <= 0) { + UDS_LOGE(__FILE__, "DoIP: Diagnostic message ACK timeout"); + return -1; + } + + if (doip_receive_data(tp, remaining_ms) < 0) { + return -1; + } + } + + // NACK received -> report error and fall back to idle state + if (tp->diag_nack_received) { + doip_change_state(tp, DOIP_STATE_READY_FOR_DIAG_REQUEST); + UDS_LOGE(__FILE__, "DoIP: Diagnostic message rejected (NACK code=0x%02X)", + tp->diag_nack_code); + return -1; + } + + doip_change_state(tp, DOIP_STATE_DIAG_MESSAGE_RESPONSE_PENDING); + + return sent; +} + +/** + * @brief Process DoIP client events (call periodically). + * @param tp DoIP client context + * @param timeout_ms Timeout in milliseconds for receiving data + */ +void doip_client_process(DoIPClient_t *tp, int timeout_ms) { + if (tp->state == DOIP_STATE_READY_FOR_DIAG_REQUEST) { + doip_receive_data(tp, timeout_ms); + } +} + +/** + * @brief Disconnect from DoIP server + * @param tp DoIP client context + */ +void doip_client_disconnect(DoIPClient_t *tp) { + tp->tcp.close(&tp->tcp); + + doip_change_state(tp, DOIP_STATE_DISCONNECTED); + tp->rx_offset = 0; + UDS_LOGI(__FILE__, "DoIP Client: Disconnected"); +} + +/** + * @brief Populates SDU info structure for DoIP transport layer. + * + * @param hdl Handle to DoIP transport layer + * @param info Pointer to SDU info structure to populate + */ +void doip_update_sdu_info(const UDSTp_t *hdl, UDSSDU_t *info) { + if (NULL == info || NULL == hdl) { + return; + } + + const DoIPClient_t *impl = (const DoIPClient_t *)hdl; + info->A_Mtype = UDS_A_MTYPE_DIAG; + info->A_SA = impl->source_address; + info->A_TA = impl->target_address; + info->A_TA_Type = UDS_A_TA_TYPE_PHYSICAL; + info->A_AE = UDS_TP_NOOP_ADDR; +} + +/* -------------------------------------------------------------------------------- + * UDS Transport Layer Interface Functions (send, recv, poll) + * -------------------------------------------------------------------------------- */ + +/** + * @brief Send UDS message via DoIP transport layer. + * + * @param hdl Handle to DoIP transport layer + * @param buf Pointer to buffer containing UDS message + * @param len Length of UDS message + * @param info Pointer to SDU info structure (optional) + * @return ssize_t Number of bytes sent, or negative on error + */ + +/* NOTE: SonarCube complains about missing const, but the interface requires non-const */ +static ssize_t doip_tp_send(UDSTp_t *hdl, uint8_t *buf, size_t len, UDSSDU_t *info) { + UDS_ASSERT(hdl); + ssize_t ret = -1; + DoIPClient_t *impl = (DoIPClient_t *)hdl; + + ret = doip_client_send_diag_message(impl, buf, len); + if (ret < 0) { + UDS_LOGE(__FILE__, "DoIP TP Send Error"); + } else { + UDS_LOG_SDU(__FILE__, buf, len, info); + UDS_LOGD(__FILE__, "DoIP TP Send: Sent %zd bytes", ret); + } + + // Populate SDU info if provided (physical addressing semantics on DoIP) + doip_update_sdu_info(hdl, info); + return ret; +} + +/** + * @brief Receive UDS message via DoIP transport layer. + * + * @param hdl Handle to DoIP transport layer + * @param buf Pointer to buffer to store received UDS message + * @param bufsize Size of the buffer + * @param info Pointer to SDU info structure (optional) + * @return ssize_t Number of bytes received, or negative on error + */ +static ssize_t doip_tp_recv(UDSTp_t *hdl, uint8_t *buf, size_t bufsize, UDSSDU_t *info) { + UDS_ASSERT(hdl); + UDS_ASSERT(buf); + DoIPClient_t *impl = (DoIPClient_t *)hdl; + + // Try to receive any pending data (non-blocking poll inside) + ssize_t rc = doip_receive_data(impl, 0); + UDS_LOGD(__FILE__, "DoIP TP Recv: doip_receive_data returned %zd", rc); + + // If we have a diagnostic response stored, return it + if (impl->state == DOIP_STATE_DIAG_MESSAGE_RESPONSE_PENDING && impl->uds_response_len > 0) { + size_t n = impl->uds_response_len; + if (n > bufsize) { + n = bufsize; + } + UDS_LOG_SDU(__FILE__, impl->uds_response, n, NULL); + memcpy(buf, impl->uds_response, n); + impl->uds_response_len = 0; // consume buffered data + doip_change_state(impl, DOIP_STATE_READY_FOR_DIAG_REQUEST); + + // Populate SDU info if provided (physical addressing semantics on DoIP) + doip_update_sdu_info(hdl, info); + UDS_LOG_SDU(__FILE__, buf, n, info); + return (ssize_t)n; + } + + return rc; +} +/** + * @brief Poll DoIP transport layer status + * @note Checks if the transport layer is ready to send/receive + * @return UDS_TP_IDLE if idle, otherwise UDS_TP_SEND_IN_PROGRESS or UDS_TP_RECV_COMPLETE + */ +static UDSTpStatus_t doip_tp_poll(UDSTp_t *hdl) { + UDS_ASSERT(hdl); + UDSTpStatus_t status = 0; + DoIPClient_t *impl = (DoIPClient_t *)hdl; + + // Basic connectivity check + if (impl->state == DOIP_STATE_DISCONNECTED || impl->tcp.fd < 0) { + status |= UDS_TP_ERR; + return status; + } + + // Pump the socket to process incoming data without blocking + ssize_t rc = doip_receive_data(impl, 0); + UDS_LOGV(__FILE__, "DoIP TP Poll: after receive_data rc=%zd", rc); + + if (impl->state == DOIP_STATE_READY_FOR_DIAG_REQUEST) { + status |= UDS_TP_IDLE; + return status; + } + + if (rc < 0) { + status |= UDS_TP_ERR; + return status; + } + + if (impl->state == DOIP_STATE_DIAG_MESSAGE_ACK_PENDING) { + // 1) If waiting for ACK/NACK, mark send in progress until one arrives. + if (!impl->diag_ack_received && !impl->diag_nack_received) { + status |= UDS_TP_SEND_IN_PROGRESS; + } else if (impl->diag_nack_received) { + status |= UDS_TP_ERR; + } else { + // ACK received; now expect diagnostic response + status |= UDS_TP_SEND_IN_PROGRESS; + } + return status; + } + + if (impl->state == DOIP_STATE_DIAG_MESSAGE_RESPONSE_PENDING) { + + // 2) If waiting for diagnostic response, indicate completion when data buffered + if (impl->uds_response_len > 0) { + status |= UDS_TP_RECV_COMPLETE; + } else { + status |= UDS_TP_SEND_IN_PROGRESS; // still waiting on response + } + return status; + } + + // Any other state is considered an error for transport purposes + status |= UDS_TP_ERR; + return status; +} + +UDSErr_t UDSDoIPInitClient(DoIPClient_t *tp, const char *ipaddress, uint16_t port, + uint16_t source_addr, uint16_t target_addr) { + if (tp == NULL || ipaddress == NULL) { + return UDS_ERR_INVALID_ARG; + } + + memset(tp, 0, sizeof(DoIPClient_t)); + + doip_change_state(tp, DOIP_STATE_DISCONNECTED); + tp->source_address = source_addr; + tp->target_address = target_addr; + + /* Copy server IP address with guaranteed null-termination */ + snprintf(tp->server_ip, sizeof(tp->server_ip), "%s", ipaddress); + if (tp->server_ip[0] == '\0') { + UDS_LOGE(__FILE__, "UDS DoIP Client: Invalid server IP address"); + return UDS_ERR_INVALID_ARG; + } + tp->server_port = port; + + tp->hdl.send = doip_tp_send; + tp->hdl.recv = doip_tp_recv; + tp->hdl.poll = doip_tp_poll; + + memset(&tp->tcp, 0, sizeof(tp->tcp)); + memset(&tp->udp, 0, sizeof(tp->udp)); + +#ifdef DOIP_MOCK_TP + /* For testing purposes, use mock transport implementations */ + tp->tcp.init = doip_mock_tcp_init; + tp->tcp.connect = doip_mock_tcp_connect; + tp->tcp.send = doip_mock_tcp_send; + tp->tcp.recv = doip_mock_tcp_recv; + tp->tcp.close = doip_mock_tcp_close; + + tp->udp.init = doip_mock_udp_init; + tp->udp.sendto = doip_mock_udp_sendto; + tp->udp.recv = doip_mock_udp_recv; + tp->udp.recvfrom = doip_mock_udp_recvfrom; + tp->udp.close = doip_mock_udp_close; +#else + /* Use real TCP/UDP transport implementations */ + tp->tcp.init = doip_tp_tcp_init; + tp->tcp.connect = doip_tp_tcp_connect; + tp->tcp.send = doip_tp_tcp_send; + tp->tcp.recv = doip_tp_tcp_recv; + tp->tcp.close = doip_tp_tcp_close; + + tp->udp.init = doip_tp_udp_init; + tp->udp.sendto = doip_tp_udp_sendto; + tp->udp.recv = doip_tp_udp_recv; + tp->udp.recvfrom = doip_tp_udp_recvfrom; + tp->udp.close = doip_tp_udp_close; + tp->udp.join_multicast = doip_tp_udp_join_default_multicast; +#endif + + // sanity checks for transport functions + if (tp->tcp.init == NULL || tp->tcp.connect == NULL || tp->tcp.send == NULL || + tp->tcp.recv == NULL || tp->tcp.close == NULL) { + UDS_LOGE(__FILE__, "UDS DoIP Client: TCP transport functions not set"); + return UDS_ERR_TPORT; + } + + if (tp->udp.init == NULL || tp->udp.sendto == NULL || tp->udp.recv == NULL || + tp->udp.recvfrom == NULL || tp->udp.close == NULL || tp->udp.join_multicast == NULL) { + UDS_LOGE(__FILE__, "UDS DoIP Client: UDP transport functions not set"); + return UDS_ERR_TPORT; + } + + UDS_LOGI(__FILE__, "UDS DoIP Client: Initialized (SA=0x%04X, TA=0x%04X)", tp->source_address, + tp->target_address); + + if (doip_client_connect(tp)) { + UDS_LOGE(__FILE__, "UDS DoIP Client: Connect error"); + return UDS_ERR_TPORT; + } + + if (doip_client_activate_routing(tp)) { + UDS_LOGE(__FILE__, "UDS DoIP Client: Routing activation error"); + return UDS_ERR_TPORT; + } + + return UDS_OK; +} + +UDSErr_t UDSDoIPActivateRouting(DoIPClient_t *tp) { + if (tp == NULL) { + return UDS_ERR_INVALID_ARG; + } + + if (doip_client_activate_routing(tp)) { + UDS_LOGE(__FILE__, "UDS DoIP Client: Routing activation error"); + return UDS_ERR_TPORT; + } + return UDS_OK; +} + +void UDSDoIPDeinit(DoIPClient_t *tp) { + if (tp == NULL) { + return; + } + + doip_client_disconnect(tp); +} + +/* -------------------------------------------------------------- + * Selection callback support + * -------------------------------------------------------------- */ +static DoIPSelectServerFn g_select_fn = NULL; +static void *g_select_user = NULL; +static bool g_discovery_request_only = false; +static bool g_discovery_dump_raw = false; + +void UDSDoIPSetSelectionCallback(DoIPClient_t *tp, DoIPSelectServerFn fn, void *user) { + (void)tp; /* per-client not required; use global for simplicity */ + g_select_fn = fn; + g_select_user = user; +} + +void UDSDoIPSetDiscoveryOptions(bool request_only, bool dump_raw) { + g_discovery_request_only = request_only; + g_discovery_dump_raw = dump_raw; +} + +/* -------------------------------------------------------------- + * UDP discovery: collect responders within timeout, allow selection + * -------------------------------------------------------------- */ +int UDSDoIPDiscoverVehiclesEx(DoIPClient_t *tp, int timeout_ms, bool loopback, uint16_t port) { + if (!tp) + return -1; + tp->udp.loopback = loopback; + if (tp->udp.init(&tp->udp, port, loopback) < 0) { + UDS_LOGE(__FILE__, "DoIP UDP: init failed"); + return -1; + } + if (!loopback && !g_discovery_request_only) { + if (tp->udp.join_multicast(&tp->udp) < 0) { + UDS_LOGE(__FILE__, "DoIP UDP: multicast join failed"); + doip_tp_udp_close(&tp->udp); + return -1; + } + } + + /* Actively send a Vehicle Identification Request */ + { + uint8_t req[DOIP_HEADER_SIZE]; + req[0] = DOIP_PROTOCOL_VERSION; + req[1] = DOIP_PROTOCOL_VERSION_INV; + req[2] = 0x00; /* payload_type MSB: 0x0001 */ + req[3] = 0x01; /* payload_type LSB */ + req[4] = 0x00; /* payload_length: 0 */ + req[5] = 0x00; + req[6] = 0x00; + req[7] = 0x00; + + const char *dst_ip = loopback ? "127.0.0.1" : "255.255.255.255"; + uint16_t dst_port = DOIP_UDP_DISCOVERY_PORT; + ssize_t sent = doip_tp_udp_sendto(&tp->udp, req, sizeof(req), dst_ip, dst_port, 500); + if (sent <= 0) { + UDS_LOGW(__FILE__, "DoIP UDP: VI request send failed (dst %s:%u)", dst_ip, dst_port); + } else { + UDS_LOGI(__FILE__, "DoIP UDP: sent Vehicle Identification Request to %s:%u", dst_ip, + dst_port); + } + } + + int found = 0; + const int slice_ms = 200; + const int resend_interval_ms = 500; + int elapsed_ms = 0; + int sent_count = 1; /* already sent one request above */ + int remaining = timeout_ms > 0 ? timeout_ms : 0; + /* Use shared tx_buffer for UDP receive (eliminates 4KB stack allocation) */ + uint8_t *buf = tp->tx_buffer; + char src_ip[64]; + uint16_t src_port = 0; + + while (remaining > 0) { + int win = remaining < slice_ms ? remaining : slice_ms; + ssize_t n = doip_tp_udp_recvfrom(&tp->udp, buf, DOIP_BUFFER_SIZE, win, src_ip, sizeof(src_ip), + &src_port); + if (n < 0) { + UDS_LOGE(__FILE__, "DoIP UDP: recvfrom error"); + break; + } else if (n == 0) { + remaining -= win; + elapsed_ms += win; + if (elapsed_ms >= sent_count * resend_interval_ms && found == 0) { + /* Resend VI request to improve discovery probability */ + uint8_t req2[DOIP_HEADER_SIZE] = {DOIP_PROTOCOL_VERSION, + DOIP_PROTOCOL_VERSION_INV, + 0x00, + 0x01, + 0x00, + 0x00, + 0x00, + 0x00}; + const char *dst_ip2 = loopback ? "127.0.0.1" : "255.255.255.255"; + uint16_t dst_port2 = DOIP_UDP_DISCOVERY_PORT; + (void)doip_tp_udp_sendto(&tp->udp, req2, sizeof(req2), dst_ip2, dst_port2, 200); + UDS_LOGV(__FILE__, "DoIP UDP: re-sent VI request to %s:%u", dst_ip2, dst_port2); + sent_count++; + } + continue; + } + + found++; + UDS_LOGI(__FILE__, "DoIP UDP: discovery frame from %s:%u (%zd bytes)", src_ip, src_port, n); + if (g_discovery_dump_raw) { + UDS_LOG_SDU(__FILE__, buf, (size_t)n, NULL); + } + + DoIPDiscoveryInfo info; + memset(&info, 0, sizeof(info)); + snprintf(info.ip, sizeof(info.ip), "%s", src_ip); + info.remote_port = src_port; + + /* Parse DoIP header to extract known fields from known payload types */ + if ((size_t)n >= DOIP_HEADER_SIZE) { + DoIPHeader_t hdr; + if (doip_header_parse(buf, &hdr)) { + const uint8_t *pl = buf + DOIP_HEADER_SIZE; + size_t plen = hdr.payload_length; + /* Vehicle identification response/announcement payloads commonly start with VIN */ + if (plen >= 17) { + memcpy(info.vin, pl, 17); + info.vin[17] = '\0'; + } + /* Next 6 bytes often EID, then 6 bytes GID */ + if (plen >= 23) { + for (int i = 0; i < 6; ++i) { + sprintf(&info.eid[i * 2], "%02X", pl[17 + i]); + } + info.eid[12] = '\0'; + } + if (plen >= 29) { + for (int i = 0; i < 6; ++i) { + sprintf(&info.gid[i * 2], "%02X", pl[23 + i]); + } + info.gid[12] = '\0'; + } + if (info.vin[0] != '\0') { + UDS_LOGI(__FILE__, "DoIP UDP: parsed VIN=%s from %s:%u", info.vin, src_ip, + src_port); + } + } else { + /* Fallback: coarse VIN scan in entire datagram */ + if ((size_t)n >= 17) { + for (size_t i = 0; i + 17 <= (size_t)n; ++i) { + bool printable = true; + for (size_t j = 0; j < 17; ++j) { + uint8_t c = buf[i + j]; + if (!(c >= '0' && c <= '9') && !(c >= 'A' && c <= 'Z')) { + printable = false; + break; + } + } + if (printable) { + memcpy(info.vin, buf + i, 17); + info.vin[17] = '\0'; + break; + } + } + } + if (info.vin[0] != '\0') { + UDS_LOGI(__FILE__, "DoIP UDP: heuristic VIN=%s from %s:%u", info.vin, src_ip, + src_port); + } + } + } + + /* Invoke selection callback if provided */ + bool choose = false; + if (g_select_fn) { + choose = g_select_fn(&info, g_select_user); + } else { + /* Default: choose first responder */ + if (found == 1) + choose = true; + } + + if (choose) { + snprintf(tp->server_ip, sizeof(tp->server_ip), "%s", info.ip); + tp->server_port = DOIP_TCP_PORT; /* default TCP port */ + UDS_LOGI(__FILE__, "DoIP: selected server %s:%u", tp->server_ip, tp->server_port); + break; /* stop after selection */ + } + + remaining -= win; + } + + doip_tp_udp_close(&tp->udp); + return found; +} + +int UDSDoIPDiscoverVehicles(DoIPClient_t *tp, int timeout_ms, bool loopback) { + /* By default, listen on the tester request port (13401) to receive announcements */ + return UDSDoIPDiscoverVehiclesEx(tp, timeout_ms, loopback, + DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT); +} + +#endif /* UDS_TP_DOIP */ + +#ifdef UDS_LINES +#line 1 "src/tp/doip/doip_tp_udp.c" +#endif +#if defined(UDS_TP_DOIP) + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/* Default DoIP multicast group for discovery */ +static const char *DOIP_DEFAULT_MCAST = "224.224.224.224"; /* per ISO 13400 */ + +#ifdef DOIP_MOCK_TP + +int doip_tp_mock_udp_init(DoIPUdpTransport *udp, uint16_t port, bool loopback) { + (void)udp; + (void)port; + (void)loopback; + return 0; +} + +ssize_t doip_tp_mock_udp_recv(DoIPUdpTransport *udp, uint8_t *buf, size_t len, int timeout_ms) { + (void)udp; + (void)buf; + (void)len; + (void)timeout_ms; + return 0; +} + +ssize_t doip_tp_mock_udp_recvfrom(DoIPUdpTransport *udp, uint8_t *buf, size_t len, int timeout_ms, + char *src_ip_out, size_t src_ip_out_sz, uint16_t *src_port_out) { + (void)udp; + (void)buf; + (void)len; + (void)timeout_ms; + (void)src_ip_out; + (void)src_ip_out_sz; + (void)src_port_out; + return 0; +} + +void doip_tp_mock_udp_close(DoIPUdpTransport *udp) { + (void)udp; +} + +ssize_t doip_tp_mock_udp_sendto(DoIPUdpTransport *udp, const uint8_t *buf, size_t len, + const char *dst_ip, uint16_t dst_port, int timeout_ms) { + (void)udp; + (void)buf; + (void)len; + (void)dst_ip; + (void)dst_port; + (void)timeout_ms; + return 0; +} + +int doip_tp_mock_udp_join_default_multicast(DoIPUdpTransport *udp) { + (void)udp; + return 0; +} + +#else +int doip_tp_udp_init(DoIPUdpTransport *udp, uint16_t port, bool loopback) { + if (!udp) return -1; + + udp->fd = -1; + udp->loopback = loopback; + /* For tester discovery, default listen port is 13401 */ + udp->port = port ? port : DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT; + + int fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + + int reuse = 1; + (void)setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + + if (loopback) { + /* Bind to loopback UDP to allow local discovery testing */ + struct sockaddr_in sa; + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_port = htons(udp->port); + sa.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + if (bind(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0) { + close(fd); + return -1; + } + + unsigned char on = 1; + (void)setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &on, sizeof(on)); + } else { + /* Bind on any address for multicast */ + struct sockaddr_in sa; + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_port = htons(udp->port); + sa.sin_addr.s_addr = htonl(INADDR_ANY); + if (bind(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0) { + close(fd); + return -1; + } + + /* Enable broadcast for sending to 255.255.255.255 */ + int broadcast = 1; + (void)setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); + } + + /* set non-blocking after successful bind */ + int flags = fcntl(fd, F_GETFL, 0); + if (flags < 0 || fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) { + close(fd); + return -1; + } + + udp->fd = fd; + return 0; +} + +int doip_tp_udp_join_default_multicast(DoIPUdpTransport *udp) { + if (!udp || udp->fd < 0) return -1; + if (udp->loopback) return 0; /* no multicast join needed */ + + struct ip_mreq mreq; + memset(&mreq, 0, sizeof(mreq)); + mreq.imr_multiaddr.s_addr = inet_addr(DOIP_DEFAULT_MCAST); + mreq.imr_interface.s_addr = htonl(INADDR_ANY); + if (setsockopt(udp->fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) { + return -1; + } + return 0; +} + +ssize_t doip_tp_udp_recv(DoIPUdpTransport *udp, uint8_t *buf, size_t len, int timeout_ms) { + if (!udp || udp->fd < 0 || !buf) return -1; + fd_set rfds; + struct timeval tv; + FD_ZERO(&rfds); + FD_SET(udp->fd, &rfds); + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + int ret = select(udp->fd + 1, &rfds, NULL, NULL, timeout_ms >= 0 ? &tv : NULL); + if (ret < 0) return -1; + if (ret == 0) return 0; /* timeout */ + + return recv(udp->fd, buf, len, 0); +} + +ssize_t doip_tp_udp_recvfrom(DoIPUdpTransport *udp, uint8_t *buf, size_t len, int timeout_ms, + char *src_ip_out, size_t src_ip_out_sz, uint16_t *src_port_out) { + if (!udp || udp->fd < 0 || !buf) return -1; + fd_set rfds; + struct timeval tv; + FD_ZERO(&rfds); + FD_SET(udp->fd, &rfds); + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + int ret = select(udp->fd + 1, &rfds, NULL, NULL, timeout_ms >= 0 ? &tv : NULL); + if (ret < 0) return -1; + if (ret == 0) return 0; /* timeout */ + + struct sockaddr_in src; + socklen_t slen = sizeof(src); + ssize_t n = recvfrom(udp->fd, buf, len, 0, (struct sockaddr *)&src, &slen); + if (n <= 0) return n; + if (src_ip_out && src_ip_out_sz > 0) { + const char *ip = inet_ntoa(src.sin_addr); + if (ip) { + snprintf(src_ip_out, src_ip_out_sz, "%s", ip); + } + } + if (src_port_out) { + *src_port_out = ntohs(src.sin_port); + } + return n; +} + +void doip_tp_udp_close(DoIPUdpTransport *udp) { + if (!udp) return; + if (udp->fd >= 0) { + close(udp->fd); + udp->fd = -1; + } +} + +ssize_t doip_tp_udp_sendto(DoIPUdpTransport *udp, const uint8_t *buf, size_t len, + const char *dst_ip, uint16_t dst_port, int timeout_ms) { + if (!udp || udp->fd < 0 || !buf || !dst_ip) return -1; + + fd_set wfds; + struct timeval tv; + FD_ZERO(&wfds); + FD_SET(udp->fd, &wfds); + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + int ret = select(udp->fd + 1, NULL, &wfds, NULL, timeout_ms >= 0 ? &tv : NULL); + if (ret < 0) return -1; + if (ret == 0) return 0; /* timeout */ + + struct sockaddr_in dst; + memset(&dst, 0, sizeof(dst)); + dst.sin_family = AF_INET; + dst.sin_port = htons(dst_port); + if (inet_pton(AF_INET, dst_ip, &dst.sin_addr) <= 0) { + return -1; + } + + return sendto(udp->fd, buf, len, 0, (struct sockaddr *)&dst, sizeof(dst)); +} +#endif /* DOIP_MOCK_TP */ +#endif /* UDS_TP_DOIP */ + + +#ifdef UDS_LINES +#line 1 "src/tp/doip/doip_tp_tcp.c" +#endif +#if defined(UDS_TP_DOIP) + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void doip_tp_set_timeouts(DoIPTcpTransport *tcp, int connect_timeout_ms, int send_timeout_ms) { + if (!tcp) return; + if (connect_timeout_ms > 0) tcp->connect_timeout_ms = connect_timeout_ms; + if (send_timeout_ms > 0) tcp->send_timeout_ms = send_timeout_ms; +} + +#ifdef DOIP_MOCK_TP +int doip_tp_mock_init(DoIPTcpTransport *tcp, const char *ip, uint16_t port) { + (void)tcp; + (void)ip; + (void)port; + return 0; +} + +int doip_tp_mock_connect(DoIPTcpTransport *tcp) { + (void)tcp; + return 0; +} + +ssize_t doip_tp_mock_send(DoIPTcpTransport *tcp, const uint8_t *buf, size_t len) { + (void)tcp; + (void)buf; + return (ssize_t)len; +} + +ssize_t doip_tp_mock_recv(DoIPTcpTransport *tcp, uint8_t *buf, size_t len, int timeout_ms) { + (void)tcp; + (void)buf; + (void)len; + (void)timeout_ms; + return 0; +} + +void doip_tp_mock_close(DoIPTcpTransport *tcp) { + (void)tcp; +} + +#else + +int doip_tp_tcp_init(DoIPTcpTransport *tcp, const char *ip, uint16_t port) { + if (!tcp || !ip) return -1; + + tcp->fd = -1; + tcp->port = port ? port : DOIP_TCP_PORT; + snprintf(tcp->ip, sizeof(tcp->ip), "%s", ip); + tcp->connect_timeout_ms = DOIP_DEFAULT_TIMEOUT_MS; + tcp->send_timeout_ms = DOIP_DEFAULT_TIMEOUT_MS; + return 0; +} + +int doip_tp_tcp_connect(DoIPTcpTransport *tcp) { + if (!tcp) return -1; + tcp->fd = socket(AF_INET, SOCK_STREAM, 0); + if (tcp->fd < 0) { + return -1; + } + + /* set non-blocking before connect to avoid blocking connect */ + int flags = fcntl(tcp->fd, F_GETFL, 0); + if (flags < 0 || fcntl(tcp->fd, F_SETFL, flags | O_NONBLOCK) < 0) { + close(tcp->fd); + tcp->fd = -1; + return -1; + } + + struct sockaddr_in sa; + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_port = htons(tcp->port); + if (inet_pton(AF_INET, tcp->ip, &sa.sin_addr) <= 0) { + close(tcp->fd); + tcp->fd = -1; + return -1; + } + int rc = connect(tcp->fd, (struct sockaddr *)&sa, sizeof(sa)); + if (rc < 0) { + if (errno != EINPROGRESS) { + close(tcp->fd); + tcp->fd = -1; + return -1; + } + /* wait for writability or error within default timeout */ + fd_set wfds; + FD_ZERO(&wfds); + FD_SET(tcp->fd, &wfds); + struct timeval tv; + int cto = (tcp->connect_timeout_ms > 0) ? tcp->connect_timeout_ms : DOIP_DEFAULT_TIMEOUT_MS; + tv.tv_sec = cto / 1000; + tv.tv_usec = (cto % 1000) * 1000; + rc = select(tcp->fd + 1, NULL, &wfds, NULL, &tv); + if (rc <= 0) { + /* timeout or select error */ + close(tcp->fd); + tcp->fd = -1; + return -1; + } + int soerr = 0; + socklen_t slen = sizeof(soerr); + if (getsockopt(tcp->fd, SOL_SOCKET, SO_ERROR, &soerr, &slen) < 0 || soerr != 0) { + close(tcp->fd); + tcp->fd = -1; + return -1; + } + } + return 0; +} + +ssize_t doip_tp_tcp_send(const DoIPTcpTransport *tcp, const uint8_t *buf, size_t len) { + if (!tcp || tcp->fd < 0 || !buf) return -1; + size_t total = 0; + int sflags = 0; +#ifdef MSG_NOSIGNAL + sflags |= MSG_NOSIGNAL; +#endif + while (total < len) { + fd_set wfds; + FD_ZERO(&wfds); + FD_SET(tcp->fd, &wfds); + struct timeval tv; + int sto = (tcp->send_timeout_ms > 0) ? tcp->send_timeout_ms : DOIP_DEFAULT_TIMEOUT_MS; + tv.tv_sec = sto / 1000; + tv.tv_usec = (sto % 1000) * 1000; + int rc = select(tcp->fd + 1, NULL, &wfds, NULL, &tv); + if (rc <= 0) { + /* timeout or error */ + return -1; + } + ssize_t n = send(tcp->fd, buf + total, len - total, sflags); + if (n > 0) { + total += (size_t)n; + continue; + } + if (n < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) { + /* try again after select */ + continue; + } + /* other error or peer closed */ + return -1; + } + return (ssize_t)total; +} + +ssize_t doip_tp_tcp_recv(DoIPTcpTransport *tcp, uint8_t *buf, size_t len, int timeout_ms) { + if (!tcp || tcp->fd < 0 || !buf) return -1; + + fd_set rfds; + struct timeval tv; + FD_ZERO(&rfds); + FD_SET(tcp->fd, &rfds); + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + int ret = select(tcp->fd + 1, &rfds, NULL, NULL, timeout_ms >= 0 ? &tv : NULL); + if (ret < 0) { + return -1; + } + if (ret == 0) { + return 0; /* timeout */ + } + return recv(tcp->fd, buf, len, 0); +} + +void doip_tp_tcp_close(DoIPTcpTransport *tcp) { + if (!tcp) return; + if (tcp->fd >= 0) { + close(tcp->fd); + tcp->fd = -1; + } +} +#endif /* DOIP_MOCK_TP */ + +#endif /* UDS_TP_DOIP */ + #if defined(UDS_TP_ISOTP_C) #ifndef ISO_TP_USER_SEND_CAN_ARG #error diff --git a/iso14229.h b/iso14229.h index b7dc7076c..bd189eb50 100644 --- a/iso14229.h +++ b/iso14229.h @@ -1757,6 +1757,375 @@ void ISOTPMockReset(void); #endif +#ifndef DOIP_DEFINES_H +#define DOIP_DEFINES_H + +#include + +/* DoIP Protocol Constants */ +#define DOIP_PROTOCOL_VERSION 0x03 +#define DOIP_PROTOCOL_VERSION_INV 0xFC +#define DOIP_TCP_PORT 13400 +#define DOIP_HEADER_SIZE 8 + +/* Header size for diagnostic message (source address + target address) */ +#define DOIP_DIAG_HEADER_SIZE 4 + +/* DoIP Payload Types (table 17)*/ +#define DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_REQ 0x0005 +#define DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_RES 0x0006 +#define DOIP_PAYLOAD_TYPE_ALIVE_CHECK_REQ 0x0007 +#define DOIP_PAYLOAD_TYPE_ALIVE_CHECK_RES 0x0008 +#define DOIP_PAYLOAD_TYPE_DIAG_MESSAGE 0x8001 +#define DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_POS_ACK 0x8002 +#define DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_NEG_ACK 0x8003 + +/* DoIP Routing Activation Response Codes (table 56)*/ +#define DOIP_ROUTING_ACTIVATION_RES_SUCCESS 0x10 +#define DOIP_ROUTING_ACTIVATION_RES_UNKNOWN_SA 0x00 +#define DOIP_ROUTING_ACTIVATION_RES_ALREADY_ACTIVE 0x01 + +/* DoIP Diagnostic Message NACK Codes (table 26) */ +#define DOIP_DIAG_NACK_INVALID_SA 0x02 +#define DOIP_DIAG_NACK_UNKNOWN_TA 0x03 +#define DOIP_DIAG_NACK_MESSAGE_TOO_LARGE 0x04 +#define DOIP_DIAG_NACK_OUT_OF_MEMORY 0x05 +#define DOIP_DIAG_NACK_TARGET_UNREACHABLE 0x06 + +/* Configuration */ +#define DOIP_BUFFER_SIZE 4096 +#define DOIP_ROUTING_ACTIVATION_TYPE 0x00 +#define DOIP_DEFAULT_TIMEOUT_MS 5000 + + +/* DoIP Header Structure */ +typedef struct { + uint8_t protocol_version; /**< DoIP protocol version (table 16). 1=2010, 2=2012, 3=2019, 4=2019-Amd1,2025 */ + uint8_t protocol_version_inv; /**< Inverse of protocol version */ + uint16_t payload_type; /**< Payload type (table 17) */ + uint32_t payload_length; /**< Payload length */ +} DoIPHeader_t; + + +/* UDP ports (ISO 13400-2 Table 48) */ +#define DOIP_UDP_DISCOVERY_PORT 13400 +#define DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT 13401 + + + +#endif /* DOIP_DEFINES_H */ + + +#ifndef DOIP_TRANSPORT_H +#define DOIP_TRANSPORT_H +#if defined(UDS_TP_DOIP) + +#include +#include +#include + +/* Forward declaration */ +typedef struct DoIPTcpTransport DoIPTcpTransport; +typedef struct DoIPUdpTransport DoIPUdpTransport; + +#ifdef DOIP_MOCK_TP +/* Mock transport functions for testing. For now these method do nothing and return + * success. + */ + +int doip_tp_mock_init(DoIPTcpTransport *tcp, const char *ip, uint16_t port); +int doip_tp_mock_connect(DoIPTcpTransport *tcp); +ssize_t doip_tp_mock_send(DoIPTcpTransport *tcp, const uint8_t *buf, size_t len); +ssize_t doip_tp_mock_recv(DoIPTcpTransport *tcp, uint8_t *buf, size_t len, int timeout_ms); +void doip_tp_mock_close(DoIPTcpTransport *tcp); + +int doip_tp_mock_udp_init(DoIPUdpTransport *t, uint16_t port, bool loopback); +ssize_t doip_tp_mock_udp_recv(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms); +ssize_t doip_tp_mock_udp_recvfrom(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms, + char *src_ip_out, size_t src_ip_out_sz, uint16_t *src_port_out); +void doip_tp_mock_udp_close(DoIPUdpTransport *t); +ssize_t doip_tp_mock_udp_sendto(DoIPUdpTransport *t, const uint8_t *buf, size_t len, + const char *dst_ip, uint16_t dst_port, int timeout_ms); + +int doip_tp_mock_udp_join_default_multicast(DoIPUdpTransport *t); +#else + +/** + * @brief Initialize DoIP TCP transport + * + * @param t Transport context + * @param ip Remote IP address + * @param port Remote port + * @retval 0 success + * @retval -1 error + */ +int doip_tp_tcp_init(DoIPTcpTransport *tcp, const char *ip, uint16_t port); + +/** + * @brief Connect DoIP TCP transport + * + * @param t Transport context + * @retval 0 success + * @retval -1 error + */ +int doip_tp_tcp_connect(DoIPTcpTransport *tcp); + +/** + * @brief Send data over DoIP TCP transport + * + * @param t Transport context + * @param buf Data buffer to send + * @param len Length of data to send + * @return ssize_t Number of bytes sent, or -1 on error + */ +ssize_t doip_tp_tcp_send(const DoIPTcpTransport *tcp, const uint8_t *buf, size_t len); + +/** + * @brief Receive data over DoIP TCP transport + * + * @param t Transport context + * @param buf Buffer to receive data into + * @param len Length of buffer + * @param timeout_ms Receive timeout in milliseconds + * @return ssize_t Number of bytes received, 0 on timeout, or -1 on error + */ +ssize_t doip_tp_tcp_recv(DoIPTcpTransport *tcp, uint8_t *buf, size_t len, int timeout_ms); + +/** + * @brief Close DoIP TCP transport + * + * @param t Transport context + */ +void doip_tp_tcp_close(DoIPTcpTransport *tcp); + +/* Optional: configure timeouts on the transport */ +/** + * @brief Set timeouts for DoIP transport + * @param t Transport context + * @param connect_timeout_ms Connect timeout in milliseconds (<=0 for default) + * @param send_timeout_ms Send timeout in milliseconds (<=0 for default) + */ +void doip_tp_set_timeouts(DoIPTcpTransport *tcp, int connect_timeout_ms, int send_timeout_ms); + +/* UDP transport helpers (vehicle discovery) */ + +/** + * @brief Initialize DoIP UDP transport + * + * @param t Transport context + * @param port Local port + * @param loopback Enable loopback mode (instead of multicast) + * @retval 0 success + * @retval -1 error + */ +int doip_tp_udp_init(DoIPUdpTransport *t, uint16_t port, bool loopback); + +/** + * @brief Join default DoIP multicast group for discovery + * + * @param t Transport context + * @retval 0 success + * @retval -1 error + */ +int doip_tp_udp_join_default_multicast(DoIPUdpTransport *t); + +/** + * @brief Receive data over DoIP UDP transport + * + * @param t Transport context + * @param buf Buffer to receive data into + * @param len Length of buffer + * @param timeout_ms Receive timeout in milliseconds + * @return ssize_t Number of bytes received, 0 on timeout, or -1 on error + */ +ssize_t doip_tp_udp_recv(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms); + +/** + * @brief Receive data over DoIP UDP transport with source address info + * + * @param t Transport context + * @param buf Buffer to receive data into + * @param len Length of buffer + * @param timeout_ms Receive timeout in milliseconds + * @param src_ip_out Output buffer for source IP address + * @param src_ip_out_sz Size of source IP output buffer + * @param src_port_out Output for source port + * @return ssize_t Number of bytes received, 0 on timeout, or -1 on error + */ +ssize_t doip_tp_udp_recvfrom(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms, + char *src_ip_out, size_t src_ip_out_sz, uint16_t *src_port_out); + +/** + * @brief Close DoIP UDP transport + * + * @param t Transport context + */ +void doip_tp_udp_close(DoIPUdpTransport *t); + +/* UDP send helper */ +/** + * @brief Send data over DoIP UDP transport + * + * @param t Transport context + * @param buf Data buffer to send + * @param len Length of data to send + * @param dst_ip Destination IP address + * @param dst_port Destination port + * @param timeout_ms Send timeout in milliseconds + * @return ssize_t Number of bytes sent, or -1 on error + */ +ssize_t doip_tp_udp_sendto(DoIPUdpTransport *t, const uint8_t *buf, size_t len, const char *dst_ip, + uint16_t dst_port, int timeout_ms); + +#endif /* DOIP_MOCK_TP */ + +/* TCP transport function pointers */ +typedef int (*tcp_init)(DoIPTcpTransport *tcp, const char *ip, uint16_t port); +typedef int (*tcp_connect)(DoIPTcpTransport *tcp); +typedef ssize_t (*tcp_send)(const DoIPTcpTransport *tcp, const uint8_t *buf, size_t len); +typedef ssize_t (*tcp_recv)(DoIPTcpTransport *tcp, uint8_t *buf, size_t len, int timeout_ms); +typedef void (*tcp_close)(DoIPTcpTransport *tcp); + +/* UDP transport function pointers */ +typedef int (*udp_init)(DoIPUdpTransport *t, uint16_t port, bool loopback); +typedef ssize_t (*udp_recv)(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms); +typedef ssize_t (*udp_recvfrom)(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms, + char *src_ip_out, size_t src_ip_out_sz, uint16_t *src_port_out); +typedef ssize_t (*udp_sendto)(DoIPUdpTransport *t, const uint8_t *buf, size_t len, const char *dst_ip, + uint16_t dst_port, int timeout_ms); +typedef void (*udp_close)(DoIPUdpTransport *t); +typedef int (*udp_join_multicast)(DoIPUdpTransport *t); + + +typedef struct DoIPTcpTransport { + int fd; + int connect_timeout_ms; /* connect timeout (ms), <=0 uses default */ + int send_timeout_ms; /* send timeout (ms), <=0 uses default */ + char ip[64]; /* remote IP (for TCP) */ + uint16_t port; /* local or remote port */ + tcp_init init; + tcp_connect connect; + tcp_send send; + tcp_recv recv; + tcp_close close; +} DoIPTcpTransport; + +typedef struct DoIPUdpTransport { + int fd; + uint16_t port; /* local or remote port */ + udp_init init; + udp_recv recv; + udp_recvfrom recvfrom; + udp_sendto sendto; + udp_close close; + udp_join_multicast join_multicast; + bool loopback; /* UDP loopback mode */ +} DoIPUdpTransport; + +#endif /* UDS_TP_DOIP */ + +#endif /* DOIP_TRANSPORT_H */ + + +#if defined(UDS_TP_DOIP) + + + +#define DOIP_ACK_TIMEOUT_MS 1000 /* 1 second for Diagnostic ACK (0x8002 or 0x8003)*/ + +/* DoIP Client State */ +typedef enum { + DOIP_STATE_DISCONNECTED, + DOIP_STATE_CONNECTED, + DOIP_STATE_ROUTING_ACTIVATION_PENDING, + DOIP_STATE_READY_FOR_DIAG_REQUEST, + // Diag message states for tracking ACK/NACK and responses + DOIP_STATE_DIAG_MESSAGE_SEND_PENDING, + DOIP_STATE_DIAG_MESSAGE_ACK_PENDING, + DOIP_STATE_DIAG_MESSAGE_RESPONSE_PENDING, + DOIP_STATE_ERROR +} DoIPClientState_t; + +/* DoIP Client Context */ +typedef struct { + UDSTp_t hdl; /* Must be the first entry! */ + //DoIPTransport tcp; /* TCP transport for diagnostics */ + //DoIPTransport udp; /* UDP transport for discovery */ + DoIPTcpTransport tcp; /* TCP transport for diagnostics */ + DoIPUdpTransport udp; /* UDP transport for discovery */ + DoIPClientState_t state; + + uint16_t source_address; /* Client logical address */ + uint16_t target_address; /* Server logical address */ + + char server_ip[64]; + uint16_t server_port; + //bool udp_loopback; /* discovery via loopback instead of multicast */ + + uint8_t rx_buffer[DOIP_BUFFER_SIZE]; /* Raw socket receive buffer */ + size_t rx_offset; + + uint8_t uds_response[DOIP_BUFFER_SIZE]; /* Processed UDS response data */ + size_t uds_response_len; + + uint8_t tx_buffer[DOIP_BUFFER_SIZE]; /* Reusable transmit buffer (eliminates stack allocations) */ + + bool routing_activated; + bool diag_ack_received; + bool diag_nack_received; + uint8_t diag_nack_code; +} DoIPClient_t; + +/* Discovery info (minimal set) */ +typedef struct { + char ip[64]; + uint16_t remote_port; + uint16_t logical_address; /* if known */ + char vin[18]; /* 17-char VIN plus NUL if parsed */ + char eid[13]; /* 6-byte EID as hex string */ + char gid[13]; /* 6-byte GID as hex string */ +} DoIPDiscoveryInfo; + +/* Optional selection callback */ +typedef bool (*DoIPSelectServerFn)(const DoIPDiscoveryInfo *info, void *user); + +void UDSDoIPSetSelectionCallback(DoIPClient_t *tp, DoIPSelectServerFn fn, void *user); +/* Discovery options */ +void UDSDoIPSetDiscoveryOptions(bool request_only, bool dump_raw); + +/** + * @brief Initialize DoIP client transport layer + * + * @param tp Pointer to DoIP client context + * @param ipaddress Server IP address as a string + * @param port Server port number + * @param source_addr Client logical address (range 0x0E00 - 0x0FFF) + * @param target_addr Server logical address + * @return UDSErr_t UDS_OK on success, error code otherwise + */ +UDSErr_t UDSDoIPInitClient(DoIPClient_t *tp, const char *ipaddress, uint16_t port, uint16_t source_addr, uint16_t target_addr); + +/** + * @brief Deinitialize DoIP client transport layer + * + * @param tp Pointer to DoIP client context + */ +void UDSDoIPDeinit(DoIPClient_t *tp); + +/** + * @brief Discover vehicles using UDP DoIP + * @param tp DoIP client context + * @param timeout_ms Receive timeout in ms + * @param loopback If true, use loopback instead of multicast + * @return number of discovery frames observed (>=0), or negative on error + */ +int UDSDoIPDiscoverVehicles(DoIPClient_t *tp, int timeout_ms, bool loopback); +/* Extended: allow overriding UDP port (0 = default 13400) */ +int UDSDoIPDiscoverVehiclesEx(DoIPClient_t *tp, int timeout_ms, bool loopback, uint16_t port); + +#endif + + #ifdef __cplusplus } #endif diff --git a/src/BUILD b/src/BUILD index c46e14681..890b9b93b 100644 --- a/src/BUILD +++ b/src/BUILD @@ -13,6 +13,9 @@ filegroup( "tp/isotp_mock.c", "tp/isotp_sock.c", "tp/isotp-c/isotp.c", + "tp/doip/doip_client.c", + "tp/doip/doip_tp_tcp.c", + "tp/doip/doip_tp_udp.c", ] ) @@ -40,6 +43,9 @@ filegroup( "tp/isotp-c/isotp_defines.h", "tp/isotp-c/isotp_user.h", "tp/isotp-c/isotp.h", + "tp/doip/doip_defines.h", + "tp/doip/doip_client.h", + "tp/doip/doip_transport.h", ] ) diff --git a/src/tp/doip/doip_client.c b/src/tp/doip/doip_client.c new file mode 100644 index 000000000..e1970fb3b --- /dev/null +++ b/src/tp/doip/doip_client.c @@ -0,0 +1,1034 @@ +#if defined(UDS_TP_DOIP) +/** + * @file doip_client.c + * @brief ISO 13400 (DoIP) Transport Layer - Client Implementation + * @details DoIP TCP transport layer client for UDS (ISO 14229) + * + * + * @note This is a simplified implementation focusing on TCP diagnostic messages. + * UDP vehicle discovery is not included. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "doip_client.h" +#include "util.h" +#include "doip_transport.h" +#include "log.h" + +/* Macro to extract 16-bit BE DoIP address from buffer */ +#define DOIP_ADDRESS(a, off) ((uint16_t)(((a[(off)]) << 8) | (a[(off) + 1]))) + +/** + * @brief Initialize DoIP header + * @param header Pointer to DoIPHeader_t structure to initialize + * @param payload_type DoIP payload type + * @param payload_length Length of DoIP payload + */ +static void doip_header_init(DoIPHeader_t *header, uint16_t payload_type, uint32_t payload_length) { + header->protocol_version = DOIP_PROTOCOL_VERSION; + header->protocol_version_inv = DOIP_PROTOCOL_VERSION_INV; + header->payload_type = htons(payload_type); + header->payload_length = htonl(payload_length); +} + +/** + * @brief Convert DoIP client state to string. + * + * @param state the state to convert + * @return const char* the string representation of the state + */ +const char *doip_client_state_to_string(DoIPClientState_t state) { + switch (state) { + case DOIP_STATE_DISCONNECTED: + return "DISCONNECTED"; + case DOIP_STATE_CONNECTED: + return "CONNECTED"; + case DOIP_STATE_ROUTING_ACTIVATION_PENDING: + return "ROUTING_ACTIVATION_PENDING"; + case DOIP_STATE_READY_FOR_DIAG_REQUEST: + return "READY"; + case DOIP_STATE_DIAG_MESSAGE_ACK_PENDING: + return "ACK_PENDING"; + case DOIP_STATE_DIAG_MESSAGE_RESPONSE_PENDING: + return "RESPONSE_PENDING"; + case DOIP_STATE_ERROR: + return "ERROR"; + default: + return "UNKNOWN_STATE"; + } +} + +/** + * @brief Helper macro to change DoIP client state with logging. + */ +#define doip_change_state(t, s) \ + { \ + if (_doip_change_state((t), (s))) { \ + UDS_LOGV(__FILE__, "DoIP: State change to %s (line %d)", \ + doip_client_state_to_string(s), __LINE__); \ + } \ + } + +/** + * @brief Change DoIP client state + * @param tp DoIP client context + * @param new_state New state to set + * @return true if state changed, false if same state + */ +static bool _doip_change_state(DoIPClient_t *tp, DoIPClientState_t new_state) { + if (tp->state == new_state) + return false; + + // UDS_LOGI(__FILE__, "DoIP: State change %d -> %d", tp->state, new_state); + tp->state = new_state; + return true; +} + +/** + * @brief Parse DoIP header from buffer + * @param buffer Pointer to buffer containing DoIP header + * @param header Pointer to DoIPHeader_t structure to fill + */ +static bool doip_header_parse(const uint8_t *buffer, DoIPHeader_t *header) { + if (NULL == buffer || NULL == header) { + return false; + } + + header->protocol_version = buffer[0]; + header->protocol_version_inv = buffer[1]; + header->payload_type = buffer[2] << 8 | buffer[3]; + header->payload_length = buffer[4] << 24 | buffer[5] << 16 | buffer[6] << 8 | buffer[7]; + + /* Validate protocol version */ + if (header->protocol_version != DOIP_PROTOCOL_VERSION || + header->protocol_version_inv != DOIP_PROTOCOL_VERSION_INV) { + return false; + } + + return true; +} + +/** + * @brief Send DoIP message. + * @param tp DoIP client context + * @param payload_type DoIP payload type + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + * @return int Number of payload bytes sent, or -1 on error + */ +static int doip_send_message(const DoIPClient_t *tp, uint16_t payload_type, const uint8_t *payload, + uint32_t payload_len) { + uint8_t buffer[DOIP_BUFFER_SIZE]; + DoIPHeader_t *header = (DoIPHeader_t *)buffer; + + if (DOIP_HEADER_SIZE + payload_len > DOIP_BUFFER_SIZE) { + return -1; + } + + doip_header_init(header, payload_type, payload_len); + + if (payload && payload_len > 0) { + memcpy(buffer + DOIP_HEADER_SIZE, payload, payload_len); + } + + ssize_t sent = tp->tcp.send(&tp->tcp, buffer, DOIP_HEADER_SIZE + payload_len); + if (sent < 0) { + perror("tcp_send"); + return -1; + } + + /* Return number of UDS payload bytes sent (strip headers) */ + return sent - DOIP_HEADER_SIZE - DOIP_DIAG_HEADER_SIZE; +} + +/** + * @brief Stores UDS response data in DoIP client context + * + * @param tp DoIP client context + * @param data Pointer to DoIP response data. Assumes a diag message payload. + * @param len Length of DoIP response data + */ +void doip_store_uds_response(DoIPClient_t *tp, const uint8_t *data, size_t len) { + if (len > DOIP_BUFFER_SIZE) { + UDS_LOGE(__FILE__, "DoIP: Response too large to store (%zu bytes)", len); + return; + } + + // Store UDS response data in separate buffer for doip_tp_recv to retrieve + if (len > DOIP_DIAG_HEADER_SIZE) { + uint16_t sa = DOIP_ADDRESS(data, 0); + + // strip diag header (sa and ta) + const uint8_t *uds_data = data + DOIP_DIAG_HEADER_SIZE; + size_t uds_len = len - DOIP_DIAG_HEADER_SIZE; + + /* Copy UDS data to uds_response buffer */ + if (uds_len <= DOIP_BUFFER_SIZE) { + memcpy(tp->uds_response, uds_data, uds_len); + tp->uds_response_len = uds_len; + UDS_LOGI(__FILE__, "DoIP: Stored diagnostic response (%zu bytes) from SA=0x%04X", + uds_len, sa); + } else { + UDS_LOGE(__FILE__, "DoIP: Diagnostic response too large (%zu bytes)", uds_len); + } + } +} + +/** + * @brief Handle routing activation response + * @param tp DoIP client context + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + */ +static void doip_handle_routing_activation_response(DoIPClient_t *tp, const uint8_t *payload, + uint32_t payload_len) { + if (payload_len < 9) { + UDS_LOGI(__FILE__, "DoIP: Invalid routing activation response length"); + doip_change_state(tp, DOIP_STATE_ERROR); + return; + } + + uint16_t client_sa = DOIP_ADDRESS(payload, 0); + uint16_t server_sa = DOIP_ADDRESS(payload, 2); + uint8_t response_code = payload[4]; + (void)client_sa; /* Validated implicitly by successful response */ + + if (response_code == DOIP_ROUTING_ACTIVATION_RES_SUCCESS) { + doip_change_state(tp, DOIP_STATE_READY_FOR_DIAG_REQUEST); + UDS_LOGI(__FILE__, "DoIP: Routing activated (SA=0x%04X, TA=0x%04X)", tp->source_address, + server_sa); + } else { + UDS_LOGI(__FILE__, "DoIP: Routing activation failed (code=0x%02X)", response_code); + doip_change_state(tp, DOIP_STATE_ERROR); + } +} + +/** + * @brief Sends alive check response when (DoIP) server requests it. + * @param tp DoIP client context + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + */ +static void doip_handle_alive_check_request(const DoIPClient_t *tp, const uint8_t *payload, + uint32_t payload_len) { + + (void)tp; + (void)payload; + (void)payload_len; + + // alive check request payload is empty, the response contains the client's source address + uint8_t response[2]; + response[0] = (tp->source_address >> 8) & 0xFF; + response[1] = tp->source_address & 0xFF; + + UDS_LOGI(__FILE__, "DoIP: Alive check request -> response from 0x%04X", tp->source_address); + int sent = doip_send_message(tp, DOIP_PAYLOAD_TYPE_ALIVE_CHECK_RES, response, sizeof(response)); + if (sent < 0) { + UDS_LOGE(__FILE__, "DoIP: Failed to send alive check response"); + } else { + UDS_LOGI(__FILE__, "DoIP: Sent alive check response (%d bytes)", sent); + } +} + +/** + * @brief Handle diagnostic message positive ACK. + * @param tp DoIP client context + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + */ +static void doip_handle_diag_pos_ack(DoIPClient_t *tp, const uint8_t *payload, + uint32_t payload_len) { + if (payload_len < 5) { + return; + } + + uint16_t source_address = DOIP_ADDRESS(payload, 0); + uint16_t target_address = DOIP_ADDRESS(payload, 2); + uint8_t ack_code = payload[4]; + + tp->diag_ack_received = true; + + UDS_LOGI(__FILE__, "DoIP: Diagnostic message ACK (SA=0x%04X, TA=0x%04X, code=0x%02X)", + source_address, target_address, ack_code); +} + +/** + * @brief Handle diagnostic message negative ACK. + * @param tp DoIP client context + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + */ +static void doip_handle_diag_neg_ack(DoIPClient_t *tp, const uint8_t *payload, + uint32_t payload_len) { + if (payload_len < 5) { + return; + } + + uint16_t source_address = DOIP_ADDRESS(payload, 0); + uint16_t target_address = DOIP_ADDRESS(payload, 2); + uint8_t nack_code = payload[4]; + + tp->diag_nack_received = true; + tp->diag_nack_code = nack_code; + + UDS_LOGW(__FILE__, "DoIP: Diagnostic message NACK (SA=0x%04X, TA=0x%04X, code=0x%02X)", + source_address, target_address, nack_code); +} + +/** + * @brief Handle diagnostic message (response from server). + * @param tp DoIP client context + * @param payload Pointer to DoIP payload + * @param payload_len Length of DoIP payload + */ +static void doip_handle_diag_message(DoIPClient_t *tp, const uint8_t *payload, + uint32_t payload_len) { + if (payload_len < 4) { + return; + } + + uint16_t target_address = DOIP_ADDRESS(payload, 2); + + /* Verify target address matches our logical address */ + if (target_address != tp->source_address) { + UDS_LOGI(__FILE__, "DoIP: Received diagnostic message for different TA=0x%04X", + target_address); + return; + } + + /* Store UDS response data in separate buffer */ + doip_store_uds_response(tp, payload, payload_len); +} + +/**#ifdef DOIP_MOCK_TP + * @brief Process received DoIP message according to payload type. + * @param tp DoIP client context + * @param header Pointer to DoIP header + * @param payload Pointer to DoIP payload + */ +static void doip_process_message(DoIPClient_t *tp, const DoIPHeader_t *header, + const uint8_t *payload) { + switch (header->payload_type) { + case DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_RES: + doip_handle_routing_activation_response(tp, payload, header->payload_length); + break; + + case DOIP_PAYLOAD_TYPE_ALIVE_CHECK_REQ: + doip_handle_alive_check_request(tp, payload, header->payload_length); + break; + + case DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_POS_ACK: + doip_handle_diag_pos_ack(tp, payload, header->payload_length); + break; + + case DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_NEG_ACK: + doip_handle_diag_neg_ack(tp, payload, header->payload_length); + break; + + case DOIP_PAYLOAD_TYPE_DIAG_MESSAGE: + doip_handle_diag_message(tp, payload, header->payload_length); + break; + + default: + UDS_LOGI(__FILE__, "DoIP: Unknown payload type 0x%04X", header->payload_type); + break; + } +} + +/** + * @brief Receive and process data + * @param tp DoIP client context + * @param timeout_ms Timeout in milliseconds + */ +static ssize_t doip_receive_data(DoIPClient_t *tp, int timeout_ms) { + ssize_t bytes_read = tp->tcp.recv(&tp->tcp, tp->rx_buffer + tp->rx_offset, + DOIP_BUFFER_SIZE - tp->rx_offset, timeout_ms); + + if (bytes_read <= 0) { + if (bytes_read == 0) { + UDS_LOGE(__FILE__, "DoIP: Server disconnected"); + } else { + perror("tcp_recv"); + } + doip_change_state(tp, DOIP_STATE_DISCONNECTED); + return -1; + } + + UDS_LOGI(__FILE__, "DoIP: Received %zd bytes", bytes_read); + + tp->rx_offset += bytes_read; + /* Process complete DoIP messages */ + while (tp->rx_offset >= DOIP_HEADER_SIZE) { + DoIPHeader_t header; + if (!doip_header_parse(tp->rx_buffer, &header)) { + UDS_LOGE(__FILE__, "DoIP: Invalid header"); + doip_change_state(tp, DOIP_STATE_ERROR); + return -1; + } + + size_t total_msg_size = DOIP_HEADER_SIZE + header.payload_length; + + if (tp->rx_offset < total_msg_size) { + /* Wait for more data */ + break; + } + + /* Process message */ + const uint8_t *payload = tp->rx_buffer + DOIP_HEADER_SIZE; + UDS_LOGI(__FILE__, "DoIP: Processing message type 0x%04X, length %u", header.payload_type, + header.payload_length); + UDS_LOG_SDU(__FILE__, payload, header.payload_length, NULL); + doip_process_message(tp, &header, payload); + + /* Remove processed message from buffer */ + if (tp->rx_offset > total_msg_size) { + memmove(tp->rx_buffer, tp->rx_buffer + total_msg_size, tp->rx_offset - total_msg_size); + } + tp->rx_offset -= total_msg_size; + } + + return bytes_read; +} + +/** + * @brief Connect to DoIP server + * @param tp DoIP client context + */ +int doip_client_connect(DoIPClient_t *tp) { + if (tp->state != DOIP_STATE_DISCONNECTED) { + UDS_LOGE(__FILE__, "DoIP: Already connected or in error state"); + return -1; + } + + if (tp->tcp.init(&tp->tcp, tp->server_ip, tp->server_port ? tp->server_port : DOIP_TCP_PORT) < + 0) { + UDS_LOGE(__FILE__, "DoIP: TCP init failed"); + return -1; + } + if (tp->tcp.connect(&tp->tcp) < 0) { + UDS_LOGE(__FILE__, "DoIP: TCP connect error (%s:%d)", tp->server_ip, + tp->server_port ? tp->server_port : DOIP_TCP_PORT); + return -1; + } + + doip_change_state(tp, DOIP_STATE_CONNECTED); + UDS_LOGI(__FILE__, "DoIP Client: Connected to %s:%d", tp->server_ip, + tp->server_port ? tp->server_port : DOIP_TCP_PORT); + + return 0; +} + +/** + * @brief Activate routing. Sends a "routing activation" request to the server. + * @param tp DoIP client context + */ +int doip_client_activate_routing(DoIPClient_t *tp) { + if (tp->state != DOIP_STATE_CONNECTED) { + UDS_LOGE(__FILE__, "DoIP: Not connected"); + return -1; + } + + /* Build routing activation request */ + uint8_t payload[11]; + payload[0] = (tp->source_address >> 8) & 0xFF; + payload[1] = tp->source_address & 0xFF; + payload[2] = DOIP_ROUTING_ACTIVATION_TYPE; + payload[3] = 0x00; /* Reserved */ + payload[4] = 0x00; + payload[5] = 0x00; + payload[6] = 0x00; + + /* Optional: OEM specific */ + payload[7] = 0x00; + payload[8] = 0x00; + payload[9] = 0x00; + payload[10] = 0x00; + + if (doip_send_message(tp, DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_REQ, payload, 11) < 0) { + return -1; + } + + doip_change_state(tp, DOIP_STATE_ROUTING_ACTIVATION_PENDING); + + /* Wait for routing activation response */ + int timeout_ms = DOIP_DEFAULT_TIMEOUT_MS; + clock_t start = clock(); + + while (tp->state == DOIP_STATE_ROUTING_ACTIVATION_PENDING) { + clock_t elapsed_ticks = clock() - start; + clock_t elapsed_ms = (elapsed_ticks * 1000LL) / CLOCKS_PER_SEC; + int remaining_ms = timeout_ms - elapsed_ms; + + if (remaining_ms <= 0) { + UDS_LOGE(__FILE__, "DoIP: Routing activation timeout"); + doip_change_state(tp, DOIP_STATE_ERROR); + return -1; + } + + if (doip_receive_data(tp, remaining_ms) < 0) { + return -1; + } + } + + if (tp->state != DOIP_STATE_READY_FOR_DIAG_REQUEST) { + UDS_LOGE(__FILE__, "DoIP: Routing activation failed"); + return -1; + } + + return 0; +} + +/** + * @brief Send diagnostic (UDS) message via DoIP. The UDS message is wrapped in a DoIP diagnostic + * message, consisting of a DoIP header and a diagnostic message header (source and target + * addresses). + * @param tp DoIP client context + * @param data Pointer to diagnostic message data. This is the UDS payload. + * @param len Length of diagnostic message data (of UDS payload) + */ +ssize_t doip_client_send_diag_message(DoIPClient_t *tp, const uint8_t *data, size_t len) { + /* Use shared tx_buffer instead of stack allocation (embedded-friendly) */ + uint8_t *payload = tp->tx_buffer; + if (len + 4 > DOIP_BUFFER_SIZE) { + UDS_LOGE(__FILE__, "DoIP: Message too large: %zu bytes > %d", len + 4, DOIP_BUFFER_SIZE); + return -1; + } + + doip_change_state(tp, DOIP_STATE_DIAG_MESSAGE_SEND_PENDING); + + /* Add diagnostic message header (source and target addresses) */ + payload[0] = (tp->source_address >> 8) & 0xFF; + payload[1] = tp->source_address & 0xFF; + payload[2] = (tp->target_address >> 8) & 0xFF; + payload[3] = tp->target_address & 0xFF; + memcpy(payload + 4, data, len); + + /* Reset ACK/NACK flags */ + tp->diag_ack_received = false; + tp->diag_nack_received = false; + + int sent = doip_send_message(tp, DOIP_PAYLOAD_TYPE_DIAG_MESSAGE, payload, len + 4); + + if (sent < 0) { + return -1; + } + + /* Wait for ACK/NACK of DoIP server*/ + doip_change_state(tp, DOIP_STATE_DIAG_MESSAGE_ACK_PENDING); + int timeout_ms = DOIP_ACK_TIMEOUT_MS; /* 1 second for ACK */ + clock_t start = clock(); + + while (!tp->diag_ack_received && !tp->diag_nack_received) { + clock_t elapsed_ticks = clock() - start; + clock_t elapsed_ms = (elapsed_ticks * 1000LL) / CLOCKS_PER_SEC; + int remaining_ms = timeout_ms - elapsed_ms; + + if (remaining_ms <= 0) { + UDS_LOGE(__FILE__, "DoIP: Diagnostic message ACK timeout"); + return -1; + } + + if (doip_receive_data(tp, remaining_ms) < 0) { + return -1; + } + } + + // NACK received -> report error and fall back to idle state + if (tp->diag_nack_received) { + doip_change_state(tp, DOIP_STATE_READY_FOR_DIAG_REQUEST); + UDS_LOGE(__FILE__, "DoIP: Diagnostic message rejected (NACK code=0x%02X)", + tp->diag_nack_code); + return -1; + } + + doip_change_state(tp, DOIP_STATE_DIAG_MESSAGE_RESPONSE_PENDING); + + return sent; +} + +/** + * @brief Process DoIP client events (call periodically). + * @param tp DoIP client context + * @param timeout_ms Timeout in milliseconds for receiving data + */ +void doip_client_process(DoIPClient_t *tp, int timeout_ms) { + if (tp->state == DOIP_STATE_READY_FOR_DIAG_REQUEST) { + doip_receive_data(tp, timeout_ms); + } +} + +/** + * @brief Disconnect from DoIP server + * @param tp DoIP client context + */ +void doip_client_disconnect(DoIPClient_t *tp) { + tp->tcp.close(&tp->tcp); + + doip_change_state(tp, DOIP_STATE_DISCONNECTED); + tp->rx_offset = 0; + UDS_LOGI(__FILE__, "DoIP Client: Disconnected"); +} + +/** + * @brief Populates SDU info structure for DoIP transport layer. + * + * @param hdl Handle to DoIP transport layer + * @param info Pointer to SDU info structure to populate + */ +void doip_update_sdu_info(const UDSTp_t *hdl, UDSSDU_t *info) { + if (NULL == info || NULL == hdl) { + return; + } + + const DoIPClient_t *impl = (const DoIPClient_t *)hdl; + info->A_Mtype = UDS_A_MTYPE_DIAG; + info->A_SA = impl->source_address; + info->A_TA = impl->target_address; + info->A_TA_Type = UDS_A_TA_TYPE_PHYSICAL; + info->A_AE = UDS_TP_NOOP_ADDR; +} + +/* -------------------------------------------------------------------------------- + * UDS Transport Layer Interface Functions (send, recv, poll) + * -------------------------------------------------------------------------------- */ + +/** + * @brief Send UDS message via DoIP transport layer. + * + * @param hdl Handle to DoIP transport layer + * @param buf Pointer to buffer containing UDS message + * @param len Length of UDS message + * @param info Pointer to SDU info structure (optional) + * @return ssize_t Number of bytes sent, or negative on error + */ + +/* NOTE: SonarCube complains about missing const, but the interface requires non-const */ +static ssize_t doip_tp_send(UDSTp_t *hdl, uint8_t *buf, size_t len, UDSSDU_t *info) { + UDS_ASSERT(hdl); + ssize_t ret = -1; + DoIPClient_t *impl = (DoIPClient_t *)hdl; + + ret = doip_client_send_diag_message(impl, buf, len); + if (ret < 0) { + UDS_LOGE(__FILE__, "DoIP TP Send Error"); + } else { + UDS_LOG_SDU(__FILE__, buf, len, info); + UDS_LOGD(__FILE__, "DoIP TP Send: Sent %zd bytes", ret); + } + + // Populate SDU info if provided (physical addressing semantics on DoIP) + doip_update_sdu_info(hdl, info); + return ret; +} + +/** + * @brief Receive UDS message via DoIP transport layer. + * + * @param hdl Handle to DoIP transport layer + * @param buf Pointer to buffer to store received UDS message + * @param bufsize Size of the buffer + * @param info Pointer to SDU info structure (optional) + * @return ssize_t Number of bytes received, or negative on error + */ +static ssize_t doip_tp_recv(UDSTp_t *hdl, uint8_t *buf, size_t bufsize, UDSSDU_t *info) { + UDS_ASSERT(hdl); + UDS_ASSERT(buf); + DoIPClient_t *impl = (DoIPClient_t *)hdl; + + // Try to receive any pending data (non-blocking poll inside) + ssize_t rc = doip_receive_data(impl, 0); + UDS_LOGD(__FILE__, "DoIP TP Recv: doip_receive_data returned %zd", rc); + + // If we have a diagnostic response stored, return it + if (impl->state == DOIP_STATE_DIAG_MESSAGE_RESPONSE_PENDING && impl->uds_response_len > 0) { + size_t n = impl->uds_response_len; + if (n > bufsize) { + n = bufsize; + } + UDS_LOG_SDU(__FILE__, impl->uds_response, n, NULL); + memcpy(buf, impl->uds_response, n); + impl->uds_response_len = 0; // consume buffered data + doip_change_state(impl, DOIP_STATE_READY_FOR_DIAG_REQUEST); + + // Populate SDU info if provided (physical addressing semantics on DoIP) + doip_update_sdu_info(hdl, info); + UDS_LOG_SDU(__FILE__, buf, n, info); + return (ssize_t)n; + } + + return rc; +} +/** + * @brief Poll DoIP transport layer status + * @note Checks if the transport layer is ready to send/receive + * @return UDS_TP_IDLE if idle, otherwise UDS_TP_SEND_IN_PROGRESS or UDS_TP_RECV_COMPLETE + */ +static UDSTpStatus_t doip_tp_poll(UDSTp_t *hdl) { + UDS_ASSERT(hdl); + UDSTpStatus_t status = 0; + DoIPClient_t *impl = (DoIPClient_t *)hdl; + + // Basic connectivity check + if (impl->state == DOIP_STATE_DISCONNECTED || impl->tcp.fd < 0) { + status |= UDS_TP_ERR; + return status; + } + + // Pump the socket to process incoming data without blocking + ssize_t rc = doip_receive_data(impl, 0); + UDS_LOGV(__FILE__, "DoIP TP Poll: after receive_data rc=%zd", rc); + + if (impl->state == DOIP_STATE_READY_FOR_DIAG_REQUEST) { + status |= UDS_TP_IDLE; + return status; + } + + if (rc < 0) { + status |= UDS_TP_ERR; + return status; + } + + if (impl->state == DOIP_STATE_DIAG_MESSAGE_ACK_PENDING) { + // 1) If waiting for ACK/NACK, mark send in progress until one arrives. + if (!impl->diag_ack_received && !impl->diag_nack_received) { + status |= UDS_TP_SEND_IN_PROGRESS; + } else if (impl->diag_nack_received) { + status |= UDS_TP_ERR; + } else { + // ACK received; now expect diagnostic response + status |= UDS_TP_SEND_IN_PROGRESS; + } + return status; + } + + if (impl->state == DOIP_STATE_DIAG_MESSAGE_RESPONSE_PENDING) { + + // 2) If waiting for diagnostic response, indicate completion when data buffered + if (impl->uds_response_len > 0) { + status |= UDS_TP_RECV_COMPLETE; + } else { + status |= UDS_TP_SEND_IN_PROGRESS; // still waiting on response + } + return status; + } + + // Any other state is considered an error for transport purposes + status |= UDS_TP_ERR; + return status; +} + +UDSErr_t UDSDoIPInitClient(DoIPClient_t *tp, const char *ipaddress, uint16_t port, + uint16_t source_addr, uint16_t target_addr) { + if (tp == NULL || ipaddress == NULL) { + return UDS_ERR_INVALID_ARG; + } + + memset(tp, 0, sizeof(DoIPClient_t)); + + doip_change_state(tp, DOIP_STATE_DISCONNECTED); + tp->source_address = source_addr; + tp->target_address = target_addr; + + /* Copy server IP address with guaranteed null-termination */ + snprintf(tp->server_ip, sizeof(tp->server_ip), "%s", ipaddress); + if (tp->server_ip[0] == '\0') { + UDS_LOGE(__FILE__, "UDS DoIP Client: Invalid server IP address"); + return UDS_ERR_INVALID_ARG; + } + tp->server_port = port; + + tp->hdl.send = doip_tp_send; + tp->hdl.recv = doip_tp_recv; + tp->hdl.poll = doip_tp_poll; + + memset(&tp->tcp, 0, sizeof(tp->tcp)); + memset(&tp->udp, 0, sizeof(tp->udp)); + +#ifdef DOIP_MOCK_TP + /* For testing purposes, use mock transport implementations */ + tp->tcp.init = doip_mock_tcp_init; + tp->tcp.connect = doip_mock_tcp_connect; + tp->tcp.send = doip_mock_tcp_send; + tp->tcp.recv = doip_mock_tcp_recv; + tp->tcp.close = doip_mock_tcp_close; + + tp->udp.init = doip_mock_udp_init; + tp->udp.sendto = doip_mock_udp_sendto; + tp->udp.recv = doip_mock_udp_recv; + tp->udp.recvfrom = doip_mock_udp_recvfrom; + tp->udp.close = doip_mock_udp_close; +#else + /* Use real TCP/UDP transport implementations */ + tp->tcp.init = doip_tp_tcp_init; + tp->tcp.connect = doip_tp_tcp_connect; + tp->tcp.send = doip_tp_tcp_send; + tp->tcp.recv = doip_tp_tcp_recv; + tp->tcp.close = doip_tp_tcp_close; + + tp->udp.init = doip_tp_udp_init; + tp->udp.sendto = doip_tp_udp_sendto; + tp->udp.recv = doip_tp_udp_recv; + tp->udp.recvfrom = doip_tp_udp_recvfrom; + tp->udp.close = doip_tp_udp_close; + tp->udp.join_multicast = doip_tp_udp_join_default_multicast; +#endif + + // sanity checks for transport functions + if (tp->tcp.init == NULL || tp->tcp.connect == NULL || tp->tcp.send == NULL || + tp->tcp.recv == NULL || tp->tcp.close == NULL) { + UDS_LOGE(__FILE__, "UDS DoIP Client: TCP transport functions not set"); + return UDS_ERR_TPORT; + } + + if (tp->udp.init == NULL || tp->udp.sendto == NULL || tp->udp.recv == NULL || + tp->udp.recvfrom == NULL || tp->udp.close == NULL || tp->udp.join_multicast == NULL) { + UDS_LOGE(__FILE__, "UDS DoIP Client: UDP transport functions not set"); + return UDS_ERR_TPORT; + } + + UDS_LOGI(__FILE__, "UDS DoIP Client: Initialized (SA=0x%04X, TA=0x%04X)", tp->source_address, + tp->target_address); + + if (doip_client_connect(tp)) { + UDS_LOGE(__FILE__, "UDS DoIP Client: Connect error"); + return UDS_ERR_TPORT; + } + + if (doip_client_activate_routing(tp)) { + UDS_LOGE(__FILE__, "UDS DoIP Client: Routing activation error"); + return UDS_ERR_TPORT; + } + + return UDS_OK; +} + +UDSErr_t UDSDoIPActivateRouting(DoIPClient_t *tp) { + if (tp == NULL) { + return UDS_ERR_INVALID_ARG; + } + + if (doip_client_activate_routing(tp)) { + UDS_LOGE(__FILE__, "UDS DoIP Client: Routing activation error"); + return UDS_ERR_TPORT; + } + return UDS_OK; +} + +void UDSDoIPDeinit(DoIPClient_t *tp) { + if (tp == NULL) { + return; + } + + doip_client_disconnect(tp); +} + +/* -------------------------------------------------------------- + * Selection callback support + * -------------------------------------------------------------- */ +static DoIPSelectServerFn g_select_fn = NULL; +static void *g_select_user = NULL; +static bool g_discovery_request_only = false; +static bool g_discovery_dump_raw = false; + +void UDSDoIPSetSelectionCallback(DoIPClient_t *tp, DoIPSelectServerFn fn, void *user) { + (void)tp; /* per-client not required; use global for simplicity */ + g_select_fn = fn; + g_select_user = user; +} + +void UDSDoIPSetDiscoveryOptions(bool request_only, bool dump_raw) { + g_discovery_request_only = request_only; + g_discovery_dump_raw = dump_raw; +} + +/* -------------------------------------------------------------- + * UDP discovery: collect responders within timeout, allow selection + * -------------------------------------------------------------- */ +int UDSDoIPDiscoverVehiclesEx(DoIPClient_t *tp, int timeout_ms, bool loopback, uint16_t port) { + if (!tp) + return -1; + tp->udp.loopback = loopback; + if (tp->udp.init(&tp->udp, port, loopback) < 0) { + UDS_LOGE(__FILE__, "DoIP UDP: init failed"); + return -1; + } + if (!loopback && !g_discovery_request_only) { + if (tp->udp.join_multicast(&tp->udp) < 0) { + UDS_LOGE(__FILE__, "DoIP UDP: multicast join failed"); + doip_tp_udp_close(&tp->udp); + return -1; + } + } + + /* Actively send a Vehicle Identification Request */ + { + uint8_t req[DOIP_HEADER_SIZE]; + req[0] = DOIP_PROTOCOL_VERSION; + req[1] = DOIP_PROTOCOL_VERSION_INV; + req[2] = 0x00; /* payload_type MSB: 0x0001 */ + req[3] = 0x01; /* payload_type LSB */ + req[4] = 0x00; /* payload_length: 0 */ + req[5] = 0x00; + req[6] = 0x00; + req[7] = 0x00; + + const char *dst_ip = loopback ? "127.0.0.1" : "255.255.255.255"; + uint16_t dst_port = DOIP_UDP_DISCOVERY_PORT; + ssize_t sent = doip_tp_udp_sendto(&tp->udp, req, sizeof(req), dst_ip, dst_port, 500); + if (sent <= 0) { + UDS_LOGW(__FILE__, "DoIP UDP: VI request send failed (dst %s:%u)", dst_ip, dst_port); + } else { + UDS_LOGI(__FILE__, "DoIP UDP: sent Vehicle Identification Request to %s:%u", dst_ip, + dst_port); + } + } + + int found = 0; + const int slice_ms = 200; + const int resend_interval_ms = 500; + int elapsed_ms = 0; + int sent_count = 1; /* already sent one request above */ + int remaining = timeout_ms > 0 ? timeout_ms : 0; + /* Use shared tx_buffer for UDP receive (eliminates 4KB stack allocation) */ + uint8_t *buf = tp->tx_buffer; + char src_ip[64]; + uint16_t src_port = 0; + + while (remaining > 0) { + int win = remaining < slice_ms ? remaining : slice_ms; + ssize_t n = doip_tp_udp_recvfrom(&tp->udp, buf, DOIP_BUFFER_SIZE, win, src_ip, sizeof(src_ip), + &src_port); + if (n < 0) { + UDS_LOGE(__FILE__, "DoIP UDP: recvfrom error"); + break; + } else if (n == 0) { + remaining -= win; + elapsed_ms += win; + if (elapsed_ms >= sent_count * resend_interval_ms && found == 0) { + /* Resend VI request to improve discovery probability */ + uint8_t req2[DOIP_HEADER_SIZE] = {DOIP_PROTOCOL_VERSION, + DOIP_PROTOCOL_VERSION_INV, + 0x00, + 0x01, + 0x00, + 0x00, + 0x00, + 0x00}; + const char *dst_ip2 = loopback ? "127.0.0.1" : "255.255.255.255"; + uint16_t dst_port2 = DOIP_UDP_DISCOVERY_PORT; + (void)doip_tp_udp_sendto(&tp->udp, req2, sizeof(req2), dst_ip2, dst_port2, 200); + UDS_LOGV(__FILE__, "DoIP UDP: re-sent VI request to %s:%u", dst_ip2, dst_port2); + sent_count++; + } + continue; + } + + found++; + UDS_LOGI(__FILE__, "DoIP UDP: discovery frame from %s:%u (%zd bytes)", src_ip, src_port, n); + if (g_discovery_dump_raw) { + UDS_LOG_SDU(__FILE__, buf, (size_t)n, NULL); + } + + DoIPDiscoveryInfo info; + memset(&info, 0, sizeof(info)); + snprintf(info.ip, sizeof(info.ip), "%s", src_ip); + info.remote_port = src_port; + + /* Parse DoIP header to extract known fields from known payload types */ + if ((size_t)n >= DOIP_HEADER_SIZE) { + DoIPHeader_t hdr; + if (doip_header_parse(buf, &hdr)) { + const uint8_t *pl = buf + DOIP_HEADER_SIZE; + size_t plen = hdr.payload_length; + /* Vehicle identification response/announcement payloads commonly start with VIN */ + if (plen >= 17) { + memcpy(info.vin, pl, 17); + info.vin[17] = '\0'; + } + /* Next 6 bytes often EID, then 6 bytes GID */ + if (plen >= 23) { + for (int i = 0; i < 6; ++i) { + sprintf(&info.eid[i * 2], "%02X", pl[17 + i]); + } + info.eid[12] = '\0'; + } + if (plen >= 29) { + for (int i = 0; i < 6; ++i) { + sprintf(&info.gid[i * 2], "%02X", pl[23 + i]); + } + info.gid[12] = '\0'; + } + if (info.vin[0] != '\0') { + UDS_LOGI(__FILE__, "DoIP UDP: parsed VIN=%s from %s:%u", info.vin, src_ip, + src_port); + } + } else { + /* Fallback: coarse VIN scan in entire datagram */ + if ((size_t)n >= 17) { + for (size_t i = 0; i + 17 <= (size_t)n; ++i) { + bool printable = true; + for (size_t j = 0; j < 17; ++j) { + uint8_t c = buf[i + j]; + if (!(c >= '0' && c <= '9') && !(c >= 'A' && c <= 'Z')) { + printable = false; + break; + } + } + if (printable) { + memcpy(info.vin, buf + i, 17); + info.vin[17] = '\0'; + break; + } + } + } + if (info.vin[0] != '\0') { + UDS_LOGI(__FILE__, "DoIP UDP: heuristic VIN=%s from %s:%u", info.vin, src_ip, + src_port); + } + } + } + + /* Invoke selection callback if provided */ + bool choose = false; + if (g_select_fn) { + choose = g_select_fn(&info, g_select_user); + } else { + /* Default: choose first responder */ + if (found == 1) + choose = true; + } + + if (choose) { + snprintf(tp->server_ip, sizeof(tp->server_ip), "%s", info.ip); + tp->server_port = DOIP_TCP_PORT; /* default TCP port */ + UDS_LOGI(__FILE__, "DoIP: selected server %s:%u", tp->server_ip, tp->server_port); + break; /* stop after selection */ + } + + remaining -= win; + } + + doip_tp_udp_close(&tp->udp); + return found; +} + +int UDSDoIPDiscoverVehicles(DoIPClient_t *tp, int timeout_ms, bool loopback) { + /* By default, listen on the tester request port (13401) to receive announcements */ + return UDSDoIPDiscoverVehiclesEx(tp, timeout_ms, loopback, + DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT); +} + +#endif /* UDS_TP_DOIP */ \ No newline at end of file diff --git a/src/tp/doip/doip_client.h b/src/tp/doip/doip_client.h new file mode 100644 index 000000000..fe89c8319 --- /dev/null +++ b/src/tp/doip/doip_client.h @@ -0,0 +1,102 @@ +#if defined(UDS_TP_DOIP) + +#pragma once +#include "tp.h" +#include "uds.h" +#include "doip_defines.h" +#include "doip_transport.h" + + +#define DOIP_ACK_TIMEOUT_MS 1000 /* 1 second for Diagnostic ACK (0x8002 or 0x8003)*/ + +/* DoIP Client State */ +typedef enum { + DOIP_STATE_DISCONNECTED, + DOIP_STATE_CONNECTED, + DOIP_STATE_ROUTING_ACTIVATION_PENDING, + DOIP_STATE_READY_FOR_DIAG_REQUEST, + // Diag message states for tracking ACK/NACK and responses + DOIP_STATE_DIAG_MESSAGE_SEND_PENDING, + DOIP_STATE_DIAG_MESSAGE_ACK_PENDING, + DOIP_STATE_DIAG_MESSAGE_RESPONSE_PENDING, + DOIP_STATE_ERROR +} DoIPClientState_t; + +/* DoIP Client Context */ +typedef struct { + UDSTp_t hdl; /* Must be the first entry! */ + //DoIPTransport tcp; /* TCP transport for diagnostics */ + //DoIPTransport udp; /* UDP transport for discovery */ + DoIPTcpTransport tcp; /* TCP transport for diagnostics */ + DoIPUdpTransport udp; /* UDP transport for discovery */ + DoIPClientState_t state; + + uint16_t source_address; /* Client logical address */ + uint16_t target_address; /* Server logical address */ + + char server_ip[64]; + uint16_t server_port; + //bool udp_loopback; /* discovery via loopback instead of multicast */ + + uint8_t rx_buffer[DOIP_BUFFER_SIZE]; /* Raw socket receive buffer */ + size_t rx_offset; + + uint8_t uds_response[DOIP_BUFFER_SIZE]; /* Processed UDS response data */ + size_t uds_response_len; + + uint8_t tx_buffer[DOIP_BUFFER_SIZE]; /* Reusable transmit buffer (eliminates stack allocations) */ + + bool routing_activated; + bool diag_ack_received; + bool diag_nack_received; + uint8_t diag_nack_code; +} DoIPClient_t; + +/* Discovery info (minimal set) */ +typedef struct { + char ip[64]; + uint16_t remote_port; + uint16_t logical_address; /* if known */ + char vin[18]; /* 17-char VIN plus NUL if parsed */ + char eid[13]; /* 6-byte EID as hex string */ + char gid[13]; /* 6-byte GID as hex string */ +} DoIPDiscoveryInfo; + +/* Optional selection callback */ +typedef bool (*DoIPSelectServerFn)(const DoIPDiscoveryInfo *info, void *user); + +void UDSDoIPSetSelectionCallback(DoIPClient_t *tp, DoIPSelectServerFn fn, void *user); +/* Discovery options */ +void UDSDoIPSetDiscoveryOptions(bool request_only, bool dump_raw); + +/** + * @brief Initialize DoIP client transport layer + * + * @param tp Pointer to DoIP client context + * @param ipaddress Server IP address as a string + * @param port Server port number + * @param source_addr Client logical address (range 0x0E00 - 0x0FFF) + * @param target_addr Server logical address + * @return UDSErr_t UDS_OK on success, error code otherwise + */ +UDSErr_t UDSDoIPInitClient(DoIPClient_t *tp, const char *ipaddress, uint16_t port, uint16_t source_addr, uint16_t target_addr); + +/** + * @brief Deinitialize DoIP client transport layer + * + * @param tp Pointer to DoIP client context + */ +void UDSDoIPDeinit(DoIPClient_t *tp); + +/** + * @brief Discover vehicles using UDP DoIP + * @param tp DoIP client context + * @param timeout_ms Receive timeout in ms + * @param loopback If true, use loopback instead of multicast + * @return number of discovery frames observed (>=0), or negative on error + */ +int UDSDoIPDiscoverVehicles(DoIPClient_t *tp, int timeout_ms, bool loopback); +/* Extended: allow overriding UDP port (0 = default 13400) */ +int UDSDoIPDiscoverVehiclesEx(DoIPClient_t *tp, int timeout_ms, bool loopback, uint16_t port); + +#endif diff --git a/src/tp/doip/doip_defines.h b/src/tp/doip/doip_defines.h new file mode 100644 index 000000000..13eb845a1 --- /dev/null +++ b/src/tp/doip/doip_defines.h @@ -0,0 +1,57 @@ +#ifndef DOIP_DEFINES_H +#define DOIP_DEFINES_H + +#include + +/* DoIP Protocol Constants */ +#define DOIP_PROTOCOL_VERSION 0x03 +#define DOIP_PROTOCOL_VERSION_INV 0xFC +#define DOIP_TCP_PORT 13400 +#define DOIP_HEADER_SIZE 8 + +/* Header size for diagnostic message (source address + target address) */ +#define DOIP_DIAG_HEADER_SIZE 4 + +/* DoIP Payload Types (table 17)*/ +#define DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_REQ 0x0005 +#define DOIP_PAYLOAD_TYPE_ROUTING_ACTIVATION_RES 0x0006 +#define DOIP_PAYLOAD_TYPE_ALIVE_CHECK_REQ 0x0007 +#define DOIP_PAYLOAD_TYPE_ALIVE_CHECK_RES 0x0008 +#define DOIP_PAYLOAD_TYPE_DIAG_MESSAGE 0x8001 +#define DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_POS_ACK 0x8002 +#define DOIP_PAYLOAD_TYPE_DIAG_MESSAGE_NEG_ACK 0x8003 + +/* DoIP Routing Activation Response Codes (table 56)*/ +#define DOIP_ROUTING_ACTIVATION_RES_SUCCESS 0x10 +#define DOIP_ROUTING_ACTIVATION_RES_UNKNOWN_SA 0x00 +#define DOIP_ROUTING_ACTIVATION_RES_ALREADY_ACTIVE 0x01 + +/* DoIP Diagnostic Message NACK Codes (table 26) */ +#define DOIP_DIAG_NACK_INVALID_SA 0x02 +#define DOIP_DIAG_NACK_UNKNOWN_TA 0x03 +#define DOIP_DIAG_NACK_MESSAGE_TOO_LARGE 0x04 +#define DOIP_DIAG_NACK_OUT_OF_MEMORY 0x05 +#define DOIP_DIAG_NACK_TARGET_UNREACHABLE 0x06 + +/* Configuration */ +#define DOIP_BUFFER_SIZE 4096 +#define DOIP_ROUTING_ACTIVATION_TYPE 0x00 +#define DOIP_DEFAULT_TIMEOUT_MS 5000 + + +/* DoIP Header Structure */ +typedef struct { + uint8_t protocol_version; /**< DoIP protocol version (table 16). 1=2010, 2=2012, 3=2019, 4=2019-Amd1,2025 */ + uint8_t protocol_version_inv; /**< Inverse of protocol version */ + uint16_t payload_type; /**< Payload type (table 17) */ + uint32_t payload_length; /**< Payload length */ +} DoIPHeader_t; + + +/* UDP ports (ISO 13400-2 Table 48) */ +#define DOIP_UDP_DISCOVERY_PORT 13400 +#define DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT 13401 + + + +#endif /* DOIP_DEFINES_H */ diff --git a/src/tp/doip/doip_tp_tcp.c b/src/tp/doip/doip_tp_tcp.c new file mode 100644 index 000000000..37cb874f7 --- /dev/null +++ b/src/tp/doip/doip_tp_tcp.c @@ -0,0 +1,187 @@ +#if defined(UDS_TP_DOIP) + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "doip_transport.h" +#include "doip_defines.h" + +void doip_tp_set_timeouts(DoIPTcpTransport *tcp, int connect_timeout_ms, int send_timeout_ms) { + if (!tcp) return; + if (connect_timeout_ms > 0) tcp->connect_timeout_ms = connect_timeout_ms; + if (send_timeout_ms > 0) tcp->send_timeout_ms = send_timeout_ms; +} + +#ifdef DOIP_MOCK_TP +int doip_tp_mock_init(DoIPTcpTransport *tcp, const char *ip, uint16_t port) { + (void)tcp; + (void)ip; + (void)port; + return 0; +} + +int doip_tp_mock_connect(DoIPTcpTransport *tcp) { + (void)tcp; + return 0; +} + +ssize_t doip_tp_mock_send(DoIPTcpTransport *tcp, const uint8_t *buf, size_t len) { + (void)tcp; + (void)buf; + return (ssize_t)len; +} + +ssize_t doip_tp_mock_recv(DoIPTcpTransport *tcp, uint8_t *buf, size_t len, int timeout_ms) { + (void)tcp; + (void)buf; + (void)len; + (void)timeout_ms; + return 0; +} + +void doip_tp_mock_close(DoIPTcpTransport *tcp) { + (void)tcp; +} + +#else + +int doip_tp_tcp_init(DoIPTcpTransport *tcp, const char *ip, uint16_t port) { + if (!tcp || !ip) return -1; + + tcp->fd = -1; + tcp->port = port ? port : DOIP_TCP_PORT; + snprintf(tcp->ip, sizeof(tcp->ip), "%s", ip); + tcp->connect_timeout_ms = DOIP_DEFAULT_TIMEOUT_MS; + tcp->send_timeout_ms = DOIP_DEFAULT_TIMEOUT_MS; + return 0; +} + +int doip_tp_tcp_connect(DoIPTcpTransport *tcp) { + if (!tcp) return -1; + tcp->fd = socket(AF_INET, SOCK_STREAM, 0); + if (tcp->fd < 0) { + return -1; + } + + /* set non-blocking before connect to avoid blocking connect */ + int flags = fcntl(tcp->fd, F_GETFL, 0); + if (flags < 0 || fcntl(tcp->fd, F_SETFL, flags | O_NONBLOCK) < 0) { + close(tcp->fd); + tcp->fd = -1; + return -1; + } + + struct sockaddr_in sa; + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_port = htons(tcp->port); + if (inet_pton(AF_INET, tcp->ip, &sa.sin_addr) <= 0) { + close(tcp->fd); + tcp->fd = -1; + return -1; + } + int rc = connect(tcp->fd, (struct sockaddr *)&sa, sizeof(sa)); + if (rc < 0) { + if (errno != EINPROGRESS) { + close(tcp->fd); + tcp->fd = -1; + return -1; + } + /* wait for writability or error within default timeout */ + fd_set wfds; + FD_ZERO(&wfds); + FD_SET(tcp->fd, &wfds); + struct timeval tv; + int cto = (tcp->connect_timeout_ms > 0) ? tcp->connect_timeout_ms : DOIP_DEFAULT_TIMEOUT_MS; + tv.tv_sec = cto / 1000; + tv.tv_usec = (cto % 1000) * 1000; + rc = select(tcp->fd + 1, NULL, &wfds, NULL, &tv); + if (rc <= 0) { + /* timeout or select error */ + close(tcp->fd); + tcp->fd = -1; + return -1; + } + int soerr = 0; + socklen_t slen = sizeof(soerr); + if (getsockopt(tcp->fd, SOL_SOCKET, SO_ERROR, &soerr, &slen) < 0 || soerr != 0) { + close(tcp->fd); + tcp->fd = -1; + return -1; + } + } + return 0; +} + +ssize_t doip_tp_tcp_send(const DoIPTcpTransport *tcp, const uint8_t *buf, size_t len) { + if (!tcp || tcp->fd < 0 || !buf) return -1; + size_t total = 0; + int sflags = 0; +#ifdef MSG_NOSIGNAL + sflags |= MSG_NOSIGNAL; +#endif + while (total < len) { + fd_set wfds; + FD_ZERO(&wfds); + FD_SET(tcp->fd, &wfds); + struct timeval tv; + int sto = (tcp->send_timeout_ms > 0) ? tcp->send_timeout_ms : DOIP_DEFAULT_TIMEOUT_MS; + tv.tv_sec = sto / 1000; + tv.tv_usec = (sto % 1000) * 1000; + int rc = select(tcp->fd + 1, NULL, &wfds, NULL, &tv); + if (rc <= 0) { + /* timeout or error */ + return -1; + } + ssize_t n = send(tcp->fd, buf + total, len - total, sflags); + if (n > 0) { + total += (size_t)n; + continue; + } + if (n < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) { + /* try again after select */ + continue; + } + /* other error or peer closed */ + return -1; + } + return (ssize_t)total; +} + +ssize_t doip_tp_tcp_recv(DoIPTcpTransport *tcp, uint8_t *buf, size_t len, int timeout_ms) { + if (!tcp || tcp->fd < 0 || !buf) return -1; + + fd_set rfds; + struct timeval tv; + FD_ZERO(&rfds); + FD_SET(tcp->fd, &rfds); + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + int ret = select(tcp->fd + 1, &rfds, NULL, NULL, timeout_ms >= 0 ? &tv : NULL); + if (ret < 0) { + return -1; + } + if (ret == 0) { + return 0; /* timeout */ + } + return recv(tcp->fd, buf, len, 0); +} + +void doip_tp_tcp_close(DoIPTcpTransport *tcp) { + if (!tcp) return; + if (tcp->fd >= 0) { + close(tcp->fd); + tcp->fd = -1; + } +} +#endif /* DOIP_MOCK_TP */ + +#endif /* UDS_TP_DOIP */ diff --git a/src/tp/doip/doip_tp_udp.c b/src/tp/doip/doip_tp_udp.c new file mode 100644 index 000000000..99e18f980 --- /dev/null +++ b/src/tp/doip/doip_tp_udp.c @@ -0,0 +1,222 @@ +#if defined(UDS_TP_DOIP) + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "doip_transport.h" +#include "doip_defines.h" +#include + +/* Default DoIP multicast group for discovery */ +static const char *DOIP_DEFAULT_MCAST = "224.224.224.224"; /* per ISO 13400 */ + +#ifdef DOIP_MOCK_TP + +int doip_tp_mock_udp_init(DoIPUdpTransport *udp, uint16_t port, bool loopback) { + (void)udp; + (void)port; + (void)loopback; + return 0; +} + +ssize_t doip_tp_mock_udp_recv(DoIPUdpTransport *udp, uint8_t *buf, size_t len, int timeout_ms) { + (void)udp; + (void)buf; + (void)len; + (void)timeout_ms; + return 0; +} + +ssize_t doip_tp_mock_udp_recvfrom(DoIPUdpTransport *udp, uint8_t *buf, size_t len, int timeout_ms, + char *src_ip_out, size_t src_ip_out_sz, uint16_t *src_port_out) { + (void)udp; + (void)buf; + (void)len; + (void)timeout_ms; + (void)src_ip_out; + (void)src_ip_out_sz; + (void)src_port_out; + return 0; +} + +void doip_tp_mock_udp_close(DoIPUdpTransport *udp) { + (void)udp; +} + +ssize_t doip_tp_mock_udp_sendto(DoIPUdpTransport *udp, const uint8_t *buf, size_t len, + const char *dst_ip, uint16_t dst_port, int timeout_ms) { + (void)udp; + (void)buf; + (void)len; + (void)dst_ip; + (void)dst_port; + (void)timeout_ms; + return 0; +} + +int doip_tp_mock_udp_join_default_multicast(DoIPUdpTransport *udp) { + (void)udp; + return 0; +} + +#else +int doip_tp_udp_init(DoIPUdpTransport *udp, uint16_t port, bool loopback) { + if (!udp) return -1; + + udp->fd = -1; + udp->loopback = loopback; + /* For tester discovery, default listen port is 13401 */ + udp->port = port ? port : DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT; + + int fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + + int reuse = 1; + (void)setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + + if (loopback) { + /* Bind to loopback UDP to allow local discovery testing */ + struct sockaddr_in sa; + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_port = htons(udp->port); + sa.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + if (bind(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0) { + close(fd); + return -1; + } + + unsigned char on = 1; + (void)setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &on, sizeof(on)); + } else { + /* Bind on any address for multicast */ + struct sockaddr_in sa; + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_port = htons(udp->port); + sa.sin_addr.s_addr = htonl(INADDR_ANY); + if (bind(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0) { + close(fd); + return -1; + } + + /* Enable broadcast for sending to 255.255.255.255 */ + int broadcast = 1; + (void)setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); + } + + /* set non-blocking after successful bind */ + int flags = fcntl(fd, F_GETFL, 0); + if (flags < 0 || fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) { + close(fd); + return -1; + } + + udp->fd = fd; + return 0; +} + +int doip_tp_udp_join_default_multicast(DoIPUdpTransport *udp) { + if (!udp || udp->fd < 0) return -1; + if (udp->loopback) return 0; /* no multicast join needed */ + + struct ip_mreq mreq; + memset(&mreq, 0, sizeof(mreq)); + mreq.imr_multiaddr.s_addr = inet_addr(DOIP_DEFAULT_MCAST); + mreq.imr_interface.s_addr = htonl(INADDR_ANY); + if (setsockopt(udp->fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) { + return -1; + } + return 0; +} + +ssize_t doip_tp_udp_recv(DoIPUdpTransport *udp, uint8_t *buf, size_t len, int timeout_ms) { + if (!udp || udp->fd < 0 || !buf) return -1; + fd_set rfds; + struct timeval tv; + FD_ZERO(&rfds); + FD_SET(udp->fd, &rfds); + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + int ret = select(udp->fd + 1, &rfds, NULL, NULL, timeout_ms >= 0 ? &tv : NULL); + if (ret < 0) return -1; + if (ret == 0) return 0; /* timeout */ + + return recv(udp->fd, buf, len, 0); +} + +ssize_t doip_tp_udp_recvfrom(DoIPUdpTransport *udp, uint8_t *buf, size_t len, int timeout_ms, + char *src_ip_out, size_t src_ip_out_sz, uint16_t *src_port_out) { + if (!udp || udp->fd < 0 || !buf) return -1; + fd_set rfds; + struct timeval tv; + FD_ZERO(&rfds); + FD_SET(udp->fd, &rfds); + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + int ret = select(udp->fd + 1, &rfds, NULL, NULL, timeout_ms >= 0 ? &tv : NULL); + if (ret < 0) return -1; + if (ret == 0) return 0; /* timeout */ + + struct sockaddr_in src; + socklen_t slen = sizeof(src); + ssize_t n = recvfrom(udp->fd, buf, len, 0, (struct sockaddr *)&src, &slen); + if (n <= 0) return n; + if (src_ip_out && src_ip_out_sz > 0) { + const char *ip = inet_ntoa(src.sin_addr); + if (ip) { + snprintf(src_ip_out, src_ip_out_sz, "%s", ip); + } + } + if (src_port_out) { + *src_port_out = ntohs(src.sin_port); + } + return n; +} + +void doip_tp_udp_close(DoIPUdpTransport *udp) { + if (!udp) return; + if (udp->fd >= 0) { + close(udp->fd); + udp->fd = -1; + } +} + +ssize_t doip_tp_udp_sendto(DoIPUdpTransport *udp, const uint8_t *buf, size_t len, + const char *dst_ip, uint16_t dst_port, int timeout_ms) { + if (!udp || udp->fd < 0 || !buf || !dst_ip) return -1; + + fd_set wfds; + struct timeval tv; + FD_ZERO(&wfds); + FD_SET(udp->fd, &wfds); + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + int ret = select(udp->fd + 1, NULL, &wfds, NULL, timeout_ms >= 0 ? &tv : NULL); + if (ret < 0) return -1; + if (ret == 0) return 0; /* timeout */ + + struct sockaddr_in dst; + memset(&dst, 0, sizeof(dst)); + dst.sin_family = AF_INET; + dst.sin_port = htons(dst_port); + if (inet_pton(AF_INET, dst_ip, &dst.sin_addr) <= 0) { + return -1; + } + + return sendto(udp->fd, buf, len, 0, (struct sockaddr *)&dst, sizeof(dst)); +} +#endif /* DOIP_MOCK_TP */ +#endif /* UDS_TP_DOIP */ diff --git a/src/tp/doip/doip_transport.h b/src/tp/doip/doip_transport.h new file mode 100644 index 000000000..60add43a2 --- /dev/null +++ b/src/tp/doip/doip_transport.h @@ -0,0 +1,209 @@ +#ifndef DOIP_TRANSPORT_H +#define DOIP_TRANSPORT_H +#if defined(UDS_TP_DOIP) + +#include +#include +#include + +/* Forward declaration */ +typedef struct DoIPTcpTransport DoIPTcpTransport; +typedef struct DoIPUdpTransport DoIPUdpTransport; + +#ifdef DOIP_MOCK_TP +/* Mock transport functions for testing. For now these method do nothing and return + * success. + */ + +int doip_tp_mock_init(DoIPTcpTransport *tcp, const char *ip, uint16_t port); +int doip_tp_mock_connect(DoIPTcpTransport *tcp); +ssize_t doip_tp_mock_send(DoIPTcpTransport *tcp, const uint8_t *buf, size_t len); +ssize_t doip_tp_mock_recv(DoIPTcpTransport *tcp, uint8_t *buf, size_t len, int timeout_ms); +void doip_tp_mock_close(DoIPTcpTransport *tcp); + +int doip_tp_mock_udp_init(DoIPUdpTransport *t, uint16_t port, bool loopback); +ssize_t doip_tp_mock_udp_recv(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms); +ssize_t doip_tp_mock_udp_recvfrom(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms, + char *src_ip_out, size_t src_ip_out_sz, uint16_t *src_port_out); +void doip_tp_mock_udp_close(DoIPUdpTransport *t); +ssize_t doip_tp_mock_udp_sendto(DoIPUdpTransport *t, const uint8_t *buf, size_t len, + const char *dst_ip, uint16_t dst_port, int timeout_ms); + +int doip_tp_mock_udp_join_default_multicast(DoIPUdpTransport *t); +#else + +/** + * @brief Initialize DoIP TCP transport + * + * @param t Transport context + * @param ip Remote IP address + * @param port Remote port + * @retval 0 success + * @retval -1 error + */ +int doip_tp_tcp_init(DoIPTcpTransport *tcp, const char *ip, uint16_t port); + +/** + * @brief Connect DoIP TCP transport + * + * @param t Transport context + * @retval 0 success + * @retval -1 error + */ +int doip_tp_tcp_connect(DoIPTcpTransport *tcp); + +/** + * @brief Send data over DoIP TCP transport + * + * @param t Transport context + * @param buf Data buffer to send + * @param len Length of data to send + * @return ssize_t Number of bytes sent, or -1 on error + */ +ssize_t doip_tp_tcp_send(const DoIPTcpTransport *tcp, const uint8_t *buf, size_t len); + +/** + * @brief Receive data over DoIP TCP transport + * + * @param t Transport context + * @param buf Buffer to receive data into + * @param len Length of buffer + * @param timeout_ms Receive timeout in milliseconds + * @return ssize_t Number of bytes received, 0 on timeout, or -1 on error + */ +ssize_t doip_tp_tcp_recv(DoIPTcpTransport *tcp, uint8_t *buf, size_t len, int timeout_ms); + +/** + * @brief Close DoIP TCP transport + * + * @param t Transport context + */ +void doip_tp_tcp_close(DoIPTcpTransport *tcp); + +/* Optional: configure timeouts on the transport */ +/** + * @brief Set timeouts for DoIP transport + * @param t Transport context + * @param connect_timeout_ms Connect timeout in milliseconds (<=0 for default) + * @param send_timeout_ms Send timeout in milliseconds (<=0 for default) + */ +void doip_tp_set_timeouts(DoIPTcpTransport *tcp, int connect_timeout_ms, int send_timeout_ms); + +/* UDP transport helpers (vehicle discovery) */ + +/** + * @brief Initialize DoIP UDP transport + * + * @param t Transport context + * @param port Local port + * @param loopback Enable loopback mode (instead of multicast) + * @retval 0 success + * @retval -1 error + */ +int doip_tp_udp_init(DoIPUdpTransport *t, uint16_t port, bool loopback); + +/** + * @brief Join default DoIP multicast group for discovery + * + * @param t Transport context + * @retval 0 success + * @retval -1 error + */ +int doip_tp_udp_join_default_multicast(DoIPUdpTransport *t); + +/** + * @brief Receive data over DoIP UDP transport + * + * @param t Transport context + * @param buf Buffer to receive data into + * @param len Length of buffer + * @param timeout_ms Receive timeout in milliseconds + * @return ssize_t Number of bytes received, 0 on timeout, or -1 on error + */ +ssize_t doip_tp_udp_recv(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms); + +/** + * @brief Receive data over DoIP UDP transport with source address info + * + * @param t Transport context + * @param buf Buffer to receive data into + * @param len Length of buffer + * @param timeout_ms Receive timeout in milliseconds + * @param src_ip_out Output buffer for source IP address + * @param src_ip_out_sz Size of source IP output buffer + * @param src_port_out Output for source port + * @return ssize_t Number of bytes received, 0 on timeout, or -1 on error + */ +ssize_t doip_tp_udp_recvfrom(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms, + char *src_ip_out, size_t src_ip_out_sz, uint16_t *src_port_out); + +/** + * @brief Close DoIP UDP transport + * + * @param t Transport context + */ +void doip_tp_udp_close(DoIPUdpTransport *t); + +/* UDP send helper */ +/** + * @brief Send data over DoIP UDP transport + * + * @param t Transport context + * @param buf Data buffer to send + * @param len Length of data to send + * @param dst_ip Destination IP address + * @param dst_port Destination port + * @param timeout_ms Send timeout in milliseconds + * @return ssize_t Number of bytes sent, or -1 on error + */ +ssize_t doip_tp_udp_sendto(DoIPUdpTransport *t, const uint8_t *buf, size_t len, const char *dst_ip, + uint16_t dst_port, int timeout_ms); + +#endif /* DOIP_MOCK_TP */ + +/* TCP transport function pointers */ +typedef int (*tcp_init)(DoIPTcpTransport *tcp, const char *ip, uint16_t port); +typedef int (*tcp_connect)(DoIPTcpTransport *tcp); +typedef ssize_t (*tcp_send)(const DoIPTcpTransport *tcp, const uint8_t *buf, size_t len); +typedef ssize_t (*tcp_recv)(DoIPTcpTransport *tcp, uint8_t *buf, size_t len, int timeout_ms); +typedef void (*tcp_close)(DoIPTcpTransport *tcp); + +/* UDP transport function pointers */ +typedef int (*udp_init)(DoIPUdpTransport *t, uint16_t port, bool loopback); +typedef ssize_t (*udp_recv)(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms); +typedef ssize_t (*udp_recvfrom)(DoIPUdpTransport *t, uint8_t *buf, size_t len, int timeout_ms, + char *src_ip_out, size_t src_ip_out_sz, uint16_t *src_port_out); +typedef ssize_t (*udp_sendto)(DoIPUdpTransport *t, const uint8_t *buf, size_t len, const char *dst_ip, + uint16_t dst_port, int timeout_ms); +typedef void (*udp_close)(DoIPUdpTransport *t); +typedef int (*udp_join_multicast)(DoIPUdpTransport *t); + + +typedef struct DoIPTcpTransport { + int fd; + int connect_timeout_ms; /* connect timeout (ms), <=0 uses default */ + int send_timeout_ms; /* send timeout (ms), <=0 uses default */ + char ip[64]; /* remote IP (for TCP) */ + uint16_t port; /* local or remote port */ + tcp_init init; + tcp_connect connect; + tcp_send send; + tcp_recv recv; + tcp_close close; +} DoIPTcpTransport; + +typedef struct DoIPUdpTransport { + int fd; + uint16_t port; /* local or remote port */ + udp_init init; + udp_recv recv; + udp_recvfrom recvfrom; + udp_sendto sendto; + udp_close close; + udp_join_multicast join_multicast; + bool loopback; /* UDP loopback mode */ +} DoIPUdpTransport; + +#endif /* UDS_TP_DOIP */ + +#endif /* DOIP_TRANSPORT_H */ diff --git a/src/tp/doip/notes.md b/src/tp/doip/notes.md new file mode 100644 index 000000000..4087c19d3 --- /dev/null +++ b/src/tp/doip/notes.md @@ -0,0 +1,93 @@ +# Onboarding Notes + +## Install bazel + +Project requires `bazel` 8.1.1 which can be installed via + +```bash +sudo apt update && sudo apt install bazel-8.1.1 +``` + +If version is not supported, `bazel` must be fetched from a different location, see [Bazel Home](https://bazel.build/install/ubuntu#install-on-ubuntu) + +```bash +sudo apt install apt-transport-https curl gnupg -y +curl -fsSL https://bazel.build/bazel-release.pub.gpg | gpg --dearmor >bazel-archive-keyring.gpg +sudo mv bazel-archive-keyring.gpg /usr/share/keyrings +echo "deb [arch=amd64 signed-by=/usr/share/keyrings/bazel-archive-keyring.gpg] https://storage.googleapis.com/bazel-apt stable jdk1.8" | sudo tee /etc/apt/sources.list.d/bazel.list +``` + +## Install required tools + +Install required Python modules + +```bash +pip3 install -r requirements.txt +``` + +For the build this is not sufficient - you need to install also `codechecker` + +```bash +pip3 install codechecker +``` + +Then you should be able to run `make` in the root dir + +```bash +make +``` + +## Return values of TP functions + +- recv: + - 0: response pending + - < 0: error + - n: message received + +- send: + - 0: send pending + - < 0: error + - n: message sent + +- poll: one of + UDS_TP_IDLE = 0x0000, // ready to send/recv + UDS_TP_SEND_IN_PROGRESS = 0x0001,// sending not complete + + UDS_TP_RECV_COMPLETE = 0x0002,// recv finished (not used/needed?) + UDS_TP_ERR = 0x0004, + + +```C +tp_status = UDSTpPoll(client->tp); +err = UDS_OK; +switch (client->state) { + case STATE_IDLE: + // reset options... + break; + case STATE_SENDING: { + // tp_recv must return 0 + // rx buffer cleared + // tp_send must return either + // > 0: + // number of sent bytes is + // a) equal to expected bytes -> STATE_AWAIT_SEND_COMPLETE + //. b) not equal to expected bytes -> err = UDS_ERR_BUFSIZ + // 0: send pending + // < 0: ERROR = UDS_ERR_TPORT + break; + } + case STATE_AWAIT_SEND_COMPLETE: { + // tp_status = UDS_TP_SEND_IN_PROGRESS -> exit UDS_OK + // -> STATE_AWAIT_RESPONSE + // start p2 timer + break; + } + case STATE_AWAIT_RESPONSE: { + // tp_recv must return either + // < 0: error = UDS_ERR_TPORT, -> STATE_IDLE + // = 0: continue receive unless p2 expired + // > 0: number of received bytes, server checks response + // a) OK -> forward to client app + // b) exit with error + break; + } diff --git a/src/tp/doip/pr.md b/src/tp/doip/pr.md new file mode 100644 index 000000000..06599b3da --- /dev/null +++ b/src/tp/doip/pr.md @@ -0,0 +1,54 @@ +## Add DoIP (ISO 13400) Transport Layer Support + +This PR implements DoIP (Diagnostic over IP - ISO 13400) as a new transport layer for UDS, enabling diagnostic communication over TCP/IP networks. + +### Features + +- **DoIP Client Implementation** (`src/tp/doip/`) + - TCP-based diagnostic message transport (ISO 13400-2) + - Routing activation and alive check handling + - Proper DoIP message framing and state management + - Support for diagnostic message ACK/NACK + - Non-blocking I/O with configurable timeouts + +### Implementation Details + +- **Protocol Version**: DoIP v3 (ISO 13400:2019) +- **Standard Port**: TCP 13400 +- **Addressing**: External test equipment range (0x0E00-0x0FFF) +- **Message Types**: Routing activation, diagnostic messages, alive check + +### Testing + +- Compiles cleanly with `-Wall -Wpedantic -Wextra` +- Integration test passes (`examples/doip_client/test.sh`) +- Successfully exchanges RDBI/WDBI messages over DoIP + +### Code Quality + +- Consistent with iso14229 coding style +- Comprehensive error handling and logging +- Well-documented with inline comments and function documentation +- No compiler warnings + +### Missing Features + +- UDP vehicle discovery +- DoIP server implementation in `src/tp/doip/` (currently only in examples) +- TLS support (port 3496) +- Unit tests for DoIP module +- Multi-client server support + +### Related Standards + +- ISO 13400-2:2019 - Road vehicles — Diagnostic communication over Internet Protocol (DoIP) +- ISO 14229 - Unified Diagnostic Services (UDS) + +--- + +**Testing Instructions:** + +```bash +cd examples/doip_client +make +[test.sh](http://_vscodecontentref_/3) # Automated integration test \ No newline at end of file diff --git a/tools/amalgamate.py b/tools/amalgamate.py index 9099d0c84..6c01b0d03 100644 --- a/tools/amalgamate.py +++ b/tools/amalgamate.py @@ -56,6 +56,9 @@ def strip_includes(src): "src/tp/isotp_c_socketcan.c", "src/tp/isotp_sock.c", "src/tp/isotp_mock.c", + "src/tp/doip/doip_client.c", + "src/tp/doip/doip_tp_udp.c", + "src/tp/doip/doip_tp_tcp.c", ]: f.write("\n") f.write("#ifdef UDS_LINES\n") @@ -115,6 +118,9 @@ def strip_includes(src): "src/tp/isotp_c_socketcan.h", "src/tp/isotp_sock.h", "src/tp/isotp_mock.h", + "src/tp/doip/doip_defines.h", + "src/tp/doip/doip_transport.h", + "src/tp/doip/doip_client.h", ]: f.write("\n") with open(src) as src_file: