diff --git a/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java b/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java index a47a364c88..7dc2e6dc6d 100644 --- a/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java @@ -11,9 +11,12 @@ public class InterruptJNI extends JNIWrapper { public static native void cleanInterrupts(int interruptHandle); - public static native int waitForInterrupt( + public static native long waitForInterrupt( int interruptHandle, double timeout, boolean ignorePrevious); + public static native long waitForMultipleInterrupts( + int interruptHandle, long mask, double timeout, boolean ignorePrevious); + public static native long readInterruptRisingTimestamp(int interruptHandle); public static native long readInterruptFallingTimestamp(int interruptHandle); diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 93e5c15d14..5eba6e4b4c 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/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); diff --git a/hal/src/main/native/athena/InterruptManager.cpp b/hal/src/main/native/athena/InterruptManager.cpp new file mode 100644 index 0000000000..0bd0672ad5 --- /dev/null +++ b/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 + +#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( + dlsym(NiFpgaLibrary, "NiFpgaDll_ReserveIrqContext")); + HAL_NiFpga_UnreserveIrqContext = + reinterpret_cast( + dlsym(NiFpgaLibrary, "NiFpgaDll_UnreserveIrqContext")); + HAL_NiFpga_WaitOnIrqs = reinterpret_cast( + dlsym(NiFpgaLibrary, "NiFpgaDll_WaitOnIrqs")); + HAL_NiFpga_AcknowledgeIrqs = reinterpret_cast( + 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; +} diff --git a/hal/src/main/native/athena/Interrupts.cpp b/hal/src/main/native/athena/Interrupts.cpp index 943a1aa060..e40c6f4550 100644 --- a/hal/src/main/native/athena/Interrupts.cpp +++ b/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 anInterrupt; - std::unique_ptr 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( - (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(timeout * 1e3), - ignorePrevious, status); + result = anInterrupt->manager.WaitForInterrupt( + anInterrupt->irqContext, anInterrupt->mask, ignorePrevious, + static_cast(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(timeout * 1e3), status); // Don't report a timeout as an error - the return code is enough to tell // that a timeout happened. diff --git a/hal/src/main/native/athena/Notifier.cpp b/hal/src/main/native/athena/Notifier.cpp index 2ec503062d..9c818d858e 100644 --- a/hal/src/main/native/athena/Notifier.cpp +++ b/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; } diff --git a/hal/src/main/native/cpp/jni/InterruptJNI.cpp b/hal/src/main/native/cpp/jni/InterruptJNI.cpp index ed56ce5410..e2201258a0 100644 --- a/hal/src/main/native/cpp/jni/InterruptJNI.cpp +++ b/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 diff --git a/hal/src/main/native/include/hal/ChipObject.h b/hal/src/main/native/include/hal/ChipObject.h index 4526a1a9c1..23dcb3894a 100644 --- a/hal/src/main/native/include/hal/ChipObject.h +++ b/hal/src/main/native/include/hal/ChipObject.h @@ -34,7 +34,6 @@ #include #include #include -#include #include #include diff --git a/hal/src/main/native/include/hal/Interrupts.h b/hal/src/main/native/include/hal/Interrupts.h index def800ce99..2bccbe9ce4 100644 --- a/hal/src/main/native/include/hal/Interrupts.h +++ b/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. * diff --git a/hal/src/main/native/include/hal/roborio/InterruptManager.h b/hal/src/main/native/include/hal/roborio/InterruptManager.h new file mode 100644 index 0000000000..ee7fc38c43 --- /dev/null +++ b/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 +#include + +#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 diff --git a/hal/src/main/native/sim/Interrupts.cpp b/hal/src/main/native/sim/Interrupts.cpp index e55316d14a..ac9f7f8ebb 100644 --- a/hal/src/main/native/sim/Interrupts.cpp +++ b/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); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java index ad48b5e5c4..8983e2728b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java @@ -90,7 +90,7 @@ public class SynchronousInterrupt implements AutoCloseable { * interrupt was read. * @return The raw hardware interrupt result */ - int waitForInterruptRaw(double timeoutSeconds, boolean ignorePrevious) { + long waitForInterruptRaw(double timeoutSeconds, boolean ignorePrevious) { return InterruptJNI.waitForInterrupt(m_handle, timeoutSeconds, ignorePrevious); } @@ -104,7 +104,7 @@ public class SynchronousInterrupt implements AutoCloseable { * @return Result of which edges were triggered, or if an timeout occurred. */ public WaitResult waitForInterrupt(double timeoutSeconds, boolean ignorePrevious) { - int result = InterruptJNI.waitForInterrupt(m_handle, timeoutSeconds, ignorePrevious); + long result = InterruptJNI.waitForInterrupt(m_handle, timeoutSeconds, ignorePrevious); // Rising edge result is the interrupt bit set in the byte 0xFF // Falling edge result is the interrupt bit set in the byte 0xFF00