diff --git a/wpilibc/Athena/src/Notifier.cpp b/wpilibc/Athena/src/Notifier.cpp index 5dffc9f005..686d09199d 100644 --- a/wpilibc/Athena/src/Notifier.cpp +++ b/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 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; diff --git a/wpilibc/simulation/include/Notifier.h b/wpilibc/simulation/include/Notifier.h index 8afc9ade08..8bf2f1cc57 100644 --- a/wpilibc/simulation/include/Notifier.h +++ b/wpilibc/simulation/include/Notifier.h @@ -7,6 +7,7 @@ #include #include +#include #include #include "ErrorBase.h" @@ -23,7 +24,6 @@ class Notifier : public ErrorBase { : Notifier(std::bind(std::forward(f), std::forward(arg), std::forward(args)...)) {} - virtual ~Notifier(); Notifier(const Notifier&) = delete; @@ -34,36 +34,36 @@ class Notifier : public ErrorBase { void Stop(); private: - static Notifier *timerQueueHead; + static std::list timerQueue; static priority_recursive_mutex queueMutex; static priority_mutex halMutex; static void *m_notifier; static std::atomic 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 m_stopped; static void Run(); diff --git a/wpilibc/simulation/src/Notifier.cpp b/wpilibc/simulation/src/Notifier.cpp index 09b8888ced..3837982edc 100644 --- a/wpilibc/simulation/src/Notifier.cpp +++ b/wpilibc/simulation/src/Notifier.cpp @@ -9,7 +9,7 @@ #include "Utility.h" #include "WPIErrors.h" -Notifier *Notifier::timerQueueHead = nullptr; +std::list Notifier::timerQueue; priority_recursive_mutex Notifier::queueMutex; std::atomic 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 sync(queueMutex); @@ -88,13 +87,20 @@ void Notifier::ProcessQueue(uint32_t mask, void *params) { std::lock_guard 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 sync(queueMutex); + isEmpty = timerQueue.empty(); + } + if (!isEmpty) + { + double expirationTime; + { + std::lock_guard sync(queueMutex); + expirationTime = timerQueue.front()->m_expirationTime; + } + Wait(expirationTime - GetClock()); } else {