diff --git a/make/mcu/STM32F4.mk b/make/mcu/STM32F4.mk
index a7abfb7ef..206bb6d56 100644
--- a/make/mcu/STM32F4.mk
+++ b/make/mcu/STM32F4.mk
@@ -37,7 +37,6 @@ EXCLUDES = stm32f4xx_crc.c \
stm32f4xx_cryp_aes.c \
stm32f4xx_hash_md5.c \
stm32f4xx_cryp_des.c \
- stm32f4xx_rtc.c \
stm32f4xx_hash.c \
stm32f4xx_dbgmcu.c \
stm32f4xx_cryp_tdes.c \
@@ -183,7 +182,8 @@ MCU_COMMON_SRC = \
drivers/serial_uart_init.c \
drivers/serial_uart_stm32f4xx.c \
drivers/system_stm32f4xx.c \
- drivers/timer_stm32f4xx.c
+ drivers/timer_stm32f4xx.c \
+ drivers/persistent.c
ifeq ($(PERIPH_DRIVER), HAL)
VCP_SRC = \
diff --git a/src/main/drivers/persistent.c b/src/main/drivers/persistent.c
new file mode 100644
index 000000000..682c7a718
--- /dev/null
+++ b/src/main/drivers/persistent.c
@@ -0,0 +1,107 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
+ * 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 this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * An implementation of persistent data storage utilizing RTC backup data register.
+ * Retains values written across software resets and boot loader activities.
+ */
+
+#include
+#include "platform.h"
+
+#include "drivers/persistent.h"
+
+#define PERSISTENT_OBJECT_MAGIC_VALUE (('B' << 24)|('e' << 16)|('f' << 8)|('1' << 0))
+
+#ifdef USE_HAL_DRIVER
+
+static RTC_HandleTypeDef rtcHandle = { .Instance = RTC } ;
+
+uint32_t persistentObjectRead(persistentObjectId_e id)
+{
+ uint32_t value = HAL_RTCEx_BKUPRead(&rtcHandle, id);
+
+ return value;
+}
+
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value)
+{
+ HAL_RTCEx_BKUPWrite(&rtcHandle, id, value);
+}
+
+void persistentObjectRTCEnable(void)
+{
+ __HAL_RCC_PWR_CLK_ENABLE();
+ HAL_PWR_EnableBkUpAccess();
+
+ __HAL_RCC_LSI_ENABLE();
+ while(__HAL_RCC_GET_FLAG(RCC_FLAG_LSIRDY) == RESET);
+
+ __HAL_RCC_RTC_CONFIG(RCC_RTCCLKSOURCE_LSI);
+ __HAL_RCC_RTC_ENABLE();
+
+ __HAL_RTC_WRITEPROTECTION_ENABLE(&rtcHandle);
+ __HAL_RTC_WRITEPROTECTION_DISABLE(&rtcHandle);
+}
+
+#else
+uint32_t persistentObjectRead(persistentObjectId_e id)
+{
+ uint32_t value = RTC_ReadBackupRegister(id);
+
+ return value;
+}
+
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value)
+{
+ RTC_WriteBackupRegister(id, value);
+}
+
+void persistentObjectRTCEnable(void)
+{
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE);
+ PWR_BackupAccessCmd(ENABLE);
+
+ RCC_LSICmd(ENABLE);
+ while (RCC_GetFlagStatus(RCC_FLAG_LSIRDY) == RESET);
+
+ RCC_RTCCLKConfig(RCC_RTCCLKSource_LSI);
+ RCC_RTCCLKCmd(ENABLE);
+
+ RTC_WriteProtectionCmd(ENABLE);
+ RTC_WriteProtectionCmd(DISABLE);
+}
+#endif
+
+void persistentObjectInit(void)
+{
+ // Configure and enable RTC for backup register access
+
+ persistentObjectRTCEnable();
+
+ // Magic value checking may be sufficient
+
+ if (!(RCC->CSR & RCC_CSR_SFTRSTF) || (persistentObjectRead(PERSISTENT_OBJECT_MAGIC) != PERSISTENT_OBJECT_MAGIC_VALUE)) {
+ for (int i = 1; i < PERSISTENT_OBJECT_COUNT; i++) {
+ persistentObjectWrite(i, 0);
+ }
+ persistentObjectWrite(PERSISTENT_OBJECT_MAGIC, PERSISTENT_OBJECT_MAGIC_VALUE);
+ }
+}
diff --git a/src/main/drivers/persistent.h b/src/main/drivers/persistent.h
new file mode 100644
index 000000000..90f0568d3
--- /dev/null
+++ b/src/main/drivers/persistent.h
@@ -0,0 +1,38 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
+ * 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 this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+// Available RTC backup registers (4-byte words) per MCU type
+// F4: 20 words
+// F7: 32 words
+// H7: 32 words
+
+typedef enum {
+ PERSISTENT_OBJECT_MAGIC = 0,
+ PERSISTENT_OBJECT_HSE_VALUE,
+ PERSISTENT_OBJECT_OVERCLOCK_LEVEL,
+ PERSISTENT_OBJECT_BOOTLOADER_REQUEST,
+ PERSISTENT_OBJECT_COUNT,
+} persistentObjectId_e;
+
+void persistentObjectInit(void);
+uint32_t persistentObjectRead(persistentObjectId_e id);
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value);
diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c
index 535445170..52039bcf8 100644
--- a/src/main/drivers/system_stm32f4xx.c
+++ b/src/main/drivers/system_stm32f4xx.c
@@ -27,6 +27,7 @@
#include "drivers/exti.h"
#include "drivers/nvic.h"
#include "drivers/system.h"
+#include "drivers/persistent.h"
#define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000)
@@ -38,12 +39,11 @@ void systemReset(void)
NVIC_SystemReset();
}
-PERSISTENT uint32_t bootloaderRequest = 0;
#define BOOTLOADER_REQUEST_COOKIE 0xDEADBEEF
void systemResetToBootloader(void)
{
- bootloaderRequest = BOOTLOADER_REQUEST_COOKIE;
+ persistentObjectWrite(PERSISTENT_OBJECT_BOOTLOADER_REQUEST, BOOTLOADER_REQUEST_COOKIE);
__disable_irq();
NVIC_SystemReset();
@@ -58,12 +58,14 @@ typedef struct isrVector_s {
void checkForBootLoaderRequest(void)
{
+ uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_BOOTLOADER_REQUEST);
+
+ persistentObjectWrite(PERSISTENT_OBJECT_BOOTLOADER_REQUEST, 0);
+
if (bootloaderRequest != BOOTLOADER_REQUEST_COOKIE) {
return;
}
- bootloaderRequest = 0;
-
extern isrVector_t system_isr_vector_table_base;
__set_MSP(system_isr_vector_table_base.stackEnd);
diff --git a/src/main/startup/startup_stm32f40xx.s b/src/main/startup/startup_stm32f40xx.s
index f28a3f158..08bcfc6c1 100644
--- a/src/main/startup/startup_stm32f40xx.s
+++ b/src/main/startup/startup_stm32f40xx.s
@@ -84,6 +84,7 @@ Reset_Handler:
dsb
// Defined in C code
+ bl persistentObjectInit
bl checkForBootLoaderRequest
/* Copy the data segment initializers from flash to SRAM */
diff --git a/src/main/startup/startup_stm32f411xe.s b/src/main/startup/startup_stm32f411xe.s
index 591fc888a..a3e38c12b 100644
--- a/src/main/startup/startup_stm32f411xe.s
+++ b/src/main/startup/startup_stm32f411xe.s
@@ -72,6 +72,7 @@ defined in linker script */
.type Reset_Handler, %function
Reset_Handler:
// Defined in C code
+ bl persistentObjectInit
bl checkForBootLoaderRequest
/* Copy the data segment initializers from flash to SRAM */
diff --git a/src/main/startup/startup_stm32f446xx.s b/src/main/startup/startup_stm32f446xx.s
index 61c8ff4a4..92b8eda3f 100644
--- a/src/main/startup/startup_stm32f446xx.s
+++ b/src/main/startup/startup_stm32f446xx.s
@@ -71,13 +71,9 @@ defined in linker script */
.weak Reset_Handler
.type Reset_Handler, %function
Reset_Handler:
- // Check for bootloader reboot
- ldr r0, =0x2001FFFC // mj666
- ldr r1, =0xDEADBEEF // mj666
- ldr r2, [r0, #0] // mj666
- str r0, [r0, #0] // mj666
- cmp r2, r1 // mj666
- beq Reboot_Loader // mj666
+ // Defined in C code
+ bl persistentObjectInit
+ bl checkForBootLoaderRequest
/* Copy the data segment initializers from flash to SRAM */
movs r1, #0
diff --git a/src/main/target/system_stm32f4xx.c b/src/main/target/system_stm32f4xx.c
index 377559d57..5eac89095 100644
--- a/src/main/target/system_stm32f4xx.c
+++ b/src/main/target/system_stm32f4xx.c
@@ -317,6 +317,8 @@
#include "stm32f4xx.h"
#include "system_stm32f4xx.h"
#include "platform.h"
+#include "drivers/persistent.h"
+
/**
* @}
@@ -460,12 +462,12 @@ static const pllConfig_t overclockLevels[] = {
#define PLL_R 7 // PLL_R output is not used, can be any descent number
#endif
-static PERSISTENT uint32_t currentOverclockLevel = 0;
-static PERSISTENT uint32_t hse_value = 8000000;
-
void SystemInitPLLParameters(void)
{
/* PLL setting for overclocking */
+
+ uint32_t currentOverclockLevel = persistentObjectRead(PERSISTENT_OBJECT_OVERCLOCK_LEVEL);
+
if (currentOverclockLevel >= ARRAYLEN(overclockLevels)) {
return;
}
@@ -487,7 +489,7 @@ void OverclockRebootIfNecessary(uint32_t overclockLevel)
// Reboot to adjust overclock frequency
if (SystemCoreClock != pll->mhz * 1000000U) {
- currentOverclockLevel = overclockLevel;
+ persistentObjectWrite(PERSISTENT_OBJECT_OVERCLOCK_LEVEL, overclockLevel);
__disable_irq();
NVIC_SystemReset();
}
@@ -495,8 +497,10 @@ void OverclockRebootIfNecessary(uint32_t overclockLevel)
void systemClockSetHSEValue(uint32_t frequency)
{
+ uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE);
+
if (hse_value != frequency) {
- hse_value = frequency;
+ persistentObjectWrite(PERSISTENT_OBJECT_HSE_VALUE, frequency);
__disable_irq();
NVIC_SystemReset();
}
@@ -504,11 +508,6 @@ void systemClockSetHSEValue(uint32_t frequency)
void SystemInit(void)
{
- if (!(RCC->CSR & RCC_CSR_SFTRSTF)) {
- currentOverclockLevel = 0;
- hse_value = 0;
- }
-
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
@@ -588,6 +587,8 @@ void SystemInit(void)
*/
void SystemCoreClockUpdate(void)
{
+ uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE);
+
uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;
#if defined(STM32F446xx)
uint32_t pllr = 2;
@@ -673,6 +674,7 @@ static int StartHSx(uint32_t onBit, uint32_t readyBit, int maxWaitCount)
*/
void SetSysClock(void)
{
+ uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE);
uint32_t hse_mhz = hse_value / 1000000;
// Switch to HSI during clock manipulation
diff --git a/src/main/vcpf4/usb_bsp.c b/src/main/vcpf4/usb_bsp.c
index a3f3e4336..27aa963b6 100644
--- a/src/main/vcpf4/usb_bsp.c
+++ b/src/main/vcpf4/usb_bsp.c
@@ -83,9 +83,6 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, ENABLE) ;
- /* enable the PWR clock */
- RCC_APB1PeriphResetCmd(RCC_APB1Periph_PWR, ENABLE);
-
EXTI_ClearITPendingBit(EXTI_Line0);
}
/**