martino/MARTe_IO_Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add TCP control + UDP multicast mode to UDPStreamer

Browse Source 
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>
This commit is contained in:
Martino Ferrari
2026-05-26 22:56:27 +02:00
parent f85ab8652c
commit b15e637f14
9 changed files with 864 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Client/WebUI/hub.go
+1 -1
View File
@@ -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":
Client/WebUI/main.go
+1 -1
View File
@@ -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")
Client/WebUI/sources.go
+20 -5
View File
@@ -13,6 +13,8 @@ import (
type SourceConfig struct {
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.
@@ -20,6 +22,8 @@ type managedSource struct {
id string
label string
addr string
multicastGroup string
dataPort int
client *UDPClient
}
@@ -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
}
Client/WebUI/udpclient.go
+177 -5
View File
@@ -1,8 +1,10 @@
package main
import (
"io"
"log"
"net"
"strconv"
"time"
)
@@ -13,21 +15,30 @@ 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
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,
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:
}
}
}
Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp
+271 -8
View File
@@ -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,7 +651,45 @@ bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
const char8 *const nextStateName) {
bool ok = true;
/* Open server socket (idempotent: skip if already valid) */
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 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) {
@@ -610,10 +704,10 @@ bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
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,21 +767,93 @@ 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) --- */
/* --- 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 }; /* non-blocking poll */
int nReady = select(static_cast<int>(sockFd) + 1, &rfds, NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
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 >= static_cast<uint32>(sizeof(UDPSPacketHeader)))) {
bool received = serverSocket.Read(
reinterpret_cast<char8 *>(cmdBuf), recvSize);
if (received && (recvSize >= hdrSize)) {
HandleClientCommand(cmdBuf, recvSize);
}
}
}
if (dataReady && clientConnected) {
/* In Auto mode, only flush when the HRT flush interval has elapsed.
@@ -734,7 +900,16 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
if (info.GetStage() == ExecutionInfo::TerminationStage) {
if (clientConnected) {
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 (!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;
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 */
Source/Components/DataSources/UDPStreamer/UDPStreamer.h
+24 -4
View File
@@ -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 */
Test/Components/DataSources/UDPStreamer/UDPStreamerGTest.cpp
+25
View File
@@ -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());
}
Test/Components/DataSources/UDPStreamer/UDPStreamerTest.cpp
+286
View File
@@ -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;
}
Test/Components/DataSources/UDPStreamer/UDPStreamerTest.h
+25
View File
@@ -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_ */