diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index a4a6d4c..5ecfd65 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -17,11 +17,9 @@ const CanMsg HYUNDAI_TX_MSGS[] = { // {1186, 0, 8} // 4a2SCC, Bus 0 }; -// TODO: missing checksum for wheel speeds message,worst failure case is -// wheel speeds stuck at 0 and we don't disengage on brake press AddrCheckStruct hyundai_rx_checks[] = { {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}}}, - {.msg = {{902, 0, 8, .check_checksum = false, .max_counter = 15U, .expected_timestep = 10000U}}}, + {.msg = {{902, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}}}, {.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}}}, }; @@ -63,6 +61,8 @@ static uint8_t hyundai_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { uint8_t chksum; if (addr == 608) { chksum = GET_BYTE(to_push, 7) & 0xF; + } else if (addr == 902) { + chksum = ((GET_BYTE(to_push, 7) >> 6) << 2) | (GET_BYTE(to_push, 5) >> 6); } else if (addr == 916) { chksum = GET_BYTE(to_push, 6) & 0xF; } else if (addr == 1057) { @@ -77,15 +77,33 @@ 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 + if (addr == 902) { + // count the bits + for (int i = 0; i < 8; i++) { + uint8_t b = GET_BYTE(to_push, i); + for (int j = 0; j < 8; j++) { + uint8_t bit = 0; + // exclude checksum and counter + if (((i != 1) || (j < 6)) && ((i != 3) || (j < 6)) && ((i != 5) || (j < 6)) && ((i != 7) || (j < 6))) { + bit = (b >> (uint8_t)j) & 1U; + } + chksum += bit; + } } - chksum += (b % 16U) + (b / 16U); + chksum = (chksum ^ 9U) & 15U; + } else { + // sum of nibbles + 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 (16U - (chksum % 16U)) % 16U; + + return chksum; } static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py index 1ecdb0c..dc55598 100644 --- a/tests/safety/test_hyundai.py +++ b/tests/safety/test_hyundai.py @@ -22,15 +22,30 @@ 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) + if addr == 902: + for i, b in enumerate(dat): + for j in range(8): + # exclude checksum and counter bits + if (i != 1 or j < 6) and (i != 3 or j < 6) and (i != 5 or j < 6) and (i != 7 or j < 6): + bit = (b >> j) & 1 + else: + bit = 0 + chksum += bit + chksum = (chksum ^ 9) & 0xF + ret = bytearray(dat) + ret[5] |= (chksum & 0x3) << 6 + ret[7] |= (chksum & 0xc) << 4 + else: + 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): @@ -72,7 +87,7 @@ class TestHyundaiSafety(common.PandaSafetyTest): values["WHL_SPD_AliveCounter_LSB"] = (self.cnt_speed % 16) & 0x3 values["WHL_SPD_AliveCounter_MSB"] = (self.cnt_speed % 16) >> 2 self.__class__.cnt_speed += 1 - return self.packer.make_can_msg_panda("WHL_SPD11", 0, values) + return self.packer.make_can_msg_panda("WHL_SPD11", 0, values, fix_checksum=checksum) def _pcm_status_msg(self, enable): values = {"ACCMode": enable, "CR_VSM_Alive": self.cnt_cruise % 16}