Implemented new C++ logic
This commit is contained in:
Martino Ferrari
@@ -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
|
||||
Reference in New Issue
Block a user