Bionic Commando

opendbc_repo/opendbc/car/gm/carcontroller.py

@@ -204,8 +204,6 @@ def should_send_acc_2cd(CP):
   return (
     CP.networkLocation == NetworkLocation.fwdCamera and
     CP.carFingerprint in CAMERA_ACC_CAR and
-    # The Trailblazer already supplies this counter on its camera bus.
-    CP.carFingerprint != CAR.CHEVROLET_TRAILBLAZER and
     CP.carFingerprint not in (CC_ONLY_CAR | SDGM_CAR) and
     not bool(getattr(CP, "flags", 0) & GMFlags.NO_CAMERA.value)
   )

opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py

@@ -361,7 +361,7 @@ def test_live_camera_path_does_not_send_pt_keepalive():
 
 
 def test_acc_2cd_replacement_only_used_with_live_camera_path():
-  assert not should_send_acc_2cd(SimpleNamespace(
+  assert should_send_acc_2cd(SimpleNamespace(
     carFingerprint=CAR.CHEVROLET_TRAILBLAZER, networkLocation=CarParams.NetworkLocation.fwdCamera, flags=0))
   assert should_send_acc_2cd(SimpleNamespace(
     carFingerprint=CAR.CHEVROLET_SILVERADO, networkLocation=CarParams.NetworkLocation.fwdCamera, flags=0))

opendbc_repo/opendbc/car/subaru/carcontroller.py

@@ -1,10 +1,10 @@
 import numpy as np
 from opendbc.can import CANPacker
-from opendbc.car import Bus, DT_CTRL, make_tester_present_msg
-from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance
+from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
+from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_std_steer_angle_limits, apply_steer_angle_limits_vm, common_fault_avoidance
 from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.subaru import subarucan
-from opendbc.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags
+from opendbc.car.subaru.values import CAR, DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags
 from opendbc.car.vehicle_model import VehicleModel
 
 # FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
@@ -14,6 +14,8 @@ MAX_STEER_RATE_FRAMES = 7  # tx control frames needed before torque can be cut
 
 _SNG_ACC_MIN_DIST = 3
 _SNG_ACC_MAX_DIST = 4.5
+_LEGACY_2025_MADS_MIN_SPEED = 0.44704
+_LEGACY_2025_MADS_MAX_STEER_ANGLE = 120.0
 
 
 def get_safety_CP():
@@ -27,6 +29,7 @@ class CarController(CarControllerBase):
     self.apply_torque_last = 0
     self.apply_steer_last = 0
     self.driver_override = False
+    self.legacy_2025_lkas_active = False
 
     self.cruise_button_prev = 0
     self.steer_rate_counter = 0
@@ -45,6 +48,28 @@ class CarController(CarControllerBase):
     self.last_standstill_frame = 0
 
   def lateral_angle(self, CC, CS):
+    if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
+      mads_only = CC.latActive and not CC.enabled
+      mads_only_ok = CS.out.vEgoRaw > _LEGACY_2025_MADS_MIN_SPEED and \
+        abs(CS.out.steeringAngleDeg) < _LEGACY_2025_MADS_MAX_STEER_ANGLE
+      lkas_active = CC.latActive and (not mads_only or mads_only_ok) and \
+        CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
+
+      if lkas_active and not self.legacy_2025_lkas_active:
+        self.apply_steer_last = CS.out.steeringAngleDeg
+
+      apply_steer = apply_std_steer_angle_limits(
+        CC.actuators.steeringAngleDeg,
+        self.apply_steer_last,
+        CS.out.vEgoRaw,
+        CS.out.steeringAngleDeg,
+        lkas_active,
+        self.p.LEGACY_2025_ANGLE_LIMITS,
+      )
+      self.apply_steer_last = apply_steer
+      self.legacy_2025_lkas_active = lkas_active
+      return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
+
     abs_torque = abs(CS.out.steeringTorque)
     if abs_torque > self.p.STEER_OVERRIDE_TORQUE_HIGH:
       self.driver_override = True

opendbc_repo/opendbc/car/subaru/interface.py

@@ -40,6 +40,8 @@ class CarInterface(CarInterfaceBase):
         ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM.value
       if ret.flags & SubaruFlags.D_PLATFORM_CAMERA:
         ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value
+      if candidate == CAR.SUBARU_LEGACY_2025:
+        ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS.value
 
     ret.steerLimitTimer = 0.4
     ret.steerActuatorDelay = 0.1

opendbc_repo/opendbc/car/subaru/tests/test_subaru.py

@@ -5,7 +5,7 @@ from types import SimpleNamespace
 import pytest
 
 from opendbc.can import CANPacker, CANParser
-from opendbc.car import Bus
+from opendbc.car import Bus, structs
 from opendbc.car.subaru import subarucan
 from opendbc.car.subaru.carcontroller import CarController
 from opendbc.car.subaru.carstate import CarState
@@ -165,6 +165,7 @@ def test_outback_2023_uses_d_platform_bus_layout():
 
   assert CP.flags & SubaruFlags.D_PLATFORM
   assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM
+  assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS)
   assert CanBus.main_for_cp(CP) == CanBus.alt
   assert CanBus.angle_for_cp(CP) == CanBus.main
   assert parsers[Bus.pt].bus == CanBus.alt
@@ -184,6 +185,7 @@ def test_legacy_2025_uses_gen2_angle_bus_layout():
   assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM)
   assert not (CP.flags & SubaruFlags.D_PLATFORM_CAMERA)
   assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA)
