From b15e637f140893251de16677d4139d1566919fed Mon Sep 17 00:00:00 2001 From: Martino Ferrari Date: Tue, 26 May 2026 22:56:27 +0200 Subject: [PATCH] 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 --- Client/WebUI/hub.go | 2 +- Client/WebUI/main.go | 2 +- Client/WebUI/sources.go | 37 +- Client/WebUI/udpclient.go | 196 ++++++++++- .../DataSources/UDPStreamer/UDPStreamer.cpp | 321 ++++++++++++++++-- .../DataSources/UDPStreamer/UDPStreamer.h | 28 +- .../UDPStreamer/UDPStreamerGTest.cpp | 25 ++ .../UDPStreamer/UDPStreamerTest.cpp | 286 ++++++++++++++++ .../DataSources/UDPStreamer/UDPStreamerTest.h | 25 ++ 9 files changed, 864 insertions(+), 58 deletions(-) diff --git a/Client/WebUI/hub.go b/Client/WebUI/hub.go index 68c3ea8..67fa7d6 100644 --- a/Client/WebUI/hub.go +++ b/Client/WebUI/hub.go @@ -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": diff --git a/Client/WebUI/main.go b/Client/WebUI/main.go index d5dd470..f3aa5fa 100644 --- a/Client/WebUI/main.go +++ b/Client/WebUI/main.go @@ -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") diff --git a/Client/WebUI/sources.go b/Client/WebUI/sources.go index cf53f78..121b1e3 100644 --- a/Client/WebUI/sources.go +++ b/Client/WebUI/sources.go @@ -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 } diff --git a/Client/WebUI/udpclient.go b/Client/WebUI/udpclient.go index 4ed07ee..929706c 100644 --- a/Client/WebUI/udpclient.go +++ b/Client/WebUI/udpclient.go @@ -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: + } + } +} diff --git a/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp b/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp index 2999760..d2102cb 100644 --- a/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp +++ b/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp @@ -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(dp)); + } + if ((dp == 0u) || (dp == port)) { + REPORT_ERROR(ErrorManagement::ParametersError, + "DataPort %u must be non-zero and differ from Port %u.", + static_cast(dp), static_cast(port)); + ok = false; + } + else { + dataPort = dp; + REPORT_ERROR(ErrorManagement::Information, + "Multicast mode: group=%s, controlPort=%u, dataPort=%u.", + multicastGroup.Buffer(), + static_cast(port), + static_cast(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(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(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(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(sockFd), &rfds); - struct timeval tv = { 0, 0 }; /* non-blocking poll */ - int nReady = select(static_cast(sockFd) + 1, &rfds, NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv); - if (nReady > 0) { - uint32 recvSize = static_cast(sizeof(cmdBuf)); - bool received = serverSocket.Read(reinterpret_cast(cmdBuf), recvSize); - if (received && (recvSize >= static_cast(sizeof(UDPSPacketHeader)))) { - HandleClientCommand(cmdBuf, recvSize); + /* --- Poll for incoming control commands --- */ + uint32 hdrSize = static_cast(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(listenFd), &rfdL); + struct timeval tvL = { 0, 0 }; + if (select(static_cast(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(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(cfd), &rfdC); + struct timeval tvC = { 0, 0 }; + if (select(static_cast(cfd) + 1, &rfdC, + NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvC) > 0) { + uint8 cmdBuf[256u]; + uint32 cmdSize = static_cast(sizeof(cmdBuf)); + bool readOk = tcpClient->Read( + reinterpret_cast(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(sockFd), &rfds); + struct timeval tv = { 0, 0 }; + int nReady = select(static_cast(sockFd) + 1, &rfds, + NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv); + if (nReady > 0) { + uint32 recvSize = static_cast(sizeof(cmdBuf)); + bool received = serverSocket.Read( + reinterpret_cast(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(sendBuf), sendSize); + if (useMulticast) { + ok = dataSocket.Write(reinterpret_cast(sendBuf), sendSize); + } + else { + ok = clientSocket.Write(reinterpret_cast(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(sizeof(UDPSPacketHeader))) { + return; + } + const UDPSPacketHeader *hdr = reinterpret_cast(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(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(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(sizeof(UDPSPacketHeader)); + uint32 totalSize = headerSize + cfgPayloadSize; + uint8 *sendBuf = reinterpret_cast(heap->Malloc(totalSize)); + if (sendBuf != NULL_PTR(uint8 *)) { + UDPSPacketHeader *outHdr = reinterpret_cast(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(sendBuf), totalSize); + if (!writeOk) { + REPORT_ERROR(ErrorManagement::Warning, + "TCP: failed to send CONFIG packet."); + clientConnected = false; + } + heap->Free(reinterpret_cast(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(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 */ diff --git a/Source/Components/DataSources/UDPStreamer/UDPStreamer.h b/Source/Components/DataSources/UDPStreamer/UDPStreamer.h index 79b4976..c8fc21b 100644 --- a/Source/Components/DataSources/UDPStreamer/UDPStreamer.h +++ b/Source/Components/DataSources/UDPStreamer/UDPStreamer.h @@ -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 */ diff --git a/Test/Components/DataSources/UDPStreamer/UDPStreamerGTest.cpp b/Test/Components/DataSources/UDPStreamer/UDPStreamerGTest.cpp index 514e90e..38ea2ee 100644 --- a/Test/Components/DataSources/UDPStreamer/UDPStreamerGTest.cpp +++ b/Test/Components/DataSources/UDPStreamer/UDPStreamerGTest.cpp @@ -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()); +} diff --git a/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.cpp b/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.cpp index 7ce3836..ea58a64 100644 --- a/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.cpp +++ b/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.cpp @@ -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 app = LoadApplication(cfg); + bool ok = app.IsValid(); + if (ok) { + ok = (app->PrepareNextState("State1") == ErrorManagement::NoError); + } + Sleep::MSec(50u); + + ReferenceT 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 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(sizeof(UDPSPacketHeader)); + ok = clientTCP.Write(reinterpret_cast(&connectHdr), sendSz); + } + + /* Step 2: Read CONFIG from TCP (header then payload) */ + if (ok) { + uint8 cfgHdrBuf[sizeof(UDPSPacketHeader)]; + uint32 recvSz = static_cast(sizeof(UDPSPacketHeader)); + ok = clientTCP.Read(reinterpret_cast(cfgHdrBuf), recvSz, TimeoutType(2000u)); + if (ok) { + const UDPSPacketHeader *cfgHdr = + reinterpret_cast(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( + heap->Malloc(cfgHdr->payloadBytes)); + uint32 plSz = cfgHdr->payloadBytes; + (void) clientTCP.Read(reinterpret_cast(payload), plSz, + TimeoutType(2000u)); + heap->Free(reinterpret_cast(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 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(sizeof(dataBuf)); + bool received = mcastReader.Read(reinterpret_cast(dataBuf), dataSz, + TimeoutType(500u)); + ok = received; + if (ok && (dataSz >= static_cast(sizeof(UDPSPacketHeader)))) { + const UDPSPacketHeader *dHdr = + reinterpret_cast(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(sizeof(UDPSPacketHeader)); + (void) clientTCP.Write(reinterpret_cast(&disPkt), disSz); + (void) clientTCP.Close(); + } + (void) mcastReader.Close(); + Sleep::MSec(50u); + + ObjectRegistryDatabase::Instance()->Purge(); + return ok; +} diff --git a/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.h b/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.h index 0121186..6d64f97 100644 --- a/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.h +++ b/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.h @@ -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_ */