From b683ef4951f27e514177775dede791725f115237 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 28 Nov 2025 02:26:40 +0100 Subject: [PATCH 01/29] progress --- SConscript | 97 +- board/boards/board_declarations.h | 7 +- board/boards/cuatro.c | 140 +++ board/boards/cuatro.h | 128 +-- board/boards/red.c | 134 +++ board/boards/red.h | 129 +-- board/boards/tres.c | 180 ++++ board/boards/tres.h | 169 +--- board/boards/unused_funcs.c | 32 + board/boards/unused_funcs.h | 40 +- board/body/boards/board_body.c | 23 + board/body/boards/board_body.h | 22 +- board/body/motor_control.c | 251 +++++ board/body/motor_control.h | 2 + board/body/motor_encoder.h | 108 --- board/body/stm32h7/board.h | 2 + board/bootstub.c | 18 + board/bootstub_declarations.h | 24 +- board/can_comms.c | 109 +++ board/can_comms.h | 107 +-- board/comms_definitions.h | 4 + board/config.h | 3 +- board/crc.c | 19 + board/crc.h | 20 +- board/critical.c | 18 + board/critical.h | 19 +- board/critical_declarations.h | 2 + board/drivers/bootkick.c | 72 ++ board/drivers/bootkick.h | 69 +- board/drivers/can_common.c | 264 ++++++ board/drivers/can_common.h | 341 +++---- board/drivers/can_common_declarations.h | 70 -- board/drivers/clock_source.c | 45 + board/drivers/clock_source.h | 48 +- board/drivers/clock_source_declarations.h | 7 - board/drivers/fake_siren.c | 115 +++ board/drivers/fake_siren.h | 121 +-- board/drivers/fan.c | 51 + board/drivers/fan.h | 67 +- board/drivers/fan_declarations.h | 16 - board/drivers/fdcan.c | 279 ++++++ board/drivers/fdcan.h | 272 +----- board/drivers/fdcan_declarations.h | 19 - board/drivers/gpio.c | 83 ++ board/drivers/gpio.h | 72 +- board/drivers/harness.c | 105 ++ board/drivers/harness.h | 142 +-- board/drivers/harness_declarations.h | 33 - board/drivers/interrupts.c | 83 ++ board/drivers/interrupts.h | 99 +- board/drivers/interrupts_declarations.h | 31 - board/drivers/led.c | 28 + board/drivers/led.h | 31 +- board/drivers/pwm.c | 56 ++ board/drivers/pwm.h | 58 +- board/drivers/registers.c | 68 ++ board/drivers/registers.h | 112 ++- board/drivers/registers_declarations.h | 24 +- board/drivers/simple_watchdog.c | 28 + board/drivers/simple_watchdog.h | 26 +- board/drivers/simple_watchdog_declarations.h | 10 - board/drivers/spi.c | 234 +++++ board/drivers/spi.h | 265 +----- board/drivers/spi_declarations.h | 44 - board/drivers/timers.c | 34 + board/drivers/timers.h | 35 +- board/drivers/uart.c | 149 +++ board/drivers/uart.h | 175 ++-- board/drivers/uart_declarations.h | 39 - board/drivers/usb.c | 799 ++++++++++++++++ board/drivers/usb.h | 897 +++--------------- board/drivers/usb_declarations.h | 111 --- board/early_init.c | 66 ++ board/early_init.h | 58 +- board/fake_stm.c | 21 + board/fake_stm.h | 18 +- board/faults.c | 25 + board/faults.h | 28 +- board/faults_declarations.h | 35 +- board/flasher.c | 163 ++++ board/flasher.h | 160 +--- board/globals copy.h | 10 + board/globals.h | 10 + board/health.h | 2 + board/libc.c | 82 ++ board/libc.h | 82 +- board/main.c | 14 +- board/main_comms.c | 345 +++++++ board/main_comms.h | 328 +------ board/main_declarations.h | 15 +- ...{main_definitions.h => main_definitions.c} | 0 board/power_saving.c | 60 ++ board/power_saving.h | 54 +- board/power_saving_declarations.h | 11 - board/provision.c | 16 + board/provision.h | 11 +- board/stm32h7/board.c | 34 + board/stm32h7/board.h | 32 +- board/stm32h7/clock.c | 119 +++ board/stm32h7/clock.h | 112 +-- board/stm32h7/interrupt_handlers.c | 143 +++ board/stm32h7/interrupt_handlers.h | 282 +++--- board/stm32h7/lladc.c | 82 ++ board/stm32h7/lladc.h | 127 +-- board/stm32h7/lladc_declarations.h | 37 - board/stm32h7/llfan.c | 31 + board/stm32h7/llfan.h | 24 +- board/stm32h7/llfdcan.c | 227 +++++ board/stm32h7/llfdcan.h | 62 +- board/stm32h7/llfdcan_declarations.h | 53 -- board/stm32h7/llflash.c | 37 + board/stm32h7/llflash.h | 39 +- board/stm32h7/lli2c.c | 169 ++++ board/stm32h7/lli2c.h | 173 +--- board/stm32h7/llspi.c | 111 +++ board/stm32h7/llspi.h | 110 +-- board/stm32h7/lluart.c | 104 ++ board/stm32h7/lluart.h | 103 +- board/stm32h7/llusb.c | 79 ++ board/stm32h7/llusb.h | 87 +- board/stm32h7/llusb_declarations.h | 16 - board/stm32h7/peripherals.c | 138 +++ board/stm32h7/peripherals.h | 131 +-- board/stm32h7/sound.c | 210 ++++ board/stm32h7/sound.h | 210 +--- board/stm32h7/stm32h7_config.c | 8 + board/stm32h7/stm32h7_config.h | 19 +- board/utils.c | 9 + board/utils.h | 6 +- setup.sh | 31 +- 130 files changed, 6804 insertions(+), 5499 deletions(-) create mode 100644 board/boards/cuatro.c create mode 100644 board/boards/red.c create mode 100644 board/boards/tres.c create mode 100644 board/boards/unused_funcs.c create mode 100644 board/body/boards/board_body.c create mode 100644 board/body/motor_control.c create mode 100644 board/can_comms.c create mode 100644 board/crc.c create mode 100644 board/critical.c create mode 100644 board/drivers/bootkick.c create mode 100644 board/drivers/can_common.c delete mode 100644 board/drivers/can_common_declarations.h create mode 100644 board/drivers/clock_source.c delete mode 100644 board/drivers/clock_source_declarations.h create mode 100644 board/drivers/fake_siren.c create mode 100644 board/drivers/fan.c delete mode 100644 board/drivers/fan_declarations.h create mode 100644 board/drivers/fdcan.c delete mode 100644 board/drivers/fdcan_declarations.h create mode 100644 board/drivers/gpio.c create mode 100644 board/drivers/harness.c delete mode 100644 board/drivers/harness_declarations.h create mode 100644 board/drivers/interrupts.c delete mode 100644 board/drivers/interrupts_declarations.h create mode 100644 board/drivers/led.c create mode 100644 board/drivers/pwm.c create mode 100644 board/drivers/registers.c create mode 100644 board/drivers/simple_watchdog.c delete mode 100644 board/drivers/simple_watchdog_declarations.h create mode 100644 board/drivers/spi.c delete mode 100644 board/drivers/spi_declarations.h create mode 100644 board/drivers/timers.c create mode 100644 board/drivers/uart.c delete mode 100644 board/drivers/uart_declarations.h create mode 100644 board/drivers/usb.c delete mode 100644 board/drivers/usb_declarations.h create mode 100644 board/early_init.c create mode 100644 board/fake_stm.c create mode 100644 board/faults.c create mode 100644 board/flasher.c create mode 100644 board/globals copy.h create mode 100644 board/globals.h create mode 100644 board/libc.c create mode 100644 board/main_comms.c rename board/{main_definitions.h => main_definitions.c} (100%) create mode 100644 board/power_saving.c delete mode 100644 board/power_saving_declarations.h create mode 100644 board/provision.c create mode 100644 board/stm32h7/board.c create mode 100644 board/stm32h7/clock.c create mode 100644 board/stm32h7/interrupt_handlers.c create mode 100644 board/stm32h7/lladc.c delete mode 100644 board/stm32h7/lladc_declarations.h create mode 100644 board/stm32h7/llfan.c create mode 100644 board/stm32h7/llfdcan.c delete mode 100644 board/stm32h7/llfdcan_declarations.h create mode 100644 board/stm32h7/llflash.c create mode 100644 board/stm32h7/lli2c.c create mode 100644 board/stm32h7/llspi.c create mode 100644 board/stm32h7/lluart.c create mode 100644 board/stm32h7/llusb.c delete mode 100644 board/stm32h7/llusb_declarations.h create mode 100644 board/stm32h7/peripherals.c create mode 100644 board/stm32h7/sound.c create mode 100644 board/stm32h7/stm32h7_config.c create mode 100644 board/utils.c diff --git a/SConscript b/SConscript index 957982cae81..b5ba1d03cd3 100644 --- a/SConscript +++ b/SConscript @@ -100,25 +100,84 @@ def build_project(project_name, project, main, extra_flags): startup = env.Object(project["STARTUP_FILE"]) + shared = [ + "./crypto/rsa.c", + "./crypto/sha.c", + + "./board/libc.c", + "./board/crc.c", + "./board/early_init.c", + "./board/stm32h7/board.c", + "./board/boards/red.c", + "./board/critical.c", + "./board/drivers/led.c", + "./board/drivers/pwm.c", + "./board/drivers/gpio.c", + "./board/drivers/fake_siren.c", + "./board/stm32h7/lli2c.c", + "./board/stm32h7/clock.c", + "./board/drivers/clock_source.c", + "./board/stm32h7/sound.c", + "./board/stm32h7/llflash.c", + "./board/stm32h7/stm32h7_config.c", + "./board/drivers/registers.c", + "./board/drivers/interrupts.c", + "./board/provision.c", + "./board/stm32h7/peripherals.c", + "./board/stm32h7/llusb.c", + "./board/drivers/usb.c", + "./board/drivers/spi.c", + "./board/drivers/timers.c", + "./board/boards/cuatro.c", + "./board/boards/tres.c", + "./board/stm32h7/lladc.c", + "./board/stm32h7/llspi.c", + "./board/faults.c", + "./board/boards/unused_funcs.c", + "./board/utils.c", + ] + + # # "./board/libc.c", + # # "./board/can_comms.c", + # # "./board/critical.c", + # # "./board/main_definitions.c", + # Build bootstub bs_env = env.Clone() + bs_obj_dir = Dir(f'./board/obj/{project_name}/bootstub') + bs_env = env.Clone(OBJPREFIX=bs_obj_dir) + bs_env.Append(CFLAGS="-DBOOTSTUB", ASFLAGS="-DBOOTSTUB", LINKFLAGS="-DBOOTSTUB") bs_elf = bs_env.Program(f"{project_dir}/bootstub.elf", [ startup, - "./crypto/rsa.c", - "./crypto/sha.c", "./board/bootstub.c", - ]) + "./board/flasher.c", + ] + shared) bs_env.Objcopy(f"./board/obj/bootstub.{project_name}.bin", bs_elf) # Build + sign main (aka app) main_elf = env.Program(f"{project_dir}/main.elf", [ startup, - main - ], LINKFLAGS=[f"-Wl,--section-start,.isr_vector={project['APP_START_ADDRESS']}"] + flags) + "./board/can_comms.c", + "./board/drivers/fan.c", + "./board/drivers/can_common.c", + "./board/power_saving.c", + "./board/drivers/uart.c", + "./board/drivers/fdcan.c", + "./board/stm32h7/llfdcan.c", + "./board/drivers/harness.c", + "./board/drivers/simple_watchdog.c", + "./board/main_comms.c", + "./board/drivers/bootkick.c", + "./board/stm32h7/llfan.c", + # "./board/stm32h7/llusb.c", + "./board/stm32h7/lluart.c", + # "./board/stm32h7/peripherals.c", + main, + ] + shared, LINKFLAGS=[f"-Wl,--section-start,.isr_vector={project['APP_START_ADDRESS']}"] + flags) main_bin = env.Objcopy(f"{project_dir}/main.bin", main_elf) sign_py = File(f"./crypto/sign.py").srcnode().relpath - env.Command(f"./board/obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}") + # env.Command(f"./board/obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}") @@ -139,8 +198,8 @@ base_project_h7 = { # Common autogenerated includes with open("board/obj/gitversion.h", "w") as f: version = get_version(BUILDER, BUILD_TYPE) - f.write(f'extern const uint8_t gitversion[{len(version)+1}];\n') - f.write(f'const uint8_t gitversion[{len(version)+1}] = "{version}";\n') + # f.write(f'extern const uint8_t gitversion[{len(version)}];\n') + f.write(f'static const uint8_t gitversion[{len(version)}] = "{version}";\n') with open("board/obj/version", "w") as f: f.write(f'{get_version(BUILDER, BUILD_TYPE)}') @@ -153,17 +212,17 @@ with open("board/obj/cert.h", "w") as f: # panda fw build_project("panda_h7", base_project_h7, "./board/main.c", []) -# panda jungle fw -flags = [ - "-DPANDA_JUNGLE", -] -if os.getenv("FINAL_PROVISIONING"): - flags += ["-DFINAL_PROVISIONING"] -build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", flags) +# # panda jungle fw +# flags = [ +# "-DPANDA_JUNGLE", +# ] +# if os.getenv("FINAL_PROVISIONING"): +# flags += ["-DFINAL_PROVISIONING"] +# build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", flags) -# body fw -build_project("body_h7", base_project_h7, "./board/body/main.c", ["-DPANDA_BODY"]) +# # body fw +# build_project("body_h7", base_project_h7, "./board/body/main.c", ["-DPANDA_BODY"]) # test files -if GetOption('extras'): - SConscript('tests/libpanda/SConscript') +# if GetOption('extras'): +# SConscript('tests/libpanda/SConscript') diff --git a/board/boards/board_declarations.h b/board/boards/board_declarations.h index ff5ca97a86b..fd88961f3c2 100644 --- a/board/boards/board_declarations.h +++ b/board/boards/board_declarations.h @@ -23,8 +23,13 @@ typedef void (*board_set_bootkick)(BootState state); typedef bool (*board_read_som_gpio)(void); typedef void (*board_set_amp_enabled)(bool enabled); +// #include "board/drivers/harness_declarations.h" +#include "board/config.h" + +struct harness_configuration; + struct board { - harness_configuration *harness_config; + struct harness_configuration *harness_config; GPIO_TypeDef * const led_GPIO[3]; const uint8_t led_pin[3]; const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM diff --git a/board/boards/cuatro.c b/board/boards/cuatro.c new file mode 100644 index 00000000000..8ab79748bdf --- /dev/null +++ b/board/boards/cuatro.c @@ -0,0 +1,140 @@ +#include "board_declarations.h" + +// ////////////////////////// // +// Cuatro (STM32H7) + Harness // +// ////////////////////////// // + +static void cuatro_enable_can_transceiver(uint8_t transceiver, bool enabled) { + switch (transceiver) { + case 1U: + set_gpio_output(GPIOB, 7, !enabled); + break; + case 2U: + set_gpio_output(GPIOB, 10, !enabled); + break; + case 3U: + set_gpio_output(GPIOD, 8, !enabled); + break; + case 4U: + set_gpio_output(GPIOB, 11, !enabled); + break; + default: + break; + } +} + +static uint32_t cuatro_read_voltage_mV(void) { + return adc_get_mV(&(const adc_signal_t) ADC_CHANNEL_DEFAULT(ADC1, 8)) * 11U; +} + +static uint32_t cuatro_read_current_mA(void) { + return adc_get_mV(&(const adc_signal_t) ADC_CHANNEL_DEFAULT(ADC1, 3)) * 2U; +} + +static void cuatro_set_fan_enabled(bool enabled) { + set_gpio_output(GPIOD, 3, !enabled); +} + +static void cuatro_set_bootkick(BootState state) { + set_gpio_output(GPIOA, 0, state != BOOT_BOOTKICK); +} + +static void cuatro_set_amp_enabled(bool enabled) { + set_gpio_output(GPIOB, 0, enabled); +} + +static void cuatro_init(void) { + common_init_gpio(); + + // open drain + set_gpio_output_type(GPIOD, 3, OUTPUT_TYPE_OPEN_DRAIN); // FAN_EN + set_gpio_output_type(GPIOC, 12, OUTPUT_TYPE_OPEN_DRAIN); // VBAT_EN + + // Power readout + set_gpio_mode(GPIOC, 5, MODE_ANALOG); + set_gpio_mode(GPIOA, 6, MODE_ANALOG); + + // CAN transceiver enables + set_gpio_pullup(GPIOB, 7, PULL_NONE); + set_gpio_mode(GPIOB, 7, MODE_OUTPUT); + set_gpio_pullup(GPIOD, 8, PULL_NONE); + set_gpio_mode(GPIOD, 8, MODE_OUTPUT); + + // FDCAN3, different pins on this package than the rest of the reds + set_gpio_pullup(GPIOD, 12, PULL_NONE); + set_gpio_alternate(GPIOD, 12, GPIO_AF5_FDCAN3); + set_gpio_pullup(GPIOD, 13, PULL_NONE); + set_gpio_alternate(GPIOD, 13, GPIO_AF5_FDCAN3); + + // C2: SOM GPIO used as input (fan control at boot) + set_gpio_mode(GPIOC, 2, MODE_INPUT); + set_gpio_pullup(GPIOC, 2, PULL_DOWN); + + // SOM bootkick + reset lines + cuatro_set_bootkick(BOOT_BOOTKICK); + + // SOM debugging UART + gpio_uart7_init(); + +#ifndef BOOTSTUB + uart_init(&uart_ring_som_debug, 115200); +#endif + + // fan setup + set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); + register_set_bits(&(GPIOC->OTYPER), GPIO_OTYPER_OT8); // open drain + + // Clock source + clock_source_init(true); + + // Sound codec + cuatro_set_amp_enabled(false); + set_gpio_alternate(GPIOA, 2, GPIO_AF8_SAI4); // SAI4_SCK_B + set_gpio_alternate(GPIOC, 0, GPIO_AF8_SAI4); // SAI4_FS_B + set_gpio_alternate(GPIOD, 11, GPIO_AF10_SAI4); // SAI4_SD_A + set_gpio_alternate(GPIOE, 3, GPIO_AF8_SAI4); // SAI4_SD_B + set_gpio_alternate(GPIOE, 4, GPIO_AF3_DFSDM1); // DFSDM1_DATIN3 + set_gpio_alternate(GPIOE, 9, GPIO_AF3_DFSDM1); // DFSDM1_CKOUT + set_gpio_alternate(GPIOE, 6, GPIO_AF10_SAI4); // SAI4_MCLK_B + sound_init(); +} + +static harness_configuration cuatro_harness_config = { + .GPIO_SBU1 = GPIOC, + .GPIO_SBU2 = GPIOA, + .GPIO_relay_SBU1 = GPIOA, + .GPIO_relay_SBU2 = GPIOA, + .pin_SBU1 = 4, + .pin_SBU2 = 1, + .pin_relay_SBU1 = 9, + .pin_relay_SBU2 = 3, + .adc_signal_SBU1 = ADC_CHANNEL_DEFAULT(ADC1, 4), + .adc_signal_SBU2 = ADC_CHANNEL_DEFAULT(ADC1, 17) +}; + +// TODO +void tres_set_can_mode(uint8_t mode); +bool tres_read_som_gpio (void); + +struct board board_cuatro = { + .harness_config = &cuatro_harness_config, + .has_spi = true, + .has_fan = true, + .avdd_mV = 1800U, + .fan_enable_cooldown_time = 3U, + .init = cuatro_init, + .init_bootloader = unused_init_bootloader, + .enable_can_transceiver = cuatro_enable_can_transceiver, + .led_GPIO = {GPIOC, GPIOC, GPIOC}, + .led_pin = {6, 7, 9}, + .led_pwm_channels = {1, 2, 4}, + .set_can_mode = tres_set_can_mode, + .read_voltage_mV = cuatro_read_voltage_mV, + .read_current_mA = cuatro_read_current_mA, + .set_fan_enabled = cuatro_set_fan_enabled, + .set_ir_power = unused_set_ir_power, + .set_siren = fake_siren_set, + .set_bootkick = cuatro_set_bootkick, + .read_som_gpio = tres_read_som_gpio, + .set_amp_enabled = cuatro_set_amp_enabled +}; diff --git a/board/boards/cuatro.h b/board/boards/cuatro.h index 0c34174cf3d..58eb32e1cf5 100644 --- a/board/boards/cuatro.h +++ b/board/boards/cuatro.h @@ -6,130 +6,4 @@ // Cuatro (STM32H7) + Harness // // ////////////////////////// // -static void cuatro_enable_can_transceiver(uint8_t transceiver, bool enabled) { - switch (transceiver) { - case 1U: - set_gpio_output(GPIOB, 7, !enabled); - break; - case 2U: - set_gpio_output(GPIOB, 10, !enabled); - break; - case 3U: - set_gpio_output(GPIOD, 8, !enabled); - break; - case 4U: - set_gpio_output(GPIOB, 11, !enabled); - break; - default: - break; - } -} - -static uint32_t cuatro_read_voltage_mV(void) { - return adc_get_mV(&(const adc_signal_t) ADC_CHANNEL_DEFAULT(ADC1, 8)) * 11U; -} - -static uint32_t cuatro_read_current_mA(void) { - return adc_get_mV(&(const adc_signal_t) ADC_CHANNEL_DEFAULT(ADC1, 3)) * 2U; -} - -static void cuatro_set_fan_enabled(bool enabled) { - set_gpio_output(GPIOD, 3, !enabled); -} - -static void cuatro_set_bootkick(BootState state) { - set_gpio_output(GPIOA, 0, state != BOOT_BOOTKICK); -} - -static void cuatro_set_amp_enabled(bool enabled) { - set_gpio_output(GPIOB, 0, enabled); -} - -static void cuatro_init(void) { - common_init_gpio(); - - // open drain - set_gpio_output_type(GPIOD, 3, OUTPUT_TYPE_OPEN_DRAIN); // FAN_EN - set_gpio_output_type(GPIOC, 12, OUTPUT_TYPE_OPEN_DRAIN); // VBAT_EN - - // Power readout - set_gpio_mode(GPIOC, 5, MODE_ANALOG); - set_gpio_mode(GPIOA, 6, MODE_ANALOG); - - // CAN transceiver enables - set_gpio_pullup(GPIOB, 7, PULL_NONE); - set_gpio_mode(GPIOB, 7, MODE_OUTPUT); - set_gpio_pullup(GPIOD, 8, PULL_NONE); - set_gpio_mode(GPIOD, 8, MODE_OUTPUT); - - // FDCAN3, different pins on this package than the rest of the reds - set_gpio_pullup(GPIOD, 12, PULL_NONE); - set_gpio_alternate(GPIOD, 12, GPIO_AF5_FDCAN3); - set_gpio_pullup(GPIOD, 13, PULL_NONE); - set_gpio_alternate(GPIOD, 13, GPIO_AF5_FDCAN3); - - // C2: SOM GPIO used as input (fan control at boot) - set_gpio_mode(GPIOC, 2, MODE_INPUT); - set_gpio_pullup(GPIOC, 2, PULL_DOWN); - - // SOM bootkick + reset lines - cuatro_set_bootkick(BOOT_BOOTKICK); - - // SOM debugging UART - gpio_uart7_init(); - uart_init(&uart_ring_som_debug, 115200); - - // fan setup - set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); - register_set_bits(&(GPIOC->OTYPER), GPIO_OTYPER_OT8); // open drain - - // Clock source - clock_source_init(true); - - // Sound codec - cuatro_set_amp_enabled(false); - set_gpio_alternate(GPIOA, 2, GPIO_AF8_SAI4); // SAI4_SCK_B - set_gpio_alternate(GPIOC, 0, GPIO_AF8_SAI4); // SAI4_FS_B - set_gpio_alternate(GPIOD, 11, GPIO_AF10_SAI4); // SAI4_SD_A - set_gpio_alternate(GPIOE, 3, GPIO_AF8_SAI4); // SAI4_SD_B - set_gpio_alternate(GPIOE, 4, GPIO_AF3_DFSDM1); // DFSDM1_DATIN3 - set_gpio_alternate(GPIOE, 9, GPIO_AF3_DFSDM1); // DFSDM1_CKOUT - set_gpio_alternate(GPIOE, 6, GPIO_AF10_SAI4); // SAI4_MCLK_B - sound_init(); -} - -static harness_configuration cuatro_harness_config = { - .GPIO_SBU1 = GPIOC, - .GPIO_SBU2 = GPIOA, - .GPIO_relay_SBU1 = GPIOA, - .GPIO_relay_SBU2 = GPIOA, - .pin_SBU1 = 4, - .pin_SBU2 = 1, - .pin_relay_SBU1 = 9, - .pin_relay_SBU2 = 3, - .adc_signal_SBU1 = ADC_CHANNEL_DEFAULT(ADC1, 4), - .adc_signal_SBU2 = ADC_CHANNEL_DEFAULT(ADC1, 17) -}; - -board board_cuatro = { - .harness_config = &cuatro_harness_config, - .has_spi = true, - .has_fan = true, - .avdd_mV = 1800U, - .fan_enable_cooldown_time = 3U, - .init = cuatro_init, - .init_bootloader = unused_init_bootloader, - .enable_can_transceiver = cuatro_enable_can_transceiver, - .led_GPIO = {GPIOC, GPIOC, GPIOC}, - .led_pin = {6, 7, 9}, - .led_pwm_channels = {1, 2, 4}, - .set_can_mode = tres_set_can_mode, - .read_voltage_mV = cuatro_read_voltage_mV, - .read_current_mA = cuatro_read_current_mA, - .set_fan_enabled = cuatro_set_fan_enabled, - .set_ir_power = unused_set_ir_power, - .set_siren = fake_siren_set, - .set_bootkick = cuatro_set_bootkick, - .read_som_gpio = tres_read_som_gpio, - .set_amp_enabled = cuatro_set_amp_enabled -}; +extern struct board board_cuatro; \ No newline at end of file diff --git a/board/boards/red.c b/board/boards/red.c new file mode 100644 index 00000000000..9aadcf04c62 --- /dev/null +++ b/board/boards/red.c @@ -0,0 +1,134 @@ +#include "red.h" + +#include "board/boards/board_declarations.h" + +// ///////////////////////////// // +// Red Panda (STM32H7) + Harness // +// ///////////////////////////// // + +void red_enable_can_transceiver(uint8_t transceiver, bool enabled) { + switch (transceiver) { + case 1U: + set_gpio_output(GPIOG, 11, !enabled); + break; + case 2U: + set_gpio_output(GPIOB, 3, !enabled); + break; + case 3U: + set_gpio_output(GPIOD, 7, !enabled); + break; + case 4U: + set_gpio_output(GPIOB, 4, !enabled); + break; + default: + break; + } +} + +void red_set_can_mode(uint8_t mode) { + red_enable_can_transceiver(2U, false); + red_enable_can_transceiver(4U, false); + switch (mode) { + case CAN_MODE_NORMAL: + case CAN_MODE_OBD_CAN2: + if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(harness.status == HARNESS_STATUS_FLIPPED)) { + // B12,B13: disable normal mode + set_gpio_pullup(GPIOB, 12, PULL_NONE); + set_gpio_mode(GPIOB, 12, MODE_ANALOG); + + set_gpio_pullup(GPIOB, 13, PULL_NONE); + set_gpio_mode(GPIOB, 13, MODE_ANALOG); + + // B5,B6: FDCAN2 mode + set_gpio_pullup(GPIOB, 5, PULL_NONE); + set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2); + + set_gpio_pullup(GPIOB, 6, PULL_NONE); + set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2); + red_enable_can_transceiver(2U, true); + } else { + // B5,B6: disable normal mode + set_gpio_pullup(GPIOB, 5, PULL_NONE); + set_gpio_mode(GPIOB, 5, MODE_ANALOG); + + set_gpio_pullup(GPIOB, 6, PULL_NONE); + set_gpio_mode(GPIOB, 6, MODE_ANALOG); + // B12,B13: FDCAN2 mode + set_gpio_pullup(GPIOB, 12, PULL_NONE); + set_gpio_alternate(GPIOB, 12, GPIO_AF9_FDCAN2); + + set_gpio_pullup(GPIOB, 13, PULL_NONE); + set_gpio_alternate(GPIOB, 13, GPIO_AF9_FDCAN2); + red_enable_can_transceiver(4U, true); + } + break; + default: + break; + } +} + +uint32_t red_read_voltage_mV(void){ + return adc_get_mV(&(const adc_signal_t) ADC_CHANNEL_DEFAULT(ADC1, 2)) * 11U; +} + +void red_init(void) { + common_init_gpio(); + + // G11,B3,D7,B4: transceiver enable + set_gpio_pullup(GPIOG, 11, PULL_NONE); + set_gpio_mode(GPIOG, 11, MODE_OUTPUT); + + set_gpio_pullup(GPIOB, 3, PULL_NONE); + set_gpio_mode(GPIOB, 3, MODE_OUTPUT); + + set_gpio_pullup(GPIOD, 7, PULL_NONE); + set_gpio_mode(GPIOD, 7, MODE_OUTPUT); + + set_gpio_pullup(GPIOB, 4, PULL_NONE); + set_gpio_mode(GPIOB, 4, MODE_OUTPUT); + + //B1: 5VOUT_S + set_gpio_pullup(GPIOB, 1, PULL_NONE); + set_gpio_mode(GPIOB, 1, MODE_ANALOG); + + // B14: usb load switch, enabled by pull resistor on board, obsolete for red panda + set_gpio_output_type(GPIOB, 14, OUTPUT_TYPE_OPEN_DRAIN); + set_gpio_pullup(GPIOB, 14, PULL_UP); + set_gpio_mode(GPIOB, 14, MODE_OUTPUT); + set_gpio_output(GPIOB, 14, 1); +} + +harness_configuration red_harness_config = { + .GPIO_SBU1 = GPIOC, + .GPIO_SBU2 = GPIOA, + .GPIO_relay_SBU1 = GPIOC, + .GPIO_relay_SBU2 = GPIOC, + .pin_SBU1 = 4, + .pin_SBU2 = 1, + .pin_relay_SBU1 = 10, + .pin_relay_SBU2 = 11, + .adc_signal_SBU1 = ADC_CHANNEL_DEFAULT(ADC1, 4), + .adc_signal_SBU2 = ADC_CHANNEL_DEFAULT(ADC1, 17) +}; + +struct board board_red = { + .set_bootkick = unused_set_bootkick, + .harness_config = &red_harness_config, + .has_spi = false, + .has_fan = false, + .avdd_mV = 3300U, + .fan_enable_cooldown_time = 0U, + .init = red_init, + .init_bootloader = unused_init_bootloader, + .enable_can_transceiver = red_enable_can_transceiver, + .led_GPIO = {GPIOE, GPIOE, GPIOE}, + .led_pin = {4, 3, 2}, + .set_can_mode = red_set_can_mode, + .read_voltage_mV = red_read_voltage_mV, + .read_current_mA = unused_read_current, + .set_fan_enabled = unused_set_fan_enabled, + .set_ir_power = unused_set_ir_power, + .set_siren = unused_set_siren, + .read_som_gpio = unused_read_som_gpio, + .set_amp_enabled = unused_set_amp_enabled +}; diff --git a/board/boards/red.h b/board/boards/red.h index 761c2799ec8..a143cc9f994 100644 --- a/board/boards/red.h +++ b/board/boards/red.h @@ -1,134 +1,11 @@ #pragma once #include "board_declarations.h" +#include "board/drivers/harness.h" // ///////////////////////////// // // Red Panda (STM32H7) + Harness // // ///////////////////////////// // -static void red_enable_can_transceiver(uint8_t transceiver, bool enabled) { - switch (transceiver) { - case 1U: - set_gpio_output(GPIOG, 11, !enabled); - break; - case 2U: - set_gpio_output(GPIOB, 3, !enabled); - break; - case 3U: - set_gpio_output(GPIOD, 7, !enabled); - break; - case 4U: - set_gpio_output(GPIOB, 4, !enabled); - break; - default: - break; - } -} - -static void red_set_can_mode(uint8_t mode) { - red_enable_can_transceiver(2U, false); - red_enable_can_transceiver(4U, false); - switch (mode) { - case CAN_MODE_NORMAL: - case CAN_MODE_OBD_CAN2: - if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(harness.status == HARNESS_STATUS_FLIPPED)) { - // B12,B13: disable normal mode - set_gpio_pullup(GPIOB, 12, PULL_NONE); - set_gpio_mode(GPIOB, 12, MODE_ANALOG); - - set_gpio_pullup(GPIOB, 13, PULL_NONE); - set_gpio_mode(GPIOB, 13, MODE_ANALOG); - - // B5,B6: FDCAN2 mode - set_gpio_pullup(GPIOB, 5, PULL_NONE); - set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2); - - set_gpio_pullup(GPIOB, 6, PULL_NONE); - set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2); - red_enable_can_transceiver(2U, true); - } else { - // B5,B6: disable normal mode - set_gpio_pullup(GPIOB, 5, PULL_NONE); - set_gpio_mode(GPIOB, 5, MODE_ANALOG); - - set_gpio_pullup(GPIOB, 6, PULL_NONE); - set_gpio_mode(GPIOB, 6, MODE_ANALOG); - // B12,B13: FDCAN2 mode - set_gpio_pullup(GPIOB, 12, PULL_NONE); - set_gpio_alternate(GPIOB, 12, GPIO_AF9_FDCAN2); - - set_gpio_pullup(GPIOB, 13, PULL_NONE); - set_gpio_alternate(GPIOB, 13, GPIO_AF9_FDCAN2); - red_enable_can_transceiver(4U, true); - } - break; - default: - break; - } -} - -static uint32_t red_read_voltage_mV(void){ - return adc_get_mV(&(const adc_signal_t) ADC_CHANNEL_DEFAULT(ADC1, 2)) * 11U; -} - -static void red_init(void) { - common_init_gpio(); - - // G11,B3,D7,B4: transceiver enable - set_gpio_pullup(GPIOG, 11, PULL_NONE); - set_gpio_mode(GPIOG, 11, MODE_OUTPUT); - - set_gpio_pullup(GPIOB, 3, PULL_NONE); - set_gpio_mode(GPIOB, 3, MODE_OUTPUT); - - set_gpio_pullup(GPIOD, 7, PULL_NONE); - set_gpio_mode(GPIOD, 7, MODE_OUTPUT); - - set_gpio_pullup(GPIOB, 4, PULL_NONE); - set_gpio_mode(GPIOB, 4, MODE_OUTPUT); - - //B1: 5VOUT_S - set_gpio_pullup(GPIOB, 1, PULL_NONE); - set_gpio_mode(GPIOB, 1, MODE_ANALOG); - - // B14: usb load switch, enabled by pull resistor on board, obsolete for red panda - set_gpio_output_type(GPIOB, 14, OUTPUT_TYPE_OPEN_DRAIN); - set_gpio_pullup(GPIOB, 14, PULL_UP); - set_gpio_mode(GPIOB, 14, MODE_OUTPUT); - set_gpio_output(GPIOB, 14, 1); -} - -static harness_configuration red_harness_config = { - .GPIO_SBU1 = GPIOC, - .GPIO_SBU2 = GPIOA, - .GPIO_relay_SBU1 = GPIOC, - .GPIO_relay_SBU2 = GPIOC, - .pin_SBU1 = 4, - .pin_SBU2 = 1, - .pin_relay_SBU1 = 10, - .pin_relay_SBU2 = 11, - .adc_signal_SBU1 = ADC_CHANNEL_DEFAULT(ADC1, 4), - .adc_signal_SBU2 = ADC_CHANNEL_DEFAULT(ADC1, 17) -}; - -board board_red = { - .set_bootkick = unused_set_bootkick, - .harness_config = &red_harness_config, - .has_spi = false, - .has_fan = false, - .avdd_mV = 3300U, - .fan_enable_cooldown_time = 0U, - .init = red_init, - .init_bootloader = unused_init_bootloader, - .enable_can_transceiver = red_enable_can_transceiver, - .led_GPIO = {GPIOE, GPIOE, GPIOE}, - .led_pin = {4, 3, 2}, - .set_can_mode = red_set_can_mode, - .read_voltage_mV = red_read_voltage_mV, - .read_current_mA = unused_read_current, - .set_fan_enabled = unused_set_fan_enabled, - .set_ir_power = unused_set_ir_power, - .set_siren = unused_set_siren, - .read_som_gpio = unused_read_som_gpio, - .set_amp_enabled = unused_set_amp_enabled -}; +extern struct harness_configuration red_harness_config; +extern struct board board_red; \ No newline at end of file diff --git a/board/boards/tres.c b/board/boards/tres.c new file mode 100644 index 00000000000..918c81ce243 --- /dev/null +++ b/board/boards/tres.c @@ -0,0 +1,180 @@ +#include "tres.h" + +#include "board/drivers/pwm.h" +#include "board/globals.h" + +// /////////////////////////// +// Tres (STM32H7) + Harness // +// /////////////////////////// + +static bool tres_ir_enabled; +static bool tres_fan_enabled; +static void tres_update_fan_ir_power(void) { + set_gpio_output(GPIOD, 3, tres_ir_enabled || tres_fan_enabled); +} + +static void tres_set_ir_power(uint8_t percentage){ + tres_ir_enabled = (percentage > 0U); + tres_update_fan_ir_power(); + pwm_set(TIM3, 4, percentage); +} + +static void tres_set_bootkick(BootState state) { + set_gpio_output(GPIOA, 0, state != BOOT_BOOTKICK); + set_gpio_output(GPIOC, 12, state != BOOT_RESET); +} + +static void tres_set_fan_enabled(bool enabled) { + // NOTE: fan controller reset doesn't work on a tres if IR is enabled + tres_fan_enabled = enabled; + tres_update_fan_ir_power(); +} + +static void tres_enable_can_transceiver(uint8_t transceiver, bool enabled) { + static bool can0_enabled = false; + static bool can2_enabled = false; + + switch (transceiver) { + case 1U: + can0_enabled = enabled; + break; + case 2U: + set_gpio_output(GPIOB, 10, !enabled); + break; + case 3U: + can2_enabled = enabled; + break; + case 4U: + set_gpio_output(GPIOB, 11, !enabled); + break; + default: + break; + } + + // CAN0 and 2 are tied, so enable both if either is enabled + set_gpio_output(GPIOG, 11, !(can0_enabled || can2_enabled)); + set_gpio_output(GPIOD, 7, !(can0_enabled || can2_enabled)); +} + +void tres_set_can_mode(uint8_t mode) { + current_board->enable_can_transceiver(2U, false); + current_board->enable_can_transceiver(4U, false); + switch (mode) { + case CAN_MODE_NORMAL: + case CAN_MODE_OBD_CAN2: + if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(harness.status == HARNESS_STATUS_FLIPPED)) { + // B12,B13: disable normal mode + set_gpio_pullup(GPIOB, 12, PULL_NONE); + set_gpio_mode(GPIOB, 12, MODE_ANALOG); + + set_gpio_pullup(GPIOB, 13, PULL_NONE); + set_gpio_mode(GPIOB, 13, MODE_ANALOG); + + // B5,B6: FDCAN2 mode + set_gpio_pullup(GPIOB, 5, PULL_NONE); + set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2); + + set_gpio_pullup(GPIOB, 6, PULL_NONE); + set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2); + current_board->enable_can_transceiver(2U, true); + } else { + // B5,B6: disable normal mode + set_gpio_pullup(GPIOB, 5, PULL_NONE); + set_gpio_mode(GPIOB, 5, MODE_ANALOG); + + set_gpio_pullup(GPIOB, 6, PULL_NONE); + set_gpio_mode(GPIOB, 6, MODE_ANALOG); + // B12,B13: FDCAN2 mode + set_gpio_pullup(GPIOB, 12, PULL_NONE); + set_gpio_alternate(GPIOB, 12, GPIO_AF9_FDCAN2); + + set_gpio_pullup(GPIOB, 13, PULL_NONE); + set_gpio_alternate(GPIOB, 13, GPIO_AF9_FDCAN2); + current_board->enable_can_transceiver(4U, true); + } + break; + default: + break; + } +} + +bool tres_read_som_gpio (void) { + return (get_gpio_input(GPIOC, 2) != 0); +} + +static void tres_init(void) { + // Enable USB 3.3V LDO for USB block + register_set_bits(&(PWR->CR3), PWR_CR3_USBREGEN); + register_set_bits(&(PWR->CR3), PWR_CR3_USB33DEN); + while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0U); + + common_init_gpio(); + + // C2: SOM GPIO used as input (fan control at boot) + set_gpio_mode(GPIOC, 2, MODE_INPUT); + set_gpio_pullup(GPIOC, 2, PULL_DOWN); + + // SOM bootkick + reset lines + // WARNING: make sure output state is set before configuring as output + tres_set_bootkick(BOOT_BOOTKICK); + set_gpio_mode(GPIOC, 12, MODE_OUTPUT); + + // SOM debugging UART + gpio_uart7_init(); +#ifndef BOOTSTUB + uart_init(&uart_ring_som_debug, 115200); +#endif + + // fan setup + set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); + + // Initialize IR PWM and set to 0% + set_gpio_alternate(GPIOC, 9, GPIO_AF2_TIM3); + pwm_init(TIM3, 4); + tres_set_ir_power(0U); + + // Fake siren + set_gpio_alternate(GPIOC, 10, GPIO_AF4_I2C5); + set_gpio_alternate(GPIOC, 11, GPIO_AF4_I2C5); + register_set_bits(&(GPIOC->OTYPER), GPIO_OTYPER_OT10 | GPIO_OTYPER_OT11); // open drain + + // Clock source + clock_source_init(false); +} + +static harness_configuration tres_harness_config = { + .GPIO_SBU1 = GPIOC, + .GPIO_SBU2 = GPIOA, + .GPIO_relay_SBU1 = GPIOA, + .GPIO_relay_SBU2 = GPIOA, + .pin_SBU1 = 4, + .pin_SBU2 = 1, + .pin_relay_SBU1 = 8, + .pin_relay_SBU2 = 3, + .adc_signal_SBU1 = ADC_CHANNEL_DEFAULT(ADC1, 4), + .adc_signal_SBU2 = ADC_CHANNEL_DEFAULT(ADC1, 17) +}; + +uint32_t red_read_voltage_mV(void); + +struct board board_tres = { + .harness_config = &tres_harness_config, + .has_spi = true, + .has_fan = true, + .avdd_mV = 1800U, + .fan_enable_cooldown_time = 3U, + .init = tres_init, + .init_bootloader = unused_init_bootloader, + .enable_can_transceiver = tres_enable_can_transceiver, + .led_GPIO = {GPIOE, GPIOE, GPIOE}, + .led_pin = {4, 3, 2}, + .set_can_mode = tres_set_can_mode, + .read_voltage_mV = red_read_voltage_mV, + .read_current_mA = unused_read_current, + .set_fan_enabled = tres_set_fan_enabled, + .set_ir_power = tres_set_ir_power, + .set_siren = fake_i2c_siren_set, + .set_bootkick = tres_set_bootkick, + .read_som_gpio = tres_read_som_gpio, + .set_amp_enabled = unused_set_amp_enabled +}; diff --git a/board/boards/tres.h b/board/boards/tres.h index 653df969925..d6b7c687cbb 100644 --- a/board/boards/tres.h +++ b/board/boards/tres.h @@ -5,171 +5,4 @@ // /////////////////////////// // Tres (STM32H7) + Harness // // /////////////////////////// - -static bool tres_ir_enabled; -static bool tres_fan_enabled; -static void tres_update_fan_ir_power(void) { - set_gpio_output(GPIOD, 3, tres_ir_enabled || tres_fan_enabled); -} - -static void tres_set_ir_power(uint8_t percentage){ - tres_ir_enabled = (percentage > 0U); - tres_update_fan_ir_power(); - pwm_set(TIM3, 4, percentage); -} - -static void tres_set_bootkick(BootState state) { - set_gpio_output(GPIOA, 0, state != BOOT_BOOTKICK); - set_gpio_output(GPIOC, 12, state != BOOT_RESET); -} - -static void tres_set_fan_enabled(bool enabled) { - // NOTE: fan controller reset doesn't work on a tres if IR is enabled - tres_fan_enabled = enabled; - tres_update_fan_ir_power(); -} - -static void tres_enable_can_transceiver(uint8_t transceiver, bool enabled) { - static bool can0_enabled = false; - static bool can2_enabled = false; - - switch (transceiver) { - case 1U: - can0_enabled = enabled; - break; - case 2U: - set_gpio_output(GPIOB, 10, !enabled); - break; - case 3U: - can2_enabled = enabled; - break; - case 4U: - set_gpio_output(GPIOB, 11, !enabled); - break; - default: - break; - } - - // CAN0 and 2 are tied, so enable both if either is enabled - set_gpio_output(GPIOG, 11, !(can0_enabled || can2_enabled)); - set_gpio_output(GPIOD, 7, !(can0_enabled || can2_enabled)); -} - -static void tres_set_can_mode(uint8_t mode) { - current_board->enable_can_transceiver(2U, false); - current_board->enable_can_transceiver(4U, false); - switch (mode) { - case CAN_MODE_NORMAL: - case CAN_MODE_OBD_CAN2: - if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(harness.status == HARNESS_STATUS_FLIPPED)) { - // B12,B13: disable normal mode - set_gpio_pullup(GPIOB, 12, PULL_NONE); - set_gpio_mode(GPIOB, 12, MODE_ANALOG); - - set_gpio_pullup(GPIOB, 13, PULL_NONE); - set_gpio_mode(GPIOB, 13, MODE_ANALOG); - - // B5,B6: FDCAN2 mode - set_gpio_pullup(GPIOB, 5, PULL_NONE); - set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2); - - set_gpio_pullup(GPIOB, 6, PULL_NONE); - set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2); - current_board->enable_can_transceiver(2U, true); - } else { - // B5,B6: disable normal mode - set_gpio_pullup(GPIOB, 5, PULL_NONE); - set_gpio_mode(GPIOB, 5, MODE_ANALOG); - - set_gpio_pullup(GPIOB, 6, PULL_NONE); - set_gpio_mode(GPIOB, 6, MODE_ANALOG); - // B12,B13: FDCAN2 mode - set_gpio_pullup(GPIOB, 12, PULL_NONE); - set_gpio_alternate(GPIOB, 12, GPIO_AF9_FDCAN2); - - set_gpio_pullup(GPIOB, 13, PULL_NONE); - set_gpio_alternate(GPIOB, 13, GPIO_AF9_FDCAN2); - current_board->enable_can_transceiver(4U, true); - } - break; - default: - break; - } -} - -static bool tres_read_som_gpio (void) { - return (get_gpio_input(GPIOC, 2) != 0); -} - -static void tres_init(void) { - // Enable USB 3.3V LDO for USB block - register_set_bits(&(PWR->CR3), PWR_CR3_USBREGEN); - register_set_bits(&(PWR->CR3), PWR_CR3_USB33DEN); - while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0U); - - common_init_gpio(); - - // C2: SOM GPIO used as input (fan control at boot) - set_gpio_mode(GPIOC, 2, MODE_INPUT); - set_gpio_pullup(GPIOC, 2, PULL_DOWN); - - // SOM bootkick + reset lines - // WARNING: make sure output state is set before configuring as output - tres_set_bootkick(BOOT_BOOTKICK); - set_gpio_mode(GPIOC, 12, MODE_OUTPUT); - - // SOM debugging UART - gpio_uart7_init(); - uart_init(&uart_ring_som_debug, 115200); - - // fan setup - set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); - - // Initialize IR PWM and set to 0% - set_gpio_alternate(GPIOC, 9, GPIO_AF2_TIM3); - pwm_init(TIM3, 4); - tres_set_ir_power(0U); - - // Fake siren - set_gpio_alternate(GPIOC, 10, GPIO_AF4_I2C5); - set_gpio_alternate(GPIOC, 11, GPIO_AF4_I2C5); - register_set_bits(&(GPIOC->OTYPER), GPIO_OTYPER_OT10 | GPIO_OTYPER_OT11); // open drain - - // Clock source - clock_source_init(false); -} - -static harness_configuration tres_harness_config = { - .GPIO_SBU1 = GPIOC, - .GPIO_SBU2 = GPIOA, - .GPIO_relay_SBU1 = GPIOA, - .GPIO_relay_SBU2 = GPIOA, - .pin_SBU1 = 4, - .pin_SBU2 = 1, - .pin_relay_SBU1 = 8, - .pin_relay_SBU2 = 3, - .adc_signal_SBU1 = ADC_CHANNEL_DEFAULT(ADC1, 4), - .adc_signal_SBU2 = ADC_CHANNEL_DEFAULT(ADC1, 17) -}; - -board board_tres = { - .harness_config = &tres_harness_config, - .has_spi = true, - .has_fan = true, - .avdd_mV = 1800U, - .fan_enable_cooldown_time = 3U, - .init = tres_init, - .init_bootloader = unused_init_bootloader, - .enable_can_transceiver = tres_enable_can_transceiver, - .led_GPIO = {GPIOE, GPIOE, GPIOE}, - .led_pin = {4, 3, 2}, - .set_can_mode = tres_set_can_mode, - .read_voltage_mV = red_read_voltage_mV, - .read_current_mA = unused_read_current, - .set_fan_enabled = tres_set_fan_enabled, - .set_ir_power = tres_set_ir_power, - .set_siren = fake_i2c_siren_set, - .set_bootkick = tres_set_bootkick, - .read_som_gpio = tres_read_som_gpio, - .set_amp_enabled = unused_set_amp_enabled -}; +extern struct board board_tres; \ No newline at end of file diff --git a/board/boards/unused_funcs.c b/board/boards/unused_funcs.c new file mode 100644 index 00000000000..27d43cef512 --- /dev/null +++ b/board/boards/unused_funcs.c @@ -0,0 +1,32 @@ +#include "unused_funcs.h" + +void unused_init_bootloader(void) { +} + +void unused_set_ir_power(uint8_t percentage) { + UNUSED(percentage); +} + +void unused_set_fan_enabled(bool enabled) { + UNUSED(enabled); +} + +void unused_set_siren(bool enabled) { + UNUSED(enabled); +} + +uint32_t unused_read_current(void) { + return 0U; +} + +void unused_set_bootkick(BootState state) { + UNUSED(state); +} + +bool unused_read_som_gpio(void) { + return false; +} + +void unused_set_amp_enabled(bool enabled) { + UNUSED(enabled); +} diff --git a/board/boards/unused_funcs.h b/board/boards/unused_funcs.h index 588cf63654d..519f7906ed0 100644 --- a/board/boards/unused_funcs.h +++ b/board/boards/unused_funcs.h @@ -1,32 +1,12 @@ #pragma once -void unused_init_bootloader(void) { -} - -void unused_set_ir_power(uint8_t percentage) { - UNUSED(percentage); -} - -void unused_set_fan_enabled(bool enabled) { - UNUSED(enabled); -} - -void unused_set_siren(bool enabled) { - UNUSED(enabled); -} - -uint32_t unused_read_current(void) { - return 0U; -} - -void unused_set_bootkick(BootState state) { - UNUSED(state); -} - -bool unused_read_som_gpio(void) { - return false; -} - -void unused_set_amp_enabled(bool enabled) { - UNUSED(enabled); -} +#include "board_declarations.h" + +void unused_init_bootloader(void); +void unused_set_ir_power(uint8_t percentage); +void unused_set_fan_enabled(bool enabled); +void unused_set_siren(bool enabled); +uint32_t unused_read_current(void); +void unused_set_bootkick(BootState state); +bool unused_read_som_gpio(void); +void unused_set_amp_enabled(bool enabled); \ No newline at end of file diff --git a/board/body/boards/board_body.c b/board/body/boards/board_body.c new file mode 100644 index 00000000000..204273d921a --- /dev/null +++ b/board/body/boards/board_body.c @@ -0,0 +1,23 @@ +#include "board_body.h" + +#include "board/config.h" + +void board_body_init(void) { + motor_init(); + motor_encoder_init(); + motor_speed_controller_init(); + motor_encoder_reset(1); + motor_encoder_reset(2); + + // Initialize CAN pins + set_gpio_pullup(GPIOD, 0, PULL_NONE); + set_gpio_alternate(GPIOD, 0, GPIO_AF9_FDCAN1); + set_gpio_pullup(GPIOD, 1, PULL_NONE); + set_gpio_alternate(GPIOD, 1, GPIO_AF9_FDCAN1); +} + +struct board board_body = { + .led_GPIO = {GPIOC, GPIOC, GPIOC}, + .led_pin = {7, 7, 7}, + .init = board_body_init, +}; diff --git a/board/body/boards/board_body.h b/board/body/boards/board_body.h index a1eebfdb8d2..8d65e9928a3 100644 --- a/board/body/boards/board_body.h +++ b/board/body/boards/board_body.h @@ -1,21 +1,7 @@ -#include "board/body/motor_control.h" +#pragma once -void board_body_init(void) { - motor_init(); - motor_encoder_init(); - motor_speed_controller_init(); - motor_encoder_reset(1); - motor_encoder_reset(2); +#include "board/body/motor_control.h" - // Initialize CAN pins - set_gpio_pullup(GPIOD, 0, PULL_NONE); - set_gpio_alternate(GPIOD, 0, GPIO_AF9_FDCAN1); - set_gpio_pullup(GPIOD, 1, PULL_NONE); - set_gpio_alternate(GPIOD, 1, GPIO_AF9_FDCAN1); -} +void board_body_init(void); -board board_body = { - .led_GPIO = {GPIOC, GPIOC, GPIOC}, - .led_pin = {7, 7, 7}, - .init = board_body_init, -}; +extern struct board board_body; \ No newline at end of file diff --git a/board/body/motor_control.c b/board/body/motor_control.c new file mode 100644 index 00000000000..8be9078b7fc --- /dev/null +++ b/board/body/motor_control.c @@ -0,0 +1,251 @@ +#pragma once + +#include +#include + +#include "board/body/motor_common.h" +#include "board/body/motor_encoder.h" + +// Motor pin map: +// M1 drive: PB8 -> TIM16_CH1, PB9 -> TIM17_CH1, PE2/PE3 enables +// M2 drive: PA2 -> TIM15_CH1, PA3 -> TIM15_CH2, PE8/PE9 enables + +#define PWM_TIMER_CLOCK_HZ 120000000U +#define PWM_FREQUENCY_HZ 5000U +#define PWM_PERCENT_MAX 100 +#define PWM_RELOAD_TICKS ((PWM_TIMER_CLOCK_HZ + (PWM_FREQUENCY_HZ / 2U)) / PWM_FREQUENCY_HZ) + +#define KP 0.25f +#define KI 0.5f +#define KD 0.008f +#define KFF 0.9f +#define MAX_RPM 100.0f +#define OUTPUT_MAX 100.0f +#define DEADBAND_RPM 1.0f +#define UPDATE_PERIOD_US 1000U + +typedef struct { + TIM_TypeDef *forward_timer; + uint8_t forward_channel; + TIM_TypeDef *reverse_timer; + uint8_t reverse_channel; + GPIO_TypeDef *pwm_port[2]; + uint8_t pwm_pin[2]; + uint8_t pwm_af[2]; + GPIO_TypeDef *enable_port[2]; + uint8_t enable_pin[2]; +} motor_pwm_config_t; + +static const motor_pwm_config_t motor_pwm_config[BODY_MOTOR_COUNT] = { + [BODY_MOTOR_LEFT - 1U] = { + TIM16, 1U, TIM17, 1U, + {GPIOB, GPIOB}, {8U, 9U}, {GPIO_AF1_TIM16, GPIO_AF1_TIM17}, + {GPIOE, GPIOE}, {2U, 3U}, + }, + [BODY_MOTOR_RIGHT - 1U] = { + TIM15, 2U, TIM15, 1U, + {GPIOA, GPIOA}, {2U, 3U}, {GPIO_AF4_TIM15, GPIO_AF4_TIM15}, + {GPIOE, GPIOE}, {8U, 9U}, + }, +}; + +typedef struct { + bool active; + float target_rpm; + float integral; + float previous_error; + float last_output; + uint32_t last_update_us; +} motor_speed_state_t; + +static inline float motor_absf(float value) { + return (value < 0.0f) ? -value : value; +} + +static inline float motor_clampf(float value, float min_value, float max_value) { + if (value < min_value) { + return min_value; + } + if (value > max_value) { + return max_value; + } + return value; +} + +static motor_speed_state_t motor_speed_states[BODY_MOTOR_COUNT]; + +static inline void motor_pwm_write(TIM_TypeDef *timer, uint8_t channel, uint8_t percentage) { + uint32_t period = (timer->ARR != 0U) ? timer->ARR : PWM_RELOAD_TICKS; + uint16_t comp = (uint16_t)((period * (uint32_t)percentage) / 100U); + if (channel == 1U) { + register_set(&(timer->CCR1), comp, 0xFFFFU); + } else if (channel == 2U) { + register_set(&(timer->CCR2), comp, 0xFFFFU); + } +} + +static inline motor_speed_state_t *motor_speed_state_get(uint8_t motor) { + return body_motor_is_valid(motor) ? &motor_speed_states[motor - 1U] : NULL; +} + +static inline void motor_speed_state_reset(motor_speed_state_t *state) { + state->active = false; + state->target_rpm = 0.0f; + state->integral = 0.0f; + state->previous_error = 0.0f; + state->last_output = 0.0f; + state->last_update_us = 0U; +} + +static void motor_speed_controller_init(void) { + for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) { + motor_speed_state_reset(&motor_speed_states[i]); + } +} + +static void motor_speed_controller_disable(uint8_t motor) { + motor_speed_state_t *state = motor_speed_state_get(motor); + if (state == NULL) { + return; + } + motor_speed_state_reset(state); +} + +static inline void motor_write_enable(uint8_t motor_index, bool enable) { + const motor_pwm_config_t *cfg = &motor_pwm_config[motor_index]; + uint8_t level = enable ? 1U : 0U; + set_gpio_output(cfg->enable_port[0], cfg->enable_pin[0], level); + set_gpio_output(cfg->enable_port[1], cfg->enable_pin[1], level); +} + +static void motor_init(void) { + register_set_bits(&(RCC->AHB4ENR), RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN | RCC_AHB4ENR_GPIOEEN); + register_set_bits(&(RCC->APB2ENR), RCC_APB2ENR_TIM16EN | RCC_APB2ENR_TIM17EN | RCC_APB2ENR_TIM15EN); + + for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) { + const motor_pwm_config_t *cfg = &motor_pwm_config[i]; + motor_write_enable(i, false); + set_gpio_alternate(cfg->pwm_port[0], cfg->pwm_pin[0], cfg->pwm_af[0]); + set_gpio_alternate(cfg->pwm_port[1], cfg->pwm_pin[1], cfg->pwm_af[1]); + + pwm_init(cfg->forward_timer, cfg->forward_channel); + pwm_init(cfg->reverse_timer, cfg->reverse_channel); + + TIM_TypeDef *forward_timer = cfg->forward_timer; + register_set(&(forward_timer->PSC), 0U, 0xFFFFU); + register_set(&(forward_timer->ARR), PWM_RELOAD_TICKS, 0xFFFFU); + forward_timer->EGR |= TIM_EGR_UG; + register_set(&(forward_timer->BDTR), TIM_BDTR_MOE, 0xFFFFU); + + if (cfg->reverse_timer != cfg->forward_timer) { + TIM_TypeDef *reverse_timer = cfg->reverse_timer; + register_set(&(reverse_timer->PSC), 0U, 0xFFFFU); + register_set(&(reverse_timer->ARR), PWM_RELOAD_TICKS, 0xFFFFU); + reverse_timer->EGR |= TIM_EGR_UG; + register_set(&(reverse_timer->BDTR), TIM_BDTR_MOE, 0xFFFFU); + } + } +} + +static void motor_apply_pwm(uint8_t motor, int32_t speed_command) { + if (!body_motor_is_valid(motor)) { + return; + } + + int8_t speed = (int8_t)motor_clampf((float)speed_command, -(float)PWM_PERCENT_MAX, (float)PWM_PERCENT_MAX); + uint8_t pwm_value = (uint8_t)((speed < 0) ? -speed : speed); + uint8_t motor_index = motor - 1U; + motor_write_enable(motor_index, speed != 0); + const motor_pwm_config_t *cfg = &motor_pwm_config[motor_index]; + + if (speed > 0) { + motor_pwm_write(cfg->forward_timer, cfg->forward_channel, pwm_value); + motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, 0U); + } else if (speed < 0) { + motor_pwm_write(cfg->forward_timer, cfg->forward_channel, 0U); + motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, pwm_value); + } else { + motor_pwm_write(cfg->forward_timer, cfg->forward_channel, 0U); + motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, 0U); + } +} + +static inline void motor_set_speed(uint8_t motor, int8_t speed) { + motor_speed_controller_disable(motor); + motor_apply_pwm(motor, (int32_t)speed); +} + +static inline void motor_speed_controller_set_target_rpm(uint8_t motor, float target_rpm) { + motor_speed_state_t *state = motor_speed_state_get(motor); + if (state == NULL) { + return; + } + + target_rpm = motor_clampf(target_rpm, -MAX_RPM, MAX_RPM); + if (motor_absf(target_rpm) <= DEADBAND_RPM) { + motor_speed_controller_disable(motor); + motor_apply_pwm(motor, 0); + return; + } + + state->active = true; + state->target_rpm = target_rpm; + state->integral = 0.0f; + state->previous_error = target_rpm - motor_encoder_get_speed_rpm(motor); + state->last_output = 0.0f; + state->last_update_us = 0U; +} + +static inline void motor_speed_controller_update(uint32_t now_us) { + for (uint8_t motor = BODY_MOTOR_LEFT; motor <= BODY_MOTOR_RIGHT; motor++) { + motor_speed_state_t *state = motor_speed_state_get(motor); + if (!state->active) { + continue; + } + + bool first_update = (state->last_update_us == 0U); + uint32_t dt_us = first_update ? UPDATE_PERIOD_US : (now_us - state->last_update_us); + if (!first_update && (dt_us < UPDATE_PERIOD_US)) { + continue; + } + + float measured_rpm = motor_encoder_get_speed_rpm(motor); + float error = state->target_rpm - measured_rpm; + + if ((motor_absf(state->target_rpm) <= DEADBAND_RPM) && (motor_absf(error) <= DEADBAND_RPM) && (motor_absf(measured_rpm) <= DEADBAND_RPM)) { + motor_speed_controller_disable(motor); + motor_apply_pwm(motor, 0); + continue; + } + + float dt_s = (float)dt_us * 1.0e-6f; + float control = KFF * state->target_rpm; + + if (dt_s > 0.0f) { + state->integral += error * dt_s; + float derivative = first_update ? 0.0f : (error - state->previous_error) / dt_s; + + control += (KP * error) + (KI * state->integral) + (KD * derivative); + } else { + state->integral = 0.0f; + control += KP * error; + } + + if ((state->target_rpm > 0.0f) && (control < 0.0f)) { + control = 0.0f; + state->integral = 0.0f; + } else if ((state->target_rpm < 0.0f) && (control > 0.0f)) { + control = 0.0f; + state->integral = 0.0f; + } + + control = motor_clampf(control, -OUTPUT_MAX, OUTPUT_MAX); + + int32_t command = (control >= 0.0f) ? (int32_t)(control + 0.5f) : (int32_t)(control - 0.5f); + motor_apply_pwm(motor, command); + + state->previous_error = error; + state->last_output = control; + state->last_update_us = now_us; + } +} diff --git a/board/body/motor_control.h b/board/body/motor_control.h index 8be9078b7fc..f3b24420959 100644 --- a/board/body/motor_control.h +++ b/board/body/motor_control.h @@ -6,6 +6,8 @@ #include "board/body/motor_common.h" #include "board/body/motor_encoder.h" +#include "board/drivers/pwm.h" + // Motor pin map: // M1 drive: PB8 -> TIM16_CH1, PB9 -> TIM17_CH1, PE2/PE3 enables // M2 drive: PA2 -> TIM15_CH1, PA3 -> TIM15_CH2, PE8/PE9 enables diff --git a/board/body/motor_encoder.h b/board/body/motor_encoder.h index 383e23ef592..7c79f9f4d04 100644 --- a/board/body/motor_encoder.h +++ b/board/body/motor_encoder.h @@ -60,111 +60,3 @@ static motor_encoder_state_t motor_encoders[BODY_MOTOR_COUNT] = { { .config = &motor_encoder_config[1] }, }; -static void motor_encoder_configure_gpio(const motor_encoder_config_t *cfg) { - set_gpio_pullup(cfg->pin_a_port, cfg->pin_a, PULL_UP); - set_gpio_output_type(cfg->pin_a_port, cfg->pin_a, OUTPUT_TYPE_PUSH_PULL); - set_gpio_alternate(cfg->pin_a_port, cfg->pin_a, cfg->pin_a_af); - - set_gpio_pullup(cfg->pin_b_port, cfg->pin_b, PULL_UP); - set_gpio_output_type(cfg->pin_b_port, cfg->pin_b, OUTPUT_TYPE_PUSH_PULL); - set_gpio_alternate(cfg->pin_b_port, cfg->pin_b, cfg->pin_b_af); -} - -static void motor_encoder_configure_timer(motor_encoder_state_t *state) { - const motor_encoder_config_t *cfg = state->config; - TIM_TypeDef *timer = cfg->timer; - timer->CR1 = 0U; - timer->CR2 = 0U; - timer->SMCR = 0U; - timer->DIER = 0U; - timer->SR = 0U; - timer->CCMR1 = (TIM_CCMR1_CC1S_0) | (TIM_CCMR1_CC2S_0) | (cfg->filter << TIM_CCMR1_IC1F_Pos) | (cfg->filter << TIM_CCMR1_IC2F_Pos); - timer->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E; - timer->PSC = 0U; - timer->ARR = 0xFFFFU; - timer->CNT = 0U; - state->last_timer_count = 0U; - state->accumulated_count = 0; - state->last_speed_count = 0; - state->cached_speed_rps = 0.0f; - timer->SMCR = (TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1); - timer->CR1 = TIM_CR1_CEN; -} - -static void motor_encoder_init(void) { - register_set_bits(&(RCC->APB1LENR), RCC_APB1LENR_TIM4EN | RCC_APB1LENR_TIM3EN); - register_set_bits(&(RCC->APB1LRSTR), RCC_APB1LRSTR_TIM4RST | RCC_APB1LRSTR_TIM3RST); - register_clear_bits(&(RCC->APB1LRSTR), RCC_APB1LRSTR_TIM4RST | RCC_APB1LRSTR_TIM3RST); - - for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) { - motor_encoder_state_t *state = &motor_encoders[i]; - const motor_encoder_config_t *cfg = state->config; - motor_encoder_configure_gpio(cfg); - motor_encoder_configure_timer(state); - state->last_speed_timestamp_us = 0U; - } -} - -static inline int32_t motor_encoder_refresh(motor_encoder_state_t *state) { - const motor_encoder_config_t *cfg = state->config; - TIM_TypeDef *timer = cfg->timer; - uint16_t raw = (uint16_t)timer->CNT; - int32_t delta = (int16_t)(raw - state->last_timer_count); - delta *= cfg->direction; - state->last_timer_count = raw; - state->accumulated_count += delta; - return state->accumulated_count; -} - -static inline int32_t motor_encoder_get_position(uint8_t motor) { - if (!body_motor_is_valid(motor)) { - return 0; - } - motor_encoder_state_t *state = &motor_encoders[motor - 1U]; - return motor_encoder_refresh(state); -} - -static void motor_encoder_reset(uint8_t motor) { - if (!body_motor_is_valid(motor)) { - return; - } - motor_encoder_state_t *state = &motor_encoders[motor - 1U]; - state->config->timer->CNT = 0U; - state->last_timer_count = 0U; - state->accumulated_count = 0; - state->last_speed_count = 0; - state->last_speed_timestamp_us = 0U; - state->cached_speed_rps = 0.0f; -} - -static float motor_encoder_get_speed_rpm(uint8_t motor) { - if (!body_motor_is_valid(motor)) { - return 0.0f; - } - motor_encoder_state_t *state = &motor_encoders[motor - 1U]; - - const motor_encoder_config_t *cfg = state->config; - motor_encoder_refresh(state); - - uint32_t now = microsecond_timer_get(); - if (state->last_speed_timestamp_us == 0U) { - state->last_speed_timestamp_us = now; - state->last_speed_count = state->accumulated_count; - state->cached_speed_rps = 0.0f; - return 0.0f; - } - - uint32_t dt = now - state->last_speed_timestamp_us; - int32_t delta = state->accumulated_count - state->last_speed_count; - if ((dt < cfg->min_dt_us) || (delta == 0)) { - return state->cached_speed_rps * 60.0f; - } - - state->last_speed_count = state->accumulated_count; - state->last_speed_timestamp_us = now; - - float counts_per_second = ((float)delta * 1000000.0f) / (float)dt; - float new_speed_rps = (cfg->counts_per_output_rev != 0U) ? (counts_per_second / (float)cfg->counts_per_output_rev) : 0.0f; - state->cached_speed_rps += cfg->speed_alpha * (new_speed_rps - state->cached_speed_rps); - return state->cached_speed_rps * 60.0f; -} diff --git a/board/body/stm32h7/board.h b/board/body/stm32h7/board.h index c063e4ddbb5..1bcd0ffaa10 100644 --- a/board/body/stm32h7/board.h +++ b/board/body/stm32h7/board.h @@ -1,3 +1,5 @@ +#pragma once + #include "board/body/boards/board_declarations.h" #include "board/body/boards/board_body.h" diff --git a/board/bootstub.c b/board/bootstub.c index a48f308ac7d..1430bd5b616 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -2,6 +2,10 @@ #define MIN_VERSION 2 // ********************* Includes ********************* +#include + +#include "board/bootstub_declarations.h" + #include "board/config.h" #include "board/drivers/led.h" @@ -18,11 +22,25 @@ #include "board/obj/gitversion.h" #include "board/flasher.h" +// TODO +uint8_t hw_type; +board *current_board; +struct harness_t harness; + // cppcheck-suppress unusedFunction ; used in headers not included in cppcheck void __initialize_hardware_early(void) { early_initialization(); } +// TODO +void print(const char *a) { + UNUSED(a); +} + +void puth(unsigned int i) { + UNUSED(i); +} + void fail(void) { soft_flasher_start(); } diff --git a/board/bootstub_declarations.h b/board/bootstub_declarations.h index 5cdec508e70..94e7dd7ca92 100644 --- a/board/bootstub_declarations.h +++ b/board/bootstub_declarations.h @@ -1,18 +1,18 @@ +#pragma once + +#include + +#include "utils.h" +#include "stm32h7/inc/stm32h7xx.h" + // ******************** Prototypes ******************** -void print(const char *a){ UNUSED(a); } -void puth(uint8_t i){ UNUSED(i); } -void puth2(uint8_t i){ UNUSED(i); } -void puth4(uint8_t i){ UNUSED(i); } -void hexdump(const void *a, int l){ UNUSED(a); UNUSED(l); } -typedef struct board board; + + + typedef struct harness_configuration harness_configuration; void pwm_init(TIM_TypeDef *TIM, uint8_t channel); void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage); -// No UART support in bootloader -typedef struct uart_ring {} uart_ring; -uart_ring uart_ring_som_debug; -void uart_init(uart_ring *q, int baud) { UNUSED(q); UNUSED(baud); } // ********************* Globals ********************** -uint8_t hw_type = 0; -board *current_board; + +#include "globals.h" \ No newline at end of file diff --git a/board/can_comms.c b/board/can_comms.c new file mode 100644 index 00000000000..dc51eea9d36 --- /dev/null +++ b/board/can_comms.c @@ -0,0 +1,109 @@ +#include "can_comms.h" +#include "drivers/can_common.h" +#include "utils.h" +#include "libc.h" +#include "can.h" +#include "drivers/usb.h" + +static asm_buffer can_read_buffer = {.ptr = 0U, .tail_size = 0U}; + +int comms_can_read(uint8_t *data, uint32_t max_len) { + uint32_t pos = 0U; + + // Send tail of previous message if it is in buffer + if (can_read_buffer.ptr > 0U) { + uint32_t overflow_len = MIN(max_len - pos, can_read_buffer.ptr); + (void)memcpy(&data[pos], can_read_buffer.data, overflow_len); + pos += overflow_len; + (void)memcpy(can_read_buffer.data, &can_read_buffer.data[overflow_len], can_read_buffer.ptr - overflow_len); + can_read_buffer.ptr -= overflow_len; + } + + if (can_read_buffer.ptr == 0U) { + // Fill rest of buffer with new data + CANPacket_t can_packet; + while ((pos < max_len) && can_pop(&can_rx_q, &can_packet)) { + uint32_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[can_packet.data_len_code]; + if ((pos + pckt_len) <= max_len) { + (void)memcpy(&data[pos], (uint8_t*)&can_packet, pckt_len); + pos += pckt_len; + } else { + (void)memcpy(&data[pos], (uint8_t*)&can_packet, max_len - pos); + can_read_buffer.ptr += pckt_len - (max_len - pos); + // cppcheck-suppress objectIndex + (void)memcpy(can_read_buffer.data, &((uint8_t*)&can_packet)[(max_len - pos)], can_read_buffer.ptr); + pos = max_len; + } + } + } + + return pos; +} + +static asm_buffer can_write_buffer = {.ptr = 0U, .tail_size = 0U}; + +// send on CAN +void comms_can_write(const uint8_t *data, uint32_t len) { + uint32_t pos = 0U; + + // Assembling can message with data from buffer + if (can_write_buffer.ptr != 0U) { + if (can_write_buffer.tail_size <= (len - pos)) { + // we have enough data to complete the buffer + CANPacket_t to_push = {0}; + (void)memcpy(&can_write_buffer.data[can_write_buffer.ptr], &data[pos], can_write_buffer.tail_size); + can_write_buffer.ptr += can_write_buffer.tail_size; + pos += can_write_buffer.tail_size; + + // send out + (void)memcpy((uint8_t*)&to_push, can_write_buffer.data, can_write_buffer.ptr); + can_send(&to_push, to_push.bus, false); + + // reset overflow buffer + can_write_buffer.ptr = 0U; + can_write_buffer.tail_size = 0U; + } else { + // maybe next time + uint32_t data_size = len - pos; + (void) memcpy(&can_write_buffer.data[can_write_buffer.ptr], &data[pos], data_size); + can_write_buffer.tail_size -= data_size; + can_write_buffer.ptr += data_size; + pos += data_size; + } + } + + // rest of the message + while (pos < len) { + uint32_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[(data[pos] >> 4U)]; + if ((pos + pckt_len) <= len) { + CANPacket_t to_push = {0}; + (void)memcpy((uint8_t*)&to_push, &data[pos], pckt_len); + can_send(&to_push, to_push.bus, false); + pos += pckt_len; + } else { + (void)memcpy(can_write_buffer.data, &data[pos], len - pos); + can_write_buffer.ptr = len - pos; + can_write_buffer.tail_size = pckt_len - can_write_buffer.ptr; + pos += can_write_buffer.ptr; + } + } + + refresh_can_tx_slots_available(); +} + +void comms_can_reset(void) { + can_write_buffer.ptr = 0U; + can_write_buffer.tail_size = 0U; + can_read_buffer.ptr = 0U; + can_read_buffer.tail_size = 0U; +} + +// TODO: make this more general! +void refresh_can_tx_slots_available(void) { + if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_USB_BULK_TRANSFER)) { + can_tx_comms_resume_usb(); + } + if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_SPI_BULK_TRANSFER)) { + can_tx_comms_resume_spi(); + } +} diff --git a/board/can_comms.h b/board/can_comms.h index 99ee968b0a0..74d4dd4f259 100644 --- a/board/can_comms.h +++ b/board/can_comms.h @@ -1,3 +1,5 @@ +#pragma once + /* CAN transactions to and from the host come in the form of a certain number of CANPacket_t. The transaction is split @@ -12,111 +14,18 @@ which is sent by the host on each start of a connection. */ +#include + typedef struct { uint32_t ptr; uint32_t tail_size; uint8_t data[72]; } asm_buffer; -static asm_buffer can_read_buffer = {.ptr = 0U, .tail_size = 0U}; - -int comms_can_read(uint8_t *data, uint32_t max_len) { - uint32_t pos = 0U; - - // Send tail of previous message if it is in buffer - if (can_read_buffer.ptr > 0U) { - uint32_t overflow_len = MIN(max_len - pos, can_read_buffer.ptr); - (void)memcpy(&data[pos], can_read_buffer.data, overflow_len); - pos += overflow_len; - (void)memcpy(can_read_buffer.data, &can_read_buffer.data[overflow_len], can_read_buffer.ptr - overflow_len); - can_read_buffer.ptr -= overflow_len; - } - - if (can_read_buffer.ptr == 0U) { - // Fill rest of buffer with new data - CANPacket_t can_packet; - while ((pos < max_len) && can_pop(&can_rx_q, &can_packet)) { - uint32_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[can_packet.data_len_code]; - if ((pos + pckt_len) <= max_len) { - (void)memcpy(&data[pos], (uint8_t*)&can_packet, pckt_len); - pos += pckt_len; - } else { - (void)memcpy(&data[pos], (uint8_t*)&can_packet, max_len - pos); - can_read_buffer.ptr += pckt_len - (max_len - pos); - // cppcheck-suppress objectIndex - (void)memcpy(can_read_buffer.data, &((uint8_t*)&can_packet)[(max_len - pos)], can_read_buffer.ptr); - pos = max_len; - } - } - } - - return pos; -} - -static asm_buffer can_write_buffer = {.ptr = 0U, .tail_size = 0U}; +int comms_can_read(uint8_t *data, uint32_t max_len); // send on CAN -void comms_can_write(const uint8_t *data, uint32_t len) { - uint32_t pos = 0U; - - // Assembling can message with data from buffer - if (can_write_buffer.ptr != 0U) { - if (can_write_buffer.tail_size <= (len - pos)) { - // we have enough data to complete the buffer - CANPacket_t to_push = {0}; - (void)memcpy(&can_write_buffer.data[can_write_buffer.ptr], &data[pos], can_write_buffer.tail_size); - can_write_buffer.ptr += can_write_buffer.tail_size; - pos += can_write_buffer.tail_size; - - // send out - (void)memcpy((uint8_t*)&to_push, can_write_buffer.data, can_write_buffer.ptr); - can_send(&to_push, to_push.bus, false); - - // reset overflow buffer - can_write_buffer.ptr = 0U; - can_write_buffer.tail_size = 0U; - } else { - // maybe next time - uint32_t data_size = len - pos; - (void) memcpy(&can_write_buffer.data[can_write_buffer.ptr], &data[pos], data_size); - can_write_buffer.tail_size -= data_size; - can_write_buffer.ptr += data_size; - pos += data_size; - } - } - - // rest of the message - while (pos < len) { - uint32_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[(data[pos] >> 4U)]; - if ((pos + pckt_len) <= len) { - CANPacket_t to_push = {0}; - (void)memcpy((uint8_t*)&to_push, &data[pos], pckt_len); - can_send(&to_push, to_push.bus, false); - pos += pckt_len; - } else { - (void)memcpy(can_write_buffer.data, &data[pos], len - pos); - can_write_buffer.ptr = len - pos; - can_write_buffer.tail_size = pckt_len - can_write_buffer.ptr; - pos += can_write_buffer.ptr; - } - } - - refresh_can_tx_slots_available(); -} - -void comms_can_reset(void) { - can_write_buffer.ptr = 0U; - can_write_buffer.tail_size = 0U; - can_read_buffer.ptr = 0U; - can_read_buffer.tail_size = 0U; -} +void comms_can_write(const uint8_t *data, uint32_t len); +void comms_can_reset(void); -// TODO: make this more general! -void refresh_can_tx_slots_available(void) { - if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_USB_BULK_TRANSFER)) { - can_tx_comms_resume_usb(); - } - if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_SPI_BULK_TRANSFER)) { - can_tx_comms_resume_spi(); - } -} +void refresh_can_tx_slots_available(void); \ No newline at end of file diff --git a/board/comms_definitions.h b/board/comms_definitions.h index 18a6d2f8134..026a50aa6c9 100644 --- a/board/comms_definitions.h +++ b/board/comms_definitions.h @@ -1,3 +1,7 @@ +#pragma once + +#include + typedef struct { uint8_t request; uint16_t param1; diff --git a/board/config.h b/board/config.h index 1b7d938cc81..d485b7b27ad 100644 --- a/board/config.h +++ b/board/config.h @@ -36,7 +36,8 @@ #ifdef STM32H7 #include "board/stm32h7/stm32h7_config.h" #else + #error help me! // TODO: uncomment this, cppcheck complains // building for tests - //#include "fake_stm.h" + #include "fake_stm.h" #endif diff --git a/board/crc.c b/board/crc.c new file mode 100644 index 00000000000..03668fd1476 --- /dev/null +++ b/board/crc.c @@ -0,0 +1,19 @@ +#include "crc.h" + +uint8_t crc_checksum(const uint8_t *dat, int len, const uint8_t poly) { + uint8_t crc = 0xFFU; + int i; + int j; + for (i = len - 1; i >= 0; i--) { + crc ^= dat[i]; + for (j = 0; j < 8; j++) { + if ((crc & 0x80U) != 0U) { + crc = (uint8_t)((crc << 1) ^ poly); + } + else { + crc <<= 1; + } + } + } + return crc; +} diff --git a/board/crc.h b/board/crc.h index 3e20fb09817..c702fe03e59 100644 --- a/board/crc.h +++ b/board/crc.h @@ -1,19 +1,5 @@ #pragma once -uint8_t crc_checksum(const uint8_t *dat, int len, const uint8_t poly) { - uint8_t crc = 0xFFU; - int i; - int j; - for (i = len - 1; i >= 0; i--) { - crc ^= dat[i]; - for (j = 0; j < 8; j++) { - if ((crc & 0x80U) != 0U) { - crc = (uint8_t)((crc << 1) ^ poly); - } - else { - crc <<= 1; - } - } - } - return crc; -} +#include + +uint8_t crc_checksum(const uint8_t *dat, int len, const uint8_t poly); \ No newline at end of file diff --git a/board/critical.c b/board/critical.c new file mode 100644 index 00000000000..5b12c4ed46e --- /dev/null +++ b/board/critical.c @@ -0,0 +1,18 @@ +#include "critical.h" + +#include "config.h" + +// ********************* Critical section helpers ********************* +uint8_t global_critical_depth = 0U; + +static volatile bool interrupts_enabled = false; + +void enable_interrupts(void) { + interrupts_enabled = true; + __enable_irq(); +} + +void disable_interrupts(void) { + interrupts_enabled = false; + __disable_irq(); +} diff --git a/board/critical.h b/board/critical.h index ae2d5c0a699..af58316613b 100644 --- a/board/critical.h +++ b/board/critical.h @@ -1,16 +1,11 @@ +#include +#include + #include "critical_declarations.h" // ********************* Critical section helpers ********************* -uint8_t global_critical_depth = 0U; - -static volatile bool interrupts_enabled = false; - -void enable_interrupts(void) { - interrupts_enabled = true; - __enable_irq(); -} +extern uint8_t global_critical_depth; +static volatile bool interrupts_enabled; -void disable_interrupts(void) { - interrupts_enabled = false; - __disable_irq(); -} +void enable_interrupts(void); +void disable_interrupts(void); diff --git a/board/critical_declarations.h b/board/critical_declarations.h index 42211d46c38..b0b35e2da9a 100644 --- a/board/critical_declarations.h +++ b/board/critical_declarations.h @@ -1,5 +1,7 @@ #pragma once +#include "config.h" + // ********************* Critical section helpers ********************* void enable_interrupts(void); void disable_interrupts(void); diff --git a/board/drivers/bootkick.c b/board/drivers/bootkick.c new file mode 100644 index 00000000000..a5a6fac0a8b --- /dev/null +++ b/board/drivers/bootkick.c @@ -0,0 +1,72 @@ +#include + +#include "board/drivers/harness.h" +#include "bootkick.h" +#include "board/globals.h" + +bool bootkick_reset_triggered = false; + +void bootkick_tick(bool ignition, bool recent_heartbeat) { + static uint16_t bootkick_last_serial_ptr = 0; + static uint8_t waiting_to_boot_countdown = 0; + static uint8_t boot_reset_countdown = 0; + static uint8_t bootkick_harness_status_prev = HARNESS_STATUS_NC; + static bool bootkick_ign_prev = false; + static BootState boot_state = BOOT_BOOTKICK; + BootState boot_state_prev = boot_state; + const bool harness_inserted = (harness.status != bootkick_harness_status_prev) && (harness.status != HARNESS_STATUS_NC); + + if ((ignition && !bootkick_ign_prev) || harness_inserted) { + // bootkick on rising edge of ignition or harness insertion + boot_state = BOOT_BOOTKICK; + } else if (recent_heartbeat) { + // disable bootkick once openpilot is up + boot_state = BOOT_STANDBY; + } else { + + } + + /* + Ensure SOM boots in case it goes into QDL mode. Reset behavior: + * shouldn't trigger on the first boot after power-on + * only try reset once per bootkick, i.e. don't keep trying until booted + * only try once per panda boot, since openpilot will reset panda on startup + * once BOOT_RESET is triggered, it stays until countdown is finished + */ + if (!bootkick_reset_triggered && (boot_state == BOOT_BOOTKICK) && (boot_state_prev == BOOT_STANDBY)) { + waiting_to_boot_countdown = 20U; + } + if (waiting_to_boot_countdown > 0U) { + bool serial_activity = uart_ring_som_debug.w_ptr_tx != bootkick_last_serial_ptr; + if (serial_activity || current_board->read_som_gpio() || (boot_state != BOOT_BOOTKICK)) { + waiting_to_boot_countdown = 0U; + } else { + // try a reset + if (waiting_to_boot_countdown == 1U) { + boot_reset_countdown = 5U; + } + } + } + + // handle reset state + if (boot_reset_countdown > 0U) { + boot_state = BOOT_RESET; + bootkick_reset_triggered = true; + } else { + if (boot_state == BOOT_RESET) { + boot_state = BOOT_BOOTKICK; + } + } + + // update state + bootkick_ign_prev = ignition; + bootkick_harness_status_prev = harness.status; + bootkick_last_serial_ptr = uart_ring_som_debug.w_ptr_tx; + if (waiting_to_boot_countdown > 0U) { + waiting_to_boot_countdown--; + } + if (boot_reset_countdown > 0U) { + boot_reset_countdown--; + } + current_board->set_bootkick(boot_state); +} diff --git a/board/drivers/bootkick.h b/board/drivers/bootkick.h index eab7da3ebc4..7c34945ba16 100644 --- a/board/drivers/bootkick.h +++ b/board/drivers/bootkick.h @@ -1,68 +1,7 @@ -#include "bootkick_declarations.h" +#pragma once -bool bootkick_reset_triggered = false; +#include -void bootkick_tick(bool ignition, bool recent_heartbeat) { - static uint16_t bootkick_last_serial_ptr = 0; - static uint8_t waiting_to_boot_countdown = 0; - static uint8_t boot_reset_countdown = 0; - static uint8_t bootkick_harness_status_prev = HARNESS_STATUS_NC; - static bool bootkick_ign_prev = false; - static BootState boot_state = BOOT_BOOTKICK; - BootState boot_state_prev = boot_state; - const bool harness_inserted = (harness.status != bootkick_harness_status_prev) && (harness.status != HARNESS_STATUS_NC); +extern bool bootkick_reset_triggered; - if ((ignition && !bootkick_ign_prev) || harness_inserted) { - // bootkick on rising edge of ignition or harness insertion - boot_state = BOOT_BOOTKICK; - } else if (recent_heartbeat) { - // disable bootkick once openpilot is up - boot_state = BOOT_STANDBY; - } else { - - } - - /* - Ensure SOM boots in case it goes into QDL mode. Reset behavior: - * shouldn't trigger on the first boot after power-on - * only try reset once per bootkick, i.e. don't keep trying until booted - * only try once per panda boot, since openpilot will reset panda on startup - * once BOOT_RESET is triggered, it stays until countdown is finished - */ - if (!bootkick_reset_triggered && (boot_state == BOOT_BOOTKICK) && (boot_state_prev == BOOT_STANDBY)) { - waiting_to_boot_countdown = 20U; - } - if (waiting_to_boot_countdown > 0U) { - bool serial_activity = uart_ring_som_debug.w_ptr_tx != bootkick_last_serial_ptr; - if (serial_activity || current_board->read_som_gpio() || (boot_state != BOOT_BOOTKICK)) { - waiting_to_boot_countdown = 0U; - } else { - // try a reset - if (waiting_to_boot_countdown == 1U) { - boot_reset_countdown = 5U; - } - } - } - - // handle reset state - if (boot_reset_countdown > 0U) { - boot_state = BOOT_RESET; - bootkick_reset_triggered = true; - } else { - if (boot_state == BOOT_RESET) { - boot_state = BOOT_BOOTKICK; - } - } - - // update state - bootkick_ign_prev = ignition; - bootkick_harness_status_prev = harness.status; - bootkick_last_serial_ptr = uart_ring_som_debug.w_ptr_tx; - if (waiting_to_boot_countdown > 0U) { - waiting_to_boot_countdown--; - } - if (boot_reset_countdown > 0U) { - boot_reset_countdown--; - } - current_board->set_bootkick(boot_state); -} +void bootkick_tick(bool ignition, bool recent_heartbeat); \ No newline at end of file diff --git a/board/drivers/can_common.c b/board/drivers/can_common.c new file mode 100644 index 00000000000..b4f897f8998 --- /dev/null +++ b/board/drivers/can_common.c @@ -0,0 +1,264 @@ +#include "can_common.h" + +#include "board/can_comms.h" +#include "timers.h" +#include "board/utils.h" +// #include "opendbc/safety/safety.h" + +#include "board/critical_declarations.h" + +uint32_t safety_tx_blocked = 0; +uint32_t safety_rx_invalid = 0; +uint32_t tx_buffer_overflow = 0; +uint32_t rx_buffer_overflow = 0; + +can_health_t can_health[PANDA_CAN_CNT] = {{0}, {0}, {0}}; + +// Ignition detected from CAN meessages +bool ignition_can = false; +uint32_t ignition_can_cnt = 0U; + +bool can_silent = true; +bool can_loopback = false; + +// ********************* instantiate queues ********************* +#define can_buffer(x, size) \ + static CANPacket_t elems_##x[size]; \ + extern can_ring can_##x; \ + can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = (size), .elems = (CANPacket_t *)&(elems_##x) }; + +#define CAN_RX_BUFFER_SIZE 4096U +#define CAN_TX_BUFFER_SIZE 416U + +#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) +#else // kept for PC +can_buffer(rx_q, CAN_RX_BUFFER_SIZE) +can_buffer(tx1_q, CAN_TX_BUFFER_SIZE) +can_buffer(tx2_q, CAN_TX_BUFFER_SIZE) +#endif +can_buffer(tx3_q, CAN_TX_BUFFER_SIZE) + +// FIXME: +// cppcheck-suppress misra-c2012-9.3 +can_ring *can_queues[PANDA_CAN_CNT] = {&can_tx1_q, &can_tx2_q, &can_tx3_q}; + +// ********************* interrupt safe queue ********************* +bool can_pop(can_ring *q, CANPacket_t *elem) { + bool ret = 0; + + ENTER_CRITICAL(); + if (q->w_ptr != q->r_ptr) { + *elem = q->elems[q->r_ptr]; + if ((q->r_ptr + 1U) == q->fifo_size) { + q->r_ptr = 0; + } else { + q->r_ptr += 1U; + } + ret = 1; + } + EXIT_CRITICAL(); + + return ret; +} + +bool can_push(can_ring *q, const CANPacket_t *elem) { + bool ret = false; + uint32_t next_w_ptr; + + ENTER_CRITICAL(); + if ((q->w_ptr + 1U) == q->fifo_size) { + next_w_ptr = 0; + } else { + next_w_ptr = q->w_ptr + 1U; + } + if (next_w_ptr != q->r_ptr) { + q->elems[q->w_ptr] = *elem; + q->w_ptr = next_w_ptr; + ret = true; + } + EXIT_CRITICAL(); + if (!ret) { + #ifdef DEBUG + print("can_push to "); + if (q == &can_rx_q) { + print("can_rx_q"); + } else if (q == &can_tx1_q) { + print("can_tx1_q"); + } else if (q == &can_tx2_q) { + print("can_tx2_q"); + } else if (q == &can_tx3_q) { + print("can_tx3_q"); + } else { + print("unknown"); + } + print(" failed!\n"); + #endif + } + return ret; +} + +uint32_t can_slots_empty(const can_ring *q) { + uint32_t ret = 0; + + ENTER_CRITICAL(); + if (q->w_ptr >= q->r_ptr) { + ret = q->fifo_size - 1U - q->w_ptr + q->r_ptr; + } else { + ret = q->r_ptr - q->w_ptr - 1U; + } + EXIT_CRITICAL(); + + return ret; +} + +void can_clear(can_ring *q) { + ENTER_CRITICAL(); + q->w_ptr = 0; + q->r_ptr = 0; + EXIT_CRITICAL(); + // handle TX buffer full with zero ECUs awake on the bus + refresh_can_tx_slots_available(); +} + +// assign CAN numbering +// bus num: CAN Bus numbers in panda, sent to/from USB +// Min: 0; Max: 127; Bit 7 marks message as receipt (bus 129 is receipt for but 1) +// cans: Look up MCU can interface from bus number +// can number: numeric lookup for MCU CAN interfaces (0 = CAN1, 1 = CAN2, etc); +// bus_lookup: Translates from 'can number' to 'bus number'. +// can_num_lookup: Translates from 'bus number' to 'can number'. +// forwarding bus: If >= 0, forward all messages from this bus to the specified bus. + +// Helpers +// Panda: Bus 0=CAN1 Bus 1=CAN2 Bus 2=CAN3 +bus_config_t bus_config[PANDA_CAN_CNT] = { + { .bus_lookup = 0U, .can_num_lookup = 0U, .forwarding_bus = -1, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_auto = false, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false }, + { .bus_lookup = 1U, .can_num_lookup = 1U, .forwarding_bus = -1, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_auto = false, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false }, + { .bus_lookup = 2U, .can_num_lookup = 2U, .forwarding_bus = -1, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_auto = false, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false }, +}; + +void can_init_all(void) { + for (uint8_t i=0U; i < PANDA_CAN_CNT; i++) { + bus_config[i].canfd_enabled = false; + can_clear(can_queues[i]); + (void)can_init(i); + } +} + +void can_set_orientation(bool flipped) { + bus_config[0].bus_lookup = flipped ? 2U : 0U; + bus_config[0].can_num_lookup = flipped ? 2U : 0U; + bus_config[2].bus_lookup = flipped ? 0U : 2U; + bus_config[2].can_num_lookup = flipped ? 0U : 2U; +} + +#ifdef PANDA_JUNGLE +void can_set_forwarding(uint8_t from, uint8_t to) { + bus_config[from].forwarding_bus = to; +} +#endif + +void ignition_can_hook(CANPacket_t *msg) { + if (msg->bus == 0U) { + int len = GET_LEN(msg); + + // GM exception + if ((msg->addr == 0x1F1U) && (len == 8)) { + // SystemPowerMode (2=Run, 3=Crank Request) + ignition_can = (msg->data[0] & 0x2U) != 0U; + ignition_can_cnt = 0U; + } + + // Rivian R1S/T GEN1 exception + if ((msg->addr == 0x152U) && (len == 8)) { + // 0x152 overlaps with Subaru pre-global which has this bit as the high beam + int counter = msg->data[1] & 0xFU; // max is only 14 + + static int prev_counter_rivian = -1; + if ((counter == ((prev_counter_rivian + 1) % 15)) && (prev_counter_rivian != -1)) { + // VDM_OutputSignals->VDM_EpasPowerMode + ignition_can = ((msg->data[7] >> 4U) & 0x3U) == 1U; // VDM_EpasPowerMode_Drive_On=1 + ignition_can_cnt = 0U; + } + prev_counter_rivian = counter; + } + + // Tesla Model 3/Y exception + if ((msg->addr == 0x221U) && (len == 8)) { + // 0x221 overlaps with Rivian which has random data on byte 0 + int counter = msg->data[6] >> 4; + + static int prev_counter_tesla = -1; + if ((counter == ((prev_counter_tesla + 1) % 16)) && (prev_counter_tesla != -1)) { + // VCFRONT_LVPowerState->VCFRONT_vehiclePowerState + int power_state = (msg->data[0] >> 5U) & 0x3U; + ignition_can = power_state == 0x3; // VEHICLE_POWER_STATE_DRIVE=3 + ignition_can_cnt = 0U; + } + prev_counter_tesla = counter; + } + + // Mazda exception + if ((msg->addr == 0x9EU) && (len == 8)) { + ignition_can = (msg->data[0] >> 5) == 0x6U; + ignition_can_cnt = 0U; + } + + } +} + +bool can_tx_check_min_slots_free(uint32_t min) { + return + (can_slots_empty(&can_tx1_q) >= min) && + (can_slots_empty(&can_tx2_q) >= min) && + (can_slots_empty(&can_tx3_q) >= min); +} + +uint8_t calculate_checksum(const uint8_t *dat, uint32_t len) { + uint8_t checksum = 0U; + for (uint32_t i = 0U; i < len; i++) { + checksum ^= dat[i]; + } + return checksum; +} + +void can_set_checksum(CANPacket_t *packet) { + packet->checksum = 0U; + packet->checksum = calculate_checksum((uint8_t *) packet, CANPACKET_HEAD_SIZE + GET_LEN(packet)); +} + +bool can_check_checksum(CANPacket_t *packet) { + return (calculate_checksum((uint8_t *) packet, CANPACKET_HEAD_SIZE + GET_LEN(packet)) == 0U); +} + +void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook) { + if (skip_tx_hook || safety_tx_hook(to_push) != 0) { + if (bus_number < PANDA_CAN_CNT) { + // add CAN packet to send queue + tx_buffer_overflow += can_push(can_queues[bus_number], to_push) ? 0U : 1U; + process_can(CAN_NUM_FROM_BUS_NUM(bus_number)); + } + } else { + safety_tx_blocked += 1U; + to_push->returned = 0U; + to_push->rejected = 1U; + + // data changed + can_set_checksum(to_push); + rx_buffer_overflow += can_push(&can_rx_q, to_push) ? 0U : 1U; + } +} + +bool is_speed_valid(uint32_t speed, const uint32_t *all_speeds, uint8_t len) { + bool ret = false; + for (uint8_t i = 0U; i < len; i++) { + if (all_speeds[i] == speed) { + ret = true; + } + } + return ret; +} diff --git a/board/drivers/can_common.h b/board/drivers/can_common.h index 35c6702e807..28e7ddfeb70 100644 --- a/board/drivers/can_common.h +++ b/board/drivers/can_common.h @@ -1,18 +1,99 @@ -#include "can_common_declarations.h" +#pragma once + +#include +#include + +#include "board/can.h" +#include "board/health.h" + +typedef struct { + volatile uint32_t w_ptr; + volatile uint32_t r_ptr; + uint32_t fifo_size; + CANPacket_t *elems; +} can_ring; + +typedef struct { + uint8_t bus_lookup; + uint8_t can_num_lookup; + int8_t forwarding_bus; + uint32_t can_speed; + uint32_t can_data_speed; + bool canfd_auto; + bool canfd_enabled; + bool brs_enabled; + bool canfd_non_iso; +} bus_config_t; + +extern uint32_t safety_tx_blocked; +extern uint32_t safety_rx_invalid; +extern uint32_t tx_buffer_overflow; +extern uint32_t rx_buffer_overflow; + +extern can_health_t can_health[PANDA_CAN_CNT]; -uint32_t safety_tx_blocked = 0; -uint32_t safety_rx_invalid = 0; -uint32_t tx_buffer_overflow = 0; -uint32_t rx_buffer_overflow = 0; +// Ignition detected from CAN meessages +extern bool ignition_can; +extern uint32_t ignition_can_cnt; + +extern bool can_silent; +extern bool can_loopback; + +// ******************* functions prototypes ********************* +bool can_init(uint8_t can_number); +void process_can(uint8_t can_number); + +// ********************* instantiate queues ********************* +extern can_ring *can_queues[PANDA_CAN_CNT]; + +// helpers +#define WORD_TO_BYTE_ARRAY(dst8, src32) 0[dst8] = ((src32) & 0xFFU); 1[dst8] = (((src32) >> 8U) & 0xFFU); 2[dst8] = (((src32) >> 16U) & 0xFFU); 3[dst8] = (((src32) >> 24U) & 0xFFU) +#define BYTE_ARRAY_TO_WORD(dst32, src8) ((dst32) = 0[src8] | (1[src8] << 8U) | (2[src8] << 16U) | (3[src8] << 24U)) + +// ********************* interrupt safe queue ********************* +bool can_pop(can_ring *q, CANPacket_t *elem); +bool can_push(can_ring *q, const CANPacket_t *elem); +uint32_t can_slots_empty(const can_ring *q); +extern bus_config_t bus_config[PANDA_CAN_CNT]; + +#define CANIF_FROM_CAN_NUM(num) (cans[num]) +#define BUS_NUM_FROM_CAN_NUM(num) (bus_config[num].bus_lookup) +#define CAN_NUM_FROM_BUS_NUM(num) (bus_config[num].can_num_lookup) + +void can_init_all(void); +void can_set_orientation(bool flipped); +#ifdef PANDA_JUNGLE +void can_set_forwarding(uint8_t from, uint8_t to); +#endif +void ignition_can_hook(CANPacket_t *to_push); +bool can_tx_check_min_slots_free(uint32_t min); +uint8_t calculate_checksum(const uint8_t *dat, uint32_t len); +void can_set_checksum(CANPacket_t *packet); +bool can_check_checksum(CANPacket_t *packet); +void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook); +bool is_speed_valid(uint32_t speed, const uint32_t *all_speeds, uint8_t len); + +#include "board/can_comms.h" +#include "timers.h" +#include "board/utils.h" +#include "opendbc/safety/declarations.h" +// #include "opendbc/safety/safety.h" + +#include "board/critical_declarations.h" + +extern uint32_t safety_tx_blocked; +extern uint32_t safety_rx_invalid; +extern uint32_t tx_buffer_overflow; +extern uint32_t rx_buffer_overflow; -can_health_t can_health[PANDA_CAN_CNT] = {{0}, {0}, {0}}; +extern can_health_t can_health[PANDA_CAN_CNT]; // Ignition detected from CAN meessages -bool ignition_can = false; -uint32_t ignition_can_cnt = 0U; +extern bool ignition_can; +extern uint32_t ignition_can_cnt; -bool can_silent = true; -bool can_loopback = false; +extern bool can_silent; +extern bool can_loopback; // ********************* instantiate queues ********************* #define can_buffer(x, size) \ @@ -25,233 +106,39 @@ bool can_loopback = false; #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"))) extern can_ring can_rx_q; +__attribute__((section(".itcmram"))) extern can_ring can_tx1_q; +__attribute__((section(".itcmram"))) extern can_ring can_tx2_q; #else // kept for PC -can_buffer(rx_q, CAN_RX_BUFFER_SIZE) -can_buffer(tx1_q, CAN_TX_BUFFER_SIZE) -can_buffer(tx2_q, CAN_TX_BUFFER_SIZE) +extern can_ring can_rx_q; +extern can_ring can_tx1_q; +extern can_ring can_tx2_q; #endif -can_buffer(tx3_q, CAN_TX_BUFFER_SIZE) +extern can_ring can_tx3_q; // FIXME: // cppcheck-suppress misra-c2012-9.3 -can_ring *can_queues[PANDA_CAN_CNT] = {&can_tx1_q, &can_tx2_q, &can_tx3_q}; +// can_ring *can_queues[PANDA_CAN_CNT] = {&can_tx1_q, &can_tx2_q, &can_tx3_q}; // ********************* interrupt safe queue ********************* -bool can_pop(can_ring *q, CANPacket_t *elem) { - bool ret = 0; - - ENTER_CRITICAL(); - if (q->w_ptr != q->r_ptr) { - *elem = q->elems[q->r_ptr]; - if ((q->r_ptr + 1U) == q->fifo_size) { - q->r_ptr = 0; - } else { - q->r_ptr += 1U; - } - ret = 1; - } - EXIT_CRITICAL(); - - return ret; -} - -bool can_push(can_ring *q, const CANPacket_t *elem) { - bool ret = false; - uint32_t next_w_ptr; - - ENTER_CRITICAL(); - if ((q->w_ptr + 1U) == q->fifo_size) { - next_w_ptr = 0; - } else { - next_w_ptr = q->w_ptr + 1U; - } - if (next_w_ptr != q->r_ptr) { - q->elems[q->w_ptr] = *elem; - q->w_ptr = next_w_ptr; - ret = true; - } - EXIT_CRITICAL(); - if (!ret) { - #ifdef DEBUG - print("can_push to "); - if (q == &can_rx_q) { - print("can_rx_q"); - } else if (q == &can_tx1_q) { - print("can_tx1_q"); - } else if (q == &can_tx2_q) { - print("can_tx2_q"); - } else if (q == &can_tx3_q) { - print("can_tx3_q"); - } else { - print("unknown"); - } - print(" failed!\n"); - #endif - } - return ret; -} - -uint32_t can_slots_empty(const can_ring *q) { - uint32_t ret = 0; +bool can_pop(can_ring *q, CANPacket_t *elem); - ENTER_CRITICAL(); - if (q->w_ptr >= q->r_ptr) { - ret = q->fifo_size - 1U - q->w_ptr + q->r_ptr; - } else { - ret = q->r_ptr - q->w_ptr - 1U; - } - EXIT_CRITICAL(); +bool can_push(can_ring *q, const CANPacket_t *elem); +uint32_t can_slots_empty(const can_ring *q); +void can_clear(can_ring *q); - return ret; -} - -void can_clear(can_ring *q) { - ENTER_CRITICAL(); - q->w_ptr = 0; - q->r_ptr = 0; - EXIT_CRITICAL(); - // handle TX buffer full with zero ECUs awake on the bus - refresh_can_tx_slots_available(); -} - -// assign CAN numbering -// bus num: CAN Bus numbers in panda, sent to/from USB -// Min: 0; Max: 127; Bit 7 marks message as receipt (bus 129 is receipt for but 1) -// cans: Look up MCU can interface from bus number -// can number: numeric lookup for MCU CAN interfaces (0 = CAN1, 1 = CAN2, etc); -// bus_lookup: Translates from 'can number' to 'bus number'. -// can_num_lookup: Translates from 'bus number' to 'can number'. -// forwarding bus: If >= 0, forward all messages from this bus to the specified bus. - -// Helpers -// Panda: Bus 0=CAN1 Bus 1=CAN2 Bus 2=CAN3 -bus_config_t bus_config[PANDA_CAN_CNT] = { - { .bus_lookup = 0U, .can_num_lookup = 0U, .forwarding_bus = -1, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_auto = false, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false }, - { .bus_lookup = 1U, .can_num_lookup = 1U, .forwarding_bus = -1, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_auto = false, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false }, - { .bus_lookup = 2U, .can_num_lookup = 2U, .forwarding_bus = -1, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_auto = false, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false }, -}; - -void can_init_all(void) { - for (uint8_t i=0U; i < PANDA_CAN_CNT; i++) { - bus_config[i].canfd_enabled = false; - can_clear(can_queues[i]); - (void)can_init(i); - } -} - -void can_set_orientation(bool flipped) { - bus_config[0].bus_lookup = flipped ? 2U : 0U; - bus_config[0].can_num_lookup = flipped ? 2U : 0U; - bus_config[2].bus_lookup = flipped ? 0U : 2U; - bus_config[2].can_num_lookup = flipped ? 0U : 2U; -} +extern bus_config_t bus_config[PANDA_CAN_CNT]; +void can_init_all(void); +void can_set_orientation(bool flipped); #ifdef PANDA_JUNGLE -void can_set_forwarding(uint8_t from, uint8_t to) { - bus_config[from].forwarding_bus = to; -} +void can_set_forwarding(uint8_t from, uint8_t to); #endif -void ignition_can_hook(CANPacket_t *msg) { - if (msg->bus == 0U) { - int len = GET_LEN(msg); - - // GM exception - if ((msg->addr == 0x1F1U) && (len == 8)) { - // SystemPowerMode (2=Run, 3=Crank Request) - ignition_can = (msg->data[0] & 0x2U) != 0U; - ignition_can_cnt = 0U; - } - - // Rivian R1S/T GEN1 exception - if ((msg->addr == 0x152U) && (len == 8)) { - // 0x152 overlaps with Subaru pre-global which has this bit as the high beam - int counter = msg->data[1] & 0xFU; // max is only 14 - - static int prev_counter_rivian = -1; - if ((counter == ((prev_counter_rivian + 1) % 15)) && (prev_counter_rivian != -1)) { - // VDM_OutputSignals->VDM_EpasPowerMode - ignition_can = ((msg->data[7] >> 4U) & 0x3U) == 1U; // VDM_EpasPowerMode_Drive_On=1 - ignition_can_cnt = 0U; - } - prev_counter_rivian = counter; - } - - // Tesla Model 3/Y exception - if ((msg->addr == 0x221U) && (len == 8)) { - // 0x221 overlaps with Rivian which has random data on byte 0 - int counter = msg->data[6] >> 4; - - static int prev_counter_tesla = -1; - if ((counter == ((prev_counter_tesla + 1) % 16)) && (prev_counter_tesla != -1)) { - // VCFRONT_LVPowerState->VCFRONT_vehiclePowerState - int power_state = (msg->data[0] >> 5U) & 0x3U; - ignition_can = power_state == 0x3; // VEHICLE_POWER_STATE_DRIVE=3 - ignition_can_cnt = 0U; - } - prev_counter_tesla = counter; - } - - // Mazda exception - if ((msg->addr == 0x9EU) && (len == 8)) { - ignition_can = (msg->data[0] >> 5) == 0x6U; - ignition_can_cnt = 0U; - } - - } -} - -bool can_tx_check_min_slots_free(uint32_t min) { - return - (can_slots_empty(&can_tx1_q) >= min) && - (can_slots_empty(&can_tx2_q) >= min) && - (can_slots_empty(&can_tx3_q) >= min); -} - -uint8_t calculate_checksum(const uint8_t *dat, uint32_t len) { - uint8_t checksum = 0U; - for (uint32_t i = 0U; i < len; i++) { - checksum ^= dat[i]; - } - return checksum; -} - -void can_set_checksum(CANPacket_t *packet) { - packet->checksum = 0U; - packet->checksum = calculate_checksum((uint8_t *) packet, CANPACKET_HEAD_SIZE + GET_LEN(packet)); -} - -bool can_check_checksum(CANPacket_t *packet) { - return (calculate_checksum((uint8_t *) packet, CANPACKET_HEAD_SIZE + GET_LEN(packet)) == 0U); -} - -void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook) { - if (skip_tx_hook || safety_tx_hook(to_push) != 0) { - if (bus_number < PANDA_CAN_CNT) { - // add CAN packet to send queue - tx_buffer_overflow += can_push(can_queues[bus_number], to_push) ? 0U : 1U; - process_can(CAN_NUM_FROM_BUS_NUM(bus_number)); - } - } else { - safety_tx_blocked += 1U; - to_push->returned = 0U; - to_push->rejected = 1U; - - // data changed - can_set_checksum(to_push); - rx_buffer_overflow += can_push(&can_rx_q, to_push) ? 0U : 1U; - } -} - -bool is_speed_valid(uint32_t speed, const uint32_t *all_speeds, uint8_t len) { - bool ret = false; - for (uint8_t i = 0U; i < len; i++) { - if (all_speeds[i] == speed) { - ret = true; - } - } - return ret; -} +void ignition_can_hook(CANPacket_t *msg); +bool can_tx_check_min_slots_free(uint32_t min); +uint8_t calculate_checksum(const uint8_t *dat, uint32_t len); +void can_set_checksum(CANPacket_t *packet); +bool can_check_checksum(CANPacket_t *packet); +void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook); +bool is_speed_valid(uint32_t speed, const uint32_t *all_speeds, uint8_t len); \ No newline at end of file diff --git a/board/drivers/can_common_declarations.h b/board/drivers/can_common_declarations.h deleted file mode 100644 index 75580b7b640..00000000000 --- a/board/drivers/can_common_declarations.h +++ /dev/null @@ -1,70 +0,0 @@ -#pragma once - -#include "board/can.h" - -typedef struct { - volatile uint32_t w_ptr; - volatile uint32_t r_ptr; - uint32_t fifo_size; - CANPacket_t *elems; -} can_ring; - -typedef struct { - uint8_t bus_lookup; - uint8_t can_num_lookup; - int8_t forwarding_bus; - uint32_t can_speed; - uint32_t can_data_speed; - bool canfd_auto; - bool canfd_enabled; - bool brs_enabled; - bool canfd_non_iso; -} bus_config_t; - -extern uint32_t safety_tx_blocked; -extern uint32_t safety_rx_invalid; -extern uint32_t tx_buffer_overflow; -extern uint32_t rx_buffer_overflow; - -extern can_health_t can_health[PANDA_CAN_CNT]; - -// Ignition detected from CAN meessages -extern bool ignition_can; -extern uint32_t ignition_can_cnt; - -extern bool can_silent; -extern bool can_loopback; - -// ******************* functions prototypes ********************* -bool can_init(uint8_t can_number); -void process_can(uint8_t can_number); - -// ********************* instantiate queues ********************* -extern can_ring *can_queues[PANDA_CAN_CNT]; - -// helpers -#define WORD_TO_BYTE_ARRAY(dst8, src32) 0[dst8] = ((src32) & 0xFFU); 1[dst8] = (((src32) >> 8U) & 0xFFU); 2[dst8] = (((src32) >> 16U) & 0xFFU); 3[dst8] = (((src32) >> 24U) & 0xFFU) -#define BYTE_ARRAY_TO_WORD(dst32, src8) ((dst32) = 0[src8] | (1[src8] << 8U) | (2[src8] << 16U) | (3[src8] << 24U)) - -// ********************* interrupt safe queue ********************* -bool can_pop(can_ring *q, CANPacket_t *elem); -bool can_push(can_ring *q, const CANPacket_t *elem); -uint32_t can_slots_empty(const can_ring *q); -extern bus_config_t bus_config[PANDA_CAN_CNT]; - -#define CANIF_FROM_CAN_NUM(num) (cans[num]) -#define BUS_NUM_FROM_CAN_NUM(num) (bus_config[num].bus_lookup) -#define CAN_NUM_FROM_BUS_NUM(num) (bus_config[num].can_num_lookup) - -void can_init_all(void); -void can_set_orientation(bool flipped); -#ifdef PANDA_JUNGLE -void can_set_forwarding(uint8_t from, uint8_t to); -#endif -void ignition_can_hook(CANPacket_t *to_push); -bool can_tx_check_min_slots_free(uint32_t min); -uint8_t calculate_checksum(const uint8_t *dat, uint32_t len); -void can_set_checksum(CANPacket_t *packet); -bool can_check_checksum(CANPacket_t *packet); -void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook); -bool is_speed_valid(uint32_t speed, const uint32_t *all_speeds, uint8_t len); diff --git a/board/drivers/clock_source.c b/board/drivers/clock_source.c new file mode 100644 index 00000000000..522cab3f405 --- /dev/null +++ b/board/drivers/clock_source.c @@ -0,0 +1,45 @@ +#include "clock_source.h" +#include "board/config.h" + +void clock_source_set_timer_params(uint16_t param1, uint16_t param2) { + // Pulse length of each channel + register_set(&(TIM1->CCR1), (((param1 & 0xFF00U) >> 8U)*10U), 0xFFFFU); + register_set(&(TIM1->CCR2), ((param1 & 0x00FFU)*10U), 0xFFFFU); + register_set(&(TIM1->CCR3), (((param2 & 0xFF00U) >> 8U)*10U), 0xFFFFU); + // Timer period + register_set(&(TIM1->ARR), (((param2 & 0x00FFU)*10U) - 1U), 0xFFFFU); +} + +void clock_source_init(bool enable_channel1) { + // Setup timer + register_set(&(TIM1->PSC), ((APB2_TIMER_FREQ*100U)-1U), 0xFFFFU); // Tick on 0.1 ms + register_set(&(TIM1->ARR), ((CLOCK_SOURCE_PERIOD_MS*10U) - 1U), 0xFFFFU); // Period + register_set(&(TIM1->CCMR1), 0U, 0xFFFFU); // No output on compare + register_set(&(TIM1->CCER), TIM_CCER_CC1E, 0xFFFFU); // Enable compare 1 + register_set(&(TIM1->CCR1), (CLOCK_SOURCE_PULSE_LEN_MS*10U), 0xFFFFU); // Compare 1 value + register_set(&(TIM1->CCR2), (CLOCK_SOURCE_PULSE_LEN_MS*10U), 0xFFFFU); // Compare 1 value + register_set(&(TIM1->CCR3), (CLOCK_SOURCE_PULSE_LEN_MS*10U), 0xFFFFU); // Compare 1 value + register_set_bits(&(TIM1->DIER), TIM_DIER_UIE | TIM_DIER_CC1IE); // Enable interrupts + register_set(&(TIM1->CR1), TIM_CR1_CEN, 0x3FU); // Enable timer + + // No interrupts + NVIC_DisableIRQ(TIM1_UP_TIM10_IRQn); + NVIC_DisableIRQ(TIM1_CC_IRQn); + + // Set GPIO as timer channels + if (enable_channel1) { + set_gpio_alternate(GPIOA, 8, GPIO_AF1_TIM1); + } + set_gpio_alternate(GPIOB, 14, GPIO_AF1_TIM1); + set_gpio_alternate(GPIOB, 15, GPIO_AF1_TIM1); + + // Set PWM mode + register_set(&(TIM1->CCMR1), (0b110UL << TIM_CCMR1_OC1M_Pos) | (0b110UL << TIM_CCMR1_OC2M_Pos), 0xFFFFU); + register_set(&(TIM1->CCMR2), (0b110UL << TIM_CCMR2_OC3M_Pos), 0xFFFFU); + + // Enable output + register_set(&(TIM1->BDTR), TIM_BDTR_MOE, 0xFFFFU); + + // Enable complementary compares + register_set_bits(&(TIM1->CCER), TIM_CCER_CC2NE | TIM_CCER_CC3NE); +} diff --git a/board/drivers/clock_source.h b/board/drivers/clock_source.h index 87d89ad063e..0a94ed21f62 100644 --- a/board/drivers/clock_source.h +++ b/board/drivers/clock_source.h @@ -1,44 +1,10 @@ -#include "clock_source_declarations.h" +#pragma once -void clock_source_set_timer_params(uint16_t param1, uint16_t param2) { - // Pulse length of each channel - register_set(&(TIM1->CCR1), (((param1 & 0xFF00U) >> 8U)*10U), 0xFFFFU); - register_set(&(TIM1->CCR2), ((param1 & 0x00FFU)*10U), 0xFFFFU); - register_set(&(TIM1->CCR3), (((param2 & 0xFF00U) >> 8U)*10U), 0xFFFFU); - // Timer period - register_set(&(TIM1->ARR), (((param2 & 0x00FFU)*10U) - 1U), 0xFFFFU); -} +#include +#include -void clock_source_init(bool enable_channel1) { - // Setup timer - register_set(&(TIM1->PSC), ((APB2_TIMER_FREQ*100U)-1U), 0xFFFFU); // Tick on 0.1 ms - register_set(&(TIM1->ARR), ((CLOCK_SOURCE_PERIOD_MS*10U) - 1U), 0xFFFFU); // Period - register_set(&(TIM1->CCMR1), 0U, 0xFFFFU); // No output on compare - register_set(&(TIM1->CCER), TIM_CCER_CC1E, 0xFFFFU); // Enable compare 1 - register_set(&(TIM1->CCR1), (CLOCK_SOURCE_PULSE_LEN_MS*10U), 0xFFFFU); // Compare 1 value - register_set(&(TIM1->CCR2), (CLOCK_SOURCE_PULSE_LEN_MS*10U), 0xFFFFU); // Compare 1 value - register_set(&(TIM1->CCR3), (CLOCK_SOURCE_PULSE_LEN_MS*10U), 0xFFFFU); // Compare 1 value - register_set_bits(&(TIM1->DIER), TIM_DIER_UIE | TIM_DIER_CC1IE); // Enable interrupts - register_set(&(TIM1->CR1), TIM_CR1_CEN, 0x3FU); // Enable timer +#define CLOCK_SOURCE_PERIOD_MS 50U +#define CLOCK_SOURCE_PULSE_LEN_MS 2U - // No interrupts - NVIC_DisableIRQ(TIM1_UP_TIM10_IRQn); - NVIC_DisableIRQ(TIM1_CC_IRQn); - - // Set GPIO as timer channels - if (enable_channel1) { - set_gpio_alternate(GPIOA, 8, GPIO_AF1_TIM1); - } - set_gpio_alternate(GPIOB, 14, GPIO_AF1_TIM1); - set_gpio_alternate(GPIOB, 15, GPIO_AF1_TIM1); - - // Set PWM mode - register_set(&(TIM1->CCMR1), (0b110UL << TIM_CCMR1_OC1M_Pos) | (0b110UL << TIM_CCMR1_OC2M_Pos), 0xFFFFU); - register_set(&(TIM1->CCMR2), (0b110UL << TIM_CCMR2_OC3M_Pos), 0xFFFFU); - - // Enable output - register_set(&(TIM1->BDTR), TIM_BDTR_MOE, 0xFFFFU); - - // Enable complementary compares - register_set_bits(&(TIM1->CCER), TIM_CCER_CC2NE | TIM_CCER_CC3NE); -} +void clock_source_set_timer_params(uint16_t param1, uint16_t param2); +void clock_source_init(bool enable_channel1); diff --git a/board/drivers/clock_source_declarations.h b/board/drivers/clock_source_declarations.h deleted file mode 100644 index d95eb961a0b..00000000000 --- a/board/drivers/clock_source_declarations.h +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once - -#define CLOCK_SOURCE_PERIOD_MS 50U -#define CLOCK_SOURCE_PULSE_LEN_MS 2U - -void clock_source_set_timer_params(uint16_t param1, uint16_t param2); -void clock_source_init(bool enable_channel1); diff --git a/board/drivers/fake_siren.c b/board/drivers/fake_siren.c new file mode 100644 index 00000000000..debe49e5021 --- /dev/null +++ b/board/drivers/fake_siren.c @@ -0,0 +1,115 @@ +#include "fake_siren.h" + +#include "board/globals.h" + +void siren_tim7_init(void) { + // Init trigger timer (around 2.5kHz) + register_set(&TIM7->PSC, 0U, 0xFFFFU); + register_set(&TIM7->ARR, 133U, 0xFFFFU); + register_set(&TIM7->CR2, (0b10U << TIM_CR2_MMS_Pos), TIM_CR2_MMS_Msk); + register_set(&TIM7->CR1, TIM_CR1_ARPE | TIM_CR1_URS, 0x088EU); + TIM7->SR = 0U; + TIM7->CR1 |= TIM_CR1_CEN; +} + +void siren_dac_init(void) { + DAC1->DHR8R1 = (1U << 7); + DAC1->DHR8R2 = (1U << 7); + register_set(&DAC1->MCR, 0U, 0xFFFFFFFFU); + register_set(&DAC1->CR, DAC_CR_TEN1 | (6U << DAC_CR_TSEL1_Pos) | DAC_CR_DMAEN1, 0xFFFFFFFFU); + register_set_bits(&DAC1->CR, DAC_CR_EN1 | DAC_CR_EN2); +} + +void siren_dma_init(void) { + // 1Vpp sine wave with 1V offset + static const uint8_t fake_siren_lut[360] = { 134U, 135U, 137U, 138U, 139U, 140U, 141U, 143U, 144U, 145U, 146U, 148U, 149U, 150U, 151U, 152U, 154U, 155U, 156U, 157U, 158U, 159U, 160U, 162U, 163U, 164U, 165U, 166U, 167U, 168U, 169U, 170U, 171U, 172U, 174U, 175U, 176U, 177U, 177U, 178U, 179U, 180U, 181U, 182U, 183U, 184U, 185U, 186U, 186U, 187U, 188U, 189U, 190U, 190U, 191U, 192U, 193U, 193U, 194U, 195U, 195U, 196U, 196U, 197U, 197U, 198U, 199U, 199U, 199U, 200U, 200U, 201U, 201U, 202U, 202U, 202U, 203U, 203U, 203U, 203U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 205U, 205U, 205U, 205U, 205U, 205U, 205U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 203U, 203U, 203U, 203U, 202U, 202U, 202U, 201U, 201U, 200U, 200U, 199U, 199U, 199U, 198U, 197U, 197U, 196U, 196U, 195U, 195U, 194U, 193U, 193U, 192U, 191U, 190U, 190U, 189U, 188U, 187U, 186U, 186U, 185U, 184U, 183U, 182U, 181U, 180U, 179U, 178U, 177U, 177U, 176U, 175U, 174U, 172U, 171U, 170U, 169U, 168U, 167U, 166U, 165U, 164U, 163U, 162U, 160U, 159U, 158U, 157U, 156U, 155U, 154U, 152U, 151U, 150U, 149U, 148U, 146U, 145U, 144U, 143U, 141U, 140U, 139U, 138U, 137U, 135U, 134U, 133U, 132U, 130U, 129U, 128U, 127U, 125U, 124U, 123U, 122U, 121U, 119U, 118U, 117U, 116U, 115U, 113U, 112U, 111U, 110U, 109U, 108U, 106U, 105U, 104U, 103U, 102U, 101U, 100U, 99U, 98U, 97U, 96U, 95U, 94U, 93U, 92U, 91U, 90U, 89U, 88U, 87U, 86U, 85U, 84U, 83U, 82U, 82U, 81U, 80U, 79U, 78U, 78U, 77U, 76U, 76U, 75U, 74U, 74U, 73U, 72U, 72U, 71U, 71U, 70U, 70U, 69U, 69U, 68U, 68U, 67U, 67U, 67U, 66U, 66U, 66U, 65U, 65U, 65U, 65U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 63U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 65U, 65U, 65U, 65U, 66U, 66U, 66U, 67U, 67U, 67U, 68U, 68U, 69U, 69U, 70U, 70U, 71U, 71U, 72U, 72U, 73U, 74U, 74U, 75U, 76U, 76U, 77U, 78U, 78U, 79U, 80U, 81U, 82U, 82U, 83U, 84U, 85U, 86U, 87U, 88U, 89U, 90U, 91U, 92U, 93U, 94U, 95U, 96U, 97U, 98U, 99U, 100U, 101U, 102U, 103U, 104U, 105U, 106U, 108U, 109U, 110U, 111U, 112U, 113U, 115U, 116U, 117U, 118U, 119U, 121U, 122U, 123U, 124U, 125U, 127U, 128U, 129U, 130U, 132U, 133U }; + // Setup DMAMUX (DAC_CH1_DMA as input) + register_set(&DMAMUX1_Channel1->CCR, 67U, DMAMUX_CxCR_DMAREQ_ID_Msk); + register_set(&DMA1_Stream1->PAR, (uint32_t)&(DAC1->DHR8R1), 0xFFFFFFFFU); + // Setup DMA + register_set(&DMA1_Stream1->M0AR, (uint32_t)fake_siren_lut, 0xFFFFFFFFU); + register_set(&DMA1_Stream1->FCR, 0U, 0x00000083U); + DMA1_Stream1->NDTR = sizeof(fake_siren_lut); + DMA1_Stream1->CR = (0b11UL << DMA_SxCR_PL_Pos) | DMA_SxCR_MINC | DMA_SxCR_CIRC | (1U << DMA_SxCR_DIR_Pos); +} + +void fake_siren_codec_enable(bool enabled) { + if (enabled) { + bool success = true; + success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2B, (1U << 1)); // Left speaker mix from INA1 + success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2C, (1U << 1)); // Right speaker mix from INA1 + success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3D, 0x17, 0b11111); // Left speaker volume + success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3E, 0x17, 0b11111); // Right speaker volume + success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x37, 0b101, 0b111); // INA gain + success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x4C, (1U << 7)); // Enable INA + success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x51, (1U << 7)); // Disable global shutdown + if (!success) { + print("Siren codec enable failed\n"); + fault_occurred(FAULT_SIREN_MALFUNCTION); + } + } else { + // Disable INA input. Make sure to retry a few times if the I2C bus is busy. + for (uint8_t i=0U; i<10U; i++) { + if (i2c_clear_reg_bits(I2C5, CODEC_I2C_ADDR, 0x4C, (1U << 7))) { + break; + } + } + } +} + +static void fake_i2c_siren_init(void) { + siren_dac_init(); + siren_dma_init(); + siren_tim7_init(); + // Enable the I2C to the codec + i2c_init(I2C5); + fake_siren_codec_enable(false); +} + +void fake_i2c_siren_set(bool enabled) { + static bool initialized = false; + static bool fake_siren_enabled = false; + + if (!initialized) { + fake_i2c_siren_init(); + initialized = true; + } + + if (enabled != fake_siren_enabled) { + fake_siren_codec_enable(enabled); + } + + if (enabled) { + register_set_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); + } else { + register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); + } + fake_siren_enabled = enabled; +} + +void fake_siren_set(bool enabled) { + static bool initialized = false; + static bool fake_siren_enabled = false; + + if (!initialized) { + siren_tim7_init(); + initialized = true; + } + + if (enabled != fake_siren_enabled) { + if (enabled) { + sound_stop_dac(); + siren_dac_init(); + siren_dma_init(); + current_board->set_amp_enabled(true); + register_set_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); + } else { + current_board->set_amp_enabled(false); + // Stop modified 8-bit DAC and start normal 12-bit DAC + sound_stop_dac(); + sound_init_dac(); + register_set_bits(&BDMA_Channel0->CCR, BDMA_CCR_EN); + } + fake_siren_enabled = enabled; + } +} diff --git a/board/drivers/fake_siren.h b/board/drivers/fake_siren.h index 3a4d8a36a57..1ff7dda5b7a 100644 --- a/board/drivers/fake_siren.h +++ b/board/drivers/fake_siren.h @@ -1,115 +1,14 @@ -#include "board/stm32h7/lli2c.h" - -#define CODEC_I2C_ADDR 0x10 - -void siren_tim7_init(void) { - // Init trigger timer (around 2.5kHz) - register_set(&TIM7->PSC, 0U, 0xFFFFU); - register_set(&TIM7->ARR, 133U, 0xFFFFU); - register_set(&TIM7->CR2, (0b10U << TIM_CR2_MMS_Pos), TIM_CR2_MMS_Msk); - register_set(&TIM7->CR1, TIM_CR1_ARPE | TIM_CR1_URS, 0x088EU); - TIM7->SR = 0U; - TIM7->CR1 |= TIM_CR1_CEN; -} - -void siren_dac_init(void) { - DAC1->DHR8R1 = (1U << 7); - DAC1->DHR8R2 = (1U << 7); - register_set(&DAC1->MCR, 0U, 0xFFFFFFFFU); - register_set(&DAC1->CR, DAC_CR_TEN1 | (6U << DAC_CR_TSEL1_Pos) | DAC_CR_DMAEN1, 0xFFFFFFFFU); - register_set_bits(&DAC1->CR, DAC_CR_EN1 | DAC_CR_EN2); -} - -void siren_dma_init(void) { - // 1Vpp sine wave with 1V offset - static const uint8_t fake_siren_lut[360] = { 134U, 135U, 137U, 138U, 139U, 140U, 141U, 143U, 144U, 145U, 146U, 148U, 149U, 150U, 151U, 152U, 154U, 155U, 156U, 157U, 158U, 159U, 160U, 162U, 163U, 164U, 165U, 166U, 167U, 168U, 169U, 170U, 171U, 172U, 174U, 175U, 176U, 177U, 177U, 178U, 179U, 180U, 181U, 182U, 183U, 184U, 185U, 186U, 186U, 187U, 188U, 189U, 190U, 190U, 191U, 192U, 193U, 193U, 194U, 195U, 195U, 196U, 196U, 197U, 197U, 198U, 199U, 199U, 199U, 200U, 200U, 201U, 201U, 202U, 202U, 202U, 203U, 203U, 203U, 203U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 205U, 205U, 205U, 205U, 205U, 205U, 205U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 203U, 203U, 203U, 203U, 202U, 202U, 202U, 201U, 201U, 200U, 200U, 199U, 199U, 199U, 198U, 197U, 197U, 196U, 196U, 195U, 195U, 194U, 193U, 193U, 192U, 191U, 190U, 190U, 189U, 188U, 187U, 186U, 186U, 185U, 184U, 183U, 182U, 181U, 180U, 179U, 178U, 177U, 177U, 176U, 175U, 174U, 172U, 171U, 170U, 169U, 168U, 167U, 166U, 165U, 164U, 163U, 162U, 160U, 159U, 158U, 157U, 156U, 155U, 154U, 152U, 151U, 150U, 149U, 148U, 146U, 145U, 144U, 143U, 141U, 140U, 139U, 138U, 137U, 135U, 134U, 133U, 132U, 130U, 129U, 128U, 127U, 125U, 124U, 123U, 122U, 121U, 119U, 118U, 117U, 116U, 115U, 113U, 112U, 111U, 110U, 109U, 108U, 106U, 105U, 104U, 103U, 102U, 101U, 100U, 99U, 98U, 97U, 96U, 95U, 94U, 93U, 92U, 91U, 90U, 89U, 88U, 87U, 86U, 85U, 84U, 83U, 82U, 82U, 81U, 80U, 79U, 78U, 78U, 77U, 76U, 76U, 75U, 74U, 74U, 73U, 72U, 72U, 71U, 71U, 70U, 70U, 69U, 69U, 68U, 68U, 67U, 67U, 67U, 66U, 66U, 66U, 65U, 65U, 65U, 65U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 63U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 65U, 65U, 65U, 65U, 66U, 66U, 66U, 67U, 67U, 67U, 68U, 68U, 69U, 69U, 70U, 70U, 71U, 71U, 72U, 72U, 73U, 74U, 74U, 75U, 76U, 76U, 77U, 78U, 78U, 79U, 80U, 81U, 82U, 82U, 83U, 84U, 85U, 86U, 87U, 88U, 89U, 90U, 91U, 92U, 93U, 94U, 95U, 96U, 97U, 98U, 99U, 100U, 101U, 102U, 103U, 104U, 105U, 106U, 108U, 109U, 110U, 111U, 112U, 113U, 115U, 116U, 117U, 118U, 119U, 121U, 122U, 123U, 124U, 125U, 127U, 128U, 129U, 130U, 132U, 133U }; - // Setup DMAMUX (DAC_CH1_DMA as input) - register_set(&DMAMUX1_Channel1->CCR, 67U, DMAMUX_CxCR_DMAREQ_ID_Msk); - register_set(&DMA1_Stream1->PAR, (uint32_t)&(DAC1->DHR8R1), 0xFFFFFFFFU); - // Setup DMA - register_set(&DMA1_Stream1->M0AR, (uint32_t)fake_siren_lut, 0xFFFFFFFFU); - register_set(&DMA1_Stream1->FCR, 0U, 0x00000083U); - DMA1_Stream1->NDTR = sizeof(fake_siren_lut); - DMA1_Stream1->CR = (0b11UL << DMA_SxCR_PL_Pos) | DMA_SxCR_MINC | DMA_SxCR_CIRC | (1U << DMA_SxCR_DIR_Pos); -} - -void fake_siren_codec_enable(bool enabled) { - if (enabled) { - bool success = true; - success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2B, (1U << 1)); // Left speaker mix from INA1 - success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2C, (1U << 1)); // Right speaker mix from INA1 - success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3D, 0x17, 0b11111); // Left speaker volume - success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3E, 0x17, 0b11111); // Right speaker volume - success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x37, 0b101, 0b111); // INA gain - success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x4C, (1U << 7)); // Enable INA - success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x51, (1U << 7)); // Disable global shutdown - if (!success) { - print("Siren codec enable failed\n"); - fault_occurred(FAULT_SIREN_MALFUNCTION); - } - } else { - // Disable INA input. Make sure to retry a few times if the I2C bus is busy. - for (uint8_t i=0U; i<10U; i++) { - if (i2c_clear_reg_bits(I2C5, CODEC_I2C_ADDR, 0x4C, (1U << 7))) { - break; - } - } - } -} +#pragma once -static void fake_i2c_siren_init(void) { - siren_dac_init(); - siren_dma_init(); - siren_tim7_init(); - // Enable the I2C to the codec - i2c_init(I2C5); - fake_siren_codec_enable(false); -} +#include -void fake_i2c_siren_set(bool enabled) { - static bool initialized = false; - static bool fake_siren_enabled = false; - - if (!initialized) { - fake_i2c_siren_init(); - initialized = true; - } - - if (enabled != fake_siren_enabled) { - fake_siren_codec_enable(enabled); - } - - if (enabled) { - register_set_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); - } else { - register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); - } - fake_siren_enabled = enabled; -} - -void fake_siren_set(bool enabled) { - static bool initialized = false; - static bool fake_siren_enabled = false; +#include "board/stm32h7/lli2c.h" - if (!initialized) { - siren_tim7_init(); - initialized = true; - } +#define CODEC_I2C_ADDR 0x10 - if (enabled != fake_siren_enabled) { - if (enabled) { - sound_stop_dac(); - siren_dac_init(); - siren_dma_init(); - current_board->set_amp_enabled(true); - register_set_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); - } else { - current_board->set_amp_enabled(false); - // Stop modified 8-bit DAC and start normal 12-bit DAC - sound_stop_dac(); - sound_init_dac(); - register_set_bits(&BDMA_Channel0->CCR, BDMA_CCR_EN); - } - fake_siren_enabled = enabled; - } -} +void siren_tim7_init(void); +void siren_dac_init(void); +void siren_dma_init(void); +void fake_siren_codec_enable(bool enabled); +void fake_i2c_siren_set(bool enabled); +void fake_siren_set(bool enabled); \ No newline at end of file diff --git a/board/drivers/fan.c b/board/drivers/fan.c new file mode 100644 index 00000000000..f3154448275 --- /dev/null +++ b/board/drivers/fan.c @@ -0,0 +1,51 @@ +#include "fan.h" +#include "board/utils.h" +#include "board/globals.h" +#include "board/drivers/pwm.h" + +struct fan_state_t fan_state; + +static const uint8_t FAN_TICK_FREQ = 8U; + +void fan_set_power(uint8_t percentage) { + if (percentage > 0U) { + fan_state.power = CLAMP(percentage, 20U, 100U); + } else { + fan_state.power = 0U; + } +} + +void fan_init(void) { + fan_state.cooldown_counter = current_board->fan_enable_cooldown_time * FAN_TICK_FREQ; + llfan_init(); +} + +// Call this at FAN_TICK_FREQ +void fan_tick(void) { + if (current_board->has_fan) { + // Measure fan RPM + uint16_t fan_rpm_fast = fan_state.tach_counter * (60U * FAN_TICK_FREQ / 4U); // 4 interrupts per rotation + fan_state.tach_counter = 0U; + fan_state.rpm = (fan_rpm_fast + (3U * fan_state.rpm)) / 4U; + + #ifdef DEBUG_FAN + puth(fan_state.target_rpm); + print(" "); puth(fan_rpm_fast); + print(" "); puth(fan_state.power); + print("\n"); + #endif + + // Cooldown counter to prevent noise on tachometer line. + if (fan_state.power > 0U) { + fan_state.cooldown_counter = current_board->fan_enable_cooldown_time * FAN_TICK_FREQ; + } else { + if (fan_state.cooldown_counter > 0U) { + fan_state.cooldown_counter--; + } + } + + // Set PWM and enable line + pwm_set(TIM3, 3, fan_state.power); + current_board->set_fan_enabled((fan_state.power > 0U) || (fan_state.cooldown_counter > 0U)); + } +} diff --git a/board/drivers/fan.h b/board/drivers/fan.h index f1041ce8563..f08a6270399 100644 --- a/board/drivers/fan.h +++ b/board/drivers/fan.h @@ -1,48 +1,21 @@ -#include "fan_declarations.h" - -struct fan_state_t fan_state; - -static const uint8_t FAN_TICK_FREQ = 8U; - -void fan_set_power(uint8_t percentage) { - if (percentage > 0U) { - fan_state.power = CLAMP(percentage, 20U, 100U); - } else { - fan_state.power = 0U; - } -} - -void fan_init(void) { - fan_state.cooldown_counter = current_board->fan_enable_cooldown_time * FAN_TICK_FREQ; - llfan_init(); -} - +#pragma once + +#include + +struct fan_state_t { + uint16_t tach_counter; + uint16_t rpm; + uint8_t power; + float error_integral; + uint8_t cooldown_counter; +}; +extern struct fan_state_t fan_state; + +void fan_set_power(uint8_t percentage); +void llfan_init(void); +void fan_init(void); // Call this at FAN_TICK_FREQ -void fan_tick(void) { - if (current_board->has_fan) { - // Measure fan RPM - uint16_t fan_rpm_fast = fan_state.tach_counter * (60U * FAN_TICK_FREQ / 4U); // 4 interrupts per rotation - fan_state.tach_counter = 0U; - fan_state.rpm = (fan_rpm_fast + (3U * fan_state.rpm)) / 4U; - - #ifdef DEBUG_FAN - puth(fan_state.target_rpm); - print(" "); puth(fan_rpm_fast); - print(" "); puth(fan_state.power); - print("\n"); - #endif - - // Cooldown counter to prevent noise on tachometer line. - if (fan_state.power > 0U) { - fan_state.cooldown_counter = current_board->fan_enable_cooldown_time * FAN_TICK_FREQ; - } else { - if (fan_state.cooldown_counter > 0U) { - fan_state.cooldown_counter--; - } - } - - // Set PWM and enable line - pwm_set(TIM3, 3, fan_state.power); - current_board->set_fan_enabled((fan_state.power > 0U) || (fan_state.cooldown_counter > 0U)); - } -} +void fan_tick(void); +void fan_set_power(uint8_t percentage); +void fan_init(void); +void fan_tick(void); \ No newline at end of file diff --git a/board/drivers/fan_declarations.h b/board/drivers/fan_declarations.h deleted file mode 100644 index 3dd3e7cfabf..00000000000 --- a/board/drivers/fan_declarations.h +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -struct fan_state_t { - uint16_t tach_counter; - uint16_t rpm; - uint8_t power; - float error_integral; - uint8_t cooldown_counter; -}; -extern struct fan_state_t fan_state; - -void fan_set_power(uint8_t percentage); -void llfan_init(void); -void fan_init(void); -// Call this at FAN_TICK_FREQ -void fan_tick(void); diff --git a/board/drivers/fdcan.c b/board/drivers/fdcan.c new file mode 100644 index 00000000000..05665455143 --- /dev/null +++ b/board/drivers/fdcan.c @@ -0,0 +1,279 @@ +#include "fdcan.h" +// #include "board/stm32h7/stm32h7_config.h" + +#include "can_common.h" + +#include "board/config.h" + +#include "led.h" + +// FIXME! +#include "board/stm32h7/llfdcan.h" + +FDCAN_GlobalTypeDef *cans[PANDA_CAN_CNT] = {FDCAN1, FDCAN2, FDCAN3}; + +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); + + ret &= llcan_set_speed( + FDCANx, + bus_config[bus_number].can_speed, + bus_config[bus_number].can_data_speed, + bus_config[bus_number].canfd_non_iso, + can_loopback, + can_silent + ); + return ret; +} + +void can_clear_send(FDCAN_GlobalTypeDef *FDCANx, uint8_t can_number) { + static uint32_t last_reset = 0U; + uint32_t time = microsecond_timer_get(); + + // Resetting CAN core is a slow blocking operation, limit frequency + if (get_ts_elapsed(time, last_reset) > 100000U) { // 10 Hz + can_health[can_number].can_core_reset_cnt += 1U; + can_health[can_number].total_tx_lost_cnt += (FDCAN_TX_FIFO_EL_CNT - (FDCANx->TXFQS & FDCAN_TXFQS_TFFL)); // TX FIFO msgs will be lost after reset + llcan_clear_send(FDCANx); + last_reset = time; + } +} + +void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg) { + uint8_t can_irq_number[PANDA_CAN_CNT][2] = { + { FDCAN1_IT0_IRQn, FDCAN1_IT1_IRQn }, + { FDCAN2_IT0_IRQn, FDCAN2_IT1_IRQn }, + { FDCAN3_IT0_IRQn, FDCAN3_IT1_IRQn }, + }; + + FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number); + uint32_t psr_reg = FDCANx->PSR; + uint32_t ecr_reg = FDCANx->ECR; + + can_health[can_number].bus_off = ((psr_reg & FDCAN_PSR_BO) >> FDCAN_PSR_BO_Pos); + can_health[can_number].bus_off_cnt += can_health[can_number].bus_off; + can_health[can_number].error_warning = ((psr_reg & FDCAN_PSR_EW) >> FDCAN_PSR_EW_Pos); + can_health[can_number].error_passive = ((psr_reg & FDCAN_PSR_EP) >> FDCAN_PSR_EP_Pos); + + can_health[can_number].last_error = ((psr_reg & FDCAN_PSR_LEC) >> FDCAN_PSR_LEC_Pos); + if ((can_health[can_number].last_error != 0U) && (can_health[can_number].last_error != 7U)) { + can_health[can_number].last_stored_error = can_health[can_number].last_error; + } + + can_health[can_number].last_data_error = ((psr_reg & FDCAN_PSR_DLEC) >> FDCAN_PSR_DLEC_Pos); + if ((can_health[can_number].last_data_error != 0U) && (can_health[can_number].last_data_error != 7U)) { + can_health[can_number].last_data_stored_error = can_health[can_number].last_data_error; + } + + can_health[can_number].receive_error_cnt = ((ecr_reg & FDCAN_ECR_REC) >> FDCAN_ECR_REC_Pos); + can_health[can_number].transmit_error_cnt = ((ecr_reg & FDCAN_ECR_TEC) >> FDCAN_ECR_TEC_Pos); + + can_health[can_number].irq0_call_rate = interrupts[can_irq_number[can_number][0]].call_rate; + can_health[can_number].irq1_call_rate = interrupts[can_irq_number[can_number][1]].call_rate; + + if (ir_reg != 0U) { + // Clear error interrupts + FDCANx->IR |= (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EP | FDCAN_IR_BO | FDCAN_IR_RF0L); + can_health[can_number].total_error_cnt += 1U; + // Check for RX FIFO overflow + if ((ir_reg & (FDCAN_IR_RF0L)) != 0U) { + can_health[can_number].total_rx_lost_cnt += 1U; + } + // Cases: + // 1. while multiplexing between buses 1 and 3 we are getting ACK errors that overwhelm CAN core, by resetting it recovers faster + // 2. H7 gets stuck in bus off recovery state indefinitely + if ((((can_health[can_number].last_error == CAN_ACK_ERROR) || (can_health[can_number].last_data_error == CAN_ACK_ERROR)) && (can_health[can_number].transmit_error_cnt > 127U)) || + ((ir_reg & FDCAN_IR_BO) != 0U)) { + can_clear_send(FDCANx, can_number); + } + } +} + +// ***************************** CAN ***************************** +// FDFDCANx_IT1 IRQ Handler (TX) +void process_can(uint8_t can_number) { + if (can_number != 0xffU) { + ENTER_CRITICAL(); + + FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number); + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); + + FDCANx->IR |= FDCAN_IR_TFE; // Clear Tx FIFO Empty flag + + if ((FDCANx->TXFQS & FDCAN_TXFQS_TFQF) == 0U) { + CANPacket_t to_send; + if (can_pop(can_queues[bus_number], &to_send)) { + if (can_check_checksum(&to_send)) { + can_health[can_number].total_tx_cnt += 1U; + + uint32_t TxFIFOSA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET) + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE); + // get the index of the next TX FIFO element (0 to FDCAN_TX_FIFO_EL_CNT - 1) + uint32_t tx_index = (FDCANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1FU; + // only send if we have received a packet + canfd_fifo *fifo; + fifo = (canfd_fifo *)(TxFIFOSA + (tx_index * FDCAN_TX_FIFO_EL_SIZE)); + + fifo->header[0] = (to_send.extended << 30) | ((to_send.extended != 0U) ? (to_send.addr) : (to_send.addr << 18)); + + // If canfd_auto is set, outgoing packets will be automatically sent as CAN-FD if an incoming CAN-FD packet was seen + bool fd = bus_config[can_number].canfd_auto ? bus_config[can_number].canfd_enabled : (bool)(to_send.fd > 0U); + uint32_t canfd_enabled_header = fd ? (1UL << 21) : 0UL; + + uint32_t brs_enabled_header = bus_config[can_number].brs_enabled ? (1UL << 20) : 0UL; + fifo->header[1] = (to_send.data_len_code << 16) | canfd_enabled_header | brs_enabled_header; + + uint8_t data_len_w = (dlc_to_len[to_send.data_len_code] / 4U); + data_len_w += ((dlc_to_len[to_send.data_len_code] % 4U) > 0U) ? 1U : 0U; + for (unsigned int i = 0; i < data_len_w; i++) { + BYTE_ARRAY_TO_WORD(fifo->data_word[i], &to_send.data[i*4U]); + } + + FDCANx->TXBAR = (1UL << tx_index); + + // Send back to USB + CANPacket_t to_push; + + to_push.fd = fd; + to_push.returned = 1U; + to_push.rejected = 0U; + to_push.extended = to_send.extended; + to_push.addr = to_send.addr; + to_push.bus = bus_number; + to_push.data_len_code = to_send.data_len_code; + (void)memcpy(to_push.data, to_send.data, dlc_to_len[to_push.data_len_code]); + can_set_checksum(&to_push); + + rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U; + } else { + can_health[can_number].total_tx_checksum_error_cnt += 1U; + } + + refresh_can_tx_slots_available(); + } + } + EXIT_CRITICAL(); + } +} + +// FDFDCANx_IT0 IRQ Handler (RX and errors) +// blink blue when we are receiving CAN messages +void can_rx(uint8_t can_number) { + FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number); + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); + + uint32_t ir_reg = FDCANx->IR; + + // Clear all new messages from Rx FIFO 0 + FDCANx->IR |= FDCAN_IR_RF0N; + while ((FDCANx->RXF0S & FDCAN_RXF0S_F0FL) != 0U) { + can_health[can_number].total_rx_cnt += 1U; + // get the index of the next RX FIFO element (0 to FDCAN_RX_FIFO_0_EL_CNT - 1) + uint32_t rx_fifo_idx = (uint8_t)((FDCANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3FU); + + // Recommended to offset get index by at least +1 if RX FIFO is in overwrite mode and full (datasheet) + if ((FDCANx->RXF0S & FDCAN_RXF0S_F0F) == FDCAN_RXF0S_F0F) { + rx_fifo_idx = ((rx_fifo_idx + 1U) >= FDCAN_RX_FIFO_0_EL_CNT) ? 0U : (rx_fifo_idx + 1U); + can_health[can_number].total_rx_lost_cnt += 1U; // At least one message was lost + } + + uint32_t RxFIFO0SA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET); + CANPacket_t to_push; + canfd_fifo *fifo; + + // getting address + fifo = (canfd_fifo *)(RxFIFO0SA + (rx_fifo_idx * FDCAN_RX_FIFO_0_EL_SIZE)); + + bool canfd_frame = ((fifo->header[1] >> 21) & 0x1U); + bool brs_frame = ((fifo->header[1] >> 20) & 0x1U); + + to_push.fd = canfd_frame; + to_push.returned = 0U; + to_push.rejected = 0U; + to_push.extended = (fifo->header[0] >> 30) & 0x1U; + to_push.addr = ((to_push.extended != 0U) ? (fifo->header[0] & 0x1FFFFFFFU) : ((fifo->header[0] >> 18) & 0x7FFU)); + to_push.bus = bus_number; + to_push.data_len_code = ((fifo->header[1] >> 16) & 0xFU); + + uint8_t data_len_w = (dlc_to_len[to_push.data_len_code] / 4U); + data_len_w += ((dlc_to_len[to_push.data_len_code] % 4U) > 0U) ? 1U : 0U; + for (unsigned int i = 0; i < data_len_w; i++) { + WORD_TO_BYTE_ARRAY(&to_push.data[i*4U], fifo->data_word[i]); + } + can_set_checksum(&to_push); + + // forwarding (panda only) + int bus_fwd_num = safety_fwd_hook(bus_number, to_push.addr); + if (bus_fwd_num < 0) { + bus_fwd_num = bus_config[can_number].forwarding_bus; + } + if (bus_fwd_num != -1) { + CANPacket_t to_send; + + to_send.fd = to_push.fd; + to_send.returned = 0U; + to_send.rejected = 0U; + to_send.extended = to_push.extended; + to_send.addr = to_push.addr; + to_send.bus = to_push.bus; + to_send.data_len_code = to_push.data_len_code; + (void)memcpy(to_send.data, to_push.data, dlc_to_len[to_push.data_len_code]); + can_set_checksum(&to_send); + + can_send(&to_send, bus_fwd_num, true); + can_health[can_number].total_fwd_cnt += 1U; + } + + safety_rx_invalid += safety_rx_hook(&to_push) ? 0U : 1U; + ignition_can_hook(&to_push); + + led_set(LED_BLUE, true); + rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U; + + // Enable CAN FD and BRS if CAN FD message was received + if (!(bus_config[can_number].canfd_enabled) && (canfd_frame)) { + bus_config[can_number].canfd_enabled = true; + } + if (!(bus_config[can_number].brs_enabled) && (brs_frame)) { + bus_config[can_number].brs_enabled = true; + } + + // update read index + FDCANx->RXF0A = rx_fifo_idx; + } + + // Error handling + if ((ir_reg & (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EP | FDCAN_IR_BO | FDCAN_IR_RF0L)) != 0U) { + update_can_health_pkt(can_number, ir_reg); + } +} + +static void FDCAN1_IT0_IRQ_Handler(void) { can_rx(0); } +static void FDCAN1_IT1_IRQ_Handler(void) { process_can(0); } + +static void FDCAN2_IT0_IRQ_Handler(void) { can_rx(1); } +static void FDCAN2_IT1_IRQ_Handler(void) { process_can(1); } + +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; + + REGISTER_INTERRUPT(FDCAN1_IT0_IRQn, FDCAN1_IT0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) + REGISTER_INTERRUPT(FDCAN1_IT1_IRQn, FDCAN1_IT1_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) + REGISTER_INTERRUPT(FDCAN2_IT0_IRQn, FDCAN2_IT0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2) + REGISTER_INTERRUPT(FDCAN2_IT1_IRQn, FDCAN2_IT1_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2) + REGISTER_INTERRUPT(FDCAN3_IT0_IRQn, FDCAN3_IT0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3) + REGISTER_INTERRUPT(FDCAN3_IT1_IRQn, FDCAN3_IT1_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3) + + if (can_number != 0xffU) { + FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number); + ret &= can_set_speed(can_number); + ret &= llcan_init(FDCANx); + // in case there are queued up messages + process_can(can_number); + } + return ret; +} diff --git a/board/drivers/fdcan.h b/board/drivers/fdcan.h index 2aa64e97dcc..a7a488d3be8 100644 --- a/board/drivers/fdcan.h +++ b/board/drivers/fdcan.h @@ -1,269 +1,27 @@ -#include "fdcan_declarations.h" +#pragma once -FDCAN_GlobalTypeDef *cans[PANDA_CAN_CNT] = {FDCAN1, FDCAN2, FDCAN3}; +#include -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); +#include "board/can.h" +#include "board/stm32h7/stm32h7_config.h" - ret &= llcan_set_speed( - FDCANx, - bus_config[bus_number].can_speed, - bus_config[bus_number].can_data_speed, - bus_config[bus_number].canfd_non_iso, - can_loopback, - can_silent - ); - return ret; -} +typedef struct { + volatile uint32_t header[2]; + volatile uint32_t data_word[CANPACKET_DATA_SIZE_MAX/4U]; +} canfd_fifo; -void can_clear_send(FDCAN_GlobalTypeDef *FDCANx, uint8_t can_number) { - static uint32_t last_reset = 0U; - uint32_t time = microsecond_timer_get(); +extern FDCAN_GlobalTypeDef *cans[PANDA_CAN_CNT]; - // Resetting CAN core is a slow blocking operation, limit frequency - if (get_ts_elapsed(time, last_reset) > 100000U) { // 10 Hz - can_health[can_number].can_core_reset_cnt += 1U; - can_health[can_number].total_tx_lost_cnt += (FDCAN_TX_FIFO_EL_CNT - (FDCANx->TXFQS & FDCAN_TXFQS_TFFL)); // TX FIFO msgs will be lost after reset - llcan_clear_send(FDCANx); - last_reset = time; - } -} +#define CAN_ACK_ERROR 3U -void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg) { - uint8_t can_irq_number[PANDA_CAN_CNT][2] = { - { FDCAN1_IT0_IRQn, FDCAN1_IT1_IRQn }, - { FDCAN2_IT0_IRQn, FDCAN2_IT1_IRQn }, - { FDCAN3_IT0_IRQn, FDCAN3_IT1_IRQn }, - }; - - FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number); - uint32_t psr_reg = FDCANx->PSR; - uint32_t ecr_reg = FDCANx->ECR; - - can_health[can_number].bus_off = ((psr_reg & FDCAN_PSR_BO) >> FDCAN_PSR_BO_Pos); - can_health[can_number].bus_off_cnt += can_health[can_number].bus_off; - can_health[can_number].error_warning = ((psr_reg & FDCAN_PSR_EW) >> FDCAN_PSR_EW_Pos); - can_health[can_number].error_passive = ((psr_reg & FDCAN_PSR_EP) >> FDCAN_PSR_EP_Pos); - - can_health[can_number].last_error = ((psr_reg & FDCAN_PSR_LEC) >> FDCAN_PSR_LEC_Pos); - if ((can_health[can_number].last_error != 0U) && (can_health[can_number].last_error != 7U)) { - can_health[can_number].last_stored_error = can_health[can_number].last_error; - } - - can_health[can_number].last_data_error = ((psr_reg & FDCAN_PSR_DLEC) >> FDCAN_PSR_DLEC_Pos); - if ((can_health[can_number].last_data_error != 0U) && (can_health[can_number].last_data_error != 7U)) { - can_health[can_number].last_data_stored_error = can_health[can_number].last_data_error; - } - - can_health[can_number].receive_error_cnt = ((ecr_reg & FDCAN_ECR_REC) >> FDCAN_ECR_REC_Pos); - can_health[can_number].transmit_error_cnt = ((ecr_reg & FDCAN_ECR_TEC) >> FDCAN_ECR_TEC_Pos); - - can_health[can_number].irq0_call_rate = interrupts[can_irq_number[can_number][0]].call_rate; - can_health[can_number].irq1_call_rate = interrupts[can_irq_number[can_number][1]].call_rate; - - if (ir_reg != 0U) { - // Clear error interrupts - FDCANx->IR |= (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EP | FDCAN_IR_BO | FDCAN_IR_RF0L); - can_health[can_number].total_error_cnt += 1U; - // Check for RX FIFO overflow - if ((ir_reg & (FDCAN_IR_RF0L)) != 0U) { - can_health[can_number].total_rx_lost_cnt += 1U; - } - // Cases: - // 1. while multiplexing between buses 1 and 3 we are getting ACK errors that overwhelm CAN core, by resetting it recovers faster - // 2. H7 gets stuck in bus off recovery state indefinitely - if ((((can_health[can_number].last_error == CAN_ACK_ERROR) || (can_health[can_number].last_data_error == CAN_ACK_ERROR)) && (can_health[can_number].transmit_error_cnt > 127U)) || - ((ir_reg & FDCAN_IR_BO) != 0U)) { - can_clear_send(FDCANx, can_number); - } - } -} +void can_clear_send(FDCAN_GlobalTypeDef *FDCANx, uint8_t can_number); +void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg); // ***************************** CAN ***************************** // FDFDCANx_IT1 IRQ Handler (TX) -void process_can(uint8_t can_number) { - if (can_number != 0xffU) { - ENTER_CRITICAL(); - - FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number); - uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); - - FDCANx->IR |= FDCAN_IR_TFE; // Clear Tx FIFO Empty flag - - if ((FDCANx->TXFQS & FDCAN_TXFQS_TFQF) == 0U) { - CANPacket_t to_send; - if (can_pop(can_queues[bus_number], &to_send)) { - if (can_check_checksum(&to_send)) { - can_health[can_number].total_tx_cnt += 1U; - - uint32_t TxFIFOSA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET) + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE); - // get the index of the next TX FIFO element (0 to FDCAN_TX_FIFO_EL_CNT - 1) - uint32_t tx_index = (FDCANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1FU; - // only send if we have received a packet - canfd_fifo *fifo; - fifo = (canfd_fifo *)(TxFIFOSA + (tx_index * FDCAN_TX_FIFO_EL_SIZE)); - - fifo->header[0] = (to_send.extended << 30) | ((to_send.extended != 0U) ? (to_send.addr) : (to_send.addr << 18)); - - // If canfd_auto is set, outgoing packets will be automatically sent as CAN-FD if an incoming CAN-FD packet was seen - bool fd = bus_config[can_number].canfd_auto ? bus_config[can_number].canfd_enabled : (bool)(to_send.fd > 0U); - uint32_t canfd_enabled_header = fd ? (1UL << 21) : 0UL; - - uint32_t brs_enabled_header = bus_config[can_number].brs_enabled ? (1UL << 20) : 0UL; - fifo->header[1] = (to_send.data_len_code << 16) | canfd_enabled_header | brs_enabled_header; - - uint8_t data_len_w = (dlc_to_len[to_send.data_len_code] / 4U); - data_len_w += ((dlc_to_len[to_send.data_len_code] % 4U) > 0U) ? 1U : 0U; - for (unsigned int i = 0; i < data_len_w; i++) { - BYTE_ARRAY_TO_WORD(fifo->data_word[i], &to_send.data[i*4U]); - } - - FDCANx->TXBAR = (1UL << tx_index); - - // Send back to USB - CANPacket_t to_push; - - to_push.fd = fd; - to_push.returned = 1U; - to_push.rejected = 0U; - to_push.extended = to_send.extended; - to_push.addr = to_send.addr; - to_push.bus = bus_number; - to_push.data_len_code = to_send.data_len_code; - (void)memcpy(to_push.data, to_send.data, dlc_to_len[to_push.data_len_code]); - can_set_checksum(&to_push); - - rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U; - } else { - can_health[can_number].total_tx_checksum_error_cnt += 1U; - } - - refresh_can_tx_slots_available(); - } - } - EXIT_CRITICAL(); - } -} +void process_can(uint8_t can_number); // FDFDCANx_IT0 IRQ Handler (RX and errors) // blink blue when we are receiving CAN messages -void can_rx(uint8_t can_number) { - FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number); - uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); - - uint32_t ir_reg = FDCANx->IR; - - // Clear all new messages from Rx FIFO 0 - FDCANx->IR |= FDCAN_IR_RF0N; - while ((FDCANx->RXF0S & FDCAN_RXF0S_F0FL) != 0U) { - can_health[can_number].total_rx_cnt += 1U; - // get the index of the next RX FIFO element (0 to FDCAN_RX_FIFO_0_EL_CNT - 1) - uint32_t rx_fifo_idx = (uint8_t)((FDCANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3FU); - - // Recommended to offset get index by at least +1 if RX FIFO is in overwrite mode and full (datasheet) - if ((FDCANx->RXF0S & FDCAN_RXF0S_F0F) == FDCAN_RXF0S_F0F) { - rx_fifo_idx = ((rx_fifo_idx + 1U) >= FDCAN_RX_FIFO_0_EL_CNT) ? 0U : (rx_fifo_idx + 1U); - can_health[can_number].total_rx_lost_cnt += 1U; // At least one message was lost - } - - uint32_t RxFIFO0SA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET); - CANPacket_t to_push; - canfd_fifo *fifo; - - // getting address - fifo = (canfd_fifo *)(RxFIFO0SA + (rx_fifo_idx * FDCAN_RX_FIFO_0_EL_SIZE)); - - bool canfd_frame = ((fifo->header[1] >> 21) & 0x1U); - bool brs_frame = ((fifo->header[1] >> 20) & 0x1U); - - to_push.fd = canfd_frame; - to_push.returned = 0U; - to_push.rejected = 0U; - to_push.extended = (fifo->header[0] >> 30) & 0x1U; - to_push.addr = ((to_push.extended != 0U) ? (fifo->header[0] & 0x1FFFFFFFU) : ((fifo->header[0] >> 18) & 0x7FFU)); - to_push.bus = bus_number; - to_push.data_len_code = ((fifo->header[1] >> 16) & 0xFU); - - uint8_t data_len_w = (dlc_to_len[to_push.data_len_code] / 4U); - data_len_w += ((dlc_to_len[to_push.data_len_code] % 4U) > 0U) ? 1U : 0U; - for (unsigned int i = 0; i < data_len_w; i++) { - WORD_TO_BYTE_ARRAY(&to_push.data[i*4U], fifo->data_word[i]); - } - can_set_checksum(&to_push); - - // forwarding (panda only) - int bus_fwd_num = safety_fwd_hook(bus_number, to_push.addr); - if (bus_fwd_num < 0) { - bus_fwd_num = bus_config[can_number].forwarding_bus; - } - if (bus_fwd_num != -1) { - CANPacket_t to_send; - - to_send.fd = to_push.fd; - to_send.returned = 0U; - to_send.rejected = 0U; - to_send.extended = to_push.extended; - to_send.addr = to_push.addr; - to_send.bus = to_push.bus; - to_send.data_len_code = to_push.data_len_code; - (void)memcpy(to_send.data, to_push.data, dlc_to_len[to_push.data_len_code]); - can_set_checksum(&to_send); - - can_send(&to_send, bus_fwd_num, true); - can_health[can_number].total_fwd_cnt += 1U; - } - - safety_rx_invalid += safety_rx_hook(&to_push) ? 0U : 1U; - ignition_can_hook(&to_push); - - led_set(LED_BLUE, true); - rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U; - - // Enable CAN FD and BRS if CAN FD message was received - if (!(bus_config[can_number].canfd_enabled) && (canfd_frame)) { - bus_config[can_number].canfd_enabled = true; - } - if (!(bus_config[can_number].brs_enabled) && (brs_frame)) { - bus_config[can_number].brs_enabled = true; - } - - // update read index - FDCANx->RXF0A = rx_fifo_idx; - } - - // Error handling - if ((ir_reg & (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EP | FDCAN_IR_BO | FDCAN_IR_RF0L)) != 0U) { - update_can_health_pkt(can_number, ir_reg); - } -} - -static void FDCAN1_IT0_IRQ_Handler(void) { can_rx(0); } -static void FDCAN1_IT1_IRQ_Handler(void) { process_can(0); } - -static void FDCAN2_IT0_IRQ_Handler(void) { can_rx(1); } -static void FDCAN2_IT1_IRQ_Handler(void) { process_can(1); } - -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; - - REGISTER_INTERRUPT(FDCAN1_IT0_IRQn, FDCAN1_IT0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) - REGISTER_INTERRUPT(FDCAN1_IT1_IRQn, FDCAN1_IT1_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) - REGISTER_INTERRUPT(FDCAN2_IT0_IRQn, FDCAN2_IT0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2) - REGISTER_INTERRUPT(FDCAN2_IT1_IRQn, FDCAN2_IT1_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2) - REGISTER_INTERRUPT(FDCAN3_IT0_IRQn, FDCAN3_IT0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3) - REGISTER_INTERRUPT(FDCAN3_IT1_IRQn, FDCAN3_IT1_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3) - - if (can_number != 0xffU) { - FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number); - ret &= can_set_speed(can_number); - ret &= llcan_init(FDCANx); - // in case there are queued up messages - process_can(can_number); - } - return ret; -} +void can_rx(uint8_t can_number); +bool can_init(uint8_t can_number); \ No newline at end of file diff --git a/board/drivers/fdcan_declarations.h b/board/drivers/fdcan_declarations.h deleted file mode 100644 index c77d1e31382..00000000000 --- a/board/drivers/fdcan_declarations.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include "board/can.h" - -typedef struct { - volatile uint32_t header[2]; - volatile uint32_t data_word[CANPACKET_DATA_SIZE_MAX/4U]; -} canfd_fifo; - -extern FDCAN_GlobalTypeDef *cans[PANDA_CAN_CNT]; - -#define CAN_ACK_ERROR 3U - -void can_clear_send(FDCAN_GlobalTypeDef *FDCANx, uint8_t can_number); -void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg); - -void process_can(uint8_t can_number); -void can_rx(uint8_t can_number); -bool can_init(uint8_t can_number); diff --git a/board/drivers/gpio.c b/board/drivers/gpio.c new file mode 100644 index 00000000000..e73ed0f92e1 --- /dev/null +++ b/board/drivers/gpio.c @@ -0,0 +1,83 @@ +#include "gpio.h" + +void set_gpio_mode(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) { + ENTER_CRITICAL(); + uint32_t tmp = GPIO->MODER; + tmp &= ~(3U << (pin * 2U)); + tmp |= (mode << (pin * 2U)); + register_set(&(GPIO->MODER), tmp, 0xFFFFFFFFU); + EXIT_CRITICAL(); +} + +void set_gpio_output(GPIO_TypeDef *GPIO, unsigned int pin, bool enabled) { + ENTER_CRITICAL(); + if (enabled) { + register_set_bits(&(GPIO->ODR), (1UL << pin)); + } else { + register_clear_bits(&(GPIO->ODR), (1UL << pin)); + } + set_gpio_mode(GPIO, pin, MODE_OUTPUT); + EXIT_CRITICAL(); +} + +void set_gpio_output_type(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int output_type){ + ENTER_CRITICAL(); + if(output_type == OUTPUT_TYPE_OPEN_DRAIN) { + register_set_bits(&(GPIO->OTYPER), (1UL << pin)); + } else { + register_clear_bits(&(GPIO->OTYPER), (1U << pin)); + } + EXIT_CRITICAL(); +} + +void set_gpio_alternate(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) { + ENTER_CRITICAL(); + uint32_t tmp = GPIO->AFR[pin >> 3U]; + tmp &= ~(0xFU << ((pin & 7U) * 4U)); + tmp |= mode << ((pin & 7U) * 4U); + register_set(&(GPIO->AFR[pin >> 3]), tmp, 0xFFFFFFFFU); + set_gpio_mode(GPIO, pin, MODE_ALTERNATE); + EXIT_CRITICAL(); +} + +void set_gpio_pullup(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) { + ENTER_CRITICAL(); + uint32_t tmp = GPIO->PUPDR; + tmp &= ~(3U << (pin * 2U)); + tmp |= (mode << (pin * 2U)); + register_set(&(GPIO->PUPDR), tmp, 0xFFFFFFFFU); + EXIT_CRITICAL(); +} + +int get_gpio_input(const GPIO_TypeDef *GPIO, unsigned int pin) { + return (GPIO->IDR & (1UL << pin)) == (1UL << pin); +} + +#ifdef PANDA_JUNGLE +typedef struct { + GPIO_TypeDef * const bank; + uint8_t pin; +} gpio_t; + +void gpio_set_all_output(gpio_t *pins, uint8_t num_pins, bool enabled) { + for (uint8_t i = 0; i < num_pins; i++) { + set_gpio_output(pins[i].bank, pins[i].pin, enabled); + } +} + +void gpio_set_bitmask(gpio_t *pins, uint8_t num_pins, uint32_t bitmask) { + for (uint8_t i = 0; i < num_pins; i++) { + set_gpio_output(pins[i].bank, pins[i].pin, (bitmask >> i) & 1U); + } +} +#endif + +#define PULL_EFFECTIVE_DELAY 4096 +bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) { + set_gpio_mode(GPIO, pin, MODE_INPUT); + set_gpio_pullup(GPIO, pin, mode); + for (volatile int i=0; iMODER; - tmp &= ~(3U << (pin * 2U)); - tmp |= (mode << (pin * 2U)); - register_set(&(GPIO->MODER), tmp, 0xFFFFFFFFU); - EXIT_CRITICAL(); -} - -void set_gpio_output(GPIO_TypeDef *GPIO, unsigned int pin, bool enabled) { - ENTER_CRITICAL(); - if (enabled) { - register_set_bits(&(GPIO->ODR), (1UL << pin)); - } else { - register_clear_bits(&(GPIO->ODR), (1UL << pin)); - } - set_gpio_mode(GPIO, pin, MODE_OUTPUT); - EXIT_CRITICAL(); -} - -void set_gpio_output_type(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int output_type){ - ENTER_CRITICAL(); - if(output_type == OUTPUT_TYPE_OPEN_DRAIN) { - register_set_bits(&(GPIO->OTYPER), (1UL << pin)); - } else { - register_clear_bits(&(GPIO->OTYPER), (1U << pin)); - } - EXIT_CRITICAL(); -} - -void set_gpio_alternate(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) { - ENTER_CRITICAL(); - uint32_t tmp = GPIO->AFR[pin >> 3U]; - tmp &= ~(0xFU << ((pin & 7U) * 4U)); - tmp |= mode << ((pin & 7U) * 4U); - register_set(&(GPIO->AFR[pin >> 3]), tmp, 0xFFFFFFFFU); - set_gpio_mode(GPIO, pin, MODE_ALTERNATE); - EXIT_CRITICAL(); -} - -void set_gpio_pullup(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) { - ENTER_CRITICAL(); - uint32_t tmp = GPIO->PUPDR; - tmp &= ~(3U << (pin * 2U)); - tmp |= (mode << (pin * 2U)); - register_set(&(GPIO->PUPDR), tmp, 0xFFFFFFFFU); - EXIT_CRITICAL(); -} - -int get_gpio_input(const GPIO_TypeDef *GPIO, unsigned int pin) { - return (GPIO->IDR & (1UL << pin)) == (1UL << pin); -} +void set_gpio_mode(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode); +void set_gpio_output(GPIO_TypeDef *GPIO, unsigned int pin, bool enabled); +void set_gpio_output_type(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int output_type); +void set_gpio_alternate(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode); +void set_gpio_pullup(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode); +int get_gpio_input(const GPIO_TypeDef *GPIO, unsigned int pin); #ifdef PANDA_JUNGLE typedef struct { @@ -83,12 +41,4 @@ void gpio_set_bitmask(gpio_t *pins, uint8_t num_pins, uint32_t bitmask) { #endif // Detection with internal pullup -#define PULL_EFFECTIVE_DELAY 4096 -bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) { - set_gpio_mode(GPIO, pin, MODE_INPUT); - set_gpio_pullup(GPIO, pin, mode); - for (volatile int i=0; iharness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, !ignition_relay); + set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, !drive_relay); + } else { + set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, !drive_relay); + set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, !ignition_relay); + } + + if (!(drive_relay || ignition_relay)) { + harness.relay_driven = false; + } +} + +bool harness_check_ignition(void) { + bool ret = false; + + // wait until we're not reading the analog voltages anymore + while (harness.sbu_adc_lock) {} + + switch(harness.status){ + case HARNESS_STATUS_NORMAL: + ret = !get_gpio_input(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1); + break; + case HARNESS_STATUS_FLIPPED: + ret = !get_gpio_input(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2); + break; + default: + break; + } + return ret; +} + +static uint8_t harness_detect_orientation(void) { + uint8_t ret = harness.status; + + #ifndef BOOTSTUB + // We can't detect orientation if the relay is being driven + if (!harness.relay_driven) { + harness.sbu_adc_lock = true; + set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_ANALOG); + set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_ANALOG); + + harness.sbu1_voltage_mV = adc_get_mV(¤t_board->harness_config->adc_signal_SBU1); + harness.sbu2_voltage_mV = adc_get_mV(¤t_board->harness_config->adc_signal_SBU2); + uint16_t detection_threshold = current_board->avdd_mV / 2U; + + // Detect connection and orientation + if((harness.sbu1_voltage_mV < detection_threshold) || (harness.sbu2_voltage_mV < detection_threshold)){ + if (harness.sbu1_voltage_mV < harness.sbu2_voltage_mV) { + // orientation flipped (PANDA_SBU1->HARNESS_SBU1(relay), PANDA_SBU2->HARNESS_SBU2(ign)) + ret = HARNESS_STATUS_FLIPPED; + } else { + // orientation normal (PANDA_SBU2->HARNESS_SBU1(relay), PANDA_SBU1->HARNESS_SBU2(ign)) + // (SBU1->SBU2 is the normal orientation connection per USB-C cable spec) + ret = HARNESS_STATUS_NORMAL; + } + } else { + ret = HARNESS_STATUS_NC; + } + + // Pins are not 5V tolerant in ADC mode + set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_INPUT); + set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_INPUT); + harness.sbu_adc_lock = false; + } + #endif + + return ret; +} + +void harness_tick(void) { + harness.status = harness_detect_orientation(); +} + +void harness_init(void) { + // init OBD_SBUx_RELAY + set_gpio_output_type(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, OUTPUT_TYPE_OPEN_DRAIN); + set_gpio_output_type(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, OUTPUT_TYPE_OPEN_DRAIN); + set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, 1); + set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, 1); + + // detect initial orientation + harness.status = harness_detect_orientation(); + + // keep buses connected by default + set_intercept_relay(false, false); +} diff --git a/board/drivers/harness.h b/board/drivers/harness.h index 3ebf109fe83..bb06765cef1 100644 --- a/board/drivers/harness.h +++ b/board/drivers/harness.h @@ -1,106 +1,40 @@ -#include "harness_declarations.h" - -struct harness_t harness; +#pragma once + +#include +#include + +#include "board/config.h" + +#define HARNESS_STATUS_NC 0U +#define HARNESS_STATUS_NORMAL 1U +#define HARNESS_STATUS_FLIPPED 2U + +struct harness_t { + uint8_t status; + uint16_t sbu1_voltage_mV; + uint16_t sbu2_voltage_mV; + bool relay_driven; + bool sbu_adc_lock; +}; +extern struct harness_t harness; + +struct harness_configuration { + GPIO_TypeDef * const GPIO_SBU1; + GPIO_TypeDef * const GPIO_SBU2; + GPIO_TypeDef * const GPIO_relay_SBU1; + GPIO_TypeDef * const GPIO_relay_SBU2; + const uint8_t pin_SBU1; + const uint8_t pin_SBU2; + const uint8_t pin_relay_SBU1; + const uint8_t pin_relay_SBU2; + const adc_signal_t adc_signal_SBU1; + const adc_signal_t adc_signal_SBU2; +}; + +typedef struct harness_configuration harness_configuration; // The ignition relay is only used for testing purposes -void set_intercept_relay(bool intercept, bool ignition_relay) { - bool drive_relay = intercept; - if (harness.status == HARNESS_STATUS_NC) { - // no harness, no relay to drive - drive_relay = false; - } - - if (drive_relay || ignition_relay) { - harness.relay_driven = true; - } - - // wait until we're not reading the analog voltages anymore - while (harness.sbu_adc_lock) {} - - if (harness.status == HARNESS_STATUS_NORMAL) { - set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, !ignition_relay); - set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, !drive_relay); - } else { - set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, !drive_relay); - set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, !ignition_relay); - } - - if (!(drive_relay || ignition_relay)) { - harness.relay_driven = false; - } -} - -bool harness_check_ignition(void) { - bool ret = false; - - // wait until we're not reading the analog voltages anymore - while (harness.sbu_adc_lock) {} - - switch(harness.status){ - case HARNESS_STATUS_NORMAL: - ret = !get_gpio_input(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1); - break; - case HARNESS_STATUS_FLIPPED: - ret = !get_gpio_input(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2); - break; - default: - break; - } - return ret; -} - -static uint8_t harness_detect_orientation(void) { - uint8_t ret = harness.status; - - #ifndef BOOTSTUB - // We can't detect orientation if the relay is being driven - if (!harness.relay_driven) { - harness.sbu_adc_lock = true; - set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_ANALOG); - set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_ANALOG); - - harness.sbu1_voltage_mV = adc_get_mV(¤t_board->harness_config->adc_signal_SBU1); - harness.sbu2_voltage_mV = adc_get_mV(¤t_board->harness_config->adc_signal_SBU2); - uint16_t detection_threshold = current_board->avdd_mV / 2U; - - // Detect connection and orientation - if((harness.sbu1_voltage_mV < detection_threshold) || (harness.sbu2_voltage_mV < detection_threshold)){ - if (harness.sbu1_voltage_mV < harness.sbu2_voltage_mV) { - // orientation flipped (PANDA_SBU1->HARNESS_SBU1(relay), PANDA_SBU2->HARNESS_SBU2(ign)) - ret = HARNESS_STATUS_FLIPPED; - } else { - // orientation normal (PANDA_SBU2->HARNESS_SBU1(relay), PANDA_SBU1->HARNESS_SBU2(ign)) - // (SBU1->SBU2 is the normal orientation connection per USB-C cable spec) - ret = HARNESS_STATUS_NORMAL; - } - } else { - ret = HARNESS_STATUS_NC; - } - - // Pins are not 5V tolerant in ADC mode - set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_INPUT); - set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_INPUT); - harness.sbu_adc_lock = false; - } - #endif - - return ret; -} - -void harness_tick(void) { - harness.status = harness_detect_orientation(); -} - -void harness_init(void) { - // init OBD_SBUx_RELAY - set_gpio_output_type(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, OUTPUT_TYPE_OPEN_DRAIN); - set_gpio_output_type(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, OUTPUT_TYPE_OPEN_DRAIN); - set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, 1); - set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, 1); - - // detect initial orientation - harness.status = harness_detect_orientation(); - - // keep buses connected by default - set_intercept_relay(false, false); -} +void set_intercept_relay(bool intercept, bool ignition_relay); +bool harness_check_ignition(void); +void harness_tick(void); +void harness_init(void); \ No newline at end of file diff --git a/board/drivers/harness_declarations.h b/board/drivers/harness_declarations.h deleted file mode 100644 index 6f4327aad98..00000000000 --- a/board/drivers/harness_declarations.h +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#define HARNESS_STATUS_NC 0U -#define HARNESS_STATUS_NORMAL 1U -#define HARNESS_STATUS_FLIPPED 2U - -struct harness_t { - uint8_t status; - uint16_t sbu1_voltage_mV; - uint16_t sbu2_voltage_mV; - bool relay_driven; - bool sbu_adc_lock; -}; -extern struct harness_t harness; - -struct harness_configuration { - GPIO_TypeDef * const GPIO_SBU1; - GPIO_TypeDef * const GPIO_SBU2; - GPIO_TypeDef * const GPIO_relay_SBU1; - GPIO_TypeDef * const GPIO_relay_SBU2; - const uint8_t pin_SBU1; - const uint8_t pin_SBU2; - const uint8_t pin_relay_SBU1; - const uint8_t pin_relay_SBU2; - const adc_signal_t adc_signal_SBU1; - const adc_signal_t adc_signal_SBU2; -}; - -// The ignition relay is only used for testing purposes -void set_intercept_relay(bool intercept, bool ignition_relay); -bool harness_check_ignition(void); -void harness_tick(void); -void harness_init(void); diff --git a/board/drivers/interrupts.c b/board/drivers/interrupts.c new file mode 100644 index 00000000000..9903d338ae5 --- /dev/null +++ b/board/drivers/interrupts.c @@ -0,0 +1,83 @@ +#include "interrupts.h" + +#include "board/libc.h" + +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); +} + +interrupt interrupts[NUM_INTERRUPTS]; + +static bool check_interrupt_rate = false; + +static uint32_t idle_time = 0U; +static uint32_t busy_time = 0U; +float interrupt_load = 0.0f; + +void handle_interrupt(IRQn_Type irq_type){ + static uint8_t interrupt_depth = 0U; + static uint32_t last_time = 0U; + ENTER_CRITICAL(); + if (interrupt_depth == 0U) { + uint32_t time = microsecond_timer_get(); + idle_time += get_ts_elapsed(time, last_time); + last_time = time; + } + interrupt_depth += 1U; + EXIT_CRITICAL(); + + interrupts[irq_type].call_counter++; + interrupts[irq_type].handler(); + + // Check that the interrupts don't fire too often + if (check_interrupt_rate && (interrupts[irq_type].call_counter > interrupts[irq_type].max_call_rate)) { + fault_occurred(interrupts[irq_type].call_rate_fault); + } + + ENTER_CRITICAL(); + interrupt_depth -= 1U; + if (interrupt_depth == 0U) { + uint32_t time = microsecond_timer_get(); + busy_time += get_ts_elapsed(time, last_time); + last_time = time; + } + EXIT_CRITICAL(); +} + +// Every second +void interrupt_timer_handler(void) { + if (INTERRUPT_TIMER->SR != 0U) { + for (uint16_t i = 0U; i < NUM_INTERRUPTS; i++) { + // Log IRQ call rate faults + if (check_interrupt_rate && (interrupts[i].call_counter > interrupts[i].max_call_rate)) { + print("Interrupt 0x"); puth(i); print(" fired too often (0x"); puth(interrupts[i].call_counter); print("/s)!\n"); + } + + // Reset interrupt counters + interrupts[i].call_rate = interrupts[i].call_counter; + interrupts[i].call_counter = 0U; + } + + // Calculate interrupt load + // The bootstub does not have the FPU enabled, so can't do float operations. +#if !defined(BOOTSTUB) + interrupt_load = ((busy_time + idle_time) > 0U) ? ((float) (((float) busy_time) / (busy_time + idle_time))) : 0.0f; +#endif + idle_time = 0U; + busy_time = 0U; + } + INTERRUPT_TIMER->SR = 0; +} + +void init_interrupts(bool check_rate_limit){ + check_interrupt_rate = check_rate_limit; + + for(uint16_t i=0U; i +#include -interrupt interrupts[NUM_INTERRUPTS]; +#include -static bool check_interrupt_rate = false; +typedef struct interrupt { + IRQn_Type irq_type; + void (*handler)(void); + uint32_t call_counter; + uint32_t call_rate; + uint32_t max_call_rate; // Call rate is defined as the amount of calls each second + uint32_t call_rate_fault; +} interrupt; -static uint32_t idle_time = 0U; -static uint32_t busy_time = 0U; -float interrupt_load = 0.0f; +void interrupt_timer_init(void); +uint32_t microsecond_timer_get(void); +void unused_interrupt_handler(void); -void handle_interrupt(IRQn_Type irq_type){ - static uint8_t interrupt_depth = 0U; - static uint32_t last_time = 0U; - ENTER_CRITICAL(); - if (interrupt_depth == 0U) { - uint32_t time = microsecond_timer_get(); - idle_time += get_ts_elapsed(time, last_time); - last_time = time; - } - interrupt_depth += 1U; - EXIT_CRITICAL(); +extern interrupt interrupts[NUM_INTERRUPTS]; - interrupts[irq_type].call_counter++; - interrupts[irq_type].handler(); +#define REGISTER_INTERRUPT(irq_num, func_ptr, call_rate_max, rate_fault) \ + interrupts[irq_num].irq_type = (irq_num); \ + interrupts[irq_num].handler = (func_ptr); \ + interrupts[irq_num].call_counter = 0U; \ + interrupts[irq_num].call_rate = 0U; \ + interrupts[irq_num].max_call_rate = (call_rate_max); \ + interrupts[irq_num].call_rate_fault = (rate_fault); - // Check that the interrupts don't fire too often - if (check_interrupt_rate && (interrupts[irq_type].call_counter > interrupts[irq_type].max_call_rate)) { - fault_occurred(interrupts[irq_type].call_rate_fault); - } - - ENTER_CRITICAL(); - interrupt_depth -= 1U; - if (interrupt_depth == 0U) { - uint32_t time = microsecond_timer_get(); - busy_time += get_ts_elapsed(time, last_time); - last_time = time; - } - EXIT_CRITICAL(); -} +extern float interrupt_load; +void handle_interrupt(IRQn_Type irq_type); // Every second -void interrupt_timer_handler(void) { - if (INTERRUPT_TIMER->SR != 0U) { - for (uint16_t i = 0U; i < NUM_INTERRUPTS; i++) { - // Log IRQ call rate faults - if (check_interrupt_rate && (interrupts[i].call_counter > interrupts[i].max_call_rate)) { - print("Interrupt 0x"); puth(i); print(" fired too often (0x"); puth(interrupts[i].call_counter); print("/s)!\n"); - } - - // Reset interrupt counters - interrupts[i].call_rate = interrupts[i].call_counter; - interrupts[i].call_counter = 0U; - } - - // Calculate interrupt load - // The bootstub does not have the FPU enabled, so can't do float operations. -#if !defined(BOOTSTUB) - interrupt_load = ((busy_time + idle_time) > 0U) ? ((float) (((float) busy_time) / (busy_time + idle_time))) : 0.0f; -#endif - idle_time = 0U; - busy_time = 0U; - } - INTERRUPT_TIMER->SR = 0; -} - -void init_interrupts(bool check_rate_limit){ - check_interrupt_rate = check_rate_limit; - - for(uint16_t i=0U; iled_pwm_channels[color] != 0U) { + pwm_set(TIM3, current_board->led_pwm_channels[color], 100U - (enabled ? LED_PWM_POWER : 0U)); + } else { + set_gpio_output(current_board->led_GPIO[color], current_board->led_pin[color], !enabled); + } + } +} + +void led_init(void) { + for (uint8_t i = 0U; i<3U; i++){ + set_gpio_pullup(current_board->led_GPIO[i], current_board->led_pin[i], PULL_NONE); + set_gpio_output_type(current_board->led_GPIO[i], current_board->led_pin[i], OUTPUT_TYPE_OPEN_DRAIN); + + if (current_board->led_pwm_channels[i] != 0U) { + set_gpio_alternate(current_board->led_GPIO[i], current_board->led_pin[i], GPIO_AF2_TIM3); + pwm_init(TIM3, current_board->led_pwm_channels[i]); + } else { + set_gpio_mode(current_board->led_GPIO[i], current_board->led_pin[i], MODE_OUTPUT); + } + + led_set(i, false); + } +} diff --git a/board/drivers/led.h b/board/drivers/led.h index 7cea94e854f..c8cb45bef56 100644 --- a/board/drivers/led.h +++ b/board/drivers/led.h @@ -1,3 +1,7 @@ +#pragma once + +#include +#include #define LED_RED 0U #define LED_GREEN 1U @@ -5,28 +9,5 @@ #define LED_PWM_POWER 2U -void led_set(uint8_t color, bool enabled) { - if (color < 3U) { - if (current_board->led_pwm_channels[color] != 0U) { - pwm_set(TIM3, current_board->led_pwm_channels[color], 100U - (enabled ? LED_PWM_POWER : 0U)); - } else { - set_gpio_output(current_board->led_GPIO[color], current_board->led_pin[color], !enabled); - } - } -} - -void led_init(void) { - for (uint8_t i = 0U; i<3U; i++){ - set_gpio_pullup(current_board->led_GPIO[i], current_board->led_pin[i], PULL_NONE); - set_gpio_output_type(current_board->led_GPIO[i], current_board->led_pin[i], OUTPUT_TYPE_OPEN_DRAIN); - - if (current_board->led_pwm_channels[i] != 0U) { - set_gpio_alternate(current_board->led_GPIO[i], current_board->led_pin[i], GPIO_AF2_TIM3); - pwm_init(TIM3, current_board->led_pwm_channels[i]); - } else { - set_gpio_mode(current_board->led_GPIO[i], current_board->led_pin[i], MODE_OUTPUT); - } - - led_set(i, false); - } -} +void led_set(uint8_t color, bool enabled); +void led_init(void); diff --git a/board/drivers/pwm.c b/board/drivers/pwm.c new file mode 100644 index 00000000000..e1162a3e42d --- /dev/null +++ b/board/drivers/pwm.c @@ -0,0 +1,56 @@ +#include "pwm.h" + +// TODO: Implement for 32-bit timers + +void pwm_init(TIM_TypeDef *TIM, uint8_t channel){ + // Enable timer and auto-reload + register_set(&(TIM->CR1), TIM_CR1_CEN | TIM_CR1_ARPE, 0x3FU); + + // Set channel as PWM mode 1 and enable output + switch(channel){ + case 1U: + register_set_bits(&(TIM->CCMR1), (TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE)); + register_set_bits(&(TIM->CCER), TIM_CCER_CC1E); + break; + case 2U: + register_set_bits(&(TIM->CCMR1), (TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2PE)); + register_set_bits(&(TIM->CCER), TIM_CCER_CC2E); + break; + case 3U: + register_set_bits(&(TIM->CCMR2), (TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE)); + register_set_bits(&(TIM->CCER), TIM_CCER_CC3E); + break; + case 4U: + register_set_bits(&(TIM->CCMR2), (TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4PE)); + register_set_bits(&(TIM->CCER), TIM_CCER_CC4E); + break; + default: + break; + } + + // Set max counter value + register_set(&(TIM->ARR), PWM_COUNTER_OVERFLOW, 0xFFFFU); + + // Update registers and clear counter + TIM->EGR |= TIM_EGR_UG; +} + +void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage){ + uint16_t comp_value = (((uint16_t) percentage * PWM_COUNTER_OVERFLOW) / 100U); + switch(channel){ + case 1U: + register_set(&(TIM->CCR1), comp_value, 0xFFFFU); + break; + case 2U: + register_set(&(TIM->CCR2), comp_value, 0xFFFFU); + break; + case 3U: + register_set(&(TIM->CCR3), comp_value, 0xFFFFU); + break; + case 4U: + register_set(&(TIM->CCR4), comp_value, 0xFFFFU); + break; + default: + break; + } +} diff --git a/board/drivers/pwm.h b/board/drivers/pwm.h index 3d0d73efdc6..ac61d85ed77 100644 --- a/board/drivers/pwm.h +++ b/board/drivers/pwm.h @@ -1,56 +1,10 @@ -#define PWM_COUNTER_OVERFLOW 4800U // To get ~25kHz - -// TODO: Implement for 32-bit timers +#pragma once -void pwm_init(TIM_TypeDef *TIM, uint8_t channel){ - // Enable timer and auto-reload - register_set(&(TIM->CR1), TIM_CR1_CEN | TIM_CR1_ARPE, 0x3FU); +#include "board/config.h" - // Set channel as PWM mode 1 and enable output - switch(channel){ - case 1U: - register_set_bits(&(TIM->CCMR1), (TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE)); - register_set_bits(&(TIM->CCER), TIM_CCER_CC1E); - break; - case 2U: - register_set_bits(&(TIM->CCMR1), (TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2PE)); - register_set_bits(&(TIM->CCER), TIM_CCER_CC2E); - break; - case 3U: - register_set_bits(&(TIM->CCMR2), (TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE)); - register_set_bits(&(TIM->CCER), TIM_CCER_CC3E); - break; - case 4U: - register_set_bits(&(TIM->CCMR2), (TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4PE)); - register_set_bits(&(TIM->CCER), TIM_CCER_CC4E); - break; - default: - break; - } - - // Set max counter value - register_set(&(TIM->ARR), PWM_COUNTER_OVERFLOW, 0xFFFFU); +#define PWM_COUNTER_OVERFLOW 4800U // To get ~25kHz - // Update registers and clear counter - TIM->EGR |= TIM_EGR_UG; -} +// TODO: Implement for 32-bit timers -void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage){ - uint16_t comp_value = (((uint16_t) percentage * PWM_COUNTER_OVERFLOW) / 100U); - switch(channel){ - case 1U: - register_set(&(TIM->CCR1), comp_value, 0xFFFFU); - break; - case 2U: - register_set(&(TIM->CCR2), comp_value, 0xFFFFU); - break; - case 3U: - register_set(&(TIM->CCR3), comp_value, 0xFFFFU); - break; - case 4U: - register_set(&(TIM->CCR4), comp_value, 0xFFFFU); - break; - default: - break; - } -} +void pwm_init(TIM_TypeDef *TIM, uint8_t channel); +void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage); \ No newline at end of file diff --git a/board/drivers/registers.c b/board/drivers/registers.c new file mode 100644 index 00000000000..facf340093c --- /dev/null +++ b/board/drivers/registers.c @@ -0,0 +1,68 @@ +#include "registers.h" +#include "board/libc.h" + +static reg register_map[REGISTER_MAP_SIZE]; + +// Hash spread in first and second iterations seems to be reasonable. +// See: tests/development/register_hashmap_spread.py +// Also, check the collision warnings in the debug output, and minimize those. +static uint16_t hash_addr(uint32_t input){ + return (((input >> 16U) ^ ((((input + 1U) & 0xFFFFU) * HASHING_PRIME) & 0xFFFFU)) & REGISTER_MAP_SIZE); +} + +// Set individual bits. Also add them to the check_mask. +// Do not use this to change bits that get reset by the hardware +void register_set_bits(volatile uint32_t *addr, uint32_t val) { + register_set(addr, val, val); +} + +// Clear individual bits. Also add them to the check_mask. +// Do not use this to clear bits that get set by the hardware +void register_clear_bits(volatile uint32_t *addr, uint32_t val) { + register_set(addr, (~val), val); +} + +// To be called periodically +void check_registers(void){ + for(uint16_t i=0U; i 0U)) { hash = hash_addr((uint32_t) hash); tries--;} + if (tries != 0U){ + register_map[hash].address = addr; + register_map[hash].value = (register_map[hash].value & (~mask)) | (val & mask); + register_map[hash].check_mask |= mask; + } else { + #ifdef DEBUG_FAULTS + print("Hash collision: address 0x"); puth((uint32_t) addr); print("!\n"); + #endif + } + EXIT_CRITICAL() +} diff --git a/board/drivers/registers.h b/board/drivers/registers.h index ce4be14a92a..913690ea09e 100644 --- a/board/drivers/registers.h +++ b/board/drivers/registers.h @@ -1,68 +1,64 @@ -#include "registers_declarations.h" +#pragma once -static reg register_map[REGISTER_MAP_SIZE]; +#include +#include -// Hash spread in first and second iterations seems to be reasonable. -// See: tests/development/register_hashmap_spread.py -// Also, check the collision warnings in the debug output, and minimize those. -static uint16_t hash_addr(uint32_t input){ - return (((input >> 16U) ^ ((((input + 1U) & 0xFFFFU) * HASHING_PRIME) & 0xFFFFU)) & REGISTER_MAP_SIZE); -} +#include "board/critical.h" + +#define FAULT_STATUS_NONE 0U +#define FAULT_STATUS_TEMPORARY 1U +#define FAULT_STATUS_PERMANENT 2U + +// Fault types, matches cereal.log.PandaState.FaultType +#define FAULT_RELAY_MALFUNCTION (1UL << 0) +#define FAULT_UNUSED_INTERRUPT_HANDLED (1UL << 1) +#define FAULT_INTERRUPT_RATE_CAN_1 (1UL << 2) +#define FAULT_INTERRUPT_RATE_CAN_2 (1UL << 3) +#define FAULT_INTERRUPT_RATE_CAN_3 (1UL << 4) +#define FAULT_INTERRUPT_RATE_TACH (1UL << 5) +#define FAULT_INTERRUPT_RATE_INTERRUPTS (1UL << 7) +#define FAULT_INTERRUPT_RATE_SPI_DMA (1UL << 8) +#define FAULT_INTERRUPT_RATE_USB (1UL << 15) +#define FAULT_REGISTER_DIVERGENT (1UL << 18) +#define FAULT_INTERRUPT_RATE_CLOCK_SOURCE (1UL << 20) +#define FAULT_INTERRUPT_RATE_TICK (1UL << 21) +#define FAULT_INTERRUPT_RATE_EXTI (1UL << 22) +#define FAULT_INTERRUPT_RATE_SPI (1UL << 23) +#define FAULT_INTERRUPT_RATE_UART_7 (1UL << 24) +#define FAULT_SIREN_MALFUNCTION (1UL << 25) +#define FAULT_HEARTBEAT_LOOP_WATCHDOG (1UL << 26) +#define FAULT_INTERRUPT_RATE_SOUND_DMA (1UL << 27) + +// Permanent faults +#define PERMANENT_FAULTS 0U + +extern uint8_t fault_status; +extern uint32_t faults; + +void fault_occurred(uint32_t fault); +void fault_recovered(uint32_t fault); -// Do not put bits in the check mask that get changed by the hardware -void register_set(volatile uint32_t *addr, uint32_t val, uint32_t mask){ - ENTER_CRITICAL() - // Set bits in register that are also in the mask - (*addr) = ((*addr) & (~mask)) | (val & mask); - // Add these values to the map - uint16_t hash = hash_addr((uint32_t) addr); - uint16_t tries = REGISTER_MAP_SIZE; - while(CHECK_COLLISION(hash, addr) && (tries > 0U)) { hash = hash_addr((uint32_t) hash); tries--;} - if (tries != 0U){ - register_map[hash].address = addr; - register_map[hash].value = (register_map[hash].value & (~mask)) | (val & mask); - register_map[hash].check_mask |= mask; - } else { - #ifdef DEBUG_FAULTS - print("Hash collision: address 0x"); puth((uint32_t) addr); print("!\n"); - #endif - } - EXIT_CRITICAL() -} +typedef struct reg { + volatile uint32_t *address; + uint32_t value; + uint32_t check_mask; + bool logged_fault; +} reg; +// 10 bit hash with 23 as a prime +#define REGISTER_MAP_SIZE 0x3FFU +#define HASHING_PRIME 23U +#define CHECK_COLLISION(hash, addr) (((uint32_t) register_map[hash].address != 0U) && (register_map[hash].address != (addr))) + +// Do not put bits in the check mask that get changed by the hardware +void register_set(volatile uint32_t *addr, uint32_t val, uint32_t mask); // Set individual bits. Also add them to the check_mask. // Do not use this to change bits that get reset by the hardware -void register_set_bits(volatile uint32_t *addr, uint32_t val) { - register_set(addr, val, val); -} - +void register_set_bits(volatile uint32_t *addr, uint32_t val); // Clear individual bits. Also add them to the check_mask. // Do not use this to clear bits that get set by the hardware -void register_clear_bits(volatile uint32_t *addr, uint32_t val) { - register_set(addr, (~val), val); -} - +void register_clear_bits(volatile uint32_t *addr, uint32_t val); // To be called periodically -void check_registers(void){ - for(uint16_t i=0U; i + +#include "simple_watchdog.h" + +#include "board/drivers/timers.h" +#include "board/utils.h" +#include "board/libc.h" +#include "board/faults.h" + +static simple_watchdog_state_t wd_state; + +void simple_watchdog_kick(void) { + uint32_t ts = microsecond_timer_get(); + + uint32_t et = get_ts_elapsed(ts, wd_state.last_ts); + if (et > wd_state.threshold) { + print("WD timeout 0x"); puth(et); print("\n"); + fault_occurred(wd_state.fault); + } + + wd_state.last_ts = ts; +} + +void simple_watchdog_init(uint32_t fault, uint32_t threshold) { + wd_state.fault = fault; + wd_state.threshold = threshold; + wd_state.last_ts = microsecond_timer_get(); +} diff --git a/board/drivers/simple_watchdog.h b/board/drivers/simple_watchdog.h index f679cb19393..3ae2ab90b4b 100644 --- a/board/drivers/simple_watchdog.h +++ b/board/drivers/simple_watchdog.h @@ -1,21 +1,13 @@ -#include "simple_watchdog_declarations.h" +#pragma once -static simple_watchdog_state_t wd_state; +#include -void simple_watchdog_kick(void) { - uint32_t ts = microsecond_timer_get(); +typedef struct simple_watchdog_state_t { + uint32_t fault; + uint32_t last_ts; + uint32_t threshold; +} simple_watchdog_state_t; - uint32_t et = get_ts_elapsed(ts, wd_state.last_ts); - if (et > wd_state.threshold) { - print("WD timeout 0x"); puth(et); print("\n"); - fault_occurred(wd_state.fault); - } +void simple_watchdog_kick(void); +void simple_watchdog_init(uint32_t fault, uint32_t threshold); - wd_state.last_ts = ts; -} - -void simple_watchdog_init(uint32_t fault, uint32_t threshold) { - wd_state.fault = fault; - wd_state.threshold = threshold; - wd_state.last_ts = microsecond_timer_get(); -} diff --git a/board/drivers/simple_watchdog_declarations.h b/board/drivers/simple_watchdog_declarations.h deleted file mode 100644 index 2bda71590ba..00000000000 --- a/board/drivers/simple_watchdog_declarations.h +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once - -typedef struct simple_watchdog_state_t { - uint32_t fault; - uint32_t last_ts; - uint32_t threshold; -} simple_watchdog_state_t; - -void simple_watchdog_kick(void); -void simple_watchdog_init(uint32_t fault, uint32_t threshold); diff --git a/board/drivers/spi.c b/board/drivers/spi.c new file mode 100644 index 00000000000..3277883928b --- /dev/null +++ b/board/drivers/spi.c @@ -0,0 +1,234 @@ + +#include "spi.h" +#include "board/crc.h" + +#include "board/libc.h" +#include "board/config.h" + +#include "board/globals.h" + +uint8_t spi_buf_rx[SPI_BUF_SIZE]; +uint8_t spi_buf_tx[SPI_BUF_SIZE]; + +uint16_t spi_error_count = 0; + +static uint8_t spi_state = SPI_STATE_HEADER; +static uint16_t spi_data_len_mosi; +static bool spi_can_tx_ready = false; +static const unsigned char version_text[] = "VERSION"; + +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 + // a panda, determine panda type, and bootstub status. + + // the response is: + // VERSION + 2 byte data length + data + CRC8 + + // echo "VERSION" + (void)memcpy(out, version_text, 7); + + // write response + uint16_t data_len = 0; + uint16_t data_pos = 7U + 2U; + + // write serial + (void)memcpy(&out[data_pos], ((uint8_t *)UID_BASE), 12); + data_len += 12U; + + // HW type + out[data_pos + data_len] = hw_type; + data_len += 1U; + + // bootstub + out[data_pos + data_len] = USB_PID & 0xFFU; + data_len += 1U; + + // SPI protocol version + out[data_pos + data_len] = 0x2; + data_len += 1U; + + // data length + out[7] = data_len & 0xFFU; + out[8] = (data_len >> 8) & 0xFFU; + + // CRC8 + uint16_t resp_len = data_pos + data_len; + out[resp_len] = crc_checksum(out, resp_len, 0xD5U); + resp_len += 1U; + + return resp_len; +} + +void spi_init(void) { + // platform init + llspi_init(); + + // Start the first packet! + spi_state = SPI_STATE_HEADER; + llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); +} + +static bool validate_checksum(const uint8_t *data, uint16_t len) { + // TODO: can speed this up by casting the bulk to uint32_t and xor-ing the bytes afterwards + uint8_t checksum = SPI_CHECKSUM_START; + for(uint16_t i = 0U; i < len; i++){ + checksum ^= data[i]; + } + return checksum == 0U; +} + +void spi_rx_done(void) { + uint16_t response_len = 0U; + uint8_t next_rx_state = SPI_STATE_HEADER_NACK; + bool checksum_valid = false; + static uint8_t spi_endpoint; + static uint16_t spi_data_len_miso; + + // parse header + spi_endpoint = spi_buf_rx[1]; + spi_data_len_mosi = (spi_buf_rx[3] << 8) | spi_buf_rx[2]; + spi_data_len_miso = (spi_buf_rx[5] << 8) | spi_buf_rx[4]; + + if (memcmp(spi_buf_rx, version_text, 7) == 0) { + response_len = spi_version_packet(spi_buf_tx); + next_rx_state = SPI_STATE_HEADER_NACK;; + } else if (spi_state == SPI_STATE_HEADER) { + checksum_valid = validate_checksum(spi_buf_rx, SPI_HEADER_SIZE); + if ((spi_buf_rx[0] == SPI_SYNC_BYTE) && checksum_valid) { + // response: ACK and start receiving data portion + spi_buf_tx[0] = SPI_HACK; + next_rx_state = SPI_STATE_HEADER_ACK; + response_len = 1U; + } else { + // response: NACK and reset state machine + #ifdef DEBUG_SPI + print("- incorrect header sync or checksum "); hexdump(spi_buf_rx, SPI_HEADER_SIZE); + #endif + spi_buf_tx[0] = SPI_NACK; + next_rx_state = SPI_STATE_HEADER_NACK; + response_len = 1U; + } + } else if (spi_state == SPI_STATE_DATA_RX) { + // We got everything! Based on the endpoint specified, call the appropriate handler + bool response_ack = false; + checksum_valid = validate_checksum(&(spi_buf_rx[SPI_HEADER_SIZE]), spi_data_len_mosi + 1U); + if (checksum_valid) { + if (spi_endpoint == 0U) { + if (spi_data_len_mosi >= sizeof(ControlPacket_t)) { + ControlPacket_t ctrl = {0}; + (void)memcpy((uint8_t*)&ctrl, &spi_buf_rx[SPI_HEADER_SIZE], sizeof(ControlPacket_t)); + response_len = comms_control_handler(&ctrl, &spi_buf_tx[3]); + response_ack = true; + } else { + print("SPI: insufficient data for control handler\n"); + } + } else if ((spi_endpoint == 1U) || (spi_endpoint == 0x81U)) { + if (spi_data_len_mosi == 0U) { + response_len = comms_can_read(&(spi_buf_tx[3]), spi_data_len_miso); + response_ack = true; + } else { + print("SPI: did not expect data for can_read\n"); + } + } else if (spi_endpoint == 2U) { + comms_endpoint2_write(&spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi); + response_ack = true; + } else if (spi_endpoint == 3U) { + if (spi_data_len_mosi > 0U) { + if (spi_can_tx_ready) { + spi_can_tx_ready = false; + comms_can_write(&spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi); + response_ack = true; + } else { + response_ack = false; + print("SPI: CAN NACK\n"); + } + } else { + print("SPI: did expect data for can_write\n"); + } + } else if (spi_endpoint == 0xABU) { + // test endpoint: mimics panda -> device transfer + response_len = spi_data_len_miso; + response_ack = true; + } else if (spi_endpoint == 0xACU) { + // test endpoint: mimics device -> panda transfer (with NACK) + response_ack = false; + } else { + print("SPI: unexpected endpoint"); puth(spi_endpoint); print("\n"); + } + } else { + // Checksum was incorrect + response_ack = false; + #ifdef DEBUG_SPI + print("- incorrect data checksum "); + puth4(spi_data_len_mosi); + print("\n"); + hexdump(spi_buf_rx, SPI_HEADER_SIZE); + hexdump(&(spi_buf_rx[SPI_HEADER_SIZE]), MIN(spi_data_len_mosi, 64)); + print("\n"); + #endif + } + + if (!response_ack) { + spi_buf_tx[0] = SPI_NACK; + next_rx_state = SPI_STATE_HEADER_NACK; + response_len = 1U; + } else { + // Setup response header + spi_buf_tx[0] = SPI_DACK; + spi_buf_tx[1] = response_len & 0xFFU; + spi_buf_tx[2] = (response_len >> 8) & 0xFFU; + + // Add checksum + uint8_t checksum = SPI_CHECKSUM_START; + for(uint16_t i = 0U; i < (response_len + 3U); i++) { + checksum ^= spi_buf_tx[i]; + } + spi_buf_tx[response_len + 3U] = checksum; + response_len += 4U; + + next_rx_state = SPI_STATE_DATA_TX; + } + } else { + print("SPI: RX unexpected state: "); puth(spi_state); print("\n"); + } + + // send out response + if (response_len == 0U) { + print("SPI: no response\n"); + spi_buf_tx[0] = SPI_NACK; + spi_state = SPI_STATE_HEADER_NACK; + response_len = 1U; + } + llspi_miso_dma(spi_buf_tx, response_len); + + spi_state = next_rx_state; + if (!checksum_valid) { + spi_error_count += 1U; + } +} + +void spi_tx_done(bool reset) { + if ((spi_state == SPI_STATE_HEADER_NACK) || reset) { + // Reset state + spi_state = SPI_STATE_HEADER; + llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); + } else if (spi_state == SPI_STATE_HEADER_ACK) { + // ACK was sent, queue up the RX buf for the data + checksum + spi_state = SPI_STATE_DATA_RX; + llspi_mosi_dma(&spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi + 1U); + } else if (spi_state == SPI_STATE_DATA_TX) { + // Reset state + spi_state = SPI_STATE_HEADER; + llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); + } else { + spi_state = SPI_STATE_HEADER; + llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); + print("SPI: TX unexpected state: "); puth(spi_state); print("\n"); + } +} + +void can_tx_comms_resume_spi(void) { + spi_can_tx_ready = true; +} diff --git a/board/drivers/spi.h b/board/drivers/spi.h index d291fe06a8d..a7182fc283d 100644 --- a/board/drivers/spi.h +++ b/board/drivers/spi.h @@ -1,230 +1,43 @@ #pragma once -#include "board/drivers/spi_declarations.h" -#include "board/crc.h" - -uint8_t spi_buf_rx[SPI_BUF_SIZE]; -uint8_t spi_buf_tx[SPI_BUF_SIZE]; - -uint16_t spi_error_count = 0; - -static uint8_t spi_state = SPI_STATE_HEADER; -static uint16_t spi_data_len_mosi; -static bool spi_can_tx_ready = false; -static const unsigned char version_text[] = "VERSION"; - -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 - // a panda, determine panda type, and bootstub status. - - // the response is: - // VERSION + 2 byte data length + data + CRC8 - - // echo "VERSION" - (void)memcpy(out, version_text, 7); - - // write response - uint16_t data_len = 0; - uint16_t data_pos = 7U + 2U; - - // write serial - (void)memcpy(&out[data_pos], ((uint8_t *)UID_BASE), 12); - data_len += 12U; - - // HW type - out[data_pos + data_len] = hw_type; - data_len += 1U; - - // bootstub - out[data_pos + data_len] = USB_PID & 0xFFU; - data_len += 1U; - - // SPI protocol version - out[data_pos + data_len] = 0x2; - data_len += 1U; - - // data length - out[7] = data_len & 0xFFU; - out[8] = (data_len >> 8) & 0xFFU; - - // CRC8 - uint16_t resp_len = data_pos + data_len; - out[resp_len] = crc_checksum(out, resp_len, 0xD5U); - resp_len += 1U; +#include +#include - return resp_len; -} - -void spi_init(void) { - // platform init - llspi_init(); - - // Start the first packet! - spi_state = SPI_STATE_HEADER; - llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); -} - -static bool validate_checksum(const uint8_t *data, uint16_t len) { - // TODO: can speed this up by casting the bulk to uint32_t and xor-ing the bytes afterwards - uint8_t checksum = SPI_CHECKSUM_START; - for(uint16_t i = 0U; i < len; i++){ - checksum ^= data[i]; - } - return checksum == 0U; -} - -void spi_rx_done(void) { - uint16_t response_len = 0U; - uint8_t next_rx_state = SPI_STATE_HEADER_NACK; - bool checksum_valid = false; - static uint8_t spi_endpoint; - static uint16_t spi_data_len_miso; - - // parse header - spi_endpoint = spi_buf_rx[1]; - spi_data_len_mosi = (spi_buf_rx[3] << 8) | spi_buf_rx[2]; - spi_data_len_miso = (spi_buf_rx[5] << 8) | spi_buf_rx[4]; - - if (memcmp(spi_buf_rx, version_text, 7) == 0) { - response_len = spi_version_packet(spi_buf_tx); - next_rx_state = SPI_STATE_HEADER_NACK;; - } else if (spi_state == SPI_STATE_HEADER) { - checksum_valid = validate_checksum(spi_buf_rx, SPI_HEADER_SIZE); - if ((spi_buf_rx[0] == SPI_SYNC_BYTE) && checksum_valid) { - // response: ACK and start receiving data portion - spi_buf_tx[0] = SPI_HACK; - next_rx_state = SPI_STATE_HEADER_ACK; - response_len = 1U; - } else { - // response: NACK and reset state machine - #ifdef DEBUG_SPI - print("- incorrect header sync or checksum "); hexdump(spi_buf_rx, SPI_HEADER_SIZE); - #endif - spi_buf_tx[0] = SPI_NACK; - next_rx_state = SPI_STATE_HEADER_NACK; - response_len = 1U; - } - } else if (spi_state == SPI_STATE_DATA_RX) { - // We got everything! Based on the endpoint specified, call the appropriate handler - bool response_ack = false; - checksum_valid = validate_checksum(&(spi_buf_rx[SPI_HEADER_SIZE]), spi_data_len_mosi + 1U); - if (checksum_valid) { - if (spi_endpoint == 0U) { - if (spi_data_len_mosi >= sizeof(ControlPacket_t)) { - ControlPacket_t ctrl = {0}; - (void)memcpy((uint8_t*)&ctrl, &spi_buf_rx[SPI_HEADER_SIZE], sizeof(ControlPacket_t)); - response_len = comms_control_handler(&ctrl, &spi_buf_tx[3]); - response_ack = true; - } else { - print("SPI: insufficient data for control handler\n"); - } - } else if ((spi_endpoint == 1U) || (spi_endpoint == 0x81U)) { - if (spi_data_len_mosi == 0U) { - response_len = comms_can_read(&(spi_buf_tx[3]), spi_data_len_miso); - response_ack = true; - } else { - print("SPI: did not expect data for can_read\n"); - } - } else if (spi_endpoint == 2U) { - comms_endpoint2_write(&spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi); - response_ack = true; - } else if (spi_endpoint == 3U) { - if (spi_data_len_mosi > 0U) { - if (spi_can_tx_ready) { - spi_can_tx_ready = false; - comms_can_write(&spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi); - response_ack = true; - } else { - response_ack = false; - print("SPI: CAN NACK\n"); - } - } else { - print("SPI: did expect data for can_write\n"); - } - } else if (spi_endpoint == 0xABU) { - // test endpoint: mimics panda -> device transfer - response_len = spi_data_len_miso; - response_ack = true; - } else if (spi_endpoint == 0xACU) { - // test endpoint: mimics device -> panda transfer (with NACK) - response_ack = false; - } else { - print("SPI: unexpected endpoint"); puth(spi_endpoint); print("\n"); - } - } else { - // Checksum was incorrect - response_ack = false; - #ifdef DEBUG_SPI - print("- incorrect data checksum "); - puth4(spi_data_len_mosi); - print("\n"); - hexdump(spi_buf_rx, SPI_HEADER_SIZE); - hexdump(&(spi_buf_rx[SPI_HEADER_SIZE]), MIN(spi_data_len_mosi, 64)); - print("\n"); - #endif - } - - if (!response_ack) { - spi_buf_tx[0] = SPI_NACK; - next_rx_state = SPI_STATE_HEADER_NACK; - response_len = 1U; - } else { - // Setup response header - spi_buf_tx[0] = SPI_DACK; - spi_buf_tx[1] = response_len & 0xFFU; - spi_buf_tx[2] = (response_len >> 8) & 0xFFU; - - // Add checksum - uint8_t checksum = SPI_CHECKSUM_START; - for(uint16_t i = 0U; i < (response_len + 3U); i++) { - checksum ^= spi_buf_tx[i]; - } - spi_buf_tx[response_len + 3U] = checksum; - response_len += 4U; - - next_rx_state = SPI_STATE_DATA_TX; - } - } else { - print("SPI: RX unexpected state: "); puth(spi_state); print("\n"); - } - - // send out response - if (response_len == 0U) { - print("SPI: no response\n"); - spi_buf_tx[0] = SPI_NACK; - spi_state = SPI_STATE_HEADER_NACK; - response_len = 1U; - } - llspi_miso_dma(spi_buf_tx, response_len); - - spi_state = next_rx_state; - if (!checksum_valid) { - spi_error_count += 1U; - } -} - -void spi_tx_done(bool reset) { - if ((spi_state == SPI_STATE_HEADER_NACK) || reset) { - // Reset state - spi_state = SPI_STATE_HEADER; - llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); - } else if (spi_state == SPI_STATE_HEADER_ACK) { - // ACK was sent, queue up the RX buf for the data + checksum - spi_state = SPI_STATE_DATA_RX; - llspi_mosi_dma(&spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi + 1U); - } else if (spi_state == SPI_STATE_DATA_TX) { - // Reset state - spi_state = SPI_STATE_HEADER; - llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); - } else { - spi_state = SPI_STATE_HEADER; - llspi_mosi_dma(spi_buf_rx, SPI_HEADER_SIZE); - print("SPI: TX unexpected state: "); puth(spi_state); print("\n"); - } -} +#include "board/drivers/spi.h" +#include "board/crc.h" -void can_tx_comms_resume_spi(void) { - spi_can_tx_ready = true; -} +#define SPI_TIMEOUT_US 10000U +// got max rate from hitting a non-existent endpoint +// in a tight loop, plus some buffer +#define SPI_IRQ_RATE 16000U +#define SPI_BUF_SIZE 4096U +#define SPI_CHECKSUM_START 0xABU +#define SPI_SYNC_BYTE 0x5AU +#define SPI_HACK 0x79U +#define SPI_DACK 0x85U +#define SPI_NACK 0x1FU +#define SPI_HEADER_SIZE 7U + +// SPI states +enum { + SPI_STATE_HEADER, + SPI_STATE_HEADER_ACK, + SPI_STATE_HEADER_NACK, + SPI_STATE_DATA_RX, + SPI_STATE_DATA_RX_ACK, + SPI_STATE_DATA_TX +}; + +extern uint16_t spi_error_count; +extern uint8_t spi_buf_rx[SPI_BUF_SIZE]; +extern uint8_t spi_buf_tx[SPI_BUF_SIZE]; + +// low level SPI prototypes +void llspi_init(void); +void llspi_mosi_dma(uint8_t *addr, int len); +void llspi_miso_dma(uint8_t *addr, int len); + +void spi_init(void); +void spi_rx_done(void); +void spi_tx_done(bool reset); +void can_tx_comms_resume_spi(void); \ No newline at end of file diff --git a/board/drivers/spi_declarations.h b/board/drivers/spi_declarations.h deleted file mode 100644 index 23254f0e87a..00000000000 --- a/board/drivers/spi_declarations.h +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -#include "board/crc.h" - -#define SPI_TIMEOUT_US 10000U - -// got max rate from hitting a non-existent endpoint -// in a tight loop, plus some buffer -#define SPI_IRQ_RATE 16000U - -#define SPI_BUF_SIZE 4096U -// H7 DMA2 located in D2 domain, so we need to use SRAM1/SRAM2 -__attribute__((section(".sram12"))) extern uint8_t spi_buf_rx[SPI_BUF_SIZE]; -__attribute__((section(".sram12"))) extern uint8_t spi_buf_tx[SPI_BUF_SIZE]; - -#define SPI_CHECKSUM_START 0xABU -#define SPI_SYNC_BYTE 0x5AU -#define SPI_HACK 0x79U -#define SPI_DACK 0x85U -#define SPI_NACK 0x1FU - -// SPI states -enum { - SPI_STATE_HEADER, - SPI_STATE_HEADER_ACK, - SPI_STATE_HEADER_NACK, - SPI_STATE_DATA_RX, - SPI_STATE_DATA_RX_ACK, - SPI_STATE_DATA_TX -}; - -extern uint16_t spi_error_count; - -#define SPI_HEADER_SIZE 7U - -// low level SPI prototypes -void llspi_init(void); -void llspi_mosi_dma(uint8_t *addr, int len); -void llspi_miso_dma(uint8_t *addr, int len); - -void can_tx_comms_resume_spi(void); -void spi_init(void); -void spi_rx_done(void); -void spi_tx_done(bool reset); diff --git a/board/drivers/timers.c b/board/drivers/timers.c new file mode 100644 index 00000000000..faa94c58f51 --- /dev/null +++ b/board/drivers/timers.c @@ -0,0 +1,34 @@ +#include "timers.h" +#include "board/config.h" + +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); + TIM->SR = 0; +} + +void microsecond_timer_init(void) { + MICROSECOND_TIMER->PSC = (APB1_TIMER_FREQ - 1U); + MICROSECOND_TIMER->CR1 = TIM_CR1_CEN; + MICROSECOND_TIMER->EGR = TIM_EGR_UG; +} + +uint32_t microsecond_timer_get(void) { + return MICROSECOND_TIMER->CNT; +} + +void interrupt_timer_init(void) { + enable_interrupt_timer(); + REGISTER_INTERRUPT(INTERRUPT_TIMER_IRQ, interrupt_timer_handler, 2U, FAULT_INTERRUPT_RATE_INTERRUPTS) + register_set(&(INTERRUPT_TIMER->PSC), ((uint16_t)(15.25*APB1_TIMER_FREQ)-1U), 0xFFFFU); + register_set(&(INTERRUPT_TIMER->DIER), TIM_DIER_UIE, 0x5F5FU); + register_set(&(INTERRUPT_TIMER->CR1), TIM_CR1_CEN, 0x3FU); + INTERRUPT_TIMER->SR = 0; + NVIC_EnableIRQ(INTERRUPT_TIMER_IRQ); +} + +void tick_timer_init(void) { + timer_init(TICK_TIMER, (uint16_t)((15.25*APB2_TIMER_FREQ)/8U)); + NVIC_EnableIRQ(TICK_TIMER_IRQ); +} diff --git a/board/drivers/timers.h b/board/drivers/timers.h index 4c046a2b49b..77d07ec10a6 100644 --- a/board/drivers/timers.h +++ b/board/drivers/timers.h @@ -1,31 +1,8 @@ -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); - TIM->SR = 0; -} +#pragma once -void microsecond_timer_init(void) { - MICROSECOND_TIMER->PSC = (APB1_TIMER_FREQ - 1U); - MICROSECOND_TIMER->CR1 = TIM_CR1_CEN; - MICROSECOND_TIMER->EGR = TIM_EGR_UG; -} +#include -uint32_t microsecond_timer_get(void) { - return MICROSECOND_TIMER->CNT; -} - -void interrupt_timer_init(void) { - enable_interrupt_timer(); - REGISTER_INTERRUPT(INTERRUPT_TIMER_IRQ, interrupt_timer_handler, 2U, FAULT_INTERRUPT_RATE_INTERRUPTS) - register_set(&(INTERRUPT_TIMER->PSC), ((uint16_t)(15.25*APB1_TIMER_FREQ)-1U), 0xFFFFU); - register_set(&(INTERRUPT_TIMER->DIER), TIM_DIER_UIE, 0x5F5FU); - register_set(&(INTERRUPT_TIMER->CR1), TIM_CR1_CEN, 0x3FU); - INTERRUPT_TIMER->SR = 0; - NVIC_EnableIRQ(INTERRUPT_TIMER_IRQ); -} - -void tick_timer_init(void) { - timer_init(TICK_TIMER, (uint16_t)((15.25*APB2_TIMER_FREQ)/8U)); - NVIC_EnableIRQ(TICK_TIMER_IRQ); -} +void microsecond_timer_init(void); +uint32_t microsecond_timer_get(void); +void interrupt_timer_init(void); +void tick_timer_init(void); \ No newline at end of file diff --git a/board/drivers/uart.c b/board/drivers/uart.c new file mode 100644 index 00000000000..b8e309ed954 --- /dev/null +++ b/board/drivers/uart.c @@ -0,0 +1,149 @@ +#include "uart.h" + +// ***************************** Definitions ***************************** + +#define UART_BUFFER(x, size_rx, size_tx, uart_ptr, callback_ptr, overwrite_mode) \ + static uint8_t elems_rx_##x[size_rx]; \ + static uint8_t elems_tx_##x[size_tx]; \ + extern uart_ring uart_ring_##x; \ + uart_ring uart_ring_##x = { \ + .w_ptr_tx = 0, \ + .r_ptr_tx = 0, \ + .elems_tx = ((uint8_t *)&(elems_tx_##x)), \ + .tx_fifo_size = (size_tx), \ + .w_ptr_rx = 0, \ + .r_ptr_rx = 0, \ + .elems_rx = ((uint8_t *)&(elems_rx_##x)), \ + .rx_fifo_size = (size_rx), \ + .uart = (uart_ptr), \ + .callback = (callback_ptr), \ + .overwrite = (overwrite_mode) \ + }; + +// ******************************** UART buffers ******************************** + +// debug = USART2 +UART_BUFFER(debug, FIFO_SIZE_INT, FIFO_SIZE_INT, USART2, debug_ring_callback, true) + +// SOM debug = UART7 +UART_BUFFER(som_debug, FIFO_SIZE_INT, FIFO_SIZE_INT, UART7, NULL, true) + +uart_ring *get_ring_by_number(int a) { + uart_ring *ring = NULL; + switch(a) { + case 0: + ring = &uart_ring_debug; + break; + case 4: + ring = &uart_ring_som_debug; + break; + default: + ring = NULL; + break; + } + return ring; +} + +// ************************* Low-level buffer functions ************************* +bool get_char(uart_ring *q, char *elem) { + bool ret = false; + + ENTER_CRITICAL(); + if (q->w_ptr_rx != q->r_ptr_rx) { + if (elem != NULL) *elem = q->elems_rx[q->r_ptr_rx]; + q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size; + ret = true; + } + EXIT_CRITICAL(); + + return ret; +} + +bool injectc(uart_ring *q, char elem) { + int ret = false; + uint16_t next_w_ptr; + + ENTER_CRITICAL(); + next_w_ptr = (q->w_ptr_rx + 1U) % q->rx_fifo_size; + + if ((next_w_ptr == q->r_ptr_rx) && q->overwrite) { + // overwrite mode: drop oldest byte + q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size; + } + + if (next_w_ptr != q->r_ptr_rx) { + q->elems_rx[q->w_ptr_rx] = elem; + q->w_ptr_rx = next_w_ptr; + ret = true; + } + EXIT_CRITICAL(); + + return ret; +} + +bool put_char(uart_ring *q, char elem) { + bool ret = false; + uint16_t next_w_ptr; + + ENTER_CRITICAL(); + next_w_ptr = (q->w_ptr_tx + 1U) % q->tx_fifo_size; + + if ((next_w_ptr == q->r_ptr_tx) && q->overwrite) { + // overwrite mode: drop oldest byte + q->r_ptr_tx = (q->r_ptr_tx + 1U) % q->tx_fifo_size; + } + + if (next_w_ptr != q->r_ptr_tx) { + q->elems_tx[q->w_ptr_tx] = elem; + q->w_ptr_tx = next_w_ptr; + ret = true; + } + EXIT_CRITICAL(); + + uart_tx_ring(q); + + return ret; +} + +// ************************ High-level debug functions ********************** +void putch(const char a) { + // misra-c2012-17.7: serial debug function, ok to ignore output + (void)injectc(&uart_ring_debug, a); +} + +void print(const char *a) { + for (const char *in = a; *in; in++) { + if (*in == '\n') putch('\r'); + putch(*in); + } +} + +void puthx(uint32_t i, uint8_t len) { + const char c[] = "0123456789abcdef"; + for (int pos = ((int)len * 4) - 4; pos > -4; pos -= 4) { + putch(c[(i >> (unsigned int)(pos)) & 0xFU]); + } +} + +void puth(unsigned int i) { + puthx(i, 8U); +} + +// #if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG) +// void puth4(unsigned int i) { +// puthx(i, 4U); +// } +// #endif + +#if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG_USB) || defined(DEBUG_COMMS) +void hexdump(const void *a, int l) { + if (a != NULL) { + for (int i=0; i < l; i++) { + if ((i != 0) && ((i & 0xf) == 0)) print("\n"); + puthx(((const unsigned char*)a)[i], 2U); + print(" "); + } + } + print("\n"); +} +#endif diff --git a/board/drivers/uart.h b/board/drivers/uart.h index aeacf84b851..7abde2a8dc3 100644 --- a/board/drivers/uart.h +++ b/board/drivers/uart.h @@ -1,4 +1,48 @@ -#include "uart_declarations.h" +#pragma once + +#include +#include + +#include "board/config.h" + +// ***************************** Definitions ***************************** +#define FIFO_SIZE_INT 0x400U + +typedef struct uart_ring { + volatile uint16_t w_ptr_tx; + volatile uint16_t r_ptr_tx; + uint8_t *elems_tx; + uint32_t tx_fifo_size; + volatile uint16_t w_ptr_rx; + volatile uint16_t r_ptr_rx; + uint8_t *elems_rx; + uint32_t rx_fifo_size; + USART_TypeDef *uart; + void (*callback)(struct uart_ring*); + bool overwrite; +} uart_ring; + +// ***************************** Function prototypes ***************************** +void debug_ring_callback(uart_ring *ring); +void uart_tx_ring(uart_ring *q); +uart_ring *get_ring_by_number(int a); +// ************************* Low-level buffer functions ************************* +bool get_char(uart_ring *q, char *elem); +bool injectc(uart_ring *q, char elem); +bool put_char(uart_ring *q, char elem); +void clear_uart_buff(uart_ring *q); +// ************************ High-level debug functions ********************** +void putch(const char a); +void print(const char *a); +void puthx(uint32_t i, uint8_t len); +void puth(unsigned int i); +#if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG) +void puth4(unsigned int i); +#endif +#if defined(DEBUG_SPI) || defined(DEBUG_USB) || defined(DEBUG_COMMS) +void hexdump(const void *a, int l); +#endif + // ***************************** Definitions ***************************** @@ -22,128 +66,17 @@ // ******************************** UART buffers ******************************** -// debug = USART2 -UART_BUFFER(debug, FIFO_SIZE_INT, FIFO_SIZE_INT, USART2, debug_ring_callback, true) +extern uart_ring uart_ring_som_debug; -// SOM debug = UART7 -UART_BUFFER(som_debug, FIFO_SIZE_INT, FIFO_SIZE_INT, UART7, NULL, true) - -uart_ring *get_ring_by_number(int a) { - uart_ring *ring = NULL; - switch(a) { - case 0: - ring = &uart_ring_debug; - break; - case 4: - ring = &uart_ring_som_debug; - break; - default: - ring = NULL; - break; - } - return ring; -} +uart_ring *get_ring_by_number(int a); // ************************* Low-level buffer functions ************************* -bool get_char(uart_ring *q, char *elem) { - bool ret = false; - - ENTER_CRITICAL(); - if (q->w_ptr_rx != q->r_ptr_rx) { - if (elem != NULL) *elem = q->elems_rx[q->r_ptr_rx]; - q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size; - ret = true; - } - EXIT_CRITICAL(); - - return ret; -} - -bool injectc(uart_ring *q, char elem) { - int ret = false; - uint16_t next_w_ptr; - - ENTER_CRITICAL(); - next_w_ptr = (q->w_ptr_rx + 1U) % q->rx_fifo_size; - - if ((next_w_ptr == q->r_ptr_rx) && q->overwrite) { - // overwrite mode: drop oldest byte - q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size; - } - - if (next_w_ptr != q->r_ptr_rx) { - q->elems_rx[q->w_ptr_rx] = elem; - q->w_ptr_rx = next_w_ptr; - ret = true; - } - EXIT_CRITICAL(); - - return ret; -} - -bool put_char(uart_ring *q, char elem) { - bool ret = false; - uint16_t next_w_ptr; - - ENTER_CRITICAL(); - next_w_ptr = (q->w_ptr_tx + 1U) % q->tx_fifo_size; - - if ((next_w_ptr == q->r_ptr_tx) && q->overwrite) { - // overwrite mode: drop oldest byte - q->r_ptr_tx = (q->r_ptr_tx + 1U) % q->tx_fifo_size; - } - - if (next_w_ptr != q->r_ptr_tx) { - q->elems_tx[q->w_ptr_tx] = elem; - q->w_ptr_tx = next_w_ptr; - ret = true; - } - EXIT_CRITICAL(); - - uart_tx_ring(q); - - return ret; -} +bool get_char(uart_ring *q, char *elem); +bool injectc(uart_ring *q, char elem); +bool put_char(uart_ring *q, char elem); // ************************ High-level debug functions ********************** -void putch(const char a) { - // misra-c2012-17.7: serial debug function, ok to ignore output - (void)injectc(&uart_ring_debug, a); -} - -void print(const char *a) { - for (const char *in = a; *in; in++) { - if (*in == '\n') putch('\r'); - putch(*in); - } -} - -void puthx(uint32_t i, uint8_t len) { - const char c[] = "0123456789abcdef"; - for (int pos = ((int)len * 4) - 4; pos > -4; pos -= 4) { - putch(c[(i >> (unsigned int)(pos)) & 0xFU]); - } -} - -void puth(unsigned int i) { - puthx(i, 8U); -} - -#if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG) -static void puth4(unsigned int i) { - puthx(i, 4U); -} -#endif - -#if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG_USB) || defined(DEBUG_COMMS) -static void hexdump(const void *a, int l) { - if (a != NULL) { - for (int i=0; i < l; i++) { - if ((i != 0) && ((i & 0xf) == 0)) print("\n"); - puthx(((const unsigned char*)a)[i], 2U); - print(" "); - } - } - print("\n"); -} -#endif +void putch(const char a); +void print(const char *a); +void puthx(uint32_t i, uint8_t len); +void puth(unsigned int i); \ No newline at end of file diff --git a/board/drivers/uart_declarations.h b/board/drivers/uart_declarations.h deleted file mode 100644 index 14ec26110f9..00000000000 --- a/board/drivers/uart_declarations.h +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -// ***************************** Definitions ***************************** -#define FIFO_SIZE_INT 0x400U - -typedef struct uart_ring { - volatile uint16_t w_ptr_tx; - volatile uint16_t r_ptr_tx; - uint8_t *elems_tx; - uint32_t tx_fifo_size; - volatile uint16_t w_ptr_rx; - volatile uint16_t r_ptr_rx; - uint8_t *elems_rx; - uint32_t rx_fifo_size; - USART_TypeDef *uart; - void (*callback)(struct uart_ring*); - bool overwrite; -} uart_ring; - -// ***************************** Function prototypes ***************************** -void debug_ring_callback(uart_ring *ring); -void uart_tx_ring(uart_ring *q); -uart_ring *get_ring_by_number(int a); -// ************************* Low-level buffer functions ************************* -bool get_char(uart_ring *q, char *elem); -bool injectc(uart_ring *q, char elem); -bool put_char(uart_ring *q, char elem); -void clear_uart_buff(uart_ring *q); -// ************************ High-level debug functions ********************** -void putch(const char a); -void print(const char *a); -void puthx(uint32_t i, uint8_t len); -void puth(unsigned int i); -#if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG) -static void puth4(unsigned int i); -#endif -#if defined(DEBUG_SPI) || defined(DEBUG_USB) || defined(DEBUG_COMMS) -static void hexdump(const void *a, int l); -#endif diff --git a/board/drivers/usb.c b/board/drivers/usb.c new file mode 100644 index 00000000000..71b5ea60644 --- /dev/null +++ b/board/drivers/usb.c @@ -0,0 +1,799 @@ + +#include + +#include "usb.h" +#include "board/config.h" +#include "board/globals.h" + +static uint8_t response[USBPACKET_MAX_SIZE]; + +// current packet +static USB_Setup_TypeDef setup; +static uint8_t* ep0_txdata = NULL; +static uint16_t ep0_txlen = 0; +static bool outep3_processing = false; + +// Store the current interface alt setting. +static int current_int0_alt_setting = 0; + +// packet read and write + +static void *USB_ReadPacket(void *dest, uint16_t len) { + uint32_t *dest_copy = (uint32_t *)dest; + uint32_t count32b = ((uint32_t)len + 3U) / 4U; + + for (uint32_t i = 0; i < count32b; i++) { + *dest_copy = USBx_DFIFO(0U); + dest_copy++; + } + return ((void *)dest_copy); +} + +static void USB_WritePacket(const void *src, uint16_t len, uint32_t ep) { + #ifdef DEBUG_USB + print("writing "); + hexdump(src, len); + #endif + + uint32_t numpacket = ((uint32_t)len + (USBPACKET_MAX_SIZE - 1U)) / USBPACKET_MAX_SIZE; + uint32_t count32b = 0; + count32b = ((uint32_t)len + 3U) / 4U; + + // TODO: revisit this + USBx_INEP(ep)->DIEPTSIZ = ((numpacket << 19) & USB_OTG_DIEPTSIZ_PKTCNT) | + (len & USB_OTG_DIEPTSIZ_XFRSIZ); + USBx_INEP(ep)->DIEPCTL |= (USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA); + + // load the FIFO + if (src != NULL) { + const uint32_t *src_copy = (const uint32_t *)src; + for (uint32_t i = 0; i < count32b; i++) { + USBx_DFIFO(ep) = *src_copy; + src_copy++; + } + } +} + +// IN EP 0 TX FIFO has a max size of 127 bytes (much smaller than the rest) +// so use TX FIFO empty interrupt to send larger amounts of data +static void USB_WritePacket_EP0(uint8_t *src, uint16_t len) { + #ifdef DEBUG_USB + print("writing "); + hexdump(src, len); + #endif + + uint16_t wplen = MIN(len, 0x40); + USB_WritePacket(src, wplen, 0); + + if (wplen < len) { + ep0_txdata = &src[wplen]; + ep0_txlen = len - wplen; + USBx_DEVICE->DIEPEMPMSK |= 1; + } else { + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + } +} + +static void usb_reset(void) { + // unmask endpoint interrupts, so many sets + USBx_DEVICE->DAINT = 0xFFFFFFFFU; + USBx_DEVICE->DAINTMSK = 0xFFFFFFFFU; + //USBx_DEVICE->DOEPMSK = (USB_OTG_DOEPMSK_STUPM | USB_OTG_DOEPMSK_XFRCM | USB_OTG_DOEPMSK_EPDM); + //USBx_DEVICE->DIEPMSK = (USB_OTG_DIEPMSK_TOM | USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_EPDM | USB_OTG_DIEPMSK_ITTXFEMSK); + //USBx_DEVICE->DIEPMSK = (USB_OTG_DIEPMSK_TOM | USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_EPDM); + + // all interrupts for debugging + USBx_DEVICE->DIEPMSK = 0xFFFFFFFFU; + USBx_DEVICE->DOEPMSK = 0xFFFFFFFFU; + + // clear interrupts + USBx_INEP(0U)->DIEPINT = 0xFF; + USBx_OUTEP(0U)->DOEPINT = 0xFF; + + // unset the address + USBx_DEVICE->DCFG &= ~USB_OTG_DCFG_DAD; + + // set up USB FIFOs + // RX start address is fixed to 0 + USBx->GRXFSIZ = 0x40; + + // 0x100 to offset past GRXFSIZ + USBx->DIEPTXF0_HNPTXFSIZ = (0x40UL << 16) | 0x40U; + + // EP1, massive + USBx->DIEPTXF[0] = (0x40UL << 16) | 0x80U; + + // flush TX fifo + USBx->GRSTCTL = USB_OTG_GRSTCTL_TXFFLSH | USB_OTG_GRSTCTL_TXFNUM_4; + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH); + // flush RX FIFO + USBx->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH; + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH); + + // no global NAK + USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGINAK; + + // ready to receive setup packets + USBx_OUTEP(0U)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1UL << 19)) | (3U << 3); +} + +static char to_hex_char(uint8_t a) { + char ret; + if (a < 10U) { + ret = '0' + a; + } else { + ret = 'a' + (a - 10U); + } + return ret; +} + +static void usb_setup(void) { + static uint8_t device_desc[] = { + DSCR_DEVICE_LEN, USB_DESC_TYPE_DEVICE, //Length, Type + 0x10, 0x02, // bcdUSB max version of USB supported (2.1) + 0xFF, 0xFF, 0xFF, 0x40, // Class, Subclass, Protocol, Max Packet Size + TOUSBORDER(USB_VID), // idVendor + TOUSBORDER(USB_PID), // idProduct + 0x00, 0x00, // bcdDevice + 0x01, 0x02, // Manufacturer, Product + 0x03, 0x01 // Serial Number, Num Configurations + }; + + static uint8_t device_qualifier[] = { + 0x0a, USB_DESC_TYPE_DEVICE_QUALIFIER, //Length, Type + 0x10, 0x02, // bcdUSB max version of USB supported (2.1) + 0xFF, 0xFF, 0xFF, 0x40, // bDeviceClass, bDeviceSubClass, bDeviceProtocol, bMaxPacketSize0 + 0x01, 0x00 // bNumConfigurations, bReserved + }; + + static uint8_t configuration_desc[] = { + DSCR_CONFIG_LEN, USB_DESC_TYPE_CONFIGURATION, // Length, Type, + TOUSBORDER(0x0045U), // Total Len (uint16) + 0x01, 0x01, STRING_OFFSET_ICONFIGURATION, // Num Interface, Config Value, Configuration + 0xc0, 0x32, // Attributes, Max Power + // interface 0 ALT 0 + DSCR_INTERFACE_LEN, USB_DESC_TYPE_INTERFACE, // Length, Type + 0x00, 0x00, 0x03, // Index, Alt Index idx, Endpoint count + 0XFF, 0xFF, 0xFF, // Class, Subclass, Protocol + 0x00, // Interface + // endpoint 1, read CAN + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type + ENDPOINT_RCV | 1, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type + TOUSBORDER(0x0040U), // Max Packet (0x0040) + 0x00, // Polling Interval (NA) + // endpoint 2, send serial + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type + ENDPOINT_SND | 2, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type + TOUSBORDER(0x0040U), // Max Packet (0x0040) + 0x00, // Polling Interval + // endpoint 3, send CAN + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type + ENDPOINT_SND | 3, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type + TOUSBORDER(0x0040U), // Max Packet (0x0040) + 0x00, // Polling Interval + // interface 0 ALT 1 + DSCR_INTERFACE_LEN, USB_DESC_TYPE_INTERFACE, // Length, Type + 0x00, 0x01, 0x03, // Index, Alt Index idx, Endpoint count + 0XFF, 0xFF, 0xFF, // Class, Subclass, Protocol + 0x00, // Interface + // endpoint 1, read CAN + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type + ENDPOINT_RCV | 1, ENDPOINT_TYPE_INT, // Endpoint Num/Direction, Type + TOUSBORDER(0x0040U), // Max Packet (0x0040) + 0x05, // Polling Interval (5 frames) + // endpoint 2, send serial + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type + ENDPOINT_SND | 2, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type + TOUSBORDER(0x0040U), // Max Packet (0x0040) + 0x00, // Polling Interval + // endpoint 3, send CAN + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type + ENDPOINT_SND | 3, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type + TOUSBORDER(0x0040U), // Max Packet (0x0040) + 0x00, // Polling Interval + }; + + // STRING_DESCRIPTOR_HEADER is for uint16 string descriptors + // it takes in a string length, which is bytes/2 because unicode + static uint16_t string_language_desc[] = { + STRING_DESCRIPTOR_HEADER(1), + 0x0409 // american english + }; + + // these strings are all uint16's so that we don't need to spam ,0 after every character + static uint16_t string_manufacturer_desc[] = { + STRING_DESCRIPTOR_HEADER(8), + 'c', 'o', 'm', 'm', 'a', '.', 'a', 'i' + }; + + static uint16_t string_product_desc[] = { + STRING_DESCRIPTOR_HEADER(5), + 'p', 'a', 'n', 'd', 'a' + }; + + // a string containing the default configuration index + static uint16_t string_configuration_desc[] = { + STRING_DESCRIPTOR_HEADER(2), + '0', '1' // "01" + }; + + // WCID (auto install WinUSB driver) + // https://github.com/pbatard/libwdi/wiki/WCID-Devices + // https://docs.microsoft.com/en-us/windows-hardware/drivers/usbcon/winusb-installation#automatic-installation-of--winusb-without-an-inf-file + // WinUSB 1.0 descriptors, this is mostly used by Windows XP + static uint8_t string_238_desc[] = { + 0x12, USB_DESC_TYPE_STRING, // bLength, bDescriptorType + 'M',0, 'S',0, 'F',0, 'T',0, '1',0, '0',0, '0',0, // qwSignature (MSFT100) + MS_VENDOR_CODE, 0x00 // bMS_VendorCode, bPad + }; + + static uint8_t winusb_ext_compatid_os_desc[] = { + 0x28, 0x00, 0x00, 0x00, // dwLength + 0x00, 0x01, // bcdVersion + 0x04, 0x00, // wIndex + 0x01, // bCount + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Reserved + 0x00, // bFirstInterfaceNumber + 0x00, // Reserved + 'W', 'I', 'N', 'U', 'S', 'B', 0x00, 0x00, // compatible ID (WINUSB) + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // subcompatible ID (none) + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 // Reserved + }; + + static uint8_t winusb_ext_prop_os_desc[] = { + 0x8e, 0x00, 0x00, 0x00, // dwLength + 0x00, 0x01, // bcdVersion + 0x05, 0x00, // wIndex + 0x01, 0x00, // wCount + // first property + 0x84, 0x00, 0x00, 0x00, // dwSize + 0x01, 0x00, 0x00, 0x00, // dwPropertyDataType + 0x28, 0x00, // wPropertyNameLength + 'D',0, 'e',0, 'v',0, 'i',0, 'c',0, 'e',0, 'I',0, 'n',0, 't',0, 'e',0, 'r',0, 'f',0, 'a',0, 'c',0, 'e',0, 'G',0, 'U',0, 'I',0, 'D',0, 0, 0, // bPropertyName (DeviceInterfaceGUID) + 0x4e, 0x00, 0x00, 0x00, // dwPropertyDataLength + '{',0, 'c',0, 'c',0, 'e',0, '5',0, '2',0, '9',0, '1',0, 'c',0, '-',0, 'a',0, '6',0, '9',0, 'f',0, '-',0, '4',0 ,'9',0 ,'9',0 ,'5',0 ,'-',0, 'a',0, '4',0, 'c',0, '2',0, '-',0, '2',0, 'a',0, 'e',0, '5',0, '7',0, 'a',0, '5',0, '1',0, 'a',0, 'd',0, 'e',0, '9',0, '}',0, 0, 0, // bPropertyData ({CCE5291C-A69F-4995-A4C2-2AE57A51ADE9}) + }; + + /* + Binary Object Store descriptor used to expose WebUSB (and more WinUSB) metadata + comments are from the wicg spec + References used: + https://wicg.github.io/webusb/#webusb-platform-capability-descriptor + https://github.com/sowbug/weblight/blob/192ad7a0e903542e2aa28c607d98254a12a6399d/firmware/webusb.c + https://os.mbed.com/users/larsgk/code/USBDevice_WebUSB/file/1d8a6665d607/WebUSBDevice/ + */ + static uint8_t binary_object_store_desc[] = { + // BOS header + BINARY_OBJECT_STORE_DESCRIPTOR_LENGTH, // bLength, this is only the length of the header + BINARY_OBJECT_STORE_DESCRIPTOR, // bDescriptorType + 0x39, 0x00, // wTotalLength (LSB, MSB) + 0x02, // bNumDeviceCaps (WebUSB + WinUSB) + + // ------------------------------------------------- + // WebUSB descriptor + // header + 0x18, // bLength, Size of this descriptor. Must be set to 24. + 0x10, // bDescriptorType, DEVICE CAPABILITY descriptor + 0x05, // bDevCapabilityType, PLATFORM capability + 0x00, // bReserved, This field is reserved and shall be set to zero. + + // PlatformCapabilityUUID, Must be set to {3408b638-09a9-47a0-8bfd-a0768815b665}. + 0x38, 0xB6, 0x08, 0x34, + 0xA9, 0x09, 0xA0, 0x47, + 0x8B, 0xFD, 0xA0, 0x76, + 0x88, 0x15, 0xB6, 0x65, + // + + 0x00, 0x01, // bcdVersion, Protocol version supported. Must be set to 0x0100. + WEBUSB_VENDOR_CODE, // bVendorCode, bRequest value used for issuing WebUSB requests. + // there used to be a concept of "allowed origins", but it was removed from the spec + // it was intended to be a security feature, but then the entire security model relies on domain ownership + // https://github.com/WICG/webusb/issues/49 + // other implementations use various other indexed to leverate this no-longer-valid feature. we wont. + // the spec says we *must* reply to index 0x03 with the url, so we'll hint that that's the right index + 0x03, // iLandingPage, URL descriptor index of the device’s landing page. + + // ------------------------------------------------- + // WinUSB descriptor + // header + 0x1C, // Descriptor size (28 bytes) + 0x10, // Descriptor type (Device Capability) + 0x05, // Capability type (Platform) + 0x00, // Reserved + + // MS OS 2.0 Platform Capability ID (D8DD60DF-4589-4CC7-9CD2-659D9E648A9F) + // Indicates the device supports the Microsoft OS 2.0 descriptor + 0xDF, 0x60, 0xDD, 0xD8, + 0x89, 0x45, 0xC7, 0x4C, + 0x9C, 0xD2, 0x65, 0x9D, + 0x9E, 0x64, 0x8A, 0x9F, + + 0x00, 0x00, 0x03, 0x06, // Windows version, currently set to 8.1 (0x06030000) + + WINUSB_PLATFORM_DESCRIPTOR_LENGTH, 0x00, // MS OS 2.0 descriptor size (word) + MS_VENDOR_CODE, 0x00 // vendor code, no alternate enumeration + }; + + // WinUSB 2.0 descriptor. This is what modern systems use + // https://github.com/sowbug/weblight/blob/192ad7a0e903542e2aa28c607d98254a12a6399d/firmware/webusb.c + // http://janaxelson.com/files/ms_os_20_descriptors.c + // https://books.google.com/books?id=pkefBgAAQBAJ&pg=PA353&lpg=PA353 + static uint8_t winusb_20_desc[WINUSB_PLATFORM_DESCRIPTOR_LENGTH] = { + // Microsoft OS 2.0 descriptor set header (table 10) + 0x0A, 0x00, // Descriptor size (10 bytes) + 0x00, 0x00, // MS OS 2.0 descriptor set header + + 0x00, 0x00, 0x03, 0x06, // Windows version (8.1) (0x06030000) + WINUSB_PLATFORM_DESCRIPTOR_LENGTH, 0x00, // Total size of MS OS 2.0 descriptor set + + // Microsoft OS 2.0 compatible ID descriptor + 0x14, 0x00, // Descriptor size (20 bytes) + 0x03, 0x00, // MS OS 2.0 compatible ID descriptor + 'W', 'I', 'N', 'U', 'S', 'B', 0x00, 0x00, // compatible ID (WINUSB) + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Sub-compatible ID + + // Registry property descriptor + 0x80, 0x00, // Descriptor size (130 bytes) + 0x04, 0x00, // Registry Property descriptor + 0x01, 0x00, // Strings are null-terminated Unicode + 0x28, 0x00, // Size of Property Name (40 bytes) "DeviceInterfaceGUID" + + // bPropertyName (DeviceInterfaceGUID) + 'D', 0x00, 'e', 0x00, 'v', 0x00, 'i', 0x00, 'c', 0x00, 'e', 0x00, 'I', 0x00, 'n', 0x00, + 't', 0x00, 'e', 0x00, 'r', 0x00, 'f', 0x00, 'a', 0x00, 'c', 0x00, 'e', 0x00, 'G', 0x00, + 'U', 0x00, 'I', 0x00, 'D', 0x00, 0x00, 0x00, + + 0x4E, 0x00, // Size of Property Data (78 bytes) + + // Vendor-defined property data: {CCE5291C-A69F-4995-A4C2-2AE57A51ADE9} + '{', 0x00, 'c', 0x00, 'c', 0x00, 'e', 0x00, '5', 0x00, '2', 0x00, '9', 0x00, '1', 0x00, // 16 + 'c', 0x00, '-', 0x00, 'a', 0x00, '6', 0x00, '9', 0x00, 'f', 0x00, '-', 0x00, '4', 0x00, // 32 + '9', 0x00, '9', 0x00, '5', 0x00, '-', 0x00, 'a', 0x00, '4', 0x00, 'c', 0x00, '2', 0x00, // 48 + '-', 0x00, '2', 0x00, 'a', 0x00, 'e', 0x00, '5', 0x00, '7', 0x00, 'a', 0x00, '5', 0x00, // 64 + '1', 0x00, 'a', 0x00, 'd', 0x00, 'e', 0x00, '9', 0x00, '}', 0x00, 0x00, 0x00 // 78 bytes + }; + + int resp_len; + ControlPacket_t control_req; + + // setup packet is ready + switch (setup.b.bRequest) { + case USB_REQ_SET_CONFIGURATION: + // enable other endpoints, has to be here? + USBx_INEP(1U)->DIEPCTL = (0x40U & USB_OTG_DIEPCTL_MPSIZ) | (2UL << 18) | (1UL << 22) | + USB_OTG_DIEPCTL_SD0PID_SEVNFRM | USB_OTG_DIEPCTL_USBAEP; + USBx_INEP(1U)->DIEPINT = 0xFF; + + USBx_OUTEP(2U)->DOEPTSIZ = (1UL << 19) | 0x40U; + USBx_OUTEP(2U)->DOEPCTL = (0x40U & USB_OTG_DOEPCTL_MPSIZ) | (2UL << 18) | + USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP; + USBx_OUTEP(2U)->DOEPINT = 0xFF; + + USBx_OUTEP(3U)->DOEPTSIZ = (32UL << 19) | 0x800U; + USBx_OUTEP(3U)->DOEPCTL = (0x40U & USB_OTG_DOEPCTL_MPSIZ) | (2UL << 18) | + USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP; + USBx_OUTEP(3U)->DOEPINT = 0xFF; + + // mark ready to receive + USBx_OUTEP(2U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(3U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + + USB_WritePacket(0, 0, 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + case USB_REQ_SET_ADDRESS: + // set now? + USBx_DEVICE->DCFG |= ((setup.b.wValue.w & 0x7fU) << 4); + + #ifdef DEBUG_USB + print(" set address\n"); + #endif + + USB_WritePacket(0, 0, 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + + break; + case USB_REQ_GET_DESCRIPTOR: + switch (setup.b.wValue.bw.lsb) { + case USB_DESC_TYPE_DEVICE: + //print(" writing device descriptor\n"); + + // set bcdDevice to hardware type + device_desc[13] = hw_type; + // setup transfer + USB_WritePacket(device_desc, MIN(sizeof(device_desc), setup.b.wLength.w), 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + + //print("D"); + break; + case USB_DESC_TYPE_CONFIGURATION: + USB_WritePacket(configuration_desc, MIN(sizeof(configuration_desc), setup.b.wLength.w), 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + case USB_DESC_TYPE_DEVICE_QUALIFIER: + USB_WritePacket(device_qualifier, MIN(sizeof(device_qualifier), setup.b.wLength.w), 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + case USB_DESC_TYPE_STRING: + switch (setup.b.wValue.bw.msb) { + case STRING_OFFSET_LANGID: + USB_WritePacket((uint8_t*)string_language_desc, MIN(sizeof(string_language_desc), setup.b.wLength.w), 0); + break; + case STRING_OFFSET_IMANUFACTURER: + USB_WritePacket((uint8_t*)string_manufacturer_desc, MIN(sizeof(string_manufacturer_desc), setup.b.wLength.w), 0); + break; + case STRING_OFFSET_IPRODUCT: + USB_WritePacket((uint8_t*)string_product_desc, MIN(sizeof(string_product_desc), setup.b.wLength.w), 0); + break; + case STRING_OFFSET_ISERIAL: + response[0] = 0x02 + (12 * 4); + response[1] = 0x03; + + // 96 bits = 12 bytes + for (int i = 0; i < 12; i++){ + uint8_t cc = ((uint8_t *)UID_BASE)[i]; + response[2 + (i * 4)] = to_hex_char((cc >> 4) & 0xFU); + response[2 + (i * 4) + 1] = '\0'; + response[2 + (i * 4) + 2] = to_hex_char((cc >> 0) & 0xFU); + response[2 + (i * 4) + 3] = '\0'; + } + + USB_WritePacket(response, MIN(response[0], setup.b.wLength.w), 0); + break; + case STRING_OFFSET_ICONFIGURATION: + USB_WritePacket((uint8_t*)string_configuration_desc, MIN(sizeof(string_configuration_desc), setup.b.wLength.w), 0); + break; + case 238: + USB_WritePacket((uint8_t*)string_238_desc, MIN(sizeof(string_238_desc), setup.b.wLength.w), 0); + break; + default: + // nothing + USB_WritePacket(0, 0, 0); + break; + } + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + case USB_DESC_TYPE_BINARY_OBJECT_STORE: + USB_WritePacket(binary_object_store_desc, MIN(sizeof(binary_object_store_desc), setup.b.wLength.w), 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + default: + // nothing here? + USB_WritePacket(0, 0, 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + } + break; + case USB_REQ_GET_STATUS: + // empty response? + response[0] = 0; + response[1] = 0; + USB_WritePacket((void*)&response, 2, 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + case USB_REQ_SET_INTERFACE: + // Store the alt setting number for IN EP behavior. + current_int0_alt_setting = setup.b.wValue.w; + USB_WritePacket(0, 0, 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + case WEBUSB_VENDOR_CODE: + // probably asking for allowed origins, which was removed from the spec + USB_WritePacket(0, 0, 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + case MS_VENDOR_CODE: + switch (setup.b.wIndex.w) { + // winusb 2.0 descriptor from BOS + case WINUSB_REQ_GET_DESCRIPTOR: + USB_WritePacket_EP0((uint8_t*)winusb_20_desc, MIN(sizeof(winusb_20_desc), setup.b.wLength.w)); + break; + // Extended Compat ID OS Descriptor + case WINUSB_REQ_GET_COMPATID_DESCRIPTOR: + USB_WritePacket_EP0((uint8_t*)winusb_ext_compatid_os_desc, MIN(sizeof(winusb_ext_compatid_os_desc), setup.b.wLength.w)); + break; + // Extended Properties OS Descriptor + case WINUSB_REQ_GET_EXT_PROPS_OS: + USB_WritePacket_EP0((uint8_t*)winusb_ext_prop_os_desc, MIN(sizeof(winusb_ext_prop_os_desc), setup.b.wLength.w)); + break; + default: + USB_WritePacket_EP0(0, 0); + } + break; + default: + control_req.request = setup.b.bRequest; + control_req.param1 = setup.b.wValue.w; + control_req.param2 = setup.b.wIndex.w; + control_req.length = setup.b.wLength.w; + + resp_len = comms_control_handler(&control_req, response); + USB_WritePacket(response, MIN(resp_len, setup.b.wLength.w), 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + } +} + + +// ***************************** USB port ***************************** + +void usb_irqhandler(void) { + //USBx->GINTMSK = 0; + static uint8_t usbdata[0x100] __attribute__((aligned(4))); + unsigned int gintsts = USBx->GINTSTS; + unsigned int gotgint = USBx->GOTGINT; + unsigned int daint = USBx_DEVICE->DAINT; + + // gintsts SUSPEND? 04008428 + #ifdef DEBUG_USB + puth(gintsts); + print(" "); + /*puth(USBx->GCCFG); + print(" ");*/ + puth(gotgint); + print(" ep "); + puth(daint); + print(" USB interrupt!\n"); + #endif + + if ((gintsts & USB_OTG_GINTSTS_CIDSCHG) != 0U) { + print("connector ID status change\n"); + } + + if ((gintsts & USB_OTG_GINTSTS_USBRST) != 0U) { + #ifdef DEBUG_USB + print("USB reset\n"); + #endif + usb_reset(); + } + + if ((gintsts & USB_OTG_GINTSTS_ENUMDNE) != 0U) { + #ifdef DEBUG_USB + print("enumeration done\n"); + #endif + // Full speed, ENUMSPD + //puth(USBx_DEVICE->DSTS); + } + + if ((gintsts & USB_OTG_GINTSTS_OTGINT) != 0U) { + #ifdef DEBUG_USB + print("OTG int:"); + puth(USBx->GOTGINT); + print("\n"); + #endif + + // getting ADTOCHG + //USBx->GOTGINT = USBx->GOTGINT; + } + + // RX FIFO first + if ((gintsts & USB_OTG_GINTSTS_RXFLVL) != 0U) { + // 1. Read the Receive status pop register + volatile unsigned int rxst = USBx->GRXSTSP; + int status = (rxst & USB_OTG_GRXSTSP_PKTSTS) >> 17; + + #ifdef DEBUG_USB + print(" RX FIFO:"); + puth(rxst); + print(" status: "); + puth(status); + print(" len: "); + puth((rxst & USB_OTG_GRXSTSP_BCNT) >> 4); + print("\n"); + #endif + + if (status == STS_DATA_UPDT) { + int endpoint = (rxst & USB_OTG_GRXSTSP_EPNUM); + int len = (rxst & USB_OTG_GRXSTSP_BCNT) >> 4; + (void)USB_ReadPacket(&usbdata, len); + #ifdef DEBUG_USB + print(" data "); + puth(len); + print("\n"); + hexdump(&usbdata, len); + #endif + + if (endpoint == 2) { + comms_endpoint2_write((uint8_t *) usbdata, len); + } + + if (endpoint == 3) { + outep3_processing = true; + comms_can_write(usbdata, len); + } + } else if (status == STS_SETUP_UPDT) { + (void)USB_ReadPacket(&setup, 8); + #ifdef DEBUG_USB + print(" setup "); + hexdump(&setup, 8); + print("\n"); + #endif + } else { + // status is neither STS_DATA_UPDT or STS_SETUP_UPDT, skip + } + } + + /*if (gintsts & USB_OTG_GINTSTS_HPRTINT) { + // host + print("HPRT:"); + puth(USBx_HOST_PORT->HPRT); + print("\n"); + if (USBx_HOST_PORT->HPRT & USB_OTG_HPRT_PCDET) { + USBx_HOST_PORT->HPRT |= USB_OTG_HPRT_PRST; + USBx_HOST_PORT->HPRT |= USB_OTG_HPRT_PCDET; + } + + }*/ + + if ((gintsts & USB_OTG_GINTSTS_BOUTNAKEFF) || (gintsts & USB_OTG_GINTSTS_GINAKEFF)) { + // no global NAK, why is this getting set? + #ifdef DEBUG_USB + print("GLOBAL NAK\n"); + #endif + USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGONAK | USB_OTG_DCTL_CGINAK; + } + + if ((gintsts & USB_OTG_GINTSTS_SRQINT) != 0U) { + // we want to do "A-device host negotiation protocol" since we are the A-device + /*print("start request\n"); + puth(USBx->GOTGCTL); + print("\n");*/ + //USBx->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD; + //USBx_HOST_PORT->HPRT = USB_OTG_HPRT_PPWR | USB_OTG_HPRT_PENA; + //USBx->GOTGCTL |= USB_OTG_GOTGCTL_SRQ; + } + + // out endpoint hit + if ((gintsts & USB_OTG_GINTSTS_OEPINT) != 0U) { + #ifdef DEBUG_USB + print(" 0:"); + puth(USBx_OUTEP(0U)->DOEPINT); + print(" 2:"); + puth(USBx_OUTEP(2U)->DOEPINT); + print(" 3:"); + puth(USBx_OUTEP(3U)->DOEPINT); + print(" "); + puth(USBx_OUTEP(3U)->DOEPCTL); + print(" 4:"); + puth(USBx_OUTEP(4)->DOEPINT); + print(" OUT ENDPOINT\n"); + #endif + + if ((USBx_OUTEP(2U)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0U) { + #ifdef DEBUG_USB + print(" OUT2 PACKET XFRC\n"); + #endif + USBx_OUTEP(2U)->DOEPTSIZ = (1UL << 19) | 0x40U; + USBx_OUTEP(2U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + } + + if ((USBx_OUTEP(3U)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0U) { + #ifdef DEBUG_USB + print(" OUT3 PACKET XFRC\n"); + #endif + // NAK cleared by process_can (if tx buffers have room) + outep3_processing = false; + refresh_can_tx_slots_available(); + } else if ((USBx_OUTEP(3U)->DOEPINT & 0x2000U) != 0U) { + #ifdef DEBUG_USB + print(" OUT3 PACKET WTF\n"); + #endif + // if NAK was set trigger this, unknown interrupt + // TODO: why was this here? fires when TX buffers when we can't clear NAK + // USBx_OUTEP(3U)->DOEPTSIZ = (1U << 19) | 0x40U; + // USBx_OUTEP(3U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + } else if ((USBx_OUTEP(3U)->DOEPINT) != 0U) { + #ifdef DEBUG_USB + print("OUTEP3 error "); + puth(USBx_OUTEP(3U)->DOEPINT); + print("\n"); + #endif + } else { + // USBx_OUTEP(3U)->DOEPINT is 0, ok to skip + } + + if ((USBx_OUTEP(0U)->DOEPINT & USB_OTG_DIEPINT_XFRC) != 0U) { + // ready for next packet + USBx_OUTEP(0U)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1UL << 19)) | (1U << 3); + } + + // respond to setup packets + if ((USBx_OUTEP(0U)->DOEPINT & USB_OTG_DOEPINT_STUP) != 0U) { + usb_setup(); + } + + USBx_OUTEP(0U)->DOEPINT = USBx_OUTEP(0U)->DOEPINT; + USBx_OUTEP(2U)->DOEPINT = USBx_OUTEP(2U)->DOEPINT; + USBx_OUTEP(3U)->DOEPINT = USBx_OUTEP(3U)->DOEPINT; + } + + // interrupt endpoint hit (Page 1221) + if ((gintsts & USB_OTG_GINTSTS_IEPINT) != 0U) { + #ifdef DEBUG_USB + print(" "); + puth(USBx_INEP(0U)->DIEPINT); + print(" "); + puth(USBx_INEP(1U)->DIEPINT); + print(" IN ENDPOINT\n"); + #endif + + // Should likely check the EP of the IN request even if there is + // only one IN endpoint. + + // No need to set NAK in OTG_DIEPCTL0 when nothing to send, + // Appears USB core automatically sets NAK. WritePacket clears it. + + // Handle the two interface alternate settings. Setting 0 has EP1 + // as bulk. Setting 1 has EP1 as interrupt. The code to handle + // these two EP variations are very similar and can be + // restructured for smaller code footprint. Keeping split out for + // now for clarity. + + //TODO add default case. Should it NAK? + switch (current_int0_alt_setting) { + case 0: ////// Bulk config + // *** IN token received when TxFIFO is empty + if ((USBx_INEP(1U)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0U) { + #ifdef DEBUG_USB + print(" IN PACKET QUEUE\n"); + #endif + // TODO: always assuming max len, can we get the length? + USB_WritePacket((void *)response, comms_can_read(response, 0x40), 1); + } + break; + + case 1: ////// Interrupt config + // *** IN token received when TxFIFO is empty + if ((USBx_INEP(1U)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0U) { + #ifdef DEBUG_USB + print(" IN PACKET QUEUE\n"); + #endif + // TODO: always assuming max len, can we get the length? + int len = comms_can_read(response, 0x40); + if (len > 0) { + USB_WritePacket((void *)response, len, 1); + } + } + break; + default: + print("current_int0_alt_setting value invalid\n"); + break; + } + + if ((USBx_INEP(0U)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0U) { + #ifdef DEBUG_USB + print(" IN PACKET QUEUE\n"); + #endif + + if ((ep0_txlen != 0U) && ((USBx_INEP(0U)->DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV) >= 0x40U)) { + uint16_t len = MIN(ep0_txlen, 0x40); + USB_WritePacket(ep0_txdata, len, 0); + ep0_txdata = &ep0_txdata[len]; + ep0_txlen -= len; + if (ep0_txlen == 0U) { + ep0_txdata = NULL; + USBx_DEVICE->DIEPEMPMSK &= ~1; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + } + } + } + + // clear interrupts + USBx_INEP(0U)->DIEPINT = USBx_INEP(0U)->DIEPINT; // Why ep0? + USBx_INEP(1U)->DIEPINT = USBx_INEP(1U)->DIEPINT; + } + + // clear all interrupts we handled + USBx_DEVICE->DAINT = daint; + USBx->GOTGINT = gotgint; + USBx->GINTSTS = gintsts; + + //USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM | USB_OTG_GINTSTS_SOF | USB_OTG_GINTSTS_EOPF); +} + +void can_tx_comms_resume_usb(void) { + ENTER_CRITICAL(); + if (!outep3_processing && (USBx_OUTEP(3U)->DOEPCTL & USB_OTG_DOEPCTL_NAKSTS) != 0U) { + USBx_OUTEP(3U)->DOEPTSIZ = (32UL << 19) | 0x800U; + USBx_OUTEP(3U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + } + EXIT_CRITICAL(); +} diff --git a/board/drivers/usb.h b/board/drivers/usb.h index d393d7e4d13..a1de7acfdb9 100644 --- a/board/drivers/usb.h +++ b/board/drivers/usb.h @@ -1,795 +1,118 @@ -#include "usb_declarations.h" +#pragma once -static uint8_t response[USBPACKET_MAX_SIZE]; +#include -// current packet -static USB_Setup_TypeDef setup; -static uint8_t* ep0_txdata = NULL; -static uint16_t ep0_txlen = 0; -static bool outep3_processing = false; +// IRQs: OTG_FS -// Store the current interface alt setting. -static int current_int0_alt_setting = 0; - -// packet read and write - -static void *USB_ReadPacket(void *dest, uint16_t len) { - uint32_t *dest_copy = (uint32_t *)dest; - uint32_t count32b = ((uint32_t)len + 3U) / 4U; - - for (uint32_t i = 0; i < count32b; i++) { - *dest_copy = USBx_DFIFO(0U); - dest_copy++; - } - return ((void *)dest_copy); -} - -static void USB_WritePacket(const void *src, uint16_t len, uint32_t ep) { - #ifdef DEBUG_USB - print("writing "); - hexdump(src, len); - #endif - - uint32_t numpacket = ((uint32_t)len + (USBPACKET_MAX_SIZE - 1U)) / USBPACKET_MAX_SIZE; - uint32_t count32b = 0; - count32b = ((uint32_t)len + 3U) / 4U; - - // TODO: revisit this - USBx_INEP(ep)->DIEPTSIZ = ((numpacket << 19) & USB_OTG_DIEPTSIZ_PKTCNT) | - (len & USB_OTG_DIEPTSIZ_XFRSIZ); - USBx_INEP(ep)->DIEPCTL |= (USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA); - - // load the FIFO - if (src != NULL) { - const uint32_t *src_copy = (const uint32_t *)src; - for (uint32_t i = 0; i < count32b; i++) { - USBx_DFIFO(ep) = *src_copy; - src_copy++; - } - } -} - -// IN EP 0 TX FIFO has a max size of 127 bytes (much smaller than the rest) -// so use TX FIFO empty interrupt to send larger amounts of data -static void USB_WritePacket_EP0(uint8_t *src, uint16_t len) { - #ifdef DEBUG_USB - print("writing "); - hexdump(src, len); - #endif - - uint16_t wplen = MIN(len, 0x40); - USB_WritePacket(src, wplen, 0); - - if (wplen < len) { - ep0_txdata = &src[wplen]; - ep0_txlen = len - wplen; - USBx_DEVICE->DIEPEMPMSK |= 1; - } else { - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - } -} - -static void usb_reset(void) { - // unmask endpoint interrupts, so many sets - USBx_DEVICE->DAINT = 0xFFFFFFFFU; - USBx_DEVICE->DAINTMSK = 0xFFFFFFFFU; - //USBx_DEVICE->DOEPMSK = (USB_OTG_DOEPMSK_STUPM | USB_OTG_DOEPMSK_XFRCM | USB_OTG_DOEPMSK_EPDM); - //USBx_DEVICE->DIEPMSK = (USB_OTG_DIEPMSK_TOM | USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_EPDM | USB_OTG_DIEPMSK_ITTXFEMSK); - //USBx_DEVICE->DIEPMSK = (USB_OTG_DIEPMSK_TOM | USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_EPDM); - - // all interrupts for debugging - USBx_DEVICE->DIEPMSK = 0xFFFFFFFFU; - USBx_DEVICE->DOEPMSK = 0xFFFFFFFFU; - - // clear interrupts - USBx_INEP(0U)->DIEPINT = 0xFF; - USBx_OUTEP(0U)->DOEPINT = 0xFF; - - // unset the address - USBx_DEVICE->DCFG &= ~USB_OTG_DCFG_DAD; - - // set up USB FIFOs - // RX start address is fixed to 0 - USBx->GRXFSIZ = 0x40; - - // 0x100 to offset past GRXFSIZ - USBx->DIEPTXF0_HNPTXFSIZ = (0x40UL << 16) | 0x40U; - - // EP1, massive - USBx->DIEPTXF[0] = (0x40UL << 16) | 0x80U; - - // flush TX fifo - USBx->GRSTCTL = USB_OTG_GRSTCTL_TXFFLSH | USB_OTG_GRSTCTL_TXFNUM_4; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH); - // flush RX FIFO - USBx->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH); - - // no global NAK - USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGINAK; - - // ready to receive setup packets - USBx_OUTEP(0U)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1UL << 19)) | (3U << 3); -} - -static char to_hex_char(uint8_t a) { - char ret; - if (a < 10U) { - ret = '0' + a; - } else { - ret = 'a' + (a - 10U); +typedef union { + uint16_t w; + struct BW { + uint8_t msb; + uint8_t lsb; } - return ret; -} - -static void usb_setup(void) { - static uint8_t device_desc[] = { - DSCR_DEVICE_LEN, USB_DESC_TYPE_DEVICE, //Length, Type - 0x10, 0x02, // bcdUSB max version of USB supported (2.1) - 0xFF, 0xFF, 0xFF, 0x40, // Class, Subclass, Protocol, Max Packet Size - TOUSBORDER(USB_VID), // idVendor - TOUSBORDER(USB_PID), // idProduct - 0x00, 0x00, // bcdDevice - 0x01, 0x02, // Manufacturer, Product - 0x03, 0x01 // Serial Number, Num Configurations - }; - - static uint8_t device_qualifier[] = { - 0x0a, USB_DESC_TYPE_DEVICE_QUALIFIER, //Length, Type - 0x10, 0x02, // bcdUSB max version of USB supported (2.1) - 0xFF, 0xFF, 0xFF, 0x40, // bDeviceClass, bDeviceSubClass, bDeviceProtocol, bMaxPacketSize0 - 0x01, 0x00 // bNumConfigurations, bReserved - }; - - static uint8_t configuration_desc[] = { - DSCR_CONFIG_LEN, USB_DESC_TYPE_CONFIGURATION, // Length, Type, - TOUSBORDER(0x0045U), // Total Len (uint16) - 0x01, 0x01, STRING_OFFSET_ICONFIGURATION, // Num Interface, Config Value, Configuration - 0xc0, 0x32, // Attributes, Max Power - // interface 0 ALT 0 - DSCR_INTERFACE_LEN, USB_DESC_TYPE_INTERFACE, // Length, Type - 0x00, 0x00, 0x03, // Index, Alt Index idx, Endpoint count - 0XFF, 0xFF, 0xFF, // Class, Subclass, Protocol - 0x00, // Interface - // endpoint 1, read CAN - DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type - ENDPOINT_RCV | 1, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type - TOUSBORDER(0x0040U), // Max Packet (0x0040) - 0x00, // Polling Interval (NA) - // endpoint 2, send serial - DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type - ENDPOINT_SND | 2, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type - TOUSBORDER(0x0040U), // Max Packet (0x0040) - 0x00, // Polling Interval - // endpoint 3, send CAN - DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type - ENDPOINT_SND | 3, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type - TOUSBORDER(0x0040U), // Max Packet (0x0040) - 0x00, // Polling Interval - // interface 0 ALT 1 - DSCR_INTERFACE_LEN, USB_DESC_TYPE_INTERFACE, // Length, Type - 0x00, 0x01, 0x03, // Index, Alt Index idx, Endpoint count - 0XFF, 0xFF, 0xFF, // Class, Subclass, Protocol - 0x00, // Interface - // endpoint 1, read CAN - DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type - ENDPOINT_RCV | 1, ENDPOINT_TYPE_INT, // Endpoint Num/Direction, Type - TOUSBORDER(0x0040U), // Max Packet (0x0040) - 0x05, // Polling Interval (5 frames) - // endpoint 2, send serial - DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type - ENDPOINT_SND | 2, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type - TOUSBORDER(0x0040U), // Max Packet (0x0040) - 0x00, // Polling Interval - // endpoint 3, send CAN - DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type - ENDPOINT_SND | 3, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type - TOUSBORDER(0x0040U), // Max Packet (0x0040) - 0x00, // Polling Interval - }; - - // STRING_DESCRIPTOR_HEADER is for uint16 string descriptors - // it takes in a string length, which is bytes/2 because unicode - static uint16_t string_language_desc[] = { - STRING_DESCRIPTOR_HEADER(1), - 0x0409 // american english - }; - - // these strings are all uint16's so that we don't need to spam ,0 after every character - static uint16_t string_manufacturer_desc[] = { - STRING_DESCRIPTOR_HEADER(8), - 'c', 'o', 'm', 'm', 'a', '.', 'a', 'i' - }; - - static uint16_t string_product_desc[] = { - STRING_DESCRIPTOR_HEADER(5), - 'p', 'a', 'n', 'd', 'a' - }; - - // a string containing the default configuration index - static uint16_t string_configuration_desc[] = { - STRING_DESCRIPTOR_HEADER(2), - '0', '1' // "01" - }; - - // WCID (auto install WinUSB driver) - // https://github.com/pbatard/libwdi/wiki/WCID-Devices - // https://docs.microsoft.com/en-us/windows-hardware/drivers/usbcon/winusb-installation#automatic-installation-of--winusb-without-an-inf-file - // WinUSB 1.0 descriptors, this is mostly used by Windows XP - static uint8_t string_238_desc[] = { - 0x12, USB_DESC_TYPE_STRING, // bLength, bDescriptorType - 'M',0, 'S',0, 'F',0, 'T',0, '1',0, '0',0, '0',0, // qwSignature (MSFT100) - MS_VENDOR_CODE, 0x00 // bMS_VendorCode, bPad - }; - - static uint8_t winusb_ext_compatid_os_desc[] = { - 0x28, 0x00, 0x00, 0x00, // dwLength - 0x00, 0x01, // bcdVersion - 0x04, 0x00, // wIndex - 0x01, // bCount - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Reserved - 0x00, // bFirstInterfaceNumber - 0x00, // Reserved - 'W', 'I', 'N', 'U', 'S', 'B', 0x00, 0x00, // compatible ID (WINUSB) - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // subcompatible ID (none) - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 // Reserved - }; - - static uint8_t winusb_ext_prop_os_desc[] = { - 0x8e, 0x00, 0x00, 0x00, // dwLength - 0x00, 0x01, // bcdVersion - 0x05, 0x00, // wIndex - 0x01, 0x00, // wCount - // first property - 0x84, 0x00, 0x00, 0x00, // dwSize - 0x01, 0x00, 0x00, 0x00, // dwPropertyDataType - 0x28, 0x00, // wPropertyNameLength - 'D',0, 'e',0, 'v',0, 'i',0, 'c',0, 'e',0, 'I',0, 'n',0, 't',0, 'e',0, 'r',0, 'f',0, 'a',0, 'c',0, 'e',0, 'G',0, 'U',0, 'I',0, 'D',0, 0, 0, // bPropertyName (DeviceInterfaceGUID) - 0x4e, 0x00, 0x00, 0x00, // dwPropertyDataLength - '{',0, 'c',0, 'c',0, 'e',0, '5',0, '2',0, '9',0, '1',0, 'c',0, '-',0, 'a',0, '6',0, '9',0, 'f',0, '-',0, '4',0 ,'9',0 ,'9',0 ,'5',0 ,'-',0, 'a',0, '4',0, 'c',0, '2',0, '-',0, '2',0, 'a',0, 'e',0, '5',0, '7',0, 'a',0, '5',0, '1',0, 'a',0, 'd',0, 'e',0, '9',0, '}',0, 0, 0, // bPropertyData ({CCE5291C-A69F-4995-A4C2-2AE57A51ADE9}) - }; - - /* - Binary Object Store descriptor used to expose WebUSB (and more WinUSB) metadata - comments are from the wicg spec - References used: - https://wicg.github.io/webusb/#webusb-platform-capability-descriptor - https://github.com/sowbug/weblight/blob/192ad7a0e903542e2aa28c607d98254a12a6399d/firmware/webusb.c - https://os.mbed.com/users/larsgk/code/USBDevice_WebUSB/file/1d8a6665d607/WebUSBDevice/ - */ - static uint8_t binary_object_store_desc[] = { - // BOS header - BINARY_OBJECT_STORE_DESCRIPTOR_LENGTH, // bLength, this is only the length of the header - BINARY_OBJECT_STORE_DESCRIPTOR, // bDescriptorType - 0x39, 0x00, // wTotalLength (LSB, MSB) - 0x02, // bNumDeviceCaps (WebUSB + WinUSB) - - // ------------------------------------------------- - // WebUSB descriptor - // header - 0x18, // bLength, Size of this descriptor. Must be set to 24. - 0x10, // bDescriptorType, DEVICE CAPABILITY descriptor - 0x05, // bDevCapabilityType, PLATFORM capability - 0x00, // bReserved, This field is reserved and shall be set to zero. - - // PlatformCapabilityUUID, Must be set to {3408b638-09a9-47a0-8bfd-a0768815b665}. - 0x38, 0xB6, 0x08, 0x34, - 0xA9, 0x09, 0xA0, 0x47, - 0x8B, 0xFD, 0xA0, 0x76, - 0x88, 0x15, 0xB6, 0x65, - // - - 0x00, 0x01, // bcdVersion, Protocol version supported. Must be set to 0x0100. - WEBUSB_VENDOR_CODE, // bVendorCode, bRequest value used for issuing WebUSB requests. - // there used to be a concept of "allowed origins", but it was removed from the spec - // it was intended to be a security feature, but then the entire security model relies on domain ownership - // https://github.com/WICG/webusb/issues/49 - // other implementations use various other indexed to leverate this no-longer-valid feature. we wont. - // the spec says we *must* reply to index 0x03 with the url, so we'll hint that that's the right index - 0x03, // iLandingPage, URL descriptor index of the device’s landing page. - - // ------------------------------------------------- - // WinUSB descriptor - // header - 0x1C, // Descriptor size (28 bytes) - 0x10, // Descriptor type (Device Capability) - 0x05, // Capability type (Platform) - 0x00, // Reserved - - // MS OS 2.0 Platform Capability ID (D8DD60DF-4589-4CC7-9CD2-659D9E648A9F) - // Indicates the device supports the Microsoft OS 2.0 descriptor - 0xDF, 0x60, 0xDD, 0xD8, - 0x89, 0x45, 0xC7, 0x4C, - 0x9C, 0xD2, 0x65, 0x9D, - 0x9E, 0x64, 0x8A, 0x9F, - - 0x00, 0x00, 0x03, 0x06, // Windows version, currently set to 8.1 (0x06030000) - - WINUSB_PLATFORM_DESCRIPTOR_LENGTH, 0x00, // MS OS 2.0 descriptor size (word) - MS_VENDOR_CODE, 0x00 // vendor code, no alternate enumeration - }; - - // WinUSB 2.0 descriptor. This is what modern systems use - // https://github.com/sowbug/weblight/blob/192ad7a0e903542e2aa28c607d98254a12a6399d/firmware/webusb.c - // http://janaxelson.com/files/ms_os_20_descriptors.c - // https://books.google.com/books?id=pkefBgAAQBAJ&pg=PA353&lpg=PA353 - static uint8_t winusb_20_desc[WINUSB_PLATFORM_DESCRIPTOR_LENGTH] = { - // Microsoft OS 2.0 descriptor set header (table 10) - 0x0A, 0x00, // Descriptor size (10 bytes) - 0x00, 0x00, // MS OS 2.0 descriptor set header - - 0x00, 0x00, 0x03, 0x06, // Windows version (8.1) (0x06030000) - WINUSB_PLATFORM_DESCRIPTOR_LENGTH, 0x00, // Total size of MS OS 2.0 descriptor set - - // Microsoft OS 2.0 compatible ID descriptor - 0x14, 0x00, // Descriptor size (20 bytes) - 0x03, 0x00, // MS OS 2.0 compatible ID descriptor - 'W', 'I', 'N', 'U', 'S', 'B', 0x00, 0x00, // compatible ID (WINUSB) - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Sub-compatible ID - - // Registry property descriptor - 0x80, 0x00, // Descriptor size (130 bytes) - 0x04, 0x00, // Registry Property descriptor - 0x01, 0x00, // Strings are null-terminated Unicode - 0x28, 0x00, // Size of Property Name (40 bytes) "DeviceInterfaceGUID" - - // bPropertyName (DeviceInterfaceGUID) - 'D', 0x00, 'e', 0x00, 'v', 0x00, 'i', 0x00, 'c', 0x00, 'e', 0x00, 'I', 0x00, 'n', 0x00, - 't', 0x00, 'e', 0x00, 'r', 0x00, 'f', 0x00, 'a', 0x00, 'c', 0x00, 'e', 0x00, 'G', 0x00, - 'U', 0x00, 'I', 0x00, 'D', 0x00, 0x00, 0x00, - - 0x4E, 0x00, // Size of Property Data (78 bytes) - - // Vendor-defined property data: {CCE5291C-A69F-4995-A4C2-2AE57A51ADE9} - '{', 0x00, 'c', 0x00, 'c', 0x00, 'e', 0x00, '5', 0x00, '2', 0x00, '9', 0x00, '1', 0x00, // 16 - 'c', 0x00, '-', 0x00, 'a', 0x00, '6', 0x00, '9', 0x00, 'f', 0x00, '-', 0x00, '4', 0x00, // 32 - '9', 0x00, '9', 0x00, '5', 0x00, '-', 0x00, 'a', 0x00, '4', 0x00, 'c', 0x00, '2', 0x00, // 48 - '-', 0x00, '2', 0x00, 'a', 0x00, 'e', 0x00, '5', 0x00, '7', 0x00, 'a', 0x00, '5', 0x00, // 64 - '1', 0x00, 'a', 0x00, 'd', 0x00, 'e', 0x00, '9', 0x00, '}', 0x00, 0x00, 0x00 // 78 bytes - }; - - int resp_len; - ControlPacket_t control_req; - - // setup packet is ready - switch (setup.b.bRequest) { - case USB_REQ_SET_CONFIGURATION: - // enable other endpoints, has to be here? - USBx_INEP(1U)->DIEPCTL = (0x40U & USB_OTG_DIEPCTL_MPSIZ) | (2UL << 18) | (1UL << 22) | - USB_OTG_DIEPCTL_SD0PID_SEVNFRM | USB_OTG_DIEPCTL_USBAEP; - USBx_INEP(1U)->DIEPINT = 0xFF; - - USBx_OUTEP(2U)->DOEPTSIZ = (1UL << 19) | 0x40U; - USBx_OUTEP(2U)->DOEPCTL = (0x40U & USB_OTG_DOEPCTL_MPSIZ) | (2UL << 18) | - USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP; - USBx_OUTEP(2U)->DOEPINT = 0xFF; - - USBx_OUTEP(3U)->DOEPTSIZ = (32UL << 19) | 0x800U; - USBx_OUTEP(3U)->DOEPCTL = (0x40U & USB_OTG_DOEPCTL_MPSIZ) | (2UL << 18) | - USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP; - USBx_OUTEP(3U)->DOEPINT = 0xFF; - - // mark ready to receive - USBx_OUTEP(2U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; - USBx_OUTEP(3U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; - - USB_WritePacket(0, 0, 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - case USB_REQ_SET_ADDRESS: - // set now? - USBx_DEVICE->DCFG |= ((setup.b.wValue.w & 0x7fU) << 4); - - #ifdef DEBUG_USB - print(" set address\n"); - #endif - - USB_WritePacket(0, 0, 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - - break; - case USB_REQ_GET_DESCRIPTOR: - switch (setup.b.wValue.bw.lsb) { - case USB_DESC_TYPE_DEVICE: - //print(" writing device descriptor\n"); - - // set bcdDevice to hardware type - device_desc[13] = hw_type; - // setup transfer - USB_WritePacket(device_desc, MIN(sizeof(device_desc), setup.b.wLength.w), 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - - //print("D"); - break; - case USB_DESC_TYPE_CONFIGURATION: - USB_WritePacket(configuration_desc, MIN(sizeof(configuration_desc), setup.b.wLength.w), 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - case USB_DESC_TYPE_DEVICE_QUALIFIER: - USB_WritePacket(device_qualifier, MIN(sizeof(device_qualifier), setup.b.wLength.w), 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - case USB_DESC_TYPE_STRING: - switch (setup.b.wValue.bw.msb) { - case STRING_OFFSET_LANGID: - USB_WritePacket((uint8_t*)string_language_desc, MIN(sizeof(string_language_desc), setup.b.wLength.w), 0); - break; - case STRING_OFFSET_IMANUFACTURER: - USB_WritePacket((uint8_t*)string_manufacturer_desc, MIN(sizeof(string_manufacturer_desc), setup.b.wLength.w), 0); - break; - case STRING_OFFSET_IPRODUCT: - USB_WritePacket((uint8_t*)string_product_desc, MIN(sizeof(string_product_desc), setup.b.wLength.w), 0); - break; - case STRING_OFFSET_ISERIAL: - response[0] = 0x02 + (12 * 4); - response[1] = 0x03; - - // 96 bits = 12 bytes - for (int i = 0; i < 12; i++){ - uint8_t cc = ((uint8_t *)UID_BASE)[i]; - response[2 + (i * 4)] = to_hex_char((cc >> 4) & 0xFU); - response[2 + (i * 4) + 1] = '\0'; - response[2 + (i * 4) + 2] = to_hex_char((cc >> 0) & 0xFU); - response[2 + (i * 4) + 3] = '\0'; - } - - USB_WritePacket(response, MIN(response[0], setup.b.wLength.w), 0); - break; - case STRING_OFFSET_ICONFIGURATION: - USB_WritePacket((uint8_t*)string_configuration_desc, MIN(sizeof(string_configuration_desc), setup.b.wLength.w), 0); - break; - case 238: - USB_WritePacket((uint8_t*)string_238_desc, MIN(sizeof(string_238_desc), setup.b.wLength.w), 0); - break; - default: - // nothing - USB_WritePacket(0, 0, 0); - break; - } - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - case USB_DESC_TYPE_BINARY_OBJECT_STORE: - USB_WritePacket(binary_object_store_desc, MIN(sizeof(binary_object_store_desc), setup.b.wLength.w), 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - default: - // nothing here? - USB_WritePacket(0, 0, 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - } - break; - case USB_REQ_GET_STATUS: - // empty response? - response[0] = 0; - response[1] = 0; - USB_WritePacket((void*)&response, 2, 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - case USB_REQ_SET_INTERFACE: - // Store the alt setting number for IN EP behavior. - current_int0_alt_setting = setup.b.wValue.w; - USB_WritePacket(0, 0, 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - case WEBUSB_VENDOR_CODE: - // probably asking for allowed origins, which was removed from the spec - USB_WritePacket(0, 0, 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - case MS_VENDOR_CODE: - switch (setup.b.wIndex.w) { - // winusb 2.0 descriptor from BOS - case WINUSB_REQ_GET_DESCRIPTOR: - USB_WritePacket_EP0((uint8_t*)winusb_20_desc, MIN(sizeof(winusb_20_desc), setup.b.wLength.w)); - break; - // Extended Compat ID OS Descriptor - case WINUSB_REQ_GET_COMPATID_DESCRIPTOR: - USB_WritePacket_EP0((uint8_t*)winusb_ext_compatid_os_desc, MIN(sizeof(winusb_ext_compatid_os_desc), setup.b.wLength.w)); - break; - // Extended Properties OS Descriptor - case WINUSB_REQ_GET_EXT_PROPS_OS: - USB_WritePacket_EP0((uint8_t*)winusb_ext_prop_os_desc, MIN(sizeof(winusb_ext_prop_os_desc), setup.b.wLength.w)); - break; - default: - USB_WritePacket_EP0(0, 0); - } - break; - default: - control_req.request = setup.b.bRequest; - control_req.param1 = setup.b.wValue.w; - control_req.param2 = setup.b.wIndex.w; - control_req.length = setup.b.wLength.w; - - resp_len = comms_control_handler(&control_req, response); - USB_WritePacket(response, MIN(resp_len, setup.b.wLength.w), 0); - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - } -} - - + bw; +} uint16_t_uint8_t; + +typedef union _USB_Setup { + uint32_t d8[2]; + struct _SetupPkt_Struc + { + uint8_t bmRequestType; + uint8_t bRequest; + uint16_t_uint8_t wValue; + uint16_t_uint8_t wIndex; + uint16_t_uint8_t wLength; + } b; +} USB_Setup_TypeDef; + +void usb_init(void); +void refresh_can_tx_slots_available(void); + +// **** supporting defines **** +#define USB_REQ_GET_STATUS 0x00 +#define USB_REQ_CLEAR_FEATURE 0x01 +#define USB_REQ_SET_FEATURE 0x03 +#define USB_REQ_SET_ADDRESS 0x05 +#define USB_REQ_GET_DESCRIPTOR 0x06 +#define USB_REQ_SET_DESCRIPTOR 0x07 +#define USB_REQ_GET_CONFIGURATION 0x08 +#define USB_REQ_SET_CONFIGURATION 0x09 +#define USB_REQ_GET_INTERFACE 0x0A +#define USB_REQ_SET_INTERFACE 0x0B +#define USB_REQ_SYNCH_FRAME 0x0C + +#define USB_DESC_TYPE_DEVICE 0x01 +#define USB_DESC_TYPE_CONFIGURATION 0x02 +#define USB_DESC_TYPE_STRING 0x03 +#define USB_DESC_TYPE_INTERFACE 0x04 +#define USB_DESC_TYPE_ENDPOINT 0x05 +#define USB_DESC_TYPE_DEVICE_QUALIFIER 0x06 +#define USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION 0x07 +#define USB_DESC_TYPE_BINARY_OBJECT_STORE 0x0f + +// offsets for configuration strings +#define STRING_OFFSET_LANGID 0x00 +#define STRING_OFFSET_IMANUFACTURER 0x01 +#define STRING_OFFSET_IPRODUCT 0x02 +#define STRING_OFFSET_ISERIAL 0x03 +#define STRING_OFFSET_ICONFIGURATION 0x04 +#define STRING_OFFSET_IINTERFACE 0x05 + +// WebUSB requests +#define WEBUSB_REQ_GET_URL 0x02 + +// WebUSB types +#define WEBUSB_DESC_TYPE_URL 0x03 +#define WEBUSB_URL_SCHEME_HTTPS 0x01 +#define WEBUSB_URL_SCHEME_HTTP 0x00 + +// WinUSB requests +#define WINUSB_REQ_GET_COMPATID_DESCRIPTOR 0x04 +#define WINUSB_REQ_GET_EXT_PROPS_OS 0x05 +#define WINUSB_REQ_GET_DESCRIPTOR 0x07 + +#define STS_GOUT_NAK 1 +#define STS_DATA_UPDT 2 +#define STS_XFER_COMP 3 +#define STS_SETUP_COMP 4 +#define STS_SETUP_UPDT 6 + +// for the repeating interfaces +#define DSCR_INTERFACE_LEN 9 +#define DSCR_ENDPOINT_LEN 7 +#define DSCR_CONFIG_LEN 9 +#define DSCR_DEVICE_LEN 18 + +// endpoint types +#define ENDPOINT_TYPE_CONTROL 0 +#define ENDPOINT_TYPE_ISO 1 +#define ENDPOINT_TYPE_BULK 2 +#define ENDPOINT_TYPE_INT 3 + +// These are arbitrary values used in bRequest +#define MS_VENDOR_CODE 0x20 +#define WEBUSB_VENDOR_CODE 0x30 + +// BOS constants +#define BINARY_OBJECT_STORE_DESCRIPTOR_LENGTH 0x05 +#define BINARY_OBJECT_STORE_DESCRIPTOR 0x0F +#define WINUSB_PLATFORM_DESCRIPTOR_LENGTH 0x9E + +// Convert machine byte order to USB byte order +#define TOUSBORDER(num)\ + ((num) & 0xFFU), (((uint16_t)(num) >> 8) & 0xFFU) + +// take in string length and return the first 2 bytes of a string descriptor +#define STRING_DESCRIPTOR_HEADER(size)\ + (((((size) * 2) + 2) & 0xFF) | 0x0300) + +#define ENDPOINT_RCV 0x80 +#define ENDPOINT_SND 0x00 // ***************************** USB port ***************************** +void can_tx_comms_resume_usb(void); -void usb_irqhandler(void) { - //USBx->GINTMSK = 0; - static uint8_t usbdata[0x100] __attribute__((aligned(4))); - unsigned int gintsts = USBx->GINTSTS; - unsigned int gotgint = USBx->GOTGINT; - unsigned int daint = USBx_DEVICE->DAINT; - - // gintsts SUSPEND? 04008428 - #ifdef DEBUG_USB - puth(gintsts); - print(" "); - /*puth(USBx->GCCFG); - print(" ");*/ - puth(gotgint); - print(" ep "); - puth(daint); - print(" USB interrupt!\n"); - #endif - - if ((gintsts & USB_OTG_GINTSTS_CIDSCHG) != 0U) { - print("connector ID status change\n"); - } - - if ((gintsts & USB_OTG_GINTSTS_USBRST) != 0U) { - #ifdef DEBUG_USB - print("USB reset\n"); - #endif - usb_reset(); - } - - if ((gintsts & USB_OTG_GINTSTS_ENUMDNE) != 0U) { - #ifdef DEBUG_USB - print("enumeration done\n"); - #endif - // Full speed, ENUMSPD - //puth(USBx_DEVICE->DSTS); - } - - if ((gintsts & USB_OTG_GINTSTS_OTGINT) != 0U) { - #ifdef DEBUG_USB - print("OTG int:"); - puth(USBx->GOTGINT); - print("\n"); - #endif - - // getting ADTOCHG - //USBx->GOTGINT = USBx->GOTGINT; - } - - // RX FIFO first - if ((gintsts & USB_OTG_GINTSTS_RXFLVL) != 0U) { - // 1. Read the Receive status pop register - volatile unsigned int rxst = USBx->GRXSTSP; - int status = (rxst & USB_OTG_GRXSTSP_PKTSTS) >> 17; - - #ifdef DEBUG_USB - print(" RX FIFO:"); - puth(rxst); - print(" status: "); - puth(status); - print(" len: "); - puth((rxst & USB_OTG_GRXSTSP_BCNT) >> 4); - print("\n"); - #endif - - if (status == STS_DATA_UPDT) { - int endpoint = (rxst & USB_OTG_GRXSTSP_EPNUM); - int len = (rxst & USB_OTG_GRXSTSP_BCNT) >> 4; - (void)USB_ReadPacket(&usbdata, len); - #ifdef DEBUG_USB - print(" data "); - puth(len); - print("\n"); - hexdump(&usbdata, len); - #endif - - if (endpoint == 2) { - comms_endpoint2_write((uint8_t *) usbdata, len); - } - - if (endpoint == 3) { - outep3_processing = true; - comms_can_write(usbdata, len); - } - } else if (status == STS_SETUP_UPDT) { - (void)USB_ReadPacket(&setup, 8); - #ifdef DEBUG_USB - print(" setup "); - hexdump(&setup, 8); - print("\n"); - #endif - } else { - // status is neither STS_DATA_UPDT or STS_SETUP_UPDT, skip - } - } - - /*if (gintsts & USB_OTG_GINTSTS_HPRTINT) { - // host - print("HPRT:"); - puth(USBx_HOST_PORT->HPRT); - print("\n"); - if (USBx_HOST_PORT->HPRT & USB_OTG_HPRT_PCDET) { - USBx_HOST_PORT->HPRT |= USB_OTG_HPRT_PRST; - USBx_HOST_PORT->HPRT |= USB_OTG_HPRT_PCDET; - } - - }*/ - - if ((gintsts & USB_OTG_GINTSTS_BOUTNAKEFF) || (gintsts & USB_OTG_GINTSTS_GINAKEFF)) { - // no global NAK, why is this getting set? - #ifdef DEBUG_USB - print("GLOBAL NAK\n"); - #endif - USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGONAK | USB_OTG_DCTL_CGINAK; - } - - if ((gintsts & USB_OTG_GINTSTS_SRQINT) != 0U) { - // we want to do "A-device host negotiation protocol" since we are the A-device - /*print("start request\n"); - puth(USBx->GOTGCTL); - print("\n");*/ - //USBx->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD; - //USBx_HOST_PORT->HPRT = USB_OTG_HPRT_PPWR | USB_OTG_HPRT_PENA; - //USBx->GOTGCTL |= USB_OTG_GOTGCTL_SRQ; - } - - // out endpoint hit - if ((gintsts & USB_OTG_GINTSTS_OEPINT) != 0U) { - #ifdef DEBUG_USB - print(" 0:"); - puth(USBx_OUTEP(0U)->DOEPINT); - print(" 2:"); - puth(USBx_OUTEP(2U)->DOEPINT); - print(" 3:"); - puth(USBx_OUTEP(3U)->DOEPINT); - print(" "); - puth(USBx_OUTEP(3U)->DOEPCTL); - print(" 4:"); - puth(USBx_OUTEP(4)->DOEPINT); - print(" OUT ENDPOINT\n"); - #endif - - if ((USBx_OUTEP(2U)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0U) { - #ifdef DEBUG_USB - print(" OUT2 PACKET XFRC\n"); - #endif - USBx_OUTEP(2U)->DOEPTSIZ = (1UL << 19) | 0x40U; - USBx_OUTEP(2U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; - } - - if ((USBx_OUTEP(3U)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0U) { - #ifdef DEBUG_USB - print(" OUT3 PACKET XFRC\n"); - #endif - // NAK cleared by process_can (if tx buffers have room) - outep3_processing = false; - refresh_can_tx_slots_available(); - } else if ((USBx_OUTEP(3U)->DOEPINT & 0x2000U) != 0U) { - #ifdef DEBUG_USB - print(" OUT3 PACKET WTF\n"); - #endif - // if NAK was set trigger this, unknown interrupt - // TODO: why was this here? fires when TX buffers when we can't clear NAK - // USBx_OUTEP(3U)->DOEPTSIZ = (1U << 19) | 0x40U; - // USBx_OUTEP(3U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - } else if ((USBx_OUTEP(3U)->DOEPINT) != 0U) { - #ifdef DEBUG_USB - print("OUTEP3 error "); - puth(USBx_OUTEP(3U)->DOEPINT); - print("\n"); - #endif - } else { - // USBx_OUTEP(3U)->DOEPINT is 0, ok to skip - } - - if ((USBx_OUTEP(0U)->DOEPINT & USB_OTG_DIEPINT_XFRC) != 0U) { - // ready for next packet - USBx_OUTEP(0U)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1UL << 19)) | (1U << 3); - } - - // respond to setup packets - if ((USBx_OUTEP(0U)->DOEPINT & USB_OTG_DOEPINT_STUP) != 0U) { - usb_setup(); - } - - USBx_OUTEP(0U)->DOEPINT = USBx_OUTEP(0U)->DOEPINT; - USBx_OUTEP(2U)->DOEPINT = USBx_OUTEP(2U)->DOEPINT; - USBx_OUTEP(3U)->DOEPINT = USBx_OUTEP(3U)->DOEPINT; - } - - // interrupt endpoint hit (Page 1221) - if ((gintsts & USB_OTG_GINTSTS_IEPINT) != 0U) { - #ifdef DEBUG_USB - print(" "); - puth(USBx_INEP(0U)->DIEPINT); - print(" "); - puth(USBx_INEP(1U)->DIEPINT); - print(" IN ENDPOINT\n"); - #endif - - // Should likely check the EP of the IN request even if there is - // only one IN endpoint. - - // No need to set NAK in OTG_DIEPCTL0 when nothing to send, - // Appears USB core automatically sets NAK. WritePacket clears it. - - // Handle the two interface alternate settings. Setting 0 has EP1 - // as bulk. Setting 1 has EP1 as interrupt. The code to handle - // these two EP variations are very similar and can be - // restructured for smaller code footprint. Keeping split out for - // now for clarity. - - //TODO add default case. Should it NAK? - switch (current_int0_alt_setting) { - case 0: ////// Bulk config - // *** IN token received when TxFIFO is empty - if ((USBx_INEP(1U)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0U) { - #ifdef DEBUG_USB - print(" IN PACKET QUEUE\n"); - #endif - // TODO: always assuming max len, can we get the length? - USB_WritePacket((void *)response, comms_can_read(response, 0x40), 1); - } - break; - - case 1: ////// Interrupt config - // *** IN token received when TxFIFO is empty - if ((USBx_INEP(1U)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0U) { - #ifdef DEBUG_USB - print(" IN PACKET QUEUE\n"); - #endif - // TODO: always assuming max len, can we get the length? - int len = comms_can_read(response, 0x40); - if (len > 0) { - USB_WritePacket((void *)response, len, 1); - } - } - break; - default: - print("current_int0_alt_setting value invalid\n"); - break; - } - - if ((USBx_INEP(0U)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0U) { - #ifdef DEBUG_USB - print(" IN PACKET QUEUE\n"); - #endif - - if ((ep0_txlen != 0U) && ((USBx_INEP(0U)->DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV) >= 0x40U)) { - uint16_t len = MIN(ep0_txlen, 0x40); - USB_WritePacket(ep0_txdata, len, 0); - ep0_txdata = &ep0_txdata[len]; - ep0_txlen -= len; - if (ep0_txlen == 0U) { - ep0_txdata = NULL; - USBx_DEVICE->DIEPEMPMSK &= ~1; - USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - } - } - } - - // clear interrupts - USBx_INEP(0U)->DIEPINT = USBx_INEP(0U)->DIEPINT; // Why ep0? - USBx_INEP(1U)->DIEPINT = USBx_INEP(1U)->DIEPINT; - } - - // clear all interrupts we handled - USBx_DEVICE->DAINT = daint; - USBx->GOTGINT = gotgint; - USBx->GINTSTS = gintsts; - - //USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM | USB_OTG_GINTSTS_SOF | USB_OTG_GINTSTS_EOPF); -} +// ***************************** USB port ***************************** -void can_tx_comms_resume_usb(void) { - ENTER_CRITICAL(); - if (!outep3_processing && (USBx_OUTEP(3U)->DOEPCTL & USB_OTG_DOEPCTL_NAKSTS) != 0U) { - USBx_OUTEP(3U)->DOEPTSIZ = (32UL << 19) | 0x800U; - USBx_OUTEP(3U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; - } - EXIT_CRITICAL(); -} +void usb_irqhandler(void); +void can_tx_comms_resume_usb(void); \ No newline at end of file diff --git a/board/drivers/usb_declarations.h b/board/drivers/usb_declarations.h deleted file mode 100644 index 379b162feb6..00000000000 --- a/board/drivers/usb_declarations.h +++ /dev/null @@ -1,111 +0,0 @@ -#pragma once - -// IRQs: OTG_FS - -typedef union { - uint16_t w; - struct BW { - uint8_t msb; - uint8_t lsb; - } - bw; -} uint16_t_uint8_t; - -typedef union _USB_Setup { - uint32_t d8[2]; - struct _SetupPkt_Struc - { - uint8_t bmRequestType; - uint8_t bRequest; - uint16_t_uint8_t wValue; - uint16_t_uint8_t wIndex; - uint16_t_uint8_t wLength; - } b; -} USB_Setup_TypeDef; - -void usb_init(void); -void refresh_can_tx_slots_available(void); - -// **** supporting defines **** -#define USB_REQ_GET_STATUS 0x00 -#define USB_REQ_CLEAR_FEATURE 0x01 -#define USB_REQ_SET_FEATURE 0x03 -#define USB_REQ_SET_ADDRESS 0x05 -#define USB_REQ_GET_DESCRIPTOR 0x06 -#define USB_REQ_SET_DESCRIPTOR 0x07 -#define USB_REQ_GET_CONFIGURATION 0x08 -#define USB_REQ_SET_CONFIGURATION 0x09 -#define USB_REQ_GET_INTERFACE 0x0A -#define USB_REQ_SET_INTERFACE 0x0B -#define USB_REQ_SYNCH_FRAME 0x0C - -#define USB_DESC_TYPE_DEVICE 0x01 -#define USB_DESC_TYPE_CONFIGURATION 0x02 -#define USB_DESC_TYPE_STRING 0x03 -#define USB_DESC_TYPE_INTERFACE 0x04 -#define USB_DESC_TYPE_ENDPOINT 0x05 -#define USB_DESC_TYPE_DEVICE_QUALIFIER 0x06 -#define USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION 0x07 -#define USB_DESC_TYPE_BINARY_OBJECT_STORE 0x0f - -// offsets for configuration strings -#define STRING_OFFSET_LANGID 0x00 -#define STRING_OFFSET_IMANUFACTURER 0x01 -#define STRING_OFFSET_IPRODUCT 0x02 -#define STRING_OFFSET_ISERIAL 0x03 -#define STRING_OFFSET_ICONFIGURATION 0x04 -#define STRING_OFFSET_IINTERFACE 0x05 - -// WebUSB requests -#define WEBUSB_REQ_GET_URL 0x02 - -// WebUSB types -#define WEBUSB_DESC_TYPE_URL 0x03 -#define WEBUSB_URL_SCHEME_HTTPS 0x01 -#define WEBUSB_URL_SCHEME_HTTP 0x00 - -// WinUSB requests -#define WINUSB_REQ_GET_COMPATID_DESCRIPTOR 0x04 -#define WINUSB_REQ_GET_EXT_PROPS_OS 0x05 -#define WINUSB_REQ_GET_DESCRIPTOR 0x07 - -#define STS_GOUT_NAK 1 -#define STS_DATA_UPDT 2 -#define STS_XFER_COMP 3 -#define STS_SETUP_COMP 4 -#define STS_SETUP_UPDT 6 - -// for the repeating interfaces -#define DSCR_INTERFACE_LEN 9 -#define DSCR_ENDPOINT_LEN 7 -#define DSCR_CONFIG_LEN 9 -#define DSCR_DEVICE_LEN 18 - -// endpoint types -#define ENDPOINT_TYPE_CONTROL 0 -#define ENDPOINT_TYPE_ISO 1 -#define ENDPOINT_TYPE_BULK 2 -#define ENDPOINT_TYPE_INT 3 - -// These are arbitrary values used in bRequest -#define MS_VENDOR_CODE 0x20 -#define WEBUSB_VENDOR_CODE 0x30 - -// BOS constants -#define BINARY_OBJECT_STORE_DESCRIPTOR_LENGTH 0x05 -#define BINARY_OBJECT_STORE_DESCRIPTOR 0x0F -#define WINUSB_PLATFORM_DESCRIPTOR_LENGTH 0x9E - -// Convert machine byte order to USB byte order -#define TOUSBORDER(num)\ - ((num) & 0xFFU), (((uint16_t)(num) >> 8) & 0xFFU) - -// take in string length and return the first 2 bytes of a string descriptor -#define STRING_DESCRIPTOR_HEADER(size)\ - (((((size) * 2) + 2) & 0xFF) | 0x0300) - -#define ENDPOINT_RCV 0x80 -#define ENDPOINT_SND 0x00 - -// ***************************** USB port ***************************** -void can_tx_comms_resume_usb(void); diff --git a/board/early_init.c b/board/early_init.c new file mode 100644 index 00000000000..fc6afb3cfbd --- /dev/null +++ b/board/early_init.c @@ -0,0 +1,66 @@ +#include + +#include "early_init.h" + +#include "board/stm32h7/stm32h7_config.h" + +#include "drivers/led.h" + +#include "bootstub_declarations.h" + +extern void *g_pfnVectors; +extern uint32_t enter_bootloader_mode; + +static void jump_to_bootloader(void) { + // do enter bootloader + enter_bootloader_mode = 0; + + bootloader_fcn_ptr bootloader_ptr = (bootloader_fcn_ptr)BOOTLOADER_ADDRESS; + bootloader_fcn bootloader = *bootloader_ptr; + + // jump to bootloader + enable_interrupts(); + bootloader(); + + // reset on exit + enter_bootloader_mode = BOOT_NORMAL; + NVIC_SystemReset(); +} + +void early_initialization(void) { + // Reset global critical depth + disable_interrupts(); + global_critical_depth = 0; + + // Init register and interrupt tables + init_registers(); + + // after it's been in the bootloader, things are initted differently, so we reset + if ((enter_bootloader_mode != BOOT_NORMAL) && + (enter_bootloader_mode != ENTER_BOOTLOADER_MAGIC) && + (enter_bootloader_mode != ENTER_SOFTLOADER_MAGIC)) { + enter_bootloader_mode = BOOT_NORMAL; + NVIC_SystemReset(); + } + + // if wrong chip, reboot + volatile unsigned int id = DBGMCU->IDCODE; + if ((id & 0xFFFU) != MCU_IDCODE) { + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + } + + // setup interrupt table + SCB->VTOR = (uint32_t)&g_pfnVectors; + + // early GPIOs float everything + early_gpio_float(); + + detect_board_type(); + + if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) { + led_init(); + current_board->init_bootloader(); + led_set(LED_GREEN, 1); + jump_to_bootloader(); + } +} diff --git a/board/early_init.h b/board/early_init.h index d5131a42cfb..543ffe95501 100644 --- a/board/early_init.h +++ b/board/early_init.h @@ -1,3 +1,7 @@ +#pragma once + +#include + // Early bringup #define ENTER_BOOTLOADER_MAGIC 0xdeadbeefU #define ENTER_SOFTLOADER_MAGIC 0xdeadc0deU @@ -9,56 +13,4 @@ extern uint32_t enter_bootloader_mode; typedef void (*bootloader_fcn)(void); typedef bootloader_fcn *bootloader_fcn_ptr; -static void jump_to_bootloader(void) { - // do enter bootloader - enter_bootloader_mode = 0; - - bootloader_fcn_ptr bootloader_ptr = (bootloader_fcn_ptr)BOOTLOADER_ADDRESS; - bootloader_fcn bootloader = *bootloader_ptr; - - // jump to bootloader - enable_interrupts(); - bootloader(); - - // reset on exit - enter_bootloader_mode = BOOT_NORMAL; - NVIC_SystemReset(); -} - -void early_initialization(void) { - // Reset global critical depth - disable_interrupts(); - global_critical_depth = 0; - - // Init register and interrupt tables - init_registers(); - - // after it's been in the bootloader, things are initted differently, so we reset - if ((enter_bootloader_mode != BOOT_NORMAL) && - (enter_bootloader_mode != ENTER_BOOTLOADER_MAGIC) && - (enter_bootloader_mode != ENTER_SOFTLOADER_MAGIC)) { - enter_bootloader_mode = BOOT_NORMAL; - NVIC_SystemReset(); - } - - // if wrong chip, reboot - volatile unsigned int id = DBGMCU->IDCODE; - if ((id & 0xFFFU) != MCU_IDCODE) { - enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; - } - - // setup interrupt table - SCB->VTOR = (uint32_t)&g_pfnVectors; - - // early GPIOs float everything - early_gpio_float(); - - detect_board_type(); - - if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) { - led_init(); - current_board->init_bootloader(); - led_set(LED_GREEN, 1); - jump_to_bootloader(); - } -} +void early_initialization(void); \ No newline at end of file diff --git a/board/fake_stm.c b/board/fake_stm.c new file mode 100644 index 00000000000..41b38ed3762 --- /dev/null +++ b/board/fake_stm.c @@ -0,0 +1,21 @@ +#include "fake_stm.h" +#include "utils.h" + +void print(const char *a) { + printf("%s", a); +} + +void puth(unsigned int i) { + printf("%u", i); +} + +TIM_TypeDef timer; +TIM_TypeDef *MICROSECOND_TIMER = &timer; + +uint32_t microsecond_timer_get(void) { + return MICROSECOND_TIMER->CNT; +} + + +// NOCHECKIN +void put_char(char c) {}; \ No newline at end of file diff --git a/board/fake_stm.h b/board/fake_stm.h index febf0544b76..9d4d9b0767e 100644 --- a/board/fake_stm.h +++ b/board/fake_stm.h @@ -10,24 +10,16 @@ #define ENTER_CRITICAL() 0 #define EXIT_CRITICAL() 0 -void print(const char *a) { - printf("%s", a); -} - -void puth(unsigned int i) { - printf("%u", i); -} +void print(const char *a); +void puth(unsigned int i); typedef struct { uint32_t CNT; } TIM_TypeDef; -TIM_TypeDef timer; -TIM_TypeDef *MICROSECOND_TIMER = &timer; -uint32_t microsecond_timer_get(void); +extern TIM_TypeDef timer; +extern TIM_TypeDef *MICROSECOND_TIMER; -uint32_t microsecond_timer_get(void) { - return MICROSECOND_TIMER->CNT; -} +uint32_t microsecond_timer_get(void); typedef uint32_t GPIO_TypeDef; diff --git a/board/faults.c b/board/faults.c new file mode 100644 index 00000000000..4a58e5e0e82 --- /dev/null +++ b/board/faults.c @@ -0,0 +1,25 @@ +#include "faults.h" + +uint8_t fault_status = FAULT_STATUS_NONE; +uint32_t faults = 0U; + +void fault_occurred(uint32_t fault) { + if ((faults & fault) == 0U) { + if ((PERMANENT_FAULTS & fault) != 0U) { + print("Permanent fault occurred: 0x"); puth(fault); print("\n"); + fault_status = FAULT_STATUS_PERMANENT; + } else { + print("Temporary fault occurred: 0x"); puth(fault); print("\n"); + fault_status = FAULT_STATUS_TEMPORARY; + } + } + faults |= fault; +} + +void fault_recovered(uint32_t fault) { + if ((PERMANENT_FAULTS & fault) == 0U) { + faults &= ~fault; + } else { + print("Cannot recover from a permanent fault!\n"); + } +} diff --git a/board/faults.h b/board/faults.h index 0fc9d2c5cfb..1c78b8bce46 100644 --- a/board/faults.h +++ b/board/faults.h @@ -1,25 +1,9 @@ -#include "faults_declarations.h" +#include -uint8_t fault_status = FAULT_STATUS_NONE; -uint32_t faults = 0U; +#include "faults_declarations.h" -void fault_occurred(uint32_t fault) { - if ((faults & fault) == 0U) { - if ((PERMANENT_FAULTS & fault) != 0U) { - print("Permanent fault occurred: 0x"); puth(fault); print("\n"); - fault_status = FAULT_STATUS_PERMANENT; - } else { - print("Temporary fault occurred: 0x"); puth(fault); print("\n"); - fault_status = FAULT_STATUS_TEMPORARY; - } - } - faults |= fault; -} +extern uint8_t fault_status; +extern uint32_t faults; -void fault_recovered(uint32_t fault) { - if ((PERMANENT_FAULTS & fault) == 0U) { - faults &= ~fault; - } else { - print("Cannot recover from a permanent fault!\n"); - } -} +void fault_occurred(uint32_t fault); +void fault_recovered(uint32_t fault); \ No newline at end of file diff --git a/board/faults_declarations.h b/board/faults_declarations.h index 981e237566d..84738ddb5e5 100644 --- a/board/faults_declarations.h +++ b/board/faults_declarations.h @@ -1,34 +1 @@ -#pragma once - -#define FAULT_STATUS_NONE 0U -#define FAULT_STATUS_TEMPORARY 1U -#define FAULT_STATUS_PERMANENT 2U - -// Fault types, matches cereal.log.PandaState.FaultType -#define FAULT_RELAY_MALFUNCTION (1UL << 0) -#define FAULT_UNUSED_INTERRUPT_HANDLED (1UL << 1) -#define FAULT_INTERRUPT_RATE_CAN_1 (1UL << 2) -#define FAULT_INTERRUPT_RATE_CAN_2 (1UL << 3) -#define FAULT_INTERRUPT_RATE_CAN_3 (1UL << 4) -#define FAULT_INTERRUPT_RATE_TACH (1UL << 5) -#define FAULT_INTERRUPT_RATE_INTERRUPTS (1UL << 7) -#define FAULT_INTERRUPT_RATE_SPI_DMA (1UL << 8) -#define FAULT_INTERRUPT_RATE_USB (1UL << 15) -#define FAULT_REGISTER_DIVERGENT (1UL << 18) -#define FAULT_INTERRUPT_RATE_CLOCK_SOURCE (1UL << 20) -#define FAULT_INTERRUPT_RATE_TICK (1UL << 21) -#define FAULT_INTERRUPT_RATE_EXTI (1UL << 22) -#define FAULT_INTERRUPT_RATE_SPI (1UL << 23) -#define FAULT_INTERRUPT_RATE_UART_7 (1UL << 24) -#define FAULT_SIREN_MALFUNCTION (1UL << 25) -#define FAULT_HEARTBEAT_LOOP_WATCHDOG (1UL << 26) -#define FAULT_INTERRUPT_RATE_SOUND_DMA (1UL << 27) - -// Permanent faults -#define PERMANENT_FAULTS 0U - -extern uint8_t fault_status; -extern uint32_t faults; - -void fault_occurred(uint32_t fault); -void fault_recovered(uint32_t fault); +#include "board/drivers/registers.h" \ No newline at end of file diff --git a/board/flasher.c b/board/flasher.c new file mode 100644 index 00000000000..cccdaaf06d9 --- /dev/null +++ b/board/flasher.c @@ -0,0 +1,163 @@ +#include "flasher.h" +#include "utils.h" +#include "libc.h" +#include "board/stm32h7/llflash.h" +#include "drivers/led.h" + +#include "globals.h" +#include "provision.h" +#include "early_init.h" +#include "obj/gitversion.h" + +// from the linker script +#define APP_START_ADDRESS 0x8020000U + +// flasher state variables +uint32_t *prog_ptr = NULL; +bool unlocked = false; + +void spi_init(void); + +int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { + int resp_len = 0; + + // flasher machine + memset(resp, 0, 4); + memcpy(resp+4, "\xde\xad\xd0\x0d", 4); + resp[0] = 0xff; + resp[2] = req->request; + resp[3] = ~req->request; + *((uint32_t **)&resp[8]) = prog_ptr; + resp_len = 0xc; + + int sec; + switch (req->request) { + // **** 0xb0: flasher echo + case 0xb0: + resp[1] = 0xff; + break; + // **** 0xb1: unlock flash + case 0xb1: + if (flash_is_locked()) { + flash_unlock(); + resp[1] = 0xff; + } + led_set(LED_GREEN, 1); + unlocked = true; + prog_ptr = (uint32_t *)APP_START_ADDRESS; + break; + // **** 0xb2: erase sector + case 0xb2: + sec = req->param1; + if (flash_erase_sector(sec, unlocked)) { + resp[1] = 0xff; + } + break; + // **** 0xc1: get hardware type + case 0xc1: + resp[0] = hw_type; + resp_len = 1; + break; + // **** 0xc3: fetch MCU UID + case 0xc3: + (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); + resp_len = 12; + break; + // **** 0xd0: fetch serial number + case 0xd0: + // addresses are OTP + if (req->param1 == 1) { + memcpy(resp, (void *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); + resp_len = 0x10; + } else { + get_provision_chunk(resp); + resp_len = PROVISION_CHUNK_LEN; + } + break; + // **** 0xd1: enter bootloader mode + case 0xd1: + // this allows reflashing of the bootstub + switch (req->param1) { + case 0: + print("-> entering bootloader\n"); + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + NVIC_SystemReset(); + break; + case 1: + print("-> entering softloader\n"); + enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; + NVIC_SystemReset(); + break; + } + break; + // **** 0xd6: get version + case 0xd6: + COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); + memcpy(resp, gitversion, sizeof(gitversion)); + resp_len = sizeof(gitversion) - 1U; + break; + // **** 0xd8: reset ST + case 0xd8: + flush_write_buffer(); + NVIC_SystemReset(); + break; + } + return resp_len; +} + +void comms_can_write(const uint8_t *data, uint32_t len) { + UNUSED(data); + UNUSED(len); +} + +int comms_can_read(uint8_t *data, uint32_t max_len) { + UNUSED(data); + UNUSED(max_len); + return 0; +} + +void refresh_can_tx_slots_available(void) {} + +void comms_endpoint2_write(const uint8_t *data, uint32_t len) { + led_set(LED_RED, 0); + for (uint32_t i = 0; i < len/4; i++) { + flash_write_word(prog_ptr, *(uint32_t*)(data+(i*4))); + + //*(uint64_t*)(&spi_tx_buf[0x30+(i*4)]) = *prog_ptr; + prog_ptr++; + } + led_set(LED_RED, 1); +} + + +void soft_flasher_start(void) { + print("\n\n\n************************ FLASHER START ************************\n"); + + enter_bootloader_mode = 0; + + flasher_peripherals_init(); + + gpio_usart2_init(); + gpio_usb_init(); + led_init(); + + // enable comms + usb_init(); + if (current_board->has_spi) { + gpio_spi_init(); + spi_init(); + } + + // green LED on for flashing + led_set(LED_GREEN, 1); + + enable_interrupts(); + + for (;;) { + // blink the green LED fast + led_set(LED_GREEN, 0); + delay(500000); + led_set(LED_GREEN, 1); + delay(500000); + } +} diff --git a/board/flasher.h b/board/flasher.h index f816abec900..754810221b1 100644 --- a/board/flasher.h +++ b/board/flasher.h @@ -1,152 +1,22 @@ +#pragma once + +#include +#include + +#include "comms_definitions.h" + // from the linker script #define APP_START_ADDRESS 0x8020000U // flasher state variables -uint32_t *prog_ptr = NULL; -bool unlocked = false; +extern uint32_t *prog_ptr; +extern bool unlocked; void spi_init(void); -int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { - int resp_len = 0; - - // flasher machine - memset(resp, 0, 4); - memcpy(resp+4, "\xde\xad\xd0\x0d", 4); - resp[0] = 0xff; - resp[2] = req->request; - resp[3] = ~req->request; - *((uint32_t **)&resp[8]) = prog_ptr; - resp_len = 0xc; - - int sec; - switch (req->request) { - // **** 0xb0: flasher echo - case 0xb0: - resp[1] = 0xff; - break; - // **** 0xb1: unlock flash - case 0xb1: - if (flash_is_locked()) { - flash_unlock(); - resp[1] = 0xff; - } - led_set(LED_GREEN, 1); - unlocked = true; - prog_ptr = (uint32_t *)APP_START_ADDRESS; - break; - // **** 0xb2: erase sector - case 0xb2: - sec = req->param1; - if (flash_erase_sector(sec, unlocked)) { - resp[1] = 0xff; - } - break; - // **** 0xc1: get hardware type - case 0xc1: - resp[0] = hw_type; - resp_len = 1; - break; - // **** 0xc3: fetch MCU UID - case 0xc3: - (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); - resp_len = 12; - break; - // **** 0xd0: fetch serial number - case 0xd0: - // addresses are OTP - if (req->param1 == 1) { - memcpy(resp, (void *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); - resp_len = 0x10; - } else { - get_provision_chunk(resp); - resp_len = PROVISION_CHUNK_LEN; - } - break; - // **** 0xd1: enter bootloader mode - case 0xd1: - // this allows reflashing of the bootstub - switch (req->param1) { - case 0: - print("-> entering bootloader\n"); - enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; - NVIC_SystemReset(); - break; - case 1: - print("-> entering softloader\n"); - enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; - NVIC_SystemReset(); - break; - } - break; - // **** 0xd6: get version - case 0xd6: - COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); - memcpy(resp, gitversion, sizeof(gitversion)); - resp_len = sizeof(gitversion) - 1U; - break; - // **** 0xd8: reset ST - case 0xd8: - flush_write_buffer(); - NVIC_SystemReset(); - break; - } - return resp_len; -} - -void comms_can_write(const uint8_t *data, uint32_t len) { - UNUSED(data); - UNUSED(len); -} - -int comms_can_read(uint8_t *data, uint32_t max_len) { - UNUSED(data); - UNUSED(max_len); - return 0; -} - -void refresh_can_tx_slots_available(void) {} - -void comms_endpoint2_write(const uint8_t *data, uint32_t len) { - led_set(LED_RED, 0); - for (uint32_t i = 0; i < len/4; i++) { - flash_write_word(prog_ptr, *(uint32_t*)(data+(i*4))); - - //*(uint64_t*)(&spi_tx_buf[0x30+(i*4)]) = *prog_ptr; - prog_ptr++; - } - led_set(LED_RED, 1); -} - - -void soft_flasher_start(void) { - print("\n\n\n************************ FLASHER START ************************\n"); - - enter_bootloader_mode = 0; - - flasher_peripherals_init(); - - gpio_usart2_init(); - gpio_usb_init(); - led_init(); - - // enable comms - usb_init(); - if (current_board->has_spi) { - gpio_spi_init(); - spi_init(); - } - - // green LED on for flashing - led_set(LED_GREEN, 1); - - enable_interrupts(); - - for (;;) { - // blink the green LED fast - led_set(LED_GREEN, 0); - delay(500000); - led_set(LED_GREEN, 1); - delay(500000); - } -} +int comms_control_handler(ControlPacket_t *req, uint8_t *resp); +void comms_can_write(const uint8_t *data, uint32_t len); +int comms_can_read(uint8_t *data, uint32_t max_len); +void refresh_can_tx_slots_available(void); +void comms_endpoint2_write(const uint8_t *data, uint32_t len); +void soft_flasher_start(void); diff --git a/board/globals copy.h b/board/globals copy.h new file mode 100644 index 00000000000..af9c489aac6 --- /dev/null +++ b/board/globals copy.h @@ -0,0 +1,10 @@ +#pragma once + +#include + +#include "boards/board_declarations.h" + +typedef struct board board; + +extern uint8_t hw_type; +extern board *current_board; diff --git a/board/globals.h b/board/globals.h new file mode 100644 index 00000000000..6d8575c250b --- /dev/null +++ b/board/globals.h @@ -0,0 +1,10 @@ +#pragma once + +#include + +#include "boards/board_declarations.h" + +typedef struct board board; + +extern uint8_t hw_type; +extern board *current_board; \ No newline at end of file diff --git a/board/health.h b/board/health.h index 9040edf5a1b..a7498118c07 100644 --- a/board/health.h +++ b/board/health.h @@ -1,3 +1,5 @@ +#pragma once + // When changing these structs, python/__init__.py needs to be kept up to date! #define HEALTH_PACKET_VERSION 17 diff --git a/board/libc.c b/board/libc.c new file mode 100644 index 00000000000..2387e7106ff --- /dev/null +++ b/board/libc.c @@ -0,0 +1,82 @@ +#include "libc.h" + +// **** libc **** + +void print(const char *a); +void puth(unsigned int i); + +void delay(uint32_t a) { + volatile uint32_t i; + for (i = 0; i < a; i++); +} + +void assert_fatal(bool condition, const char *msg) { + if (!condition) { + print("ASSERT FAILED\n"); + print(msg); + while (1) { + // hang + } + } +} + +// cppcheck-suppress misra-c2012-21.2 +void *memset(void *str, int c, unsigned int n) { + uint8_t *s = str; + for (unsigned int i = 0; i < n; i++) { + *s = c; + s++; + } + return str; +} + +#define UNALIGNED(X, Y) \ + (((uint32_t)(X) & (sizeof(uint32_t) - 1U)) | ((uint32_t)(Y) & (sizeof(uint32_t) - 1U))) + +// cppcheck-suppress misra-c2012-21.2 +void *memcpy(void *dest, const void *src, unsigned int len) { + unsigned int n = len; + uint8_t *d8 = dest; + const uint8_t *s8 = src; + + if ((n >= 4U) && !UNALIGNED(s8, d8)) { + uint32_t *d32 = (uint32_t *)d8; // cppcheck-suppress misra-c2012-11.3 ; already checked that it's properly aligned + const uint32_t *s32 = (const uint32_t *)s8; // cppcheck-suppress misra-c2012-11.3 ; already checked that it's properly aligned + + while(n >= 16U) { + *d32 = *s32; d32++; s32++; + *d32 = *s32; d32++; s32++; + *d32 = *s32; d32++; s32++; + *d32 = *s32; d32++; s32++; + n -= 16U; + } + + while(n >= 4U) { + *d32 = *s32; d32++; s32++; + n -= 4U; + } + + d8 = (uint8_t *)d32; + s8 = (const uint8_t *)s32; + } + while (n-- > 0U) { + *d8 = *s8; d8++; s8++; + } + return dest; +} + +// cppcheck-suppress misra-c2012-21.2 +int memcmp(const void * ptr1, const void * ptr2, unsigned int num) { + int ret = 0; + const uint8_t *p1 = ptr1; + const uint8_t *p2 = ptr2; + for (unsigned int i = 0; i < num; i++) { + if (*p1 != *p2) { + ret = -1; + break; + } + p1++; + p2++; + } + return ret; +} diff --git a/board/libc.h b/board/libc.h index c5f02193509..7bc3eb3db28 100644 --- a/board/libc.h +++ b/board/libc.h @@ -1,77 +1,17 @@ -// **** libc **** - -void delay(uint32_t a) { - volatile uint32_t i; - for (i = 0; i < a; i++); -} +#pragma once -void assert_fatal(bool condition, const char *msg) { - if (!condition) { - print("ASSERT FAILED\n"); - print(msg); - while (1) { - // hang - } - } -} +#include +#include -// cppcheck-suppress misra-c2012-21.2 -void *memset(void *str, int c, unsigned int n) { - uint8_t *s = str; - for (unsigned int i = 0; i < n; i++) { - *s = c; - s++; - } - return str; -} +// **** libc **** #define UNALIGNED(X, Y) \ (((uint32_t)(X) & (sizeof(uint32_t) - 1U)) | ((uint32_t)(Y) & (sizeof(uint32_t) - 1U))) -// cppcheck-suppress misra-c2012-21.2 -void *memcpy(void *dest, const void *src, unsigned int len) { - unsigned int n = len; - uint8_t *d8 = dest; - const uint8_t *s8 = src; - - if ((n >= 4U) && !UNALIGNED(s8, d8)) { - uint32_t *d32 = (uint32_t *)d8; // cppcheck-suppress misra-c2012-11.3 ; already checked that it's properly aligned - const uint32_t *s32 = (const uint32_t *)s8; // cppcheck-suppress misra-c2012-11.3 ; already checked that it's properly aligned - - while(n >= 16U) { - *d32 = *s32; d32++; s32++; - *d32 = *s32; d32++; s32++; - *d32 = *s32; d32++; s32++; - *d32 = *s32; d32++; s32++; - n -= 16U; - } - - while(n >= 4U) { - *d32 = *s32; d32++; s32++; - n -= 4U; - } - - d8 = (uint8_t *)d32; - s8 = (const uint8_t *)s32; - } - while (n-- > 0U) { - *d8 = *s8; d8++; s8++; - } - return dest; -} - -// cppcheck-suppress misra-c2012-21.2 -int memcmp(const void * ptr1, const void * ptr2, unsigned int num) { - int ret = 0; - const uint8_t *p1 = ptr1; - const uint8_t *p2 = ptr2; - for (unsigned int i = 0; i < num; i++) { - if (*p1 != *p2) { - ret = -1; - break; - } - p1++; - p2++; - } - return ret; -} +void delay(uint32_t a); +void assert_fatal(bool condition, const char *msg); +void *memset(void *str, int c, unsigned int n); +void *memcpy(void *dest, const void *src, unsigned int len); +int memcmp(const void * ptr1, const void * ptr2, unsigned int num); +void print(const char *a); +void puth(unsigned int i); \ No newline at end of file diff --git a/board/main.c b/board/main.c index ac0fda9ba43..9f183e56bb2 100644 --- a/board/main.c +++ b/board/main.c @@ -10,6 +10,8 @@ #include "board/early_init.h" #include "board/provision.h" +#include "libc.h" + #include "opendbc/safety/safety.h" #include "board/health.h" @@ -24,7 +26,17 @@ #include "board/can_comms.h" #include "board/main_comms.h" - +#include "board/main_declarations.h" + +uint8_t hw_type; +board *current_board; +struct harness_t harness; +// TODO +uint32_t heartbeat_counter; +bool heartbeat_lost; +bool heartbeat_disabled; +bool siren_enabled; +uint32_t uptime_cnt; // ********************* Serial debugging ********************* diff --git a/board/main_comms.c b/board/main_comms.c new file mode 100644 index 00000000000..4646608f7ca --- /dev/null +++ b/board/main_comms.c @@ -0,0 +1,345 @@ +#include +#include + +#include "board/utils.h" +#include "board/config.h" +#include "board/health.h" +#include "board/main_declarations.h" +#include "board/drivers/can_common.h" +#include "board/power_saving.h" +#include "board/drivers/spi.h" +#include "board/drivers/bootkick.h" +#include "board/drivers/fdcan.h" +#include "board/provision.h" +#include "board/early_init.h" +#include "board/obj/gitversion.h" + +extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used + +// Prototypes +void set_safety_mode(uint16_t mode, uint16_t param); +bool is_car_safety_mode(uint16_t mode); + +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; + + health->uptime_pkt = uptime_cnt; + health->voltage_pkt = current_board->read_voltage_mV(); + health->current_pkt = current_board->read_current_mA(); + + health->ignition_line_pkt = (uint8_t)(harness_check_ignition()); + health->ignition_can_pkt = ignition_can; + + health->controls_allowed_pkt = controls_allowed; + health->safety_tx_blocked_pkt = safety_tx_blocked; + health->safety_rx_invalid_pkt = safety_rx_invalid; + health->tx_buffer_overflow_pkt = tx_buffer_overflow; + health->rx_buffer_overflow_pkt = rx_buffer_overflow; + health->car_harness_status_pkt = harness.status; + health->safety_mode_pkt = (uint8_t)(current_safety_mode); + health->safety_param_pkt = current_safety_param; + health->alternative_experience_pkt = alternative_experience; + health->power_save_enabled_pkt = power_save_status == POWER_SAVE_STATUS_ENABLED; + health->heartbeat_lost_pkt = heartbeat_lost; + health->safety_rx_checks_invalid_pkt = safety_rx_checks_invalid; + + health->spi_error_count_pkt = spi_error_count; + + health->fault_status_pkt = fault_status; + health->faults_pkt = faults; + + health->interrupt_load_pkt = interrupt_load; + + health->fan_power = fan_state.power; + + health->sbu1_voltage_mV = harness.sbu1_voltage_mV; + health->sbu2_voltage_mV = harness.sbu2_voltage_mV; + + health->som_reset_triggered = bootkick_reset_triggered; + + return sizeof(*health); +} + +// send on serial, first byte to select the ring +void comms_endpoint2_write(const uint8_t *data, uint32_t len) { + uart_ring *ur = get_ring_by_number(data[0]); + if ((len != 0U) && (ur != NULL)) { + if ((data[0] < 2U) || (data[0] >= 4U)) { + for (uint32_t i = 1; i < len; i++) { + while (!put_char(ur, data[i])) { + // wait + } + } + } + } +} + +int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { + unsigned int resp_len = 0; + uart_ring *ur = NULL; + uint32_t time; + +#ifdef DEBUG_COMMS + print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n"); + print("- request "); puth(req->request); print("\n"); + print("- param1 "); puth(req->param1); print("\n"); + print("- param2 "); puth(req->param2); print("\n"); +#endif + + switch (req->request) { + // **** 0xa8: get microsecond timer + case 0xa8: + time = microsecond_timer_get(); + resp[0] = (time & 0x000000FFU); + resp[1] = ((time & 0x0000FF00U) >> 8U); + resp[2] = ((time & 0x00FF0000U) >> 16U); + resp[3] = ((time & 0xFF000000U) >> 24U); + resp_len = 4U; + break; + // **** 0xb0: set IR power + case 0xb0: + current_board->set_ir_power(req->param1); + break; + // **** 0xb1: set fan power + case 0xb1: + fan_set_power(req->param1); + break; + // **** 0xb2: get fan rpm + case 0xb2: + resp[0] = (fan_state.rpm & 0x00FFU); + resp[1] = ((fan_state.rpm & 0xFF00U) >> 8U); + resp_len = 2; + break; + // **** 0xc0: reset communications state + case 0xc0: + comms_can_reset(); + break; + // **** 0xc1: get hardware type + case 0xc1: + resp[0] = hw_type; + resp_len = 1; + break; + // **** 0xc2: CAN health stats + case 0xc2: + COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); + if (req->param1 < 3U) { + update_can_health_pkt(req->param1, 0U); + can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); + can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); + can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; + can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; + can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; + resp_len = sizeof(can_health[req->param1]); + (void)memcpy(resp, (uint8_t*)(&can_health[req->param1]), resp_len); + } + break; + // **** 0xc3: fetch MCU UID + case 0xc3: + (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); + resp_len = 12; + break; + // **** 0xc4: get interrupt call rate + case 0xc4: + if (req->param1 < NUM_INTERRUPTS) { + uint32_t load = interrupts[req->param1].call_rate; + resp[0] = (load & 0x000000FFU); + resp[1] = ((load & 0x0000FF00U) >> 8U); + resp[2] = ((load & 0x00FF0000U) >> 16U); + resp[3] = ((load & 0xFF000000U) >> 24U); + resp_len = 4U; + } + break; + // **** 0xc5: DEBUG: drive relay + case 0xc5: + set_intercept_relay((req->param1 & 0x1U), (req->param1 & 0x2U)); + break; + // **** 0xc6: DEBUG: read SOM GPIO + case 0xc6: + resp[0] = current_board->read_som_gpio(); + resp_len = 1; + break; + // **** 0xd0: fetch serial (aka the provisioned dongle ID) + case 0xd0: + // addresses are OTP + if (req->param1 == 1U) { + (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); + resp_len = 0x10; + } else { + get_provision_chunk(resp); + resp_len = PROVISION_CHUNK_LEN; + } + break; + // **** 0xd1: enter bootloader mode + case 0xd1: + // this allows reflashing of the bootstub + switch (req->param1) { + case 0: + // only allow bootloader entry on debug builds + #ifdef ALLOW_DEBUG + print("-> entering bootloader\n"); + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + NVIC_SystemReset(); + #endif + break; + case 1: + print("-> entering softloader\n"); + enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; + NVIC_SystemReset(); + break; + default: + print("Bootloader mode invalid\n"); + break; + } + break; + // **** 0xd2: get health packet + case 0xd2: + resp_len = get_health_pkt(resp); + break; + // **** 0xd3: get first 64 bytes of signature + case 0xd3: + { + resp_len = 64; + char * code = (char*)_app_start; + int code_len = _app_start[0]; + (void)memcpy(resp, &code[code_len], resp_len); + } + break; + // **** 0xd4: get second 64 bytes of signature + case 0xd4: + { + resp_len = 64; + char * code = (char*)_app_start; + int code_len = _app_start[0]; + (void)memcpy(resp, &code[code_len + 64], resp_len); + } + break; + // **** 0xd6: get version + case 0xd6: + COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); + (void)memcpy(resp, gitversion, sizeof(gitversion)); + resp_len = sizeof(gitversion) - 1U; + break; + // **** 0xd8: reset ST + case 0xd8: + NVIC_SystemReset(); + break; + // **** 0xdb: set OBD CAN multiplexing mode + case 0xdb: + current_board->set_can_mode((req->param1 == 1U) ? CAN_MODE_OBD_CAN2 : CAN_MODE_NORMAL); + break; + // **** 0xdc: set safety mode + case 0xdc: + set_safety_mode(req->param1, (uint16_t)req->param2); + break; + // **** 0xdd: get healthpacket and CANPacket versions + case 0xdd: + resp[0] = HEALTH_PACKET_VERSION; + resp[1] = CAN_PACKET_VERSION; + resp[2] = CAN_HEALTH_PACKET_VERSION; + resp_len = 3; + break; + // **** 0xde: set can bitrate + case 0xde: + if ((req->param1 < PANDA_CAN_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { + bus_config[req->param1].can_speed = req->param2; + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + // **** 0xdf: set alternative experience + case 0xdf: + // you can only set this if you are in a non car safety mode + if (!is_car_safety_mode(current_safety_mode)) { + alternative_experience = req->param1; + } + break; + // **** 0xe0: uart read + case 0xe0: + ur = get_ring_by_number(req->param1); + if (!ur) { + break; + } + + // read + uint16_t req_length = MIN(req->length, USBPACKET_MAX_SIZE); + while ((resp_len < req_length) && + get_char(ur, (char*)&resp[resp_len])) { + ++resp_len; + } + break; + // **** 0xe5: set CAN loopback (for testing) + case 0xe5: + can_loopback = req->param1 > 0U; + can_init_all(); + break; + // **** 0xe6: set custom clock source period and pulse length + case 0xe6: + clock_source_set_timer_params(req->param1, req->param2); + break; + // **** 0xe7: set power save state + case 0xe7: + set_power_save_state(req->param1); + break; + // **** 0xe8: set can-fd auto swithing mode + case 0xe8: + bus_config[req->param1].canfd_auto = req->param2 > 0U; + break; + // **** 0xf1: Clear CAN ring buffer. + case 0xf1: + if (req->param1 == 0xFFFFU) { + print("Clearing CAN Rx queue\n"); + can_clear(&can_rx_q); + } else if (req->param1 < PANDA_CAN_CNT) { + print("Clearing CAN Tx queue\n"); + can_clear(can_queues[req->param1]); + } else { + print("Clearing CAN CAN ring buffer failed: wrong bus number\n"); + } + break; + // **** 0xf3: Heartbeat. Resets heartbeat counter. + case 0xf3: + { + heartbeat_counter = 0U; + heartbeat_lost = false; + heartbeat_disabled = false; + heartbeat_engaged = (req->param1 == 1U); + break; + } + // **** 0xf6: set siren enabled + case 0xf6: + siren_enabled = (req->param1 != 0U); + break; + // **** 0xf8: disable heartbeat checks + case 0xf8: + if (!is_car_safety_mode(current_safety_mode)) { + heartbeat_disabled = true; + } + break; + // **** 0xf9: set CAN FD data bitrate + case 0xf9: + if ((req->param1 < PANDA_CAN_CNT) && + is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { + bus_config[req->param1].can_data_speed = req->param2; + bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); + bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + // **** 0xfc: set CAN FD non-ISO mode + case 0xfc: + if (req->param1 < PANDA_CAN_CNT) { + bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + default: + print("NO HANDLER "); + puth(req->request); + print("\n"); + break; + } + return resp_len; +} diff --git a/board/main_comms.h b/board/main_comms.h index ce61f625dd8..5b8cc49e017 100644 --- a/board/main_comms.h +++ b/board/main_comms.h @@ -1,329 +1,13 @@ +#pragma once + +#include "comms_definitions.h" + extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used // Prototypes void set_safety_mode(uint16_t mode, uint16_t param); bool is_car_safety_mode(uint16_t mode); -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; - - health->uptime_pkt = uptime_cnt; - health->voltage_pkt = current_board->read_voltage_mV(); - health->current_pkt = current_board->read_current_mA(); - - health->ignition_line_pkt = (uint8_t)(harness_check_ignition()); - health->ignition_can_pkt = ignition_can; - - health->controls_allowed_pkt = controls_allowed; - health->safety_tx_blocked_pkt = safety_tx_blocked; - health->safety_rx_invalid_pkt = safety_rx_invalid; - health->tx_buffer_overflow_pkt = tx_buffer_overflow; - health->rx_buffer_overflow_pkt = rx_buffer_overflow; - health->car_harness_status_pkt = harness.status; - health->safety_mode_pkt = (uint8_t)(current_safety_mode); - health->safety_param_pkt = current_safety_param; - health->alternative_experience_pkt = alternative_experience; - health->power_save_enabled_pkt = power_save_status == POWER_SAVE_STATUS_ENABLED; - health->heartbeat_lost_pkt = heartbeat_lost; - health->safety_rx_checks_invalid_pkt = safety_rx_checks_invalid; - - health->spi_error_count_pkt = spi_error_count; - - health->fault_status_pkt = fault_status; - health->faults_pkt = faults; - - health->interrupt_load_pkt = interrupt_load; - - health->fan_power = fan_state.power; - - health->sbu1_voltage_mV = harness.sbu1_voltage_mV; - health->sbu2_voltage_mV = harness.sbu2_voltage_mV; - - health->som_reset_triggered = bootkick_reset_triggered; - - return sizeof(*health); -} - // send on serial, first byte to select the ring -void comms_endpoint2_write(const uint8_t *data, uint32_t len) { - uart_ring *ur = get_ring_by_number(data[0]); - if ((len != 0U) && (ur != NULL)) { - if ((data[0] < 2U) || (data[0] >= 4U)) { - for (uint32_t i = 1; i < len; i++) { - while (!put_char(ur, data[i])) { - // wait - } - } - } - } -} - -int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { - unsigned int resp_len = 0; - uart_ring *ur = NULL; - uint32_t time; - -#ifdef DEBUG_COMMS - print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n"); - print("- request "); puth(req->request); print("\n"); - print("- param1 "); puth(req->param1); print("\n"); - print("- param2 "); puth(req->param2); print("\n"); -#endif - - switch (req->request) { - // **** 0xa8: get microsecond timer - case 0xa8: - time = microsecond_timer_get(); - resp[0] = (time & 0x000000FFU); - resp[1] = ((time & 0x0000FF00U) >> 8U); - resp[2] = ((time & 0x00FF0000U) >> 16U); - resp[3] = ((time & 0xFF000000U) >> 24U); - resp_len = 4U; - break; - // **** 0xb0: set IR power - case 0xb0: - current_board->set_ir_power(req->param1); - break; - // **** 0xb1: set fan power - case 0xb1: - fan_set_power(req->param1); - break; - // **** 0xb2: get fan rpm - case 0xb2: - resp[0] = (fan_state.rpm & 0x00FFU); - resp[1] = ((fan_state.rpm & 0xFF00U) >> 8U); - resp_len = 2; - break; - // **** 0xc0: reset communications state - case 0xc0: - comms_can_reset(); - break; - // **** 0xc1: get hardware type - case 0xc1: - resp[0] = hw_type; - resp_len = 1; - break; - // **** 0xc2: CAN health stats - case 0xc2: - COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); - if (req->param1 < 3U) { - update_can_health_pkt(req->param1, 0U); - can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); - can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); - can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; - can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; - can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; - resp_len = sizeof(can_health[req->param1]); - (void)memcpy(resp, (uint8_t*)(&can_health[req->param1]), resp_len); - } - break; - // **** 0xc3: fetch MCU UID - case 0xc3: - (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); - resp_len = 12; - break; - // **** 0xc4: get interrupt call rate - case 0xc4: - if (req->param1 < NUM_INTERRUPTS) { - uint32_t load = interrupts[req->param1].call_rate; - resp[0] = (load & 0x000000FFU); - resp[1] = ((load & 0x0000FF00U) >> 8U); - resp[2] = ((load & 0x00FF0000U) >> 16U); - resp[3] = ((load & 0xFF000000U) >> 24U); - resp_len = 4U; - } - break; - // **** 0xc5: DEBUG: drive relay - case 0xc5: - set_intercept_relay((req->param1 & 0x1U), (req->param1 & 0x2U)); - break; - // **** 0xc6: DEBUG: read SOM GPIO - case 0xc6: - resp[0] = current_board->read_som_gpio(); - resp_len = 1; - break; - // **** 0xd0: fetch serial (aka the provisioned dongle ID) - case 0xd0: - // addresses are OTP - if (req->param1 == 1U) { - (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); - resp_len = 0x10; - } else { - get_provision_chunk(resp); - resp_len = PROVISION_CHUNK_LEN; - } - break; - // **** 0xd1: enter bootloader mode - case 0xd1: - // this allows reflashing of the bootstub - switch (req->param1) { - case 0: - // only allow bootloader entry on debug builds - #ifdef ALLOW_DEBUG - print("-> entering bootloader\n"); - enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; - NVIC_SystemReset(); - #endif - break; - case 1: - print("-> entering softloader\n"); - enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; - NVIC_SystemReset(); - break; - default: - print("Bootloader mode invalid\n"); - break; - } - break; - // **** 0xd2: get health packet - case 0xd2: - resp_len = get_health_pkt(resp); - break; - // **** 0xd3: get first 64 bytes of signature - case 0xd3: - { - resp_len = 64; - char * code = (char*)_app_start; - int code_len = _app_start[0]; - (void)memcpy(resp, &code[code_len], resp_len); - } - break; - // **** 0xd4: get second 64 bytes of signature - case 0xd4: - { - resp_len = 64; - char * code = (char*)_app_start; - int code_len = _app_start[0]; - (void)memcpy(resp, &code[code_len + 64], resp_len); - } - break; - // **** 0xd6: get version - case 0xd6: - COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); - (void)memcpy(resp, gitversion, sizeof(gitversion)); - resp_len = sizeof(gitversion) - 1U; - break; - // **** 0xd8: reset ST - case 0xd8: - NVIC_SystemReset(); - break; - // **** 0xdb: set OBD CAN multiplexing mode - case 0xdb: - current_board->set_can_mode((req->param1 == 1U) ? CAN_MODE_OBD_CAN2 : CAN_MODE_NORMAL); - break; - // **** 0xdc: set safety mode - case 0xdc: - set_safety_mode(req->param1, (uint16_t)req->param2); - break; - // **** 0xdd: get healthpacket and CANPacket versions - case 0xdd: - resp[0] = HEALTH_PACKET_VERSION; - resp[1] = CAN_PACKET_VERSION; - resp[2] = CAN_HEALTH_PACKET_VERSION; - resp_len = 3; - break; - // **** 0xde: set can bitrate - case 0xde: - if ((req->param1 < PANDA_CAN_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { - bus_config[req->param1].can_speed = req->param2; - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - // **** 0xdf: set alternative experience - case 0xdf: - // you can only set this if you are in a non car safety mode - if (!is_car_safety_mode(current_safety_mode)) { - alternative_experience = req->param1; - } - break; - // **** 0xe0: uart read - case 0xe0: - ur = get_ring_by_number(req->param1); - if (!ur) { - break; - } - - // read - uint16_t req_length = MIN(req->length, USBPACKET_MAX_SIZE); - while ((resp_len < req_length) && - get_char(ur, (char*)&resp[resp_len])) { - ++resp_len; - } - break; - // **** 0xe5: set CAN loopback (for testing) - case 0xe5: - can_loopback = req->param1 > 0U; - can_init_all(); - break; - // **** 0xe6: set custom clock source period and pulse length - case 0xe6: - clock_source_set_timer_params(req->param1, req->param2); - break; - // **** 0xe7: set power save state - case 0xe7: - set_power_save_state(req->param1); - break; - // **** 0xe8: set can-fd auto swithing mode - case 0xe8: - bus_config[req->param1].canfd_auto = req->param2 > 0U; - break; - // **** 0xf1: Clear CAN ring buffer. - case 0xf1: - if (req->param1 == 0xFFFFU) { - print("Clearing CAN Rx queue\n"); - can_clear(&can_rx_q); - } else if (req->param1 < PANDA_CAN_CNT) { - print("Clearing CAN Tx queue\n"); - can_clear(can_queues[req->param1]); - } else { - print("Clearing CAN CAN ring buffer failed: wrong bus number\n"); - } - break; - // **** 0xf3: Heartbeat. Resets heartbeat counter. - case 0xf3: - { - heartbeat_counter = 0U; - heartbeat_lost = false; - heartbeat_disabled = false; - heartbeat_engaged = (req->param1 == 1U); - break; - } - // **** 0xf6: set siren enabled - case 0xf6: - siren_enabled = (req->param1 != 0U); - break; - // **** 0xf8: disable heartbeat checks - case 0xf8: - if (!is_car_safety_mode(current_safety_mode)) { - heartbeat_disabled = true; - } - break; - // **** 0xf9: set CAN FD data bitrate - case 0xf9: - if ((req->param1 < PANDA_CAN_CNT) && - is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { - bus_config[req->param1].can_data_speed = req->param2; - bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); - bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - // **** 0xfc: set CAN FD non-ISO mode - case 0xfc: - if (req->param1 < PANDA_CAN_CNT) { - bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - default: - print("NO HANDLER "); - puth(req->request); - print("\n"); - break; - } - return resp_len; -} +void comms_endpoint2_write(const uint8_t *data, uint32_t len); +int comms_control_handler(ControlPacket_t *req, uint8_t *resp); \ No newline at end of file diff --git a/board/main_declarations.h b/board/main_declarations.h index 52aaa6c10d9..dcc7932be26 100644 --- a/board/main_declarations.h +++ b/board/main_declarations.h @@ -1,16 +1,19 @@ #pragma once +#include +#include + +#include "boards/board_declarations.h" + // ******************** Prototypes ******************** -void print(const char *a); -void puth(unsigned int i); -typedef struct board board; -typedef struct harness_configuration harness_configuration; + + void pwm_init(TIM_TypeDef *TIM, uint8_t channel); void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage); // ********************* Globals ********************** -extern uint8_t hw_type; -extern board *current_board; +#include "globals.h" + extern uint32_t uptime_cnt; // heartbeat state diff --git a/board/main_definitions.h b/board/main_definitions.c similarity index 100% rename from board/main_definitions.h rename to board/main_definitions.c diff --git a/board/power_saving.c b/board/power_saving.c new file mode 100644 index 00000000000..a088e2fb3b3 --- /dev/null +++ b/board/power_saving.c @@ -0,0 +1,60 @@ +#include + +#include "power_saving.h" +#include "main_declarations.h" +#include "config.h" +#include "board/drivers/fdcan.h" +// WARNING: To stay in compliance with the SIL2 rules laid out in STM UM1840, we should never implement any of the available hardware low power modes. +// See rule: CoU_3 + +// FIXME! +void llcan_irq_disable(const FDCAN_GlobalTypeDef *FDCANx); +void llcan_irq_enable(const FDCAN_GlobalTypeDef *FDCANx); + +int power_save_status = POWER_SAVE_STATUS_DISABLED; + +void enable_can_transceivers(bool enabled) { + // Leave main CAN always on for CAN-based ignition detection + uint8_t main_bus = (harness.status == HARNESS_STATUS_FLIPPED) ? 3U : 1U; + for(uint8_t i=1U; i<=4U; i++){ + current_board->enable_can_transceiver(i, (i == main_bus) || enabled); + } +} + +void set_power_save_state(int state) { + bool is_valid_state = (state == POWER_SAVE_STATUS_ENABLED) || (state == POWER_SAVE_STATUS_DISABLED); + if (is_valid_state && (state != power_save_status)) { + bool enable = false; + if (state == POWER_SAVE_STATUS_ENABLED) { + print("enable power savings\n"); + + // Disable CAN interrupts + if (harness.status == HARNESS_STATUS_FLIPPED) { + llcan_irq_disable(cans[0]); + } else { + llcan_irq_disable(cans[2]); + } + llcan_irq_disable(cans[1]); + } else { + print("disable power savings\n"); + + if (harness.status == HARNESS_STATUS_FLIPPED) { + llcan_irq_enable(cans[0]); + } else { + llcan_irq_enable(cans[2]); + } + llcan_irq_enable(cans[1]); + + enable = true; + } + + enable_can_transceivers(enable); + + // Switch off IR when in power saving + if(!enable){ + current_board->set_ir_power(0U); + } + + power_save_status = state; + } +} diff --git a/board/power_saving.h b/board/power_saving.h index 1e95c951399..1d226798efc 100644 --- a/board/power_saving.h +++ b/board/power_saving.h @@ -1,52 +1,14 @@ -#include "power_saving_declarations.h" +#pragma once + +#include // WARNING: To stay in compliance with the SIL2 rules laid out in STM UM1840, we should never implement any of the available hardware low power modes. // See rule: CoU_3 -int power_save_status = POWER_SAVE_STATUS_DISABLED; - -void enable_can_transceivers(bool enabled) { - // Leave main CAN always on for CAN-based ignition detection - uint8_t main_bus = (harness.status == HARNESS_STATUS_FLIPPED) ? 3U : 1U; - for(uint8_t i=1U; i<=4U; i++){ - current_board->enable_can_transceiver(i, (i == main_bus) || enabled); - } -} - -void set_power_save_state(int state) { - bool is_valid_state = (state == POWER_SAVE_STATUS_ENABLED) || (state == POWER_SAVE_STATUS_DISABLED); - if (is_valid_state && (state != power_save_status)) { - bool enable = false; - if (state == POWER_SAVE_STATUS_ENABLED) { - print("enable power savings\n"); - - // Disable CAN interrupts - if (harness.status == HARNESS_STATUS_FLIPPED) { - llcan_irq_disable(cans[0]); - } else { - llcan_irq_disable(cans[2]); - } - llcan_irq_disable(cans[1]); - } else { - print("disable power savings\n"); - - if (harness.status == HARNESS_STATUS_FLIPPED) { - llcan_irq_enable(cans[0]); - } else { - llcan_irq_enable(cans[2]); - } - llcan_irq_enable(cans[1]); - - enable = true; - } - - enable_can_transceivers(enable); +#define POWER_SAVE_STATUS_DISABLED 0 +#define POWER_SAVE_STATUS_ENABLED 1 - // Switch off IR when in power saving - if(!enable){ - current_board->set_ir_power(0U); - } +extern int power_save_status; - power_save_status = state; - } -} +void set_power_save_state(int state); +void enable_can_transceivers(bool enabled); \ No newline at end of file diff --git a/board/power_saving_declarations.h b/board/power_saving_declarations.h deleted file mode 100644 index 7a474a55949..00000000000 --- a/board/power_saving_declarations.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -// WARNING: To stay in compliance with the SIL2 rules laid out in STM UM1840, we should never implement any of the available hardware low power modes. -// See rule: CoU_3 - -#define POWER_SAVE_STATUS_DISABLED 0 -#define POWER_SAVE_STATUS_ENABLED 1 - -extern int power_save_status; - -void set_power_save_state(int state); diff --git a/board/provision.c b/board/provision.c new file mode 100644 index 00000000000..bcf5c41ff0f --- /dev/null +++ b/board/provision.c @@ -0,0 +1,16 @@ +// this is where we manage the dongle ID assigned during our +// manufacturing. aside from this, there's a UID for the MCU + +#include "provision.h" +#include "board/stm32h7/stm32h7_config.h" + +#define PROVISION_CHUNK_LEN 0x20 + +void get_provision_chunk(uint8_t *resp) { + const unsigned char unprovisioned_text[] = "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff"; + + (void)memcpy(resp, (uint8_t *)PROVISION_CHUNK_ADDRESS, PROVISION_CHUNK_LEN); + if (memcmp(resp, unprovisioned_text, 0x20) == 0) { + (void)memcpy(resp, "unprovisioned\x00\x00\x00testing123\x00\x00\xa3\xa6\x99\xec", 0x20); + } +} diff --git a/board/provision.h b/board/provision.h index d191e53f677..5c217a3de59 100644 --- a/board/provision.h +++ b/board/provision.h @@ -1,13 +1,8 @@ // this is where we manage the dongle ID assigned during our // manufacturing. aside from this, there's a UID for the MCU -#define PROVISION_CHUNK_LEN 0x20 +#include -void get_provision_chunk(uint8_t *resp) { - const unsigned char unprovisioned_text[] = "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff"; +#define PROVISION_CHUNK_LEN 0x20 - (void)memcpy(resp, (uint8_t *)PROVISION_CHUNK_ADDRESS, PROVISION_CHUNK_LEN); - if (memcmp(resp, unprovisioned_text, 0x20) == 0) { - (void)memcpy(resp, "unprovisioned\x00\x00\x00testing123\x00\x00\xa3\xa6\x99\xec", 0x20); - } -} +void get_provision_chunk(uint8_t *resp); \ No newline at end of file diff --git a/board/stm32h7/board.c b/board/stm32h7/board.c new file mode 100644 index 00000000000..4faae4151b7 --- /dev/null +++ b/board/stm32h7/board.c @@ -0,0 +1,34 @@ +#include "board/stm32h7/board.h" + +#include "board/globals.h" + +void detect_board_type(void) { + // On STM32H7 pandas, we use two different sets of pins. + const uint8_t id1 = detect_with_pull(GPIOF, 7, PULL_UP) | + (detect_with_pull(GPIOF, 8, PULL_UP) << 1U) | + (detect_with_pull(GPIOF, 9, PULL_UP) << 2U) | + (detect_with_pull(GPIOF, 10, PULL_UP) << 3U); + + const uint8_t id2 = detect_with_pull(GPIOD, 4, PULL_UP) | + (detect_with_pull(GPIOD, 5, PULL_UP) << 1U) | + (detect_with_pull(GPIOD, 6, PULL_UP) << 2U) | + (detect_with_pull(GPIOD, 7, PULL_UP) << 3U); + + if (id2 == 3U) { + hw_type = HW_TYPE_CUATRO; + current_board = &board_cuatro; + } else if (id1 == 0U) { + hw_type = HW_TYPE_RED_PANDA; + current_board = &board_red; + } else if (id1 == 1U) { + // deprecated + //hw_type = HW_TYPE_RED_PANDA_V2; + hw_type = HW_TYPE_UNKNOWN; + } else if (id1 == 2U) { + hw_type = HW_TYPE_TRES; + current_board = &board_tres; + } else { + hw_type = HW_TYPE_UNKNOWN; + print("Hardware type is UNKNOWN!\n"); + } +} diff --git a/board/stm32h7/board.h b/board/stm32h7/board.h index 05c5c5e7155..43d73610547 100644 --- a/board/stm32h7/board.h +++ b/board/stm32h7/board.h @@ -16,34 +16,4 @@ #include "board/boards/tres.h" #include "board/boards/cuatro.h" - -void detect_board_type(void) { - // On STM32H7 pandas, we use two different sets of pins. - const uint8_t id1 = detect_with_pull(GPIOF, 7, PULL_UP) | - (detect_with_pull(GPIOF, 8, PULL_UP) << 1U) | - (detect_with_pull(GPIOF, 9, PULL_UP) << 2U) | - (detect_with_pull(GPIOF, 10, PULL_UP) << 3U); - - const uint8_t id2 = detect_with_pull(GPIOD, 4, PULL_UP) | - (detect_with_pull(GPIOD, 5, PULL_UP) << 1U) | - (detect_with_pull(GPIOD, 6, PULL_UP) << 2U) | - (detect_with_pull(GPIOD, 7, PULL_UP) << 3U); - - if (id2 == 3U) { - hw_type = HW_TYPE_CUATRO; - current_board = &board_cuatro; - } else if (id1 == 0U) { - hw_type = HW_TYPE_RED_PANDA; - current_board = &board_red; - } else if (id1 == 1U) { - // deprecated - //hw_type = HW_TYPE_RED_PANDA_V2; - hw_type = HW_TYPE_UNKNOWN; - } else if (id1 == 2U) { - hw_type = HW_TYPE_TRES; - current_board = &board_tres; - } else { - hw_type = HW_TYPE_UNKNOWN; - print("Hardware type is UNKNOWN!\n"); - } -} +void detect_board_type(void); \ No newline at end of file diff --git a/board/stm32h7/clock.c b/board/stm32h7/clock.c new file mode 100644 index 00000000000..5b5e3843d5c --- /dev/null +++ b/board/stm32h7/clock.c @@ -0,0 +1,119 @@ +#include "clock.h" + +#include "stm32h7xx.h" +#include "board/drivers/registers.h" + +/* +HSE: 25MHz +PLL1Q: 80MHz (for FDCAN) +HSI48 enabled (for USB) +CPU: 240MHz +CPU Systick: 240MHz +AXI: 120MHz +HCLK3: 60MHz +APB3 per: 60MHz +AHB1,2 per: 120MHz +APB1 per: 60MHz +APB1 tim: 120MHz +APB2 per: 60MHz +APB2 tim: 120MHz +AHB4 per: 120MHz +APB4 per: 60MHz +PCLK1: 60MHz (for USART2,3,4,5,7,8) +*/ + +// TODO: find a better way to distinguish between H725 (using SMPS) and H723 (lacking SMPS) +// The package will do for now, since we have only used TFBGA100 for H723 +static PackageSMPSType get_package_smps_type(void) { + PackageSMPSType ret; + RCC->APB4ENR |= RCC_APB4ENR_SYSCFGEN; // make sure SYSCFG clock is enabled. does seem to read fine without too though + + switch(SYSCFG->PKGR & 0xFU) { + case 0b0001U: // TFBGA100 Legacy + case 0b0011U: // TFBGA100 + ret = PACKAGE_WITHOUT_SMPS; + break; + case 0b0101U: // LQFP144 Legacy + case 0b0111U: // LQFP144 Industrial + case 0b1000U: // UFBGA169 + ret = PACKAGE_WITH_SMPS; + break; + default: + ret = PACKAGE_UNKNOWN; + } + return ret; +} + +void clock_init(void) { + /* + WARNING: PWR->CR3's lower byte can only be written once + * subsequent writes will silently fail + * only cleared with a full power-on-reset, not soft reset or reset pin + * some H7 have a bootrom with a DFU routine that writes (and locks) CR3 + * if the CR3 config doesn't match the HW, the core will deadlock and require immediately going into DFU from a cold boot + + In a normal bootup, the bootstub will be the first to write this. The app section calls clock_init again, but the CR3 write will silently fail. This is fine for most cases, but caution should be taken that the bootstub and app always write the same config. + */ + + // Set power mode to direct SMPS power supply (depends on the board layout) + PackageSMPSType package_smps = get_package_smps_type(); + if (package_smps == PACKAGE_WITHOUT_SMPS) { + register_set(&(PWR->CR3), PWR_CR3_LDOEN, 0xFU); // no SMPS, so powered by LDO + } else if (package_smps == PACKAGE_WITH_SMPS) { + register_set(&(PWR->CR3), PWR_CR3_SMPSEN, 0xFU); // powered only by SMPS + } else { + while(true); // unknown package, let's hang here + } + + // Set VOS level (VOS3 to 170Mhz, VOS2 to 300Mhz, VOS1 to 400Mhz, VOS0 to 550Mhz) + register_set(&(PWR->D3CR), PWR_D3CR_VOS_1 | PWR_D3CR_VOS_0, 0xC000U); //VOS1, needed for 80Mhz CAN FD + while ((PWR->CSR1 & PWR_CSR1_ACTVOSRDY) == 0U); + while ((PWR->CSR1 & PWR_CSR1_ACTVOS) != (PWR->D3CR & PWR_D3CR_VOS)); // check that VOS level was actually set + + // Configure Flash ACR register LATENCY and WRHIGHFREQ (VOS0 range!) + register_set(&(FLASH->ACR), FLASH_ACR_LATENCY_2WS | 0x20U, 0x3FU); // VOS2, AXI 100MHz-150MHz + // enable external oscillator HSE + register_set_bits(&(RCC->CR), RCC_CR_HSEON); + while ((RCC->CR & RCC_CR_HSERDY) == 0U); + // enable internal HSI48 for USB FS kernel + register_set_bits(&(RCC->CR), RCC_CR_HSI48ON); + while ((RCC->CR & RCC_CR_HSI48RDY) == 0U); + // Specify the frequency source for PLL1, divider for DIVM1, DIVM2, DIVM3 : HSE, 5, 5, 5 + register_set(&(RCC->PLLCKSELR), RCC_PLLCKSELR_PLLSRC_HSE | RCC_PLLCKSELR_DIVM1_0 | RCC_PLLCKSELR_DIVM1_2 | RCC_PLLCKSELR_DIVM2_0 | RCC_PLLCKSELR_DIVM2_2 | RCC_PLLCKSELR_DIVM3_0 | RCC_PLLCKSELR_DIVM3_2, 0x3F3F3F3U); + + // *** PLL1 start *** + // Specify multiplier N and dividers P, Q, R for PLL1 : 48, 1, 3, 2 (clock 240Mhz, PLL1Q 80Mhz for CAN FD) + register_set(&(RCC->PLL1DIVR), 0x102002FU, 0x7F7FFFFFU); + // Specify the input and output frequency ranges, enable dividers for PLL1 + register_set(&(RCC->PLLCFGR), RCC_PLLCFGR_PLL1RGE_2 | RCC_PLLCFGR_DIVP1EN | RCC_PLLCFGR_DIVQ1EN | RCC_PLLCFGR_DIVR1EN, 0x7000CU); + // Enable PLL1 + register_set_bits(&(RCC->CR), RCC_CR_PLL1ON); + while((RCC->CR & RCC_CR_PLL1RDY) == 0U); + // *** PLL1 end *** + + //////////////OTHER CLOCKS//////////////////// + // RCC HCLK Clock Source / RCC APB3 Clock Source / RCC SYS Clock Source + register_set(&(RCC->D1CFGR), RCC_D1CFGR_HPRE_DIV2 | RCC_D1CFGR_D1PPRE_DIV2 | RCC_D1CFGR_D1CPRE_DIV1, 0xF7FU); + // RCC APB1 Clock Source / RCC APB2 Clock Source + register_set(&(RCC->D2CFGR), RCC_D2CFGR_D2PPRE1_DIV2 | RCC_D2CFGR_D2PPRE2_DIV2, 0x770U); + // RCC APB4 Clock Source + register_set(&(RCC->D3CFGR), RCC_D3CFGR_D3PPRE_DIV2, 0x70U); + + // Set SysClock source to PLL + register_set(&(RCC->CFGR), RCC_CFGR_SW_PLL1, 0x7U); + while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL1); + //////////////END OTHER CLOCKS//////////////////// + + // Configure clock source for USB (HSI48) + register_set(&(RCC->D2CCIP2R), RCC_D2CCIP2R_USBSEL_1 | RCC_D2CCIP2R_USBSEL_0, RCC_D2CCIP2R_USBSEL); + // Configure clock source for FDCAN (PLL1Q at 80Mhz) + register_set(&(RCC->D2CCIP1R), RCC_D2CCIP1R_FDCANSEL_0, RCC_D2CCIP1R_FDCANSEL); + // Configure clock source for DFSDM1 + register_set_bits(&(RCC->D2CCIP1R), RCC_D2CCIP1R_DFSDM1SEL); + // Configure clock source for ADC1,2,3 (per_ck(currently HSE)) + register_set(&(RCC->D3CCIPR), RCC_D3CCIPR_ADCSEL_1, RCC_D3CCIPR_ADCSEL); + //Enable the Clock Security System + register_set_bits(&(RCC->CR), RCC_CR_CSSHSEON); + //Enable Vdd33usb supply level detector + register_set_bits(&(PWR->CR3), PWR_CR3_USB33DEN); +} diff --git a/board/stm32h7/clock.h b/board/stm32h7/clock.h index 40ad06043e5..5695034ea8f 100644 --- a/board/stm32h7/clock.h +++ b/board/stm32h7/clock.h @@ -1,21 +1,4 @@ -/* -HSE: 25MHz -PLL1Q: 80MHz (for FDCAN) -HSI48 enabled (for USB) -CPU: 240MHz -CPU Systick: 240MHz -AXI: 120MHz -HCLK3: 60MHz -APB3 per: 60MHz -AHB1,2 per: 120MHz -APB1 per: 60MHz -APB1 tim: 120MHz -APB2 per: 60MHz -APB2 tim: 120MHz -AHB4 per: 120MHz -APB4 per: 60MHz -PCLK1: 60MHz (for USART2,3,4,5,7,8) -*/ +#pragma once typedef enum { PACKAGE_UNKNOWN = 0, @@ -25,96 +8,5 @@ typedef enum { // TODO: find a better way to distinguish between H725 (using SMPS) and H723 (lacking SMPS) // The package will do for now, since we have only used TFBGA100 for H723 -static PackageSMPSType get_package_smps_type(void) { - PackageSMPSType ret; - RCC->APB4ENR |= RCC_APB4ENR_SYSCFGEN; // make sure SYSCFG clock is enabled. does seem to read fine without too though - switch(SYSCFG->PKGR & 0xFU) { - case 0b0001U: // TFBGA100 Legacy - case 0b0011U: // TFBGA100 - ret = PACKAGE_WITHOUT_SMPS; - break; - case 0b0101U: // LQFP144 Legacy - case 0b0111U: // LQFP144 Industrial - case 0b1000U: // UFBGA169 - ret = PACKAGE_WITH_SMPS; - break; - default: - ret = PACKAGE_UNKNOWN; - } - return ret; -} - -void clock_init(void) { - /* - WARNING: PWR->CR3's lower byte can only be written once - * subsequent writes will silently fail - * only cleared with a full power-on-reset, not soft reset or reset pin - * some H7 have a bootrom with a DFU routine that writes (and locks) CR3 - * if the CR3 config doesn't match the HW, the core will deadlock and require immediately going into DFU from a cold boot - - In a normal bootup, the bootstub will be the first to write this. The app section calls clock_init again, but the CR3 write will silently fail. This is fine for most cases, but caution should be taken that the bootstub and app always write the same config. - */ - - // Set power mode to direct SMPS power supply (depends on the board layout) - PackageSMPSType package_smps = get_package_smps_type(); - if (package_smps == PACKAGE_WITHOUT_SMPS) { - register_set(&(PWR->CR3), PWR_CR3_LDOEN, 0xFU); // no SMPS, so powered by LDO - } else if (package_smps == PACKAGE_WITH_SMPS) { - register_set(&(PWR->CR3), PWR_CR3_SMPSEN, 0xFU); // powered only by SMPS - } else { - while(true); // unknown package, let's hang here - } - - // Set VOS level (VOS3 to 170Mhz, VOS2 to 300Mhz, VOS1 to 400Mhz, VOS0 to 550Mhz) - register_set(&(PWR->D3CR), PWR_D3CR_VOS_1 | PWR_D3CR_VOS_0, 0xC000U); //VOS1, needed for 80Mhz CAN FD - while ((PWR->CSR1 & PWR_CSR1_ACTVOSRDY) == 0U); - while ((PWR->CSR1 & PWR_CSR1_ACTVOS) != (PWR->D3CR & PWR_D3CR_VOS)); // check that VOS level was actually set - - // Configure Flash ACR register LATENCY and WRHIGHFREQ (VOS0 range!) - register_set(&(FLASH->ACR), FLASH_ACR_LATENCY_2WS | 0x20U, 0x3FU); // VOS2, AXI 100MHz-150MHz - // enable external oscillator HSE - register_set_bits(&(RCC->CR), RCC_CR_HSEON); - while ((RCC->CR & RCC_CR_HSERDY) == 0U); - // enable internal HSI48 for USB FS kernel - register_set_bits(&(RCC->CR), RCC_CR_HSI48ON); - while ((RCC->CR & RCC_CR_HSI48RDY) == 0U); - // Specify the frequency source for PLL1, divider for DIVM1, DIVM2, DIVM3 : HSE, 5, 5, 5 - register_set(&(RCC->PLLCKSELR), RCC_PLLCKSELR_PLLSRC_HSE | RCC_PLLCKSELR_DIVM1_0 | RCC_PLLCKSELR_DIVM1_2 | RCC_PLLCKSELR_DIVM2_0 | RCC_PLLCKSELR_DIVM2_2 | RCC_PLLCKSELR_DIVM3_0 | RCC_PLLCKSELR_DIVM3_2, 0x3F3F3F3U); - - // *** PLL1 start *** - // Specify multiplier N and dividers P, Q, R for PLL1 : 48, 1, 3, 2 (clock 240Mhz, PLL1Q 80Mhz for CAN FD) - register_set(&(RCC->PLL1DIVR), 0x102002FU, 0x7F7FFFFFU); - // Specify the input and output frequency ranges, enable dividers for PLL1 - register_set(&(RCC->PLLCFGR), RCC_PLLCFGR_PLL1RGE_2 | RCC_PLLCFGR_DIVP1EN | RCC_PLLCFGR_DIVQ1EN | RCC_PLLCFGR_DIVR1EN, 0x7000CU); - // Enable PLL1 - register_set_bits(&(RCC->CR), RCC_CR_PLL1ON); - while((RCC->CR & RCC_CR_PLL1RDY) == 0U); - // *** PLL1 end *** - - //////////////OTHER CLOCKS//////////////////// - // RCC HCLK Clock Source / RCC APB3 Clock Source / RCC SYS Clock Source - register_set(&(RCC->D1CFGR), RCC_D1CFGR_HPRE_DIV2 | RCC_D1CFGR_D1PPRE_DIV2 | RCC_D1CFGR_D1CPRE_DIV1, 0xF7FU); - // RCC APB1 Clock Source / RCC APB2 Clock Source - register_set(&(RCC->D2CFGR), RCC_D2CFGR_D2PPRE1_DIV2 | RCC_D2CFGR_D2PPRE2_DIV2, 0x770U); - // RCC APB4 Clock Source - register_set(&(RCC->D3CFGR), RCC_D3CFGR_D3PPRE_DIV2, 0x70U); - - // Set SysClock source to PLL - register_set(&(RCC->CFGR), RCC_CFGR_SW_PLL1, 0x7U); - while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL1); - //////////////END OTHER CLOCKS//////////////////// - - // Configure clock source for USB (HSI48) - register_set(&(RCC->D2CCIP2R), RCC_D2CCIP2R_USBSEL_1 | RCC_D2CCIP2R_USBSEL_0, RCC_D2CCIP2R_USBSEL); - // Configure clock source for FDCAN (PLL1Q at 80Mhz) - register_set(&(RCC->D2CCIP1R), RCC_D2CCIP1R_FDCANSEL_0, RCC_D2CCIP1R_FDCANSEL); - // Configure clock source for DFSDM1 - register_set_bits(&(RCC->D2CCIP1R), RCC_D2CCIP1R_DFSDM1SEL); - // Configure clock source for ADC1,2,3 (per_ck(currently HSE)) - register_set(&(RCC->D3CCIPR), RCC_D3CCIPR_ADCSEL_1, RCC_D3CCIPR_ADCSEL); - //Enable the Clock Security System - register_set_bits(&(RCC->CR), RCC_CR_CSSHSEON); - //Enable Vdd33usb supply level detector - register_set_bits(&(PWR->CR3), PWR_CR3_USB33DEN); -} +void clock_init(void); \ No newline at end of file diff --git a/board/stm32h7/interrupt_handlers.c b/board/stm32h7/interrupt_handlers.c new file mode 100644 index 00000000000..4c8c2cac974 --- /dev/null +++ b/board/stm32h7/interrupt_handlers.c @@ -0,0 +1,143 @@ +#include "interrupt_handlers.h" +#include "stm32h7_config.h" + +void WWDG_IRQHandler(void) {handle_interrupt(WWDG_IRQn);} +void PVD_AVD_IRQHandler(void) {handle_interrupt(PVD_AVD_IRQn);} +void TAMP_STAMP_IRQHandler(void) {handle_interrupt(TAMP_STAMP_IRQn);} +void RTC_WKUP_IRQHandler(void) {handle_interrupt(RTC_WKUP_IRQn);} +void FLASH_IRQHandler(void) {handle_interrupt(FLASH_IRQn);} +void RCC_IRQHandler(void) {handle_interrupt(RCC_IRQn);} +void EXTI0_IRQHandler(void) {handle_interrupt(EXTI0_IRQn);} +void EXTI1_IRQHandler(void) {handle_interrupt(EXTI1_IRQn);} +void EXTI2_IRQHandler(void) {handle_interrupt(EXTI2_IRQn);} +void EXTI3_IRQHandler(void) {handle_interrupt(EXTI3_IRQn);} +void EXTI4_IRQHandler(void) {handle_interrupt(EXTI4_IRQn);} +void DMA1_Stream0_IRQHandler(void) {handle_interrupt(DMA1_Stream0_IRQn);} +void DMA1_Stream1_IRQHandler(void) {handle_interrupt(DMA1_Stream1_IRQn);} +void DMA1_Stream2_IRQHandler(void) {handle_interrupt(DMA1_Stream2_IRQn);} +void DMA1_Stream3_IRQHandler(void) {handle_interrupt(DMA1_Stream3_IRQn);} +void DMA1_Stream4_IRQHandler(void) {handle_interrupt(DMA1_Stream4_IRQn);} +void DMA1_Stream5_IRQHandler(void) {handle_interrupt(DMA1_Stream5_IRQn);} +void DMA1_Stream6_IRQHandler(void) {handle_interrupt(DMA1_Stream6_IRQn);} +void ADC_IRQHandler(void) {handle_interrupt(ADC_IRQn);} +void FDCAN1_IT0_IRQHandler(void) {handle_interrupt(FDCAN1_IT0_IRQn);} +void FDCAN2_IT0_IRQHandler(void) {handle_interrupt(FDCAN2_IT0_IRQn);} +void FDCAN1_IT1_IRQHandler(void) {handle_interrupt(FDCAN1_IT1_IRQn);} +void FDCAN2_IT1_IRQHandler(void) {handle_interrupt(FDCAN2_IT1_IRQn);} +void EXTI9_5_IRQHandler(void) {handle_interrupt(EXTI9_5_IRQn);} +void TIM1_BRK_IRQHandler(void) {handle_interrupt(TIM1_BRK_IRQn);} +void TIM1_UP_TIM10_IRQHandler(void) {handle_interrupt(TIM1_UP_TIM10_IRQn);} +void TIM1_TRG_COM_IRQHandler(void) {handle_interrupt(TIM1_TRG_COM_IRQn);} +void TIM1_CC_IRQHandler(void) {handle_interrupt(TIM1_CC_IRQn);} +void TIM2_IRQHandler(void) {handle_interrupt(TIM2_IRQn);} +void TIM3_IRQHandler(void) {handle_interrupt(TIM3_IRQn);} +void TIM4_IRQHandler(void) {handle_interrupt(TIM4_IRQn);} +void I2C1_EV_IRQHandler(void) {handle_interrupt(I2C1_EV_IRQn);} +void I2C1_ER_IRQHandler(void) {handle_interrupt(I2C1_ER_IRQn);} +void I2C2_EV_IRQHandler(void) {handle_interrupt(I2C2_EV_IRQn);} +void I2C2_ER_IRQHandler(void) {handle_interrupt(I2C2_ER_IRQn);} +void SPI1_IRQHandler(void) {handle_interrupt(SPI1_IRQn);} +void SPI2_IRQHandler(void) {handle_interrupt(SPI2_IRQn);} +void USART1_IRQHandler(void) {handle_interrupt(USART1_IRQn);} +void USART2_IRQHandler(void) {handle_interrupt(USART2_IRQn);} +void USART3_IRQHandler(void) {handle_interrupt(USART3_IRQn);} +void EXTI15_10_IRQHandler(void) {handle_interrupt(EXTI15_10_IRQn);} +void RTC_Alarm_IRQHandler(void) {handle_interrupt(RTC_Alarm_IRQn);} +void TIM8_BRK_TIM12_IRQHandler(void) {handle_interrupt(TIM8_BRK_TIM12_IRQn);} +void TIM8_UP_TIM13_IRQHandler(void) {handle_interrupt(TIM8_UP_TIM13_IRQn);} +void TIM8_TRG_COM_TIM14_IRQHandler(void) {handle_interrupt(TIM8_TRG_COM_TIM14_IRQn);} +void TIM8_CC_IRQHandler(void) {handle_interrupt(TIM8_CC_IRQn);} +void DMA1_Stream7_IRQHandler(void) {handle_interrupt(DMA1_Stream7_IRQn);} +void FMC_IRQHandler(void) {handle_interrupt(FMC_IRQn);} +void SDMMC1_IRQHandler(void) {handle_interrupt(SDMMC1_IRQn);} +void TIM5_IRQHandler(void) {handle_interrupt(TIM5_IRQn);} +void SPI3_IRQHandler(void) {handle_interrupt(SPI3_IRQn);} +void UART4_IRQHandler(void) {handle_interrupt(UART4_IRQn);} +void UART5_IRQHandler(void) {handle_interrupt(UART5_IRQn);} +void TIM6_DAC_IRQHandler(void) {handle_interrupt(TIM6_DAC_IRQn);} +void TIM7_IRQHandler(void) {handle_interrupt(TIM7_IRQn);} +void DMA2_Stream0_IRQHandler(void) {handle_interrupt(DMA2_Stream0_IRQn);} +void DMA2_Stream1_IRQHandler(void) {handle_interrupt(DMA2_Stream1_IRQn);} +void DMA2_Stream2_IRQHandler(void) {handle_interrupt(DMA2_Stream2_IRQn);} +void DMA2_Stream3_IRQHandler(void) {handle_interrupt(DMA2_Stream3_IRQn);} +void DMA2_Stream4_IRQHandler(void) {handle_interrupt(DMA2_Stream4_IRQn);} +void ETH_IRQHandler(void) {handle_interrupt(ETH_IRQn);} +void ETH_WKUP_IRQHandler(void) {handle_interrupt(ETH_WKUP_IRQn);} +void FDCAN_CAL_IRQHandler(void) {handle_interrupt(FDCAN_CAL_IRQn);} +void DMA2_Stream5_IRQHandler(void) {handle_interrupt(DMA2_Stream5_IRQn);} +void DMA2_Stream6_IRQHandler(void) {handle_interrupt(DMA2_Stream6_IRQn);} +void DMA2_Stream7_IRQHandler(void) {handle_interrupt(DMA2_Stream7_IRQn);} +void USART6_IRQHandler(void) {handle_interrupt(USART6_IRQn);} +void I2C3_EV_IRQHandler(void) {handle_interrupt(I2C3_EV_IRQn);} +void I2C3_ER_IRQHandler(void) {handle_interrupt(I2C3_ER_IRQn);} +void OTG_HS_EP1_OUT_IRQHandler(void) {handle_interrupt(OTG_HS_EP1_OUT_IRQn);} +void OTG_HS_EP1_IN_IRQHandler(void) {handle_interrupt(OTG_HS_EP1_IN_IRQn);} +void OTG_HS_WKUP_IRQHandler(void) {handle_interrupt(OTG_HS_WKUP_IRQn);} +void OTG_HS_IRQHandler(void) {handle_interrupt(OTG_HS_IRQn);} +void DCMI_PSSI_IRQHandler(void) {handle_interrupt(DCMI_PSSI_IRQn);} +void CRYP_IRQHandler(void) {handle_interrupt(CRYP_IRQn);} +void HASH_RNG_IRQHandler(void) {handle_interrupt(HASH_RNG_IRQn);} +void FPU_IRQHandler(void) {handle_interrupt(FPU_IRQn);} +void UART7_IRQHandler(void) {handle_interrupt(UART7_IRQn);} +void UART8_IRQHandler(void) {handle_interrupt(UART8_IRQn);} +void SPI4_IRQHandler(void) {handle_interrupt(SPI4_IRQn);} +void SPI5_IRQHandler(void) {handle_interrupt(SPI5_IRQn);} +void SPI6_IRQHandler(void) {handle_interrupt(SPI6_IRQn);} +void SAI1_IRQHandler(void) {handle_interrupt(SAI1_IRQn);} +void LTDC_IRQHandler(void) {handle_interrupt(LTDC_IRQn);} +void LTDC_ER_IRQHandler(void) {handle_interrupt(LTDC_ER_IRQn);} +void DMA2D_IRQHandler(void) {handle_interrupt(DMA2D_IRQn);} +void OCTOSPI1_IRQHandler(void) {handle_interrupt(OCTOSPI1_IRQn);} +void LPTIM1_IRQHandler(void) {handle_interrupt(LPTIM1_IRQn);} +void CEC_IRQHandler(void) {handle_interrupt(CEC_IRQn);} +void I2C4_EV_IRQHandler(void) {handle_interrupt(I2C4_EV_IRQn);} +void I2C4_ER_IRQHandler(void) {handle_interrupt(I2C4_ER_IRQn);} +void SPDIF_RX_IRQHandler(void) {handle_interrupt(SPDIF_RX_IRQn);} +void DMAMUX1_OVR_IRQHandler(void) {handle_interrupt(DMAMUX1_OVR_IRQn);} +void DFSDM1_FLT0_IRQHandler(void) {handle_interrupt(DFSDM1_FLT0_IRQn);} +void DFSDM1_FLT1_IRQHandler(void) {handle_interrupt(DFSDM1_FLT1_IRQn);} +void DFSDM1_FLT2_IRQHandler(void) {handle_interrupt(DFSDM1_FLT2_IRQn);} +void DFSDM1_FLT3_IRQHandler(void) {handle_interrupt(DFSDM1_FLT3_IRQn);} +void SWPMI1_IRQHandler(void) {handle_interrupt(SWPMI1_IRQn);} +void TIM15_IRQHandler(void) {handle_interrupt(TIM15_IRQn);} +void TIM16_IRQHandler(void) {handle_interrupt(TIM16_IRQn);} +void TIM17_IRQHandler(void) {handle_interrupt(TIM17_IRQn);} +void MDIOS_WKUP_IRQHandler(void) {handle_interrupt(MDIOS_WKUP_IRQn);} +void MDIOS_IRQHandler(void) {handle_interrupt(MDIOS_IRQn);} +void MDMA_IRQHandler(void) {handle_interrupt(MDMA_IRQn);} +void SDMMC2_IRQHandler(void) {handle_interrupt(SDMMC2_IRQn);} +void HSEM1_IRQHandler(void) {handle_interrupt(HSEM1_IRQn);} +void ADC3_IRQHandler(void) {handle_interrupt(ADC3_IRQn);} +void DMAMUX2_OVR_IRQHandler(void) {handle_interrupt(DMAMUX2_OVR_IRQn);} +void BDMA_Channel0_IRQHandler(void) {handle_interrupt(BDMA_Channel0_IRQn);} +void BDMA_Channel1_IRQHandler(void) {handle_interrupt(BDMA_Channel1_IRQn);} +void BDMA_Channel2_IRQHandler(void) {handle_interrupt(BDMA_Channel2_IRQn);} +void BDMA_Channel3_IRQHandler(void) {handle_interrupt(BDMA_Channel3_IRQn);} +void BDMA_Channel4_IRQHandler(void) {handle_interrupt(BDMA_Channel4_IRQn);} +void BDMA_Channel5_IRQHandler(void) {handle_interrupt(BDMA_Channel5_IRQn);} +void BDMA_Channel6_IRQHandler(void) {handle_interrupt(BDMA_Channel6_IRQn);} +void BDMA_Channel7_IRQHandler(void) {handle_interrupt(BDMA_Channel7_IRQn);} +void COMP_IRQHandler(void) {handle_interrupt(COMP_IRQn);} +void LPTIM2_IRQHandler(void) {handle_interrupt(LPTIM2_IRQn);} +void LPTIM3_IRQHandler(void) {handle_interrupt(LPTIM3_IRQn);} +void LPTIM4_IRQHandler(void) {handle_interrupt(LPTIM4_IRQn);} +void LPTIM5_IRQHandler(void) {handle_interrupt(LPTIM5_IRQn);} +void LPUART1_IRQHandler(void) {handle_interrupt(LPUART1_IRQn);} +void CRS_IRQHandler(void) {handle_interrupt(CRS_IRQn);} +void ECC_IRQHandler(void) {handle_interrupt(ECC_IRQn);} +void SAI4_IRQHandler(void) {handle_interrupt(SAI4_IRQn);} +void DTS_IRQHandler(void) {handle_interrupt(DTS_IRQn);} +void WAKEUP_PIN_IRQHandler(void) {handle_interrupt(WAKEUP_PIN_IRQn);} +void OCTOSPI2_IRQHandler(void) {handle_interrupt(OCTOSPI2_IRQn);} +void OTFDEC1_IRQHandler(void) {handle_interrupt(OTFDEC1_IRQn);} +void OTFDEC2_IRQHandler(void) {handle_interrupt(OTFDEC2_IRQn);} +void FMAC_IRQHandler(void) {handle_interrupt(FMAC_IRQn);} +void CORDIC_IRQHandler(void) {handle_interrupt(CORDIC_IRQn);} +void UART9_IRQHandler(void) {handle_interrupt(UART9_IRQn);} +void USART10_IRQHandler(void) {handle_interrupt(USART10_IRQn);} +void I2C5_EV_IRQHandler(void) {handle_interrupt(I2C5_EV_IRQn);} +void I2C5_ER_IRQHandler(void) {handle_interrupt(I2C5_ER_IRQn);} +void FDCAN3_IT0_IRQHandler(void) {handle_interrupt(FDCAN3_IT0_IRQn);} +void FDCAN3_IT1_IRQHandler(void) {handle_interrupt(FDCAN3_IT1_IRQn);} +void TIM23_IRQHandler(void) {handle_interrupt(TIM23_IRQn);} +void TIM24_IRQHandler(void) {handle_interrupt(TIM24_IRQn);} diff --git a/board/stm32h7/interrupt_handlers.h b/board/stm32h7/interrupt_handlers.h index 8148021d697..f73d43241ba 100644 --- a/board/stm32h7/interrupt_handlers.h +++ b/board/stm32h7/interrupt_handlers.h @@ -1,143 +1,145 @@ // ********************* Bare interrupt handlers ********************* // Interrupts for STM32H7x5 -void WWDG_IRQHandler(void) {handle_interrupt(WWDG_IRQn);} -void PVD_AVD_IRQHandler(void) {handle_interrupt(PVD_AVD_IRQn);} -void TAMP_STAMP_IRQHandler(void) {handle_interrupt(TAMP_STAMP_IRQn);} -void RTC_WKUP_IRQHandler(void) {handle_interrupt(RTC_WKUP_IRQn);} -void FLASH_IRQHandler(void) {handle_interrupt(FLASH_IRQn);} -void RCC_IRQHandler(void) {handle_interrupt(RCC_IRQn);} -void EXTI0_IRQHandler(void) {handle_interrupt(EXTI0_IRQn);} -void EXTI1_IRQHandler(void) {handle_interrupt(EXTI1_IRQn);} -void EXTI2_IRQHandler(void) {handle_interrupt(EXTI2_IRQn);} -void EXTI3_IRQHandler(void) {handle_interrupt(EXTI3_IRQn);} -void EXTI4_IRQHandler(void) {handle_interrupt(EXTI4_IRQn);} -void DMA1_Stream0_IRQHandler(void) {handle_interrupt(DMA1_Stream0_IRQn);} -void DMA1_Stream1_IRQHandler(void) {handle_interrupt(DMA1_Stream1_IRQn);} -void DMA1_Stream2_IRQHandler(void) {handle_interrupt(DMA1_Stream2_IRQn);} -void DMA1_Stream3_IRQHandler(void) {handle_interrupt(DMA1_Stream3_IRQn);} -void DMA1_Stream4_IRQHandler(void) {handle_interrupt(DMA1_Stream4_IRQn);} -void DMA1_Stream5_IRQHandler(void) {handle_interrupt(DMA1_Stream5_IRQn);} -void DMA1_Stream6_IRQHandler(void) {handle_interrupt(DMA1_Stream6_IRQn);} -void ADC_IRQHandler(void) {handle_interrupt(ADC_IRQn);} -void FDCAN1_IT0_IRQHandler(void) {handle_interrupt(FDCAN1_IT0_IRQn);} -void FDCAN2_IT0_IRQHandler(void) {handle_interrupt(FDCAN2_IT0_IRQn);} -void FDCAN1_IT1_IRQHandler(void) {handle_interrupt(FDCAN1_IT1_IRQn);} -void FDCAN2_IT1_IRQHandler(void) {handle_interrupt(FDCAN2_IT1_IRQn);} -void EXTI9_5_IRQHandler(void) {handle_interrupt(EXTI9_5_IRQn);} -void TIM1_BRK_IRQHandler(void) {handle_interrupt(TIM1_BRK_IRQn);} -void TIM1_UP_TIM10_IRQHandler(void) {handle_interrupt(TIM1_UP_TIM10_IRQn);} -void TIM1_TRG_COM_IRQHandler(void) {handle_interrupt(TIM1_TRG_COM_IRQn);} -void TIM1_CC_IRQHandler(void) {handle_interrupt(TIM1_CC_IRQn);} -void TIM2_IRQHandler(void) {handle_interrupt(TIM2_IRQn);} -void TIM3_IRQHandler(void) {handle_interrupt(TIM3_IRQn);} -void TIM4_IRQHandler(void) {handle_interrupt(TIM4_IRQn);} -void I2C1_EV_IRQHandler(void) {handle_interrupt(I2C1_EV_IRQn);} -void I2C1_ER_IRQHandler(void) {handle_interrupt(I2C1_ER_IRQn);} -void I2C2_EV_IRQHandler(void) {handle_interrupt(I2C2_EV_IRQn);} -void I2C2_ER_IRQHandler(void) {handle_interrupt(I2C2_ER_IRQn);} -void SPI1_IRQHandler(void) {handle_interrupt(SPI1_IRQn);} -void SPI2_IRQHandler(void) {handle_interrupt(SPI2_IRQn);} -void USART1_IRQHandler(void) {handle_interrupt(USART1_IRQn);} -void USART2_IRQHandler(void) {handle_interrupt(USART2_IRQn);} -void USART3_IRQHandler(void) {handle_interrupt(USART3_IRQn);} -void EXTI15_10_IRQHandler(void) {handle_interrupt(EXTI15_10_IRQn);} -void RTC_Alarm_IRQHandler(void) {handle_interrupt(RTC_Alarm_IRQn);} -void TIM8_BRK_TIM12_IRQHandler(void) {handle_interrupt(TIM8_BRK_TIM12_IRQn);} -void TIM8_UP_TIM13_IRQHandler(void) {handle_interrupt(TIM8_UP_TIM13_IRQn);} -void TIM8_TRG_COM_TIM14_IRQHandler(void) {handle_interrupt(TIM8_TRG_COM_TIM14_IRQn);} -void TIM8_CC_IRQHandler(void) {handle_interrupt(TIM8_CC_IRQn);} -void DMA1_Stream7_IRQHandler(void) {handle_interrupt(DMA1_Stream7_IRQn);} -void FMC_IRQHandler(void) {handle_interrupt(FMC_IRQn);} -void SDMMC1_IRQHandler(void) {handle_interrupt(SDMMC1_IRQn);} -void TIM5_IRQHandler(void) {handle_interrupt(TIM5_IRQn);} -void SPI3_IRQHandler(void) {handle_interrupt(SPI3_IRQn);} -void UART4_IRQHandler(void) {handle_interrupt(UART4_IRQn);} -void UART5_IRQHandler(void) {handle_interrupt(UART5_IRQn);} -void TIM6_DAC_IRQHandler(void) {handle_interrupt(TIM6_DAC_IRQn);} -void TIM7_IRQHandler(void) {handle_interrupt(TIM7_IRQn);} -void DMA2_Stream0_IRQHandler(void) {handle_interrupt(DMA2_Stream0_IRQn);} -void DMA2_Stream1_IRQHandler(void) {handle_interrupt(DMA2_Stream1_IRQn);} -void DMA2_Stream2_IRQHandler(void) {handle_interrupt(DMA2_Stream2_IRQn);} -void DMA2_Stream3_IRQHandler(void) {handle_interrupt(DMA2_Stream3_IRQn);} -void DMA2_Stream4_IRQHandler(void) {handle_interrupt(DMA2_Stream4_IRQn);} -void ETH_IRQHandler(void) {handle_interrupt(ETH_IRQn);} -void ETH_WKUP_IRQHandler(void) {handle_interrupt(ETH_WKUP_IRQn);} -void FDCAN_CAL_IRQHandler(void) {handle_interrupt(FDCAN_CAL_IRQn);} -void DMA2_Stream5_IRQHandler(void) {handle_interrupt(DMA2_Stream5_IRQn);} -void DMA2_Stream6_IRQHandler(void) {handle_interrupt(DMA2_Stream6_IRQn);} -void DMA2_Stream7_IRQHandler(void) {handle_interrupt(DMA2_Stream7_IRQn);} -void USART6_IRQHandler(void) {handle_interrupt(USART6_IRQn);} -void I2C3_EV_IRQHandler(void) {handle_interrupt(I2C3_EV_IRQn);} -void I2C3_ER_IRQHandler(void) {handle_interrupt(I2C3_ER_IRQn);} -void OTG_HS_EP1_OUT_IRQHandler(void) {handle_interrupt(OTG_HS_EP1_OUT_IRQn);} -void OTG_HS_EP1_IN_IRQHandler(void) {handle_interrupt(OTG_HS_EP1_IN_IRQn);} -void OTG_HS_WKUP_IRQHandler(void) {handle_interrupt(OTG_HS_WKUP_IRQn);} -void OTG_HS_IRQHandler(void) {handle_interrupt(OTG_HS_IRQn);} -void DCMI_PSSI_IRQHandler(void) {handle_interrupt(DCMI_PSSI_IRQn);} -void CRYP_IRQHandler(void) {handle_interrupt(CRYP_IRQn);} -void HASH_RNG_IRQHandler(void) {handle_interrupt(HASH_RNG_IRQn);} -void FPU_IRQHandler(void) {handle_interrupt(FPU_IRQn);} -void UART7_IRQHandler(void) {handle_interrupt(UART7_IRQn);} -void UART8_IRQHandler(void) {handle_interrupt(UART8_IRQn);} -void SPI4_IRQHandler(void) {handle_interrupt(SPI4_IRQn);} -void SPI5_IRQHandler(void) {handle_interrupt(SPI5_IRQn);} -void SPI6_IRQHandler(void) {handle_interrupt(SPI6_IRQn);} -void SAI1_IRQHandler(void) {handle_interrupt(SAI1_IRQn);} -void LTDC_IRQHandler(void) {handle_interrupt(LTDC_IRQn);} -void LTDC_ER_IRQHandler(void) {handle_interrupt(LTDC_ER_IRQn);} -void DMA2D_IRQHandler(void) {handle_interrupt(DMA2D_IRQn);} -void OCTOSPI1_IRQHandler(void) {handle_interrupt(OCTOSPI1_IRQn);} -void LPTIM1_IRQHandler(void) {handle_interrupt(LPTIM1_IRQn);} -void CEC_IRQHandler(void) {handle_interrupt(CEC_IRQn);} -void I2C4_EV_IRQHandler(void) {handle_interrupt(I2C4_EV_IRQn);} -void I2C4_ER_IRQHandler(void) {handle_interrupt(I2C4_ER_IRQn);} -void SPDIF_RX_IRQHandler(void) {handle_interrupt(SPDIF_RX_IRQn);} -void DMAMUX1_OVR_IRQHandler(void) {handle_interrupt(DMAMUX1_OVR_IRQn);} -void DFSDM1_FLT0_IRQHandler(void) {handle_interrupt(DFSDM1_FLT0_IRQn);} -void DFSDM1_FLT1_IRQHandler(void) {handle_interrupt(DFSDM1_FLT1_IRQn);} -void DFSDM1_FLT2_IRQHandler(void) {handle_interrupt(DFSDM1_FLT2_IRQn);} -void DFSDM1_FLT3_IRQHandler(void) {handle_interrupt(DFSDM1_FLT3_IRQn);} -void SWPMI1_IRQHandler(void) {handle_interrupt(SWPMI1_IRQn);} -void TIM15_IRQHandler(void) {handle_interrupt(TIM15_IRQn);} -void TIM16_IRQHandler(void) {handle_interrupt(TIM16_IRQn);} -void TIM17_IRQHandler(void) {handle_interrupt(TIM17_IRQn);} -void MDIOS_WKUP_IRQHandler(void) {handle_interrupt(MDIOS_WKUP_IRQn);} -void MDIOS_IRQHandler(void) {handle_interrupt(MDIOS_IRQn);} -void MDMA_IRQHandler(void) {handle_interrupt(MDMA_IRQn);} -void SDMMC2_IRQHandler(void) {handle_interrupt(SDMMC2_IRQn);} -void HSEM1_IRQHandler(void) {handle_interrupt(HSEM1_IRQn);} -void ADC3_IRQHandler(void) {handle_interrupt(ADC3_IRQn);} -void DMAMUX2_OVR_IRQHandler(void) {handle_interrupt(DMAMUX2_OVR_IRQn);} -void BDMA_Channel0_IRQHandler(void) {handle_interrupt(BDMA_Channel0_IRQn);} -void BDMA_Channel1_IRQHandler(void) {handle_interrupt(BDMA_Channel1_IRQn);} -void BDMA_Channel2_IRQHandler(void) {handle_interrupt(BDMA_Channel2_IRQn);} -void BDMA_Channel3_IRQHandler(void) {handle_interrupt(BDMA_Channel3_IRQn);} -void BDMA_Channel4_IRQHandler(void) {handle_interrupt(BDMA_Channel4_IRQn);} -void BDMA_Channel5_IRQHandler(void) {handle_interrupt(BDMA_Channel5_IRQn);} -void BDMA_Channel6_IRQHandler(void) {handle_interrupt(BDMA_Channel6_IRQn);} -void BDMA_Channel7_IRQHandler(void) {handle_interrupt(BDMA_Channel7_IRQn);} -void COMP_IRQHandler(void) {handle_interrupt(COMP_IRQn);} -void LPTIM2_IRQHandler(void) {handle_interrupt(LPTIM2_IRQn);} -void LPTIM3_IRQHandler(void) {handle_interrupt(LPTIM3_IRQn);} -void LPTIM4_IRQHandler(void) {handle_interrupt(LPTIM4_IRQn);} -void LPTIM5_IRQHandler(void) {handle_interrupt(LPTIM5_IRQn);} -void LPUART1_IRQHandler(void) {handle_interrupt(LPUART1_IRQn);} -void CRS_IRQHandler(void) {handle_interrupt(CRS_IRQn);} -void ECC_IRQHandler(void) {handle_interrupt(ECC_IRQn);} -void SAI4_IRQHandler(void) {handle_interrupt(SAI4_IRQn);} -void DTS_IRQHandler(void) {handle_interrupt(DTS_IRQn);} -void WAKEUP_PIN_IRQHandler(void) {handle_interrupt(WAKEUP_PIN_IRQn);} -void OCTOSPI2_IRQHandler(void) {handle_interrupt(OCTOSPI2_IRQn);} -void OTFDEC1_IRQHandler(void) {handle_interrupt(OTFDEC1_IRQn);} -void OTFDEC2_IRQHandler(void) {handle_interrupt(OTFDEC2_IRQn);} -void FMAC_IRQHandler(void) {handle_interrupt(FMAC_IRQn);} -void CORDIC_IRQHandler(void) {handle_interrupt(CORDIC_IRQn);} -void UART9_IRQHandler(void) {handle_interrupt(UART9_IRQn);} -void USART10_IRQHandler(void) {handle_interrupt(USART10_IRQn);} -void I2C5_EV_IRQHandler(void) {handle_interrupt(I2C5_EV_IRQn);} -void I2C5_ER_IRQHandler(void) {handle_interrupt(I2C5_ER_IRQn);} -void FDCAN3_IT0_IRQHandler(void) {handle_interrupt(FDCAN3_IT0_IRQn);} -void FDCAN3_IT1_IRQHandler(void) {handle_interrupt(FDCAN3_IT1_IRQn);} -void TIM23_IRQHandler(void) {handle_interrupt(TIM23_IRQn);} -void TIM24_IRQHandler(void) {handle_interrupt(TIM24_IRQn);} +#pragma once + +void WWDG_IRQHandler(void); +void PVD_AVD_IRQHandler(void); +void TAMP_STAMP_IRQHandler(void); +void RTC_WKUP_IRQHandler(void); +void FLASH_IRQHandler(void); +void RCC_IRQHandler(void); +void EXTI0_IRQHandler(void); +void EXTI1_IRQHandler(void); +void EXTI2_IRQHandler(void); +void EXTI3_IRQHandler(void); +void EXTI4_IRQHandler(void); +void DMA1_Stream0_IRQHandler(void); +void DMA1_Stream1_IRQHandler(void); +void DMA1_Stream2_IRQHandler(void); +void DMA1_Stream3_IRQHandler(void); +void DMA1_Stream4_IRQHandler(void); +void DMA1_Stream5_IRQHandler(void); +void DMA1_Stream6_IRQHandler(void); +void ADC_IRQHandler(void); +void FDCAN1_IT0_IRQHandler(void); +void FDCAN2_IT0_IRQHandler(void); +void FDCAN1_IT1_IRQHandler(void); +void FDCAN2_IT1_IRQHandler(void); +void EXTI9_5_IRQHandler(void); +void TIM1_BRK_IRQHandler(void); +void TIM1_UP_TIM10_IRQHandler(void); +void TIM1_TRG_COM_IRQHandler(void); +void TIM1_CC_IRQHandler(void); +void TIM2_IRQHandler(void); +void TIM3_IRQHandler(void); +void TIM4_IRQHandler(void); +void I2C1_EV_IRQHandler(void); +void I2C1_ER_IRQHandler(void); +void I2C2_EV_IRQHandler(void); +void I2C2_ER_IRQHandler(void); +void SPI1_IRQHandler(void); +void SPI2_IRQHandler(void); +void USART1_IRQHandler(void); +void USART2_IRQHandler(void); +void USART3_IRQHandler(void); +void EXTI15_10_IRQHandler(void); +void RTC_Alarm_IRQHandler(void); +void TIM8_BRK_TIM12_IRQHandler(void); +void TIM8_UP_TIM13_IRQHandler(void); +void TIM8_TRG_COM_TIM14_IRQHandler(void); +void TIM8_CC_IRQHandler(void); +void DMA1_Stream7_IRQHandler(void); +void FMC_IRQHandler(void); +void SDMMC1_IRQHandler(void); +void TIM5_IRQHandler(void); +void SPI3_IRQHandler(void); +void UART4_IRQHandler(void); +void UART5_IRQHandler(void); +void TIM6_DAC_IRQHandler(void); +void TIM7_IRQHandler(void); +void DMA2_Stream0_IRQHandler(void); +void DMA2_Stream1_IRQHandler(void); +void DMA2_Stream2_IRQHandler(void); +void DMA2_Stream3_IRQHandler(void); +void DMA2_Stream4_IRQHandler(void); +void ETH_IRQHandler(void); +void ETH_WKUP_IRQHandler(void); +void FDCAN_CAL_IRQHandler(void); +void DMA2_Stream5_IRQHandler(void); +void DMA2_Stream6_IRQHandler(void); +void DMA2_Stream7_IRQHandler(void); +void USART6_IRQHandler(void); +void I2C3_EV_IRQHandler(void); +void I2C3_ER_IRQHandler(void); +void OTG_HS_EP1_OUT_IRQHandler(void); +void OTG_HS_EP1_IN_IRQHandler(void); +void OTG_HS_WKUP_IRQHandler(void); +void OTG_HS_IRQHandler(void); +void DCMI_PSSI_IRQHandler(void); +void CRYP_IRQHandler(void); +void HASH_RNG_IRQHandler(void); +void FPU_IRQHandler(void); +void UART7_IRQHandler(void); +void UART8_IRQHandler(void); +void SPI4_IRQHandler(void); +void SPI5_IRQHandler(void); +void SPI6_IRQHandler(void); +void SAI1_IRQHandler(void); +void LTDC_IRQHandler(void); +void LTDC_ER_IRQHandler(void); +void DMA2D_IRQHandler(void); +void OCTOSPI1_IRQHandler(void); +void LPTIM1_IRQHandler(void); +void CEC_IRQHandler(void); +void I2C4_EV_IRQHandler(void); +void I2C4_ER_IRQHandler(void); +void SPDIF_RX_IRQHandler(void); +void DMAMUX1_OVR_IRQHandler(void); +void DFSDM1_FLT0_IRQHandler(void); +void DFSDM1_FLT1_IRQHandler(void); +void DFSDM1_FLT2_IRQHandler(void); +void DFSDM1_FLT3_IRQHandler(void); +void SWPMI1_IRQHandler(void); +void TIM15_IRQHandler(void); +void TIM16_IRQHandler(void); +void TIM17_IRQHandler(void); +void MDIOS_WKUP_IRQHandler(void); +void MDIOS_IRQHandler(void); +void MDMA_IRQHandler(void); +void SDMMC2_IRQHandler(void); +void HSEM1_IRQHandler(void); +void ADC3_IRQHandler(void); +void DMAMUX2_OVR_IRQHandler(void); +void BDMA_Channel0_IRQHandler(void); +void BDMA_Channel1_IRQHandler(void); +void BDMA_Channel2_IRQHandler(void); +void BDMA_Channel3_IRQHandler(void); +void BDMA_Channel4_IRQHandler(void); +void BDMA_Channel5_IRQHandler(void); +void BDMA_Channel6_IRQHandler(void); +void BDMA_Channel7_IRQHandler(void); +void COMP_IRQHandler(void); +void LPTIM2_IRQHandler(void); +void LPTIM3_IRQHandler(void); +void LPTIM4_IRQHandler(void); +void LPTIM5_IRQHandler(void); +void LPUART1_IRQHandler(void); +void CRS_IRQHandler(void); +void ECC_IRQHandler(void); +void SAI4_IRQHandler(void); +void DTS_IRQHandler(void); +void WAKEUP_PIN_IRQHandler(void); +void OCTOSPI2_IRQHandler(void); +void OTFDEC1_IRQHandler(void); +void OTFDEC2_IRQHandler(void); +void FMAC_IRQHandler(void); +void CORDIC_IRQHandler(void); +void UART9_IRQHandler(void); +void USART10_IRQHandler(void); +void I2C5_EV_IRQHandler(void); +void I2C5_ER_IRQHandler(void); +void FDCAN3_IT0_IRQHandler(void); +void FDCAN3_IT1_IRQHandler(void); +void TIM23_IRQHandler(void); +void TIM24_IRQHandler(void); diff --git a/board/stm32h7/lladc.c b/board/stm32h7/lladc.c new file mode 100644 index 00000000000..1d0d8f876f0 --- /dev/null +++ b/board/stm32h7/lladc.c @@ -0,0 +1,82 @@ +#include "lladc.h" + +#include "board/libc.h" + +static uint32_t adc_avdd_mV = 0U; + +void adc_init(ADC_TypeDef *adc) { + adc->CR &= ~(ADC_CR_ADEN); // Disable ADC + adc->CR &= ~(ADC_CR_DEEPPWD); // Reset deep-power-down mode + adc->CR |= ADC_CR_ADVREGEN; // Enable ADC regulator + while(!(adc->ISR & ADC_ISR_LDORDY) && (adc != ADC3)); + + if (adc != ADC3) { + adc->CR &= ~(ADC_CR_ADCALDIF); // Choose single-ended calibration + adc->CR |= ADC_CR_ADCALLIN; // Lineriality calibration + } + adc->CR |= ADC_CR_ADCAL; // Start calibration + while((adc->CR & ADC_CR_ADCAL) != 0U); + + adc->ISR |= ADC_ISR_ADRDY; + adc->CR |= ADC_CR_ADEN; + while(!(adc->ISR & ADC_ISR_ADRDY)); +} + +uint16_t adc_get_raw(const adc_signal_t *signal) { + signal->adc->SQR1 &= ~(ADC_SQR1_L); + signal->adc->SQR1 = ((uint32_t) signal->channel << 6U); + + // sample time + if (signal->channel < 10U) { + signal->adc->SMPR1 = ((uint32_t) signal->sample_time << (signal->channel * 3U)); + } else { + signal->adc->SMPR2 = ((uint32_t) signal->sample_time << ((signal->channel - 10U) * 3U)); + } + + // select channel + signal->adc->PCSEL_RES0 = (0x1UL << signal->channel); + + // oversampling + signal->adc->CFGR2 = (((1U << (uint32_t) signal->oversampling) - 1U) << ADC_CFGR2_OVSR_Pos) | ((uint32_t) signal->oversampling << ADC_CFGR2_OVSS_Pos); + signal->adc->CFGR2 |= (signal->oversampling != OVERSAMPLING_1) ? ADC_CFGR2_ROVSE : 0U; + + // start conversion + signal->adc->CR |= ADC_CR_ADSTART; + while (!(signal->adc->ISR & ADC_ISR_EOC)); + + uint16_t res = signal->adc->DR; + + while (!(signal->adc->ISR & ADC_ISR_EOS)); + signal->adc->ISR |= ADC_ISR_EOS; + + return res; +} + +static void adc_calibrate_vdda(void) { + // ADC2 used for calibration + adc_init(ADC2); + + // enable VREFINT channel + ADC3_COMMON->CCR |= ADC_CCR_VREFEN; + SYSCFG->ADC2ALT |= SYSCFG_ADC2ALT_ADC2_ROUT1; + + // measure VREFINT and derive AVDD + uint16_t raw_vrefint = adc_get_raw(&(adc_signal_t){.adc = ADC2, .channel = 17U, .sample_time = SAMPLETIME_810_CYCLES, .oversampling = OVERSAMPLING_256}); + adc_avdd_mV = (uint32_t) *VREFINT_CAL_ADDR * 16U * 3300U / raw_vrefint; + print(" AVDD: 0x"); puth(adc_avdd_mV); print(" mV\n"); +} + +uint16_t adc_get_mV(const adc_signal_t *signal) { + uint16_t ret = 0; + + if (adc_avdd_mV == 0U) { + adc_calibrate_vdda(); + } + + if ((signal->adc == ADC1) || (signal->adc == ADC2)) { + ret = (adc_get_raw(signal) * adc_avdd_mV) / 65535U; + } else if (signal->adc == ADC3) { + ret = (adc_get_raw(signal) * adc_avdd_mV) / 4095U; + } else {} + return ret; +} diff --git a/board/stm32h7/lladc.h b/board/stm32h7/lladc.h index 0a7e983c9ef..347bb0b7905 100644 --- a/board/stm32h7/lladc.h +++ b/board/stm32h7/lladc.h @@ -1,80 +1,47 @@ -#include "lladc_declarations.h" - -static uint32_t adc_avdd_mV = 0U; - -void adc_init(ADC_TypeDef *adc) { - adc->CR &= ~(ADC_CR_ADEN); // Disable ADC - adc->CR &= ~(ADC_CR_DEEPPWD); // Reset deep-power-down mode - adc->CR |= ADC_CR_ADVREGEN; // Enable ADC regulator - while(!(adc->ISR & ADC_ISR_LDORDY) && (adc != ADC3)); - - if (adc != ADC3) { - adc->CR &= ~(ADC_CR_ADCALDIF); // Choose single-ended calibration - adc->CR |= ADC_CR_ADCALLIN; // Lineriality calibration - } - adc->CR |= ADC_CR_ADCAL; // Start calibration - while((adc->CR & ADC_CR_ADCAL) != 0U); - - adc->ISR |= ADC_ISR_ADRDY; - adc->CR |= ADC_CR_ADEN; - while(!(adc->ISR & ADC_ISR_ADRDY)); -} - -uint16_t adc_get_raw(const adc_signal_t *signal) { - signal->adc->SQR1 &= ~(ADC_SQR1_L); - signal->adc->SQR1 = ((uint32_t) signal->channel << 6U); - - // sample time - if (signal->channel < 10U) { - signal->adc->SMPR1 = ((uint32_t) signal->sample_time << (signal->channel * 3U)); - } else { - signal->adc->SMPR2 = ((uint32_t) signal->sample_time << ((signal->channel - 10U) * 3U)); - } - - // select channel - signal->adc->PCSEL_RES0 = (0x1UL << signal->channel); - - // oversampling - signal->adc->CFGR2 = (((1U << (uint32_t) signal->oversampling) - 1U) << ADC_CFGR2_OVSR_Pos) | ((uint32_t) signal->oversampling << ADC_CFGR2_OVSS_Pos); - signal->adc->CFGR2 |= (signal->oversampling != OVERSAMPLING_1) ? ADC_CFGR2_ROVSE : 0U; - - // start conversion - signal->adc->CR |= ADC_CR_ADSTART; - while (!(signal->adc->ISR & ADC_ISR_EOC)); - - uint16_t res = signal->adc->DR; - - while (!(signal->adc->ISR & ADC_ISR_EOS)); - signal->adc->ISR |= ADC_ISR_EOS; - - return res; -} - -static void adc_calibrate_vdda(void) { - // ADC2 used for calibration - adc_init(ADC2); - - // enable VREFINT channel - ADC3_COMMON->CCR |= ADC_CCR_VREFEN; - SYSCFG->ADC2ALT |= SYSCFG_ADC2ALT_ADC2_ROUT1; - - // measure VREFINT and derive AVDD - uint16_t raw_vrefint = adc_get_raw(&(adc_signal_t){.adc = ADC2, .channel = 17U, .sample_time = SAMPLETIME_810_CYCLES, .oversampling = OVERSAMPLING_256}); - adc_avdd_mV = (uint32_t) *VREFINT_CAL_ADDR * 16U * 3300U / raw_vrefint; - print(" AVDD: 0x"); puth(adc_avdd_mV); print(" mV\n"); -} - -uint16_t adc_get_mV(const adc_signal_t *signal) { - uint16_t ret = 0; - - if (adc_avdd_mV == 0U) { - adc_calibrate_vdda(); - } - - if ((signal->adc == ADC1) || (signal->adc == ADC2)) { - ret = (adc_get_raw(signal) * adc_avdd_mV) / 65535U; - } else if (signal->adc == ADC3) { - ret = (adc_get_raw(signal) * adc_avdd_mV) / 4095U; - } else {} - return ret; -} +#pragma once + +#include + +// #include "board/stm32h7/stm32h7_config.h" +// TODO: I don't know man... +#include "stm32h7xx.h" + +typedef enum { + SAMPLETIME_1_CYCLE = 0, + SAMPLETIME_2_CYCLES = 1, + SAMPLETIME_8_CYCLES = 2, + SAMPLETIME_16_CYCLES = 3, + SAMPLETIME_32_CYCLES = 4, + SAMPLETIME_64_CYCLES = 5, + SAMPLETIME_387_CYCLES = 6, + SAMPLETIME_810_CYCLES = 7 +} adc_sample_time_t; + +typedef enum { + OVERSAMPLING_1 = 0, + OVERSAMPLING_2 = 1, + OVERSAMPLING_4 = 2, + OVERSAMPLING_8 = 3, + OVERSAMPLING_16 = 4, + OVERSAMPLING_32 = 5, + OVERSAMPLING_64 = 6, + OVERSAMPLING_128 = 7, + OVERSAMPLING_256 = 8, + OVERSAMPLING_512 = 9, + OVERSAMPLING_1024 = 10 +} adc_oversampling_t; + +typedef struct { + ADC_TypeDef *adc; + uint8_t channel; + adc_sample_time_t sample_time; + adc_oversampling_t oversampling; +} adc_signal_t; + +#define ADC_CHANNEL_DEFAULT(a, c) {.adc = (a), .channel = (c), .sample_time = SAMPLETIME_32_CYCLES, .oversampling = OVERSAMPLING_64} + +#define VREFINT_CAL_ADDR ((uint16_t *)0x1FF1E860UL) + +void adc_init(ADC_TypeDef *adc); +uint16_t adc_get_raw(const adc_signal_t *signal); +uint16_t adc_get_mV(const adc_signal_t *signal); \ No newline at end of file diff --git a/board/stm32h7/lladc_declarations.h b/board/stm32h7/lladc_declarations.h deleted file mode 100644 index 979919c8c98..00000000000 --- a/board/stm32h7/lladc_declarations.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -typedef enum { - SAMPLETIME_1_CYCLE = 0, - SAMPLETIME_2_CYCLES = 1, - SAMPLETIME_8_CYCLES = 2, - SAMPLETIME_16_CYCLES = 3, - SAMPLETIME_32_CYCLES = 4, - SAMPLETIME_64_CYCLES = 5, - SAMPLETIME_387_CYCLES = 6, - SAMPLETIME_810_CYCLES = 7 -} adc_sample_time_t; - -typedef enum { - OVERSAMPLING_1 = 0, - OVERSAMPLING_2 = 1, - OVERSAMPLING_4 = 2, - OVERSAMPLING_8 = 3, - OVERSAMPLING_16 = 4, - OVERSAMPLING_32 = 5, - OVERSAMPLING_64 = 6, - OVERSAMPLING_128 = 7, - OVERSAMPLING_256 = 8, - OVERSAMPLING_512 = 9, - OVERSAMPLING_1024 = 10 -} adc_oversampling_t; - -typedef struct { - ADC_TypeDef *adc; - uint8_t channel; - adc_sample_time_t sample_time; - adc_oversampling_t oversampling; -} adc_signal_t; - -#define ADC_CHANNEL_DEFAULT(a, c) {.adc = (a), .channel = (c), .sample_time = SAMPLETIME_32_CYCLES, .oversampling = OVERSAMPLING_64} - -#define VREFINT_CAL_ADDR ((uint16_t *)0x1FF1E860UL) diff --git a/board/stm32h7/llfan.c b/board/stm32h7/llfan.c new file mode 100644 index 00000000000..89f1f45d308 --- /dev/null +++ b/board/stm32h7/llfan.c @@ -0,0 +1,31 @@ +#include "llfan.h" + +#include "stm32h7xx.h" + +#include "board/drivers/interrupts.h" +#include "board/drivers/fan.h" +#include "board/drivers/pwm.h" + +// TACH interrupt handler +static void EXTI2_IRQ_Handler(void) { + volatile unsigned int pr = EXTI->PR1 & (1U << 2); + if ((pr & (1U << 2)) != 0U) { + fan_state.tach_counter++; + } + EXTI->PR1 = (1U << 2); +} + +void llfan_init(void) { + // 12000RPM * 4 tach edges / 60 seconds + REGISTER_INTERRUPT(EXTI2_IRQn, EXTI2_IRQ_Handler, 1000U, FAULT_INTERRUPT_RATE_TACH) + + // Init PWM speed control + pwm_init(TIM3, 3); + + // Init TACH interrupt + register_set(&(SYSCFG->EXTICR[0]), SYSCFG_EXTICR1_EXTI2_PD, 0xF00U); + register_set_bits(&(EXTI->IMR1), (1U << 2)); + register_set_bits(&(EXTI->RTSR1), (1U << 2)); + register_set_bits(&(EXTI->FTSR1), (1U << 2)); + NVIC_EnableIRQ(EXTI2_IRQn); +} diff --git a/board/stm32h7/llfan.h b/board/stm32h7/llfan.h index 06309d0633f..5294dee230c 100644 --- a/board/stm32h7/llfan.h +++ b/board/stm32h7/llfan.h @@ -1,23 +1,3 @@ -// TACH interrupt handler -static void EXTI2_IRQ_Handler(void) { - volatile unsigned int pr = EXTI->PR1 & (1U << 2); - if ((pr & (1U << 2)) != 0U) { - fan_state.tach_counter++; - } - EXTI->PR1 = (1U << 2); -} +#pragma once -void llfan_init(void) { - // 12000RPM * 4 tach edges / 60 seconds - REGISTER_INTERRUPT(EXTI2_IRQn, EXTI2_IRQ_Handler, 1000U, FAULT_INTERRUPT_RATE_TACH) - - // Init PWM speed control - pwm_init(TIM3, 3); - - // Init TACH interrupt - register_set(&(SYSCFG->EXTICR[0]), SYSCFG_EXTICR1_EXTI2_PD, 0xF00U); - register_set_bits(&(EXTI->IMR1), (1U << 2)); - register_set_bits(&(EXTI->RTSR1), (1U << 2)); - register_set_bits(&(EXTI->FTSR1), (1U << 2)); - NVIC_EnableIRQ(EXTI2_IRQn); -} +void llfan_init(void); \ No newline at end of file diff --git a/board/stm32h7/llfdcan.c b/board/stm32h7/llfdcan.c new file mode 100644 index 00000000000..609c56f1b76 --- /dev/null +++ b/board/stm32h7/llfdcan.c @@ -0,0 +1,227 @@ +#include "llfdcan.h" + +// kbps multiplied by 10 +const uint32_t speeds[SPEEDS_ARRAY_SIZE] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U}; +const uint32_t data_speeds[DATA_SPEEDS_ARRAY_SIZE] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U, 20000U, 50000U}; + +static bool fdcan_request_init(FDCAN_GlobalTypeDef *FDCANx) { + bool ret = true; + // Exit from sleep mode + FDCANx->CCCR &= ~(FDCAN_CCCR_CSR); + while ((FDCANx->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA); + + // Request init + uint32_t timeout_counter = 0U; + FDCANx->CCCR |= FDCAN_CCCR_INIT; + while ((FDCANx->CCCR & FDCAN_CCCR_INIT) == 0U) { + // Delay for about 1ms + delay(10000); + timeout_counter++; + + if (timeout_counter >= CAN_INIT_TIMEOUT_MS){ + ret = false; + break; + } + } + return ret; +} + +static bool fdcan_exit_init(FDCAN_GlobalTypeDef *FDCANx) { + bool ret = true; + + FDCANx->CCCR &= ~(FDCAN_CCCR_INIT); + uint32_t timeout_counter = 0U; + while ((FDCANx->CCCR & FDCAN_CCCR_INIT) != 0U) { + // Delay for about 1ms + delay(10000); + timeout_counter++; + + if (timeout_counter >= CAN_INIT_TIMEOUT_MS) { + ret = false; + break; + } + } + return ret; +} + +bool llcan_set_speed(FDCAN_GlobalTypeDef *FDCANx, uint32_t speed, uint32_t data_speed, bool non_iso, bool loopback, bool silent) { + UNUSED(speed); + bool ret = fdcan_request_init(FDCANx); + + if (ret) { + // Enable config change + FDCANx->CCCR |= FDCAN_CCCR_CCE; + + //Reset operation mode to Normal + FDCANx->CCCR &= ~(FDCAN_CCCR_TEST); + FDCANx->TEST &= ~(FDCAN_TEST_LBCK); + FDCANx->CCCR &= ~(FDCAN_CCCR_MON); + FDCANx->CCCR &= ~(FDCAN_CCCR_ASM); + FDCANx->CCCR &= ~(FDCAN_CCCR_NISO); + + // TODO: add as a separate safety mode + // Enable ASM restricted operation(for debug or automatic bitrate switching) + //FDCANx->CCCR |= FDCAN_CCCR_ASM; + + uint8_t prescaler = BITRATE_PRESCALER; + if (speed < 2500U) { + // The only way to support speeds lower than 250Kbit/s (down to 10Kbit/s) + prescaler = BITRATE_PRESCALER * 16U; + } + + // Set the nominal bit timing values + uint32_t tq = CAN_QUANTA(speed, prescaler); + uint32_t sp = CAN_SP_NOMINAL; + uint32_t seg1 = CAN_SEG1(tq, sp); + uint32_t seg2 = CAN_SEG2(tq, sp); + uint8_t sjw = MIN(127U, seg2); + + FDCANx->NBTP = (((sjw & 0x7FUL)-1U)<DBTP = (((sjw & 0xFUL)-1U)<CCCR |= FDCAN_CCCR_NISO; + } + + // Silent loopback is known as internal loopback in the docs + if (loopback) { + FDCANx->CCCR |= FDCAN_CCCR_TEST; + FDCANx->TEST |= FDCAN_TEST_LBCK; + FDCANx->CCCR |= FDCAN_CCCR_MON; + } + // Silent is known as bus monitoring in the docs + if (silent) { + FDCANx->CCCR |= FDCAN_CCCR_MON; + } + ret = fdcan_exit_init(FDCANx); + if (!ret) { + print(CAN_NAME_FROM_CANIF(FDCANx)); print(" set_speed timed out! (2)\n"); + } + } else { + print(CAN_NAME_FROM_CANIF(FDCANx)); print(" set_speed timed out! (1)\n"); + } + return ret; +} + +void llcan_irq_disable(const FDCAN_GlobalTypeDef *FDCANx) { + if (FDCANx == FDCAN1) { + NVIC_DisableIRQ(FDCAN1_IT0_IRQn); + NVIC_DisableIRQ(FDCAN1_IT1_IRQn); + } else if (FDCANx == FDCAN2) { + NVIC_DisableIRQ(FDCAN2_IT0_IRQn); + NVIC_DisableIRQ(FDCAN2_IT1_IRQn); + } else if (FDCANx == FDCAN3) { + NVIC_DisableIRQ(FDCAN3_IT0_IRQn); + NVIC_DisableIRQ(FDCAN3_IT1_IRQn); + } else { + } +} + +void llcan_irq_enable(const FDCAN_GlobalTypeDef *FDCANx) { + if (FDCANx == FDCAN1) { + NVIC_EnableIRQ(FDCAN1_IT0_IRQn); + NVIC_EnableIRQ(FDCAN1_IT1_IRQn); + } else if (FDCANx == FDCAN2) { + NVIC_EnableIRQ(FDCAN2_IT0_IRQn); + NVIC_EnableIRQ(FDCAN2_IT1_IRQn); + } else if (FDCANx == FDCAN3) { + NVIC_EnableIRQ(FDCAN3_IT0_IRQn); + NVIC_EnableIRQ(FDCAN3_IT1_IRQn); + } else { + } +} + +bool llcan_init(FDCAN_GlobalTypeDef *FDCANx) { + uint32_t can_number = CAN_NUM_FROM_CANIF(FDCANx); + bool ret = fdcan_request_init(FDCANx); + + if (ret) { + // Enable config change + FDCANx->CCCR |= FDCAN_CCCR_CCE; + // Enable automatic retransmission + FDCANx->CCCR &= ~(FDCAN_CCCR_DAR); + // Enable transmission pause feature + FDCANx->CCCR |= FDCAN_CCCR_TXP; + // Disable protocol exception handling + FDCANx->CCCR |= FDCAN_CCCR_PXHD; + // FD with BRS + FDCANx->CCCR |= (FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE); + + // Set TX mode to FIFO + FDCANx->TXBC &= ~(FDCAN_TXBC_TFQM); + // Configure TX element data size + FDCANx->TXESC |= 0x7U << FDCAN_TXESC_TBDS_Pos; // 64 bytes + //Configure RX FIFO0 element data size + FDCANx->RXESC |= 0x7U << FDCAN_RXESC_F0DS_Pos; + // Disable filtering, accept all valid frames received + FDCANx->XIDFC &= ~(FDCAN_XIDFC_LSE); // No extended filters + FDCANx->SIDFC &= ~(FDCAN_SIDFC_LSS); // No standard filters + FDCANx->GFC &= ~(FDCAN_GFC_RRFE); // Accept extended remote frames + FDCANx->GFC &= ~(FDCAN_GFC_RRFS); // Accept standard remote frames + FDCANx->GFC &= ~(FDCAN_GFC_ANFE); // Accept extended frames to FIFO 0 + FDCANx->GFC &= ~(FDCAN_GFC_ANFS); // Accept standard frames to FIFO 0 + + uint32_t RxFIFO0SA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET); + uint32_t TxFIFOSA = RxFIFO0SA + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE); + + // RX FIFO 0 + FDCANx->RXF0C |= (FDCAN_RX_FIFO_0_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_RXF0C_F0SA_Pos; + FDCANx->RXF0C |= FDCAN_RX_FIFO_0_EL_CNT << FDCAN_RXF0C_F0S_Pos; + // RX FIFO 0 switch to non-blocking (overwrite) mode + FDCANx->RXF0C |= FDCAN_RXF0C_F0OM; + + // TX FIFO (mode set earlier) + FDCANx->TXBC |= (FDCAN_TX_FIFO_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_TXBC_TBSA_Pos; + FDCANx->TXBC |= FDCAN_TX_FIFO_EL_CNT << FDCAN_TXBC_TFQS_Pos; + + // Flush allocated RAM + uint32_t EndAddress = TxFIFOSA + (FDCAN_TX_FIFO_EL_CNT * FDCAN_TX_FIFO_EL_SIZE); + for (uint32_t RAMcounter = RxFIFO0SA; RAMcounter < EndAddress; RAMcounter += 4U) { + *(uint32_t *)(RAMcounter) = 0x00000000; + } + + // Enable both interrupts for each module + FDCANx->ILE = (FDCAN_ILE_EINT0 | FDCAN_ILE_EINT1); + + FDCANx->IE &= 0x0U; // Reset all interrupts + // Messages for INT0 + FDCANx->IE |= FDCAN_IE_RF0NE; // Rx FIFO 0 new message + FDCANx->IE |= FDCAN_IE_PEDE | FDCAN_IE_PEAE | FDCAN_IE_BOE | FDCAN_IE_EPE | FDCAN_IE_RF0LE; + + // Messages for INT1 (Only TFE works??) + FDCANx->ILS |= FDCAN_ILS_TFEL; + FDCANx->IE |= FDCAN_IE_TFEE; // Tx FIFO empty + + ret = fdcan_exit_init(FDCANx); + if(!ret) { + print(CAN_NAME_FROM_CANIF(FDCANx)); print(" llcan_init timed out (2)!\n"); + } + + llcan_irq_enable(FDCANx); + + } else { + print(CAN_NAME_FROM_CANIF(FDCANx)); print(" llcan_init timed out (1)!\n"); + } + return ret; +} + +void llcan_clear_send(FDCAN_GlobalTypeDef *FDCANx) { + // from datasheet: "Transmit cancellation is not intended for Tx FIFO operation." + // so we need to clear pending transmission manually by resetting FDCAN core + FDCANx->IR |= 0x3FCFFFFFU; // clear all interrupts + bool ret = llcan_init(FDCANx); + UNUSED(ret); +} diff --git a/board/stm32h7/llfdcan.h b/board/stm32h7/llfdcan.h index 64a40bd072e..ebd53c0de45 100644 --- a/board/stm32h7/llfdcan.h +++ b/board/stm32h7/llfdcan.h @@ -1,5 +1,64 @@ -#include "llfdcan_declarations.h" +#pragma once +#include +#include + +#include "board/config.h" + +// SAE J2284-4 document specifies a bus-line network running at 2 Mbit/s +// SAE J2284-5 document specifies a point-to-point communication running at 5 Mbit/s + +#define CAN_PCLK 80000U // KHz, sourced from PLL1Q +#define BITRATE_PRESCALER 2U // Valid from 250Kbps to 5Mbps with 80Mhz clock +#define CAN_SP_NOMINAL 80U // 80% for both SAE J2284-4 and SAE J2284-5 +#define CAN_SP_DATA_2M 80U // 80% for SAE J2284-4 +#define CAN_SP_DATA_5M 75U // 75% for SAE J2284-5 +#define CAN_QUANTA(speed, prescaler) (CAN_PCLK / ((speed) / 10U * (prescaler))) +#define CAN_SEG1(tq, sp) (((tq) * (sp) / 100U)- 1U) +#define CAN_SEG2(tq, sp) ((tq) * (100U - (sp)) / 100U) + +// FDCAN core settings +#define FDCAN_START_ADDRESS 0x4000AC00UL +#define FDCAN_OFFSET 3384UL // bytes for each FDCAN module, equally +#define FDCAN_OFFSET_W 846UL // words for each FDCAN module, equally + +// FDCAN_RX_FIFO_0_EL_CNT + FDCAN_TX_FIFO_EL_CNT can't exceed 47 elements (47 * 72 bytes = 3,384 bytes) per FDCAN module + +// RX FIFO 0 +#define FDCAN_RX_FIFO_0_EL_CNT 46UL +#define FDCAN_RX_FIFO_0_HEAD_SIZE 8UL // bytes +#define FDCAN_RX_FIFO_0_DATA_SIZE 64UL // bytes +#define FDCAN_RX_FIFO_0_EL_SIZE (FDCAN_RX_FIFO_0_HEAD_SIZE + FDCAN_RX_FIFO_0_DATA_SIZE) +#define FDCAN_RX_FIFO_0_EL_W_SIZE (FDCAN_RX_FIFO_0_EL_SIZE / 4UL) +#define FDCAN_RX_FIFO_0_OFFSET 0UL + +// TX FIFO +#define FDCAN_TX_FIFO_EL_CNT 1UL +#define FDCAN_TX_FIFO_HEAD_SIZE 8UL // bytes +#define FDCAN_TX_FIFO_DATA_SIZE 64UL // bytes +#define FDCAN_TX_FIFO_EL_SIZE (FDCAN_TX_FIFO_HEAD_SIZE + FDCAN_TX_FIFO_DATA_SIZE) +#define FDCAN_TX_FIFO_OFFSET (FDCAN_RX_FIFO_0_OFFSET + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_W_SIZE)) + +#define CAN_NAME_FROM_CANIF(CAN_DEV) (((CAN_DEV)==FDCAN1) ? "FDCAN1" : (((CAN_DEV) == FDCAN2) ? "FDCAN2" : "FDCAN3")) +#define CAN_NUM_FROM_CANIF(CAN_DEV) (((CAN_DEV)==FDCAN1) ? 0UL : (((CAN_DEV) == FDCAN2) ? 1UL : 2UL)) + + +void print(const char *a); + +// kbps multiplied by 10 +#define SPEEDS_ARRAY_SIZE 8 +extern const uint32_t speeds[SPEEDS_ARRAY_SIZE]; +#define DATA_SPEEDS_ARRAY_SIZE 10 +extern const uint32_t data_speeds[DATA_SPEEDS_ARRAY_SIZE]; + +bool llcan_set_speed(FDCAN_GlobalTypeDef *FDCANx, uint32_t speed, uint32_t data_speed, bool non_iso, bool loopback, bool silent); +void llcan_irq_disable(const FDCAN_GlobalTypeDef *FDCANx); +void llcan_irq_enable(const FDCAN_GlobalTypeDef *FDCANx); +bool llcan_init(FDCAN_GlobalTypeDef *FDCANx); +void llcan_clear_send(FDCAN_GlobalTypeDef *FDCANx); + + +/* // kbps multiplied by 10 const uint32_t speeds[SPEEDS_ARRAY_SIZE] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U}; const uint32_t data_speeds[DATA_SPEEDS_ARRAY_SIZE] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U, 20000U, 50000U}; @@ -225,3 +284,4 @@ void llcan_clear_send(FDCAN_GlobalTypeDef *FDCANx) { bool ret = llcan_init(FDCANx); UNUSED(ret); } +*/ \ No newline at end of file diff --git a/board/stm32h7/llfdcan_declarations.h b/board/stm32h7/llfdcan_declarations.h deleted file mode 100644 index 793849011eb..00000000000 --- a/board/stm32h7/llfdcan_declarations.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -// SAE J2284-4 document specifies a bus-line network running at 2 Mbit/s -// SAE J2284-5 document specifies a point-to-point communication running at 5 Mbit/s - -#define CAN_PCLK 80000U // KHz, sourced from PLL1Q -#define BITRATE_PRESCALER 2U // Valid from 250Kbps to 5Mbps with 80Mhz clock -#define CAN_SP_NOMINAL 80U // 80% for both SAE J2284-4 and SAE J2284-5 -#define CAN_SP_DATA_2M 80U // 80% for SAE J2284-4 -#define CAN_SP_DATA_5M 75U // 75% for SAE J2284-5 -#define CAN_QUANTA(speed, prescaler) (CAN_PCLK / ((speed) / 10U * (prescaler))) -#define CAN_SEG1(tq, sp) (((tq) * (sp) / 100U)- 1U) -#define CAN_SEG2(tq, sp) ((tq) * (100U - (sp)) / 100U) - -// FDCAN core settings -#define FDCAN_START_ADDRESS 0x4000AC00UL -#define FDCAN_OFFSET 3384UL // bytes for each FDCAN module, equally -#define FDCAN_OFFSET_W 846UL // words for each FDCAN module, equally - -// FDCAN_RX_FIFO_0_EL_CNT + FDCAN_TX_FIFO_EL_CNT can't exceed 47 elements (47 * 72 bytes = 3,384 bytes) per FDCAN module - -// RX FIFO 0 -#define FDCAN_RX_FIFO_0_EL_CNT 46UL -#define FDCAN_RX_FIFO_0_HEAD_SIZE 8UL // bytes -#define FDCAN_RX_FIFO_0_DATA_SIZE 64UL // bytes -#define FDCAN_RX_FIFO_0_EL_SIZE (FDCAN_RX_FIFO_0_HEAD_SIZE + FDCAN_RX_FIFO_0_DATA_SIZE) -#define FDCAN_RX_FIFO_0_EL_W_SIZE (FDCAN_RX_FIFO_0_EL_SIZE / 4UL) -#define FDCAN_RX_FIFO_0_OFFSET 0UL - -// TX FIFO -#define FDCAN_TX_FIFO_EL_CNT 1UL -#define FDCAN_TX_FIFO_HEAD_SIZE 8UL // bytes -#define FDCAN_TX_FIFO_DATA_SIZE 64UL // bytes -#define FDCAN_TX_FIFO_EL_SIZE (FDCAN_TX_FIFO_HEAD_SIZE + FDCAN_TX_FIFO_DATA_SIZE) -#define FDCAN_TX_FIFO_OFFSET (FDCAN_RX_FIFO_0_OFFSET + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_W_SIZE)) - -#define CAN_NAME_FROM_CANIF(CAN_DEV) (((CAN_DEV)==FDCAN1) ? "FDCAN1" : (((CAN_DEV) == FDCAN2) ? "FDCAN2" : "FDCAN3")) -#define CAN_NUM_FROM_CANIF(CAN_DEV) (((CAN_DEV)==FDCAN1) ? 0UL : (((CAN_DEV) == FDCAN2) ? 1UL : 2UL)) - - -void print(const char *a); - -// kbps multiplied by 10 -#define SPEEDS_ARRAY_SIZE 8 -extern const uint32_t speeds[SPEEDS_ARRAY_SIZE]; -#define DATA_SPEEDS_ARRAY_SIZE 10 -extern const uint32_t data_speeds[DATA_SPEEDS_ARRAY_SIZE]; - -bool llcan_set_speed(FDCAN_GlobalTypeDef *FDCANx, uint32_t speed, uint32_t data_speed, bool non_iso, bool loopback, bool silent); -void llcan_irq_disable(const FDCAN_GlobalTypeDef *FDCANx); -void llcan_irq_enable(const FDCAN_GlobalTypeDef *FDCANx); -bool llcan_init(FDCAN_GlobalTypeDef *FDCANx); -void llcan_clear_send(FDCAN_GlobalTypeDef *FDCANx); diff --git a/board/stm32h7/llflash.c b/board/stm32h7/llflash.c new file mode 100644 index 00000000000..21320632c97 --- /dev/null +++ b/board/stm32h7/llflash.c @@ -0,0 +1,37 @@ +#include "llflash.h" + +#include "stm32h7_config.h" + +bool flash_is_locked(void) { + return (FLASH->CR1 & FLASH_CR_LOCK); +} + +void flash_unlock(void) { + FLASH->KEYR1 = 0x45670123; + FLASH->KEYR1 = 0xCDEF89AB; +} + +bool flash_erase_sector(uint8_t sector, bool unlocked) { + // don't erase the bootloader(sector 0) + if (sector != 0 && sector < 8 && unlocked) { + FLASH->CR1 = (sector << 8) | FLASH_CR_SER; + FLASH->CR1 |= FLASH_CR_START; + while (FLASH->SR1 & FLASH_SR_QW); + return true; + } + return false; +} + +void flash_write_word(void *prog_ptr, uint32_t data) { + uint32_t *pp = prog_ptr; + FLASH->CR1 |= FLASH_CR_PG; + *pp = data; + while (FLASH->SR1 & FLASH_SR_QW); +} + +void flush_write_buffer(void) { + if (FLASH->SR1 & FLASH_SR_WBNE) { + FLASH->CR1 |= FLASH_CR_FW; + while (FLASH->SR1 & FLASH_CR_FW); + } +} diff --git a/board/stm32h7/llflash.h b/board/stm32h7/llflash.h index b95011a9ed8..e56a2f600a5 100644 --- a/board/stm32h7/llflash.h +++ b/board/stm32h7/llflash.h @@ -1,33 +1,10 @@ -bool flash_is_locked(void) { - return (FLASH->CR1 & FLASH_CR_LOCK); -} +#pragma once -void flash_unlock(void) { - FLASH->KEYR1 = 0x45670123; - FLASH->KEYR1 = 0xCDEF89AB; -} +#include +#include -bool flash_erase_sector(uint8_t sector, bool unlocked) { - // don't erase the bootloader(sector 0) - if (sector != 0 && sector < 8 && unlocked) { - FLASH->CR1 = (sector << 8) | FLASH_CR_SER; - FLASH->CR1 |= FLASH_CR_START; - while (FLASH->SR1 & FLASH_SR_QW); - return true; - } - return false; -} - -void flash_write_word(void *prog_ptr, uint32_t data) { - uint32_t *pp = prog_ptr; - FLASH->CR1 |= FLASH_CR_PG; - *pp = data; - while (FLASH->SR1 & FLASH_SR_QW); -} - -void flush_write_buffer(void) { - if (FLASH->SR1 & FLASH_SR_WBNE) { - FLASH->CR1 |= FLASH_CR_FW; - while (FLASH->SR1 & FLASH_CR_FW); - } -} +bool flash_is_locked(void); +void flash_unlock(void); +bool flash_erase_sector(uint8_t sector, bool unlocked); +void flash_write_word(void *prog_ptr, uint32_t data); +void flush_write_buffer(void); \ No newline at end of file diff --git a/board/stm32h7/lli2c.c b/board/stm32h7/lli2c.c new file mode 100644 index 00000000000..bda5321c26c --- /dev/null +++ b/board/stm32h7/lli2c.c @@ -0,0 +1,169 @@ +// TODO: this driver relies heavily on polling, +// if we want it to be more async, we should use interrupts + +#include "lli2c.h" + +#define I2C_RETRY_COUNT 10U +#define I2C_TIMEOUT_US 100000U + +bool i2c_status_wait(const volatile uint32_t *reg, uint32_t mask, uint32_t val) { + uint32_t start_time = microsecond_timer_get(); + while(((*reg & mask) != val) && (get_ts_elapsed(microsecond_timer_get(), start_time) < I2C_TIMEOUT_US)); + return ((*reg & mask) == val); +} + +void i2c_reset(I2C_TypeDef *I2C) { + // peripheral reset + register_clear_bits(&I2C->CR1, I2C_CR1_PE); + while ((I2C->CR1 & I2C_CR1_PE) != 0U); + register_set_bits(&I2C->CR1, I2C_CR1_PE); +} + +bool i2c_write_reg(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t value) { + bool ret = false; + + // Setup transfer and send START + addr + for (uint32_t i = 0U; i < I2C_RETRY_COUNT; i++) { + register_clear_bits(&I2C->CR2, I2C_CR2_ADD10); + I2C->CR2 = ((uint32_t)addr << 1U) & I2C_CR2_SADD_Msk; + register_clear_bits(&I2C->CR2, I2C_CR2_RD_WRN); + register_set_bits(&I2C->CR2, I2C_CR2_AUTOEND); + I2C->CR2 |= 2UL << I2C_CR2_NBYTES_Pos; + + I2C->CR2 |= I2C_CR2_START; + if(!i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U)) { + continue; + } + + // check if we lost arbitration + if ((I2C->ISR & I2C_ISR_ARLO) != 0U) { + register_set_bits(&I2C->ICR, I2C_ICR_ARLOCF); + } else { + ret = true; + break; + } + } + + if (!ret) { + goto end; + } + + // Send data + ret = i2c_status_wait(&I2C->ISR, I2C_ISR_TXIS, I2C_ISR_TXIS); + if(!ret) { + goto end; + } + I2C->TXDR = reg; + + ret = i2c_status_wait(&I2C->ISR, I2C_ISR_TXIS, I2C_ISR_TXIS); + if(!ret) { + goto end; + } + I2C->TXDR = value; + +end: + return ret; +} + +bool i2c_read_reg(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t *value) { + bool ret = false; + + // Setup transfer and send START + addr + for (uint32_t i = 0U; i < I2C_RETRY_COUNT; i++) { + register_clear_bits(&I2C->CR2, I2C_CR2_ADD10); + I2C->CR2 = ((uint32_t)addr << 1U) & I2C_CR2_SADD_Msk; + register_clear_bits(&I2C->CR2, I2C_CR2_RD_WRN); + register_clear_bits(&I2C->CR2, I2C_CR2_AUTOEND); + I2C->CR2 |= 1UL << I2C_CR2_NBYTES_Pos; + + I2C->CR2 |= I2C_CR2_START; + if(!i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U)) { + continue; + } + + // check if we lost arbitration + if ((I2C->ISR & I2C_ISR_ARLO) != 0U) { + register_set_bits(&I2C->ICR, I2C_ICR_ARLOCF); + } else { + ret = true; + break; + } + } + + if (!ret) { + goto end; + } + + // Send data + ret = i2c_status_wait(&I2C->ISR, I2C_ISR_TXIS, I2C_ISR_TXIS); + if(!ret) { + goto end; + } + I2C->TXDR = reg; + + // Restart + I2C->CR2 = (((addr << 1) | 0x1U) & I2C_CR2_SADD_Msk) | (1UL << I2C_CR2_NBYTES_Pos) | I2C_CR2_RD_WRN | I2C_CR2_START; + ret = i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U); + if(!ret) { + goto end; + } + + // check if we lost arbitration + if ((I2C->ISR & I2C_ISR_ARLO) != 0U) { + register_set_bits(&I2C->ICR, I2C_ICR_ARLOCF); + ret = false; + goto end; + } + + // Read data + ret = i2c_status_wait(&I2C->ISR, I2C_ISR_RXNE, I2C_ISR_RXNE); + if(!ret) { + goto end; + } + *value = I2C->RXDR; + + // Stop + I2C->CR2 |= I2C_CR2_STOP; + +end: + + if (!ret) { + i2c_reset(I2C); + } + + return ret; +} + +bool i2c_set_reg_bits(I2C_TypeDef *I2C, uint8_t address, uint8_t regis, uint8_t bits) { + uint8_t value; + bool ret = i2c_read_reg(I2C, address, regis, &value); + if(ret) { + ret = i2c_write_reg(I2C, address, regis, value | bits); + } + return ret; +} + +bool i2c_clear_reg_bits(I2C_TypeDef *I2C, uint8_t address, uint8_t regis, uint8_t bits) { + uint8_t value; + bool ret = i2c_read_reg(I2C, address, regis, &value); + if (ret) { + ret = i2c_write_reg(I2C, address, regis, value & (uint8_t) (~bits)); + } + return ret; +} + +bool i2c_set_reg_mask(I2C_TypeDef *I2C, uint8_t address, uint8_t regis, uint8_t value, uint8_t mask) { + uint8_t old_value; + bool ret = i2c_read_reg(I2C, address, regis, &old_value); + if(ret) { + ret = i2c_write_reg(I2C, address, regis, (old_value & (uint8_t) (~mask)) | (value & mask)); + } + return ret; +} + +void i2c_init(I2C_TypeDef *I2C) { + // 100kHz clock speed + I2C->TIMINGR = 0x107075B0; + + i2c_reset(I2C); +} diff --git a/board/stm32h7/lli2c.h b/board/stm32h7/lli2c.h index 5d79beb653e..918c1265ce7 100644 --- a/board/stm32h7/lli2c.h +++ b/board/stm32h7/lli2c.h @@ -1,167 +1,22 @@ // TODO: this driver relies heavily on polling, // if we want it to be more async, we should use interrupts -#define I2C_RETRY_COUNT 10U -#define I2C_TIMEOUT_US 100000U - -bool i2c_status_wait(const volatile uint32_t *reg, uint32_t mask, uint32_t val) { - uint32_t start_time = microsecond_timer_get(); - while(((*reg & mask) != val) && (get_ts_elapsed(microsecond_timer_get(), start_time) < I2C_TIMEOUT_US)); - return ((*reg & mask) == val); -} - -void i2c_reset(I2C_TypeDef *I2C) { - // peripheral reset - register_clear_bits(&I2C->CR1, I2C_CR1_PE); - while ((I2C->CR1 & I2C_CR1_PE) != 0U); - register_set_bits(&I2C->CR1, I2C_CR1_PE); -} - -bool i2c_write_reg(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t value) { - bool ret = false; - - // Setup transfer and send START + addr - for (uint32_t i = 0U; i < I2C_RETRY_COUNT; i++) { - register_clear_bits(&I2C->CR2, I2C_CR2_ADD10); - I2C->CR2 = ((uint32_t)addr << 1U) & I2C_CR2_SADD_Msk; - register_clear_bits(&I2C->CR2, I2C_CR2_RD_WRN); - register_set_bits(&I2C->CR2, I2C_CR2_AUTOEND); - I2C->CR2 |= 2UL << I2C_CR2_NBYTES_Pos; - - I2C->CR2 |= I2C_CR2_START; - if(!i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U)) { - continue; - } - - // check if we lost arbitration - if ((I2C->ISR & I2C_ISR_ARLO) != 0U) { - register_set_bits(&I2C->ICR, I2C_ICR_ARLOCF); - } else { - ret = true; - break; - } - } - - if (!ret) { - goto end; - } - - // Send data - ret = i2c_status_wait(&I2C->ISR, I2C_ISR_TXIS, I2C_ISR_TXIS); - if(!ret) { - goto end; - } - I2C->TXDR = reg; - - ret = i2c_status_wait(&I2C->ISR, I2C_ISR_TXIS, I2C_ISR_TXIS); - if(!ret) { - goto end; - } - I2C->TXDR = value; - -end: - return ret; -} - -bool i2c_read_reg(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t *value) { - bool ret = false; - - // Setup transfer and send START + addr - for (uint32_t i = 0U; i < I2C_RETRY_COUNT; i++) { - register_clear_bits(&I2C->CR2, I2C_CR2_ADD10); - I2C->CR2 = ((uint32_t)addr << 1U) & I2C_CR2_SADD_Msk; - register_clear_bits(&I2C->CR2, I2C_CR2_RD_WRN); - register_clear_bits(&I2C->CR2, I2C_CR2_AUTOEND); - I2C->CR2 |= 1UL << I2C_CR2_NBYTES_Pos; +#pragma once - I2C->CR2 |= I2C_CR2_START; - if(!i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U)) { - continue; - } +#include +#include - // check if we lost arbitration - if ((I2C->ISR & I2C_ISR_ARLO) != 0U) { - register_set_bits(&I2C->ICR, I2C_ICR_ARLOCF); - } else { - ret = true; - break; - } - } +#include "stm32h7_config.h" - if (!ret) { - goto end; - } - - // Send data - ret = i2c_status_wait(&I2C->ISR, I2C_ISR_TXIS, I2C_ISR_TXIS); - if(!ret) { - goto end; - } - I2C->TXDR = reg; - - // Restart - I2C->CR2 = (((addr << 1) | 0x1U) & I2C_CR2_SADD_Msk) | (1UL << I2C_CR2_NBYTES_Pos) | I2C_CR2_RD_WRN | I2C_CR2_START; - ret = i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U); - if(!ret) { - goto end; - } - - // check if we lost arbitration - if ((I2C->ISR & I2C_ISR_ARLO) != 0U) { - register_set_bits(&I2C->ICR, I2C_ICR_ARLOCF); - ret = false; - goto end; - } - - // Read data - ret = i2c_status_wait(&I2C->ISR, I2C_ISR_RXNE, I2C_ISR_RXNE); - if(!ret) { - goto end; - } - *value = I2C->RXDR; - - // Stop - I2C->CR2 |= I2C_CR2_STOP; - -end: - - if (!ret) { - i2c_reset(I2C); - } - - return ret; -} - -bool i2c_set_reg_bits(I2C_TypeDef *I2C, uint8_t address, uint8_t regis, uint8_t bits) { - uint8_t value; - bool ret = i2c_read_reg(I2C, address, regis, &value); - if(ret) { - ret = i2c_write_reg(I2C, address, regis, value | bits); - } - return ret; -} - -bool i2c_clear_reg_bits(I2C_TypeDef *I2C, uint8_t address, uint8_t regis, uint8_t bits) { - uint8_t value; - bool ret = i2c_read_reg(I2C, address, regis, &value); - if (ret) { - ret = i2c_write_reg(I2C, address, regis, value & (uint8_t) (~bits)); - } - return ret; -} - -bool i2c_set_reg_mask(I2C_TypeDef *I2C, uint8_t address, uint8_t regis, uint8_t value, uint8_t mask) { - uint8_t old_value; - bool ret = i2c_read_reg(I2C, address, regis, &old_value); - if(ret) { - ret = i2c_write_reg(I2C, address, regis, (old_value & (uint8_t) (~mask)) | (value & mask)); - } - return ret; -} +#define I2C_RETRY_COUNT 10U +#define I2C_TIMEOUT_US 100000U -void i2c_init(I2C_TypeDef *I2C) { - // 100kHz clock speed - I2C->TIMINGR = 0x107075B0; +bool i2c_status_wait(const volatile uint32_t *reg, uint32_t mask, uint32_t val); +void i2c_reset(I2C_TypeDef *I2C); - i2c_reset(I2C); -} +bool i2c_write_reg(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t value); +bool i2c_read_reg(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t *value); +bool i2c_set_reg_bits(I2C_TypeDef *I2C, uint8_t address, uint8_t regis, uint8_t bits); +bool i2c_clear_reg_bits(I2C_TypeDef *I2C, uint8_t address, uint8_t regis, uint8_t bits); +bool i2c_set_reg_mask(I2C_TypeDef *I2C, uint8_t address, uint8_t regis, uint8_t value, uint8_t mask); +void i2c_init(I2C_TypeDef *I2C); \ No newline at end of file diff --git a/board/stm32h7/llspi.c b/board/stm32h7/llspi.c new file mode 100644 index 00000000000..01288fe99b5 --- /dev/null +++ b/board/stm32h7/llspi.c @@ -0,0 +1,111 @@ +#include "llspi.h" + +#include "board/drivers/registers.h" + +// master -> panda DMA start +void llspi_mosi_dma(uint8_t *addr, int len) { + // disable DMA + SPI + register_clear_bits(&(SPI4->CFG1), SPI_CFG1_RXDMAEN); + DMA2_Stream2->CR &= ~DMA_SxCR_EN; + register_clear_bits(&(SPI4->CR1), SPI_CR1_SPE); + + // drain the bus + while ((SPI4->SR & SPI_SR_RXP) != 0U) { + volatile uint8_t dat = SPI4->RXDR; + (void)dat; + } + + // clear all pending + SPI4->IFCR |= (0x1FFU << 3U); + register_set(&(SPI4->IER), 0, 0x3FFU); + + // setup destination and length + register_set(&(DMA2_Stream2->M0AR), (uint32_t)addr, 0xFFFFFFFFU); + DMA2_Stream2->NDTR = len; + + // enable DMA + SPI + DMA2_Stream2->CR |= DMA_SxCR_EN; + register_set_bits(&(SPI4->CFG1), SPI_CFG1_RXDMAEN); + register_set_bits(&(SPI4->CR1), SPI_CR1_SPE); +} + +// panda -> master DMA start +void llspi_miso_dma(uint8_t *addr, int len) { + // disable DMA + SPI + DMA2_Stream3->CR &= ~DMA_SxCR_EN; + register_clear_bits(&(SPI4->CFG1), SPI_CFG1_TXDMAEN); + register_clear_bits(&(SPI4->CR1), SPI_CR1_SPE); + + // setup source and length + register_set(&(DMA2_Stream3->M0AR), (uint32_t)addr, 0xFFFFFFFFU); + DMA2_Stream3->NDTR = len; + + // clear under-run while we were reading + SPI4->IFCR |= (0x1FFU << 3U); + + // setup interrupt on TXC + register_set(&(SPI4->IER), (1U << SPI_IER_EOTIE_Pos), 0x3FFU); + + // enable DMA + SPI + register_set_bits(&(SPI4->CFG1), SPI_CFG1_TXDMAEN); + DMA2_Stream3->CR |= DMA_SxCR_EN; + register_set_bits(&(SPI4->CR1), SPI_CR1_SPE); +} + +static bool spi_tx_dma_done = false; +// master -> panda DMA finished +static void DMA2_Stream2_IRQ_Handler(void) { + // Clear interrupt flag + DMA2->LIFCR = DMA_LIFCR_CTCIF2; + + spi_rx_done(); +} + +// panda -> master DMA finished +static void DMA2_Stream3_IRQ_Handler(void) { + ENTER_CRITICAL(); + + DMA2->LIFCR = DMA_LIFCR_CTCIF3; + spi_tx_dma_done = true; + + EXIT_CRITICAL(); +} + +// panda TX finished +static void SPI4_IRQ_Handler(void) { + // clear flag + SPI4->IFCR |= (0x1FFU << 3U); + + if (spi_tx_dma_done && ((SPI4->SR & SPI_SR_TXC) != 0U)) { + spi_tx_dma_done = false; + spi_tx_done(false); + } +} + + +void llspi_init(void) { + REGISTER_INTERRUPT(SPI4_IRQn, SPI4_IRQ_Handler, (SPI_IRQ_RATE * 2U), FAULT_INTERRUPT_RATE_SPI) + REGISTER_INTERRUPT(DMA2_Stream2_IRQn, DMA2_Stream2_IRQ_Handler, SPI_IRQ_RATE, FAULT_INTERRUPT_RATE_SPI_DMA) + REGISTER_INTERRUPT(DMA2_Stream3_IRQn, DMA2_Stream3_IRQ_Handler, SPI_IRQ_RATE, FAULT_INTERRUPT_RATE_SPI_DMA) + + // Setup MOSI DMA + register_set(&(DMAMUX1_Channel10->CCR), 83U, 0xFFFFFFFFU); + register_set(&(DMA2_Stream2->CR), (DMA_SxCR_MINC | DMA_SxCR_TCIE), 0x1E077EFEU); + register_set(&(DMA2_Stream2->PAR), (uint32_t)&(SPI4->RXDR), 0xFFFFFFFFU); + + // Setup MISO DMA, memory -> peripheral + register_set(&(DMAMUX1_Channel11->CCR), 84U, 0xFFFFFFFFU); + register_set(&(DMA2_Stream3->CR), (DMA_SxCR_MINC | DMA_SxCR_DIR_0 | DMA_SxCR_TCIE), 0x1E077EFEU); + register_set(&(DMA2_Stream3->PAR), (uint32_t)&(SPI4->TXDR), 0xFFFFFFFFU); + + // Enable SPI + register_set(&(SPI4->IER), 0, 0x3FFU); + register_set(&(SPI4->CFG1), (7U << SPI_CFG1_DSIZE_Pos), SPI_CFG1_DSIZE_Msk); + register_set(&(SPI4->UDRDR), 0xcd, 0xFFFFU); // set under-run value for debugging + register_set(&(SPI4->CR1), SPI_CR1_SPE, 0xFFFFU); + register_set(&(SPI4->CR2), 0, 0xFFFFU); + + NVIC_EnableIRQ(DMA2_Stream2_IRQn); + NVIC_EnableIRQ(DMA2_Stream3_IRQn); + NVIC_EnableIRQ(SPI4_IRQn); +} diff --git a/board/stm32h7/llspi.h b/board/stm32h7/llspi.h index 05f8e22f9a8..2e348dd1c49 100644 --- a/board/stm32h7/llspi.h +++ b/board/stm32h7/llspi.h @@ -1,107 +1,9 @@ -// master -> panda DMA start -void llspi_mosi_dma(uint8_t *addr, int len) { - // disable DMA + SPI - register_clear_bits(&(SPI4->CFG1), SPI_CFG1_RXDMAEN); - DMA2_Stream2->CR &= ~DMA_SxCR_EN; - register_clear_bits(&(SPI4->CR1), SPI_CR1_SPE); - - // drain the bus - while ((SPI4->SR & SPI_SR_RXP) != 0U) { - volatile uint8_t dat = SPI4->RXDR; - (void)dat; - } - - // clear all pending - SPI4->IFCR |= (0x1FFU << 3U); - register_set(&(SPI4->IER), 0, 0x3FFU); +#pragma once - // setup destination and length - register_set(&(DMA2_Stream2->M0AR), (uint32_t)addr, 0xFFFFFFFFU); - DMA2_Stream2->NDTR = len; - - // enable DMA + SPI - DMA2_Stream2->CR |= DMA_SxCR_EN; - register_set_bits(&(SPI4->CFG1), SPI_CFG1_RXDMAEN); - register_set_bits(&(SPI4->CR1), SPI_CR1_SPE); -} +#include +// master -> panda DMA start +void llspi_mosi_dma(uint8_t *addr, int len); // panda -> master DMA start -void llspi_miso_dma(uint8_t *addr, int len) { - // disable DMA + SPI - DMA2_Stream3->CR &= ~DMA_SxCR_EN; - register_clear_bits(&(SPI4->CFG1), SPI_CFG1_TXDMAEN); - register_clear_bits(&(SPI4->CR1), SPI_CR1_SPE); - - // setup source and length - register_set(&(DMA2_Stream3->M0AR), (uint32_t)addr, 0xFFFFFFFFU); - DMA2_Stream3->NDTR = len; - - // clear under-run while we were reading - SPI4->IFCR |= (0x1FFU << 3U); - - // setup interrupt on TXC - register_set(&(SPI4->IER), (1U << SPI_IER_EOTIE_Pos), 0x3FFU); - - // enable DMA + SPI - register_set_bits(&(SPI4->CFG1), SPI_CFG1_TXDMAEN); - DMA2_Stream3->CR |= DMA_SxCR_EN; - register_set_bits(&(SPI4->CR1), SPI_CR1_SPE); -} - -static bool spi_tx_dma_done = false; -// master -> panda DMA finished -static void DMA2_Stream2_IRQ_Handler(void) { - // Clear interrupt flag - DMA2->LIFCR = DMA_LIFCR_CTCIF2; - - spi_rx_done(); -} - -// panda -> master DMA finished -static void DMA2_Stream3_IRQ_Handler(void) { - ENTER_CRITICAL(); - - DMA2->LIFCR = DMA_LIFCR_CTCIF3; - spi_tx_dma_done = true; - - EXIT_CRITICAL(); -} - -// panda TX finished -static void SPI4_IRQ_Handler(void) { - // clear flag - SPI4->IFCR |= (0x1FFU << 3U); - - if (spi_tx_dma_done && ((SPI4->SR & SPI_SR_TXC) != 0U)) { - spi_tx_dma_done = false; - spi_tx_done(false); - } -} - - -void llspi_init(void) { - REGISTER_INTERRUPT(SPI4_IRQn, SPI4_IRQ_Handler, (SPI_IRQ_RATE * 2U), FAULT_INTERRUPT_RATE_SPI) - REGISTER_INTERRUPT(DMA2_Stream2_IRQn, DMA2_Stream2_IRQ_Handler, SPI_IRQ_RATE, FAULT_INTERRUPT_RATE_SPI_DMA) - REGISTER_INTERRUPT(DMA2_Stream3_IRQn, DMA2_Stream3_IRQ_Handler, SPI_IRQ_RATE, FAULT_INTERRUPT_RATE_SPI_DMA) - - // Setup MOSI DMA - register_set(&(DMAMUX1_Channel10->CCR), 83U, 0xFFFFFFFFU); - register_set(&(DMA2_Stream2->CR), (DMA_SxCR_MINC | DMA_SxCR_TCIE), 0x1E077EFEU); - register_set(&(DMA2_Stream2->PAR), (uint32_t)&(SPI4->RXDR), 0xFFFFFFFFU); - - // Setup MISO DMA, memory -> peripheral - register_set(&(DMAMUX1_Channel11->CCR), 84U, 0xFFFFFFFFU); - register_set(&(DMA2_Stream3->CR), (DMA_SxCR_MINC | DMA_SxCR_DIR_0 | DMA_SxCR_TCIE), 0x1E077EFEU); - register_set(&(DMA2_Stream3->PAR), (uint32_t)&(SPI4->TXDR), 0xFFFFFFFFU); - - // Enable SPI - register_set(&(SPI4->IER), 0, 0x3FFU); - register_set(&(SPI4->CFG1), (7U << SPI_CFG1_DSIZE_Pos), SPI_CFG1_DSIZE_Msk); - register_set(&(SPI4->UDRDR), 0xcd, 0xFFFFU); // set under-run value for debugging - register_set(&(SPI4->CR1), SPI_CR1_SPE, 0xFFFFU); - register_set(&(SPI4->CR2), 0, 0xFFFFU); - - NVIC_EnableIRQ(DMA2_Stream2_IRQn); - NVIC_EnableIRQ(DMA2_Stream3_IRQn); - NVIC_EnableIRQ(SPI4_IRQn); -} +void llspi_miso_dma(uint8_t *addr, int len); +void llspi_init(void); \ No newline at end of file diff --git a/board/stm32h7/lluart.c b/board/stm32h7/lluart.c new file mode 100644 index 00000000000..97e3e4ea143 --- /dev/null +++ b/board/stm32h7/lluart.c @@ -0,0 +1,104 @@ +#include "board/drivers/uart.h" + +static void uart_rx_ring(uart_ring *q){ + // Do not read out directly if DMA enabled + ENTER_CRITICAL(); + + // Read out RX buffer + uint8_t c = q->uart->RDR; // This read after reading SR clears a bunch of interrupts + + uint16_t next_w_ptr = (q->w_ptr_rx + 1U) % q->rx_fifo_size; + + if ((next_w_ptr == q->r_ptr_rx) && q->overwrite) { + // overwrite mode: drop oldest byte + q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size; + } + + // Do not overwrite buffer data + if (next_w_ptr != q->r_ptr_rx) { + q->elems_rx[q->w_ptr_rx] = c; + q->w_ptr_rx = next_w_ptr; + if (q->callback != NULL) { + q->callback(q); + } + } + + EXIT_CRITICAL(); +} + +void uart_tx_ring(uart_ring *q){ + ENTER_CRITICAL(); + // Send out next byte of TX buffer + if (q->w_ptr_tx != q->r_ptr_tx) { + // Only send if transmit register is empty (aka last byte has been sent) + if ((q->uart->ISR & USART_ISR_TXE_TXFNF) != 0U) { + q->uart->TDR = q->elems_tx[q->r_ptr_tx]; // This clears TXE + q->r_ptr_tx = (q->r_ptr_tx + 1U) % q->tx_fifo_size; + } + + // Enable TXE interrupt if there is still data to be sent + if(q->r_ptr_tx != q->w_ptr_tx){ + q->uart->CR1 |= USART_CR1_TXEIE; + } else { + q->uart->CR1 &= ~USART_CR1_TXEIE; + } + } + EXIT_CRITICAL(); +} + +// 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); + +static void uart_interrupt_handler(uart_ring *q) { + ENTER_CRITICAL(); + + // Read UART status. This is also the first step necessary in clearing most interrupts + uint32_t status = q->uart->ISR; + + // If RXFNE is set, perform a read. This clears RXFNE, ORE, IDLE, NF and FE + if((status & USART_ISR_RXNE_RXFNE) != 0U){ + uart_rx_ring(q); + } + + // Detect errors and clear them + uint32_t err = (status & USART_ISR_ORE) | (status & USART_ISR_NE) | (status & USART_ISR_FE) | (status & USART_ISR_PE); + if(err != 0U){ + #ifdef DEBUG_UART + print("Encountered UART error: "); puth(err); print("\n"); + #endif + UART_READ_RDR(q->uart) + } + + if ((err & USART_ISR_ORE) != 0U) { + q->uart->ICR |= USART_ICR_ORECF; + } else if ((err & USART_ISR_NE) != 0U) { + q->uart->ICR |= USART_ICR_NECF; + } else if ((err & USART_ISR_FE) != 0U) { + q->uart->ICR |= USART_ICR_FECF; + } else if ((err & USART_ISR_PE) != 0U) { + q->uart->ICR |= USART_ICR_PECF; + } else {} + + // Send if necessary + uart_tx_ring(q); + + EXIT_CRITICAL(); +} + +static void UART7_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_som_debug); } + +void uart_init(uart_ring *q, unsigned int baud) { + if (q->uart == UART7) { + REGISTER_INTERRUPT(UART7_IRQn, UART7_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_7) + + // UART7 is connected to APB1 at 60MHz + q->uart->BRR = 60000000U / baud; + q->uart->CR1 = USART_CR1_UE | USART_CR1_TE | USART_CR1_RE; + + // Enable interrupt on RX not empty + q->uart->CR1 |= USART_CR1_RXNEIE; + + // Enable UART interrupts + NVIC_EnableIRQ(UART7_IRQn); + } +} diff --git a/board/stm32h7/lluart.h b/board/stm32h7/lluart.h index e18f1e9f6f3..0f512dab273 100644 --- a/board/stm32h7/lluart.h +++ b/board/stm32h7/lluart.h @@ -1,102 +1,9 @@ -static void uart_rx_ring(uart_ring *q){ - // Do not read out directly if DMA enabled - ENTER_CRITICAL(); - - // Read out RX buffer - uint8_t c = q->uart->RDR; // This read after reading SR clears a bunch of interrupts - - uint16_t next_w_ptr = (q->w_ptr_rx + 1U) % q->rx_fifo_size; - - if ((next_w_ptr == q->r_ptr_rx) && q->overwrite) { - // overwrite mode: drop oldest byte - q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size; - } - - // Do not overwrite buffer data - if (next_w_ptr != q->r_ptr_rx) { - q->elems_rx[q->w_ptr_rx] = c; - q->w_ptr_rx = next_w_ptr; - if (q->callback != NULL) { - q->callback(q); - } - } - - EXIT_CRITICAL(); -} - -void uart_tx_ring(uart_ring *q){ - ENTER_CRITICAL(); - // Send out next byte of TX buffer - if (q->w_ptr_tx != q->r_ptr_tx) { - // Only send if transmit register is empty (aka last byte has been sent) - if ((q->uart->ISR & USART_ISR_TXE_TXFNF) != 0U) { - q->uart->TDR = q->elems_tx[q->r_ptr_tx]; // This clears TXE - q->r_ptr_tx = (q->r_ptr_tx + 1U) % q->tx_fifo_size; - } - - // Enable TXE interrupt if there is still data to be sent - if(q->r_ptr_tx != q->w_ptr_tx){ - q->uart->CR1 |= USART_CR1_TXEIE; - } else { - q->uart->CR1 &= ~USART_CR1_TXEIE; - } - } - EXIT_CRITICAL(); -} +#pragma once +struct uart_ring; +typedef struct uart_ring uart_ring; // 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); -static void uart_interrupt_handler(uart_ring *q) { - ENTER_CRITICAL(); - - // Read UART status. This is also the first step necessary in clearing most interrupts - uint32_t status = q->uart->ISR; - - // If RXFNE is set, perform a read. This clears RXFNE, ORE, IDLE, NF and FE - if((status & USART_ISR_RXNE_RXFNE) != 0U){ - uart_rx_ring(q); - } - - // Detect errors and clear them - uint32_t err = (status & USART_ISR_ORE) | (status & USART_ISR_NE) | (status & USART_ISR_FE) | (status & USART_ISR_PE); - if(err != 0U){ - #ifdef DEBUG_UART - print("Encountered UART error: "); puth(err); print("\n"); - #endif - UART_READ_RDR(q->uart) - } - - if ((err & USART_ISR_ORE) != 0U) { - q->uart->ICR |= USART_ICR_ORECF; - } else if ((err & USART_ISR_NE) != 0U) { - q->uart->ICR |= USART_ICR_NECF; - } else if ((err & USART_ISR_FE) != 0U) { - q->uart->ICR |= USART_ICR_FECF; - } else if ((err & USART_ISR_PE) != 0U) { - q->uart->ICR |= USART_ICR_PECF; - } else {} - - // Send if necessary - uart_tx_ring(q); - - EXIT_CRITICAL(); -} - -static void UART7_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_som_debug); } - -void uart_init(uart_ring *q, unsigned int baud) { - if (q->uart == UART7) { - REGISTER_INTERRUPT(UART7_IRQn, UART7_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_7) - - // UART7 is connected to APB1 at 60MHz - q->uart->BRR = 60000000U / baud; - q->uart->CR1 = USART_CR1_UE | USART_CR1_TE | USART_CR1_RE; - - // Enable interrupt on RX not empty - q->uart->CR1 |= USART_CR1_RXNEIE; - - // Enable UART interrupts - NVIC_EnableIRQ(UART7_IRQn); - } -} +void uart_init(uart_ring *q, unsigned int baud); +void uart_tx_ring(uart_ring *q); \ No newline at end of file diff --git a/board/stm32h7/llusb.c b/board/stm32h7/llusb.c new file mode 100644 index 00000000000..6744c8aeb78 --- /dev/null +++ b/board/stm32h7/llusb.c @@ -0,0 +1,79 @@ +#include "llusb.h" + +USB_OTG_GlobalTypeDef *USBx = USB_OTG_HS; + +static void OTG_HS_IRQ_Handler(void) { + NVIC_DisableIRQ(OTG_HS_IRQn); + usb_irqhandler(); + NVIC_EnableIRQ(OTG_HS_IRQn); +} + +void usb_init(void) { + REGISTER_INTERRUPT(OTG_HS_IRQn, OTG_HS_IRQ_Handler, 1500000U, FAULT_INTERRUPT_RATE_USB) // TODO: Find out a better rate limit for USB. Now it's the 1.5MB/s rate + + // Disable global interrupt + USBx->GAHBCFG &= ~(USB_OTG_GAHBCFG_GINT); + // Select FS Embedded PHY + USBx->GUSBCFG |= USB_OTG_GUSBCFG_PHYSEL; + // Force device mode + USBx->GUSBCFG &= ~(USB_OTG_GUSBCFG_FHMOD | USB_OTG_GUSBCFG_FDMOD); + USBx->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD; + delay(250000); // Wait for about 25ms (explicitly stated in H7 ref manual) + // Wait for AHB master IDLE state. + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0U); + // Core Soft Reset + USBx->GRSTCTL |= USB_OTG_GRSTCTL_CSRST; + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_CSRST) == USB_OTG_GRSTCTL_CSRST); + // Activate the USB Transceiver + USBx->GCCFG |= USB_OTG_GCCFG_PWRDWN; + + for (uint8_t i = 0U; i < 15U; i++) { + USBx->DIEPTXF[i] = 0U; + } + + // VBUS Sensing setup + USBx_DEVICE->DCTL |= USB_OTG_DCTL_SDIS; + // Deactivate VBUS Sensing B + USBx->GCCFG &= ~(USB_OTG_GCCFG_VBDEN); + // B-peripheral session valid override enable + USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOEN; + USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL; + // Restart the Phy Clock + USBx_PCGCCTL = 0U; + // Device mode configuration + USBx_DEVICE->DCFG |= DCFG_FRAME_INTERVAL_80; + USBx_DEVICE->DCFG |= USB_OTG_SPEED_FULL | USB_OTG_DCFG_NZLSOHSK; + + // Flush FIFOs + USBx->GRSTCTL = (USB_OTG_GRSTCTL_TXFFLSH | (0x10U << 6)); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH); + + USBx->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH; + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH); + + // Clear all pending Device Interrupts + USBx_DEVICE->DIEPMSK = 0U; + USBx_DEVICE->DOEPMSK = 0U; + USBx_DEVICE->DAINTMSK = 0U; + USBx_DEVICE->DIEPMSK &= ~(USB_OTG_DIEPMSK_TXFURM); + + // Disable all interrupts. + USBx->GINTMSK = 0U; + // Clear any pending interrupts + USBx->GINTSTS = 0xBFFFFFFFU; + // Enable interrupts matching to the Device mode ONLY + USBx->GINTMSK = USB_OTG_GINTMSK_USBRST | USB_OTG_GINTMSK_ENUMDNEM | USB_OTG_GINTMSK_OTGINT | + USB_OTG_GINTMSK_RXFLVLM | USB_OTG_GINTMSK_GONAKEFFM | USB_OTG_GINTMSK_GINAKEFFM | + USB_OTG_GINTMSK_OEPINT | USB_OTG_GINTMSK_IEPINT | + USB_OTG_GINTMSK_CIDSCHGM | USB_OTG_GINTMSK_SRQIM | USB_OTG_GINTMSK_MMISM; + + // Set USB Turnaround time + USBx->GUSBCFG |= ((USBD_FS_TRDT_VALUE << 10) & USB_OTG_GUSBCFG_TRDT); + // Enables the controller's Global Int in the AHB Config reg + USBx->GAHBCFG |= USB_OTG_GAHBCFG_GINT; + // Soft disconnect disable: + USBx_DEVICE->DCTL &= ~(USB_OTG_DCTL_SDIS); + + // enable the IRQ + NVIC_EnableIRQ(OTG_HS_IRQn); +} diff --git a/board/stm32h7/llusb.h b/board/stm32h7/llusb.h index f1bcf16ad5e..6e47defd298 100644 --- a/board/stm32h7/llusb.h +++ b/board/stm32h7/llusb.h @@ -1,79 +1,18 @@ -#include "llusb_declarations.h" +#pragma once -USB_OTG_GlobalTypeDef *USBx = USB_OTG_HS; +#include "stm32h7_config.h" -static void OTG_HS_IRQ_Handler(void) { - NVIC_DisableIRQ(OTG_HS_IRQn); - usb_irqhandler(); - NVIC_EnableIRQ(OTG_HS_IRQn); -} +extern USB_OTG_GlobalTypeDef *USBx; -void usb_init(void) { - REGISTER_INTERRUPT(OTG_HS_IRQn, OTG_HS_IRQ_Handler, 1500000U, FAULT_INTERRUPT_RATE_USB) // TODO: Find out a better rate limit for USB. Now it's the 1.5MB/s rate +#define USBx_DEVICE ((USB_OTG_DeviceTypeDef *)((uint32_t)USBx + USB_OTG_DEVICE_BASE)) +#define USBx_INEP(i) ((USB_OTG_INEndpointTypeDef *)((uint32_t)USBx + USB_OTG_IN_ENDPOINT_BASE + ((i) * USB_OTG_EP_REG_SIZE))) +#define USBx_OUTEP(i) ((USB_OTG_OUTEndpointTypeDef *)((uint32_t)USBx + USB_OTG_OUT_ENDPOINT_BASE + ((i) * USB_OTG_EP_REG_SIZE))) +#define USBx_DFIFO(i) *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_FIFO_BASE + ((i) * USB_OTG_FIFO_SIZE)) +#define USBx_PCGCCTL *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_PCGCCTL_BASE) - // Disable global interrupt - USBx->GAHBCFG &= ~(USB_OTG_GAHBCFG_GINT); - // Select FS Embedded PHY - USBx->GUSBCFG |= USB_OTG_GUSBCFG_PHYSEL; - // Force device mode - USBx->GUSBCFG &= ~(USB_OTG_GUSBCFG_FHMOD | USB_OTG_GUSBCFG_FDMOD); - USBx->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD; - delay(250000); // Wait for about 25ms (explicitly stated in H7 ref manual) - // Wait for AHB master IDLE state. - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0U); - // Core Soft Reset - USBx->GRSTCTL |= USB_OTG_GRSTCTL_CSRST; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_CSRST) == USB_OTG_GRSTCTL_CSRST); - // Activate the USB Transceiver - USBx->GCCFG |= USB_OTG_GCCFG_PWRDWN; +#define USBD_FS_TRDT_VALUE 6UL +#define USB_OTG_SPEED_FULL 3U +#define DCFG_FRAME_INTERVAL_80 0U - for (uint8_t i = 0U; i < 15U; i++) { - USBx->DIEPTXF[i] = 0U; - } - - // VBUS Sensing setup - USBx_DEVICE->DCTL |= USB_OTG_DCTL_SDIS; - // Deactivate VBUS Sensing B - USBx->GCCFG &= ~(USB_OTG_GCCFG_VBDEN); - // B-peripheral session valid override enable - USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOEN; - USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL; - // Restart the Phy Clock - USBx_PCGCCTL = 0U; - // Device mode configuration - USBx_DEVICE->DCFG |= DCFG_FRAME_INTERVAL_80; - USBx_DEVICE->DCFG |= USB_OTG_SPEED_FULL | USB_OTG_DCFG_NZLSOHSK; - - // Flush FIFOs - USBx->GRSTCTL = (USB_OTG_GRSTCTL_TXFFLSH | (0x10U << 6)); - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH); - - USBx->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH); - - // Clear all pending Device Interrupts - USBx_DEVICE->DIEPMSK = 0U; - USBx_DEVICE->DOEPMSK = 0U; - USBx_DEVICE->DAINTMSK = 0U; - USBx_DEVICE->DIEPMSK &= ~(USB_OTG_DIEPMSK_TXFURM); - - // Disable all interrupts. - USBx->GINTMSK = 0U; - // Clear any pending interrupts - USBx->GINTSTS = 0xBFFFFFFFU; - // Enable interrupts matching to the Device mode ONLY - USBx->GINTMSK = USB_OTG_GINTMSK_USBRST | USB_OTG_GINTMSK_ENUMDNEM | USB_OTG_GINTMSK_OTGINT | - USB_OTG_GINTMSK_RXFLVLM | USB_OTG_GINTMSK_GONAKEFFM | USB_OTG_GINTMSK_GINAKEFFM | - USB_OTG_GINTMSK_OEPINT | USB_OTG_GINTMSK_IEPINT | - USB_OTG_GINTMSK_CIDSCHGM | USB_OTG_GINTMSK_SRQIM | USB_OTG_GINTMSK_MMISM; - - // Set USB Turnaround time - USBx->GUSBCFG |= ((USBD_FS_TRDT_VALUE << 10) & USB_OTG_GUSBCFG_TRDT); - // Enables the controller's Global Int in the AHB Config reg - USBx->GAHBCFG |= USB_OTG_GAHBCFG_GINT; - // Soft disconnect disable: - USBx_DEVICE->DCTL &= ~(USB_OTG_DCTL_SDIS); - - // enable the IRQ - NVIC_EnableIRQ(OTG_HS_IRQn); -} +void usb_irqhandler(void); +void usb_init(void); \ No newline at end of file diff --git a/board/stm32h7/llusb_declarations.h b/board/stm32h7/llusb_declarations.h deleted file mode 100644 index a11d3f52c6b..00000000000 --- a/board/stm32h7/llusb_declarations.h +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -extern USB_OTG_GlobalTypeDef *USBx; - -#define USBx_DEVICE ((USB_OTG_DeviceTypeDef *)((uint32_t)USBx + USB_OTG_DEVICE_BASE)) -#define USBx_INEP(i) ((USB_OTG_INEndpointTypeDef *)((uint32_t)USBx + USB_OTG_IN_ENDPOINT_BASE + ((i) * USB_OTG_EP_REG_SIZE))) -#define USBx_OUTEP(i) ((USB_OTG_OUTEndpointTypeDef *)((uint32_t)USBx + USB_OTG_OUT_ENDPOINT_BASE + ((i) * USB_OTG_EP_REG_SIZE))) -#define USBx_DFIFO(i) *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_FIFO_BASE + ((i) * USB_OTG_FIFO_SIZE)) -#define USBx_PCGCCTL *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_PCGCCTL_BASE) - -#define USBD_FS_TRDT_VALUE 6UL -#define USB_OTG_SPEED_FULL 3U -#define DCFG_FRAME_INTERVAL_80 0U - -void usb_irqhandler(void); -void usb_init(void); diff --git a/board/stm32h7/peripherals.c b/board/stm32h7/peripherals.c new file mode 100644 index 00000000000..d537fa23286 --- /dev/null +++ b/board/stm32h7/peripherals.c @@ -0,0 +1,138 @@ +#include "peripherals.h" +#include "board/drivers/gpio.h" + +#ifdef BOOTSTUB +void gpio_usb_init(void) { +#else +void gpio_usb_init(void) { +#endif + // A11,A12: USB + set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG1_FS); + set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG1_FS); + GPIOA->OSPEEDR = GPIO_OSPEEDR_OSPEED11 | GPIO_OSPEEDR_OSPEED12; +} + +void gpio_spi_init(void) { + set_gpio_alternate(GPIOE, 11, GPIO_AF5_SPI4); + set_gpio_alternate(GPIOE, 12, GPIO_AF5_SPI4); + set_gpio_alternate(GPIOE, 13, GPIO_AF5_SPI4); + set_gpio_alternate(GPIOE, 14, GPIO_AF5_SPI4); + register_set_bits(&(GPIOE->OSPEEDR), GPIO_OSPEEDR_OSPEED11 | GPIO_OSPEEDR_OSPEED12 | GPIO_OSPEEDR_OSPEED13 | GPIO_OSPEEDR_OSPEED14); +} + +#ifdef BOOTSTUB +void gpio_usart2_init(void) { + // A2,A3: USART 2 for debugging + set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2); + set_gpio_alternate(GPIOA, 3, GPIO_AF7_USART2); +} +#endif + +void gpio_uart7_init(void) { + // E7,E8: UART 7 for debugging + set_gpio_alternate(GPIOE, 7, GPIO_AF7_UART7); + set_gpio_alternate(GPIOE, 8, GPIO_AF7_UART7); +} + +// Common GPIO initialization +void common_init_gpio(void) { + //F11: VOLT_S + set_gpio_pullup(GPIOF, 11, PULL_NONE); + set_gpio_mode(GPIOF, 11, MODE_ANALOG); + + gpio_usb_init(); + + // B8,B9: FDCAN1 + set_gpio_pullup(GPIOB, 8, PULL_NONE); + set_gpio_alternate(GPIOB, 8, GPIO_AF9_FDCAN1); + + set_gpio_pullup(GPIOB, 9, PULL_NONE); + set_gpio_alternate(GPIOB, 9, GPIO_AF9_FDCAN1); + + // B5,B6 (mplex to B12,B13): FDCAN2 + set_gpio_pullup(GPIOB, 12, PULL_NONE); + set_gpio_pullup(GPIOB, 13, PULL_NONE); + + set_gpio_pullup(GPIOB, 5, PULL_NONE); + set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2); + + set_gpio_pullup(GPIOB, 6, PULL_NONE); + set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2); + + // G9,G10: FDCAN3 + set_gpio_pullup(GPIOG, 9, PULL_NONE); + set_gpio_alternate(GPIOG, 9, GPIO_AF2_FDCAN3); + + set_gpio_pullup(GPIOG, 10, PULL_NONE); + set_gpio_alternate(GPIOG, 10, GPIO_AF2_FDCAN3); +} + +#ifdef BOOTSTUB +void flasher_peripherals_init(void) { + RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN; + + // SPI + DMA + RCC->APB2ENR |= RCC_APB2ENR_SPI4EN; + RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; + + // LED PWM + RCC->APB1LENR |= RCC_APB1LENR_TIM3EN; +} +#endif + +// Peripheral initialization +void peripherals_init(void) { + // enable GPIO(A,B,C,D,E,F,G,H) + RCC->AHB4ENR |= RCC_AHB4ENR_GPIOAEN; + RCC->AHB4ENR |= RCC_AHB4ENR_GPIOBEN; + RCC->AHB4ENR |= RCC_AHB4ENR_GPIOCEN; + RCC->AHB4ENR |= RCC_AHB4ENR_GPIODEN; + RCC->AHB4ENR |= RCC_AHB4ENR_GPIOEEN; + RCC->AHB4ENR |= RCC_AHB4ENR_GPIOFEN; + RCC->AHB4ENR |= RCC_AHB4ENR_GPIOGEN; + + // Enable CPU access to SRAMs for DMA + RCC->AHB2ENR |= RCC_AHB2ENR_SRAM1EN | RCC_AHB2ENR_SRAM2EN; + + // Supplemental + RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN; // DAC DMA + RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; // SPI DMA + RCC->APB4ENR |= RCC_APB4ENR_SYSCFGEN; + RCC->AHB4ENR |= RCC_AHB4ENR_BDMAEN; // Audio DMA + + // Connectivity + RCC->APB2ENR |= RCC_APB2ENR_SPI4EN; // SPI + RCC->APB1LENR |= RCC_APB1LENR_I2C5EN; // codec I2C + RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN; // USB + RCC->AHB1LPENR |= RCC_AHB1LPENR_USB1OTGHSLPEN; // USB LP needed for CSleep state(__WFI()) + RCC->AHB1LPENR &= ~(RCC_AHB1LPENR_USB1OTGHSULPILPEN); // disable USB ULPI + RCC->APB1LENR |= RCC_APB1LENR_UART7EN; // SOM uart + RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; // FDCAN core enable + + // Analog + RCC->AHB1ENR |= RCC_AHB1ENR_ADC12EN; // Enable ADC12 clocks + RCC->AHB4ENR |= RCC_AHB4ENR_ADC3EN; // Enable ADC3 clocks + RCC->APB1LENR |= RCC_APB1LENR_DAC12EN; // DAC + + // Audio + RCC->APB2ENR |= RCC_APB2ENR_DFSDM1EN; // D/S demodulator for mic + RCC->APB4ENR |= RCC_APB4ENR_SAI4EN; // SAI4 + + // Timers + RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // clock source timer + RCC->APB1LENR |= RCC_APB1LENR_TIM2EN; // main counter + RCC->APB1LENR |= RCC_APB1LENR_TIM3EN; // fan + led pwm + RCC->APB1LENR |= RCC_APB1LENR_TIM6EN; // interrupt timer + RCC->APB1LENR |= RCC_APB1LENR_TIM7EN; // DMA trigger timer + RCC->APB2ENR |= RCC_APB2ENR_TIM8EN; // tick timer + RCC->APB1LENR |= RCC_APB1LENR_TIM12EN; // slow loop + RCC->APB1LENR |= RCC_APB1LENR_TIM5EN; // sound trigger timer + +#ifdef PANDA_JUNGLE + RCC->AHB3ENR |= RCC_AHB3ENR_SDMMC1EN; // SDMMC +#endif +} + +void enable_interrupt_timer(void) { + register_set_bits(&(RCC->APB1LENR), RCC_APB1LENR_TIM6EN); // Enable interrupt timer peripheral +} diff --git a/board/stm32h7/peripherals.h b/board/stm32h7/peripherals.h index 8eb384307f3..b9043eac219 100644 --- a/board/stm32h7/peripherals.h +++ b/board/stm32h7/peripherals.h @@ -1,135 +1,22 @@ -#ifdef BOOTSTUB -void gpio_usb_init(void) { -#else -static void gpio_usb_init(void) { -#endif - // A11,A12: USB - set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG1_FS); - set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG1_FS); - GPIOA->OSPEEDR = GPIO_OSPEEDR_OSPEED11 | GPIO_OSPEEDR_OSPEED12; -} +#pragma once -void gpio_spi_init(void) { - set_gpio_alternate(GPIOE, 11, GPIO_AF5_SPI4); - set_gpio_alternate(GPIOE, 12, GPIO_AF5_SPI4); - set_gpio_alternate(GPIOE, 13, GPIO_AF5_SPI4); - set_gpio_alternate(GPIOE, 14, GPIO_AF5_SPI4); - register_set_bits(&(GPIOE->OSPEEDR), GPIO_OSPEEDR_OSPEED11 | GPIO_OSPEEDR_OSPEED12 | GPIO_OSPEEDR_OSPEED13 | GPIO_OSPEEDR_OSPEED14); -} +void gpio_usb_init(void); +void gpio_spi_init(void); #ifdef BOOTSTUB -void gpio_usart2_init(void) { - // A2,A3: USART 2 for debugging - set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2); - set_gpio_alternate(GPIOA, 3, GPIO_AF7_USART2); -} +void gpio_usart2_init(void); #endif -void gpio_uart7_init(void) { - // E7,E8: UART 7 for debugging - set_gpio_alternate(GPIOE, 7, GPIO_AF7_UART7); - set_gpio_alternate(GPIOE, 8, GPIO_AF7_UART7); -} +void gpio_uart7_init(void); // Common GPIO initialization -void common_init_gpio(void) { - //F11: VOLT_S - set_gpio_pullup(GPIOF, 11, PULL_NONE); - set_gpio_mode(GPIOF, 11, MODE_ANALOG); - - gpio_usb_init(); - - // B8,B9: FDCAN1 - set_gpio_pullup(GPIOB, 8, PULL_NONE); - set_gpio_alternate(GPIOB, 8, GPIO_AF9_FDCAN1); - - set_gpio_pullup(GPIOB, 9, PULL_NONE); - set_gpio_alternate(GPIOB, 9, GPIO_AF9_FDCAN1); - - // B5,B6 (mplex to B12,B13): FDCAN2 - set_gpio_pullup(GPIOB, 12, PULL_NONE); - set_gpio_pullup(GPIOB, 13, PULL_NONE); - - set_gpio_pullup(GPIOB, 5, PULL_NONE); - set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2); - - set_gpio_pullup(GPIOB, 6, PULL_NONE); - set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2); - - // G9,G10: FDCAN3 - set_gpio_pullup(GPIOG, 9, PULL_NONE); - set_gpio_alternate(GPIOG, 9, GPIO_AF2_FDCAN3); - - set_gpio_pullup(GPIOG, 10, PULL_NONE); - set_gpio_alternate(GPIOG, 10, GPIO_AF2_FDCAN3); -} +void common_init_gpio(void); #ifdef BOOTSTUB -void flasher_peripherals_init(void) { - RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN; - - // SPI + DMA - RCC->APB2ENR |= RCC_APB2ENR_SPI4EN; - RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; - - // LED PWM - RCC->APB1LENR |= RCC_APB1LENR_TIM3EN; -} +void flasher_peripherals_init(void); #endif // Peripheral initialization -void peripherals_init(void) { - // enable GPIO(A,B,C,D,E,F,G,H) - RCC->AHB4ENR |= RCC_AHB4ENR_GPIOAEN; - RCC->AHB4ENR |= RCC_AHB4ENR_GPIOBEN; - RCC->AHB4ENR |= RCC_AHB4ENR_GPIOCEN; - RCC->AHB4ENR |= RCC_AHB4ENR_GPIODEN; - RCC->AHB4ENR |= RCC_AHB4ENR_GPIOEEN; - RCC->AHB4ENR |= RCC_AHB4ENR_GPIOFEN; - RCC->AHB4ENR |= RCC_AHB4ENR_GPIOGEN; - - // Enable CPU access to SRAMs for DMA - RCC->AHB2ENR |= RCC_AHB2ENR_SRAM1EN | RCC_AHB2ENR_SRAM2EN; - - // Supplemental - RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN; // DAC DMA - RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; // SPI DMA - RCC->APB4ENR |= RCC_APB4ENR_SYSCFGEN; - RCC->AHB4ENR |= RCC_AHB4ENR_BDMAEN; // Audio DMA - - // Connectivity - RCC->APB2ENR |= RCC_APB2ENR_SPI4EN; // SPI - RCC->APB1LENR |= RCC_APB1LENR_I2C5EN; // codec I2C - RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN; // USB - RCC->AHB1LPENR |= RCC_AHB1LPENR_USB1OTGHSLPEN; // USB LP needed for CSleep state(__WFI()) - RCC->AHB1LPENR &= ~(RCC_AHB1LPENR_USB1OTGHSULPILPEN); // disable USB ULPI - RCC->APB1LENR |= RCC_APB1LENR_UART7EN; // SOM uart - RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; // FDCAN core enable - - // Analog - RCC->AHB1ENR |= RCC_AHB1ENR_ADC12EN; // Enable ADC12 clocks - RCC->AHB4ENR |= RCC_AHB4ENR_ADC3EN; // Enable ADC3 clocks - RCC->APB1LENR |= RCC_APB1LENR_DAC12EN; // DAC - - // Audio - RCC->APB2ENR |= RCC_APB2ENR_DFSDM1EN; // D/S demodulator for mic - RCC->APB4ENR |= RCC_APB4ENR_SAI4EN; // SAI4 - - // Timers - RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // clock source timer - RCC->APB1LENR |= RCC_APB1LENR_TIM2EN; // main counter - RCC->APB1LENR |= RCC_APB1LENR_TIM3EN; // fan + led pwm - RCC->APB1LENR |= RCC_APB1LENR_TIM6EN; // interrupt timer - RCC->APB1LENR |= RCC_APB1LENR_TIM7EN; // DMA trigger timer - RCC->APB2ENR |= RCC_APB2ENR_TIM8EN; // tick timer - RCC->APB1LENR |= RCC_APB1LENR_TIM12EN; // slow loop - RCC->APB1LENR |= RCC_APB1LENR_TIM5EN; // sound trigger timer - -#ifdef PANDA_JUNGLE - RCC->AHB3ENR |= RCC_AHB3ENR_SDMMC1EN; // SDMMC -#endif -} +void peripherals_init(void); -void enable_interrupt_timer(void) { - register_set_bits(&(RCC->APB1LENR), RCC_APB1LENR_TIM6EN); // Enable interrupt timer peripheral -} +void enable_interrupt_timer(void); \ No newline at end of file diff --git a/board/stm32h7/sound.c b/board/stm32h7/sound.c new file mode 100644 index 00000000000..2d0c7174c63 --- /dev/null +++ b/board/stm32h7/sound.c @@ -0,0 +1,210 @@ +#include + +#include "board/globals.h" +#include "sound.h" + +__attribute__((section(".sram4"))) static uint16_t sound_rx_buf[2][SOUND_RX_BUF_SIZE]; +__attribute__((section(".sram4"))) static uint16_t sound_tx_buf[2][SOUND_TX_BUF_SIZE]; +__attribute__((section(".sram4"))) static uint32_t mic_rx_buf[2][MIC_RX_BUF_SIZE]; +__attribute__((section(".sram4"))) static uint16_t mic_tx_buf[2][MIC_TX_BUF_SIZE]; + +static uint8_t sound_idle_count; +static uint8_t mic_idle_count; +static uint8_t mic_buffer_count; + +void sound_tick(void) { + if (sound_idle_count > 0U) { + sound_idle_count--; + if (sound_idle_count == 0U) { + current_board->set_amp_enabled(false); + register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); + } + } + + if (mic_idle_count > 0U) { + mic_idle_count--; + if (mic_idle_count == 0U) { + register_clear_bits(&DFSDM1_Channel0->CHCFGR1, DFSDM_CHCFGR1_DFSDMEN); + mic_buffer_count = 0U; + } + } +} + +// Recording processing +static void DMA1_Stream0_IRQ_Handler(void) { + DMA1->LIFCR |= 0x7DU; // clear flags + + uint8_t tx_buf_idx = (((BDMA_Channel1->CCR & BDMA_CCR_CT) >> BDMA_CCR_CT_Pos) == 1U) ? 0U : 1U; + + if (mic_buffer_count < MIC_SKIP_BUFFERS) { + // Send silence during settling + mic_buffer_count++; + for (uint16_t i = 0U; i < MIC_TX_BUF_SIZE; i++) { + mic_tx_buf[tx_buf_idx][i] = 0U; + } + } else { + // process samples + uint8_t buf_idx = (((DMA1_Stream0->CR & DMA_SxCR_CT) >> DMA_SxCR_CT_Pos) == 1U) ? 0U : 1U; + for (uint16_t i=0U; i < MIC_RX_BUF_SIZE; i++) { + mic_tx_buf[tx_buf_idx][2U*i] = ((mic_rx_buf[buf_idx][i] >> 16U) & 0xFFFFU); + mic_tx_buf[tx_buf_idx][(2U*i)+1U] = mic_tx_buf[tx_buf_idx][2U*i]; + } + } +} + +// Playback processing +static void BDMA_Channel0_IRQ_Handler(void) { + static uint8_t playback_buf = 0U; + + BDMA->IFCR |= BDMA_IFCR_CGIF0; // clear flag + + uint8_t rx_buf_idx = (((BDMA_Channel0->CCR & BDMA_CCR_CT) >> BDMA_CCR_CT_Pos) == 1U) ? 0U : 1U; + playback_buf = 1U - playback_buf; + + // wait until we're done playing the current buf + for (uint32_t timeout_counter = 100000U; timeout_counter > 0U; timeout_counter--){ + uint8_t playing_buf = (DMA1_Stream1->CR & DMA_SxCR_CT) >> DMA_SxCR_CT_Pos; + if ((playing_buf != playback_buf) || ((DMA1_Stream1->CR & DMA_SxCR_EN) == 0U)) { + break; + } + } + + // process samples (shift to 12b and bias to be unsigned) + bool sound_playing = false; + for (uint16_t i=0U; i < SOUND_RX_BUF_SIZE; i += 2U) { + // since we are playing mono and receiving stereo, we take every other sample + sound_tx_buf[playback_buf][i/2U] = ((sound_rx_buf[rx_buf_idx][i] + (1UL << 14)) >> 3); + if (sound_rx_buf[rx_buf_idx][i] > 0U) { + sound_playing = true; + } + } + + // manage amp state + if (sound_playing) { + if (sound_idle_count == 0U) { + current_board->set_amp_enabled(true); + + // empty the other buf and start playing that + for (uint16_t i=0U; i < SOUND_TX_BUF_SIZE; i++) { + sound_tx_buf[1U - playback_buf][i] = (1UL << 11); + } + register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); + DMA1_Stream1->CR = (DMA1_Stream1->CR & ~DMA_SxCR_CT_Msk) | ((1UL - playback_buf) << DMA_SxCR_CT_Pos); + register_set_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); + } + sound_idle_count = SOUND_IDLE_TIMEOUT; + } + + // manage mic state + if (mic_idle_count == 0U) { + register_set_bits(&DFSDM1_Channel0->CHCFGR1, DFSDM_CHCFGR1_DFSDMEN); + DFSDM1_Filter0->FLTCR1 |= DFSDM_FLTCR1_RSWSTART; + } + mic_idle_count = SOUND_IDLE_TIMEOUT; + + sound_tick(); +} + +void sound_init_dac(void) { + // Init DAC + DAC1->DHR12R1 = (1UL << 11); + DAC1->DHR12R2 = (1UL << 11); + register_set(&DAC1->MCR, 0U, 0xFFFFFFFFU); + register_set(&DAC1->CR, DAC_CR_TEN1 | (4U << DAC_CR_TSEL1_Pos) | DAC_CR_DMAEN1, 0xFFFFFFFFU); + register_set_bits(&DAC1->CR, DAC_CR_EN1 | DAC_CR_EN2); + + // Setup DMAMUX (DAC_CH1_DMA as input) + register_set(&DMAMUX1_Channel1->CCR, 67U, DMAMUX_CxCR_DMAREQ_ID_Msk); + + // Setup DMA + register_set(&DMA1_Stream1->PAR, (uint32_t) &(DAC1->DHR12R1), 0xFFFFFFFFU); + register_set(&DMA1_Stream1->M0AR, (uint32_t) sound_tx_buf[0], 0xFFFFFFFFU); + register_set(&DMA1_Stream1->M1AR, (uint32_t) sound_tx_buf[1], 0xFFFFFFFFU); + register_set(&DMA1_Stream1->FCR, 0U, 0x00000083U); + DMA1_Stream1->NDTR = SOUND_TX_BUF_SIZE; + DMA1_Stream1->CR = DMA_SxCR_DBM | (0b11UL << DMA_SxCR_PL_Pos) | (0b01UL << DMA_SxCR_MSIZE_Pos) | (0b01UL << DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | (1U << DMA_SxCR_DIR_Pos); +} + +void sound_stop_dac(void) { + register_clear_bits(&BDMA_Channel0->CCR, BDMA_CCR_EN); + BDMA->IFCR = 0xFFFFFFFFU; + + register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); + while ((DMA1_Stream1->CR & DMA_SxCR_EN) != 0U) {} + DMA1->LIFCR = (0x3FU << 6); + + register_clear_bits(&DAC1->CR, DAC_CR_EN1 | DAC_CR_EN2); + while ((DAC1->CR & (DAC_CR_EN1 | DAC_CR_EN2)) != 0U) {} +} + +void sound_init(void) { + REGISTER_INTERRUPT(BDMA_Channel0_IRQn, BDMA_Channel0_IRQ_Handler, 128U, FAULT_INTERRUPT_RATE_SOUND_DMA) + REGISTER_INTERRUPT(DMA1_Stream0_IRQn, DMA1_Stream0_IRQ_Handler, 128U, FAULT_INTERRUPT_RATE_SOUND_DMA) + + // Init DAC and its DMA + sound_init_dac(); + + // Init trigger timer (little slower than 48kHz, pulled in sync by SAI4_FS_B) + register_set(&TIM5->PSC, 2600U, 0xFFFFU); + register_set(&TIM5->ARR, 100U, 0xFFFFFFFFU); + register_set(&TIM5->AF1, (0b0010UL << TIM5_AF1_ETRSEL_Pos), TIM5_AF1_ETRSEL_Msk); + register_set(&TIM5->CR2, (0b010U << TIM_CR2_MMS_Pos), TIM_CR2_MMS_Msk); + register_set(&TIM5->SMCR, TIM_SMCR_ECE | (0b00111UL << TIM_SMCR_TS_Pos)| (0b0100UL << TIM_SMCR_SMS_Pos), 0x31FFF7U); + TIM5->CNT = 0U; TIM5->SR = 0U; + TIM5->CR1 |= TIM_CR1_CEN; + + // sync both SAIs + register_set(&SAI4->GCR, (0b10UL << SAI_GCR_SYNCOUT_Pos), SAI_GCR_SYNCIN_Msk | SAI_GCR_SYNCOUT_Msk); + + // stereo audio in + register_set(&SAI4_Block_B->CR1, SAI_xCR1_DMAEN | (0b00UL << SAI_xCR1_SYNCEN_Pos) | (0b100U << SAI_xCR1_DS_Pos) | (0b11U << SAI_xCR1_MODE_Pos), 0x0FFB3FEFU); + register_set(&SAI4_Block_B->CR2, (0b001U << SAI_xCR2_FTH_Pos), 0xFFFBU); + register_set(&SAI4_Block_B->FRCR, (31U << SAI_xFRCR_FRL_Pos), 0x7FFFFU); + register_set(&SAI4_Block_B->SLOTR, (0b11UL << SAI_xSLOTR_SLOTEN_Pos) | (1UL << SAI_xSLOTR_NBSLOT_Pos) | (0b01UL << SAI_xSLOTR_SLOTSZ_Pos), 0xFFFF0FDFU); // NBSLOT definition is vague + + // init sound DMA (SAI4_B -> memory, double buffers) + register_set(&BDMA_Channel0->CPAR, (uint32_t) &(SAI4_Block_B->DR), 0xFFFFFFFFU); + register_set(&BDMA_Channel0->CM0AR, (uint32_t) sound_rx_buf[0], 0xFFFFFFFFU); + register_set(&BDMA_Channel0->CM1AR, (uint32_t) sound_rx_buf[1], 0xFFFFFFFFU); + BDMA_Channel0->CNDTR = SOUND_RX_BUF_SIZE; + register_set(&BDMA_Channel0->CCR, BDMA_CCR_DBM | (0b01UL << BDMA_CCR_MSIZE_Pos) | (0b01UL << BDMA_CCR_PSIZE_Pos) | BDMA_CCR_MINC | BDMA_CCR_CIRC | BDMA_CCR_TCIE, 0xFFFFU); + register_set(&DMAMUX2_Channel0->CCR, 16U, DMAMUX_CxCR_DMAREQ_ID_Msk); // SAI4_B_DMA + register_set_bits(&BDMA_Channel0->CCR, BDMA_CCR_EN); + + // mic output + register_set(&SAI4_Block_A->CR1, SAI_xCR1_DMAEN | (0b01UL << SAI_xCR1_SYNCEN_Pos) | (0b100UL << SAI_xCR1_DS_Pos) | (0b10UL << SAI_xCR1_MODE_Pos), 0x0FFB3FEFU); + register_set(&SAI4_Block_A->CR2, 0U, 0xFFFBU); + register_set(&SAI4_Block_A->FRCR, (31U << SAI_xFRCR_FRL_Pos), 0x7FFFFU); + register_set(&SAI4_Block_A->SLOTR, (0b11UL << SAI_xSLOTR_SLOTEN_Pos) | (1UL << SAI_xSLOTR_NBSLOT_Pos) | (0b01U << SAI_xSLOTR_SLOTSZ_Pos), 0xFFFF0FDFU); // NBSLOT definition is vague + + // init DFSDM for PDM mic + register_set(&DFSDM1_Channel0->CHCFGR1, (76UL << DFSDM_CHCFGR1_CKOUTDIV_Pos) | DFSDM_CHCFGR1_CHEN, 0xC0FFF1EFU); // CH0 controls the clock + register_set(&DFSDM1_Channel3->CHCFGR1, (0b01UL << DFSDM_CHCFGR1_SPICKSEL_Pos) | (0b00U << DFSDM_CHCFGR1_SITP_Pos) | DFSDM_CHCFGR1_CHEN, 0x0000F1EFU); // SITP determines sample edge + register_set(&DFSDM1_Filter0->FLTFCR, (0U << DFSDM_FLTFCR_IOSR_Pos) | (64UL << DFSDM_FLTFCR_FOSR_Pos) | (4UL << DFSDM_FLTFCR_FORD_Pos), 0xE3FF00FFU); + register_set(&DFSDM1_Filter0->FLTCR1, DFSDM_FLTCR1_FAST | (3UL << DFSDM_FLTCR1_RCH_Pos) | DFSDM_FLTCR1_RDMAEN | DFSDM_FLTCR1_RCONT | DFSDM_FLTCR1_DFEN, 0x672E7F3BU); + + // DMA (DFSDM1 -> memory) + register_set(&DMA1_Stream0->PAR, (uint32_t) &DFSDM1_Filter0->FLTRDATAR, 0xFFFFFFFFU); + register_set(&DMA1_Stream0->M0AR, (uint32_t)mic_rx_buf[0], 0xFFFFFFFFU); + register_set(&DMA1_Stream0->M1AR, (uint32_t)mic_rx_buf[1], 0xFFFFFFFFU); + DMA1_Stream0->NDTR = MIC_RX_BUF_SIZE; + register_set(&DMA1_Stream0->CR, DMA_SxCR_DBM | (0b10UL << DMA_SxCR_MSIZE_Pos) | (0b10UL << DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | DMA_SxCR_CIRC | DMA_SxCR_TCIE, 0x01F7FFFFU); + register_set(&DMAMUX1_Channel0->CCR, 101U, DMAMUX_CxCR_DMAREQ_ID_Msk); // DFSDM1_DMA0 + register_set_bits(&DMA1_Stream0->CR, DMA_SxCR_EN); + DMA1->LIFCR |= 0x7DU; // clear flags + + // DMA (memory -> SAI4) + register_set(&BDMA_Channel1->CPAR, (uint32_t) &(SAI4_Block_A->DR), 0xFFFFFFFFU); + register_set(&BDMA_Channel1->CM0AR, (uint32_t) mic_tx_buf[0], 0xFFFFFFFFU); + register_set(&BDMA_Channel1->CM1AR, (uint32_t) mic_tx_buf[1], 0xFFFFFFFFU); + BDMA_Channel1->CNDTR = MIC_TX_BUF_SIZE; + register_set(&BDMA_Channel1->CCR, BDMA_CCR_DBM | (0b01UL << BDMA_CCR_MSIZE_Pos) |(0b01UL << BDMA_CCR_PSIZE_Pos) | BDMA_CCR_MINC | BDMA_CCR_CIRC | (0b1U << BDMA_CCR_DIR_Pos), 0xFFFFU); + register_set(&DMAMUX2_Channel1->CCR, 15U, DMAMUX_CxCR_DMAREQ_ID_Msk); // SAI4_A_DMA + register_set_bits(&BDMA_Channel1->CCR, BDMA_CCR_EN); + + // enable all initted blocks + register_set_bits(&SAI4_Block_A->CR1, SAI_xCR1_SAIEN); + register_set_bits(&SAI4_Block_B->CR1, SAI_xCR1_SAIEN); + NVIC_EnableIRQ(BDMA_Channel0_IRQn); + NVIC_EnableIRQ(DMA1_Stream0_IRQn); +} diff --git a/board/stm32h7/sound.h b/board/stm32h7/sound.h index f9445dde92b..d35490b25db 100644 --- a/board/stm32h7/sound.h +++ b/board/stm32h7/sound.h @@ -1,211 +1,13 @@ +#pragma once + #define SOUND_RX_BUF_SIZE 1000U #define SOUND_TX_BUF_SIZE (SOUND_RX_BUF_SIZE/2U) #define MIC_RX_BUF_SIZE 512U #define MIC_TX_BUF_SIZE (MIC_RX_BUF_SIZE * 2U) -__attribute__((section(".sram4"))) static uint16_t sound_rx_buf[2][SOUND_RX_BUF_SIZE]; -__attribute__((section(".sram4"))) static uint16_t sound_tx_buf[2][SOUND_TX_BUF_SIZE]; -__attribute__((section(".sram4"))) static uint32_t mic_rx_buf[2][MIC_RX_BUF_SIZE]; -__attribute__((section(".sram4"))) static uint16_t mic_tx_buf[2][MIC_TX_BUF_SIZE]; - #define SOUND_IDLE_TIMEOUT 4U #define MIC_SKIP_BUFFERS 2U // Skip first 2 buffers (1024 samples = ~21ms at 48kHz) -static uint8_t sound_idle_count; -static uint8_t mic_idle_count; -static uint8_t mic_buffer_count; - -void sound_tick(void) { - if (sound_idle_count > 0U) { - sound_idle_count--; - if (sound_idle_count == 0U) { - current_board->set_amp_enabled(false); - register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); - } - } - - if (mic_idle_count > 0U) { - mic_idle_count--; - if (mic_idle_count == 0U) { - register_clear_bits(&DFSDM1_Channel0->CHCFGR1, DFSDM_CHCFGR1_DFSDMEN); - mic_buffer_count = 0U; - } - } -} - -// Recording processing -static void DMA1_Stream0_IRQ_Handler(void) { - DMA1->LIFCR |= 0x7DU; // clear flags - - uint8_t tx_buf_idx = (((BDMA_Channel1->CCR & BDMA_CCR_CT) >> BDMA_CCR_CT_Pos) == 1U) ? 0U : 1U; - - if (mic_buffer_count < MIC_SKIP_BUFFERS) { - // Send silence during settling - mic_buffer_count++; - for (uint16_t i = 0U; i < MIC_TX_BUF_SIZE; i++) { - mic_tx_buf[tx_buf_idx][i] = 0U; - } - } else { - // process samples - uint8_t buf_idx = (((DMA1_Stream0->CR & DMA_SxCR_CT) >> DMA_SxCR_CT_Pos) == 1U) ? 0U : 1U; - for (uint16_t i=0U; i < MIC_RX_BUF_SIZE; i++) { - mic_tx_buf[tx_buf_idx][2U*i] = ((mic_rx_buf[buf_idx][i] >> 16U) & 0xFFFFU); - mic_tx_buf[tx_buf_idx][(2U*i)+1U] = mic_tx_buf[tx_buf_idx][2U*i]; - } - } -} - -// Playback processing -static void BDMA_Channel0_IRQ_Handler(void) { - static uint8_t playback_buf = 0U; - - BDMA->IFCR |= BDMA_IFCR_CGIF0; // clear flag - - uint8_t rx_buf_idx = (((BDMA_Channel0->CCR & BDMA_CCR_CT) >> BDMA_CCR_CT_Pos) == 1U) ? 0U : 1U; - playback_buf = 1U - playback_buf; - - // wait until we're done playing the current buf - for (uint32_t timeout_counter = 100000U; timeout_counter > 0U; timeout_counter--){ - uint8_t playing_buf = (DMA1_Stream1->CR & DMA_SxCR_CT) >> DMA_SxCR_CT_Pos; - if ((playing_buf != playback_buf) || ((DMA1_Stream1->CR & DMA_SxCR_EN) == 0U)) { - break; - } - } - - // process samples (shift to 12b and bias to be unsigned) - bool sound_playing = false; - for (uint16_t i=0U; i < SOUND_RX_BUF_SIZE; i += 2U) { - // since we are playing mono and receiving stereo, we take every other sample - sound_tx_buf[playback_buf][i/2U] = ((sound_rx_buf[rx_buf_idx][i] + (1UL << 14)) >> 3); - if (sound_rx_buf[rx_buf_idx][i] > 0U) { - sound_playing = true; - } - } - - // manage amp state - if (sound_playing) { - if (sound_idle_count == 0U) { - current_board->set_amp_enabled(true); - - // empty the other buf and start playing that - for (uint16_t i=0U; i < SOUND_TX_BUF_SIZE; i++) { - sound_tx_buf[1U - playback_buf][i] = (1UL << 11); - } - register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); - DMA1_Stream1->CR = (DMA1_Stream1->CR & ~DMA_SxCR_CT_Msk) | ((1UL - playback_buf) << DMA_SxCR_CT_Pos); - register_set_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); - } - sound_idle_count = SOUND_IDLE_TIMEOUT; - } - - // manage mic state - if (mic_idle_count == 0U) { - register_set_bits(&DFSDM1_Channel0->CHCFGR1, DFSDM_CHCFGR1_DFSDMEN); - DFSDM1_Filter0->FLTCR1 |= DFSDM_FLTCR1_RSWSTART; - } - mic_idle_count = SOUND_IDLE_TIMEOUT; - - sound_tick(); -} - -void sound_init_dac(void) { - // Init DAC - DAC1->DHR12R1 = (1UL << 11); - DAC1->DHR12R2 = (1UL << 11); - register_set(&DAC1->MCR, 0U, 0xFFFFFFFFU); - register_set(&DAC1->CR, DAC_CR_TEN1 | (4U << DAC_CR_TSEL1_Pos) | DAC_CR_DMAEN1, 0xFFFFFFFFU); - register_set_bits(&DAC1->CR, DAC_CR_EN1 | DAC_CR_EN2); - - // Setup DMAMUX (DAC_CH1_DMA as input) - register_set(&DMAMUX1_Channel1->CCR, 67U, DMAMUX_CxCR_DMAREQ_ID_Msk); - - // Setup DMA - register_set(&DMA1_Stream1->PAR, (uint32_t) &(DAC1->DHR12R1), 0xFFFFFFFFU); - register_set(&DMA1_Stream1->M0AR, (uint32_t) sound_tx_buf[0], 0xFFFFFFFFU); - register_set(&DMA1_Stream1->M1AR, (uint32_t) sound_tx_buf[1], 0xFFFFFFFFU); - register_set(&DMA1_Stream1->FCR, 0U, 0x00000083U); - DMA1_Stream1->NDTR = SOUND_TX_BUF_SIZE; - DMA1_Stream1->CR = DMA_SxCR_DBM | (0b11UL << DMA_SxCR_PL_Pos) | (0b01UL << DMA_SxCR_MSIZE_Pos) | (0b01UL << DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | (1U << DMA_SxCR_DIR_Pos); -} - -static void sound_stop_dac(void) { - register_clear_bits(&BDMA_Channel0->CCR, BDMA_CCR_EN); - BDMA->IFCR = 0xFFFFFFFFU; - - register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN); - while ((DMA1_Stream1->CR & DMA_SxCR_EN) != 0U) {} - DMA1->LIFCR = (0x3FU << 6); - - register_clear_bits(&DAC1->CR, DAC_CR_EN1 | DAC_CR_EN2); - while ((DAC1->CR & (DAC_CR_EN1 | DAC_CR_EN2)) != 0U) {} -} - -void sound_init(void) { - REGISTER_INTERRUPT(BDMA_Channel0_IRQn, BDMA_Channel0_IRQ_Handler, 128U, FAULT_INTERRUPT_RATE_SOUND_DMA) - REGISTER_INTERRUPT(DMA1_Stream0_IRQn, DMA1_Stream0_IRQ_Handler, 128U, FAULT_INTERRUPT_RATE_SOUND_DMA) - - // Init DAC and its DMA - sound_init_dac(); - - // Init trigger timer (little slower than 48kHz, pulled in sync by SAI4_FS_B) - register_set(&TIM5->PSC, 2600U, 0xFFFFU); - register_set(&TIM5->ARR, 100U, 0xFFFFFFFFU); - register_set(&TIM5->AF1, (0b0010UL << TIM5_AF1_ETRSEL_Pos), TIM5_AF1_ETRSEL_Msk); - register_set(&TIM5->CR2, (0b010U << TIM_CR2_MMS_Pos), TIM_CR2_MMS_Msk); - register_set(&TIM5->SMCR, TIM_SMCR_ECE | (0b00111UL << TIM_SMCR_TS_Pos)| (0b0100UL << TIM_SMCR_SMS_Pos), 0x31FFF7U); - TIM5->CNT = 0U; TIM5->SR = 0U; - TIM5->CR1 |= TIM_CR1_CEN; - - // sync both SAIs - register_set(&SAI4->GCR, (0b10UL << SAI_GCR_SYNCOUT_Pos), SAI_GCR_SYNCIN_Msk | SAI_GCR_SYNCOUT_Msk); - - // stereo audio in - register_set(&SAI4_Block_B->CR1, SAI_xCR1_DMAEN | (0b00UL << SAI_xCR1_SYNCEN_Pos) | (0b100U << SAI_xCR1_DS_Pos) | (0b11U << SAI_xCR1_MODE_Pos), 0x0FFB3FEFU); - register_set(&SAI4_Block_B->CR2, (0b001U << SAI_xCR2_FTH_Pos), 0xFFFBU); - register_set(&SAI4_Block_B->FRCR, (31U << SAI_xFRCR_FRL_Pos), 0x7FFFFU); - register_set(&SAI4_Block_B->SLOTR, (0b11UL << SAI_xSLOTR_SLOTEN_Pos) | (1UL << SAI_xSLOTR_NBSLOT_Pos) | (0b01UL << SAI_xSLOTR_SLOTSZ_Pos), 0xFFFF0FDFU); // NBSLOT definition is vague - - // init sound DMA (SAI4_B -> memory, double buffers) - register_set(&BDMA_Channel0->CPAR, (uint32_t) &(SAI4_Block_B->DR), 0xFFFFFFFFU); - register_set(&BDMA_Channel0->CM0AR, (uint32_t) sound_rx_buf[0], 0xFFFFFFFFU); - register_set(&BDMA_Channel0->CM1AR, (uint32_t) sound_rx_buf[1], 0xFFFFFFFFU); - BDMA_Channel0->CNDTR = SOUND_RX_BUF_SIZE; - register_set(&BDMA_Channel0->CCR, BDMA_CCR_DBM | (0b01UL << BDMA_CCR_MSIZE_Pos) | (0b01UL << BDMA_CCR_PSIZE_Pos) | BDMA_CCR_MINC | BDMA_CCR_CIRC | BDMA_CCR_TCIE, 0xFFFFU); - register_set(&DMAMUX2_Channel0->CCR, 16U, DMAMUX_CxCR_DMAREQ_ID_Msk); // SAI4_B_DMA - register_set_bits(&BDMA_Channel0->CCR, BDMA_CCR_EN); - - // mic output - register_set(&SAI4_Block_A->CR1, SAI_xCR1_DMAEN | (0b01UL << SAI_xCR1_SYNCEN_Pos) | (0b100UL << SAI_xCR1_DS_Pos) | (0b10UL << SAI_xCR1_MODE_Pos), 0x0FFB3FEFU); - register_set(&SAI4_Block_A->CR2, 0U, 0xFFFBU); - register_set(&SAI4_Block_A->FRCR, (31U << SAI_xFRCR_FRL_Pos), 0x7FFFFU); - register_set(&SAI4_Block_A->SLOTR, (0b11UL << SAI_xSLOTR_SLOTEN_Pos) | (1UL << SAI_xSLOTR_NBSLOT_Pos) | (0b01U << SAI_xSLOTR_SLOTSZ_Pos), 0xFFFF0FDFU); // NBSLOT definition is vague - - // init DFSDM for PDM mic - register_set(&DFSDM1_Channel0->CHCFGR1, (76UL << DFSDM_CHCFGR1_CKOUTDIV_Pos) | DFSDM_CHCFGR1_CHEN, 0xC0FFF1EFU); // CH0 controls the clock - register_set(&DFSDM1_Channel3->CHCFGR1, (0b01UL << DFSDM_CHCFGR1_SPICKSEL_Pos) | (0b00U << DFSDM_CHCFGR1_SITP_Pos) | DFSDM_CHCFGR1_CHEN, 0x0000F1EFU); // SITP determines sample edge - register_set(&DFSDM1_Filter0->FLTFCR, (0U << DFSDM_FLTFCR_IOSR_Pos) | (64UL << DFSDM_FLTFCR_FOSR_Pos) | (4UL << DFSDM_FLTFCR_FORD_Pos), 0xE3FF00FFU); - register_set(&DFSDM1_Filter0->FLTCR1, DFSDM_FLTCR1_FAST | (3UL << DFSDM_FLTCR1_RCH_Pos) | DFSDM_FLTCR1_RDMAEN | DFSDM_FLTCR1_RCONT | DFSDM_FLTCR1_DFEN, 0x672E7F3BU); - - // DMA (DFSDM1 -> memory) - register_set(&DMA1_Stream0->PAR, (uint32_t) &DFSDM1_Filter0->FLTRDATAR, 0xFFFFFFFFU); - register_set(&DMA1_Stream0->M0AR, (uint32_t)mic_rx_buf[0], 0xFFFFFFFFU); - register_set(&DMA1_Stream0->M1AR, (uint32_t)mic_rx_buf[1], 0xFFFFFFFFU); - DMA1_Stream0->NDTR = MIC_RX_BUF_SIZE; - register_set(&DMA1_Stream0->CR, DMA_SxCR_DBM | (0b10UL << DMA_SxCR_MSIZE_Pos) | (0b10UL << DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | DMA_SxCR_CIRC | DMA_SxCR_TCIE, 0x01F7FFFFU); - register_set(&DMAMUX1_Channel0->CCR, 101U, DMAMUX_CxCR_DMAREQ_ID_Msk); // DFSDM1_DMA0 - register_set_bits(&DMA1_Stream0->CR, DMA_SxCR_EN); - DMA1->LIFCR |= 0x7DU; // clear flags - - // DMA (memory -> SAI4) - register_set(&BDMA_Channel1->CPAR, (uint32_t) &(SAI4_Block_A->DR), 0xFFFFFFFFU); - register_set(&BDMA_Channel1->CM0AR, (uint32_t) mic_tx_buf[0], 0xFFFFFFFFU); - register_set(&BDMA_Channel1->CM1AR, (uint32_t) mic_tx_buf[1], 0xFFFFFFFFU); - BDMA_Channel1->CNDTR = MIC_TX_BUF_SIZE; - register_set(&BDMA_Channel1->CCR, BDMA_CCR_DBM | (0b01UL << BDMA_CCR_MSIZE_Pos) |(0b01UL << BDMA_CCR_PSIZE_Pos) | BDMA_CCR_MINC | BDMA_CCR_CIRC | (0b1U << BDMA_CCR_DIR_Pos), 0xFFFFU); - register_set(&DMAMUX2_Channel1->CCR, 15U, DMAMUX_CxCR_DMAREQ_ID_Msk); // SAI4_A_DMA - register_set_bits(&BDMA_Channel1->CCR, BDMA_CCR_EN); - // enable all initted blocks - register_set_bits(&SAI4_Block_A->CR1, SAI_xCR1_SAIEN); - register_set_bits(&SAI4_Block_B->CR1, SAI_xCR1_SAIEN); - NVIC_EnableIRQ(BDMA_Channel0_IRQn); - NVIC_EnableIRQ(DMA1_Stream0_IRQn); -} +void sound_tick(void); +void sound_init_dac(void); +void sound_init(void); +void sound_stop_dac(void); \ No newline at end of file diff --git a/board/stm32h7/stm32h7_config.c b/board/stm32h7/stm32h7_config.c new file mode 100644 index 00000000000..309002443c0 --- /dev/null +++ b/board/stm32h7/stm32h7_config.c @@ -0,0 +1,8 @@ +#include "stm32h7_config.h" + +void early_gpio_float(void) { + RCC->AHB4ENR = RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN | RCC_AHB4ENR_GPIOCEN | RCC_AHB4ENR_GPIODEN | RCC_AHB4ENR_GPIOEEN | RCC_AHB4ENR_GPIOFEN | RCC_AHB4ENR_GPIOGEN | RCC_AHB4ENR_GPIOHEN; + GPIOA->MODER = 0xAB000000U; GPIOB->MODER = 0; GPIOC->MODER = 0; GPIOD->MODER = 0; GPIOE->MODER = 0; GPIOF->MODER = 0; GPIOG->MODER = 0; GPIOH->MODER = 0; + GPIOA->ODR = 0; GPIOB->ODR = 0; GPIOC->ODR = 0; GPIOD->ODR = 0; GPIOE->ODR = 0; GPIOF->ODR = 0; GPIOG->ODR = 0; GPIOH->ODR = 0; + GPIOA->PUPDR = 0; GPIOB->PUPDR = 0; GPIOC->PUPDR = 0; GPIOD->PUPDR = 0; GPIOE->PUPDR = 0; GPIOF->PUPDR = 0; GPIOG->PUPDR = 0; GPIOH->PUPDR = 0; +} diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h index 1a3b413620a..90c4e064495 100644 --- a/board/stm32h7/stm32h7_config.h +++ b/board/stm32h7/stm32h7_config.h @@ -1,3 +1,5 @@ +#pragma once + #include "stm32h7xx.h" #include "stm32h7xx_hal_gpio_ex.h" #define MCU_IDCODE 0x483U @@ -46,11 +48,11 @@ separate IRQs for RX and TX. #include "board/can.h" #include "board/comms_definitions.h" -#ifndef BOOTSTUB - #include "board/main_definitions.h" -#else - #include "board/bootstub_declarations.h" -#endif +// #ifndef BOOTSTUB +// #include "board/main_declarations.h" +// #else +// #include "board/bootstub_declarations.h" +// #endif #include "board/libc.h" #include "board/critical.h" @@ -89,9 +91,4 @@ separate IRQs for RX and TX. #include "board/drivers/spi.h" #include "board/stm32h7/llspi.h" -void early_gpio_float(void) { - RCC->AHB4ENR = RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN | RCC_AHB4ENR_GPIOCEN | RCC_AHB4ENR_GPIODEN | RCC_AHB4ENR_GPIOEEN | RCC_AHB4ENR_GPIOFEN | RCC_AHB4ENR_GPIOGEN | RCC_AHB4ENR_GPIOHEN; - GPIOA->MODER = 0xAB000000U; GPIOB->MODER = 0; GPIOC->MODER = 0; GPIOD->MODER = 0; GPIOE->MODER = 0; GPIOF->MODER = 0; GPIOG->MODER = 0; GPIOH->MODER = 0; - GPIOA->ODR = 0; GPIOB->ODR = 0; GPIOC->ODR = 0; GPIOD->ODR = 0; GPIOE->ODR = 0; GPIOF->ODR = 0; GPIOG->ODR = 0; GPIOH->ODR = 0; - GPIOA->PUPDR = 0; GPIOB->PUPDR = 0; GPIOC->PUPDR = 0; GPIOD->PUPDR = 0; GPIOE->PUPDR = 0; GPIOF->PUPDR = 0; GPIOG->PUPDR = 0; GPIOH->PUPDR = 0; -} +void early_gpio_float(void); \ No newline at end of file diff --git a/board/utils.c b/board/utils.c new file mode 100644 index 00000000000..6c31eec8193 --- /dev/null +++ b/board/utils.c @@ -0,0 +1,9 @@ +#include + +#include "utils.h" + +// compute the time elapsed (in microseconds) from 2 counter samples +// case where ts < ts_last is ok: overflow is properly re-casted into uint32_t +uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) { + return ts - ts_last; +} diff --git a/board/utils.h b/board/utils.h index f355ce8c2f2..30699a0f047 100644 --- a/board/utils.h +++ b/board/utils.h @@ -1,3 +1,5 @@ +#pragma once + // cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension #define MIN(a, b) ({ \ __typeof__ (a) _a = (a); \ @@ -42,6 +44,4 @@ // compute the time elapsed (in microseconds) from 2 counter samples // case where ts < ts_last is ok: overflow is properly re-casted into uint32_t -uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) { - return ts - ts_last; -} +uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last); \ No newline at end of file diff --git a/setup.sh b/setup.sh index 042b4978af1..cea01d5aff1 100755 --- a/setup.sh +++ b/setup.sh @@ -7,36 +7,7 @@ cd $DIR PLATFORM=$(uname -s) echo "installing dependencies" -if [[ $PLATFORM == "Darwin" ]]; then - export HOMEBREW_NO_AUTO_UPDATE=1 - brew install --cask gcc-arm-embedded - brew install python3 gcc@13 -elif [[ $PLATFORM == "Linux" ]]; then - # for AGNOS since we clear the apt lists - if [[ ! -d /"var/lib/apt/" ]]; then - sudo apt update - fi - - sudo apt-get install -y --no-install-recommends \ - curl ca-certificates \ - make g++ git libnewlib-arm-none-eabi \ - libusb-1.0-0 \ - gcc-arm-none-eabi \ - python3-dev python3-pip python3-venv -else - echo "WARNING: unsupported platform. skipping apt/brew install." -fi - -if ! command -v uv &>/dev/null; then - echo "'uv' is not installed. Installing 'uv'..." - curl -LsSf https://astral.sh/uv/install.sh | sh - - # doesn't require sourcing on all platforms - set +e - source $HOME/.local/bin/env - set -e -fi export UV_PROJECT_ENVIRONMENT="$DIR/.venv" -uv sync --all-extras --upgrade +# uv sync --all-extras --upgrade source "$DIR/.venv/bin/activate" From 20c7ae689aa433a101c53f87d7cd98bee8046949 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 28 Nov 2025 17:12:20 +0100 Subject: [PATCH 02/29] Progress 2 --- SConscript | 15 +++++-------- board/boards/red.c | 1 + board/body/motor_encoder.h | 28 +----------------------- board/bootstub.c | 3 +++ board/can_comms.c | 2 ++ board/config.h | 1 - board/critical.h | 16 +++++++++++++- board/critical_declarations.h | 19 ---------------- board/drivers/can_common.c | 3 --- board/drivers/can_common.h | 3 +-- board/drivers/fake_siren.c | 1 + board/drivers/fake_siren.h | 2 -- board/drivers/fdcan.c | 4 ---- board/drivers/fdcan.h | 2 +- board/drivers/gpio.h | 2 +- board/drivers/led.c | 3 ++- board/drivers/registers.h | 18 ++++----------- board/drivers/registers_declarations.h | 3 --- board/drivers/spi.c | 1 - board/drivers/spi.h | 3 --- board/early_init.c | 7 ++---- board/faults.c | 1 + board/faults.h | 2 +- board/faults_declarations.h | 1 - board/flasher.c | 2 ++ board/globals copy.h | 10 --------- board/globals.h | 4 +++- board/jungle/boards/board_declarations.h | 12 +++++++--- board/provision.c | 4 +--- board/stm32h7/lladc.h | 1 - 30 files changed, 57 insertions(+), 117 deletions(-) delete mode 100644 board/critical_declarations.h delete mode 100644 board/drivers/registers_declarations.h delete mode 100644 board/faults_declarations.h delete mode 100644 board/globals copy.h diff --git a/SConscript b/SConscript index b5ba1d03cd3..4f848d8ae51 100644 --- a/SConscript +++ b/SConscript @@ -137,9 +137,6 @@ def build_project(project_name, project, main, extra_flags): "./board/utils.c", ] - # # "./board/libc.c", - # # "./board/can_comms.c", - # # "./board/critical.c", # # "./board/main_definitions.c", # Build bootstub @@ -177,7 +174,7 @@ def build_project(project_name, project, main, extra_flags): ] + shared, LINKFLAGS=[f"-Wl,--section-start,.isr_vector={project['APP_START_ADDRESS']}"] + flags) main_bin = env.Objcopy(f"{project_dir}/main.bin", main_elf) sign_py = File(f"./crypto/sign.py").srcnode().relpath - # env.Command(f"./board/obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}") + env.Command(f"./board/obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}") @@ -213,11 +210,11 @@ with open("board/obj/cert.h", "w") as f: build_project("panda_h7", base_project_h7, "./board/main.c", []) # # panda jungle fw -# flags = [ -# "-DPANDA_JUNGLE", -# ] -# if os.getenv("FINAL_PROVISIONING"): -# flags += ["-DFINAL_PROVISIONING"] +flags = [ + "-DPANDA_JUNGLE", +] +if os.getenv("FINAL_PROVISIONING"): + flags += ["-DFINAL_PROVISIONING"] # build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", flags) # # body fw diff --git a/board/boards/red.c b/board/boards/red.c index 9aadcf04c62..c4b9a2cf0fd 100644 --- a/board/boards/red.c +++ b/board/boards/red.c @@ -1,6 +1,7 @@ #include "red.h" #include "board/boards/board_declarations.h" +#include "board/boards/unused_funcs.h" // ///////////////////////////// // // Red Panda (STM32H7) + Harness // diff --git a/board/body/motor_encoder.h b/board/body/motor_encoder.h index 7c79f9f4d04..dc5a896e9cb 100644 --- a/board/body/motor_encoder.h +++ b/board/body/motor_encoder.h @@ -32,31 +32,5 @@ typedef struct { float cached_speed_rps; } motor_encoder_state_t; -static const motor_encoder_config_t motor_encoder_config[BODY_MOTOR_COUNT] = { - [BODY_MOTOR_LEFT - 1U] = { - .timer = TIM4, - .pin_a_port = GPIOB, .pin_a = 6U, .pin_a_af = GPIO_AF2_TIM4, - .pin_b_port = GPIOB, .pin_b = 7U, .pin_b_af = GPIO_AF2_TIM4, - .direction = -1, - .counts_per_output_rev = 44U * 90U, - .min_dt_us = 250U, - .speed_alpha = 0.2f, - .filter = 3U, - }, - [BODY_MOTOR_RIGHT - 1U] = { - .timer = TIM3, - .pin_a_port = GPIOA, .pin_a = 6U, .pin_a_af = GPIO_AF2_TIM3, - .pin_b_port = GPIOA, .pin_b = 7U, .pin_b_af = GPIO_AF2_TIM3, - .direction = 1, - .counts_per_output_rev = 44U * 90U, - .min_dt_us = 250U, - .speed_alpha = 0.2f, - .filter = 3U, - }, -}; - -static motor_encoder_state_t motor_encoders[BODY_MOTOR_COUNT] = { - { .config = &motor_encoder_config[0] }, - { .config = &motor_encoder_config[1] }, -}; +float motor_encoder_get_speed_rpm(uint8_t motor); \ No newline at end of file diff --git a/board/bootstub.c b/board/bootstub.c index 1430bd5b616..ee2552c83c6 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -22,6 +22,9 @@ #include "board/obj/gitversion.h" #include "board/flasher.h" +#include "globals.h" + + // TODO uint8_t hw_type; board *current_board; diff --git a/board/can_comms.c b/board/can_comms.c index dc51eea9d36..52a031b9755 100644 --- a/board/can_comms.c +++ b/board/can_comms.c @@ -5,6 +5,8 @@ #include "can.h" #include "drivers/usb.h" +#include "config.h" + static asm_buffer can_read_buffer = {.ptr = 0U, .tail_size = 0U}; int comms_can_read(uint8_t *data, uint32_t max_len) { diff --git a/board/config.h b/board/config.h index d485b7b27ad..aed07207675 100644 --- a/board/config.h +++ b/board/config.h @@ -36,7 +36,6 @@ #ifdef STM32H7 #include "board/stm32h7/stm32h7_config.h" #else - #error help me! // TODO: uncomment this, cppcheck complains // building for tests #include "fake_stm.h" diff --git a/board/critical.h b/board/critical.h index af58316613b..4b428cc9de8 100644 --- a/board/critical.h +++ b/board/critical.h @@ -1,10 +1,24 @@ +#pragma once + #include #include -#include "critical_declarations.h" +#include "config.h" // ********************* Critical section helpers ********************* extern uint8_t global_critical_depth; + +#define ENTER_CRITICAL() \ + __disable_irq(); \ + global_critical_depth += 1U; + +#define EXIT_CRITICAL() \ + global_critical_depth -= 1U; \ + if ((global_critical_depth == 0U) && interrupts_enabled) { \ + __enable_irq(); \ + } + +extern uint8_t global_critical_depth; static volatile bool interrupts_enabled; void enable_interrupts(void); diff --git a/board/critical_declarations.h b/board/critical_declarations.h deleted file mode 100644 index b0b35e2da9a..00000000000 --- a/board/critical_declarations.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include "config.h" - -// ********************* Critical section helpers ********************* -void enable_interrupts(void); -void disable_interrupts(void); - -extern uint8_t global_critical_depth; - -#define ENTER_CRITICAL() \ - __disable_irq(); \ - global_critical_depth += 1U; - -#define EXIT_CRITICAL() \ - global_critical_depth -= 1U; \ - if ((global_critical_depth == 0U) && interrupts_enabled) { \ - __enable_irq(); \ - } diff --git a/board/drivers/can_common.c b/board/drivers/can_common.c index b4f897f8998..2467bf445ad 100644 --- a/board/drivers/can_common.c +++ b/board/drivers/can_common.c @@ -3,9 +3,6 @@ #include "board/can_comms.h" #include "timers.h" #include "board/utils.h" -// #include "opendbc/safety/safety.h" - -#include "board/critical_declarations.h" uint32_t safety_tx_blocked = 0; uint32_t safety_rx_invalid = 0; diff --git a/board/drivers/can_common.h b/board/drivers/can_common.h index 28e7ddfeb70..ac2d228f3c5 100644 --- a/board/drivers/can_common.h +++ b/board/drivers/can_common.h @@ -77,9 +77,8 @@ bool is_speed_valid(uint32_t speed, const uint32_t *all_speeds, uint8_t len); #include "timers.h" #include "board/utils.h" #include "opendbc/safety/declarations.h" -// #include "opendbc/safety/safety.h" -#include "board/critical_declarations.h" +#include "board/critical.h" extern uint32_t safety_tx_blocked; extern uint32_t safety_rx_invalid; diff --git a/board/drivers/fake_siren.c b/board/drivers/fake_siren.c index debe49e5021..189bb12ba34 100644 --- a/board/drivers/fake_siren.c +++ b/board/drivers/fake_siren.c @@ -1,5 +1,6 @@ #include "fake_siren.h" +#include "board/stm32h7/lli2c.h" #include "board/globals.h" void siren_tim7_init(void) { diff --git a/board/drivers/fake_siren.h b/board/drivers/fake_siren.h index 1ff7dda5b7a..3bc99837f11 100644 --- a/board/drivers/fake_siren.h +++ b/board/drivers/fake_siren.h @@ -2,8 +2,6 @@ #include -#include "board/stm32h7/lli2c.h" - #define CODEC_I2C_ADDR 0x10 void siren_tim7_init(void); diff --git a/board/drivers/fdcan.c b/board/drivers/fdcan.c index 05665455143..dd2d3442962 100644 --- a/board/drivers/fdcan.c +++ b/board/drivers/fdcan.c @@ -1,5 +1,4 @@ #include "fdcan.h" -// #include "board/stm32h7/stm32h7_config.h" #include "can_common.h" @@ -7,9 +6,6 @@ #include "led.h" -// FIXME! -#include "board/stm32h7/llfdcan.h" - FDCAN_GlobalTypeDef *cans[PANDA_CAN_CNT] = {FDCAN1, FDCAN2, FDCAN3}; static bool can_set_speed(uint8_t can_number) { diff --git a/board/drivers/fdcan.h b/board/drivers/fdcan.h index a7a488d3be8..0c5180477ce 100644 --- a/board/drivers/fdcan.h +++ b/board/drivers/fdcan.h @@ -3,7 +3,7 @@ #include #include "board/can.h" -#include "board/stm32h7/stm32h7_config.h" +#include "board/config.h" typedef struct { volatile uint32_t header[2]; diff --git a/board/drivers/gpio.h b/board/drivers/gpio.h index 3948dc09bd3..86bc7a3e18c 100644 --- a/board/drivers/gpio.h +++ b/board/drivers/gpio.h @@ -41,4 +41,4 @@ void gpio_set_bitmask(gpio_t *pins, uint8_t num_pins, uint32_t bitmask) { #endif // Detection with internal pullup -bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode); \ No newline at end of file +bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode); diff --git a/board/drivers/led.c b/board/drivers/led.c index 860f008eeaf..fd5b918b28a 100644 --- a/board/drivers/led.c +++ b/board/drivers/led.c @@ -1,5 +1,6 @@ #include "led.h" -#include "../main_declarations.h" +#include "board/globals.h" +#include "pwm.h" void led_set(uint8_t color, bool enabled) { if (color < 3U) { diff --git a/board/drivers/registers.h b/board/drivers/registers.h index 913690ea09e..7aa617395d8 100644 --- a/board/drivers/registers.h +++ b/board/drivers/registers.h @@ -29,15 +29,10 @@ #define FAULT_HEARTBEAT_LOOP_WATCHDOG (1UL << 26) #define FAULT_INTERRUPT_RATE_SOUND_DMA (1UL << 27) -// Permanent faults -#define PERMANENT_FAULTS 0U - -extern uint8_t fault_status; -extern uint32_t faults; - -void fault_occurred(uint32_t fault); -void fault_recovered(uint32_t fault); - +// 10 bit hash with 23 as a prime +#define REGISTER_MAP_SIZE 0x3FFU +#define HASHING_PRIME 23U +#define CHECK_COLLISION(hash, addr) (((uint32_t) register_map[hash].address != 0U) && (register_map[hash].address != (addr))) typedef struct reg { volatile uint32_t *address; @@ -46,11 +41,6 @@ typedef struct reg { bool logged_fault; } reg; -// 10 bit hash with 23 as a prime -#define REGISTER_MAP_SIZE 0x3FFU -#define HASHING_PRIME 23U -#define CHECK_COLLISION(hash, addr) (((uint32_t) register_map[hash].address != 0U) && (register_map[hash].address != (addr))) - // Do not put bits in the check mask that get changed by the hardware void register_set(volatile uint32_t *addr, uint32_t val, uint32_t mask); // Set individual bits. Also add them to the check_mask. diff --git a/board/drivers/registers_declarations.h b/board/drivers/registers_declarations.h deleted file mode 100644 index aa9056261c4..00000000000 --- a/board/drivers/registers_declarations.h +++ /dev/null @@ -1,3 +0,0 @@ -#pragma once - -#include "registers.h" diff --git a/board/drivers/spi.c b/board/drivers/spi.c index 3277883928b..b5fb39afc96 100644 --- a/board/drivers/spi.c +++ b/board/drivers/spi.c @@ -1,4 +1,3 @@ - #include "spi.h" #include "board/crc.h" diff --git a/board/drivers/spi.h b/board/drivers/spi.h index a7182fc283d..bf9122861a3 100644 --- a/board/drivers/spi.h +++ b/board/drivers/spi.h @@ -3,9 +3,6 @@ #include #include -#include "board/drivers/spi.h" -#include "board/crc.h" - #define SPI_TIMEOUT_US 10000U // got max rate from hitting a non-existent endpoint // in a tight loop, plus some buffer diff --git a/board/early_init.c b/board/early_init.c index fc6afb3cfbd..a6e420f599d 100644 --- a/board/early_init.c +++ b/board/early_init.c @@ -1,12 +1,9 @@ #include #include "early_init.h" - -#include "board/stm32h7/stm32h7_config.h" - #include "drivers/led.h" - -#include "bootstub_declarations.h" +#include "globals.h" +#include "board/config.h" extern void *g_pfnVectors; extern uint32_t enter_bootloader_mode; diff --git a/board/faults.c b/board/faults.c index 4a58e5e0e82..6ce6a59b501 100644 --- a/board/faults.c +++ b/board/faults.c @@ -1,4 +1,5 @@ #include "faults.h" +#include "board/drivers/registers.h" uint8_t fault_status = FAULT_STATUS_NONE; uint32_t faults = 0U; diff --git a/board/faults.h b/board/faults.h index 1c78b8bce46..84bd80edd13 100644 --- a/board/faults.h +++ b/board/faults.h @@ -1,6 +1,6 @@ #include -#include "faults_declarations.h" +#define PERMANENT_FAULTS 0U extern uint8_t fault_status; extern uint32_t faults; diff --git a/board/faults_declarations.h b/board/faults_declarations.h deleted file mode 100644 index 84738ddb5e5..00000000000 --- a/board/faults_declarations.h +++ /dev/null @@ -1 +0,0 @@ -#include "board/drivers/registers.h" \ No newline at end of file diff --git a/board/flasher.c b/board/flasher.c index cccdaaf06d9..712634360c6 100644 --- a/board/flasher.c +++ b/board/flasher.c @@ -9,6 +9,8 @@ #include "early_init.h" #include "obj/gitversion.h" +#include "board/config.h" + // from the linker script #define APP_START_ADDRESS 0x8020000U diff --git a/board/globals copy.h b/board/globals copy.h deleted file mode 100644 index af9c489aac6..00000000000 --- a/board/globals copy.h +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once - -#include - -#include "boards/board_declarations.h" - -typedef struct board board; - -extern uint8_t hw_type; -extern board *current_board; diff --git a/board/globals.h b/board/globals.h index 6d8575c250b..2f884be2d3b 100644 --- a/board/globals.h +++ b/board/globals.h @@ -3,8 +3,10 @@ #include #include "boards/board_declarations.h" +#include "board/drivers/harness.h" typedef struct board board; extern uint8_t hw_type; -extern board *current_board; \ No newline at end of file +extern board *current_board; +extern struct harness_t harness; \ No newline at end of file diff --git a/board/jungle/boards/board_declarations.h b/board/jungle/boards/board_declarations.h index aad54dede70..8793f606b34 100644 --- a/board/jungle/boards/board_declarations.h +++ b/board/jungle/boards/board_declarations.h @@ -1,3 +1,9 @@ +#pragma once + +#include + +#include "board/config.h" + // ******************** Prototypes ******************** typedef void (*board_init)(void); typedef void (*board_board_tick)(void); @@ -55,6 +61,6 @@ struct board { #define SBU2 1U // ********************* Globals ********************** -uint8_t harness_orientation = HARNESS_ORIENTATION_NONE; -uint8_t can_mode = CAN_MODE_NORMAL; -uint8_t ignition = 0U; +extern uint8_t harness_orientation; +extern uint8_t can_mode; +extern uint8_t ignition; diff --git a/board/provision.c b/board/provision.c index bcf5c41ff0f..fbe14b49999 100644 --- a/board/provision.c +++ b/board/provision.c @@ -2,9 +2,7 @@ // manufacturing. aside from this, there's a UID for the MCU #include "provision.h" -#include "board/stm32h7/stm32h7_config.h" - -#define PROVISION_CHUNK_LEN 0x20 +#include "config.h" void get_provision_chunk(uint8_t *resp) { const unsigned char unprovisioned_text[] = "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff"; diff --git a/board/stm32h7/lladc.h b/board/stm32h7/lladc.h index 347bb0b7905..beea312e96a 100644 --- a/board/stm32h7/lladc.h +++ b/board/stm32h7/lladc.h @@ -2,7 +2,6 @@ #include -// #include "board/stm32h7/stm32h7_config.h" // TODO: I don't know man... #include "stm32h7xx.h" From 8efa1884c259fd18e54792d173a6e47f3b596be9 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 28 Nov 2025 17:25:30 +0100 Subject: [PATCH 03/29] Don't break the setup code... --- setup.sh | 31 ++++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/setup.sh b/setup.sh index cea01d5aff1..042b4978af1 100755 --- a/setup.sh +++ b/setup.sh @@ -7,7 +7,36 @@ cd $DIR PLATFORM=$(uname -s) echo "installing dependencies" +if [[ $PLATFORM == "Darwin" ]]; then + export HOMEBREW_NO_AUTO_UPDATE=1 + brew install --cask gcc-arm-embedded + brew install python3 gcc@13 +elif [[ $PLATFORM == "Linux" ]]; then + # for AGNOS since we clear the apt lists + if [[ ! -d /"var/lib/apt/" ]]; then + sudo apt update + fi + + sudo apt-get install -y --no-install-recommends \ + curl ca-certificates \ + make g++ git libnewlib-arm-none-eabi \ + libusb-1.0-0 \ + gcc-arm-none-eabi \ + python3-dev python3-pip python3-venv +else + echo "WARNING: unsupported platform. skipping apt/brew install." +fi + +if ! command -v uv &>/dev/null; then + echo "'uv' is not installed. Installing 'uv'..." + curl -LsSf https://astral.sh/uv/install.sh | sh + + # doesn't require sourcing on all platforms + set +e + source $HOME/.local/bin/env + set -e +fi export UV_PROJECT_ENVIRONMENT="$DIR/.venv" -# uv sync --all-extras --upgrade +uv sync --all-extras --upgrade source "$DIR/.venv/bin/activate" From 1ed0b7516398ebb2f1c2dfa16f07b67ee817ffdf Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Thu, 4 Dec 2025 20:05:01 +0100 Subject: [PATCH 04/29] Body is building --- SConscript | 31 +- board/board_forward.h | 33 ++ board/board_struct.h | 65 +++ board/boards/board_declarations.h | 122 ++--- board/boards/boot_state.h | 7 + board/boards/cuatro.c | 8 +- board/boards/red.c | 1 + board/boards/unused_funcs.h | 1 + board/body/boards/board_body.c | 1 + board/body/boards/board_declarations.h | 60 ++- board/body/can.h | 2 +- board/body/main.c | 8 + board/body/main_comms.h | 4 + board/body/motor_common.h | 4 +- board/body/motor_control.c | 57 +- board/body/motor_control.h | 159 +----- board/body/motor_encoder.c | 138 +++++ board/body/motor_encoder.h | 6 + board/body/stm32h7/board.c | 9 + board/body/stm32h7/board.h | 13 +- board/bootstub_declarations.h | 3 +- board/config.h | 3 + board/critical.h | 4 + board/drivers/can_common.h | 37 +- board/drivers/harness.h | 2 + board/fake_stm.h | 5 + board/jungle/boards/board_declarations.h | 81 +-- board/jungle/stm32h7/board.h | 2 + board/main.c | 348 ++++++++++++ board/main_comms.c | 654 ++++++++++++----------- board/stm32h7/board.c | 4 +- board/stm32h7/stm32h7_config.h | 7 + tests/libpanda/SConscript | 47 +- tests/libpanda/panda.c | 2 +- 34 files changed, 1230 insertions(+), 698 deletions(-) create mode 100644 board/board_forward.h create mode 100644 board/board_struct.h create mode 100644 board/boards/boot_state.h create mode 100644 board/body/motor_encoder.c create mode 100644 board/body/stm32h7/board.c diff --git a/SConscript b/SConscript index 4f848d8ae51..74fa081473f 100644 --- a/SConscript +++ b/SConscript @@ -103,12 +103,11 @@ def build_project(project_name, project, main, extra_flags): shared = [ "./crypto/rsa.c", "./crypto/sha.c", - + "board/body/stm32h7/board.c", "./board/libc.c", "./board/crc.c", "./board/early_init.c", - "./board/stm32h7/board.c", - "./board/boards/red.c", + # "./board/boards/red.c", "./board/critical.c", "./board/drivers/led.c", "./board/drivers/pwm.c", @@ -128,8 +127,8 @@ def build_project(project_name, project, main, extra_flags): "./board/drivers/usb.c", "./board/drivers/spi.c", "./board/drivers/timers.c", - "./board/boards/cuatro.c", - "./board/boards/tres.c", + # "./board/boards/cuatro.c", + # "./board/boards/tres.c", "./board/stm32h7/lladc.c", "./board/stm32h7/llspi.c", "./board/faults.c", @@ -195,7 +194,6 @@ base_project_h7 = { # Common autogenerated includes with open("board/obj/gitversion.h", "w") as f: version = get_version(BUILDER, BUILD_TYPE) - # f.write(f'extern const uint8_t gitversion[{len(version)}];\n') f.write(f'static const uint8_t gitversion[{len(version)}] = "{version}";\n') with open("board/obj/version", "w") as f: @@ -207,7 +205,12 @@ with open("board/obj/cert.h", "w") as f: f.write("\n".join(cert) + "\n") # panda fw -build_project("panda_h7", base_project_h7, "./board/main.c", []) +panda_c = [ + "./board/stm32h7/board.c", + "./board/main.c", +] + +# build_project("panda_h7", base_project_h7, panda_c, ["-DPANDA"]) # # panda jungle fw flags = [ @@ -215,11 +218,19 @@ flags = [ ] if os.getenv("FINAL_PROVISIONING"): flags += ["-DFINAL_PROVISIONING"] + # build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", flags) # # body fw -# build_project("body_h7", base_project_h7, "./board/body/main.c", ["-DPANDA_BODY"]) +body_c = [ + "./board/body/motor_control.c", + "./board/body/main.c", + "./board/body/boards/board_body.c", + "./board/body/motor_encoder.c" +] + +build_project("body_h7", base_project_h7, body_c, ["-DPANDA_BODY"]) # test files -# if GetOption('extras'): -# SConscript('tests/libpanda/SConscript') +if GetOption('extras'): + SConscript('tests/libpanda/SConscript') diff --git a/board/board_forward.h b/board/board_forward.h new file mode 100644 index 00000000000..0a12673e443 --- /dev/null +++ b/board/board_forward.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include + +#include "board/boards/boot_state.h" + +typedef void (*board_init)(void); +typedef void (*board_board_tick)(void); +typedef bool (*board_get_button)(void); +typedef void (*board_init_bootloader)(void); +typedef void (*board_set_panda_power)(bool enabled); +typedef void (*board_set_panda_individual_power)(uint8_t port_num, bool enabled); +typedef void (*board_set_ignition)(bool enabled); +typedef void (*board_set_individual_ignition)(uint8_t bitmask); +typedef void (*board_set_harness_orientation)(uint8_t orientation); +typedef void (*board_set_can_mode)(uint8_t mode); +typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); +typedef void (*board_enable_header_pin)(uint8_t pin_num, bool enabled); +typedef float (*board_get_channel_power)(uint8_t channel); +typedef uint16_t (*board_get_sbu_mV)(uint8_t channel, uint8_t sbu); +typedef void (*board_init)(void); +typedef void (*board_init_bootloader)(void); +typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); +typedef void (*board_set_can_mode)(uint8_t mode); +typedef uint32_t (*board_read_voltage_mV)(void); +typedef uint32_t (*board_read_current_mA)(void); +typedef void (*board_set_ir_power)(uint8_t percentage); +typedef void (*board_set_fan_enabled)(bool enabled); +typedef void (*board_set_siren)(bool enabled); +typedef void (*board_set_bootkick)(BootState state); +typedef bool (*board_read_som_gpio)(void); +typedef void (*board_set_amp_enabled)(bool enabled); diff --git a/board/board_struct.h b/board/board_struct.h new file mode 100644 index 00000000000..05ceaeea6c9 --- /dev/null +++ b/board/board_struct.h @@ -0,0 +1,65 @@ +#pragma once + +#include "board/config.h" + +#include "board/board_forward.h" +#include "board/boards/boot_state.h" + +typedef struct board { + struct harness_configuration *harness_config; + GPIO_TypeDef * const led_GPIO[3]; + const uint8_t led_pin[3]; + const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM + const bool has_spi; + const bool has_fan; + const uint16_t avdd_mV; + const uint8_t fan_enable_cooldown_time; + board_init init; + board_init_bootloader init_bootloader; + board_enable_can_transceiver enable_can_transceiver; + board_set_can_mode set_can_mode; + board_read_voltage_mV read_voltage_mV; + board_read_current_mA read_current_mA; + board_set_ir_power set_ir_power; + board_set_fan_enabled set_fan_enabled; + board_set_siren set_siren; + board_set_bootkick set_bootkick; + board_read_som_gpio read_som_gpio; + board_set_amp_enabled set_amp_enabled; +} board; + + +#ifdef PANDA +#define HW_TYPE_UNKNOWN 0U +#define HW_TYPE_RED_PANDA 7U +#define HW_TYPE_TRES 9U +#define HW_TYPE_CUATRO 10U + +// CAN modes +#define CAN_MODE_NORMAL 0U +#define CAN_MODE_OBD_CAN2 1U + +#elif defined(PANDA_JUNGLE) +#define HW_TYPE_UNKNOWN 0U +#define HW_TYPE_V2 2U + +// CAN modes +#define CAN_MODE_NORMAL 0U +#define CAN_MODE_OBD_CAN2 3U + +// Harness states +#define HARNESS_ORIENTATION_NONE 0U +#define HARNESS_ORIENTATION_1 1U +#define HARNESS_ORIENTATION_2 2U + +#define SBU1 0U +#define SBU2 1U + +#elif defined(PANDA_JUNGLE) + +#elif defined(PANDA_BODY) +#elif defined(LIB_PANDA) +// NO IDEA BOI +#else +#error FUCK YOU +#endif \ No newline at end of file diff --git a/board/boards/board_declarations.h b/board/boards/board_declarations.h index fd88961f3c2..58f7f15cfdb 100644 --- a/board/boards/board_declarations.h +++ b/board/boards/board_declarations.h @@ -1,67 +1,59 @@ #pragma once -#include -#include - -// ******************** Prototypes ******************** -typedef enum { - BOOT_STANDBY, - BOOT_BOOTKICK, - BOOT_RESET, -} BootState; - -typedef void (*board_init)(void); -typedef void (*board_init_bootloader)(void); -typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); -typedef void (*board_set_can_mode)(uint8_t mode); -typedef uint32_t (*board_read_voltage_mV)(void); -typedef uint32_t (*board_read_current_mA)(void); -typedef void (*board_set_ir_power)(uint8_t percentage); -typedef void (*board_set_fan_enabled)(bool enabled); -typedef void (*board_set_siren)(bool enabled); -typedef void (*board_set_bootkick)(BootState state); -typedef bool (*board_read_som_gpio)(void); -typedef void (*board_set_amp_enabled)(bool enabled); - -// #include "board/drivers/harness_declarations.h" -#include "board/config.h" - -struct harness_configuration; - -struct board { - struct harness_configuration *harness_config; - GPIO_TypeDef * const led_GPIO[3]; - const uint8_t led_pin[3]; - const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM - const bool has_spi; - const bool has_fan; - const uint16_t avdd_mV; - const uint8_t fan_enable_cooldown_time; - board_init init; - board_init_bootloader init_bootloader; - board_enable_can_transceiver enable_can_transceiver; - board_set_can_mode set_can_mode; - board_read_voltage_mV read_voltage_mV; - board_read_current_mA read_current_mA; - board_set_ir_power set_ir_power; - board_set_fan_enabled set_fan_enabled; - board_set_siren set_siren; - board_set_bootkick set_bootkick; - board_read_som_gpio read_som_gpio; - board_set_amp_enabled set_amp_enabled; -}; - -// ******************* Definitions ******************** -// These should match the enums in cereal/log.capnp and __init__.py -#define HW_TYPE_UNKNOWN 0U -#define HW_TYPE_RED_PANDA 7U -#define HW_TYPE_TRES 9U -#define HW_TYPE_CUATRO 10U - -// CAN modes -#define CAN_MODE_NORMAL 0U -#define CAN_MODE_OBD_CAN2 1U - -extern struct board board_tres; -extern struct board board_cuatro; -extern struct board board_red; +#include "board/board_struct.h" + +// #include +// #include + +// // ******************** Prototypes ******************** + +// #include "boot_state.h" +// #include "board/board_forward.h" + +// typedef void (*board_init)(void); +// typedef void (*board_init_bootloader)(void); +// typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); +// typedef void (*board_set_can_mode)(uint8_t mode); +// typedef uint32_t (*board_read_voltage_mV)(void); +// typedef uint32_t (*board_read_current_mA)(void); +// typedef void (*board_set_ir_power)(uint8_t percentage); +// typedef void (*board_set_fan_enabled)(bool enabled); +// typedef void (*board_set_siren)(bool enabled); +// typedef void (*board_set_bootkick)(BootState state); +// typedef bool (*board_read_som_gpio)(void); +// typedef void (*board_set_amp_enabled)(bool enabled); + +// // #include "board/drivers/harness_declarations.h" +// #include "board/config.h" + +// struct harness_configuration; + +// typedef struct board { +// struct harness_configuration *harness_config; +// GPIO_TypeDef * const led_GPIO[3]; +// const uint8_t led_pin[3]; +// const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM +// const bool has_spi; +// const bool has_fan; +// const uint16_t avdd_mV; +// const uint8_t fan_enable_cooldown_time; +// board_init init; +// board_init_bootloader init_bootloader; +// board_enable_can_transceiver enable_can_transceiver; +// board_set_can_mode set_can_mode; +// board_read_voltage_mV read_voltage_mV; +// board_read_current_mA read_current_mA; +// board_set_ir_power set_ir_power; +// board_set_fan_enabled set_fan_enabled; +// board_set_siren set_siren; +// board_set_bootkick set_bootkick; +// board_read_som_gpio read_som_gpio; +// board_set_amp_enabled set_amp_enabled; +// } board; + +// // ******************* Definitions ******************** +// // These should match the enums in cereal/log.capnp and __init__.py + +// extern struct board board_tres; +// extern struct board board_cuatro; +// extern struct board board_red; diff --git a/board/boards/boot_state.h b/board/boards/boot_state.h new file mode 100644 index 00000000000..2e2fc59ef39 --- /dev/null +++ b/board/boards/boot_state.h @@ -0,0 +1,7 @@ +#pragma once + +typedef enum { + BOOT_STANDBY, + BOOT_BOOTKICK, + BOOT_RESET, +} BootState; diff --git a/board/boards/cuatro.c b/board/boards/cuatro.c index 8ab79748bdf..e27eec8c28b 100644 --- a/board/boards/cuatro.c +++ b/board/boards/cuatro.c @@ -1,4 +1,10 @@ -#include "board_declarations.h" +// #include "board_declarations.h" +// #include "board/stm32h7/lladc.h" + +#include "board/config.h" +#include "board/board_struct.h" +#include "board/boards/unused_funcs.h" +#include "board/drivers/fake_siren.h" // ////////////////////////// // // Cuatro (STM32H7) + Harness // diff --git a/board/boards/red.c b/board/boards/red.c index c4b9a2cf0fd..f2719320c2b 100644 --- a/board/boards/red.c +++ b/board/boards/red.c @@ -2,6 +2,7 @@ #include "board/boards/board_declarations.h" #include "board/boards/unused_funcs.h" +#include "board/board_struct.h" // ///////////////////////////// // // Red Panda (STM32H7) + Harness // diff --git a/board/boards/unused_funcs.h b/board/boards/unused_funcs.h index 519f7906ed0..cbb30eb7e70 100644 --- a/board/boards/unused_funcs.h +++ b/board/boards/unused_funcs.h @@ -1,6 +1,7 @@ #pragma once #include "board_declarations.h" +#include "board/boards/boot_state.h" void unused_init_bootloader(void); void unused_set_ir_power(uint8_t percentage); diff --git a/board/body/boards/board_body.c b/board/body/boards/board_body.c index 204273d921a..0aa8e0a2c14 100644 --- a/board/body/boards/board_body.c +++ b/board/body/boards/board_body.c @@ -1,6 +1,7 @@ #include "board_body.h" #include "board/config.h" +#include "board/board_struct.h" void board_body_init(void) { motor_init(); diff --git a/board/body/boards/board_declarations.h b/board/body/boards/board_declarations.h index 09c43f86672..96cd604b526 100644 --- a/board/body/boards/board_declarations.h +++ b/board/body/boards/board_declarations.h @@ -1,21 +1,43 @@ #pragma once -#include -#include - -// ******************** Prototypes ******************** -typedef void (*board_init)(void); -typedef void (*board_init_bootloader)(void); -typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); - -struct board { - GPIO_TypeDef * const led_GPIO[3]; - const uint8_t led_pin[3]; - const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM - board_init init; - board_init_bootloader init_bootloader; - const bool has_spi; -}; - -// ******************* Definitions ******************** -#define HW_TYPE_BODY 0xB1U +#include "board/board_struct.h" + +// #pragma once + +// #include +// #include + +// #include "board/board_forward.h" + +// // ******************** Prototypes ******************** +// typedef void (*board_init)(void); +// typedef void (*board_init_bootloader)(void); +// typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); + +// typedef struct board { +// struct harness_configuration *harness_config; +// GPIO_TypeDef * const led_GPIO[3]; +// const uint8_t led_pin[3]; +// const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM +// const bool has_spi; +// const bool has_fan; +// const uint16_t avdd_mV; +// const uint8_t fan_enable_cooldown_time; +// board_init init; +// board_init_bootloader init_bootloader; +// board_enable_can_transceiver enable_can_transceiver; +// board_set_can_mode set_can_mode; +// board_read_voltage_mV read_voltage_mV; +// board_read_current_mA read_current_mA; +// board_set_ir_power set_ir_power; +// board_set_fan_enabled set_fan_enabled; +// board_set_siren set_siren; +// board_set_bootkick set_bootkick; +// board_read_som_gpio read_som_gpio; +// board_set_amp_enabled set_amp_enabled; +// } board; + + + +// // ******************* Definitions ******************** +// #define HW_TYPE_BODY 0xB1U diff --git a/board/body/can.h b/board/body/can.h index 94010ded051..8d76880b47c 100644 --- a/board/body/can.h +++ b/board/body/can.h @@ -6,7 +6,7 @@ #include "board/can.h" #include "board/health.h" #include "board/body/motor_control.h" -#include "board/drivers/can_common_declarations.h" +#include "board/drivers/can_common.h" #include "opendbc/safety/declarations.h" #define BODY_CAN_ADDR_MOTOR_SPEED 0x201U diff --git a/board/body/main.c b/board/body/main.c index 4a2d0b4c537..7141a731d15 100644 --- a/board/body/main.c +++ b/board/body/main.c @@ -14,10 +14,18 @@ #include "board/drivers/fdcan.h" #include "board/can_comms.h" +// TODO +#include "board/body/boards/board_body.h" + extern int _app_start[0xc000]; #include "board/body/main_comms.h" +// TODO +uint8_t hw_type; +board *current_board; +struct harness_t harness; + void debug_ring_callback(uart_ring *ring) { char rcv; while (get_char(ring, &rcv)) { diff --git a/board/body/main_comms.h b/board/body/main_comms.h index 95801332315..1eeaf024aaa 100644 --- a/board/body/main_comms.h +++ b/board/body/main_comms.h @@ -1,3 +1,7 @@ +#include "board/globals.h" + +// TODO ME! + void comms_endpoint2_write(const uint8_t *data, uint32_t len) { UNUSED(data); UNUSED(len); diff --git a/board/body/motor_common.h b/board/body/motor_common.h index c2fa0548938..3c8df5d39d0 100644 --- a/board/body/motor_common.h +++ b/board/body/motor_common.h @@ -10,6 +10,4 @@ typedef enum { BODY_MOTOR_RIGHT = 2U, } body_motor_id_e; -static inline bool body_motor_is_valid(uint8_t motor) { - return (motor > 0U) && (motor <= BODY_MOTOR_COUNT); -} +bool body_motor_is_valid(uint8_t motor); diff --git a/board/body/motor_control.c b/board/body/motor_control.c index 8be9078b7fc..6c18084bc18 100644 --- a/board/body/motor_control.c +++ b/board/body/motor_control.c @@ -1,11 +1,11 @@ -#pragma once - #include #include #include "board/body/motor_common.h" #include "board/body/motor_encoder.h" +#include "board/drivers/pwm.h" + // Motor pin map: // M1 drive: PB8 -> TIM16_CH1, PB9 -> TIM17_CH1, PE2/PE3 enables // M2 drive: PA2 -> TIM15_CH1, PA3 -> TIM15_CH2, PE8/PE9 enables @@ -36,7 +36,7 @@ typedef struct { uint8_t enable_pin[2]; } motor_pwm_config_t; -static const motor_pwm_config_t motor_pwm_config[BODY_MOTOR_COUNT] = { +const motor_pwm_config_t motor_pwm_config[BODY_MOTOR_COUNT] = { [BODY_MOTOR_LEFT - 1U] = { TIM16, 1U, TIM17, 1U, {GPIOB, GPIOB}, {8U, 9U}, {GPIO_AF1_TIM16, GPIO_AF1_TIM17}, @@ -49,20 +49,11 @@ static const motor_pwm_config_t motor_pwm_config[BODY_MOTOR_COUNT] = { }, }; -typedef struct { - bool active; - float target_rpm; - float integral; - float previous_error; - float last_output; - uint32_t last_update_us; -} motor_speed_state_t; - -static inline float motor_absf(float value) { +float motor_absf(float value) { return (value < 0.0f) ? -value : value; } -static inline float motor_clampf(float value, float min_value, float max_value) { +float motor_clampf(float value, float min_value, float max_value) { if (value < min_value) { return min_value; } @@ -72,9 +63,18 @@ static inline float motor_clampf(float value, float min_value, float max_value) return value; } -static motor_speed_state_t motor_speed_states[BODY_MOTOR_COUNT]; +typedef struct { + bool active; + float target_rpm; + float integral; + float previous_error; + float last_output; + uint32_t last_update_us; +} motor_speed_state_t; + +motor_speed_state_t motor_speed_states[BODY_MOTOR_COUNT]; -static inline void motor_pwm_write(TIM_TypeDef *timer, uint8_t channel, uint8_t percentage) { +void motor_pwm_write(TIM_TypeDef *timer, uint8_t channel, uint8_t percentage) { uint32_t period = (timer->ARR != 0U) ? timer->ARR : PWM_RELOAD_TICKS; uint16_t comp = (uint16_t)((period * (uint32_t)percentage) / 100U); if (channel == 1U) { @@ -84,11 +84,11 @@ static inline void motor_pwm_write(TIM_TypeDef *timer, uint8_t channel, uint8_t } } -static inline motor_speed_state_t *motor_speed_state_get(uint8_t motor) { +motor_speed_state_t *motor_speed_state_get(uint8_t motor) { return body_motor_is_valid(motor) ? &motor_speed_states[motor - 1U] : NULL; } -static inline void motor_speed_state_reset(motor_speed_state_t *state) { +void motor_speed_state_reset(motor_speed_state_t *state) { state->active = false; state->target_rpm = 0.0f; state->integral = 0.0f; @@ -97,13 +97,13 @@ static inline void motor_speed_state_reset(motor_speed_state_t *state) { state->last_update_us = 0U; } -static void motor_speed_controller_init(void) { +void motor_speed_controller_init(void) { for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) { motor_speed_state_reset(&motor_speed_states[i]); } } -static void motor_speed_controller_disable(uint8_t motor) { +void motor_speed_controller_disable(uint8_t motor) { motor_speed_state_t *state = motor_speed_state_get(motor); if (state == NULL) { return; @@ -111,14 +111,14 @@ static void motor_speed_controller_disable(uint8_t motor) { motor_speed_state_reset(state); } -static inline void motor_write_enable(uint8_t motor_index, bool enable) { +void motor_write_enable(uint8_t motor_index, bool enable) { const motor_pwm_config_t *cfg = &motor_pwm_config[motor_index]; uint8_t level = enable ? 1U : 0U; set_gpio_output(cfg->enable_port[0], cfg->enable_pin[0], level); set_gpio_output(cfg->enable_port[1], cfg->enable_pin[1], level); } -static void motor_init(void) { +void motor_init(void) { register_set_bits(&(RCC->AHB4ENR), RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN | RCC_AHB4ENR_GPIOEEN); register_set_bits(&(RCC->APB2ENR), RCC_APB2ENR_TIM16EN | RCC_APB2ENR_TIM17EN | RCC_APB2ENR_TIM15EN); @@ -147,7 +147,7 @@ static void motor_init(void) { } } -static void motor_apply_pwm(uint8_t motor, int32_t speed_command) { +void motor_apply_pwm(uint8_t motor, int32_t speed_command) { if (!body_motor_is_valid(motor)) { return; } @@ -170,12 +170,12 @@ static void motor_apply_pwm(uint8_t motor, int32_t speed_command) { } } -static inline void motor_set_speed(uint8_t motor, int8_t speed) { +void motor_set_speed(uint8_t motor, int8_t speed) { motor_speed_controller_disable(motor); motor_apply_pwm(motor, (int32_t)speed); } -static inline void motor_speed_controller_set_target_rpm(uint8_t motor, float target_rpm) { +void motor_speed_controller_set_target_rpm(uint8_t motor, float target_rpm) { motor_speed_state_t *state = motor_speed_state_get(motor); if (state == NULL) { return; @@ -196,7 +196,7 @@ static inline void motor_speed_controller_set_target_rpm(uint8_t motor, float ta state->last_update_us = 0U; } -static inline void motor_speed_controller_update(uint32_t now_us) { +void motor_speed_controller_update(uint32_t now_us) { for (uint8_t motor = BODY_MOTOR_LEFT; motor <= BODY_MOTOR_RIGHT; motor++) { motor_speed_state_t *state = motor_speed_state_get(motor); if (!state->active) { @@ -249,3 +249,8 @@ static inline void motor_speed_controller_update(uint32_t now_us) { state->last_update_us = now_us; } } + + +inline bool body_motor_is_valid(uint8_t motor) { + return (motor > 0U) && (motor <= BODY_MOTOR_COUNT); +} diff --git a/board/body/motor_control.h b/board/body/motor_control.h index f3b24420959..ac97443f267 100644 --- a/board/body/motor_control.h +++ b/board/body/motor_control.h @@ -99,155 +99,10 @@ static inline void motor_speed_state_reset(motor_speed_state_t *state) { state->last_update_us = 0U; } -static void motor_speed_controller_init(void) { - for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) { - motor_speed_state_reset(&motor_speed_states[i]); - } -} - -static void motor_speed_controller_disable(uint8_t motor) { - motor_speed_state_t *state = motor_speed_state_get(motor); - if (state == NULL) { - return; - } - motor_speed_state_reset(state); -} - -static inline void motor_write_enable(uint8_t motor_index, bool enable) { - const motor_pwm_config_t *cfg = &motor_pwm_config[motor_index]; - uint8_t level = enable ? 1U : 0U; - set_gpio_output(cfg->enable_port[0], cfg->enable_pin[0], level); - set_gpio_output(cfg->enable_port[1], cfg->enable_pin[1], level); -} - -static void motor_init(void) { - register_set_bits(&(RCC->AHB4ENR), RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN | RCC_AHB4ENR_GPIOEEN); - register_set_bits(&(RCC->APB2ENR), RCC_APB2ENR_TIM16EN | RCC_APB2ENR_TIM17EN | RCC_APB2ENR_TIM15EN); - - for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) { - const motor_pwm_config_t *cfg = &motor_pwm_config[i]; - motor_write_enable(i, false); - set_gpio_alternate(cfg->pwm_port[0], cfg->pwm_pin[0], cfg->pwm_af[0]); - set_gpio_alternate(cfg->pwm_port[1], cfg->pwm_pin[1], cfg->pwm_af[1]); - - pwm_init(cfg->forward_timer, cfg->forward_channel); - pwm_init(cfg->reverse_timer, cfg->reverse_channel); - - TIM_TypeDef *forward_timer = cfg->forward_timer; - register_set(&(forward_timer->PSC), 0U, 0xFFFFU); - register_set(&(forward_timer->ARR), PWM_RELOAD_TICKS, 0xFFFFU); - forward_timer->EGR |= TIM_EGR_UG; - register_set(&(forward_timer->BDTR), TIM_BDTR_MOE, 0xFFFFU); - - if (cfg->reverse_timer != cfg->forward_timer) { - TIM_TypeDef *reverse_timer = cfg->reverse_timer; - register_set(&(reverse_timer->PSC), 0U, 0xFFFFU); - register_set(&(reverse_timer->ARR), PWM_RELOAD_TICKS, 0xFFFFU); - reverse_timer->EGR |= TIM_EGR_UG; - register_set(&(reverse_timer->BDTR), TIM_BDTR_MOE, 0xFFFFU); - } - } -} - -static void motor_apply_pwm(uint8_t motor, int32_t speed_command) { - if (!body_motor_is_valid(motor)) { - return; - } - - int8_t speed = (int8_t)motor_clampf((float)speed_command, -(float)PWM_PERCENT_MAX, (float)PWM_PERCENT_MAX); - uint8_t pwm_value = (uint8_t)((speed < 0) ? -speed : speed); - uint8_t motor_index = motor - 1U; - motor_write_enable(motor_index, speed != 0); - const motor_pwm_config_t *cfg = &motor_pwm_config[motor_index]; - - if (speed > 0) { - motor_pwm_write(cfg->forward_timer, cfg->forward_channel, pwm_value); - motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, 0U); - } else if (speed < 0) { - motor_pwm_write(cfg->forward_timer, cfg->forward_channel, 0U); - motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, pwm_value); - } else { - motor_pwm_write(cfg->forward_timer, cfg->forward_channel, 0U); - motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, 0U); - } -} - -static inline void motor_set_speed(uint8_t motor, int8_t speed) { - motor_speed_controller_disable(motor); - motor_apply_pwm(motor, (int32_t)speed); -} - -static inline void motor_speed_controller_set_target_rpm(uint8_t motor, float target_rpm) { - motor_speed_state_t *state = motor_speed_state_get(motor); - if (state == NULL) { - return; - } - - target_rpm = motor_clampf(target_rpm, -MAX_RPM, MAX_RPM); - if (motor_absf(target_rpm) <= DEADBAND_RPM) { - motor_speed_controller_disable(motor); - motor_apply_pwm(motor, 0); - return; - } - - state->active = true; - state->target_rpm = target_rpm; - state->integral = 0.0f; - state->previous_error = target_rpm - motor_encoder_get_speed_rpm(motor); - state->last_output = 0.0f; - state->last_update_us = 0U; -} - -static inline void motor_speed_controller_update(uint32_t now_us) { - for (uint8_t motor = BODY_MOTOR_LEFT; motor <= BODY_MOTOR_RIGHT; motor++) { - motor_speed_state_t *state = motor_speed_state_get(motor); - if (!state->active) { - continue; - } - - bool first_update = (state->last_update_us == 0U); - uint32_t dt_us = first_update ? UPDATE_PERIOD_US : (now_us - state->last_update_us); - if (!first_update && (dt_us < UPDATE_PERIOD_US)) { - continue; - } - - float measured_rpm = motor_encoder_get_speed_rpm(motor); - float error = state->target_rpm - measured_rpm; - - if ((motor_absf(state->target_rpm) <= DEADBAND_RPM) && (motor_absf(error) <= DEADBAND_RPM) && (motor_absf(measured_rpm) <= DEADBAND_RPM)) { - motor_speed_controller_disable(motor); - motor_apply_pwm(motor, 0); - continue; - } - - float dt_s = (float)dt_us * 1.0e-6f; - float control = KFF * state->target_rpm; - - if (dt_s > 0.0f) { - state->integral += error * dt_s; - float derivative = first_update ? 0.0f : (error - state->previous_error) / dt_s; - - control += (KP * error) + (KI * state->integral) + (KD * derivative); - } else { - state->integral = 0.0f; - control += KP * error; - } - - if ((state->target_rpm > 0.0f) && (control < 0.0f)) { - control = 0.0f; - state->integral = 0.0f; - } else if ((state->target_rpm < 0.0f) && (control > 0.0f)) { - control = 0.0f; - state->integral = 0.0f; - } - - control = motor_clampf(control, -OUTPUT_MAX, OUTPUT_MAX); - - int32_t command = (control >= 0.0f) ? (int32_t)(control + 0.5f) : (int32_t)(control - 0.5f); - motor_apply_pwm(motor, command); - - state->previous_error = error; - state->last_output = control; - state->last_update_us = now_us; - } -} +void motor_speed_controller_init(void); +void motor_speed_controller_disable(uint8_t motor); +void motor_init(void); +void motor_apply_pwm(uint8_t motor, int32_t speed_command); +void motor_set_speed(uint8_t motor, int8_t speed); +void motor_speed_controller_set_target_rpm(uint8_t motor, float target_rpm); +void motor_speed_controller_update(uint32_t now_us); diff --git a/board/body/motor_encoder.c b/board/body/motor_encoder.c new file mode 100644 index 00000000000..e3f6b712d70 --- /dev/null +++ b/board/body/motor_encoder.c @@ -0,0 +1,138 @@ +#include "motor_encoder.h" + +motor_encoder_config_t motor_encoder_config[BODY_MOTOR_COUNT] = { + [BODY_MOTOR_LEFT - 1U] = { + .timer = TIM4, + .pin_a_port = GPIOB, .pin_a = 6U, .pin_a_af = GPIO_AF2_TIM4, + .pin_b_port = GPIOB, .pin_b = 7U, .pin_b_af = GPIO_AF2_TIM4, + .direction = -1, + .counts_per_output_rev = 44U * 90U, + .min_dt_us = 250U, + .speed_alpha = 0.2f, + .filter = 3U, + }, + [BODY_MOTOR_RIGHT - 1U] = { + .timer = TIM3, + .pin_a_port = GPIOA, .pin_a = 6U, .pin_a_af = GPIO_AF2_TIM3, + .pin_b_port = GPIOA, .pin_b = 7U, .pin_b_af = GPIO_AF2_TIM3, + .direction = 1, + .counts_per_output_rev = 44U * 90U, + .min_dt_us = 250U, + .speed_alpha = 0.2f, + .filter = 3U, + }, +}; + +motor_encoder_state_t motor_encoders[BODY_MOTOR_COUNT] = { + { .config = &motor_encoder_config[0] }, + { .config = &motor_encoder_config[1] }, +}; + +void motor_encoder_configure_gpio(const motor_encoder_config_t *cfg) { + set_gpio_pullup(cfg->pin_a_port, cfg->pin_a, PULL_UP); + set_gpio_output_type(cfg->pin_a_port, cfg->pin_a, OUTPUT_TYPE_PUSH_PULL); + set_gpio_alternate(cfg->pin_a_port, cfg->pin_a, cfg->pin_a_af); + + set_gpio_pullup(cfg->pin_b_port, cfg->pin_b, PULL_UP); + set_gpio_output_type(cfg->pin_b_port, cfg->pin_b, OUTPUT_TYPE_PUSH_PULL); + set_gpio_alternate(cfg->pin_b_port, cfg->pin_b, cfg->pin_b_af); +} + +void motor_encoder_configure_timer(motor_encoder_state_t *state) { + const motor_encoder_config_t *cfg = state->config; + TIM_TypeDef *timer = cfg->timer; + timer->CR1 = 0U; + timer->CR2 = 0U; + timer->SMCR = 0U; + timer->DIER = 0U; + timer->SR = 0U; + timer->CCMR1 = (TIM_CCMR1_CC1S_0) | (TIM_CCMR1_CC2S_0) | (cfg->filter << TIM_CCMR1_IC1F_Pos) | (cfg->filter << TIM_CCMR1_IC2F_Pos); + timer->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E; + timer->PSC = 0U; + timer->ARR = 0xFFFFU; + timer->CNT = 0U; + state->last_timer_count = 0U; + state->accumulated_count = 0; + state->last_speed_count = 0; + state->cached_speed_rps = 0.0f; + timer->SMCR = (TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1); + timer->CR1 = TIM_CR1_CEN; +} + +void motor_encoder_init(void) { + register_set_bits(&(RCC->APB1LENR), RCC_APB1LENR_TIM4EN | RCC_APB1LENR_TIM3EN); + register_set_bits(&(RCC->APB1LRSTR), RCC_APB1LRSTR_TIM4RST | RCC_APB1LRSTR_TIM3RST); + register_clear_bits(&(RCC->APB1LRSTR), RCC_APB1LRSTR_TIM4RST | RCC_APB1LRSTR_TIM3RST); + + for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) { + motor_encoder_state_t *state = &motor_encoders[i]; + const motor_encoder_config_t *cfg = state->config; + motor_encoder_configure_gpio(cfg); + motor_encoder_configure_timer(state); + state->last_speed_timestamp_us = 0U; + } +} + +int32_t motor_encoder_refresh(motor_encoder_state_t *state) { + const motor_encoder_config_t *cfg = state->config; + TIM_TypeDef *timer = cfg->timer; + uint16_t raw = (uint16_t)timer->CNT; + int32_t delta = (int16_t)(raw - state->last_timer_count); + delta *= cfg->direction; + state->last_timer_count = raw; + state->accumulated_count += delta; + return state->accumulated_count; +} + +int32_t motor_encoder_get_position(uint8_t motor) { + if (!body_motor_is_valid(motor)) { + return 0; + } + motor_encoder_state_t *state = &motor_encoders[motor - 1U]; + return motor_encoder_refresh(state); +} + +void motor_encoder_reset(uint8_t motor) { + if (!body_motor_is_valid(motor)) { + return; + } + motor_encoder_state_t *state = &motor_encoders[motor - 1U]; + state->config->timer->CNT = 0U; + state->last_timer_count = 0U; + state->accumulated_count = 0; + state->last_speed_count = 0; + state->last_speed_timestamp_us = 0U; + state->cached_speed_rps = 0.0f; +} + +float motor_encoder_get_speed_rpm(uint8_t motor) { + if (!body_motor_is_valid(motor)) { + return 0.0f; + } + motor_encoder_state_t *state = &motor_encoders[motor - 1U]; + + const motor_encoder_config_t *cfg = state->config; + motor_encoder_refresh(state); + + uint32_t now = microsecond_timer_get(); + if (state->last_speed_timestamp_us == 0U) { + state->last_speed_timestamp_us = now; + state->last_speed_count = state->accumulated_count; + state->cached_speed_rps = 0.0f; + return 0.0f; + } + + uint32_t dt = now - state->last_speed_timestamp_us; + int32_t delta = state->accumulated_count - state->last_speed_count; + if ((dt < cfg->min_dt_us) || (delta == 0)) { + return state->cached_speed_rps * 60.0f; + } + + state->last_speed_count = state->accumulated_count; + state->last_speed_timestamp_us = now; + + float counts_per_second = ((float)delta * 1000000.0f) / (float)dt; + float new_speed_rps = (cfg->counts_per_output_rev != 0U) ? (counts_per_second / (float)cfg->counts_per_output_rev) : 0.0f; + state->cached_speed_rps += cfg->speed_alpha * (new_speed_rps - state->cached_speed_rps); + return state->cached_speed_rps * 60.0f; +} diff --git a/board/body/motor_encoder.h b/board/body/motor_encoder.h index dc5a896e9cb..686a4767baa 100644 --- a/board/body/motor_encoder.h +++ b/board/body/motor_encoder.h @@ -3,6 +3,7 @@ #include #include "board/body/motor_common.h" +#include "board/config.h" // Encoder pin map: // Left motor: PB6 -> TIM4_CH1, PB7 -> TIM4_CH2 @@ -32,5 +33,10 @@ typedef struct { float cached_speed_rps; } motor_encoder_state_t; +extern motor_encoder_state_t motor_encoders[BODY_MOTOR_COUNT]; +void motor_encoder_configure_gpio(const motor_encoder_config_t *cfg); +void motor_encoder_init(void); +int32_t motor_encoder_get_position(uint8_t motor); +void motor_encoder_reset(uint8_t motor); float motor_encoder_get_speed_rpm(uint8_t motor); \ No newline at end of file diff --git a/board/body/stm32h7/board.c b/board/body/stm32h7/board.c new file mode 100644 index 00000000000..c48a58dbade --- /dev/null +++ b/board/body/stm32h7/board.c @@ -0,0 +1,9 @@ +#include "board.h" + +#ifndef PANDA_BODY +#error This should only be used on Panda Body! +#endif + +void detect_board_type(void) { + // Board type set explicitly in main() +} \ No newline at end of file diff --git a/board/body/stm32h7/board.h b/board/body/stm32h7/board.h index 1bcd0ffaa10..9d07845b68c 100644 --- a/board/body/stm32h7/board.h +++ b/board/body/stm32h7/board.h @@ -1,11 +1,10 @@ #pragma once -#include "board/body/boards/board_declarations.h" -#include "board/body/boards/board_body.h" +// #include "board/body/boards/board_declarations.h" +// #include "board/body/boards/board_body.h" -extern board *current_board; -extern uint8_t hw_type; +// extern board *current_board; +// extern uint8_t hw_type; -void detect_board_type(void) { - // Board type set explicitly in main() -} +// ******************* Definitions ******************** +#define HW_TYPE_BODY 0xB1U diff --git a/board/bootstub_declarations.h b/board/bootstub_declarations.h index 94e7dd7ca92..fef155970ce 100644 --- a/board/bootstub_declarations.h +++ b/board/bootstub_declarations.h @@ -3,7 +3,8 @@ #include #include "utils.h" -#include "stm32h7/inc/stm32h7xx.h" +// #include "stm32h7/inc/stm32h7xx.h" +#include "config.h" // ******************** Prototypes ******************** diff --git a/board/config.h b/board/config.h index aed07207675..ab8d922c5ad 100644 --- a/board/config.h +++ b/board/config.h @@ -32,6 +32,9 @@ #endif #endif +// TODO, better spot +void detect_board_type(void); + // platform includes #ifdef STM32H7 #include "board/stm32h7/stm32h7_config.h" diff --git a/board/critical.h b/board/critical.h index 4b428cc9de8..9f6ee59c193 100644 --- a/board/critical.h +++ b/board/critical.h @@ -8,15 +8,19 @@ // ********************* Critical section helpers ********************* extern uint8_t global_critical_depth; +#ifndef ENTER_CRITICAL #define ENTER_CRITICAL() \ __disable_irq(); \ global_critical_depth += 1U; +#endif +#ifndef EXIT_CRITICAL #define EXIT_CRITICAL() \ global_critical_depth -= 1U; \ if ((global_critical_depth == 0U) && interrupts_enabled) { \ __enable_irq(); \ } +#endif extern uint8_t global_critical_depth; static volatile bool interrupts_enabled; diff --git a/board/drivers/can_common.h b/board/drivers/can_common.h index ac2d228f3c5..3dbd85d7328 100644 --- a/board/drivers/can_common.h +++ b/board/drivers/can_common.h @@ -6,6 +6,13 @@ #include "board/can.h" #include "board/health.h" +#include "board/can_comms.h" +#include "timers.h" +#include "board/utils.h" +#include "opendbc/safety/declarations.h" + +#include "board/critical.h" + typedef struct { volatile uint32_t w_ptr; volatile uint32_t r_ptr; @@ -54,6 +61,7 @@ extern can_ring *can_queues[PANDA_CAN_CNT]; bool can_pop(can_ring *q, CANPacket_t *elem); bool can_push(can_ring *q, const CANPacket_t *elem); uint32_t can_slots_empty(const can_ring *q); +void can_clear(can_ring *q); extern bus_config_t bus_config[PANDA_CAN_CNT]; #define CANIF_FROM_CAN_NUM(num) (cans[num]) @@ -73,12 +81,6 @@ bool can_check_checksum(CANPacket_t *packet); void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook); bool is_speed_valid(uint32_t speed, const uint32_t *all_speeds, uint8_t len); -#include "board/can_comms.h" -#include "timers.h" -#include "board/utils.h" -#include "opendbc/safety/declarations.h" - -#include "board/critical.h" extern uint32_t safety_tx_blocked; extern uint32_t safety_rx_invalid; @@ -118,26 +120,3 @@ extern can_ring can_tx3_q; // FIXME: // cppcheck-suppress misra-c2012-9.3 // can_ring *can_queues[PANDA_CAN_CNT] = {&can_tx1_q, &can_tx2_q, &can_tx3_q}; - -// ********************* interrupt safe queue ********************* -bool can_pop(can_ring *q, CANPacket_t *elem); - -bool can_push(can_ring *q, const CANPacket_t *elem); -uint32_t can_slots_empty(const can_ring *q); -void can_clear(can_ring *q); - -extern bus_config_t bus_config[PANDA_CAN_CNT]; -void can_init_all(void); -void can_set_orientation(bool flipped); - -#ifdef PANDA_JUNGLE -void can_set_forwarding(uint8_t from, uint8_t to); -#endif - -void ignition_can_hook(CANPacket_t *msg); -bool can_tx_check_min_slots_free(uint32_t min); -uint8_t calculate_checksum(const uint8_t *dat, uint32_t len); -void can_set_checksum(CANPacket_t *packet); -bool can_check_checksum(CANPacket_t *packet); -void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook); -bool is_speed_valid(uint32_t speed, const uint32_t *all_speeds, uint8_t len); \ No newline at end of file diff --git a/board/drivers/harness.h b/board/drivers/harness.h index bb06765cef1..1058b345e69 100644 --- a/board/drivers/harness.h +++ b/board/drivers/harness.h @@ -4,6 +4,8 @@ #include #include "board/config.h" +// TODO +#include "board/stm32h7/lladc.h" #define HARNESS_STATUS_NC 0U #define HARNESS_STATUS_NORMAL 1U diff --git a/board/fake_stm.h b/board/fake_stm.h index 9d4d9b0767e..cda8582661c 100644 --- a/board/fake_stm.h +++ b/board/fake_stm.h @@ -1,3 +1,4 @@ +#pragma once // minimal code to fake a panda for tests #include #include @@ -7,8 +8,12 @@ #define ALLOW_DEBUG +#ifndef ENTER_CRITICAL #define ENTER_CRITICAL() 0 +#endif +#ifndef EXIT_CRITICAL #define EXIT_CRITICAL() 0 +#endif void print(const char *a); void puth(unsigned int i); diff --git a/board/jungle/boards/board_declarations.h b/board/jungle/boards/board_declarations.h index 8793f606b34..329608090df 100644 --- a/board/jungle/boards/board_declarations.h +++ b/board/jungle/boards/board_declarations.h @@ -3,62 +3,35 @@ #include #include "board/config.h" - -// ******************** Prototypes ******************** -typedef void (*board_init)(void); -typedef void (*board_board_tick)(void); -typedef bool (*board_get_button)(void); -typedef void (*board_init_bootloader)(void); -typedef void (*board_set_panda_power)(bool enabled); -typedef void (*board_set_panda_individual_power)(uint8_t port_num, bool enabled); -typedef void (*board_set_ignition)(bool enabled); -typedef void (*board_set_individual_ignition)(uint8_t bitmask); -typedef void (*board_set_harness_orientation)(uint8_t orientation); -typedef void (*board_set_can_mode)(uint8_t mode); -typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); -typedef void (*board_enable_header_pin)(uint8_t pin_num, bool enabled); -typedef float (*board_get_channel_power)(uint8_t channel); -typedef uint16_t (*board_get_sbu_mV)(uint8_t channel, uint8_t sbu); - -struct board { - GPIO_TypeDef * const led_GPIO[3]; - const uint8_t led_pin[3]; - const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM - const uint16_t avdd_mV; - board_init init; - board_board_tick board_tick; - board_get_button get_button; - board_init_bootloader init_bootloader; - board_set_panda_power set_panda_power; - board_set_panda_individual_power set_panda_individual_power; - board_set_ignition set_ignition; - board_set_individual_ignition set_individual_ignition; - board_set_harness_orientation set_harness_orientation; - board_set_can_mode set_can_mode; - board_enable_can_transceiver enable_can_transceiver; - board_enable_header_pin enable_header_pin; - board_get_channel_power get_channel_power; - board_get_sbu_mV get_sbu_mV; - - // TODO: shouldn't need these - bool has_spi; -}; +#include "board/board_struct.h" + +// #include "board_forward.h" + +// struct board { +// GPIO_TypeDef * const led_GPIO[3]; +// const uint8_t led_pin[3]; +// const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM +// const uint16_t avdd_mV; +// board_init init; +// board_board_tick board_tick; +// board_get_button get_button; +// board_init_bootloader init_bootloader; +// board_set_panda_power set_panda_power; +// board_set_panda_individual_power set_panda_individual_power; +// board_set_ignition set_ignition; +// board_set_individual_ignition set_individual_ignition; +// board_set_harness_orientation set_harness_orientation; +// board_set_can_mode set_can_mode; +// board_enable_can_transceiver enable_can_transceiver; +// board_enable_header_pin enable_header_pin; +// board_get_channel_power get_channel_power; +// board_get_sbu_mV get_sbu_mV; + +// // TODO: shouldn't need these +// bool has_spi; +// }; // ******************* Definitions ******************** -#define HW_TYPE_UNKNOWN 0U -#define HW_TYPE_V2 2U - -// CAN modes -#define CAN_MODE_NORMAL 0U -#define CAN_MODE_OBD_CAN2 3U - -// Harness states -#define HARNESS_ORIENTATION_NONE 0U -#define HARNESS_ORIENTATION_1 1U -#define HARNESS_ORIENTATION_2 2U - -#define SBU1 0U -#define SBU2 1U // ********************* Globals ********************** extern uint8_t harness_orientation; diff --git a/board/jungle/stm32h7/board.h b/board/jungle/stm32h7/board.h index 760a70416ac..c5be1dd43c0 100644 --- a/board/jungle/stm32h7/board.h +++ b/board/jungle/stm32h7/board.h @@ -1,3 +1,5 @@ +#include "board/config.h" + #include "board/jungle/boards/board_declarations.h" #include "board/stm32h7/lladc.h" diff --git a/board/main.c b/board/main.c index 9f183e56bb2..5aff52dcc21 100644 --- a/board/main.c +++ b/board/main.c @@ -49,6 +49,354 @@ void debug_ring_callback(uart_ring *ring) { // ****************************** safety mode ****************************** +#include +#include + +#include "board/utils.h" +#include "board/config.h" +#include "board/health.h" +#include "board/main_declarations.h" +#include "board/drivers/can_common.h" +#include "board/power_saving.h" +#include "board/drivers/spi.h" +#include "board/drivers/bootkick.h" +#include "board/drivers/fdcan.h" +#include "board/provision.h" +#include "board/early_init.h" +#include "board/drivers/fan.h" +#include "board/drivers/clock_source.h" + +extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used + +// Prototypes +void set_safety_mode(uint16_t mode, uint16_t param); +bool is_car_safety_mode(uint16_t mode); + +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; + + health->uptime_pkt = uptime_cnt; + health->voltage_pkt = current_board->read_voltage_mV(); + health->current_pkt = current_board->read_current_mA(); + + health->ignition_line_pkt = (uint8_t)(harness_check_ignition()); + health->ignition_can_pkt = ignition_can; + + health->controls_allowed_pkt = controls_allowed; + health->safety_tx_blocked_pkt = safety_tx_blocked; + health->safety_rx_invalid_pkt = safety_rx_invalid; + health->tx_buffer_overflow_pkt = tx_buffer_overflow; + health->rx_buffer_overflow_pkt = rx_buffer_overflow; + health->car_harness_status_pkt = harness.status; + health->safety_mode_pkt = (uint8_t)(current_safety_mode); + health->safety_param_pkt = current_safety_param; + health->alternative_experience_pkt = alternative_experience; + health->power_save_enabled_pkt = power_save_status == POWER_SAVE_STATUS_ENABLED; + health->heartbeat_lost_pkt = heartbeat_lost; + health->safety_rx_checks_invalid_pkt = safety_rx_checks_invalid; + + health->spi_error_count_pkt = spi_error_count; + + health->fault_status_pkt = fault_status; + health->faults_pkt = faults; + + health->interrupt_load_pkt = interrupt_load; + + health->fan_power = fan_state.power; + + health->sbu1_voltage_mV = harness.sbu1_voltage_mV; + health->sbu2_voltage_mV = harness.sbu2_voltage_mV; + + health->som_reset_triggered = bootkick_reset_triggered; + + return sizeof(*health); +} + +// send on serial, first byte to select the ring +void comms_endpoint2_write(const uint8_t *data, uint32_t len) { + uart_ring *ur = get_ring_by_number(data[0]); + if ((len != 0U) && (ur != NULL)) { + if ((data[0] < 2U) || (data[0] >= 4U)) { + for (uint32_t i = 1; i < len; i++) { + while (!put_char(ur, data[i])) { + // wait + } + } + } + } +} + +int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { + unsigned int resp_len = 0; + uart_ring *ur = NULL; + uint32_t time; + +#ifdef DEBUG_COMMS + print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n"); + print("- request "); puth(req->request); print("\n"); + print("- param1 "); puth(req->param1); print("\n"); + print("- param2 "); puth(req->param2); print("\n"); +#endif + + switch (req->request) { + // **** 0xa8: get microsecond timer + case 0xa8: + time = microsecond_timer_get(); + resp[0] = (time & 0x000000FFU); + resp[1] = ((time & 0x0000FF00U) >> 8U); + resp[2] = ((time & 0x00FF0000U) >> 16U); + resp[3] = ((time & 0xFF000000U) >> 24U); + resp_len = 4U; + break; + // **** 0xb0: set IR power + case 0xb0: + current_board->set_ir_power(req->param1); + break; + // **** 0xb1: set fan power + case 0xb1: + fan_set_power(req->param1); + break; + // **** 0xb2: get fan rpm + case 0xb2: + resp[0] = (fan_state.rpm & 0x00FFU); + resp[1] = ((fan_state.rpm & 0xFF00U) >> 8U); + resp_len = 2; + break; + // **** 0xc0: reset communications state + case 0xc0: + comms_can_reset(); + break; + // **** 0xc1: get hardware type + case 0xc1: + resp[0] = hw_type; + resp_len = 1; + break; + // **** 0xc2: CAN health stats + case 0xc2: + COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); + if (req->param1 < 3U) { + update_can_health_pkt(req->param1, 0U); + can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); + can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); + can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; + can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; + can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; + resp_len = sizeof(can_health[req->param1]); + (void)memcpy(resp, (uint8_t*)(&can_health[req->param1]), resp_len); + } + break; + // **** 0xc3: fetch MCU UID + case 0xc3: + (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); + resp_len = 12; + break; + // **** 0xc4: get interrupt call rate + case 0xc4: + if (req->param1 < NUM_INTERRUPTS) { + uint32_t load = interrupts[req->param1].call_rate; + resp[0] = (load & 0x000000FFU); + resp[1] = ((load & 0x0000FF00U) >> 8U); + resp[2] = ((load & 0x00FF0000U) >> 16U); + resp[3] = ((load & 0xFF000000U) >> 24U); + resp_len = 4U; + } + break; + // **** 0xc5: DEBUG: drive relay + case 0xc5: + set_intercept_relay((req->param1 & 0x1U), (req->param1 & 0x2U)); + break; + // **** 0xc6: DEBUG: read SOM GPIO + case 0xc6: + resp[0] = current_board->read_som_gpio(); + resp_len = 1; + break; + // **** 0xd0: fetch serial (aka the provisioned dongle ID) + case 0xd0: + // addresses are OTP + if (req->param1 == 1U) { + (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); + resp_len = 0x10; + } else { + get_provision_chunk(resp); + resp_len = PROVISION_CHUNK_LEN; + } + break; + // **** 0xd1: enter bootloader mode + case 0xd1: + // this allows reflashing of the bootstub + switch (req->param1) { + case 0: + // only allow bootloader entry on debug builds + #ifdef ALLOW_DEBUG + print("-> entering bootloader\n"); + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + NVIC_SystemReset(); + #endif + break; + case 1: + print("-> entering softloader\n"); + enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; + NVIC_SystemReset(); + break; + default: + print("Bootloader mode invalid\n"); + break; + } + break; + // **** 0xd2: get health packet + case 0xd2: + resp_len = get_health_pkt(resp); + break; + // **** 0xd3: get first 64 bytes of signature + case 0xd3: + { + resp_len = 64; + char * code = (char*)_app_start; + int code_len = _app_start[0]; + (void)memcpy(resp, &code[code_len], resp_len); + } + break; + // **** 0xd4: get second 64 bytes of signature + case 0xd4: + { + resp_len = 64; + char * code = (char*)_app_start; + int code_len = _app_start[0]; + (void)memcpy(resp, &code[code_len + 64], resp_len); + } + break; + // **** 0xd6: get version + case 0xd6: + COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); + (void)memcpy(resp, gitversion, sizeof(gitversion)); + resp_len = sizeof(gitversion) - 1U; + break; + // **** 0xd8: reset ST + case 0xd8: + NVIC_SystemReset(); + break; + // **** 0xdb: set OBD CAN multiplexing mode + case 0xdb: + current_board->set_can_mode((req->param1 == 1U) ? CAN_MODE_OBD_CAN2 : CAN_MODE_NORMAL); + break; + // **** 0xdc: set safety mode + case 0xdc: + set_safety_mode(req->param1, (uint16_t)req->param2); + break; + // **** 0xdd: get healthpacket and CANPacket versions + case 0xdd: + resp[0] = HEALTH_PACKET_VERSION; + resp[1] = CAN_PACKET_VERSION; + resp[2] = CAN_HEALTH_PACKET_VERSION; + resp_len = 3; + break; + // **** 0xde: set can bitrate + case 0xde: + if ((req->param1 < PANDA_CAN_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { + bus_config[req->param1].can_speed = req->param2; + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + // **** 0xdf: set alternative experience + case 0xdf: + // you can only set this if you are in a non car safety mode + if (!is_car_safety_mode(current_safety_mode)) { + alternative_experience = req->param1; + } + break; + // **** 0xe0: uart read + case 0xe0: + ur = get_ring_by_number(req->param1); + if (!ur) { + break; + } + + // read + uint16_t req_length = MIN(req->length, USBPACKET_MAX_SIZE); + while ((resp_len < req_length) && + get_char(ur, (char*)&resp[resp_len])) { + ++resp_len; + } + break; + // **** 0xe5: set CAN loopback (for testing) + case 0xe5: + can_loopback = req->param1 > 0U; + can_init_all(); + break; + // **** 0xe6: set custom clock source period and pulse length + case 0xe6: + clock_source_set_timer_params(req->param1, req->param2); + break; + // **** 0xe7: set power save state + case 0xe7: + set_power_save_state(req->param1); + break; + // **** 0xe8: set can-fd auto swithing mode + case 0xe8: + bus_config[req->param1].canfd_auto = req->param2 > 0U; + break; + // **** 0xf1: Clear CAN ring buffer. + case 0xf1: + if (req->param1 == 0xFFFFU) { + print("Clearing CAN Rx queue\n"); + can_clear(&can_rx_q); + } else if (req->param1 < PANDA_CAN_CNT) { + print("Clearing CAN Tx queue\n"); + can_clear(can_queues[req->param1]); + } else { + print("Clearing CAN CAN ring buffer failed: wrong bus number\n"); + } + break; + // **** 0xf3: Heartbeat. Resets heartbeat counter. + case 0xf3: + { + heartbeat_counter = 0U; + heartbeat_lost = false; + heartbeat_disabled = false; + heartbeat_engaged = (req->param1 == 1U); + break; + } + // **** 0xf6: set siren enabled + case 0xf6: + siren_enabled = (req->param1 != 0U); + break; + // **** 0xf8: disable heartbeat checks + case 0xf8: + if (!is_car_safety_mode(current_safety_mode)) { + heartbeat_disabled = true; + } + break; + // **** 0xf9: set CAN FD data bitrate + case 0xf9: + if ((req->param1 < PANDA_CAN_CNT) && + is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { + bus_config[req->param1].can_data_speed = req->param2; + bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); + bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + // **** 0xfc: set CAN FD non-ISO mode + case 0xfc: + if (req->param1 < PANDA_CAN_CNT) { + bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + default: + print("NO HANDLER "); + puth(req->request); + print("\n"); + break; + } + return resp_len; +} + + // this is the only way to leave silent mode void set_safety_mode(uint16_t mode, uint16_t param) { uint16_t mode_copy = mode; diff --git a/board/main_comms.c b/board/main_comms.c index 4646608f7ca..42ffe47bc75 100644 --- a/board/main_comms.c +++ b/board/main_comms.c @@ -1,345 +1,347 @@ -#include -#include +// #include +// #include -#include "board/utils.h" -#include "board/config.h" -#include "board/health.h" -#include "board/main_declarations.h" -#include "board/drivers/can_common.h" -#include "board/power_saving.h" -#include "board/drivers/spi.h" -#include "board/drivers/bootkick.h" -#include "board/drivers/fdcan.h" -#include "board/provision.h" -#include "board/early_init.h" -#include "board/obj/gitversion.h" +// #include "board/utils.h" +// #include "board/config.h" +// #include "board/health.h" +// #include "board/main_declarations.h" +// #include "board/drivers/can_common.h" +// #include "board/power_saving.h" +// #include "board/drivers/spi.h" +// #include "board/drivers/bootkick.h" +// #include "board/drivers/fdcan.h" +// #include "board/provision.h" +// #include "board/early_init.h" +// #include "board/obj/gitversion.h" +// #include "board/drivers/fan.h" +// #include "board/drivers/clock_source.h" -extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used +// extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used -// Prototypes -void set_safety_mode(uint16_t mode, uint16_t param); -bool is_car_safety_mode(uint16_t mode); +// // Prototypes +// void set_safety_mode(uint16_t mode, uint16_t param); +// bool is_car_safety_mode(uint16_t mode); -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; +// 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; - health->uptime_pkt = uptime_cnt; - health->voltage_pkt = current_board->read_voltage_mV(); - health->current_pkt = current_board->read_current_mA(); +// health->uptime_pkt = uptime_cnt; +// health->voltage_pkt = current_board->read_voltage_mV(); +// health->current_pkt = current_board->read_current_mA(); - health->ignition_line_pkt = (uint8_t)(harness_check_ignition()); - health->ignition_can_pkt = ignition_can; +// health->ignition_line_pkt = (uint8_t)(harness_check_ignition()); +// health->ignition_can_pkt = ignition_can; - health->controls_allowed_pkt = controls_allowed; - health->safety_tx_blocked_pkt = safety_tx_blocked; - health->safety_rx_invalid_pkt = safety_rx_invalid; - health->tx_buffer_overflow_pkt = tx_buffer_overflow; - health->rx_buffer_overflow_pkt = rx_buffer_overflow; - health->car_harness_status_pkt = harness.status; - health->safety_mode_pkt = (uint8_t)(current_safety_mode); - health->safety_param_pkt = current_safety_param; - health->alternative_experience_pkt = alternative_experience; - health->power_save_enabled_pkt = power_save_status == POWER_SAVE_STATUS_ENABLED; - health->heartbeat_lost_pkt = heartbeat_lost; - health->safety_rx_checks_invalid_pkt = safety_rx_checks_invalid; +// health->controls_allowed_pkt = controls_allowed; +// health->safety_tx_blocked_pkt = safety_tx_blocked; +// health->safety_rx_invalid_pkt = safety_rx_invalid; +// health->tx_buffer_overflow_pkt = tx_buffer_overflow; +// health->rx_buffer_overflow_pkt = rx_buffer_overflow; +// health->car_harness_status_pkt = harness.status; +// health->safety_mode_pkt = (uint8_t)(current_safety_mode); +// health->safety_param_pkt = current_safety_param; +// health->alternative_experience_pkt = alternative_experience; +// health->power_save_enabled_pkt = power_save_status == POWER_SAVE_STATUS_ENABLED; +// health->heartbeat_lost_pkt = heartbeat_lost; +// health->safety_rx_checks_invalid_pkt = safety_rx_checks_invalid; - health->spi_error_count_pkt = spi_error_count; +// health->spi_error_count_pkt = spi_error_count; - health->fault_status_pkt = fault_status; - health->faults_pkt = faults; +// health->fault_status_pkt = fault_status; +// health->faults_pkt = faults; - health->interrupt_load_pkt = interrupt_load; +// health->interrupt_load_pkt = interrupt_load; - health->fan_power = fan_state.power; +// health->fan_power = fan_state.power; - health->sbu1_voltage_mV = harness.sbu1_voltage_mV; - health->sbu2_voltage_mV = harness.sbu2_voltage_mV; +// health->sbu1_voltage_mV = harness.sbu1_voltage_mV; +// health->sbu2_voltage_mV = harness.sbu2_voltage_mV; - health->som_reset_triggered = bootkick_reset_triggered; +// health->som_reset_triggered = bootkick_reset_triggered; - return sizeof(*health); -} +// return sizeof(*health); +// } -// send on serial, first byte to select the ring -void comms_endpoint2_write(const uint8_t *data, uint32_t len) { - uart_ring *ur = get_ring_by_number(data[0]); - if ((len != 0U) && (ur != NULL)) { - if ((data[0] < 2U) || (data[0] >= 4U)) { - for (uint32_t i = 1; i < len; i++) { - while (!put_char(ur, data[i])) { - // wait - } - } - } - } -} +// // send on serial, first byte to select the ring +// void comms_endpoint2_write(const uint8_t *data, uint32_t len) { +// uart_ring *ur = get_ring_by_number(data[0]); +// if ((len != 0U) && (ur != NULL)) { +// if ((data[0] < 2U) || (data[0] >= 4U)) { +// for (uint32_t i = 1; i < len; i++) { +// while (!put_char(ur, data[i])) { +// // wait +// } +// } +// } +// } +// } -int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { - unsigned int resp_len = 0; - uart_ring *ur = NULL; - uint32_t time; +// int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { +// unsigned int resp_len = 0; +// uart_ring *ur = NULL; +// uint32_t time; -#ifdef DEBUG_COMMS - print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n"); - print("- request "); puth(req->request); print("\n"); - print("- param1 "); puth(req->param1); print("\n"); - print("- param2 "); puth(req->param2); print("\n"); -#endif +// #ifdef DEBUG_COMMS +// print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n"); +// print("- request "); puth(req->request); print("\n"); +// print("- param1 "); puth(req->param1); print("\n"); +// print("- param2 "); puth(req->param2); print("\n"); +// #endif - switch (req->request) { - // **** 0xa8: get microsecond timer - case 0xa8: - time = microsecond_timer_get(); - resp[0] = (time & 0x000000FFU); - resp[1] = ((time & 0x0000FF00U) >> 8U); - resp[2] = ((time & 0x00FF0000U) >> 16U); - resp[3] = ((time & 0xFF000000U) >> 24U); - resp_len = 4U; - break; - // **** 0xb0: set IR power - case 0xb0: - current_board->set_ir_power(req->param1); - break; - // **** 0xb1: set fan power - case 0xb1: - fan_set_power(req->param1); - break; - // **** 0xb2: get fan rpm - case 0xb2: - resp[0] = (fan_state.rpm & 0x00FFU); - resp[1] = ((fan_state.rpm & 0xFF00U) >> 8U); - resp_len = 2; - break; - // **** 0xc0: reset communications state - case 0xc0: - comms_can_reset(); - break; - // **** 0xc1: get hardware type - case 0xc1: - resp[0] = hw_type; - resp_len = 1; - break; - // **** 0xc2: CAN health stats - case 0xc2: - COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); - if (req->param1 < 3U) { - update_can_health_pkt(req->param1, 0U); - can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); - can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); - can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; - can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; - can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; - resp_len = sizeof(can_health[req->param1]); - (void)memcpy(resp, (uint8_t*)(&can_health[req->param1]), resp_len); - } - break; - // **** 0xc3: fetch MCU UID - case 0xc3: - (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); - resp_len = 12; - break; - // **** 0xc4: get interrupt call rate - case 0xc4: - if (req->param1 < NUM_INTERRUPTS) { - uint32_t load = interrupts[req->param1].call_rate; - resp[0] = (load & 0x000000FFU); - resp[1] = ((load & 0x0000FF00U) >> 8U); - resp[2] = ((load & 0x00FF0000U) >> 16U); - resp[3] = ((load & 0xFF000000U) >> 24U); - resp_len = 4U; - } - break; - // **** 0xc5: DEBUG: drive relay - case 0xc5: - set_intercept_relay((req->param1 & 0x1U), (req->param1 & 0x2U)); - break; - // **** 0xc6: DEBUG: read SOM GPIO - case 0xc6: - resp[0] = current_board->read_som_gpio(); - resp_len = 1; - break; - // **** 0xd0: fetch serial (aka the provisioned dongle ID) - case 0xd0: - // addresses are OTP - if (req->param1 == 1U) { - (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); - resp_len = 0x10; - } else { - get_provision_chunk(resp); - resp_len = PROVISION_CHUNK_LEN; - } - break; - // **** 0xd1: enter bootloader mode - case 0xd1: - // this allows reflashing of the bootstub - switch (req->param1) { - case 0: - // only allow bootloader entry on debug builds - #ifdef ALLOW_DEBUG - print("-> entering bootloader\n"); - enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; - NVIC_SystemReset(); - #endif - break; - case 1: - print("-> entering softloader\n"); - enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; - NVIC_SystemReset(); - break; - default: - print("Bootloader mode invalid\n"); - break; - } - break; - // **** 0xd2: get health packet - case 0xd2: - resp_len = get_health_pkt(resp); - break; - // **** 0xd3: get first 64 bytes of signature - case 0xd3: - { - resp_len = 64; - char * code = (char*)_app_start; - int code_len = _app_start[0]; - (void)memcpy(resp, &code[code_len], resp_len); - } - break; - // **** 0xd4: get second 64 bytes of signature - case 0xd4: - { - resp_len = 64; - char * code = (char*)_app_start; - int code_len = _app_start[0]; - (void)memcpy(resp, &code[code_len + 64], resp_len); - } - break; - // **** 0xd6: get version - case 0xd6: - COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); - (void)memcpy(resp, gitversion, sizeof(gitversion)); - resp_len = sizeof(gitversion) - 1U; - break; - // **** 0xd8: reset ST - case 0xd8: - NVIC_SystemReset(); - break; - // **** 0xdb: set OBD CAN multiplexing mode - case 0xdb: - current_board->set_can_mode((req->param1 == 1U) ? CAN_MODE_OBD_CAN2 : CAN_MODE_NORMAL); - break; - // **** 0xdc: set safety mode - case 0xdc: - set_safety_mode(req->param1, (uint16_t)req->param2); - break; - // **** 0xdd: get healthpacket and CANPacket versions - case 0xdd: - resp[0] = HEALTH_PACKET_VERSION; - resp[1] = CAN_PACKET_VERSION; - resp[2] = CAN_HEALTH_PACKET_VERSION; - resp_len = 3; - break; - // **** 0xde: set can bitrate - case 0xde: - if ((req->param1 < PANDA_CAN_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { - bus_config[req->param1].can_speed = req->param2; - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - // **** 0xdf: set alternative experience - case 0xdf: - // you can only set this if you are in a non car safety mode - if (!is_car_safety_mode(current_safety_mode)) { - alternative_experience = req->param1; - } - break; - // **** 0xe0: uart read - case 0xe0: - ur = get_ring_by_number(req->param1); - if (!ur) { - break; - } +// switch (req->request) { +// // **** 0xa8: get microsecond timer +// case 0xa8: +// time = microsecond_timer_get(); +// resp[0] = (time & 0x000000FFU); +// resp[1] = ((time & 0x0000FF00U) >> 8U); +// resp[2] = ((time & 0x00FF0000U) >> 16U); +// resp[3] = ((time & 0xFF000000U) >> 24U); +// resp_len = 4U; +// break; +// // **** 0xb0: set IR power +// case 0xb0: +// current_board->set_ir_power(req->param1); +// break; +// // **** 0xb1: set fan power +// case 0xb1: +// fan_set_power(req->param1); +// break; +// // **** 0xb2: get fan rpm +// case 0xb2: +// resp[0] = (fan_state.rpm & 0x00FFU); +// resp[1] = ((fan_state.rpm & 0xFF00U) >> 8U); +// resp_len = 2; +// break; +// // **** 0xc0: reset communications state +// case 0xc0: +// comms_can_reset(); +// break; +// // **** 0xc1: get hardware type +// case 0xc1: +// resp[0] = hw_type; +// resp_len = 1; +// break; +// // **** 0xc2: CAN health stats +// case 0xc2: +// COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); +// if (req->param1 < 3U) { +// update_can_health_pkt(req->param1, 0U); +// can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); +// can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); +// can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; +// can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; +// can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; +// resp_len = sizeof(can_health[req->param1]); +// (void)memcpy(resp, (uint8_t*)(&can_health[req->param1]), resp_len); +// } +// break; +// // **** 0xc3: fetch MCU UID +// case 0xc3: +// (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); +// resp_len = 12; +// break; +// // **** 0xc4: get interrupt call rate +// case 0xc4: +// if (req->param1 < NUM_INTERRUPTS) { +// uint32_t load = interrupts[req->param1].call_rate; +// resp[0] = (load & 0x000000FFU); +// resp[1] = ((load & 0x0000FF00U) >> 8U); +// resp[2] = ((load & 0x00FF0000U) >> 16U); +// resp[3] = ((load & 0xFF000000U) >> 24U); +// resp_len = 4U; +// } +// break; +// // **** 0xc5: DEBUG: drive relay +// case 0xc5: +// set_intercept_relay((req->param1 & 0x1U), (req->param1 & 0x2U)); +// break; +// // **** 0xc6: DEBUG: read SOM GPIO +// case 0xc6: +// resp[0] = current_board->read_som_gpio(); +// resp_len = 1; +// break; +// // **** 0xd0: fetch serial (aka the provisioned dongle ID) +// case 0xd0: +// // addresses are OTP +// if (req->param1 == 1U) { +// (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); +// resp_len = 0x10; +// } else { +// get_provision_chunk(resp); +// resp_len = PROVISION_CHUNK_LEN; +// } +// break; +// // **** 0xd1: enter bootloader mode +// case 0xd1: +// // this allows reflashing of the bootstub +// switch (req->param1) { +// case 0: +// // only allow bootloader entry on debug builds +// #ifdef ALLOW_DEBUG +// print("-> entering bootloader\n"); +// enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; +// NVIC_SystemReset(); +// #endif +// break; +// case 1: +// print("-> entering softloader\n"); +// enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; +// NVIC_SystemReset(); +// break; +// default: +// print("Bootloader mode invalid\n"); +// break; +// } +// break; +// // **** 0xd2: get health packet +// case 0xd2: +// resp_len = get_health_pkt(resp); +// break; +// // **** 0xd3: get first 64 bytes of signature +// case 0xd3: +// { +// resp_len = 64; +// char * code = (char*)_app_start; +// int code_len = _app_start[0]; +// (void)memcpy(resp, &code[code_len], resp_len); +// } +// break; +// // **** 0xd4: get second 64 bytes of signature +// case 0xd4: +// { +// resp_len = 64; +// char * code = (char*)_app_start; +// int code_len = _app_start[0]; +// (void)memcpy(resp, &code[code_len + 64], resp_len); +// } +// break; +// // **** 0xd6: get version +// case 0xd6: +// COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); +// (void)memcpy(resp, gitversion, sizeof(gitversion)); +// resp_len = sizeof(gitversion) - 1U; +// break; +// // **** 0xd8: reset ST +// case 0xd8: +// NVIC_SystemReset(); +// break; +// // **** 0xdb: set OBD CAN multiplexing mode +// case 0xdb: +// current_board->set_can_mode((req->param1 == 1U) ? CAN_MODE_OBD_CAN2 : CAN_MODE_NORMAL); +// break; +// // **** 0xdc: set safety mode +// case 0xdc: +// set_safety_mode(req->param1, (uint16_t)req->param2); +// break; +// // **** 0xdd: get healthpacket and CANPacket versions +// case 0xdd: +// resp[0] = HEALTH_PACKET_VERSION; +// resp[1] = CAN_PACKET_VERSION; +// resp[2] = CAN_HEALTH_PACKET_VERSION; +// resp_len = 3; +// break; +// // **** 0xde: set can bitrate +// case 0xde: +// if ((req->param1 < PANDA_CAN_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { +// bus_config[req->param1].can_speed = req->param2; +// bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); +// UNUSED(ret); +// } +// break; +// // **** 0xdf: set alternative experience +// case 0xdf: +// // you can only set this if you are in a non car safety mode +// if (!is_car_safety_mode(current_safety_mode)) { +// alternative_experience = req->param1; +// } +// break; +// // **** 0xe0: uart read +// case 0xe0: +// ur = get_ring_by_number(req->param1); +// if (!ur) { +// break; +// } - // read - uint16_t req_length = MIN(req->length, USBPACKET_MAX_SIZE); - while ((resp_len < req_length) && - get_char(ur, (char*)&resp[resp_len])) { - ++resp_len; - } - break; - // **** 0xe5: set CAN loopback (for testing) - case 0xe5: - can_loopback = req->param1 > 0U; - can_init_all(); - break; - // **** 0xe6: set custom clock source period and pulse length - case 0xe6: - clock_source_set_timer_params(req->param1, req->param2); - break; - // **** 0xe7: set power save state - case 0xe7: - set_power_save_state(req->param1); - break; - // **** 0xe8: set can-fd auto swithing mode - case 0xe8: - bus_config[req->param1].canfd_auto = req->param2 > 0U; - break; - // **** 0xf1: Clear CAN ring buffer. - case 0xf1: - if (req->param1 == 0xFFFFU) { - print("Clearing CAN Rx queue\n"); - can_clear(&can_rx_q); - } else if (req->param1 < PANDA_CAN_CNT) { - print("Clearing CAN Tx queue\n"); - can_clear(can_queues[req->param1]); - } else { - print("Clearing CAN CAN ring buffer failed: wrong bus number\n"); - } - break; - // **** 0xf3: Heartbeat. Resets heartbeat counter. - case 0xf3: - { - heartbeat_counter = 0U; - heartbeat_lost = false; - heartbeat_disabled = false; - heartbeat_engaged = (req->param1 == 1U); - break; - } - // **** 0xf6: set siren enabled - case 0xf6: - siren_enabled = (req->param1 != 0U); - break; - // **** 0xf8: disable heartbeat checks - case 0xf8: - if (!is_car_safety_mode(current_safety_mode)) { - heartbeat_disabled = true; - } - break; - // **** 0xf9: set CAN FD data bitrate - case 0xf9: - if ((req->param1 < PANDA_CAN_CNT) && - is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { - bus_config[req->param1].can_data_speed = req->param2; - bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); - bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - // **** 0xfc: set CAN FD non-ISO mode - case 0xfc: - if (req->param1 < PANDA_CAN_CNT) { - bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - default: - print("NO HANDLER "); - puth(req->request); - print("\n"); - break; - } - return resp_len; -} +// // read +// uint16_t req_length = MIN(req->length, USBPACKET_MAX_SIZE); +// while ((resp_len < req_length) && +// get_char(ur, (char*)&resp[resp_len])) { +// ++resp_len; +// } +// break; +// // **** 0xe5: set CAN loopback (for testing) +// case 0xe5: +// can_loopback = req->param1 > 0U; +// can_init_all(); +// break; +// // **** 0xe6: set custom clock source period and pulse length +// case 0xe6: +// clock_source_set_timer_params(req->param1, req->param2); +// break; +// // **** 0xe7: set power save state +// case 0xe7: +// set_power_save_state(req->param1); +// break; +// // **** 0xe8: set can-fd auto swithing mode +// case 0xe8: +// bus_config[req->param1].canfd_auto = req->param2 > 0U; +// break; +// // **** 0xf1: Clear CAN ring buffer. +// case 0xf1: +// if (req->param1 == 0xFFFFU) { +// print("Clearing CAN Rx queue\n"); +// can_clear(&can_rx_q); +// } else if (req->param1 < PANDA_CAN_CNT) { +// print("Clearing CAN Tx queue\n"); +// can_clear(can_queues[req->param1]); +// } else { +// print("Clearing CAN CAN ring buffer failed: wrong bus number\n"); +// } +// break; +// // **** 0xf3: Heartbeat. Resets heartbeat counter. +// case 0xf3: +// { +// heartbeat_counter = 0U; +// heartbeat_lost = false; +// heartbeat_disabled = false; +// heartbeat_engaged = (req->param1 == 1U); +// break; +// } +// // **** 0xf6: set siren enabled +// case 0xf6: +// siren_enabled = (req->param1 != 0U); +// break; +// // **** 0xf8: disable heartbeat checks +// case 0xf8: +// if (!is_car_safety_mode(current_safety_mode)) { +// heartbeat_disabled = true; +// } +// break; +// // **** 0xf9: set CAN FD data bitrate +// case 0xf9: +// if ((req->param1 < PANDA_CAN_CNT) && +// is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { +// bus_config[req->param1].can_data_speed = req->param2; +// bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); +// bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); +// bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); +// UNUSED(ret); +// } +// break; +// // **** 0xfc: set CAN FD non-ISO mode +// case 0xfc: +// if (req->param1 < PANDA_CAN_CNT) { +// bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); +// bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); +// UNUSED(ret); +// } +// break; +// default: +// print("NO HANDLER "); +// puth(req->request); +// print("\n"); +// break; +// } +// return resp_len; +// } diff --git a/board/stm32h7/board.c b/board/stm32h7/board.c index 4faae4151b7..b86d4dc9c5b 100644 --- a/board/stm32h7/board.c +++ b/board/stm32h7/board.c @@ -1,7 +1,9 @@ -#include "board/stm32h7/board.h" +#include +#include "board/stm32h7/board.h" #include "board/globals.h" + void detect_board_type(void) { // On STM32H7 pandas, we use two different sets of pins. const uint8_t id1 = detect_with_pull(GPIOF, 7, PULL_UP) | diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h index 90c4e064495..d54296945a6 100644 --- a/board/stm32h7/stm32h7_config.h +++ b/board/stm32h7/stm32h7_config.h @@ -72,12 +72,19 @@ separate IRQs for RX and TX. #endif #ifdef PANDA_JUNGLE +#error NOT DOING THIS BOY #include "board/jungle/stm32h7/board.h" #elif defined(PANDA_BODY) +#include "board/boards/boot_state.h" #include "board/body/stm32h7/board.h" +#include "board/stm32h7/lladc.h" +#include "board/drivers/clock_source.h" +#include "board/stm32h7/sound.h" +#include "board/drivers/harness.h" #else #include "board/stm32h7/board.h" #endif + #include "board/stm32h7/clock.h" #ifdef BOOTSTUB diff --git a/tests/libpanda/SConscript b/tests/libpanda/SConscript index a5bdd7ce745..ca22cf89e3f 100644 --- a/tests/libpanda/SConscript +++ b/tests/libpanda/SConscript @@ -20,6 +20,7 @@ env = Environment( '-std=gnu11', '-Wfatal-errors', '-Wno-pointer-to-int-cast', + "-DLIB_PANDA", ], CPPPATH=[".", "../../", "../../board/", opendbc.INCLUDE_PATH], ) @@ -34,5 +35,47 @@ if GetOption('ubsan'): env['CFLAGS'] += flags env['LINKFLAGS'] += flags -panda = env.SharedObject("panda.os", "panda.c") -libpanda = env.SharedLibrary("libpanda.so", [panda]) +shared = [ + "./crypto/rsa.c", + "./crypto/sha.c", + "./board/can_comms.c", + "./board/drivers/can_common.c", + "./board/fake_stm.c", + + # "./board/libc.c", + # "./board/crc.c", + # "./board/early_init.c", + # "./board/stm32h7/board.c", + # "./board/boards/red.c", + # "./board/critical.c", + # "./board/drivers/led.c", + # "./board/drivers/pwm.c", + # "./board/drivers/gpio.c", + # "./board/drivers/fake_siren.c", + # "./board/stm32h7/lli2c.c", + # "./board/stm32h7/clock.c", + # "./board/drivers/clock_source.c", + # "./board/stm32h7/sound.c", + # "./board/stm32h7/llflash.c", + # "./board/stm32h7/stm32h7_config.c", + # "./board/drivers/registers.c", + # "./board/drivers/interrupts.c", + # "./board/provision.c", + # "./board/stm32h7/peripherals.c", + # "./board/stm32h7/llusb.c", + # "./board/drivers/usb.c", + # "./board/drivers/spi.c", + # "./board/drivers/timers.c", + # "./board/boards/cuatro.c", + # "./board/boards/tres.c", + # "./board/stm32h7/lladc.c", + # "./board/stm32h7/llspi.c", + # "./board/faults.c", + # "./board/boards/unused_funcs.c", + "./board/utils.c", +] + +c = ["panda.c"] + ["../../" + s for s in shared] + +objs = env.SharedObject(c) +libpanda = env.SharedLibrary("libpanda.so", objs) diff --git a/tests/libpanda/panda.c b/tests/libpanda/panda.c index 2d17d64e34c..22269475ad6 100644 --- a/tests/libpanda/panda.c +++ b/tests/libpanda/panda.c @@ -16,7 +16,7 @@ void can_tx_comms_resume_spi(void) { }; #include "libc.h" #include "boards/board_declarations.h" #include "opendbc/safety/safety.h" -#include "main_definitions.h" +// #include "main_definitions.h" #include "drivers/can_common.h" can_ring *rx_q = &can_rx_q; From c9608dc87245d51f20794a7d178a131d0e1dce43 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Thu, 4 Dec 2025 20:52:51 +0100 Subject: [PATCH 05/29] Building all of the things --- SConscript | 34 +++-- board/board_struct.h | 46 ++---- board/can_comms.c | 1 + board/config.h | 33 +++++ board/drivers/fake_siren.c | 1 + board/drivers/gpio.c | 5 - board/drivers/gpio.h | 13 +- board/globals.h | 3 +- board/jungle/boards/board_v2.c | 246 ++++++++++++++++++++++++++++++++ board/jungle/boards/board_v2.h | 249 ++------------------------------- board/jungle/main.c | 4 + board/jungle/main_comms.h | 6 + board/jungle/stm32h7/board.c | 15 ++ board/jungle/stm32h7/board.h | 7 +- board/main_declarations.h | 1 - board/stm32h7/stm32h7_config.h | 1 - 16 files changed, 358 insertions(+), 307 deletions(-) create mode 100644 board/jungle/boards/board_v2.c create mode 100644 board/jungle/stm32h7/board.c diff --git a/SConscript b/SConscript index 74fa081473f..a776a26cd13 100644 --- a/SConscript +++ b/SConscript @@ -103,7 +103,6 @@ def build_project(project_name, project, main, extra_flags): shared = [ "./crypto/rsa.c", "./crypto/sha.c", - "board/body/stm32h7/board.c", "./board/libc.c", "./board/crc.c", "./board/early_init.c", @@ -127,8 +126,7 @@ def build_project(project_name, project, main, extra_flags): "./board/drivers/usb.c", "./board/drivers/spi.c", "./board/drivers/timers.c", - # "./board/boards/cuatro.c", - # "./board/boards/tres.c", + "./board/stm32h7/lladc.c", "./board/stm32h7/llspi.c", "./board/faults.c", @@ -136,6 +134,28 @@ def build_project(project_name, project, main, extra_flags): "./board/utils.c", ] + if "-DPANDA_JUNGLE" in flags: + shared += [ + "board/jungle/stm32h7/board.c", + "board/jungle/boards/board_v2.c" + ] + + if "-DPANDA" in flags: + shared += [ + "board/stm32h7/board.c", + "board/boards/tres.c", + "board/boards/red.c", + "board/boards/cuatro.c", + ] + + if "-DPANDA_BODY" in flags: + shared += [ + "board/body/boards/board_body.c", + "board/body/stm32h7/board.c", + "board/body/motor_control.c", + "board/body/motor_encoder.c" + ] + # board/body/boards/board_body.c # # "./board/main_definitions.c", # Build bootstub @@ -206,11 +226,10 @@ with open("board/obj/cert.h", "w") as f: # panda fw panda_c = [ - "./board/stm32h7/board.c", "./board/main.c", ] -# build_project("panda_h7", base_project_h7, panda_c, ["-DPANDA"]) +build_project("panda_h7", base_project_h7, panda_c, ["-DPANDA"]) # # panda jungle fw flags = [ @@ -219,14 +238,11 @@ flags = [ if os.getenv("FINAL_PROVISIONING"): flags += ["-DFINAL_PROVISIONING"] -# build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", flags) +build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", flags) # # body fw body_c = [ - "./board/body/motor_control.c", "./board/body/main.c", - "./board/body/boards/board_body.c", - "./board/body/motor_encoder.c" ] build_project("body_h7", base_project_h7, body_c, ["-DPANDA_BODY"]) diff --git a/board/board_struct.h b/board/board_struct.h index 05ceaeea6c9..c7b72c37690 100644 --- a/board/board_struct.h +++ b/board/board_struct.h @@ -16,6 +16,9 @@ typedef struct board { const uint8_t fan_enable_cooldown_time; board_init init; board_init_bootloader init_bootloader; + board_board_tick board_tick; + board_set_panda_power set_panda_power; + board_get_button get_button; board_enable_can_transceiver enable_can_transceiver; board_set_can_mode set_can_mode; board_read_voltage_mV read_voltage_mV; @@ -26,40 +29,11 @@ typedef struct board { board_set_bootkick set_bootkick; board_read_som_gpio read_som_gpio; board_set_amp_enabled set_amp_enabled; + board_set_panda_individual_power set_panda_individual_power; + board_set_ignition set_ignition; + board_set_individual_ignition set_individual_ignition; + board_set_harness_orientation set_harness_orientation; + board_enable_header_pin enable_header_pin; + board_get_channel_power get_channel_power; + board_get_sbu_mV get_sbu_mV; } board; - - -#ifdef PANDA -#define HW_TYPE_UNKNOWN 0U -#define HW_TYPE_RED_PANDA 7U -#define HW_TYPE_TRES 9U -#define HW_TYPE_CUATRO 10U - -// CAN modes -#define CAN_MODE_NORMAL 0U -#define CAN_MODE_OBD_CAN2 1U - -#elif defined(PANDA_JUNGLE) -#define HW_TYPE_UNKNOWN 0U -#define HW_TYPE_V2 2U - -// CAN modes -#define CAN_MODE_NORMAL 0U -#define CAN_MODE_OBD_CAN2 3U - -// Harness states -#define HARNESS_ORIENTATION_NONE 0U -#define HARNESS_ORIENTATION_1 1U -#define HARNESS_ORIENTATION_2 2U - -#define SBU1 0U -#define SBU2 1U - -#elif defined(PANDA_JUNGLE) - -#elif defined(PANDA_BODY) -#elif defined(LIB_PANDA) -// NO IDEA BOI -#else -#error FUCK YOU -#endif \ No newline at end of file diff --git a/board/can_comms.c b/board/can_comms.c index 52a031b9755..2304ad13522 100644 --- a/board/can_comms.c +++ b/board/can_comms.c @@ -4,6 +4,7 @@ #include "libc.h" #include "can.h" #include "drivers/usb.h" +#include "board/drivers/spi.h" #include "config.h" diff --git a/board/config.h b/board/config.h index ab8d922c5ad..10c4785cd33 100644 --- a/board/config.h +++ b/board/config.h @@ -32,6 +32,39 @@ #endif #endif +#ifdef PANDA +#define HW_TYPE_UNKNOWN 0U +#define HW_TYPE_RED_PANDA 7U +#define HW_TYPE_TRES 9U +#define HW_TYPE_CUATRO 10U + +// CAN modes +#define CAN_MODE_NORMAL 0U +#define CAN_MODE_OBD_CAN2 1U + +#elif defined(PANDA_JUNGLE) +#define HW_TYPE_UNKNOWN 0U +#define HW_TYPE_V2 2U + +// CAN modes +#define CAN_MODE_NORMAL 0U +#define CAN_MODE_OBD_CAN2 3U + +// Harness states +#define HARNESS_ORIENTATION_NONE 0U +#define HARNESS_ORIENTATION_1 1U +#define HARNESS_ORIENTATION_2 2U + +#define SBU1 0U +#define SBU2 1U + +#elif defined(PANDA_BODY) +#elif defined(LIB_PANDA) +// NO IDEA BOI +#else +#error FUCK YOU +#endif + // TODO, better spot void detect_board_type(void); diff --git a/board/drivers/fake_siren.c b/board/drivers/fake_siren.c index 189bb12ba34..deda28c024f 100644 --- a/board/drivers/fake_siren.c +++ b/board/drivers/fake_siren.c @@ -2,6 +2,7 @@ #include "board/stm32h7/lli2c.h" #include "board/globals.h" +#include "board/stm32h7/sound.h" void siren_tim7_init(void) { // Init trigger timer (around 2.5kHz) diff --git a/board/drivers/gpio.c b/board/drivers/gpio.c index e73ed0f92e1..98faf1b0330 100644 --- a/board/drivers/gpio.c +++ b/board/drivers/gpio.c @@ -54,11 +54,6 @@ int get_gpio_input(const GPIO_TypeDef *GPIO, unsigned int pin) { } #ifdef PANDA_JUNGLE -typedef struct { - GPIO_TypeDef * const bank; - uint8_t pin; -} gpio_t; - void gpio_set_all_output(gpio_t *pins, uint8_t num_pins, bool enabled) { for (uint8_t i = 0; i < num_pins; i++) { set_gpio_output(pins[i].bank, pins[i].pin, enabled); diff --git a/board/drivers/gpio.h b/board/drivers/gpio.h index 86bc7a3e18c..a9afb9e40ba 100644 --- a/board/drivers/gpio.h +++ b/board/drivers/gpio.h @@ -27,17 +27,8 @@ typedef struct { uint8_t pin; } gpio_t; -void gpio_set_all_output(gpio_t *pins, uint8_t num_pins, bool enabled) { - for (uint8_t i = 0; i < num_pins; i++) { - set_gpio_output(pins[i].bank, pins[i].pin, enabled); - } -} - -void gpio_set_bitmask(gpio_t *pins, uint8_t num_pins, uint32_t bitmask) { - for (uint8_t i = 0; i < num_pins; i++) { - set_gpio_output(pins[i].bank, pins[i].pin, (bitmask >> i) & 1U); - } -} +void gpio_set_all_output(gpio_t *pins, uint8_t num_pins, bool enabled); +void gpio_set_bitmask(gpio_t *pins, uint8_t num_pins, uint32_t bitmask); #endif // Detection with internal pullup diff --git a/board/globals.h b/board/globals.h index 2f884be2d3b..4ccb31b17d2 100644 --- a/board/globals.h +++ b/board/globals.h @@ -9,4 +9,5 @@ typedef struct board board; extern uint8_t hw_type; extern board *current_board; -extern struct harness_t harness; \ No newline at end of file +extern struct harness_t harness; +extern uint32_t uptime_cnt; \ No newline at end of file diff --git a/board/jungle/boards/board_v2.c b/board/jungle/boards/board_v2.c new file mode 100644 index 00000000000..903a29dac92 --- /dev/null +++ b/board/jungle/boards/board_v2.c @@ -0,0 +1,246 @@ +#include "board/board_struct.h" +#include "board/jungle/boards/board_v2.h" + +uint8_t harness_orientation = HARNESS_ORIENTATION_NONE; +uint8_t can_mode = CAN_MODE_NORMAL; +uint8_t ignition = 0U; + +void board_v2_set_harness_orientation(uint8_t orientation) { + switch (orientation) { + case HARNESS_ORIENTATION_NONE: + gpio_set_all_output(sbu1_ignition_pins, sizeof(sbu1_ignition_pins) / sizeof(gpio_t), false); + gpio_set_all_output(sbu1_relay_pins, sizeof(sbu1_relay_pins) / sizeof(gpio_t), false); + gpio_set_all_output(sbu2_ignition_pins, sizeof(sbu2_ignition_pins) / sizeof(gpio_t), false); + gpio_set_all_output(sbu2_relay_pins, sizeof(sbu2_relay_pins) / sizeof(gpio_t), false); + harness_orientation = orientation; + break; + case HARNESS_ORIENTATION_1: + gpio_set_all_output(sbu1_ignition_pins, sizeof(sbu1_ignition_pins) / sizeof(gpio_t), false); + gpio_set_all_output(sbu1_relay_pins, sizeof(sbu1_relay_pins) / sizeof(gpio_t), true); + gpio_set_bitmask(sbu2_ignition_pins, sizeof(sbu2_ignition_pins) / sizeof(gpio_t), ignition); + gpio_set_all_output(sbu2_relay_pins, sizeof(sbu2_relay_pins) / sizeof(gpio_t), false); + harness_orientation = orientation; + break; + case HARNESS_ORIENTATION_2: + gpio_set_bitmask(sbu1_ignition_pins, sizeof(sbu1_ignition_pins) / sizeof(gpio_t), ignition); + gpio_set_all_output(sbu1_relay_pins, sizeof(sbu1_relay_pins) / sizeof(gpio_t), false); + gpio_set_all_output(sbu2_ignition_pins, sizeof(sbu2_ignition_pins) / sizeof(gpio_t), false); + gpio_set_all_output(sbu2_relay_pins, sizeof(sbu2_relay_pins) / sizeof(gpio_t), true); + harness_orientation = orientation; + break; + default: + print("Tried to set an unsupported harness orientation: "); puth(orientation); print("\n"); + break; + } +} + +void board_v2_enable_can_transceiver(uint8_t transceiver, bool enabled) { + switch (transceiver) { + case 1U: + set_gpio_output(GPIOG, 11, !enabled); + break; + case 2U: + set_gpio_output(GPIOB, 3, !enabled); + break; + case 3U: + set_gpio_output(GPIOD, 7, !enabled); + break; + case 4U: + set_gpio_output(GPIOB, 4, !enabled); + break; + default: + print("Invalid CAN transceiver ("); puth(transceiver); print("): enabling failed\n"); + break; + } +} + +void board_v2_enable_header_pin(uint8_t pin_num, bool enabled) { + if (pin_num < 8U) { + set_gpio_output(GPIOG, pin_num, enabled); + } else { + print("Invalid pin number ("); puth(pin_num); print("): enabling failed\n"); + } +} + +void board_v2_set_can_mode(uint8_t mode) { + board_v2_enable_can_transceiver(2U, false); + board_v2_enable_can_transceiver(4U, false); + switch (mode) { + case CAN_MODE_NORMAL: + // B12,B13: disable normal mode + set_gpio_pullup(GPIOB, 12, PULL_NONE); + set_gpio_mode(GPIOB, 12, MODE_ANALOG); + + set_gpio_pullup(GPIOB, 13, PULL_NONE); + set_gpio_mode(GPIOB, 13, MODE_ANALOG); + + // B5,B6: FDCAN2 mode + set_gpio_pullup(GPIOB, 5, PULL_NONE); + set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2); + + set_gpio_pullup(GPIOB, 6, PULL_NONE); + set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2); + can_mode = CAN_MODE_NORMAL; + board_v2_enable_can_transceiver(2U, true); + break; + case CAN_MODE_OBD_CAN2: + // B5,B6: disable normal mode + set_gpio_pullup(GPIOB, 5, PULL_NONE); + set_gpio_mode(GPIOB, 5, MODE_ANALOG); + + set_gpio_pullup(GPIOB, 6, PULL_NONE); + set_gpio_mode(GPIOB, 6, MODE_ANALOG); + // B12,B13: FDCAN2 mode + set_gpio_pullup(GPIOB, 12, PULL_NONE); + set_gpio_alternate(GPIOB, 12, GPIO_AF9_FDCAN2); + + set_gpio_pullup(GPIOB, 13, PULL_NONE); + set_gpio_alternate(GPIOB, 13, GPIO_AF9_FDCAN2); + can_mode = CAN_MODE_OBD_CAN2; + board_v2_enable_can_transceiver(4U, true); + break; + default: + break; + } +} + +bool panda_power = false; +uint8_t panda_power_bitmask = 0U; +void board_v2_set_panda_power(bool enable) { + panda_power = enable; + gpio_set_all_output(power_pins, sizeof(power_pins) / sizeof(gpio_t), enable); + if (enable) { + panda_power_bitmask = 0xFFU; + } else { + panda_power_bitmask = 0U; + } +} + +void board_v2_set_panda_individual_power(uint8_t port_num, bool enable) { + port_num -= 1U; + if (port_num < 6U) { + panda_power_bitmask &= ~(1U << port_num); + panda_power_bitmask |= (enable ? 1U : 0U) << port_num; + } else { + print("Invalid port number ("); puth(port_num); print("): enabling failed\n"); + } + gpio_set_bitmask(power_pins, sizeof(power_pins) / sizeof(gpio_t), (uint32_t)panda_power_bitmask); +} + +bool board_v2_get_button(void) { + return get_gpio_input(GPIOG, 15); +} + +void board_v2_set_ignition(bool enabled) { + ignition = enabled ? 0xFFU : 0U; + board_v2_set_harness_orientation(harness_orientation); +} + +void board_v2_set_individual_ignition(uint8_t bitmask) { + ignition = bitmask; + board_v2_set_harness_orientation(harness_orientation); +} + +float board_v2_get_channel_power(uint8_t channel) { + float ret = 0.0f; + if ((channel >= 1U) && (channel <= 6U)) { + uint16_t readout = adc_get_mV(&(const adc_signal_t) ADC_CHANNEL(ADC1, channel - 1U)); // these are mapped nicely in hardware + ret = (((float) readout / 33e6) - 0.8e-6) / 52e-6 * 12.0f; + } else { + print("Invalid channel ("); puth(channel); print(")\n"); + } + return ret; +} + +uint16_t board_v2_get_sbu_mV(uint8_t channel, uint8_t sbu) { + uint16_t ret = 0U; + if ((channel >= 1U) && (channel <= 6U)) { + switch(sbu){ + case SBU1: + ret = adc_get_mV(&sbu1_channels[channel - 1U]); + break; + case SBU2: + ret = adc_get_mV(&sbu2_channels[channel - 1U]); + break; + default: + print("Invalid SBU ("); puth(sbu); print(")\n"); + break; + } + } else { + print("Invalid channel ("); puth(channel); print(")\n"); + } + return ret; +} + +void board_v2_init(void) { + common_init_gpio(); + + // Normal CAN mode + board_v2_set_can_mode(CAN_MODE_NORMAL); + + // Enable CAN transceivers + for(uint8_t i = 1; i <= 4; i++) { + board_v2_enable_can_transceiver(i, true); + } + + // Set to no harness orientation + board_v2_set_harness_orientation(HARNESS_ORIENTATION_NONE); + + // Enable panda power by default + board_v2_set_panda_power(true); + + // Current monitor channels + adc_init(ADC1); + register_set_bits(&SYSCFG->PMCR, SYSCFG_PMCR_PA0SO | SYSCFG_PMCR_PA1SO); // open up analog switches for PA0_C and PA1_C + set_gpio_mode(GPIOF, 11, MODE_ANALOG); + set_gpio_mode(GPIOA, 6, MODE_ANALOG); + set_gpio_mode(GPIOC, 4, MODE_ANALOG); + set_gpio_mode(GPIOB, 1, MODE_ANALOG); + + // SBU channels + adc_init(ADC3); + set_gpio_mode(GPIOC, 2, MODE_ANALOG); + set_gpio_mode(GPIOC, 3, MODE_ANALOG); + set_gpio_mode(GPIOF, 9, MODE_ANALOG); + set_gpio_mode(GPIOF, 7, MODE_ANALOG); + set_gpio_mode(GPIOF, 5, MODE_ANALOG); + set_gpio_mode(GPIOF, 3, MODE_ANALOG); + set_gpio_mode(GPIOF, 10, MODE_ANALOG); + set_gpio_mode(GPIOF, 8, MODE_ANALOG); + set_gpio_mode(GPIOF, 6, MODE_ANALOG); + set_gpio_mode(GPIOF, 4, MODE_ANALOG); + set_gpio_mode(GPIOC, 0, MODE_ANALOG); + set_gpio_mode(GPIOC, 1, MODE_ANALOG); + + // Header pins + set_gpio_mode(GPIOG, 0, MODE_OUTPUT); + set_gpio_mode(GPIOG, 1, MODE_OUTPUT); + set_gpio_mode(GPIOG, 2, MODE_OUTPUT); + set_gpio_mode(GPIOG, 3, MODE_OUTPUT); + set_gpio_mode(GPIOG, 4, MODE_OUTPUT); + set_gpio_mode(GPIOG, 5, MODE_OUTPUT); + set_gpio_mode(GPIOG, 6, MODE_OUTPUT); + set_gpio_mode(GPIOG, 7, MODE_OUTPUT); +} + +void board_v2_tick(void) {} + +board board_v2 = { + .avdd_mV = 3300U, + .init = &board_v2_init, + .init_bootloader = &board_v2_tick, + .led_GPIO = {GPIOE, GPIOE, GPIOE}, + .led_pin = {4, 3, 2}, + .board_tick = &board_v2_tick, + .get_button = &board_v2_get_button, + .set_panda_power = &board_v2_set_panda_power, + .set_panda_individual_power = &board_v2_set_panda_individual_power, + .set_ignition = &board_v2_set_ignition, + .set_individual_ignition = &board_v2_set_individual_ignition, + .set_harness_orientation = &board_v2_set_harness_orientation, + .set_can_mode = &board_v2_set_can_mode, + .enable_can_transceiver = &board_v2_enable_can_transceiver, + .enable_header_pin = &board_v2_enable_header_pin, + .get_channel_power = &board_v2_get_channel_power, + .get_sbu_mV = &board_v2_get_sbu_mV, +}; diff --git a/board/jungle/boards/board_v2.h b/board/jungle/boards/board_v2.h index 45f8fb92797..3587f1c91da 100644 --- a/board/jungle/boards/board_v2.h +++ b/board/jungle/boards/board_v2.h @@ -1,7 +1,17 @@ +#pragma once + // ///////////////////////// // // Jungle board v2 (STM32H7) // // ///////////////////////// // +#include "board/drivers/registers.h" +#include "board/board_struct.h" +#include "board/drivers/gpio.h" + +#ifndef PANDA_JUNGLE +#error no pandas! +#endif + #define ADC_CHANNEL(a, c) {.adc = (a), .channel = (c), .sample_time = SAMPLETIME_810_CYCLES, .oversampling = OVERSAMPLING_1} gpio_t power_pins[] = { @@ -67,242 +77,5 @@ const adc_signal_t sbu2_channels[] = { ADC_CHANNEL(ADC3, 11), }; -void board_v2_set_harness_orientation(uint8_t orientation) { - switch (orientation) { - case HARNESS_ORIENTATION_NONE: - gpio_set_all_output(sbu1_ignition_pins, sizeof(sbu1_ignition_pins) / sizeof(gpio_t), false); - gpio_set_all_output(sbu1_relay_pins, sizeof(sbu1_relay_pins) / sizeof(gpio_t), false); - gpio_set_all_output(sbu2_ignition_pins, sizeof(sbu2_ignition_pins) / sizeof(gpio_t), false); - gpio_set_all_output(sbu2_relay_pins, sizeof(sbu2_relay_pins) / sizeof(gpio_t), false); - harness_orientation = orientation; - break; - case HARNESS_ORIENTATION_1: - gpio_set_all_output(sbu1_ignition_pins, sizeof(sbu1_ignition_pins) / sizeof(gpio_t), false); - gpio_set_all_output(sbu1_relay_pins, sizeof(sbu1_relay_pins) / sizeof(gpio_t), true); - gpio_set_bitmask(sbu2_ignition_pins, sizeof(sbu2_ignition_pins) / sizeof(gpio_t), ignition); - gpio_set_all_output(sbu2_relay_pins, sizeof(sbu2_relay_pins) / sizeof(gpio_t), false); - harness_orientation = orientation; - break; - case HARNESS_ORIENTATION_2: - gpio_set_bitmask(sbu1_ignition_pins, sizeof(sbu1_ignition_pins) / sizeof(gpio_t), ignition); - gpio_set_all_output(sbu1_relay_pins, sizeof(sbu1_relay_pins) / sizeof(gpio_t), false); - gpio_set_all_output(sbu2_ignition_pins, sizeof(sbu2_ignition_pins) / sizeof(gpio_t), false); - gpio_set_all_output(sbu2_relay_pins, sizeof(sbu2_relay_pins) / sizeof(gpio_t), true); - harness_orientation = orientation; - break; - default: - print("Tried to set an unsupported harness orientation: "); puth(orientation); print("\n"); - break; - } -} - -void board_v2_enable_can_transceiver(uint8_t transceiver, bool enabled) { - switch (transceiver) { - case 1U: - set_gpio_output(GPIOG, 11, !enabled); - break; - case 2U: - set_gpio_output(GPIOB, 3, !enabled); - break; - case 3U: - set_gpio_output(GPIOD, 7, !enabled); - break; - case 4U: - set_gpio_output(GPIOB, 4, !enabled); - break; - default: - print("Invalid CAN transceiver ("); puth(transceiver); print("): enabling failed\n"); - break; - } -} - -void board_v2_enable_header_pin(uint8_t pin_num, bool enabled) { - if (pin_num < 8U) { - set_gpio_output(GPIOG, pin_num, enabled); - } else { - print("Invalid pin number ("); puth(pin_num); print("): enabling failed\n"); - } -} - -void board_v2_set_can_mode(uint8_t mode) { - board_v2_enable_can_transceiver(2U, false); - board_v2_enable_can_transceiver(4U, false); - switch (mode) { - case CAN_MODE_NORMAL: - // B12,B13: disable normal mode - set_gpio_pullup(GPIOB, 12, PULL_NONE); - set_gpio_mode(GPIOB, 12, MODE_ANALOG); - - set_gpio_pullup(GPIOB, 13, PULL_NONE); - set_gpio_mode(GPIOB, 13, MODE_ANALOG); - - // B5,B6: FDCAN2 mode - set_gpio_pullup(GPIOB, 5, PULL_NONE); - set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2); - - set_gpio_pullup(GPIOB, 6, PULL_NONE); - set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2); - can_mode = CAN_MODE_NORMAL; - board_v2_enable_can_transceiver(2U, true); - break; - case CAN_MODE_OBD_CAN2: - // B5,B6: disable normal mode - set_gpio_pullup(GPIOB, 5, PULL_NONE); - set_gpio_mode(GPIOB, 5, MODE_ANALOG); - - set_gpio_pullup(GPIOB, 6, PULL_NONE); - set_gpio_mode(GPIOB, 6, MODE_ANALOG); - // B12,B13: FDCAN2 mode - set_gpio_pullup(GPIOB, 12, PULL_NONE); - set_gpio_alternate(GPIOB, 12, GPIO_AF9_FDCAN2); - - set_gpio_pullup(GPIOB, 13, PULL_NONE); - set_gpio_alternate(GPIOB, 13, GPIO_AF9_FDCAN2); - can_mode = CAN_MODE_OBD_CAN2; - board_v2_enable_can_transceiver(4U, true); - break; - default: - break; - } -} - -bool panda_power = false; -uint8_t panda_power_bitmask = 0U; -void board_v2_set_panda_power(bool enable) { - panda_power = enable; - gpio_set_all_output(power_pins, sizeof(power_pins) / sizeof(gpio_t), enable); - if (enable) { - panda_power_bitmask = 0xFFU; - } else { - panda_power_bitmask = 0U; - } -} - -void board_v2_set_panda_individual_power(uint8_t port_num, bool enable) { - port_num -= 1U; - if (port_num < 6U) { - panda_power_bitmask &= ~(1U << port_num); - panda_power_bitmask |= (enable ? 1U : 0U) << port_num; - } else { - print("Invalid port number ("); puth(port_num); print("): enabling failed\n"); - } - gpio_set_bitmask(power_pins, sizeof(power_pins) / sizeof(gpio_t), (uint32_t)panda_power_bitmask); -} - -bool board_v2_get_button(void) { - return get_gpio_input(GPIOG, 15); -} -void board_v2_set_ignition(bool enabled) { - ignition = enabled ? 0xFFU : 0U; - board_v2_set_harness_orientation(harness_orientation); -} - -void board_v2_set_individual_ignition(uint8_t bitmask) { - ignition = bitmask; - board_v2_set_harness_orientation(harness_orientation); -} - -float board_v2_get_channel_power(uint8_t channel) { - float ret = 0.0f; - if ((channel >= 1U) && (channel <= 6U)) { - uint16_t readout = adc_get_mV(&(const adc_signal_t) ADC_CHANNEL(ADC1, channel - 1U)); // these are mapped nicely in hardware - ret = (((float) readout / 33e6) - 0.8e-6) / 52e-6 * 12.0f; - } else { - print("Invalid channel ("); puth(channel); print(")\n"); - } - return ret; -} - -uint16_t board_v2_get_sbu_mV(uint8_t channel, uint8_t sbu) { - uint16_t ret = 0U; - if ((channel >= 1U) && (channel <= 6U)) { - switch(sbu){ - case SBU1: - ret = adc_get_mV(&sbu1_channels[channel - 1U]); - break; - case SBU2: - ret = adc_get_mV(&sbu2_channels[channel - 1U]); - break; - default: - print("Invalid SBU ("); puth(sbu); print(")\n"); - break; - } - } else { - print("Invalid channel ("); puth(channel); print(")\n"); - } - return ret; -} - -void board_v2_init(void) { - common_init_gpio(); - - // Normal CAN mode - board_v2_set_can_mode(CAN_MODE_NORMAL); - - // Enable CAN transceivers - for(uint8_t i = 1; i <= 4; i++) { - board_v2_enable_can_transceiver(i, true); - } - - // Set to no harness orientation - board_v2_set_harness_orientation(HARNESS_ORIENTATION_NONE); - - // Enable panda power by default - board_v2_set_panda_power(true); - - // Current monitor channels - adc_init(ADC1); - register_set_bits(&SYSCFG->PMCR, SYSCFG_PMCR_PA0SO | SYSCFG_PMCR_PA1SO); // open up analog switches for PA0_C and PA1_C - set_gpio_mode(GPIOF, 11, MODE_ANALOG); - set_gpio_mode(GPIOA, 6, MODE_ANALOG); - set_gpio_mode(GPIOC, 4, MODE_ANALOG); - set_gpio_mode(GPIOB, 1, MODE_ANALOG); - - // SBU channels - adc_init(ADC3); - set_gpio_mode(GPIOC, 2, MODE_ANALOG); - set_gpio_mode(GPIOC, 3, MODE_ANALOG); - set_gpio_mode(GPIOF, 9, MODE_ANALOG); - set_gpio_mode(GPIOF, 7, MODE_ANALOG); - set_gpio_mode(GPIOF, 5, MODE_ANALOG); - set_gpio_mode(GPIOF, 3, MODE_ANALOG); - set_gpio_mode(GPIOF, 10, MODE_ANALOG); - set_gpio_mode(GPIOF, 8, MODE_ANALOG); - set_gpio_mode(GPIOF, 6, MODE_ANALOG); - set_gpio_mode(GPIOF, 4, MODE_ANALOG); - set_gpio_mode(GPIOC, 0, MODE_ANALOG); - set_gpio_mode(GPIOC, 1, MODE_ANALOG); - - // Header pins - set_gpio_mode(GPIOG, 0, MODE_OUTPUT); - set_gpio_mode(GPIOG, 1, MODE_OUTPUT); - set_gpio_mode(GPIOG, 2, MODE_OUTPUT); - set_gpio_mode(GPIOG, 3, MODE_OUTPUT); - set_gpio_mode(GPIOG, 4, MODE_OUTPUT); - set_gpio_mode(GPIOG, 5, MODE_OUTPUT); - set_gpio_mode(GPIOG, 6, MODE_OUTPUT); - set_gpio_mode(GPIOG, 7, MODE_OUTPUT); -} - -void board_v2_tick(void) {} - -board board_v2 = { - .avdd_mV = 3300U, - .init = &board_v2_init, - .init_bootloader = &board_v2_tick, - .led_GPIO = {GPIOE, GPIOE, GPIOE}, - .led_pin = {4, 3, 2}, - .board_tick = &board_v2_tick, - .get_button = &board_v2_get_button, - .set_panda_power = &board_v2_set_panda_power, - .set_panda_individual_power = &board_v2_set_panda_individual_power, - .set_ignition = &board_v2_set_ignition, - .set_individual_ignition = &board_v2_set_individual_ignition, - .set_harness_orientation = &board_v2_set_harness_orientation, - .set_can_mode = &board_v2_set_can_mode, - .enable_can_transceiver = &board_v2_enable_can_transceiver, - .enable_header_pin = &board_v2_enable_header_pin, - .get_channel_power = &board_v2_get_channel_power, - .get_sbu_mV = &board_v2_get_sbu_mV, -}; +extern struct board board_v2; \ No newline at end of file diff --git a/board/jungle/main.c b/board/jungle/main.c index 7bac6d751b9..7473f79f737 100644 --- a/board/jungle/main.c +++ b/board/jungle/main.c @@ -22,6 +22,10 @@ #include "board/can_comms.h" #include "board/jungle/main_comms.h" +uint8_t hw_type; +board *current_board; +struct harness_t harness; +uint32_t uptime_cnt = 0; // ********************* Serial debugging ********************* diff --git a/board/jungle/main_comms.h b/board/jungle/main_comms.h index b19f799ad9e..304bd52cf08 100644 --- a/board/jungle/main_comms.h +++ b/board/jungle/main_comms.h @@ -2,6 +2,12 @@ extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used bool generated_can_traffic = false; +#include "board/globals.h" + + +// TODO: +extern bool panda_power; + int get_jungle_health_pkt(void *dat) { COMPILE_TIME_ASSERT(sizeof(struct jungle_health_t) <= USBPACKET_MAX_SIZE); struct jungle_health_t * health = (struct jungle_health_t*)dat; diff --git a/board/jungle/stm32h7/board.c b/board/jungle/stm32h7/board.c new file mode 100644 index 00000000000..30405a1a5bf --- /dev/null +++ b/board/jungle/stm32h7/board.c @@ -0,0 +1,15 @@ +#include "board.h" + +#ifndef PANDA_JUNGLE +#error FUCK YOU +#endif + +#include "board/globals.h" +#include "board/config.h" + +extern board board_v2; + +void detect_board_type(void) { + hw_type = HW_TYPE_V2; + current_board = &board_v2; +} diff --git a/board/jungle/stm32h7/board.h b/board/jungle/stm32h7/board.h index c5be1dd43c0..0a79b86a093 100644 --- a/board/jungle/stm32h7/board.h +++ b/board/jungle/stm32h7/board.h @@ -3,9 +3,6 @@ #include "board/jungle/boards/board_declarations.h" #include "board/stm32h7/lladc.h" -#include "board/jungle/boards/board_v2.h" +// #include "board/jungle/boards/board_v2.h" -void detect_board_type(void) { - hw_type = HW_TYPE_V2; - current_board = &board_v2; -} +void detect_board_type(void); \ No newline at end of file diff --git a/board/main_declarations.h b/board/main_declarations.h index dcc7932be26..6ff9aefa166 100644 --- a/board/main_declarations.h +++ b/board/main_declarations.h @@ -14,7 +14,6 @@ void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage); // ********************* Globals ********************** #include "globals.h" -extern uint32_t uptime_cnt; // heartbeat state extern uint32_t heartbeat_counter; diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h index d54296945a6..be2aa7b12be 100644 --- a/board/stm32h7/stm32h7_config.h +++ b/board/stm32h7/stm32h7_config.h @@ -72,7 +72,6 @@ separate IRQs for RX and TX. #endif #ifdef PANDA_JUNGLE -#error NOT DOING THIS BOY #include "board/jungle/stm32h7/board.h" #elif defined(PANDA_BODY) #include "board/boards/boot_state.h" From 6500188a1b5d0d9293453431e25c34ac1152d859 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 00:26:02 +0100 Subject: [PATCH 06/29] Progress --- SConscript | 58 +++---- board/boards/board_declarations.h | 59 ------- board/boards/cuatro.h | 2 +- board/boards/red.c | 1 - board/boards/red.h | 2 +- board/boards/tres.h | 2 +- board/boards/unused_funcs.c | 2 + board/boards/unused_funcs.h | 4 +- board/body/boards/board_declarations.h | 43 ----- board/body/main.c | 5 - board/body/stm32h7/board.h | 8 +- board/bootstub.c | 5 - board/globals.c | 5 + board/globals.h | 2 +- board/jungle/main.c | 3 - board/main.c | 4 +- board/main_declarations.h | 2 +- board/power_saving.c | 4 - board/stm32h7/board.h | 2 +- board/stm32h7/llfdcan.h | 230 +------------------------ board/stm32h7/stm32h7_config.h | 6 - tests/libpanda/SConscript | 31 ---- tests/libpanda/panda.c | 2 - 23 files changed, 43 insertions(+), 439 deletions(-) delete mode 100644 board/boards/board_declarations.h delete mode 100644 board/body/boards/board_declarations.h create mode 100644 board/globals.c diff --git a/SConscript b/SConscript index a776a26cd13..9e4326276dc 100644 --- a/SConscript +++ b/SConscript @@ -60,7 +60,7 @@ def to_c_uint32(x): return "{" + 'U,'.join(map(str, nums)) + "U}" -def build_project(project_name, project, main, extra_flags): +def build_project(project_name, project, main, shared, extra_flags): project_dir = Dir(f'./board/obj/{project_name}/') flags = project["FLAGS"] + extra_flags + common_flags + [ @@ -100,13 +100,12 @@ def build_project(project_name, project, main, extra_flags): startup = env.Object(project["STARTUP_FILE"]) - shared = [ + shared += [ "./crypto/rsa.c", "./crypto/sha.c", "./board/libc.c", "./board/crc.c", "./board/early_init.c", - # "./board/boards/red.c", "./board/critical.c", "./board/drivers/led.c", "./board/drivers/pwm.c", @@ -132,32 +131,9 @@ def build_project(project_name, project, main, extra_flags): "./board/faults.c", "./board/boards/unused_funcs.c", "./board/utils.c", + "./board/globals.c", ] - if "-DPANDA_JUNGLE" in flags: - shared += [ - "board/jungle/stm32h7/board.c", - "board/jungle/boards/board_v2.c" - ] - - if "-DPANDA" in flags: - shared += [ - "board/stm32h7/board.c", - "board/boards/tres.c", - "board/boards/red.c", - "board/boards/cuatro.c", - ] - - if "-DPANDA_BODY" in flags: - shared += [ - "board/body/boards/board_body.c", - "board/body/stm32h7/board.c", - "board/body/motor_control.c", - "board/body/motor_encoder.c" - ] - # board/body/boards/board_body.c - # # "./board/main_definitions.c", - # Build bootstub bs_env = env.Clone() bs_obj_dir = Dir(f'./board/obj/{project_name}/bootstub') @@ -186,9 +162,7 @@ def build_project(project_name, project, main, extra_flags): "./board/main_comms.c", "./board/drivers/bootkick.c", "./board/stm32h7/llfan.c", - # "./board/stm32h7/llusb.c", "./board/stm32h7/lluart.c", - # "./board/stm32h7/peripherals.c", main, ] + shared, LINKFLAGS=[f"-Wl,--section-start,.isr_vector={project['APP_START_ADDRESS']}"] + flags) main_bin = env.Objcopy(f"{project_dir}/main.bin", main_elf) @@ -223,13 +197,13 @@ certs = [get_key_header(n) for n in ["debug", "release"]] with open("board/obj/cert.h", "w") as f: for cert in certs: f.write("\n".join(cert) + "\n") - -# panda fw -panda_c = [ - "./board/main.c", +shared = [ + "board/stm32h7/board.c", + "board/boards/tres.c", + "board/boards/red.c", + "board/boards/cuatro.c", ] - -build_project("panda_h7", base_project_h7, panda_c, ["-DPANDA"]) +build_project("panda_h7", base_project_h7, "./board/main.c", shared, ["-DPANDA"]) # # panda jungle fw flags = [ @@ -238,14 +212,24 @@ flags = [ if os.getenv("FINAL_PROVISIONING"): flags += ["-DFINAL_PROVISIONING"] -build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", flags) +shared = [ + "board/jungle/stm32h7/board.c", + "board/jungle/boards/board_v2.c" +] +build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", shared, flags) # # body fw body_c = [ "./board/body/main.c", ] -build_project("body_h7", base_project_h7, body_c, ["-DPANDA_BODY"]) +shared = [ + "board/body/boards/board_body.c", + "board/body/stm32h7/board.c", + "board/body/motor_control.c", + "board/body/motor_encoder.c" +] +build_project("body_h7", base_project_h7, body_c, shared, ["-DPANDA_BODY"]) # test files if GetOption('extras'): diff --git a/board/boards/board_declarations.h b/board/boards/board_declarations.h deleted file mode 100644 index 58f7f15cfdb..00000000000 --- a/board/boards/board_declarations.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once - -#include "board/board_struct.h" - -// #include -// #include - -// // ******************** Prototypes ******************** - -// #include "boot_state.h" -// #include "board/board_forward.h" - -// typedef void (*board_init)(void); -// typedef void (*board_init_bootloader)(void); -// typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); -// typedef void (*board_set_can_mode)(uint8_t mode); -// typedef uint32_t (*board_read_voltage_mV)(void); -// typedef uint32_t (*board_read_current_mA)(void); -// typedef void (*board_set_ir_power)(uint8_t percentage); -// typedef void (*board_set_fan_enabled)(bool enabled); -// typedef void (*board_set_siren)(bool enabled); -// typedef void (*board_set_bootkick)(BootState state); -// typedef bool (*board_read_som_gpio)(void); -// typedef void (*board_set_amp_enabled)(bool enabled); - -// // #include "board/drivers/harness_declarations.h" -// #include "board/config.h" - -// struct harness_configuration; - -// typedef struct board { -// struct harness_configuration *harness_config; -// GPIO_TypeDef * const led_GPIO[3]; -// const uint8_t led_pin[3]; -// const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM -// const bool has_spi; -// const bool has_fan; -// const uint16_t avdd_mV; -// const uint8_t fan_enable_cooldown_time; -// board_init init; -// board_init_bootloader init_bootloader; -// board_enable_can_transceiver enable_can_transceiver; -// board_set_can_mode set_can_mode; -// board_read_voltage_mV read_voltage_mV; -// board_read_current_mA read_current_mA; -// board_set_ir_power set_ir_power; -// board_set_fan_enabled set_fan_enabled; -// board_set_siren set_siren; -// board_set_bootkick set_bootkick; -// board_read_som_gpio read_som_gpio; -// board_set_amp_enabled set_amp_enabled; -// } board; - -// // ******************* Definitions ******************** -// // These should match the enums in cereal/log.capnp and __init__.py - -// extern struct board board_tres; -// extern struct board board_cuatro; -// extern struct board board_red; diff --git a/board/boards/cuatro.h b/board/boards/cuatro.h index 58eb32e1cf5..79a39033b86 100644 --- a/board/boards/cuatro.h +++ b/board/boards/cuatro.h @@ -1,6 +1,6 @@ #pragma once -#include "board_declarations.h" +#include "board/board_struct.h" // ////////////////////////// // // Cuatro (STM32H7) + Harness // diff --git a/board/boards/red.c b/board/boards/red.c index f2719320c2b..6b69350f9d3 100644 --- a/board/boards/red.c +++ b/board/boards/red.c @@ -1,6 +1,5 @@ #include "red.h" -#include "board/boards/board_declarations.h" #include "board/boards/unused_funcs.h" #include "board/board_struct.h" diff --git a/board/boards/red.h b/board/boards/red.h index a143cc9f994..e00e163b107 100644 --- a/board/boards/red.h +++ b/board/boards/red.h @@ -1,6 +1,6 @@ #pragma once -#include "board_declarations.h" +// #include "board_declarations.h" #include "board/drivers/harness.h" // ///////////////////////////// // diff --git a/board/boards/tres.h b/board/boards/tres.h index d6b7c687cbb..73f90e379cc 100644 --- a/board/boards/tres.h +++ b/board/boards/tres.h @@ -1,6 +1,6 @@ #pragma once -#include "board_declarations.h" +#include "board/board_struct.h" // /////////////////////////// // Tres (STM32H7) + Harness // diff --git a/board/boards/unused_funcs.c b/board/boards/unused_funcs.c index 27d43cef512..d3eb2648c00 100644 --- a/board/boards/unused_funcs.c +++ b/board/boards/unused_funcs.c @@ -1,5 +1,7 @@ #include "unused_funcs.h" +#include "board/utils.h" + void unused_init_bootloader(void) { } diff --git a/board/boards/unused_funcs.h b/board/boards/unused_funcs.h index cbb30eb7e70..50ca5aae62f 100644 --- a/board/boards/unused_funcs.h +++ b/board/boards/unused_funcs.h @@ -1,6 +1,8 @@ #pragma once -#include "board_declarations.h" +#include +#include + #include "board/boards/boot_state.h" void unused_init_bootloader(void); diff --git a/board/body/boards/board_declarations.h b/board/body/boards/board_declarations.h deleted file mode 100644 index 96cd604b526..00000000000 --- a/board/body/boards/board_declarations.h +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include "board/board_struct.h" - -// #pragma once - -// #include -// #include - -// #include "board/board_forward.h" - -// // ******************** Prototypes ******************** -// typedef void (*board_init)(void); -// typedef void (*board_init_bootloader)(void); -// typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); - -// typedef struct board { -// struct harness_configuration *harness_config; -// GPIO_TypeDef * const led_GPIO[3]; -// const uint8_t led_pin[3]; -// const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM -// const bool has_spi; -// const bool has_fan; -// const uint16_t avdd_mV; -// const uint8_t fan_enable_cooldown_time; -// board_init init; -// board_init_bootloader init_bootloader; -// board_enable_can_transceiver enable_can_transceiver; -// board_set_can_mode set_can_mode; -// board_read_voltage_mV read_voltage_mV; -// board_read_current_mA read_current_mA; -// board_set_ir_power set_ir_power; -// board_set_fan_enabled set_fan_enabled; -// board_set_siren set_siren; -// board_set_bootkick set_bootkick; -// board_read_som_gpio read_som_gpio; -// board_set_amp_enabled set_amp_enabled; -// } board; - - - -// // ******************* Definitions ******************** -// #define HW_TYPE_BODY 0xB1U diff --git a/board/body/main.c b/board/body/main.c index 7141a731d15..68ed46944b1 100644 --- a/board/body/main.c +++ b/board/body/main.c @@ -21,11 +21,6 @@ extern int _app_start[0xc000]; #include "board/body/main_comms.h" -// TODO -uint8_t hw_type; -board *current_board; -struct harness_t harness; - void debug_ring_callback(uart_ring *ring) { char rcv; while (get_char(ring, &rcv)) { diff --git a/board/body/stm32h7/board.h b/board/body/stm32h7/board.h index 9d07845b68c..574f08ef450 100644 --- a/board/body/stm32h7/board.h +++ b/board/body/stm32h7/board.h @@ -1,10 +1,8 @@ #pragma once -// #include "board/body/boards/board_declarations.h" -// #include "board/body/boards/board_body.h" - -// extern board *current_board; -// extern uint8_t hw_type; +#ifdef HW_TYPE_BODY +#error HW_TYPE_BODY already defined! +#endif // ******************* Definitions ******************** #define HW_TYPE_BODY 0xB1U diff --git a/board/bootstub.c b/board/bootstub.c index ee2552c83c6..77df93d8e41 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -25,11 +25,6 @@ #include "globals.h" -// TODO -uint8_t hw_type; -board *current_board; -struct harness_t harness; - // cppcheck-suppress unusedFunction ; used in headers not included in cppcheck void __initialize_hardware_early(void) { early_initialization(); diff --git a/board/globals.c b/board/globals.c new file mode 100644 index 00000000000..f9012cb2f80 --- /dev/null +++ b/board/globals.c @@ -0,0 +1,5 @@ +#include "globals.h" + +uint8_t hw_type; +board *current_board; +struct harness_t harness; \ No newline at end of file diff --git a/board/globals.h b/board/globals.h index 4ccb31b17d2..ea640a9be8c 100644 --- a/board/globals.h +++ b/board/globals.h @@ -2,7 +2,7 @@ #include -#include "boards/board_declarations.h" +#include "board/board_struct.h" #include "board/drivers/harness.h" typedef struct board board; diff --git a/board/jungle/main.c b/board/jungle/main.c index 7473f79f737..74ac169ef3e 100644 --- a/board/jungle/main.c +++ b/board/jungle/main.c @@ -22,9 +22,6 @@ #include "board/can_comms.h" #include "board/jungle/main_comms.h" -uint8_t hw_type; -board *current_board; -struct harness_t harness; uint32_t uptime_cnt = 0; // ********************* Serial debugging ********************* diff --git a/board/main.c b/board/main.c index 5aff52dcc21..c74d995fe5b 100644 --- a/board/main.c +++ b/board/main.c @@ -28,9 +28,7 @@ #include "board/main_comms.h" #include "board/main_declarations.h" -uint8_t hw_type; -board *current_board; -struct harness_t harness; + // TODO uint32_t heartbeat_counter; bool heartbeat_lost; diff --git a/board/main_declarations.h b/board/main_declarations.h index 6ff9aefa166..f6beb302249 100644 --- a/board/main_declarations.h +++ b/board/main_declarations.h @@ -3,7 +3,7 @@ #include #include -#include "boards/board_declarations.h" +#include "board/board_struct.h" // ******************** Prototypes ******************** diff --git a/board/power_saving.c b/board/power_saving.c index a088e2fb3b3..94d2c4c3499 100644 --- a/board/power_saving.c +++ b/board/power_saving.c @@ -7,10 +7,6 @@ // WARNING: To stay in compliance with the SIL2 rules laid out in STM UM1840, we should never implement any of the available hardware low power modes. // See rule: CoU_3 -// FIXME! -void llcan_irq_disable(const FDCAN_GlobalTypeDef *FDCANx); -void llcan_irq_enable(const FDCAN_GlobalTypeDef *FDCANx); - int power_save_status = POWER_SAVE_STATUS_DISABLED; void enable_can_transceivers(bool enabled) { diff --git a/board/stm32h7/board.h b/board/stm32h7/board.h index 43d73610547..73ea1cde475 100644 --- a/board/stm32h7/board.h +++ b/board/stm32h7/board.h @@ -1,7 +1,7 @@ // ///////////////////////////////////////////////////////////// // // Hardware abstraction layer for all different supported boards // // ///////////////////////////////////////////////////////////// // -#include "board/boards/board_declarations.h" +// #include "board/boards/board_declarations.h" #include "board/boards/unused_funcs.h" // ///// Board definition and detection ///// // diff --git a/board/stm32h7/llfdcan.h b/board/stm32h7/llfdcan.h index ebd53c0de45..2df45f56985 100644 --- a/board/stm32h7/llfdcan.h +++ b/board/stm32h7/llfdcan.h @@ -57,231 +57,5 @@ void llcan_irq_enable(const FDCAN_GlobalTypeDef *FDCANx); bool llcan_init(FDCAN_GlobalTypeDef *FDCANx); void llcan_clear_send(FDCAN_GlobalTypeDef *FDCANx); - -/* -// kbps multiplied by 10 -const uint32_t speeds[SPEEDS_ARRAY_SIZE] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U}; -const uint32_t data_speeds[DATA_SPEEDS_ARRAY_SIZE] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U, 20000U, 50000U}; - -static bool fdcan_request_init(FDCAN_GlobalTypeDef *FDCANx) { - bool ret = true; - // Exit from sleep mode - FDCANx->CCCR &= ~(FDCAN_CCCR_CSR); - while ((FDCANx->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA); - - // Request init - uint32_t timeout_counter = 0U; - FDCANx->CCCR |= FDCAN_CCCR_INIT; - while ((FDCANx->CCCR & FDCAN_CCCR_INIT) == 0U) { - // Delay for about 1ms - delay(10000); - timeout_counter++; - - if (timeout_counter >= CAN_INIT_TIMEOUT_MS){ - ret = false; - break; - } - } - return ret; -} - -static bool fdcan_exit_init(FDCAN_GlobalTypeDef *FDCANx) { - bool ret = true; - - FDCANx->CCCR &= ~(FDCAN_CCCR_INIT); - uint32_t timeout_counter = 0U; - while ((FDCANx->CCCR & FDCAN_CCCR_INIT) != 0U) { - // Delay for about 1ms - delay(10000); - timeout_counter++; - - if (timeout_counter >= CAN_INIT_TIMEOUT_MS) { - ret = false; - break; - } - } - return ret; -} - -bool llcan_set_speed(FDCAN_GlobalTypeDef *FDCANx, uint32_t speed, uint32_t data_speed, bool non_iso, bool loopback, bool silent) { - UNUSED(speed); - bool ret = fdcan_request_init(FDCANx); - - if (ret) { - // Enable config change - FDCANx->CCCR |= FDCAN_CCCR_CCE; - - //Reset operation mode to Normal - FDCANx->CCCR &= ~(FDCAN_CCCR_TEST); - FDCANx->TEST &= ~(FDCAN_TEST_LBCK); - FDCANx->CCCR &= ~(FDCAN_CCCR_MON); - FDCANx->CCCR &= ~(FDCAN_CCCR_ASM); - FDCANx->CCCR &= ~(FDCAN_CCCR_NISO); - - // TODO: add as a separate safety mode - // Enable ASM restricted operation(for debug or automatic bitrate switching) - //FDCANx->CCCR |= FDCAN_CCCR_ASM; - - uint8_t prescaler = BITRATE_PRESCALER; - if (speed < 2500U) { - // The only way to support speeds lower than 250Kbit/s (down to 10Kbit/s) - prescaler = BITRATE_PRESCALER * 16U; - } - - // Set the nominal bit timing values - uint32_t tq = CAN_QUANTA(speed, prescaler); - uint32_t sp = CAN_SP_NOMINAL; - uint32_t seg1 = CAN_SEG1(tq, sp); - uint32_t seg2 = CAN_SEG2(tq, sp); - uint8_t sjw = MIN(127U, seg2); - - FDCANx->NBTP = (((sjw & 0x7FUL)-1U)<DBTP = (((sjw & 0xFUL)-1U)<CCCR |= FDCAN_CCCR_NISO; - } - - // Silent loopback is known as internal loopback in the docs - if (loopback) { - FDCANx->CCCR |= FDCAN_CCCR_TEST; - FDCANx->TEST |= FDCAN_TEST_LBCK; - FDCANx->CCCR |= FDCAN_CCCR_MON; - } - // Silent is known as bus monitoring in the docs - if (silent) { - FDCANx->CCCR |= FDCAN_CCCR_MON; - } - ret = fdcan_exit_init(FDCANx); - if (!ret) { - print(CAN_NAME_FROM_CANIF(FDCANx)); print(" set_speed timed out! (2)\n"); - } - } else { - print(CAN_NAME_FROM_CANIF(FDCANx)); print(" set_speed timed out! (1)\n"); - } - return ret; -} - -void llcan_irq_disable(const FDCAN_GlobalTypeDef *FDCANx) { - if (FDCANx == FDCAN1) { - NVIC_DisableIRQ(FDCAN1_IT0_IRQn); - NVIC_DisableIRQ(FDCAN1_IT1_IRQn); - } else if (FDCANx == FDCAN2) { - NVIC_DisableIRQ(FDCAN2_IT0_IRQn); - NVIC_DisableIRQ(FDCAN2_IT1_IRQn); - } else if (FDCANx == FDCAN3) { - NVIC_DisableIRQ(FDCAN3_IT0_IRQn); - NVIC_DisableIRQ(FDCAN3_IT1_IRQn); - } else { - } -} - -void llcan_irq_enable(const FDCAN_GlobalTypeDef *FDCANx) { - if (FDCANx == FDCAN1) { - NVIC_EnableIRQ(FDCAN1_IT0_IRQn); - NVIC_EnableIRQ(FDCAN1_IT1_IRQn); - } else if (FDCANx == FDCAN2) { - NVIC_EnableIRQ(FDCAN2_IT0_IRQn); - NVIC_EnableIRQ(FDCAN2_IT1_IRQn); - } else if (FDCANx == FDCAN3) { - NVIC_EnableIRQ(FDCAN3_IT0_IRQn); - NVIC_EnableIRQ(FDCAN3_IT1_IRQn); - } else { - } -} - -bool llcan_init(FDCAN_GlobalTypeDef *FDCANx) { - uint32_t can_number = CAN_NUM_FROM_CANIF(FDCANx); - bool ret = fdcan_request_init(FDCANx); - - if (ret) { - // Enable config change - FDCANx->CCCR |= FDCAN_CCCR_CCE; - // Enable automatic retransmission - FDCANx->CCCR &= ~(FDCAN_CCCR_DAR); - // Enable transmission pause feature - FDCANx->CCCR |= FDCAN_CCCR_TXP; - // Disable protocol exception handling - FDCANx->CCCR |= FDCAN_CCCR_PXHD; - // FD with BRS - FDCANx->CCCR |= (FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE); - - // Set TX mode to FIFO - FDCANx->TXBC &= ~(FDCAN_TXBC_TFQM); - // Configure TX element data size - FDCANx->TXESC |= 0x7U << FDCAN_TXESC_TBDS_Pos; // 64 bytes - //Configure RX FIFO0 element data size - FDCANx->RXESC |= 0x7U << FDCAN_RXESC_F0DS_Pos; - // Disable filtering, accept all valid frames received - FDCANx->XIDFC &= ~(FDCAN_XIDFC_LSE); // No extended filters - FDCANx->SIDFC &= ~(FDCAN_SIDFC_LSS); // No standard filters - FDCANx->GFC &= ~(FDCAN_GFC_RRFE); // Accept extended remote frames - FDCANx->GFC &= ~(FDCAN_GFC_RRFS); // Accept standard remote frames - FDCANx->GFC &= ~(FDCAN_GFC_ANFE); // Accept extended frames to FIFO 0 - FDCANx->GFC &= ~(FDCAN_GFC_ANFS); // Accept standard frames to FIFO 0 - - uint32_t RxFIFO0SA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET); - uint32_t TxFIFOSA = RxFIFO0SA + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE); - - // RX FIFO 0 - FDCANx->RXF0C |= (FDCAN_RX_FIFO_0_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_RXF0C_F0SA_Pos; - FDCANx->RXF0C |= FDCAN_RX_FIFO_0_EL_CNT << FDCAN_RXF0C_F0S_Pos; - // RX FIFO 0 switch to non-blocking (overwrite) mode - FDCANx->RXF0C |= FDCAN_RXF0C_F0OM; - - // TX FIFO (mode set earlier) - FDCANx->TXBC |= (FDCAN_TX_FIFO_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_TXBC_TBSA_Pos; - FDCANx->TXBC |= FDCAN_TX_FIFO_EL_CNT << FDCAN_TXBC_TFQS_Pos; - - // Flush allocated RAM - uint32_t EndAddress = TxFIFOSA + (FDCAN_TX_FIFO_EL_CNT * FDCAN_TX_FIFO_EL_SIZE); - for (uint32_t RAMcounter = RxFIFO0SA; RAMcounter < EndAddress; RAMcounter += 4U) { - *(uint32_t *)(RAMcounter) = 0x00000000; - } - - // Enable both interrupts for each module - FDCANx->ILE = (FDCAN_ILE_EINT0 | FDCAN_ILE_EINT1); - - FDCANx->IE &= 0x0U; // Reset all interrupts - // Messages for INT0 - FDCANx->IE |= FDCAN_IE_RF0NE; // Rx FIFO 0 new message - FDCANx->IE |= FDCAN_IE_PEDE | FDCAN_IE_PEAE | FDCAN_IE_BOE | FDCAN_IE_EPE | FDCAN_IE_RF0LE; - - // Messages for INT1 (Only TFE works??) - FDCANx->ILS |= FDCAN_ILS_TFEL; - FDCANx->IE |= FDCAN_IE_TFEE; // Tx FIFO empty - - ret = fdcan_exit_init(FDCANx); - if(!ret) { - print(CAN_NAME_FROM_CANIF(FDCANx)); print(" llcan_init timed out (2)!\n"); - } - - llcan_irq_enable(FDCANx); - - } else { - print(CAN_NAME_FROM_CANIF(FDCANx)); print(" llcan_init timed out (1)!\n"); - } - return ret; -} - -void llcan_clear_send(FDCAN_GlobalTypeDef *FDCANx) { - // from datasheet: "Transmit cancellation is not intended for Tx FIFO operation." - // so we need to clear pending transmission manually by resetting FDCAN core - FDCANx->IR |= 0x3FCFFFFFU; // clear all interrupts - bool ret = llcan_init(FDCANx); - UNUSED(ret); -} -*/ \ No newline at end of file +void llcan_irq_disable(const FDCAN_GlobalTypeDef *FDCANx); +void llcan_irq_enable(const FDCAN_GlobalTypeDef *FDCANx); diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h index be2aa7b12be..23e0172a864 100644 --- a/board/stm32h7/stm32h7_config.h +++ b/board/stm32h7/stm32h7_config.h @@ -48,12 +48,6 @@ separate IRQs for RX and TX. #include "board/can.h" #include "board/comms_definitions.h" -// #ifndef BOOTSTUB -// #include "board/main_declarations.h" -// #else -// #include "board/bootstub_declarations.h" -// #endif - #include "board/libc.h" #include "board/critical.h" #include "board/faults.h" diff --git a/tests/libpanda/SConscript b/tests/libpanda/SConscript index ca22cf89e3f..b37da4e3e65 100644 --- a/tests/libpanda/SConscript +++ b/tests/libpanda/SConscript @@ -41,37 +41,6 @@ shared = [ "./board/can_comms.c", "./board/drivers/can_common.c", "./board/fake_stm.c", - - # "./board/libc.c", - # "./board/crc.c", - # "./board/early_init.c", - # "./board/stm32h7/board.c", - # "./board/boards/red.c", - # "./board/critical.c", - # "./board/drivers/led.c", - # "./board/drivers/pwm.c", - # "./board/drivers/gpio.c", - # "./board/drivers/fake_siren.c", - # "./board/stm32h7/lli2c.c", - # "./board/stm32h7/clock.c", - # "./board/drivers/clock_source.c", - # "./board/stm32h7/sound.c", - # "./board/stm32h7/llflash.c", - # "./board/stm32h7/stm32h7_config.c", - # "./board/drivers/registers.c", - # "./board/drivers/interrupts.c", - # "./board/provision.c", - # "./board/stm32h7/peripherals.c", - # "./board/stm32h7/llusb.c", - # "./board/drivers/usb.c", - # "./board/drivers/spi.c", - # "./board/drivers/timers.c", - # "./board/boards/cuatro.c", - # "./board/boards/tres.c", - # "./board/stm32h7/lladc.c", - # "./board/stm32h7/llspi.c", - # "./board/faults.c", - # "./board/boards/unused_funcs.c", "./board/utils.c", ] diff --git a/tests/libpanda/panda.c b/tests/libpanda/panda.c index 22269475ad6..9d46cff9124 100644 --- a/tests/libpanda/panda.c +++ b/tests/libpanda/panda.c @@ -14,9 +14,7 @@ void can_tx_comms_resume_spi(void) { }; #include "health.h" #include "faults.h" #include "libc.h" -#include "boards/board_declarations.h" #include "opendbc/safety/safety.h" -// #include "main_definitions.h" #include "drivers/can_common.h" can_ring *rx_q = &can_rx_q; From 5a142358d868aa8655b1a0321d6367367d5ef0d7 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 01:26:45 +0100 Subject: [PATCH 07/29] Progress --- SConscript | 44 +- board/boards/red.c | 2 + board/body/main_comms.c | 121 +++++ board/body/main_comms.h | 112 +---- board/bootstub.c | 12 +- board/bootstub_declarations.h | 19 - board/config.h | 3 - board/critical.c | 4 +- board/critical.h | 3 +- board/drivers/bootkick_declarations.h | 5 - board/drivers/can_common.h | 15 - board/drivers/harness.h | 1 - board/fake_stm.c | 3 +- board/fake_stm.h | 2 - board/jungle/main_comms.c | 275 +++++++++++ board/jungle/main_comms.h | 262 +--------- board/main.c | 376 +-------------- board/main_comms.c | 663 +++++++++++++------------- board/main_comms.h | 2 +- board/main_declarations.h | 16 +- board/main_definitions.c | 8 +- board/power_saving.c | 8 +- board/stm32h7/stm32h7_config.h | 3 +- tests/misra/checkers.txt | 2 +- tests/misra/test_misra.sh | 2 +- 25 files changed, 812 insertions(+), 1151 deletions(-) create mode 100644 board/body/main_comms.c delete mode 100644 board/bootstub_declarations.h delete mode 100644 board/drivers/bootkick_declarations.h create mode 100644 board/jungle/main_comms.c diff --git a/SConscript b/SConscript index 9e4326276dc..fb7bcc174f4 100644 --- a/SConscript +++ b/SConscript @@ -90,7 +90,7 @@ def build_project(project_name, project, main, shared, extra_flags): CFLAGS=flags, ASFLAGS=flags, LINKFLAGS=flags, - CPPPATH=[Dir("./"), "./board/stm32h7/inc", opendbc.INCLUDE_PATH], + CPPPATH=[Dir(""), "board/stm32h7/inc", opendbc.INCLUDE_PATH], ASCOM="$AS $ASFLAGS -o $TARGET -c $SOURCES", BUILDERS={ 'Objcopy': Builder(generator=objcopy, suffix='.bin', src_suffix='.elf') @@ -125,14 +125,15 @@ def build_project(project_name, project, main, shared, extra_flags): "./board/drivers/usb.c", "./board/drivers/spi.c", "./board/drivers/timers.c", - "./board/stm32h7/lladc.c", "./board/stm32h7/llspi.c", "./board/faults.c", "./board/boards/unused_funcs.c", "./board/utils.c", "./board/globals.c", + ] + # Build bootstub bs_env = env.Clone() @@ -145,35 +146,36 @@ def build_project(project_name, project, main, shared, extra_flags): "./board/bootstub.c", "./board/flasher.c", ] + shared) - bs_env.Objcopy(f"./board/obj/bootstub.{project_name}.bin", bs_elf) + bs_env.Objcopy(f"board/obj/bootstub.{project_name}.bin", bs_elf) # Build + sign main (aka app) main_elf = env.Program(f"{project_dir}/main.elf", [ startup, "./board/can_comms.c", "./board/drivers/fan.c", - "./board/drivers/can_common.c", + "./board/power_saving.c", "./board/drivers/uart.c", - "./board/drivers/fdcan.c", + "./board/stm32h7/llfdcan.c", "./board/drivers/harness.c", "./board/drivers/simple_watchdog.c", - "./board/main_comms.c", "./board/drivers/bootkick.c", "./board/stm32h7/llfan.c", "./board/stm32h7/lluart.c", + "./board/drivers/fdcan.c", + "./board/drivers/can_common.c", main, ] + shared, LINKFLAGS=[f"-Wl,--section-start,.isr_vector={project['APP_START_ADDRESS']}"] + flags) main_bin = env.Objcopy(f"{project_dir}/main.bin", main_elf) - sign_py = File(f"./crypto/sign.py").srcnode().relpath - env.Command(f"./board/obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}") + sign_py = File(f"crypto/sign.py").srcnode().relpath + env.Command(f"board/obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}") base_project_h7 = { - "STARTUP_FILE": "./board/stm32h7/startup_stm32h7x5xx.s", - "LINKER_SCRIPT": "./board/stm32h7/stm32h7x5_flash.ld", + "STARTUP_FILE": "board/stm32h7/startup_stm32h7x5xx.s", + "LINKER_SCRIPT": "board/stm32h7/stm32h7x5_flash.ld", "APP_START_ADDRESS": "0x8020000", "FLAGS": [ "-mcpu=cortex-m7", @@ -197,13 +199,21 @@ certs = [get_key_header(n) for n in ["debug", "release"]] with open("board/obj/cert.h", "w") as f: for cert in certs: f.write("\n".join(cert) + "\n") + shared = [ "board/stm32h7/board.c", "board/boards/tres.c", "board/boards/red.c", "board/boards/cuatro.c", + "board/main_definitions.c" +] + +project = [ + "board/main_comms.c", + "board/main.c" ] -build_project("panda_h7", base_project_h7, "./board/main.c", shared, ["-DPANDA"]) + +build_project("panda_h7", base_project_h7, project, shared, ["-DPANDA"]) # # panda jungle fw flags = [ @@ -214,13 +224,18 @@ if os.getenv("FINAL_PROVISIONING"): shared = [ "board/jungle/stm32h7/board.c", - "board/jungle/boards/board_v2.c" + "board/jungle/boards/board_v2.c", +] +project = [ + "board/jungle/main_comms.c", + "board/jungle/main.c", ] -build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", shared, flags) +build_project("panda_jungle_h7", base_project_h7, project, shared, flags) # # body fw body_c = [ - "./board/body/main.c", + "board/body/main.c", + "board/body/main_comms.c", ] shared = [ @@ -229,6 +244,7 @@ shared = [ "board/body/motor_control.c", "board/body/motor_encoder.c" ] + build_project("body_h7", base_project_h7, body_c, shared, ["-DPANDA_BODY"]) # test files diff --git a/board/boards/red.c b/board/boards/red.c index 6b69350f9d3..4a4183020d0 100644 --- a/board/boards/red.c +++ b/board/boards/red.c @@ -3,6 +3,8 @@ #include "board/boards/unused_funcs.h" #include "board/board_struct.h" +#include "board/globals.h" + // ///////////////////////////// // // Red Panda (STM32H7) + Harness // // ///////////////////////////// // diff --git a/board/body/main_comms.c b/board/body/main_comms.c new file mode 100644 index 00000000000..3dce519198e --- /dev/null +++ b/board/body/main_comms.c @@ -0,0 +1,121 @@ +#include "board/body/main_comms.h" + +#include "board/globals.h" +#include "board/config.h" +#include "main_comms.h" +#include "board/health.h" +#include "board/provision.h" +#include "board/early_init.h" +#include "board/obj/gitversion.h" +#include "board/stm32h7/llfdcan.h" +#include "board/libc.h" +#include "board/drivers/uart.h" +#include "board/body/motor_control.h" + +extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used + +void comms_endpoint2_write(const uint8_t *data, uint32_t len) { + UNUSED(data); + UNUSED(len); +} + +int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { + unsigned int resp_len = 0; + + switch (req->request) { + // **** 0xc1: get hardware type + case 0xc1: + resp[0] = hw_type; + resp_len = 1; + break; + + // **** 0xd1: enter bootloader mode + case 0xd1: + switch (req->param1) { + case 0: + print("-> entering bootloader\n"); + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + NVIC_SystemReset(); + break; + case 1: + print("-> entering softloader\n"); + enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; + NVIC_SystemReset(); + break; + default: + print("Bootloader mode invalid\n"); + break; + } + break; + + // **** 0xd3/0xd4: signature bytes + case 0xd3: + case 0xd4: { + uint8_t offset = (req->request == 0xd3) ? 0U : 64U; + resp_len = 64U; + char *code = (char *)_app_start; + int code_len = _app_start[0]; + (void)memcpy(resp, &code[code_len + offset], resp_len); + break; + } + + // **** 0xd6: get version + case 0xd6: + (void)memcpy(resp, gitversion, sizeof(gitversion)); + resp_len = sizeof(gitversion) - 1U; + break; + + // **** 0xd8: reset ST + case 0xd8: + NVIC_SystemReset(); + break; + + // **** 0xe0: set motor speed + case 0xe0: + motor_set_speed((uint8_t)req->param1, (int8_t)req->param2); + break; + + // **** 0xe1: stop motor + case 0xe1: + motor_set_speed((uint8_t)req->param1, 0); + break; + + // **** 0xe2: get motor encoder state + case 0xe2: { + uint8_t motor = (uint8_t)req->param1; + int32_t position = motor_encoder_get_position(motor); + float rpm = motor_encoder_get_speed_rpm(motor); + int32_t rpm_milli = (int32_t)(rpm * 1000.0f); + (void)memcpy(resp, &position, sizeof(position)); + (void)memcpy(resp + sizeof(position), &rpm_milli, sizeof(rpm_milli)); + resp_len = (uint8_t)(sizeof(position) + sizeof(rpm_milli)); + break; + } + + // **** 0xe3: reset encoder position + case 0xe3: + motor_encoder_reset((uint8_t)req->param1); + break; + + // **** 0xe4: set motor target speed (rpm * 0.1) + case 0xe4: { + uint8_t motor = (uint8_t)req->param1; + float target_rpm = ((int16_t)req->param2) * 0.1f; + motor_speed_controller_set_target_rpm(motor, target_rpm); + break; + } + + // **** 0xdd: get healthpacket and CANPacket versions + case 0xdd: + resp[0] = HEALTH_PACKET_VERSION; + resp[1] = CAN_PACKET_VERSION; + resp[2] = CAN_HEALTH_PACKET_VERSION; + resp_len = 3U; + break; + + default: + // Ignore unhandled requests + break; + } + return resp_len; +} diff --git a/board/body/main_comms.h b/board/body/main_comms.h index 1eeaf024aaa..eb08baa1922 100644 --- a/board/body/main_comms.h +++ b/board/body/main_comms.h @@ -1,109 +1,7 @@ -#include "board/globals.h" - -// TODO ME! - -void comms_endpoint2_write(const uint8_t *data, uint32_t len) { - UNUSED(data); - UNUSED(len); -} - -int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { - unsigned int resp_len = 0; - - switch (req->request) { - // **** 0xc1: get hardware type - case 0xc1: - resp[0] = hw_type; - resp_len = 1; - break; - - // **** 0xd1: enter bootloader mode - case 0xd1: - switch (req->param1) { - case 0: - print("-> entering bootloader\n"); - enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; - NVIC_SystemReset(); - break; - case 1: - print("-> entering softloader\n"); - enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; - NVIC_SystemReset(); - break; - default: - print("Bootloader mode invalid\n"); - break; - } - break; - - // **** 0xd3/0xd4: signature bytes - case 0xd3: - case 0xd4: { - uint8_t offset = (req->request == 0xd3) ? 0U : 64U; - resp_len = 64U; - char *code = (char *)_app_start; - int code_len = _app_start[0]; - (void)memcpy(resp, &code[code_len + offset], resp_len); - break; - } - - // **** 0xd6: get version - case 0xd6: - (void)memcpy(resp, gitversion, sizeof(gitversion)); - resp_len = sizeof(gitversion) - 1U; - break; +#pragma once - // **** 0xd8: reset ST - case 0xd8: - NVIC_SystemReset(); - break; - - // **** 0xe0: set motor speed - case 0xe0: - motor_set_speed((uint8_t)req->param1, (int8_t)req->param2); - break; - - // **** 0xe1: stop motor - case 0xe1: - motor_set_speed((uint8_t)req->param1, 0); - break; - - // **** 0xe2: get motor encoder state - case 0xe2: { - uint8_t motor = (uint8_t)req->param1; - int32_t position = motor_encoder_get_position(motor); - float rpm = motor_encoder_get_speed_rpm(motor); - int32_t rpm_milli = (int32_t)(rpm * 1000.0f); - (void)memcpy(resp, &position, sizeof(position)); - (void)memcpy(resp + sizeof(position), &rpm_milli, sizeof(rpm_milli)); - resp_len = (uint8_t)(sizeof(position) + sizeof(rpm_milli)); - break; - } - - // **** 0xe3: reset encoder position - case 0xe3: - motor_encoder_reset((uint8_t)req->param1); - break; - - // **** 0xe4: set motor target speed (rpm * 0.1) - case 0xe4: { - uint8_t motor = (uint8_t)req->param1; - float target_rpm = ((int16_t)req->param2) * 0.1f; - motor_speed_controller_set_target_rpm(motor, target_rpm); - break; - } - - // **** 0xdd: get healthpacket and CANPacket versions - case 0xdd: - resp[0] = HEALTH_PACKET_VERSION; - resp[1] = CAN_PACKET_VERSION; - resp[2] = CAN_HEALTH_PACKET_VERSION; - resp_len = 3U; - break; +#include "board/globals.h" +#include "board/comms_definitions.h" - default: - // Ignore unhandled requests - break; - } - return resp_len; -} +void comms_endpoint2_write(const uint8_t *data, uint32_t len); +int comms_control_handler(ControlPacket_t *req, uint8_t *resp); \ No newline at end of file diff --git a/board/bootstub.c b/board/bootstub.c index 77df93d8e41..69ad85688e5 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -4,22 +4,22 @@ // ********************* Includes ********************* #include -#include "board/bootstub_declarations.h" +// #include "board/bootstub_declarations.h" -#include "board/config.h" +// #include "board/config.h" -#include "board/drivers/led.h" -#include "board/drivers/pwm.h" +// #include "board/drivers/led.h" +// #include "board/drivers/pwm.h" #include "board/drivers/usb.h" #include "board/early_init.h" -#include "board/provision.h" +// #include "board/provision.h" #include "crypto/rsa.h" #include "crypto/sha.h" #include "board/obj/cert.h" -#include "board/obj/gitversion.h" +// #include "board/obj/gitversion.h" #include "board/flasher.h" #include "globals.h" diff --git a/board/bootstub_declarations.h b/board/bootstub_declarations.h deleted file mode 100644 index fef155970ce..00000000000 --- a/board/bootstub_declarations.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include - -#include "utils.h" -// #include "stm32h7/inc/stm32h7xx.h" -#include "config.h" - -// ******************** Prototypes ******************** - - - -typedef struct harness_configuration harness_configuration; -void pwm_init(TIM_TypeDef *TIM, uint8_t channel); -void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage); - -// ********************* Globals ********************** - -#include "globals.h" \ No newline at end of file diff --git a/board/config.h b/board/config.h index 10c4785cd33..0df9022cf40 100644 --- a/board/config.h +++ b/board/config.h @@ -65,9 +65,6 @@ #error FUCK YOU #endif -// TODO, better spot -void detect_board_type(void); - // platform includes #ifdef STM32H7 #include "board/stm32h7/stm32h7_config.h" diff --git a/board/critical.c b/board/critical.c index 5b12c4ed46e..605645295aa 100644 --- a/board/critical.c +++ b/board/critical.c @@ -1,11 +1,9 @@ #include "critical.h" -#include "config.h" - // ********************* Critical section helpers ********************* uint8_t global_critical_depth = 0U; -static volatile bool interrupts_enabled = false; +volatile bool interrupts_enabled = false; void enable_interrupts(void) { interrupts_enabled = true; diff --git a/board/critical.h b/board/critical.h index 9f6ee59c193..ab32d202add 100644 --- a/board/critical.h +++ b/board/critical.h @@ -22,8 +22,7 @@ extern uint8_t global_critical_depth; } #endif -extern uint8_t global_critical_depth; -static volatile bool interrupts_enabled; +extern volatile bool interrupts_enabled; void enable_interrupts(void); void disable_interrupts(void); diff --git a/board/drivers/bootkick_declarations.h b/board/drivers/bootkick_declarations.h deleted file mode 100644 index 3b55b72ac66..00000000000 --- a/board/drivers/bootkick_declarations.h +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -extern bool bootkick_reset_triggered; - -void bootkick_tick(bool ignition, bool recent_heartbeat); diff --git a/board/drivers/can_common.h b/board/drivers/can_common.h index 3dbd85d7328..209053a9ae2 100644 --- a/board/drivers/can_common.h +++ b/board/drivers/can_common.h @@ -81,21 +81,6 @@ bool can_check_checksum(CANPacket_t *packet); void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook); bool is_speed_valid(uint32_t speed, const uint32_t *all_speeds, uint8_t len); - -extern uint32_t safety_tx_blocked; -extern uint32_t safety_rx_invalid; -extern uint32_t tx_buffer_overflow; -extern uint32_t rx_buffer_overflow; - -extern can_health_t can_health[PANDA_CAN_CNT]; - -// Ignition detected from CAN meessages -extern bool ignition_can; -extern uint32_t ignition_can_cnt; - -extern bool can_silent; -extern bool can_loopback; - // ********************* instantiate queues ********************* #define can_buffer(x, size) \ static CANPacket_t elems_##x[size]; \ diff --git a/board/drivers/harness.h b/board/drivers/harness.h index 1058b345e69..811253ce7b4 100644 --- a/board/drivers/harness.h +++ b/board/drivers/harness.h @@ -18,7 +18,6 @@ struct harness_t { bool relay_driven; bool sbu_adc_lock; }; -extern struct harness_t harness; struct harness_configuration { GPIO_TypeDef * const GPIO_SBU1; diff --git a/board/fake_stm.c b/board/fake_stm.c index 41b38ed3762..d30651d6afc 100644 --- a/board/fake_stm.c +++ b/board/fake_stm.c @@ -1,5 +1,6 @@ #include "fake_stm.h" -#include "utils.h" + +#include void print(const char *a) { printf("%s", a); diff --git a/board/fake_stm.h b/board/fake_stm.h index cda8582661c..bce64f8f183 100644 --- a/board/fake_stm.h +++ b/board/fake_stm.h @@ -1,8 +1,6 @@ #pragma once // minimal code to fake a panda for tests -#include #include -#include #include "utils.h" diff --git a/board/jungle/main_comms.c b/board/jungle/main_comms.c new file mode 100644 index 00000000000..71179cd360e --- /dev/null +++ b/board/jungle/main_comms.c @@ -0,0 +1,275 @@ +#include "board/globals.h" +#include "board/config.h" +#include "main_comms.h" +#include "jungle_health.h" +#include "board/health.h" +#include "board/drivers/fdcan.h" +#include "board/drivers/can_common.h" +#include "board/provision.h" +#include "board/early_init.h" +#include "board/obj/gitversion.h" +#include "board/stm32h7/llfdcan.h" +#include "board/libc.h" +#include "board/drivers/uart.h" + +extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used + +bool generated_can_traffic = false; + +static int get_jungle_health_pkt(void *dat) { + COMPILE_TIME_ASSERT(sizeof(struct jungle_health_t) <= USBPACKET_MAX_SIZE); + struct jungle_health_t * health = (struct jungle_health_t*)dat; + + health->uptime_pkt = uptime_cnt; + health->ch1_power = current_board->get_channel_power(1U); + health->ch2_power = current_board->get_channel_power(2U); + health->ch3_power = current_board->get_channel_power(3U); + health->ch4_power = current_board->get_channel_power(4U); + health->ch5_power = current_board->get_channel_power(5U); + health->ch6_power = current_board->get_channel_power(6U); + + health->ch1_sbu1_mV = current_board->get_sbu_mV(1U, SBU1); + health->ch1_sbu2_mV = current_board->get_sbu_mV(1U, SBU2); + health->ch2_sbu1_mV = current_board->get_sbu_mV(2U, SBU1); + health->ch2_sbu2_mV = current_board->get_sbu_mV(2U, SBU2); + health->ch3_sbu1_mV = current_board->get_sbu_mV(3U, SBU1); + health->ch3_sbu2_mV = current_board->get_sbu_mV(3U, SBU2); + health->ch4_sbu1_mV = current_board->get_sbu_mV(4U, SBU1); + health->ch4_sbu2_mV = current_board->get_sbu_mV(4U, SBU2); + health->ch5_sbu1_mV = current_board->get_sbu_mV(5U, SBU1); + health->ch5_sbu2_mV = current_board->get_sbu_mV(5U, SBU2); + health->ch6_sbu1_mV = current_board->get_sbu_mV(6U, SBU1); + health->ch6_sbu2_mV = current_board->get_sbu_mV(6U, SBU2); + + return sizeof(*health); +} + +// send on serial, first byte to select the ring +void comms_endpoint2_write(const uint8_t *data, uint32_t len) { + UNUSED(data); + UNUSED(len); +} + +int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { + unsigned int resp_len = 0; + uint32_t time; + +#ifdef DEBUG_COMMS + print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n"); + print("- request "); puth(req->request); print("\n"); + print("- param1 "); puth(req->param1); print("\n"); + print("- param2 "); puth(req->param2); print("\n"); +#endif + + switch (req->request) { + // **** 0xa0: Set panda power. + case 0xa0: + current_board->set_panda_power((req->param1 == 1U)); + break; + // **** 0xa1: Set harness orientation. + case 0xa1: + current_board->set_harness_orientation(req->param1); + break; + // **** 0xa2: Set ignition. + case 0xa2: + current_board->set_ignition((req->param1 == 1U)); + break; + // **** 0xa3: Set panda power per channel by bitmask. + case 0xa3: + current_board->set_panda_individual_power(req->param1, (req->param2 > 0U)); + break; + // **** 0xa4: Enable generated CAN traffic. + case 0xa4: + generated_can_traffic = (req->param1 > 0U); + break; + // **** 0xa8: get microsecond timer + case 0xa8: + time = microsecond_timer_get(); + resp[0] = (time & 0x000000FFU); + resp[1] = ((time & 0x0000FF00U) >> 8U); + resp[2] = ((time & 0x00FF0000U) >> 16U); + resp[3] = ((time & 0xFF000000U) >> 24U); + resp_len = 4U; + break; + // **** 0xc0: reset communications + case 0xc0: + comms_can_reset(); + break; + // **** 0xc1: get hardware type + case 0xc1: + resp[0] = hw_type; + resp_len = 1; + break; + // **** 0xc2: CAN health stats + case 0xc2: + COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); + if (req->param1 < 3U) { + update_can_health_pkt(req->param1, 0U); + can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); + can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); + can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; + can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; + can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; + resp_len = sizeof(can_health[req->param1]); + (void)memcpy(resp, &can_health[req->param1], resp_len); + } + break; + // **** 0xc3: fetch MCU UID + case 0xc3: + (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); + resp_len = 12; + break; + // **** 0xd0: fetch serial (aka the provisioned dongle ID) + case 0xd0: + // addresses are OTP + if (req->param1 == 1U) { + (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); + resp_len = 0x10; + } else { + get_provision_chunk(resp); + resp_len = PROVISION_CHUNK_LEN; + } + break; + // **** 0xd1: enter bootloader mode + case 0xd1: + // this allows reflashing of the bootstub + switch (req->param1) { + case 0: + // only allow bootloader entry on debug builds + #ifdef ALLOW_DEBUG + print("-> entering bootloader\n"); + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + NVIC_SystemReset(); + #endif + break; + case 1: + print("-> entering softloader\n"); + enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; + NVIC_SystemReset(); + break; + default: + print("Bootloader mode invalid\n"); + break; + } + break; + // **** 0xd2: get health packet + case 0xd2: + resp_len = get_jungle_health_pkt(resp); + break; + // **** 0xd3: get first 64 bytes of signature + case 0xd3: + { + resp_len = 64; + char * code = (char*)_app_start; + int code_len = _app_start[0]; + (void)memcpy(resp, &code[code_len], resp_len); + } + break; + // **** 0xd4: get second 64 bytes of signature + case 0xd4: + { + resp_len = 64; + char * code = (char*)_app_start; + int code_len = _app_start[0]; + (void)memcpy(resp, &code[code_len + 64], resp_len); + } + break; + // **** 0xd6: get version + case 0xd6: + COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); + (void)memcpy(resp, gitversion, sizeof(gitversion)); + resp_len = sizeof(gitversion) - 1U; + break; + // **** 0xd8: reset ST + case 0xd8: + NVIC_SystemReset(); + break; + // **** 0xdb: set OBD CAN multiplexing mode + case 0xdb: + if (req->param1 == 1U) { + // Enable OBD CAN + current_board->set_can_mode(CAN_MODE_OBD_CAN2); + } else { + // Disable OBD CAN + current_board->set_can_mode(CAN_MODE_NORMAL); + } + break; + // **** 0xdd: get healthpacket and CANPacket versions + case 0xdd: + resp[0] = JUNGLE_HEALTH_PACKET_VERSION; + resp[1] = CAN_PACKET_VERSION; + resp[2] = CAN_HEALTH_PACKET_VERSION; + resp_len = 3; + break; + // **** 0xde: set can bitrate + case 0xde: + if ((req->param1 < PANDA_CAN_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { + bus_config[req->param1].can_speed = req->param2; + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + // **** 0xe0: debug read + case 0xe0: + // read + while ((resp_len < MIN(req->length, USBPACKET_MAX_SIZE)) && get_char(get_ring_by_number(0), (char*)&resp[resp_len])) { + ++resp_len; + } + break; + // **** 0xe5: set CAN loopback (for testing) + case 0xe5: + can_loopback = (req->param1 > 0U); + can_init_all(); + break; + // **** 0xf1: Clear CAN ring buffer. + case 0xf1: + if (req->param1 == 0xFFFFU) { + print("Clearing CAN Rx queue\n"); + can_clear(&can_rx_q); + } else if (req->param1 < PANDA_CAN_CNT) { + print("Clearing CAN Tx queue\n"); + can_clear(can_queues[req->param1]); + } else { + print("Clearing CAN CAN ring buffer failed: wrong bus number\n"); + } + break; + // **** 0xf4: Set CAN transceiver enable pin + case 0xf4: + current_board->enable_can_transceiver(req->param1, req->param2 > 0U); + break; + // **** 0xf5: Set CAN silent mode + case 0xf5: + can_silent = (req->param1 > 0U); + can_init_all(); + break; + // **** 0xf7: enable/disable header pin by number + case 0xf7: + current_board->enable_header_pin(req->param1, req->param2 > 0U); + break; + // **** 0xf9: set CAN FD data bitrate + case 0xf9: + if ((req->param1 < PANDA_CAN_CNT) && + is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { + bus_config[req->param1].can_data_speed = req->param2; + bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); + bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + // **** 0xfc: set CAN FD non-ISO mode + case 0xfc: + if (req->param1 < PANDA_CAN_CNT) { + bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + default: + print("NO HANDLER "); + puth(req->request); + print("\n"); + break; + } + return resp_len; +} diff --git a/board/jungle/main_comms.h b/board/jungle/main_comms.h index 304bd52cf08..e069d097526 100644 --- a/board/jungle/main_comms.h +++ b/board/jungle/main_comms.h @@ -1,267 +1,15 @@ +#include "board/config.h" + extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used -bool generated_can_traffic = false; #include "board/globals.h" - // TODO: +extern bool generated_can_traffic; extern bool panda_power; -int get_jungle_health_pkt(void *dat) { - COMPILE_TIME_ASSERT(sizeof(struct jungle_health_t) <= USBPACKET_MAX_SIZE); - struct jungle_health_t * health = (struct jungle_health_t*)dat; - - health->uptime_pkt = uptime_cnt; - health->ch1_power = current_board->get_channel_power(1U); - health->ch2_power = current_board->get_channel_power(2U); - health->ch3_power = current_board->get_channel_power(3U); - health->ch4_power = current_board->get_channel_power(4U); - health->ch5_power = current_board->get_channel_power(5U); - health->ch6_power = current_board->get_channel_power(6U); - - health->ch1_sbu1_mV = current_board->get_sbu_mV(1U, SBU1); - health->ch1_sbu2_mV = current_board->get_sbu_mV(1U, SBU2); - health->ch2_sbu1_mV = current_board->get_sbu_mV(2U, SBU1); - health->ch2_sbu2_mV = current_board->get_sbu_mV(2U, SBU2); - health->ch3_sbu1_mV = current_board->get_sbu_mV(3U, SBU1); - health->ch3_sbu2_mV = current_board->get_sbu_mV(3U, SBU2); - health->ch4_sbu1_mV = current_board->get_sbu_mV(4U, SBU1); - health->ch4_sbu2_mV = current_board->get_sbu_mV(4U, SBU2); - health->ch5_sbu1_mV = current_board->get_sbu_mV(5U, SBU1); - health->ch5_sbu2_mV = current_board->get_sbu_mV(5U, SBU2); - health->ch6_sbu1_mV = current_board->get_sbu_mV(6U, SBU1); - health->ch6_sbu2_mV = current_board->get_sbu_mV(6U, SBU2); - - return sizeof(*health); -} // send on serial, first byte to select the ring -void comms_endpoint2_write(const uint8_t *data, uint32_t len) { - UNUSED(data); - UNUSED(len); -} - -int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { - unsigned int resp_len = 0; - uint32_t time; - -#ifdef DEBUG_COMMS - print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n"); - print("- request "); puth(req->request); print("\n"); - print("- param1 "); puth(req->param1); print("\n"); - print("- param2 "); puth(req->param2); print("\n"); -#endif - - switch (req->request) { - // **** 0xa0: Set panda power. - case 0xa0: - current_board->set_panda_power((req->param1 == 1U)); - break; - // **** 0xa1: Set harness orientation. - case 0xa1: - current_board->set_harness_orientation(req->param1); - break; - // **** 0xa2: Set ignition. - case 0xa2: - current_board->set_ignition((req->param1 == 1U)); - break; - // **** 0xa3: Set panda power per channel by bitmask. - case 0xa3: - current_board->set_panda_individual_power(req->param1, (req->param2 > 0U)); - break; - // **** 0xa4: Enable generated CAN traffic. - case 0xa4: - generated_can_traffic = (req->param1 > 0U); - break; - // **** 0xa8: get microsecond timer - case 0xa8: - time = microsecond_timer_get(); - resp[0] = (time & 0x000000FFU); - resp[1] = ((time & 0x0000FF00U) >> 8U); - resp[2] = ((time & 0x00FF0000U) >> 16U); - resp[3] = ((time & 0xFF000000U) >> 24U); - resp_len = 4U; - break; - // **** 0xc0: reset communications - case 0xc0: - comms_can_reset(); - break; - // **** 0xc1: get hardware type - case 0xc1: - resp[0] = hw_type; - resp_len = 1; - break; - // **** 0xc2: CAN health stats - case 0xc2: - COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); - if (req->param1 < 3U) { - update_can_health_pkt(req->param1, 0U); - can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); - can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); - can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; - can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; - can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; - resp_len = sizeof(can_health[req->param1]); - (void)memcpy(resp, &can_health[req->param1], resp_len); - } - break; - // **** 0xc3: fetch MCU UID - case 0xc3: - (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); - resp_len = 12; - break; - // **** 0xd0: fetch serial (aka the provisioned dongle ID) - case 0xd0: - // addresses are OTP - if (req->param1 == 1U) { - (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); - resp_len = 0x10; - } else { - get_provision_chunk(resp); - resp_len = PROVISION_CHUNK_LEN; - } - break; - // **** 0xd1: enter bootloader mode - case 0xd1: - // this allows reflashing of the bootstub - switch (req->param1) { - case 0: - // only allow bootloader entry on debug builds - #ifdef ALLOW_DEBUG - print("-> entering bootloader\n"); - enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; - NVIC_SystemReset(); - #endif - break; - case 1: - print("-> entering softloader\n"); - enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; - NVIC_SystemReset(); - break; - default: - print("Bootloader mode invalid\n"); - break; - } - break; - // **** 0xd2: get health packet - case 0xd2: - resp_len = get_jungle_health_pkt(resp); - break; - // **** 0xd3: get first 64 bytes of signature - case 0xd3: - { - resp_len = 64; - char * code = (char*)_app_start; - int code_len = _app_start[0]; - (void)memcpy(resp, &code[code_len], resp_len); - } - break; - // **** 0xd4: get second 64 bytes of signature - case 0xd4: - { - resp_len = 64; - char * code = (char*)_app_start; - int code_len = _app_start[0]; - (void)memcpy(resp, &code[code_len + 64], resp_len); - } - break; - // **** 0xd6: get version - case 0xd6: - COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); - (void)memcpy(resp, gitversion, sizeof(gitversion)); - resp_len = sizeof(gitversion) - 1U; - break; - // **** 0xd8: reset ST - case 0xd8: - NVIC_SystemReset(); - break; - // **** 0xdb: set OBD CAN multiplexing mode - case 0xdb: - if (req->param1 == 1U) { - // Enable OBD CAN - current_board->set_can_mode(CAN_MODE_OBD_CAN2); - } else { - // Disable OBD CAN - current_board->set_can_mode(CAN_MODE_NORMAL); - } - break; - // **** 0xdd: get healthpacket and CANPacket versions - case 0xdd: - resp[0] = JUNGLE_HEALTH_PACKET_VERSION; - resp[1] = CAN_PACKET_VERSION; - resp[2] = CAN_HEALTH_PACKET_VERSION; - resp_len = 3; - break; - // **** 0xde: set can bitrate - case 0xde: - if ((req->param1 < PANDA_CAN_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { - bus_config[req->param1].can_speed = req->param2; - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - // **** 0xe0: debug read - case 0xe0: - // read - while ((resp_len < MIN(req->length, USBPACKET_MAX_SIZE)) && get_char(get_ring_by_number(0), (char*)&resp[resp_len])) { - ++resp_len; - } - break; - // **** 0xe5: set CAN loopback (for testing) - case 0xe5: - can_loopback = (req->param1 > 0U); - can_init_all(); - break; - // **** 0xf1: Clear CAN ring buffer. - case 0xf1: - if (req->param1 == 0xFFFFU) { - print("Clearing CAN Rx queue\n"); - can_clear(&can_rx_q); - } else if (req->param1 < PANDA_CAN_CNT) { - print("Clearing CAN Tx queue\n"); - can_clear(can_queues[req->param1]); - } else { - print("Clearing CAN CAN ring buffer failed: wrong bus number\n"); - } - break; - // **** 0xf4: Set CAN transceiver enable pin - case 0xf4: - current_board->enable_can_transceiver(req->param1, req->param2 > 0U); - break; - // **** 0xf5: Set CAN silent mode - case 0xf5: - can_silent = (req->param1 > 0U); - can_init_all(); - break; - // **** 0xf7: enable/disable header pin by number - case 0xf7: - current_board->enable_header_pin(req->param1, req->param2 > 0U); - break; - // **** 0xf9: set CAN FD data bitrate - case 0xf9: - if ((req->param1 < PANDA_CAN_CNT) && - is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { - bus_config[req->param1].can_data_speed = req->param2; - bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); - bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - // **** 0xfc: set CAN FD non-ISO mode - case 0xfc: - if (req->param1 < PANDA_CAN_CNT) { - bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - default: - print("NO HANDLER "); - puth(req->request); - print("\n"); - break; - } - return resp_len; -} +void comms_endpoint2_write(const uint8_t *data, uint32_t len); +int comms_control_handler(ControlPacket_t *req, uint8_t *resp); \ No newline at end of file diff --git a/board/main.c b/board/main.c index c74d995fe5b..964c0b9c9fa 100644 --- a/board/main.c +++ b/board/main.c @@ -3,37 +3,33 @@ #include "board/drivers/led.h" #include "board/drivers/pwm.h" -#include "board/drivers/usb.h" +// #include "board/drivers/usb.h" #include "board/drivers/simple_watchdog.h" #include "board/drivers/bootkick.h" #include "board/early_init.h" -#include "board/provision.h" +// #include "board/provision.h" -#include "libc.h" +// #include "libc.h" -#include "opendbc/safety/safety.h" - -#include "board/health.h" +#include "board/power_saving.h" +#include "board/globals.h" +#include "opendbc/safety/safety.h" #include "board/drivers/can_common.h" - #include "board/drivers/fdcan.h" - -#include "board/power_saving.h" - #include "board/obj/gitversion.h" -#include "board/can_comms.h" +// #include "board/can_comms.h" #include "board/main_comms.h" -#include "board/main_declarations.h" +// #include "board/main_declarations.h" // TODO -uint32_t heartbeat_counter; -bool heartbeat_lost; -bool heartbeat_disabled; -bool siren_enabled; +extern uint32_t heartbeat_counter; +extern bool heartbeat_lost; +extern bool heartbeat_disabled; +extern bool siren_enabled; uint32_t uptime_cnt; // ********************* Serial debugging ********************* @@ -47,354 +43,6 @@ void debug_ring_callback(uart_ring *ring) { // ****************************** safety mode ****************************** -#include -#include - -#include "board/utils.h" -#include "board/config.h" -#include "board/health.h" -#include "board/main_declarations.h" -#include "board/drivers/can_common.h" -#include "board/power_saving.h" -#include "board/drivers/spi.h" -#include "board/drivers/bootkick.h" -#include "board/drivers/fdcan.h" -#include "board/provision.h" -#include "board/early_init.h" -#include "board/drivers/fan.h" -#include "board/drivers/clock_source.h" - -extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used - -// Prototypes -void set_safety_mode(uint16_t mode, uint16_t param); -bool is_car_safety_mode(uint16_t mode); - -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; - - health->uptime_pkt = uptime_cnt; - health->voltage_pkt = current_board->read_voltage_mV(); - health->current_pkt = current_board->read_current_mA(); - - health->ignition_line_pkt = (uint8_t)(harness_check_ignition()); - health->ignition_can_pkt = ignition_can; - - health->controls_allowed_pkt = controls_allowed; - health->safety_tx_blocked_pkt = safety_tx_blocked; - health->safety_rx_invalid_pkt = safety_rx_invalid; - health->tx_buffer_overflow_pkt = tx_buffer_overflow; - health->rx_buffer_overflow_pkt = rx_buffer_overflow; - health->car_harness_status_pkt = harness.status; - health->safety_mode_pkt = (uint8_t)(current_safety_mode); - health->safety_param_pkt = current_safety_param; - health->alternative_experience_pkt = alternative_experience; - health->power_save_enabled_pkt = power_save_status == POWER_SAVE_STATUS_ENABLED; - health->heartbeat_lost_pkt = heartbeat_lost; - health->safety_rx_checks_invalid_pkt = safety_rx_checks_invalid; - - health->spi_error_count_pkt = spi_error_count; - - health->fault_status_pkt = fault_status; - health->faults_pkt = faults; - - health->interrupt_load_pkt = interrupt_load; - - health->fan_power = fan_state.power; - - health->sbu1_voltage_mV = harness.sbu1_voltage_mV; - health->sbu2_voltage_mV = harness.sbu2_voltage_mV; - - health->som_reset_triggered = bootkick_reset_triggered; - - return sizeof(*health); -} - -// send on serial, first byte to select the ring -void comms_endpoint2_write(const uint8_t *data, uint32_t len) { - uart_ring *ur = get_ring_by_number(data[0]); - if ((len != 0U) && (ur != NULL)) { - if ((data[0] < 2U) || (data[0] >= 4U)) { - for (uint32_t i = 1; i < len; i++) { - while (!put_char(ur, data[i])) { - // wait - } - } - } - } -} - -int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { - unsigned int resp_len = 0; - uart_ring *ur = NULL; - uint32_t time; - -#ifdef DEBUG_COMMS - print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n"); - print("- request "); puth(req->request); print("\n"); - print("- param1 "); puth(req->param1); print("\n"); - print("- param2 "); puth(req->param2); print("\n"); -#endif - - switch (req->request) { - // **** 0xa8: get microsecond timer - case 0xa8: - time = microsecond_timer_get(); - resp[0] = (time & 0x000000FFU); - resp[1] = ((time & 0x0000FF00U) >> 8U); - resp[2] = ((time & 0x00FF0000U) >> 16U); - resp[3] = ((time & 0xFF000000U) >> 24U); - resp_len = 4U; - break; - // **** 0xb0: set IR power - case 0xb0: - current_board->set_ir_power(req->param1); - break; - // **** 0xb1: set fan power - case 0xb1: - fan_set_power(req->param1); - break; - // **** 0xb2: get fan rpm - case 0xb2: - resp[0] = (fan_state.rpm & 0x00FFU); - resp[1] = ((fan_state.rpm & 0xFF00U) >> 8U); - resp_len = 2; - break; - // **** 0xc0: reset communications state - case 0xc0: - comms_can_reset(); - break; - // **** 0xc1: get hardware type - case 0xc1: - resp[0] = hw_type; - resp_len = 1; - break; - // **** 0xc2: CAN health stats - case 0xc2: - COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); - if (req->param1 < 3U) { - update_can_health_pkt(req->param1, 0U); - can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); - can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); - can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; - can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; - can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; - resp_len = sizeof(can_health[req->param1]); - (void)memcpy(resp, (uint8_t*)(&can_health[req->param1]), resp_len); - } - break; - // **** 0xc3: fetch MCU UID - case 0xc3: - (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); - resp_len = 12; - break; - // **** 0xc4: get interrupt call rate - case 0xc4: - if (req->param1 < NUM_INTERRUPTS) { - uint32_t load = interrupts[req->param1].call_rate; - resp[0] = (load & 0x000000FFU); - resp[1] = ((load & 0x0000FF00U) >> 8U); - resp[2] = ((load & 0x00FF0000U) >> 16U); - resp[3] = ((load & 0xFF000000U) >> 24U); - resp_len = 4U; - } - break; - // **** 0xc5: DEBUG: drive relay - case 0xc5: - set_intercept_relay((req->param1 & 0x1U), (req->param1 & 0x2U)); - break; - // **** 0xc6: DEBUG: read SOM GPIO - case 0xc6: - resp[0] = current_board->read_som_gpio(); - resp_len = 1; - break; - // **** 0xd0: fetch serial (aka the provisioned dongle ID) - case 0xd0: - // addresses are OTP - if (req->param1 == 1U) { - (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); - resp_len = 0x10; - } else { - get_provision_chunk(resp); - resp_len = PROVISION_CHUNK_LEN; - } - break; - // **** 0xd1: enter bootloader mode - case 0xd1: - // this allows reflashing of the bootstub - switch (req->param1) { - case 0: - // only allow bootloader entry on debug builds - #ifdef ALLOW_DEBUG - print("-> entering bootloader\n"); - enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; - NVIC_SystemReset(); - #endif - break; - case 1: - print("-> entering softloader\n"); - enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; - NVIC_SystemReset(); - break; - default: - print("Bootloader mode invalid\n"); - break; - } - break; - // **** 0xd2: get health packet - case 0xd2: - resp_len = get_health_pkt(resp); - break; - // **** 0xd3: get first 64 bytes of signature - case 0xd3: - { - resp_len = 64; - char * code = (char*)_app_start; - int code_len = _app_start[0]; - (void)memcpy(resp, &code[code_len], resp_len); - } - break; - // **** 0xd4: get second 64 bytes of signature - case 0xd4: - { - resp_len = 64; - char * code = (char*)_app_start; - int code_len = _app_start[0]; - (void)memcpy(resp, &code[code_len + 64], resp_len); - } - break; - // **** 0xd6: get version - case 0xd6: - COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); - (void)memcpy(resp, gitversion, sizeof(gitversion)); - resp_len = sizeof(gitversion) - 1U; - break; - // **** 0xd8: reset ST - case 0xd8: - NVIC_SystemReset(); - break; - // **** 0xdb: set OBD CAN multiplexing mode - case 0xdb: - current_board->set_can_mode((req->param1 == 1U) ? CAN_MODE_OBD_CAN2 : CAN_MODE_NORMAL); - break; - // **** 0xdc: set safety mode - case 0xdc: - set_safety_mode(req->param1, (uint16_t)req->param2); - break; - // **** 0xdd: get healthpacket and CANPacket versions - case 0xdd: - resp[0] = HEALTH_PACKET_VERSION; - resp[1] = CAN_PACKET_VERSION; - resp[2] = CAN_HEALTH_PACKET_VERSION; - resp_len = 3; - break; - // **** 0xde: set can bitrate - case 0xde: - if ((req->param1 < PANDA_CAN_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { - bus_config[req->param1].can_speed = req->param2; - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - // **** 0xdf: set alternative experience - case 0xdf: - // you can only set this if you are in a non car safety mode - if (!is_car_safety_mode(current_safety_mode)) { - alternative_experience = req->param1; - } - break; - // **** 0xe0: uart read - case 0xe0: - ur = get_ring_by_number(req->param1); - if (!ur) { - break; - } - - // read - uint16_t req_length = MIN(req->length, USBPACKET_MAX_SIZE); - while ((resp_len < req_length) && - get_char(ur, (char*)&resp[resp_len])) { - ++resp_len; - } - break; - // **** 0xe5: set CAN loopback (for testing) - case 0xe5: - can_loopback = req->param1 > 0U; - can_init_all(); - break; - // **** 0xe6: set custom clock source period and pulse length - case 0xe6: - clock_source_set_timer_params(req->param1, req->param2); - break; - // **** 0xe7: set power save state - case 0xe7: - set_power_save_state(req->param1); - break; - // **** 0xe8: set can-fd auto swithing mode - case 0xe8: - bus_config[req->param1].canfd_auto = req->param2 > 0U; - break; - // **** 0xf1: Clear CAN ring buffer. - case 0xf1: - if (req->param1 == 0xFFFFU) { - print("Clearing CAN Rx queue\n"); - can_clear(&can_rx_q); - } else if (req->param1 < PANDA_CAN_CNT) { - print("Clearing CAN Tx queue\n"); - can_clear(can_queues[req->param1]); - } else { - print("Clearing CAN CAN ring buffer failed: wrong bus number\n"); - } - break; - // **** 0xf3: Heartbeat. Resets heartbeat counter. - case 0xf3: - { - heartbeat_counter = 0U; - heartbeat_lost = false; - heartbeat_disabled = false; - heartbeat_engaged = (req->param1 == 1U); - break; - } - // **** 0xf6: set siren enabled - case 0xf6: - siren_enabled = (req->param1 != 0U); - break; - // **** 0xf8: disable heartbeat checks - case 0xf8: - if (!is_car_safety_mode(current_safety_mode)) { - heartbeat_disabled = true; - } - break; - // **** 0xf9: set CAN FD data bitrate - case 0xf9: - if ((req->param1 < PANDA_CAN_CNT) && - is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { - bus_config[req->param1].can_data_speed = req->param2; - bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); - bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - // **** 0xfc: set CAN FD non-ISO mode - case 0xfc: - if (req->param1 < PANDA_CAN_CNT) { - bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); - bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); - UNUSED(ret); - } - break; - default: - print("NO HANDLER "); - puth(req->request); - print("\n"); - break; - } - return resp_len; -} - - // this is the only way to leave silent mode void set_safety_mode(uint16_t mode, uint16_t param) { uint16_t mode_copy = mode; diff --git a/board/main_comms.c b/board/main_comms.c index 42ffe47bc75..14a2b4986e6 100644 --- a/board/main_comms.c +++ b/board/main_comms.c @@ -1,347 +1,354 @@ -// #include -// #include +#include +#include -// #include "board/utils.h" -// #include "board/config.h" -// #include "board/health.h" -// #include "board/main_declarations.h" -// #include "board/drivers/can_common.h" -// #include "board/power_saving.h" -// #include "board/drivers/spi.h" -// #include "board/drivers/bootkick.h" -// #include "board/drivers/fdcan.h" -// #include "board/provision.h" -// #include "board/early_init.h" -// #include "board/obj/gitversion.h" -// #include "board/drivers/fan.h" -// #include "board/drivers/clock_source.h" +#include "board/utils.h" +#include "board/config.h" +#include "board/health.h" +#include "board/main_declarations.h" +#include "board/drivers/can_common.h" +#include "board/power_saving.h" +#include "board/drivers/spi.h" +#include "board/drivers/bootkick.h" +#include "board/drivers/fdcan.h" +#include "board/provision.h" +#include "board/early_init.h" +#include "board/obj/gitversion.h" +#include "board/drivers/fan.h" +#include "board/drivers/clock_source.h" +#include "board/globals.h" +#include "board/drivers/uart.h" -// extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used +#include "board/stm32h7/llfdcan.h" +#include "board/main_comms.h" -// // Prototypes -// void set_safety_mode(uint16_t mode, uint16_t param); -// bool is_car_safety_mode(uint16_t mode); +extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used -// 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; +extern uint32_t heartbeat_counter; +extern bool heartbeat_lost; +extern bool heartbeat_disabled; -// health->uptime_pkt = uptime_cnt; -// health->voltage_pkt = current_board->read_voltage_mV(); -// health->current_pkt = current_board->read_current_mA(); +void set_safety_mode(uint16_t mode, uint16_t param); -// health->ignition_line_pkt = (uint8_t)(harness_check_ignition()); -// health->ignition_can_pkt = ignition_can; +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; -// health->controls_allowed_pkt = controls_allowed; -// health->safety_tx_blocked_pkt = safety_tx_blocked; -// health->safety_rx_invalid_pkt = safety_rx_invalid; -// health->tx_buffer_overflow_pkt = tx_buffer_overflow; -// health->rx_buffer_overflow_pkt = rx_buffer_overflow; -// health->car_harness_status_pkt = harness.status; -// health->safety_mode_pkt = (uint8_t)(current_safety_mode); -// health->safety_param_pkt = current_safety_param; -// health->alternative_experience_pkt = alternative_experience; -// health->power_save_enabled_pkt = power_save_status == POWER_SAVE_STATUS_ENABLED; -// health->heartbeat_lost_pkt = heartbeat_lost; -// health->safety_rx_checks_invalid_pkt = safety_rx_checks_invalid; + health->uptime_pkt = uptime_cnt; + health->voltage_pkt = current_board->read_voltage_mV(); + health->current_pkt = current_board->read_current_mA(); -// health->spi_error_count_pkt = spi_error_count; + health->ignition_line_pkt = (uint8_t)(harness_check_ignition()); + health->ignition_can_pkt = ignition_can; -// health->fault_status_pkt = fault_status; -// health->faults_pkt = faults; + health->controls_allowed_pkt = controls_allowed; + health->safety_tx_blocked_pkt = safety_tx_blocked; + health->safety_rx_invalid_pkt = safety_rx_invalid; + health->tx_buffer_overflow_pkt = tx_buffer_overflow; + health->rx_buffer_overflow_pkt = rx_buffer_overflow; + health->car_harness_status_pkt = harness.status; + health->safety_mode_pkt = (uint8_t)(current_safety_mode); + health->safety_param_pkt = current_safety_param; + health->alternative_experience_pkt = alternative_experience; + health->power_save_enabled_pkt = power_save_status == POWER_SAVE_STATUS_ENABLED; + health->heartbeat_lost_pkt = heartbeat_lost; + health->safety_rx_checks_invalid_pkt = safety_rx_checks_invalid; -// health->interrupt_load_pkt = interrupt_load; + health->spi_error_count_pkt = spi_error_count; -// health->fan_power = fan_state.power; + health->fault_status_pkt = fault_status; + health->faults_pkt = faults; -// health->sbu1_voltage_mV = harness.sbu1_voltage_mV; -// health->sbu2_voltage_mV = harness.sbu2_voltage_mV; + health->interrupt_load_pkt = interrupt_load; -// health->som_reset_triggered = bootkick_reset_triggered; + health->fan_power = fan_state.power; -// return sizeof(*health); -// } + health->sbu1_voltage_mV = harness.sbu1_voltage_mV; + health->sbu2_voltage_mV = harness.sbu2_voltage_mV; -// // send on serial, first byte to select the ring -// void comms_endpoint2_write(const uint8_t *data, uint32_t len) { -// uart_ring *ur = get_ring_by_number(data[0]); -// if ((len != 0U) && (ur != NULL)) { -// if ((data[0] < 2U) || (data[0] >= 4U)) { -// for (uint32_t i = 1; i < len; i++) { -// while (!put_char(ur, data[i])) { -// // wait -// } -// } -// } -// } -// } + health->som_reset_triggered = bootkick_reset_triggered; -// int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { -// unsigned int resp_len = 0; -// uart_ring *ur = NULL; -// uint32_t time; + return sizeof(*health); +} -// #ifdef DEBUG_COMMS -// print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n"); -// print("- request "); puth(req->request); print("\n"); -// print("- param1 "); puth(req->param1); print("\n"); -// print("- param2 "); puth(req->param2); print("\n"); -// #endif +// send on serial, first byte to select the ring +void comms_endpoint2_write(const uint8_t *data, uint32_t len) { + uart_ring *ur = get_ring_by_number(data[0]); + if ((len != 0U) && (ur != NULL)) { + if ((data[0] < 2U) || (data[0] >= 4U)) { + for (uint32_t i = 1; i < len; i++) { + while (!put_char(ur, data[i])) { + // wait + } + } + } + } +} -// switch (req->request) { -// // **** 0xa8: get microsecond timer -// case 0xa8: -// time = microsecond_timer_get(); -// resp[0] = (time & 0x000000FFU); -// resp[1] = ((time & 0x0000FF00U) >> 8U); -// resp[2] = ((time & 0x00FF0000U) >> 16U); -// resp[3] = ((time & 0xFF000000U) >> 24U); -// resp_len = 4U; -// break; -// // **** 0xb0: set IR power -// case 0xb0: -// current_board->set_ir_power(req->param1); -// break; -// // **** 0xb1: set fan power -// case 0xb1: -// fan_set_power(req->param1); -// break; -// // **** 0xb2: get fan rpm -// case 0xb2: -// resp[0] = (fan_state.rpm & 0x00FFU); -// resp[1] = ((fan_state.rpm & 0xFF00U) >> 8U); -// resp_len = 2; -// break; -// // **** 0xc0: reset communications state -// case 0xc0: -// comms_can_reset(); -// break; -// // **** 0xc1: get hardware type -// case 0xc1: -// resp[0] = hw_type; -// resp_len = 1; -// break; -// // **** 0xc2: CAN health stats -// case 0xc2: -// COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); -// if (req->param1 < 3U) { -// update_can_health_pkt(req->param1, 0U); -// can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); -// can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); -// can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; -// can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; -// can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; -// resp_len = sizeof(can_health[req->param1]); -// (void)memcpy(resp, (uint8_t*)(&can_health[req->param1]), resp_len); -// } -// break; -// // **** 0xc3: fetch MCU UID -// case 0xc3: -// (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); -// resp_len = 12; -// break; -// // **** 0xc4: get interrupt call rate -// case 0xc4: -// if (req->param1 < NUM_INTERRUPTS) { -// uint32_t load = interrupts[req->param1].call_rate; -// resp[0] = (load & 0x000000FFU); -// resp[1] = ((load & 0x0000FF00U) >> 8U); -// resp[2] = ((load & 0x00FF0000U) >> 16U); -// resp[3] = ((load & 0xFF000000U) >> 24U); -// resp_len = 4U; -// } -// break; -// // **** 0xc5: DEBUG: drive relay -// case 0xc5: -// set_intercept_relay((req->param1 & 0x1U), (req->param1 & 0x2U)); -// break; -// // **** 0xc6: DEBUG: read SOM GPIO -// case 0xc6: -// resp[0] = current_board->read_som_gpio(); -// resp_len = 1; -// break; -// // **** 0xd0: fetch serial (aka the provisioned dongle ID) -// case 0xd0: -// // addresses are OTP -// if (req->param1 == 1U) { -// (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); -// resp_len = 0x10; -// } else { -// get_provision_chunk(resp); -// resp_len = PROVISION_CHUNK_LEN; -// } -// break; -// // **** 0xd1: enter bootloader mode -// case 0xd1: -// // this allows reflashing of the bootstub -// switch (req->param1) { -// case 0: -// // only allow bootloader entry on debug builds -// #ifdef ALLOW_DEBUG -// print("-> entering bootloader\n"); -// enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; -// NVIC_SystemReset(); -// #endif -// break; -// case 1: -// print("-> entering softloader\n"); -// enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; -// NVIC_SystemReset(); -// break; -// default: -// print("Bootloader mode invalid\n"); -// break; -// } -// break; -// // **** 0xd2: get health packet -// case 0xd2: -// resp_len = get_health_pkt(resp); -// break; -// // **** 0xd3: get first 64 bytes of signature -// case 0xd3: -// { -// resp_len = 64; -// char * code = (char*)_app_start; -// int code_len = _app_start[0]; -// (void)memcpy(resp, &code[code_len], resp_len); -// } -// break; -// // **** 0xd4: get second 64 bytes of signature -// case 0xd4: -// { -// resp_len = 64; -// char * code = (char*)_app_start; -// int code_len = _app_start[0]; -// (void)memcpy(resp, &code[code_len + 64], resp_len); -// } -// break; -// // **** 0xd6: get version -// case 0xd6: -// COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); -// (void)memcpy(resp, gitversion, sizeof(gitversion)); -// resp_len = sizeof(gitversion) - 1U; -// break; -// // **** 0xd8: reset ST -// case 0xd8: -// NVIC_SystemReset(); -// break; -// // **** 0xdb: set OBD CAN multiplexing mode -// case 0xdb: -// current_board->set_can_mode((req->param1 == 1U) ? CAN_MODE_OBD_CAN2 : CAN_MODE_NORMAL); -// break; -// // **** 0xdc: set safety mode -// case 0xdc: -// set_safety_mode(req->param1, (uint16_t)req->param2); -// break; -// // **** 0xdd: get healthpacket and CANPacket versions -// case 0xdd: -// resp[0] = HEALTH_PACKET_VERSION; -// resp[1] = CAN_PACKET_VERSION; -// resp[2] = CAN_HEALTH_PACKET_VERSION; -// resp_len = 3; -// break; -// // **** 0xde: set can bitrate -// case 0xde: -// if ((req->param1 < PANDA_CAN_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { -// bus_config[req->param1].can_speed = req->param2; -// bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); -// UNUSED(ret); -// } -// break; -// // **** 0xdf: set alternative experience -// case 0xdf: -// // you can only set this if you are in a non car safety mode -// if (!is_car_safety_mode(current_safety_mode)) { -// alternative_experience = req->param1; -// } -// break; -// // **** 0xe0: uart read -// case 0xe0: -// ur = get_ring_by_number(req->param1); -// if (!ur) { -// break; -// } +int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { + unsigned int resp_len = 0; + uart_ring *ur = NULL; + uint32_t time; -// // read -// uint16_t req_length = MIN(req->length, USBPACKET_MAX_SIZE); -// while ((resp_len < req_length) && -// get_char(ur, (char*)&resp[resp_len])) { -// ++resp_len; -// } -// break; -// // **** 0xe5: set CAN loopback (for testing) -// case 0xe5: -// can_loopback = req->param1 > 0U; -// can_init_all(); -// break; -// // **** 0xe6: set custom clock source period and pulse length -// case 0xe6: -// clock_source_set_timer_params(req->param1, req->param2); -// break; -// // **** 0xe7: set power save state -// case 0xe7: -// set_power_save_state(req->param1); -// break; -// // **** 0xe8: set can-fd auto swithing mode -// case 0xe8: -// bus_config[req->param1].canfd_auto = req->param2 > 0U; -// break; -// // **** 0xf1: Clear CAN ring buffer. -// case 0xf1: -// if (req->param1 == 0xFFFFU) { -// print("Clearing CAN Rx queue\n"); -// can_clear(&can_rx_q); -// } else if (req->param1 < PANDA_CAN_CNT) { -// print("Clearing CAN Tx queue\n"); -// can_clear(can_queues[req->param1]); -// } else { -// print("Clearing CAN CAN ring buffer failed: wrong bus number\n"); -// } -// break; -// // **** 0xf3: Heartbeat. Resets heartbeat counter. -// case 0xf3: -// { -// heartbeat_counter = 0U; -// heartbeat_lost = false; -// heartbeat_disabled = false; -// heartbeat_engaged = (req->param1 == 1U); -// break; -// } -// // **** 0xf6: set siren enabled -// case 0xf6: -// siren_enabled = (req->param1 != 0U); -// break; -// // **** 0xf8: disable heartbeat checks -// case 0xf8: -// if (!is_car_safety_mode(current_safety_mode)) { -// heartbeat_disabled = true; -// } -// break; -// // **** 0xf9: set CAN FD data bitrate -// case 0xf9: -// if ((req->param1 < PANDA_CAN_CNT) && -// is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { -// bus_config[req->param1].can_data_speed = req->param2; -// bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); -// bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); -// bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); -// UNUSED(ret); -// } -// break; -// // **** 0xfc: set CAN FD non-ISO mode -// case 0xfc: -// if (req->param1 < PANDA_CAN_CNT) { -// bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); -// bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); -// UNUSED(ret); -// } -// break; -// default: -// print("NO HANDLER "); -// puth(req->request); -// print("\n"); -// break; -// } -// return resp_len; -// } +#ifdef DEBUG_COMMS + print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n"); + print("- request "); puth(req->request); print("\n"); + print("- param1 "); puth(req->param1); print("\n"); + print("- param2 "); puth(req->param2); print("\n"); +#endif + + switch (req->request) { + // **** 0xa8: get microsecond timer + case 0xa8: + time = microsecond_timer_get(); + resp[0] = (time & 0x000000FFU); + resp[1] = ((time & 0x0000FF00U) >> 8U); + resp[2] = ((time & 0x00FF0000U) >> 16U); + resp[3] = ((time & 0xFF000000U) >> 24U); + resp_len = 4U; + break; + // **** 0xb0: set IR power + case 0xb0: + current_board->set_ir_power(req->param1); + break; + // **** 0xb1: set fan power + case 0xb1: + fan_set_power(req->param1); + break; + // **** 0xb2: get fan rpm + case 0xb2: + resp[0] = (fan_state.rpm & 0x00FFU); + resp[1] = ((fan_state.rpm & 0xFF00U) >> 8U); + resp_len = 2; + break; + // **** 0xc0: reset communications state + case 0xc0: + comms_can_reset(); + break; + // **** 0xc1: get hardware type + case 0xc1: + resp[0] = hw_type; + resp_len = 1; + break; + // **** 0xc2: CAN health stats + case 0xc2: + COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE); + if (req->param1 < 3U) { + update_can_health_pkt(req->param1, 0U); + can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U); + can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U); + can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled; + can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled; + can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso; + resp_len = sizeof(can_health[req->param1]); + (void)memcpy(resp, (uint8_t*)(&can_health[req->param1]), resp_len); + } + break; + // **** 0xc3: fetch MCU UID + case 0xc3: + (void)memcpy(resp, ((uint8_t *)UID_BASE), 12); + resp_len = 12; + break; + // **** 0xc4: get interrupt call rate + case 0xc4: + if (req->param1 < NUM_INTERRUPTS) { + uint32_t load = interrupts[req->param1].call_rate; + resp[0] = (load & 0x000000FFU); + resp[1] = ((load & 0x0000FF00U) >> 8U); + resp[2] = ((load & 0x00FF0000U) >> 16U); + resp[3] = ((load & 0xFF000000U) >> 24U); + resp_len = 4U; + } + break; + // **** 0xc5: DEBUG: drive relay + case 0xc5: + set_intercept_relay((req->param1 & 0x1U), (req->param1 & 0x2U)); + break; + // **** 0xc6: DEBUG: read SOM GPIO + case 0xc6: + resp[0] = current_board->read_som_gpio(); + resp_len = 1; + break; + // **** 0xd0: fetch serial (aka the provisioned dongle ID) + case 0xd0: + // addresses are OTP + if (req->param1 == 1U) { + (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); + resp_len = 0x10; + } else { + get_provision_chunk(resp); + resp_len = PROVISION_CHUNK_LEN; + } + break; + // **** 0xd1: enter bootloader mode + case 0xd1: + // this allows reflashing of the bootstub + switch (req->param1) { + case 0: + // only allow bootloader entry on debug builds + #ifdef ALLOW_DEBUG + print("-> entering bootloader\n"); + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + NVIC_SystemReset(); + #endif + break; + case 1: + print("-> entering softloader\n"); + enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; + NVIC_SystemReset(); + break; + default: + print("Bootloader mode invalid\n"); + break; + } + break; + // **** 0xd2: get health packet + case 0xd2: + resp_len = get_health_pkt(resp); + break; + // **** 0xd3: get first 64 bytes of signature + case 0xd3: + { + resp_len = 64; + char * code = (char*)_app_start; + int code_len = _app_start[0]; + (void)memcpy(resp, &code[code_len], resp_len); + } + break; + // **** 0xd4: get second 64 bytes of signature + case 0xd4: + { + resp_len = 64; + char * code = (char*)_app_start; + int code_len = _app_start[0]; + (void)memcpy(resp, &code[code_len + 64], resp_len); + } + break; + // **** 0xd6: get version + case 0xd6: + COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); + (void)memcpy(resp, gitversion, sizeof(gitversion)); + resp_len = sizeof(gitversion) - 1U; + break; + // **** 0xd8: reset ST + case 0xd8: + NVIC_SystemReset(); + break; + // **** 0xdb: set OBD CAN multiplexing mode + case 0xdb: + current_board->set_can_mode((req->param1 == 1U) ? CAN_MODE_OBD_CAN2 : CAN_MODE_NORMAL); + break; + // **** 0xdc: set safety mode + case 0xdc: + set_safety_mode(req->param1, (uint16_t)req->param2); + break; + // **** 0xdd: get healthpacket and CANPacket versions + case 0xdd: + resp[0] = HEALTH_PACKET_VERSION; + resp[1] = CAN_PACKET_VERSION; + resp[2] = CAN_HEALTH_PACKET_VERSION; + resp_len = 3; + break; + // **** 0xde: set can bitrate + case 0xde: + if ((req->param1 < PANDA_CAN_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) { + bus_config[req->param1].can_speed = req->param2; + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + // **** 0xdf: set alternative experience + case 0xdf: + // you can only set this if you are in a non car safety mode + if (!is_car_safety_mode(current_safety_mode)) { + alternative_experience = req->param1; + } + break; + // **** 0xe0: uart read + case 0xe0: + ur = get_ring_by_number(req->param1); + if (!ur) { + break; + } + + // read + uint16_t req_length = MIN(req->length, USBPACKET_MAX_SIZE); + while ((resp_len < req_length) && + get_char(ur, (char*)&resp[resp_len])) { + ++resp_len; + } + break; + // **** 0xe5: set CAN loopback (for testing) + case 0xe5: + can_loopback = req->param1 > 0U; + can_init_all(); + break; + // **** 0xe6: set custom clock source period and pulse length + case 0xe6: + clock_source_set_timer_params(req->param1, req->param2); + break; + // **** 0xe7: set power save state + case 0xe7: + set_power_save_state(req->param1); + break; + // **** 0xe8: set can-fd auto swithing mode + case 0xe8: + bus_config[req->param1].canfd_auto = req->param2 > 0U; + break; + // **** 0xf1: Clear CAN ring buffer. + case 0xf1: + if (req->param1 == 0xFFFFU) { + print("Clearing CAN Rx queue\n"); + can_clear(&can_rx_q); + } else if (req->param1 < PANDA_CAN_CNT) { + print("Clearing CAN Tx queue\n"); + can_clear(can_queues[req->param1]); + } else { + print("Clearing CAN CAN ring buffer failed: wrong bus number\n"); + } + break; + // **** 0xf3: Heartbeat. Resets heartbeat counter. + case 0xf3: + { + heartbeat_counter = 0U; + heartbeat_lost = false; + heartbeat_disabled = false; + heartbeat_engaged = (req->param1 == 1U); + break; + } + // **** 0xf6: set siren enabled + case 0xf6: + siren_enabled = (req->param1 != 0U); + break; + // **** 0xf8: disable heartbeat checks + case 0xf8: + if (!is_car_safety_mode(current_safety_mode)) { + heartbeat_disabled = true; + } + break; + // **** 0xf9: set CAN FD data bitrate + case 0xf9: + if ((req->param1 < PANDA_CAN_CNT) && + is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) { + bus_config[req->param1].can_data_speed = req->param2; + bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed); + bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + // **** 0xfc: set CAN FD non-ISO mode + case 0xfc: + if (req->param1 < PANDA_CAN_CNT) { + bus_config[req->param1].canfd_non_iso = (req->param2 != 0U); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1)); + UNUSED(ret); + } + break; + default: + print("NO HANDLER "); + puth(req->request); + print("\n"); + break; + } + return resp_len; +} diff --git a/board/main_comms.h b/board/main_comms.h index 5b8cc49e017..ddbb8f34cb2 100644 --- a/board/main_comms.h +++ b/board/main_comms.h @@ -1,6 +1,6 @@ #pragma once -#include "comms_definitions.h" +// #include "comms_definitions.h" extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used diff --git a/board/main_declarations.h b/board/main_declarations.h index f6beb302249..988b9dfca0b 100644 --- a/board/main_declarations.h +++ b/board/main_declarations.h @@ -3,22 +3,10 @@ #include #include -#include "board/board_struct.h" - -// ******************** Prototypes ******************** - - -void pwm_init(TIM_TypeDef *TIM, uint8_t channel); -void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage); - -// ********************* Globals ********************** -#include "globals.h" - - // heartbeat state extern uint32_t heartbeat_counter; extern bool heartbeat_lost; -extern bool heartbeat_disabled; +extern bool heartbeat_disabled; // set over USB -// siren state + // siren state extern bool siren_enabled; diff --git a/board/main_definitions.c b/board/main_definitions.c index 83c91090579..6f082084aba 100644 --- a/board/main_definitions.c +++ b/board/main_definitions.c @@ -1,9 +1,11 @@ #include "main_declarations.h" +#include "board/board_struct.h" + // ********************* Globals ********************** -uint8_t hw_type = 0; -board *current_board; -uint32_t uptime_cnt = 0; +// uint8_t hw_type = 0; +// board *current_board; +// uint32_t uptime_cnt = 0; // heartbeat state uint32_t heartbeat_counter = 0; diff --git a/board/power_saving.c b/board/power_saving.c index 94d2c4c3499..eafda699b06 100644 --- a/board/power_saving.c +++ b/board/power_saving.c @@ -1,9 +1,11 @@ #include -#include "power_saving.h" -#include "main_declarations.h" -#include "config.h" +#include "board/power_saving.h" +#include "board/main_declarations.h" +#include "board/config.h" #include "board/drivers/fdcan.h" +#include "board/globals.h" + // WARNING: To stay in compliance with the SIL2 rules laid out in STM UM1840, we should never implement any of the available hardware low power modes. // See rule: CoU_3 diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h index 23e0172a864..7cc301f6fa5 100644 --- a/board/stm32h7/stm32h7_config.h +++ b/board/stm32h7/stm32h7_config.h @@ -91,4 +91,5 @@ separate IRQs for RX and TX. #include "board/drivers/spi.h" #include "board/stm32h7/llspi.h" -void early_gpio_float(void); \ No newline at end of file +void early_gpio_float(void); +void detect_board_type(void); \ No newline at end of file diff --git a/tests/misra/checkers.txt b/tests/misra/checkers.txt index 44e6aa13f34..13036c65d95 100644 --- a/tests/misra/checkers.txt +++ b/tests/misra/checkers.txt @@ -5,7 +5,7 @@ Cppcheck checkers list from test_misra.sh: TEST variant options: ---enable=all --disable=unusedFunction --addon=misra -DSTM32H7 -DSTM32H725xx -I /board/stm32h7/inc/ /board/main.c +--enable=all --disable=unusedFunction --addon=misra -DSTM32H7 -DSTM32H725xx -I /board/stm32h7/inc/ /board/main.c -DPANDA Critical errors diff --git a/tests/misra/test_misra.sh b/tests/misra/test_misra.sh index d7b9c6efdf8..5a91802cb41 100755 --- a/tests/misra/test_misra.sh +++ b/tests/misra/test_misra.sh @@ -67,7 +67,7 @@ cppcheck() { PANDA_OPTS="--enable=all --disable=unusedFunction --addon=misra" printf "\n${GREEN}** PANDA H7 CODE **${NC}\n" -cppcheck $PANDA_OPTS -DSTM32H7 -DSTM32H725xx -I $PANDA_DIR/board/stm32h7/inc/ $PANDA_DIR/board/main.c +cppcheck $PANDA_OPTS -DSTM32H7 -DSTM32H725xx -I $PANDA_DIR/board/stm32h7/inc/ $PANDA_DIR/board/main.c -DPANDA # unused needs to run globally #printf "\n${GREEN}** UNUSED ALL CODE **${NC}\n" From dfe88d24c6e5e8f2ea3ce713d067b24a37410ab4 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 11:56:03 +0100 Subject: [PATCH 08/29] Misra fixups --- board/body/stm32h7/board.c | 1 + board/bootstub.c | 2 +- board/config.h | 2 ++ board/drivers/usb.c | 4 +-- board/libc.c | 2 +- board/libc.h | 3 +++ board/stm32h7/clock.c | 14 +++++----- board/stm32h7/llfdcan.c | 2 +- board/stm32h7/llflash.c | 6 ++--- board/stm32h7/llusb.c | 8 +++--- crypto/rsa.c | 52 +++++++++++++++++++------------------- crypto/sha.c | 40 ++++++++++++++--------------- 12 files changed, 71 insertions(+), 65 deletions(-) diff --git a/board/body/stm32h7/board.c b/board/body/stm32h7/board.c index c48a58dbade..55026ddc3b6 100644 --- a/board/body/stm32h7/board.c +++ b/board/body/stm32h7/board.c @@ -1,4 +1,5 @@ #include "board.h" +#include "board/config.h" #ifndef PANDA_BODY #error This should only be used on Panda Body! diff --git a/board/bootstub.c b/board/bootstub.c index 69ad85688e5..7ffe6c3a995 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -73,7 +73,7 @@ int main(void) { // verify version, last bytes in the signed area uint32_t vers[2] = {0}; - memcpy(&vers, ((void*)&_app_start[0]) + len - sizeof(vers), sizeof(vers)); + (void)memcpy(&vers, ((void*)&_app_start[0]) + len - sizeof(vers), sizeof(vers)); if (vers[0] != VERS_TAG || vers[1] < MIN_VERSION) { goto fail; } diff --git a/board/config.h b/board/config.h index 0df9022cf40..b0218f3281d 100644 --- a/board/config.h +++ b/board/config.h @@ -73,3 +73,5 @@ // building for tests #include "fake_stm.h" #endif + +void detect_board_type(void); \ No newline at end of file diff --git a/board/drivers/usb.c b/board/drivers/usb.c index 71b5ea60644..e156c4a6c1c 100644 --- a/board/drivers/usb.c +++ b/board/drivers/usb.c @@ -105,10 +105,10 @@ static void usb_reset(void) { // flush TX fifo USBx->GRSTCTL = USB_OTG_GRSTCTL_TXFFLSH | USB_OTG_GRSTCTL_TXFNUM_4; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH) {} // flush RX FIFO USBx->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH) {} // no global NAK USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGINAK; diff --git a/board/libc.c b/board/libc.c index 2387e7106ff..de6484afe23 100644 --- a/board/libc.c +++ b/board/libc.c @@ -7,7 +7,7 @@ void puth(unsigned int i); void delay(uint32_t a) { volatile uint32_t i; - for (i = 0; i < a; i++); + for (i = 0; i < a; i++) {} } void assert_fatal(bool condition, const char *msg) { diff --git a/board/libc.h b/board/libc.h index 7bc3eb3db28..8e7f79ef5e5 100644 --- a/board/libc.h +++ b/board/libc.h @@ -10,8 +10,11 @@ void delay(uint32_t a); void assert_fatal(bool condition, const char *msg); +// cppcheck-suppress misra-c2012-21.2 void *memset(void *str, int c, unsigned int n); +// cppcheck-suppress misra-c2012-21.2 void *memcpy(void *dest, const void *src, unsigned int len); +// cppcheck-suppress misra-c2012-21.2 int memcmp(const void * ptr1, const void * ptr2, unsigned int num); void print(const char *a); void puth(unsigned int i); \ No newline at end of file diff --git a/board/stm32h7/clock.c b/board/stm32h7/clock.c index 5b5e3843d5c..8c4d13cf76d 100644 --- a/board/stm32h7/clock.c +++ b/board/stm32h7/clock.c @@ -62,22 +62,22 @@ void clock_init(void) { } else if (package_smps == PACKAGE_WITH_SMPS) { register_set(&(PWR->CR3), PWR_CR3_SMPSEN, 0xFU); // powered only by SMPS } else { - while(true); // unknown package, let's hang here + while(true) {} // unknown package, let's hang here } // Set VOS level (VOS3 to 170Mhz, VOS2 to 300Mhz, VOS1 to 400Mhz, VOS0 to 550Mhz) register_set(&(PWR->D3CR), PWR_D3CR_VOS_1 | PWR_D3CR_VOS_0, 0xC000U); //VOS1, needed for 80Mhz CAN FD - while ((PWR->CSR1 & PWR_CSR1_ACTVOSRDY) == 0U); - while ((PWR->CSR1 & PWR_CSR1_ACTVOS) != (PWR->D3CR & PWR_D3CR_VOS)); // check that VOS level was actually set + while ((PWR->CSR1 & PWR_CSR1_ACTVOSRDY) == 0U) {} + while ((PWR->CSR1 & PWR_CSR1_ACTVOS) != (PWR->D3CR & PWR_D3CR_VOS)) {} // check that VOS level was actually set // Configure Flash ACR register LATENCY and WRHIGHFREQ (VOS0 range!) register_set(&(FLASH->ACR), FLASH_ACR_LATENCY_2WS | 0x20U, 0x3FU); // VOS2, AXI 100MHz-150MHz // enable external oscillator HSE register_set_bits(&(RCC->CR), RCC_CR_HSEON); - while ((RCC->CR & RCC_CR_HSERDY) == 0U); + while ((RCC->CR & RCC_CR_HSERDY) == 0U) {} // enable internal HSI48 for USB FS kernel register_set_bits(&(RCC->CR), RCC_CR_HSI48ON); - while ((RCC->CR & RCC_CR_HSI48RDY) == 0U); + while ((RCC->CR & RCC_CR_HSI48RDY) == 0U) {} // Specify the frequency source for PLL1, divider for DIVM1, DIVM2, DIVM3 : HSE, 5, 5, 5 register_set(&(RCC->PLLCKSELR), RCC_PLLCKSELR_PLLSRC_HSE | RCC_PLLCKSELR_DIVM1_0 | RCC_PLLCKSELR_DIVM1_2 | RCC_PLLCKSELR_DIVM2_0 | RCC_PLLCKSELR_DIVM2_2 | RCC_PLLCKSELR_DIVM3_0 | RCC_PLLCKSELR_DIVM3_2, 0x3F3F3F3U); @@ -88,7 +88,7 @@ void clock_init(void) { register_set(&(RCC->PLLCFGR), RCC_PLLCFGR_PLL1RGE_2 | RCC_PLLCFGR_DIVP1EN | RCC_PLLCFGR_DIVQ1EN | RCC_PLLCFGR_DIVR1EN, 0x7000CU); // Enable PLL1 register_set_bits(&(RCC->CR), RCC_CR_PLL1ON); - while((RCC->CR & RCC_CR_PLL1RDY) == 0U); + while((RCC->CR & RCC_CR_PLL1RDY) == 0U) {} // *** PLL1 end *** //////////////OTHER CLOCKS//////////////////// @@ -101,7 +101,7 @@ void clock_init(void) { // Set SysClock source to PLL register_set(&(RCC->CFGR), RCC_CFGR_SW_PLL1, 0x7U); - while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL1); + while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL1) {} //////////////END OTHER CLOCKS//////////////////// // Configure clock source for USB (HSI48) diff --git a/board/stm32h7/llfdcan.c b/board/stm32h7/llfdcan.c index 609c56f1b76..a89cdf00066 100644 --- a/board/stm32h7/llfdcan.c +++ b/board/stm32h7/llfdcan.c @@ -8,7 +8,7 @@ static bool fdcan_request_init(FDCAN_GlobalTypeDef *FDCANx) { bool ret = true; // Exit from sleep mode FDCANx->CCCR &= ~(FDCAN_CCCR_CSR); - while ((FDCANx->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA); + while ((FDCANx->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA) {} // Request init uint32_t timeout_counter = 0U; diff --git a/board/stm32h7/llflash.c b/board/stm32h7/llflash.c index 21320632c97..1fe7e9be620 100644 --- a/board/stm32h7/llflash.c +++ b/board/stm32h7/llflash.c @@ -16,7 +16,7 @@ bool flash_erase_sector(uint8_t sector, bool unlocked) { if (sector != 0 && sector < 8 && unlocked) { FLASH->CR1 = (sector << 8) | FLASH_CR_SER; FLASH->CR1 |= FLASH_CR_START; - while (FLASH->SR1 & FLASH_SR_QW); + while (FLASH->SR1 & FLASH_SR_QW) {} return true; } return false; @@ -26,12 +26,12 @@ void flash_write_word(void *prog_ptr, uint32_t data) { uint32_t *pp = prog_ptr; FLASH->CR1 |= FLASH_CR_PG; *pp = data; - while (FLASH->SR1 & FLASH_SR_QW); + while (FLASH->SR1 & FLASH_SR_QW) {} } void flush_write_buffer(void) { if (FLASH->SR1 & FLASH_SR_WBNE) { FLASH->CR1 |= FLASH_CR_FW; - while (FLASH->SR1 & FLASH_CR_FW); + while (FLASH->SR1 & FLASH_CR_FW) {} } } diff --git a/board/stm32h7/llusb.c b/board/stm32h7/llusb.c index 6744c8aeb78..c7f5286d967 100644 --- a/board/stm32h7/llusb.c +++ b/board/stm32h7/llusb.c @@ -20,10 +20,10 @@ void usb_init(void) { USBx->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD; delay(250000); // Wait for about 25ms (explicitly stated in H7 ref manual) // Wait for AHB master IDLE state. - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0U); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0U) {} // Core Soft Reset USBx->GRSTCTL |= USB_OTG_GRSTCTL_CSRST; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_CSRST) == USB_OTG_GRSTCTL_CSRST); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_CSRST) == USB_OTG_GRSTCTL_CSRST) {} // Activate the USB Transceiver USBx->GCCFG |= USB_OTG_GCCFG_PWRDWN; @@ -46,10 +46,10 @@ void usb_init(void) { // Flush FIFOs USBx->GRSTCTL = (USB_OTG_GRSTCTL_TXFFLSH | (0x10U << 6)); - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH) {} USBx->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH) {} // Clear all pending Device Interrupts USBx_DEVICE->DIEPMSK = 0U; diff --git a/crypto/rsa.c b/crypto/rsa.c index 24171e87909..047fcf5e749 100644 --- a/crypto/rsa.c +++ b/crypto/rsa.c @@ -27,13 +27,13 @@ #include "rsa.h" #include "sha.h" +#include "board/utils.h" // a[] -= mod static void subM(const RSAPublicKey* key, uint32_t* a) { int64_t A = 0; - int i; - for (i = 0; i < key->len; ++i) { + for (int i = 0; i < key->len; ++i) { A += (uint64_t)a[i] - key->n[i]; a[i] = (uint32_t)A; A >>= 32; @@ -43,9 +43,8 @@ static void subM(const RSAPublicKey* key, // return a[] >= mod static int geM(const RSAPublicKey* key, const uint32_t* a) { - int i; - for (i = key->len; i;) { - --i; + + for (int i = key->len; i; --i) { if (a[i] < key->n[i]) return 0; if (a[i] > key->n[i]) return 1; } @@ -98,7 +97,7 @@ static void modpow(const RSAPublicKey* key, uint32_t a[RSANUMWORDS]; uint32_t aR[RSANUMWORDS]; uint32_t aaR[RSANUMWORDS]; - uint32_t* aaa = 0; + uint32_t* aaa = NULL; int i; // Convert from big endian byte array to little endian word array. @@ -184,26 +183,6 @@ static const uint8_t sha_padding[RSANUMBYTES] = { }; */ -static const uint8_t sha_padding_1024[RSANUMBYTES] = { - 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0x00, - - // 20 bytes of hash go here. - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -}; - // SHA-1 of PKCS1.5 signature sha_padding for 2048 bit, as above. // At the location of the bytes of the hash all 00 are hashed. /*static const uint8_t kExpectedPadShaRsa2048[SHA_DIGEST_SIZE] = { @@ -224,6 +203,27 @@ int RSA_verify(const RSAPublicKey *key, const int len, const uint8_t *hash, const int hash_len) { + + static const uint8_t sha_padding_1024[RSANUMBYTES] = { + 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0x00, + + // 20 bytes of hash go here. + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 + }; + uint8_t buf[RSANUMBYTES]; int i; //const uint8_t* padding_hash; diff --git a/crypto/sha.c b/crypto/sha.c index a13162c5fac..e0ce3733250 100644 --- a/crypto/sha.c +++ b/crypto/sha.c @@ -27,9 +27,8 @@ // Optimized for minimal code size. -void *memcpy(void *str1, const void *str2, unsigned int n); - #include "sha.h" +#include "board/libc.h" #define rol(bits, value) (((value) << (bits)) | ((value) >> (32 - (bits)))) @@ -60,14 +59,15 @@ static void SHA1_Transform(SHA_CTX* ctx) { for(t = 0; t < 80; t++) { uint32_t tmp = rol(5,A) + E + W[t]; - if (t < 20) + if (t < 20){ tmp += (D^(B&(C^D))) + 0x5A827999; - else if ( t < 40) + } else if ( t < 40) { tmp += (B^C^D) + 0x6ED9EBA1; - else if ( t < 60) + } else if ( t < 60) { tmp += ((B&C)|(D&(B|C))) + 0x8F1BBCDC; - else + } else { tmp += (B^C^D) + 0xCA62C1D6; + } E = D; D = C; @@ -93,11 +93,11 @@ static const HASH_VTAB SHA_VTAB = { void SHA_init(SHA_CTX* ctx) { ctx->f = &SHA_VTAB; - ctx->state[0] = 0x67452301; - ctx->state[1] = 0xEFCDAB89; - ctx->state[2] = 0x98BADCFE; - ctx->state[3] = 0x10325476; - ctx->state[4] = 0xC3D2E1F0; + ctx->state[0] = 0x67452301u; + ctx->state[1] = 0xEFCDAB89u; + ctx->state[2] = 0x98BADCFEu; + ctx->state[3] = 0x10325476u; + ctx->state[4] = 0xC3D2E1F0u; ctx->count = 0; } @@ -142,21 +142,21 @@ const uint8_t* SHA_final(SHA_CTX* ctx) { uint8_t tmp = 0; tmp = (uint8_t) (cnt >> ((7 - 0) * 8)); - SHA_update(ctx, &tmp, 1); + SHA_update(ctx, (void*)&tmp, 1); tmp = (uint8_t) (cnt >> ((7 - 1) * 8)); - SHA_update(ctx, &tmp, 1); + SHA_update(ctx, (void*)&tmp, 1); tmp = (uint8_t) (cnt >> ((7 - 2) * 8)); - SHA_update(ctx, &tmp, 1); + SHA_update(ctx, (void*)&tmp, 1); tmp = (uint8_t) (cnt >> ((7 - 3) * 8)); - SHA_update(ctx, &tmp, 1); + SHA_update(ctx, (void*)&tmp, 1); tmp = (uint8_t) (cnt >> ((7 - 4) * 8)); - SHA_update(ctx, &tmp, 1); + SHA_update(ctx, (void*)&tmp, 1); tmp = (uint8_t) (cnt >> ((7 - 5) * 8)); - SHA_update(ctx, &tmp, 1); + SHA_update(ctx, (void*)&tmp, 1); tmp = (uint8_t) (cnt >> ((7 - 6) * 8)); - SHA_update(ctx, &tmp, 1); + SHA_update(ctx, (void*)&tmp, 1); tmp = (uint8_t) (cnt >> ((7 - 7) * 8)); - SHA_update(ctx, &tmp, 1); + SHA_update(ctx, (void*)&tmp, 1); for (i = 0; i < 5; i++) { uint32_t tmp = ctx->state[i]; @@ -174,6 +174,6 @@ const uint8_t* SHA_hash(const void* data, int len, uint8_t* digest) { SHA_CTX ctx; SHA_init(&ctx); SHA_update(&ctx, data, len); - memcpy(digest, SHA_final(&ctx), SHA_DIGEST_SIZE); + (void)memcpy(digest, SHA_final(&ctx), SHA_DIGEST_SIZE); return digest; } From 55e9725c6a0bbc58b399fcf98d63f820595acbdb Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 11:59:58 +0100 Subject: [PATCH 09/29] Analyze all files with misra --- tests/misra/test_misra.sh | 19 ++++++------------- 1 file changed, 6 insertions(+), 13 deletions(-) diff --git a/tests/misra/test_misra.sh b/tests/misra/test_misra.sh index 5a91802cb41..89ff7e375ea 100755 --- a/tests/misra/test_misra.sh +++ b/tests/misra/test_misra.sh @@ -36,8 +36,6 @@ CHECKLIST=$DIR/checkers.txt echo "Cppcheck checkers list from test_misra.sh:" > $CHECKLIST cppcheck() { - # get all gcc defines: arm-none-eabi-gcc -dM -E - < /dev/null - COMMON_DEFINES="-D__GNUC__=9 -UCMSIS_NVIC_VIRTUAL -UCMSIS_VECTAB_VIRTUAL -UPANDA_JUNGLE -UBOOTSTUB" # note that cppcheck build cache results in inconsistent results as of v2.13.0 OUTPUT=$DIR/.output.log @@ -46,13 +44,12 @@ cppcheck() { echo -e ""${@//$PANDA_DIR/}"\n\n" >> $CHECKLIST # (absolute path removed) $CPPCHECK_DIR/cppcheck --inline-suppr \ - -I $PANDA_DIR \ - -I "$(arm-none-eabi-gcc -print-file-name=include)" \ - -I $OPENDBC_ROOT \ + --enable=all --addon=misra \ + -j `nproc` \ --suppressions-list=$DIR/suppressions.txt --suppress=*:*inc/* \ --suppress=*:*include/* --error-exitcode=2 --check-level=exhaustive --safety \ - --platform=arm32-wchar_t4 $COMMON_DEFINES --checkers-report=$CHECKLIST.tmp \ - --std=c11 "$@" 2>&1 | tee $OUTPUT + --platform=arm32-wchar_t4 --checkers-report=$CHECKLIST.tmp \ + --std=c11 --project=compile_commands.json --file-filter=*.c 2>&1 | tee $OUTPUT cat $CHECKLIST.tmp >> $CHECKLIST rm $CHECKLIST.tmp @@ -64,14 +61,10 @@ cppcheck() { fi } -PANDA_OPTS="--enable=all --disable=unusedFunction --addon=misra" +PANDA_OPTS="--enable=all --addon=misra" printf "\n${GREEN}** PANDA H7 CODE **${NC}\n" -cppcheck $PANDA_OPTS -DSTM32H7 -DSTM32H725xx -I $PANDA_DIR/board/stm32h7/inc/ $PANDA_DIR/board/main.c -DPANDA - -# unused needs to run globally -#printf "\n${GREEN}** UNUSED ALL CODE **${NC}\n" -#cppcheck --enable=unusedFunction --quiet $PANDA_DIR/board/ +cppcheck $PANDA_OPTS printf "\n${GREEN}Success!${NC} took $SECONDS seconds\n" From 54f27080ae9aa2af397856e31dbac58ab748c98f Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 14:32:13 +0100 Subject: [PATCH 10/29] Cleanup --- board/board_struct.h | 1 - board/boards/cuatro.c | 6 ++--- board/boards/red.h | 1 - board/boards/tres.h | 11 ++++---- board/body/main.c | 4 --- board/bootstub.c | 17 +------------ board/can_comms.c | 12 ++++----- board/config.h | 4 +-- board/crc.c | 20 ++------------- board/crc.h | 2 +- board/drivers/harness.h | 2 -- board/drivers/simple_watchdog.c | 19 +------------- board/drivers/simple_watchdog.h | 4 +-- board/drivers/spi.c | 18 +++++++++++++ board/early_init.c | 2 +- board/fake_stm.c | 2 -- board/faults.c | 2 +- board/flasher.c | 2 +- board/globals.h | 3 ++- board/jungle/boards/board_declarations.h | 28 --------------------- board/jungle/main_comms.h | 5 ---- board/main.c | 32 ++++++++++++++---------- board/main_comms.c | 16 ++++++------ board/main_definitions.c | 10 +++----- board/stm32h7/board.c | 3 ++- board/stm32h7/board.h | 5 ++-- board/stm32h7/lladc.h | 3 +-- board/stm32h7/stm32h7_config.h | 1 - crypto/sha.c | 14 +++++------ 29 files changed, 86 insertions(+), 163 deletions(-) diff --git a/board/board_struct.h b/board/board_struct.h index c7b72c37690..834578e8bb2 100644 --- a/board/board_struct.h +++ b/board/board_struct.h @@ -1,7 +1,6 @@ #pragma once #include "board/config.h" - #include "board/board_forward.h" #include "board/boards/boot_state.h" diff --git a/board/boards/cuatro.c b/board/boards/cuatro.c index e27eec8c28b..092ce4cadcf 100644 --- a/board/boards/cuatro.c +++ b/board/boards/cuatro.c @@ -5,6 +5,8 @@ #include "board/board_struct.h" #include "board/boards/unused_funcs.h" #include "board/drivers/fake_siren.h" +#include "board/boards/tres.h" +#include "board/drivers/harness.h" // ////////////////////////// // // Cuatro (STM32H7) + Harness // @@ -118,10 +120,6 @@ static harness_configuration cuatro_harness_config = { .adc_signal_SBU2 = ADC_CHANNEL_DEFAULT(ADC1, 17) }; -// TODO -void tres_set_can_mode(uint8_t mode); -bool tres_read_som_gpio (void); - struct board board_cuatro = { .harness_config = &cuatro_harness_config, .has_spi = true, diff --git a/board/boards/red.h b/board/boards/red.h index e00e163b107..4c94f3025c1 100644 --- a/board/boards/red.h +++ b/board/boards/red.h @@ -1,6 +1,5 @@ #pragma once -// #include "board_declarations.h" #include "board/drivers/harness.h" // ///////////////////////////// // diff --git a/board/boards/tres.h b/board/boards/tres.h index 73f90e379cc..51343fb9630 100644 --- a/board/boards/tres.h +++ b/board/boards/tres.h @@ -1,8 +1,9 @@ #pragma once -#include "board/board_struct.h" +#include +#include -// /////////////////////////// -// Tres (STM32H7) + Harness // -// /////////////////////////// -extern struct board board_tres; \ No newline at end of file +extern struct board board_tres; + +void tres_set_can_mode(uint8_t mode); +bool tres_read_som_gpio (void); diff --git a/board/body/main.c b/board/body/main.c index 68ed46944b1..6d582942764 100644 --- a/board/body/main.c +++ b/board/body/main.c @@ -13,12 +13,8 @@ #include "board/drivers/can_common.h" #include "board/drivers/fdcan.h" #include "board/can_comms.h" - -// TODO #include "board/body/boards/board_body.h" -extern int _app_start[0xc000]; - #include "board/body/main_comms.h" void debug_ring_callback(uart_ring *ring) { diff --git a/board/bootstub.c b/board/bootstub.c index 7ffe6c3a995..8b8a8954838 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -4,33 +4,21 @@ // ********************* Includes ********************* #include -// #include "board/bootstub_declarations.h" - -// #include "board/config.h" - -// #include "board/drivers/led.h" -// #include "board/drivers/pwm.h" -#include "board/drivers/usb.h" - #include "board/early_init.h" -// #include "board/provision.h" +#include "board/flasher.h" #include "crypto/rsa.h" #include "crypto/sha.h" #include "board/obj/cert.h" -// #include "board/obj/gitversion.h" -#include "board/flasher.h" #include "globals.h" - // cppcheck-suppress unusedFunction ; used in headers not included in cppcheck void __initialize_hardware_early(void) { early_initialization(); } -// TODO void print(const char *a) { UNUSED(a); } @@ -43,9 +31,6 @@ void fail(void) { soft_flasher_start(); } -// know where to sig check -extern void *_app_start[]; - int main(void) { // Init interrupt table init_interrupts(true); diff --git a/board/can_comms.c b/board/can_comms.c index 2304ad13522..8e12f1a5857 100644 --- a/board/can_comms.c +++ b/board/can_comms.c @@ -1,12 +1,12 @@ -#include "can_comms.h" +// #include "can_comms.h" #include "drivers/can_common.h" -#include "utils.h" -#include "libc.h" -#include "can.h" +// #include "utils.h" +// #include "libc.h" +// #include "can.h" #include "drivers/usb.h" -#include "board/drivers/spi.h" +// #include "board/drivers/spi.h" -#include "config.h" +// #include "config.h" static asm_buffer can_read_buffer = {.ptr = 0U, .tail_size = 0U}; diff --git a/board/config.h b/board/config.h index b0218f3281d..ca099f90924 100644 --- a/board/config.h +++ b/board/config.h @@ -60,16 +60,14 @@ #elif defined(PANDA_BODY) #elif defined(LIB_PANDA) -// NO IDEA BOI #else -#error FUCK YOU +#error Unknown board type #endif // platform includes #ifdef STM32H7 #include "board/stm32h7/stm32h7_config.h" #else - // TODO: uncomment this, cppcheck complains // building for tests #include "fake_stm.h" #endif diff --git a/board/crc.c b/board/crc.c index 03668fd1476..dd627f5353e 100644 --- a/board/crc.c +++ b/board/crc.c @@ -1,19 +1,3 @@ -#include "crc.h" +// #include "crc.h" + -uint8_t crc_checksum(const uint8_t *dat, int len, const uint8_t poly) { - uint8_t crc = 0xFFU; - int i; - int j; - for (i = len - 1; i >= 0; i--) { - crc ^= dat[i]; - for (j = 0; j < 8; j++) { - if ((crc & 0x80U) != 0U) { - crc = (uint8_t)((crc << 1) ^ poly); - } - else { - crc <<= 1; - } - } - } - return crc; -} diff --git a/board/crc.h b/board/crc.h index c702fe03e59..b1fc1de823b 100644 --- a/board/crc.h +++ b/board/crc.h @@ -2,4 +2,4 @@ #include -uint8_t crc_checksum(const uint8_t *dat, int len, const uint8_t poly); \ No newline at end of file +// uint8_t crc_checksum(const uint8_t *dat, int len, const uint8_t poly); \ No newline at end of file diff --git a/board/drivers/harness.h b/board/drivers/harness.h index 811253ce7b4..b00e331124a 100644 --- a/board/drivers/harness.h +++ b/board/drivers/harness.h @@ -4,8 +4,6 @@ #include #include "board/config.h" -// TODO -#include "board/stm32h7/lladc.h" #define HARNESS_STATUS_NC 0U #define HARNESS_STATUS_NORMAL 1U diff --git a/board/drivers/simple_watchdog.c b/board/drivers/simple_watchdog.c index 54635428c1f..554ae14ef6d 100644 --- a/board/drivers/simple_watchdog.c +++ b/board/drivers/simple_watchdog.c @@ -7,22 +7,5 @@ #include "board/libc.h" #include "board/faults.h" -static simple_watchdog_state_t wd_state; +simple_watchdog_state_t wd_state; -void simple_watchdog_kick(void) { - uint32_t ts = microsecond_timer_get(); - - uint32_t et = get_ts_elapsed(ts, wd_state.last_ts); - if (et > wd_state.threshold) { - print("WD timeout 0x"); puth(et); print("\n"); - fault_occurred(wd_state.fault); - } - - wd_state.last_ts = ts; -} - -void simple_watchdog_init(uint32_t fault, uint32_t threshold) { - wd_state.fault = fault; - wd_state.threshold = threshold; - wd_state.last_ts = microsecond_timer_get(); -} diff --git a/board/drivers/simple_watchdog.h b/board/drivers/simple_watchdog.h index 3ae2ab90b4b..be762f61d57 100644 --- a/board/drivers/simple_watchdog.h +++ b/board/drivers/simple_watchdog.h @@ -8,6 +8,4 @@ typedef struct simple_watchdog_state_t { uint32_t threshold; } simple_watchdog_state_t; -void simple_watchdog_kick(void); -void simple_watchdog_init(uint32_t fault, uint32_t threshold); - +extern simple_watchdog_state_t wd_state; \ No newline at end of file diff --git a/board/drivers/spi.c b/board/drivers/spi.c index b5fb39afc96..089c33c1acb 100644 --- a/board/drivers/spi.c +++ b/board/drivers/spi.c @@ -16,6 +16,24 @@ static uint16_t spi_data_len_mosi; static bool spi_can_tx_ready = false; static const unsigned char version_text[] = "VERSION"; +static uint8_t crc_checksum(const uint8_t *dat, int len, const uint8_t poly) { + uint8_t crc = 0xFFU; + int i; + int j; + for (i = len - 1; i >= 0; i--) { + crc ^= dat[i]; + for (j = 0; j < 8; j++) { + if ((crc & 0x80U) != 0U) { + crc = (uint8_t)((crc << 1) ^ poly); + } + else { + crc <<= 1; + } + } + } + return crc; +} + 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 diff --git a/board/early_init.c b/board/early_init.c index a6e420f599d..6a8a83d8c53 100644 --- a/board/early_init.c +++ b/board/early_init.c @@ -3,7 +3,7 @@ #include "early_init.h" #include "drivers/led.h" #include "globals.h" -#include "board/config.h" +// #include "board/config.h" extern void *g_pfnVectors; extern uint32_t enter_bootloader_mode; diff --git a/board/fake_stm.c b/board/fake_stm.c index d30651d6afc..a80eef21aeb 100644 --- a/board/fake_stm.c +++ b/board/fake_stm.c @@ -17,6 +17,4 @@ uint32_t microsecond_timer_get(void) { return MICROSECOND_TIMER->CNT; } - -// NOCHECKIN void put_char(char c) {}; \ No newline at end of file diff --git a/board/faults.c b/board/faults.c index 6ce6a59b501..9488d3b61d2 100644 --- a/board/faults.c +++ b/board/faults.c @@ -1,4 +1,4 @@ -#include "faults.h" +// #include "faults.h" #include "board/drivers/registers.h" uint8_t fault_status = FAULT_STATUS_NONE; diff --git a/board/flasher.c b/board/flasher.c index 712634360c6..6c39405b758 100644 --- a/board/flasher.c +++ b/board/flasher.c @@ -1,5 +1,5 @@ #include "flasher.h" -#include "utils.h" +// #include "utils.h" #include "libc.h" #include "board/stm32h7/llflash.h" #include "drivers/led.h" diff --git a/board/globals.h b/board/globals.h index ea640a9be8c..02df9cc8898 100644 --- a/board/globals.h +++ b/board/globals.h @@ -10,4 +10,5 @@ typedef struct board board; extern uint8_t hw_type; extern board *current_board; extern struct harness_t harness; -extern uint32_t uptime_cnt; \ No newline at end of file +extern uint32_t uptime_cnt; +extern int _app_start[0xc000]; \ No newline at end of file diff --git a/board/jungle/boards/board_declarations.h b/board/jungle/boards/board_declarations.h index 329608090df..a3cfb475f70 100644 --- a/board/jungle/boards/board_declarations.h +++ b/board/jungle/boards/board_declarations.h @@ -5,34 +5,6 @@ #include "board/config.h" #include "board/board_struct.h" -// #include "board_forward.h" - -// struct board { -// GPIO_TypeDef * const led_GPIO[3]; -// const uint8_t led_pin[3]; -// const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM -// const uint16_t avdd_mV; -// board_init init; -// board_board_tick board_tick; -// board_get_button get_button; -// board_init_bootloader init_bootloader; -// board_set_panda_power set_panda_power; -// board_set_panda_individual_power set_panda_individual_power; -// board_set_ignition set_ignition; -// board_set_individual_ignition set_individual_ignition; -// board_set_harness_orientation set_harness_orientation; -// board_set_can_mode set_can_mode; -// board_enable_can_transceiver enable_can_transceiver; -// board_enable_header_pin enable_header_pin; -// board_get_channel_power get_channel_power; -// board_get_sbu_mV get_sbu_mV; - -// // TODO: shouldn't need these -// bool has_spi; -// }; - -// ******************* Definitions ******************** - // ********************* Globals ********************** extern uint8_t harness_orientation; extern uint8_t can_mode; diff --git a/board/jungle/main_comms.h b/board/jungle/main_comms.h index e069d097526..475c94929cc 100644 --- a/board/jungle/main_comms.h +++ b/board/jungle/main_comms.h @@ -1,15 +1,10 @@ #include "board/config.h" -extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used - - #include "board/globals.h" -// TODO: extern bool generated_can_traffic; extern bool panda_power; - // send on serial, first byte to select the ring void comms_endpoint2_write(const uint8_t *data, uint32_t len); int comms_control_handler(ControlPacket_t *req, uint8_t *resp); \ No newline at end of file diff --git a/board/main.c b/board/main.c index 964c0b9c9fa..a6c20ce05d4 100644 --- a/board/main.c +++ b/board/main.c @@ -3,14 +3,12 @@ #include "board/drivers/led.h" #include "board/drivers/pwm.h" -// #include "board/drivers/usb.h" #include "board/drivers/simple_watchdog.h" #include "board/drivers/bootkick.h" #include "board/early_init.h" -// #include "board/provision.h" -// #include "libc.h" +#include "board/libc.h" #include "board/power_saving.h" #include "board/globals.h" @@ -18,19 +16,27 @@ #include "opendbc/safety/safety.h" #include "board/drivers/can_common.h" #include "board/drivers/fdcan.h" -#include "board/obj/gitversion.h" +#include "board/main_declarations.h" -// #include "board/can_comms.h" -#include "board/main_comms.h" -// #include "board/main_declarations.h" +static void simple_watchdog_kick(void) { + uint32_t ts = microsecond_timer_get(); -// TODO -extern uint32_t heartbeat_counter; -extern bool heartbeat_lost; -extern bool heartbeat_disabled; -extern bool siren_enabled; -uint32_t uptime_cnt; + uint32_t et = get_ts_elapsed(ts, wd_state.last_ts); + if (et > wd_state.threshold) { + print("WD timeout 0x"); puth(et); print("\n"); + fault_occurred(wd_state.fault); + } + + wd_state.last_ts = ts; +} + + +static void simple_watchdog_init(uint32_t fault, uint32_t threshold) { + wd_state.fault = fault; + wd_state.threshold = threshold; + wd_state.last_ts = microsecond_timer_get(); +} // ********************* Serial debugging ********************* diff --git a/board/main_comms.c b/board/main_comms.c index 14a2b4986e6..30323bed8b3 100644 --- a/board/main_comms.c +++ b/board/main_comms.c @@ -1,24 +1,24 @@ #include #include -#include "board/utils.h" -#include "board/config.h" -#include "board/health.h" +// #include "board/utils.h" +// #include "board/config.h" +// #include "board/health.h" #include "board/main_declarations.h" #include "board/drivers/can_common.h" #include "board/power_saving.h" -#include "board/drivers/spi.h" +// #include "board/drivers/spi.h" #include "board/drivers/bootkick.h" #include "board/drivers/fdcan.h" #include "board/provision.h" #include "board/early_init.h" #include "board/obj/gitversion.h" -#include "board/drivers/fan.h" -#include "board/drivers/clock_source.h" +// #include "board/drivers/fan.h" +// #include "board/drivers/clock_source.h" #include "board/globals.h" -#include "board/drivers/uart.h" +// #include "board/drivers/uart.h" -#include "board/stm32h7/llfdcan.h" +// #include "board/stm32h7/llfdcan.h" #include "board/main_comms.h" extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used diff --git a/board/main_definitions.c b/board/main_definitions.c index 6f082084aba..e86608d8885 100644 --- a/board/main_definitions.c +++ b/board/main_definitions.c @@ -1,11 +1,7 @@ -#include "main_declarations.h" +#include +#include -#include "board/board_struct.h" - -// ********************* Globals ********************** -// uint8_t hw_type = 0; -// board *current_board; -// uint32_t uptime_cnt = 0; +uint32_t uptime_cnt = 0; // heartbeat state uint32_t heartbeat_counter = 0; diff --git a/board/stm32h7/board.c b/board/stm32h7/board.c index b86d4dc9c5b..3ac48a2f97f 100644 --- a/board/stm32h7/board.c +++ b/board/stm32h7/board.c @@ -2,7 +2,8 @@ #include "board/stm32h7/board.h" #include "board/globals.h" - +#include "board/boards/red.h" +#include "board/boards/tres.h" void detect_board_type(void) { // On STM32H7 pandas, we use two different sets of pins. diff --git a/board/stm32h7/board.h b/board/stm32h7/board.h index 73ea1cde475..f98c665ead0 100644 --- a/board/stm32h7/board.h +++ b/board/stm32h7/board.h @@ -6,14 +6,13 @@ // ///// Board definition and detection ///// // #include "board/stm32h7/lladc.h" -#include "board/drivers/harness.h" #include "board/drivers/fan.h" #include "board/stm32h7/llfan.h" #include "board/stm32h7/sound.h" #include "board/drivers/fake_siren.h" #include "board/drivers/clock_source.h" -#include "board/boards/red.h" -#include "board/boards/tres.h" +// #include "board/boards/red.h" +// #include "board/boards/tres.h" #include "board/boards/cuatro.h" void detect_board_type(void); \ No newline at end of file diff --git a/board/stm32h7/lladc.h b/board/stm32h7/lladc.h index beea312e96a..cafde667ecf 100644 --- a/board/stm32h7/lladc.h +++ b/board/stm32h7/lladc.h @@ -2,8 +2,7 @@ #include -// TODO: I don't know man... -#include "stm32h7xx.h" +#include "board/stm32h7/stm32h7_config.h" typedef enum { SAMPLETIME_1_CYCLE = 0, diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h index 7cc301f6fa5..534f4d9e737 100644 --- a/board/stm32h7/stm32h7_config.h +++ b/board/stm32h7/stm32h7_config.h @@ -73,7 +73,6 @@ separate IRQs for RX and TX. #include "board/stm32h7/lladc.h" #include "board/drivers/clock_source.h" #include "board/stm32h7/sound.h" -#include "board/drivers/harness.h" #else #include "board/stm32h7/board.h" #endif diff --git a/crypto/sha.c b/crypto/sha.c index e0ce3733250..9aff2ecedff 100644 --- a/crypto/sha.c +++ b/crypto/sha.c @@ -60,13 +60,13 @@ static void SHA1_Transform(SHA_CTX* ctx) { uint32_t tmp = rol(5,A) + E + W[t]; if (t < 20){ - tmp += (D^(B&(C^D))) + 0x5A827999; + tmp += (D^(B&(C^D))) + 0x5A827999u; } else if ( t < 40) { - tmp += (B^C^D) + 0x6ED9EBA1; + tmp += (B^C^D) + 0x6ED9EBA1u; } else if ( t < 60) { - tmp += ((B&C)|(D&(B|C))) + 0x8F1BBCDC; + tmp += ((B&C)|(D&(B|C))) + 0x8F1BBCDCu; } else { - tmp += (B^C^D) + 0xCA62C1D6; + tmp += (B^C^D) + 0xCA62C1D6u; } E = D; @@ -108,7 +108,7 @@ void SHA_update(SHA_CTX* ctx, const void* data, int len) { ctx->count += len; - while (len--) { + while (len-- > 0) { ctx->buf[i++] = *p++; if (i == 64) { SHA1_Transform(ctx); @@ -123,9 +123,9 @@ const uint8_t* SHA_final(SHA_CTX* ctx) { uint64_t cnt = ctx->count * 8; int i; - SHA_update(ctx, (uint8_t*)"\x80", 1); + SHA_update(ctx, (const uint8_t*)"\x80", 1); while ((ctx->count & 63) != 56) { - SHA_update(ctx, (uint8_t*)"\0", 1); + SHA_update(ctx, (const uint8_t*)"\0", 1); } /* Hack - right shift operator with non const argument requires From b00615a29e54de640aa06b969ed64afdb8b10620 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 14:37:39 +0100 Subject: [PATCH 11/29] Build script cleanup --- SConscript | 131 ++++++++++++++++++-------------------- tests/libpanda/SConscript | 12 ++-- 2 files changed, 68 insertions(+), 75 deletions(-) diff --git a/SConscript b/SConscript index fb7bcc174f4..fc9a5cf92ef 100644 --- a/SConscript +++ b/SConscript @@ -101,39 +101,37 @@ def build_project(project_name, project, main, shared, extra_flags): startup = env.Object(project["STARTUP_FILE"]) shared += [ - "./crypto/rsa.c", - "./crypto/sha.c", - "./board/libc.c", - "./board/crc.c", - "./board/early_init.c", - "./board/critical.c", - "./board/drivers/led.c", - "./board/drivers/pwm.c", - "./board/drivers/gpio.c", - "./board/drivers/fake_siren.c", - "./board/stm32h7/lli2c.c", - "./board/stm32h7/clock.c", - "./board/drivers/clock_source.c", - "./board/stm32h7/sound.c", - "./board/stm32h7/llflash.c", - "./board/stm32h7/stm32h7_config.c", - "./board/drivers/registers.c", - "./board/drivers/interrupts.c", - "./board/provision.c", - "./board/stm32h7/peripherals.c", - "./board/stm32h7/llusb.c", - "./board/drivers/usb.c", - "./board/drivers/spi.c", - "./board/drivers/timers.c", - "./board/stm32h7/lladc.c", - "./board/stm32h7/llspi.c", - "./board/faults.c", - "./board/boards/unused_funcs.c", - "./board/utils.c", - "./board/globals.c", - + "crypto/rsa.c", + "crypto/sha.c", + "board/libc.c", + "board/crc.c", + "board/early_init.c", + "board/critical.c", + "board/drivers/led.c", + "board/drivers/pwm.c", + "board/drivers/gpio.c", + "board/drivers/fake_siren.c", + "board/stm32h7/lli2c.c", + "board/stm32h7/clock.c", + "board/drivers/clock_source.c", + "board/stm32h7/sound.c", + "board/stm32h7/llflash.c", + "board/stm32h7/stm32h7_config.c", + "board/drivers/registers.c", + "board/drivers/interrupts.c", + "board/provision.c", + "board/stm32h7/peripherals.c", + "board/stm32h7/llusb.c", + "board/drivers/usb.c", + "board/drivers/spi.c", + "board/drivers/timers.c", + "board/stm32h7/lladc.c", + "board/stm32h7/llspi.c", + "board/faults.c", + "board/boards/unused_funcs.c", + "board/utils.c", + "board/globals.c", ] - # Build bootstub bs_env = env.Clone() @@ -143,36 +141,32 @@ def build_project(project_name, project, main, shared, extra_flags): bs_env.Append(CFLAGS="-DBOOTSTUB", ASFLAGS="-DBOOTSTUB", LINKFLAGS="-DBOOTSTUB") bs_elf = bs_env.Program(f"{project_dir}/bootstub.elf", [ startup, - "./board/bootstub.c", - "./board/flasher.c", + "board/bootstub.c", + "board/flasher.c", ] + shared) bs_env.Objcopy(f"board/obj/bootstub.{project_name}.bin", bs_elf) # Build + sign main (aka app) main_elf = env.Program(f"{project_dir}/main.elf", [ startup, - "./board/can_comms.c", - "./board/drivers/fan.c", - - "./board/power_saving.c", - "./board/drivers/uart.c", - - "./board/stm32h7/llfdcan.c", - "./board/drivers/harness.c", - "./board/drivers/simple_watchdog.c", - "./board/drivers/bootkick.c", - "./board/stm32h7/llfan.c", - "./board/stm32h7/lluart.c", - "./board/drivers/fdcan.c", - "./board/drivers/can_common.c", + "board/can_comms.c", + "board/drivers/fan.c", + "board/power_saving.c", + "board/drivers/uart.c", + "board/stm32h7/llfdcan.c", + "board/drivers/harness.c", + "board/drivers/simple_watchdog.c", + "board/drivers/bootkick.c", + "board/stm32h7/llfan.c", + "board/stm32h7/lluart.c", + "board/drivers/fdcan.c", + "board/drivers/can_common.c", main, ] + shared, LINKFLAGS=[f"-Wl,--section-start,.isr_vector={project['APP_START_ADDRESS']}"] + flags) main_bin = env.Objcopy(f"{project_dir}/main.bin", main_elf) sign_py = File(f"crypto/sign.py").srcnode().relpath env.Command(f"board/obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}") - - base_project_h7 = { "STARTUP_FILE": "board/stm32h7/startup_stm32h7x5xx.s", "LINKER_SCRIPT": "board/stm32h7/stm32h7x5_flash.ld", @@ -200,7 +194,12 @@ with open("board/obj/cert.h", "w") as f: for cert in certs: f.write("\n".join(cert) + "\n") -shared = [ +panda_main = [ + "board/main_comms.c", + "board/main.c" +] + +panda_shared = [ "board/stm32h7/board.c", "board/boards/tres.c", "board/boards/red.c", @@ -208,44 +207,38 @@ shared = [ "board/main_definitions.c" ] -project = [ - "board/main_comms.c", - "board/main.c" -] +build_project("panda_h7", base_project_h7, panda_main, panda_shared, ["-DPANDA"]) -build_project("panda_h7", base_project_h7, project, shared, ["-DPANDA"]) - -# # panda jungle fw +# panda jungle fw flags = [ "-DPANDA_JUNGLE", ] if os.getenv("FINAL_PROVISIONING"): flags += ["-DFINAL_PROVISIONING"] -shared = [ - "board/jungle/stm32h7/board.c", - "board/jungle/boards/board_v2.c", -] -project = [ +jungle_main = [ "board/jungle/main_comms.c", "board/jungle/main.c", ] -build_project("panda_jungle_h7", base_project_h7, project, shared, flags) +jungle_shared = [ + "board/jungle/stm32h7/board.c", + "board/jungle/boards/board_v2.c", +] +build_project("panda_jungle_h7", base_project_h7, jungle_main, jungle_shared, flags) -# # body fw -body_c = [ +# body fw +body_main = [ "board/body/main.c", "board/body/main_comms.c", ] - -shared = [ +body_shared = [ "board/body/boards/board_body.c", "board/body/stm32h7/board.c", "board/body/motor_control.c", "board/body/motor_encoder.c" ] -build_project("body_h7", base_project_h7, body_c, shared, ["-DPANDA_BODY"]) +build_project("body_h7", base_project_h7, body_main, body_shared, ["-DPANDA_BODY"]) # test files if GetOption('extras'): diff --git a/tests/libpanda/SConscript b/tests/libpanda/SConscript index b37da4e3e65..381a3257677 100644 --- a/tests/libpanda/SConscript +++ b/tests/libpanda/SConscript @@ -36,12 +36,12 @@ if GetOption('ubsan'): env['LINKFLAGS'] += flags shared = [ - "./crypto/rsa.c", - "./crypto/sha.c", - "./board/can_comms.c", - "./board/drivers/can_common.c", - "./board/fake_stm.c", - "./board/utils.c", + "crypto/rsa.c", + "crypto/sha.c", + "board/can_comms.c", + "board/drivers/can_common.c", + "board/fake_stm.c", + "board/utils.c", ] c = ["panda.c"] + ["../../" + s for s in shared] From c32528828abbdc5bcb5d0b23c6ec356f473ffe9b Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 14:49:44 +0100 Subject: [PATCH 12/29] Cleanup --- board/body/main_comms.h | 2 +- board/{comms_definitions.h => comms.h} | 3 --- board/drivers/spi.c | 1 + board/drivers/usb.c | 1 + board/flasher.h | 2 +- board/main_comms.h | 2 +- board/stm32h7/stm32h7_config.h | 2 +- tests/libpanda/panda.c | 2 +- 8 files changed, 7 insertions(+), 8 deletions(-) rename board/{comms_definitions.h => comms.h} (68%) diff --git a/board/body/main_comms.h b/board/body/main_comms.h index eb08baa1922..04ea8274d61 100644 --- a/board/body/main_comms.h +++ b/board/body/main_comms.h @@ -1,7 +1,7 @@ #pragma once #include "board/globals.h" -#include "board/comms_definitions.h" +#include "board/comms.h" void comms_endpoint2_write(const uint8_t *data, uint32_t len); int comms_control_handler(ControlPacket_t *req, uint8_t *resp); \ No newline at end of file diff --git a/board/comms_definitions.h b/board/comms.h similarity index 68% rename from board/comms_definitions.h rename to board/comms.h index 026a50aa6c9..d452c60d60f 100644 --- a/board/comms_definitions.h +++ b/board/comms.h @@ -11,6 +11,3 @@ typedef struct { int comms_control_handler(ControlPacket_t *req, uint8_t *resp); void comms_endpoint2_write(const uint8_t *data, uint32_t len); -void comms_can_write(const uint8_t *data, uint32_t len); -int comms_can_read(uint8_t *data, uint32_t max_len); -void comms_can_reset(void); diff --git a/board/drivers/spi.c b/board/drivers/spi.c index 089c33c1acb..79a18a86cd3 100644 --- a/board/drivers/spi.c +++ b/board/drivers/spi.c @@ -3,6 +3,7 @@ #include "board/libc.h" #include "board/config.h" +#include "board/can_comms.h" #include "board/globals.h" diff --git a/board/drivers/usb.c b/board/drivers/usb.c index e156c4a6c1c..4ce87e581d9 100644 --- a/board/drivers/usb.c +++ b/board/drivers/usb.c @@ -4,6 +4,7 @@ #include "usb.h" #include "board/config.h" #include "board/globals.h" +#include "board/can_comms.h" static uint8_t response[USBPACKET_MAX_SIZE]; diff --git a/board/flasher.h b/board/flasher.h index 754810221b1..173dc9d4145 100644 --- a/board/flasher.h +++ b/board/flasher.h @@ -3,7 +3,7 @@ #include #include -#include "comms_definitions.h" +#include "comms.h" // from the linker script #define APP_START_ADDRESS 0x8020000U diff --git a/board/main_comms.h b/board/main_comms.h index ddbb8f34cb2..ffc9175934e 100644 --- a/board/main_comms.h +++ b/board/main_comms.h @@ -1,6 +1,6 @@ #pragma once -// #include "comms_definitions.h" +#include "comms.h" extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h index 534f4d9e737..e3ca9389916 100644 --- a/board/stm32h7/stm32h7_config.h +++ b/board/stm32h7/stm32h7_config.h @@ -46,7 +46,7 @@ separate IRQs for RX and TX. #define DEVICE_SERIAL_NUMBER_ADDRESS 0x080FFFC0U #include "board/can.h" -#include "board/comms_definitions.h" +#include "board/comms.h" #include "board/libc.h" #include "board/critical.h" diff --git a/tests/libpanda/panda.c b/tests/libpanda/panda.c index 9d46cff9124..b661b38837d 100644 --- a/tests/libpanda/panda.c +++ b/tests/libpanda/panda.c @@ -22,5 +22,5 @@ can_ring *tx1_q = &can_tx1_q; can_ring *tx2_q = &can_tx2_q; can_ring *tx3_q = &can_tx3_q; -#include "comms_definitions.h" +#include "comms.h" #include "can_comms.h" From b008e0ea7ccf0d79228c8c5c6d4224230ecabc44 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 14:54:49 +0100 Subject: [PATCH 13/29] Cleanup --- SConscript | 1 - board/boards/cuatro.c | 3 --- board/body/main_comms.c | 2 -- board/crc.c | 3 --- board/crc.h | 5 ----- board/drivers/spi.c | 2 -- board/jungle/main_comms.c | 2 -- board/main_comms.c | 15 --------------- 8 files changed, 33 deletions(-) delete mode 100644 board/crc.c delete mode 100644 board/crc.h diff --git a/SConscript b/SConscript index fc9a5cf92ef..6175381033e 100644 --- a/SConscript +++ b/SConscript @@ -104,7 +104,6 @@ def build_project(project_name, project, main, shared, extra_flags): "crypto/rsa.c", "crypto/sha.c", "board/libc.c", - "board/crc.c", "board/early_init.c", "board/critical.c", "board/drivers/led.c", diff --git a/board/boards/cuatro.c b/board/boards/cuatro.c index 092ce4cadcf..4ded6c4654d 100644 --- a/board/boards/cuatro.c +++ b/board/boards/cuatro.c @@ -1,6 +1,3 @@ -// #include "board_declarations.h" -// #include "board/stm32h7/lladc.h" - #include "board/config.h" #include "board/board_struct.h" #include "board/boards/unused_funcs.h" diff --git a/board/body/main_comms.c b/board/body/main_comms.c index 3dce519198e..8c511efdbc2 100644 --- a/board/body/main_comms.c +++ b/board/body/main_comms.c @@ -12,8 +12,6 @@ #include "board/drivers/uart.h" #include "board/body/motor_control.h" -extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used - void comms_endpoint2_write(const uint8_t *data, uint32_t len) { UNUSED(data); UNUSED(len); diff --git a/board/crc.c b/board/crc.c deleted file mode 100644 index dd627f5353e..00000000000 --- a/board/crc.c +++ /dev/null @@ -1,3 +0,0 @@ -// #include "crc.h" - - diff --git a/board/crc.h b/board/crc.h deleted file mode 100644 index b1fc1de823b..00000000000 --- a/board/crc.h +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -#include - -// uint8_t crc_checksum(const uint8_t *dat, int len, const uint8_t poly); \ No newline at end of file diff --git a/board/drivers/spi.c b/board/drivers/spi.c index 79a18a86cd3..f89426755ff 100644 --- a/board/drivers/spi.c +++ b/board/drivers/spi.c @@ -1,6 +1,4 @@ #include "spi.h" -#include "board/crc.h" - #include "board/libc.h" #include "board/config.h" #include "board/can_comms.h" diff --git a/board/jungle/main_comms.c b/board/jungle/main_comms.c index 71179cd360e..f9e40401bdb 100644 --- a/board/jungle/main_comms.c +++ b/board/jungle/main_comms.c @@ -12,8 +12,6 @@ #include "board/libc.h" #include "board/drivers/uart.h" -extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used - bool generated_can_traffic = false; static int get_jungle_health_pkt(void *dat) { diff --git a/board/main_comms.c b/board/main_comms.c index 30323bed8b3..46d97b82ddd 100644 --- a/board/main_comms.c +++ b/board/main_comms.c @@ -1,32 +1,17 @@ #include #include -// #include "board/utils.h" -// #include "board/config.h" -// #include "board/health.h" #include "board/main_declarations.h" #include "board/drivers/can_common.h" #include "board/power_saving.h" -// #include "board/drivers/spi.h" #include "board/drivers/bootkick.h" #include "board/drivers/fdcan.h" #include "board/provision.h" #include "board/early_init.h" #include "board/obj/gitversion.h" -// #include "board/drivers/fan.h" -// #include "board/drivers/clock_source.h" #include "board/globals.h" -// #include "board/drivers/uart.h" - -// #include "board/stm32h7/llfdcan.h" #include "board/main_comms.h" -extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used - -extern uint32_t heartbeat_counter; -extern bool heartbeat_lost; -extern bool heartbeat_disabled; - void set_safety_mode(uint16_t mode, uint16_t param); static int get_health_pkt(void *dat) { From b8fc942e52e37f8d2b241df024123909fff86def Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 19:20:24 +0100 Subject: [PATCH 14/29] Cleanup --- SConscript | 5 ++-- board/boards/cuatro.c | 7 ++--- board/boards/tres.c | 7 ++--- board/body/motor_control.c | 39 ++++++++++++------------ board/body/motor_control.h | 53 --------------------------------- board/body/motor_encoder.c | 12 ++++---- board/body/motor_encoder.h | 3 -- board/body/stm32h7/board.h | 5 ---- board/can_comms.c | 12 ++++---- board/can_comms.h | 4 +-- board/config.h | 7 +++-- board/drivers/fake_siren.c | 8 ++--- board/drivers/fake_siren.h | 4 --- board/drivers/fan.c | 1 + board/drivers/fan.h | 6 +--- board/drivers/fdcan.c | 2 +- board/drivers/fdcan.h | 8 ++--- board/drivers/gpio.c | 1 + board/drivers/interrupts.h | 2 +- board/drivers/registers.c | 43 +++++++++++++------------- board/drivers/registers.h | 24 --------------- board/drivers/simple_watchdog.c | 17 +++++++++++ board/drivers/simple_watchdog.h | 5 +++- board/drivers/spi.c | 5 ++-- board/drivers/spi.h | 6 ++-- board/drivers/uart.c | 12 ++++---- board/drivers/uart.h | 20 ------------- board/drivers/usb.c | 4 +-- board/drivers/usb.h | 5 ---- board/early_init.c | 5 +--- board/fake_stm.h | 9 +++--- board/faults.c | 2 +- board/faults.h | 24 +++++++++++++++ board/flasher.c | 6 ---- board/globals.c | 3 +- board/jungle/boards/board_v2.h | 5 +--- board/jungle/main.c | 2 -- board/jungle/stm32h7/board.c | 4 +-- board/jungle/stm32h7/board.h | 4 ++- board/libc.c | 5 ---- board/main.c | 20 ------------- board/main_comms.c | 1 - board/main_comms.h | 2 -- board/main_definitions.c | 2 -- board/stm32h7/board.h | 2 -- board/stm32h7/clock.c | 2 -- board/stm32h7/lladc.h | 2 +- board/stm32h7/llfan.c | 2 -- board/stm32h7/llfdcan.h | 6 ---- board/stm32h7/peripherals.c | 4 --- board/stm32h7/stm32h7_config.h | 3 +- 51 files changed, 153 insertions(+), 289 deletions(-) diff --git a/SConscript b/SConscript index 6175381033e..32cbae6c401 100644 --- a/SConscript +++ b/SConscript @@ -154,7 +154,7 @@ def build_project(project_name, project, main, shared, extra_flags): "board/drivers/uart.c", "board/stm32h7/llfdcan.c", "board/drivers/harness.c", - "board/drivers/simple_watchdog.c", + "board/drivers/bootkick.c", "board/stm32h7/llfan.c", "board/stm32h7/lluart.c", @@ -195,7 +195,8 @@ with open("board/obj/cert.h", "w") as f: panda_main = [ "board/main_comms.c", - "board/main.c" + "board/main.c", + "board/drivers/simple_watchdog.c", ] panda_shared = [ diff --git a/board/boards/cuatro.c b/board/boards/cuatro.c index 4ded6c4654d..3c14821855c 100644 --- a/board/boards/cuatro.c +++ b/board/boards/cuatro.c @@ -80,10 +80,7 @@ static void cuatro_init(void) { // SOM debugging UART gpio_uart7_init(); - -#ifndef BOOTSTUB - uart_init(&uart_ring_som_debug, 115200); -#endif + // uart_init(&uart_ring_som_debug, 115200); // TODO // fan setup set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); @@ -117,7 +114,7 @@ static harness_configuration cuatro_harness_config = { .adc_signal_SBU2 = ADC_CHANNEL_DEFAULT(ADC1, 17) }; -struct board board_cuatro = { +board board_cuatro = { .harness_config = &cuatro_harness_config, .has_spi = true, .has_fan = true, diff --git a/board/boards/tres.c b/board/boards/tres.c index 918c81ce243..95ca05a1056 100644 --- a/board/boards/tres.c +++ b/board/boards/tres.c @@ -121,9 +121,7 @@ static void tres_init(void) { // SOM debugging UART gpio_uart7_init(); -#ifndef BOOTSTUB - uart_init(&uart_ring_som_debug, 115200); -#endif + // uart_init(&uart_ring_som_debug, 115200); // TODO // fan setup set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); @@ -155,9 +153,10 @@ static harness_configuration tres_harness_config = { .adc_signal_SBU2 = ADC_CHANNEL_DEFAULT(ADC1, 17) }; +// TODO: Move me uint32_t red_read_voltage_mV(void); -struct board board_tres = { +board board_tres = { .harness_config = &tres_harness_config, .has_spi = true, .has_fan = true, diff --git a/board/body/motor_control.c b/board/body/motor_control.c index 6c18084bc18..8e7c1b8e89b 100644 --- a/board/body/motor_control.c +++ b/board/body/motor_control.c @@ -36,7 +36,7 @@ typedef struct { uint8_t enable_pin[2]; } motor_pwm_config_t; -const motor_pwm_config_t motor_pwm_config[BODY_MOTOR_COUNT] = { +static const motor_pwm_config_t motor_pwm_config[BODY_MOTOR_COUNT] = { [BODY_MOTOR_LEFT - 1U] = { TIM16, 1U, TIM17, 1U, {GPIOB, GPIOB}, {8U, 9U}, {GPIO_AF1_TIM16, GPIO_AF1_TIM17}, @@ -49,11 +49,20 @@ const motor_pwm_config_t motor_pwm_config[BODY_MOTOR_COUNT] = { }, }; -float motor_absf(float value) { +typedef struct { + bool active; + float target_rpm; + float integral; + float previous_error; + float last_output; + uint32_t last_update_us; +} motor_speed_state_t; + +static inline float motor_absf(float value) { return (value < 0.0f) ? -value : value; } -float motor_clampf(float value, float min_value, float max_value) { +static inline float motor_clampf(float value, float min_value, float max_value) { if (value < min_value) { return min_value; } @@ -63,18 +72,9 @@ float motor_clampf(float value, float min_value, float max_value) { return value; } -typedef struct { - bool active; - float target_rpm; - float integral; - float previous_error; - float last_output; - uint32_t last_update_us; -} motor_speed_state_t; +static motor_speed_state_t motor_speed_states[BODY_MOTOR_COUNT]; -motor_speed_state_t motor_speed_states[BODY_MOTOR_COUNT]; - -void motor_pwm_write(TIM_TypeDef *timer, uint8_t channel, uint8_t percentage) { +static inline void motor_pwm_write(TIM_TypeDef *timer, uint8_t channel, uint8_t percentage) { uint32_t period = (timer->ARR != 0U) ? timer->ARR : PWM_RELOAD_TICKS; uint16_t comp = (uint16_t)((period * (uint32_t)percentage) / 100U); if (channel == 1U) { @@ -84,11 +84,11 @@ void motor_pwm_write(TIM_TypeDef *timer, uint8_t channel, uint8_t percentage) { } } -motor_speed_state_t *motor_speed_state_get(uint8_t motor) { +static inline motor_speed_state_t *motor_speed_state_get(uint8_t motor) { return body_motor_is_valid(motor) ? &motor_speed_states[motor - 1U] : NULL; } -void motor_speed_state_reset(motor_speed_state_t *state) { +static inline void motor_speed_state_reset(motor_speed_state_t *state) { state->active = false; state->target_rpm = 0.0f; state->integral = 0.0f; @@ -103,7 +103,7 @@ void motor_speed_controller_init(void) { } } -void motor_speed_controller_disable(uint8_t motor) { +static void motor_speed_controller_disable(uint8_t motor) { motor_speed_state_t *state = motor_speed_state_get(motor); if (state == NULL) { return; @@ -111,7 +111,7 @@ void motor_speed_controller_disable(uint8_t motor) { motor_speed_state_reset(state); } -void motor_write_enable(uint8_t motor_index, bool enable) { +static inline void motor_write_enable(uint8_t motor_index, bool enable) { const motor_pwm_config_t *cfg = &motor_pwm_config[motor_index]; uint8_t level = enable ? 1U : 0U; set_gpio_output(cfg->enable_port[0], cfg->enable_pin[0], level); @@ -147,7 +147,7 @@ void motor_init(void) { } } -void motor_apply_pwm(uint8_t motor, int32_t speed_command) { +static void motor_apply_pwm(uint8_t motor, int32_t speed_command) { if (!body_motor_is_valid(motor)) { return; } @@ -250,7 +250,6 @@ void motor_speed_controller_update(uint32_t now_us) { } } - inline bool body_motor_is_valid(uint8_t motor) { return (motor > 0U) && (motor <= BODY_MOTOR_COUNT); } diff --git a/board/body/motor_control.h b/board/body/motor_control.h index ac97443f267..4610f3451d6 100644 --- a/board/body/motor_control.h +++ b/board/body/motor_control.h @@ -38,19 +38,6 @@ typedef struct { uint8_t enable_pin[2]; } motor_pwm_config_t; -static const motor_pwm_config_t motor_pwm_config[BODY_MOTOR_COUNT] = { - [BODY_MOTOR_LEFT - 1U] = { - TIM16, 1U, TIM17, 1U, - {GPIOB, GPIOB}, {8U, 9U}, {GPIO_AF1_TIM16, GPIO_AF1_TIM17}, - {GPIOE, GPIOE}, {2U, 3U}, - }, - [BODY_MOTOR_RIGHT - 1U] = { - TIM15, 2U, TIM15, 1U, - {GPIOA, GPIOA}, {2U, 3U}, {GPIO_AF4_TIM15, GPIO_AF4_TIM15}, - {GPIOE, GPIOE}, {8U, 9U}, - }, -}; - typedef struct { bool active; float target_rpm; @@ -60,47 +47,7 @@ typedef struct { uint32_t last_update_us; } motor_speed_state_t; -static inline float motor_absf(float value) { - return (value < 0.0f) ? -value : value; -} - -static inline float motor_clampf(float value, float min_value, float max_value) { - if (value < min_value) { - return min_value; - } - if (value > max_value) { - return max_value; - } - return value; -} - -static motor_speed_state_t motor_speed_states[BODY_MOTOR_COUNT]; - -static inline void motor_pwm_write(TIM_TypeDef *timer, uint8_t channel, uint8_t percentage) { - uint32_t period = (timer->ARR != 0U) ? timer->ARR : PWM_RELOAD_TICKS; - uint16_t comp = (uint16_t)((period * (uint32_t)percentage) / 100U); - if (channel == 1U) { - register_set(&(timer->CCR1), comp, 0xFFFFU); - } else if (channel == 2U) { - register_set(&(timer->CCR2), comp, 0xFFFFU); - } -} - -static inline motor_speed_state_t *motor_speed_state_get(uint8_t motor) { - return body_motor_is_valid(motor) ? &motor_speed_states[motor - 1U] : NULL; -} - -static inline void motor_speed_state_reset(motor_speed_state_t *state) { - state->active = false; - state->target_rpm = 0.0f; - state->integral = 0.0f; - state->previous_error = 0.0f; - state->last_output = 0.0f; - state->last_update_us = 0U; -} - void motor_speed_controller_init(void); -void motor_speed_controller_disable(uint8_t motor); void motor_init(void); void motor_apply_pwm(uint8_t motor, int32_t speed_command); void motor_set_speed(uint8_t motor, int8_t speed); diff --git a/board/body/motor_encoder.c b/board/body/motor_encoder.c index e3f6b712d70..fe408ddbca5 100644 --- a/board/body/motor_encoder.c +++ b/board/body/motor_encoder.c @@ -1,6 +1,8 @@ +#include + #include "motor_encoder.h" -motor_encoder_config_t motor_encoder_config[BODY_MOTOR_COUNT] = { +static const motor_encoder_config_t motor_encoder_config[BODY_MOTOR_COUNT] = { [BODY_MOTOR_LEFT - 1U] = { .timer = TIM4, .pin_a_port = GPIOB, .pin_a = 6U, .pin_a_af = GPIO_AF2_TIM4, @@ -23,12 +25,12 @@ motor_encoder_config_t motor_encoder_config[BODY_MOTOR_COUNT] = { }, }; -motor_encoder_state_t motor_encoders[BODY_MOTOR_COUNT] = { +static motor_encoder_state_t motor_encoders[BODY_MOTOR_COUNT] = { { .config = &motor_encoder_config[0] }, { .config = &motor_encoder_config[1] }, }; -void motor_encoder_configure_gpio(const motor_encoder_config_t *cfg) { +static void motor_encoder_configure_gpio(const motor_encoder_config_t *cfg) { set_gpio_pullup(cfg->pin_a_port, cfg->pin_a, PULL_UP); set_gpio_output_type(cfg->pin_a_port, cfg->pin_a, OUTPUT_TYPE_PUSH_PULL); set_gpio_alternate(cfg->pin_a_port, cfg->pin_a, cfg->pin_a_af); @@ -38,7 +40,7 @@ void motor_encoder_configure_gpio(const motor_encoder_config_t *cfg) { set_gpio_alternate(cfg->pin_b_port, cfg->pin_b, cfg->pin_b_af); } -void motor_encoder_configure_timer(motor_encoder_state_t *state) { +static void motor_encoder_configure_timer(motor_encoder_state_t *state) { const motor_encoder_config_t *cfg = state->config; TIM_TypeDef *timer = cfg->timer; timer->CR1 = 0U; @@ -73,7 +75,7 @@ void motor_encoder_init(void) { } } -int32_t motor_encoder_refresh(motor_encoder_state_t *state) { +static inline int32_t motor_encoder_refresh(motor_encoder_state_t *state) { const motor_encoder_config_t *cfg = state->config; TIM_TypeDef *timer = cfg->timer; uint16_t raw = (uint16_t)timer->CNT; diff --git a/board/body/motor_encoder.h b/board/body/motor_encoder.h index 686a4767baa..f29a33f66dd 100644 --- a/board/body/motor_encoder.h +++ b/board/body/motor_encoder.h @@ -33,9 +33,6 @@ typedef struct { float cached_speed_rps; } motor_encoder_state_t; -extern motor_encoder_state_t motor_encoders[BODY_MOTOR_COUNT]; - -void motor_encoder_configure_gpio(const motor_encoder_config_t *cfg); void motor_encoder_init(void); int32_t motor_encoder_get_position(uint8_t motor); void motor_encoder_reset(uint8_t motor); diff --git a/board/body/stm32h7/board.h b/board/body/stm32h7/board.h index 574f08ef450..b9f9c9ef140 100644 --- a/board/body/stm32h7/board.h +++ b/board/body/stm32h7/board.h @@ -1,8 +1,3 @@ #pragma once -#ifdef HW_TYPE_BODY -#error HW_TYPE_BODY already defined! -#endif - -// ******************* Definitions ******************** #define HW_TYPE_BODY 0xB1U diff --git a/board/can_comms.c b/board/can_comms.c index 8e12f1a5857..2304ad13522 100644 --- a/board/can_comms.c +++ b/board/can_comms.c @@ -1,12 +1,12 @@ -// #include "can_comms.h" +#include "can_comms.h" #include "drivers/can_common.h" -// #include "utils.h" -// #include "libc.h" -// #include "can.h" +#include "utils.h" +#include "libc.h" +#include "can.h" #include "drivers/usb.h" -// #include "board/drivers/spi.h" +#include "board/drivers/spi.h" -// #include "config.h" +#include "config.h" static asm_buffer can_read_buffer = {.ptr = 0U, .tail_size = 0U}; diff --git a/board/can_comms.h b/board/can_comms.h index 74d4dd4f259..3eceb2e8b1e 100644 --- a/board/can_comms.h +++ b/board/can_comms.h @@ -1,5 +1,7 @@ #pragma once +#include + /* CAN transactions to and from the host come in the form of a certain number of CANPacket_t. The transaction is split @@ -14,8 +16,6 @@ which is sent by the host on each start of a connection. */ -#include - typedef struct { uint32_t ptr; uint32_t tail_size; diff --git a/board/config.h b/board/config.h index ca099f90924..a1f2657bc5d 100644 --- a/board/config.h +++ b/board/config.h @@ -2,6 +2,7 @@ #include +// TODO: This should be included everywhere!, and checked! //#define DEBUG //#define DEBUG_UART //#define DEBUG_USB @@ -32,19 +33,19 @@ #endif #endif -#ifdef PANDA #define HW_TYPE_UNKNOWN 0U +#define HW_TYPE_V2 2U #define HW_TYPE_RED_PANDA 7U #define HW_TYPE_TRES 9U #define HW_TYPE_CUATRO 10U +#ifdef PANDA + // CAN modes #define CAN_MODE_NORMAL 0U #define CAN_MODE_OBD_CAN2 1U #elif defined(PANDA_JUNGLE) -#define HW_TYPE_UNKNOWN 0U -#define HW_TYPE_V2 2U // CAN modes #define CAN_MODE_NORMAL 0U diff --git a/board/drivers/fake_siren.c b/board/drivers/fake_siren.c index deda28c024f..a29a8479d16 100644 --- a/board/drivers/fake_siren.c +++ b/board/drivers/fake_siren.c @@ -4,7 +4,7 @@ #include "board/globals.h" #include "board/stm32h7/sound.h" -void siren_tim7_init(void) { +static void siren_tim7_init(void) { // Init trigger timer (around 2.5kHz) register_set(&TIM7->PSC, 0U, 0xFFFFU); register_set(&TIM7->ARR, 133U, 0xFFFFU); @@ -14,7 +14,7 @@ void siren_tim7_init(void) { TIM7->CR1 |= TIM_CR1_CEN; } -void siren_dac_init(void) { +static void siren_dac_init(void) { DAC1->DHR8R1 = (1U << 7); DAC1->DHR8R2 = (1U << 7); register_set(&DAC1->MCR, 0U, 0xFFFFFFFFU); @@ -22,7 +22,7 @@ void siren_dac_init(void) { register_set_bits(&DAC1->CR, DAC_CR_EN1 | DAC_CR_EN2); } -void siren_dma_init(void) { +static void siren_dma_init(void) { // 1Vpp sine wave with 1V offset static const uint8_t fake_siren_lut[360] = { 134U, 135U, 137U, 138U, 139U, 140U, 141U, 143U, 144U, 145U, 146U, 148U, 149U, 150U, 151U, 152U, 154U, 155U, 156U, 157U, 158U, 159U, 160U, 162U, 163U, 164U, 165U, 166U, 167U, 168U, 169U, 170U, 171U, 172U, 174U, 175U, 176U, 177U, 177U, 178U, 179U, 180U, 181U, 182U, 183U, 184U, 185U, 186U, 186U, 187U, 188U, 189U, 190U, 190U, 191U, 192U, 193U, 193U, 194U, 195U, 195U, 196U, 196U, 197U, 197U, 198U, 199U, 199U, 199U, 200U, 200U, 201U, 201U, 202U, 202U, 202U, 203U, 203U, 203U, 203U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 205U, 205U, 205U, 205U, 205U, 205U, 205U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 203U, 203U, 203U, 203U, 202U, 202U, 202U, 201U, 201U, 200U, 200U, 199U, 199U, 199U, 198U, 197U, 197U, 196U, 196U, 195U, 195U, 194U, 193U, 193U, 192U, 191U, 190U, 190U, 189U, 188U, 187U, 186U, 186U, 185U, 184U, 183U, 182U, 181U, 180U, 179U, 178U, 177U, 177U, 176U, 175U, 174U, 172U, 171U, 170U, 169U, 168U, 167U, 166U, 165U, 164U, 163U, 162U, 160U, 159U, 158U, 157U, 156U, 155U, 154U, 152U, 151U, 150U, 149U, 148U, 146U, 145U, 144U, 143U, 141U, 140U, 139U, 138U, 137U, 135U, 134U, 133U, 132U, 130U, 129U, 128U, 127U, 125U, 124U, 123U, 122U, 121U, 119U, 118U, 117U, 116U, 115U, 113U, 112U, 111U, 110U, 109U, 108U, 106U, 105U, 104U, 103U, 102U, 101U, 100U, 99U, 98U, 97U, 96U, 95U, 94U, 93U, 92U, 91U, 90U, 89U, 88U, 87U, 86U, 85U, 84U, 83U, 82U, 82U, 81U, 80U, 79U, 78U, 78U, 77U, 76U, 76U, 75U, 74U, 74U, 73U, 72U, 72U, 71U, 71U, 70U, 70U, 69U, 69U, 68U, 68U, 67U, 67U, 67U, 66U, 66U, 66U, 65U, 65U, 65U, 65U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 63U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 65U, 65U, 65U, 65U, 66U, 66U, 66U, 67U, 67U, 67U, 68U, 68U, 69U, 69U, 70U, 70U, 71U, 71U, 72U, 72U, 73U, 74U, 74U, 75U, 76U, 76U, 77U, 78U, 78U, 79U, 80U, 81U, 82U, 82U, 83U, 84U, 85U, 86U, 87U, 88U, 89U, 90U, 91U, 92U, 93U, 94U, 95U, 96U, 97U, 98U, 99U, 100U, 101U, 102U, 103U, 104U, 105U, 106U, 108U, 109U, 110U, 111U, 112U, 113U, 115U, 116U, 117U, 118U, 119U, 121U, 122U, 123U, 124U, 125U, 127U, 128U, 129U, 130U, 132U, 133U }; // Setup DMAMUX (DAC_CH1_DMA as input) @@ -35,7 +35,7 @@ void siren_dma_init(void) { DMA1_Stream1->CR = (0b11UL << DMA_SxCR_PL_Pos) | DMA_SxCR_MINC | DMA_SxCR_CIRC | (1U << DMA_SxCR_DIR_Pos); } -void fake_siren_codec_enable(bool enabled) { +static void fake_siren_codec_enable(bool enabled) { if (enabled) { bool success = true; success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2B, (1U << 1)); // Left speaker mix from INA1 diff --git a/board/drivers/fake_siren.h b/board/drivers/fake_siren.h index 3bc99837f11..d70283e217b 100644 --- a/board/drivers/fake_siren.h +++ b/board/drivers/fake_siren.h @@ -4,9 +4,5 @@ #define CODEC_I2C_ADDR 0x10 -void siren_tim7_init(void); -void siren_dac_init(void); -void siren_dma_init(void); -void fake_siren_codec_enable(bool enabled); void fake_i2c_siren_set(bool enabled); void fake_siren_set(bool enabled); \ No newline at end of file diff --git a/board/drivers/fan.c b/board/drivers/fan.c index f3154448275..3bb06c1c749 100644 --- a/board/drivers/fan.c +++ b/board/drivers/fan.c @@ -1,4 +1,5 @@ #include "fan.h" +#include "board/stm32h7/llfan.h" #include "board/utils.h" #include "board/globals.h" #include "board/drivers/pwm.h" diff --git a/board/drivers/fan.h b/board/drivers/fan.h index f08a6270399..16537cc06c8 100644 --- a/board/drivers/fan.h +++ b/board/drivers/fan.h @@ -11,11 +11,7 @@ struct fan_state_t { }; extern struct fan_state_t fan_state; -void fan_set_power(uint8_t percentage); -void llfan_init(void); void fan_init(void); -// Call this at FAN_TICK_FREQ -void fan_tick(void); void fan_set_power(uint8_t percentage); -void fan_init(void); +// Call this at FAN_TICK_FREQ void fan_tick(void); \ No newline at end of file diff --git a/board/drivers/fdcan.c b/board/drivers/fdcan.c index dd2d3442962..59b064b533c 100644 --- a/board/drivers/fdcan.c +++ b/board/drivers/fdcan.c @@ -155,7 +155,7 @@ void process_can(uint8_t can_number) { // FDFDCANx_IT0 IRQ Handler (RX and errors) // blink blue when we are receiving CAN messages -void can_rx(uint8_t can_number) { +static void can_rx(uint8_t can_number) { FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); diff --git a/board/drivers/fdcan.h b/board/drivers/fdcan.h index 0c5180477ce..a4b9f74f76b 100644 --- a/board/drivers/fdcan.h +++ b/board/drivers/fdcan.h @@ -5,6 +5,8 @@ #include "board/can.h" #include "board/config.h" +#define CAN_ACK_ERROR 3U + typedef struct { volatile uint32_t header[2]; volatile uint32_t data_word[CANPACKET_DATA_SIZE_MAX/4U]; @@ -12,16 +14,10 @@ typedef struct { extern FDCAN_GlobalTypeDef *cans[PANDA_CAN_CNT]; -#define CAN_ACK_ERROR 3U - void can_clear_send(FDCAN_GlobalTypeDef *FDCANx, uint8_t can_number); void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg); // ***************************** CAN ***************************** // FDFDCANx_IT1 IRQ Handler (TX) void process_can(uint8_t can_number); - -// FDFDCANx_IT0 IRQ Handler (RX and errors) -// blink blue when we are receiving CAN messages -void can_rx(uint8_t can_number); bool can_init(uint8_t can_number); \ No newline at end of file diff --git a/board/drivers/gpio.c b/board/drivers/gpio.c index 98faf1b0330..4c350c559f6 100644 --- a/board/drivers/gpio.c +++ b/board/drivers/gpio.c @@ -67,6 +67,7 @@ void gpio_set_bitmask(gpio_t *pins, uint8_t num_pins, uint32_t bitmask) { } #endif +// Detection with internal pullup #define PULL_EFFECTIVE_DELAY 4096 bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) { set_gpio_mode(GPIO, pin, MODE_INPUT); diff --git a/board/drivers/interrupts.h b/board/drivers/interrupts.h index 1fb1ec01992..6ba684dde75 100644 --- a/board/drivers/interrupts.h +++ b/board/drivers/interrupts.h @@ -3,7 +3,7 @@ #include #include -#include +#include "board/config.h" typedef struct interrupt { IRQn_Type irq_type; diff --git a/board/drivers/registers.c b/board/drivers/registers.c index facf340093c..d7c08b0b77f 100644 --- a/board/drivers/registers.c +++ b/board/drivers/registers.c @@ -10,6 +10,28 @@ static uint16_t hash_addr(uint32_t input){ return (((input >> 16U) ^ ((((input + 1U) & 0xFFFFU) * HASHING_PRIME) & 0xFFFFU)) & REGISTER_MAP_SIZE); } +// Do not put bits in the check mask that get changed by the hardware +void register_set(volatile uint32_t *addr, uint32_t val, uint32_t mask){ + ENTER_CRITICAL() + // Set bits in register that are also in the mask + (*addr) = ((*addr) & (~mask)) | (val & mask); + + // Add these values to the map + uint16_t hash = hash_addr((uint32_t) addr); + uint16_t tries = REGISTER_MAP_SIZE; + while(CHECK_COLLISION(hash, addr) && (tries > 0U)) { hash = hash_addr((uint32_t) hash); tries--;} + if (tries != 0U){ + register_map[hash].address = addr; + register_map[hash].value = (register_map[hash].value & (~mask)) | (val & mask); + register_map[hash].check_mask |= mask; + } else { + #ifdef DEBUG_FAULTS + print("Hash collision: address 0x"); puth((uint32_t) addr); print("!\n"); + #endif + } + EXIT_CRITICAL() +} + // Set individual bits. Also add them to the check_mask. // Do not use this to change bits that get reset by the hardware void register_set_bits(volatile uint32_t *addr, uint32_t val) { @@ -45,24 +67,3 @@ void init_registers(void) { register_map[i].check_mask = 0U; } } - -void register_set(volatile uint32_t *addr, uint32_t val, uint32_t mask){ - ENTER_CRITICAL() - // Set bits in register that are also in the mask - (*addr) = ((*addr) & (~mask)) | (val & mask); - - // Add these values to the map - uint16_t hash = hash_addr((uint32_t) addr); - uint16_t tries = REGISTER_MAP_SIZE; - while(CHECK_COLLISION(hash, addr) && (tries > 0U)) { hash = hash_addr((uint32_t) hash); tries--;} - if (tries != 0U){ - register_map[hash].address = addr; - register_map[hash].value = (register_map[hash].value & (~mask)) | (val & mask); - register_map[hash].check_mask |= mask; - } else { - #ifdef DEBUG_FAULTS - print("Hash collision: address 0x"); puth((uint32_t) addr); print("!\n"); - #endif - } - EXIT_CRITICAL() -} diff --git a/board/drivers/registers.h b/board/drivers/registers.h index 7aa617395d8..b8c9959178b 100644 --- a/board/drivers/registers.h +++ b/board/drivers/registers.h @@ -5,30 +5,6 @@ #include "board/critical.h" -#define FAULT_STATUS_NONE 0U -#define FAULT_STATUS_TEMPORARY 1U -#define FAULT_STATUS_PERMANENT 2U - -// Fault types, matches cereal.log.PandaState.FaultType -#define FAULT_RELAY_MALFUNCTION (1UL << 0) -#define FAULT_UNUSED_INTERRUPT_HANDLED (1UL << 1) -#define FAULT_INTERRUPT_RATE_CAN_1 (1UL << 2) -#define FAULT_INTERRUPT_RATE_CAN_2 (1UL << 3) -#define FAULT_INTERRUPT_RATE_CAN_3 (1UL << 4) -#define FAULT_INTERRUPT_RATE_TACH (1UL << 5) -#define FAULT_INTERRUPT_RATE_INTERRUPTS (1UL << 7) -#define FAULT_INTERRUPT_RATE_SPI_DMA (1UL << 8) -#define FAULT_INTERRUPT_RATE_USB (1UL << 15) -#define FAULT_REGISTER_DIVERGENT (1UL << 18) -#define FAULT_INTERRUPT_RATE_CLOCK_SOURCE (1UL << 20) -#define FAULT_INTERRUPT_RATE_TICK (1UL << 21) -#define FAULT_INTERRUPT_RATE_EXTI (1UL << 22) -#define FAULT_INTERRUPT_RATE_SPI (1UL << 23) -#define FAULT_INTERRUPT_RATE_UART_7 (1UL << 24) -#define FAULT_SIREN_MALFUNCTION (1UL << 25) -#define FAULT_HEARTBEAT_LOOP_WATCHDOG (1UL << 26) -#define FAULT_INTERRUPT_RATE_SOUND_DMA (1UL << 27) - // 10 bit hash with 23 as a prime #define REGISTER_MAP_SIZE 0x3FFU #define HASHING_PRIME 23U diff --git a/board/drivers/simple_watchdog.c b/board/drivers/simple_watchdog.c index 554ae14ef6d..4b096647b12 100644 --- a/board/drivers/simple_watchdog.c +++ b/board/drivers/simple_watchdog.c @@ -9,3 +9,20 @@ simple_watchdog_state_t wd_state; +void simple_watchdog_kick(void) { + uint32_t ts = microsecond_timer_get(); + + uint32_t et = get_ts_elapsed(ts, wd_state.last_ts); + if (et > wd_state.threshold) { + print("WD timeout 0x"); puth(et); print("\n"); + fault_occurred(wd_state.fault); + } + + wd_state.last_ts = ts; +} + +void simple_watchdog_init(uint32_t fault, uint32_t threshold) { + wd_state.fault = fault; + wd_state.threshold = threshold; + wd_state.last_ts = microsecond_timer_get(); +} diff --git a/board/drivers/simple_watchdog.h b/board/drivers/simple_watchdog.h index be762f61d57..63ce3e17d69 100644 --- a/board/drivers/simple_watchdog.h +++ b/board/drivers/simple_watchdog.h @@ -8,4 +8,7 @@ typedef struct simple_watchdog_state_t { uint32_t threshold; } simple_watchdog_state_t; -extern simple_watchdog_state_t wd_state; \ No newline at end of file +extern simple_watchdog_state_t wd_state; + +void simple_watchdog_kick(void); +void simple_watchdog_init(uint32_t fault, uint32_t threshold); \ No newline at end of file diff --git a/board/drivers/spi.c b/board/drivers/spi.c index f89426755ff..a37a4121bcf 100644 --- a/board/drivers/spi.c +++ b/board/drivers/spi.c @@ -5,8 +5,9 @@ #include "board/globals.h" -uint8_t spi_buf_rx[SPI_BUF_SIZE]; -uint8_t spi_buf_tx[SPI_BUF_SIZE]; +// H7 DMA2 located in D2 domain, so we need to use SRAM1/SRAM2 +__attribute__((section(".sram12"))) uint8_t spi_buf_rx[SPI_BUF_SIZE]; +__attribute__((section(".sram12"))) uint8_t spi_buf_tx[SPI_BUF_SIZE]; uint16_t spi_error_count = 0; diff --git a/board/drivers/spi.h b/board/drivers/spi.h index bf9122861a3..6cadf5b484e 100644 --- a/board/drivers/spi.h +++ b/board/drivers/spi.h @@ -4,10 +4,12 @@ #include #define SPI_TIMEOUT_US 10000U + // got max rate from hitting a non-existent endpoint // in a tight loop, plus some buffer #define SPI_IRQ_RATE 16000U #define SPI_BUF_SIZE 4096U + #define SPI_CHECKSUM_START 0xABU #define SPI_SYNC_BYTE 0x5AU #define SPI_HACK 0x79U @@ -26,15 +28,13 @@ enum { }; extern uint16_t spi_error_count; -extern uint8_t spi_buf_rx[SPI_BUF_SIZE]; -extern uint8_t spi_buf_tx[SPI_BUF_SIZE]; // low level SPI prototypes void llspi_init(void); void llspi_mosi_dma(uint8_t *addr, int len); void llspi_miso_dma(uint8_t *addr, int len); +void can_tx_comms_resume_spi(void); void spi_init(void); void spi_rx_done(void); void spi_tx_done(bool reset); -void can_tx_comms_resume_spi(void); \ No newline at end of file diff --git a/board/drivers/uart.c b/board/drivers/uart.c index b8e309ed954..2a3750db733 100644 --- a/board/drivers/uart.c +++ b/board/drivers/uart.c @@ -1,7 +1,5 @@ #include "uart.h" -// ***************************** Definitions ***************************** - #define UART_BUFFER(x, size_rx, size_tx, uart_ptr, callback_ptr, overwrite_mode) \ static uint8_t elems_rx_##x[size_rx]; \ static uint8_t elems_tx_##x[size_tx]; \ @@ -129,11 +127,11 @@ void puth(unsigned int i) { puthx(i, 8U); } -// #if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG) -// void puth4(unsigned int i) { -// puthx(i, 4U); -// } -// #endif +#if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG) +void puth4(unsigned int i) { + puthx(i, 4U); +} +#endif #if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG_USB) || defined(DEBUG_COMMS) void hexdump(const void *a, int l) { diff --git a/board/drivers/uart.h b/board/drivers/uart.h index 7abde2a8dc3..a0e4bfc0432 100644 --- a/board/drivers/uart.h +++ b/board/drivers/uart.h @@ -44,26 +44,6 @@ void hexdump(const void *a, int l); #endif -// ***************************** Definitions ***************************** - -#define UART_BUFFER(x, size_rx, size_tx, uart_ptr, callback_ptr, overwrite_mode) \ - static uint8_t elems_rx_##x[size_rx]; \ - static uint8_t elems_tx_##x[size_tx]; \ - extern uart_ring uart_ring_##x; \ - uart_ring uart_ring_##x = { \ - .w_ptr_tx = 0, \ - .r_ptr_tx = 0, \ - .elems_tx = ((uint8_t *)&(elems_tx_##x)), \ - .tx_fifo_size = (size_tx), \ - .w_ptr_rx = 0, \ - .r_ptr_rx = 0, \ - .elems_rx = ((uint8_t *)&(elems_rx_##x)), \ - .rx_fifo_size = (size_rx), \ - .uart = (uart_ptr), \ - .callback = (callback_ptr), \ - .overwrite = (overwrite_mode) \ - }; - // ******************************** UART buffers ******************************** extern uart_ring uart_ring_som_debug; diff --git a/board/drivers/usb.c b/board/drivers/usb.c index 4ce87e581d9..1fd696d737e 100644 --- a/board/drivers/usb.c +++ b/board/drivers/usb.c @@ -106,10 +106,10 @@ static void usb_reset(void) { // flush TX fifo USBx->GRSTCTL = USB_OTG_GRSTCTL_TXFFLSH | USB_OTG_GRSTCTL_TXFNUM_4; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH) {} + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH); // flush RX FIFO USBx->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH) {} + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH); // no global NAK USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGINAK; diff --git a/board/drivers/usb.h b/board/drivers/usb.h index a1de7acfdb9..87f285d0ba4 100644 --- a/board/drivers/usb.h +++ b/board/drivers/usb.h @@ -111,8 +111,3 @@ void refresh_can_tx_slots_available(void); // ***************************** USB port ***************************** void can_tx_comms_resume_usb(void); - -// ***************************** USB port ***************************** - -void usb_irqhandler(void); -void can_tx_comms_resume_usb(void); \ No newline at end of file diff --git a/board/early_init.c b/board/early_init.c index 6a8a83d8c53..214b5aa1a18 100644 --- a/board/early_init.c +++ b/board/early_init.c @@ -1,12 +1,9 @@ #include +#include "board/config.h" #include "early_init.h" #include "drivers/led.h" #include "globals.h" -// #include "board/config.h" - -extern void *g_pfnVectors; -extern uint32_t enter_bootloader_mode; static void jump_to_bootloader(void) { // do enter bootloader diff --git a/board/fake_stm.h b/board/fake_stm.h index bce64f8f183..07113847c95 100644 --- a/board/fake_stm.h +++ b/board/fake_stm.h @@ -1,4 +1,9 @@ #pragma once + +#ifdef STM32H7 +#error This code only makes on a non stm32h7 platform +#endif + // minimal code to fake a panda for tests #include @@ -6,12 +11,8 @@ #define ALLOW_DEBUG -#ifndef ENTER_CRITICAL #define ENTER_CRITICAL() 0 -#endif -#ifndef EXIT_CRITICAL #define EXIT_CRITICAL() 0 -#endif void print(const char *a); void puth(unsigned int i); diff --git a/board/faults.c b/board/faults.c index 9488d3b61d2..6ce6a59b501 100644 --- a/board/faults.c +++ b/board/faults.c @@ -1,4 +1,4 @@ -// #include "faults.h" +#include "faults.h" #include "board/drivers/registers.h" uint8_t fault_status = FAULT_STATUS_NONE; diff --git a/board/faults.h b/board/faults.h index 84bd80edd13..b21057fd7e7 100644 --- a/board/faults.h +++ b/board/faults.h @@ -2,6 +2,30 @@ #define PERMANENT_FAULTS 0U +#define FAULT_STATUS_NONE 0U +#define FAULT_STATUS_TEMPORARY 1U +#define FAULT_STATUS_PERMANENT 2U + +// Fault types, matches cereal.log.PandaState.FaultType +#define FAULT_RELAY_MALFUNCTION (1UL << 0) +#define FAULT_UNUSED_INTERRUPT_HANDLED (1UL << 1) +#define FAULT_INTERRUPT_RATE_CAN_1 (1UL << 2) +#define FAULT_INTERRUPT_RATE_CAN_2 (1UL << 3) +#define FAULT_INTERRUPT_RATE_CAN_3 (1UL << 4) +#define FAULT_INTERRUPT_RATE_TACH (1UL << 5) +#define FAULT_INTERRUPT_RATE_INTERRUPTS (1UL << 7) +#define FAULT_INTERRUPT_RATE_SPI_DMA (1UL << 8) +#define FAULT_INTERRUPT_RATE_USB (1UL << 15) +#define FAULT_REGISTER_DIVERGENT (1UL << 18) +#define FAULT_INTERRUPT_RATE_CLOCK_SOURCE (1UL << 20) +#define FAULT_INTERRUPT_RATE_TICK (1UL << 21) +#define FAULT_INTERRUPT_RATE_EXTI (1UL << 22) +#define FAULT_INTERRUPT_RATE_SPI (1UL << 23) +#define FAULT_INTERRUPT_RATE_UART_7 (1UL << 24) +#define FAULT_SIREN_MALFUNCTION (1UL << 25) +#define FAULT_HEARTBEAT_LOOP_WATCHDOG (1UL << 26) +#define FAULT_INTERRUPT_RATE_SOUND_DMA (1UL << 27) + extern uint8_t fault_status; extern uint32_t faults; diff --git a/board/flasher.c b/board/flasher.c index 6c39405b758..77f9eef3e80 100644 --- a/board/flasher.c +++ b/board/flasher.c @@ -1,5 +1,4 @@ #include "flasher.h" -// #include "utils.h" #include "libc.h" #include "board/stm32h7/llflash.h" #include "drivers/led.h" @@ -11,15 +10,10 @@ #include "board/config.h" -// from the linker script -#define APP_START_ADDRESS 0x8020000U - // flasher state variables uint32_t *prog_ptr = NULL; bool unlocked = false; -void spi_init(void); - int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { int resp_len = 0; diff --git a/board/globals.c b/board/globals.c index f9012cb2f80..7f8df52c78f 100644 --- a/board/globals.c +++ b/board/globals.c @@ -2,4 +2,5 @@ uint8_t hw_type; board *current_board; -struct harness_t harness; \ No newline at end of file +struct harness_t harness; +uint32_t uptime_cnt = 0; \ No newline at end of file diff --git a/board/jungle/boards/board_v2.h b/board/jungle/boards/board_v2.h index 3587f1c91da..33ac71f7522 100644 --- a/board/jungle/boards/board_v2.h +++ b/board/jungle/boards/board_v2.h @@ -9,7 +9,7 @@ #include "board/drivers/gpio.h" #ifndef PANDA_JUNGLE -#error no pandas! +#error This should only be used on Panda Body! #endif #define ADC_CHANNEL(a, c) {.adc = (a), .channel = (c), .sample_time = SAMPLETIME_810_CYCLES, .oversampling = OVERSAMPLING_1} @@ -76,6 +76,3 @@ const adc_signal_t sbu2_channels[] = { ADC_CHANNEL(ADC3, 9), ADC_CHANNEL(ADC3, 11), }; - - -extern struct board board_v2; \ No newline at end of file diff --git a/board/jungle/main.c b/board/jungle/main.c index 74ac169ef3e..2f05e9301f5 100644 --- a/board/jungle/main.c +++ b/board/jungle/main.c @@ -22,8 +22,6 @@ #include "board/can_comms.h" #include "board/jungle/main_comms.h" -uint32_t uptime_cnt = 0; - // ********************* Serial debugging ********************* void debug_ring_callback(uart_ring *ring) { diff --git a/board/jungle/stm32h7/board.c b/board/jungle/stm32h7/board.c index 30405a1a5bf..deeef0f010d 100644 --- a/board/jungle/stm32h7/board.c +++ b/board/jungle/stm32h7/board.c @@ -1,14 +1,12 @@ #include "board.h" #ifndef PANDA_JUNGLE -#error FUCK YOU +#error This should only be used on Panda Jungle #endif #include "board/globals.h" #include "board/config.h" -extern board board_v2; - void detect_board_type(void) { hw_type = HW_TYPE_V2; current_board = &board_v2; diff --git a/board/jungle/stm32h7/board.h b/board/jungle/stm32h7/board.h index 0a79b86a093..d390655a7e5 100644 --- a/board/jungle/stm32h7/board.h +++ b/board/jungle/stm32h7/board.h @@ -3,6 +3,8 @@ #include "board/jungle/boards/board_declarations.h" #include "board/stm32h7/lladc.h" -// #include "board/jungle/boards/board_v2.h" +#include "board/board_struct.h" + +extern struct board board_v2; void detect_board_type(void); \ No newline at end of file diff --git a/board/libc.c b/board/libc.c index de6484afe23..847821cb74d 100644 --- a/board/libc.c +++ b/board/libc.c @@ -1,10 +1,5 @@ #include "libc.h" -// **** libc **** - -void print(const char *a); -void puth(unsigned int i); - void delay(uint32_t a) { volatile uint32_t i; for (i = 0; i < a; i++) {} diff --git a/board/main.c b/board/main.c index a6c20ce05d4..93a50dfc3ff 100644 --- a/board/main.c +++ b/board/main.c @@ -18,26 +18,6 @@ #include "board/drivers/fdcan.h" #include "board/main_declarations.h" - -static void simple_watchdog_kick(void) { - uint32_t ts = microsecond_timer_get(); - - uint32_t et = get_ts_elapsed(ts, wd_state.last_ts); - if (et > wd_state.threshold) { - print("WD timeout 0x"); puth(et); print("\n"); - fault_occurred(wd_state.fault); - } - - wd_state.last_ts = ts; -} - - -static void simple_watchdog_init(uint32_t fault, uint32_t threshold) { - wd_state.fault = fault; - wd_state.threshold = threshold; - wd_state.last_ts = microsecond_timer_get(); -} - // ********************* Serial debugging ********************* void debug_ring_callback(uart_ring *ring) { diff --git a/board/main_comms.c b/board/main_comms.c index 46d97b82ddd..40b62a18351 100644 --- a/board/main_comms.c +++ b/board/main_comms.c @@ -12,7 +12,6 @@ #include "board/globals.h" #include "board/main_comms.h" -void set_safety_mode(uint16_t mode, uint16_t param); static int get_health_pkt(void *dat) { COMPILE_TIME_ASSERT(sizeof(struct health_t) <= USBPACKET_MAX_SIZE); diff --git a/board/main_comms.h b/board/main_comms.h index ffc9175934e..3a4905ea8fb 100644 --- a/board/main_comms.h +++ b/board/main_comms.h @@ -2,8 +2,6 @@ #include "comms.h" -extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used - // Prototypes void set_safety_mode(uint16_t mode, uint16_t param); bool is_car_safety_mode(uint16_t mode); diff --git a/board/main_definitions.c b/board/main_definitions.c index e86608d8885..c26658b1db3 100644 --- a/board/main_definitions.c +++ b/board/main_definitions.c @@ -1,8 +1,6 @@ #include #include -uint32_t uptime_cnt = 0; - // heartbeat state uint32_t heartbeat_counter = 0; bool heartbeat_lost = false; diff --git a/board/stm32h7/board.h b/board/stm32h7/board.h index f98c665ead0..95f535c16ad 100644 --- a/board/stm32h7/board.h +++ b/board/stm32h7/board.h @@ -11,8 +11,6 @@ #include "board/stm32h7/sound.h" #include "board/drivers/fake_siren.h" #include "board/drivers/clock_source.h" -// #include "board/boards/red.h" -// #include "board/boards/tres.h" #include "board/boards/cuatro.h" void detect_board_type(void); \ No newline at end of file diff --git a/board/stm32h7/clock.c b/board/stm32h7/clock.c index 8c4d13cf76d..99bb9b2de1c 100644 --- a/board/stm32h7/clock.c +++ b/board/stm32h7/clock.c @@ -1,6 +1,4 @@ #include "clock.h" - -#include "stm32h7xx.h" #include "board/drivers/registers.h" /* diff --git a/board/stm32h7/lladc.h b/board/stm32h7/lladc.h index cafde667ecf..2d5c5d42d5b 100644 --- a/board/stm32h7/lladc.h +++ b/board/stm32h7/lladc.h @@ -3,6 +3,7 @@ #include #include "board/stm32h7/stm32h7_config.h" +#include "board/config.h" typedef enum { SAMPLETIME_1_CYCLE = 0, @@ -37,7 +38,6 @@ typedef struct { } adc_signal_t; #define ADC_CHANNEL_DEFAULT(a, c) {.adc = (a), .channel = (c), .sample_time = SAMPLETIME_32_CYCLES, .oversampling = OVERSAMPLING_64} - #define VREFINT_CAL_ADDR ((uint16_t *)0x1FF1E860UL) void adc_init(ADC_TypeDef *adc); diff --git a/board/stm32h7/llfan.c b/board/stm32h7/llfan.c index 89f1f45d308..c1232a6de15 100644 --- a/board/stm32h7/llfan.c +++ b/board/stm32h7/llfan.c @@ -1,7 +1,5 @@ #include "llfan.h" -#include "stm32h7xx.h" - #include "board/drivers/interrupts.h" #include "board/drivers/fan.h" #include "board/drivers/pwm.h" diff --git a/board/stm32h7/llfdcan.h b/board/stm32h7/llfdcan.h index 2df45f56985..8d19beaee8b 100644 --- a/board/stm32h7/llfdcan.h +++ b/board/stm32h7/llfdcan.h @@ -42,9 +42,6 @@ #define CAN_NAME_FROM_CANIF(CAN_DEV) (((CAN_DEV)==FDCAN1) ? "FDCAN1" : (((CAN_DEV) == FDCAN2) ? "FDCAN2" : "FDCAN3")) #define CAN_NUM_FROM_CANIF(CAN_DEV) (((CAN_DEV)==FDCAN1) ? 0UL : (((CAN_DEV) == FDCAN2) ? 1UL : 2UL)) - -void print(const char *a); - // kbps multiplied by 10 #define SPEEDS_ARRAY_SIZE 8 extern const uint32_t speeds[SPEEDS_ARRAY_SIZE]; @@ -56,6 +53,3 @@ void llcan_irq_disable(const FDCAN_GlobalTypeDef *FDCANx); void llcan_irq_enable(const FDCAN_GlobalTypeDef *FDCANx); bool llcan_init(FDCAN_GlobalTypeDef *FDCANx); void llcan_clear_send(FDCAN_GlobalTypeDef *FDCANx); - -void llcan_irq_disable(const FDCAN_GlobalTypeDef *FDCANx); -void llcan_irq_enable(const FDCAN_GlobalTypeDef *FDCANx); diff --git a/board/stm32h7/peripherals.c b/board/stm32h7/peripherals.c index d537fa23286..6129ac465e3 100644 --- a/board/stm32h7/peripherals.c +++ b/board/stm32h7/peripherals.c @@ -1,11 +1,7 @@ #include "peripherals.h" #include "board/drivers/gpio.h" -#ifdef BOOTSTUB -void gpio_usb_init(void) { -#else void gpio_usb_init(void) { -#endif // A11,A12: USB set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG1_FS); set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG1_FS); diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h index e3ca9389916..ce3d12e08e4 100644 --- a/board/stm32h7/stm32h7_config.h +++ b/board/stm32h7/stm32h7_config.h @@ -90,5 +90,4 @@ separate IRQs for RX and TX. #include "board/drivers/spi.h" #include "board/stm32h7/llspi.h" -void early_gpio_float(void); -void detect_board_type(void); \ No newline at end of file +void early_gpio_float(void); \ No newline at end of file From ed6d4bac641ebd1f3580dd309251b69670cdf66d Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 20:09:36 +0100 Subject: [PATCH 15/29] Verify debugging flags --- board/bootstub.c | 12 +++++------- board/config.h | 1 - board/drivers/can_common.c | 1 + board/drivers/fan.c | 1 + board/drivers/registers.c | 1 + board/drivers/spi.c | 1 + board/drivers/uart.c | 1 + board/drivers/usb.c | 1 + board/main_definitions.c | 2 ++ board/stm32h7/lluart.c | 1 + tests/misra/test_misra.sh | 8 +++----- 11 files changed, 17 insertions(+), 13 deletions(-) diff --git a/board/bootstub.c b/board/bootstub.c index 8b8a8954838..8128628fde8 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -19,13 +19,11 @@ void __initialize_hardware_early(void) { early_initialization(); } -void print(const char *a) { - UNUSED(a); -} - -void puth(unsigned int i) { - UNUSED(i); -} +void print(const char *a){ UNUSED(a); } +void puth(unsigned int i){ UNUSED(i); } +void puth2(uint8_t i){ UNUSED(i); } +void puth4(uint8_t i){ UNUSED(i); } +void hexdump(const void *a, int l){ UNUSED(a); UNUSED(l); } void fail(void) { soft_flasher_start(); diff --git a/board/config.h b/board/config.h index a1f2657bc5d..90a03529583 100644 --- a/board/config.h +++ b/board/config.h @@ -2,7 +2,6 @@ #include -// TODO: This should be included everywhere!, and checked! //#define DEBUG //#define DEBUG_UART //#define DEBUG_USB diff --git a/board/drivers/can_common.c b/board/drivers/can_common.c index 2467bf445ad..f0155e584d1 100644 --- a/board/drivers/can_common.c +++ b/board/drivers/can_common.c @@ -1,3 +1,4 @@ +#include "board/config.h" #include "can_common.h" #include "board/can_comms.h" diff --git a/board/drivers/fan.c b/board/drivers/fan.c index 3bb06c1c749..1a49b645d8a 100644 --- a/board/drivers/fan.c +++ b/board/drivers/fan.c @@ -30,6 +30,7 @@ void fan_tick(void) { fan_state.rpm = (fan_rpm_fast + (3U * fan_state.rpm)) / 4U; #ifdef DEBUG_FAN + // TODO: Broken since a2064b8 puth(fan_state.target_rpm); print(" "); puth(fan_rpm_fast); print(" "); puth(fan_state.power); diff --git a/board/drivers/registers.c b/board/drivers/registers.c index d7c08b0b77f..3059d6889e8 100644 --- a/board/drivers/registers.c +++ b/board/drivers/registers.c @@ -1,3 +1,4 @@ +#include "board/config.h" #include "registers.h" #include "board/libc.h" diff --git a/board/drivers/spi.c b/board/drivers/spi.c index a37a4121bcf..1531660950f 100644 --- a/board/drivers/spi.c +++ b/board/drivers/spi.c @@ -2,6 +2,7 @@ #include "board/libc.h" #include "board/config.h" #include "board/can_comms.h" +#include "board/drivers/uart.h" #include "board/globals.h" diff --git a/board/drivers/uart.c b/board/drivers/uart.c index 2a3750db733..cb45421a51e 100644 --- a/board/drivers/uart.c +++ b/board/drivers/uart.c @@ -1,3 +1,4 @@ +#include "board/config.h" #include "uart.h" #define UART_BUFFER(x, size_rx, size_tx, uart_ptr, callback_ptr, overwrite_mode) \ diff --git a/board/drivers/usb.c b/board/drivers/usb.c index 1fd696d737e..c1282b2f309 100644 --- a/board/drivers/usb.c +++ b/board/drivers/usb.c @@ -5,6 +5,7 @@ #include "board/config.h" #include "board/globals.h" #include "board/can_comms.h" +#include "board/drivers/uart.h" static uint8_t response[USBPACKET_MAX_SIZE]; diff --git a/board/main_definitions.c b/board/main_definitions.c index c26658b1db3..2fec4c4a430 100644 --- a/board/main_definitions.c +++ b/board/main_definitions.c @@ -1,6 +1,8 @@ #include #include +#include "main_declarations.h" + // heartbeat state uint32_t heartbeat_counter = 0; bool heartbeat_lost = false; diff --git a/board/stm32h7/lluart.c b/board/stm32h7/lluart.c index 97e3e4ea143..fb7c532a76e 100644 --- a/board/stm32h7/lluart.c +++ b/board/stm32h7/lluart.c @@ -1,3 +1,4 @@ +#include "board/config.h" #include "board/drivers/uart.h" static void uart_rx_ring(uart_ring *q){ diff --git a/tests/misra/test_misra.sh b/tests/misra/test_misra.sh index 89ff7e375ea..91f0d388bf8 100755 --- a/tests/misra/test_misra.sh +++ b/tests/misra/test_misra.sh @@ -45,11 +45,10 @@ cppcheck() { $CPPCHECK_DIR/cppcheck --inline-suppr \ --enable=all --addon=misra \ - -j `nproc` \ --suppressions-list=$DIR/suppressions.txt --suppress=*:*inc/* \ --suppress=*:*include/* --error-exitcode=2 --check-level=exhaustive --safety \ --platform=arm32-wchar_t4 --checkers-report=$CHECKLIST.tmp \ - --std=c11 --project=compile_commands.json --file-filter=*.c 2>&1 | tee $OUTPUT + --std=c11 --project=compile_commands.json -icrypto 2>&1 | tee $OUTPUT cat $CHECKLIST.tmp >> $CHECKLIST rm $CHECKLIST.tmp @@ -61,10 +60,9 @@ cppcheck() { fi } -PANDA_OPTS="--enable=all --addon=misra" - printf "\n${GREEN}** PANDA H7 CODE **${NC}\n" -cppcheck $PANDA_OPTS + +cppcheck printf "\n${GREEN}Success!${NC} took $SECONDS seconds\n" From c1eae35d2d7f987852fa2829ddf8434b50206c0f Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 20:44:05 +0100 Subject: [PATCH 16/29] Misra fixups --- board/boards/red.c | 6 +++--- board/boards/red.h | 4 +++- board/boards/tres.c | 6 ++---- board/body/main_comms.c | 2 +- board/body/motor_control.c | 1 + board/bootstub.c | 11 ++++++++--- board/drivers/fake_siren.c | 1 + board/drivers/gpio.c | 2 +- board/drivers/interrupts.c | 2 +- board/drivers/registers.c | 1 + board/drivers/simple_watchdog.c | 1 + board/drivers/spi.c | 5 +++-- board/drivers/uart.c | 1 + board/drivers/uart.h | 17 ----------------- board/drivers/usb.c | 5 +++-- board/early_init.h | 3 ++- board/faults.c | 1 + board/flasher.c | 1 + board/jungle/boards/board_v2.c | 1 + board/jungle/main.c | 1 + board/jungle/main_comms.c | 1 + board/libc.c | 1 + board/libc.h | 4 +--- board/main.c | 1 + board/main_comms.c | 1 + board/power_saving.c | 1 + board/print.h | 15 +++++++++++++++ board/stm32h7/board.c | 1 + board/stm32h7/lladc.c | 11 ++++++----- board/stm32h7/llfdcan.c | 1 + board/stm32h7/llflash.c | 4 ++-- board/stm32h7/lli2c.c | 4 ++-- 32 files changed, 69 insertions(+), 48 deletions(-) create mode 100644 board/print.h diff --git a/board/boards/red.c b/board/boards/red.c index 4a4183020d0..0447ba9b5ef 100644 --- a/board/boards/red.c +++ b/board/boards/red.c @@ -9,7 +9,7 @@ // Red Panda (STM32H7) + Harness // // ///////////////////////////// // -void red_enable_can_transceiver(uint8_t transceiver, bool enabled) { +static void red_enable_can_transceiver(uint8_t transceiver, bool enabled) { switch (transceiver) { case 1U: set_gpio_output(GPIOG, 11, !enabled); @@ -28,7 +28,7 @@ void red_enable_can_transceiver(uint8_t transceiver, 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) { @@ -74,7 +74,7 @@ uint32_t red_read_voltage_mV(void){ return adc_get_mV(&(const adc_signal_t) ADC_CHANNEL_DEFAULT(ADC1, 2)) * 11U; } -void red_init(void) { +static void red_init(void) { common_init_gpio(); // G11,B3,D7,B4: transceiver enable diff --git a/board/boards/red.h b/board/boards/red.h index 4c94f3025c1..2a26f561670 100644 --- a/board/boards/red.h +++ b/board/boards/red.h @@ -7,4 +7,6 @@ // ///////////////////////////// // extern struct harness_configuration red_harness_config; -extern struct board board_red; \ No newline at end of file +extern struct board board_red; + +uint32_t red_read_voltage_mV(void); \ No newline at end of file diff --git a/board/boards/tres.c b/board/boards/tres.c index 95ca05a1056..5b97d8e813f 100644 --- a/board/boards/tres.c +++ b/board/boards/tres.c @@ -1,4 +1,5 @@ #include "tres.h" +#include "red.h" #include "board/drivers/pwm.h" #include "board/globals.h" @@ -106,7 +107,7 @@ static void tres_init(void) { // Enable USB 3.3V LDO for USB block register_set_bits(&(PWR->CR3), PWR_CR3_USBREGEN); register_set_bits(&(PWR->CR3), PWR_CR3_USB33DEN); - while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0U); + while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0U) {} common_init_gpio(); @@ -153,9 +154,6 @@ static harness_configuration tres_harness_config = { .adc_signal_SBU2 = ADC_CHANNEL_DEFAULT(ADC1, 17) }; -// TODO: Move me -uint32_t red_read_voltage_mV(void); - board board_tres = { .harness_config = &tres_harness_config, .has_spi = true, diff --git a/board/body/main_comms.c b/board/body/main_comms.c index 8c511efdbc2..192572480f3 100644 --- a/board/body/main_comms.c +++ b/board/body/main_comms.c @@ -1,5 +1,4 @@ #include "board/body/main_comms.h" - #include "board/globals.h" #include "board/config.h" #include "main_comms.h" @@ -11,6 +10,7 @@ #include "board/libc.h" #include "board/drivers/uart.h" #include "board/body/motor_control.h" +#include "board/print.h" void comms_endpoint2_write(const uint8_t *data, uint32_t len) { UNUSED(data); diff --git a/board/body/motor_control.c b/board/body/motor_control.c index 8e7c1b8e89b..8bb45aa8bd7 100644 --- a/board/body/motor_control.c +++ b/board/body/motor_control.c @@ -2,6 +2,7 @@ #include #include "board/body/motor_common.h" +#include "board/body/motor_control.h" #include "board/body/motor_encoder.h" #include "board/drivers/pwm.h" diff --git a/board/bootstub.c b/board/bootstub.c index 8128628fde8..851021d4a37 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -4,6 +4,7 @@ // ********************* Includes ********************* #include +#include "board/config.h" #include "board/early_init.h" #include "board/flasher.h" @@ -11,6 +12,7 @@ #include "crypto/sha.h" #include "board/obj/cert.h" +#include "board/print.h" #include "globals.h" @@ -21,11 +23,14 @@ void __initialize_hardware_early(void) { void print(const char *a){ UNUSED(a); } void puth(unsigned int i){ UNUSED(i); } -void puth2(uint8_t i){ UNUSED(i); } -void puth4(uint8_t i){ UNUSED(i); } +#if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG) +void puth4(unsigned int i){ UNUSED(i); } +#endif +#if defined(DEBUG_SPI) || defined(DEBUG_USB) || defined(DEBUG_COMMS) void hexdump(const void *a, int l){ UNUSED(a); UNUSED(l); } +#endif -void fail(void) { +static void fail(void) { soft_flasher_start(); } diff --git a/board/drivers/fake_siren.c b/board/drivers/fake_siren.c index a29a8479d16..b0b2dff818c 100644 --- a/board/drivers/fake_siren.c +++ b/board/drivers/fake_siren.c @@ -3,6 +3,7 @@ #include "board/stm32h7/lli2c.h" #include "board/globals.h" #include "board/stm32h7/sound.h" +#include "board/print.h" static void siren_tim7_init(void) { // Init trigger timer (around 2.5kHz) diff --git a/board/drivers/gpio.c b/board/drivers/gpio.c index 4c350c559f6..42d3296f562 100644 --- a/board/drivers/gpio.c +++ b/board/drivers/gpio.c @@ -72,7 +72,7 @@ void gpio_set_bitmask(gpio_t *pins, uint8_t num_pins, uint32_t bitmask) { bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) { set_gpio_mode(GPIO, pin, MODE_INPUT); set_gpio_pullup(GPIO, pin, mode); - for (volatile int i=0; iGRSTCTL = USB_OTG_GRSTCTL_TXFFLSH | USB_OTG_GRSTCTL_TXFNUM_4; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH) {} // flush RX FIFO USBx->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH; - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH) {} // no global NAK USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGINAK; diff --git a/board/early_init.h b/board/early_init.h index 543ffe95501..6db0a5d4b0d 100644 --- a/board/early_init.h +++ b/board/early_init.h @@ -13,4 +13,5 @@ extern uint32_t enter_bootloader_mode; typedef void (*bootloader_fcn)(void); typedef bootloader_fcn *bootloader_fcn_ptr; -void early_initialization(void); \ No newline at end of file +void early_initialization(void); +void __initialize_hardware_early(void); \ No newline at end of file diff --git a/board/faults.c b/board/faults.c index 6ce6a59b501..5ef8134fbc4 100644 --- a/board/faults.c +++ b/board/faults.c @@ -1,5 +1,6 @@ #include "faults.h" #include "board/drivers/registers.h" +#include "print.h" uint8_t fault_status = FAULT_STATUS_NONE; uint32_t faults = 0U; diff --git a/board/flasher.c b/board/flasher.c index 77f9eef3e80..f69bf74424b 100644 --- a/board/flasher.c +++ b/board/flasher.c @@ -2,6 +2,7 @@ #include "libc.h" #include "board/stm32h7/llflash.h" #include "drivers/led.h" +#include "print.h" #include "globals.h" #include "provision.h" diff --git a/board/jungle/boards/board_v2.c b/board/jungle/boards/board_v2.c index 903a29dac92..97b43ef862e 100644 --- a/board/jungle/boards/board_v2.c +++ b/board/jungle/boards/board_v2.c @@ -1,5 +1,6 @@ #include "board/board_struct.h" #include "board/jungle/boards/board_v2.h" +#include "board/print.h" uint8_t harness_orientation = HARNESS_ORIENTATION_NONE; uint8_t can_mode = CAN_MODE_NORMAL; diff --git a/board/jungle/main.c b/board/jungle/main.c index 2f05e9301f5..8de4e31cc60 100644 --- a/board/jungle/main.c +++ b/board/jungle/main.c @@ -21,6 +21,7 @@ #include "board/can_comms.h" #include "board/jungle/main_comms.h" +#include "board/print.h" // ********************* Serial debugging ********************* diff --git a/board/jungle/main_comms.c b/board/jungle/main_comms.c index f9e40401bdb..8fdb49a9bb8 100644 --- a/board/jungle/main_comms.c +++ b/board/jungle/main_comms.c @@ -11,6 +11,7 @@ #include "board/stm32h7/llfdcan.h" #include "board/libc.h" #include "board/drivers/uart.h" +#include "board/print.h" bool generated_can_traffic = false; diff --git a/board/libc.c b/board/libc.c index 847821cb74d..780c49a8656 100644 --- a/board/libc.c +++ b/board/libc.c @@ -1,4 +1,5 @@ #include "libc.h" +#include "print.h" void delay(uint32_t a) { volatile uint32_t i; diff --git a/board/libc.h b/board/libc.h index 8e7f79ef5e5..6d89c54f62c 100644 --- a/board/libc.h +++ b/board/libc.h @@ -15,6 +15,4 @@ void *memset(void *str, int c, unsigned int n); // cppcheck-suppress misra-c2012-21.2 void *memcpy(void *dest, const void *src, unsigned int len); // cppcheck-suppress misra-c2012-21.2 -int memcmp(const void * ptr1, const void * ptr2, unsigned int num); -void print(const char *a); -void puth(unsigned int i); \ No newline at end of file +int memcmp(const void * ptr1, const void * ptr2, unsigned int num); \ No newline at end of file diff --git a/board/main.c b/board/main.c index 93a50dfc3ff..db4e3714ac2 100644 --- a/board/main.c +++ b/board/main.c @@ -17,6 +17,7 @@ #include "board/drivers/can_common.h" #include "board/drivers/fdcan.h" #include "board/main_declarations.h" +#include "board/print.h" // ********************* Serial debugging ********************* diff --git a/board/main_comms.c b/board/main_comms.c index 40b62a18351..1830c4a4ae7 100644 --- a/board/main_comms.c +++ b/board/main_comms.c @@ -11,6 +11,7 @@ #include "board/obj/gitversion.h" #include "board/globals.h" #include "board/main_comms.h" +#include "board/print.h" static int get_health_pkt(void *dat) { diff --git a/board/power_saving.c b/board/power_saving.c index eafda699b06..0364446bb6c 100644 --- a/board/power_saving.c +++ b/board/power_saving.c @@ -5,6 +5,7 @@ #include "board/config.h" #include "board/drivers/fdcan.h" #include "board/globals.h" +#include "board/print.h" // WARNING: To stay in compliance with the SIL2 rules laid out in STM UM1840, we should never implement any of the available hardware low power modes. // See rule: CoU_3 diff --git a/board/print.h b/board/print.h new file mode 100644 index 00000000000..d6e6cb515a1 --- /dev/null +++ b/board/print.h @@ -0,0 +1,15 @@ +#pragma once + +#include + +// ************************ High-level debug functions ********************** +void putch(const char a); +void print(const char *a); +void puthx(uint32_t i, uint8_t len); +void puth(unsigned int i); +#if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG) +void puth4(unsigned int i); +#endif +#if defined(DEBUG_SPI) || defined(DEBUG_USB) || defined(DEBUG_COMMS) +void hexdump(const void *a, int l); +#endif \ No newline at end of file diff --git a/board/stm32h7/board.c b/board/stm32h7/board.c index 3ac48a2f97f..bd98ef46e9f 100644 --- a/board/stm32h7/board.c +++ b/board/stm32h7/board.c @@ -4,6 +4,7 @@ #include "board/globals.h" #include "board/boards/red.h" #include "board/boards/tres.h" +#include "board/print.h" void detect_board_type(void) { // On STM32H7 pandas, we use two different sets of pins. diff --git a/board/stm32h7/lladc.c b/board/stm32h7/lladc.c index 1d0d8f876f0..23a89f7977e 100644 --- a/board/stm32h7/lladc.c +++ b/board/stm32h7/lladc.c @@ -1,6 +1,7 @@ #include "lladc.h" #include "board/libc.h" +#include "board/print.h" static uint32_t adc_avdd_mV = 0U; @@ -8,18 +9,18 @@ void adc_init(ADC_TypeDef *adc) { adc->CR &= ~(ADC_CR_ADEN); // Disable ADC adc->CR &= ~(ADC_CR_DEEPPWD); // Reset deep-power-down mode adc->CR |= ADC_CR_ADVREGEN; // Enable ADC regulator - while(!(adc->ISR & ADC_ISR_LDORDY) && (adc != ADC3)); + while(!(adc->ISR & ADC_ISR_LDORDY) && (adc != ADC3)) {} if (adc != ADC3) { adc->CR &= ~(ADC_CR_ADCALDIF); // Choose single-ended calibration adc->CR |= ADC_CR_ADCALLIN; // Lineriality calibration } adc->CR |= ADC_CR_ADCAL; // Start calibration - while((adc->CR & ADC_CR_ADCAL) != 0U); + while((adc->CR & ADC_CR_ADCAL) != 0U) {} adc->ISR |= ADC_ISR_ADRDY; adc->CR |= ADC_CR_ADEN; - while(!(adc->ISR & ADC_ISR_ADRDY)); + while(!(adc->ISR & ADC_ISR_ADRDY)) {} } uint16_t adc_get_raw(const adc_signal_t *signal) { @@ -42,11 +43,11 @@ uint16_t adc_get_raw(const adc_signal_t *signal) { // start conversion signal->adc->CR |= ADC_CR_ADSTART; - while (!(signal->adc->ISR & ADC_ISR_EOC)); + while (!(signal->adc->ISR & ADC_ISR_EOC)) {} uint16_t res = signal->adc->DR; - while (!(signal->adc->ISR & ADC_ISR_EOS)); + while (!(signal->adc->ISR & ADC_ISR_EOS)) {} signal->adc->ISR |= ADC_ISR_EOS; return res; diff --git a/board/stm32h7/llfdcan.c b/board/stm32h7/llfdcan.c index a89cdf00066..f520f1f4489 100644 --- a/board/stm32h7/llfdcan.c +++ b/board/stm32h7/llfdcan.c @@ -1,4 +1,5 @@ #include "llfdcan.h" +#include "board/print.h" // kbps multiplied by 10 const uint32_t speeds[SPEEDS_ARRAY_SIZE] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U}; diff --git a/board/stm32h7/llflash.c b/board/stm32h7/llflash.c index 1fe7e9be620..6ec54c282a2 100644 --- a/board/stm32h7/llflash.c +++ b/board/stm32h7/llflash.c @@ -7,8 +7,8 @@ bool flash_is_locked(void) { } void flash_unlock(void) { - FLASH->KEYR1 = 0x45670123; - FLASH->KEYR1 = 0xCDEF89AB; + FLASH->KEYR1 = 0x45670123u; + FLASH->KEYR1 = 0xCDEF89ABu; } bool flash_erase_sector(uint8_t sector, bool unlocked) { diff --git a/board/stm32h7/lli2c.c b/board/stm32h7/lli2c.c index bda5321c26c..8afa0fa141c 100644 --- a/board/stm32h7/lli2c.c +++ b/board/stm32h7/lli2c.c @@ -8,14 +8,14 @@ bool i2c_status_wait(const volatile uint32_t *reg, uint32_t mask, uint32_t val) { uint32_t start_time = microsecond_timer_get(); - while(((*reg & mask) != val) && (get_ts_elapsed(microsecond_timer_get(), start_time) < I2C_TIMEOUT_US)); + while(((*reg & mask) != val) && (get_ts_elapsed(microsecond_timer_get(), start_time) < I2C_TIMEOUT_US)) {} return ((*reg & mask) == val); } void i2c_reset(I2C_TypeDef *I2C) { // peripheral reset register_clear_bits(&I2C->CR1, I2C_CR1_PE); - while ((I2C->CR1 & I2C_CR1_PE) != 0U); + while ((I2C->CR1 & I2C_CR1_PE) != 0U) {} register_set_bits(&I2C->CR1, I2C_CR1_PE); } From 9ee502a00c8933de54cb7e7fc023e32522bb0f98 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 20:44:18 +0100 Subject: [PATCH 17/29] Misra test fixups --- tests/misra/test_misra.sh | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/tests/misra/test_misra.sh b/tests/misra/test_misra.sh index 91f0d388bf8..f6fd56da229 100755 --- a/tests/misra/test_misra.sh +++ b/tests/misra/test_misra.sh @@ -1,5 +1,4 @@ #!/usr/bin/env bash -set -e DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" PANDA_DIR=$(realpath $DIR/../../) @@ -36,18 +35,21 @@ CHECKLIST=$DIR/checkers.txt echo "Cppcheck checkers list from test_misra.sh:" > $CHECKLIST cppcheck() { + # get all gcc defines: arm-none-eabi-gcc -dM -E - < /dev/null + COMMON_DEFINES="-D__GNUC__=9 -UCMSIS_NVIC_VIRTUAL -UCMSIS_VECTAB_VIRTUAL -UPANDA_JUNGLE -UBOOTSTUB" # note that cppcheck build cache results in inconsistent results as of v2.13.0 OUTPUT=$DIR/.output.log - + echo -e "\n\n\n\n\nTEST variant options:" >> $CHECKLIST echo -e ""${@//$PANDA_DIR/}"\n\n" >> $CHECKLIST # (absolute path removed) $CPPCHECK_DIR/cppcheck --inline-suppr \ + -I "$(arm-none-eabi-gcc -print-file-name=include)" \ --enable=all --addon=misra \ --suppressions-list=$DIR/suppressions.txt --suppress=*:*inc/* \ --suppress=*:*include/* --error-exitcode=2 --check-level=exhaustive --safety \ - --platform=arm32-wchar_t4 --checkers-report=$CHECKLIST.tmp \ + --platform=arm32-wchar_t4 $COMMON_DEFINES --checkers-report=$CHECKLIST.tmp \ --std=c11 --project=compile_commands.json -icrypto 2>&1 | tee $OUTPUT cat $CHECKLIST.tmp >> $CHECKLIST From ef27acba2e011086a1561d8d24ca50e159bd1e4c Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 21:31:10 +0100 Subject: [PATCH 18/29] Misra fixups --- board/board_forward.h | 36 ++++++++++++++++-------------------- board/body/motor_control.h | 22 ---------------------- board/drivers/interrupts.h | 2 -- board/drivers/usb.h | 3 --- board/globals.h | 4 +--- board/main.c | 1 + board/stm32h7/board.h | 4 +--- board/stm32h7/lluart.h | 5 ++--- 8 files changed, 21 insertions(+), 56 deletions(-) diff --git a/board/board_forward.h b/board/board_forward.h index 0a12673e443..7bf7ec4efc0 100644 --- a/board/board_forward.h +++ b/board/board_forward.h @@ -5,29 +5,25 @@ #include "board/boards/boot_state.h" -typedef void (*board_init)(void); -typedef void (*board_board_tick)(void); typedef bool (*board_get_button)(void); -typedef void (*board_init_bootloader)(void); -typedef void (*board_set_panda_power)(bool enabled); -typedef void (*board_set_panda_individual_power)(uint8_t port_num, bool enabled); -typedef void (*board_set_ignition)(bool enabled); -typedef void (*board_set_individual_ignition)(uint8_t bitmask); -typedef void (*board_set_harness_orientation)(uint8_t orientation); -typedef void (*board_set_can_mode)(uint8_t mode); -typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); -typedef void (*board_enable_header_pin)(uint8_t pin_num, bool enabled); +typedef bool (*board_read_som_gpio)(void); typedef float (*board_get_channel_power)(uint8_t channel); typedef uint16_t (*board_get_sbu_mV)(uint8_t channel, uint8_t sbu); -typedef void (*board_init)(void); -typedef void (*board_init_bootloader)(void); +typedef uint32_t (*board_read_current_mA)(void); +typedef uint32_t (*board_read_voltage_mV)(void); +typedef void (*board_board_tick)(void); typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled); +typedef void (*board_enable_header_pin)(uint8_t pin_num, bool enabled); +typedef void (*board_init_bootloader)(void); +typedef void (*board_init)(void); +typedef void (*board_set_amp_enabled)(bool enabled); +typedef void (*board_set_bootkick)(BootState state); typedef void (*board_set_can_mode)(uint8_t mode); -typedef uint32_t (*board_read_voltage_mV)(void); -typedef uint32_t (*board_read_current_mA)(void); -typedef void (*board_set_ir_power)(uint8_t percentage); typedef void (*board_set_fan_enabled)(bool enabled); -typedef void (*board_set_siren)(bool enabled); -typedef void (*board_set_bootkick)(BootState state); -typedef bool (*board_read_som_gpio)(void); -typedef void (*board_set_amp_enabled)(bool enabled); +typedef void (*board_set_harness_orientation)(uint8_t orientation); +typedef void (*board_set_ignition)(bool enabled); +typedef void (*board_set_individual_ignition)(uint8_t bitmask); +typedef void (*board_set_ir_power)(uint8_t percentage); +typedef void (*board_set_panda_individual_power)(uint8_t port_num, bool enabled); +typedef void (*board_set_panda_power)(bool enabled); +typedef void (*board_set_siren)(bool enabled); \ No newline at end of file diff --git a/board/body/motor_control.h b/board/body/motor_control.h index 4610f3451d6..615081ddb37 100644 --- a/board/body/motor_control.h +++ b/board/body/motor_control.h @@ -26,30 +26,8 @@ #define DEADBAND_RPM 1.0f #define UPDATE_PERIOD_US 1000U -typedef struct { - TIM_TypeDef *forward_timer; - uint8_t forward_channel; - TIM_TypeDef *reverse_timer; - uint8_t reverse_channel; - GPIO_TypeDef *pwm_port[2]; - uint8_t pwm_pin[2]; - uint8_t pwm_af[2]; - GPIO_TypeDef *enable_port[2]; - uint8_t enable_pin[2]; -} motor_pwm_config_t; - -typedef struct { - bool active; - float target_rpm; - float integral; - float previous_error; - float last_output; - uint32_t last_update_us; -} motor_speed_state_t; - void motor_speed_controller_init(void); void motor_init(void); -void motor_apply_pwm(uint8_t motor, int32_t speed_command); void motor_set_speed(uint8_t motor, int8_t speed); void motor_speed_controller_set_target_rpm(uint8_t motor, float target_rpm); void motor_speed_controller_update(uint32_t now_us); diff --git a/board/drivers/interrupts.h b/board/drivers/interrupts.h index 6ba684dde75..d62ffbc5988 100644 --- a/board/drivers/interrupts.h +++ b/board/drivers/interrupts.h @@ -14,8 +14,6 @@ typedef struct interrupt { uint32_t call_rate_fault; } interrupt; -void interrupt_timer_init(void); -uint32_t microsecond_timer_get(void); void unused_interrupt_handler(void); extern interrupt interrupts[NUM_INTERRUPTS]; diff --git a/board/drivers/usb.h b/board/drivers/usb.h index 87f285d0ba4..9ebd8f821cd 100644 --- a/board/drivers/usb.h +++ b/board/drivers/usb.h @@ -25,9 +25,6 @@ typedef union _USB_Setup { } b; } USB_Setup_TypeDef; -void usb_init(void); -void refresh_can_tx_slots_available(void); - // **** supporting defines **** #define USB_REQ_GET_STATUS 0x00 #define USB_REQ_CLEAR_FEATURE 0x01 diff --git a/board/globals.h b/board/globals.h index 02df9cc8898..2e7a48c95dc 100644 --- a/board/globals.h +++ b/board/globals.h @@ -5,10 +5,8 @@ #include "board/board_struct.h" #include "board/drivers/harness.h" -typedef struct board board; - extern uint8_t hw_type; -extern board *current_board; +extern struct board *current_board; extern struct harness_t harness; extern uint32_t uptime_cnt; extern int _app_start[0xc000]; \ No newline at end of file diff --git a/board/main.c b/board/main.c index db4e3714ac2..342d18a63b4 100644 --- a/board/main.c +++ b/board/main.c @@ -18,6 +18,7 @@ #include "board/drivers/fdcan.h" #include "board/main_declarations.h" #include "board/print.h" +#include "board/main_comms.h" // ********************* Serial debugging ********************* diff --git a/board/stm32h7/board.h b/board/stm32h7/board.h index 95f535c16ad..cabd7cd5804 100644 --- a/board/stm32h7/board.h +++ b/board/stm32h7/board.h @@ -11,6 +11,4 @@ #include "board/stm32h7/sound.h" #include "board/drivers/fake_siren.h" #include "board/drivers/clock_source.h" -#include "board/boards/cuatro.h" - -void detect_board_type(void); \ No newline at end of file +#include "board/boards/cuatro.h" \ No newline at end of file diff --git a/board/stm32h7/lluart.h b/board/stm32h7/lluart.h index 0f512dab273..f2ee48b1e77 100644 --- a/board/stm32h7/lluart.h +++ b/board/stm32h7/lluart.h @@ -1,9 +1,8 @@ #pragma once struct uart_ring; -typedef struct uart_ring uart_ring; // 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_init(uart_ring *q, unsigned int baud); -void uart_tx_ring(uart_ring *q); \ No newline at end of file +void uart_init(struct uart_ring *q, unsigned int baud); +void uart_tx_ring(struct uart_ring *q); \ No newline at end of file From bb0b5fcbf7271d426f05a71e2b86f35d6b84fc27 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 21:32:11 +0100 Subject: [PATCH 19/29] Misra test - specify files manually --- tests/misra/test_misra.sh | 61 +++++++++++++++++++++++++++++++++++---- 1 file changed, 56 insertions(+), 5 deletions(-) diff --git a/tests/misra/test_misra.sh b/tests/misra/test_misra.sh index f6fd56da229..fab23d68d55 100755 --- a/tests/misra/test_misra.sh +++ b/tests/misra/test_misra.sh @@ -1,4 +1,5 @@ #!/usr/bin/env bash +set -e DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" PANDA_DIR=$(realpath $DIR/../../) @@ -40,17 +41,18 @@ cppcheck() { # note that cppcheck build cache results in inconsistent results as of v2.13.0 OUTPUT=$DIR/.output.log - + echo -e "\n\n\n\n\nTEST variant options:" >> $CHECKLIST echo -e ""${@//$PANDA_DIR/}"\n\n" >> $CHECKLIST # (absolute path removed) $CPPCHECK_DIR/cppcheck --inline-suppr \ + -I $PANDA_DIR \ -I "$(arm-none-eabi-gcc -print-file-name=include)" \ - --enable=all --addon=misra \ + -I $OPENDBC_ROOT \ --suppressions-list=$DIR/suppressions.txt --suppress=*:*inc/* \ --suppress=*:*include/* --error-exitcode=2 --check-level=exhaustive --safety \ --platform=arm32-wchar_t4 $COMMON_DEFINES --checkers-report=$CHECKLIST.tmp \ - --std=c11 --project=compile_commands.json -icrypto 2>&1 | tee $OUTPUT + --std=c11 "$@" 2>&1 | tee $OUTPUT cat $CHECKLIST.tmp >> $CHECKLIST rm $CHECKLIST.tmp @@ -62,9 +64,58 @@ cppcheck() { fi } -printf "\n${GREEN}** PANDA H7 CODE **${NC}\n" +PANDA_OPTS="--enable=all --disable=unusedFunction --addon=misra" -cppcheck + +printf "\n${GREEN}** PANDA H7 CODE **${NC}\n" +cppcheck $PANDA_OPTS -DSTM32H7 -DSTM32H725xx -I $PANDA_DIR/board/stm32h7/inc/ -DPANDA \ + $PANDA_DIR/board/libc.c \ + $PANDA_DIR/board/early_init.c \ + $PANDA_DIR/board/critical.c \ + $PANDA_DIR/board/drivers/led.c \ + $PANDA_DIR/board/drivers/pwm.c \ + $PANDA_DIR/board/drivers/gpio.c \ + $PANDA_DIR/board/drivers/fake_siren.c \ + $PANDA_DIR/board/stm32h7/lli2c.c \ + $PANDA_DIR/board/stm32h7/clock.c \ + $PANDA_DIR/board/drivers/clock_source.c \ + $PANDA_DIR/board/stm32h7/sound.c \ + $PANDA_DIR/board/stm32h7/llflash.c \ + $PANDA_DIR/board/stm32h7/stm32h7_config.c \ + $PANDA_DIR/board/drivers/registers.c \ + $PANDA_DIR/board/drivers/interrupts.c \ + $PANDA_DIR/board/provision.c \ + $PANDA_DIR/board/stm32h7/peripherals.c \ + $PANDA_DIR/board/stm32h7/llusb.c \ + $PANDA_DIR/board/drivers/usb.c \ + $PANDA_DIR/board/drivers/spi.c \ + $PANDA_DIR/board/drivers/timers.c \ + $PANDA_DIR/board/stm32h7/lladc.c \ + $PANDA_DIR/board/stm32h7/llspi.c \ + $PANDA_DIR/board/faults.c \ + $PANDA_DIR/board/boards/unused_funcs.c \ + $PANDA_DIR/board/utils.c \ + $PANDA_DIR/board/globals.c \ + $PANDA_DIR/board/obj/gitversion.c \ + $PANDA_DIR/board/can_comms.c \ + $PANDA_DIR/board/drivers/fan.c \ + $PANDA_DIR/board/power_saving.c \ + $PANDA_DIR/board/drivers/uart.c \ + $PANDA_DIR/board/stm32h7/llfdcan.c \ + $PANDA_DIR/board/drivers/harness.c \ + $PANDA_DIR/board/drivers/bootkick.c \ + $PANDA_DIR/board/stm32h7/llfan.c \ + $PANDA_DIR/board/stm32h7/lluart.c \ + $PANDA_DIR/board/drivers/fdcan.c \ + $PANDA_DIR/board/drivers/can_common.c \ + $PANDA_DIR/board/main_comms.c \ + $PANDA_DIR/board/main.c \ + $PANDA_DIR/board/drivers/simple_watchdog.c \ + $PANDA_DIR/board/stm32h7/board.c \ + $PANDA_DIR/board/boards/tres.c \ + $PANDA_DIR/board/boards/red.c \ + $PANDA_DIR/board/boards/cuatro.c \ + $PANDA_DIR/board/main_definitions.c printf "\n${GREEN}Success!${NC} took $SECONDS seconds\n" From 5b37eb33ff5f74536384ffa30dc8fc53087c77dd Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 21:51:27 +0100 Subject: [PATCH 20/29] Misra fixups --- board/drivers/can_common.c | 1 - board/drivers/can_common.h | 10 +--------- board/drivers/fdcan.h | 7 +------ board/drivers/spi.c | 2 +- board/drivers/uart.c | 13 +++++++++---- board/drivers/uart.h | 1 + 6 files changed, 13 insertions(+), 21 deletions(-) diff --git a/board/drivers/can_common.c b/board/drivers/can_common.c index f0155e584d1..dc22d2272a2 100644 --- a/board/drivers/can_common.c +++ b/board/drivers/can_common.c @@ -22,7 +22,6 @@ bool can_loopback = false; // ********************* instantiate queues ********************* #define can_buffer(x, size) \ static CANPacket_t elems_##x[size]; \ - extern can_ring can_##x; \ can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = (size), .elems = (CANPacket_t *)&(elems_##x) }; #define CAN_RX_BUFFER_SIZE 4096U diff --git a/board/drivers/can_common.h b/board/drivers/can_common.h index 209053a9ae2..09cbca04a37 100644 --- a/board/drivers/can_common.h +++ b/board/drivers/can_common.h @@ -39,7 +39,7 @@ extern uint32_t rx_buffer_overflow; extern can_health_t can_health[PANDA_CAN_CNT]; -// Ignition detected from CAN meessages +// Ignition detected from CAN messages extern bool ignition_can; extern uint32_t ignition_can_cnt; @@ -82,10 +82,6 @@ void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook); bool is_speed_valid(uint32_t speed, const uint32_t *all_speeds, uint8_t len); // ********************* instantiate queues ********************* -#define can_buffer(x, size) \ - static CANPacket_t elems_##x[size]; \ - extern can_ring can_##x; \ - can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = (size), .elems = (CANPacket_t *)&(elems_##x) }; #define CAN_RX_BUFFER_SIZE 4096U #define CAN_TX_BUFFER_SIZE 416U @@ -101,7 +97,3 @@ extern can_ring can_tx1_q; extern can_ring can_tx2_q; #endif extern can_ring can_tx3_q; - -// FIXME: -// cppcheck-suppress misra-c2012-9.3 -// can_ring *can_queues[PANDA_CAN_CNT] = {&can_tx1_q, &can_tx2_q, &can_tx3_q}; diff --git a/board/drivers/fdcan.h b/board/drivers/fdcan.h index a4b9f74f76b..89e1ab3c380 100644 --- a/board/drivers/fdcan.h +++ b/board/drivers/fdcan.h @@ -15,9 +15,4 @@ typedef struct { extern FDCAN_GlobalTypeDef *cans[PANDA_CAN_CNT]; void can_clear_send(FDCAN_GlobalTypeDef *FDCANx, uint8_t can_number); -void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg); - -// ***************************** CAN ***************************** -// FDFDCANx_IT1 IRQ Handler (TX) -void process_can(uint8_t can_number); -bool can_init(uint8_t can_number); \ No newline at end of file +void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg); \ No newline at end of file diff --git a/board/drivers/spi.c b/board/drivers/spi.c index 5f060d55b7a..663e6e4b720 100644 --- a/board/drivers/spi.c +++ b/board/drivers/spi.c @@ -9,7 +9,6 @@ // H7 DMA2 located in D2 domain, so we need to use SRAM1/SRAM2 __attribute__((section(".sram12"))) static uint8_t spi_buf_rx[SPI_BUF_SIZE]; -__attribute__((section(".sram12"))) static uint8_t spi_buf_tx[SPI_BUF_SIZE]; uint16_t spi_error_count = 0; @@ -104,6 +103,7 @@ void spi_rx_done(void) { bool checksum_valid = false; static uint8_t spi_endpoint; static uint16_t spi_data_len_miso; + __attribute__((section(".sram12"))) static uint8_t spi_buf_tx[SPI_BUF_SIZE]; // parse header spi_endpoint = spi_buf_rx[1]; diff --git a/board/drivers/uart.c b/board/drivers/uart.c index 9123f7ec15d..b8ba52542eb 100644 --- a/board/drivers/uart.c +++ b/board/drivers/uart.c @@ -5,7 +5,6 @@ #define UART_BUFFER(x, size_rx, size_tx, uart_ptr, callback_ptr, overwrite_mode) \ static uint8_t elems_rx_##x[size_rx]; \ static uint8_t elems_tx_##x[size_tx]; \ - extern uart_ring uart_ring_##x; \ uart_ring uart_ring_##x = { \ .w_ptr_tx = 0, \ .r_ptr_tx = 0, \ @@ -50,7 +49,9 @@ bool get_char(uart_ring *q, char *elem) { ENTER_CRITICAL(); if (q->w_ptr_rx != q->r_ptr_rx) { - if (elem != NULL) *elem = q->elems_rx[q->r_ptr_rx]; + if (elem != NULL) { + *elem = q->elems_rx[q->r_ptr_rx]; + } q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size; ret = true; } @@ -113,7 +114,9 @@ void putch(const char a) { void print(const char *a) { for (const char *in = a; *in; in++) { - if (*in == '\n') putch('\r'); + if (*in == '\n') { + putch('\r'); + } putch(*in); } } @@ -139,7 +142,9 @@ void puth4(unsigned int i) { void hexdump(const void *a, int l) { if (a != NULL) { for (int i=0; i < l; i++) { - if ((i != 0) && ((i & 0xf) == 0)) print("\n"); + if ((i != 0) && ((i & 0xf) == 0)) { + print("\n"); + } puthx(((const unsigned char*)a)[i], 2U); print(" "); } diff --git a/board/drivers/uart.h b/board/drivers/uart.h index a249baf7166..c3197591962 100644 --- a/board/drivers/uart.h +++ b/board/drivers/uart.h @@ -35,6 +35,7 @@ void clear_uart_buff(uart_ring *q); // ******************************** UART buffers ******************************** +extern uart_ring uart_ring_debug; extern uart_ring uart_ring_som_debug; uart_ring *get_ring_by_number(int a); From 5cb152a99308820f9bd41dc2834f50056e8d57e0 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 22:05:57 +0100 Subject: [PATCH 21/29] Misra fixups --- board/boards/unused_funcs.c | 1 + board/drivers/interrupts.c | 1 + board/libc.c | 1 + board/main.c | 1 + board/stm32h7/clock.c | 1 + board/stm32h7/llflash.c | 18 ++++++++++++------ board/stm32h7/lluart.c | 1 + 7 files changed, 18 insertions(+), 6 deletions(-) diff --git a/board/boards/unused_funcs.c b/board/boards/unused_funcs.c index d3eb2648c00..28ce3d2e18d 100644 --- a/board/boards/unused_funcs.c +++ b/board/boards/unused_funcs.c @@ -29,6 +29,7 @@ bool unused_read_som_gpio(void) { return false; } +// cppcheck-suppress misra-c2012-8.7 void unused_set_amp_enabled(bool enabled) { UNUSED(enabled); } diff --git a/board/drivers/interrupts.c b/board/drivers/interrupts.c index a3edfd81bf7..f577fcb7cdb 100644 --- a/board/drivers/interrupts.c +++ b/board/drivers/interrupts.c @@ -16,6 +16,7 @@ static uint32_t idle_time = 0U; static uint32_t busy_time = 0U; float interrupt_load = 0.0f; +// cppcheck-suppress misra-c2012-8.7 void handle_interrupt(IRQn_Type irq_type){ static uint8_t interrupt_depth = 0U; static uint32_t last_time = 0U; diff --git a/board/libc.c b/board/libc.c index 780c49a8656..500a4f88d78 100644 --- a/board/libc.c +++ b/board/libc.c @@ -16,6 +16,7 @@ void assert_fatal(bool condition, const char *msg) { } } +// cppcheck-suppress misra-c2012-8.7 // cppcheck-suppress misra-c2012-21.2 void *memset(void *str, int c, unsigned int n) { uint8_t *s = str; diff --git a/board/main.c b/board/main.c index 342d18a63b4..024638a6120 100644 --- a/board/main.c +++ b/board/main.c @@ -93,6 +93,7 @@ bool is_car_safety_mode(uint16_t mode) { // cppcheck-suppress unusedFunction ; used in headers not included in cppcheck // cppcheck-suppress misra-c2012-8.4 +// cppcheck-suppress misra-c2012-8.7 void __initialize_hardware_early(void) { early_initialization(); } diff --git a/board/stm32h7/clock.c b/board/stm32h7/clock.c index 99bb9b2de1c..1252ea4d6e0 100644 --- a/board/stm32h7/clock.c +++ b/board/stm32h7/clock.c @@ -38,6 +38,7 @@ static PackageSMPSType get_package_smps_type(void) { break; default: ret = PACKAGE_UNKNOWN; + break; } return ret; } diff --git a/board/stm32h7/llflash.c b/board/stm32h7/llflash.c index 6ec54c282a2..69448944189 100644 --- a/board/stm32h7/llflash.c +++ b/board/stm32h7/llflash.c @@ -2,36 +2,42 @@ #include "stm32h7_config.h" +// cppcheck-suppress misra-c2012-8.7 bool flash_is_locked(void) { return (FLASH->CR1 & FLASH_CR_LOCK); } +// cppcheck-suppress misra-c2012-8.7 void flash_unlock(void) { FLASH->KEYR1 = 0x45670123u; FLASH->KEYR1 = 0xCDEF89ABu; } +// cppcheck-suppress misra-c2012-8.7 bool flash_erase_sector(uint8_t sector, bool unlocked) { // don't erase the bootloader(sector 0) + bool ret = false; if (sector != 0 && sector < 8 && unlocked) { FLASH->CR1 = (sector << 8) | FLASH_CR_SER; FLASH->CR1 |= FLASH_CR_START; - while (FLASH->SR1 & FLASH_SR_QW) {} - return true; + while ((FLASH->SR1 & FLASH_SR_QW) != 0u) {} + ret = true; } - return false; + return ret; } +// cppcheck-suppress misra-c2012-8.7 void flash_write_word(void *prog_ptr, uint32_t data) { uint32_t *pp = prog_ptr; FLASH->CR1 |= FLASH_CR_PG; *pp = data; - while (FLASH->SR1 & FLASH_SR_QW) {} + while ((FLASH->SR1 & FLASH_SR_QW) != 0u) {} } +// cppcheck-suppress misra-c2012-8.7 void flush_write_buffer(void) { - if (FLASH->SR1 & FLASH_SR_WBNE) { + if ((FLASH->SR1 & FLASH_SR_WBNE) != 0u) { FLASH->CR1 |= FLASH_CR_FW; - while (FLASH->SR1 & FLASH_CR_FW) {} + while ((FLASH->SR1 & FLASH_CR_FW) != 0u) {} } } diff --git a/board/stm32h7/lluart.c b/board/stm32h7/lluart.c index fb7c532a76e..4a5df01fe0f 100644 --- a/board/stm32h7/lluart.c +++ b/board/stm32h7/lluart.c @@ -88,6 +88,7 @@ static void uart_interrupt_handler(uart_ring *q) { static void UART7_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_som_debug); } +// cppcheck-suppress misra-c2012-8.7 void uart_init(uart_ring *q, unsigned int baud) { if (q->uart == UART7) { REGISTER_INTERRUPT(UART7_IRQn, UART7_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_7) From 10e9ed08ff9a7fe220ba5c9698731715b592801e Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 22:22:19 +0100 Subject: [PATCH 22/29] Misra fixups --- board/drivers/usb.c | 5 +++-- board/stm32h7/llflash.c | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/board/drivers/usb.c b/board/drivers/usb.c index b900b2436dd..f29e2be592c 100644 --- a/board/drivers/usb.c +++ b/board/drivers/usb.c @@ -501,6 +501,7 @@ static void usb_setup(void) { break; default: USB_WritePacket_EP0(0, 0); + break; } break; default: @@ -614,7 +615,7 @@ void usb_irqhandler(void) { } } - /*if (gintsts & USB_OTG_GINTSTS_HPRTINT) { +#if 0 // host print("HPRT:"); puth(USBx_HOST_PORT->HPRT); @@ -624,7 +625,7 @@ void usb_irqhandler(void) { USBx_HOST_PORT->HPRT |= USB_OTG_HPRT_PCDET; } - }*/ +#endif if ((gintsts & USB_OTG_GINTSTS_BOUTNAKEFF) || (gintsts & USB_OTG_GINTSTS_GINAKEFF)) { // no global NAK, why is this getting set? diff --git a/board/stm32h7/llflash.c b/board/stm32h7/llflash.c index 69448944189..28e6c707a93 100644 --- a/board/stm32h7/llflash.c +++ b/board/stm32h7/llflash.c @@ -17,8 +17,8 @@ void flash_unlock(void) { bool flash_erase_sector(uint8_t sector, bool unlocked) { // don't erase the bootloader(sector 0) bool ret = false; - if (sector != 0 && sector < 8 && unlocked) { - FLASH->CR1 = (sector << 8) | FLASH_CR_SER; + if ((sector != 0u) && (sector < 8u) && unlocked) { + FLASH->CR1 = ((uint32_t) sector << 8u) | FLASH_CR_SER; FLASH->CR1 |= FLASH_CR_START; while ((FLASH->SR1 & FLASH_SR_QW) != 0u) {} ret = true; From e95af6628947718dcda3082a2ad4c4f9f127e138 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 22:23:28 +0100 Subject: [PATCH 23/29] Silence misra errors raised by opendbc From e25e1ef48b5b6d308c53b80a065d93c06a87694e Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 22:28:44 +0100 Subject: [PATCH 24/29] Cleanups build script --- SConscript | 146 ++++++++++++++++++++++++++++------------------------- 1 file changed, 77 insertions(+), 69 deletions(-) diff --git a/SConscript b/SConscript index 32cbae6c401..bae412b3e5a 100644 --- a/SConscript +++ b/SConscript @@ -90,7 +90,7 @@ def build_project(project_name, project, main, shared, extra_flags): CFLAGS=flags, ASFLAGS=flags, LINKFLAGS=flags, - CPPPATH=[Dir(""), "board/stm32h7/inc", opendbc.INCLUDE_PATH], + CPPPATH=[Dir("./"), "./board/stm32h7/inc", opendbc.INCLUDE_PATH], ASCOM="$AS $ASFLAGS -o $TARGET -c $SOURCES", BUILDERS={ 'Objcopy': Builder(generator=objcopy, suffix='.bin', src_suffix='.elf') @@ -101,35 +101,36 @@ def build_project(project_name, project, main, shared, extra_flags): startup = env.Object(project["STARTUP_FILE"]) shared += [ - "crypto/rsa.c", - "crypto/sha.c", - "board/libc.c", - "board/early_init.c", - "board/critical.c", - "board/drivers/led.c", - "board/drivers/pwm.c", - "board/drivers/gpio.c", - "board/drivers/fake_siren.c", - "board/stm32h7/lli2c.c", - "board/stm32h7/clock.c", - "board/drivers/clock_source.c", - "board/stm32h7/sound.c", - "board/stm32h7/llflash.c", - "board/stm32h7/stm32h7_config.c", - "board/drivers/registers.c", - "board/drivers/interrupts.c", - "board/provision.c", - "board/stm32h7/peripherals.c", - "board/stm32h7/llusb.c", - "board/drivers/usb.c", - "board/drivers/spi.c", - "board/drivers/timers.c", - "board/stm32h7/lladc.c", - "board/stm32h7/llspi.c", - "board/faults.c", - "board/boards/unused_funcs.c", - "board/utils.c", - "board/globals.c", + "./crypto/rsa.c", + "./crypto/sha.c", + "./board/libc.c", + "./board/early_init.c", + "./board/critical.c", + "./board/drivers/led.c", + "./board/drivers/pwm.c", + "./board/drivers/gpio.c", + "./board/drivers/fake_siren.c", + "./board/stm32h7/lli2c.c", + "./board/stm32h7/clock.c", + "./board/drivers/clock_source.c", + "./board/stm32h7/sound.c", + "./board/stm32h7/llflash.c", + "./board/stm32h7/stm32h7_config.c", + "./board/drivers/registers.c", + "./board/drivers/interrupts.c", + "./board/provision.c", + "./board/stm32h7/peripherals.c", + "./board/stm32h7/llusb.c", + "./board/drivers/usb.c", + "./board/drivers/spi.c", + "./board/drivers/timers.c", + "./board/stm32h7/lladc.c", + "./board/stm32h7/llspi.c", + "./board/faults.c", + "./board/boards/unused_funcs.c", + "./board/utils.c", + "./board/globals.c", + "./board/obj/gitversion.c", ] # Build bootstub @@ -140,35 +141,35 @@ def build_project(project_name, project, main, shared, extra_flags): bs_env.Append(CFLAGS="-DBOOTSTUB", ASFLAGS="-DBOOTSTUB", LINKFLAGS="-DBOOTSTUB") bs_elf = bs_env.Program(f"{project_dir}/bootstub.elf", [ startup, - "board/bootstub.c", - "board/flasher.c", + "./board/bootstub.c", + "./board/flasher.c", ] + shared) - bs_env.Objcopy(f"board/obj/bootstub.{project_name}.bin", bs_elf) + bs_env.Objcopy(f"./board/obj/bootstub.{project_name}.bin", bs_elf) # Build + sign main (aka app) main_elf = env.Program(f"{project_dir}/main.elf", [ startup, - "board/can_comms.c", - "board/drivers/fan.c", - "board/power_saving.c", - "board/drivers/uart.c", - "board/stm32h7/llfdcan.c", - "board/drivers/harness.c", + "./board/can_comms.c", + "./board/drivers/fan.c", + "./board/power_saving.c", + "./board/drivers/uart.c", + "./board/stm32h7/llfdcan.c", + "./board/drivers/harness.c", - "board/drivers/bootkick.c", - "board/stm32h7/llfan.c", - "board/stm32h7/lluart.c", - "board/drivers/fdcan.c", - "board/drivers/can_common.c", + "./board/drivers/bootkick.c", + "./board/stm32h7/llfan.c", + "./board/stm32h7/lluart.c", + "./board/drivers/fdcan.c", + "./board/drivers/can_common.c", main, ] + shared, LINKFLAGS=[f"-Wl,--section-start,.isr_vector={project['APP_START_ADDRESS']}"] + flags) main_bin = env.Objcopy(f"{project_dir}/main.bin", main_elf) sign_py = File(f"crypto/sign.py").srcnode().relpath - env.Command(f"board/obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}") + env.Command(f"./board/obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}") base_project_h7 = { - "STARTUP_FILE": "board/stm32h7/startup_stm32h7x5xx.s", - "LINKER_SCRIPT": "board/stm32h7/stm32h7x5_flash.ld", + "STARTUP_FILE": "./board/stm32h7/startup_stm32h7x5xx.s", + "LINKER_SCRIPT": "./board/stm32h7/stm32h7x5_flash.ld", "APP_START_ADDRESS": "0x8020000", "FLAGS": [ "-mcpu=cortex-m7", @@ -181,30 +182,37 @@ base_project_h7 = { } # Common autogenerated includes -with open("board/obj/gitversion.h", "w") as f: +with open("./board/obj/gitversion.c", "w") as f: version = get_version(BUILDER, BUILD_TYPE) - f.write(f'static const uint8_t gitversion[{len(version)}] = "{version}";\n') + f.write('#include "gitversion.h"\n') + f.write(f'const uint8_t gitversion[{len(version)}] = "{version}";\n') -with open("board/obj/version", "w") as f: +with open("./board/obj/gitversion.h", "w") as f: + version = get_version(BUILDER, BUILD_TYPE) + f.write("#pragma once\n") + f.write("#include \n") + f.write(f'extern const uint8_t gitversion[{len(version)}];\n') + +with open("./board/obj/version", "w") as f: f.write(f'{get_version(BUILDER, BUILD_TYPE)}') certs = [get_key_header(n) for n in ["debug", "release"]] -with open("board/obj/cert.h", "w") as f: +with open("./board/obj/cert.h", "w") as f: for cert in certs: f.write("\n".join(cert) + "\n") panda_main = [ - "board/main_comms.c", - "board/main.c", - "board/drivers/simple_watchdog.c", + "./board/main_comms.c", + "./board/main.c", + "./board/drivers/simple_watchdog.c", ] panda_shared = [ - "board/stm32h7/board.c", - "board/boards/tres.c", - "board/boards/red.c", - "board/boards/cuatro.c", - "board/main_definitions.c" + "./board/stm32h7/board.c", + "./board/boards/tres.c", + "./board/boards/red.c", + "./board/boards/cuatro.c", + "./board/main_definitions.c" ] build_project("panda_h7", base_project_h7, panda_main, panda_shared, ["-DPANDA"]) @@ -217,25 +225,25 @@ if os.getenv("FINAL_PROVISIONING"): flags += ["-DFINAL_PROVISIONING"] jungle_main = [ - "board/jungle/main_comms.c", - "board/jungle/main.c", + "./board/jungle/main_comms.c", + "./board/jungle/main.c", ] jungle_shared = [ - "board/jungle/stm32h7/board.c", - "board/jungle/boards/board_v2.c", + "./board/jungle/stm32h7/board.c", + "./board/jungle/boards/board_v2.c", ] build_project("panda_jungle_h7", base_project_h7, jungle_main, jungle_shared, flags) # body fw body_main = [ - "board/body/main.c", - "board/body/main_comms.c", + "./board/body/main.c", + "./board/body/main_comms.c", ] body_shared = [ - "board/body/boards/board_body.c", - "board/body/stm32h7/board.c", - "board/body/motor_control.c", - "board/body/motor_encoder.c" + "./board/body/boards/board_body.c", + "./board/body/stm32h7/board.c", + "./board/body/motor_control.c", + "./board/body/motor_encoder.c" ] build_project("body_h7", base_project_h7, body_main, body_shared, ["-DPANDA_BODY"]) From 3424c0795a0d96ee3e6540fe1efe9b71827afd50 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 23:21:58 +0100 Subject: [PATCH 25/29] Misra fixups --- board/drivers/interrupts.c | 3 ++- board/flasher.c | 32 +++++++++++++++++++------------- board/flasher.h | 2 -- board/stm32h7/llflash.c | 4 ++-- tests/misra/checkers.txt | 4 ++-- tests/misra/test_misra.sh | 4 ++-- 6 files changed, 27 insertions(+), 22 deletions(-) diff --git a/board/drivers/interrupts.c b/board/drivers/interrupts.c index f577fcb7cdb..3da5410ca86 100644 --- a/board/drivers/interrupts.c +++ b/board/drivers/interrupts.c @@ -17,7 +17,8 @@ static uint32_t busy_time = 0U; float interrupt_load = 0.0f; // cppcheck-suppress misra-c2012-8.7 -void handle_interrupt(IRQn_Type irq_type){ +// cppcheck-suppress unusedFunction ; used in assembly +void handle_interrupt(IRQn_Type irq_type) { static uint8_t interrupt_depth = 0U; static uint32_t last_time = 0U; ENTER_CRITICAL(); diff --git a/board/flasher.c b/board/flasher.c index f69bf74424b..9d3cc1e5963 100644 --- a/board/flasher.c +++ b/board/flasher.c @@ -12,19 +12,19 @@ #include "board/config.h" // flasher state variables -uint32_t *prog_ptr = NULL; +uint32_t *program_ptr = NULL; bool unlocked = false; -int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { +int comms_control_handler(ControlPacket_t * const req, uint8_t *resp) { int resp_len = 0; // flasher machine - memset(resp, 0, 4); - memcpy(resp+4, "\xde\xad\xd0\x0d", 4); + (void)memset(resp, 0, 4); + (void)memcpy(resp+4, "\xde\xad\xd0\x0d", 4); resp[0] = 0xff; resp[2] = req->request; resp[3] = ~req->request; - *((uint32_t **)&resp[8]) = prog_ptr; + *((uint32_t **)&resp[8]) = program_ptr; resp_len = 0xc; int sec; @@ -41,7 +41,7 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { } led_set(LED_GREEN, 1); unlocked = true; - prog_ptr = (uint32_t *)APP_START_ADDRESS; + program_ptr = (uint32_t *)APP_START_ADDRESS; break; // **** 0xb2: erase sector case 0xb2: @@ -63,8 +63,9 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { // **** 0xd0: fetch serial number case 0xd0: // addresses are OTP - if (req->param1 == 1) { - memcpy(resp, (void *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10); + if (req->param1 == 1u) { + volatile const uint8_t *deviceSerialPtr = (volatile const uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS; + memcpy(resp, (void *)deviceSerialPtr, 0x10); resp_len = 0x10; } else { get_provision_chunk(resp); @@ -85,12 +86,14 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; NVIC_SystemReset(); break; + default: + break; } break; // **** 0xd6: get version case 0xd6: COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE); - memcpy(resp, gitversion, sizeof(gitversion)); + (void)memcpy(resp, gitversion, sizeof(gitversion)); resp_len = sizeof(gitversion) - 1U; break; // **** 0xd8: reset ST @@ -98,6 +101,9 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { flush_write_buffer(); NVIC_SystemReset(); break; + default: + // Unknown request + break; } return resp_len; } @@ -117,11 +123,11 @@ void refresh_can_tx_slots_available(void) {} void comms_endpoint2_write(const uint8_t *data, uint32_t len) { led_set(LED_RED, 0); - for (uint32_t i = 0; i < len/4; i++) { - flash_write_word(prog_ptr, *(uint32_t*)(data+(i*4))); + for (uint32_t i = 0; i < len/4u; i++) { + flash_write_word(program_ptr, *(uint32_t*)(data+(i*4u))); - //*(uint64_t*)(&spi_tx_buf[0x30+(i*4)]) = *prog_ptr; - prog_ptr++; + //*(uint64_t*)(&spi_tx_buf[0x30+(i*4)]) = *program_ptr; + program_ptr++; } led_set(LED_RED, 1); } diff --git a/board/flasher.h b/board/flasher.h index 173dc9d4145..2a25423456c 100644 --- a/board/flasher.h +++ b/board/flasher.h @@ -12,8 +12,6 @@ extern uint32_t *prog_ptr; extern bool unlocked; -void spi_init(void); - int comms_control_handler(ControlPacket_t *req, uint8_t *resp); void comms_can_write(const uint8_t *data, uint32_t len); int comms_can_read(uint8_t *data, uint32_t max_len); diff --git a/board/stm32h7/llflash.c b/board/stm32h7/llflash.c index 28e6c707a93..ba96253845e 100644 --- a/board/stm32h7/llflash.c +++ b/board/stm32h7/llflash.c @@ -14,10 +14,10 @@ void flash_unlock(void) { } // cppcheck-suppress misra-c2012-8.7 -bool flash_erase_sector(uint8_t sector, bool unlocked) { +bool flash_erase_sector(uint8_t sector, bool is_unlocked) { // don't erase the bootloader(sector 0) bool ret = false; - if ((sector != 0u) && (sector < 8u) && unlocked) { + if ((sector != 0u) && (sector < 8u) && is_unlocked) { FLASH->CR1 = ((uint32_t) sector << 8u) | FLASH_CR_SER; FLASH->CR1 |= FLASH_CR_START; while ((FLASH->SR1 & FLASH_SR_QW) != 0u) {} diff --git a/tests/misra/checkers.txt b/tests/misra/checkers.txt index 13036c65d95..9c9f0b9af0d 100644 --- a/tests/misra/checkers.txt +++ b/tests/misra/checkers.txt @@ -5,7 +5,7 @@ Cppcheck checkers list from test_misra.sh: TEST variant options: ---enable=all --disable=unusedFunction --addon=misra -DSTM32H7 -DSTM32H725xx -I /board/stm32h7/inc/ /board/main.c -DPANDA +--enable=all --addon=misra -DSTM32H7 -DSTM32H725xx -I /board/stm32h7/inc/ -DPANDA /board/libc.c /board/early_init.c /board/critical.c /board/drivers/led.c /board/drivers/pwm.c /board/drivers/gpio.c /board/drivers/fake_siren.c /board/stm32h7/lli2c.c /board/stm32h7/clock.c /board/drivers/clock_source.c /board/stm32h7/sound.c /board/stm32h7/llflash.c /board/stm32h7/stm32h7_config.c /board/drivers/registers.c /board/drivers/interrupts.c /board/provision.c /board/stm32h7/peripherals.c /board/stm32h7/llusb.c /board/drivers/usb.c /board/drivers/spi.c /board/drivers/timers.c /board/stm32h7/lladc.c /board/stm32h7/llspi.c /board/faults.c /board/boards/unused_funcs.c /board/utils.c /board/globals.c /board/obj/gitversion.c /board/can_comms.c /board/drivers/fan.c /board/power_saving.c /board/drivers/uart.c /board/stm32h7/llfdcan.c /board/drivers/harness.c /board/drivers/bootkick.c /board/stm32h7/llfan.c /board/stm32h7/lluart.c /board/drivers/fdcan.c /board/drivers/can_common.c /board/main_comms.c /board/main.c /board/drivers/simple_watchdog.c /board/stm32h7/board.c /board/boards/tres.c /board/boards/red.c /board/boards/cuatro.c /board/main_definitions.c Critical errors @@ -194,7 +194,7 @@ Yes CheckType::checkSignConversion Yes CheckType::checkTooBigBitwiseShift Yes CheckUninitVar::check Yes CheckUninitVar::valueFlowUninit -No CheckUnusedFunctions::check require:unusedFunction +Yes CheckUnusedFunctions::check Yes CheckUnusedVar::checkFunctionVariableUsage Yes CheckUnusedVar::checkStructMemberUsage Yes CheckVaarg::va_list_usage diff --git a/tests/misra/test_misra.sh b/tests/misra/test_misra.sh index fab23d68d55..2ab271bd29b 100755 --- a/tests/misra/test_misra.sh +++ b/tests/misra/test_misra.sh @@ -49,6 +49,7 @@ cppcheck() { -I $PANDA_DIR \ -I "$(arm-none-eabi-gcc -print-file-name=include)" \ -I $OPENDBC_ROOT \ + --suppress=misra-c2012-5.9:$OPENDBC_ROOT/* \ --suppressions-list=$DIR/suppressions.txt --suppress=*:*inc/* \ --suppress=*:*include/* --error-exitcode=2 --check-level=exhaustive --safety \ --platform=arm32-wchar_t4 $COMMON_DEFINES --checkers-report=$CHECKLIST.tmp \ @@ -64,8 +65,7 @@ cppcheck() { fi } -PANDA_OPTS="--enable=all --disable=unusedFunction --addon=misra" - +PANDA_OPTS="--enable=all --addon=misra" printf "\n${GREEN}** PANDA H7 CODE **${NC}\n" cppcheck $PANDA_OPTS -DSTM32H7 -DSTM32H725xx -I $PANDA_DIR/board/stm32h7/inc/ -DPANDA \ From a44ae6367eeb547567d39020006aae14ac177eba Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 23:35:49 +0100 Subject: [PATCH 26/29] Misra fixups --- board/libc.c | 1 + board/stm32h7/llflash.c | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/board/libc.c b/board/libc.c index 500a4f88d78..2d4f1eaf5a4 100644 --- a/board/libc.c +++ b/board/libc.c @@ -18,6 +18,7 @@ void assert_fatal(bool condition, const char *msg) { // cppcheck-suppress misra-c2012-8.7 // cppcheck-suppress misra-c2012-21.2 +// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck void *memset(void *str, int c, unsigned int n) { uint8_t *s = str; for (unsigned int i = 0; i < n; i++) { diff --git a/board/stm32h7/llflash.c b/board/stm32h7/llflash.c index ba96253845e..c901f571b40 100644 --- a/board/stm32h7/llflash.c +++ b/board/stm32h7/llflash.c @@ -3,17 +3,20 @@ #include "stm32h7_config.h" // cppcheck-suppress misra-c2012-8.7 +// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck bool flash_is_locked(void) { return (FLASH->CR1 & FLASH_CR_LOCK); } // cppcheck-suppress misra-c2012-8.7 +// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck void flash_unlock(void) { FLASH->KEYR1 = 0x45670123u; FLASH->KEYR1 = 0xCDEF89ABu; } // cppcheck-suppress misra-c2012-8.7 +// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck bool flash_erase_sector(uint8_t sector, bool is_unlocked) { // don't erase the bootloader(sector 0) bool ret = false; @@ -27,6 +30,7 @@ bool flash_erase_sector(uint8_t sector, bool is_unlocked) { } // cppcheck-suppress misra-c2012-8.7 +// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck void flash_write_word(void *prog_ptr, uint32_t data) { uint32_t *pp = prog_ptr; FLASH->CR1 |= FLASH_CR_PG; @@ -35,6 +39,7 @@ void flash_write_word(void *prog_ptr, uint32_t data) { } // cppcheck-suppress misra-c2012-8.7 +// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck void flush_write_buffer(void) { if ((FLASH->SR1 & FLASH_SR_WBNE) != 0u) { FLASH->CR1 |= FLASH_CR_FW; From 39c1716e7e75dfdeb61b267b118aa7206c69b87d Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Fri, 5 Dec 2025 23:46:19 +0100 Subject: [PATCH 27/29] Fix TODO --- SConscript | 5 ++--- board/boards/cuatro.c | 4 +++- board/boards/tres.c | 4 +++- board/bootstub.c | 13 +++++-------- board/drivers/uart.h | 5 ----- 5 files changed, 13 insertions(+), 18 deletions(-) diff --git a/SConscript b/SConscript index bae412b3e5a..6399e8648d6 100644 --- a/SConscript +++ b/SConscript @@ -131,6 +131,8 @@ def build_project(project_name, project, main, shared, extra_flags): "./board/utils.c", "./board/globals.c", "./board/obj/gitversion.c", + "./board/stm32h7/lluart.c", + "./board/drivers/uart.c", ] # Build bootstub @@ -152,13 +154,10 @@ def build_project(project_name, project, main, shared, extra_flags): "./board/can_comms.c", "./board/drivers/fan.c", "./board/power_saving.c", - "./board/drivers/uart.c", "./board/stm32h7/llfdcan.c", "./board/drivers/harness.c", - "./board/drivers/bootkick.c", "./board/stm32h7/llfan.c", - "./board/stm32h7/lluart.c", "./board/drivers/fdcan.c", "./board/drivers/can_common.c", main, diff --git a/board/boards/cuatro.c b/board/boards/cuatro.c index 3c14821855c..82914cf5fe7 100644 --- a/board/boards/cuatro.c +++ b/board/boards/cuatro.c @@ -4,6 +4,8 @@ #include "board/drivers/fake_siren.h" #include "board/boards/tres.h" #include "board/drivers/harness.h" +#include "board/drivers/uart.h" +#include "board/stm32h7/lluart.h" // ////////////////////////// // // Cuatro (STM32H7) + Harness // @@ -80,7 +82,7 @@ static void cuatro_init(void) { // SOM debugging UART gpio_uart7_init(); - // uart_init(&uart_ring_som_debug, 115200); // TODO + uart_init(&uart_ring_som_debug, 115200); // fan setup set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); diff --git a/board/boards/tres.c b/board/boards/tres.c index 5b97d8e813f..cef8dda19a2 100644 --- a/board/boards/tres.c +++ b/board/boards/tres.c @@ -3,6 +3,8 @@ #include "board/drivers/pwm.h" #include "board/globals.h" +#include "board/drivers/uart.h" +#include "board/stm32h7/lluart.h" // /////////////////////////// // Tres (STM32H7) + Harness // @@ -122,7 +124,7 @@ static void tres_init(void) { // SOM debugging UART gpio_uart7_init(); - // uart_init(&uart_ring_som_debug, 115200); // TODO + uart_init(&uart_ring_som_debug, 115200); // fan setup set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); diff --git a/board/bootstub.c b/board/bootstub.c index 851021d4a37..8fb2abd96a7 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -13,6 +13,7 @@ #include "board/obj/cert.h" #include "board/print.h" +#include "board/drivers/uart.h" #include "globals.h" @@ -21,14 +22,10 @@ void __initialize_hardware_early(void) { early_initialization(); } -void print(const char *a){ UNUSED(a); } -void puth(unsigned int i){ UNUSED(i); } -#if defined(DEBUG_SPI) || defined(BOOTSTUB) || defined(DEBUG) -void puth4(unsigned int i){ UNUSED(i); } -#endif -#if defined(DEBUG_SPI) || defined(DEBUG_USB) || defined(DEBUG_COMMS) -void hexdump(const void *a, int l){ UNUSED(a); UNUSED(l); } -#endif +void debug_ring_callback(uart_ring *ring) { + char rcv; + while (get_char(ring, &rcv)) { } // Intentionally disabled, no uart in bootstub +} static void fail(void) { soft_flasher_start(); diff --git a/board/drivers/uart.h b/board/drivers/uart.h index c3197591962..6caec120e5c 100644 --- a/board/drivers/uart.h +++ b/board/drivers/uart.h @@ -39,8 +39,3 @@ extern uart_ring uart_ring_debug; extern uart_ring uart_ring_som_debug; uart_ring *get_ring_by_number(int a); - -// ************************* Low-level buffer functions ************************* -bool get_char(uart_ring *q, char *elem); -bool injectc(uart_ring *q, char elem); -bool put_char(uart_ring *q, char elem); From 45c876ba386f247ccbc9f3046d9eb22d3e31394e Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Tue, 6 Jan 2026 17:18:03 +0100 Subject: [PATCH 28/29] Misra cleanups --- board/drivers/spi.h | 5 -- board/fake_stm.h | 2 +- board/stm32h7/interrupt_handlers.c | 1 + board/stm32h7/interrupt_handlers.h | 2 + board/stm32h7/llflash.c | 10 ---- board/stm32h7/lluart.h | 3 +- board/stm32h7/stm32h7_config.h | 1 - tests/misra/checkers.txt | 2 +- tests/misra/suppressions.txt | 3 +- tests/misra/test_misra.sh | 74 +++++++++++------------------- tests/misra/test_mutation.py | 2 + 11 files changed, 36 insertions(+), 69 deletions(-) diff --git a/board/drivers/spi.h b/board/drivers/spi.h index 6cadf5b484e..0d90ba8d8d0 100644 --- a/board/drivers/spi.h +++ b/board/drivers/spi.h @@ -29,11 +29,6 @@ enum { extern uint16_t spi_error_count; -// low level SPI prototypes -void llspi_init(void); -void llspi_mosi_dma(uint8_t *addr, int len); -void llspi_miso_dma(uint8_t *addr, int len); - void can_tx_comms_resume_spi(void); void spi_init(void); void spi_rx_done(void); diff --git a/board/fake_stm.h b/board/fake_stm.h index 07113847c95..45e0aab0086 100644 --- a/board/fake_stm.h +++ b/board/fake_stm.h @@ -1,7 +1,7 @@ #pragma once #ifdef STM32H7 -#error This code only makes on a non stm32h7 platform +#error This code only makes sense on a non stm32h7 platform #endif // minimal code to fake a panda for tests diff --git a/board/stm32h7/interrupt_handlers.c b/board/stm32h7/interrupt_handlers.c index 4c8c2cac974..92fb42eb156 100644 --- a/board/stm32h7/interrupt_handlers.c +++ b/board/stm32h7/interrupt_handlers.c @@ -1,5 +1,6 @@ #include "interrupt_handlers.h" #include "stm32h7_config.h" +#include "board/drivers/interrupts.h" void WWDG_IRQHandler(void) {handle_interrupt(WWDG_IRQn);} void PVD_AVD_IRQHandler(void) {handle_interrupt(PVD_AVD_IRQn);} diff --git a/board/stm32h7/interrupt_handlers.h b/board/stm32h7/interrupt_handlers.h index f73d43241ba..96e6ed32dda 100644 --- a/board/stm32h7/interrupt_handlers.h +++ b/board/stm32h7/interrupt_handlers.h @@ -3,6 +3,8 @@ #pragma once +#include "stm32h7_config.h" + void WWDG_IRQHandler(void); void PVD_AVD_IRQHandler(void); void TAMP_STAMP_IRQHandler(void); diff --git a/board/stm32h7/llflash.c b/board/stm32h7/llflash.c index c901f571b40..f6563447cce 100644 --- a/board/stm32h7/llflash.c +++ b/board/stm32h7/llflash.c @@ -2,21 +2,15 @@ #include "stm32h7_config.h" -// cppcheck-suppress misra-c2012-8.7 -// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck bool flash_is_locked(void) { return (FLASH->CR1 & FLASH_CR_LOCK); } -// cppcheck-suppress misra-c2012-8.7 -// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck void flash_unlock(void) { FLASH->KEYR1 = 0x45670123u; FLASH->KEYR1 = 0xCDEF89ABu; } -// cppcheck-suppress misra-c2012-8.7 -// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck bool flash_erase_sector(uint8_t sector, bool is_unlocked) { // don't erase the bootloader(sector 0) bool ret = false; @@ -29,8 +23,6 @@ bool flash_erase_sector(uint8_t sector, bool is_unlocked) { return ret; } -// cppcheck-suppress misra-c2012-8.7 -// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck void flash_write_word(void *prog_ptr, uint32_t data) { uint32_t *pp = prog_ptr; FLASH->CR1 |= FLASH_CR_PG; @@ -38,8 +30,6 @@ void flash_write_word(void *prog_ptr, uint32_t data) { while ((FLASH->SR1 & FLASH_SR_QW) != 0u) {} } -// cppcheck-suppress misra-c2012-8.7 -// cppcheck-suppress unusedFunction; Used in board/flasher.c, which is excluded from cppcheck void flush_write_buffer(void) { if ((FLASH->SR1 & FLASH_SR_WBNE) != 0u) { FLASH->CR1 |= FLASH_CR_FW; diff --git a/board/stm32h7/lluart.h b/board/stm32h7/lluart.h index f2ee48b1e77..c297f85d247 100644 --- a/board/stm32h7/lluart.h +++ b/board/stm32h7/lluart.h @@ -4,5 +4,4 @@ struct uart_ring; // 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_init(struct uart_ring *q, unsigned int baud); -void uart_tx_ring(struct uart_ring *q); \ No newline at end of file +void uart_init(struct uart_ring *q, unsigned int baud); \ No newline at end of file diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h index ce3d12e08e4..5caf5810213 100644 --- a/board/stm32h7/stm32h7_config.h +++ b/board/stm32h7/stm32h7_config.h @@ -57,7 +57,6 @@ separate IRQs for RX and TX. #include "board/drivers/interrupts.h" #include "board/drivers/gpio.h" #include "board/stm32h7/peripherals.h" -#include "board/stm32h7/interrupt_handlers.h" #include "board/drivers/timers.h" #if !defined(BOOTSTUB) diff --git a/tests/misra/checkers.txt b/tests/misra/checkers.txt index 9c9f0b9af0d..4058e1b8468 100644 --- a/tests/misra/checkers.txt +++ b/tests/misra/checkers.txt @@ -5,7 +5,7 @@ Cppcheck checkers list from test_misra.sh: TEST variant options: ---enable=all --addon=misra -DSTM32H7 -DSTM32H725xx -I /board/stm32h7/inc/ -DPANDA /board/libc.c /board/early_init.c /board/critical.c /board/drivers/led.c /board/drivers/pwm.c /board/drivers/gpio.c /board/drivers/fake_siren.c /board/stm32h7/lli2c.c /board/stm32h7/clock.c /board/drivers/clock_source.c /board/stm32h7/sound.c /board/stm32h7/llflash.c /board/stm32h7/stm32h7_config.c /board/drivers/registers.c /board/drivers/interrupts.c /board/provision.c /board/stm32h7/peripherals.c /board/stm32h7/llusb.c /board/drivers/usb.c /board/drivers/spi.c /board/drivers/timers.c /board/stm32h7/lladc.c /board/stm32h7/llspi.c /board/faults.c /board/boards/unused_funcs.c /board/utils.c /board/globals.c /board/obj/gitversion.c /board/can_comms.c /board/drivers/fan.c /board/power_saving.c /board/drivers/uart.c /board/stm32h7/llfdcan.c /board/drivers/harness.c /board/drivers/bootkick.c /board/stm32h7/llfan.c /board/stm32h7/lluart.c /board/drivers/fdcan.c /board/drivers/can_common.c /board/main_comms.c /board/main.c /board/drivers/simple_watchdog.c /board/stm32h7/board.c /board/boards/tres.c /board/boards/red.c /board/boards/cuatro.c /board/main_definitions.c +--enable=all --addon=misra -DSTM32H7 -DSTM32H725xx -I /board/stm32h7/inc/ -DPANDA /board/utils.c /board/stm32h7/board.c /board/stm32h7/llfan.c /board/stm32h7/llfdcan.c /board/stm32h7/peripherals.c /board/stm32h7/sound.c /board/stm32h7/llspi.c /board/stm32h7/lli2c.c /board/stm32h7/lluart.c /board/stm32h7/llusb.c /board/stm32h7/stm32h7_config.c /board/stm32h7/clock.c /board/stm32h7/interrupt_handlers.c /board/stm32h7/lladc.c /board/critical.c /board/faults.c /board/main_definitions.c /board/globals.c /board/main.c /board/boards/cuatro.c /board/boards/unused_funcs.c /board/boards/tres.c /board/boards/red.c /board/can_comms.c /board/power_saving.c /board/provision.c /board/main_comms.c /board/drivers/harness.c /board/drivers/clock_source.c /board/drivers/usb.c /board/drivers/uart.c /board/drivers/timers.c /board/drivers/led.c /board/drivers/simple_watchdog.c /board/drivers/fan.c /board/drivers/interrupts.c /board/drivers/spi.c /board/drivers/bootkick.c /board/drivers/pwm.c /board/drivers/registers.c /board/drivers/can_common.c /board/drivers/fdcan.c /board/drivers/fake_siren.c /board/drivers/gpio.c /board/early_init.c /board/libc.c Critical errors diff --git a/tests/misra/suppressions.txt b/tests/misra/suppressions.txt index 4800a270bcb..be223dfceb8 100644 --- a/tests/misra/suppressions.txt +++ b/tests/misra/suppressions.txt @@ -13,7 +13,8 @@ misra-c2012-20.10 unmatchedSuppression # All interrupt handlers are defined, including ones we don't use -unusedFunction:*/interrupt_handlers*.h +unusedFunction:*/interrupt_handlers* +misra-c2012-8.7:*/interrupt_handlers.c # all of the below suppressions are from new checks introduced after updating # cppcheck from 2.5 -> 2.13. they are listed here to separate the update from diff --git a/tests/misra/test_misra.sh b/tests/misra/test_misra.sh index 2ab271bd29b..46211e435a3 100755 --- a/tests/misra/test_misra.sh +++ b/tests/misra/test_misra.sh @@ -68,54 +68,32 @@ cppcheck() { PANDA_OPTS="--enable=all --addon=misra" printf "\n${GREEN}** PANDA H7 CODE **${NC}\n" -cppcheck $PANDA_OPTS -DSTM32H7 -DSTM32H725xx -I $PANDA_DIR/board/stm32h7/inc/ -DPANDA \ - $PANDA_DIR/board/libc.c \ - $PANDA_DIR/board/early_init.c \ - $PANDA_DIR/board/critical.c \ - $PANDA_DIR/board/drivers/led.c \ - $PANDA_DIR/board/drivers/pwm.c \ - $PANDA_DIR/board/drivers/gpio.c \ - $PANDA_DIR/board/drivers/fake_siren.c \ - $PANDA_DIR/board/stm32h7/lli2c.c \ - $PANDA_DIR/board/stm32h7/clock.c \ - $PANDA_DIR/board/drivers/clock_source.c \ - $PANDA_DIR/board/stm32h7/sound.c \ - $PANDA_DIR/board/stm32h7/llflash.c \ - $PANDA_DIR/board/stm32h7/stm32h7_config.c \ - $PANDA_DIR/board/drivers/registers.c \ - $PANDA_DIR/board/drivers/interrupts.c \ - $PANDA_DIR/board/provision.c \ - $PANDA_DIR/board/stm32h7/peripherals.c \ - $PANDA_DIR/board/stm32h7/llusb.c \ - $PANDA_DIR/board/drivers/usb.c \ - $PANDA_DIR/board/drivers/spi.c \ - $PANDA_DIR/board/drivers/timers.c \ - $PANDA_DIR/board/stm32h7/lladc.c \ - $PANDA_DIR/board/stm32h7/llspi.c \ - $PANDA_DIR/board/faults.c \ - $PANDA_DIR/board/boards/unused_funcs.c \ - $PANDA_DIR/board/utils.c \ - $PANDA_DIR/board/globals.c \ - $PANDA_DIR/board/obj/gitversion.c \ - $PANDA_DIR/board/can_comms.c \ - $PANDA_DIR/board/drivers/fan.c \ - $PANDA_DIR/board/power_saving.c \ - $PANDA_DIR/board/drivers/uart.c \ - $PANDA_DIR/board/stm32h7/llfdcan.c \ - $PANDA_DIR/board/drivers/harness.c \ - $PANDA_DIR/board/drivers/bootkick.c \ - $PANDA_DIR/board/stm32h7/llfan.c \ - $PANDA_DIR/board/stm32h7/lluart.c \ - $PANDA_DIR/board/drivers/fdcan.c \ - $PANDA_DIR/board/drivers/can_common.c \ - $PANDA_DIR/board/main_comms.c \ - $PANDA_DIR/board/main.c \ - $PANDA_DIR/board/drivers/simple_watchdog.c \ - $PANDA_DIR/board/stm32h7/board.c \ - $PANDA_DIR/board/boards/tres.c \ - $PANDA_DIR/board/boards/red.c \ - $PANDA_DIR/board/boards/cuatro.c \ - $PANDA_DIR/board/main_definitions.c + +IGNORED_PATHS=( + "$PANDA_DIR/board/obj" + "$PANDA_DIR/board/jungle" + "$PANDA_DIR/board/body" + "$PANDA_DIR/board/stm32h7/inc" + "$PANDA_DIR/board/fake_stm.h" + "$PANDA_DIR/board/fake_stm.c" + "$PANDA_DIR/board/flasher.h" + "$PANDA_DIR/board/flasher.c" + "$PANDA_DIR/board/bootstub.c" + "$PANDA_DIR/board/bootstub_declarations.h" + "$PANDA_DIR/board/stm32h7/llflash.h" + "$PANDA_DIR/board/stm32h7/llflash.c" +) + +# build the find prune expression +PRUNE_EXPR="" +for p in "${IGNORED_PATHS[@]}"; do + PRUNE_EXPR="$PRUNE_EXPR -path $p -prune -o" +done + +# find all .c files excluding ignored paths +C_FILES=$(eval "find $PANDA_DIR/board $PRUNE_EXPR -name '*.c' -print") + +cppcheck $PANDA_OPTS -DSTM32H7 -DSTM32H725xx -I $PANDA_DIR/board/stm32h7/inc/ -DPANDA $C_FILES printf "\n${GREEN}Success!${NC} took $SECONDS seconds\n" diff --git a/tests/misra/test_mutation.py b/tests/misra/test_mutation.py index c0d804863b8..724a8d7e0fc 100755 --- a/tests/misra/test_mutation.py +++ b/tests/misra/test_mutation.py @@ -15,10 +15,12 @@ 'board/jungle', 'board/body', 'board/stm32h7/inc', + 'board/fake_stm.c', 'board/fake_stm.h', # bootstub only files 'board/flasher.h', + 'board/flasher.c', 'board/bootstub.c', 'board/bootstub_declarations.h', 'board/stm32h7/llflash.h', From 257ab16f047ebdc1b8a5e56e38e7858489fbb726 Mon Sep 17 00:00:00 2001 From: Jakub Berkop Date: Tue, 6 Jan 2026 18:26:30 +0100 Subject: [PATCH 29/29] Sort file names arguments --- tests/misra/checkers.txt | 2 +- tests/misra/test_misra.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/misra/checkers.txt b/tests/misra/checkers.txt index 4058e1b8468..431970c9584 100644 --- a/tests/misra/checkers.txt +++ b/tests/misra/checkers.txt @@ -5,7 +5,7 @@ Cppcheck checkers list from test_misra.sh: TEST variant options: ---enable=all --addon=misra -DSTM32H7 -DSTM32H725xx -I /board/stm32h7/inc/ -DPANDA /board/utils.c /board/stm32h7/board.c /board/stm32h7/llfan.c /board/stm32h7/llfdcan.c /board/stm32h7/peripherals.c /board/stm32h7/sound.c /board/stm32h7/llspi.c /board/stm32h7/lli2c.c /board/stm32h7/lluart.c /board/stm32h7/llusb.c /board/stm32h7/stm32h7_config.c /board/stm32h7/clock.c /board/stm32h7/interrupt_handlers.c /board/stm32h7/lladc.c /board/critical.c /board/faults.c /board/main_definitions.c /board/globals.c /board/main.c /board/boards/cuatro.c /board/boards/unused_funcs.c /board/boards/tres.c /board/boards/red.c /board/can_comms.c /board/power_saving.c /board/provision.c /board/main_comms.c /board/drivers/harness.c /board/drivers/clock_source.c /board/drivers/usb.c /board/drivers/uart.c /board/drivers/timers.c /board/drivers/led.c /board/drivers/simple_watchdog.c /board/drivers/fan.c /board/drivers/interrupts.c /board/drivers/spi.c /board/drivers/bootkick.c /board/drivers/pwm.c /board/drivers/registers.c /board/drivers/can_common.c /board/drivers/fdcan.c /board/drivers/fake_siren.c /board/drivers/gpio.c /board/early_init.c /board/libc.c +--enable=all --addon=misra -DSTM32H7 -DSTM32H725xx -I /board/stm32h7/inc/ -DPANDA /board/boards/cuatro.c /board/boards/red.c /board/boards/tres.c /board/boards/unused_funcs.c /board/can_comms.c /board/critical.c /board/drivers/bootkick.c /board/drivers/can_common.c /board/drivers/clock_source.c /board/drivers/fake_siren.c /board/drivers/fan.c /board/drivers/fdcan.c /board/drivers/gpio.c /board/drivers/harness.c /board/drivers/interrupts.c /board/drivers/led.c /board/drivers/pwm.c /board/drivers/registers.c /board/drivers/simple_watchdog.c /board/drivers/spi.c /board/drivers/timers.c /board/drivers/uart.c /board/drivers/usb.c /board/early_init.c /board/faults.c /board/globals.c /board/libc.c /board/main.c /board/main_comms.c /board/main_definitions.c /board/power_saving.c /board/provision.c /board/stm32h7/board.c /board/stm32h7/clock.c /board/stm32h7/interrupt_handlers.c /board/stm32h7/lladc.c /board/stm32h7/llfan.c /board/stm32h7/llfdcan.c /board/stm32h7/lli2c.c /board/stm32h7/llspi.c /board/stm32h7/lluart.c /board/stm32h7/llusb.c /board/stm32h7/peripherals.c /board/stm32h7/sound.c /board/stm32h7/stm32h7_config.c /board/utils.c Critical errors diff --git a/tests/misra/test_misra.sh b/tests/misra/test_misra.sh index 46211e435a3..7356b4e32cf 100755 --- a/tests/misra/test_misra.sh +++ b/tests/misra/test_misra.sh @@ -91,7 +91,7 @@ for p in "${IGNORED_PATHS[@]}"; do done # find all .c files excluding ignored paths -C_FILES=$(eval "find $PANDA_DIR/board $PRUNE_EXPR -name '*.c' -print") +C_FILES=$(eval "find $PANDA_DIR/board $PRUNE_EXPR -name '*.c' -print" | sort) cppcheck $PANDA_OPTS -DSTM32H7 -DSTM32H725xx -I $PANDA_DIR/board/stm32h7/inc/ -DPANDA $C_FILES