despite it being bad code, move isotp

python/__init__.py

@@ -13,6 +13,7 @@ from esptool import ESPROM, CesantaFlasher
 from flash_release import flash_release
 from update import ensure_st_up_to_date
 from serial import PandaSerial
+from isotp import isotp_send, isotp_recv
 
 __version__ = '0.0.6'
 

python/isotp.py

@@ -0,0 +1,135 @@
+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)
+      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)
+        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
+