-
Notifications
You must be signed in to change notification settings - Fork 8.7k
/
isotp.py
127 lines (105 loc) · 3.02 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
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_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]
panda.can_send_many([(addr, None, s, 0) for s in sends])
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)
dat += mm[2:]
idx += 1
elif ord(msg[1])&0xf0 == 0x00:
# single
tlen = ord(msg[1]) & 0xf
dat = msg[2:]
else:
assert False
return dat[0:tlen]
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)
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