DoIP Server POC implementation

.gitignore

@@ -0,0 +1 @@
+/target

doip-server-poc/Cargo.toml

@@ -0,0 +1,6 @@
+[package]
+name = "DoIPServer-POC"
+version = "0.1.0"
+edition = "2024"
+
+[dependencies]

doip-server-poc/README.md

@@ -0,0 +1,132 @@
+# DoIP Server POC
+
+A minimal Diagnostics over IP (DoIP) server implementation in Rust for proof-of-concept and testing purposes.
+
+## Overview
+
+This project implements a DoIP server according to ISO 13400-2, supporting:
+- **UDP Vehicle Identification** (Discovery)
+- **TCP Routing Activation** (Session establishment)
+- **TCP Diagnostic Messages** (UDS over DoIP)
+
+## Architecture
+
+```
+┌──────────────┐     UDP/TCP      ┌──────────────────┐
+│  UDS Tester  │ ───────────────► │  DoIP Server     │
+│  (Client)    │     Port 13400   │  (This Project)  │
+│              │ ◄─────────────── │                  │
+└──────────────┘                  └──────────────────┘
+```
+
+## Features
+
+| Feature | Payload Type | Status |
+|---------|--------------|--------|
+| Vehicle Identification Request | 0x0001 | ✅ |
+| Vehicle Identification Response | 0x0004 | ✅ |
+| Routing Activation Request | 0x0005 | ✅ |
+| Routing Activation Response | 0x0006 | ✅ |
+| Diagnostic Message | 0x8001 | ✅ |
+| Diagnostic Message Response | 0x8002 | ✅ |
+
+## Prerequisites
+
+- Rust 1.70+ ([Install Rust](https://rustup.rs/))
+- Python 3.x (for testing)
+
+## Build & Run
+
+```bash
+# Build
+cargo build
+
+# Run
+cargo run
+```
+
+The server listens on:
+- **UDP 13400** - Vehicle discovery
+- **TCP 13400** - Diagnostic communication
+
+## Testing
+
+### Using the Python Test Script
+
+```bash
+python3 test_doip.py
+```
+
+### Manual Testing with netcat
+
+**UDP Discovery:**
+```bash
+echo -ne '\x02\xFD\x00\x01\x00\x00\x00\x00' | nc -u 127.0.0.1 13400
+```
+
+**TCP Routing Activation:**
+```bash
+echo -ne '\x02\xFD\x00\x05\x00\x00\x00\x07\x0E\x00\x00\x00\x00\x00\x00' | nc 127.0.0.1 13400
+```
+
+## Protocol Flow
+
+### 1. Vehicle Discovery (UDP)
+```
+Tester → Server: Vehicle Identification Request (0x0001)
+Server → Tester: Vehicle Identification Response (0x0004)
+```
+
+### 2. Session Establishment (TCP)
+```
+Tester → Server: TCP Connect (port 13400)
+Tester → Server: Routing Activation Request (0x0005)
+Server → Tester: Routing Activation Response (0x0006)
+```
+
+### 3. Diagnostic Communication (TCP)
+```
+Tester → Server: Diagnostic Message (0x8001) + UDS payload
+Server → Tester: Diagnostic Response (0x8002) + UDS response
+```
+
+## DoIP Message Structure
+
+| Offset | Size | Field |
+|--------|------|-------|
+| 0 | 1 | Protocol Version (0x02) |
+| 1 | 1 | Inverse Version (0xFD) |
+| 2-3 | 2 | Payload Type (big-endian) |
+| 4-7 | 4 | Payload Length (big-endian) |
+| 8+ | N | Payload Data |
+
+## Configuration
+
+| Parameter | Value | Description |
+|-----------|-------|-------------|
+| Port | 13400 | DoIP standard port |
+| Logical Address | 0x1000 | ECU address |
+| Vehicle ID | "DOIP-ECU" | Identification string |
+
+## Project Structure
+
+```
+DoIPServer-POC/
+├── Cargo.toml          # Rust dependencies
+├── README.md           # This file
+├── src/
+│   └── main.rs         # Server implementation
+├── test_doip.py        # Python test script
+└── flowchartw/
+    └── flowchart.md    # Protocol flowchart
+```
+
+## Limitations (POC Scope)
+
+- Single TCP client support
+- Dummy UDS responses (NRC 0x11 - Service Not Supported)
+- No TLS/security
+- Hardcoded configuration
+## License
+
+MIT License

doip-server-poc/flowchartw/flowchart.md

@@ -0,0 +1,31 @@
+flowchart TD
+
+%% -------------------------
+%% Minimal POC (TCP only)
+%% -------------------------
+subgraph POC_Minimal["Minimal POC — TCP Only (Implemented)"]
+    A[UDS Tester]
+    B[DoIP Server]
+
+    A -->|1. TCP Connect\n(port 13400)| B
+    A -->|2. Routing Activation Request\n(DoIP, TCP)| B
+    B -->|3. Routing Activation Response\n(DoIP, TCP)| A
+    A -->|4. Diagnostic Message\n(UDS inside DoIP, TCP)| B
+    B -->|5. Diagnostic Response\n(DoIP, TCP)| A
+end
+
+%% -------------------------
+%% Production Flow (With UDP)
+%% -------------------------
+subgraph Production["Production Flow — With UDP (Implemented)"]
+    C[UDS Tester]
+    D[DoIP Server]
+
+    C -->|1. Vehicle Identification Request\n(DoIP, UDP)| D
+    D -->|2. Vehicle Identification Response\n(DoIP, UDP)| C
+    C -->|3. TCP Connect\n(port 13400)| D
+    C -->|4. Routing Activation Request\n(DoIP, TCP)| D
+    D -->|5. Routing Activation Response\n(DoIP, TCP)| C
+    C -->|6. Diagnostic Message\n(UDS inside DoIP, TCP)| D
+    D -->|7. Diagnostic Response\n(DoIP, TCP)| C
+end

doip-server-poc/src/main.rs

@@ -0,0 +1,212 @@
+#![allow(dead_code)]
+
+use std::io::{Read, Write};
+use std::net::{TcpListener, TcpStream, UdpSocket};
+use std::thread;
+
+/* ---------------------------------------------------------
+ * DoIP: Routing Activation (TCP)
+ * --------------------------------------------------------- */
+fn send_routing_activation_response(
+    stream: &mut TcpStream,
+) -> std::io::Result<()> {
+    // DoIP header
+    let version: u8 = 0x02;
+    let inverse_version: u8 = 0xFD;
+    let payload_type: u16 = 0x0006; // Routing Activation Response
+    let payload_length: u32 = 9;
+
+    // Minimal positive response payload
+    let payload: [u8; 9] = [
+        0x10, 0x00, // DoIP entity logical address
+        0x00, 0x00, // Reserved
+        0x10,       // Routing activation successful
+        0x00, 0x00, 0x00, 0x00,
+    ];
+
+    let mut message = Vec::with_capacity(8 + payload.len());
+    message.push(version);
+    message.push(inverse_version);
+    message.extend_from_slice(&payload_type.to_be_bytes());
+    message.extend_from_slice(&payload_length.to_be_bytes());
+    message.extend_from_slice(&payload);
+
+    stream.write_all(&message)?;
+    println!("Routing Activation Response sent");
+
+    Ok(())
+}
+
+/* ---------------------------------------------------------
+ * DoIP: Diagnostic Message Handling
+ * --------------------------------------------------------- */
+fn parse_diagnostic_message(
+    buffer: &[u8],
+    bytes_read: usize,
+) -> Option<(u16, u16, u8)> {
+    // Minimum length:
+    // 8 bytes DoIP header + 2 src + 2 tgt + 1 UDS
+    if bytes_read < 13 {
+        println!("Diagnostic message too short");
+        return None;
+    }
+
+    let source = u16::from_be_bytes([buffer[8], buffer[9]]);
+    let target = u16::from_be_bytes([buffer[10], buffer[11]]);
+    let uds_payload = &buffer[12..bytes_read];
+
+    if uds_payload.is_empty() {
+        println!("Empty UDS payload");
+        return None;
+    }
+
+    println!(
+        "Diagnostic Message:\n  Source: 0x{:04X}\n  Target: 0x{:04X}\n  UDS: {:02X?}",
+        source, target, uds_payload
+    );
+
+    Some((source, target, uds_payload[0]))
+}
+
+fn send_dummy_diagnostic_response(
+    stream: &mut TcpStream,
+    source: u16,
+    target: u16,
+    service_id: u8,
+) -> std::io::Result<()> {
+    // NRC: Service Not Supported (0x11)
+    let uds_response: [u8; 3] = [0x7F, service_id, 0x11];
+
+    let version: u8 = 0x02;
+    let inverse_version: u8 = 0xFD;
+    let payload_type: u16 = 0x8002; // Diagnostic Response
+    let payload_length: u32 = 4 + uds_response.len() as u32;
+
+    let mut message =
+        Vec::with_capacity(8 + 4 + uds_response.len());
+
+    message.push(version);
+    message.push(inverse_version);
+    message.extend_from_slice(&payload_type.to_be_bytes());
+    message.extend_from_slice(&payload_length.to_be_bytes());
+
+    // Swap addresses (server → tester)
+    message.extend_from_slice(&target.to_be_bytes());
+    message.extend_from_slice(&source.to_be_bytes());
+    message.extend_from_slice(&uds_response);
+
+    stream.write_all(&message)?;
+    println!("Dummy diagnostic response sent");
+
+    Ok(())
+}
+
+/* ---------------------------------------------------------
+ * DoIP: UDP Vehicle Identification
+ * --------------------------------------------------------- */
+fn run_udp_vehicle_identification() -> std::io::Result<()> {
+    let socket = UdpSocket::bind("0.0.0.0:13400")?;
+    println!("UDP discovery listening on port 13400");
+
+    let mut buffer = [0u8; 1024];
+
+    loop {
+        let (size, sender) = socket.recv_from(&mut buffer)?;
+        if size < 8 {
+            continue; // Ignore malformed packets
+        }
+
+        println!(
+            "Vehicle Identification request from {}",
+            sender
+        );
+
+        // Static Vehicle Identification Response
+        let response: [u8; 18] = [
+            0x02, 0xFD,             // Version + inverse
+            0x00, 0x04,             // Vehicle ID Response
+            0x00, 0x00, 0x00, 0x0A, // Payload length (10 bytes)
+            b'D', b'O', b'I', b'P', b'-', b'E', b'C', b'U',
+            0x10, 0x00,             // Logical address
+        ];
+
+        socket.send_to(&response, sender)?;
+        println!("Vehicle Identification Response sent");
+    }
+}
+
+/* ---------------------------------------------------------
+ * Main
+ * --------------------------------------------------------- */
+fn main() -> std::io::Result<()> {
+    // Start UDP discovery in parallel (production merge)
+    thread::spawn(|| {
+        if let Err(e) = run_udp_vehicle_identification() {
+            eprintln!("UDP error: {}", e);
+        }
+    });
+
+    // TCP DoIP server (existing POC)
+    let listener = TcpListener::bind("0.0.0.0:13400")?;
+    println!("DoIP TCP Server listening on port 13400");
+
+    let (mut stream, client) = listener.accept()?;
+    println!("TCP client connected from {}", client);
+
+    let mut buffer = [0u8; 1024];
+    let mut routing_activated = false;
+
+    loop {
+        let bytes_read = stream.read(&mut buffer)?;
+        if bytes_read == 0 {
+            println!("Client disconnected");
+            break;
+        }
+
+        println!(
+            "Received {} bytes: {:02X?}",
+            bytes_read,
+            &buffer[..bytes_read]
+        );
+
+        if bytes_read < 8 {
+            continue;
+        }
+
+        let payload_type =
+            u16::from_be_bytes([buffer[2], buffer[3]]);
+        println!("DoIP Payload Type: 0x{:04X}", payload_type);
+
+        match payload_type {
+            0x0005 => {
+                println!("Routing Activation Request received");
+                send_routing_activation_response(&mut stream)?;
+                routing_activated = true;
+            }
+            0x8001 => {
+                if !routing_activated {
+                    println!(
+                        "Diagnostic received before routing activation — ignored"
+                    );
+                    continue;
+                }
+
+                if let Some((src, tgt, sid)) =
+                    parse_diagnostic_message(&buffer, bytes_read)
+                {
+                    send_dummy_diagnostic_response(
+                        &mut stream,
+                        src,
+                        tgt,
+                        sid,
+                    )?;
+                }
+            }
+            _ => {
+                println!("Unhandled DoIP payload type");
+            }
+        }
+    }
+
+    Ok(())
+}