From bf956ab4aa894794b9d249590149697c8fb1aa20 Mon Sep 17 00:00:00 2001
From: sasan-golchin <ahmad.golchin@mail.polimi.it>
Date: Mon, 14 Nov 2016 14:17:42 +0100
Subject: [PATCH] CSTimer and new scheduling system ported to
 stm32f207ig_stm3220g-eval board

---
 .../common/interfaces-impl/cstimer.cpp        | 179 ++++++++++++++++++
 .../common/interfaces-impl/portability.cpp    |  42 +---
 .../core/stage_1_boot.cpp                     |   3 +-
 miosix/config/Makefile.inc                    |   5 +-
 4 files changed, 188 insertions(+), 41 deletions(-)
 create mode 100644 miosix/arch/cortexM3_stm32f2/common/interfaces-impl/cstimer.cpp

diff --git a/miosix/arch/cortexM3_stm32f2/common/interfaces-impl/cstimer.cpp b/miosix/arch/cortexM3_stm32f2/common/interfaces-impl/cstimer.cpp
new file mode 100644
index 00000000..66f709eb
--- /dev/null
+++ b/miosix/arch/cortexM3_stm32f2/common/interfaces-impl/cstimer.cpp
@@ -0,0 +1,179 @@
+
+#include "interfaces/cstimer.h"
+#include "interfaces/arch_registers.h"
+#include "kernel/kernel.h"
+#include "kernel/scheduler/timer_interrupt.h"
+#include "kernel/timeconversion.h"
+
+using namespace miosix;
+
+const unsigned int timerBits=32;
+const unsigned long long overflowIncrement=(1LL<<timerBits);
+const unsigned long long lowerMask=overflowIncrement-1;
+const unsigned long long upperMask=0xFFFFFFFFFFFFFFFFLL-lowerMask;
+
+static long long ms32time = 0; //most significant 32 bits of counter
+static long long ms32chkp = 0; //most significant 32 bits of check point
+static bool lateIrq=false;
+
+static TimeConversion *tc;
+
+static inline long long nextInterrupt()
+{
+    return ms32chkp | TIM2->CCR1;
+}
+
+static inline long long IRQgetTick()
+{
+    //THE PENDING BIT TRICK, version 2
+    //This algorithm is the main part that allows to extend in software a
+    //32bit timer to a 64bit one. The basic idea is this: the lower bits of the
+    //64bit timer are kept by the counter register of the timer, while the upper
+    //bits are kept in a software variable. When the hardware timer overflows, 
+    //an interrupt is used to update the upper bits.
+    //Reading the timer may appear to be doable by just an OR operation between
+    //the software variable and the hardware counter, but is actually way
+    //trickier than it seems, because user code may:
+    //1 disable interrupts,
+    //2 spend a little time with interrupts disabled,
+    //3 call this function.
+    //Now, if a timer overflow occurs while interrupts are disabled, the upper
+    //bits have not yet been updated, so we would return the wrong time.
+    //To fix this, we check the timer overflow pending bit, and if it is set
+    //we return the time adjusted accordingly. This almost works, the last
+    //issue to fix is that reading the timer counter and the pending bit
+    //is not an atomic operation, and the counter may roll over exactly at that
+    //point in time. To solve this, we read the timer a second time to see if
+    //it had rolled over.
+    //Note that this algorithm imposes a limit on the maximum time interrupts
+    //can be disabeld, equals to one hardware timer period minus the time
+    //between the two timer reads in this algorithm.
+    unsigned int counter=TIM2->CNT;
+    if((TIM2->SR & TIM_SR_UIF) && TIM2->CNT>=counter)
+        return (ms32time | static_cast<long long>(counter)) + overflowIncrement;
+    return ms32time | static_cast<long long>(counter);
+}
+
+void __attribute__((naked)) TIM2_IRQHandler()
+{
+    saveContext();
+    asm volatile ("bl _Z9cstirqhndv");
+    restoreContext();
+}
+
+void __attribute__((used)) cstirqhnd()
+{
+    if(TIM2->SR & TIM_SR_CC1IF || lateIrq)
+    {
+        TIM2->SR = ~TIM_SR_CC1IF;
+        if(ms32time==ms32chkp || lateIrq)
+        {
+            lateIrq=false;
+            IRQtimerInterrupt(tc->tick2ns(IRQgetTick()));
+        }
+
+    }
+    //Rollover
+    if(TIM2->SR & TIM_SR_UIF)
+    {
+        TIM2->SR = ~TIM_SR_UIF;
+        ms32time += overflowIncrement;
+    }
+}
+
+//
+// class ContextSwitchTimer
+//
+
+namespace miosix {
+
+ContextSwitchTimer& ContextSwitchTimer::instance()
+{
+    static ContextSwitchTimer instance;
+    return instance;
+}
+
+void ContextSwitchTimer::IRQsetNextInterrupt(long long ns)
+{
+    long long tick = tc->ns2tick(ns);
+    ms32chkp = tick & upperMask;
+    TIM2->CCR1 = static_cast<unsigned int>(tick & lowerMask);
+    if(IRQgetTick() >= nextInterrupt())
+    {
+        NVIC_SetPendingIRQ(TIM2_IRQn);
+        lateIrq=true;
+    }
+}
+
+long long ContextSwitchTimer::getNextInterrupt() const
+{
+    return tc->tick2ns(nextInterrupt());
+}
+
+long long ContextSwitchTimer::getCurrentTime() const
+{
+    bool interrupts=areInterruptsEnabled();
+    //TODO: optimization opportunity, if we can guarantee that no call to this
+    //function occurs before kernel is started, then we can use
+    //fastInterruptDisable())
+    if(interrupts) disableInterrupts();
+    long long result=tc->tick2ns(IRQgetTick());
+    if(interrupts) enableInterrupts();
+    return result;
+}
+
+long long ContextSwitchTimer::IRQgetCurrentTime() const
+{
+    return tc->tick2ns(IRQgetTick());
+}
+
+ContextSwitchTimer::~ContextSwitchTimer() {}
+
+ContextSwitchTimer::ContextSwitchTimer()
+{
+    // TIM2 Source Clock (from APB1) Enable
+    {
+        //NOTE: Not FastInterruptDisableLock as this is called before kernel
+        //is started
+        InterruptDisableLock idl;
+        RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
+        RCC_SYNC();
+        DBGMCU->APB1FZ|=DBGMCU_APB1_FZ_DBG_TIM2_STOP; //Tim2 stops while debugging
+    }
+    // Setup TIM2 base configuration
+    // Mode: Up-counter
+    // Interrupts: counter overflow, Compare/Capture on channel 1
+    TIM2->CR1=TIM_CR1_URS;
+    TIM2->DIER=TIM_DIER_UIE | TIM_DIER_CC1IE;
+    NVIC_SetPriority(TIM2_IRQn,3); //High priority for TIM2 (Max=0, min=15)
+    NVIC_EnableIRQ(TIM2_IRQn);
+    // Configure channel 1 as:
+    // Output channel (CC1S=0)
+    // No preload(OC1PE=0), hence TIM2_CCR1 can be written at anytime
+    // No effect on the output signal on match (OC1M = 0)
+    TIM2->CCMR1 = 0;
+    TIM2->CCR1 = 0;
+    // TIM2 Operation Frequency Configuration: Max Freq. and longest period
+    TIM2->PSC = 0;
+    TIM2->ARR = 0xFFFFFFFF;
+    
+    // Enable TIM2 Counter
+    TIM2->EGR = TIM_EGR_UG; //To enforce the timer to apply PSC
+    TIM2->CR1 |= TIM_CR1_CEN;
+    
+    // The global variable SystemCoreClock from ARM's CMSIS allows to know
+    // the CPU frequency.
+    timerFreq=SystemCoreClock;
+
+    // The timer frequency may however be a submultiple of the CPU frequency,
+    // due to the bus at whch the periheral is connected being slower. The
+    // RCC->CFGR register tells us how slower the APB1 bus is running.
+    // This formula takes into account that if the APB1 clock is divided by a
+    // factor of two or greater, the timer is clocked at twice the bus
+    // interface. After this, the freq variable contains the frequency in Hz
+    // at which the timer prescaler is clocked.
+    if(RCC->CFGR & RCC_CFGR_PPRE1_2) timerFreq/=1<<((RCC->CFGR>>10) & 0x3);
+    static TimeConversion stc(timerFreq);
+    tc = &stc;
+}
+} //namespace miosix
diff --git a/miosix/arch/cortexM3_stm32f2/common/interfaces-impl/portability.cpp b/miosix/arch/cortexM3_stm32f2/common/interfaces-impl/portability.cpp
index c7f4c9e9..7239e249 100644
--- a/miosix/arch/cortexM3_stm32f2/common/interfaces-impl/portability.cpp
+++ b/miosix/arch/cortexM3_stm32f2/common/interfaces-impl/portability.cpp
@@ -39,22 +39,6 @@
 #include <cstring>
 #include <cassert>
 
