Revert "monitor chestnut USB in hardwared (#38741)" (#38744)

This reverts commit 7d5596d5c3.

openpilot/cereal/log.capnp

@@ -2593,7 +2593,6 @@ struct Event {
     clocks @35 :Clocks;
     deviceState @6 :DeviceState;
     chestnutState @152 :ChestnutState;
-    chestnutGpuState @153 :ChestnutState;
     logMessage @18 :Text;
     errorLogMessage @85 :Text;
 

openpilot/cereal/services.py

@@ -26,7 +26,6 @@ _services: dict[str, tuple] = {
   "temperatureSensor": (True, 2., 200),
   "deviceState": (True, 2., 1),
   "chestnutState": (True, 10., 10),
-  "chestnutGpuState": (False, 10.),
   "touch": (True, 20., 1),
   "can": (True, 100., 2053, QueueSize.BIG),  # decimation gives ~3 msgs in a full segment
   "controlsState": (True, 100., 10, QueueSize.MEDIUM),

openpilot/selfdrive/modeld/modeld.py

@@ -6,6 +6,8 @@ import os
 os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
 from tinygrad.tensor import Tensor
 from tinygrad.device import Device
+import usb1
+import struct
 import threading
 import time
 import numpy as np
@@ -30,6 +32,7 @@ from openpilot.selfdrive.modeld.parse_model_outputs import Parser
 from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_INPUTS, POLICY_INPUTS
 from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_driving_model_data, fill_pose_msg, PublishState
 from openpilot.common.file_chunker import open_file_chunked
+from openpilot.common.hardware.usb import CHESTNUT_USB_IDS
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld.helpers import chestnut_present, chestnut_compiled, chestnut_ready, modeld_pkl_path, get_tg_input_devices, load_oob
 
@@ -70,14 +73,45 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
                                 shouldStop=bool(stop))
 
 
-class ChestnutGpuState:
-  # SMU metrics are only accessible from modeld.
+class ChestnutState:
+  # only modeld can access chestnut
   def __init__(self, pm: PubMaster, big: bool):
     self.pm = pm
     self.big = big
     self.valid = True
     self.sends = 0
     self.metrics = {}
+    self._asm_usb = None
+
+  def _close_asm_usb(self) -> None:
+    if self._asm_usb is not None:
+      self._asm_usb.close()
+      self._asm_usb = None
+
+  def _open_asm_usb(self):
+    context = usb1.USBContext()
+    for vendor_id, product_id in CHESTNUT_USB_IDS:
+      if (handle := context.openByVendorIDAndProductID(vendor_id, product_id, skip_on_error=True)) is not None:
+        return handle
+    context.close()
+
+  def _read_ina(self) -> tuple[int, int, bool]:
+    if "AMD" in Device._opened_devices and self._asm_usb is None:
+      try:
+        raw = Device["AMD"].iface.pci_dev.usb.usb.control_read(0xC0, 5)
+        return struct.unpack('<Hh?', bytes(raw))
+      except Exception:
+        pass
+    if self._asm_usb is None:
+      self._asm_usb = self._open_asm_usb()
+    if self._asm_usb is None:
+      raise usb1.USBErrorNoDevice
+    try:
+      raw = self._asm_usb.controlRead(0xC0, 0xC0, 0, 0, 5, timeout=100)
+    except usb1.USBError:
+      self._close_asm_usb()
+      raise
+    return struct.unpack('<Hh?', bytes(raw))
 
   @cached_property
   def power_limit(self) -> int:
@@ -85,6 +119,8 @@ class ChestnutGpuState:
     return smu._send_msg(smu.smu_mod.PPSMC_MSG_GetPptLimit, 0, read_back_arg=True, timeout=100)
 
   def send(self) -> None:
+    msg = messaging.new_message('chestnutState')
+    state = msg.chestnutState
     self.sends += 1
     if self.big and "AMD" in Device._opened_devices and self.sends % 100 == 1:
       try:
@@ -106,14 +142,25 @@ class ChestnutGpuState:
           cloudlog.exception("chestnut state read failed")
         self.valid = False
         self.metrics.clear()
-
-    msg = messaging.new_message('chestnutGpuState')
-    state = msg.chestnutGpuState
     if self.big:
       for k, v in self.metrics.items():
         setattr(state, k, v)
-    msg.valid = self.big and self.valid
-    self.pm.send('chestnutGpuState', msg)
+
+    asm_valid = False
+    try:
+      # ASM runs on USB-C power, these still read without a gpu
+      state.supplyVoltage, state.supplyCurrent, state.supplyFault = self._read_ina()
+      asm_valid = True
+    except Exception:
+      pass
+    if "AMD" in Device._opened_devices:
+      try:
+        state.pcieLtssm = Device["AMD"].iface.pci_dev.usb.read(0xB450, 1)[0]
+      except Exception:
+        pass
+
+    msg.valid = asm_valid and (not self.big or self.valid)
+    self.pm.send('chestnutState', msg)
 
 
 class FrameMeta:
@@ -280,13 +327,13 @@ def main(demo=False):
   cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
 
   # messaging
-  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry"] + (["chestnutGpuState"] if CHESTNUT else [])
+  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry"] + (["chestnutState"] if CHESTNUT else [])
   pm = PubMaster(pub_socks)
   sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
 
   publish_state = PublishState()
   params = Params()
-  chestnut_state = ChestnutGpuState(pm, model.chestnut) if CHESTNUT else None
+  chestnut_state = ChestnutState(pm, model.chestnut) if CHESTNUT else None
 
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ)
@@ -395,7 +442,7 @@ def main(demo=False):
     mt1 = time.perf_counter()
     try:
       send_chestnut = (chestnut_state is not None and
-                       run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutGpuState'].frequency) == 0)
+                       run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0)
       model_output = model.run(bufs, transforms, inputs, chestnut_state.send if send_chestnut else None)
     except Exception:
       if not params.get_bool("ChestnutActive"):

openpilot/system/hardware/chestnut/monitoring.py

@@ -1,124 +0,0 @@
-import struct
-from contextlib import suppress
-
-import usb1
-
-import openpilot.cereal.messaging as messaging
-from openpilot.cereal.services import SERVICE_LIST
-from openpilot.common.hardware.usb import CHESTNUT_USB_IDS
-
-
-USB_TIMEOUT_MS = 100
-PCIE_LTSSM_ADDRESS = 0xB450
-
-
-class ChestnutUsb:
-  def __init__(self):
-    self.context: usb1.USBContext | None = None
-    self.handle = None
-
-  def close(self) -> None:
-    handle, context = self.handle, self.context
-    self.handle = None
-    self.context = None
-    with suppress(Exception):
-      if handle is not None:
-        handle.close()
-    with suppress(Exception):
-      if context is not None:
-        context.close()
-
-  def connect(self) -> bool:
-    if self.handle is not None:
-      return True
-
-    context = usb1.USBContext()
-    for vendor_id, product_id in CHESTNUT_USB_IDS:
-      handle = context.openByVendorIDAndProductID(vendor_id, product_id, skip_on_error=True)
-      if handle is not None:
-        self.context = context
-        self.handle = handle
-        return True
-    context.close()
-    return False
-
-  def _read(self, request: int, value: int, length: int) -> bytes:
-    if self.handle is None:
-      raise usb1.USBErrorNoDevice
-    raw = bytes(self.handle.controlRead(0xC0, request, value, 0, length, timeout=USB_TIMEOUT_MS))
-    if len(raw) != length:
-      raise ValueError(f"short chestnut USB response: {len(raw)}/{length}")
-    return raw
-
-  def read_ina(self) -> tuple[int, int, bool]:
-    return struct.unpack('<Hh?', self._read(0xC0, 0, 5))
-
-  def read_pcie_ltssm(self) -> int:
-    return self._read(0xE4, PCIE_LTSSM_ADDRESS, 1)[0]
-
-
-class ChestnutMonitoring:
-  def __init__(self, usb: ChestnutUsb | None = None):
-    self.usb = usb or ChestnutUsb()
-    self.gpu_state = None
-    self.seen = False
-    self.enabled = False
-    self.usb_failed = False
-
-  def set_enabled(self, enabled: bool) -> None:
-    if self.enabled == enabled:
-      return
-    self.enabled = enabled
-    self.usb.close()
-    self.usb_failed = False
-
-  def retry(self) -> None:
-    self.usb_failed = False
-
-  def model_alive(self, sm: messaging.SubMaster, now: float) -> bool:
-    modeld = next((p for p in sm['managerState'].processes if p.name == 'modeld'), None)
-    if modeld is not None and modeld.shouldBeRunning and not modeld.running:
-      return False
-    recv_time = sm.recv_time['chestnutGpuState']
-    return recv_time > 0. and now - recv_time < 10. / SERVICE_LIST['chestnutGpuState'].frequency
-
-  def update_gpu_state(self, sm: messaging.SubMaster, now: float) -> None:
-    if sm.updated['chestnutGpuState']:
-      self.gpu_state = sm['chestnutGpuState'] if sm.valid['chestnutGpuState'] else None
-    elif not self.model_alive(sm, now):
-      self.gpu_state = None
-
-  def update(self, sm: messaging.SubMaster, now: float, model_loading: bool = False):
-    self.update_gpu_state(sm, now)
-    return self.build_message(model_loading)
-
-  def build_message(self, model_loading: bool = False):
-    if not self.enabled:
-      return None
-
-    msg = messaging.new_message('chestnutState')
-    if self.gpu_state is not None:
-      msg.chestnutState = self.gpu_state
-    state = msg.chestnutState
-
-    if self.usb_failed:
-      return msg if self.seen else None
-
-    try:
-      if not self.usb.connect():
-        self.usb_failed = True
-        return msg if self.seen else None
-
-      self.seen = True
-      voltage, current, fault = self.usb.read_ina()
-      pcie_ltssm = self.usb.read_pcie_ltssm()
-      state.supplyVoltage = voltage
-      state.supplyCurrent = current
-      state.supplyFault = fault
-      state.pcieLtssm = pcie_ltssm
-      msg.valid = True
-    except Exception as e:
-      if not model_loading or not isinstance(e, usb1.USBErrorTimeout):
-        self.usb.close()
-        self.usb_failed = True
-    return msg

openpilot/system/hardware/chestnut/status.py

@@ -21,13 +21,14 @@ class ChestnutStatus:
     self.power_lost = False
     self.power_restored = False
     self.link_failures = 0
+    self.model_loading_seen = False
     self.model_attempted = False
     self.overheated = False
     self.usb_seen = False
     self.usb_failed = False
 
   def update(self, offroad: bool, branch: str, usb_state: list[dict], firmware_failed: bool,
-             model_active: bool | None, state, usb_failed: bool, set_alert) -> None:
+             model_loading: bool, model_active: bool | None, state, set_alert) -> None:
     detected = [d for d in usb_state if is_chestnut_usb_id(d["vendorId"], d["productId"], include_bootloader=True)]
     devices = [d for d in detected if is_chestnut_usb_id(d["vendorId"], d["productId"])]
     firmware_ok = len(devices) == 1 and devices[0]["product"] == CHESTNUT_USB_PRODUCT
