diff --git a/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp b/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp
index f56e6d526c36339b160e7ad5b0283de8f6bf1934..4b8ef538c66fdd9043630f8f75dea1de30b6a216 100644
--- a/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp
+++ b/miosix/arch/cortexM4_stm32f4/common/interfaces-impl/cstimer.cpp
@@ -67,7 +67,7 @@ void __attribute__((used)) cstirqhnd()
         if(ms32time==ms32chkp || lateIrq)
         {
             lateIrq=false;
-            IRQtimerInterrupt(tc->tick2ns(nextInterrupt()));
+            IRQtimerInterrupt(tc->tick2ns(IRQgetTick()));
         }
 
     }
diff --git a/miosix/kernel/kernel.cpp b/miosix/kernel/kernel.cpp
index ce336f3dbb75b46b3670b86daf134abfd9e649bd..13018e2c609cd80366f85cadf160533aa185390a 100644
--- a/miosix/kernel/kernel.cpp
+++ b/miosix/kernel/kernel.cpp
@@ -41,6 +41,7 @@
 #include "timeconversion.h"
 #include <stdexcept>
 #include <algorithm>
+#include <limits>
 #include <string.h>
 #include <reent.h>
 
@@ -64,7 +65,9 @@ volatile Thread *cur=NULL;///<\internal Thread currently running
 ///\internal True if there are threads in the DELETED status. Used by idle thread
 static volatile bool exist_deleted=false;
 
-static IntrusiveList<SleepData> *sleepingList=nullptr;///list of sleeping threads
+IntrusiveList<SleepData> *sleepingList=nullptr;///list of sleeping threads
+///Contains head of the sleeping list in terms of timer's ticks
+long long firstSleepItemTicks = std::numeric_limits<long long>::max();
 
 #ifndef USE_CSTIMER
 static volatile long long tick=0;///<\internal Kernel tick
@@ -84,15 +87,6 @@ static unsigned char interruptDisableNesting=0;
 
 static int deepSleepCounter = 0;
 
-#ifdef USE_CSTIMER
-
-static ContextSwitchTimer *timer = nullptr; // FIXME please
-
-/// Used for context switches with the high resolution timer
-static SleepData *csRecord=nullptr;
-
-#endif //USE_CSTIMER
-
 #ifdef WITH_PROCESSES
 
 /// The proc field of the Thread class for kernel threads points to this object
@@ -208,23 +202,12 @@ void deepSleepUnlock()
 #endif
 void startKernel()
 {
-    #ifdef USE_CSTIMER
-    timer = &ContextSwitchTimer::instance();
-    sleepingList = new(std::nothrow) IntrusiveList<SleepData>;
-        csRecord = new(std::nothrow) SleepData;
-    if(sleepingList==nullptr || csRecord==nullptr)
-    {
-        errorHandler(OUT_OF_MEMORY);
-        return;
-    }
-    #else //USE_CSTIMER
     sleepingList = new(std::nothrow) IntrusiveList<SleepData>;
     if(sleepingList==nullptr)
     {
         errorHandler(OUT_OF_MEMORY);
         return;
     }
-    #endif //USE_CSTIMER
     #ifdef WITH_PROCESSES
     try {
         kernel=new ProcessBase;
@@ -260,13 +243,6 @@ void startKernel()
     setCReentrancyCallback(Thread::getCReent);
     
     // Dispatch the task to the architecture-specific function
-    #ifdef USE_CSTIMER
-    // Set the first checkpoint interrupt
-    csRecord->p = nullptr;
-    csRecord->wakeup_time = preemptionPeriodNs;
-    sleepingList->push_front(csRecord);
-    timer->IRQsetNextInterrupt(tc->ns2tick(preemptionPeriodNs));
-    #endif //USE_CSTIMER
     miosix_private::IRQportableStartKernel();
     kernel_started=true;
     miosix_private::IRQportableFinishKernelStartup();
@@ -294,7 +270,7 @@ long long getTick()
         if(a==b) return a;
     }
     #else //USE_CSTIMER
-    return tc->tick2ns(timer->getCurrentTick())/preemptionPeriodNs;
+    return tc->tick2ns(ContextSwitchTimer::instance().getCurrentTick())/preemptionPeriodNs;
     #endif //USE_CSTIMER
 }
 
@@ -318,42 +294,9 @@ void IRQaddToSleepingList(SleepData *x)
         while (it != sleepingList->end() && (*it)->wakeup_time < x->wakeup_time ) ++it;
         sleepingList->insert(it,x);
     }
-#ifdef USE_CSTIMER
-    //Upon any change to the sleepingList the ContextSwitchTimer should have
-    //its interrupt set to the head of the list in order to keep it sync with
-    //the list
-    timer->IRQsetNextInterrupt(tc->ns2tick(sleepingList->front()->wakeup_time));
-#endif
+    firstSleepItemTicks = tc->ns2tick(sleepingList->front()->wakeup_time);
 }
 
