diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index b1ae26388..9efa3e444 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -35,12 +35,7 @@ #define EXTI_CALLBACK_HANDLER_COUNT 1 #endif -typedef struct extiCallbackHandlerConfig_s { - IRQn_Type irqn; - extiCallbackHandlerFunc* fn; -} extiCallbackHandlerConfig_t; - -static extiCallbackHandlerConfig_t extiHandlerConfigs[EXTI_CALLBACK_HANDLER_COUNT]; +extiCallbackHandlerConfig_t extiHandlerConfigs[EXTI_CALLBACK_HANDLER_COUNT]; void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn) { @@ -62,7 +57,7 @@ static volatile uint32_t sysTickUptime = 0; // cached value of RCC->CSR uint32_t cachedRccCsrValue; -static void cycleCounterInit(void) +void cycleCounterInit(void) { RCC_ClocksTypeDef clocks; RCC_GetClocksFreq(&clocks); @@ -97,59 +92,6 @@ uint32_t millis(void) return sysTickUptime; } -void systemInit(void) -{ -#ifdef CC3D - /* 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); -#endif - // Configure NVIC preempt/priority groups - NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); - -#ifdef STM32F10X - // Turn on clocks for stuff we use - RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); -#endif - - // cache RCC->CSR value to use it in isMPUSoftreset() and others - cachedRccCsrValue = RCC->CSR; -#ifdef STM32F4 - /* 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); - RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE); -#endif - RCC_ClearFlag(); - - enableGPIOPowerUsageAndNoiseReductions(); - -#ifdef STM32F10X - // Set USART1 TX (PA9) to output and high state to prevent a rs232 break condition on reset. - // See issue https://github.com/cleanflight/cleanflight/issues/1433 - gpio_config_t gpio; - - gpio.mode = Mode_Out_PP; - gpio.speed = Speed_2MHz; - gpio.pin = Pin_9; - digitalHi(GPIOA, gpio.pin); - gpioInit(GPIOA, &gpio); - - // Turn off JTAG port 'cause we're using the GPIO for leds -#define AFIO_MAPR_SWJ_CFG_NO_JTAG_SW (0x2 << 24) - AFIO->MAPR |= AFIO_MAPR_SWJ_CFG_NO_JTAG_SW; -#endif - - // Init cycle counter - cycleCounterInit(); - - - memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); - // SysTick - SysTick_Config(SystemCoreClock / 1000); -} - #if 1 void delayMicroseconds(uint32_t us) { diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 65ad02376..6577034ee 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -41,13 +41,22 @@ void failureMode(failureMode_e mode); void systemReset(void); void systemResetToBootloader(void); bool isMPUSoftReset(void); +void cycleCounterInit(void); void enableGPIOPowerUsageAndNoiseReductions(void); // current crystal frequency - 8 or 12MHz + extern uint32_t hse_value; typedef void extiCallbackHandlerFunc(void); +typedef struct extiCallbackHandlerConfig_s { + IRQn_Type irqn; + extiCallbackHandlerFunc* fn; +} extiCallbackHandlerConfig_t; + +extern extiCallbackHandlerConfig_t extiHandlerConfigs[EXTI_CALLBACK_HANDLER_COUNT]; + void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn); void unregisterExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn); diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index 17f3638f9..4ad526850 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#include #include #include #include @@ -22,10 +23,14 @@ #include "platform.h" #include "gpio.h" +#include "nvic.h" #include "system.h" #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) +// from system_stm32f10x.c +void SetSysClock(bool overclock); + void systemReset(void) { // Generate system reset @@ -40,7 +45,6 @@ void systemResetToBootloader(void) { systemReset(); } - void enableGPIOPowerUsageAndNoiseReductions(void) { RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC, ENABLE); @@ -61,3 +65,48 @@ bool isMPUSoftReset(void) else return false; } + +void systemInit(void) +{ + SetSysClock(false); + +#ifdef CC3D + /* 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); +#endif + // Configure NVIC preempt/priority groups + NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); + + // Turn on clocks for stuff we use + RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); + + // cache RCC->CSR value to use it in isMPUSoftreset() and others + cachedRccCsrValue = RCC->CSR; + RCC_ClearFlag(); + + enableGPIOPowerUsageAndNoiseReductions(); + + // Set USART1 TX (PA9) to output and high state to prevent a rs232 break condition on reset. + // See issue https://github.com/cleanflight/cleanflight/issues/1433 + gpio_config_t gpio; + + gpio.mode = Mode_Out_PP; + gpio.speed = Speed_2MHz; + gpio.pin = Pin_9; + digitalHi(GPIOA, gpio.pin); + gpioInit(GPIOA, &gpio); + + // Turn off JTAG port 'cause we're using the GPIO for leds +#define AFIO_MAPR_SWJ_CFG_NO_JTAG_SW (0x2 << 24) + AFIO->MAPR |= AFIO_MAPR_SWJ_CFG_NO_JTAG_SW; + + // Init cycle counter + cycleCounterInit(); + + memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); + // SysTick + SysTick_Config(SystemCoreClock / 1000); +} + diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index e7e22c4c8..7e58ab061 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#include #include #include #include @@ -22,9 +23,11 @@ #include "platform.h" #include "gpio.h" +#include "nvic.h" #include "system.h" #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) +void SetSysClock(); void systemReset(void) { @@ -76,3 +79,26 @@ bool isMPUSoftReset(void) else return false; } + +void systemInit(void) +{ + // Enable FPU + SCB->CPACR = (0x3 << (10 * 2)) | (0x3 << (11 * 2)); + SetSysClock(); + + // Configure NVIC preempt/priority groups + NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); + + // cache RCC->CSR value to use it in isMPUSoftreset() and others + cachedRccCsrValue = RCC->CSR; + RCC_ClearFlag(); + + enableGPIOPowerUsageAndNoiseReductions(); + + // Init cycle counter + cycleCounterInit(); + + memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); + // SysTick + SysTick_Config(SystemCoreClock / 1000); +} diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 78149b18e..bcaf71c9d 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#include #include #include #include @@ -22,6 +23,8 @@ #include "platform.h" #include "gpio.h" +#include "nvic.h" +#include "system.h" #include "exti.h" @@ -35,6 +38,7 @@ #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) +void SetSysClock(void); void systemReset(void) { @@ -81,11 +85,9 @@ void enableGPIOPowerUsageAndNoiseReductions(void) 0, ENABLE ); - RCC_AHB2PeriphClockCmd( - 0, ENABLE); + RCC_AHB2PeriphClockCmd(0, ENABLE); #ifdef STM32F40_41xxx - RCC_AHB3PeriphClockCmd( - 0, ENABLE); + RCC_AHB3PeriphClockCmd(0, ENABLE); #endif RCC_APB1PeriphClockCmd( RCC_APB1Periph_TIM2 | @@ -165,4 +167,30 @@ bool isMPUSoftReset(void) return false; } +void systemInit(void) +{ + SetSysClock(); + + // Configure NVIC preempt/priority groups + NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); + + // cache RCC->CSR 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); + RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE); + + RCC_ClearFlag(); + + enableGPIOPowerUsageAndNoiseReductions(); + + // Init cycle counter + cycleCounterInit(); + + memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); + // SysTick + SysTick_Config(SystemCoreClock / 1000); +} diff --git a/src/main/main.c b/src/main/main.c index f00e88af3..3c6a97059 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -135,13 +135,6 @@ void spektrumBind(rxConfig_t *rxConfig); const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig); void sonarInit(const sonarHardware_t *sonarHardware); -#ifdef STM32F10X -// from system_stm32f10x.c -void SetSysClock(bool overclock); -#else -void SetSysClock(void); -#endif - typedef enum { SYSTEM_STATE_INITIALISING = 0, SYSTEM_STATE_CONFIG_LOADED = (1 << 0), @@ -167,24 +160,13 @@ void init(void) systemState |= SYSTEM_STATE_CONFIG_LOADED; -#ifdef STM32F3 - // start fpu - SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); - SetSysClock(); -#endif -#ifdef STM32F1 - // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers - // Configure the Flash Latency cycles and enable prefetch buffer - SetSysClock(0); // TODO - Remove from config in the future -#endif -#ifdef STM32F4 - SetSysClock(); -#endif + systemInit(); //i2cSetOverclock(masterConfig.i2c_overclock); - systemInit(); - + // initialize IO (needed for all IO operations) + IOInitGlobal(); + debugMode = masterConfig.debug_mode; #ifdef USE_HARDWARE_REVISION_DETECTION @@ -194,9 +176,6 @@ void init(void) // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); - // initialize IO (needed for all IO operations) - IOInitGlobal(); - #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_1) { ledInit(false);