diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 6fd7c02b3065b9..cd9bb5b9863b73 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -13,12 +13,12 @@ const AddrBus SUBARU_L_TX_MSGS[] = {{0x164, 0}, {0x221, 0}, {0x322, 0}}; const int SUBARU_TX_MSGS_LEN = sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]); const int SUBARU_L_TX_MSGS_LEN = sizeof(SUBARU_L_TX_MSGS) / sizeof(SUBARU_L_TX_MSGS[0]); -// TODO: do checksum and counter checks after adding the signals to the outback dbc file AddrCheckStruct subaru_rx_checks[] = { - {.addr = { 0x40}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {0x119}, .bus = 0, .expected_timestep = 20000U}, - {.addr = {0x240}, .bus = 0, .expected_timestep = 50000U}, + {.addr = { 0x40}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, + {.addr = {0x119}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, + {.addr = {0x240}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, }; +// TODO: do checksum and counter checks after adding the signals to the outback dbc file AddrCheckStruct subaru_l_rx_checks[] = { {.addr = {0x140}, .bus = 0, .expected_timestep = 10000U}, {.addr = {0x371}, .bus = 0, .expected_timestep = 20000U}, @@ -35,12 +35,30 @@ bool subaru_gas_last = false; bool subaru_global = false; struct sample_t subaru_torque_driver; // last few driver torques measured +static uint8_t subaru_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + return (uint8_t)GET_BYTE(to_push, 0); +} + +static uint8_t subaru_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { + return (uint8_t)(GET_BYTE(to_push, 1) & 0xF); +} + +static uint8_t subaru_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + int addr = GET_ADDR(to_push); + int len = GET_LEN(to_push); + uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U); + for (int i = 1; i < len; i++) { + checksum += (uint8_t)GET_BYTE(to_push, i); + } + return checksum; +} + static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = false; if (subaru_global) { valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN, - NULL, NULL, NULL); + subaru_get_checksum, subaru_compute_checksum, subaru_get_counter); } else { valid = addr_safety_check(to_push, subaru_l_rx_checks, SUBARU_L_RX_CHECK_LEN, NULL, NULL, NULL); diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index 2f02718be9b314..ce2eca6ee9ad44 100644 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -30,12 +30,25 @@ def sign(a): else: return -1 +def subaru_checksum(msg, addr, len_msg): + checksum = addr + (addr >> 8) + for i in range(len_msg): + if i < 4: + checksum += (msg.RDLR >> (8 * i)) + else: + checksum += (msg.RDHR >> (8 * (i - 4))) + return checksum & 0xff + + class TestSubaruSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU, 0) cls.safety.init_tests_subaru() + cls.cnt_gas = 0 + cls.cnt_torque_driver = 0 + cls.cnt_cruise = 0 def _set_prev_torque(self, t): self.safety.set_subaru_desired_torque_last(t) @@ -46,6 +59,9 @@ def _torque_driver_msg(self, torque): if self.safety.get_subaru_global(): to_send = make_msg(0, 0x119) to_send[0].RDLR = ((t & 0x7FF) << 16) + to_send[0].RDLR |= (self.cnt_torque_driver & 0xF) << 8 + to_send[0].RDLR |= subaru_checksum(to_send, 0x119, 8) + self.cnt_torque_driver += 1 else: to_send = make_msg(0, 0x371) to_send[0].RDLR = (t & 0x7) << 29 @@ -66,6 +82,9 @@ def _gas_msg(self, gas): if self.safety.get_subaru_global(): to_send = make_msg(0, 0x40) to_send[0].RDHR = gas & 0xFF + to_send[0].RDLR |= (self.cnt_gas & 0xF) << 8 + to_send[0].RDLR |= subaru_checksum(to_send, 0x40, 8) + self.cnt_gas += 1 else: to_send = make_msg(0, 0x140) to_send[0].RDLR = gas & 0xFF @@ -75,6 +94,9 @@ def _cruise_msg(self, cruise): if self.safety.get_subaru_global(): to_send = make_msg(0, 0x240) to_send[0].RDHR = cruise << 9 + to_send[0].RDLR |= (self.cnt_cruise & 0xF) << 8 + to_send[0].RDLR |= subaru_checksum(to_send, 0x240, 8) + self.cnt_cruise += 1 else: to_send = make_msg(0, 0x144) to_send[0].RDHR = cruise << 17