diff --git a/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp b/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp
index e95fd9d81dcfd1033d036c15c64c48c582a2e825..1416065633aab43641837e758839bc06d7d9e642 100644
--- a/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp
+++ b/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp
@@ -3,6 +3,7 @@
 #include "interfaces/portability.h"
 #include "kernel/kernel.h"
 #include "kernel/logging.h"
+#include "kernel/scheduler/tick_interrupt.h"
 #include <cstdlib>
 
 using namespace miosix;
@@ -12,10 +13,6 @@ void IRQaddToSleepingList(SleepData *x);
 extern SleepData *sleeping_list;
 }
 
-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
@@ -83,7 +80,7 @@ void __attribute__((used)) cstirqhnd()
                 csRecord.wakeup_time += CST_QUANTUM;
                 IRQaddToSleepingList(&csRecord); //It would also set the next timer interrupt
             }
-            miosix_private::ISR_preempt();
+            IRQtickInterrupt();
         }
 
     }
@@ -127,6 +124,9 @@ long long ContextSwitchTimer::getNextInterrupt() const
 long long ContextSwitchTimer::getCurrentTick() 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=IRQgetTick();
     if(interrupts) enableInterrupts();
diff --git a/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/portability.cpp b/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/portability.cpp
index 7600d8d7b9a537cb7a69d33f99c8ef7b18ec2765..b1d5c2c057b493888d5de7ebc16928069ad176b4 100644
--- a/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/portability.cpp
+++ b/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/portability.cpp
@@ -95,6 +95,7 @@ void TIM3_IRQHandler()
 
 namespace miosix_private {
 
+#ifndef USE_CSTIMER
 /**
  * \internal
  * Called by the timer interrupt, preempt to next thread
@@ -108,6 +109,7 @@ void ISR_preempt()
     IRQstackOverflowCheck();
     miosix::IRQtickInterrupt();
 }
+#endif //USE_CSTIMER
 
 /**
  * \internal
diff --git a/miosix/interfaces/cstimer.h b/miosix/interfaces/cstimer.h
index f7e3f0e945682b046acdaf7457255b5d851bf4e5..f11337824ad1eb33348d054f6e072e91e5751d38 100644
--- a/miosix/interfaces/cstimer.h
+++ b/miosix/interfaces/cstimer.h
@@ -3,7 +3,7 @@
 
 namespace miosix {
 
-#define CST_QUANTUM 42000//84000 ///FIXME: remove
+#define CST_QUANTUM 84000 ///FIXME: remove
 
 /**
  * This class is a low level interface to a hardware timer, that is used as
diff --git a/miosix/kernel/scheduler/tick_interrupt.h b/miosix/kernel/scheduler/tick_interrupt.h
index 84d096b23a9b2551ffeb8a2f97f9b05cd60cd5d0..0a2bbf2a476573a9d69cf826d1aa287c7f7eb4bb 100644
--- a/miosix/kernel/scheduler/tick_interrupt.h
+++ b/miosix/kernel/scheduler/tick_interrupt.h
@@ -29,6 +29,7 @@
 #define	TICK_INTERRUPT_H
 
 #include "config/miosix_settings.h"
+#include "interfaces/portability.h"
 #include "scheduler.h"
 
 namespace miosix {
@@ -43,6 +44,9 @@ extern bool IRQwakeThreads();///\internal Do not use outside the kernel
 
 inline void IRQtickInterrupt()
 {
+    #ifdef USE_CSTIMER
+    miosix_private::IRQstackOverflowCheck();
+    #endif //USE_CSTIMER
     bool woken=IRQwakeThreads();//Increment tick and wake threads,if any
     (void)woken; //Avoid unused variable warning.