Switch away from NI interrupt manager to custom implementation (#3705)

* Switch away from NI interrupt manager to custom implementation

* Formatting

* Fix tidy

* Formatting

* Fix loading

* Make interrupt api public

* Add multiple wait api

* Formatting

* Fix build

* Fix review comments

* wpiformat

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

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

@@ -28,6 +28,7 @@
 #include "hal/Errors.h"
 #include "hal/Notifier.h"
 #include "hal/handles/HandlesInternal.h"
+#include "hal/roborio/InterruptManager.h"
 #include "visa/visa.h"
 
 using namespace hal;
@@ -424,6 +425,12 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
     return false;
   }
 
+  status = InterruptManager::Initialize(global->getSystemInterface());
+
+  if (status != 0) {
+    return false;
+  }
+
   HAL_InitializeDriverStation();
 
   dsStartTime = HAL_GetFPGATime(&status);

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

@@ -0,0 +1,123 @@
+// 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/InterruptManager.h"
+
+#include <fmt/format.h>
+
+#include "HALInternal.h"
+#include "dlfcn.h"
+#include "hal/Errors.h"
+
+// Low level FPGA calls
+using HAL_NiFpga_ReserveIrqContextFunc =
+    NiFpga_Status (*)(NiFpga_Session session, NiFpga_IrqContext* context);
+
+static HAL_NiFpga_ReserveIrqContextFunc HAL_NiFpga_ReserveIrqContext;
+
+using HAL_NiFpga_UnreserveIrqContextFunc =
+    NiFpga_Status (*)(NiFpga_Session session, NiFpga_IrqContext context);
+
+static HAL_NiFpga_UnreserveIrqContextFunc HAL_NiFpga_UnreserveIrqContext;
+
+using HAL_NiFpga_WaitOnIrqsFunc = NiFpga_Status (*)(
+    NiFpga_Session session, NiFpga_IrqContext context, uint32_t irqs,
+    uint32_t timeout, uint32_t* irqsAsserted, NiFpga_Bool* timedOut);
+
+static HAL_NiFpga_WaitOnIrqsFunc HAL_NiFpga_WaitOnIrqs;
+
+using HAL_NiFpga_AcknowledgeIrqsFunc = NiFpga_Status (*)(NiFpga_Session session,
+                                                         uint32_t irqs);
+
+static HAL_NiFpga_AcknowledgeIrqsFunc HAL_NiFpga_AcknowledgeIrqs;
+
+static void* NiFpgaLibrary = nullptr;
+
+using namespace hal;
+
+InterruptManager& InterruptManager::GetInstance() {
+  static InterruptManager manager;
+  return manager;
+}
+
+int32_t InterruptManager::Initialize(tSystemInterface* baseSystem) {
+  auto& manager = GetInstance();
+  manager.fpgaSession = baseSystem->getHandle();
+
+  NiFpgaLibrary = dlopen("libNiFpga.so", RTLD_LAZY);
+  if (!NiFpgaLibrary) {
+    return errno;
+  }
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+  HAL_NiFpga_ReserveIrqContext =
+      reinterpret_cast<HAL_NiFpga_ReserveIrqContextFunc>(
+          dlsym(NiFpgaLibrary, "NiFpgaDll_ReserveIrqContext"));
+  HAL_NiFpga_UnreserveIrqContext =
+      reinterpret_cast<HAL_NiFpga_UnreserveIrqContextFunc>(
+          dlsym(NiFpgaLibrary, "NiFpgaDll_UnreserveIrqContext"));
+  HAL_NiFpga_WaitOnIrqs = reinterpret_cast<HAL_NiFpga_WaitOnIrqsFunc>(
+      dlsym(NiFpgaLibrary, "NiFpgaDll_WaitOnIrqs"));
+  HAL_NiFpga_AcknowledgeIrqs = reinterpret_cast<HAL_NiFpga_AcknowledgeIrqsFunc>(
+      dlsym(NiFpgaLibrary, "NiFpgaDll_AcknowledgeIrqs"));
+#pragma GCC diagnostic pop
+
+  if (HAL_NiFpga_ReserveIrqContext == nullptr ||
+      HAL_NiFpga_UnreserveIrqContext == nullptr ||
+      HAL_NiFpga_WaitOnIrqs == nullptr ||
+      HAL_NiFpga_AcknowledgeIrqs == nullptr) {
+    return NO_AVAILABLE_RESOURCES;
+  }
+
+  return HAL_SUCCESS;
+}
+
+NiFpga_IrqContext InterruptManager::GetContext() noexcept {
+  NiFpga_IrqContext context;
+  HAL_NiFpga_ReserveIrqContext(fpgaSession, &context);
+  return context;
+}
+
+void InterruptManager::ReleaseContext(NiFpga_IrqContext context) noexcept {
+  HAL_NiFpga_UnreserveIrqContext(fpgaSession, context);
+}
+
+uint32_t InterruptManager::WaitForInterrupt(NiFpga_IrqContext context,
+                                            uint32_t mask, bool ignorePrevious,
+                                            uint32_t timeoutMs,
+                                            int32_t* status) {
+  {
+    // Make sure we can safely use this
+    std::scoped_lock lock(currentMaskMutex);
+    if ((currentMask & mask) != 0) {
+      *status = PARAMETER_OUT_OF_RANGE;
+      hal::SetLastError(
+          status, fmt::format("Interrupt mask {} has bits {} already in use",
+                              mask, (currentMask & mask)));
+      return 0;
+    }
+    currentMask |= mask;
+  }
+
+  if (ignorePrevious) {
+    HAL_NiFpga_AcknowledgeIrqs(fpgaSession, mask);
+  }
+
+  uint32_t irqsAsserted = 0;
+  NiFpga_Bool timedOut = 0;
+  *status = HAL_NiFpga_WaitOnIrqs(fpgaSession, context, mask, timeoutMs,
+                                  &irqsAsserted, &timedOut);
+
+  if (!timedOut) {
+    HAL_NiFpga_AcknowledgeIrqs(fpgaSession, irqsAsserted);
+  }
+
+  {
+    std::scoped_lock lock(currentMaskMutex);
+    currentMask &= ~mask;
+  }
+
+  return irqsAsserted;
+}

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

