Hands-On: Complete UDS Sequence

#!/usr/bin/env python3
# Complete UDS reprogramming sequence via simulated DoIP
# Runs against ECU stub (Exercise 2) or real hardware via python-can

import struct, socket, time, hashlib

# DoIP client simplified (see ethernet course for full implementation)
class UdsTester:
    def __init__(self, ip: str, port: int = 13400):
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.connect((ip, port))
        self.sock.settimeout(10.0)
        self._activate_routing()

    def _activate_routing(self):
        payload = struct.pack(">HBI", 0xE000, 0x00, 0)
        hdr = struct.pack(">BBHI", 0x02, 0xFD, 0x0005, len(payload))
        self.sock.sendall(hdr + payload)
        self.sock.recv(256)  # routing activation response

    def send_uds(self, data: bytes, timeout: float = 5.0) -> bytes:
        payload = struct.pack(">HH", 0xE000, 0x0001) + data
        hdr = struct.pack(">BBHI", 0x02, 0xFD, 0x8001, len(payload))
        self.sock.sendall(hdr + payload)
        self.sock.settimeout(timeout)
        while True:
            resp = self.sock.recv(65536)
            ptype = struct.unpack(">H", resp[2:4])[0]
            if ptype == 0x8001:
                uds = resp[12:]
                if len(uds) >= 3 and uds[0] == 0x7F and uds[2] == 0x78:
                    continue  # 0x78 responsePending: wait for real response
                return uds

def reprogram_ecu(firmware: bytes, target_ip: str = "127.0.0.1"):
    t = UdsTester(target_ip)
    crc32 = struct.pack(">I", 0xFFFFFFFF & ~int.from_bytes(
        hashlib.new("crc32c", firmware).digest(), "big"))

    steps = [
        ("Extended Session", bytes([0x10, 0x03])),
        ("CommControl OFF",  bytes([0x28, 0x03, 0x01])),
        ("SA L1 Request",    bytes([0x27, 0x01])),
    ]
    for name, req in steps:
        resp = t.send_uds(req)
        print(f"  {name}: {resp.hex()}")
        assert resp[0] not in [0x7F], f"FAILED: {name}"

    # SecurityAccess Level 1: compute key from seed
    seed = struct.unpack(">I", t.send_uds(bytes([0x27, 0x01]))[2:6])[0]
    key  = seed ^ 0xDEADC0DE  # dev key — replace with HMAC in production
    resp = t.send_uds(bytes([0x27, 0x02]) + struct.pack(">I", key))
    assert resp[0] == 0x67, "Security L1 failed"
    print("  Security L1: granted")

    # Enter Programming Session
    resp = t.send_uds(bytes([0x10, 0x02]))
    assert resp[0] == 0x50, "Programming session failed"

    # Security Level 2 (same pattern, sub-functions 0x03/0x04)
    seed2 = struct.unpack(">I", t.send_uds(bytes([0x27, 0x03]))[2:6])[0]
    key2  = seed2 ^ 0xC0DEBABE
    resp  = t.send_uds(bytes([0x27, 0x04]) + struct.pack(">I", key2))
    assert resp[0] == 0x67, "Security L2 failed"
    print("  Security L2: granted")

    # Erase (30s timeout — erase may take 10-30s)
    print("  Erasing...", end="", flush=True)
    erase_req = bytes([0x31,0x01,0xFF,0x00,0x80,0x02,0x00,0x00,0x00,0x78,0x00,0x00])
    resp = t.send_uds(erase_req, timeout=35.0)
    assert resp[0] == 0x71, "Erase failed"
    print(" done")

    # RequestDownload
    addr = 0x80020000
    total = len(firmware)
    rd = bytes([0x34,0x00,0x44])+struct.pack(">II", addr, total)
    resp = t.send_uds(rd)
    assert resp[0] == 0x74, "RequestDownload failed"
    max_block = struct.unpack(">I", b'' + resp[2:5])[0]
    data_per_block = max_block - 2
    print(f"  Block size: {data_per_block} bytes")

    # TransferData
    block = 1; offset = 0
    while offset < total:
        chunk = firmware[offset:offset+data_per_block]
        resp  = t.send_uds(bytes([0x36, block]) + chunk)
        assert resp[0] == 0x76, f"TransferData block {block} failed"
        offset += len(chunk); block = (block % 0xFF) + 1
    print(f"  Transferred {total} bytes in {block-1} blocks")

    # RequestTransferExit + CheckProgramming + Reset
    t.send_uds(bytes([0x37]))
    resp = t.send_uds(bytes([0x31,0x01,0xFF,0x01]))
    assert resp[0] == 0x71, "CheckProgramming failed"
    t.send_uds(bytes([0x11, 0x03]))
    print("  ECU reset — programming complete!")

reprogram_ecu(b"\xDE\xAD\xC0\xDE" + b"\xAB" * (10240 - 4))