694 lines
24 KiB
C++
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
|