diff --git a/opendbc_repo/opendbc/safety/declarations.h b/opendbc_repo/opendbc/safety/declarations.h index 503eba5ac9..72b1d1279b 100644 --- a/opendbc_repo/opendbc/safety/declarations.h +++ b/opendbc_repo/opendbc/safety/declarations.h @@ -42,13 +42,16 @@ #define GET_BIT(msg, b) ((bool)!!(((msg)->data[((b) / 8U)] >> ((b) % 8U)) & 0x1U)) #define GET_FLAG(value, mask) (((value) & (mask)) == (mask)) -#define BUILD_SAFETY_CFG(rx, tx) ((safety_config){(rx), (sizeof((rx)) / sizeof((rx)[0])), \ +// length of an RX check array; fails to compile if it has more checks than rx_status can track +#define RX_CHECKS_LEN(rx) ((sizeof((rx)) / sizeof((rx)[0])) + \ + (0U * sizeof(char[((sizeof((rx)) / sizeof((rx)[0])) <= MAX_RX_CHECKS) ? 1 : -1]))) +#define BUILD_SAFETY_CFG(rx, tx) ((safety_config){(rx), RX_CHECKS_LEN(rx), \ (tx), (sizeof((tx)) / sizeof((tx)[0])), \ false}) #define SET_RX_CHECKS(rx, config) \ do { \ (config).rx_checks = (rx); \ - (config).rx_checks_len = sizeof((rx)) / sizeof((rx)[0]); \ + (config).rx_checks_len = RX_CHECKS_LEN(rx); \ (config).disable_forwarding = false; \ } while (0); @@ -67,6 +70,8 @@ uint64_t GET_BYTES_64_LE(const CANPacket_t *msg, int start, int len); extern const int MAX_WRONG_COUNTERS; #define MAX_ADDR_CHECK_MSGS 3U +// max number of RX checks in a safety config; their dynamic state is shared by all safety modes +#define MAX_RX_CHECKS 12U #define MAX_SAMPLE_VALS 6 // used to represent floating point vehicle speed in a sample_t #define VEHICLE_SPEED_FACTOR 1000.0 @@ -188,26 +193,28 @@ typedef struct { const bool ignore_quality_flag; // true if quality flag check is skipped } CanMsgCheck; +// dynamic state of an RX check, reset on safety mode init typedef struct { - // dynamic flags, reset on safety mode init - bool msg_seen; - int index; // if multiple messages are allowed to be checked, this stores the index of the first one seen. only msg[msg_index] will be used - bool valid_checksum; // true if and only if checksum check is passed - int wrong_counters; // counter of wrong counters, saturated between 0 and MAX_WRONG_COUNTERS - bool valid_quality_flag; // true if the message's quality/health/status signals are valid - uint8_t last_counter; // last counter value uint32_t last_timestamp; // micro-s + int wrong_counters; // counter of wrong counters, saturated between 0 and MAX_WRONG_COUNTERS + uint8_t index; // if multiple messages are allowed to be checked, this stores the index of the first one seen. only msg[msg_index] will be used + uint8_t last_counter; // last counter value + bool msg_seen; + bool valid_checksum; // true if and only if checksum check is passed + bool valid_quality_flag; // true if the message's quality/health/status signals are valid bool lagging; // true if and only if the time between updates is excessive } RxStatus; -// params and flags about checksum, counter and frequency checks for each monitored address +// params and flags about checksum, counter and frequency checks for each monitored address. +// constant: the state of the current safety config's checks is in rx_status, indexed alike typedef struct { - CanMsgCheck msg[MAX_ADDR_CHECK_MSGS]; // check either messages (e.g. honda steer) - RxStatus status; + const CanMsgCheck msg[MAX_ADDR_CHECK_MSGS]; // check either messages (e.g. honda steer) } RxCheck; +extern RxStatus rx_status[MAX_RX_CHECKS]; + typedef struct { - RxCheck *rx_checks; + const RxCheck *rx_checks; int rx_checks_len; const CanMsg *tx_msgs; int tx_msgs_len; diff --git a/opendbc_repo/opendbc/safety/safety.h b/opendbc_repo/opendbc/safety/safety.h index 84f949a940..f45e535dea 100644 --- a/opendbc_repo/opendbc/safety/safety.h +++ b/opendbc_repo/opendbc/safety/safety.h @@ -169,14 +169,16 @@ uint16_t current_safety_mode = SAFETY_SILENT; uint16_t current_safety_param = 0; static const safety_hooks *current_hooks = &nooutput_hooks; safety_config current_safety_config; +RxStatus rx_status[MAX_RX_CHECKS]; // dynamic state of current_safety_config.rx_checks static void generic_rx_checks(void); static void stock_ecu_check(bool stock_ecu_detected, bool immediate); -static bool is_msg_valid(RxCheck addr_list[], int index) { +static bool is_msg_valid(int index) { bool valid = true; if (index != -1) { - if (!addr_list[index].status.valid_checksum || !addr_list[index].status.valid_quality_flag || (addr_list[index].status.wrong_counters >= MAX_WRONG_COUNTERS)) { + const RxStatus *status = &rx_status[index]; + if (!status->valid_checksum || !status->valid_quality_flag || (status->wrong_counters >= MAX_WRONG_COUNTERS)) { valid = false; controls_allowed = false; aol_host_axis_mask = 0U; @@ -188,23 +190,24 @@ static bool is_msg_valid(RxCheck addr_list[], int index) { return valid; } -static int get_addr_check_index(const CANPacket_t *msg, RxCheck addr_list[], const int len) { +static int get_addr_check_index(const CANPacket_t *msg, const RxCheck addr_list[], const int len) { int index = -1; for (int i = 0; i < len; i++) { + RxStatus *status = &rx_status[i]; // if multiple msgs are allowed, determine which one is present on the bus - if (!addr_list[i].status.msg_seen) { + if (!status->msg_seen) { for (uint8_t j = 0U; (j < MAX_ADDR_CHECK_MSGS) && (addr_list[i].msg[j].addr != 0); j++) { if (msg_matches(msg, addr_list[i].msg[j].addr, addr_list[i].msg[j].bus, addr_list[i].msg[j].len)) { - addr_list[i].status.index = j; - addr_list[i].status.msg_seen = true; + status->index = j; + status->msg_seen = true; break; } } } - if (addr_list[i].status.msg_seen) { - int idx = addr_list[i].status.index; - if (msg_matches(msg, addr_list[i].msg[idx].addr, addr_list[i].msg[idx].bus, addr_list[i].msg[idx].len)) { + if (status->msg_seen) { + const CanMsgCheck *m = &addr_list[i].msg[status->index]; + if (msg_matches(msg, m->addr, m->bus, m->len)) { index = i; break; } @@ -213,20 +216,11 @@ static int get_addr_check_index(const CANPacket_t *msg, RxCheck addr_list[], con return index; } -static void update_addr_timestamp(RxCheck addr_list[], int index) { - if (index != -1) { - uint32_t ts = microsecond_timer_get(); - addr_list[index].status.last_timestamp = ts; - } -} - -static void update_counter(RxCheck addr_list[], int index, uint8_t counter) { - if (index != -1) { - uint8_t expected_counter = (addr_list[index].status.last_counter + 1U) % (addr_list[index].msg[addr_list[index].status.index].max_counter + 1U); - addr_list[index].status.wrong_counters += (expected_counter == counter) ? -1 : 1; - addr_list[index].status.wrong_counters = SAFETY_CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS); - addr_list[index].status.last_counter = counter; - } +static void update_counter(RxStatus *status, const CanMsgCheck *m, uint8_t counter) { + uint8_t expected_counter = (status->last_counter + 1U) % (m->max_counter + 1U); + status->wrong_counters += (expected_counter == counter) ? -1 : 1; + status->wrong_counters = SAFETY_CLAMP(status->wrong_counters, 0, MAX_WRONG_COUNTERS); + status->last_counter = counter; } static bool rx_msg_safety_check(const CANPacket_t *msg, @@ -234,34 +228,37 @@ static bool rx_msg_safety_check(const CANPacket_t *msg, const safety_hooks *safety_hooks) { int index = get_addr_check_index(msg, cfg->rx_checks, cfg->rx_checks_len); - update_addr_timestamp(cfg->rx_checks, index); if (index != -1) { + RxStatus *status = &rx_status[index]; + const CanMsgCheck *m = &cfg->rx_checks[index].msg[status->index]; + status->last_timestamp = microsecond_timer_get(); + // checksum check - if ((safety_hooks->get_checksum != NULL) && (safety_hooks->compute_checksum != NULL) && !cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_checksum) { + if ((safety_hooks->get_checksum != NULL) && (safety_hooks->compute_checksum != NULL) && !m->ignore_checksum) { uint32_t checksum = safety_hooks->get_checksum(msg); uint32_t checksum_comp = safety_hooks->compute_checksum(msg); - cfg->rx_checks[index].status.valid_checksum = checksum_comp == checksum; + status->valid_checksum = checksum_comp == checksum; } else { - cfg->rx_checks[index].status.valid_checksum = cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_checksum; + status->valid_checksum = m->ignore_checksum; } // counter check - if ((safety_hooks->get_counter != NULL) && (cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].max_counter > 0U)) { + if ((safety_hooks->get_counter != NULL) && (m->max_counter > 0U)) { uint8_t counter = safety_hooks->get_counter(msg); - update_counter(cfg->rx_checks, index, counter); + update_counter(status, m, counter); } else { - cfg->rx_checks[index].status.wrong_counters = cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_counter ? 0 : MAX_WRONG_COUNTERS; + status->wrong_counters = m->ignore_counter ? 0 : MAX_WRONG_COUNTERS; } // quality flag check - if ((safety_hooks->get_quality_flag_valid != NULL) && !cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_quality_flag) { - cfg->rx_checks[index].status.valid_quality_flag = safety_hooks->get_quality_flag_valid(msg); + if ((safety_hooks->get_quality_flag_valid != NULL) && !m->ignore_quality_flag) { + status->valid_quality_flag = safety_hooks->get_quality_flag_valid(msg); } else { - cfg->rx_checks[index].status.valid_quality_flag = cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_quality_flag; + status->valid_quality_flag = m->ignore_quality_flag; } } - return is_msg_valid(cfg->rx_checks, index); + return is_msg_valid(index); } bool safety_rx_hook(const CANPacket_t *msg) { @@ -366,18 +363,19 @@ void safety_tick(void) { bool rx_checks_invalid = false; uint32_t ts = microsecond_timer_get(); for (int i=0; i < current_safety_config.rx_checks_len; i++) { - uint32_t elapsed_time = safety_get_ts_elapsed(ts, current_safety_config.rx_checks[i].status.last_timestamp); + RxStatus *status = &rx_status[i]; + uint32_t elapsed_time = safety_get_ts_elapsed(ts, status->last_timestamp); // lag threshold is max of: 1s and MAX_MISSED_MSGS * expected timestep. // Quite conservative to not risk false triggers. // 2s of lag is worse case, since the function is called at 1Hz - uint32_t frequency = current_safety_config.rx_checks[i].msg[current_safety_config.rx_checks[i].status.index].frequency; + uint32_t frequency = current_safety_config.rx_checks[i].msg[status->index].frequency; uint32_t timestep = 1000000U / SAFETY_MAX(frequency, 1U); // frequency < 10Hz is invalid below bool lagging = elapsed_time > SAFETY_MAX(timestep * MAX_MISSED_MSGS, 1000000U); - current_safety_config.rx_checks[i].status.lagging = lagging; + status->lagging = lagging; // enforce minimum frequency for safety-relevant messages bool frequency_invalid = frequency < 10U; - if (lagging || frequency_invalid || !is_msg_valid(current_safety_config.rx_checks, i)) { + if (lagging || frequency_invalid || !is_msg_valid(i)) { rx_checks_invalid = true; controls_allowed = false; aol_host_axis_mask = 0U; @@ -551,7 +549,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { current_safety_config = current_hooks->init(param); // reset all dynamic fields in addr struct for (int j = 0; j < current_safety_config.rx_checks_len; j++) { - current_safety_config.rx_checks[j].status = (RxStatus){0}; + rx_status[j] = (RxStatus){0}; } } return set_status; diff --git a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c b/opendbc_repo/opendbc/safety/tests/libsafety/safety.c index 3e0044d9e8..9e29c5a920 100644 --- a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c +++ b/opendbc_repo/opendbc/safety/tests/libsafety/safety.c @@ -57,10 +57,10 @@ bool safety_config_valid() { } for (int i = 0; i < current_safety_config.rx_checks_len; i++) { - const RxCheck addr = current_safety_config.rx_checks[i]; - bool valid = addr.status.msg_seen && !addr.status.lagging && addr.status.valid_checksum && (addr.status.wrong_counters < MAX_WRONG_COUNTERS) && addr.status.valid_quality_flag; + const RxStatus status = rx_status[i]; + bool valid = status.msg_seen && !status.lagging && status.valid_checksum && (status.wrong_counters < MAX_WRONG_COUNTERS) && status.valid_quality_flag; if (!valid) { - // printf("i %d seen %d lagging %d valid checksum %d wrong counters %d valid quality flag %d\n", i, addr.status.msg_seen, addr.status.lagging, addr.status.valid_checksum, addr.status.wrong_counters, addr.status.valid_quality_flag); + // printf("i %d seen %d lagging %d valid checksum %d wrong counters %d valid quality flag %d\n", i, status.msg_seen, status.lagging, status.valid_checksum, status.wrong_counters, status.valid_quality_flag); return false; } }