Martino Ferrari
ed5d381d32
Reorder Execute() main stage so the background thread waits on dataSem (sleeping until the RT thread posts) before doing the socket select(). The socket poll is now non-blocking (timeout=0) since command latency bounded by UDPS_DATA_WAIT_MS is acceptable for CONNECT/DISCONNECT. Also force-remove old udpstreamer-webui binary in run.sh before rebuild so stale embedded assets are never served. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
999 lines
38 KiB
C++
999 lines
38 KiB
C++
/**
|
|
* @file UDPStreamer.cpp
|
|
* @brief Source file for class UDPStreamer
|
|
* @date 13/05/2026
|
|
* @author Martino Ferrari
|
|
*
|
|
* @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
|
|
* the Development of Fusion Energy ('Fusion for Energy').
|
|
* Licensed under the EUPL, Version 1.1 or - as soon they will be approved
|
|
* by the European Commission - subsequent versions of the EUPL (the "Licence")
|
|
* You may not use this work except in compliance with the Licence.
|
|
* You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
|
|
*
|
|
* @warning Unless required by applicable law or agreed to in writing,
|
|
* software distributed under the Licence is distributed on an "AS IS"
|
|
* basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
|
|
* or implied. See the Licence permissions and limitations under the Licence.
|
|
*
|
|
* @details This source file contains the definition of all the methods for
|
|
* the class UDPStreamer (public, protected, and private). Be aware that some
|
|
* methods, such as those inline could be defined on the header file, instead.
|
|
*/
|
|
|
|
#define DLL_API
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Standard header includes */
|
|
/*---------------------------------------------------------------------------*/
|
|
#include <sys/select.h>
|
|
#include <unistd.h>
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Project header includes */
|
|
/*---------------------------------------------------------------------------*/
|
|
#include "AdvancedErrorManagement.h"
|
|
#include "EmbeddedThreadI.h"
|
|
#include "GlobalObjectsDatabase.h"
|
|
#include "HighResolutionTimer.h"
|
|
#include "MemoryMapSynchronisedOutputBroker.h"
|
|
#include "MemoryOperationsHelper.h"
|
|
#include "Sleep.h"
|
|
#include "Threads.h"
|
|
#include "UDPStreamer.h"
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Static definitions */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
namespace MARTe {
|
|
|
|
/** Default port used when none is specified. */
|
|
static const uint16 UDPS_DEFAULT_PORT = 44500u;
|
|
|
|
/** Default max payload per UDP datagram (bytes). */
|
|
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;
|
|
|
|
/** Server socket receive timeout in milliseconds.
|
|
* Set to 0 for a pure non-blocking poll so the send loop can keep pace with
|
|
* the RT thread regardless of its frequency. Client commands (CONNECT /
|
|
* DISCONNECT) are still caught on the very next loop iteration. */
|
|
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;
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Method definitions */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
UDPStreamer::UDPStreamer() :
|
|
MemoryDataSourceI(),
|
|
EmbeddedServiceMethodBinderI(),
|
|
executor(*this) {
|
|
port = UDPS_DEFAULT_PORT;
|
|
maxPayloadSize = UDPS_DEFAULT_MAX_PAYLOAD;
|
|
cpuMask = 0xFFFFFFFFu;
|
|
stackSize = THREADS_DEFAULT_STACKSIZE;
|
|
numSigs = 0u;
|
|
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
|
|
readyBuffer = NULL_PTR(uint8 *);
|
|
scratchBuffer = NULL_PTR(uint8 *);
|
|
wireBuffer = NULL_PTR(uint8 *);
|
|
totalSrcBytes = 0u;
|
|
totalWireBytes = 0u;
|
|
syncTimestamp = 0u;
|
|
clientConnected = false;
|
|
packetCounter = 0u;
|
|
|
|
if (!dataSem.Create()) {
|
|
REPORT_ERROR(ErrorManagement::FatalError, "Could not create EventSem.");
|
|
}
|
|
bufMutex.Create(false);
|
|
}
|
|
|
|
/*lint -e{1551} Destructor must guarantee thread and socket cleanup. */
|
|
UDPStreamer::~UDPStreamer() {
|
|
/* Unblock the background thread's dataSem wait so it can exit */
|
|
(void) dataSem.Post();
|
|
|
|
if (executor.GetStatus() != EmbeddedThreadI::OffState) {
|
|
if (!executor.Stop()) {
|
|
REPORT_ERROR(ErrorManagement::Warning,
|
|
"First Stop() attempt failed; retrying.");
|
|
if (!executor.Stop()) {
|
|
REPORT_ERROR(ErrorManagement::FatalError,
|
|
"Could not stop background thread.");
|
|
}
|
|
}
|
|
}
|
|
|
|
if (serverSocket.IsValid()) {
|
|
(void) serverSocket.Close();
|
|
}
|
|
if (clientSocket.IsValid()) {
|
|
(void) clientSocket.Close();
|
|
}
|
|
|
|
if (signalInfos != NULL_PTR(UDPStreamerSignalInfo *)) {
|
|
delete[] signalInfos;
|
|
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
|
|
}
|
|
|
|
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
|
if (readyBuffer != NULL_PTR(uint8 *)) {
|
|
heap->Free(reinterpret_cast<void *&>(readyBuffer));
|
|
}
|
|
if (scratchBuffer != NULL_PTR(uint8 *)) {
|
|
heap->Free(reinterpret_cast<void *&>(scratchBuffer));
|
|
}
|
|
if (wireBuffer != NULL_PTR(uint8 *)) {
|
|
heap->Free(reinterpret_cast<void *&>(wireBuffer));
|
|
}
|
|
|
|
(void) dataSem.Close();
|
|
}
|
|
|
|
bool UDPStreamer::Initialise(StructuredDataI &data) {
|
|
bool ok = MemoryDataSourceI::Initialise(data);
|
|
|
|
if (ok) {
|
|
if (!data.Read("Port", port)) {
|
|
port = UDPS_DEFAULT_PORT;
|
|
REPORT_ERROR(ErrorManagement::Information,
|
|
"Port not specified; using default %u.",
|
|
static_cast<uint32>(port));
|
|
}
|
|
if (port <= 1024u) {
|
|
REPORT_ERROR(ErrorManagement::Warning,
|
|
"Port %u is in the privileged range (<= 1024).",
|
|
static_cast<uint32>(port));
|
|
}
|
|
}
|
|
|
|
if (ok) {
|
|
if (!data.Read("MaxPayloadSize", maxPayloadSize)) {
|
|
maxPayloadSize = UDPS_DEFAULT_MAX_PAYLOAD;
|
|
REPORT_ERROR(ErrorManagement::Information,
|
|
"MaxPayloadSize not specified; using default %u.",
|
|
maxPayloadSize);
|
|
}
|
|
if (maxPayloadSize < UDPS_MIN_PAYLOAD) {
|
|
REPORT_ERROR(ErrorManagement::ParametersError,
|
|
"MaxPayloadSize %u is too small (minimum %u).",
|
|
maxPayloadSize, UDPS_MIN_PAYLOAD);
|
|
ok = false;
|
|
}
|
|
}
|
|
|
|
if (ok) {
|
|
uint32 cpuMaskIn = 0xFFFFFFFFu;
|
|
if (!data.Read("CPUMask", cpuMaskIn)) {
|
|
REPORT_ERROR(ErrorManagement::Information,
|
|
"CPUMask not specified; using 0xFFFFFFFF.");
|
|
}
|
|
cpuMask = cpuMaskIn;
|
|
}
|
|
|
|
if (ok) {
|
|
if (!data.Read("StackSize", stackSize)) {
|
|
stackSize = THREADS_DEFAULT_STACKSIZE;
|
|
REPORT_ERROR(ErrorManagement::Information,
|
|
"StackSize not specified; using MARTe2 default %u.",
|
|
stackSize);
|
|
}
|
|
if (stackSize == 0u) {
|
|
REPORT_ERROR(ErrorManagement::ParametersError, "StackSize must be > 0.");
|
|
ok = false;
|
|
}
|
|
}
|
|
|
|
return ok;
|
|
}
|
|
|
|
bool UDPStreamer::SetConfiguredDatabase(StructuredDataI &data) {
|
|
bool ok = MemoryDataSourceI::SetConfiguredDatabase(data);
|
|
if (!ok) {
|
|
return false;
|
|
}
|
|
|
|
numSigs = GetNumberOfSignals();
|
|
if (numSigs == 0u) {
|
|
REPORT_ERROR(ErrorManagement::ParametersError,
|
|
"At least one signal must be defined.");
|
|
return false;
|
|
}
|
|
|
|
signalInfos = new UDPStreamerSignalInfo[numSigs];
|
|
/* Local array to hold time-signal names (resolved to indices in pass 3) */
|
|
StreamString *timeSignalNames = new StreamString[numSigs];
|
|
|
|
/* --- Pass 1: populate from the MARTe2 framework APIs --- */
|
|
totalSrcBytes = 0u;
|
|
totalWireBytes = UDPS_TIMESTAMP_BYTES;
|
|
|
|
for (uint32 i = 0u; i < numSigs && ok; i++) {
|
|
StreamString sigName;
|
|
ok = GetSignalName(i, sigName);
|
|
if (!ok) {
|
|
REPORT_ERROR(ErrorManagement::FatalError,
|
|
"Could not get name for signal %u.", i);
|
|
break;
|
|
}
|
|
signalInfos[i].name = sigName;
|
|
signalInfos[i].type = GetSignalType(i);
|
|
signalInfos[i].numDimensions = 0u;
|
|
signalInfos[i].numElements = 1u;
|
|
signalInfos[i].numRows = 1u;
|
|
signalInfos[i].numCols = 1u;
|
|
signalInfos[i].quantType = UDPStreamerQuantNone;
|
|
signalInfos[i].rangeMin = 0.0;
|
|
signalInfos[i].rangeMax = 1.0;
|
|
signalInfos[i].timeMode = UDPStreamerTimePacket;
|
|
signalInfos[i].samplingRate = 0.0;
|
|
signalInfos[i].timeSignalIdx = UDPS_NO_TIME_SIGNAL;
|
|
signalInfos[i].unit = "";
|
|
signalInfos[i].srcByteSize = 0u;
|
|
signalInfos[i].wireByteSize = 0u;
|
|
signalInfos[i].bufferOffset = 0u;
|
|
timeSignalNames[i] = "";
|
|
|
|
uint8 ndims = 0u;
|
|
(void) GetSignalNumberOfDimensions(i, ndims);
|
|
signalInfos[i].numDimensions = ndims;
|
|
|
|
uint32 nelems = 1u;
|
|
(void) GetSignalNumberOfElements(i, nelems);
|
|
signalInfos[i].numElements = nelems;
|
|
signalInfos[i].numCols = nelems;
|
|
|
|
uint32 bsz = 0u;
|
|
(void) GetSignalByteSize(i, bsz);
|
|
signalInfos[i].srcByteSize = bsz;
|
|
signalInfos[i].bufferOffset = totalSrcBytes;
|
|
totalSrcBytes += bsz;
|
|
}
|
|
|
|
/* --- Pass 2: read custom per-signal fields from signalsDatabase ---
|
|
* Note: DataSourceI::AddSignals() leaves signalsDatabase positioned at the
|
|
* "Signals" node. We must reset to root before navigating. */
|
|
if (ok) {
|
|
(void) signalsDatabase.MoveToRoot();
|
|
bool moved = signalsDatabase.MoveRelative("Signals");
|
|
if (!moved) {
|
|
REPORT_ERROR(ErrorManagement::FatalError,
|
|
"Could not navigate to Signals in signalsDatabase.");
|
|
ok = false;
|
|
}
|
|
}
|
|
|
|
for (uint32 i = 0u; i < numSigs && ok; i++) {
|
|
bool moved = signalsDatabase.MoveRelative(signalInfos[i].name.Buffer());
|
|
if (!moved) {
|
|
/* Signal added by framework with no user-configured custom fields */
|
|
continue;
|
|
}
|
|
|
|
/* Unit */
|
|
StreamString unit = "";
|
|
(void) signalsDatabase.Read("Unit", unit);
|
|
signalInfos[i].unit = unit;
|
|
|
|
/* Range */
|
|
(void) signalsDatabase.Read("RangeMin", signalInfos[i].rangeMin);
|
|
(void) signalsDatabase.Read("RangeMax", signalInfos[i].rangeMax);
|
|
|
|
/* QuantizedType */
|
|
StreamString quantStr = "";
|
|
if (signalsDatabase.Read("QuantizedType", quantStr)) {
|
|
if (quantStr == "uint8") {
|
|
signalInfos[i].quantType = UDPStreamerQuantUint8;
|
|
}
|
|
else if (quantStr == "int8") {
|
|
signalInfos[i].quantType = UDPStreamerQuantInt8;
|
|
}
|
|
else if (quantStr == "uint16") {
|
|
signalInfos[i].quantType = UDPStreamerQuantUint16;
|
|
}
|
|
else if (quantStr == "int16") {
|
|
signalInfos[i].quantType = UDPStreamerQuantInt16;
|
|
}
|
|
else if (quantStr == "none") {
|
|
signalInfos[i].quantType = UDPStreamerQuantNone;
|
|
}
|
|
else {
|
|
REPORT_ERROR(ErrorManagement::ParametersError,
|
|
"Signal %s: unknown QuantizedType '%s'. "
|
|
"Allowed: none|uint8|int8|uint16|int16.",
|
|
signalInfos[i].name.Buffer(), quantStr.Buffer());
|
|
ok = false;
|
|
}
|
|
if (ok && (signalInfos[i].quantType != UDPStreamerQuantNone)) {
|
|
TypeDescriptor td = signalInfos[i].type;
|
|
bool isFloat = ((td == Float32Bit) || (td == Float64Bit));
|
|
if (!isFloat) {
|
|
REPORT_ERROR(ErrorManagement::ParametersError,
|
|
"Signal %s: QuantizedType only supported for "
|
|
"float32/float64 signals.",
|
|
signalInfos[i].name.Buffer());
|
|
ok = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* TimeMode */
|
|
if (ok) {
|
|
StreamString timeModeStr;
|
|
(void) signalsDatabase.Read("TimeMode", timeModeStr);
|
|
if (timeModeStr.Size() == 0u) {
|
|
timeModeStr = "PacketTime";
|
|
}
|
|
if (timeModeStr == "PacketTime") {
|
|
signalInfos[i].timeMode = UDPStreamerTimePacket;
|
|
}
|
|
else if (timeModeStr == "FullArray") {
|
|
signalInfos[i].timeMode = UDPStreamerTimeFullArray;
|
|
}
|
|
else if (timeModeStr == "FirstSample") {
|
|
signalInfos[i].timeMode = UDPStreamerTimeFirstSample;
|
|
}
|
|
else if (timeModeStr == "LastSample") {
|
|
signalInfos[i].timeMode = UDPStreamerTimeLastSample;
|
|
}
|
|
else {
|
|
REPORT_ERROR(ErrorManagement::ParametersError,
|
|
"Signal %s: unknown TimeMode '%s'. "
|
|
"Allowed: PacketTime|FullArray|FirstSample|LastSample.",
|
|
signalInfos[i].name.Buffer(), timeModeStr.Buffer());
|
|
ok = false;
|
|
}
|
|
}
|
|
|
|
/* TimeSignal (required when TimeMode != PacketTime) */
|
|
if (ok && (signalInfos[i].timeMode != UDPStreamerTimePacket)) {
|
|
StreamString tsName = "";
|
|
if (!signalsDatabase.Read("TimeSignal", tsName)) {
|
|
REPORT_ERROR(ErrorManagement::ParametersError,
|
|
"Signal %s: TimeSignal must be specified when "
|
|
"TimeMode != PacketTime.",
|
|
signalInfos[i].name.Buffer());
|
|
ok = false;
|
|
}
|
|
else {
|
|
timeSignalNames[i] = tsName;
|
|
/* Index resolved in pass 3 */
|
|
signalInfos[i].timeSignalIdx = UDPS_NO_TIME_SIGNAL;
|
|
}
|
|
}
|
|
|
|
/* SamplingRate */
|
|
if (ok) {
|
|
(void) signalsDatabase.Read("SamplingRate", signalInfos[i].samplingRate);
|
|
bool needsRate = (signalInfos[i].timeMode == UDPStreamerTimeFirstSample ||
|
|
signalInfos[i].timeMode == UDPStreamerTimeLastSample);
|
|
if (needsRate && (signalInfos[i].samplingRate <= 0.0)) {
|
|
REPORT_ERROR(ErrorManagement::ParametersError,
|
|
"Signal %s: SamplingRate > 0 is required for "
|
|
"FirstSample/LastSample TimeMode.",
|
|
signalInfos[i].name.Buffer());
|
|
ok = false;
|
|
}
|
|
}
|
|
|
|
(void) signalsDatabase.MoveToAncestor(1u);
|
|
}
|
|
|
|
if (ok || true) { /* always attempt to restore navigation */
|
|
(void) signalsDatabase.MoveToAncestor(1u);
|
|
}
|
|
|
|
/* --- Pass 3: resolve TimeSignal names to signal indices --- */
|
|
for (uint32 i = 0u; i < numSigs && ok; i++) {
|
|
if (signalInfos[i].timeMode == UDPStreamerTimePacket) {
|
|
continue; /* no time signal needed */
|
|
}
|
|
bool found = false;
|
|
for (uint32 j = 0u; j < numSigs; j++) {
|
|
if (signalInfos[j].name == timeSignalNames[i]) {
|
|
signalInfos[i].timeSignalIdx = j;
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!found) {
|
|
REPORT_ERROR(ErrorManagement::ParametersError,
|
|
"Signal %s: TimeSignal '%s' not found among declared signals.",
|
|
signalInfos[i].name.Buffer(),
|
|
timeSignalNames[i].Buffer());
|
|
ok = false;
|
|
}
|
|
}
|
|
|
|
delete[] timeSignalNames;
|
|
timeSignalNames = NULL_PTR(StreamString *);
|
|
|
|
/* --- Pass 4: validate time-signal dimensions and compute wire sizes --- */
|
|
for (uint32 i = 0u; i < numSigs && ok; i++) {
|
|
/* Compute wire byte size per element */
|
|
uint32 elemWireBytes = 0u;
|
|
switch (signalInfos[i].quantType) {
|
|
case UDPStreamerQuantUint8:
|
|
case UDPStreamerQuantInt8:
|
|
elemWireBytes = 1u;
|
|
break;
|
|
case UDPStreamerQuantUint16:
|
|
case UDPStreamerQuantInt16:
|
|
elemWireBytes = 2u;
|
|
break;
|
|
default:
|
|
/* Raw copy: element size = total / numElements */
|
|
if (signalInfos[i].numElements > 0u) {
|
|
elemWireBytes = signalInfos[i].srcByteSize / signalInfos[i].numElements;
|
|
}
|
|
break;
|
|
}
|
|
signalInfos[i].wireByteSize = elemWireBytes * signalInfos[i].numElements;
|
|
totalWireBytes += signalInfos[i].wireByteSize;
|
|
|
|
/* Validate time signal dimensions */
|
|
uint32 tsIdx = signalInfos[i].timeSignalIdx;
|
|
if (tsIdx != UDPS_NO_TIME_SIGNAL) {
|
|
uint32 tsElems = signalInfos[tsIdx].numElements;
|
|
if (signalInfos[i].timeMode == UDPStreamerTimeFullArray) {
|
|
if (tsElems != signalInfos[i].numElements) {
|
|
REPORT_ERROR(ErrorManagement::ParametersError,
|
|
"Signal %s: FullArray TimeMode requires TimeSignal "
|
|
"%s to have the same NumberOfElements (%u vs %u).",
|
|
signalInfos[i].name.Buffer(),
|
|
signalInfos[tsIdx].name.Buffer(),
|
|
tsElems, signalInfos[i].numElements);
|
|
ok = false;
|
|
}
|
|
}
|
|
else if ((signalInfos[i].timeMode == UDPStreamerTimeFirstSample) ||
|
|
(signalInfos[i].timeMode == UDPStreamerTimeLastSample)) {
|
|
if (tsElems != 1u) {
|
|
REPORT_ERROR(ErrorManagement::ParametersError,
|
|
"Signal %s: FirstSample/LastSample TimeMode requires "
|
|
"a scalar TimeSignal (found %u elements).",
|
|
signalInfos[i].name.Buffer(), tsElems);
|
|
ok = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return ok;
|
|
}
|
|
|
|
bool UDPStreamer::AllocateMemory() {
|
|
bool ok = MemoryDataSourceI::AllocateMemory();
|
|
if (!ok) {
|
|
return false;
|
|
}
|
|
|
|
/* stateMemorySize is populated by MemoryDataSourceI::AllocateMemory() */
|
|
if (totalSrcBytes == 0u) {
|
|
totalSrcBytes = stateMemorySize;
|
|
}
|
|
|
|
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
|
|
|
/* readyBuffer: copy of signal memory shared with background thread */
|
|
readyBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalSrcBytes));
|
|
if (readyBuffer == NULL_PTR(uint8 *)) {
|
|
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate readyBuffer.");
|
|
return false;
|
|
}
|
|
(void) MemoryOperationsHelper::Set(readyBuffer, 0, totalSrcBytes);
|
|
|
|
/* scratchBuffer: background-thread-private copy for serialization */
|
|
scratchBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalSrcBytes));
|
|
if (scratchBuffer == NULL_PTR(uint8 *)) {
|
|
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate scratchBuffer.");
|
|
return false;
|
|
}
|
|
(void) MemoryOperationsHelper::Set(scratchBuffer, 0, totalSrcBytes);
|
|
|
|
/* wireBuffer: serialized/quantized payload for transmission */
|
|
wireBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalWireBytes));
|
|
if (wireBuffer == NULL_PTR(uint8 *)) {
|
|
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate wireBuffer.");
|
|
return false;
|
|
}
|
|
(void) MemoryOperationsHelper::Set(wireBuffer, 0, totalWireBytes);
|
|
|
|
/* Update buffer offsets to match actual MemoryDataSourceI layout */
|
|
for (uint32 i = 0u; i < numSigs; i++) {
|
|
void *addr = NULL_PTR(void *);
|
|
if (GetSignalMemoryBuffer(i, 0u, addr)) {
|
|
signalInfos[i].bufferOffset =
|
|
static_cast<uint32>(reinterpret_cast<uint8 *>(addr) - memory);
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
const char8 *UDPStreamer::GetBrokerName(StructuredDataI &data,
|
|
const SignalDirection direction) {
|
|
const char8 *brokerName = "";
|
|
if (direction == OutputSignals) {
|
|
brokerName = "MemoryMapSynchronisedOutputBroker";
|
|
}
|
|
return brokerName;
|
|
}
|
|
|
|
bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
|
|
const char8 *const nextStateName) {
|
|
bool ok = true;
|
|
|
|
/* Open 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));
|
|
}
|
|
}
|
|
/* Socket stays blocking; Read() uses the timeout via select() internally. */
|
|
}
|
|
|
|
/* Start the background thread (idempotent) */
|
|
if (ok && (executor.GetStatus() == EmbeddedThreadI::OffState)) {
|
|
executor.SetName(GetName());
|
|
executor.SetCPUMask(ProcessorType(cpuMask));
|
|
executor.SetStackSize(stackSize);
|
|
ErrorManagement::ErrorType startErr = executor.Start();
|
|
ok = (startErr == ErrorManagement::NoError);
|
|
if (!ok) {
|
|
REPORT_ERROR(ErrorManagement::FatalError,
|
|
"Could not start background thread.");
|
|
}
|
|
}
|
|
|
|
return ok;
|
|
}
|
|
|
|
bool UDPStreamer::Synchronise() {
|
|
/* Capture timestamp as early as possible */
|
|
uint64 ts = HighResolutionTimer::Counter();
|
|
|
|
/* RT-safe copy of signal memory → readyBuffer */
|
|
bufMutex.FastLock(TTInfiniteWait);
|
|
(void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
|
|
syncTimestamp = ts;
|
|
bufMutex.FastUnLock();
|
|
|
|
/* Wake the background sender thread */
|
|
(void) dataSem.Post();
|
|
|
|
return true;
|
|
}
|
|
|
|
ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
|
ErrorManagement::ErrorType ret = ErrorManagement::NoError;
|
|
|
|
if (info.GetStage() == ExecutionInfo::StartupStage) {
|
|
REPORT_ERROR(ErrorManagement::Information,
|
|
"UDPStreamer background thread started (port %u).",
|
|
static_cast<uint32>(port));
|
|
}
|
|
|
|
if (info.GetStage() == ExecutionInfo::MainStage) {
|
|
/* --- Wait for RT thread to post new data ---
|
|
* ResetWait sleeps the background thread until the RT thread calls
|
|
* Synchronise() and posts dataSem, or until the timeout expires.
|
|
* Doing this FIRST means the thread spends nearly all its time here
|
|
* instead of spinning on the non-blocking select() below.
|
|
* Command latency is bounded by UDPS_DATA_WAIT_MS (acceptable for
|
|
* CONNECT / DISCONNECT). */
|
|
ErrorManagement::ErrorType waitErr =
|
|
dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
|
|
bool dataReady = (waitErr == ErrorManagement::NoError);
|
|
|
|
/* --- Poll server socket for incoming client commands (non-blocking) --- */
|
|
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 }; /* non-blocking poll */
|
|
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 >= static_cast<uint32>(sizeof(UDPSPacketHeader)))) {
|
|
HandleClientCommand(cmdBuf, recvSize);
|
|
}
|
|
}
|
|
|
|
if (dataReady && clientConnected) {
|
|
/* Copy readyBuffer → scratchBuffer under brief spinlock */
|
|
uint64 ts = 0u;
|
|
bufMutex.FastLock(TTInfiniteWait);
|
|
(void) MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer, totalSrcBytes);
|
|
ts = syncTimestamp;
|
|
bufMutex.FastUnLock();
|
|
|
|
/* Serialize signal data into wireBuffer */
|
|
QuantizeAndSerialize(scratchBuffer, ts);
|
|
|
|
/* Send (fragmented if needed) */
|
|
packetCounter++;
|
|
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter, wireBuffer, totalWireBytes)) {
|
|
REPORT_ERROR(ErrorManagement::Warning,
|
|
"Failed to send DATA packet (counter=%u).", packetCounter);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (info.GetStage() == ExecutionInfo::TerminationStage) {
|
|
if (clientConnected) {
|
|
(void) clientSocket.Close();
|
|
clientConnected = false;
|
|
}
|
|
REPORT_ERROR(ErrorManagement::Information,
|
|
"UDPStreamer background thread terminated.");
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
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) {
|
|
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;
|
|
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 (clientSocket.IsValid()) {
|
|
(void) clientSocket.Close();
|
|
}
|
|
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,
|
|
uint32 &payloadSize) {
|
|
payloadSize = 0u;
|
|
|
|
/* 4 bytes: number of signals */
|
|
if ((payloadSize + 4u) > bufSize) {
|
|
return false;
|
|
}
|
|
(void) MemoryOperationsHelper::Copy(buf + payloadSize, &numSigs, 4u);
|
|
payloadSize += 4u;
|
|
|
|
for (uint32 i = 0u; i < numSigs; i++) {
|
|
if ((payloadSize + UDPS_SIGNAL_DESC_SIZE) > bufSize) {
|
|
return false;
|
|
}
|
|
|
|
uint8 *p = buf + payloadSize;
|
|
|
|
/* Name: 64 bytes, zero-padded */
|
|
(void) MemoryOperationsHelper::Set(p, 0, UDPS_MAX_SIGNAL_NAME);
|
|
uint32 nameLen = static_cast<uint32>(signalInfos[i].name.Size());
|
|
if (nameLen >= UDPS_MAX_SIGNAL_NAME) {
|
|
nameLen = UDPS_MAX_SIGNAL_NAME - 1u;
|
|
}
|
|
(void) MemoryOperationsHelper::Copy(p, signalInfos[i].name.Buffer(), nameLen);
|
|
p += UDPS_MAX_SIGNAL_NAME;
|
|
|
|
/* Type code: 1 byte */
|
|
*p = TypeDescriptorToCode(signalInfos[i].type);
|
|
p += 1u;
|
|
|
|
/* Quant type: 1 byte */
|
|
*p = static_cast<uint8>(signalInfos[i].quantType);
|
|
p += 1u;
|
|
|
|
/* numDimensions: 1 byte */
|
|
*p = signalInfos[i].numDimensions;
|
|
p += 1u;
|
|
|
|
/* numRows: 4 bytes */
|
|
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].numRows, 4u);
|
|
p += 4u;
|
|
|
|
/* numCols: 4 bytes */
|
|
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].numCols, 4u);
|
|
p += 4u;
|
|
|
|
/* rangeMin: 8 bytes (float64) */
|
|
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].rangeMin, 8u);
|
|
p += 8u;
|
|
|
|
/* rangeMax: 8 bytes (float64) */
|
|
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].rangeMax, 8u);
|
|
p += 8u;
|
|
|
|
/* timeMode: 1 byte */
|
|
*p = static_cast<uint8>(signalInfos[i].timeMode);
|
|
p += 1u;
|
|
|
|
/* samplingRate: 8 bytes (float64) */
|
|
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].samplingRate, 8u);
|
|
p += 8u;
|
|
|
|
/* timeSignalIdx: 4 bytes */
|
|
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].timeSignalIdx, 4u);
|
|
p += 4u;
|
|
|
|
/* Unit: 32 bytes, zero-padded */
|
|
(void) MemoryOperationsHelper::Set(p, 0, UDPS_MAX_UNIT_LEN);
|
|
uint32 unitLen = static_cast<uint32>(signalInfos[i].unit.Size());
|
|
if (unitLen >= UDPS_MAX_UNIT_LEN) {
|
|
unitLen = UDPS_MAX_UNIT_LEN - 1u;
|
|
}
|
|
(void) MemoryOperationsHelper::Copy(p, signalInfos[i].unit.Buffer(), unitLen);
|
|
p += UDPS_MAX_UNIT_LEN;
|
|
|
|
payloadSize += UDPS_SIGNAL_DESC_SIZE;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void UDPStreamer::QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp) {
|
|
uint8 *dst = wireBuffer;
|
|
|
|
/* 8-byte packet timestamp */
|
|
(void) MemoryOperationsHelper::Copy(dst, ×tamp, UDPS_TIMESTAMP_BYTES);
|
|
dst += UDPS_TIMESTAMP_BYTES;
|
|
|
|
for (uint32 i = 0u; i < numSigs; i++) {
|
|
const uint8 *src = srcBuf + signalInfos[i].bufferOffset;
|
|
|
|
if (signalInfos[i].quantType == UDPStreamerQuantNone) {
|
|
/* Raw copy */
|
|
(void) MemoryOperationsHelper::Copy(dst, src, signalInfos[i].srcByteSize);
|
|
dst += signalInfos[i].srcByteSize;
|
|
}
|
|
else {
|
|
float64 rMin = signalInfos[i].rangeMin;
|
|
float64 rRange = signalInfos[i].rangeMax - rMin;
|
|
if (rRange == 0.0) {
|
|
rRange = 1.0; /* guard against divide-by-zero */
|
|
}
|
|
bool isSrcFloat32 = (signalInfos[i].type == Float32Bit);
|
|
uint32 nelems = signalInfos[i].numElements;
|
|
const uint8 *s = src;
|
|
|
|
for (uint32 e = 0u; e < nelems; e++) {
|
|
float64 rawVal = 0.0;
|
|
if (isSrcFloat32) {
|
|
float32 f32 = 0.0f;
|
|
(void) MemoryOperationsHelper::Copy(&f32, s, 4u);
|
|
rawVal = static_cast<float64>(f32);
|
|
s += 4u;
|
|
}
|
|
else {
|
|
(void) MemoryOperationsHelper::Copy(&rawVal, s, 8u);
|
|
s += 8u;
|
|
}
|
|
|
|
/* Normalize and clamp to [0.0, 1.0] */
|
|
float64 norm = (rawVal - rMin) / rRange;
|
|
if (norm < 0.0) { norm = 0.0; }
|
|
if (norm > 1.0) { norm = 1.0; }
|
|
|
|
switch (signalInfos[i].quantType) {
|
|
case UDPStreamerQuantUint8: {
|
|
uint8 q = static_cast<uint8>(norm * 255.0);
|
|
*dst = q;
|
|
dst += 1u;
|
|
break;
|
|
}
|
|
case UDPStreamerQuantInt8: {
|
|
int8 q = static_cast<int8>((norm * 254.0) - 127.0);
|
|
(void) MemoryOperationsHelper::Copy(dst, &q, 1u);
|
|
dst += 1u;
|
|
break;
|
|
}
|
|
case UDPStreamerQuantUint16: {
|
|
uint16 q = static_cast<uint16>(norm * 65535.0);
|
|
(void) MemoryOperationsHelper::Copy(dst, &q, 2u);
|
|
dst += 2u;
|
|
break;
|
|
}
|
|
case UDPStreamerQuantInt16: {
|
|
int16 q = static_cast<int16>((norm * 65534.0) - 32767.0);
|
|
(void) MemoryOperationsHelper::Copy(dst, &q, 2u);
|
|
dst += 2u;
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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;
|
|
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; }
|
|
else if (td == SignedInteger8Bit) { code = UDPS_TYPECODE_INT8; }
|
|
else if (td == UnsignedInteger16Bit) { code = UDPS_TYPECODE_UINT16; }
|
|
else if (td == SignedInteger16Bit) { code = UDPS_TYPECODE_INT16; }
|
|
else if (td == UnsignedInteger32Bit) { code = UDPS_TYPECODE_UINT32; }
|
|
else if (td == SignedInteger32Bit) { code = UDPS_TYPECODE_INT32; }
|
|
else if (td == UnsignedInteger64Bit) { code = UDPS_TYPECODE_UINT64; }
|
|
else if (td == SignedInteger64Bit) { code = UDPS_TYPECODE_INT64; }
|
|
else if (td == Float32Bit) { code = UDPS_TYPECODE_FLOAT32; }
|
|
else if (td == Float64Bit) { code = UDPS_TYPECODE_FLOAT64; }
|
|
return code;
|
|
}
|
|
|
|
uint16 UDPStreamer::GetPort() const {
|
|
return port;
|
|
}
|
|
|
|
uint32 UDPStreamer::GetMaxPayloadSize() const {
|
|
return maxPayloadSize;
|
|
}
|
|
|
|
bool UDPStreamer::IsClientConnected() const {
|
|
return clientConnected;
|
|
}
|
|
|
|
CLASS_REGISTER(UDPStreamer, "1.0")
|
|
|
|
} /* namespace MARTe */
|