Add timeout and adjustment to LSI

1 files changed, 23 insertions, 9 deletions

diff --git a/ports/stm/supervisor/port.c b/ports/stm/supervisor/port.c
index 779348c02..328a8d660 100644
--- a/ports/stm/supervisor/port.c
+++ b/ports/stm/supervisor/port.c
@@ -153,9 +153,9 @@ __attribute__((used, naked)) void Reset_Handler(void) {
 static RTC_HandleTypeDef _hrtc;
 
 #if BOARD_HAS_LOW_SPEED_CRYSTAL
-#define RTC_CLOCK_FREQUENCY LSE_VALUE
+static uint32_t rtc_clock_frequency = LSE_VALUE;
 #else
-#define RTC_CLOCK_FREQUENCY LSI_VALUE
+static uint32_t rtc_clock_frequency = LSI_VALUE;
 #endif
 
 safe_mode_t port_init(void) {
@@ -172,22 +172,36 @@ safe_mode_t port_init(void) {
     HAL_PWR_EnableBkUpAccess();
     #if BOARD_HAS_LOW_SPEED_CRYSTAL
     __HAL_RCC_LSE_CONFIG(RCC_LSE_ON);
-    while(__HAL_RCC_GET_FLAG(RCC_FLAG_LSERDY) == RESET) {}
+    bool lse_setupsuccess = true;
+    uint32_t i = 0;
+    while(__HAL_RCC_GET_FLAG(RCC_FLAG_LSERDY) == RESET) {
+        if ( ++i > 1000000 )
+        {
+            lse_setupsuccess = false;
+            __HAL_RCC_LSE_CONFIG(RCC_LSE_OFF);
+            __HAL_RCC_LSI_ENABLE();
+            rtc_clock_frequency = LSI_VALUE;
+        }
+    }
     #else
     __HAL_RCC_LSI_ENABLE();
     #endif
     #if BOARD_HAS_LOW_SPEED_CRYSTAL
-    __HAL_RCC_RTC_CONFIG(RCC_RTCCLKSOURCE_LSE);
+    if (lse_setupsuccess) {
+        __HAL_RCC_RTC_CONFIG(RCC_RTCCLKSOURCE_LSE);
+    } else {
+        __HAL_RCC_RTC_CONFIG(RCC_RTCCLKSOURCE_LSI);
+    }
     #else
     __HAL_RCC_RTC_CONFIG(RCC_RTCCLKSOURCE_LSI);
     #endif
     __HAL_RCC_RTC_ENABLE();
     _hrtc.Instance = RTC;
     _hrtc.Init.HourFormat = RTC_HOURFORMAT_24;
-    // Divide async as little as possible so that we have RTC_CLOCK_FREQUENCY count in subseconds.
+    // Divide async as little as possible so that we have rtc_clock_frequency count in subseconds.
     // This ensures our timing > 1 second is correct.
     _hrtc.Init.AsynchPrediv = 0x0;
-    _hrtc.Init.SynchPrediv = RTC_CLOCK_FREQUENCY - 1;
+    _hrtc.Init.SynchPrediv = rtc_clock_frequency - 1;
     _hrtc.Init.OutPut = RTC_OUTPUT_DISABLE;
 
     HAL_RTC_Init(&_hrtc);
@@ -293,7 +307,7 @@ volatile uint32_t cached_date = 0;
 volatile uint32_t seconds_to_minute = 0;
 volatile uint32_t cached_hours_minutes = 0;
 uint64_t port_get_raw_ticks(uint8_t* subticks) {
-    uint32_t subseconds = RTC_CLOCK_FREQUENCY - (uint32_t)(RTC->SSR);
+    uint32_t subseconds = rtc_clock_frequency - (uint32_t)(RTC->SSR);
     uint32_t time = (uint32_t)(RTC->TR & RTC_TR_RESERVED_MASK);
     uint32_t date = (uint32_t)(RTC->DR & RTC_DR_RESERVED_MASK);
     if (date != cached_date) {
@@ -341,7 +355,7 @@ void RTC_Alarm_IRQHandler(void) {
 
 // Enable 1/1024 second tick.
 void port_enable_tick(void) {
-    HAL_RTCEx_SetWakeUpTimer_IT(&_hrtc, RTC_CLOCK_FREQUENCY / 1024 / 2, RTC_WAKEUPCLOCK_RTCCLK_DIV2);
+    HAL_RTCEx_SetWakeUpTimer_IT(&_hrtc, rtc_clock_frequency / 1024 / 2, RTC_WAKEUPCLOCK_RTCCLK_DIV2);
     HAL_NVIC_SetPriority(RTC_WKUP_IRQn, 1, 0U);
     HAL_NVIC_EnableIRQ(RTC_WKUP_IRQn);
 }
@@ -372,7 +386,7 @@ void port_interrupt_after_ticks(uint32_t ticks) {
         alarm.AlarmMask = RTC_ALARMMASK_ALL;
     }
 
-    alarm.AlarmTime.SubSeconds = RTC_CLOCK_FREQUENCY -
+    alarm.AlarmTime.SubSeconds = rtc_clock_frequency -
                                  ((raw_ticks % 1024) * 32);
     alarm.AlarmTime.DayLightSaving = RTC_DAYLIGHTSAVING_NONE;
     alarm.AlarmTime.StoreOperation = RTC_STOREOPERATION_SET;