Martino FerrariandClaude Opus 4.6
5562877c99
DATA packets timestamp with the raw value of the producer's high-resolution counter, and the wire never said how fast that counter runs. The hub divided by its own timer's frequency instead, which is only the same number while producer and hub share a machine — on x86 it is the TSC frequency and differs from model to model. Off-box, every accumulated batch was therefore laid out over the wrong span of time: the samples in it drift away from where they belong and start colliding with the next packet's, which is the "same" symptom as a stale time base even though nothing is out of order. CONFIG now carries the rate as a trailing uint64, alongside the publish-mode byte and read the same tolerant way: absent or zero means the producer did not say, and the hub falls back to its own timer as before. Anything below 1 kHz is not a high-resolution timer and is refused, so a mis-parsed payload cannot stretch a millisecond batch across seconds. The Accumulate DATA payload is unchanged, so this costs nothing per packet and the period *within* a batch is still estimated from the gap between packets. The Go, C and browser parsers already ignore trailer bytes they do not know, so they read the new CONFIG unchanged; none of them uses the HRT timestamp. Also corrects the Accumulate DATA layout in all three protocol documents: they described it as one snapshot per array signal, where it has always been one per accumulated cycle. Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
1096 lines
43 KiB
C++
1096 lines
43 KiB
C++
/**
|
|
* @file UDPSourceSession.cpp
|
|
* @brief UDPSourceSession implementation.
|
|
*/
|
|
|
|
#include "UDPSourceSession.h"
|
|
#include "AdvancedErrorManagement.h"
|
|
#include "ConfigurationDatabase.h"
|
|
#include "Sleep.h"
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <time.h>
|
|
|
|
namespace StreamHub {
|
|
|
|
using MARTe::UDPS_SIGNAL_DESC_SIZE;
|
|
using MARTe::UDPS_QUANT_NONE;
|
|
using MARTe::UDPS_QUANT_UINT8;
|
|
using MARTe::UDPS_QUANT_INT8;
|
|
using MARTe::UDPS_QUANT_UINT16;
|
|
using MARTe::UDPS_QUANT_INT16;
|
|
using MARTe::UDPS_TYPECODE_UINT8;
|
|
using MARTe::UDPS_TYPECODE_INT8;
|
|
using MARTe::UDPS_TYPECODE_UINT16;
|
|
using MARTe::UDPS_TYPECODE_INT16;
|
|
using MARTe::UDPS_TYPECODE_UINT32;
|
|
using MARTe::UDPS_TYPECODE_INT32;
|
|
using MARTe::UDPS_TYPECODE_UINT64;
|
|
using MARTe::UDPS_TYPECODE_INT64;
|
|
using MARTe::UDPS_TYPECODE_FLOAT32;
|
|
using MARTe::UDPS_TYPECODE_FLOAT64;
|
|
using MARTe::UDPS_TIMEMODE_FIRST_SAMPLE;
|
|
using MARTe::UDPS_TIMEMODE_LAST_SAMPLE;
|
|
using MARTe::UDPS_TIMEMODE_FULL_ARRAY;
|
|
using MARTe::UDPS_NO_TIME_SIGNAL;
|
|
using MARTe::UDPS_PUBLISH_ACCUMULATE;
|
|
using MARTe::ConfigurationDatabase;
|
|
|
|
/** Local absolute value (avoids pulling in <math.h>). */
|
|
static inline float64 Fabs(float64 x) { return (x < 0.0) ? -x : x; }
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Constructor / Destructor */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
UDPSourceSession::UDPSourceSession()
|
|
: port_(0u),
|
|
dataPort_(0u),
|
|
maxPoints_(20000u),
|
|
ringTemporal_(1000000u),
|
|
ringScalar_(100000u),
|
|
initialised_(false),
|
|
numSignals_(0u),
|
|
publishMode_(0u),
|
|
configured_(false),
|
|
statSeenFirst_(false),
|
|
statLastCounter_(0u),
|
|
statTotalRx_(0u),
|
|
statTotalLost_(0u),
|
|
ctHead_(0u),
|
|
ctFull_(false),
|
|
statLastRxTicks_(0u),
|
|
statFragCount_(0u),
|
|
statByteCount_(0u),
|
|
hrtFreq_(static_cast<float64>(MARTe::HighResolutionTimer::Frequency())),
|
|
timeScratch_(static_cast<float64 *>(0)),
|
|
valScratch_(static_cast<float64 *>(0)),
|
|
tsBatchScratch_(static_cast<float64 *>(0)),
|
|
timeScratchLen_(0u),
|
|
trigEngine_(static_cast<TriggerEngine *>(0)),
|
|
trigEpochSeen_(0xFFFFFFFFu),
|
|
trigSigIdx_(-1),
|
|
trigElemIdx_(-1),
|
|
recPendingEpoch_(0u),
|
|
recSeenEpoch_(0u) {
|
|
stateStr_ = "disconnected";
|
|
memset(&recCfg_, 0, sizeof(recCfg_));
|
|
recPendingSpec_[0] = '\0';
|
|
strncpy(recActiveSpec_, "all", sizeof(recActiveSpec_) - 1u);
|
|
recActiveSpec_[sizeof(recActiveSpec_) - 1u] = '\0';
|
|
recSpecMutex_.Create();
|
|
ResetCalibration();
|
|
}
|
|
|
|
UDPSourceSession::~UDPSourceSession() {
|
|
(void) Stop();
|
|
delete[] timeScratch_;
|
|
delete[] valScratch_;
|
|
delete[] tsBatchScratch_;
|
|
}
|
|
|
|
void UDPSourceSession::ResetCalibration() {
|
|
for (uint32 i = 0u; i < UDPSS_MAX_SIGNALS; i++) {
|
|
timeSigCalibValid_[i] = false;
|
|
timeSigCalib_[i] = 0.0;
|
|
timeSigLastValid_[i] = false;
|
|
timeSigLastTimerS_[i] = 0.0;
|
|
lastPktWallValid_[i] = false;
|
|
lastPktWallS_[i] = 0.0;
|
|
accScalarPrevN_[i] = 0u;
|
|
accScalarDtValid_[i] = false;
|
|
accScalarDtEMA_[i] = 0.0;
|
|
}
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Initialise / Start / Stop */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
bool UDPSourceSession::Initialise(const char *id, const char *label,
|
|
const char *addr, uint16 port,
|
|
uint32 maxPts,
|
|
const char *mcGroup,
|
|
uint16 dataPort) {
|
|
(void) Stop();
|
|
|
|
id_ = id;
|
|
label_ = label;
|
|
addr_ = addr;
|
|
port_ = port;
|
|
mcGroup_ = (mcGroup != static_cast<const char *>(0)) ? mcGroup : "";
|
|
dataPort_ = dataPort;
|
|
maxPoints_ = (maxPts > 0u) ? maxPts : 20000u;
|
|
|
|
/* Build ConfigurationDatabase for UDPSClient */
|
|
ConfigurationDatabase cfg;
|
|
(void) cfg.Write("ServerAddr", addr);
|
|
(void) cfg.Write("Port", static_cast<uint32>(port));
|
|
if ((mcGroup != static_cast<const char *>(0)) && (strlen(mcGroup) > 0u)) {
|
|
(void) cfg.Write("MulticastGroup", mcGroup);
|
|
if (dataPort > 0u) {
|
|
(void) cfg.Write("DataPort", static_cast<uint32>(dataPort));
|
|
}
|
|
}
|
|
|
|
if (!client_.Initialise(cfg)) {
|
|
REPORT_ERROR_STATIC(MARTe::ErrorManagement::FatalError,
|
|
"UDPSourceSession[%s]: Failed to initialise UDPSClient.", id);
|
|
return false;
|
|
}
|
|
client_.SetListener(this);
|
|
|
|
(void) metaMutex_.FastLock();
|
|
numSignals_ = 0u;
|
|
configured_ = false;
|
|
metaMutex_.FastUnLock();
|
|
|
|
initialised_ = true;
|
|
return true;
|
|
}
|
|
|
|
bool UDPSourceSession::Start() {
|
|
if (!initialised_) { return false; }
|
|
(void) statsMutex_.FastLock();
|
|
/* Reset all stat accumulators */
|
|
statSeenFirst_ = false;
|
|
statLastCounter_ = 0u;
|
|
statTotalRx_ = 0u;
|
|
statTotalLost_ = 0u;
|
|
ctHead_ = 0u;
|
|
ctFull_ = false;
|
|
statLastRxTicks_ = 0u;
|
|
statFragCount_ = 0u;
|
|
statByteCount_ = 0u;
|
|
stateStr_ = "connecting";
|
|
statsMutex_.FastUnLock();
|
|
return client_.Start();
|
|
}
|
|
|
|
bool UDPSourceSession::Stop() {
|
|
if (!initialised_) { return true; }
|
|
return client_.Stop();
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* UDPSClientListener callbacks */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
void UDPSourceSession::OnUDPSConnected() {
|
|
ResetCalibration();
|
|
(void) statsMutex_.FastLock();
|
|
stateStr_ = "connected";
|
|
statsMutex_.FastUnLock();
|
|
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
|
|
"UDPSourceSession[%s]: Connected.", id_.Buffer());
|
|
}
|
|
|
|
void UDPSourceSession::OnUDPSDisconnected() {
|
|
(void) statsMutex_.FastLock();
|
|
stateStr_ = "disconnected";
|
|
statsMutex_.FastUnLock();
|
|
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
|
|
"UDPSourceSession[%s]: Disconnected.", id_.Buffer());
|
|
}
|
|
|
|
void UDPSourceSession::OnUDPSConfig(const uint8 *payload, uint32 payloadSize) {
|
|
ParseConfigPayload(payload, payloadSize);
|
|
}
|
|
|
|
void UDPSourceSession::OnUDPSData(const uint8 *payload, uint32 payloadSize) {
|
|
/* Valid only for the duration of this callback. */
|
|
ParseDataPayload(payload, payloadSize, client_.GetLastDataGap());
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* CONFIG parsing */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
void UDPSourceSession::ParseConfigPayload(const uint8 *payload, uint32 size) {
|
|
if (size < 4u) { return; }
|
|
|
|
uint32 numSigs = 0u;
|
|
memcpy(&numSigs, payload, 4u);
|
|
if (numSigs > UDPSS_MAX_SIGNALS) { numSigs = UDPSS_MAX_SIGNALS; }
|
|
|
|
uint32 expectedSize = 4u + numSigs * UDPS_SIGNAL_DESC_SIZE + 1u;
|
|
if (size < expectedSize) { return; }
|
|
|
|
(void) metaMutex_.FastLock();
|
|
|
|
for (uint32 i = 0u; i < numSigs; i++) {
|
|
memcpy(&sigDescs_[i],
|
|
payload + 4u + i * UDPS_SIGNAL_DESC_SIZE,
|
|
UDPS_SIGNAL_DESC_SIZE);
|
|
/* MD-3: force null-termination of name/unit to prevent intra-struct OOB read */
|
|
sigDescs_[i].name[sizeof(sigDescs_[i].name) - 1u] = '\0';
|
|
sigDescs_[i].unit[sizeof(sigDescs_[i].unit) - 1u] = '\0';
|
|
}
|
|
publishMode_ = payload[4u + numSigs * UDPS_SIGNAL_DESC_SIZE];
|
|
hrtFreq_ = UDPSConfigHrtFrequency(
|
|
payload, size, numSigs,
|
|
static_cast<float64>(MARTe::HighResolutionTimer::Frequency()));
|
|
numSignals_ = numSigs;
|
|
configured_ = true;
|
|
|
|
metaMutex_.FastUnLock();
|
|
|
|
/* Config change invalidates wall-clock calibration (Go hub configSeq). */
|
|
ResetCalibration();
|
|
|
|
/* Signal list changed: force trigger-key re-resolution on the next DATA. */
|
|
trigEpochSeen_ = 0xFFFFFFFFu;
|
|
trigSigIdx_ = -1;
|
|
|
|
/* (Re)allocate the time-signal decode scratch to the largest element count. */
|
|
uint32 maxElems = 1u;
|
|
for (uint32 i = 0u; i < numSigs; i++) {
|
|
uint64 ne = static_cast<uint64>(sigDescs_[i].numRows) *
|
|
static_cast<uint64>(sigDescs_[i].numCols);
|
|
if (ne == 0u) { ne = 1u; }
|
|
if (ne > 0x100000u) { ne = 0x100000u; /* sanity cap */ }
|
|
if (ne > maxElems) { maxElems = static_cast<uint32>(ne); }
|
|
}
|
|
if (maxElems > timeScratchLen_) {
|
|
delete[] timeScratch_;
|
|
delete[] valScratch_;
|
|
delete[] tsBatchScratch_;
|
|
timeScratch_ = new float64[maxElems];
|
|
valScratch_ = new float64[maxElems];
|
|
tsBatchScratch_ = new float64[maxElems];
|
|
timeScratchLen_ = maxElems;
|
|
}
|
|
|
|
/* Allocate ring buffers (outside metaMutex to avoid long hold) */
|
|
AllocateRingBuffers();
|
|
|
|
/* (Re)configure the recorder: a CONFIG change re-headers a fresh file and
|
|
* adopts any pending subset spec. */
|
|
if (recCfg_.enabled) {
|
|
bool mask[UDPSS_MAX_SIGNALS];
|
|
(void) recSpecMutex_.FastLock();
|
|
strncpy(recActiveSpec_, recPendingSpec_, sizeof(recActiveSpec_) - 1u);
|
|
recActiveSpec_[sizeof(recActiveSpec_) - 1u] = '\0';
|
|
recSeenEpoch_ = recPendingEpoch_;
|
|
recSpecMutex_.FastUnLock();
|
|
ComputeIncludeMask(sigDescs_, numSigs, recActiveSpec_, mask);
|
|
recorder_.Configure(sigDescs_, numSigs, mask);
|
|
}
|
|
|
|
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
|
|
"UDPSourceSession[%s]: CONFIG received — %u signals, "
|
|
"producer HRT %.0f Hz.",
|
|
id_.Buffer(), numSigs, hrtFreq_);
|
|
}
|
|
|
|
void UDPSourceSession::AllocateRingBuffers() {
|
|
(void) metaMutex_.FastLock();
|
|
uint32 nSigs = numSignals_;
|
|
uint32 nElems[UDPSS_MAX_SIGNALS];
|
|
for (uint32 i = 0u; i < nSigs; i++) {
|
|
nElems[i] = sigDescs_[i].numRows * sigDescs_[i].numCols;
|
|
if (nElems[i] == 0u) { nElems[i] = 1u; }
|
|
}
|
|
metaMutex_.FastUnLock();
|
|
|
|
/* Go hub policy (hub.go): scalar signals get the small ring; any
|
|
* multi-element signal (waveform) gets the large temporal ring. */
|
|
for (uint32 i = 0u; i < nSigs; i++) {
|
|
const uint32 cap = (nElems[i] > 1u) ? ringTemporal_ : ringScalar_;
|
|
(void) rings_[i].Allocate(cap);
|
|
}
|
|
}
|
|
|
|
bool UDPSourceSession::GrowRingsForSeconds(float64 seconds, uint32 maxPts) {
|
|
if ((seconds <= 0.0) || (maxPts == 0u)) { return false; }
|
|
|
|
(void) metaMutex_.FastLock();
|
|
const uint32 nSigs = numSignals_;
|
|
metaMutex_.FastUnLock();
|
|
|
|
bool grew = false;
|
|
for (uint32 i = 0u; i < nSigs; i++) {
|
|
const uint32 count = rings_[i].Count();
|
|
const float64 span = rings_[i].TimeSpan();
|
|
/* Need a decent sample of the stream before extrapolating a rate;
|
|
* a couple of packets' worth of span is enough at any rate. */
|
|
if ((count < 2u) || (span <= 0.0)) { continue; }
|
|
|
|
const float64 rate = static_cast<float64>(count) / span;
|
|
/* 20 % headroom absorbs rate jitter and the capture margin. */
|
|
float64 need = rate * seconds * 1.2;
|
|
if (need > static_cast<float64>(maxPts)) {
|
|
need = static_cast<float64>(maxPts);
|
|
}
|
|
const uint32 needPts = static_cast<uint32>(need);
|
|
if (needPts > rings_[i].Capacity()) {
|
|
if (rings_[i].Grow(needPts)) { grew = true; }
|
|
}
|
|
}
|
|
return grew;
|
|
}
|
|
|
|
uint32 UDPSourceSession::GetMaxRingCapacity() const {
|
|
(void) metaMutex_.FastLock();
|
|
const uint32 nSigs = numSignals_;
|
|
metaMutex_.FastUnLock();
|
|
|
|
uint32 maxCap = 0u;
|
|
for (uint32 i = 0u; i < nSigs; i++) {
|
|
const uint32 c = rings_[i].Capacity();
|
|
if (c > maxCap) { maxCap = c; }
|
|
}
|
|
return maxCap;
|
|
}
|
|
|
|
float64 UDPSourceSession::ProducerNewestTime() const {
|
|
/* Mirror exactly the ParseDataPayload branches that timestamp from the
|
|
* referenced time signal; every other branch stamps on arrival and so
|
|
* would report "now" in the hub's clock, not the producer's. The time
|
|
* signal itself is one of those — it is PACKET-timed. */
|
|
(void) metaMutex_.FastLock();
|
|
const uint32 nSigs = numSignals_;
|
|
bool producerTimed[UDPSS_MAX_SIGNALS];
|
|
for (uint32 i = 0u; i < nSigs; i++) {
|
|
const UDPSSignalDescriptor &d = sigDescs_[i];
|
|
uint64 ne = static_cast<uint64>(d.numRows) *
|
|
static_cast<uint64>(d.numCols);
|
|
if (ne == 0u) { ne = 1u; }
|
|
const bool hasTimeSig = (d.timeSignalIdx != UDPS_NO_TIME_SIGNAL) &&
|
|
(d.timeSignalIdx < nSigs);
|
|
const bool isFirstLast = (ne > 1u) &&
|
|
((d.timeMode == UDPS_TIMEMODE_FIRST_SAMPLE) ||
|
|
(d.timeMode == UDPS_TIMEMODE_LAST_SAMPLE));
|
|
const bool isFullArray = (d.timeMode == UDPS_TIMEMODE_FULL_ARRAY);
|
|
producerTimed[i] = hasTimeSig && (isFullArray || isFirstLast);
|
|
}
|
|
metaMutex_.FastUnLock();
|
|
|
|
/* Signals of one source share a packet, so they advance together; the max
|
|
* is "how far this source has produced" without stalling on a signal that
|
|
* simply is not being sent. */
|
|
float64 newest = 0.0;
|
|
for (uint32 i = 0u; i < nSigs; i++) {
|
|
if (!producerTimed[i]) { continue; }
|
|
const float64 t = rings_[i].NewestTime();
|
|
if (t > newest) { newest = t; }
|
|
}
|
|
return newest;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* DATA parsing */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size,
|
|
uint32 lostPackets) {
|
|
if (size < 8u) { return; }
|
|
|
|
/* Copy metadata under lock */
|
|
(void) metaMutex_.FastLock();
|
|
uint32 nSigs = numSignals_;
|
|
uint8 pm = publishMode_;
|
|
UDPSSignalDescriptor descs[UDPSS_MAX_SIGNALS];
|
|
if (nSigs > 0u) {
|
|
memcpy(descs, sigDescs_, nSigs * sizeof(UDPSSignalDescriptor));
|
|
}
|
|
metaMutex_.FastUnLock();
|
|
|
|
if (nSigs == 0u) { return; }
|
|
|
|
uint64 hrtTimestamp = 0u;
|
|
memcpy(&hrtTimestamp, payload, 8u);
|
|
|
|
/* Re-resolve the trigger signal key when the trigger config changes. */
|
|
if (trigEngine_ != static_cast<TriggerEngine *>(0)) {
|
|
const uint32 ep = trigEngine_->GetConfigEpoch();
|
|
if (ep != trigEpochSeen_) {
|
|
trigEpochSeen_ = ep;
|
|
ResolveTriggerSignal(descs, nSigs);
|
|
}
|
|
}
|
|
|
|
/* Packet arrival wall-clock time — basis of all timestamps (Unix seconds),
|
|
* mirroring the Go hub's DataSample.WallTime semantics. */
|
|
struct timespec tsNow;
|
|
(void) clock_gettime(CLOCK_REALTIME, &tsNow);
|
|
const float64 wallNowS = static_cast<float64>(tsNow.tv_sec) +
|
|
static_cast<float64>(tsNow.tv_nsec) * 1.0e-9;
|
|
|
|
uint32 offset = 8u;
|
|
uint32 numSamples = 1u;
|
|
|
|
if (pm == UDPS_PUBLISH_ACCUMULATE) {
|
|
if (offset + 4u > size) { return; }
|
|
memcpy(&numSamples, payload + offset, 4u);
|
|
offset += 4u;
|
|
if (numSamples == 0u) { numSamples = 1u; }
|
|
}
|
|
|
|
/* Pass 1: compute each signal's payload offset + element count, bounds-checked. */
|
|
uint32 sigOff[UDPSS_MAX_SIGNALS];
|
|
uint32 sigElems[UDPSS_MAX_SIGNALS];
|
|
uint32 off = offset;
|
|
for (uint32 s = 0u; s < nSigs; s++) {
|
|
const UDPSSignalDescriptor &desc = descs[s];
|
|
/* HI-1: use 64-bit multiply to avoid overflow on attacker-controlled numRows/numCols */
|
|
uint64 numElements64 = static_cast<uint64>(desc.numRows) *
|
|
static_cast<uint64>(desc.numCols);
|
|
if (numElements64 == 0u) { numElements64 = 1u; }
|
|
if (numElements64 > 0x100000u) { return; /* sanity cap: 1M elements */ }
|
|
uint32 numElements = static_cast<uint32>(numElements64);
|
|
|
|
uint32 wireElemBytes = (desc.quantType != UDPS_QUANT_NONE)
|
|
? QuantWireBytes(desc.quantType)
|
|
: MARTe::UDPSTypeCodeByteSize(desc.typeCode);
|
|
if (wireElemBytes == 0u) { return; }
|
|
|
|
/* Accumulate mode batches one full snapshot (all elements) per RT
|
|
* cycle for every signal (scalar or array) — see UDPStreamer's
|
|
* SerializeAccumulated. HI-1: 64-bit multiply to avoid overflow on
|
|
* attacker-controlled numSamples. */
|
|
uint64 elemsToRead64 = (pm == UDPS_PUBLISH_ACCUMULATE)
|
|
? (numElements64 * static_cast<uint64>(numSamples))
|
|
: numElements64;
|
|
if (elemsToRead64 > 0x100000u) { return; /* sanity cap: 1M elements */ }
|
|
uint32 elemsToRead = static_cast<uint32>(elemsToRead64);
|
|
|
|
/* HI-1: 64-bit bounds check to prevent uint32 multiply overflow */
|
|
uint64 bytesNeeded = static_cast<uint64>(off) +
|
|
static_cast<uint64>(elemsToRead) *
|
|
static_cast<uint64>(wireElemBytes);
|
|
if (bytesNeeded > static_cast<uint64>(size)) { return; }
|
|
sigOff[s] = off;
|
|
sigElems[s] = elemsToRead;
|
|
off += static_cast<uint32>(elemsToRead * wireElemBytes);
|
|
}
|
|
|
|
/* The decode scratch is sized at CONFIG time to the largest per-signal
|
|
* element count, but an Accumulate packet writes numSamples elements per
|
|
* accumulated scalar — for a lone scalar source that count is 1, so the
|
|
* batch decode would overrun valScratch_/tsBatchScratch_ (heap corruption
|
|
* that scrambled timestamps into the value stream). Grow the scratch to
|
|
* the largest element count actually present in THIS packet before pass 2.
|
|
* Runs on the receive thread (the only writer of these buffers) and only
|
|
* when the batch first exceeds the current capacity. */
|
|
uint32 maxNeeded = 1u;
|
|
for (uint32 s = 0u; s < nSigs; s++) {
|
|
if (sigElems[s] > maxNeeded) { maxNeeded = sigElems[s]; }
|
|
}
|
|
if (maxNeeded > timeScratchLen_) {
|
|
delete[] timeScratch_;
|
|
delete[] valScratch_;
|
|
delete[] tsBatchScratch_;
|
|
timeScratch_ = new float64[maxNeeded];
|
|
valScratch_ = new float64[maxNeeded];
|
|
tsBatchScratch_ = new float64[maxNeeded];
|
|
timeScratchLen_ = maxNeeded;
|
|
}
|
|
|
|
/* Pass 2: decode values and timestamps; write ring buffers.
|
|
* Timestamp logic mirrors Go buildBinaryDataMessageForSource (hub.go). */
|
|
static const float64 kRecalibThresholdS = 2.0;
|
|
|
|
for (uint32 s = 0u; s < nSigs; s++) {
|
|
const UDPSSignalDescriptor &desc = descs[s];
|
|
uint64 ne64 = static_cast<uint64>(desc.numRows) *
|
|
static_cast<uint64>(desc.numCols);
|
|
if (ne64 == 0u) { ne64 = 1u; }
|
|
uint32 numElements = static_cast<uint32>(ne64);
|
|
const uint32 nElems = sigElems[s];
|
|
|
|
const bool isFirstLast = (numElements > 1u) &&
|
|
((desc.timeMode == UDPS_TIMEMODE_FIRST_SAMPLE) ||
|
|
(desc.timeMode == UDPS_TIMEMODE_LAST_SAMPLE));
|
|
const bool isFullArray = (desc.timeMode == UDPS_TIMEMODE_FULL_ARRAY);
|
|
|
|
/* Resolve the referenced time signal (if any). */
|
|
const bool hasTimeSig = (desc.timeSignalIdx != UDPS_NO_TIME_SIGNAL) &&
|
|
(desc.timeSignalIdx < nSigs);
|
|
const uint32 tIdx = hasTimeSig ? desc.timeSignalIdx : 0u;
|
|
const float64 timerToSec = (hasTimeSig &&
|
|
(descs[tIdx].typeCode == UDPS_TYPECODE_UINT64))
|
|
? 1.0e-9 : 1.0e-6;
|
|
|
|
if (isFirstLast) {
|
|
/* Anchor from time signal (first element) or arrival time. */
|
|
bool anchorIsFirstSample = (desc.timeMode == UDPS_TIMEMODE_FIRST_SAMPLE);
|
|
float64 anchor = wallNowS;
|
|
if (hasTimeSig && (sigElems[tIdx] >= 1u)) {
|
|
float64 tv0 = 0.0;
|
|
DecodeElems(payload, sigOff[tIdx], descs[tIdx], 1u, &tv0);
|
|
const float64 timerS = tv0 * timerToSec;
|
|
const float64 calib = CalibrateTimeSignal(tIdx, timerS, wallNowS);
|
|
anchor = calib + timerS;
|
|
} else {
|
|
anchorIsFirstSample = false;
|
|
}
|
|
/* Batch decode: compute all timestamps and values, then batch write. */
|
|
const float64 dt = (desc.samplingRate > 0.0) ? (1.0 / desc.samplingRate) : 0.0;
|
|
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
|
|
for (uint32 e = 0u; e < nElems; e++) {
|
|
tsBatchScratch_[e] = anchorIsFirstSample
|
|
? (anchor + static_cast<float64>(e) * dt)
|
|
: (anchor - static_cast<float64>(nElems - 1u - e) * dt);
|
|
}
|
|
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
|
|
}
|
|
else if (isFullArray) {
|
|
/* Per-element timestamps from the referenced time-signal array.
|
|
* Batch decode + batch write (single ring lock per signal). */
|
|
if (hasTimeSig && (sigElems[tIdx] >= nElems) &&
|
|
(nElems <= timeScratchLen_)) {
|
|
DecodeElems(payload, sigOff[tIdx], descs[tIdx], nElems, timeScratch_);
|
|
const float64 timer0S = timeScratch_[0] * timerToSec;
|
|
const float64 calib = CalibrateTimeSignal(tIdx, timer0S, wallNowS);
|
|
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
|
|
for (uint32 e = 0u; e < nElems; e++) {
|
|
tsBatchScratch_[e] = calib + timeScratch_[e] * timerToSec;
|
|
}
|
|
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
|
|
} else {
|
|
/* No time signal: all samples get arrival wall time. */
|
|
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
|
|
for (uint32 e = 0u; e < nElems; e++) {
|
|
tsBatchScratch_[e] = wallNowS;
|
|
}
|
|
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
|
|
}
|
|
}
|
|
else if (numElements == 1u) {
|
|
if (nElems <= 1u) {
|
|
/* Plain scalar: arrival wall time (Go n==1 case). */
|
|
const float64 val = DecodeOneElem(payload, sigOff[s], desc, 0u);
|
|
WriteSample(s, 0u, wallNowS, val);
|
|
}
|
|
else {
|
|
/* Accumulated scalar: reconstruct per-sample timestamps from
|
|
* the packet's embedded sender HRT (the acquisition time of the
|
|
* oldest sample), NOT the packet arrival time. The kernel
|
|
* frequently delivers several queued datagrams in one burst, so
|
|
* two packets can be processed microseconds apart even though
|
|
* each represents ~10 ms of signal; arrival-time interpolation
|
|
* then crams a packet's samples into that tiny gap, rendering
|
|
* the trace as a sawtooth ("chainsaw"). The sender HRT is
|
|
* immune to this because it is sampled at acquisition.
|
|
*
|
|
* hrtTimestamp is the HRT counter of sample 0; hrtFreq_ (the
|
|
* producer's tick rate, taken from the CONFIG trailer) converts
|
|
* it to seconds, then a one-time calibration maps the sender
|
|
* clock onto wall-clock. */
|
|
const float64 hrt0Sec = static_cast<float64>(hrtTimestamp) /
|
|
hrtFreq_;
|
|
if ((!timeSigCalibValid_[s]) ||
|
|
(Fabs((timeSigCalib_[s] + hrt0Sec) - wallNowS) >
|
|
kRecalibThresholdS)) {
|
|
timeSigCalib_[s] = wallNowS - hrt0Sec;
|
|
timeSigCalibValid_[s] = true;
|
|
}
|
|
|
|
/* Per-sample dt: samplingRate if present, else derive it from
|
|
* the sender-HRT gap to the previous packet.
|
|
*
|
|
* The gap is divided by the number of RT cycles it actually
|
|
* spans, not by the previous packet's sample count. Those two
|
|
* agree only while nothing is lost; once a packet goes missing
|
|
* the gap covers cycles the previous count never saw, and
|
|
* dividing by that count inflates dt until this packet's
|
|
* samples overrun into the next packet's range. lostPackets
|
|
* comes from the producer's packet counter, so the divisor
|
|
* widens with the gap and dt is unchanged. */
|
|
float64 dt;
|
|
if (desc.samplingRate > 0.0) {
|
|
dt = 1.0 / desc.samplingRate;
|
|
} else if (lastPktWallValid_[s] && (accScalarPrevN_[s] > 0u) &&
|
|
(hrt0Sec > lastPktWallS_[s])) {
|
|
dt = UDPSEstimateAccumDt(hrt0Sec - lastPktWallS_[s],
|
|
accScalarPrevN_[s], lostPackets,
|
|
accScalarDtEMA_[s],
|
|
accScalarDtValid_[s]);
|
|
} else {
|
|
dt = 1.0e-3; /* 1 kHz default until the gap is known */
|
|
}
|
|
|
|
const float64 base = timeSigCalib_[s] + hrt0Sec;
|
|
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
|
|
for (uint32 e = 0u; e < nElems; e++) {
|
|
tsBatchScratch_[e] = base + static_cast<float64>(e) * dt;
|
|
}
|
|
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
|
|
|
|
lastPktWallS_[s] = hrt0Sec; /* sender seconds for next dt */
|
|
lastPktWallValid_[s] = true;
|
|
accScalarPrevN_[s] = nElems;
|
|
}
|
|
}
|
|
else {
|
|
/* Packed TIMEMODE_PACKET array: interpolate per-element timestamps
|
|
* from the inter-packet wall-clock gap (Go default case). The very
|
|
* first packet is skipped to avoid poisoning the ring with
|
|
* wrongly-spaced timestamps.
|
|
*
|
|
* Unlike the Go hub (which extrapolates forward from arrival time
|
|
* and overlaps the next packet under jitter), elements span
|
|
* (lastPktWall, wallNow]: the samples were acquired before the
|
|
* packet arrived, and this keeps ring time strictly monotonic. */
|
|
if (lastPktWallValid_[s] && (wallNowS >= lastPktWallS_[s])) {
|
|
float64 dt;
|
|
float64 base;
|
|
if (wallNowS > lastPktWallS_[s]) {
|
|
dt = (wallNowS - lastPktWallS_[s]) /
|
|
static_cast<float64>(nElems);
|
|
base = lastPktWallS_[s];
|
|
} else {
|
|
dt = (desc.samplingRate > 0.0)
|
|
? (1.0 / desc.samplingRate) : 1.0e-6;
|
|
base = lastPktWallS_[s] - static_cast<float64>(nElems) * dt;
|
|
}
|
|
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
|
|
for (uint32 e = 0u; e < nElems; e++) {
|
|
tsBatchScratch_[e] = base + static_cast<float64>(e + 1u) * dt;
|
|
}
|
|
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
|
|
}
|
|
lastPktWallS_[s] = wallNowS;
|
|
lastPktWallValid_[s] = true;
|
|
}
|
|
}
|
|
|
|
/* Binary recorder: adopt any pending subset spec, then capture the packet
|
|
* verbatim (native types, lossless) for disk recording. */
|
|
if (recCfg_.enabled) {
|
|
if (recPendingEpoch_ != recSeenEpoch_) {
|
|
bool mask[UDPSS_MAX_SIGNALS];
|
|
(void) recSpecMutex_.FastLock();
|
|
strncpy(recActiveSpec_, recPendingSpec_, sizeof(recActiveSpec_) - 1u);
|
|
recActiveSpec_[sizeof(recActiveSpec_) - 1u] = '\0';
|
|
recSeenEpoch_ = recPendingEpoch_;
|
|
recSpecMutex_.FastUnLock();
|
|
ComputeIncludeMask(descs, nSigs, recActiveSpec_, mask);
|
|
recorder_.Configure(descs, nSigs, mask);
|
|
}
|
|
recorder_.CapturePacket(payload, sigOff, sigElems, pm, numSamples);
|
|
}
|
|
}
|
|
|
|
float64 UDPSourceSession::DecodeOneElem(const uint8 *payload, uint32 off,
|
|
const UDPSSignalDescriptor &desc,
|
|
uint32 e) const {
|
|
const uint32 wireElemBytes = (desc.quantType != UDPS_QUANT_NONE)
|
|
? QuantWireBytes(desc.quantType)
|
|
: MARTe::UDPSTypeCodeByteSize(desc.typeCode);
|
|
const uint8 *ptr = payload + off + e * wireElemBytes;
|
|
if (desc.quantType != UDPS_QUANT_NONE) {
|
|
const float64 raw = DecodeRawValue(ptr,
|
|
(desc.quantType == UDPS_QUANT_UINT8) ? UDPS_TYPECODE_UINT8 :
|
|
(desc.quantType == UDPS_QUANT_INT8) ? UDPS_TYPECODE_INT8 :
|
|
(desc.quantType == UDPS_QUANT_UINT16) ? UDPS_TYPECODE_UINT16 :
|
|
UDPS_TYPECODE_INT16);
|
|
return DequantizeValue(raw, desc.quantType, desc.rangeMin, desc.rangeMax, false);
|
|
}
|
|
return DecodeRawValue(ptr, desc.typeCode);
|
|
}
|
|
|
|
void UDPSourceSession::DecodeElems(const uint8 *payload, uint32 off,
|
|
const UDPSSignalDescriptor &desc,
|
|
uint32 nElems, float64 *out) const {
|
|
for (uint32 e = 0u; e < nElems; e++) {
|
|
out[e] = DecodeOneElem(payload, off, desc, e);
|
|
}
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Value decode helpers */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
float64 UDPSourceSession::DecodeRawValue(const uint8 *ptr, uint8 typeCode) const {
|
|
switch (typeCode) {
|
|
case UDPS_TYPECODE_UINT8: {
|
|
uint8 v = 0u; memcpy(&v, ptr, 1u); return static_cast<float64>(v);
|
|
}
|
|
case UDPS_TYPECODE_INT8: {
|
|
MARTe::int8 v = 0; memcpy(&v, ptr, 1u); return static_cast<float64>(v);
|
|
}
|
|
case UDPS_TYPECODE_UINT16: {
|
|
uint16 v = 0u; memcpy(&v, ptr, 2u); return static_cast<float64>(v);
|
|
}
|
|
case UDPS_TYPECODE_INT16: {
|
|
MARTe::int16 v = 0; memcpy(&v, ptr, 2u); return static_cast<float64>(v);
|
|
}
|
|
case UDPS_TYPECODE_UINT32: {
|
|
uint32 v = 0u; memcpy(&v, ptr, 4u); return static_cast<float64>(v);
|
|
}
|
|
case UDPS_TYPECODE_INT32: {
|
|
MARTe::int32 v = 0; memcpy(&v, ptr, 4u); return static_cast<float64>(v);
|
|
}
|
|
case UDPS_TYPECODE_UINT64: {
|
|
uint64 v = 0u; memcpy(&v, ptr, 8u); return static_cast<float64>(v);
|
|
}
|
|
case UDPS_TYPECODE_INT64: {
|
|
MARTe::int64 v = 0; memcpy(&v, ptr, 8u); return static_cast<float64>(v);
|
|
}
|
|
case UDPS_TYPECODE_FLOAT32: {
|
|
float v = 0.0f; memcpy(&v, ptr, 4u); return static_cast<float64>(v);
|
|
}
|
|
case UDPS_TYPECODE_FLOAT64: {
|
|
float64 v = 0.0; memcpy(&v, ptr, 8u); return v;
|
|
}
|
|
default: return 0.0;
|
|
}
|
|
}
|
|
|
|
float64 UDPSourceSession::DequantizeValue(float64 rawOrQuant, uint8 quantType,
|
|
float64 rangeMin, float64 rangeMax,
|
|
bool /*isRaw*/) const {
|
|
float64 range = rangeMax - rangeMin;
|
|
switch (quantType) {
|
|
case UDPS_QUANT_UINT8:
|
|
return rangeMin + (rawOrQuant / 255.0) * range;
|
|
case UDPS_QUANT_INT8:
|
|
return rangeMin + ((rawOrQuant + 127.0) / 254.0) * range;
|
|
case UDPS_QUANT_UINT16:
|
|
return rangeMin + (rawOrQuant / 65535.0) * range;
|
|
case UDPS_QUANT_INT16:
|
|
return rangeMin + ((rawOrQuant + 32767.0) / 65534.0) * range;
|
|
default:
|
|
return rawOrQuant;
|
|
}
|
|
}
|
|
|
|
uint32 UDPSourceSession::QuantWireBytes(uint8 quantType) const {
|
|
switch (quantType) {
|
|
case UDPS_QUANT_UINT8: return 1u;
|
|
case UDPS_QUANT_INT8: return 1u;
|
|
case UDPS_QUANT_UINT16: return 2u;
|
|
case UDPS_QUANT_INT16: return 2u;
|
|
default: return 0u;
|
|
}
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Statistics (port of Go SourceStat.RecordFragment, stats.go) */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
void UDPSourceSession::OnUDPSFragment(uint32 counter, uint32 nBytes,
|
|
bool complete) {
|
|
const uint64 nowTicks = MARTe::HighResolutionTimer::Counter();
|
|
|
|
(void) statsMutex_.FastLock();
|
|
|
|
statFragCount_++;
|
|
statByteCount_ += nBytes;
|
|
|
|
if (!complete) {
|
|
statsMutex_.FastUnLock();
|
|
return;
|
|
}
|
|
|
|
statTotalRx_++;
|
|
if (statSeenFirst_) {
|
|
const uint32 delta = counter - statLastCounter_; /* wraps correctly */
|
|
if (delta > 1u) {
|
|
statTotalLost_ += static_cast<uint64>(delta - 1u);
|
|
}
|
|
} else {
|
|
statSeenFirst_ = true;
|
|
}
|
|
statLastCounter_ = counter;
|
|
|
|
if (statLastRxTicks_ != 0u) {
|
|
const float64 hrtFreq =
|
|
static_cast<float64>(MARTe::HighResolutionTimer::Frequency());
|
|
const uint32 idx = ctHead_;
|
|
ctRing_[idx] = static_cast<float64>(nowTicks - statLastRxTicks_) / hrtFreq;
|
|
fragRing_[idx] = statFragCount_;
|
|
byteRing_[idx] = statByteCount_;
|
|
ctHead_ = (ctHead_ + 1u) % kStatRingSize;
|
|
if (ctHead_ == 0u) { ctFull_ = true; }
|
|
}
|
|
statLastRxTicks_ = nowTicks;
|
|
statFragCount_ = 0u;
|
|
statByteCount_ = 0u;
|
|
|
|
statsMutex_.FastUnLock();
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Thread-safe read accessors */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
uint32 UDPSourceSession::GetNumSignals() const {
|
|
(void) metaMutex_.FastLock();
|
|
uint32 n = numSignals_;
|
|
metaMutex_.FastUnLock();
|
|
return n;
|
|
}
|
|
|
|
bool UDPSourceSession::GetSignalDescriptor(uint32 idx, UDPSSignalDescriptor &desc) const {
|
|
(void) metaMutex_.FastLock();
|
|
bool ok = (configured_ && idx < numSignals_);
|
|
if (ok) { memcpy(&desc, &sigDescs_[idx], sizeof(UDPSSignalDescriptor)); }
|
|
metaMutex_.FastUnLock();
|
|
return ok;
|
|
}
|
|
|
|
uint8 UDPSourceSession::GetPublishMode() const {
|
|
(void) metaMutex_.FastLock();
|
|
uint8 pm = publishMode_;
|
|
metaMutex_.FastUnLock();
|
|
return pm;
|
|
}
|
|
|
|
uint32 UDPSourceSession::ReadSignalLast(uint32 idx, uint32 n,
|
|
float64 *tOut, float64 *vOut) const {
|
|
if (idx >= UDPSS_MAX_SIGNALS) { return 0u; }
|
|
return rings_[idx].Read(tOut, vOut, n);
|
|
}
|
|
|
|
uint32 UDPSourceSession::ReadSignalSince(uint32 idx, MARTe::uint64 &cursor,
|
|
float64 *tOut, float64 *vOut,
|
|
uint32 maxOut) const {
|
|
if (idx >= UDPSS_MAX_SIGNALS) { return 0u; }
|
|
return rings_[idx].ReadSince(cursor, tOut, vOut, maxOut);
|
|
}
|
|
|
|
uint32 UDPSourceSession::ReadSignalRange(uint32 idx, float64 t0, float64 t1,
|
|
float64 *tOut, float64 *vOut,
|
|
uint32 maxOut) const {
|
|
if (idx >= UDPSS_MAX_SIGNALS) { return 0u; }
|
|
return rings_[idx].ReadRange(t0, t1, tOut, vOut, maxOut);
|
|
}
|
|
|
|
UDPSourceStats UDPSourceSession::GetStats() const {
|
|
/* Port of Go SourceStat.Snapshot (stats.go). */
|
|
UDPSourceStats si;
|
|
|
|
(void) statsMutex_.FastLock();
|
|
|
|
si.state = stateStr_;
|
|
si.totalReceived = statTotalRx_;
|
|
si.totalLost = statTotalLost_;
|
|
|
|
const uint32 n = ctFull_ ? kStatRingSize : ctHead_;
|
|
if (n == 0u) {
|
|
statsMutex_.FastUnLock();
|
|
return si;
|
|
}
|
|
|
|
float64 sum = 0.0;
|
|
float64 sumSq = 0.0;
|
|
float64 minV = 1.0e308;
|
|
float64 maxV = 0.0;
|
|
uint64 fragSum = 0u;
|
|
uint64 byteSum = 0u;
|
|
for (uint32 i = 0u; i < n; i++) {
|
|
const float64 v = ctRing_[i];
|
|
sum += v;
|
|
sumSq += v * v;
|
|
if (v < minV) { minV = v; }
|
|
if (v > maxV) { maxV = v; }
|
|
fragSum += fragRing_[i];
|
|
byteSum += byteRing_[i];
|
|
}
|
|
const float64 fn = static_cast<float64>(n);
|
|
const float64 avg = sum / fn;
|
|
float64 variance = (sumSq / fn) - (avg * avg);
|
|
if (variance < 0.0) { variance = 0.0; }
|
|
/* sqrt without <math.h>: Newton iterations (variance ≥ 0) */
|
|
float64 stdv = variance;
|
|
if (stdv > 0.0) {
|
|
float64 x = variance;
|
|
for (uint32 it = 0u; it < 32u; it++) {
|
|
x = 0.5 * (x + variance / x);
|
|
}
|
|
stdv = x;
|
|
}
|
|
|
|
si.cycleAvgMs = avg * 1.0e3;
|
|
si.cycleStdMs = stdv * 1.0e3;
|
|
si.cycleMinMs = minV * 1.0e3;
|
|
si.cycleMaxMs = maxV * 1.0e3;
|
|
si.rateHz = (avg > 0.0) ? (1.0 / avg) : 0.0;
|
|
si.rateStdHz = (avg > 0.0) ? (stdv / (avg * avg)) : 0.0;
|
|
si.fragsPerCycle = static_cast<float64>(fragSum) / fn;
|
|
si.bytesPerCycle = static_cast<float64>(byteSum) / fn;
|
|
|
|
/* 20-bin histogram over [minV, maxV] */
|
|
si.cycleHistMin = minV * 1.0e3;
|
|
si.cycleHistMax = maxV * 1.0e3;
|
|
si.histValid = true;
|
|
const float64 span = maxV - minV;
|
|
for (uint32 i = 0u; i < n; i++) {
|
|
uint32 bin;
|
|
if (span > 0.0) {
|
|
bin = static_cast<uint32>(((ctRing_[i] - minV) / span) *
|
|
static_cast<float64>(UDPS_STAT_HIST_BINS));
|
|
if (bin >= UDPS_STAT_HIST_BINS) { bin = UDPS_STAT_HIST_BINS - 1u; }
|
|
} else {
|
|
bin = UDPS_STAT_HIST_BINS / 2u;
|
|
}
|
|
si.cycleHist[bin]++;
|
|
}
|
|
|
|
statsMutex_.FastUnLock();
|
|
return si;
|
|
}
|
|
|
|
StreamString UDPSourceSession::GetId() const { return id_; }
|
|
|
|
StreamString UDPSourceSession::GetLabel() const { return label_; }
|
|
|
|
StreamString UDPSourceSession::GetAddr() const { return addr_; }
|
|
|
|
uint16 UDPSourceSession::GetPort() const { return port_; }
|
|
|
|
StreamString UDPSourceSession::GetMulticastGroup() const { return mcGroup_; }
|
|
|
|
uint16 UDPSourceSession::GetDataPort() const { return dataPort_; }
|
|
|
|
bool UDPSourceSession::IsConfigured() const {
|
|
(void) metaMutex_.FastLock();
|
|
bool c = configured_;
|
|
metaMutex_.FastUnLock();
|
|
return c;
|
|
}
|
|
|
|
bool UDPSourceSession::IsInitialised() const { return initialised_; }
|
|
|
|
bool UDPSourceSession::SetMaxPoints(uint32 maxPts) {
|
|
if (maxPts == 0u) { return false; }
|
|
maxPoints_ = maxPts;
|
|
return true;
|
|
}
|
|
|
|
void UDPSourceSession::SetRingCapacities(uint32 temporal, uint32 scalar) {
|
|
if (temporal > 0u) { ringTemporal_ = temporal; }
|
|
if (scalar > 0u) { ringScalar_ = scalar; }
|
|
}
|
|
|
|
void UDPSourceSession::SetTriggerEngine(TriggerEngine *engine) {
|
|
trigEngine_ = engine;
|
|
trigEpochSeen_ = 0xFFFFFFFFu;
|
|
trigSigIdx_ = -1;
|
|
trigElemIdx_ = -1;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Binary recorder control */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
void UDPSourceSession::SetRecorderConfig(const RecorderConfig &cfg) {
|
|
recCfg_ = cfg;
|
|
if (!cfg.enabled) { return; }
|
|
recorder_.Init(cfg, id_.Buffer());
|
|
(void) recSpecMutex_.FastLock();
|
|
strncpy(recPendingSpec_, cfg.signals, sizeof(recPendingSpec_) - 1u);
|
|
recPendingSpec_[sizeof(recPendingSpec_) - 1u] = '\0';
|
|
strncpy(recActiveSpec_, cfg.signals, sizeof(recActiveSpec_) - 1u);
|
|
recActiveSpec_[sizeof(recActiveSpec_) - 1u] = '\0';
|
|
recPendingEpoch_++;
|
|
recSpecMutex_.FastUnLock();
|
|
}
|
|
|
|
void UDPSourceSession::SetRecorderSignals(const char *spec) {
|
|
if (spec == static_cast<const char *>(0)) { return; }
|
|
(void) recSpecMutex_.FastLock();
|
|
strncpy(recPendingSpec_, spec, sizeof(recPendingSpec_) - 1u);
|
|
recPendingSpec_[sizeof(recPendingSpec_) - 1u] = '\0';
|
|
recPendingEpoch_++;
|
|
recSpecMutex_.FastUnLock();
|
|
}
|
|
|
|
void UDPSourceSession::RequestRecArm() {
|
|
if (recCfg_.enabled) { recorder_.RequestArm(); }
|
|
}
|
|
|
|
void UDPSourceSession::RequestRecDisarm() {
|
|
if (recCfg_.enabled) { recorder_.RequestDisarm(); }
|
|
}
|
|
|
|
void UDPSourceSession::RecorderFlushTick(uint32 nowSec) {
|
|
if (recCfg_.enabled) { recorder_.FlushTick(nowSec); }
|
|
}
|
|
|
|
void UDPSourceSession::GetRecorderInfo(bool &recording, char *file, uint32 fileSz,
|
|
uint64 &bytesWritten, uint64 &rowsWritten,
|
|
uint64 &droppedRows, uint64 &freeMB) const {
|
|
recorder_.GetInfo(recording, file, fileSz, bytesWritten, rowsWritten,
|
|
droppedRows, freeMB);
|
|
}
|
|
|
|
void UDPSourceSession::ComputeIncludeMask(const UDPSSignalDescriptor *descs,
|
|
uint32 n, const char *spec,
|
|
bool *maskOut) {
|
|
if ((spec == static_cast<const char *>(0)) || (spec[0] == '\0') ||
|
|
(strcmp(spec, "all") == 0)) {
|
|
for (uint32 i = 0u; i < n; i++) { maskOut[i] = true; }
|
|
return;
|
|
}
|
|
for (uint32 i = 0u; i < n; i++) {
|
|
char key[160];
|
|
(void) snprintf(key, sizeof(key), "%s:%s", id_.Buffer(), descs[i].name);
|
|
const size_t klen = strlen(key);
|
|
maskOut[i] = false;
|
|
const char *p = spec;
|
|
while (*p != '\0') {
|
|
while ((*p == ',') || (*p == ' ')) { p++; }
|
|
const char *start = p;
|
|
while ((*p != '\0') && (*p != ',')) { p++; }
|
|
size_t tlen = static_cast<size_t>(p - start);
|
|
while ((tlen > 0u) && (start[tlen - 1u] == ' ')) { tlen--; }
|
|
if ((tlen == klen) && (strncmp(start, key, klen) == 0)) {
|
|
maskOut[i] = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void UDPSourceSession::ResolveTriggerSignal(const UDPSSignalDescriptor *descs,
|
|
uint32 nSigs) {
|
|
trigSigIdx_ = -1;
|
|
trigElemIdx_ = -1;
|
|
if (trigEngine_ == static_cast<TriggerEngine *>(0)) { return; }
|
|
|
|
TriggerConfig cfg = trigEngine_->GetConfig();
|
|
const char *key = cfg.signalKey.Buffer();
|
|
if ((key == static_cast<const char *>(0)) || (key[0] == '\0')) { return; }
|
|
|
|
/* Key format: "src:sig" or "src:sig[i]" */
|
|
const char *colon = strchr(key, ':');
|
|
if (colon == static_cast<const char *>(0)) { return; }
|
|
|
|
/* Source id must match this session */
|
|
const uint32 srcLen = static_cast<uint32>(colon - key);
|
|
const char *myId = id_.Buffer();
|
|
if ((strlen(myId) != srcLen) || (strncmp(myId, key, srcLen) != 0)) {
|
|
return;
|
|
}
|
|
|
|
/* Split optional "[i]" element suffix */
|
|
char sigName[128];
|
|
strncpy(sigName, colon + 1, sizeof(sigName) - 1u);
|
|
sigName[sizeof(sigName) - 1u] = '\0';
|
|
MARTe::int32 elemIdx = -1;
|
|
char *bracket = strchr(sigName, '[');
|
|
if (bracket != static_cast<char *>(0)) {
|
|
elemIdx = static_cast<MARTe::int32>(strtol(bracket + 1,
|
|
static_cast<char **>(0), 10));
|
|
*bracket = '\0';
|
|
}
|
|
|
|
for (uint32 s = 0u; s < nSigs; s++) {
|
|
if (strcmp(descs[s].name, sigName) == 0) {
|
|
trigSigIdx_ = static_cast<MARTe::int32>(s);
|
|
trigElemIdx_ = elemIdx;
|
|
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
|
|
"UDPSourceSession[%s]: trigger watching signal '%s' (idx %u, elem %d).",
|
|
id_.Buffer(), sigName, s, elemIdx);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
} /* namespace StreamHub */
|