@@ -39,15 +40,16 @@ class ChestnutStatus:
       self.power_lost = False
       self.power_restored = False
       self.link_failures = 0
+      self.model_loading_seen = False
       self.model_attempted = False
       self.usb_seen = firmware_ok
       self.usb_failed = False
 
-    self.model_attempted |= model_active is not None
+    self.model_loading_seen |= model_loading
+    self.model_attempted |= self.model_loading_seen and not model_loading and model_active is not None
 
-    if not offroad:
-      self.usb_seen |= firmware_ok
-    self.usb_failed = not offroad and self.usb_seen and (not firmware_ok or usb_failed)
+    if not offroad and self.usb_seen and not firmware_ok:
+      self.usb_failed = True
 
     if not offroad and state is not None:
       powered = state.supplyVoltage >= CHESTNUT_POWERED_VOLTAGE
@@ -78,28 +80,17 @@ class ChestnutStatus:
     release = branch in CHESTNUT_RELEASE_BRANCHES
     missing = self.usb_failed or (offroad and release and time.monotonic() - self.started > 10. and len(detected) != 1)
     slow_usb = offroad and len(devices) == 1 and devices[0]["speedMbps"] < 5000
-    update_failed = offroad and firmware_failed
-    compiled = firmware_ok and chestnut_compiled()
-    uncompiled = offroad and firmware_ok and not compiled
-
+    set_alert("Offroad_ChestnutBranch", not release and len(devices) == 1)
+    set_alert("Offroad_ChestnutNotDetected", missing)
+    set_alert("Offroad_ChestnutOverheated", self.overheated, f"{state.tempC:.0f} °C" if state is not None else None)
+    set_alert("Offroad_ChestnutUsbSlow", slow_usb, f"{devices[0]['speedMbps']} Mbps" if slow_usb else None)
     if self.power_lost:
       pcie_alert = ("Chestnut power restored. 12V is stable again, cycle ignition." if self.power_restored else
                     "Chestnut power disconnected. Check 12V connection, then cycle ignition." if self.power_unavailable else
                     "Chestnut power lost. Possibly caused by an engine-crank voltage drop. Check 12V connection, then cycle ignition.")
     else:
       pcie_alert = "Chestnut GPU unavailable. PCIe link is not up. Check the GPU is securely seated."