+  assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS
   assert CanBus.main_for_cp(CP) == CanBus.main
   assert CanBus.angle_for_cp(CP) == CanBus.main
   assert parsers[Bus.pt].bus == CanBus.main
@@ -194,6 +196,36 @@ def test_legacy_2025_uses_gen2_angle_bus_layout():
   assert controller.status_bus == CanBus.main
 
 
+def test_legacy_2025_uses_validated_angle_request_limits():
+  CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
+  controller = CarController({}, CP)
+  CC = SimpleNamespace(
+    enabled=False,
+    latActive=True,
+    actuators=SimpleNamespace(steeringAngleDeg=-73.05),
+  )
+  CS = SimpleNamespace(out=SimpleNamespace(
+    vEgoRaw=2.4,
+    steeringAngleDeg=-75.27,
+    gearShifter=structs.CarState.GearShifter.drive,
+    standstill=False,
+  ))
+
+  msg = controller.lateral_angle(CC, CS)
+  parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
+  parser.update([(1, [msg])])
+
+  assert parser.can_valid
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(-73.05)
+
+  CS.out.standstill = True
+  msg = controller.lateral_angle(CC, CS)
+  parser.update([(2, [msg])])
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
+
+
 def test_ascent_2023_uses_d_platform_bus_layout():
   CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
   parsers = CarState.get_can_parsers(CP)

opendbc_repo/opendbc/car/subaru/values.py

@@ -19,6 +19,11 @@ class CarControllerParams:
     MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * 0.06),
     MAX_ANGLE_RATE=1,
   )
+  LEGACY_2025_ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
+    545,
+    ([0., 5., 35.], [5., .8, .15]),
+    ([0., 5., 35.], [5., .8, .15]),
+  )
 
   def __init__(self, CP):
     self.STEER_STEP = 2                # how often we update the steer cmd
@@ -76,6 +81,7 @@ class SubaruSafetyFlags(IntFlag):
   LKAS_ANGLE = 16
   D_PLATFORM = 32
   D_PLATFORM_CAMERA = 64
+  LEGACY_2025_ANGLE_LIMITS = 128
 
 
 class SubaruFlags(IntFlag):

opendbc_repo/opendbc/safety/modes/subaru.h

@@ -107,6 +107,7 @@ static bool subaru_longitudinal = false;
 static bool subaru_stop_and_go = false;
 static bool subaru_lkas_angle = false;
 static bool subaru_d_platform = false;
+static bool subaru_legacy_2025_angle_limits = false;
 
 static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
   return (uint8_t)msg->data[0];
@@ -191,6 +192,20 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
     .frequency = 50U,
   };
 
