Martino Ferrari
894 lines
31 KiB
C++
894 lines
31 KiB
C++
/**
|
|
* @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
|