-
-    alerts = (
-      ("Offroad_ChestnutNotDetected", missing, None),
-      ("Offroad_ChestnutUpdateFailed", update_failed, None),
-      ("Offroad_ChestnutUncompiled", uncompiled, None),
-      ("Offroad_ChestnutPcieUnavailable", self.pcie_failed, pcie_alert),
-      ("Offroad_ChestnutOverheated", self.overheated, f"{state.tempC:.0f} °C" if state is not None else None),
-      ("Offroad_ChestnutUsbSlow", slow_usb, f"{devices[0]['speedMbps']} Mbps" if slow_usb else None),
-    )
-    active_alert = next((name for name, active, _ in alerts if active), None)
-
-    set_alert("Offroad_ChestnutBranch", not release and len(devices) == 1 and not missing)
-    for name, _, extra_text in alerts:
-      set_alert(name, name == active_alert, extra_text)
+    set_alert("Offroad_ChestnutPcieUnavailable", self.pcie_failed, pcie_alert)
+    set_alert("Offroad_ChestnutUncompiled", offroad and firmware_ok and not chestnut_compiled())
+    set_alert("Offroad_ChestnutUpdateFailed", offroad and firmware_failed)
     self.offroad = offroad

openpilot/system/hardware/hardwared.py

@@ -27,7 +27,6 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.system.hardware.power_monitoring import PowerMonitoring
 from openpilot.system.hardware.fan_controller import FanController
 from openpilot.system.hardware.chestnut.status import ChestnutStatus
