Merge branch 'master' of https://github.com/sunnypilot/panda

python/__init__.py

@@ -116,6 +116,7 @@ class Panda:
   HW_TYPE_RED_PANDA = b'\x07'
   HW_TYPE_TRES = b'\x09'
   HW_TYPE_CUATRO = b'\x0a'
+  HW_TYPE_BODY = b'\xb1'
 
   CAN_PACKET_VERSION = 4
   HEALTH_PACKET_VERSION = 17
@@ -124,7 +125,7 @@ class Panda:
   CAN_HEALTH_STRUCT = struct.Struct("<BIBBBBBBBBIIIIIIIHHBBBIIII")
 
   F4_DEVICES = [HW_TYPE_WHITE, HW_TYPE_BLACK]
-  H7_DEVICES = [HW_TYPE_RED_PANDA, HW_TYPE_TRES, HW_TYPE_CUATRO]
+  H7_DEVICES = [HW_TYPE_RED_PANDA, HW_TYPE_TRES, HW_TYPE_CUATRO, HW_TYPE_BODY]
   SUPPORTED_DEVICES = H7_DEVICES
 
   INTERNAL_DEVICES = (HW_TYPE_TRES, HW_TYPE_CUATRO)
@@ -743,14 +744,14 @@ class Panda:
 
   # ******************* serial *******************
 
-  def serial_read(self, port_number):
-    ret = []
+  def serial_read(self, port_number, maxlen=1024):
+    ret = b''
     while 1:
-      lret = bytes(self._handle.controlRead(Panda.REQUEST_IN, 0xe0, port_number, 0, 0x40))
-      if len(lret) == 0:
+      r = bytes(self._handle.controlRead(Panda.REQUEST_IN, 0xe0, port_number, 0, 0x40))
+      if len(r) == 0 or len(ret) >= maxlen:
         break
-      ret.append(lret)
-    return b''.join(ret)
+      ret += r
+    return ret
 
   def serial_write(self, port_number, ln):
     ret = 0

python/socketpanda.py

@@ -1,5 +1,6 @@
 import socket
 import struct
+import time
 
 # /**
 #  * struct canfd_frame - CAN flexible data rate frame structure
@@ -23,6 +24,9 @@ CAN_HEADER_LEN = struct.calcsize(CAN_HEADER_FMT)
 CAN_MAX_DLEN = 8
 CANFD_MAX_DLEN = 64
 
+CAN_CONFIRM_FLAG = 0x800
+CAN_EFF_FLAG = 0x80000000
+
 CANFD_BRS = 0x01 # bit rate switch (second bitrate for payload data)
 CANFD_FDF = 0x04 # mark CAN FD for dual use of struct canfd_frame
 
@@ -32,11 +36,12 @@ SO_RXQ_OVFL = 40
 
 import typing
 @typing.no_type_check # mypy struggles with macOS here...
-def create_socketcan(interface:str, recv_buffer_size:int, fd:bool) -> socket.socket:
+def create_socketcan(interface:str, recv_buffer_size:int) -> socket.socket:
   # settings mostly from https://github.com/linux-can/can-utils/blob/master/candump.c
   socketcan = socket.socket(socket.AF_CAN, socket.SOCK_RAW, socket.CAN_RAW)
-  if fd:
-    socketcan.setsockopt(socket.SOL_CAN_RAW, socket.CAN_RAW_FD_FRAMES, 1)
+  socketcan.setblocking(False)
+  socketcan.setsockopt(socket.SOL_CAN_RAW, socket.CAN_RAW_FD_FRAMES, 1)
+  socketcan.setsockopt(socket.SOL_CAN_RAW, socket.CAN_RAW_RECV_OWN_MSGS, 1)
   socketcan.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, recv_buffer_size)
   # TODO: why is it always 2x the requested size?
   assert socketcan.getsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF) == recv_buffer_size * 2
@@ -47,50 +52,72 @@ def create_socketcan(interface:str, recv_buffer_size:int, fd:bool) -> socket.soc
 
 # Panda class substitute for socketcan device (to support using the uds/iso-tp/xcp/ccp library)
 class SocketPanda():
