From b920248693b79d3beb2afb3dcf2fe593b72e9a8b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=81=82=E3=81=8F?= Date: Thu, 11 Mar 2021 09:47:07 +0300 Subject: [PATCH] API HAL OS: replace CMP based ticks with ARR based one, hard reset lptimer on reconfiguration. (#377) --- .../targets/f4/api-hal/api-hal-os-timer.h | 111 ++++++--------- firmware/targets/f4/api-hal/api-hal-os.c | 130 ++++++------------ firmware/targets/f4/target.mk | 2 + .../targets/f5/api-hal/api-hal-os-timer.h | 111 ++++++--------- firmware/targets/f5/api-hal/api-hal-os.c | 130 ++++++------------ firmware/targets/f5/target.mk | 2 + 6 files changed, 172 insertions(+), 314 deletions(-) diff --git a/firmware/targets/f4/api-hal/api-hal-os-timer.h b/firmware/targets/f4/api-hal/api-hal-os-timer.h index a1e775b3..f700e6b6 100644 --- a/firmware/targets/f4/api-hal/api-hal-os-timer.h +++ b/firmware/targets/f4/api-hal/api-hal-os-timer.h @@ -1,42 +1,58 @@ #pragma once #include -#include - -static inline void assert(bool value) { - if (!value) asm("bkpt 1"); -} +#include +#include // 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(); - - LL_LPTIM_Enable(API_HAL_OS_TIMER); - while(!LL_LPTIM_IsEnabled(API_HAL_OS_TIMER)) {} - - 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); - + // Configure clock source + LL_RCC_SetLPTIMClockSource(LL_RCC_LPTIM2_CLKSOURCE_LSE); + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_LPTIM2); + // Set interrupt priority and enable them 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; -} diff --git a/firmware/targets/f4/api-hal/api-hal-os.c b/firmware/targets/f4/api-hal/api-hal-os.c index 1415183e..f6b01e0e 100644 --- a/firmware/targets/f4/api-hal/api-hal-os.c +++ b/firmware/targets/f4/api-hal/api-hal-os.c @@ -13,114 +13,78 @@ #ifdef API_HAL_OS_DEBUG #include -#define LED_GREEN_PORT GPIOA -#define LED_GREEN_PIN LL_GPIO_PIN_2 +#define LED_SLEEP_PORT GPIOA +#define LED_SLEEP_PIN LL_GPIO_PIN_7 +#define LED_TICK_PORT GPIOA +#define LED_TICK_PIN LL_GPIO_PIN_6 #endif -typedef struct { - // 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, -}; +volatile uint32_t api_hal_os_skew = 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); - LL_LPTIM_EnableIT_ARRM(API_HAL_OS_TIMER); + api_hal_os_timer_init(); + api_hal_os_timer_continuous(API_HAL_OS_CLK_PER_TICK); - LL_LPTIM_SetAutoReload(API_HAL_OS_TIMER, API_HAL_OS_TIMER_MAX); - LL_LPTIM_SetCompare(API_HAL_OS_TIMER, API_HAL_OS_CLK_PER_TICK); - - LL_LPTIM_StartCounter(API_HAL_OS_TIMER, LL_LPTIM_OPERATING_MODE_CONTINUOUS); +#ifdef API_HAL_OS_DEBUG + 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); +#endif } void LPTIM2_IRQHandler(void) { // Autoreload - const bool arrm_flag = LL_LPTIM_IsActiveFlag_ARRM(API_HAL_OS_TIMER); - if(arrm_flag) { + if(LL_LPTIM_IsActiveFlag_ARRM(API_HAL_OS_TIMER)) { 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 - const uint16_t before_cnt = api_hal_os_timer_get_cnt(); - 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); - + // Stop ticks + api_hal_os_timer_reset(); 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 - 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; + return after_tick; } 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 - || !api_hal_os_timer_is_safe()) { + if (eTaskConfirmSleepModeStatus() == eAbortSleep) { __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); diff --git a/firmware/targets/f4/target.mk b/firmware/targets/f4/target.mk index 7c063807..63499d17 100644 --- a/firmware/targets/f4/target.mk +++ b/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 \ diff --git a/firmware/targets/f5/api-hal/api-hal-os-timer.h b/firmware/targets/f5/api-hal/api-hal-os-timer.h index a1e775b3..f700e6b6 100644 --- a/firmware/targets/f5/api-hal/api-hal-os-timer.h +++ b/firmware/targets/f5/api-hal/api-hal-os-timer.h @@ -1,42 +1,58 @@ #pragma once #include -#include - -static inline void assert(bool value) { - if (!value) asm("bkpt 1"); -} +#include +#include // 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(); - - LL_LPTIM_Enable(API_HAL_OS_TIMER); - while(!LL_LPTIM_IsEnabled(API_HAL_OS_TIMER)) {} - - 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); - + // Configure clock source + LL_RCC_SetLPTIMClockSource(LL_RCC_LPTIM2_CLKSOURCE_LSE); + LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_LPTIM2); + // Set interrupt priority and enable them 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; -} diff --git a/firmware/targets/f5/api-hal/api-hal-os.c b/firmware/targets/f5/api-hal/api-hal-os.c index dc83bd45..f6b01e0e 100644 --- a/firmware/targets/f5/api-hal/api-hal-os.c +++ b/firmware/targets/f5/api-hal/api-hal-os.c @@ -13,114 +13,78 @@ #ifdef API_HAL_OS_DEBUG #include -#define LED_GREEN_PORT GPIOA -#define LED_GREEN_PIN LL_GPIO_PIN_7 +#define LED_SLEEP_PORT GPIOA +#define LED_SLEEP_PIN LL_GPIO_PIN_7 +#define LED_TICK_PORT GPIOA +#define LED_TICK_PIN LL_GPIO_PIN_6 #endif -typedef struct { - // 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, -}; +volatile uint32_t api_hal_os_skew = 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); - LL_LPTIM_EnableIT_ARRM(API_HAL_OS_TIMER); + api_hal_os_timer_init(); + api_hal_os_timer_continuous(API_HAL_OS_CLK_PER_TICK); - LL_LPTIM_SetAutoReload(API_HAL_OS_TIMER, API_HAL_OS_TIMER_MAX); - LL_LPTIM_SetCompare(API_HAL_OS_TIMER, API_HAL_OS_CLK_PER_TICK); - - LL_LPTIM_StartCounter(API_HAL_OS_TIMER, LL_LPTIM_OPERATING_MODE_CONTINUOUS); +#ifdef API_HAL_OS_DEBUG + 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); +#endif } void LPTIM2_IRQHandler(void) { // Autoreload - const bool arrm_flag = LL_LPTIM_IsActiveFlag_ARRM(API_HAL_OS_TIMER); - if(arrm_flag) { + if(LL_LPTIM_IsActiveFlag_ARRM(API_HAL_OS_TIMER)) { 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 - const uint16_t before_cnt = api_hal_os_timer_get_cnt(); - 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); - + // Stop ticks + api_hal_os_timer_reset(); 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 - 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; + return after_tick; } 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 - || !api_hal_os_timer_is_safe()) { + if (eTaskConfirmSleepModeStatus() == eAbortSleep) { __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); diff --git a/firmware/targets/f5/target.mk b/firmware/targets/f5/target.mk index 7c063807..63499d17 100644 --- a/firmware/targets/f5/target.mk +++ b/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 \