diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index a6166ab53a6bdc..5c6ac470b68a83 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -10,11 +10,11 @@ const CanMsg HYUNDAI_TX_MSGS[] = {{832, 0, 8}, {1265, 0, 4}, {1157, 0, 4}}; // TODO: do checksum checks AddrCheckStruct hyundai_rx_checks[] = { - {.msg = {{608, 0, 8}}, .max_counter = 3U, .expected_timestep = 10000U}, + {.msg = {{608, 0, 8}}, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, {.msg = {{897, 0, 8}}, .max_counter = 255U, .expected_timestep = 10000U}, {.msg = {{902, 0, 8}}, .max_counter = 15U, .expected_timestep = 10000U}, - {.msg = {{916, 0, 8}}, .max_counter = 7U, .expected_timestep = 10000U}, - {.msg = {{1057, 0, 8}}, .max_counter = 15U, .expected_timestep = 20000U}, + {.msg = {{916, 0, 8}}, .check_checksum = true, .max_counter = 7U, .expected_timestep = 10000U}, + {.msg = {{1057, 0, 8}}, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, }; const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]); @@ -38,10 +38,43 @@ static uint8_t hyundai_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { return cnt; } +static uint8_t hyundai_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t chksum; + if (addr == 608) { + chksum = GET_BYTE(to_push, 7) & 0xF; + } else if (addr == 916) { + chksum = GET_BYTE(to_push, 6) & 0xF; + } else if (addr == 1057) { + chksum = GET_BYTE(to_push, 7) >> 4; + } else { + chksum = 0; + } + return chksum; +} + +static uint8_t hyundai_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t chksum = 0; + // same algorithm, but checksum is in a different place + for (int i = 0; i < 8; i++) { + uint8_t b = GET_BYTE(to_push, i); + if (((addr == 608) && (i == 7)) || ((addr == 916) && (i == 6)) || ((addr == 1057) && (i == 7))) { + b &= (addr == 1057) ? 0x0FU : 0xF0U; // remove checksum + } + chksum += (b % 16U) + (b / 16U); + } + chksum = (16U - (chksum % 16U)) % 16U; + return chksum; +} + static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, hyundai_rx_checks, HYUNDAI_RX_CHECK_LEN, - NULL, NULL, hyundai_get_counter); + hyundai_get_checksum, hyundai_compute_checksum, + hyundai_get_counter); bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; @@ -191,7 +224,6 @@ static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return bus_fwd; } - const safety_hooks hyundai_hooks = { .init = nooutput_init, .rx = hyundai_rx_hook, diff --git a/tests/safety/common.py b/tests/safety/common.py index 8d81f115c18b77..945a974f2e648b 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -32,8 +32,10 @@ def make_msg(bus, addr, length=8): return package_can_msg([addr, 0, b'\x00'*length, bus]) class CANPackerPanda(CANPacker): - def make_can_msg_panda(self, name_or_addr, bus, values, counter=-1): + def make_can_msg_panda(self, name_or_addr, bus, values, counter=-1, fix_checksum=None): msg = self.make_can_msg(name_or_addr, bus, values, counter=-1) + if fix_checksum is not None: + msg = fix_checksum(msg) return package_can_msg(msg) class PandaSafetyTestBase(unittest.TestCase): diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py index 88c1aa46b79745..9f173253c2644a 100644 --- a/tests/safety/test_hyundai.py +++ b/tests/safety/test_hyundai.py @@ -16,6 +16,22 @@ DRIVER_TORQUE_ALLOWANCE = 50 DRIVER_TORQUE_FACTOR = 2 +# 4 bit checkusm used in some hyundai messages +# lives outside the can packer because we never send this msg +def checksum(msg): + addr, t, dat, bus = msg + + chksum = 0 + for i, b in enumerate(dat): + if addr in [608, 1057] and i == 7: + b &= 0x0F if addr == 1057 else 0xF0 + elif addr == 916 and i == 6: + b &= 0xF0 + chksum += sum(divmod(b, 16)) + chksum = (16 - chksum) % 16 + ret = bytearray(dat) + ret[6 if addr == 916 else 7] |= chksum << (4 if addr == 1057 else 0) + return addr, t, ret, bus class TestHyundaiSafety(common.PandaSafetyTest): TX_MSGS = [[832, 0], [1265, 0], [1157, 0]] @@ -43,12 +59,12 @@ def _button_msg(self, buttons): def _gas_msg(self, val): values = {"CF_Ems_AclAct": val, "AliveCounter": self.cnt_gas % 4} self.__class__.cnt_gas += 1 - return self.packer.make_can_msg_panda("EMS16", 0, values) + return self.packer.make_can_msg_panda("EMS16", 0, values, fix_checksum=checksum) def _brake_msg(self, brake): values = {"DriverBraking": brake, "AliveCounterTCS": self.cnt_brake % 8} self.__class__.cnt_brake += 1 - return self.packer.make_can_msg_panda("TCS13", 0, values) + return self.packer.make_can_msg_panda("TCS13", 0, values, fix_checksum=checksum) def _speed_msg(self, speed): # panda safety doesn't scale, so undo the scaling @@ -61,7 +77,7 @@ def _speed_msg(self, speed): def _pcm_status_msg(self, enabled): values = {"ACCMode": enabled, "CR_VSM_Alive": self.cnt_cruise % 16} self.__class__.cnt_cruise += 1 - return self.packer.make_can_msg_panda("SCC12", 0, values) + return self.packer.make_can_msg_panda("SCC12", 0, values, fix_checksum=checksum) def _set_prev_torque(self, t): self.safety.set_desired_torque_last(t)