Support Toyota Matrix stock-control AOL

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

@@ -47,6 +47,53 @@ class TestRetrofit(unittest.TestCase):
       if identity == CAR.TOYOTA_PRIUS_RETROFIT:
         self.assertTrue(cp.dashcamOnly)
 
+  def test_matrix_stock_aol_admission(self):
+    import struct
+    from openpilot.starpilot.car.toyota.aol import qualified, native_accepts_cp
+    from opendbc.car import structs
+
+    for alpha in (False, True):
+      cp = params(CAR.TOYOTA_MATRIX_RETROFIT, alpha=alpha)
+      self.assertEqual(cp.safetyConfigs[0].safetyParam, 856)
+      self.assertEqual(cp.flags, 0)
+      self.assertTrue(qualified(cp))
+      cp.alternativeExperience = 32
+      self.assertTrue(qualified(cp, marked_only=True))
+      self.assertTrue(native_accepts_cp(cp, int(structs.CarParams.SafetyModel.toyota), 856))
+      self.assertFalse(cp.openpilotLongitudinalControl)
+      self.assertEqual(cp.lateralTuning.torque.latAccelFactor, struct.unpack('<f', struct.pack('<f', 4.05))[0])
+      self.assertEqual(cp.lateralTuning.torque.friction, struct.unpack('<f', struct.pack('<f', .10))[0])
+      for field, value in (('passive', True), ('notCar', True), ('dashcamOnly', True),
+                           ('pcmCruise', False), ('openpilotLongitudinalControl', True),
+                           ('secOcRequired', True), ('flags', int(ToyotaFlags.GAS_INTERCEPTOR)),
+                           ('carFingerprint', CAR.TOYOTA_COROLLA), ('alternativeExperience', 160)):
+        rejected = cp.as_reader().as_builder()
+        setattr(rejected, field, value)
+        self.assertFalse(qualified(rejected), field)
+      for word in (855, 857, 88, 344, 600, 73, 585, 4169, 4681):
+        rejected = cp.as_reader().as_builder()
+        rejected.safetyConfigs[0].safetyParam = word
+        self.assertFalse(qualified(rejected), word)
+        self.assertFalse(native_accepts_cp(rejected, int(structs.CarParams.SafetyModel.toyota), word), word)
+      for ae in (0, 1, 33, 128, 160, 256, 288):
+        rejected = cp.as_reader().as_builder()
+        rejected.alternativeExperience = ae
+        self.assertFalse(qualified(rejected, marked_only=True), ae)
+        self.assertFalse(native_accepts_cp(rejected, int(structs.CarParams.SafetyModel.toyota), 856), ae)
+      rejected = cp.as_reader().as_builder()
+      rejected.steerControlType = structs.CarParams.SteerControlType.angle
+      self.assertFalse(qualified(rejected))
+      rejected = cp.as_reader().as_builder()
+      rejected.safetyConfigs = []
+      self.assertFalse(qualified(rejected))
+      rejected = cp.as_reader().as_builder()
+      rejected.safetyConfigs[0].safetyModel = structs.CarParams.SafetyModel.noOutput
+      self.assertFalse(qualified(rejected))
+      for fp in ({0: {0x2FF: 8}, 1: {}, 2: {}}, {0: {}, 1: {}, 2: {0x343: 8}}):
+        takeover = params(CAR.TOYOTA_MATRIX_RETROFIT, fp, alpha)
+        self.assertTrue(takeover.dashcamOnly)
+        self.assertFalse(qualified(takeover))
+
   def test_original_stock_contracts(self):
     for car, word, factor, friction in ((CAR.TOYOTA_MATRIX_RETROFIT, 856, 4.05, .10),
                                         (CAR.TOYOTA_PRIUS_RETROFIT, 585, 1.60, .151515)):

opendbc_repo/opendbc/safety/modes/toyota.h

@@ -208,7 +208,7 @@ static void toyota_optional_rx_hook(const CANPacket_t *msg) {
     toyota_camera_2aa_ambiguous = true;
   }
   const bool aol_checksum_source = (!toyota_alt_cruise && (msg->addr == 0x1D3U)) || (msg->addr == 0x262U) || (toyota_aol_secoc && (msg->addr == 0x127U));