@@ -17,6 +17,7 @@
 #include "hal/HALBase.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/LimitedHandleResource.h"
+#include "hal/roborio/InterruptManager.h"
 
 using namespace hal;
 
@@ -24,7 +25,9 @@ namespace {
 
 struct Interrupt {
   std::unique_ptr<tInterrupt> anInterrupt;
-  std::unique_ptr<tInterruptManager> manager;
+  InterruptManager& manager = InterruptManager::GetInstance();
+  NiFpga_IrqContext irqContext = nullptr;
+  uint32_t mask;
 };
 
 }  // namespace
@@ -55,8 +58,8 @@ HAL_InterruptHandle HAL_InitializeInterrupts(int32_t* status) {
   // Expects the calling leaf class to allocate an interrupt index.
   anInterrupt->anInterrupt.reset(tInterrupt::create(interruptIndex, status));
   anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status);
-  anInterrupt->manager = std::make_unique<tInterruptManager>(
-      (1u << interruptIndex) | (1u << (interruptIndex + 8u)), true, status);
+  anInterrupt->irqContext = anInterrupt->manager.GetContext();
+  anInterrupt->mask = (1u << interruptIndex) | (1u << (interruptIndex + 8u));
   return handle;
 }
 
@@ -66,6 +69,9 @@ void HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle) {
   if (anInterrupt == nullptr) {
     return;
   }
+  if (anInterrupt->irqContext) {
+    anInterrupt->manager.ReleaseContext(anInterrupt->irqContext);
+  }
 }
 
 int64_t HAL_WaitForInterrupt(HAL_InterruptHandle interruptHandle,
@@ -78,8 +84,33 @@ int64_t HAL_WaitForInterrupt(HAL_InterruptHandle interruptHandle,
     return 0;
   }
 
-  result = anInterrupt->manager->watch(static_cast<int32_t>(timeout * 1e3),
-                                       ignorePrevious, status);
+  result = anInterrupt->manager.WaitForInterrupt(
+      anInterrupt->irqContext, anInterrupt->mask, ignorePrevious,
+      static_cast<uint32_t>(timeout * 1e3), status);
+
+  // Don't report a timeout as an error - the return code is enough to tell
+  // that a timeout happened.
+  if (*status == -NiFpga_Status_IrqTimeout) {
+    *status = NiFpga_Status_Success;
+  }
+
+  return result;
+}
+
+int64_t HAL_WaitForMultipleInterrupts(HAL_InterruptHandle interruptHandle,
+                                      int64_t mask, double timeout,
+                                      HAL_Bool ignorePrevious,
+                                      int32_t* status) {
+  uint32_t result;
+  auto anInterrupt = interruptHandles->Get(interruptHandle);
+  if (anInterrupt == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  result = anInterrupt->manager.WaitForInterrupt(
+      anInterrupt->irqContext, mask, ignorePrevious,
+      static_cast<uint32_t>(timeout * 1e3), status);
 
   // Don't report a timeout as an error - the return code is enough to tell
   // that a timeout happened.

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

@@ -20,6 +20,7 @@
 #include "hal/HAL.h"
 #include "hal/Threads.h"
 #include "hal/handles/UnlimitedHandleResource.h"
+#include "hal/roborio/InterruptManager.h"
 
 using namespace hal;
 
@@ -106,10 +107,15 @@ static void alarmCallback() {
 }
 
 static void notifierThreadMain() {
-  tRioStatusCode status = 0;
-  tInterruptManager manager{1 << kTimerInterruptNumber, true, &status};
+  InterruptManager& manager = InterruptManager::GetInstance();
+  NiFpga_IrqContext context = manager.GetContext();
+  uint32_t mask = 1 << kTimerInterruptNumber;
+  int32_t status = 0;
+
   while (notifierRunning) {
-    auto triggeredMask = manager.watch(10000, false, &status);
+    status = 0;
+    auto triggeredMask =
+        manager.WaitForInterrupt(context, mask, false, 10000, &status);
     if (!notifierRunning) {
       break;
     }