miosix/CMakeLists.txt

@@ -68,6 +68,7 @@ foreach(OPT_BOARD ${BOARDS})
         kernel/process_pool.cpp
         kernel/timeconversion.cpp
         kernel/SystemMap.cpp
+        kernel/intrusive.cpp
         kernel/scheduler/priority/priority_scheduler.cpp
         kernel/scheduler/control/control_scheduler.cpp
         kernel/scheduler/edf/edf_scheduler.cpp

miosix/Makefile

@@ -23,6 +23,7 @@ kernel/process.cpp                                                         \
 kernel/process_pool.cpp                                                    \
 kernel/timeconversion.cpp                                                  \
 kernel/SystemMap.cpp                                                       \
+kernel/intrusive.cpp                                                       \
 kernel/scheduler/priority/priority_scheduler.cpp                           \
 kernel/scheduler/control/control_scheduler.cpp                             \
 kernel/scheduler/edf/edf_scheduler.cpp                                     \

miosix/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4/interfaces-impl/bsp.cpp

@@ -333,7 +333,7 @@ void IRQbspInit()
     using namespace radio;
     cs::mode(Mode::OUTPUT);
     cs::getPin().high();
-    // dio0::mode(Mode::INPUT);
+    dio0::mode(Mode::INPUT);
     dio1::mode(Mode::INPUT);
     dio3::mode(Mode::INPUT);
     tx_enable::mode(Mode::OUTPUT);
@@ -349,7 +349,7 @@ void IRQbspInit()
     nosecone_detach::mode(Mode::INPUT);
 
     using namespace actuators;
-    buzzer::mode(Mode::ALTERNATE_OD_PULL_DOWN);
+    buzzer::mode(Mode::ALTERNATE_PULL_DOWN);
 
     DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(new STM32Serial(
         defaultSerial, defaultSerialSpeed, STM32Serial::NOFLOWCTRL)));

miosix/kernel/intrusive.cpp

