diff --git a/rtos/TARGET_CORTEX/SysTimer.cpp b/rtos/TARGET_CORTEX/SysTimer.cpp index 35efb02620..5ab9b40c01 100644 --- a/rtos/TARGET_CORTEX/SysTimer.cpp +++ b/rtos/TARGET_CORTEX/SysTimer.cpp @@ -25,6 +25,7 @@ #include "hal/lp_ticker_api.h" #include "mbed_critical.h" +#include "mbed_assert.h" #if defined(TARGET_CORTEX_A) #include "rtx_core_ca.h" #else//Cortex-M @@ -37,6 +38,9 @@ extern "C" { #endif } +#define US_IN_TICK (1000000 / OS_TICK_FREQ) +MBED_STATIC_ASSERT(1000000 % OS_TICK_FREQ == 0, "OS_TICK_FREQ must be a divisor of 1000000 for correct tick calculations"); + #if (defined(NO_SYSTICK)) /** * Return an IRQ number that can be used in the absence of SysTick @@ -54,17 +58,17 @@ namespace rtos { namespace internal { SysTimer::SysTimer() : - TimerEvent(get_lp_ticker_data()), _start_time(0), _tick(0) + TimerEvent(get_lp_ticker_data()), _time_us(0), _tick(0) { - _start_time = ticker_read_us(_ticker_data); + _time_us = ticker_read_us(_ticker_data); _suspend_time_passed = true; _suspended = false; } SysTimer::SysTimer(const ticker_data_t *data) : - TimerEvent(data), _start_time(0), _tick(0) + TimerEvent(data), _time_us(0), _tick(0) { - _start_time = ticker_read_us(_ticker_data); + _time_us = ticker_read_us(_ticker_data); } void SysTimer::setup_irq() @@ -106,16 +110,16 @@ uint32_t SysTimer::resume() _suspend_time_passed = true; remove(); - uint64_t new_tick = (ticker_read_us(_ticker_data) - _start_time) * OS_TICK_FREQ / 1000000; - if (new_tick > _tick) { + uint64_t elapsed_ticks = (ticker_read_us(_ticker_data) - _time_us) / US_IN_TICK; + if (elapsed_ticks > 0) { // Don't update to the current tick. Instead, update to the // previous tick and let the SysTick handler increment it // to the current value. This allows scheduling restart // successfully after the OS is resumed. - new_tick--; + elapsed_ticks--; } - uint32_t elapsed_ticks = new_tick - _tick; - _tick = new_tick; + _time_us += elapsed_ticks * US_IN_TICK; + _tick += elapsed_ticks; core_util_critical_section_exit(); return elapsed_ticks; @@ -125,7 +129,7 @@ void SysTimer::schedule_tick(uint32_t delta) { core_util_critical_section_enter(); - insert_absolute(_start_time + (_tick + delta) * 1000000ULL / OS_TICK_FREQ); + insert_absolute(_time_us + delta * US_IN_TICK); core_util_critical_section_exit(); } @@ -171,6 +175,7 @@ void SysTimer::_increment_tick() // Protected function synchronized externally _tick++; + _time_us += US_IN_TICK; } void SysTimer::handler() diff --git a/rtos/TARGET_CORTEX/SysTimer.h b/rtos/TARGET_CORTEX/SysTimer.h index e4c008e317..68b6eec891 100644 --- a/rtos/TARGET_CORTEX/SysTimer.h +++ b/rtos/TARGET_CORTEX/SysTimer.h @@ -117,7 +117,7 @@ protected: virtual void handler(); void _increment_tick(); static void _set_irq_pending(); - us_timestamp_t _start_time; + us_timestamp_t _time_us; uint64_t _tick; bool _suspend_time_passed; bool _suspended;