diff --git a/Docs/UDPStreamer.md b/Docs/UDPStreamer.md index 1e773af..2124a55 100644 --- a/Docs/UDPStreamer.md +++ b/Docs/UDPStreamer.md @@ -66,34 +66,34 @@ thread. ### Top-level Parameters -| Parameter | Type | Default | Description | -|-----------|------|---------|-------------| -| `Port` | uint16 | 44500 | UDP server port | -| `MaxPayloadSize` | uint32 | 1400 | Max payload bytes per UDP datagram (min 18) | -| `CPUMask` | uint32 | 0 (any) | Background thread CPU affinity | -| `StackSize` | uint32 | 1 048 576 | Background thread stack size in bytes | +| Parameter | Type | Default | Description | +| ---------------- | ------ | --------- | ------------------------------------------- | +| `Port` | uint16 | 44500 | UDP server port | +| `MaxPayloadSize` | uint32 | 1400 | Max payload bytes per UDP datagram (min 18) | +| `CPUMask` | uint32 | 0 (any) | Background thread CPU affinity | +| `StackSize` | uint32 | 1 048 576 | Background thread stack size in bytes | ### Per-signal Parameters -| Parameter | Type | Default | Applies to | -|-----------|------|---------|------------| -| `Unit` | string | `""` | Any type — informational, forwarded to client in CONFIG | -| `RangeMin` | float64 | 0.0 | float32/float64 with `QuantizedType` | -| `RangeMax` | float64 | 1.0 | float32/float64 with `QuantizedType` | -| `QuantizedType` | string | `none` | float32/float64 only | -| `TimeMode` | string | `PacketTime` | Signals with `NumberOfElements > 1` | -| `TimeSignal` | string | — | Required when `TimeMode` ≠ `PacketTime` | -| `SamplingRate` | float64 | 0.0 | Required when `TimeMode` = `FirstSample` or `LastSample` | +| Parameter | Type | Default | Applies to | +| --------------- | ------- | ------------ | -------------------------------------------------------- | +| `Unit` | string | `""` | Any type — informational, forwarded to client in CONFIG | +| `RangeMin` | float64 | 0.0 | float32/float64 with `QuantizedType` | +| `RangeMax` | float64 | 1.0 | float32/float64 with `QuantizedType` | +| `QuantizedType` | string | `none` | float32/float64 only | +| `TimeMode` | string | `PacketTime` | Signals with `NumberOfElements > 1` | +| `TimeSignal` | string | — | Required when `TimeMode` ≠ `PacketTime` | +| `SamplingRate` | float64 | 0.0 | Required when `TimeMode` = `FirstSample` or `LastSample` | ### Quantization Types -| Value | Wire type | Bit depth | Notes | -|-------|-----------|-----------|-------| -| `none` | same as source | — | Raw copy, no quantization | -| `uint8` | uint8 | 8-bit | Maps `[RangeMin, RangeMax]` → `[0, 255]` | -| `int8` | int8 | 8-bit | Maps `[RangeMin, RangeMax]` → `[-127, 127]` | -| `uint16` | uint16 | 16-bit | Maps `[RangeMin, RangeMax]` → `[0, 65 535]` | -| `int16` | int16 | 16-bit | Maps `[RangeMin, RangeMax]` → `[-32 767, 32 767]` | +| Value | Wire type | Bit depth | Notes | +| -------- | -------------- | --------- | ------------------------------------------------- | +| `none` | same as source | — | Raw copy, no quantization | +| `uint8` | uint8 | 8-bit | Maps `[RangeMin, RangeMax]` → `[0, 255]` | +| `int8` | int8 | 8-bit | Maps `[RangeMin, RangeMax]` → `[-127, 127]` | +| `uint16` | uint16 | 16-bit | Maps `[RangeMin, RangeMax]` → `[0, 65 535]` | +| `int16` | int16 | 16-bit | Maps `[RangeMin, RangeMax]` → `[-32 767, 32 767]` | Quantization formula (unsigned, e.g. uint16): @@ -104,12 +104,12 @@ wire_value = (uint16)(normalized × 65535) ### Time Modes -| Value | Meaning | Requirements | -|-------|---------|--------------| -| `PacketTime` | The HRT counter captured at `Synchronise()` time is used as the packet timestamp. No per-signal time metadata. | — | -| `FullArray` | `TimeSignal` carries one timestamp per element (same `NumberOfElements`). | `TimeSignal` must have the same `NumberOfElements`. | -| `FirstSample` | `TimeSignal` is a scalar giving the timestamp of element `[0]`. Elements `[1..N-1]` are inferred at `1/SamplingRate` intervals. | Scalar `TimeSignal`; `SamplingRate > 0`. | -| `LastSample` | Same as `FirstSample` but `TimeSignal` is the timestamp of element `[N-1]`. | Scalar `TimeSignal`; `SamplingRate > 0`. | +| Value | Meaning | Requirements | +| ------------- | ------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------- | +| `PacketTime` | The HRT counter captured at `Synchronise()` time is used as the packet timestamp. No per-signal time metadata. | — | +| `FullArray` | `TimeSignal` carries one timestamp per element (same `NumberOfElements`). | `TimeSignal` must have the same `NumberOfElements`. | +| `FirstSample` | `TimeSignal` is a scalar giving the timestamp of element `[0]`. Elements `[1..N-1]` are inferred at `1/SamplingRate` intervals. | Scalar `TimeSignal`; `SamplingRate > 0`. | +| `LastSample` | Same as `FirstSample` but `TimeSignal` is the timestamp of element `[N-1]`. | Scalar `TimeSignal`; `SamplingRate > 0`. | --- diff --git a/README.md b/README.md index 8216563..d4afbf0 100644 --- a/README.md +++ b/README.md @@ -9,15 +9,15 @@ for control applications built with [MARTe2](https://vcis.f4e.europa.eu/marte2-d This repository integrates two complementary capabilities: -| Capability | Component | Purpose | -|---|---|---| -| **Signal streaming** | `UDPStreamer` DataSource | Continuously stream selected signals to a browser-based oscilloscope over UDP | -| **Signal debugging** | `DebugService` Interface | On-demand signal tracing, value forcing, and conditional breakpoints — zero application code changes required | -| **Sine generation** | `SineArrayGAM` | Generate continuous sine-wave arrays for testing and simulation | -| **Time stamping** | `TimeArrayGAM` | Provide time-reference arrays aligned to an RT cycle | -| **Log forwarding** | `TCPLogger` Interface | Forward `REPORT_ERROR` log events to TCP clients in real time | -| **Integrated client** | `Common/Client/go` | Go packages for UDPS protocol and WebSocket hub | -| **Debug web client** | `Client/debugger` | Browser-based debug UI communicating with `DebugService` | +| Capability | Component | Purpose | +| --------------------- | ------------------------ | ------------------------------------------------------------------------------------------------------------- | +| **Signal streaming** | `UDPStreamer` DataSource | Continuously stream selected signals to a browser-based oscilloscope over UDP | +| **Signal debugging** | `DebugService` Interface | On-demand signal tracing, value forcing, and conditional breakpoints — zero application code changes required | +| **Sine generation** | `SineArrayGAM` | Generate continuous sine-wave arrays for testing and simulation | +| **Time stamping** | `TimeArrayGAM` | Provide time-reference arrays aligned to an RT cycle | +| **Log forwarding** | `TCPLogger` Interface | Forward `REPORT_ERROR` log events to TCP clients in real time | +| **Integrated client** | `Common/Client/go` | Go packages for UDPS protocol and WebSocket hub | +| **Debug web client** | `Client/debugger` | Browser-based debug UI communicating with `DebugService` | --- @@ -49,9 +49,9 @@ MARTe_Integrated_components/ ### UDPStreamer DataSource -Streams MARTe2 signals over UDP using the UDPS binary protocol. Clients register by +Streams MARTe2 signals over UDP using the UDPS binary protocol. Clients register by sending a `CONNECT` packet; the server then sends `CONFIG` (signal metadata) and continuous -`DATA` packets. Features: +`DATA` packets. Features: - Optional 16-bit quantization (configurable per signal: `QuantizedType`) - Packed high-frequency bursts (`NumberOfElements > 1` with `SamplingRate`) @@ -61,8 +61,8 @@ See `Docs/UDPStreamer.md` and `Docs/Protocol.md`. ### SineArrayGAM -Generates a continuous float32 sine-wave array every RT cycle. Used as a signal -source for testing and demo applications. Configurable: `Frequency`, `Amplitude`, +Generates a continuous float32 sine-wave array every RT cycle. Used as a signal +source for testing and demo applications. Configurable: `Frequency`, `Amplitude`, `Phase`, `SamplingRate`, `NumberOfElements`. See `Docs/SineArrayGAM.md`. @@ -75,12 +75,13 @@ configured `SamplingRate`. ### DebugService Interface -Instruments a running MARTe2 application **without modifying its source code**. On +Instruments a running MARTe2 application **without modifying its source code**. On `Initialise()` it patches the `ClassRegistryDatabase` to wrap all standard -`MemoryMap*Broker` types. When `RealTimeApplication::ConfigureApplication()` runs +`MemoryMap*Broker` types. When `RealTimeApplication::ConfigureApplication()` runs afterward the application transparently uses the wrapped brokers. Capabilities accessible over TCP (port 8080 by default): + - `DISCOVER` — enumerate all signals with type and alias metadata - `TRACE` — enable/disable high-speed UDP telemetry per signal (with decimation) - `FORCE` / `UNFORCE` — inject persistent values into signals on the RT path @@ -98,7 +99,7 @@ See `Docs/DebugService.md`. ### TCPLogger Interface A `LoggerConsumerI` that forwards every MARTe2 `REPORT_ERROR` call to up to 8 TCP -clients on a configurable port. Works as a sidecar to `DebugService`. +clients on a configurable port. Works as a sidecar to `DebugService`. ### StreamHub Application @@ -116,7 +117,7 @@ See `Docs/StreamHub-UserGuide.md`, `Docs/StreamHub-API.md` and ### UDPS Protocol The `Common/UDP/UDPSProtocol.h` header defines the shared binary wire format used by -both `UDPStreamer` and `DebugService`. It is intentionally free of MARTe2-specific +both `UDPStreamer` and `DebugService`. It is intentionally free of MARTe2-specific dependencies so it can also be used by Go clients (via `Common/Client/go/udpsprotocol`). See `Docs/Protocol.md`. @@ -224,18 +225,18 @@ Open `http://localhost:9090`, explore the object tree, trace signals, force valu ## Documentation -| Document | Contents | -|---|---| -| `Docs/Protocol.md` | UDPS binary wire protocol specification | -| `Docs/UDPStreamer.md` | UDPStreamer DataSource configuration reference | -| `Docs/SineArrayGAM.md` | SineArrayGAM configuration reference | -| `Docs/DebugService.md` | DebugService TCP API and architecture | -| `Docs/Tutorial.md` | Step-by-step tutorial covering both components | -| `Docs/WebUI.md` | Web client user guide | -| `Docs/StreamHub-UserGuide.md` | StreamHub oscilloscope user guide (web + ImGui clients) | -| `Docs/StreamHub-API.md` | StreamHub WebSocket protocol (commands, events, binary frames) | -| `Docs/StreamHub-Developer.md` | StreamHub internals, threading, time base, build & E2E tests | -| `ARCHITECTURE.md` | System architecture overview | +| Document | Contents | +| ----------------------------- | -------------------------------------------------------------- | +| `Docs/Protocol.md` | UDPS binary wire protocol specification | +| `Docs/UDPStreamer.md` | UDPStreamer DataSource configuration reference | +| `Docs/SineArrayGAM.md` | SineArrayGAM configuration reference | +| `Docs/DebugService.md` | DebugService TCP API and architecture | +| `Docs/Tutorial.md` | Step-by-step tutorial covering both components | +| `Docs/WebUI.md` | Web client user guide | +| `Docs/StreamHub-UserGuide.md` | StreamHub oscilloscope user guide (web + ImGui clients) | +| `Docs/StreamHub-API.md` | StreamHub WebSocket protocol (commands, events, binary frames) | +| `Docs/StreamHub-Developer.md` | StreamHub internals, threading, time base, build & E2E tests | +| `ARCHITECTURE.md` | System architecture overview | --- diff --git a/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp b/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp index c3e2ec6..2436d41 100644 --- a/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp +++ b/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp @@ -21,6 +21,8 @@ * methods, such as those inline could be defined on the header file, instead. */ +#include "ErrorType.h" +#include "StreamString.h" #define DLL_API /*---------------------------------------------------------------------------*/ @@ -37,10 +39,7 @@ #include "EmbeddedThreadI.h" #include "GlobalObjectsDatabase.h" #include "HighResolutionTimer.h" -#include "MemoryMapSynchronisedOutputBroker.h" #include "MemoryOperationsHelper.h" -#include "Sleep.h" -#include "Threads.h" #include "UDPStreamer.h" /*---------------------------------------------------------------------------*/ @@ -52,7 +51,8 @@ namespace MARTe { /** Default port used when none is specified. */ static const uint16 UDPS_DEFAULT_PORT = 44500u; -/** Default data port offset: dataPort = port + this value when DataPort is not specified. */ +/** Default data port offset: dataPort = port + this value when DataPort is not + * specified. */ static const uint16 UDPS_DEFAULT_DATA_PORT_OFFSET = 1u; /** Maximum pending TCP connections on the listener backlog. */ @@ -80,1232 +80,1252 @@ 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; - publishMode = UDPStreamerPublishStrict; - minRefreshRate = 0.0; - flushPeriodTicks = 0u; - numSigs = 0u; - signalInfos = NULL_PTR(UDPStreamerSignalInfo *); - readyBuffer = NULL_PTR(uint8 *); - scratchBuffer = NULL_PTR(uint8 *); - wireBuffer = NULL_PTR(uint8 *); - totalSrcBytes = 0u; - totalWireBytes = 0u; - syncTimestamp = 0u; - packetCounter = 0u; - maxBatchCount = 0u; - singleCycleWireBytes = 0u; - lastPublishTs = 0u; - accumBuffer = NULL_PTR(uint8 *); - accumTimestamps = NULL_PTR(uint64 *); - accumFill = 0u; - readyTimestamps = NULL_PTR(uint64 *); - scratchTimestamps = NULL_PTR(uint64 *); - readyFill = 0u; - decimateRatio = 1u; - decimateCounter = 0u; +UDPStreamer::UDPStreamer() + : MemoryDataSourceI(), EmbeddedServiceMethodBinderI(), executor(*this) { + port = UDPS_DEFAULT_PORT; + maxPayloadSize = UDPS_DEFAULT_MAX_PAYLOAD; + cpuMask = 0xFFFFFFFFu; + stackSize = THREADS_DEFAULT_STACKSIZE; + publishMode = UDPStreamerPublishStrict; + minRefreshRate = 0.0; + flushPeriodTicks = 0u; + numSigs = 0u; + signalInfos = NULL_PTR(UDPStreamerSignalInfo *); + readyBuffer = NULL_PTR(uint8 *); + scratchBuffer = NULL_PTR(uint8 *); + wireBuffer = NULL_PTR(uint8 *); + totalSrcBytes = 0u; + totalWireBytes = 0u; + syncTimestamp = 0u; + packetCounter = 0u; + maxBatchCount = 0u; + singleCycleWireBytes = 0u; + lastPublishTs = 0u; + accumBuffer = NULL_PTR(uint8 *); + accumTimestamps = NULL_PTR(uint64 *); + accumFill = 0u; + readyTimestamps = NULL_PTR(uint64 *); + scratchTimestamps = NULL_PTR(uint64 *); + readyFill = 0u; + decimateRatio = 1u; + decimateCounter = 0u; - if (!dataSem.Create()) { - REPORT_ERROR(ErrorManagement::FatalError, "Could not create EventSem."); - } - bufMutex.Create(false); + 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(); + /* 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 (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."); + } } + } - (void) server.Stop(); + (void)server.Stop(); - HeapI *heapAccum = GlobalObjectsDatabase::Instance()->GetStandardHeap(); - if (accumBuffer != NULL_PTR(uint8 *)) { - heapAccum->Free(reinterpret_cast(accumBuffer)); - } - if (accumTimestamps != NULL_PTR(uint64 *)) { - delete[] accumTimestamps; - accumTimestamps = NULL_PTR(uint64 *); - } - if (readyTimestamps != NULL_PTR(uint64 *)) { - delete[] readyTimestamps; - readyTimestamps = NULL_PTR(uint64 *); - } - if (scratchTimestamps != NULL_PTR(uint64 *)) { - delete[] scratchTimestamps; - scratchTimestamps = NULL_PTR(uint64 *); - } + HeapI *heapAccum = GlobalObjectsDatabase::Instance()->GetStandardHeap(); + if (accumBuffer != NULL_PTR(uint8 *)) { + heapAccum->Free(reinterpret_cast(accumBuffer)); + } + if (accumTimestamps != NULL_PTR(uint64 *)) { + delete[] accumTimestamps; + accumTimestamps = NULL_PTR(uint64 *); + } + if (readyTimestamps != NULL_PTR(uint64 *)) { + delete[] readyTimestamps; + readyTimestamps = NULL_PTR(uint64 *); + } + if (scratchTimestamps != NULL_PTR(uint64 *)) { + delete[] scratchTimestamps; + scratchTimestamps = NULL_PTR(uint64 *); + } - if (signalInfos != NULL_PTR(UDPStreamerSignalInfo *)) { - delete[] signalInfos; - signalInfos = NULL_PTR(UDPStreamerSignalInfo *); - } + 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(readyBuffer)); - } - if (scratchBuffer != NULL_PTR(uint8 *)) { - heap->Free(reinterpret_cast(scratchBuffer)); - } - if (wireBuffer != NULL_PTR(uint8 *)) { - heap->Free(reinterpret_cast(wireBuffer)); - } + HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap(); + if (readyBuffer != NULL_PTR(uint8 *)) { + heap->Free(reinterpret_cast(readyBuffer)); + } + if (scratchBuffer != NULL_PTR(uint8 *)) { + heap->Free(reinterpret_cast(scratchBuffer)); + } + if (wireBuffer != NULL_PTR(uint8 *)) { + heap->Free(reinterpret_cast(wireBuffer)); + } - (void) dataSem.Close(); + (void)dataSem.Close(); } bool UDPStreamer::Initialise(StructuredDataI &data) { - bool ok = MemoryDataSourceI::Initialise(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(port)); - } - if (port <= 1024u) { - REPORT_ERROR(ErrorManagement::Warning, - "Port %u is in the privileged range (<= 1024).", - static_cast(port)); - } + if (ok) { + if (!data.Read("Port", port)) { + port = UDPS_DEFAULT_PORT; + REPORT_ERROR(ErrorManagement::Information, + "Port not specified; using default %u.", + static_cast(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 (port <= 1024u) { + REPORT_ERROR(ErrorManagement::Warning, + "Port %u is in the privileged range (<= 1024).", + static_cast(port)); } + } - 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("MaxPayloadSize", maxPayloadSize)) { + maxPayloadSize = UDPS_DEFAULT_MAX_PAYLOAD; + REPORT_ERROR(ErrorManagement::Information, + "MaxPayloadSize not specified; using default %u.", + maxPayloadSize); } - - 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; - } + if (maxPayloadSize < UDPS_MIN_PAYLOAD) { + REPORT_ERROR(ErrorManagement::ParametersError, + "MaxPayloadSize %u is too small (minimum %u).", + maxPayloadSize, UDPS_MIN_PAYLOAD); + ok = false; } + } - if (ok) { - StreamString publishStr = ""; - (void) data.Read("PublishingMode", publishStr); - if ((publishStr.Size() == 0u) || (publishStr == "Strict")) { - publishMode = UDPStreamerPublishStrict; - } - else if (publishStr == "Accumulate") { - publishMode = UDPStreamerPublishAccumulate; - } - else if (publishStr == "Decimate") { - publishMode = UDPStreamerPublishDecimate; - } - else { - REPORT_ERROR(ErrorManagement::ParametersError, - "Unknown PublishingMode '%s'. Allowed: Strict|Accumulate|Decimate.", - publishStr.Buffer()); - 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 && (publishMode == UDPStreamerPublishAccumulate)) { - /* MinRefreshRate controls the time-based flush: flush when - * (now - lastPublishTs) >= flushPeriodTicks, or when adding one more - * sample would overflow MaxPayloadSize. Whichever fires first. */ - if (!data.Read("MinRefreshRate", minRefreshRate) || (minRefreshRate <= 0.0)) { - REPORT_ERROR(ErrorManagement::ParametersError, - "MinRefreshRate > 0 is required when PublishingMode = Accumulate."); - ok = false; - } - else { - float64 hrtFreq = static_cast(HighResolutionTimer::Frequency()); - flushPeriodTicks = static_cast(hrtFreq / minRefreshRate); - REPORT_ERROR(ErrorManagement::Information, - "Accumulate mode: MinRefreshRate=%.1f Hz, flushPeriodTicks=%llu.", - minRefreshRate, - static_cast(flushPeriodTicks)); - } + if (ok) { + if (!data.Read("StackSize", stackSize)) { + stackSize = THREADS_DEFAULT_STACKSIZE; + REPORT_ERROR(ErrorManagement::Information, + "StackSize not specified; using MARTe2 default %u.", + stackSize); } - - if (ok && (publishMode == UDPStreamerPublishDecimate)) { - /* Ratio: send 1 packet every Ratio Synchronise() calls. */ - uint32 ratio = 0u; - if (!data.Read("Ratio", ratio) || (ratio == 0u)) { - REPORT_ERROR(ErrorManagement::ParametersError, - "Ratio >= 1 is required when PublishingMode = Decimate."); - ok = false; - } - else { - decimateRatio = ratio; - if (decimateRatio == 1u) { - REPORT_ERROR(ErrorManagement::Warning, - "Decimate mode with Ratio=1 is equivalent to Strict mode."); - } - REPORT_ERROR(ErrorManagement::Information, - "Decimate mode: Ratio=%u (1 packet per %u RT cycle(s)).", - decimateRatio, decimateRatio); - } + if (stackSize == 0u) { + REPORT_ERROR(ErrorManagement::ParametersError, "StackSize must be > 0."); + ok = false; } + } - if (ok) { - // 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(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); - } - } - uint32 clientTimeout = 0u; - if (data.Read("ClientTimeout", clientTimeout)) { - (void) serverCfg.Write("ClientTimeout", clientTimeout); - } - ok = server.Initialise(serverCfg); + if (ok) { + StreamString publishStr = ""; + (void)data.Read("PublishingMode", publishStr); + if ((publishStr.Size() == 0u) || (publishStr == "Strict")) { + publishMode = UDPStreamerPublishStrict; + } else if (publishStr == "Accumulate") { + publishMode = UDPStreamerPublishAccumulate; + } else if (publishStr == "Decimate") { + publishMode = UDPStreamerPublishDecimate; + } else { + REPORT_ERROR( + ErrorManagement::ParametersError, + "Unknown PublishingMode '%s'. Allowed: Strict|Accumulate|Decimate.", + publishStr.Buffer()); + ok = false; } + } - return ok; + if (ok && (publishMode == UDPStreamerPublishAccumulate)) { + /* MinRefreshRate controls the time-based flush: flush when + * (now - lastPublishTs) >= flushPeriodTicks, or when adding one more + * sample would overflow MaxPayloadSize. Whichever fires first. */ + if (!data.Read("MinRefreshRate", minRefreshRate) || + (minRefreshRate <= 0.0)) { + REPORT_ERROR( + ErrorManagement::ParametersError, + "MinRefreshRate > 0 is required when PublishingMode = Accumulate."); + ok = false; + } else { + float64 hrtFreq = static_cast(HighResolutionTimer::Frequency()); + flushPeriodTicks = static_cast(hrtFreq / minRefreshRate); + REPORT_ERROR( + ErrorManagement::Information, + "Accumulate mode: MinRefreshRate=%.1f Hz, flushPeriodTicks=%llu.", + minRefreshRate, static_cast(flushPeriodTicks)); + } + } + + if (ok && (publishMode == UDPStreamerPublishDecimate)) { + /* Ratio: send 1 packet every Ratio Synchronise() calls. */ + uint32 ratio = 0u; + if (!data.Read("Ratio", ratio) || (ratio == 0u)) { + REPORT_ERROR(ErrorManagement::ParametersError, + "Ratio >= 1 is required when PublishingMode = Decimate."); + ok = false; + } else { + decimateRatio = ratio; + if (decimateRatio == 1u) { + REPORT_ERROR( + ErrorManagement::Warning, + "Decimate mode with Ratio=1 is equivalent to Strict mode."); + } + REPORT_ERROR(ErrorManagement::Information, + "Decimate mode: Ratio=%u (1 packet per %u RT cycle(s)).", + decimateRatio, decimateRatio); + } + } + + if (ok) { + // 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(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); + } + StreamString iface; + if (data.Read("Interface", iface)) { + (void)serverCfg.Write("Interface", iface); + } else { + ok = false; + REPORT_ERROR(ErrorManagement::InitialisationError, + "Missing mandatory interface for multicasting"); + } + } + uint32 clientTimeout = 0u; + if (data.Read("ClientTimeout", clientTimeout)) { + (void)serverCfg.Write("ClientTimeout", clientTimeout); + } + ok = server.Initialise(serverCfg); + } + + return ok; } bool UDPStreamer::SetConfiguredDatabase(StructuredDataI &data) { - bool ok = MemoryDataSourceI::SetConfiguredDatabase(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) { - return false; + 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; } - numSigs = GetNumberOfSignals(); - if (numSigs == 0u) { + /* 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, - "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; + "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; } - 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. */ + /* TimeMode */ 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; - } + 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; + } } - 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); + /* 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; + } } - if (ok || true) { /* always attempt to restore navigation */ - (void) signalsDatabase.MoveToAncestor(1u); + /* 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; + } } - /* --- 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 */ + (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++) { + /* Initialise accumulated flag: false until pass 5 may flip it */ + signalInfos[i].accumulated = false; + + /* 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; } - bool found = false; - for (uint32 j = 0u; j < numSigs; j++) { - if (signalInfos[j].name == timeSignalNames[i]) { - signalInfos[i].timeSignalIdx = j; - found = true; - break; - } + } 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; } - if (!found) { - REPORT_ERROR(ErrorManagement::ParametersError, - "Signal %s: TimeSignal '%s' not found among declared signals.", - signalInfos[i].name.Buffer(), - timeSignalNames[i].Buffer()); - ok = false; + } + } + } + + /* --- Pass 5: Accumulate mode setup --- + * + * ALL signals (scalars and arrays alike) are tagged accumulated = true: + * one full snapshot (all elements) is captured and transmitted per RT + * cycle, for every cycle in the batch. This avoids silently discarding + * intermediate RT-cycle values for array ("passenger") signals — only the + * most recent slot used to be sent, whereas scalar signals always got a + * value from every slot. Scalars additionally get a FullArray time + * reference auto-assigned if a primary time signal exists. numCols / numRows + * are left at 1 for scalars — the actual per-packet element count is + * determined at runtime and transmitted as a 4-byte numSamples field in the + * DATA payload header. + * + * Compute singleCycleWireBytes (sum of all signals' wireByteSize, i.e. the + * bytes needed for one RT-cycle snapshot of every signal). Override + * totalWireBytes to the maximum possible DATA payload for wireBuffer + * allocation: 12 + maxBatchCount × singleCycleWireBytes. + */ + if (ok && (publishMode == UDPStreamerPublishAccumulate)) { + + /* Find primary time signal: prefer Unit="us"/"ns", fall back to first + * integer scalar */ + uint32 primaryTsIdx = UDPS_NO_TIME_SIGNAL; + for (uint32 i = 0u; i < numSigs && (primaryTsIdx == UDPS_NO_TIME_SIGNAL); + i++) { + if (signalInfos[i].numElements == 1u) { + if ((signalInfos[i].unit == "us") || (signalInfos[i].unit == "ns")) { + primaryTsIdx = i; } + } + } + if (primaryTsIdx == UDPS_NO_TIME_SIGNAL) { + for (uint32 i = 0u; i < numSigs && (primaryTsIdx == UDPS_NO_TIME_SIGNAL); + i++) { + if (signalInfos[i].numElements == 1u) { + TypeDescriptor td = signalInfos[i].type; + if ((td == UnsignedInteger32Bit) || (td == UnsignedInteger64Bit) || + (td == SignedInteger32Bit) || (td == SignedInteger64Bit)) { + primaryTsIdx = i; + } + } + } + } + if (primaryTsIdx != UDPS_NO_TIME_SIGNAL) { + REPORT_ERROR(ErrorManagement::Information, + "Accumulate: primary time signal '%s' (idx=%u).", + signalInfos[primaryTsIdx].name.Buffer(), primaryTsIdx); } - delete[] timeSignalNames; - timeSignalNames = NULL_PTR(StreamString *); - - /* --- Pass 4: validate time-signal dimensions and compute wire sizes --- */ - for (uint32 i = 0u; i < numSigs && ok; i++) { - /* Initialise accumulated flag: false until pass 5 may flip it */ - signalInfos[i].accumulated = false; - - /* 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; - } - } - } + /* Every signal (scalar or array) is accumulated: one full snapshot per + * RT cycle. Auto-assign FullArray time mode for scalars that had + * PacketTime. */ + singleCycleWireBytes = 0u; + for (uint32 i = 0u; i < numSigs; i++) { + signalInfos[i].accumulated = true; + singleCycleWireBytes += signalInfos[i].wireByteSize; + /* Auto-assign time reference for non-primary, non-time scalars */ + if ((signalInfos[i].numElements == 1u) && (i != primaryTsIdx) && + (primaryTsIdx != UDPS_NO_TIME_SIGNAL) && + (signalInfos[i].timeMode == UDPStreamerTimePacket)) { + signalInfos[i].timeMode = UDPStreamerTimeFullArray; + signalInfos[i].timeSignalIdx = primaryTsIdx; + } } - /* --- Pass 5: Accumulate mode setup --- - * - * ALL signals (scalars and arrays alike) are tagged accumulated = true: - * one full snapshot (all elements) is captured and transmitted per RT - * cycle, for every cycle in the batch. This avoids silently discarding - * intermediate RT-cycle values for array ("passenger") signals — only the - * most recent slot used to be sent, whereas scalar signals always got a - * value from every slot. Scalars additionally get a FullArray time - * reference auto-assigned if a primary time signal exists. numCols / numRows - * are left at 1 for scalars — the actual per-packet element count is - * determined at runtime and transmitted as a 4-byte numSamples field in the - * DATA payload header. - * - * Compute singleCycleWireBytes (sum of all signals' wireByteSize, i.e. the - * bytes needed for one RT-cycle snapshot of every signal). Override - * totalWireBytes to the maximum possible DATA payload for wireBuffer - * allocation: 12 + maxBatchCount × singleCycleWireBytes. - */ - if (ok && (publishMode == UDPStreamerPublishAccumulate)) { - - /* Find primary time signal: prefer Unit="us"/"ns", fall back to first integer scalar */ - uint32 primaryTsIdx = UDPS_NO_TIME_SIGNAL; - for (uint32 i = 0u; i < numSigs && (primaryTsIdx == UDPS_NO_TIME_SIGNAL); i++) { - if (signalInfos[i].numElements == 1u) { - if ((signalInfos[i].unit == "us") || (signalInfos[i].unit == "ns")) { - primaryTsIdx = i; - } - } - } - if (primaryTsIdx == UDPS_NO_TIME_SIGNAL) { - for (uint32 i = 0u; i < numSigs && (primaryTsIdx == UDPS_NO_TIME_SIGNAL); i++) { - if (signalInfos[i].numElements == 1u) { - TypeDescriptor td = signalInfos[i].type; - if ((td == UnsignedInteger32Bit) || (td == UnsignedInteger64Bit) || - (td == SignedInteger32Bit) || (td == SignedInteger64Bit)) { - primaryTsIdx = i; - } - } - } - } - if (primaryTsIdx != UDPS_NO_TIME_SIGNAL) { - REPORT_ERROR(ErrorManagement::Information, - "Accumulate: primary time signal '%s' (idx=%u).", - signalInfos[primaryTsIdx].name.Buffer(), primaryTsIdx); - } - - /* Every signal (scalar or array) is accumulated: one full snapshot per - * RT cycle. Auto-assign FullArray time mode for scalars that had - * PacketTime. */ - singleCycleWireBytes = 0u; - for (uint32 i = 0u; i < numSigs; i++) { - signalInfos[i].accumulated = true; - singleCycleWireBytes += signalInfos[i].wireByteSize; - /* Auto-assign time reference for non-primary, non-time scalars */ - if ((signalInfos[i].numElements == 1u) && - (i != primaryTsIdx) && (primaryTsIdx != UDPS_NO_TIME_SIGNAL) && - (signalInfos[i].timeMode == UDPStreamerTimePacket)) { - signalInfos[i].timeMode = UDPStreamerTimeFullArray; - signalInfos[i].timeSignalIdx = primaryTsIdx; - } - } - - if (singleCycleWireBytes == 0u) { - REPORT_ERROR(ErrorManagement::ParametersError, - "Accumulate mode: no signals found to accumulate."); - ok = false; - } - - if (ok) { - /* DATA payload: [8 HRT][4 numSamples][numSamples × singleCycle] */ - static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u; /* 12 bytes */ - if ((ACCUM_HEADER + singleCycleWireBytes) > maxPayloadSize) { - REPORT_ERROR(ErrorManagement::ParametersError, - "Accumulate mode: even a single sample (%u B) exceeds " - "MaxPayloadSize (%u B).", - ACCUM_HEADER + singleCycleWireBytes, - maxPayloadSize); - ok = false; - } - } - - if (ok) { - static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u; - maxBatchCount = (maxPayloadSize - ACCUM_HEADER) / singleCycleWireBytes; - /* Override totalWireBytes: size of the largest possible DATA payload */ - totalWireBytes = ACCUM_HEADER + maxBatchCount * singleCycleWireBytes; - REPORT_ERROR(ErrorManagement::Information, - "Accumulate mode: singleCycleWireBytes=%u, " - "maxBatchCount=%u, maxPayloadSize=%u, totalWireBytes=%u.", - singleCycleWireBytes, - maxBatchCount, maxPayloadSize, totalWireBytes); - } + if (singleCycleWireBytes == 0u) { + REPORT_ERROR(ErrorManagement::ParametersError, + "Accumulate mode: no signals found to accumulate."); + ok = false; } - return ok; + if (ok) { + /* DATA payload: [8 HRT][4 numSamples][numSamples × singleCycle] */ + static const uint32 ACCUM_HEADER = + UDPS_TIMESTAMP_BYTES + 4u; /* 12 bytes */ + if ((ACCUM_HEADER + singleCycleWireBytes) > maxPayloadSize) { + REPORT_ERROR(ErrorManagement::ParametersError, + "Accumulate mode: even a single sample (%u B) exceeds " + "MaxPayloadSize (%u B).", + ACCUM_HEADER + singleCycleWireBytes, maxPayloadSize); + ok = false; + } + } + + if (ok) { + static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u; + maxBatchCount = (maxPayloadSize - ACCUM_HEADER) / singleCycleWireBytes; + /* Override totalWireBytes: size of the largest possible DATA payload */ + totalWireBytes = ACCUM_HEADER + maxBatchCount * singleCycleWireBytes; + REPORT_ERROR(ErrorManagement::Information, + "Accumulate mode: singleCycleWireBytes=%u, " + "maxBatchCount=%u, maxPayloadSize=%u, totalWireBytes=%u.", + singleCycleWireBytes, maxBatchCount, maxPayloadSize, + totalWireBytes); + } + } + + return ok; } bool UDPStreamer::AllocateMemory() { - bool ok = MemoryDataSourceI::AllocateMemory(); - if (!ok) { - return false; + bool ok = MemoryDataSourceI::AllocateMemory(); + if (!ok) { + return false; + } + + /* stateMemorySize is populated by MemoryDataSourceI::AllocateMemory() */ + if (totalSrcBytes == 0u) { + totalSrcBytes = stateMemorySize; + } + + HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap(); + + /* In Accumulate mode, readyBuffer / scratchBuffer hold maxBatchCount + * consecutive snapshots instead of a single one. */ + /* HI-3: use 64-bit arithmetic to prevent overflow in maxBatchCount * + * totalSrcBytes */ + uint64 readyBufSize64 = (maxBatchCount > 0u) + ? (static_cast(maxBatchCount) * + static_cast(totalSrcBytes)) + : static_cast(totalSrcBytes); + if (readyBufSize64 > 0xFFFFFFFFu) { + REPORT_ERROR(ErrorManagement::FatalError, + "Accumulate buffer size overflow (maxBatchCount=%u * " + "totalSrcBytes=%u).", + maxBatchCount, totalSrcBytes); + return false; + } + uint32 readyBufSize = static_cast(readyBufSize64); + + /* readyBuffer: copy of signal memory shared with background thread */ + readyBuffer = reinterpret_cast(heap->Malloc(readyBufSize)); + if (readyBuffer == NULL_PTR(uint8 *)) { + REPORT_ERROR(ErrorManagement::FatalError, + "Could not allocate readyBuffer."); + return false; + } + (void)MemoryOperationsHelper::Set(readyBuffer, 0, readyBufSize); + + /* scratchBuffer: background-thread-private copy for serialization */ + scratchBuffer = reinterpret_cast(heap->Malloc(readyBufSize)); + if (scratchBuffer == NULL_PTR(uint8 *)) { + REPORT_ERROR(ErrorManagement::FatalError, + "Could not allocate scratchBuffer."); + return false; + } + (void)MemoryOperationsHelper::Set(scratchBuffer, 0, readyBufSize); + + /* wireBuffer: serialized/quantized payload for transmission */ + wireBuffer = reinterpret_cast(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(reinterpret_cast(addr) - memory); } + } - /* stateMemorySize is populated by MemoryDataSourceI::AllocateMemory() */ - if (totalSrcBytes == 0u) { - totalSrcBytes = stateMemorySize; + /* --- Accumulate-mode extra buffers --- */ + if (maxBatchCount > 0u) { + /* Linear fill buffer: RT thread writes one snapshot per slot + * (0..maxBatchCount-1) */ + uint32 accumBufSize = maxBatchCount * totalSrcBytes; + accumBuffer = reinterpret_cast(heap->Malloc(accumBufSize)); + if (accumBuffer == NULL_PTR(uint8 *)) { + REPORT_ERROR(ErrorManagement::FatalError, + "Could not allocate accumBuffer."); + return false; } + (void)MemoryOperationsHelper::Set(accumBuffer, 0, accumBufSize); - HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap(); - - /* In Accumulate mode, readyBuffer / scratchBuffer hold maxBatchCount consecutive - * snapshots instead of a single one. */ - /* HI-3: use 64-bit arithmetic to prevent overflow in maxBatchCount * totalSrcBytes */ - uint64 readyBufSize64 = (maxBatchCount > 0u) - ? (static_cast(maxBatchCount) * static_cast(totalSrcBytes)) - : static_cast(totalSrcBytes); - if (readyBufSize64 > 0xFFFFFFFFu) { - REPORT_ERROR(ErrorManagement::FatalError, - "Accumulate buffer size overflow (maxBatchCount=%u * totalSrcBytes=%u).", - maxBatchCount, totalSrcBytes); - return false; + /* Per-slot HRT timestamp arrays */ + accumTimestamps = new uint64[maxBatchCount]; + readyTimestamps = new uint64[maxBatchCount]; + scratchTimestamps = new uint64[maxBatchCount]; + if ((accumTimestamps == NULL_PTR(uint64 *)) || + (readyTimestamps == NULL_PTR(uint64 *)) || + (scratchTimestamps == NULL_PTR(uint64 *))) { + REPORT_ERROR(ErrorManagement::FatalError, + "Could not allocate timestamp arrays."); + return false; } - uint32 readyBufSize = static_cast(readyBufSize64); + uint32 tsBytes = maxBatchCount * static_cast(sizeof(uint64)); + (void)MemoryOperationsHelper::Set( + reinterpret_cast(accumTimestamps), 0, tsBytes); + (void)MemoryOperationsHelper::Set( + reinterpret_cast(readyTimestamps), 0, tsBytes); + (void)MemoryOperationsHelper::Set( + reinterpret_cast(scratchTimestamps), 0, tsBytes); - /* readyBuffer: copy of signal memory shared with background thread */ - readyBuffer = reinterpret_cast(heap->Malloc(readyBufSize)); - if (readyBuffer == NULL_PTR(uint8 *)) { - REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate readyBuffer."); - return false; - } - (void) MemoryOperationsHelper::Set(readyBuffer, 0, readyBufSize); + accumFill = 0u; + readyFill = 0u; - /* scratchBuffer: background-thread-private copy for serialization */ - scratchBuffer = reinterpret_cast(heap->Malloc(readyBufSize)); - if (scratchBuffer == NULL_PTR(uint8 *)) { - REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate scratchBuffer."); - return false; - } - (void) MemoryOperationsHelper::Set(scratchBuffer, 0, readyBufSize); + REPORT_ERROR(ErrorManagement::Information, + "Accumulate buffers: maxBatchCount=%u, accumBufSize=%u B, " + "readyBufSize=%u B.", + maxBatchCount, accumBufSize, readyBufSize); + } - /* wireBuffer: serialized/quantized payload for transmission */ - wireBuffer = reinterpret_cast(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(reinterpret_cast(addr) - memory); - } - } - - /* --- Accumulate-mode extra buffers --- */ - if (maxBatchCount > 0u) { - /* Linear fill buffer: RT thread writes one snapshot per slot (0..maxBatchCount-1) */ - uint32 accumBufSize = maxBatchCount * totalSrcBytes; - accumBuffer = reinterpret_cast(heap->Malloc(accumBufSize)); - if (accumBuffer == NULL_PTR(uint8 *)) { - REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate accumBuffer."); - return false; - } - (void) MemoryOperationsHelper::Set(accumBuffer, 0, accumBufSize); - - /* Per-slot HRT timestamp arrays */ - accumTimestamps = new uint64[maxBatchCount]; - readyTimestamps = new uint64[maxBatchCount]; - scratchTimestamps = new uint64[maxBatchCount]; - if ((accumTimestamps == NULL_PTR(uint64 *)) || - (readyTimestamps == NULL_PTR(uint64 *)) || - (scratchTimestamps == NULL_PTR(uint64 *))) { - REPORT_ERROR(ErrorManagement::FatalError, - "Could not allocate timestamp arrays."); - return false; - } - uint32 tsBytes = maxBatchCount * static_cast(sizeof(uint64)); - (void) MemoryOperationsHelper::Set( - reinterpret_cast(accumTimestamps), 0, tsBytes); - (void) MemoryOperationsHelper::Set( - reinterpret_cast(readyTimestamps), 0, tsBytes); - (void) MemoryOperationsHelper::Set( - reinterpret_cast(scratchTimestamps), 0, tsBytes); - - accumFill = 0u; - readyFill = 0u; - - REPORT_ERROR(ErrorManagement::Information, - "Accumulate buffers: maxBatchCount=%u, accumBufSize=%u B, readyBufSize=%u B.", - maxBatchCount, accumBufSize, readyBufSize); - } - - return true; + return true; } const char8 *UDPStreamer::GetBrokerName(StructuredDataI &data, const SignalDirection direction) { - const char8 *brokerName = ""; - if (direction == OutputSignals) { - brokerName = "MemoryMapSynchronisedOutputBroker"; - } - return brokerName; + const char8 *brokerName = ""; + if (direction == OutputSignals) { + brokerName = "MemoryMapSynchronisedOutputBroker"; + } + return brokerName; } bool UDPStreamer::PrepareNextState(const char8 *const currentStateName, const char8 *const nextStateName) { - bool ok = true; + bool ok = true; - ok = server.Start(); + ok = server.Start(); - /* 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(heap->Malloc(configBufSize)); - if (cfgBuf != NULL_PTR(uint8 *)) { - uint32 cfgPayloadSize = 0u; - if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) { - (void) server.SendConfig(cfgBuf, cfgPayloadSize); - } - else { - REPORT_ERROR(ErrorManagement::Warning, - "Could not build initial CONFIG payload."); - } - heap->Free(reinterpret_cast(cfgBuf)); - } - else { - REPORT_ERROR(ErrorManagement::Warning, - "Could not allocate CONFIG buffer."); - } + /* 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(heap->Malloc(configBufSize)); + if (cfgBuf != NULL_PTR(uint8 *)) { + uint32 cfgPayloadSize = 0u; + if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) { + (void)server.SendConfig(cfgBuf, cfgPayloadSize); + } else { + REPORT_ERROR(ErrorManagement::Warning, + "Could not build initial CONFIG payload."); + } + heap->Free(reinterpret_cast(cfgBuf)); + } else { + REPORT_ERROR(ErrorManagement::Warning, + "Could not allocate CONFIG buffer."); } + } - /* Initialise the flush timestamp so the first Accumulate flush is deferred - * until MinRefreshRate elapses (not immediately on the first Synchronise). */ - if (ok && (publishMode == UDPStreamerPublishAccumulate)) { - lastPublishTs = HighResolutionTimer::Counter(); - accumFill = 0u; - readyFill = 0u; + /* Initialise the flush timestamp so the first Accumulate flush is deferred + * until MinRefreshRate elapses (not immediately on the first Synchronise). */ + if (ok && (publishMode == UDPStreamerPublishAccumulate)) { + lastPublishTs = HighResolutionTimer::Counter(); + accumFill = 0u; + readyFill = 0u; + } + + /* Start the background thread (idempotent; shared by both modes) */ + 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."); } + } - /* Start the background thread (idempotent; shared by both modes) */ - 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; + return ok; } bool UDPStreamer::Synchronise() { - /* Capture HRT timestamp as early as possible. */ - uint64 ts = HighResolutionTimer::Counter(); + /* Capture HRT timestamp as early as possible. */ + uint64 ts = HighResolutionTimer::Counter(); - if (publishMode == UDPStreamerPublishAccumulate) { - /* --- Accumulate path --- - * - * Append this snapshot to the linear accumulation buffer, then check - * the two flush conditions (from the user spec): - * - * (a) size: accumulate_size + next_sample_size >= MaxPayloadSize - * (adding one more would overflow the UDP datagram) - * (b) time: expected_next_cycle_time - lastPublishTs >= flushPeriodTicks - * approximated as: ts - lastPublishTs >= flushPeriodTicks - * - * When either fires, the completed batch is promoted to readyBuffer / - * readyTimestamps and dataSem is posted. The background thread sends - * the ready batch without any additional timer check. */ - bufMutex.FastLock(TTInfiniteWait); - /* HI-3: if accumFill reached maxBatchCount, force-flush before writing */ - if (accumFill >= maxBatchCount) { - uint32 filled = accumFill; - (void) MemoryOperationsHelper::Copy( - readyBuffer, accumBuffer, filled * totalSrcBytes); - (void) MemoryOperationsHelper::Copy( - reinterpret_cast(readyTimestamps), - reinterpret_cast(accumTimestamps), - filled * static_cast(sizeof(uint64))); - readyFill = filled; - accumFill = 0u; - lastPublishTs = ts; - bufMutex.FastUnLock(); - (void) dataSem.Post(); - bufMutex.FastLock(TTInfiniteWait); - } - uint8 *slot = accumBuffer + (accumFill * totalSrcBytes); - (void) MemoryOperationsHelper::Copy(slot, memory, totalSrcBytes); - accumTimestamps[accumFill] = ts; - accumFill++; - uint32 filled = accumFill; - bufMutex.FastUnLock(); - - /* Check flush conditions (volatile read of lastPublishTs is safe on x86). */ - static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u; /* 12 bytes */ - uint32 curPayload = ACCUM_HEADER + filled * singleCycleWireBytes; - uint32 nextPayload = curPayload + singleCycleWireBytes; - bool sizeCondition = (nextPayload >= maxPayloadSize); - bool timeCondition = ((ts - lastPublishTs) >= flushPeriodTicks); - - if (sizeCondition || timeCondition) { - bufMutex.FastLock(TTInfiniteWait); - (void) MemoryOperationsHelper::Copy( - readyBuffer, accumBuffer, filled * totalSrcBytes); - (void) MemoryOperationsHelper::Copy( - reinterpret_cast(readyTimestamps), - reinterpret_cast(accumTimestamps), - filled * static_cast(sizeof(uint64))); - readyFill = filled; - accumFill = 0u; - bufMutex.FastUnLock(); - - /* Reset the time-based deadline (volatile write). */ - lastPublishTs = ts; - (void) dataSem.Post(); - } - } - else if (publishMode == UDPStreamerPublishDecimate) { - /* --- Decimate path --- - * Post dataSem only every decimateRatio calls. */ - decimateCounter++; - if (decimateCounter >= decimateRatio) { - decimateCounter = 0u; - bufMutex.FastLock(TTInfiniteWait); - (void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes); - syncTimestamp = ts; - bufMutex.FastUnLock(); - (void) dataSem.Post(); - } - } - else { - /* --- Strict path: post every call --- */ - bufMutex.FastLock(TTInfiniteWait); - (void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes); - syncTimestamp = ts; - bufMutex.FastUnLock(); - (void) dataSem.Post(); + if (publishMode == UDPStreamerPublishAccumulate) { + /* --- Accumulate path --- + * + * Append this snapshot to the linear accumulation buffer, then check + * the two flush conditions (from the user spec): + * + * (a) size: accumulate_size + next_sample_size >= MaxPayloadSize + * (adding one more would overflow the UDP datagram) + * (b) time: expected_next_cycle_time - lastPublishTs >= flushPeriodTicks + * approximated as: ts - lastPublishTs >= flushPeriodTicks + * + * When either fires, the completed batch is promoted to readyBuffer / + * readyTimestamps and dataSem is posted. The background thread sends + * the ready batch without any additional timer check. */ + bufMutex.FastLock(TTInfiniteWait); + /* HI-3: if accumFill reached maxBatchCount, force-flush before writing */ + if (accumFill >= maxBatchCount) { + uint32 filled = accumFill; + (void)MemoryOperationsHelper::Copy(readyBuffer, accumBuffer, + filled * totalSrcBytes); + (void)MemoryOperationsHelper::Copy( + reinterpret_cast(readyTimestamps), + reinterpret_cast(accumTimestamps), + filled * static_cast(sizeof(uint64))); + readyFill = filled; + accumFill = 0u; + lastPublishTs = ts; + bufMutex.FastUnLock(); + (void)dataSem.Post(); + bufMutex.FastLock(TTInfiniteWait); } + uint8 *slot = accumBuffer + (accumFill * totalSrcBytes); + (void)MemoryOperationsHelper::Copy(slot, memory, totalSrcBytes); + accumTimestamps[accumFill] = ts; + accumFill++; + uint32 filled = accumFill; + bufMutex.FastUnLock(); - return true; + /* Check flush conditions (volatile read of lastPublishTs is safe on x86). + */ + static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u; /* 12 bytes */ + uint32 curPayload = ACCUM_HEADER + filled * singleCycleWireBytes; + uint32 nextPayload = curPayload + singleCycleWireBytes; + bool sizeCondition = (nextPayload >= maxPayloadSize); + bool timeCondition = ((ts - lastPublishTs) >= flushPeriodTicks); + + if (sizeCondition || timeCondition) { + bufMutex.FastLock(TTInfiniteWait); + (void)MemoryOperationsHelper::Copy(readyBuffer, accumBuffer, + filled * totalSrcBytes); + (void)MemoryOperationsHelper::Copy( + reinterpret_cast(readyTimestamps), + reinterpret_cast(accumTimestamps), + filled * static_cast(sizeof(uint64))); + readyFill = filled; + accumFill = 0u; + bufMutex.FastUnLock(); + + /* Reset the time-based deadline (volatile write). */ + lastPublishTs = ts; + (void)dataSem.Post(); + } + } else if (publishMode == UDPStreamerPublishDecimate) { + /* --- Decimate path --- + * Post dataSem only every decimateRatio calls. */ + decimateCounter++; + if (decimateCounter >= decimateRatio) { + decimateCounter = 0u; + bufMutex.FastLock(TTInfiniteWait); + (void)MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes); + syncTimestamp = ts; + bufMutex.FastUnLock(); + (void)dataSem.Post(); + } + } else { + /* --- Strict path: post every call --- */ + bufMutex.FastLock(TTInfiniteWait); + (void)MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes); + syncTimestamp = ts; + bufMutex.FastUnLock(); + (void)dataSem.Post(); + } + + return true; } ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) { - ErrorManagement::ErrorType ret = ErrorManagement::NoError; + ErrorManagement::ErrorType ret = ErrorManagement::NoError; - if (info.GetStage() == ExecutionInfo::StartupStage) { - const char8 *modeStr = "Strict"; - if (publishMode == UDPStreamerPublishAccumulate) { modeStr = "Accumulate"; } - else if (publishMode == UDPStreamerPublishDecimate) { modeStr = "Decimate"; } - REPORT_ERROR(ErrorManagement::Information, - "UDPStreamer background thread started (port %u, mode %s).", - static_cast(port), modeStr); + if (info.GetStage() == ExecutionInfo::StartupStage) { + const char8 *modeStr = "Strict"; + if (publishMode == UDPStreamerPublishAccumulate) { + modeStr = "Accumulate"; + } else if (publishMode == UDPStreamerPublishDecimate) { + modeStr = "Decimate"; } + REPORT_ERROR(ErrorManagement::Information, + "UDPStreamer background thread started (port %u, mode %s).", + static_cast(port), modeStr); + } - 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); + 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 for incoming control commands (CONNECT / DISCONNECT / ACK) --- */ - server.ServiceClients(); + /* --- Poll for incoming control commands (CONNECT / DISCONNECT / ACK) --- + */ + server.ServiceClients(); - 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. */ - if (publishMode == UDPStreamerPublishAccumulate) { - /* --- Accumulate batch send --- */ - uint32 fill = 0u; - bufMutex.FastLock(TTInfiniteWait); - fill = readyFill; - if (fill > 0u) { - (void) MemoryOperationsHelper::Copy( - scratchBuffer, readyBuffer, fill * totalSrcBytes); - (void) MemoryOperationsHelper::Copy( - reinterpret_cast(scratchTimestamps), - reinterpret_cast(readyTimestamps), - fill * static_cast(sizeof(uint64))); - } - bufMutex.FastUnLock(); - - if (fill > 0u) { - SerializeAccumulated(scratchBuffer, scratchTimestamps, fill); - uint32 sendBytes = UDPS_TIMESTAMP_BYTES + 4u + - fill * singleCycleWireBytes; - packetCounter++; - if (!server.SendData(packetCounter, wireBuffer, sendBytes)) { - REPORT_ERROR(ErrorManagement::Warning, - "Failed to send Accumulate DATA packet (counter=%u).", - packetCounter); - } - } - } - else { - /* --- Single-snapshot send (Strict or Decimate) --- */ - uint64 ts = 0u; - bufMutex.FastLock(TTInfiniteWait); - (void) MemoryOperationsHelper::Copy( - scratchBuffer, readyBuffer, totalSrcBytes); - ts = syncTimestamp; - bufMutex.FastUnLock(); - - QuantizeAndSerialize(scratchBuffer, ts); - - packetCounter++; - if (!server.SendData(packetCounter, wireBuffer, totalWireBytes)) { - REPORT_ERROR(ErrorManagement::Warning, - "Failed to send DATA packet (counter=%u).", - packetCounter); - } - } + 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. */ + if (publishMode == UDPStreamerPublishAccumulate) { + /* --- Accumulate batch send --- */ + uint32 fill = 0u; + bufMutex.FastLock(TTInfiniteWait); + fill = readyFill; + if (fill > 0u) { + (void)MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer, + fill * totalSrcBytes); + (void)MemoryOperationsHelper::Copy( + reinterpret_cast(scratchTimestamps), + reinterpret_cast(readyTimestamps), + fill * static_cast(sizeof(uint64))); } - } + bufMutex.FastUnLock(); - if (info.GetStage() == ExecutionInfo::TerminationStage) { - (void) server.Stop(); - REPORT_ERROR(ErrorManagement::Information, - "UDPStreamer background thread terminated."); - } + if (fill > 0u) { + SerializeAccumulated(scratchBuffer, scratchTimestamps, fill); + uint32 sendBytes = + UDPS_TIMESTAMP_BYTES + 4u + fill * singleCycleWireBytes; + packetCounter++; + if (!server.SendData(packetCounter, wireBuffer, sendBytes)) { + REPORT_ERROR(ErrorManagement::Warning, + "Failed to send Accumulate DATA packet (counter=%u).", + packetCounter); + } + } + } else { + /* --- Single-snapshot send (Strict or Decimate) --- */ + uint64 ts = 0u; + bufMutex.FastLock(TTInfiniteWait); + (void)MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer, + totalSrcBytes); + ts = syncTimestamp; + bufMutex.FastUnLock(); - return ret; + QuantizeAndSerialize(scratchBuffer, ts); + + packetCounter++; + if (!server.SendData(packetCounter, wireBuffer, totalWireBytes)) { + REPORT_ERROR(ErrorManagement::Warning, + "Failed to send DATA packet (counter=%u).", + packetCounter); + } + } + } + } + + if (info.GetStage() == ExecutionInfo::TerminationStage) { + (void)server.Stop(); + REPORT_ERROR(ErrorManagement::Information, + "UDPStreamer background thread terminated."); + } + + return ret; } void UDPStreamer::SerializeAccumulated(const uint8 *src, - const uint64 *timestamps, - uint32 numSamples) { - /* Wire layout (Accumulate mode DATA payload): - * [8 bytes] : HRT of slot 0 (oldest sample) - * [4 bytes] : numSamples (uint32, little-endian) - * for each signal : numSamples snapshots in order (slot 0 = oldest), - * each snapshot holding all of the signal's elements - * (1 for scalars, numElements for arrays) - */ - uint8 *dst = wireBuffer; + const uint64 *timestamps, + uint32 numSamples) { + /* Wire layout (Accumulate mode DATA payload): + * [8 bytes] : HRT of slot 0 (oldest sample) + * [4 bytes] : numSamples (uint32, little-endian) + * for each signal : numSamples snapshots in order (slot 0 = oldest), + * each snapshot holding all of the signal's elements + * (1 for scalars, numElements for arrays) + */ + uint8 *dst = wireBuffer; - /* 8-byte packet-level HRT timestamp = timestamp of the first (oldest) sample */ - (void) MemoryOperationsHelper::Copy(dst, ×tamps[0u], UDPS_TIMESTAMP_BYTES); - dst += UDPS_TIMESTAMP_BYTES; + /* 8-byte packet-level HRT timestamp = timestamp of the first (oldest) sample + */ + (void)MemoryOperationsHelper::Copy(dst, ×tamps[0u], + UDPS_TIMESTAMP_BYTES); + dst += UDPS_TIMESTAMP_BYTES; - /* 4-byte sample count */ - (void) MemoryOperationsHelper::Copy(dst, &numSamples, 4u); - dst += 4u; + /* 4-byte sample count */ + (void)MemoryOperationsHelper::Copy(dst, &numSamples, 4u); + dst += 4u; - for (uint32 i = 0u; i < numSigs; i++) { - const uint32 nelems = signalInfos[i].numElements; - const bool isSrcFloat32 = (signalInfos[i].type == Float32Bit); - const float64 rMin = signalInfos[i].rangeMin; - float64 rRange = signalInfos[i].rangeMax - rMin; - if (rRange == 0.0) { rRange = 1.0; } - - /* Pack one snapshot (all elements) from each slot, in order */ - for (uint32 k = 0u; k < numSamples; k++) { - const uint8 *slotSrc = src + (k * totalSrcBytes) + signalInfos[i].bufferOffset; - - if (signalInfos[i].quantType == UDPStreamerQuantNone) { - (void) MemoryOperationsHelper::Copy(dst, slotSrc, signalInfos[i].srcByteSize); - dst += signalInfos[i].srcByteSize; - } - else { - const uint8 *s = slotSrc; - 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(f32); - s += 4u; - } - else { - (void) MemoryOperationsHelper::Copy(&rawVal, s, 8u); - s += 8u; - } - 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(norm * 255.0); - *dst = q; dst += 1u; - break; - } - case UDPStreamerQuantInt8: { - int8 q = static_cast((norm * 254.0) - 127.0); - (void) MemoryOperationsHelper::Copy(dst, &q, 1u); - dst += 1u; - break; - } - case UDPStreamerQuantUint16: { - uint16 q = static_cast(norm * 65535.0); - (void) MemoryOperationsHelper::Copy(dst, &q, 2u); - dst += 2u; - break; - } - case UDPStreamerQuantInt16: { - int16 q = static_cast((norm * 65534.0) - 32767.0); - (void) MemoryOperationsHelper::Copy(dst, &q, 2u); - dst += 2u; - break; - } - default: - break; - } - } - } - } + for (uint32 i = 0u; i < numSigs; i++) { + const uint32 nelems = signalInfos[i].numElements; + const bool isSrcFloat32 = (signalInfos[i].type == Float32Bit); + const float64 rMin = signalInfos[i].rangeMin; + float64 rRange = signalInfos[i].rangeMax - rMin; + if (rRange == 0.0) { + rRange = 1.0; } + + /* Pack one snapshot (all elements) from each slot, in order */ + for (uint32 k = 0u; k < numSamples; k++) { + const uint8 *slotSrc = + src + (k * totalSrcBytes) + signalInfos[i].bufferOffset; + + if (signalInfos[i].quantType == UDPStreamerQuantNone) { + (void)MemoryOperationsHelper::Copy(dst, slotSrc, + signalInfos[i].srcByteSize); + dst += signalInfos[i].srcByteSize; + } else { + const uint8 *s = slotSrc; + 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(f32); + s += 4u; + } else { + (void)MemoryOperationsHelper::Copy(&rawVal, s, 8u); + s += 8u; + } + 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(norm * 255.0); + *dst = q; + dst += 1u; + break; + } + case UDPStreamerQuantInt8: { + int8 q = static_cast((norm * 254.0) - 127.0); + (void)MemoryOperationsHelper::Copy(dst, &q, 1u); + dst += 1u; + break; + } + case UDPStreamerQuantUint16: { + uint16 q = static_cast(norm * 65535.0); + (void)MemoryOperationsHelper::Copy(dst, &q, 2u); + dst += 2u; + break; + } + case UDPStreamerQuantInt16: { + int16 q = static_cast((norm * 65534.0) - 32767.0); + (void)MemoryOperationsHelper::Copy(dst, &q, 2u); + dst += 2u; + break; + } + default: + break; + } + } + } + } + } } - -bool UDPStreamer::BuildConfigPayload(uint8 *buf, - uint32 bufSize, +bool UDPStreamer::BuildConfigPayload(uint8 *buf, uint32 bufSize, uint32 &payloadSize) { - payloadSize = 0u; + payloadSize = 0u; - /* 4 bytes: number of signals */ - if ((payloadSize + 4u) > bufSize) { - return false; - } - (void) MemoryOperationsHelper::Copy(buf + payloadSize, &numSigs, 4u); - payloadSize += 4u; + /* 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(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(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(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(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; + for (uint32 i = 0u; i < numSigs; i++) { + if ((payloadSize + UDPS_SIGNAL_DESC_SIZE) > bufSize) { + return false; } - /* 1 byte: publishing mode (so clients can parse DATA payloads correctly) */ - if ((payloadSize + 1u) > bufSize) { - return false; - } - buf[payloadSize] = static_cast(publishMode); - payloadSize += 1u; + uint8 *p = buf + payloadSize; - return true; + /* Name: 64 bytes, zero-padded */ + (void)MemoryOperationsHelper::Set(p, 0, UDPS_MAX_SIGNAL_NAME); + uint32 nameLen = static_cast(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(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(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(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; + } + + /* 1 byte: publishing mode (so clients can parse DATA payloads correctly) */ + if ((payloadSize + 1u) > bufSize) { + return false; + } + buf[payloadSize] = static_cast(publishMode); + payloadSize += 1u; + + return true; } void UDPStreamer::QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp) { - uint8 *dst = wireBuffer; + uint8 *dst = wireBuffer; - /* 8-byte packet timestamp */ - (void) MemoryOperationsHelper::Copy(dst, ×tamp, UDPS_TIMESTAMP_BYTES); - dst += UDPS_TIMESTAMP_BYTES; + /* 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; + 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; + 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(f32); + s += 4u; + } else { + (void)MemoryOperationsHelper::Copy(&rawVal, s, 8u); + s += 8u; } - 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(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(norm * 255.0); - *dst = q; - dst += 1u; - break; - } - case UDPStreamerQuantInt8: { - int8 q = static_cast((norm * 254.0) - 127.0); - (void) MemoryOperationsHelper::Copy(dst, &q, 1u); - dst += 1u; - break; - } - case UDPStreamerQuantUint16: { - uint16 q = static_cast(norm * 65535.0); - (void) MemoryOperationsHelper::Copy(dst, &q, 2u); - dst += 2u; - break; - } - case UDPStreamerQuantInt16: { - int16 q = static_cast((norm * 65534.0) - 32767.0); - (void) MemoryOperationsHelper::Copy(dst, &q, 2u); - dst += 2u; - break; - } - default: - break; - } - } + /* 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(norm * 255.0); + *dst = q; + dst += 1u; + break; + } + case UDPStreamerQuantInt8: { + int8 q = static_cast((norm * 254.0) - 127.0); + (void)MemoryOperationsHelper::Copy(dst, &q, 1u); + dst += 1u; + break; + } + case UDPStreamerQuantUint16: { + uint16 q = static_cast(norm * 65535.0); + (void)MemoryOperationsHelper::Copy(dst, &q, 2u); + dst += 2u; + break; + } + case UDPStreamerQuantInt16: { + int16 q = static_cast((norm * 65534.0) - 32767.0); + (void)MemoryOperationsHelper::Copy(dst, &q, 2u); + dst += 2u; + break; + } + default: + break; + } + } } + } } 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; + 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; -} +uint16 UDPStreamer::GetPort() const { return port; } -uint32 UDPStreamer::GetMaxPayloadSize() const { - return maxPayloadSize; -} +uint32 UDPStreamer::GetMaxPayloadSize() const { return maxPayloadSize; } -bool UDPStreamer::IsClientConnected() const { - return server.HasClients(); -} +bool UDPStreamer::IsClientConnected() const { return server.HasClients(); } -bool UDPStreamer::IsMulticast() const { - return server.IsMulticast(); -} +bool UDPStreamer::IsMulticast() const { return server.IsMulticast(); } CLASS_REGISTER(UDPStreamer, "1.0") diff --git a/Source/Components/DataSources/UDPStreamer/UDPStreamer.h b/Source/Components/DataSources/UDPStreamer/UDPStreamer.h index 20776cd..9606f5b 100644 --- a/Source/Components/DataSources/UDPStreamer/UDPStreamer.h +++ b/Source/Components/DataSources/UDPStreamer/UDPStreamer.h @@ -140,16 +140,17 @@ struct UDPStreamerSignalInfo { * fragmented into multiple datagrams if payload exceeds MaxPayloadSize). * * @par Top-level configuration parameters - * | Parameter | Type | Default | Description | - * |-----------------|---------|---------|-------------| - * | Port | uint16 | 44500 | TCP control port (multicast) or UDP server port (unicast). Values ≤ 1024 produce a warning. | - * | MulticastGroup | string | *(absent)* | **Enables multicast mode.** IPv4 multicast address, e.g. `"239.0.0.1"`. Must be in 224.0.0.0/4. Absent or empty = unicast. | - * | DataPort | uint16 | Port+1 | UDP port for multicast DATA datagrams. Ignored in unicast mode. Must be non-zero and differ from Port. | - * | MaxPayloadSize | uint32 | 1400 | Maximum bytes of signal payload per UDP datagram (excluding the 17-byte header). Larger signals are fragmented. | - * | PublishingMode | string | Strict | `Strict`: send one packet every Synchronise() call. `Auto`: rate-limited; flush only when MinRefreshRate interval has elapsed. | - * | MinRefreshRate | float64 | — | Required when PublishingMode = Auto. Flush frequency in Hz (e.g. 120.0). | - * | MaxBatchSize | uint32 | 1 | Optional when PublishingMode = Auto. Number of RT cycles to accumulate before flushing one packet. Scalar signals are expanded to arrays of MaxBatchSize elements; the first scalar with Unit="us" or "ns" is auto-promoted as the per-sample FullArray timestamp reference for all other scalars. When omitted or 1, the most-recent single value is sent at MinRefreshRate. | - * | CPUMask | uint32 | 0xFFFFFFFF | CPU affinity bitmask for the background thread. | + * | Parameter | Type | Default | Description | + * |-----------------|---------|------------------|-------------| + * | Port | uint16 | 44500 | TCP control port (multicast) or UDP server port (unicast). Values ≤ 1024 produce a warning. | + * | MulticastGroup | string | *(absent)* | **Enables multicast mode.** IPv4 multicast address, e.g. `"239.0.0.1"`. Must be in 224.0.0.0/4. Absent or empty = unicast. | + * | Interface | string | *(absent)* | Multicast binded interface **ONLY FOR MULTICAST** | + * | DataPort | uint16 | Port+1 | UDP port for multicast DATA datagrams. Ignored in unicast mode. Must be non-zero and differ from Port. | + * | MaxPayloadSize | uint32 | 1400 | Maximum bytes of signal payload per UDP datagram (excluding the 17-byte header). Larger signals are fragmented. | + * | PublishingMode | string | Strict | `Strict`: send one packet every Synchronise() call. `Auto`: rate-limited; flush only when MinRefreshRate interval has elapsed. | + * | MinRefreshRate | float64 | — | Required when PublishingMode = Auto. Flush frequency in Hz (e.g. 120.0). | + * | MaxBatchSize | uint32 | 1 | Optional when PublishingMode = Auto. Number of RT cycles to accumulate before flushing one packet. Scalar signals are expanded to arrays of MaxBatchSize elements; the first scalar with Unit="us" or "ns" is auto-promoted as the per-sample FullArray timestamp reference for all other scalars. When omitted or 1, the most-recent single value is sent at MinRefreshRate. | + * | CPUMask | uint32 | 0xFFFFFFFF | CPU affinity bitmask for the background thread. | * | StackSize | uint32 | (MARTe2 default) | Stack size in bytes for the background thread. | * * @par Per-signal configuration parameters diff --git a/Source/Components/Interfaces/UDPStream/UDPSServer.cpp b/Source/Components/Interfaces/UDPStream/UDPSServer.cpp index 84ed738..0a0279e 100644 --- a/Source/Components/Interfaces/UDPStream/UDPSServer.cpp +++ b/Source/Components/Interfaces/UDPStream/UDPSServer.cpp @@ -6,12 +6,13 @@ #include "UDPSServer.h" #include "AdvancedErrorManagement.h" +#include "ErrorType.h" #include "HighResolutionTimer.h" #include "MemoryOperationsHelper.h" #include "StreamString.h" -#include #include +#include namespace MARTe { @@ -20,83 +21,80 @@ namespace MARTe { // --------------------------------------------------------------------------- UDPSServer::UDPSServer() - : port(0u), - maxPayloadSize(UDPS_SERVER_DEFAULT_MAX_PAYLOAD), - dataPort(0u), - useMulticast(false), - clientTimeoutTicks(0u), - numUnicastClients(0u), - numTCPClients(0u), - cachedConfig(NULL_PTR(uint8 *)), - cachedConfigSize(0u), - sendBuf(NULL_PTR(uint8 *)), - sendBufCapacity(0u), - configCounter(0u), + : port(0u), maxPayloadSize(UDPS_SERVER_DEFAULT_MAX_PAYLOAD), dataPort(0u), + useMulticast(false), clientTimeoutTicks(0u), numUnicastClients(0u), + numTCPClients(0u), cachedConfig(NULL_PTR(uint8 *)), cachedConfigSize(0u), + sendBuf(NULL_PTR(uint8 *)), sendBufCapacity(0u), configCounter(0u), started(false) { - for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { - unicastClients[i].ipAddr[0] = '\0'; - unicastClients[i].clientPort = 0u; - unicastClients[i].lastSeenTicks = 0u; - unicastClients[i].active = false; - unicastClients[i].isStatic = false; - } + for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { + unicastClients[i].ipAddr[0] = '\0'; + unicastClients[i].clientPort = 0u; + unicastClients[i].lastSeenTicks = 0u; + unicastClients[i].active = false; + unicastClients[i].isStatic = false; + } - for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { - tcpClients[i] = NULL_PTR(BasicTCPSocket *); - } + for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { + tcpClients[i] = NULL_PTR(BasicTCPSocket *); + } } -UDPSServer::~UDPSServer() { - (void) Stop(); -} +UDPSServer::~UDPSServer() { (void)Stop(); } // --------------------------------------------------------------------------- // Initialise // --------------------------------------------------------------------------- bool UDPSServer::Initialise(StructuredDataI &data) { - uint32 portU32 = 0u; - if (data.Read("Port", portU32)) { - port = static_cast(portU32); + uint32 portU32 = 0u; + if (data.Read("Port", portU32)) { + port = static_cast(portU32); + } else { + REPORT_ERROR_STATIC(ErrorManagement::ParametersError, + "UDPSServer: Port not specified."); + return false; + } + /* port == 0 is valid: unicast push-only mode (no serverSocket bind, + * no CONNECT/DISCONNECT/ACK reception). Clients added via AddStaticClient(). + */ + + StreamString mcGroup; + if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) { + multicastGroup = mcGroup; + useMulticast = true; + } + + if (useMulticast) { + uint32 dpU32 = static_cast(port) + 1u; + (void)data.Read("DataPort", dpU32); + dataPort = static_cast(dpU32); + if (dataPort == port) { + REPORT_ERROR_STATIC( + ErrorManagement::ParametersError, + "UDPSServer: DataPort (%u) must differ from Port (%u).", + static_cast(dataPort), static_cast(port)); + return false; } - else { - REPORT_ERROR_STATIC(ErrorManagement::ParametersError, - "UDPSServer: Port not specified."); - return false; + if (!data.Read("Interface", interface)) { + REPORT_ERROR_STATIC( + ErrorManagement::ParametersError, + "Missing mandatory `Interface` field for multicast operations"); + return false; } - /* port == 0 is valid: unicast push-only mode (no serverSocket bind, - * no CONNECT/DISCONNECT/ACK reception). Clients added via AddStaticClient(). */ + } - StreamString mcGroup; - if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) { - multicastGroup = mcGroup; - useMulticast = true; - } + uint32 mps = UDPS_SERVER_DEFAULT_MAX_PAYLOAD; + (void)data.Read("MaxPayloadSize", mps); + maxPayloadSize = mps; - if (useMulticast) { - uint32 dpU32 = static_cast(port) + 1u; - (void) data.Read("DataPort", dpU32); - dataPort = static_cast(dpU32); - if (dataPort == port) { - REPORT_ERROR_STATIC(ErrorManagement::ParametersError, - "UDPSServer: DataPort (%u) must differ from Port (%u).", - static_cast(dataPort), static_cast(port)); - return false; - } - } + uint32 timeoutSecs = UDPS_SERVER_DEFAULT_CLIENT_TIMEOUT_S; + (void)data.Read("ClientTimeout", timeoutSecs); + clientTimeoutTicks = (timeoutSecs > 0u) ? (static_cast(timeoutSecs) * + HighResolutionTimer::Frequency()) + : 0u; - uint32 mps = UDPS_SERVER_DEFAULT_MAX_PAYLOAD; - (void) data.Read("MaxPayloadSize", mps); - maxPayloadSize = mps; - - uint32 timeoutSecs = UDPS_SERVER_DEFAULT_CLIENT_TIMEOUT_S; - (void) data.Read("ClientTimeout", timeoutSecs); - clientTimeoutTicks = (timeoutSecs > 0u) - ? (static_cast(timeoutSecs) * HighResolutionTimer::Frequency()) - : 0u; - - return true; + return true; } // --------------------------------------------------------------------------- @@ -104,117 +102,115 @@ bool UDPSServer::Initialise(StructuredDataI &data) { // --------------------------------------------------------------------------- bool UDPSServer::Start() { - if (started) { - return true; + if (started) { + return true; + } + + sendBufCapacity = UDPS_HEADER_SIZE + maxPayloadSize; + sendBuf = new uint8[sendBufCapacity]; + if (sendBuf == NULL_PTR(uint8 *)) { + REPORT_ERROR_STATIC(ErrorManagement::FatalError, + "UDPSServer: Could not allocate send buffer."); + return false; + } + + bool ok = true; + + if (useMulticast) { + // TCP listener for CONNECT + CONFIG + ok = tcpListener.Open(); + if (ok) { + ok = tcpListener.Listen(port, 5); + } + if (ok) { + tcpListener.SetBlocking(false); + } + // UDP data socket connected to multicast group + ok &= dataSocket.Open(); + ok &= dataSocket.Join(multicastGroup.Buffer(), interface.Buffer()); + ok &= dataSocket.Connect(multicastGroup.Buffer(), dataPort); + if (!ok) { + REPORT_ERROR_STATIC( + ErrorManagement::FatalError, + "UDPSServer: Failed to open multicast sockets on port %u.", + static_cast(port)); + } + } else { + // Unicast send socket (unconnected; SetDestination per Write) + ok = uniSendSocket.Open(); + if (!ok) { + REPORT_ERROR_STATIC(ErrorManagement::FatalError, + "UDPSServer: Failed to open unicast send socket."); } - sendBufCapacity = UDPS_HEADER_SIZE + maxPayloadSize; - sendBuf = new uint8[sendBufCapacity]; - if (sendBuf == NULL_PTR(uint8 *)) { - REPORT_ERROR_STATIC(ErrorManagement::FatalError, - "UDPSServer: Could not allocate send buffer."); - return false; + // Receive socket: only bind if port != 0 (skip for push-only mode) + if (ok && (port != 0u)) { + ok = serverSocket.Open(); + if (ok) { + ok = serverSocket.Listen(port); // UDP "listen" = bind + } + if (!ok) { + REPORT_ERROR_STATIC( + ErrorManagement::FatalError, + "UDPSServer: Failed to bind receive socket on port %u.", + static_cast(port)); + } } + } - bool ok = true; - - if (useMulticast) { - // TCP listener for CONNECT + CONFIG - ok = tcpListener.Open(); - if (ok) { - ok = tcpListener.Listen(port, 5); - } - if (ok) { - tcpListener.SetBlocking(false); - } - // UDP data socket connected to multicast group - if (ok) { - ok = dataSocket.Open(); - } - if (ok) { - ok = dataSocket.Connect(multicastGroup.Buffer(), dataPort); - } - if (!ok) { - REPORT_ERROR_STATIC(ErrorManagement::FatalError, - "UDPSServer: Failed to open multicast sockets on port %u.", - static_cast(port)); - } - } - else { - // Unicast send socket (unconnected; SetDestination per Write) - ok = uniSendSocket.Open(); - if (!ok) { - REPORT_ERROR_STATIC(ErrorManagement::FatalError, - "UDPSServer: Failed to open unicast send socket."); - } - - // Receive socket: only bind if port != 0 (skip for push-only mode) - if (ok && (port != 0u)) { - ok = serverSocket.Open(); - if (ok) { - ok = serverSocket.Listen(port); // UDP "listen" = bind - } - if (!ok) { - REPORT_ERROR_STATIC(ErrorManagement::FatalError, - "UDPSServer: Failed to bind receive socket on port %u.", - static_cast(port)); - } - } - } - - started = ok; - return ok; + started = ok; + return ok; } bool UDPSServer::Stop() { - if (!started) { - return true; - } - - // Evict all unicast clients - for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { - if (unicastClients[i].active) { - EvictUnicastClient(i); - } - } - numUnicastClients = 0u; - - // Close all TCP clients - for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { - if (tcpClients[i] != NULL_PTR(BasicTCPSocket *)) { - EvictTCPClient(i); - } - } - numTCPClients = 0u; - - // Close sockets - if (serverSocket.IsValid()) { - (void) serverSocket.Close(); - } - if (uniSendSocket.IsValid()) { - (void) uniSendSocket.Close(); - } - if (tcpListener.IsValid()) { - (void) tcpListener.Close(); - } - if (dataSocket.IsValid()) { - (void) dataSocket.Close(); - } - - // Free heap buffers - if (cachedConfig != NULL_PTR(uint8 *)) { - delete[] cachedConfig; - cachedConfig = NULL_PTR(uint8 *); - cachedConfigSize = 0u; - } - if (sendBuf != NULL_PTR(uint8 *)) { - delete[] sendBuf; - sendBuf = NULL_PTR(uint8 *); - sendBufCapacity = 0u; - } - - started = false; + if (!started) { return true; + } + + // Evict all unicast clients + for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { + if (unicastClients[i].active) { + EvictUnicastClient(i); + } + } + numUnicastClients = 0u; + + // Close all TCP clients + for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { + if (tcpClients[i] != NULL_PTR(BasicTCPSocket *)) { + EvictTCPClient(i); + } + } + numTCPClients = 0u; + + // Close sockets + if (serverSocket.IsValid()) { + (void)serverSocket.Close(); + } + if (uniSendSocket.IsValid()) { + (void)uniSendSocket.Close(); + } + if (tcpListener.IsValid()) { + (void)tcpListener.Close(); + } + if (dataSocket.IsValid()) { + (void)dataSocket.Close(); + } + + // Free heap buffers + if (cachedConfig != NULL_PTR(uint8 *)) { + delete[] cachedConfig; + cachedConfig = NULL_PTR(uint8 *); + cachedConfigSize = 0u; + } + if (sendBuf != NULL_PTR(uint8 *)) { + delete[] sendBuf; + sendBuf = NULL_PTR(uint8 *); + sendBufCapacity = 0u; + } + + started = false; + return true; } // --------------------------------------------------------------------------- @@ -222,130 +218,134 @@ bool UDPSServer::Stop() { // --------------------------------------------------------------------------- void UDPSServer::ServiceClients() { - if (!started) { - return; - } + if (!started) { + return; + } - if (useMulticast) { - // Poll TCP listener for new connections (non-blocking). - // Guard with poll() first so WaitConnection is never called on an empty - // queue — avoids the MARTe2 "Failed accept in unblocking mode" log spam. - if (tcpListener.IsValid()) { - struct pollfd pfd; - pfd.fd = static_cast(tcpListener.GetReadHandle()); - pfd.events = POLLIN; - pfd.revents = 0; - bool pending = (::poll(&pfd, 1u, 0) > 0) && - ((pfd.revents & POLLIN) != 0); - BasicTCPSocket *newConn = pending - ? tcpListener.WaitConnection(0u) - : NULL_PTR(BasicTCPSocket *); - if (newConn != NULL_PTR(BasicTCPSocket *)) { - // Find free TCP client slot - uint32 freeSlot = UDPS_SERVER_MAX_TCP_CLIENTS; - for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { - if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) { - freeSlot = i; - break; - } - } - if (freeSlot < UDPS_SERVER_MAX_TCP_CLIENTS) { - HandleMulticastTCPConnect(newConn, freeSlot); - } - else { - REPORT_ERROR_STATIC(ErrorManagement::Warning, - "UDPSServer: TCP client table full, rejecting new connection."); - (void) newConn->Close(); - delete newConn; - } - } - } - - // Poll existing TCP clients for DISCONNECT + if (useMulticast) { + // Poll TCP listener for new connections (non-blocking). + // Guard with poll() first so WaitConnection is never called on an empty + // queue — avoids the MARTe2 "Failed accept in unblocking mode" log spam. + if (tcpListener.IsValid()) { + struct pollfd pfd; + pfd.fd = static_cast(tcpListener.GetReadHandle()); + pfd.events = POLLIN; + pfd.revents = 0; + bool pending = (::poll(&pfd, 1u, 0) > 0) && ((pfd.revents & POLLIN) != 0); + BasicTCPSocket *newConn = + pending ? tcpListener.WaitConnection(0u) : NULL_PTR(BasicTCPSocket *); + if (newConn != NULL_PTR(BasicTCPSocket *)) { + // Find free TCP client slot + uint32 freeSlot = UDPS_SERVER_MAX_TCP_CLIENTS; for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { - if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) { - continue; - } - // Non-blocking peek - int fd = tcpClients[i]->GetReadHandle(); - if (fd < 0 || fd >= FD_SETSIZE) { continue; /* HI-6: skip FDs outside select range */ } - fd_set rset; - FD_ZERO(&rset); - FD_SET(fd, &rset); - struct timeval tv; - tv.tv_sec = 0; tv.tv_usec = 0; - int nready = select(fd + 1, &rset, NULL, NULL, &tv); - if (nready > 0) { - uint8 pktBuf[UDPS_HEADER_SIZE]; - uint32 recvSize = UDPS_HEADER_SIZE; - bool recvOk = tcpClients[i]->Read(reinterpret_cast(pktBuf), recvSize); - if (!recvOk || (recvSize == 0u)) { - REPORT_ERROR_STATIC(ErrorManagement::Information, - "UDPSServer: TCP client disconnected (slot %u).", i); - EvictTCPClient(i); - continue; - } - if (recvSize >= UDPS_HEADER_SIZE) { - const UDPSPacketHeader *hdr = - reinterpret_cast(pktBuf); - if ((hdr->magic == UDPS_MAGIC) && - (hdr->type == UDPS_TYPE_DISCONNECT)) { - REPORT_ERROR_STATIC(ErrorManagement::Information, - "UDPSServer: TCP client sent DISCONNECT (slot %u).", i); - EvictTCPClient(i); - } - } - } + if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) { + freeSlot = i; + break; + } } + if (freeSlot < UDPS_SERVER_MAX_TCP_CLIENTS) { + HandleMulticastTCPConnect(newConn, freeSlot); + } else { + REPORT_ERROR_STATIC( + ErrorManagement::Warning, + "UDPSServer: TCP client table full, rejecting new connection."); + (void)newConn->Close(); + delete newConn; + } + } } - else { - // Unicast: poll serverSocket for CONNECT / DISCONNECT / ACK - if (!serverSocket.IsValid()) { - return; - } - int fd = serverSocket.GetReadHandle(); - if (fd < 0 || fd >= FD_SETSIZE) { return; /* HI-6 */ } - fd_set rset; - FD_ZERO(&rset); - FD_SET(fd, &rset); - struct timeval tv = {0, 0}; - int nready = select(fd + 1, &rset, NULL, NULL, &tv); - while (nready > 0) { - uint8 pktBuf[UDPS_HEADER_SIZE + 4u]; - uint32 recvSize = static_cast(sizeof(pktBuf)); - bool recvOk = serverSocket.Read(reinterpret_cast(pktBuf), recvSize); - if (!recvOk || (recvSize < UDPS_HEADER_SIZE)) { - break; - } - const UDPSPacketHeader *hdr = - reinterpret_cast(pktBuf); - if (hdr->magic != UDPS_MAGIC) { - break; - } - InternetHost src = serverSocket.GetSource(); - if (hdr->type == UDPS_TYPE_CONNECT) { - HandleUnicastConnect(src); - } - else if (hdr->type == UDPS_TYPE_DISCONNECT) { - HandleUnicastDisconnect(src); - } - else if (hdr->type == UDPS_TYPE_ACK) { - HandleUnicastAck(src); - } - // Check if more data is ready - FD_ZERO(&rset); - FD_SET(fd, &rset); - tv.tv_sec = 0; - tv.tv_usec = 0; - nready = select(fd + 1, &rset, NULL, NULL, &tv); + // Poll existing TCP clients for DISCONNECT + for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { + if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) { + continue; + } + // Non-blocking peek + int fd = tcpClients[i]->GetReadHandle(); + if (fd < 0 || fd >= FD_SETSIZE) { + continue; /* HI-6: skip FDs outside select range */ + } + fd_set rset; + FD_ZERO(&rset); + FD_SET(fd, &rset); + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 0; + int nready = select(fd + 1, &rset, NULL, NULL, &tv); + if (nready > 0) { + uint8 pktBuf[UDPS_HEADER_SIZE]; + uint32 recvSize = UDPS_HEADER_SIZE; + bool recvOk = + tcpClients[i]->Read(reinterpret_cast(pktBuf), recvSize); + if (!recvOk || (recvSize == 0u)) { + REPORT_ERROR_STATIC(ErrorManagement::Information, + "UDPSServer: TCP client disconnected (slot %u).", + i); + EvictTCPClient(i); + continue; } - - // Evict stale clients (if timeout is configured) - if (clientTimeoutTicks > 0u) { - EvictStaleUnicastClients(); + if (recvSize >= UDPS_HEADER_SIZE) { + const UDPSPacketHeader *hdr = + reinterpret_cast(pktBuf); + if ((hdr->magic == UDPS_MAGIC) && + (hdr->type == UDPS_TYPE_DISCONNECT)) { + REPORT_ERROR_STATIC( + ErrorManagement::Information, + "UDPSServer: TCP client sent DISCONNECT (slot %u).", i); + EvictTCPClient(i); + } } + } } + } else { + // Unicast: poll serverSocket for CONNECT / DISCONNECT / ACK + if (!serverSocket.IsValid()) { + return; + } + int fd = serverSocket.GetReadHandle(); + if (fd < 0 || fd >= FD_SETSIZE) { + return; /* HI-6 */ + } + fd_set rset; + FD_ZERO(&rset); + FD_SET(fd, &rset); + struct timeval tv = {0, 0}; + int nready = select(fd + 1, &rset, NULL, NULL, &tv); + while (nready > 0) { + uint8 pktBuf[UDPS_HEADER_SIZE + 4u]; + uint32 recvSize = static_cast(sizeof(pktBuf)); + bool recvOk = + serverSocket.Read(reinterpret_cast(pktBuf), recvSize); + if (!recvOk || (recvSize < UDPS_HEADER_SIZE)) { + break; + } + const UDPSPacketHeader *hdr = + reinterpret_cast(pktBuf); + if (hdr->magic != UDPS_MAGIC) { + break; + } + InternetHost src = serverSocket.GetSource(); + if (hdr->type == UDPS_TYPE_CONNECT) { + HandleUnicastConnect(src); + } else if (hdr->type == UDPS_TYPE_DISCONNECT) { + HandleUnicastDisconnect(src); + } else if (hdr->type == UDPS_TYPE_ACK) { + HandleUnicastAck(src); + } + + // Check if more data is ready + FD_ZERO(&rset); + FD_SET(fd, &rset); + tv.tv_sec = 0; + tv.tv_usec = 0; + nready = select(fd + 1, &rset, NULL, NULL, &tv); + } + + // Evict stale clients (if timeout is configured) + if (clientTimeoutTicks > 0u) { + EvictStaleUnicastClients(); + } + } } // --------------------------------------------------------------------------- @@ -353,95 +353,96 @@ void UDPSServer::ServiceClients() { // --------------------------------------------------------------------------- bool UDPSServer::SendConfig(const uint8 *payload, uint32 payloadSize) { - if (!started) { - return false; + if (!started) { + return false; + } + + // Cache for future CONNECT clients + (void)CacheConfig(payload, payloadSize); + + configCounter++; + + bool ok = true; + + if (useMulticast) { + // Send CONFIG over TCP to each connected client + for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { + if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) { + continue; + } + bool sent = SendFragmentedTCP(*tcpClients[i], UDPS_TYPE_CONFIG, + configCounter, payload, payloadSize); + if (!sent) { + REPORT_ERROR_STATIC( + ErrorManagement::Warning, + "UDPSServer: CONFIG send failed to TCP client %u, evicting.", i); + EvictTCPClient(i); + ok = false; + } } - - // Cache for future CONNECT clients - (void) CacheConfig(payload, payloadSize); - - configCounter++; - - bool ok = true; - - if (useMulticast) { - // Send CONFIG over TCP to each connected client - for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { - if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) { - continue; - } - bool sent = SendFragmentedTCP(*tcpClients[i], UDPS_TYPE_CONFIG, - configCounter, payload, payloadSize); - if (!sent) { - REPORT_ERROR_STATIC(ErrorManagement::Warning, - "UDPSServer: CONFIG send failed to TCP client %u, evicting.", i); - EvictTCPClient(i); - ok = false; - } - } - } - else { - // Send CONFIG to each unicast client - for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { - if (!unicastClients[i].active) { - continue; - } - InternetHost dest(unicastClients[i].clientPort, unicastClients[i].ipAddr); - bool sent = SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG, - configCounter, payload, payloadSize); - if (!sent) { - REPORT_ERROR_STATIC(ErrorManagement::Warning, - "UDPSServer: CONFIG send failed to %s:%u.", - unicastClients[i].ipAddr, - static_cast(unicastClients[i].clientPort)); - ok = false; - } - } + } else { + // Send CONFIG to each unicast client + for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { + if (!unicastClients[i].active) { + continue; + } + InternetHost dest(unicastClients[i].clientPort, unicastClients[i].ipAddr); + bool sent = SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG, + configCounter, payload, payloadSize); + if (!sent) { + REPORT_ERROR_STATIC(ErrorManagement::Warning, + "UDPSServer: CONFIG send failed to %s:%u.", + unicastClients[i].ipAddr, + static_cast(unicastClients[i].clientPort)); + ok = false; + } } + } - return ok; + return ok; } // --------------------------------------------------------------------------- // SendData // --------------------------------------------------------------------------- -bool UDPSServer::SendData(uint32 counter, const uint8 *payload, uint32 payloadSize) { - if (!started) { - return false; - } +bool UDPSServer::SendData(uint32 counter, const uint8 *payload, + uint32 payloadSize) { + if (!started) { + return false; + } - bool ok = true; + bool ok = true; - if (useMulticast) { - // Single multicast write (no dest needed — socket already connected) - bool sent = SendFragmentedUDP(dataSocket, NULL_PTR(InternetHost *), - UDPS_TYPE_DATA, counter, payload, payloadSize); - if (!sent) { - REPORT_ERROR_STATIC(ErrorManagement::Warning, - "UDPSServer: DATA send to multicast group failed."); - ok = false; - } + if (useMulticast) { + // Single multicast write (no dest needed — socket already connected) + bool sent = + SendFragmentedUDP(dataSocket, NULL_PTR(InternetHost *), UDPS_TYPE_DATA, + counter, payload, payloadSize); + if (!sent) { + REPORT_ERROR_STATIC(ErrorManagement::Warning, + "UDPSServer: DATA send to multicast group failed."); + ok = false; } - else { - for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { - if (!unicastClients[i].active) { - continue; - } - InternetHost dest(unicastClients[i].clientPort, unicastClients[i].ipAddr); - bool sent = SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_DATA, - counter, payload, payloadSize); - if (!sent) { - REPORT_ERROR_STATIC(ErrorManagement::Warning, - "UDPSServer: DATA send failed to %s:%u.", - unicastClients[i].ipAddr, - static_cast(unicastClients[i].clientPort)); - ok = false; - } - } + } else { + for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { + if (!unicastClients[i].active) { + continue; + } + InternetHost dest(unicastClients[i].clientPort, unicastClients[i].ipAddr); + bool sent = SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_DATA, + counter, payload, payloadSize); + if (!sent) { + REPORT_ERROR_STATIC(ErrorManagement::Warning, + "UDPSServer: DATA send failed to %s:%u.", + unicastClients[i].ipAddr, + static_cast(unicastClients[i].clientPort)); + ok = false; + } } + } - return ok; + return ok; } // --------------------------------------------------------------------------- @@ -449,49 +450,50 @@ bool UDPSServer::SendData(uint32 counter, const uint8 *payload, uint32 payloadSi // --------------------------------------------------------------------------- bool UDPSServer::AddStaticClient(const char8 *ip, uint16 port_) { - if ((ip == NULL_PTR(const char8 *)) || (ip[0] == '\0')) { - return false; - } + if ((ip == NULL_PTR(const char8 *)) || (ip[0] == '\0')) { + return false; + } - // Check for duplicate - uint32 existing = FindUnicastClient(ip, port_); - if (existing < UDPS_SERVER_MAX_UNICAST_CLIENTS) { - unicastClients[existing].isStatic = true; // ensure it's marked static - return true; - } - - // Find free slot - uint32 freeSlot = UDPS_SERVER_MAX_UNICAST_CLIENTS; - for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { - if (!unicastClients[i].active) { - freeSlot = i; - break; - } - } - if (freeSlot >= UDPS_SERVER_MAX_UNICAST_CLIENTS) { - REPORT_ERROR_STATIC(ErrorManagement::Warning, - "UDPSServer: Unicast client table full, cannot add static client %s:%u.", - ip, static_cast(port_)); - return false; - } - - // Populate slot - uint32 ipLen = 0u; - while ((ipLen < 63u) && (ip[ipLen] != '\0')) { - unicastClients[freeSlot].ipAddr[ipLen] = ip[ipLen]; - ipLen++; - } - unicastClients[freeSlot].ipAddr[ipLen] = '\0'; - unicastClients[freeSlot].clientPort = port_; - unicastClients[freeSlot].lastSeenTicks = HighResolutionTimer::Counter(); - unicastClients[freeSlot].active = true; - unicastClients[freeSlot].isStatic = true; - numUnicastClients++; - - REPORT_ERROR_STATIC(ErrorManagement::Information, - "UDPSServer: Static client added: %s:%u.", - ip, static_cast(port_)); + // Check for duplicate + uint32 existing = FindUnicastClient(ip, port_); + if (existing < UDPS_SERVER_MAX_UNICAST_CLIENTS) { + unicastClients[existing].isStatic = true; // ensure it's marked static return true; + } + + // Find free slot + uint32 freeSlot = UDPS_SERVER_MAX_UNICAST_CLIENTS; + for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { + if (!unicastClients[i].active) { + freeSlot = i; + break; + } + } + if (freeSlot >= UDPS_SERVER_MAX_UNICAST_CLIENTS) { + REPORT_ERROR_STATIC(ErrorManagement::Warning, + "UDPSServer: Unicast client table full, cannot add " + "static client %s:%u.", + ip, static_cast(port_)); + return false; + } + + // Populate slot + uint32 ipLen = 0u; + while ((ipLen < 63u) && (ip[ipLen] != '\0')) { + unicastClients[freeSlot].ipAddr[ipLen] = ip[ipLen]; + ipLen++; + } + unicastClients[freeSlot].ipAddr[ipLen] = '\0'; + unicastClients[freeSlot].clientPort = port_; + unicastClients[freeSlot].lastSeenTicks = HighResolutionTimer::Counter(); + unicastClients[freeSlot].active = true; + unicastClients[freeSlot].isStatic = true; + numUnicastClients++; + + REPORT_ERROR_STATIC(ErrorManagement::Information, + "UDPSServer: Static client added: %s:%u.", ip, + static_cast(port_)); + return true; } // --------------------------------------------------------------------------- @@ -499,134 +501,115 @@ bool UDPSServer::AddStaticClient(const char8 *ip, uint16 port_) { // --------------------------------------------------------------------------- uint32 UDPSServer::GetClientCount() const { - uint32 count = 0u; - for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { - if (unicastClients[i].active) { - count++; - } + uint32 count = 0u; + for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { + if (unicastClients[i].active) { + count++; } - for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { - if (tcpClients[i] != NULL_PTR(BasicTCPSocket *)) { - count++; - } + } + for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) { + if (tcpClients[i] != NULL_PTR(BasicTCPSocket *)) { + count++; } - return count; + } + return count; } -bool UDPSServer::HasClients() const { - return (GetClientCount() > 0u); -} +bool UDPSServer::HasClients() const { return (GetClientCount() > 0u); } -bool UDPSServer::IsMulticast() const { - return useMulticast; -} +bool UDPSServer::IsMulticast() const { return useMulticast; } -uint16 UDPSServer::GetPort() const { - return port; -} +uint16 UDPSServer::GetPort() const { return port; } -uint32 UDPSServer::GetMaxPayloadSize() const { - return maxPayloadSize; -} +uint32 UDPSServer::GetMaxPayloadSize() const { return maxPayloadSize; } // --------------------------------------------------------------------------- // Private: SendFragmentedUDP // --------------------------------------------------------------------------- -bool UDPSServer::SendFragmentedUDP(BasicUDPSocket &sock, - InternetHost *dest, - uint8 type, - uint32 counter, - const uint8 *payload, - uint32 payloadSize) { - uint32 maxChunk = maxPayloadSize; // payload bytes per fragment (excl. header) - uint32 totalFrags = (payloadSize == 0u) ? 1u : - ((payloadSize + maxChunk - 1u) / maxChunk); +bool UDPSServer::SendFragmentedUDP(BasicUDPSocket &sock, InternetHost *dest, + uint8 type, uint32 counter, + const uint8 *payload, uint32 payloadSize) { + uint32 maxChunk = maxPayloadSize; // payload bytes per fragment (excl. header) + uint32 totalFrags = + (payloadSize == 0u) ? 1u : ((payloadSize + maxChunk - 1u) / maxChunk); - bool ok = true; - uint32 offs = 0u; + bool ok = true; + uint32 offs = 0u; - for (uint32 f = 0u; (f < totalFrags) && ok; f++) { - uint32 chunkSize = payloadSize - offs; - if (chunkSize > maxChunk) { - chunkSize = maxChunk; - } - - UDPSBuildHeader(sendBuf, type, counter, - static_cast(f), - static_cast(totalFrags), - chunkSize); - - if (chunkSize > 0u) { - (void) MemoryOperationsHelper::Copy(sendBuf + UDPS_HEADER_SIZE, - payload + offs, - chunkSize); - } - - uint32 sendSize = UDPS_HEADER_SIZE + chunkSize; - - if (dest != NULL_PTR(InternetHost *)) { - (void) sock.SetDestination(*dest); - } - - ok = sock.Write(reinterpret_cast(sendBuf), sendSize); - if (!ok) { - REPORT_ERROR_STATIC(ErrorManagement::Warning, - "UDPSServer: UDP fragment %u/%u write failed.", - f + 1u, totalFrags); - } - - offs += chunkSize; + for (uint32 f = 0u; (f < totalFrags) && ok; f++) { + uint32 chunkSize = payloadSize - offs; + if (chunkSize > maxChunk) { + chunkSize = maxChunk; } - return ok; + UDPSBuildHeader(sendBuf, type, counter, static_cast(f), + static_cast(totalFrags), chunkSize); + + if (chunkSize > 0u) { + (void)MemoryOperationsHelper::Copy(sendBuf + UDPS_HEADER_SIZE, + payload + offs, chunkSize); + } + + uint32 sendSize = UDPS_HEADER_SIZE + chunkSize; + + if (dest != NULL_PTR(InternetHost *)) { + (void)sock.SetDestination(*dest); + } + + ok = sock.Write(reinterpret_cast(sendBuf), sendSize); + if (!ok) { + REPORT_ERROR_STATIC(ErrorManagement::Warning, + "UDPSServer: UDP fragment %u/%u write failed.", + f + 1u, totalFrags); + } + + offs += chunkSize; + } + + return ok; } // --------------------------------------------------------------------------- // Private: SendFragmentedTCP // --------------------------------------------------------------------------- -bool UDPSServer::SendFragmentedTCP(BasicTCPSocket &sock, - uint8 type, - uint32 counter, - const uint8 *payload, - uint32 payloadSize) { - uint32 maxChunk = maxPayloadSize; - uint32 totalFrags = (payloadSize == 0u) ? 1u : - ((payloadSize + maxChunk - 1u) / maxChunk); +bool UDPSServer::SendFragmentedTCP(BasicTCPSocket &sock, uint8 type, + uint32 counter, const uint8 *payload, + uint32 payloadSize) { + uint32 maxChunk = maxPayloadSize; + uint32 totalFrags = + (payloadSize == 0u) ? 1u : ((payloadSize + maxChunk - 1u) / maxChunk); - bool ok = true; - uint32 offs = 0u; + bool ok = true; + uint32 offs = 0u; - for (uint32 f = 0u; (f < totalFrags) && ok; f++) { - uint32 chunkSize = payloadSize - offs; - if (chunkSize > maxChunk) { - chunkSize = maxChunk; - } - - UDPSBuildHeader(sendBuf, type, counter, - static_cast(f), - static_cast(totalFrags), - chunkSize); - - if (chunkSize > 0u) { - (void) MemoryOperationsHelper::Copy(sendBuf + UDPS_HEADER_SIZE, - payload + offs, - chunkSize); - } - - uint32 sendSize = UDPS_HEADER_SIZE + chunkSize; - ok = sock.Write(reinterpret_cast(sendBuf), sendSize); - if (!ok) { - REPORT_ERROR_STATIC(ErrorManagement::Warning, - "UDPSServer: TCP fragment %u/%u write failed.", - f + 1u, totalFrags); - } - - offs += chunkSize; + for (uint32 f = 0u; (f < totalFrags) && ok; f++) { + uint32 chunkSize = payloadSize - offs; + if (chunkSize > maxChunk) { + chunkSize = maxChunk; } - return ok; + UDPSBuildHeader(sendBuf, type, counter, static_cast(f), + static_cast(totalFrags), chunkSize); + + if (chunkSize > 0u) { + (void)MemoryOperationsHelper::Copy(sendBuf + UDPS_HEADER_SIZE, + payload + offs, chunkSize); + } + + uint32 sendSize = UDPS_HEADER_SIZE + chunkSize; + ok = sock.Write(reinterpret_cast(sendBuf), sendSize); + if (!ok) { + REPORT_ERROR_STATIC(ErrorManagement::Warning, + "UDPSServer: TCP fragment %u/%u write failed.", + f + 1u, totalFrags); + } + + offs += chunkSize; + } + + return ok; } // --------------------------------------------------------------------------- @@ -634,20 +617,20 @@ bool UDPSServer::SendFragmentedTCP(BasicTCPSocket &sock, // --------------------------------------------------------------------------- void UDPSServer::EvictStaleUnicastClients() { - uint64 now = HighResolutionTimer::Counter(); - for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { - if (!unicastClients[i].active || unicastClients[i].isStatic) { - continue; - } - uint64 elapsed = now - unicastClients[i].lastSeenTicks; - if (elapsed >= clientTimeoutTicks) { - REPORT_ERROR_STATIC(ErrorManagement::Information, - "UDPSServer: Evicting stale client %s:%u.", - unicastClients[i].ipAddr, - static_cast(unicastClients[i].clientPort)); - EvictUnicastClient(i); - } + uint64 now = HighResolutionTimer::Counter(); + for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { + if (!unicastClients[i].active || unicastClients[i].isStatic) { + continue; } + uint64 elapsed = now - unicastClients[i].lastSeenTicks; + if (elapsed >= clientTimeoutTicks) { + REPORT_ERROR_STATIC(ErrorManagement::Information, + "UDPSServer: Evicting stale client %s:%u.", + unicastClients[i].ipAddr, + static_cast(unicastClients[i].clientPort)); + EvictUnicastClient(i); + } + } } // --------------------------------------------------------------------------- @@ -655,73 +638,73 @@ void UDPSServer::EvictStaleUnicastClients() { // --------------------------------------------------------------------------- void UDPSServer::HandleUnicastConnect(const InternetHost &src) { - StreamString srcAddrStr = src.GetAddress(); - const char8 *srcAddr = srcAddrStr.Buffer(); - uint16 srcPort = src.GetPort(); + StreamString srcAddrStr = src.GetAddress(); + const char8 *srcAddr = srcAddrStr.Buffer(); + uint16 srcPort = src.GetPort(); - REPORT_ERROR_STATIC(ErrorManagement::Information, - "UDPSServer: CONNECT from %s:%u.", - srcAddr, static_cast(srcPort)); + REPORT_ERROR_STATIC(ErrorManagement::Information, + "UDPSServer: CONNECT from %s:%u.", srcAddr, + static_cast(srcPort)); - // Check if this client is already known - uint32 existing = FindUnicastClient(srcAddr, srcPort); - if (existing < UDPS_SERVER_MAX_UNICAST_CLIENTS) { - // Refresh last-seen and resend CONFIG - unicastClients[existing].lastSeenTicks = HighResolutionTimer::Counter(); - if (cachedConfig != NULL_PTR(uint8 *)) { - InternetHost dest(srcPort, srcAddr); - configCounter++; - (void) SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG, - configCounter, cachedConfig, cachedConfigSize); - } - return; - } - - // New client — find a free slot (or evict oldest non-static) - uint32 slot = UDPS_SERVER_MAX_UNICAST_CLIENTS; - for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { - if (!unicastClients[i].active) { - slot = i; - break; - } - } - if (slot >= UDPS_SERVER_MAX_UNICAST_CLIENTS) { - slot = FindOldestUnicastClient(); - if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) { - REPORT_ERROR_STATIC(ErrorManagement::Information, - "UDPSServer: Client table full — evicting %s:%u.", - unicastClients[slot].ipAddr, - static_cast(unicastClients[slot].clientPort)); - EvictUnicastClient(slot); - } - else { - REPORT_ERROR_STATIC(ErrorManagement::Warning, - "UDPSServer: All slots occupied by static clients; rejecting %s:%u.", - srcAddr, static_cast(srcPort)); - return; - } - } - - // Fill slot - uint32 addrLen = 0u; - while ((addrLen < 63u) && (srcAddr[addrLen] != '\0')) { - unicastClients[slot].ipAddr[addrLen] = srcAddr[addrLen]; - addrLen++; - } - unicastClients[slot].ipAddr[addrLen] = '\0'; - unicastClients[slot].clientPort = srcPort; - unicastClients[slot].lastSeenTicks = HighResolutionTimer::Counter(); - unicastClients[slot].active = true; - unicastClients[slot].isStatic = false; - numUnicastClients++; - - // Send cached CONFIG if available + // Check if this client is already known + uint32 existing = FindUnicastClient(srcAddr, srcPort); + if (existing < UDPS_SERVER_MAX_UNICAST_CLIENTS) { + // Refresh last-seen and resend CONFIG + unicastClients[existing].lastSeenTicks = HighResolutionTimer::Counter(); if (cachedConfig != NULL_PTR(uint8 *)) { - InternetHost dest(srcPort, srcAddr); - configCounter++; - (void) SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG, - configCounter, cachedConfig, cachedConfigSize); + InternetHost dest(srcPort, srcAddr); + configCounter++; + (void)SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG, + configCounter, cachedConfig, cachedConfigSize); } + return; + } + + // New client — find a free slot (or evict oldest non-static) + uint32 slot = UDPS_SERVER_MAX_UNICAST_CLIENTS; + for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { + if (!unicastClients[i].active) { + slot = i; + break; + } + } + if (slot >= UDPS_SERVER_MAX_UNICAST_CLIENTS) { + slot = FindOldestUnicastClient(); + if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) { + REPORT_ERROR_STATIC(ErrorManagement::Information, + "UDPSServer: Client table full — evicting %s:%u.", + unicastClients[slot].ipAddr, + static_cast(unicastClients[slot].clientPort)); + EvictUnicastClient(slot); + } else { + REPORT_ERROR_STATIC( + ErrorManagement::Warning, + "UDPSServer: All slots occupied by static clients; rejecting %s:%u.", + srcAddr, static_cast(srcPort)); + return; + } + } + + // Fill slot + uint32 addrLen = 0u; + while ((addrLen < 63u) && (srcAddr[addrLen] != '\0')) { + unicastClients[slot].ipAddr[addrLen] = srcAddr[addrLen]; + addrLen++; + } + unicastClients[slot].ipAddr[addrLen] = '\0'; + unicastClients[slot].clientPort = srcPort; + unicastClients[slot].lastSeenTicks = HighResolutionTimer::Counter(); + unicastClients[slot].active = true; + unicastClients[slot].isStatic = false; + numUnicastClients++; + + // Send cached CONFIG if available + if (cachedConfig != NULL_PTR(uint8 *)) { + InternetHost dest(srcPort, srcAddr); + configCounter++; + (void)SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG, + configCounter, cachedConfig, cachedConfigSize); + } } // --------------------------------------------------------------------------- @@ -729,17 +712,17 @@ void UDPSServer::HandleUnicastConnect(const InternetHost &src) { // --------------------------------------------------------------------------- void UDPSServer::HandleUnicastDisconnect(const InternetHost &src) { - StreamString srcAddrStr = src.GetAddress(); - const char8 *srcAddr = srcAddrStr.Buffer(); - uint16 srcPort = src.GetPort(); + StreamString srcAddrStr = src.GetAddress(); + const char8 *srcAddr = srcAddrStr.Buffer(); + uint16 srcPort = src.GetPort(); - uint32 slot = FindUnicastClient(srcAddr, srcPort); - if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) { - REPORT_ERROR_STATIC(ErrorManagement::Information, - "UDPSServer: DISCONNECT from %s:%u.", - srcAddr, static_cast(srcPort)); - EvictUnicastClient(slot); - } + uint32 slot = FindUnicastClient(srcAddr, srcPort); + if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) { + REPORT_ERROR_STATIC(ErrorManagement::Information, + "UDPSServer: DISCONNECT from %s:%u.", srcAddr, + static_cast(srcPort)); + EvictUnicastClient(slot); + } } // --------------------------------------------------------------------------- @@ -747,14 +730,14 @@ void UDPSServer::HandleUnicastDisconnect(const InternetHost &src) { // --------------------------------------------------------------------------- void UDPSServer::HandleUnicastAck(const InternetHost &src) { - StreamString srcAddrStr = src.GetAddress(); - const char8 *srcAddr = srcAddrStr.Buffer(); - uint16 srcPort = src.GetPort(); + StreamString srcAddrStr = src.GetAddress(); + const char8 *srcAddr = srcAddrStr.Buffer(); + uint16 srcPort = src.GetPort(); - uint32 slot = FindUnicastClient(srcAddr, srcPort); - if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) { - unicastClients[slot].lastSeenTicks = HighResolutionTimer::Counter(); - } + uint32 slot = FindUnicastClient(srcAddr, srcPort); + if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) { + unicastClients[slot].lastSeenTicks = HighResolutionTimer::Counter(); + } } // --------------------------------------------------------------------------- @@ -762,30 +745,30 @@ void UDPSServer::HandleUnicastAck(const InternetHost &src) { // --------------------------------------------------------------------------- uint32 UDPSServer::FindUnicastClient(const char8 *ip, uint16 port_) const { - for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { - if (!unicastClients[i].active) { - continue; - } - if (unicastClients[i].clientPort != port_) { - continue; - } - // String compare - bool match = true; - uint32 k = 0u; - while ((k < 63u) && match) { - if (unicastClients[i].ipAddr[k] != ip[k]) { - match = false; - } - if (ip[k] == '\0') { - break; - } - k++; - } - if (match) { - return i; - } + for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { + if (!unicastClients[i].active) { + continue; } - return UDPS_SERVER_MAX_UNICAST_CLIENTS; + if (unicastClients[i].clientPort != port_) { + continue; + } + // String compare + bool match = true; + uint32 k = 0u; + while ((k < 63u) && match) { + if (unicastClients[i].ipAddr[k] != ip[k]) { + match = false; + } + if (ip[k] == '\0') { + break; + } + k++; + } + if (match) { + return i; + } + } + return UDPS_SERVER_MAX_UNICAST_CLIENTS; } // --------------------------------------------------------------------------- @@ -793,19 +776,19 @@ uint32 UDPSServer::FindUnicastClient(const char8 *ip, uint16 port_) const { // --------------------------------------------------------------------------- uint32 UDPSServer::FindOldestUnicastClient() const { - uint32 oldest = UDPS_SERVER_MAX_UNICAST_CLIENTS; - uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL; + uint32 oldest = UDPS_SERVER_MAX_UNICAST_CLIENTS; + uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL; - for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { - if (!unicastClients[i].active || unicastClients[i].isStatic) { - continue; - } - if (unicastClients[i].lastSeenTicks < oldestTick) { - oldestTick = unicastClients[i].lastSeenTicks; - oldest = i; - } + for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) { + if (!unicastClients[i].active || unicastClients[i].isStatic) { + continue; } - return oldest; + if (unicastClients[i].lastSeenTicks < oldestTick) { + oldestTick = unicastClients[i].lastSeenTicks; + oldest = i; + } + } + return oldest; } // --------------------------------------------------------------------------- @@ -813,41 +796,45 @@ uint32 UDPSServer::FindOldestUnicastClient() const { // --------------------------------------------------------------------------- void UDPSServer::EvictUnicastClient(uint32 idx) { - if (idx >= UDPS_SERVER_MAX_UNICAST_CLIENTS) { - return; - } - unicastClients[idx].active = false; - if (numUnicastClients > 0u) { - numUnicastClients--; - } + if (idx >= UDPS_SERVER_MAX_UNICAST_CLIENTS) { + return; + } + unicastClients[idx].active = false; + if (numUnicastClients > 0u) { + numUnicastClients--; + } } // --------------------------------------------------------------------------- // Private: HandleMulticastTCPConnect // --------------------------------------------------------------------------- -void UDPSServer::HandleMulticastTCPConnect(BasicTCPSocket *newClient, uint32 idx) { - if (idx >= UDPS_SERVER_MAX_TCP_CLIENTS) { - return; - } +void UDPSServer::HandleMulticastTCPConnect(BasicTCPSocket *newClient, + uint32 idx) { + if (idx >= UDPS_SERVER_MAX_TCP_CLIENTS) { + return; + } - tcpClients[idx] = newClient; - numTCPClients++; + tcpClients[idx] = newClient; + numTCPClients++; - REPORT_ERROR_STATIC(ErrorManagement::Information, - "UDPSServer: Multicast TCP client connected (slot %u).", idx); + REPORT_ERROR_STATIC(ErrorManagement::Information, + "UDPSServer: Multicast TCP client connected (slot %u).", + idx); - // Send cached CONFIG over TCP - if (cachedConfig != NULL_PTR(uint8 *)) { - configCounter++; - bool sent = SendFragmentedTCP(*newClient, UDPS_TYPE_CONFIG, - configCounter, cachedConfig, cachedConfigSize); - if (!sent) { - REPORT_ERROR_STATIC(ErrorManagement::Warning, - "UDPSServer: Failed to send CONFIG to new TCP client (slot %u).", idx); - EvictTCPClient(idx); - } + // Send cached CONFIG over TCP + if (cachedConfig != NULL_PTR(uint8 *)) { + configCounter++; + bool sent = SendFragmentedTCP(*newClient, UDPS_TYPE_CONFIG, configCounter, + cachedConfig, cachedConfigSize); + if (!sent) { + REPORT_ERROR_STATIC( + ErrorManagement::Warning, + "UDPSServer: Failed to send CONFIG to new TCP client (slot %u).", + idx); + EvictTCPClient(idx); } + } } // --------------------------------------------------------------------------- @@ -855,17 +842,17 @@ void UDPSServer::HandleMulticastTCPConnect(BasicTCPSocket *newClient, uint32 idx // --------------------------------------------------------------------------- void UDPSServer::EvictTCPClient(uint32 idx) { - if (idx >= UDPS_SERVER_MAX_TCP_CLIENTS) { - return; - } - if (tcpClients[idx] != NULL_PTR(BasicTCPSocket *)) { - (void) tcpClients[idx]->Close(); - delete tcpClients[idx]; - tcpClients[idx] = NULL_PTR(BasicTCPSocket *); - if (numTCPClients > 0u) { - numTCPClients--; - } + if (idx >= UDPS_SERVER_MAX_TCP_CLIENTS) { + return; + } + if (tcpClients[idx] != NULL_PTR(BasicTCPSocket *)) { + (void)tcpClients[idx]->Close(); + delete tcpClients[idx]; + tcpClients[idx] = NULL_PTR(BasicTCPSocket *); + if (numTCPClients > 0u) { + numTCPClients--; } + } } // --------------------------------------------------------------------------- @@ -873,23 +860,23 @@ void UDPSServer::EvictTCPClient(uint32 idx) { // --------------------------------------------------------------------------- bool UDPSServer::CacheConfig(const uint8 *payload, uint32 payloadSize) { - if (cachedConfig != NULL_PTR(uint8 *)) { - delete[] cachedConfig; - cachedConfig = NULL_PTR(uint8 *); - cachedConfigSize = 0u; - } - if ((payload == NULL_PTR(const uint8 *)) || (payloadSize == 0u)) { - return true; - } - cachedConfig = new uint8[payloadSize]; - if (cachedConfig == NULL_PTR(uint8 *)) { - REPORT_ERROR_STATIC(ErrorManagement::FatalError, - "UDPSServer: Failed to allocate cached CONFIG buffer."); - return false; - } - (void) MemoryOperationsHelper::Copy(cachedConfig, payload, payloadSize); - cachedConfigSize = payloadSize; + if (cachedConfig != NULL_PTR(uint8 *)) { + delete[] cachedConfig; + cachedConfig = NULL_PTR(uint8 *); + cachedConfigSize = 0u; + } + if ((payload == NULL_PTR(const uint8 *)) || (payloadSize == 0u)) { return true; + } + cachedConfig = new uint8[payloadSize]; + if (cachedConfig == NULL_PTR(uint8 *)) { + REPORT_ERROR_STATIC(ErrorManagement::FatalError, + "UDPSServer: Failed to allocate cached CONFIG buffer."); + return false; + } + (void)MemoryOperationsHelper::Copy(cachedConfig, payload, payloadSize); + cachedConfigSize = payloadSize; + return true; } } // namespace MARTe diff --git a/Source/Components/Interfaces/UDPStream/UDPSServer.h b/Source/Components/Interfaces/UDPStream/UDPSServer.h index f00a3f6..8de04d7 100644 --- a/Source/Components/Interfaces/UDPStream/UDPSServer.h +++ b/Source/Components/Interfaces/UDPStream/UDPSServer.h @@ -235,6 +235,7 @@ private: uint16 port; uint32 maxPayloadSize; StreamString multicastGroup; + StreamString interface; uint16 dataPort; bool useMulticast; uint64 clientTimeoutTicks; ///< 0 = disabled diff --git a/vgcore.26968 b/vgcore.26968 deleted file mode 100644 index 21a7b0f..0000000 Binary files a/vgcore.26968 and /dev/null differ