diff --git a/cereal/log.capnp b/cereal/log.capnp index 6cf4781228aaa4..37ee2476f47d8b 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -414,6 +414,10 @@ struct CanData { struct DeviceState @0xa4d8b5af2aa492eb { deviceType @45 :InitData.DeviceType; + # usb + chestnutPresent @51 :Bool; + usbState @52 :UsbState; + networkType @22 :NetworkType; networkInfo @31 :NetworkInfo; networkStrength @24 :NetworkStrength; @@ -684,6 +688,26 @@ struct PeripheralState { } } +struct UsbState { + vbusMv @0 :UInt32; + devices @1 :List(Device); + + struct Device { + busnum @0 :UInt8; + devnum @1 :UInt8; + vendorId @2 :UInt16; + productId @3 :UInt16; + speedMbps @4 :UInt16; + product @5 :Text; + pmActive @6 :Bool; + runtimeSuspendedMs @7 :UInt64; + + # error counters + overCurrentCount @8 :UInt32; + linkErrorCount @9 :UInt32; + } +} + struct RadarState @0x9a185389d6fdd05f { mdMonoTime @6 :UInt64; carStateMonoTime @11 :UInt64; diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index 5db73403e11f98..4c7a9e86a6942a 100755 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -18,6 +18,7 @@ from openpilot.common.realtime import DT_HW from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.system.hardware import HARDWARE, TICI, AGNOS, PC +from openpilot.system.hardware.usb import UsbLogger from openpilot.system.loggerd.config import get_available_percent from openpilot.system.statsd import statlog from openpilot.common.swaglog import cloudlog @@ -153,6 +154,7 @@ def hardware_thread(end_event, hw_queue) -> None: pm = messaging.PubMaster(['deviceState']) sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates"], poll="pandaStates") + usb_logger = UsbLogger() count = 0 onroad_conditions: dict[str, bool] = { @@ -256,6 +258,11 @@ def hardware_thread(end_event, hw_queue) -> None: msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness() + try: + usb_logger.update(msg.deviceState) + except Exception: + cloudlog.exception("usb_logger update failed") + # this subset is only used for offroad temp_sources = [ msg.deviceState.memoryTempC, diff --git a/system/hardware/usb.py b/system/hardware/usb.py new file mode 100644 index 00000000000000..5bd60caa31de3c --- /dev/null +++ b/system/hardware/usb.py @@ -0,0 +1,90 @@ +from pathlib import Path + +from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.modeld.helpers import USBGPU_VID, USBGPU_PID + + +def read(path: Path) -> str | None: + try: + return path.read_text().strip() + except OSError: + return None + + +def read_int(path: Path, base: int = 10) -> int: + s = read(path) + try: + return int(s, base) if s is not None else 0 + except ValueError: + return 0 + + +def usb_devices() -> list[Path]: + # enumerated USB devices + devices = (d for d in Path("/sys/bus/usb/devices").glob("*") if (d / "idVendor").exists()) + return sorted(devices, key=lambda p: p.name) + + +def root_hub_port(device: Path) -> Path: + bus, _, port = device.name.partition("-") + return Path(f"/sys/bus/usb/devices/usb{bus}/{bus}-0:1.0/usb{bus}-port{port}") + + +def controller(device: Path) -> Path | None: + # get SS port registers + for parent in device.resolve().parents: + if parent.name.endswith(".ssusb"): + return parent + return None + + +class UsbLogger: + def __init__(self): + self.prev: set[tuple[int, int]] = set() + + def update(self, device_state) -> None: + devices = usb_devices() + + # low level state + state = device_state.usbState + state.vbusMv = read_int(Path("/sys/class/power_supply/usb/voltage_now")) // 1000 + entries = state.init('devices', len(devices)) + + present: dict[tuple[int, int], Path] = {} + chestnut_present = False + for entry, device in zip(entries, devices, strict=True): + vendor_id = read_int(device / "idVendor", 16) + product_id = read_int(device / "idProduct", 16) + busnum = read_int(device / "busnum") + devnum = read_int(device / "devnum") + present[(busnum, devnum)] = device + + entry.busnum = busnum + entry.devnum = devnum + entry.vendorId = vendor_id + entry.productId = product_id + entry.speedMbps = read_int(device / "speed") + entry.product = read(device / "product") or "" + entry.pmActive = read(device / "power/runtime_status") == "active" + entry.runtimeSuspendedMs = read_int(device / "power/runtime_suspended_time") + entry.overCurrentCount = read_int(root_hub_port(device) / "over_current_count") + + ctrl = controller(device) + if ctrl is not None: + entry.linkErrorCount = read_int(ctrl / "portli", 0) & 0xFFFF # decode PORTLI[15:0] + + if (vendor_id, product_id) == (USBGPU_VID, USBGPU_PID): + chestnut_present = True + + # parse peripherals + device_state.chestnutPresent = chestnut_present + + # connect/disconnect events + for key in present.keys() - self.prev: + device = present[key] + cloudlog.event("usb_connected", busnum=key[0], devnum=key[1], + vid=read(device / "idVendor"), pid=read(device / "idProduct"), + speed=read(device / "speed"), product=read(device / "product")) + for key in self.prev - present.keys(): + cloudlog.event("usb_disconnected", busnum=key[0], devnum=key[1]) + self.prev = set(present)