[wpiutil] timestamp: Call FPGA functions directly (#5235)

This works around an exit race with wpi::Now() on Rio; it was overridden
to call HAL_GetFPGATime(), which calls chipobject, but on exit, because
there was not a library dependency, the chipobject could be destroyed
prior to wpiutil/wpinet being shut down.

wpiutil/src/main/native/cpp/timestamp.cpp

@@ -6,6 +6,23 @@
 
 #include <atomic>
 
+#ifdef __FRC_ROBORIO__
+#include <stdint.h>
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#pragma GCC diagnostic ignored "-Wignored-qualifiers"
+#include <FRC_FPGA_ChipObject/RoboRIO_FRC_ChipObject_Aliases.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/nInterfaceGlobals.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tGlobal.h>
+#include <FRC_NetworkCommunication/LoadOut.h>
+#pragma GCC diagnostic pop
+namespace fpga {
+using namespace nFPGA;
+using namespace nRoboRIO_FPGANamespace;
+}  // namespace fpga
+#include <memory>
+#endif
+
 #ifdef _WIN32
 #include <windows.h>
 
@@ -15,6 +32,14 @@
 #include <chrono>
 #endif
 
+#include <cstdio>
+
+#include "fmt/format.h"
+
+#ifdef __FRC_ROBORIO__
+static std::unique_ptr<fpga::tGlobal> global;
+#endif
+
 // offset in microseconds
 static uint64_t time_since_epoch() noexcept {
 #ifdef _WIN32
@@ -88,12 +113,57 @@ uint64_t wpi::NowDefault() {
 
 static std::atomic<uint64_t (*)()> now_impl{wpi::NowDefault};
 
+void wpi::impl::SetupNowRio() {
+#ifdef __FRC_ROBORIO__
+  if (!global) {
+    nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
+        nLoadOut::getTargetClass();
+    int32_t status = 0;
+    global.reset(fpga::tGlobal::create(&status));
+  }
+#endif
+}
+
 void wpi::SetNowImpl(uint64_t (*func)(void)) {
   now_impl = func ? func : NowDefault;
 }
 
 uint64_t wpi::Now() {
+#ifdef __FRC_ROBORIO__
+  // Same code as HAL_GetFPGATime()
+  if (!global) {
+    std::fputs(
+        "FPGA not yet configured in wpi::Now(). Time will not be correct",
+        stderr);
+    std::fflush(stderr);
+    return 0;
+  }
+  int32_t status = 0;
+  uint64_t upper1 = global->readLocalTimeUpper(&status);
+  uint32_t lower = global->readLocalTime(&status);
+  uint64_t upper2 = global->readLocalTimeUpper(&status);
+  if (status != 0) {
+    goto err;
+  }
+  if (upper1 != upper2) {
+    // Rolled over between the lower call, reread lower
+    lower = global->readLocalTime(&status);
+    if (status != 0) {
+      goto err;
+    }
+  }
+  return (upper2 << 32) + lower;
+
+err:
+  fmt::print(stderr,
+             "Call to FPGA failed in wpi::Now() with status {}. "
+             "Initialization might have failed. Time will not be correct\n",
+             status);
+  std::fflush(stderr);
+  return 0;
+#else
   return (now_impl.load())();
+#endif
 }
 
 uint64_t wpi::GetSystemTime() {
@@ -102,6 +172,10 @@ uint64_t wpi::GetSystemTime() {
 
 extern "C" {
 
+void WPI_Impl_SetupNowRio(void) {
+  return wpi::impl::SetupNowRio();
+}
+
 uint64_t WPI_NowDefault(void) {
   return wpi::NowDefault();
 }

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

@@ -11,6 +11,13 @@
 extern "C" {
 #endif
 
+/**
+ * Initialize the on-Rio Now() implementation to use the FPGA timestamp.
+ * No effect on non-Rio platforms. This is called by HAL_Initialize() and
+ * thus should generally not be called by user code.
+ */
+void WPI_Impl_SetupNowRio(void);
+
 /**
  * The default implementation used for Now().
  * In general this is the time returned by the operating system.
@@ -48,12 +55,21 @@ uint64_t WPI_GetSystemTime(void);
 #ifdef __cplusplus
 namespace wpi {
 
+namespace impl {
+/**
+ * Initialize the on-Rio Now() implementation to use the FPGA timestamp.
+ * No effect on non-Rio platforms. This is called by HAL_Initialize() and
+ * thus should generally not be called by user code.
+ */
+void SetupNowRio();
+}  // namespace impl
+
 /**
  * The default implementation used for Now().
  * In general this is the time returned by the operating system.
  * @return Time in microseconds.
  */
-uint64_t NowDefault(void);
+uint64_t NowDefault();
 
 /**
  * Set the implementation used by Now().
@@ -68,7 +84,7 @@ void SetNowImpl(uint64_t (*func)());
  * This is a monotonic clock with an undefined epoch.
  * @return Time in microseconds.
  */
-uint64_t Now(void);
+uint64_t Now();
 
 /**
  * Return the current system time in microseconds since the Unix epoch