openpilot v0.9.2 release

date: 2023-05-18T22:02:42
master commit: c7d3b28b93faa6c955fb24bc64031512ee985ee9

panda/python/__init__.py

@@ -13,6 +13,7 @@ from functools import wraps
 from typing import Optional
 from itertools import accumulate
 
+from .base import BaseHandle
 from .constants import McuType
 from .dfu import PandaDFU
 from .isotp import isotp_send, isotp_recv
@@ -25,12 +26,12 @@ __version__ = '0.0.10'
 LOGLEVEL = os.environ.get('LOGLEVEL', 'INFO').upper()
 logging.basicConfig(level=LOGLEVEL, format='%(message)s')
 
-
 USBPACKET_MAX_SIZE = 0x40
 CANPACKET_HEAD_SIZE = 0x6
 DLC_TO_LEN = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64]
 LEN_TO_DLC = {length: dlc for (dlc, length) in enumerate(DLC_TO_LEN)}
 
+
 def calculate_checksum(data):
   res = 0
   for b in data:
@@ -149,7 +150,7 @@ class Panda:
   SAFETY_NOOUTPUT = 19
   SAFETY_HONDA_BOSCH = 20
   SAFETY_VOLKSWAGEN_PQ = 21
-  SAFETY_SUBARU_LEGACY = 22
+  SAFETY_SUBARU_PREGLOBAL = 22
   SAFETY_HYUNDAI_LEGACY = 23
   SAFETY_HYUNDAI_COMMUNITY = 24
   SAFETY_STELLANTIS = 25
@@ -181,21 +182,32 @@ class Panda:
   HW_TYPE_TRES = b'\x09'
 
   CAN_PACKET_VERSION = 4
-  HEALTH_PACKET_VERSION = 11
+  HEALTH_PACKET_VERSION = 14
   CAN_HEALTH_PACKET_VERSION = 4
-  HEALTH_STRUCT = struct.Struct("<IIIIIIIIIBBBBBBHBBBHfBB")
+  HEALTH_STRUCT = struct.Struct("<IIIIIIIIIBBBBBBHBBBHfBBHBHH")
   CAN_HEALTH_STRUCT = struct.Struct("<BIBBBBBBBBIIIIIIIHHBBB")
 
   F2_DEVICES = (HW_TYPE_PEDAL, )
   F4_DEVICES = (HW_TYPE_WHITE_PANDA, HW_TYPE_GREY_PANDA, HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS)
   H7_DEVICES = (HW_TYPE_RED_PANDA, HW_TYPE_RED_PANDA_V2, HW_TYPE_TRES)
 
-  INTERNAL_DEVICES = (HW_TYPE_UNO, HW_TYPE_DOS)
+  INTERNAL_DEVICES = (HW_TYPE_UNO, HW_TYPE_DOS, HW_TYPE_TRES)
   HAS_OBD = (HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS, HW_TYPE_RED_PANDA, HW_TYPE_RED_PANDA_V2, HW_TYPE_TRES)
 
+  MAX_FAN_RPMs = {
+    HW_TYPE_UNO: 5100,
+    HW_TYPE_DOS: 6500,
+    HW_TYPE_TRES: 6600,
+  }
+
+  HARNESS_STATUS_NC = 0
+  HARNESS_STATUS_NORMAL = 1
+  HARNESS_STATUS_FLIPPED = 2
+
   # first byte is for EPS scaling factor
   FLAG_TOYOTA_ALT_BRAKE = (1 << 8)
   FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8)
+  FLAG_TOYOTA_LTA = (4 << 8)
 
   FLAG_HONDA_ALT_BRAKE = 1
   FLAG_HONDA_BOSCH_LONG = 2
@@ -223,11 +235,14 @@ class Panda:
   FLAG_GM_HW_CAM = 1
   FLAG_GM_HW_CAM_LONG = 2
 
+  FLAG_FORD_LONG_CONTROL = 1
+
   def __init__(self, serial: Optional[str] = None, claim: bool = True, disable_checks: bool = True):
-    self._serial = serial
+    self._connect_serial = serial
     self._disable_checks = disable_checks
 
-    self._handle = None
+    self._handle: BaseHandle
+    self._handle_open = False
     self.can_rx_overflow_buffer = b''
 
     # connect and set mcu type
@@ -243,18 +258,17 @@ class Panda:
     self.close()
 
   def close(self):
-    self._handle.close()
-    self._handle = None
+    if self._handle_open:
+      self._handle.close()
+      self._handle_open = False
 
   def connect(self, claim=True, wait=False):
-    if self._handle is not None:
-      self.close()
-    self._handle = None
+    self.close()
 
     # try USB first, then SPI
-    self._handle, serial, self.bootstub, bcd = self.usb_connect(self._serial, claim=claim, wait=wait)
+    self._handle, serial, self.bootstub, bcd = self.usb_connect(self._connect_serial, claim=claim, wait=wait)
     if self._handle is None:
-      self._handle, serial, self.bootstub, bcd = self.spi_connect(self._serial)
+      self._handle, serial, self.bootstub, bcd = self.spi_connect(self._connect_serial)
 
     if self._handle is None:
       raise Exception("failed to connect to panda")
@@ -277,6 +291,8 @@ class Panda:
     self._assume_f4_mcu = (self._bcd_hw_type is None) and missing_hw_type_endpoint
 
     self._serial = serial
+    self._connect_serial = serial
+    self._handle_open = True
     self._mcu_type = self.get_mcu_type()
     self.health_version, self.can_version, self.can_health_version = self.get_packets_versions()
     logging.debug("connected")
@@ -291,26 +307,29 @@ class Panda:
     # get UID to confirm slave is present and up
     handle = None
     spi_serial = None
+    bootstub = None
     try:
       handle = PandaSpiHandle()
-      dat = handle.controlRead(Panda.REQUEST_IN, 0xc3, 0, 0, 12)
+      dat = handle.controlRead(Panda.REQUEST_IN, 0xc3, 0, 0, 12, timeout=100)
       spi_serial = binascii.hexlify(dat).decode()
+      bootstub = Panda.flasher_present(handle)
     except PandaSpiException:
       pass
 
     # no connection or wrong panda