-/**
- * \internal
- * \return 
- */
-void IRQsetNextPreemption(long long preemptionTime)
-{
-#ifdef USE_CSTIMER
-    // Remove all the preemption points from the list
-    IntrusiveList<SleepData>::iterator it(csRecord);
-    sleepingList->erase(it);
-    
-    //This piece of code is a duplication of IRQaddToSleepingList
-    //that is in-lined for performance issues
-    csRecord->wakeup_time = preemptionTime;
-    if(sleepingList->empty() || sleepingList->front()->wakeup_time >= preemptionTime)
-    {
-        sleepingList->push_front(csRecord);
-    } else {
-        auto it = sleepingList->begin();
-        while (it != sleepingList->end() && (*it)->wakeup_time < preemptionTime ) ++it;
-        sleepingList->insert(it,csRecord);
-    }
-    //Upon any change to the sleepingList the ContextSwitchTimer should have
-    //its interrupt set to the head of the list in order to keep it sync with
-    //the list
-    timer->IRQsetNextInterrupt(tc->ns2tick(sleepingList->front()->wakeup_time));
-#endif
-}
 /**
  * \internal
  * Called @ every tick to check if it's time to wake some thread.
@@ -388,7 +331,8 @@ bool IRQwakeThreads(long long currentTick)
     #else //USE_CSTIMER
 
     //If no item in list, return
-    if(sleepingList->empty()) return false;
+    if(sleepingList->empty())
+        return false;
     
     bool result=false;
     //Since list is sorted, if we don't need to wake the first element
@@ -403,6 +347,10 @@ bool IRQwakeThreads(long long currentTick)
             result = true;
         }
     }
+    if(sleepingList->empty()) 
+        firstSleepItemTicks = std::numeric_limits<long long>::max();
+    else
+        firstSleepItemTicks = tc->ns2tick(sleepingList->front()->wakeup_time);
     return result;
     #endif //USE_CSTIMER
 }
@@ -468,7 +416,7 @@ bool Thread::testTerminate()
 }
 
 #ifdef USE_CSTIMER
-void Thread::nanoSleep(unsigned int ns)
+void Thread::nanoSleep(long long ns)
 {
     if(ns==0) return; //TODO: should be (ns &lt; resolution + epsilon)
     //TODO: Mutual Exclusion issue
@@ -501,7 +449,7 @@ void Thread::nanoSleepUntil(long long absoluteTime)
 void Thread::sleep(unsigned int ms)
 {
 #ifdef USE_CSTIMER
-    nanoSleep(ms * 1000000);
+    nanoSleep(mul32x32to64(ms,1000000));
 #else
     if(ms==0) return;
     //pauseKernel() here is not enough since even if the kernel is stopped
diff --git a/miosix/kernel/kernel.h b/miosix/kernel/kernel.h
index 125592e2292cb5e49df343badbd3af8b25831fff..da9d602f93570231eb790b7c4beec5010048bdd8 100644
--- a/miosix/kernel/kernel.h
+++ b/miosix/kernel/kernel.h
@@ -519,7 +519,7 @@ public:
      * CANNOT be called when the kernel is paused.
      */
     static void sleep(unsigned int ms);
