Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/wpilibc/Athena/src/Notifier.cpp b/wpilibc/Athena/src/Notifier.cpp
new file mode 100644
index 0000000..fc3059c
--- /dev/null
+++ b/wpilibc/Athena/src/Notifier.cpp
@@ -0,0 +1,263 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Notifier.h"
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include "HAL/HAL.hpp"
+
+Notifier *Notifier::timerQueueHead = nullptr;
+priority_recursive_mutex Notifier::queueMutex;
+priority_mutex Notifier::halMutex;
+void *Notifier::m_notifier = nullptr;
+std::atomic<int> Notifier::refcount{0};
+
+/**
+ * Create a Notifier for timer event notification.
+ * @param handler The handler is called at the notification time which is set
+ * using StartSingle or StartPeriodic.
+ */
+Notifier::Notifier(TimerEventHandler handler, void *param) {
+ if (handler == nullptr)
+ wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+ m_handler = handler;
+ m_param = param;
+ // do the first time intialization of static variables
+ if (refcount.fetch_add(1) == 0) {
+ int32_t status = 0;
+ {
+ std::lock_guard<priority_mutex> sync(halMutex);
+ if (!m_notifier)
+ m_notifier = initializeNotifier(ProcessQueue, nullptr, &status);
+ }
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * Free the resources for a timer event.
+ * All resources will be freed and the timer event will be removed from the
+ * queue if necessary.
+ */
+Notifier::~Notifier() {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ DeleteFromQueue();
+ }
+
+ // Delete the static variables when the last one is going away
+ if (refcount.fetch_sub(1) == 1) {
+ int32_t status = 0;
+ {
+ std::lock_guard<priority_mutex> sync(halMutex);
+ if (m_notifier) {
+ cleanNotifier(m_notifier, &status);
+ m_notifier = nullptr;
+ }
+ }
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+
+ // 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);
+}
+
+/**
+ * Update the alarm hardware to reflect the current first element in the queue.
+ * Compute the time the next alarm should occur based on the current time and
+ * the
+ * period for the first element in the timer queue.
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere
+ * that is taking care of synchronizing access to the queue.
+ */
+void Notifier::UpdateAlarm() {
+ if (timerQueueHead != nullptr) {
+ int32_t status = 0;
+ // This locking is necessary in order to avoid two things:
+ // 1) Race condition issues with calling cleanNotifer() and
+ // updateNotifierAlarm() at the same time.
+ // 2) Avoid deadlock by making it so that this won't block waiting
+ // for the mutex to unlock.
+ // Checking refcount as well is unnecessary, but will not hurt.
+ if (halMutex.try_lock() && refcount != 0) {
+ if (m_notifier)
+ updateNotifierAlarm(m_notifier,
+ (uint32_t)(timerQueueHead->m_expirationTime * 1e6),
+ &status);
+ halMutex.unlock();
+ }
+ wpi_setStaticErrorWithContext(timerQueueHead, status,
+ getHALErrorMessage(status));
+ }
+}
+
+/**
+ * ProcessQueue is called whenever there is a timer interrupt.
+ * We need to wake up and process the current top item in the timer queue as
+ * long
+ * as its scheduled time is after the current time. Then the item is removed or
+ * rescheduled (repetitive events) in the queue.
+ */
+void Notifier::ProcessQueue(uint32_t currentTimeInt, void *params) {
+ Notifier *current;
+ while (true) // keep processing past events until no more
+ {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ double currentTime = currentTimeInt * 1.0e-6;
+ current = timerQueueHead;
+ if (current == nullptr || current->m_expirationTime > currentTime) {
+ break; // no more timer events to process
+ }
+ // need to process this entry
+ timerQueueHead = current->m_nextEvent;
+ if (current->m_periodic) {
+ // if periodic, requeue the event
+ // compute when to put into queue
+ current->InsertInQueue(true);
+ } else {
+ // not periodic; removed from queue
+ current->m_queued = false;
+ }
+ // Take handler mutex while holding queue mutex to make sure
+ // the handler will execute to completion in case we are being deleted.
+ current->m_handlerMutex.lock();
+ }
+
+ current->m_handler(current->m_param); // call the event handler
+ current->m_handlerMutex.unlock();
+ }
+ // reschedule the first item in the queue
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ UpdateAlarm();
+}
+
+/**
+ * Insert this Notifier into the timer queue in right place.
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere
+ * that is taking care of synchronizing access to the queue.
+ * @param reschedule If false, the scheduled alarm is based on the current time
+ * and UpdateAlarm
+ * method is called which will enable the alarm if necessary.
+ * If true, update the time by adding the period (no drift) when rescheduled
+ * periodic from ProcessQueue.
+ * This ensures that the public methods only update the queue after finishing
+ * inserting.
+ */
+void Notifier::InsertInQueue(bool reschedule) {
+ if (reschedule) {
+ m_expirationTime += m_period;
+ } else {
+ m_expirationTime = GetClock() + m_period;
+ }
+ if (m_expirationTime > Timer::kRolloverTime) {
+ m_expirationTime -= Timer::kRolloverTime;
+ }
+ if (timerQueueHead == nullptr ||
+ timerQueueHead->m_expirationTime >= this->m_expirationTime) {
+ // the queue is empty or greater than the new entry
+ // the new entry becomes the first element
+ this->m_nextEvent = timerQueueHead;
+ timerQueueHead = this;
+ if (!reschedule) {
+ // since the first element changed, update alarm, unless we already plan
+ // to
+ UpdateAlarm();
+ }
+ } 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;
+}
+
+/**
+ * Delete this Notifier from the timer queue.
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere
+ * that is taking care of synchronizing access to the queue.
+ * Remove this Notifier from the timer queue and adjust the next interrupt time
+ * to reflect
+ * the current top of the queue.
+ */
+void Notifier::DeleteFromQueue() {
+ if (m_queued) {
+ m_queued = false;
+ wpi_assert(timerQueueHead != nullptr);
+ if (timerQueueHead == this) {
+ // remove the first item in the list - update the alarm
+ timerQueueHead = this->m_nextEvent;
+ 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
+ }
+ }
+ }
+ }
+}
+
+/**
+ * Register for single event notification.
+ * A timer event is queued for a single event after the specified delay.
+ * @param delay Seconds to wait before the handler is called.
+ */
+void Notifier::StartSingle(double delay) {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ m_periodic = false;
+ m_period = delay;
+ DeleteFromQueue();
+ InsertInQueue(false);
+}
+
+/**
+ * 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.
+ * @param period Period in seconds to call the handler starting one period after
+ * the call to this method.
+ */
+void Notifier::StartPeriodic(double period) {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ m_periodic = true;
+ m_period = period;
+ DeleteFromQueue();
+ InsertInQueue(false);
+}
+
+/**
+ * Stop timer events from occuring.
+ * Stop any repeating timer events from occuring. This will also remove any
+ * 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.
+ */
+void Notifier::Stop() {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ DeleteFromQueue();
+ }
+ // Wait for a currently executing handler to complete before returning from
+ // Stop()
+ std::lock_guard<priority_mutex> sync(m_handlerMutex);
+}