-    if spi_serial is None or (serial is not None and (spi_serial != serial)):
+    if None in (spi_serial, bootstub) or (serial is not None and (spi_serial != serial)):
       handle = None
       spi_serial = None
+      bootstub = False
 
-    # TODO: detect bootstub
-    return handle, spi_serial, False, None
+    return handle, spi_serial, bootstub, None
 
   @staticmethod
   def usb_connect(serial, claim=True, wait=False):
     handle, usb_serial, bootstub, bcd = None, None, None, None
-    context = usb1.USBContext()
     while 1:
+      context = usb1.USBContext()
+      context.open()
       try:
         for device in context.getDeviceList(skip_on_error=True):
           if device.getVendorID() == 0xbbaa and device.getProductID() in (0xddcc, 0xddee):
@@ -341,7 +360,7 @@ class Panda:
         logging.exception("USB connect error")
       if not wait or handle is not None:
         break
-      context = usb1.USBContext()  # New context needed so new devices show up
+      context.close()
 
     usb_handle = None
     if handle is not None:
@@ -357,21 +376,21 @@ class Panda:
 
   @staticmethod
   def usb_list():
-    context = usb1.USBContext()
     ret = []
     try:
-      for device in context.getDeviceList(skip_on_error=True):
-        if device.getVendorID() == 0xbbaa and device.getProductID() in (0xddcc, 0xddee):
-          try:
-            serial = device.getSerialNumber()
-            if len(serial) == 24:
-              ret.append(serial)
-            else:
-              warnings.warn(f"found device with panda descriptors but invalid serial: {serial}", RuntimeWarning)
-          except Exception:
-            continue
+      with usb1.USBContext() as context:
+        for device in context.getDeviceList(skip_on_error=True):
+          if device.getVendorID() == 0xbbaa and device.getProductID() in (0xddcc, 0xddee):
+            try:
+              serial = device.getSerialNumber()
+              if len(serial) == 24:
+                ret.append(serial)
+              else:
+                warnings.warn(f"found device with panda descriptors but invalid serial: {serial}", RuntimeWarning)
+            except Exception:
+              continue
     except Exception:
-      pass
+      logging.exception("exception while listing pandas")
     return ret
 
   @staticmethod
@@ -395,8 +414,12 @@ class Panda:
     if not enter_bootloader and reconnect:
       self.reconnect()
 
+  @property
+  def connected(self) -> bool:
+    return self._handle_open
+
   def reconnect(self):
-    if self._handle is not None:
+    if self._handle_open:
       self.close()
       time.sleep(1.0)
 
@@ -410,7 +433,7 @@ class Panda:
       except Exception:
         logging.debug("reconnecting is taking %d seconds...", i + 1)
         try:
-          dfu = PandaDFU(PandaDFU.st_serial_to_dfu_serial(self._serial, self._mcu_type))
+          dfu = PandaDFU(self.get_dfu_serial())
           dfu.recover()
         except Exception:
           pass
@@ -418,13 +441,17 @@ class Panda:
     if not success:
       raise Exception("reconnect failed")
 
+  @staticmethod
+  def flasher_present(handle: BaseHandle) -> bool:
+    fr = handle.controlRead(Panda.REQUEST_IN, 0xb0, 0, 0, 0xc)
+    return fr[4:8] == b"\xde\xad\xd0\x0d"
+
   @staticmethod
   def flash_static(handle, code, mcu_type):
     assert mcu_type is not None, "must set valid mcu_type to flash"
 
     # confirm flasher is present
-    fr = handle.controlRead(Panda.REQUEST_IN, 0xb0, 0, 0, 0xc)
-    assert fr[4:8] == b"\xde\xad\xd0\x0d"
+    assert Panda.flasher_present(handle)
 
     # determine sectors to erase
     apps_sectors_cumsum = accumulate(mcu_type.config.sector_sizes[1:])
@@ -477,8 +504,8 @@ class Panda:
     if reconnect:
       self.reconnect()
 
-  def recover(self, timeout: Optional[int] = None, reset: bool = True) -> bool:
-    dfu_serial = PandaDFU.st_serial_to_dfu_serial(self._serial, self._mcu_type)
+  def recover(self, timeout: Optional[int] = 60, reset: bool = True) -> bool:
+    dfu_serial = self.get_dfu_serial()
 
     if reset:
       self.reset(enter_bootstub=True)
@@ -505,6 +532,11 @@ class Panda:
         return False
     return True
 
+  def up_to_date(self) -> bool:
+    current = self.get_signature()
+    expected = Panda.get_signature_from_firmware(self.get_mcu_type().config.app_path)
+    return (current == expected)
+
   def call_control_api(self, msg):
     self._handle.controlWrite(Panda.REQUEST_OUT, msg, 0, 0, b'')
 
@@ -538,6 +570,10 @@ class Panda:
       "interrupt_load": a[20],
       "fan_power": a[21],
       "safety_rx_checks_invalid": a[22],
+      "spi_checksum_error_count": a[23],
+      "fan_stall_count": a[24],
+      "sbu1_voltage_mV": a[25],
+      "sbu2_voltage_mV": a[26],
     }
 
   @ensure_can_health_packet_version
@@ -592,11 +628,11 @@ class Panda:
 
   @staticmethod
   def get_signature_from_firmware(fn) -> bytes:
-    f = open(fn, 'rb')
-    f.seek(-128, 2)  # Seek from end of file
-    return f.read(128)
+    with open(fn, 'rb') as f:
+      f.seek(-128, 2)  # Seek from end of file
+      return f.read(128)
 
-  def get_signature(self):
+  def get_signature(self) -> bytes:
     part_1 = self._handle.controlRead(Panda.REQUEST_IN, 0xd3, 0, 0, 0x40)
     part_2 = self._handle.controlRead(Panda.REQUEST_IN, 0xd4, 0, 0, 0x40)
     return bytes(part_1 + part_2)
@@ -656,6 +692,9 @@ class Panda:
     """
     return self._serial
 
+  def get_dfu_serial(self):
+    return PandaDFU.st_serial_to_dfu_serial(self._serial, self._mcu_type)
+
   def get_uid(self):
     """
       Returns the UID from the MCU

