diff --git a/miosix/kernel/pthread_private.h b/miosix/kernel/pthread_private.h
index 1c43e211c10a2f7e4e6b2b396ffa305a71c533e2..4814ee56c58d79a3333e2eb6c643e57534e2a24f 100644
--- a/miosix/kernel/pthread_private.h
+++ b/miosix/kernel/pthread_private.h
@@ -54,8 +54,8 @@ static inline void IRQdoMutexLock(pthread_mutex_t *mutex,
     }
 
     //This check is very important. Without this attempting to lock the same
-    //mutex twice won't cause a deadlock because the Thread::IRQwait() is
-    //enclosed in a while(owner!=p) which is immeditely false.
+    //mutex twice won't cause a deadlock because the wait is enclosed in a
+    //while(owner!=p) which is immeditely false.
     if(mutex->owner==p)
     {
         if(mutex->recursive>=0)
@@ -77,17 +77,8 @@ static inline void IRQdoMutexLock(pthread_mutex_t *mutex,
         mutex->last=&waiting;
     }
 
-    //The while is necessary because some other thread might call wakeup()
-    //on this thread. So the thread can wakeup also for other reasons not
-    //related to the mutex becoming free
-    while(mutex->owner!=p)
-    {
-        Thread::IRQwait();//Returns immediately
-        {
-            FastInterruptEnableLock eLock(d);
-            Thread::yield(); //Now the IRQwait becomes effective
-        }
-    }
+    //The while is necessary to protect against spurious wakeups
+    while(mutex->owner!=p) Thread::IRQenableIrqAndWait(d);
 }
 
 /**
@@ -113,8 +104,8 @@ static inline void IRQdoMutexLockToDepth(pthread_mutex_t *mutex,
     }
 
     //This check is very important. Without this attempting to lock the same
-    //mutex twice won't cause a deadlock because the Thread::IRQwait() is
-    //enclosed in a while(owner!=p) which is immeditely false.
+    //mutex twice won't cause a deadlock because the wait is enclosed in a
+    //while(owner!=p) which is immeditely false.
     if(mutex->owner==p)
     {
         if(mutex->recursive>=0)
@@ -136,17 +127,8 @@ static inline void IRQdoMutexLockToDepth(pthread_mutex_t *mutex,
         mutex->last=&waiting;
     }
 
-    //The while is necessary because some other thread might call wakeup()
-    //on this thread. So the thread can wakeup also for other reasons not
-    //related to the mutex becoming free
-    while(mutex->owner!=p)
-    {
-        Thread::IRQwait();//Returns immediately
-        {
-            FastInterruptEnableLock eLock(d);
-            Thread::yield(); //Now the IRQwait becomes effective
-        }
-    }
+    //The while is necessary to protect against spurious wakeups
+    while(mutex->owner!=p) Thread::IRQenableIrqAndWait(d);
     if(mutex->recursive>=0) mutex->recursive=depth;
 }
 
diff --git a/miosix/kernel/queue.h b/miosix/kernel/queue.h
index eb8d78e40d20da07338cf653f5bab883f0af2676..faaa432e0a20cba4e79255e384ce5f2149525443 100644
--- a/miosix/kernel/queue.h
+++ b/miosix/kernel/queue.h
@@ -1,5 +1,5 @@
 /***************************************************************************
- *   Copyright (C) 2014 by Terraneo Federico                               *
+ *   Copyright (C) 2014 - 2023 by Terraneo Federico                        *
  *                                                                         *
  *   This program is free software; you can redistribute it and/or modify  *
  *   it under the terms of the GNU General Public License as published by  *
@@ -25,8 +25,7 @@
  *   along with this program; if not, see <http://www.gnu.org/licenses/>   *
  ***************************************************************************/
 
-#ifndef QUEUE_H
-#define	QUEUE_H
+#pragma once
 
 #include "kernel.h"
 #include "error.h"
@@ -57,7 +56,7 @@ public:
     /**
      * Constructor, create a new empty queue.
      */
-    Queue() : waiting(0), numElem(0), putPos(0), getPos(0) {}
+    Queue() : waiting(nullptr), numElem(0), putPos(0), getPos(0) {}
 
     /**
      * \return true if the queue is empty
@@ -178,7 +177,7 @@ private:
     {
         if(!waiting) return;
         waiting->IRQwakeup();//Wakeup eventual waiting thread
-        waiting=0;
+        waiting=nullptr;
     }
 
     //Queue data
@@ -197,11 +196,7 @@ void Queue<T,len>::waitUntilNotEmpty()
     while(isEmpty())
     {
         waiting=Thread::IRQgetCurrentThread();
-        Thread::IRQwait();
-        {
-            FastInterruptEnableLock eLock(dLock);
-            Thread::yield();
-        }
+        Thread::IRQenableIrqAndWait(dLock);
         IRQwakeWaitingThread();
     }
 }
@@ -214,11 +209,7 @@ void Queue<T,len>::waitUntilNotFull()
     while(isFull())
     {
         waiting=Thread::IRQgetCurrentThread();
-        Thread::IRQwait();
-        {
-            FastInterruptEnableLock eLock(dLock);
-            Thread::yield();
-        }
+        Thread::IRQenableIrqAndWait(dLock);
         IRQwakeWaitingThread();
     }
 }
@@ -231,11 +222,7 @@ void Queue<T,len>::get(T& elem)
     while(isEmpty())
     {
         waiting=Thread::IRQgetCurrentThread();
-        Thread::IRQwait();
-        {
-            FastInterruptEnableLock eLock(dLock);
-            Thread::yield();
-        }
+        Thread::IRQenableIrqAndWait(dLock);
         IRQwakeWaitingThread();
     }
     numElem--;
@@ -251,11 +238,7 @@ void Queue<T,len>::put(const T& elem)
     while(isFull())
     {
         waiting=Thread::IRQgetCurrentThread();
-        Thread::IRQwait();
-        {
-            FastInterruptEnableLock eLock(dLock);
-            Thread::yield();
-        }
+        Thread::IRQenableIrqAndWait(dLock);
         IRQwakeWaitingThread();
     }
     numElem++;
@@ -550,5 +533,3 @@ template<typename T, unsigned int size> class BufferQueue<T,size,1> {};
  */
 
 } //namespace miosix
