From 254ce23fb056c6b092c6870c13eaf39291570753 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Sun, 7 Dec 2025 13:07:08 +0100 Subject: [PATCH 1/8] doc: Remove dup image --- doc/ExampleDoIPServer.md | 5 ----- 1 file changed, 5 deletions(-) diff --git a/doc/ExampleDoIPServer.md b/doc/ExampleDoIPServer.md index 2cb826c..181a7d7 100644 --- a/doc/ExampleDoIPServer.md +++ b/doc/ExampleDoIPServer.md @@ -174,11 +174,6 @@ m_model.onDownstreamRequest = [this](IConnectionContext &ctx, const DoIPMessage Below is a PlantUML diagram illustrating `DoIPServer`, `DoIPConnection`, `DoIPServerModel` and a downstream UDS/CAN backend interaction. -[\image html ]ServerModel.svg - -For GitHub or other markdown viewers that don't process the Doxygen -directive, the SVG is also referenced directly: - ![ServerModel](diagrams/ServerModel.svg) ## Logging and debugging tips From 8953ada1b0dfe64a80c6f184c093a43697804dd5 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Sun, 7 Dec 2025 13:14:20 +0100 Subject: [PATCH 2/8] doc: Fix links --- README.md | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 1dddb40..cf02eed 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ [![CMake](https://img.shields.io/badge/CMake-3.15+-blue.svg)](https://cmake.org/) [![doctest](https://img.shields.io/badge/Tests-doctest-green.svg)](https://github.com/doctest/doctest) -C/C++ library for Diagnostics over IP (DoIP) (fork from https://github.com/AVL-DiTEST-DiagDev/libdoip) +C/C++ library for Diagnostics over IP (DoIP) (fork from [https://github.com/AVL-DiTEST-DiagDev/libdoip](https://github.com/AVL-DiTEST-DiagDev/libdoip)) **CAUTION** The current API is under construction any may change at any time. @@ -26,9 +26,9 @@ See [Logging](./doc/LOGGING.md) for details. Quick start — read the generated tutorial for the example server: -- Online (published): https://magolves.github.io/libdoip/index.html +- Online (published): [https://magolves.github.io/libdoip/index.html](https://magolves.github.io/libdoip/index.html) - Local example page: see `doc/ExampleDoIPServer.md` (included in the Doxygen HTML under "Example DoIP Server Tutorial"). -- Example tutorial (direct): https://magolves.github.io/libdoip/ExampleDoIPServer.html +- Example tutorial (direct): [https://magolves.github.io/libdoip/md_doc_ExampleDoIPServer.html](https://magolves.github.io/libdoip/md_doc_ExampleDoIPServer.html) If you want to generate the docs locally, install Doxygen and Graphviz and run: @@ -47,7 +47,7 @@ xdg-open docs/html/index.html git clone https://github.com/Magolves/libdoip.git ``` -2. Enter the directory 'libdoip' and build the library with: +1. Enter the directory 'libdoip' and build the library with: ```bash cmake . -Bbuild @@ -83,14 +83,13 @@ handlers. - Example source files: `examples/exampleDoIPServer.cpp`, `examples/ExampleDoIPServerModel.h` -- Example tutorial (published): https://magolves.github.io/libdoip/ExampleDoIPServer.html +- Example tutorial: [https://magolves.github.io/libdoip/ExampleDoIPServer.html](https://magolves.github.io/libdoip/ExampleDoIPServer.html) See the "Examples" section in the generated Doxygen main page for additional annotated links to these files. - ## References - [ISO 13400-2:2019(en) Road vehicles — Diagnostic communication over Internet Protocol (DoIP) — Part 2: Transport protocol and network layer services]() - [Specification of Diagnostic over IP]() -- [Diagnostics over Internet Protocol (DoIP)]() \ No newline at end of file +- [Diagnostics over Internet Protocol (DoIP)]() From f68d6b6bb816be9bd8c467a228b8c1c9efc84b85 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Sun, 7 Dec 2025 13:14:34 +0100 Subject: [PATCH 3/8] refactor: Remove simple DoIP server example and add warning to client code refactor: Remove simple DoIP server example from CMakeLists --- examples/CMakeLists.txt | 19 +--- examples/exampleDoIPClient.cpp | 4 + examples/exampleDoIPServerSimple.cpp | 141 --------------------------- 3 files changed, 6 insertions(+), 158 deletions(-) delete mode 100644 examples/exampleDoIPServerSimple.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 80c0aa5..4545639 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -29,27 +29,12 @@ set_target_properties(exampleDoIPClient PROPERTIES CXX_EXTENSIONS OFF ) -# Simple DoIP Server Example (using high-level API) -add_executable(exampleDoIPServerSimple exampleDoIPServerSimple.cpp) -target_link_libraries(exampleDoIPServerSimple - PRIVATE - ${DOIP_NAME} - Threads::Threads -) - -# Set properties for the simple server example -set_target_properties(exampleDoIPServerSimple PROPERTIES - CXX_STANDARD 17 - CXX_STANDARD_REQUIRED ON - CXX_EXTENSIONS OFF -) - # Disable switch-default warning for examples using spdlog target_compile_options(exampleDoIPServer PRIVATE -Wno-switch-default) target_compile_options(exampleDoIPClient PRIVATE -Wno-switch-default) -target_compile_options(exampleDoIPServerSimple PRIVATE -Wno-switch-default) + # Install examples (optional) -install(TARGETS exampleDoIPServer exampleDoIPClient exampleDoIPServerSimple +install(TARGETS exampleDoIPServer exampleDoIPClient RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/examples ) \ No newline at end of file diff --git a/examples/exampleDoIPClient.cpp b/examples/exampleDoIPClient.cpp index 75e1deb..d80155b 100644 --- a/examples/exampleDoIPClient.cpp +++ b/examples/exampleDoIPClient.cpp @@ -20,6 +20,10 @@ static void printUsage(const char *progName) { } int main(int argc, char *argv[]) { + std::cerr << "The client code does not work currently - use at your own risk!\n"; + // remove this line for enabling the client anyway + return EXIT_FAILURE; + string serverAddress = "224.0.0.2"; // Default multicast address // Parse command line arguments diff --git a/examples/exampleDoIPServerSimple.cpp b/examples/exampleDoIPServerSimple.cpp deleted file mode 100644 index 4e04e04..0000000 --- a/examples/exampleDoIPServerSimple.cpp +++ /dev/null @@ -1,141 +0,0 @@ -#include "DoIPAddress.h" -#include "DoIPServer.h" -#include "Logger.h" - -#include "ExampleDoIPServerModel.h" - -#include -#include -#include - -using namespace doip; -using namespace std; - -static const DoIPAddress LOGICAL_ADDRESS(0x0028); -static std::atomic g_shutdownRequested{false}; - -// Forward declarations -static void signalHandler(int signal); -static std::optional onConnectionAccepted(DoIPConnection *connection); -static void configureServer(DoIPServer &server); - -// Signal handler for graceful shutdown -static void signalHandler(int signal) { - (void)signal; - cout << "\nShutdown requested..." << endl; - g_shutdownRequested.store(true); -} - -// Callback invoked when a new connection is accepted -static std::optional onConnectionAccepted(DoIPConnection *connection) { - (void)connection; - cout << "New client connected!" << endl; - - // Create a server model for this connection - DoIPServerModel model; - model.serverAddress = LOGICAL_ADDRESS; - - model.onDiagnosticMessage = [](IConnectionContext &ctx, const DoIPMessage &msg) noexcept -> DoIPDiagnosticAck { - (void)ctx; - cout << "Received Diagnostic message: " << msg << '\n'; - - auto payload = msg.getPayload(); - if (payload.second > 0) { - cout << " Service ID: 0x" << hex << setw(2) << setfill('0') - << static_cast(payload.first[0]) << dec << '\n'; - } - - // Example logic: NACK if service ID is 0x22 (Read Data by Identifier) - if (payload.second > 0 && payload.first[0] == 0x22) { - cout << " - Sending NACK for service ID 0x22\n"; - return DoIPNegativeDiagnosticAck::UnknownTargetAddress; - } - - return std::nullopt; // Send positive ACK - }; - - model.onDiagnosticNotification = [](IConnectionContext &ctx, DoIPDiagnosticAck ack) noexcept { - (void)ctx; - if (ack.has_value()) { - cout << "Diagnostic NACK sent: " << static_cast(ack.value()) << '\n'; - } else { - cout << "Diagnostic ACK sent\n"; - } - }; - - model.onCloseConnection = [](IConnectionContext &ctx, DoIPCloseReason reason) noexcept { - (void)ctx; - cout << "Connection closed (" << reason << ")" << endl; - }; - - return model; -} - -static void configureServer(DoIPServer &server) { - // VIN needs to have a fixed length of 17 bytes. - // Shorter VINs will be padded with '0' - server.setVIN("EXAMPLESERVER1234"); - server.setLogicalGatewayAddress(LOGICAL_ADDRESS.toUint16()); - server.setGID(0x1122334455667788); - server.setEID(0xAABBCCDDEEFF0011); - server.setFAR(DoIPFurtherAction::NoFurtherAction); - - // More relaxed timing for testing - server.setAnnounceInterval(2000); - server.setAnnounceNum(10); -} - -int main(int argc, char *argv[]) { - bool useLoopback = false; - - // Parse command line arguments - for (int i = 1; i < argc; i++) { - string arg = argv[i]; - if (arg == "--loopback") { - useLoopback = true; - } else if (arg == "--help") { - cout << "Usage: " << argv[0] << " [OPTIONS]\n"; - cout << "Options:\n"; - cout << " --loopback Use loopback for announcements\n"; - cout << " --help Show this help\n"; - return 0; - } else { - cerr << "Unknown argument: " << arg << endl; - return 1; - } - } - - // Setup signal handlers for graceful shutdown - signal(SIGINT, signalHandler); - signal(SIGTERM, signalHandler); - - // Configure logging - doip::Logger::setLevel(spdlog::level::info); - LOG_DOIP_INFO("Starting Simple DoIP Server Example"); - - DoIPServer server; - configureServer(server); - - if (useLoopback) { - server.setAnnouncementMode(true); - } - - // Start the server with automatic connection handling - if (!server.start(onConnectionAccepted, true)) { - LOG_DOIP_ERROR("Failed to start server"); - return 1; - } - - LOG_DOIP_INFO("Server is running. Press Ctrl+C to stop."); - - // Main thread just waits for shutdown signal - while (!g_shutdownRequested.load()) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - - // Graceful shutdown - server.stop(); - LOG_DOIP_INFO("Server terminated cleanly"); - - return 0; -} From 1cc3fc206809726dbc94712ef6b7eb8e5789cfd0 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Sun, 7 Dec 2025 13:21:04 +0100 Subject: [PATCH 4/8] refactor: Remove early exit from client main function --- examples/exampleDoIPClient.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/exampleDoIPClient.cpp b/examples/exampleDoIPClient.cpp index d80155b..60103e7 100644 --- a/examples/exampleDoIPClient.cpp +++ b/examples/exampleDoIPClient.cpp @@ -21,8 +21,6 @@ static void printUsage(const char *progName) { int main(int argc, char *argv[]) { std::cerr << "The client code does not work currently - use at your own risk!\n"; - // remove this line for enabling the client anyway - return EXIT_FAILURE; string serverAddress = "224.0.0.2"; // Default multicast address From db2c9aa96f4a607e9fb77744a2acfd5eb9daccd7 Mon Sep 17 00:00:00 2001 From: Oliver <56022454+Magolves@users.noreply.github.com> Date: Tue, 9 Dec 2025 16:13:28 +0100 Subject: [PATCH 5/8] Feature/refactor doip server (#10) * feat: Add server config, introduce daemonize * WIP: Track UDP reception problem on server * fix: Split up rx/tx socket (WIP - still not work) * doc: Add generated UDP example * WIP: Migration to new code * fix: UDP connection/discover works now * fix: Return explicit return code * style: Reorder includes; replace printf with LOG_... * chore: Remove dup test; add enable_testing * refactor: Update DoIPServer to use structured types and improve method names * refactor: Update Doxyfile configuration for improved documentation generation * style: Add comments, format code * WIP: Code cleanup * chore: revisit clang-tidy rules * refactor: Introduce sendUdpResponse fix: Make setMulticastGroup const * feat: Introduce DOIP_MAXIMUM_MTU * refactor: Adjust member names * feat: Add accessors for VIn, EID, GID and logical address * feat: Add stream operator * - refcator: Cleanup client code - refactor: Rename identifier types --------- Co-authored-by: Oliver Wieland (HC/XAG1) --- .clang-tidy | 28 +- CMakeLists.txt | 3 +- Doxyfile | 8 +- doc/Fix-UDP-reception.md | 123 ++++++++ doc/udp/Makefile | 16 + doc/udp/README.md | 117 +++++++ doc/udp/doip_client.c | 308 +++++++++++++++++++ doc/udp/doip_server.c | 285 +++++++++++++++++ examples/CMakeLists.txt | 76 +++-- examples/exampleDoIPClient.cpp | 9 +- examples/exampleDoIPDiscover.cpp | 71 +++++ examples/exampleDoIPServer.cpp | 113 ++++--- inc/DoIPClient.h | 23 +- inc/DoIPConnection.h | 7 +- inc/DoIPFurtherAction.h | 17 + inc/DoIPIdentifiers.h | 31 +- inc/DoIPMessage.h | 96 +++++- inc/DoIPServer.h | 285 +++++++++-------- inc/gen/DoIPConfig.h.in | 13 +- src/DoIPClient.cpp | 186 ++++++----- src/DoIPConnection.cpp | 4 +- src/DoIPServer.cpp | 460 ++++++++++++++++------------ test/DoIPMessage_Test.cpp | 43 +++ test/DoIPServer_Test.cpp | 14 +- test/Identifiers_Test.cpp | 188 ++++++------ test/VehicleIdentification_Test.cpp | 14 +- 26 files changed, 1882 insertions(+), 656 deletions(-) create mode 100644 doc/Fix-UDP-reception.md create mode 100644 doc/udp/Makefile create mode 100644 doc/udp/README.md create mode 100644 doc/udp/doip_client.c create mode 100644 doc/udp/doip_server.c create mode 100644 examples/exampleDoIPDiscover.cpp diff --git a/.clang-tidy b/.clang-tidy index 0a0462f..bcca1e5 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,5 +1,31 @@ --- -Checks: '*,-llvmlibc-*,-fuchsia-*,-google-readability-todo,-readability-else-after-return,-llvm-header-guard,-llvm-namespace-comment,-modernize-use-trailing-return-type,-altera-struct-pack-align,-google-explicit-constructor,-hicpp-explicit-conversions,-cppcoreguidelines-pro-bounds-pointer-arithmetic,-hicpp-signed-bitwise,-readability-identifier-length,-bugprone-reserved-identifier,-cert-dcl37-c,-cert-dcl51-cpp,-google-runtime-int,-misc-include-cleaner,-misc-non-private-member-variables-in-classes,-google-build-using-namespace,-readability-convert-member-functions-to-static,-llvm-include-order,-misc-const-correctness,-cppcoreguidelines-avoid-magic-numbers,-readability-magic-numbers,-modernize-use-nodiscard' +Checks: | + -bugprone-* + -performance-* + -readability-braces-around-statements + -readability-inconsistent-declaration-parameter-name + -readability-implicit-bool-conversion + -readability-redundant-declaration + -readability-redundant-member-init + -readability-static-definition-in-anonymous-namespace + -readability-uppercase-literal-suffix + -cppcoreguidelines-avoid-goto + -cppcoreguidelines-avoid-magic-numbers + -cppcoreguidelines-no-malloc + -cppcoreguidelines-owning-memory + -cppcoreguidelines-slicing + -modernize-use-override + -modernize-avoid-c-arrays + -modernize-loop-convert + -modernize-redundant-void-arg + -modernize-use-auto + -modernize-use-equals-default + -modernize-use-equals-delete + -modernize-use-nullptr + -misc-definitions-in-headers + -misc-misplaced-const + -misc-no-recursion + -misc-static-assert WarningsAsErrors: '' HeaderFilterRegex: '.*' FormatStyle: none \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 02a81e7..b1c33d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,9 +58,10 @@ endif() # Configuration options set(DOIP_ALIVE_CHECK_RETRIES "1" CACHE STRING "Number of retries for DoIP alive check messages") +set(DOIP_MAXIMUM_MTU "4095" CACHE STRING "Maximum Transmission Unit (MTU) size for DoIP messages") # Validate numeric options -foreach(VAR DOIP_ALIVE_CHECK_RETRIES) +foreach(VAR DOIP_ALIVE_CHECK_RETRIES DOIP_MAXIMUM_MTU) if(NOT ${VAR} MATCHES "^[0-9]+$") message(FATAL_ERROR "${VAR} must be a positive integer") endif() diff --git a/Doxyfile b/Doxyfile index 2213d85..dd57630 100644 --- a/Doxyfile +++ b/Doxyfile @@ -49,7 +49,7 @@ INLINE_GROUPED_CLASSES = NO INLINE_SIMPLE_STRUCTS = NO TYPEDEF_HIDES_STRUCT = NO LOOKUP_CACHE_SIZE = 0 -NUM_PROC_THREADS = 1 +NUM_PROC_THREADS = 4 #--------------------------------------------------------------------------- # Build related configuration options @@ -60,7 +60,7 @@ EXTRACT_PRIV_VIRTUAL = NO EXTRACT_PACKAGE = NO EXTRACT_STATIC = YES EXTRACT_LOCAL_CLASSES = YES -EXTRACT_LOCAL_METHODS = NO +EXTRACT_LOCAL_METHODS = YES EXTRACT_ANON_NSPACES = NO RESOLVE_UNNAMED_PARAMS = YES HIDE_UNDOC_MEMBERS = NO @@ -103,7 +103,7 @@ WARNINGS = YES WARN_IF_UNDOCUMENTED = YES WARN_IF_DOC_ERROR = YES WARN_IF_INCOMPLETE_DOC = YES -WARN_NO_PARAMDOC = NO +WARN_NO_PARAMDOC = YES WARN_AS_ERROR = NO WARN_FORMAT = "$file:$line: $text" WARN_LINE_FORMAT = "at line $line of file $file" @@ -153,7 +153,7 @@ USE_MDFILE_AS_MAINPAGE = README.md #--------------------------------------------------------------------------- SOURCE_BROWSER = YES INLINE_SOURCES = NO -STRIP_CODE_COMMENTS = YES +STRIP_CODE_COMMENTS = NO REFERENCED_BY_RELATION = YES REFERENCES_RELATION = YES REFERENCES_LINK_SOURCE = YES diff --git a/doc/Fix-UDP-reception.md b/doc/Fix-UDP-reception.md new file mode 100644 index 0000000..aab174c --- /dev/null +++ b/doc/Fix-UDP-reception.md @@ -0,0 +1,123 @@ +From your **tcpdump output**, the **8-byte UDP packet** from the client (port 50290) to the server (port 13400) **is arriving** at the loopback interface: + +``` +17:31:49.091339 lo In IP localhost.50290 > localhost.13400: UDP, length 8 + 0x0020: 04fb 0001 0000 0000 +``` +This is a **DoIP Vehicle Identification Request** (type `0x0001`), and it is **reaching the kernel**. + +--- + +### **Root Cause Analysis** +Since the packet is visible in `tcpdump` but **not logged by your server**, the issue is **almost certainly in your server code**. Here’s what’s happening: + +#### **1. The Server Socket is Not Receiving the Packet** +- The server socket is bound to port 13400, but **something is preventing `recvfrom` from returning the packet**. +- Possible reasons: + - The socket is **not in the expected state** (e.g., closed, re-bound, or filtered). + - The socket is **not the same as the one bound to port 13400** (e.g., `m_udp_sock` was overwritten or closed). + - The socket is **blocked by a firewall or SELinux** (unlikely for loopback, but possible). + - The socket is **not actually bound to port 13400** (e.g., `bind` failed silently). + +#### **2. The Server is Stuck in a Timeout Loop** +- Your logs show `recvfrom returned: -1 (errno: 11)` repeatedly. +- This means `recvfrom` is **timing out** (`EAGAIN`), but the packet is **not being delivered to userspace**. +- This suggests the socket is **not properly bound** or is **not the same socket** as the one receiving the packet. + +--- + +### **Debugging Steps** +#### **1. Verify the Socket is Bound Correctly** +Add this **right after `bind()`** in `setupUdpSocket()`: +```cpp +// After bind() +sockaddr_in bound_addr; +socklen_t bound_addr_len = sizeof(bound_addr); +getsockname(m_udp_sock, (sockaddr*)&bound_addr, &bound_addr_len); +LOG_UDP_DEBUG( + "Socket {} bound to {}:{}", + m_udp_sock, + inet_ntoa(bound_addr.sin_addr), + ntohs(bound_addr.sin_port) +); +``` +- If this logs `0.0.0.0:13400` or `127.0.0.1:13400`, the socket is bound correctly. +- If not, `bind()` failed or was not called. + +#### **2. Check for Socket Descriptor Leaks or Overwrites** +- Log `m_udp_sock` in `receiveUdpMessage()`: + ```cpp + LOG_UDP_DEBUG("Using socket fd: {}", m_udp_sock); + ``` +- If the value changes between `setupUdpSocket()` and `receiveUdpMessage()`, something is overwriting it. + +#### **3. Test with a Minimal UDP Server** +Replace your `receiveUdpMessage()` with this **minimal test**: +```cpp +ssize_t DoIPServer::receiveUdpMessage() { + char buf[1024]; + sockaddr_in client_addr; + socklen_t client_len = sizeof(client_addr); + ssize_t read_bytes = recvfrom( + m_udp_sock, buf, sizeof(buf), 0, + (sockaddr*)&client_addr, &client_len + ); + LOG_UDP_DEBUG( + "recvfrom returned: {} (errno: {})", + read_bytes, errno + ); + if (read_bytes > 0) { + LOG_UDP_INFO( + "RX {} bytes from {}:{}", + read_bytes, + inet_ntoa(client_addr.sin_addr), + ntohs(client_addr.sin_port) + ); + } + return read_bytes; +} +``` +- If this works, the issue is in your buffer or processing logic. +- If it still times out, the socket is **not receiving packets**. + +#### **4. Check for Firewall/SELinux** +Even for loopback, run: +```bash +sudo iptables -L -n -v # Check firewall rules +sudo setenforce 0 # Disable SELinux temporarily +``` +Retest. If it works, adjust your firewall/SELinux rules. + +#### **5. Use `strace` to Confirm `recvfrom` Behavior** +Run the server with: +```bash +strace -e trace=recvfrom ./your_server +``` +- If `recvfrom` is **never called**, your loop is broken. +- If it returns `-1 EAGAIN` repeatedly, the socket is **not receiving packets**. + +#### **6. Check for Multiple Sockets** +- If your code creates **multiple UDP sockets**, ensure the correct one is used for `recvfrom`. +- Log all socket creation and `bind()` calls. + +--- + +### **Most Likely Fixes** +1. **The socket is not bound to port 13400.** + - Confirm `bind()` succeeds and logs the correct port. +2. **`m_udp_sock` is overwritten or closed.** + - Log its value in `setupUdpSocket()` and `receiveUdpMessage()`. +3. **The socket is non-blocking and the timeout is too short.** + - Try removing `SO_RCVTIMEO` temporarily to see if packets arrive. + +--- + +### **Summary** +- The packet **is arriving at the kernel** (confirmed by `tcpdump`). +- The server **is not receiving it**, likely due to: + - Socket not bound correctly. + - Wrong socket file descriptor used in `recvfrom`. + - Firewall/SELinux blocking delivery to userspace. + +**Next step:** +Add the debug logs for `m_udp_sock` and `getsockname()` as shown above. If the socket is correct, test with the minimal `recvfrom` code. If it still fails, the issue is **outside your code** (firewall, SELinux, or kernel networking stack). \ No newline at end of file diff --git a/doc/udp/Makefile b/doc/udp/Makefile new file mode 100644 index 0000000..97121ff --- /dev/null +++ b/doc/udp/Makefile @@ -0,0 +1,16 @@ +CC = gcc +CFLAGS = -Wall -Wextra -pthread -g +LDFLAGS = -pthread + +all: doip_server doip_client + +doip_server: doip_server.c + $(CC) $(CFLAGS) -o doip_server doip_server.c $(LDFLAGS) + +doip_client: doip_client.c + $(CC) $(CFLAGS) -o doip_client doip_client.c $(LDFLAGS) + +clean: + rm -f doip_server doip_client + +.PHONY: all clean diff --git a/doc/udp/README.md b/doc/udp/README.md new file mode 100644 index 0000000..66c33b8 --- /dev/null +++ b/doc/udp/README.md @@ -0,0 +1,117 @@ +# Minimal DoIP UDP Discovery Implementation + +This is a minimal implementation of the DoIP (ISO 13400) UDP discovery mechanism with both server and client. + +## Features + +- **Server**: Sends periodic Vehicle Announcements and responds to Vehicle Identification Requests +- **Client**: Listens for Vehicle Announcements, stores server IP, and sends Vehicle Identification Request +- **Loopback mode**: For testing client and server on the same host (avoids multicast issues) + +## Building + +```bash +make +``` + +This will create two executables: +- `doip_server` - The DoIP server +- `doip_client` - The DoIP client + +## Usage + +### Running in Loopback Mode (Recommended for Testing) + +**Terminal 1 - Start the server:** +```bash +./doip_server --loopback +``` + +**Terminal 2 - Start the client:** +```bash +./doip_client --loopback +``` + +### Running in Broadcast Mode (For Network Testing) + +**Terminal 1 - Start the server:** +```bash +./doip_server +``` + +**Terminal 2 - Start the client:** +```bash +./doip_client +``` + +## How It Works + +### Server (Port 13400) +1. Creates two UDP sockets (both bound to port 13400 with SO_REUSEADDR): + - Receive socket: Listens for incoming Vehicle Identification Requests + - Send socket: Sends Vehicle Announcements and responses +2. Starts two threads: + - Listener thread: Continuously listens for requests + - Announcement thread: Sends 5 Vehicle Announcements (every 2 seconds) to port 13401 +3. When a Vehicle Identification Request is received: + - Parses the request + - Sends a Vehicle Identification Response back to the client + +### Client (Port 13401) +1. Creates two UDP sockets: + - Request socket: Sends Vehicle Identification Requests (unbound, OS assigns port) + - Announcement socket: Bound to port 13401 to receive Vehicle Announcements +2. Workflow: + - Listens for Vehicle Announcement from server + - Extracts vehicle information (VIN, Logical Address, EID, GID, IP address) + - Sends Vehicle Identification Request to the discovered server IP on port 13400 + - Waits for and displays the response + +## Ports + +- **13400**: DoIP UDP Discovery Port (server listens here, client sends requests here) +- **13401**: DoIP Test Equipment Port (client listens here for announcements) + +## Protocol Details + +### DoIP Header (8 bytes) +- Protocol Version: 0x04 +- Inverse Protocol Version: 0xFB +- Payload Type: 2 bytes +- Payload Length: 4 bytes + +### Payload Types +- `0x0001`: Vehicle Identification Request (no payload) +- `0x0004`: Vehicle Identification Response (33 bytes minimum) + +### Vehicle Identification Response Payload +- VIN: 17 bytes (ASCII) +- Logical Address: 2 bytes +- EID: 6 bytes +- GID: 6 bytes +- Further Action Required: 1 byte +- VIN/GID sync status: 1 byte + +## Key Design Decisions + +1. **Two sockets on server**: Avoids race conditions between sending announcements and receiving requests +2. **SO_REUSEADDR**: Allows both server sockets to bind to port 13400 +3. **Loopback mode**: Uses unicast (127.0.0.1) instead of broadcast for local testing +4. **Non-blocking receive**: Server uses timeout to allow clean shutdown +5. **Separate announcement socket on client**: Dedicated socket for receiving broadcasts on port 13401 + +## Troubleshooting + +If the client doesn't receive announcements: +- Check firewall rules +- Verify both programs are running +- Use `tcpdump` to monitor UDP traffic: + ```bash + sudo tcpdump -i any udp port 13400 or udp port 13401 -X + ``` + +## Cleanup + +```bash +make clean +``` diff --git a/doc/udp/doip_client.c b/doc/udp/doip_client.c new file mode 100644 index 0000000..e7c60bf --- /dev/null +++ b/doc/udp/doip_client.c @@ -0,0 +1,308 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#define DOIP_UDP_DISCOVERY_PORT 13400 +#define DOIP_UDP_TEST_EQUIPMENT_PORT 13401 +#define DOIP_PROTOCOL_VERSION 0x04 +#define DOIP_INVERSE_PROTOCOL_VERSION 0xFB + +// DoIP Payload Types +#define VEHICLE_IDENTIFICATION_REQUEST 0x0001 +#define VEHICLE_IDENTIFICATION_RESPONSE 0x0004 + +// Discovered vehicle information +typedef struct { + char vin[18]; + uint16_t logical_address; + uint8_t eid[6]; + uint8_t gid[6]; + char ip_address[INET_ADDRSTRLEN]; + uint16_t port; +} VehicleInfo; + +// Create DoIP header +void create_doip_header(uint8_t *buffer, uint16_t payload_type, uint32_t payload_length) { + buffer[0] = DOIP_PROTOCOL_VERSION; + buffer[1] = DOIP_INVERSE_PROTOCOL_VERSION; + buffer[2] = (payload_type >> 8) & 0xFF; + buffer[3] = payload_type & 0xFF; + buffer[4] = (payload_length >> 24) & 0xFF; + buffer[5] = (payload_length >> 16) & 0xFF; + buffer[6] = (payload_length >> 8) & 0xFF; + buffer[7] = payload_length & 0xFF; +} + +// Create Vehicle Identification Request +int create_vehicle_identification_request(uint8_t *buffer) { + create_doip_header(buffer, VEHICLE_IDENTIFICATION_REQUEST, 0); + return 8; // Header only, no payload +} + +// Parse DoIP header +bool parse_doip_header(const uint8_t *buffer, size_t length, uint16_t *payload_type, uint32_t *payload_length) { + if (length < 8) { + return false; + } + + if (buffer[0] != DOIP_PROTOCOL_VERSION || buffer[1] != DOIP_INVERSE_PROTOCOL_VERSION) { + printf("[CLIENT] Invalid DoIP protocol version\n"); + return false; + } + + *payload_type = (buffer[2] << 8) | buffer[3]; + *payload_length = (buffer[4] << 24) | (buffer[5] << 16) | (buffer[6] << 8) | buffer[7]; + + return true; +} + +// Parse Vehicle Identification Response +bool parse_vehicle_identification_response(const uint8_t *buffer, size_t length, VehicleInfo *info) { + if (length < 41) { // 8 (header) + 33 (minimum payload) + printf("[CLIENT] Message too short for Vehicle Identification Response\n"); + return false; + } + + int offset = 8; // Skip header + + // VIN (17 bytes) + memcpy(info->vin, buffer + offset, 17); + info->vin[17] = '\0'; + offset += 17; + + // Logical Address (2 bytes) + info->logical_address = (buffer[offset] << 8) | buffer[offset + 1]; + offset += 2; + + // EID (6 bytes) + memcpy(info->eid, buffer + offset, 6); + offset += 6; + + // GID (6 bytes) + memcpy(info->gid, buffer + offset, 6); + offset += 6; + + return true; +} + +// Print vehicle information +void print_vehicle_info(const VehicleInfo *info) { + printf("\n[CLIENT] ========== Vehicle Information ==========\n"); + printf("[CLIENT] VIN: %s\n", info->vin); + printf("[CLIENT] Logical Address: 0x%04X\n", info->logical_address); + printf("[CLIENT] EID: %02X:%02X:%02X:%02X:%02X:%02X\n", + info->eid[0], info->eid[1], info->eid[2], + info->eid[3], info->eid[4], info->eid[5]); + printf("[CLIENT] GID: %02X:%02X:%02X:%02X:%02X:%02X\n", + info->gid[0], info->gid[1], info->gid[2], + info->gid[3], info->gid[4], info->gid[5]); + printf("[CLIENT] Server IP: %s\n", info->ip_address); + printf("[CLIENT] Server Port: %d\n", info->port); + printf("[CLIENT] ============================================\n\n"); +} + +int main(int argc, char *argv[]) { + bool use_loopback = false; + + // Parse command line arguments + for (int i = 1; i < argc; i++) { + if (strcmp(argv[i], "--loopback") == 0) { + use_loopback = true; + } + } + + printf("[CLIENT] Starting DoIP Client\n"); + printf("[CLIENT] Mode: %s\n", use_loopback ? "Loopback" : "Broadcast"); + + // Create socket for sending requests + int request_sock = socket(AF_INET, SOCK_DGRAM, 0); + if (request_sock < 0) { + perror("[CLIENT] Failed to create request socket"); + return 1; + } + + // Create socket for receiving announcements + int announcement_sock = socket(AF_INET, SOCK_DGRAM, 0); + if (announcement_sock < 0) { + perror("[CLIENT] Failed to create announcement socket"); + close(request_sock); + return 1; + } + + // Enable SO_REUSEADDR for announcement socket + int reuse = 1; + setsockopt(announcement_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + + // Enable broadcast reception + int broadcast = 1; + if (setsockopt(announcement_sock, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) < 0) { + perror("[CLIENT] Failed to enable broadcast reception"); + } + + // Bind announcement socket to port 13401 + struct sockaddr_in announcement_addr; + memset(&announcement_addr, 0, sizeof(announcement_addr)); + announcement_addr.sin_family = AF_INET; + announcement_addr.sin_addr.s_addr = htonl(INADDR_ANY); + announcement_addr.sin_port = htons(DOIP_UDP_TEST_EQUIPMENT_PORT); + + if (bind(announcement_sock, (struct sockaddr *)&announcement_addr, sizeof(announcement_addr)) < 0) { + perror("[CLIENT] Failed to bind announcement socket"); + close(request_sock); + close(announcement_sock); + return 1; + } + + printf("[CLIENT] Announcement socket bound to 0.0.0.0:%d\n", DOIP_UDP_TEST_EQUIPMENT_PORT); + + // Set timeout for announcement socket + struct timeval timeout; + timeout.tv_sec = 5; + timeout.tv_usec = 0; + setsockopt(announcement_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + + // Listen for Vehicle Announcement + printf("[CLIENT] Listening for Vehicle Announcements...\n"); + + uint8_t buffer[512]; + struct sockaddr_in server_addr; + socklen_t server_len = sizeof(server_addr); + + ssize_t received = recvfrom(announcement_sock, buffer, sizeof(buffer), 0, + (struct sockaddr *)&server_addr, &server_len); + + if (received < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { + printf("[CLIENT] Timeout: No Vehicle Announcement received\n"); + } else { + perror("[CLIENT] Error receiving announcement"); + } + close(request_sock); + close(announcement_sock); + return 1; + } + + char server_ip[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &server_addr.sin_addr, server_ip, sizeof(server_ip)); + printf("[CLIENT] Received announcement: %zd bytes from %s:%d\n", + received, server_ip, ntohs(server_addr.sin_port)); + + // Parse the announcement + uint16_t payload_type; + uint32_t payload_length; + + if (!parse_doip_header(buffer, received, &payload_type, &payload_length)) { + printf("[CLIENT] Failed to parse DoIP header\n"); + close(request_sock); + close(announcement_sock); + return 1; + } + + if (payload_type != VEHICLE_IDENTIFICATION_RESPONSE) { + printf("[CLIENT] Unexpected payload type: 0x%04X\n", payload_type); + close(request_sock); + close(announcement_sock); + return 1; + } + + VehicleInfo vehicle_info; + memset(&vehicle_info, 0, sizeof(vehicle_info)); + + if (!parse_vehicle_identification_response(buffer, received, &vehicle_info)) { + printf("[CLIENT] Failed to parse Vehicle Identification Response\n"); + close(request_sock); + close(announcement_sock); + return 1; + } + + // Store server information + strncpy(vehicle_info.ip_address, server_ip, sizeof(vehicle_info.ip_address)); + vehicle_info.port = DOIP_UDP_DISCOVERY_PORT; // Requests go to port 13400 + + print_vehicle_info(&vehicle_info); + + // Now send a Vehicle Identification Request to the discovered server + printf("[CLIENT] Sending Vehicle Identification Request to %s:%d\n", + vehicle_info.ip_address, vehicle_info.port); + + struct sockaddr_in request_addr; + memset(&request_addr, 0, sizeof(request_addr)); + request_addr.sin_family = AF_INET; + request_addr.sin_port = htons(vehicle_info.port); + inet_pton(AF_INET, vehicle_info.ip_address, &request_addr.sin_addr); + + uint8_t request[8]; + int request_len = create_vehicle_identification_request(request); + + ssize_t sent = sendto(request_sock, request, request_len, 0, + (struct sockaddr *)&request_addr, sizeof(request_addr)); + + if (sent < 0) { + perror("[CLIENT] Failed to send request"); + close(request_sock); + close(announcement_sock); + return 1; + } + + printf("[CLIENT] Sent Vehicle Identification Request: %zd bytes\n", sent); + + // Wait for response + timeout.tv_sec = 3; + timeout.tv_usec = 0; + setsockopt(request_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + + struct sockaddr_in response_addr; + socklen_t response_len = sizeof(response_addr); + + received = recvfrom(request_sock, buffer, sizeof(buffer), 0, + (struct sockaddr *)&response_addr, &response_len); + + if (received < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { + printf("[CLIENT] Timeout: No response received\n"); + } else { + perror("[CLIENT] Error receiving response"); + } + close(request_sock); + close(announcement_sock); + return 1; + } + + char response_ip[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &response_addr.sin_addr, response_ip, sizeof(response_ip)); + printf("[CLIENT] Received response: %zd bytes from %s:%d\n", + received, response_ip, ntohs(response_addr.sin_port)); + + // Parse the response + if (!parse_doip_header(buffer, received, &payload_type, &payload_length)) { + printf("[CLIENT] Failed to parse response header\n"); + close(request_sock); + close(announcement_sock); + return 1; + } + + if (payload_type == VEHICLE_IDENTIFICATION_RESPONSE) { + VehicleInfo response_info; + memset(&response_info, 0, sizeof(response_info)); + + if (parse_vehicle_identification_response(buffer, received, &response_info)) { + strncpy(response_info.ip_address, response_ip, sizeof(response_info.ip_address)); + response_info.port = ntohs(response_addr.sin_port); + + printf("[CLIENT] Vehicle Identification Response received:\n"); + print_vehicle_info(&response_info); + } + } + + // Cleanup + close(request_sock); + close(announcement_sock); + + printf("[CLIENT] Discovery complete\n"); + return 0; +} diff --git a/doc/udp/doip_server.c b/doc/udp/doip_server.c new file mode 100644 index 0000000..157c506 --- /dev/null +++ b/doc/udp/doip_server.c @@ -0,0 +1,285 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define DOIP_UDP_DISCOVERY_PORT 13400 +#define DOIP_UDP_TEST_EQUIPMENT_PORT 13401 +#define DOIP_PROTOCOL_VERSION 0x04 +#define DOIP_INVERSE_PROTOCOL_VERSION 0xFB + +// DoIP Payload Types +#define VEHICLE_IDENTIFICATION_REQUEST 0x0001 +#define VEHICLE_IDENTIFICATION_RESPONSE 0x0004 + +// Server configuration +typedef struct { + char vin[17]; + uint16_t logical_address; + uint8_t eid[6]; + uint8_t gid[6]; + bool use_loopback; +} ServerConfig; + +// Global server state +static bool server_running = true; +static int udp_sock = -1; + +// Create DoIP header +void create_doip_header(uint8_t *buffer, uint16_t payload_type, uint32_t payload_length) { + buffer[0] = DOIP_PROTOCOL_VERSION; + buffer[1] = DOIP_INVERSE_PROTOCOL_VERSION; + buffer[2] = (payload_type >> 8) & 0xFF; + buffer[3] = payload_type & 0xFF; + buffer[4] = (payload_length >> 24) & 0xFF; + buffer[5] = (payload_length >> 16) & 0xFF; + buffer[6] = (payload_length >> 8) & 0xFF; + buffer[7] = payload_length & 0xFF; +} + +// Create Vehicle Identification Response +int create_vehicle_identification_response(uint8_t *buffer, const ServerConfig *config) { + uint32_t payload_length = 33; // VIN(17) + LogAddr(2) + EID(6) + GID(6) + FAR(1) + VIN/GID_sync(1) + + create_doip_header(buffer, VEHICLE_IDENTIFICATION_RESPONSE, payload_length); + + int offset = 8; + + // VIN (17 bytes) + memcpy(buffer + offset, config->vin, 17); + offset += 17; + + // Logical Address (2 bytes) + buffer[offset++] = (config->logical_address >> 8) & 0xFF; + buffer[offset++] = config->logical_address & 0xFF; + + // EID (6 bytes) + memcpy(buffer + offset, config->eid, 6); + offset += 6; + + // GID (6 bytes) + memcpy(buffer + offset, config->gid, 6); + offset += 6; + + // Further Action Required (1 byte) + buffer[offset++] = 0x00; + + // VIN/GID sync status (1 byte) - optional, set to 0x00 + buffer[offset++] = 0x00; + + return offset; // Total message length +} + +// Parse DoIP header +bool parse_doip_header(const uint8_t *buffer, size_t length, uint16_t *payload_type, uint32_t *payload_length) { + if (length < 8) { + return false; + } + + if (buffer[0] != DOIP_PROTOCOL_VERSION || buffer[1] != DOIP_INVERSE_PROTOCOL_VERSION) { + printf("Invalid DoIP protocol version\n"); + return false; + } + + *payload_type = (buffer[2] << 8) | buffer[3]; + *payload_length = (buffer[4] << 24) | (buffer[5] << 16) | (buffer[6] << 8) | buffer[7]; + + return true; +} + +// Send Vehicle Announcement +void send_vehicle_announcement(const ServerConfig *config) { + uint8_t buffer[256]; + int msg_len = create_vehicle_identification_response(buffer, config); + + struct sockaddr_in dest_addr; + memset(&dest_addr, 0, sizeof(dest_addr)); + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(DOIP_UDP_TEST_EQUIPMENT_PORT); + + const char *dest_ip; + if (config->use_loopback) { + dest_ip = "127.0.0.1"; + inet_pton(AF_INET, dest_ip, &dest_addr.sin_addr); + } else { + dest_ip = "255.255.255.255"; + dest_addr.sin_addr.s_addr = htonl(INADDR_BROADCAST); + + // Enable broadcast + int broadcast = 1; + setsockopt(udp_sock, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); + } + + ssize_t sent = sendto(udp_sock, buffer, msg_len, 0, + (struct sockaddr *)&dest_addr, sizeof(dest_addr)); + + if (sent > 0) { + printf("[SERVER] Sent Vehicle Announcement: %zd bytes to %s:%d\n", + sent, dest_ip, DOIP_UDP_TEST_EQUIPMENT_PORT); + } else { + perror("[SERVER] Failed to send announcement"); + } +} + +// UDP Listener Thread +void *udp_listener_thread(void *arg) { + ServerConfig *config = (ServerConfig *)arg; + uint8_t buffer[512]; + struct sockaddr_in client_addr; + socklen_t client_len = sizeof(client_addr); + + printf("[SERVER] UDP listener thread started\n"); + + while (server_running) { + ssize_t received = recvfrom(udp_sock, buffer, sizeof(buffer), 0, + (struct sockaddr *)&client_addr, &client_len); + + if (received < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { + // Timeout, continue + continue; + } + if (server_running) { + perror("[SERVER] recvfrom error"); + } + break; + } + + if (received > 0) { + char client_ip[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &client_addr.sin_addr, client_ip, sizeof(client_ip)); + printf("[SERVER] Received %zd bytes from %s:%d\n", + received, client_ip, ntohs(client_addr.sin_port)); + + uint16_t payload_type; + uint32_t payload_length; + + if (parse_doip_header(buffer, received, &payload_type, &payload_length)) { + printf("[SERVER] Payload Type: 0x%04X\n", payload_type); + + if (payload_type == VEHICLE_IDENTIFICATION_REQUEST) { + printf("[SERVER] Vehicle Identification Request received\n"); + + // Send response back to client + uint8_t response[256]; + int resp_len = create_vehicle_identification_response(response, config); + + ssize_t sent = sendto(udp_sock, response, resp_len, 0, + (struct sockaddr *)&client_addr, client_len); + + if (sent > 0) { + printf("[SERVER] Sent Vehicle Identification Response: %zd bytes to %s:%d\n", + sent, client_ip, ntohs(client_addr.sin_port)); + } else { + perror("[SERVER] Failed to send response"); + } + } + } + } + } + + printf("[SERVER] UDP listener thread stopped\n"); + return NULL; +} + +// Announcement Thread +void *announcement_thread(void *arg) { + ServerConfig *config = (ServerConfig *)arg; + + printf("[SERVER] Announcement thread started\n"); + + // Send 5 announcements with 2 second interval + for (int i = 0; i < 5 && server_running; i++) { + send_vehicle_announcement(config); + sleep(2); + } + + printf("[SERVER] Announcement thread stopped\n"); + return NULL; +} + +int main(int argc, char *argv[]) { + bool use_loopback = false; + + // Parse command line arguments + for (int i = 1; i < argc; i++) { + if (strcmp(argv[i], "--loopback") == 0) { + use_loopback = true; + } + } + + // Configure server + ServerConfig config = { + .vin = "EXAMPLESERVER0000", + .logical_address = 0x0028, + .eid = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, + .gid = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, + .use_loopback = use_loopback + }; + + printf("[SERVER] Starting DoIP Server\n"); + printf("[SERVER] Mode: %s\n", use_loopback ? "Loopback" : "Broadcast"); + printf("[SERVER] VIN: %s\n", config.vin); + printf("[SERVER] Logical Address: 0x%04X\n", config.logical_address); + + // Create UDP socket + udp_sock = socket(AF_INET, SOCK_DGRAM, 0); + if (udp_sock < 0) { + perror("Failed to create socket"); + return 1; + } + + // Set socket to non-blocking with timeout + struct timeval timeout; + timeout.tv_sec = 1; + timeout.tv_usec = 0; + setsockopt(udp_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + + // Enable SO_REUSEADDR + int reuse = 1; + setsockopt(udp_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + + // Bind socket to port 13400 + struct sockaddr_in server_addr; + memset(&server_addr, 0, sizeof(server_addr)); + server_addr.sin_family = AF_INET; + server_addr.sin_addr.s_addr = htonl(INADDR_ANY); + server_addr.sin_port = htons(DOIP_UDP_DISCOVERY_PORT); + + if (bind(udp_sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) { + perror("Failed to bind socket"); + close(udp_sock); + return 1; + } + + printf("[SERVER] Socket bound to 0.0.0.0:%d\n", DOIP_UDP_DISCOVERY_PORT); + + // Start threads + pthread_t listener_tid, announcement_tid; + + pthread_create(&listener_tid, NULL, udp_listener_thread, &config); + pthread_create(&announcement_tid, NULL, announcement_thread, &config); + + // Wait for announcement thread to complete + pthread_join(announcement_tid, NULL); + + // Keep server running for a bit to handle requests + printf("[SERVER] Announcements complete, waiting for requests...\n"); + sleep(10); + + // Shutdown + printf("[SERVER] Shutting down...\n"); + server_running = false; + + pthread_join(listener_tid, NULL); + + close(udp_sock); + + printf("[SERVER] Server stopped\n"); + return 0; +} diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 4545639..45cf336 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,40 +1,54 @@ # Examples CMakeLists.txt -# DoIP Server Example -add_executable(exampleDoIPServer exampleDoIPServer.cpp) -target_link_libraries(exampleDoIPServer - PRIVATE - ${DOIP_NAME} - Threads::Threads +set (EXAMPLE_SOURCES + exampleDoIPServer.cpp + exampleDoIPClient.cpp + exampleDoIPDiscover.cpp ) -# Set properties for the example -set_target_properties(exampleDoIPServer PROPERTIES - CXX_STANDARD 17 - CXX_STANDARD_REQUIRED ON - CXX_EXTENSIONS OFF -) +foreach(example_source ${EXAMPLE_SOURCES}) + get_filename_component(example_name ${example_source} NAME_WE) + add_executable(${example_name} ${example_source}) + target_link_libraries(${example_name} + PRIVATE + ${DOIP_NAME} + Threads::Threads + ) -# DoIP Client Example -add_executable(exampleDoIPClient exampleDoIPClient.cpp) -target_link_libraries(exampleDoIPClient - PRIVATE - ${DOIP_NAME} -) + # Set properties for the example + set_target_properties(${example_name} PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS OFF + ) -# Set properties for the client example -set_target_properties(exampleDoIPClient PROPERTIES - CXX_STANDARD 17 - CXX_STANDARD_REQUIRED ON - CXX_EXTENSIONS OFF -) + # Disable switch-default warning for examples using spdlog + target_compile_options(${example_name} PRIVATE -Wno-switch-default) + + # Install examples (optional) + install(TARGETS ${example_name} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/examples + ) + +endforeach() -# Disable switch-default warning for examples using spdlog -target_compile_options(exampleDoIPServer PRIVATE -Wno-switch-default) -target_compile_options(exampleDoIPClient PRIVATE -Wno-switch-default) +if (WITH_UNIT_TEST) + enable_testing() + # Integration fixture: start exampleDoIPServer as a daemon + add_test(NAME Integration_StartExampleServer + COMMAND $ --daemonize --loopback + ) + set_tests_properties(Integration_StartExampleServer PROPERTIES FIXTURES_SETUP example_server TIMEOUT 20) + # Integration test: run discover against the running server + add_test(NAME Integration_DiscoverRuns + COMMAND $ --loopback + ) + set_tests_properties(Integration_DiscoverRuns PROPERTIES FIXTURES_REQUIRED example_server TIMEOUT 20) -# Install examples (optional) -install(TARGETS exampleDoIPServer exampleDoIPClient - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}/examples -) \ No newline at end of file + # Teardown fixture: stop exampleDoIPServer + add_test(NAME Integration_StopExampleServer + COMMAND pkill -f exampleDoIPServer + ) + set_tests_properties(Integration_StopExampleServer PROPERTIES FIXTURES_CLEANUP example_server TIMEOUT 10) +endif() diff --git a/examples/exampleDoIPClient.cpp b/examples/exampleDoIPClient.cpp index 60103e7..fe18cdf 100644 --- a/examples/exampleDoIPClient.cpp +++ b/examples/exampleDoIPClient.cpp @@ -51,7 +51,10 @@ int main(int argc, char *argv[]) { // Listen for Vehicle Announcements first LOG_DOIP_INFO("Listening for Vehicle Announcements..."); - client.receiveVehicleAnnouncement(); + if (!client.receiveVehicleAnnouncement()) { + LOG_DOIP_WARN("No Vehicle Announcement received"); + return EXIT_FAILURE; + } // Send Vehicle Identification Request to configured address if (client.sendVehicleIdentificationRequest(serverAddress.c_str()) > 0) { @@ -59,6 +62,8 @@ int main(int argc, char *argv[]) { client.receiveUdpMessage(); } + client.closeUdpConnection(); + // Now start TCP connection for diagnostic communication LOG_DOIP_INFO("Starting TCP connection for diagnostic messages"); client.startTcpConnection(); @@ -84,5 +89,5 @@ int main(int argc, char *argv[]) { std::this_thread::sleep_for(2s); client.closeTcpConnection(); - client.closeUdpConnection(); + //client.closeUdpConnection(); } diff --git a/examples/exampleDoIPDiscover.cpp b/examples/exampleDoIPDiscover.cpp new file mode 100644 index 0000000..c786657 --- /dev/null +++ b/examples/exampleDoIPDiscover.cpp @@ -0,0 +1,71 @@ +#include "DoIPClient.h" +#include "DoIPMessage.h" +#include "Logger.h" + +#include +#include +#include + +using namespace doip; +using namespace std; + +DoIPClient client; + +static void printUsage(const char *progName) { + cout << "Usage: " << progName << " [OPTIONS]\n"; + cout << "Options:\n"; + cout << " --loopback Use loopback (127.0.0.1) instead of multicast\n"; + cout << " --server Connect to specific server IP\n"; + cout << " --help Show this help message\n"; +} + +int main(int argc, char *argv[]) { + std::cerr << "The client code does not work currently - use at your own risk!\n"; + + string serverAddress = "224.0.0.2"; // Default multicast address + + // Parse command line arguments + for (int i = 1; i < argc; i++) { + string arg = argv[i]; + if (arg == "--loopback") { + serverAddress = "127.0.0.1"; + LOG_DOIP_INFO("Loopback mode enabled - using 127.0.0.1"); + } else if (arg == "--server" && i + 1 < argc) { + serverAddress = argv[++i]; + LOG_DOIP_INFO("Using custom server address: {}", serverAddress); + } else if (arg == "--help") { + printUsage(argv[0]); + return 0; + } else { + cout << "Unknown argument: " << arg << endl; + printUsage(argv[0]); + return 1; + } + } + + LOG_DOIP_INFO("Starting DoIP Client"); + + // Start UDP connections (don't start TCP yet) + client.startUdpConnection(); + client.startAnnouncementListener(); // Listen for Vehicle Announcements on port 13401 + + // Listen for Vehicle Announcements first + LOG_DOIP_INFO("Listening for Vehicle Announcements..."); + if (!client.receiveVehicleAnnouncement()) { + LOG_DOIP_WARN("No Vehicle Announcement received"); + return EXIT_FAILURE; + } + + client.printVehicleInformationResponse(); + + // Send Vehicle Identification Request to configured address + if (client.sendVehicleIdentificationRequest(serverAddress.c_str()) > 0) { + LOG_DOIP_INFO("Vehicle Identification Request sent successfully"); + client.receiveUdpMessage(); + } + + // Now start TCP connection for diagnostic communication + LOG_DOIP_INFO("Discovery complete, closing UDP connections"); + client.closeUdpConnection(); + return 0; +} diff --git a/examples/exampleDoIPServer.cpp b/examples/exampleDoIPServer.cpp index 24c8a8a..eb782d5 100644 --- a/examples/exampleDoIPServer.cpp +++ b/examples/exampleDoIPServer.cpp @@ -16,28 +16,13 @@ using namespace std; static const DoIPAddress LOGICAL_ADDRESS(0x0028); -DoIPServer server; +std::unique_ptr server; std::vector doipReceiver; bool serverActive = false; std::unique_ptr tcpConnection(nullptr); -void listenUdp(); void listenTcp(); -/* - * Check permantly if udp message was received - */ -void listenUdp() { - LOG_UDP_INFO("UDP listener thread started"); - while (serverActive) { - ssize_t result = server.receiveUdpMessage(); - // If timeout (result == 0), sleep briefly to prevent CPU spinning - if (result == 0) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - } -} - /* * Check permantly if tcp message was received */ @@ -45,7 +30,7 @@ void listenTcp() { LOG_UDP_INFO("TCP listener thread started"); while (true) { - tcpConnection = server.waitForTcpConnection(); + tcpConnection = server->waitForTcpConnection(); while (tcpConnection->isSocketActive()) { tcpConnection->receiveTcpMessage(); @@ -53,39 +38,48 @@ void listenTcp() { } } -static void ConfigureDoipServer() { - // VIN needs to have a fixed length of 17 bytes. - // Shorter VINs will be padded with '0' - server.setVIN("EXAMPLESERVER"); - server.setLogicalGatewayAddress(LOGICAL_ADDRESS.toUint16()); - server.setGID(0); - server.setFAR(DoIPFurtherAction::NoFurtherAction); - server.setEID(0); - - // be more relaxed for testing purposes - server.setAnnounceInterval(2000); - server.setAnnounceNum(10); - - // doipserver->setAnnounceNum(tempNum); - // doipserver->setAnnounceInterval(tempInterval); -} +// Default example settings are applied in main when building ServerConfig static void printUsage(const char *progName) { cout << "Usage: " << progName << " [OPTIONS]\n"; cout << "Options:\n"; cout << " --loopback Use loopback (127.0.0.1) for announcements instead of broadcast\n"; + cout << " --daemonize / --no-daemonize Enable or disable daemon mode (default: enabled)\n"; + cout << " --eid <6chars> Set EID (6 ASCII chars)\n"; + cout << " --gid <6chars> Set GID (6 ASCII chars)\n"; + cout << " --vin <17chars> Set VIN (17 ASCII chars)\n"; + cout << " --logical-address Set logical gateway address (default: 0x0E00)\n"; cout << " --help Show this help message\n"; } int main(int argc, char *argv[]) { bool useLoopback = false; + bool daemonize = false; + std::string eid_str; + std::string gid_str; + std::string vin_str = "EXAMPLESERVER"; + std::string logical_addr_str; // Parse command line arguments - for (int i = 1; i < argc; i++) { + for (int i = 1; i < argc; ++i) { string arg = argv[i]; if (arg == "--loopback") { useLoopback = true; LOG_DOIP_INFO("Loopback mode enabled"); + } else if (arg == "--daemonize") { + daemonize = true; + LOG_DOIP_INFO("Daemonize enabled"); + } else if (arg == "--no-daemonize") { + daemonize = false; + LOG_DOIP_INFO("Daemonize disabled"); + } else if (arg == "--eid" && i + 1 < argc) { + eid_str = argv[++i]; + } else if (arg == "--gid" && i + 1 < argc) { + gid_str = argv[++i]; + } else if (arg == "--vin" && i + 1 < argc) { + vin_str = argv[++i]; + } else if (arg == "--logical-address" && i + 1 < argc) { + logical_addr_str = argv[++i]; } else if (arg == "--help") { printUsage(argv[0]); return 0; @@ -100,33 +94,58 @@ int main(int argc, char *argv[]) { doip::Logger::setLevel(spdlog::level::debug); LOG_DOIP_INFO("Starting DoIP Server Example"); - ConfigureDoipServer(); - - // Configure server based on mode - if (useLoopback) { - server.setAnnouncementMode(true); + // Build server config from example settings and CLI + doip::ServerConfig cfg; + cfg.loopback = useLoopback; + cfg.daemonize = daemonize; + // TODO: Use CLI11 or similar for argument parsing + if (!vin_str.empty()) cfg.vin = DoIpVin(vin_str); + if (!eid_str.empty()) cfg.eid = DoIpEid(eid_str); + if (!gid_str.empty()) cfg.gid = DoIpGid(gid_str); + if (!logical_addr_str.empty()) { + // parse hex (0x...) or decimal + try { + size_t pos = 0; + unsigned long val = 0; + if (logical_addr_str.rfind("0x", 0) == 0 || logical_addr_str.rfind("0X", 0) == 0) { + val = std::stoul(logical_addr_str, &pos, 16); + } else { + val = std::stoul(logical_addr_str, &pos, 0); + } + cfg.logicalAddress = DoIPAddress(static_cast(val & 0xFFFF)); + } catch (...) { + LOG_DOIP_WARN("Failed to parse logical address '{}', using default 0x0E00", logical_addr_str); + } + } else { + cfg.logicalAddress = LOGICAL_ADDRESS; } - if (!server.setupUdpSocket()) { + server = std::make_unique(cfg); + // Apply defaults used previously in example + server->setFurtherActionRequired(DoIPFurtherAction::NoFurtherAction); + server->setAnnounceInterval(2000); + server->setAnnounceNum(10); + + if (!server->setupUdpSocket()) { LOG_DOIP_CRITICAL("Failed to set up UDP socket"); return 1; } - if (!server.setupTcpSocket()) { + if (!server->setupTcpSocket()) { LOG_DOIP_CRITICAL("Failed to set up TCP socket"); return 1; } serverActive = true; - LOG_DOIP_INFO("Starting UDP and TCP listener threads"); - doipReceiver.push_back(thread(&listenUdp)); - doipReceiver.push_back(thread(&listenTcp)); - server.sendVehicleAnnouncement(); - LOG_DOIP_INFO("Vehicle announcement sent"); + // TODO:: Add signal handler + while(server->isRunning()) { + sleep(1); + } + doipReceiver.push_back(thread(&listenTcp)); + LOG_DOIP_INFO("Starting TCP listener threads"); doipReceiver.at(0).join(); - doipReceiver.at(1).join(); LOG_DOIP_INFO("DoIP Server Example terminated"); return 0; } diff --git a/inc/DoIPClient.h b/inc/DoIPClient.h index a82f8e8..ec651ab 100644 --- a/inc/DoIPClient.h +++ b/inc/DoIPClient.h @@ -22,6 +22,8 @@ using DoIPRequest = std::pair; class DoIPClient { public: + DoIPClient() {m_receiveBuf.reserve(DOIP_MAXIMUM_MTU);} + void startTcpConnection(); void startUdpConnection(); void startAnnouncementListener(); @@ -29,7 +31,8 @@ class DoIPClient { ssize_t sendVehicleIdentificationRequest(const char *inet_address); void receiveRoutingActivationResponse(); void receiveUdpMessage(); - void receiveVehicleAnnouncement(); + [[nodiscard]] + bool receiveVehicleAnnouncement(); /* * Send the builded request over the tcp-connection to server */ @@ -46,7 +49,7 @@ class DoIPClient { */ ssize_t sendAliveCheckResponse(); void setSourceAddress(const DoIPAddress &address); - void displayVIResponseInformation(); + void printVehicleInformationResponse(); void closeTcpConnection(); void closeUdpConnection(); void reconnectServer(); @@ -55,19 +58,19 @@ class DoIPClient { int getConnected(); private: - uint8_t _receivedData[_maxDataSize] = {0}; - int _sockFd{-1}, _sockFd_udp{-1}, _sockFd_announcement{-1}, _connected{-1}; + ByteArray m_receiveBuf; + int m_tcpSocket{-1}, m_udpSocket{-1}, m_udpAnnouncementSocket{-1}, m_connected{-1}; int m_broadcast = 1; - struct sockaddr_in _serverAddr, _clientAddr, _announcementAddr; + struct sockaddr_in m_serverAddress, m_clientAddress, m_announcementAddress; DoIPAddress m_sourceAddress = DoIPAddress(0xE000); - uint8_t VINResult[17] = {0}; + DoIpVin m_vin{0}; DoIPAddress m_logicalAddress = DoIPAddress::ZeroAddress; - uint8_t EIDResult[6] = {0}; - uint8_t GIDResult[6] = {0}; - uint8_t FurtherActionReqResult = 0x00; + DoIpEid m_eid{0}; + DoIpGid m_gid{0}; + DoIPFurtherAction m_furtherActionReqResult = DoIPFurtherAction::NoFurtherAction; - void parseVIResponseInformation(const uint8_t *data); + void parseVehicleIdentificationResponse(const DoIPMessage& msg); int emptyMessageCounter = 0; }; diff --git a/inc/DoIPConnection.h b/inc/DoIPConnection.h index 73ce0b0..3a3a187 100644 --- a/inc/DoIPConnection.h +++ b/inc/DoIPConnection.h @@ -2,6 +2,7 @@ #define DOIPCONNECTION_H +#include "DoIPConfig.h" #include "DoIPMessage.h" #include "DoIPNegativeAck.h" #include "DoIPNegativeDiagnosticAck.h" @@ -20,8 +21,6 @@ namespace doip { -/** Maximum size of the ISO-TP message - used as initial value for RX buffer to avoid reallocs */ -constexpr size_t MAX_ISOTP_MTU = 4095; class DoIPConnection : public DoIPDefaultConnection { public: @@ -100,11 +99,11 @@ class DoIPConnection : public DoIPDefaultConnection { bool hasDownstreamHandler() const override; private: - DoIPAddress m_gatewayAddress; + DoIPAddress m_logicalAddress; // TCP socket-specific members int m_tcpSocket; - std::array m_receiveBuf{}; + std::array m_receiveBuf{}; bool m_isClosing{false}; // TODO: Guard against recursive closeConnection calls -> solve this std::optional m_pendingDownstreamRequest; diff --git a/inc/DoIPFurtherAction.h b/inc/DoIPFurtherAction.h index cb93ff5..04689bb 100644 --- a/inc/DoIPFurtherAction.h +++ b/inc/DoIPFurtherAction.h @@ -11,6 +11,23 @@ namespace doip { RoutingActivationForCentralSecurity = 0x10, // 0x11 to 0xFE: reserved for VM manufacturer specific use }; + + inline std::ostream &operator<<(std::ostream &os, const DoIPFurtherAction far) { + switch (far) { + case DoIPFurtherAction::NoFurtherAction: + os << "None"; + break; + case DoIPFurtherAction::RoutingActivationForCentralSecurity: + os << "Routing Activation for Central Security Required"; + break; + default: + os << "Reserved Further Action Code: 0x" << std::hex << static_cast(far) << std::dec; + break; + } + + return os; + } + } // namespace doip #endif /* DOIPFURTHERACTION_H */ diff --git a/inc/DoIPIdentifiers.h b/inc/DoIPIdentifiers.h index 905f90c..7db4ed4 100644 --- a/inc/DoIPIdentifiers.h +++ b/inc/DoIPIdentifiers.h @@ -346,17 +346,42 @@ inline const GenericFixedId GenericFixedId; +using DoIpVin = GenericFixedId<17, true, '0'>; /** * @brief Entity Identifier (EID) - 6 bytes for unique entity identification */ -using DoIPEID = GenericFixedId<6, false>; +using DoIpEid = GenericFixedId<6, false>; /** * @brief Group Identifier (GID) - 6 bytes for group identification */ -using DoIPGID = GenericFixedId<6, false>; +using DoIpGid = GenericFixedId<6, false>; + +/** + * @brief Stream output operator for DoIpVin, DoIpEid, and DoIpGid + * + * @param os the operation stream + * @param vin the DoIpVin to output + * @return std::ostream& the operation stream + */ +inline std::ostream &operator<<(std::ostream &os, const DoIpVin &vin) { + os << vin.toString(); + return os; +} + +/** + * @brief Stream output operator for DoIpEid/DoIpGid + * + * @param os the operation stream + * @param eid the DoIpEid/DoIpGid to output + * @return std::ostream& @ref {type} ["{type}"] // @return Returns @c true in the case of success, @c false otherwise. + */ +inline std::ostream &operator<<(std::ostream &os, const DoIpEid &eid) { + os << eid.toHexString(); + return os; +} + } // namespace doip diff --git a/inc/DoIPMessage.h b/inc/DoIPMessage.h index 8271a58..046c0a7 100644 --- a/inc/DoIPMessage.h +++ b/inc/DoIPMessage.h @@ -250,6 +250,22 @@ class DoIPMessage { return m_data; } + /** + * @brief Check if the message has a Source Address field. + * + * @return Returns @c true in the case of success, @c false otherwise. + */ + bool hasSourceAddress() const { + auto payloadRef = getPayload(); + auto plType = getPayloadType(); + bool result = plType == DoIPPayloadType::DiagnosticMessage || + plType == DoIPPayloadType::RoutingActivationRequest || + plType == DoIPPayloadType::RoutingActivationResponse || + plType == DoIPPayloadType::AliveCheckResponse; + + return result && payloadRef.second >= 2; + } + /** * @brief Get the Source Address of the message (if message is a Diagnostic Message). * @@ -258,17 +274,21 @@ class DoIPMessage { std::optional getSourceAddress() const { auto payloadRef = getPayload(); // todo: Simplify - if (getPayloadType() == DoIPPayloadType::DiagnosticMessage && payloadRef.second >= 2) { + if (hasSourceAddress()) { return DoIPAddress(payloadRef.first, 0); } - if (getPayloadType() == DoIPPayloadType::RoutingActivationRequest && payloadRef.second >= 2) { - return DoIPAddress(payloadRef.first, 0); - } - if (getPayloadType() == DoIPPayloadType::RoutingActivationResponse && payloadRef.second >= 2) { - return DoIPAddress(payloadRef.first, 0); - } - if (getPayloadType() == DoIPPayloadType::AliveCheckResponse && payloadRef.second >= 2) { - return DoIPAddress(payloadRef.first, 0); + return std::nullopt; + } + + /** + * @brief Get the Logical Address of the message (if message is a Vehicle Identification Response). + * + * @return std::optional The logical address if present, std::nullopt otherwise + */ + std::optional getLogicalAddress() const { + auto payloadRef = getPayload(); + if (getPayloadType() == DoIPPayloadType::VehicleIdentificationResponse && payloadRef.second >= 19) { + return DoIPAddress(payloadRef.first + 17); } return std::nullopt; } @@ -286,6 +306,58 @@ class DoIPMessage { return std::nullopt; } + /** + * @brief Get the vehicle identification number (VIN) if message is a Vehicle Identification Response. + * + * @return std::optional The VIN if present, std::nullopt otherwise + */ + std::optional getVin() const { + auto payloadRef = getPayload(); + if (getPayloadType() == DoIPPayloadType::VehicleIdentificationResponse && payloadRef.second >= 17) { + return DoIpVin(payloadRef.first, 17); + } + return std::nullopt; + } + + /** + * @brief Get the entity id (EID) if message is a Vehicle Identification Response. + * + * @return std::optional The VIN if present, std::nullopt otherwise + */ + std::optional getEid() const { + auto payloadRef = getPayload(); + if (getPayloadType() == DoIPPayloadType::VehicleIdentificationResponse && payloadRef.second >= 25) { + return DoIpEid(payloadRef.first + 19, 6); + } + return std::nullopt; + } + + /** + * @brief Get the group id (GID) if message is a Vehicle Identification Response. + * + * @return std::optional The VIN if present, std::nullopt otherwise + */ + std::optional getGid() const { + auto payloadRef = getPayload(); + if (getPayloadType() == DoIPPayloadType::VehicleIdentificationResponse && payloadRef.second >= 31) { + return DoIpGid(payloadRef.first + 25, 6); + } + return std::nullopt; + } + + /** + * @brief Get the Further Action Request object if message is a Vehicle Identification Response. + * + * @return std::optional The Further Action Request if present, std::nullopt otherwise + */ + std::optional getFurtherActionRequest() const { + auto payloadRef = getPayload(); + if (getPayloadType() == DoIPPayloadType::VehicleIdentificationResponse && payloadRef.second >= 31) { + return DoIPFurtherAction(payloadRef.first[31]); + } + return std::nullopt; + } + /** * @brief Checks if the message is valid. * @@ -454,10 +526,10 @@ inline DoIPMessage makeVehicleIdentificationRequest() { * @return DoIPMessage the vehicle identification response message */ inline DoIPMessage makeVehicleIdentificationResponse( - const DoIPVIN &vin, + const DoIpVin &vin, const DoIPAddress &logicalAddress, - const DoIPEID &entityType, - const DoIPGID &groupId, + const DoIpEid &entityType, + const DoIpGid &groupId, DoIPFurtherAction furtherAction = DoIPFurtherAction::NoFurtherAction, DoIPSyncStatus syncStatus = DoIPSyncStatus::GidVinSynchronized) { diff --git a/inc/DoIPServer.h b/inc/DoIPServer.h index 890b93a..d7985ff 100644 --- a/inc/DoIPServer.h +++ b/inc/DoIPServer.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -17,8 +18,8 @@ #include #include "ByteArray.h" -#include "DoIPConnection.h" #include "DoIPConfig.h" +#include "DoIPConnection.h" #include "DoIPFurtherAction.h" #include "DoIPIdentifiers.h" #include "DoIPNegativeAck.h" @@ -27,7 +28,33 @@ namespace doip { -constexpr int DOIP_SERVER_PORT = 13400; +/** + * @brief Server configuration structure used to initialize a DoIP server. + */ +struct ServerConfig { + // EID and GID as fixed identifiers (6 bytes). Default: zeros. + DoIpEid eid = DoIpEid::Zero; + DoIpGid gid = DoIpGid::Zero; + + // VIN as fixed identifier (17 bytes). Default: zeros. + DoIpVin vin = DoIpVin::Zero; + + // Logical/server address (default 0x0E00) + DoIPAddress logicalAddress = DoIPAddress(0x0028); + + // Use loopback announcements instead of broadcast + bool loopback = false; + + // Run the server as a daemon by default + bool daemonize = false; + + int announceCount = 3; // Default Value = 3 + unsigned int announceInterval = 500; // Default Value = 500ms +}; + +const ServerConfig DefaultServerConfig{}; + +constexpr int DOIP_SERVER_TCP_PORT = 13400; /** * @brief Callback invoked when a new TCP connection is established @@ -38,19 +65,20 @@ using ConnectionAcceptedHandler = std::function(D /** * @brief DoIP Server class to handle incoming DoIP connections and UDP messages. * - * This class manages the low-level TCP/UDP socket handling and provides a clean - * callback-based interface for application logic. The server can operate in two modes: - * - * 1. Manual mode: User calls waitForTcpConnection() and manages the receive loop - * 2. Automatic mode: User calls start() with a callback, server manages everything + * This class manages the low-level TCP/UDP socket handling. */ class DoIPServer { public: - DoIPServer() { - m_receiveBuf.reserve(MAX_ISOTP_MTU); - }; + /** + * @brief Construct a DoIP server with the given configuration. + * @param config Server configuration (EID/GID/VIN, announce params, etc.). + */ + explicit DoIPServer(const ServerConfig &config = DefaultServerConfig); + /** + * @brief Destructor. Ensures sockets/threads are closed/stopped. + */ ~DoIPServer(); DoIPServer(const DoIPServer &) = delete; @@ -58,57 +86,64 @@ class DoIPServer { DoIPServer(DoIPServer &&) = delete; DoIPServer &operator=(DoIPServer &&) = delete; - // === Low-level API (manual mode) === [[nodiscard]] + /** + * @brief Initialize and bind the TCP socket for DoIP. + * @return true on success, false otherwise. + */ bool setupTcpSocket(); template - std::unique_ptr waitForTcpConnection(); - - [[nodiscard]] - bool setupUdpSocket(); - ssize_t receiveUdpMessage(); - - // === High-level API (automatic mode) === /** - * @brief Start the DoIP server with automatic connection handling - * - * This method sets up TCP and UDP sockets, spawns background threads to handle - * UDP messages and TCP connections, and returns immediately. The server continues - * running until stop() is called. - * - * @param onConnectionAccepted Callback invoked when a new client connects. - * Return a DoIPServerModel to accept the connection, - * or std::nullopt to reject it. - * @param sendAnnouncements If true, send vehicle announcements on startup - * @return true if server started successfully, false otherwise + * @brief Block until a TCP client connects and create a DoIP connection. + * @tparam Model Server model type used by the connection (default `DefaultDoIPServerModel`). + * @return Unique pointer to established `DoIPConnection`, or nullptr on failure. */ - template - bool start(ConnectionAcceptedHandler onConnectionAccepted, bool sendAnnouncements = true); + std::unique_ptr waitForTcpConnection(); + [[nodiscard]] /** - * @brief Stop the DoIP server and wait for all threads to terminate - * - * This gracefully shuts down all active connections and background threads. - * Blocks until all cleanup is complete. + * @brief Initialize and bind the UDP socket for announcements and UDP messages. + * @return true on success, false otherwise. */ - void stop(); + bool setupUdpSocket(); /** * @brief Check if the server is currently running */ + [[nodiscard]] bool isRunning() const { return m_running.load(); } + /** + * @brief Set the number of vehicle announcements to send. + * @param Num Count of announcements. + */ void setAnnounceNum(int Num); + /** + * @brief Set the interval between announcements in milliseconds. + * @param Interval Interval in ms. + */ void setAnnounceInterval(unsigned int Interval); - void setAnnouncementMode(bool useLoopback); + /** + * @brief Enable/disable loopback mode for announcements (no broadcast). + * @param useLoopback True to use loopback, false for broadcast. + */ + void setLoopbackMode(bool useLoopback); + /** + * @brief Close the TCP socket if open. + */ void closeTcpSocket(); + /** + * @brief Close the UDP socket if open. + */ void closeUdpSocket(); - void setLogicalGatewayAddress(const unsigned short inputLogAdd); - - ssize_t sendVehicleAnnouncement(); + /** + * @brief Set the logical DoIP gateway address. + * @param logicalAddress Logical address value. + */ + void setLogicalGatewayAddress(unsigned short logicalAddress); /** * @brief Sets the EID to a default value based on the MAC address. @@ -116,58 +151,103 @@ class DoIPServer { * @return true if the EID was successfully set to the default value. * @return false if the default EID could not be set. */ - bool setEIDdefault(); + bool setDefaultEid(); - void setVIN(const std::string &VINString); - const DoIPVIN &getVIN() const { return m_VIN; } + /** + * @brief Set VIN from a 17-character string. + * @param VINString VIN string (17 bytes expected). + */ + void setVin(const std::string &VINString); + /** + * @brief Set VIN from a `DoIpVin` instance. + * @param vin VIN value. + */ + void setVin(const DoIpVin &vin); + /** + * @brief Get current VIN. + * @return Reference to configured VIN. + */ + const DoIpVin &getVin() const { return m_config.vin; } - void setEID(const uint64_t inputEID); - const DoIPEID &getEID() const { return m_EID; } + /** + * @brief Set EID value. + * @param nputEID EID as 64-bit value (lower 48 bits used). + */ + void setEid(uint64_t nputEID); + /** + * @brief Get current EID. + * @return Reference to configured EID. + */ + const DoIpEid &getEid() const { return m_config.eid; } - void setGID(const uint64_t inputGID); - const DoIPGID &getGID() const { return m_GID; } + /** + * @brief Set GID value. + * @param inputGID GID as 64-bit value (lower 48 bits used). + */ + void setGid(uint64_t inputGID); + /** + * @brief Get current GID. + * @return Reference to configured GID. + */ + const DoIpGid &getGid() const { return m_config.gid; } + + /** + * @brief Get current further action requirement status. + * @return Current `DoIPFurtherAction` value. + */ + DoIPFurtherAction getFurtherActionRequired() const { return m_FurtherActionReq; } + /** + * @brief Set further action requirement status. + * @param furtherActionRequired Value to set. + */ + void setFurtherActionRequired(DoIPFurtherAction furtherActionRequired); - void setFAR(DoIPFurtherAction inputFAR); + /** + * @brief Get last accepted client IP (string form). + * @return IP address string. + */ + std::string getClientIp() const { return m_clientIp; } + /** + * @brief Get last accepted client TCP port. + * @return Client port number. + */ + int getClientPort() const { return m_clientPort; } private: int m_tcp_sock{-1}; int m_udp_sock{-1}; - struct sockaddr_in m_serverAddress {}; - struct sockaddr_in m_clientAddress {}; + struct sockaddr_in m_serverAddress{}; + struct sockaddr_in m_clientAddress{}; ByteArray m_receiveBuf{}; - - DoIPVIN m_VIN; - DoIPAddress m_gatewayAddress = DoIPAddress::ZeroAddress; - DoIPEID m_EID = DoIPEID::Zero; - DoIPGID m_GID = DoIPGID::Zero; + std::string m_clientIp{}; + int m_clientPort{}; DoIPFurtherAction m_FurtherActionReq = DoIPFurtherAction::NoFurtherAction; - //TimerManager m_timerManager{}; - - int m_announceNum = 3; // Default Value = 3 - unsigned int m_announceInterval = 500; // Default Value = 500ms - bool m_useLoopbackAnnouncements = false; // Default: use broadcast - - int m_broadcast = 1; // Automatic mode state std::atomic m_running{false}; std::vector m_workerThreads; - ConnectionAcceptedHandler m_connectionHandler; + std::mutex m_mutex; - ssize_t reactToReceivedUdpMessage(size_t bytesRead); - ssize_t sendUdpMessage(const uint8_t *message, size_t messageLength); + // Server configuration + ServerConfig m_config; - void setMulticastGroup(const char *address); + void stop(); + void daemonize(); - ssize_t sendNegativeUdpAck(DoIPNegativeAck ackCode); + void setMulticastGroup(const char *address) const; - // Worker thread functions - void udpListenerThread(); + ssize_t sendNegativeUdpAck(DoIPNegativeAck ackCode); template void tcpListenerThread(); void connectionHandlerThread(std::unique_ptr connection); + + void udpListenerThread(); + void udpAnnouncementThread(); + ssize_t sendVehicleAnnouncement(); + + ssize_t sendUdpResponse(DoIPMessage msg); }; // Template implementation must be in header for external linkage @@ -186,19 +266,6 @@ std::unique_ptr DoIPServer::waitForTcpConnection() { return nullptr; } - // Create a default server model with the gateway address - // Model model; - // model.serverAddress = m_gatewayAddress; - // if (model.onDiagnosticMessage == nullptr) { - // model.onDiagnosticMessage = [](IConnectionContext& ctx, const DoIPMessage &msg) noexcept -> DoIPDiagnosticAck { - // (void)ctx; - // (void)msg; - // LOG_DOIP_CRITICAL("Diagnostic message received on default-constructed Model"); - // // Default: always ACK - // return std::nullopt; - // }; - // } - return std::unique_ptr(new DoIPConnection(tcpSocket, std::make_unique())); } @@ -228,60 +295,6 @@ void DoIPServer::tcpListenerThread() { LOG_DOIP_INFO("TCP listener thread stopped"); } -/* - * High-level API: Start the server with automatic connection handling - */ -template -bool DoIPServer::start(ConnectionAcceptedHandler onConnectionAccepted, bool sendAnnouncements) { - if (m_running.load()) { - LOG_DOIP_WARN("Server is already running"); - return false; - } - - if (!onConnectionAccepted) { - LOG_DOIP_ERROR("Connection handler callback is required"); - return false; - } - - m_connectionHandler = onConnectionAccepted; - - // Setup sockets - if (!setupUdpSocket()) { - LOG_DOIP_ERROR("Failed to setup UDP socket"); - return false; - } - - - if (setupTcpSocket()) { - LOG_DOIP_ERROR("Failed to setup TCP socket"); - closeUdpSocket(); - return false; - } - - // Start background threads - m_running.store(true); - - try { - m_workerThreads.emplace_back(&DoIPServer::udpListenerThread, this); - m_workerThreads.emplace_back(&DoIPServer::tcpListenerThread, this); - - LOG_DOIP_INFO("DoIP Server started successfully"); - - // Send vehicle announcements if requested - if (sendAnnouncements) { - sendVehicleAnnouncement(); - } - - return true; - } catch (const std::exception &e) { - LOG_DOIP_ERROR("Failed to start worker threads: {}", e.what()); - m_running.store(false); - closeUdpSocket(); - closeTcpSocket(); - return false; - } -} - } // namespace doip #endif /* DOIPSERVER_H */ \ No newline at end of file diff --git a/inc/gen/DoIPConfig.h.in b/inc/gen/DoIPConfig.h.in index b017259..669f986 100644 --- a/inc/gen/DoIPConfig.h.in +++ b/inc/gen/DoIPConfig.h.in @@ -1,5 +1,7 @@ #pragma once +#include + #define DOIP_TIMEOUT_MS @DOIP_TIMEOUT_MS@ #define DOIP_MAX_CONNECTIONS @DOIP_MAX_CONNECTIONS@ @@ -10,7 +12,14 @@ * @note This value is configurable via CMake option DOIP_ALIVE_CHECK_RETRIES. * @note The standard does not mandate retries, so the default is 1 (no retries). For robustness, retries can be enabled. */ -#define DOIP_ALIVE_CHECK_RETRIES @DOIP_ALIVE_CHECK_RETRIES@ +constexpr uint8_t DOIP_ALIVE_CHECK_RETRIES = @DOIP_ALIVE_CHECK_RETRIES@; + +/** + * @brief Maximum Transmission Unit (MTU) size for DoIP messages. + * Currently set to 4095 bytes as per ISO 13400-2:2019 recommendations. + */ +constexpr uint32_t DOIP_MAXIMUM_MTU = @DOIP_MAXIMUM_MTU@; + // Table 48: UDP Ports for DoIP /** @@ -19,6 +28,6 @@ constexpr int DOIP_UDP_DISCOVERY_PORT = 13400; /** - * @brief UDP test equipment request port for DoIP as per ISO 13400-2:2019 + * @brief UDP test equipment request port for DoIP as per ISO 13400-2:2019. */ constexpr int DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT = 13401; \ No newline at end of file diff --git a/src/DoIPClient.cpp b/src/DoIPClient.cpp index bd7c19a..743b32f 100644 --- a/src/DoIPClient.cpp +++ b/src/DoIPClient.cpp @@ -12,20 +12,20 @@ using namespace doip; */ void DoIPClient::startTcpConnection() { - _sockFd = socket(AF_INET, SOCK_STREAM, 0); + m_tcpSocket = socket(AF_INET, SOCK_STREAM, 0); - if (_sockFd >= 0) { + if (m_tcpSocket >= 0) { LOG_TCP_INFO("Client TCP-Socket created successfully"); bool connectedFlag = false; const char *ipAddr = "127.0.0.1"; - _serverAddr.sin_family = AF_INET; - _serverAddr.sin_port = htons(DOIP_UDP_DISCOVERY_PORT); - inet_aton(ipAddr, &(_serverAddr.sin_addr)); + m_serverAddress.sin_family = AF_INET; + m_serverAddress.sin_port = htons(DOIP_UDP_DISCOVERY_PORT); + inet_aton(ipAddr, &(m_serverAddress.sin_addr)); while (!connectedFlag) { - _connected = connect(_sockFd, reinterpret_cast(&_serverAddr), sizeof(_serverAddr)); - if (_connected != -1) { + m_connected = connect(m_tcpSocket, reinterpret_cast(&m_serverAddress), sizeof(m_serverAddress)); + if (m_connected != -1) { connectedFlag = true; LOG_TCP_INFO("Connection to server established"); } @@ -35,48 +35,48 @@ void DoIPClient::startTcpConnection() { void DoIPClient::startUdpConnection() { - _sockFd_udp = socket(AF_INET, SOCK_DGRAM, 0); + m_udpSocket = socket(AF_INET, SOCK_DGRAM, 0); - if (_sockFd_udp >= 0) { + if (m_udpSocket >= 0) { LOG_UDP_INFO("Client-UDP-Socket created successfully"); - _serverAddr.sin_family = AF_INET; - _serverAddr.sin_port = htons(DOIP_UDP_DISCOVERY_PORT); - _serverAddr.sin_addr.s_addr = htonl(INADDR_ANY); + m_serverAddress.sin_family = AF_INET; + m_serverAddress.sin_port = htons(DOIP_UDP_DISCOVERY_PORT); // 13400 + m_serverAddress.sin_addr.s_addr = htonl(INADDR_ANY); - _clientAddr.sin_family = AF_INET; - _clientAddr.sin_port = htons(DOIP_UDP_DISCOVERY_PORT); - _clientAddr.sin_addr.s_addr = htonl(INADDR_ANY); + m_clientAddress.sin_family = AF_INET; + m_clientAddress.sin_port = htons(DOIP_UDP_DISCOVERY_PORT); + m_clientAddress.sin_addr.s_addr = htonl(INADDR_ANY); // binds the socket to any IP DoIPAddress and the Port Number 13400 - bind(_sockFd_udp, reinterpret_cast(&_clientAddr), sizeof(_clientAddr)); + bind(m_udpSocket, reinterpret_cast(&m_clientAddress), sizeof(m_clientAddress)); } } void DoIPClient::startAnnouncementListener() { - _sockFd_announcement = socket(AF_INET, SOCK_DGRAM, 0); + m_udpAnnouncementSocket = socket(AF_INET, SOCK_DGRAM, 0); - if (_sockFd_announcement >= 0) { + if (m_udpAnnouncementSocket >= 0) { LOG_UDP_INFO("Client-Announcement-Socket created successfully"); // Allow socket reuse for broadcast int reuse = 1; - setsockopt(_sockFd_announcement, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + setsockopt(m_udpAnnouncementSocket, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); // Enable broadcast reception int broadcast = 1; - if (setsockopt(_sockFd_announcement, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) < 0) { + if (setsockopt(m_udpAnnouncementSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) < 0) { LOG_UDP_ERROR("Failed to enable broadcast reception: {}", strerror(errno)); } else { LOG_UDP_INFO("Broadcast reception enabled for announcements"); } - _announcementAddr.sin_family = AF_INET; - _announcementAddr.sin_port = htons(DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT); // Port 13401 - _announcementAddr.sin_addr.s_addr = htonl(INADDR_ANY); + m_announcementAddress.sin_family = AF_INET; + m_announcementAddress.sin_port = htons(DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT); // Port 13401 + m_announcementAddress.sin_addr.s_addr = htonl(INADDR_ANY); // Bind to port 13401 for Vehicle Announcements - if (bind(_sockFd_announcement, reinterpret_cast(&_announcementAddr), sizeof(_announcementAddr)) < 0) { + if (bind(m_udpAnnouncementSocket, reinterpret_cast(&m_announcementAddress), sizeof(m_announcementAddress)) < 0) { LOG_UDP_ERROR("Failed to bind announcement socket to port {}: {}", DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT, strerror(errno)); } else { LOG_UDP_INFO("Announcement socket bound to port {} successfully", DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT); @@ -90,13 +90,13 @@ void DoIPClient::startAnnouncementListener() { * closes the client-socket */ void DoIPClient::closeTcpConnection() { - close(_sockFd); + close(m_tcpSocket); } void DoIPClient::closeUdpConnection() { - close(_sockFd_udp); - if (_sockFd_announcement >= 0) { - close(_sockFd_announcement); + close(m_udpSocket); + if (m_udpAnnouncementSocket >= 0) { + close(m_udpAnnouncementSocket); } } @@ -108,20 +108,20 @@ void DoIPClient::reconnectServer() { ssize_t DoIPClient::sendRoutingActivationRequest() { DoIPMessage routingActReq = message::makeRoutingActivationRequest(m_sourceAddress); LOG_DOIP_INFO("TX: {}", fmt::streamed(routingActReq)); - return write(_sockFd, routingActReq.data(), routingActReq.size()); + return write(m_tcpSocket, routingActReq.data(), routingActReq.size()); } ssize_t DoIPClient::sendDiagnosticMessage(const ByteArray &payload) { DoIPMessage msg = message::makeDiagnosticMessage(m_sourceAddress, m_logicalAddress, payload); LOG_DOIP_INFO("TX: {}", fmt::streamed(msg)); - return write(_sockFd, msg.data(), msg.size()); + return write(m_tcpSocket, msg.data(), msg.size()); } ssize_t DoIPClient::sendAliveCheckResponse() { DoIPMessage msg = message::makeAliveCheckResponse(m_sourceAddress); LOG_DOIP_INFO("TX: {}", fmt::streamed(msg)); - return write(_sockFd, msg.data(), msg.size()); + return write(m_tcpSocket, msg.data(), msg.size()); } /* @@ -129,7 +129,7 @@ ssize_t DoIPClient::sendAliveCheckResponse() { */ void DoIPClient::receiveMessage() { - ssize_t bytesRead = recv(_sockFd, _receivedData, _maxDataSize, 0); + ssize_t bytesRead = recv(m_tcpSocket, m_receiveBuf.data(), _maxDataSize, 0); if (bytesRead < 0) { LOG_DOIP_ERROR("Error receiving data from server"); @@ -148,7 +148,7 @@ void DoIPClient::receiveMessage() { return; } - auto optMmsg = DoIPMessage::tryParse(_receivedData, static_cast(bytesRead)); + auto optMmsg = DoIPMessage::tryParse(m_receiveBuf.data(), static_cast(bytesRead)); if (!optMmsg.has_value()) { LOG_DOIP_ERROR("Failed to parse DoIP message from received data"); return; @@ -159,16 +159,16 @@ void DoIPClient::receiveMessage() { void DoIPClient::receiveUdpMessage() { - unsigned int length = sizeof(_clientAddr); + unsigned int length = sizeof(m_clientAddress); // Set socket to timeout after 3 seconds struct timeval timeout; timeout.tv_sec = 3; timeout.tv_usec = 0; - setsockopt(_sockFd_udp, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + setsockopt(m_udpSocket, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); int bytesRead; - bytesRead = recvfrom(_sockFd_udp, _receivedData, _maxDataSize, 0, reinterpret_cast(&_clientAddr), &length); + bytesRead = recvfrom(m_udpSocket, m_receiveBuf.data(), _maxDataSize, 0, reinterpret_cast(&m_clientAddress), &length); if (bytesRead < 0) { if (errno == EAGAIN) { @@ -181,7 +181,7 @@ void DoIPClient::receiveUdpMessage() { LOG_UDP_INFO("Received {} bytes from UDP", bytesRead); - auto optMmsg = DoIPMessage::tryParse(_receivedData, static_cast(bytesRead)); + auto optMmsg = DoIPMessage::tryParse(m_receiveBuf.data(), static_cast(bytesRead)); if (!optMmsg.has_value()) { LOG_UDP_ERROR("Failed to parse DoIP message from UDP data"); return; @@ -192,8 +192,8 @@ void DoIPClient::receiveUdpMessage() { LOG_UDP_INFO("RX: {}", fmt::streamed(msg)); } -void DoIPClient::receiveVehicleAnnouncement() { - unsigned int length = sizeof(_announcementAddr); +bool DoIPClient::receiveVehicleAnnouncement() { + unsigned int length = sizeof(m_announcementAddress); int bytesRead; LOG_UDP_DEBUG("Listening for Vehicle Announcements on port {}", DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT); @@ -202,38 +202,38 @@ void DoIPClient::receiveVehicleAnnouncement() { struct timeval timeout; timeout.tv_sec = 2; // 2 second timeout timeout.tv_usec = 0; - setsockopt(_sockFd_announcement, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + setsockopt(m_udpAnnouncementSocket, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); - bytesRead = recvfrom(_sockFd_announcement, _receivedData, _maxDataSize, 0, - reinterpret_cast(&_announcementAddr), &length); + bytesRead = recvfrom(m_udpAnnouncementSocket, m_receiveBuf.data(), _maxDataSize, 0, + reinterpret_cast(&m_announcementAddress), &length); if (bytesRead < 0) { if (errno == EAGAIN) { LOG_UDP_WARN("Timeout waiting for Vehicle Announcement"); } else { LOG_UDP_ERROR("Error receiving Vehicle Announcement: {}", strerror(errno)); } - return; + return false; } - auto optMsg = DoIPMessage::tryParse(_receivedData, static_cast(bytesRead)); + auto optMsg = DoIPMessage::tryParse(m_receiveBuf.data(), static_cast(bytesRead)); if (!optMsg.has_value()) { LOG_UDP_ERROR("Failed to parse Vehicle Announcement message"); - return; + return false; } DoIPMessage msg = optMsg.value(); - LOG_UDP_INFO("Vehicle Announcement received: {}", fmt::streamed(msg)); - // Parse and display the announcement information if (msg.getPayloadType() == DoIPPayloadType::VehicleIdentificationResponse) { - parseVIResponseInformation(msg.data()); - displayVIResponseInformation(); + LOG_UDP_INFO("Vehicle Announcement received: {}", fmt::streamed(msg)); + parseVehicleIdentificationResponse(msg); + return true; } + return false; } ssize_t DoIPClient::sendVehicleIdentificationRequest(const char *inet_address) { - int setAddressError = inet_aton(inet_address, &(_serverAddr.sin_addr)); + int setAddressError = inet_aton(inet_address, &(m_serverAddress.sin_addr)); if (setAddressError != 0) { LOG_UDP_INFO("Address set successfully"); @@ -241,7 +241,7 @@ ssize_t DoIPClient::sendVehicleIdentificationRequest(const char *inet_address) { LOG_UDP_ERROR("Could not set address. Try again"); } - int socketError = setsockopt(_sockFd_udp, SOL_SOCKET, SO_BROADCAST, &m_broadcast, sizeof(m_broadcast)); + int socketError = setsockopt(m_udpSocket, SOL_SOCKET, SO_BROADCAST, &m_broadcast, sizeof(m_broadcast)); if (socketError == 0) { LOG_UDP_INFO("Broadcast Option set successfully"); @@ -249,7 +249,8 @@ ssize_t DoIPClient::sendVehicleIdentificationRequest(const char *inet_address) { DoIPMessage vehicleIdReq = message::makeVehicleIdentificationRequest(); - ssize_t bytesSent = sendto(_sockFd_udp, vehicleIdReq.data(), vehicleIdReq.size(), 0, reinterpret_cast(&_serverAddr), sizeof(_serverAddr)); + ssize_t bytesSent = sendto(m_udpSocket, vehicleIdReq.data(), vehicleIdReq.size(), 0, reinterpret_cast(&m_serverAddress), sizeof(m_serverAddress)); + LOG_UDP_INFO("Sent Vehicle Identification Request to {}:{}", inet_address, ntohs(m_serverAddress.sin_port)); if (bytesSent > 0) { LOG_DOIP_INFO("Sending Vehicle Identification Request"); @@ -270,65 +271,51 @@ void DoIPClient::setSourceAddress(const DoIPAddress &address) { * Getter for _sockFD */ int DoIPClient::getSockFd() { - return _sockFd; + return m_tcpSocket; } /* - * Getter for _connected + * Getter for m_connected */ int DoIPClient::getConnected() { - return _connected; + return m_connected; } -void DoIPClient::parseVIResponseInformation(const uint8_t *data) { - - // VIN - int j = 0; - for (int i = 8; i <= 24; i++) { - VINResult[j] = data[i]; - j++; - } - - // Logical Adress - m_logicalAddress.update(data, 25); +void DoIPClient::parseVehicleIdentificationResponse(const DoIPMessage &msg) { + auto optVin = msg.getVin(); + auto optEid = msg.getEid(); + auto optGid = msg.getGid(); + auto optLogicalAddress = msg.getLogicalAddress(); + auto optFurtherAction = msg.getFurtherActionRequest(); - // EID - j = 0; - for (int i = 27; i <= 32; i++) { - EIDResult[j] = data[i]; - j++; + if (!optVin || !optEid || !optGid || !optLogicalAddress || !optFurtherAction) { + LOG_DOIP_WARN("Incomplete Vehicle Identification Response received: Missing VIN, EID, GID, Logical Address or Further Action Request"); } - // GID - j = 0; - for (int i = 33; i <= 38; i++) { - GIDResult[j] = data[i]; - j++; - } - - // FurtherActionRequest - FurtherActionReqResult = data[39]; + m_vin = optVin.value(); + m_eid = optEid.value(); + m_gid = optGid.value(); + m_logicalAddress = optLogicalAddress.value(); + m_furtherActionReqResult = optFurtherAction.value(); } -void DoIPClient::displayVIResponseInformation() { +void DoIPClient::printVehicleInformationResponse() { std::ostringstream ss; // output VIN - ss << "VIN: "; + ss << "VIN: " ; if (Logger::colorsSupported()) { ss << ansi::bold_green; } - for (int i = 0; i < 17; i++) { - ss << VINResult[i]; - } - if (Logger::colorsSupported()) { - ss << ansi::reset; - } + ss << m_vin << ansi::reset ; LOG_DOIP_INFO(ss.str()); // output LogicalAddress ss = std::ostringstream{}; - ss << "LogicalAddress: "; - ss << m_logicalAddress; + ss << "LA : "; + if (Logger::colorsSupported()) { + ss << ansi::bold_green; + } + ss << m_logicalAddress << ansi::reset; LOG_DOIP_INFO(ss.str()); // output EID @@ -337,25 +324,24 @@ void DoIPClient::displayVIResponseInformation() { if (Logger::colorsSupported()) { ss << ansi::bold_green; } - for (int i = 0; i < 6; i++) { - ss << std::hex << std::setfill('0') << std::setw(2) << +EIDResult[i] << std::dec; - } - if (Logger::colorsSupported()) { - ss << ansi::reset; - } + ss << m_eid << ansi::reset; LOG_DOIP_INFO(ss.str()); // output GID ss = std::ostringstream{}; ss << "GID: "; - for (int i = 0; i < 6; i++) { - ss << std::hex << std::setfill('0') << std::setw(2) << +GIDResult[i] << std::dec; + if (Logger::colorsSupported()) { + ss << ansi::bold_green; } + ss << m_gid << ansi::reset; LOG_DOIP_INFO(ss.str()); // output FurtherActionRequest ss = std::ostringstream{}; - ss << "FurtherActionRequest: "; - ss << std::hex << std::setfill('0') << std::setw(2) << FurtherActionReqResult << std::dec; + ss << "FAR: "; + if (Logger::colorsSupported()) { + ss << ansi::bold_green; + } + ss << m_furtherActionReqResult << ansi::reset; LOG_DOIP_INFO(ss.str()); } diff --git a/src/DoIPConnection.cpp b/src/DoIPConnection.cpp index f454ff2..be4b3fe 100644 --- a/src/DoIPConnection.cpp +++ b/src/DoIPConnection.cpp @@ -148,11 +148,11 @@ DoIPAddress DoIPConnection::getServerAddress() const { } DoIPAddress DoIPConnection::getClientAddress() const { - return m_gatewayAddress; + return m_logicalAddress; } void DoIPConnection::setClientAddress(const DoIPAddress &address) { - m_gatewayAddress = address; + m_logicalAddress = address; } DoIPDiagnosticAck DoIPConnection::notifyDiagnosticMessage(const DoIPMessage &msg) { diff --git a/src/DoIPServer.cpp b/src/DoIPServer.cpp index bb74162..a1470f7 100644 --- a/src/DoIPServer.cpp +++ b/src/DoIPServer.cpp @@ -1,32 +1,92 @@ -#include "DoIPServer.h" +#include // for std::remove_if +#include // for errno +#include // for strerror +#include +#include +#include +#include + #include "DoIPConnection.h" #include "DoIPMessage.h" +#include "DoIPServer.h" #include "DoIPServerModel.h" #include "Logger.h" #include "MacAddress.h" -#include // for std::remove_if -#include // for errno -#include // for strerror using namespace doip; -#if defined(__linux__) -const char *DEFAULT_IFACE = "eth0"; -#elif defined(__APPLE__) -const char *DEFAULT_IFACE = "en0"; -#else -#pragma error "Unsupported platform" -#endif - DoIPServer::~DoIPServer() { if (m_running.load()) { stop(); } } +DoIPServer::DoIPServer(const ServerConfig &config) + : m_config(config) { + m_receiveBuf.reserve(DOIP_MAXIMUM_MTU); + + setLoopbackMode(m_config.loopback); + + if (m_config.daemonize) { + daemonize(); + } +} + +void DoIPServer::daemonize() { + LOG_DOIP_INFO("Daemonizing DoIP Server..."); + pid_t pid = fork(); + if (pid < 0) { + LOG_DOIP_ERROR("First fork failed: {}", strerror(errno)); + return; + } + + if (pid > 0) { + // Parent exits; child continues + _exit(0); + } + + // Child: create new session and become session leader + if (setsid() < 0) { + LOG_DOIP_ERROR("setsid failed: {}", strerror(errno)); + return; + } + + // Second fork to ensure the daemon can't reacquire a tty + pid = fork(); + if (pid < 0) { + LOG_DOIP_ERROR("Second fork failed: {}", strerror(errno)); + return; + } + if (pid > 0) { + _exit(0); + } + + // Set file mode creation mask to a safe default + umask(0); + + // Change working directory to root to avoid blocking mounts + if (chdir("/") != 0) { + LOG_DOIP_WARN("chdir to / failed: {}", strerror(errno)); + } + + // Close and redirect standard file descriptors to /dev/null + int fd = open("/dev/null", O_RDWR); + if (fd >= 0) { + dup2(fd, STDIN_FILENO); + dup2(fd, STDOUT_FILENO); + dup2(fd, STDERR_FILENO); + if (fd > STDERR_FILENO) { + close(fd); + } + } else { + LOG_DOIP_WARN("Failed to open /dev/null: {}", strerror(errno)); + } + + LOG_DOIP_INFO("DoIP Server daemonized and running"); +} /* - * High-level API: Stop the server and cleanup + * Stop the server and cleanup */ void DoIPServer::stop() { LOG_DOIP_INFO("Stopping DoIP Server..."); @@ -47,28 +107,6 @@ void DoIPServer::stop() { LOG_DOIP_INFO("DoIP Server stopped"); } -/* - * Background thread: UDP message listener - */ -void DoIPServer::udpListenerThread() { - LOG_DOIP_INFO("UDP listener thread started"); - - while (m_running.load()) { - ssize_t result = receiveUdpMessage(); - - // If timeout (result == 0), continue without delay - // The socket already has a timeout configured - if (result < 0 && m_running.load()) { - // Only log errors if we're still supposed to be running - LOG_UDP_DEBUG("UDP receive error, continuing..."); - } - } - - LOG_DOIP_INFO("UDP listener thread stopped"); -} - - - /* * Background thread: Handle individual TCP connection */ @@ -92,7 +130,7 @@ void DoIPServer::connectionHandlerThread(std::unique_ptr connect * Set up a tcp socket, so the socket is ready to accept a connection */ bool DoIPServer::setupTcpSocket() { - LOG_DOIP_DEBUG("Setting up TCP socket on port {}", DOIP_SERVER_PORT); + LOG_DOIP_DEBUG("Setting up TCP socket on port {}", DOIP_SERVER_TCP_PORT); m_tcp_sock = socket(AF_INET, SOCK_STREAM, 0); if (m_tcp_sock < 0) { @@ -108,7 +146,7 @@ bool DoIPServer::setupTcpSocket() { m_serverAddress.sin_family = AF_INET; m_serverAddress.sin_addr.s_addr = htonl(INADDR_ANY); - m_serverAddress.sin_port = htons(DOIP_SERVER_PORT); + m_serverAddress.sin_port = htons(DOIP_SERVER_TCP_PORT); // binds the socket to the address and port number if (bind(m_tcp_sock, reinterpret_cast(&m_serverAddress), sizeof(m_serverAddress)) < 0) { @@ -116,33 +154,7 @@ bool DoIPServer::setupTcpSocket() { return false; } - LOG_TCP_INFO("TCP socket successfully bound to port {}", DOIP_SERVER_PORT); - return true; -} - -bool DoIPServer::setupUdpSocket() { - LOG_UDP_DEBUG("Setting up UDP socket on port {}", DOIP_UDP_DISCOVERY_PORT); - - m_udp_sock = socket(AF_INET, SOCK_DGRAM, 0); - - m_serverAddress.sin_family = AF_INET; - m_serverAddress.sin_addr.s_addr = htonl(INADDR_ANY); - m_serverAddress.sin_port = htons(DOIP_UDP_DISCOVERY_PORT); - - if (m_udp_sock < 0) { - LOG_UDP_ERROR("Failed to create UDP socket: {}", strerror(errno)); - return false; - } - - // binds the socket to any IP DoIPAddress and the Port Number 13400 - if (bind(m_udp_sock, reinterpret_cast(&m_serverAddress), sizeof(m_serverAddress)) < 0) { - LOG_UDP_ERROR("Failed to bind UDP socket: {}", strerror(errno)); - return false; - } - - // setting the IP DoIPAddress for Multicast - setMulticastGroup("224.0.0.2"); - LOG_UDP_INFO("UDP socket successfully bound to port {} with multicast group", DOIP_UDP_DISCOVERY_PORT); + LOG_TCP_INFO("TCP socket successfully bound to port {}", DOIP_SERVER_TCP_PORT); return true; } @@ -153,150 +165,123 @@ void DoIPServer::closeTcpSocket() { close(m_tcp_sock); } -void DoIPServer::closeUdpSocket() { - close(m_udp_sock); -} +bool DoIPServer::setupUdpSocket() { + LOG_UDP_DEBUG("Setting up UDP socket on port {}", DOIP_UDP_DISCOVERY_PORT); -/* - * Receives a udp message and calls reactToReceivedUdpMessage method - * @return amount of bytes which were send back to client - * or -1 if error occurred - */ -ssize_t DoIPServer::receiveUdpMessage() { - // Set socket timeout to prevent blocking indefinitely + m_udp_sock = socket(AF_INET, SOCK_DGRAM, 0); + if (m_udp_sock < 0) { + perror("Failed to create socket"); + return 1; + } + + // Set socket to non-blocking with timeout struct timeval timeout; - timeout.tv_sec = 1; // 1 second timeout + timeout.tv_sec = 1; timeout.tv_usec = 0; setsockopt(m_udp_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); - struct sockaddr_in clientAddr; - unsigned int length = sizeof(clientAddr); - const ssize_t readBytes = recvfrom(m_udp_sock, m_receiveBuf.data(), m_receiveBuf.size(), 0, reinterpret_cast(&clientAddr), &length); - - if (readBytes < 0) { - if (errno == EAGAIN) { - // Timeout - this is normal, just continue - return 0; - } else { - LOG_UDP_ERROR("Error receiving UDP message: {}", strerror(errno)); - return -1; - } + // Enable SO_REUSEADDR + int reuse = 1; + setsockopt(m_udp_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + + // Bind socket to port 13400 + struct sockaddr_in server_addr; + memset(&server_addr, 0, sizeof(server_addr)); + server_addr.sin_family = AF_INET; + server_addr.sin_addr.s_addr = htonl(INADDR_ANY); + server_addr.sin_port = htons(DOIP_UDP_DISCOVERY_PORT); + + if (bind(m_udp_sock, reinterpret_cast(&server_addr), sizeof(server_addr)) < 0) { + perror("Failed to bind socket"); + close(m_udp_sock); + return 1; } - - // Don't log if no data received (can happen with some socket configurations) - if (readBytes > 0) { - LOG_UDP_INFO("RX {} bytes from {}:{}", readBytes, - inet_ntoa(clientAddr.sin_addr), ntohs(clientAddr.sin_port)); + // setting the IP DoIPAddress for Multicast/Broadcast + if (!m_config.loopback) { // + setMulticastGroup("224.0.0.2"); + LOG_UDP_INFO("UDP socket successfully bound to port {} with multicast group", DOIP_UDP_DISCOVERY_PORT); } else { - // For debugging: log zero-byte messages at debug level - LOG_UDP_DEBUG("RX 0 bytes from {}:{}", - inet_ntoa(clientAddr.sin_addr), ntohs(clientAddr.sin_port)); - return 0; // Return early for zero-byte messages + LOG_UDP_INFO("UDP socket successfully bound to port {} with broadcast", DOIP_UDP_DISCOVERY_PORT); } - // Store client address for response - m_clientAddress = clientAddr; + LOG_UDP_DEBUG( + "Socket {} bound to {}:{}", + m_udp_sock, + inet_ntoa(m_serverAddress.sin_addr), + ntohs(m_serverAddress.sin_port)); - ssize_t sentBytes = reactToReceivedUdpMessage(static_cast(readBytes)); + m_running.store(true); + m_workerThreads.emplace_back([this]() { udpListenerThread(); }); + m_workerThreads.emplace_back([this]() { udpAnnouncementThread(); }); - return sentBytes; + return true; } -/* - * Receives a udp message and determine how to process the message - * @return amount of bytes which were send back to client - * or -1 if error occurred - */ -ssize_t DoIPServer::reactToReceivedUdpMessage(size_t bytesRead) { - (void)bytesRead; - ssize_t sentBytes = -1; - // GenericHeaderAction action = parseGenericHeader(data, bytesRead); - - auto optHeader = DoIPMessage::tryParseHeader(m_receiveBuf.data(), DOIP_HEADER_SIZE); - if (!optHeader.has_value()) { - return sendNegativeUdpAck(DoIPNegativeAck::IncorrectPatternFormat); - } - - auto plType = optHeader->first; - // auto payloadLength = optHeader->second; - LOG_UDP_INFO("RX: {}", fmt::streamed(plType)); - switch (plType) { - case DoIPPayloadType::VehicleIdentificationRequest: { - DoIPMessage msg = message::makeVehicleIdentificationResponse(m_VIN, m_gatewayAddress, m_EID, m_GID); - LOG_DOIP_PROTOCOL("TX {}", fmt::streamed(msg)); - ssize_t sendedBytes = sendUdpMessage(msg.data(), DOIP_HEADER_SIZE + msg.size()); - - return static_cast(sendedBytes); - } - - default: { - LOG_DOIP_ERROR("Invalid payload type 0x{:04X} received (receiveUdpMessage())", static_cast(plType)); - return sendNegativeUdpAck(DoIPNegativeAck::UnknownPayloadType); - } +void DoIPServer::closeUdpSocket() { + m_running.store(false); + for (auto &thread : m_workerThreads) { + if (thread.joinable()) { + thread.join(); + } } - return sentBytes; -} - -ssize_t DoIPServer::sendUdpMessage(const uint8_t *message, size_t messageLength) { // sendUdpMessage after receiving a message from the client - // if the server receives a message from a client, than the response should be send back to the client address and port - m_clientAddress.sin_port = m_serverAddress.sin_port; - m_clientAddress.sin_addr.s_addr = m_serverAddress.sin_addr.s_addr; - - int result = sendto(m_udp_sock, message, messageLength, 0, reinterpret_cast(&m_clientAddress), sizeof(m_clientAddress)); - return result; + close(m_udp_sock); } -bool DoIPServer::setEIDdefault() { +bool DoIPServer::setDefaultEid() { MacAddress mac = {0}; if (!getFirstMacAddress(mac)) { LOG_DOIP_ERROR("Failed to get MAC address, using default EID"); - m_EID = DoIPEID::Zero; + m_config.eid = DoIpEid::Zero; return false; } // Set EID based on MAC address (last 6 bytes) - m_EID = DoIPEID(mac.data(), m_EID.ID_LENGTH); + m_config.eid = DoIpEid(mac.data(), m_config.eid.ID_LENGTH); return true; } -void DoIPServer::setVIN(const std::string &VINString) { +void DoIPServer::setVin(const std::string &VINString) { + + m_config.vin = DoIpVin(VINString); +} - m_VIN = DoIPVIN(VINString); +void DoIPServer::setVin(const DoIpVin &vin) { + m_config.vin = vin; } -void DoIPServer::setLogicalGatewayAddress(const unsigned short inputLogAdd) { - m_gatewayAddress.update(inputLogAdd); +void DoIPServer::setLogicalGatewayAddress(unsigned short logicalAddress) { + m_config.logicalAddress.update(logicalAddress); } -void DoIPServer::setEID(const uint64_t inputEID) { - m_EID = DoIPEID(inputEID); +void DoIPServer::setEid(const uint64_t inputEID) { + m_config.eid = DoIpEid(inputEID); } -void DoIPServer::setGID(const uint64_t inputGID) { - m_GID = DoIPGID(inputGID); +void DoIPServer::setGid(const uint64_t inputGID) { + m_config.gid = DoIpGid(inputGID); } -void DoIPServer::setFAR(DoIPFurtherAction inputFAR) { - m_FurtherActionReq = inputFAR; +void DoIPServer::setFurtherActionRequired(DoIPFurtherAction furtherActionRequired) { + m_FurtherActionReq = furtherActionRequired; } void DoIPServer::setAnnounceNum(int Num) { - m_announceNum = Num; + m_config.announceCount = Num; } void DoIPServer::setAnnounceInterval(unsigned int Interval) { - m_announceInterval = Interval; + m_config.announceInterval = Interval; } -void DoIPServer::setAnnouncementMode(bool useLoopback) { - m_useLoopbackAnnouncements = useLoopback; - if (useLoopback) { +void DoIPServer::setLoopbackMode(bool useLoopback) { + m_config.loopback = useLoopback; + if (m_config.loopback) { LOG_DOIP_INFO("Vehicle announcements will use loopback (127.0.0.1)"); } else { LOG_DOIP_INFO("Vehicle announcements will use broadcast (255.255.255.255)"); } } -void DoIPServer::setMulticastGroup(const char *address) { +void DoIPServer::setMulticastGroup(const char *address) const { int loop = 1; // set Option using the same Port for multiple Sockets @@ -312,56 +297,145 @@ void DoIPServer::setMulticastGroup(const char *address) { mreq.imr_interface.s_addr = htonl(INADDR_ANY); // set Option to join Multicast Group - int setGroup = setsockopt(m_udp_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, reinterpret_cast(&mreq), sizeof(mreq)); + int setGroup = setsockopt(m_udp_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, reinterpret_cast(&mreq), sizeof(mreq)); if (setGroup < 0) { LOG_UDP_ERROR("Setting address failed: {}", strerror(errno)); } } -ssize_t DoIPServer::sendVehicleAnnouncement() { - - // Choose address based on mode - const char *address = m_useLoopbackAnnouncements ? "127.0.0.1" : "255.255.255.255"; +ssize_t DoIPServer::sendNegativeUdpAck(DoIPNegativeAck ackCode) { + DoIPMessage msg = message::makeNegativeAckMessage(ackCode); - // setting the destination port for the Announcement to 13401 - m_clientAddress.sin_port = htons(13401); + return sendUdpResponse(msg); +} - int setAddressError = inet_aton(address, &(m_clientAddress.sin_addr)); +// new version starts here +void DoIPServer::udpListenerThread() { + socklen_t client_len = sizeof(m_clientAddress); + + LOG_UDP_INFO("UDP listener thread started"); + + while (m_running) { + ssize_t received = recvfrom(m_udp_sock, m_receiveBuf.data(), sizeof(m_receiveBuf), 0, + reinterpret_cast(&m_clientAddress), &client_len); + + if (received < 0) { + if (errno == EAGAIN /* || errno == EWOULDBLOCK*/) { + // Timeout, continue + continue; + } + if (m_running) { + perror("recvfrom error"); + } + break; + } - if (setAddressError != 0) { - LOG_DOIP_INFO("{} address set successfully: {}", - m_useLoopbackAnnouncements ? "Loopback" : "Broadcast", address); + if (received > 0) { + std::scoped_lock lock(m_mutex); + char client_ip[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &m_clientAddress.sin_addr, client_ip, sizeof(client_ip)); + m_clientIp = std::string(client_ip); + m_clientPort = ntohs(m_clientAddress.sin_port); + + LOG_UDP_INFO("Received {} bytes from {}:{}", received, m_clientIp, m_clientPort); + + auto optHeader = DoIPMessage::tryParseHeader(m_receiveBuf.data(), DOIP_HEADER_SIZE); + if (!optHeader.has_value()) { + auto sentBytes = sendNegativeUdpAck(DoIPNegativeAck::IncorrectPatternFormat); + if (sentBytes < 0) { + if (errno == EAGAIN /*|| errno == EWOULDBLOCK*/) { + usleep(100); + continue; + } + break; + } + } + auto plType = optHeader->first; + // auto payloadLength = optHeader->second; + LOG_UDP_INFO("RX: {}", fmt::streamed(plType)); + + ssize_t sentBytes = 0; + switch (plType) { + case DoIPPayloadType::VehicleIdentificationRequest: { + DoIPMessage msg = message::makeVehicleIdentificationResponse(m_config.vin, m_config.logicalAddress, m_config.eid, m_config.gid); + sentBytes = sendUdpResponse(msg); + } break; + + default: { + LOG_DOIP_ERROR("Invalid payload type 0x{:04X} received (receiveUdpMessage())", static_cast(plType)); + sentBytes = sendNegativeUdpAck(DoIPNegativeAck::UnknownPayloadType); + } + if (sentBytes < 0) { + if (errno == EAGAIN /*|| errno == EWOULDBLOCK*/) { + usleep(100); + continue; + } + break; + } + } + } } - if (!m_useLoopbackAnnouncements) { - // Only set broadcast option for broadcast mode - int socketError = setsockopt(m_udp_sock, SOL_SOCKET, SO_BROADCAST, &m_broadcast, sizeof(m_broadcast)); - if (socketError == 0) { - LOG_DOIP_INFO("Broadcast Option set successfully"); - } + LOG_UDP_INFO("UDP listener thread stopped"); +} + +void DoIPServer::udpAnnouncementThread() { + LOG_DOIP_INFO("Announcement thread started"); + + // Send announcements with configured interval and count + for (int i = 0; i < m_config.announceCount && m_running; i++) { + sendVehicleAnnouncement(); + usleep(m_config.announceInterval * 1000); } - ssize_t sentBytes = -1; + LOG_DOIP_INFO("Announcement thread stopped"); +} - // uint8_t *message = createVehicleIdentificationResponse(VIN, m_gatewayAddress, m_EID, GID, FurtherActionReq); - DoIPMessage msg = message::makeVehicleIdentificationResponse(m_VIN, m_gatewayAddress, m_EID, m_GID, m_FurtherActionReq); +ssize_t DoIPServer::sendVehicleAnnouncement() { + DoIPMessage msg = message::makeVehicleIdentificationResponse(m_config.vin, m_config.logicalAddress, m_config.eid, m_config.gid); - for (int i = 0; i < m_announceNum; i++) { - sentBytes = sendto(m_udp_sock, msg.data(), msg.size(), 0, reinterpret_cast(&m_clientAddress), sizeof(m_clientAddress)); + struct sockaddr_in dest_addr; + memset(&dest_addr, 0, sizeof(dest_addr)); + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT); - if (sentBytes > 0) { - LOG_UDP_INFO("Sent Vehicle Announcement"); - } else { - LOG_UDP_ERROR("Failed sending Vehicle Announcement: {}", strerror(errno)); - LOG_UDP_ERROR("Message: {}", fmt::streamed(msg)); - } - usleep(m_announceInterval * 1000); + const char *dest_ip; + if (m_config.loopback) { + dest_ip = "127.0.0.1"; + inet_pton(AF_INET, dest_ip, &dest_addr.sin_addr); + } else { + dest_ip = "255.255.255.255"; + dest_addr.sin_addr.s_addr = htonl(INADDR_BROADCAST); + + // Enable broadcast + int broadcast = 1; + setsockopt(m_udp_sock, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); + } + + ssize_t sentBytes = sendto(m_udp_sock, msg.data(), msg.size(), 0, + reinterpret_cast(&dest_addr), sizeof(dest_addr)); + + LOG_DOIP_INFO("TX {}", fmt::streamed(msg)); + if (sentBytes > 0) { + LOG_UDP_INFO("Sent Vehicle Announcement: {} bytes to {}:{}", + sentBytes, dest_ip, DOIP_UDP_TEST_EQUIPMENT_REQUEST_PORT); + } else { + LOG_UDP_ERROR("Failed to send announcement: {}", strerror(errno)); } return sentBytes; } -ssize_t DoIPServer::sendNegativeUdpAck(DoIPNegativeAck ackCode) { - DoIPMessage message = message::makeNegativeAckMessage(ackCode); - return sendUdpMessage(message.data(), message.size()); -} +ssize_t DoIPServer::sendUdpResponse(DoIPMessage msg) { + auto sentBytes = sendto(m_udp_sock, msg.data(), msg.size(), 0, + reinterpret_cast(&m_clientAddress), sizeof(m_clientAddress)); + + if (sentBytes > 0) { + LOG_DOIP_INFO("TX {}", fmt::streamed(msg)); + LOG_UDP_INFO("Sent UDS response: {} bytes to {}:{}", + sentBytes, m_clientIp, ntohs(m_clientAddress.sin_port)); + } else { + LOG_DOIP_ERROR("Failed to send message: {}", strerror(errno)); + } + return sentBytes; +} \ No newline at end of file diff --git a/test/DoIPMessage_Test.cpp b/test/DoIPMessage_Test.cpp index d861995..726af2b 100644 --- a/test/DoIPMessage_Test.cpp +++ b/test/DoIPMessage_Test.cpp @@ -137,6 +137,49 @@ TEST_SUITE("DoIPMessage") { } } + TEST_CASE("Message factory - makeVehicleIdentificationRequest") { + DoIPMessage msg = message::makeVehicleIdentificationRequest(); + + CHECK(msg.getPayloadSize() == 0); + CHECK(msg.getMessageSize() == 8); + CHECK(msg.getPayloadType() == DoIPPayloadType::VehicleIdentificationRequest); + } + + TEST_CASE("Message factory - makeVehicleIdentificationResponse") { + DoIpVin vin = DoIpVin("1HGCM82633A123456"); + DoIPAddress logicalAddress = DoIPAddress(1234); + DoIpEid entityType = DoIpEid("EID123"); + DoIpGid groupId = DoIpGid("GID456"); + DoIPFurtherAction furtherAction = DoIPFurtherAction::RoutingActivationForCentralSecurity; + DoIPMessage msg = message::makeVehicleIdentificationResponse(vin, logicalAddress, entityType, groupId, furtherAction); + + INFO(msg, "\n", logicalAddress); + CHECK(msg.getPayloadSize() >= 31); // 17 + 2 + 6 + 6 + 1 (+ 1 optional byte for sync status) + CHECK(msg.getMessageSize() >= 40); + CHECK(msg.getPayloadType() == DoIPPayloadType::VehicleIdentificationResponse); + + // Example: + // VIN: 45.58.41.4D.50.4C.45.53.45.52.56.45.52.30.30.30.30.00. + // LA: 28.00. + // EID: 00.00.00.00.00.00 + // GID: 00.00.00.00.00.00 + // FAR: 00 + // (sync status: optional) + + + CHECK(msg.getVin().has_value()); + CHECK(msg.getVin().value().toString() == vin.toString()); + CHECK(msg.getLogicalAddress().has_value()); + CHECK(msg.getLogicalAddress().value().toUint16() == logicalAddress.toUint16()); + CHECK(msg.getEid().has_value()); + CHECK(msg.getEid().value().toString() == entityType.toString()); + CHECK(msg.getGid().has_value()); + CHECK(msg.getGid().value().toString() == groupId.toString()); + CHECK(msg.getFurtherActionRequest().has_value()); + CHECK(msg.getFurtherActionRequest().value() == furtherAction); + } + + TEST_CASE("Init from raw bytes - invalid args") { const uint8_t short_msg[] = {PROTOCOL_VERSION, PROTOCOL_VERSION_INV, 0x80, 0x01}; const uint8_t inv_protocol[] = {PROTOCOL_VERSION - 1, PROTOCOL_VERSION_INV + 1, 0x80, 0x01}; diff --git a/test/DoIPServer_Test.cpp b/test/DoIPServer_Test.cpp index 24c0e75..0111c29 100644 --- a/test/DoIPServer_Test.cpp +++ b/test/DoIPServer_Test.cpp @@ -28,8 +28,8 @@ TEST_SUITE("DoIPServer Tests") { */ TEST_CASE_FIXTURE(DoIPServerFixture, "Set VIN Test") { std::string testVIN = "TESTVIN1234567890"; - server.setVIN(testVIN); - DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVIN(), DoIPAddress::ZeroAddress, server.getEID(), DoIPGID::Zero, DoIPFurtherAction::NoFurtherAction); + server.setVin(testVIN); + DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVin(), DoIPAddress::ZeroAddress, server.getEid(), DoIpGid::Zero, DoIPFurtherAction::NoFurtherAction); ByteArrayRef payload = msg.getPayload(); // Check that the VIN in the payload matches the set VIN @@ -40,8 +40,8 @@ TEST_SUITE("DoIPServer Tests") { TEST_CASE_FIXTURE(DoIPServerFixture, "Set EID Test") { uint64_t testEID = 0x123456789ABC; - server.setEID(testEID); - DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVIN(), DoIPAddress::ZeroAddress, server.getEID(), DoIPGID::Zero, DoIPFurtherAction::NoFurtherAction); + server.setEid(testEID); + DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVin(), DoIPAddress::ZeroAddress, server.getEid(), DoIpGid::Zero, DoIPFurtherAction::NoFurtherAction); ByteArrayRef payload = msg.getPayload(); // Check that the EID in the payload matches the set EID @@ -51,13 +51,13 @@ TEST_SUITE("DoIPServer Tests") { } TEST_CASE_FIXTURE(DoIPServerFixture, "Set EID default") { - bool result = server.setEIDdefault(); + bool result = server.setDefaultEid(); CHECK(result == true); - DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVIN(), DoIPAddress::ZeroAddress, server.getEID(), DoIPGID::Zero, DoIPFurtherAction::NoFurtherAction); + DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVin(), DoIPAddress::ZeroAddress, server.getEid(), DoIpGid::Zero, DoIPFurtherAction::NoFurtherAction); ByteArrayRef payload = msg.getPayload(); - std::cerr << "EID set to: " << server.getEID().toHexString() << '\n'; + std::cerr << "EID set to: " << server.getEid().toHexString() << '\n'; auto zeros = std::count_if(payload.first + 17 + 2, payload.first + 17 + 2 + 6, [](uint8_t byte) { return byte == 0; }); CHECK(zeros < 6); // At least one byte should not be zero } diff --git a/test/Identifiers_Test.cpp b/test/Identifiers_Test.cpp index cc57d1e..8c3fbba 100644 --- a/test/Identifiers_Test.cpp +++ b/test/Identifiers_Test.cpp @@ -9,7 +9,7 @@ using namespace doip; TEST_SUITE("GenericFixedId") { TEST_CASE("Default constructor creates empty VIN") { - DoIPVIN vin; + DoIpVin vin; // Verify all bytes are '0' for (size_t i = 0; i < 17; ++i) { @@ -19,7 +19,7 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("Construction from string - exact length") { const std::string test_vin = "1HGBH41JXMN109186"; - DoIPVIN vin(test_vin); + DoIpVin vin(test_vin); CHECK_FALSE(vin.isEmpty()); CHECK(vin.toString() == test_vin); @@ -32,7 +32,7 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("Construction from string - shorter than 17 characters") { const std::string test_vin = "ABC12300000000000"; - DoIPVIN vin(test_vin); + DoIpVin vin(test_vin); CHECK_FALSE(vin.isEmpty()); CHECK(vin.toString() == test_vin); @@ -47,7 +47,7 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("Construction from string - longer than 17 characters") { const std::string test_vin = "1HGBH41JXMN109186TOOLONGSTRING"; - DoIPVIN vin(test_vin); + DoIpVin vin(test_vin); CHECK_FALSE(vin.isEmpty()); CHECK(vin.toString() == "1HGBH41JXMN109186"); @@ -59,16 +59,16 @@ TEST_SUITE("GenericFixedId") { } TEST_CASE("Construction from empty string") { - DoIPVIN vin(""); + DoIpVin vin(""); CHECK(vin.isEmpty()); CHECK(vin.toString() == "00000000000000000"); - CHECK_BYTE_ARRAY_REF_EQ(vin.asByteArray(), DoIPVIN::Zero.asByteArray()); + CHECK_BYTE_ARRAY_REF_EQ(vin.asByteArray(), DoIpVin::Zero.asByteArray()); } TEST_CASE("Construction from C-style string") { const char* test_vin = "WVWZZZ1JZYW123456"; - DoIPVIN vin(test_vin); + DoIpVin vin(test_vin); CHECK(vin.toString() == test_vin); CHECK(vin[0] == 'W'); @@ -77,15 +77,15 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("Construction from nullptr C-style string") { const char* null_ptr = nullptr; - DoIPVIN vin(null_ptr); + DoIpVin vin(null_ptr); CHECK(vin.isEmpty()); - CHECK_BYTE_ARRAY_REF_EQ(vin.asByteArray(), DoIPVIN::Zero.asByteArray()); + CHECK_BYTE_ARRAY_REF_EQ(vin.asByteArray(), DoIpVin::Zero.asByteArray()); } TEST_CASE("Construction from byte sequence") { const uint8_t bytes[] = {'T', 'E', 'S', 'T', 'V', 'I', 'N', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0'}; - DoIPVIN vin(bytes, sizeof(bytes)); + DoIpVin vin(bytes, sizeof(bytes)); CHECK(vin.toString() == "TESTVIN1234567890"); CHECK(vin[0] == 'T'); @@ -94,7 +94,7 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("Construction from byte sequence - shorter") { const uint8_t bytes[] = {'S', 'H', 'O', 'R', 'T', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0'}; - DoIPVIN vin(bytes, sizeof(bytes)); + DoIpVin vin(bytes, sizeof(bytes)); CHECK(vin.toString() == "SHORT000000000000"); CHECK(vin[0] == 'S'); @@ -105,22 +105,22 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("Construction from byte sequence - longer") { const uint8_t bytes[] = {'V', 'E', 'R', 'Y', 'L', 'O', 'N', 'G', 'V', 'I', 'N', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0'}; - DoIPVIN vin(bytes, sizeof(bytes)); + DoIpVin vin(bytes, sizeof(bytes)); CHECK(vin.toString() == "VERYLONGVIN123456"); CHECK(vin[16] == '6'); } TEST_CASE("Construction from null byte sequence") { - DoIPVIN vin(nullptr, 10); + DoIpVin vin(nullptr, 10); CHECK(vin.isEmpty()); - CHECK(vin == DoIPVIN::Zero); + CHECK(vin == DoIpVin::Zero); } TEST_CASE("Construction from ByteArray - exact length") { ByteArray bytes = {'1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H'}; - DoIPVIN vin(bytes); + DoIpVin vin(bytes); CHECK(vin.toString() == "123456789ABCDEFGH"); CHECK(vin[0] == '1'); @@ -129,7 +129,7 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("Construction from ByteArray - shorter") { ByteArray bytes = {'X', 'Y', 'Z', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0'}; - DoIPVIN vin(bytes); + DoIpVin vin(bytes); CHECK(vin.toString() == "XYZ00000000000000"); CHECK(vin[0] == 'X'); @@ -139,7 +139,7 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("Construction from ByteArray - longer") { ByteArray bytes = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T'}; - DoIPVIN vin(bytes); + DoIpVin vin(bytes); CHECK(vin.toString() == "ABCDEFGHIJKLMNOPQ"); CHECK(vin[16] == 'Q'); @@ -147,23 +147,23 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("Construction from empty ByteArray") { ByteArray bytes; - DoIPVIN vin(bytes); + DoIpVin vin(bytes); CHECK(vin.isEmpty()); - CHECK_BYTE_ARRAY_REF_EQ(vin.asByteArray(), DoIPVIN::Zero.asByteArray()); + CHECK_BYTE_ARRAY_REF_EQ(vin.asByteArray(), DoIpVin::Zero.asByteArray()); } TEST_CASE("Copy constructor") { - DoIPVIN vin1("ORIGINALVIN123456"); - DoIPVIN vin2(vin1); + DoIpVin vin1("ORIGINALVIN123456"); + DoIpVin vin2(vin1); CHECK(vin1 == vin2); CHECK(vin2.toString() == "ORIGINALVIN123456"); } TEST_CASE("Move assignment") { - DoIPVIN vin1("MOVEASGN123456789"); - DoIPVIN vin2; + DoIpVin vin1("MOVEASGN123456789"); + DoIpVin vin2; vin2 = std::move(vin1); @@ -172,23 +172,23 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("toString method") { SUBCASE("Full VIN") { - DoIPVIN vin("FULLVIN1234567890"); + DoIpVin vin("FULLVIN1234567890"); CHECK(vin.toString() == "FULLVIN1234567890"); } SUBCASE("Partial VIN with padding") { - DoIPVIN vin("PART"); + DoIpVin vin("PART"); CHECK(vin.toString() == "PART0000000000000"); } SUBCASE("Empty VIN") { - DoIPVIN vin; + DoIpVin vin; CHECK(vin.toString() == "00000000000000000"); } } TEST_CASE("getArray method") { - DoIPVIN vin("ARRAYTEST12345678"); + DoIpVin vin("ARRAYTEST12345678"); const auto& arr = vin.getArray(); CHECK(arr.size() == 17); @@ -197,7 +197,7 @@ TEST_SUITE("GenericFixedId") { } TEST_CASE("data method") { - DoIPVIN vin("DATATEST123456789"); + DoIpVin vin("DATATEST123456789"); const uint8_t* ptr = vin.data(); CHECK(ptr != nullptr); @@ -206,9 +206,9 @@ TEST_SUITE("GenericFixedId") { } TEST_CASE("size method") { - DoIPVIN vin1; - DoIPVIN vin2("SHORT"); - DoIPVIN vin3("EXACTSEVENTEENVIN"); + DoIpVin vin1; + DoIpVin vin2("SHORT"); + DoIpVin vin3("EXACTSEVENTEENVIN"); CHECK(vin1.size() == 17); CHECK(vin2.size() == 17); @@ -217,58 +217,58 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("isEmpty method") { SUBCASE("Empty VIN") { - DoIPVIN vin; + DoIpVin vin; CHECK(vin.isEmpty()); } SUBCASE("Empty string") { - DoIPVIN vin(""); + DoIpVin vin(""); CHECK(vin.isEmpty()); } SUBCASE("Zero instance") { - CHECK(DoIPVIN::Zero.isEmpty()); + CHECK(DoIpVin::Zero.isEmpty()); } SUBCASE("Non-empty VIN") { - DoIPVIN vin("X"); + DoIpVin vin("X"); CHECK_FALSE(vin.isEmpty()); } SUBCASE("Full VIN") { - DoIPVIN vin("FULLVIN1234567890"); + DoIpVin vin("FULLVIN1234567890"); CHECK_FALSE(vin.isEmpty()); } } TEST_CASE("Equality operator") { - DoIPVIN vin1("SAMEVIN1234567890"); - DoIPVIN vin2("SAMEVIN1234567890"); - DoIPVIN vin3("DIFFVIN1234567890"); + DoIpVin vin1("SAMEVIN1234567890"); + DoIpVin vin2("SAMEVIN1234567890"); + DoIpVin vin3("DIFFVIN1234567890"); CHECK(vin1 == vin2); CHECK_FALSE(vin1 == vin3); - DoIPVIN vin4; - DoIPVIN vin5; + DoIpVin vin4; + DoIpVin vin5; CHECK(vin4 == vin5); - CHECK_BYTE_ARRAY_REF_EQ(vin4.asByteArray(), DoIPVIN::Zero.asByteArray()); + CHECK_BYTE_ARRAY_REF_EQ(vin4.asByteArray(), DoIpVin::Zero.asByteArray()); } TEST_CASE("Inequality operator") { - DoIPVIN vin1("VIN1_12345678901"); - DoIPVIN vin2("VIN2_12345678901"); - DoIPVIN vin3("VIN1_12345678901"); + DoIpVin vin1("VIN1_12345678901"); + DoIpVin vin2("VIN2_12345678901"); + DoIpVin vin3("VIN1_12345678901"); CHECK(vin1 != vin2); CHECK_FALSE(vin1 != vin3); - DoIPVIN vin4; + DoIpVin vin4; CHECK(vin1 != vin4); } TEST_CASE("Array subscript operator") { - DoIPVIN vin("SUBSCRIPT12345678"); + DoIpVin vin("SUBSCRIPT12345678"); CHECK(vin[0] == 'S'); CHECK(vin[1] == 'U'); @@ -277,7 +277,7 @@ TEST_SUITE("GenericFixedId") { } TEST_CASE("Array subscript with padding") { - DoIPVIN vin("PAD"); + DoIpVin vin("PAD"); CHECK(vin[0] == 'P'); CHECK(vin[1] == 'A'); @@ -287,7 +287,7 @@ TEST_SUITE("GenericFixedId") { } TEST_CASE("VIN with special characters") { - DoIPVIN vin("VIN-WITH_SPEC.IAL"); + DoIpVin vin("VIN-WITH_SPEC.IAL"); CHECK(vin.toString() == "VIN-WITH_SPEC.IAL"); CHECK(vin[3] == '-'); @@ -296,7 +296,7 @@ TEST_SUITE("GenericFixedId") { } TEST_CASE("VIN with numeric characters") { - DoIPVIN vin("12345678901234567"); + DoIpVin vin("12345678901234567"); CHECK(vin.toString() == "12345678901234567"); CHECK(vin[0] == '1'); @@ -304,39 +304,39 @@ TEST_SUITE("GenericFixedId") { } TEST_CASE("VIN with lowercase characters") { - DoIPVIN vin("lowercase12345678"); + DoIpVin vin("lowercase12345678"); CHECK(vin.toString() == "lowercase12345678"); CHECK(vin[0] == 'l'); } TEST_CASE("VIN with mixed case") { - DoIPVIN vin("MiXeDcAsE12345678"); + DoIpVin vin("MiXeDcAsE12345678"); CHECK(vin.toString() == "MiXeDcAsE12345678"); } TEST_CASE("Real-world VIN examples") { SUBCASE("Honda VIN") { - DoIPVIN vin("1HGBH41JXMN109186"); + DoIpVin vin("1HGBH41JXMN109186"); CHECK(vin.toString() == "1HGBH41JXMN109186"); CHECK_FALSE(vin.isEmpty()); } SUBCASE("Volkswagen VIN") { - DoIPVIN vin("WVWZZZ1JZYW123456"); + DoIpVin vin("WVWZZZ1JZYW123456"); CHECK(vin.toString() == "WVWZZZ1JZYW123456"); CHECK_FALSE(vin.isEmpty()); } SUBCASE("BMW VIN") { - DoIPVIN vin("WBA3B1G59DNP26082"); + DoIpVin vin("WBA3B1G59DNP26082"); CHECK(vin.toString() == "WBA3B1G59DNP26082"); CHECK_FALSE(vin.isEmpty()); } SUBCASE("Mercedes VIN") { - DoIPVIN vin("WDDUG8CB9DA123456"); + DoIpVin vin("WDDUG8CB9DA123456"); CHECK(vin.toString() == "WDDUG8CB9DA123456"); CHECK_FALSE(vin.isEmpty()); } @@ -345,9 +345,9 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("VIN conversion round-trip") { const std::string original = "ROUNDTRIP12345678"; - DoIPVIN vin1(original); + DoIpVin vin1(original); std::string str = vin1.toString(); - DoIPVIN vin2(str); + DoIpVin vin2(str); CHECK(vin1 == vin2); CHECK(vin2.toString() == original); @@ -356,9 +356,9 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("ByteArray conversion round-trip") { const std::string original = "BYTEARRAYTRIP1234"; - DoIPVIN vin1(original); + DoIpVin vin1(original); ByteArrayRef bytes = vin1.asByteArray(); - DoIPVIN vin2(bytes.first, bytes.second); + DoIpVin vin2(bytes.first, bytes.second); CHECK(vin1 == vin2); CHECK(bytes.second == 17); @@ -366,7 +366,7 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("VIN with null bytes in middle") { uint8_t data[17] = {'V', 'I', 'N', 0, 'N', 'U', 'L', 'L', 0, 'B', 'Y', 'T', 'E', 'S', '1', '2', '3'}; - DoIPVIN vin(data, 17); + DoIpVin vin(data, 17); // toString should stop at first null byte CHECK(vin.toString() == "VIN"); @@ -377,7 +377,7 @@ TEST_SUITE("GenericFixedId") { } TEST_CASE("Constant correctness") { - const DoIPVIN vin("CONSTVIN123456789"); + const DoIpVin vin("CONSTVIN123456789"); CHECK(vin.toString() == "CONSTVIN123456789"); CHECK(vin.size() == 17); @@ -391,28 +391,28 @@ TEST_SUITE("GenericFixedId") { CHECK(ptr[0] == 'C'); } - TEST_CASE("DoIPEID - Entity Identifier (6 bytes)") { + TEST_CASE("DoIpEid - Entity Identifier (6 bytes)") { SUBCASE("Default constructor") { - DoIPEID eid; + DoIpEid eid; CHECK(eid.isEmpty()); CHECK(eid.size() == 6); CHECK(eid.toString().empty()); } SUBCASE("Static Zero instance") { - CHECK(DoIPEID::Zero.isEmpty()); - CHECK(DoIPEID::Zero.size() == 6); + CHECK(DoIpEid::Zero.isEmpty()); + CHECK(DoIpEid::Zero.size() == 6); } SUBCASE("Construction from string - exact length") { - DoIPEID eid("ABC123"); + DoIpEid eid("ABC123"); CHECK(eid.toString() == "ABC123"); CHECK(eid.size() == 6); CHECK_FALSE(eid.isEmpty()); } SUBCASE("Construction from string - shorter") { - DoIPEID eid("EID"); + DoIpEid eid("EID"); CHECK(eid.toString() == "EID"); CHECK(eid.size() == 6); CHECK(eid[0] == 'E'); @@ -422,14 +422,14 @@ TEST_SUITE("GenericFixedId") { } SUBCASE("Construction from string - longer") { - DoIPEID eid("TOOLONGEID"); + DoIpEid eid("TOOLONGEID"); CHECK(eid.toString() == "TOOLON"); CHECK(eid.size() == 6); } SUBCASE("Construction from byte array") { const uint8_t bytes[] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06}; - DoIPEID eid(bytes, sizeof(bytes)); + DoIpEid eid(bytes, sizeof(bytes)); CHECK(eid.size() == 6); CHECK(eid[0] == 0x01); CHECK(eid[5] == 0x06); @@ -438,16 +438,16 @@ TEST_SUITE("GenericFixedId") { SUBCASE("Construction from ByteArray") { ByteArray bytes = {0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF}; - DoIPEID eid(bytes); + DoIpEid eid(bytes); CHECK(eid.size() == 6); CHECK(eid[0] == 0xAA); CHECK(eid[5] == 0xFF); } SUBCASE("Equality and comparison") { - DoIPEID eid1("EID001"); - DoIPEID eid2("EID001"); - DoIPEID eid3("EID002"); + DoIpEid eid1("EID001"); + DoIpEid eid2("EID001"); + DoIpEid eid3("EID002"); CHECK(eid1 == eid2); CHECK(eid1 != eid3); @@ -455,7 +455,7 @@ TEST_SUITE("GenericFixedId") { } SUBCASE("asByteArray method") { - DoIPEID eid("TEST12"); + DoIpEid eid("TEST12"); ByteArrayRef result = eid.asByteArray(); CHECK(result.second == 6); CHECK(result.first[0] == 'T'); @@ -463,28 +463,28 @@ TEST_SUITE("GenericFixedId") { } } - TEST_CASE("DoIPGID - Group Identifier (6 bytes)") { + TEST_CASE("DoIpGid - Group Identifier (6 bytes)") { SUBCASE("Default constructor") { - DoIPGID gid; + DoIpGid gid; CHECK(gid.isEmpty()); CHECK(gid.size() == 6); CHECK(gid.toString().empty()); } SUBCASE("Static Zero instance") { - CHECK(DoIPGID::Zero.isEmpty()); - CHECK(DoIPGID::Zero.size() == 6); + CHECK(DoIpGid::Zero.isEmpty()); + CHECK(DoIpGid::Zero.size() == 6); } SUBCASE("Construction from string - exact length") { - DoIPGID gid("GRP001"); + DoIpGid gid("GRP001"); CHECK(gid.toString() == "GRP001"); CHECK(gid.size() == 6); CHECK_FALSE(gid.isEmpty()); } SUBCASE("Construction from string - shorter") { - DoIPGID gid("GID"); + DoIpGid gid("GID"); CHECK(gid.toString() == "GID"); CHECK(gid.size() == 6); CHECK(gid[0] == 'G'); @@ -494,14 +494,14 @@ TEST_SUITE("GenericFixedId") { } SUBCASE("Construction from string - longer") { - DoIPGID gid("TOOLONGGID"); + DoIpGid gid("TOOLONGGID"); CHECK(gid.toString() == "TOOLON"); CHECK(gid.size() == 6); } SUBCASE("Construction from uint32_t - longer") { uint32_t long_value = 0x544F4F4C; // ASCII for "TOOL" - DoIPGID gid(long_value); + DoIpGid gid(long_value); CHECK(gid.toString() == "TOOL"); CHECK(gid.size() == 6); } @@ -509,14 +509,14 @@ TEST_SUITE("GenericFixedId") { SUBCASE("Construction from uint64_t - longer") { uint64_t long_value = 0x544F4F4C4F4E47; // ASCII for "TOOLONG" - DoIPGID gid(long_value); + DoIpGid gid(long_value); CHECK(gid.toString() == "OOLONG"); // CHECK(gid.size() == 6); } SUBCASE("Construction from byte array") { const uint8_t bytes[] = {0x10, 0x20, 0x30, 0x40, 0x50, 0x60}; - DoIPGID gid(bytes, sizeof(bytes)); + DoIpGid gid(bytes, sizeof(bytes)); CHECK(gid.size() == 6); CHECK(gid[0] == 0x10); CHECK(gid[5] == 0x60); @@ -525,16 +525,16 @@ TEST_SUITE("GenericFixedId") { SUBCASE("Construction from ByteArray") { ByteArray bytes = {0x11, 0x22, 0x33, 0x44, 0x55, 0x66}; - DoIPGID gid(bytes); + DoIpGid gid(bytes); CHECK(gid.size() == 6); CHECK(gid[0] == 0x11); CHECK(gid[5] == 0x66); } SUBCASE("Equality and comparison") { - DoIPGID gid1("GROUP1"); - DoIPGID gid2("GROUP1"); - DoIPGID gid3("GROUP2"); + DoIpGid gid1("GROUP1"); + DoIpGid gid2("GROUP1"); + DoIpGid gid3("GROUP2"); CHECK(gid1 == gid2); CHECK(gid1 != gid3); @@ -542,7 +542,7 @@ TEST_SUITE("GenericFixedId") { } SUBCASE("asByteArray method") { - DoIPGID gid("MYGRP1"); + DoIpGid gid("MYGRP1"); ByteArrayRef result = gid.asByteArray(); CHECK(result.second == 6); CHECK(result.first[0] == 'M'); @@ -552,16 +552,16 @@ TEST_SUITE("GenericFixedId") { TEST_CASE("Different identifier types are independent") { // Even though EID and GID have the same length, they're different types - DoIPEID eid("ABC123"); - DoIPGID gid("ABC123"); + DoIpEid eid("ABC123"); + DoIpGid gid("ABC123"); // They should have the same content but be different types CHECK(eid.toString() == gid.toString()); CHECK(eid.size() == gid.size()); // Verify they're truly independent instances - DoIPEID eid2(eid); - DoIPGID gid2(gid); + DoIpEid eid2(eid); + DoIpGid gid2(gid); CHECK(eid == eid2); CHECK(gid == gid2); } diff --git a/test/VehicleIdentification_Test.cpp b/test/VehicleIdentification_Test.cpp index 553104f..53a898d 100644 --- a/test/VehicleIdentification_Test.cpp +++ b/test/VehicleIdentification_Test.cpp @@ -10,12 +10,12 @@ using namespace std; TEST_SUITE("VehicleIdentificationHandler") { struct VehicleIdentificationHandlerFixture { - DoIPVIN matchingVIN = DoIPVIN("MatchingVin_12345"); - DoIPVIN shortVIN = DoIPVIN("shortVin"); - DoIPVIN shortVINPadded = DoIPVIN("shortVin000000000"); - DoIPEID EID = DoIPEID::Zero; - DoIPGID GID = DoIPGID::Zero; - DoIPFurtherAction far = DoIPFurtherAction::NoFurtherAction; + DoIpVin matchingVIN = DoIpVin("MatchingVin_12345"); + DoIpVin shortVIN = DoIpVin("shortVin"); + DoIpVin shortVINPadded = DoIpVin("shortVin000000000"); + DoIpEid EID = DoIpEid::Zero; + DoIpGid GID = DoIpGid::Zero; + DoIPFurtherAction furtherActionRequired = DoIPFurtherAction::NoFurtherAction; DoIPFurtherAction far_cs = DoIPFurtherAction::RoutingActivationForCentralSecurity; VehicleIdentificationHandlerFixture() { @@ -31,7 +31,7 @@ TEST_SUITE("VehicleIdentificationHandler") { * Checks if a VIN with 17 bytes matches correctly the input data */ TEST_CASE_FIXTURE(VehicleIdentificationHandlerFixture, "VIN 17 Bytes") { - DoIPMessage msg = message::makeVehicleIdentificationResponse(matchingVIN, DoIPAddress::ZeroAddress, EID, GID, far); + DoIPMessage msg = message::makeVehicleIdentificationResponse(matchingVIN, DoIPAddress::ZeroAddress, EID, GID, furtherActionRequired); ByteArrayRef payload = msg.getPayload(); ByteArray expected{ // VIN (17 bytes) From 4447e12fcf206658b8352dacff73786c268269ef Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Tue, 9 Dec 2025 16:20:50 +0100 Subject: [PATCH 6/8] doc: Remove obsolete code --- doc/udp/Makefile | 16 --- doc/udp/README.md | 117 ---------------- doc/udp/doip_client.c | 308 ------------------------------------------ doc/udp/doip_server.c | 285 -------------------------------------- 4 files changed, 726 deletions(-) delete mode 100644 doc/udp/Makefile delete mode 100644 doc/udp/README.md delete mode 100644 doc/udp/doip_client.c delete mode 100644 doc/udp/doip_server.c diff --git a/doc/udp/Makefile b/doc/udp/Makefile deleted file mode 100644 index 97121ff..0000000 --- a/doc/udp/Makefile +++ /dev/null @@ -1,16 +0,0 @@ -CC = gcc -CFLAGS = -Wall -Wextra -pthread -g -LDFLAGS = -pthread - -all: doip_server doip_client - -doip_server: doip_server.c - $(CC) $(CFLAGS) -o doip_server doip_server.c $(LDFLAGS) - -doip_client: doip_client.c - $(CC) $(CFLAGS) -o doip_client doip_client.c $(LDFLAGS) - -clean: - rm -f doip_server doip_client - -.PHONY: all clean diff --git a/doc/udp/README.md b/doc/udp/README.md deleted file mode 100644 index 66c33b8..0000000 --- a/doc/udp/README.md +++ /dev/null @@ -1,117 +0,0 @@ -# Minimal DoIP UDP Discovery Implementation - -This is a minimal implementation of the DoIP (ISO 13400) UDP discovery mechanism with both server and client. - -## Features - -- **Server**: Sends periodic Vehicle Announcements and responds to Vehicle Identification Requests -- **Client**: Listens for Vehicle Announcements, stores server IP, and sends Vehicle Identification Request -- **Loopback mode**: For testing client and server on the same host (avoids multicast issues) - -## Building - -```bash -make -``` - -This will create two executables: -- `doip_server` - The DoIP server -- `doip_client` - The DoIP client - -## Usage - -### Running in Loopback Mode (Recommended for Testing) - -**Terminal 1 - Start the server:** -```bash -./doip_server --loopback -``` - -**Terminal 2 - Start the client:** -```bash -./doip_client --loopback -``` - -### Running in Broadcast Mode (For Network Testing) - -**Terminal 1 - Start the server:** -```bash -./doip_server -``` - -**Terminal 2 - Start the client:** -```bash -./doip_client -``` - -## How It Works - -### Server (Port 13400) -1. Creates two UDP sockets (both bound to port 13400 with SO_REUSEADDR): - - Receive socket: Listens for incoming Vehicle Identification Requests - - Send socket: Sends Vehicle Announcements and responses -2. Starts two threads: - - Listener thread: Continuously listens for requests - - Announcement thread: Sends 5 Vehicle Announcements (every 2 seconds) to port 13401 -3. When a Vehicle Identification Request is received: - - Parses the request - - Sends a Vehicle Identification Response back to the client - -### Client (Port 13401) -1. Creates two UDP sockets: - - Request socket: Sends Vehicle Identification Requests (unbound, OS assigns port) - - Announcement socket: Bound to port 13401 to receive Vehicle Announcements -2. Workflow: - - Listens for Vehicle Announcement from server - - Extracts vehicle information (VIN, Logical Address, EID, GID, IP address) - - Sends Vehicle Identification Request to the discovered server IP on port 13400 - - Waits for and displays the response - -## Ports - -- **13400**: DoIP UDP Discovery Port (server listens here, client sends requests here) -- **13401**: DoIP Test Equipment Port (client listens here for announcements) - -## Protocol Details - -### DoIP Header (8 bytes) -- Protocol Version: 0x04 -- Inverse Protocol Version: 0xFB -- Payload Type: 2 bytes -- Payload Length: 4 bytes - -### Payload Types -- `0x0001`: Vehicle Identification Request (no payload) -- `0x0004`: Vehicle Identification Response (33 bytes minimum) - -### Vehicle Identification Response Payload -- VIN: 17 bytes (ASCII) -- Logical Address: 2 bytes -- EID: 6 bytes -- GID: 6 bytes -- Further Action Required: 1 byte -- VIN/GID sync status: 1 byte - -## Key Design Decisions - -1. **Two sockets on server**: Avoids race conditions between sending announcements and receiving requests -2. **SO_REUSEADDR**: Allows both server sockets to bind to port 13400 -3. **Loopback mode**: Uses unicast (127.0.0.1) instead of broadcast for local testing -4. **Non-blocking receive**: Server uses timeout to allow clean shutdown -5. **Separate announcement socket on client**: Dedicated socket for receiving broadcasts on port 13401 - -## Troubleshooting - -If the client doesn't receive announcements: -- Check firewall rules -- Verify both programs are running -- Use `tcpdump` to monitor UDP traffic: - ```bash - sudo tcpdump -i any udp port 13400 or udp port 13401 -X - ``` - -## Cleanup - -```bash -make clean -``` diff --git a/doc/udp/doip_client.c b/doc/udp/doip_client.c deleted file mode 100644 index e7c60bf..0000000 --- a/doc/udp/doip_client.c +++ /dev/null @@ -1,308 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#define DOIP_UDP_DISCOVERY_PORT 13400 -#define DOIP_UDP_TEST_EQUIPMENT_PORT 13401 -#define DOIP_PROTOCOL_VERSION 0x04 -#define DOIP_INVERSE_PROTOCOL_VERSION 0xFB - -// DoIP Payload Types -#define VEHICLE_IDENTIFICATION_REQUEST 0x0001 -#define VEHICLE_IDENTIFICATION_RESPONSE 0x0004 - -// Discovered vehicle information -typedef struct { - char vin[18]; - uint16_t logical_address; - uint8_t eid[6]; - uint8_t gid[6]; - char ip_address[INET_ADDRSTRLEN]; - uint16_t port; -} VehicleInfo; - -// Create DoIP header -void create_doip_header(uint8_t *buffer, uint16_t payload_type, uint32_t payload_length) { - buffer[0] = DOIP_PROTOCOL_VERSION; - buffer[1] = DOIP_INVERSE_PROTOCOL_VERSION; - buffer[2] = (payload_type >> 8) & 0xFF; - buffer[3] = payload_type & 0xFF; - buffer[4] = (payload_length >> 24) & 0xFF; - buffer[5] = (payload_length >> 16) & 0xFF; - buffer[6] = (payload_length >> 8) & 0xFF; - buffer[7] = payload_length & 0xFF; -} - -// Create Vehicle Identification Request -int create_vehicle_identification_request(uint8_t *buffer) { - create_doip_header(buffer, VEHICLE_IDENTIFICATION_REQUEST, 0); - return 8; // Header only, no payload -} - -// Parse DoIP header -bool parse_doip_header(const uint8_t *buffer, size_t length, uint16_t *payload_type, uint32_t *payload_length) { - if (length < 8) { - return false; - } - - if (buffer[0] != DOIP_PROTOCOL_VERSION || buffer[1] != DOIP_INVERSE_PROTOCOL_VERSION) { - printf("[CLIENT] Invalid DoIP protocol version\n"); - return false; - } - - *payload_type = (buffer[2] << 8) | buffer[3]; - *payload_length = (buffer[4] << 24) | (buffer[5] << 16) | (buffer[6] << 8) | buffer[7]; - - return true; -} - -// Parse Vehicle Identification Response -bool parse_vehicle_identification_response(const uint8_t *buffer, size_t length, VehicleInfo *info) { - if (length < 41) { // 8 (header) + 33 (minimum payload) - printf("[CLIENT] Message too short for Vehicle Identification Response\n"); - return false; - } - - int offset = 8; // Skip header - - // VIN (17 bytes) - memcpy(info->vin, buffer + offset, 17); - info->vin[17] = '\0'; - offset += 17; - - // Logical Address (2 bytes) - info->logical_address = (buffer[offset] << 8) | buffer[offset + 1]; - offset += 2; - - // EID (6 bytes) - memcpy(info->eid, buffer + offset, 6); - offset += 6; - - // GID (6 bytes) - memcpy(info->gid, buffer + offset, 6); - offset += 6; - - return true; -} - -// Print vehicle information -void print_vehicle_info(const VehicleInfo *info) { - printf("\n[CLIENT] ========== Vehicle Information ==========\n"); - printf("[CLIENT] VIN: %s\n", info->vin); - printf("[CLIENT] Logical Address: 0x%04X\n", info->logical_address); - printf("[CLIENT] EID: %02X:%02X:%02X:%02X:%02X:%02X\n", - info->eid[0], info->eid[1], info->eid[2], - info->eid[3], info->eid[4], info->eid[5]); - printf("[CLIENT] GID: %02X:%02X:%02X:%02X:%02X:%02X\n", - info->gid[0], info->gid[1], info->gid[2], - info->gid[3], info->gid[4], info->gid[5]); - printf("[CLIENT] Server IP: %s\n", info->ip_address); - printf("[CLIENT] Server Port: %d\n", info->port); - printf("[CLIENT] ============================================\n\n"); -} - -int main(int argc, char *argv[]) { - bool use_loopback = false; - - // Parse command line arguments - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "--loopback") == 0) { - use_loopback = true; - } - } - - printf("[CLIENT] Starting DoIP Client\n"); - printf("[CLIENT] Mode: %s\n", use_loopback ? "Loopback" : "Broadcast"); - - // Create socket for sending requests - int request_sock = socket(AF_INET, SOCK_DGRAM, 0); - if (request_sock < 0) { - perror("[CLIENT] Failed to create request socket"); - return 1; - } - - // Create socket for receiving announcements - int announcement_sock = socket(AF_INET, SOCK_DGRAM, 0); - if (announcement_sock < 0) { - perror("[CLIENT] Failed to create announcement socket"); - close(request_sock); - return 1; - } - - // Enable SO_REUSEADDR for announcement socket - int reuse = 1; - setsockopt(announcement_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); - - // Enable broadcast reception - int broadcast = 1; - if (setsockopt(announcement_sock, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) < 0) { - perror("[CLIENT] Failed to enable broadcast reception"); - } - - // Bind announcement socket to port 13401 - struct sockaddr_in announcement_addr; - memset(&announcement_addr, 0, sizeof(announcement_addr)); - announcement_addr.sin_family = AF_INET; - announcement_addr.sin_addr.s_addr = htonl(INADDR_ANY); - announcement_addr.sin_port = htons(DOIP_UDP_TEST_EQUIPMENT_PORT); - - if (bind(announcement_sock, (struct sockaddr *)&announcement_addr, sizeof(announcement_addr)) < 0) { - perror("[CLIENT] Failed to bind announcement socket"); - close(request_sock); - close(announcement_sock); - return 1; - } - - printf("[CLIENT] Announcement socket bound to 0.0.0.0:%d\n", DOIP_UDP_TEST_EQUIPMENT_PORT); - - // Set timeout for announcement socket - struct timeval timeout; - timeout.tv_sec = 5; - timeout.tv_usec = 0; - setsockopt(announcement_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); - - // Listen for Vehicle Announcement - printf("[CLIENT] Listening for Vehicle Announcements...\n"); - - uint8_t buffer[512]; - struct sockaddr_in server_addr; - socklen_t server_len = sizeof(server_addr); - - ssize_t received = recvfrom(announcement_sock, buffer, sizeof(buffer), 0, - (struct sockaddr *)&server_addr, &server_len); - - if (received < 0) { - if (errno == EAGAIN || errno == EWOULDBLOCK) { - printf("[CLIENT] Timeout: No Vehicle Announcement received\n"); - } else { - perror("[CLIENT] Error receiving announcement"); - } - close(request_sock); - close(announcement_sock); - return 1; - } - - char server_ip[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &server_addr.sin_addr, server_ip, sizeof(server_ip)); - printf("[CLIENT] Received announcement: %zd bytes from %s:%d\n", - received, server_ip, ntohs(server_addr.sin_port)); - - // Parse the announcement - uint16_t payload_type; - uint32_t payload_length; - - if (!parse_doip_header(buffer, received, &payload_type, &payload_length)) { - printf("[CLIENT] Failed to parse DoIP header\n"); - close(request_sock); - close(announcement_sock); - return 1; - } - - if (payload_type != VEHICLE_IDENTIFICATION_RESPONSE) { - printf("[CLIENT] Unexpected payload type: 0x%04X\n", payload_type); - close(request_sock); - close(announcement_sock); - return 1; - } - - VehicleInfo vehicle_info; - memset(&vehicle_info, 0, sizeof(vehicle_info)); - - if (!parse_vehicle_identification_response(buffer, received, &vehicle_info)) { - printf("[CLIENT] Failed to parse Vehicle Identification Response\n"); - close(request_sock); - close(announcement_sock); - return 1; - } - - // Store server information - strncpy(vehicle_info.ip_address, server_ip, sizeof(vehicle_info.ip_address)); - vehicle_info.port = DOIP_UDP_DISCOVERY_PORT; // Requests go to port 13400 - - print_vehicle_info(&vehicle_info); - - // Now send a Vehicle Identification Request to the discovered server - printf("[CLIENT] Sending Vehicle Identification Request to %s:%d\n", - vehicle_info.ip_address, vehicle_info.port); - - struct sockaddr_in request_addr; - memset(&request_addr, 0, sizeof(request_addr)); - request_addr.sin_family = AF_INET; - request_addr.sin_port = htons(vehicle_info.port); - inet_pton(AF_INET, vehicle_info.ip_address, &request_addr.sin_addr); - - uint8_t request[8]; - int request_len = create_vehicle_identification_request(request); - - ssize_t sent = sendto(request_sock, request, request_len, 0, - (struct sockaddr *)&request_addr, sizeof(request_addr)); - - if (sent < 0) { - perror("[CLIENT] Failed to send request"); - close(request_sock); - close(announcement_sock); - return 1; - } - - printf("[CLIENT] Sent Vehicle Identification Request: %zd bytes\n", sent); - - // Wait for response - timeout.tv_sec = 3; - timeout.tv_usec = 0; - setsockopt(request_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); - - struct sockaddr_in response_addr; - socklen_t response_len = sizeof(response_addr); - - received = recvfrom(request_sock, buffer, sizeof(buffer), 0, - (struct sockaddr *)&response_addr, &response_len); - - if (received < 0) { - if (errno == EAGAIN || errno == EWOULDBLOCK) { - printf("[CLIENT] Timeout: No response received\n"); - } else { - perror("[CLIENT] Error receiving response"); - } - close(request_sock); - close(announcement_sock); - return 1; - } - - char response_ip[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &response_addr.sin_addr, response_ip, sizeof(response_ip)); - printf("[CLIENT] Received response: %zd bytes from %s:%d\n", - received, response_ip, ntohs(response_addr.sin_port)); - - // Parse the response - if (!parse_doip_header(buffer, received, &payload_type, &payload_length)) { - printf("[CLIENT] Failed to parse response header\n"); - close(request_sock); - close(announcement_sock); - return 1; - } - - if (payload_type == VEHICLE_IDENTIFICATION_RESPONSE) { - VehicleInfo response_info; - memset(&response_info, 0, sizeof(response_info)); - - if (parse_vehicle_identification_response(buffer, received, &response_info)) { - strncpy(response_info.ip_address, response_ip, sizeof(response_info.ip_address)); - response_info.port = ntohs(response_addr.sin_port); - - printf("[CLIENT] Vehicle Identification Response received:\n"); - print_vehicle_info(&response_info); - } - } - - // Cleanup - close(request_sock); - close(announcement_sock); - - printf("[CLIENT] Discovery complete\n"); - return 0; -} diff --git a/doc/udp/doip_server.c b/doc/udp/doip_server.c deleted file mode 100644 index 157c506..0000000 --- a/doc/udp/doip_server.c +++ /dev/null @@ -1,285 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define DOIP_UDP_DISCOVERY_PORT 13400 -#define DOIP_UDP_TEST_EQUIPMENT_PORT 13401 -#define DOIP_PROTOCOL_VERSION 0x04 -#define DOIP_INVERSE_PROTOCOL_VERSION 0xFB - -// DoIP Payload Types -#define VEHICLE_IDENTIFICATION_REQUEST 0x0001 -#define VEHICLE_IDENTIFICATION_RESPONSE 0x0004 - -// Server configuration -typedef struct { - char vin[17]; - uint16_t logical_address; - uint8_t eid[6]; - uint8_t gid[6]; - bool use_loopback; -} ServerConfig; - -// Global server state -static bool server_running = true; -static int udp_sock = -1; - -// Create DoIP header -void create_doip_header(uint8_t *buffer, uint16_t payload_type, uint32_t payload_length) { - buffer[0] = DOIP_PROTOCOL_VERSION; - buffer[1] = DOIP_INVERSE_PROTOCOL_VERSION; - buffer[2] = (payload_type >> 8) & 0xFF; - buffer[3] = payload_type & 0xFF; - buffer[4] = (payload_length >> 24) & 0xFF; - buffer[5] = (payload_length >> 16) & 0xFF; - buffer[6] = (payload_length >> 8) & 0xFF; - buffer[7] = payload_length & 0xFF; -} - -// Create Vehicle Identification Response -int create_vehicle_identification_response(uint8_t *buffer, const ServerConfig *config) { - uint32_t payload_length = 33; // VIN(17) + LogAddr(2) + EID(6) + GID(6) + FAR(1) + VIN/GID_sync(1) - - create_doip_header(buffer, VEHICLE_IDENTIFICATION_RESPONSE, payload_length); - - int offset = 8; - - // VIN (17 bytes) - memcpy(buffer + offset, config->vin, 17); - offset += 17; - - // Logical Address (2 bytes) - buffer[offset++] = (config->logical_address >> 8) & 0xFF; - buffer[offset++] = config->logical_address & 0xFF; - - // EID (6 bytes) - memcpy(buffer + offset, config->eid, 6); - offset += 6; - - // GID (6 bytes) - memcpy(buffer + offset, config->gid, 6); - offset += 6; - - // Further Action Required (1 byte) - buffer[offset++] = 0x00; - - // VIN/GID sync status (1 byte) - optional, set to 0x00 - buffer[offset++] = 0x00; - - return offset; // Total message length -} - -// Parse DoIP header -bool parse_doip_header(const uint8_t *buffer, size_t length, uint16_t *payload_type, uint32_t *payload_length) { - if (length < 8) { - return false; - } - - if (buffer[0] != DOIP_PROTOCOL_VERSION || buffer[1] != DOIP_INVERSE_PROTOCOL_VERSION) { - printf("Invalid DoIP protocol version\n"); - return false; - } - - *payload_type = (buffer[2] << 8) | buffer[3]; - *payload_length = (buffer[4] << 24) | (buffer[5] << 16) | (buffer[6] << 8) | buffer[7]; - - return true; -} - -// Send Vehicle Announcement -void send_vehicle_announcement(const ServerConfig *config) { - uint8_t buffer[256]; - int msg_len = create_vehicle_identification_response(buffer, config); - - struct sockaddr_in dest_addr; - memset(&dest_addr, 0, sizeof(dest_addr)); - dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(DOIP_UDP_TEST_EQUIPMENT_PORT); - - const char *dest_ip; - if (config->use_loopback) { - dest_ip = "127.0.0.1"; - inet_pton(AF_INET, dest_ip, &dest_addr.sin_addr); - } else { - dest_ip = "255.255.255.255"; - dest_addr.sin_addr.s_addr = htonl(INADDR_BROADCAST); - - // Enable broadcast - int broadcast = 1; - setsockopt(udp_sock, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); - } - - ssize_t sent = sendto(udp_sock, buffer, msg_len, 0, - (struct sockaddr *)&dest_addr, sizeof(dest_addr)); - - if (sent > 0) { - printf("[SERVER] Sent Vehicle Announcement: %zd bytes to %s:%d\n", - sent, dest_ip, DOIP_UDP_TEST_EQUIPMENT_PORT); - } else { - perror("[SERVER] Failed to send announcement"); - } -} - -// UDP Listener Thread -void *udp_listener_thread(void *arg) { - ServerConfig *config = (ServerConfig *)arg; - uint8_t buffer[512]; - struct sockaddr_in client_addr; - socklen_t client_len = sizeof(client_addr); - - printf("[SERVER] UDP listener thread started\n"); - - while (server_running) { - ssize_t received = recvfrom(udp_sock, buffer, sizeof(buffer), 0, - (struct sockaddr *)&client_addr, &client_len); - - if (received < 0) { - if (errno == EAGAIN || errno == EWOULDBLOCK) { - // Timeout, continue - continue; - } - if (server_running) { - perror("[SERVER] recvfrom error"); - } - break; - } - - if (received > 0) { - char client_ip[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &client_addr.sin_addr, client_ip, sizeof(client_ip)); - printf("[SERVER] Received %zd bytes from %s:%d\n", - received, client_ip, ntohs(client_addr.sin_port)); - - uint16_t payload_type; - uint32_t payload_length; - - if (parse_doip_header(buffer, received, &payload_type, &payload_length)) { - printf("[SERVER] Payload Type: 0x%04X\n", payload_type); - - if (payload_type == VEHICLE_IDENTIFICATION_REQUEST) { - printf("[SERVER] Vehicle Identification Request received\n"); - - // Send response back to client - uint8_t response[256]; - int resp_len = create_vehicle_identification_response(response, config); - - ssize_t sent = sendto(udp_sock, response, resp_len, 0, - (struct sockaddr *)&client_addr, client_len); - - if (sent > 0) { - printf("[SERVER] Sent Vehicle Identification Response: %zd bytes to %s:%d\n", - sent, client_ip, ntohs(client_addr.sin_port)); - } else { - perror("[SERVER] Failed to send response"); - } - } - } - } - } - - printf("[SERVER] UDP listener thread stopped\n"); - return NULL; -} - -// Announcement Thread -void *announcement_thread(void *arg) { - ServerConfig *config = (ServerConfig *)arg; - - printf("[SERVER] Announcement thread started\n"); - - // Send 5 announcements with 2 second interval - for (int i = 0; i < 5 && server_running; i++) { - send_vehicle_announcement(config); - sleep(2); - } - - printf("[SERVER] Announcement thread stopped\n"); - return NULL; -} - -int main(int argc, char *argv[]) { - bool use_loopback = false; - - // Parse command line arguments - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "--loopback") == 0) { - use_loopback = true; - } - } - - // Configure server - ServerConfig config = { - .vin = "EXAMPLESERVER0000", - .logical_address = 0x0028, - .eid = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, - .gid = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, - .use_loopback = use_loopback - }; - - printf("[SERVER] Starting DoIP Server\n"); - printf("[SERVER] Mode: %s\n", use_loopback ? "Loopback" : "Broadcast"); - printf("[SERVER] VIN: %s\n", config.vin); - printf("[SERVER] Logical Address: 0x%04X\n", config.logical_address); - - // Create UDP socket - udp_sock = socket(AF_INET, SOCK_DGRAM, 0); - if (udp_sock < 0) { - perror("Failed to create socket"); - return 1; - } - - // Set socket to non-blocking with timeout - struct timeval timeout; - timeout.tv_sec = 1; - timeout.tv_usec = 0; - setsockopt(udp_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); - - // Enable SO_REUSEADDR - int reuse = 1; - setsockopt(udp_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); - - // Bind socket to port 13400 - struct sockaddr_in server_addr; - memset(&server_addr, 0, sizeof(server_addr)); - server_addr.sin_family = AF_INET; - server_addr.sin_addr.s_addr = htonl(INADDR_ANY); - server_addr.sin_port = htons(DOIP_UDP_DISCOVERY_PORT); - - if (bind(udp_sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) { - perror("Failed to bind socket"); - close(udp_sock); - return 1; - } - - printf("[SERVER] Socket bound to 0.0.0.0:%d\n", DOIP_UDP_DISCOVERY_PORT); - - // Start threads - pthread_t listener_tid, announcement_tid; - - pthread_create(&listener_tid, NULL, udp_listener_thread, &config); - pthread_create(&announcement_tid, NULL, announcement_thread, &config); - - // Wait for announcement thread to complete - pthread_join(announcement_tid, NULL); - - // Keep server running for a bit to handle requests - printf("[SERVER] Announcements complete, waiting for requests...\n"); - sleep(10); - - // Shutdown - printf("[SERVER] Shutting down...\n"); - server_running = false; - - pthread_join(listener_tid, NULL); - - close(udp_sock); - - printf("[SERVER] Server stopped\n"); - return 0; -} From 0871efe1dedbc852753d532478f14caba3f9f751 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Tue, 9 Dec 2025 20:20:45 +0100 Subject: [PATCH 7/8] refactor: Simplify DoIPAddress to uint_16 --- examples/exampleDoIPServer.cpp | 2 +- inc/DoIPAddress.h | 233 +++++----------------------- inc/DoIPClient.h | 2 +- inc/DoIPMessage.h | 44 +++--- inc/DoIPServer.h | 2 +- src/DoIPDefaultConnection.cpp | 8 +- src/DoIPServer.cpp | 21 +-- test/DoIPDefaultConnection_Test.cpp | 3 +- test/DoIPMessage_Test.cpp | 8 +- test/DoIPServer_Test.cpp | 6 +- test/VehicleIdentification_Test.cpp | 4 +- 11 files changed, 90 insertions(+), 243 deletions(-) diff --git a/examples/exampleDoIPServer.cpp b/examples/exampleDoIPServer.cpp index eb782d5..78bd9b9 100644 --- a/examples/exampleDoIPServer.cpp +++ b/examples/exampleDoIPServer.cpp @@ -112,7 +112,7 @@ int main(int argc, char *argv[]) { } else { val = std::stoul(logical_addr_str, &pos, 0); } - cfg.logicalAddress = DoIPAddress(static_cast(val & 0xFFFF)); + cfg.logicalAddress = static_cast(val & 0xFFFF); } catch (...) { LOG_DOIP_WARN("Failed to parse logical address '{}', using default 0x0E00", logical_addr_str); } diff --git a/inc/DoIPAddress.h b/inc/DoIPAddress.h index fbad14f..a2f00fa 100644 --- a/inc/DoIPAddress.h +++ b/inc/DoIPAddress.h @@ -23,208 +23,55 @@ constexpr size_t DOIP_ADDRESS_SIZE = 2; * * @note The address follows big-endian byte ordering (HSB first, then LSB). */ +using DoIPAddress = uint16_t; -struct DoIPAddress { - /** - * @brief Constructs an DoIPAddress with specified high and low significant bytes. - * - * @param address the address - */ - constexpr explicit DoIPAddress(uint16_t address = 0x0000) : m_bytes{static_cast(address >> 8), static_cast(address & 0xff)} {} +constexpr DoIPAddress ZERO_ADDRESS = 0x0000; +constexpr DoIPAddress MIN_SOURCE_ADDRESS = 0xE000; +constexpr DoIPAddress MAX_SOURCE_ADDRESS = 0xE3FF; - /** - * @brief Constructs an DoIPAddress from a byte array starting at the specified offset. - * - * This constructor reads two consecutive bytes from the provided data array, - * starting at the given offset. The first byte becomes the HSB and the second - * byte becomes the LSB. - * - * @param data Pointer to the byte array containing address data - * @param offset Starting offset in the data array (default: 0) - * - * @note If data is nullptr, the address is initialized to {0, 0} - * @warning No bounds checking is performed on the data array - */ - constexpr explicit DoIPAddress(const uint8_t *data, size_t offset = 0) : m_bytes{0, 0} { - if (data != nullptr) { - m_bytes[HSB] = data[offset]; - m_bytes[LSB] = data[offset + 1]; - } - } - - /** - * @brief Gets the low significant byte of the address. - * - * @return The low significant byte (LSB) as uint8_t - */ - uint8_t lsb() const { return m_bytes[LSB]; } - - /** - * @brief Gets the high significant byteconst DoIPAddress& sa of the address. - * - * @return The high significant byte (HSB) as uint8_t - */ - uint8_t hsb() const { return m_bytes[HSB]; } - - /** - * @brief Converts the address to a 16-bit unsigned integer. - * - * The conversion follows big-endian byte ordering where the HSB forms - * the upper 8 bits and the LSB forms the lower 8 bits. - * - * @return The address as a 16-bit unsigned integer in host byte order - */ - uint16_t toUint16() const { return (m_bytes[0] << 8) | m_bytes[1]; } - - /** - * @brief Gets a pointer to the internal byte array. - * - * This method provides direct access to the underlying byte storage. - * The returned pointer points to a 2-byte array where: - * - data()[0] contains the HSB - * - data()[1] contains the LSB - * - * @return Const pointer to the internal 2-byte array - */ - const uint8_t *data() const { return m_bytes.data(); } - - /** - * @brief Size of the address in bytes. - * - * @return constexpr size_t the address size - */ - constexpr size_t size() const { return DOIP_ADDRESS_SIZE; } - - /** - * @brief Updates the address with new high and low significant bytes. - * - * @param hsb New high significant byte - * @param lsb New low significant byte - */ - void update(uint8_t hsb, uint8_t lsb) { - m_bytes[HSB] = hsb; - m_bytes[LSB] = lsb; - } - - void update(uint16_t hsblsb) { - update((hsblsb >> 8) & 0xFF, hsblsb & 0xFF); - } - - /** - * @brief Updates the address from a byte array starting at the specified offset. - * - * This method reads two consecutive bytes from the provided data array, - * starting at the given offset. The first byte becomes the HSB and the second - * byte becomes the LSB, overwriting the current address values. - * - * @param data Pointer to the byte array containing new address data - * @param offset Starting offset in the data array (default: 0) - * - * @warning No bounds checking is performed on the data array. Ensure that - * data[offset] and data[offset+1] are valid memory locations. - * @warning No null pointer checking is performed. Ensure data is not nullptr. - */ - void update(const uint8_t *data, size_t offset = 0) { - if (data == nullptr) { - return; - } - update(data[offset], data[offset + 1]); - } - - /** - * @brief Appends this address to the given byte array. - * - * @param bytes the byte array to append to - * @return ByteArray& the modified byte array - */ - ByteArray &appendTo(ByteArray &bytes) const { - bytes.emplace_back(hsb()); - bytes.emplace_back(lsb()); - return bytes; - } - - /** - * @brief Compares two DoIPAddress objects for equality. - * - * Two addresses are considered equal if both their high significant bytes - * and low significant bytes are identical. - * - * @param other The DoIPAddress object to compare with - * @return true if both addresses are equal, false otherwise - */ - bool operator==(const DoIPAddress &other) const { - return toUint16() == other.toUint16(); - } - - /** - * @brief Compares two DoIPAddress objects for inequality. - * - * Two addresses are considered unequal if either their high significant bytes - * or low significant bytes differ. - * - * @param other The DoIPAddress object to compare with - * @return true if the addresses are not equal, false otherwise - */ - bool operator!=(const DoIPAddress &other) const { - return !(*this == other); - } - - /** - * @brief Check if source address is valid - * - * @return true the source address is valid - * @return false the source address is NOT valid - */ - bool isValidSourceAddress() { - return isValidSourceAddress(m_bytes.data()); - } - - /** - * @brief Check if source address is valid - * @param data the data array containing the address - * @param offset the offset in the data array where the address starts - * @return true the source address is valid - * @return false the source address is NOT valid - */ - static bool isValidSourceAddress(const uint8_t *data, size_t offset = 0) { - uint16_t addr_value = (data[offset] << 8) | data[offset + 1]; - - return MIN_SOURCE_ADDRESS <= addr_value && MAX_SOURCE_ADDRESS >= addr_value; - } - - // Table 13 - static constexpr uint16_t MIN_SOURCE_ADDRESS = 0xE000; - static constexpr uint16_t MAX_SOURCE_ADDRESS = 0xE3FF; +/** + * @brief Check if source address is valid. + * @param data the data array containing the address + * @param offset the offset in the data array where the address starts + * @return true the source address is valid + * @return false the source address is NOT valid + */ +inline bool isValidSourceAddress(const uint8_t *data, size_t offset = 0) { + uint16_t addr_value = (data[offset] << 8) | data[offset + 1]; - /** - * @brief Constant for a DoIP zero address (0x0000). - */ - static const DoIPAddress ZeroAddress; + return MIN_SOURCE_ADDRESS <= addr_value && MAX_SOURCE_ADDRESS >= addr_value; +} - private: - static constexpr uint8_t HSB = 0; ///< Index for High Significant Byte in the byte array - static constexpr uint8_t LSB = 1; ///< Index for Low Significant Byte in the byte array +/** + * @brief Try read the DoIP address from a byte array. + * @note No bounds checking + * @param data the pointer to the data array + * @param offset the offset in bytes + * @param address the address read + * @return true address was read successfully + * @return false invalid arguments + */ +inline bool tryReadAddressFrom(const uint8_t *data, size_t offset, DoIPAddress &address) { + if (!data) + return false; - std::array m_bytes; ///< Internal storage for the 2-byte address -}; + address = static_cast(data[offset] << 8 | data[offset + 1]); + return true; +} /** - * @brief Stream output operator for DoIPAddress objects. - * - * Outputs the address in a human-readable hexadecimal format. - * The format is "0xHHHH" where HHHH is the 4-digit hexadecimal representation - * of the 16-bit address value. - * - * @param os The output stream to write to - * @param addr The DoIPAddress object to output - * @return Reference to the output stream for chaining + * @brief Reads the DoIP address from a byte array. + * @note No bounds checking + * @param data the pointer to the data array + * @param offset the offset in bytes + * @return DoIPAddress the parsed DoIP address. If data is nullptr, then ZERO_ADDRESS is returned. */ -inline std::ostream &operator<<(std::ostream &os, const DoIPAddress &addr) { - os << "0x" << std::hex << std::uppercase << std::setfill('0') << std::setw(4) << addr.toUint16(); - return os; -} +inline DoIPAddress readAddressFrom(const uint8_t *data, size_t offset = 0) { + if (!data) + return ZERO_ADDRESS; -inline const DoIPAddress DoIPAddress::ZeroAddress = DoIPAddress(); + return static_cast(data[offset] << 8 | data[offset + 1]); +} } // namespace doip diff --git a/inc/DoIPClient.h b/inc/DoIPClient.h index ec651ab..e49f90b 100644 --- a/inc/DoIPClient.h +++ b/inc/DoIPClient.h @@ -65,7 +65,7 @@ class DoIPClient { DoIPAddress m_sourceAddress = DoIPAddress(0xE000); DoIpVin m_vin{0}; - DoIPAddress m_logicalAddress = DoIPAddress::ZeroAddress; + DoIPAddress m_logicalAddress = ZERO_ADDRESS; DoIpEid m_eid{0}; DoIpGid m_gid{0}; DoIPFurtherAction m_furtherActionReqResult = DoIPFurtherAction::NoFurtherAction; diff --git a/inc/DoIPMessage.h b/inc/DoIPMessage.h index 046c0a7..62689af 100644 --- a/inc/DoIPMessage.h +++ b/inc/DoIPMessage.h @@ -275,7 +275,7 @@ class DoIPMessage { auto payloadRef = getPayload(); // todo: Simplify if (hasSourceAddress()) { - return DoIPAddress(payloadRef.first, 0); + return readAddressFrom(payloadRef.first, 0); } return std::nullopt; } @@ -288,7 +288,7 @@ class DoIPMessage { std::optional getLogicalAddress() const { auto payloadRef = getPayload(); if (getPayloadType() == DoIPPayloadType::VehicleIdentificationResponse && payloadRef.second >= 19) { - return DoIPAddress(payloadRef.first + 17); + return readAddressFrom(payloadRef.first + 17); } return std::nullopt; } @@ -301,7 +301,7 @@ class DoIPMessage { std::optional getTargetAddress() const { auto payloadRef = getPayload(); if (getPayloadType() == DoIPPayloadType::DiagnosticMessage && payloadRef.second >= 4) { - return DoIPAddress(payloadRef.first, 2); + return readAddressFrom(payloadRef.first, 2); } return std::nullopt; } @@ -534,10 +534,10 @@ inline DoIPMessage makeVehicleIdentificationResponse( DoIPSyncStatus syncStatus = DoIPSyncStatus::GidVinSynchronized) { ByteArray payload; - payload.reserve(vin.size() + logicalAddress.size() + entityType.size() + groupId.size() + 2); + payload.reserve(vin.size() + sizeof(logicalAddress) + entityType.size() + groupId.size() + 2); payload.insert(payload.end(), vin.begin(), vin.end()); - payload.writeU16BE(logicalAddress.toUint16()); + payload.writeU16BE(logicalAddress); payload.insert(payload.end(), entityType.begin(), entityType.end()); payload.insert(payload.end(), groupId.begin(), groupId.end()); payload.writeEnum(furtherAction); @@ -570,10 +570,10 @@ inline DoIPMessage makeDiagnosticMessage( const ByteArray &msg_payload) { ByteArray payload; - payload.reserve(sa.size() + ta.size() + msg_payload.size()); + payload.reserve(sizeof(sa) + sizeof(ta) + msg_payload.size()); - sa.appendTo(payload); - ta.appendTo(payload); + payload.writeU16BE(sa); + payload.writeU16BE(ta); payload.insert(payload.end(), msg_payload.begin(), msg_payload.end()); return DoIPMessage(DoIPPayloadType::DiagnosticMessage, std::move(payload)); @@ -593,10 +593,10 @@ inline DoIPMessage makeDiagnosticPositiveResponse( const ByteArray &msg_payload) { ByteArray payload; - payload.reserve(sa.size() + ta.size() + msg_payload.size() + 1); + payload.reserve(sizeof(sa) + sizeof(ta) + msg_payload.size() + 1); - sa.appendTo(payload); - ta.appendTo(payload); + payload.writeU16BE(sa); + payload.writeU16BE(ta); payload.emplace_back(DIAGNOSTIC_MESSAGE_ACK); payload.insert(payload.end(), msg_payload.begin(), msg_payload.end()); @@ -619,10 +619,10 @@ inline DoIPMessage makeDiagnosticNegativeResponse( const ByteArray &msg_payload) { ByteArray payload; - payload.reserve(sa.size() + ta.size() + msg_payload.size() + 1); + payload.reserve(sizeof(sa) + sizeof(ta) + msg_payload.size() + 1); - sa.appendTo(payload); - ta.appendTo(payload); + payload.writeU16BE(sa); + payload.writeU16BE(ta); payload.emplace_back(static_cast(nack)); payload.insert(payload.end(), msg_payload.begin(), msg_payload.end()); @@ -646,7 +646,7 @@ inline DoIPMessage makeAliveCheckRequest() { */ inline DoIPMessage makeAliveCheckResponse(const DoIPAddress &sa) { ByteArray payload; - payload.writeU16BE(sa.toUint16()); + payload.writeU16BE(sa); return DoIPMessage(DoIPPayloadType::AliveCheckResponse, std::move(payload)); } @@ -662,8 +662,8 @@ inline DoIPMessage makeRoutingActivationRequest( DoIPRoutingActivationType actType = DoIPRoutingActivationType::Default) { ByteArray payload; - payload.reserve(ea.size() + 1 + 4); - payload.writeU16BE(ea.toUint16()); + payload.reserve(sizeof(ea) + 1 + 4); + payload.writeU16BE(ea); payload.writeEnum(actType); // Reserved 4 bytes for future use payload.insert(payload.end(), {0, 0, 0, 0}); @@ -685,14 +685,14 @@ inline DoIPMessage makeRoutingActivationResponse( DoIPRoutingActivationType actType = DoIPRoutingActivationType::Default) { ByteArray payload; - payload.reserve(ea.size() + 1 + 4); + payload.reserve(sizeof(ea) + 1 + 4); - auto sourceAddr = routingReq.getSourceAddress(); - if (sourceAddr) { - sourceAddr.value().appendTo(payload); + auto optSourceAddress = routingReq.getSourceAddress(); + if (optSourceAddress) { + payload.writeU16BE(optSourceAddress.value()); } - ea.appendTo(payload); + payload.writeU16BE(ea); payload.emplace_back(static_cast(actType)); // Reserved 4 bytes for future use payload.insert(payload.end(), {0, 0, 0, 0}); diff --git a/inc/DoIPServer.h b/inc/DoIPServer.h index d7985ff..2a22ee9 100644 --- a/inc/DoIPServer.h +++ b/inc/DoIPServer.h @@ -143,7 +143,7 @@ class DoIPServer { * @brief Set the logical DoIP gateway address. * @param logicalAddress Logical address value. */ - void setLogicalGatewayAddress(unsigned short logicalAddress); + void setLogicalGatewayAddress(DoIPAddress logicalAddress); /** * @brief Sets the EID to a default value based on the MAC address. diff --git a/src/DoIPDefaultConnection.cpp b/src/DoIPDefaultConnection.cpp index 8378d25..d630c68 100644 --- a/src/DoIPDefaultConnection.cpp +++ b/src/DoIPDefaultConnection.cpp @@ -234,7 +234,7 @@ void DoIPDefaultConnection::handleRoutingActivated(DoIPServerEvent event, OptDoI return; default: LOG_DOIP_WARN("Received unsupported message type {} in Routing Activated state", fmt::streamed(message.getPayloadType())); - sendDiagnosticMessageResponse(DoIPAddress::ZeroAddress, DoIPNegativeDiagnosticAck::TransportProtocolError); + sendDiagnosticMessageResponse(ZERO_ADDRESS, DoIPNegativeDiagnosticAck::TransportProtocolError); // closeConnection(DoIPCloseReason::InvalidMessage); return; } @@ -300,7 +300,7 @@ void DoIPDefaultConnection::handleWaitAliveCheckResponse(DoIPServerEvent event, return; default: LOG_DOIP_WARN("Received unsupported message type {} in Wait Alive Check Response state", fmt::streamed(message.getPayloadType())); - sendDiagnosticMessageResponse(DoIPAddress::ZeroAddress, DoIPNegativeDiagnosticAck::TransportProtocolError); + sendDiagnosticMessageResponse(ZERO_ADDRESS, DoIPNegativeDiagnosticAck::TransportProtocolError); return; } } @@ -359,8 +359,8 @@ ssize_t DoIPDefaultConnection::sendRoutingActivationResponse(const DoIPAddress & // Build response payload manually ByteArray payload; - source_address.appendTo(payload); - serverAddr.appendTo(payload); + payload.writeU16BE(source_address); + payload.writeU16BE(serverAddr); payload.push_back(static_cast(response_code)); // Reserved 4 bytes payload.insert(payload.end(), {0x00, 0x00, 0x00, 0x00}); diff --git a/src/DoIPServer.cpp b/src/DoIPServer.cpp index a1470f7..20ab8c3 100644 --- a/src/DoIPServer.cpp +++ b/src/DoIPServer.cpp @@ -248,8 +248,8 @@ void DoIPServer::setVin(const DoIpVin &vin) { m_config.vin = vin; } -void DoIPServer::setLogicalGatewayAddress(unsigned short logicalAddress) { - m_config.logicalAddress.update(logicalAddress); +void DoIPServer::setLogicalGatewayAddress(DoIPAddress logicalAddress) { + m_config.logicalAddress = logicalAddress; } void DoIPServer::setEid(const uint64_t inputEID) { @@ -362,17 +362,18 @@ void DoIPServer::udpListenerThread() { sentBytes = sendUdpResponse(msg); } break; - default: { + default: LOG_DOIP_ERROR("Invalid payload type 0x{:04X} received (receiveUdpMessage())", static_cast(plType)); sentBytes = sendNegativeUdpAck(DoIPNegativeAck::UnknownPayloadType); - } - if (sentBytes < 0) { - if (errno == EAGAIN /*|| errno == EWOULDBLOCK*/) { - usleep(100); - continue; - } - break; + + } // switch + + if (sentBytes < 0) { + if (errno == EAGAIN /*|| errno == EWOULDBLOCK*/) { + usleep(100); + continue; } + break; } } } diff --git a/test/DoIPDefaultConnection_Test.cpp b/test/DoIPDefaultConnection_Test.cpp index cac4a35..bd58c6d 100644 --- a/test/DoIPDefaultConnection_Test.cpp +++ b/test/DoIPDefaultConnection_Test.cpp @@ -32,8 +32,7 @@ TEST_SUITE("DoIPDefaultConnection") { TEST_CASE_FIXTURE(DoIPDefaultConnectionTestFixture, "DoIPDefaultConnection: Get Server Address") { DoIPAddress serverAddress = connection->getServerAddress(); INFO("Server Address: " << serverAddress); - CHECK(serverAddress.hsb() == 0x0E); - CHECK(serverAddress.lsb() == 0x00); + CHECK(serverAddress == 0x0E00); } TEST_CASE_FIXTURE(DoIPDefaultConnectionTestFixture, "DoIPDefaultConnection: Send Protocol Message") { diff --git a/test/DoIPMessage_Test.cpp b/test/DoIPMessage_Test.cpp index 726af2b..ed47c61 100644 --- a/test/DoIPMessage_Test.cpp +++ b/test/DoIPMessage_Test.cpp @@ -170,7 +170,7 @@ TEST_SUITE("DoIPMessage") { CHECK(msg.getVin().has_value()); CHECK(msg.getVin().value().toString() == vin.toString()); CHECK(msg.getLogicalAddress().has_value()); - CHECK(msg.getLogicalAddress().value().toUint16() == logicalAddress.toUint16()); + CHECK(msg.getLogicalAddress().value() == logicalAddress); CHECK(msg.getEid().has_value()); CHECK(msg.getEid().value().toString() == entityType.toString()); CHECK(msg.getGid().has_value()); @@ -199,7 +199,7 @@ TEST_SUITE("DoIPMessage") { TEST_CASE("Init from raw bytes - diagnostic message") { // Diag message with RDBI request - const uint8_t example_diag[] = {PROTOCOL_VERSION, PROTOCOL_VERSION_INV, 0x80, 0x01, 0x00, 0x00, 0x00, 0x07, DoIPAddress::MIN_SOURCE_ADDRESS >> 8, DoIPAddress::MIN_SOURCE_ADDRESS & 0xFF, 0xca, 0xfe, 0x22, 0xFD, 0x10}; + const uint8_t example_diag[] = {PROTOCOL_VERSION, PROTOCOL_VERSION_INV, 0x80, 0x01, 0x00, 0x00, 0x00, 0x07, MIN_SOURCE_ADDRESS >> 8, MIN_SOURCE_ADDRESS & 0xFF, 0xca, 0xfe, 0x22, 0xFD, 0x10}; auto opt_msg = DoIPMessage::tryParse(example_diag, sizeof(example_diag)); REQUIRE_MESSAGE(opt_msg.has_value(), "No message was created"); @@ -217,11 +217,11 @@ TEST_SUITE("DoIPMessage") { auto optSa = msg.getSourceAddress(); REQUIRE_MESSAGE(optSa.has_value(), "No source address extracted"); - CHECK(optSa->toUint16() == DoIPAddress::MIN_SOURCE_ADDRESS); + CHECK(optSa.value() == MIN_SOURCE_ADDRESS); auto optTa = msg.getTargetAddress(); REQUIRE_MESSAGE(optTa.has_value(), "No target address extracted"); - CHECK(optTa->toUint16() == 0xcafe); + CHECK(optTa.value() == 0xcafe); auto optPayload = msg.getPayload(); diff --git a/test/DoIPServer_Test.cpp b/test/DoIPServer_Test.cpp index 0111c29..5a5dce3 100644 --- a/test/DoIPServer_Test.cpp +++ b/test/DoIPServer_Test.cpp @@ -29,7 +29,7 @@ TEST_SUITE("DoIPServer Tests") { TEST_CASE_FIXTURE(DoIPServerFixture, "Set VIN Test") { std::string testVIN = "TESTVIN1234567890"; server.setVin(testVIN); - DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVin(), DoIPAddress::ZeroAddress, server.getEid(), DoIpGid::Zero, DoIPFurtherAction::NoFurtherAction); + DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVin(), ZERO_ADDRESS, server.getEid(), DoIpGid::Zero, DoIPFurtherAction::NoFurtherAction); ByteArrayRef payload = msg.getPayload(); // Check that the VIN in the payload matches the set VIN @@ -41,7 +41,7 @@ TEST_SUITE("DoIPServer Tests") { TEST_CASE_FIXTURE(DoIPServerFixture, "Set EID Test") { uint64_t testEID = 0x123456789ABC; server.setEid(testEID); - DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVin(), DoIPAddress::ZeroAddress, server.getEid(), DoIpGid::Zero, DoIPFurtherAction::NoFurtherAction); + DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVin(), ZERO_ADDRESS, server.getEid(), DoIpGid::Zero, DoIPFurtherAction::NoFurtherAction); ByteArrayRef payload = msg.getPayload(); // Check that the EID in the payload matches the set EID @@ -54,7 +54,7 @@ TEST_SUITE("DoIPServer Tests") { bool result = server.setDefaultEid(); CHECK(result == true); - DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVin(), DoIPAddress::ZeroAddress, server.getEid(), DoIpGid::Zero, DoIPFurtherAction::NoFurtherAction); + DoIPMessage msg = message::makeVehicleIdentificationResponse(server.getVin(), ZERO_ADDRESS, server.getEid(), DoIpGid::Zero, DoIPFurtherAction::NoFurtherAction); ByteArrayRef payload = msg.getPayload(); std::cerr << "EID set to: " << server.getEid().toHexString() << '\n'; diff --git a/test/VehicleIdentification_Test.cpp b/test/VehicleIdentification_Test.cpp index 53a898d..ccf5982 100644 --- a/test/VehicleIdentification_Test.cpp +++ b/test/VehicleIdentification_Test.cpp @@ -31,7 +31,7 @@ TEST_SUITE("VehicleIdentificationHandler") { * Checks if a VIN with 17 bytes matches correctly the input data */ TEST_CASE_FIXTURE(VehicleIdentificationHandlerFixture, "VIN 17 Bytes") { - DoIPMessage msg = message::makeVehicleIdentificationResponse(matchingVIN, DoIPAddress::ZeroAddress, EID, GID, furtherActionRequired); + DoIPMessage msg = message::makeVehicleIdentificationResponse(matchingVIN, ZERO_ADDRESS, EID, GID, furtherActionRequired); ByteArrayRef payload = msg.getPayload(); ByteArray expected{ // VIN (17 bytes) @@ -58,7 +58,7 @@ TEST_SUITE("VehicleIdentificationHandler") { TEST_CASE_FIXTURE(VehicleIdentificationHandlerFixture, "VIN Less Than 17 Bytes") { - DoIPMessage msg = message::makeVehicleIdentificationResponse(shortVIN, DoIPAddress::ZeroAddress, EID, GID, far_cs); + DoIPMessage msg = message::makeVehicleIdentificationResponse(shortVIN, ZERO_ADDRESS, EID, GID, far_cs); ByteArrayRef payload = msg.getPayload(); ByteArray expected{ // VIN (17 bytes) From 9e6af2d94928f5c55b16b94973c9a0bd6a339b11 Mon Sep 17 00:00:00 2001 From: Oliver Wieland Date: Tue, 9 Dec 2025 20:22:26 +0100 Subject: [PATCH 8/8] fix: Add missing initializer --- src/DoIPConnection.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/DoIPConnection.cpp b/src/DoIPConnection.cpp index be4b3fe..80e5b4e 100644 --- a/src/DoIPConnection.cpp +++ b/src/DoIPConnection.cpp @@ -10,6 +10,7 @@ namespace doip { DoIPConnection::DoIPConnection(int tcpSocket, UniqueServerModelPtr model) : DoIPDefaultConnection(std::move(model)), + m_logicalAddress(ZERO_ADDRESS), m_tcpSocket(tcpSocket) { }