[sim] Add HAL hooks for simulationPeriodic (#2881)

This allows vendor libs to hook into the begin or end of simulationPeriodic().

hal/src/main/native/athena/HAL.cpp

@@ -422,6 +422,10 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
 
 void HAL_Shutdown(void) {}
 
+void HAL_SimPeriodicBefore(void) {}
+
+void HAL_SimPeriodicAfter(void) {}
+
 int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
                    const char* feature) {
   if (feature == nullptr) {

hal/src/main/native/athena/mockdata/MockHooks.cpp

@@ -33,4 +33,18 @@ void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {}
 
 void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {}
 
+int32_t HALSIM_RegisterSimPeriodicBeforeCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return 0;
+}
+
+void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) {}
+
+int32_t HALSIM_RegisterSimPeriodicAfterCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return 0;
+}
+
+void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) {}
+
 }  // extern "C"

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

@@ -84,6 +84,30 @@ Java_edu_wpi_first_hal_HAL_exitMain
   HAL_ExitMain();
 }
 
+/*
+ * Class:     edu_wpi_first_hal_HAL
+ * Method:    simPeriodicBeforeNative
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_simPeriodicBeforeNative
+  (JNIEnv*, jclass)
+{
+  HAL_SimPeriodicBefore();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_HAL
+ * Method:    simPeriodicAfterNative
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_simPeriodicAfterNative
+  (JNIEnv*, jclass)
+{
+  HAL_SimPeriodicAfter();
+}
+
 /*
  * Class:     edu_wpi_first_hal_HAL
  * Method:    observeUserProgramStarting

hal/src/main/native/include/hal/HALBase.h

@@ -157,6 +157,18 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode);
  */
 void HAL_Shutdown(void);
 
+/**
+ * Calls registered SimPeriodic "before" callbacks (only in simulation mode).
+ * This should be called prior to user code periodic simulation functions.
+ */
+void HAL_SimPeriodicBefore(void);
+
+/**
+ * Calls registered SimPeriodic "after" callbacks (only in simulation mode).
+ * This should be called after user code periodic simulation functions.
+ */
+void HAL_SimPeriodicAfter(void);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif

hal/src/main/native/include/hal/simulation/MockHooks.h

@@ -30,4 +30,13 @@ void HALSIM_SetSendError(HALSIM_SendErrorHandler handler);
 typedef int32_t (*HALSIM_SendConsoleLineHandler)(const char* line);
 void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler);
 
+typedef void (*HALSIM_SimPeriodicCallback)(void* param);
+int32_t HALSIM_RegisterSimPeriodicBeforeCallback(
+    HALSIM_SimPeriodicCallback callback, void* param);
+void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid);
+
+int32_t HALSIM_RegisterSimPeriodicAfterCallback(
+    HALSIM_SimPeriodicCallback callback, void* param);
+void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid);
+
 }  // extern "C"

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

@@ -26,13 +26,38 @@
 #include "hal/Extensions.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/simulation/DriverStationData.h"
+#include "hal/simulation/SimCallbackRegistry.h"
 #include "mockdata/RoboRioDataInternal.h"
 
 using namespace hal;
 
+namespace {
+class SimPeriodicCallbackRegistry : public impl::SimCallbackRegistryBase {
+ public:
+  int32_t Register(HALSIM_SimPeriodicCallback callback, void* param) {
+    std::scoped_lock lock(m_mutex);
+    return DoRegister(reinterpret_cast<RawFunctor>(callback), param);
+  }
+
+  void operator()() const {
+#ifdef _MSC_VER  // work around VS2019 16.4.0 bug
+    std::scoped_lock<wpi::recursive_spinlock> lock(m_mutex);
+#else
+    std::scoped_lock lock(m_mutex);
+#endif
+    if (m_callbacks) {
+      for (auto&& cb : *m_callbacks)
+        reinterpret_cast<HALSIM_SimPeriodicCallback>(cb.callback)(cb.param);
+    }
+  }
+};
+}  // namespace
+
 static HAL_RuntimeType runtimeType{HAL_Mock};
 static wpi::spinlock gOnShutdownMutex;
 static std::vector<std::pair<void*, void (*)(void*)>> gOnShutdown;
+static SimPeriodicCallbackRegistry gSimPeriodicBefore;
+static SimPeriodicCallbackRegistry gSimPeriodicAfter;
 
 namespace hal {
 namespace init {
@@ -333,6 +358,28 @@ void HAL_OnShutdown(void* param, void (*func)(void*)) {
   gOnShutdown.emplace_back(param, func);
 }
 
+void HAL_SimPeriodicBefore(void) { gSimPeriodicBefore(); }
+
+void HAL_SimPeriodicAfter(void) { gSimPeriodicAfter(); }
+
+int32_t HALSIM_RegisterSimPeriodicBeforeCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return gSimPeriodicBefore.Register(callback, param);
+}
+
+void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) {
+  gSimPeriodicBefore.Cancel(uid);
+}
+
+int32_t HALSIM_RegisterSimPeriodicAfterCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return gSimPeriodicAfter.Register(callback, param);
+}
+
+void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) {
+  gSimPeriodicAfter.Cancel(uid);
+}
+
 int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
                    const char* feature) {
   return 0;  // Do nothing for now