diff --git a/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp b/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp
index e135f05234f2bf8c9e5492e6cb506dfa41fb790a..e95fd9d81dcfd1033d036c15c64c48c582a2e825 100644
--- a/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp
+++ b/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp
@@ -16,6 +16,8 @@ namespace miosix_private {
 void ISR_preempt();
 }
 
+//TODO: comment me
+static const uint32_t threshold = 0xffffffff/4*3;
 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;
@@ -26,6 +28,16 @@ static inline long long nextInterrupt()
     return ms32chkp | TIM2->CCR1;
 }
 
+static inline long long IRQgetTick()
+{
+    //If overflow occurs while interrupts disabled
+    //counter should be checked before rollover interrupt flag
+    uint32_t counter = TIM2->CNT;
+    if((TIM2->SR & TIM_SR_UIF) && counter < threshold)
+        return (ms32time | static_cast<long long>(counter)) + 0x100000000ll;
+    return ms32time | static_cast<long long>(counter);
+}
+
 void __attribute__((naked)) TIM2_IRQHandler()
 {
     saveContext();
@@ -35,7 +47,6 @@ void __attribute__((naked)) TIM2_IRQHandler()
 
 void __attribute__((used)) cstirqhnd()
 {
-    //IRQbootlog("TIM2-IRQ\r\n");
     if(TIM2->SR & TIM_SR_CC1IF || lateIrq)
     {
         //Checkpoint met
@@ -46,7 +57,7 @@ void __attribute__((used)) cstirqhnd()
         if(ms32time==ms32chkp || lateIrq)
         {
             lateIrq=false;
-            long long tick = ContextSwitchTimer::instance().getCurrentTick();
+            long long tick = IRQgetTick();
             
             //Add next context switch time to the sleeping list iff this is a
             //context switch
@@ -101,7 +112,7 @@ void ContextSwitchTimer::IRQsetNextInterrupt(long long tick)
 {
     ms32chkp = tick & 0xFFFFFFFF00000000;
     TIM2->CCR1 = static_cast<unsigned int>(tick & 0xFFFFFFFF);
-    if(getCurrentTick() > nextInterrupt())
+    if(IRQgetTick() > nextInterrupt())
     {
         NVIC_SetPendingIRQ(TIM2_IRQn);
         lateIrq=true;
@@ -117,23 +128,16 @@ long long ContextSwitchTimer::getCurrentTick() const
 {
     bool interrupts=areInterruptsEnabled();
     if(interrupts) disableInterrupts();
-    //If overflow occurs while interrupts disabled
-    /*
-     * counter should be checked before rollover interrupt flag
-     *
-     */
-    uint32_t counter = TIM2->CNT;
-    if((TIM2->SR & TIM_SR_UIF) && counter < 0x80000000)
-    {
-        long long result=(ms32time | counter) + 0x100000000ll;
-        if(interrupts) enableInterrupts();
-        return result;
-    }
-    long long result=ms32time | counter;
+    long long result=IRQgetTick();
     if(interrupts) enableInterrupts();
     return result;
 }
 
+long long ContextSwitchTimer::IRQgetCurrentTick() const
+{
+    return IRQgetTick();
+}
+
 ContextSwitchTimer::~ContextSwitchTimer() {}
 
 ContextSwitchTimer::ContextSwitchTimer()
diff --git a/miosix/interfaces/cstimer.h b/miosix/interfaces/cstimer.h
index 0e4ded1800680763cb1847c797319edf5b95c7a5..f7e3f0e945682b046acdaf7457255b5d851bf4e5 100644
--- a/miosix/interfaces/cstimer.h
+++ b/miosix/interfaces/cstimer.h
@@ -33,10 +33,18 @@ public:
     
     /**
      * Could be call both when the interrupts are enabled/disabled!
+     * TODO: investigate if it's possible to remove the possibility to call
+     * this with IRQ disabled and use IRQgetCurrentTick() instead
      * \return the current tick count of the timer
      */
     long long getCurrentTick() const;
     
+    /**
+     * \return the current tick count of the timer.
+     * Can only be called with interrupts disabled or within an IRQ
+     */
+    long long IRQgetCurrentTick() const;
+    
     /**
      * \return the timer frequency in Hz
      */