-  def __init__(self, interface:str="can0", bus:int=0, fd:bool=False, recv_buffer_size:int=212992) -> None:
+  def __init__(self, interface:str="can0", recv_buffer_size:int=212992) -> None:
     self.interface = interface
-    self.bus = bus
-    self.fd = fd
-    self.flags = CANFD_BRS | CANFD_FDF if fd else 0
-    self.data_len = CANFD_MAX_DLEN if fd else CAN_MAX_DLEN
     self.recv_buffer_size = recv_buffer_size
-    self.socket = create_socketcan(interface, recv_buffer_size, fd)
+    self.socket = create_socketcan(interface, recv_buffer_size)
 
   def __del__(self):
     self.socket.close()
 
   def get_serial(self) -> tuple[int, int]:
-    return (0, 0) # TODO: implemented in panda socketcan driver
+    return (0, 0)
 
   def get_version(self) -> int:
-    return 0 # TODO: implemented in panda socketcan driver
+    return 0
 
   def can_clear(self, bus:int) -> None:
-    # TODO: implemented in panda socketcan driver
     self.socket.close()
-    self.socket = create_socketcan(self.interface, self.recv_buffer_size, self.fd)
+    self.socket = create_socketcan(self.interface, self.recv_buffer_size)
 
   def set_safety_mode(self, mode:int, param=0) -> None:
-    pass # TODO: implemented in panda socketcan driver
+    pass
 
-  def has_obd(self) -> bool:
-    return False # TODO: implemented in panda socketcan driver
+  def can_send_many(self, arr, *, fd=False, timeout=0) -> None:
+    for msg in arr:
+      self.can_send(*msg, fd=fd, timeout=timeout)
 
-  def can_send(self, addr, dat, bus=0, timeout=0) -> None:
+  def can_send(self, addr, dat, bus, *, fd=False, timeout=0) -> None:
+    # Even if the CANFD_FDF flag is not set, the data still must be 8 bytes for classic CAN frames.
+    data_len = CANFD_MAX_DLEN if fd else CAN_MAX_DLEN
     msg_len = len(dat)
-    msg_dat = dat.ljust(self.data_len, b'\x00')
-    can_frame = struct.pack(CAN_HEADER_FMT, addr, msg_len, self.flags) + msg_dat
-    self.socket.sendto(can_frame, (self.interface,))
+    msg_dat = dat.ljust(data_len, b'\x00')
+
+    # Set extended ID flag
+    if addr > 0x7ff:
+      addr |= CAN_EFF_FLAG
+
+    # Set FD flags
+    flags = CANFD_BRS | CANFD_FDF if fd else 0
+
+    can_frame = struct.pack(CAN_HEADER_FMT, addr, msg_len, flags) + msg_dat
+
+    # Try to send until timeout. sendto might block if the TX buffer is full.
+    # TX buffer size can also be adjusted through `ip link set can0 txqueuelen <size>` if needed
+    start_t = time.monotonic()
+    while (time.monotonic() - start_t < (timeout / 1000)) or (timeout == 0):
+      try:
+        self.socket.sendto(can_frame, (self.interface,))
+        break
+      except (BlockingIOError, OSError):
+        continue
+    else:
+      raise TimeoutError
+
 
   def can_recv(self) -> list[tuple[int, bytes, int]]:
     msgs = list()
     while True:
       try:
-        dat, _ = self.socket.recvfrom(self.recv_buffer_size, socket.MSG_DONTWAIT)
-        assert len(dat) == CAN_HEADER_LEN + self.data_len, f"ERROR: received {len(dat)} bytes"
+        dat, _, msg_flags, _ = self.socket.recvmsg(self.recv_buffer_size)
+        assert len(dat) >= CAN_HEADER_LEN, f"ERROR: received {len(dat)} bytes"
+
         can_id, msg_len, _ = struct.unpack(CAN_HEADER_FMT, dat[:CAN_HEADER_LEN])
+        assert len(dat) >= CAN_HEADER_LEN + msg_len, f"ERROR: received {len(dat)} bytes, expected at least {CAN_HEADER_LEN + msg_len} bytes"
+
         msg_dat = dat[CAN_HEADER_LEN:CAN_HEADER_LEN+msg_len]
