[hal,wpilib] Fix TimedRobot notifier race (#8445)

It was possible for the alarm to fire between the set alarm and ack,
resulting in a hang on next wait. It's not possible to ack before set
alarm due to a race in sim step timing, so the fix is to provide an
atomic ack and set alarm; the easiest way to implement this in the API
was to change ack to optionally also set the alarm again.

hal/src/main/native/cpp/jni/NotifierJNI.cpp

@@ -113,14 +113,18 @@ Java_org_wpilib_hardware_hal_NotifierJNI_cancelNotifierAlarm
 /*
  * Class:     org_wpilib_hardware_hal_NotifierJNI
  * Method:    acknowledgeNotifierAlarm
- * Signature: (I)V
+ * Signature: (IZJJZ)V
  */
 JNIEXPORT void JNICALL
 Java_org_wpilib_hardware_hal_NotifierJNI_acknowledgeNotifierAlarm
-  (JNIEnv* env, jclass cls, jint notifierHandle)
+  (JNIEnv* env, jclass cls, jint notifierHandle, jboolean setAlarm,
+   jlong alarmTime, jlong intervalTime, jboolean absolute)
 {
   int32_t status = 0;
-  HAL_AcknowledgeNotifierAlarm((HAL_NotifierHandle)notifierHandle, &status);
+  HAL_AcknowledgeNotifierAlarm((HAL_NotifierHandle)notifierHandle, setAlarm,
+                               static_cast<uint64_t>(alarmTime),
+                               static_cast<uint64_t>(intervalTime), absolute,
+                               &status);
 
   CheckStatus(env, status);
 }

hal/src/main/native/include/wpi/hal/Notifier.h

@@ -103,13 +103,28 @@ void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
                              int32_t* status);
 
 /**
- * Indicates the notifier alarm has been serviced. This must be called before
- * waiting for the next alarm.
+ * Indicates the notifier alarm has been serviced and optionally sets a new
+ * alarm time. This must be called before waiting for the next alarm.
+ *
+ * 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] setAlarm       true to set a new alarm time, false to leave the
+ *                           alarm unchanged
+ * @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_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle,
+                                  HAL_Bool setAlarm, uint64_t alarmTime,
+                                  uint64_t intervalTime, HAL_Bool absolute,
                                   int32_t* status);
 
 /**

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

@@ -46,6 +46,10 @@ class NotifierThread : public wpi::util::SafeThread {
  public:
   void Main() override;
 
+  void SetAlarm(HAL_NotifierHandle notifierHandle,
+                std::shared_ptr<Notifier>& notifier, uint64_t alarmTime,
+                uint64_t intervalTime, bool absolute, int32_t* status);
+
   void ProcessAlarms(wpi::util::SmallVectorImpl<HAL_NotifierHandle>* signaled);
 
   bool m_paused = false;
@@ -111,6 +115,30 @@ void NotifierThread::Main() {
   }
 }
 
+void NotifierThread::SetAlarm(HAL_NotifierHandle notifierHandle,
+                              std::shared_ptr<Notifier>& notifier,
+                              uint64_t alarmTime, uint64_t intervalTime,
+                              bool absolute, int32_t* status) {
+  if (!absolute) {
+    alarmTime += HAL_GetFPGATime(status);
+  }
+
+  uint64_t prevWakeup = UINT64_MAX;
+  if (!m_alarmQueue.empty()) {
+    prevWakeup = m_alarmQueue.top().notifier->alarmTime;
+    m_alarmQueue.remove({notifierHandle, notifier});
+  }
+  notifier->alarmTime = alarmTime;
+  notifier->intervalTime = intervalTime;
+  notifier->overrunCount = 0;
+  m_alarmQueue.push({notifierHandle, notifier});
+
+  // wake up notifier thread if needed
+  if (alarmTime < prevWakeup) {
+    m_cond.notify_all();
+  }
+}
+
 void NotifierThread::ProcessAlarms(
     wpi::util::SmallVectorImpl<HAL_NotifierHandle>* signaled) {
   int32_t status = 0;
@@ -256,25 +284,8 @@ void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime,
   if (!notifier) {
     return;
   }
-
-  if (!absolute) {
-    alarmTime += HAL_GetFPGATime(status);
-  }
-
-  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();
-  }
+  thr->SetAlarm(notifierHandle, notifier, alarmTime, intervalTime, absolute,
+                status);
 }
 
 void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
@@ -287,16 +298,23 @@ void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
 
   thr->m_alarmQueue.remove({notifierHandle, notifier});
   notifier->alarmTime = UINT64_MAX;
+  notifier->handlerSignaled.clear();
 }
 
 void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle,
+                                  HAL_Bool setAlarm, uint64_t alarmTime,
+                                  uint64_t intervalTime, HAL_Bool absolute,
                                   int32_t* status) {
-  auto notifier =
-      notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
+  auto thr = notifierInstance->owner.GetThread();
+  auto notifier = thr->m_handles.Get(notifierHandle);
   if (!notifier) {
     return;
   }
   notifier->handlerSignaled.clear();
+  if (setAlarm) {
+    thr->SetAlarm(notifierHandle, notifier, alarmTime, intervalTime, absolute,
+                  status);
+  }
 }
 
 int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle,

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

@@ -39,6 +39,10 @@ class NotifierThread : public wpi::util::SafeThread {
  public:
   void Main() override;
 
+  void SetAlarm(HAL_NotifierHandle notifierHandle,
+                std::shared_ptr<Notifier>& notifier, uint64_t alarmTime,
+                uint64_t intervalTime, bool absolute, int32_t* status);
+
   void ProcessAlarms();
 
   UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
@@ -97,6 +101,30 @@ void NotifierThread::Main() {
   }
 }
 
+void NotifierThread::SetAlarm(HAL_NotifierHandle notifierHandle,
+                              std::shared_ptr<Notifier>& notifier,
+                              uint64_t alarmTime, uint64_t intervalTime,
+                              bool absolute, int32_t* status) {
+  if (!absolute) {
+    alarmTime += HAL_GetFPGATime(status);
+  }
+
+  uint64_t prevWakeup = UINT64_MAX;
+  if (!m_alarmQueue.empty()) {
+    prevWakeup = m_alarmQueue.top().notifier->alarmTime;
+    m_alarmQueue.remove({notifierHandle, notifier});
+  }
+  notifier->alarmTime = alarmTime;
+  notifier->intervalTime = intervalTime;
+  notifier->overrunCount = 0;
+  m_alarmQueue.push({notifierHandle, notifier});
+
+  // wake up notifier thread if needed
+  if (alarmTime < prevWakeup) {
+    m_cond.notify_all();
+  }
+}
+
 void NotifierThread::ProcessAlarms() {
   int32_t status = 0;
   uint64_t curTime = HAL_GetFPGATime(&status);
@@ -182,25 +210,8 @@ void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime,
   if (!notifier) {
     return;
   }
-
-  if (!absolute) {
-    alarmTime += HAL_GetFPGATime(status);
-  }
-
-  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();
-  }
+  thr->SetAlarm(notifierHandle, notifier, alarmTime, intervalTime, absolute,
+                status);
 }
 
 void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
@@ -213,16 +224,23 @@ void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
 
   thr->m_alarmQueue.remove({notifierHandle, notifier});
   notifier->alarmTime = UINT64_MAX;
+  notifier->handlerSignaled.clear();
 }
 
 void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle,
+                                  HAL_Bool setAlarm, uint64_t alarmTime,
+                                  uint64_t intervalTime, HAL_Bool absolute,
                                   int32_t* status) {
-  auto notifier =
-      notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
+  auto thr = notifierInstance->owner.GetThread();
+  auto notifier = thr->m_handles.Get(notifierHandle);
   if (!notifier) {
     return;
   }
   notifier->handlerSignaled.clear();
+  if (setAlarm) {
+    thr->SetAlarm(notifierHandle, notifier, alarmTime, intervalTime, absolute,
+                  status);
+  }
 }
 
 int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle,