forked from commaai/panda
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathisotp.py
135 lines (113 loc) · 3.28 KB
/
isotp.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
DEBUG = False
def msg(x):
if DEBUG:
print "S:",x.encode("hex")
if len(x) <= 7:
ret = chr(len(x)) + x
else:
assert False
return ret.ljust(8, "\x00")
kmsgs = []
def recv(panda, cnt, addr, nbus):
global kmsgs
ret = []
while len(ret) < cnt:
kmsgs += panda.can_recv()
nmsgs = []
for ids, ts, dat, bus in kmsgs:
if ids == addr and bus == nbus and len(ret) < cnt:
ret.append(dat)
else:
# leave around
nmsgs.append((ids, ts, dat, bus))
kmsgs = nmsgs[-256:]
return map(str, ret)
def isotp_recv_subaddr(panda, addr, bus, sendaddr, subaddr):
msg = recv(panda, 1, addr, bus)[0]
# TODO: handle other subaddr also communicating
assert ord(msg[0]) == subaddr
if ord(msg[1])&0xf0 == 0x10:
# first
tlen = ((ord(msg[1]) & 0xf) << 8) | ord(msg[2])
dat = msg[3:]
# 0 block size?
CONTINUE = chr(subaddr) + "\x30" + "\x00"*6
panda.can_send(sendaddr, CONTINUE, bus)
idx = 1
for mm in recv(panda, (tlen-len(dat) + 5)/6, addr, bus):
assert ord(mm[0]) == subaddr
assert ord(mm[1]) == (0x20 | (idx&0xF))
dat += mm[2:]
idx += 1
elif ord(msg[1])&0xf0 == 0x00:
# single
tlen = ord(msg[1]) & 0xf
dat = msg[2:]
else:
print msg.encode("hex")
assert False
return dat[0:tlen]
# **** import below this line ****
def isotp_send(panda, x, addr, bus=0, recvaddr=None, subaddr=None):
if recvaddr is None:
recvaddr = addr+8
if len(x) <= 7 and subaddr is None:
panda.can_send(addr, msg(x), bus)
elif len(x) <= 6 and subaddr is not None:
panda.can_send(addr, chr(subaddr)+msg(x)[0:7], bus)
else:
if subaddr:
ss = chr(subaddr) + chr(0x10 + (len(x)>>8)) + chr(len(x)&0xFF) + x[0:5]
x = x[5:]
else:
ss = chr(0x10 + (len(x)>>8)) + chr(len(x)&0xFF) + x[0:6]
x = x[6:]
idx = 1
sends = []
while len(x) > 0:
if subaddr:
sends.append(((chr(subaddr) + chr(0x20 + (idx&0xF)) + x[0:6]).ljust(8, "\x00")))
x = x[6:]
else:
sends.append(((chr(0x20 + (idx&0xF)) + x[0:7]).ljust(8, "\x00")))
x = x[7:]
idx += 1
# actually send
panda.can_send(addr, ss, bus)
rr = recv(panda, 1, recvaddr, bus)[0]
if rr.find("\x30\x01") != -1:
for s in sends[:-1]:
panda.can_send(addr, s, 0)
rr = recv(panda, 1, recvaddr, bus)[0]
panda.can_send(addr, sends[-1], 0)
else:
panda.can_send_many([(addr, None, s, 0) for s in sends])
def isotp_recv(panda, addr, bus=0, sendaddr=None, subaddr=None):
if sendaddr is None:
sendaddr = addr-8
if subaddr is not None:
dat = isotp_recv_subaddr(panda, addr, bus, sendaddr, subaddr)
else:
msg = recv(panda, 1, addr, bus)[0]
if ord(msg[0])&0xf0 == 0x10:
# first
tlen = ((ord(msg[0]) & 0xf) << 8) | ord(msg[1])
dat = msg[2:]
# 0 block size?
CONTINUE = "\x30" + "\x00"*7
panda.can_send(sendaddr, CONTINUE, bus)
idx = 1
for mm in recv(panda, (tlen-len(dat) + 6)/7, addr, bus):
assert ord(mm[0]) == (0x20 | (idx&0xF))
dat += mm[1:]
idx += 1
elif ord(msg[0])&0xf0 == 0x00:
# single
tlen = ord(msg[0]) & 0xf
dat = msg[1:]
else:
assert False
dat = dat[0:tlen]
if DEBUG:
print "R:",dat.encode("hex")
return dat