Initial release

2026-05-29 13:29:59 +02:00
commit 617b5bd712
110 changed files with 29234 additions and 0 deletions
@@ -0,0 +1,618 @@
+#include "BasicTCPSocket.h"
+#include "ConfigurationDatabase.h"
+#include "DebugService.h"
+#include "GlobalObjectsDatabase.h"
+#include "HighResolutionTimer.h"
+#include "LoggerService.h"
+#include "Message.h"
+#include "ObjectRegistryDatabase.h"
+#include "ReferenceT.h"
+#include "Sleep.h"
+#include "StreamString.h"
+#include "StringHelper.h"
+#include "Threads.h"
+#include "TimeoutType.h"
+#include "UDPSProtocol.h"
+
+namespace MARTe {
+
+CLASS_REGISTER(DebugService, "1.0")
+
+// C++98 ODR definitions for static constants
+const uint32 DebugService::CMD_RATE_LIMIT;
+const uint32 DebugService::CLIENT_IDLE_TIMEOUT_MS;
+const uint32 DebugService::INPUT_BUFFER_MAX;
+
+// ---------------------------------------------------------------------------
+// Constructor / Destructor
+// ---------------------------------------------------------------------------
+
+DebugService::DebugService()
+    : DebugServiceBase(),
+      EmbeddedServiceMethodBinderI(),
+      binderServer(this, ServiceBinder::ServerType),
+      binderStreamer(this, ServiceBinder::StreamerType),
+      threadService(binderServer),
+      streamerService(binderStreamer) {
+    controlPort          = 0u;
+    streamPort           = 8081u;
+    logPort              = 8082u;
+    streamIP             = "127.0.0.1";
+    isServer             = false;
+    suppressTimeoutLogs  = true;
+    activeClient         = NULL_PTR(BasicTCPSocket *);
+    cmdCountInWindow     = 0u;
+    cmdWindowStartMs     = 0u;
+    lastDataTimeMs       = 0u;
+    inputBuffer          = "";
+
+    // UDPS members
+    udpsNumSlots         = 0u;
+    udpsDataPayload      = NULL_PTR(uint8 *);
+    udpsDataPayloadSize  = 0u;
+    udpsPacketCounter    = 0u;
+    udpsConfigPending    = false;
+}
+
+DebugService::~DebugService() {
+    if (DebugServiceI::GetInstance() == this) {
+        DebugServiceI::SetInstance(NULL_PTR(DebugServiceI *));
+    }
+    threadService.Stop();
+    streamerService.Stop();
+    tcpServer.Close();
+    udpSocket.Close();
+    if (activeClient != NULL_PTR(BasicTCPSocket *)) {
+        activeClient->Close();
+        delete activeClient;
+        activeClient = NULL_PTR(BasicTCPSocket *);
+    }
+    if (udpsDataPayload != NULL_PTR(uint8 *)) {
+        delete[] udpsDataPayload;
+        udpsDataPayload = NULL_PTR(uint8 *);
+    }
+    // signals owned by DebugServiceBase destructor
+}
+
+// ---------------------------------------------------------------------------
+// Initialise
+// ---------------------------------------------------------------------------
+
+bool DebugService::Initialise(StructuredDataI &data) {
+    if (!ReferenceContainer::Initialise(data)) return false;
+
+    uint32 port = 0u;
+    if (data.Read("ControlPort", port)) {
+        controlPort = (uint16)port;
+    } else {
+        (void)data.Read("TcpPort", port);
+        controlPort = (uint16)port;
+    }
+
+    if (controlPort > 0u) {
+        isServer = true;
+        DebugServiceI::SetInstance(this);
+    }
+
+    port = 8081u;
+    if (data.Read("StreamPort", port)) {
+        streamPort = (uint16)port;
+    } else {
+        (void)data.Read("UdpPort", port);
+        streamPort = (uint16)port;
+    }
+
+    port = 8082u;
+    if (data.Read("LogPort", port)) {
+        logPort = (uint16)port;
+    } else {
+        (void)data.Read("TcpLogPort", port);
+        logPort = (uint16)port;
+    }
+
+    StreamString tempIP;
+    if (data.Read("StreamIP", tempIP)) {
+        streamIP = tempIP;
+    } else {
+        streamIP = "127.0.0.1";
+    }
+
+    uint32 suppress = 1u;
+    if (data.Read("SuppressTimeoutLogs", suppress)) {
+        suppressTimeoutLogs = (suppress == 1u);
+    }
+
+    // Capture only the local subtree — do NOT call MoveToRoot() on the shared CDB.
+    (void)data.Copy(fullConfig);
+
+    if (isServer) {
+        if (!traceBuffer.Init(8 * 1024 * 1024)) return false;
+        PatchRegistry();
+
+        ConfigurationDatabase threadData;
+        threadData.Write("Timeout", (uint32)1000);
+        threadService.Initialise(threadData);
+        streamerService.Initialise(threadData);
+
+        if (!tcpServer.Open())              return false;
+        if (!tcpServer.Listen(controlPort)) return false;
+        if (!udpSocket.Open())              return false;
+        // Note: do NOT bind udpSocket to streamPort here. The Go client
+        // must own that port to receive streamed data. The socket sends
+        // via an ephemeral source port, which is fine for UDP.
+
+        if (threadService.Start()   != ErrorManagement::NoError) return false;
+        if (streamerService.Start() != ErrorManagement::NoError) return false;
+
+        // Send initial (empty) CONFIG so clients know the schema version
+        SendUDPSConfig();
+    }
+    return true;
+}
+
+// ---------------------------------------------------------------------------
+// Transport config hook (called by RebuildConfigFromRegistry in base)
+// ---------------------------------------------------------------------------
+
+void DebugService::RebuildTransportConfig() {
+    const char8 *myName = GetName();
+    if (myName != NULL_PTR(const char8 *)) {
+        if (fullConfig.MoveRelative(myName)) {
+            (void)fullConfig.Write("ControlPort", static_cast<uint32>(controlPort));
+            (void)fullConfig.Write("UdpPort",     static_cast<uint32>(streamPort));
+            (void)fullConfig.Write("LogPort",     static_cast<uint32>(logPort));
+            if (streamIP.Size() > 0u)
+                (void)fullConfig.Write("StreamIP", streamIP.Buffer());
+            (void)fullConfig.MoveToAncestor(1u);
+        }
+    }
+}
+
+// ---------------------------------------------------------------------------
+// SERVICE_INFO hook
+// ---------------------------------------------------------------------------
+
+void DebugService::GetServiceInfo(StreamString &out) {
+    out.Printf("OK SERVICE_INFO TCP_CTRL:%u UDP_STREAM:%u TCP_LOG:%u STATE:%s\n",
+               controlPort, streamPort, logPort,
+               isPaused ? "PAUSED" : "RUNNING");
+}
+
+// ---------------------------------------------------------------------------
+// Execute / HandleMessage (framework boilerplate)
+// ---------------------------------------------------------------------------
+
+ErrorManagement::ErrorType DebugService::Execute(ExecutionInfo &info) {
+    (void)info;
+    return ErrorManagement::FatalError;
+}
+
+ErrorManagement::ErrorType DebugService::HandleMessage(ReferenceT<Message> &data) {
+    (void)data;
+    return ErrorManagement::NoError;
+}
+
+// ---------------------------------------------------------------------------
+// TraceSignal override — sets config-pending flag after base handling
+// ---------------------------------------------------------------------------
+
+uint32 DebugService::TraceSignal(const char8 *name, bool enable, uint32 decimation) {
+    uint32 ret = DebugServiceBase::TraceSignal(name, enable, decimation);
+    udpsConfigPending = true;
+    return ret;
+}
+
+// ---------------------------------------------------------------------------
+// InjectTcpLoggerIfNeeded
+// ---------------------------------------------------------------------------
+
+void DebugService::InjectTcpLoggerIfNeeded() {
+    if (logPort == 0u) return;
+
+    Reference existing = ObjectRegistryDatabase::Instance()->Find("LoggerService");
+    if (existing.IsValid()) {
+        ReferenceContainer *rc =
+            dynamic_cast<ReferenceContainer *>(existing.operator->());
+        if (rc != NULL_PTR(ReferenceContainer *)) {
+            for (uint32 i = 0u; i < rc->Size(); i++) {
+                Reference child = rc->Get(i);
+                // Check by class name to avoid a direct symbol dependency on TcpLogger.so
+                if (child.IsValid()) {
+                    const ClassProperties *cp = child->GetClassProperties();
+                    if (cp != NULL_PTR(const ClassProperties *)) {
+                        if (StringHelper::Compare(cp->GetName(), "TcpLogger") == 0) {
+                            return;  // already has a TcpLogger
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    ConfigurationDatabase lsCdb;
+    (void)lsCdb.Write("Class", "LoggerService");
+    uint32 cpus = 1u;
+    (void)lsCdb.Write("CPUs", cpus);
+    if (lsCdb.CreateRelative("+DebugConsumer")) {
+        (void)lsCdb.Write("Class", "TcpLogger");
+        uint32 p = static_cast<uint32>(logPort);
+        (void)lsCdb.Write("Port", p);
+        (void)lsCdb.MoveToAncestor(1u);
+    }
+    (void)lsCdb.MoveToRoot();
+
+    ReferenceT<LoggerService> ls(
+        "LoggerService", GlobalObjectsDatabase::Instance()->GetStandardHeap());
+    if (!ls.IsValid()) return;
+    ls->SetName("LoggerService");
+    if (!ls->Initialise(lsCdb)) return;
+    (void)ObjectRegistryDatabase::Instance()->Insert(ls);
+}
+
+// ---------------------------------------------------------------------------
+// Server thread
+// ---------------------------------------------------------------------------
+
+ErrorManagement::ErrorType DebugService::Server(ExecutionInfo &info) {
+    if (info.GetStage() == ExecutionInfo::TerminationStage)
+        return ErrorManagement::NoError;
+    if (info.GetStage() == ExecutionInfo::StartupStage) {
+        serverThreadId = Threads::Id();
+        Sleep::MSec(500u);
+        InjectTcpLoggerIfNeeded();
+        return ErrorManagement::NoError;
+    }
+
+    uint64 nowMs = (uint64)((float64)HighResolutionTimer::Counter() *
+                             HighResolutionTimer::Period() * 1000.0);
+
+    if (activeClient == NULL_PTR(BasicTCPSocket *)) {
+        BasicTCPSocket *newClient = tcpServer.WaitConnection(TimeoutType(100));
+        if (newClient != NULL_PTR(BasicTCPSocket *)) {
+            clientMutex.FastLock();
+            activeClient = newClient;
+            clientMutex.FastUnLock();
+            cmdCountInWindow = 0u;
+            cmdWindowStartMs = nowMs;
+            lastDataTimeMs   = nowMs;
+        }
+    } else {
+        if (nowMs - lastDataTimeMs > CLIENT_IDLE_TIMEOUT_MS) {
+            REPORT_ERROR_STATIC(ErrorManagement::Warning,
+                "Server: TCP client idle for >%u ms — closing connection.",
+                CLIENT_IDLE_TIMEOUT_MS);
+            inputBuffer = "";
+            clientMutex.FastLock();
+            activeClient->Close();
+            delete activeClient;
+            activeClient = NULL_PTR(BasicTCPSocket *);
+            clientMutex.FastUnLock();
+            cmdCountInWindow = 0u;
+        } else if (!activeClient->IsConnected()) {
+            inputBuffer = "";
+            clientMutex.FastLock();
+            activeClient->Close();
+            delete activeClient;
+            activeClient = NULL_PTR(BasicTCPSocket *);
+            clientMutex.FastUnLock();
+        } else {
+            char buffer[1024];
+            uint32 size = 1024u;
+            if (activeClient->Read(buffer, size)) {
+                if (size > 0u) {
+                    lastDataTimeMs = nowMs;
+
+                    if (nowMs - cmdWindowStartMs >= 1000u) {
+                        cmdWindowStartMs = nowMs;
+                        cmdCountInWindow = 0u;
+                    }
+
+                    if (inputBuffer.Size() + (uint32)size > INPUT_BUFFER_MAX) {
+                        REPORT_ERROR_STATIC(ErrorManagement::Warning,
+                            "Server: input buffer overflow (>%u bytes without newline) "
+                            "— disconnecting.", INPUT_BUFFER_MAX);
+                        inputBuffer = "";
+                        clientMutex.FastLock();
+                        activeClient->Close();
+                        delete activeClient;
+                        activeClient = NULL_PTR(BasicTCPSocket *);
+                        clientMutex.FastUnLock();
+                        cmdCountInWindow = 0u;
+                    } else {
+                        (void)inputBuffer.Seek(inputBuffer.Size());
+                        uint32 writeSize = (uint32)size;
+                        inputBuffer.Write(buffer, writeSize);
+
+                        const char8 *raw   = inputBuffer.Buffer();
+                        uint32       total = (uint32)inputBuffer.Size();
+                        uint32       lineStart = 0u;
+                        bool         rateLimitExceeded = false;
+
+                        for (uint32 pos = 0u; pos < total && !rateLimitExceeded; pos++) {
+                            if (raw[pos] != '\n') continue;
+                            uint32 len = pos - lineStart;
+                            if (len > 0u && raw[lineStart + len - 1u] == '\r') len--;
+                            if (len > 0u) {
+                                cmdCountInWindow++;
+                                if (cmdCountInWindow > CMD_RATE_LIMIT) {
+                                    REPORT_ERROR_STATIC(ErrorManagement::Warning,
+                                        "Server: client exceeded rate limit (%u cmd/s) "
+                                        "— disconnecting.", CMD_RATE_LIMIT);
+                                    rateLimitExceeded = true;
+                                    break;
+                                }
+                                StreamString command;
+                                uint32 cmdLen = len;
+                                command.Write(raw + lineStart, cmdLen);
+
+                                // Dispatch via base HandleCommand, write response to socket.
+                                StreamString out;
+                                HandleCommand(command, out);
+                                if (out.Size() > 0u) {
+                                    const char8 *wPtr = out.Buffer();
+                                    uint32 remaining  = (uint32)out.Size();
+                                    lastDataTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
+                                                              HighResolutionTimer::Period() * 1000.0);
+                                    while (remaining > 0u) {
+                                        uint32 wrote = remaining;
+                                        if (!activeClient->Write(wPtr, wrote) || wrote == 0u) {
+                                            break;
+                                        }
+                                        wPtr      += wrote;
+                                        remaining -= wrote;
+                                        lastDataTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
+                                                                  HighResolutionTimer::Period() * 1000.0);
+                                    }
+                                }
+                            }
+                            lineStart = pos + 1u;
+                        }
+
+                        if (rateLimitExceeded) {
+                            inputBuffer = "";
+                            clientMutex.FastLock();
+                            activeClient->Close();
+                            delete activeClient;
+                            activeClient = NULL_PTR(BasicTCPSocket *);
+                            clientMutex.FastUnLock();
+                            cmdCountInWindow = 0u;
+                        } else {
+                            StreamString newInputBuffer;
+                            if (lineStart < total) {
+                                uint32 remLen = total - lineStart;
+                                newInputBuffer.Write(raw + lineStart, remLen);
+                            }
+                            inputBuffer = newInputBuffer;
+                        }
+                    }
+                }
+            } else {
+                inputBuffer = "";
+                clientMutex.FastLock();
+                activeClient->Close();
+                delete activeClient;
+                activeClient = NULL_PTR(BasicTCPSocket *);
+                clientMutex.FastUnLock();
+            }
+        }
+    }
+    return ErrorManagement::NoError;
+}
+
+// ---------------------------------------------------------------------------
+// Streamer thread — UDPS binary telemetry
+// ---------------------------------------------------------------------------
+
+ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
+    if (info.GetStage() == ExecutionInfo::TerminationStage) {
+        if (udpsDataPayload != NULL_PTR(uint8 *)) {
+            delete[] udpsDataPayload;
+            udpsDataPayload = NULL_PTR(uint8 *);
+        }
+        return ErrorManagement::NoError;
+    }
+    if (info.GetStage() == ExecutionInfo::StartupStage) {
+        streamerThreadId = Threads::Id();
+        return ErrorManagement::NoError;
+    }
+
+    // Set UDP destination
+    InternetHost dest(streamPort, streamIP.Buffer());
+    (void)udpSocket.SetDestination(dest);
+
+    // a) If config has changed, rebuild and send CONFIG packet
+    if (udpsConfigPending) {
+        SendUDPSConfig();
+        udpsConfigPending = false;
+    }
+
+    // b) Drain traceBuffer — pack each sample into udpsDataPayload
+    bool anyData = false;
+    uint32 id, size;
+    uint64 ts;
+    uint8  udpsSampleBuf[UDPS_MAX_SAMPLE_BYTES];
+
+    while (traceBuffer.Pop(id, ts, udpsSampleBuf, size, UDPS_MAX_SAMPLE_BYTES)) {
+        // Find matching slot by internalID
+        for (uint32 i = 0u; i < udpsNumSlots; i++) {
+            if (udpsSlots[i].internalID == id) {
+                if ((udpsDataPayload != NULL_PTR(uint8 *)) &&
+                    (8u + udpsSlots[i].wireOffset + udpsSlots[i].wireSize <= udpsDataPayloadSize)) {
+                    uint32 copySize = size;
+                    if (copySize > udpsSlots[i].wireSize) copySize = udpsSlots[i].wireSize;
+                    memcpy(udpsDataPayload + 8u + udpsSlots[i].wireOffset, udpsSampleBuf, copySize);
+                    udpsSlots[i].everFilled = true;
+                }
+                anyData = true;
+                break;
+            }
+        }
+    }
+
+    // c) If we have data, stamp with HRT and send
+    if (anyData && udpsNumSlots > 0u && udpsDataPayload != NULL_PTR(uint8 *)) {
+        uint64 hrt = HighResolutionTimer::Counter();
+        memcpy(udpsDataPayload, &hrt, 8u);
+        SendUDPSFragmented(UDPS_TYPE_DATA, udpsDataPayload, udpsDataPayloadSize);
+        udpsPacketCounter++;
+    }
+
+    if (!anyData) {
+        Sleep::MSec(1u);
+    }
+
+    return ErrorManagement::NoError;
+}
+
+// ---------------------------------------------------------------------------
+// SendUDPSConfig — build and send a UDPS CONFIG packet
+// ---------------------------------------------------------------------------
+
+bool DebugService::SendUDPSConfig() {
+    // Snapshot traced signals under mutex
+    mutex.FastLock();
+
+    // Count traced signals
+    uint32 tracedCount = 0u;
+    for (uint32 i = 0u; i < signals.GetNumberOfElements(); i++) {
+        if (signals[i]->isTracing) {
+            tracedCount++;
+        }
+    }
+
+    // Build CONFIG payload into udpsTxBuf starting at UDPS_HEADER_SIZE offset
+    uint8 *payload = udpsTxBuf + UDPS_HEADER_SIZE;
+
+    // Write numTraced (uint32 LE)
+    memcpy(payload, &tracedCount, 4u);
+    uint32 payloadOffset = 4u;
+
+    // Build slot table in parallel
+    uint32 currentOffset = 0u;
+    uint32 newNumSlots = 0u;
+    uint32 totalWireBytes = 0u;
+
+    for (uint32 i = 0u; i < signals.GetNumberOfElements(); i++) {
+        if (!signals[i]->isTracing) continue;
+        if (newNumSlots >= UDPS_MAX_SLOTS) break;
+
+        uint8 typeCode = UDPSTypeDescriptorToCode(signals[i]->type);
+        if (typeCode == UDPS_TYPECODE_UNKNOWN) {
+            // Skip unsupported types
+            continue;
+        }
+
+        uint32 numElements = signals[i]->numberOfElements;
+        if (numElements == 0u) numElements = 1u;
+
+        uint32 numRows, numCols;
+        if (signals[i]->numberOfDimensions >= 2u) {
+            // For matrix: approximate square root
+            numCols = numElements;
+            numRows = 1u;
+        } else if (signals[i]->numberOfDimensions == 1u) {
+            numRows = numElements;
+            numCols = 1u;
+        } else {
+            numRows = 1u;
+            numCols = 1u;
+        }
+
+        uint32 wireSize = UDPSTypeCodeByteSize(typeCode) * numElements;
+
+        // Write signal descriptor (136 bytes) to payload
+        if (payloadOffset + UDPS_SIGNAL_DESC_SIZE <= sizeof(udpsTxBuf) - UDPS_HEADER_SIZE - 1u) {
+            UDPSSignalDescriptor *desc = reinterpret_cast<UDPSSignalDescriptor *>(payload + payloadOffset);
+            memset(desc, 0, UDPS_SIGNAL_DESC_SIZE);
+            if (signals[i]->name.Size() > 0u) {
+                strncpy(desc->name, signals[i]->name.Buffer(), UDPS_MAX_SIGNAL_NAME - 1u);
+            }
+            desc->typeCode       = typeCode;
+            desc->quantType      = UDPS_QUANT_NONE;
+            desc->numDimensions  = signals[i]->numberOfDimensions;
+            desc->numRows        = numRows;
+            desc->numCols        = numCols;
+            desc->rangeMin       = 0.0;
+            desc->rangeMax       = 0.0;
+            desc->timeMode       = UDPS_TIMEMODE_PACKET;
+            desc->samplingRate   = 0.0;
+            desc->timeSignalIdx  = UDPS_NO_TIME_SIGNAL;
+            // unit stays zero
+
+            payloadOffset += UDPS_SIGNAL_DESC_SIZE;
+        }
+
+        // Fill slot table entry
+        udpsSlots[newNumSlots].internalID  = signals[i]->internalID;
+        udpsSlots[newNumSlots].wireOffset  = currentOffset;
+        udpsSlots[newNumSlots].wireSize    = wireSize;
+        udpsSlots[newNumSlots].everFilled  = false;
+        newNumSlots++;
+
+        currentOffset    += wireSize;
+        totalWireBytes   += wireSize;
+    }
+
+    // Write publish mode byte
+    if (payloadOffset < sizeof(udpsTxBuf) - UDPS_HEADER_SIZE) {
+        payload[payloadOffset] = UDPS_PUBLISH_STRICT;
+        payloadOffset++;
+    }
+
+    udpsNumSlots = newNumSlots;
+
+    mutex.FastUnLock();
+
+    // Reallocate data payload buffer
+    if (udpsDataPayload != NULL_PTR(uint8 *)) {
+        delete[] udpsDataPayload;
+        udpsDataPayload = NULL_PTR(uint8 *);
+    }
+    udpsDataPayloadSize = 8u + totalWireBytes; // 8 bytes HRT + signal data
+    if (udpsDataPayloadSize > 0u) {
+        udpsDataPayload = new uint8[udpsDataPayloadSize];
+        memset(udpsDataPayload, 0, udpsDataPayloadSize);
+    }
+
+    // Send CONFIG packet
+    SendUDPSFragmented(UDPS_TYPE_CONFIG, payload, payloadOffset);
+
+    return true;
+}
+
+// ---------------------------------------------------------------------------
+// SendUDPSFragmented — fragment and send a UDPS packet
+// ---------------------------------------------------------------------------
+
+bool DebugService::SendUDPSFragmented(uint8 type, const uint8 *payload, uint32 payloadSize) {
+    if (payloadSize <= UDPS_MAX_PAYLOAD) {
+        // Single datagram
+        UDPSBuildHeader(udpsTxBuf, type, udpsPacketCounter, 0u, 1u, payloadSize);
+        if (payload != (udpsTxBuf + UDPS_HEADER_SIZE)) {
+            memcpy(udpsTxBuf + UDPS_HEADER_SIZE, payload, payloadSize);
+        }
+        uint32 toWrite = UDPS_HEADER_SIZE + payloadSize;
+        (void)udpSocket.Write(reinterpret_cast<char8 *>(udpsTxBuf), toWrite);
+    } else {
+        // Fragmented
+        uint32 numFrags = (payloadSize + UDPS_MAX_PAYLOAD - 1u) / UDPS_MAX_PAYLOAD;
+        uint32 offset = 0u;
+        for (uint32 i = 0u; i < numFrags; i++) {
+            uint32 chunkSize = UDPS_MAX_PAYLOAD;
+            if (offset + chunkSize > payloadSize) {
+                chunkSize = payloadSize - offset;
+            }
+            UDPSBuildHeader(udpsTxBuf, type, udpsPacketCounter,
+                            static_cast<uint16>(i),
+                            static_cast<uint16>(numFrags),
+                            chunkSize);
+            memcpy(udpsTxBuf + UDPS_HEADER_SIZE, payload + offset, chunkSize);
+            uint32 toWrite = UDPS_HEADER_SIZE + chunkSize;
+            (void)udpSocket.Write(reinterpret_cast<char8 *>(udpsTxBuf), toWrite);
+            offset += chunkSize;
+        }
+    }
+    return true;
+}
+
+} // namespace MARTe