martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implemented new C++ logic

Browse Source
This commit is contained in:
Martino Ferrari
2026-06-12 15:25:13 +02:00
parent 617b5bd712
commit f25bd7f08e
220 changed files with 39185 additions and 850 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Source/Components/DataSources/UDPStreamer/Makefile.inc
+3
View File
@@ -32,6 +32,7 @@ include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
INCLUDES += -I.
INCLUDES += -I$(ROOT_DIR)/Common/UDP
INCLUDES += -I$(ROOT_DIR)/Source/Components/Interfaces/UDPStream
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L0Types
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L1Portability
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L2Objects
@@ -46,6 +47,8 @@ INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L5GAMs
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L1Portability
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L3Streams
LIBRARIES += -L$(BUILD_DIR)/../../Interfaces/UDPStream -lUDPStream
all: $(OBJS) \
$(BUILD_DIR)/UDPStreamer$(LIBEXT) \
$(BUILD_DIR)/UDPStreamer$(DLLEXT)
Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp
+45 -445
View File
@@ -33,6 +33,7 @@
/* Project header includes */
/*---------------------------------------------------------------------------*/
#include "AdvancedErrorManagement.h"
#include "ConfigurationDatabase.h"
#include "EmbeddedThreadI.h"
#include "GlobalObjectsDatabase.h"
#include "HighResolutionTimer.h"
@@ -61,7 +62,7 @@ static const int32 UDPS_TCP_MAX_CONNECTIONS = 4;
static const uint32 UDPS_DEFAULT_MAX_PAYLOAD = 1400u;
/** Minimum MaxPayloadSize: header + at least 1 byte of payload. */
static const uint32 UDPS_MIN_PAYLOAD = static_cast<uint32>(sizeof(UDPSPacketHeader)) + 1u;
static const uint32 UDPS_MIN_PAYLOAD = UDPS_HEADER_SIZE + 1u;
/** Server socket receive timeout in milliseconds.
* Set to 0 for a pure non-blocking poll so the send loop can keep pace with
@@ -72,30 +73,6 @@ static const uint32 UDPS_RECV_TIMEOUT_MS = 0u;
/** EventSem wait timeout in milliseconds for the data loop. */
static const uint32 UDPS_DATA_WAIT_MS = 10u;
/** Sentinel: no time-signal reference; use the packet-level timestamp. */
static const uint32 UDPS_NO_TIME_SIGNAL = 0xFFFFFFFFu;
/** Max signal name length in CONFIG packet (including null terminator). */
static const uint32 UDPS_MAX_SIGNAL_NAME = 64u;
/** Max unit string length in CONFIG packet (including null terminator). */
static const uint32 UDPS_MAX_UNIT_LEN = 32u;
/** Size in bytes of one signal descriptor in the CONFIG payload. */
static const uint32 UDPS_SIGNAL_DESC_SIZE =
UDPS_MAX_SIGNAL_NAME /* name */
+ 1u /* typeCode */
+ 1u /* quantType */
+ 1u /* numDimensions*/
+ 4u /* numRows */
+ 4u /* numCols */
+ 8u /* rangeMin */
+ 8u /* rangeMax */
+ 1u /* timeMode */
+ 8u /* samplingRate */
+ 4u /* timeSignalIdx*/
+ UDPS_MAX_UNIT_LEN; /* unit */
/** Bytes prepended to each DATA payload for the HRT packet timestamp. */
static const uint32 UDPS_TIMESTAMP_BYTES = 8u;
@@ -114,9 +91,6 @@ UDPStreamer::UDPStreamer() :
publishMode = UDPStreamerPublishStrict;
minRefreshRate = 0.0;
flushPeriodTicks = 0u;
dataPort = UDPS_DEFAULT_PORT + UDPS_DEFAULT_DATA_PORT_OFFSET;
useMulticast = false;
tcpClient = NULL_PTR(BasicTCPSocket *);
numSigs = 0u;
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
readyBuffer = NULL_PTR(uint8 *);
@@ -125,7 +99,6 @@ UDPStreamer::UDPStreamer() :
totalSrcBytes = 0u;
totalWireBytes = 0u;
syncTimestamp = 0u;
clientConnected = false;
packetCounter = 0u;
maxBatchCount = 0u;
singleCycleWireBytes = 0u;
@@ -162,12 +135,7 @@ UDPStreamer::~UDPStreamer() {
}
}
if (serverSocket.IsValid()) {
(void) serverSocket.Close();
}
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
(void) server.Stop();
HeapI *heapAccum = GlobalObjectsDatabase::Instance()->GetStandardHeap();
if (accumBuffer != NULL_PTR(uint8 *)) {
@@ -186,19 +154,6 @@ UDPStreamer::~UDPStreamer() {
scratchTimestamps = NULL_PTR(uint64 *);
}
/* Multicast-mode cleanup */
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
}
if (tcpListener.IsValid()) {
(void) tcpListener.Close();
}
if (dataSocket.IsValid()) {
(void) dataSocket.Close();
}
if (signalInfos != NULL_PTR(UDPStreamerSignalInfo *)) {
delete[] signalInfos;
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
@@ -332,37 +287,25 @@ bool UDPStreamer::Initialise(StructuredDataI &data) {
}
if (ok) {
StreamString mcastStr = "";
(void) data.Read("MulticastGroup", mcastStr);
if (mcastStr.Size() > 0u) {
multicastGroup = mcastStr;
useMulticast = true;
uint16 dp = 0u;
if (!data.Read("DataPort", dp)) {
dp = port + UDPS_DEFAULT_DATA_PORT_OFFSET;
REPORT_ERROR(ErrorManagement::Information,
"DataPort not specified; using Port+1 = %u.",
static_cast<uint32>(dp));
}
if ((dp == 0u) || (dp == port)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"DataPort %u must be non-zero and differ from Port %u.",
static_cast<uint32>(dp), static_cast<uint32>(port));
ok = false;
}
else {
dataPort = dp;
REPORT_ERROR(ErrorManagement::Information,
"Multicast mode: group=%s, controlPort=%u, dataPort=%u.",
multicastGroup.Buffer(),
static_cast<uint32>(port),
static_cast<uint32>(dataPort));
// Build server config with already-resolved Port and MaxPayloadSize so
// UDPSServer::Initialise() always sees them, even when defaults were used.
ConfigurationDatabase serverCfg;
(void) serverCfg.Write("Port", static_cast<uint32>(port));
(void) serverCfg.Write("MaxPayloadSize", maxPayloadSize);
// Forward optional multicast / timeout params if present in caller's data.
StreamString mcGroup;
if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
(void) serverCfg.Write("MulticastGroup", mcGroup.Buffer());
uint32 dp = 0u;
if (data.Read("DataPort", dp)) {
(void) serverCfg.Write("DataPort", dp);
}
}
else {
useMulticast = false;
uint32 clientTimeout = 0u;
if (data.Read("ClientTimeout", clientTimeout)) {
(void) serverCfg.Write("ClientTimeout", clientTimeout);
}
ok = server.Initialise(serverCfg);
}
return ok;
@@ -843,59 +786,28 @@ bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
const char8 *const nextStateName) {
bool ok = true;
if (useMulticast) {
/* Open TCP listener for control (CONNECT/DISCONNECT) */
if (!tcpListener.IsValid()) {
ok = tcpListener.Open();
if (ok) {
ok = tcpListener.Listen(port, UDPS_TCP_MAX_CONNECTIONS);
}
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not open TCP listener on port %u.",
static_cast<uint32>(port));
}
/* SetBlocking(false) makes WaitConnection(TimeoutType(0u)) non-blocking.
* If it fails, the select() guard in Execute() protects us. */
if (ok) {
if (!tcpListener.SetBlocking(false)) {
REPORT_ERROR(ErrorManagement::Warning,
"SetBlocking(false) on TCP listener failed; "
"using select() guard only.");
}
}
}
ok = server.Start();
/* Open UDP data socket aimed at multicast group */
if (ok && !dataSocket.IsValid()) {
ok = dataSocket.Open();
if (ok) {
ok = dataSocket.Connect(multicastGroup.Buffer(), dataPort);
/* Build the CONFIG payload and cache it in the server so any CONNECT client
* receives it immediately. The config is static for the lifetime of this state. */
if (ok) {
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u + 1u;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
if (cfgBuf != NULL_PTR(uint8 *)) {
uint32 cfgPayloadSize = 0u;
if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
(void) server.SendConfig(cfgBuf, cfgPayloadSize);
}
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not connect data socket to %s:%u.",
multicastGroup.Buffer(),
static_cast<uint32>(dataPort));
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not build initial CONFIG payload.");
}
heap->Free(reinterpret_cast<void *&>(cfgBuf));
}
}
else {
/* Unicast mode: existing UDP server socket (idempotent: skip if already valid) */
if (!serverSocket.IsValid()) {
ok = serverSocket.Open();
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not open server UDP socket.");
}
if (ok) {
ok = serverSocket.Listen(port);
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not bind server socket to port %u.",
static_cast<uint32>(port));
}
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not allocate CONFIG buffer.");
}
}
@@ -1022,95 +934,10 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
bool dataReady = (waitErr == ErrorManagement::NoError);
/* --- Poll for incoming control commands --- */
uint32 hdrSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
if (useMulticast) {
/* Multicast mode: non-blocking TCP accept + poll existing TCP client */
Handle listenFd = tcpListener.GetReadHandle();
fd_set rfdL;
FD_ZERO(&rfdL);
FD_SET(static_cast<int>(listenFd), &rfdL);
struct timeval tvL = { 0, 0 };
if (select(static_cast<int>(listenFd) + 1, &rfdL,
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvL) > 0) {
BasicTCPSocket *newClient = tcpListener.WaitConnection(TimeoutType(0u));
if (newClient != NULL_PTR(BasicTCPSocket *)) {
/* Evict any existing client */
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
clientConnected = false;
}
tcpClient = newClient;
/* Read CONNECT packet from the new TCP connection */
uint8 connBuf[sizeof(UDPSPacketHeader)];
uint32 recvSize = hdrSize;
bool readOk = tcpClient->Read(
reinterpret_cast<char8 *>(connBuf), recvSize);
if (readOk && (recvSize >= hdrSize)) {
HandleTCPConnect(connBuf, recvSize);
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"TCP: incomplete CONNECT (%u bytes); closing.",
recvSize);
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
}
}
}
/* --- Poll for incoming control commands (CONNECT / DISCONNECT / ACK) --- */
server.ServiceClients();
/* Poll existing TCP client for DISCONNECT or closed connection */
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
Handle cfd = tcpClient->GetReadHandle();
fd_set rfdC;
FD_ZERO(&rfdC);
FD_SET(static_cast<int>(cfd), &rfdC);
struct timeval tvC = { 0, 0 };
if (select(static_cast<int>(cfd) + 1, &rfdC,
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvC) > 0) {
uint8 cmdBuf[256u];
uint32 cmdSize = static_cast<uint32>(sizeof(cmdBuf));
bool readOk = tcpClient->Read(
reinterpret_cast<char8 *>(cmdBuf), cmdSize);
if (readOk && (cmdSize >= hdrSize)) {
HandleClientCommand(cmdBuf, cmdSize);
}
else {
/* Zero-byte / error = TCP peer closed */
REPORT_ERROR(ErrorManagement::Information,
"TCP client disconnected (read %u bytes).", cmdSize);
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
clientConnected = false;
}
}
}
}
else {
/* Unicast mode: existing UDP serverSocket non-blocking poll */
uint8 cmdBuf[256u];
Handle sockFd = serverSocket.GetReadHandle();
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(static_cast<int>(sockFd), &rfds);
struct timeval tv = { 0, 0 };
int nReady = select(static_cast<int>(sockFd) + 1, &rfds,
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
if (nReady > 0) {
uint32 recvSize = static_cast<uint32>(sizeof(cmdBuf));
bool received = serverSocket.Read(
reinterpret_cast<char8 *>(cmdBuf), recvSize);
if (received && (recvSize >= hdrSize)) {
HandleClientCommand(cmdBuf, recvSize);
}
}
}
if (dataReady && clientConnected) {
if (dataReady && server.HasClients()) {
/* Synchronise() already gates posting dataSem to the correct rate
* (size/time for Accumulate, every-Nth for Decimate, every call for
* Strict). Execute() just sends whatever is in the ready buffers. */
@@ -1134,8 +961,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
uint32 sendBytes = UDPS_TIMESTAMP_BYTES + 4u +
fill * singleCycleWireBytes + fixedWireBytes;
packetCounter++;
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
wireBuffer, sendBytes)) {
if (!server.SendData(packetCounter, wireBuffer, sendBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
"Failed to send Accumulate DATA packet (counter=%u).",
packetCounter);
@@ -1154,8 +980,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
QuantizeAndSerialize(scratchBuffer, ts);
packetCounter++;
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
wireBuffer, totalWireBytes)) {
if (!server.SendData(packetCounter, wireBuffer, totalWireBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
"Failed to send DATA packet (counter=%u).",
packetCounter);
@@ -1165,19 +990,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
}
if (info.GetStage() == ExecutionInfo::TerminationStage) {
if (clientConnected) {
if (useMulticast) {
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
}
}
else {
(void) clientSocket.Close();
}
clientConnected = false;
}
(void) server.Stop();
REPORT_ERROR(ErrorManagement::Information,
"UDPStreamer background thread terminated.");
}
@@ -1331,88 +1144,6 @@ void UDPStreamer::SerializeAccumulated(const uint8 *src,
}
}
void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
return;
}
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
if (hdr->magic != UDPS_MAGIC) {
return;
}
if (hdr->type == UDPS_TYPE_CONNECT) {
if (useMulticast) {
/* In multicast mode, CONNECT is handled by HandleTCPConnect(); ignore here */
return;
}
InternetHost src = serverSocket.GetSource();
/* Disconnect any previous client */
if (clientConnected) {
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
clientConnected = false;
}
/* Open a new client socket and connect to the requesting address */
bool sockOk = clientSocket.Open();
if (sockOk) {
sockOk = clientSocket.Connect(src.GetAddress().Buffer(), src.GetPort());
}
if (sockOk) {
clientConnected = true;
REPORT_ERROR(ErrorManagement::Information,
"Client connected from %s:%u.",
src.GetAddress().Buffer(),
static_cast<uint32>(src.GetPort()));
/* Send CONFIG packet */
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u + 1u;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
if (cfgBuf != NULL_PTR(uint8 *)) {
uint32 cfgPayloadSize = 0u;
if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
(void) SendFragmented(UDPS_TYPE_CONFIG, 0u, cfgBuf, cfgPayloadSize);
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not build CONFIG payload.");
}
heap->Free(reinterpret_cast<void *&>(cfgBuf));
}
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not connect to client %s:%u.",
src.GetAddress().Buffer(),
static_cast<uint32>(src.GetPort()));
}
}
else if (hdr->type == UDPS_TYPE_DISCONNECT) {
REPORT_ERROR(ErrorManagement::Information, "Client sent DISCONNECT.");
if (clientConnected) {
if (!useMulticast) {
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
}
/* In multicast mode, tcpClient cleanup is done by the Execute() poll caller */
clientConnected = false;
}
}
else if (hdr->type == UDPS_TYPE_ACK) {
/* Optional: track acknowledged counters for loss detection */
if (size >= static_cast<uint32>(sizeof(UDPSPacketHeader)) + 4u) {
uint32 ackedCounter = 0u;
const uint8 *pl = buf + sizeof(UDPSPacketHeader);
(void) MemoryOperationsHelper::Copy(&ackedCounter, pl, 4u);
REPORT_ERROR(ErrorManagement::Debug,
"ACK received for packet counter %u.", ackedCounter);
}
}
}
bool UDPStreamer::BuildConfigPayload(uint8 *buf,
uint32 bufSize,
@@ -1580,65 +1311,6 @@ void UDPStreamer::QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp) {
}
}
bool UDPStreamer::SendFragmented(uint8 type,
uint32 counter,
const uint8 *payload,
uint32 payloadSize) {
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
uint32 maxChunk = maxPayloadSize - headerSize;
uint32 totalFrags = (payloadSize == 0u) ? 1u :
((payloadSize + maxChunk - 1u) / maxChunk);
uint32 sendBufSize = headerSize + maxChunk;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(sendBufSize));
if (sendBuf == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate send buffer (%u bytes).", sendBufSize);
return false;
}
bool ok = true;
uint32 offs = 0u;
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
uint32 chunkSize = payloadSize - offs;
if (chunkSize > maxChunk) {
chunkSize = maxChunk;
}
UDPSPacketHeader *hdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
hdr->magic = UDPS_MAGIC;
hdr->type = type;
hdr->counter = counter;
hdr->fragmentIdx = static_cast<uint16>(f);
hdr->totalFragments = static_cast<uint16>(totalFrags);
hdr->payloadBytes = chunkSize;
if (chunkSize > 0u) {
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
payload + offs,
chunkSize);
}
uint32 sendSize = headerSize + chunkSize;
if (useMulticast) {
ok = dataSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
}
else {
ok = clientSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
}
if (!ok) {
REPORT_ERROR(ErrorManagement::Warning,
"Fragment %u/%u send failed.", f + 1u, totalFrags);
}
offs += chunkSize;
}
heap->Free(reinterpret_cast<void *&>(sendBuf));
return ok;
}
uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
uint8 code = UDPS_TYPECODE_UNKNOWN;
if (td == UnsignedInteger8Bit) { code = UDPS_TYPECODE_UINT8; }
@@ -1654,78 +1326,6 @@ uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
return code;
}
void UDPStreamer::HandleTCPConnect(const uint8 *buf, uint32 size) {
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
return;
}
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
if (hdr->magic != UDPS_MAGIC) {
REPORT_ERROR(ErrorManagement::Warning,
"TCP CONNECT: bad magic 0x%08X.", hdr->magic);
return;
}
if (hdr->type != UDPS_TYPE_CONNECT) {
REPORT_ERROR(ErrorManagement::Warning,
"TCP CONNECT: unexpected packet type %u.", hdr->type);
return;
}
clientConnected = true;
REPORT_ERROR(ErrorManagement::Information,
"TCP client connected; DATA will multicast to %s:%u.",
multicastGroup.Buffer(), static_cast<uint32>(dataPort));
/* Build CONFIG payload and send as a single TCP message (no fragmentation needed) */
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
if (cfgBuf == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate CONFIG buffer.");
clientConnected = false;
return;
}
uint32 cfgPayloadSize = 0u;
bool built = BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize);
if (built) {
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
uint32 totalSize = headerSize + cfgPayloadSize;
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(totalSize));
if (sendBuf != NULL_PTR(uint8 *)) {
UDPSPacketHeader *outHdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
outHdr->magic = UDPS_MAGIC;
outHdr->type = UDPS_TYPE_CONFIG;
outHdr->counter = 0u;
outHdr->fragmentIdx = 0u;
outHdr->totalFragments = 1u;
outHdr->payloadBytes = cfgPayloadSize;
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
cfgBuf, cfgPayloadSize);
bool writeOk = tcpClient->Write(
reinterpret_cast<const char8 *>(sendBuf), totalSize);
if (!writeOk) {
REPORT_ERROR(ErrorManagement::Warning,
"TCP: failed to send CONFIG packet.");
clientConnected = false;
}
heap->Free(reinterpret_cast<void *&>(sendBuf));
}
else {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate TCP send buffer.");
clientConnected = false;
}
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"BuildConfigPayload failed; client not fully connected.");
clientConnected = false;
}
heap->Free(reinterpret_cast<void *&>(cfgBuf));
}
uint16 UDPStreamer::GetPort() const {
return port;
}
@@ -1735,11 +1335,11 @@ uint32 UDPStreamer::GetMaxPayloadSize() const {
}
bool UDPStreamer::IsClientConnected() const {
return clientConnected;
return server.HasClients();
}
bool UDPStreamer::IsMulticast() const {
return useMulticast;
return server.IsMulticast();
}
CLASS_REGISTER(UDPStreamer, "1.0")
Source/Components/DataSources/UDPStreamer/UDPStreamer.h
+3 -73
View File
@@ -31,8 +31,6 @@
/*---------------------------------------------------------------------------*/
/* Project header includes */
/*---------------------------------------------------------------------------*/
#include "BasicTCPSocket.h"
#include "BasicUDPSocket.h"
#include "CompilerTypes.h"
#include "EmbeddedServiceMethodBinderI.h"
#include "EventSem.h"
@@ -40,6 +38,7 @@
#include "MemoryDataSourceI.h"
#include "SingleThreadService.h"
#include "StreamString.h"
#include "UDPSServer.h"
/*---------------------------------------------------------------------------*/
/* Class declaration */
@@ -107,48 +106,6 @@ struct UDPStreamerSignalInfo {
bool accumulated; /**< True when this scalar was expanded to flushCount elements in Auto accumulation mode */
};
/**
* @brief Magic number for UDPStreamer packets: 'UDPS' in little-endian.
*/
static const uint32 UDPS_MAGIC = 0x53504455u;
/**
* @brief Packet type codes.
*/
static const uint8 UDPS_TYPE_DATA = 0u; /**< Server → Client: signal data */
static const uint8 UDPS_TYPE_CONFIG = 1u; /**< Server → Client: signal configuration */
static const uint8 UDPS_TYPE_ACK = 2u; /**< Client → Server: acknowledge counter */
static const uint8 UDPS_TYPE_CONNECT = 3u; /**< Client → Server: connect request */
static const uint8 UDPS_TYPE_DISCONNECT = 4u; /**< Client → Server: disconnect */
/**
* @brief Wire packet header (17 bytes, packed).
*/
struct UDPSPacketHeader {
uint32 magic; /**< Must equal UDPS_MAGIC */
uint8 type; /**< One of UDPS_TYPE_* */
uint32 counter; /**< Sequence counter (per data update, not per fragment) */
uint16 fragmentIdx; /**< 0-based index of this fragment */
uint16 totalFragments; /**< Total fragments for this update (1 = no fragmentation) */
uint32 payloadBytes; /**< Bytes of payload immediately following this header */
} __attribute__((packed));
/**
* @brief Signal type codes transmitted in the CONFIG packet.
* These are simplified type codes independent of MARTe2 internals.
*/
static const uint8 UDPS_TYPECODE_UINT8 = 0u;
static const uint8 UDPS_TYPECODE_INT8 = 1u;
static const uint8 UDPS_TYPECODE_UINT16 = 2u;
static const uint8 UDPS_TYPECODE_INT16 = 3u;
static const uint8 UDPS_TYPECODE_UINT32 = 4u;
static const uint8 UDPS_TYPECODE_INT32 = 5u;
static const uint8 UDPS_TYPECODE_UINT64 = 6u;
static const uint8 UDPS_TYPECODE_INT64 = 7u;
static const uint8 UDPS_TYPECODE_FLOAT32 = 8u;
static const uint8 UDPS_TYPECODE_FLOAT64 = 9u;
static const uint8 UDPS_TYPECODE_UNKNOWN = 255u;
/**
* @brief A DataSource that streams MARTe2 signals to UDP clients.
*
@@ -378,23 +335,6 @@ private:
*/
void QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp);
/**
* @brief Sends payload as one or more fragmented UDP datagrams to the connected client.
* @return true if all datagrams were sent without error.
*/
bool SendFragmented(uint8 type, uint32 counter, const uint8 *payload, uint32 payloadSize);
/**
* @brief Handles a single received command packet from a client (unicast mode).
*/
void HandleClientCommand(const uint8 *buf, uint32 size);
/**
* @brief Handles a CONNECT received on the TCP control socket (multicast mode).
* @details Validates magic+type, sets clientConnected, builds and sends CONFIG via tcpClient.
*/
void HandleTCPConnect(const uint8 *buf, uint32 size);
/**
* @brief Serializes an accumulated batch into wireBuffer.
* @param src Flat buffer [numSamples × totalSrcBytes], slot 0 = oldest.
@@ -447,18 +387,8 @@ private:
uint8 *wireBuffer; /**< Pre-allocated buffer for the serialized DATA payload */
uint32 totalWireBytes; /**< Total bytes of all signals after quantization + 8-byte timestamp prefix */
/* Networking — unicast mode */
BasicUDPSocket serverSocket; /**< Bound to port; receives client commands (unicast) */
BasicUDPSocket clientSocket; /**< Configured to send to the connected client (unicast) */
volatile bool clientConnected; /**< True when a client has successfully connected */
/* Networking — multicast mode (only used when useMulticast == true) */
StreamString multicastGroup; /**< Multicast group IP, e.g. "239.0.0.1"; empty = unicast */
uint16 dataPort; /**< UDP port for DATA datagrams in multicast mode */
bool useMulticast; /**< Derived from multicastGroup.Size() > 0 in Initialise() */
BasicTCPSocket tcpListener; /**< TCP server socket: accepts control connections */
BasicTCPSocket *tcpClient; /**< Accepted TCP control connection (heap by WaitConnection) */
BasicUDPSocket dataSocket; /**< UDP socket aimed at multicastGroup:dataPort */
/* Networking — handled by UDPSServer */
UDPSServer server; /**< Multi-client session manager (unicast and multicast) */
/* Thread management */
SingleThreadService executor; /**< Background thread service */
Source/Components/DataSources/UDPStreamer/depends.x86-linux
+27 -24
View File
@@ -77,15 +77,8 @@
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/IOBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/CharBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StreamI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/Object.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StringHelper.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/StructuredDataI.h \
@@ -93,14 +86,6 @@
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/CLASSREGISTER.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ClassRegistryItemT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ObjectBuilderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabaseNode.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Fnv1aHashFunction.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/HashFunction.h \
@@ -121,10 +106,33 @@
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Vector.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamString.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamStringIOBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/StatefulI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/ExecutableI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/Sleep.h \
UDPStreamer.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h \
../../../..//Source/Components/Interfaces/UDPStream/UDPSServer.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicTCPSocket.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicSocket.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
@@ -133,10 +141,5 @@
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HandleI.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/Environment/Linux/SocketCore.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicUDPSocket.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
../../../..//Common/UDP/UDPSProtocol.h
Source/Components/DataSources/UDPStreamer/dependsRaw.x86-linux
+27 -24
View File
@@ -77,15 +77,8 @@ UDPStreamer.o: UDPStreamer.cpp \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/IOBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/CharBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StreamI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/Object.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StringHelper.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/StructuredDataI.h \
@@ -93,14 +86,6 @@ UDPStreamer.o: UDPStreamer.cpp \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/CLASSREGISTER.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ClassRegistryItemT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ObjectBuilderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabaseNode.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Fnv1aHashFunction.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/HashFunction.h \
@@ -121,10 +106,33 @@ UDPStreamer.o: UDPStreamer.cpp \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Vector.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamString.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamStringIOBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/StatefulI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/ExecutableI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/Sleep.h \
UDPStreamer.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h \
../../../..//Source/Components/Interfaces/UDPStream/UDPSServer.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicTCPSocket.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicSocket.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
@@ -133,10 +141,5 @@ UDPStreamer.o: UDPStreamer.cpp \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HandleI.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/Environment/Linux/SocketCore.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicUDPSocket.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
../../../..//Common/UDP/UDPSProtocol.h
Source/Components/GAMs/TimeArrayGAM/TimeArrayGAM.cpp
+2 -2
View File
@@ -71,12 +71,12 @@ bool TimeArrayGAM::Setup() {
uint32 sz = 0u;
ok = GetSignalByteSize(OutputSignals, 0u, sz);
if (ok) {
nElements = sz / static_cast<uint32>(sizeof(uint32));
nElements = sz / static_cast<uint32>(sizeof(uint64));
ok = (nElements > 0u);
}
if (!ok) {
REPORT_ERROR(ErrorManagement::InitialisationError,
"TimeArrayGAM: output signal must be a non-empty uint32 array.");
"TimeArrayGAM: output signal must be a non-empty uint64 array.");
}
return ok;
}
Source/Components/Interfaces/DebugService/DebugService.cpp
+25 -52
View File
@@ -61,7 +61,7 @@ DebugService::~DebugService() {
threadService.Stop();
streamerService.Stop();
tcpServer.Close();
udpSocket.Close();
(void) udpsServer.Stop();
if (activeClient != NULL_PTR(BasicTCPSocket *)) {
activeClient->Close();
delete activeClient;
@@ -136,10 +136,16 @@ bool DebugService::Initialise(StructuredDataI &data) {
if (!tcpServer.Open()) return false;
if (!tcpServer.Listen(controlPort)) return false;
if (!udpSocket.Open()) return false;
// Note: do NOT bind udpSocket to streamPort here. The Go client
// must own that port to receive streamed data. The socket sends
// via an ephemeral source port, which is fine for UDP.
// Initialise UDP streamer in push-only mode (port=0 skips serverSocket bind)
ConfigurationDatabase udpsConfig;
(void)udpsConfig.Write("Port", 0u);
(void)udpsConfig.Write("MaxPayloadSize", static_cast<uint32>(UDPSServer::UDPS_SERVER_DEFAULT_MAX_PAYLOAD));
if (!udpsServer.Initialise(udpsConfig)) return false;
if (!udpsServer.Start()) return false;
if (streamPort > 0u) {
(void)udpsServer.AddStaticClient(streamIP.Buffer(), streamPort);
}
if (threadService.Start() != ErrorManagement::NoError) return false;
if (streamerService.Start() != ErrorManagement::NoError) return false;
@@ -416,11 +422,10 @@ ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
return ErrorManagement::NoError;
}
// Set UDP destination
InternetHost dest(streamPort, streamIP.Buffer());
(void)udpSocket.SetDestination(dest);
// a) Poll for new CONNECT clients (no-op in push-only mode with port=0)
udpsServer.ServiceClients();
// a) If config has changed, rebuild and send CONFIG packet
// b) If config has changed, rebuild and send CONFIG packet
if (udpsConfigPending) {
SendUDPSConfig();
udpsConfigPending = false;
@@ -449,12 +454,12 @@ ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
}
}
// c) If we have data, stamp with HRT and send
// c) If we have data, stamp with HRT and send via udpsServer
if (anyData && udpsNumSlots > 0u && udpsDataPayload != NULL_PTR(uint8 *)) {
uint64 hrt = HighResolutionTimer::Counter();
memcpy(udpsDataPayload, &hrt, 8u);
SendUDPSFragmented(UDPS_TYPE_DATA, udpsDataPayload, udpsDataPayloadSize);
udpsPacketCounter++;
(void)udpsServer.SendData(udpsPacketCounter, udpsDataPayload, udpsDataPayloadSize);
}
if (!anyData) {
@@ -480,8 +485,10 @@ bool DebugService::SendUDPSConfig() {
}
}
// Build CONFIG payload into udpsTxBuf starting at UDPS_HEADER_SIZE offset
uint8 *payload = udpsTxBuf + UDPS_HEADER_SIZE;
// Build CONFIG payload into a local stack buffer
static const uint32 CFG_BUF_SIZE = 65535u;
uint8 cfgBuf[CFG_BUF_SIZE];
uint8 *payload = cfgBuf;
// Write numTraced (uint32 LE)
memcpy(payload, &tracedCount, 4u);
@@ -521,7 +528,7 @@ bool DebugService::SendUDPSConfig() {
uint32 wireSize = UDPSTypeCodeByteSize(typeCode) * numElements;
// Write signal descriptor (136 bytes) to payload
if (payloadOffset + UDPS_SIGNAL_DESC_SIZE <= sizeof(udpsTxBuf) - UDPS_HEADER_SIZE - 1u) {
if (payloadOffset + UDPS_SIGNAL_DESC_SIZE <= CFG_BUF_SIZE - 1u) {
UDPSSignalDescriptor *desc = reinterpret_cast<UDPSSignalDescriptor *>(payload + payloadOffset);
memset(desc, 0, UDPS_SIGNAL_DESC_SIZE);
if (signals[i]->name.Size() > 0u) {
@@ -554,7 +561,7 @@ bool DebugService::SendUDPSConfig() {
}
// Write publish mode byte
if (payloadOffset < sizeof(udpsTxBuf) - UDPS_HEADER_SIZE) {
if (payloadOffset < CFG_BUF_SIZE) {
payload[payloadOffset] = UDPS_PUBLISH_STRICT;
payloadOffset++;
}
@@ -574,45 +581,11 @@ bool DebugService::SendUDPSConfig() {
memset(udpsDataPayload, 0, udpsDataPayloadSize);
}
// Send CONFIG packet
SendUDPSFragmented(UDPS_TYPE_CONFIG, payload, payloadOffset);
// Send CONFIG packet (also cached in udpsServer for new CONNECT clients)
udpsPacketCounter++;
(void)udpsServer.SendConfig(payload, payloadOffset);
return true;
}
// ---------------------------------------------------------------------------
// SendUDPSFragmented — fragment and send a UDPS packet
// ---------------------------------------------------------------------------
bool DebugService::SendUDPSFragmented(uint8 type, const uint8 *payload, uint32 payloadSize) {
if (payloadSize <= UDPS_MAX_PAYLOAD) {
// Single datagram
UDPSBuildHeader(udpsTxBuf, type, udpsPacketCounter, 0u, 1u, payloadSize);
if (payload != (udpsTxBuf + UDPS_HEADER_SIZE)) {
memcpy(udpsTxBuf + UDPS_HEADER_SIZE, payload, payloadSize);
}
uint32 toWrite = UDPS_HEADER_SIZE + payloadSize;
(void)udpSocket.Write(reinterpret_cast<char8 *>(udpsTxBuf), toWrite);
} else {
// Fragmented
uint32 numFrags = (payloadSize + UDPS_MAX_PAYLOAD - 1u) / UDPS_MAX_PAYLOAD;
uint32 offset = 0u;
for (uint32 i = 0u; i < numFrags; i++) {
uint32 chunkSize = UDPS_MAX_PAYLOAD;
if (offset + chunkSize > payloadSize) {
chunkSize = payloadSize - offset;
}
UDPSBuildHeader(udpsTxBuf, type, udpsPacketCounter,
static_cast<uint16>(i),
static_cast<uint16>(numFrags),
chunkSize);
memcpy(udpsTxBuf + UDPS_HEADER_SIZE, payload + offset, chunkSize);
uint32 toWrite = UDPS_HEADER_SIZE + chunkSize;
(void)udpSocket.Write(reinterpret_cast<char8 *>(udpsTxBuf), toWrite);
offset += chunkSize;
}
}
return true;
}
} // namespace MARTe
Source/Components/Interfaces/DebugService/DebugService.h
+2 -15
View File
@@ -2,13 +2,12 @@
#define DEBUGSERVICE_H
#include "BasicTCPSocket.h"
#include "BasicUDPSocket.h"
#include "DebugServiceBase.h"
#include "EmbeddedServiceMethodBinderI.h"
#include "MessageI.h"
#include "ReferenceT.h"
#include "SingleThreadService.h"
#include "UDPSProtocol.h"
#include "UDPSServer.h"
namespace MARTe {
@@ -67,15 +66,6 @@ private:
*/
bool SendUDPSConfig();
/**
* @brief Fragment and send a payload as one or more UDPS datagrams.
* @param type Packet type (UDPS_TYPE_CONFIG or UDPS_TYPE_DATA).
* @param payload Pointer to fully-built payload.
* @param payloadSize Byte count of payload.
* @return true if all datagrams were written without error.
*/
bool SendUDPSFragmented(uint8 type, const uint8 *payload, uint32 payloadSize);
// -----------------------------------------------------------------------
// TCP/UDP transport configuration
// -----------------------------------------------------------------------
@@ -87,7 +77,7 @@ private:
bool suppressTimeoutLogs;
BasicTCPSocket tcpServer;
BasicUDPSocket udpSocket;
UDPSServer udpsServer; ///< Handles fragmentation and multi-client sending
// -----------------------------------------------------------------------
// Server-thread helper class
@@ -165,9 +155,6 @@ private:
/** Staging buffer: a single sample popped from traceBuffer. */
uint8 udpsSampleBuf[65535u];
/** General-purpose TX buffer for CONFIG and DATA packet assembly. */
uint8 udpsTxBuf[65535u];
/** Packet sequence counter (incremented per DATA/CONFIG datagram group). */
uint32 udpsPacketCounter;
Source/Components/Interfaces/DebugService/Makefile.inc
+4 -1
View File
@@ -9,9 +9,12 @@ ROOT_DIR=../../../../
MAKEDEFAULTDIR=$(MARTe2_DIR)/MakeDefaults
include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
# Shared UDPS protocol header (from Common/)
# Shared UDPS protocol header (from Common/) and UDPStream library
INCLUDES += -I$(ROOT_DIR)/Common/UDP
INCLUDES += -I$(ROOT_DIR)/Source/Components/Interfaces/UDPStream
INCLUDES += -I$(ROOT_DIR)/Source/Components/Interfaces/TCPLogger
LIBRARIES += -L$(BUILD_DIR)/../UDPStream -lUDPStream
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L0Types
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L1Portability
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L2Objects
Source/Components/Interfaces/Makefile.gcc
+2
View File
@@ -1,9 +1,11 @@
all:
$(MAKE) -C TCPLogger -f Makefile.gcc
$(MAKE) -C UDPStream -f Makefile.gcc
$(MAKE) -C DebugService -f Makefile.gcc
clean:
$(MAKE) -C TCPLogger -f Makefile.gcc clean
$(MAKE) -C UDPStream -f Makefile.gcc clean
$(MAKE) -C DebugService -f Makefile.gcc clean
.PHONY: all clean
Source/Components/Interfaces/UDPStream/Makefile.gcc
+1
View File
@@ -0,0 +1 @@
include Makefile.inc
Source/Components/Interfaces/UDPStream/Makefile.inc
+30
View File
@@ -0,0 +1,30 @@
#############################################################
# MARTe2 Integrated Components — UDPStream
#############################################################
OBJSX=UDPSServer.x UDPSClient.x
PACKAGE=Components/Interfaces
ROOT_DIR=../../../../
MAKEDEFAULTDIR=$(MARTe2_DIR)/MakeDefaults
include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
INCLUDES += -I.
INCLUDES += -I$(ROOT_DIR)/Common/UDP
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L0Types
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L1Portability
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L2Objects
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L3Streams
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Messages
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Configuration
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L1Portability
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L3Services
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L1Portability
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L3Streams
all: $(OBJS) $(SUBPROJ) \
$(BUILD_DIR)/UDPStream$(LIBEXT) \
$(BUILD_DIR)/UDPStream$(DLLEXT)
echo $(OBJS)
include $(MAKEDEFAULTDIR)/MakeStdLibRules.$(TARGET)
Source/Components/Interfaces/UDPStream/UDPSClient.cpp
+693
View File
@@ -0,0 +1,693 @@
/**
* @file UDPSClient.cpp
* @brief Implementation of UDPSClient — auto-reconnecting UDPS receiver.
*/
#include "UDPSClient.h"
#include "AdvancedErrorManagement.h"
#include "MemoryOperationsHelper.h"
#include <sys/select.h>
#include <errno.h>
namespace MARTe {
// ---------------------------------------------------------------------------
// Constructor / Destructor
// ---------------------------------------------------------------------------
UDPSClient::UDPSClient()
: serverPort(0u),
dataPort(0u),
useMulticast(false),
silenceTimeoutTicks(0u),
reconnectDelayTicks(0u),
maxPayloadSize(UDPS_CLIENT_DEFAULT_MAX_PAYLOAD),
cpuMask(0xFFFFFFFFu),
stackSize(65536u),
listener(NULL_PTR(UDPSClientListener *)),
threadService(*this),
connected(false),
lastDataTicks(0u),
disconnectTick(0u),
localPort(0u),
lastGcTicks(0u) {
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
reassemblySlots[i].counter = 0u;
reassemblySlots[i].type = 0u;
reassemblySlots[i].totalFragments = 0u;
reassemblySlots[i].receivedFragments = 0u;
reassemblySlots[i].active = false;
reassemblySlots[i].firstSeenTicks = 0u;
reassemblySlots[i].chunkSize = 0u;
(void) MemoryOperationsHelper::Set(reassemblySlots[i].recvMask, 0, 32u);
}
}
UDPSClient::~UDPSClient() {
(void) Stop();
}
// ---------------------------------------------------------------------------
// Initialise
// ---------------------------------------------------------------------------
bool UDPSClient::Initialise(StructuredDataI &data) {
StreamString saddr;
if (!data.Read("ServerAddr", saddr) || (saddr.Size() == 0u)) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"UDPSClient: ServerAddr not specified.");
return false;
}
serverAddr = saddr;
uint32 portU32 = 0u;
if (!data.Read("Port", portU32)) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"UDPSClient: Port not specified.");
return false;
}
serverPort = static_cast<uint16>(portU32);
StreamString mcGroup;
if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
multicastGroup = mcGroup;
useMulticast = true;
}
if (useMulticast) {
uint32 dpU32 = static_cast<uint32>(serverPort) + 1u;
(void) data.Read("DataPort", dpU32);
dataPort = static_cast<uint16>(dpU32);
}
uint32 silenceS = UDPS_CLIENT_DEFAULT_SILENCE_TIMEOUT_S;
(void) data.Read("SilenceTimeout", silenceS);
silenceTimeoutTicks = static_cast<uint64>(silenceS) * HighResolutionTimer::Frequency();
uint32 reconnectS = UDPS_CLIENT_DEFAULT_RECONNECT_DELAY_S;
(void) data.Read("ReconnectDelay", reconnectS);
reconnectDelayTicks = static_cast<uint64>(reconnectS) * HighResolutionTimer::Frequency();
uint32 mps = UDPS_CLIENT_DEFAULT_MAX_PAYLOAD;
(void) data.Read("MaxPayloadSize", mps);
maxPayloadSize = mps;
(void) data.Read("CPUMask", cpuMask);
(void) data.Read("StackSize", stackSize);
return true;
}
// ---------------------------------------------------------------------------
// SetListener / Start / Stop
// ---------------------------------------------------------------------------
void UDPSClient::SetListener(UDPSClientListener *l) {
listener = l;
}
bool UDPSClient::Start() {
threadService.SetCPUMask(cpuMask);
threadService.SetStackSize(stackSize);
ErrorManagement::ErrorType err = threadService.Start();
return (err == ErrorManagement::NoError);
}
bool UDPSClient::Stop() {
ErrorManagement::ErrorType err = threadService.Stop();
Disconnect();
return (err == ErrorManagement::NoError);
}
// ---------------------------------------------------------------------------
// Execute (thread entry)
// ---------------------------------------------------------------------------
ErrorManagement::ErrorType UDPSClient::Execute(ExecutionInfo &info) {
if (info.GetStage() == ExecutionInfo::StartupStage) {
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSClient: Receive thread started.");
return ErrorManagement::NoError;
}
if (info.GetStage() == ExecutionInfo::TerminationStage) {
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSClient: Receive thread stopping.");
Disconnect();
return ErrorManagement::NoError;
}
// MainStage
uint64 now = HighResolutionTimer::Counter();
if (!connected) {
// Wait reconnectDelay before retrying
if ((disconnectTick == 0u) ||
((now - disconnectTick) >= reconnectDelayTicks)) {
if (!Connect()) {
disconnectTick = HighResolutionTimer::Counter();
}
}
return ErrorManagement::NoError;
}
// Connected: receive + process
bool ok = ReceiveAndProcess();
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Receive error; disconnecting.");
Disconnect();
return ErrorManagement::NoError;
}
/* Re-sample time AFTER ReceiveAndProcess() so that lastDataTicks (which may
* have been updated inside ReceiveAndProcess) is never newer than `now`.
* Without this, if a packet arrives during ReceiveAndProcess(), `now` (captured
* before the call) < lastDataTicks, causing unsigned wraparound in the
* subtraction and a spurious silence timeout. */
now = HighResolutionTimer::Counter();
// Check silence timeout
if (silenceTimeoutTicks > 0u) {
uint64 lastSeen = lastDataTicks;
if ((lastSeen > 0u) && ((now - lastSeen) >= silenceTimeoutTicks)) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Server silent; disconnecting.");
Disconnect();
}
}
// Periodic GC of stale reassembly slots (~every 1 s)
uint64 gcFreq = HighResolutionTimer::Frequency();
if ((now - lastGcTicks) >= gcFreq) {
GcReassemblySlots();
lastGcTicks = now;
}
return ErrorManagement::NoError;
}
// ---------------------------------------------------------------------------
// Private: Connect
// ---------------------------------------------------------------------------
bool UDPSClient::Connect() {
bool ok = useMulticast ? ConnectMulticast() : ConnectUnicast();
if (ok) {
connected = true;
lastDataTicks = HighResolutionTimer::Counter();
if (listener != NULL_PTR(UDPSClientListener *)) {
listener->OnUDPSConnected();
}
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSClient: Connected to %s:%u.",
serverAddr.Buffer(), static_cast<uint32>(serverPort));
}
return ok;
}
bool UDPSClient::ConnectUnicast() {
// Open a local UDP socket bound to an ephemeral port
if (!recvSocket.Open()) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Could not open receive socket.");
return false;
}
if (!recvSocket.Listen(0u)) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Could not bind receive socket.");
(void) recvSocket.Close();
return false;
}
// Build and send CONNECT packet to server
uint8 connectPkt[UDPS_HEADER_SIZE];
UDPSBuildHeader(connectPkt, UDPS_TYPE_CONNECT, 0u, 0u, 1u, 0u);
InternetHost serverDest(serverPort, serverAddr.Buffer());
(void) recvSocket.SetDestination(serverDest);
uint32 sendSize = UDPS_HEADER_SIZE;
bool ok = recvSocket.Write(reinterpret_cast<const char8 *>(connectPkt), sendSize);
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Could not send CONNECT to %s:%u.",
serverAddr.Buffer(), static_cast<uint32>(serverPort));
(void) recvSocket.Close();
return false;
}
return true;
}
bool UDPSClient::ConnectMulticast() {
/* Join the multicast group BEFORE sending CONNECT over TCP.
* The UDPStreamer broadcasts CONFIG via multicast immediately when it
* receives a CONNECT packet. If the multicast socket is not yet joined,
* that CONFIG packet is dropped by the kernel and the session never becomes
* configured. Correct order: join → CONNECT → receive CONFIG. */
// Open and bind the multicast receive socket first
if (!mcastSocket.Open()) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Could not open multicast socket.");
return false;
}
bool ok = mcastSocket.Listen(dataPort);
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Could not bind multicast socket on port %u.",
static_cast<uint32>(dataPort));
(void) mcastSocket.Close();
return false;
}
ok = mcastSocket.Join(multicastGroup.Buffer());
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Could not join multicast group %s.",
multicastGroup.Buffer());
(void) mcastSocket.Close();
return false;
}
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSClient: Joined multicast group %s on port %u.",
multicastGroup.Buffer(), static_cast<uint32>(dataPort));
// Now open the TCP control connection and announce ourselves
if (!tcpSocket.Open()) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Could not open TCP socket.");
(void) mcastSocket.Close();
return false;
}
ok = tcpSocket.Connect(serverAddr.Buffer(), serverPort);
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: TCP connect to %s:%u failed.",
serverAddr.Buffer(), static_cast<uint32>(serverPort));
(void) tcpSocket.Close();
(void) mcastSocket.Close();
return false;
}
// Send CONNECT — UDPStreamer will now multicast CONFIG, which we are ready to receive
uint8 connectPkt[UDPS_HEADER_SIZE];
UDPSBuildHeader(connectPkt, UDPS_TYPE_CONNECT, 0u, 0u, 1u, 0u);
uint32 sendSize = UDPS_HEADER_SIZE;
ok = tcpSocket.Write(reinterpret_cast<const char8 *>(connectPkt), sendSize);
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Could not send CONNECT over TCP.");
(void) tcpSocket.Close();
(void) mcastSocket.Close();
return false;
}
return true;
}
// ---------------------------------------------------------------------------
// Private: Disconnect
// ---------------------------------------------------------------------------
void UDPSClient::Disconnect() {
if (!connected) {
return;
}
// Send DISCONNECT
if (useMulticast) {
if (tcpSocket.IsValid()) {
uint8 disconnPkt[UDPS_HEADER_SIZE];
UDPSBuildHeader(disconnPkt, UDPS_TYPE_DISCONNECT, 0u, 0u, 1u, 0u);
uint32 sendSize = UDPS_HEADER_SIZE;
(void) tcpSocket.Write(reinterpret_cast<const char8 *>(disconnPkt), sendSize);
(void) tcpSocket.Close();
}
if (mcastSocket.IsValid()) {
(void) mcastSocket.Close();
}
}
else {
if (recvSocket.IsValid()) {
uint8 disconnPkt[UDPS_HEADER_SIZE];
UDPSBuildHeader(disconnPkt, UDPS_TYPE_DISCONNECT, 0u, 0u, 1u, 0u);
InternetHost serverDest(serverPort, serverAddr.Buffer());
(void) recvSocket.SetDestination(serverDest);
uint32 sendSize = UDPS_HEADER_SIZE;
(void) recvSocket.Write(reinterpret_cast<const char8 *>(disconnPkt), sendSize);
(void) recvSocket.Close();
}
}
connected = false;
disconnectTick = HighResolutionTimer::Counter();
if (listener != NULL_PTR(UDPSClientListener *)) {
listener->OnUDPSDisconnected();
}
}
// ---------------------------------------------------------------------------
// Private: ReceiveAndProcess
// ---------------------------------------------------------------------------
bool UDPSClient::ReceiveAndProcess() {
// Select the receive socket(s)
int fd = -1;
int tcpFd = -1;
if (useMulticast) {
fd = mcastSocket.GetReadHandle();
/* In multicast mode the server delivers CONFIG over the TCP control
* connection — it must be polled too, otherwise the session never
* becomes configured. */
tcpFd = tcpSocket.GetReadHandle();
}
else {
fd = recvSocket.GetReadHandle();
}
if (fd < 0) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: ReceiveAndProcess: socket fd < 0 (socket invalid).");
return false;
}
fd_set rset;
FD_ZERO(&rset);
FD_SET(fd, &rset);
int maxFd = fd;
if (tcpFd >= 0) {
FD_SET(tcpFd, &rset);
if (tcpFd > maxFd) {
maxFd = tcpFd;
}
}
// 10 ms timeout so Execute() doesn't busy-spin
struct timeval tv;
tv.tv_sec = 0; tv.tv_usec = 10000;
int nready = select(maxFd + 1, &rset, NULL, NULL, &tv);
if (nready < 0) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: ReceiveAndProcess: select() returned %d (errno=%d).",
nready, errno);
return false; // socket error
}
if (nready == 0) {
return true; // timeout, no data
}
// TCP control frame (multicast CONFIG path)
if ((tcpFd >= 0) && FD_ISSET(tcpFd, &rset)) {
if (!ReceiveTCPFrame()) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: TCP control connection lost.");
return false;
}
lastDataTicks = HighResolutionTimer::Counter();
}
if (!FD_ISSET(fd, &rset)) {
return true; // only the TCP socket was readable
}
uint32 recvSize = static_cast<uint32>(sizeof(recvBuf));
bool ok;
if (useMulticast) {
ok = mcastSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
}
else {
ok = recvSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
}
if (!ok || (recvSize < UDPS_HEADER_SIZE)) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: ReceiveAndProcess: Read() failed or short packet "
"(ok=%s, recvSize=%u, HEADER_SIZE=%u).",
ok ? "true" : "false", recvSize, UDPS_HEADER_SIZE);
return false;
}
lastDataTicks = HighResolutionTimer::Counter();
ProcessDatagram(recvBuf, recvSize);
return true;
}
// ---------------------------------------------------------------------------
// Private: ReceiveTCPFrame / ReadExactTCP
// ---------------------------------------------------------------------------
bool UDPSClient::ReceiveTCPFrame() {
/* TCP is a byte stream: read exactly one UDPS frame (17-byte header
* followed by payloadBytes of payload) and hand it to ProcessDatagram.
* Fragmented CONFIGs arrive as consecutive frames and go through the
* normal reassembly path. */
if (!ReadExactTCP(recvBuf, UDPS_HEADER_SIZE)) {
return false;
}
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(recvBuf);
if (hdr->magic != UDPS_MAGIC) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: TCP stream desynchronised (bad magic).");
return false;
}
uint32 payloadBytes = hdr->payloadBytes;
if (payloadBytes > (static_cast<uint32>(sizeof(recvBuf)) - UDPS_HEADER_SIZE)) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: TCP frame payload too large (%u).", payloadBytes);
return false;
}
if (payloadBytes > 0u) {
if (!ReadExactTCP(&recvBuf[UDPS_HEADER_SIZE], payloadBytes)) {
return false;
}
}
ProcessDatagram(recvBuf, UDPS_HEADER_SIZE + payloadBytes);
return true;
}
bool UDPSClient::ReadExactTCP(uint8 *dst, uint32 n) {
uint32 got = 0u;
while (got < n) {
uint32 chunk = n - got;
if (!tcpSocket.Read(reinterpret_cast<char8 *>(&dst[got]), chunk)) {
return false;
}
if (chunk == 0u) {
return false; // orderly close
}
got += chunk;
}
return true;
}
// ---------------------------------------------------------------------------
// Private: ProcessDatagram
// ---------------------------------------------------------------------------
void UDPSClient::ProcessDatagram(const uint8 *buf, uint32 size) {
if (size < UDPS_HEADER_SIZE) {
return;
}
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
if (hdr->magic != UDPS_MAGIC) {
return;
}
if ((hdr->type != UDPS_TYPE_DATA) && (hdr->type != UDPS_TYPE_CONFIG)) {
return;
}
uint32 payloadBytes = hdr->payloadBytes;
if ((payloadBytes + UDPS_HEADER_SIZE) > size) {
return; // truncated
}
if (hdr->totalFragments == 1u) {
if ((hdr->type == UDPS_TYPE_DATA) && (listener != NULL_PTR(UDPSClientListener *))) {
listener->OnUDPSFragment(hdr->counter, size, true);
}
// Single-fragment shortcut: deliver immediately
if (listener != NULL_PTR(UDPSClientListener *)) {
const uint8 *pl = buf + UDPS_HEADER_SIZE;
if (hdr->type == UDPS_TYPE_CONFIG) {
listener->OnUDPSConfig(pl, payloadBytes);
}
else {
listener->OnUDPSData(pl, payloadBytes);
}
}
return;
}
bool completed = PlaceFragment(hdr, buf + UDPS_HEADER_SIZE, payloadBytes);
if ((hdr->type == UDPS_TYPE_DATA) && (listener != NULL_PTR(UDPSClientListener *))) {
listener->OnUDPSFragment(hdr->counter, size, completed);
}
}
// ---------------------------------------------------------------------------
// Private: PlaceFragment
// ---------------------------------------------------------------------------
bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
const uint8 *payload,
uint32 payloadBytes) {
uint32 counter = hdr->counter;
uint16 fragIdx = hdr->fragmentIdx;
uint16 totalFrags = hdr->totalFragments;
if ((fragIdx >= totalFrags) || (totalFrags > 512u)) {
return false; // sanity check
}
// Find existing slot for this counter
uint32 slot = UDPS_CLIENT_MAX_REASSEMBLY_SLOTS;
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
if (reassemblySlots[i].active && (reassemblySlots[i].counter == counter)) {
slot = i;
break;
}
}
// Allocate new slot if not found
if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
if (!reassemblySlots[i].active) {
slot = i;
break;
}
}
if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
// All slots occupied — evict the oldest
uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL;
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
if (reassemblySlots[i].firstSeenTicks < oldestTick) {
oldestTick = reassemblySlots[i].firstSeenTicks;
slot = i;
}
}
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Reassembly slots full; evicting oldest.");
}
reassemblySlots[slot].counter = counter;
reassemblySlots[slot].type = hdr->type;
reassemblySlots[slot].totalFragments = totalFrags;
reassemblySlots[slot].receivedFragments = 0u;
reassemblySlots[slot].active = true;
reassemblySlots[slot].firstSeenTicks = HighResolutionTimer::Counter();
reassemblySlots[slot].chunkSize = 0u;
(void) MemoryOperationsHelper::Set(reassemblySlots[slot].recvMask, 0, 32u);
}
UDPSReassemblySlot &s = reassemblySlots[slot];
// Skip duplicate
uint32 byteIdx = fragIdx / 8u;
uint8 bitMask = static_cast<uint8>(1u << (fragIdx % 8u));
if (byteIdx < 32u) {
if ((s.recvMask[byteIdx] & bitMask) != 0u) {
return false; // already have this fragment
}
}
// Compute placement offset
uint32 chunkSize = s.chunkSize;
if (chunkSize == 0u) {
// Learn chunk size from first non-last fragment
if (fragIdx == 0u) {
chunkSize = payloadBytes;
s.chunkSize = chunkSize;
}
else {
// Can't place yet without knowing chunk size — drop (rare edge case)
return false;
}
}
uint32 offset = static_cast<uint32>(fragIdx) * chunkSize;
if ((offset + payloadBytes) > static_cast<uint32>(sizeof(s.payload))) {
return false; // overflow guard
}
if (payloadBytes > 0u) {
(void) MemoryOperationsHelper::Copy(s.payload + offset, payload, payloadBytes);
}
if (byteIdx < 32u) {
s.recvMask[byteIdx] |= bitMask;
}
s.receivedFragments++;
// Check if complete
if (s.receivedFragments >= s.totalFragments) {
DeliverAssembled(s);
s.active = false;
return true;
}
return false;
}
// ---------------------------------------------------------------------------
// Private: DeliverAssembled
// ---------------------------------------------------------------------------
void UDPSClient::DeliverAssembled(UDPSReassemblySlot &s) {
if (listener == NULL_PTR(UDPSClientListener *)) {
return;
}
// Total assembled size = (totalFrags-1)*chunkSize + lastFragSize
// We don't store lastFragSize separately, but the last receivedFragments
// Write placed payloadBytes at its offset — total = chunkSize*(totalFrags-1) + lastBytes.
// Since we don't track lastBytes directly, use the mask to infer completeness.
// The assembled payload size is not trivially known without tracking it.
// Simplest correct approach: accumulate total bytes written.
// For now, estimate as chunkSize * totalFragments (may be slightly over for last frag).
// Fix: track totalPayloadBytes in slot.
//
// Since we don't have that, compute based on known structure:
// totalFrags-1 full chunks + one last chunk (which might be smaller).
// We'd need to save the last fragment's payloadBytes.
// As a pragmatic solution, deliver chunkSize * totalFragments as upper bound —
// the receiver (CONFIG/DATA parser) knows the actual sizes from content.
//
// Better: save the actual total in the slot. We don't have that field.
// Use the simplest approximation that is always correct for aligned payloads,
// and let the application-layer parser determine exact size from content.
uint32 totalSize = s.chunkSize * static_cast<uint32>(s.totalFragments);
if (s.type == UDPS_TYPE_CONFIG) {
listener->OnUDPSConfig(s.payload, totalSize);
}
else {
listener->OnUDPSData(s.payload, totalSize);
}
}
// ---------------------------------------------------------------------------
// Private: GcReassemblySlots
// ---------------------------------------------------------------------------
void UDPSClient::GcReassemblySlots() {
uint64 staleThreshold = 2u * HighResolutionTimer::Frequency(); // 2 seconds
uint64 now = HighResolutionTimer::Counter();
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
if (!reassemblySlots[i].active) {
continue;
}
if ((now - reassemblySlots[i].firstSeenTicks) > staleThreshold) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Discarding stale reassembly slot (counter %u).",
reassemblySlots[i].counter);
reassemblySlots[i].active = false;
}
}
}
} // namespace MARTe
Source/Components/Interfaces/UDPStream/UDPSClient.h
+210
View File
@@ -0,0 +1,210 @@
#ifndef UDPS_CLIENT_H_
#define UDPS_CLIENT_H_
/**
* @file UDPSClient.h
* @brief Auto-reconnecting UDPS receiver client (C++ MARTe2 library class).
*
* UDPSClient runs its own background thread (via SingleThreadService).
* On start it connects to a UDPSServer, receives CONFIG + DATA packets, and
* reassembles fragmented updates. If the server goes silent for longer than
* SilenceTimeout, it automatically disconnects and retries.
*
* Unicast mode: sends CONNECT to server UDP port; receives on an ephemeral UDP port.
* Multicast mode: TCP to server port for CONFIG; joins UDP multicast group for DATA.
*
* Threading: UDPSClientListener callbacks are invoked from the internal receive
* thread — the implementation must be thread-safe with respect to the caller.
*/
#include "BasicTCPSocket.h"
#include "BasicUDPSocket.h"
#include "EmbeddedServiceMethodBinderI.h"
#include "ExecutionInfo.h"
#include "HighResolutionTimer.h"
#include "InternetHost.h"
#include "SingleThreadService.h"
#include "StreamString.h"
#include "StructuredDataI.h"
#include "UDPSProtocol.h"
namespace MARTe {
// ---------------------------------------------------------------------------
// Listener interface
// ---------------------------------------------------------------------------
/**
* @brief Callback interface for UDPSClient events.
*
* Implement this interface and pass an instance to UDPSClient::SetListener().
* All callbacks are invoked from the UDPSClient internal thread.
*/
class UDPSClientListener {
public:
virtual ~UDPSClientListener() {}
/** Called when a fully-reassembled CONFIG payload is available. */
virtual void OnUDPSConfig(const uint8 *payload, uint32 payloadSize) {}
/** Called when a fully-reassembled DATA payload is available. */
virtual void OnUDPSData(const uint8 *payload, uint32 payloadSize) {}
/**
* @brief Called for every received DATA datagram (fragment).
* @param counter DATA packet counter from the UDPS header.
* @param nBytes Raw datagram size (header + payload).
* @param complete True iff this fragment completed the DATA reassembly.
*/
virtual void OnUDPSFragment(uint32 counter, uint32 nBytes, bool complete) {}
/** Called when the connection to the server has been established. */
virtual void OnUDPSConnected() {}
/** Called when the connection has been lost or closed. */
virtual void OnUDPSDisconnected() {}
};
// ---------------------------------------------------------------------------
// UDPSClient
// ---------------------------------------------------------------------------
/**
* @brief UDPS receiver client with auto-reconnect and fragment reassembly.
*/
class UDPSClient : public EmbeddedServiceMethodBinderI {
public:
/** Maximum number of concurrent in-flight reassembly slots. */
static const uint32 UDPS_CLIENT_MAX_REASSEMBLY_SLOTS = 4u;
/** Default silence timeout before reconnect (seconds). */
static const uint32 UDPS_CLIENT_DEFAULT_SILENCE_TIMEOUT_S = 5u;
/** Default delay between reconnect attempts (seconds). */
static const uint32 UDPS_CLIENT_DEFAULT_RECONNECT_DELAY_S = 2u;
/** Default maximum payload size (bytes, excluding 17-byte header). */
static const uint32 UDPS_CLIENT_DEFAULT_MAX_PAYLOAD = 1400u;
UDPSClient();
virtual ~UDPSClient();
/**
* @brief Read configuration from a StructuredDataI node.
*
* Expected keys:
* - ServerAddr (char*) Server IPv4 address. Required.
* - Port (uint16) Server UDP port (unicast) or TCP listen port (multicast). Required.
* - MulticastGroup (char*) IPv4 multicast address; presence enables multicast mode.
* - DataPort (uint16) UDP multicast data port (defaults to Port+1).
* - SilenceTimeout (uint32) Seconds of no data before reconnect. Default 5.
* - ReconnectDelay (uint32) Seconds to wait between reconnect attempts. Default 2.
* - MaxPayloadSize (uint32) Max payload bytes per datagram, excluding header. Default 1400.
* - CPUMask (uint32) CPU affinity mask for the receive thread. Default 0xFFFFFFFF.
* - StackSize (uint32) Stack size for the receive thread. Default 65536.
*/
bool Initialise(StructuredDataI &data);
/**
* @brief Register the event listener. Must be called before Start().
*/
void SetListener(UDPSClientListener *listener);
/**
* @brief Start the receive thread.
* @return true on success.
*/
bool Start();
/**
* @brief Stop the receive thread and close all sockets.
* @return true on success.
*/
bool Stop();
/**
* @brief Internal thread entry point (EmbeddedServiceMethodBinderI).
* @details Do NOT call directly.
*/
virtual ErrorManagement::ErrorType Execute(ExecutionInfo &info);
private:
// -------------------------------------------------------------------------
// Fragment reassembly slot
// -------------------------------------------------------------------------
struct UDPSReassemblySlot {
uint32 counter; ///< Packet counter this slot belongs to
uint8 type; ///< UDPS_TYPE_DATA or UDPS_TYPE_CONFIG
uint16 totalFragments; ///< Expected fragment count
uint16 receivedFragments; ///< How many we have so far
uint8 recvMask[32]; ///< Bitmask: bit f set iff fragment f received
uint8 payload[65535]; ///< Assembled payload buffer
uint64 firstSeenTicks; ///< For GC (2 s stale detection)
bool active; ///< Slot in use
uint32 chunkSize; ///< Payload bytes per fragment (from first fragment)
};
// -------------------------------------------------------------------------
// Connection state machine helpers
// -------------------------------------------------------------------------
bool Connect();
void Disconnect();
bool ReceiveAndProcess();
bool ConnectUnicast();
bool ConnectMulticast();
void ProcessDatagram(const uint8 *buf, uint32 size);
/** Read one full UDPS frame (header + payload) from the TCP control socket. */
bool ReceiveTCPFrame();
/** Read exactly n bytes from the TCP control socket. */
bool ReadExactTCP(uint8 *dst, uint32 n);
/** @return true iff this fragment completed the reassembly (payload delivered). */
bool PlaceFragment(const UDPSPacketHeader *hdr, const uint8 *payload, uint32 payloadBytes);
void GcReassemblySlots();
void DeliverAssembled(UDPSReassemblySlot &slot);
// -------------------------------------------------------------------------
// Configuration
// -------------------------------------------------------------------------
StreamString serverAddr;
uint16 serverPort;
StreamString multicastGroup;
uint16 dataPort;
bool useMulticast;
uint64 silenceTimeoutTicks;
uint64 reconnectDelayTicks;
uint32 maxPayloadSize;
uint32 cpuMask;
uint32 stackSize;
// -------------------------------------------------------------------------
// Runtime state
// -------------------------------------------------------------------------
UDPSClientListener *listener;
SingleThreadService threadService;
bool connected;
uint64 lastDataTicks; ///< Ticks at last received DATA/CONFIG
uint64 disconnectTick; ///< Ticks when we disconnected (for delay)
// Unicast
BasicUDPSocket recvSocket; ///< Bound to ephemeral port; receives DATA
uint16 localPort; ///< Ephemeral port we're listening on
// Multicast
BasicTCPSocket tcpSocket; ///< TCP connection to server (for CONNECT + CONFIG)
BasicUDPSocket mcastSocket; ///< Joined multicast group
// Reassembly
UDPSReassemblySlot reassemblySlots[UDPS_CLIENT_MAX_REASSEMBLY_SLOTS];
uint64 lastGcTicks; ///< Ticks at last GC run
// Receive scratch buffer
uint8 recvBuf[65535u + UDPS_HEADER_SIZE];
};
} // namespace MARTe
#endif // UDPS_CLIENT_H_
Source/Components/Interfaces/UDPStream/UDPSServer.cpp
+893
View File
@@ -0,0 +1,893 @@
/**
* @file UDPSServer.cpp
* @brief Implementation of UDPSServer — multi-client UDPS session management.
*/
#include "UDPSServer.h"
#include "AdvancedErrorManagement.h"
#include "HighResolutionTimer.h"
#include "MemoryOperationsHelper.h"
#include "StreamString.h"
#include <sys/select.h>
#include <poll.h>
namespace MARTe {
// ---------------------------------------------------------------------------
// Constructor / Destructor
// ---------------------------------------------------------------------------
UDPSServer::UDPSServer()
: port(0u),
maxPayloadSize(UDPS_SERVER_DEFAULT_MAX_PAYLOAD),
dataPort(0u),
useMulticast(false),
clientTimeoutTicks(0u),
numUnicastClients(0u),
numTCPClients(0u),
cachedConfig(NULL_PTR(uint8 *)),
cachedConfigSize(0u),
sendBuf(NULL_PTR(uint8 *)),
sendBufCapacity(0u),
configCounter(0u),
started(false) {
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
unicastClients[i].ipAddr[0] = '\0';
unicastClients[i].clientPort = 0u;
unicastClients[i].lastSeenTicks = 0u;
unicastClients[i].active = false;
unicastClients[i].isStatic = false;
}
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
tcpClients[i] = NULL_PTR(BasicTCPSocket *);
}
}
UDPSServer::~UDPSServer() {
(void) Stop();
}
// ---------------------------------------------------------------------------
// Initialise
// ---------------------------------------------------------------------------
bool UDPSServer::Initialise(StructuredDataI &data) {
uint32 portU32 = 0u;
if (data.Read("Port", portU32)) {
port = static_cast<uint16>(portU32);
}
else {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"UDPSServer: Port not specified.");
return false;
}
/* port == 0 is valid: unicast push-only mode (no serverSocket bind,
* no CONNECT/DISCONNECT/ACK reception). Clients added via AddStaticClient(). */
StreamString mcGroup;
if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
multicastGroup = mcGroup;
useMulticast = true;
}
if (useMulticast) {
uint32 dpU32 = static_cast<uint32>(port) + 1u;
(void) data.Read("DataPort", dpU32);
dataPort = static_cast<uint16>(dpU32);
if (dataPort == port) {
REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
"UDPSServer: DataPort (%u) must differ from Port (%u).",
static_cast<uint32>(dataPort), static_cast<uint32>(port));
return false;
}
}
uint32 mps = UDPS_SERVER_DEFAULT_MAX_PAYLOAD;
(void) data.Read("MaxPayloadSize", mps);
maxPayloadSize = mps;
uint32 timeoutSecs = UDPS_SERVER_DEFAULT_CLIENT_TIMEOUT_S;
(void) data.Read("ClientTimeout", timeoutSecs);
clientTimeoutTicks = (timeoutSecs > 0u)
? (static_cast<uint64>(timeoutSecs) * HighResolutionTimer::Frequency())
: 0u;
return true;
}
// ---------------------------------------------------------------------------
// Start / Stop
// ---------------------------------------------------------------------------
bool UDPSServer::Start() {
if (started) {
return true;
}
sendBufCapacity = UDPS_HEADER_SIZE + maxPayloadSize;
sendBuf = new uint8[sendBufCapacity];
if (sendBuf == NULL_PTR(uint8 *)) {
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
"UDPSServer: Could not allocate send buffer.");
return false;
}
bool ok = true;
if (useMulticast) {
// TCP listener for CONNECT + CONFIG
ok = tcpListener.Open();
if (ok) {
ok = tcpListener.Listen(port, 5);
}
if (ok) {
tcpListener.SetBlocking(false);
}
// UDP data socket connected to multicast group
if (ok) {
ok = dataSocket.Open();
}
if (ok) {
ok = dataSocket.Connect(multicastGroup.Buffer(), dataPort);
}
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
"UDPSServer: Failed to open multicast sockets on port %u.",
static_cast<uint32>(port));
}
}
else {
// Unicast send socket (unconnected; SetDestination per Write)
ok = uniSendSocket.Open();
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
"UDPSServer: Failed to open unicast send socket.");
}
// Receive socket: only bind if port != 0 (skip for push-only mode)
if (ok && (port != 0u)) {
ok = serverSocket.Open();
if (ok) {
ok = serverSocket.Listen(port); // UDP "listen" = bind
}
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
"UDPSServer: Failed to bind receive socket on port %u.",
static_cast<uint32>(port));
}
}
}
started = ok;
return ok;
}
bool UDPSServer::Stop() {
if (!started) {
return true;
}
// Evict all unicast clients
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
if (unicastClients[i].active) {
EvictUnicastClient(i);
}
}
numUnicastClients = 0u;
// Close all TCP clients
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
if (tcpClients[i] != NULL_PTR(BasicTCPSocket *)) {
EvictTCPClient(i);
}
}
numTCPClients = 0u;
// Close sockets
if (serverSocket.IsValid()) {
(void) serverSocket.Close();
}
if (uniSendSocket.IsValid()) {
(void) uniSendSocket.Close();
}
if (tcpListener.IsValid()) {
(void) tcpListener.Close();
}
if (dataSocket.IsValid()) {
(void) dataSocket.Close();
}
// Free heap buffers
if (cachedConfig != NULL_PTR(uint8 *)) {
delete[] cachedConfig;
cachedConfig = NULL_PTR(uint8 *);
cachedConfigSize = 0u;
}
if (sendBuf != NULL_PTR(uint8 *)) {
delete[] sendBuf;
sendBuf = NULL_PTR(uint8 *);
sendBufCapacity = 0u;
}
started = false;
return true;
}
// ---------------------------------------------------------------------------
// ServiceClients
// ---------------------------------------------------------------------------
void UDPSServer::ServiceClients() {
if (!started) {
return;
}
if (useMulticast) {
// Poll TCP listener for new connections (non-blocking).
// Guard with poll() first so WaitConnection is never called on an empty
// queue — avoids the MARTe2 "Failed accept in unblocking mode" log spam.
if (tcpListener.IsValid()) {
struct pollfd pfd;
pfd.fd = static_cast<int>(tcpListener.GetReadHandle());
pfd.events = POLLIN;
pfd.revents = 0;
bool pending = (::poll(&pfd, 1u, 0) > 0) &&
((pfd.revents & POLLIN) != 0);
BasicTCPSocket *newConn = pending
? tcpListener.WaitConnection(0u)
: NULL_PTR(BasicTCPSocket *);
if (newConn != NULL_PTR(BasicTCPSocket *)) {
// Find free TCP client slot
uint32 freeSlot = UDPS_SERVER_MAX_TCP_CLIENTS;
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) {
freeSlot = i;
break;
}
}
if (freeSlot < UDPS_SERVER_MAX_TCP_CLIENTS) {
HandleMulticastTCPConnect(newConn, freeSlot);
}
else {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSServer: TCP client table full, rejecting new connection.");
(void) newConn->Close();
delete newConn;
}
}
}
// Poll existing TCP clients for DISCONNECT
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) {
continue;
}
// Non-blocking peek
int fd = tcpClients[i]->GetReadHandle();
fd_set rset;
FD_ZERO(&rset);
FD_SET(fd, &rset);
struct timeval tv;
tv.tv_sec = 0; tv.tv_usec = 0;
int nready = select(fd + 1, &rset, NULL, NULL, &tv);
if (nready > 0) {
uint8 pktBuf[UDPS_HEADER_SIZE];
uint32 recvSize = UDPS_HEADER_SIZE;
bool recvOk = tcpClients[i]->Read(reinterpret_cast<char8 *>(pktBuf), recvSize);
if (!recvOk || (recvSize == 0u)) {
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSServer: TCP client disconnected (slot %u).", i);
EvictTCPClient(i);
continue;
}
if (recvSize >= UDPS_HEADER_SIZE) {
const UDPSPacketHeader *hdr =
reinterpret_cast<const UDPSPacketHeader *>(pktBuf);
if ((hdr->magic == UDPS_MAGIC) &&
(hdr->type == UDPS_TYPE_DISCONNECT)) {
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSServer: TCP client sent DISCONNECT (slot %u).", i);
EvictTCPClient(i);
}
}
}
}
}
else {
// Unicast: poll serverSocket for CONNECT / DISCONNECT / ACK
if (!serverSocket.IsValid()) {
return;
}
int fd = serverSocket.GetReadHandle();
fd_set rset;
FD_ZERO(&rset);
FD_SET(fd, &rset);
struct timeval tv = {0, 0};
int nready = select(fd + 1, &rset, NULL, NULL, &tv);
while (nready > 0) {
uint8 pktBuf[UDPS_HEADER_SIZE + 4u];
uint32 recvSize = static_cast<uint32>(sizeof(pktBuf));
bool recvOk = serverSocket.Read(reinterpret_cast<char8 *>(pktBuf), recvSize);
if (!recvOk || (recvSize < UDPS_HEADER_SIZE)) {
break;
}
const UDPSPacketHeader *hdr =
reinterpret_cast<const UDPSPacketHeader *>(pktBuf);
if (hdr->magic != UDPS_MAGIC) {
break;
}
InternetHost src = serverSocket.GetSource();
if (hdr->type == UDPS_TYPE_CONNECT) {
HandleUnicastConnect(src);
}
else if (hdr->type == UDPS_TYPE_DISCONNECT) {
HandleUnicastDisconnect(src);
}
else if (hdr->type == UDPS_TYPE_ACK) {
HandleUnicastAck(src);
}
// Check if more data is ready
FD_ZERO(&rset);
FD_SET(fd, &rset);
tv.tv_sec = 0;
tv.tv_usec = 0;
nready = select(fd + 1, &rset, NULL, NULL, &tv);
}
// Evict stale clients (if timeout is configured)
if (clientTimeoutTicks > 0u) {
EvictStaleUnicastClients();
}
}
}
// ---------------------------------------------------------------------------
// SendConfig
// ---------------------------------------------------------------------------
bool UDPSServer::SendConfig(const uint8 *payload, uint32 payloadSize) {
if (!started) {
return false;
}
// Cache for future CONNECT clients
(void) CacheConfig(payload, payloadSize);
configCounter++;
bool ok = true;
if (useMulticast) {
// Send CONFIG over TCP to each connected client
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) {
continue;
}
bool sent = SendFragmentedTCP(*tcpClients[i], UDPS_TYPE_CONFIG,
configCounter, payload, payloadSize);
if (!sent) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSServer: CONFIG send failed to TCP client %u, evicting.", i);
EvictTCPClient(i);
ok = false;
}
}
}
else {
// Send CONFIG to each unicast client
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
if (!unicastClients[i].active) {
continue;
}
InternetHost dest(unicastClients[i].clientPort, unicastClients[i].ipAddr);
bool sent = SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG,
configCounter, payload, payloadSize);
if (!sent) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSServer: CONFIG send failed to %s:%u.",
unicastClients[i].ipAddr,
static_cast<uint32>(unicastClients[i].clientPort));
ok = false;
}
}
}
return ok;
}
// ---------------------------------------------------------------------------
// SendData
// ---------------------------------------------------------------------------
bool UDPSServer::SendData(uint32 counter, const uint8 *payload, uint32 payloadSize) {
if (!started) {
return false;
}
bool ok = true;
if (useMulticast) {
// Single multicast write (no dest needed — socket already connected)
bool sent = SendFragmentedUDP(dataSocket, NULL_PTR(InternetHost *),
UDPS_TYPE_DATA, counter, payload, payloadSize);
if (!sent) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSServer: DATA send to multicast group failed.");
ok = false;
}
}
else {
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
if (!unicastClients[i].active) {
continue;
}
InternetHost dest(unicastClients[i].clientPort, unicastClients[i].ipAddr);
bool sent = SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_DATA,
counter, payload, payloadSize);
if (!sent) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSServer: DATA send failed to %s:%u.",
unicastClients[i].ipAddr,
static_cast<uint32>(unicastClients[i].clientPort));
ok = false;
}
}
}
return ok;
}
// ---------------------------------------------------------------------------
// AddStaticClient
// ---------------------------------------------------------------------------
bool UDPSServer::AddStaticClient(const char8 *ip, uint16 port_) {
if ((ip == NULL_PTR(const char8 *)) || (ip[0] == '\0')) {
return false;
}
// Check for duplicate
uint32 existing = FindUnicastClient(ip, port_);
if (existing < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
unicastClients[existing].isStatic = true; // ensure it's marked static
return true;
}
// Find free slot
uint32 freeSlot = UDPS_SERVER_MAX_UNICAST_CLIENTS;
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
if (!unicastClients[i].active) {
freeSlot = i;
break;
}
}
if (freeSlot >= UDPS_SERVER_MAX_UNICAST_CLIENTS) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSServer: Unicast client table full, cannot add static client %s:%u.",
ip, static_cast<uint32>(port_));
return false;
}
// Populate slot
uint32 ipLen = 0u;
while ((ipLen < 63u) && (ip[ipLen] != '\0')) {
unicastClients[freeSlot].ipAddr[ipLen] = ip[ipLen];
ipLen++;
}
unicastClients[freeSlot].ipAddr[ipLen] = '\0';
unicastClients[freeSlot].clientPort = port_;
unicastClients[freeSlot].lastSeenTicks = HighResolutionTimer::Counter();
unicastClients[freeSlot].active = true;
unicastClients[freeSlot].isStatic = true;
numUnicastClients++;
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSServer: Static client added: %s:%u.",
ip, static_cast<uint32>(port_));
return true;
}
// ---------------------------------------------------------------------------
// Accessors
// ---------------------------------------------------------------------------
uint32 UDPSServer::GetClientCount() const {
uint32 count = 0u;
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
if (unicastClients[i].active) {
count++;
}
}
for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
if (tcpClients[i] != NULL_PTR(BasicTCPSocket *)) {
count++;
}
}
return count;
}
bool UDPSServer::HasClients() const {
return (GetClientCount() > 0u);
}
bool UDPSServer::IsMulticast() const {
return useMulticast;
}
uint16 UDPSServer::GetPort() const {
return port;
}
uint32 UDPSServer::GetMaxPayloadSize() const {
return maxPayloadSize;
}
// ---------------------------------------------------------------------------
// Private: SendFragmentedUDP
// ---------------------------------------------------------------------------
bool UDPSServer::SendFragmentedUDP(BasicUDPSocket &sock,
InternetHost *dest,
uint8 type,
uint32 counter,
const uint8 *payload,
uint32 payloadSize) {
uint32 maxChunk = maxPayloadSize; // payload bytes per fragment (excl. header)
uint32 totalFrags = (payloadSize == 0u) ? 1u :
((payloadSize + maxChunk - 1u) / maxChunk);
bool ok = true;
uint32 offs = 0u;
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
uint32 chunkSize = payloadSize - offs;
if (chunkSize > maxChunk) {
chunkSize = maxChunk;
}
UDPSBuildHeader(sendBuf, type, counter,
static_cast<uint16>(f),
static_cast<uint16>(totalFrags),
chunkSize);
if (chunkSize > 0u) {
(void) MemoryOperationsHelper::Copy(sendBuf + UDPS_HEADER_SIZE,
payload + offs,
chunkSize);
}
uint32 sendSize = UDPS_HEADER_SIZE + chunkSize;
if (dest != NULL_PTR(InternetHost *)) {
(void) sock.SetDestination(*dest);
}
ok = sock.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSServer: UDP fragment %u/%u write failed.",
f + 1u, totalFrags);
}
offs += chunkSize;
}
return ok;
}
// ---------------------------------------------------------------------------
// Private: SendFragmentedTCP
// ---------------------------------------------------------------------------
bool UDPSServer::SendFragmentedTCP(BasicTCPSocket &sock,
uint8 type,
uint32 counter,
const uint8 *payload,
uint32 payloadSize) {
uint32 maxChunk = maxPayloadSize;
uint32 totalFrags = (payloadSize == 0u) ? 1u :
((payloadSize + maxChunk - 1u) / maxChunk);
bool ok = true;
uint32 offs = 0u;
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
uint32 chunkSize = payloadSize - offs;
if (chunkSize > maxChunk) {
chunkSize = maxChunk;
}
UDPSBuildHeader(sendBuf, type, counter,
static_cast<uint16>(f),
static_cast<uint16>(totalFrags),
chunkSize);
if (chunkSize > 0u) {
(void) MemoryOperationsHelper::Copy(sendBuf + UDPS_HEADER_SIZE,
payload + offs,
chunkSize);
}
uint32 sendSize = UDPS_HEADER_SIZE + chunkSize;
ok = sock.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
if (!ok) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSServer: TCP fragment %u/%u write failed.",
f + 1u, totalFrags);
}
offs += chunkSize;
}
return ok;
}
// ---------------------------------------------------------------------------
// Private: EvictStaleUnicastClients
// ---------------------------------------------------------------------------
void UDPSServer::EvictStaleUnicastClients() {
uint64 now = HighResolutionTimer::Counter();
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
if (!unicastClients[i].active || unicastClients[i].isStatic) {
continue;
}
uint64 elapsed = now - unicastClients[i].lastSeenTicks;
if (elapsed >= clientTimeoutTicks) {
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSServer: Evicting stale client %s:%u.",
unicastClients[i].ipAddr,
static_cast<uint32>(unicastClients[i].clientPort));
EvictUnicastClient(i);
}
}
}
// ---------------------------------------------------------------------------
// Private: HandleUnicastConnect
// ---------------------------------------------------------------------------
void UDPSServer::HandleUnicastConnect(const InternetHost &src) {
StreamString srcAddrStr = src.GetAddress();
const char8 *srcAddr = srcAddrStr.Buffer();
uint16 srcPort = src.GetPort();
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSServer: CONNECT from %s:%u.",
srcAddr, static_cast<uint32>(srcPort));
// Check if this client is already known
uint32 existing = FindUnicastClient(srcAddr, srcPort);
if (existing < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
// Refresh last-seen and resend CONFIG
unicastClients[existing].lastSeenTicks = HighResolutionTimer::Counter();
if (cachedConfig != NULL_PTR(uint8 *)) {
InternetHost dest(srcPort, srcAddr);
configCounter++;
(void) SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG,
configCounter, cachedConfig, cachedConfigSize);
}
return;
}
// New client — find a free slot (or evict oldest non-static)
uint32 slot = UDPS_SERVER_MAX_UNICAST_CLIENTS;
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
if (!unicastClients[i].active) {
slot = i;
break;
}
}
if (slot >= UDPS_SERVER_MAX_UNICAST_CLIENTS) {
slot = FindOldestUnicastClient();
if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSServer: Client table full — evicting %s:%u.",
unicastClients[slot].ipAddr,
static_cast<uint32>(unicastClients[slot].clientPort));
EvictUnicastClient(slot);
}
else {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSServer: All slots occupied by static clients; rejecting %s:%u.",
srcAddr, static_cast<uint32>(srcPort));
return;
}
}
// Fill slot
uint32 addrLen = 0u;
while ((addrLen < 63u) && (srcAddr[addrLen] != '\0')) {
unicastClients[slot].ipAddr[addrLen] = srcAddr[addrLen];
addrLen++;
}
unicastClients[slot].ipAddr[addrLen] = '\0';
unicastClients[slot].clientPort = srcPort;
unicastClients[slot].lastSeenTicks = HighResolutionTimer::Counter();
unicastClients[slot].active = true;
unicastClients[slot].isStatic = false;
numUnicastClients++;
// Send cached CONFIG if available
if (cachedConfig != NULL_PTR(uint8 *)) {
InternetHost dest(srcPort, srcAddr);
configCounter++;
(void) SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG,
configCounter, cachedConfig, cachedConfigSize);
}
}
// ---------------------------------------------------------------------------
// Private: HandleUnicastDisconnect
// ---------------------------------------------------------------------------
void UDPSServer::HandleUnicastDisconnect(const InternetHost &src) {
StreamString srcAddrStr = src.GetAddress();
const char8 *srcAddr = srcAddrStr.Buffer();
uint16 srcPort = src.GetPort();
uint32 slot = FindUnicastClient(srcAddr, srcPort);
if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSServer: DISCONNECT from %s:%u.",
srcAddr, static_cast<uint32>(srcPort));
EvictUnicastClient(slot);
}
}
// ---------------------------------------------------------------------------
// Private: HandleUnicastAck
// ---------------------------------------------------------------------------
void UDPSServer::HandleUnicastAck(const InternetHost &src) {
StreamString srcAddrStr = src.GetAddress();
const char8 *srcAddr = srcAddrStr.Buffer();
uint16 srcPort = src.GetPort();
uint32 slot = FindUnicastClient(srcAddr, srcPort);
if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
unicastClients[slot].lastSeenTicks = HighResolutionTimer::Counter();
}
}
// ---------------------------------------------------------------------------
// Private: FindUnicastClient
// ---------------------------------------------------------------------------
uint32 UDPSServer::FindUnicastClient(const char8 *ip, uint16 port_) const {
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
if (!unicastClients[i].active) {
continue;
}
if (unicastClients[i].clientPort != port_) {
continue;
}
// String compare
bool match = true;
uint32 k = 0u;
while ((k < 63u) && match) {
if (unicastClients[i].ipAddr[k] != ip[k]) {
match = false;
}
if (ip[k] == '\0') {
break;
}
k++;
}
if (match) {
return i;
}
}
return UDPS_SERVER_MAX_UNICAST_CLIENTS;
}
// ---------------------------------------------------------------------------
// Private: FindOldestUnicastClient
// ---------------------------------------------------------------------------
uint32 UDPSServer::FindOldestUnicastClient() const {
uint32 oldest = UDPS_SERVER_MAX_UNICAST_CLIENTS;
uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL;
for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
if (!unicastClients[i].active || unicastClients[i].isStatic) {
continue;
}
if (unicastClients[i].lastSeenTicks < oldestTick) {
oldestTick = unicastClients[i].lastSeenTicks;
oldest = i;
}
}
return oldest;
}
// ---------------------------------------------------------------------------
// Private: EvictUnicastClient
// ---------------------------------------------------------------------------
void UDPSServer::EvictUnicastClient(uint32 idx) {
if (idx >= UDPS_SERVER_MAX_UNICAST_CLIENTS) {
return;
}
unicastClients[idx].active = false;
if (numUnicastClients > 0u) {
numUnicastClients--;
}
}
// ---------------------------------------------------------------------------
// Private: HandleMulticastTCPConnect
// ---------------------------------------------------------------------------
void UDPSServer::HandleMulticastTCPConnect(BasicTCPSocket *newClient, uint32 idx) {
if (idx >= UDPS_SERVER_MAX_TCP_CLIENTS) {
return;
}
tcpClients[idx] = newClient;
numTCPClients++;
REPORT_ERROR_STATIC(ErrorManagement::Information,
"UDPSServer: Multicast TCP client connected (slot %u).", idx);
// Send cached CONFIG over TCP
if (cachedConfig != NULL_PTR(uint8 *)) {
configCounter++;
bool sent = SendFragmentedTCP(*newClient, UDPS_TYPE_CONFIG,
configCounter, cachedConfig, cachedConfigSize);
if (!sent) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSServer: Failed to send CONFIG to new TCP client (slot %u).", idx);
EvictTCPClient(idx);
}
}
}
// ---------------------------------------------------------------------------
// Private: EvictTCPClient
// ---------------------------------------------------------------------------
void UDPSServer::EvictTCPClient(uint32 idx) {
if (idx >= UDPS_SERVER_MAX_TCP_CLIENTS) {
return;
}
if (tcpClients[idx] != NULL_PTR(BasicTCPSocket *)) {
(void) tcpClients[idx]->Close();
delete tcpClients[idx];
tcpClients[idx] = NULL_PTR(BasicTCPSocket *);
if (numTCPClients > 0u) {
numTCPClients--;
}
}
}
// ---------------------------------------------------------------------------
// Private: CacheConfig
// ---------------------------------------------------------------------------
bool UDPSServer::CacheConfig(const uint8 *payload, uint32 payloadSize) {
if (cachedConfig != NULL_PTR(uint8 *)) {
delete[] cachedConfig;
cachedConfig = NULL_PTR(uint8 *);
cachedConfigSize = 0u;
}
if ((payload == NULL_PTR(const uint8 *)) || (payloadSize == 0u)) {
return true;
}
cachedConfig = new uint8[payloadSize];
if (cachedConfig == NULL_PTR(uint8 *)) {
REPORT_ERROR_STATIC(ErrorManagement::FatalError,
"UDPSServer: Failed to allocate cached CONFIG buffer.");
return false;
}
(void) MemoryOperationsHelper::Copy(cachedConfig, payload, payloadSize);
cachedConfigSize = payloadSize;
return true;
}
} // namespace MARTe
Source/Components/Interfaces/UDPStream/UDPSServer.h
+274
View File
@@ -0,0 +1,274 @@
#ifndef UDPS_SERVER_H_
#define UDPS_SERVER_H_
/**
* @file UDPSServer.h
* @brief Multi-client UDPS session-management and fragmented-send helper.
*
* UDPSServer is a plain C++ helper (not a MARTe2 Object) that encapsulates:
* - Unicast mode: up to UDPS_SERVER_MAX_UNICAST_CLIENTS simultaneous UDP clients
* that self-register via CONNECT datagrams.
* - Multicast mode: TCP listener for CONNECT + CONFIG delivery; UDP multicast
* socket for DATA datagrams.
* - Static clients: pre-configured destinations that never need to send CONNECT
* (backward-compat for push-to-fixed-IP use cases).
*
* Threading model: UDPSServer is NOT thread-safe. All public methods must be
* called from the same thread (the owner's Execute() thread).
*/
#include "BasicTCPSocket.h"
#include "BasicUDPSocket.h"
#include "InternetHost.h"
#include "StreamString.h"
#include "StructuredDataI.h"
#include "UDPSProtocol.h"
namespace MARTe {
/**
* @brief Multi-client / multicast UDPS server helper.
*
* Usage pattern:
* @code
* UDPSServer server;
* server.Initialise(data); // in component Initialise()
* server.Start(); // in PrepareNextState() / Start()
* // inside Execute() loop:
* server.ServiceClients(); // poll CONNECT / DISCONNECT / ACK
* if (server.HasClients()) {
* server.SendConfig(cfgBuf, cfgSize); // when config changes
* server.SendData(counter, dataBuf, dataSize);
* }
* server.Stop(); // in StopCurrentStateExecution()
* @endcode
*/
class UDPSServer {
public:
/** Maximum number of simultaneous unicast clients. */
static const uint32 UDPS_SERVER_MAX_UNICAST_CLIENTS = 16u;
/** Maximum number of simultaneous multicast TCP control connections. */
static const uint32 UDPS_SERVER_MAX_TCP_CLIENTS = 8u;
/** Default stale-client eviction timeout (seconds). */
static const uint32 UDPS_SERVER_DEFAULT_CLIENT_TIMEOUT_S = 30u;
/** Default maximum UDP payload size (bytes, EXCLUDING the 17-byte header). */
static const uint32 UDPS_SERVER_DEFAULT_MAX_PAYLOAD = 1400u;
UDPSServer();
~UDPSServer();
/**
* @brief Read configuration from a StructuredDataI node.
*
* Expected keys (all optional except Port for unicast):
* - Port (uint16) UDP port to bind (unicast) or TCP listen port (multicast).
* - MulticastGroup (char*) IPv4 multicast address; presence enables multicast mode.
* - DataPort (uint16) UDP port used for DATA datagrams in multicast mode
* (defaults to Port+1 if omitted).
* - MaxPayloadSize (uint32) Max payload bytes per datagram, excluding header.
* Defaults to UDPS_SERVER_DEFAULT_MAX_PAYLOAD.
* - ClientTimeout (uint32) Seconds before a silent unicast client is evicted.
* Defaults to UDPS_SERVER_DEFAULT_CLIENT_TIMEOUT_S.
* Set to 0 to disable timeout-based eviction.
*/
bool Initialise(StructuredDataI &data);
/**
* @brief Open sockets and begin accepting clients.
* @return true on success.
*/
bool Start();
/**
* @brief Close all sockets and evict all clients. Frees cached CONFIG.
*/
bool Stop();
/**
* @brief Non-blocking poll for incoming CONNECT / DISCONNECT / ACK messages.
*
* Must be called regularly from the owner's Execute() main loop.
* In multicast mode also polls the TCP listener for new connections.
*/
void ServiceClients();
/**
* @brief Send a CONFIG payload to all active clients.
*
* Also stores a copy as the cached CONFIG, which is automatically sent to
* any new CONNECT client.
*
* @param payload Pointer to fully-assembled CONFIG payload (caller-owned).
* @param payloadSize Byte count of payload.
* @return true if at least one send succeeded (or no clients are connected).
*/
bool SendConfig(const uint8 *payload, uint32 payloadSize);
/**
* @brief Send a DATA payload to all active clients.
*
* @param counter Per-update sequence counter (same for all fragments).
* @param payload Pointer to fully-assembled DATA payload (caller-owned).
* @param payloadSize Byte count of payload.
* @return true if all sends succeeded.
*/
bool SendData(uint32 counter, const uint8 *payload, uint32 payloadSize);
/**
* @brief Add a static unicast destination that never needs to send CONNECT.
*
* The entry is never evicted by the timeout mechanism. Duplicate
* (ip, port) pairs are silently ignored.
*
* @param ip Destination IPv4 address string.
* @param port Destination UDP port.
* @return true if the client was added (or was already present).
*/
bool AddStaticClient(const char8 *ip, uint16 port);
/** @return Number of currently active clients (unicast + TCP). */
uint32 GetClientCount() const;
/** @return true if at least one client is active. */
bool HasClients() const;
/** @return true if server is in multicast mode. */
bool IsMulticast() const;
/** @return Configured port number. */
uint16 GetPort() const;
/** @return Configured maximum payload size (excluding header). */
uint32 GetMaxPayloadSize() const;
private:
// -------------------------------------------------------------------------
// Per-client state (unicast)
// -------------------------------------------------------------------------
struct UDPSClientEntry {
char8 ipAddr[64]; ///< Client IPv4 address string
uint16 clientPort; ///< Client source port
uint64 lastSeenTicks; ///< HighResolutionTimer ticks at last contact
bool active; ///< Slot is in use
bool isStatic; ///< Never evicted by timeout; no CONNECT required
};
// -------------------------------------------------------------------------
// Private helpers
// -------------------------------------------------------------------------
/**
* @brief Send a fragmented UDPS packet via a UDP socket.
*
* @param sock Socket to write through.
* @param dest If non-NULL, SetDestination is called before each write
* (unicast unconnected socket). If NULL, assumes the socket
* is already connected (multicast dataSocket).
* @param type UDPS_TYPE_DATA or UDPS_TYPE_CONFIG.
* @param counter Sequence counter.
* @param payload Payload bytes.
* @param payloadSize Payload byte count.
* @return true if all fragments were written without error.
*/
bool SendFragmentedUDP(BasicUDPSocket &sock,
InternetHost *dest,
uint8 type,
uint32 counter,
const uint8 *payload,
uint32 payloadSize);
/**
* @brief Send a fragmented UDPS packet via a TCP socket.
*
* Used in multicast mode to push CONFIG to each TCP control client.
*/
bool SendFragmentedTCP(BasicTCPSocket &sock,
uint8 type,
uint32 counter,
const uint8 *payload,
uint32 payloadSize);
/** Evict unicast clients that have been silent longer than clientTimeoutTicks. */
void EvictStaleUnicastClients();
/** Handle a CONNECT datagram received on serverSocket. */
void HandleUnicastConnect(const InternetHost &src);
/** Handle a DISCONNECT datagram received on serverSocket. */
void HandleUnicastDisconnect(const InternetHost &src);
/** Handle an ACK datagram received on serverSocket. */
void HandleUnicastAck(const InternetHost &src);
/**
* @brief Find a unicast client slot by IP + port.
* @return Slot index, or UDPS_SERVER_MAX_UNICAST_CLIENTS if not found.
*/
uint32 FindUnicastClient(const char8 *ip, uint16 port) const;
/**
* @brief Find the oldest (lowest lastSeenTicks) non-static unicast client.
* @return Slot index, or UDPS_SERVER_MAX_UNICAST_CLIENTS if none.
*/
uint32 FindOldestUnicastClient() const;
/** Close slot @p idx and mark it inactive. */
void EvictUnicastClient(uint32 idx);
/** Accept a new multicast TCP connection and send cached CONFIG. */
void HandleMulticastTCPConnect(BasicTCPSocket *newClient, uint32 idx);
/** Close TCP client slot @p idx. */
void EvictTCPClient(uint32 idx);
/** Cache a copy of a CONFIG payload for future CONNECT clients. */
bool CacheConfig(const uint8 *payload, uint32 payloadSize);
// -------------------------------------------------------------------------
// Configuration
// -------------------------------------------------------------------------
uint16 port;
uint32 maxPayloadSize;
StreamString multicastGroup;
uint16 dataPort;
bool useMulticast;
uint64 clientTimeoutTicks; ///< 0 = disabled
// -------------------------------------------------------------------------
// Unicast state
// -------------------------------------------------------------------------
UDPSClientEntry unicastClients[UDPS_SERVER_MAX_UNICAST_CLIENTS];
uint32 numUnicastClients;
BasicUDPSocket serverSocket; ///< Bound to port; receives CONNECT/DISCONNECT/ACK
BasicUDPSocket uniSendSocket; ///< Unconnected; SetDestination before each Write
// -------------------------------------------------------------------------
// Multicast state
// -------------------------------------------------------------------------
BasicTCPSocket tcpListener;
BasicTCPSocket *tcpClients[UDPS_SERVER_MAX_TCP_CLIENTS];
uint32 numTCPClients;
BasicUDPSocket dataSocket; ///< Connected to multicastGroup:dataPort
// -------------------------------------------------------------------------
// Shared state
// -------------------------------------------------------------------------
uint8 *cachedConfig; ///< Last SendConfig() payload (heap-allocated)
uint32 cachedConfigSize;
uint8 *sendBuf; ///< Pre-allocated fragment TX buffer
uint32 sendBufCapacity; ///< UDPS_HEADER_SIZE + maxPayloadSize
uint32 configCounter; ///< Sequence counter for CONFIG packets
bool started;
};
} // namespace MARTe
#endif // UDPS_SERVER_H_