Replaced linked list in Notifier with std::list

Change-Id: I3bcb1195102d792db994dba98adc7425767b16e8

wpilibc/Athena/src/Notifier.cpp

@@ -33,8 +33,9 @@ Notifier::~Notifier() {
   cleanNotifier(m_notifier, &status);
   wpi_setErrorWithContext(status, getHALErrorMessage(status));
 
-  // Acquire the mutex; this makes certain that the handler is
-  // not being executed by the interrupt manager.
+  /* Acquire the mutex; this makes certain that the handler is not being
+   * executed by the interrupt manager.
+   */
   std::lock_guard<priority_mutex> lock(m_handlerMutex);
 }
 
@@ -85,8 +86,8 @@ void Notifier::StartSingle(double delay) {
 /**
  * Register for periodic event notification.
  * A timer event is queued for periodic event notification. Each time the
- * interrupt
- * occurs, the event will be immediately requeued for the same time interval.
+ * interrupt occurs, the event will be immediately requeued for the same time
+ * interval.
  * @param period Period in seconds to call the handler starting one period after
  * the call to this method.
  */
@@ -101,11 +102,9 @@ void Notifier::StartPeriodic(double period) {
 /**
  * Stop timer events from occuring.
  * Stop any repeating timer events from occuring. This will also remove any
- * single
- * notification events from the queue.
+ * single notification events from the queue.
  * If a timer-based call to the registered handler is in progress, this function
- * will
- * block until the handler call is complete.
+ * will block until the handler call is complete.
  */
 void Notifier::Stop() {
   int32_t status = 0;

wpilibc/simulation/include/Notifier.h

@@ -7,6 +7,7 @@
 
 #include <atomic>
 #include <functional>
+#include <list>
 #include <thread>
 
 #include "ErrorBase.h"
@@ -23,7 +24,6 @@ class Notifier : public ErrorBase {
       : Notifier(std::bind(std::forward<Callable>(f),
                            std::forward<Arg>(arg),
                            std::forward<Args>(args)...)) {}
-
   virtual ~Notifier();
 
   Notifier(const Notifier&) = delete;
@@ -34,36 +34,36 @@ class Notifier : public ErrorBase {
   void Stop();
 
  private:
-  static Notifier *timerQueueHead;
+  static std::list<Notifier*> timerQueue;
   static priority_recursive_mutex queueMutex;
   static priority_mutex halMutex;
   static void *m_notifier;
   static std::atomic<int> refcount;
 
-  // process the timer queue on a timer event
+  // Process the timer queue on a timer event
   static void ProcessQueue(uint32_t mask, void *params);
-  // update the FPGA alarm since the queue has changed
+
+  // Update the FPGA alarm since the queue has changed
   static void UpdateAlarm();
-  // insert this Notifier in the timer queue
+
+  // Insert the Notifier in the timer queue
   void InsertInQueue(bool reschedule);
-  // delete this Notifier from the timer queue
+
+  // Delete this Notifier from the timer queue
   void DeleteFromQueue();
 
-  // address of the handler
+  // Address of the handler
   TimerEventHandler m_handler;
-  // the relative time (either periodic or single)
+  // The relative time (either periodic or single)
   double m_period = 0;
-  // absolute expiration time for the current event
+  // Absolute expiration time for the current event
   double m_expirationTime = 0;
-  // next Nofifier event
-  Notifier *m_nextEvent = nullptr;
-  // true if this is a periodic event
+  // True if this is a periodic event
   bool m_periodic = false;
-  // indicates if this entry is queued
+  // Indicates if this entry is queued
   bool m_queued = false;
-  // held by interrupt manager task while handler call is in progress
+  // Held by interrupt manager task while handler call is in progress
   priority_mutex m_handlerMutex;
-
   static std::thread m_task;
   static std::atomic<bool> m_stopped;
   static void Run();

wpilibc/simulation/src/Notifier.cpp

@@ -9,7 +9,7 @@
 #include "Utility.h"
 #include "WPIErrors.h"
 
-Notifier *Notifier::timerQueueHead = nullptr;
+std::list<Notifier*> Notifier::timerQueue;
 priority_recursive_mutex Notifier::queueMutex;
 std::atomic<int> Notifier::refcount{0};
 std::thread Notifier::m_task;
@@ -28,7 +28,6 @@ Notifier::Notifier(TimerEventHandler handler)
 	m_periodic = false;
 	m_expirationTime = 0;
 	m_period = 0;
-	m_nextEvent = nullptr;
 	m_queued = false;
 	{
 		std::lock_guard<priority_recursive_mutex> sync(queueMutex);
@@ -88,13 +87,20 @@ void Notifier::ProcessQueue(uint32_t mask, void *params)
 		{
 			std::lock_guard<priority_recursive_mutex> sync(queueMutex);
 			double currentTime = GetClock();
-			current = timerQueueHead;
-			if (current == nullptr || current->m_expirationTime > currentTime)
+
+			if (timerQueue.empty())
+			{
+				break;
+			}
+			current = timerQueue.front();
+			if (current->m_expirationTime > currentTime)
 			{
 				break;		// no more timer events to process
 			}
-			// need to process this entry
-			timerQueueHead = current->m_nextEvent;
+			// remove next entry before processing it
+			timerQueue.pop_front();
+
+			current->m_queued = false;
 			if (current->m_periodic)
 			{
 				// if periodic, requeue the event
@@ -138,32 +144,34 @@ void Notifier::InsertInQueue(bool reschedule)
 	{
 		m_expirationTime = GetClock() + m_period;
 	}
-	if (timerQueueHead == nullptr || timerQueueHead->m_expirationTime >= this->m_expirationTime)
+
+	// Attempt to insert new entry into queue
+	for (auto i = timerQueue.begin(); i != timerQueue.end(); i++)
 	{
-		// the queue is empty or greater than the new entry
-		// the new entry becomes the first element
-		this->m_nextEvent = timerQueueHead;
-		timerQueueHead = this;
+		if ((*i)->m_expirationTime > m_expirationTime)
+		{
+			timerQueue.insert(i, this);
+			m_queued = true;
+		}
+	}
+
+	/* If the new entry wasn't queued, either the queue was empty or the first
+	 * element was greater than the new entry.
+	 */
+	if (!m_queued)
+	{
+		timerQueue.push_front(this);
+
 		if (!reschedule)
 		{
-			// since the first element changed, update alarm, unless we already plan to
+			/* Since the first element changed, update alarm, unless we already
+			 * plan to
+			 */
 			UpdateAlarm();
 		}
+
+		m_queued = true;
 	}
-	else
-	{
-		for (Notifier **npp = &(timerQueueHead->m_nextEvent); ; npp = &(*npp)->m_nextEvent)
-		{
-			Notifier *n = *npp;
-			if (n == nullptr || n->m_expirationTime > this->m_expirationTime)
-			{
-				*npp = this;
-				this->m_nextEvent = n;
-				break;
-			}
-		}
-	}
-	m_queued = true;
 }
 
 /**
@@ -178,23 +186,16 @@ void Notifier::DeleteFromQueue()
 	if (m_queued)
 	{
 		m_queued = false;
-		wpi_assert(timerQueueHead != nullptr);
-		if (timerQueueHead == this)
+		wpi_assert(!timerQueue.empty());
+		if (timerQueue.front() == this)
 		{
 			// remove the first item in the list - update the alarm
-			timerQueueHead = this->m_nextEvent;
+			timerQueue.pop_front();
 			UpdateAlarm();
 		}
 		else
 		{
-			for (Notifier *n = timerQueueHead; n != nullptr; n = n->m_nextEvent)
-			{
-				if (n->m_nextEvent == this)
-				{
-					// this element is the next element from *n from the queue
-					n->m_nextEvent = this->m_nextEvent;	// point around this one
-				}
-			}
+			timerQueue.remove(this);
 		}
 	}
 }
@@ -248,9 +249,19 @@ void Notifier::Stop()
 void Notifier::Run() {
     while (!m_stopped) {
         Notifier::ProcessQueue(0, nullptr);
-        if (timerQueueHead != nullptr)
+        bool isEmpty;
         {
-            Wait(timerQueueHead->m_expirationTime - GetClock());
+            std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+            isEmpty = timerQueue.empty();
+        }
+        if (!isEmpty)
+        {
+            double expirationTime;
+            {
+                std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+                expirationTime = timerQueue.front()->m_expirationTime;
+            }
+            Wait(expirationTime - GetClock());
         }
         else
         {