-/**
- * \internal
- * timer interrupt routine.
- * Since inside naked functions only assembler code is allowed, this function
- * only calls the ctxsave/ctxrestore macros (which are in assembler), and calls
- * the implementation code in ISR_preempt()
- */
-void SysTick_Handler()   __attribute__((naked));
-void SysTick_Handler()
-{
-    saveContext();
-    //Call ISR_preempt(). Name is a C++ mangled name.
-    asm volatile("bl _ZN14miosix_private11ISR_preemptEv");
-    restoreContext();
-}
-
 /**
  * \internal
  * software interrupt routine.
@@ -92,19 +76,6 @@ void TIM3_IRQHandler()
 
 namespace miosix_private {
 
-/**
- * \internal
- * Called by the timer interrupt, preempt to next thread
- * Declared noinline to avoid the compiler trying to inline it into the caller,
- * which would violate the requirement on naked functions. Function is not
- * static because otherwise the compiler optimizes it out...
- */
-void ISR_preempt() __attribute__((noinline));
-void ISR_preempt()
-{
-    miosix::IRQtimerInterrupt();  
-}
-
 /**
  * \internal
  * Called by the software interrupt, yield to next thread
@@ -297,24 +268,20 @@ void IRQportableStartKernel()
     SCB->CCR |= SCB_CCR_DIV_0_TRP_Msk;
     NVIC_SetPriorityGrouping(7);//This should disable interrupt nesting
     NVIC_SetPriority(SVCall_IRQn,3);//High priority for SVC (Max=0, min=15)
-    NVIC_SetPriority(SysTick_IRQn,3);//High priority for SysTick (Max=0, min=15)
     NVIC_SetPriority(MemoryManagement_IRQn,2);//Higher priority for MemoryManagement (Max=0, min=15)
-    SysTick->LOAD=SystemCoreClock/miosix::TICK_FREQ;
-    //Start SysTick, set to generate interrupts
-    SysTick->CTRL=SysTick_CTRL_ENABLE_Msk | SysTick_CTRL_TICKINT_Msk |
-            SysTick_CTRL_CLKSOURCE_Msk;
 
     #ifdef WITH_PROCESSES
     miosix::IRQenableMPUatBoot();
     #endif //WITH_PROCESSES
-    #ifdef SCHED_TYPE_CONTROL_BASED
-    AuxiliaryTimer::IRQinit();
-    #endif //SCHED_TYPE_CONTROL_BASED
     
     //create a temporary space to save current registers. This data is useless
     //since there's no way to stop the sheduler, but we need to save it anyway.
     unsigned int s_ctxsave[miosix::CTXSAVE_SIZE];
     ctxsave=s_ctxsave;//make global ctxsave point to it
+}
+
+void IRQportableFinishKernelStartup()
+{
     //Note, we can't use enableInterrupts() now since the call is not mathced
     //by a call to disableInterrupts()
     __enable_fault_irq();
@@ -322,7 +289,6 @@ void IRQportableStartKernel()
     miosix::Thread::yield();
     //Never reaches here
 }
-
 void sleepCpu()
 {
     __WFI();
diff --git a/miosix/arch/cortexM3_stm32f2/stm32f207ig_stm3220g-eval/core/stage_1_boot.cpp b/miosix/arch/cortexM3_stm32f2/stm32f207ig_stm3220g-eval/core/stage_1_boot.cpp
index 5c166eef..1cbc8710 100644
--- a/miosix/arch/cortexM3_stm32f2/stm32f207ig_stm3220g-eval/core/stage_1_boot.cpp
+++ b/miosix/arch/cortexM3_stm32f2/stm32f207ig_stm3220g-eval/core/stage_1_boot.cpp
@@ -134,7 +134,7 @@ void /*__attribute__((weak))*/ UsageFault_Handler();
 void /*__attribute__((weak))*/ SVC_Handler();
 void /*__attribute__((weak))*/ DebugMon_Handler();
 void /*__attribute__((weak))*/ PendSV_Handler();
