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
@@ -1,7 +1,7 @@
|
||||
#############################################################
|
||||
# MARTe2 Integrated Components — UDPStream
|
||||
#############################################################
|
||||
OBJSX=UDPSServer.x UDPSClient.x
|
||||
OBJSX=UDPSServer.x UDPSClient.x TCPSServer.x TCPSClient.x
|
||||
|
||||
PACKAGE=Components/Interfaces
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -0,0 +1,163 @@
|
||||
#ifndef TCPS_CLIENT_H_
|
||||
#define TCPS_CLIENT_H_
|
||||
|
||||
/**
|
||||
* @file TCPSClient.h
|
||||
* @brief Auto-reconnecting TCP receiver for the UDPS wire format.
|
||||
*
|
||||
* TCPSClient is the consumer-side counterpart of TCPSServer. It connects to a
|
||||
* TCPSServer, reads UDPS frames (17-byte header + payload) from the byte stream
|
||||
* and delivers CONFIG / DATA payloads through UDPSClientListener, so any
|
||||
* listener written for UDPSClient works unchanged.
|
||||
*
|
||||
* It runs its own thread (SingleThreadService); every wait inside Execute() is
|
||||
* bounded by 50 ms so Stop() is prompt. A partial frame survives across
|
||||
* iterations. Bad magic, an oversize length, or a fragmented frame means the
|
||||
* stream cannot be trusted any more: the connection is dropped and re-opened.
|
||||
*
|
||||
* There is no CONNECT / ACK / keepalive traffic: the TCP connection is the
|
||||
* session and the server sends CONFIG on accept.
|
||||
*/
|
||||
|
||||
#include "EmbeddedServiceMethodBinderI.h"
|
||||
#include "FastPollingMutexSem.h"
|
||||
#include "ExecutionInfo.h"
|
||||
#include "SingleThreadService.h"
|
||||
#include "StreamString.h"
|
||||
#include "StructuredDataI.h"
|
||||
#include "UDPSClient.h"
|
||||
#include "UDPSProtocol.h"
|
||||
#include "UDPSTransportI.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
class TCPSClient;
|
||||
|
||||
/**
|
||||
* One stream of a (possibly multiplexed) TCPSClient connection, seen as an
|
||||
* independent UDPSClientI. Obtained from TCPSClient::GetStream(). Start()/Stop()
|
||||
* are reference counted so the shared connection runs while any stream wants it.
|
||||
*/
|
||||
class TCPSClientStream : public UDPSClientI {
|
||||
public:
|
||||
TCPSClientStream(TCPSClient &owner, uint32 index);
|
||||
virtual ~TCPSClientStream();
|
||||
/** The connection is configured through its TCPSClient; always succeeds. */
|
||||
virtual bool Initialise(StructuredDataI &data);
|
||||
virtual void SetListener(UDPSClientListener *newListener);
|
||||
virtual bool Start();
|
||||
virtual bool Stop();
|
||||
virtual uint32 GetLastDataGap() const;
|
||||
private:
|
||||
TCPSClient &owner;
|
||||
uint32 index;
|
||||
bool started;
|
||||
};
|
||||
|
||||
class TCPSClient : public UDPSClientI, public EmbeddedServiceMethodBinderI {
|
||||
public:
|
||||
|
||||
/** Most streams one connection can multiplex. */
|
||||
static const uint32 TCPS_CLIENT_MAX_STREAMS = UDPS_TCP_MAX_STREAMS;
|
||||
|
||||
/** Default delay between reconnect attempts (seconds). */
|
||||
static const uint32 TCPS_CLIENT_DEFAULT_RECONNECT_DELAY_S = 2u;
|
||||
|
||||
TCPSClient();
|
||||
virtual ~TCPSClient();
|
||||
|
||||
/**
|
||||
* Keys:
|
||||
* - ServerAddr (char*) IPv4 address or host name. Default 127.0.0.1.
|
||||
* - Port (uint32) Server TCP port. Required.
|
||||
* - StreamName (char*) Optional. When set, a SUBSCRIBE frame carrying
|
||||
* this name is sent right after every connect, to
|
||||
* select one stream of a TCPStreamService. A plain
|
||||
* TCPSServer ignores (discards) it. A comma
|
||||
* separated list ("a,b,c") multiplexes several
|
||||
* streams over this one connection; stream i of
|
||||
* the list is reached through GetStream(i).
|
||||
* - SilenceTimeout (float32) Seconds without any received byte before the
|
||||
* connection is considered dead. Default 0
|
||||
* (disabled: TCP reports a dead peer itself).
|
||||
* - ReconnectDelay (float32) Seconds between connection attempts. Default 2.
|
||||
* - CPUMask (uint32) Receive thread affinity. Default 0xFFFFFFFF.
|
||||
* - StackSize (uint32) Receive thread stack. Default 65536.
|
||||
*/
|
||||
virtual bool Initialise(StructuredDataI &data);
|
||||
virtual void SetListener(UDPSClientListener *newListener);
|
||||
/** Listener of stream @p index (0 = SetListener()). Call before Start(). */
|
||||
void SetStreamListener(uint32 index, UDPSClientListener *newListener);
|
||||
/** @return number of streams subscribed to (1 unless StreamName is a list). */
|
||||
uint32 GetNumStreams() const;
|
||||
/**
|
||||
* @return a UDPSClientI for stream @p index, owned by this client (NULL if out of
|
||||
* range). Valid after Initialise().
|
||||
*/
|
||||
UDPSClientI *GetStream(uint32 index);
|
||||
/** Counter gap of the DATA frame of stream @p index being delivered. */
|
||||
uint32 GetStreamDataGap(uint32 index) const;
|
||||
/** Reference-counted Start()/Stop() used by the TCPSClientStream facades. */
|
||||
bool AcquireStart();
|
||||
bool ReleaseStop();
|
||||
virtual bool Start();
|
||||
virtual bool Stop();
|
||||
|
||||
/** Thread entry point; do not call directly. */
|
||||
virtual ErrorManagement::ErrorType Execute(ExecutionInfo &info);
|
||||
|
||||
/** Packets the server dropped for this client just before the DATA frame being delivered. */
|
||||
virtual uint32 GetLastDataGap() const;
|
||||
|
||||
/** @return true while a connection is established. */
|
||||
bool IsConnected() const;
|
||||
/** @return connections established since Start(). */
|
||||
uint32 GetConnectionCount() const;
|
||||
/** @return connections dropped because of a protocol violation. */
|
||||
uint32 GetProtocolErrors() const;
|
||||
|
||||
private:
|
||||
bool Connect();
|
||||
void Disconnect();
|
||||
/** One bounded receive step. @return false when the connection is lost. */
|
||||
bool ReceiveStep();
|
||||
/** Deliver the complete frame sitting in rxBuf. @return false on a protocol error. */
|
||||
bool Dispatch();
|
||||
/** Validate StreamName as one name or a comma separated list; sets numStreams. */
|
||||
bool ParseStreamList();
|
||||
|
||||
StreamString serverAddr;
|
||||
StreamString streamName;
|
||||
uint32 serverPort;
|
||||
uint64 silenceTimeoutTicks;
|
||||
uint64 reconnectDelayTicks;
|
||||
uint32 cpuMask;
|
||||
uint32 stackSize;
|
||||
|
||||
uint32 numStreams;
|
||||
UDPSClientListener *listeners[UDPS_TCP_MAX_STREAMS];
|
||||
TCPSClientStream *facades[UDPS_TCP_MAX_STREAMS];
|
||||
FastPollingMutexSem refMutex;
|
||||
uint32 startRefs;
|
||||
SingleThreadService threadService;
|
||||
|
||||
int fd;
|
||||
volatile bool connected;
|
||||
volatile bool stopping;
|
||||
uint64 lastDataTicks;
|
||||
uint64 disconnectTick;
|
||||
volatile uint32 connectionCount;
|
||||
volatile uint32 protocolErrors;
|
||||
|
||||
uint8 *rxBuf; ///< header + payload of the frame being received
|
||||
uint32 rxHave; ///< bytes of the current frame received so far
|
||||
uint32 rxWant; ///< total bytes the current frame needs (header first)
|
||||
bool rxHeaderOk; ///< header validated; rxWant covers the payload
|
||||
uint32 lastDataCounter[UDPS_TCP_MAX_STREAMS]; ///< counter of the last delivered DATA frame
|
||||
bool lastDataCounterValid[UDPS_TCP_MAX_STREAMS]; ///< false until the first DATA frame of a connection
|
||||
uint32 lastDataGap[UDPS_TCP_MAX_STREAMS]; ///< frames missing before the one being delivered
|
||||
};
|
||||
|
||||
} // namespace MARTe
|
||||
|
||||
#endif // TCPS_CLIENT_H_
|
||||
@@ -0,0 +1,606 @@
|
||||
/**
|
||||
* @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
|
||||
@@ -0,0 +1,160 @@
|
||||
#ifndef TCPS_SERVER_H_
|
||||
#define TCPS_SERVER_H_
|
||||
|
||||
/**
|
||||
* @file TCPSServer.h
|
||||
* @brief TCP transport for the UDPS wire format (producer side).
|
||||
*
|
||||
* TCPSServer carries the normal UDPS framing (17-byte header + CONFIG / DATA
|
||||
* payload, totalFragments always 1) over TCP byte streams. It implements
|
||||
* UDPSServerI so UDPStreamer can publish through it unchanged.
|
||||
*
|
||||
* Threading model: NOT thread-safe, exactly like UDPSServer. Every call must
|
||||
* come from the owner's sender thread. There is no thread per client: sockets
|
||||
* are non-blocking and everything is driven from ServiceClients() / SendData().
|
||||
*
|
||||
* Slow consumers: every client owns a bounded queue of ClientQueueDepth DATA
|
||||
* frames. When it is full the oldest queued DATA frame that is not already
|
||||
* (partially) on the wire is discarded and counted; other clients and the
|
||||
* producer are never affected. A client that makes no progress for
|
||||
* ClientStallTimeout seconds with data queued is disconnected.
|
||||
*
|
||||
* Ordering: a client always sees CONFIG before any DATA. A CONFIG is never
|
||||
* dropped. When a new CONFIG is published, the frames queued behind the one
|
||||
* currently on the wire are stale (encoded for the old layout) and are
|
||||
* discarded, so DATA is never decoded against the wrong CONFIG.
|
||||
*/
|
||||
|
||||
#include "StructuredDataI.h"
|
||||
#include "UDPSProtocol.h"
|
||||
#include "UDPSTransportI.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
class TCPSServer : public UDPSServerI {
|
||||
public:
|
||||
|
||||
/** Default TCP listen port. */
|
||||
static const uint32 TCPS_DEFAULT_PORT = 44500u;
|
||||
/** Default maximum payload bytes per frame. */
|
||||
static const uint32 TCPS_DEFAULT_MAX_PAYLOAD = 60000u;
|
||||
/** Hard ceiling on a frame payload (matches UDPSClient's packet limit). */
|
||||
static const uint32 TCPS_MAX_FRAME_PAYLOAD = 1048576u;
|
||||
/** Default simultaneous clients. */
|
||||
static const uint32 TCPS_DEFAULT_MAX_CLIENTS = 16u;
|
||||
/** Upper bound accepted for MaxClients. */
|
||||
static const uint32 TCPS_MAX_CLIENTS_LIMIT = 1024u;
|
||||
/** Default DATA frames queued per client. */
|
||||
static const uint32 TCPS_DEFAULT_QUEUE_DEPTH = 8u;
|
||||
/** Upper bound accepted for ClientQueueDepth. */
|
||||
static const uint32 TCPS_QUEUE_DEPTH_LIMIT = 4096u;
|
||||
/** Default stall timeout (seconds); 0 disables. */
|
||||
static const uint32 TCPS_DEFAULT_STALL_TIMEOUT_S = 5u;
|
||||
|
||||
TCPSServer();
|
||||
virtual ~TCPSServer();
|
||||
|
||||
/**
|
||||
* @brief Read configuration.
|
||||
*
|
||||
* Keys (all optional):
|
||||
* - Port (uint32) TCP listen port, default 44500. 0 picks an
|
||||
* ephemeral port (see GetPort() after Start()).
|
||||
* - MaxPayloadSize (uint32) 1..1 MiB, default 60000. Sizes Accumulate
|
||||
* batches in the owner; frames larger than 1 MiB are
|
||||
* refused by SendData().
|
||||
* - MaxClients (uint32) 1..1024, default 16. Extra connections
|
||||
* are accepted and closed immediately.
|
||||
* - ClientQueueDepth (uint32) 1..4096, default 8.
|
||||
* - ClientStallTimeout (uint32) Seconds, default 5; 0 disables.
|
||||
* - SendBufferSize (uint32) SO_SNDBUF for client sockets; 0 (default)
|
||||
* keeps the OS default.
|
||||
*/
|
||||
virtual bool Initialise(StructuredDataI &data);
|
||||
|
||||
/** Open the listening socket (idempotent). */
|
||||
virtual bool Start();
|
||||
|
||||
/** Close every socket and free all buffers (idempotent). */
|
||||
virtual bool Stop();
|
||||
|
||||
/**
|
||||
* Accept new clients, drop closed ones, flush queues, evict stalled
|
||||
* clients. Never blocks.
|
||||
*/
|
||||
virtual void ServiceClients();
|
||||
|
||||
/**
|
||||
* Cache the CONFIG and queue it on every connected client. New clients
|
||||
* receive the cached CONFIG on accept.
|
||||
*/
|
||||
virtual bool SendConfig(const uint8 *payload, uint32 payloadSize);
|
||||
|
||||
/**
|
||||
* Queue a DATA frame on every connected client and try to flush.
|
||||
* @return false only if the frame is larger than TCPS_MAX_FRAME_PAYLOAD.
|
||||
*/
|
||||
virtual bool SendData(uint32 counter, const uint8 *payload, uint32 payloadSize);
|
||||
|
||||
virtual bool HasClients() const;
|
||||
virtual bool IsMulticast() const;
|
||||
|
||||
/** @return the bound port once started, otherwise the configured port. */
|
||||
virtual uint16 GetPort() const;
|
||||
|
||||
/** @return number of connected clients. */
|
||||
uint32 GetClientCount() const;
|
||||
|
||||
/** @return DATA frames discarded (all clients, all time) because a client queue was full. */
|
||||
uint64 GetClientDrops() const;
|
||||
|
||||
/** @return clients disconnected by the stall timeout (all time). */
|
||||
uint32 GetStallEvictions() const;
|
||||
|
||||
uint32 GetMaxPayloadSize() const;
|
||||
|
||||
private:
|
||||
struct Slot;
|
||||
struct Client;
|
||||
|
||||
bool CacheConfig(const uint8 *payload, uint32 payloadSize);
|
||||
void AcceptPending();
|
||||
bool Enqueue(Client &c, bool isConfig, const uint8 *frame, uint32 frameSize);
|
||||
bool Flush(Client &c);
|
||||
bool PeerAlive(Client &c);
|
||||
void Evict(Client &c);
|
||||
bool BuildFrame(uint8 type, uint32 counter, const uint8 *payload, uint32 payloadSize);
|
||||
void ReportDrops();
|
||||
|
||||
uint32 port;
|
||||
uint16 boundPort;
|
||||
uint32 maxPayloadSize;
|
||||
uint32 maxClients;
|
||||
uint32 queueDepth;
|
||||
uint32 stallTimeoutS;
|
||||
uint32 sendBufferSize;
|
||||
|
||||
int listenFd;
|
||||
bool started;
|
||||
|
||||
Client *clients;
|
||||
uint32 numClients;
|
||||
|
||||
uint8 *cachedConfig;
|
||||
uint32 cachedConfigSize;
|
||||
uint32 configCounter;
|
||||
|
||||
uint8 *frameBuf; ///< scratch: header + payload built once per call
|
||||
uint32 frameCapacity;
|
||||
uint32 frameSize;
|
||||
|
||||
uint64 stallTicks;
|
||||
uint64 totalDrops;
|
||||
uint64 loggedDrops;
|
||||
uint64 lastDropLogTicks;
|
||||
uint32 stallEvictions;
|
||||
};
|
||||
|
||||
} // namespace MARTe
|
||||
|
||||
#endif // TCPS_SERVER_H_
|
||||
@@ -27,6 +27,7 @@
|
||||
#include "StreamString.h"
|
||||
#include "StructuredDataI.h"
|
||||
#include "UDPSProtocol.h"
|
||||
#include "UDPSTransportI.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
@@ -72,7 +73,7 @@ public:
|
||||
/**
|
||||
* @brief UDPS receiver client with auto-reconnect and fragment reassembly.
|
||||
*/
|
||||
class UDPSClient : public EmbeddedServiceMethodBinderI {
|
||||
class UDPSClient : public UDPSClientI, public EmbeddedServiceMethodBinderI {
|
||||
public:
|
||||
|
||||
/** Maximum number of concurrent in-flight reassembly slots. */
|
||||
|
||||
@@ -23,6 +23,7 @@
|
||||
#include "StreamString.h"
|
||||
#include "StructuredDataI.h"
|
||||
#include "UDPSProtocol.h"
|
||||
#include "UDPSTransportI.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
@@ -43,7 +44,7 @@ namespace MARTe {
|
||||
* server.Stop(); // in StopCurrentStateExecution()
|
||||
* @endcode
|
||||
*/
|
||||
class UDPSServer {
|
||||
class UDPSServer : public UDPSServerI {
|
||||
public:
|
||||
|
||||
/** Maximum number of simultaneous unicast clients. */
|
||||
|
||||
@@ -0,0 +1,71 @@
|
||||
#ifndef UDPS_TRANSPORT_I_H_
|
||||
#define UDPS_TRANSPORT_I_H_
|
||||
|
||||
/**
|
||||
* @file UDPSTransportI.h
|
||||
* @brief Abstract transport seams used by the UDPStreamer / UDPStreamerClient
|
||||
* DataSources.
|
||||
*
|
||||
* The DataSources only ever touch their transport through these few calls, so
|
||||
* a different transport (TCP) can be plugged in without duplicating the
|
||||
* serialisation, quantisation, publishing and decode logic.
|
||||
*/
|
||||
|
||||
#include "CompilerTypes.h"
|
||||
#include "StructuredDataI.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
class UDPSClientListener;
|
||||
|
||||
/**
|
||||
* @brief Producer-side transport. NOT thread-safe: every call must come from
|
||||
* the owner's sender thread.
|
||||
*/
|
||||
class UDPSServerI {
|
||||
public:
|
||||
virtual ~UDPSServerI() {}
|
||||
|
||||
/** Read the transport configuration. */
|
||||
virtual bool Initialise(StructuredDataI &data) = 0;
|
||||
/** Open sockets and start accepting clients (idempotent). */
|
||||
virtual bool Start() = 0;
|
||||
/** Close everything (idempotent). */
|
||||
virtual bool Stop() = 0;
|
||||
/** Non-blocking housekeeping: accept/evict clients, drain queues. */
|
||||
virtual void ServiceClients() = 0;
|
||||
/** Publish (and cache) a CONFIG payload. */
|
||||
virtual bool SendConfig(const uint8 *payload, uint32 payloadSize) = 0;
|
||||
/** Publish a DATA payload. */
|
||||
virtual bool SendData(uint32 counter, const uint8 *payload, uint32 payloadSize) = 0;
|
||||
/** @return true when at least one client is attached. */
|
||||
virtual bool HasClients() const = 0;
|
||||
/** @return true when the transport uses multicast. */
|
||||
virtual bool IsMulticast() const = 0;
|
||||
/** @return the (bound) port. */
|
||||
virtual uint16 GetPort() const = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Consumer-side transport. Delivers through UDPSClientListener.
|
||||
*/
|
||||
class UDPSClientI {
|
||||
public:
|
||||
virtual ~UDPSClientI() {}
|
||||
|
||||
virtual bool Initialise(StructuredDataI &data) = 0;
|
||||
/** Must be called before Start(). */
|
||||
virtual void SetListener(UDPSClientListener *listener) = 0;
|
||||
virtual bool Start() = 0;
|
||||
virtual bool Stop() = 0;
|
||||
/**
|
||||
* @return Packets missing between the previous DATA packet and the one
|
||||
* being delivered (valid inside OnUDPSData). 0 when the transport does
|
||||
* not track it.
|
||||
*/
|
||||
virtual uint32 GetLastDataGap() const { return 0u; }
|
||||
};
|
||||
|
||||
} // namespace MARTe
|
||||
|
||||
#endif // UDPS_TRANSPORT_I_H_
|
||||
Reference in new issue
Block a user