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;
     }

hal/src/main/native/cpp/jni/InterruptJNI.cpp

@@ -50,21 +50,40 @@ Java_edu_wpi_first_hal_InterruptJNI_cleanInterrupts
 /*
  * Class:     edu_wpi_first_hal_InterruptJNI
  * Method:    waitForInterrupt
- * Signature: (IDZ)I
+ * Signature: (IDZ)J
  */
-JNIEXPORT jint JNICALL
+JNIEXPORT jlong JNICALL
 Java_edu_wpi_first_hal_InterruptJNI_waitForInterrupt
   (JNIEnv* env, jclass, jint interruptHandle, jdouble timeout,
    jboolean ignorePrevious)
 {
   int32_t status = 0;
-  int32_t result = HAL_WaitForInterrupt((HAL_InterruptHandle)interruptHandle,
+  int64_t result = HAL_WaitForInterrupt((HAL_InterruptHandle)interruptHandle,
                                         timeout, ignorePrevious, &status);
 
   CheckStatus(env, status);
   return result;
 }
 
+/*
+ * Class:     edu_wpi_first_hal_InterruptJNI
+ * Method:    waitForMultipleInterrupts
+ * Signature: (IJDZ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_waitForMultipleInterrupts
+  (JNIEnv* env, jclass, jint interruptHandle, jlong mask, jdouble timeout,
+   jboolean ignorePrevious)
+{
+  int32_t status = 0;
+  int64_t result =
+      HAL_WaitForMultipleInterrupts((HAL_InterruptHandle)interruptHandle, mask,
+                                    timeout, ignorePrevious, &status);
+
+  CheckStatus(env, status);
+  return result;
+}
+
 /*
  * Class:     edu_wpi_first_hal_InterruptJNI
  * Method:    readInterruptRisingTimestamp

hal/src/main/native/include/hal/ChipObject.h

@@ -34,7 +34,6 @@
 #include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tSysWatchdog.h>
 #include <FRC_FPGA_ChipObject/tDMAChannelDescriptor.h>
 #include <FRC_FPGA_ChipObject/tDMAManager.h>
-#include <FRC_FPGA_ChipObject/tInterruptManager.h>
 #include <FRC_FPGA_ChipObject/tSystem.h>
 #include <FRC_FPGA_ChipObject/tSystemInterface.h>
 

hal/src/main/native/include/hal/Interrupts.h

@@ -35,7 +35,7 @@ HAL_InterruptHandle HAL_InitializeInterrupts(int32_t* status);
 void HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle);
 
 /**
- * In synchronous mode, waits for the defined interrupt to occur.
+ * Waits for the defined interrupt to occur.
  *
  * @param[in] interruptHandle the interrupt handle
  * @param[in] timeout         timeout in seconds
@@ -48,6 +48,21 @@ int64_t HAL_WaitForInterrupt(HAL_InterruptHandle interruptHandle,
                              double timeout, HAL_Bool ignorePrevious,
                              int32_t* status);
 
+/**
+ * Waits for any interrupt covered by the mask to occur.
+ *
+ * @param[in] interruptHandle the interrupt handle to use for the context
+ * @param[in] mask            the mask of interrupts to wait for
+ * @param[in] timeout         timeout in seconds
+ * @param[in] ignorePrevious  if true, ignore interrupts that happened before
+ *                            waitForInterrupt was called
+ * @param[out] status         Error status variable. 0 on success.
+ * @return the mask of interrupts that fired
+ */
+int64_t HAL_WaitForMultipleInterrupts(HAL_InterruptHandle interruptHandle,
+                                      int64_t mask, double timeout,
+                                      HAL_Bool ignorePrevious, int32_t* status);
+
 /**
  * Returns the timestamp for the rising interrupt that occurred most recently.
  *

hal/src/main/native/include/hal/roborio/InterruptManager.h

@@ -0,0 +1,33 @@
+// 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.
+
+#pragma once
+
+#include <FRC_FPGA_ChipObject/fpgainterfacecapi/NiFpga.h>
+#include <wpi/mutex.h>
+
+#include "hal/ChipObject.h"
+#include "hal/Types.h"
+
+namespace hal {
+class InterruptManager {
+ public:
+  static InterruptManager& GetInstance();
+  static int32_t Initialize(tSystemInterface* baseSystem);
+
+  NiFpga_IrqContext GetContext() noexcept;
+  void ReleaseContext(NiFpga_IrqContext context) noexcept;
+
+  uint32_t WaitForInterrupt(NiFpga_IrqContext context, uint32_t mask,
+                            bool ignorePrevious, uint32_t timeoutInMs,
+                            int32_t* status);
+
+ private:
+  InterruptManager() = default;
+
+  wpi::priority_mutex currentMaskMutex;
+  uint32_t currentMask;
+  NiFpga_Session fpgaSession;
+};
+}  // namespace hal

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

@@ -351,6 +351,26 @@ int64_t HAL_WaitForInterrupt(HAL_InterruptHandle interruptHandle,
   }
 }
 
+int64_t HAL_WaitForMultipleInterrupts(HAL_InterruptHandle interruptHandle,
+                                      int64_t mask, double timeout,
+                                      HAL_Bool ignorePrevious,
+                                      int32_t* status) {
+  // TODO make this properly work, will require a decent rewrite
+  auto interrupt = interruptHandles->Get(interruptHandle);
+  if (interrupt == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return WaitResult::Timeout;
+  }
+
+  if (interrupt->isAnalog) {
+    return WaitForInterruptAnalog(interruptHandle, interrupt.get(), timeout,
+                                  ignorePrevious);
+  } else {
+    return WaitForInterruptDigital(interruptHandle, interrupt.get(), timeout,
+                                   ignorePrevious);
+  }
+}
+
 int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
                                          int32_t* status) {
   auto interrupt = interruptHandles->Get(interruptHandle);