API HAL OS: replace CMP based ticks with ARR based one, hard reset lptimer on reconfiguration. (#377)

firmware/targets/f4/api-hal/api-hal-os-timer.h

@@ -1,42 +1,58 @@
 #pragma once
 
 #include <stm32wbxx_ll_lptim.h>
-#include <stdbool.h>
+#include <stm32wbxx_ll_bus.h>
-
+#include <stdint.h>
-static inline void assert(bool value) {
-    if (!value) asm("bkpt 1");
-}
 
 // Timer used for system ticks
 #define API_HAL_OS_TIMER_MAX  0xFFFF
 #define API_HAL_OS_TIMER_REG_LOAD_DLY 0x1
 #define API_HAL_OS_TIMER       LPTIM2
 #define API_HAL_OS_TIMER_IRQ   LPTIM2_IRQn
-#define API_HAL_OS_TIMER_CLOCK_INIT() \
-{ \
-    LL_RCC_SetLPTIMClockSource(LL_RCC_LPTIM2_CLKSOURCE_LSE); \
-    LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_LPTIM2); \
-} \
 
 static inline void api_hal_os_timer_init() {
-    API_HAL_OS_TIMER_CLOCK_INIT();
+    // Configure clock source
-
+    LL_RCC_SetLPTIMClockSource(LL_RCC_LPTIM2_CLKSOURCE_LSE);
-    LL_LPTIM_Enable(API_HAL_OS_TIMER);
+    LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_LPTIM2);
-    while(!LL_LPTIM_IsEnabled(API_HAL_OS_TIMER)) {}
+    // Set interrupt priority and enable them
-
-    LL_LPTIM_SetClockSource(API_HAL_OS_TIMER, LL_LPTIM_CLK_SOURCE_INTERNAL);
-    LL_LPTIM_SetPrescaler(API_HAL_OS_TIMER, LL_LPTIM_PRESCALER_DIV1);
-    LL_LPTIM_SetPolarity(API_HAL_OS_TIMER, LL_LPTIM_OUTPUT_POLARITY_REGULAR);
-    LL_LPTIM_SetUpdateMode(API_HAL_OS_TIMER, LL_LPTIM_UPDATE_MODE_IMMEDIATE);
-    LL_LPTIM_SetCounterMode(API_HAL_OS_TIMER, LL_LPTIM_COUNTER_MODE_INTERNAL);
-    LL_LPTIM_TrigSw(API_HAL_OS_TIMER);
-    LL_LPTIM_SetInput1Src(API_HAL_OS_TIMER, LL_LPTIM_INPUT1_SRC_GPIO);
-    LL_LPTIM_SetInput2Src(API_HAL_OS_TIMER, LL_LPTIM_INPUT2_SRC_GPIO);
-
     NVIC_SetPriority(API_HAL_OS_TIMER_IRQ, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 15, 0));
     NVIC_EnableIRQ(API_HAL_OS_TIMER_IRQ);
 }
 
+static inline void api_hal_os_timer_continuous(uint32_t count) {
+    // Enable timer
+    LL_LPTIM_Enable(API_HAL_OS_TIMER);
+    while(!LL_LPTIM_IsEnabled(API_HAL_OS_TIMER));
+
+    // Enable rutoreload match interrupt
+    LL_LPTIM_EnableIT_ARRM(API_HAL_OS_TIMER);
+
+    // Set autoreload and start counter
+    LL_LPTIM_SetAutoReload(API_HAL_OS_TIMER, count);
+    LL_LPTIM_StartCounter(API_HAL_OS_TIMER, LL_LPTIM_OPERATING_MODE_CONTINUOUS);
+}
+
+static inline void api_hal_os_timer_single(uint32_t count) {
+    // Enable timer
+    LL_LPTIM_Enable(API_HAL_OS_TIMER);
+    while(!LL_LPTIM_IsEnabled(API_HAL_OS_TIMER));
+
+    // Enable compare match interrupt
+    LL_LPTIM_EnableIT_CMPM(API_HAL_OS_TIMER);
+
+    // Set compare, autoreload and start counter
+    // Include some marging to workaround ARRM behaviour
+    LL_LPTIM_SetCompare(API_HAL_OS_TIMER, count-3);
+    LL_LPTIM_SetAutoReload(API_HAL_OS_TIMER, count);
+    LL_LPTIM_StartCounter(API_HAL_OS_TIMER, LL_LPTIM_OPERATING_MODE_ONESHOT);
+}
+
+static inline void api_hal_os_timer_reset() {
+    // Hard reset timer
+    // THE ONLY RELIABLEWAY to stop it according to errata
+    LL_LPTIM_DeInit(API_HAL_OS_TIMER);
+}
+
 static inline uint32_t api_hal_os_timer_get_cnt() {
     uint32_t counter = LL_LPTIM_GetCounter(API_HAL_OS_TIMER);
     uint32_t counter_shadow = LL_LPTIM_GetCounter(API_HAL_OS_TIMER);
@@ -46,50 +62,3 @@ static inline uint32_t api_hal_os_timer_get_cnt() {
     }
     return counter;
 }