-  const unsigned int aol_source_len = ((toyota_aol_rav4h_filter || toyota_alt_cruise || toyota_legacy_sdsu || toyota_legacy_advert()) && (msg->addr == 0x262U)) ? 5U : 8U;
+  const unsigned int aol_source_len = ((toyota_aol_rav4h_filter || toyota_alt_cruise || toyota_legacy_sdsu || toyota_legacy_advert() || (current_safety_param == 856U)) && (msg->addr == 0x262U)) ? 5U : 8U;
   if (!aol_checksum_source || ((GET_LEN(msg) == aol_source_len) && (toyota_get_checksum(msg) == toyota_compute_checksum(msg)))) {
     toyota_aol_observe(msg);
   } else if ((toyota_aol_enabled || toyota_legacy_advert() || toyota_camera_2aa() || toyota_prius_filter()) && (msg->bus == 0U)) {
@@ -246,7 +246,7 @@ static void toyota_optional_rx_hook(const CANPacket_t *msg) {
 static void toyota_rx_hook(const CANPacket_t *msg) {
   // Required sources have already passed RX length/checksum validation here.
   // Optional ingress only runs for messages outside the required RX catalog.
-  if ((toyota_legacy_advert() || toyota_camera_2aa() || toyota_prius_filter()) && ((msg->addr == 0x262U) || (!toyota_alt_cruise && (msg->addr == 0x1D3U)))) {
+  if ((toyota_legacy_advert() || toyota_camera_2aa() || toyota_prius_filter() || (current_safety_param == 856U)) && ((msg->addr == 0x262U) || (!toyota_alt_cruise && (msg->addr == 0x1D3U)))) {
     toyota_aol_observe(msg);
   }
   if (toyota_alt_cruise && msg_matches(msg, 0x365U, 0U) && (GET_LEN(msg) == 7U)) {
@@ -692,7 +692,7 @@ static safety_config toyota_init(uint16_t param) {
     // Restore the sixth row on every warm switch before specializing the new owner.
     toyota_rx_checks[5].msg[0] = (CanMsgCheck){.addr = 0x365, .bus = 0U, .len = 7, .frequency = 10U,
       .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true};
-    if (toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_advert_param(param)) {
+    if (toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_advert_param(param) || (param == 856U)) {
       toyota_rx_checks[4].msg[0] = (CanMsgCheck){.addr = 0x262, .bus = 0U, .len = (toyota_camera_2aa_param(param) || toyota_prius_advert_param(param)) ? 8 : 5, .frequency = 25U,
         .ignore_counter = true, .ignore_quality_flag = true};
       if (!toyota_alt_cruise) {
@@ -710,7 +710,7 @@ static safety_config toyota_init(uint16_t param) {
     ret.disable_forwarding = false;
     if (toyota_prius_sdsu_param(param)) {
       ret.rx_checks_len = 7;
-    } else if ((toyota_legacy_sdsu && toyota_alt_cruise) || toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_advert_param(param)) {
+    } else if ((toyota_legacy_sdsu && toyota_alt_cruise) || toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_advert_param(param) || (param == 856U)) {
       ret.rx_checks_len = 6;
     } else if (toyota_pedal || toyota_alt_cruise || toyota_sdsu_status) {
       ret.rx_checks_len = 5;

opendbc_repo/opendbc/safety/modes/toyota_aol.h

@@ -115,7 +115,7 @@ static void toyota_aol_observe(const CANPacket_t *msg) {
   unsigned int expected_len = 8U;
   if (toyota_alt_cruise && (msg->addr == main_addr)) {
     expected_len = 7U;
-  } else if ((toyota_aol_rav4h_filter || toyota_alt_cruise || toyota_legacy_sdsu || toyota_legacy_advert()) && (msg->addr == 0x262U)) {
+  } else if ((toyota_aol_rav4h_filter || toyota_alt_cruise || toyota_legacy_sdsu || toyota_legacy_advert() || (current_safety_param == 856U)) && (msg->addr == 0x262U)) {
     expected_len = 5U;
   } else {
   }
@@ -183,7 +183,7 @@ static void toyota_aol_configure(uint16_t param) {
   toyota_pedal = (param == 0x2049U) || (param == 0x2249U);
   toyota_aol_rav4h_filter = (param == 0x9049U) || (param == 0x9249U);
   toyota_sdsu_status = toyota_aol_rav4h_filter || toyota_eps8_sdsu || toyota_legacy_sdsu || toyota_prius_sdsu_param(param);
-  toyota_aol_stock = (param == 585U) || (param == 4681U) || (param == 0x9249U) || (param == 0x2249U) || (param == 0x424DU) || (param == 0x649U) || (param == 0x1649U) || (param == 0x8249U) || (param == 0x8649U) || ((toyota_legacy_sdsu || toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_filter_param(param)) && ((param & 0x200U) != 0U));
+  toyota_aol_stock = (param == 585U) || (param == 856U) || (param == 4681U) || (param == 0x9249U) || (param == 0x2249U) || (param == 0x424DU) || (param == 0x649U) || (param == 0x1649U) || (param == 0x8249U) || (param == 0x8649U) || ((toyota_legacy_sdsu || toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_filter_param(param)) && ((param & 0x200U) != 0U));
 #ifdef ALLOW_DEBUG
   toyota_aol_secoc = (param == 2121U);
 #else

opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py

@@ -0,0 +1,203 @@
+"""Exact manual Matrix stock PCM native observer and authority regressions."""
+import pytest
+
+from opendbc.can import CANPacker
+from opendbc.car import structs
+from opendbc.safety.tests.libsafety import libsafety_py
+from openpilot.starpilot.car.toyota.tests import test_rav4h_caller_native as helpers
+
+# Read actual native required-RX timestamps; never modify them.
+libsafety_py.ffi.cdef('extern safety_config current_safety_config;', override=True)
+
+source_matched_native = helpers.source_matched_native
+
+
+class Physical:
+  def __init__(self, *, word=856, experience=32):
+    self.safety = libsafety_py.libsafety
+    self.now = 1_000_000
+    self.packer = CANPacker('toyota_new_mc_pt_generated')
+    self.safety.set_alternative_experience(experience)
+    assert self.safety.set_safety_hooks(structs.CarParams.SafetyModel.toyota, word) == 0
+    self.safety.init_tests()
+    self.safety.set_alternative_experience(experience)
+    self.safety.set_aol_test_heartbeat(True)
+
+  def tick(self, *, fault=None, omit=None, **fields):
+    self.now += 10_000
+    self.safety.set_timer(self.now)
+    frames = helpers.physical(self.packer, cruise=fields.pop('cruise', False), sdsu=None, **fields)
+    for address, data, bus in frames:
+      if address == omit:
+        continue
+      required = address in (0xAA, 0x260, 0x1D2, 0x224, 0x262, 0x1D3)
+      corrupted = fault == ('crc', address)
+      if corrupted:
+        data = data[:-1] + bytes([data[-1] ^ 1])
+      if fault == ('length', address):
+        data = data + bytes(3) if address == 0x262 else data[:-1]
+      if fault == ('bus', address):
+        bus = 2
+      accepted = bool(self.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)))
+      if required and corrupted:
+        assert not accepted, (hex(address), 'corrupt required source was accepted')
+      elif required and fault != ('length', address):
+        assert accepted, (hex(address), data.hex())
+    self.safety.safety_tick()
+    self.safety.aol_set_host_request(3)
+
+  def warm(self):
+    for _ in range(25):
+      self.tick(cruise=False)
+    for _ in range(25):
+      self.tick(cruise=True)
+
+  def tx(self, name, values):
+    address, data, bus = self.packer.make_can_msg(name, 0, values)
+    return bool(self.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)))
+
+  def close(self):
+    self.safety.set_alternative_experience(0)
+    self.safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
+
+
+@pytest.fixture
+def matrix():
+  case = Physical()
+  try:
+    yield case
+  finally:
+    case.close()
+
+
+def test_actual_dlc5_stock_lateral_only(matrix):
+  matrix.warm()
+  assert matrix.safety.safety_config_valid()
+  assert matrix.safety.aol_get_permission_mask() == 1
+  assert matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1})
+  assert not matrix.tx('ACC_CONTROL', {'ACCEL_CMD': .1})
+  assert not matrix.tx('ACC_CONTROL', {'ACCEL_CMD': -.1})
+  assert matrix.tx('ACC_CONTROL', {'ACCEL_CMD': 0, 'CANCEL_REQ': 1})
+
+
+@pytest.mark.parametrize('fields', ({'main': False}, {'gear': 8}, {'gear': 16},
+                                   {'eps': 0}, {'eps': 3}, {'eps': 9}, {'eps': 17}))
+def test_physical_refusals_and_recovery(matrix, fields):
+  matrix.warm()
+  assert matrix.safety.aol_get_permission_mask() == 1
+  matrix.tick(**fields)
+  assert matrix.safety.aol_get_permission_mask() == 0
+  assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1})
+  matrix.tick()
+  assert matrix.safety.aol_get_permission_mask() == 1
+
+
+@pytest.mark.parametrize('address', (0x262, 0x1D3, 0x260))
+@pytest.mark.parametrize('kind', ('crc', 'length'))
+def test_actual_required_rx_refusal(matrix, address, kind):
+  matrix.warm()
+  assert matrix.safety.aol_get_permission_mask() == 1
+  config = matrix.safety.current_safety_config
+  check = next(config.rx_checks[i] for i in range(config.rx_checks_len) if config.rx_checks[i].msg[0].addr == address)
+  original_timestamp = check.status.last_timestamp
+  matrix.tick(fault=(kind, address))
+  if address == 0x260 and kind == 'length':
+    # Generic required RX matches address, bus AND DLC. A wrong-DLC frame must
+    # never renew the last good source; the unchanged native tick expires it.
+    assert check.msg[0].frequency == 50
+    assert check.status.last_timestamp == original_timestamp
+    for _ in range(19):
+      matrix.tick(fault=(kind, address))
+      assert check.status.last_timestamp == original_timestamp
+    # AOL's unchanged ten-period freshness bound is tighter than generic lag.
+    assert matrix.now - original_timestamp == 200_000
+    assert not check.status.lagging and matrix.safety.aol_get_permission_mask() == 1
+    matrix.tick(fault=(kind, address))
+    assert matrix.now - original_timestamp == 210_000
+    assert check.status.last_timestamp == original_timestamp and not check.status.lagging
+    assert matrix.safety.aol_get_permission_mask() == 0
+    assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1})
+    for _ in range(79):
+      matrix.tick(fault=(kind, address))
+      assert check.status.last_timestamp == original_timestamp
+    assert matrix.now - original_timestamp == 1_000_000
+    assert not check.status.lagging and matrix.safety.aol_get_permission_mask() == 0
+    matrix.tick(fault=(kind, address))
+    assert check.status.last_timestamp == original_timestamp and check.status.lagging
+    assert matrix.now - original_timestamp == 1_010_000
+  # EPS/MAIN observer DLC and required-source checksum refusals are immediate.
+  assert matrix.safety.aol_get_permission_mask() == 0
+  assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1})
+
+
+@pytest.mark.parametrize('address', (0x262, 0x1D3, 0x620, 0x3BC))
+def test_wrong_bus_cannot_establish_source(matrix, address):
+  for _ in range(50):
+    matrix.tick(fault=('bus', address))
+  assert matrix.safety.aol_get_permission_mask() == 0
+
+
+@pytest.mark.parametrize('address,elapsed', ((0x262, 400001), (0x1D3, 303032)))
+def test_original_source_expiry(matrix, address, elapsed):
+  matrix.warm()
+  assert matrix.safety.aol_get_permission_mask() == 1
+  matrix.now += elapsed
+  matrix.tick(omit=address)
+  assert matrix.safety.aol_get_permission_mask() == 0
+
+
+def test_original_host_request_expiry(matrix):
+  matrix.warm()
+  assert matrix.safety.aol_get_permission_mask() == 1
+  matrix.safety.set_timer(matrix.now + 300001)
+  assert matrix.safety.aol_get_permission_mask() == 0
+
+
+def test_injected_relay_fault_refusal(matrix):
+  matrix.warm()
+  matrix.safety.set_relay_malfunction(True)
+  assert matrix.safety.aol_get_permission_mask() == 0
+  assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1})
+
+
+@pytest.mark.parametrize('experience', (0, 1, 33, 128, 160, 256, 288))
+def test_other_alternative_experiences_remain_unadmitted(experience):
+  case = Physical(experience=experience)
+  try:
+    case.warm()
+    assert case.safety.aol_get_permission_mask() == 0
+  finally:
+    case.close()
+
+
+def test_brake_and_unbelted_preserve_only_lateral(matrix):
+  matrix.warm()
+  matrix.tick(brake=True, cruise=False)
+  assert matrix.safety.aol_get_permission_mask() == 1
+  address, data, bus = matrix.packer.make_can_msg('BODY_CONTROL_STATE', 0, {'SEATBELT_DRIVER_UNLATCHED': 1})
+  assert matrix.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
+  assert matrix.safety.aol_get_permission_mask() == 1
+
+
+def test_heartbeat_loss(matrix):
+  matrix.warm()
+  matrix.safety.set_aol_test_heartbeat(False)
+  assert matrix.safety.aol_get_permission_mask() == 0
+  assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1})
+
+
+def test_open_door_denies_lateral(matrix):
+  matrix.warm()
+  address, data, bus = matrix.packer.make_can_msg('BODY_CONTROL_STATE', 0, {'DOOR_OPEN_FL': 1})
+  assert matrix.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
+  assert matrix.safety.aol_get_permission_mask() == 0
+
+
+def test_real_stock_steering_rx_triggers_relay_refusal(matrix):
+  matrix.warm()
+  assert not matrix.safety.get_relay_malfunction()
+  address, data, bus = matrix.packer.make_can_msg('STEERING_LKA', 0, {})
+  matrix.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
+  assert matrix.safety.get_relay_malfunction()
+  assert matrix.safety.aol_get_permission_mask() == 0
+  assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1})

