From c426cac11e2350808bff0a4bc1b92216080bf2d1 Mon Sep 17 00:00:00 2001 From: diggit Date: Sat, 11 Apr 2020 15:09:54 +0200 Subject: [PATCH 1/3] Move all ticking functionality into ar_port This change allows to utilize HW timers more easily. (default ar_port.cpp using systick needs updating) --- src/ar_internal.h | 11 ++++---- src/ar_kernel.cpp | 67 ++++++++++------------------------------------ src/ar_runloop.cpp | 8 +++--- src/ar_thread.cpp | 4 +-- src/ar_timer.cpp | 10 +++---- 5 files changed, 31 insertions(+), 69 deletions(-) diff --git a/src/ar_internal.h b/src/ar_internal.h index 480fed4..69a7f19 100644 --- a/src/ar_internal.h +++ b/src/ar_internal.h @@ -138,8 +138,6 @@ typedef struct _ar_kernel { uint32_t version; //!< Argon version in BCD, same as #AR_VERSION. ar_deferred_action_queue_t deferredActions; //!< Actions deferred from interrupt context. volatile int32_t lockCount; //!< Whether the kernel is locked. - volatile uint32_t tickCount; //!< Current tick count. - int32_t missedTickCount; //!< Number of ticks that occurred while the kernel was locked. uint32_t nextWakeup; //!< Time of the next wakeup event. uint32_t threadIdCounter; //!< Counter for generating unique thread IDs. #if AR_ENABLE_SYSTEM_LOAD @@ -175,8 +173,11 @@ extern ar_kernel_t g_ar; //@{ void ar_port_init_system(); void ar_port_init_tick_timer(); -void ar_port_set_timer_delay(bool enable, uint32_t delay_us); -uint32_t ar_port_get_timer_elapsed_us(); +uint32_t ar_port_set_time_delay(bool enable, uint32_t delay_us); +uint16_t ar_port_get_time_since_alignment_us(); +uint32_t ar_port_get_time_absolute_ticks(); +uint64_t ar_port_get_time_absolute_us(); +uint32_t ar_port_get_time_absolute_ms(); void ar_port_prepare_stack(ar_thread_t * thread, uint32_t stackSize, void * param); void ar_port_service_call(); bool ar_port_get_irq_state(); @@ -184,7 +185,7 @@ bool ar_port_get_irq_state(); //! @name Kernel internals //@{ -bool ar_kernel_increment_tick_count(unsigned ticks); +bool ar_kernel_process_ticks(); void ar_kernel_enter_scheduler(); void ar_kernel_run_deferred_actions(); void ar_kernel_scheduler(); diff --git a/src/ar_kernel.cpp b/src/ar_kernel.cpp index 5ce47fc..4917ab6 100644 --- a/src/ar_kernel.cpp +++ b/src/ar_kernel.cpp @@ -104,7 +104,7 @@ static void idle_entry(void * param) { while (1) { - while (g_ar.tickCount < g_ar.nextWakeup) + while (ar_get_tick_count() < g_ar.nextWakeup) { } @@ -207,22 +207,12 @@ void ar_kernel_periodic_timer_isr() // as the kernel gets unlocked. if (g_ar.lockCount) { - ar_atomic_add32(&g_ar.missedTickCount, 1); - g_ar.flags.needsReschedule = true; return; } -#if AR_ENABLE_TICKLESS_IDLE - // Get the elapsed time since the last timer tick. - uint32_t us = ar_port_get_timer_elapsed_us(); - uint32_t elapsed_ticks = us / (kSchedulerQuanta_ms * 1000); -#else // AR_ENABLE_TICKLESS_IDLE - uint32_t elapsed_ticks = 1; -#endif // AR_ENABLE_TICKLESS_IDLE - // Process elapsed time. Invoke the scheduler if any threads were woken or if // round robin scheduling is in effect. - if (ar_kernel_increment_tick_count(elapsed_ticks) || g_ar.flags.needsRoundRobin) + if (ar_kernel_process_ticks() || g_ar.flags.needsRoundRobin) { ar_port_service_call(); } @@ -243,28 +233,10 @@ uint32_t ar_kernel_yield_isr(uint32_t topOfStack) g_ar.currentThread->m_stackPointer = reinterpret_cast(topOfStack); } - // Handle any missed ticks. - { - uint32_t missedTicks = g_ar.missedTickCount; - while (!ar_atomic_cas32(&g_ar.missedTickCount, missedTicks, 0)) - { - missedTicks = g_ar.missedTickCount; - } - - if (missedTicks) - { - ar_kernel_increment_tick_count(missedTicks); - } - } - // Process any deferred actions. ar_kernel_run_deferred_actions(); -#if AR_ENABLE_TICKLESS_IDLE - // Process elapsed time to keep tick count up to date. - uint32_t elapsed_ticks = ar_port_get_timer_elapsed_us() / (kSchedulerQuanta_ms * 1000); - ar_kernel_increment_tick_count(elapsed_ticks); -#endif // AR_ENABLE_TICKLESS_IDLE + ar_kernel_process_ticks(); // Run the scheduler. It will modify g_ar.currentThread if switching threads. ar_kernel_scheduler(); @@ -285,15 +257,12 @@ uint32_t ar_kernel_yield_isr(uint32_t topOfStack) //! and must be at least 1, but may be higher if interrupts are disabled for a //! long time. //! @return Flag indicating whether any threads were modified. -bool ar_kernel_increment_tick_count(unsigned ticks) +bool ar_kernel_process_ticks() { // assert(ticks > 0); - // Increment tick count. - g_ar.tickCount += ticks; - // Compare against next wakeup time we previously computed. - if (g_ar.tickCount < g_ar.nextWakeup) + if (ar_get_tick_count() < g_ar.nextWakeup) { return false; } @@ -309,7 +278,8 @@ bool ar_kernel_increment_tick_count(unsigned ticks) ar_list_node_t * next = node->m_next; // Is it time to wake this thread? - if (g_ar.tickCount >= thread->m_wakeupTime) + //TODO: would it be safe to cache tick_count? + if (ar_get_tick_count() >= thread->m_wakeupTime) { wasThreadWoken = true; @@ -524,12 +494,12 @@ void ar_kernel_scheduler() { g_ar.nextWakeup = wakeup; uint32_t delay = 0; - if (g_ar.nextWakeup && g_ar.nextWakeup > g_ar.tickCount) + if (g_ar.nextWakeup && g_ar.nextWakeup > ar_get_tick_count()) { - delay = (g_ar.nextWakeup - g_ar.tickCount) * kSchedulerQuanta_ms * 1000; + delay = (g_ar.nextWakeup - ar_get_tick_count()) * kSchedulerQuanta_ms * 1000; } bool enable = (g_ar.nextWakeup != 0); - ar_port_set_timer_delay(enable, delay); + ar_port_set_time_delay(enable, delay); } #endif // AR_ENABLE_TICKLESS_IDLE } @@ -572,7 +542,7 @@ uint32_t ar_kernel_get_next_wakeup_time() if (g_ar.flags.needsRoundRobin) { // No need to check sleeping threads! - return g_ar.tickCount + 1; + return ar_get_tick_count() + 1; } // Check for a sleeping thread. The sleeping list is sorted by wakeup time, so we only @@ -610,23 +580,14 @@ uint32_t ar_get_system_load(void) // See ar_kernel.h for documentation of this function. uint32_t ar_get_tick_count(void) { -#if AR_ENABLE_TICKLESS_IDLE - uint32_t elapsed_ticks = ar_port_get_timer_elapsed_us() / (kSchedulerQuanta_ms * 1000); - return g_ar.tickCount + elapsed_ticks; -#else - return g_ar.tickCount; -#endif // AR_ENABLE_TICKLESS_IDLE + return ar_port_get_time_absolute_ticks(); } // See ar_kernel.h for documentation of this function. uint32_t ar_get_millisecond_count(void) { -#if AR_ENABLE_TICKLESS_IDLE - uint32_t elapsed_ms = ar_port_get_timer_elapsed_us() / 1000; - return g_ar.tickCount * ar_get_milliseconds_per_tick() + elapsed_ms; -#else - return g_ar.tickCount * ar_get_milliseconds_per_tick(); -#endif // AR_ENABLE_TICKLESS_IDLE + return ar_port_get_time_absolute_ms(); + // return ar_port_get_time_absolute_ticks() * kSchedulerQuanta_ms; //do we have to limit resolution? } //! Updates the list node's links and those of @a node so that the object is inserted before diff --git a/src/ar_runloop.cpp b/src/ar_runloop.cpp index f0851ac..569fb2a 100644 --- a/src/ar_runloop.cpp +++ b/src/ar_runloop.cpp @@ -132,7 +132,7 @@ ar_status_t ar_runloop_run(ar_runloop_t * runloop, uint32_t timeout, ar_runloop_ } // Prepare timeout. - uint32_t startTime = g_ar.tickCount; + uint32_t startTime = ar_get_tick_count(); uint32_t timeoutTicks = timeout; if (timeout != kArInfiniteTimeout) { @@ -197,7 +197,7 @@ ar_status_t ar_runloop_run(ar_runloop_t * runloop, uint32_t timeout, ar_runloop_ uint32_t blockTimeoutTicks = timeoutTicks; if (blockTimeoutTicks != kArInfiniteTimeout) { - uint32_t deltaTicks = g_ar.tickCount - startTime; + uint32_t deltaTicks = ar_get_tick_count() - startTime; if (deltaTicks >= timeoutTicks) { // Timed out, exit runloop. @@ -213,9 +213,9 @@ ar_status_t ar_runloop_run(ar_runloop_t * runloop, uint32_t timeout, ar_runloop_ ar_timer_t * timer = runloop->m_timers.getHead(); // Timers should always have a wakeup time in the future. - assert (timer->m_wakeupTime >= g_ar.tickCount); + assert (timer->m_wakeupTime >= ar_get_tick_count()); - uint32_t wakeupDeltaTicks = timer->m_wakeupTime - g_ar.tickCount; + uint32_t wakeupDeltaTicks = timer->m_wakeupTime - ar_get_tick_count(); // kArInfiniteTimeout is the max 32-bit value, so wakeupDeltaTicks will always be <= if (wakeupDeltaTicks < blockTimeoutTicks) { diff --git a/src/ar_thread.cpp b/src/ar_thread.cpp index db7909f..eb54bd6 100644 --- a/src/ar_thread.cpp +++ b/src/ar_thread.cpp @@ -415,7 +415,7 @@ void ar_thread_sleep(uint32_t milliseconds) } else { - ar_thread_sleep_until(ar_ticks_to_milliseconds(g_ar.tickCount) + milliseconds); + ar_thread_sleep_until(ar_ticks_to_milliseconds(ar_get_tick_count()) + milliseconds); } } @@ -541,7 +541,7 @@ void _ar_thread::block(ar_list_t & blockedList, uint32_t timeout) // If a valid timeout was given, put the thread on the sleeping list. if (timeout != kArInfiniteTimeout) { - m_wakeupTime = g_ar.tickCount + ar_milliseconds_to_ticks(timeout); + m_wakeupTime = ar_get_tick_count() + ar_milliseconds_to_ticks(timeout); g_ar.sleepingList.add(this); } else diff --git a/src/ar_timer.cpp b/src/ar_timer.cpp index a415124..071fc94 100644 --- a/src/ar_timer.cpp +++ b/src/ar_timer.cpp @@ -144,7 +144,7 @@ ar_status_t ar_timer_start(ar_timer_t * timer) // The callback should have been verified by the create function. assert(timer->m_callback); - uint32_t wakeupTime = g_ar.tickCount + timer->m_delay; + uint32_t wakeupTime = ar_get_tick_count() + timer->m_delay; // Handle irq state by deferring the operation. if (ar_port_get_irq_state()) @@ -243,7 +243,7 @@ void ar_kernel_run_timers(ar_list_t & timersList) assert(timer); // Exit loop if all remaining timers on the list wake up in the future. - if (timer->m_wakeupTime > g_ar.tickCount) + if (timer->m_wakeupTime > ar_get_tick_count()) { break; } @@ -269,16 +269,16 @@ void ar_kernel_run_timers(ar_list_t & timersList) // Restart a periodic timer without introducing (much) jitter. Also handle // the cases where the timer callback ran longer than the next wakeup. uint32_t wakeupTime = timer->m_wakeupTime + timer->m_delay; - if (wakeupTime == g_ar.tickCount) + if (wakeupTime == ar_get_tick_count()) { // Push the wakeup out another period into the future. wakeupTime += timer->m_delay; } - else if (wakeupTime < g_ar.tickCount) + else if (wakeupTime < ar_get_tick_count()) { // Compute the delay to the next wakeup in the future that is aligned // to the timer's period. - uint32_t delta = (g_ar.tickCount - timer->m_wakeupTime + timer->m_delay - 1) + uint32_t delta = (ar_get_tick_count() - timer->m_wakeupTime + timer->m_delay - 1) / timer->m_delay * timer->m_delay; wakeupTime = timer->m_wakeupTime + delta; } From 060ce54972ea9d01a7ff5e4433297ea2fd92726a Mon Sep 17 00:00:00 2001 From: diggit Date: Sat, 11 Apr 2020 15:21:42 +0200 Subject: [PATCH 2/3] add ar_port examples for STM32F303 HW timers *Single* uses only one timer as microsecond counter between ticks. *Chained* uses two chained timer s (on hardware level) to count microseconds between tics and ticks themselves. Most precise timing with this solution. --- src/cortex_m/ar_port_f303_chained.cpp | 444 ++++++++++++++++++++++ src/cortex_m/ar_port_f303_single.cpp | 509 ++++++++++++++++++++++++++ 2 files changed, 953 insertions(+) create mode 100644 src/cortex_m/ar_port_f303_chained.cpp create mode 100644 src/cortex_m/ar_port_f303_single.cpp diff --git a/src/cortex_m/ar_port_f303_chained.cpp b/src/cortex_m/ar_port_f303_chained.cpp new file mode 100644 index 0000000..35de435 --- /dev/null +++ b/src/cortex_m/ar_port_f303_chained.cpp @@ -0,0 +1,444 @@ +/* + * Copyright (c) 2013-2015 Immo Software + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * o Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * o Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * o Neither the name of the copyright holder nor the names of its contributors may + * be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include "../ar_internal.h" +#include "ar_port.h" +#include +#include +#include + +#include "ar_config.h" + +using namespace Ar; + +//------------------------------------------------------------------------------ +// Definitions +//------------------------------------------------------------------------------ + +//! Initial thread register values. +enum +{ + kInitialxPSR = 0x01000000u, //!< Set T bit. + kInitialLR = 0u //!< Set to 0 to stop stack crawl by debugger. +}; + +//! @brief Priorities for kernel exceptions. +enum _exception_priorities +{ + //! All handlers use the same, lowest priority. + kHandlerPriority = 0xff +}; + +//uTIM counts microseconds between system ticks, overflow increments sTIM +#define F_uTIM F_TIM3 +#define uTIM TIM3 +#define uTIM_IRQn TIM3_IRQn +#define sTIM_MAX UINT16_MAX + +//sTIM counts system ticks +#define F_sTIM F_TIM2 +#define sTIM TIM2 +#define sTIM_IRQn TIM2_IRQn +#define sTIM_IRQHandler TIM2_IRQHandler +#define sTIM_MAX UINT32_MAX + + +//------------------------------------------------------------------------------ +// Prototypes +//------------------------------------------------------------------------------ + +extern "C" void SysTick_Handler(void); +extern "C" uint32_t ar_port_yield_isr(uint32_t topOfStack, uint32_t isExtendedFrame); + +//------------------------------------------------------------------------------ +// Variables +//------------------------------------------------------------------------------ + +//! @brief Global used solely to pass info back to asm PendSV handler code. +bool g_ar_hasExtendedFrame = false; + +//------------------------------------------------------------------------------ +// Code +//------------------------------------------------------------------------------ + +void ar_port_init_system() +{ + // Enable FPU on Cortex-M4F. +#if __FPU_USED + // Enable full access to coprocessors 10 and 11 to enable FPU access. + SCB->CPACR |= (3 << 20) | (3 << 22); + + // Disable lazy FP context save. + FPU->FPCCR |= FPU_FPCCR_ASPEN_Msk; + FPU->FPCCR &= ~FPU_FPCCR_LSPEN_Msk; +#endif // __FPU_USED + + // Init PSP. + __set_PSP((uint32_t)g_ar.idleThread.m_stackPointer); + + // Set priorities for the exceptions we use in the kernel. + NVIC_SetPriority(SVCall_IRQn, kHandlerPriority); + NVIC_SetPriority(PendSV_IRQn, kHandlerPriority); + //TODO: use uTIM overflow irq in tick mode + NVIC_EnableIRQ(sTIM_IRQn); + NVIC_SetPriority(sTIM_IRQn, kHandlerPriority); +} + + + +void ar_port_init_tick_timer() +{ + //DBGMCU->APB1FZ |= DBGMCU_APB1_FZ_DBG_TIM3_STOP; //stop timer in debug, useful for stepping + RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; + RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; + // NVIC_ClearPendingIRQ + uTIM->CR1 = 0; + uTIM->CR2 = (2 << TIM_CR2_MMS_Pos); //UPDATE EVENT as TRGO + uTIM->PSC = (F_uTIM / 1000000UL)-1;//psc to have 1Mhz (1us) ticking + uTIM->ARR = (1000 * kSchedulerQuanta_ms) - 1;//kSchedulerQuanta_ms must be below 65ms!!!! + uTIM->EGR = TIM_EGR_UG;//apply config + uTIM->SR = ~TIM_SR_UIF; + + sTIM->CR1 = 0; + sTIM->CR2 = 0; + sTIM->SMCR = (TIM_SMCR_TS_1) | (TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1 | TIM_SMCR_SMS_2);//count on TRGI, ITR2 (TIM3) + sTIM->PSC = 0; + sTIM->ARR = sTIM_MAX; + sTIM->EGR = TIM_EGR_UG;//apply config + sTIM->SR = ~TIM_SR_UIF; + + +#if AR_ENABLE_TICKLESS_IDLE + ar_port_set_time_delay(false, 0); +#else // AR_ENABLE_TICKLESS_IDLE + ar_port_set_timer_delay(true, kSchedulerQuanta_ms * 1000); +#endif // AR_ENABLE_TICKLESS_IDLE + + uTIM->CR1 |= TIM_CR1_CEN; + sTIM->CR1 |= TIM_CR1_CEN; +} + +//! Schedules next interrupt from timer. +//! +//! @param enables/disables timer +//! @param configures delay of next interrupt from timer +//! @return real value of delay, because clamping may occur +//! returns 0 when timer does not run or is delay:_us is 0 +//! and interrupt is fired up immediately +uint32_t ar_port_set_time_delay(bool enable, uint32_t delay_us) +{ + if (enable) + { + uint32_t delay_ticks = delay_us/1000 / kSchedulerQuanta_ms; + // If the delay is 0, just make the SysTick interrupt pending. + if (delay_ticks == 0) + { + //disable compare interrupts + sTIM->DIER &= ~TIM_DIER_CC1IE; + NVIC_SetPendingIRQ(sTIM_IRQn); + return 0; + } + + sTIM->CCR1 = sTIM->CNT + delay_ticks; + sTIM->DIER |= TIM_DIER_CC1IE; + + return delay_ticks * (kSchedulerQuanta_ms * 1000); + } + else + { + sTIM->DIER &= ~TIM_DIER_CC1IE; //just don't fire interrupts on compare + return 0; + } +} + +uint16_t ar_port_get_time_since_alignment_us() +{ + return uTIM->CNT; +} + +uint32_t ar_port_get_time_absolute_ticks() +{ + return sTIM->CNT; +} + +uint64_t ar_port_get_time_absolute_us() +{ + uint16_t us_preread = uTIM->CNT; + uint32_t ticks = sTIM->CNT; + uint16_t us = uTIM->CNT; + if (us_preread > us) { //overflow occurred during reading + ticks = sTIM->CNT; + } + return static_cast(ticks * (kSchedulerQuanta_ms * 1000)) + us; +} + +//TODO: is return type sufficient? +uint32_t ar_port_get_time_absolute_ms() +{ + uint16_t us_preread = uTIM->CNT; + uint32_t ticks = sTIM->CNT; + uint16_t us = uTIM->CNT; + if (us_preread > us) { //overflow occurred during reading + ticks = sTIM->CNT; + } + return static_cast(ticks * kSchedulerQuanta_ms) + us/1000; +} + +//! A total of 64 bytes of stack space is required to hold the initial +//! thread context. +//! +//! The entire remainder of the stack is filled with the pattern 0xba +//! as an easy way to tell what the high watermark of stack usage is. +void ar_port_prepare_stack(ar_thread_t * thread, uint32_t stackSize, void * param) +{ +#if __FPU_USED + // Clear the extended frame flag. + thread->m_portData.m_hasExtendedFrame = false; +#endif // __FPU_USED + + // 8-byte align stack. + uint32_t sp = reinterpret_cast(thread->m_stackBottom) + stackSize; + uint32_t delta = sp & 7; + sp -= delta; + stackSize = (stackSize - delta) & ~7; + thread->m_stackTop = reinterpret_cast(sp); + thread->m_stackBottom = reinterpret_cast(sp - stackSize); + +#if AR_THREAD_STACK_PATTERN_FILL + // Fill the stack with a pattern. We just take the low byte of the fill pattern since + // memset() is a byte fill. This assumes each byte of the fill pattern is the same. + memset(thread->m_stackBottom, kStackFillValue & 0xff, stackSize); +#endif // AR_THREAD_STACK_PATTERN_FILL + + // Save new top of stack. Also, make sure stack is 8-byte aligned. + sp -= sizeof(ThreadContext); + thread->m_stackPointer = reinterpret_cast(sp); + + // Set the initial context on stack. + ThreadContext * context = reinterpret_cast(sp); + context->xpsr = kInitialxPSR; + context->pc = reinterpret_cast(ar_thread_wrapper); + context->lr = kInitialLR; + context->r0 = reinterpret_cast(thread); // Pass pointer to Thread object as first argument. + context->r1 = reinterpret_cast(param); // Pass arbitrary parameter as second argument. + + // For debug builds, set registers to initial values that are easy to identify on the stack. +#if DEBUG + context->r2 = 0x22222222; + context->r3 = 0x33333333; + context->r4 = 0x44444444; + context->r5 = 0x55555555; + context->r6 = 0x66666666; + context->r7 = 0x77777777; + context->r8 = 0x88888888; + context->r9 = 0x99999999; + context->r10 = 0xaaaaaaaa; + context->r11 = 0xbbbbbbbb; + context->r12 = 0xcccccccc; +#endif + + // Write a check value to the bottom of the stack. + *thread->m_stackBottom = kStackCheckValue; +} + +// Provide atomic operations for Cortex-M0+ that doesn't have load/store exclusive +// instructions. We just have to disable interrupts. +#if (__CORTEX_M < 3) +int8_t ar_atomic_add8(volatile int8_t * value, int8_t delta) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int8_t originalValue = *value; + *value += delta; + __set_PRIMASK(primask); + return originalValue; +} + +int16_t ar_atomic_add16(volatile int16_t * value, int16_t delta) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int16_t originalValue = *value; + *value += delta; + __set_PRIMASK(primask); + return originalValue; +} + +int32_t ar_atomic_add32(volatile int32_t * value, int32_t delta) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int32_t originalValue = *value; + *value += delta; + __set_PRIMASK(primask); + return originalValue; +} + +bool ar_atomic_cas8(volatile int8_t * value, int8_t expectedValue, int8_t newValue) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int8_t oldValue = *value; + if (oldValue == expectedValue) + { + *value = newValue; + __set_PRIMASK(primask); + return true; + } + __set_PRIMASK(primask); + return false; +} + +bool ar_atomic_cas16(volatile int16_t * value, int16_t expectedValue, int16_t newValue) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int16_t oldValue = *value; + if (oldValue == expectedValue) + { + *value = newValue; + __set_PRIMASK(primask); + return true; + } + __set_PRIMASK(primask); + return false; +} + +bool ar_atomic_cas32(volatile int32_t * value, int32_t expectedValue, int32_t newValue) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int32_t oldValue = *value; + if (oldValue == expectedValue) + { + *value = newValue; + __set_PRIMASK(primask); + return true; + } + __set_PRIMASK(primask); + return false; +} +#endif // (__CORTEX_M < 3) + +extern "C" void sTIM_IRQHandler(void) +{ + sTIM->SR = 0;// clear all interrupt flags, we use CC1IF only + ar_kernel_periodic_timer_isr(); +} + +uint32_t ar_port_yield_isr(uint32_t topOfStack, uint32_t isExtendedFrame) +{ +#if __FPU_USED + // Save whether there is an extended frame. + if (g_ar.currentThread) + { + g_ar.currentThread->m_portData.m_hasExtendedFrame = isExtendedFrame; + } +#endif // __FPU_USED + + // Run the scheduler. + uint32_t stack = ar_kernel_yield_isr(topOfStack); + +#if __FPU_USED + // Pass whether there is an extended frame back to the asm code. + g_ar_hasExtendedFrame = g_ar.currentThread->m_portData.m_hasExtendedFrame; +#endif // __FPU_USED + + return stack; +} + +#if DEBUG +void ar_port_service_call() +{ + assert(g_ar.lockCount == 0); + SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk; + __DSB(); + __ISB(); +} +#endif // DEBUG + +//TODO: move ar_get_microseconds() somewhere else and redirect to ar_port_get_time_absolute_us() ? +WEAK uint64_t ar_get_microseconds() +{ + return ar_port_get_time_absolute_us(); +} + +#if AR_ENABLE_TRACE +void ar_trace_init() +{ +} + +//! @brief Send one trace event word via ITM port 31. +void ar_trace_1(uint8_t eventID, uint32_t data) +{ + if (ITM->TCR & ITM_TCR_ITMENA_Msk) + { + // Wait until we can send the event. + while (!ITM->PORT[31].u32) + { + } + + // Event consists of 8-bit event ID plus 24-bits of event data. + ITM->PORT[31].u32 = (static_cast(eventID) << 24) | (data & 0x00ffffff); + } +} + +//! @brief Send a 2-word trace event via ITM ports 31 and 30. +void ar_trace_2(uint8_t eventID, uint32_t data0, void * data1) +{ + if (ITM->TCR & ITM_TCR_ITMENA_Msk) + { + // Wait until we can send the event. + while (!ITM->PORT[31].u32) + { + } + + // Event consists of 8-bit event ID plus 24-bits of event data. + ITM->PORT[31].u32 = (static_cast(eventID) << 24) | (data0 & 0x00ffffff); + + // Wait until we can send the event. + while (!ITM->PORT[30].u32) + { + } + + // Send second data on port 30. + ITM->PORT[30].u32 = reinterpret_cast(data1); + } +} +#endif // AR_ENABLE_TRACE + +//------------------------------------------------------------------------------ +// EOF +//------------------------------------------------------------------------------ diff --git a/src/cortex_m/ar_port_f303_single.cpp b/src/cortex_m/ar_port_f303_single.cpp new file mode 100644 index 0000000..125149d --- /dev/null +++ b/src/cortex_m/ar_port_f303_single.cpp @@ -0,0 +1,509 @@ +/* + * Copyright (c) 2013-2015 Immo Software + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * o Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * o Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * o Neither the name of the copyright holder nor the names of its contributors may + * be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include "../ar_internal.h" +#include "ar_port.h" +#include +#include +#include + +#include "clock.h" +#include "ar_config.h" + +using namespace Ar; + +//------------------------------------------------------------------------------ +// Definitions +//------------------------------------------------------------------------------ + +//! Initial thread register values. +enum +{ + kInitialxPSR = 0x01000000u, //!< Set T bit. + kInitialLR = 0u //!< Set to 0 to stop stack crawl by debugger. +}; + +//! @brief Priorities for kernel exceptions. +enum _exception_priorities +{ + //! All handlers use the same, lowest priority. + kHandlerPriority = 0xff +}; + +#define F_sTIM F_TIM3 +#define sTIM TIM3 +#define sTIM_IRQn TIM3_IRQn +#define sTIM_IRQHandler TIM3_IRQHandler +#define sTIM_MAX UINT16_MAX +#define sTIM_MAX_QUANTAS (sTIM_MAX/(kSchedulerQuanta_ms * 1000)) +#define sTIM_MAX_ALIGNED_us (sTIM_MAX_QUANTAS*(kSchedulerQuanta_ms * 1000)) + + +//------------------------------------------------------------------------------ +// Prototypes +//------------------------------------------------------------------------------ + +extern "C" void SysTick_Handler(void); +extern "C" uint32_t ar_port_yield_isr(uint32_t topOfStack, uint32_t isExtendedFrame); + +//------------------------------------------------------------------------------ +// Variables +//------------------------------------------------------------------------------ + +//! @brief Global used solely to pass info back to asm PendSV handler code. +bool g_ar_hasExtendedFrame = false; +static volatile int32_t lastTick = 0; // signed because we have atomics for signed only +static volatile bool propagateTicksToArgon = false; +static volatile uint32_t nextWakeup_tick = 0; //value for irq to know which value to put into lastTick +static volatile uint32_t targetedNextWakeup_tick = 0; //value for irq to know which value to put into lastTick + +//------------------------------------------------------------------------------ +// Code +//------------------------------------------------------------------------------ + +void ar_port_init_system() +{ + // Enable FPU on Cortex-M4F. +#if __FPU_USED + // Enable full access to coprocessors 10 and 11 to enable FPU access. + SCB->CPACR |= (3 << 20) | (3 << 22); + + // Disable lazy FP context save. + FPU->FPCCR |= FPU_FPCCR_ASPEN_Msk; + FPU->FPCCR &= ~FPU_FPCCR_LSPEN_Msk; +#endif // __FPU_USED + + // Init PSP. + __set_PSP((uint32_t)g_ar.idleThread.m_stackPointer); + + // Set priorities for the exceptions we use in the kernel. + NVIC_SetPriority(SVCall_IRQn, kHandlerPriority); + NVIC_SetPriority(PendSV_IRQn, kHandlerPriority); + //TODO: config timers priority irq + //TODO: use uTIM overflow irq in tick mode + NVIC_EnableIRQ(sTIM_IRQn); + NVIC_SetPriority(sTIM_IRQn, kHandlerPriority); +} + + + +void ar_port_init_tick_timer() +{ + //DBGMCU->APB1FZ |= DBGMCU_APB1_FZ_DBG_TIM3_STOP; //stop timer in debug, useful for stepping + RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; + (void)RCC->APB1ENR;//force on tick before accessing timer + sTIM->CR1 = TIM_CR1_URS;// | TIM_CR1_ARPE; //only ovf generated Update IRQ, enable preload on ARR + sTIM->CR2 = 0; + sTIM->PSC = (F_sTIM / 1000000UL)-1;//psc to have 1Mhz (1us) ticking + sTIM->ARR = sTIM_MAX_ALIGNED_us - 1; + // sTIM->EGR = TIM_EGR_UG;//apply config + sTIM->DIER = TIM_DIER_UIE; //update (overflow) interrupt enable + + +#if AR_ENABLE_TICKLESS_IDLE + ar_port_set_time_delay(false, 0); +#else // AR_ENABLE_TICKLESS_IDLE + ar_port_set_time_delay(true, kSchedulerQuanta_ms * 1000); +#endif // AR_ENABLE_TICKLESS_IDLE + + sTIM->CR1 |= TIM_CR1_CEN; +} + + +//! Schedules next interrupt from timer. +//! +//! @param enables/disables timer +//! @param configures delay of next interrupt from timer +//! @return real value of delay, because clamping may occur +//! returns 0 when timer does not run or is delay:_us is 0 +//! and interrupt is fired up immediately +uint32_t ar_port_set_time_delay(bool enable, uint32_t delay_us) +{ + //self preemption never happen here (called only from scheduler[IRQ] or tick[IRQ] which share same priority) + propagateTicksToArgon = enable; + + if (enable) + { + // sTIM->CR1 &= ~TIM_CR1_CEN;//temporary timer stop, just for debug + if (delay_us == 100000U) { + asm("NOP"); + } + uint32_t delay_ticks = delay_us/1000 / kSchedulerQuanta_ms; + // If the delay is 0, just make the SysTick interrupt pending. + if (delay_ticks == 0) + { + _halt();//this really should not happen, this will trigger unaligned tick and that is BAD + return 0; + } + if (delay_ticks == 9) { + asm("NOP"); + } else { + asm("NOP"); + asm("NOP"); + } + + //never alter CNT + uint32_t ovfTicks = (sTIM->SR & TIM_SR_UIF ? (sTIM->ARR + 1) / (kSchedulerQuanta_ms * 1000) : 0); + const uint32_t alignmentsSinceTimerStart = sTIM->CNT / (kSchedulerQuanta_ms * 1000); + + targetedNextWakeup_tick = lastTick + ovfTicks + alignmentsSinceTimerStart + delay_ticks; + if (sTIM_MAX_QUANTAS - alignmentsSinceTimerStart < delay_ticks) { + delay_ticks = sTIM_MAX_QUANTAS - alignmentsSinceTimerStart; + } + nextWakeup_tick = lastTick + ovfTicks + alignmentsSinceTimerStart + delay_ticks; + // sTIM->CR1 &= ~TIM_CR1_ARPE;//disable shadowing, to get access to active register + sTIM->ARR = (alignmentsSinceTimerStart + delay_ticks) * (kSchedulerQuanta_ms * 1000) - 1; + // sTIM->CR1 |= TIM_CR1_ARPE;//reenable shadowing, to prepare backup value to load after ovf (in case system can't handle ovf soon) + // sTIM->ARR = sTIM_MAX_ALIGNED_us - 1; //after overflow, timer reconfigures to maximal value (allows for quite late handling of overflow) + // sTIM->CR1 |= TIM_CR1_CEN;//temporary timer stop, just for debug + return delay_ticks * (kSchedulerQuanta_ms * 1000); + } + else + { + // sTIM->CR1 &= ~TIM_CR1_ARPE;//disable shadowing, to get access to active register + sTIM->ARR = sTIM_MAX_ALIGNED_us - 1; //maximum length ticking + // sTIM->CR1 |= TIM_CR1_ARPE;//TODO: not necessary? + return 0; + } +} + + +// uint16_t ar_port_get_time_since_alignment_us() +// { +// return sTIM->CNT % (kSchedulerQuanta_ms * 1000); +// } + +uint32_t ar_port_get_time_absolute_ticks() +{ + return ar_port_get_time_absolute_ms()/kSchedulerQuanta_ms; +} + +// uint64_t ar_port_get_time_absolute_us() +// { +// auto cnt = sTIM->CNT; +// auto ticks = lastTick; +// auto ovf = sTIM->SR & TIM_SR_UIF; +// auto nextTick = nextWakeup_tick; + +// decltype(cnt) preread_cnt; +// decltype(ticks) preread_ticks; +// decltype(ovf) preread_ovf; +// decltype(nextTick) preread_nextTick; +// do { +// preread_cnt = cnt; +// preread_ticks = ticks; +// preread_ovf = ovf; +// preread_nextTick = nextTick; +// cnt = sTIM->CNT; +// ticks = lastTick; +// ovf = sTIM->SR & TIM_SR_UIF; +// nextTick = nextWakeup_tick; + +// }while(preread_cnt > cnt || preread_ticks != ticks || preread_ovf != ovf || nextTick!= preread_nextTick); +// //TODO: is this contraption enough to have synced 3 variables? + +// return static_cast(ovf ? nextTick : ticks) * (kSchedulerQuanta_ms * 1000) + cnt; +// } + +//TODO: is return type sufficient? +uint32_t ar_port_get_time_absolute_ms() +{ + auto cnt = sTIM->CNT; + auto ticks = lastTick; + auto ovf = sTIM->SR & TIM_SR_UIF; + auto nextTick = nextWakeup_tick; + + decltype(cnt) preread_cnt; + decltype(ticks) preread_ticks; + decltype(ovf) preread_ovf; + decltype(nextTick) preread_nextTick; + do { + preread_cnt = cnt; + preread_ticks = ticks; + preread_ovf = ovf; + preread_nextTick = nextTick; + cnt = sTIM->CNT; + ticks = lastTick; + ovf = sTIM->SR & TIM_SR_UIF; + nextTick = nextWakeup_tick; + + }while(preread_cnt > cnt || preread_ticks != ticks || preread_ovf != ovf || nextTick!= preread_nextTick); + //TODO: is this contraption enough to have synced 3 variables? + volatile uint32_t output = static_cast(ovf ? nextTick : ticks) * (kSchedulerQuanta_ms) + cnt /1000; + return output; +} + + + + +//! A total of 64 bytes of stack space is required to hold the initial +//! thread context. +//! +//! The entire remainder of the stack is filled with the pattern 0xba +//! as an easy way to tell what the high watermark of stack usage is. +void ar_port_prepare_stack(ar_thread_t * thread, uint32_t stackSize, void * param) +{ +#if __FPU_USED + // Clear the extended frame flag. + thread->m_portData.m_hasExtendedFrame = false; +#endif // __FPU_USED + + // 8-byte align stack. + uint32_t sp = reinterpret_cast(thread->m_stackBottom) + stackSize; + uint32_t delta = sp & 7; + sp -= delta; + stackSize = (stackSize - delta) & ~7; + thread->m_stackTop = reinterpret_cast(sp); + thread->m_stackBottom = reinterpret_cast(sp - stackSize); + +#if AR_THREAD_STACK_PATTERN_FILL + // Fill the stack with a pattern. We just take the low byte of the fill pattern since + // memset() is a byte fill. This assumes each byte of the fill pattern is the same. + memset(thread->m_stackBottom, kStackFillValue & 0xff, stackSize); +#endif // AR_THREAD_STACK_PATTERN_FILL + + // Save new top of stack. Also, make sure stack is 8-byte aligned. + sp -= sizeof(ThreadContext); + thread->m_stackPointer = reinterpret_cast(sp); + + // Set the initial context on stack. + ThreadContext * context = reinterpret_cast(sp); + context->xpsr = kInitialxPSR; + context->pc = reinterpret_cast(ar_thread_wrapper); + context->lr = kInitialLR; + context->r0 = reinterpret_cast(thread); // Pass pointer to Thread object as first argument. + context->r1 = reinterpret_cast(param); // Pass arbitrary parameter as second argument. + + // For debug builds, set registers to initial values that are easy to identify on the stack. +#if DEBUG + context->r2 = 0x22222222; + context->r3 = 0x33333333; + context->r4 = 0x44444444; + context->r5 = 0x55555555; + context->r6 = 0x66666666; + context->r7 = 0x77777777; + context->r8 = 0x88888888; + context->r9 = 0x99999999; + context->r10 = 0xaaaaaaaa; + context->r11 = 0xbbbbbbbb; + context->r12 = 0xcccccccc; +#endif + + // Write a check value to the bottom of the stack. + *thread->m_stackBottom = kStackCheckValue; +} + +// Provide atomic operations for Cortex-M0+ that doesn't have load/store exclusive +// instructions. We just have to disable interrupts. +#if (__CORTEX_M < 3) +int8_t ar_atomic_add8(volatile int8_t * value, int8_t delta) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int8_t originalValue = *value; + *value += delta; + __set_PRIMASK(primask); + return originalValue; +} + +int16_t ar_atomic_add16(volatile int16_t * value, int16_t delta) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int16_t originalValue = *value; + *value += delta; + __set_PRIMASK(primask); + return originalValue; +} + +int32_t ar_atomic_add32(volatile int32_t * value, int32_t delta) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int32_t originalValue = *value; + *value += delta; + __set_PRIMASK(primask); + return originalValue; +} + +bool ar_atomic_cas8(volatile int8_t * value, int8_t expectedValue, int8_t newValue) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int8_t oldValue = *value; + if (oldValue == expectedValue) + { + *value = newValue; + __set_PRIMASK(primask); + return true; + } + __set_PRIMASK(primask); + return false; +} + +bool ar_atomic_cas16(volatile int16_t * value, int16_t expectedValue, int16_t newValue) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int16_t oldValue = *value; + if (oldValue == expectedValue) + { + *value = newValue; + __set_PRIMASK(primask); + return true; + } + __set_PRIMASK(primask); + return false; +} + +bool ar_atomic_cas32(volatile int32_t * value, int32_t expectedValue, int32_t newValue) +{ + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DSB(); + int32_t oldValue = *value; + if (oldValue == expectedValue) + { + *value = newValue; + __set_PRIMASK(primask); + return true; + } + __set_PRIMASK(primask); + return false; +} +#endif // (__CORTEX_M < 3) + +extern "C" void sTIM_IRQHandler(void) +{ + sTIM->SR &= ~TIM_SR_UIF; + lastTick = nextWakeup_tick + sTIM->CNT/(kSchedulerQuanta_ms * 1000); + if (propagateTicksToArgon) + { + ar_kernel_periodic_timer_isr(); + } + if (nextWakeup_tick == lastTick) + { + //not updated next tick + if (targetedNextWakeup_tick > nextWakeup_tick) + { + // and still not there + ar_port_set_time_delay(true, (targetedNextWakeup_tick - nextWakeup_tick) * kSchedulerQuanta_ms * 1000); + } + } +} + +uint32_t ar_port_yield_isr(uint32_t topOfStack, uint32_t isExtendedFrame) +{ +#if __FPU_USED + // Save whether there is an extended frame. + if (g_ar.currentThread) + { + g_ar.currentThread->m_portData.m_hasExtendedFrame = isExtendedFrame; + } +#endif // __FPU_USED + + // Run the scheduler. + uint32_t stack = ar_kernel_yield_isr(topOfStack); + +#if __FPU_USED + // Pass whether there is an extended frame back to the asm code. + g_ar_hasExtendedFrame = g_ar.currentThread->m_portData.m_hasExtendedFrame; +#endif // __FPU_USED + + return stack; +} + +#if DEBUG +void ar_port_service_call() +{ + assert(g_ar.lockCount == 0); + SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk; + __DSB(); + __ISB(); +} +#endif // DEBUG + +//TODO: move ar_get_microseconds() somewhere else and redirect to ar_port_get_time_absolute_us() ? +WEAK uint64_t ar_get_microseconds() +{ + return ar_port_get_time_absolute_us(); +} + +#if AR_ENABLE_TRACE +void ar_trace_init() +{ +} + +//! @brief Send one trace event word via ITM port 31. +void ar_trace_1(uint8_t eventID, uint32_t data) +{ + if (ITM->TCR & ITM_TCR_ITMENA_Msk) + { + // Wait until we can send the event. + while (!ITM->PORT[31].u32) + { + } + + // Event consists of 8-bit event ID plus 24-bits of event data. + ITM->PORT[31].u32 = (static_cast(eventID) << 24) | (data & 0x00ffffff); + } +} + +//! @brief Send a 2-word trace event via ITM ports 31 and 30. +void ar_trace_2(uint8_t eventID, uint32_t data0, void * data1) +{ + if (ITM->TCR & ITM_TCR_ITMENA_Msk) + { + // Wait until we can send the event. + while (!ITM->PORT[31].u32) + { + } + + // Event consists of 8-bit event ID plus 24-bits of event data. + ITM->PORT[31].u32 = (static_cast(eventID) << 24) | (data0 & 0x00ffffff); + + // Wait until we can send the event. + while (!ITM->PORT[30].u32) + { + } + + // Send second data on port 30. + ITM->PORT[30].u32 = reinterpret_cast(data1); + } +} +#endif // AR_ENABLE_TRACE + +//------------------------------------------------------------------------------ +// EOF +//------------------------------------------------------------------------------ From da2b0b343944f254166eabce0ecfd6bbbcf0062b Mon Sep 17 00:00:00 2001 From: diggit Date: Sun, 12 Apr 2020 18:42:47 +0200 Subject: [PATCH 3/3] add systick example port and cleanups - some f303 port cleanups - add port using generic ARM SysTick with explained limitations only lightly tested! --- src/ar_internal.h | 28 ++- src/cortex_m/ar_port_f303_chained.cpp | 26 +-- src/cortex_m/ar_port_f303_single.cpp | 110 +++++---- .../{ar_port.cpp => ar_port_systick.cpp} | 210 +++++++++++++----- 4 files changed, 246 insertions(+), 128 deletions(-) rename src/cortex_m/{ar_port.cpp => ar_port_systick.cpp} (62%) diff --git a/src/ar_internal.h b/src/ar_internal.h index 69a7f19..cb1828d 100644 --- a/src/ar_internal.h +++ b/src/ar_internal.h @@ -173,11 +173,10 @@ extern ar_kernel_t g_ar; //@{ void ar_port_init_system(); void ar_port_init_tick_timer(); -uint32_t ar_port_set_time_delay(bool enable, uint32_t delay_us); -uint16_t ar_port_get_time_since_alignment_us(); +void ar_port_set_time_delay(bool enable, uint32_t delay_us); uint32_t ar_port_get_time_absolute_ticks(); -uint64_t ar_port_get_time_absolute_us(); uint32_t ar_port_get_time_absolute_ms(); +uint64_t ar_port_get_time_absolute_us(); void ar_port_prepare_stack(ar_thread_t * thread, uint32_t stackSize, void * param); void ar_port_service_call(); bool ar_port_get_irq_state(); @@ -292,6 +291,29 @@ class KernelGuard typedef KernelGuard KernelLock; //!< Lock kernel. typedef KernelGuard KernelUnlock; //!< Unlock kernel. +// when there is no other way and you really have to disable interrupt for short moment in some scope +class IrqGuard +{ +private: + // in fact we really need just one bit to store, + uint32_t cpuSR; // occupies 4B on stack but saves 3 masking instruction + // uint8_t cpuSR; // occupies 1B on stack (depends on alignment of surrounding data), adds cca 2 masking instructions +public: + IrqGuard(void) + { + asm ( \ + "mrs %[output], PRIMASK\n\t" \ + "cpsid I\n\t" \ + : [output] "=r" (cpuSR) ::); + } + ~IrqGuard(void) + { + asm ( \ + "msr PRIMASK, %[input];\n\t" \ + ::[input] "r" (cpuSR) : ); + }; +}; + #endif // _AR_INTERNAL_H_ //------------------------------------------------------------------------------ // EOF diff --git a/src/cortex_m/ar_port_f303_chained.cpp b/src/cortex_m/ar_port_f303_chained.cpp index 35de435..6e18084 100644 --- a/src/cortex_m/ar_port_f303_chained.cpp +++ b/src/cortex_m/ar_port_f303_chained.cpp @@ -33,6 +33,7 @@ #include #include +#include "clock.h" #include "ar_config.h" using namespace Ar; @@ -59,7 +60,8 @@ enum _exception_priorities #define F_uTIM F_TIM3 #define uTIM TIM3 #define uTIM_IRQn TIM3_IRQn -#define sTIM_MAX UINT16_MAX +#define uTIM_MAX UINT16_MAX +#define uTIM_CLK_EN() do{RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; (void)RCC->APB1ENR;}while(0) //sTIM counts system ticks #define F_sTIM F_TIM2 @@ -67,13 +69,12 @@ enum _exception_priorities #define sTIM_IRQn TIM2_IRQn #define sTIM_IRQHandler TIM2_IRQHandler #define sTIM_MAX UINT32_MAX - +#define sTIM_CLK_EN() do{RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; (void)RCC->APB1ENR;}while(0) //------------------------------------------------------------------------------ // Prototypes //------------------------------------------------------------------------------ -extern "C" void SysTick_Handler(void); extern "C" uint32_t ar_port_yield_isr(uint32_t topOfStack, uint32_t isExtendedFrame); //------------------------------------------------------------------------------ @@ -114,9 +115,9 @@ void ar_port_init_system() void ar_port_init_tick_timer() { - //DBGMCU->APB1FZ |= DBGMCU_APB1_FZ_DBG_TIM3_STOP; //stop timer in debug, useful for stepping - RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; - RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; + //se corresponding bits in DBGMCU->APBxFZ to stop timer in debug, useful for stepping + sTIM_CLK_EN(); + uTIM_CLK_EN(); // NVIC_ClearPendingIRQ uTIM->CR1 = 0; uTIM->CR2 = (2 << TIM_CR2_MMS_Pos); //UPDATE EVENT as TRGO @@ -151,7 +152,7 @@ void ar_port_init_tick_timer() //! @return real value of delay, because clamping may occur //! returns 0 when timer does not run or is delay:_us is 0 //! and interrupt is fired up immediately -uint32_t ar_port_set_time_delay(bool enable, uint32_t delay_us) +void ar_port_set_time_delay(bool enable, uint32_t delay_us) { if (enable) { @@ -159,28 +160,19 @@ uint32_t ar_port_set_time_delay(bool enable, uint32_t delay_us) // If the delay is 0, just make the SysTick interrupt pending. if (delay_ticks == 0) { - //disable compare interrupts - sTIM->DIER &= ~TIM_DIER_CC1IE; NVIC_SetPendingIRQ(sTIM_IRQn); - return 0; + return; } sTIM->CCR1 = sTIM->CNT + delay_ticks; sTIM->DIER |= TIM_DIER_CC1IE; - - return delay_ticks * (kSchedulerQuanta_ms * 1000); } else { sTIM->DIER &= ~TIM_DIER_CC1IE; //just don't fire interrupts on compare - return 0; } } -uint16_t ar_port_get_time_since_alignment_us() -{ - return uTIM->CNT; -} uint32_t ar_port_get_time_absolute_ticks() { diff --git a/src/cortex_m/ar_port_f303_single.cpp b/src/cortex_m/ar_port_f303_single.cpp index 125149d..e1409fa 100644 --- a/src/cortex_m/ar_port_f303_single.cpp +++ b/src/cortex_m/ar_port_f303_single.cpp @@ -60,16 +60,16 @@ enum _exception_priorities #define sTIM TIM3 #define sTIM_IRQn TIM3_IRQn #define sTIM_IRQHandler TIM3_IRQHandler -#define sTIM_MAX UINT16_MAX +#define sTIM_MAX (UINT16_MAX) #define sTIM_MAX_QUANTAS (sTIM_MAX/(kSchedulerQuanta_ms * 1000)) #define sTIM_MAX_ALIGNED_us (sTIM_MAX_QUANTAS*(kSchedulerQuanta_ms * 1000)) +#define sTIM_CLK_EN() do{RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; (void)RCC->APB1ENR;}while(0) //------------------------------------------------------------------------------ // Prototypes //------------------------------------------------------------------------------ -extern "C" void SysTick_Handler(void); extern "C" uint32_t ar_port_yield_isr(uint32_t topOfStack, uint32_t isExtendedFrame); //------------------------------------------------------------------------------ @@ -105,7 +105,6 @@ void ar_port_init_system() // Set priorities for the exceptions we use in the kernel. NVIC_SetPriority(SVCall_IRQn, kHandlerPriority); NVIC_SetPriority(PendSV_IRQn, kHandlerPriority); - //TODO: config timers priority irq //TODO: use uTIM overflow irq in tick mode NVIC_EnableIRQ(sTIM_IRQn); NVIC_SetPriority(sTIM_IRQn, kHandlerPriority); @@ -115,9 +114,8 @@ void ar_port_init_system() void ar_port_init_tick_timer() { - //DBGMCU->APB1FZ |= DBGMCU_APB1_FZ_DBG_TIM3_STOP; //stop timer in debug, useful for stepping - RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; - (void)RCC->APB1ENR;//force on tick before accessing timer + //se corresponding bits in DBGMCU->APBxFZ to stop timer in debug, useful for stepping + sTIM_CLK_EN(); sTIM->CR1 = TIM_CR1_URS;// | TIM_CR1_ARPE; //only ovf generated Update IRQ, enable preload on ARR sTIM->CR2 = 0; sTIM->PSC = (F_sTIM / 1000000UL)-1;//psc to have 1Mhz (1us) ticking @@ -143,14 +141,13 @@ void ar_port_init_tick_timer() //! @return real value of delay, because clamping may occur //! returns 0 when timer does not run or is delay:_us is 0 //! and interrupt is fired up immediately -uint32_t ar_port_set_time_delay(bool enable, uint32_t delay_us) +void ar_port_set_time_delay(bool enable, uint32_t delay_us) { //self preemption never happen here (called only from scheduler[IRQ] or tick[IRQ] which share same priority) propagateTicksToArgon = enable; if (enable) { - // sTIM->CR1 &= ~TIM_CR1_CEN;//temporary timer stop, just for debug if (delay_us == 100000U) { asm("NOP"); } @@ -158,14 +155,12 @@ uint32_t ar_port_set_time_delay(bool enable, uint32_t delay_us) // If the delay is 0, just make the SysTick interrupt pending. if (delay_ticks == 0) { - _halt();//this really should not happen, this will trigger unaligned tick and that is BAD - return 0; - } - if (delay_ticks == 9) { - asm("NOP"); - } else { - asm("NOP"); - asm("NOP"); + if (sTIM->SR & TIM_SR_UIF) + { + return; // already pending interrupt, that is fine + } + assert(1); + return; } //never alter CNT @@ -177,19 +172,11 @@ uint32_t ar_port_set_time_delay(bool enable, uint32_t delay_us) delay_ticks = sTIM_MAX_QUANTAS - alignmentsSinceTimerStart; } nextWakeup_tick = lastTick + ovfTicks + alignmentsSinceTimerStart + delay_ticks; - // sTIM->CR1 &= ~TIM_CR1_ARPE;//disable shadowing, to get access to active register sTIM->ARR = (alignmentsSinceTimerStart + delay_ticks) * (kSchedulerQuanta_ms * 1000) - 1; - // sTIM->CR1 |= TIM_CR1_ARPE;//reenable shadowing, to prepare backup value to load after ovf (in case system can't handle ovf soon) - // sTIM->ARR = sTIM_MAX_ALIGNED_us - 1; //after overflow, timer reconfigures to maximal value (allows for quite late handling of overflow) - // sTIM->CR1 |= TIM_CR1_CEN;//temporary timer stop, just for debug - return delay_ticks * (kSchedulerQuanta_ms * 1000); } else { - // sTIM->CR1 &= ~TIM_CR1_ARPE;//disable shadowing, to get access to active register sTIM->ARR = sTIM_MAX_ALIGNED_us - 1; //maximum length ticking - // sTIM->CR1 |= TIM_CR1_ARPE;//TODO: not necessary? - return 0; } } @@ -204,32 +191,32 @@ uint32_t ar_port_get_time_absolute_ticks() return ar_port_get_time_absolute_ms()/kSchedulerQuanta_ms; } -// uint64_t ar_port_get_time_absolute_us() -// { -// auto cnt = sTIM->CNT; -// auto ticks = lastTick; -// auto ovf = sTIM->SR & TIM_SR_UIF; -// auto nextTick = nextWakeup_tick; - -// decltype(cnt) preread_cnt; -// decltype(ticks) preread_ticks; -// decltype(ovf) preread_ovf; -// decltype(nextTick) preread_nextTick; -// do { -// preread_cnt = cnt; -// preread_ticks = ticks; -// preread_ovf = ovf; -// preread_nextTick = nextTick; -// cnt = sTIM->CNT; -// ticks = lastTick; -// ovf = sTIM->SR & TIM_SR_UIF; -// nextTick = nextWakeup_tick; - -// }while(preread_cnt > cnt || preread_ticks != ticks || preread_ovf != ovf || nextTick!= preread_nextTick); -// //TODO: is this contraption enough to have synced 3 variables? - -// return static_cast(ovf ? nextTick : ticks) * (kSchedulerQuanta_ms * 1000) + cnt; -// } +uint64_t ar_port_get_time_absolute_us() +{ + auto cnt = sTIM->CNT; + auto ticks = lastTick; + auto ovf = sTIM->SR & TIM_SR_UIF; + auto nextTick = nextWakeup_tick; + + decltype(cnt) preread_cnt; + decltype(ticks) preread_ticks; + decltype(ovf) preread_ovf; + decltype(nextTick) preread_nextTick; + do { + preread_cnt = cnt; + preread_ticks = ticks; + preread_ovf = ovf; + preread_nextTick = nextTick; + cnt = sTIM->CNT; + ticks = lastTick; + ovf = sTIM->SR & TIM_SR_UIF; + nextTick = nextWakeup_tick; + + }while(preread_cnt > cnt || preread_ticks != ticks || preread_ovf != ovf || nextTick!= preread_nextTick); + //TODO: is this contraption enough to have synced 3 variables? + + return static_cast(ovf ? nextTick : ticks) * (kSchedulerQuanta_ms * 1000) + cnt; +} //TODO: is return type sufficient? uint32_t ar_port_get_time_absolute_ms() @@ -406,19 +393,28 @@ bool ar_atomic_cas32(volatile int32_t * value, int32_t expectedValue, int32_t ne extern "C" void sTIM_IRQHandler(void) { - sTIM->SR &= ~TIM_SR_UIF; - lastTick = nextWakeup_tick + sTIM->CNT/(kSchedulerQuanta_ms * 1000); + bool ticked = sTIM->SR & TIM_SR_UIF; + if (ticked) + { + sTIM->SR &= ~TIM_SR_UIF; + // update lastTick only on interrupd due to overflow (non SW trigger) + lastTick = nextWakeup_tick + sTIM->CNT/(kSchedulerQuanta_ms * 1000); + } + if (propagateTicksToArgon) { ar_kernel_periodic_timer_isr(); } - if (nextWakeup_tick == lastTick) + + if (ticked) { - //not updated next tick - if (targetedNextWakeup_tick > nextWakeup_tick) + if (nextWakeup_tick == lastTick) //this is time when we scheduled next tick { - // and still not there - ar_port_set_time_delay(true, (targetedNextWakeup_tick - nextWakeup_tick) * kSchedulerQuanta_ms * 1000); + if (targetedNextWakeup_tick > nextWakeup_tick) //but it is not target wakeup time (limited by timer may delay capability) + { + // reconfigure delay again + ar_port_set_time_delay(true, (targetedNextWakeup_tick - nextWakeup_tick) * kSchedulerQuanta_ms * 1000); + } } } } diff --git a/src/cortex_m/ar_port.cpp b/src/cortex_m/ar_port_systick.cpp similarity index 62% rename from src/cortex_m/ar_port.cpp rename to src/cortex_m/ar_port_systick.cpp index 997a306..b3bca46 100644 --- a/src/cortex_m/ar_port.cpp +++ b/src/cortex_m/ar_port_systick.cpp @@ -27,10 +27,22 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +// Due to systick limitation, it is not entirely possible to have true tickless mode. +// Systick value cannot be preset thus period is always tied to configured delay. +// Eg you can't let systick fire interrupt in 500us and then reload period back to +// kSchedulerQuanta_ms. Systick version of tickless mode ticks periodically, +// but calls kernel only when it is time to do so. +// Configuring low kSchedulerQuanta_ms raises load caused by systick exception, +// but 1kHz is still ok on device having clock of tens of MHz. + #include "../ar_internal.h" #include "ar_port.h" #include #include +#include + +#include "clock.h" +#include "ar_config.h" using namespace Ar; @@ -39,18 +51,17 @@ using namespace Ar; //------------------------------------------------------------------------------ //! Initial thread register values. -enum -{ - kInitialxPSR = 0x01000000u, //!< Set T bit. - kInitialLR = 0u //!< Set to 0 to stop stack crawl by debugger. -}; +constexpr uint32_t kInitialxPSR = 0x01000000u; //!< Set T bit. +constexpr uint32_t kInitialLR = 0u; //!< Set to 0 to stop stack crawl by debugger. //! @brief Priorities for kernel exceptions. -enum _exception_priorities -{ - //! All handlers use the same, lowest priority. - kHandlerPriority = 0xff -}; +//! All handlers use the same, lowest priority. +constexpr uint8_t kHandlerPriority = 0xff; + +//! useful constants +constexpr uint32_t kSchedulerQuanta_ticks = SystemCoreClock/1000*kSchedulerQuanta_ms; +constexpr uint32_t kSchedulerQuanta_us = kSchedulerQuanta_ms * 1000; + //------------------------------------------------------------------------------ // Prototypes @@ -66,6 +77,13 @@ extern "C" uint32_t ar_port_yield_isr(uint32_t topOfStack, uint32_t isExtendedFr //! @brief Global used solely to pass info back to asm PendSV handler code. bool g_ar_hasExtendedFrame = false; +//! @brief variables holding data of ticks +static bool hasSystickOverflow = false; +static volatile int32_t lastTick = 0; // signed because we have atomics for signed only +static volatile bool propagateTicksToArgon = false; +static volatile uint32_t nextWakeup_tick = 0; //value for irq to know which value to put into lastTick +static volatile uint32_t targetedNextWakeup_tick = 0; //value for irq to know which value to put into lastTick + //------------------------------------------------------------------------------ // Code //------------------------------------------------------------------------------ @@ -91,78 +109,141 @@ void ar_port_init_system() NVIC_SetPriority(SysTick_IRQn, kHandlerPriority); } + + void ar_port_init_tick_timer() { - // Set SysTick clock source to processor clock. - SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk; - // Clear any pending SysTick IRQ. SCB->ICSR = SCB_ICSR_PENDSTCLR_Msk; + SysTick->LOAD = kSchedulerQuanta_ticks - 1; + SysTick->VAL = 0; #if AR_ENABLE_TICKLESS_IDLE - ar_port_set_timer_delay(false, 0); + ar_port_set_time_delay(false, 0); #else // AR_ENABLE_TICKLESS_IDLE - ar_port_set_timer_delay(true, kSchedulerQuanta_ms * 1000); + ar_port_set_time_delay(true, kSchedulerQuanta_ms * 1000); #endif // AR_ENABLE_TICKLESS_IDLE + + // Set SysTick clock source to processor clock, enable irq and start counting + SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk; +} + + +// we have to buffer systick overglow flag info this way, because reading clears flag :/ +static inline bool ar_port_has_systick_overflow() +{ + hasSystickOverflow |= SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk; + return hasSystickOverflow; } -void ar_port_set_timer_delay(bool enable, uint32_t delay_us) +//! Schedules next interrupt from timer. +//! +//! @param enables/disables timer +//! @param configures delay of next interrupt from timer +//! @return real value of delay, because clamping may occur +//! returns 0 when timer does not run or is delay:_us is 0 +//! and interrupt is fired up immediately +void ar_port_set_time_delay(bool enable, uint32_t delay_us) { - // Disable SysTick while we adjust it. - SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk; //&= ~(SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk); + //self preemption never happen here (called only from scheduler[IRQ] or tick[IRQ] which share same priority) + propagateTicksToArgon = enable; if (enable) { + uint32_t delay_ticks = delay_us/1000/kSchedulerQuanta_ms; // If the delay is 0, just make the SysTick interrupt pending. - if (delay_us == 0) + if (delay_ticks == 0) { - // Clear reload and counter so the elapsed time reads as 0 in ar_port_get_timer_elapsed_us(). - SysTick->LOAD = 0; - SysTick->VAL = 0; - - // Pend SysTick. - SCB->ICSR = SCB_ICSR_PENDSTSET_Msk; + if (ar_port_has_systick_overflow()) + { + return; // already pending interrupt, that is fine + } + assert(1); return; } - // Calculate SysTick reload value. If the desired delay overflows the SysTick counter, - // we just use the max delay (24 bits for SysTick). - uint32_t ticks = SystemCoreClock / 1000000 * delay_us - 1; - if (ticks > SysTick_LOAD_RELOAD_Msk) - { - ticks = SysTick_LOAD_RELOAD_Msk; - } + targetedNextWakeup_tick = lastTick + delay_ticks; + // as descrived above, we can't alter systick to preserve timing precission + nextWakeup_tick = lastTick + 1; + } + else + { + //we have to keep systick ticking (incl. irq), otherwise we loose time info + } +} - // Update SysTick reload value. - SysTick->LOAD = ticks; +uint32_t ar_port_get_time_absolute_ticks() +{ + return lastTick; +} - // Reset the timer to count from the new load value. This also clears COUNTFLAG. - SysTick->VAL = 0; +uint64_t ar_port_get_time_absolute_us() +{ + uint32_t max = SysTick->LOAD; - // Enable SysTick and its IRQ. - SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk; - } - else + uint32_t val = SysTick->VAL; + uint32_t ticks = lastTick; + bool overflown = ar_port_has_systick_overflow(); + decltype(val) preread_val; + decltype(overflown) preread_overflown; + decltype(ticks) preread_ticks; + + //we have to read values with twice without change to be tore they are in sync + do { + preread_val = val; + preread_overflown = overflown; + preread_ticks = ticks; + val = SysTick->VAL; + overflown = ar_port_has_systick_overflow(); + ticks = lastTick; + }while(val > preread_val || overflown != preread_overflown || ticks != preread_ticks); + + // later here we work with buffered (synced) value + uint32_t counter = max - val; + if (overflown) { - // Enable SysTick for maximum count but disable IRQ. - SysTick->LOAD = SysTick_LOAD_RELOAD_Msk; - SysTick->VAL = 0; - SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk; + counter += max; } + return static_cast(ticks) * kSchedulerQuanta_ms * 1000 + counter / (SystemCoreClock / 1000000); } -uint32_t ar_port_get_timer_elapsed_us() +//TODO: is return type sufficient? +uint32_t ar_port_get_time_absolute_ms() { uint32_t max = SysTick->LOAD; - uint32_t counter = max - SysTick->VAL; - if (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) + + + // we could just use ar_port_get_time_absolute_us()/1000 but calling tha fcn involves + // 64bit math *could* be slower + uint32_t val = SysTick->VAL; + uint32_t ticks = lastTick; + bool overflown = ar_port_has_systick_overflow(); + decltype(val) preread_val; + decltype(overflown) preread_overflown; + decltype(ticks) preread_ticks; + + //we have to read values with twice without change to be tore they are in sync + do { + preread_val = val; + preread_overflown = overflown; + preread_ticks = ticks; + val = SysTick->VAL; + overflown = ar_port_has_systick_overflow(); + ticks = lastTick; + }while(val > preread_val || overflown != preread_overflown || ticks != preread_ticks); + + // later here we work with buffered (synced) value + uint32_t counter = max - val; + if (overflown) { counter += max; } - return counter / (SystemCoreClock / 1000000); + return ticks * kSchedulerQuanta_ms + counter / (SystemCoreClock / 1000); } + + //! A total of 64 bytes of stack space is required to hold the initial //! thread context. //! @@ -305,9 +386,35 @@ bool ar_atomic_cas32(volatile int32_t * value, int32_t expectedValue, int32_t ne } #endif // (__CORTEX_M < 3) -void SysTick_Handler(void) +extern "C" void SysTick_Handler(void) { - ar_kernel_periodic_timer_isr(); + bool overflown = ar_port_has_systick_overflow(); + if (overflown) + { + // this tick was most probably caused by systick overflow, + + // these two value should both change at same time :/ + // solution would be disabling irqs for this moment + { + // IrqGuard guard; // comment out, if you are brave enough + hasSystickOverflow = false; + lastTick = nextWakeup_tick; + } + + if (targetedNextWakeup_tick != nextWakeup_tick) + { + // not yet on targeted wakeup time + // in fact this cll will just add 1 to nextWakeup_tick, microoptimize? + // ar_port_set_time_delay(true, (targetedNextWakeup_tick - nextWakeup_tick) * kSchedulerQuanta_ms * 1000); + nextWakeup_tick += 1; + return; + } + } + + if (propagateTicksToArgon) + { + ar_kernel_periodic_timer_isr(); + } } uint32_t ar_port_yield_isr(uint32_t topOfStack, uint32_t isExtendedFrame) @@ -341,9 +448,10 @@ void ar_port_service_call() } #endif // DEBUG +//TODO: move ar_get_microseconds() somewhere else and redirect to ar_port_get_time_absolute_us() ? WEAK uint64_t ar_get_microseconds() { - return static_cast(ar_get_millisecond_count()) * 1000ull; + return ar_port_get_time_absolute_us(); } #if AR_ENABLE_TRACE