From 03c43a46d77ebd388af45ae0a868719c26afc678 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 21 Nov 2015 22:17:11 -0800 Subject: [PATCH] HAL: Add support for multiple Notifiers. This is a poor man's version of the multi-instance Notifier support in the higher level languages. It's intended primarily so that notifiers can be created internal to the HAL. One benefit of this change is that the current FPGA timestamp is passed as the first parameter to the ProcessQueue function (rather than the useless interrupt mask). Caution for other languages wrapping the HAL: this adds a parameter to initializeNotifier(). An atexit hook is used for safe cleanup at program termination. Change-Id: I782b3a74c10215588ae9b7191906fb4186a81028 --- hal/include/HAL/Notifier.hpp | 2 +- hal/lib/Athena/Notifier.cpp | 141 ++++++++++++++++++--- wpilibc/Athena/src/Notifier.cpp | 8 +- wpilibj/src/athena/cpp/lib/NotifierJNI.cpp | 4 +- 4 files changed, 129 insertions(+), 26 deletions(-) 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;