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
@@ -474,7 +474,7 @@ func (h *Hub) Run() {

 			case "wsAddSource":
 				if h.sm != nil {
-					go func(label, addr string) { h.sm.Add(label, addr) }(cmd.label, cmd.addr)
+					go func(label, addr string) { h.sm.Add(label, addr, "", 0) }(cmd.label, cmd.addr)
 				}

 			case "wsRemoveSource":
@@ -44,7 +44,7 @@ func main() {
 	}
 	for _, arg := range sourceArgs {
 		label, addr := ParseSourceArg(arg)
-		sm.Add(label, addr)
+		sm.Add(label, addr, "", 0)
 	}

 	sub, err := fs.Sub(staticFiles, "static")
@@ -11,16 +11,20 @@ import (

 // SourceConfig is the serialisable description of one data source (for file save/load).
 type SourceConfig struct {
-	Label string `json:"label"`
-	Addr  string `json:"addr"`
+	Label          string `json:"label"`
+	Addr           string `json:"addr"`
+	MulticastGroup string `json:"multicastGroup,omitempty"` // "" = unicast mode
+	DataPort       int    `json:"dataPort,omitempty"`        // 0 = Addr port+1
 }

 // managedSource is the SourceManager's view of one running source.
 type managedSource struct {
-	id     string
-	label  string
-	addr   string
-	client *UDPClient
+	id             string
+	label          string
+	addr           string
+	multicastGroup string
+	dataPort       int
+	client         *UDPClient
 }

 // SourceManager owns the lifecycle of all active data sources.
@@ -48,14 +52,20 @@ func (sm *SourceManager) genID() string {
 }

 // Add creates a new source, registers it in the hub, and starts connecting.
+// multicastGroup: multicast IP (e.g. "239.0.0.1") or "" for unicast mode.
+// dataPort: UDP data port in multicast mode; 0 = Addr port+1.
 // Returns the source ID.
-func (sm *SourceManager) Add(label, addr string) string {
+func (sm *SourceManager) Add(label, addr, multicastGroup string, dataPort int) string {
 	if label == "" {
 		label = addr
 	}
 	id := sm.genID()
-	c := NewUDPClient(addr, id, sm.hub)
-	ms := &managedSource{id: id, label: label, addr: addr, client: c}
+	c := NewUDPClient(addr, id, sm.hub, multicastGroup, dataPort)
+	ms := &managedSource{
+		id: id, label: label, addr: addr,
+		multicastGroup: multicastGroup, dataPort: dataPort,
+		client: c,
+	}

 	sm.mu.Lock()
 	sm.sources[id] = ms
@@ -90,7 +100,12 @@ func (sm *SourceManager) Save() error {
 	sm.mu.RLock()
 	cfgs := make([]SourceConfig, 0, len(sm.sources))
 	for _, ms := range sm.sources {
-		cfgs = append(cfgs, SourceConfig{Label: ms.label, Addr: ms.addr})
+		cfgs = append(cfgs, SourceConfig{
+			Label:          ms.label,
+			Addr:           ms.addr,
+			MulticastGroup: ms.multicastGroup,
+			DataPort:       ms.dataPort,
+		})
 	}
 	sm.mu.RUnlock()

@@ -113,7 +128,7 @@ func (sm *SourceManager) Load(path string) error {
 	}
 	sm.filePath = path
 	for _, cfg := range cfgs {
-		sm.Add(cfg.Label, cfg.Addr)
+		sm.Add(cfg.Label, cfg.Addr, cfg.MulticastGroup, cfg.DataPort)
 	}
 	return nil
 }
@@ -1,8 +1,10 @@
 package main

 import (
+	"io"
 	"log"
 	"net"
+	"strconv"
 	"time"
 )

@@ -13,22 +15,31 @@ const (
 	udpRcvBufSize  = 8 * 1024 * 1024 // 8 MB OS receive buffer — absorbs bursts at high data rates
 )

-// UDPClient manages the UDP connection to one MARTe2 streamer source.
+// UDPClient manages the connection to one MARTe2 streamer source.
+// In unicast mode (multicastGroup == "") it uses a single UDP socket for control and data.
+// In multicast mode it uses TCP for control (CONNECT/CONFIG/DISCONNECT) and
+// joins a UDP multicast group to receive DATA packets.
 type UDPClient struct {
-	serverAddr string
-	sourceID   string
-	hub        *Hub
+	serverAddr     string
+	sourceID       string
+	hub            *Hub
+	multicastGroup string // "" = unicast mode
+	dataPort       int    // UDP data port in multicast mode; 0 = serverAddr port+1

 	stopCh chan struct{}
 }

 // NewUDPClient creates a UDPClient bound to a specific source ID. Call Run() in a goroutine.
-func NewUDPClient(serverAddr, sourceID string, hub *Hub) *UDPClient {
+// multicastGroup: multicast IP (e.g. "239.0.0.1") or "" for unicast.
+// dataPort: UDP data port; 0 = serverAddr port+1.
+func NewUDPClient(serverAddr, sourceID string, hub *Hub, multicastGroup string, dataPort int) *UDPClient {
 	return &UDPClient{
-		serverAddr: serverAddr,
-		sourceID:   sourceID,
-		hub:        hub,
-		stopCh:     make(chan struct{}),
+		serverAddr:     serverAddr,
+		sourceID:       sourceID,
+		hub:            hub,
+		multicastGroup: multicastGroup,
+		dataPort:       dataPort,
+		stopCh:         make(chan struct{}),
 	}
 }

@@ -47,10 +58,16 @@ func (u *UDPClient) Run() {
 		}

 		u.hub.SetSourceState(u.sourceID, "connecting")
-		log.Printf("[%s] udp: connecting to %s", u.sourceID, u.serverAddr)
-		err := u.runSession()
+		log.Printf("[%s] connecting to %s", u.sourceID, u.serverAddr)
+
+		var err error
+		if u.multicastGroup != "" {
+			err = u.runMulticastSession()
+		} else {
+			err = u.runSession()
+		}
 		if err != nil {
-			log.Printf("[%s] udp: session ended: %v", u.sourceID, err)
+			log.Printf("[%s] session ended: %v", u.sourceID, err)
 		}
 		u.hub.SetSourceState(u.sourceID, "disconnected")

@@ -165,3 +182,158 @@ func (u *UDPClient) runSession() error {
 		}
 	}
 }
+
+// runMulticastSession handles the multicast mode session:
+//   - TCP connection to serverAddr for control (CONNECT → CONFIG, DISCONNECT)
+//   - UDP multicast socket joined to multicastGroup:dataPort for DATA packets
+func (u *UDPClient) runMulticastSession() error {
+	// 1. Resolve TCP control address
+	tcpAddr, err := net.ResolveTCPAddr("tcp4", u.serverAddr)
+	if err != nil {
+		return err
+	}
+
+	// 2. Dial TCP control connection
+	tcpConn, err := net.DialTCP("tcp4", nil, tcpAddr)
+	if err != nil {
+		return err
+	}
+	defer tcpConn.Close()
+
+	// 3. Send CONNECT via TCP
+	if _, err := tcpConn.Write(BuildConnectPacket()); err != nil {
+		return err
+	}
+	log.Printf("[%s] tcp: sent CONNECT to %s", u.sourceID, u.serverAddr)
+
+	// 4. Read CONFIG header via TCP (io.ReadFull to handle partial reads)
+	hdrBuf := make([]byte, HeaderSize)
+	if _, err := io.ReadFull(tcpConn, hdrBuf); err != nil {
+		return err
+	}
+	cfgHdr, err := ParseHeader(hdrBuf)
+	if err != nil {
+		return err
+	}
+	if cfgHdr.Type != PktConfig {
+		return net.ErrClosed // unexpected packet type
+	}
+	cfgPayload := make([]byte, cfgHdr.PayloadBytes)
+	if cfgHdr.PayloadBytes > 0 {
+		if _, err := io.ReadFull(tcpConn, cfgPayload); err != nil {
+			return err
+		}
+	}
+	currentSigs, err := ParseConfig(cfgPayload)
+	if err != nil {
+		return err
+	}
+	log.Printf("[%s] tcp: received CONFIG (%d signals)", u.sourceID, len(currentSigs))
+	u.hub.SetSourceState(u.sourceID, "connected")
+	u.hub.UpdateConfigForSource(u.sourceID, currentSigs)
+
+	// 5. Determine multicast data port (dataPort or serverAddr port+1)
+	mcastPort := u.dataPort
+	if mcastPort == 0 {
+		mcastPort = tcpAddr.Port + 1
+	}
+
+	// 6. Join multicast group for DATA
+	mcastIP := net.ParseIP(u.multicastGroup)
+	if mcastIP == nil {
+		return &net.AddrError{Err: "invalid multicast group IP", Addr: u.multicastGroup}
+	}
+	mcastAddr := &net.UDPAddr{IP: mcastIP, Port: mcastPort}
+	mcastConn, err := net.ListenMulticastUDP("udp4", nil, mcastAddr)
+	if err != nil {
+		return err
+	}
+	defer mcastConn.Close()
+	if err := mcastConn.SetReadBuffer(udpRcvBufSize); err != nil {
+		log.Printf("[%s] multicast SetReadBuffer: %v", u.sourceID, err)
+	}
+	log.Printf("[%s] joined multicast %s:%s", u.sourceID, u.multicastGroup,
+		strconv.Itoa(mcastPort))
+
+	// 7. Background goroutine: watch TCP conn for DISCONNECT or closure
+	tcpDone := make(chan error, 1)
+	go func() {
+		buf := make([]byte, HeaderSize+64)
+		for {
+			n, readErr := tcpConn.Read(buf)
+			if readErr != nil {
+				tcpDone <- readErr
+				return
+			}
+			if n >= HeaderSize {
+				hdr, parseErr := ParseHeader(buf[:n])
+				if parseErr == nil && hdr.Type == PktDisconnect {
+					tcpDone <- nil
+					return
+				}
+			}
+		}
+	}()
+
+	// 8. Main loop: receive DATA packets from multicast group
+	reassembler := NewReassembler(2 * time.Second)
+	buf := make([]byte, readBufSize)
+
+	for {
+		mcastConn.SetReadDeadline(time.Now().Add(silenceTimeout))
+		n, _, readErr := mcastConn.ReadFromUDP(buf)
+		arrivalTime := time.Now()
+		if readErr != nil {
+			select {
+			case <-tcpDone:
+				return nil
+			default:
+			}
+			tcpConn.Write(BuildDisconnectPacket())
+			return readErr
+		}
+
+		if n < HeaderSize {
+			continue
+		}
+		hdr, parseErr := ParseHeader(buf[:n])
+		if parseErr != nil {
+			log.Printf("[%s] multicast: parse header: %v", u.sourceID, parseErr)
+			continue
+		}
+
+		payload := make([]byte, n-HeaderSize)
+		copy(payload, buf[HeaderSize:n])
+
+		complete, ok := reassembler.AddFragment(hdr, payload)
+		if hdr.Type == PktData {
+			u.hub.RecordDataFragment(u.sourceID, hdr.Counter, n,
+				arrivalTime.UnixNano(), ok)
+		}
+		if !ok {
+			continue
+		}
+
+		if hdr.Type == PktData {
+			if len(currentSigs) == 0 {
+				continue
+			}
+			sample, parseErr := ParseData(complete, currentSigs, arrivalTime)
+			if parseErr != nil {
+				log.Printf("[%s] multicast: parse data: %v", u.sourceID, parseErr)
+				continue
+			}
+			u.hub.PushDataForSource(u.sourceID, sample)
+		}
+
+		select {
+		case <-u.stopCh:
+			tcpConn.Write(BuildDisconnectPacket())
+			return nil
+		case tcpErr := <-tcpDone:
+			log.Printf("[%s] tcp control closed: %v", u.sourceID, tcpErr)
+			return nil
+		default:
+		}
+	}
+}
@@ -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 */

@@ -200,3 +200,28 @@ TEST(UDPStreamerGTest, TestHighFrequency_DataIntegrity) {
    UDPStreamerTest test;
    ASSERT_TRUE(test.TestHighFrequency_DataIntegrity());
 }
+
+TEST(UDPStreamerGTest, TestInitialise_MulticastMode_Valid) {
+    UDPStreamerTest test;
+    ASSERT_TRUE(test.TestInitialise_MulticastMode_Valid());
+}
+
+TEST(UDPStreamerGTest, TestInitialise_MulticastMode_DefaultDataPort) {
+    UDPStreamerTest test;
+    ASSERT_TRUE(test.TestInitialise_MulticastMode_DefaultDataPort());
+}
+
+TEST(UDPStreamerGTest, TestInitialise_MulticastMode_InvalidDataPort) {
+    UDPStreamerTest test;
+    ASSERT_TRUE(test.TestInitialise_MulticastMode_InvalidDataPort());
+}
+
+TEST(UDPStreamerGTest, TestPrepareNextState_Multicast) {
+    UDPStreamerTest test;
+    ASSERT_TRUE(test.TestPrepareNextState_Multicast());
+}
+
+TEST(UDPStreamerGTest, TestExecute_MulticastConnectDataDisconnect) {
+    UDPStreamerTest test;
+    ASSERT_TRUE(test.TestExecute_MulticastConnectDataDisconnect());
+}
@@ -31,6 +31,7 @@
 /*                         Project header includes                           */
 /*---------------------------------------------------------------------------*/
 #include "AdvancedErrorManagement.h"
+#include "BasicTCPSocket.h"
 #include "BasicUDPSocket.h"
 #include "ConfigurationDatabase.h"
 #include "GAM.h"
@@ -1547,3 +1548,288 @@ bool UDPStreamerTest::TestHighFrequency_DataIntegrity() {
    ObjectRegistryDatabase::Instance()->Purge();
    return ok;
 }
+
+/* ============================================================
+ * Multicast mode tests (ports 44710-44729)
+ * ============================================================ */
+
+bool UDPStreamerTest::TestInitialise_MulticastMode_Valid() {
+    using namespace MARTe;
+    UDPStreamer ds;
+    ConfigurationDatabase cdb;
+    cdb.Write("Port", 44710u);
+    cdb.Write("MulticastGroup", "239.0.0.1");
+    cdb.Write("DataPort", 44711u);
+    cdb.CreateRelative("Signals");
+    cdb.MoveToRoot();
+    bool ok = ds.Initialise(cdb);
+    ok &= ds.IsMulticast();
+    ok &= (ds.GetPort() == 44710u);
+    return ok;
+}
+
+bool UDPStreamerTest::TestInitialise_MulticastMode_DefaultDataPort() {
+    using namespace MARTe;
+    UDPStreamer ds;
+    ConfigurationDatabase cdb;
+    cdb.Write("Port", 44712u);
+    cdb.Write("MulticastGroup", "239.0.0.1");
+    /* DataPort intentionally omitted: should default to 44713 */
+    cdb.CreateRelative("Signals");
+    cdb.MoveToRoot();
+    bool ok = ds.Initialise(cdb);
+    ok &= ds.IsMulticast();
+    return ok;
+}
+
+bool UDPStreamerTest::TestInitialise_MulticastMode_InvalidDataPort() {
+    using namespace MARTe;
+    UDPStreamer ds;
+    ConfigurationDatabase cdb;
+    cdb.Write("Port", 44714u);
+    cdb.Write("MulticastGroup", "239.0.0.1");
+    cdb.Write("DataPort", 44714u); /* same as Port — must be rejected */
+    cdb.CreateRelative("Signals");
+    cdb.MoveToRoot();
+    return !ds.Initialise(cdb);
+}
+
+bool UDPStreamerTest::TestPrepareNextState_Multicast() {
+    using namespace MARTe;
+
+    static const char8 *const cfg =
+        "+Test = {\n"
+        "    Class = RealTimeApplication\n"
+        "    +Functions = {\n"
+        "        Class = ReferenceContainer\n"
+        "        +Writer = {\n"
+        "            Class = UDPStreamerTestOutputGAM\n"
+        "            OutputSignals = {\n"
+        "                Counter = {\n"
+        "                    DataSource = Streamer\n"
+        "                    Type = uint32\n"
+        "                }\n"
+        "            }\n"
+        "        }\n"
+        "    }\n"
+        "    +Data = {\n"
+        "        Class = ReferenceContainer\n"
+        "        +Streamer = {\n"
+        "            Class = UDPStreamer\n"
+        "            Port = 44716\n"
+        "            MulticastGroup = \"239.0.0.1\"\n"
+        "            DataPort = 44717\n"
+        "            MaxPayloadSize = 1400\n"
+        "            Signals = {\n"
+        "                Counter = {\n"
+        "                    Type = uint32\n"
+        "                }\n"
+        "            }\n"
+        "        }\n"
+        "        +Timings = {\n"
+        "            Class = TimingDataSource\n"
+        "        }\n"
+        "    }\n"
+        "    +States = {\n"
+        "        Class = ReferenceContainer\n"
+        "        +State1 = {\n"
+        "            Class = RealTimeState\n"
+        "            +Threads = {\n"
+        "                Class = ReferenceContainer\n"
+        "                +Thread1 = {\n"
+        "                    Class = RealTimeThread\n"
+        "                    Functions = { Writer }\n"
+        "                }\n"
+        "            }\n"
+        "        }\n"
+        "    }\n"
+        "    +Scheduler = {\n"
+        "        Class = GAMScheduler\n"
+        "        TimingDataSource = Timings\n"
+        "    }\n"
+        "}\n";
+
+    ReferenceT<RealTimeApplication> app = LoadApplication(cfg);
+    bool ok = app.IsValid();
+    if (ok) {
+        ok = (app->PrepareNextState("State1") == ErrorManagement::NoError);
+    }
+    Sleep::MSec(50u);
+
+    ReferenceT<UDPStreamer> ds = ObjectRegistryDatabase::Instance()->Find("Test.Data.Streamer");
+    if (ok) {
+        ok = ds.IsValid();
+    }
+    if (ok) {
+        ok = ds->IsMulticast();
+    }
+    if (ok) {
+        ok = !ds->IsClientConnected(); /* no client connected yet */
+    }
+
+    ObjectRegistryDatabase::Instance()->Purge();
+    return ok;
+}
+
+bool UDPStreamerTest::TestExecute_MulticastConnectDataDisconnect() {
+    using namespace MARTe;
+
+    static const char8 *const cfg =
+        "+Test = {\n"
+        "    Class = RealTimeApplication\n"
+        "    +Functions = {\n"
+        "        Class = ReferenceContainer\n"
+        "        +Writer = {\n"
+        "            Class = UDPStreamerTestOutputGAM\n"
+        "            OutputSignals = {\n"
+        "                Counter = {\n"
+        "                    DataSource = Streamer\n"
+        "                    Type = uint32\n"
+        "                }\n"
+        "            }\n"
+        "        }\n"
+        "    }\n"
+        "    +Data = {\n"
+        "        Class = ReferenceContainer\n"
+        "        +Streamer = {\n"
+        "            Class = UDPStreamer\n"
+        "            Port = 44720\n"
+        "            MulticastGroup = \"239.0.0.1\"\n"
+        "            DataPort = 44721\n"
+        "            MaxPayloadSize = 1400\n"
+        "            Signals = {\n"
+        "                Counter = {\n"
+        "                    Type = uint32\n"
+        "                }\n"
+        "            }\n"
+        "        }\n"
+        "        +Timings = {\n"
+        "            Class = TimingDataSource\n"
+        "        }\n"
+        "    }\n"
+        "    +States = {\n"
+        "        Class = ReferenceContainer\n"
+        "        +State1 = {\n"
+        "            Class = RealTimeState\n"
+        "            +Threads = {\n"
+        "                Class = ReferenceContainer\n"
+        "                +Thread1 = {\n"
+        "                    Class = RealTimeThread\n"
+        "                    Functions = { Writer }\n"
+        "                }\n"
+        "            }\n"
+        "        }\n"
+        "    }\n"
+        "    +Scheduler = {\n"
+        "        Class = GAMScheduler\n"
+        "        TimingDataSource = Timings\n"
+        "    }\n"
+        "}\n";
+
+    ReferenceT<RealTimeApplication> app = LoadApplication(cfg);
+    bool ok = app.IsValid();
+    if (ok) {
+        ok = (app->PrepareNextState("State1") == ErrorManagement::NoError);
+    }
+    Sleep::MSec(100u);
+
+    /* Step 1: TCP CONNECT to control port 44720 */
+    BasicTCPSocket clientTCP;
+    if (ok) {
+        ok = clientTCP.Open();
+    }
+    if (ok) {
+        ok = clientTCP.Connect("127.0.0.1", 44720u, TimeoutType(2000u));
+    }
+
+    /* Send CONNECT packet over TCP */
+    if (ok) {
+        UDPSPacketHeader connectHdr;
+        connectHdr.magic          = UDPS_MAGIC;
+        connectHdr.type           = UDPS_TYPE_CONNECT;
+        connectHdr.counter        = 0u;
+        connectHdr.fragmentIdx    = 0u;
+        connectHdr.totalFragments = 1u;
+        connectHdr.payloadBytes   = 0u;
+        uint32 sendSz = static_cast<uint32>(sizeof(UDPSPacketHeader));
+        ok = clientTCP.Write(reinterpret_cast<const char8 *>(&connectHdr), sendSz);
+    }
+
+    /* Step 2: Read CONFIG from TCP (header then payload) */
+    if (ok) {
+        uint8 cfgHdrBuf[sizeof(UDPSPacketHeader)];
+        uint32 recvSz = static_cast<uint32>(sizeof(UDPSPacketHeader));
+        ok = clientTCP.Read(reinterpret_cast<char8 *>(cfgHdrBuf), recvSz, TimeoutType(2000u));
+        if (ok) {
+            const UDPSPacketHeader *cfgHdr =
+                reinterpret_cast<const UDPSPacketHeader *>(cfgHdrBuf);
+            ok = (cfgHdr->magic == UDPS_MAGIC) && (cfgHdr->type == UDPS_TYPE_CONFIG);
+            if (ok && (cfgHdr->payloadBytes > 0u)) {
+                /* Drain the payload so the TCP stream is clean */
+                HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
+                uint8 *payload = reinterpret_cast<uint8 *>(
+                    heap->Malloc(cfgHdr->payloadBytes));
+                uint32 plSz = cfgHdr->payloadBytes;
+                (void) clientTCP.Read(reinterpret_cast<char8 *>(payload), plSz,
+                                      TimeoutType(2000u));
+                heap->Free(reinterpret_cast<void *&>(payload));
+            }
+        }
+    }
+
+    /* Step 3: Join multicast group 239.0.0.1 on data port 44721 */
+    BasicUDPSocket mcastReader;
+    if (ok) {
+        ok = mcastReader.Open();
+    }
+    if (ok) {
+        ok = mcastReader.Listen(44721u);
+    }
+    if (ok) {
+        ok = mcastReader.Join("239.0.0.1");
+    }
+
+    /* Step 4: Trigger Synchronise() to generate a DATA packet */
+    if (ok) {
+        ReferenceT<UDPStreamer> ds =
+            ObjectRegistryDatabase::Instance()->Find("Test.Data.Streamer");
+        ok = ds.IsValid() && ds->IsClientConnected();
+        if (ok) {
+            ok = ds->Synchronise();
+        }
+    }
+    Sleep::MSec(100u);
+
+    /* Step 5: Receive at least one DATA fragment from the multicast group */
+    if (ok) {
+        uint8 dataBuf[2048u];
+        uint32 dataSz = static_cast<uint32>(sizeof(dataBuf));
+        bool received = mcastReader.Read(reinterpret_cast<char8 *>(dataBuf), dataSz,
+                                         TimeoutType(500u));
+        ok = received;
+        if (ok && (dataSz >= static_cast<uint32>(sizeof(UDPSPacketHeader)))) {
+            const UDPSPacketHeader *dHdr =
+                reinterpret_cast<const UDPSPacketHeader *>(dataBuf);
+            ok = (dHdr->magic == UDPS_MAGIC) && (dHdr->type == UDPS_TYPE_DATA);
+        }
+    }
+
+    /* Step 6: Send DISCONNECT via TCP */
+    if (clientTCP.IsValid()) {
+        UDPSPacketHeader disPkt;
+        disPkt.magic          = UDPS_MAGIC;
+        disPkt.type           = UDPS_TYPE_DISCONNECT;
+        disPkt.counter        = 0u;
+        disPkt.fragmentIdx    = 0u;
+        disPkt.totalFragments = 1u;
+        disPkt.payloadBytes   = 0u;
+        uint32 disSz = static_cast<uint32>(sizeof(UDPSPacketHeader));
+        (void) clientTCP.Write(reinterpret_cast<const char8 *>(&disPkt), disSz);
+        (void) clientTCP.Close();
+    }
+    (void) mcastReader.Close();
+    Sleep::MSec(50u);
+
+    ObjectRegistryDatabase::Instance()->Purge();
+    return ok;
+}
@@ -209,6 +209,31 @@ public:
     * Reassembles all fragments and verifies the total payload size.
     */
    bool TestHighFrequency_DataIntegrity();
+
+    /**
+     * @brief Tests Initialise with MulticastGroup and explicit DataPort.
+     */
+    bool TestInitialise_MulticastMode_Valid();
+
+    /**
+     * @brief Tests that DataPort defaults to Port+1 when MulticastGroup is set but DataPort is absent.
+     */
+    bool TestInitialise_MulticastMode_DefaultDataPort();
+
+    /**
+     * @brief Tests that DataPort equal to Port is rejected.
+     */
+    bool TestInitialise_MulticastMode_InvalidDataPort();
+
+    /**
+     * @brief Tests PrepareNextState in multicast mode: TCP listener opens, data socket connects.
+     */
+    bool TestPrepareNextState_Multicast();
+
+    /**
+     * @brief Tests full TCP CONNECT → CONFIG → DATA via multicast → DISCONNECT on loopback.
+     */
+    bool TestExecute_MulticastConnectDataDisconnect();
 };

 #endif /* UDPSTREAMERTEST_H_ */