martino/MARTe-Integrated-Components
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
05b9490b06ad8380bec685d743ca030d68760df1
MARTe-Integrated-Components/Source/Components/Interfaces/UDPStream/TCPSClient.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

518 lines
16 KiB
C++
Raw Blame History

/**
* @file TCPSClient.cpp
* @brief Implementation of TCPSClient — UDPS framing over TCP, consumer side.
*/
#include "TCPSClient.h"
#include "AdvancedErrorManagement.h"
#include "ErrorType.h"
#include "HighResolutionTimer.h"
#include "Sleep.h"
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <netdb.h>
#include <netinet/in.h>
#include <poll.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
namespace MARTe {
const uint32 TCPSClient::TCPS_CLIENT_DEFAULT_RECONNECT_DELAY_S;
/** Longest single wait inside Execute(), so Stop() is prompt. */
static const int TCPS_CLIENT_SLICE_MS = 50;
/** Total time allowed for one connect() attempt. */
static const int TCPS_CLIENT_CONNECT_TIMEOUT_MS = 2000;
TCPSClient::TCPSClient()
: serverPort(0u), silenceTimeoutTicks(0u), reconnectDelayTicks(0u),
cpuMask(0xFFFFFFFFu), stackSize(65536u),
numStreams(1u), startRefs(0u), threadService(*this), fd(-1),
connected(false), stopping(false), lastDataTicks(0u), disconnectTick(0u),
connectionCount(0u), protocolErrors(0u), rxBuf(NULL_PTR(uint8 *)),
rxHave(0u), rxWant(UDPS_HEADER_SIZE), rxHeaderOk(false)
{
for (uint32 i = 0u; i < UDPS_TCP_MAX_STREAMS; i++) {
listeners[i] = NULL_PTR(UDPSClientListener *);
facades[i] = NULL_PTR(TCPSClientStream *);
lastDataCounter[i] = 0u;
lastDataCounterValid[i] = false;
lastDataGap[i] = 0u;
}
reconnectDelayTicks = static_cast<uint64>(TCPS_CLIENT_DEFAULT_RECONNECT_DELAY_S) *
HighResolutionTimer::Frequency();
}
TCPSClient::~TCPSClient() {
(void)Stop();
for (uint32 i = 0u; i < UDPS_TCP_MAX_STREAMS; i++) {
if (facades[i] != NULL_PTR(TCPSClientStream *)) {
delete facades[i];
facades[i] = NULL_PTR(TCPSClientStream *);
}
}
if (rxBuf != NULL_PTR(uint8 *)) {
delete[] rxBuf;
rxBuf = NULL_PTR(uint8 *);
}
}
bool TCPSClient::Initialise(StructuredDataI &data) {
bool ok = true;
if (!data.Read("ServerAddr", serverAddr) || (serverAddr.Size() == 0u)) {
serverAddr = "127.0.0.1";
}
if (!data.Read("Port", serverPort) || (serverPort == 0u) || (serverPort > 65535u)) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"TCPSClient: a valid Port (1..65535) is required.");
ok = false;
}
streamName = "";
numStreams = 1u;
if (data.Read("StreamName", streamName)) {
ok = ParseStreamList();
}
float32 silence = 0.0f;
(void)data.Read("SilenceTimeout", silence);
if (silence < 0.0f) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"TCPSClient: SilenceTimeout must be >= 0.");
ok = false;
silence = 0.0f;
}
silenceTimeoutTicks = static_cast<uint64>(
static_cast<float64>(silence) * static_cast<float64>(HighResolutionTimer::Frequency()));
float32 delay = static_cast<float32>(TCPS_CLIENT_DEFAULT_RECONNECT_DELAY_S);
(void)data.Read("ReconnectDelay", delay);
if (delay < 0.0f) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"TCPSClient: ReconnectDelay must be >= 0.");
ok = false;
delay = 0.0f;
}
reconnectDelayTicks = static_cast<uint64>(
static_cast<float64>(delay) * static_cast<float64>(HighResolutionTimer::Frequency()));
(void)data.Read("CPUMask", cpuMask);
(void)data.Read("StackSize", stackSize);
return ok;
}
/** Validate streamName as "a" or "a,b,c" and set numStreams. */
bool TCPSClient::ParseStreamList() {
bool ok = (streamName.Size() <= UDPS_TCP_MAX_SUBSCRIBE_PAYLOAD);
const char8 *s = streamName.Buffer();
uint32 n = 0u;
while (ok && (s != NULL_PTR(const char8 *)) && (streamName.Size() > 0u)) {
const char8 *comma = strchr(s, ',');
const uint32 len = (comma != NULL_PTR(const char8 *)) ? static_cast<uint32>(comma - s)
: static_cast<uint32>(strlen(s));
ok = (len > 0u) && (len <= 255u) && (n < UDPS_TCP_MAX_STREAMS);
// Duplicates would be refused by the server; catch them here.
const char8 *t = streamName.Buffer();
for (uint32 k = 0u; ok && (k < n); k++) {
const char8 *tc = strchr(t, ',');
const uint32 tl = (tc != NULL_PTR(const char8 *)) ? static_cast<uint32>(tc - t)
: static_cast<uint32>(strlen(t));
if ((tl == len) && (strncmp(t, s, len) == 0)) {
ok = false;
}
t = (tc != NULL_PTR(const char8 *)) ? (tc + 1) : t;
}
n++;
s = (comma != NULL_PTR(const char8 *)) ? (comma + 1) : NULL_PTR(const char8 *);
}
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"TCPSClient: StreamName must be 1..%u distinct names (1..255 chars each, "
"comma separated, %u bytes max).", UDPS_TCP_MAX_STREAMS,
UDPS_TCP_MAX_SUBSCRIBE_PAYLOAD);
} else if (n > 0u) {
numStreams = n;
}
return ok;
}
void TCPSClient::SetListener(UDPSClientListener *newListener) {
listeners[0] = newListener;
}
void TCPSClient::SetStreamListener(uint32 index, UDPSClientListener *newListener) {
if (index < UDPS_TCP_MAX_STREAMS) {
listeners[index] = newListener;
}
}
uint32 TCPSClient::GetNumStreams() const {
return numStreams;
}
UDPSClientI *TCPSClient::GetStream(uint32 index) {
if (index >= numStreams) {
return NULL_PTR(UDPSClientI *);
}
if (facades[index] == NULL_PTR(TCPSClientStream *)) {
facades[index] = new TCPSClientStream(*this, index);
}
return facades[index];
}
uint32 TCPSClient::GetStreamDataGap(uint32 index) const {
return (index < UDPS_TCP_MAX_STREAMS) ? lastDataGap[index] : 0u;
}
bool TCPSClient::AcquireStart() {
bool ok = true;
refMutex.FastLock();
if (startRefs == 0u) {
ok = Start();
}
if (ok) {
startRefs++;
}
refMutex.FastUnLock();
return ok;
}
bool TCPSClient::ReleaseStop() {
bool ok = true;
refMutex.FastLock();
if (startRefs > 0u) {
startRefs--;
if (startRefs == 0u) {
ok = Stop();
}
}
refMutex.FastUnLock();
return ok;
}
TCPSClientStream::TCPSClientStream(TCPSClient &o, uint32 i) : owner(o), index(i), started(false) {
}
TCPSClientStream::~TCPSClientStream() {
(void)Stop();
}
bool TCPSClientStream::Initialise(StructuredDataI &data) {
(void)data;
return true;
}
void TCPSClientStream::SetListener(UDPSClientListener *newListener) {
owner.SetStreamListener(index, newListener);
}
bool TCPSClientStream::Start() {
bool ok = true;
if (!started) {
ok = owner.AcquireStart();
started = ok;
}
return ok;
}
bool TCPSClientStream::Stop() {
bool ok = true;
if (started) {
started = false;
ok = owner.ReleaseStop();
}
return ok;
}
uint32 TCPSClientStream::GetLastDataGap() const {
return owner.GetStreamDataGap(index);
}
bool TCPSClient::Start() {
if (rxBuf == NULL_PTR(uint8 *)) {
rxBuf = new uint8[UDPS_HEADER_SIZE + UDPSClient::UDPS_CLIENT_MAX_PACKET_BYTES];
}
stopping = false;
connectionCount = 0u;
disconnectTick = 0u;
threadService.SetCPUMask(cpuMask);
threadService.SetStackSize(stackSize);
ErrorManagement::ErrorType err = threadService.Start();
return (err == ErrorManagement::NoError);
}
bool TCPSClient::Stop() {
stopping = true;
ErrorManagement::ErrorType err = threadService.Stop();
Disconnect();
return (err == ErrorManagement::NoError);
}
bool TCPSClient::IsConnected() const {
return connected;
}
uint32 TCPSClient::GetLastDataGap() const {
return lastDataGap[0];
}
uint32 TCPSClient::GetConnectionCount() const {
return connectionCount;
}
uint32 TCPSClient::GetProtocolErrors() const {
return protocolErrors;
}
// ---------------------------------------------------------------------------
// Thread entry
// ---------------------------------------------------------------------------
ErrorManagement::ErrorType TCPSClient::Execute(ExecutionInfo &info) {
if (info.GetStage() == ExecutionInfo::TerminationStage) {
Disconnect();
return ErrorManagement::NoError;
}
if (info.GetStage() != ExecutionInfo::MainStage) {
return ErrorManagement::NoError;
}
if (!connected) {
const uint64 now = HighResolutionTimer::Counter();
if ((disconnectTick == 0u) || ((now - disconnectTick) >= reconnectDelayTicks)) {
if (!Connect()) {
disconnectTick = HighResolutionTimer::Counter();
}
} else {
Sleep::MSec(10u);
}
return ErrorManagement::NoError;
}
if (!ReceiveStep()) {
Disconnect();
return ErrorManagement::NoError;
}
if (silenceTimeoutTicks > 0u) {
const uint64 now = HighResolutionTimer::Counter();
if ((now > lastDataTicks) && ((now - lastDataTicks) >= silenceTimeoutTicks)) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"TCPSClient: server silent; reconnecting.");
Disconnect();
}
}
return ErrorManagement::NoError;
}
// ---------------------------------------------------------------------------
// Connection management
// ---------------------------------------------------------------------------
bool TCPSClient::Connect() {
struct sockaddr_in addr;
memset(&addr, 0, sizeof(addr));
addr.sin_family = AF_INET;
addr.sin_port = htons(static_cast<uint16>(serverPort));
if (inet_pton(AF_INET, serverAddr.Buffer(), &addr.sin_addr) != 1) {
struct addrinfo hints;
struct addrinfo *res = NULL_PTR(struct addrinfo *);
memset(&hints, 0, sizeof(hints));
hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_STREAM;
if ((getaddrinfo(serverAddr.Buffer(), NULL_PTR(const char8 *), &hints, &res) != 0) ||
(res == NULL_PTR(struct addrinfo *))) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"TCPSClient: cannot resolve '%s'.", serverAddr.Buffer());
return false;
}
addr.sin_addr = reinterpret_cast<struct sockaddr_in *>(res->ai_addr)->sin_addr;
freeaddrinfo(res);
}
int s = socket(AF_INET, SOCK_STREAM, 0);
if (s < 0) {
return false;
}
int flags = fcntl(s, F_GETFL, 0);
if ((flags < 0) || (fcntl(s, F_SETFL, flags | O_NONBLOCK) != 0)) {
(void)close(s);
return false;
}
bool ok = true;
int r = connect(s, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr));
if ((r != 0) && (errno == EINPROGRESS)) {
ok = false;
for (int waited = 0; (waited < TCPS_CLIENT_CONNECT_TIMEOUT_MS) && !stopping;
waited += TCPS_CLIENT_SLICE_MS) {
struct pollfd p;
p.fd = s;
p.events = POLLOUT;
p.revents = 0;
int pr = poll(&p, 1, TCPS_CLIENT_SLICE_MS);
if (pr > 0) {
int soErr = 0;
socklen_t len = static_cast<socklen_t>(sizeof(soErr));
ok = (getsockopt(s, SOL_SOCKET, SO_ERROR, &soErr, &len) == 0) && (soErr == 0);
break;
}
if ((pr < 0) && (errno != EINTR)) {
break;
}
}
} else if (r != 0) {
ok = false;
}
if (!ok) {
(void)close(s);
return false;
}
if (streamName.Size() > 0u) {
// A few dozen bytes on a fresh socket: never blocks.
uint8 sub[UDPS_HEADER_SIZE + UDPS_TCP_MAX_SUBSCRIBE_PAYLOAD];
const uint32 n = static_cast<uint32>(streamName.Size());
UDPSBuildHeader(sub, UDPS_TYPE_SUBSCRIBE, 0u, 0u, 1u, n);
memcpy(&sub[UDPS_HEADER_SIZE], streamName.Buffer(), n);
if (send(s, sub, UDPS_HEADER_SIZE + n, MSG_NOSIGNAL) != static_cast<ssize_t>(UDPS_HEADER_SIZE + n)) {
(void)close(s);
return false;
}
}
fd = s;
rxHave = 0u;
rxWant = UDPS_HEADER_SIZE;
rxHeaderOk = false;
for (uint32 i = 0u; i < UDPS_TCP_MAX_STREAMS; i++) {
lastDataCounterValid[i] = false;
}
lastDataTicks = HighResolutionTimer::Counter();
connectionCount++;
connected = true;
for (uint32 i = 0u; i < numStreams; i++) {
if (listeners[i] != NULL_PTR(UDPSClientListener *)) {
listeners[i]->OnUDPSConnected();
}
}
return true;
}
void TCPSClient::Disconnect() {
const bool was = connected;
connected = false;
if (fd >= 0) {
(void)close(fd);
fd = -1;
}
rxHave = 0u;
rxWant = UDPS_HEADER_SIZE;
rxHeaderOk = false;
disconnectTick = HighResolutionTimer::Counter();
for (uint32 i = 0u; was && (i < numStreams); i++) {
if (listeners[i] != NULL_PTR(UDPSClientListener *)) {
listeners[i]->OnUDPSDisconnected();
}
}
}
// ---------------------------------------------------------------------------
// Frame reader
// ---------------------------------------------------------------------------
bool TCPSClient::ReceiveStep() {
struct pollfd p;
p.fd = fd;
p.events = POLLIN;
p.revents = 0;
int pr = poll(&p, 1, TCPS_CLIENT_SLICE_MS);
if (pr < 0) {
return (errno == EINTR);
}
if (pr == 0) {
return true;
}
ssize_t n = recv(fd, &rxBuf[rxHave], rxWant - rxHave, MSG_DONTWAIT);
if (n == 0) {
REPORT_ERROR_STATIC(ErrorManagement::Information,
"TCPSClient: server closed the connection.");
return false;
}
if (n < 0) {
if ((errno == EAGAIN) || (errno == EWOULDBLOCK) || (errno == EINTR)) {
return true;
}
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"TCPSClient: recv failed: %s.", strerror(errno));
return false;
}
rxHave += static_cast<uint32>(n);
lastDataTicks = HighResolutionTimer::Counter();
if (!rxHeaderOk && (rxHave >= UDPS_HEADER_SIZE)) {
UDPSPacketHeader h;
memcpy(&h, rxBuf, UDPS_HEADER_SIZE);
if ((h.magic != UDPS_MAGIC) ||
(h.payloadBytes > UDPSClient::UDPS_CLIENT_MAX_PACKET_BYTES) ||
(h.totalFragments != 1u) || (h.fragmentIdx >= numStreams)) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"TCPSClient: invalid frame header (magic 0x%x, %u payload bytes); reconnecting.",
h.magic, h.payloadBytes);
protocolErrors++;
return false;
}
rxHeaderOk = true;
rxWant = UDPS_HEADER_SIZE + h.payloadBytes;
}
if (rxHeaderOk && (rxHave == rxWant)) {
const bool ok = Dispatch();
rxHave = 0u;
rxWant = UDPS_HEADER_SIZE;
rxHeaderOk = false;
return ok;
}
return true;
}
bool TCPSClient::Dispatch() {
UDPSPacketHeader h;
memcpy(&h, rxBuf, UDPS_HEADER_SIZE);
const uint8 *payload = &rxBuf[UDPS_HEADER_SIZE];
const uint32 idx = h.fragmentIdx; // validated < numStreams
UDPSClientListener *listener = listeners[idx];
if (listener != NULL_PTR(UDPSClientListener *)) {
if (h.type == UDPS_TYPE_CONFIG) {
listener->OnUDPSConfig(payload, h.payloadBytes);
} else if (h.type == UDPS_TYPE_DATA) {
lastDataGap[idx] = 0u;
if (lastDataCounterValid[idx]) {
const int32 delta = static_cast<int32>(h.counter - lastDataCounter[idx]);
if (delta > 1) {
lastDataGap[idx] = static_cast<uint32>(delta) - 1u;
}
}
lastDataCounter[idx] = h.counter;
lastDataCounterValid[idx] = true;
listener->OnUDPSFragment(h.counter, rxWant, true);
listener->OnUDPSData(payload, h.payloadBytes);
} else {
// Unknown / client-to-server types are ignored (forward compatible).
}
}
return true;
}
} // namespace MARTe
Reference in new issue View Git Blame Copy Permalink