API HAL OS: replace CMP based ticks with ARR based one, hard reset lptimer on reconfiguration. (#377)
This commit is contained in:
parent
2f14f6dac5
commit
b920248693
@ -1,42 +1,58 @@
|
||||
#pragma once
|
||||
|
||||
#include <stm32wbxx_ll_lptim.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
static inline void assert(bool value) {
|
||||
if (!value) asm("bkpt 1");
|
||||
}
|
||||
#include <stm32wbxx_ll_bus.h>
|
||||
#include <stdint.h>
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
@ -13,114 +13,78 @@
|
||||
|
||||
#ifdef API_HAL_OS_DEBUG
|
||||
#include <stm32wbxx_ll_gpio.h>
|
||||
#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);
|
||||
|
@ -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 \
|
||||
|
@ -1,42 +1,58 @@
|
||||
#pragma once
|
||||
|
||||
#include <stm32wbxx_ll_lptim.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
static inline void assert(bool value) {
|
||||
if (!value) asm("bkpt 1");
|
||||
}
|
||||
#include <stm32wbxx_ll_bus.h>
|
||||
#include <stdint.h>
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
@ -13,114 +13,78 @@
|
||||
|
||||
#ifdef API_HAL_OS_DEBUG
|
||||
#include <stm32wbxx_ll_gpio.h>
|
||||
#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);
|
||||
|
@ -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 \
|
||||
|
Loading…
Reference in New Issue
Block a user