diff --git a/make/mcu/STM32G4.mk b/make/mcu/STM32G4.mk new file mode 100644 index 0000000000..b9afa638e2 --- /dev/null +++ b/make/mcu/STM32G4.mk @@ -0,0 +1,209 @@ +# +# G4 Make file include +# + +ifeq ($(DEBUG_HARDFAULTS),G4) +CFLAGS += -DDEBUG_HARDFAULTS +endif + +#CMSIS +CMSIS_DIR := $(ROOT)/lib/main/CMSIS + +#STDPERIPH +STDPERIPH_DIR = $(ROOT)/lib/main/STM32G4/Drivers/STM32G4xx_HAL_Driver +STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/Src/*.c)) +EXCLUDES = \ + stm32g4xx_hal_comp.c \ + stm32g4xx_hal_crc.c \ + stm32g4xx_hal_crc_ex.c \ + stm32g4xx_hal_cryp.c \ + stm32g4xx_hal_cryp_ex.c \ + stm32g4xx_hal_dac.c \ + stm32g4xx_hal_dac_ex.c \ + stm32g4xx_hal_dma_ex.c \ + stm32g4xx_hal_flash_ramfunc.c \ + stm33g4xx_hal_fmac.c \ + stm32g4xx_hal_hrtim.c \ + stm32g4xx_hal_i2s.c \ + stm32g4xx_hal_irda.c \ + stm32g4xx_hal_iwdg.c \ + stm32g4xx_hal_lptim.c \ + stm32g4xx_hal_msp_template.c \ + stm32g4xx_hal_nand.c \ + stm32g4xx_hal_nor.c \ + stm32g4xx_hal_opamp.c \ + stm32g4xx_hal_opamp_ex.c \ + stm32g4xx_hal_qspi.c \ + stm32g4xx_hal_rng.c \ + stm32g4xx_hal_sai.c \ + stm32g4xx_hal_sai_ex.c \ + stm32g4xx_hal_smartcard.c \ + stm32g4xx_hal_smartcard_ex.c \ + stm32g4xx_hal_smbus.c \ + stm32g4xx_hal_spi_ex.c \ + stm32g4xx_hal_sram.c \ + stm32g4xx_hal_timebase_tim_template.c \ + stm32g4xx_hal_usart.c \ + stm32g4xx_hal_usart_ex.c \ + stm32g4xx_hal_wwdg.c \ + stm32g4xx_ll_adc.c \ + stm32g4xx_ll_comp.c \ + stm32g4xx_ll_cordic.c \ + stm32g4xx_ll_crc.c \ + stm32g4xx_ll_crs.c \ + stm32g4xx_ll_dac.c \ + stm32g4xx_ll_exti.c \ + stm32g4xx_ll_fmac.c \ + stm32g4xx_ll_fmc.c \ + stm32g4xx_ll_gpio.c \ + stm32g4xx_ll_hrtim.c \ + stm32g4xx_ll_i2c.c \ + stm32g4xx_ll_lptim.c \ + stm32g4xx_ll_lpuart.c \ + stm32g4xx_ll_opamp.c \ + stm32g4xx_ll_pwr.c \ + stm32g4xx_ll_rcc.c \ + stm32g4xx_ll_rng.c \ + stm32g4xx_ll_rtc.c \ + stm32g4xx_ll_spi.c \ + stm32g4xx_ll_ucpd.c \ + stm32g4xx_ll_usart.c \ + stm32g4xx_ll_utils.c + +STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC)) + +#USB +USBCORE_DIR = $(ROOT)/lib/main/STM32G4/Middlewares/ST/STM32_USB_Device_Library/Core +USBCORE_SRC = $(notdir $(wildcard $(USBCORE_DIR)/Src/*.c)) +EXCLUDES = usbd_conf_template.c +USBCORE_SRC := $(filter-out ${EXCLUDES}, $(USBCORE_SRC)) + +USBCDC_DIR = $(ROOT)/lib/main/STM32G4/Middlewares/ST/STM32_USB_Device_Library/Class/CDC +USBCDC_SRC = $(notdir $(wildcard $(USBCDC_DIR)/Src/*.c)) +EXCLUDES = usbd_cdc_if_template.c +USBCDC_SRC := $(filter-out ${EXCLUDES}, $(USBCDC_SRC)) + +USBHID_DIR = $(ROOT)/lib/main/STM32G4/Middlewares/ST/STM32_USB_Device_Library/Class/HID +USBHID_SRC = $(notdir $(wildcard $(USBHID_DIR)/Src/*.c)) + +USBMSC_DIR = $(ROOT)/lib/main/STM32G4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC +USBMSC_SRC = $(notdir $(wildcard $(USBMSC_DIR)/Src/*.c)) +EXCLUDES = usbd_msc_storage_template.c +USBMSC_SRC := $(filter-out ${EXCLUDES}, $(USBMSC_SRC)) + +VPATH := $(VPATH):$(USBCDC_DIR)/Src:$(USBCORE_DIR)/Src:$(USBHID_DIR)/Src:$(USBMSC_DIR)/Src + +DEVICE_STDPERIPH_SRC := $(STDPERIPH_SRC) \ + $(USBCORE_SRC) \ + $(USBCDC_SRC) \ + $(USBHID_SRC) \ + $(USBMSC_SRC) + +#CMSIS +VPATH := $(VPATH):$(CMSIS_DIR)/Include:$(CMSIS_DIR)/Device/ST/STM32G4xx +VPATH := $(VPATH):$(STDPERIPH_DIR)/Src +CMSIS_SRC := +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(STDPERIPH_DIR)/Inc \ + $(USBCORE_DIR)/Inc \ + $(USBCDC_DIR)/Inc \ + $(USBHID_DIR)/Inc \ + $(USBMSC_DIR)/Inc \ + $(CMSIS_DIR)/Core/Include \ + $(ROOT)/lib/main/STM32G4/Drivers/CMSIS/Device/ST/STM32G4xx/Include \ + $(ROOT)/src/main/vcp_hal + +ifneq ($(filter SDCARD_SPI,$(FEATURES)),) +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(FATFS_DIR) +VPATH := $(VPATH):$(FATFS_DIR) +endif + +ifneq ($(filter SDCARD_SDIO,$(FEATURES)),) +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(FATFS_DIR) +VPATH := $(VPATH):$(FATFS_DIR) +endif + +#Flags +ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion + +DEVICE_FLAGS = -DUSE_HAL_DRIVER -DUSE_FULL_LL_DRIVER -DUSE_DMA_RAM -DMAX_MPU_REGIONS=16 + +# G4X3_TARGETS includes G47{3,4}{RE,CE,CEU} + +ifeq ($(TARGET),$(filter $(TARGET),$(G4X3_TARGETS))) +DEVICE_FLAGS += -DSTM32G474xx +LD_SCRIPT = $(LINKER_DIR)/stm32_flash_g474.ld +STARTUP_SRC = startup_stm32g474xx.s +MCU_FLASH_SIZE = 512 +else +$(error Unknown MCU for G4 target) +endif +DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) + +TARGET_FLAGS = -D$(TARGET) + +VCP_SRC = \ + vcp_hal/usbd_desc.c \ + vcp_hal/usbd_conf_stm32g4xx.c \ + vcp_hal/usbd_cdc_hid.c \ + vcp_hal/usbd_cdc_interface.c \ + drivers/serial_usb_vcp.c \ + drivers/usb_io.c + +MCU_COMMON_SRC = \ + drivers/accgyro/accgyro_mpu.c \ + drivers/adc_stm32g4xx.c \ + drivers/bus_i2c_hal.c \ + drivers/bus_i2c_hal_init.c \ + drivers/bus_spi_hal.c \ + drivers/dma_stm32g4xx.c \ + drivers/dshot_bitbang.c \ + drivers/dshot_bitbang_decode.c \ + drivers/dshot_bitbang_ll.c \ + drivers/light_ws2811strip_hal.c \ + drivers/memprot_hal.c \ + drivers/memprot_stm32g4xx.c \ + drivers/persistent.c \ + drivers/pwm_output_dshot_shared.c \ + drivers/pwm_output_dshot_hal.c \ + drivers/timer_hal.c \ + drivers/timer_stm32g4xx.c \ + drivers/transponder_ir_io_hal.c \ + drivers/system_stm32g4xx.c \ + drivers/serial_uart_stm32g4xx.c \ + drivers/serial_uart_hal.c \ + startup/system_stm32g4xx.c + +MCU_EXCLUDES = \ + drivers/bus_i2c.c \ + drivers/timer.c + +# G4's MSC use the same driver layer file with F7 +MSC_SRC = \ + drivers/usb_msc_common.c \ + drivers/usb_msc_f7xx.c \ + msc/usbd_storage.c + +ifneq ($(filter SDCARD_SDIO,$(FEATURES)),) +MCU_COMMON_SRC += \ + drivers/sdio_g4xx.c +MSC_SRC += \ + msc/usbd_storage_sdio.c +endif + +ifneq ($(filter SDCARD_SPI,$(FEATURES)),) +MSC_SRC += \ + msc/usbd_storage_sd_spi.c +endif + +ifneq ($(filter ONBOARDFLASH,$(FEATURES)),) +MSC_SRC += \ + msc/usbd_storage_emfat.c \ + msc/emfat.c \ + msc/emfat_file.c +endif + +DSP_LIB := $(ROOT)/lib/main/CMSIS/DSP +DEVICE_FLAGS += -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE -DARM_MATH_CM4 diff --git a/make/targets.mk b/make/targets.mk index 635eb2c00b..73161f0b5d 100644 --- a/make/targets.mk +++ b/make/targets.mk @@ -18,14 +18,15 @@ endif F4_TARGETS := $(F405_TARGETS) $(F411_TARGETS) $(F446_TARGETS) F7_TARGETS := $(F7X2RE_TARGETS) $(F7X5XE_TARGETS) $(F7X5XG_TARGETS) $(F7X5XI_TARGETS) $(F7X6XG_TARGETS) +G4_TARGETS := $(G4X3_TARGETS) H7_TARGETS := $(H743xI_TARGETS) $(H750xB_TARGETS) ifeq ($(filter $(TARGET),$(VALID_TARGETS)),) $(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS). Have you prepared a valid target.mk?) endif -ifeq ($(filter $(TARGET),$(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS) $(H7_TARGETS) $(SITL_TARGETS)),) -$(error Target '$(TARGET)' has not specified a valid STM group, must be one of F1, F3, F405, F411, F446, F7X2RE, F7X5XE, F7X5XG, F7X5XI, F7X6XG or H7X3XI. Have you prepared a valid target.mk?) +ifeq ($(filter $(TARGET),$(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS) $(G4_TARGETS) $(H7_TARGETS) $(SITL_TARGETS)),) +$(error Target '$(TARGET)' has not specified a valid STM group, must be one of F1, F3, F405, F411, F446, F7X2RE, F7X5XE, F7X5XG, F7X5XI, F7X6XG, G4X3 or H7X3XI. Have you prepared a valid target.mk?) endif ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS))) @@ -37,6 +38,9 @@ TARGET_MCU := STM32F4 else ifeq ($(TARGET),$(filter $(TARGET), $(F7_TARGETS))) TARGET_MCU := STM32F7 +else ifeq ($(TARGET),$(filter $(TARGET), $(G4_TARGETS))) +TARGET_MCU := STM32G4 + else ifeq ($(TARGET),$(filter $(TARGET), $(H7_TARGETS))) TARGET_MCU := STM32H7 diff --git a/src/link/stm32_flash_g474.ld b/src/link/stm32_flash_g474.ld new file mode 100644 index 0000000000..9f628913e4 --- /dev/null +++ b/src/link/stm32_flash_g474.ld @@ -0,0 +1,31 @@ +/* +***************************************************************************** +** +** File : stm32_flash.ld +** +** Abstract : Linker script for STM32G474 (Category 3) device with +** 512KByte FLASH and 96KByte SRAM and 32KByte CCM SRAM +** +***************************************************************************** +*/ + +/* Specify the memory areas. */ +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 4K + FLASH1 (rx) : ORIGIN = 0x08001000, LENGTH = 492K + FLASH_CONFIG (r) : ORIGIN = 0x0807C000, LENGTH = 8K + FLASH_CUSTOM_DEFAULTS (r) : ORIGIN = 0x0807E000, LENGTH = 8K + + SYSTEM_MEMORY (r) : ORIGIN = 0x1FFF0000, LENGTH = 64K + + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 96K + CCM (xrw) : ORIGIN = 0x20018000, LENGTH = 32K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +REGION_ALIAS("STACKRAM", CCM) +REGION_ALIAS("FASTRAM", CCM) +REGION_ALIAS("VECTAB", CCM) + +INCLUDE "stm32_flash_split_g4.ld" diff --git a/src/link/stm32_flash_split_g4.ld b/src/link/stm32_flash_split_g4.ld new file mode 100644 index 0000000000..5b6f363a02 --- /dev/null +++ b/src/link/stm32_flash_split_g4.ld @@ -0,0 +1,269 @@ +/* +***************************************************************************** +** +** File : stm32_flash_split.ld +** +** Abstract : Common linker script for STM32 devices. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_Hot_Reboot_Flags_Size = 16; +_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM) - _Hot_Reboot_Flags_Size; /* end of RAM */ + +/* Base address where the config is stored. */ +__config_start = ORIGIN(FLASH_CONFIG); +__config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG); + +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0; /* required amount of heap */ +_Min_Stack_Size = 0x800; /* required amount of stack */ + +/* Define output sections */ +SECTIONS +{ + /* + * The ISR vector table is loaded at the beginning of the FLASH, + * But it is linked (space reserved) at the beginning of the VECTAB region, + * which is aliased either to FLASH or RAM. + * When linked to RAM, the table can optionally be copied from FLASH to RAM + * for table relocation. + */ + + _isr_vector_table_flash_base = LOADADDR(.isr_vector); + PROVIDE (isr_vector_table_flash_base = _isr_vector_table_flash_base); + + .isr_vector : + { + . = ALIGN(512); + PROVIDE (isr_vector_table_base = .); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + PROVIDE (isr_vector_table_end = .); + } >VECTAB AT> FLASH + + /* System memory (read-only bootloader) interrupt vector */ + .system_isr_vector (NOLOAD) : + { + . = ALIGN(4); + PROVIDE (system_isr_vector_table_base = .); + KEEP(*(.system_isr_vector)) /* Bootloader code */ + . = ALIGN(4); + } >SYSTEM_MEMORY + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH1 + + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH1 + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH1 + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(.fini_array*)) + KEEP (*(SORT(.fini_array.*))) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH1 + .pg_registry : + { + PROVIDE_HIDDEN (__pg_registry_start = .); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = .); + } >FLASH1 + .pg_resetdata : + { + PROVIDE_HIDDEN (__pg_resetdata_start = .); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = .); + } >FLASH1 + + /* Storage for the address for the configuration section so we can grab it out of the hex file */ + .custom_defaults : + { + . = ALIGN(4); + KEEP (*(.custom_defaults_start_address)) + . = ALIGN(4); + KEEP (*(.custom_defaults_end_address)) + . = ALIGN(4); + __custom_defaults_internal_start = .; + *(.custom_defaults); + } >FLASH_CUSTOM_DEFAULTS + + PROVIDE_HIDDEN (__custom_defaults_start = DEFINED(USE_CUSTOM_DEFAULTS_EXTENDED) ? ORIGIN(FLASH_CUSTOM_DEFAULTS_EXTENDED) : __custom_defaults_internal_start); + PROVIDE_HIDDEN (__custom_defaults_end = DEFINED(USE_CUSTOM_DEFAULTS_EXTENDED) ? ORIGIN(FLASH_CUSTOM_DEFAULTS_EXTENDED) + LENGTH(FLASH_CUSTOM_DEFAULTS_EXTENDED) : ORIGIN(FLASH_CUSTOM_DEFAULTS) + LENGTH(FLASH_CUSTOM_DEFAULTS)); + + /* used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM AT> FLASH1 + + /* Uninitialized data section */ + . = ALIGN(4); + .bss (NOLOAD) : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(SORT_BY_ALIGNMENT(.bss*)) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* used during startup to initialized fastram_data */ + _sfastram_idata = LOADADDR(.fastram_data); + + /* Initialized FAST_RAM section for unsuspecting developers */ + .fastram_data : + { + . = ALIGN(4); + _sfastram_data = .; /* create a global symbol at data start */ + *(.fastram_data) /* .data sections */ + *(.fastram_data*) /* .data* sections */ + + . = ALIGN(4); + _efastram_data = .; /* define a global symbol at data end */ + } >FASTRAM AT> FLASH1 + + . = ALIGN(4); + .fastram_bss (NOLOAD) : + { + _sfastram_bss = .; + __fastram_bss_start__ = _sfastram_bss; + *(.fastram_bss) + *(SORT_BY_ALIGNMENT(.fastram_bss*)) + . = ALIGN(4); + _efastram_bss = .; + __fastram_bss_end__ = _efastram_bss; + } >FASTRAM + + .persistent_data (NOLOAD) : + { + __persistent_data_start__ = .; + *(.persistent_data) + . = ALIGN(4); + __persistent_data_end__ = .; + } >RAM + + .DMA_RAM_R (NOLOAD) : + { + . = ALIGN(32); + _sdma_ram_r = .; + PROVIDE(dma_ram_r_start = .); + _dma_ram_r_start__ = _sdma_ram_r; + KEEP(*(.DMA_RAM_R)) + PROVIDE(dma_ram_r_end = .); + _edma_ram_r = .; + _dma_ram_r_end__ = _edma_ram_r; + } >RAM + + .DMA_RAM_W (NOLOAD) : + { + . = ALIGN(32); + _sdma_ram_w = .; + PROVIDE(dma_ram_w_start = .); + _dma_ram_w_start__ = _sdma_ram_w; + KEEP(*(.DMA_RAM_W)) + PROVIDE(dma_ram_w_end = .); + _edma_ram_w = .; + _dma_ram_w_end__ = _edma_ram_w; + } >RAM + + .DMA_RAM_RW (NOLOAD) : + { + . = ALIGN(32); + _sdma_ram_rw = .; + PROVIDE(dma_ram_rw_start = .); + _dma_ram_rw_start__ = _sdma_ram_rw; + KEEP(*(.DMA_RAM_RW)) + PROVIDE(dma_ram_rw_end = .); + _edma_ram_rw = .; + _dma_ram_rw_end__ = _edma_ram_rw; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + _heap_stack_end = ORIGIN(STACKRAM) + LENGTH(STACKRAM) - _Hot_Reboot_Flags_Size; + _heap_stack_begin = _heap_stack_end - _Min_Stack_Size - _Min_Heap_Size; + . = _heap_stack_begin; + ._user_heap_stack : + { + . = ALIGN(4); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(4); + } >STACKRAM = 0xa5 + + /* MEMORY_bank1 section, code must be located here explicitly */ + /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ + .memory_b1_text : + { + *(.mb1text) /* .mb1text sections (code) */ + *(.mb1text*) /* .mb1text* sections (code) */ + *(.mb1rodata) /* read-only data (constants) */ + *(.mb1rodata*) + } >MEMORY_B1 + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index f8f75df65b..9e0ee08c5f 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -69,6 +69,9 @@ uint32_t getCycleCounter(void); #if defined(STM32H7) || defined(STM32G4) void systemCheckResetReason(void); #endif +#if defined(STM32G4) +void systemBOOT0PinBootLoaderEnable(void); +#endif void initialiseMemorySections(void); diff --git a/src/main/drivers/system_stm32g4xx.c b/src/main/drivers/system_stm32g4xx.c new file mode 100644 index 0000000000..c06f2d0f24 --- /dev/null +++ b/src/main/drivers/system_stm32g4xx.c @@ -0,0 +1,207 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#include "drivers/accgyro/accgyro_mpu.h" +#include "drivers/exti.h" +#include "drivers/nvic.h" +#include "drivers/memprot.h" +#include "drivers/persistent.h" +#include "drivers/system.h" + +bool isMPUSoftReset(void) +{ + if (cachedRccCsrValue & RCC_CSR_SFTRSTF) + return true; + else + return false; +} + +void systemInit(void) +{ + memProtReset(); + memProtConfigure(mpuRegions, mpuRegionCount); + + // Configure NVIC preempt/priority groups + HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITY_GROUPING); + + // cache RCC->RSR value to use it in isMPUSoftReset() and others + cachedRccCsrValue = RCC->CSR; + + /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */ + //extern void *isr_vector_table_base; + //NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0); + //__HAL_RCC_USB_OTG_FS_CLK_DISABLE; + + //RCC_ClearFlag(); + + // Init cycle counter + cycleCounterInit(); + + // SysTick is updated whenever HAL_RCC_ClockConfig is called. + + // Disable UCPD pull-down functionality on PB4 + // AN5093 6.3.3: + // (after device startup this pull-down on PB4 can be disabled by + // setting UCPD1_DBDIS bit in the PWR_CR3 register). + // + // There is also a similar UCPD stand-by functionality control, + // but don't fiddle with this one. In particular, disabling it by + // calling HAL_PWREx_DisableUSBStandByModePD() will cause boot loader + // invocation to fail. + +#if defined(PWR_CR3_UCPD_DBDIS) + HAL_PWREx_DisableUSBDeadBatteryPD(); +#endif +} + +void systemReset(void) +{ + // SCB_DisableDCache(); + // SCB_DisableICache(); + + __disable_irq(); + NVIC_SystemReset(); +} + +void forcedSystemResetWithoutDisablingCaches(void) +{ + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_FORCED); + + __disable_irq(); + NVIC_SystemReset(); +} + +void systemResetToBootloader(bootloaderRequestType_e requestType) +{ + switch (requestType) { +#if defined(USE_FLASH_BOOT_LOADER) + case BOATLOADER_REQUEST_FLASH: + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_FLASH_BOOTLOADER_REQUEST); + + break; +#endif + case BOOTLOADER_REQUEST_ROM: + default: + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM); + + break; + } + + __disable_irq(); + NVIC_SystemReset(); +} + +#define SYSMEMBOOT_VECTOR_TABLE ((uint32_t *)0x1fff0000) + +typedef void *(*bootJumpPtr)(void); + +void systemJumpToBootloader(void) +{ + __SYSCFG_CLK_ENABLE(); + + uint32_t bootStack = SYSMEMBOOT_VECTOR_TABLE[0]; + + bootJumpPtr SysMemBootJump = (bootJumpPtr)SYSMEMBOOT_VECTOR_TABLE[1]; + + __set_MSP(bootStack); //Set the main stack pointer to its default values + + SysMemBootJump(); + + while (1); +} + +static uint32_t bootloaderRequest; + +void systemCheckResetReason(void) +{ + bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON); + + switch (bootloaderRequest) { +#if defined(USE_FLASH_BOOT_LOADER) + case BOATLOADER_REQUEST_FLASH: +#endif + case RESET_BOOTLOADER_REQUEST_ROM: + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_POST); + break; + + case RESET_MSC_REQUEST: + // RESET_REASON will be reset by MSC + return; + + case RESET_FORCED: + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE); + return; + + case RESET_NONE: + if (!(RCC->CSR & RCC_CSR_SFTRSTF)) { + // Direct hard reset case + return; + } + // Soft reset; boot loader may have been active with BOOT pin pulled high. + FALLTHROUGH; + + case RESET_BOOTLOADER_POST: + // Boot loader activity magically prevents SysTick from interrupting. + // Issue a soft reset to prevent the condition. + forcedSystemResetWithoutDisablingCaches(); // observed that disabling dcache after cold boot with BOOT pin high causes segfault. + } + + void (*SysMemBootJump)(void); + __SYSCFG_CLK_ENABLE(); + + systemJumpToBootloader(); + +#define SYSTEM_BOOTLOADER_VEC 0x1fff0000 + + uint32_t p = (*((uint32_t *)SYSTEM_BOOTLOADER_VEC)); + __set_MSP(p); //Set the main stack pointer to its defualt values + SysMemBootJump = (void (*)(void)) (*((uint32_t *)(SYSTEM_BOOTLOADER_VEC + 4))); // Point the PC to the System Memory reset vector (+4) + SysMemBootJump(); + while (1); +} + +// Nucleo-G474RE board seems to come with software BOOT0 enabled. +// Call this function once from init() to honor PB8-BOOT0 pin status for boot loader invocation. +void systemBOOT0PinBootLoaderEnable(void) +{ + FLASH_OBProgramInitTypeDef OBInit; + + HAL_FLASH_Unlock(); + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPTVERR); + HAL_FLASH_OB_Unlock(); + + HAL_FLASHEx_OBGetConfig(&OBInit); + + if ((OBInit.USERConfig & (OB_BOOT0_FROM_PIN|OB_BOOT1_SYSTEM)) != (OB_BOOT0_FROM_PIN|OB_BOOT1_SYSTEM)) { + OBInit.OptionType = OPTIONBYTE_USER; + OBInit.USERType = OB_USER_nSWBOOT0|OB_USER_nBOOT1; + OBInit.USERConfig = OB_BOOT0_FROM_PIN|OB_BOOT1_SYSTEM; + HAL_FLASHEx_OBProgram(&OBInit); + + HAL_FLASH_OB_Launch(); + } + + HAL_FLASH_OB_Lock(); + HAL_FLASH_Lock(); +} diff --git a/src/main/platform.h b/src/main/platform.h index 5c58a1aeef..c61764267d 100644 --- a/src/main/platform.h +++ b/src/main/platform.h @@ -26,7 +26,30 @@ #pragma GCC poison sprintf snprintf #endif -#if defined(STM32H743xx) || defined(STM32H750xx) +#if defined(STM32G474xx) +#include "stm32g4xx.h" +#include "stm32g4xx_hal.h" +#include "system_stm32g4xx.h" + +#include "stm32g4xx_ll_spi.h" +#include "stm32g4xx_ll_gpio.h" +#include "stm32g4xx_ll_dma.h" +#include "stm32g4xx_ll_rcc.h" +#include "stm32g4xx_ll_bus.h" +#include "stm32g4xx_ll_tim.h" +#include "stm32g4xx_ll_system.h" +#include "drivers/stm32g4xx_ll_ex.h" + +// Chip Unique ID on G4 +#define U_ID_0 (*(uint32_t*)UID_BASE) +#define U_ID_1 (*(uint32_t*)(UID_BASE + 4)) +#define U_ID_2 (*(uint32_t*)(UID_BASE + 8)) + +#ifndef STM32G4 +#define STM32G4 +#endif + +#elif defined(STM32H743xx) || defined(STM32H750xx) #include "stm32h7xx.h" #include "stm32h7xx_hal.h" #include "system_stm32h7xx.h" diff --git a/src/main/startup/startup_stm32g474xx.s b/src/main/startup/startup_stm32g474xx.s new file mode 100755 index 0000000000..76a993713a --- /dev/null +++ b/src/main/startup/startup_stm32g474xx.s @@ -0,0 +1,608 @@ +/** + ****************************************************************************** + * @file startup_stm32g474xx.s + * @author MCD Application Team + * @brief STM32G474xx devices vector table GCC toolchain. + * This module performs: + * - Set the initial SP + * - Set the initial PC == Reset_Handler, + * - Set the vector table entries with the exceptions ISR address, + * - Configure the clock system + * - Branches to main in the C library (which eventually + * calls main()). + * After Reset the Cortex-M4 processor is in Thread mode, + * priority is Privileged, and the Stack is set to Main. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2017 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + + .syntax unified + .cpu cortex-m4 + .fpu softvfp + .thumb + +.global g_pfnVectors +.global Default_Handler + +/* start address for the initialization values of the .data section. +defined in linker script */ +.word _sidata +/* start address for the .data section. defined in linker script */ +.word _sdata +/* end address for the .data section. defined in linker script */ +.word _edata +/* start address for the .bss section. defined in linker script */ +.word _sbss +/* end address for the .bss section. defined in linker script */ +.word _ebss + +.equ BootRAM, 0xF1E0F85F +/** + * @brief This is the code that gets called when the processor first + * starts execution following a reset event. Only the absolutely + * necessary set is performed, after which the application + * supplied main() routine is called. + * @param None + * @retval : None +*/ + + .section .text.Reset_Handler + .weak Reset_Handler + .type Reset_Handler, %function +Reset_Handler: + ldr r0, =_estack + mov sp, r0 /* set stack pointer */ + + bl persistentObjectInit + +/* Copy the data segment initializers from flash to SRAM */ + ldr r0, =_sdata + ldr r1, =_edata + ldr r2, =_sidata + movs r3, #0 + b LoopCopyDataInit + +CopyDataInit: + ldr r4, [r2, r3] + str r4, [r0, r3] + adds r3, r3, #4 + +LoopCopyDataInit: + adds r4, r0, r3 + cmp r4, r1 + bcc CopyDataInit + +/* Zero fill the bss segment. */ + ldr r2, =_sbss + ldr r4, =_ebss + movs r3, #0 + b LoopFillZerobss + +FillZerobss: + str r3, [r2] + adds r2, r2, #4 + +LoopFillZerobss: + cmp r2, r4 + bcc FillZerobss + +/* Zero fill the sfastram_bss segment */ + ldr r2, =_sfastram_bss + ldr r4, =_efastram_bss + movs r3, #0 + b LoopFillZerofastram_bss + +FillZerofastram_bss: + str r3, [r2] + adds r2, r2, #4 + +LoopFillZerofastram_bss: + cmp r2, r4 + bcc FillZerofastram_bss + +/* Call the clock system intitialization function.*/ + bl SystemInit +/* Call static constructors */ +/* bl __libc_init_array */ +/* Call the application's entry point.*/ + bl main + +LoopForever: + b LoopForever + +.size Reset_Handler, .-Reset_Handler + +/** + * @brief This is the code that gets called when the processor receives an + * unexpected interrupt. This simply enters an infinite loop, preserving + * the system state for examination by a debugger. + * + * @param None + * @retval : None +*/ + .section .text.Default_Handler,"ax",%progbits +Default_Handler: +Infinite_Loop: + b Infinite_Loop + .size Default_Handler, .-Default_Handler +/****************************************************************************** +* +* The minimal vector table for a Cortex-M4. Note that the proper constructs +* must be placed on this to ensure that it ends up at physical address +* 0x0000.0000. +* +******************************************************************************/ + .section .isr_vector,"a",%progbits + .type g_pfnVectors, %object + .size g_pfnVectors, .-g_pfnVectors + + +g_pfnVectors: + .word _estack + .word Reset_Handler + .word NMI_Handler + .word HardFault_Handler + .word MemManage_Handler + .word BusFault_Handler + .word UsageFault_Handler + .word 0 + .word 0 + .word 0 + .word 0 + .word SVC_Handler + .word DebugMon_Handler + .word 0 + .word PendSV_Handler + .word SysTick_Handler + .word WWDG_IRQHandler + .word PVD_PVM_IRQHandler + .word RTC_TAMP_LSECSS_IRQHandler + .word RTC_WKUP_IRQHandler + .word FLASH_IRQHandler + .word RCC_IRQHandler + .word EXTI0_IRQHandler + .word EXTI1_IRQHandler + .word EXTI2_IRQHandler + .word EXTI3_IRQHandler + .word EXTI4_IRQHandler + .word DMA1_Channel1_IRQHandler + .word DMA1_Channel2_IRQHandler + .word DMA1_Channel3_IRQHandler + .word DMA1_Channel4_IRQHandler + .word DMA1_Channel5_IRQHandler + .word DMA1_Channel6_IRQHandler + .word DMA1_Channel7_IRQHandler + .word ADC1_2_IRQHandler + .word USB_HP_IRQHandler + .word USB_LP_IRQHandler + .word FDCAN1_IT0_IRQHandler + .word FDCAN1_IT1_IRQHandler + .word EXTI9_5_IRQHandler + .word TIM1_BRK_TIM15_IRQHandler + .word TIM1_UP_TIM16_IRQHandler + .word TIM1_TRG_COM_TIM17_IRQHandler + .word TIM1_CC_IRQHandler + .word TIM2_IRQHandler + .word TIM3_IRQHandler + .word TIM4_IRQHandler + .word I2C1_EV_IRQHandler + .word I2C1_ER_IRQHandler + .word I2C2_EV_IRQHandler + .word I2C2_ER_IRQHandler + .word SPI1_IRQHandler + .word SPI2_IRQHandler + .word USART1_IRQHandler + .word USART2_IRQHandler + .word USART3_IRQHandler + .word EXTI15_10_IRQHandler + .word RTC_Alarm_IRQHandler + .word USBWakeUp_IRQHandler + .word TIM8_BRK_IRQHandler + .word TIM8_UP_IRQHandler + .word TIM8_TRG_COM_IRQHandler + .word TIM8_CC_IRQHandler + .word ADC3_IRQHandler + .word FMC_IRQHandler + .word LPTIM1_IRQHandler + .word TIM5_IRQHandler + .word SPI3_IRQHandler + .word UART4_IRQHandler + .word UART5_IRQHandler + .word TIM6_DAC_IRQHandler + .word TIM7_DAC_IRQHandler + .word DMA2_Channel1_IRQHandler + .word DMA2_Channel2_IRQHandler + .word DMA2_Channel3_IRQHandler + .word DMA2_Channel4_IRQHandler + .word DMA2_Channel5_IRQHandler + .word ADC4_IRQHandler + .word ADC5_IRQHandler + .word UCPD1_IRQHandler + .word COMP1_2_3_IRQHandler + .word COMP4_5_6_IRQHandler + .word COMP7_IRQHandler + .word HRTIM1_Master_IRQHandler + .word HRTIM1_TIMA_IRQHandler + .word HRTIM1_TIMB_IRQHandler + .word HRTIM1_TIMC_IRQHandler + .word HRTIM1_TIMD_IRQHandler + .word HRTIM1_TIME_IRQHandler + .word HRTIM1_FLT_IRQHandler + .word HRTIM1_TIMF_IRQHandler + .word CRS_IRQHandler + .word SAI1_IRQHandler + .word TIM20_BRK_IRQHandler + .word TIM20_UP_IRQHandler + .word TIM20_TRG_COM_IRQHandler + .word TIM20_CC_IRQHandler + .word FPU_IRQHandler + .word I2C4_EV_IRQHandler + .word I2C4_ER_IRQHandler + .word SPI4_IRQHandler + .word 0 + .word FDCAN2_IT0_IRQHandler + .word FDCAN2_IT1_IRQHandler + .word FDCAN3_IT0_IRQHandler + .word FDCAN3_IT1_IRQHandler + .word RNG_IRQHandler + .word LPUART1_IRQHandler + .word I2C3_EV_IRQHandler + .word I2C3_ER_IRQHandler + .word DMAMUX_OVR_IRQHandler + .word QUADSPI_IRQHandler + .word DMA1_Channel8_IRQHandler + .word DMA2_Channel6_IRQHandler + .word DMA2_Channel7_IRQHandler + .word DMA2_Channel8_IRQHandler + .word CORDIC_IRQHandler + .word FMAC_IRQHandler + +/******************************************************************************* +* +* Provide weak aliases for each Exception handler to the Default_Handler. +* As they are weak aliases, any function with the same name will override +* this definition. +* +*******************************************************************************/ + + .weak NMI_Handler + .thumb_set NMI_Handler,Default_Handler + + .weak HardFault_Handler + .thumb_set HardFault_Handler,Default_Handler + + .weak MemManage_Handler + .thumb_set MemManage_Handler,Default_Handler + + .weak BusFault_Handler + .thumb_set BusFault_Handler,Default_Handler + + .weak UsageFault_Handler + .thumb_set UsageFault_Handler,Default_Handler + + .weak SVC_Handler + .thumb_set SVC_Handler,Default_Handler + + .weak DebugMon_Handler + .thumb_set DebugMon_Handler,Default_Handler + + .weak PendSV_Handler + .thumb_set PendSV_Handler,Default_Handler + + .weak SysTick_Handler + .thumb_set SysTick_Handler,Default_Handler + + .weak WWDG_IRQHandler + .thumb_set WWDG_IRQHandler,Default_Handler + + .weak PVD_PVM_IRQHandler + .thumb_set PVD_PVM_IRQHandler,Default_Handler + + .weak RTC_TAMP_LSECSS_IRQHandler + .thumb_set RTC_TAMP_LSECSS_IRQHandler,Default_Handler + + .weak RTC_WKUP_IRQHandler + .thumb_set RTC_WKUP_IRQHandler,Default_Handler + + .weak FLASH_IRQHandler + .thumb_set FLASH_IRQHandler,Default_Handler + + .weak RCC_IRQHandler + .thumb_set RCC_IRQHandler,Default_Handler + + .weak EXTI0_IRQHandler + .thumb_set EXTI0_IRQHandler,Default_Handler + + .weak EXTI1_IRQHandler + .thumb_set EXTI1_IRQHandler,Default_Handler + + .weak EXTI2_IRQHandler + .thumb_set EXTI2_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler + .thumb_set EXTI3_IRQHandler,Default_Handler + + .weak EXTI4_IRQHandler + .thumb_set EXTI4_IRQHandler,Default_Handler + + .weak DMA1_Channel1_IRQHandler + .thumb_set DMA1_Channel1_IRQHandler,Default_Handler + + .weak DMA1_Channel2_IRQHandler + .thumb_set DMA1_Channel2_IRQHandler,Default_Handler + + .weak DMA1_Channel3_IRQHandler + .thumb_set DMA1_Channel3_IRQHandler,Default_Handler + + .weak DMA1_Channel4_IRQHandler + .thumb_set DMA1_Channel4_IRQHandler,Default_Handler + + .weak DMA1_Channel5_IRQHandler + .thumb_set DMA1_Channel5_IRQHandler,Default_Handler + + .weak DMA1_Channel6_IRQHandler + .thumb_set DMA1_Channel6_IRQHandler,Default_Handler + + .weak DMA1_Channel7_IRQHandler + .thumb_set DMA1_Channel7_IRQHandler,Default_Handler + + .weak ADC1_2_IRQHandler + .thumb_set ADC1_2_IRQHandler,Default_Handler + + .weak USB_HP_IRQHandler + .thumb_set USB_HP_IRQHandler,Default_Handler + + .weak USB_LP_IRQHandler + .thumb_set USB_LP_IRQHandler,Default_Handler + + .weak FDCAN1_IT0_IRQHandler + .thumb_set FDCAN1_IT0_IRQHandler,Default_Handler + + .weak FDCAN1_IT1_IRQHandler + .thumb_set FDCAN1_IT1_IRQHandler,Default_Handler + + .weak EXTI9_5_IRQHandler + .thumb_set EXTI9_5_IRQHandler,Default_Handler + + .weak TIM1_BRK_TIM15_IRQHandler + .thumb_set TIM1_BRK_TIM15_IRQHandler,Default_Handler + + .weak TIM1_UP_TIM16_IRQHandler + .thumb_set TIM1_UP_TIM16_IRQHandler,Default_Handler + + .weak TIM1_TRG_COM_TIM17_IRQHandler + .thumb_set TIM1_TRG_COM_TIM17_IRQHandler,Default_Handler + + .weak TIM1_CC_IRQHandler + .thumb_set TIM1_CC_IRQHandler,Default_Handler + + .weak TIM2_IRQHandler + .thumb_set TIM2_IRQHandler,Default_Handler + + .weak TIM3_IRQHandler + .thumb_set TIM3_IRQHandler,Default_Handler + + .weak TIM4_IRQHandler + .thumb_set TIM4_IRQHandler,Default_Handler + + .weak I2C1_EV_IRQHandler + .thumb_set I2C1_EV_IRQHandler,Default_Handler + + .weak I2C1_ER_IRQHandler + .thumb_set I2C1_ER_IRQHandler,Default_Handler + + .weak I2C2_EV_IRQHandler + .thumb_set I2C2_EV_IRQHandler,Default_Handler + + .weak I2C2_ER_IRQHandler + .thumb_set I2C2_ER_IRQHandler,Default_Handler + + .weak SPI1_IRQHandler + .thumb_set SPI1_IRQHandler,Default_Handler + + .weak SPI2_IRQHandler + .thumb_set SPI2_IRQHandler,Default_Handler + + .weak USART1_IRQHandler + .thumb_set USART1_IRQHandler,Default_Handler + + .weak USART2_IRQHandler + .thumb_set USART2_IRQHandler,Default_Handler + + .weak USART3_IRQHandler + .thumb_set USART3_IRQHandler,Default_Handler + + .weak EXTI15_10_IRQHandler + .thumb_set EXTI15_10_IRQHandler,Default_Handler + + .weak RTC_Alarm_IRQHandler + .thumb_set RTC_Alarm_IRQHandler,Default_Handler + + .weak USBWakeUp_IRQHandler + .thumb_set USBWakeUp_IRQHandler,Default_Handler + + .weak TIM8_BRK_IRQHandler + .thumb_set TIM8_BRK_IRQHandler,Default_Handler + + .weak TIM8_UP_IRQHandler + .thumb_set TIM8_UP_IRQHandler,Default_Handler + + .weak TIM8_TRG_COM_IRQHandler + .thumb_set TIM8_TRG_COM_IRQHandler,Default_Handler + + .weak TIM8_CC_IRQHandler + .thumb_set TIM8_CC_IRQHandler,Default_Handler + + .weak ADC3_IRQHandler + .thumb_set ADC3_IRQHandler,Default_Handler + + .weak FMC_IRQHandler + .thumb_set FMC_IRQHandler,Default_Handler + + .weak LPTIM1_IRQHandler + .thumb_set LPTIM1_IRQHandler,Default_Handler + + .weak TIM5_IRQHandler + .thumb_set TIM5_IRQHandler,Default_Handler + + .weak SPI3_IRQHandler + .thumb_set SPI3_IRQHandler,Default_Handler + + .weak UART4_IRQHandler + .thumb_set UART4_IRQHandler,Default_Handler + + .weak UART5_IRQHandler + .thumb_set UART5_IRQHandler,Default_Handler + + .weak TIM6_DAC_IRQHandler + .thumb_set TIM6_DAC_IRQHandler,Default_Handler + + .weak TIM7_DAC_IRQHandler + .thumb_set TIM7_DAC_IRQHandler,Default_Handler + + .weak DMA2_Channel1_IRQHandler + .thumb_set DMA2_Channel1_IRQHandler,Default_Handler + + .weak DMA2_Channel2_IRQHandler + .thumb_set DMA2_Channel2_IRQHandler,Default_Handler + + .weak DMA2_Channel3_IRQHandler + .thumb_set DMA2_Channel3_IRQHandler,Default_Handler + + .weak DMA2_Channel4_IRQHandler + .thumb_set DMA2_Channel4_IRQHandler,Default_Handler + + .weak DMA2_Channel5_IRQHandler + .thumb_set DMA2_Channel5_IRQHandler,Default_Handler + + .weak ADC4_IRQHandler + .thumb_set ADC4_IRQHandler,Default_Handler + + .weak ADC5_IRQHandler + .thumb_set ADC5_IRQHandler,Default_Handler + + .weak UCPD1_IRQHandler + .thumb_set UCPD1_IRQHandler,Default_Handler + + .weak COMP1_2_3_IRQHandler + .thumb_set COMP1_2_3_IRQHandler,Default_Handler + + .weak COMP4_5_6_IRQHandler + .thumb_set COMP4_5_6_IRQHandler,Default_Handler + + .weak COMP7_IRQHandler + .thumb_set COMP7_IRQHandler,Default_Handler + + .weak HRTIM1_Master_IRQHandler + .thumb_set HRTIM1_Master_IRQHandler,Default_Handler + + .weak HRTIM1_TIMA_IRQHandler + .thumb_set HRTIM1_TIMA_IRQHandler,Default_Handler + + .weak HRTIM1_TIMB_IRQHandler + .thumb_set HRTIM1_TIMB_IRQHandler,Default_Handler + + .weak HRTIM1_TIMC_IRQHandler + .thumb_set HRTIM1_TIMC_IRQHandler,Default_Handler + + .weak HRTIM1_TIMD_IRQHandler + .thumb_set HRTIM1_TIMD_IRQHandler,Default_Handler + + .weak HRTIM1_TIME_IRQHandler + .thumb_set HRTIM1_TIME_IRQHandler,Default_Handler + + .weak HRTIM1_FLT_IRQHandler + .thumb_set HRTIM1_FLT_IRQHandler,Default_Handler + + .weak HRTIM1_TIMF_IRQHandler + .thumb_set HRTIM1_TIMF_IRQHandler,Default_Handler + + .weak CRS_IRQHandler + .thumb_set CRS_IRQHandler,Default_Handler + + .weak SAI1_IRQHandler + .thumb_set SAI1_IRQHandler,Default_Handler + + .weak TIM20_BRK_IRQHandler + .thumb_set TIM20_BRK_IRQHandler,Default_Handler + + .weak TIM20_UP_IRQHandler + .thumb_set TIM20_UP_IRQHandler,Default_Handler + + .weak TIM20_TRG_COM_IRQHandler + .thumb_set TIM20_TRG_COM_IRQHandler,Default_Handler + + .weak TIM20_CC_IRQHandler + .thumb_set TIM20_CC_IRQHandler,Default_Handler + + .weak FPU_IRQHandler + .thumb_set FPU_IRQHandler,Default_Handler + + .weak I2C4_EV_IRQHandler + .thumb_set I2C4_EV_IRQHandler,Default_Handler + + .weak I2C4_ER_IRQHandler + .thumb_set I2C4_ER_IRQHandler,Default_Handler + + .weak SPI4_IRQHandler + .thumb_set SPI4_IRQHandler,Default_Handler + + .weak FDCAN2_IT0_IRQHandler + .thumb_set FDCAN2_IT0_IRQHandler,Default_Handler + + .weak FDCAN2_IT1_IRQHandler + .thumb_set FDCAN2_IT1_IRQHandler,Default_Handler + + .weak FDCAN3_IT0_IRQHandler + .thumb_set FDCAN3_IT0_IRQHandler,Default_Handler + + .weak FDCAN3_IT1_IRQHandler + .thumb_set FDCAN3_IT1_IRQHandler,Default_Handler + + .weak RNG_IRQHandler + .thumb_set RNG_IRQHandler,Default_Handler + + .weak LPUART1_IRQHandler + .thumb_set LPUART1_IRQHandler,Default_Handler + + .weak I2C3_EV_IRQHandler + .thumb_set I2C3_EV_IRQHandler,Default_Handler + + .weak I2C3_ER_IRQHandler + .thumb_set I2C3_ER_IRQHandler,Default_Handler + + .weak DMAMUX_OVR_IRQHandler + .thumb_set DMAMUX_OVR_IRQHandler,Default_Handler + + .weak QUADSPI_IRQHandler + .thumb_set QUADSPI_IRQHandler,Default_Handler + + .weak DMA1_Channel8_IRQHandler + .thumb_set DMA1_Channel8_IRQHandler,Default_Handler + + .weak DMA2_Channel6_IRQHandler + .thumb_set DMA2_Channel6_IRQHandler,Default_Handler + + .weak DMA2_Channel7_IRQHandler + .thumb_set DMA2_Channel7_IRQHandler,Default_Handler + + .weak DMA2_Channel8_IRQHandler + .thumb_set DMA2_Channel8_IRQHandler,Default_Handler + + .weak CORDIC_IRQHandler + .thumb_set CORDIC_IRQHandler,Default_Handler + + .weak FMAC_IRQHandler + .thumb_set FMAC_IRQHandler,Default_Handler + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/startup/stm32g4xx_hal_conf.h b/src/main/startup/stm32g4xx_hal_conf.h new file mode 100755 index 0000000000..2a89d187ed --- /dev/null +++ b/src/main/startup/stm32g4xx_hal_conf.h @@ -0,0 +1,382 @@ +/** + ****************************************************************************** + * @file stm32g4xx_hal_conf.h + * @author MCD Application Team + * @brief HAL configuration template file. + * This file should be copied to the application folder and renamed + * to stm32g4xx_hal_conf.h. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2018 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef STM32G4xx_HAL_CONF_H +#define STM32G4xx_HAL_CONF_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/* ########################## Module Selection ############################## */ +/** + * @brief This is the list of modules to be used in the HAL driver + */ +#define HAL_MODULE_ENABLED +#define HAL_ADC_MODULE_ENABLED +//#define HAL_COMP_MODULE_ENABLED +#define HAL_CORDIC_MODULE_ENABLED +#define HAL_CORTEX_MODULE_ENABLED +//#define HAL_CRC_MODULE_ENABLED +//#define HAL_CRYP_MODULE_ENABLED +//#define HAL_DAC_MODULE_ENABLED +#define HAL_DMA_MODULE_ENABLED +#define HAL_EXTI_MODULE_ENABLED +#define HAL_FDCAN_MODULE_ENABLED +#define HAL_FLASH_MODULE_ENABLED +//#define HAL_FMAC_MODULE_ENABLED +#define HAL_GPIO_MODULE_ENABLED +//#define HAL_HRTIM_MODULE_ENABLED +//#define HAL_IRDA_MODULE_ENABLED +//#define HAL_IWDG_MODULE_ENABLED +#define HAL_I2C_MODULE_ENABLED +//#define HAL_I2S_MODULE_ENABLED +#define HAL_LPTIM_MODULE_ENABLED +//#define HAL_NAND_MODULE_ENABLED +//#define HAL_NOR_MODULE_ENABLED +//#define HAL_OPAMP_MODULE_ENABLED +#define HAL_PCD_MODULE_ENABLED +#define HAL_PWR_MODULE_ENABLED +//#define HAL_QSPI_MODULE_ENABLED +#define HAL_RCC_MODULE_ENABLED +//#define HAL_RNG_MODULE_ENABLED +#define HAL_RTC_MODULE_ENABLED +//#define HAL_SAI_MODULE_ENABLED +//#define HAL_SMARTCARD_MODULE_ENABLED +//#define HAL_SMBUS_MODULE_ENABLED +#define HAL_SPI_MODULE_ENABLED +//#define HAL_SRAM_MODULE_ENABLED +#define HAL_TIM_MODULE_ENABLED +#define HAL_UART_MODULE_ENABLED +#define HAL_USART_MODULE_ENABLED +//#define HAL_WWDG_MODULE_ENABLED + +/* ########################## Register Callbacks selection ############################## */ +/** + * @brief This is the list of modules where register callback can be used + */ + +#define USE_HAL_ADC_REGISTER_CALLBACKS 0U +#define USE_HAL_COMP_REGISTER_CALLBACKS 0U +#define USE_HAL_CORDIC_REGISTER_CALLBACKS 0U +#define USE_HAL_CRYP_REGISTER_CALLBACKS 0U +#define USE_HAL_DAC_REGISTER_CALLBACKS 0U +#define USE_HAL_EXTI_REGISTER_CALLBACKS 0U +#define USE_HAL_FDCAN_REGISTER_CALLBACKS 0U +#define USE_HAL_FMAC_REGISTER_CALLBACKS 0U +#define USE_HAL_HRTIM_REGISTER_CALLBACKS 0U +#define USE_HAL_I2C_REGISTER_CALLBACKS 0U +#define USE_HAL_I2S_REGISTER_CALLBACKS 0U +#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U +#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0U +#define USE_HAL_NAND_REGISTER_CALLBACKS 0U +#define USE_HAL_NOR_REGISTER_CALLBACKS 0U +#define USE_HAL_OPAMP_REGISTER_CALLBACKS 0U +#define USE_HAL_PCD_REGISTER_CALLBACKS 0U +#define USE_HAL_QSPI_REGISTER_CALLBACKS 0U +#define USE_HAL_RNG_REGISTER_CALLBACKS 0U +#define USE_HAL_RTC_REGISTER_CALLBACKS 0U +#define USE_HAL_SAI_REGISTER_CALLBACKS 0U +#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS 0U +#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U +#define USE_HAL_SPI_REGISTER_CALLBACKS 0U +#define USE_HAL_SRAM_REGISTER_CALLBACKS 0U +#define USE_HAL_TIM_REGISTER_CALLBACKS 0U +#define USE_HAL_UART_REGISTER_CALLBACKS 0U +#define USE_HAL_USART_REGISTER_CALLBACKS 0U +#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U + +/* ########################## Oscillator Values adaptation ####################*/ +/** + * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSE is used as system clock source, directly or through the PLL). + */ +#if !defined (HSE_VALUE) +#define HSE_VALUE (8000000UL) /*!< Value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ + +#if !defined (HSE_STARTUP_TIMEOUT) +#define HSE_STARTUP_TIMEOUT (100UL) /*!< Time out for HSE start up, in ms */ +#endif /* HSE_STARTUP_TIMEOUT */ + +/** + * @brief Internal High Speed oscillator (HSI) value. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSI is used as system clock source, directly or through the PLL). + */ +#if !defined (HSI_VALUE) +#define HSI_VALUE (16000000UL) /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ + +/** + * @brief Internal High Speed oscillator (HSI48) value for USB FS and RNG. + * This internal oscillator is mainly dedicated to provide a high precision clock to + * the USB peripheral by means of a special Clock Recovery System (CRS) circuitry. + * When the CRS is not used, the HSI48 RC oscillator runs on it default frequency + * which is subject to manufacturing process variations. + */ +#if !defined (HSI48_VALUE) +#define HSI48_VALUE (48000000UL) /*!< Value of the Internal High Speed oscillator for USB FS/RNG in Hz. + The real value my vary depending on manufacturing process variations.*/ +#endif /* HSI48_VALUE */ + +/** + * @brief Internal Low Speed oscillator (LSI) value. + */ +#if !defined (LSI_VALUE) +/*!< Value of the Internal Low Speed oscillator in Hz +The real value may vary depending on the variations in voltage and temperature.*/ +#define LSI_VALUE (32000UL) /*!< LSI Typical Value in Hz*/ +#endif /* LSI_VALUE */ +/** + * @brief External Low Speed oscillator (LSE) value. + * This value is used by the UART, RTC HAL module to compute the system frequency + */ +#if !defined (LSE_VALUE) +#define LSE_VALUE (32768UL) /*!< Value of the External oscillator in Hz*/ +#endif /* LSE_VALUE */ + +#if !defined (LSE_STARTUP_TIMEOUT) +#define LSE_STARTUP_TIMEOUT (5000UL) /*!< Time out for LSE start up, in ms */ +#endif /* LSE_STARTUP_TIMEOUT */ + +/** + * @brief External clock source for I2S and SAI peripherals + * This value is used by the I2S and SAI HAL modules to compute the I2S and SAI clock source + * frequency, this source is inserted directly through I2S_CKIN pad. + */ +#if !defined (EXTERNAL_CLOCK_VALUE) +#define EXTERNAL_CLOCK_VALUE (48000UL) /*!< Value of the External clock source in Hz*/ +#endif /* EXTERNAL_CLOCK_VALUE */ + +/* Tip: To avoid modifying this file each time you need to use different HSE, + === you can define the HSE value in your toolchain compiler preprocessor. */ + +/* ########################### System Configuration ######################### */ +/** + * @brief This is the HAL system configuration section + */ +#define VDD_VALUE (3300UL) /*!< Value of VDD in mv */ +#define TICK_INT_PRIORITY (0x0FUL) /*!< tick interrupt priority */ +#define USE_RTOS 0U +#define PREFETCH_ENABLE 1U +#define INSTRUCTION_CACHE_ENABLE 1U +#define DATA_CACHE_ENABLE 1U + +/* ########################## Assert Selection ############################## */ +/** + * @brief Uncomment the line below to expanse the "assert_param" macro in the + * HAL drivers code + */ +/* #define USE_FULL_ASSERT 1U */ + +/* ################## SPI peripheral configuration ########################## */ + +/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver + * Activated: CRC code is present inside driver + * Deactivated: CRC code cleaned from driver + */ + +#define USE_SPI_CRC 1U + +/* Includes ------------------------------------------------------------------*/ +/** + * @brief Include module's header file + */ + +#ifdef HAL_RCC_MODULE_ENABLED +#include "stm32g4xx_hal_rcc.h" +#endif /* HAL_RCC_MODULE_ENABLED */ + +#ifdef HAL_GPIO_MODULE_ENABLED +#include "stm32g4xx_hal_gpio.h" +#endif /* HAL_GPIO_MODULE_ENABLED */ + +#ifdef HAL_DMA_MODULE_ENABLED +#include "stm32g4xx_hal_dma.h" +#endif /* HAL_DMA_MODULE_ENABLED */ + +#ifdef HAL_CORTEX_MODULE_ENABLED +#include "stm32g4xx_hal_cortex.h" +#endif /* HAL_CORTEX_MODULE_ENABLED */ + +#ifdef HAL_ADC_MODULE_ENABLED +#include "stm32g4xx_hal_adc.h" +#endif /* HAL_ADC_MODULE_ENABLED */ + +#ifdef HAL_COMP_MODULE_ENABLED +#include "stm32g4xx_hal_comp.h" +#endif /* HAL_COMP_MODULE_ENABLED */ + +#ifdef HAL_CORDIC_MODULE_ENABLED +#include "stm32g4xx_hal_cordic.h" +#endif /* HAL_CORDIC_MODULE_ENABLED */ + +#ifdef HAL_CRC_MODULE_ENABLED +#include "stm32g4xx_hal_crc.h" +#endif /* HAL_CRC_MODULE_ENABLED */ + +#ifdef HAL_CRYP_MODULE_ENABLED +#include "stm32g4xx_hal_cryp.h" +#endif /* HAL_CRYP_MODULE_ENABLED */ + +#ifdef HAL_DAC_MODULE_ENABLED +#include "stm32g4xx_hal_dac.h" +#endif /* HAL_DAC_MODULE_ENABLED */ + +#ifdef HAL_EXTI_MODULE_ENABLED +#include "stm32g4xx_hal_exti.h" +#endif /* HAL_EXTI_MODULE_ENABLED */ + +#ifdef HAL_FDCAN_MODULE_ENABLED +#include "stm32g4xx_hal_fdcan.h" +#endif /* HAL_FDCAN_MODULE_ENABLED */ + +#ifdef HAL_FLASH_MODULE_ENABLED +#include "stm32g4xx_hal_flash.h" +#endif /* HAL_FLASH_MODULE_ENABLED */ + +#ifdef HAL_FMAC_MODULE_ENABLED +#include "stm32g4xx_hal_fmac.h" +#endif /* HAL_FMAC_MODULE_ENABLED */ + +#ifdef HAL_HRTIM_MODULE_ENABLED +#include "stm32g4xx_hal_hrtim.h" +#endif /* HAL_HRTIM_MODULE_ENABLED */ + +#ifdef HAL_IRDA_MODULE_ENABLED +#include "stm32g4xx_hal_irda.h" +#endif /* HAL_IRDA_MODULE_ENABLED */ + +#ifdef HAL_IWDG_MODULE_ENABLED +#include "stm32g4xx_hal_iwdg.h" +#endif /* HAL_IWDG_MODULE_ENABLED */ + +#ifdef HAL_I2C_MODULE_ENABLED +#include "stm32g4xx_hal_i2c.h" +#endif /* HAL_I2C_MODULE_ENABLED */ + +#ifdef HAL_I2S_MODULE_ENABLED +#include "stm32g4xx_hal_i2s.h" +#endif /* HAL_I2S_MODULE_ENABLED */ + +#ifdef HAL_LPTIM_MODULE_ENABLED +#include "stm32g4xx_hal_lptim.h" +#endif /* HAL_LPTIM_MODULE_ENABLED */ + +#ifdef HAL_NAND_MODULE_ENABLED +#include "stm32g4xx_hal_nand.h" +#endif /* HAL_NAND_MODULE_ENABLED */ + +#ifdef HAL_NOR_MODULE_ENABLED +#include "stm32g4xx_hal_nor.h" +#endif /* HAL_NOR_MODULE_ENABLED */ + +#ifdef HAL_OPAMP_MODULE_ENABLED +#include "stm32g4xx_hal_opamp.h" +#endif /* HAL_OPAMP_MODULE_ENABLED */ + +#ifdef HAL_PCD_MODULE_ENABLED +#include "stm32g4xx_hal_pcd.h" +#endif /* HAL_PCD_MODULE_ENABLED */ + +#ifdef HAL_PWR_MODULE_ENABLED +#include "stm32g4xx_hal_pwr.h" +#endif /* HAL_PWR_MODULE_ENABLED */ + +#ifdef HAL_QSPI_MODULE_ENABLED +#include "stm32g4xx_hal_qspi.h" +#endif /* HAL_QSPI_MODULE_ENABLED */ + +#ifdef HAL_RNG_MODULE_ENABLED +#include "stm32g4xx_hal_rng.h" +#endif /* HAL_RNG_MODULE_ENABLED */ + +#ifdef HAL_RTC_MODULE_ENABLED +#include "stm32g4xx_hal_rtc.h" +#endif /* HAL_RTC_MODULE_ENABLED */ + +#ifdef HAL_SAI_MODULE_ENABLED +#include "stm32g4xx_hal_sai.h" +#endif /* HAL_SAI_MODULE_ENABLED */ + +#ifdef HAL_SMARTCARD_MODULE_ENABLED +#include "stm32g4xx_hal_smartcard.h" +#endif /* HAL_SMARTCARD_MODULE_ENABLED */ + +#ifdef HAL_SMBUS_MODULE_ENABLED +#include "stm32g4xx_hal_smbus.h" +#endif /* HAL_SMBUS_MODULE_ENABLED */ + +#ifdef HAL_SPI_MODULE_ENABLED +#include "stm32g4xx_hal_spi.h" +#endif /* HAL_SPI_MODULE_ENABLED */ + +#ifdef HAL_SRAM_MODULE_ENABLED +#include "stm32g4xx_hal_sram.h" +#endif /* HAL_SRAM_MODULE_ENABLED */ + +#ifdef HAL_TIM_MODULE_ENABLED +#include "stm32g4xx_hal_tim.h" +#endif /* HAL_TIM_MODULE_ENABLED */ + +#ifdef HAL_UART_MODULE_ENABLED +#include "stm32g4xx_hal_uart.h" +#endif /* HAL_UART_MODULE_ENABLED */ + +#ifdef HAL_USART_MODULE_ENABLED +#include "stm32g4xx_hal_usart.h" +#endif /* HAL_USART_MODULE_ENABLED */ + +#ifdef HAL_WWDG_MODULE_ENABLED +#include "stm32g4xx_hal_wwdg.h" +#endif /* HAL_WWDG_MODULE_ENABLED */ + +/* Exported macro ------------------------------------------------------------*/ +#ifdef USE_FULL_ASSERT +/** + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ +#define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__)) +/* Exported functions ------------------------------------------------------- */ +void assert_failed(uint8_t *file, uint32_t line); +#else +#define assert_param(expr) ((void)0U) +#endif /* USE_FULL_ASSERT */ + +#ifdef __cplusplus +} +#endif + +#endif /* STM32G4xx_HAL_CONF_H */ + + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/startup/system_stm32g4xx.c b/src/main/startup/system_stm32g4xx.c new file mode 100644 index 0000000000..f6f9258284 --- /dev/null +++ b/src/main/startup/system_stm32g4xx.c @@ -0,0 +1,456 @@ +/** + ****************************************************************************** + * @file system_stm32g4xx.c + * @author MCD Application Team + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File + * + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2017 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + +#include + +#include "stm32g4xx.h" +#include "drivers/system.h" +#include "platform.h" +#include "drivers/persistent.h" + +#if !defined (HSE_VALUE) + #define HSE_VALUE 8000000U /*!< Value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ + +#if !defined (HSI_VALUE) + #define HSI_VALUE 16000000U /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ + + /* The SystemCoreClock variable is updated in three ways: + 1) by calling CMSIS function SystemCoreClockUpdate() + 2) by calling HAL API function HAL_RCC_GetHCLKFreq() + 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency + Note: If you use this function to configure the system clock; then there + is no need to call the 2 first functions listed above, since SystemCoreClock + variable is updated automatically. + */ + uint32_t SystemCoreClock = HSI_VALUE; + + const uint8_t AHBPrescTable[16] = {0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U, 6U, 7U, 8U, 9U}; + const uint8_t APBPrescTable[8] = {0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U}; + +void SystemClock_Config(void); // Forward + +void SystemInit(void) +{ + systemCheckResetReason(); + + initialiseMemorySections(); + + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << (10*2))|(3UL << (11*2))); /* set CP10 and CP11 Full Access */ + #endif + + /* Configure the Vector Table location add offset address ------------------*/ + + extern uint8_t isr_vector_table_flash_base; + extern uint8_t isr_vector_table_base; + extern uint8_t isr_vector_table_end; + + if (&isr_vector_table_base != &isr_vector_table_flash_base) { + memcpy(&isr_vector_table_base, &isr_vector_table_flash_base, (size_t) (&isr_vector_table_end - &isr_vector_table_base)); + } + + SCB->VTOR = (uint32_t)&isr_vector_table_base; + +#ifdef USE_HAL_DRIVER + HAL_Init(); +#endif + + SystemClock_Config(); + SystemCoreClockUpdate(); + + // Enable BOOT0 pin, which some STM32G4 board, notably Nucleo-G474RE, comes disabled. + systemBOOT0PinBootLoaderEnable(); +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(**) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(***) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(***) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (**) HSI_VALUE is a constant defined in stm32g4xx_hal.h file (default value + * 16 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (***) HSE_VALUE is a constant defined in stm32g4xx_hal.h file (default value + * 8 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate(void) +{ + uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE); + + uint32_t tmp, pllvco, pllr, pllsource, pllm; + + /* Get SYSCLK source -------------------------------------------------------*/ + switch (RCC->CFGR & RCC_CFGR_SWS) + { + case 0x04: /* HSI used as system clock source */ + SystemCoreClock = HSI_VALUE; + break; + + case 0x08: /* HSE used as system clock source */ + SystemCoreClock = hse_value; + break; + + case 0x0C: /* PLL used as system clock source */ + /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN + SYSCLK = PLL_VCO / PLLR + */ + pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC); + pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U ; + if (pllsource == 0x02UL) /* HSI used as PLL clock source */ + { + pllvco = (HSI_VALUE / pllm); + } + else /* HSE used as PLL clock source */ + { + pllvco = (hse_value / pllm); + } + pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8); + pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U; + SystemCoreClock = pllvco/pllr; + break; + + default: + break; + } + /* Compute HCLK clock frequency --------------------------------------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +// SystemSYSCLKSource +// 0: HSI +// 1: HSE +// (2: PLLP) +// 3: PLLR + +int SystemSYSCLKSource(void) +{ + uint32_t rawSrc = RCC->CFGR & RCC_CFGR_SW; + int src = 0; + + switch (rawSrc) { + case 0: // can't happen, fall through + FALLTHROUGH; + case 1: + src = 0; // HSI + break; + + case 2: + src = 1; // HSE + break; + + case 3: + src = 3; // PLL-R + } + + return src; +} + +// SystemPLLSource +// 0: HSI +// 1: HSE + +int SystemPLLSource(void) +{ + return (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) & 1; // LSB determines HSI(0) or HSE(1) +} + +void Error_Handler(void) +{ + while (1) { + } +} + +/* + * G4 is capable of fine granularity overclocking thanks to a separate 48MHz source, + * but we keep the overclocking capability to match that of F405 (clocks at 168, 192, 216, 240). + * + * However, due to restrictions on MCO source, designs that wishes to generate 27MHz on MCO + * must use a 27MHz HSE source. The 27MHz HSE source will produce slightly different series of clocks + * due to restriction on PLL multipler range. + * + * If mhz == 8, 16 or 24 then scale it down to 4 and start with PLLN=42 for base 168MHz, + * with PLLN increment of 6 (4 * 6 = 24MHz a part) + * + * If mhz == 27 then scale it down to 9 with PLL=19 for base 171MHz with PLLN increment of 3 (9 * 3 = 27MHz a part) + * + * We don't prepare a separate frequency selection for 27MHz series in CLI, so what is set with "cpu_overclock" + * will result in slightly higher clock when examined with "status" command. + */ + +// Target frequencies for cpu_overclock (Levels 0 through 3) + +uint16_t sysclkSeries8[] = { 168, 192, 216, 240 }; +uint16_t sysclkSeries27[] = { 171, 198, 225, 252 }; +#define OVERCLOCK_LEVELS ARRAYLEN(sysclkSeries8) + +// Generic fine granularity PLL parameter calculation + +static bool systemComputePLLParameters(uint8_t src, uint16_t target, int *sysclk, int *pllm, int *plln, int *pllr) +{ + int vcoTarget = target * 2; + int vcoBase; + int vcoDiff; + int multDiff; + int vcoFreq; + + *pllr = 2; + + if (src == 8 || src == 16 || src == 24) { + *pllm = src / 8; + vcoBase = 168 * 2; + vcoDiff = vcoTarget - vcoBase; + multDiff = vcoDiff / 16 * 2; + *plln = 42 + multDiff; + vcoFreq = 8 * *plln; + } else if (src == 27) { + *pllm = 3; + vcoBase = 171 * 2; + vcoDiff = vcoTarget - vcoBase; + multDiff = vcoDiff / 18 * 2; + *plln = 38 + multDiff; + vcoFreq = 9 * *plln; + } else { + return false; + } + + // VCO seems to top out at 590MHz or so. Give it some margin. + + if (vcoFreq >= 560) { + return false; + } + *sysclk = vcoFreq / 2; + + return true; +} + +static int pll_m; +static int pll_n; +static int pll_r; +static uint32_t pllSrc; +static int sysclkMhz; + +static bool systemClock_PLLConfig(int overclockLevel) +{ + uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE); + int pllInput; + int targetMhz; + + if (hse_value == 0) { + pllInput = 16; // HSI + pllSrc = RCC_PLLSOURCE_HSI; + targetMhz = 168; + } else { + pllInput = hse_value / 1000000; + pllSrc = RCC_PLLSOURCE_HSE; + if (pllInput == 8 || pllInput == 16 || pllInput == 24) { + targetMhz = sysclkSeries8[overclockLevel]; + } else if (pllInput == 27) { + targetMhz = sysclkSeries8[overclockLevel]; + } else { + return false; + } + } + + return systemComputePLLParameters(pllInput, targetMhz, &sysclkMhz, &pll_m, &pll_n, &pll_r); +} + +void systemClockSetHSEValue(uint32_t frequency) +{ + uint32_t freqMhz = frequency / 1000000; + + // Only supported HSE crystal/resonator is 27MHz or integer multiples of 8MHz + + if (freqMhz != 27 && (freqMhz / 8) * 8 != freqMhz) { + return; + } + + uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE); + + if (hse_value != frequency) { + persistentObjectWrite(PERSISTENT_OBJECT_HSE_VALUE, frequency); + + if (hse_value != 0) { + __disable_irq(); + NVIC_SystemReset(); + } + } +} + +void OverclockRebootIfNecessary(unsigned requestedOverclockLevel) +{ + uint32_t currentOverclockLevel = persistentObjectRead(PERSISTENT_OBJECT_OVERCLOCK_LEVEL); + + if (requestedOverclockLevel >= OVERCLOCK_LEVELS ) { + requestedOverclockLevel = 0; + } + + // If we are not running at the requested speed or + // we are running on PLL-HSI even HSE has been set, + // then remember the requested clock and issue soft reset + + uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE); + + if ((currentOverclockLevel != requestedOverclockLevel) || + (hse_value && + SystemSYSCLKSource() == 3 /* PLL-R */ && + SystemPLLSource() != 1 /* HSE */)) { + + // Make sure we can configure the requested clock. + if (!systemClock_PLLConfig(requestedOverclockLevel)) { + return; + } + persistentObjectWrite(PERSISTENT_OBJECT_OVERCLOCK_LEVEL, requestedOverclockLevel); + __disable_irq(); + NVIC_SystemReset(); + } +} + +/** + * @brief System Clock Configuration + * @retval None + */ +// Extracted from MX generated main.c + +void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; + RCC_PeriphCLKInitTypeDef PeriphClkInit = {0}; + RCC_CRSInitTypeDef pInit = {0}; + + systemClock_PLLConfig(persistentObjectRead(PERSISTENT_OBJECT_OVERCLOCK_LEVEL)); + + // Configure the main internal regulator output voltage + + HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST); + + // Initializes the CPU, AHB and APB busses clocks + + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_HSI48 + |RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE; + + RCC_OscInitStruct.HSEState = RCC_HSE_ON; + RCC_OscInitStruct.HSIState = RCC_HSI_ON; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.LSIState = RCC_LSI_ON; + RCC_OscInitStruct.HSI48State = RCC_HSI48_ON; + + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = pllSrc; + RCC_OscInitStruct.PLL.PLLM = pll_m; + RCC_OscInitStruct.PLL.PLLN = pll_n; + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; + RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2; + RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2; + + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + // Initializes the CPU, AHB and APB busses clocks + + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK + |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_8) != HAL_OK) + { + Error_Handler(); + } + + // Initializes the peripherals clocks + + PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USART1|RCC_PERIPHCLK_USART2 + |RCC_PERIPHCLK_USART3|RCC_PERIPHCLK_UART4 + |RCC_PERIPHCLK_UART5|RCC_PERIPHCLK_I2C1 + |RCC_PERIPHCLK_I2C2|RCC_PERIPHCLK_I2C3 + |RCC_PERIPHCLK_I2C4|RCC_PERIPHCLK_USB + |RCC_PERIPHCLK_ADC12 + |RCC_PERIPHCLK_ADC345 + |RCC_PERIPHCLK_FDCAN + |RCC_PERIPHCLK_LPUART1 + ; + PeriphClkInit.Usart1ClockSelection = RCC_USART1CLKSOURCE_PCLK2; + PeriphClkInit.Usart2ClockSelection = RCC_USART2CLKSOURCE_PCLK1; + PeriphClkInit.Usart3ClockSelection = RCC_USART3CLKSOURCE_PCLK1; + PeriphClkInit.Uart4ClockSelection = RCC_UART4CLKSOURCE_PCLK1; + PeriphClkInit.Uart5ClockSelection = RCC_UART5CLKSOURCE_PCLK1; + PeriphClkInit.Lpuart1ClockSelection = RCC_LPUART1CLKSOURCE_PCLK1; + PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1; + PeriphClkInit.I2c2ClockSelection = RCC_I2C2CLKSOURCE_PCLK1; + PeriphClkInit.I2c3ClockSelection = RCC_I2C3CLKSOURCE_PCLK1; + PeriphClkInit.I2c4ClockSelection = RCC_I2C4CLKSOURCE_PCLK1; + PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_SYSCLK; + PeriphClkInit.Adc345ClockSelection = RCC_ADC345CLKSOURCE_SYSCLK; + PeriphClkInit.FdcanClockSelection = RCC_FDCANCLKSOURCE_PCLK1; + PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_HSI48; + PeriphClkInit.RTCClockSelection = RCC_RTCCLKSOURCE_LSI; + + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) + { + Error_Handler(); + } + + // Configures CRS + + pInit.Prescaler = RCC_CRS_SYNC_DIV1; + pInit.Source = RCC_CRS_SYNC_SOURCE_USB; + pInit.Polarity = RCC_CRS_SYNC_POLARITY_RISING; + pInit.ReloadValue = __HAL_RCC_CRS_RELOADVALUE_CALCULATE(48000000,1000); + pInit.ErrorLimitValue = 34; + pInit.HSI48CalibrationValue = 32; + + HAL_RCCEx_CRSConfig(&pInit); +} diff --git a/src/main/startup/system_stm32g4xx.h b/src/main/startup/system_stm32g4xx.h new file mode 100755 index 0000000000..587324f6b2 --- /dev/null +++ b/src/main/startup/system_stm32g4xx.h @@ -0,0 +1,105 @@ +/** + ****************************************************************************** + * @file system_stm32g4xx.h + * @author MCD Application Team + * @brief CMSIS Cortex-M4 Device System Source File for STM32G4xx devices. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2017 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ +#pragma once + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32g4xx_system + * @{ + */ + +#ifdef __cplusplus + extern "C" { +#endif + +/** @addtogroup STM32G4xx_System_Includes + * @{ + */ + +/** + * @} + */ + + +/** @addtogroup STM32G4xx_System_Exported_Variables + * @{ + */ + /* The SystemCoreClock variable is updated in three ways: + 1) by calling CMSIS function SystemCoreClockUpdate() + 2) by calling HAL API function HAL_RCC_GetSysClockFreq() + 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency + Note: If you use this function to configure the system clock; then there + is no need to call the 2 first functions listed above, since SystemCoreClock + variable is updated automatically. + */ +extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ + +extern const uint8_t AHBPrescTable[16]; /*!< AHB prescalers table values */ +extern const uint8_t APBPrescTable[8]; /*!< APB prescalers table values */ + +/** + * @} + */ + +/** @addtogroup STM32G4xx_System_Exported_Constants + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32G4xx_System_Exported_Macros + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32G4xx_System_Exported_Functions + * @{ + */ + +extern void SystemInit(void); +extern void SystemCoreClockUpdate(void); + +extern void systemClockSetHSEValue(uint32_t frequency); +extern void OverclockRebootIfNecessary(unsigned requestedOverclockLevel); +extern int SystemSYSCLKSource(void); +extern int SystemPLLSource(void); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 03d73d7ea2..79b6b1663c 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -125,7 +125,27 @@ #define USE_DMA_RAM #endif -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) +#ifdef STM32G4 +#define USE_FAST_RAM +#define USE_DSHOT +#define USE_DSHOT_BITBANG +#define USE_DSHOT_TELEMETRY +#define USE_DSHOT_TELEMETRY_STATS +#define USE_RPM_FILTER +#define USE_DYN_IDLE +#define I2C3_OVERCLOCK true +#define I2C4_OVERCLOCK true +#define USE_OVERCLOCK +#define USE_GYRO_DATA_ANALYSE +#define USE_ADC_INTERNAL +#define USE_USB_MSC +#define USE_USB_CDC_HID +#define USE_MCO +#define USE_DMA_SPEC +#define USE_TIMER_MGMT +#endif + +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) #define TASK_GYROPID_DESIRED_PERIOD 125 // 125us = 8kHz #define SCHEDULER_DELAY_LIMIT 10 #else @@ -182,11 +202,20 @@ #endif #ifdef USE_DMA_RAM +#if defined(STM32H7) #define DMA_RAM __attribute__((section(".DMA_RAM"))) #define DMA_RW_AXI __attribute__((section(".DMA_RW_AXI"))) +#elif defined(STM32G4) +#define DMA_RAM_R __attribute__((section(".DMA_RAM_R"))) +#define DMA_RAM_W __attribute__((section(".DMA_RAM_W"))) +#define DMA_RAM_RW __attribute__((section(".DMA_RAM_RW"))) +#endif #else #define DMA_RAM #define DMA_RW_AXI +#define DMA_RAM_R +#define DMA_RAM_W +#define DMA_RAM_RW #endif #define USE_BRUSHED_ESC_AUTODETECT // Detect if brushed motors are connected and set defaults appropriately to avoid motors spinning on boot