From 3093e5b799c82780ef8b957099e1bdef866565eb Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 7 Nov 2026 17:58:51 -0600 Subject: [PATCH] Preserve Polestar stock status relay --- opendbc_repo/opendbc/safety/modes/volvo_cma.h | 68 +++++- .../opendbc/safety/modes/volvo_dispatch.h | 2 +- .../safety/tests/test_polestar_counters.py | 196 ++++++++++++++++++ 3 files changed, 263 insertions(+), 3 deletions(-) diff --git a/opendbc_repo/opendbc/safety/modes/volvo_cma.h b/opendbc_repo/opendbc/safety/modes/volvo_cma.h index 60fa4b9612..d93e366ae0 100644 --- a/opendbc_repo/opendbc/safety/modes/volvo_cma.h +++ b/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 }; diff --git a/opendbc_repo/opendbc/safety/modes/volvo_dispatch.h b/opendbc_repo/opendbc/safety/modes/volvo_dispatch.h index 1aaecebf9f..080b450e03 100644 --- a/opendbc_repo/opendbc/safety/modes/volvo_dispatch.h +++ b/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, diff --git a/opendbc_repo/opendbc/safety/tests/test_polestar_counters.py b/opendbc_repo/opendbc/safety/tests/test_polestar_counters.py index c8b9dae6d2..2f31d5809b 100644 --- a/opendbc_repo/opendbc/safety/tests/test_polestar_counters.py +++ b/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())