panda/python/base.py

@@ -1,25 +1,67 @@
 from abc import ABC, abstractmethod
 from typing import List
 
+from .constants import McuType
+
+TIMEOUT = int(15 * 1e3)  # default timeout, in milliseconds
 
-# This mimics the handle given by libusb1 for easy interoperability
 class BaseHandle(ABC):
+  """
+    A handle to talk to a panda.
+    Borrows heavily from the libusb1 handle API.
+  """
+
   @abstractmethod
   def close(self) -> None:
     ...
 
   @abstractmethod
-  def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = 0) -> int:
+  def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT) -> int:
     ...
 
   @abstractmethod
-  def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = 0) -> bytes:
+  def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = TIMEOUT) -> bytes:
     ...
 
   @abstractmethod
-  def bulkWrite(self, endpoint: int, data: List[int], timeout: int = 0) -> int:
+  def bulkWrite(self, endpoint: int, data: List[int], timeout: int = TIMEOUT) -> int:
     ...
 
   @abstractmethod
-  def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes:
+  def bulkRead(self, endpoint: int, length: int, timeout: int = TIMEOUT) -> bytes:
     ...
+
+
+class BaseSTBootloaderHandle(ABC):
+  """
+    A handle to talk to a panda while it's in the STM32 bootloader.
+  """
+
+  @abstractmethod
+  def get_mcu_type(self) -> McuType:
+    ...
+
+  @abstractmethod
+  def close(self) -> None:
+    ...
+
+  @abstractmethod
+  def clear_status(self) -> None:
+    ...
+
+  @abstractmethod
+  def program(self, address: int, dat: bytes) -> None:
+    ...
+
+  @abstractmethod
+  def erase_app(self) -> None:
+    ...
+
+  @abstractmethod
+  def erase_bootstub(self) -> None:
+    ...
+
+  @abstractmethod
+  def jump(self, address: int) -> None:
+    ...
+

panda/python/constants.py

@@ -8,6 +8,7 @@ BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
 class McuConfig(NamedTuple):
   mcu: str
   mcu_idcode: int
+  uid_address: int
   block_size: int
   sector_sizes: List[int]
   serial_number_address: int
@@ -17,6 +18,7 @@ class McuConfig(NamedTuple):
   bootstub_path: str
 
 Fx = (
+  0x1FFF7A10,
   0x800,
   [0x4000 for _ in range(4)] + [0x10000] + [0x20000 for _ in range(11)],
   0x1FFF79C0,
@@ -31,6 +33,7 @@ F4Config = McuConfig("STM32F4", 0x463, *Fx)
 H7Config = McuConfig(
   "STM32H7",
   0x483,
+  0x1FF1E800,
   0x400,
   # there is an 8th sector, but we use that for the provisioning chunk, so don't program over that!
   [0x20000 for _ in range(7)],
@@ -50,3 +53,5 @@ class McuType(enum.Enum):
   @property
   def config(self):
     return self.value
+
+MCU_TYPE_BY_IDCODE = {m.config.mcu_idcode: m for m in McuType}

panda/python/dfu.py

@@ -1,52 +1,96 @@
 import usb1
 import struct
 import binascii
+from typing import List, Optional
 
+from .base import BaseSTBootloaderHandle
+from .spi import STBootloaderSPIHandle, PandaSpiException
+from .usb import STBootloaderUSBHandle
 from .constants import McuType
 
 
-# *** DFU mode ***
-DFU_DNLOAD = 1
-DFU_UPLOAD = 2
-DFU_GETSTATUS = 3
-DFU_CLRSTATUS = 4
-DFU_ABORT = 6
-
 class PandaDFU:
-  def __init__(self, dfu_serial):
-    self._handle = None
+  def __init__(self, dfu_serial: Optional[str]):
+    # try USB, then SPI
+    handle: Optional[BaseSTBootloaderHandle]
+    handle = PandaDFU.usb_connect(dfu_serial)
+    if handle is None:
+      handle = PandaDFU.spi_connect(dfu_serial)
+
+    if handle is None:
+      raise Exception(f"failed to open DFU device {dfu_serial}")
+
+    self._handle: BaseSTBootloaderHandle = handle
+    self._mcu_type: McuType = self._handle.get_mcu_type()
+
+  @staticmethod
+  def usb_connect(dfu_serial: Optional[str]) -> Optional[STBootloaderUSBHandle]:
+    handle = None
     context = usb1.USBContext()
+    context.open()
     for device in context.getDeviceList(skip_on_error=True):
       if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11:
         try:
           this_dfu_serial = device.open().getASCIIStringDescriptor(3)
         except Exception:
           continue
+
         if this_dfu_serial == dfu_serial or dfu_serial is None:
-          self._handle = device.open()
-          self._mcu_type = self.get_mcu_type(device)
+          handle = STBootloaderUSBHandle(device, device.open())
           break
 
-    if self._handle is None:
-      raise Exception(f"failed to open DFU device {dfu_serial}")
+    return handle
 
   @staticmethod
-  def list():
-    context = usb1.USBContext()
+  def spi_connect(dfu_serial: Optional[str]) -> Optional[STBootloaderSPIHandle]:
+    handle = None
+    this_dfu_serial = None
+
+    try:
+      handle = STBootloaderSPIHandle()
+      this_dfu_serial = PandaDFU.st_serial_to_dfu_serial(handle.get_uid(), handle.get_mcu_type())
+    except PandaSpiException:
+      handle = None
+
+    if dfu_serial is not None and dfu_serial != this_dfu_serial:
+      handle = None
+
+    return handle
+
+  @staticmethod
+  def list() -> List[str]:
+    ret = PandaDFU.usb_list()
+    ret += PandaDFU.spi_list()
+    return list(set(ret))
+
+  @staticmethod
+  def usb_list() -> List[str]:
     dfu_serials = []
     try:
-      for device in context.getDeviceList(skip_on_error=True):
-        if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11:
-          try:
-            dfu_serials.append(device.open().getASCIIStringDescriptor(3))
-          except Exception:
-            pass
+      with usb1.USBContext() as context:
+        for device in context.getDeviceList(skip_on_error=True):
+          if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11:
+            try:
+              dfu_serials.append(device.open().getASCIIStringDescriptor(3))
+            except Exception:
+              pass
     except Exception:
       pass
     return dfu_serials
 
   @staticmethod
-  def st_serial_to_dfu_serial(st, mcu_type=McuType.F4):
+  def spi_list() -> List[str]:
+    try:
+      h = PandaDFU.spi_connect(None)
+      if h is not None:
+        dfu_serial = PandaDFU.st_serial_to_dfu_serial(h.get_uid(), h.get_mcu_type())
+        return [dfu_serial, ]
+    except PandaSpiException:
+      pass
+    return []
+
+  @staticmethod
+  def st_serial_to_dfu_serial(st: str, mcu_type: McuType = McuType.F4):
     if st is None or st == "none":
       return None
     uid_base = struct.unpack("H" * 6, bytes.fromhex(st))
@@ -55,52 +99,17 @@ class PandaDFU:
     else:
       return binascii.hexlify(struct.pack("!HHH", uid_base[1] + uid_base[5], uid_base[0] + uid_base[4] + 0xA, uid_base[3])).upper().decode("utf-8")
 
-  def get_mcu_type(self, dev) -> McuType:
-    # TODO: Find a way to detect F4 vs F2
-    # TODO: also check F4 BCD, don't assume in else
-    return McuType.H7 if dev.getbcdDevice() == 512 else McuType.F4
+  def get_mcu_type(self) -> McuType:
+    return self._mcu_type
 
-  def status(self):
-    while 1:
-      dat = self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6)
-      if dat[1] == 0:
-        break
-
-  def clear_status(self):
-    # Clear status
-    stat = self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6)
-    if stat[4] == 0xa:
-      self._handle.controlRead(0x21, DFU_CLRSTATUS, 0, 0, 0)
-    elif stat[4] == 0x9:
-      self._handle.controlWrite(0x21, DFU_ABORT, 0, 0, b"")
-      self.status()
-    stat = str(self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6))
-
-  def erase(self, address):
-    self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, b"\x41" + struct.pack("I", address))
-    self.status()
-
-  def program(self, address, dat, block_size=None):
-    if block_size is None:
-      block_size = len(dat)
-
-    # Set Address Pointer
-    self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", address))
-    self.status()
-
-    # Program
-    dat += b"\xFF" * ((block_size - len(dat)) % block_size)
-    for i in range(0, len(dat) // block_size):
-      ldat = dat[i * block_size:(i + 1) * block_size]
-      print("programming %d with length %d" % (i, len(ldat)))
-      self._handle.controlWrite(0x21, DFU_DNLOAD, 2 + i, 0, ldat)
-      self.status()
+  def reset(self):
+    self._handle.jump(self._mcu_type.config.bootstub_address)
 
   def program_bootstub(self, code_bootstub):
-    self.clear_status()
-    self.erase(self._mcu_type.config.bootstub_address)
-    self.erase(self._mcu_type.config.app_address)
-    self.program(self._mcu_type.config.bootstub_address, code_bootstub, self._mcu_type.config.block_size)
+    self._handle.clear_status()
+    self._handle.erase_bootstub()
+    self._handle.erase_app()
+    self._handle.program(self._mcu_type.config.bootstub_address, code_bootstub)
     self.reset()
 
   def recover(self):
@@ -108,11 +117,3 @@ class PandaDFU:
       code = f.read()
     self.program_bootstub(code)
 
-  def reset(self):
-    self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", self._mcu_type.config.bootstub_address))
-    self.status()
-    try:
-      self._handle.controlWrite(0x21, DFU_DNLOAD, 2, 0, b"")
-      _ = str(self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6))
-    except Exception:
-      pass

