FIrs
This commit is contained in:
Martino Ferrari
@@ -0,0 +1,995 @@
|
||||
/**
|
||||
* @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. */
|
||||
static const uint32 UDPS_RECV_TIMEOUT_MS = 5u;
|
||||
|
||||
/** 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) {
|
||||
/* --- Poll server socket for incoming client commands ---
|
||||
* Use select() directly so we get a silent timeout with no log spam.
|
||||
* MARTe2's Read(buf, size, timeout) calls recvfrom() and logs an error
|
||||
* on every timeout (EAGAIN from SO_RCVTIMEO). */
|
||||
uint8 cmdBuf[256u];
|
||||
Handle sockFd = serverSocket.GetReadHandle();
|
||||
fd_set rfds;
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(static_cast<int>(sockFd), &rfds);
|
||||
struct timeval tv;
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = static_cast<long>(UDPS_RECV_TIMEOUT_MS) * 1000L;
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
/* --- Wait for RT thread to post new data --- */
|
||||
/* ResetWait atomically lowers the barrier then waits, preventing missed posts */
|
||||
ErrorManagement::ErrorType waitErr =
|
||||
dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
|
||||
bool dataReady = (waitErr == ErrorManagement::NoError);
|
||||
|
||||
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 */
|
||||
Reference in New Issue
Block a user