Merge "HAL: Add support for multiple Notifiers."

hal/include/HAL/Notifier.hpp

@@ -4,7 +4,7 @@
 
 extern "C"
 {
-	void* initializeNotifier(void (*ProcessQueue)(uint32_t, void*), int32_t *status);
+	void* initializeNotifier(void (*ProcessQueue)(uint32_t, void*), void* param, int32_t *status);
 	void cleanNotifier(void* notifier_pointer, int32_t *status);
 
 	void updateNotifierAlarm(void* notifier_pointer, uint32_t triggerTime, int32_t *status);

hal/lib/Athena/Notifier.cpp

@@ -1,40 +1,143 @@
 #include "HAL/Notifier.hpp"
 #include "ChipObject.h"
+#include "HAL/HAL.hpp"
+#include "HAL/cpp/priority_mutex.h"
+#include <atomic>
+#include <cstdlib>
+#include <mutex>
 
 static const uint32_t kTimerInterruptNumber = 28;
 
-struct Notifier
-{
-	tAlarm *alarm;
-	tInterruptManager *manager;
+static priority_mutex notifierInterruptMutex;
+static priority_recursive_mutex notifierMutex;
+static tAlarm *notifierAlarm = nullptr;
+static tInterruptManager *notifierManager = nullptr;
+static uint32_t closestTrigger = UINT32_MAX;
+struct Notifier {
+	Notifier *prev, *next;
+	void *param;
+	void (*process)(uint32_t, void*);
+	uint32_t triggerTime = UINT32_MAX;
 };
+static Notifier *notifiers = nullptr;
+static std::atomic_flag notifierAtexitRegistered = ATOMIC_FLAG_INIT;
+static std::atomic_int notifierRefCount = ATOMIC_VAR_INIT(0);
 
-void* initializeNotifier(void (*ProcessQueue)(uint32_t, void*), int32_t *status)
+static void alarmCallback(uint32_t, void*)
 {
+	std::unique_lock<priority_recursive_mutex> sync(notifierMutex);
+
+	int32_t status = 0;
+	uint32_t currentTime = 0;
+
+	// the hardware disables itself after each alarm
+	closestTrigger = UINT32_MAX;
+
+	// process all notifiers
+	Notifier *notifier = notifiers;
+	while (notifier) {
+		if (notifier->triggerTime != UINT32_MAX) {
+			if (currentTime == 0)
+				currentTime = getFPGATime(&status);
+			if (notifier->triggerTime < currentTime) {
+				notifier->triggerTime = UINT32_MAX;
+				auto process = notifier->process;
+				auto param = notifier->param;
+				sync.unlock();
+				process(currentTime, param);
+				sync.lock();
+			} else if (notifier->triggerTime < closestTrigger) {
+				updateNotifierAlarm(notifier, notifier->triggerTime, &status);
+			}
+		}
+		notifier = notifier->next;
+	}
+}
+
+static void cleanupNotifierAtExit() {
+	notifierAlarm = nullptr;
+	notifierManager = nullptr;
+}
+
+void* initializeNotifier(void (*ProcessQueue)(uint32_t, void*), void *param, int32_t *status)
+{
+	if (!ProcessQueue) {
+		*status = NULL_PARAMETER;
+		return nullptr;
+	}
+	if (!notifierAtexitRegistered.test_and_set())
+		std::atexit(cleanupNotifierAtExit);
+	if (notifierRefCount.fetch_add(1) == 0) {
+		std::lock_guard<priority_mutex> sync(notifierInterruptMutex);
+		// create manager and alarm if not already created
+		if (!notifierManager) {
+			notifierManager = new tInterruptManager(1 << kTimerInterruptNumber, false, status);
+			notifierManager->registerHandler(alarmCallback, NULL, status);
+			notifierManager->enable(status);
+		}
+		if (!notifierAlarm) notifierAlarm = tAlarm::create(status);
+	}
+
+	std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
+	// create notifier structure and add to list
 	Notifier* notifier = new Notifier();
-	notifier->manager = new tInterruptManager(1 << kTimerInterruptNumber, false, status);
-	notifier->manager->registerHandler(ProcessQueue, NULL, status);
-	notifier->manager->enable(status);
-	notifier->alarm = tAlarm::create(status);
+	notifier->prev = nullptr;
+	notifier->next = notifiers;
+	if (notifier->next) notifier->next->prev = notifier;
+	notifier->param = param;
+	notifier->process = ProcessQueue;
+	notifiers = notifier;
 	return notifier;
 }
 
 void cleanNotifier(void* notifier_pointer, int32_t *status)
 {
-	Notifier* notifier = (Notifier*)notifier_pointer;
-	notifier->alarm->writeEnable(false, status);
-	delete notifier->alarm;
-	notifier->alarm = NULL;
-	notifier->manager->disable(status);
-	delete notifier->manager;
-	notifier->manager = NULL;
+	{
+		std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
+		Notifier* notifier = (Notifier*)notifier_pointer;
+
+		// remove from list and delete
+		if (notifier->prev) notifier->prev->next = notifier->next;
+		if (notifier->next) notifier->next->prev = notifier->prev;
+		if (notifiers == notifier) notifiers = notifier->next;
+		delete notifier;
+	}
+
+	if (notifierRefCount.fetch_sub(1) == 1) {
+		std::lock_guard<priority_mutex> sync(notifierInterruptMutex);
+		// if this was the last notifier, clean up alarm and manager
+		if (notifierAlarm) {
+			notifierAlarm->writeEnable(false, status);
+			delete notifierAlarm;
+			notifierAlarm = nullptr;
+		}
+		if (notifierManager) {
+			notifierManager->disable(status);
+			delete notifierManager;
+			notifierManager = nullptr;
+		}
+	}
 }
 
 void updateNotifierAlarm(void* notifier_pointer, uint32_t triggerTime, int32_t *status)
 {
+	std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
+
 	Notifier* notifier = (Notifier*)notifier_pointer;
-	// write the first item in the queue into the trigger time
-	notifier->alarm->writeTriggerTime(triggerTime, status);
+	notifier->triggerTime = triggerTime;
+	bool wasActive = (closestTrigger != UINT32_MAX);
+
+        if (!notifierInterruptMutex.try_lock() || notifierRefCount == 0 ||
+            !notifierAlarm)
+          return;
+
+        // Update alarm time if closer than current.
+	if (triggerTime < closestTrigger) {
+		closestTrigger = triggerTime;
+		notifierAlarm->writeTriggerTime(triggerTime, status);
+	}
 	// Enable the alarm.  The hardware disables itself after each alarm.
-	notifier->alarm->writeEnable(true, status);
+	if (!wasActive) notifierAlarm->writeEnable(true, status);
+
+	notifierInterruptMutex.unlock();
 }

wpilibc/Athena/src/Notifier.cpp

@@ -15,7 +15,7 @@ Notifier *Notifier::timerQueueHead = nullptr;
 priority_recursive_mutex Notifier::queueMutex;
 priority_mutex Notifier::halMutex;
 void *Notifier::m_notifier = nullptr;
-std::atomic<int> Notifier::refcount{0};
+std::atomic<int> Notifier::refcount = ATOMIC_VAR_INIT(0);
 
 /**
  * Create a Notifier for timer event notification.
@@ -33,7 +33,7 @@ Notifier::Notifier(TimerEventHandler handler, void *param) {
     {
       std::lock_guard<priority_mutex> sync(halMutex);
       if (!m_notifier)
-        m_notifier = initializeNotifier(ProcessQueue, &status);
+        m_notifier = initializeNotifier(ProcessQueue, nullptr, &status);
     }
     wpi_setErrorWithContext(status, getHALErrorMessage(status));
   }
@@ -105,13 +105,13 @@ void Notifier::UpdateAlarm() {
  * 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 mask, void *params) {
+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 = GetClock();
+      double currentTime = currentTimeInt * 1.0e-6;
       current = timerQueueHead;
       if (current == nullptr || current->m_expirationTime > currentTime) {
         break;  // no more timer events to process

wpilibj/src/athena/cpp/lib/NotifierJNI.cpp

@@ -22,7 +22,7 @@ static jmethodID mid_global;
 
 // The arguments are unused by the HAL Notifier; they just satisfy a particular
 // function signature.
-void notifierHandler(uint32_t mask, void* param) {
+void notifierHandler(uint32_t currentTimeInt, void* param) {
   jobject handler_obj = func_global;
   jmethodID mid = mid_global;
 
@@ -82,7 +82,7 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_initializeNot
   mid_global = mid;
 
 	int32_t status = 0;
-	void *notifierPtr = initializeNotifier(notifierHandler, &status);
+	void *notifierPtr = initializeNotifier(notifierHandler, nullptr, &status);
 
 	NOTIFIERJNI_LOG(logDEBUG) << "Notifier Ptr = " << notifierPtr;
 	NOTIFIERJNI_LOG(logDEBUG) << "Status = " << status;