panda/python/spi.py

@@ -1,3 +1,4 @@
+import binascii
 import os
 import fcntl
 import math
@@ -7,9 +8,10 @@ import logging
 import threading
 from contextlib import contextmanager
 from functools import reduce
-from typing import List
+from typing import List, Optional
 
-from .base import BaseHandle
+from .base import BaseHandle, BaseSTBootloaderHandle, TIMEOUT
+from .constants import McuType, MCU_TYPE_BY_IDCODE
 
 try:
   import spidev
@@ -23,10 +25,10 @@ DACK = 0x85
 NACK = 0x1F
 CHECKSUM_START = 0xAB
 
-ACK_TIMEOUT_SECONDS = 0.1
+MIN_ACK_TIMEOUT_MS = 100
 MAX_XFER_RETRY_COUNT = 5
 
-USB_MAX_SIZE = 0x40
+XFER_SIZE = 1000
 
 DEV_PATH = "/dev/spidev0.0"
 
@@ -56,7 +58,14 @@ class SpiDevice:
   """
   Provides locked, thread-safe access to a panda's SPI interface.
   """
-  def __init__(self, speed=30000000):
+
+  # 50MHz is the max of the 845. older rev comma three
+  # may not support the full 50MHz
+  MAX_SPEED = 50000000
+
+  def __init__(self, speed=MAX_SPEED):
+    assert speed <= self.MAX_SPEED
+
     if not os.path.exists(DEV_PATH):
       raise PandaSpiUnavailable(f"SPI device not found: {DEV_PATH}")
     if spidev is None:
@@ -94,10 +103,12 @@ class PandaSpiHandle(BaseHandle):
       cksum ^= b
     return cksum
 
-  def _wait_for_ack(self, spi, ack_val: int) -> None:
+  def _wait_for_ack(self, spi, ack_val: int, timeout: int, tx: int) -> None:
+    timeout_s = max(MIN_ACK_TIMEOUT_MS, timeout) * 1e-3
+
     start = time.monotonic()
-    while (time.monotonic() - start) < ACK_TIMEOUT_SECONDS:
-      dat = spi.xfer2(b"\x12")[0]
+    while (timeout == 0) or ((time.monotonic() - start) < timeout_s):
+      dat = spi.xfer2([tx, ])[0]
       if dat == NACK:
         raise PandaSpiNackResponse
       elif dat == ack_val:
