From 3ad5d2e42d7262baf7ebac0d0f5587c744517edc Mon Sep 17 00:00:00 2001 From: Thad House Date: Thu, 3 Aug 2023 23:46:55 -0700 Subject: [PATCH] [hal,wpiutil] Use HMB for FPGA Timestamps (#5499) Current timestamp read code uses FPGA register reads. Through testing, this read was slower then clock_gettime by about 4-5x. However, another method of reading the FPGA time is available, using HMB. HMB is memory mapped IO from RAM to the FPGA. So to code side, reading the value is just a memory barrier and a memory read. There is some latency on the write side, so a very small artifical delay (5us) is added to avoid register reads such as interrupts being ahead of current timestamps, which could cause issues. Below is read times for 1000 calls to clock_gettime, register reads and hmb reads. ``` Clock: Rise 1.72939400 s Fall 1.72990700 s Delta 0.00051300 s FPGA : Rise 1.72999000 s Fall 1.73429300 s Delta 0.00430300 s HMB : Rise 1.73466800 s Fall 1.73481900 s Delta 0.00015100 s ``` Also add full HMB struct to HAL for future usage. --- hal/src/main/native/athena/HAL.cpp | 41 +++++-- hal/src/main/native/athena/HMB.cpp | 82 +++++++++++++ hal/src/main/native/include/hal/roborio/HMB.h | 112 ++++++++++++++++++ wpiutil/src/main/native/cpp/timestamp.cpp | 110 ++++++++++++----- 4 files changed, 304 insertions(+), 41 deletions(-) create mode 100644 hal/src/main/native/athena/HMB.cpp create mode 100644 hal/src/main/native/include/hal/roborio/HMB.h 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