Martino FerrariandClaude Opus 4.6
deabd257e5
Reported as samples sporadically carrying a previous packet's timestamp: holes on one side of the stream and collisions on the other, in both the Go and the MARTe2 receiver. That it appeared in both is what located it -- the shared cause is upstream of either client. Four independent defects, all of which end in a packet's values being placed at a time that is not theirs. Reassembly slot exhaustion (the "Reassembly slots full; evicting oldest" flood). Chunk size was learnt only from fragment 0, so an out-of-order burst destroyed a packet whose bytes had all arrived and left the slot occupied until the 2 s GC. Slots were keyed on the counter alone, but DATA and CONFIG number independently, so equal counters merged the two streams. The 32-byte received-mask covered 256 of the 512 fragments the client accepts, so a duplicate above 255 was counted as new and the packet was delivered with a hole of stale bytes in it. And one datagram was read per Execute(), which cannot drain a fast producer. Fixed with a pendingTail deferral, (counter, type) keying, a 64-byte mask, a 256-datagram drain, counter-age slot reclamation, and a 1 Hz aggregated warning in place of the per-eviction flood. UDPStreamer dropping whole Accumulate batches. EventSem::ResetWait is Reset-then-Wait, so a Post() landing while the sender thread was inside ServiceClients()/SendData() was destroyed by the next Reset. The batch was then skipped with dataReady false, readyFill was never cleared, and the following flush overwrote it: an entire run of RT cycles never reached the wire. The record of pending work now lives in the buffers rather than in the semaphore edge, which also removes up to UDPS_DATA_WAIT_MS of latency; genuine backpressure overwrites are counted and reported. Against the unfixed code the new test sees 2999/3000 batches never consumed. Period inflation after loss. Accumulated scalars carry no SamplingRate, so the receiver derives dt from the sender-clock gap -- but dividing it by the previous packet's sample count is only right while nothing is lost. One loss doubles the reported period, which spreads a batch a full batch past its own end and into the range the next packet claims. That is the hole and the collision, exactly. Inferring the cycle count from the estimate's own period is not a way out: it has a stable fixed point wherever gap/dt is an integer, so a real rate change locks it at the old one for good (AccumDtGTest.FollowsSustainedRateChange). The packet counter removes the ambiguity, so all three receivers now order on it: a DATA packet that does not advance the counter is dropped rather than delivered, because its values are older than data already handed over. Ordering is on the signed difference so it survives the uint32 wrap, and the sequence resets on reconnect, where the producer's counter restarts independently of ours. The loss count that falls out of the same delta feeds the period estimate as cycles = prevN * (1 + lost), which reduces exactly to gap/prevN when nothing is lost and therefore still tracks a genuine rate change. UDPSClient::AcceptDataCounter (C++), udpsprotocol.SequenceGate (Go), decode_data (C). The C client's existing gap counter was wrap-unsafe and let a stale packet rewind last_counter, which made every subsequent gap wrong; it uses the same code now. Docs/Protocol.md gains an Ordering DATA section stating the requirement for any receiver, including ones outside this repository. Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
1092 lines
42 KiB
C++
1092 lines
42 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];
|
|
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.",
|
|
id_.Buffer(), numSigs);
|
|
}
|
|
|
|
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
|
|
* local HRT frequency, identical to the sender on the same
|
|
* host) 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 */
|