body v1 updater

opendbc/car/body/fingerprints.py

@@ -9,7 +9,7 @@
 
 FINGERPRINTS = {
   CAR.COMMA_BODY: [{
-    513: 8, 516: 8, 514: 3, 515: 4
+    513: 8, 516: 8, 514: 3, 515: 4, 517: 8, 518: 6, 519: 6
   }],
 }
 

opendbc/car/body/flash.py

@@ -0,0 +1,148 @@
+import os
+import time
+import struct
+import subprocess
+from itertools import accumulate
+
+from opendbc.car.uds import CanClient, IsoTpMessage, MessageTimeoutError
+
+REQUEST_IN = 0xC0
+REQUEST_OUT = 0x40
+DEFAULT_ISOTP_TIMEOUT = 2
+
+F4Config = {
+  "sector_sizes": [0x4000 for _ in range(4)] + [0x10000] + [0x20000 for _ in range(11)],
+}
+
+class CanHandle:
+  def __init__(self, can_send, can_recv, bus):
+    self.client = CanClient(can_send, can_recv, tx_addr=1, rx_addr=2, bus=bus)
+
+  def transact(self, dat, timeout=DEFAULT_ISOTP_TIMEOUT, expect_disconnect=False):
+    try:
+      msg = IsoTpMessage(self.client, timeout=timeout)
+      msg.send(dat)
+      if expect_disconnect:
+        deadline = time.monotonic() + timeout
+        while not msg.tx_done:
+          msg.recv(timeout=0)
+          if not msg.tx_done and time.monotonic() > deadline:
+            raise MessageTimeoutError("timeout waiting for flow control")
+          time.sleep(0.01)
+        return b""
+      ret, _ = msg.recv()
+      return ret
+    except MessageTimeoutError as e:
+      raise TimeoutError from e
+
+  def controlWrite(self, request_type, request, value, index, data, timeout=DEFAULT_ISOTP_TIMEOUT, expect_disconnect=False):
+    dat = struct.pack("HHBBHHH", 0, 0, request_type, request, value, index, 0)
+    return self.transact(dat, timeout=timeout, expect_disconnect=expect_disconnect)
+
+  def controlRead(self, request_type, request, value, index, length, timeout=DEFAULT_ISOTP_TIMEOUT):
+    dat = struct.pack("HHBBHHH", 0, 0, request_type, request, value, index, length)
+    return self.transact(dat, timeout=timeout)
+
+  def bulkWrite(self, endpoint, data, timeout=DEFAULT_ISOTP_TIMEOUT):
+    dat = struct.pack("HH", endpoint, len(data)) + data
+    return self.transact(dat, timeout=timeout)
+
+  def bulkRead(self, endpoint, timeout=DEFAULT_ISOTP_TIMEOUT):
+    dat = struct.pack("HH", endpoint, 0)
+    return self.transact(dat, timeout=timeout)
+
+
+def flush_recv_buffer(can_recv):
+  while (1):
+    if len(can_recv()) == 0:
+      break
+
+
+def fetch_bin(bin_path, update_url):
+  os.makedirs(os.path.dirname(bin_path), exist_ok=True)
+  result = subprocess.run(
+    ["curl", "-L", "-o", bin_path, "-w", "%{http_code}", "--silent", update_url],
+    capture_output=True, text=True
+  )
+  status = result.stdout.strip()
+  if status == "200":
+    print("downloaded latest body firmware binary")
+  else:
+    raise RuntimeError(f"download failed with HTTP {status}")
+
+
+def flash_can(handle, code, mcu_config):
+  assert mcu_config is not None, "must set valid mcu_type to flash"
+
+  # confirm flasher is present
+  fr = handle.controlRead(REQUEST_IN, 0xb0, 0, 0, 0xc)
+  assert fr[4:8] == b"\xde\xad\xd0\x0d"
+
+  apps_sectors_cumsum = accumulate(mcu_config["sector_sizes"][1:])
+  last_sector = next((i + 1 for i, v in enumerate(apps_sectors_cumsum) if v > len(code)), -1)
+  assert last_sector >= 1, "Binary too small? No sector to erase."
+  assert last_sector < 7, "Binary too large! Risk of overwriting provisioning chunk."
+
+  print("flash: unlocking")
+  handle.controlWrite(REQUEST_IN, 0xb1, 0, 0, b'')
+
+  print(f"flash: erasing sectors 1 - {last_sector}")
+  for i in range(1, last_sector + 1):
+    handle.controlWrite(REQUEST_IN, 0xb2, i, 0, b'')
+
+  STEP = 0x10
+  print("flash: flashing")
+  for i in range(0, len(code), STEP):
+    handle.bulkWrite(2, code[i:i + STEP])
+
+
+def reset_body(handle):
+  print("flash: resetting")
+  handle.controlWrite(REQUEST_IN, 0xd8, 0, 0, b'', expect_disconnect=True)
+
+
+def update(can_send, can_recv, addr, bus, file, update_url, current_signature=None):
+  if not os.path.exists(file):
+    print("local bin is not up-to-date, fetching latest")
+    fetch_bin(file, update_url)
+
+  if current_signature is not None:
+    print("checking body firmware signature")
+    with open(file, "rb") as f:
+      expected_signature = f.read()[-128:]
+
+    print(f"expected body signature: {expected_signature.hex()}")
+    print(f"current body signature: {current_signature.hex()}")
+
+  if current_signature is None or current_signature != expected_signature:
+    print("flashing motherboard")
+    can_send(addr, b"\xce\xfa\xad\xde\x1e\x0b\xb0\x0a", bus)
+    time.sleep(0.1)
+
+    print("flashing", file)
+    flush_recv_buffer(can_recv)
+    with open(file, "rb") as f:
+      code = f.read()
+    handle = CanHandle(can_send, can_recv, bus)
+    retries = 3
+    for i in range(retries):
+      try:
+        flash_can(handle, code, F4Config)
+        reset_body(handle)
+      except (TimeoutError, RuntimeError) as e:
+        print(f"flash failed (attempt {i + 1}/{retries}): {e}, trying again...")
+      else:
+        print("successfully flashed")
+
+        # Clear cached CarParams so FW queries run fresh after flash
+        car_params_cache = "/data/params/d/CarParamsCache"
+        if os.path.isfile(car_params_cache):
+          os.remove(car_params_cache)
+          print("cleared CarParamsCache")
+        return
+
+    # on fail: attempt to exit bootloader
+    reset_body(handle)
+    raise RuntimeError(f"flash failed after {retries} attempts")
+  else:
+    print("body firmware is up to date")

