2024-11-30 18:04:00 +00:00
|
|
|
// Copyright (c) FIRST and other WPILib contributors.
|
|
|
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
|
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
|
|
2025-11-07 19:56:21 -05:00
|
|
|
#include "wpi/hal/Notifier.h"
|
2024-11-30 18:04:00 +00:00
|
|
|
|
|
|
|
|
#include <atomic>
|
|
|
|
|
#include <chrono>
|
2025-11-29 11:00:18 -08:00
|
|
|
#include <functional>
|
2024-11-30 18:04:00 +00:00
|
|
|
#include <memory>
|
|
|
|
|
#include <string>
|
|
|
|
|
#include <utility>
|
2025-11-29 11:00:18 -08:00
|
|
|
#include <vector>
|
2024-11-30 18:04:00 +00:00
|
|
|
|
2026-01-04 00:41:53 -08:00
|
|
|
#include "HALInitializer.hpp"
|
2025-11-07 19:56:21 -05:00
|
|
|
#include "wpi/hal/Errors.h"
|
2026-03-13 17:19:39 -07:00
|
|
|
#include "wpi/hal/HAL.h"
|
2025-11-29 11:00:18 -08:00
|
|
|
#include "wpi/hal/Threads.h"
|
|
|
|
|
#include "wpi/hal/Types.h"
|
2026-01-04 00:41:53 -08:00
|
|
|
#include "wpi/hal/handles/UnlimitedHandleResource.hpp"
|
2025-11-29 11:00:18 -08:00
|
|
|
#include "wpi/util/SafeThread.hpp"
|
2026-01-04 10:22:33 -08:00
|
|
|
#include "wpi/util/Synchronization.hpp"
|
2026-04-06 08:49:43 -07:00
|
|
|
#include "wpi/util/print.hpp"
|
2025-11-29 11:00:18 -08:00
|
|
|
#include "wpi/util/priority_queue.hpp"
|
2026-01-04 10:22:33 -08:00
|
|
|
#include "wpi/util/string.hpp"
|
2024-11-30 18:04:00 +00:00
|
|
|
|
|
|
|
|
namespace {
|
|
|
|
|
struct Notifier {
|
|
|
|
|
std::string name;
|
2025-11-29 11:00:18 -08:00
|
|
|
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{};
|
2024-11-30 18:04:00 +00:00
|
|
|
};
|
|
|
|
|
} // namespace
|
|
|
|
|
|
2025-11-07 20:00:05 -05:00
|
|
|
using namespace wpi::hal;
|
2024-11-30 18:04:00 +00:00
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
class NotifierThread : public wpi::util::SafeThread {
|
|
|
|
|
public:
|
|
|
|
|
void Main() override;
|
|
|
|
|
|
|
|
|
|
void ProcessAlarms();
|
|
|
|
|
|
|
|
|
|
UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
|
2026-03-16 21:49:21 -07:00
|
|
|
HAL_HandleEnum::NOTIFIER>
|
2025-11-29 11:00:18 -08:00
|
|
|
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;
|
|
|
|
|
};
|
2024-11-30 18:04:00 +00:00
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
class NotifierInstance {
|
2024-11-30 18:04:00 +00:00
|
|
|
public:
|
2025-11-29 11:00:18 -08:00
|
|
|
NotifierInstance() { owner.Start(); }
|
|
|
|
|
wpi::util::SafeThreadOwner<NotifierThread> owner;
|
2024-11-30 18:04:00 +00:00
|
|
|
};
|
|
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
static NotifierInstance* notifierInstance;
|
2024-11-30 18:04:00 +00:00
|
|
|
|
2025-11-07 20:00:05 -05:00
|
|
|
namespace wpi::hal::init {
|
2024-11-30 18:04:00 +00:00
|
|
|
void InitializeNotifier() {
|
2025-11-29 11:00:18 -08:00
|
|
|
static NotifierInstance n;
|
|
|
|
|
notifierInstance = &n;
|
2024-11-30 18:04:00 +00:00
|
|
|
}
|
2025-11-07 20:00:05 -05:00
|
|
|
} // namespace wpi::hal::init
|
2024-11-30 18:04:00 +00:00
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
void NotifierThread::Main() {
|
2026-04-06 08:49:43 -07:00
|
|
|
if (HAL_SetCurrentThreadPriority(40) != 0) {
|
|
|
|
|
wpi::util::print("Failed to set HAL Notifier thread priority\n");
|
|
|
|
|
}
|
|
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
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();
|
2026-03-15 15:08:41 -07:00
|
|
|
uint64_t curTime = HAL_GetMonotonicTime();
|
2025-11-29 11:00:18 -08:00
|
|
|
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() {
|
2026-03-15 15:08:41 -07:00
|
|
|
uint64_t curTime = HAL_GetMonotonicTime();
|
2025-11-29 11:00:18 -08:00
|
|
|
|
|
|
|
|
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);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2024-11-30 18:04:00 +00:00
|
|
|
extern "C" {
|
|
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
HAL_NotifierHandle HAL_CreateNotifier(int32_t* status) {
|
2025-11-07 20:00:05 -05:00
|
|
|
wpi::hal::init::CheckInit();
|
2024-11-30 18:04:00 +00:00
|
|
|
std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
|
2025-11-29 11:00:18 -08:00
|
|
|
HAL_NotifierHandle handle =
|
|
|
|
|
notifierInstance->owner.GetThread()->m_handles.Allocate(notifier);
|
2026-03-21 00:34:46 -07:00
|
|
|
if (handle == HAL_INVALID_HANDLE) {
|
2024-11-30 18:04:00 +00:00
|
|
|
*status = HAL_HANDLE_ERROR;
|
2026-03-21 00:34:46 -07:00
|
|
|
return HAL_INVALID_HANDLE;
|
2024-11-30 18:04:00 +00:00
|
|
|
}
|
2025-11-29 11:00:18 -08:00
|
|
|
wpi::util::CreateSignalObject(handle);
|
2024-11-30 18:04:00 +00:00
|
|
|
return handle;
|
|
|
|
|
}
|
|
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
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);
|
2024-11-30 18:04:00 +00:00
|
|
|
if (!notifier) {
|
|
|
|
|
return;
|
|
|
|
|
}
|
2025-11-29 11:00:18 -08:00
|
|
|
notifier->name = wpi::util::to_string_view(name);
|
2024-11-30 18:04:00 +00:00
|
|
|
}
|
|
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
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});
|
2024-11-30 18:04:00 +00:00
|
|
|
}
|
|
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime,
|
|
|
|
|
uint64_t intervalTime, HAL_Bool absolute,
|
2025-12-09 20:28:15 -07:00
|
|
|
HAL_Bool ack, int32_t* status) {
|
2025-11-29 11:00:18 -08:00
|
|
|
auto thr = notifierInstance->owner.GetThread();
|
|
|
|
|
auto notifier = thr->m_handles.Get(notifierHandle);
|
2024-11-30 18:04:00 +00:00
|
|
|
if (!notifier) {
|
|
|
|
|
return;
|
|
|
|
|
}
|
2025-12-09 20:28:15 -07:00
|
|
|
|
|
|
|
|
if (ack) {
|
|
|
|
|
notifier->handlerSignaled.clear();
|
2025-12-11 23:31:49 -07:00
|
|
|
wpi::util::ResetSignalObject(notifierHandle);
|
2025-12-09 20:28:15 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (!absolute) {
|
2026-03-15 15:08:41 -07:00
|
|
|
alarmTime += HAL_GetMonotonicTime();
|
2025-12-09 20:28:15 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
uint64_t prevWakeup = UINT64_MAX;
|
|
|
|
|
if (!thr->m_alarmQueue.empty()) {
|
|
|
|
|
prevWakeup = thr->m_alarmQueue.top().notifier->alarmTime;
|
|
|
|
|
thr->m_alarmQueue.remove({notifierHandle, notifier});
|
|
|
|
|
}
|
|
|
|
|
notifier->alarmTime = alarmTime;
|
|
|
|
|
notifier->intervalTime = intervalTime;
|
|
|
|
|
notifier->overrunCount = 0;
|
|
|
|
|
thr->m_alarmQueue.push({notifierHandle, notifier});
|
|
|
|
|
|
|
|
|
|
// wake up notifier thread if needed
|
|
|
|
|
if (alarmTime < prevWakeup) {
|
|
|
|
|
thr->m_cond.notify_all();
|
|
|
|
|
}
|
2024-11-30 18:04:00 +00:00
|
|
|
}
|
|
|
|
|
|
2025-12-09 20:28:15 -07:00
|
|
|
void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle, HAL_Bool ack,
|
2024-11-30 18:04:00 +00:00
|
|
|
int32_t* status) {
|
2025-11-29 11:00:18 -08:00
|
|
|
auto thr = notifierInstance->owner.GetThread();
|
|
|
|
|
auto notifier = thr->m_handles.Get(notifierHandle);
|
2024-11-30 18:04:00 +00:00
|
|
|
if (!notifier) {
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
2025-12-09 20:28:15 -07:00
|
|
|
if (ack) {
|
|
|
|
|
notifier->handlerSignaled.clear();
|
2025-12-11 23:31:49 -07:00
|
|
|
wpi::util::ResetSignalObject(notifierHandle);
|
2025-12-09 20:28:15 -07:00
|
|
|
}
|
|
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
thr->m_alarmQueue.remove({notifierHandle, notifier});
|
|
|
|
|
notifier->alarmTime = UINT64_MAX;
|
2024-11-30 18:04:00 +00:00
|
|
|
}
|
|
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle,
|
2024-11-30 18:04:00 +00:00
|
|
|
int32_t* status) {
|
2025-12-04 09:59:59 -07:00
|
|
|
auto thr = notifierInstance->owner.GetThread();
|
|
|
|
|
auto notifier = thr->m_handles.Get(notifierHandle);
|
2024-11-30 18:04:00 +00:00
|
|
|
if (!notifier) {
|
2025-11-29 11:00:18 -08:00
|
|
|
return;
|
2024-11-30 18:04:00 +00:00
|
|
|
}
|
2025-11-29 11:00:18 -08:00
|
|
|
notifier->handlerSignaled.clear();
|
2025-12-11 23:31:49 -07:00
|
|
|
wpi::util::ResetSignalObject(notifierHandle);
|
2025-11-29 11:00:18 -08:00
|
|
|
}
|
2024-11-30 18:04:00 +00:00
|
|
|
|
2025-11-29 11:00:18 -08:00
|
|
|
int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle,
|
|
|
|
|
int32_t* status) {
|
|
|
|
|
auto notifier =
|
|
|
|
|
notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
|
|
|
|
|
if (!notifier) {
|
|
|
|
|
return -1;
|
2024-11-30 18:04:00 +00:00
|
|
|
}
|
2025-11-29 11:00:18 -08:00
|
|
|
return notifier->userOverrunCount;
|
2024-11-30 18:04:00 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
} // extern "C"
|