@@ -105,7 +116,7 @@ class PandaSpiHandle(BaseHandle):
 
     raise PandaSpiMissingAck
 
-  def _transfer(self, spi, endpoint: int, data, max_rx_len: int = 1000) -> bytes:
+  def _transfer(self, spi, endpoint: int, data, timeout: int, max_rx_len: int = 1000) -> bytes:
     logging.debug("starting transfer: endpoint=%d, max_rx_len=%d", endpoint, max_rx_len)
     logging.debug("==============================================")
 
@@ -119,7 +130,7 @@ class PandaSpiHandle(BaseHandle):
         spi.xfer2(packet)
 
         logging.debug("- waiting for header ACK")
-        self._wait_for_ack(spi, HACK)
+        self._wait_for_ack(spi, HACK, timeout, 0x11)
 
         # send data
         logging.debug("- sending data")
@@ -127,11 +138,13 @@ class PandaSpiHandle(BaseHandle):
         spi.xfer2(packet)
 
         logging.debug("- waiting for data ACK")
-        self._wait_for_ack(spi, DACK)
+        self._wait_for_ack(spi, DACK, timeout, 0x13)
 
         # get response length, then response
         response_len_bytes = bytes(spi.xfer2(b"\x00" * 2))
         response_len = struct.unpack("<H", response_len_bytes)[0]
+        if response_len > max_rx_len:
+          raise PandaSpiException("response length greater than max")
 
         logging.debug("- receiving response")
         dat = bytes(spi.xfer2(b"\x00" * (response_len + 1)))
@@ -141,34 +154,177 @@ class PandaSpiHandle(BaseHandle):
         return dat[:-1]
       except PandaSpiException as e:
         exc = e
-        logging.debug("SPI transfer failed, %d retries left", n, exc_info=True)
+        logging.debug("SPI transfer failed, %d retries left", MAX_XFER_RETRY_COUNT - n - 1, exc_info=True)
     raise exc
 
   # libusb1 functions
   def close(self):
     self.dev.close()
 
-  def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = 0):
+  def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT):
     with self.dev.acquire() as spi:
-      return self._transfer(spi, 0, struct.pack("<BHHH", request, value, index, 0))
+      return self._transfer(spi, 0, struct.pack("<BHHH", request, value, index, 0), timeout)
 
-  def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = 0):
+  def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = TIMEOUT):
     with self.dev.acquire() as spi:
-      return self._transfer(spi, 0, struct.pack("<BHHH", request, value, index, length))
+      return self._transfer(spi, 0, struct.pack("<BHHH", request, value, index, length), timeout)
 
   # TODO: implement these properly
-  def bulkWrite(self, endpoint: int, data: List[int], timeout: int = 0) -> int:
+  def bulkWrite(self, endpoint: int, data: List[int], timeout: int = TIMEOUT) -> int:
     with self.dev.acquire() as spi:
-      for x in range(math.ceil(len(data) / USB_MAX_SIZE)):
-        self._transfer(spi, endpoint, data[USB_MAX_SIZE*x:USB_MAX_SIZE*(x+1)])
+      for x in range(math.ceil(len(data) / XFER_SIZE)):
+        self._transfer(spi, endpoint, data[XFER_SIZE*x:XFER_SIZE*(x+1)], timeout)
       return len(data)
 
-  def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes:
+  def bulkRead(self, endpoint: int, length: int, timeout: int = TIMEOUT) -> bytes:
     ret: List[int] = []
     with self.dev.acquire() as spi:
-      for _ in range(math.ceil(length / USB_MAX_SIZE)):
-        d = self._transfer(spi, endpoint, [], max_rx_len=USB_MAX_SIZE)
+      for _ in range(math.ceil(length / XFER_SIZE)):
+        d = self._transfer(spi, endpoint, [], timeout, max_rx_len=XFER_SIZE)
         ret += d
-        if len(d) < USB_MAX_SIZE:
+        if len(d) < XFER_SIZE:
           break
     return bytes(ret)
