[wpiutil] RFC: Add mechanism to register global reset functions

hal/src/main/native/cpp/handles/HandlesInternal.cpp

@@ -6,6 +6,7 @@
 
 #include <algorithm>
 
+#include <wpi/GlobalState.h>
 #include <wpi/SmallVector.h>
 #include <wpi/mutex.h>
 
@@ -94,3 +95,6 @@ HAL_Handle createHandle(int16_t index, HAL_HandleEnum handleType,
   return handle;
 }
 }  // namespace hal
+
+static wpi::impl::RegisterGlobalStateResetHelper _(
+    wpi::impl::GSPriorityHalHandles, hal::HandleBase::ResetGlobalHandles);

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

@@ -7,6 +7,7 @@
 #include <cstdio>
 #include <vector>
 
+#include <wpi/GlobalState.h>
 #include <wpi/mutex.h>
 #include <wpi/spinlock.h>
 
@@ -426,6 +427,9 @@ void HALSIM_CancelAllSimPeriodicCallbacks(void) {
   gSimPeriodicAfter.Reset();
 }
 
+static wpi::impl::RegisterGlobalStateResetHelper _(
+    wpi::impl::GSPriorityHalCallbacks, HALSIM_CancelAllSimPeriodicCallbacks);
+
 int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
                    const char* feature) {
   return 0;  // Do nothing for now

hal/src/main/native/sim/mockdata/Reset.cpp

@@ -25,6 +25,8 @@
 #include <hal/simulation/SPIData.h>
 #include <hal/simulation/SimDeviceData.h>
 
+#include <wpi/GlobalState.h>
+
 #include "../PortsInternal.h"
 
 extern "C" void HALSIM_ResetAllSimData(void) {
@@ -107,3 +109,6 @@ extern "C" void HALSIM_ResetAllSimData(void) {
     HALSIM_ResetSPIData(i);
   }
 }
+
+static wpi::impl::RegisterGlobalStateResetHelper _(
+    wpi::impl::GSPriorityHalSimData, HALSIM_ResetAllSimData);

wpilibc/src/main/native/cpp/MotorSafety.cpp

@@ -8,6 +8,7 @@
 #include <utility>
 
 #include <hal/DriverStation.h>
+#include <wpi/GlobalState.h>
 #include <wpi/SafeThread.h>
 #include <wpi/SmallPtrSet.h>
 
@@ -81,6 +82,9 @@ void ResetMotorSafety() {
   manager.threadStarted = false;
 }
 }  // namespace frc::impl