+  const AngleSteeringLimits SUBARU_LEGACY_2025_ANGLE_STEERING_LIMITS = {
+    .max_angle = 545 * 100,
+    .angle_deg_to_can = 100.,
+    .angle_rate_up_lookup = {
+      {0.0, 5.0, 35.0},
+      {5.0, 0.8, 0.15},
+    },
+    .angle_rate_down_lookup = {
+      {0.0, 5.0, 35.0},
+      {5.0, 0.8, 0.15},
+    },
+    .frequency = 50U,
+  };
+
   const AngleSteeringParams SUBARU_ANGLE_STEERING_PARAMS = {
     .slip_factor = -0.000580374471400815,
     .steer_ratio = 13.5,
@@ -226,7 +241,11 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
     desired_angle = -1 * to_signed(desired_angle, 17);
     bool lkas_request = GET_BIT(msg, 12U);
 
-    violation |= steer_angle_cmd_checks_vm(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS, SUBARU_ANGLE_STEERING_PARAMS);
+    if (subaru_legacy_2025_angle_limits) {
+      violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_LEGACY_2025_ANGLE_STEERING_LIMITS);
+    } else {
+      violation |= steer_angle_cmd_checks_vm(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS, SUBARU_ANGLE_STEERING_PARAMS);
+    }
   }
 
   // check es_brake brake_pressure limits
@@ -356,6 +375,9 @@ static safety_config subaru_init(uint16_t param) {
   const uint16_t SUBARU_PARAM_D_PLATFORM_CAMERA = 64;
   const bool subaru_d_platform_camera = GET_FLAG(param, SUBARU_PARAM_D_PLATFORM_CAMERA);
 
+  const uint16_t SUBARU_PARAM_LEGACY_2025_ANGLE_LIMITS = 128;
+  subaru_legacy_2025_angle_limits = GET_FLAG(param, SUBARU_PARAM_LEGACY_2025_ANGLE_LIMITS);
+
 #ifdef ALLOW_DEBUG
   const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
   subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);

opendbc_repo/opendbc/safety/tests/test_subaru.py

@@ -208,6 +208,8 @@ class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafety
     super().setUp()
 
   def _get_steer_cmd_angle_max(self, speed):
+    if self.FLAGS & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS:
+      return self.STEER_ANGLE_MAX
     return get_max_angle_vm(max(speed, 1), self.VM, CarControllerParams)
 
   def _angle_cmd_msg(self, angle, enabled, increment_timer=True):
@@ -356,6 +358,14 @@ class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafe
   TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS, SubaruMsg.ES_LKAS_ANGLE)
 
 
+class TestSubaruGen2Legacy2025AngleSafety(TestSubaruGen2AngleStockLongitudinalSafety):
+  FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS
+  STEER_ANGLE_MAX = 545
+  ANGLE_RATE_BP = [0., 5., 35.]
+  ANGLE_RATE_UP = [5., .8, .15]
+  ANGLE_RATE_DOWN = [5., .8, .15]
+
+
 class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase):
   FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM
   ALT_MAIN_BUS = SUBARU_ALT_BUS

tinygrad_repo/test/unit/test_usb_mmio_scalar.py

