From d264f002cd3e52e76fc6730b16cf54732eba3bc0 Mon Sep 17 00:00:00 2001
From: Andrey Gusakov <dron0gus@gmail.com>
Date: Wed, 8 Jan 2025 18:13:17 +0300
Subject: [PATCH] RTC fixup for STM32F7

Switch RTC to LSE clock if it is detected on start.

It is not allowed to change source clock of RTC module on STM32.
Source clock can be selected only once after reset of BKP module.
If at first start LSE is failed FW will fallback to inaccurate
LSI (internal RC). If Vbat is present RTC/BKP is not reset between
restarts and RTC is never switched to more accurate LSE event if it
is present.
Implement fixup that will check for the LSE and switch RTC to LSE
through BKP (RTC) module reset and new clock source selection.
---
 .../hw_layer/ports/stm32/stm32f7/hw_ports.mk  |  1 +
 .../ports/stm32/stm32f7/stm32f7xx_rtc.cpp     | 79 +++++++++++++++++++
 firmware/hw_layer/rtc_helper.cpp              |  6 ++
 3 files changed, 86 insertions(+)
 create mode 100644 firmware/hw_layer/ports/stm32/stm32f7/stm32f7xx_rtc.cpp

diff --git a/firmware/hw_layer/ports/stm32/stm32f7/hw_ports.mk b/firmware/hw_layer/ports/stm32/stm32f7/hw_ports.mk
index fee0a40ac6..f832087c4a 100644
--- a/firmware/hw_layer/ports/stm32/stm32f7/hw_ports.mk
+++ b/firmware/hw_layer/ports/stm32/stm32f7/hw_ports.mk
@@ -5,6 +5,7 @@ HW_LAYER_PORT += $(PROJECT_DIR)/hw_layer/ports/stm32/stm32f7/stm32f7xx_hal_flash
 
 HW_LAYER_PORT_CPP += $(PROJECT_DIR)/hw_layer/ports/stm32/stm32f7/mpu_util.cpp \
 					$(PROJECT_DIR)/hw_layer/ports/stm32/stm32_adc_v2.cpp \
+					$(PROJECT_DIR)/hw_layer/ports/stm32/stm32f7/stm32f7xx_rtc.cpp \
 
 # This MCU has a cache, align functions to a cache line for maximum cache efficiency
 USE_OPT += -falign-functions=16
diff --git a/firmware/hw_layer/ports/stm32/stm32f7/stm32f7xx_rtc.cpp b/firmware/hw_layer/ports/stm32/stm32f7/stm32f7xx_rtc.cpp
new file mode 100644
index 0000000000..fe56924188
--- /dev/null
+++ b/firmware/hw_layer/ports/stm32/stm32f7/stm32f7xx_rtc.cpp
@@ -0,0 +1,79 @@
+/**
+ * @file stm32f7xx_rtc.cpp
+ * @brief Real Time Clock STM32F7xx switched from LSE to LSI
+ *
+ * @date Jan 8, 2025
+ * @author Andrey Gusakov
+ */
+
+#include "pch.h"
+
+#if HAL_USE_RTC
+
+/* switch to LSE clock if ECU previously falledback to LSI clock.
+ * This will preserve current time and set it back to RTC after reinit
+ * On STM32 change of RTC source clock can be done only through reset of whole backup domain
+ * This reset does no affect backup ram */
+void hal_lld_rtc_fixup(void)
+{
+#if (STM32_RTCSEL == STM32_RTCSEL_LSE)
+	// we need some more time than defined in RUSEFI_STM32_LSE_WAIT_MAX
+	// this is safe as we check that LSE is runnig before reseting BKP domain (stopping LSE)
+	// After that we are starting LSE again and waiting for LSERDY.
+	int timeout = 1000000000;
+	if ((RCC->BDCR & STM32_RTCSEL_MASK) == STM32_RTCSEL) {
+		// Backup domain is already driven by expected clock
+		return;
+	}
+	if ((RCC->BDCR & RCC_BDCR_LSERDY) == 0) {
+		// LSE is failed to start
+		efiPrintf("LSE in not ready");
+		return;
+	}
+
+	efiPrintf("Switching RTC to LSE clock");
+
+	// Get current time
+	RTCDateTime timespec;
+	rtcGetTime(&RTCD1, &timespec);
+
+	// Reset BKP domain
+	// The BKPSRAM is not affected by this reset
+	// This will also reset LSEON
+	RCC->BDCR |= RCC_BDCR_BDRST;
+	RCC->BDCR &= ~RCC_BDCR_BDRST;
+
+#if defined(STM32_LSE_BYPASS)
+	// LSE Bypass.
+	RCC->BDCR |= STM32_LSEDRV | RCC_BDCR_LSEON | RCC_BDCR_LSEBYP;
+#else
+	// No LSE Bypass.
+	RCC->BDCR |= STM32_LSEDRV | RCC_BDCR_LSEON;
+#endif
+	// Waits until LSE is stable or times out.
+	while (((RCC->BDCR & RCC_BDCR_LSERDY) == 0) && (timeout--)) {
+		//this is executed when RTOS is not ready
+		//chThdSleepMilliseconds(1);
+	}
+
+	// Lets check again
+	if (RCC->BDCR & RCC_BDCR_LSERDY) {
+		RCC->BDCR |= STM32_RTCSEL;
+	} else {
+		// LSE is failed to start
+		efiPrintf("LSE in not ready after restart attemp");
+		// Keep initing
+    RCC->BDCR |= RUSEFI_STM32_LSE_WAIT_MAX_RTCSEL;
+	}
+
+  /* RTC clock enabled.*/
+  RCC->BDCR |= RCC_BDCR_RTCEN;
+
+	// init RTC again
+	rtcInit();
+	// Set previously saved time
+	rtcSetTime(&RTCD1, &timespec);
+#endif
+}
+
+#endif //HAL_USE_RTC
diff --git a/firmware/hw_layer/rtc_helper.cpp b/firmware/hw_layer/rtc_helper.cpp
index b184428103..05db533245 100644
--- a/firmware/hw_layer/rtc_helper.cpp
+++ b/firmware/hw_layer/rtc_helper.cpp
@@ -27,8 +27,14 @@ extern "C" int _gettimeofday(timeval* tv, void* tzvp) {
 #endif // EFI_PROD_CODE
 
 #if EFI_RTC
+
+void PUBLIC_API_WEAK hal_lld_rtc_fixup(void) {
+    /* nop */
+}
+
 void initRtc() {
 	efiPrintf("initRtc()");
+	hal_lld_rtc_fixup();
 	printDateTime(); // this would test RTC, see #311
 }