[hal] Cache sim TCP data to update during HAL_RefreshDSData (#7360)

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

@@ -55,6 +55,7 @@ struct FRCDriverStation {
   ~FRCDriverStation() { gShutdown = true; }
   wpi::EventVector newDataEvents;
   wpi::mutex cacheMutex;
+  wpi::mutex tcpCacheMutex;
 };
 }  // namespace
 
@@ -90,6 +91,29 @@ static std::atomic<JoystickDataCache*> currentCache{nullptr};
 static JoystickDataCache* lastGiven = &caches[1];
 static JoystickDataCache* cacheToUpdate = &caches[2];
 
+namespace {
+struct TcpCache {
+  TcpCache() { std::memset(this, 0, sizeof(*this)); }
+  void Update();
+  void CloneTo(TcpCache* other) { std::memcpy(other, this, sizeof(*this)); }
+
+  HAL_MatchInfo matchInfo;
+  HAL_JoystickDescriptor descriptors[HAL_kMaxJoysticks];
+};
+static_assert(std::is_standard_layout_v<TcpCache>);
+}  // namespace
+
+static TcpCache tcpCache;
+static TcpCache tcpCurrent;
+
+void TcpCache::Update() {
+  SimDriverStationData->GetMatchInfo(&matchInfo);
+
+  for (int i = 0; i < HAL_kMaxJoysticks; i++) {
+    SimDriverStationData->GetJoystickDescriptor(i, &descriptors[i]);
+  }
+}
+
 static ::FRCDriverStation* driverStation;
 
 namespace hal::init {
@@ -256,32 +280,47 @@ void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs,
 int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
                                   HAL_JoystickDescriptor* desc) {
   CHECK_JOYSTICK_NUMBER(joystickNum);
-  SimDriverStationData->GetJoystickDescriptor(joystickNum, desc);
+  std::scoped_lock lock{driverStation->tcpCacheMutex};
+  *desc = tcpCurrent.descriptors[joystickNum];
   return 0;
 }
 
 HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) {
-  HAL_JoystickDescriptor desc;
-  SimDriverStationData->GetJoystickDescriptor(joystickNum, &desc);
-  return desc.isXbox;
+  HAL_JoystickDescriptor joystickDesc;
+  if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
+    return 0;
+  } else {
+    return joystickDesc.isXbox;
+  }
 }
 
 int32_t HAL_GetJoystickType(int32_t joystickNum) {
-  HAL_JoystickDescriptor desc;
-  SimDriverStationData->GetJoystickDescriptor(joystickNum, &desc);
-  return desc.type;
+  HAL_JoystickDescriptor joystickDesc;
+  if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
+    return -1;
+  } else {
+    return joystickDesc.type;
+  }
 }
 
 void HAL_GetJoystickName(struct WPI_String* name, int32_t joystickNum) {
-  HAL_JoystickDescriptor desc;
-  SimDriverStationData->GetJoystickDescriptor(joystickNum, &desc);
-  size_t len = std::strlen(desc.name);
+  HAL_JoystickDescriptor joystickDesc;
+  const char* cName = joystickDesc.name;
+  if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
+    cName = "";
+  }
+  auto len = std::strlen(cName);
   auto write = WPI_AllocateString(name, len);
-  std::memcpy(write, desc.name, len);
+  std::memcpy(write, cName, len);
 }
 
 int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) {
-  return 0;
+  HAL_JoystickDescriptor joystickDesc;
+  if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
+    return -1;
+  } else {
+    return joystickDesc.axisTypes[axis];
+  }
 }
 
 int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
@@ -300,7 +339,8 @@ double HAL_GetMatchTime(int32_t* status) {
 }
 
 int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
-  SimDriverStationData->GetMatchInfo(info);
+  std::scoped_lock lock{driverStation->tcpCacheMutex};
+  *info = tcpCurrent.matchInfo;
   return 0;
 }
 
@@ -329,31 +369,42 @@ HAL_Bool HAL_RefreshDSData(void) {
     return false;
   }
   bool dsAttached = SimDriverStationData->dsAttached;
-  std::scoped_lock lock{driverStation->cacheMutex};
-  JoystickDataCache* prev = currentCache.exchange(nullptr);
-  if (prev != nullptr) {
-    currentRead = prev;
+  JoystickDataCache* prev;
+  {
+    std::scoped_lock lock{driverStation->cacheMutex};
+    prev = currentCache.exchange(nullptr);
+    if (prev != nullptr) {
+      currentRead = prev;
+    }
+    // If newest state shows we have a DS attached, just use the
+    // control word out of the cache, As it will be the one in sync
+    // with the data. If no data has been updated, at this point,
+    // and a DS wasn't attached previously, this will still return
+    // a zeroed out control word, with is the correct state for
+    // no new data.
+    if (!dsAttached) {
+      // If the DS is not attached, we need to zero out the control word.
+      // This is because HAL_RefreshDSData is called asynchronously from
+      // the DS data. The dsAttached variable comes directly from netcomm
+      // and could be updated before the caches are. If that happens,
+      // we would end up returning the previous cached control word,
+      // which is out of sync with the current control word and could
+      // break invariants such as which alliance station is in used.
+      // Also, when the DS has never been connected the rest of the fields
+      // in control word are garbage, so we also need to zero out in that
+      // case too
+      std::memset(&currentRead->controlWord, 0,
+                  sizeof(currentRead->controlWord));
+    }
+    newestControlWord = currentRead->controlWord;
   }
-  // If newest state shows we have a DS attached, just use the
-  // control word out of the cache, As it will be the one in sync
-  // with the data. If no data has been updated, at this point,
-  // and a DS wasn't attached previously, this will still return
-  // a zeroed out control word, with is the correct state for
-  // no new data.
-  if (!dsAttached) {
-    // If the DS is not attached, we need to zero out the control word.
-    // This is because HAL_RefreshDSData is called asynchronously from
-    // the DS data. The dsAttached variable comes directly from netcomm
-    // and could be updated before the caches are. If that happens,
-    // we would end up returning the previous cached control word,
-    // which is out of sync with the current control word and could
-    // break invariants such as which alliance station is in used.
-    // Also, when the DS has never been connected the rest of the fields
-    // in control word are garbage, so we also need to zero out in that
-    // case too
-    std::memset(&currentRead->controlWord, 0, sizeof(currentRead->controlWord));
+
+  {
+    tcpCache.Update();
+    std::scoped_lock tcpLock(driverStation->tcpCacheMutex);
+    tcpCache.CloneTo(&tcpCurrent);
   }
-  newestControlWord = currentRead->controlWord;
+
   return prev != nullptr;
 }