+
+
+class STBootloaderSPIHandle(BaseSTBootloaderHandle):
+  """
+    Implementation of the STM32 SPI bootloader protocol described in:
+    https://www.st.com/resource/en/application_note/an4286-spi-protocol-used-in-the-stm32-bootloader-stmicroelectronics.pdf
+  """
+
+  SYNC = 0x5A
+  ACK = 0x79
+  NACK = 0x1F
+
+  def __init__(self):
+    self.dev = SpiDevice(speed=1000000)
+
+    # say hello
+    try:
+      with self.dev.acquire() as spi:
+        spi.xfer([self.SYNC, ])
+        try:
+          self._get_ack(spi)
+        except (PandaSpiNackResponse, PandaSpiMissingAck):
+          # NACK ok here, will only ACK the first time
+          pass
+
+      self._mcu_type = MCU_TYPE_BY_IDCODE[self.get_chip_id()]
+    except PandaSpiException:
+      raise PandaSpiException("failed to connect to panda")  # pylint: disable=W0707
+
+  def _get_ack(self, spi, timeout=1.0):
+    data = 0x00
+    start_time = time.monotonic()
+    while data not in (self.ACK, self.NACK) and (time.monotonic() - start_time < timeout):
+      data = spi.xfer([0x00, ])[0]
+      time.sleep(0.001)
+    spi.xfer([self.ACK, ])
+
+    if data == self.NACK:
+      raise PandaSpiNackResponse
+    elif data != self.ACK:
+      raise PandaSpiMissingAck
+
+  def _cmd_no_retry(self, cmd: int, data: Optional[List[bytes]] = None, read_bytes: int = 0, predata=None) -> bytes:
+    ret = b""
+    with self.dev.acquire() as spi:
+      # sync + command
+      spi.xfer([self.SYNC, ])
+      spi.xfer([cmd, cmd ^ 0xFF])
+      self._get_ack(spi, timeout=0.1)
+
+      # "predata" - for commands that send the first data without a checksum
+      if predata is not None:
+        spi.xfer(predata)
+        self._get_ack(spi)
+
+      # send data
+      if data is not None:
+        for d in data:
+          if predata is not None:
+            spi.xfer(d + self._checksum(predata + d))
+          else:
+            spi.xfer(d + self._checksum(d))
+          self._get_ack(spi, timeout=20)
+
+      # receive
+      if read_bytes > 0:
+        ret = spi.xfer([0x00, ]*(read_bytes + 1))[1:]
+        if data is None or len(data) == 0:
+          self._get_ack(spi)
+
+    return bytes(ret)
+
+  def _cmd(self, cmd: int, data: Optional[List[bytes]] = None, read_bytes: int = 0, predata=None) -> bytes:
+    exc = PandaSpiException()
+    for n in range(MAX_XFER_RETRY_COUNT):
+      try:
+        return self._cmd_no_retry(cmd, data, read_bytes, predata)
+      except PandaSpiException as e:
+        exc = e
+        logging.debug("SPI transfer failed, %d retries left", MAX_XFER_RETRY_COUNT - n - 1, exc_info=True)
+    raise exc
+
+  def _checksum(self, data: bytes) -> bytes:
+    if len(data) == 1:
+      ret = data[0] ^ 0xFF
+    else:
+      ret = reduce(lambda a, b: a ^ b, data)
+    return bytes([ret, ])
+
+  # *** Bootloader commands ***
+
+  def read(self, address: int, length: int):
+    data = [struct.pack('>I', address), struct.pack('B', length - 1)]
+    return self._cmd(0x11, data=data, read_bytes=length)
+
+  def get_chip_id(self) -> int:
+    r = self._cmd(0x02, read_bytes=3)
+    assert r[0] == 1  # response length - 1
+    return ((r[1] << 8) + r[2])
+
+  def go_cmd(self, address: int) -> None:
+    self._cmd(0x21, data=[struct.pack('>I', address), ])
+
+  # *** helpers ***
+
+  def get_uid(self):
+    dat = self.read(McuType.H7.config.uid_address, 12)
+    return binascii.hexlify(dat).decode()
+
+  def erase_sector(self, sector: int):
+    p = struct.pack('>H', 0)  # number of sectors to erase
+    d = struct.pack('>H', sector)
+    self._cmd(0x44, data=[d, ], predata=p)
+
+  # *** PandaDFU API ***
+
+  def erase_app(self):
+    self.erase_sector(1)
+
+  def erase_bootstub(self):
+    self.erase_sector(0)
+
+  def get_mcu_type(self):
+    return self._mcu_type
+
+  def clear_status(self):
+    pass
+
+  def close(self):
+    self.dev.close()
+
+  def program(self, address, dat):
+    bs = 256  # max block size for writing to flash over SPI
+    dat += b"\xFF" * ((bs - len(dat)) % bs)
+    for i in range(0, len(dat) // bs):
+      block = dat[i * bs:(i + 1) * bs]
+      self._cmd(0x31, data=[
+        struct.pack('>I', address + i*bs),
+        bytes([len(block) - 1]) + block,
+      ])
+
+  def jump(self, address):
+    self.go_cmd(self._mcu_type.config.bootstub_address)

panda/python/spi_dfu.py

@@ -1,118 +0,0 @@
-import time
-import struct
-from functools import reduce
-
-from .constants import McuType
-from .spi import SpiDevice
-
-SYNC = 0x5A
-ACK = 0x79
-NACK = 0x1F
-
-# https://www.st.com/resource/en/application_note/an4286-spi-protocol-used-in-the-stm32-bootloader-stmicroelectronics.pdf
-class PandaSpiDFU:
-  def __init__(self, dfu_serial):
-    self.dev = SpiDevice(speed=1000000)
-
-    # say hello
-    with self.dev.acquire() as spi:
-      try:
-        spi.xfer([SYNC, ])
-        self._get_ack(spi)
-      except Exception:
-        raise Exception("failed to connect to panda")  # pylint: disable=W0707
-
-    self._mcu_type = self.get_mcu_type()
-
-  def _get_ack(self, spi, timeout=1.0):
-    data = 0x00
-    start_time = time.monotonic()
-    while data not in (ACK, NACK) and (time.monotonic() - start_time < timeout):
-      data = spi.xfer([0x00, ])[0]
-      time.sleep(0.001)
-    spi.xfer([ACK, ])
-
-    if data == NACK:
-      raise Exception("Got NACK response")
-    elif data != ACK:
-      raise Exception("Missing ACK")
-
-  def _cmd(self, cmd, data=None, read_bytes=0) -> bytes:
-    ret = b""
-    with self.dev.acquire() as spi:
-      # sync
-      spi.xfer([SYNC, ])
-
-      # send command
-      spi.xfer([cmd, cmd ^ 0xFF])
-      self._get_ack(spi)
-
-      # send data
-      if data is not None:
-        for d in data:
-          spi.xfer(self.add_checksum(d))
-          self._get_ack(spi, timeout=20)
-
-      # receive
-      if read_bytes > 0:
-        # send busy byte
-        ret = spi.xfer([0x00, ]*(read_bytes + 1))[1:]
-        self._get_ack(spi)
-
-    return ret
-
-  def add_checksum(self, data):
-    return data + bytes([reduce(lambda a, b: a ^ b, data)])
-
-  # ***** ST Bootloader functions *****
-
-  def get_bootloader_version(self) -> int:
-    return self._cmd(0x01, read_bytes=1)[0]
-
-  def get_id(self) -> int:
-    ret = self._cmd(0x02, read_bytes=3)
-    assert ret[0] == 1
-    return ((ret[1] << 8) + ret[2])
-
-  def go_cmd(self, address: int) -> None:
-    self._cmd(0x21, data=[struct.pack('>I', address), ])
-
-  def erase(self, address: int) -> None:
-    d = struct.pack('>H', address)
-    self._cmd(0x44, data=[d, ])
-
-  # ***** panda api *****
-
-  def get_mcu_type(self) -> McuType:
-    mcu_by_id = {mcu.config.mcu_idcode: mcu for mcu in McuType}
-    return mcu_by_id[self.get_id()]
-
-  def global_erase(self):
-    self.erase(0xFFFF)
-
-  def program_file(self, address, fn):
-    with open(fn, 'rb') as f:
-      code = f.read()
-
-    i = 0
-    while i < len(code):
-      #print(i, len(code))
-      block = code[i:i+256]
-      if len(block) < 256:
-        block += b'\xFF' * (256 - len(block))
-
-      self._cmd(0x31, data=[
-        struct.pack('>I', address + i),
-        bytes([len(block) - 1]) + block,
-      ])
-      #print(f"Written {len(block)} bytes to {hex(address + i)}")
-      i += 256
-
-  def program_bootstub(self):
-    self.program_file(self._mcu_type.config.bootstub_address, self._mcu_type.config.bootstub_path)
-
-  def program_app(self):
-    self.program_file(self._mcu_type.config.app_address, self._mcu_type.config.app_path)
-
-  def reset(self):
-    self.go_cmd(self._mcu_type.config.bootstub_address)

panda/python/uds.py

@@ -141,6 +141,12 @@ class DYNAMIC_DEFINITION_TYPE(IntEnum):
   DEFINE_BY_MEMORY_ADDRESS = 2
   CLEAR_DYNAMICALLY_DEFINED_DATA_IDENTIFIER = 3
 
+class ISOTP_FRAME_TYPE(IntEnum):
+  SINGLE = 0
+  FIRST = 1
+  CONSECUTIVE = 2
+  FLOW = 3
+
 class DynamicSourceDefinition(NamedTuple):
   data_identifier: int
   position: int
@@ -438,38 +444,42 @@ class IsoTpMessage():
       timeout = self.timeout
 
     start_time = time.monotonic()
-    updated = False
+    rx_in_progress = False
     try:
       while True:
         for msg in self._can_client.recv():
-          self._isotp_rx_next(msg)
+          frame_type = self._isotp_rx_next(msg)
           start_time = time.monotonic()
-          updated = True
+          rx_in_progress = frame_type == ISOTP_FRAME_TYPE.CONSECUTIVE
           if self.tx_done and self.rx_done:
-            return self.rx_dat, updated
+            return self.rx_dat, False
         # no timeout indicates non-blocking
         if timeout == 0:
-          return None, updated
+          return None, rx_in_progress
         if time.monotonic() - start_time > timeout:
           raise MessageTimeoutError("timeout waiting for response")
     finally:
       if self.debug and self.rx_dat:
         print(f"ISO-TP: RESPONSE - {hex(self._can_client.rx_addr)} 0x{bytes.hex(self.rx_dat)}")
 
-  def _isotp_rx_next(self, rx_data: bytes) -> None:
-    # single rx_frame
-    if rx_data[0] >> 4 == 0x0:
+  def _isotp_rx_next(self, rx_data: bytes) -> ISOTP_FRAME_TYPE:
+    # TODO: Handle CAN frame data optimization, which is allowed with some frame types
+    # # ISO 15765-2 specifies an eight byte CAN frame for ISO-TP communication
+    # assert len(rx_data) == self.max_len, f"isotp - rx: invalid CAN frame length: {len(rx_data)}"
+
+    if rx_data[0] >> 4 == ISOTP_FRAME_TYPE.SINGLE:
       self.rx_len = rx_data[0] & 0xFF
+      assert self.rx_len < self.max_len, f"isotp - rx: invalid single frame length: {self.rx_len}"
       self.rx_dat = rx_data[1:1 + self.rx_len]
       self.rx_idx = 0
       self.rx_done = True
       if self.debug:
         print(f"ISO-TP: RX - single frame - {hex(self._can_client.rx_addr)} idx={self.rx_idx} done={self.rx_done}")
-      return
+      return ISOTP_FRAME_TYPE.SINGLE
 
-    # first rx_frame
-    if rx_data[0] >> 4 == 0x1:
+    elif rx_data[0] >> 4 == ISOTP_FRAME_TYPE.FIRST:
       self.rx_len = ((rx_data[0] & 0x0F) << 8) + rx_data[1]
+      assert self.max_len <= self.rx_len, f"isotp - rx: invalid first frame length: {self.rx_len}"
       self.rx_dat = rx_data[2:]
       self.rx_idx = 0
       self.rx_done = False
@@ -479,10 +489,9 @@ class IsoTpMessage():
         print(f"ISO-TP: TX - flow control continue - {hex(self._can_client.tx_addr)}")
       # send flow control message
       self._can_client.send([self.flow_control_msg])
-      return
+      return ISOTP_FRAME_TYPE.FIRST
 
-    # consecutive rx frame
-    if rx_data[0] >> 4 == 0x2:
+    elif rx_data[0] >> 4 == ISOTP_FRAME_TYPE.CONSECUTIVE:
       assert not self.rx_done, "isotp - rx: consecutive frame with no active frame"
       self.rx_idx += 1
       assert self.rx_idx & 0xF == rx_data[0] & 0xF, "isotp - rx: invalid consecutive frame index"
@@ -495,10 +504,9 @@ class IsoTpMessage():
         self._can_client.send([self.flow_control_msg])
       if self.debug:
         print(f"ISO-TP: RX - consecutive frame - {hex(self._can_client.rx_addr)} idx={self.rx_idx} done={self.rx_done}")
-      return
+      return ISOTP_FRAME_TYPE.CONSECUTIVE
 
-    # flow control
-    if rx_data[0] >> 4 == 0x3:
+    elif rx_data[0] >> 4 == ISOTP_FRAME_TYPE.FLOW:
       assert not self.tx_done, "isotp - rx: flow control with no active frame"
       assert rx_data[0] != 0x32, "isotp - rx: flow-control overflow/abort"
       assert rx_data[0] == 0x30 or rx_data[0] == 0x31, "isotp - rx: flow-control transfer state indicator invalid"
@@ -512,7 +520,7 @@ class IsoTpMessage():
 
         # first frame = 6 bytes, each consecutive frame = 7 bytes
         num_bytes = self.max_len - 1
-        start = 6 + self.tx_idx * num_bytes
+        start = self.max_len - 2 + self.tx_idx * num_bytes
         count = rx_data[1]
         end = start + count * num_bytes if count > 0 else self.tx_len
         tx_msgs = []
@@ -531,9 +539,16 @@ class IsoTpMessage():
         # wait (do nothing until next flow control message)
         if self.debug:
           print(f"ISO-TP: TX - flow control wait - {hex(self._can_client.tx_addr)}")
+      return ISOTP_FRAME_TYPE.FLOW
+
+    # 4-15 - reserved
+    else:
+      raise Exception(f"isotp - rx: invalid frame type: {rx_data[0] >> 4}")
+
 
 FUNCTIONAL_ADDRS = [0x7DF, 0x18DB33F1]
 
+
 def get_rx_addr_for_tx_addr(tx_addr, rx_offset=0x8):
   if tx_addr in FUNCTIONAL_ADDRS:
     return None
@@ -551,15 +566,16 @@ def get_rx_addr_for_tx_addr(tx_addr, rx_offset=0x8):
 
 
 class UdsClient():
-  def __init__(self, panda, tx_addr: int, rx_addr: int = None, bus: int = 0, timeout: float = 1, debug: bool = False,
-               tx_timeout: float = 1, response_pending_timeout: float = 10):
+  def __init__(self, panda, tx_addr: int, rx_addr: int = None, bus: int = 0, sub_addr: int = None, timeout: float = 1,
+               debug: bool = False, tx_timeout: float = 1, response_pending_timeout: float = 10):
     self.bus = bus
     self.tx_addr = tx_addr
     self.rx_addr = rx_addr if rx_addr is not None else get_rx_addr_for_tx_addr(tx_addr)
+    self.sub_addr = sub_addr
     self.timeout = timeout
     self.debug = debug
     can_send_with_timeout = partial(panda.can_send, timeout=int(tx_timeout*1000))
-    self._can_client = CanClient(can_send_with_timeout, panda.can_recv, self.tx_addr, self.rx_addr, self.bus, debug=self.debug)
+    self._can_client = CanClient(can_send_with_timeout, panda.can_recv, self.tx_addr, self.rx_addr, self.bus, self.sub_addr, debug=self.debug)
     self.response_pending_timeout = response_pending_timeout
 
   # generic uds request
@@ -571,7 +587,8 @@ class UdsClient():
       req += data
 
     # send request, wait for response
-    isotp_msg = IsoTpMessage(self._can_client, timeout=self.timeout, debug=self.debug)
+    max_len = 8 if self.sub_addr is None else 7
+    isotp_msg = IsoTpMessage(self._can_client, timeout=self.timeout, debug=self.debug, max_len=max_len)
     isotp_msg.send(req)
     response_pending = False
     while True:

panda/python/usb.py

@@ -1,6 +1,8 @@
+import struct
 from typing import List
 
-from .base import BaseHandle
+from .base import BaseHandle, BaseSTBootloaderHandle, TIMEOUT
+from .constants import McuType
 
 class PandaUsbHandle(BaseHandle):
   def __init__(self, libusb_handle):
@@ -9,15 +11,85 @@ class PandaUsbHandle(BaseHandle):
   def close(self):
     self._libusb_handle.close()
 
-  def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = 0):
+  def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT):
     return self._libusb_handle.controlWrite(request_type, request, value, index, data, timeout)
 
