diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ea8c4bf --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +/target diff --git a/doip-server-poc/Cargo.lock b/doip-server-poc/Cargo.lock new file mode 100644 index 0000000..5f3d20f --- /dev/null +++ b/doip-server-poc/Cargo.lock @@ -0,0 +1,7 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "DoIPServer-POC" +version = "0.1.0" diff --git a/doip-server-poc/Cargo.toml b/doip-server-poc/Cargo.toml new file mode 100644 index 0000000..06e78a3 --- /dev/null +++ b/doip-server-poc/Cargo.toml @@ -0,0 +1,6 @@ +[package] +name = "DoIPServer-POC" +version = "0.1.0" +edition = "2024" + +[dependencies] diff --git a/doip-server-poc/README.md b/doip-server-poc/README.md new file mode 100644 index 0000000..2d7ccab --- /dev/null +++ b/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 diff --git a/doip-server-poc/flowchartw/flowchart.md b/doip-server-poc/flowchartw/flowchart.md new file mode 100644 index 0000000..7d437d8 --- /dev/null +++ b/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 diff --git a/doip-server-poc/src/main.rs b/doip-server-poc/src/main.rs new file mode 100644 index 0000000..830f962 --- /dev/null +++ b/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(()) +} diff --git a/doip-server-poc/test_doip.py b/doip-server-poc/test_doip.py new file mode 100644 index 0000000..2a91f80 --- /dev/null +++ b/doip-server-poc/test_doip.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python3 +"""DoIP Server Test Script""" + +import socket +import time + +HOST = '127.0.0.1' +PORT = 13400 + + +def test_udp_vehicle_discovery(): + """Test UDP Vehicle Identification Request""" + print("\n=== UDP Vehicle Discovery Test ===") + + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.settimeout(3) + + # DoIP Vehicle Identification Request (payload type 0x0001) + request = bytes([ + 0x02, 0xFD, # Protocol version + inverse + 0x00, 0x01, # Payload type: Vehicle ID Request + 0x00, 0x00, 0x00, 0x00 # Payload length: 0 + ]) + + print(f"Sending: {request.hex()}") + sock.sendto(request, (HOST, PORT)) + + try: + response, addr = sock.recvfrom(1024) + print(f"Response from {addr}: {response.hex()}") + print(f" Vehicle ID: {response[8:16].decode('ascii', errors='ignore')}") + except socket.timeout: + print("No response (timeout)") + + sock.close() + + +def test_tcp_routing_activation(): + """Test TCP Routing Activation""" + print("\n=== TCP Routing Activation Test ===") + + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sock.settimeout(5) + sock.connect((HOST, PORT)) + print(f"Connected to {HOST}:{PORT}") + + # DoIP Routing Activation Request (payload type 0x0005) + request = bytes([ + 0x02, 0xFD, # Protocol version + inverse + 0x00, 0x05, # Payload type: Routing Activation Request + 0x00, 0x00, 0x00, 0x07, # Payload length: 7 + 0x0E, 0x00, # Source address (tester) + 0x00, # Activation type + 0x00, 0x00, 0x00, 0x00 # Reserved + ]) + + print(f"Sending Routing Activation: {request.hex()}") + sock.send(request) + + response = sock.recv(1024) + print(f"Response: {response.hex()}") + + if len(response) >= 13: + payload_type = int.from_bytes(response[2:4], 'big') + if payload_type == 0x0006: + print(" ✓ Routing Activation Response received") + activation_code = response[12] + print(f" Activation code: 0x{activation_code:02X}") + + return sock + + +def test_tcp_diagnostic_message(sock): + """Test TCP Diagnostic Message (requires active routing)""" + print("\n=== TCP Diagnostic Message Test ===") + + # UDS: Read Data By Identifier (0x22) - Read ECU Serial Number (0xF18C) + uds_payload = bytes([0x22, 0xF1, 0x8C]) + + payload_length = 4 + len(uds_payload) # src(2) + tgt(2) + uds + + # DoIP Diagnostic Message (payload type 0x8001) + request = bytes([ + 0x02, 0xFD, # Protocol version + inverse + 0x80, 0x01, # Payload type: Diagnostic Message + 0x00, 0x00, 0x00, payload_length, + 0x0E, 0x00, # Source address (tester) + 0x10, 0x00, # Target address (ECU) + ]) + uds_payload + + print(f"Sending Diagnostic (UDS 0x22): {request.hex()}") + sock.send(request) + + response = sock.recv(1024) + print(f"Response: {response.hex()}") + + if len(response) >= 12: + payload_type = int.from_bytes(response[2:4], 'big') + if payload_type == 0x8002: + print(" ✓ Diagnostic Response received") + uds_response = response[12:] + print(f" UDS Response: {uds_response.hex()}") + if uds_response[0] == 0x7F: + print(f" Negative Response: Service 0x{uds_response[1]:02X}, NRC 0x{uds_response[2]:02X}") + + +def main(): + print("DoIP Server Test Script") + print("=" * 40) + print(f"Target: {HOST}:{PORT}") + + # Test 1: UDP Discovery + try: + test_udp_vehicle_discovery() + except Exception as e: + print(f"UDP test failed: {e}") + + time.sleep(0.5) + + # Test 2: TCP Routing + Diagnostic + try: + sock = test_tcp_routing_activation() + time.sleep(0.5) + test_tcp_diagnostic_message(sock) + sock.close() + except Exception as e: + print(f"TCP test failed: {e}") + + print("\n=== Tests Complete ===") + + +if __name__ == "__main__": + main()