diff --git a/hal/include/HAL/Notifier.hpp b/hal/include/HAL/Notifier.hpp index 7eae360071..f16572ff8b 100644 --- a/hal/include/HAL/Notifier.hpp +++ b/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); diff --git a/hal/lib/Athena/Notifier.cpp b/hal/lib/Athena/Notifier.cpp index c532512775..8953c6c30c 100644 --- a/hal/lib/Athena/Notifier.cpp +++ b/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 +#include +#include 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 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 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 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 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 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 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(); } diff --git a/wpilibc/Athena/src/Notifier.cpp b/wpilibc/Athena/src/Notifier.cpp index faee0bc8b0..d70a2c869d 100644 --- a/wpilibc/Athena/src/Notifier.cpp +++ b/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 Notifier::refcount{0}; +std::atomic 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 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 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 diff --git a/wpilibj/src/athena/cpp/lib/NotifierJNI.cpp b/wpilibj/src/athena/cpp/lib/NotifierJNI.cpp index 3b83eedfb0..43fe78acf5 100644 --- a/wpilibj/src/athena/cpp/lib/NotifierJNI.cpp +++ b/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;