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>
This commit is contained in:
Martino FerrariandClaude Opus 4.6 committed
1 parent
5562877c99
commit
50d85363a3
79 files changed
+12348
-138
No files matched your search
@@ -0,0 +1,517 @@
|
||||
/**
|
||||
* @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
Block a user