-void /*__attribute__((weak))*/ SysTick_Handler();
+void __attribute__((weak)) SysTick_Handler();
 
 //Interrupt handlers
 void __attribute__((weak)) WWDG_IRQHandler();
@@ -409,3 +409,4 @@ void (* const __Vectors[])() __attribute__ ((section(".isr_vector"))) =
 #pragma weak DCMI_IRQHandler = Default_Handler
 #pragma weak CRYP_IRQHandler = Default_Handler
 #pragma weak HASH_RNG_IRQHandler = Default_Handler
+#pragma weak SysTick_Handler = Default_Handler
diff --git a/miosix/config/Makefile.inc b/miosix/config/Makefile.inc
index 35a7f64b..ef99836b 100644
--- a/miosix/config/Makefile.inc
+++ b/miosix/config/Makefile.inc
@@ -18,7 +18,7 @@
 #OPT_BOARD := stm32f103ve_strive_mini
 #OPT_BOARD := stm32f103ze_redbull_v2
 #OPT_BOARD := stm32f407vg_stm32f4discovery
-#OPT_BOARD := stm32f207ig_stm3220g-eval
+OPT_BOARD := stm32f207ig_stm3220g-eval
 #OPT_BOARD := stm32f207zg_ethboard_v2
 #OPT_BOARD := stm32f207ze_als_camboard
 #OPT_BOARD := stm32l151_als_mainboard
@@ -59,7 +59,7 @@
 ## size and speed
 ##
 #OPT_OPTIMIZATION := -O0
-#OPT_OPTIMIZATION := -O2
+OPT_OPTIMIZATION := -O2
 #OPT_OPTIMIZATION := -O3
 #OPT_OPTIMIZATION := -Os
 
@@ -2005,6 +2005,7 @@ else ifeq ($(ARCH),cortexM3_stm32f2)
     arch/common/drivers/stm32_hardware_rng.cpp               \
     $(ARCH_INC)/interfaces-impl/portability.cpp              \
     $(ARCH_INC)/interfaces-impl/gpio_impl.cpp                \
+    $(ARCH_INC)/interfaces-impl/cstimer.cpp      \
     arch/common/CMSIS/Device/ST/STM32F2xx/Source/Templates/system_stm32f2xx.c
 
 ##-----------------------------------------------------------------------------
-- 
GitLab