From 7b8c932ab73402366fae3f55629277392c8ef7a6 Mon Sep 17 00:00:00 2001 From: Martino Ferrari Date: Fri, 6 Nov 2026 13:44:26 +0100 Subject: [PATCH] udpsclient: a TCP transport mode (SUBSCRIBE, no CONNECT) and a TCP mock server Co-Authored-By: Claude Opus 4.6 --- deps/udpsclient/udps_client.c | 62 +++++++++++++++++- deps/udpsclient/udps_client.h | 17 ++++- tools/mock_tcpstreamer.py | 119 ++++++++++++++++++++++++++++++++++ 3 files changed, 194 insertions(+), 4 deletions(-) create mode 100755 tools/mock_tcpstreamer.py diff --git a/deps/udpsclient/udps_client.c b/deps/udpsclient/udps_client.c index e6825a2..8372f2a 100644 --- a/deps/udpsclient/udps_client.c +++ b/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,9 +1150,50 @@ 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) - : connect_unicast(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(); return rc; @@ -1174,7 +1216,10 @@ static void do_disconnect(udps_client_t *c, const char *why) { if (!c->connected) { return; } - (void)send_control(c, UDPS_PKT_DISCONNECT); + 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; diff --git a/deps/udpsclient/udps_client.h b/deps/udpsclient/udps_client.h index 49f56ee..c8b6b69 100644 --- a/deps/udpsclient/udps_client.h +++ b/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. */ diff --git a/tools/mock_tcpstreamer.py b/tools/mock_tcpstreamer.py new file mode 100755 index 0000000..c5268dc --- /dev/null +++ b/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(" 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()