+/***************************************************************************
+ *   Copyright (C) 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  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   As a special exception, if other files instantiate templates or use   *
+ *   macros or inline functions from this file, or you compile this file   *
+ *   and link it with other works to produce a work based on this file,    *
+ *   this file does not by itself cause the resulting work to be covered   *
+ *   by the GNU General Public License. However the source code for this   *
+ *   file must still be made available in accordance with the GNU General  *
+ *   Public License. This exception does not invalidate any other reasons  *
+ *   why a work based on this file might be covered by the GNU General     *
+ *   Public License.                                                       *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, see <http://www.gnu.org/licenses/>   *
+ ***************************************************************************/
+
+#ifndef TEST_ALGORITHM
+
+#include "intrusive.h"
+
+#else //TEST_ALGORITHM
+
+#include <iostream>
+#include <cassert>
+
+// Unused stubs as the test code only tests IntrusiveList
+inline int atomicSwap(volatile int*, int) { return 0; }
+void *atomicFetchAndIncrement(void *const volatile*, int, int) { return nullptr; }
+
+//C++ glassbox testing trick
+#define private public
+#define protected public
+#include "intrusive.h"
+#undef private
+#undef public
+
+using namespace std;
+using namespace miosix;
+
+#endif //TEST_ALGORITHM
+
+namespace miosix {
+
+//
+// class IntrusiveListBase
+//
+
+void IntrusiveListBase::push_back(IntrusiveListItem *item)
+{
+    #ifdef INTRUSIVE_LIST_ERROR_CHECK
+    if((head!=nullptr) ^ (tail!=nullptr)) fail();
+    if(!empty() && head==tail && (head->prev || head->next)) fail();
+    if(item->prev!=nullptr || item->next!=nullptr) fail();
+    #endif //INTRUSIVE_LIST_ERROR_CHECK
+    if(empty()) head=item;
+    else {
+        item->prev=tail;
+        tail->next=item;
+    }
+    tail=item;
+}
+
+void IntrusiveListBase::pop_back()
+{
+    #ifdef INTRUSIVE_LIST_ERROR_CHECK
+    if(head==nullptr || tail==nullptr) fail();
+    if(!empty() && head==tail && (head->prev || head->next)) fail();
+    #endif //INTRUSIVE_LIST_ERROR_CHECK
+    IntrusiveListItem *removedItem=tail;
+    tail=removedItem->prev;
+    if(tail!=nullptr)
+    {
+        tail->next=nullptr;
+        removedItem->prev=nullptr;
+    } else head=nullptr;
+}
+
+void IntrusiveListBase::push_front(IntrusiveListItem *item)
+{
+    #ifdef INTRUSIVE_LIST_ERROR_CHECK
+    if((head!=nullptr) ^ (tail!=nullptr)) fail();
+    if(!empty() && head==tail && (head->prev || head->next)) fail();
+    if(item->prev!=nullptr || item->next!=nullptr) fail();
+    #endif //INTRUSIVE_LIST_ERROR_CHECK
+    if(empty()) tail=item;
+    else {
+        head->prev=item;
+        item->next=head;
+    }
+    head=item;
+}
+
+void IntrusiveListBase::pop_front()
+{
+    #ifdef INTRUSIVE_LIST_ERROR_CHECK
+    if(head==nullptr || tail==nullptr) fail();
+    if(!empty() && head==tail && (head->prev || head->next)) fail();
+    #endif //INTRUSIVE_LIST_ERROR_CHECK
+    IntrusiveListItem *removedItem=head;
+    head=removedItem->next;
+    if(head!=nullptr)
+    {
+        head->prev=nullptr;
+        removedItem->next=nullptr;
+    } else tail=nullptr;
+}
+
+void IntrusiveListBase::insert(IntrusiveListItem *cur, IntrusiveListItem *item)
+{
+    #ifdef INTRUSIVE_LIST_ERROR_CHECK
+    if((head!=nullptr) ^ (tail!=nullptr)) fail();
+    if(!empty() && head==tail && (head->prev || head->next)) fail();
+    if(cur!=nullptr)
+    {
+        if(cur->prev==nullptr && cur!=head) fail();
+        if(cur->next==nullptr && cur!=tail) fail();
+    }
+    if(item->prev!=nullptr || item->next!=nullptr) fail();
+    #endif //INTRUSIVE_LIST_ERROR_CHECK
+    item->next=cur;
+    if(cur!=nullptr)
+    {
+        item->prev=cur->prev;
+        cur->prev=item;
+    } else {
+        item->prev=tail;
+        tail=item;
+    }
+    if(item->prev!=nullptr) item->prev->next=item;
+    else head=item;
+}
+
+IntrusiveListItem *IntrusiveListBase::erase(IntrusiveListItem *cur)
+{
+    #ifdef INTRUSIVE_LIST_ERROR_CHECK
+    if(head==nullptr || tail==nullptr) fail();
+    if(!empty() && head==tail && (head->prev || head->next)) fail();
+    if(cur==nullptr) fail();
+    if(cur->prev==nullptr && cur!=head) fail();
+    if(cur->next==nullptr && cur!=tail) fail();
+    #endif //INTRUSIVE_LIST_ERROR_CHECK
+    if(cur->prev!=nullptr) cur->prev->next=cur->next;
+    else head=cur->next;
+    if(cur->next!=nullptr) cur->next->prev=cur->prev;
+    else tail=cur->prev;
+    auto result=cur->next;
+    cur->prev=nullptr;
+    cur->next=nullptr;
+    return result;
+}
+
+#ifdef INTRUSIVE_LIST_ERROR_CHECK
+#warning "INTRUSIVE_LIST_ERROR_CHECK should not be enabled in release builds"
+void IntrusiveListBase::fail()
+{
+    #ifndef TEST_ALGORITHM
+    errorHandler(UNEXPECTED);
+    #else //TEST_ALGORITHM
+    assert(false);
+    #endif //TEST_ALGORITHM
+}
+#endif //INTRUSIVE_LIST_ERROR_CHECK
+
+} //namespace miosix
+
+//Testsuite for IntrusiveList. Compile with:
+//g++ -DTEST_ALGORITHM -DINTRUSIVE_LIST_ERROR_CHECK -fsanitize=address -m32
+//    -std=c++14 -Wall -O2 -o test intrusive.cpp; ./test
+#ifdef TEST_ALGORITHM
+
+void emptyCheck(IntrusiveListItem& x)
+{
+    //Glass box check
+    assert(x.next==nullptr); assert(x.prev==nullptr);
+}
+
+void emptyCheck(IntrusiveList<IntrusiveListItem>& list)
+{
+    //Black box check
+    assert(list.empty());
+    assert(list.begin()==list.end());
+    //Glass box check
+    assert(list.head==nullptr);
+    assert(list.tail==nullptr);
+}
+
+void oneItemCheck(IntrusiveList<IntrusiveListItem>& list, IntrusiveListItem& a)
+{
+    IntrusiveList<IntrusiveListItem>::iterator it;
+    //Black box check
+    assert(list.empty()==false);
+    assert(list.front()==&a);
+    assert(list.back()==&a);
+    assert(list.begin()!=list.end());
+    assert(*list.begin()==&a);
+    assert(++list.begin()==list.end());
+    it=list.begin(); it++; assert(it==list.end());
+    assert(--list.end()==list.begin());
+    it=list.end(); it--; assert(it==list.begin());
+    //Glass box check
+    assert(list.head==&a);
+    assert(list.tail==&a);
+    assert(a.prev==nullptr);
+    assert(a.next==nullptr);
+}
+
+void twoItemCheck(IntrusiveList<IntrusiveListItem>& list, IntrusiveListItem& a,
+                  IntrusiveListItem& b)
+{
+    IntrusiveList<IntrusiveListItem>::iterator it;
+    //Black box check
+    assert(list.empty()==false);
+    assert(list.front()==&a);
+    assert(list.back()==&b);
+    assert(list.begin()!=list.end());
+    it=list.begin();
+    assert(*it++==&a);
+    assert(*it++==&b);
+    assert(it==list.end());
+    it=list.begin();
+    assert(*it==&a);
+    ++it;
+    assert(*it==&b);
+    ++it;
+    assert(it==list.end());
+    it=list.end();
+    it--;
+    assert(*it==&b);
+    it--;
+    assert(*it==&a);
+    assert(it==list.begin());
+    it=list.end();
+    assert(*--it==&b);
+    assert(*--it==&a);
+    assert(it==list.begin());
+    //Glass box check
+    assert(list.head==&a);
+    assert(list.tail==&b);
+    assert(a.prev==nullptr);
+    assert(a.next==&b);
+    assert(b.prev==&a);
+    assert(b.next==nullptr);
+}
+
+void threeItemCheck(IntrusiveList<IntrusiveListItem>& list, IntrusiveListItem& a,
+                  IntrusiveListItem& b, IntrusiveListItem& c)
+{
+    IntrusiveList<IntrusiveListItem>::iterator it;
+    //Black box check
+    assert(list.empty()==false);
+    assert(list.front()==&a);
+    assert(list.back()==&c);
+    assert(list.begin()!=list.end());
+    it=list.begin();
+    assert(*it++==&a);
+    assert(*it++==&b);
+    assert(*it++==&c);
+    assert(it==list.end());
+    it=list.begin();
+    assert(*it==&a);
+    ++it;
+    assert(*it==&b);
+    ++it;
+    assert(*it==&c);
+    ++it;
+    assert(it==list.end());
+    it=list.end();
+    it--;
+    assert(*it==&c);
+    it--;
+    assert(*it==&b);
+    it--;
+    assert(*it==&a);
+    assert(it==list.begin());
+    it=list.end();
+    assert(*--it==&c);
+    assert(*--it==&b);
+    assert(*--it==&a);
+    assert(it==list.begin());
+    //Glass box check
+    assert(list.head==&a);
+    assert(list.tail==&c);
+    assert(a.prev==nullptr);
+    assert(a.next==&b);
+    assert(b.prev==&a);
+    assert(b.next==&c);
+    assert(c.prev==&b);
+    assert(c.next==nullptr);
+}
+
+int main()
+{
+    IntrusiveListItem a,b,c;
+    IntrusiveList<IntrusiveListItem> list;
+    emptyCheck(a);
+    emptyCheck(b);
+    emptyCheck(c);
+    emptyCheck(list);
+
+    //
+    // Testing push_back / pop_back
+    //
+    list.push_back(&a);
+    oneItemCheck(list,a);
+    list.push_back(&b);
+    twoItemCheck(list,a,b);
+    list.pop_back();
+    oneItemCheck(list,a);
+    emptyCheck(b);
+    list.pop_back();
+    emptyCheck(list);
+    emptyCheck(a);
+
+    //
+    // Testing push_front / pop_front
+    //
+    list.push_front(&a);
+    oneItemCheck(list,a);
+    list.push_front(&b);
+    twoItemCheck(list,b,a);
+    list.pop_front();
+    oneItemCheck(list,a);
+    emptyCheck(b);
+    list.pop_front();
+    emptyCheck(list);
+    emptyCheck(a);
+
+    //
+    // Testing insert / erase
+    //
+    list.insert(list.end(),&a);
+    oneItemCheck(list,a);
+    list.insert(list.end(),&b);
+    twoItemCheck(list,a,b);
+    list.erase(++list.begin()); //Erase second item first
+    oneItemCheck(list,a);
+    emptyCheck(b);
+    list.erase(list.begin());   //Erase only item
+    emptyCheck(list);
+    emptyCheck(a);
+    list.insert(list.begin(),&a);
+    oneItemCheck(list,a);
+    list.insert(list.begin(),&b);
+    twoItemCheck(list,b,a);
+    list.erase(list.begin());   //Erase first item first
+    oneItemCheck(list,a);
+    emptyCheck(b);
+    list.erase(list.begin());   //Erase only item
+    emptyCheck(list);
+    emptyCheck(a);
+    list.insert(list.end(),&a);
+    oneItemCheck(list,a);
+    list.insert(list.end(),&c);
+    twoItemCheck(list,a,c);
+    list.insert(++list.begin(),&b); //Insert in the middle
+    threeItemCheck(list,a,b,c);
+    list.erase(++list.begin());     //Erase in the middle
+    twoItemCheck(list,a,c);
+    emptyCheck(b);
+    list.erase(list.begin());
+    oneItemCheck(list,c);
+    emptyCheck(a);
+    list.erase(list.begin());
+    emptyCheck(list);
+    emptyCheck(c);
+
+    //
+    // Testing removeFast
+    //
+    assert(list.removeFast(&a)==false); //Not present, list empty
+    emptyCheck(list);
+    emptyCheck(a);
+    list.push_front(&a);
+    assert(list.removeFast(&b)==false); //Not present, list not empty
+    oneItemCheck(list,a);
+    emptyCheck(b);
+    assert(list.removeFast(&a)==true); //Present, only element
+    emptyCheck(list);
+    emptyCheck(a);
+    list.push_front(&c);
+    list.push_front(&b);
+    list.push_front(&a);
+    assert(list.removeFast(&a)==true); //Present, at list head
+    twoItemCheck(list,b,c);
+    emptyCheck(a);
+    list.push_front(&a);
+    assert(list.removeFast(&b)==true); //Present, at in the middle
+    twoItemCheck(list,a,c);
+    emptyCheck(b);
+    assert(list.removeFast(&c)==true); //Present, at list tail
+    oneItemCheck(list,a);
+    emptyCheck(c);
+    list.pop_front(); //Just to end with empty list
+    emptyCheck(list);
+    emptyCheck(a);
+
+    cout<<"Test passed"<<endl;
+    return 0;
+}
+
+#endif //TEST_ALGORITHM

