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/UDPStream/TCPSServer.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

607 lines
19 KiB
C++
Raw Permalink Blame History

/**
* @file TCPSServer.cpp
* @brief Implementation of TCPSServer — UDPS framing over TCP.
*/
#include "TCPSServer.h"
#include "AdvancedErrorManagement.h"
#include "ErrorType.h"
#include "HighResolutionTimer.h"
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
namespace MARTe {
// Out-of-class definitions: the constants are passed by reference (gtest,
// error-report varargs templates), which odr-uses them.
const uint32 TCPSServer::TCPS_DEFAULT_PORT;
const uint32 TCPSServer::TCPS_DEFAULT_MAX_PAYLOAD;
const uint32 TCPSServer::TCPS_MAX_FRAME_PAYLOAD;
const uint32 TCPSServer::TCPS_DEFAULT_MAX_CLIENTS;
const uint32 TCPSServer::TCPS_MAX_CLIENTS_LIMIT;
const uint32 TCPSServer::TCPS_DEFAULT_QUEUE_DEPTH;
const uint32 TCPSServer::TCPS_QUEUE_DEPTH_LIMIT;
const uint32 TCPSServer::TCPS_DEFAULT_STALL_TIMEOUT_S;
// ---------------------------------------------------------------------------
// Per-client state
// ---------------------------------------------------------------------------
/** One queued frame (header + payload). The buffer is reused across frames. */
struct TCPSServer::Slot {
Slot() : buf(NULL_PTR(uint8 *)), cap(0u), len(0u), isConfig(false) {}
~Slot() {
if (buf != NULL_PTR(uint8 *)) {
delete[] buf;
}
}
uint8 *buf;
uint32 cap;
uint32 len;
bool isConfig;
};
/**
* Queue layout: slots[0..count) are pending in wire order; slots[count..cap)
* are free (their buffers are kept for reuse). When sent > 0, slots[0] is
* partially on the wire and must not be touched.
*
* Capacity: at most queueDepth DATA frames, one CONFIG that is on the wire
* and one CONFIG waiting behind it => queueDepth + 2.
*/
struct TCPSServer::Client {
Client() : fd(-1), slots(NULL_PTR(Slot *)), cap(0u), count(0u), sent(0u),
lastProgress(0u) {}
~Client() {
if (slots != NULL_PTR(Slot *)) {
delete[] slots;
}
if (fd >= 0) {
(void)close(fd);
}
}
int fd;
Slot *slots;
uint32 cap;
uint32 count;
uint32 sent; ///< bytes of slots[0] already written
uint64 lastProgress; ///< HRT ticks of the last byte written / queue-empty->non-empty
};
// ---------------------------------------------------------------------------
// Constructor / Destructor
// ---------------------------------------------------------------------------
TCPSServer::TCPSServer()
: port(TCPS_DEFAULT_PORT), boundPort(0u),
maxPayloadSize(TCPS_DEFAULT_MAX_PAYLOAD),
maxClients(TCPS_DEFAULT_MAX_CLIENTS),
queueDepth(TCPS_DEFAULT_QUEUE_DEPTH),
stallTimeoutS(TCPS_DEFAULT_STALL_TIMEOUT_S), sendBufferSize(0u),
listenFd(-1), started(false), clients(NULL_PTR(Client *)), numClients(0u),
cachedConfig(NULL_PTR(uint8 *)), cachedConfigSize(0u), configCounter(0u),
frameBuf(NULL_PTR(uint8 *)), frameCapacity(0u), frameSize(0u),
stallTicks(0u), totalDrops(0u), loggedDrops(0u), lastDropLogTicks(0u),
stallEvictions(0u) {
}
TCPSServer::~TCPSServer() {
(void)Stop();
}
// ---------------------------------------------------------------------------
// Initialise
// ---------------------------------------------------------------------------
bool TCPSServer::Initialise(StructuredDataI &data) {
bool ok = true;
port = TCPS_DEFAULT_PORT;
(void)data.Read("Port", port);
if (port > 65535u) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"TCPSServer: Port %u out of range.", port);
ok = false;
}
maxPayloadSize = TCPS_DEFAULT_MAX_PAYLOAD;
(void)data.Read("MaxPayloadSize", maxPayloadSize);
if ((maxPayloadSize == 0u) || (maxPayloadSize > TCPS_MAX_FRAME_PAYLOAD)) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"TCPSServer: MaxPayloadSize %u must be in 1..%u.",
maxPayloadSize, TCPS_MAX_FRAME_PAYLOAD);
ok = false;
}
maxClients = TCPS_DEFAULT_MAX_CLIENTS;
(void)data.Read("MaxClients", maxClients);
if ((maxClients == 0u) || (maxClients > TCPS_MAX_CLIENTS_LIMIT)) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"TCPSServer: MaxClients %u must be in 1..%u.",
maxClients, TCPS_MAX_CLIENTS_LIMIT);
ok = false;
}
queueDepth = TCPS_DEFAULT_QUEUE_DEPTH;
(void)data.Read("ClientQueueDepth", queueDepth);
if ((queueDepth == 0u) || (queueDepth > TCPS_QUEUE_DEPTH_LIMIT)) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"TCPSServer: ClientQueueDepth %u must be in 1..%u.",
queueDepth, TCPS_QUEUE_DEPTH_LIMIT);
ok = false;
}
stallTimeoutS = TCPS_DEFAULT_STALL_TIMEOUT_S;
(void)data.Read("ClientStallTimeout", stallTimeoutS);
stallTicks = static_cast<uint64>(stallTimeoutS) * HighResolutionTimer::Frequency();
sendBufferSize = 0u;
(void)data.Read("SendBufferSize", sendBufferSize);
return ok;
}
// ---------------------------------------------------------------------------
// Start / Stop
// ---------------------------------------------------------------------------
bool TCPSServer::Start() {
if (started) {
return true;
}
int fd = socket(AF_INET, SOCK_STREAM, 0);
bool ok = (fd >= 0);
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
"TCPSServer: socket() failed: %s.", strerror(errno));
}
if (ok) {
int one = 1;
(void)setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
struct sockaddr_in addr;
memset(&addr, 0, sizeof(addr));
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = htonl(INADDR_ANY);
addr.sin_port = htons(static_cast<uint16>(port));
if (bind(fd, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr)) != 0) {
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
"TCPSServer: bind(%u) failed: %s.", port, strerror(errno));
ok = false;
}
}
if (ok && (listen(fd, 16) != 0)) {
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
"TCPSServer: listen() failed: %s.", strerror(errno));
ok = false;
}
if (ok) {
int flags = fcntl(fd, F_GETFL, 0);
ok = (flags >= 0) && (fcntl(fd, F_SETFL, flags | O_NONBLOCK) == 0);
}
if (ok) {
struct sockaddr_in addr;
socklen_t len = static_cast<socklen_t>(sizeof(addr));
memset(&addr, 0, sizeof(addr));
if (getsockname(fd, reinterpret_cast<struct sockaddr *>(&addr), &len) == 0) {
boundPort = ntohs(addr.sin_port);
} else {
boundPort = static_cast<uint16>(port);
}
}
if (ok) {
clients = new Client[maxClients];
listenFd = fd;
numClients = 0u;
started = true;
} else if (fd >= 0) {
(void)close(fd);
}
return ok;
}
bool TCPSServer::Stop() {
if (clients != NULL_PTR(Client *)) {
delete[] clients; // closes every client socket
clients = NULL_PTR(Client *);
}
numClients = 0u;
if (listenFd >= 0) {
(void)close(listenFd);
listenFd = -1;
}
if (cachedConfig != NULL_PTR(uint8 *)) {
delete[] cachedConfig;
cachedConfig = NULL_PTR(uint8 *);
}
cachedConfigSize = 0u;
if (frameBuf != NULL_PTR(uint8 *)) {
delete[] frameBuf;
frameBuf = NULL_PTR(uint8 *);
}
frameCapacity = 0u;
frameSize = 0u;
started = false;
return true;
}
// ---------------------------------------------------------------------------
// Frame building
// ---------------------------------------------------------------------------
bool TCPSServer::BuildFrame(uint8 type, uint32 counter, const uint8 *payload,
uint32 payloadSize) {
bool ok = (payloadSize <= TCPS_MAX_FRAME_PAYLOAD);
if (ok) {
uint32 need = UDPS_HEADER_SIZE + payloadSize;
if (frameCapacity < need) {
if (frameBuf != NULL_PTR(uint8 *)) {
delete[] frameBuf;
}
frameBuf = new uint8[need];
frameCapacity = need;
}
UDPSBuildHeader(frameBuf, type, counter, 0u, 1u, payloadSize);
if (payloadSize > 0u) {
memcpy(&frameBuf[UDPS_HEADER_SIZE], payload, payloadSize);
}
frameSize = need;
}
return ok;
}
// ---------------------------------------------------------------------------
// Queueing
// ---------------------------------------------------------------------------
bool TCPSServer::Enqueue(Client &c, bool isConfig, const uint8 *frame,
uint32 frameBytes) {
const uint32 firstFree = (c.sent > 0u) ? 1u : 0u; // slots below this are on the wire
if (isConfig) {
// Frames behind the one on the wire were encoded for the old layout.
while (c.count > firstFree) {
c.count--;
if (!c.slots[c.count].isConfig) {
totalDrops++;
}
}
} else {
uint32 dataCount = 0u;
for (uint32 i = 0u; i < c.count; i++) {
if (!c.slots[i].isConfig) {
dataCount++;
}
}
if (dataCount >= queueDepth) {
uint32 victim = c.count;
for (uint32 i = firstFree; (i < c.count) && (victim == c.count); i++) {
if (!c.slots[i].isConfig) {
victim = i;
}
}
if (victim == c.count) {
// Only the frame on the wire is queued: nothing may be discarded.
totalDrops++;
return false;
}
// Remove `victim`, keeping its buffer as the (new) free slot.
Slot tmp = c.slots[victim];
for (uint32 i = victim + 1u; i < c.count; i++) {
c.slots[i - 1u] = c.slots[i];
}
c.slots[c.count - 1u] = tmp;
// The array copies above moved raw pointers; make sure `tmp` going
// out of scope does not free the buffer.
tmp.buf = NULL_PTR(uint8 *);
c.count--;
totalDrops++;
}
}
if (c.count >= c.cap) {
// Cannot happen with the capacity argument above; refuse defensively.
totalDrops++;
return false;
}
Slot &s = c.slots[c.count];
if (s.cap < frameBytes) {
if (s.buf != NULL_PTR(uint8 *)) {
delete[] s.buf;
}
s.buf = new uint8[frameBytes];
s.cap = frameBytes;
}
memcpy(s.buf, frame, frameBytes);
s.len = frameBytes;
s.isConfig = isConfig;
if (c.count == 0u) {
c.sent = 0u;
c.lastProgress = HighResolutionTimer::Counter();
}
c.count++;
return true;
}
bool TCPSServer::Flush(Client &c) {
bool alive = true;
while (alive && (c.count > 0u)) {
Slot &s = c.slots[0];
ssize_t n = send(c.fd, &s.buf[c.sent], s.len - c.sent,
MSG_NOSIGNAL | MSG_DONTWAIT);
if (n > 0) {
c.sent += static_cast<uint32>(n);
c.lastProgress = HighResolutionTimer::Counter();
if (c.sent >= s.len) {
// Rotate the finished slot to the free area, keep its buffer.
Slot tmp = c.slots[0];
for (uint32 i = 1u; i < c.count; i++) {
c.slots[i - 1u] = c.slots[i];
}
c.slots[c.count - 1u] = tmp;
tmp.buf = NULL_PTR(uint8 *);
c.count--;
c.sent = 0u;
}
} else if ((n < 0) && ((errno == EAGAIN) || (errno == EWOULDBLOCK))) {
break;
} else if ((n < 0) && (errno == EINTR)) {
continue;
} else {
alive = false;
}
}
return alive;
}
/** Detect a peer that closed (or sent garbage: clients never talk to us). */
bool TCPSServer::PeerAlive(Client &c) {
uint8 scratch[64];
bool alive = true;
for (;;) {
ssize_t n = recv(c.fd, scratch, sizeof(scratch), MSG_DONTWAIT);
if (n > 0) {
continue; // discard
}
if (n == 0) {
alive = false;
} else if ((errno == EAGAIN) || (errno == EWOULDBLOCK)) {
alive = true;
} else if (errno == EINTR) {
continue;
} else {
alive = false;
}
break;
}
return alive;
}
void TCPSServer::Evict(Client &c) {
if (c.fd >= 0) {
(void)close(c.fd);
c.fd = -1;
}
// Slots and their buffers are freed here: the slot may stay idle a long time.
if (c.slots != NULL_PTR(Slot *)) {
delete[] c.slots;
c.slots = NULL_PTR(Slot *);
}
c.cap = 0u;
c.count = 0u;
c.sent = 0u;
if (numClients > 0u) {
numClients--;
}
}
// ---------------------------------------------------------------------------
// Public operations
// ---------------------------------------------------------------------------
void TCPSServer::AcceptPending() {
for (;;) {
int fd = accept(listenFd, NULL_PTR(struct sockaddr *), NULL_PTR(socklen_t *));
if (fd < 0) {
break; // EAGAIN, or a transient error: retry next call
}
Client *slot = NULL_PTR(Client *);
for (uint32 i = 0u; (i < maxClients) && (slot == NULL_PTR(Client *)); i++) {
if (clients[i].fd < 0) {
slot = &clients[i];
}
}
if (slot == NULL_PTR(Client *)) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"TCPSServer: MaxClients (%u) reached; connection refused.",
maxClients);
(void)close(fd);
continue;
}
int flags = fcntl(fd, F_GETFL, 0);
if ((flags < 0) || (fcntl(fd, F_SETFL, flags | O_NONBLOCK) != 0)) {
(void)close(fd);
continue;
}
int one = 1;
(void)setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
if (sendBufferSize > 0u) {
int sz = static_cast<int>(sendBufferSize);
(void)setsockopt(fd, SOL_SOCKET, SO_SNDBUF, &sz, sizeof(sz));
}
slot->fd = fd;
slot->cap = queueDepth + 2u;
slot->slots = new Slot[slot->cap];
slot->count = 0u;
slot->sent = 0u;
slot->lastProgress = HighResolutionTimer::Counter();
numClients++;
// CONFIG first, so a client always decodes DATA against a CONFIG.
if (cachedConfig != NULL_PTR(uint8 *)) {
if (BuildFrame(UDPS_TYPE_CONFIG, configCounter, cachedConfig, cachedConfigSize)) {
(void)Enqueue(*slot, true, frameBuf, frameSize);
if (!Flush(*slot)) {
Evict(*slot);
}
}
}
}
}
void TCPSServer::ServiceClients() {
if (!started) {
return;
}
AcceptPending();
for (uint32 i = 0u; i < maxClients; i++) {
Client &c = clients[i];
if (c.fd < 0) {
continue;
}
if (!PeerAlive(c) || !Flush(c)) {
Evict(c);
continue;
}
// Read the clock after Flush(): it stamps lastProgress itself.
const uint64 now = HighResolutionTimer::Counter();
if ((stallTicks > 0u) && (c.count > 0u) && (now > c.lastProgress) &&
((now - c.lastProgress) > stallTicks)) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"TCPSServer: client stalled for more than %u s; disconnecting.",
stallTimeoutS);
stallEvictions++;
Evict(c);
}
}
ReportDrops();
}
bool TCPSServer::CacheConfig(const uint8 *payload, uint32 payloadSize) {
if (cachedConfig != NULL_PTR(uint8 *)) {
delete[] cachedConfig;
cachedConfig = NULL_PTR(uint8 *);
cachedConfigSize = 0u;
}
if (payloadSize > 0u) {
cachedConfig = new uint8[payloadSize];
memcpy(cachedConfig, payload, payloadSize);
cachedConfigSize = payloadSize;
}
return true;
}
bool TCPSServer::SendConfig(const uint8 *payload, uint32 payloadSize) {
if (!started) {
return false;
}
if (payloadSize > TCPS_MAX_FRAME_PAYLOAD) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"TCPSServer: CONFIG of %u bytes exceeds the frame limit.",
payloadSize);
return false;
}
(void)CacheConfig(payload, payloadSize);
configCounter++;
if ((numClients == 0u) || !BuildFrame(UDPS_TYPE_CONFIG, configCounter, payload, payloadSize)) {
return true;
}
for (uint32 i = 0u; i < maxClients; i++) {
Client &c = clients[i];
if (c.fd < 0) {
continue;
}
(void)Enqueue(c, true, frameBuf, frameSize);
if (!Flush(c)) {
Evict(c);
}
}
return true;
}
bool TCPSServer::SendData(uint32 counter, const uint8 *payload, uint32 payloadSize) {
if (!started) {
return false;
}
if (numClients == 0u) {
return true;
}
if (!BuildFrame(UDPS_TYPE_DATA, counter, payload, payloadSize)) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"TCPSServer: DATA of %u bytes exceeds the frame limit.",
payloadSize);
return false;
}
for (uint32 i = 0u; i < maxClients; i++) {
Client &c = clients[i];
if (c.fd < 0) {
continue;
}
(void)Enqueue(c, false, frameBuf, frameSize);
if (!Flush(c)) {
Evict(c);
}
}
return true;
}
/** Report queue drops at most once per second. */
void TCPSServer::ReportDrops() {
if (totalDrops != loggedDrops) {
const uint64 now = HighResolutionTimer::Counter();
if ((now - lastDropLogTicks) >= HighResolutionTimer::Frequency()) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"TCPSServer: %u DATA frame(s) dropped for slow client(s) (total %u).",
static_cast<uint32>(totalDrops - loggedDrops),
static_cast<uint32>(totalDrops));
loggedDrops = totalDrops;
lastDropLogTicks = now;
}
}
}
bool TCPSServer::HasClients() const {
return numClients > 0u;
}
bool TCPSServer::IsMulticast() const {
return false;
}
uint16 TCPSServer::GetPort() const {
return started ? boundPort : static_cast<uint16>(port);
}
uint32 TCPSServer::GetClientCount() const {
return numClients;
}
uint64 TCPSServer::GetClientDrops() const {
return totalDrops;
}
uint32 TCPSServer::GetStallEvictions() const {
return stallEvictions;
}
uint32 TCPSServer::GetMaxPayloadSize() const {
return maxPayloadSize;
}
} // namespace MARTe
Reference in new issue View Git Blame Copy Permalink