Preserve Polestar stock status relay

opendbc_repo/opendbc/safety/modes/volvo_cma.h

@@ -280,6 +280,65 @@ static bool volvo_cma_receiver_quality(const CANPacket_t *msg) {
   return volvo_cma_quality(msg) && volvo_cma_lca2_quality(msg);
 }
 
+// Unchanged native status is transport evidence, never valid control evidence.
+static uint8_t volvo_cma_pscm_status_data[4][8];
+static uint32_t volvo_cma_pscm_status_ts[4];
+static bool volvo_cma_pscm_status_valid[4];
+static uint8_t volvo_cma_pscm_status_index;
+static uint8_t volvo_cma_pscm_status_nibble;
+static bool volvo_cma_pscm_status_checksum_valid;
+static bool volvo_cma_pscm_status_quality_valid;
+
+static void volvo_cma_clear_pscm_status(void) {
+  for (uint8_t i = 0U; i < 4U; i++) {
+    volvo_cma_pscm_status_valid[i] = false;
+  }
+  volvo_cma_pscm_status_index = 0U;
+}
+
+static bool volvo_cma_receiver_quality_rx(const CANPacket_t *msg) {
+  const bool quality_valid = volvo_cma_receiver_quality(msg);
+  if (volvo_cma_lca2_mod15 && msg_matches(msg, 0x17U, 2U, 8U) &&
+      !msg->returned && !msg->rejected && !msg->extended && !msg->fd) {
+    const uint8_t nibble = msg->data[2] >> 4U;
+    const bool checksum_valid = volvo_cma_get_checksum(msg) == volvo_cma_compute_checksum(msg);
+    // Retain the existing four-frame transport tolerance, but never replay an
+    // earlier status/validation class after a newly observed native transition.
+    if ((nibble != volvo_cma_pscm_status_nibble) ||
+        (checksum_valid != volvo_cma_pscm_status_checksum_valid) ||
+        (quality_valid != volvo_cma_pscm_status_quality_valid)) {
+      volvo_cma_clear_pscm_status();
+    }
+    volvo_cma_pscm_status_nibble = nibble;
+    volvo_cma_pscm_status_checksum_valid = checksum_valid;
+    volvo_cma_pscm_status_quality_valid = quality_valid;
+    const uint8_t slot = volvo_cma_pscm_status_index;
+    for (uint8_t i = 0U; i < 8U; i++) {
+      volvo_cma_pscm_status_data[slot][i] = msg->data[i];
+    }
+    volvo_cma_pscm_status_ts[slot] = microsecond_timer_get();
+    volvo_cma_pscm_status_valid[slot] = true;
+    volvo_cma_pscm_status_index = (slot + 1U) % 4U;
+  }
+  return quality_valid;
+}
+
+static bool volvo_cma_exact_pscm_status(const CANPacket_t *msg) {
+  bool matches = false;
+  if (msg_matches(msg, 0x17U, 0U, 8U) &&
+      !msg->returned && !msg->rejected && !msg->extended && !msg->fd) {
+    for (uint8_t slot = 0U; slot < 4U; slot++) {
+      bool same = volvo_cma_pscm_status_valid[slot] &&
+        (safety_get_ts_elapsed(microsecond_timer_get(), volvo_cma_pscm_status_ts[slot]) <= 100000U);
+      for (uint8_t i = 0U; i < 8U; i++) {
+        same &= msg->data[i] == volvo_cma_pscm_status_data[slot][i];
+      }
+      matches |= same;
+    }
+  }
+  return matches;
+}
+
 // The Polestar source clocks advance nominally2/4 per50Hz frame, with
 // observed sampling jitter of1..3/3..5. Frozen/jumped counters retain the shared tolerance.
 static uint8_t volvo_cma_lca2_counter(const CANPacket_t *msg) {
@@ -383,7 +442,8 @@ static bool volvo_cma_tx_hook(const CANPacket_t *msg) {
   // policy or intent and retains the existing health deadlines/tolerances.
   bool tx;
   if (!controls_allowed || !volvo_cma_drive_current() || !aol_rx_healthy()) {
-    tx = volvo_cma_exact_stock(msg);
+    tx = (volvo_cma_lca2_mod15 && (msg->addr == 0x17U)) ?
+      volvo_cma_exact_pscm_status(msg) : volvo_cma_exact_stock(msg);
   } else {
     tx = volvo_cma_receiver_quality(msg) && volvo_cma_source_projection(msg);
     if ((msg->addr == 0x69U) || (msg->addr == 0x67U) || (msg->addr == 0x17U)) {
@@ -514,6 +574,10 @@ static RxCheck *volvo_cma_rx_checks_workspace(void) {
 static safety_config volvo_cma_init(uint16_t param) {
   RxCheck *const rx_workspace = volvo_cma_rx_checks_workspace();
   volvo_cma_lca2_mod15 = param == 5U;
+  volvo_cma_clear_pscm_status();
+  volvo_cma_pscm_status_nibble = 0U;
+  volvo_cma_pscm_status_checksum_valid = false;
+  volvo_cma_pscm_status_quality_valid = false;
   volvo_cma_lca2_second_seen = false;
   volvo_cma_lca2_second_last = 0U;
   volvo_cma_lca2_second_wrong = 0;
@@ -635,6 +699,6 @@ const safety_hooks volvo_cma_hooks = {
   .get_counter = volvo_cma_lca2_counter,
   .get_checksum = volvo_cma_get_checksum,
   .compute_checksum = volvo_cma_compute_checksum,
-  .get_quality_flag_valid = volvo_cma_receiver_quality,
+  .get_quality_flag_valid = volvo_cma_receiver_quality_rx,
   // No custom fwd hook - stock LCA always blocked by .check_relay = true
 };

opendbc_repo/opendbc/safety/modes/volvo_dispatch.h

@@ -29,7 +29,7 @@ static uint32_t volvo_dispatch_compute_checksum(const CANPacket_t *msg) {
   return volvo_dispatch_c1 ? 0U : volvo_cma_compute_checksum(msg);
 }
 static bool volvo_dispatch_quality(const CANPacket_t *msg) {
-  return volvo_dispatch_c1 || volvo_cma_receiver_quality(msg);
+  return volvo_dispatch_c1 || volvo_cma_receiver_quality_rx(msg);
 }
 const safety_hooks volvo_dispatch_hooks = {.init = volvo_dispatch_init, .rx = volvo_dispatch_rx, .tx = volvo_dispatch_tx,
   .get_counter = volvo_dispatch_get_counter,

opendbc_repo/opendbc/safety/tests/test_polestar_counters.py

@@ -86,3 +86,199 @@ class TestPolestarCounters(unittest.TestCase):
     self.assertTrue(self.safety.safety_config_valid())
     self.assertTrue(self.safety.get_controls_allowed())
     self.assertTrue(self.tx(self.lca_command()))
+
+  def unknown_pscm_status(self):
+    return 0x17, bytes.fromhex('d400a0858000ffff'), 2
+
+  def test_unknown_status_relays_without_becoming_control_evidence(self):
+    for cruise in (False, True):
+      with self.subTest(cruise=cruise):
+        self.reset()
+        incoming = self.feed(cruise=cruise)
+        status = self.unknown_pscm_status()
+        self.assertFalse(self.rx(status))
+        self.assertFalse(self.safety.get_controls_allowed())
+        self.safety.safety_tick()
+        self.assertFalse(self.safety.safety_config_valid())
+        self.assertTrue(self.tx(self.relay(status)))
+        self.assertFalse(self.tx(self.relay(incoming[0x17])))
+        self.assertFalse(self.tx(self.lca_command()))
+        self.safety.set_controls_allowed(True)
+        self.assertTrue(self.tx(self.relay(status)))
+        self.assertFalse(self.tx(self.lca_command()))
+        for address in cma.RELAY_BUSES:
+          if address != 0x17:
+            self.assertTrue(self.tx(self.relay(incoming[address])))
+        self.assertEqual(self.safety.safety_fwd_hook(2, 0x17), -1)
+
+  def test_unknown_status_requires_exact_native_bytes(self):
+    self.feed()
+    status = self.unknown_pscm_status()
+    self.assertFalse(self.rx(status))
+    for bit in range(64):
+      data = bytearray(status[1])
+      data[bit // 8] ^= 1 << (bit % 8)
+      self.assertFalse(self.tx((0x17, data, 0)), bit)
+    self.assertTrue(self.tx(self.relay(status)))
+
+  def test_unknown_status_source_identity_cannot_be_forged(self):
+    status = self.unknown_pscm_status()
+    for field in ('returned', 'rejected', 'extended', 'fd'):
+      with self.subTest(field=field):
+        self.reset()
+        self.feed(cruise=False)
+        packet = cma.libsafety_py.make_CANPacket(status[0], 2, status[1])
+        setattr(packet[0], field, 1)
+        self.safety.safety_rx_hook(packet)
+        self.assertFalse(self.tx(self.relay(status)))
+    for bus in (0, 1, 3, 4, 5, 6, 7):
+      self.reset()
+      self.feed(cruise=False)
+      self.rx((0x17, status[1], bus))
+      self.assertFalse(self.tx(self.relay(status)))
+    for length in (*range(8), 12):
+      self.reset()
+      self.feed(cruise=False)
+      self.rx((0x17, (status[1] + bytes(4))[:length], 2))
+      self.assertFalse(self.tx(self.relay(status)))
+
+  def test_unknown_status_tx_cannot_seed_or_refresh_source(self):
+    self.feed(cruise=False)
+    status = self.unknown_pscm_status()
+    self.assertFalse(self.tx(self.relay(status)))
+    self.assertFalse(self.rx(status))
+    self.safety.set_timer(110000)
+    self.assertTrue(self.tx(self.relay(status)))
+    self.safety.set_timer(110001)
+    self.assertFalse(self.tx(self.relay(status)))
+    self.assertFalse(self.tx(self.relay(status)))
+
+  def test_unknown_status_original_age_survives_timer_wrap(self):
+    self.feed(cruise=False)
+    start = 0xFFFFFF00
+    self.safety.set_timer(start)
+    status = self.unknown_pscm_status()
+    self.assertFalse(self.rx(status))
+    for age, accepted in ((90000, True), (100000, True), (100001, False)):
+      self.safety.set_timer((start + age) & 0xFFFFFFFF)
+      self.assertEqual(bool(self.tx(self.relay(status))), accepted)
+
+  def test_status_preserves_four_frame_transport_tolerance(self):
+    for unknown in (False, True):
+      with self.subTest(unknown=unknown):
+        self.reset()
+        incoming = self.feed(cruise=False)
+        source = self.unknown_pscm_status() if unknown else incoming[0x17]
+        frames = [(0x17, source[1][:-1] + bytes([i]), 2) for i in range(5)]
+        for i, frame in enumerate(frames):
+          self.safety.set_timer(20000 + i * 10000)
+          self.assertEqual(bool(self.rx(frame)), not unknown)
+          for retained in frames[max(0, i - 3):i + 1]:
+            self.assertTrue(self.tx(self.relay(retained)))
+        self.assertFalse(self.tx(self.relay(frames[0])))
+        # A new native arrival never refreshes another slot's original age.
+        self.safety.set_timer(130000)
+        self.assertTrue(self.tx(self.relay(frames[1])))
+        self.safety.set_timer(130001)
+        self.assertFalse(self.tx(self.relay(frames[1])))
+        self.assertTrue(self.tx(self.relay(frames[2])))
+
+  def test_native_status_transition_flushes_old_transport_evidence(self):
+    incoming = self.feed(cruise=False)
+    healthy = incoming[0x17]
+    first = self.unknown_pscm_status()
+    second = first[0], first[1][:-1] + b'\xfe', first[2]
+    restored = healthy[0], healthy[1][:-1] + b'\x01', healthy[2]
+    self.assertFalse(self.rx(first))
+    self.assertTrue(self.tx(self.relay(first)))
+    self.assertFalse(self.tx(self.relay(healthy)))
+    self.assertFalse(self.rx(second))
+    self.assertTrue(self.tx(self.relay(first)))
+    self.assertTrue(self.tx(self.relay(second)))
+    self.assertTrue(self.rx(restored))
+    for stale in (healthy, first, second):
+      self.assertFalse(self.tx(self.relay(stale)))
+    self.assertTrue(self.tx(self.relay(restored)))
+    self.assertFalse(self.safety.get_controls_allowed())
+
+  def test_each_native_validation_class_change_flushes_transport_evidence(self):
+    healthy = self.feed(cruise=False)[0x17]
+    # Keep the high nibble at 8 and change one validation boolean at a time:
+    # (checksum, quality): TT -> FT -> FF -> TF -> TT.
+    bad_checksum = bytearray(healthy[1])
+    bad_checksum[0] ^= 1
+    bad_both = bytearray(bad_checksum)
+    bad_both[1] |= 8  # This quality bit is unconstrained by the checksum.
+    bad_quality = bytearray(healthy[1])
+    bad_quality[1] |= 8
+    frames = [healthy, (0x17, bad_checksum, 2), (0x17, bad_both, 2),
+              (0x17, bad_quality, 2), (0x17, healthy[1][:-1] + b'\x01', 2)]
+    for i, frame in enumerate(frames):
+      self.assertEqual(bool(self.rx(frame)), i in (0, 4))
+      self.assertTrue(self.tx(self.relay(frame)))
+      for stale in frames[:i]:
+        self.assertFalse(self.tx(self.relay(stale)))
+      self.assertFalse(self.safety.get_controls_allowed())
+
+  def test_unknown_status_cache_does_not_cross_mode_reset(self):
+    self.feed(cruise=False)
+    status = self.unknown_pscm_status()
+    self.assertFalse(self.rx(status))
+    self.assertTrue(self.tx(self.relay(status)))
+    self.reset()
+    self.assertFalse(self.tx(self.relay(status)))
+    self.feed(cruise=False)
+    self.assertFalse(self.tx(self.relay(status)))
+
+  def test_unknown_status_does_not_expand_other_profiles(self):
+    status = self.unknown_pscm_status()
+    for profile in (0, 1):
+      self.assertEqual(self.safety.set_safety_hooks(36, profile), 0)
+      self.safety.init_tests()
+      self.safety.set_timer(10000)
+      self.assertFalse(self.rx(status))
+      self.assertFalse(self.tx(self.relay(status)))
+    self.assertNotEqual(self.safety.set_safety_hooks(36, 4), 0)
+
+  def test_unknown_status_remains_blocked_on_relay_malfunction(self):
+    self.feed(cruise=False)
+    status = self.unknown_pscm_status()
+    self.assertFalse(self.rx(status))
+    self.assertTrue(self.tx(self.relay(status)))
+    self.rx((0x58, bytes(8), 2))
+    self.assertTrue(self.safety.get_relay_malfunction())
+    self.assertFalse(self.tx(self.relay(status)))
+
+  def test_unknown_status_tx_identity_matches_native_framing(self):
+    self.feed(cruise=False)
+    status = self.unknown_pscm_status()
+    self.assertFalse(self.rx(status))
+    for field in ('returned', 'rejected', 'extended', 'fd'):
+      packet = cma.libsafety_py.make_CANPacket(0x17, 0, status[1])
+      setattr(packet[0], field, 1)
+      self.assertFalse(self.safety.safety_tx_hook(packet), field)
+      self.assertTrue(self.tx(self.relay(status)))
+    for bus in range(1, 8):
+      self.assertFalse(self.tx((0x17, status[1], bus)))
+    for length in (*range(8), 12):
+      self.assertFalse(self.tx((0x17, (status[1] + bytes(4))[:length], 0)))
+    self.assertFalse(self.tx((0x16, status[1], 0)))
+    self.assertTrue(self.tx(self.relay(status)))
+
+  def test_modified_status_is_rejected_until_controller_returns_to_stock(self):
+    self.feed()
+    status = self.unknown_pscm_status()
+    stock = {
+      'CHECKSUM_1': 0xD4, 'LCA_ENABLED_ECHO': 0, 'SIG1_BYTE_1_HI_NIBBLE': 0,
+      'SIG1_REPLICA_BYTE_2_LO_NIBLE': 0, 'NEW_SIGNAL_2': 10, 'CHECKSUM_2': 0x85,
+      'BYTE_4_5': 0x8000, 'BYTE_6': 0xFF, 'BYTE_7': 0xFF,
+    }
+    self.assertFalse(self.rx(status))
+    self.assertFalse(self.safety.get_controls_allowed())
+    modified = cma.cma_can.create_pscm_related_message(self.mid, True, False, stock, 1)
+    self.assertNotEqual(modified[1], status[1])
+    self.assertFalse(self.tx(modified))
+    unchanged = cma.cma_can.create_pscm_related_message(self.mid, False, False, stock, 1)
+    self.assertEqual(unchanged[1], status[1])
+    self.assertTrue(self.tx(unchanged))
+    self.assertFalse(self.safety.get_controls_allowed())