Add TCP control + UDP multicast mode to UDPStreamer

DataSource (C++): - New optional config: MulticastGroup (e.g. "239.0.0.1") and DataPort (default Port+1); when set, enables multicast mode; absent = unicast - Control plane: BasicTCPSocket listener on Port; CONNECT received over TCP triggers HandleTCPConnect() which sends CONFIG via the same TCP connection - Data plane: BasicUDPSocket aimed at MulticastGroup:DataPort; all DATA fragments sent via dataSocket.Write() so any joined client receives them - useMulticast is the single branch point; every unicast code path is unchanged when MulticastGroup is absent - New methods: HandleTCPConnect(), IsMulticast() - 5 new unit tests (ports 44710-44729); all 38 tests passing WebUI hub (Go): - SourceConfig gains MulticastGroup and DataPort fields (JSON, optional) - UDPClient gains multicastGroup/dataPort; Run() routes to new runMulticastSession() when multicastGroup is non-empty - runMulticastSession(): TCP dial for CONNECT/CONFIG, net.ListenMulticastUDP for DATA, background goroutine watches TCP for DISCONNECT/close - All existing sm.Add() call sites updated (unicast callers pass "", 0) Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-26 22:56:27 +02:00
parent f85ab8652c
commit b15e637f14
9 changed files with 864 additions and 58 deletions
@@ -51,6 +51,12 @@ 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. */
+static const uint16 UDPS_DEFAULT_DATA_PORT_OFFSET = 1u;
+
+/** Maximum pending TCP connections on the listener backlog. */
+static const int32 UDPS_TCP_MAX_CONNECTIONS = 4;
+
 /** Default max payload per UDP datagram (bytes). */
 static const uint32 UDPS_DEFAULT_MAX_PAYLOAD = 1400u;

@@ -108,6 +114,9 @@ UDPStreamer::UDPStreamer() :
    publishMode      = UDPStreamerPublishStrict;
    minRefreshRate   = 0.0;
    flushPeriodTicks = 0u;
+    dataPort         = UDPS_DEFAULT_PORT + UDPS_DEFAULT_DATA_PORT_OFFSET;
+    useMulticast     = false;
+    tcpClient        = NULL_PTR(BasicTCPSocket *);
    numSigs          = 0u;
    signalInfos      = NULL_PTR(UDPStreamerSignalInfo *);
    readyBuffer      = NULL_PTR(uint8 *);
@@ -148,6 +157,19 @@ UDPStreamer::~UDPStreamer() {
        (void) clientSocket.Close();
    }

+    /* Multicast-mode cleanup */
+    if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
+        (void) tcpClient->Close();
+        delete tcpClient;
+        tcpClient = NULL_PTR(BasicTCPSocket *);
+    }
+    if (tcpListener.IsValid()) {
+        (void) tcpListener.Close();
+    }
+    if (dataSocket.IsValid()) {
+        (void) dataSocket.Close();
+    }
+
    if (signalInfos != NULL_PTR(UDPStreamerSignalInfo *)) {
        delete[] signalInfos;
        signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
@@ -255,6 +277,40 @@ bool UDPStreamer::Initialise(StructuredDataI &data) {
        }
    }

+    if (ok) {
+        StreamString mcastStr = "";
+        (void) data.Read("MulticastGroup", mcastStr);
+        if (mcastStr.Size() > 0u) {
+            multicastGroup = mcastStr;
+            useMulticast   = true;
+
+            uint16 dp = 0u;
+            if (!data.Read("DataPort", dp)) {
+                dp = port + UDPS_DEFAULT_DATA_PORT_OFFSET;
+                REPORT_ERROR(ErrorManagement::Information,
+                             "DataPort not specified; using Port+1 = %u.",
+                             static_cast<uint32>(dp));
+            }
+            if ((dp == 0u) || (dp == port)) {
+                REPORT_ERROR(ErrorManagement::ParametersError,
+                             "DataPort %u must be non-zero and differ from Port %u.",
+                             static_cast<uint32>(dp), static_cast<uint32>(port));
+                ok = false;
+            }
+            else {
+                dataPort = dp;
+                REPORT_ERROR(ErrorManagement::Information,
+                             "Multicast mode: group=%s, controlPort=%u, dataPort=%u.",
+                             multicastGroup.Buffer(),
+                             static_cast<uint32>(port),
+                             static_cast<uint32>(dataPort));
+            }
+        }
+        else {
+            useMulticast = false;
+        }
+    }
+
    return ok;
 }

