Implemented new C++ logic
This commit is contained in:
Martino Ferrari
@@ -0,0 +1 @@
|
||||
include Makefile.inc
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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_
|
||||
@@ -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
|
||||
@@ -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_
|
||||
Reference in New Issue
Block a user