Files
MARTe-Integrated-Components/Source/Components/Interfaces/UDPStream/UDPSClient.cpp
T
2026-06-12 15:25:13 +02:00

694 lines
24 KiB
C++

/**
* @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