diff --git a/board/boards/black.h b/board/boards/black.h index 4b574f6698..abead6c73a 100644 --- a/board/boards/black.h +++ b/board/boards/black.h @@ -22,7 +22,7 @@ void black_enable_can_transceiver(uint8_t transceiver, bool enabled) { } } -void black_enable_can_transceivers(bool enabled) { +static void black_enable_can_transceivers(bool enabled) { for(uint8_t i=1U; i<=4U; i++){ // Leave main CAN always on for CAN-based ignition detection if((harness.status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ @@ -49,11 +49,11 @@ void black_set_led(uint8_t color, bool enabled) { } } -void black_set_usb_load_switch(bool enabled) { +static void black_set_usb_load_switch(bool enabled) { set_gpio_output(GPIOB, 1, !enabled); } -void black_set_can_mode(uint8_t mode) { +static void black_set_can_mode(uint8_t mode) { black_enable_can_transceiver(2U, false); black_enable_can_transceiver(4U, false); switch (mode) { @@ -148,7 +148,7 @@ void black_init_bootloader(void) { set_gpio_output(GPIOC, 12, 0); } -const harness_configuration black_harness_config = { +static const harness_configuration black_harness_config = { .has_harness = true, .GPIO_SBU1 = GPIOC, .GPIO_SBU2 = GPIOC, diff --git a/board/boards/dos.h b/board/boards/dos.h index 1a7bc878e5..2b6b1bfb39 100644 --- a/board/boards/dos.h +++ b/board/boards/dos.h @@ -22,7 +22,7 @@ void dos_enable_can_transceiver(uint8_t transceiver, bool enabled) { } } -void dos_enable_can_transceivers(bool enabled) { +static void dos_enable_can_transceivers(bool enabled) { for(uint8_t i=1U; i<=4U; i++){ // Leave main CAN always on for CAN-based ignition detection if((harness.status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ @@ -49,11 +49,11 @@ void dos_set_led(uint8_t color, bool enabled) { } } -void dos_set_bootkick(BootState state) { +static void dos_set_bootkick(BootState state) { set_gpio_output(GPIOC, 4, state != BOOT_BOOTKICK); } -void dos_set_can_mode(uint8_t mode) { +static void dos_set_can_mode(uint8_t mode) { dos_enable_can_transceiver(2U, false); dos_enable_can_transceiver(4U, false); switch (mode) { @@ -94,7 +94,7 @@ void dos_set_usb_switch(bool phone){ set_gpio_output(GPIOB, 3, phone); } -void dos_set_ir_power(uint8_t percentage){ +static void dos_set_ir_power(uint8_t percentage){ pwm_set(TIM4, 2, percentage); } @@ -177,7 +177,7 @@ void dos_init(void) { clock_source_init(); } -const harness_configuration dos_harness_config = { +static const harness_configuration dos_harness_config = { .has_harness = true, .GPIO_SBU1 = GPIOC, .GPIO_SBU2 = GPIOC, diff --git a/board/boards/pedal.h b/board/boards/pedal.h index bdc4de1568..aea8263efe 100644 --- a/board/boards/pedal.h +++ b/board/boards/pedal.h @@ -2,7 +2,7 @@ // Pedal // // ///// // -void pedal_enable_can_transceiver(uint8_t transceiver, bool enabled) { +static void pedal_enable_can_transceiver(uint8_t transceiver, bool enabled) { switch (transceiver){ case 1: set_gpio_output(GPIOB, 3, !enabled); @@ -13,7 +13,7 @@ void pedal_enable_can_transceiver(uint8_t transceiver, bool enabled) { } } -void pedal_enable_can_transceivers(bool enabled) { +static void pedal_enable_can_transceivers(bool enabled) { pedal_enable_can_transceiver(1U, enabled); } @@ -62,7 +62,7 @@ void pedal_init(void) { pedal_set_led(LED_GREEN, false); } -const harness_configuration pedal_harness_config = { +static const harness_configuration pedal_harness_config = { .has_harness = false }; diff --git a/board/boards/red.h b/board/boards/red.h index c618c48ad4..806cdc26e4 100644 --- a/board/boards/red.h +++ b/board/boards/red.h @@ -21,7 +21,7 @@ void red_enable_can_transceiver(uint8_t transceiver, bool enabled) { } } -void red_enable_can_transceivers(bool enabled) { +static void red_enable_can_transceivers(bool enabled) { uint8_t main_bus = (harness.status == HARNESS_STATUS_FLIPPED) ? 3U : 1U; for (uint8_t i=1U; i<=4U; i++) { // Leave main CAN always on for CAN-based ignition detection @@ -49,7 +49,7 @@ void red_set_led(uint8_t color, bool enabled) { } } -void red_set_can_mode(uint8_t mode) { +static void red_set_can_mode(uint8_t mode) { red_enable_can_transceiver(2U, false); red_enable_can_transceiver(4U, false); switch (mode) { @@ -156,7 +156,7 @@ void red_init(void) { } } -const harness_configuration red_harness_config = { +static const harness_configuration red_harness_config = { .has_harness = true, .GPIO_SBU1 = GPIOC, .GPIO_SBU2 = GPIOA, diff --git a/board/boards/red_chiplet.h b/board/boards/red_chiplet.h index 1301cd3e54..b2878ad48c 100644 --- a/board/boards/red_chiplet.h +++ b/board/boards/red_chiplet.h @@ -23,7 +23,7 @@ void red_chiplet_enable_can_transceiver(uint8_t transceiver, bool enabled) { } } -void red_chiplet_enable_can_transceivers(bool enabled) { +static void red_chiplet_enable_can_transceivers(bool enabled) { uint8_t main_bus = (harness.status == HARNESS_STATUS_FLIPPED) ? 3U : 1U; for (uint8_t i=1U; i<=4U; i++) { // Leave main CAN always on for CAN-based ignition detection @@ -35,7 +35,7 @@ void red_chiplet_enable_can_transceivers(bool enabled) { } } -void red_chiplet_set_can_mode(uint8_t mode) { +static void red_chiplet_set_can_mode(uint8_t mode) { red_chiplet_enable_can_transceiver(2U, false); red_chiplet_enable_can_transceiver(4U, false); switch (mode) { diff --git a/board/boards/tres.h b/board/boards/tres.h index 599516e232..fbf423d16f 100644 --- a/board/boards/tres.h +++ b/board/boards/tres.h @@ -8,13 +8,13 @@ void tres_update_fan_ir_power(void) { red_chiplet_set_fan_or_usb_load_switch(tres_ir_enabled || tres_fan_enabled); } -void tres_set_ir_power(uint8_t percentage){ +static void tres_set_ir_power(uint8_t percentage){ tres_ir_enabled = (percentage > 0U); tres_update_fan_ir_power(); pwm_set(TIM3, 4, percentage); } -void tres_set_bootkick(BootState state) { +static void tres_set_bootkick(BootState state) { set_gpio_output(GPIOA, 0, state != BOOT_BOOTKICK); set_gpio_output(GPIOC, 12, state != BOOT_RESET); } diff --git a/board/boards/uno.h b/board/boards/uno.h index d03782cc06..8b2f713055 100644 --- a/board/boards/uno.h +++ b/board/boards/uno.h @@ -22,7 +22,7 @@ void uno_enable_can_transceiver(uint8_t transceiver, bool enabled) { } } -void uno_enable_can_transceivers(bool enabled) { +static void uno_enable_can_transceivers(bool enabled) { for(uint8_t i=1U; i<=4U; i++){ // Leave main CAN always on for CAN-based ignition detection if((harness.status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ @@ -49,7 +49,7 @@ void uno_set_led(uint8_t color, bool enabled) { } } -void uno_set_bootkick(BootState state) { +static void uno_set_bootkick(BootState state) { if (state == BOOT_BOOTKICK) { set_gpio_output(GPIOB, 14, false); } else { @@ -58,7 +58,7 @@ void uno_set_bootkick(BootState state) { } } -void uno_set_can_mode(uint8_t mode) { +static void uno_set_can_mode(uint8_t mode) { uno_enable_can_transceiver(2U, false); uno_enable_can_transceiver(4U, false); switch (mode) { @@ -99,7 +99,7 @@ void uno_set_usb_switch(bool phone){ set_gpio_output(GPIOB, 3, phone); } -void uno_set_ir_power(uint8_t percentage){ +static void uno_set_ir_power(uint8_t percentage){ pwm_set(TIM4, 2, percentage); } @@ -184,7 +184,7 @@ void uno_init_bootloader(void) { set_gpio_output(GPIOC, 12, 0); } -const harness_configuration uno_harness_config = { +static const harness_configuration uno_harness_config = { .has_harness = true, .GPIO_SBU1 = GPIOC, .GPIO_SBU2 = GPIOC, diff --git a/board/boards/white.h b/board/boards/white.h index 39bad3e760..9f7c59a2c6 100644 --- a/board/boards/white.h +++ b/board/boards/white.h @@ -2,7 +2,7 @@ // White Panda // // /////////// // -void white_enable_can_transceiver(uint8_t transceiver, bool enabled) { +static void white_enable_can_transceiver(uint8_t transceiver, bool enabled) { switch (transceiver){ case 1U: set_gpio_output(GPIOC, 1, !enabled); @@ -19,7 +19,7 @@ void white_enable_can_transceiver(uint8_t transceiver, bool enabled) { } } -void white_enable_can_transceivers(bool enabled) { +static void white_enable_can_transceivers(bool enabled) { uint8_t t1 = enabled ? 1U : 2U; // leave transceiver 1 enabled to detect CAN ignition for(uint8_t i=t1; i<=3U; i++) { white_enable_can_transceiver(i, enabled); @@ -65,7 +65,7 @@ void white_set_usb_power_mode(uint8_t mode){ } } -void white_set_can_mode(uint8_t mode){ +static void white_set_can_mode(uint8_t mode){ switch (mode) { case CAN_MODE_NORMAL: // B12,B13: disable GMLAN mode diff --git a/board/drivers/bxcan.h b/board/drivers/bxcan.h index 080c9f4a9d..32caacd773 100644 --- a/board/drivers/bxcan.h +++ b/board/drivers/bxcan.h @@ -9,7 +9,7 @@ uint8_t can_irq_number[3][3] = { { CAN3_TX_IRQn, CAN3_RX0_IRQn, CAN3_SCE_IRQn }, }; -bool can_set_speed(uint8_t can_number) { +static bool can_set_speed(uint8_t can_number) { bool ret = true; CAN_TypeDef *CANx = CANIF_FROM_CAN_NUM(can_number); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); @@ -227,17 +227,17 @@ void can_rx(uint8_t can_number) { } } -void CAN1_TX_IRQ_Handler(void) { process_can(0); } -void CAN1_RX0_IRQ_Handler(void) { can_rx(0); } -void CAN1_SCE_IRQ_Handler(void) { can_sce(0); } +static void CAN1_TX_IRQ_Handler(void) { process_can(0); } +static void CAN1_RX0_IRQ_Handler(void) { can_rx(0); } +static void CAN1_SCE_IRQ_Handler(void) { can_sce(0); } -void CAN2_TX_IRQ_Handler(void) { process_can(1); } -void CAN2_RX0_IRQ_Handler(void) { can_rx(1); } -void CAN2_SCE_IRQ_Handler(void) { can_sce(1); } +static void CAN2_TX_IRQ_Handler(void) { process_can(1); } +static void CAN2_RX0_IRQ_Handler(void) { can_rx(1); } +static void CAN2_SCE_IRQ_Handler(void) { can_sce(1); } -void CAN3_TX_IRQ_Handler(void) { process_can(2); } -void CAN3_RX0_IRQ_Handler(void) { can_rx(2); } -void CAN3_SCE_IRQ_Handler(void) { can_sce(2); } +static void CAN3_TX_IRQ_Handler(void) { process_can(2); } +static void CAN3_RX0_IRQ_Handler(void) { can_rx(2); } +static void CAN3_SCE_IRQ_Handler(void) { can_sce(2); } bool can_init(uint8_t can_number) { bool ret = false; diff --git a/board/drivers/can_common.h b/board/drivers/can_common.h index 574c7cfd56..adc8e4881a 100644 --- a/board/drivers/can_common.h +++ b/board/drivers/can_common.h @@ -58,16 +58,16 @@ void process_can(uint8_t can_number); #ifdef STM32H7 // ITCM RAM and DTCM RAM are the fastest for Cortex-M7 core access -__attribute__((section(".axisram"))) can_buffer(rx_q, CAN_RX_BUFFER_SIZE) -__attribute__((section(".itcmram"))) can_buffer(tx1_q, CAN_TX_BUFFER_SIZE) -__attribute__((section(".itcmram"))) can_buffer(tx2_q, CAN_TX_BUFFER_SIZE) +__attribute__((section(".axisram"))) static can_buffer(rx_q, CAN_RX_BUFFER_SIZE) +__attribute__((section(".itcmram"))) static can_buffer(tx1_q, CAN_TX_BUFFER_SIZE) +__attribute__((section(".itcmram"))) static can_buffer(tx2_q, CAN_TX_BUFFER_SIZE) #else -can_buffer(rx_q, CAN_RX_BUFFER_SIZE) -can_buffer(tx1_q, CAN_TX_BUFFER_SIZE) -can_buffer(tx2_q, CAN_TX_BUFFER_SIZE) +static can_buffer(rx_q, CAN_RX_BUFFER_SIZE) +static can_buffer(tx1_q, CAN_TX_BUFFER_SIZE) +static can_buffer(tx2_q, CAN_TX_BUFFER_SIZE) #endif -can_buffer(tx3_q, CAN_TX_BUFFER_SIZE) -can_buffer(txgmlan_q, GMLAN_TX_BUFFER_SIZE) +static can_buffer(tx3_q, CAN_TX_BUFFER_SIZE) +static can_buffer(txgmlan_q, GMLAN_TX_BUFFER_SIZE) // FIXME: // cppcheck-suppress misra-c2012-9.3 can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q}; diff --git a/board/drivers/fan.h b/board/drivers/fan.h index 15296079ae..6c265f2ea3 100644 --- a/board/drivers/fan.h +++ b/board/drivers/fan.h @@ -11,8 +11,6 @@ struct fan_state_t { } fan_state_t; struct fan_state_t fan_state; -const float FAN_I = 0.001f; - const uint8_t FAN_TICK_FREQ = 8U; const uint8_t FAN_STALL_THRESHOLD_MIN = 3U; const uint8_t FAN_STALL_THRESHOLD_MAX = 8U; @@ -22,7 +20,7 @@ void fan_set_power(uint8_t percentage) { fan_state.target_rpm = ((current_board->fan_max_rpm * CLAMP(percentage, 0U, 100U)) / 100U); } -void llfan_init(void); +static void llfan_init(void); void fan_init(void) { fan_state.stall_threshold = FAN_STALL_THRESHOLD_MIN; fan_state.cooldown_counter = current_board->fan_enable_cooldown_time * FAN_TICK_FREQ; @@ -31,6 +29,8 @@ void fan_init(void) { // Call this at FAN_TICK_FREQ void fan_tick(void) { + const float FAN_I = 0.001f; + if (current_board->fan_max_rpm > 0U) { // Measure fan RPM uint16_t fan_rpm_fast = fan_state.tach_counter * (60U * FAN_TICK_FREQ / 4U); // 4 interrupts per rotation diff --git a/board/drivers/fdcan.h b/board/drivers/fdcan.h index 4343d5c97f..89af165a58 100644 --- a/board/drivers/fdcan.h +++ b/board/drivers/fdcan.h @@ -19,7 +19,7 @@ uint8_t can_irq_number[3][2] = { #define CAN_ACK_ERROR 3U -bool can_set_speed(uint8_t can_number) { +static bool can_set_speed(uint8_t can_number) { bool ret = true; FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); @@ -242,14 +242,14 @@ void can_rx(uint8_t can_number) { } } -void FDCAN1_IT0_IRQ_Handler(void) { can_rx(0); } -void FDCAN1_IT1_IRQ_Handler(void) { process_can(0); } +static void FDCAN1_IT0_IRQ_Handler(void) { can_rx(0); } +static void FDCAN1_IT1_IRQ_Handler(void) { process_can(0); } -void FDCAN2_IT0_IRQ_Handler(void) { can_rx(1); } -void FDCAN2_IT1_IRQ_Handler(void) { process_can(1); } +static void FDCAN2_IT0_IRQ_Handler(void) { can_rx(1); } +static void FDCAN2_IT1_IRQ_Handler(void) { process_can(1); } -void FDCAN3_IT0_IRQ_Handler(void) { can_rx(2); } -void FDCAN3_IT1_IRQ_Handler(void) { process_can(2); } +static void FDCAN3_IT0_IRQ_Handler(void) { can_rx(2); } +static void FDCAN3_IT1_IRQ_Handler(void) { process_can(2); } bool can_init(uint8_t can_number) { bool ret = false; diff --git a/board/drivers/gmlan_alt.h b/board/drivers/gmlan_alt.h index 407062bbbd..afc88d0a1e 100644 --- a/board/drivers/gmlan_alt.h +++ b/board/drivers/gmlan_alt.h @@ -12,7 +12,7 @@ int gmlan_alt_mode = DISABLED; // returns out_len -int do_bitstuff(char *out, const char *in, int in_len) { +static int do_bitstuff(char *out, const char *in, int in_len) { int last_bit = -1; int bit_cnt = 0; int j = 0; @@ -40,7 +40,7 @@ int do_bitstuff(char *out, const char *in, int in_len) { return j; } -int append_crc(char *in, int in_len) { +static int append_crc(char *in, int in_len) { unsigned int crc = 0; for (int i = 0; i < in_len; i++) { crc <<= 1; @@ -57,7 +57,7 @@ int append_crc(char *in, int in_len) { return in_len_copy; } -int append_bits(char *in, int in_len, const char *app, int app_len) { +static int append_bits(char *in, int in_len, const char *app, int app_len) { int in_len_copy = in_len; for (int i = 0; i < app_len; i++) { in[in_len_copy] = app[i]; @@ -75,7 +75,7 @@ int append_int(char *in, int in_len, int val, int val_len) { return in_len_copy; } -int get_bit_message(char *out, const CANPacket_t *to_bang) { +static int get_bit_message(char *out, const CANPacket_t *to_bang) { char pkt[MAX_BITS_CAN_PACKET]; char footer[] = { 1, // CRC delimiter @@ -121,9 +121,9 @@ int get_bit_message(char *out, const CANPacket_t *to_bang) { return len; } -void TIM12_IRQ_Handler(void); +static void TIM12_IRQ_Handler(void); -void setup_timer(void) { +static void setup_timer(void) { // register interrupt REGISTER_INTERRUPT(TIM8_BRK_TIM12_IRQn, TIM12_IRQ_Handler, 40000U, FAULT_INTERRUPT_RATE_GMLAN) @@ -191,7 +191,7 @@ int gmlan_fail_count = 0; #define REQUIRED_SILENT_TIME 10 #define MAX_FAIL_COUNT 10 -void TIM12_IRQ_Handler(void) { +static void TIM12_IRQ_Handler(void) { if (gmlan_alt_mode == BITBANG) { if ((TIM12->SR & TIM_SR_UIF) && (gmlan_sendmax != -1)) { int read = get_gpio_input(GPIOB, 12); @@ -290,6 +290,7 @@ bool bitbang_gmlan(const CANPacket_t *to_bang) { } #else UNUSED(to_bang); + UNUSED(get_bit_message); #endif return gmlan_send_ok; diff --git a/board/drivers/harness.h b/board/drivers/harness.h index 2b6b5e6d8f..dbd2a70726 100644 --- a/board/drivers/harness.h +++ b/board/drivers/harness.h @@ -26,7 +26,7 @@ struct harness_configuration { }; // The ignition relay is only used for testing purposes -void set_intercept_relay(bool intercept, bool ignition_relay) { +static void set_intercept_relay(bool intercept, bool ignition_relay) { if (current_board->harness_config->has_harness) { bool drive_relay = intercept; if (harness.status == HARNESS_STATUS_NC) { diff --git a/board/drivers/interrupts.h b/board/drivers/interrupts.h index 9cb46d4b29..5b585bffef 100644 --- a/board/drivers/interrupts.h +++ b/board/drivers/interrupts.h @@ -10,7 +10,7 @@ typedef struct interrupt { void interrupt_timer_init(void); uint32_t microsecond_timer_get(void); -void unused_interrupt_handler(void) { +static void unused_interrupt_handler(void) { // Something is wrong if this handler is called! print("Unused interrupt handler called!\n"); fault_occurred(FAULT_UNUSED_INTERRUPT_HANDLED); @@ -80,6 +80,8 @@ void interrupt_timer_handler(void) { // The bootstub does not have the FPU enabled, so can't do float operations. #if !defined(PEDAL) && !defined(BOOTSTUB) interrupt_load = ((busy_time + idle_time) > 0U) ? ((float) busy_time) / (busy_time + idle_time) : 0.0f; +#else + interrupt_load = 0.0f; #endif idle_time = 0U; busy_time = 0U; diff --git a/board/drivers/spi.h b/board/drivers/spi.h index 1a7b9be590..a0e040f0f5 100644 --- a/board/drivers/spi.h +++ b/board/drivers/spi.h @@ -35,7 +35,6 @@ enum { SPI_STATE_DATA_TX }; -bool spi_tx_dma_done = false; uint8_t spi_state = SPI_STATE_HEADER; uint8_t spi_endpoint; uint16_t spi_data_len_mosi; @@ -56,7 +55,7 @@ void can_tx_comms_resume_spi(void) { spi_can_tx_ready = true; } -uint16_t spi_version_packet(uint8_t *out) { +static uint16_t spi_version_packet(uint8_t *out) { // this protocol version request is a stable portion of // the panda's SPI protocol. its contents match that of the // panda USB descriptors and are sufficent to list/enumerate diff --git a/board/drivers/timers.h b/board/drivers/timers.h index d7f3f01fd0..49d9f5c4d1 100644 --- a/board/drivers/timers.h +++ b/board/drivers/timers.h @@ -1,4 +1,4 @@ -void timer_init(TIM_TypeDef *TIM, int psc) { +static void timer_init(TIM_TypeDef *TIM, int psc) { register_set(&(TIM->PSC), (psc-1), 0xFFFFU); register_set(&(TIM->DIER), TIM_DIER_UIE, 0x5F5FU); register_set(&(TIM->CR1), TIM_CR1_CEN, 0x3FU); diff --git a/board/drivers/uart.h b/board/drivers/uart.h index e37dfcd6bd..7d52a94ecb 100644 --- a/board/drivers/uart.h +++ b/board/drivers/uart.h @@ -18,8 +18,8 @@ typedef struct uart_ring { } uart_ring; #define UART_BUFFER(x, size_rx, size_tx, uart_ptr, callback_ptr, overwrite_mode) \ - uint8_t elems_rx_##x[size_rx]; \ - uint8_t elems_tx_##x[size_tx]; \ + static uint8_t elems_rx_##x[size_rx]; \ + static uint8_t elems_tx_##x[size_tx]; \ uart_ring uart_ring_##x = { \ .w_ptr_tx = 0, \ .r_ptr_tx = 0, \ @@ -83,7 +83,7 @@ bool get_char(uart_ring *q, char *elem) { return ret; } -bool injectc(uart_ring *q, char elem) { +static bool injectc(uart_ring *q, char elem) { int ret = false; uint16_t next_w_ptr; diff --git a/board/drivers/usb.h b/board/drivers/usb.h index 99ba126cec..8f18b8757c 100644 --- a/board/drivers/usb.h +++ b/board/drivers/usb.h @@ -423,7 +423,7 @@ void USB_WritePacket_EP0(uint8_t *src, uint16_t len) { } } -void usb_reset(void) { +static void usb_reset(void) { // unmask endpoint interrupts, so many sets USBx_DEVICE->DAINT = 0xFFFFFFFFU; USBx_DEVICE->DAINTMSK = 0xFFFFFFFFU; @@ -482,7 +482,7 @@ void usb_tick(void) { usb_last_frame_num = current_frame_num; } -void usb_setup(void) { +static void usb_setup(void) { int resp_len; ControlPacket_t control_req; diff --git a/board/drivers/watchdog.h b/board/drivers/watchdog.h index d0ee32cb2d..d0f2f5eadf 100644 --- a/board/drivers/watchdog.h +++ b/board/drivers/watchdog.h @@ -9,7 +9,7 @@ typedef enum { WATCHDOG_500_MS = 4000U, } WatchdogTimeout; -void watchdog_feed(void) { +static void watchdog_feed(void) { IND_WDG->KR = 0xAAAAU; } diff --git a/board/early_init.h b/board/early_init.h index ae652aebce..80ce2f9218 100644 --- a/board/early_init.h +++ b/board/early_init.h @@ -6,7 +6,7 @@ extern void *g_pfnVectors; extern uint32_t enter_bootloader_mode; -void jump_to_bootloader(void) { +static void jump_to_bootloader(void) { // do enter bootloader enter_bootloader_mode = 0; void (*bootloader)(void) = (void (*)(void)) (*((uint32_t *)BOOTLOADER_ADDRESS)); diff --git a/board/main.c b/board/main.c index 2361f10255..6e341817da 100644 --- a/board/main.c +++ b/board/main.c @@ -134,7 +134,7 @@ void __initialize_hardware_early(void) { early_initialization(); } -void __attribute__ ((noinline)) enable_fpu(void) { +static void __attribute__ ((noinline)) enable_fpu(void) { // enable the FPU SCB->CPACR |= ((3UL << (10U * 2U)) | (3UL << (11U * 2U))); } @@ -145,7 +145,7 @@ void __attribute__ ((noinline)) enable_fpu(void) { // called at 8Hz uint8_t loop_counter = 0U; -void tick_handler(void) { +static void tick_handler(void) { if (TICK_TIMER->SR != 0) { // siren current_board->set_siren((loop_counter & 1U) && (siren_enabled || (siren_countdown > 0U))); @@ -295,7 +295,7 @@ void EXTI_IRQ_Handler(void) { } uint8_t rtc_counter = 0; -void RTC_WKUP_IRQ_Handler(void) { +static void RTC_WKUP_IRQ_Handler(void) { exti_irq_clear(); clock_init(); diff --git a/board/main_comms.h b/board/main_comms.h index 4adfc8d744..a730ba0253 100644 --- a/board/main_comms.h +++ b/board/main_comms.h @@ -4,7 +4,7 @@ extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used void set_safety_mode(uint16_t mode, uint16_t param); bool is_car_safety_mode(uint16_t mode); -int get_health_pkt(void *dat) { +static int get_health_pkt(void *dat) { COMPILE_TIME_ASSERT(sizeof(struct health_t) <= USBPACKET_MAX_SIZE); struct health_t * health = (struct health_t*)dat; @@ -48,7 +48,7 @@ int get_health_pkt(void *dat) { return sizeof(*health); } -int get_rtc_pkt(void *dat) { +static int get_rtc_pkt(void *dat) { timestamp_t t = rtc_get_time(); (void)memcpy(dat, &t, sizeof(t)); return sizeof(t); diff --git a/board/pedal/main.c b/board/pedal/main.c index 704ee54365..88ada15350 100644 --- a/board/pedal/main.c +++ b/board/pedal/main.c @@ -96,7 +96,7 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { #define CAN_GAS_SIZE 6 #define COUNTER_CYCLE 0xFU -void CAN1_TX_IRQ_Handler(void) { +static void CAN1_TX_IRQ_Handler(void) { // clear interrupt CAN->TSR |= CAN_TSR_RQCP0; } @@ -119,7 +119,7 @@ uint32_t current_index = 0; uint8_t state = FAULT_STARTUP; const uint8_t crc_poly = 0xD5U; // standard crc8 -void CAN1_RX0_IRQ_Handler(void) { +static void CAN1_RX0_IRQ_Handler(void) { while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) { #ifdef DEBUG print("CAN RX\n"); @@ -182,7 +182,7 @@ void CAN1_RX0_IRQ_Handler(void) { } } -void CAN1_SCE_IRQ_Handler(void) { +static void CAN1_SCE_IRQ_Handler(void) { state = FAULT_SCE; llcan_clear_send(CAN); } @@ -193,7 +193,7 @@ unsigned int pkt_idx = 0; int led_value = 0; -void TIM3_IRQ_Handler(void) { +static void TIM3_IRQ_Handler(void) { #ifdef DEBUG puth(TIM3->CNT); print(" "); @@ -242,7 +242,7 @@ void TIM3_IRQ_Handler(void) { // ***************************** main code ***************************** -void pedal(void) { +static void pedal(void) { // read/write pdl0 = adc_get_raw(ADCCHAN_ACCEL0); pdl1 = adc_get_raw(ADCCHAN_ACCEL1); diff --git a/board/safety.h b/board/safety.h index e48a75502e..a825f6f75a 100644 --- a/board/safety.h +++ b/board/safety.h @@ -121,7 +121,7 @@ void gen_crc_lookup_table_16(uint16_t poly, uint16_t crc_lut[]) { } } -bool msg_allowed(const CANPacket_t *to_send, const CanMsg msg_list[], int len) { +static bool msg_allowed(const CANPacket_t *to_send, const CanMsg msg_list[], int len) { int addr = GET_ADDR(to_send); int bus = GET_BUS(to_send); int length = GET_LEN(to_send); @@ -136,7 +136,7 @@ bool msg_allowed(const CANPacket_t *to_send, const CanMsg msg_list[], int len) { return allowed; } -int get_addr_check_index(const CANPacket_t *to_push, RxCheck addr_list[], const int len) { +static int get_addr_check_index(const CANPacket_t *to_push, RxCheck addr_list[], const int len) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); int length = GET_LEN(to_push); @@ -193,7 +193,7 @@ void safety_tick(const safety_config *cfg) { safety_rx_checks_invalid = rx_checks_invalid; } -void update_counter(RxCheck addr_list[], int index, uint8_t counter) { +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; @@ -213,14 +213,14 @@ bool is_msg_valid(RxCheck addr_list[], int index) { return valid; } -void update_addr_timestamp(RxCheck addr_list[], int 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; } } -bool rx_msg_safety_check(const CANPacket_t *to_push, +static bool rx_msg_safety_check(const CANPacket_t *to_push, const safety_config *cfg, const safety_hooks *safety_hooks) { @@ -280,12 +280,12 @@ void generic_rx_checks(bool stock_ecu_detected) { } } -void relay_malfunction_set(void) { +static void relay_malfunction_set(void) { relay_malfunction = true; fault_occurred(FAULT_RELAY_MALFUNCTION); } -void relay_malfunction_reset(void) { +static void relay_malfunction_reset(void) { relay_malfunction = false; fault_recovered(FAULT_RELAY_MALFUNCTION); } @@ -429,7 +429,7 @@ bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) { } // check that commanded torque value isn't too far from measured -bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, +static bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) { // *** val rate limit check *** @@ -445,7 +445,7 @@ bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, } // check that commanded value isn't fighting against driver -bool driver_limit_check(int val, int val_last, const struct sample_t *val_driver, +static bool driver_limit_check(int val, int val_last, const struct sample_t *val_driver, const int MAX_VAL, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ALLOWANCE, const int DRIVER_FACTOR) { @@ -469,7 +469,7 @@ bool driver_limit_check(int val, int val_last, const struct sample_t *val_driver // real time check, mainly used for steer torque rate limiter -bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) { +static bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) { // *** torque real time rate limit check *** int highest_val = MAX(val_last, 0) + MAX_RT_DELTA; diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 71bd19d300..8a0643fec5 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -1,33 +1,3 @@ -const SteeringLimits CHRYSLER_STEERING_LIMITS = { - .max_steer = 261, - .max_rt_delta = 112, - .max_rt_interval = 250000, - .max_rate_up = 3, - .max_rate_down = 3, - .max_torque_error = 80, - .type = TorqueMotorLimited, -}; - -const SteeringLimits CHRYSLER_RAM_DT_STEERING_LIMITS = { - .max_steer = 350, - .max_rt_delta = 112, - .max_rt_interval = 250000, - .max_rate_up = 6, - .max_rate_down = 6, - .max_torque_error = 80, - .type = TorqueMotorLimited, -}; - -const SteeringLimits CHRYSLER_RAM_HD_STEERING_LIMITS = { - .max_steer = 361, - .max_rt_delta = 182, - .max_rt_interval = 250000, - .max_rate_up = 14, - .max_rate_down = 14, - .max_torque_error = 80, - .type = TorqueMotorLimited, -}; - typedef struct { const int EPS_2; const int ESP_1; @@ -214,6 +184,36 @@ static void chrysler_rx_hook(const CANPacket_t *to_push) { } static bool chrysler_tx_hook(const CANPacket_t *to_send) { + const SteeringLimits CHRYSLER_STEERING_LIMITS = { + .max_steer = 261, + .max_rt_delta = 112, + .max_rt_interval = 250000, + .max_rate_up = 3, + .max_rate_down = 3, + .max_torque_error = 80, + .type = TorqueMotorLimited, + }; + + const SteeringLimits CHRYSLER_RAM_DT_STEERING_LIMITS = { + .max_steer = 350, + .max_rt_delta = 112, + .max_rt_interval = 250000, + .max_rate_up = 6, + .max_rate_down = 6, + .max_torque_error = 80, + .type = TorqueMotorLimited, + }; + + const SteeringLimits CHRYSLER_RAM_HD_STEERING_LIMITS = { + .max_steer = 361, + .max_rt_delta = 182, + .max_rt_interval = 250000, + .max_rate_up = 14, + .max_rate_down = 14, + .max_torque_error = 80, + .type = TorqueMotorLimited, + }; + bool tx = true; int addr = GET_ADDR(to_send); diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index fd944761e6..ca7fafe756 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -1,28 +1,3 @@ -const SteeringLimits GM_STEERING_LIMITS = { - .max_steer = 300, - .max_rate_up = 10, - .max_rate_down = 15, - .driver_torque_allowance = 65, - .driver_torque_factor = 4, - .max_rt_delta = 128, - .max_rt_interval = 250000, - .type = TorqueDriverLimited, -}; - -const LongitudinalLimits GM_ASCM_LONG_LIMITS = { - .max_gas = 3072, - .min_gas = 1404, - .inactive_gas = 1404, - .max_brake = 400, -}; - -const LongitudinalLimits GM_CAM_LONG_LIMITS = { - .max_gas = 3400, - .min_gas = 1514, - .inactive_gas = 1554, - .max_brake = 400, -}; - const LongitudinalLimits *gm_long_limits; const int GM_STANDSTILL_THRSLD = 10; // 0.311kph @@ -136,6 +111,17 @@ static void gm_rx_hook(const CANPacket_t *to_push) { } static bool gm_tx_hook(const CANPacket_t *to_send) { + const SteeringLimits GM_STEERING_LIMITS = { + .max_steer = 300, + .max_rate_up = 10, + .max_rate_down = 15, + .driver_torque_allowance = 65, + .driver_torque_factor = 4, + .max_rt_delta = 128, + .max_rt_interval = 250000, + .type = TorqueDriverLimited, + }; + bool tx = true; int addr = GET_ADDR(to_send); @@ -215,6 +201,20 @@ static int gm_fwd_hook(int bus_num, int addr) { } static safety_config gm_init(uint16_t param) { + static const LongitudinalLimits GM_ASCM_LONG_LIMITS = { + .max_gas = 3072, + .min_gas = 1404, + .inactive_gas = 1404, + .max_brake = 400, + }; + + static const LongitudinalLimits GM_CAM_LONG_LIMITS = { + .max_gas = 3400, + .min_gas = 1514, + .inactive_gas = 1554, + .max_brake = 400, + }; + gm_hw = GET_FLAG(param, GM_PARAM_HW_CAM) ? GM_CAM : GM_ASCM; if (gm_hw == GM_ASCM) { diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index 7bbc8e6616..dc74aa77ba 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -8,7 +8,6 @@ const CanMsg HONDA_RADARLESS_LONG_TX_MSGS[] = {{0xE4, 0, 5}, {0x33D, 0, 8}, {0x1 // panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches // If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state // Threshold calculated from DBC gains: round(((83.3 / 0.253984064) + (83.3 / 0.126992032)) / 2) = 492 -const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 492; #define HONDA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks const LongitudinalLimits HONDA_BOSCH_LONG_LIMITS = { @@ -141,6 +140,8 @@ static uint8_t honda_get_counter(const CANPacket_t *to_push) { } static void honda_rx_hook(const CANPacket_t *to_push) { + const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 492; + const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || \ ((honda_hw == HONDA_NIDEC) && !enable_gas_interceptor); int pt_bus = honda_get_pt_bus(); diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index d88762c0d0..1b7bdb03d1 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -17,9 +17,6 @@ .has_steer_req_tolerance = true, \ } -const SteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(384, 3, 7); -const SteeringLimits HYUNDAI_STEERING_LIMITS_ALT = HYUNDAI_LIMITS(270, 2, 3); - const LongitudinalLimits HYUNDAI_LONG_LIMITS = { .max_accel = 200, // 1/100 m/s2 .min_accel = -350, // 1/100 m/s2 @@ -215,6 +212,9 @@ static void hyundai_rx_hook(const CANPacket_t *to_push) { } static bool hyundai_tx_hook(const CANPacket_t *to_send) { + const SteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(384, 3, 7); + const SteeringLimits HYUNDAI_STEERING_LIMITS_ALT = HYUNDAI_LIMITS(270, 2, 3); + bool tx = true; int addr = GET_ADDR(to_send); diff --git a/board/safety/safety_hyundai_canfd.h b/board/safety/safety_hyundai_canfd.h index be026a9840..394afa03ca 100644 --- a/board/safety/safety_hyundai_canfd.h +++ b/board/safety/safety_hyundai_canfd.h @@ -1,23 +1,5 @@ #include "safety_hyundai_common.h" -const SteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = { - .max_steer = 270, - .max_rt_delta = 112, - .max_rt_interval = 250000, - .max_rate_up = 2, - .max_rate_down = 3, - .driver_torque_allowance = 250, - .driver_torque_factor = 2, - .type = TorqueDriverLimited, - - // the EPS faults when the steering angle is above a certain threshold for too long. to prevent this, - // we allow setting torque actuation bit to 0 while maintaining the requested torque value for two consecutive frames - .min_valid_request_frames = 89, - .max_invalid_request_frames = 2, - .min_valid_request_rt_interval = 810000, // 810ms; a ~10% buffer on cutting every 90 frames - .has_steer_req_tolerance = true, -}; - const CanMsg HYUNDAI_CANFD_HDA2_TX_MSGS[] = { {0x50, 0, 16}, // LKAS {0x1CF, 1, 8}, // CRUISE_BUTTON @@ -228,6 +210,24 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) { } static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send) { + const SteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = { + .max_steer = 270, + .max_rt_delta = 112, + .max_rt_interval = 250000, + .max_rate_up = 2, + .max_rate_down = 3, + .driver_torque_allowance = 250, + .driver_torque_factor = 2, + .type = TorqueDriverLimited, + + // the EPS faults when the steering angle is above a certain threshold for too long. to prevent this, + // we allow setting torque actuation bit to 0 while maintaining the requested torque value for two consecutive frames + .min_valid_request_frames = 89, + .max_invalid_request_frames = 2, + .min_valid_request_rt_interval = 810000, // 810ms; a ~10% buffer on cutting every 90 frames + .has_steer_req_tolerance = true, + }; + bool tx = true; int addr = GET_ADDR(to_send); diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index 7c6d8be9c7..938534130b 100644 --- a/board/safety/safety_mazda.h +++ b/board/safety/safety_mazda.h @@ -12,17 +12,6 @@ #define MAZDA_AUX 1 #define MAZDA_CAM 2 -const SteeringLimits MAZDA_STEERING_LIMITS = { - .max_steer = 800, - .max_rate_up = 10, - .max_rate_down = 25, - .max_rt_delta = 300, - .max_rt_interval = 250000, - .driver_torque_factor = 1, - .driver_torque_allowance = 15, - .type = TorqueDriverLimited, -}; - const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}}; RxCheck mazda_rx_checks[] = { @@ -69,6 +58,17 @@ static void mazda_rx_hook(const CANPacket_t *to_push) { } static bool mazda_tx_hook(const CANPacket_t *to_send) { + static const SteeringLimits MAZDA_STEERING_LIMITS = { + .max_steer = 800, + .max_rate_up = 10, + .max_rate_down = 25, + .max_rt_delta = 300, + .max_rt_interval = 250000, + .driver_torque_factor = 1, + .driver_torque_allowance = 15, + .type = TorqueDriverLimited, + }; + bool tx = true; int addr = GET_ADDR(to_send); int bus = GET_BUS(to_send); diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 201c6fe5e7..86df05acf8 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -16,11 +16,6 @@ .has_steer_req_tolerance = true, \ } - -const SteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70); -const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40); - - const LongitudinalLimits SUBARU_LONG_LIMITS = { .min_gas = 808, // appears to be engine braking .max_gas = 3400, // approx 2 m/s^2 when maxing cruise_rpm and cruise_throttle @@ -180,6 +175,9 @@ static void subaru_rx_hook(const CANPacket_t *to_push) { } static bool subaru_tx_hook(const CANPacket_t *to_send) { + const SteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70); + const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40); + bool tx = true; int addr = GET_ADDR(to_send); bool violation = false; diff --git a/board/safety/safety_subaru_preglobal.h b/board/safety/safety_subaru_preglobal.h index 37e389afb2..0c3c8bcc90 100644 --- a/board/safety/safety_subaru_preglobal.h +++ b/board/safety/safety_subaru_preglobal.h @@ -1,14 +1,3 @@ -const SteeringLimits SUBARU_PG_STEERING_LIMITS = { - .max_steer = 2047, - .max_rt_delta = 940, - .max_rt_interval = 250000, - .max_rate_up = 50, - .max_rate_down = 70, - .driver_torque_factor = 10, - .driver_torque_allowance = 75, - .type = TorqueDriverLimited, -}; - // Preglobal platform // 0x161 is ES_CruiseThrottle // 0x164 is ES_LKAS @@ -78,6 +67,17 @@ static void subaru_preglobal_rx_hook(const CANPacket_t *to_push) { } static bool subaru_preglobal_tx_hook(const CANPacket_t *to_send) { + const SteeringLimits SUBARU_PG_STEERING_LIMITS = { + .max_steer = 2047, + .max_rt_delta = 940, + .max_rt_interval = 250000, + .max_rate_up = 50, + .max_rate_down = 70, + .driver_torque_factor = 10, + .driver_torque_allowance = 75, + .type = TorqueDriverLimited, + }; + bool tx = true; int addr = GET_ADDR(to_send); diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index 4fa0df84aa..b2a1735bab 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -1,15 +1,3 @@ -const SteeringLimits TESLA_STEERING_LIMITS = { - .angle_deg_to_can = 10, - .angle_rate_up_lookup = { - {0., 5., 15.}, - {10., 1.6, .3} - }, - .angle_rate_down_lookup = { - {0., 5., 15.}, - {10., 7.0, .8} - }, -}; - const LongitudinalLimits TESLA_LONG_LIMITS = { .max_accel = 425, // 2. m/s^2 .min_accel = 287, // -3.52 m/s^2 // TODO: limit to -3.48 @@ -117,6 +105,18 @@ static void tesla_rx_hook(const CANPacket_t *to_push) { static bool tesla_tx_hook(const CANPacket_t *to_send) { + static const SteeringLimits TESLA_STEERING_LIMITS = { + .angle_deg_to_can = 10, + .angle_rate_up_lookup = { + {0., 5., 15.}, + {10., 1.6, .3} + }, + .angle_rate_down_lookup = { + {0., 5., 15.}, + {10., 7.0, .8} + }, + }; + bool tx = true; int addr = GET_ADDR(to_send); bool violation = false; diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 0f7e671c4a..b863678012 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -28,8 +28,6 @@ const SteeringLimits TOYOTA_STEERING_LIMITS = { }; const int TOYOTA_LTA_MAX_ANGLE = 1657; // EPS only accepts up to 94.9461 -const int TOYOTA_LTA_MAX_MEAS_TORQUE = 1500; -const int TOYOTA_LTA_MAX_DRIVER_TORQUE = 150; // longitudinal limits const LongitudinalLimits TOYOTA_LONG_LIMITS = { @@ -40,7 +38,6 @@ const LongitudinalLimits TOYOTA_LONG_LIMITS = { // panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches // If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state // Threshold calculated from DBC gains: round((((15 + 75.555) / 0.159375) + ((15 + 151.111) / 0.159375)) / 2) = 805 -const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805; #define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks #define TOYOTA_COMMON_TX_MSGS \ @@ -86,7 +83,6 @@ RxCheck toyota_lta_interceptor_rx_checks[] = { // safety param flags // first byte is for EPS factor, second is for flags const uint32_t TOYOTA_PARAM_OFFSET = 8U; -const uint32_t TOYOTA_EPS_FACTOR = (1UL << TOYOTA_PARAM_OFFSET) - 1U; const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1UL << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2UL << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_LTA = 4UL << TOYOTA_PARAM_OFFSET; @@ -134,6 +130,8 @@ static bool toyota_get_quality_flag_valid(const CANPacket_t *to_push) { } static void toyota_rx_hook(const CANPacket_t *to_push) { + const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805; + if (GET_BUS(to_push) == 0U) { int addr = GET_ADDR(to_push); @@ -214,6 +212,9 @@ static void toyota_rx_hook(const CANPacket_t *to_push) { } static bool toyota_tx_hook(const CANPacket_t *to_send) { + const int TOYOTA_LTA_MAX_DRIVER_TORQUE = 150; + const int TOYOTA_LTA_MAX_MEAS_TORQUE = 1500; + bool tx = true; int addr = GET_ADDR(to_send); int bus = GET_BUS(to_send); @@ -303,6 +304,7 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { } int eps_torque = MIN(ABS(torque_meas.min), ABS(torque_meas.max)); + if ((eps_torque > TOYOTA_LTA_MAX_MEAS_TORQUE) && (torque_wind_down != 0)) { tx = false; } @@ -331,6 +333,8 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { } static safety_config toyota_init(uint16_t param) { + const uint32_t TOYOTA_EPS_FACTOR = (1UL << TOYOTA_PARAM_OFFSET) - 1U; + toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE); toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL); toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA); diff --git a/board/safety/safety_volkswagen_mqb.h b/board/safety/safety_volkswagen_mqb.h index 3145ea9072..e11dbe41d9 100644 --- a/board/safety/safety_volkswagen_mqb.h +++ b/board/safety/safety_volkswagen_mqb.h @@ -1,25 +1,5 @@ #include "safety_volkswagen_common.h" -// lateral limits -const SteeringLimits VOLKSWAGEN_MQB_STEERING_LIMITS = { - .max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) - .max_rt_delta = 75, // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75 - .max_rt_interval = 250000, // 250ms between real time checks - .max_rate_up = 4, // 2.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) - .max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) - .driver_torque_allowance = 80, - .driver_torque_factor = 3, - .type = TorqueDriverLimited, -}; - -// longitudinal limits -// acceleration in m/s2 * 1000 to avoid floating point math -const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS = { - .max_accel = 2000, - .min_accel = -3500, - .inactive_accel = 3010, // VW sends one increment above the max range when inactive -}; - #define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds #define MSG_LH_EPS_03 0x09F // RX from EPS, for driver steering torque #define MSG_ESP_05 0x106 // RX from ABS, for brake switch state @@ -194,6 +174,26 @@ static void volkswagen_mqb_rx_hook(const CANPacket_t *to_push) { } static bool volkswagen_mqb_tx_hook(const CANPacket_t *to_send) { + // lateral limits + static const SteeringLimits VOLKSWAGEN_MQB_STEERING_LIMITS = { + .max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) + .max_rt_delta = 75, // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75 + .max_rt_interval = 250000, // 250ms between real time checks + .max_rate_up = 4, // 2.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) + .max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) + .driver_torque_allowance = 80, + .driver_torque_factor = 3, + .type = TorqueDriverLimited, + }; + + // longitudinal limits + // acceleration in m/s2 * 1000 to avoid floating point math + static const LongitudinalLimits VOLKSWAGEN_MQB_LONG_LIMITS = { + .max_accel = 2000, + .min_accel = -3500, + .inactive_accel = 3010, // VW sends one increment above the max range when inactive + }; + int addr = GET_ADDR(to_send); bool tx = true; diff --git a/board/safety/safety_volkswagen_pq.h b/board/safety/safety_volkswagen_pq.h index b9a2eedd72..8b61d24ae5 100644 --- a/board/safety/safety_volkswagen_pq.h +++ b/board/safety/safety_volkswagen_pq.h @@ -1,25 +1,5 @@ #include "safety_volkswagen_common.h" -// lateral limits -const SteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = { - .max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) - .max_rt_delta = 113, // 6 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 75 ; 125 * 1.5 for safety pad = 113 - .max_rt_interval = 250000, // 250ms between real time checks - .max_rate_up = 6, // 3.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) - .max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) - .driver_torque_factor = 3, - .driver_torque_allowance = 80, - .type = TorqueDriverLimited, -}; - -// longitudinal limits -// acceleration in m/s2 * 1000 to avoid floating point math -const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS = { - .max_accel = 2000, - .min_accel = -3500, - .inactive_accel = 3010, // VW sends one increment above the max range when inactive -}; - #define MSG_LENKHILFE_3 0x0D0 // RX from EPS, for steering angle and driver steering torque #define MSG_HCA_1 0x0D2 // TX by OP, Heading Control Assist steering torque #define MSG_BREMSE_1 0x1A0 // RX from ABS, for ego speed @@ -170,6 +150,26 @@ static void volkswagen_pq_rx_hook(const CANPacket_t *to_push) { } static bool volkswagen_pq_tx_hook(const CANPacket_t *to_send) { + // lateral limits + const SteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = { + .max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) + .max_rt_delta = 113, // 6 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 75 ; 125 * 1.5 for safety pad = 113 + .max_rt_interval = 250000, // 250ms between real time checks + .max_rate_up = 6, // 3.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) + .max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) + .driver_torque_factor = 3, + .driver_torque_allowance = 80, + .type = TorqueDriverLimited, + }; + + // longitudinal limits + // acceleration in m/s2 * 1000 to avoid floating point math + const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS = { + .max_accel = 2000, + .min_accel = -3500, + .inactive_accel = 3010, // VW sends one increment above the max range when inactive + }; + int addr = GET_ADDR(to_send); bool tx = true; diff --git a/board/safety_declarations.h b/board/safety_declarations.h index eb4ece4741..5633819eda 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -34,7 +34,7 @@ struct sample_t { int values[MAX_SAMPLE_VALS]; int min; int max; -} sample_t_default = {.values = {0}, .min = 0, .max = 0}; +}; // safety code requires floats struct lookup_t { @@ -174,28 +174,28 @@ void reset_sample(struct sample_t *sample); bool max_limit_check(int val, const int MAX, const int MIN); bool angle_dist_to_meas_check(int val, struct sample_t *val_meas, const int MAX_ERROR, const int MAX_VAL); -bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, +static bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR); -bool driver_limit_check(int val, int val_last, const struct sample_t *val_driver, +static bool driver_limit_check(int val, int val_last, const struct sample_t *val_driver, const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ALLOWANCE, const int DRIVER_FACTOR); bool get_longitudinal_allowed(void); -bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA); +static bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA); float interpolate(struct lookup_t xy, float x); int ROUND(float val); 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(const CANPacket_t *to_send, const CanMsg msg_list[], int len); -int get_addr_check_index(const CANPacket_t *to_push, RxCheck addr_list[], const int len); -void update_counter(RxCheck addr_list[], int index, uint8_t counter); -void update_addr_timestamp(RxCheck addr_list[], int index); +static bool msg_allowed(const CANPacket_t *to_send, const CanMsg msg_list[], int len); +static int get_addr_check_index(const CANPacket_t *to_push, RxCheck addr_list[], const int len); +static void update_counter(RxCheck addr_list[], int index, uint8_t counter); +static void update_addr_timestamp(RxCheck addr_list[], int index); bool is_msg_valid(RxCheck addr_list[], int index); -bool rx_msg_safety_check(const CANPacket_t *to_push, +static bool rx_msg_safety_check(const CANPacket_t *to_push, const safety_config *cfg, const safety_hooks *safety_hooks); void generic_rx_checks(bool stock_ecu_detected); -void relay_malfunction_set(void); -void relay_malfunction_reset(void); +static void relay_malfunction_set(void); +static void relay_malfunction_reset(void); bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits); bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits); bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits); diff --git a/board/stm32fx/llbxcan.h b/board/stm32fx/llbxcan.h index a6c5936f05..a434cda4d5 100644 --- a/board/stm32fx/llbxcan.h +++ b/board/stm32fx/llbxcan.h @@ -94,7 +94,7 @@ void llcan_irq_disable(const CAN_TypeDef *CANx) { } } -void llcan_irq_enable(const CAN_TypeDef *CANx) { +static void llcan_irq_enable(const CAN_TypeDef *CANx) { if (CANx == CAN1) { NVIC_EnableIRQ(CAN1_TX_IRQn); NVIC_EnableIRQ(CAN1_RX0_IRQn); diff --git a/board/stm32fx/llfan.h b/board/stm32fx/llfan.h index 9e3f0c654b..36dce94266 100644 --- a/board/stm32fx/llfan.h +++ b/board/stm32fx/llfan.h @@ -1,5 +1,5 @@ // TACH interrupt handler -void EXTI2_IRQ_Handler(void) { +static void EXTI2_IRQ_Handler(void) { volatile unsigned int pr = EXTI->PR & (1U << 2); if ((pr & (1U << 2)) != 0U) { fan_state.tach_counter++; @@ -7,7 +7,7 @@ void EXTI2_IRQ_Handler(void) { EXTI->PR = (1U << 2); } -void llfan_init(void) { +static void llfan_init(void) { // 5000RPM * 4 tach edges / 60 seconds REGISTER_INTERRUPT(EXTI2_IRQn, EXTI2_IRQ_Handler, 700U, FAULT_INTERRUPT_RATE_TACH) diff --git a/board/stm32fx/llspi.h b/board/stm32fx/llspi.h index 31e419b6f7..a83e322cec 100644 --- a/board/stm32fx/llspi.h +++ b/board/stm32fx/llspi.h @@ -31,7 +31,7 @@ void llspi_mosi_dma(uint8_t *addr, int len) { } // SPI MOSI DMA FINISHED -void DMA2_Stream2_IRQ_Handler(void) { +static void DMA2_Stream2_IRQ_Handler(void) { // Clear interrupt flag ENTER_CRITICAL(); DMA2->LIFCR = DMA_LIFCR_CTCIF2; @@ -42,7 +42,7 @@ void DMA2_Stream2_IRQ_Handler(void) { } // SPI MISO DMA FINISHED -void DMA2_Stream3_IRQ_Handler(void) { +static void DMA2_Stream3_IRQ_Handler(void) { // Clear interrupt flag DMA2->LIFCR = DMA_LIFCR_CTCIF3; diff --git a/board/stm32fx/lluart.h b/board/stm32fx/lluart.h index 1e4e9015df..e572474952 100644 --- a/board/stm32fx/lluart.h +++ b/board/stm32fx/lluart.h @@ -20,7 +20,7 @@ void uart_tx_ring(uart_ring *q){ EXIT_CRITICAL(); } -void uart_rx_ring(uart_ring *q){ +static void uart_rx_ring(uart_ring *q){ ENTER_CRITICAL(); // Read out RX buffer @@ -53,7 +53,7 @@ void uart_send_break(uart_ring *u) { // This read after reading SR clears all error interrupts. We don't want compiler warnings, nor optimizations #define UART_READ_DR(uart) volatile uint8_t t = (uart)->DR; UNUSED(t); -void uart_interrupt_handler(uart_ring *q) { +static void uart_interrupt_handler(uart_ring *q) { ENTER_CRITICAL(); // Read UART status. This is also the first step necessary in clearing most interrupts @@ -78,7 +78,7 @@ void uart_interrupt_handler(uart_ring *q) { EXIT_CRITICAL(); } -void USART2_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_debug); } +static void USART2_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_debug); } // ***************************** Hardware setup ***************************** diff --git a/board/stm32fx/llusb.h b/board/stm32fx/llusb.h index b6eea1dad2..d4853b8e7d 100644 --- a/board/stm32fx/llusb.h +++ b/board/stm32fx/llusb.h @@ -13,7 +13,7 @@ USB_OTG_GlobalTypeDef *USBx = USB_OTG_FS; void usb_irqhandler(void); -void OTG_FS_IRQ_Handler(void) { +static void OTG_FS_IRQ_Handler(void) { NVIC_DisableIRQ(OTG_FS_IRQn); //__disable_irq(); usb_irqhandler(); diff --git a/board/stm32fx/peripherals.h b/board/stm32fx/peripherals.h index 5bdb8a056c..c63b74b7c2 100644 --- a/board/stm32fx/peripherals.h +++ b/board/stm32fx/peripherals.h @@ -1,4 +1,4 @@ -void gpio_usb_init(void) { +static void gpio_usb_init(void) { // A11,A12: USB set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG_FS); set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG_FS); diff --git a/board/stm32h7/board.h b/board/stm32h7/board.h index d991e433b1..d86e329ecd 100644 --- a/board/stm32h7/board.h +++ b/board/stm32h7/board.h @@ -20,7 +20,7 @@ #include "boards/tres.h" -uint8_t get_board_id(void) { +static uint8_t get_board_id(void) { return detect_with_pull(GPIOF, 7, PULL_UP) | (detect_with_pull(GPIOF, 8, PULL_UP) << 1U) | (detect_with_pull(GPIOF, 9, PULL_UP) << 2U) | diff --git a/board/stm32h7/lladc.h b/board/stm32h7/lladc.h index 0a52b78d37..38b19efcd9 100644 --- a/board/stm32h7/lladc.h +++ b/board/stm32h7/lladc.h @@ -17,7 +17,7 @@ void adc_init(void) { while(!(ADC1->ISR & ADC_ISR_ADRDY)); } -uint16_t adc_get_raw(uint8_t channel) { +static uint16_t adc_get_raw(uint8_t channel) { ADC1->SQR1 &= ~(ADC_SQR1_L); ADC1->SQR1 = ((uint32_t) channel << 6U); diff --git a/board/stm32h7/llfan.h b/board/stm32h7/llfan.h index dce622503a..5a069fb482 100644 --- a/board/stm32h7/llfan.h +++ b/board/stm32h7/llfan.h @@ -1,5 +1,5 @@ // TACH interrupt handler -void EXTI2_IRQ_Handler(void) { +static void EXTI2_IRQ_Handler(void) { volatile unsigned int pr = EXTI->PR1 & (1U << 2); if ((pr & (1U << 2)) != 0U) { fan_state.tach_counter++; @@ -7,7 +7,7 @@ void EXTI2_IRQ_Handler(void) { EXTI->PR1 = (1U << 2); } -void llfan_init(void) { +static void llfan_init(void) { // 5000RPM * 4 tach edges / 60 seconds REGISTER_INTERRUPT(EXTI2_IRQn, EXTI2_IRQ_Handler, 700U, FAULT_INTERRUPT_RATE_TACH) diff --git a/board/stm32h7/llspi.h b/board/stm32h7/llspi.h index 1947803ac2..d1c0a8c0d1 100644 --- a/board/stm32h7/llspi.h +++ b/board/stm32h7/llspi.h @@ -1,3 +1,5 @@ +static bool spi_tx_dma_done = false; + // master -> panda DMA start void llspi_mosi_dma(uint8_t *addr, int len) { // disable DMA + SPI @@ -49,7 +51,7 @@ void llspi_miso_dma(uint8_t *addr, int len) { } // master -> panda DMA finished -void DMA2_Stream2_IRQ_Handler(void) { +static void DMA2_Stream2_IRQ_Handler(void) { // Clear interrupt flag DMA2->LIFCR = DMA_LIFCR_CTCIF2; @@ -57,7 +59,7 @@ void DMA2_Stream2_IRQ_Handler(void) { } // panda -> master DMA finished -void DMA2_Stream3_IRQ_Handler(void) { +static void DMA2_Stream3_IRQ_Handler(void) { ENTER_CRITICAL(); DMA2->LIFCR = DMA_LIFCR_CTCIF3; @@ -67,7 +69,7 @@ void DMA2_Stream3_IRQ_Handler(void) { } // panda TX finished -void SPI4_IRQ_Handler(void) { +static void SPI4_IRQ_Handler(void) { // clear flag SPI4->IFCR |= (0x1FFU << 3U); diff --git a/board/stm32h7/lluart.h b/board/stm32h7/lluart.h index 0ad7b6a867..b5419b6a83 100644 --- a/board/stm32h7/lluart.h +++ b/board/stm32h7/lluart.h @@ -1,4 +1,4 @@ -void uart_rx_ring(uart_ring *q){ +static void uart_rx_ring(uart_ring *q){ // Do not read out directly if DMA enabled ENTER_CRITICAL(); @@ -52,7 +52,7 @@ void uart_set_baud(USART_TypeDef *u, unsigned int baud) { // This read after reading ISR clears all error interrupts. We don't want compiler warnings, nor optimizations #define UART_READ_RDR(uart) volatile uint8_t t = (uart)->RDR; UNUSED(t); -void uart_interrupt_handler(uart_ring *q) { +static void uart_interrupt_handler(uart_ring *q) { ENTER_CRITICAL(); // Read UART status. This is also the first step necessary in clearing most interrupts @@ -88,7 +88,7 @@ void uart_interrupt_handler(uart_ring *q) { EXIT_CRITICAL(); } -void UART7_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_som_debug); } +static void UART7_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_som_debug); } void uart_init(uart_ring *q, int baud) { if (q->uart == UART7) { diff --git a/board/stm32h7/llusb.h b/board/stm32h7/llusb.h index ada1630f8b..73bccaabe4 100644 --- a/board/stm32h7/llusb.h +++ b/board/stm32h7/llusb.h @@ -14,7 +14,7 @@ USB_OTG_GlobalTypeDef *USBx = USB_OTG_HS; void usb_irqhandler(void); -void OTG_HS_IRQ_Handler(void) { +static void OTG_HS_IRQ_Handler(void) { NVIC_DisableIRQ(OTG_HS_IRQn); usb_irqhandler(); NVIC_EnableIRQ(OTG_HS_IRQn); diff --git a/board/stm32h7/peripherals.h b/board/stm32h7/peripherals.h index b60f19016d..d757835eaf 100644 --- a/board/stm32h7/peripherals.h +++ b/board/stm32h7/peripherals.h @@ -1,4 +1,4 @@ -void gpio_usb_init(void) { +static void gpio_usb_init(void) { // A11,A12: USB set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG1_FS); set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG1_FS); diff --git a/tests/misra/suppressions.txt b/tests/misra/suppressions.txt index 57db05a5fb..772f6154cd 100644 --- a/tests/misra/suppressions.txt +++ b/tests/misra/suppressions.txt @@ -21,7 +21,6 @@ unusedFunction misra-config misra-c2012-1.2 # this is from the extensions (e.g. __typeof__) used in the MIN, MAX, ABS, and CLAMP macros misra-c2012-2.5 -misra-c2012-8.7 misra-c2012-8.4 misra-c2012-10.6 misra-c2012-10.3