-
-static inline bool api_hal_os_timer_arr_is_ok() {
-    return LL_LPTIM_IsActiveFlag_ARROK(API_HAL_OS_TIMER);
-}
-
-static inline uint32_t api_hal_os_timer_get_arr() {
-    return LL_LPTIM_GetAutoReload(API_HAL_OS_TIMER);;
-}
-
-static inline void api_hal_os_timer_set_arr(uint32_t value) {
-    value &= API_HAL_OS_TIMER_MAX;
-    if (value != api_hal_os_timer_get_arr()) {
-        assert(api_hal_os_timer_arr_is_ok());
-        LL_LPTIM_ClearFlag_ARROK(API_HAL_OS_TIMER);
-        LL_LPTIM_SetAutoReload(API_HAL_OS_TIMER, value);
-    }
-}
-
-static inline bool api_hal_os_timer_cmp_is_ok() {
-    return LL_LPTIM_IsActiveFlag_CMPOK(API_HAL_OS_TIMER);
-}
-
-static inline uint32_t api_hal_os_timer_get_cmp() {
-    return LL_LPTIM_GetCompare(API_HAL_OS_TIMER);
-}
-
-static inline void api_hal_os_timer_set_cmp(uint32_t value) {
-    value &= API_HAL_OS_TIMER_MAX;
-    if (value != api_hal_os_timer_get_cmp()) {
-        assert(api_hal_os_timer_cmp_is_ok());
-        LL_LPTIM_ClearFlag_CMPOK(API_HAL_OS_TIMER);
-        LL_LPTIM_SetCompare(API_HAL_OS_TIMER, value);
-    }
-}
-
-static inline bool api_hal_os_timer_is_safe() {
-    uint16_t cmp = api_hal_os_timer_get_cmp();
-    uint16_t cnt = api_hal_os_timer_get_cnt();
-    uint16_t margin = (cmp > cnt) ? cmp - cnt : cnt - cmp;
-    if (margin < 8) {
-        return false;
-    }
-    if (!api_hal_os_timer_cmp_is_ok()) {
-        return false;
-    }
-    return true;
-}

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

@@ -13,114 +13,78 @@
 
 #ifdef API_HAL_OS_DEBUG
 #include <stm32wbxx_ll_gpio.h>
-#define LED_GREEN_PORT GPIOA
+#define LED_SLEEP_PORT GPIOA
-#define LED_GREEN_PIN LL_GPIO_PIN_2
+#define LED_SLEEP_PIN LL_GPIO_PIN_7
+#define LED_TICK_PORT GPIOA
+#define LED_TICK_PIN LL_GPIO_PIN_6
 #endif
 