@@ -595,25 +651,63 @@ bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
                                   const char8 *const nextStateName) {
    bool ok = true;

-    /* Open server socket (idempotent: skip if already valid) */
-    if (!serverSocket.IsValid()) {
-        ok = serverSocket.Open();
-        if (!ok) {
-            REPORT_ERROR(ErrorManagement::FatalError,
-                         "Could not open server UDP socket.");
-        }
-        if (ok) {
-            ok = serverSocket.Listen(port);
+    if (useMulticast) {
+        /* Open TCP listener for control (CONNECT/DISCONNECT) */
+        if (!tcpListener.IsValid()) {
+            ok = tcpListener.Open();
+            if (ok) {
+                ok = tcpListener.Listen(port, UDPS_TCP_MAX_CONNECTIONS);
+            }
            if (!ok) {
                REPORT_ERROR(ErrorManagement::FatalError,
-                             "Could not bind server socket to port %u.",
+                             "Could not open TCP listener on port %u.",
                             static_cast<uint32>(port));
            }
+            /* SetBlocking(false) makes WaitConnection(TimeoutType(0u)) non-blocking.
+             * If it fails, the select() guard in Execute() protects us. */
+            if (ok) {
+                if (!tcpListener.SetBlocking(false)) {
+                    REPORT_ERROR(ErrorManagement::Warning,
+                                 "SetBlocking(false) on TCP listener failed; "
+                                 "using select() guard only.");
+                }
+            }
+        }
+
+        /* Open UDP data socket aimed at multicast group */
+        if (ok && !dataSocket.IsValid()) {
+            ok = dataSocket.Open();
+            if (ok) {
+                ok = dataSocket.Connect(multicastGroup.Buffer(), dataPort);
+            }
+            if (!ok) {
+                REPORT_ERROR(ErrorManagement::FatalError,
+                             "Could not connect data socket to %s:%u.",
+                             multicastGroup.Buffer(),
+                             static_cast<uint32>(dataPort));
+            }
+        }
+    }
+    else {
+        /* Unicast mode: existing UDP server socket (idempotent: skip if already valid) */
+        if (!serverSocket.IsValid()) {
+            ok = serverSocket.Open();
+            if (!ok) {
+                REPORT_ERROR(ErrorManagement::FatalError,
+                             "Could not open server UDP socket.");
+            }
+            if (ok) {
+                ok = serverSocket.Listen(port);
+                if (!ok) {
+                    REPORT_ERROR(ErrorManagement::FatalError,
+                                 "Could not bind server socket to port %u.",
+                                 static_cast<uint32>(port));
+                }
+            }
        }
-        /* Socket stays blocking; Read() uses the timeout via select() internally. */
    }

-    /* Start the background thread (idempotent) */
+    /* Start the background thread (idempotent; shared by both modes) */
    if (ok && (executor.GetStatus() == EmbeddedThreadI::OffState)) {
        executor.SetName(GetName());
        executor.SetCPUMask(ProcessorType(cpuMask));
@@ -673,19 +767,91 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
            dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
        bool dataReady = (waitErr == ErrorManagement::NoError);

-        /* --- Poll server socket for incoming client commands (non-blocking) --- */
-        uint8 cmdBuf[256u];
-        Handle sockFd = serverSocket.GetReadHandle();
-        fd_set rfds;
-        FD_ZERO(&rfds);
-        FD_SET(static_cast<int>(sockFd), &rfds);
-        struct timeval tv = { 0, 0 };  /* non-blocking poll */
-        int nReady = select(static_cast<int>(sockFd) + 1, &rfds, NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
-        if (nReady > 0) {
-            uint32 recvSize = static_cast<uint32>(sizeof(cmdBuf));
-            bool received = serverSocket.Read(reinterpret_cast<char8 *>(cmdBuf), recvSize);
-            if (received && (recvSize >= static_cast<uint32>(sizeof(UDPSPacketHeader)))) {
-                HandleClientCommand(cmdBuf, recvSize);
+        /* --- Poll for incoming control commands --- */
+        uint32 hdrSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
+        if (useMulticast) {
+            /* Multicast mode: non-blocking TCP accept + poll existing TCP client */
+            Handle listenFd = tcpListener.GetReadHandle();
+            fd_set rfdL;
+            FD_ZERO(&rfdL);
+            FD_SET(static_cast<int>(listenFd), &rfdL);
+            struct timeval tvL = { 0, 0 };
+            if (select(static_cast<int>(listenFd) + 1, &rfdL,
+                       NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvL) > 0) {
+                BasicTCPSocket *newClient = tcpListener.WaitConnection(TimeoutType(0u));
+                if (newClient != NULL_PTR(BasicTCPSocket *)) {
+                    /* Evict any existing client */
+                    if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
+                        (void) tcpClient->Close();
+                        delete tcpClient;
+                        tcpClient       = NULL_PTR(BasicTCPSocket *);
+                        clientConnected = false;
+                    }
+                    tcpClient = newClient;
+                    /* Read CONNECT packet from the new TCP connection */
+                    uint8 connBuf[sizeof(UDPSPacketHeader)];
+                    uint32 recvSize = hdrSize;
+                    bool readOk = tcpClient->Read(
+                        reinterpret_cast<char8 *>(connBuf), recvSize);
+                    if (readOk && (recvSize >= hdrSize)) {
+                        HandleTCPConnect(connBuf, recvSize);
+                    }
+                    else {
+                        REPORT_ERROR(ErrorManagement::Warning,
+                                     "TCP: incomplete CONNECT (%u bytes); closing.",
+                                     recvSize);
+                        (void) tcpClient->Close();
+                        delete tcpClient;
+                        tcpClient = NULL_PTR(BasicTCPSocket *);
+                    }
+                }
+            }
+
+            /* Poll existing TCP client for DISCONNECT or closed connection */
+            if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
+                Handle cfd = tcpClient->GetReadHandle();
+                fd_set rfdC;
+                FD_ZERO(&rfdC);
+                FD_SET(static_cast<int>(cfd), &rfdC);
+                struct timeval tvC = { 0, 0 };
+                if (select(static_cast<int>(cfd) + 1, &rfdC,
+                           NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvC) > 0) {
+                    uint8 cmdBuf[256u];
+                    uint32 cmdSize = static_cast<uint32>(sizeof(cmdBuf));
+                    bool readOk = tcpClient->Read(
+                        reinterpret_cast<char8 *>(cmdBuf), cmdSize);
+                    if (readOk && (cmdSize >= hdrSize)) {
+                        HandleClientCommand(cmdBuf, cmdSize);
+                    }
+                    else {
+                        /* Zero-byte / error = TCP peer closed */
+                        REPORT_ERROR(ErrorManagement::Information,
+                                     "TCP client disconnected (read %u bytes).", cmdSize);
+                        (void) tcpClient->Close();
+                        delete tcpClient;
+                        tcpClient       = NULL_PTR(BasicTCPSocket *);
+                        clientConnected = false;
+                    }
+                }
+            }
+        }
+        else {
+            /* Unicast mode: existing UDP serverSocket non-blocking poll */
+            uint8 cmdBuf[256u];
+            Handle sockFd = serverSocket.GetReadHandle();
+            fd_set rfds;
+            FD_ZERO(&rfds);
+            FD_SET(static_cast<int>(sockFd), &rfds);
+            struct timeval tv = { 0, 0 };
+            int nReady = select(static_cast<int>(sockFd) + 1, &rfds,
+                                NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
+            if (nReady > 0) {
+                uint32 recvSize = static_cast<uint32>(sizeof(cmdBuf));
+                bool received = serverSocket.Read(
+                    reinterpret_cast<char8 *>(cmdBuf), recvSize);
+                if (received && (recvSize >= hdrSize)) {
+                    HandleClientCommand(cmdBuf, recvSize);
+                }
            }
        }

@@ -734,7 +900,16 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {

    if (info.GetStage() == ExecutionInfo::TerminationStage) {
        if (clientConnected) {
-            (void) clientSocket.Close();
+            if (useMulticast) {
+                if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
+                    (void) tcpClient->Close();
+                    delete tcpClient;
+                    tcpClient = NULL_PTR(BasicTCPSocket *);
+                }
+            }
+            else {
+                (void) clientSocket.Close();
+            }
            clientConnected = false;
        }
        REPORT_ERROR(ErrorManagement::Information,
@@ -754,6 +929,10 @@ void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
    }

    if (hdr->type == UDPS_TYPE_CONNECT) {
+        if (useMulticast) {
+            /* In multicast mode, CONNECT is handled by HandleTCPConnect(); ignore here */
+            return;
+        }
        InternetHost src = serverSocket.GetSource();

        /* Disconnect any previous client */
@@ -802,9 +981,12 @@ void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
    else if (hdr->type == UDPS_TYPE_DISCONNECT) {
        REPORT_ERROR(ErrorManagement::Information, "Client sent DISCONNECT.");
        if (clientConnected) {
-            if (clientSocket.IsValid()) {
-                (void) clientSocket.Close();
+            if (!useMulticast) {
+                if (clientSocket.IsValid()) {
+                    (void) clientSocket.Close();
+                }
            }
+            /* In multicast mode, tcpClient cleanup is done by the Execute() poll caller */
            clientConnected = false;
        }
    }
@@ -1021,7 +1203,12 @@ bool UDPStreamer::SendFragmented(uint8 type,
        }

        uint32 sendSize = headerSize + chunkSize;
-        ok = clientSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
+        if (useMulticast) {
+            ok = dataSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
+        }
+        else {
+            ok = clientSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
+        }
        if (!ok) {
            REPORT_ERROR(ErrorManagement::Warning,
                         "Fragment %u/%u send failed.", f + 1u, totalFrags);
@@ -1048,6 +1235,78 @@ uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
    return code;
 }

+void UDPStreamer::HandleTCPConnect(const uint8 *buf, uint32 size) {
+    if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
+        return;
+    }
+    const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
+    if (hdr->magic != UDPS_MAGIC) {
+        REPORT_ERROR(ErrorManagement::Warning,
+                     "TCP CONNECT: bad magic 0x%08X.", hdr->magic);
+        return;
+    }
+    if (hdr->type != UDPS_TYPE_CONNECT) {
+        REPORT_ERROR(ErrorManagement::Warning,
+                     "TCP CONNECT: unexpected packet type %u.", hdr->type);
+        return;
+    }
+
+    clientConnected = true;
+    REPORT_ERROR(ErrorManagement::Information,
+                 "TCP client connected; DATA will multicast to %s:%u.",
+                 multicastGroup.Buffer(), static_cast<uint32>(dataPort));
+
+    /* Build CONFIG payload and send as a single TCP message (no fragmentation needed) */
+    uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u;
+    HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
+    uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
+    if (cfgBuf == NULL_PTR(uint8 *)) {
+        REPORT_ERROR(ErrorManagement::FatalError,
+                     "Could not allocate CONFIG buffer.");
+        clientConnected = false;
+        return;
+    }
+
+    uint32 cfgPayloadSize = 0u;
+    bool built = BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize);
+    if (built) {
+        uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
+        uint32 totalSize  = headerSize + cfgPayloadSize;
+        uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(totalSize));
+        if (sendBuf != NULL_PTR(uint8 *)) {
+            UDPSPacketHeader *outHdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
+            outHdr->magic          = UDPS_MAGIC;
+            outHdr->type           = UDPS_TYPE_CONFIG;
+            outHdr->counter        = 0u;
+            outHdr->fragmentIdx    = 0u;
+            outHdr->totalFragments = 1u;
+            outHdr->payloadBytes   = cfgPayloadSize;
+            (void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
+                                                cfgBuf, cfgPayloadSize);
+            bool writeOk = tcpClient->Write(
+                reinterpret_cast<const char8 *>(sendBuf), totalSize);
+            if (!writeOk) {
+                REPORT_ERROR(ErrorManagement::Warning,
+                             "TCP: failed to send CONFIG packet.");
+                clientConnected = false;
+            }
+            heap->Free(reinterpret_cast<void *&>(sendBuf));
+        }
+        else {
+            REPORT_ERROR(ErrorManagement::FatalError,
+                         "Could not allocate TCP send buffer.");
+            clientConnected = false;
+        }
+    }
+    else {
+        REPORT_ERROR(ErrorManagement::Warning,
+                     "BuildConfigPayload failed; client not fully connected.");
+        clientConnected = false;
+    }
+
+    heap->Free(reinterpret_cast<void *&>(cfgBuf));
+}
+
 uint16 UDPStreamer::GetPort() const {
    return port;
 }
@@ -1060,6 +1319,10 @@ bool UDPStreamer::IsClientConnected() const {
    return clientConnected;
 }

+bool UDPStreamer::IsMulticast() const {
+    return useMulticast;
+}
+
 CLASS_REGISTER(UDPStreamer, "1.0")

 } /* namespace MARTe */
@@ -31,6 +31,7 @@
 /*---------------------------------------------------------------------------*/
 /*                        Project header includes                            */
 /*---------------------------------------------------------------------------*/
+#include "BasicTCPSocket.h"
 #include "BasicUDPSocket.h"
 #include "CompilerTypes.h"
 #include "EmbeddedServiceMethodBinderI.h"
@@ -275,6 +276,11 @@ public:
     */
    bool IsClientConnected() const;

+    /**
+     * @brief Returns true when multicast mode is active (MulticastGroup was set in config).
+     */
+    bool IsMulticast() const;
+
 private:
    /**
     * @brief Serializes the CONFIG payload into buf and sets payloadSize.
@@ -296,10 +302,16 @@ private:
    bool SendFragmented(uint8 type, uint32 counter, const uint8 *payload, uint32 payloadSize);

    /**
-     * @brief Handles a single received command packet from a client.
+     * @brief Handles a single received command packet from a client (unicast mode).
     */
    void HandleClientCommand(const uint8 *buf, uint32 size);

+    /**
+     * @brief Handles a CONNECT received on the TCP control socket (multicast mode).
+     * @details Validates magic+type, sets clientConnected, builds and sends CONFIG via tcpClient.
+     */
+    void HandleTCPConnect(const uint8 *buf, uint32 size);
+
    /**
     * @brief Maps a MARTe2 TypeDescriptor to the UDPS_TYPECODE_* constants.
     */
@@ -330,11 +342,19 @@ private:
    uint8  *wireBuffer;      /**< Pre-allocated buffer for the serialized DATA payload */
    uint32  totalWireBytes;  /**< Total bytes of all signals after quantization + 8-byte timestamp prefix */

-    /* Networking */
-    BasicUDPSocket  serverSocket;    /**< Bound to port; receives client commands */
-    BasicUDPSocket  clientSocket;    /**< Configured to send to the connected client */
+    /* Networking — unicast mode */
+    BasicUDPSocket  serverSocket;    /**< Bound to port; receives client commands (unicast) */
+    BasicUDPSocket  clientSocket;    /**< Configured to send to the connected client (unicast) */
    volatile bool   clientConnected; /**< True when a client has successfully connected */

+    /* Networking — multicast mode (only used when useMulticast == true) */
+    StreamString    multicastGroup;  /**< Multicast group IP, e.g. "239.0.0.1"; empty = unicast */
+    uint16          dataPort;        /**< UDP port for DATA datagrams in multicast mode */
+    bool            useMulticast;    /**< Derived from multicastGroup.Size() > 0 in Initialise() */
+    BasicTCPSocket  tcpListener;     /**< TCP server socket: accepts control connections */
+    BasicTCPSocket *tcpClient;       /**< Accepted TCP control connection (heap by WaitConnection) */
+    BasicUDPSocket  dataSocket;      /**< UDP socket aimed at multicastGroup:dataPort */
+
    /* Thread management */
    SingleThreadService executor;    /**< Background thread service */