@@ -0,0 +1,54 @@
+from tinygrad.runtime.support.usb import USBMMIOInterface
+
+
+class FakeUSB:
+  def __init__(self):
+    self.calls = []
+
+  def pcie_mem_req(self, address, value=None, size=4):
+    self.calls.append(("scalar", address, value, size))
+    return 0x11223344 if value is None else None
+
+  def pcie_mem_read(self, address, size):
+    self.calls.append(("read", address, size))
+    return bytes(size)
+
+  def pcie_mem_write(self, address, data):
+    self.calls.append(("write", address, data))
+
+
+def test_scalar_mmio_uses_single_tlp():
+  usb = FakeUSB()
+  mmio = USBMMIOInterface(usb, 0x1000, 0x100, "I")
+
+  mmio[2] = 0xAABBCCDD
+  assert mmio[3] == 0x11223344
+
+  assert usb.calls == [
+    ("scalar", 0x1008, 0xAABBCCDD, 4),
+    ("scalar", 0x100C, None, 4),
+  ]
+
+
+def test_slice_mmio_keeps_streaming_path():
+  usb = FakeUSB()
+  mmio = USBMMIOInterface(usb, 0x2000, 0x100, "I")
+
+  mmio[0:2] = b"\x01\x02\x03\x04\x05\x06\x07\x08"
+  assert mmio[0:2] == bytes(8)
+
+  assert usb.calls == [
+    ("write", 0x2000, b"\x01\x02\x03\x04\x05\x06\x07\x08"),
+    ("read", 0x2000, 8),
+  ]
+
+
+def test_scalar_mmio_falls_back_to_streaming_transport():
+  class StreamingUSB:
+    def __init__(self): self.data = bytearray(4)
+    def pcie_mem_read(self, address, size): return self.data[address-0x3000:address-0x3000+size]
+    def pcie_mem_write(self, address, data): self.data[address-0x3000:address-0x3000+len(data)] = data
+
+  mmio = USBMMIOInterface(StreamingUSB(), 0x3000, 4, "I")
+  mmio[0] = 0xAABBCCDD
+  assert mmio[0] == 0xAABBCCDD

tinygrad_repo/tinygrad/runtime/support/usb.py

@@ -135,6 +135,9 @@ class CustomASM24Controller:
     address = (bus << 24) | (dev << 19) | (fn << 16) | (byte_addr & 0xfff)
     return self.pcie_request(fmt_type, address, value, size)
 
+  def pcie_mem_req(self, address:int, value:int|None=None, size:int=4):
+    return self.pcie_request(0x60 if value is not None else 0x20, address, value, size)
+
   def pcie_mem_write(self, address:int, data:bytes):
     """Streaming PCIe memory write via 0xF0 mode 1 + bulk OUT. Data is little-endian dwords on the wire."""
     if not data: return
@@ -183,16 +186,31 @@ class USBMMIOInterface(MMIOInterface):
     if isinstance(index, slice): return ((index.start or 0) * self.el_sz, ((index.stop or len(self))-(index.start or 0)) * self.el_sz)
     return (index * self.el_sz, self.el_sz)
 
+  def _scalar(self, off:int, size:int, value:int|None=None):
+    assert size in (1, 2, 4, 8), f"invalid scalar PCIe access size {size}"
+    if not hasattr(self.usb, "pcie_mem_req"):
+      if value is not None:
+        self.usb.pcie_mem_write(self.addr + off, value.to_bytes(size, "little"))
+        return
+      return int.from_bytes(self.usb.pcie_mem_read(self.addr + off, size), "little")
+    upper = 0 if size < 8 else self.usb.pcie_mem_req(self.addr + off + 4, value if value is None else value >> 32, 4)
+    lower = self.usb.pcie_mem_req(self.addr + off, value if value is None else value & 0xffffffff, min(size, 4))
+    if value is None: return lower | (upper << 32)
+
   def __getitem__(self, index):
     off, sz = self._off_from_index(index)
     if self.pcimem:
+      if not isinstance(index, slice): return self._scalar(off, sz)
       assert sz % 4 == 0 and off % 4 == 0, f"pcie_mem_read requires 4-byte aligned access, got off={off}, sz={sz}"
       data = self.usb.pcie_mem_read(self.addr + off, sz)
     else: data = self.usb.scsi_read(sz) if self.addr == 0xf000 else self.usb.read(self.addr + off, sz)
     return int.from_bytes(data, "little") if sz == self.el_sz else data
 
   def __setitem__(self, index, data):
-    off, _ = self._off_from_index(index)
+    off, sz = self._off_from_index(index)
+    if self.pcimem and not isinstance(index, slice) and isinstance(data, int):
+      self._scalar(off, sz, data)
+      return
     data = struct.pack(self.fmt, data) if isinstance(data, int) else bytes(data)
     if not self.pcimem: self.usb.scsi_write(data) if self.addr == 0xf000 else self.usb.write(self.addr + off, data)
     else: self.usb.pcie_mem_write(self.addr+off, data)