Support manual Matrix CAN filter configurations

opendbc_repo/opendbc/car/toyota/interface.py

@@ -150,6 +150,8 @@ class CarInterface(CarInterfaceBase):
     ret.openpilotLongitudinalControl = ((bool(ret.flags & ToyotaFlags.TSS2) and not (ret.flags & ToyotaFlags.RADAR_ACC)) or
                                         bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) or (radar_filter and alpha_long))
 
+    from opendbc.car.toyota import matrix_filter
+    manual_matrix_filter = any(matrix_filter.hardware_supported(candidate, fingerprint, car_fw, length=size) for size in (4, 8))
     if candidate in (CAR.TOYOTA_MATRIX_RETROFIT, CAR.TOYOTA_PRIUS_RETROFIT):
       # These manually selected retrofits currently own stock PCM longitudinal.
       # Observed takeover hardware needs its paired controller/native port.
@@ -167,7 +169,7 @@ class CarInterface(CarInterfaceBase):
       if smart_dsu:
         ret.flags |= int(ToyotaFlags.LONG_FILTER)
         ret.safetyConfigs[0].safetyParam |= int(ToyotaSafetyFlags.LONG_FILTER)
-      elif takeover:
+      elif takeover and not manual_matrix_filter:
         ret.dashcamOnly = True
       ret.openpilotLongitudinalControl = smart_dsu
       ret.alphaLongitudinalAvailable = False

opendbc_repo/opendbc/car/toyota/legacy_advert_filter.py

