etc

~ scripts and random things
git clone https://github.com/djnnvx/etc.git
doip.py (8932B)
      1 #!/usr/bin/env python3
      2 """
      3 Minimal DoIP / UDS client (ISO 13400, ISO 14229).
      4 
      5   ./doip.py discover
      6   ./doip.py info
      7   ./doip.py send 22 F1 90
      8   ./doip.py scan
      9   ./doip.py shell
     10 
     11 In shell: type hex to send UDS, .ta <addr> to retarget an ECU behind the
     12 gateway, .scan to enumerate them, .info, .q to quit.
     13 Raw hex is raw. 0x11 resets, 0x2E writes, 0x31 runs routines.
     14 """
     15 import argparse, socket, struct, sys
     16 
     17 V = 0x03
     18 ACT = {0x00: 'unknown source address', 0x01: 'all sockets active', 0x02: 'SA differs from activated',
     19        0x03: 'SA registered elsewhere', 0x04: 'missing authentication', 0x05: 'rejected confirmation',
     20        0x06: 'unsupported activation type', 0x07: 'encrypted link required',
     21        0x10: 'activated', 0x11: 'confirmation required'}
     22 NRC = {0x10: 'generalReject', 0x11: 'serviceNotSupported', 0x12: 'subFunctionNotSupported',
     23        0x13: 'incorrectMessageLengthOrInvalidFormat', 0x14: 'responseTooLong', 0x21: 'busyRepeatRequest',
     24        0x22: 'conditionsNotCorrect', 0x24: 'requestSequenceError', 0x25: 'noResponseFromSubnetComponent',
     25        0x31: 'requestOutOfRange', 0x33: 'securityAccessDenied', 0x34: 'authenticationRequired',
     26        0x35: 'invalidKey', 0x36: 'exceedNumberOfAttempts', 0x37: 'requiredTimeDelayNotExpired',
     27        0x70: 'uploadDownloadNotAccepted', 0x72: 'generalProgrammingFailure', 0x78: 'responsePending',
     28        0x7E: 'subFunctionNotSupportedInActiveSession', 0x7F: 'serviceNotSupportedInActiveSession'}
     29 DIDS = {0xF190: 'VIN', 0xF191: 'vehicleManufacturerECUHardwareNumber', 0xF193: 'hardwareVersion',
     30         0xF194: 'softwareIdentification', 0xF195: 'softwareVersion', 0xF197: 'systemNameOrEngineType',
     31         0xF18C: 'ECUSerialNumber', 0xF1A0: 'vehicleManufacturerSpecific', 0xF186: 'activeDiagnosticSession'}
     32 
     33 
     34 def hdr(pt, pl=b''):
     35     return struct.pack('>BBHI', V, 0xFF ^ V, pt, len(pl)) + pl
     36 
     37 
     38 def rd(sock):
     39     h = sock.recv(8)
     40     if len(h) < 8:
     41         return None, b''
     42     _, _, pt, ln = struct.unpack('>BBHI', h)
     43     b = b''
     44     while len(b) < ln:
     45         c = sock.recv(ln - len(b))
     46         if not c:
     47             break
     48         b += c
     49     return pt, b
     50 
     51 
     52 def udp(target, ptype, timeout=3):
     53     s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
     54     s.settimeout(timeout)
     55     s.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
     56     s.sendto(hdr(ptype), (target, 13400))
     57     try:
     58         d, a = s.recvfrom(4096)
     59     except socket.timeout:
     60         return None, b'', None
     61     _, _, pt, ln = struct.unpack('>BBHI', d[:8])
     62     return pt, d[8:8 + ln], a[0]
     63 
     64 
     65 def discover(bcast):
     66     pt, b, ip = udp(bcast, 0x0001)
     67     if pt != 0x0004:
     68         print('no vehicle announcement'); return None
     69     print(f'entity          : {ip}')
     70     print(f'VIN             : {b[0:17].decode("ascii", "replace")}')
     71     print(f'logical address : 0x{int.from_bytes(b[17:19], "big"):04X}')
     72     print(f'EID / GID       : {b[19:25].hex(":")} / {b[25:31].hex(":")}')
     73     fa = b[31]
     74     print(f'further action  : 0x{fa:02X}' + (' (routing activation required)' if fa else ' (none)'))
     75     return ip
     76 
     77 
     78 def info(target):
     79     for pt, name in ((0x4001, 'entity status'), (0x4003, 'power mode')):
     80         rt, b, _ = udp(target, pt)
     81         print(f'{name:15} : {b.hex() if b else "(no reply)"}')
     82         if rt == 0x4002 and len(b) >= 7:
     83             print(f'                  node_type=0x{b[0]:02X} max_sockets={b[1]} open={b[2]} '
     84                   f'max_data={int.from_bytes(b[3:7], "big")}')
     85 
     86 
     87 class Conn:
     88     def __init__(self, target, sa, timeout):
     89         self.sa, self.timeout = sa, timeout
     90         self.s = socket.create_connection((target, 13400), timeout)
     91         self.s.settimeout(timeout)
     92         self.s.sendall(hdr(0x0005, struct.pack('>HB', sa, 0) + b'\x00\x00\x00\x00'))
     93         pt, b = rd(self.s)
     94         if pt != 0x0006 or len(b) < 5:
     95             raise SystemExit(f'routing activation failed: ptype=0x{pt:04x} {b.hex()}')
     96         ta, ea, code = struct.unpack('>HHB', b[:5])
     97         print(f'[+] routing activation: tester=0x{ta:04X} entity=0x{ea:04X} code=0x{code:02X} ({ACT.get(code, "?")})')
     98         if code != 0x10:
     99             raise SystemExit('not activated')
    100 
    101     def uds(self, ta, payload, quiet=False):
    102         self.s.sendall(hdr(0x8001, struct.pack('>HH', self.sa, ta) + payload))
    103         while True:
    104             try:
    105                 pt, b = rd(self.s)
    106             except socket.timeout:
    107                 return None
    108             if pt == 0x0007:                      # AliveCheckRequest
    109                 self.s.sendall(hdr(0x0008, struct.pack('>H', self.sa)))
    110                 continue
    111             if pt == 0x8002:
    112                 continue
    113             if pt == 0x8003:
    114                 if not quiet:
    115                     print(f'    diag NACK 0x{b[4]:02X}' if len(b) > 4 else f'    diag NACK {b.hex()}')
    116                 return None
    117             if pt != 0x8001:
    118                 if not quiet:
    119                     print(f'    ptype=0x{pt:04x} {b.hex()}')
    120                 return None
    121             r = b[4:]
    122             if len(r) >= 3 and r[0] == 0x7F and r[2] == 0x78:
    123                 continue
    124             return r
    125 
    126 
    127 def show(r):
    128     if r is None:
    129         print('    (no response)')
    130     elif len(r) >= 3 and r[0] == 0x7F:
    131         print(f'    NEGATIVE svc=0x{r[1]:02X} nrc=0x{r[2]:02X} {NRC.get(r[2], "?")}')
    132     else:
    133         txt = ''.join(chr(c) if 32 <= c < 127 else '.' for c in r)
    134         print(f'    {r.hex()}\n    "{txt}"')
    135 
    136 
    137 def scan(c, lo=0x0000, hi=0x0FFF):
    138     """Pipelined: blast a window of TesterPresent then drain."""
    139     print(f'[*] scanning 0x{lo:04X}-0x{hi:04X} with 3E00 (TesterPresent)')
    140     found = set()
    141     for base in range(lo, hi + 1, 64):
    142         for ta in range(base, min(base + 64, hi + 1)):
    143             c.s.sendall(hdr(0x8001, struct.pack('>HH', c.sa, ta) + b'\x3e\x00'))
    144         c.s.settimeout(0.6)
    145         while True:
    146             try:
    147                 pt, b = rd(c.s)
    148             except socket.timeout:
    149                 break
    150             if pt == 0x8001 and len(b) >= 5:
    151                 src, r = int.from_bytes(b[0:2], 'big'), b[4:]
    152                 if r[:1] == b'\x7e':
    153                     found.add((src, 'positive'))
    154                 elif r[:1] == b'\x7f':
    155                     found.add((src, f'nrc=0x{r[2]:02X} {NRC.get(r[2], "?")}'))
    156     c.s.settimeout(c.timeout)
    157     print(f'[+] {len(found)} responding logical addresses:')
    158     for a, k in sorted(found):
    159         print(f'   0x{a:04X}  {k}')
    160     return [a for a, _ in sorted(found)]
    161 
    162 
    163 def ident(c, addrs):
    164     for ta in addrs:
    165         print(f'\n=== ECU 0x{ta:04X}')
    166         for d, n in DIDS.items():
    167             r = c.uds(ta, b'\x22' + struct.pack('>H', d), quiet=True)
    168             if r and r[0] == 0x62:
    169                 v = r[3:]
    170                 t = ''.join(chr(x) if 32 <= x < 127 else '.' for x in v)
    171                 print(f'  {d:04X} {n:34} "{t[:48]}"')
    172 
    173 
    174 def main():
    175     a = argparse.ArgumentParser()
    176     a.add_argument('cmd', choices=['discover', 'info', 'send', 'scan', 'ident', 'shell'])
    177     a.add_argument('data', nargs='*')
    178     a.add_argument('-t', '--target', default='127.0.0.1')
    179     a.add_argument('-b', '--broadcast', default='169.254.255.255')
    180     a.add_argument('-s', '--sa', default='0x0EF4')
    181     a.add_argument('-d', '--ta', default='0x0010')
    182     a.add_argument('--timeout', type=float, default=4.0)
    183     o = a.parse_args()
    184     sa, ta = int(o.sa, 0), int(o.ta, 0)
    185 
    186     if o.cmd == 'discover':
    187         discover(o.broadcast); return
    188     if o.cmd == 'info':
    189         info(o.target); return
    190 
    191     c = Conn(o.target, sa, o.timeout)
    192     if o.cmd == 'send':
    193         show(c.uds(ta, bytes.fromhex(''.join(o.data))))
    194     elif o.cmd == 'scan':
    195         scan(c)
    196     elif o.cmd == 'ident':
    197         ident(c, [int(x, 0) for x in o.data] or scan(c))
    198     else:
    199         print('hex to send, .ta <addr>, .scan, .ident, .info, .dids, .q')
    200         while True:
    201             try:
    202                 line = input(f'0x{ta:04X}> ').strip()
    203             except (EOFError, KeyboardInterrupt):
    204                 break
    205             if not line:
    206                 continue
    207             if line in ('.q', '.quit'):
    208                 break
    209             if line.startswith('.ta'):
    210                 ta = int(line.split()[1], 0); continue
    211             if line == '.scan':
    212                 scan(c); continue
    213             if line == '.ident':
    214                 ident(c, [ta]); continue
    215             if line == '.info':
    216                 info(o.target); continue
    217             if line == '.dids':
    218                 for d, n in DIDS.items():
    219                     r = c.uds(ta, b'\x22' + struct.pack('>H', d), quiet=True)
    220                     if r and r[0] == 0x62:
    221                         v = r[3:]
    222                         txt = ''.join(chr(x) if 32 <= x < 127 else '.' for x in v)
    223                         print(f'  {d:04X} {n:42} {txt}')
    224                 continue
    225             try:
    226                 show(c.uds(ta, bytes.fromhex(line.replace(' ', ''))))
    227             except ValueError:
    228                 print('  not hex')
    229 
    230 
    231 if __name__ == '__main__':
    232     sys.exit(main())