profile usb gpu link stability (#38368)

* profile usb gpu link before using

* ci

openpilot/selfdrive/modeld/compile_modeld.py

@@ -13,6 +13,7 @@ from collections import namedtuple
 import numpy as np
 
 from openpilot.selfdrive.modeld.helpers import dump_oob, load_oob
+from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link
 
 def _patch_tinygrad_fetch_fw():
   import hashlib
@@ -311,6 +312,9 @@ if __name__ == "__main__":
   p.add_argument('--frame-skip', type=int, required=True)
   args = p.parse_args()
 
+  if 'USB+AMD' in os.environ.get('DEV', ''):
+    wait_usbgpu_link()
+
   model_path = read_file_chunked_to_disk(args.onnx)
   model_w, model_h = args.model_size
 

openpilot/selfdrive/modeld/modeld.py

@@ -25,6 +25,7 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_drivi
 from openpilot.common.file_chunker import open_file_chunked, get_manifest_path
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld.helpers import usbgpu_present, modeld_pkl_path, get_tg_input_devices, load_oob
+from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link
 
 PROCESS_NAME = "openpilot.selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -168,6 +169,8 @@ def main(demo=False):
   if use_extra_client:
     cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
 
+  if USBGPU:
+    wait_usbgpu_link()
   st = time.monotonic()
   cloudlog.warning("loading model")
   model = ModelState(vipc_client_main.width, vipc_client_main.height, USBGPU)

openpilot/selfdrive/modeld/usbgpu_link.py

@@ -0,0 +1,34 @@
+import time
+from pathlib import Path
+
+from openpilot.common.swaglog import cloudlog
+from openpilot.common.hardware.usb import CHESTNUT_VENDOR_ID, CHESTNUT_PRODUCT_ID, usb_devices, controller, read_int
+
+STABLE_SECONDS = 2.0
+STABLE_THRESHOLD = 5.0  # link errors per second
+
+
+def _chestnut_portli() -> Path | None:
+  for device in usb_devices():
+    if read_int(device / "idVendor", 16) == CHESTNUT_VENDOR_ID and \
+       read_int(device / "idProduct", 16) == CHESTNUT_PRODUCT_ID:
+      ctrl = controller(device)
+      if ctrl is not None and (ctrl / "portli").exists():
+        return ctrl / "portli"
+  return None
+
+
+def wait_usbgpu_link(timeout: float = 30.0) -> None:
+  portli = _chestnut_portli()
+  if portli is None:
+    return
+
+  t0 = time.monotonic()
+  while time.monotonic() - t0 < timeout:
+    start = read_int(portli, 0)
+    time.sleep(STABLE_SECONDS)
+    rate = (read_int(portli, 0) - start) / STABLE_SECONDS
+    if rate <= STABLE_THRESHOLD:
+      return
+    cloudlog.warning(f"usbgpu link not stable: {rate:.0f} errors/s")
+  cloudlog.error("usbgpu link never stabilized")