openpilot/selfdrive/pandad/tests/test_aol_protocol.cc

@@ -179,7 +179,7 @@ int main() {
   }
 
   for (uint32_t word = 0U; word <= 65535U; ++word) {
-    const bool expected = (word == 0xC042U) || (word == 0xC242U) || (word == 0x8042U) || (word == 0x8242U) || (word == 0xC049U) || (word == 0xC249U) || (word == 73U) || (word == 585U) || (word == 4169U) || (word == 4681U) || (word == 2121U) || (word == 0x9049U) || (word == 0x9249U) || (word == 0x2049U) || (word == 0x2249U) || (word == 0x424DU) || (word == 0x449U) || (word == 0x649U) || (word == 0x1449U) || (word == 0x1649U) || (word == 0x8049U) || (word == 0x8249U) || (word == 0x8449U) || (word == 0x8649U) || (word == 0x9158U) || (word == 0x9358U) || (word == 0x9149U) || (word == 0x9349U) || (word == 0x9164U) || (word == 0x9364U) || (word == 0xd149U) || (word == 0xd349U) || (word == 0xd04dU) || (word == 0xd24dU) || (word == 0x9449U) || (word == 0x9549U) || (word == 0x9558U) || (word == 0x9564U) || (word == 0x9649U) || (word == 0x9749U) || (word == 0x9758U) || (word == 0x9764U) || (word == 0xD44DU) || (word == 0xD549U) || (word == 0xD64DU) || (word == 0xD749U);
+    const bool expected = (word == 0xC042U) || (word == 0xC242U) || (word == 0x8042U) || (word == 0x8242U) || (word == 0xC049U) || (word == 0xC249U) || (word == 73U) || (word == 585U) || (word == 856U) || (word == 4169U) || (word == 4681U) || (word == 2121U) || (word == 0x9049U) || (word == 0x9249U) || (word == 0x2049U) || (word == 0x2249U) || (word == 0x424DU) || (word == 0x449U) || (word == 0x649U) || (word == 0x1449U) || (word == 0x1649U) || (word == 0x8049U) || (word == 0x8249U) || (word == 0x8449U) || (word == 0x8649U) || (word == 0x9158U) || (word == 0x9358U) || (word == 0x9149U) || (word == 0x9349U) || (word == 0x9164U) || (word == 0x9364U) || (word == 0xd149U) || (word == 0xd349U) || (word == 0xd04dU) || (word == 0xd24dU) || (word == 0x9449U) || (word == 0x9549U) || (word == 0x9558U) || (word == 0x9564U) || (word == 0x9649U) || (word == 0x9749U) || (word == 0x9758U) || (word == 0x9764U) || (word == 0xD44DU) || (word == 0xD549U) || (word == 0xD64DU) || (word == 0xD749U);
     auto toyota = status();
     toyota.safety_mode = 2U;
     toyota.safety_param = static_cast<uint16_t>(word);

openpilot/starpilot/car/toyota/aol.py

@@ -9,7 +9,7 @@ AOL_WORDS = frozenset((
   0x8042, 0x8242, 0xC042, 0xC242,
   0xC049, 0xC249,
   0x9449, 0x9549, 0x9558, 0x9564, 0x9649, 0x9749, 0x9758, 0x9764, 0xD44D, 0xD549, 0xD64D, 0xD749,
-  73, 585, 4169, 4681, 2121, 0x9049, 0x9249,
+  73, 585, 856, 4169, 4681, 2121, 0x9049, 0x9249,
   0x2049, 0x2249, 0x424D, 0x449, 0x649, 0x1449, 0x1649,
   0x8049, 0x8249, 0x8449, 0x8649, 0x9158, 0x9358, 0x9149,
   0x9349, 0x9164, 0x9364, 0xd149, 0xd349, 0xd04d, 0xd24d,
@@ -74,7 +74,20 @@ def angle_qualified(cp, *, marked_only=False):
   return configured(cp) and cp.alternativeExperience in ((32,) if marked_only else (0, 32))
 
 
+def matrix_stock_qualified(cp, *, marked_only=False):
+  if (cp is None or cp.brand != 'toyota' or cp.carFingerprint != CAR.TOYOTA_MATRIX_RETROFIT or
+      cp.notCar or cp.passive or cp.dashcamOnly or not cp.pcmCruise or cp.openpilotLongitudinalControl or
+      cp.secOcRequired or cp.flags != 0 or
+      cp.steerControlType != structs.CarParams.SteerControlType.torque or len(cp.safetyConfigs) != 1 or
+      cp.alternativeExperience not in ((32,) if marked_only else (0, 32))):
+    return False
+  config = cp.safetyConfigs[0]
+  return config.safetyModel == structs.CarParams.SafetyModel.toyota and config.safetyParam == 856
+
+
 def qualified(cp, *, marked_only=False):
+  if matrix_stock_qualified(cp, marked_only=marked_only):
+    return True
   if angle_qualified(cp, marked_only=marked_only):
     return True
   from opendbc.car.toyota.advert_filter import configured as advert_filter_configured

openpilot/starpilot/car/toyota/aol_policy.h

@@ -3,7 +3,7 @@
 #include "selfdrive/pandad/aol_protocol.h"
 
 inline bool toyota_aol_param(uint16_t param) {
-  return (param == 0xC042U || param == 0xC242U || param == 0x8042U || param == 0x8242U || param == 0xC049U) || (param == 0xC249U) || (param == 73U) || (param == 585U) || (param == 4169U) || (param == 4681U) || (param == 2121U) || (param == 0x9049U) || (param == 0x9249U) || (param == 0x2049U) || (param == 0x2249U) || (param == 0x424DU) || (param == 0x449U) || (param == 0x649U) || (param == 0x1449U) || (param == 0x1649U) || (param == 0x8049U) || (param == 0x8249U) || (param == 0x8449U) || (param == 0x8649U) || (param == 0x9158U) || (param == 0x9358U) || (param == 0x9149U) || (param == 0x9349U) || (param == 0x9164U) || (param == 0x9364U) || (param == 0xd149U) || (param == 0xd349U) || (param == 0xd04dU) || (param == 0xd24dU) || (param == 0x9449U) || (param == 0x9549U) || (param == 0x9558U) || (param == 0x9564U) || (param == 0x9649U) || (param == 0x9749U) || (param == 0x9758U) || (param == 0x9764U) || (param == 0xD44DU) || (param == 0xD549U) || (param == 0xD64DU) || (param == 0xD749U);
+  return (param == 0xC042U || param == 0xC242U || param == 0x8042U || param == 0x8242U || param == 0xC049U) || (param == 0xC249U) || (param == 73U) || (param == 585U) || (param == 856U) || (param == 4169U) || (param == 4681U) || (param == 2121U) || (param == 0x9049U) || (param == 0x9249U) || (param == 0x2049U) || (param == 0x2249U) || (param == 0x424DU) || (param == 0x449U) || (param == 0x649U) || (param == 0x1449U) || (param == 0x1649U) || (param == 0x8049U) || (param == 0x8249U) || (param == 0x8449U) || (param == 0x8649U) || (param == 0x9158U) || (param == 0x9358U) || (param == 0x9149U) || (param == 0x9349U) || (param == 0x9164U) || (param == 0x9364U) || (param == 0xd149U) || (param == 0xd349U) || (param == 0xd04dU) || (param == 0xd24dU) || (param == 0x9449U) || (param == 0x9549U) || (param == 0x9558U) || (param == 0x9564U) || (param == 0x9649U) || (param == 0x9749U) || (param == 0x9758U) || (param == 0x9764U) || (param == 0xD44DU) || (param == 0xD549U) || (param == 0xD64DU) || (param == 0xD749U);
 }
 
 inline constexpr AolSafetyProfile TOYOTA_AOL_PROFILE{2U, toyota_aol_param, true};

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

@@ -0,0 +1,157 @@
+"""Manual Matrix stock PCM through real Card, SelfdriveD, Controls and native TX.
+
+Synthetic boundaries are manual vehicle selection, CAN/perception producers,
+input time and USB transport. Native permission, process health and actuator
+commands are never forced. This does not establish hardware timing acceptance.
+"""
+from dataclasses import replace
+import time
+
+import pytest
+from opendbc.car.toyota.values import CAR
+from openpilot.starpilot.aol.wire import decode_safety, encode_safety
+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
+
+
+def run(scenario, bridge):
+  tick = 0
+  actual_vetoes = 0
+  fault_ns = 0
+
+  def physical(packer, **fields):
+    nonlocal tick, fault_ns
+    tick += 1
+    fields['sdsu'] = None
+    if scenario in ('startup-main', 'standstill'):
+      fields['cruise'] = False
+      fields['stopped'] = scenario == 'standstill'
+    if scenario == 'pcm-cancellation' and tick >= 260:
+      fields.update(cruise=False, main=True)
+    if scenario == 'settings-off' and 260 <= tick < 470:
+      fields['cruise'] = False
+    if scenario in ('eps-temporary', 'eps-permanent') and 260 <= tick < 315:
+      fields['eps'] = 9 if scenario == 'eps-temporary' else 3
+    if scenario == 'main-off' and tick >= 260:
+      fields.update(main=False, cruise=False)
+    frames = helpers.physical(packer, **fields)
+    assert len(next(data for address, data, bus in frames if address == 0x262 and bus == 0)) == 5
+    if scenario in ('main-expiry', 'eps-expiry') and 260 <= tick < 315:
+      missing = 0x1D3 if scenario == 'main-expiry' else 0x262
+      frames = [frame for frame in frames if not (frame[0] == missing and frame[2] == 0)]
+    if scenario == 'required-rx' and 260 <= tick < 315:
+      if tick == 260:
+        fault_ns = time.monotonic_ns()
+      frames = [(address, data[:-1] + bytes([data[-1] ^ 1]) if address == 0x260 and bus == 0 else data, bus)
+                for address, data, bus in frames]
+    return frames
+
+  def on_state(current, host, cs):
+    assert host.CP.carFingerprint == CAR.TOYOTA_MATRIX_RETROFIT
+    assert host.CP.safetyConfigs[0].safetyParam == 856 and host.CP.flags == 0
+    assert host.CP.pcmCruise and not host.CP.openpilotLongitudinalControl
+    assert abs(host.CP.lateralTuning.torque.latAccelFactor - 4.05) < 1e-6
+    assert abs(host.CP.lateralTuning.torque.friction - .10) < 1e-6
+    if scenario == 'eps-temporary' and 270 <= current < 310:
+      assert cs.steerFaultTemporary
+      assert host.aol_card_intent.allowed_latch
+    if scenario == 'eps-permanent' and 270 <= current < 310:
+      assert cs.steerFaultPermanent
+      # Toyota derives the request from physical MAIN. Faults veto commands;
+      # this is not another family's explicit button latch.
+      assert not host.aol_card_intent.explicit_latch
+      assert host.aol_card_intent.allowed_latch == cs.cruiseState.available
+
+  def observe(current, host, cs, cc, safety, native):
+    assert not cc.longActive and not safety.aol_get_permission_mask() & 2
+    if scenario == 'standstill' and 220 <= current < 250:
+      assert host.CP.minSteerSpeed <= 0 and host.CP.steerAtStandstill
+      assert cs.vEgo < .01 and cc.latActive and not cc.longActive
+    if scenario == 'startup-main' and 220 <= current < 250:
+      assert cs.cruiseState.available and not cs.cruiseState.enabled
+      assert host.aol_card_intent is not None and host.aol_card_intent.allowed_latch
+      assert cc.latActive and safety.aol_get_permission_mask() == 1
+    if scenario == 'pcm-cancellation' and current >= 300:
+      assert cs.cruiseState.available and not cs.cruiseState.enabled
+      assert host.aol_card_intent.allowed_latch and cc.latActive
+    if scenario in ('main-expiry', 'eps-expiry', 'required-rx', 'native-identity', 'eps-temporary', 'eps-permanent') and 306 <= current < 310:
+      assert not cc.latActive and not cc.longActive
+    if scenario == 'eps-permanent' and 270 <= current < 310:
+      assert cs.steerFaultPermanent and not cc.latActive and not cc.longActive
+      assert safety.aol_get_permission_mask() == 0
+    if scenario == 'required-rx' and 270 <= current < 315:
+      assert not cc.latActive and not cc.longActive
+      assert safety.aol_get_permission_mask() == 0 and actual_vetoes > 0
+      if current == 270:
+        wire = next(event for event in native if event.which() == 'aolSafetyWire')
+        observed = decode_safety(bytes(wire.aolSafetyWire))
+        assert observed is not None and not observed.lateralAllowed and not observed.longitudinalAllowed
+        assert fault_ns <= observed.observedMonoTime <= fault_ns + 100_000_000
+        assert observed.validUntilMonoTime - observed.observedMonoTime == 200_000_000
+    if scenario == 'main-off' and current >= 290:
+      assert not cc.latActive and not cc.longActive
+    if scenario == 'settings-off' and current == 470:
+      assert not cc.latActive and not cc.longActive
+    if scenario == 'settings-off' and current >= 480:
+      assert cc.enabled and cc.latActive and not cc.longActive
+      assert not host.aol_card_intent.settings.enabled
+    if scenario == 'settings-off' and 300 <= current < 315:
+      assert not host.aol_card_intent.settings.enabled
+      assert not host.aol_card_intent.allowed_latch and not cc.latActive and not cc.longActive
+
+  def rx_expected(address, data, bus, default, cp):
+    del data, cp
+    if scenario == 'required-rx' and address == 0x260 and bus == 0 and 260 <= tick < 315:
+      return False
+    return default
+
+  def tx_expected(current, frame, cp, safety):
+    nonlocal actual_vetoes
+    if scenario == 'required-rx' and 260 <= current < 315 and frame.address == 0x2E4:
+      assert not safety.safety_config_valid()
+      assert safety.aol_get_permission_mask() == safety.aol_get_request_mask() == 0
+      decoded = helpers.decode(cp, (frame.address, bytes(frame.dat), frame.src), 'STEERING_LKA')
+      active = bool(decoded['STEER_REQUEST'] or decoded['STEER_TORQUE_CMD'])
+      # A fresh 10Hz Panda report has not yet reached the host on the first
+      # corrupt RX. The independent native hook must veto every active packet.
+      actual_vetoes += int(active)
+      return not active
+    return True
+
+  original_native = helpers.native_events
+
+  def native_events(*args, **kwargs):
+    messages = original_native(*args, **kwargs)
+    if scenario == 'native-identity' and 260 <= tick < 315:
+      result = []
+      for event in messages:
+        if event.which() == 'aolSafetyWire':
+          wire = event.as_reader().as_builder()
+          state = decode_safety(bytes(wire.aolSafetyWire))
+          assert state is not None
+          wire.aolSafetyWire = encode_safety(replace(state, safetyParam=state.safetyParam ^ 1))
+          event = wire.as_reader()
+        result.append(event)
+      return result
+    return messages
+
+  permanent = scenario == 'main-off'
+  actual_scenario = ('identity_refusal' if permanent else 'physical' if scenario == 'physical' else
+                     'cold_low' if scenario == 'standstill' else 'nominal' if scenario == 'startup-main' else 'main_lease')
+  with pytest.MonkeyPatch.context() as monkeypatch:
+    if scenario == 'native-identity':
+      monkeypatch.setattr(helpers, 'native_events', native_events)
+    helpers.run_case(True, actual_scenario, candidate=CAR.TOYOTA_MATRIX_RETROFIT,
+                     bridge=bridge, input_clock=True, aol=scenario != 'settings-off', sdsu_words=(856, 856),
+                     firmware_override=[], fingerprint_frames=[], manual_selection=True,
+                     physical_frames=physical, on_state=on_state, on_sample=observe,
+                     native_rx_expected=rx_expected, native_tx_expected=tx_expected)
+
+
+@pytest.mark.parametrize('scenario', ('startup-main', 'standstill', 'physical', 'pcm-cancellation', 'main-expiry', 'eps-expiry', 'required-rx',
+                                      'native-identity', 'eps-temporary', 'eps-permanent', 'main-off', 'settings-off'))
+def test_actual_matrix_stock_lifecycle(scenario, host_heartbeat_bridge):
+  run(scenario, host_heartbeat_bridge)

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

@@ -9,6 +9,7 @@ installed-hardware scheduling or timing acceptance.
 """
 
 import json
+import hashlib
 import ctypes
 import gc
 import time
@@ -227,7 +228,7 @@ 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):
+             brake_pause_mps=None, native_rx_expected=None, on_state=None, fingerprint_frames=None, manual_selection=False, native_tx_expected=None):
   SYNTHETIC_LAST.clear()
   NATIVE_LAST.clear()
   safety = libsafety_py.libsafety
@@ -246,8 +247,12 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H,
     if brake_pause_mps is not None:
       params.put('AolBrakePauseSpeedMps', float(brake_pause_mps), block=True)
     fw = firmware(candidate) if firmware_override is None else firmware_override
-    exact, found = match_fw_to_car(fw, '', log=False)
-    assert exact and found == {candidate}
+    if manual_selection:
+      assert candidate == CAR.TOYOTA_MATRIX_RETROFIT and stock and not fw
+      assert firmware_override == [] and fingerprint_frames == []
+    else:
+      exact, found = match_fw_to_car(fw, '', log=False)
+      assert exact and found == {candidate}
     fp = gen_empty_fingerprint()
     if fingerprint_frames is None:
       fp[0][0x2FF] = 4 if filter_advert else 8
@@ -258,7 +263,8 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H,
       fp[2][0x2FF] = 8
     if camera_messages:
       fp[2].update({0x343: 8, 0x4CB: 8})
-    observed = (candidate, fp, '0' * 17, fw, structs.CarParams.FingerprintSource.fw, True)
+    source = structs.CarParams.FingerprintSource.fixed if manual_selection else structs.CarParams.FingerprintSource.fw
+    observed = (candidate, fp, '0' * 17, fw, source, True)
     with (
       patch.object(car_helpers, 'fingerprint', return_value=observed),
       patch.object(card_module, 'can_comm_callbacks', return_value=(lambda *a: [], lambda x: None)),
@@ -426,13 +432,45 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H,
         cc_event = published['carControl']
         cc = cc_event.carControl
         deliver(host.sm, [cc_event, *native, *sd_events.values()])
+        # A valid previous publication is not a new producer event. Card may
+        # deliberately emit nothing when the current control message is invalid.
+        previous_sendcan = published.pop('sendcan', None)
         host.controls_update(cs, cc)
         outgoing = published.get('sendcan')
-        assert outgoing is not None
-        if not outgoing.valid:
-          assert not outgoing.sendcan and not cs.canValid, ('startup must stay neutral until real CAN health', tick, cs.canValid)
-        for f in outgoing.sendcan:
-          assert safety.safety_tx_hook(libsafety_py.make_CANPacket(f.address, f.src, bytes(f.dat))), (tick, hex(f.address), bytes(f.dat).hex())
+        if outgoing is None:
+          assert not cs.canValid or cs.canTimeout
+          assert not host.sm.valid['carControl']
+          assert not cc.latActive and not cc.longActive
+          assert safety.aol_get_request_mask() == safety.aol_get_permission_mask() == 0
+          outgoing_frames = ()
+        else:
+          outgoing_frames = outgoing.sendcan
+          if not outgoing.valid:
+            assert not outgoing_frames and not cs.canValid, ('startup must stay neutral until real CAN health', tick, cs.canValid)
+        native_tx_accepted = native_tx_rejected = 0
+        for f in outgoing_frames:
+          before_tx = {
+            'nativePermission': int(safety.aol_get_permission_mask()), 'nativeRequest': int(safety.aol_get_request_mask()),
+            'controlsAllowed': bool(safety.get_controls_allowed()), 'configValid': bool(safety.safety_config_valid()),
+            'heartbeatEngaged': ctypes.c_bool.in_dll(native_globals, 'heartbeat_engaged').value,
+            'globals': {name: ctypes.c_int.in_dll(native_globals, name).value for name in
+                        ('desired_torque_last', 'rt_torque_last', 'valid_steer_req_count', 'invalid_steer_req_count')},
+          }
+          expected_tx = True if native_tx_expected is None else native_tx_expected(tick, f, host.CP, safety)
+          assert type(expected_tx) is bool
+          accepted_tx = bool(safety.safety_tx_hook(libsafety_py.make_CANPacket(f.address, f.src, bytes(f.dat))))
+          native_tx_accepted += int(accepted_tx)
+          native_tx_rejected += int(not accepted_tx)
+          if accepted_tx != expected_tx:
+            refusal = {'tick': tick, 'candidate': str(candidate), 'scenario': scenario, 'address': f.address,
+                       'data': bytes(f.dat).hex(), 'bus': f.src, 'outgoingValid': bool(outgoing.valid),
+                       'CP': host.CP.to_dict(), 'CS': cs.to_dict(), 'CC': cc.to_dict(), 'beforeNativeTX': before_tx,
+                       'controllerLastTorque': host.CI.CC.last_torque,
+                       'nativeAfterPermission': int(safety.aol_get_permission_mask()),
+                       'axis': {'lateral': wanted.desired_lateral, 'longitudinal': wanted.desired_longitudinal},
+                       'state': str(sd.state_machine.state), 'fault': sd.aol_fault_reason}
+            (OUT / 'matrix-native-tx-refusal.json').write_text(json.dumps(refusal, indent=2, default=str) + '\n')
+          assert accepted_tx == expected_tx, (tick, hex(f.address), bytes(f.dat).hex(), accepted_tx, expected_tx)
           if f.address == 0x2E4:
             decoded = decode(host.CP, (f.address, bytes(f.dat), f.src), 'STEERING_LKA')
             positive_steering |= bool(decoded['STEER_REQUEST'] and decoded['STEER_TORQUE_CMD'])
@@ -449,6 +487,12 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H,
           {
             'tick': tick,
             'canValid': cs.canValid,
+            'freshSendcan': outgoing is not None,
+            'actualSendcanFrames': len(outgoing_frames),
+            'nativeAcceptedFrames': native_tx_accepted,
+            'nativeRejectedFrames': native_tx_rejected,
+            'previousSendcanMonoTime': int(previous_sendcan.logMonoTime) if previous_sendcan is not None else 0,
+            'previousSendcanSha256': hashlib.sha256(previous_sendcan.as_builder().to_bytes()).hexdigest() if previous_sendcan is not None else '',
             'lat': cc.latActive,
             'long': cc.longActive,
             'sd': str(sd.state_machine.state),
@@ -514,7 +558,7 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H,
               host.CI.CC.advert_filter_input if filter_advert or fingerprint_frames is not None and any(
                 address == 0x2AA for address, _, _ in fingerprint_frames) else host.CI.CC.rav4h_filter_input)
             assert preference is not None and not preference.update(time.monotonic_ns())
-            for f in outgoing.sendcan:
+            for f in outgoing_frames:
               if f.address == 0x343:
                 acc = decode(host.CP, (f.address, bytes(f.dat), f.src), 'ACC_CONTROL')
                 assert acc['ACCEL_CMD'] == 0 and acc['CANCEL_REQ'] and not acc['RELEASE_STANDSTILL']

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "10bc0a35396529a54c069434d8d2cc9f0b63f814338ab004b3b783bcc4eda793", "schema_version": 1}
+{"protected_inventory_sha256": "6f7a43aaa35dc95e301b5f12a1ca0bad066c62c21c3542bde54ea96478ffa111", "schema_version": 1}

tools/ci/run_vehicle_tests.py

@@ -159,6 +159,8 @@ AOL_LIFECYCLE_TARGETS = (
   "openpilot/starpilot/car/subaru/tests/test_smoothing_settings.py",
   "openpilot/starpilot/car/subaru/tests/test_smoothing_model_join.py",
   "opendbc_repo/opendbc/safety/tests/test_subaru_outback_angle.py",
+  'openpilot/starpilot/car/toyota/tests/test_matrix_stock_caller_native.py',
+  'opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py',
 )
 
 AOL_LIFECYCLE_SCOPE = {
@@ -454,6 +456,8 @@ RELEASE_TARGETS = (
   "opendbc_repo/opendbc/safety/tests/test_subaru_forester_angle.py",
   "opendbc_repo/opendbc/safety/tests/test_subaru_receive_guards.py",
   "opendbc_repo/opendbc/safety/tests/test_subaru_outback_angle.py",
+  'openpilot/starpilot/car/toyota/tests/test_matrix_stock_caller_native.py',
+  'opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py',
 )
 
 # Vehicle tests import both the vendored car code and these host control owners.

tools/ci/tests/test_matrix_registration.py

@@ -0,0 +1,27 @@
+"""Matrix qualification must remain mandatory in lifecycle and RELEASE CI."""
+import ast
+from pathlib import Path
+import unittest
+
+
+ROOT = Path(__file__).resolve().parents[3]
+CALLER = 'openpilot/starpilot/car/toyota/tests/test_matrix_stock_caller_native.py'
+NATIVE = 'opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py'
+
+
+class TestMatrixRegistration(unittest.TestCase):
+  def test_matrix_actual_callers_and_native_are_registered(self):
+    module = ast.parse((ROOT / 'tools/ci/run_vehicle_tests.py').read_text())
+    values = {node.targets[0].id: ast.literal_eval(node.value) for node in module.body
+              if isinstance(node, ast.Assign) and len(node.targets) == 1
+              and isinstance(node.targets[0], ast.Name)
+              and node.targets[0].id in ('AOL_LIFECYCLE_TARGETS', 'RELEASE_TARGETS')}
+    for collection in ('AOL_LIFECYCLE_TARGETS', 'RELEASE_TARGETS'):
+      for target in (CALLER, NATIVE):
+        assert values[collection].count(target) == 1
+        assert (ROOT / target).is_file()
+    native = ast.parse((ROOT / NATIVE).read_text())
+    caller = ast.parse((ROOT / CALLER).read_text())
+    for module in (native, caller):
+      assert any(isinstance(node, ast.Assign) and any(isinstance(target, ast.Name)
+                 and target.id == 'source_matched_native' for target in node.targets) for node in module.body)