Change HAL notifier to polling. (#627)

This moves the thread code to the WPILib layer, fixing various potential
races and significantly simplifying the HAL implementation.

hal/src/main/native/sim/Notifier.cpp

@@ -8,9 +8,9 @@
 #include "HAL/Notifier.h"
 
 #include <chrono>
-#include <iostream>
 
-#include <support/SafeThread.h>
+#include <support/condition_variable.h>
+#include <support/mutex.h>
 #include <support/timestamp.h>
 
 #include "HAL/HAL.h"
@@ -19,138 +19,128 @@
 #include "HAL/handles/UnlimitedHandleResource.h"
 
 namespace {
-class NotifierThread : public wpi::SafeThread {
- public:
-  void Main() {
-    int32_t status = 0;
-    std::unique_lock<wpi::mutex> lock(m_mutex);
-    while (m_active) {
-    startNotifierLoop:
-      double waitTime = m_waitTime * 1e-6;
-      if (!m_running) {
-        status = 0;
-        waitTime = (HAL_GetFPGATime(&status) * 1e-6) + 1000.0;
-        // If not running, wait 1000 seconds
-      }
-
-      auto timeoutTime =
-          hal::fpga_clock::epoch() + std::chrono::duration<double>(waitTime);
-      // auto timeoutTime = std::chrono::steady_clock::now() +
-      // std::chrono::duration<double>(1.0);
-      m_cond.wait_until(lock, timeoutTime);
-      if (m_updatedAlarm) {
-        m_updatedAlarm = false;
-        goto startNotifierLoop;
-      }
-      if (!m_running) continue;
-      if (!m_active) break;
-      m_running = false;
-      int32_t status = 0;
-      uint64_t currentTime = HAL_GetFPGATime(&status);
-      HAL_NotifierHandle handle = m_handle;
-      HAL_NotifierProcessFunction process = m_process;
-      lock.unlock();  // don't hold mutex during callback execution
-      process(currentTime, handle);
-      m_updatedAlarm = false;
-      lock.lock();
-    }
-  }
-
-  HAL_NotifierHandle m_handle = HAL_kInvalidHandle;
-  HAL_NotifierProcessFunction m_process;
-  uint64_t m_waitTime;
-  bool m_updatedAlarm = false;
-  bool m_running = false;
-};
-
-class NotifierThreadOwner : public wpi::SafeThreadOwner<NotifierThread> {
- public:
-  void SetFunc(HAL_NotifierProcessFunction process, HAL_NotifierHandle handle) {
-    auto thr = GetThread();
-    if (!thr) return;
-    thr->m_process = process;
-    thr->m_handle = handle;
-  }
-
-  void UpdateAlarm(uint64_t triggerTime) {
-    auto thr = GetThread();
-    if (!thr) return;
-    thr->m_waitTime = triggerTime;
-    thr->m_running = true;
-    thr->m_updatedAlarm = true;
-    thr->m_cond.notify_one();
-  }
-
-  void StopAlarm() {
-    auto thr = GetThread();
-    if (!thr) return;
-    thr->m_running = false;
-  }
-};
 struct Notifier {
-  std::unique_ptr<NotifierThreadOwner> thread;
-  void* param;
+  uint64_t waitTime;
+  bool updatedAlarm = false;
+  bool active = true;
+  bool running = false;
+  wpi::mutex mutex;
+  wpi::condition_variable cond;
 };
 }  // namespace
 
 using namespace hal;
 
-static UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
-                               HAL_HandleEnum::Notifier>
-    notifierHandles;
+class NotifierHandleContainer
+    : public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
+                                     HAL_HandleEnum::Notifier> {
+ public:
+  ~NotifierHandleContainer() {
+    ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
+      {
+        std::lock_guard<wpi::mutex> lock(notifier->mutex);
+        notifier->active = false;
+        notifier->running = false;
+      }
+      notifier->cond.notify_all();  // wake up any waiting threads
+    });
+  }
+};
+
+static NotifierHandleContainer notifierHandles;
 
 extern "C" {
 
-HAL_NotifierHandle HAL_InitializeNotifierNonThreadedUnsafe(
-    HAL_NotifierProcessFunction process, void* param, int32_t* status) {
-  return HAL_InitializeNotifier(process, param, status);
-}
-
-HAL_NotifierHandle HAL_InitializeNotifier(HAL_NotifierProcessFunction process,
-                                          void* param, int32_t* status) {
-  if (!process) {
-    *status = NULL_PARAMETER;
-    return 0;
-  }
-
+HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
   std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
   HAL_NotifierHandle handle = notifierHandles.Allocate(notifier);
   if (handle == HAL_kInvalidHandle) {
     *status = HAL_HANDLE_ERROR;
     return HAL_kInvalidHandle;
   }
-
-  notifier->thread = std::make_unique<NotifierThreadOwner>();
-
-  notifier->thread->Start();
-  notifier->thread->SetFunc(process, handle);
-
-  notifier->param = param;
-
   return handle;
 }
 
-void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
+void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
   auto notifier = notifierHandles.Get(notifierHandle);
   if (!notifier) return;
-  notifier->thread->StopAlarm();
-  notifierHandles.Free(notifierHandle);
+
+  {
+    std::lock_guard<wpi::mutex> lock(notifier->mutex);
+    notifier->active = false;
+    notifier->running = false;
+  }
+  notifier->cond.notify_all();
 }