+
+static wpi::impl::RegisterGlobalStateResetHelper _(
+    wpi::impl::GSPriorityMotorSafety, frc::impl::ResetMotorSafety);
 #endif
 
 MotorSafety::MotorSafety() {

wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp

@@ -8,6 +8,7 @@
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
 #include <networktables/StringTopic.h>
+#include <wpi/GlobalState.h>
 #include <wpi/mutex.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableRegistry.h>
@@ -63,11 +64,9 @@ static Instance& GetInstance() {
 }
 
 #ifndef __FRC_ROBORIO__
-namespace frc::impl {
-void ResetLiveWindow() {
-  std::make_unique<Instance>().swap(GetInstanceHolder());
-}
-}  // namespace frc::impl
+static wpi::impl::RegisterGlobalStateResetHelper _(
+    wpi::impl::GSPriorityLiveWindow,
+    []() { std::make_unique<Instance>().swap(GetInstanceHolder()); });
 #endif
 
 std::shared_ptr<Component> Instance::GetOrAdd(wpi::Sendable* sendable) {

wpilibc/src/main/native/cpp/shuffleboard/Shuffleboard.cpp

@@ -5,6 +5,7 @@
 #include "frc/shuffleboard/Shuffleboard.h"
 
 #include <networktables/NetworkTableInstance.h>
+#include <wpi/GlobalState.h>
 
 #include "frc/shuffleboard/ShuffleboardTab.h"
 
@@ -71,12 +72,11 @@ static std::unique_ptr<detail::ShuffleboardInstance>& GetInstanceHolder() {
 }
 
 #ifndef __FRC_ROBORIO__
-namespace frc::impl {
-void ResetShuffleboardInstance() {
-  GetInstanceHolder() = std::make_unique<detail::ShuffleboardInstance>(
-      nt::NetworkTableInstance::GetDefault());
-}
-}  // namespace frc::impl
+static wpi::impl::RegisterGlobalStateResetHelper _(
+    wpi::impl::GSPriorityShuffleboard, []() {
+      GetInstanceHolder() = std::make_unique<detail::ShuffleboardInstance>(
+          nt::NetworkTableInstance::GetDefault());
+    });
 #endif
 
 detail::ShuffleboardInstance& Shuffleboard::GetInstance() {

wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp

@@ -7,6 +7,7 @@
 #include <hal/FRCUsageReporting.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
+#include <wpi/GlobalState.h>
 #include <wpi/StringMap.h>
 #include <wpi/mutex.h>
 #include <wpi/sendable/SendableRegistry.h>
@@ -39,11 +40,9 @@ static Instance& GetInstance() {
 }
 
 #ifndef __FRC_ROBORIO__
-namespace frc::impl {
-void ResetSmartDashboardInstance() {
-  std::make_unique<Instance>().swap(GetInstanceHolder());
-}
-}  // namespace frc::impl
+static wpi::impl::RegisterGlobalStateResetHelper _(
+    wpi::impl::GSPrioritySmartDashboard,
+    []() { std::make_unique<Instance>().swap(GetInstanceHolder()); });
 #endif
 
 void SmartDashboard::init() {

wpiutil/src/main/native/cpp/GlobalState.cpp

@@ -0,0 +1,65 @@
+// 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.
+
+#include "wpi/GlobalState.h"
+
+#include <map>
+
+#include "wpi/mutex.h"
+
+using namespace wpi;
+
+#ifndef __FRC_ROBORIO__
+
+namespace {
+
+struct GlobalStateImpl {
+  void Register(int priority, std::function<void()> fn) {
+    std::scoped_lock lock{m_map_mutex};
+    m_functions.emplace(priority, std::move(fn));
+  }
+
+  void Reset() {
+    std::multimap<int, wpi::impl::ResetCallback> functions;
+    {
+      std::scoped_lock lock{m_map_mutex};
+      functions = m_functions;
+    }
+
+    {
+      // ensures that reset functions are never called concurrently, and
+      // that accidental calls to the Register function won't deadlock
+      std::scoped_lock lock{m_call_mutex};
+      for (const auto& kv : functions) {
+        kv.second();
+      }
+    }
+  }
+
+  wpi::mutex m_map_mutex;
+  wpi::mutex m_call_mutex;
+  std::multimap<int, wpi::impl::ResetCallback> m_functions;
+};
+
+static GlobalStateImpl& GetGlobalState() {
+  static GlobalStateImpl* impl = new GlobalStateImpl();
+  return *impl;
+}
+
+}  // namespace
+
+void ResetGlobalState() {
+  GetGlobalState().Reset();
+}
+
+void impl::RegisterGlobalStateReset(int priority, std::function<void()> fn) {
+  GetGlobalState().Register(priority, std::move(fn));
+}
+
+#else  // !__FRC_ROBORIO__
+
+void ResetGlobalState() {}
+void impl::RegisterGlobalStateReset(int priority, std::function<void()> fn) {}
+
+#endif

wpiutil/src/main/native/cpp/sendable/SendableRegistry.cpp

@@ -8,6 +8,7 @@
 
 #include "fmt/format.h"
 #include "wpi/DenseMap.h"
+#include "wpi/GlobalState.h"
 #include "wpi/SmallVector.h"
 #include "wpi/UidVector.h"
 #include "wpi/mutex.h"
@@ -71,11 +72,10 @@ static SendableRegistryInst& GetInstance() {
 }
 
 #ifndef __FRC_ROBORIO__
-namespace wpi::impl {
-void ResetSendableRegistry() {
-  std::make_unique<SendableRegistryInst>().swap(GetInstanceHolder());
-}
-}  // namespace wpi::impl
+static wpi::impl::RegisterGlobalStateResetHelper _(
+    wpi::impl::GSPrioritySendableRegistry, []() {
+      std::make_unique<SendableRegistryInst>().swap(GetInstanceHolder());
+    });
 #endif
 
 void SendableRegistry::SetLiveWindowBuilderFactory(

wpiutil/src/main/native/include/wpi/GlobalState.h

@@ -0,0 +1,88 @@
+// 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.
+
+#ifndef WPIUTIL_WPI_GLOBALSTATE_H_
+#define WPIUTIL_WPI_GLOBALSTATE_H_
+
+#include <functional>
+#include <utility>
+
+namespace wpi {
+
+/**
+ * Calls all registered reset functions.
+ *
+ * Intended to be called before and/or after a unit test. May crash and/or
+ * segfault your robot code. Does nothing on a RoboRIO.
+ */
+void ResetGlobalState();
+
+namespace impl {
+
+using ResetCallback = std::function<void()>;
+
+//
+// Reset priorities. These should be ordered such that it is less likely
+// that a segfault will occur.
+//
+
+// networktables should be reset first, as resetting some of the later items
+// may setup networktables listeners and/or callbacks
+static constexpr int GSPriorityNT = 100;
+// various singletons
+static constexpr int GSPrioritySmartDashboard = 150;
+static constexpr int GSPriorityShuffleboard = 200;
+static constexpr int GSPriorityLiveWindow = 250;
+static constexpr int GSPriorityMotorSafety = 300;
+static constexpr int GSPrioritySendableRegistry = 350;
+// HAL related data
+static constexpr int GSPriorityHalCallbacks = 600;
+static constexpr int GSPriorityHalHandles = 650;
+static constexpr int GSPriorityHalSimData = 700;
+
+// vendors should be reset after all WPILib things. They are generally
+// not going to depend on each other
+static constexpr int GSPriorityVendor = 900;
+
+/**
+ * Registers a reset function. Intended to be used internally and by vendors.
+ *
+ * Reset functions should be registered once when program execution begins.
+ * Reset functions may be called at any time from any thread, but will never
+ * be called on a RoboRIO, and will never be called concurrently. It is not
+ * guaranteed that they ever will be called.
+ *
+ * Reset functions should:
+ *
+ * - Run quickly
+ * - Reset all global singletons such that the old state is torn down and
+ *   the new state is immediately usable once the reset finishes. We
+ *   suggest using std::unique_ptr to hold your singletons.
+ *
+ * Reset functions should not:
+ *
+ * - Throw exceptions
+ */
+void RegisterGlobalStateReset(int priority, ResetCallback fn);
+
+/**
+ * Helper for statically registering global state functions:
+ *
+ * @code{.cpp}
+ * static RegisterGlobalStateResetHelper my_helper(GSPriorityVendor, []() {
+ *   // do some reset thing here
+ *   // my_singleton = std::make_unique<MySingleton>();
+ * });
+ * @endcode
+ */
+struct RegisterGlobalStateResetHelper {
+  RegisterGlobalStateResetHelper(int priority, ResetCallback fn) {
+    RegisterGlobalStateReset(priority, std::move(fn));
+  }
+};
+
+}  // namespace impl
+}  // namespace wpi
+
+#endif  // WPIUTIL_WPI_GLOBALSTATE_H_