diff --git a/hal/src/main/native/athena/Notifier.cpp b/hal/src/main/native/athena/Notifier.cpp index c30e8d1048..d454e2f954 100644 --- a/hal/src/main/native/athena/Notifier.cpp +++ b/hal/src/main/native/athena/Notifier.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2016-2020 FIRST. 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 the root directory of */ /* the project. */ @@ -10,11 +10,13 @@ #include #include // For std::atexit() #include +#include #include #include #include "HALInitializer.h" +#include "HALInternal.h" #include "hal/ChipObject.h" #include "hal/Errors.h" #include "hal/HAL.h" @@ -26,7 +28,7 @@ static constexpr int32_t kTimerInterruptNumber = 28; static wpi::mutex notifierMutex; static std::unique_ptr notifierAlarm; -static std::unique_ptr notifierManager; +static std::thread notifierThread; static uint64_t closestTrigger{UINT64_MAX}; namespace { @@ -43,6 +45,7 @@ struct Notifier { static std::atomic_flag notifierAtexitRegistered{ATOMIC_FLAG_INIT}; static std::atomic_int notifierRefCount{0}; +static std::atomic_bool notifierRunning{false}; using namespace hal; @@ -65,7 +68,7 @@ class NotifierHandleContainer static NotifierHandleContainer* notifierHandles; -static void alarmCallback(uint32_t, void*) { +static void alarmCallback() { std::scoped_lock lock(notifierMutex); int32_t status = 0; uint64_t currentTime = 0; @@ -97,9 +100,24 @@ static void alarmCallback(uint32_t, void*) { } } +static void notifierThreadMain() { + tRioStatusCode status = 0; + tInterruptManager manager{1 << kTimerInterruptNumber, true, &status}; + while (notifierRunning) { + auto triggeredMask = manager.watch(10000, false, &status); + if (!notifierRunning) break; + if (triggeredMask == 0) continue; + alarmCallback(); + } +} + static void cleanupNotifierAtExit() { + int32_t status = 0; + if (notifierAlarm) notifierAlarm->writeEnable(false, &status); notifierAlarm = nullptr; - notifierManager = nullptr; + notifierRunning = false; + hal::ReleaseFPGAInterrupt(kTimerInterruptNumber); + if (notifierThread.joinable()) notifierThread.join(); } namespace hal { @@ -120,14 +138,8 @@ HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) { if (notifierRefCount.fetch_add(1) == 0) { std::scoped_lock lock(notifierMutex); - // create manager and alarm if not already created - if (!notifierManager) { - notifierManager = std::make_unique( - 1 << kTimerInterruptNumber, false, status); - notifierManager->registerHandler(alarmCallback, nullptr, status); - notifierManager->enable(status); - } - if (!notifierAlarm) notifierAlarm.reset(tAlarm::create(status)); + notifierThread = std::thread(notifierThreadMain); + notifierAlarm.reset(tAlarm::create(status)); } std::shared_ptr notifier = std::make_shared(); @@ -169,21 +181,19 @@ void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) { notifier->cond.notify_all(); if (notifierRefCount.fetch_sub(1) == 1) { - // if this was the last notifier, clean up alarm and manager + // if this was the last notifier, clean up alarm and thread // the notifier can call back into our callback, so don't hold the lock // here (the atomic fetch_sub will prevent multiple parallel entries // into this function) - // Cleaning up the manager takes up to a second to complete, so don't do - // that here. Fix it more permanently in 2019... + if (notifierAlarm) notifierAlarm->writeEnable(false, status); + notifierRunning = false; + hal::ReleaseFPGAInterrupt(kTimerInterruptNumber); + if (notifierThread.joinable()) notifierThread.join(); - // if (notifierAlarm) notifierAlarm->writeEnable(false, status); - // if (notifierManager) notifierManager->disable(status); - - // std::scoped_lock lock(notifierMutex); - // notifierAlarm = nullptr; - // notifierManager = nullptr; - // closestTrigger = UINT64_MAX; + std::scoped_lock lock(notifierMutex); + notifierAlarm = nullptr; + closestTrigger = UINT64_MAX; } }