From 75463a249f800723ddc1abe6d18661ed5424261c Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 23 Oct 2016 14:34:43 -0700 Subject: [PATCH] Implements threaded notifiers and interrupts in the HAL (#281) --- hal/include/HAL/Interrupts.h | 3 ++ hal/include/HAL/Notifier.h | 2 + hal/lib/athena/Interrupts.cpp | 63 ++++++++++++++++++++++ hal/lib/athena/Notifier.cpp | 98 ++++++++++++++++++++++++++++++++++- 4 files changed, 165 insertions(+), 1 deletion(-) diff --git a/hal/include/HAL/Interrupts.h b/hal/include/HAL/Interrupts.h index 2578673e63..10ee6720f4 100644 --- a/hal/include/HAL/Interrupts.h +++ b/hal/include/HAL/Interrupts.h @@ -39,6 +39,9 @@ void HAL_RequestInterrupts(HAL_InterruptHandle interruptHandle, void HAL_AttachInterruptHandler(HAL_InterruptHandle interruptHandle, HAL_InterruptHandlerFunction handler, void* param, int32_t* status); +void HAL_AttachInterruptHandlerThreaded(HAL_InterruptHandle interruptHandle, + HAL_InterruptHandlerFunction handler, + void* param, int32_t* status); void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle, HAL_Bool risingEdge, HAL_Bool fallingEdge, int32_t* status); diff --git a/hal/include/HAL/Notifier.h b/hal/include/HAL/Notifier.h index aca5e6bdf8..b20c7b0f0a 100644 --- a/hal/include/HAL/Notifier.h +++ b/hal/include/HAL/Notifier.h @@ -20,6 +20,8 @@ typedef void (*HAL_NotifierProcessFunction)(uint64_t currentTime, HAL_NotifierHandle HAL_InitializeNotifier(HAL_NotifierProcessFunction process, void* param, int32_t* status); +HAL_NotifierHandle HAL_InitializeNotifierThreaded( + HAL_NotifierProcessFunction process, void* param, int32_t* status); void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status); void* HAL_GetNotifierParam(HAL_NotifierHandle notifierHandle, int32_t* status); void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle, diff --git a/hal/lib/athena/Interrupts.cpp b/hal/lib/athena/Interrupts.cpp index caf0c6f56c..7481601024 100644 --- a/hal/lib/athena/Interrupts.cpp +++ b/hal/lib/athena/Interrupts.cpp @@ -16,6 +16,7 @@ #include "HAL/handles/HandlesInternal.h" #include "HAL/handles/LimitedHandleResource.h" #include "PortsInternal.h" +#include "support/SafeThread.h" using namespace hal; @@ -24,6 +25,53 @@ struct Interrupt { std::unique_ptr anInterrupt; std::unique_ptr manager; }; + +// Safe thread to allow callbacks to run on their own thread +class InterruptThread : public wpi::SafeThread { + public: + void Main() { + std::unique_lock lock(m_mutex); + while (m_active) { + m_cond.wait(lock, [&] { return !m_active || m_notify; }); + if (!m_active) break; + m_notify = false; + HAL_InterruptHandlerFunction handler = m_handler; + uint32_t mask = m_mask; + void* param = m_param; + lock.unlock(); // don't hold mutex during callback execution + handler(mask, param); + lock.lock(); + } + } + + bool m_notify = false; + HAL_InterruptHandlerFunction m_handler; + void* m_param; + uint32_t m_mask; +}; + +class InterruptThreadOwner : public wpi::SafeThreadOwner { + public: + void SetFunc(HAL_InterruptHandlerFunction handler, void* param) { + auto thr = GetThread(); + if (!thr) return; + thr->m_handler = handler; + thr->m_param = param; + } + + void Notify(uint32_t mask) { + auto thr = GetThread(); + if (!thr) return; + thr->m_mask = mask; + thr->m_notify = true; + thr->m_cond.notify_one(); + } +}; + +} // namespace + +static void threadedInterruptHandler(uint32_t mask, void* param) { + (static_cast(param))->Notify(mask); } static LimitedHandleResourcemanager->registerHandler(handler, param, status); } +void HAL_AttachInterruptHandlerThreaded(HAL_InterruptHandle interrupt_handle, + HAL_InterruptHandlerFunction handler, + void* param, int32_t* status) { + InterruptThreadOwner* intr = new InterruptThreadOwner; + intr->Start(); + intr->SetFunc(handler, param); + + HAL_AttachInterruptHandler(interrupt_handle, threadedInterruptHandler, intr, + status); + + if (*status != 0) { + delete intr; + } +} + void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle, HAL_Bool risingEdge, HAL_Bool fallingEdge, int32_t* status) { diff --git a/hal/lib/athena/Notifier.cpp b/hal/lib/athena/Notifier.cpp index 8a74b87495..4a19eeb132 100644 --- a/hal/lib/athena/Notifier.cpp +++ b/hal/lib/athena/Notifier.cpp @@ -19,6 +19,7 @@ #include "HAL/cpp/make_unique.h" #include "HAL/cpp/priority_mutex.h" #include "HAL/handles/UnlimitedHandleResource.h" +#include "support/SafeThread.h" static const int32_t kTimerInterruptNumber = 28; @@ -35,8 +36,55 @@ struct Notifier { HAL_NotifierProcessFunction process; uint64_t triggerTime = UINT64_MAX; HAL_NotifierHandle handle; + bool threaded; }; -} + +// Safe thread to allow callbacks to run on their own thread +class NotifierThread : public wpi::SafeThread { + public: + void Main() { + std::unique_lock lock(m_mutex); + while (m_active) { + m_cond.wait(lock, [&] { return !m_active || m_notify; }); + if (!m_active) break; + m_notify = false; + uint64_t currentTime = m_currentTime; + HAL_NotifierHandle handle = m_handle; + HAL_NotifierProcessFunction process = m_process; + lock.unlock(); // don't hold mutex during callback execution + process(currentTime, handle); + lock.lock(); + } + } + + bool m_notify = false; + HAL_NotifierHandle m_handle = HAL_kInvalidHandle; + HAL_NotifierProcessFunction m_process; + uint64_t m_currentTime; +}; + +class NotifierThreadOwner : public wpi::SafeThreadOwner { + public: + void SetFunc(HAL_NotifierProcessFunction process, void* param) { + auto thr = GetThread(); + if (!thr) return; + thr->m_process = process; + m_param = param; + } + + void Notify(uint64_t currentTime, HAL_NotifierHandle handle) { + auto thr = GetThread(); + if (!thr) return; + thr->m_currentTime = currentTime; + thr->m_handle = handle; + thr->m_notify = true; + thr->m_cond.notify_one(); + } + + void* m_param; +}; +} // namespace + static std::shared_ptr notifiers; static std::atomic_flag notifierAtexitRegistered = ATOMIC_FLAG_INIT; static std::atomic_int notifierRefCount{0}; @@ -108,6 +156,18 @@ static void cleanupNotifierAtExit() { notifierManager = nullptr; } +static void threadedNotifierHandler(uint64_t currentTimeInt, + HAL_NotifierHandle handle) { + // Grab notifier and get handler param + auto notifier = notifierHandles.Get(handle); + if (!notifier) return; + auto notifierPointer = notifier->param; + if (notifierPointer == nullptr) return; + NotifierThreadOwner* owner = + static_cast(notifierPointer); + owner->Notify(currentTimeInt, handle); +} + extern "C" { HAL_NotifierHandle HAL_InitializeNotifier(HAL_NotifierProcessFunction process, @@ -143,10 +203,34 @@ HAL_NotifierHandle HAL_InitializeNotifier(HAL_NotifierProcessFunction process, notifier->param = param; notifier->process = process; notifier->handle = handle; + notifier->threaded = false; notifiers = notifier; return handle; } +HAL_NotifierHandle HAL_InitializeNotifierThreaded( + HAL_NotifierProcessFunction process, void* param, int32_t* status) { + NotifierThreadOwner* notify = new NotifierThreadOwner; + notify->Start(); + notify->SetFunc(process, param); + + auto notifierHandle = + HAL_InitializeNotifier(threadedNotifierHandler, notify, status); + + if (notifierHandle == HAL_kInvalidHandle || *status != 0) { + delete notify; + return HAL_kInvalidHandle; + } + + auto notifier = notifierHandles.Get(notifierHandle); + if (!notifier) { + return HAL_kInvalidHandle; + } + notifier->threaded = true; + + return notifierHandle; +} + void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) { { std::lock_guard sync(notifierMutex); @@ -158,6 +242,12 @@ void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) { if (notifier->next) notifier->next->prev = notifier->prev; if (notifiers == notifier) notifiers = notifier->next; notifierHandles.Free(notifierHandle); + + if (notifier->threaded) { + NotifierThreadOwner* owner = + static_cast(notifier->param); + delete owner; + } } if (notifierRefCount.fetch_sub(1) == 1) { @@ -178,6 +268,12 @@ void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) { void* HAL_GetNotifierParam(HAL_NotifierHandle notifierHandle, int32_t* status) { auto notifier = notifierHandles.Get(notifierHandle); if (!notifier) return nullptr; + if (notifier->threaded) { + // If threaded, return thread param rather then notifier param + NotifierThreadOwner* owner = + static_cast(notifier->param); + return owner->m_param; + } return notifier->param; }