Implemented new C++ logic
This commit is contained in:
Martino Ferrari
@@ -0,0 +1,54 @@
|
||||
/**
|
||||
* @file Base64.h
|
||||
* @brief Inline Base64 encoder.
|
||||
*
|
||||
* Used exclusively for the WebSocket HTTP upgrade handshake.
|
||||
*/
|
||||
|
||||
#ifndef STREAMHUB_BASE64_H_
|
||||
#define STREAMHUB_BASE64_H_
|
||||
|
||||
#include "CompilerTypes.h"
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::uint8;
|
||||
using MARTe::uint32;
|
||||
|
||||
static const char kBase64Chars[] =
|
||||
"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
|
||||
|
||||
/**
|
||||
* @brief Base64-encode @p inLen bytes from @p in into @p out (null-terminated).
|
||||
* @param out Must have room for at least ceil(inLen/3)*4 + 1 bytes.
|
||||
* @return Number of characters written (excluding null terminator).
|
||||
*/
|
||||
inline uint32 Base64Encode(const uint8 *in, uint32 inLen, char *out) {
|
||||
uint32 outIdx = 0u;
|
||||
uint32 i = 0u;
|
||||
while (i + 3u <= inLen) {
|
||||
out[outIdx++] = kBase64Chars[(in[i] >> 2u) & 0x3Fu];
|
||||
out[outIdx++] = kBase64Chars[((in[i] & 0x03u) << 4u) | ((in[i+1u] >> 4u) & 0x0Fu)];
|
||||
out[outIdx++] = kBase64Chars[((in[i+1u] & 0x0Fu) << 2u) | ((in[i+2u] >> 6u) & 0x03u)];
|
||||
out[outIdx++] = kBase64Chars[in[i+2u] & 0x3Fu];
|
||||
i += 3u;
|
||||
}
|
||||
uint32 rem = inLen - i;
|
||||
if (rem == 1u) {
|
||||
out[outIdx++] = kBase64Chars[(in[i] >> 2u) & 0x3Fu];
|
||||
out[outIdx++] = kBase64Chars[(in[i] & 0x03u) << 4u];
|
||||
out[outIdx++] = '=';
|
||||
out[outIdx++] = '=';
|
||||
} else if (rem == 2u) {
|
||||
out[outIdx++] = kBase64Chars[(in[i] >> 2u) & 0x3Fu];
|
||||
out[outIdx++] = kBase64Chars[((in[i] & 0x03u) << 4u) | ((in[i+1u] >> 4u) & 0x0Fu)];
|
||||
out[outIdx++] = kBase64Chars[(in[i+1u] & 0x0Fu) << 2u];
|
||||
out[outIdx++] = '=';
|
||||
}
|
||||
out[outIdx] = '\0';
|
||||
return outIdx;
|
||||
}
|
||||
|
||||
} /* namespace StreamHub */
|
||||
|
||||
#endif /* STREAMHUB_BASE64_H_ */
|
||||
@@ -0,0 +1,117 @@
|
||||
/**
|
||||
* @file LTTB.h
|
||||
* @brief Largest Triangle Three Buckets (LTTB) time-series decimation.
|
||||
*
|
||||
* Direct C++ translation of the Go lttbDecimate function in wshub/hub.go.
|
||||
* Preserves visual fidelity when reducing a large time-series to a smaller
|
||||
* number of representative points.
|
||||
*
|
||||
* No external dependencies (uses only <math.h> for fabs).
|
||||
*/
|
||||
|
||||
#ifndef STREAMHUB_LTTB_H_
|
||||
#define STREAMHUB_LTTB_H_
|
||||
|
||||
#include "CompilerTypes.h"
|
||||
#include <math.h>
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::uint32;
|
||||
using MARTe::float64;
|
||||
|
||||
/**
|
||||
* @brief Decimate time-series (tIn, vIn) of length nIn to at most threshold points.
|
||||
*
|
||||
* @param tIn Input time array (float64, monotonically increasing).
|
||||
* @param vIn Input value array (float64), same length as tIn.
|
||||
* @param nIn Number of input points.
|
||||
* @param tOut Output time array (pre-allocated, capacity >= threshold).
|
||||
* @param vOut Output value array (pre-allocated, capacity >= threshold).
|
||||
* @param threshold Target number of output points (must be >= 3 for LTTB to apply;
|
||||
* otherwise all points are copied).
|
||||
* @return Number of output points written (always <= threshold and <= nIn).
|
||||
*/
|
||||
inline uint32 LTTBDecimate(const float64 *tIn, const float64 *vIn, uint32 nIn,
|
||||
float64 *tOut, float64 *vOut, uint32 threshold)
|
||||
{
|
||||
if (nIn == 0u) {
|
||||
return 0u;
|
||||
}
|
||||
if (nIn <= threshold || threshold < 3u) {
|
||||
/* Fast path: no decimation needed */
|
||||
for (uint32 i = 0u; i < nIn; i++) {
|
||||
tOut[i] = tIn[i];
|
||||
vOut[i] = vIn[i];
|
||||
}
|
||||
return nIn;
|
||||
}
|
||||
|
||||
uint32 nOut = 0u;
|
||||
|
||||
/* Always include the first point */
|
||||
tOut[nOut] = tIn[0u];
|
||||
vOut[nOut] = vIn[0u];
|
||||
nOut++;
|
||||
|
||||
float64 bucketSize = static_cast<float64>(nIn - 2u) /
|
||||
static_cast<float64>(threshold - 2u);
|
||||
|
||||
uint32 a = 0u; /* index of last selected point */
|
||||
|
||||
for (uint32 i = 0u; i < threshold - 2u; i++) {
|
||||
|
||||
/* Compute average point in the NEXT bucket (for area calculation) */
|
||||
uint32 avgStart = static_cast<uint32>((static_cast<float64>(i + 1u) * bucketSize)) + 1u;
|
||||
uint32 avgEnd = static_cast<uint32>((static_cast<float64>(i + 2u) * bucketSize)) + 1u;
|
||||
if (avgEnd > nIn) { avgEnd = nIn; }
|
||||
|
||||
float64 avgT = 0.0;
|
||||
float64 avgV = 0.0;
|
||||
uint32 avgCount = avgEnd - avgStart;
|
||||
if (avgCount > 0u) {
|
||||
for (uint32 j = avgStart; j < avgEnd; j++) {
|
||||
avgT += tIn[j];
|
||||
avgV += vIn[j];
|
||||
}
|
||||
avgT /= static_cast<float64>(avgCount);
|
||||
avgV /= static_cast<float64>(avgCount);
|
||||
}
|
||||
|
||||
/* Find point in current bucket with maximum triangle area */
|
||||
uint32 bStart = static_cast<uint32>((static_cast<float64>(i) * bucketSize)) + 1u;
|
||||
uint32 bEnd = static_cast<uint32>((static_cast<float64>(i + 1u) * bucketSize)) + 1u;
|
||||
if (bEnd > nIn) { bEnd = nIn; }
|
||||
|
||||
float64 maxArea = -1.0;
|
||||
uint32 nextA = bStart;
|
||||
|
||||
float64 aT = tIn[a];
|
||||
float64 aV = vIn[a];
|
||||
|
||||
for (uint32 j = bStart; j < bEnd; j++) {
|
||||
float64 area = fabs((aT - avgT) * (vIn[j] - aV) -
|
||||
(aT - tIn[j]) * (avgV - aV)) * 0.5;
|
||||
if (area > maxArea) {
|
||||
maxArea = area;
|
||||
nextA = j;
|
||||
}
|
||||
}
|
||||
|
||||
tOut[nOut] = tIn[nextA];
|
||||
vOut[nOut] = vIn[nextA];
|
||||
nOut++;
|
||||
a = nextA;
|
||||
}
|
||||
|
||||
/* Always include the last point */
|
||||
tOut[nOut] = tIn[nIn - 1u];
|
||||
vOut[nOut] = vIn[nIn - 1u];
|
||||
nOut++;
|
||||
|
||||
return nOut;
|
||||
}
|
||||
|
||||
} /* namespace StreamHub */
|
||||
|
||||
#endif /* STREAMHUB_LTTB_H_ */
|
||||
@@ -0,0 +1,2 @@
|
||||
TARGET = x86-linux
|
||||
include Makefile.inc
|
||||
@@ -0,0 +1,33 @@
|
||||
OBJSX = StreamHub.x UDPSourceSession.x WSServer.x TriggerEngine.x
|
||||
|
||||
PACKAGE =
|
||||
ROOT_DIR = ../../..
|
||||
MAKEDEFAULTDIR = $(MARTe2_DIR)/MakeDefaults
|
||||
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
|
||||
|
||||
INCLUDES += -I.
|
||||
INCLUDES += -I$(ROOT_DIR)/Common/UDP
|
||||
INCLUDES += -I$(ROOT_DIR)/Source/Components/Interfaces/UDPStream
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L0Types
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L2Objects
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L3Streams
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Configuration
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L3Services
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L3Streams
|
||||
|
||||
LIBRARIES += -L$(MARTe2_DIR)/Build/$(TARGET)/Core -lMARTe2
|
||||
LIBRARIES += -L$(ROOT_DIR)/Build/$(TARGET)/Components/Interfaces/UDPStream -lUDPStream
|
||||
LIBRARIES += -lpthread -lrt -ldl -lm
|
||||
|
||||
all: $(OBJS) $(BUILD_DIR)/StreamHub$(EXEEXT)
|
||||
echo $(OBJS)
|
||||
|
||||
# StreamHub.ex is named after 'main' entry point; override the stem via explicit target
|
||||
$(BUILD_DIR)/StreamHub$(EXEEXT): createLibrary $(BUILD_DIR)/main.o $(OBJS)
|
||||
$(COMPILER) $(LFLAGS) $(BUILD_DIR)/main.o $(OBJS) $(LIBRARIES) $(LIBRARIES_STATIC) -o $(BUILD_DIR)/StreamHub$(EXEEXT)
|
||||
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibRules.$(TARGET)
|
||||
@@ -0,0 +1,115 @@
|
||||
/**
|
||||
* @file SHA1.h
|
||||
* @brief Inline SHA-1 implementation (public domain).
|
||||
*
|
||||
* Used exclusively for the WebSocket HTTP upgrade handshake
|
||||
* (Sec-WebSocket-Accept computation per RFC 6455).
|
||||
* No external dependencies.
|
||||
*/
|
||||
|
||||
#ifndef STREAMHUB_SHA1_H_
|
||||
#define STREAMHUB_SHA1_H_
|
||||
|
||||
#include "CompilerTypes.h"
|
||||
#include <string.h>
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::uint8;
|
||||
using MARTe::uint32;
|
||||
using MARTe::uint64;
|
||||
|
||||
/**
|
||||
* @brief Compute SHA-1 hash of @p data (len bytes) into @p digest (20 bytes).
|
||||
*/
|
||||
inline void SHA1(const uint8 *data, uint32 len, uint8 digest[20]) {
|
||||
|
||||
uint32 h0 = 0x67452301u;
|
||||
uint32 h1 = 0xEFCDAB89u;
|
||||
uint32 h2 = 0x98BADCFEu;
|
||||
uint32 h3 = 0x10325476u;
|
||||
uint32 h4 = 0xC3D2E1F0u;
|
||||
|
||||
/* ---- pre-process: append bit '1', zeros, 64-bit BE length ----------- */
|
||||
uint8 msg[128];
|
||||
memset(msg, 0, sizeof(msg));
|
||||
|
||||
uint32 msgLen = len;
|
||||
if (len < 56u) {
|
||||
memcpy(msg, data, len);
|
||||
msg[len] = 0x80u;
|
||||
/* 8-byte big-endian bit count at bytes 56-63 */
|
||||
uint64 bitLen = static_cast<uint64>(len) * 8u;
|
||||
for (int i = 7; i >= 0; i--) {
|
||||
msg[56 + i] = static_cast<uint8>(bitLen & 0xFFu);
|
||||
bitLen >>= 8;
|
||||
}
|
||||
msgLen = 64u;
|
||||
} else {
|
||||
/* two-block path for inputs up to 55+64=119 bytes — sufficient for WS keys */
|
||||
memcpy(msg, data, len);
|
||||
msg[len] = 0x80u;
|
||||
uint64 bitLen = static_cast<uint64>(len) * 8u;
|
||||
for (int i = 7; i >= 0; i--) {
|
||||
msg[120 + i] = static_cast<uint8>(bitLen & 0xFFu);
|
||||
bitLen >>= 8;
|
||||
}
|
||||
msgLen = 128u;
|
||||
}
|
||||
|
||||
/* ---- process each 64-byte block -------------------------------------- */
|
||||
for (uint32 blk = 0u; blk < msgLen; blk += 64u) {
|
||||
uint32 w[80];
|
||||
const uint8 *b = msg + blk;
|
||||
uint32 i;
|
||||
for (i = 0u; i < 16u; i++) {
|
||||
w[i] = (static_cast<uint32>(b[i*4u + 0u]) << 24u) |
|
||||
(static_cast<uint32>(b[i*4u + 1u]) << 16u) |
|
||||
(static_cast<uint32>(b[i*4u + 2u]) << 8u) |
|
||||
(static_cast<uint32>(b[i*4u + 3u]));
|
||||
}
|
||||
for (i = 16u; i < 80u; i++) {
|
||||
uint32 v = w[i-3u] ^ w[i-8u] ^ w[i-14u] ^ w[i-16u];
|
||||
w[i] = (v << 1u) | (v >> 31u);
|
||||
}
|
||||
|
||||
uint32 a = h0, bv = h1, c = h2, d = h3, e = h4;
|
||||
for (i = 0u; i < 80u; i++) {
|
||||
uint32 f, k;
|
||||
if (i < 20u) {
|
||||
f = (bv & c) | ((~bv) & d);
|
||||
k = 0x5A827999u;
|
||||
} else if (i < 40u) {
|
||||
f = bv ^ c ^ d;
|
||||
k = 0x6ED9EBA1u;
|
||||
} else if (i < 60u) {
|
||||
f = (bv & c) | (bv & d) | (c & d);
|
||||
k = 0x8F1BBCDCu;
|
||||
} else {
|
||||
f = bv ^ c ^ d;
|
||||
k = 0xCA62C1D6u;
|
||||
}
|
||||
uint32 tmp = ((a << 5u) | (a >> 27u)) + f + e + k + w[i];
|
||||
e = d; d = c;
|
||||
c = (bv << 30u) | (bv >> 2u);
|
||||
bv = a; a = tmp;
|
||||
}
|
||||
h0 += a; h1 += bv; h2 += c; h3 += d; h4 += e;
|
||||
}
|
||||
|
||||
/* ---- produce big-endian digest --------------------------------------- */
|
||||
digest[ 0] = static_cast<uint8>(h0 >> 24u); digest[ 1] = static_cast<uint8>(h0 >> 16u);
|
||||
digest[ 2] = static_cast<uint8>(h0 >> 8u); digest[ 3] = static_cast<uint8>(h0);
|
||||
digest[ 4] = static_cast<uint8>(h1 >> 24u); digest[ 5] = static_cast<uint8>(h1 >> 16u);
|
||||
digest[ 6] = static_cast<uint8>(h1 >> 8u); digest[ 7] = static_cast<uint8>(h1);
|
||||
digest[ 8] = static_cast<uint8>(h2 >> 24u); digest[ 9] = static_cast<uint8>(h2 >> 16u);
|
||||
digest[10] = static_cast<uint8>(h2 >> 8u); digest[11] = static_cast<uint8>(h2);
|
||||
digest[12] = static_cast<uint8>(h3 >> 24u); digest[13] = static_cast<uint8>(h3 >> 16u);
|
||||
digest[14] = static_cast<uint8>(h3 >> 8u); digest[15] = static_cast<uint8>(h3);
|
||||
digest[16] = static_cast<uint8>(h4 >> 24u); digest[17] = static_cast<uint8>(h4 >> 16u);
|
||||
digest[18] = static_cast<uint8>(h4 >> 8u); digest[19] = static_cast<uint8>(h4);
|
||||
}
|
||||
|
||||
} /* namespace StreamHub */
|
||||
|
||||
#endif /* STREAMHUB_SHA1_H_ */
|
||||
@@ -0,0 +1,285 @@
|
||||
/**
|
||||
* @file SignalRingBuffer.h
|
||||
* @brief Thread-safe fixed-capacity circular buffer of (float64 time, float64 value) pairs.
|
||||
*
|
||||
* Write() is called from the UDPSClient receive thread; Read/ReadRange are called
|
||||
* from the StreamHub push thread. A FastPollingMutexSem protects all operations.
|
||||
*/
|
||||
|
||||
#ifndef STREAMHUB_SIGNAL_RING_BUFFER_H_
|
||||
#define STREAMHUB_SIGNAL_RING_BUFFER_H_
|
||||
|
||||
#include "CompilerTypes.h"
|
||||
#include "FastPollingMutexSem.h"
|
||||
#include <string.h>
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::uint32;
|
||||
using MARTe::float64;
|
||||
using MARTe::FastPollingMutexSem;
|
||||
|
||||
/**
|
||||
* @brief Circular buffer of (time, value) float64 pairs.
|
||||
*
|
||||
* When full, Write() silently overwrites the oldest entry.
|
||||
*/
|
||||
class SignalRingBuffer {
|
||||
public:
|
||||
|
||||
SignalRingBuffer();
|
||||
~SignalRingBuffer();
|
||||
|
||||
/**
|
||||
* @brief Allocate buffer for @p maxPts points.
|
||||
* Frees any previously allocated buffer.
|
||||
* @return true on success.
|
||||
*/
|
||||
bool Allocate(uint32 maxPts);
|
||||
|
||||
/**
|
||||
* @brief Write one (t, v) pair. Called from the receive thread.
|
||||
*/
|
||||
void Write(float64 t, float64 v);
|
||||
|
||||
/**
|
||||
* @brief Copy the last (oldest-to-newest) min(n, count) points into tOut/vOut.
|
||||
* @return Number of points actually written.
|
||||
*/
|
||||
uint32 Read(float64 *tOut, float64 *vOut, uint32 n) const;
|
||||
|
||||
/**
|
||||
* @brief Copy all points with time in [t0, t1] into tOut/vOut (up to maxOut).
|
||||
* Uses binary search — requires monotonically non-decreasing time values.
|
||||
* @return Number of points actually written.
|
||||
*/
|
||||
uint32 ReadRange(float64 t0, float64 t1,
|
||||
float64 *tOut, float64 *vOut, uint32 maxOut) const;
|
||||
|
||||
/**
|
||||
* @brief Copy points written after @p cursor (a previously returned
|
||||
* TotalWritten() value) into tOut/vOut, oldest first. On overrun the
|
||||
* read is clamped to the oldest available point. @p cursor is advanced
|
||||
* by the number of points consumed (capped by @p maxOut).
|
||||
* @return Number of points actually written.
|
||||
*/
|
||||
uint32 ReadSince(MARTe::uint64 &cursor,
|
||||
float64 *tOut, float64 *vOut, uint32 maxOut) const;
|
||||
|
||||
/** @return Total number of points ever written (monotonic). */
|
||||
MARTe::uint64 TotalWritten() const;
|
||||
|
||||
/** @return Current number of stored points (≤ capacity). */
|
||||
uint32 Count() const;
|
||||
|
||||
/** @brief Discard all stored points. */
|
||||
void Clear();
|
||||
|
||||
private:
|
||||
float64 *tBuf;
|
||||
float64 *vBuf;
|
||||
uint32 capacity;
|
||||
uint32 head; /* next write position */
|
||||
uint32 count; /* number of valid entries */
|
||||
MARTe::uint64 totalWritten; /* monotonic write counter */
|
||||
mutable FastPollingMutexSem mutex;
|
||||
};
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Inline implementation */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
inline SignalRingBuffer::SignalRingBuffer()
|
||||
: tBuf(static_cast<float64 *>(0)),
|
||||
vBuf(static_cast<float64 *>(0)),
|
||||
capacity(0u),
|
||||
head(0u),
|
||||
count(0u),
|
||||
totalWritten(0u) {
|
||||
}
|
||||
|
||||
inline SignalRingBuffer::~SignalRingBuffer() {
|
||||
delete[] tBuf;
|
||||
delete[] vBuf;
|
||||
}
|
||||
|
||||
inline bool SignalRingBuffer::Allocate(uint32 maxPts) {
|
||||
if (maxPts == 0u) { return false; }
|
||||
|
||||
/* Allocate outside the lock, swap under it (readers may be active). */
|
||||
float64 *newT = new float64[maxPts];
|
||||
float64 *newV = new float64[maxPts];
|
||||
if ((newT == static_cast<float64 *>(0)) ||
|
||||
(newV == static_cast<float64 *>(0))) {
|
||||
delete[] newT;
|
||||
delete[] newV;
|
||||
return false;
|
||||
}
|
||||
|
||||
(void) mutex.FastLock();
|
||||
float64 *oldT = tBuf;
|
||||
float64 *oldV = vBuf;
|
||||
tBuf = newT;
|
||||
vBuf = newV;
|
||||
capacity = maxPts;
|
||||
head = 0u;
|
||||
count = 0u;
|
||||
totalWritten = 0u;
|
||||
mutex.FastUnLock();
|
||||
|
||||
delete[] oldT;
|
||||
delete[] oldV;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline void SignalRingBuffer::Write(float64 t, float64 v) {
|
||||
(void) mutex.FastLock();
|
||||
if (capacity > 0u) {
|
||||
tBuf[head] = t;
|
||||
vBuf[head] = v;
|
||||
head = (head + 1u) % capacity;
|
||||
if (count < capacity) { count++; }
|
||||
totalWritten++;
|
||||
}
|
||||
mutex.FastUnLock();
|
||||
}
|
||||
|
||||
inline uint32 SignalRingBuffer::Read(float64 *tOut, float64 *vOut, uint32 n) const {
|
||||
if ((capacity == 0u) || (count == 0u)) { return 0u; }
|
||||
(void) mutex.FastLock();
|
||||
|
||||
uint32 avail = count;
|
||||
if (n > avail) { n = avail; }
|
||||
|
||||
/* Oldest entry index: (head - count + capacity) % capacity */
|
||||
uint32 start = (head + capacity - avail) % capacity;
|
||||
/* We want the last n entries: skip (avail - n) entries */
|
||||
uint32 skip = avail - n;
|
||||
start = (start + skip) % capacity;
|
||||
|
||||
for (uint32 i = 0u; i < n; i++) {
|
||||
uint32 idx = (start + i) % capacity;
|
||||
tOut[i] = tBuf[idx];
|
||||
vOut[i] = vBuf[idx];
|
||||
}
|
||||
|
||||
mutex.FastUnLock();
|
||||
return n;
|
||||
}
|
||||
|
||||
inline uint32 SignalRingBuffer::ReadRange(float64 t0, float64 t1,
|
||||
float64 *tOut, float64 *vOut,
|
||||
uint32 maxOut) const {
|
||||
(void) mutex.FastLock();
|
||||
const uint32 avail = count;
|
||||
if ((capacity == 0u) || (avail == 0u) || (t1 < t0)) {
|
||||
mutex.FastUnLock();
|
||||
return 0u;
|
||||
}
|
||||
|
||||
const uint32 oldest = (head + capacity - avail) % capacity;
|
||||
|
||||
/* Binary search over the logically-ordered ring (time is monotonically
|
||||
* non-decreasing per ring). Port of Go ringbuf.go readRange. */
|
||||
|
||||
/* lo = first logical index with t >= t0 */
|
||||
uint32 lo = 0u;
|
||||
{
|
||||
uint32 a = 0u;
|
||||
uint32 b = avail;
|
||||
while (a < b) {
|
||||
const uint32 mid = a + ((b - a) >> 1);
|
||||
const float64 tm = tBuf[(oldest + mid) % capacity];
|
||||
if (tm < t0) { a = mid + 1u; } else { b = mid; }
|
||||
}
|
||||
lo = a;
|
||||
}
|
||||
/* hi = first logical index with t > t1 */
|
||||
uint32 hi = lo;
|
||||
{
|
||||
uint32 a = lo;
|
||||
uint32 b = avail;
|
||||
while (a < b) {
|
||||
const uint32 mid = a + ((b - a) >> 1);
|
||||
const float64 tm = tBuf[(oldest + mid) % capacity];
|
||||
if (tm <= t1) { a = mid + 1u; } else { b = mid; }
|
||||
}
|
||||
hi = a;
|
||||
}
|
||||
|
||||
uint32 nOut = hi - lo;
|
||||
if (nOut > maxOut) { nOut = maxOut; }
|
||||
for (uint32 i = 0u; i < nOut; i++) {
|
||||
const uint32 idx = (oldest + lo + i) % capacity;
|
||||
tOut[i] = tBuf[idx];
|
||||
vOut[i] = vBuf[idx];
|
||||
}
|
||||
|
||||
mutex.FastUnLock();
|
||||
return nOut;
|
||||
}
|
||||
|
||||
inline uint32 SignalRingBuffer::ReadSince(MARTe::uint64 &cursor,
|
||||
float64 *tOut, float64 *vOut,
|
||||
uint32 maxOut) const {
|
||||
(void) mutex.FastLock();
|
||||
|
||||
if (cursor > totalWritten) {
|
||||
/* Ring was reset (re-Allocate) — restart from current position. */
|
||||
cursor = totalWritten;
|
||||
}
|
||||
MARTe::uint64 pending = totalWritten - cursor;
|
||||
if ((capacity == 0u) || (pending == 0u) || (maxOut == 0u)) {
|
||||
mutex.FastUnLock();
|
||||
return 0u;
|
||||
}
|
||||
/* Overrun: oldest retained point is totalWritten - count. */
|
||||
if (pending > static_cast<MARTe::uint64>(count)) {
|
||||
pending = static_cast<MARTe::uint64>(count);
|
||||
cursor = totalWritten - pending;
|
||||
}
|
||||
|
||||
uint32 n = (pending > static_cast<MARTe::uint64>(maxOut))
|
||||
? maxOut : static_cast<uint32>(pending);
|
||||
|
||||
/* Logical index of the first pending point, relative to the oldest. */
|
||||
const uint32 avail = count;
|
||||
const uint32 oldest = (head + capacity - avail) % capacity;
|
||||
const uint32 firstLogical =
|
||||
avail - static_cast<uint32>(pending);
|
||||
|
||||
for (uint32 i = 0u; i < n; i++) {
|
||||
const uint32 idx = (oldest + firstLogical + i) % capacity;
|
||||
tOut[i] = tBuf[idx];
|
||||
vOut[i] = vBuf[idx];
|
||||
}
|
||||
cursor += static_cast<MARTe::uint64>(n);
|
||||
|
||||
mutex.FastUnLock();
|
||||
return n;
|
||||
}
|
||||
|
||||
inline MARTe::uint64 SignalRingBuffer::TotalWritten() const {
|
||||
(void) mutex.FastLock();
|
||||
const MARTe::uint64 tw = totalWritten;
|
||||
mutex.FastUnLock();
|
||||
return tw;
|
||||
}
|
||||
|
||||
inline uint32 SignalRingBuffer::Count() const {
|
||||
(void) mutex.FastLock();
|
||||
uint32 c = count;
|
||||
mutex.FastUnLock();
|
||||
return c;
|
||||
}
|
||||
|
||||
inline void SignalRingBuffer::Clear() {
|
||||
(void) mutex.FastLock();
|
||||
head = 0u;
|
||||
count = 0u;
|
||||
mutex.FastUnLock();
|
||||
}
|
||||
|
||||
} /* namespace StreamHub */
|
||||
|
||||
#endif /* STREAMHUB_SIGNAL_RING_BUFFER_H_ */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,236 @@
|
||||
/**
|
||||
* @file StreamHub.h
|
||||
* @brief Top-level orchestrator: N UDP sources, M WebSocket clients, push loop, trigger.
|
||||
*
|
||||
* StreamHub owns:
|
||||
* - Up to kMaxSessions UDPSourceSessions (one per MARTe2 UDPStreamer source)
|
||||
* - WSServer (multi-client WebSocket server)
|
||||
* - TriggerEngine (continuous / armed / triggered FSM)
|
||||
*
|
||||
* The push loop runs at pushRateHz Hz and:
|
||||
* 1. For each configured session, reads ring buffers, applies LTTB decimation,
|
||||
* serializes a binary data frame, broadcasts to all WS clients.
|
||||
* 2. At statsRateHz intervals, serializes a JSON stats frame and broadcasts.
|
||||
*
|
||||
* Commands are received as JSON text frames from any connected WS client via the
|
||||
* WSCommandCallback interface.
|
||||
*/
|
||||
|
||||
#ifndef STREAMHUB_H_
|
||||
#define STREAMHUB_H_
|
||||
|
||||
#include "CompilerTypes.h"
|
||||
#include "FastPollingMutexSem.h"
|
||||
#include "StreamString.h"
|
||||
#include "StructuredDataI.h"
|
||||
#include "Threads.h"
|
||||
|
||||
#include "UDPSourceSession.h"
|
||||
#include "WSServer.h"
|
||||
#include "TriggerEngine.h"
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::uint8;
|
||||
using MARTe::uint16;
|
||||
using MARTe::uint32;
|
||||
using MARTe::uint64;
|
||||
using MARTe::float64;
|
||||
using MARTe::StreamString;
|
||||
using MARTe::FastPollingMutexSem;
|
||||
using MARTe::StructuredDataI;
|
||||
|
||||
/** Maximum number of simultaneously connected UDPStreamer sources. */
|
||||
static const uint32 kMaxSessions = 32u;
|
||||
|
||||
/**
|
||||
* @brief Top-level StreamHub orchestrator.
|
||||
*
|
||||
* Usage:
|
||||
* StreamHub hub;
|
||||
* hub.Initialise(cfg); // parse WSPort, MaxPoints, PushRate, Sources
|
||||
* hub.Run(); // blocking push loop; returns on Stop()
|
||||
*/
|
||||
class StreamHub : public WSCommandCallback {
|
||||
public:
|
||||
|
||||
StreamHub();
|
||||
virtual ~StreamHub();
|
||||
|
||||
/**
|
||||
* @brief Configure the hub from a MARTe2 StructuredDataI.
|
||||
*
|
||||
* Expected keys:
|
||||
* WSPort (uint32, default 8090)
|
||||
* MaxPoints (uint32, default 20000) — ring buffer capacity per signal
|
||||
* PushRate (uint32, default 30) — push loop rate in Hz
|
||||
* MaxPushPoints (uint32, default 500) — LTTB threshold for live push
|
||||
* StatsRate (uint32, default 1) — stats broadcast rate in Hz
|
||||
* +Sources { +<id> { Label=...; Addr=...; Port=... } }
|
||||
*
|
||||
* @return true on success.
|
||||
*/
|
||||
bool Initialise(StructuredDataI &cfg);
|
||||
|
||||
/**
|
||||
* @brief Start the WebSocket server and run the push loop (blocking).
|
||||
* Returns when Stop() is called (e.g. from SIGINT handler).
|
||||
*/
|
||||
bool Run();
|
||||
|
||||
/**
|
||||
* @brief Signal the push loop to exit. Thread-safe.
|
||||
*/
|
||||
void Stop();
|
||||
|
||||
/* ---- WSCommandCallback (called from WS client read threads) ---------- */
|
||||
virtual void OnWSCommand(const char *json, uint32 len, uint32 slotIdx);
|
||||
virtual void OnWSClientConnected();
|
||||
virtual void OnWSClientDisconnected();
|
||||
|
||||
private:
|
||||
|
||||
/* ---- Push loop helpers ----------------------------------------------- */
|
||||
|
||||
/** One push tick: for each session emit a binary frame; handle trigger. */
|
||||
void PushData();
|
||||
|
||||
/** Serialize one binary data frame for session idx into pushBuf_. */
|
||||
uint32 SerializeBinaryFrame(uint32 sessionIdx, uint8 *buf, uint32 bufSize);
|
||||
|
||||
/** Broadcast JSON stats for all sessions. */
|
||||
void PushStats();
|
||||
|
||||
/** Broadcast {"type":"sources"} listing. */
|
||||
void BroadcastSources();
|
||||
|
||||
/** Broadcast {"type":"config","sourceId":...} for one session. */
|
||||
void BroadcastConfig(uint32 sessionIdx);
|
||||
|
||||
/* ---- Trigger (push loop side) ----------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief Per-tick trigger servicing: finalize captures, auto-rearm,
|
||||
* broadcast state transitions.
|
||||
*/
|
||||
void TriggerTick(float64 wallNowS);
|
||||
|
||||
/** Broadcast {"type":"triggerState",...} reflecting the current FSM. */
|
||||
void BroadcastTriggerState();
|
||||
|
||||
/**
|
||||
* @brief Build and broadcast the version=2 binary capture frame:
|
||||
* [u8 2][f64 trigTime][f64 preSec][f64 postSec][u32 nSig]
|
||||
* {[u16 keyLen][fullKey][u32 N][t f64×N][v f64×N]}
|
||||
*/
|
||||
void BroadcastTriggerCapture(float64 trigTime, float64 preSec,
|
||||
float64 postSec);
|
||||
|
||||
/* ---- Command handlers (called from OnWSCommand) ---------------------- */
|
||||
|
||||
void HandleAddSource(const char *json);
|
||||
void HandleRemoveSource(const char *json);
|
||||
void HandleSaveSources();
|
||||
void HandleGetSources();
|
||||
void HandleGetConfig(const char *json);
|
||||
void HandleGetStats();
|
||||
void HandleArm();
|
||||
void HandleDisarm();
|
||||
void HandleRearm();
|
||||
void HandleTrigStop(const char *json);
|
||||
void HandleSetTrigger(const char *json);
|
||||
void HandleZoom(const char *json, uint32 slotIdx);
|
||||
void HandleSetMaxPoints(const char *json);
|
||||
void HandlePing(uint32 slotIdx);
|
||||
|
||||
/* ---- Sources persistence (Go SourceConfig schema) --------------------- */
|
||||
|
||||
/**
|
||||
* @brief Start a new dynamic session from "host:port" + options.
|
||||
* Generates the session id ("s1", "s2", ...).
|
||||
* @return true if the session was started.
|
||||
*/
|
||||
bool AddSourceInternal(const char *label, const char *addrPort,
|
||||
const char *mcGroup, uint16 dataPort);
|
||||
|
||||
/**
|
||||
* @brief Load sources from sourcesFile_ (JSON array of
|
||||
* {"label","addr","multicastGroup","dataPort"}) and start them.
|
||||
*/
|
||||
void LoadSourcesFile();
|
||||
|
||||
/* ---- Tiny JSON helpers ----------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief Extract a string field from flat JSON: "key":"value".
|
||||
* @return true if found; value written into out (up to outSize-1 chars).
|
||||
*/
|
||||
static bool JsonGetString(const char *json, const char *key,
|
||||
char *out, uint32 outSize);
|
||||
|
||||
/**
|
||||
* @brief Extract a numeric field from flat JSON: "key":number.
|
||||
* @return true if found.
|
||||
*/
|
||||
static bool JsonGetFloat(const char *json, const char *key, float64 &out);
|
||||
static bool JsonGetUint32(const char *json, const char *key, uint32 &out);
|
||||
static bool JsonGetBool(const char *json, const char *key, bool &out);
|
||||
|
||||
/* ---- Members --------------------------------------------------------- */
|
||||
|
||||
UDPSourceSession sessions_[kMaxSessions];
|
||||
uint32 numSessions_; ///< Count of active sessions (not an upper slot bound)
|
||||
bool sessionActive_[kMaxSessions]; ///< Per-slot in-use flag (slots are not compacted on removal)
|
||||
bool configBroadcast_[kMaxSessions]; ///< True once BroadcastConfig was sent for this session
|
||||
FastPollingMutexSem sessionsMutex_; ///< Serializes add/remove (WS threads); push thread reads flags lock-free
|
||||
|
||||
WSServer wsServer_;
|
||||
TriggerEngine trigger_;
|
||||
|
||||
/* Configuration */
|
||||
uint16 wsPort_;
|
||||
uint32 maxPoints_;
|
||||
uint32 pushRateHz_;
|
||||
uint32 maxPushPoints_;
|
||||
uint32 statsRateHz_;
|
||||
uint32 ringTemporal_; ///< Ring capacity for multi-element (waveform) signals
|
||||
uint32 ringScalar_; ///< Ring capacity for scalar signals
|
||||
StreamString sourcesFile_; ///< Persistent dynamic source list (JSON)
|
||||
uint32 nextSourceId_; ///< Counter for generated session ids ("sN")
|
||||
|
||||
/* Push loop state */
|
||||
volatile bool running_;
|
||||
uint32 tickCount_; ///< incremented each push tick
|
||||
|
||||
/* Deferred setMaxPoints (applied by the push loop, not the WS thread) */
|
||||
volatile bool pendingMaxPointsSet_;
|
||||
uint32 pendingMaxPoints_;
|
||||
|
||||
/* Per-push scratch buffer (~8 MiB): one binary frame at a time */
|
||||
static const uint32 kPushBufSize = 8u * 1024u * 1024u;
|
||||
uint8 *pushBuf_;
|
||||
|
||||
/* Decimated output scratch (LTTB): maxPushPoints × 2 arrays per signal */
|
||||
float64 *lttbT_;
|
||||
float64 *lttbV_;
|
||||
|
||||
/* Ring-buffer read scratch for the push loop (preallocated in Initialise) */
|
||||
static const uint32 kPushScratchPts = 262144u; ///< Max new pts/signal/tick
|
||||
float64 *pushT_;
|
||||
float64 *pushV_;
|
||||
|
||||
/* Per-(session, signal) monotonic read cursors for ReadSignalSince.
|
||||
* Only new samples since the last tick are pushed (fixes re-LTTB
|
||||
* corruption of overlapping windows). */
|
||||
uint64 pushCursor_[kMaxSessions][UDPSS_MAX_SIGNALS];
|
||||
|
||||
/* Trigger servicing state (push thread only) */
|
||||
static const uint32 kTrigCapturePts = 20000u; ///< LTTB cap per captured signal
|
||||
TrigState lastTrigState_; ///< Last broadcast FSM state
|
||||
bool rearmPending_; ///< Normal-mode auto-rearm scheduled
|
||||
float64 rearmAtWallS_; ///< Wall time of the scheduled auto-rearm
|
||||
};
|
||||
|
||||
} /* namespace StreamHub */
|
||||
|
||||
#endif /* STREAMHUB_H_ */
|
||||
@@ -0,0 +1,153 @@
|
||||
/**
|
||||
* @file TriggerEngine.cpp
|
||||
* @brief Hub-side trigger FSM implementation (web client semantics).
|
||||
*/
|
||||
|
||||
#include "TriggerEngine.h"
|
||||
#include "AdvancedErrorManagement.h"
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
TriggerEngine::TriggerEngine()
|
||||
: epoch_(0u),
|
||||
state_(kTrigIdle),
|
||||
stopped_(false),
|
||||
prevValue_(0.0),
|
||||
prevValid_(false),
|
||||
trigTime_(0.0),
|
||||
firedPreSec_(0.0),
|
||||
firedPostSec_(0.0),
|
||||
firedValid_(false) {
|
||||
}
|
||||
|
||||
void TriggerEngine::SetConfig(const TriggerConfig &cfg) {
|
||||
(void) mutex_.FastLock();
|
||||
config_ = cfg;
|
||||
/* Clamp to web UI bounds */
|
||||
if (config_.windowSec < 1.0e-4) { config_.windowSec = 1.0e-4; }
|
||||
if (config_.windowSec > 10.0) { config_.windowSec = 10.0; }
|
||||
if (config_.prePercent < 0.0) { config_.prePercent = 0.0; }
|
||||
if (config_.prePercent > 100.0) { config_.prePercent = 100.0; }
|
||||
epoch_++;
|
||||
prevValid_ = false;
|
||||
prevValue_ = 0.0;
|
||||
mutex_.FastUnLock();
|
||||
}
|
||||
|
||||
TriggerConfig TriggerEngine::GetConfig() const {
|
||||
(void) mutex_.FastLock();
|
||||
TriggerConfig ret = config_;
|
||||
mutex_.FastUnLock();
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint32 TriggerEngine::GetConfigEpoch() const {
|
||||
(void) mutex_.FastLock();
|
||||
uint32 ret = epoch_;
|
||||
mutex_.FastUnLock();
|
||||
return ret;
|
||||
}
|
||||
|
||||
void TriggerEngine::Arm() {
|
||||
(void) mutex_.FastLock();
|
||||
state_ = kTrigArmed;
|
||||
prevValid_ = false;
|
||||
prevValue_ = 0.0;
|
||||
mutex_.FastUnLock();
|
||||
}
|
||||
|
||||
void TriggerEngine::Disarm() {
|
||||
(void) mutex_.FastLock();
|
||||
state_ = kTrigIdle;
|
||||
stopped_ = false;
|
||||
prevValid_ = false;
|
||||
prevValue_ = 0.0;
|
||||
firedValid_ = false;
|
||||
mutex_.FastUnLock();
|
||||
}
|
||||
|
||||
void TriggerEngine::SetStopped(bool stopped) {
|
||||
(void) mutex_.FastLock();
|
||||
stopped_ = stopped;
|
||||
mutex_.FastUnLock();
|
||||
}
|
||||
|
||||
bool TriggerEngine::GetStopped() const {
|
||||
(void) mutex_.FastLock();
|
||||
bool ret = stopped_;
|
||||
mutex_.FastUnLock();
|
||||
return ret;
|
||||
}
|
||||
|
||||
void TriggerEngine::CheckSample(float64 t, float64 v) {
|
||||
(void) mutex_.FastLock();
|
||||
|
||||
if (state_ != kTrigArmed) {
|
||||
mutex_.FastUnLock();
|
||||
return;
|
||||
}
|
||||
if (!prevValid_) {
|
||||
prevValue_ = v;
|
||||
prevValid_ = true;
|
||||
mutex_.FastUnLock();
|
||||
return;
|
||||
}
|
||||
|
||||
const float64 thr = config_.threshold;
|
||||
const bool up = (prevValue_ < thr) && (v >= thr);
|
||||
const bool down = (prevValue_ > thr) && (v <= thr);
|
||||
prevValue_ = v;
|
||||
|
||||
bool fired = false;
|
||||
switch (config_.edge) {
|
||||
case kEdgeRising: fired = up; break;
|
||||
case kEdgeFalling: fired = down; break;
|
||||
case kEdgeBoth: fired = (up || down); break;
|
||||
default: break;
|
||||
}
|
||||
|
||||
if (fired) {
|
||||
state_ = kTrigCollecting;
|
||||
trigTime_ = t;
|
||||
/* Latch the window at fire time so later config edits do not
|
||||
* affect this capture (web client snap._preS/_postS). */
|
||||
firedPreSec_ = config_.windowSec * config_.prePercent / 100.0;
|
||||
firedPostSec_ = config_.windowSec - firedPreSec_;
|
||||
firedValid_ = true;
|
||||
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
|
||||
"TriggerEngine: fired at t=%.6f (pre=%.4fs post=%.4fs)",
|
||||
t, firedPreSec_, firedPostSec_);
|
||||
}
|
||||
|
||||
mutex_.FastUnLock();
|
||||
}
|
||||
|
||||
TrigState TriggerEngine::GetState() const {
|
||||
(void) mutex_.FastLock();
|
||||
TrigState ret = state_;
|
||||
mutex_.FastUnLock();
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool TriggerEngine::GetFiredWindow(float64 &trigTime, float64 &preSec,
|
||||
float64 &postSec) const {
|
||||
(void) mutex_.FastLock();
|
||||
const bool ok = firedValid_;
|
||||
if (ok) {
|
||||
trigTime = trigTime_;
|
||||
preSec = firedPreSec_;
|
||||
postSec = firedPostSec_;
|
||||
}
|
||||
mutex_.FastUnLock();
|
||||
return ok;
|
||||
}
|
||||
|
||||
void TriggerEngine::MarkTriggered() {
|
||||
(void) mutex_.FastLock();
|
||||
if (state_ == kTrigCollecting) {
|
||||
state_ = kTrigTriggered;
|
||||
}
|
||||
mutex_.FastUnLock();
|
||||
}
|
||||
|
||||
} /* namespace StreamHub */
|
||||
@@ -0,0 +1,146 @@
|
||||
/**
|
||||
* @file TriggerEngine.h
|
||||
* @brief Hub-side trigger FSM with web-oscilloscope semantics.
|
||||
*
|
||||
* Mirrors the web client's trigger logic (Client/udpstreamer/static/app.js):
|
||||
*
|
||||
* IDLE --Arm()--> ARMED
|
||||
* ARMED --edge fires--> COLLECTING (trigTime + pre/post window latched)
|
||||
* COLLECTING -capture done-> TRIGGERED (StreamHub broadcasts the capture)
|
||||
* TRIGGERED --Arm()--> ARMED (auto after ~200 ms in normal mode,
|
||||
* manual "rearm" in single mode)
|
||||
* any --Disarm()--> IDLE
|
||||
*
|
||||
* Configuration is the web client's: full signal key ("src:sig" or
|
||||
* "src:sig[i]"), edge rising/falling/both, threshold, capture window length
|
||||
* windowSec, pre-trigger percentage, and acquisition mode normal/single with
|
||||
* a "stopped" flag pausing auto-rearm.
|
||||
*
|
||||
* CheckSample() is called from the UDPSClient receive thread for every decoded
|
||||
* sample of the configured signal. All state is protected by a
|
||||
* FastPollingMutexSem so the StreamHub push thread can poll it safely.
|
||||
*/
|
||||
|
||||
#ifndef STREAMHUB_TRIGGER_ENGINE_H_
|
||||
#define STREAMHUB_TRIGGER_ENGINE_H_
|
||||
|
||||
#include "CompilerTypes.h"
|
||||
#include "FastPollingMutexSem.h"
|
||||
#include "StreamString.h"
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::uint32;
|
||||
using MARTe::float64;
|
||||
using MARTe::StreamString;
|
||||
using MARTe::FastPollingMutexSem;
|
||||
|
||||
/** Trigger FSM states (badge mapping: idle/armed/collecting/triggered). */
|
||||
enum TrigState {
|
||||
kTrigIdle = 0,
|
||||
kTrigArmed = 1,
|
||||
kTrigCollecting = 2,
|
||||
kTrigTriggered = 3
|
||||
};
|
||||
|
||||
/** Edge selection. */
|
||||
enum TrigEdge {
|
||||
kEdgeRising = 0,
|
||||
kEdgeFalling = 1,
|
||||
kEdgeBoth = 2
|
||||
};
|
||||
|
||||
/** Acquisition mode. */
|
||||
enum TrigAcqMode {
|
||||
kTrigNormal = 0, ///< Auto-rearm after each capture (unless stopped)
|
||||
kTrigSingle = 1 ///< Stay TRIGGERED until an explicit rearm
|
||||
};
|
||||
|
||||
/** @brief Trigger configuration (web client semantics). */
|
||||
struct TriggerConfig {
|
||||
TriggerConfig();
|
||||
StreamString signalKey; ///< Full key: "src:sig" or "src:sig[i]"
|
||||
TrigEdge edge; ///< Rising / falling / both
|
||||
float64 threshold; ///< Trigger threshold (physical units)
|
||||
float64 windowSec; ///< Capture window length [1e-4 .. 10] s
|
||||
float64 prePercent; ///< Pre-trigger part of the window [0 .. 100] %
|
||||
TrigAcqMode mode; ///< Normal (auto-rearm) or single
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Hub-side trigger FSM.
|
||||
*/
|
||||
class TriggerEngine {
|
||||
public:
|
||||
|
||||
TriggerEngine();
|
||||
|
||||
/** @brief Replace the configuration (bumps the config epoch). */
|
||||
void SetConfig(const TriggerConfig &cfg);
|
||||
|
||||
/** @return Copy of the current configuration. */
|
||||
TriggerConfig GetConfig() const;
|
||||
|
||||
/**
|
||||
* @return Monotonic configuration epoch. UDPSourceSession caches the
|
||||
* resolved (signal idx, element idx) per epoch and re-resolves on change.
|
||||
*/
|
||||
uint32 GetConfigEpoch() const;
|
||||
|
||||
/** @brief Arm: any state → ARMED (resets edge detection). */
|
||||
void Arm();
|
||||
|
||||
/** @brief Disarm: any state → IDLE; clears the stopped flag. */
|
||||
void Disarm();
|
||||
|
||||
/** @brief Set/clear the stopped flag (pauses normal-mode auto-rearm). */
|
||||
void SetStopped(bool stopped);
|
||||
|
||||
/** @return Current stopped flag. */
|
||||
bool GetStopped() const;
|
||||
|
||||
/**
|
||||
* @brief Edge-detect one decoded sample of the configured signal.
|
||||
* Receive-thread context. Only acts in ARMED state; on a matching edge
|
||||
* latches trigTime and the pre/post window and moves to COLLECTING.
|
||||
*/
|
||||
void CheckSample(float64 t, float64 v);
|
||||
|
||||
/** @return Current FSM state. */
|
||||
TrigState GetState() const;
|
||||
|
||||
/**
|
||||
* @brief Get the latched capture window of the last fired trigger.
|
||||
* @return false if no trigger has fired since the last Disarm().
|
||||
*/
|
||||
bool GetFiredWindow(float64 &trigTime, float64 &preSec,
|
||||
float64 &postSec) const;
|
||||
|
||||
/** @brief COLLECTING → TRIGGERED (push thread, after the capture frame). */
|
||||
void MarkTriggered();
|
||||
|
||||
private:
|
||||
mutable FastPollingMutexSem mutex_;
|
||||
TriggerConfig config_;
|
||||
uint32 epoch_;
|
||||
TrigState state_;
|
||||
bool stopped_;
|
||||
float64 prevValue_; ///< Last sample (edge detection)
|
||||
bool prevValid_; ///< First-sample guard in ARMED state
|
||||
float64 trigTime_; ///< Latched trigger time (Unix s)
|
||||
float64 firedPreSec_; ///< Window pre-part latched at fire time
|
||||
float64 firedPostSec_; ///< Window post-part latched at fire time
|
||||
bool firedValid_; ///< true after a fire, until Disarm()
|
||||
};
|
||||
|
||||
inline TriggerConfig::TriggerConfig()
|
||||
: edge(kEdgeRising),
|
||||
threshold(0.0),
|
||||
windowSec(1.0),
|
||||
prePercent(20.0),
|
||||
mode(kTrigNormal) {
|
||||
}
|
||||
|
||||
} /* namespace StreamHub */
|
||||
|
||||
#endif /* STREAMHUB_TRIGGER_ENGINE_H_ */
|
||||
@@ -0,0 +1,799 @@
|
||||
/**
|
||||
* @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),
|
||||
timeScratch_(static_cast<float64 *>(0)),
|
||||
timeScratchLen_(0u),
|
||||
trigEngine_(static_cast<TriggerEngine *>(0)),
|
||||
trigEpochSeen_(0xFFFFFFFFu),
|
||||
trigSigIdx_(-1),
|
||||
trigElemIdx_(-1) {
|
||||
stateStr_ = "disconnected";
|
||||
ResetCalibration();
|
||||
}
|
||||
|
||||
UDPSourceSession::~UDPSourceSession() {
|
||||
(void) Stop();
|
||||
delete[] timeScratch_;
|
||||
}
|
||||
|
||||
void UDPSourceSession::ResetCalibration() {
|
||||
for (uint32 i = 0u; i < UDPSS_MAX_SIGNALS; i++) {
|
||||
timeSigCalibValid_[i] = false;
|
||||
timeSigCalib_[i] = 0.0;
|
||||
lastPktWallValid_[i] = false;
|
||||
lastPktWallS_[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) {
|
||||
ParseDataPayload(payload, payloadSize);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* 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);
|
||||
}
|
||||
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++) {
|
||||
uint32 ne = sigDescs_[i].numRows * sigDescs_[i].numCols;
|
||||
if (ne > maxElems) { maxElems = ne; }
|
||||
}
|
||||
if (maxElems > timeScratchLen_) {
|
||||
delete[] timeScratch_;
|
||||
timeScratch_ = new float64[maxElems];
|
||||
timeScratchLen_ = maxElems;
|
||||
}
|
||||
|
||||
/* Allocate ring buffers (outside metaMutex to avoid long hold) */
|
||||
AllocateRingBuffers();
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* DATA parsing */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
|
||||
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];
|
||||
uint32 numElements = desc.numRows * desc.numCols;
|
||||
if (numElements == 0u) { numElements = 1u; }
|
||||
|
||||
uint32 wireElemBytes = (desc.quantType != UDPS_QUANT_NONE)
|
||||
? QuantWireBytes(desc.quantType)
|
||||
: MARTe::UDPSTypeCodeByteSize(desc.typeCode);
|
||||
if (wireElemBytes == 0u) { return; }
|
||||
|
||||
uint32 elemsToRead = ((pm == UDPS_PUBLISH_ACCUMULATE) && (numElements == 1u))
|
||||
? numSamples
|
||||
: numElements;
|
||||
|
||||
if (off + elemsToRead * wireElemBytes > size) { return; }
|
||||
sigOff[s] = off;
|
||||
sigElems[s] = elemsToRead;
|
||||
off += elemsToRead * wireElemBytes;
|
||||
}
|
||||
|
||||
/* 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];
|
||||
uint32 numElements = desc.numRows * desc.numCols;
|
||||
if (numElements == 0u) { numElements = 1u; }
|
||||
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;
|
||||
if ((!timeSigCalibValid_[tIdx]) ||
|
||||
(Fabs((timeSigCalib_[tIdx] + timerS) - wallNowS) > kRecalibThresholdS)) {
|
||||
timeSigCalib_[tIdx] = wallNowS - timerS;
|
||||
timeSigCalibValid_[tIdx] = true;
|
||||
}
|
||||
anchor = timeSigCalib_[tIdx] + timerS;
|
||||
} else {
|
||||
anchorIsFirstSample = false;
|
||||
}
|
||||
const float64 dt = (desc.samplingRate > 0.0) ? (1.0 / desc.samplingRate) : 0.0;
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
const float64 t = anchorIsFirstSample
|
||||
? (anchor + static_cast<float64>(e) * dt)
|
||||
: (anchor - static_cast<float64>(nElems - 1u - e) * dt);
|
||||
WriteSample(s, e, t, val);
|
||||
}
|
||||
}
|
||||
else if (isFullArray) {
|
||||
/* Per-element timestamps from the referenced time-signal array. */
|
||||
if (hasTimeSig && (sigElems[tIdx] >= nElems) &&
|
||||
(nElems <= timeScratchLen_)) {
|
||||
DecodeElems(payload, sigOff[tIdx], descs[tIdx], nElems, timeScratch_);
|
||||
const float64 timer0S = timeScratch_[0] * timerToSec;
|
||||
if ((!timeSigCalibValid_[tIdx]) ||
|
||||
(Fabs((timeSigCalib_[tIdx] + timer0S) - wallNowS) > kRecalibThresholdS)) {
|
||||
timeSigCalib_[tIdx] = wallNowS - timer0S;
|
||||
timeSigCalibValid_[tIdx] = true;
|
||||
}
|
||||
const float64 calib = timeSigCalib_[tIdx];
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
WriteSample(s, e, calib + timeScratch_[e] * timerToSec, val);
|
||||
}
|
||||
} else {
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
WriteSample(s, e, wallNowS, val);
|
||||
}
|
||||
}
|
||||
}
|
||||
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 if (lastPktWallValid_[s] && (wallNowS > lastPktWallS_[s])) {
|
||||
/* Accumulated scalar: the packet carries one sample per RT
|
||||
* cycle — spread them across the inter-packet wall-clock gap
|
||||
* (same scheme as packed arrays) instead of stamping them all
|
||||
* with the arrival time, which renders as a stair plot. */
|
||||
const float64 dt = (wallNowS - lastPktWallS_[s]) /
|
||||
static_cast<float64>(nElems);
|
||||
const float64 base = lastPktWallS_[s];
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
WriteSample(s, 0u, base + static_cast<float64>(e + 1u) * dt, val);
|
||||
}
|
||||
}
|
||||
if (nElems > 1u) {
|
||||
lastPktWallS_[s] = wallNowS;
|
||||
lastPktWallValid_[s] = true;
|
||||
}
|
||||
}
|
||||
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])) {
|
||||
const float64 dt = (wallNowS - lastPktWallS_[s]) /
|
||||
static_cast<float64>(nElems);
|
||||
const float64 base = lastPktWallS_[s];
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
WriteSample(s, e, base + static_cast<float64>(e + 1u) * dt, val);
|
||||
}
|
||||
}
|
||||
lastPktWallS_[s] = wallNowS;
|
||||
lastPktWallValid_[s] = true;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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 */
|
||||
@@ -0,0 +1,273 @@
|
||||
/**
|
||||
* @file UDPSourceSession.h
|
||||
* @brief One UDPStreamer source connection: wraps UDPSClient, owns ring buffers + stats.
|
||||
*
|
||||
* Implements UDPSClientListener so it receives CONFIG and DATA callbacks from
|
||||
* the UDPSClient thread. Thread-safe accessors are provided for the StreamHub
|
||||
* push thread to read signal data and statistics.
|
||||
*/
|
||||
|
||||
#ifndef STREAMHUB_UDP_SOURCE_SESSION_H_
|
||||
#define STREAMHUB_UDP_SOURCE_SESSION_H_
|
||||
|
||||
#include "UDPSClient.h"
|
||||
#include "UDPSProtocol.h"
|
||||
#include "FastPollingMutexSem.h"
|
||||
#include "HighResolutionTimer.h"
|
||||
#include "StreamString.h"
|
||||
#include "ConfigurationDatabase.h"
|
||||
#include "SignalRingBuffer.h"
|
||||
#include "UDPSourceStats.h"
|
||||
#include "TriggerEngine.h"
|
||||
#include <string.h>
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::uint8;
|
||||
using MARTe::uint16;
|
||||
using MARTe::uint32;
|
||||
using MARTe::uint64;
|
||||
using MARTe::float64;
|
||||
using MARTe::StreamString;
|
||||
using MARTe::UDPSClient;
|
||||
using MARTe::UDPSClientListener;
|
||||
using MARTe::UDPSSignalDescriptor;
|
||||
using MARTe::FastPollingMutexSem;
|
||||
using MARTe::ConfigurationDatabase;
|
||||
|
||||
/** Maximum number of signals per source session. */
|
||||
static const uint32 UDPSS_MAX_SIGNALS = 256u;
|
||||
|
||||
/**
|
||||
* @brief One connected UDPStreamer source.
|
||||
*
|
||||
* Lifecycle: Initialise() → Start() → [running] → Stop()
|
||||
* Can be re-Initialised for a different address without destroying the object.
|
||||
*/
|
||||
class UDPSourceSession : public UDPSClientListener {
|
||||
public:
|
||||
|
||||
UDPSourceSession();
|
||||
virtual ~UDPSourceSession();
|
||||
|
||||
/**
|
||||
* @brief Configure and allocate this session.
|
||||
* @param id Short identifier string (used as source ID in JSON/binary frames).
|
||||
* @param label Human-readable label.
|
||||
* @param addr Server IPv4 address string.
|
||||
* @param port UDPStreamer port.
|
||||
* @param maxPts Ring buffer capacity per signal.
|
||||
* @param mcGroup Multicast group (empty string = unicast).
|
||||
* @param dataPort Multicast data port (0 = port+1).
|
||||
* @return true on success.
|
||||
*/
|
||||
bool Initialise(const char *id, const char *label,
|
||||
const char *addr, uint16 port, uint32 maxPts,
|
||||
const char *mcGroup = "", uint16 dataPort = 0u);
|
||||
|
||||
/** @brief Start the UDPSClient background thread. */
|
||||
bool Start();
|
||||
|
||||
/** @brief Stop the UDPSClient background thread. */
|
||||
bool Stop();
|
||||
|
||||
/* ---- UDPSClientListener callbacks (UDPSClient thread) -------------- */
|
||||
virtual void OnUDPSConfig(const uint8 *payload, uint32 payloadSize);
|
||||
virtual void OnUDPSData(const uint8 *payload, uint32 payloadSize);
|
||||
virtual void OnUDPSFragment(uint32 counter, uint32 nBytes, bool complete);
|
||||
virtual void OnUDPSConnected();
|
||||
virtual void OnUDPSDisconnected();
|
||||
|
||||
/* ---- Thread-safe accessors (push thread) --------------------------- */
|
||||
|
||||
/** @return Signal count (0 until first CONFIG received). */
|
||||
uint32 GetNumSignals() const;
|
||||
|
||||
/**
|
||||
* @brief Get a copy of one signal's descriptor.
|
||||
* @return false if idx is out of range or no CONFIG received yet.
|
||||
*/
|
||||
bool GetSignalDescriptor(uint32 idx, UDPSSignalDescriptor &desc) const;
|
||||
|
||||
/**
|
||||
* @brief Get publish mode (UDPS_PUBLISH_STRICT/ACCUMULATE/DECIMATE).
|
||||
*/
|
||||
uint8 GetPublishMode() const;
|
||||
|
||||
/**
|
||||
* @brief Copy the last (up to) n points from signal idx into tOut/vOut.
|
||||
* @return Number of points written.
|
||||
*/
|
||||
uint32 ReadSignalLast(uint32 idx, uint32 n,
|
||||
float64 *tOut, float64 *vOut) const;
|
||||
|
||||
/**
|
||||
* @brief Copy points written after @p cursor (monotonic write counter) into tOut/vOut.
|
||||
* Updates @p cursor. Clamps to the oldest available point on overrun.
|
||||
* @return Number of points written (capped by maxOut).
|
||||
*/
|
||||
uint32 ReadSignalSince(uint32 idx, MARTe::uint64 &cursor,
|
||||
float64 *tOut, float64 *vOut, uint32 maxOut) const;
|
||||
|
||||
/**
|
||||
* @brief Copy all ring-buffer points for signal idx in the time window [t0,t1].
|
||||
* @return Number of points written (capped by maxOut).
|
||||
*/
|
||||
uint32 ReadSignalRange(uint32 idx, float64 t0, float64 t1,
|
||||
float64 *tOut, float64 *vOut, uint32 maxOut) const;
|
||||
|
||||
/** @brief Thread-safe snapshot of current statistics. */
|
||||
UDPSourceStats GetStats() const;
|
||||
|
||||
/** @return Session identifier string. */
|
||||
StreamString GetId() const;
|
||||
|
||||
/** @return Human-readable label. */
|
||||
StreamString GetLabel() const;
|
||||
|
||||
/** @return Server address (for "sources" broadcast). */
|
||||
StreamString GetAddr() const;
|
||||
|
||||
/** @return Server port. */
|
||||
uint16 GetPort() const;
|
||||
|
||||
/** @return Multicast group ("" = unicast). */
|
||||
StreamString GetMulticastGroup() const;
|
||||
|
||||
/** @return Multicast data port (0 = port+1 default). */
|
||||
uint16 GetDataPort() const;
|
||||
|
||||
/** @return true after the first CONFIG packet has been fully parsed. */
|
||||
bool IsConfigured() const;
|
||||
|
||||
/** @return true if the session has been Initialised (but may not be Started). */
|
||||
bool IsInitialised() const;
|
||||
|
||||
/** @brief Update the live-read point cap (does not resize ring buffers). */
|
||||
bool SetMaxPoints(uint32 maxPts);
|
||||
|
||||
/**
|
||||
* @brief Set per-signal ring capacities (applied at the next CONFIG).
|
||||
* @param temporal Capacity for multi-element (waveform) signals.
|
||||
* @param scalar Capacity for scalar signals.
|
||||
*/
|
||||
void SetRingCapacities(uint32 temporal, uint32 scalar);
|
||||
|
||||
/**
|
||||
* @brief Attach the (shared) hub trigger engine.
|
||||
* Every decoded sample of the trigger's configured signal — resolved
|
||||
* against this session's id and signal names per config epoch — is fed
|
||||
* to TriggerEngine::CheckSample from the receive thread.
|
||||
*/
|
||||
void SetTriggerEngine(TriggerEngine *engine);
|
||||
|
||||
private:
|
||||
|
||||
/* DATA payload parsing */
|
||||
void ParseConfigPayload(const uint8 *payload, uint32 size);
|
||||
void ParseDataPayload(const uint8 *payload, uint32 size);
|
||||
void AllocateRingBuffers();
|
||||
|
||||
/** @brief Invalidate all wall-clock calibration state (receive thread only). */
|
||||
void ResetCalibration();
|
||||
|
||||
/**
|
||||
* @brief Re-resolve the trigger signal key against this session
|
||||
* (receive thread only; called when the trigger config epoch changes).
|
||||
*/
|
||||
void ResolveTriggerSignal(const UDPSSignalDescriptor *descs, uint32 nSigs);
|
||||
|
||||
/** @brief Ring write + trigger edge check for element @p e of signal @p s. */
|
||||
inline void WriteSample(uint32 s, uint32 e, float64 t, float64 v) {
|
||||
rings_[s].Write(t, v);
|
||||
if ((trigSigIdx_ >= 0) && (static_cast<uint32>(trigSigIdx_) == s)) {
|
||||
if ((trigElemIdx_ < 0) || (static_cast<uint32>(trigElemIdx_) == e)) {
|
||||
trigEngine_->CheckSample(t, v);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Decode @p nElems consecutive elements of signal @p desc starting
|
||||
* at payload offset @p off into @p out (dequantised physical values).
|
||||
*/
|
||||
void DecodeElems(const uint8 *payload, uint32 off,
|
||||
const UDPSSignalDescriptor &desc,
|
||||
uint32 nElems, float64 *out) const;
|
||||
|
||||
/** @brief Decode element @p e of signal @p desc at payload offset @p off. */
|
||||
float64 DecodeOneElem(const uint8 *payload, uint32 off,
|
||||
const UDPSSignalDescriptor &desc, uint32 e) const;
|
||||
|
||||
/* Value decoding helpers */
|
||||
float64 DecodeRawValue(const uint8 *ptr, uint8 typeCode) const;
|
||||
float64 DequantizeValue(float64 rawOrQuant, uint8 quantType,
|
||||
float64 rangeMin, float64 rangeMax,
|
||||
bool isRaw) const;
|
||||
uint32 QuantWireBytes(uint8 quantType) const;
|
||||
|
||||
/* Configuration (set by Initialise, immutable afterwards) */
|
||||
StreamString id_;
|
||||
StreamString label_;
|
||||
StreamString addr_;
|
||||
uint16 port_;
|
||||
StreamString mcGroup_; ///< Multicast group ("" = unicast)
|
||||
uint16 dataPort_; ///< Multicast data port (0 = port+1)
|
||||
uint32 maxPoints_;
|
||||
uint32 ringTemporal_; ///< Ring capacity for multi-element signals
|
||||
uint32 ringScalar_; ///< Ring capacity for scalar signals
|
||||
bool initialised_;
|
||||
|
||||
/* UDPSClient (owns the background thread) */
|
||||
UDPSClient client_;
|
||||
|
||||
/* Signal metadata — protected by metaMutex_ */
|
||||
mutable FastPollingMutexSem metaMutex_;
|
||||
UDPSSignalDescriptor sigDescs_[UDPSS_MAX_SIGNALS];
|
||||
SignalRingBuffer rings_[UDPSS_MAX_SIGNALS];
|
||||
uint32 numSignals_;
|
||||
uint8 publishMode_;
|
||||
bool configured_;
|
||||
|
||||
/* Statistics — protected by statsMutex_ (port of Go SourceStat, stats.go) */
|
||||
static const uint32 kStatRingSize = 512u;
|
||||
mutable FastPollingMutexSem statsMutex_;
|
||||
StreamString stateStr_; ///< Connection state string
|
||||
bool statSeenFirst_; ///< First DATA counter seen
|
||||
uint32 statLastCounter_; ///< Last DATA packet counter
|
||||
uint64 statTotalRx_; ///< Complete DATA packets
|
||||
uint64 statTotalLost_; ///< Lost packets (counter gaps)
|
||||
float64 ctRing_[kStatRingSize]; ///< Cycle times (s)
|
||||
uint32 fragRing_[kStatRingSize]; ///< Datagrams per cycle
|
||||
uint32 byteRing_[kStatRingSize]; ///< Raw bytes per cycle
|
||||
uint32 ctHead_; ///< Next write index
|
||||
bool ctFull_; ///< Ring has wrapped
|
||||
uint64 statLastRxTicks_; ///< HRT ticks at last complete DATA
|
||||
uint32 statFragCount_; ///< Per-cycle datagram accumulator
|
||||
uint32 statByteCount_; ///< Per-cycle byte accumulator
|
||||
|
||||
/* Wall-clock calibration of time signals (receive thread only).
|
||||
* timeSigCalib_[i] holds the offset (wall − timer·scale) for time signal i,
|
||||
* mirroring the Go hub's timeSigCalib map (hub.go). */
|
||||
float64 timeSigCalib_[UDPSS_MAX_SIGNALS];
|
||||
bool timeSigCalibValid_[UDPSS_MAX_SIGNALS];
|
||||
|
||||
/* Last packet arrival wall time per signal — used to interpolate per-element
|
||||
* timestamps for packed TIMEMODE_PACKET arrays (Go hub lastPktNs). */
|
||||
float64 lastPktWallS_[UDPSS_MAX_SIGNALS];
|
||||
bool lastPktWallValid_[UDPSS_MAX_SIGNALS];
|
||||
|
||||
/* Scratch for decoding referenced time-signal arrays (receive thread only). */
|
||||
float64 *timeScratch_;
|
||||
uint32 timeScratchLen_;
|
||||
|
||||
/* Trigger hookup (resolution cache is receive-thread only). */
|
||||
TriggerEngine *trigEngine_; ///< Shared hub trigger (may be NULL)
|
||||
uint32 trigEpochSeen_; ///< Last resolved trigger config epoch
|
||||
MARTe::int32 trigSigIdx_; ///< Watched signal index (-1 = none)
|
||||
MARTe::int32 trigElemIdx_; ///< Watched element index (-1 = all)
|
||||
};
|
||||
|
||||
} /* namespace StreamHub */
|
||||
|
||||
#endif /* STREAMHUB_UDP_SOURCE_SESSION_H_ */
|
||||
@@ -0,0 +1,75 @@
|
||||
/**
|
||||
* @file UDPSourceStats.h
|
||||
* @brief Per-source statistics snapshot for StreamHub (Go hub StatInfo parity).
|
||||
*/
|
||||
|
||||
#ifndef STREAMHUB_UDP_SOURCE_STATS_H_
|
||||
#define STREAMHUB_UDP_SOURCE_STATS_H_
|
||||
|
||||
#include "CompilerTypes.h"
|
||||
#include "StreamString.h"
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::uint32;
|
||||
using MARTe::uint64;
|
||||
using MARTe::float64;
|
||||
using MARTe::StreamString;
|
||||
|
||||
/** Number of bins in the cycle-time histogram. */
|
||||
static const uint32 UDPS_STAT_HIST_BINS = 20u;
|
||||
|
||||
/**
|
||||
* @brief Statistics snapshot for one UDPStreamer source.
|
||||
*
|
||||
* Field set mirrors the Go hub's StatInfo (stats.go) — same JSON names are
|
||||
* emitted by StreamHub::PushStats — plus a connection state string.
|
||||
*/
|
||||
struct UDPSourceStats {
|
||||
|
||||
UDPSourceStats();
|
||||
|
||||
uint64 totalReceived; ///< Total complete DATA packets received
|
||||
uint64 totalLost; ///< Estimated lost DATA packets (counter gaps)
|
||||
|
||||
float64 rateHz; ///< Mean cycle rate (1/avg cycle time)
|
||||
float64 rateStdHz; ///< Std-dev of the cycle rate
|
||||
float64 fragsPerCycle; ///< Mean UDP datagrams per DATA cycle
|
||||
float64 bytesPerCycle; ///< Mean raw bytes per DATA cycle
|
||||
|
||||
float64 cycleAvgMs; ///< Mean cycle time (ms)
|
||||
float64 cycleStdMs; ///< Std-dev of cycle time (ms)
|
||||
float64 cycleMinMs; ///< Min cycle time in window (ms)
|
||||
float64 cycleMaxMs; ///< Max cycle time in window (ms)
|
||||
|
||||
uint32 cycleHist[UDPS_STAT_HIST_BINS]; ///< Cycle-time histogram counts
|
||||
float64 cycleHistMin; ///< Histogram lower bound (ms)
|
||||
float64 cycleHistMax; ///< Histogram upper bound (ms)
|
||||
bool histValid; ///< True if the histogram has data
|
||||
|
||||
StreamString state; ///< "disconnected" | "connecting" | "connected" | "error"
|
||||
};
|
||||
|
||||
inline UDPSourceStats::UDPSourceStats()
|
||||
: totalReceived(0u),
|
||||
totalLost(0u),
|
||||
rateHz(0.0),
|
||||
rateStdHz(0.0),
|
||||
fragsPerCycle(0.0),
|
||||
bytesPerCycle(0.0),
|
||||
cycleAvgMs(0.0),
|
||||
cycleStdMs(0.0),
|
||||
cycleMinMs(0.0),
|
||||
cycleMaxMs(0.0),
|
||||
cycleHistMin(0.0),
|
||||
cycleHistMax(0.0),
|
||||
histValid(false) {
|
||||
for (uint32 i = 0u; i < UDPS_STAT_HIST_BINS; i++) {
|
||||
cycleHist[i] = 0u;
|
||||
}
|
||||
state = "disconnected";
|
||||
}
|
||||
|
||||
} /* namespace StreamHub */
|
||||
|
||||
#endif /* STREAMHUB_UDP_SOURCE_STATS_H_ */
|
||||
@@ -0,0 +1,142 @@
|
||||
/**
|
||||
* @file WSFrame.h
|
||||
* @brief WebSocket frame constants, opcode definitions, and frame-building helpers.
|
||||
*
|
||||
* RFC 6455 WebSocket framing layer utilities.
|
||||
*/
|
||||
|
||||
#ifndef STREAMHUB_WSFRAME_H_
|
||||
#define STREAMHUB_WSFRAME_H_
|
||||
|
||||
#include "CompilerTypes.h"
|
||||
#include <string.h>
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::uint8;
|
||||
using MARTe::uint16;
|
||||
using MARTe::uint32;
|
||||
using MARTe::uint64;
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Opcodes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
static const uint8 WS_OPCODE_CONTINUATION = 0x00u;
|
||||
static const uint8 WS_OPCODE_TEXT = 0x01u;
|
||||
static const uint8 WS_OPCODE_BINARY = 0x02u;
|
||||
static const uint8 WS_OPCODE_CLOSE = 0x08u;
|
||||
static const uint8 WS_OPCODE_PING = 0x09u;
|
||||
static const uint8 WS_OPCODE_PONG = 0x0Au;
|
||||
|
||||
static const uint8 WS_FIN_BIT = 0x80u;
|
||||
static const uint8 WS_MASK_BIT = 0x80u;
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Frame encoder (server → client, no masking) */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
/**
|
||||
* @brief Encode a WebSocket frame header into @p buf.
|
||||
* @param buf Output buffer (must have at least 10 bytes available).
|
||||
* @param opcode WS_OPCODE_TEXT or WS_OPCODE_BINARY.
|
||||
* @param payloadLen Number of payload bytes that follow the header.
|
||||
* @return Number of header bytes written (2, 4, or 10).
|
||||
*/
|
||||
inline uint32 WSEncodeHeader(uint8 *buf, uint8 opcode, uint64 payloadLen) {
|
||||
buf[0] = WS_FIN_BIT | opcode;
|
||||
if (payloadLen < 126u) {
|
||||
buf[1] = static_cast<uint8>(payloadLen);
|
||||
return 2u;
|
||||
} else if (payloadLen < 65536u) {
|
||||
buf[1] = 126u;
|
||||
buf[2] = static_cast<uint8>((payloadLen >> 8u) & 0xFFu);
|
||||
buf[3] = static_cast<uint8>(payloadLen & 0xFFu);
|
||||
return 4u;
|
||||
} else {
|
||||
buf[1] = 127u;
|
||||
for (int i = 7; i >= 0; i--) {
|
||||
buf[2 + i] = static_cast<uint8>(payloadLen & 0xFFu);
|
||||
payloadLen >>= 8u;
|
||||
}
|
||||
return 10u;
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Frame decoder (client → server, with masking) */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
/**
|
||||
* @brief Parsed fields from a received WebSocket frame header.
|
||||
*/
|
||||
struct WSFrameHeader {
|
||||
bool fin;
|
||||
uint8 opcode;
|
||||
bool masked;
|
||||
uint64 payloadLen;
|
||||
uint8 maskKey[4];
|
||||
uint32 headerSize; ///< Total header byte count (2 + 0|2|8 length + 0|4 mask)
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Try to parse a WebSocket frame header from @p buf (bufLen available bytes).
|
||||
* @param buf Received bytes.
|
||||
* @param bufLen Number of bytes available.
|
||||
* @param hdr Output: parsed header fields.
|
||||
* @return true if a complete header was parsed; false if more bytes are needed.
|
||||
*/
|
||||
inline bool WSParseHeader(const uint8 *buf, uint32 bufLen, WSFrameHeader &hdr) {
|
||||
if (bufLen < 2u) { return false; }
|
||||
|
||||
hdr.fin = (buf[0] & WS_FIN_BIT) != 0u;
|
||||
hdr.opcode = buf[0] & 0x0Fu;
|
||||
hdr.masked = (buf[1] & WS_MASK_BIT) != 0u;
|
||||
|
||||
uint32 hdrSize = 2u;
|
||||
uint64 plen = static_cast<uint64>(buf[1] & 0x7Fu);
|
||||
|
||||
if (plen == 126u) {
|
||||
if (bufLen < 4u) { return false; }
|
||||
plen = (static_cast<uint64>(buf[2]) << 8u) | static_cast<uint64>(buf[3]);
|
||||
hdrSize += 2u;
|
||||
} else if (plen == 127u) {
|
||||
if (bufLen < 10u) { return false; }
|
||||
plen = 0u;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
plen = (plen << 8u) | static_cast<uint64>(buf[2 + i]);
|
||||
}
|
||||
hdrSize += 8u;
|
||||
}
|
||||
|
||||
if (hdr.masked) {
|
||||
if (bufLen < hdrSize + 4u) { return false; }
|
||||
hdr.maskKey[0] = buf[hdrSize];
|
||||
hdr.maskKey[1] = buf[hdrSize + 1u];
|
||||
hdr.maskKey[2] = buf[hdrSize + 2u];
|
||||
hdr.maskKey[3] = buf[hdrSize + 3u];
|
||||
hdrSize += 4u;
|
||||
} else {
|
||||
hdr.maskKey[0] = hdr.maskKey[1] = hdr.maskKey[2] = hdr.maskKey[3] = 0u;
|
||||
}
|
||||
|
||||
hdr.payloadLen = plen;
|
||||
hdr.headerSize = hdrSize;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief XOR-unmask a masked WebSocket payload in place.
|
||||
* @param payload Payload buffer.
|
||||
* @param len Payload length.
|
||||
* @param mask 4-byte masking key.
|
||||
*/
|
||||
inline void WSUnmask(uint8 *payload, uint32 len, const uint8 mask[4]) {
|
||||
for (uint32 i = 0u; i < len; i++) {
|
||||
payload[i] ^= mask[i % 4u];
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace StreamHub */
|
||||
|
||||
#endif /* STREAMHUB_WSFRAME_H_ */
|
||||
@@ -0,0 +1,447 @@
|
||||
/**
|
||||
* @file WSServer.cpp
|
||||
* @brief Multi-client WebSocket server implementation.
|
||||
*/
|
||||
|
||||
#include "WSServer.h"
|
||||
#include "WSFrame.h"
|
||||
#include "SHA1.h"
|
||||
#include "Base64.h"
|
||||
#include "AdvancedErrorManagement.h"
|
||||
#include "Sleep.h"
|
||||
#include "Threads.h"
|
||||
#include "TimeoutType.h"
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::TimeoutType;
|
||||
using MARTe::Sleep;
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Thread trampolines */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
struct AcceptThreadArg { WSServer *srv; };
|
||||
struct ClientThreadArg { WSServer *srv; uint32 slot; };
|
||||
|
||||
static void AcceptThreadEntry(const void *arg) {
|
||||
WSServer *srv = reinterpret_cast<AcceptThreadArg *>(
|
||||
const_cast<void *>(arg))->srv;
|
||||
delete reinterpret_cast<const AcceptThreadArg *>(arg);
|
||||
srv->AcceptLoop();
|
||||
}
|
||||
|
||||
static void ClientReadEntry(const void *arg) {
|
||||
const ClientThreadArg *a = reinterpret_cast<const ClientThreadArg *>(arg);
|
||||
WSServer *srv = a->srv;
|
||||
uint32 slot = a->slot;
|
||||
delete a;
|
||||
srv->ClientReadLoop(slot);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Tiny JSON helpers (no library, only what we need) */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
/** Locate the first occurrence of pattern in s, return pointer or NULL. */
|
||||
static const char *FindSubstr(const char *s, const char *pattern) {
|
||||
return strstr(s, pattern);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* WSServer */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
WSServer::WSServer()
|
||||
: numClients(0u),
|
||||
callback(static_cast<WSCommandCallback *>(0)),
|
||||
running(false),
|
||||
acceptTid(MARTe::InvalidThreadIdentifier) {
|
||||
|
||||
for (uint32 i = 0u; i < WS_MAX_CLIENTS; i++) {
|
||||
clients[i].sock = static_cast<BasicTCPSocket *>(0);
|
||||
clients[i].active = false;
|
||||
clients[i].readTid = MARTe::InvalidThreadIdentifier;
|
||||
}
|
||||
}
|
||||
|
||||
WSServer::~WSServer() {
|
||||
(void) Stop();
|
||||
}
|
||||
|
||||
bool WSServer::Start(uint16 port, WSCommandCallback *cb) {
|
||||
if (running) { return true; }
|
||||
callback = cb;
|
||||
running = true;
|
||||
|
||||
if (!tcpListener.Open()) {
|
||||
REPORT_ERROR_STATIC(MARTe::ErrorManagement::FatalError,
|
||||
"WSServer: Failed to open TCP listener socket.");
|
||||
running = false;
|
||||
return false;
|
||||
}
|
||||
if (!tcpListener.Listen(port, static_cast<MARTe::int32>(WS_MAX_CLIENTS))) {
|
||||
REPORT_ERROR_STATIC(MARTe::ErrorManagement::FatalError,
|
||||
"WSServer: Failed to listen on port %u.", static_cast<uint32>(port));
|
||||
running = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
AcceptThreadArg *arg = new AcceptThreadArg();
|
||||
arg->srv = this;
|
||||
acceptTid = MARTe::Threads::BeginThread(
|
||||
reinterpret_cast<MARTe::ThreadFunctionType>(AcceptThreadEntry),
|
||||
arg, MARTe::THREADS_DEFAULT_STACKSIZE, "WSAccept");
|
||||
|
||||
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
|
||||
"WSServer: Listening on port %u.", static_cast<uint32>(port));
|
||||
return true;
|
||||
}
|
||||
|
||||
bool WSServer::Stop() {
|
||||
if (!running) { return true; }
|
||||
running = false;
|
||||
Sleep::MSec(200u);
|
||||
|
||||
/* Close all client connections — their read threads will exit on error */
|
||||
(void) clientsMutex.FastLock();
|
||||
for (uint32 i = 0u; i < WS_MAX_CLIENTS; i++) {
|
||||
if (clients[i].active && (clients[i].sock != static_cast<BasicTCPSocket *>(0))) {
|
||||
clients[i].sock->Close();
|
||||
}
|
||||
}
|
||||
clientsMutex.FastUnLock();
|
||||
Sleep::MSec(200u);
|
||||
|
||||
tcpListener.Close();
|
||||
Sleep::MSec(100u);
|
||||
|
||||
/* Free any remaining slots */
|
||||
(void) clientsMutex.FastLock();
|
||||
for (uint32 i = 0u; i < WS_MAX_CLIENTS; i++) {
|
||||
if (clients[i].sock != static_cast<BasicTCPSocket *>(0)) {
|
||||
delete clients[i].sock;
|
||||
clients[i].sock = static_cast<BasicTCPSocket *>(0);
|
||||
clients[i].active = false;
|
||||
}
|
||||
}
|
||||
numClients = 0u;
|
||||
clientsMutex.FastUnLock();
|
||||
return true;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Accept loop */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
void WSServer::AcceptLoop() {
|
||||
while (running) {
|
||||
BasicTCPSocket *client = tcpListener.WaitConnection(
|
||||
TimeoutType(500u), static_cast<BasicTCPSocket *>(0));
|
||||
if (client == static_cast<BasicTCPSocket *>(0)) {
|
||||
/* Timeout — loop again to check running flag */
|
||||
continue;
|
||||
}
|
||||
|
||||
/* HTTP upgrade */
|
||||
if (!UpgradeHTTP(client)) {
|
||||
client->Close();
|
||||
delete client;
|
||||
continue;
|
||||
}
|
||||
|
||||
uint32 slot = AllocSlot(client);
|
||||
if (slot == WS_MAX_CLIENTS) {
|
||||
/* No room */
|
||||
static const char *full = "HTTP/1.1 503 Service Unavailable\r\n\r\n";
|
||||
uint32 l = static_cast<uint32>(strlen(full));
|
||||
(void) client->Write(full, l);
|
||||
client->Close();
|
||||
delete client;
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Notify StreamHub */
|
||||
if (callback != static_cast<WSCommandCallback *>(0)) {
|
||||
callback->OnWSClientConnected();
|
||||
}
|
||||
|
||||
/* Start per-client read thread */
|
||||
ClientThreadArg *arg = new ClientThreadArg();
|
||||
arg->srv = this;
|
||||
arg->slot = slot;
|
||||
clients[slot].readTid = MARTe::Threads::BeginThread(
|
||||
reinterpret_cast<MARTe::ThreadFunctionType>(ClientReadEntry),
|
||||
arg, MARTe::THREADS_DEFAULT_STACKSIZE, "WSClient");
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* HTTP upgrade */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
bool WSServer::UpgradeHTTP(BasicTCPSocket *sock) {
|
||||
/* Read raw HTTP request until \r\n\r\n */
|
||||
static const uint32 kMaxHdr = 4096u;
|
||||
char hdrBuf[4096];
|
||||
uint32 totalRead = 0u;
|
||||
|
||||
while (totalRead < kMaxHdr - 1u) {
|
||||
uint32 rem = kMaxHdr - 1u - totalRead;
|
||||
bool ok = sock->Read(hdrBuf + totalRead, rem,
|
||||
TimeoutType(3000u));
|
||||
if (!ok || rem == 0u) { return false; }
|
||||
totalRead += rem;
|
||||
hdrBuf[totalRead] = '\0';
|
||||
if (strstr(hdrBuf, "\r\n\r\n") != static_cast<char *>(0)) { break; }
|
||||
}
|
||||
|
||||
/* Find Sec-WebSocket-Key */
|
||||
const char *keyHdr = FindSubstr(hdrBuf, "Sec-WebSocket-Key:");
|
||||
if (keyHdr == static_cast<const char *>(0)) { return false; }
|
||||
keyHdr += 18; /* skip "Sec-WebSocket-Key:" */
|
||||
while (*keyHdr == ' ') { keyHdr++; }
|
||||
|
||||
char key[64];
|
||||
uint32 keyLen = 0u;
|
||||
while (keyHdr[keyLen] != '\r' && keyHdr[keyLen] != '\n' &&
|
||||
keyHdr[keyLen] != '\0' && keyLen < 63u) {
|
||||
key[keyLen] = keyHdr[keyLen];
|
||||
keyLen++;
|
||||
}
|
||||
key[keyLen] = '\0';
|
||||
|
||||
/* Compute Accept = Base64(SHA1(key + GUID)) */
|
||||
static const char *kGUID = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11";
|
||||
char concat[128];
|
||||
snprintf(concat, sizeof(concat), "%s%s", key, kGUID);
|
||||
|
||||
uint8 digest[20];
|
||||
SHA1(reinterpret_cast<const uint8 *>(concat),
|
||||
static_cast<uint32>(strlen(concat)), digest);
|
||||
|
||||
char accept[64];
|
||||
Base64Encode(digest, 20u, accept);
|
||||
|
||||
/* Send 101 response */
|
||||
char resp[512];
|
||||
snprintf(resp, sizeof(resp),
|
||||
"HTTP/1.1 101 Switching Protocols\r\n"
|
||||
"Upgrade: websocket\r\n"
|
||||
"Connection: Upgrade\r\n"
|
||||
"Sec-WebSocket-Accept: %s\r\n"
|
||||
"\r\n", accept);
|
||||
uint32 respLen = static_cast<uint32>(strlen(resp));
|
||||
return sock->Write(resp, respLen);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Client read loop */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
void WSServer::ClientReadLoop(uint32 slotIdx) {
|
||||
WSClientSlot &slot = clients[slotIdx];
|
||||
BasicTCPSocket *sock = slot.sock;
|
||||
|
||||
/* Receive buffer (grows as needed by simple state machine) */
|
||||
static const uint32 kRecvBuf = WS_MAX_RECV_PAYLOAD + 14u;
|
||||
uint8 *buf = new uint8[kRecvBuf];
|
||||
uint32 filled = 0u;
|
||||
|
||||
while (running && slot.active) {
|
||||
/* Read more bytes (with short timeout so we can check running) */
|
||||
uint32 want = kRecvBuf - filled;
|
||||
if (want == 0u) {
|
||||
/* Buffer full — discard old frame (shouldn't happen with reasonable clients) */
|
||||
filled = 0u;
|
||||
continue;
|
||||
}
|
||||
bool ok = sock->Read(reinterpret_cast<char *>(buf + filled), want,
|
||||
TimeoutType(500u));
|
||||
if (!ok) {
|
||||
/* Timeout or error — check running and retry */
|
||||
if (!running) { break; }
|
||||
if (want == kRecvBuf) {
|
||||
/* Zero bytes read — connection likely closed */
|
||||
break;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
filled += want;
|
||||
|
||||
/* Parse as many complete frames as possible */
|
||||
uint32 consumed = 0u;
|
||||
while (consumed < filled) {
|
||||
const uint8 *frameStart = buf + consumed;
|
||||
uint32 available = filled - consumed;
|
||||
|
||||
WSFrameHeader hdr;
|
||||
if (!WSParseHeader(frameStart, available, hdr)) {
|
||||
break; /* incomplete header — need more data */
|
||||
}
|
||||
if (hdr.payloadLen > static_cast<uint64>(WS_MAX_RECV_PAYLOAD)) {
|
||||
/* Payload too large — close connection */
|
||||
goto client_done;
|
||||
}
|
||||
uint32 plen = static_cast<uint32>(hdr.payloadLen);
|
||||
uint32 totalFrameBytes = hdr.headerSize + plen;
|
||||
if (available < totalFrameBytes) {
|
||||
break; /* incomplete payload — need more data */
|
||||
}
|
||||
|
||||
/* Unmask payload */
|
||||
uint8 *payload = const_cast<uint8 *>(frameStart) + hdr.headerSize;
|
||||
if (hdr.masked) {
|
||||
WSUnmask(payload, plen, hdr.maskKey);
|
||||
}
|
||||
|
||||
/* Handle opcode */
|
||||
if (hdr.opcode == WS_OPCODE_CLOSE) {
|
||||
goto client_done;
|
||||
} else if (hdr.opcode == WS_OPCODE_PING) {
|
||||
/* Send pong */
|
||||
(void) slot.writeMutex.FastLock();
|
||||
SendFrame(slot, WS_OPCODE_PONG, payload, plen);
|
||||
slot.writeMutex.FastUnLock();
|
||||
} else if (hdr.opcode == WS_OPCODE_TEXT) {
|
||||
if (callback != static_cast<WSCommandCallback *>(0)) {
|
||||
/* NUL-terminate in place for the callback, but restore the
|
||||
* byte afterwards: when several client frames coalesce in
|
||||
* one TCP read, payload[plen] is the first header byte of
|
||||
* the NEXT frame. */
|
||||
uint8 savedByte = payload[plen];
|
||||
payload[plen] = '\0'; /* safe: buf has extra byte */
|
||||
callback->OnWSCommand(reinterpret_cast<const char *>(payload), plen, slotIdx);
|
||||
payload[plen] = savedByte;
|
||||
}
|
||||
}
|
||||
/* Binary frames from client are ignored (server only sends binary) */
|
||||
|
||||
consumed += totalFrameBytes;
|
||||
}
|
||||
|
||||
/* Compact buffer: move unconsumed bytes to front */
|
||||
if (consumed > 0u && consumed < filled) {
|
||||
memmove(buf, buf + consumed, filled - consumed);
|
||||
}
|
||||
filled = (consumed <= filled) ? (filled - consumed) : 0u;
|
||||
}
|
||||
|
||||
client_done:
|
||||
delete[] buf;
|
||||
if (callback != static_cast<WSCommandCallback *>(0)) {
|
||||
callback->OnWSClientDisconnected();
|
||||
}
|
||||
FreeSlot(slotIdx);
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Broadcast helpers */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
void WSServer::BroadcastText(const char *json, uint32 len) {
|
||||
for (uint32 i = 0u; i < WS_MAX_CLIENTS; i++) {
|
||||
if (!clients[i].active) { continue; }
|
||||
(void) clients[i].writeMutex.FastLock();
|
||||
if (clients[i].active) {
|
||||
(void) SendFrame(clients[i], WS_OPCODE_TEXT,
|
||||
reinterpret_cast<const uint8 *>(json), len);
|
||||
}
|
||||
clients[i].writeMutex.FastUnLock();
|
||||
}
|
||||
}
|
||||
|
||||
void WSServer::BroadcastBinary(const uint8 *buf, uint32 len) {
|
||||
for (uint32 i = 0u; i < WS_MAX_CLIENTS; i++) {
|
||||
if (!clients[i].active) { continue; }
|
||||
(void) clients[i].writeMutex.FastLock();
|
||||
if (clients[i].active) {
|
||||
(void) SendFrame(clients[i], WS_OPCODE_BINARY, buf, len);
|
||||
}
|
||||
clients[i].writeMutex.FastUnLock();
|
||||
}
|
||||
}
|
||||
|
||||
void WSServer::SendText(uint32 slotIdx, const char *json, uint32 len) {
|
||||
if (slotIdx >= WS_MAX_CLIENTS) { return; }
|
||||
WSClientSlot &slot = clients[slotIdx];
|
||||
if (!slot.active) { return; }
|
||||
(void) slot.writeMutex.FastLock();
|
||||
if (slot.active) {
|
||||
(void) SendFrame(slot, WS_OPCODE_TEXT,
|
||||
reinterpret_cast<const uint8 *>(json), len);
|
||||
}
|
||||
slot.writeMutex.FastUnLock();
|
||||
}
|
||||
|
||||
uint32 WSServer::ClientCount() const {
|
||||
(void) clientsMutex.FastLock();
|
||||
uint32 c = numClients;
|
||||
clientsMutex.FastUnLock();
|
||||
return c;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Internal helpers */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
bool WSServer::SendFrame(WSClientSlot &slot, uint8 opcode,
|
||||
const uint8 *payload, uint32 payloadLen) {
|
||||
if (!slot.active || (slot.sock == static_cast<BasicTCPSocket *>(0))) {
|
||||
return false;
|
||||
}
|
||||
uint8 hdrBuf[10];
|
||||
uint32 hdrLen = WSEncodeHeader(hdrBuf, opcode,
|
||||
static_cast<uint64>(payloadLen));
|
||||
|
||||
/* Write header */
|
||||
uint32 sz = hdrLen;
|
||||
if (!slot.sock->Write(reinterpret_cast<const char *>(hdrBuf), sz)) {
|
||||
return false;
|
||||
}
|
||||
/* Write payload */
|
||||
sz = payloadLen;
|
||||
if (payloadLen > 0u) {
|
||||
if (!slot.sock->Write(reinterpret_cast<const char *>(payload), sz)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32 WSServer::AllocSlot(BasicTCPSocket *sock) {
|
||||
(void) clientsMutex.FastLock();
|
||||
uint32 idx = WS_MAX_CLIENTS;
|
||||
for (uint32 i = 0u; i < WS_MAX_CLIENTS; i++) {
|
||||
if (!clients[i].active) {
|
||||
clients[i].sock = sock;
|
||||
clients[i].active = true;
|
||||
clients[i].readTid = MARTe::InvalidThreadIdentifier;
|
||||
idx = i;
|
||||
numClients++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
clientsMutex.FastUnLock();
|
||||
return idx;
|
||||
}
|
||||
|
||||
void WSServer::FreeSlot(uint32 idx) {
|
||||
if (idx >= WS_MAX_CLIENTS) { return; }
|
||||
(void) clientsMutex.FastLock();
|
||||
if (clients[idx].active) {
|
||||
clients[idx].active = false;
|
||||
if (clients[idx].sock != static_cast<BasicTCPSocket *>(0)) {
|
||||
clients[idx].sock->Close();
|
||||
delete clients[idx].sock;
|
||||
clients[idx].sock = static_cast<BasicTCPSocket *>(0);
|
||||
}
|
||||
if (numClients > 0u) { numClients--; }
|
||||
}
|
||||
clientsMutex.FastUnLock();
|
||||
}
|
||||
|
||||
} /* namespace StreamHub */
|
||||
@@ -0,0 +1,132 @@
|
||||
/**
|
||||
* @file WSServer.h
|
||||
* @brief Multi-client WebSocket server using MARTe2 BasicTCPSocket.
|
||||
*
|
||||
* Accepts WebSocket connections over HTTP/1.1 (RFC 6455 upgrade).
|
||||
* - One accept thread (started by Start()).
|
||||
* - One read thread per connected client (started when client upgrades).
|
||||
* - BroadcastText / BroadcastBinary called from any thread (push loop or
|
||||
* command handlers); per-client write is protected by a mutex.
|
||||
*
|
||||
* Maximum concurrent clients: WS_MAX_CLIENTS (16).
|
||||
*/
|
||||
|
||||
#ifndef STREAMHUB_WSSERVER_H_
|
||||
#define STREAMHUB_WSSERVER_H_
|
||||
|
||||
#include "BasicTCPSocket.h"
|
||||
#include "FastPollingMutexSem.h"
|
||||
|
||||
namespace StreamHub {
|
||||
|
||||
using MARTe::uint8;
|
||||
using MARTe::uint16;
|
||||
using MARTe::uint32;
|
||||
using MARTe::BasicTCPSocket;
|
||||
using MARTe::FastPollingMutexSem;
|
||||
|
||||
/** Maximum simultaneously connected WebSocket clients. */
|
||||
static const uint32 WS_MAX_CLIENTS = 16u;
|
||||
|
||||
/** Maximum WebSocket frame payload we will receive (commands are JSON, small). */
|
||||
static const uint32 WS_MAX_RECV_PAYLOAD = 65536u;
|
||||
|
||||
/** Maximum WebSocket frame payload we will send (data frames can be large). */
|
||||
static const uint32 WS_MAX_SEND_PAYLOAD = 4u * 1024u * 1024u; /* 4 MiB */
|
||||
|
||||
/**
|
||||
* @brief Callback interface — implemented by StreamHub.
|
||||
*/
|
||||
class WSCommandCallback {
|
||||
public:
|
||||
virtual ~WSCommandCallback() {}
|
||||
/** Called from client read thread with a fully-received text frame.
|
||||
* @param slotIdx Client slot index — usable with WSServer::SendText for
|
||||
* unicast replies. */
|
||||
virtual void OnWSCommand(const char *json, uint32 len, uint32 slotIdx) = 0;
|
||||
/** Called from accept thread when a new client upgrades. */
|
||||
virtual void OnWSClientConnected() = 0;
|
||||
/** Called from client read thread when the connection closes. */
|
||||
virtual void OnWSClientDisconnected() = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief One connected WebSocket client slot.
|
||||
*/
|
||||
struct WSClientSlot {
|
||||
BasicTCPSocket *sock; ///< Owned pointer; NULL when slot is free
|
||||
bool active;
|
||||
MARTe::ThreadIdentifier readTid;
|
||||
FastPollingMutexSem writeMutex; ///< Serialises writes from push + pong threads
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Minimal HTTP/1.1 + WebSocket server.
|
||||
*/
|
||||
class WSServer {
|
||||
public:
|
||||
|
||||
WSServer();
|
||||
~WSServer();
|
||||
|
||||
/**
|
||||
* @brief Open listen socket and start the accept thread.
|
||||
* @param port TCP port to listen on.
|
||||
* @param cb Command callback (StreamHub); must outlive WSServer.
|
||||
* @return true on success.
|
||||
*/
|
||||
bool Start(uint16 port, WSCommandCallback *cb);
|
||||
|
||||
/**
|
||||
* @brief Stop accept thread; close all client connections; close listener.
|
||||
*/
|
||||
bool Stop();
|
||||
|
||||
/**
|
||||
* @brief Broadcast a text (JSON) frame to all connected clients.
|
||||
* Safe to call from any thread.
|
||||
*/
|
||||
void BroadcastText(const char *json, uint32 len);
|
||||
|
||||
/**
|
||||
* @brief Broadcast a binary frame to all connected clients.
|
||||
* Safe to call from any thread.
|
||||
*/
|
||||
void BroadcastBinary(const uint8 *buf, uint32 len);
|
||||
|
||||
/**
|
||||
* @brief Send a text frame to a single client slot.
|
||||
* Used internally to send pong / direct responses.
|
||||
*/
|
||||
void SendText(uint32 slotIdx, const char *json, uint32 len);
|
||||
|
||||
/** @return Number of currently active client connections. */
|
||||
uint32 ClientCount() const;
|
||||
|
||||
/* ---- Internal thread entry points (public for Threads trampoline) ---- */
|
||||
void AcceptLoop();
|
||||
void ClientReadLoop(uint32 slotIdx);
|
||||
|
||||
private:
|
||||
|
||||
bool UpgradeHTTP(BasicTCPSocket *sock);
|
||||
bool SendFrame(WSClientSlot &slot, uint8 opcode,
|
||||
const uint8 *payload, uint32 payloadLen);
|
||||
|
||||
uint32 AllocSlot(BasicTCPSocket *sock);
|
||||
void FreeSlot(uint32 idx);
|
||||
|
||||
BasicTCPSocket tcpListener;
|
||||
WSClientSlot clients[WS_MAX_CLIENTS];
|
||||
uint32 numClients;
|
||||
mutable FastPollingMutexSem clientsMutex; ///< Protects numClients and clients[] array
|
||||
|
||||
WSCommandCallback *callback;
|
||||
volatile bool running;
|
||||
|
||||
MARTe::ThreadIdentifier acceptTid;
|
||||
};
|
||||
|
||||
} /* namespace StreamHub */
|
||||
|
||||
#endif /* STREAMHUB_WSSERVER_H_ */
|
||||
@@ -0,0 +1,143 @@
|
||||
/**
|
||||
* @file main.cpp
|
||||
* @brief StreamHub entry point.
|
||||
*
|
||||
* Usage:
|
||||
* StreamHub.ex [-cfg <file.cfg>] [-port <N>] [-maxPoints <N>]
|
||||
*
|
||||
* If -cfg is not given a minimal in-memory configuration is used with defaults.
|
||||
* -port and -maxPoints override values from the config file.
|
||||
*/
|
||||
|
||||
#include "StreamHub.h"
|
||||
#include "ConfigurationDatabase.h"
|
||||
#include "StandardParser.h"
|
||||
#include "BasicFile.h"
|
||||
#include "AdvancedErrorManagement.h"
|
||||
#include "ErrorManagement.h"
|
||||
#include "StreamString.h"
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <signal.h>
|
||||
|
||||
/* Simple stderr logger for MARTe2 REPORT_ERROR_STATIC messages */
|
||||
static void StderrErrorProcessFunction(const MARTe::ErrorManagement::ErrorInformation &info,
|
||||
const MARTe::char8 * const errorDescription) {
|
||||
const char *level = "Info";
|
||||
MARTe::ErrorManagement::ErrorType et = info.header.errorType;
|
||||
if (et == MARTe::ErrorManagement::FatalError) { level = "FatalError"; }
|
||||
else if (et == MARTe::ErrorManagement::Warning) { level = "Warning"; }
|
||||
else if (et == MARTe::ErrorManagement::Information) { level = "Information"; }
|
||||
else if (et == MARTe::ErrorManagement::Debug) { level = "Debug"; }
|
||||
const char *fname = info.fileName;
|
||||
if (fname == static_cast<const char *>(0)) { fname = "?"; }
|
||||
/* strip path — just filename */
|
||||
for (const char *p = fname; *p != '\0'; p++) {
|
||||
if (*p == '/') { fname = p + 1; }
|
||||
}
|
||||
fprintf(stderr, "[StreamHub][%s - %s:%u]: %s\n",
|
||||
level, fname,
|
||||
static_cast<unsigned int>(info.header.lineNumber),
|
||||
errorDescription != static_cast<const char *>(0) ? errorDescription : "");
|
||||
}
|
||||
|
||||
using MARTe::ConfigurationDatabase;
|
||||
using MARTe::StandardParser;
|
||||
using MARTe::StreamString;
|
||||
using MARTe::uint32;
|
||||
using MARTe::BasicFile;
|
||||
|
||||
/* Global flag set by SIGINT/SIGTERM handler */
|
||||
static volatile int gStop = 0;
|
||||
static StreamHub::StreamHub *gHub = static_cast<StreamHub::StreamHub *>(0);
|
||||
|
||||
static void SignalHandler(int /*sig*/) {
|
||||
gStop = 1;
|
||||
if (gHub != static_cast<StreamHub::StreamHub *>(0)) {
|
||||
gHub->Stop();
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
/* Install MARTe2 error handler so REPORT_ERROR_STATIC messages go to stderr */
|
||||
MARTe::ErrorManagement::SetErrorProcessFunction(&StderrErrorProcessFunction);
|
||||
|
||||
const char *cfgFile = static_cast<const char *>(0);
|
||||
uint32 portOverride = 0u;
|
||||
uint32 maxPtsOverride = 0u;
|
||||
|
||||
/* Parse command line */
|
||||
for (int i = 1; i < argc; i++) {
|
||||
if ((strcmp(argv[i], "-cfg") == 0) && (i + 1 < argc)) {
|
||||
cfgFile = argv[++i];
|
||||
} else if ((strcmp(argv[i], "-port") == 0) && (i + 1 < argc)) {
|
||||
portOverride = static_cast<uint32>(atoi(argv[++i]));
|
||||
} else if ((strcmp(argv[i], "-maxPoints") == 0) && (i + 1 < argc)) {
|
||||
maxPtsOverride = static_cast<uint32>(atoi(argv[++i]));
|
||||
} else if ((strcmp(argv[i], "-h") == 0) ||
|
||||
(strcmp(argv[i], "--help") == 0)) {
|
||||
printf("Usage: StreamHub.ex [-cfg file.cfg] [-port N] [-maxPoints N]\n");
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* Build ConfigurationDatabase */
|
||||
ConfigurationDatabase cfg;
|
||||
|
||||
if (cfgFile != static_cast<const char *>(0)) {
|
||||
/* Load from file via StandardParser */
|
||||
BasicFile f;
|
||||
if (!f.Open(cfgFile,
|
||||
BasicFile::ACCESS_MODE_R | BasicFile::FLAG_CREAT)) {
|
||||
fprintf(stderr, "StreamHub: cannot open config file '%s'\n", cfgFile);
|
||||
return 1;
|
||||
}
|
||||
|
||||
StreamString cfgContent;
|
||||
StreamString errStream;
|
||||
StandardParser parser(f, cfg, &errStream);
|
||||
if (!parser.Parse()) {
|
||||
fprintf(stderr, "StreamHub: config parse error: %s\n",
|
||||
errStream.Buffer());
|
||||
f.Close();
|
||||
return 1;
|
||||
}
|
||||
f.Close();
|
||||
|
||||
/* Navigate into +Hub node if present */
|
||||
if (cfg.MoveRelative("Hub")) {
|
||||
/* already inside */
|
||||
}
|
||||
}
|
||||
|
||||
/* Apply command-line overrides */
|
||||
if (portOverride > 0u) {
|
||||
(void) cfg.Write("WSPort", portOverride);
|
||||
}
|
||||
if (maxPtsOverride > 0u) {
|
||||
(void) cfg.Write("MaxPoints", maxPtsOverride);
|
||||
}
|
||||
|
||||
/* Install signal handlers */
|
||||
signal(SIGINT, SignalHandler);
|
||||
signal(SIGTERM, SignalHandler);
|
||||
|
||||
/* Allocate on the heap: StreamHub embeds 32 UDPSourceSession objects,
|
||||
* each ~327 KB (4 reassembly slots × 65 KB + recv buffer), totalling
|
||||
* ~10 MB — well above the default 8 MB thread stack limit. */
|
||||
StreamHub::StreamHub *hub = new StreamHub::StreamHub();
|
||||
gHub = hub;
|
||||
|
||||
if (!hub->Initialise(cfg)) {
|
||||
fprintf(stderr, "StreamHub: Initialise() failed.\n");
|
||||
delete hub;
|
||||
return 1;
|
||||
}
|
||||
|
||||
(void) hub->Run();
|
||||
|
||||
delete hub;
|
||||
printf("StreamHub: stopped.\n");
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user