-
-#endif //QUEUE_H
diff --git a/miosix/kernel/sync.cpp b/miosix/kernel/sync.cpp
index fae7ec0e0164c5986fe4dda8327a460661ee2257..d893946010221ffce02d70b7a8d0cbef6e473da8 100644
--- a/miosix/kernel/sync.cpp
+++ b/miosix/kernel/sync.cpp
@@ -61,8 +61,8 @@ void Mutex::PKlock(PauseKernelLock& dLock)
     }
 
     //This check is very important. Without this attempting to lock the same
-    //mutex twice won't cause a deadlock because the Thread::IRQwait() is
-    //enclosed in a while(owner!=p) which is immeditely false.
+    //mutex twice won't cause a deadlock because the wait is enclosed in a
+    //while(owner!=p) which is immeditely false.
     if(owner==p)
     {
         if(recursiveDepth>=0)
@@ -80,7 +80,7 @@ void Mutex::PKlock(PauseKernelLock& dLock)
     //Handle priority inheritance
     if(p->mutexWaiting!=nullptr) errorHandler(UNEXPECTED);
     p->mutexWaiting=this;
-    if (owner->getPriority().mutexLessOp(p->getPriority()))
+    if(owner->getPriority().mutexLessOp(p->getPriority()))
     {
         Thread *walk=owner;
         for(;;)
@@ -93,24 +93,8 @@ void Mutex::PKlock(PauseKernelLock& dLock)
         }
     }
 
-    //The while is necessary because some other thread might call wakeup()
-    //on this thread. So the thread can wakeup also for other reasons not
-    //related to the mutex becoming free
-    while(owner!=p)
-    {
-        //Wait can only be called with kernel started, while IRQwait can
-        //only be called with interupts disabled, so that's why interrupts
-        //are disabled
-        {
-            FastInterruptDisableLock l;
-            Thread::IRQwait();//Return immediately
-        }
-        {
-            RestartKernelLock eLock(dLock);
-            //Now the IRQwait becomes effective
-            Thread::yield();
-        }
-    }
+    //The while is necessary to protect against spurious wakeups
+    while(owner!=p) Thread::PKrestartKernelAndWait(dLock);
 }
 
 void Mutex::PKlockToDepth(PauseKernelLock& dLock, unsigned int depth)
@@ -130,8 +114,8 @@ void Mutex::PKlockToDepth(PauseKernelLock& dLock, unsigned int depth)
     }
 
     //This check is very important. Without this attempting to lock the same
-    //mutex twice won't cause a deadlock because the Thread::IRQwait() is
-    //enclosed in a while(owner!=p) which is immeditely false.
+    //mutex twice won't cause a deadlock because the wait is enclosed in a
+    //while(owner!=p) which is immeditely false.
     if(owner==p)
     {
         if(recursiveDepth>=0)
@@ -149,7 +133,7 @@ void Mutex::PKlockToDepth(PauseKernelLock& dLock, unsigned int depth)
     //Handle priority inheritance
     if(p->mutexWaiting!=nullptr) errorHandler(UNEXPECTED);
     p->mutexWaiting=this;
-    if (owner->getPriority().mutexLessOp(p->getPriority()))
+    if(owner->getPriority().mutexLessOp(p->getPriority()))
     {
         Thread *walk=owner;
         for(;;)
@@ -162,24 +146,8 @@ void Mutex::PKlockToDepth(PauseKernelLock& dLock, unsigned int depth)
         }
     }
 
-    //The while is necessary because some other thread might call wakeup()
-    //on this thread. So the thread can wakeup also for other reasons not
-    //related to the mutex becoming free
-    while(owner!=p)
-    {
-        //Wait can only be called with kernel started, while IRQwait can
-        //only be called with interupts disabled, so that's why interrupts
-        //are disabled
-        {
-            FastInterruptDisableLock l;
-            Thread::IRQwait();//Return immediately
-        }
-        {
-            RestartKernelLock eLock(dLock);
-            //Now the IRQwait becomes effective
-            Thread::yield();
-        }
-    }
+    //The while is necessary to protect against spurious wakeups
+    while(owner!=p) Thread::PKrestartKernelAndWait(dLock);
     if(recursiveDepth>=0) recursiveDepth=depth;
 }