Adds way to reset and version all HAL handles (#545)

Useful in the sim to force a full reset. On roboRIO, the information is
still created and added, but is not checked because of speed
considerations.

hal/lib/shared/handles/HandlesInternal.cpp

@@ -7,7 +7,51 @@
 
 #include "HAL/handles/HandlesInternal.h"
 
+#include <algorithm>
+
+#include "HAL/cpp/priority_mutex.h"
+#include "llvm/SmallVector.h"
+
 namespace hal {
+static llvm::SmallVector<HandleBase*, 32> globalHandles;
+static priority_mutex globalHandleMutex;
+
+HandleBase::HandleBase() {
+  std::lock_guard<priority_mutex> lock(globalHandleMutex);
+  auto index = std::find(globalHandles.begin(), globalHandles.end(), this);
+  if (index == globalHandles.end()) {
+    globalHandles.push_back(this);
+  } else {
+    *index = this;
+  }
+}
+
+HandleBase::~HandleBase() {
+  std::lock_guard<priority_mutex> lock(globalHandleMutex);
+  auto index = std::find(globalHandles.begin(), globalHandles.end(), this);
+  if (index != globalHandles.end()) {
+    *index = nullptr;
+  }
+}
+
+void HandleBase::ResetHandles() {
+  m_version++;
+  if (m_version > 255) {
+    m_version = 0;
+  }
+}
+
+void HandleBase::ResetGlobalHandles() {
+  std::unique_lock<priority_mutex> lock(globalHandleMutex);
+  for (auto&& i : globalHandles) {
+    if (i != nullptr) {
+      lock.unlock();
+      i->ResetHandles();
+      lock.lock();
+    }
+  }
+}
+
 HAL_PortHandle createPortHandle(uint8_t channel, uint8_t module) {
   // set last 8 bits, then shift to first 8 bits
   HAL_PortHandle handle = static_cast<HAL_PortHandle>(HAL_HandleEnum::Port);
@@ -36,13 +80,16 @@ HAL_PortHandle createPortHandleForSPI(uint8_t channel) {
   return handle;
 }
 
-HAL_Handle createHandle(int16_t index, HAL_HandleEnum handleType) {
+HAL_Handle createHandle(int16_t index, HAL_HandleEnum handleType,
+                        int16_t version) {
   if (index < 0) return HAL_kInvalidHandle;
   uint8_t hType = static_cast<uint8_t>(handleType);
   if (hType == 0 || hType > 127) return HAL_kInvalidHandle;
   // set last 8 bits, then shift to first 8 bits
   HAL_Handle handle = hType;
-  handle = handle << 24;
+  handle = handle << 8;
+  handle += static_cast<uint8_t>(version);
+  handle = handle << 16;
   // add index to set last 16 bits
   handle += index;
   return handle;