diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index dd37f05..7914ead 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -9,10 +9,10 @@ const AddrBus CHRYSLER_TX_MSGS[] = {{571, 0}, {658, 0}, {678, 0}}; // TODO: do checksum and counter checks AddrCheckStruct chrysler_rx_checks[] = { - {.addr = {544}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {514}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {500}, .bus = 0, .expected_timestep = 20000U}, - {.addr = {308}, .bus = 0, .expected_timestep = 20000U}, // verify ts + {.addr = {544}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, + {.addr = {514}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U}, + {.addr = {500}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, + {.addr = {308}, .bus = 0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 20000U}, }; const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]); @@ -24,10 +24,54 @@ int chrysler_speed = 0; uint32_t chrysler_ts_last = 0; struct sample_t chrysler_torque_meas; // last few torques measured +static uint8_t chrysler_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + int checksum_byte = GET_LEN(to_push) - 1; + return (uint8_t)(GET_BYTE(to_push, checksum_byte)); +} + +static uint8_t chrysler_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + /* This function does not want the checksum byte in the input data. + jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf */ + uint8_t checksum = 0xFF; + int len = GET_LEN(to_push); + for (int j = 0; j < (len - 1); j++) { + uint8_t shift = 0x80; + uint8_t curr = (uint8_t)GET_BYTE(to_push, j); + for (int i=0; i<8; i++) { + uint8_t bit_sum = curr & shift; + uint8_t temp_chk = checksum & 0x80U; + if (bit_sum != 0U) { + bit_sum = 0x1C; + if (temp_chk != 0U) { + bit_sum = 1; + } + checksum = checksum << 1; + temp_chk = checksum | 1U; + bit_sum ^= temp_chk; + } else { + if (temp_chk != 0U) { + bit_sum = 0x1D; + } + checksum = checksum << 1; + bit_sum ^= checksum; + } + checksum = bit_sum; + shift = shift >> 1; + } + } + return ~checksum; +} + +static uint8_t chrysler_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { + // Well defined counter only for 8 bytes messages + return (uint8_t)(GET_BYTE(to_push, 6) >> 4); +} + static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, chrysler_rx_checks, CHRYSLER_RX_CHECK_LEN, - NULL, NULL, NULL); + chrysler_get_checksum, chrysler_compute_checksum, + chrysler_get_counter); if (valid) { int bus = GET_BUS(to_push); diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index b49c929..6ce179f 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -48,7 +48,7 @@ static uint8_t honda_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { while (addr > 0U) { checksum += (addr & 0xFU); addr >>= 4; } - for (int j = 0; (j < len); j++) { + for (int j = 0; j < len; j++) { uint8_t byte = GET_BYTE(to_push, j); checksum += (byte & 0xFU) + (byte >> 4U); if (j == (len - 1)) { diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index 09720bf..0e7b716 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -16,18 +16,54 @@ MAX_TORQUE_ERROR = 80 TX_MSGS = [[571, 0], [658, 0], [678, 0]] +def chrysler_checksum(msg, len_msg): + checksum = 0xFF + for idx in range(0, len_msg-1): + curr = (msg.RDLR >> (8*idx)) if idx < 4 else (msg.RDHR >> (8*(idx - 4))) + curr &= 0xFF + shift = 0x80 + for i in range(0, 8): + bit_sum = curr & shift + temp_chk = checksum & 0x80 + if (bit_sum != 0): + bit_sum = 0x1C + if (temp_chk != 0): + bit_sum = 1 + checksum = checksum << 1 + temp_chk = checksum | 1 + bit_sum ^= temp_chk + else: + if (temp_chk != 0): + bit_sum = 0x1D + checksum = checksum << 1 + bit_sum ^= checksum + checksum = bit_sum + shift = shift >> 1 + return ~checksum & 0xFF + class TestChryslerSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety cls.safety.set_safety_hooks(Panda.SAFETY_CHRYSLER, 0) cls.safety.init_tests_chrysler() + cls.cnt_torque_meas = 0 + cls.cnt_gas = 0 + cls.cnt_cruise = 0 def _button_msg(self, buttons): to_send = make_msg(0, 571) to_send[0].RDLR = buttons return to_send + def _cruise_msg(self, active): + to_send = make_msg(0, 500) + to_send[0].RDLR = 0x380000 if active else 0 + to_send[0].RDHR |= (self.cnt_cruise % 16) << 20 + to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24 + self.cnt_cruise += 1 + return to_send + def _speed_msg(self, speed): speed = int(speed / 0.071028) to_send = make_msg(0, 514, 4) @@ -38,6 +74,8 @@ class TestChryslerSafety(unittest.TestCase): def _gas_msg(self, gas): to_send = make_msg(0, 308) to_send[0].RDHR = (gas & 0x7F) << 8 + to_send[0].RDHR |= (self.cnt_gas % 16) << 20 + self.cnt_gas += 1 return to_send def _set_prev_torque(self, t): @@ -48,6 +86,9 @@ class TestChryslerSafety(unittest.TestCase): def _torque_meas_msg(self, torque): to_send = make_msg(0, 544) to_send[0].RDHR = ((torque + 1024) >> 8) + (((torque + 1024) & 0xff) << 8) + to_send[0].RDHR |= (self.cnt_torque_meas % 16) << 20 + to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24 + self.cnt_torque_meas += 1 return to_send def _torque_msg(self, torque): @@ -78,16 +119,12 @@ class TestChryslerSafety(unittest.TestCase): test_manually_enable_controls_allowed(self) def test_enable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x1F4) - to_push[0].RDLR = 0x380000 - + to_push = self._cruise_msg(True) self.safety.safety_rx_hook(to_push) self.assertTrue(self.safety.get_controls_allowed()) def test_disable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x1F4) - to_push[0].RDLR = 0 - + to_push = self._cruise_msg(False) self.safety.set_controls_allowed(1) self.safety.safety_rx_hook(to_push) self.assertFalse(self.safety.get_controls_allowed())