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;
|
||||
}
|
||||
@@ -32,6 +32,7 @@ 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
|
||||
@@ -46,6 +47,8 @@ INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L5GAMs
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L3Streams
|
||||
|
||||
LIBRARIES += -L$(BUILD_DIR)/../../Interfaces/UDPStream -lUDPStream
|
||||
|
||||
all: $(OBJS) \
|
||||
$(BUILD_DIR)/UDPStreamer$(LIBEXT) \
|
||||
$(BUILD_DIR)/UDPStreamer$(DLLEXT)
|
||||
|
||||
@@ -33,6 +33,7 @@
|
||||
/* Project header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
#include "AdvancedErrorManagement.h"
|
||||
#include "ConfigurationDatabase.h"
|
||||
#include "EmbeddedThreadI.h"
|
||||
#include "GlobalObjectsDatabase.h"
|
||||
#include "HighResolutionTimer.h"
|
||||
@@ -61,7 +62,7 @@ static const int32 UDPS_TCP_MAX_CONNECTIONS = 4;
|
||||
static const uint32 UDPS_DEFAULT_MAX_PAYLOAD = 1400u;
|
||||
|
||||
/** Minimum MaxPayloadSize: header + at least 1 byte of payload. */
|
||||
static const uint32 UDPS_MIN_PAYLOAD = static_cast<uint32>(sizeof(UDPSPacketHeader)) + 1u;
|
||||
static const uint32 UDPS_MIN_PAYLOAD = UDPS_HEADER_SIZE + 1u;
|
||||
|
||||
/** Server socket receive timeout in milliseconds.
|
||||
* Set to 0 for a pure non-blocking poll so the send loop can keep pace with
|
||||
@@ -72,30 +73,6 @@ static const uint32 UDPS_RECV_TIMEOUT_MS = 0u;
|
||||
/** EventSem wait timeout in milliseconds for the data loop. */
|
||||
static const uint32 UDPS_DATA_WAIT_MS = 10u;
|
||||
|
||||
/** Sentinel: no time-signal reference; use the packet-level timestamp. */
|
||||
static const uint32 UDPS_NO_TIME_SIGNAL = 0xFFFFFFFFu;
|
||||
|
||||
/** Max signal name length in CONFIG packet (including null terminator). */
|
||||
static const uint32 UDPS_MAX_SIGNAL_NAME = 64u;
|
||||
|
||||
/** Max unit string length in CONFIG packet (including null terminator). */
|
||||
static const uint32 UDPS_MAX_UNIT_LEN = 32u;
|
||||
|
||||
/** Size in bytes of one signal descriptor in the CONFIG payload. */
|
||||
static const uint32 UDPS_SIGNAL_DESC_SIZE =
|
||||
UDPS_MAX_SIGNAL_NAME /* name */
|
||||
+ 1u /* typeCode */
|
||||
+ 1u /* quantType */
|
||||
+ 1u /* numDimensions*/
|
||||
+ 4u /* numRows */
|
||||
+ 4u /* numCols */
|
||||
+ 8u /* rangeMin */
|
||||
+ 8u /* rangeMax */
|
||||
+ 1u /* timeMode */
|
||||
+ 8u /* samplingRate */
|
||||
+ 4u /* timeSignalIdx*/
|
||||
+ UDPS_MAX_UNIT_LEN; /* unit */
|
||||
|
||||
/** Bytes prepended to each DATA payload for the HRT packet timestamp. */
|
||||
static const uint32 UDPS_TIMESTAMP_BYTES = 8u;
|
||||
|
||||
@@ -114,9 +91,6 @@ UDPStreamer::UDPStreamer() :
|
||||
publishMode = UDPStreamerPublishStrict;
|
||||
minRefreshRate = 0.0;
|
||||
flushPeriodTicks = 0u;
|
||||
dataPort = UDPS_DEFAULT_PORT + UDPS_DEFAULT_DATA_PORT_OFFSET;
|
||||
useMulticast = false;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
numSigs = 0u;
|
||||
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
|
||||
readyBuffer = NULL_PTR(uint8 *);
|
||||
@@ -125,7 +99,6 @@ UDPStreamer::UDPStreamer() :
|
||||
totalSrcBytes = 0u;
|
||||
totalWireBytes = 0u;
|
||||
syncTimestamp = 0u;
|
||||
clientConnected = false;
|
||||
packetCounter = 0u;
|
||||
maxBatchCount = 0u;
|
||||
singleCycleWireBytes = 0u;
|
||||
@@ -162,12 +135,7 @@ UDPStreamer::~UDPStreamer() {
|
||||
}
|
||||
}
|
||||
|
||||
if (serverSocket.IsValid()) {
|
||||
(void) serverSocket.Close();
|
||||
}
|
||||
if (clientSocket.IsValid()) {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
(void) server.Stop();
|
||||
|
||||
HeapI *heapAccum = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
if (accumBuffer != NULL_PTR(uint8 *)) {
|
||||
@@ -186,19 +154,6 @@ UDPStreamer::~UDPStreamer() {
|
||||
scratchTimestamps = NULL_PTR(uint64 *);
|
||||
}
|
||||
|
||||
/* Multicast-mode cleanup */
|
||||
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
(void) tcpClient->Close();
|
||||
delete tcpClient;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
}
|
||||
if (tcpListener.IsValid()) {
|
||||
(void) tcpListener.Close();
|
||||
}
|
||||
if (dataSocket.IsValid()) {
|
||||
(void) dataSocket.Close();
|
||||
}
|
||||
|
||||
if (signalInfos != NULL_PTR(UDPStreamerSignalInfo *)) {
|
||||
delete[] signalInfos;
|
||||
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
|
||||
@@ -332,37 +287,25 @@ bool UDPStreamer::Initialise(StructuredDataI &data) {
|
||||
}
|
||||
|
||||
if (ok) {
|
||||
StreamString mcastStr = "";
|
||||
(void) data.Read("MulticastGroup", mcastStr);
|
||||
if (mcastStr.Size() > 0u) {
|
||||
multicastGroup = mcastStr;
|
||||
useMulticast = true;
|
||||
|
||||
uint16 dp = 0u;
|
||||
if (!data.Read("DataPort", dp)) {
|
||||
dp = port + UDPS_DEFAULT_DATA_PORT_OFFSET;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"DataPort not specified; using Port+1 = %u.",
|
||||
static_cast<uint32>(dp));
|
||||
}
|
||||
if ((dp == 0u) || (dp == port)) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"DataPort %u must be non-zero and differ from Port %u.",
|
||||
static_cast<uint32>(dp), static_cast<uint32>(port));
|
||||
ok = false;
|
||||
}
|
||||
else {
|
||||
dataPort = dp;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"Multicast mode: group=%s, controlPort=%u, dataPort=%u.",
|
||||
multicastGroup.Buffer(),
|
||||
static_cast<uint32>(port),
|
||||
static_cast<uint32>(dataPort));
|
||||
// Build server config with already-resolved Port and MaxPayloadSize so
|
||||
// UDPSServer::Initialise() always sees them, even when defaults were used.
|
||||
ConfigurationDatabase serverCfg;
|
||||
(void) serverCfg.Write("Port", static_cast<uint32>(port));
|
||||
(void) serverCfg.Write("MaxPayloadSize", maxPayloadSize);
|
||||
// Forward optional multicast / timeout params if present in caller's data.
|
||||
StreamString mcGroup;
|
||||
if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
|
||||
(void) serverCfg.Write("MulticastGroup", mcGroup.Buffer());
|
||||
uint32 dp = 0u;
|
||||
if (data.Read("DataPort", dp)) {
|
||||
(void) serverCfg.Write("DataPort", dp);
|
||||
}
|
||||
}
|
||||
else {
|
||||
useMulticast = false;
|
||||
uint32 clientTimeout = 0u;
|
||||
if (data.Read("ClientTimeout", clientTimeout)) {
|
||||
(void) serverCfg.Write("ClientTimeout", clientTimeout);
|
||||
}
|
||||
ok = server.Initialise(serverCfg);
|
||||
}
|
||||
|
||||
return ok;
|
||||
@@ -843,59 +786,28 @@ bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
|
||||
const char8 *const nextStateName) {
|
||||
bool ok = true;
|
||||
|
||||
if (useMulticast) {
|
||||
/* Open TCP listener for control (CONNECT/DISCONNECT) */
|
||||
if (!tcpListener.IsValid()) {
|
||||
ok = tcpListener.Open();
|
||||
if (ok) {
|
||||
ok = tcpListener.Listen(port, UDPS_TCP_MAX_CONNECTIONS);
|
||||
}
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not open TCP listener on port %u.",
|
||||
static_cast<uint32>(port));
|
||||
}
|
||||
/* SetBlocking(false) makes WaitConnection(TimeoutType(0u)) non-blocking.
|
||||
* If it fails, the select() guard in Execute() protects us. */
|
||||
if (ok) {
|
||||
if (!tcpListener.SetBlocking(false)) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"SetBlocking(false) on TCP listener failed; "
|
||||
"using select() guard only.");
|
||||
}
|
||||
}
|
||||
}
|
||||
ok = server.Start();
|
||||
|
||||
/* Open UDP data socket aimed at multicast group */
|
||||
if (ok && !dataSocket.IsValid()) {
|
||||
ok = dataSocket.Open();
|
||||
if (ok) {
|
||||
ok = dataSocket.Connect(multicastGroup.Buffer(), dataPort);
|
||||
/* Build the CONFIG payload and cache it in the server so any CONNECT client
|
||||
* receives it immediately. The config is static for the lifetime of this state. */
|
||||
if (ok) {
|
||||
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u + 1u;
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
|
||||
if (cfgBuf != NULL_PTR(uint8 *)) {
|
||||
uint32 cfgPayloadSize = 0u;
|
||||
if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
|
||||
(void) server.SendConfig(cfgBuf, cfgPayloadSize);
|
||||
}
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not connect data socket to %s:%u.",
|
||||
multicastGroup.Buffer(),
|
||||
static_cast<uint32>(dataPort));
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Could not build initial CONFIG payload.");
|
||||
}
|
||||
heap->Free(reinterpret_cast<void *&>(cfgBuf));
|
||||
}
|
||||
}
|
||||
else {
|
||||
/* Unicast mode: existing UDP server socket (idempotent: skip if already valid) */
|
||||
if (!serverSocket.IsValid()) {
|
||||
ok = serverSocket.Open();
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not open server UDP socket.");
|
||||
}
|
||||
if (ok) {
|
||||
ok = serverSocket.Listen(port);
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not bind server socket to port %u.",
|
||||
static_cast<uint32>(port));
|
||||
}
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Could not allocate CONFIG buffer.");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1022,95 +934,10 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
|
||||
bool dataReady = (waitErr == ErrorManagement::NoError);
|
||||
|
||||
/* --- Poll for incoming control commands --- */
|
||||
uint32 hdrSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
|
||||
if (useMulticast) {
|
||||
/* Multicast mode: non-blocking TCP accept + poll existing TCP client */
|
||||
Handle listenFd = tcpListener.GetReadHandle();
|
||||
fd_set rfdL;
|
||||
FD_ZERO(&rfdL);
|
||||
FD_SET(static_cast<int>(listenFd), &rfdL);
|
||||
struct timeval tvL = { 0, 0 };
|
||||
if (select(static_cast<int>(listenFd) + 1, &rfdL,
|
||||
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvL) > 0) {
|
||||
BasicTCPSocket *newClient = tcpListener.WaitConnection(TimeoutType(0u));
|
||||
if (newClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
/* Evict any existing client */
|
||||
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
(void) tcpClient->Close();
|
||||
delete tcpClient;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
clientConnected = false;
|
||||
}
|
||||
tcpClient = newClient;
|
||||
/* Read CONNECT packet from the new TCP connection */
|
||||
uint8 connBuf[sizeof(UDPSPacketHeader)];
|
||||
uint32 recvSize = hdrSize;
|
||||
bool readOk = tcpClient->Read(
|
||||
reinterpret_cast<char8 *>(connBuf), recvSize);
|
||||
if (readOk && (recvSize >= hdrSize)) {
|
||||
HandleTCPConnect(connBuf, recvSize);
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"TCP: incomplete CONNECT (%u bytes); closing.",
|
||||
recvSize);
|
||||
(void) tcpClient->Close();
|
||||
delete tcpClient;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
}
|
||||
}
|
||||
}
|
||||
/* --- Poll for incoming control commands (CONNECT / DISCONNECT / ACK) --- */
|
||||
server.ServiceClients();
|
||||
|
||||
/* Poll existing TCP client for DISCONNECT or closed connection */
|
||||
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
Handle cfd = tcpClient->GetReadHandle();
|
||||
fd_set rfdC;
|
||||
FD_ZERO(&rfdC);
|
||||
FD_SET(static_cast<int>(cfd), &rfdC);
|
||||
struct timeval tvC = { 0, 0 };
|
||||
if (select(static_cast<int>(cfd) + 1, &rfdC,
|
||||
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvC) > 0) {
|
||||
uint8 cmdBuf[256u];
|
||||
uint32 cmdSize = static_cast<uint32>(sizeof(cmdBuf));
|
||||
bool readOk = tcpClient->Read(
|
||||
reinterpret_cast<char8 *>(cmdBuf), cmdSize);
|
||||
if (readOk && (cmdSize >= hdrSize)) {
|
||||
HandleClientCommand(cmdBuf, cmdSize);
|
||||
}
|
||||
else {
|
||||
/* Zero-byte / error = TCP peer closed */
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"TCP client disconnected (read %u bytes).", cmdSize);
|
||||
(void) tcpClient->Close();
|
||||
delete tcpClient;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
clientConnected = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
/* Unicast mode: existing UDP serverSocket non-blocking poll */
|
||||
uint8 cmdBuf[256u];
|
||||
Handle sockFd = serverSocket.GetReadHandle();
|
||||
fd_set rfds;
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(static_cast<int>(sockFd), &rfds);
|
||||
struct timeval tv = { 0, 0 };
|
||||
int nReady = select(static_cast<int>(sockFd) + 1, &rfds,
|
||||
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
|
||||
if (nReady > 0) {
|
||||
uint32 recvSize = static_cast<uint32>(sizeof(cmdBuf));
|
||||
bool received = serverSocket.Read(
|
||||
reinterpret_cast<char8 *>(cmdBuf), recvSize);
|
||||
if (received && (recvSize >= hdrSize)) {
|
||||
HandleClientCommand(cmdBuf, recvSize);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (dataReady && clientConnected) {
|
||||
if (dataReady && server.HasClients()) {
|
||||
/* Synchronise() already gates posting dataSem to the correct rate
|
||||
* (size/time for Accumulate, every-Nth for Decimate, every call for
|
||||
* Strict). Execute() just sends whatever is in the ready buffers. */
|
||||
@@ -1134,8 +961,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
uint32 sendBytes = UDPS_TIMESTAMP_BYTES + 4u +
|
||||
fill * singleCycleWireBytes + fixedWireBytes;
|
||||
packetCounter++;
|
||||
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
|
||||
wireBuffer, sendBytes)) {
|
||||
if (!server.SendData(packetCounter, wireBuffer, sendBytes)) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Failed to send Accumulate DATA packet (counter=%u).",
|
||||
packetCounter);
|
||||
@@ -1154,8 +980,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
QuantizeAndSerialize(scratchBuffer, ts);
|
||||
|
||||
packetCounter++;
|
||||
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
|
||||
wireBuffer, totalWireBytes)) {
|
||||
if (!server.SendData(packetCounter, wireBuffer, totalWireBytes)) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Failed to send DATA packet (counter=%u).",
|
||||
packetCounter);
|
||||
@@ -1165,19 +990,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
}
|
||||
|
||||
if (info.GetStage() == ExecutionInfo::TerminationStage) {
|
||||
if (clientConnected) {
|
||||
if (useMulticast) {
|
||||
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
(void) tcpClient->Close();
|
||||
delete tcpClient;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
}
|
||||
}
|
||||
else {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
clientConnected = false;
|
||||
}
|
||||
(void) server.Stop();
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"UDPStreamer background thread terminated.");
|
||||
}
|
||||
@@ -1331,88 +1144,6 @@ void UDPStreamer::SerializeAccumulated(const uint8 *src,
|
||||
}
|
||||
}
|
||||
|
||||
void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
|
||||
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
|
||||
return;
|
||||
}
|
||||
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
|
||||
if (hdr->magic != UDPS_MAGIC) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (hdr->type == UDPS_TYPE_CONNECT) {
|
||||
if (useMulticast) {
|
||||
/* In multicast mode, CONNECT is handled by HandleTCPConnect(); ignore here */
|
||||
return;
|
||||
}
|
||||
InternetHost src = serverSocket.GetSource();
|
||||
|
||||
/* Disconnect any previous client */
|
||||
if (clientConnected) {
|
||||
if (clientSocket.IsValid()) {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
clientConnected = false;
|
||||
}
|
||||
|
||||
/* Open a new client socket and connect to the requesting address */
|
||||
bool sockOk = clientSocket.Open();
|
||||
if (sockOk) {
|
||||
sockOk = clientSocket.Connect(src.GetAddress().Buffer(), src.GetPort());
|
||||
}
|
||||
if (sockOk) {
|
||||
clientConnected = true;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"Client connected from %s:%u.",
|
||||
src.GetAddress().Buffer(),
|
||||
static_cast<uint32>(src.GetPort()));
|
||||
|
||||
/* Send CONFIG packet */
|
||||
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u + 1u;
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
|
||||
if (cfgBuf != NULL_PTR(uint8 *)) {
|
||||
uint32 cfgPayloadSize = 0u;
|
||||
if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
|
||||
(void) SendFragmented(UDPS_TYPE_CONFIG, 0u, cfgBuf, cfgPayloadSize);
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Could not build CONFIG payload.");
|
||||
}
|
||||
heap->Free(reinterpret_cast<void *&>(cfgBuf));
|
||||
}
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Could not connect to client %s:%u.",
|
||||
src.GetAddress().Buffer(),
|
||||
static_cast<uint32>(src.GetPort()));
|
||||
}
|
||||
}
|
||||
else if (hdr->type == UDPS_TYPE_DISCONNECT) {
|
||||
REPORT_ERROR(ErrorManagement::Information, "Client sent DISCONNECT.");
|
||||
if (clientConnected) {
|
||||
if (!useMulticast) {
|
||||
if (clientSocket.IsValid()) {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
}
|
||||
/* In multicast mode, tcpClient cleanup is done by the Execute() poll caller */
|
||||
clientConnected = false;
|
||||
}
|
||||
}
|
||||
else if (hdr->type == UDPS_TYPE_ACK) {
|
||||
/* Optional: track acknowledged counters for loss detection */
|
||||
if (size >= static_cast<uint32>(sizeof(UDPSPacketHeader)) + 4u) {
|
||||
uint32 ackedCounter = 0u;
|
||||
const uint8 *pl = buf + sizeof(UDPSPacketHeader);
|
||||
(void) MemoryOperationsHelper::Copy(&ackedCounter, pl, 4u);
|
||||
REPORT_ERROR(ErrorManagement::Debug,
|
||||
"ACK received for packet counter %u.", ackedCounter);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool UDPStreamer::BuildConfigPayload(uint8 *buf,
|
||||
uint32 bufSize,
|
||||
@@ -1580,65 +1311,6 @@ void UDPStreamer::QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp) {
|
||||
}
|
||||
}
|
||||
|
||||
bool UDPStreamer::SendFragmented(uint8 type,
|
||||
uint32 counter,
|
||||
const uint8 *payload,
|
||||
uint32 payloadSize) {
|
||||
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
|
||||
uint32 maxChunk = maxPayloadSize - headerSize;
|
||||
uint32 totalFrags = (payloadSize == 0u) ? 1u :
|
||||
((payloadSize + maxChunk - 1u) / maxChunk);
|
||||
|
||||
uint32 sendBufSize = headerSize + maxChunk;
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(sendBufSize));
|
||||
if (sendBuf == NULL_PTR(uint8 *)) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not allocate send buffer (%u bytes).", sendBufSize);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ok = true;
|
||||
uint32 offs = 0u;
|
||||
|
||||
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
|
||||
uint32 chunkSize = payloadSize - offs;
|
||||
if (chunkSize > maxChunk) {
|
||||
chunkSize = maxChunk;
|
||||
}
|
||||
|
||||
UDPSPacketHeader *hdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
|
||||
hdr->magic = UDPS_MAGIC;
|
||||
hdr->type = type;
|
||||
hdr->counter = counter;
|
||||
hdr->fragmentIdx = static_cast<uint16>(f);
|
||||
hdr->totalFragments = static_cast<uint16>(totalFrags);
|
||||
hdr->payloadBytes = chunkSize;
|
||||
|
||||
if (chunkSize > 0u) {
|
||||
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
|
||||
payload + offs,
|
||||
chunkSize);
|
||||
}
|
||||
|
||||
uint32 sendSize = headerSize + chunkSize;
|
||||
if (useMulticast) {
|
||||
ok = dataSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
|
||||
}
|
||||
else {
|
||||
ok = clientSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
|
||||
}
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Fragment %u/%u send failed.", f + 1u, totalFrags);
|
||||
}
|
||||
offs += chunkSize;
|
||||
}
|
||||
|
||||
heap->Free(reinterpret_cast<void *&>(sendBuf));
|
||||
return ok;
|
||||
}
|
||||
|
||||
uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
|
||||
uint8 code = UDPS_TYPECODE_UNKNOWN;
|
||||
if (td == UnsignedInteger8Bit) { code = UDPS_TYPECODE_UINT8; }
|
||||
@@ -1654,78 +1326,6 @@ uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
|
||||
return code;
|
||||
}
|
||||
|
||||
void UDPStreamer::HandleTCPConnect(const uint8 *buf, uint32 size) {
|
||||
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
|
||||
return;
|
||||
}
|
||||
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
|
||||
if (hdr->magic != UDPS_MAGIC) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"TCP CONNECT: bad magic 0x%08X.", hdr->magic);
|
||||
return;
|
||||
}
|
||||
if (hdr->type != UDPS_TYPE_CONNECT) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"TCP CONNECT: unexpected packet type %u.", hdr->type);
|
||||
return;
|
||||
}
|
||||
|
||||
clientConnected = true;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"TCP client connected; DATA will multicast to %s:%u.",
|
||||
multicastGroup.Buffer(), static_cast<uint32>(dataPort));
|
||||
|
||||
/* Build CONFIG payload and send as a single TCP message (no fragmentation needed) */
|
||||
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u;
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
|
||||
if (cfgBuf == NULL_PTR(uint8 *)) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not allocate CONFIG buffer.");
|
||||
clientConnected = false;
|
||||
return;
|
||||
}
|
||||
|
||||
uint32 cfgPayloadSize = 0u;
|
||||
bool built = BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize);
|
||||
if (built) {
|
||||
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
|
||||
uint32 totalSize = headerSize + cfgPayloadSize;
|
||||
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(totalSize));
|
||||
if (sendBuf != NULL_PTR(uint8 *)) {
|
||||
UDPSPacketHeader *outHdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
|
||||
outHdr->magic = UDPS_MAGIC;
|
||||
outHdr->type = UDPS_TYPE_CONFIG;
|
||||
outHdr->counter = 0u;
|
||||
outHdr->fragmentIdx = 0u;
|
||||
outHdr->totalFragments = 1u;
|
||||
outHdr->payloadBytes = cfgPayloadSize;
|
||||
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
|
||||
cfgBuf, cfgPayloadSize);
|
||||
bool writeOk = tcpClient->Write(
|
||||
reinterpret_cast<const char8 *>(sendBuf), totalSize);
|
||||
if (!writeOk) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"TCP: failed to send CONFIG packet.");
|
||||
clientConnected = false;
|
||||
}
|
||||
heap->Free(reinterpret_cast<void *&>(sendBuf));
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not allocate TCP send buffer.");
|
||||
clientConnected = false;
|
||||
}
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"BuildConfigPayload failed; client not fully connected.");
|
||||
clientConnected = false;
|
||||
}
|
||||
|
||||
heap->Free(reinterpret_cast<void *&>(cfgBuf));
|
||||
}
|
||||
|
||||
uint16 UDPStreamer::GetPort() const {
|
||||
return port;
|
||||
}
|
||||
@@ -1735,11 +1335,11 @@ uint32 UDPStreamer::GetMaxPayloadSize() const {
|
||||
}
|
||||
|
||||
bool UDPStreamer::IsClientConnected() const {
|
||||
return clientConnected;
|
||||
return server.HasClients();
|
||||
}
|
||||
|
||||
bool UDPStreamer::IsMulticast() const {
|
||||
return useMulticast;
|
||||
return server.IsMulticast();
|
||||
}
|
||||
|
||||
CLASS_REGISTER(UDPStreamer, "1.0")
|
||||
|
||||
@@ -31,8 +31,6 @@
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Project header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
#include "BasicTCPSocket.h"
|
||||
#include "BasicUDPSocket.h"
|
||||
#include "CompilerTypes.h"
|
||||
#include "EmbeddedServiceMethodBinderI.h"
|
||||
#include "EventSem.h"
|
||||
@@ -40,6 +38,7 @@
|
||||
#include "MemoryDataSourceI.h"
|
||||
#include "SingleThreadService.h"
|
||||
#include "StreamString.h"
|
||||
#include "UDPSServer.h"
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Class declaration */
|
||||
@@ -107,48 +106,6 @@ struct UDPStreamerSignalInfo {
|
||||
bool accumulated; /**< True when this scalar was expanded to flushCount elements in Auto accumulation mode */
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Magic number for UDPStreamer packets: 'UDPS' in little-endian.
|
||||
*/
|
||||
static const uint32 UDPS_MAGIC = 0x53504455u;
|
||||
|
||||
/**
|
||||
* @brief Packet type codes.
|
||||
*/
|
||||
static const uint8 UDPS_TYPE_DATA = 0u; /**< Server → Client: signal data */
|
||||
static const uint8 UDPS_TYPE_CONFIG = 1u; /**< Server → Client: signal configuration */
|
||||
static const uint8 UDPS_TYPE_ACK = 2u; /**< Client → Server: acknowledge counter */
|
||||
static const uint8 UDPS_TYPE_CONNECT = 3u; /**< Client → Server: connect request */
|
||||
static const uint8 UDPS_TYPE_DISCONNECT = 4u; /**< Client → Server: disconnect */
|
||||
|
||||
/**
|
||||
* @brief Wire packet header (17 bytes, packed).
|
||||
*/
|
||||
struct UDPSPacketHeader {
|
||||
uint32 magic; /**< Must equal UDPS_MAGIC */
|
||||
uint8 type; /**< One of UDPS_TYPE_* */
|
||||
uint32 counter; /**< Sequence counter (per data update, not per fragment) */
|
||||
uint16 fragmentIdx; /**< 0-based index of this fragment */
|
||||
uint16 totalFragments; /**< Total fragments for this update (1 = no fragmentation) */
|
||||
uint32 payloadBytes; /**< Bytes of payload immediately following this header */
|
||||
} __attribute__((packed));
|
||||
|
||||
/**
|
||||
* @brief Signal type codes transmitted in the CONFIG packet.
|
||||
* These are simplified type codes independent of MARTe2 internals.
|
||||
*/
|
||||
static const uint8 UDPS_TYPECODE_UINT8 = 0u;
|
||||
static const uint8 UDPS_TYPECODE_INT8 = 1u;
|
||||
static const uint8 UDPS_TYPECODE_UINT16 = 2u;
|
||||
static const uint8 UDPS_TYPECODE_INT16 = 3u;
|
||||
static const uint8 UDPS_TYPECODE_UINT32 = 4u;
|
||||
static const uint8 UDPS_TYPECODE_INT32 = 5u;
|
||||
static const uint8 UDPS_TYPECODE_UINT64 = 6u;
|
||||
static const uint8 UDPS_TYPECODE_INT64 = 7u;
|
||||
static const uint8 UDPS_TYPECODE_FLOAT32 = 8u;
|
||||
static const uint8 UDPS_TYPECODE_FLOAT64 = 9u;
|
||||
static const uint8 UDPS_TYPECODE_UNKNOWN = 255u;
|
||||
|
||||
/**
|
||||
* @brief A DataSource that streams MARTe2 signals to UDP clients.
|
||||
*
|
||||
@@ -378,23 +335,6 @@ private:
|
||||
*/
|
||||
void QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp);
|
||||
|
||||
/**
|
||||
* @brief Sends payload as one or more fragmented UDP datagrams to the connected client.
|
||||
* @return true if all datagrams were sent without error.
|
||||
*/
|
||||
bool SendFragmented(uint8 type, uint32 counter, const uint8 *payload, uint32 payloadSize);
|
||||
|
||||
/**
|
||||
* @brief Handles a single received command packet from a client (unicast mode).
|
||||
*/
|
||||
void HandleClientCommand(const uint8 *buf, uint32 size);
|
||||
|
||||
/**
|
||||
* @brief Handles a CONNECT received on the TCP control socket (multicast mode).
|
||||
* @details Validates magic+type, sets clientConnected, builds and sends CONFIG via tcpClient.
|
||||
*/
|
||||
void HandleTCPConnect(const uint8 *buf, uint32 size);
|
||||
|
||||
/**
|
||||
* @brief Serializes an accumulated batch into wireBuffer.
|
||||
* @param src Flat buffer [numSamples × totalSrcBytes], slot 0 = oldest.
|
||||
@@ -447,18 +387,8 @@ private:
|
||||
uint8 *wireBuffer; /**< Pre-allocated buffer for the serialized DATA payload */
|
||||
uint32 totalWireBytes; /**< Total bytes of all signals after quantization + 8-byte timestamp prefix */
|
||||
|
||||
/* Networking — unicast mode */
|
||||
BasicUDPSocket serverSocket; /**< Bound to port; receives client commands (unicast) */
|
||||
BasicUDPSocket clientSocket; /**< Configured to send to the connected client (unicast) */
|
||||
volatile bool clientConnected; /**< True when a client has successfully connected */
|
||||
|
||||
/* Networking — multicast mode (only used when useMulticast == true) */
|
||||
StreamString multicastGroup; /**< Multicast group IP, e.g. "239.0.0.1"; empty = unicast */
|
||||
uint16 dataPort; /**< UDP port for DATA datagrams in multicast mode */
|
||||
bool useMulticast; /**< Derived from multicastGroup.Size() > 0 in Initialise() */
|
||||
BasicTCPSocket tcpListener; /**< TCP server socket: accepts control connections */
|
||||
BasicTCPSocket *tcpClient; /**< Accepted TCP control connection (heap by WaitConnection) */
|
||||
BasicUDPSocket dataSocket; /**< UDP socket aimed at multicastGroup:dataPort */
|
||||
/* Networking — handled by UDPSServer */
|
||||
UDPSServer server; /**< Multi-client session manager (unicast and multicast) */
|
||||
|
||||
/* Thread management */
|
||||
SingleThreadService executor; /**< Background thread service */
|
||||
|
||||
@@ -77,15 +77,8 @@
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/IOBuffer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/CharBuffer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StreamI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/Object.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StringHelper.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/StructuredDataI.h \
|
||||
@@ -93,14 +86,6 @@
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/CLASSREGISTER.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ClassRegistryItemT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ObjectBuilderT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabaseNode.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Fnv1aHashFunction.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/HashFunction.h \
|
||||
@@ -121,10 +106,33 @@
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Vector.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamString.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamStringIOBuffer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/StatefulI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/ExecutableI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/Sleep.h \
|
||||
UDPStreamer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h \
|
||||
../../../..//Source/Components/Interfaces/UDPStream/UDPSServer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicTCPSocket.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicSocket.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
|
||||
@@ -133,10 +141,5 @@
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HandleI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/Environment/Linux/SocketCore.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicUDPSocket.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
|
||||
../../../..//Common/UDP/UDPSProtocol.h
|
||||
|
||||
@@ -77,15 +77,8 @@ UDPStreamer.o: UDPStreamer.cpp \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/IOBuffer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/CharBuffer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StreamI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/Object.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StringHelper.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/StructuredDataI.h \
|
||||
@@ -93,14 +86,6 @@ UDPStreamer.o: UDPStreamer.cpp \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/CLASSREGISTER.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ClassRegistryItemT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ObjectBuilderT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabaseNode.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Fnv1aHashFunction.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/HashFunction.h \
|
||||
@@ -121,10 +106,33 @@ UDPStreamer.o: UDPStreamer.cpp \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Vector.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamString.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamStringIOBuffer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/StatefulI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/ExecutableI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/Sleep.h \
|
||||
UDPStreamer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h \
|
||||
../../../..//Source/Components/Interfaces/UDPStream/UDPSServer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicTCPSocket.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicSocket.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
|
||||
@@ -133,10 +141,5 @@ UDPStreamer.o: UDPStreamer.cpp \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HandleI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/Environment/Linux/SocketCore.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicUDPSocket.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
|
||||
../../../..//Common/UDP/UDPSProtocol.h
|
||||
|
||||
@@ -71,12 +71,12 @@ bool TimeArrayGAM::Setup() {
|
||||
uint32 sz = 0u;
|
||||
ok = GetSignalByteSize(OutputSignals, 0u, sz);
|
||||
if (ok) {
|
||||
nElements = sz / static_cast<uint32>(sizeof(uint32));
|
||||
nElements = sz / static_cast<uint32>(sizeof(uint64));
|
||||
ok = (nElements > 0u);
|
||||
}
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::InitialisationError,
|
||||
"TimeArrayGAM: output signal must be a non-empty uint32 array.");
|
||||
"TimeArrayGAM: output signal must be a non-empty uint64 array.");
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
@@ -61,7 +61,7 @@ DebugService::~DebugService() {
|
||||
threadService.Stop();
|
||||
streamerService.Stop();
|
||||
tcpServer.Close();
|
||||
udpSocket.Close();
|
||||
(void) udpsServer.Stop();
|
||||
if (activeClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
activeClient->Close();
|
||||
delete activeClient;
|
||||
@@ -136,10 +136,16 @@ bool DebugService::Initialise(StructuredDataI &data) {
|
||||
|
||||
if (!tcpServer.Open()) return false;
|
||||
if (!tcpServer.Listen(controlPort)) return false;
|
||||
if (!udpSocket.Open()) return false;
|
||||
// Note: do NOT bind udpSocket to streamPort here. The Go client
|
||||
// must own that port to receive streamed data. The socket sends
|
||||
// via an ephemeral source port, which is fine for UDP.
|
||||
|
||||
// Initialise UDP streamer in push-only mode (port=0 skips serverSocket bind)
|
||||
ConfigurationDatabase udpsConfig;
|
||||
(void)udpsConfig.Write("Port", 0u);
|
||||
(void)udpsConfig.Write("MaxPayloadSize", static_cast<uint32>(UDPSServer::UDPS_SERVER_DEFAULT_MAX_PAYLOAD));
|
||||
if (!udpsServer.Initialise(udpsConfig)) return false;
|
||||
if (!udpsServer.Start()) return false;
|
||||
if (streamPort > 0u) {
|
||||
(void)udpsServer.AddStaticClient(streamIP.Buffer(), streamPort);
|
||||
}
|
||||
|
||||
if (threadService.Start() != ErrorManagement::NoError) return false;
|
||||
if (streamerService.Start() != ErrorManagement::NoError) return false;
|
||||
@@ -416,11 +422,10 @@ ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
// Set UDP destination
|
||||
InternetHost dest(streamPort, streamIP.Buffer());
|
||||
(void)udpSocket.SetDestination(dest);
|
||||
// a) Poll for new CONNECT clients (no-op in push-only mode with port=0)
|
||||
udpsServer.ServiceClients();
|
||||
|
||||
// a) If config has changed, rebuild and send CONFIG packet
|
||||
// b) If config has changed, rebuild and send CONFIG packet
|
||||
if (udpsConfigPending) {
|
||||
SendUDPSConfig();
|
||||
udpsConfigPending = false;
|
||||
@@ -449,12 +454,12 @@ ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
|
||||
}
|
||||
}
|
||||
|
||||
// c) If we have data, stamp with HRT and send
|
||||
// c) If we have data, stamp with HRT and send via udpsServer
|
||||
if (anyData && udpsNumSlots > 0u && udpsDataPayload != NULL_PTR(uint8 *)) {
|
||||
uint64 hrt = HighResolutionTimer::Counter();
|
||||
memcpy(udpsDataPayload, &hrt, 8u);
|
||||
SendUDPSFragmented(UDPS_TYPE_DATA, udpsDataPayload, udpsDataPayloadSize);
|
||||
udpsPacketCounter++;
|
||||
(void)udpsServer.SendData(udpsPacketCounter, udpsDataPayload, udpsDataPayloadSize);
|
||||
}
|
||||
|
||||
if (!anyData) {
|
||||
@@ -480,8 +485,10 @@ bool DebugService::SendUDPSConfig() {
|
||||
}
|
||||
}
|
||||
|
||||
// Build CONFIG payload into udpsTxBuf starting at UDPS_HEADER_SIZE offset
|
||||
uint8 *payload = udpsTxBuf + UDPS_HEADER_SIZE;
|
||||
// Build CONFIG payload into a local stack buffer
|
||||
static const uint32 CFG_BUF_SIZE = 65535u;
|
||||
uint8 cfgBuf[CFG_BUF_SIZE];
|
||||
uint8 *payload = cfgBuf;
|
||||
|
||||
// Write numTraced (uint32 LE)
|
||||
memcpy(payload, &tracedCount, 4u);
|
||||
@@ -521,7 +528,7 @@ bool DebugService::SendUDPSConfig() {
|
||||
uint32 wireSize = UDPSTypeCodeByteSize(typeCode) * numElements;
|
||||
|
||||
// Write signal descriptor (136 bytes) to payload
|
||||
if (payloadOffset + UDPS_SIGNAL_DESC_SIZE <= sizeof(udpsTxBuf) - UDPS_HEADER_SIZE - 1u) {
|
||||
if (payloadOffset + UDPS_SIGNAL_DESC_SIZE <= CFG_BUF_SIZE - 1u) {
|
||||
UDPSSignalDescriptor *desc = reinterpret_cast<UDPSSignalDescriptor *>(payload + payloadOffset);
|
||||
memset(desc, 0, UDPS_SIGNAL_DESC_SIZE);
|
||||
if (signals[i]->name.Size() > 0u) {
|
||||
@@ -554,7 +561,7 @@ bool DebugService::SendUDPSConfig() {
|
||||
}
|
||||
|
||||
// Write publish mode byte
|
||||
if (payloadOffset < sizeof(udpsTxBuf) - UDPS_HEADER_SIZE) {
|
||||
if (payloadOffset < CFG_BUF_SIZE) {
|
||||
payload[payloadOffset] = UDPS_PUBLISH_STRICT;
|
||||
payloadOffset++;
|
||||
}
|
||||
@@ -574,45 +581,11 @@ bool DebugService::SendUDPSConfig() {
|
||||
memset(udpsDataPayload, 0, udpsDataPayloadSize);
|
||||
}
|
||||
|
||||
// Send CONFIG packet
|
||||
SendUDPSFragmented(UDPS_TYPE_CONFIG, payload, payloadOffset);
|
||||
// Send CONFIG packet (also cached in udpsServer for new CONNECT clients)
|
||||
udpsPacketCounter++;
|
||||
(void)udpsServer.SendConfig(payload, payloadOffset);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// SendUDPSFragmented — fragment and send a UDPS packet
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool DebugService::SendUDPSFragmented(uint8 type, const uint8 *payload, uint32 payloadSize) {
|
||||
if (payloadSize <= UDPS_MAX_PAYLOAD) {
|
||||
// Single datagram
|
||||
UDPSBuildHeader(udpsTxBuf, type, udpsPacketCounter, 0u, 1u, payloadSize);
|
||||
if (payload != (udpsTxBuf + UDPS_HEADER_SIZE)) {
|
||||
memcpy(udpsTxBuf + UDPS_HEADER_SIZE, payload, payloadSize);
|
||||
}
|
||||
uint32 toWrite = UDPS_HEADER_SIZE + payloadSize;
|
||||
(void)udpSocket.Write(reinterpret_cast<char8 *>(udpsTxBuf), toWrite);
|
||||
} else {
|
||||
// Fragmented
|
||||
uint32 numFrags = (payloadSize + UDPS_MAX_PAYLOAD - 1u) / UDPS_MAX_PAYLOAD;
|
||||
uint32 offset = 0u;
|
||||
for (uint32 i = 0u; i < numFrags; i++) {
|
||||
uint32 chunkSize = UDPS_MAX_PAYLOAD;
|
||||
if (offset + chunkSize > payloadSize) {
|
||||
chunkSize = payloadSize - offset;
|
||||
}
|
||||
UDPSBuildHeader(udpsTxBuf, type, udpsPacketCounter,
|
||||
static_cast<uint16>(i),
|
||||
static_cast<uint16>(numFrags),
|
||||
chunkSize);
|
||||
memcpy(udpsTxBuf + UDPS_HEADER_SIZE, payload + offset, chunkSize);
|
||||
uint32 toWrite = UDPS_HEADER_SIZE + chunkSize;
|
||||
(void)udpSocket.Write(reinterpret_cast<char8 *>(udpsTxBuf), toWrite);
|
||||
offset += chunkSize;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace MARTe
|
||||
|
||||
@@ -2,13 +2,12 @@
|
||||
#define DEBUGSERVICE_H
|
||||
|
||||
#include "BasicTCPSocket.h"
|
||||
#include "BasicUDPSocket.h"
|
||||
#include "DebugServiceBase.h"
|
||||
#include "EmbeddedServiceMethodBinderI.h"
|
||||
#include "MessageI.h"
|
||||
#include "ReferenceT.h"
|
||||
#include "SingleThreadService.h"
|
||||
#include "UDPSProtocol.h"
|
||||
#include "UDPSServer.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
@@ -67,15 +66,6 @@ private:
|
||||
*/
|
||||
bool SendUDPSConfig();
|
||||
|
||||
/**
|
||||
* @brief Fragment and send a payload as one or more UDPS datagrams.
|
||||
* @param type Packet type (UDPS_TYPE_CONFIG or UDPS_TYPE_DATA).
|
||||
* @param payload Pointer to fully-built payload.
|
||||
* @param payloadSize Byte count of payload.
|
||||
* @return true if all datagrams were written without error.
|
||||
*/
|
||||
bool SendUDPSFragmented(uint8 type, const uint8 *payload, uint32 payloadSize);
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// TCP/UDP transport configuration
|
||||
// -----------------------------------------------------------------------
|
||||
@@ -87,7 +77,7 @@ private:
|
||||
bool suppressTimeoutLogs;
|
||||
|
||||
BasicTCPSocket tcpServer;
|
||||
BasicUDPSocket udpSocket;
|
||||
UDPSServer udpsServer; ///< Handles fragmentation and multi-client sending
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// Server-thread helper class
|
||||
@@ -165,9 +155,6 @@ private:
|
||||
/** Staging buffer: a single sample popped from traceBuffer. */
|
||||
uint8 udpsSampleBuf[65535u];
|
||||
|
||||
/** General-purpose TX buffer for CONFIG and DATA packet assembly. */
|
||||
uint8 udpsTxBuf[65535u];
|
||||
|
||||
/** Packet sequence counter (incremented per DATA/CONFIG datagram group). */
|
||||
uint32 udpsPacketCounter;
|
||||
|
||||
|
||||
@@ -9,9 +9,12 @@ ROOT_DIR=../../../../
|
||||
MAKEDEFAULTDIR=$(MARTe2_DIR)/MakeDefaults
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
|
||||
|
||||
# Shared UDPS protocol header (from Common/)
|
||||
# Shared UDPS protocol header (from Common/) and UDPStream library
|
||||
INCLUDES += -I$(ROOT_DIR)/Common/UDP
|
||||
INCLUDES += -I$(ROOT_DIR)/Source/Components/Interfaces/UDPStream
|
||||
INCLUDES += -I$(ROOT_DIR)/Source/Components/Interfaces/TCPLogger
|
||||
|
||||
LIBRARIES += -L$(BUILD_DIR)/../UDPStream -lUDPStream
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L0Types
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L2Objects
|
||||
|
||||
@@ -1,9 +1,11 @@
|
||||
all:
|
||||
$(MAKE) -C TCPLogger -f Makefile.gcc
|
||||
$(MAKE) -C UDPStream -f Makefile.gcc
|
||||
$(MAKE) -C DebugService -f Makefile.gcc
|
||||
|
||||
clean:
|
||||
$(MAKE) -C TCPLogger -f Makefile.gcc clean
|
||||
$(MAKE) -C UDPStream -f Makefile.gcc clean
|
||||
$(MAKE) -C DebugService -f Makefile.gcc clean
|
||||
|
||||
.PHONY: all clean
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
include Makefile.inc
|
||||
@@ -0,0 +1,30 @@
|
||||
#############################################################
|
||||
# MARTe2 Integrated Components — UDPStream
|
||||
#############################################################
|
||||
OBJSX=UDPSServer.x UDPSClient.x
|
||||
|
||||
PACKAGE=Components/Interfaces
|
||||
|
||||
ROOT_DIR=../../../../
|
||||
MAKEDEFAULTDIR=$(MARTe2_DIR)/MakeDefaults
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
|
||||
|
||||
INCLUDES += -I.
|
||||
INCLUDES += -I$(ROOT_DIR)/Common/UDP
|
||||
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/L4Messages
|
||||
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
|
||||
|
||||
all: $(OBJS) $(SUBPROJ) \
|
||||
$(BUILD_DIR)/UDPStream$(LIBEXT) \
|
||||
$(BUILD_DIR)/UDPStream$(DLLEXT)
|
||||
echo $(OBJS)
|
||||
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibRules.$(TARGET)
|
||||
@@ -0,0 +1,693 @@
|
||||
/**
|
||||
* @file UDPSClient.cpp
|
||||
* @brief Implementation of UDPSClient — auto-reconnecting UDPS receiver.
|
||||
*/
|
||||
|
||||
#include "UDPSClient.h"
|
||||
|
||||
#include "AdvancedErrorManagement.h"
|
||||
#include "MemoryOperationsHelper.h"
|
||||
|
||||
#include <sys/select.h>
|
||||
#include <errno.h>
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Constructor / Destructor
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
UDPSClient::UDPSClient()
|
||||
: serverPort(0u),
|
||||
dataPort(0u),
|
||||
useMulticast(false),
|
||||
silenceTimeoutTicks(0u),
|
||||
reconnectDelayTicks(0u),
|
||||
maxPayloadSize(UDPS_CLIENT_DEFAULT_MAX_PAYLOAD),
|
||||
cpuMask(0xFFFFFFFFu),
|
||||
stackSize(65536u),
|
||||
listener(NULL_PTR(UDPSClientListener *)),
|
||||
threadService(*this),
|
||||
connected(false),
|
||||
lastDataTicks(0u),
|
||||
disconnectTick(0u),
|
||||
localPort(0u),
|
||||
lastGcTicks(0u) {
|
||||
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
reassemblySlots[i].counter = 0u;
|
||||
reassemblySlots[i].type = 0u;
|
||||
reassemblySlots[i].totalFragments = 0u;
|
||||
reassemblySlots[i].receivedFragments = 0u;
|
||||
reassemblySlots[i].active = false;
|
||||
reassemblySlots[i].firstSeenTicks = 0u;
|
||||
reassemblySlots[i].chunkSize = 0u;
|
||||
(void) MemoryOperationsHelper::Set(reassemblySlots[i].recvMask, 0, 32u);
|
||||
}
|
||||
}
|
||||
|
||||
UDPSClient::~UDPSClient() {
|
||||
(void) Stop();
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Initialise
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSClient::Initialise(StructuredDataI &data) {
|
||||
StreamString saddr;
|
||||
if (!data.Read("ServerAddr", saddr) || (saddr.Size() == 0u)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
|
||||
"UDPSClient: ServerAddr not specified.");
|
||||
return false;
|
||||
}
|
||||
serverAddr = saddr;
|
||||
|
||||
uint32 portU32 = 0u;
|
||||
if (!data.Read("Port", portU32)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
|
||||
"UDPSClient: Port not specified.");
|
||||
return false;
|
||||
}
|
||||
serverPort = static_cast<uint16>(portU32);
|
||||
|
||||
StreamString mcGroup;
|
||||
if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
|
||||
multicastGroup = mcGroup;
|
||||
useMulticast = true;
|
||||
}
|
||||
|
||||
if (useMulticast) {
|
||||
uint32 dpU32 = static_cast<uint32>(serverPort) + 1u;
|
||||
(void) data.Read("DataPort", dpU32);
|
||||
dataPort = static_cast<uint16>(dpU32);
|
||||
}
|
||||
|
||||
uint32 silenceS = UDPS_CLIENT_DEFAULT_SILENCE_TIMEOUT_S;
|
||||
(void) data.Read("SilenceTimeout", silenceS);
|
||||
silenceTimeoutTicks = static_cast<uint64>(silenceS) * HighResolutionTimer::Frequency();
|
||||
|
||||
uint32 reconnectS = UDPS_CLIENT_DEFAULT_RECONNECT_DELAY_S;
|
||||
(void) data.Read("ReconnectDelay", reconnectS);
|
||||
reconnectDelayTicks = static_cast<uint64>(reconnectS) * HighResolutionTimer::Frequency();
|
||||
|
||||
uint32 mps = UDPS_CLIENT_DEFAULT_MAX_PAYLOAD;
|
||||
(void) data.Read("MaxPayloadSize", mps);
|
||||
maxPayloadSize = mps;
|
||||
|
||||
(void) data.Read("CPUMask", cpuMask);
|
||||
(void) data.Read("StackSize", stackSize);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// SetListener / Start / Stop
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSClient::SetListener(UDPSClientListener *l) {
|
||||
listener = l;
|
||||
}
|
||||
|
||||
bool UDPSClient::Start() {
|
||||
threadService.SetCPUMask(cpuMask);
|
||||
threadService.SetStackSize(stackSize);
|
||||
ErrorManagement::ErrorType err = threadService.Start();
|
||||
return (err == ErrorManagement::NoError);
|
||||
}
|
||||
|
||||
bool UDPSClient::Stop() {
|
||||
ErrorManagement::ErrorType err = threadService.Stop();
|
||||
Disconnect();
|
||||
return (err == ErrorManagement::NoError);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Execute (thread entry)
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
ErrorManagement::ErrorType UDPSClient::Execute(ExecutionInfo &info) {
|
||||
if (info.GetStage() == ExecutionInfo::StartupStage) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSClient: Receive thread started.");
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
if (info.GetStage() == ExecutionInfo::TerminationStage) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSClient: Receive thread stopping.");
|
||||
Disconnect();
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
// MainStage
|
||||
uint64 now = HighResolutionTimer::Counter();
|
||||
|
||||
if (!connected) {
|
||||
// Wait reconnectDelay before retrying
|
||||
if ((disconnectTick == 0u) ||
|
||||
((now - disconnectTick) >= reconnectDelayTicks)) {
|
||||
if (!Connect()) {
|
||||
disconnectTick = HighResolutionTimer::Counter();
|
||||
}
|
||||
}
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
// Connected: receive + process
|
||||
bool ok = ReceiveAndProcess();
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Receive error; disconnecting.");
|
||||
Disconnect();
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
/* Re-sample time AFTER ReceiveAndProcess() so that lastDataTicks (which may
|
||||
* have been updated inside ReceiveAndProcess) is never newer than `now`.
|
||||
* Without this, if a packet arrives during ReceiveAndProcess(), `now` (captured
|
||||
* before the call) < lastDataTicks, causing unsigned wraparound in the
|
||||
* subtraction and a spurious silence timeout. */
|
||||
now = HighResolutionTimer::Counter();
|
||||
|
||||
// Check silence timeout
|
||||
if (silenceTimeoutTicks > 0u) {
|
||||
uint64 lastSeen = lastDataTicks;
|
||||
if ((lastSeen > 0u) && ((now - lastSeen) >= silenceTimeoutTicks)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Server silent; disconnecting.");
|
||||
Disconnect();
|
||||
}
|
||||
}
|
||||
|
||||
// Periodic GC of stale reassembly slots (~every 1 s)
|
||||
uint64 gcFreq = HighResolutionTimer::Frequency();
|
||||
if ((now - lastGcTicks) >= gcFreq) {
|
||||
GcReassemblySlots();
|
||||
lastGcTicks = now;
|
||||
}
|
||||
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: Connect
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSClient::Connect() {
|
||||
bool ok = useMulticast ? ConnectMulticast() : ConnectUnicast();
|
||||
if (ok) {
|
||||
connected = true;
|
||||
lastDataTicks = HighResolutionTimer::Counter();
|
||||
if (listener != NULL_PTR(UDPSClientListener *)) {
|
||||
listener->OnUDPSConnected();
|
||||
}
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSClient: Connected to %s:%u.",
|
||||
serverAddr.Buffer(), static_cast<uint32>(serverPort));
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool UDPSClient::ConnectUnicast() {
|
||||
// Open a local UDP socket bound to an ephemeral port
|
||||
if (!recvSocket.Open()) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Could not open receive socket.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!recvSocket.Listen(0u)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Could not bind receive socket.");
|
||||
(void) recvSocket.Close();
|
||||
return false;
|
||||
}
|
||||
|
||||
// Build and send CONNECT packet to server
|
||||
uint8 connectPkt[UDPS_HEADER_SIZE];
|
||||
UDPSBuildHeader(connectPkt, UDPS_TYPE_CONNECT, 0u, 0u, 1u, 0u);
|
||||
|
||||
InternetHost serverDest(serverPort, serverAddr.Buffer());
|
||||
(void) recvSocket.SetDestination(serverDest);
|
||||
uint32 sendSize = UDPS_HEADER_SIZE;
|
||||
bool ok = recvSocket.Write(reinterpret_cast<const char8 *>(connectPkt), sendSize);
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Could not send CONNECT to %s:%u.",
|
||||
serverAddr.Buffer(), static_cast<uint32>(serverPort));
|
||||
(void) recvSocket.Close();
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UDPSClient::ConnectMulticast() {
|
||||
/* Join the multicast group BEFORE sending CONNECT over TCP.
|
||||
* The UDPStreamer broadcasts CONFIG via multicast immediately when it
|
||||
* receives a CONNECT packet. If the multicast socket is not yet joined,
|
||||
* that CONFIG packet is dropped by the kernel and the session never becomes
|
||||
* configured. Correct order: join → CONNECT → receive CONFIG. */
|
||||
|
||||
// Open and bind the multicast receive socket first
|
||||
if (!mcastSocket.Open()) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Could not open multicast socket.");
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ok = mcastSocket.Listen(dataPort);
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Could not bind multicast socket on port %u.",
|
||||
static_cast<uint32>(dataPort));
|
||||
(void) mcastSocket.Close();
|
||||
return false;
|
||||
}
|
||||
|
||||
ok = mcastSocket.Join(multicastGroup.Buffer());
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Could not join multicast group %s.",
|
||||
multicastGroup.Buffer());
|
||||
(void) mcastSocket.Close();
|
||||
return false;
|
||||
}
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSClient: Joined multicast group %s on port %u.",
|
||||
multicastGroup.Buffer(), static_cast<uint32>(dataPort));
|
||||
|
||||
// Now open the TCP control connection and announce ourselves
|
||||
if (!tcpSocket.Open()) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Could not open TCP socket.");
|
||||
(void) mcastSocket.Close();
|
||||
return false;
|
||||
}
|
||||
|
||||
ok = tcpSocket.Connect(serverAddr.Buffer(), serverPort);
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: TCP connect to %s:%u failed.",
|
||||
serverAddr.Buffer(), static_cast<uint32>(serverPort));
|
||||
(void) tcpSocket.Close();
|
||||
(void) mcastSocket.Close();
|
||||
return false;
|
||||
}
|
||||
|
||||
// Send CONNECT — UDPStreamer will now multicast CONFIG, which we are ready to receive
|
||||
uint8 connectPkt[UDPS_HEADER_SIZE];
|
||||
UDPSBuildHeader(connectPkt, UDPS_TYPE_CONNECT, 0u, 0u, 1u, 0u);
|
||||
uint32 sendSize = UDPS_HEADER_SIZE;
|
||||
ok = tcpSocket.Write(reinterpret_cast<const char8 *>(connectPkt), sendSize);
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Could not send CONNECT over TCP.");
|
||||
(void) tcpSocket.Close();
|
||||
(void) mcastSocket.Close();
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: Disconnect
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSClient::Disconnect() {
|
||||
if (!connected) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Send DISCONNECT
|
||||
if (useMulticast) {
|
||||
if (tcpSocket.IsValid()) {
|
||||
uint8 disconnPkt[UDPS_HEADER_SIZE];
|
||||
UDPSBuildHeader(disconnPkt, UDPS_TYPE_DISCONNECT, 0u, 0u, 1u, 0u);
|
||||
uint32 sendSize = UDPS_HEADER_SIZE;
|
||||
(void) tcpSocket.Write(reinterpret_cast<const char8 *>(disconnPkt), sendSize);
|
||||
(void) tcpSocket.Close();
|
||||
}
|
||||
if (mcastSocket.IsValid()) {
|
||||
(void) mcastSocket.Close();
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (recvSocket.IsValid()) {
|
||||
uint8 disconnPkt[UDPS_HEADER_SIZE];
|
||||
UDPSBuildHeader(disconnPkt, UDPS_TYPE_DISCONNECT, 0u, 0u, 1u, 0u);
|
||||
InternetHost serverDest(serverPort, serverAddr.Buffer());
|
||||
(void) recvSocket.SetDestination(serverDest);
|
||||
uint32 sendSize = UDPS_HEADER_SIZE;
|
||||
(void) recvSocket.Write(reinterpret_cast<const char8 *>(disconnPkt), sendSize);
|
||||
(void) recvSocket.Close();
|
||||
}
|
||||
}
|
||||
|
||||
connected = false;
|
||||
disconnectTick = HighResolutionTimer::Counter();
|
||||
|
||||
if (listener != NULL_PTR(UDPSClientListener *)) {
|
||||
listener->OnUDPSDisconnected();
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: ReceiveAndProcess
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSClient::ReceiveAndProcess() {
|
||||
// Select the receive socket(s)
|
||||
int fd = -1;
|
||||
int tcpFd = -1;
|
||||
if (useMulticast) {
|
||||
fd = mcastSocket.GetReadHandle();
|
||||
/* In multicast mode the server delivers CONFIG over the TCP control
|
||||
* connection — it must be polled too, otherwise the session never
|
||||
* becomes configured. */
|
||||
tcpFd = tcpSocket.GetReadHandle();
|
||||
}
|
||||
else {
|
||||
fd = recvSocket.GetReadHandle();
|
||||
}
|
||||
if (fd < 0) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: ReceiveAndProcess: socket fd < 0 (socket invalid).");
|
||||
return false;
|
||||
}
|
||||
|
||||
fd_set rset;
|
||||
FD_ZERO(&rset);
|
||||
FD_SET(fd, &rset);
|
||||
int maxFd = fd;
|
||||
if (tcpFd >= 0) {
|
||||
FD_SET(tcpFd, &rset);
|
||||
if (tcpFd > maxFd) {
|
||||
maxFd = tcpFd;
|
||||
}
|
||||
}
|
||||
// 10 ms timeout so Execute() doesn't busy-spin
|
||||
struct timeval tv;
|
||||
tv.tv_sec = 0; tv.tv_usec = 10000;
|
||||
int nready = select(maxFd + 1, &rset, NULL, NULL, &tv);
|
||||
if (nready < 0) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: ReceiveAndProcess: select() returned %d (errno=%d).",
|
||||
nready, errno);
|
||||
return false; // socket error
|
||||
}
|
||||
if (nready == 0) {
|
||||
return true; // timeout, no data
|
||||
}
|
||||
|
||||
// TCP control frame (multicast CONFIG path)
|
||||
if ((tcpFd >= 0) && FD_ISSET(tcpFd, &rset)) {
|
||||
if (!ReceiveTCPFrame()) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: TCP control connection lost.");
|
||||
return false;
|
||||
}
|
||||
lastDataTicks = HighResolutionTimer::Counter();
|
||||
}
|
||||
|
||||
if (!FD_ISSET(fd, &rset)) {
|
||||
return true; // only the TCP socket was readable
|
||||
}
|
||||
|
||||
uint32 recvSize = static_cast<uint32>(sizeof(recvBuf));
|
||||
bool ok;
|
||||
if (useMulticast) {
|
||||
ok = mcastSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
|
||||
}
|
||||
else {
|
||||
ok = recvSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
|
||||
}
|
||||
|
||||
if (!ok || (recvSize < UDPS_HEADER_SIZE)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: ReceiveAndProcess: Read() failed or short packet "
|
||||
"(ok=%s, recvSize=%u, HEADER_SIZE=%u).",
|
||||
ok ? "true" : "false", recvSize, UDPS_HEADER_SIZE);
|
||||
return false;
|
||||
}
|
||||
|
||||
lastDataTicks = HighResolutionTimer::Counter();
|
||||
ProcessDatagram(recvBuf, recvSize);
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: ReceiveTCPFrame / ReadExactTCP
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSClient::ReceiveTCPFrame() {
|
||||
/* TCP is a byte stream: read exactly one UDPS frame (17-byte header
|
||||
* followed by payloadBytes of payload) and hand it to ProcessDatagram.
|
||||
* Fragmented CONFIGs arrive as consecutive frames and go through the
|
||||
* normal reassembly path. */
|
||||
if (!ReadExactTCP(recvBuf, UDPS_HEADER_SIZE)) {
|
||||
return false;
|
||||
}
|
||||
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(recvBuf);
|
||||
if (hdr->magic != UDPS_MAGIC) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: TCP stream desynchronised (bad magic).");
|
||||
return false;
|
||||
}
|
||||
uint32 payloadBytes = hdr->payloadBytes;
|
||||
if (payloadBytes > (static_cast<uint32>(sizeof(recvBuf)) - UDPS_HEADER_SIZE)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: TCP frame payload too large (%u).", payloadBytes);
|
||||
return false;
|
||||
}
|
||||
if (payloadBytes > 0u) {
|
||||
if (!ReadExactTCP(&recvBuf[UDPS_HEADER_SIZE], payloadBytes)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
ProcessDatagram(recvBuf, UDPS_HEADER_SIZE + payloadBytes);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UDPSClient::ReadExactTCP(uint8 *dst, uint32 n) {
|
||||
uint32 got = 0u;
|
||||
while (got < n) {
|
||||
uint32 chunk = n - got;
|
||||
if (!tcpSocket.Read(reinterpret_cast<char8 *>(&dst[got]), chunk)) {
|
||||
return false;
|
||||
}
|
||||
if (chunk == 0u) {
|
||||
return false; // orderly close
|
||||
}
|
||||
got += chunk;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: ProcessDatagram
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSClient::ProcessDatagram(const uint8 *buf, uint32 size) {
|
||||
if (size < UDPS_HEADER_SIZE) {
|
||||
return;
|
||||
}
|
||||
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
|
||||
if (hdr->magic != UDPS_MAGIC) {
|
||||
return;
|
||||
}
|
||||
if ((hdr->type != UDPS_TYPE_DATA) && (hdr->type != UDPS_TYPE_CONFIG)) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint32 payloadBytes = hdr->payloadBytes;
|
||||
if ((payloadBytes + UDPS_HEADER_SIZE) > size) {
|
||||
return; // truncated
|
||||
}
|
||||
|
||||
if (hdr->totalFragments == 1u) {
|
||||
if ((hdr->type == UDPS_TYPE_DATA) && (listener != NULL_PTR(UDPSClientListener *))) {
|
||||
listener->OnUDPSFragment(hdr->counter, size, true);
|
||||
}
|
||||
// Single-fragment shortcut: deliver immediately
|
||||
if (listener != NULL_PTR(UDPSClientListener *)) {
|
||||
const uint8 *pl = buf + UDPS_HEADER_SIZE;
|
||||
if (hdr->type == UDPS_TYPE_CONFIG) {
|
||||
listener->OnUDPSConfig(pl, payloadBytes);
|
||||
}
|
||||
else {
|
||||
listener->OnUDPSData(pl, payloadBytes);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
bool completed = PlaceFragment(hdr, buf + UDPS_HEADER_SIZE, payloadBytes);
|
||||
if ((hdr->type == UDPS_TYPE_DATA) && (listener != NULL_PTR(UDPSClientListener *))) {
|
||||
listener->OnUDPSFragment(hdr->counter, size, completed);
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: PlaceFragment
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
|
||||
const uint8 *payload,
|
||||
uint32 payloadBytes) {
|
||||
uint32 counter = hdr->counter;
|
||||
uint16 fragIdx = hdr->fragmentIdx;
|
||||
uint16 totalFrags = hdr->totalFragments;
|
||||
|
||||
if ((fragIdx >= totalFrags) || (totalFrags > 512u)) {
|
||||
return false; // sanity check
|
||||
}
|
||||
|
||||
// Find existing slot for this counter
|
||||
uint32 slot = UDPS_CLIENT_MAX_REASSEMBLY_SLOTS;
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
if (reassemblySlots[i].active && (reassemblySlots[i].counter == counter)) {
|
||||
slot = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Allocate new slot if not found
|
||||
if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
if (!reassemblySlots[i].active) {
|
||||
slot = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
|
||||
// All slots occupied — evict the oldest
|
||||
uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL;
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
if (reassemblySlots[i].firstSeenTicks < oldestTick) {
|
||||
oldestTick = reassemblySlots[i].firstSeenTicks;
|
||||
slot = i;
|
||||
}
|
||||
}
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Reassembly slots full; evicting oldest.");
|
||||
}
|
||||
|
||||
reassemblySlots[slot].counter = counter;
|
||||
reassemblySlots[slot].type = hdr->type;
|
||||
reassemblySlots[slot].totalFragments = totalFrags;
|
||||
reassemblySlots[slot].receivedFragments = 0u;
|
||||
reassemblySlots[slot].active = true;
|
||||
reassemblySlots[slot].firstSeenTicks = HighResolutionTimer::Counter();
|
||||
reassemblySlots[slot].chunkSize = 0u;
|
||||
(void) MemoryOperationsHelper::Set(reassemblySlots[slot].recvMask, 0, 32u);
|
||||
}
|
||||
|
||||
UDPSReassemblySlot &s = reassemblySlots[slot];
|
||||
|
||||
// Skip duplicate
|
||||
uint32 byteIdx = fragIdx / 8u;
|
||||
uint8 bitMask = static_cast<uint8>(1u << (fragIdx % 8u));
|
||||
if (byteIdx < 32u) {
|
||||
if ((s.recvMask[byteIdx] & bitMask) != 0u) {
|
||||
return false; // already have this fragment
|
||||
}
|
||||
}
|
||||
|
||||
// Compute placement offset
|
||||
uint32 chunkSize = s.chunkSize;
|
||||
if (chunkSize == 0u) {
|
||||
// Learn chunk size from first non-last fragment
|
||||
if (fragIdx == 0u) {
|
||||
chunkSize = payloadBytes;
|
||||
s.chunkSize = chunkSize;
|
||||
}
|
||||
else {
|
||||
// Can't place yet without knowing chunk size — drop (rare edge case)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
uint32 offset = static_cast<uint32>(fragIdx) * chunkSize;
|
||||
if ((offset + payloadBytes) > static_cast<uint32>(sizeof(s.payload))) {
|
||||
return false; // overflow guard
|
||||
}
|
||||
|
||||
if (payloadBytes > 0u) {
|
||||
(void) MemoryOperationsHelper::Copy(s.payload + offset, payload, payloadBytes);
|
||||
}
|
||||
|
||||
if (byteIdx < 32u) {
|
||||
s.recvMask[byteIdx] |= bitMask;
|
||||
}
|
||||
s.receivedFragments++;
|
||||
|
||||
// Check if complete
|
||||
if (s.receivedFragments >= s.totalFragments) {
|
||||
DeliverAssembled(s);
|
||||
s.active = false;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: DeliverAssembled
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSClient::DeliverAssembled(UDPSReassemblySlot &s) {
|
||||
if (listener == NULL_PTR(UDPSClientListener *)) {
|
||||
return;
|
||||
}
|
||||
// Total assembled size = (totalFrags-1)*chunkSize + lastFragSize
|
||||
// We don't store lastFragSize separately, but the last receivedFragments
|
||||
// Write placed payloadBytes at its offset — total = chunkSize*(totalFrags-1) + lastBytes.
|
||||
// Since we don't track lastBytes directly, use the mask to infer completeness.
|
||||
// The assembled payload size is not trivially known without tracking it.
|
||||
// Simplest correct approach: accumulate total bytes written.
|
||||
// For now, estimate as chunkSize * totalFragments (may be slightly over for last frag).
|
||||
// Fix: track totalPayloadBytes in slot.
|
||||
//
|
||||
// Since we don't have that, compute based on known structure:
|
||||
// totalFrags-1 full chunks + one last chunk (which might be smaller).
|
||||
// We'd need to save the last fragment's payloadBytes.
|
||||
// As a pragmatic solution, deliver chunkSize * totalFragments as upper bound —
|
||||
// the receiver (CONFIG/DATA parser) knows the actual sizes from content.
|
||||
//
|
||||
// Better: save the actual total in the slot. We don't have that field.
|
||||
// Use the simplest approximation that is always correct for aligned payloads,
|
||||
// and let the application-layer parser determine exact size from content.
|
||||
uint32 totalSize = s.chunkSize * static_cast<uint32>(s.totalFragments);
|
||||
|
||||
if (s.type == UDPS_TYPE_CONFIG) {
|
||||
listener->OnUDPSConfig(s.payload, totalSize);
|
||||
}
|
||||
else {
|
||||
listener->OnUDPSData(s.payload, totalSize);
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: GcReassemblySlots
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSClient::GcReassemblySlots() {
|
||||
uint64 staleThreshold = 2u * HighResolutionTimer::Frequency(); // 2 seconds
|
||||
uint64 now = HighResolutionTimer::Counter();
|
||||
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
if (!reassemblySlots[i].active) {
|
||||
continue;
|
||||
}
|
||||
if ((now - reassemblySlots[i].firstSeenTicks) > staleThreshold) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Discarding stale reassembly slot (counter %u).",
|
||||
reassemblySlots[i].counter);
|
||||
reassemblySlots[i].active = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace MARTe
|
||||
@@ -0,0 +1,210 @@
|
||||
#ifndef UDPS_CLIENT_H_
|
||||
#define UDPS_CLIENT_H_
|
||||
|
||||
/**
|
||||
* @file UDPSClient.h
|
||||
* @brief Auto-reconnecting UDPS receiver client (C++ MARTe2 library class).
|
||||
*
|
||||
* UDPSClient runs its own background thread (via SingleThreadService).
|
||||
* On start it connects to a UDPSServer, receives CONFIG + DATA packets, and
|
||||
* reassembles fragmented updates. If the server goes silent for longer than
|
||||
* SilenceTimeout, it automatically disconnects and retries.
|
||||
*
|
||||
* Unicast mode: sends CONNECT to server UDP port; receives on an ephemeral UDP port.
|
||||
* Multicast mode: TCP to server port for CONFIG; joins UDP multicast group for DATA.
|
||||
*
|
||||
* Threading: UDPSClientListener callbacks are invoked from the internal receive
|
||||
* thread — the implementation must be thread-safe with respect to the caller.
|
||||
*/
|
||||
|
||||
#include "BasicTCPSocket.h"
|
||||
#include "BasicUDPSocket.h"
|
||||
#include "EmbeddedServiceMethodBinderI.h"
|
||||
#include "ExecutionInfo.h"
|
||||
#include "HighResolutionTimer.h"
|
||||
#include "InternetHost.h"
|
||||
#include "SingleThreadService.h"
|
||||
#include "StreamString.h"
|
||||
#include "StructuredDataI.h"
|
||||
#include "UDPSProtocol.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Listener interface
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* @brief Callback interface for UDPSClient events.
|
||||
*
|
||||
* Implement this interface and pass an instance to UDPSClient::SetListener().
|
||||
* All callbacks are invoked from the UDPSClient internal thread.
|
||||
*/
|
||||
class UDPSClientListener {
|
||||
public:
|
||||
virtual ~UDPSClientListener() {}
|
||||
|
||||
/** Called when a fully-reassembled CONFIG payload is available. */
|
||||
virtual void OnUDPSConfig(const uint8 *payload, uint32 payloadSize) {}
|
||||
|
||||
/** Called when a fully-reassembled DATA payload is available. */
|
||||
virtual void OnUDPSData(const uint8 *payload, uint32 payloadSize) {}
|
||||
|
||||
/**
|
||||
* @brief Called for every received DATA datagram (fragment).
|
||||
* @param counter DATA packet counter from the UDPS header.
|
||||
* @param nBytes Raw datagram size (header + payload).
|
||||
* @param complete True iff this fragment completed the DATA reassembly.
|
||||
*/
|
||||
virtual void OnUDPSFragment(uint32 counter, uint32 nBytes, bool complete) {}
|
||||
|
||||
/** Called when the connection to the server has been established. */
|
||||
virtual void OnUDPSConnected() {}
|
||||
|
||||
/** Called when the connection has been lost or closed. */
|
||||
virtual void OnUDPSDisconnected() {}
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// UDPSClient
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* @brief UDPS receiver client with auto-reconnect and fragment reassembly.
|
||||
*/
|
||||
class UDPSClient : public EmbeddedServiceMethodBinderI {
|
||||
public:
|
||||
|
||||
/** Maximum number of concurrent in-flight reassembly slots. */
|
||||
static const uint32 UDPS_CLIENT_MAX_REASSEMBLY_SLOTS = 4u;
|
||||
|
||||
/** Default silence timeout before reconnect (seconds). */
|
||||
static const uint32 UDPS_CLIENT_DEFAULT_SILENCE_TIMEOUT_S = 5u;
|
||||
|
||||
/** Default delay between reconnect attempts (seconds). */
|
||||
static const uint32 UDPS_CLIENT_DEFAULT_RECONNECT_DELAY_S = 2u;
|
||||
|
||||
/** Default maximum payload size (bytes, excluding 17-byte header). */
|
||||
static const uint32 UDPS_CLIENT_DEFAULT_MAX_PAYLOAD = 1400u;
|
||||
|
||||
UDPSClient();
|
||||
virtual ~UDPSClient();
|
||||
|
||||
/**
|
||||
* @brief Read configuration from a StructuredDataI node.
|
||||
*
|
||||
* Expected keys:
|
||||
* - ServerAddr (char*) Server IPv4 address. Required.
|
||||
* - Port (uint16) Server UDP port (unicast) or TCP listen port (multicast). Required.
|
||||
* - MulticastGroup (char*) IPv4 multicast address; presence enables multicast mode.
|
||||
* - DataPort (uint16) UDP multicast data port (defaults to Port+1).
|
||||
* - SilenceTimeout (uint32) Seconds of no data before reconnect. Default 5.
|
||||
* - ReconnectDelay (uint32) Seconds to wait between reconnect attempts. Default 2.
|
||||
* - MaxPayloadSize (uint32) Max payload bytes per datagram, excluding header. Default 1400.
|
||||
* - CPUMask (uint32) CPU affinity mask for the receive thread. Default 0xFFFFFFFF.
|
||||
* - StackSize (uint32) Stack size for the receive thread. Default 65536.
|
||||
*/
|
||||
bool Initialise(StructuredDataI &data);
|
||||
|
||||
/**
|
||||
* @brief Register the event listener. Must be called before Start().
|
||||
*/
|
||||
void SetListener(UDPSClientListener *listener);
|
||||
|
||||
/**
|
||||
* @brief Start the receive thread.
|
||||
* @return true on success.
|
||||
*/
|
||||
bool Start();
|
||||
|
||||
/**
|
||||
* @brief Stop the receive thread and close all sockets.
|
||||
* @return true on success.
|
||||
*/
|
||||
bool Stop();
|
||||
|
||||
/**
|
||||
* @brief Internal thread entry point (EmbeddedServiceMethodBinderI).
|
||||
* @details Do NOT call directly.
|
||||
*/
|
||||
virtual ErrorManagement::ErrorType Execute(ExecutionInfo &info);
|
||||
|
||||
private:
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Fragment reassembly slot
|
||||
// -------------------------------------------------------------------------
|
||||
struct UDPSReassemblySlot {
|
||||
uint32 counter; ///< Packet counter this slot belongs to
|
||||
uint8 type; ///< UDPS_TYPE_DATA or UDPS_TYPE_CONFIG
|
||||
uint16 totalFragments; ///< Expected fragment count
|
||||
uint16 receivedFragments; ///< How many we have so far
|
||||
uint8 recvMask[32]; ///< Bitmask: bit f set iff fragment f received
|
||||
uint8 payload[65535]; ///< Assembled payload buffer
|
||||
uint64 firstSeenTicks; ///< For GC (2 s stale detection)
|
||||
bool active; ///< Slot in use
|
||||
uint32 chunkSize; ///< Payload bytes per fragment (from first fragment)
|
||||
};
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Connection state machine helpers
|
||||
// -------------------------------------------------------------------------
|
||||
bool Connect();
|
||||
void Disconnect();
|
||||
bool ReceiveAndProcess();
|
||||
|
||||
bool ConnectUnicast();
|
||||
bool ConnectMulticast();
|
||||
|
||||
void ProcessDatagram(const uint8 *buf, uint32 size);
|
||||
/** Read one full UDPS frame (header + payload) from the TCP control socket. */
|
||||
bool ReceiveTCPFrame();
|
||||
/** Read exactly n bytes from the TCP control socket. */
|
||||
bool ReadExactTCP(uint8 *dst, uint32 n);
|
||||
/** @return true iff this fragment completed the reassembly (payload delivered). */
|
||||
bool PlaceFragment(const UDPSPacketHeader *hdr, const uint8 *payload, uint32 payloadBytes);
|
||||
void GcReassemblySlots();
|
||||
void DeliverAssembled(UDPSReassemblySlot &slot);
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Configuration
|
||||
// -------------------------------------------------------------------------
|
||||
StreamString serverAddr;
|
||||
uint16 serverPort;
|
||||
StreamString multicastGroup;
|
||||
uint16 dataPort;
|
||||
bool useMulticast;
|
||||
uint64 silenceTimeoutTicks;
|
||||
uint64 reconnectDelayTicks;
|
||||
uint32 maxPayloadSize;
|
||||
uint32 cpuMask;
|
||||
uint32 stackSize;
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Runtime state
|
||||
// -------------------------------------------------------------------------
|
||||
UDPSClientListener *listener;
|
||||
SingleThreadService threadService;
|
||||
bool connected;
|
||||
uint64 lastDataTicks; ///< Ticks at last received DATA/CONFIG
|
||||
uint64 disconnectTick; ///< Ticks when we disconnected (for delay)
|
||||
|
||||
// Unicast
|
||||
BasicUDPSocket recvSocket; ///< Bound to ephemeral port; receives DATA
|
||||
uint16 localPort; ///< Ephemeral port we're listening on
|
||||
|
||||
// Multicast
|
||||
BasicTCPSocket tcpSocket; ///< TCP connection to server (for CONNECT + CONFIG)
|
||||
BasicUDPSocket mcastSocket; ///< Joined multicast group
|
||||
|
||||
// Reassembly
|
||||
UDPSReassemblySlot reassemblySlots[UDPS_CLIENT_MAX_REASSEMBLY_SLOTS];
|
||||
uint64 lastGcTicks; ///< Ticks at last GC run
|
||||
|
||||
// Receive scratch buffer
|
||||
uint8 recvBuf[65535u + UDPS_HEADER_SIZE];
|
||||
};
|
||||
|
||||
} // namespace MARTe
|
||||
|
||||
#endif // UDPS_CLIENT_H_
|
||||
@@ -0,0 +1,893 @@
|
||||
/**
|
||||
* @file UDPSServer.cpp
|
||||
* @brief Implementation of UDPSServer — multi-client UDPS session management.
|
||||
*/
|
||||
|
||||
#include "UDPSServer.h"
|
||||
|
||||
#include "AdvancedErrorManagement.h"
|
||||
#include "HighResolutionTimer.h"
|
||||
#include "MemoryOperationsHelper.h"
|
||||
#include "StreamString.h"
|
||||
|
||||
#include <sys/select.h>
|
||||
#include <poll.h>
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Constructor / Destructor
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
UDPSServer::UDPSServer()
|
||||
: port(0u),
|
||||
maxPayloadSize(UDPS_SERVER_DEFAULT_MAX_PAYLOAD),
|
||||
dataPort(0u),
|
||||
useMulticast(false),
|
||||
clientTimeoutTicks(0u),
|
||||
numUnicastClients(0u),
|
||||
numTCPClients(0u),
|
||||
cachedConfig(NULL_PTR(uint8 *)),
|
||||
cachedConfigSize(0u),
|
||||
sendBuf(NULL_PTR(uint8 *)),
|
||||
sendBufCapacity(0u),
|
||||
configCounter(0u),
|
||||
started(false) {
|
||||
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
|
||||
unicastClients[i].ipAddr[0] = '\0';
|
||||
unicastClients[i].clientPort = 0u;
|
||||
unicastClients[i].lastSeenTicks = 0u;
|
||||
unicastClients[i].active = false;
|
||||
unicastClients[i].isStatic = false;
|
||||
}
|
||||
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
|
||||
tcpClients[i] = NULL_PTR(BasicTCPSocket *);
|
||||
}
|
||||
}
|
||||
|
||||
UDPSServer::~UDPSServer() {
|
||||
(void) Stop();
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Initialise
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSServer::Initialise(StructuredDataI &data) {
|
||||
uint32 portU32 = 0u;
|
||||
if (data.Read("Port", portU32)) {
|
||||
port = static_cast<uint16>(portU32);
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
|
||||
"UDPSServer: Port not specified.");
|
||||
return false;
|
||||
}
|
||||
/* port == 0 is valid: unicast push-only mode (no serverSocket bind,
|
||||
* no CONNECT/DISCONNECT/ACK reception). Clients added via AddStaticClient(). */
|
||||
|
||||
StreamString mcGroup;
|
||||
if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
|
||||
multicastGroup = mcGroup;
|
||||
useMulticast = true;
|
||||
}
|
||||
|
||||
if (useMulticast) {
|
||||
uint32 dpU32 = static_cast<uint32>(port) + 1u;
|
||||
(void) data.Read("DataPort", dpU32);
|
||||
dataPort = static_cast<uint16>(dpU32);
|
||||
if (dataPort == port) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
|
||||
"UDPSServer: DataPort (%u) must differ from Port (%u).",
|
||||
static_cast<uint32>(dataPort), static_cast<uint32>(port));
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
uint32 mps = UDPS_SERVER_DEFAULT_MAX_PAYLOAD;
|
||||
(void) data.Read("MaxPayloadSize", mps);
|
||||
maxPayloadSize = mps;
|
||||
|
||||
uint32 timeoutSecs = UDPS_SERVER_DEFAULT_CLIENT_TIMEOUT_S;
|
||||
(void) data.Read("ClientTimeout", timeoutSecs);
|
||||
clientTimeoutTicks = (timeoutSecs > 0u)
|
||||
? (static_cast<uint64>(timeoutSecs) * HighResolutionTimer::Frequency())
|
||||
: 0u;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Start / Stop
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSServer::Start() {
|
||||
if (started) {
|
||||
return true;
|
||||
}
|
||||
|
||||
sendBufCapacity = UDPS_HEADER_SIZE + maxPayloadSize;
|
||||
sendBuf = new uint8[sendBufCapacity];
|
||||
if (sendBuf == NULL_PTR(uint8 *)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
|
||||
"UDPSServer: Could not allocate send buffer.");
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ok = true;
|
||||
|
||||
if (useMulticast) {
|
||||
// TCP listener for CONNECT + CONFIG
|
||||
ok = tcpListener.Open();
|
||||
if (ok) {
|
||||
ok = tcpListener.Listen(port, 5);
|
||||
}
|
||||
if (ok) {
|
||||
tcpListener.SetBlocking(false);
|
||||
}
|
||||
// UDP data socket connected to multicast group
|
||||
if (ok) {
|
||||
ok = dataSocket.Open();
|
||||
}
|
||||
if (ok) {
|
||||
ok = dataSocket.Connect(multicastGroup.Buffer(), dataPort);
|
||||
}
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
|
||||
"UDPSServer: Failed to open multicast sockets on port %u.",
|
||||
static_cast<uint32>(port));
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Unicast send socket (unconnected; SetDestination per Write)
|
||||
ok = uniSendSocket.Open();
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
|
||||
"UDPSServer: Failed to open unicast send socket.");
|
||||
}
|
||||
|
||||
// Receive socket: only bind if port != 0 (skip for push-only mode)
|
||||
if (ok && (port != 0u)) {
|
||||
ok = serverSocket.Open();
|
||||
if (ok) {
|
||||
ok = serverSocket.Listen(port); // UDP "listen" = bind
|
||||
}
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
|
||||
"UDPSServer: Failed to bind receive socket on port %u.",
|
||||
static_cast<uint32>(port));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
started = ok;
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool UDPSServer::Stop() {
|
||||
if (!started) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Evict all unicast clients
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
|
||||
if (unicastClients[i].active) {
|
||||
EvictUnicastClient(i);
|
||||
}
|
||||
}
|
||||
numUnicastClients = 0u;
|
||||
|
||||
// Close all TCP clients
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
|
||||
if (tcpClients[i] != NULL_PTR(BasicTCPSocket *)) {
|
||||
EvictTCPClient(i);
|
||||
}
|
||||
}
|
||||
numTCPClients = 0u;
|
||||
|
||||
// Close sockets
|
||||
if (serverSocket.IsValid()) {
|
||||
(void) serverSocket.Close();
|
||||
}
|
||||
if (uniSendSocket.IsValid()) {
|
||||
(void) uniSendSocket.Close();
|
||||
}
|
||||
if (tcpListener.IsValid()) {
|
||||
(void) tcpListener.Close();
|
||||
}
|
||||
if (dataSocket.IsValid()) {
|
||||
(void) dataSocket.Close();
|
||||
}
|
||||
|
||||
// Free heap buffers
|
||||
if (cachedConfig != NULL_PTR(uint8 *)) {
|
||||
delete[] cachedConfig;
|
||||
cachedConfig = NULL_PTR(uint8 *);
|
||||
cachedConfigSize = 0u;
|
||||
}
|
||||
if (sendBuf != NULL_PTR(uint8 *)) {
|
||||
delete[] sendBuf;
|
||||
sendBuf = NULL_PTR(uint8 *);
|
||||
sendBufCapacity = 0u;
|
||||
}
|
||||
|
||||
started = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// ServiceClients
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSServer::ServiceClients() {
|
||||
if (!started) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (useMulticast) {
|
||||
// Poll TCP listener for new connections (non-blocking).
|
||||
// Guard with poll() first so WaitConnection is never called on an empty
|
||||
// queue — avoids the MARTe2 "Failed accept in unblocking mode" log spam.
|
||||
if (tcpListener.IsValid()) {
|
||||
struct pollfd pfd;
|
||||
pfd.fd = static_cast<int>(tcpListener.GetReadHandle());
|
||||
pfd.events = POLLIN;
|
||||
pfd.revents = 0;
|
||||
bool pending = (::poll(&pfd, 1u, 0) > 0) &&
|
||||
((pfd.revents & POLLIN) != 0);
|
||||
BasicTCPSocket *newConn = pending
|
||||
? tcpListener.WaitConnection(0u)
|
||||
: NULL_PTR(BasicTCPSocket *);
|
||||
if (newConn != NULL_PTR(BasicTCPSocket *)) {
|
||||
// Find free TCP client slot
|
||||
uint32 freeSlot = UDPS_SERVER_MAX_TCP_CLIENTS;
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
|
||||
if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) {
|
||||
freeSlot = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (freeSlot < UDPS_SERVER_MAX_TCP_CLIENTS) {
|
||||
HandleMulticastTCPConnect(newConn, freeSlot);
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSServer: TCP client table full, rejecting new connection.");
|
||||
(void) newConn->Close();
|
||||
delete newConn;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Poll existing TCP clients for DISCONNECT
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
|
||||
if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) {
|
||||
continue;
|
||||
}
|
||||
// Non-blocking peek
|
||||
int fd = tcpClients[i]->GetReadHandle();
|
||||
fd_set rset;
|
||||
FD_ZERO(&rset);
|
||||
FD_SET(fd, &rset);
|
||||
struct timeval tv;
|
||||
tv.tv_sec = 0; tv.tv_usec = 0;
|
||||
int nready = select(fd + 1, &rset, NULL, NULL, &tv);
|
||||
if (nready > 0) {
|
||||
uint8 pktBuf[UDPS_HEADER_SIZE];
|
||||
uint32 recvSize = UDPS_HEADER_SIZE;
|
||||
bool recvOk = tcpClients[i]->Read(reinterpret_cast<char8 *>(pktBuf), recvSize);
|
||||
if (!recvOk || (recvSize == 0u)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSServer: TCP client disconnected (slot %u).", i);
|
||||
EvictTCPClient(i);
|
||||
continue;
|
||||
}
|
||||
if (recvSize >= UDPS_HEADER_SIZE) {
|
||||
const UDPSPacketHeader *hdr =
|
||||
reinterpret_cast<const UDPSPacketHeader *>(pktBuf);
|
||||
if ((hdr->magic == UDPS_MAGIC) &&
|
||||
(hdr->type == UDPS_TYPE_DISCONNECT)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSServer: TCP client sent DISCONNECT (slot %u).", i);
|
||||
EvictTCPClient(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Unicast: poll serverSocket for CONNECT / DISCONNECT / ACK
|
||||
if (!serverSocket.IsValid()) {
|
||||
return;
|
||||
}
|
||||
int fd = serverSocket.GetReadHandle();
|
||||
fd_set rset;
|
||||
FD_ZERO(&rset);
|
||||
FD_SET(fd, &rset);
|
||||
struct timeval tv = {0, 0};
|
||||
int nready = select(fd + 1, &rset, NULL, NULL, &tv);
|
||||
while (nready > 0) {
|
||||
uint8 pktBuf[UDPS_HEADER_SIZE + 4u];
|
||||
uint32 recvSize = static_cast<uint32>(sizeof(pktBuf));
|
||||
bool recvOk = serverSocket.Read(reinterpret_cast<char8 *>(pktBuf), recvSize);
|
||||
if (!recvOk || (recvSize < UDPS_HEADER_SIZE)) {
|
||||
break;
|
||||
}
|
||||
const UDPSPacketHeader *hdr =
|
||||
reinterpret_cast<const UDPSPacketHeader *>(pktBuf);
|
||||
if (hdr->magic != UDPS_MAGIC) {
|
||||
break;
|
||||
}
|
||||
InternetHost src = serverSocket.GetSource();
|
||||
if (hdr->type == UDPS_TYPE_CONNECT) {
|
||||
HandleUnicastConnect(src);
|
||||
}
|
||||
else if (hdr->type == UDPS_TYPE_DISCONNECT) {
|
||||
HandleUnicastDisconnect(src);
|
||||
}
|
||||
else if (hdr->type == UDPS_TYPE_ACK) {
|
||||
HandleUnicastAck(src);
|
||||
}
|
||||
|
||||
// Check if more data is ready
|
||||
FD_ZERO(&rset);
|
||||
FD_SET(fd, &rset);
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = 0;
|
||||
nready = select(fd + 1, &rset, NULL, NULL, &tv);
|
||||
}
|
||||
|
||||
// Evict stale clients (if timeout is configured)
|
||||
if (clientTimeoutTicks > 0u) {
|
||||
EvictStaleUnicastClients();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// SendConfig
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSServer::SendConfig(const uint8 *payload, uint32 payloadSize) {
|
||||
if (!started) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Cache for future CONNECT clients
|
||||
(void) CacheConfig(payload, payloadSize);
|
||||
|
||||
configCounter++;
|
||||
|
||||
bool ok = true;
|
||||
|
||||
if (useMulticast) {
|
||||
// Send CONFIG over TCP to each connected client
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
|
||||
if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) {
|
||||
continue;
|
||||
}
|
||||
bool sent = SendFragmentedTCP(*tcpClients[i], UDPS_TYPE_CONFIG,
|
||||
configCounter, payload, payloadSize);
|
||||
if (!sent) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSServer: CONFIG send failed to TCP client %u, evicting.", i);
|
||||
EvictTCPClient(i);
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Send CONFIG to each unicast client
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
|
||||
if (!unicastClients[i].active) {
|
||||
continue;
|
||||
}
|
||||
InternetHost dest(unicastClients[i].clientPort, unicastClients[i].ipAddr);
|
||||
bool sent = SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG,
|
||||
configCounter, payload, payloadSize);
|
||||
if (!sent) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSServer: CONFIG send failed to %s:%u.",
|
||||
unicastClients[i].ipAddr,
|
||||
static_cast<uint32>(unicastClients[i].clientPort));
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ok;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// SendData
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSServer::SendData(uint32 counter, const uint8 *payload, uint32 payloadSize) {
|
||||
if (!started) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ok = true;
|
||||
|
||||
if (useMulticast) {
|
||||
// Single multicast write (no dest needed — socket already connected)
|
||||
bool sent = SendFragmentedUDP(dataSocket, NULL_PTR(InternetHost *),
|
||||
UDPS_TYPE_DATA, counter, payload, payloadSize);
|
||||
if (!sent) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSServer: DATA send to multicast group failed.");
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
else {
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
|
||||
if (!unicastClients[i].active) {
|
||||
continue;
|
||||
}
|
||||
InternetHost dest(unicastClients[i].clientPort, unicastClients[i].ipAddr);
|
||||
bool sent = SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_DATA,
|
||||
counter, payload, payloadSize);
|
||||
if (!sent) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSServer: DATA send failed to %s:%u.",
|
||||
unicastClients[i].ipAddr,
|
||||
static_cast<uint32>(unicastClients[i].clientPort));
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ok;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// AddStaticClient
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSServer::AddStaticClient(const char8 *ip, uint16 port_) {
|
||||
if ((ip == NULL_PTR(const char8 *)) || (ip[0] == '\0')) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Check for duplicate
|
||||
uint32 existing = FindUnicastClient(ip, port_);
|
||||
if (existing < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
|
||||
unicastClients[existing].isStatic = true; // ensure it's marked static
|
||||
return true;
|
||||
}
|
||||
|
||||
// Find free slot
|
||||
uint32 freeSlot = UDPS_SERVER_MAX_UNICAST_CLIENTS;
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
|
||||
if (!unicastClients[i].active) {
|
||||
freeSlot = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (freeSlot >= UDPS_SERVER_MAX_UNICAST_CLIENTS) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSServer: Unicast client table full, cannot add static client %s:%u.",
|
||||
ip, static_cast<uint32>(port_));
|
||||
return false;
|
||||
}
|
||||
|
||||
// Populate slot
|
||||
uint32 ipLen = 0u;
|
||||
while ((ipLen < 63u) && (ip[ipLen] != '\0')) {
|
||||
unicastClients[freeSlot].ipAddr[ipLen] = ip[ipLen];
|
||||
ipLen++;
|
||||
}
|
||||
unicastClients[freeSlot].ipAddr[ipLen] = '\0';
|
||||
unicastClients[freeSlot].clientPort = port_;
|
||||
unicastClients[freeSlot].lastSeenTicks = HighResolutionTimer::Counter();
|
||||
unicastClients[freeSlot].active = true;
|
||||
unicastClients[freeSlot].isStatic = true;
|
||||
numUnicastClients++;
|
||||
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSServer: Static client added: %s:%u.",
|
||||
ip, static_cast<uint32>(port_));
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Accessors
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
uint32 UDPSServer::GetClientCount() const {
|
||||
uint32 count = 0u;
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
|
||||
if (unicastClients[i].active) {
|
||||
count++;
|
||||
}
|
||||
}
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
|
||||
if (tcpClients[i] != NULL_PTR(BasicTCPSocket *)) {
|
||||
count++;
|
||||
}
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
bool UDPSServer::HasClients() const {
|
||||
return (GetClientCount() > 0u);
|
||||
}
|
||||
|
||||
bool UDPSServer::IsMulticast() const {
|
||||
return useMulticast;
|
||||
}
|
||||
|
||||
uint16 UDPSServer::GetPort() const {
|
||||
return port;
|
||||
}
|
||||
|
||||
uint32 UDPSServer::GetMaxPayloadSize() const {
|
||||
return maxPayloadSize;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: SendFragmentedUDP
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSServer::SendFragmentedUDP(BasicUDPSocket &sock,
|
||||
InternetHost *dest,
|
||||
uint8 type,
|
||||
uint32 counter,
|
||||
const uint8 *payload,
|
||||
uint32 payloadSize) {
|
||||
uint32 maxChunk = maxPayloadSize; // payload bytes per fragment (excl. header)
|
||||
uint32 totalFrags = (payloadSize == 0u) ? 1u :
|
||||
((payloadSize + maxChunk - 1u) / maxChunk);
|
||||
|
||||
bool ok = true;
|
||||
uint32 offs = 0u;
|
||||
|
||||
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
|
||||
uint32 chunkSize = payloadSize - offs;
|
||||
if (chunkSize > maxChunk) {
|
||||
chunkSize = maxChunk;
|
||||
}
|
||||
|
||||
UDPSBuildHeader(sendBuf, type, counter,
|
||||
static_cast<uint16>(f),
|
||||
static_cast<uint16>(totalFrags),
|
||||
chunkSize);
|
||||
|
||||
if (chunkSize > 0u) {
|
||||
(void) MemoryOperationsHelper::Copy(sendBuf + UDPS_HEADER_SIZE,
|
||||
payload + offs,
|
||||
chunkSize);
|
||||
}
|
||||
|
||||
uint32 sendSize = UDPS_HEADER_SIZE + chunkSize;
|
||||
|
||||
if (dest != NULL_PTR(InternetHost *)) {
|
||||
(void) sock.SetDestination(*dest);
|
||||
}
|
||||
|
||||
ok = sock.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSServer: UDP fragment %u/%u write failed.",
|
||||
f + 1u, totalFrags);
|
||||
}
|
||||
|
||||
offs += chunkSize;
|
||||
}
|
||||
|
||||
return ok;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: SendFragmentedTCP
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSServer::SendFragmentedTCP(BasicTCPSocket &sock,
|
||||
uint8 type,
|
||||
uint32 counter,
|
||||
const uint8 *payload,
|
||||
uint32 payloadSize) {
|
||||
uint32 maxChunk = maxPayloadSize;
|
||||
uint32 totalFrags = (payloadSize == 0u) ? 1u :
|
||||
((payloadSize + maxChunk - 1u) / maxChunk);
|
||||
|
||||
bool ok = true;
|
||||
uint32 offs = 0u;
|
||||
|
||||
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
|
||||
uint32 chunkSize = payloadSize - offs;
|
||||
if (chunkSize > maxChunk) {
|
||||
chunkSize = maxChunk;
|
||||
}
|
||||
|
||||
UDPSBuildHeader(sendBuf, type, counter,
|
||||
static_cast<uint16>(f),
|
||||
static_cast<uint16>(totalFrags),
|
||||
chunkSize);
|
||||
|
||||
if (chunkSize > 0u) {
|
||||
(void) MemoryOperationsHelper::Copy(sendBuf + UDPS_HEADER_SIZE,
|
||||
payload + offs,
|
||||
chunkSize);
|
||||
}
|
||||
|
||||
uint32 sendSize = UDPS_HEADER_SIZE + chunkSize;
|
||||
ok = sock.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
|
||||
if (!ok) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSServer: TCP fragment %u/%u write failed.",
|
||||
f + 1u, totalFrags);
|
||||
}
|
||||
|
||||
offs += chunkSize;
|
||||
}
|
||||
|
||||
return ok;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: EvictStaleUnicastClients
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSServer::EvictStaleUnicastClients() {
|
||||
uint64 now = HighResolutionTimer::Counter();
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
|
||||
if (!unicastClients[i].active || unicastClients[i].isStatic) {
|
||||
continue;
|
||||
}
|
||||
uint64 elapsed = now - unicastClients[i].lastSeenTicks;
|
||||
if (elapsed >= clientTimeoutTicks) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSServer: Evicting stale client %s:%u.",
|
||||
unicastClients[i].ipAddr,
|
||||
static_cast<uint32>(unicastClients[i].clientPort));
|
||||
EvictUnicastClient(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: HandleUnicastConnect
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSServer::HandleUnicastConnect(const InternetHost &src) {
|
||||
StreamString srcAddrStr = src.GetAddress();
|
||||
const char8 *srcAddr = srcAddrStr.Buffer();
|
||||
uint16 srcPort = src.GetPort();
|
||||
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSServer: CONNECT from %s:%u.",
|
||||
srcAddr, static_cast<uint32>(srcPort));
|
||||
|
||||
// Check if this client is already known
|
||||
uint32 existing = FindUnicastClient(srcAddr, srcPort);
|
||||
if (existing < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
|
||||
// Refresh last-seen and resend CONFIG
|
||||
unicastClients[existing].lastSeenTicks = HighResolutionTimer::Counter();
|
||||
if (cachedConfig != NULL_PTR(uint8 *)) {
|
||||
InternetHost dest(srcPort, srcAddr);
|
||||
configCounter++;
|
||||
(void) SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG,
|
||||
configCounter, cachedConfig, cachedConfigSize);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// New client — find a free slot (or evict oldest non-static)
|
||||
uint32 slot = UDPS_SERVER_MAX_UNICAST_CLIENTS;
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
|
||||
if (!unicastClients[i].active) {
|
||||
slot = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (slot >= UDPS_SERVER_MAX_UNICAST_CLIENTS) {
|
||||
slot = FindOldestUnicastClient();
|
||||
if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSServer: Client table full — evicting %s:%u.",
|
||||
unicastClients[slot].ipAddr,
|
||||
static_cast<uint32>(unicastClients[slot].clientPort));
|
||||
EvictUnicastClient(slot);
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSServer: All slots occupied by static clients; rejecting %s:%u.",
|
||||
srcAddr, static_cast<uint32>(srcPort));
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Fill slot
|
||||
uint32 addrLen = 0u;
|
||||
while ((addrLen < 63u) && (srcAddr[addrLen] != '\0')) {
|
||||
unicastClients[slot].ipAddr[addrLen] = srcAddr[addrLen];
|
||||
addrLen++;
|
||||
}
|
||||
unicastClients[slot].ipAddr[addrLen] = '\0';
|
||||
unicastClients[slot].clientPort = srcPort;
|
||||
unicastClients[slot].lastSeenTicks = HighResolutionTimer::Counter();
|
||||
unicastClients[slot].active = true;
|
||||
unicastClients[slot].isStatic = false;
|
||||
numUnicastClients++;
|
||||
|
||||
// Send cached CONFIG if available
|
||||
if (cachedConfig != NULL_PTR(uint8 *)) {
|
||||
InternetHost dest(srcPort, srcAddr);
|
||||
configCounter++;
|
||||
(void) SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG,
|
||||
configCounter, cachedConfig, cachedConfigSize);
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: HandleUnicastDisconnect
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSServer::HandleUnicastDisconnect(const InternetHost &src) {
|
||||
StreamString srcAddrStr = src.GetAddress();
|
||||
const char8 *srcAddr = srcAddrStr.Buffer();
|
||||
uint16 srcPort = src.GetPort();
|
||||
|
||||
uint32 slot = FindUnicastClient(srcAddr, srcPort);
|
||||
if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSServer: DISCONNECT from %s:%u.",
|
||||
srcAddr, static_cast<uint32>(srcPort));
|
||||
EvictUnicastClient(slot);
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: HandleUnicastAck
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSServer::HandleUnicastAck(const InternetHost &src) {
|
||||
StreamString srcAddrStr = src.GetAddress();
|
||||
const char8 *srcAddr = srcAddrStr.Buffer();
|
||||
uint16 srcPort = src.GetPort();
|
||||
|
||||
uint32 slot = FindUnicastClient(srcAddr, srcPort);
|
||||
if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
|
||||
unicastClients[slot].lastSeenTicks = HighResolutionTimer::Counter();
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: FindUnicastClient
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
uint32 UDPSServer::FindUnicastClient(const char8 *ip, uint16 port_) const {
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
|
||||
if (!unicastClients[i].active) {
|
||||
continue;
|
||||
}
|
||||
if (unicastClients[i].clientPort != port_) {
|
||||
continue;
|
||||
}
|
||||
// String compare
|
||||
bool match = true;
|
||||
uint32 k = 0u;
|
||||
while ((k < 63u) && match) {
|
||||
if (unicastClients[i].ipAddr[k] != ip[k]) {
|
||||
match = false;
|
||||
}
|
||||
if (ip[k] == '\0') {
|
||||
break;
|
||||
}
|
||||
k++;
|
||||
}
|
||||
if (match) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return UDPS_SERVER_MAX_UNICAST_CLIENTS;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: FindOldestUnicastClient
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
uint32 UDPSServer::FindOldestUnicastClient() const {
|
||||
uint32 oldest = UDPS_SERVER_MAX_UNICAST_CLIENTS;
|
||||
uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL;
|
||||
|
||||
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
|
||||
if (!unicastClients[i].active || unicastClients[i].isStatic) {
|
||||
continue;
|
||||
}
|
||||
if (unicastClients[i].lastSeenTicks < oldestTick) {
|
||||
oldestTick = unicastClients[i].lastSeenTicks;
|
||||
oldest = i;
|
||||
}
|
||||
}
|
||||
return oldest;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: EvictUnicastClient
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSServer::EvictUnicastClient(uint32 idx) {
|
||||
if (idx >= UDPS_SERVER_MAX_UNICAST_CLIENTS) {
|
||||
return;
|
||||
}
|
||||
unicastClients[idx].active = false;
|
||||
if (numUnicastClients > 0u) {
|
||||
numUnicastClients--;
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: HandleMulticastTCPConnect
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSServer::HandleMulticastTCPConnect(BasicTCPSocket *newClient, uint32 idx) {
|
||||
if (idx >= UDPS_SERVER_MAX_TCP_CLIENTS) {
|
||||
return;
|
||||
}
|
||||
|
||||
tcpClients[idx] = newClient;
|
||||
numTCPClients++;
|
||||
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Information,
|
||||
"UDPSServer: Multicast TCP client connected (slot %u).", idx);
|
||||
|
||||
// Send cached CONFIG over TCP
|
||||
if (cachedConfig != NULL_PTR(uint8 *)) {
|
||||
configCounter++;
|
||||
bool sent = SendFragmentedTCP(*newClient, UDPS_TYPE_CONFIG,
|
||||
configCounter, cachedConfig, cachedConfigSize);
|
||||
if (!sent) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSServer: Failed to send CONFIG to new TCP client (slot %u).", idx);
|
||||
EvictTCPClient(idx);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: EvictTCPClient
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void UDPSServer::EvictTCPClient(uint32 idx) {
|
||||
if (idx >= UDPS_SERVER_MAX_TCP_CLIENTS) {
|
||||
return;
|
||||
}
|
||||
if (tcpClients[idx] != NULL_PTR(BasicTCPSocket *)) {
|
||||
(void) tcpClients[idx]->Close();
|
||||
delete tcpClients[idx];
|
||||
tcpClients[idx] = NULL_PTR(BasicTCPSocket *);
|
||||
if (numTCPClients > 0u) {
|
||||
numTCPClients--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private: CacheConfig
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSServer::CacheConfig(const uint8 *payload, uint32 payloadSize) {
|
||||
if (cachedConfig != NULL_PTR(uint8 *)) {
|
||||
delete[] cachedConfig;
|
||||
cachedConfig = NULL_PTR(uint8 *);
|
||||
cachedConfigSize = 0u;
|
||||
}
|
||||
if ((payload == NULL_PTR(const uint8 *)) || (payloadSize == 0u)) {
|
||||
return true;
|
||||
}
|
||||
cachedConfig = new uint8[payloadSize];
|
||||
if (cachedConfig == NULL_PTR(uint8 *)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
|
||||
"UDPSServer: Failed to allocate cached CONFIG buffer.");
|
||||
return false;
|
||||
}
|
||||
(void) MemoryOperationsHelper::Copy(cachedConfig, payload, payloadSize);
|
||||
cachedConfigSize = payloadSize;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace MARTe
|
||||
@@ -0,0 +1,274 @@
|
||||
#ifndef UDPS_SERVER_H_
|
||||
#define UDPS_SERVER_H_
|
||||
|
||||
/**
|
||||
* @file UDPSServer.h
|
||||
* @brief Multi-client UDPS session-management and fragmented-send helper.
|
||||
*
|
||||
* UDPSServer is a plain C++ helper (not a MARTe2 Object) that encapsulates:
|
||||
* - Unicast mode: up to UDPS_SERVER_MAX_UNICAST_CLIENTS simultaneous UDP clients
|
||||
* that self-register via CONNECT datagrams.
|
||||
* - Multicast mode: TCP listener for CONNECT + CONFIG delivery; UDP multicast
|
||||
* socket for DATA datagrams.
|
||||
* - Static clients: pre-configured destinations that never need to send CONNECT
|
||||
* (backward-compat for push-to-fixed-IP use cases).
|
||||
*
|
||||
* Threading model: UDPSServer is NOT thread-safe. All public methods must be
|
||||
* called from the same thread (the owner's Execute() thread).
|
||||
*/
|
||||
|
||||
#include "BasicTCPSocket.h"
|
||||
#include "BasicUDPSocket.h"
|
||||
#include "InternetHost.h"
|
||||
#include "StreamString.h"
|
||||
#include "StructuredDataI.h"
|
||||
#include "UDPSProtocol.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
/**
|
||||
* @brief Multi-client / multicast UDPS server helper.
|
||||
*
|
||||
* Usage pattern:
|
||||
* @code
|
||||
* UDPSServer server;
|
||||
* server.Initialise(data); // in component Initialise()
|
||||
* server.Start(); // in PrepareNextState() / Start()
|
||||
* // inside Execute() loop:
|
||||
* server.ServiceClients(); // poll CONNECT / DISCONNECT / ACK
|
||||
* if (server.HasClients()) {
|
||||
* server.SendConfig(cfgBuf, cfgSize); // when config changes
|
||||
* server.SendData(counter, dataBuf, dataSize);
|
||||
* }
|
||||
* server.Stop(); // in StopCurrentStateExecution()
|
||||
* @endcode
|
||||
*/
|
||||
class UDPSServer {
|
||||
public:
|
||||
|
||||
/** Maximum number of simultaneous unicast clients. */
|
||||
static const uint32 UDPS_SERVER_MAX_UNICAST_CLIENTS = 16u;
|
||||
|
||||
/** Maximum number of simultaneous multicast TCP control connections. */
|
||||
static const uint32 UDPS_SERVER_MAX_TCP_CLIENTS = 8u;
|
||||
|
||||
/** Default stale-client eviction timeout (seconds). */
|
||||
static const uint32 UDPS_SERVER_DEFAULT_CLIENT_TIMEOUT_S = 30u;
|
||||
|
||||
/** Default maximum UDP payload size (bytes, EXCLUDING the 17-byte header). */
|
||||
static const uint32 UDPS_SERVER_DEFAULT_MAX_PAYLOAD = 1400u;
|
||||
|
||||
UDPSServer();
|
||||
~UDPSServer();
|
||||
|
||||
/**
|
||||
* @brief Read configuration from a StructuredDataI node.
|
||||
*
|
||||
* Expected keys (all optional except Port for unicast):
|
||||
* - Port (uint16) UDP port to bind (unicast) or TCP listen port (multicast).
|
||||
* - MulticastGroup (char*) IPv4 multicast address; presence enables multicast mode.
|
||||
* - DataPort (uint16) UDP port used for DATA datagrams in multicast mode
|
||||
* (defaults to Port+1 if omitted).
|
||||
* - MaxPayloadSize (uint32) Max payload bytes per datagram, excluding header.
|
||||
* Defaults to UDPS_SERVER_DEFAULT_MAX_PAYLOAD.
|
||||
* - ClientTimeout (uint32) Seconds before a silent unicast client is evicted.
|
||||
* Defaults to UDPS_SERVER_DEFAULT_CLIENT_TIMEOUT_S.
|
||||
* Set to 0 to disable timeout-based eviction.
|
||||
*/
|
||||
bool Initialise(StructuredDataI &data);
|
||||
|
||||
/**
|
||||
* @brief Open sockets and begin accepting clients.
|
||||
* @return true on success.
|
||||
*/
|
||||
bool Start();
|
||||
|
||||
/**
|
||||
* @brief Close all sockets and evict all clients. Frees cached CONFIG.
|
||||
*/
|
||||
bool Stop();
|
||||
|
||||
/**
|
||||
* @brief Non-blocking poll for incoming CONNECT / DISCONNECT / ACK messages.
|
||||
*
|
||||
* Must be called regularly from the owner's Execute() main loop.
|
||||
* In multicast mode also polls the TCP listener for new connections.
|
||||
*/
|
||||
void ServiceClients();
|
||||
|
||||
/**
|
||||
* @brief Send a CONFIG payload to all active clients.
|
||||
*
|
||||
* Also stores a copy as the cached CONFIG, which is automatically sent to
|
||||
* any new CONNECT client.
|
||||
*
|
||||
* @param payload Pointer to fully-assembled CONFIG payload (caller-owned).
|
||||
* @param payloadSize Byte count of payload.
|
||||
* @return true if at least one send succeeded (or no clients are connected).
|
||||
*/
|
||||
bool SendConfig(const uint8 *payload, uint32 payloadSize);
|
||||
|
||||
/**
|
||||
* @brief Send a DATA payload to all active clients.
|
||||
*
|
||||
* @param counter Per-update sequence counter (same for all fragments).
|
||||
* @param payload Pointer to fully-assembled DATA payload (caller-owned).
|
||||
* @param payloadSize Byte count of payload.
|
||||
* @return true if all sends succeeded.
|
||||
*/
|
||||
bool SendData(uint32 counter, const uint8 *payload, uint32 payloadSize);
|
||||
|
||||
/**
|
||||
* @brief Add a static unicast destination that never needs to send CONNECT.
|
||||
*
|
||||
* The entry is never evicted by the timeout mechanism. Duplicate
|
||||
* (ip, port) pairs are silently ignored.
|
||||
*
|
||||
* @param ip Destination IPv4 address string.
|
||||
* @param port Destination UDP port.
|
||||
* @return true if the client was added (or was already present).
|
||||
*/
|
||||
bool AddStaticClient(const char8 *ip, uint16 port);
|
||||
|
||||
/** @return Number of currently active clients (unicast + TCP). */
|
||||
uint32 GetClientCount() const;
|
||||
|
||||
/** @return true if at least one client is active. */
|
||||
bool HasClients() const;
|
||||
|
||||
/** @return true if server is in multicast mode. */
|
||||
bool IsMulticast() const;
|
||||
|
||||
/** @return Configured port number. */
|
||||
uint16 GetPort() const;
|
||||
|
||||
/** @return Configured maximum payload size (excluding header). */
|
||||
uint32 GetMaxPayloadSize() const;
|
||||
|
||||
private:
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Per-client state (unicast)
|
||||
// -------------------------------------------------------------------------
|
||||
struct UDPSClientEntry {
|
||||
char8 ipAddr[64]; ///< Client IPv4 address string
|
||||
uint16 clientPort; ///< Client source port
|
||||
uint64 lastSeenTicks; ///< HighResolutionTimer ticks at last contact
|
||||
bool active; ///< Slot is in use
|
||||
bool isStatic; ///< Never evicted by timeout; no CONNECT required
|
||||
};
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Private helpers
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* @brief Send a fragmented UDPS packet via a UDP socket.
|
||||
*
|
||||
* @param sock Socket to write through.
|
||||
* @param dest If non-NULL, SetDestination is called before each write
|
||||
* (unicast unconnected socket). If NULL, assumes the socket
|
||||
* is already connected (multicast dataSocket).
|
||||
* @param type UDPS_TYPE_DATA or UDPS_TYPE_CONFIG.
|
||||
* @param counter Sequence counter.
|
||||
* @param payload Payload bytes.
|
||||
* @param payloadSize Payload byte count.
|
||||
* @return true if all fragments were written without error.
|
||||
*/
|
||||
bool SendFragmentedUDP(BasicUDPSocket &sock,
|
||||
InternetHost *dest,
|
||||
uint8 type,
|
||||
uint32 counter,
|
||||
const uint8 *payload,
|
||||
uint32 payloadSize);
|
||||
|
||||
/**
|
||||
* @brief Send a fragmented UDPS packet via a TCP socket.
|
||||
*
|
||||
* Used in multicast mode to push CONFIG to each TCP control client.
|
||||
*/
|
||||
bool SendFragmentedTCP(BasicTCPSocket &sock,
|
||||
uint8 type,
|
||||
uint32 counter,
|
||||
const uint8 *payload,
|
||||
uint32 payloadSize);
|
||||
|
||||
/** Evict unicast clients that have been silent longer than clientTimeoutTicks. */
|
||||
void EvictStaleUnicastClients();
|
||||
|
||||
/** Handle a CONNECT datagram received on serverSocket. */
|
||||
void HandleUnicastConnect(const InternetHost &src);
|
||||
|
||||
/** Handle a DISCONNECT datagram received on serverSocket. */
|
||||
void HandleUnicastDisconnect(const InternetHost &src);
|
||||
|
||||
/** Handle an ACK datagram received on serverSocket. */
|
||||
void HandleUnicastAck(const InternetHost &src);
|
||||
|
||||
/**
|
||||
* @brief Find a unicast client slot by IP + port.
|
||||
* @return Slot index, or UDPS_SERVER_MAX_UNICAST_CLIENTS if not found.
|
||||
*/
|
||||
uint32 FindUnicastClient(const char8 *ip, uint16 port) const;
|
||||
|
||||
/**
|
||||
* @brief Find the oldest (lowest lastSeenTicks) non-static unicast client.
|
||||
* @return Slot index, or UDPS_SERVER_MAX_UNICAST_CLIENTS if none.
|
||||
*/
|
||||
uint32 FindOldestUnicastClient() const;
|
||||
|
||||
/** Close slot @p idx and mark it inactive. */
|
||||
void EvictUnicastClient(uint32 idx);
|
||||
|
||||
/** Accept a new multicast TCP connection and send cached CONFIG. */
|
||||
void HandleMulticastTCPConnect(BasicTCPSocket *newClient, uint32 idx);
|
||||
|
||||
/** Close TCP client slot @p idx. */
|
||||
void EvictTCPClient(uint32 idx);
|
||||
|
||||
/** Cache a copy of a CONFIG payload for future CONNECT clients. */
|
||||
bool CacheConfig(const uint8 *payload, uint32 payloadSize);
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Configuration
|
||||
// -------------------------------------------------------------------------
|
||||
uint16 port;
|
||||
uint32 maxPayloadSize;
|
||||
StreamString multicastGroup;
|
||||
uint16 dataPort;
|
||||
bool useMulticast;
|
||||
uint64 clientTimeoutTicks; ///< 0 = disabled
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Unicast state
|
||||
// -------------------------------------------------------------------------
|
||||
UDPSClientEntry unicastClients[UDPS_SERVER_MAX_UNICAST_CLIENTS];
|
||||
uint32 numUnicastClients;
|
||||
|
||||
BasicUDPSocket serverSocket; ///< Bound to port; receives CONNECT/DISCONNECT/ACK
|
||||
BasicUDPSocket uniSendSocket; ///< Unconnected; SetDestination before each Write
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Multicast state
|
||||
// -------------------------------------------------------------------------
|
||||
BasicTCPSocket tcpListener;
|
||||
BasicTCPSocket *tcpClients[UDPS_SERVER_MAX_TCP_CLIENTS];
|
||||
uint32 numTCPClients;
|
||||
BasicUDPSocket dataSocket; ///< Connected to multicastGroup:dataPort
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Shared state
|
||||
// -------------------------------------------------------------------------
|
||||
uint8 *cachedConfig; ///< Last SendConfig() payload (heap-allocated)
|
||||
uint32 cachedConfigSize;
|
||||
|
||||
uint8 *sendBuf; ///< Pre-allocated fragment TX buffer
|
||||
uint32 sendBufCapacity; ///< UDPS_HEADER_SIZE + maxPayloadSize
|
||||
|
||||
uint32 configCounter; ///< Sequence counter for CONFIG packets
|
||||
bool started;
|
||||
};
|
||||
|
||||
} // namespace MARTe
|
||||
|
||||
#endif // UDPS_SERVER_H_
|
||||
Reference in New Issue
Block a user