Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal] Revamp notifiers (#8424)

Browse Source 
This changes the HAL notifier interface to:
- Use wpiutil signal objects. This means waiting is done through the
`WPI_WaitObject` API instead of a dedicated function and allows for
higher level code to simultaneously wait on notifiers and other events.
- Interval timers are supported at the HAL layer
- Handlers are now required to acknowledge notifications. This is
invisible to users unless they're directly using the HAL API.
- For interval timers, an overrun count is maintained to detect if the
handler didn't acknowledge

The underlying implementation still uses condition variables for the
actual waiting. In basic testing using this approach seemed to be lower
jitter than timerfd.

Currently, the simulation and systemcore implementations are nearly
identical except for a few additional sim hook bits. This could be
refactored, but keeping them separate may make sense to keep the
systemcore implementation easy to read and reason about, or if we ever
choose to use a different underlying timer implementation on systemcore.

The simulation side API is unchanged in form but does change in
function--waiting for notifiers now only waits for currently running (or
newly signaled) notifiers to acknowledge. To avoid a race condition in
sim stepTiming, users of the low level API must make any alarm updates
(especially for one-shot alarms) prior to acknowledging the previous
alarm.

The only current use of the interval timer feature is the `Notifier`
class. The `TimedRobot` implementation still uses a single notifier and
its own interval timing logic to ensure consistent callback order. Using
separate notifiers for each user-level interval would substantially
increase complexity. `Watchdog` also doesn't use the interval timer, as
it's looking for an amount of time since the last `set` call rather than
a recurring interval time.

To reduce flicker, the sim GUI uses a fade out when a timeout goes from
set to unset.

This fixes tsan for wpilib and commands, and also fixes some spurious
test failures.
This commit is contained in:
Peter Johnson
2025-11-29 11:00:18 -08:00
committed by GitHub
parent 704b6ccaee
commit 02c8d5c9db
22 changed files with 738 additions and 712 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
hal/src/main/native/cpp/jni/NotifierJNI.cpp
View File

@@ -11,6 +11,7 @@
#include "org_wpilib_hardware_hal_NotifierJNI.h"
#include "wpi/hal/Notifier.h"
#include "wpi/util/jni_util.hpp"
#include "wpi/util/string.h"
using namespace wpi::hal;
@@ -18,15 +19,15 @@ extern "C" {
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: initializeNotifier
* Method: createNotifier
* Signature: ()I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_initializeNotifier
Java_org_wpilib_hardware_hal_NotifierJNI_createNotifier
(JNIEnv* env, jclass)
{
int32_t status = 0;
HAL_NotifierHandle notifierHandle = HAL_InitializeNotifier(&status);
HAL_NotifierHandle notifierHandle = HAL_CreateNotifier(&status);
if (notifierHandle <= 0 || !CheckStatusForceThrow(env, status)) {
return 0; // something went wrong in HAL
@@ -58,51 +59,40 @@ Java_org_wpilib_hardware_hal_NotifierJNI_setNotifierName
(JNIEnv* env, jclass cls, jint notifierHandle, jstring name)
{
int32_t status = 0;
HAL_SetNotifierName((HAL_NotifierHandle)notifierHandle,
wpi::util::java::JStringRef{env, name}.c_str(), &status);
wpi::util::java::JStringRef jname{env, name};
WPI_String wpiName = wpi::util::make_string(jname);
HAL_SetNotifierName((HAL_NotifierHandle)notifierHandle, &wpiName, &status);
CheckStatus(env, status);
}
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: stopNotifier
* Method: destroyNotifier
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_stopNotifier
(JNIEnv* env, jclass cls, jint notifierHandle)
{
int32_t status = 0;
HAL_StopNotifier((HAL_NotifierHandle)notifierHandle, &status);
CheckStatus(env, status);
}
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: cleanNotifier
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_cleanNotifier
Java_org_wpilib_hardware_hal_NotifierJNI_destroyNotifier
(JNIEnv* env, jclass, jint notifierHandle)
{
if (notifierHandle != HAL_kInvalidHandle) {
HAL_CleanNotifier((HAL_NotifierHandle)notifierHandle);
HAL_DestroyNotifier((HAL_NotifierHandle)notifierHandle);
}
}
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: updateNotifierAlarm
* Signature: (IJ)V
* Method: setNotifierAlarm
* Signature: (IJJZ)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_updateNotifierAlarm
(JNIEnv* env, jclass cls, jint notifierHandle, jlong triggerTime)
Java_org_wpilib_hardware_hal_NotifierJNI_setNotifierAlarm
(JNIEnv* env, jclass cls, jint notifierHandle, jlong alarmTime,
jlong intervalTime, jboolean absolute)
{
int32_t status = 0;
HAL_UpdateNotifierAlarm((HAL_NotifierHandle)notifierHandle,
static_cast<uint64_t>(triggerTime), &status);
HAL_SetNotifierAlarm((HAL_NotifierHandle)notifierHandle,
static_cast<uint64_t>(alarmTime),
static_cast<uint64_t>(intervalTime), absolute, &status);
CheckStatus(env, status);
}
@@ -122,20 +112,35 @@ Java_org_wpilib_hardware_hal_NotifierJNI_cancelNotifierAlarm
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: waitForNotifierAlarm
* Signature: (I)J
* Method: acknowledgeNotifierAlarm
* Signature: (I)V
*/
JNIEXPORT jlong JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_waitForNotifierAlarm
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_acknowledgeNotifierAlarm
(JNIEnv* env, jclass cls, jint notifierHandle)
{
int32_t status = 0;
uint64_t time =
HAL_WaitForNotifierAlarm((HAL_NotifierHandle)notifierHandle, &status);
HAL_AcknowledgeNotifierAlarm((HAL_NotifierHandle)notifierHandle, &status);
CheckStatus(env, status);
}
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: getNotifierOverrun
* Signature: (I)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_getNotifierOverrun
(JNIEnv* env, jclass cls, jint notifierHandle)
{
int32_t status = 0;
int32_t count =
HAL_GetNotifierOverrun((HAL_NotifierHandle)notifierHandle, &status);
CheckStatus(env, status);
return (jlong)time;
return (jint)count;
}
} // extern "C"

102
hal/src/main/native/include/wpi/hal/Notifier.h
View File

@@ -6,8 +6,12 @@
#include <stdint.h>
#ifdef __cplusplus
#include <string_view>
#endif
#include "wpi/hal/Types.h"
#include "wpi/util/nodiscard.h"
#include "wpi/util/string.h"
/**
* @defgroup hal_notifier Notifier Functions
@@ -20,20 +24,20 @@ extern "C" {
#endif
/**
* Initializes a notifier.
* Creates a notifier.
*
* A notifier is an FPGA controller timer that triggers at requested intervals
* based on the FPGA time. This can be used to make precise control loops.
* A notifier is an timer that alarms at an initial and (optionally) repeated
* intervals. This can be used to make precise control loops.
*
* @param[out] status Error status variable. 0 on success.
* @return the created notifier
*/
HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status);
HAL_NotifierHandle HAL_CreateNotifier(int32_t* status);
/**
* Sets the HAL notifier thread priority.
*
* The HAL notifier thread is responsible for managing the FPGA's notifier
* The HAL notifier thread is responsible for managing the system's notifier
* interrupt and waking up user's Notifiers when it's their time to run.
* Giving the HAL notifier thread real-time priority helps ensure the user's
* real-time Notifiers, if any, are notified to run in a timely manner.
@@ -56,45 +60,41 @@ HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
* @param[in] name name
* @param[out] status Error status variable. 0 on success.
*/
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
int32_t* status);
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle,
const struct WPI_String* name, int32_t* status);
/**
* Stops a notifier from running.
* Destroys a notifier.
*
* This will cause any call into HAL_WaitForNotifierAlarm to return with time =
* 0.
*
* @param[in] notifierHandle the notifier handle
* @param[out] status Error status variable. 0 on success.
*/
void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status);
/**
* Cleans a notifier.
*
* Note this also stops a notifier if it is already running.
* Destruction wakes up any waiters.
*
* @param[in] notifierHandle the notifier handle
*/
void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle);
void HAL_DestroyNotifier(HAL_NotifierHandle notifierHandle);
/**
* Updates the trigger time for a notifier.
* Updates the initial and interval alarm times for a notifier.
*
* Note that this time is an absolute time relative to HAL_GetFPGATime()
* The alarmTime is an absolute time (using the WPI_Now() time base) if
* absolute is true, or relative to the current time if absolute is false.
*
* If intervalTime is non-zero, the notifier will alarm periodically following
* alarmTime at the given interval.
*
* If an absolute alarmTime is in the past, the notifier will alarm immediately.
*
* @param[in] notifierHandle the notifier handle
* @param[in] triggerTime the updated trigger time
* @param[in] alarmTime the first alarm time (in microseconds)
* @param[in] intervalTime the periodic interval time (in microseconds)
* @param[in] absolute true if the alarm time is absolute
* @param[out] status Error status variable. 0 on success.
*/
void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
uint64_t triggerTime, int32_t* status);
void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime,
uint64_t intervalTime, HAL_Bool absolute,
int32_t* status);
/**
* Cancels the next notifier alarm.
*
* This does not cause HAL_WaitForNotifierAlarm to return.
* Cancels all future notifier alarms for a notifier.
*
* @param[in] notifierHandle the notifier handle
* @param[out] status Error status variable. 0 on success.
@@ -103,22 +103,44 @@ void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status);
/**
* Waits for the next alarm for the specific notifier.
*
* This is a blocking call until either the time elapses or HAL_StopNotifier
* gets called. If the latter occurs, this function will return zero and any
* loops using this function should exit. Failing to do so can lead to
* use-after-frees.
* Indicates the notifier alarm has been serviced. This must be called before
* waiting for the next alarm.
*
* @param[in] notifierHandle the notifier handle
* @param[out] status Error status variable. 0 on success.
* @return the FPGA time the notifier returned
* @param[out] status Error status variable. 0 on success.
*/
WPI_NODISCARD
uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status);
/**
* Gets the overrun count for a notifier.
*
* An overrun occurs when a notifier's alarm is not serviced before the next
* scheduled alarm time.
*
* @param[in] notifierHandle the notifier handle
* @param[out] status Error status variable. 0 on success.
* @return overrun count
*/
int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle,
int32_t* status);
#ifdef __cplusplus
} // extern "C"
#endif
/** @} */
#ifdef __cplusplus
/**
* Sets the name of a notifier.
*
* @param[in] notifierHandle the notifier handle
* @param[in] name name
* @param[out] status Error status variable. 0 on success.
*/
inline void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle,
std::string_view name, int32_t* status) {
WPI_String nameStr = wpi::util::make_string(name);
HAL_SetNotifierName(notifierHandle, &nameStr, status);
}
#endif

