isotp untested support for subaddr

examples/isotp.py

@@ -26,57 +26,99 @@ def recv(panda, cnt, addr, nbus):
     kmsgs = nmsgs[-256:]
   return map(str, ret)
 
-def isotp_send(panda, x, addr, bus=0, recvaddr=None):
+def isotp_send(panda, x, addr, bus=0, recvaddr=None, subaddr=None):
   if recvaddr is None:
     recvaddr = addr+8
 
-  if len(x) <= 7:
+  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:
-    ss = chr(0x10 + (len(x)>>8)) + chr(len(x)&0xFF) + x[0:6]
-    x = x[6:]
+    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:
-      sends.append(((chr(0x20 + (idx&0xF)) + x[0:7]).ljust(8, "\x00")))
-      x = x[7:]
+      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(panda, addr, bus=0, sendaddr=None):
+def isotp_recv_subaddr(panda, addr, bus, sendaddr, subaddr):
   msg = recv(panda, 1, addr, bus)[0]
 
-  if sendaddr is None:
-    sendaddr = addr-8
-
+  # TODO: handle other subaddr also communicating 
+  assert ord(msg[0]) == chr(subaddr)
 
-  if ord(msg[0])&0xf0 == 0x10:
+  if ord(msg[1])&0xf0 == 0x10:
     # first
-    tlen = ((ord(msg[0]) & 0xf) << 8) | ord(msg[1])
-    dat = msg[2:]
+    tlen = ((ord(msg[1]) & 0xf) << 8) | ord(msg[2])
+    dat = msg[3:]
 
     # 0 block size?
-    CONTINUE = "\x30" + "\x00"*7
-
+    CONTINUE = chr(subaddr) + "\x30" + "\x00"*6
     panda.can_send(sendaddr, CONTINUE, bus)
 
     idx = 1
-    for mm in recv(panda, (tlen-len(dat) + 7)/8, addr, bus):
-      assert ord(mm[0]) == (0x20 | idx)
-      dat += mm[1:]
+    for mm in recv(panda, (tlen-len(dat) + 5)/6, addr, bus):
+      assert ord(mm[0]) == chr(subaddr)
+      assert ord(mm[1]) == (0x20 | idx)
+      dat += mm[2:]
       idx += 1
-  elif ord(msg[0])&0xf0 == 0x00:
+  elif ord(msg[1])&0xf0 == 0x00:
     # single
-    tlen = ord(msg[0]) & 0xf
-    dat = msg[1:]
+    tlen = ord(msg[1]) & 0xf
+    dat = msg[2:]
   else:
     assert False
 
-  dat = dat[0:tlen]
+  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")