martino/MARTe-Integrated-Components
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
MARTe-Integrated-Components/Source/Components/Interfaces/TCPStreamService/TCPStreamJoin.cpp
T
Martino FerrariandClaude Opus 4.6 50d85363a3 feat(tcp): TCPStreamer DataSources, in-process TCPStreamService, stream multiplexing and TCPStreamJoin 
Add TCPStreamer/TCPStreamerClient (TCP transport behind the UDPSServerI/UDPSClientI
seam), an optional single in-process TCPStreamService serving many streams over
worker threads, multiplexed SUBSCRIBE (comma list, stream index in fragmentIdx),
and TCPStreamJoin, which merges several streams into one sample-and-hold stream
paced by a master. StreamHub takes a comma list of streams over one connection.
Includes GTests, E2E scenarios s58-s73 and docs.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
2026-11-06 13:41:27 +01:00

495 lines
17 KiB
C++
Raw Permalink Blame History

/**
* @file TCPStreamJoin.cpp
* @brief Source file for class TCPStreamJoin
* @date 06/11/2026
* @author Martino Ferrari
*
* @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
* the Development of Fusion Energy ('Fusion for Energy').
* Licensed under the EUPL, Version 1.1 or - as soon they will be approved
* by the European Commission - subsequent versions of the EUPL (the "Licence")
* You may not use this work except in compliance with the Licence.
* You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
*
* @warning Unless required by applicable law or agreed to in writing,
* software distributed under the Licence is distributed on an "AS IS"
* basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
* or implied. See the Licence permissions and limitations under the Licence.
*/
#define DLL_API
#include "TCPStreamJoin.h"
#include "AdvancedErrorManagement.h"
#include "ConfigurationDatabase.h"
#include "ObjectRegistryDatabase.h"
#include "UDPSProtocol.h"
#include <stdio.h>
#include <string.h>
namespace MARTe {
const uint32 TCPStreamJoin::TCPSJ_MAX_SOURCES;
TCPStreamJoin::Source::Source()
: stream(NULL_PTR(TCPSServiceStream *)), configOk(false), cfg(NULL_PTR(uint8 *)),
cfgBytes(0u), numSigs(0u), hrt(0u), body(NULL_PTR(uint8 *)), bodyCap(0u),
bodySize(0u), haveBody(false) {
}
TCPStreamJoin::Source::~Source() {
if (cfg != NULL_PTR(uint8 *)) {
delete[] cfg;
}
if (body != NULL_PTR(uint8 *)) {
delete[] body;
}
}
TCPStreamJoin::TCPStreamJoin()
: Object(), TCPSStreamTapI(), TCPSStreamObserverI(), out(NULL_PTR(TCPSServiceStream *)),
observing(false), src(NULL_PTR(Source *)), numSrc(0u), master(0u), maxPayload(60000u),
active(false), configPublished(false), sizeLogged(false), outCounter(0u), emitted(0u),
scratch(NULL_PTR(uint8 *)), scratchCap(0u) {
}
TCPStreamJoin::~TCPStreamJoin() {
if (observing) {
service->RemoveObserver(this);
observing = false;
}
for (uint32 i = 0u; i < numSrc; i++) {
mtx.FastLock();
TCPSServiceStream *s = src[i].stream;
src[i].stream = NULL_PTR(TCPSServiceStream *);
mtx.FastUnLock();
if (s != NULL_PTR(TCPSServiceStream *)) {
s->RemoveTap(this);
}
}
if (out != NULL_PTR(TCPSServiceStream *)) {
delete out; // unregisters the merged stream
out = NULL_PTR(TCPSServiceStream *);
}
if (src != NULL_PTR(Source *)) {
delete[] src;
}
if (scratch != NULL_PTR(uint8 *)) {
delete[] scratch;
}
}
bool TCPStreamJoin::Initialise(StructuredDataI &data) {
bool ok = Object::Initialise(data);
StreamString serviceName;
if (ok && (!data.Read("Service", serviceName) || (serviceName.Size() == 0u))) {
REPORT_ERROR(ErrorManagement::ParametersError, "Parameter 'Service' is required.");
ok = false;
}
if (ok && (!data.Read("StreamName", outName) || (outName.Size() == 0u))) {
outName = GetName();
}
StreamString sources;
if (ok && (!data.Read("Sources", sources) || (sources.Size() == 0u))) {
REPORT_ERROR(ErrorManagement::ParametersError, "Parameter 'Sources' is required.");
ok = false;
}
StreamString masterName;
if (ok && !data.Read("Master", masterName)) {
masterName = "";
}
maxPayload = 60000u;
(void)data.Read("MaxPayloadSize", maxPayload);
if (ok) {
// Split "a, b ,c" into trimmed names.
src = new Source[TCPSJ_MAX_SOURCES];
const char8 *p = sources.Buffer();
while (ok && (p != NULL_PTR(const char8 *))) {
const char8 *comma = strchr(p, ',');
const char8 *end = (comma != NULL_PTR(const char8 *)) ? comma : (p + strlen(p));
while ((p < end) && (*p == ' ')) {
p++;
}
const char8 *e = end;
while ((e > p) && (e[-1] == ' ')) {
e--;
}
const uint32 len = static_cast<uint32>(e - p);
if ((len == 0u) || (len > TCPS_MAX_STREAM_NAME) || (numSrc >= TCPSJ_MAX_SOURCES)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Sources must list 1..%u names of 1..%u characters.",
TCPSJ_MAX_SOURCES, TCPS_MAX_STREAM_NAME);
ok = false;
}
if (ok) {
StreamString name;
uint32 wlen = len;
(void)name.Write(p, wlen);
for (uint32 i = 0u; i < numSrc; i++) {
if (src[i].name == name) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Source '%s' is listed twice.", name.Buffer());
ok = false;
}
}
if (name == outName) {
REPORT_ERROR(ErrorManagement::ParametersError,
"The merged stream '%s' cannot be one of its own sources.",
name.Buffer());
ok = false;
}
src[numSrc].name = name;
numSrc++;
}
p = (comma != NULL_PTR(const char8 *)) ? (comma + 1) : NULL_PTR(const char8 *);
}
}
if (ok && (masterName.Size() > 0u)) {
const int32 m = FindByName(masterName.Buffer());
if (m < 0) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Master '%s' is not one of the Sources.", masterName.Buffer());
ok = false;
}
else {
master = static_cast<uint32>(m);
}
}
if (ok) {
service = ObjectRegistryDatabase::Instance()->Find(serviceName.Buffer());
if (!service.IsValid()) {
REPORT_ERROR(ErrorManagement::InitialisationError,
"No TCPStreamService named '%s' (declare it before the join).",
serviceName.Buffer());
ok = false;
}
}
if (ok) {
out = new TCPSServiceStream(serviceName.Buffer(), outName.Buffer());
ConfigurationDatabase scdb;
ok = scdb.Write("MaxPayloadSize", maxPayload) && scdb.MoveToRoot() && out->Initialise(scdb);
}
if (ok) {
// Last: the replay of already registered streams may complete the join right here.
observing = service->AddObserver(this);
if (!observing) {
REPORT_ERROR(ErrorManagement::InitialisationError,
"The service cannot take another observer.");
ok = false;
}
}
return ok;
}
int32 TCPStreamJoin::FindByName(const char8 *name) const {
int32 r = -1;
for (uint32 i = 0u; (i < numSrc) && (r < 0); i++) {
if (strcmp(src[i].name.Buffer(), name) == 0) {
r = static_cast<int32>(i);
}
}
return r;
}
int32 TCPStreamJoin::FindByStream(const TCPSServiceStream *s) const {
int32 r = -1;
for (uint32 i = 0u; (i < numSrc) && (r < 0); i++) {
if (src[i].stream == s) {
r = static_cast<int32>(i);
}
}
return r;
}
bool TCPStreamJoin::AllRegistered() const {
bool all = true;
for (uint32 i = 0u; i < numSrc; i++) {
all = all && (src[i].stream != NULL_PTR(TCPSServiceStream *));
}
return all;
}
bool TCPStreamJoin::IsActive() const {
return active;
}
bool TCPStreamJoin::HasConfig() const {
return configPublished;
}
uint64 TCPStreamJoin::GetFramesEmitted() const {
return emitted;
}
uint32 TCPStreamJoin::GetNumSources() const {
return numSrc;
}
void TCPStreamJoin::OnStreamRegistered(TCPSServiceStream *stream) {
bool mine = false;
int32 idx = -1;
mtx.FastLock();
idx = FindByName(stream->GetStreamName());
if ((idx >= 0) && (src[idx].stream == NULL_PTR(TCPSServiceStream *))) {
src[idx].stream = stream;
mine = true;
}
mtx.FastUnLock();
if (!mine) {
return;
}
// Replays the cached CONFIG into OnTapConfig; the join mutex must not be held.
if (!stream->AddTap(this)) {
REPORT_ERROR(ErrorManagement::InitialisationError,
"Stream '%s' cannot take another tap.", stream->GetStreamName());
mtx.FastLock();
src[idx].stream = NULL_PTR(TCPSServiceStream *);
mtx.FastUnLock();
return;
}
Evaluate();
}
void TCPStreamJoin::Evaluate() {
mtx.FastLock();
const bool start = !active && AllRegistered();
mtx.FastUnLock();
if (start) {
// Registering may call back into observers (including this one): no lock held.
const bool ok = out->Start();
mtx.FastLock();
if (ok) {
active = true;
outCounter = 0u;
PublishConfigLocked();
}
mtx.FastUnLock();
}
}
void TCPStreamJoin::OnStreamUnregistered(TCPSServiceStream *stream) {
mtx.FastLock();
const int32 idx = FindByStream(stream);
bool wasActive = false;
if (idx >= 0) {
Source &s = src[idx];
s.stream = NULL_PTR(TCPSServiceStream *);
s.configOk = false;
s.haveBody = false;
s.bodySize = 0u;
wasActive = active;
active = false;
configPublished = false;
}
mtx.FastUnLock();
if (idx < 0) {
return;
}
stream->RemoveTap(this);
if (wasActive) {
(void)out->Stop(); // subscribers of the merged stream are dropped
}
}
bool TCPStreamJoin::ParseConfig(Source &s, const uint8 *payload, uint32 n) {
bool ok = (n >= 5u);
uint32 numSigs = 0u;
if (ok) {
memcpy(&numSigs, payload, 4u);
const uint64 need = 4u + static_cast<uint64>(numSigs) * UDPS_SIGNAL_DESC_SIZE + 1u;
ok = (numSigs > 0u) && (need <= n);
}
if (ok) {
const uint32 off = 4u + numSigs * UDPS_SIGNAL_DESC_SIZE;
if (payload[off] == UDPS_PUBLISH_ACCUMULATE) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Source '%s' uses Accumulate publishing, which cannot be merged.",
s.name.Buffer());
ok = false;
}
else {
s.numSigs = numSigs;
s.hrt = 0u;
if (n >= (off + 1u + 8u)) {
memcpy(&s.hrt, &payload[off + 1u], 8u);
}
}
}
else {
REPORT_ERROR(ErrorManagement::ParametersError,
"Source '%s' sent a malformed CONFIG.", s.name.Buffer());
}
return ok;
}
void TCPStreamJoin::OnTapConfig(TCPSServiceStream *source, const uint8 *payload, uint32 payloadSize) {
mtx.FastLock();
const int32 idx = FindByStream(source);
if (idx >= 0) {
Source &s = src[idx];
s.haveBody = false;
s.bodySize = 0u;
configPublished = false;
sizeLogged = false;
s.configOk = ParseConfig(s, payload, payloadSize);
if (s.configOk) {
if (s.cfgBytes < payloadSize) {
if (s.cfg != NULL_PTR(uint8 *)) {
delete[] s.cfg;
}
s.cfg = new uint8[payloadSize];
}
memcpy(s.cfg, payload, payloadSize);
s.cfgBytes = payloadSize;
}
PublishConfigLocked();
}
mtx.FastUnLock();
}
/** Caller holds `mtx`. */
void TCPStreamJoin::PublishConfigLocked() {
configPublished = false;
bool ok = active;
uint64 totalSigs = 0u;
uint64 hrt = UDPS_HRT_FREQUENCY_UNKNOWN;
for (uint32 i = 0u; ok && (i < numSrc); i++) {
ok = src[i].configOk;
totalSigs += src[i].numSigs;
if (ok && (src[i].hrt != UDPS_HRT_FREQUENCY_UNKNOWN)) {
if ((hrt != UDPS_HRT_FREQUENCY_UNKNOWN) && (hrt != src[i].hrt)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Join '%s': sources have different HRT frequencies, cannot merge.",
outName.Buffer());
ok = false;
}
hrt = src[i].hrt;
}
}
const uint64 cfgSize = 4u + totalSigs * UDPS_SIGNAL_DESC_SIZE + 1u + 8u;
if (ok && (cfgSize > maxPayload)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Join '%s': merged CONFIG of %u bytes exceeds MaxPayloadSize.",
outName.Buffer(), static_cast<uint32>(cfgSize));
ok = false;
}
if (!ok) {
return;
}
uint8 *buf = new uint8[static_cast<uint32>(cfgSize)];
const uint32 nSigs = static_cast<uint32>(totalSigs);
memcpy(buf, &nSigs, 4u);
uint32 pos = 4u;
uint32 base = 0u;
for (uint32 i = 0u; ok && (i < numSrc); i++) {
const char8 *prefix = src[i].name.Buffer();
for (uint32 k = 0u; ok && (k < src[i].numSigs); k++) {
UDPSSignalDescriptor d;
memcpy(&d, &src[i].cfg[4u + k * UDPS_SIGNAL_DESC_SIZE], UDPS_SIGNAL_DESC_SIZE);
d.name[UDPS_MAX_SIGNAL_NAME - 1u] = '\0';
const uint32 len = static_cast<uint32>(strlen(prefix) + 1u + strlen(d.name));
if (len >= UDPS_MAX_SIGNAL_NAME) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Join '%s': signal name '%s_%s' is longer than %u characters.",
outName.Buffer(), prefix, d.name, UDPS_MAX_SIGNAL_NAME - 1u);
ok = false;
}
else {
char8 merged[UDPS_MAX_SIGNAL_NAME];
(void)snprintf(merged, sizeof(merged), "%s_%s", prefix, d.name);
memset(d.name, 0, sizeof(d.name));
memcpy(d.name, merged, strlen(merged));
if (d.timeSignalIdx != UDPS_NO_TIME_SIGNAL) {
d.timeSignalIdx += base;
}
memcpy(&buf[pos], &d, UDPS_SIGNAL_DESC_SIZE);
pos += UDPS_SIGNAL_DESC_SIZE;
}
}
base += src[i].numSigs;
}
if (ok) {
buf[pos] = UDPS_PUBLISH_STRICT;
pos += 1u;
memcpy(&buf[pos], &hrt, 8u);
pos += 8u;
ok = out->SendConfig(buf, pos);
}
configPublished = ok;
delete[] buf;
}
void TCPStreamJoin::OnTapData(TCPSServiceStream *source, uint32 counter, const uint8 *payload,
uint32 payloadSize) {
(void)counter;
mtx.FastLock();
const int32 idx = FindByStream(source);
if ((idx >= 0) && src[idx].configOk && (payloadSize >= 8u)) {
Source &s = src[idx];
const uint32 bl = payloadSize - 8u;
// A size change without a new CONFIG would corrupt the merged layout: ignore.
if (!s.haveBody || (s.bodySize == bl)) {
if (s.bodyCap < bl) {
if (s.body != NULL_PTR(uint8 *)) {
delete[] s.body;
}
s.body = new uint8[(bl > 0u) ? bl : 1u];
s.bodyCap = (bl > 0u) ? bl : 1u;
}
if (bl > 0u) {
memcpy(s.body, &payload[8], bl);
}
s.bodySize = bl;
s.haveBody = true;
if (active && configPublished && (static_cast<uint32>(idx) == master)) {
uint64 total = 8u;
bool ready = true;
for (uint32 i = 0u; i < numSrc; i++) {
ready = ready && src[i].haveBody;
total += src[i].bodySize;
}
if (ready && (total > maxPayload)) {
if (!sizeLogged) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Join '%s': merged frame of %u bytes exceeds MaxPayloadSize.",
outName.Buffer(), static_cast<uint32>(total));
sizeLogged = true;
}
ready = false;
}
if (ready) {
if (scratchCap < total) {
if (scratch != NULL_PTR(uint8 *)) {
delete[] scratch;
}
scratch = new uint8[static_cast<uint32>(total)];
scratchCap = static_cast<uint32>(total);
}
memcpy(scratch, payload, 8u); // master timestamp
uint32 pos = 8u;
for (uint32 i = 0u; i < numSrc; i++) {
if (src[i].bodySize > 0u) {
memcpy(&scratch[pos], src[i].body, src[i].bodySize);
}
pos += src[i].bodySize;
}
outCounter++;
if (out->SendData(outCounter, scratch, pos)) {
emitted = emitted + 1u;
}
}
}
}
}
mtx.FastUnLock();
}
CLASS_REGISTER(TCPStreamJoin, "1.0")
} /* namespace MARTe */
Reference in new issue View Git Blame Copy Permalink