From 36c62afa0c170aa1b7a39bcae3316ffb844499e8 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 20 May 2022 00:59:58 +0200 Subject: [PATCH] Kia EV6 (#905) * block lkas * taco time * local changes * start * tx checks * counter + vehicle moving * support big can fd * check crc * add torque to rx checks * tests * little more * little more * get some misra coverage Co-authored-by: Comma Device Co-authored-by: Adeeb Shihadeh --- board/drivers/fdcan.h | 2 + board/safety.h | 30 +++- board/safety/safety_hyundai_hda2.h | 211 +++++++++++++++++++++++++++ board/safety/safety_volkswagen_mqb.h | 2 +- board/safety_declarations.h | 3 +- python/__init__.py | 1 + tests/misra/test_misra.sh | 2 +- tests/safety/common.py | 11 +- tests/safety/test.c | 2 + tests/safety/test_hyundai_hda2.py | 76 ++++++++++ 10 files changed, 332 insertions(+), 8 deletions(-) create mode 100644 board/safety/safety_hyundai_hda2.h create mode 100755 tests/safety/test_hyundai_hda2.py diff --git a/board/drivers/fdcan.h b/board/drivers/fdcan.h index 1cd637ef4..8a87c4149 100644 --- a/board/drivers/fdcan.h +++ b/board/drivers/fdcan.h @@ -2,6 +2,8 @@ // FDCAN2_IT0, FDCAN2_IT1 // FDCAN3_IT0, FDCAN3_IT1 +#define CANFD + #define BUS_OFF_FAIL_LIMIT 2U uint8_t bus_off_err[] = {0U, 0U, 0U}; diff --git a/board/safety.h b/board/safety.h index 3bce2b615..875e9e408 100644 --- a/board/safety.h +++ b/board/safety.h @@ -17,6 +17,11 @@ #include "safety/safety_elm327.h" #include "safety/safety_body.h" +// CAN-FD only safety modes +#ifdef CANFD +#include "safety/safety_hyundai_hda2.h" +#endif + // from cereal.car.CarParams.SafetyModel #define SAFETY_SILENT 0U #define SAFETY_HONDA_NIDEC 1U @@ -43,6 +48,7 @@ #define SAFETY_STELLANTIS 25U #define SAFETY_FAW 26U #define SAFETY_BODY 27U +#define SAFETY_HYUNDAI_HDA2 28U uint16_t current_safety_mode = SAFETY_SILENT; uint16_t current_safety_param = 0; @@ -71,14 +77,29 @@ bool get_longitudinal_allowed(void) { // Given a CRC-8 poly, generate a static lookup table to use with a fast CRC-8 // algorithm. Called at init time for safety modes using CRC-8. -void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) { +void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]) { for (int i = 0; i < 256; i++) { uint8_t crc = i; for (int j = 0; j < 8; j++) { - if ((crc & 0x80U) != 0U) + if ((crc & 0x80U) != 0U) { crc = (uint8_t)((crc << 1) ^ poly); - else + } else { crc <<= 1; + } + } + crc_lut[i] = crc; + } +} + +void gen_crc_lookup_table_16(uint16_t poly, uint16_t crc_lut[]) { + for (uint16_t i = 0; i < 256U; i++) { + uint16_t crc = i << 8U; + for (uint16_t j = 0; j < 8U; j++) { + if ((crc & 0x8000U) != 0U) { + crc = (uint16_t)((crc << 1) ^ poly); + } else { + crc <<= 1; + } } crc_lut[i] = crc; } @@ -253,6 +274,9 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_HYUNDAI_LEGACY, &hyundai_legacy_hooks}, {SAFETY_MAZDA, &mazda_hooks}, {SAFETY_BODY, &body_hooks}, +#ifdef CANFD + {SAFETY_HYUNDAI_HDA2, &hyundai_hda2_hooks}, +#endif #ifdef ALLOW_DEBUG {SAFETY_TESLA, &tesla_hooks}, {SAFETY_SUBARU_LEGACY, &subaru_legacy_hooks}, diff --git a/board/safety/safety_hyundai_hda2.h b/board/safety/safety_hyundai_hda2.h new file mode 100644 index 000000000..b28b470e9 --- /dev/null +++ b/board/safety/safety_hyundai_hda2.h @@ -0,0 +1,211 @@ +const int HYUNDAI_HDA2_MAX_STEER = 150; +const int HYUNDAI_HDA2_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks +const uint32_t HYUNDAI_HDA2_RT_INTERVAL = 250000; // 250ms between real time checks +const int HYUNDAI_HDA2_MAX_RATE_UP = 3; +const int HYUNDAI_HDA2_MAX_RATE_DOWN = 7; +const int HYUNDAI_HDA2_DRIVER_TORQUE_ALLOWANCE = 50; +const int HYUNDAI_HDA2_DRIVER_TORQUE_FACTOR = 2; +const uint32_t HYUNDAI_HDA2_STANDSTILL_THRSLD = 30; // ~1kph + +const CanMsg HYUNDAI_HDA2_TX_MSGS[] = { + {0x50, 0, 16}, + {0x1CF, 1, 8}, +}; + +AddrCheckStruct hyundai_hda2_addr_checks[] = { + {.msg = {{0x35, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{0x65, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{0xa0, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{0xea, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{0x175, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}}, +}; +#define HYUNDAI_HDA2_ADDR_CHECK_LEN (sizeof(hyundai_hda2_addr_checks) / sizeof(hyundai_hda2_addr_checks[0])) + +addr_checks hyundai_hda2_rx_checks = {hyundai_hda2_addr_checks, HYUNDAI_HDA2_ADDR_CHECK_LEN}; + +uint16_t hyundai_hda2_crc_lut[256]; + +static uint8_t hyundai_hda2_get_counter(CANPacket_t *to_push) { + return GET_BYTE(to_push, 2); +} + +static uint32_t hyundai_hda2_get_checksum(CANPacket_t *to_push) { + uint32_t chksum = GET_BYTE(to_push, 0) | (GET_BYTE(to_push, 1) << 8); + return chksum; +} + +static uint32_t hyundai_hda2_compute_checksum(CANPacket_t *to_push) { + int len = GET_LEN(to_push); + uint32_t address = GET_ADDR(to_push); + + uint16_t crc = 0; + + for (int i = 2; i < len; i++) { + crc = (crc << 8U) ^ hyundai_hda2_crc_lut[(crc >> 8U) ^ GET_BYTE(to_push, i)]; + } + + // Add address to crc + crc = (crc << 8U) ^ hyundai_hda2_crc_lut[(crc >> 8U) ^ ((address >> 0U) & 0xFFU)]; + crc = (crc << 8U) ^ hyundai_hda2_crc_lut[(crc >> 8U) ^ ((address >> 8U) & 0xFFU)]; + + if (len == 8) { + crc ^= 0x5f29U; + } else if (len == 16) { + crc ^= 0x041dU; + } else if (len == 24) { + crc ^= 0x819dU; + } else if (len == 32) { + crc ^= 0x9f5bU; + } else { + + } + + return crc; +} + +static int hyundai_hda2_rx_hook(CANPacket_t *to_push) { + + bool valid = addr_safety_check(to_push, &hyundai_hda2_rx_checks, + hyundai_hda2_get_checksum, hyundai_hda2_compute_checksum, hyundai_hda2_get_counter); + + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + if (valid && (bus == 1)) { + + if (addr == 0xea) { + int torque_driver_new = ((GET_BYTE(to_push, 11) & 0x1fU) << 8U) | GET_BYTE(to_push, 10); + torque_driver_new -= 4095; + update_sample(&torque_driver, torque_driver_new); + } + + if (addr == 0x175) { + bool cruise_engaged = GET_BIT(to_push, 68U); + + if (cruise_engaged && !cruise_engaged_prev) { + controls_allowed = 1; + } + + if (!cruise_engaged) { + controls_allowed = 0; + } + cruise_engaged_prev = cruise_engaged; + } + + if (addr == 0x35) { + gas_pressed = GET_BYTE(to_push, 5) != 0U; + } + + if (addr == 0x65) { + brake_pressed = GET_BIT(to_push, 57U) != 0U; + } + + if (addr == 0xa0) { + uint32_t speed = 0; + for (int i = 8; i < 15; i+=2) { + speed += GET_BYTE(to_push, i) | (GET_BYTE(to_push, i + 1) << 8U); + } + vehicle_moving = (speed / 4U) > HYUNDAI_HDA2_STANDSTILL_THRSLD; + } + } + + generic_rx_checks((addr == 0x50) && (bus == 0)); + + return valid; +} + +static int hyundai_hda2_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { + UNUSED(longitudinal_allowed); + + int tx = msg_allowed(to_send, HYUNDAI_HDA2_TX_MSGS, sizeof(HYUNDAI_HDA2_TX_MSGS)/sizeof(HYUNDAI_HDA2_TX_MSGS[0])); + int addr = GET_ADDR(to_send); + int bus = GET_BUS(to_send); + + // steering + if ((addr == 0x50) && (bus == 0)) { + int desired_torque = ((GET_BYTE(to_send, 6) & 0xFU) << 7U) | (GET_BYTE(to_send, 5) >> 1U); + desired_torque -= 1024; + uint32_t ts = microsecond_timer_get(); + bool violation = 0; + + if (controls_allowed) { + // *** global torque limit check *** + violation |= max_limit_check(desired_torque, HYUNDAI_HDA2_MAX_STEER, -HYUNDAI_HDA2_MAX_STEER); + + // *** torque rate limit check *** + violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, + HYUNDAI_HDA2_MAX_STEER, HYUNDAI_HDA2_MAX_RATE_UP, HYUNDAI_HDA2_MAX_RATE_DOWN, + HYUNDAI_HDA2_DRIVER_TORQUE_ALLOWANCE, HYUNDAI_HDA2_DRIVER_TORQUE_FACTOR); + + // used next time + desired_torque_last = desired_torque; + + // *** torque real time rate limit check *** + violation |= rt_rate_limit_check(desired_torque, rt_torque_last, HYUNDAI_MAX_RT_DELTA); + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); + if (ts_elapsed > HYUNDAI_RT_INTERVAL) { + rt_torque_last = desired_torque; + ts_last = ts; + } + } + + // no torque if controls is not allowed + if (!controls_allowed && (desired_torque != 0)) { + violation = 1; + } + + // reset to 0 if either controls is not allowed or there's a violation + if (violation || !controls_allowed) { + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = ts; + } + + if (violation) { + tx = 0; + } + } + + // cruise buttons check + if ((addr == 0x1cf) && (bus == 1)) { + bool is_cancel = GET_BYTE(to_send, 2) == 4U; + bool is_resume = GET_BYTE(to_send, 2) == 1U; + bool allowed = (is_cancel && cruise_engaged_prev) || (is_resume && controls_allowed); + if (!allowed) { + tx = 0; + } + } + + return tx; +} + +static int hyundai_hda2_fwd_hook(int bus_num, CANPacket_t *to_fwd) { + + int bus_fwd = -1; + int addr = GET_ADDR(to_fwd); + + if (bus_num == 0) { + bus_fwd = 2; + } + if ((bus_num == 2) && (addr != 0x50)) { + bus_fwd = 0; + } + + return bus_fwd; +} + +static const addr_checks* hyundai_hda2_init(uint16_t param) { + UNUSED(param); + gen_crc_lookup_table_16(0x1021, hyundai_hda2_crc_lut); + return &hyundai_hda2_rx_checks; +} + +const safety_hooks hyundai_hda2_hooks = { + .init = hyundai_hda2_init, + .rx = hyundai_hda2_rx_hook, + .tx = hyundai_hda2_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .fwd = hyundai_hda2_fwd_hook, +}; diff --git a/board/safety/safety_volkswagen_mqb.h b/board/safety/safety_volkswagen_mqb.h index f5e46c2bd..7e085f7ec 100644 --- a/board/safety/safety_volkswagen_mqb.h +++ b/board/safety/safety_volkswagen_mqb.h @@ -79,7 +79,7 @@ static uint32_t volkswagen_mqb_compute_crc(CANPacket_t *to_push) { static const addr_checks* volkswagen_mqb_init(uint16_t param) { UNUSED(param); - gen_crc_lookup_table(0x2F, volkswagen_crc8_lut_8h2f); + gen_crc_lookup_table_8(0x2F, volkswagen_crc8_lut_8h2f); return &volkswagen_mqb_rx_checks; } diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 969fec1bf..9c91b9d33 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -69,7 +69,8 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver, bool get_longitudinal_allowed(void); bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA); float interpolate(struct lookup_t xy, float x); -void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]); +void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]); +void gen_crc_lookup_table_16(uint16_t poly, uint16_t crc_lut[]); bool msg_allowed(CANPacket_t *to_send, const CanMsg msg_list[], int len); int get_addr_check_index(CANPacket_t *to_push, AddrCheckStruct addr_list[], const int len); void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter); diff --git a/python/__init__.py b/python/__init__.py index 01d692620..c20da46a3 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -149,6 +149,7 @@ class Panda: SAFETY_STELLANTIS = 25 SAFETY_FAW = 26 SAFETY_BODY = 27 + SAFETY_HYUNDAI_HDA2 = 28 SERIAL_DEBUG = 0 SERIAL_ESP = 1 diff --git a/tests/misra/test_misra.sh b/tests/misra/test_misra.sh index fa1a051dd..f0842132c 100755 --- a/tests/misra/test_misra.sh +++ b/tests/misra/test_misra.sh @@ -22,7 +22,7 @@ misra_f4_output=$( cat /tmp/misra/misra_f4_output.txt | grep -v ": information: printf "\nPANDA H7 CODE\n" -cppcheck -DPANDA -DSTM32H7 -UPEDAL -DUID_BASE \ +cppcheck -DPANDA -DSTM32H7 -DCANFD -UPEDAL -DUID_BASE \ --suppressions-list=suppressions.txt --suppress=*:*inc/* \ -I $PANDA_DIR/board/ --dump --enable=all --inline-suppr --force \ $PANDA_DIR/board/main.c 2>/tmp/misra/cppcheck_h7_output.txt diff --git a/tests/safety/common.py b/tests/safety/common.py index c12987c55..0883559c5 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -3,7 +3,9 @@ import abc import unittest import importlib import numpy as np +from collections import defaultdict from typing import Optional, List, Dict + from opendbc.can.packer import CANPacker # pylint: disable=import-error from panda import ALTERNATIVE_EXPERIENCE, LEN_TO_DLC from panda.tests.safety import libpandasafety_py @@ -25,8 +27,13 @@ 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, fix_checksum=None): - msg = self.make_can_msg(name_or_addr, bus, values, counter=-1) + _counters: Dict[str, int] = defaultdict(lambda: -1) + + def make_can_msg_panda(self, name_or_addr, bus, values, counter=False, fix_checksum=None): + if counter: + self._counters[name_or_addr] += 1 + + msg = self.make_can_msg(name_or_addr, bus, values, counter=self._counters[name_or_addr]) if fix_checksum is not None: msg = fix_checksum(msg) return package_can_msg(msg) diff --git a/tests/safety/test.c b/tests/safety/test.c index 01dfcc4dd..769af357f 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -7,6 +7,8 @@ #include "can_definitions.h" #include "utils.h" +#define CANFD + typedef struct { uint32_t CNT; } TIM_TypeDef; diff --git a/tests/safety/test_hyundai_hda2.py b/tests/safety/test_hyundai_hda2.py new file mode 100755 index 000000000..df6358c53 --- /dev/null +++ b/tests/safety/test_hyundai_hda2.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +import unittest +from panda import Panda +from panda.tests.safety import libpandasafety_py +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda + + +class TestHyundaiHDA2(common.PandaSafetyTest): + + TX_MSGS = [[0x50, 0], [0x1CF, 1]] + STANDSTILL_THRESHOLD = 30 # ~1kph + RELAY_MALFUNCTION_ADDR = 0x50 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [0x50]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + MAX_RATE_UP = 3 + MAX_RATE_DOWN = 7 + MAX_TORQUE = 150 + + MAX_RT_DELTA = 112 + RT_INTERVAL = 250000 + + DRIVER_TORQUE_ALLOWANCE = 50 + DRIVER_TORQUE_FACTOR = 2 + + def setUp(self): + self.packer = CANPackerPanda("kia_ev6") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI_HDA2, 0) + self.safety.init_tests() + + def _torque_driver_msg(self, torque): + values = {"STEERING_COL_TORQUE": torque} + return self.packer.make_can_msg_panda("MDPS", 1, values, counter=True) + + def _torque_msg(self, torque, steer_req=1): + values = {"TORQUE_REQUEST": torque} + return self.packer.make_can_msg_panda("LKAS", 0, values, counter=True) + + def _speed_msg(self, speed): + values = {f"WHEEL_SPEED_{i}": speed * 0.03125 for i in range(1, 5)} + return self.packer.make_can_msg_panda("WHEEL_SPEEDS", 1, values, counter=True) + + def _user_brake_msg(self, brake): + values = {"BRAKE_PRESSED": brake} + return self.packer.make_can_msg_panda("BRAKE", 1, values, counter=True) + + def _user_gas_msg(self, gas): + values = {"ACCELERATOR_PEDAL": gas} + return self.packer.make_can_msg_panda("ACCELERATOR", 1, values, counter=True) + + def _pcm_status_msg(self, enable): + values = {"CRUISE_ACTIVE": enable} + return self.packer.make_can_msg_panda("SCC1", 1, values, counter=True) + + def _button_msg(self, resume=False, cancel=False): + values = { + "DISTANCE_BTN": resume, + "PAUSE_RESUME_BTN": cancel, + } + return self.packer.make_can_msg_panda("CRUISE_BUTTONS", 1, values) + + def test_buttons(self): + for controls_allowed in (True, False): + for cruise_engaged in (True, False): + self._rx(self._pcm_status_msg(cruise_engaged)) + self.safety.set_controls_allowed(controls_allowed) + + self.assertEqual(cruise_engaged, self._tx(self._button_msg(cancel=True))) + self.assertEqual(controls_allowed, self._tx(self._button_msg(resume=True))) + + +if __name__ == "__main__": + unittest.main()