[hal,wpiutil] Use HMB for FPGA Timestamps (#5499)

Current timestamp read code uses FPGA register reads. Through testing,
this read was slower then clock_gettime by about 4-5x. However, another
method of reading the FPGA time is available, using HMB. HMB
is memory mapped IO from RAM to the FPGA. So to code side,
reading the value is just a memory barrier and a memory read.

There is some latency on the write side, so a very small artifical delay
(5us) is added to avoid register reads such as interrupts being ahead
of current timestamps, which could cause issues.

Below is read times for 1000 calls to clock_gettime, register reads and
hmb reads.
```
Clock: Rise 1.72939400 s Fall 1.72990700 s Delta 0.00051300 s
FPGA : Rise 1.72999000 s Fall 1.73429300 s Delta 0.00430300 s
HMB  : Rise 1.73466800 s Fall 1.73481900 s Delta 0.00015100 s
```

Also add full HMB struct to HAL for future usage.

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

@@ -33,6 +33,7 @@
 #include "hal/Errors.h"
 #include "hal/Notifier.h"
 #include "hal/handles/HandlesInternal.h"
+#include "hal/roborio/HMB.h"
 #include "hal/roborio/InterruptManager.h"
 #include "visa/visa.h"
 
@@ -46,6 +47,9 @@ static char roboRioCommentsString[64];
 static size_t roboRioCommentsStringSize;
 static bool roboRioCommentsStringInitialized;
 
+static const volatile HAL_HMBData* hmbBuffer;
+#define HAL_HMB_TIMESTAMP_OFFSET 5
+
 using namespace hal;
 
 namespace hal {
@@ -351,25 +355,29 @@ size_t HAL_GetComments(char* buffer, size_t size) {
 
 uint64_t HAL_GetFPGATime(int32_t* status) {
   hal::init::CheckInit();
-  if (!global) {
+  if (!hmbBuffer) {
     *status = NiFpga_Status_ResourceNotInitialized;
     return 0;
   }
-  *status = 0;
-  uint64_t upper1 = global->readLocalTimeUpper(status);
-  uint32_t lower = global->readLocalTime(status);
-  uint64_t upper2 = global->readLocalTimeUpper(status);
-  if (*status != 0) {
-    return 0;
-  }
+
+  asm("dmb");
+  uint64_t upper1 = hmbBuffer->Timestamp.Upper;
+  asm("dmb");
+  uint32_t lower = hmbBuffer->Timestamp.Lower;
+  asm("dmb");
+  uint64_t upper2 = hmbBuffer->Timestamp.Upper;
+
   if (upper1 != upper2) {
     // Rolled over between the lower call, reread lower
-    lower = global->readLocalTime(status);
-    if (*status != 0) {
-      return 0;
-    }
+    asm("dmb");
+    lower = hmbBuffer->Timestamp.Lower;
   }
-  return (upper2 << 32) + lower;
+  // 5 is added here because the time to write from the FPGA
+  // to the HMB buffer is longer then the time to read
+  // from the time register. This would cause register based
+  // timestamps to be ahead of HMB timestamps, which could
+  // be very bad.
+  return (upper2 << 32) + lower + HAL_HMB_TIMESTAMP_OFFSET;
 }
 
 uint64_t HAL_ExpandFPGATime(uint32_t unexpandedLower, int32_t* status) {
@@ -521,6 +529,13 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
   wpi::impl::SetupNowRio();
 
   int32_t status = 0;
+
+  HAL_InitializeHMB(&status);
+  if (status != 0) {
+    return false;
+  }
+  hmbBuffer = HAL_GetHMBBuffer();
+
   global.reset(tGlobal::create(&status));
   watchdog.reset(tSysWatchdog::create(&status));
 

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

@@ -0,0 +1,82 @@
+// 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 "hal/roborio/HMB.h"
+
+#include <memory>
+
+#include "FPGACalls.h"
+#include "hal/ChipObject.h"
+
+using namespace hal;
+
+// 16 classes of data, each takes up 16 uint16_t's
+static_assert(sizeof(HAL_HMBData) == 0x10 * 16 * sizeof(uint32_t));
+// Timestamp is the last class, and should be offset correctly
+static_assert(offsetof(HAL_HMBData, Timestamp) == 0x10 * 15 * sizeof(uint32_t));
+
+static volatile HAL_HMBData* hmbBuffer;
+static size_t hmbBufferSize;
+static constexpr const char hmbName[] = "HMB_0_RAM";
+static std::unique_ptr<tHMB> hmb;
+
+namespace {
+struct HMBHolder {
+  ~HMBHolder() {
+    if (hmbBuffer) {
+      hal::HAL_NiFpga_CloseHmb(hmb->getSystemInterface()->getHandle(), hmbName);
+    }
+  }
+};
+}  // namespace
+
+extern "C" {
+
+void HAL_InitializeHMB(int32_t* status) {
+  static HMBHolder holder;
+
+  hmb.reset(tHMB::create(status));
+  if (*status != 0) {
+    return;
+  }
+
+  *status = hal::HAL_NiFpga_OpenHmb(
+      hmb->getSystemInterface()->getHandle(), hmbName, &hmbBufferSize,
+      reinterpret_cast<void**>(const_cast<HAL_HMBData**>(&hmbBuffer)));
+
+  if (*status != 0) {
+    return;
+  }
+
+  auto cfg = hmb->readConfig(status);
+  cfg.Enables_AI0_Low = 1;
+  cfg.Enables_AI0_High = 1;
+  cfg.Enables_AIAveraged0_Low = 1;
+  cfg.Enables_AIAveraged0_High = 1;
+  cfg.Enables_Accumulator0 = 1;
+  cfg.Enables_Accumulator1 = 1;
+  cfg.Enables_DI = 1;
+  cfg.Enables_AnalogTriggers = 1;
+  cfg.Enables_Counters_Low = 1;
+  cfg.Enables_Counters_High = 1;
+  cfg.Enables_CounterTimers_Low = 1;
+  cfg.Enables_CounterTimers_High = 1;
+  cfg.Enables_Encoders_Low = 1;
+  cfg.Enables_Encoders_High = 1;
+  cfg.Enables_EncoderTimers_Low = 1;
+  cfg.Enables_EncoderTimers_High = 1;
+  cfg.Enables_DutyCycle_Low = 1;
+  cfg.Enables_DutyCycle_High = 1;
+  cfg.Enables_Interrupts = 1;
+  cfg.Enables_PWM = 1;
+  cfg.Enables_PWM_MXP = 1;
+  cfg.Enables_Relay_DO_AO = 1;
+  cfg.Enables_Timestamp = 1;
+  hmb->writeConfig(cfg, status);
+}
+
+const volatile HAL_HMBData* HAL_GetHMBBuffer(void) {
+  return hmbBuffer;
+}
+}  // extern "C"