-        msgs.append((can_id, msg_dat, self.bus))
+        bus = 128 if (msg_flags & CAN_CONFIRM_FLAG) else 0
+        msgs.append((can_id, msg_dat, bus))
       except BlockingIOError:
         break # buffered data exhausted
     return msgs

python/spi.py

@@ -16,10 +16,6 @@ try:
   import spidev
 except ImportError:
   spidev = None
-try:
-  import spidev2
-except ImportError:
-  spidev2 = None
 
 # Constants
 SYNC = 0x5A
@@ -121,8 +117,7 @@ class PandaSpiHandle(BaseHandle):
 
   def __init__(self) -> None:
     self.dev = SpiDevice()
-    if spidev2 is not None and "SPI2" in os.environ:
-      self._spi2 = spidev2.SPIBus("/dev/spidev0.0", "w+b", bits_per_word=8, speed_hz=50_000_000)
+    self.no_retry = "NO_RETRY" in os.environ
 
   # helpers
   def _calc_checksum(self, data: bytes) -> int:
@@ -183,52 +178,6 @@ class PandaSpiHandle(BaseHandle):
 
       return dat[3:-1]
 
-  def _transfer_spidev2(self, spi, endpoint: int, data, timeout: int, max_rx_len: int = USBPACKET_MAX_SIZE, expect_disconnect: bool = False) -> bytes:
-    max_rx_len = max(USBPACKET_MAX_SIZE, max_rx_len)
-
-    header = self.HEADER.pack(SYNC, endpoint, len(data), max_rx_len)
-
-    header_ack = bytearray(1)
-
-    # ACK + <2 bytes for response length> + data + checksum
-    data_rx = bytearray(3+max_rx_len+1)
-
-    self._spi2.submitTransferList(spidev2.SPITransferList((
-      # header
-      {'tx_buf': header + bytes([self._calc_checksum(header), ]), 'delay_usecs': 0, 'cs_change': True},
-      {'rx_buf': header_ack, 'delay_usecs': 0, 'cs_change': True},
-
-      # send data
-      {'tx_buf': bytes([*data, self._calc_checksum(data)]), 'delay_usecs': 0, 'cs_change': True},
-      {'rx_buf': data_rx, 'delay_usecs': 0, 'cs_change': True},
-    )))
-
-    if header_ack[0] != HACK:
-      raise PandaSpiMissingAck
-
-    if expect_disconnect:
-      logger.debug("- expecting disconnect, returning")
-      return b""
-    else:
-      dat = bytes(data_rx)
-      if dat[0] != DACK:
-        if dat[0] == NACK:
-          raise PandaSpiNackResponse
-
-        print("trying again")
-        dat = self._wait_for_ack(spi, DACK, timeout, 0x13, length=3 + max_rx_len)
-
-      # get response length, then response
-      response_len = struct.unpack("<H", dat[1:3])[0]
-      if response_len > max_rx_len:
-        raise PandaSpiException(f"response length greater than max ({max_rx_len} {response_len})")
-
-      dat = dat[:3 + response_len + 1]
-      if self._calc_checksum(dat) != 0:
-        raise PandaSpiBadChecksum
-
-      return dat[3:-1]
-
   def _transfer(self, endpoint: int, data, timeout: int, max_rx_len: int = 1000, expect_disconnect: bool = False) -> bytes:
     logger.debug("starting transfer: endpoint=%d, max_rx_len=%d", endpoint, max_rx_len)
     logger.debug("==============================================")
@@ -241,12 +190,12 @@ class PandaSpiHandle(BaseHandle):
       logger.debug("\ntry #%d", n)
       with self.dev.acquire() as spi:
         try:
-          fn = self._transfer_spidev
-          #fn = self._transfer_spidev2
-          return fn(spi, endpoint, data, timeout, max_rx_len, expect_disconnect)
+          return self._transfer_spidev(spi, endpoint, data, timeout, max_rx_len, expect_disconnect)
         except PandaSpiException as e:
           exc = e
           logger.debug("SPI transfer failed, retrying", exc_info=True)
+          if self.no_retry:
+            break
 
           # ensure slave is in a consistent state and ready for the next transfer
           # (e.g. slave TX buffer isn't stuck full)