diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 3497ba3a09..44cff93f82 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/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)); diff --git a/hal/src/main/native/athena/HMB.cpp b/hal/src/main/native/athena/HMB.cpp new file mode 100644 index 0000000000..d22fcda721 --- /dev/null +++ b/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 + +#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 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(const_cast(&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" diff --git a/hal/src/main/native/include/hal/roborio/HMB.h b/hal/src/main/native/include/hal/roborio/HMB.h new file mode 100644 index 0000000000..fc7381bbed --- /dev/null +++ b/hal/src/main/native/include/hal/roborio/HMB.h @@ -0,0 +1,112 @@ +// 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 + +struct HAL_HMBData { + struct AnalogInputs { + uint32_t Values[8]; + uint32_t Reserved[8]; + } AnalogInputs; + struct AveragedAnalogInputs { + uint32_t Values[8]; + uint32_t Reserved[8]; + } AveragedAnalogInputs; + struct Accumulator0 { + uint32_t Count; + uint32_t Value0; + uint32_t Value1; + uint32_t Reserved[13]; + } Accumulator0; + struct Accumulator1 { + uint32_t Count; + uint32_t Value0; + uint32_t Value1; + uint32_t Reserved[13]; + } Accumulator1; + struct DI { + uint32_t Values; + uint32_t FilteredValues; + uint32_t Reserved[14]; + } DI; + struct AnalogTriggers { + struct Trigger { + uint8_t InHysteresis0 : 1; + uint8_t OverLimit0 : 1; + uint8_t Rising0 : 1; + uint8_t Falling0 : 1; + uint8_t InHysteresis1 : 1; + uint8_t OverLimit1 : 1; + uint8_t Rising1 : 1; + uint8_t Falling1 : 1; + } Trigger[4]; + uint32_t Reserved[15]; + } AnalogTriggers; + struct Counters { + struct Counter { + uint32_t Direction : 1; + int32_t Value : 31; + } Counter[8]; + uint32_t Reserved[8]; + } Counters; + struct CounterTimers { + struct Timer { + uint32_t Period : 23; + int32_t Count : 8; + uint32_t Stalled : 1; + } Timer[8]; + uint32_t Reserved[8]; + } CounterTimers; + struct Encoders { + struct Encoder { + uint32_t Direction : 1; + int32_t Value : 31; + } Encoder[8]; + uint32_t Reserved[8]; + } Encoders; + struct EncoderTimers { + struct Timer { + uint32_t Period : 23; + int32_t Count : 8; + uint32_t Stalled : 1; + } Timer[8]; + uint32_t Reserved[8]; + } EncoderTimers; + struct DutyCycle { + uint32_t Output[8]; + uint32_t Reserved[8]; + } DutyCycle; + struct Interrupts { + struct Interrupt { + uint32_t FallingTimestamp; + uint32_t RisingTimestamp; + } Interrupt[8]; + } Interrupts; + struct PWM { + uint32_t Headers[10]; + uint32_t Reserved[6]; + uint32_t MXP[10]; + uint32_t Reserved2[6]; + } PWM; + struct RelayDOAO { + uint32_t Relays; + uint32_t Reserved; + uint32_t AO[2]; + uint32_t Reserved2[12]; + } RelayDOAO; + struct Timestamp { + uint32_t Lower; + uint32_t Upper; + uint32_t Reserved[14]; + } Timestamp; +}; + +extern "C" { + +void HAL_InitializeHMB(int32_t* status); + +const volatile HAL_HMBData* HAL_GetHMBBuffer(void); +} // extern "C" diff --git a/wpiutil/src/main/native/cpp/timestamp.cpp b/wpiutil/src/main/native/cpp/timestamp.cpp index 84f5022b13..1bfc1fc324 100644 --- a/wpiutil/src/main/native/cpp/timestamp.cpp +++ b/wpiutil/src/main/native/cpp/timestamp.cpp @@ -13,7 +13,7 @@ #pragma GCC diagnostic ignored "-Wignored-qualifiers" #include #include -#include +#include #include #pragma GCC diagnostic pop namespace fpga { @@ -21,6 +21,8 @@ using namespace nFPGA; using namespace nRoboRIO_FPGANamespace; } // namespace fpga #include + +#include "dlfcn.h" #endif #ifdef _WIN32 @@ -37,7 +39,66 @@ using namespace nRoboRIO_FPGANamespace; #include "fmt/format.h" #ifdef __FRC_ROBORIO__ -static std::unique_ptr global; +namespace { +static constexpr const char hmbName[] = "HMB_0_RAM"; +static constexpr int timestampLowerOffset = 0xF0; +static constexpr int timestampUpperOffset = 0xF1; +static constexpr int hmbTimestampOffset = 5; // 5 us offset +using NiFpga_CloseHmbFunc = NiFpga_Status (*)(const NiFpga_Session session, + const char* memoryName); +using NiFpga_OpenHmbFunc = NiFpga_Status (*)(const NiFpga_Session session, + const char* memoryName, + size_t* memorySize, + void** virtualAddress); +struct HMBHolder { + ~HMBHolder() { + if (hmb) { + closeHmb(hmb->getSystemInterface()->getHandle(), hmbName); + dlclose(niFpga); + } + } + explicit operator bool() const { return hmb != nullptr; } + void Configure() { + nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass = + nLoadOut::getTargetClass(); + int32_t status = 0; + hmb.reset(fpga::tHMB::create(&status)); + niFpga = dlopen("libNiFpga.so", RTLD_LAZY); + if (!niFpga) { + hmb = nullptr; + return; + } + NiFpga_OpenHmbFunc openHmb = reinterpret_cast( + dlsym(niFpga, "NiFpgaDll_OpenHmb")); + closeHmb = reinterpret_cast( + dlsym(niFpga, "NiFpgaDll_CloseHmb")); + if (openHmb == nullptr || closeHmb == nullptr) { + closeHmb = nullptr; + dlclose(niFpga); + hmb = nullptr; + return; + } + size_t hmbBufferSize = 0; + status = + openHmb(hmb->getSystemInterface()->getHandle(), hmbName, &hmbBufferSize, + reinterpret_cast(const_cast(&hmbBuffer))); + if (status != 0) { + closeHmb = nullptr; + dlclose(niFpga); + hmb = nullptr; + return; + } + auto cfg = hmb->readConfig(&status); + cfg.Enables_Timestamp = 1; + hmb->writeConfig(cfg, &status); + } + std::unique_ptr hmb; + void* niFpga = nullptr; + NiFpga_CloseHmbFunc closeHmb = nullptr; + volatile uint32_t* hmbBuffer = nullptr; +}; +static HMBHolder hmb; +} // namespace #endif // offset in microseconds @@ -115,11 +176,8 @@ static std::atomic 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)); + if (!hmb) { + hmb.Configure(); } #endif } @@ -131,36 +189,32 @@ void wpi::SetNowImpl(uint64_t (*func)(void)) { uint64_t wpi::Now() { #ifdef __FRC_ROBORIO__ // Same code as HAL_GetFPGATime() - if (!global) { + if (!hmb) { 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; - } + + asm("dmb"); + uint64_t upper1 = hmb.hmbBuffer[timestampUpperOffset]; + asm("dmb"); + uint32_t lower = hmb.hmbBuffer[timestampLowerOffset]; + asm("dmb"); + uint64_t upper2 = hmb.hmbBuffer[timestampUpperOffset]; + if (upper1 != upper2) { // Rolled over between the lower call, reread lower - lower = global->readLocalTime(&status); - if (status != 0) { - goto err; - } + asm("dmb"); + lower = hmb.hmbBuffer[timestampLowerOffset]; } - 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; + // 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 + hmbTimestampOffset; #else return (now_impl.load())(); #endif