pandad: flasher for Rivian long upgrade module (#1712)

* flasher for Rivian long upgrade

* self-contained no dependency on Panda.F4_DEVICES

* standalone flasher

* move to sp module

* use brand field directly

* use brand field directly

* use brand field directly

* bump

* add some logging

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

selfdrive/pandad/pandad.py

@@ -12,6 +12,8 @@ from openpilot.common.params import Params
 from openpilot.system.hardware import HARDWARE
 from openpilot.common.swaglog import cloudlog
 
+from openpilot.sunnypilot.selfdrive.pandad.rivian_long_flasher import flash_rivian_long
+
 
 def get_expected_signature() -> bytes:
   try:
@@ -129,6 +131,9 @@ def main() -> None:
       for serial in panda_serials:
         pandas.append(flash_panda(serial))
 
+      # flash Rivian longitudinal upgrade panda
+      flash_rivian_long(pandas)
+
       # Ensure internal panda is present if expected
       internal_pandas = [panda for panda in pandas if panda.is_internal()]
       if HARDWARE.has_internal_panda() and len(internal_pandas) == 0:

sunnypilot/selfdrive/pandad/rivian_long_flasher.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import os
+from itertools import accumulate
+
+from cereal import car, messaging
+from panda import Panda
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+
+FW_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), "rivian_long_fw.bin.signed")
+SECTOR_SIZES = [0x4000] * 4 + [0x10000] + [0x20000] * 11
+
+
+def _is_rivian() -> bool:
+  params = Params()
+
+  # check fixed fingerprint
+  if bundle := params.get("CarPlatformBundle"):
+    if bundle.get("brand") == "rivian":
+      return True
+
+  # check cached fingerprint
+  CP_bytes = params.get("CarParamsPersistent")
+  if CP_bytes is not None:
+    CP = messaging.log_from_bytes(CP_bytes, car.CarParams)
+    if CP.brand == "rivian":
+      return True
+
+  return False
+
+
+def _flash_static(handle, code):
+  assert Panda.flasher_present(handle)
+  last_sector = next((i + 1 for i, v in enumerate(accumulate(SECTOR_SIZES[1:])) if v > len(code)), -1)
+  assert 1 <= last_sector < 7, "Invalid firmware size"
+
+  handle.controlWrite(Panda.REQUEST_IN, 0xb1, 0, 0, b'')
+  for i in range(1, last_sector + 1):
+    handle.controlWrite(Panda.REQUEST_IN, 0xb2, i, 0, b'')
+  for i in range(0, len(code), 0x10):
+    handle.bulkWrite(2, code[i:i + 0x10])
+  try:
+    handle.controlWrite(Panda.REQUEST_IN, 0xd8, 0, 0, b'', expect_disconnect=True)
+  except Exception:
+    pass
+
+
+def _flash_panda(panda: Panda) -> None:
+  expected_sig = Panda.get_signature_from_firmware(FW_PATH)
+  if not panda.bootstub and panda.get_signature() == expected_sig:
+    cloudlog.info(f"F4 panda {panda.get_usb_serial()} already up to date")
+    return
+
+  cloudlog.info(f"Flashing F4 panda {panda.get_usb_serial()}")
+  with open(FW_PATH, "rb") as f:
+    code = f.read()
+
+  if not panda.bootstub:
+    # enter bootstub directly, panda.reset() rejects deprecated hw types
+    try:
+      panda._handle.controlWrite(Panda.REQUEST_IN, 0xd1, 1, 0, b'', timeout=15000, expect_disconnect=True)
+    except Exception:
+      pass
+    panda.close()
+    panda.reconnect()
+
+  _flash_static(panda._handle, code)
+  panda.reconnect()
+
+
+def flash_rivian_long(pandas: list[Panda]) -> None:
+  if not os.path.isfile(FW_PATH):
+    cloudlog.error(f"Rivian longitudinal upgrade firmware not found at {FW_PATH}")
+    return
+
+  if not _is_rivian():
+    cloudlog.info("Not a Rivian, skipping longitudinal upgrade...")
+    return
+
+  for panda in pandas:
+    # only flash external black pandas (HW_TYPE_BLACK = 0x03)
+    if panda.get_type() == b'\x03' and not panda.is_internal():
+      try:
+        _flash_panda(panda)
+      except Exception:
+        cloudlog.exception(f"Failed to flash F4 panda {panda.get_usb_serial()}")
+
+
+if __name__ == '__main__':
+  flash_rivian_long([Panda(s) for s in Panda.list()])