miosix/kernel/intrusive.h

 /***************************************************************************
- *   Copyright (C) 2013 by Terraneo Federico                               *
+ *   Copyright (C) 2013-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,17 +25,19 @@
  *   along with this program; if not, see <http://www.gnu.org/licenses/>   *
  ***************************************************************************/
 
-#ifndef INTRUSIVE_H
-#define INTRUSIVE_H
+#pragma once
 
 #include <ostream>
 #include <cstddef>
 #include <cassert>
+#include <type_traits>
+#ifndef TEST_ALGORITHM
 #include "interfaces/atomic_ops.h"
+#include "error.h"
+#endif //TEST_ALGORITHM
 
-#if __cplusplus > 199711L
-#include <type_traits>
-#endif // c++11
+//Only enable when testing code that uses IntrusiveList
+//#define INTRUSIVE_LIST_ERROR_CHECK
 
 namespace miosix {
 
@@ -181,7 +183,7 @@ public:
     /**
      * Default constructor
      */
-    intrusive_ref_ptr() : object(0) {}
+    intrusive_ref_ptr() : object(nullptr) {}
     
     /**
      * Constructor, with raw pointer
@@ -280,6 +282,14 @@ public:
         return object==0 ? 0 : &SafeBoolStruct::b;
     }
 
+    /**
+     * \return true if the object contains a callback
+     */
+    explicit operator bool() const
+    {
+        return object!=nullptr;
+    }
+
     /**
      * Swap the managed object with another intrusive_ref_ptr
      * \param rhs the other smart pointer
@@ -294,9 +304,9 @@ public:
     void reset()
     {
         if(decrementRefCount()) delete object;
-        // Object needs to be set to 0 regardless
+        // Object needs to be set to nullptr regardless
         // of whether the object is deleted
-        object=0;
+        object=nullptr;
     }
     
     /**
@@ -349,7 +359,7 @@ private:
      */
     bool decrementRefCount()
     {
-        if(object==0) return false;
+        if(object==nullptr) return false;
         return atomicAddExchange(&object->intrusive.referenceCount,-1)==1;
     }
 
@@ -395,7 +405,7 @@ intrusive_ref_ptr<T>& intrusive_ref_ptr<T>::operator= (T* o)
 template<typename T>
 intrusive_ref_ptr<T> intrusive_ref_ptr<T>::atomic_load() const
 {
-    intrusive_ref_ptr<T> result; // This gets initialized with 0
+    intrusive_ref_ptr<T> result; // This gets initialized with nullptr
     
     // According to the C++ standard, this causes undefined behaviour if
     // T has virtual functions, but GCC (and clang) have an implementation
@@ -442,7 +452,7 @@ intrusive_ref_ptr<T> intrusive_ref_ptr<T>::atomic_exchange(
     volatile int *objectAddrInt=reinterpret_cast<volatile int*>(&object);
     temp=reinterpret_cast<T*>(atomicSwap(objectAddrInt,tempInt));
     
-    intrusive_ref_ptr<T> result; // This gets initialized with 0
+    intrusive_ref_ptr<T> result; // This gets initialized with nullptr
     // This does not increment referenceCount, as the pointer was swapped
     result.object=temp;
     return result;
@@ -595,7 +605,7 @@ intrusive_ref_ptr<T> const_pointer_cast(const intrusive_ref_ptr<U>& r)
 template<typename T>
 intrusive_ref_ptr<T> atomic_load(const intrusive_ref_ptr<T> *p)
 {
-    if(p==0) return intrusive_ref_ptr<T>();
+    if(p==nullptr) return intrusive_ref_ptr<T>();
     return p->atomic_load();
 }
 
@@ -630,10 +640,271 @@ template<typename T>
 intrusive_ref_ptr<T> atomic_exchange(intrusive_ref_ptr<T> *p,
         intrusive_ref_ptr<T> r)
 {
-    if(p==0) return intrusive_ref_ptr<T>();
+    if(p==nullptr) return intrusive_ref_ptr<T>();
     return p->atomic_exchange(r);
 }
 
-} //namenpace miosix
+//Forward declarations
+class IntrusiveListBase;
+template<typename T>
+class IntrusiveList;
+
+/**
+ * Base class from which all items to be put in an IntrusiveList must derive,
+ * contains the next and prev pointer that create the list
+ */
+class IntrusiveListItem
+{
+private:
+    IntrusiveListItem *next=nullptr;
+    IntrusiveListItem *prev=nullptr;
+
+    friend class IntrusiveListBase;
+    template<typename T>
+    friend class IntrusiveList;
+};
+
+/**
+ * \internal
+ * Base class of IntrusiveList with the non-template-dependent part to improve
+ * code size when instantiationg multiple IntrusiveLists
+ */
+class IntrusiveListBase
+{
+protected:
+    IntrusiveListBase() : head(nullptr), tail(nullptr) {}
+
+    void push_back(IntrusiveListItem *item);
+
+    void pop_back();
+
+    void push_front(IntrusiveListItem *item);
+
+    void pop_front();
+
+    void insert(IntrusiveListItem *cur, IntrusiveListItem *item);
+
+    IntrusiveListItem *erase(IntrusiveListItem *cur);
+
+    IntrusiveListItem* front() { return head; }
+
+    IntrusiveListItem* back() { return tail; }
+
+    bool empty() const { return head==nullptr; }
+
+    #ifdef INTRUSIVE_LIST_ERROR_CHECK
+    static void fail();
+    #endif //INTRUSIVE_LIST_ERROR_CHECK
+
+private:
+    IntrusiveListItem *head;
+    IntrusiveListItem *tail;
+};
+
+/**
+ * A doubly linked list that only accepts objects that derive from
+ * IntrusiveListItem.
+ * 
+ * Compared to std::list, this class offers the guarantee that no dynamic memory
+ * allocation is performed. Differently from std::list, objects are not copied
+ * when put in the list, so this is a non-owning container. For this reason,
+ * this class unlike std::list accepts objects by pointer instead of reference
+ * in member functions like insert() or push_front(), and returns objects by
+ * pointer when dereferncing iterators or in member functions like front().
+ * The caller is thus responsible for managing the lifetime of objects put in
+ * this list.
+ */
+template<typename T>
+class IntrusiveList : private IntrusiveListBase
+{
+public:
+    /**
+     * Intrusive list iterator type
+     */
+    class iterator
+    {
+    public:
+        iterator() : list(nullptr), cur(nullptr) {}
+
+        T* operator*()
+        {
+            #ifdef INTRUSIVE_LIST_ERROR_CHECK
+            if(list==nullptr || cur==nullptr) IntrusiveListBase::fail();
+            #endif //INTRUSIVE_LIST_ERROR_CHECK
+            return static_cast<T*>(cur);
+        }
+
+        iterator operator++()
+        {
+            #ifdef INTRUSIVE_LIST_ERROR_CHECK
+            if(list==nullptr || cur==nullptr) IntrusiveListBase::fail();
+            #endif //INTRUSIVE_LIST_ERROR_CHECK
+            cur=cur->next; return *this;
+        }
+
+        iterator operator--()
+        {
+            #ifdef INTRUSIVE_LIST_ERROR_CHECK
+            if(list==nullptr || list->empty()) IntrusiveListBase::fail();
+            #endif //INTRUSIVE_LIST_ERROR_CHECK
+            if(cur!=nullptr) cur=cur->prev;
+            else cur=list->IntrusiveListBase::back(); //Special case: decrementing end()
+            return *this;
+        }
+
+        iterator operator++(int)
+        {
+            #ifdef INTRUSIVE_LIST_ERROR_CHECK
+            if(list==nullptr || cur==nullptr) IntrusiveListBase::fail();
+            #endif //INTRUSIVE_LIST_ERROR_CHECK
+            iterator result=*this;
+            cur=cur->next;
+            return result;
+        }
+
+        iterator operator--(int)
+        {
+            #ifdef INTRUSIVE_LIST_ERROR_CHECK
+            if(list==nullptr || list->empty()) IntrusiveListBase::fail();
+            #endif //INTRUSIVE_LIST_ERROR_CHECK
+            iterator result=*this;
+            if(cur!=nullptr) cur=cur->prev;
+            else cur=list->IntrusiveListBase::back(); //Special case: decrementing end()
+            return result;
+        }
+
+        bool operator==(const iterator& rhs) { return cur==rhs.cur; }
+        bool operator!=(const iterator& rhs) { return cur!=rhs.cur; }
+
+    private:
+        iterator(IntrusiveList<T> *list, IntrusiveListItem *cur)
+            : list(list), cur(cur) {}
+
+        IntrusiveList<T> *list;
+        IntrusiveListItem *cur;
+
+        friend class IntrusiveList<T>;
+    };
+    
+    /**
+     * Constructor, produces an empty list
+     */
+    IntrusiveList() {}
+
+    /**
+     * Disabled copy constructor and operator=
+     * Since intrusive lists do not store objects by value, and an item can
+     * only belong to at most one list, intrusive lists are not copyable.
+     */
+    IntrusiveList(const IntrusiveList&)=delete;
+    IntrusiveList& operator=(const IntrusiveList&)=delete;
+    
+    /**
+     * Adds item to the end of the list
+     * \param item item to add
+     */
+    void push_back(T *item) { IntrusiveListBase::push_back(item); }
+    
+    /**
+     * Removes the last element in the list
+     */
+    void pop_back() { IntrusiveListBase::pop_back(); }
+    
+    /**
+     * Adds item to the front of the list
+     * \param item item to add
+     */
+    void push_front(T *item) { IntrusiveListBase::push_front(item); }
+    
+    /**
+     * Removes the first item of the list
+     */
+    void pop_front() { IntrusiveListBase::pop_front(); }
+    
+    /**
+     * Inserts the given item before the position indicated by the iterator
+     * \param it position where to insert the item
+     * \param item item to insert
+     */
+    void insert(iterator it, T *item)
+    {
+        #ifdef INTRUSIVE_LIST_ERROR_CHECK
+        if(it.list!=this) fail();
+        #endif //INTRUSIVE_LIST_ERROR_CHECK
+        IntrusiveListItem *cur=it.cur; //Safe even if it==end() -> cur=nullptr
+        IntrusiveListBase::insert(cur,item);
+    }
+    
+    /**
+     * Removes the specified item from the list
+     * \param it iterator to the item to remove
+     * \return an iterator to the next item
+     */
+    iterator erase(iterator it)
+    {
+        #ifdef INTRUSIVE_LIST_ERROR_CHECK
+        if(it.list!=this) fail();
+        #endif //INTRUSIVE_LIST_ERROR_CHECK
+        IntrusiveListItem *cur=it.cur;
+        return iterator(this,IntrusiveListBase::erase(cur));
+    }
+
+    /**
+     * Nonportable version of std::list::remove that is O(1) since it relies on
+     * the list being intrusive
+     * NOTE: can ONLY be called if you are sure the item to remove is either not
+     * in any list (in this case, nothing is done) or is in the list it is being
+     * removed from. Trying to remove an item that is present in another list
+     * produces undefined bahavior.
+     * \param item item to remove, must not be nullptr
+     * \return true if the item was removed, false if the item was not present
+     * in the list
+     */
+    bool removeFast(T *item)
+    {
+        if(item->prev==nullptr && IntrusiveListBase::front()!=item) return false;
+        IntrusiveListBase::erase(item);
+        return true;
+    }
+    
+    /**
+     * \return an iterator to the first item
+     */
+    iterator begin() { return iterator(this,IntrusiveListBase::front()); }
+    
+    /**
+     * \return an iterator to the last item
+     */
+    iterator end() { return iterator(this,nullptr); }
+    
+    /**
+     * \return a pointer to the first item. List must not be empty
+     */
+    T* front()
+    {
+        auto result=IntrusiveListBase::front();
+        #ifdef INTRUSIVE_LIST_ERROR_CHECK
+        if(result==nullptr) fail();
+        #endif //INTRUSIVE_LIST_ERROR_CHECK
+        return static_cast<T*>(result);
+    }
+    
+    /**
+     * \return a pointer to the last item. List must not be empty
+     */
+    T* back()
+    {
+        auto result=IntrusiveListBase::back();
+        #ifdef INTRUSIVE_LIST_ERROR_CHECK
+        if(result==nullptr) fail();
+        #endif //INTRUSIVE_LIST_ERROR_CHECK
+        return static_cast<T*>(result);
+    }
+    
+    /**
+     * \return true if the list is empty
+     */
+    bool empty() const { return IntrusiveListBase::empty(); }
+};
 
-#endif //INTRUSIVE_H
+} //namespace miosix

miosix/kernel/kernel.cpp

@@ -62,7 +62,7 @@ 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 SleepData *sleeping_list=NULL;///<\internal list of sleeping threads
+static IntrusiveList<SleepData> sleeping_list;///<\internal list of sleeping threads
 
 static volatile long long tick=0;///<\internal Kernel tick
 
@@ -228,38 +228,26 @@ long long getTick()
 
 /**
  * \internal
- * Used by Thread::sleep() to add a thread to sleeping list. The list is sorted
- * by the wakeup_time field to reduce time required to wake threads during
- * context switch.
- * Also sets thread SLEEP_FLAG. It is labeled IRQ not because it is meant to be
- * used inside an IRQ, but because interrupts must be disabled prior to calling
- * this function.
+ * Used by Thread::sleep() to add a thread to
+ * sleeping list. The list is sorted by the wakeupTime field to reduce time
+ * required to wake threads during context switch.
+ * Interrupts must be disabled prior to calling this function.
  */
-void IRQaddToSleepingList(SleepData *x)
+static void IRQaddToSleepingList(SleepData *x)
 {
-    x->p->flags.IRQsetSleep(true);
-    if((sleeping_list==NULL)||(x->wakeup_time <= sleeping_list->wakeup_time))
+    if(sleeping_list.empty() || sleeping_list.front()->wakeupTime>=x->wakeupTime)
     {
-        x->next=sleeping_list;
-        sleeping_list=x;   
+        sleeping_list.push_front(x);
     } else {
-       SleepData *cur=sleeping_list;
-       for(;;)
-       {
-           if((cur->next==NULL)||(x->wakeup_time <= cur->next->wakeup_time))
-           {
-               x->next=cur->next;
-               cur->next=x;
-               break;
-           }
-           cur=cur->next;
-       }
+        auto it=sleeping_list.begin();
+        while(it!=sleeping_list.end() && (*it)->wakeupTime<x->wakeupTime) ++it;
+        sleeping_list.insert(it,x);
     }
 }
 
 /**
  * \internal
- * Called @ every tick to check if it's time to wake some thread.
+ * Called to check if it's time to wake some thread.
  * Also increases the system tick.
  * Takes care of clearing SLEEP_FLAG.
  * It is used by the kernel, and should not be used by end users.
@@ -268,15 +256,18 @@ void IRQaddToSleepingList(SleepData *x)
 bool IRQwakeThreads()
 {
     tick++;//Increment tick
+
+    if(sleeping_list.empty()) return false; //If no item in list, return
+    
     bool result=false;
-    for(;;)
-    {
-        if(sleeping_list==NULL) break;//If no item in list, return
     //Since list is sorted, if we don't need to wake the first element
     //we don't need to wake the other too
-        if(tick != sleeping_list->wakeup_time) break;
-        sleeping_list->p->flags.IRQsetSleep(false);//Wake thread
-        sleeping_list=sleeping_list->next;//Remove from list
+    for(auto it=sleeping_list.begin();it!=sleeping_list.end();)
+    {
+        if(tick<(*it)->wakeupTime) break;
+        //Wake both threads doing absoluteSleep() and timedWait()
+        (*it)->thread->flags.IRQclearSleepAndWait();
+        it=sleeping_list.erase(it);
         result=true;
     }
     return result;
@@ -353,11 +344,12 @@ void Thread::sleep(unsigned int ms)
     //the tick isr will wake threads, modifying the sleeping_list
     {
         FastInterruptDisableLock lock;
-        d.p=const_cast<Thread*>(cur);
-        if(((ms*TICK_FREQ)/1000)>0) d.wakeup_time=getTick()+(ms*TICK_FREQ)/1000;
+        d.thread=const_cast<Thread*>(cur);
+        if(((ms*TICK_FREQ)/1000)>0) d.wakeupTime=getTick()+(ms*TICK_FREQ)/1000;
         //If tick resolution is too low, wait one tick
-        else d.wakeup_time=getTick()+1;
-        IRQaddToSleepingList(&d);//Also sets SLEEP_FLAG
+        else d.wakeupTime=getTick()+1;
+        d.thread->flags.IRQsetSleep(true); //Sleeping thread: set sleep flag
+        IRQaddToSleepingList(&d);
     }
     Thread::yield();
 }
@@ -373,9 +365,10 @@ void Thread::sleepUntil(long long absoluteTime)
     {
         FastInterruptDisableLock lock;
         if(absoluteTime<=getTick()) return; //Wakeup time in the past, return
-        d.p=const_cast<Thread*>(cur);
-        d.wakeup_time=absoluteTime;
-        IRQaddToSleepingList(&d);//Also sets SLEEP_FLAG
+        d.thread=const_cast<Thread*>(cur);
+        d.wakeupTime=absoluteTime;
+        d.thread->flags.IRQsetSleep(true); //Sleeping thread: set sleep flag
+        IRQaddToSleepingList(&d);
     }
     Thread::yield();
 }
@@ -564,6 +557,42 @@ void Thread::IRQwait()
     const_cast<Thread*>(cur)->flags.IRQsetWait(true);
 }
 
+void Thread::PKrestartKernelAndWait(PauseKernelLock& dLock)
+{
+    (void)dLock;
+    //Implemented by upgrading the lock to an interrupt disable one
+    FastInterruptDisableLock dLockIrq;
+    auto savedNesting=kernel_running;
+    kernel_running=0;
+    IRQenableIrqAndWaitImpl();
+    if(kernel_running!=0) errorHandler(UNEXPECTED);
+    kernel_running=savedNesting;
+}
+
+TimedWaitResult Thread::timedWaitMs(long long ms)
+{
+    if (ms <= 0) return TimedWaitResult::Timeout;
+
+    FastInterruptDisableLock dLock;
+    long long ticks = std::max((ms * TICK_FREQ) / 1000, 1LL);
+    long long absoluteTime = getTick() + ticks;
+    return IRQenableIrqAndTimedWaitMsImpl(absoluteTime);
+}
+
+TimedWaitResult Thread::PKrestartKernelAndTimedWaitMs(PauseKernelLock& dLock,
+        long long absoluteTime)
+{
+    (void)dLock;
+    //Implemented by upgrading the lock to an interrupt disable one
+    FastInterruptDisableLock dLockIrq;
+    auto savedNesting=kernel_running;
+    kernel_running=0;
+    auto result=IRQenableIrqAndTimedWaitMsImpl(absoluteTime);
+    if(kernel_running!=0) errorHandler(UNEXPECTED);
+    kernel_running=savedNesting;
+    return result;
+}
+
 void Thread::IRQwakeup()
 {
     this->flags.IRQsetWait(false);
@@ -700,6 +729,37 @@ void Thread::threadLauncher(void *(*threadfunc)(void*), void *argv)
     errorHandler(UNEXPECTED);
 }
 
+void Thread::IRQenableIrqAndWaitImpl()
+{
+    const_cast<Thread*>(cur)->flags.IRQsetWait(true);
+    auto savedNesting=interruptDisableNesting; //For InterruptDisableLock
+    interruptDisableNesting=0;
+    miosix_private::doEnableInterrupts();
+    Thread::yield(); //Here the wait becomes effective
+    miosix_private::doDisableInterrupts();
+    if(interruptDisableNesting!=0) errorHandler(UNEXPECTED);
+    interruptDisableNesting=savedNesting;
+}
+
+TimedWaitResult Thread::IRQenableIrqAndTimedWaitMsImpl(long long absoluteTime)
+{
+    absoluteTime=std::max(absoluteTime,1LL);
+    Thread *t=const_cast<Thread*>(cur);
+    SleepData sleepData(t,absoluteTime);
+    t->flags.IRQsetWait(true); //timedWait thread: set wait flag
+    IRQaddToSleepingList(&sleepData);
+    auto savedNesting=interruptDisableNesting; //For InterruptDisableLock
+    interruptDisableNesting=0;
+    miosix_private::doEnableInterrupts();
+    Thread::yield(); //Here the wait becomes effective
+    miosix_private::doDisableInterrupts();
+    if(interruptDisableNesting!=0) errorHandler(UNEXPECTED);
+    interruptDisableNesting=savedNesting;
+    bool removed=sleeping_list.removeFast(&sleepData);
+    //If the thread was still in the sleeping list, it was woken up by a wakeup()
+    return removed ? TimedWaitResult::NoTimeout : TimedWaitResult::Timeout;
+}
+
 Thread *Thread::allocateIdleThread()
 {
     //NOTE: this function is only called once before the kernel is started, so
@@ -831,6 +891,12 @@ void Thread::ThreadFlags::IRQsetSleep(bool sleeping)
     Scheduler::IRQwaitStatusHook();
 }
 
+void Thread::ThreadFlags::IRQclearSleepAndWait()
+{
+    flags &= ~(WAIT | SLEEP);
+    Scheduler::IRQwaitStatusHook();
+}
+
 void Thread::ThreadFlags::IRQsetDeleted()
 {
     flags |= DELETED;

miosix/kernel/kernel.h

@@ -34,6 +34,7 @@
 #include "interfaces/portability.h"
 #include "kernel/scheduler/sched_types.h"
 #include "stdlib_integration/libstdcpp_integration.h"
+#include "intrusive.h"
 #include <cstdlib>
 #include <new>
 #include <functional>
@@ -388,6 +389,15 @@ bool isKernelRunning();
  */
 long long getTick();
 
+/**
+ * Possible return values of timedWait
+ */
+enum class TimedWaitResult
+{
+    NoTimeout,
+    Timeout
+};
+
 //Forwrd declaration
 struct SleepData;
 class MemoryProfiling;
@@ -580,10 +590,11 @@ public:
     void terminate();
 
     /**
-     * This method stops the thread until another thread calls wakeup() on this
-     * thread.<br>Calls to wait are not cumulative. If wait() is called two
-     * times, only one call to wakeup() is needed to wake the thread.
-     * <br>CANNOT be called when the kernel is paused.
+     * This method stops the thread until wakeup() is called.
+     * Ths method is useful to implement any kind of blocking primitive,
+     * including device drivers.
+     *
+     * CANNOT be called when the kernel is paused.
      */
     static void wait();
 
@@ -595,7 +606,7 @@ public:
 
     /**
      * Wakeup a thread.
-     * <br>Can be called when the kernel is paused.
+     * <br>Can only be called when the kernel is paused.
      */
     void PKwakeup();
 
@@ -644,8 +655,10 @@ public:
     Priority IRQgetPriority();
 
     /**
-     * Same as wait(), but is meant to be used only inside an IRQ or when
-     * interrupts are disabled.<br>
+     * This method stops the thread until wakeup() is called.
+     * Ths method is useful to implement any kind of blocking primitive,
+     * including device drivers.
+     *
      * Note: this method is meant to put the current thread in wait status in a
      * piece of code where interrupts are disbled; it returns immediately, so
      * the user is responsible for re-enabling interrupts and calling yield to
@@ -658,12 +671,160 @@ public:
      * enableInterrupts();
      * Thread::yield(); //After this, thread is in wait status
      * \endcode
+     *
+     * Consider using IRQenableIrqAndWait() instead.
      */
     static void IRQwait();
 
     /**
-     * Same as wakeup(), but is meant to be used only inside an IRQ or when
-     * interrupts are disabled.
+     * This method stops the thread until wakeup() is called.
+     * Ths method is useful to implement any kind of blocking primitive,
+     * including device drivers.
+     *
+     * NOTE: this method is meant to put the current thread in wait status in a
+     * piece of code where the kernel is paused (preemption disabled).
+     * Preemption will be enabled during the waiting period, and disabled back
+     * before this method returns.
+     *
+     * \param dLock the PauseKernelLock object that was used to disable
+     * preemption in the current context.
+     */
+    static void PKrestartKernelAndWait(PauseKernelLock& dLock);
+
+    /**
+     * This method stops the thread until wakeup() is called.
+     * Ths method is useful to implement any kind of blocking primitive,
+     * including device drivers.
+     *
+     * NOTE: this method is meant to put the current thread in wait status in a
+     * piece of code where interrupts are disbled, interrupts will be enabled
+     * during the waiting period, and disabled back before this method returns.
+     *
+     * \param dLock the InterruptDisableLock object that was used to disable
+     * interrupts in the current context.
+     */
+    static void IRQenableIrqAndWait(InterruptDisableLock& dLock)
+    {
+        (void)dLock; //Common implementation doesn't need it
+        return IRQenableIrqAndWaitImpl();
+    }
+
+    /**
+     * This method stops the thread until wakeup() is called.
+     * Ths method is useful to implement any kind of blocking primitive,
+     * including device drivers.
+     *
+     * NOTE: this method is meant to put the current thread in wait status in a
+     * piece of code where interrupts are disbled, interrupts will be enabled
+     * during the waiting period, and disabled back before this method returns.
+     *
+     * \param dLock the FastInterruptDisableLock object that was used to disable
+     * interrupts in the current context.
+     */
+    static void IRQenableIrqAndWait(FastInterruptDisableLock& dLock)
+    {
+        (void)dLock; //Common implementation doesn't need it
+        return IRQenableIrqAndWaitImpl();
+    }
+
+    /**
+     * This method stops the thread until wakeup() is called or the specified
+     * time in milliseconds has passed.
+     * Ths method is thus a combined IRQwait() and absoluteSleep(), and is
+     * useful to implement any kind of blocking primitive with timeout,
+     * including device drivers.
+     *
+     * \param ms the number of millisecond. If it is <=0 this method will
+     * time out immediately
+     * \return TimedWaitResult::Timeout if the wait timed out
+     */
+    static TimedWaitResult timedWaitMs(long long ms);
+
+    /**
+     * This method stops the thread until wakeup() is called or the specified
+     * absolute time in milliseconds is reached.
+     * Ths method is thus a combined IRQwait() and absoluteSleep(), and is
+     * useful to implement any kind of blocking primitive with timeout,
+     * including device drivers.
+     *
+     * \param absoluteTimeout absolute time after which the wait times out
+     * \return TimedWaitResult::Timeout if the wait timed out
+     */
+    static TimedWaitResult timedWaitUntilMs(long long absoluteTime)
+    {
+        FastInterruptDisableLock dLock;
+        return IRQenableIrqAndTimedWaitMsImpl(absoluteTime);
+    }
+
+    /**
+     * This method stops the thread until wakeup() is called or the specified
+     * absolute time in milliseconds is reached.
+     * Ths method is thus a combined IRQwait() and absoluteSleep(), and is
+     * useful to implement any kind of blocking primitive with timeout,
+     * including device drivers.
+     *
+     * NOTE: this method is meant to put the current thread in wait status in a
+     * piece of code where the kernel is paused (preemption disabled).
+     * Preemption will be enabled during the waiting period, and disabled back
+     * before this method returns.
+     *
+     * \param dLock the PauseKernelLock object that was used to disable
+     * preemption in the current context.
+     * \param absoluteTimeout absolute time after which the wait times out
+     * \return TimedWaitResult::Timeout if the wait timed out
+     */
+    static TimedWaitResult PKrestartKernelAndTimedWaitMs(PauseKernelLock& dLock,
+            long long absoluteTime);
+
+    /**
+     * This method stops the thread until wakeup() is called or the specified
+     * absolute time in milliseconds is reached.
+     * Ths method is thus a combined IRQwait() and absoluteSleep(), and is
+     * useful to implement any kind of blocking primitive with timeout,
+     * including device drivers.
+     *
+     * NOTE: this method is meant to put the current thread in wait status in a
+     * piece of code where interrupts are disbled, interrupts will be enabled
+     * during the waiting period, and disabled back before this method returns.
+     *
+     * \param dLock the InterruptDisableLock object that was used to disable
+     * interrupts in the current context.
+     * \param absoluteTimeout absolute time after which the wait times out
+     * \return TimedWaitResult::Timeout if the wait timed out
+     */
+    static TimedWaitResult IRQenableIrqAndTimedWaitMs(InterruptDisableLock& dLock,
+            long long absoluteTime)
+    {
+        (void)dLock; //Common implementation doesn't need it
+        return IRQenableIrqAndTimedWaitMsImpl(absoluteTime);
+    }
+
+    /**
+     * This method stops the thread until wakeup() is called or the specified
+     * absolute time in milliseconds is reached.
+     * Ths method is thus a combined IRQwait() and absoluteSleep(), and is
+     * useful to implement any kind of blocking primitive with timeout,
+     * including device drivers.
+     *
+     * NOTE: this method is meant to put the current thread in wait status in a
+     * piece of code where interrupts are disbled, interrupts will be enabled
+     * during the waiting period, and disabled back before this method returns.
+     *
+     * \param dLock the FastInterruptDisableLock object that was used to disable
+     * interrupts in the current context.
+     * \param absoluteTimeout absolute time after which the wait times out
+     * \return TimedWaitResult::Timeout if the wait timed out
+     */
+    static TimedWaitResult IRQenableIrqAndTimedWaitMs(FastInterruptDisableLock& dLock,
+            long long absoluteTime)
+    {
+        (void)dLock; //Common implementation doesn't need it
+        return IRQenableIrqAndTimedWaitMsImpl(absoluteTime);
+    }
+
+    /**
+     * Wakeup a thread.
+     * <br>Can only be called inside an IRQ or when interrupts are disabled.
      */
     void IRQwakeup();
 
@@ -755,6 +916,12 @@ private:
          */
         void IRQsetSleep(bool sleeping);
 
+        /**
+         * Used by IRQwakeThreads to clear both the sleep and wait flags,
+         * waking threads doing absoluteSleep() as well as timedWait()
+         */
+        void IRQclearSleepAndWait();
+
         /**
          * Set the deleted flag of the thread. This flag can't be cleared.
          * Can only be called with interrupts disabled or within an interrupt.
@@ -943,6 +1110,16 @@ private:
      */
     static void threadLauncher(void *(*threadfunc)(void*), void *argv);
 
+    /**
+     * Common implementation of all IRQenableIrqAndWait calls
+     */
+    static void IRQenableIrqAndWaitImpl();
+
+    /**
+     * Common implementation of all timedWait calls
+     */
+    static TimedWaitResult IRQenableIrqAndTimedWaitMsImpl(long long absoluteTime);
+
     /**
      * Allocates the idle thread and makes cur point to it
      * Can only be called before the kernel is started, is called exactly once
@@ -996,8 +1173,6 @@ private:
     //friend functions
     //Needs access to watermark, ctxsave
     friend void miosix_private::IRQstackOverflowCheck();
-    //Need access to status
-    friend void IRQaddToSleepingList(SleepData *x);
     //Needs access to status
     friend bool IRQwakeThreads();
     //Needs access to watermark, status, next
@@ -1052,20 +1227,24 @@ public:
 
 /**
  * \internal
- * \struct Sleep_data
- * This struct is used to make a list of sleeping threads.
+ * This class is used to make a list of sleeping threads.
  * It is used by the kernel, and should not be used by end users.
  */
-struct SleepData
+class SleepData : public IntrusiveListItem
 {
+public:
+    // Default constructor declared for compatibility with the old SleepData class usages
+    SleepData() = default;
+
+    SleepData(Thread *thread, long long wakeupTime)
+        : thread(thread), wakeupTime(wakeupTime) {}
+
     ///\internal Thread that is sleeping
-    Thread *p;
+    Thread *thread;
     
     ///\internal When this number becomes equal to the kernel tick,
     ///the thread will wake
-    long long wakeup_time;
-    
-    SleepData *next;///<\internal Next thread in the list
+    long long wakeupTime;
 };
 
 /**

miosix/kernel/sync.cpp

@@ -361,60 +361,44 @@ unsigned int Mutex::PKunlockAllDepthLevels(PauseKernelLock& dLock)
 // class ConditionVariable
 //
 
-ConditionVariable::ConditionVariable(): first(0), last(0) {}
-
 void ConditionVariable::wait(Mutex& m)
 {
+    WaitToken listItem(Thread::IRQgetCurrentThread());
+    
     PauseKernelLock dLock;
+    unsigned int depth=m.PKunlockAllDepthLevels(dLock);
+    condList.push_back(&listItem);
 
-    WaitingData w;
-    w.p=Thread::getCurrentThread();
-    w.next=0; 
-    //Add entry to tail of list
-    if(first==0)
-    {
-        first=last=&w;
-    } else {
-       last->next=&w;
-       last=&w;
-    }
-    //Unlock mutex and wait
+    // Set the cond wait flag and wait
     {
         FastInterruptDisableLock l;
-        w.p->flags.IRQsetCondWait(true);
+        listItem.thread->flags.IRQsetCondWait(true);
     }
-
-    unsigned int depth=m.PKunlockAllDepthLevels(dLock);
     {
         RestartKernelLock eLock(dLock);
         Thread::yield(); //Here the wait becomes effective
     }
+
+    condList.removeFast(&listItem);
     m.PKlockToDepth(dLock,depth);
 }
 
 void ConditionVariable::wait(FastMutex& m)
 {
-    FastInterruptDisableLock dLock;
-    
-    WaitingData w;
-    w.p=Thread::getCurrentThread();
-    w.next=0;
-    //Add entry to tail of list
-    if(first==0)
-    {
-        first=last=&w;
-    } else {
-       last->next=&w;
-       last=&w;
-    }
-    //Unlock mutex and wait
-    w.p->flags.IRQsetCondWait(true);
+    WaitToken listItem(Thread::IRQgetCurrentThread());
  
+    FastInterruptDisableLock dLock;
     unsigned int depth=IRQdoMutexUnlockAllDepthLevels(m.get());
+    condList.push_back(&listItem);
+
+    // Set the cond wait flag and wait
+    listItem.thread->flags.IRQsetCondWait(true);
     {
         FastInterruptEnableLock eLock(dLock);
         Thread::yield(); //Here the wait becomes effective
     }
+
+    condList.removeFast(&listItem);
     IRQdoMutexLockToDepth(m.get(),dLock,depth);
 }
 
@@ -426,14 +410,14 @@ void ConditionVariable::signal()
         //that can only be called with irq disabled, othrwise we would use
         //PauseKernelLock
         FastInterruptDisableLock lock;
-        if(first==0) return;
-        //Wakeup
-        first->p->flags.IRQsetCondWait(false);
+        if(condList.empty()) return;
+        //Remove from list and wakeup
+        Thread *t=condList.front()->thread;
+        condList.pop_front();
+        t->flags.IRQsetCondWait(false);
         //Check for priority issues
-        if(first->p->IRQgetPriority() >
-                Thread::IRQgetCurrentThread()->IRQgetPriority()) hppw=true;
-        //Remove from list
-        first=first->next;
+        if(t->IRQgetPriority()>Thread::IRQgetCurrentThread()->IRQgetPriority())
+            hppw=true;
     }
     //If the woken thread has higher priority than our priority, yield
     if(hppw) Thread::yield();
@@ -447,15 +431,15 @@ void ConditionVariable::broadcast()
         //that can only be called with irq disabled, othrwise we would use
         //PauseKernelLock
         FastInterruptDisableLock lock;
-        while(first!=0)
+        while(!condList.empty())
         {
-            //Wakeup
-            first->p->flags.IRQsetCondWait(false);
+            //Remove from list and wakeup
+            Thread *t=condList.front()->thread;
+            condList.pop_front();
+            t->flags.IRQsetCondWait(false);
             //Check for priority issues
-            if(first->p->IRQgetPriority() >
-                Thread::IRQgetCurrentThread()->IRQgetPriority()) hppw=true;
-            //Remove from list
-            first=first->next;
+            if(t->IRQgetPriority()>Thread::IRQgetCurrentThread()->IRQgetPriority())
+                hppw=true;
         }
     }
     //If at least one of the woken thread has higher priority than our priority,

miosix/kernel/sync.h

@@ -402,7 +402,7 @@ public:
     /**
      * Constructor, initializes the ConditionVariable.
      */
-    ConditionVariable();
+    ConditionVariable() {}
 
     /**
      * Unlock the mutex and wait.
@@ -443,24 +443,23 @@ public:
      */
     void broadcast();
 
-private:
     //Unwanted methods
-    ConditionVariable(const ConditionVariable& );
-    ConditionVariable& operator= (const ConditionVariable& );
+    ConditionVariable(const ConditionVariable& ) = delete;
+    ConditionVariable& operator= (const ConditionVariable& ) = delete;
 
+private:
     /**
-     * \internal
-     * \struct WaitingData
-     * This struct is used to make a list of waiting threads.
+     * \internal Element of a thread waiting list
      */
-    struct WaitingData
+    class WaitToken : public IntrusiveListItem
     {
-        Thread *p;///<\internal Thread that is waiting
-        WaitingData *next;///<\internal Next thread in the list
+    public:
+        WaitToken(Thread *thread) : thread(thread) {}
+        Thread *thread; ///<\internal Waiting thread
     };
 
-    WaitingData *first;///<Pointer to first element of waiting fifo
-    WaitingData *last;///<Pointer to last element of waiting fifo
+    // The list of threads waiting on this condition variable
+    IntrusiveList<WaitToken> condList;
 };
 
 /**