-from openpilot.system.hardware.chestnut.monitoring import ChestnutMonitoring
 from openpilot.common.version import terms_version, training_version
 from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID
 
@@ -196,9 +195,8 @@ def hw_state_thread(end_event, hw_queue):
 
 def hardware_thread(end_event, hw_queue) -> None:
   system_stats = LinuxSystemStats()
-  pm = messaging.PubMaster(['deviceState', 'chestnutState'])
-  sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates",
-                            "chestnutState", "chestnutGpuState", "managerState"], poll="pandaStates")
+  pm = messaging.PubMaster(['deviceState'])
+  sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates", "chestnutState"], poll="pandaStates")
 
   count = 0
 
@@ -246,9 +244,7 @@ def hardware_thread(end_event, hw_queue) -> None:
 
   fan_controller = FanController(int(1./DT_HW))
   chestnut = Chestnut()
-  chestnut_monitoring = ChestnutMonitoring()
   chestnut_status = ChestnutStatus()
-  model_loading = params.get_bool("ChestnutLoading")
   branch = get_short_branch()
 
   while not end_event.is_set():
@@ -280,8 +276,6 @@ def hardware_thread(end_event, hw_queue) -> None:
     # Run at 2Hz, plus either edge of ignition
     ign_edge = (started_ts is not None) != all(onroad_conditions.values())
     if (sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_HW) != 0) and not ign_edge:
-      if (chestnut_msg := chestnut_monitoring.update(sm, time.monotonic(), model_loading)) is not None:
-        pm.send('chestnutState', chestnut_msg)
       continue
 
     msg = messaging.new_message('deviceState', valid=True)
@@ -315,11 +309,9 @@ def hardware_thread(end_event, hw_queue) -> None:
     chestnut.update(started_ts is None, last_hw_state.usb_state)
     chestnut_state = sm["chestnutState"]
     chestnut_valid = sm.alive["chestnutState"] and sm.valid["chestnutState"]
-    model_loading = params.get_bool("ChestnutLoading")
-    model_active = params.get("ChestnutActive")
     chestnut_status.update(started_ts is None, branch, last_hw_state.usb_state, chestnut.failed,
-                           model_active, chestnut_state if chestnut_valid else None, chestnut_monitoring.usb_failed,
-                           set_offroad_alert_if_changed)
+                           params.get_bool("ChestnutLoading"), params.get("ChestnutActive"),
+                           chestnut_state if chestnut_valid else None, set_offroad_alert_if_changed)
     # this subset is only used for offroad
     temp_sources = [
       msg.deviceState.memoryTempC,
@@ -427,15 +419,6 @@ def hardware_thread(end_event, hw_queue) -> None:
       if off_ts is None:
         off_ts = time.monotonic()
 
-    chestnut_usb_ready = any(is_chestnut_usb_id(d["vendorId"], d["productId"]) and d["product"] == CHESTNUT_USB_PRODUCT
-                             for d in last_hw_state.usb_state)
-    flash_active = chestnut.thread is not None and chestnut.thread.is_alive()
-    chestnut_monitoring.set_enabled(started_ts is not None and (chestnut_usb_ready or chestnut_monitoring.seen) and not flash_active)
-    if chestnut_usb_ready and chestnut_monitoring.usb_failed:
-      chestnut_monitoring.retry()
-    if (chestnut_msg := chestnut_monitoring.update(sm, time.monotonic(), model_loading)) is not None:
-      pm.send('chestnutState', chestnut_msg)
-
     # Offroad power monitoring
     voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage
     power_monitor.calculate(voltage, onroad_conditions["ignition"])