-typedef struct {
+volatile uint32_t api_hal_os_skew = 0;
-    // Tick counters
-    volatile uint32_t in_sleep;
-    volatile uint32_t in_awake;
-    // Error counters
-    volatile uint32_t sleep_error;
-    volatile uint32_t awake_error;
-} ApiHalOs;
-
-ApiHalOs api_hal_os = {
-    .in_sleep = 0,
-    .in_awake = 0,
-    .sleep_error = 0,
-    .awake_error = 0,
-};
 
 void api_hal_os_init() {
-    api_hal_os_timer_init();
     LL_DBGMCU_APB1_GRP2_FreezePeriph(LL_DBGMCU_APB1_GRP2_LPTIM2_STOP);
 
-    LL_LPTIM_EnableIT_CMPM(API_HAL_OS_TIMER);
+    api_hal_os_timer_init();
-    LL_LPTIM_EnableIT_ARRM(API_HAL_OS_TIMER);
+    api_hal_os_timer_continuous(API_HAL_OS_CLK_PER_TICK);
 
-    LL_LPTIM_SetAutoReload(API_HAL_OS_TIMER, API_HAL_OS_TIMER_MAX);
+#ifdef API_HAL_OS_DEBUG
-    LL_LPTIM_SetCompare(API_HAL_OS_TIMER, API_HAL_OS_CLK_PER_TICK);
+    LL_GPIO_SetPinMode(LED_SLEEP_PORT, LED_SLEEP_PIN, LL_GPIO_MODE_OUTPUT);
-
+    LL_GPIO_SetPinMode(LED_TICK_PORT, LED_TICK_PIN, LL_GPIO_MODE_OUTPUT);
-    LL_LPTIM_StartCounter(API_HAL_OS_TIMER, LL_LPTIM_OPERATING_MODE_CONTINUOUS);
+#endif
 }
 
 void LPTIM2_IRQHandler(void) {
     // Autoreload
-    const bool arrm_flag = LL_LPTIM_IsActiveFlag_ARRM(API_HAL_OS_TIMER);
+    if(LL_LPTIM_IsActiveFlag_ARRM(API_HAL_OS_TIMER)) {
-    if(arrm_flag) {
         LL_LPTIM_ClearFLAG_ARRM(API_HAL_OS_TIMER);
+        if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
+            #ifdef API_HAL_OS_DEBUG
+            LL_GPIO_TogglePin(LED_TICK_PORT, LED_TICK_PIN);
+            #endif
+            xPortSysTickHandler();
+        }
     }
     if(LL_LPTIM_IsActiveFlag_CMPM(API_HAL_OS_TIMER)) {
         LL_LPTIM_ClearFLAG_CMPM(API_HAL_OS_TIMER);
-
-        // Store important value
-        uint16_t cnt = api_hal_os_timer_get_cnt();
-        uint16_t cmp = api_hal_os_timer_get_cmp();
-        uint16_t current_tick = cnt / API_HAL_OS_CLK_PER_TICK;
-        uint16_t compare_tick = cmp / API_HAL_OS_CLK_PER_TICK;
-
-        // Calculate error
-        // happens when HAL or other high priority IRQ takes our time
-        int32_t error = (int32_t)compare_tick - current_tick;
-        api_hal_os.awake_error += ((error>0) ? error : -error);
-
-        // Calculate and set next tick 
-        uint16_t next_tick = current_tick + 1;
-        api_hal_os_timer_set_cmp(next_tick * API_HAL_OS_CLK_PER_TICK);
-
-        // Notify OS
-        api_hal_os.in_awake ++;
-        if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
-            xPortSysTickHandler();
-        }
     }
 }
 
 static inline uint32_t api_hal_os_sleep(TickType_t expected_idle_ticks) {
-    // Store important value before going to sleep
+    // Stop ticks
-    const uint16_t before_cnt = api_hal_os_timer_get_cnt();
+    api_hal_os_timer_reset();
-    const uint16_t before_tick = before_cnt / API_HAL_OS_CLK_PER_TICK;
-
-    // Calculate and set next wakeup compare value
-    const uint16_t expected_cnt = (before_tick + expected_idle_ticks - 2) * API_HAL_OS_CLK_PER_TICK;
-    api_hal_os_timer_set_cmp(expected_cnt);
-
     HAL_SuspendTick();
+
+    // Start wakeup timer
+    api_hal_os_timer_single(expected_idle_ticks * API_HAL_OS_CLK_PER_TICK);
+
+#ifdef API_HAL_OS_DEBUG
+    LL_GPIO_SetOutputPin(LED_SLEEP_PORT, LED_SLEEP_PIN);
+#endif
+
     // Go to stop2 mode
-#ifdef API_HAL_OS_DEBUG
-    LL_GPIO_SetOutputPin(LED_GREEN_PORT, LED_GREEN_PIN);
-#endif
     api_hal_power_deep_sleep();
+
 #ifdef API_HAL_OS_DEBUG
-    LL_GPIO_ResetOutputPin(LED_GREEN_PORT, LED_GREEN_PIN);
+    LL_GPIO_ResetOutputPin(LED_SLEEP_PORT, LED_SLEEP_PIN);
 #endif
 
+    // Calculate how much time we spent in the sleep
+    uint32_t after_cnt = api_hal_os_timer_get_cnt() + api_hal_os_skew;
+    uint32_t after_tick = after_cnt / API_HAL_OS_CLK_PER_TICK;
+    api_hal_os_skew = after_cnt % API_HAL_OS_CLK_PER_TICK;
+
+    // Prepare tick timer for new round
+    api_hal_os_timer_reset();
+
+    // Resume ticks
     HAL_ResumeTick();
+    api_hal_os_timer_continuous(API_HAL_OS_CLK_PER_TICK);
 
-    // Spin till we are in timer safe zone
+    return after_tick;
-    while(!api_hal_os_timer_is_safe()) {}
-
-    // Store current counter value, calculate current tick
-    const uint16_t after_cnt = api_hal_os_timer_get_cnt();
-    const uint16_t after_tick = after_cnt / API_HAL_OS_CLK_PER_TICK;
-
-    // Store and clear interrupt flags
-    // we don't want handler to be called after renabling IRQ
-    bool arrm_flag = LL_LPTIM_IsActiveFlag_ARRM(API_HAL_OS_TIMER);
-
-    // Calculate and set next wakeup compare value
-    const uint16_t next_cmp = (after_tick + 1) * API_HAL_OS_CLK_PER_TICK;
-    api_hal_os_timer_set_cmp(next_cmp);
-
-    // Calculate ticks count spent in sleep and perform sanity checks
-    int32_t completed_ticks = arrm_flag ? (int32_t)before_tick - after_tick : (int32_t)after_tick - before_tick;
-
-    return completed_ticks;
 }
 
 void vPortSuppressTicksAndSleep(TickType_t expected_idle_ticks) {
+    // Check if sleep is available now
     if (!api_hal_power_deep_available()) {
         return;
     }
@@ -136,26 +100,20 @@ void vPortSuppressTicksAndSleep(TickType_t expected_idle_ticks) {
     // Confirm OS that sleep is still possible
     // And check if timer is in safe zone
     // (8 clocks till any IRQ event or ongoing synchronization)
-    if (eTaskConfirmSleepModeStatus() == eAbortSleep
+    if (eTaskConfirmSleepModeStatus() == eAbortSleep) {
-        || !api_hal_os_timer_is_safe()) {
         __enable_irq();
         return;
     }
 
+    // Sleep and track how much ticks we spent sleeping
     uint32_t completed_ticks = api_hal_os_sleep(expected_idle_ticks);
-    assert(completed_ticks >= 0);
 
     // Reenable IRQ
     __enable_irq();
 
     // Notify system about time spent in sleep
     if (completed_ticks > 0) {
-        api_hal_os.in_sleep += completed_ticks;
         if (completed_ticks > expected_idle_ticks) {
-            // We are late, count error
-            api_hal_os.sleep_error += (completed_ticks - expected_idle_ticks);
-            // Freertos is not happy when we overleep
-            // But we are not going to tell her
             vTaskStepTick(expected_idle_ticks);
         } else {
             vTaskStepTick(completed_ticks);

firmware/targets/f4/target.mk

@@ -74,6 +74,8 @@ C_SOURCES		+= \
 	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_adc.c \
 	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_gpio.c \
 	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_i2c.c \
+	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_rcc.c \
+	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_lptim.c \
 	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_usb.c \
 	$(CUBE_DIR)/Middlewares/Third_Party/FreeRTOS/Source/croutine.c \
 	$(CUBE_DIR)/Middlewares/Third_Party/FreeRTOS/Source/event_groups.c \

firmware/targets/f5/api-hal/api-hal-os-timer.h

@@ -1,42 +1,58 @@
 #pragma once
 
 #include <stm32wbxx_ll_lptim.h>
-#include <stdbool.h>
+#include <stm32wbxx_ll_bus.h>
-
+#include <stdint.h>
-static inline void assert(bool value) {
-    if (!value) asm("bkpt 1");
-}
 
 // Timer used for system ticks
 #define API_HAL_OS_TIMER_MAX  0xFFFF
 #define API_HAL_OS_TIMER_REG_LOAD_DLY 0x1
 #define API_HAL_OS_TIMER       LPTIM2
 #define API_HAL_OS_TIMER_IRQ   LPTIM2_IRQn
-#define API_HAL_OS_TIMER_CLOCK_INIT() \
-{ \
-    LL_RCC_SetLPTIMClockSource(LL_RCC_LPTIM2_CLKSOURCE_LSE); \
-    LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_LPTIM2); \
-} \
 
 static inline void api_hal_os_timer_init() {
-    API_HAL_OS_TIMER_CLOCK_INIT();
+    // Configure clock source
-
+    LL_RCC_SetLPTIMClockSource(LL_RCC_LPTIM2_CLKSOURCE_LSE);
-    LL_LPTIM_Enable(API_HAL_OS_TIMER);
+    LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_LPTIM2);
-    while(!LL_LPTIM_IsEnabled(API_HAL_OS_TIMER)) {}
+    // Set interrupt priority and enable them
-
-    LL_LPTIM_SetClockSource(API_HAL_OS_TIMER, LL_LPTIM_CLK_SOURCE_INTERNAL);
-    LL_LPTIM_SetPrescaler(API_HAL_OS_TIMER, LL_LPTIM_PRESCALER_DIV1);
-    LL_LPTIM_SetPolarity(API_HAL_OS_TIMER, LL_LPTIM_OUTPUT_POLARITY_REGULAR);
-    LL_LPTIM_SetUpdateMode(API_HAL_OS_TIMER, LL_LPTIM_UPDATE_MODE_IMMEDIATE);
-    LL_LPTIM_SetCounterMode(API_HAL_OS_TIMER, LL_LPTIM_COUNTER_MODE_INTERNAL);
-    LL_LPTIM_TrigSw(API_HAL_OS_TIMER);
-    LL_LPTIM_SetInput1Src(API_HAL_OS_TIMER, LL_LPTIM_INPUT1_SRC_GPIO);
-    LL_LPTIM_SetInput2Src(API_HAL_OS_TIMER, LL_LPTIM_INPUT2_SRC_GPIO);
-
     NVIC_SetPriority(API_HAL_OS_TIMER_IRQ, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 15, 0));
     NVIC_EnableIRQ(API_HAL_OS_TIMER_IRQ);
 }
 
+static inline void api_hal_os_timer_continuous(uint32_t count) {
+    // Enable timer
+    LL_LPTIM_Enable(API_HAL_OS_TIMER);
+    while(!LL_LPTIM_IsEnabled(API_HAL_OS_TIMER));
+
+    // Enable rutoreload match interrupt
+    LL_LPTIM_EnableIT_ARRM(API_HAL_OS_TIMER);
+
+    // Set autoreload and start counter
+    LL_LPTIM_SetAutoReload(API_HAL_OS_TIMER, count);
+    LL_LPTIM_StartCounter(API_HAL_OS_TIMER, LL_LPTIM_OPERATING_MODE_CONTINUOUS);
+}
+
+static inline void api_hal_os_timer_single(uint32_t count) {
+    // Enable timer
+    LL_LPTIM_Enable(API_HAL_OS_TIMER);
+    while(!LL_LPTIM_IsEnabled(API_HAL_OS_TIMER));
+
+    // Enable compare match interrupt
+    LL_LPTIM_EnableIT_CMPM(API_HAL_OS_TIMER);
+
+    // Set compare, autoreload and start counter
+    // Include some marging to workaround ARRM behaviour
+    LL_LPTIM_SetCompare(API_HAL_OS_TIMER, count-3);
+    LL_LPTIM_SetAutoReload(API_HAL_OS_TIMER, count);
+    LL_LPTIM_StartCounter(API_HAL_OS_TIMER, LL_LPTIM_OPERATING_MODE_ONESHOT);
+}
+
+static inline void api_hal_os_timer_reset() {
+    // Hard reset timer
+    // THE ONLY RELIABLEWAY to stop it according to errata
+    LL_LPTIM_DeInit(API_HAL_OS_TIMER);
+}
+
 static inline uint32_t api_hal_os_timer_get_cnt() {
     uint32_t counter = LL_LPTIM_GetCounter(API_HAL_OS_TIMER);
     uint32_t counter_shadow = LL_LPTIM_GetCounter(API_HAL_OS_TIMER);
@@ -46,50 +62,3 @@ static inline uint32_t api_hal_os_timer_get_cnt() {
     }
     return counter;
 }
-
-static inline bool api_hal_os_timer_arr_is_ok() {
-    return LL_LPTIM_IsActiveFlag_ARROK(API_HAL_OS_TIMER);
-}
-
-static inline uint32_t api_hal_os_timer_get_arr() {
-    return LL_LPTIM_GetAutoReload(API_HAL_OS_TIMER);;
-}
-
-static inline void api_hal_os_timer_set_arr(uint32_t value) {
-    value &= API_HAL_OS_TIMER_MAX;
-    if (value != api_hal_os_timer_get_arr()) {
-        assert(api_hal_os_timer_arr_is_ok());
-        LL_LPTIM_ClearFlag_ARROK(API_HAL_OS_TIMER);
-        LL_LPTIM_SetAutoReload(API_HAL_OS_TIMER, value);
-    }
-}
-
-static inline bool api_hal_os_timer_cmp_is_ok() {
-    return LL_LPTIM_IsActiveFlag_CMPOK(API_HAL_OS_TIMER);
-}
-
-static inline uint32_t api_hal_os_timer_get_cmp() {
-    return LL_LPTIM_GetCompare(API_HAL_OS_TIMER);
-}
-
-static inline void api_hal_os_timer_set_cmp(uint32_t value) {
-    value &= API_HAL_OS_TIMER_MAX;
-    if (value != api_hal_os_timer_get_cmp()) {
-        assert(api_hal_os_timer_cmp_is_ok());
-        LL_LPTIM_ClearFlag_CMPOK(API_HAL_OS_TIMER);
-        LL_LPTIM_SetCompare(API_HAL_OS_TIMER, value);
-    }
-}
-
-static inline bool api_hal_os_timer_is_safe() {
-    uint16_t cmp = api_hal_os_timer_get_cmp();
-    uint16_t cnt = api_hal_os_timer_get_cnt();
-    uint16_t margin = (cmp > cnt) ? cmp - cnt : cnt - cmp;
-    if (margin < 8) {
-        return false;
-    }
-    if (!api_hal_os_timer_cmp_is_ok()) {
-        return false;
-    }
-    return true;
-}

firmware/targets/f5/api-hal/api-hal-os.c

@@ -13,114 +13,78 @@
 
 #ifdef API_HAL_OS_DEBUG
 #include <stm32wbxx_ll_gpio.h>
-#define LED_GREEN_PORT GPIOA
+#define LED_SLEEP_PORT GPIOA
-#define LED_GREEN_PIN LL_GPIO_PIN_7
+#define LED_SLEEP_PIN LL_GPIO_PIN_7
+#define LED_TICK_PORT GPIOA
+#define LED_TICK_PIN LL_GPIO_PIN_6
 #endif
 
-typedef struct {
+volatile uint32_t api_hal_os_skew = 0;
-    // Tick counters
-    volatile uint32_t in_sleep;
-    volatile uint32_t in_awake;
-    // Error counters
-    volatile uint32_t sleep_error;
-    volatile uint32_t awake_error;
-} ApiHalOs;
-
-ApiHalOs api_hal_os = {
-    .in_sleep = 0,
-    .in_awake = 0,
-    .sleep_error = 0,
-    .awake_error = 0,
-};
 
 void api_hal_os_init() {
-    api_hal_os_timer_init();
     LL_DBGMCU_APB1_GRP2_FreezePeriph(LL_DBGMCU_APB1_GRP2_LPTIM2_STOP);
 
-    LL_LPTIM_EnableIT_CMPM(API_HAL_OS_TIMER);
+    api_hal_os_timer_init();
-    LL_LPTIM_EnableIT_ARRM(API_HAL_OS_TIMER);
+    api_hal_os_timer_continuous(API_HAL_OS_CLK_PER_TICK);
 
-    LL_LPTIM_SetAutoReload(API_HAL_OS_TIMER, API_HAL_OS_TIMER_MAX);
+#ifdef API_HAL_OS_DEBUG
-    LL_LPTIM_SetCompare(API_HAL_OS_TIMER, API_HAL_OS_CLK_PER_TICK);
+    LL_GPIO_SetPinMode(LED_SLEEP_PORT, LED_SLEEP_PIN, LL_GPIO_MODE_OUTPUT);
-
+    LL_GPIO_SetPinMode(LED_TICK_PORT, LED_TICK_PIN, LL_GPIO_MODE_OUTPUT);
-    LL_LPTIM_StartCounter(API_HAL_OS_TIMER, LL_LPTIM_OPERATING_MODE_CONTINUOUS);
+#endif
 }
 
 void LPTIM2_IRQHandler(void) {
     // Autoreload
-    const bool arrm_flag = LL_LPTIM_IsActiveFlag_ARRM(API_HAL_OS_TIMER);
+    if(LL_LPTIM_IsActiveFlag_ARRM(API_HAL_OS_TIMER)) {
-    if(arrm_flag) {
         LL_LPTIM_ClearFLAG_ARRM(API_HAL_OS_TIMER);
+        if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
+            #ifdef API_HAL_OS_DEBUG
+            LL_GPIO_TogglePin(LED_TICK_PORT, LED_TICK_PIN);
+            #endif
+            xPortSysTickHandler();
+        }
     }
     if(LL_LPTIM_IsActiveFlag_CMPM(API_HAL_OS_TIMER)) {
         LL_LPTIM_ClearFLAG_CMPM(API_HAL_OS_TIMER);
-
-        // Store important value
-        uint16_t cnt = api_hal_os_timer_get_cnt();
-        uint16_t cmp = api_hal_os_timer_get_cmp();
-        uint16_t current_tick = cnt / API_HAL_OS_CLK_PER_TICK;
-        uint16_t compare_tick = cmp / API_HAL_OS_CLK_PER_TICK;
-
-        // Calculate error
-        // happens when HAL or other high priority IRQ takes our time
-        int32_t error = (int32_t)compare_tick - current_tick;
-        api_hal_os.awake_error += ((error>0) ? error : -error);
-
-        // Calculate and set next tick 
-        uint16_t next_tick = current_tick + 1;
-        api_hal_os_timer_set_cmp(next_tick * API_HAL_OS_CLK_PER_TICK);
-
-        // Notify OS
-        api_hal_os.in_awake ++;
-        if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
-            xPortSysTickHandler();
-        }
     }
 }
 
 static inline uint32_t api_hal_os_sleep(TickType_t expected_idle_ticks) {
-    // Store important value before going to sleep
+    // Stop ticks
-    const uint16_t before_cnt = api_hal_os_timer_get_cnt();
+    api_hal_os_timer_reset();
-    const uint16_t before_tick = before_cnt / API_HAL_OS_CLK_PER_TICK;
-
-    // Calculate and set next wakeup compare value
-    const uint16_t expected_cnt = (before_tick + expected_idle_ticks - 2) * API_HAL_OS_CLK_PER_TICK;
-    api_hal_os_timer_set_cmp(expected_cnt);
-
     HAL_SuspendTick();
+
+    // Start wakeup timer
+    api_hal_os_timer_single(expected_idle_ticks * API_HAL_OS_CLK_PER_TICK);
+
+#ifdef API_HAL_OS_DEBUG
+    LL_GPIO_SetOutputPin(LED_SLEEP_PORT, LED_SLEEP_PIN);
+#endif
+
     // Go to stop2 mode
-#ifdef API_HAL_OS_DEBUG
-    LL_GPIO_SetOutputPin(LED_GREEN_PORT, LED_GREEN_PIN);
-#endif
     api_hal_power_deep_sleep();
+
 #ifdef API_HAL_OS_DEBUG
-    LL_GPIO_ResetOutputPin(LED_GREEN_PORT, LED_GREEN_PIN);
+    LL_GPIO_ResetOutputPin(LED_SLEEP_PORT, LED_SLEEP_PIN);
 #endif
 
+    // Calculate how much time we spent in the sleep
+    uint32_t after_cnt = api_hal_os_timer_get_cnt() + api_hal_os_skew;
+    uint32_t after_tick = after_cnt / API_HAL_OS_CLK_PER_TICK;
+    api_hal_os_skew = after_cnt % API_HAL_OS_CLK_PER_TICK;
+
+    // Prepare tick timer for new round
+    api_hal_os_timer_reset();
+
+    // Resume ticks
     HAL_ResumeTick();
+    api_hal_os_timer_continuous(API_HAL_OS_CLK_PER_TICK);
 
-    // Spin till we are in timer safe zone
+    return after_tick;
-    while(!api_hal_os_timer_is_safe()) {}
-
-    // Store current counter value, calculate current tick
-    const uint16_t after_cnt = api_hal_os_timer_get_cnt();
-    const uint16_t after_tick = after_cnt / API_HAL_OS_CLK_PER_TICK;
-
-    // Store and clear interrupt flags
-    // we don't want handler to be called after renabling IRQ
-    bool arrm_flag = LL_LPTIM_IsActiveFlag_ARRM(API_HAL_OS_TIMER);
-
-    // Calculate and set next wakeup compare value
-    const uint16_t next_cmp = (after_tick + 1) * API_HAL_OS_CLK_PER_TICK;
-    api_hal_os_timer_set_cmp(next_cmp);
-
-    // Calculate ticks count spent in sleep and perform sanity checks
-    int32_t completed_ticks = arrm_flag ? (int32_t)before_tick - after_tick : (int32_t)after_tick - before_tick;
-
-    return completed_ticks;
 }
 
 void vPortSuppressTicksAndSleep(TickType_t expected_idle_ticks) {
+    // Check if sleep is available now
     if (!api_hal_power_deep_available()) {
         return;
     }
@@ -136,26 +100,20 @@ void vPortSuppressTicksAndSleep(TickType_t expected_idle_ticks) {
     // Confirm OS that sleep is still possible
     // And check if timer is in safe zone
     // (8 clocks till any IRQ event or ongoing synchronization)
-    if (eTaskConfirmSleepModeStatus() == eAbortSleep
+    if (eTaskConfirmSleepModeStatus() == eAbortSleep) {
-        || !api_hal_os_timer_is_safe()) {
         __enable_irq();
         return;
     }
 
+    // Sleep and track how much ticks we spent sleeping
     uint32_t completed_ticks = api_hal_os_sleep(expected_idle_ticks);
-    assert(completed_ticks >= 0);
 
     // Reenable IRQ
     __enable_irq();
 
     // Notify system about time spent in sleep
     if (completed_ticks > 0) {
-        api_hal_os.in_sleep += completed_ticks;
         if (completed_ticks > expected_idle_ticks) {
-            // We are late, count error
-            api_hal_os.sleep_error += (completed_ticks - expected_idle_ticks);
-            // Freertos is not happy when we overleep
-            // But we are not going to tell her
             vTaskStepTick(expected_idle_ticks);
         } else {
             vTaskStepTick(completed_ticks);

firmware/targets/f5/target.mk

@@ -74,6 +74,8 @@ C_SOURCES		+= \
 	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_adc.c \
 	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_gpio.c \
 	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_i2c.c \
+	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_rcc.c \
+	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_lptim.c \
 	$(CUBE_DIR)/Drivers/STM32WBxx_HAL_Driver/Src/stm32wbxx_ll_usb.c \
 	$(CUBE_DIR)/Middlewares/Third_Party/FreeRTOS/Source/croutine.c \
 	$(CUBE_DIR)/Middlewares/Third_Party/FreeRTOS/Source/event_groups.c \