Implemented new C++ logic
This commit is contained in:
@@ -33,6 +33,7 @@
|
||||
/* Project header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
#include "AdvancedErrorManagement.h"
|
||||
#include "ConfigurationDatabase.h"
|
||||
#include "EmbeddedThreadI.h"
|
||||
#include "GlobalObjectsDatabase.h"
|
||||
#include "HighResolutionTimer.h"
|
||||
@@ -61,7 +62,7 @@ static const int32 UDPS_TCP_MAX_CONNECTIONS = 4;
|
||||
static const uint32 UDPS_DEFAULT_MAX_PAYLOAD = 1400u;
|
||||
|
||||
/** Minimum MaxPayloadSize: header + at least 1 byte of payload. */
|
||||
static const uint32 UDPS_MIN_PAYLOAD = static_cast<uint32>(sizeof(UDPSPacketHeader)) + 1u;
|
||||
static const uint32 UDPS_MIN_PAYLOAD = UDPS_HEADER_SIZE + 1u;
|
||||
|
||||
/** Server socket receive timeout in milliseconds.
|
||||
* Set to 0 for a pure non-blocking poll so the send loop can keep pace with
|
||||
@@ -72,30 +73,6 @@ static const uint32 UDPS_RECV_TIMEOUT_MS = 0u;
|
||||
/** EventSem wait timeout in milliseconds for the data loop. */
|
||||
static const uint32 UDPS_DATA_WAIT_MS = 10u;
|
||||
|
||||
/** Sentinel: no time-signal reference; use the packet-level timestamp. */
|
||||
static const uint32 UDPS_NO_TIME_SIGNAL = 0xFFFFFFFFu;
|
||||
|
||||
/** Max signal name length in CONFIG packet (including null terminator). */
|
||||
static const uint32 UDPS_MAX_SIGNAL_NAME = 64u;
|
||||
|
||||
/** Max unit string length in CONFIG packet (including null terminator). */
|
||||
static const uint32 UDPS_MAX_UNIT_LEN = 32u;
|
||||
|
||||
/** Size in bytes of one signal descriptor in the CONFIG payload. */
|
||||
static const uint32 UDPS_SIGNAL_DESC_SIZE =
|
||||
UDPS_MAX_SIGNAL_NAME /* name */
|
||||
+ 1u /* typeCode */
|
||||
+ 1u /* quantType */
|
||||
+ 1u /* numDimensions*/
|
||||
+ 4u /* numRows */
|
||||
+ 4u /* numCols */
|
||||
+ 8u /* rangeMin */
|
||||
+ 8u /* rangeMax */
|
||||
+ 1u /* timeMode */
|
||||
+ 8u /* samplingRate */
|
||||
+ 4u /* timeSignalIdx*/
|
||||
+ UDPS_MAX_UNIT_LEN; /* unit */
|
||||
|
||||
/** Bytes prepended to each DATA payload for the HRT packet timestamp. */
|
||||
static const uint32 UDPS_TIMESTAMP_BYTES = 8u;
|
||||
|
||||
@@ -114,9 +91,6 @@ UDPStreamer::UDPStreamer() :
|
||||
publishMode = UDPStreamerPublishStrict;
|
||||
minRefreshRate = 0.0;
|
||||
flushPeriodTicks = 0u;
|
||||
dataPort = UDPS_DEFAULT_PORT + UDPS_DEFAULT_DATA_PORT_OFFSET;
|
||||
useMulticast = false;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
numSigs = 0u;
|
||||
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
|
||||
readyBuffer = NULL_PTR(uint8 *);
|
||||
@@ -125,7 +99,6 @@ UDPStreamer::UDPStreamer() :
|
||||
totalSrcBytes = 0u;
|
||||
totalWireBytes = 0u;
|
||||
syncTimestamp = 0u;
|
||||
clientConnected = false;
|
||||
packetCounter = 0u;
|
||||
maxBatchCount = 0u;
|
||||
singleCycleWireBytes = 0u;
|
||||
@@ -162,12 +135,7 @@ UDPStreamer::~UDPStreamer() {
|
||||
}
|
||||
}
|
||||
|
||||
if (serverSocket.IsValid()) {
|
||||
(void) serverSocket.Close();
|
||||
}
|
||||
if (clientSocket.IsValid()) {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
(void) server.Stop();
|
||||
|
||||
HeapI *heapAccum = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
if (accumBuffer != NULL_PTR(uint8 *)) {
|
||||
@@ -186,19 +154,6 @@ UDPStreamer::~UDPStreamer() {
|
||||
scratchTimestamps = NULL_PTR(uint64 *);
|
||||
}
|
||||
|
||||
/* Multicast-mode cleanup */
|
||||
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
(void) tcpClient->Close();
|
||||
delete tcpClient;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
}
|
||||
if (tcpListener.IsValid()) {
|
||||
(void) tcpListener.Close();
|
||||
}
|
||||
if (dataSocket.IsValid()) {
|
||||
(void) dataSocket.Close();
|
||||
}
|
||||
|
||||
if (signalInfos != NULL_PTR(UDPStreamerSignalInfo *)) {
|
||||
delete[] signalInfos;
|
||||
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
|
||||
@@ -332,37 +287,25 @@ bool UDPStreamer::Initialise(StructuredDataI &data) {
|
||||
}
|
||||
|
||||
if (ok) {
|
||||
StreamString mcastStr = "";
|
||||
(void) data.Read("MulticastGroup", mcastStr);
|
||||
if (mcastStr.Size() > 0u) {
|
||||
multicastGroup = mcastStr;
|
||||
useMulticast = true;
|
||||
|
||||
uint16 dp = 0u;
|
||||
if (!data.Read("DataPort", dp)) {
|
||||
dp = port + UDPS_DEFAULT_DATA_PORT_OFFSET;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"DataPort not specified; using Port+1 = %u.",
|
||||
static_cast<uint32>(dp));
|
||||
}
|
||||
if ((dp == 0u) || (dp == port)) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"DataPort %u must be non-zero and differ from Port %u.",
|
||||
static_cast<uint32>(dp), static_cast<uint32>(port));
|
||||
ok = false;
|
||||
}
|
||||
else {
|
||||
dataPort = dp;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"Multicast mode: group=%s, controlPort=%u, dataPort=%u.",
|
||||
multicastGroup.Buffer(),
|
||||
static_cast<uint32>(port),
|
||||
static_cast<uint32>(dataPort));
|
||||
// Build server config with already-resolved Port and MaxPayloadSize so
|
||||
// UDPSServer::Initialise() always sees them, even when defaults were used.
|
||||
ConfigurationDatabase serverCfg;
|
||||
(void) serverCfg.Write("Port", static_cast<uint32>(port));
|
||||
(void) serverCfg.Write("MaxPayloadSize", maxPayloadSize);
|
||||
// Forward optional multicast / timeout params if present in caller's data.
|
||||
StreamString mcGroup;
|
||||
if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
|
||||
(void) serverCfg.Write("MulticastGroup", mcGroup.Buffer());
|
||||
uint32 dp = 0u;
|
||||
if (data.Read("DataPort", dp)) {
|
||||
(void) serverCfg.Write("DataPort", dp);
|
||||
}
|
||||
}
|
||||
else {
|
||||
useMulticast = false;
|
||||
uint32 clientTimeout = 0u;
|
||||
if (data.Read("ClientTimeout", clientTimeout)) {
|
||||
(void) serverCfg.Write("ClientTimeout", clientTimeout);
|
||||
}
|
||||
ok = server.Initialise(serverCfg);
|
||||
}
|
||||
|
||||
return ok;
|
||||
@@ -843,59 +786,28 @@ bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
|
||||
const char8 *const nextStateName) {
|
||||
bool ok = true;
|
||||
|
||||
if (useMulticast) {
|
||||
/* Open TCP listener for control (CONNECT/DISCONNECT) */
|
||||
if (!tcpListener.IsValid()) {
|
||||
ok = tcpListener.Open();
|
||||
if (ok) {
|
||||
ok = tcpListener.Listen(port, UDPS_TCP_MAX_CONNECTIONS);
|
||||
}
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not open TCP listener on port %u.",
|
||||
static_cast<uint32>(port));
|
||||
}
|
||||
/* SetBlocking(false) makes WaitConnection(TimeoutType(0u)) non-blocking.
|
||||
* If it fails, the select() guard in Execute() protects us. */
|
||||
if (ok) {
|
||||
if (!tcpListener.SetBlocking(false)) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"SetBlocking(false) on TCP listener failed; "
|
||||
"using select() guard only.");
|
||||
}
|
||||
}
|
||||
}
|
||||
ok = server.Start();
|
||||
|
||||
/* Open UDP data socket aimed at multicast group */
|
||||
if (ok && !dataSocket.IsValid()) {
|
||||
ok = dataSocket.Open();
|
||||
if (ok) {
|
||||
ok = dataSocket.Connect(multicastGroup.Buffer(), dataPort);
|
||||
/* Build the CONFIG payload and cache it in the server so any CONNECT client
|
||||
* receives it immediately. The config is static for the lifetime of this state. */
|
||||
if (ok) {
|
||||
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u + 1u;
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
|
||||
if (cfgBuf != NULL_PTR(uint8 *)) {
|
||||
uint32 cfgPayloadSize = 0u;
|
||||
if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
|
||||
(void) server.SendConfig(cfgBuf, cfgPayloadSize);
|
||||
}
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not connect data socket to %s:%u.",
|
||||
multicastGroup.Buffer(),
|
||||
static_cast<uint32>(dataPort));
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Could not build initial CONFIG payload.");
|
||||
}
|
||||
heap->Free(reinterpret_cast<void *&>(cfgBuf));
|
||||
}
|
||||
}
|
||||
else {
|
||||
/* Unicast mode: existing UDP server socket (idempotent: skip if already valid) */
|
||||
if (!serverSocket.IsValid()) {
|
||||
ok = serverSocket.Open();
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not open server UDP socket.");
|
||||
}
|
||||
if (ok) {
|
||||
ok = serverSocket.Listen(port);
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not bind server socket to port %u.",
|
||||
static_cast<uint32>(port));
|
||||
}
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Could not allocate CONFIG buffer.");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1022,95 +934,10 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
|
||||
bool dataReady = (waitErr == ErrorManagement::NoError);
|
||||
|
||||
/* --- Poll for incoming control commands --- */
|
||||
uint32 hdrSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
|
||||
if (useMulticast) {
|
||||
/* Multicast mode: non-blocking TCP accept + poll existing TCP client */
|
||||
Handle listenFd = tcpListener.GetReadHandle();
|
||||
fd_set rfdL;
|
||||
FD_ZERO(&rfdL);
|
||||
FD_SET(static_cast<int>(listenFd), &rfdL);
|
||||
struct timeval tvL = { 0, 0 };
|
||||
if (select(static_cast<int>(listenFd) + 1, &rfdL,
|
||||
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvL) > 0) {
|
||||
BasicTCPSocket *newClient = tcpListener.WaitConnection(TimeoutType(0u));
|
||||
if (newClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
/* Evict any existing client */
|
||||
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
(void) tcpClient->Close();
|
||||
delete tcpClient;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
clientConnected = false;
|
||||
}
|
||||
tcpClient = newClient;
|
||||
/* Read CONNECT packet from the new TCP connection */
|
||||
uint8 connBuf[sizeof(UDPSPacketHeader)];
|
||||
uint32 recvSize = hdrSize;
|
||||
bool readOk = tcpClient->Read(
|
||||
reinterpret_cast<char8 *>(connBuf), recvSize);
|
||||
if (readOk && (recvSize >= hdrSize)) {
|
||||
HandleTCPConnect(connBuf, recvSize);
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"TCP: incomplete CONNECT (%u bytes); closing.",
|
||||
recvSize);
|
||||
(void) tcpClient->Close();
|
||||
delete tcpClient;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
}
|
||||
}
|
||||
}
|
||||
/* --- Poll for incoming control commands (CONNECT / DISCONNECT / ACK) --- */
|
||||
server.ServiceClients();
|
||||
|
||||
/* Poll existing TCP client for DISCONNECT or closed connection */
|
||||
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
Handle cfd = tcpClient->GetReadHandle();
|
||||
fd_set rfdC;
|
||||
FD_ZERO(&rfdC);
|
||||
FD_SET(static_cast<int>(cfd), &rfdC);
|
||||
struct timeval tvC = { 0, 0 };
|
||||
if (select(static_cast<int>(cfd) + 1, &rfdC,
|
||||
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvC) > 0) {
|
||||
uint8 cmdBuf[256u];
|
||||
uint32 cmdSize = static_cast<uint32>(sizeof(cmdBuf));
|
||||
bool readOk = tcpClient->Read(
|
||||
reinterpret_cast<char8 *>(cmdBuf), cmdSize);
|
||||
if (readOk && (cmdSize >= hdrSize)) {
|
||||
HandleClientCommand(cmdBuf, cmdSize);
|
||||
}
|
||||
else {
|
||||
/* Zero-byte / error = TCP peer closed */
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"TCP client disconnected (read %u bytes).", cmdSize);
|
||||
(void) tcpClient->Close();
|
||||
delete tcpClient;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
clientConnected = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
/* Unicast mode: existing UDP serverSocket non-blocking poll */
|
||||
uint8 cmdBuf[256u];
|
||||
Handle sockFd = serverSocket.GetReadHandle();
|
||||
fd_set rfds;
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(static_cast<int>(sockFd), &rfds);
|
||||
struct timeval tv = { 0, 0 };
|
||||
int nReady = select(static_cast<int>(sockFd) + 1, &rfds,
|
||||
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
|
||||
if (nReady > 0) {
|
||||
uint32 recvSize = static_cast<uint32>(sizeof(cmdBuf));
|
||||
bool received = serverSocket.Read(
|
||||
reinterpret_cast<char8 *>(cmdBuf), recvSize);
|
||||
if (received && (recvSize >= hdrSize)) {
|
||||
HandleClientCommand(cmdBuf, recvSize);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (dataReady && clientConnected) {
|
||||
if (dataReady && server.HasClients()) {
|
||||
/* Synchronise() already gates posting dataSem to the correct rate
|
||||
* (size/time for Accumulate, every-Nth for Decimate, every call for
|
||||
* Strict). Execute() just sends whatever is in the ready buffers. */
|
||||
@@ -1134,8 +961,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
uint32 sendBytes = UDPS_TIMESTAMP_BYTES + 4u +
|
||||
fill * singleCycleWireBytes + fixedWireBytes;
|
||||
packetCounter++;
|
||||
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
|
||||
wireBuffer, sendBytes)) {
|
||||
if (!server.SendData(packetCounter, wireBuffer, sendBytes)) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Failed to send Accumulate DATA packet (counter=%u).",
|
||||
packetCounter);
|
||||
@@ -1154,8 +980,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
QuantizeAndSerialize(scratchBuffer, ts);
|
||||
|
||||
packetCounter++;
|
||||
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
|
||||
wireBuffer, totalWireBytes)) {
|
||||
if (!server.SendData(packetCounter, wireBuffer, totalWireBytes)) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Failed to send DATA packet (counter=%u).",
|
||||
packetCounter);
|
||||
@@ -1165,19 +990,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
}
|
||||
|
||||
if (info.GetStage() == ExecutionInfo::TerminationStage) {
|
||||
if (clientConnected) {
|
||||
if (useMulticast) {
|
||||
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
(void) tcpClient->Close();
|
||||
delete tcpClient;
|
||||
tcpClient = NULL_PTR(BasicTCPSocket *);
|
||||
}
|
||||
}
|
||||
else {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
clientConnected = false;
|
||||
}
|
||||
(void) server.Stop();
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"UDPStreamer background thread terminated.");
|
||||
}
|
||||
@@ -1331,88 +1144,6 @@ void UDPStreamer::SerializeAccumulated(const uint8 *src,
|
||||
}
|
||||
}
|
||||
|
||||
void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
|
||||
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
|
||||
return;
|
||||
}
|
||||
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
|
||||
if (hdr->magic != UDPS_MAGIC) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (hdr->type == UDPS_TYPE_CONNECT) {
|
||||
if (useMulticast) {
|
||||
/* In multicast mode, CONNECT is handled by HandleTCPConnect(); ignore here */
|
||||
return;
|
||||
}
|
||||
InternetHost src = serverSocket.GetSource();
|
||||
|
||||
/* Disconnect any previous client */
|
||||
if (clientConnected) {
|
||||
if (clientSocket.IsValid()) {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
clientConnected = false;
|
||||
}
|
||||
|
||||
/* Open a new client socket and connect to the requesting address */
|
||||
bool sockOk = clientSocket.Open();
|
||||
if (sockOk) {
|
||||
sockOk = clientSocket.Connect(src.GetAddress().Buffer(), src.GetPort());
|
||||
}
|
||||
if (sockOk) {
|
||||
clientConnected = true;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"Client connected from %s:%u.",
|
||||
src.GetAddress().Buffer(),
|
||||
static_cast<uint32>(src.GetPort()));
|
||||
|
||||
/* Send CONFIG packet */
|
||||
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u + 1u;
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
|
||||
if (cfgBuf != NULL_PTR(uint8 *)) {
|
||||
uint32 cfgPayloadSize = 0u;
|
||||
if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
|
||||
(void) SendFragmented(UDPS_TYPE_CONFIG, 0u, cfgBuf, cfgPayloadSize);
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Could not build CONFIG payload.");
|
||||
}
|
||||
heap->Free(reinterpret_cast<void *&>(cfgBuf));
|
||||
}
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Could not connect to client %s:%u.",
|
||||
src.GetAddress().Buffer(),
|
||||
static_cast<uint32>(src.GetPort()));
|
||||
}
|
||||
}
|
||||
else if (hdr->type == UDPS_TYPE_DISCONNECT) {
|
||||
REPORT_ERROR(ErrorManagement::Information, "Client sent DISCONNECT.");
|
||||
if (clientConnected) {
|
||||
if (!useMulticast) {
|
||||
if (clientSocket.IsValid()) {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
}
|
||||
/* In multicast mode, tcpClient cleanup is done by the Execute() poll caller */
|
||||
clientConnected = false;
|
||||
}
|
||||
}
|
||||
else if (hdr->type == UDPS_TYPE_ACK) {
|
||||
/* Optional: track acknowledged counters for loss detection */
|
||||
if (size >= static_cast<uint32>(sizeof(UDPSPacketHeader)) + 4u) {
|
||||
uint32 ackedCounter = 0u;
|
||||
const uint8 *pl = buf + sizeof(UDPSPacketHeader);
|
||||
(void) MemoryOperationsHelper::Copy(&ackedCounter, pl, 4u);
|
||||
REPORT_ERROR(ErrorManagement::Debug,
|
||||
"ACK received for packet counter %u.", ackedCounter);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool UDPStreamer::BuildConfigPayload(uint8 *buf,
|
||||
uint32 bufSize,
|
||||
@@ -1580,65 +1311,6 @@ void UDPStreamer::QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp) {
|
||||
}
|
||||
}
|
||||
|
||||
bool UDPStreamer::SendFragmented(uint8 type,
|
||||
uint32 counter,
|
||||
const uint8 *payload,
|
||||
uint32 payloadSize) {
|
||||
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
|
||||
uint32 maxChunk = maxPayloadSize - headerSize;
|
||||
uint32 totalFrags = (payloadSize == 0u) ? 1u :
|
||||
((payloadSize + maxChunk - 1u) / maxChunk);
|
||||
|
||||
uint32 sendBufSize = headerSize + maxChunk;
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(sendBufSize));
|
||||
if (sendBuf == NULL_PTR(uint8 *)) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not allocate send buffer (%u bytes).", sendBufSize);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ok = true;
|
||||
uint32 offs = 0u;
|
||||
|
||||
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
|
||||
uint32 chunkSize = payloadSize - offs;
|
||||
if (chunkSize > maxChunk) {
|
||||
chunkSize = maxChunk;
|
||||
}
|
||||
|
||||
UDPSPacketHeader *hdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
|
||||
hdr->magic = UDPS_MAGIC;
|
||||
hdr->type = type;
|
||||
hdr->counter = counter;
|
||||
hdr->fragmentIdx = static_cast<uint16>(f);
|
||||
hdr->totalFragments = static_cast<uint16>(totalFrags);
|
||||
hdr->payloadBytes = chunkSize;
|
||||
|
||||
if (chunkSize > 0u) {
|
||||
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
|
||||
payload + offs,
|
||||
chunkSize);
|
||||
}
|
||||
|
||||
uint32 sendSize = headerSize + chunkSize;
|
||||
if (useMulticast) {
|
||||
ok = dataSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
|
||||
}
|
||||
else {
|
||||
ok = clientSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
|
||||
}
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Fragment %u/%u send failed.", f + 1u, totalFrags);
|
||||
}
|
||||
offs += chunkSize;
|
||||
}
|
||||
|
||||
heap->Free(reinterpret_cast<void *&>(sendBuf));
|
||||
return ok;
|
||||
}
|
||||
|
||||
uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
|
||||
uint8 code = UDPS_TYPECODE_UNKNOWN;
|
||||
if (td == UnsignedInteger8Bit) { code = UDPS_TYPECODE_UINT8; }
|
||||
@@ -1654,78 +1326,6 @@ uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
|
||||
return code;
|
||||
}
|
||||
|
||||
void UDPStreamer::HandleTCPConnect(const uint8 *buf, uint32 size) {
|
||||
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
|
||||
return;
|
||||
}
|
||||
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
|
||||
if (hdr->magic != UDPS_MAGIC) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"TCP CONNECT: bad magic 0x%08X.", hdr->magic);
|
||||
return;
|
||||
}
|
||||
if (hdr->type != UDPS_TYPE_CONNECT) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"TCP CONNECT: unexpected packet type %u.", hdr->type);
|
||||
return;
|
||||
}
|
||||
|
||||
clientConnected = true;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"TCP client connected; DATA will multicast to %s:%u.",
|
||||
multicastGroup.Buffer(), static_cast<uint32>(dataPort));
|
||||
|
||||
/* Build CONFIG payload and send as a single TCP message (no fragmentation needed) */
|
||||
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u;
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
|
||||
if (cfgBuf == NULL_PTR(uint8 *)) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not allocate CONFIG buffer.");
|
||||
clientConnected = false;
|
||||
return;
|
||||
}
|
||||
|
||||
uint32 cfgPayloadSize = 0u;
|
||||
bool built = BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize);
|
||||
if (built) {
|
||||
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
|
||||
uint32 totalSize = headerSize + cfgPayloadSize;
|
||||
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(totalSize));
|
||||
if (sendBuf != NULL_PTR(uint8 *)) {
|
||||
UDPSPacketHeader *outHdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
|
||||
outHdr->magic = UDPS_MAGIC;
|
||||
outHdr->type = UDPS_TYPE_CONFIG;
|
||||
outHdr->counter = 0u;
|
||||
outHdr->fragmentIdx = 0u;
|
||||
outHdr->totalFragments = 1u;
|
||||
outHdr->payloadBytes = cfgPayloadSize;
|
||||
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
|
||||
cfgBuf, cfgPayloadSize);
|
||||
bool writeOk = tcpClient->Write(
|
||||
reinterpret_cast<const char8 *>(sendBuf), totalSize);
|
||||
if (!writeOk) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"TCP: failed to send CONFIG packet.");
|
||||
clientConnected = false;
|
||||
}
|
||||
heap->Free(reinterpret_cast<void *&>(sendBuf));
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not allocate TCP send buffer.");
|
||||
clientConnected = false;
|
||||
}
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"BuildConfigPayload failed; client not fully connected.");
|
||||
clientConnected = false;
|
||||
}
|
||||
|
||||
heap->Free(reinterpret_cast<void *&>(cfgBuf));
|
||||
}
|
||||
|
||||
uint16 UDPStreamer::GetPort() const {
|
||||
return port;
|
||||
}
|
||||
@@ -1735,11 +1335,11 @@ uint32 UDPStreamer::GetMaxPayloadSize() const {
|
||||
}
|
||||
|
||||
bool UDPStreamer::IsClientConnected() const {
|
||||
return clientConnected;
|
||||
return server.HasClients();
|
||||
}
|
||||
|
||||
bool UDPStreamer::IsMulticast() const {
|
||||
return useMulticast;
|
||||
return server.IsMulticast();
|
||||
}
|
||||
|
||||
CLASS_REGISTER(UDPStreamer, "1.0")
|
||||
|
||||
Reference in New Issue
Block a user