5
hal/src/main/native/include/wpi/hal/simulation/NotifierData.h
View File

@@ -13,8 +13,9 @@ extern "C" {
struct HALSIM_NotifierInfo {
HAL_NotifierHandle handle;
char name[64];
uint64_t timeout;
HAL_Bool waitTimeValid;
uint64_t alarmTime;
uint64_t intervalTime;
int32_t overrunCount;
};
uint64_t HALSIM_GetNextNotifierTimeout(void);

432
hal/src/main/native/sim/Notifier.cpp
View File

@@ -4,317 +4,330 @@
#include "wpi/hal/Notifier.h"
#include <sys/types.h>
#include <atomic>
#include <chrono>
#include <cstdio>
#include <cstring>
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "HALInitializer.h"
#include "NotifierInternal.h"
#include "wpi/hal/Errors.h"
#include "wpi/hal/HALBase.h"
#include "wpi/hal/cpp/fpga_clock.h"
#include "wpi/hal/Threads.h"
#include "wpi/hal/Types.h"
#include "wpi/hal/handles/UnlimitedHandleResource.h"
#include "wpi/hal/simulation/NotifierData.h"
#include "wpi/util/SafeThread.hpp"
#include "wpi/util/SmallVector.hpp"
#include "wpi/util/StringExtras.hpp"
#include "wpi/util/condition_variable.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/Synchronization.h"
#include "wpi/util/priority_queue.hpp"
namespace {
struct Notifier {
std::string name;
uint64_t waitTime = UINT64_MAX;
bool active = true;
bool waitTimeValid = false; // True if waitTime is set and in the future
bool waitingForAlarm = false; // True if in HAL_WaitForNotifierAlarm()
uint64_t waitCount = 0; // Counts calls to HAL_WaitForNotifierAlarm()
wpi::util::mutex mutex;
wpi::util::condition_variable cond;
std::atomic<uint64_t> alarmTime = UINT64_MAX;
uint64_t intervalTime = 0;
std::atomic<int32_t> userOverrunCount = 0;
int32_t overrunCount = 0;
std::atomic_flag handlerSignaled{};
};
} // namespace
using namespace wpi::hal;
static wpi::util::mutex notifiersWaiterMutex;
static wpi::util::condition_variable notifiersWaiterCond;
class NotifierHandleContainer
: public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
HAL_HandleEnum::Notifier> {
class NotifierThread : public wpi::util::SafeThread {
public:
~NotifierHandleContainer() {
ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
}
notifier->cond.notify_all(); // wake up any waiting threads
});
notifiersWaiterCond.notify_all();
}
void Main() override;
void ProcessAlarms(wpi::util::SmallVectorImpl<HAL_NotifierHandle>* signaled);
bool m_paused = false;
UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
HAL_HandleEnum::Notifier>
m_handles;
struct Alarm {
HAL_NotifierHandle handle;
std::shared_ptr<Notifier> notifier;
bool operator==(const Alarm& rhs) const { return handle == rhs.handle; }
bool operator>(const Alarm& rhs) const {
return notifier->alarmTime > rhs.notifier->alarmTime;
}
};
wpi::util::priority_queue<Alarm, std::vector<Alarm>, std::greater<Alarm>>
m_alarmQueue;
};
static NotifierHandleContainer* notifierHandles;
static std::atomic<bool> notifiersPaused{false};
class NotifierInstance {
public:
NotifierInstance() { owner.Start(); }
wpi::util::SafeThreadOwner<NotifierThread> owner;
};
namespace wpi::hal {
namespace init {
static NotifierInstance* notifierInstance;
namespace wpi::hal::init {
void InitializeNotifier() {
static NotifierHandleContainer nH;
notifierHandles = &nH;
static NotifierInstance n;
notifierInstance = &n;
}
} // namespace init
} // namespace wpi::hal::init
void PauseNotifiers() {
notifiersPaused = true;
}
void ResumeNotifiers() {
notifiersPaused = false;
WakeupNotifiers();
}
void WakeupNotifiers() {
notifierHandles->ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
notifier->cond.notify_all();
});
}
void WaitNotifiers() {
std::unique_lock ulock(notifiersWaiterMutex);
wpi::util::SmallVector<HAL_NotifierHandle, 8> waiters;
// Wait for all Notifiers to hit HAL_WaitForNotifierAlarm()
notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
std::scoped_lock lock(notifier->mutex);
if (notifier->active && !notifier->waitingForAlarm) {
waiters.emplace_back(handle);
void NotifierThread::Main() {
std::unique_lock lock(m_mutex);
while (m_active) {
if (m_paused || m_alarmQueue.empty()) {
// No alarms, wait indefinitely
m_cond.wait(lock);
continue;
}
});
for (;;) {
int count = 0;
int end = waiters.size();
while (count < end) {
auto& it = waiters[count];
if (auto notifier = notifierHandles->Get(it)) {
std::scoped_lock lock(notifier->mutex);
if (notifier->active && !notifier->waitingForAlarm) {
++count;
continue;
}
}
// No longer need to wait for it, put at end so it can be erased
std::swap(it, waiters[--end]);
// Wait until next alarm
const Alarm& alarm = m_alarmQueue.top();
int32_t status = 0;
uint64_t curTime = HAL_GetFPGATime(&status);
if (alarm.notifier->alarmTime > curTime) {
m_cond.wait_for(
lock, std::chrono::microseconds{alarm.notifier->alarmTime - curTime});
}
if (count == 0) {
if (!m_active) {
break;
}
waiters.resize(count);
notifiersWaiterCond.wait_for(ulock, std::chrono::duration<double>(1));
// Check paused again as we may have been paused while waiting
if (m_paused) {
continue;
}
ProcessAlarms(nullptr);
}
}
void WakeupWaitNotifiers() {
std::unique_lock ulock(notifiersWaiterMutex);
void NotifierThread::ProcessAlarms(
wpi::util::SmallVectorImpl<HAL_NotifierHandle>* signaled) {
int32_t status = 0;
uint64_t curTime = HAL_GetFPGATime(&status);
wpi::util::SmallVector<std::pair<HAL_NotifierHandle, uint64_t>, 8> waiters;
// Wake up Notifiers that have expired timeouts
notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
std::scoped_lock lock(notifier->mutex);
// Process alarms
while (!m_alarmQueue.empty() &&
m_alarmQueue.top().notifier->alarmTime <= curTime) {
Alarm alarm = m_alarmQueue.pop();
HAL_NotifierHandle handle = alarm.handle;
Notifier& notifier = *alarm.notifier;
// Only wait for the Notifier if it has a valid timeout that's expired
if (notifier->active && notifier->waitTimeValid &&
curTime >= notifier->waitTime) {
waiters.emplace_back(handle, notifier->waitCount);
notifier->cond.notify_all();
}
});
for (;;) {
int count = 0;
int end = waiters.size();
while (count < end) {
auto& it = waiters[count];
if (auto notifier = notifierHandles->Get(it.first)) {
std::scoped_lock lock(notifier->mutex);
// waitCount is used here instead of waitingForAlarm because we want to
// wait until HAL_WaitForNotifierAlarm() is exited, then reentered
if (notifier->active && notifier->waitCount == it.second) {
++count;
continue;
}
if (notifier.intervalTime > 0) {
// Schedule next alarm
notifier.alarmTime += notifier.intervalTime;
if (curTime >= notifier.alarmTime) {
// We missed at least one interval
int32_t missed = static_cast<int32_t>((curTime - notifier.alarmTime) /
notifier.intervalTime) +
1;
notifier.overrunCount += missed;
notifier.alarmTime +=
missed * notifier.intervalTime; // Skip missed intervals
}
// No longer need to wait for it, put at end so it can be erased
it.swap(waiters[--end]);
// Reinsert into queue
m_alarmQueue.push(std::move(alarm));
} else {
// Disable one-shot alarm
notifier.alarmTime = UINT64_MAX;
}
if (count == 0) {
break;
// If the last call was acknowledged, signal the handler
if (!notifier.handlerSignaled.test_and_set()) {
if (signaled) {
signaled->emplace_back(handle);
}
// copy the overrun count for the handler to read, reset the local count
notifier.userOverrunCount = notifier.overrunCount;
notifier.overrunCount = 0;
wpi::util::SetSignalObject(handle);
}
waiters.resize(count);
notifiersWaiterCond.wait_for(ulock, std::chrono::duration<double>(1));
}
}
} // namespace wpi::hal
void wpi::hal::PauseNotifiers() {
auto thr = notifierInstance->owner.GetThread();
thr->m_paused = true;
}
void wpi::hal::ResumeNotifiers() {
auto thr = notifierInstance->owner.GetThread();
thr->m_paused = false;
thr->m_cond.notify_all();
}
void wpi::hal::WakeupNotifiers() {
auto thr = notifierInstance->owner.GetThread();
thr->ProcessAlarms(nullptr);
}
static void DoWaitNotifiers(
wpi::util::detail::SafeThreadProxy<NotifierThread>& thr,
wpi::util::SmallVectorImpl<HAL_NotifierHandle>& signaled) {
// Wait for signaled notifiers to acknowledge their last alarm
for (;;) {
signaled.erase(std::remove_if(signaled.begin(), signaled.end(),
[&](HAL_NotifierHandle handle) {
auto notifier = thr->m_handles.Get(handle);
return !notifier ||
!notifier->handlerSignaled.test();
}),
signaled.end());
if (signaled.empty()) {
break;
}
thr->m_cond.wait_for(thr.GetLock(), std::chrono::milliseconds{1});
}
}
void wpi::hal::WaitNotifiers() {
auto thr = notifierInstance->owner.GetThread();
wpi::util::SmallVector<HAL_NotifierHandle, 8> signaled;
thr->m_handles.ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
if (notifier->handlerSignaled.test()) {
signaled.emplace_back(handle);
}
});
DoWaitNotifiers(thr, signaled);
}
void wpi::hal::WakeupWaitNotifiers() {
auto thr = notifierInstance->owner.GetThread();
wpi::util::SmallVector<HAL_NotifierHandle, 8> signaled;
thr->ProcessAlarms(&signaled);
DoWaitNotifiers(thr, signaled);
}
extern "C" {
HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
HAL_NotifierHandle HAL_CreateNotifier(int32_t* status) {
wpi::hal::init::CheckInit();
std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
HAL_NotifierHandle handle = notifierHandles->Allocate(notifier);
HAL_NotifierHandle handle =
notifierInstance->owner.GetThread()->m_handles.Allocate(notifier);
if (handle == HAL_kInvalidHandle) {
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
wpi::util::CreateSignalObject(handle);
return handle;
}
HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
int32_t* status) {
return true;
auto native = notifierInstance->owner.GetNativeThreadHandle();
return HAL_SetThreadPriority(&native, realTime, priority, status);
}
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle,
const WPI_String* name, int32_t* status) {
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
std::scoped_lock lock(notifier->mutex);
notifier->name = name;
notifier->name = wpi::util::to_string_view(name);
}
void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
void HAL_DestroyNotifier(HAL_NotifierHandle notifierHandle) {
wpi::util::DestroySignalObject(notifierHandle);
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Free(notifierHandle);
thr->m_alarmQueue.remove({notifierHandle, notifier});
}
void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime,
uint64_t intervalTime, HAL_Bool absolute,
int32_t* status) {
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
}
notifier->cond.notify_all();
}
void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle) {
auto notifier = notifierHandles->Free(notifierHandle);
if (!notifier) {
return;
if (!absolute) {
alarmTime += HAL_GetFPGATime(status);
}
// Just in case HAL_StopNotifier() wasn't called...
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
uint64_t prevWakeup = UINT64_MAX;
if (!thr->m_alarmQueue.empty()) {
prevWakeup = thr->m_alarmQueue.top().notifier->alarmTime;
thr->m_alarmQueue.remove({notifierHandle, notifier});
}
notifier->cond.notify_all();
}
notifier->alarmTime = alarmTime;
notifier->intervalTime = intervalTime;
notifier->overrunCount = 0;
thr->m_alarmQueue.push({notifierHandle, notifier});
void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
uint64_t triggerTime, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
if (!notifier) {
return;
// wake up notifier thread if needed
if (alarmTime < prevWakeup) {
thr->m_cond.notify_all();
}
{
std::scoped_lock lock(notifier->mutex);
notifier->waitTime = triggerTime;
notifier->waitTimeValid = (triggerTime != UINT64_MAX);
}
// We wake up any waiters to change how long they're sleeping for
notifier->cond.notify_all();
}
void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->waitTimeValid = false;
}
thr->m_alarmQueue.remove({notifierHandle, notifier});
notifier->alarmTime = UINT64_MAX;
}
uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
auto notifier =
notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
if (!notifier) {
return 0;
return;
}
notifier->handlerSignaled.clear();
}
std::unique_lock ulock(notifiersWaiterMutex);
std::unique_lock lock(notifier->mutex);
notifier->waitingForAlarm = true;
++notifier->waitCount;
ulock.unlock();
notifiersWaiterCond.notify_all();
while (notifier->active) {
uint64_t curTime = HAL_GetFPGATime(status);
if (notifier->waitTimeValid && curTime >= notifier->waitTime) {
notifier->waitTimeValid = false;
notifier->waitingForAlarm = false;
return curTime;
}
double waitDuration;
if (!notifier->waitTimeValid || notifiersPaused) {
// If not running, wait 1000 seconds
waitDuration = 1000.0;
} else {
waitDuration = (notifier->waitTime - curTime) * 1e-6;
}
notifier->cond.wait_for(lock, std::chrono::duration<double>(waitDuration));
int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier =
notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
if (!notifier) {
return -1;
}
notifier->waitingForAlarm = false;
return 0;
return notifier->userOverrunCount;
}
uint64_t HALSIM_GetNextNotifierTimeout(void) {
uint64_t timeout = UINT64_MAX;
notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
std::scoped_lock lock(notifier->mutex);
if (notifier->active && notifier->waitTimeValid &&
timeout > notifier->waitTime) {
timeout = notifier->waitTime;
}
});
return timeout;
auto thr = notifierInstance->owner.GetThread();
if (thr->m_alarmQueue.empty()) {
return UINT64_MAX;
}
return thr->m_alarmQueue.top().notifier->alarmTime;
}
int32_t HALSIM_GetNumNotifiers(void) {
auto thr = notifierInstance->owner.GetThread();
int32_t count = 0;
notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
std::scoped_lock lock(notifier->mutex);
if (notifier->active) {
++count;
}
});
thr->m_handles.ForEach([&](auto, auto) { ++count; });
return count;
}
int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) {
auto thr = notifierInstance->owner.GetThread();
int32_t num = 0;
notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
std::scoped_lock lock(notifier->mutex);
if (!notifier->active) {
return;
}
thr->m_handles.ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
if (num < size) {
arr[num].handle = handle;
if (notifier->name.empty()) {
@@ -326,8 +339,9 @@ int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) {
sizeof(arr[num].name) - 1);
arr[num].name[sizeof(arr[num].name) - 1] = '\0';
}
arr[num].timeout = notifier->waitTime;
arr[num].waitTimeValid = notifier->waitTimeValid;
arr[num].alarmTime = notifier->alarmTime;
arr[num].intervalTime = notifier->intervalTime;
arr[num].overrunCount = notifier->overrunCount;
}
++num;
});

