HAL Timebase, Power, Clock: semaphore guarded access to clock and power modes, better sleep mode. (#307)

firmware/targets/api-hal-include/api-hal-power.h

@@ -16,6 +16,9 @@ typedef enum {
 /* Initialize drivers */
 void api_hal_power_init();
 
+/* Go to deep sleep */
+void api_hal_power_deep_sleep();
+
 /* Get predicted remaining battery capacity in percents */
 uint8_t api_hal_power_get_pct();
 

firmware/targets/f4/api-hal/api-hal-clock.c

@@ -0,0 +1,27 @@
+#include <api-hal-clock.h>
+
+#include <stm32wbxx_ll_rcc.h>
+
+void api_hal_clock_switch_to_hsi() {
+    LL_RCC_HSI_Enable( );
+
+    while(!LL_RCC_HSI_IsReady());
+
+    LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_HSI);
+    LL_RCC_SetSMPSClockSource(LL_RCC_SMPS_CLKSOURCE_HSI);
+
+    while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_HSI);
+}
+
+void api_hal_clock_switch_to_pll() {
+    LL_RCC_HSE_Enable();
+    LL_RCC_PLL_Enable();
+
+    while(!LL_RCC_HSE_IsReady());
+    while(!LL_RCC_PLL_IsReady());
+
+    LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL);
+    LL_RCC_SetSMPSClockSource(LL_RCC_SMPS_CLKSOURCE_HSE);
+
+    while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL);
+}

firmware/targets/f4/api-hal/api-hal-clock.h

@@ -0,0 +1,7 @@
+#pragma once
+
+/* Switch to HSI clock */
+void api_hal_clock_switch_to_hsi();
+
+/* Switch to PLL clock */
+void api_hal_clock_switch_to_pll();

firmware/targets/f4/api-hal/api-hal-power.c

@@ -1,5 +1,13 @@
 #include <api-hal-power.h>
+#include <api-hal-clock.h>
+
+#include <stm32wbxx_ll_rcc.h>
+#include <stm32wbxx_ll_pwr.h>
+#include <stm32wbxx_ll_hsem.h>
+#include <stm32wbxx_ll_cortex.h>
+
 #include <main.h>
+#include <hw_conf.h>
 #include <bq27220.h>
 #include <bq25896.h>
 
@@ -14,6 +22,50 @@ void api_hal_power_init() {
     bq25896_init();
 }
 
+void api_hal_power_deep_sleep() {
+  while( LL_HSEM_1StepLock(HSEM, CFG_HW_RCC_SEMID));
+
+  if (!LL_HSEM_1StepLock(HSEM, CFG_HW_ENTRY_STOP_MODE_SEMID)) {
+        if(LL_PWR_IsActiveFlag_C2DS()) {
+            // Release ENTRY_STOP_MODE semaphore
+            LL_HSEM_ReleaseLock(HSEM, CFG_HW_ENTRY_STOP_MODE_SEMID, 0);
+
+            // The switch on HSI before entering Stop Mode is required 
+            api_hal_clock_switch_to_hsi();
+        }
+    } else {
+        /**
+         * The switch on HSI before entering Stop Mode is required 
+         */
+        api_hal_clock_switch_to_hsi();
+    }
+
+    /* Release RCC semaphore */
+    LL_HSEM_ReleaseLock(HSEM, CFG_HW_RCC_SEMID, 0);
+
+    // Prepare deep sleep
+    LL_PWR_SetPowerMode(LL_PWR_MODE_STOP2);
+    LL_LPM_EnableDeepSleep();
+
+#if defined ( __CC_ARM)
+    // Force store operations
+    __force_stores();
+#endif
+
+    __WFI();
+
+    /* Release ENTRY_STOP_MODE semaphore */
+    LL_HSEM_ReleaseLock(HSEM, CFG_HW_ENTRY_STOP_MODE_SEMID, 0);
+
+    while(LL_HSEM_1StepLock(HSEM, CFG_HW_RCC_SEMID));
+
+    if(LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) {
+        api_hal_clock_switch_to_pll();
+    }
+
+    LL_HSEM_ReleaseLock(HSEM, CFG_HW_RCC_SEMID, 0);
+}
+
 uint8_t api_hal_power_get_pct() {
     return bq27220_get_state_of_charge();
 }

firmware/targets/f4/api-hal/api-hal-timebase.c

@@ -1,8 +1,7 @@
 #include <api-hal-timebase.h>
 #include <api-hal-timebase-timer.h>
+#include <api-hal-power.h>
 
-#include <stm32wbxx_hal.h>
-#include <stm32wbxx_ll_gpio.h>
 #include <FreeRTOS.h>
 #include <cmsis_os.h>
 
@@ -88,11 +87,6 @@ void LPTIM2_IRQHandler(void) {
     }
 }
 
-static inline uint32_t api_hal_timebase_nap(TickType_t expected_idle_ticks) {
-    __WFI();
-    return 0;
-}
-
 static inline uint32_t api_hal_timebase_sleep(TickType_t expected_idle_ticks) {
     // Store important value before going to sleep
     const uint16_t before_cnt = api_hal_timebase_timer_get_cnt();
@@ -103,7 +97,7 @@ static inline uint32_t api_hal_timebase_sleep(TickType_t expected_idle_ticks) {
     api_hal_timebase_timer_set_cmp(expected_cnt);
 
     // Go to stop2 mode
-    HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI);
+    api_hal_power_deep_sleep();
 
     // Spin till we are in timer safe zone
     while(!api_hal_timebase_timer_is_safe()) {}
@@ -135,6 +129,9 @@ void vPortSuppressTicksAndSleep(TickType_t expected_idle_ticks) {
         expected_idle_ticks = API_HAL_TIMEBASE_MAX_SLEEP;
     }
 
+    if (api_hal_timebase.insomnia) 
+        return;
+    
     // Stop IRQ handling, no one should disturb us till we finish 
     __disable_irq();
 
@@ -147,12 +144,7 @@ void vPortSuppressTicksAndSleep(TickType_t expected_idle_ticks) {
         return;
     }
 
-    uint32_t completed_ticks;
+    uint32_t completed_ticks = api_hal_timebase_sleep(expected_idle_ticks);
-    if (api_hal_timebase.insomnia) {
-        completed_ticks = api_hal_timebase_nap(expected_idle_ticks);
-    } else {
-        completed_ticks = api_hal_timebase_sleep(expected_idle_ticks);
-    }
     assert(completed_ticks >= 0);
 
     // Reenable IRQ