@@ -14,6 +14,9 @@ AUTO_RESUME_CARS = frozenset((CAR.TOYOTA_RAV4H, CAR.TOYOTA_HIGHLANDER, CAR.TOYOT
 
 
 def hardware_supported(candidate, fingerprint, car_fw):
+  if candidate == CAR.TOYOTA_MATRIX_RETROFIT:
+    from opendbc.car.toyota import matrix_filter
+    return matrix_filter.hardware_supported(candidate, fingerprint, car_fw, length=4)
   if (candidate not in PROFILES or fingerprint.get(0, {}).get(0x2FF) != 4 or
       any(address in messages for messages in fingerprint.values() for address in (0x201, 0x2AA)) or
       any(bus not in (0, 2) or messages[0x2FF] != 4 for bus, messages in fingerprint.items() if 0x2FF in messages) or
@@ -44,6 +47,9 @@ def apply(cp):
 
 
 def configured(cp):
+  if cp is not None and cp.carFingerprint == CAR.TOYOTA_MATRIX_RETROFIT:
+    from opendbc.car.toyota import matrix_filter
+    return matrix_filter.configured(cp, length=4)
   if (cp is None or cp.brand != 'toyota' or cp.carFingerprint not in PROFILES or cp.notCar or cp.passive or cp.dashcamOnly or
       cp.secOcRequired or not cp.pcmCruise or len(cp.safetyConfigs) != 1 or
       cp.steerControlType != structs.CarParams.SteerControlType.torque):

opendbc_repo/opendbc/car/toyota/legacy_sdsu.py

@@ -6,6 +6,7 @@ from opendbc.car.toyota.values import CAR, ToyotaFlags
 
 PROFILES = {
   CAR.TOYOTA_COROLLA: (0x9158, 0x9358),
+  CAR.TOYOTA_MATRIX_RETROFIT: (0x9158, 0x9358),
   CAR.TOYOTA_RAV4: (0x9149, 0x9349),
   CAR.LEXUS_ES: (0x9149, 0x9349),
   CAR.TOYOTA_PRIUS_V: (0x9164, 0x9364),
@@ -19,6 +20,9 @@ ALT_MAIN_CARS = frozenset((CAR.LEXUS_GS_F, CAR.LEXUS_IS, CAR.LEXUS_RC))
 
 
 def hardware_supported(candidate, fingerprint, car_fw):
+  if candidate == CAR.TOYOTA_MATRIX_RETROFIT:
+    from opendbc.car.toyota import matrix_filter
+    return matrix_filter.hardware_supported(candidate, fingerprint, car_fw, length=8)
   if (
     candidate not in PROFILES
     or fingerprint.get(0, {}).get(0x2FF) != 8
@@ -54,6 +58,9 @@ def apply(cp):
 
 
 def configured(cp):
+  if cp is not None and cp.carFingerprint == CAR.TOYOTA_MATRIX_RETROFIT:
+    from opendbc.car.toyota import matrix_filter
+    return matrix_filter.configured(cp, length=8)
   if (
     cp is None
     or cp.brand != "toyota"

opendbc_repo/opendbc/car/toyota/matrix_filter.py

@@ -0,0 +1,37 @@
+"""Explicit manual Matrix enrollment into existing EPS88 filter protocols.
+
+This identity is manually selected, not a firmware-catalog match. The filter
+never supplies physical MAIN or EPS authority. Their current native sources,
+checksums, health checks and permissions remain required.
+"""
+from opendbc.car import structs
+from opendbc.car.toyota.values import CAR, ToyotaFlags
+
+PROTOCOLS = {8: (0x9158, 0x9358), 4: (0x9558, 0x9758)}
+
+
+def hardware_supported(candidate, fingerprint, car_fw, *, length):
+  if (candidate != CAR.TOYOTA_MATRIX_RETROFIT or length not in PROTOCOLS or
+      fingerprint.get(0, {}).get(0x2FF) != length or
+      any(0x2FF in messages and (bus not in (0, 2) or messages[0x2FF] != length)
+          for bus, messages in fingerprint.items()) or
+      any(address in messages for messages in fingerprint.values() for address in (0x201, 0x2AA)) or
+      any(fingerprint.get(0, {}).get(address, size) != size for address, size in ((0x1D3, 8), (0x262, 5))) or
+      any(fingerprint.get(2, {}).get(address, 8) != 8 for address in (0x343, 0x4CB))):
+    return False
+  # Preserve the manual-selection boundary without inventing catalog replies.
+  critical = (structs.CarParams.Ecu.eps, structs.CarParams.Ecu.fwdCamera, structs.CarParams.Ecu.fwdRadar)
+  return not any(fw.ecu in critical and (fw.logging or fw.bus != 0 or fw.brand != 'toyota' or not bytes(fw.fwVersion))
+                 for fw in car_fw)
+
+
+def configured(cp, *, length):
+  if (cp is None or cp.brand != 'toyota' or cp.carFingerprint != CAR.TOYOTA_MATRIX_RETROFIT or
+      length not in PROTOCOLS or cp.notCar or cp.passive or cp.dashcamOnly or cp.secOcRequired or
+      not cp.pcmCruise or cp.steerControlType != structs.CarParams.SteerControlType.torque or
+      len(cp.safetyConfigs) != 1 or cp.autoResumeSng or cp.alphaLongitudinalAvailable):
+    return False
+  flags = ToyotaFlags.LONG_FILTER if length == 8 else ToyotaFlags.RADAR_CAN_FILTER
+  config = cp.safetyConfigs[0]
+  return (cp.flags == int(flags) and config.safetyModel == structs.CarParams.SafetyModel.toyota and
+          config.safetyParam == PROTOCOLS[length][not cp.openpilotLongitudinalControl])

opendbc_repo/opendbc/car/toyota/tests/test_matrix_filter.py

@@ -0,0 +1,80 @@
+"""Manual Matrix filter enrollment and unchanged factory values."""
+import struct
+
+import pytest
+from opendbc.car import gen_empty_fingerprint, structs
+from opendbc.car.toyota import legacy_advert_filter, legacy_sdsu, matrix_filter
+from opendbc.car.toyota.interface import CarInterface
+from opendbc.car.toyota.values import CAR
+
+
+def factory(fingerprint, firmware=(), alpha=False):
+  return CarInterface.get_params(CAR.TOYOTA_MATRIX_RETROFIT, fingerprint, list(firmware), alpha, False, False)
+
+
+@pytest.mark.parametrize('length', (4, 8))
+@pytest.mark.parametrize('alpha', (False, True))
+@pytest.mark.parametrize('stock', (False, True))
+def test_manual_matrix_filter_factory_preserves_tunes(length, alpha, stock):
+  baseline = factory(gen_empty_fingerprint(), alpha=alpha)
+  fingerprint = gen_empty_fingerprint()
+  fingerprint[0][0x2FF] = length
+  cp = factory(fingerprint, alpha=alpha)
+  assert not cp.dashcamOnly, 'MANUAL_MATRIX_FILTER_NOT_ADMITTED'
+  assert cp.openpilotLongitudinalControl
+  assert matrix_filter.configured(cp, length=length)
+  if stock:
+    owner = legacy_sdsu if length == 8 else legacy_advert_filter
+    assert owner.prepare_stock(cp)
+  assert cp.safetyConfigs[0].safetyParam == matrix_filter.PROTOCOLS[length][stock]
+  assert cp.openpilotLongitudinalControl == (not stock)
+  assert not cp.autoResumeSng and not cp.alphaLongitudinalAvailable
+  assert cp.lateralTuning.torque.latAccelFactor == struct.unpack('<f', struct.pack('<f', 4.05))[0]
+  assert cp.lateralTuning.torque.friction == struct.unpack('<f', struct.pack('<f', .1))[0]
+  actual, before = cp.to_dict(), baseline.to_dict()
+  for state in (actual, before):
+    for field in ('flags', 'safetyConfigs', 'openpilotLongitudinalControl'):
+      state.pop(field)
+  assert actual == before, 'Filter enrollment must preserve all other existing Matrix configuration and tunes'
+
+
+@pytest.mark.parametrize('length', (4, 8))
+@pytest.mark.parametrize('mutation', ('wrong-primary-length', 'wrong-mirror-length', 'foreign-bus',
+                                    'pedal-pt', 'pedal-cam', 'camera-filter-pt', 'camera-filter-cam',
+                                    'wrong-eps-length', 'wrong-main-length', 'wrong-camera-acc-length'))
+def test_manual_matrix_filter_rejects_other_topologies(length, mutation):
+  fingerprint = gen_empty_fingerprint()
+  fingerprint[0][0x2FF] = length
+  if mutation == 'wrong-primary-length':
+    fingerprint[0][0x2FF] = 7
+  elif mutation == 'wrong-mirror-length':
+    fingerprint[2][0x2FF] = 8 if length == 4 else 4
+  elif mutation == 'foreign-bus':
+    fingerprint[1][0x2FF] = length
+  elif mutation.startswith('pedal-'):
+    fingerprint[0 if mutation.endswith('pt') else 2][0x201] = 6
+  elif mutation.startswith('camera-filter-'):
+    fingerprint[0 if mutation.endswith('pt') else 2][0x2AA] = 8
+  elif mutation == 'wrong-eps-length':
+    fingerprint[0][0x262] = 8
+  elif mutation == 'wrong-main-length':
+    fingerprint[0][0x1D3] = 7
+  else:
+    fingerprint[2][0x343] = 7
+  assert not matrix_filter.hardware_supported(CAR.TOYOTA_MATRIX_RETROFIT, fingerprint, [], length=length)
+  cp = factory(fingerprint)
+  assert cp.dashcamOnly and not cp.openpilotLongitudinalControl
+  assert not matrix_filter.configured(cp, length=length)
+
+
+@pytest.mark.parametrize('length', (4, 8))
+@pytest.mark.parametrize('field,value', (('logging', True), ('bus', 1), ('brand', 'gm'), ('fwVersion', b'')))
+def test_manual_matrix_filter_rejects_invalid_acquired_firmware_metadata(length, field, value):
+  fingerprint = gen_empty_fingerprint()
+  fingerprint[0][0x2FF] = length
+  fw = structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.eps, address=0x7A1,
+                              fwVersion=b'negative-metadata-fixture', brand='toyota', bus=0)
+  setattr(fw, field, value)
+  assert not matrix_filter.hardware_supported(CAR.TOYOTA_MATRIX_RETROFIT, fingerprint, [fw], length=length)
+  cp = factory(fingerprint, [fw])
+  assert cp.dashcamOnly and not cp.openpilotLongitudinalControl

openpilot/starpilot/car/toyota/tests/test_matrix_filter_callers.py

@@ -0,0 +1,59 @@
+"""Manual Matrix filter through real factory, daemons, C++ wire and native TX.
+
+Manual selection and CAN/perception/USB/input-clock producers are fixtures.
+No firmware match, health, permission or physical-road acceptance is claimed.
+"""
+import struct
+
+import pytest
+from opendbc.car.toyota import matrix_filter
+from opendbc.car.toyota.values import CAR, ToyotaFlags
+from openpilot.starpilot.car.toyota.tests import test_rav4h_caller_native as helpers
+from openpilot.starpilot.car.toyota.tests.test_toyota_cpp_transport import host_heartbeat_bridge as host_heartbeat_bridge
+
+source_matched_native = helpers.source_matched_native
+daemon_gc_scope = helpers.daemon_gc_scope
+
+
+@pytest.mark.parametrize('length', (4, 8))
+@pytest.mark.parametrize('stock', (False, True))
+@pytest.mark.parametrize('scenario', ('nominal', 'physical'))
+def test_manual_matrix_filter_actual_callers(length, stock, scenario, host_heartbeat_bridge):
+  def physical(packer, **fields):
+    if length == 4:
+      fields['sdsu'] = None
+    frames = helpers.physical(packer, **fields)
+    assert len(next(data for address, data, bus in frames if address == 0x262 and bus == 0)) == 5
+    assert len(next(data for address, data, bus in frames if address == 0x1D3 and bus == 0)) == 8
+    if length == 4:
+      # The high nibble of byte2 is not an SDSU state for this protocol.
+      frames.append((0x2FF, bytes.fromhex('8000f4ff'), 0))
+    return frames
+
+  def state(tick, host, cs):
+    cp = host.CP
+    assert cp.carFingerprint == CAR.TOYOTA_MATRIX_RETROFIT
+    assert cp.safetyConfigs[0].safetyParam == matrix_filter.PROTOCOLS[length][stock]
+    assert cp.flags == int(ToyotaFlags.RADAR_CAN_FILTER if length == 4 else ToyotaFlags.LONG_FILTER)
+    assert cp.lateralTuning.torque.latAccelFactor == struct.unpack('<f', struct.pack('<f', 4.05))[0]
+    assert cp.lateralTuning.torque.friction == struct.unpack('<f', struct.pack('<f', .1))[0]
+    assert not cp.autoResumeSng and not cp.alphaLongitudinalAvailable
+
+  def observe(tick, host, cs, cc, safety, native):
+    if scenario == 'physical' and tick == 355:
+      assert cs.brakePressed and cs.standstill and cc.latActive
+      assert not cc.longActive and safety.aol_get_permission_mask() == 1
+    if stock:
+      assert not cc.longActive and not safety.aol_get_permission_mask() & 2
+    if scenario == 'nominal' and tick >= 220:
+      assert cc.latActive and cc.longActive == (not stock)
+      assert safety.aol_get_permission_mask() == (1 if stock else 3)
+    if scenario == 'physical' and tick == 425:
+      assert str(cs.gearShifter) == 'reverse'
+      assert not cc.latActive and not cc.longActive and safety.aol_get_permission_mask() == 0
+
+  helpers.run_case(stock, scenario, candidate=CAR.TOYOTA_MATRIX_RETROFIT,
+                   bridge=host_heartbeat_bridge, input_clock=True, brake_stop_check_start=360,
+                   sdsu_words=matrix_filter.PROTOCOLS[length], firmware_override=[],
+                   fingerprint_frames=[(0x2FF, bytes(length), 0)], manual_selection=True, manual_filter_length=length,
+                   filter_advert=length == 4, physical_frames=physical, on_state=state, on_sample=observe)

openpilot/starpilot/car/toyota/tests/test_rav4h_caller_native.py

@@ -228,7 +228,8 @@ def control_inputs(controls, published, state, sd_events, native, now):
 def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H, bridge=None, input_clock=False, brake_stop_check_start=350,
              sdsu_words=(0x9049, 0x9249), alpha_long=False, disable_setting=None, physical_frames=None, angle=False,
              firmware_override=None, assert_hybrid_release=False, sdsu_mirror=False, on_sample=None, camera_messages=False, filter_advert=False,
-             brake_pause_mps=None, native_rx_expected=None, on_state=None, fingerprint_frames=None, manual_selection=False, native_tx_expected=None):
+             brake_pause_mps=None, native_rx_expected=None, on_state=None, fingerprint_frames=None, manual_selection=False, native_tx_expected=None,
+             manual_filter_length=None):
   SYNTHETIC_LAST.clear()
   NATIVE_LAST.clear()
   safety = libsafety_py.libsafety
@@ -248,13 +249,18 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H,
       params.put('AolBrakePauseSpeedMps', float(brake_pause_mps), block=True)
     fw = firmware(candidate) if firmware_override is None else firmware_override
     if manual_selection:
-      assert stock and fingerprint_frames == []
-      if candidate == CAR.TOYOTA_MATRIX_RETROFIT:
+      if manual_filter_length is not None:
+        assert candidate == CAR.TOYOTA_MATRIX_RETROFIT and manual_filter_length in (4, 8)
         assert not fw and firmware_override == []
+        assert fingerprint_frames == [(0x2FF, bytes(manual_filter_length), 0)]
       else:
-        assert candidate == CAR.TOYOTA_PRIUS_RETROFIT and len(fw) == 1
-        assert fw[0].ecu == structs.CarParams.Ecu.eps and fw[0].address == 0x7A1 and fw[0].bus == 0
-        assert bytes(fw[0].fwVersion) == b'8965B47070' + bytes(6)
+        assert stock and fingerprint_frames == []
+        if candidate == CAR.TOYOTA_MATRIX_RETROFIT:
+          assert not fw and firmware_override == []
+        else:
+          assert candidate == CAR.TOYOTA_PRIUS_RETROFIT and len(fw) == 1
+          assert fw[0].ecu == structs.CarParams.Ecu.eps and fw[0].address == 0x7A1 and fw[0].bus == 0
+          assert bytes(fw[0].fwVersion) == b'8965B47070' + bytes(6)
     else:
       exact, found = match_fw_to_car(fw, '', log=False)
       assert exact and found == {candidate}

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "f66ce7991c8b276625982b5f71707ca60643482518ba9cc76b415cae47e59edc", "schema_version": 1}
+{"protected_inventory_sha256": "bdf5369d5a70cda8e2cdb146d9cbb9021beb511c89c6d5f936a5366e5e8e2647", "schema_version": 1}

tools/ci/run_vehicle_tests.py

@@ -27,6 +27,8 @@ AOL_LIFECYCLE_TARGETS = (
   'openpilot/starpilot/aol/tests/test_hkg_early_long_withdrawal.py',
   'openpilot/starpilot/car/toyota/tests/test_prius_retrofit_stock.py',
   'openpilot/starpilot/car/toyota/tests/test_prius_retrofit_stock_callers.py',
+  'opendbc_repo/opendbc/car/toyota/tests/test_matrix_filter.py',
+  'openpilot/starpilot/car/toyota/tests/test_matrix_filter_callers.py',
   'openpilot/starpilot/aol/tests/test_crossmake_native_admission.py',
   'openpilot/starpilot/aol/tests/test_hkg_source_policy_cache.py',
   'openpilot/starpilot/aol/tests/test_hkg_native_admission.py',
@@ -211,6 +213,8 @@ DEBUG_SAFETY_CLASS_TARGETS = {
 RELEASE_TARGETS = (
   'openpilot/starpilot/car/toyota/tests/test_prius_retrofit_stock.py',
   'openpilot/starpilot/car/toyota/tests/test_prius_retrofit_stock_callers.py',
+  'opendbc_repo/opendbc/car/toyota/tests/test_matrix_filter.py',
+  'openpilot/starpilot/car/toyota/tests/test_matrix_filter_callers.py',
   'opendbc_repo/opendbc/car/hyundai/tests/test_long_cp_copy.py',
   'opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py',
   'opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py',