-void* HAL_GetNotifierParam(HAL_NotifierHandle notifierHandle, int32_t* status) {
-  auto notifier = notifierHandles.Get(notifierHandle);
-  if (!notifier) return nullptr;
-  return notifier->param;
+
+void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
+  auto notifier = notifierHandles.Free(notifierHandle);
+  if (!notifier) return;
+
+  // Just in case HAL_StopNotifier() wasn't called...
+  {
+    std::lock_guard<wpi::mutex> lock(notifier->mutex);
+    notifier->active = false;
+    notifier->running = false;
+  }
+  notifier->cond.notify_all();
 }
+
 void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
                              uint64_t triggerTime, int32_t* status) {
   auto notifier = notifierHandles.Get(notifierHandle);
   if (!notifier) return;
-  notifier->thread->UpdateAlarm(triggerTime);
+
+  {
+    std::lock_guard<wpi::mutex> lock(notifier->mutex);
+    notifier->waitTime = triggerTime;
+    notifier->running = true;
+    notifier->updatedAlarm = true;
+  }
+
+  // We wake up any waiters to change how long they're sleeping for
+  notifier->cond.notify_all();
 }
-void HAL_StopNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status) {
+
+void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
+                             int32_t* status) {
   auto notifier = notifierHandles.Get(notifierHandle);
   if (!notifier) return;
-  notifier->thread->StopAlarm();
+
+  {
+    std::lock_guard<wpi::mutex> lock(notifier->mutex);
+    notifier->running = false;
+  }
+}
+
+uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
+                                  int32_t* status) {
+  auto notifier = notifierHandles.Get(notifierHandle);
+  if (!notifier) return 0;
+
+  std::unique_lock<wpi::mutex> lock(notifier->mutex);
+  while (notifier->active) {
+    double waitTime;
+    if (!notifier->running) {
+      waitTime = (HAL_GetFPGATime(status) * 1e-6) + 1000.0;
+      // If not running, wait 1000 seconds
+    } else {
+      waitTime = notifier->waitTime * 1e-6;
+    }
+
+    auto timeoutTime =
+        hal::fpga_clock::epoch() + std::chrono::duration<double>(waitTime);
+    notifier->cond.wait_until(lock, timeoutTime);
+    if (notifier->updatedAlarm) {
+      notifier->updatedAlarm = false;
+      continue;
+    }
+    if (!notifier->running) continue;
+    if (!notifier->active) break;
+    notifier->running = false;
+    return HAL_GetFPGATime(status);
+  }
+  return 0;
 }
 
 }  // extern "C"