Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal,wpiutil] Use HMB for FPGA Timestamps (#5499)

Browse Source 
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.
This commit is contained in:
Thad House
2023-08-03 23:46:55 -07:00
committed by GitHub
parent b46a872494
commit 3ad5d2e42d
4 changed files with 304 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
wpiutil/src/main/native/cpp/timestamp.cpp
View File

@@ -13,7 +13,7 @@
#pragma GCC diagnostic ignored "-Wignored-qualifiers"
#include <FRC_FPGA_ChipObject/RoboRIO_FRC_ChipObject_Aliases.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/nInterfaceGlobals.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tGlobal.h>
#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tHMB.h>
#include <FRC_NetworkCommunication/LoadOut.h>
#pragma GCC diagnostic pop
namespace fpga {
@@ -21,6 +21,8 @@ using namespace nFPGA;
using namespace nRoboRIO_FPGANamespace;
} // namespace fpga
#include <memory>
#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<fpga::tGlobal> 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<NiFpga_OpenHmbFunc>(
dlsym(niFpga, "NiFpgaDll_OpenHmb"));
closeHmb = reinterpret_cast<NiFpga_CloseHmbFunc>(
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<void**>(const_cast<uint32_t**>(&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<fpga::tHMB> 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<uint64_t (*)()> 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