udpsclient: a TCP transport mode (SUBSCRIBE, no CONNECT) and a TCP mock server

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

deps/udpsclient/udps_client.c

@@ -174,6 +174,7 @@ struct udps_client {
     char server_addr[128];
     char mcast_group[128];
     char iface_addr[128];
+    char stream_name[256];
 
     struct sockaddr_in server_sa;
     int    udp_fd;   /**< Unicast receive socket, or joined multicast socket. */
@@ -1149,8 +1150,49 @@ static int connect_multicast(udps_client_t *c) {
     return 0;
 }
 
+/** TCP transport: connect, then SUBSCRIBE when a stream name is set. */
+static int connect_tcp(udps_client_t *c) {
+    struct timeval tv;
+    size_t         name_len = strlen(c->stream_name);
+
+    c->tcp_fd = socket(AF_INET, SOCK_STREAM, 0);
+    if (c->tcp_fd < 0) {
+        return fail(c, "TCP socket() failed: %s", strerror(errno));
+    }
+    set_recv_buffer(c->tcp_fd, c->cfg.recv_buffer_bytes);
+    /* Linux bounds a blocking connect() with SO_SNDTIMEO, so an unreachable host
+     * cannot hold the polling thread for minutes. */
+    tv.tv_sec  = 3;
+    tv.tv_usec = 0;
+    (void)setsockopt(c->tcp_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, (socklen_t)sizeof tv);
+    if (connect(c->tcp_fd, (struct sockaddr *)&c->server_sa,
+                (socklen_t)sizeof c->server_sa) != 0) {
+        int e = errno;
+        close_sockets(c);
+        return fail(c, "TCP connect to %s:%u failed: %s", c->server_addr,
+                    (unsigned)c->cfg.server_port, strerror(e));
+    }
+    /* A half-received frame must not wedge the poll loop (read_exact_tcp blocks). */
+    tv.tv_sec = 1;
+    (void)setsockopt(c->tcp_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, (socklen_t)sizeof tv);
+
+    if (name_len > 0u) {
+        uint8_t pkt[UDPS_HEADER_SIZE + 255u];
+        build_header(pkt, UDPS_PKT_SUBSCRIBE, 0u, 0u, 1u, (uint32_t)name_len);
+        memcpy(pkt + UDPS_HEADER_SIZE, c->stream_name, name_len);
+        if (send(c->tcp_fd, pkt, UDPS_HEADER_SIZE + name_len, MSG_NOSIGNAL) !=
+            (ssize_t)(UDPS_HEADER_SIZE + name_len)) {
+            int e = errno;
+            close_sockets(c);
+            return fail(c, "SUBSCRIBE failed: %s", strerror(e));
+        }
+    }
+    return 0;
+}
+
 static int do_connect(udps_client_t *c) {
-    int rc = (c->mcast_group[0] != '\0') ? connect_multicast(c)
+    int rc = c->cfg.use_tcp ? connect_tcp(c)
+             : (c->mcast_group[0] != '\0') ? connect_multicast(c)
                                            : connect_unicast(c);
     if (rc != 0) {
         c->disconnect_t = now_mono();
@@ -1174,7 +1216,10 @@ static void do_disconnect(udps_client_t *c, const char *why) {
     if (!c->connected) {
         return;
     }
+    if (!c->cfg.use_tcp) {
+        /* TCP: a write to a peer that closed would raise SIGPIPE; the server sees the close itself. */
         (void)send_control(c, UDPS_PKT_DISCONNECT);
+    }
     close_sockets(c);
     c->connected    = 0;
     c->disconnect_t = now_mono();
@@ -1267,17 +1312,28 @@ udps_client_t *udps_client_create(const udps_client_config_t *cfg) {
     if (c == NULL) {
         return NULL;
     }
+    if (cfg->use_tcp && cfg->stream_name != NULL && strlen(cfg->stream_name) > 255u) {
+        free(c);
+        return NULL;
+    }
     c->cfg    = *cfg;
     c->udp_fd = -1;
     c->tcp_fd = -1;
     copy_str(c->server_addr, sizeof c->server_addr, cfg->server_addr);
     copy_str(c->mcast_group, sizeof c->mcast_group, cfg->multicast_group);
     copy_str(c->iface_addr, sizeof c->iface_addr, cfg->interface_addr);
+    copy_str(c->stream_name, sizeof c->stream_name, cfg->stream_name);
     /* The config's string pointers must not be used again: they belong to the
      * caller and the copies above are what the client works from. */
     c->cfg.server_addr     = c->server_addr;
     c->cfg.multicast_group = (c->mcast_group[0] != '\0') ? c->mcast_group : NULL;
     c->cfg.interface_addr  = (c->iface_addr[0] != '\0') ? c->iface_addr : NULL;
+    c->cfg.stream_name     = c->stream_name;
+    if (c->cfg.use_tcp) {
+        c->cfg.multicast_group  = NULL;
+        c->mcast_group[0]       = '\0';
+        c->cfg.reorder_window_s = 0.0;
+    }
 
     if (c->cfg.max_packet_bytes == 0u) {
         c->cfg.max_packet_bytes = 1u * 1024u * 1024u;

deps/udpsclient/udps_client.h

@@ -52,7 +52,8 @@ enum {
     UDPS_PKT_CONFIG     = 1, /**< Server -> client: signal metadata. */
     UDPS_PKT_ACK        = 2, /**< Client -> server: keepalive. */
     UDPS_PKT_CONNECT    = 3, /**< Client -> server: open a session. */
-    UDPS_PKT_DISCONNECT = 4  /**< Either direction: close a session. */
+    UDPS_PKT_DISCONNECT = 4, /**< Either direction: close a session. */
+    UDPS_PKT_SUBSCRIBE  = 5  /**< Client -> server (TCP stream service): payload = stream name. */
 };
 
 /** Sample type codes (udps_signal_t::type_code). */
@@ -279,6 +280,20 @@ typedef struct {
      * the drop-on-late behaviour. Default 0.020.
      */
     double      reorder_window_s;
+    /**
+     * Non-zero selects the TCP transport (a MARTe2 TCPStreamer / TCPStreamService):
+     * @c server_port is the TCP port, no CONNECT or DISCONNECT frame is sent, the
+     * server streams CONFIG then DATA on connect, and the reorder window is forced
+     * to 0 (TCP is ordered, so a counter gap is a server-side drop). @c multicast_group,
+     * @c interface_addr and @c data_port are ignored.
+     */
+    int         use_tcp;
+    /**
+     * TCP mode only: when non-empty (1..255 bytes) a SUBSCRIBE frame naming this stream is
+     * sent right after every connect, as a TCPStreamService requires. A plain
+     * TCPStreamer ignores it. NULL or "" sends nothing.
+     */
+    const char *stream_name;
 } udps_client_config_t;
 
 /** Fills @p cfg with the defaults documented in Docs/UDPS-C-Client.md. */

tools/mock_tcpstreamer.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+# SPDX-License-Identifier: MIT
+# SPDX-FileCopyrightText: 2026 Martino Ferrari
+"""A minimal MARTe2 TCPStreamer / TCPStreamService producer, for exercising udpscope.
+
+Listens on `--port`, serves one client at a time with the same frames as
+mock_udpstreamer.py (CONFIG, then Accumulate DATA packets of sine/square/ramp/noise), each
+framed UDPS-over-TCP. Without `--stream` it behaves like a standalone TCPStreamer: the
+stream starts at once and anything the client sends is ignored. With `--stream NAME` it
+behaves like a TCPStreamService: the client must open with a SUBSCRIBE frame naming NAME
+within 5 s, otherwise the connection is closed.
+
+    ./tools/mock_tcpstreamer.py --port 44500 --stream fast --close-after 6
+"""
+
+import argparse
+import os
+import select
+import socket
+import struct
+import sys
+import time
+
+sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
+import mock_udpstreamer as m  # noqa: E402
+
+PKT_SUBSCRIBE = 5
+HEADER = 17
+
+
+def read_exact(conn, n, deadline):
+    buf = b""
+    while len(buf) < n:
+        left = deadline - time.monotonic()
+        if left <= 0:
+            return None
+        r, _, _ = select.select([conn], [], [], left)
+        if not r:
+            return None
+        chunk = conn.recv(n - len(buf))
+        if not chunk:
+            return None
+        buf += chunk
+    return buf
+
+
+def subscribed_to(conn, want):
+    """True once a SUBSCRIBE frame naming `want` arrived within 5 s."""
+    deadline = time.monotonic() + 5.0
+    head = read_exact(conn, HEADER, deadline)
+    if head is None:
+        return False
+    magic, ptype, _, _, _, plen = struct.unpack("<IBIHHI", head)
+    if magic != m.MAGIC or ptype != PKT_SUBSCRIBE or not 0 < plen <= 4096:
+        return False
+    name = read_exact(conn, plen, deadline)
+    return name is not None and name.decode(errors="replace") == want
+
+
+def serve(conn, args):
+    dt = 1.0 / args.rate
+    period = args.batch / args.rate
+    conn.sendall(m.build_config(args.rate, m.TIME_FULL_ARRAY, "V"))
+    counter, t_stream = 1, 0.0
+    started = time.monotonic()
+    next_send = started
+    while True:
+        now = time.monotonic()
+        if args.close_after > 0 and now - started >= args.close_after:
+            print("mock tcpstreamer: closing the connection (--close-after)", flush=True)
+            return
+        if now < next_send:
+            time.sleep(min(next_send - now, 0.002))
+            continue
+        pkt = m.build_data(counter, t_stream, dt, args.batch, args.freq)
+        if not (args.drop_every > 0 and counter % args.drop_every == 0):
+            conn.sendall(pkt)
+        counter += 1
+        t_stream += args.batch * dt
+        next_send += period
+
+
+def main():
+    ap = argparse.ArgumentParser()
+    ap.add_argument("--port", type=int, default=44500)
+    ap.add_argument("--stream", default="", help="TCPStreamService mode: require SUBSCRIBE of this name")
+    ap.add_argument("--rate", type=float, default=10000.0, help="samples per second")
+    ap.add_argument("--batch", type=int, default=100, help="samples per packet")
+    ap.add_argument("--freq", type=float, default=5.0, help="waveform frequency, Hz")
+    ap.add_argument("--close-after", type=float, default=0.0, metavar="S",
+                    help="close each connection after S seconds (the client must reconnect); 0 = never")
+    ap.add_argument("--drop-every", type=int, default=0, metavar="N",
+                    help="skip sending every Nth packet but advance the counter (a server-side drop); 0 = never")
+    args = ap.parse_args()
+
+    srv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    srv.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+    srv.bind(("0.0.0.0", args.port))
+    srv.listen(4)
+    print("mock tcpstreamer: listening on :%d (%s)" % (
+        args.port, "service stream '%s'" % args.stream if args.stream else "standalone"), flush=True)
+    while True:
+        conn, addr = srv.accept()
+        print("mock tcpstreamer: client %s:%d" % addr, flush=True)
+        try:
+            if args.stream and not subscribed_to(conn, args.stream):
+                print("mock tcpstreamer: no valid SUBSCRIBE for '%s'; closing" % args.stream, flush=True)
+                continue
+            if args.stream:
+                print("mock tcpstreamer: SUBSCRIBE '%s'" % args.stream, flush=True)
+            serve(conn, args)
+        except (BrokenPipeError, ConnectionResetError):
+            print("mock tcpstreamer: client went away", flush=True)
+        finally:
+            conn.close()
+
+
+if __name__ == "__main__":
+    main()