opendbc/car/body/interface.py

@@ -2,14 +2,34 @@
 from opendbc.car import get_safety_config, structs
 from opendbc.car.body.carcontroller import CarController
 from opendbc.car.body.carstate import CarState
-from opendbc.car.body.values import SPEED_FROM_RPM
+from opendbc.car.body.values import SPEED_FROM_RPM, BIN_PATH, BIN_URL, FLASH_ADDR, BUS
 from opendbc.car.interfaces import CarInterfaceBase
+from opendbc.car.body.flash import update
+from opendbc.car.fw_query_definitions import StdQueries
+from opendbc.car.can_definitions import CanData
 
 
 class CarInterface(CarInterfaceBase):
   CarState = CarState
   CarController = CarController
 
+  @staticmethod
+  def init(CP, can_recv, can_send, communication_control=None):
+    fw_signature = next(
+      (fw.fwVersion for fw in CP.carFw
+      if fw.ecu == structs.CarParams.Ecu.engine
+      and fw.request[1] == StdQueries.APPLICATION_SOFTWARE_FINGERPRINT_REQUEST),
+      b""
+    )
+
+    def p_can_send(addr: int, dat: bytes, bus: int):
+      can_send([CanData(address=addr, dat=dat, src=bus)])
+
+    def p_can_recv():
+      return [(msg.address, msg.dat, msg.src) for packet in can_recv() for msg in packet]
+
+    update(p_can_send, p_can_recv, FLASH_ADDR, BUS, BIN_PATH, BIN_URL, fw_signature)
+
   @staticmethod
   def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
     ret.notCar = True

opendbc/car/body/values.py

@@ -1,3 +1,4 @@
+import os
 from opendbc.car import Bus, CarSpecs, PlatformConfig, Platforms
 from opendbc.car.structs import CarParams
 from opendbc.car.docs_definitions import CarDocs
@@ -7,6 +8,15 @@
 
 SPEED_FROM_RPM = 0.008587
 
+FIRMWARE_VERSION = "v0.3.1"
+BIN_URL = f"https://github.com/commaai/body/releases/download/{FIRMWARE_VERSION}/body.bin.signed"
+BIN_NAME = f"body-v1-{FIRMWARE_VERSION}.bin.signed"
+BODY_DIR = os.path.dirname(os.path.realpath(__file__))
+_PANDA_BIN_DIR = os.path.join(BODY_DIR, "../../../../panda/board/body/v1")
+BIN_PATH = os.path.join(_PANDA_BIN_DIR, BIN_NAME) if os.path.isdir(_PANDA_BIN_DIR) else os.path.join(BODY_DIR, BIN_NAME)
+FLASH_ADDR = 0x250
+BUS = 0
+
 
 class CarControllerParams:
   ANGLE_DELTA_BP = [0., 5., 15.]
@@ -34,6 +44,11 @@ class CAR(Platforms):
       [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
       bus=0,
     ),
+    Request(
+      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.APPLICATION_SOFTWARE_FINGERPRINT_REQUEST],
+      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.APPLICATION_SOFTWARE_FINGERPRINT_RESPONSE],
+      bus=0,
+    ),
   ],
 )
 

opendbc/car/fw_query_definitions.py

@@ -65,6 +65,11 @@ class StdQueries:
   UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
     p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
 
+  APPLICATION_SOFTWARE_FINGERPRINT_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
+    p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_FINGERPRINT)
+  APPLICATION_SOFTWARE_FINGERPRINT_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
+    p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_FINGERPRINT)
+
   OBD_VERSION_REQUEST = b'\x09\x04'
   OBD_VERSION_RESPONSE = b'\x49\x04'
 

opendbc/car/tests/test_fw_fingerprint.py

@@ -262,10 +262,10 @@ def fake_get_ecu_addrs(*_, timeout):
         print(f'get_vin {name} case, query time={self.total_time / self.N} seconds')
 
   def test_fw_query_timing(self):
-    total_ref_time = 7.4
+    total_ref_time = 7.5
     brand_ref_times = {
       'gm': 1.0,
-      'body': 0.1,
+      'body': 0.2,
       'chrysler': 0.3,
       'ford': 1.5,
       'honda': 0.45,