Add simulation pause/resume/step support

Calling HALSIM_PauseTiming pauses the FPGA clock and notifiers.
Calling HALSIM_ResumeTiming resumes the FPGA clock and notifiers.
Calling HALSIM_StepTiming steps the FPGA clock and runs applicable notifiers.

This will effectively pause TimedRobot and any other notifier-based events,
but of course will not pause user threads that do not use the notifier (e.g.
image processing).

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 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.                                                               */
@@ -13,10 +13,12 @@
 #include <wpi/timestamp.h>
 
 #include "MockHooksInternal.h"
+#include "NotifierInternal.h"
 
 static std::atomic<bool> programStarted{false};
 
 static std::atomic<uint64_t> programStartTime{0};
+static std::atomic<uint64_t> programPauseTime{0};
 
 namespace hal {
 namespace init {
@@ -25,12 +27,32 @@ void InitializeMockHooks() {}
 }  // namespace hal
 
 namespace hal {
-void RestartTiming() { programStartTime = wpi::Now(); }
+void RestartTiming() {
+  programStartTime = wpi::Now();
+  if (programPauseTime != 0) programPauseTime = programStartTime.load();
+}
+
+void PauseTiming() {
+  if (programPauseTime == 0) programPauseTime = wpi::Now();
+}
+
+void ResumeTiming() {
+  if (programPauseTime != 0) {
+    programStartTime += wpi::Now() - programPauseTime;
+    programPauseTime = 0;
+  }
+}
+
+bool IsTimingPaused() { return programPauseTime != 0; }
+
+void StepTiming(uint64_t delta) {
+  if (programPauseTime != 0) programPauseTime += delta;
+}
 
 int64_t GetFPGATime() {
-  auto now = wpi::Now();
-  auto currentTime = now - programStartTime;
-  return currentTime;
+  uint64_t curTime = programPauseTime;
+  if (curTime == 0) curTime = wpi::Now();
+  return curTime - programStartTime;
 }
 
 double GetFPGATimestamp() { return GetFPGATime() * 1.0e-6; }
@@ -56,4 +78,21 @@ void HALSIM_SetProgramStarted(void) { SetProgramStarted(); }
 HAL_Bool HALSIM_GetProgramStarted(void) { return GetProgramStarted(); }
 
 void HALSIM_RestartTiming(void) { RestartTiming(); }
+
+void HALSIM_PauseTiming(void) {
+  PauseTiming();
+  PauseNotifiers();
+}
+
+void HALSIM_ResumeTiming(void) {
+  ResumeTiming();
+  ResumeNotifiers();
+}
+
+HAL_Bool HALSIM_IsTimingPaused(void) { return IsTimingPaused(); }
+
+void HALSIM_StepTiming(uint64_t delta) {
+  StepTiming(delta);
+  WakeupNotifiers();
+}
 }  // extern "C"

hal/src/main/native/sim/MockHooksInternal.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 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.                                                               */
@@ -14,6 +14,14 @@
 namespace hal {
 void RestartTiming();
 
+void PauseTiming();
+
+void ResumeTiming();
+
+bool IsTimingPaused();
+
+void StepTiming(uint64_t delta);
+
 int64_t GetFPGATime();
 
 double GetFPGATimestamp();

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

@@ -7,6 +7,7 @@
 
 #include "hal/Notifier.h"
 
+#include <atomic>
 #include <chrono>
 #include <cstdio>
 #include <cstring>
@@ -17,15 +18,16 @@
 #include <wpi/timestamp.h>
 
 #include "HALInitializer.h"
+#include "NotifierInternal.h"
 #include "hal/HAL.h"
 #include "hal/cpp/fpga_clock.h"
 #include "hal/handles/UnlimitedHandleResource.h"
+#include "mockdata/NotifierData.h"
 
 namespace {
 struct Notifier {
   std::string name;
   uint64_t waitTime;
-  bool updatedAlarm = false;
   bool active = true;
   bool running = false;
   wpi::mutex mutex;
@@ -52,6 +54,7 @@ class NotifierHandleContainer
 };
 
 static NotifierHandleContainer* notifierHandles;
+static std::atomic<bool> notifiersPaused{false};
 
 namespace hal {
 namespace init {
@@ -60,6 +63,19 @@ void InitializeNotifier() {
   notifierHandles = &nH;
 }
 }  // namespace init
+
+void PauseNotifiers() { notifiersPaused = true; }
+
+void ResumeNotifiers() {
+  notifiersPaused = false;
+  WakeupNotifiers();
+}
+
+void WakeupNotifiers() {
+  notifierHandles->ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
+    notifier->cond.notify_all();
+  });
+}
 }  // namespace hal
 
 extern "C" {
@@ -117,7 +133,6 @@ void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
     std::scoped_lock 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
@@ -143,27 +158,22 @@ uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
   std::unique_lock lock(notifier->mutex);
   while (notifier->active) {
     double waitTime;
-    if (!notifier->running) {
+    if (!notifier->running || notifiersPaused) {
       waitTime = (HAL_GetFPGATime(status) * 1e-6) + 1000.0;
       // If not running, wait 1000 seconds
     } else {
       waitTime = notifier->waitTime * 1e-6;
     }
 
-    // Don't wait twice
-    notifier->updatedAlarm = false;
-
     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;
+    uint64_t curTime = HAL_GetFPGATime(status);
+    if (curTime < notifier->waitTime) continue;
     notifier->running = false;
-    return HAL_GetFPGATime(status);
+    return curTime;
   }
   return 0;
 }

hal/src/main/native/sim/NotifierInternal.h

@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace hal {
+void PauseNotifiers();
+void ResumeNotifiers();
+void WakeupNotifiers();
+}  // namespace hal

hal/src/main/native/sim/jni/SimulatorJNI.cpp

@@ -142,6 +142,54 @@ Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_restartTiming
   HALSIM_RestartTiming();
 }
 
+/*
+ * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method:    pauseTiming
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_pauseTiming
+  (JNIEnv*, jclass)
+{
+  HALSIM_PauseTiming();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method:    resumeTiming
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_resumeTiming
+  (JNIEnv*, jclass)
+{
+  HALSIM_ResumeTiming();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method:    isTimingPaused
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_isTimingPaused
+  (JNIEnv*, jclass)
+{
+  return HALSIM_IsTimingPaused();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
+ * Method:    stepTiming
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_stepTiming
+  (JNIEnv*, jclass, jlong delta)
+{
+  HALSIM_StepTiming(delta);
+}
+
 /*
  * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
  * Method:    resetHandles