-    static void nanoSleep(unsigned int ns);
+    static void nanoSleep(long long ns);
     /**
      * Put the thread to sleep until the specified absolute time is reached.
      * If the time is in the past, returns immediately.
diff --git a/miosix/kernel/scheduler/priority/priority_scheduler.cpp b/miosix/kernel/scheduler/priority/priority_scheduler.cpp
index 231c79f7994c1fabe0e72e689ae60066a4b4f6f2..1c2395120aaa93effa8b85dd35a8c4d31f7ae2ed 100644
--- a/miosix/kernel/scheduler/priority/priority_scheduler.cpp
+++ b/miosix/kernel/scheduler/priority/priority_scheduler.cpp
@@ -28,6 +28,8 @@
 #include "priority_scheduler.h"
 #include "kernel/error.h"
 #include "kernel/process.h"
+#include "interfaces/cstimer.h"
+#include "kernel/timeconversion.h"
 
 #ifdef SCHED_TYPE_PRIORITY
 namespace miosix {
@@ -35,6 +37,9 @@ namespace miosix {
 //These are defined in kernel.cpp
 extern volatile Thread *cur;
 extern volatile int kernel_running;
+static ContextSwitchTimer& timer = ContextSwitchTimer::instance();
+extern long long firstSleepItemTicks;
+extern IntrusiveList<SleepData> *sleepingList;
 
 //
 // class PriorityScheduler
@@ -196,6 +201,15 @@ void PriorityScheduler::IRQsetIdleThread(Thread *idleThread)
     idle=idleThread;
 }
 
+static void setNextPreemption(){
+    static long long preemptionPeriodTicks = tc->ns2tick(preemptionPeriodNs);
+    long long nextPeriodicPreemption = timer.IRQgetCurrentTick() + preemptionPeriodTicks;   
+    if (firstSleepItemTicks < nextPeriodicPreemption )
+        timer.IRQsetNextInterrupt(firstSleepItemTicks);
+    else
+        timer.IRQsetNextInterrupt(nextPeriodicPreemption);
+}
+
 unsigned int PriorityScheduler::IRQfindNextThread()
 {
     if(kernel_running!=0) return preemptionPeriodNs;//If kernel is paused, do nothing
@@ -225,6 +239,7 @@ unsigned int PriorityScheduler::IRQfindNextThread()
                 //Rotate to next thread so that next time the list is walked
                 //a different thread, if available, will be chosen first
                 thread_list[i]=temp;
+                setNextPreemption();
                 return preemptionPeriodNs;
             } else temp=temp->schedData.next;
             if(temp==thread_list[i]->schedData.next) break;
@@ -236,6 +251,7 @@ unsigned int PriorityScheduler::IRQfindNextThread()
     #ifdef WITH_PROCESSES
     MPUConfiguration::IRQdisable();
     #endif //WITH_PROCESSES
+    setNextPreemption();
     return preemptionPeriodNs;
 }
 
diff --git a/miosix/kernel/scheduler/timer_interrupt.h b/miosix/kernel/scheduler/timer_interrupt.h
index 6dd9b0cb2bc1d2ec14d18cf8fb87d7e5d4d73812..7de8fd6ff580cc782497dd5348dcd38b9ca03731 100644
--- a/miosix/kernel/scheduler/timer_interrupt.h
+++ b/miosix/kernel/scheduler/timer_interrupt.h
@@ -40,7 +40,6 @@ namespace miosix {
 extern volatile int kernel_running;///\internal Do not use outside the kernel
 extern volatile bool tick_skew;///\internal Do not use outside the kernel
 extern volatile Thread *cur;///\internal Do not use outside the kernel
-extern void IRQsetNextPreemption(long long preemptionTime); ///\internal Do not use outside the kernel
 extern bool IRQwakeThreads(long long currentTick);///\internal Do not use outside the kernel
 
 /**
@@ -51,10 +50,8 @@ inline void IRQtimerInterrupt(long long currentTick)
     miosix_private::IRQstackOverflowCheck();
     IRQwakeThreads(currentTick);
     
-    unsigned int burst = Scheduler::IRQfindNextThread();//If the kernel is running, preempt
+    Scheduler::IRQfindNextThread();//If the kernel is running, preempt
     if(kernel_running!=0) tick_skew=true;
-   
-    IRQsetNextPreemption(currentTick+burst);
     
     #ifndef SCHED_TYPE_PRIORITY
     //TODO: the old tick scheduler called this function periodically,
diff --git a/miosix/kernel/timeconversion.cpp b/miosix/kernel/timeconversion.cpp
index e44805b6a3035d36638c36b414bbddcf9de2d2be..58d3be91feaaf75bfb8c10c9121145bee625d1ef 100644
--- a/miosix/kernel/timeconversion.cpp
+++ b/miosix/kernel/timeconversion.cpp
@@ -56,18 +56,6 @@ static inline unsigned int lo(unsigned long long x) { return x & 0xffffffff; }
  */
 static inline unsigned int hi(unsigned long long x) { return x>>32; }
 
-/**
- * \param a 32 bit unsigned number
- * \param b 32 bit unsigned number
- * \return a * b as a 64 unsigned number 
- */
-static inline unsigned long long mul32x32to64(unsigned int a, unsigned int b)
-{
-    //Casts are to produce a 64 bit result. Compiles to a single asm instruction
-    //in processors having 32x32 multiplication with 64 bit result
-    return static_cast<unsigned long long>(a)*static_cast<unsigned long long>(b);
-}
-
 unsigned long long mul64x32d32(unsigned long long a,
                                unsigned int bi, unsigned int bf)
 {
diff --git a/miosix/kernel/timeconversion.h b/miosix/kernel/timeconversion.h
index ee3f413af00188e522321f115ff2bea179e8c12d..9dad209739efdad4a149ff64f2e9d578efda5e4b 100644
--- a/miosix/kernel/timeconversion.h
+++ b/miosix/kernel/timeconversion.h
@@ -29,6 +29,18 @@
 #define TIMECONVERSION_H
 
 namespace miosix {
+    
+/**
+ * \param a 32 bit unsigned number
+ * \param b 32 bit unsigned number
+ * \return a * b as a 64 unsigned number 
+ */
+inline unsigned long long mul32x32to64(unsigned int a, unsigned int b)
+{
+    //Casts are to produce a 64 bit result. Compiles to a single asm instruction
+    //in processors having 32x32 multiplication with 64 bit result
+    return static_cast<unsigned long long>(a)*static_cast<unsigned long long>(b);
+}
 
 /**
  * Multiplication between a 64 bit integer and a 32.32 fixed point number,