From 1c2eb3f73145f0dcef6744e535caf64a39d3eab5 Mon Sep 17 00:00:00 2001 From: Faduf Date: Sun, 25 Jun 2017 20:01:00 +0200 Subject: [PATCH] Add F4 overclock possibility F4 overclock activation via CLI and not automaticaly when Gyro 32k is activated Add condition in target file to alow overclock Replace ALLOW_OVERCLOCK by DISABLE_OVERCLOCK Remanant flag and soft reboot for overclocking --- src/main/fc/config.c | 11 +++++ src/main/fc/config.h | 3 ++ src/main/fc/fc_init.c | 14 ++++++ src/main/fc/settings.c | 3 ++ src/main/startup/startup_stm32f40xx.s | 68 ++++++++++++++++++++++++++- src/main/target/system_stm32f4xx.c | 29 +++++++++--- src/main/target/system_stm32f4xx.h | 1 + 7 files changed, 121 insertions(+), 8 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index b4d3d6822d..f275e12737 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -120,6 +120,16 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); #ifndef USE_OSD_SLAVE +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) +PG_RESET_TEMPLATE(systemConfig_t, systemConfig, + .pidProfileIndex = 0, + .activeRateProfile = 0, + .debug_mode = DEBUG_MODE, + .task_statistics = true, + .cpu_overclock = false, + .name = { 0 } // FIXME misplaced, see PG_PILOT_CONFIG in CF v1.x +); +#else PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .pidProfileIndex = 0, .activeRateProfile = 0, @@ -128,6 +138,7 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .name = { 0 } // FIXME misplaced, see PG_PILOT_CONFIG in CF v1.x ); #endif +#endif #ifdef USE_OSD_SLAVE PG_RESET_TEMPLATE(systemConfig_t, systemConfig, diff --git a/src/main/fc/config.h b/src/main/fc/config.h index a1b96a08a0..8a27a4e251 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -72,6 +72,9 @@ typedef struct systemConfig_s { uint8_t activeRateProfile; uint8_t debug_mode; uint8_t task_statistics; +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + uint8_t cpu_overclock; +#endif char name[MAX_NAME_LENGTH + 1]; // FIXME misplaced, see PG_PILOT_CONFIG in CF v1.x } systemConfig_t; #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 2858710c3f..64dfe4c582 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -266,6 +266,20 @@ void init(void) ensureEEPROMContainsValidData(); readEEPROM(); +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + // If F4 Overclocking is set and System core clock is not correct a reset is forced + if (systemConfig()->cpu_overclock && SystemCoreClock != 240000000) { + *((uint32_t *)0x2001FFFC) = 0xDEADBABE; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } else if (!systemConfig()->cpu_overclock && SystemCoreClock == 240000000) { + *((uint32_t *)0x2001FFFC) = 0x0; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } + +#endif + systemState |= SYSTEM_STATE_CONFIG_LOADED; //i2cSetOverclock(masterConfig.i2c_overclock); diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 8f3a028ec5..01527d3f16 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -694,6 +694,9 @@ const clivalue_t valueTable[] = { { "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, task_statistics) }, #endif { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DEBUG }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, debug_mode) }, +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + { "cpu_overclock", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, cpu_overclock) }, +#endif // PG_VTX_CONFIG #ifdef VTX_RTC6705 diff --git a/src/main/startup/startup_stm32f40xx.s b/src/main/startup/startup_stm32f40xx.s index db7c6635f9..6fb4ff1372 100644 --- a/src/main/startup/startup_stm32f40xx.s +++ b/src/main/startup/startup_stm32f40xx.s @@ -79,6 +79,14 @@ Reset_Handler: str r1, [r0, #0x30] dsb + // Check for overclocking request + ldr r0, =0x2001FFFC // Faduf + ldr r1, =0xDEADBABE // Faduf + ldr r2, [r0, #0] // Faduf + str r0, [r0, #0] // Faduf + cmp r2, r1 // Faduf + beq Boot_OC // Faduf + // Check for bootloader reboot ldr r0, =0x2001FFFC // mj666 ldr r1, =0xDEADBEEF // mj666 @@ -135,7 +143,8 @@ LoopMarkHeapStack: str r1,[r0] /* Call the clock system intitialization function.*/ - bl SystemInit +/* Done in system_stm32f4xx.c */ + bl SystemInit /* Call the application's entry point.*/ bl main @@ -144,6 +153,63 @@ LoopMarkHeapStack: LoopForever: b LoopForever +Boot_OC: +/* Copy the data segment initializers from flash to SRAM */ + movs r1, #0 + b LoopCopyDataInitOC + +CopyDataInitOC: + ldr r3, =_sidata + ldr r3, [r3, r1] + str r3, [r0, r1] + adds r1, r1, #4 + +LoopCopyDataInitOC: + ldr r0, =_sdata + ldr r3, =_edata + adds r2, r0, r1 + cmp r2, r3 + bcc CopyDataInitOC + ldr r2, =_sbss + b LoopFillZerobssOC +/* Zero fill the bss segment. */ +FillZerobssOC: + movs r3, #0 + str r3, [r2], #4 + +LoopFillZerobssOC: + ldr r3, = _ebss + cmp r2, r3 + bcc FillZerobssOC + +/* Mark the heap and stack */ + ldr r2, =_heap_stack_begin + b LoopMarkHeapStackOC + +MarkHeapStackOC: + movs r3, 0xa5a5a5a5 + str r3, [r2], #4 + +LoopMarkHeapStackOC: + ldr r3, = _heap_stack_end + cmp r2, r3 + bcc MarkHeapStackOC + +/*FPU settings*/ + ldr r0, =0xE000ED88 /* Enable CP10,CP11 */ + ldr r1,[r0] + orr r1,r1,#(0xF << 20) + str r1,[r0] + +/* Call the clock system intitialization function.*/ +/* Done in system_stm32f4xx.c */ + bl SystemInitOC + +/* Call the application's entry point.*/ + bl main + bx lr + + Reboot_Loader: // mj666 // Reboot to ROM // mj666 diff --git a/src/main/target/system_stm32f4xx.c b/src/main/target/system_stm32f4xx.c index e7e6d04063..8dcc984d3b 100644 --- a/src/main/target/system_stm32f4xx.c +++ b/src/main/target/system_stm32f4xx.c @@ -422,8 +422,6 @@ uint32_t hse_value = HSE_VALUE; /** @addtogroup STM32F4xx_System_Private_Variables * @{ */ -/* core clock is simply a mhz of PLL_N / PLL_P */ -uint32_t SystemCoreClock = (PLL_N / PLL_P) * 1000000; __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; @@ -456,8 +454,15 @@ static void SystemInit_ExtMemCtl(void); * @param None * @retval None */ + +uint32_t SystemCoreClock; +uint32_t pll_p = PLL_P, pll_n = PLL_N, pll_q = PLL_Q; + void SystemInit(void) { + /* core clock is simply a mhz of PLL_N / PLL_P */ + SystemCoreClock = (pll_n / pll_p) * 1000000; + /* FPU settings ------------------------------------------------------------*/ #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ @@ -487,7 +492,7 @@ void SystemInit(void) /* Configure the System clock source, PLL Multiplier and Divider factors, AHB/APBx prescalers and Flash settings ----------------------------------*/ - SetSysClock(); + //SetSysClock(); /* Configure the Vector Table location add offset address ------------------*/ #ifdef VECT_TAB_SRAM @@ -497,6 +502,16 @@ void SystemInit(void) #endif } +void SystemInitOC(void) +{ + /* PLL setting for overclocking */ + pll_n = 480; + pll_p = 2; + pll_q = 10; + + SystemInit(); +} + /** * @brief Update SystemCoreClock variable according to Clock Register Values. * The SystemCoreClock variable contains the core clock (HCLK), it can @@ -673,12 +688,12 @@ void SetSysClock(void) #if defined(STM32F446xx) /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | - (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24) | (PLL_R << 28); + RCC->PLLCFGR = PLL_M | (pll_n << 6) | (((pll_p >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (pll_q << 24) | (PLL_R << 28); #else /* Configure the main PLL */ - RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | - (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); + RCC->PLLCFGR = PLL_M | (pll_n << 6) | (((pll_p >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (pll_q << 24); #endif /* STM32F446xx */ /* Enable the main PLL */ diff --git a/src/main/target/system_stm32f4xx.h b/src/main/target/system_stm32f4xx.h index e8f4570489..5fcc8374e2 100644 --- a/src/main/target/system_stm32f4xx.h +++ b/src/main/target/system_stm32f4xx.h @@ -34,6 +34,7 @@ extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ extern void SystemInit(void); +extern void SystemInitOC(void); extern void SystemCoreClockUpdate(void); #ifdef __cplusplus