-  def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = 0):
+  def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = TIMEOUT):
     return self._libusb_handle.controlRead(request_type, request, value, index, length, timeout)
 
-  def bulkWrite(self, endpoint: int, data: List[int], timeout: int = 0) -> int:
+  def bulkWrite(self, endpoint: int, data: List[int], timeout: int = TIMEOUT) -> int:
     return self._libusb_handle.bulkWrite(endpoint, data, timeout)  # type: ignore
 
-  def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes:
+  def bulkRead(self, endpoint: int, length: int, timeout: int = TIMEOUT) -> bytes:
     return self._libusb_handle.bulkRead(endpoint, length, timeout)  # type: ignore
 
+
+
+class STBootloaderUSBHandle(BaseSTBootloaderHandle):
+  DFU_DNLOAD = 1
+  DFU_UPLOAD = 2
+  DFU_GETSTATUS = 3
+  DFU_CLRSTATUS = 4
+  DFU_ABORT = 6
+
+  def __init__(self, libusb_device, libusb_handle):
+    self._libusb_handle = libusb_handle
+
+    # TODO: Find a way to detect F4 vs F2
+    # TODO: also check F4 BCD, don't assume in else
+    self._mcu_type = McuType.H7 if libusb_device.getbcdDevice() == 512 else McuType.F4
+
+  def _status(self) -> None:
+    while 1:
+      dat = self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6)
+      if dat[1] == 0:
+        break
+
+  def _erase_page_address(self, address: int) -> None:
+    self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 0, 0, b"\x41" + struct.pack("I", address))
+    self._status()
+
+  def get_mcu_type(self):
+    return self._mcu_type
+
+  def erase_app(self):
+    self._erase_page_address(self._mcu_type.config.app_address)
+
+  def erase_bootstub(self):
+    self._erase_page_address(self._mcu_type.config.bootstub_address)
+
+  def clear_status(self):
+    # Clear status
+    stat = self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6)
+    if stat[4] == 0xa:
+      self._libusb_handle.controlRead(0x21, self.DFU_CLRSTATUS, 0, 0, 0)
+    elif stat[4] == 0x9:
+      self._libusb_handle.controlWrite(0x21, self.DFU_ABORT, 0, 0, b"")
+      self._status()
+    stat = str(self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6))
+
+  def close(self):
+    self._libusb_handle.close()
+
+  def program(self, address, dat):
+    # Set Address Pointer
+    self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", address))
+    self._status()
+
+    # Program
+    bs = self._mcu_type.config.block_size
+    dat += b"\xFF" * ((bs - len(dat)) % bs)
+    for i in range(0, len(dat) // bs):
+      ldat = dat[i * bs:(i + 1) * bs]
+      print("programming %d with length %d" % (i, len(ldat)))
+      self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 2 + i, 0, ldat)
+      self._status()
+
+  def jump(self, address):
+    self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", address))
+    self._status()
+    try:
+      self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 2, 0, b"")
+      _ = str(self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6))
+    except Exception:
+      pass