263
hal/src/main/native/systemcore/Notifier.cpp
View File

@@ -6,182 +6,233 @@
#include <atomic>
#include <chrono>
#include <cstdio>
#include <cstring>
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "HALInitializer.h"
#include "wpi/hal/Errors.h"
#include "wpi/hal/HALBase.h"
#include "wpi/hal/cpp/fpga_clock.h"
#include "wpi/hal/Threads.h"
#include "wpi/hal/Types.h"
#include "wpi/hal/handles/UnlimitedHandleResource.h"
#include "wpi/hal/simulation/NotifierData.h"
#include "wpi/util/SmallVector.hpp"
#include "wpi/util/StringExtras.hpp"
#include "wpi/util/condition_variable.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/SafeThread.hpp"
#include "wpi/util/Synchronization.h"
#include "wpi/util/priority_queue.hpp"
namespace {
struct Notifier {
std::string name;
uint64_t waitTime = UINT64_MAX;
bool active = true;
bool waitTimeValid = false; // True if waitTime is set and in the future
wpi::util::mutex mutex;
wpi::util::condition_variable cond;
std::atomic<uint64_t> alarmTime = UINT64_MAX;
uint64_t intervalTime = 0;
std::atomic<int32_t> userOverrunCount = 0;
int32_t overrunCount = 0;
std::atomic_flag handlerSignaled{};
};
} // namespace
using namespace wpi::hal;
static wpi::util::mutex notifiersWaiterMutex;
static wpi::util::condition_variable notifiersWaiterCond;
class NotifierHandleContainer
: public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
HAL_HandleEnum::Notifier> {
class NotifierThread : public wpi::util::SafeThread {
public:
~NotifierHandleContainer() {
ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
}
notifier->cond.notify_all(); // wake up any waiting threads
});
notifiersWaiterCond.notify_all();
}
void Main() override;
void ProcessAlarms();
UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
HAL_HandleEnum::Notifier>
m_handles;
struct Alarm {
HAL_NotifierHandle handle;
std::shared_ptr<Notifier> notifier;
bool operator==(const Alarm& rhs) const { return handle == rhs.handle; }
bool operator>(const Alarm& rhs) const {
return notifier->alarmTime > rhs.notifier->alarmTime;
}
};
wpi::util::priority_queue<Alarm, std::vector<Alarm>, std::greater<Alarm>>
m_alarmQueue;
};
static NotifierHandleContainer* notifierHandles;
class NotifierInstance {
public:
NotifierInstance() { owner.Start(); }
wpi::util::SafeThreadOwner<NotifierThread> owner;
};
static NotifierInstance* notifierInstance;
namespace wpi::hal::init {
void InitializeNotifier() {
static NotifierHandleContainer nH;
notifierHandles = &nH;
static NotifierInstance n;
notifierInstance = &n;
}
} // namespace wpi::hal::init
void NotifierThread::Main() {
std::unique_lock lock(m_mutex);
while (m_active) {
if (m_alarmQueue.empty()) {
// No alarms, wait indefinitely
m_cond.wait(lock);
continue;
}
// Wait until next alarm
const Alarm& alarm = m_alarmQueue.top();
int32_t status = 0;
uint64_t curTime = HAL_GetFPGATime(&status);
if (alarm.notifier->alarmTime > curTime) {
m_cond.wait_for(
lock, std::chrono::microseconds{alarm.notifier->alarmTime - curTime});
}
if (!m_active) {
break;
}
ProcessAlarms();
}
}
void NotifierThread::ProcessAlarms() {
int32_t status = 0;
uint64_t curTime = HAL_GetFPGATime(&status);
while (!m_alarmQueue.empty() &&
m_alarmQueue.top().notifier->alarmTime <= curTime) {
Alarm alarm = m_alarmQueue.pop();
HAL_NotifierHandle handle = alarm.handle;
Notifier& notifier = *alarm.notifier;
if (notifier.intervalTime > 0) {
// Schedule next alarm
notifier.alarmTime += notifier.intervalTime;
if (curTime >= notifier.alarmTime) {
// We missed at least one interval
int32_t missed = static_cast<int32_t>((curTime - notifier.alarmTime) /
notifier.intervalTime) +
1;
notifier.overrunCount += missed;
notifier.alarmTime +=
missed * notifier.intervalTime; // Skip missed intervals
}
// Reinsert into queue
m_alarmQueue.push(std::move(alarm));
} else {
// Disable one-shot alarm
notifier.alarmTime = UINT64_MAX;
}
// If the last call was acknowledged, signal the handler
if (!notifier.handlerSignaled.test_and_set()) {
// copy the overrun count for the handler to read, reset the local count
notifier.userOverrunCount = notifier.overrunCount;
notifier.overrunCount = 0;
wpi::util::SetSignalObject(handle);
}
}
}
extern "C" {
HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
HAL_NotifierHandle HAL_CreateNotifier(int32_t* status) {
wpi::hal::init::CheckInit();
std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
HAL_NotifierHandle handle = notifierHandles->Allocate(notifier);
HAL_NotifierHandle handle =
notifierInstance->owner.GetThread()->m_handles.Allocate(notifier);
if (handle == HAL_kInvalidHandle) {
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
wpi::util::CreateSignalObject(handle);
return handle;
}
HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
int32_t* status) {
// There is no thread, so this can be removed.
return true;
auto native = notifierInstance->owner.GetNativeThreadHandle();
return HAL_SetThreadPriority(&native, realTime, priority, status);
}
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle,
const WPI_String* name, int32_t* status) {
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
std::scoped_lock lock(notifier->mutex);
notifier->name = name;
notifier->name = wpi::util::to_string_view(name);
}
void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
void HAL_DestroyNotifier(HAL_NotifierHandle notifierHandle) {
wpi::util::DestroySignalObject(notifierHandle);
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Free(notifierHandle);
thr->m_alarmQueue.remove({notifierHandle, notifier});
}
void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime,
uint64_t intervalTime, HAL_Bool absolute,
int32_t* status) {
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
}
notifier->cond.notify_all();
}
void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle) {
auto notifier = notifierHandles->Free(notifierHandle);
if (!notifier) {
return;
if (!absolute) {
alarmTime += HAL_GetFPGATime(status);
}
// Just in case HAL_StopNotifier() wasn't called...
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
uint64_t prevWakeup = UINT64_MAX;
if (!thr->m_alarmQueue.empty()) {
prevWakeup = thr->m_alarmQueue.top().notifier->alarmTime;
thr->m_alarmQueue.remove({notifierHandle, notifier});
}
notifier->cond.notify_all();
}
notifier->alarmTime = alarmTime;
notifier->intervalTime = intervalTime;
notifier->overrunCount = 0;
thr->m_alarmQueue.push({notifierHandle, notifier});
void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
uint64_t triggerTime, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
if (!notifier) {
return;
// wake up notifier thread if needed
if (alarmTime < prevWakeup) {
thr->m_cond.notify_all();
}
{
std::scoped_lock lock(notifier->mutex);
notifier->waitTime = triggerTime;
notifier->waitTimeValid = (triggerTime != UINT64_MAX);
}
// We wake up any waiters to change how long they're sleeping for
notifier->cond.notify_all();
}
void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->waitTimeValid = false;
}
thr->m_alarmQueue.remove({notifierHandle, notifier});
notifier->alarmTime = UINT64_MAX;
}
uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
auto notifier =
notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
if (!notifier) {
return 0;
return;
}
notifier->handlerSignaled.clear();
}
std::unique_lock ulock(notifiersWaiterMutex);
std::unique_lock lock(notifier->mutex);
ulock.unlock();
notifiersWaiterCond.notify_all();
while (notifier->active) {
uint64_t curTime = HAL_GetFPGATime(status);
if (notifier->waitTimeValid && curTime >= notifier->waitTime) {
notifier->waitTimeValid = false;
return curTime;
}
double waitDuration;
if (!notifier->waitTimeValid) {
// If not running, wait 1000 seconds
waitDuration = 1000.0;
} else {
waitDuration = (notifier->waitTime - curTime) * 1e-6;
}
notifier->cond.wait_for(lock, std::chrono::duration<double>(waitDuration));
int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier =
notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
if (!notifier) {
return -1;
}
return 0;
return notifier->userOverrunCount;
}
} // extern "C"