/** * @file udps_client.c * @brief Implementation of the standalone UDPS receiver library. * * Layering, bottom up: little-endian readers, fragment reassembly, CONFIG/DATA * decoding, socket transport, and finally udps_client_poll() which stitches * them together into the connect/receive/reconnect state machine. */ #define _POSIX_C_SOURCE 200809L /* struct ip_mreq is not in strict POSIX; _BSD_SOURCE is the pre-2.19 glibc name. */ #define _DEFAULT_SOURCE 1 #define _BSD_SOURCE 1 #include "udps_client.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include /*---------------------------------------------------------------------------*/ /* Tunable limits */ /*---------------------------------------------------------------------------*/ /** Concurrent in-flight reassemblies. The server interleaves at most a couple. */ #define UDPS_MAX_SLOTS 4 /** Fragments per update; also bounds the receive bitmask below. */ #define UDPS_MAX_FRAGMENTS 512 #define UDPS_FRAG_MASK_BYTES (UDPS_MAX_FRAGMENTS / 8) /** Largest datagram we can receive, header included. */ #define UDPS_RX_BUF_BYTES (65535 + 17) /** An incomplete reassembly older than this is abandoned. */ #define UDPS_SLOT_STALE_S 2.0 /** Datagrams drained per poll before housekeeping runs again. */ #define UDPS_DRAIN_LIMIT 256 /** Refuse CONFIGs claiming more signals than this. */ #define UDPS_MAX_SIGNALS 4096 /*---------------------------------------------------------------------------*/ /* Little-endian primitives */ /*---------------------------------------------------------------------------*/ static uint16_t rd_u16(const uint8_t *p) { return (uint16_t)((uint16_t)p[0] | ((uint16_t)p[1] << 8)); } static uint32_t rd_u32(const uint8_t *p) { return (uint32_t)p[0] | ((uint32_t)p[1] << 8) | ((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24); } static uint64_t rd_u64(const uint8_t *p) { return (uint64_t)rd_u32(p) | ((uint64_t)rd_u32(p + 4) << 32); } static float rd_f32(const uint8_t *p) { uint32_t bits = rd_u32(p); float f; memcpy(&f, &bits, sizeof f); return f; } static double rd_f64(const uint8_t *p) { uint64_t bits = rd_u64(p); double d; memcpy(&d, &bits, sizeof d); return d; } static void wr_u16(uint8_t *p, uint16_t v) { p[0] = (uint8_t)(v & 0xFFu); p[1] = (uint8_t)(v >> 8); } static void wr_u32(uint8_t *p, uint32_t v) { p[0] = (uint8_t)(v & 0xFFu); p[1] = (uint8_t)((v >> 8) & 0xFFu); p[2] = (uint8_t)((v >> 16) & 0xFFu); p[3] = (uint8_t)((v >> 24) & 0xFFu); } /** Serialises a header into @p buf, which must hold UDPS_HEADER_SIZE bytes. */ static void build_header(uint8_t *buf, uint8_t type, uint32_t counter, uint16_t frag_idx, uint16_t total_frags, uint32_t payload_bytes) { wr_u32(buf, UDPS_MAGIC); buf[4] = type; wr_u32(buf + 5, counter); wr_u16(buf + 9, frag_idx); wr_u16(buf + 11, total_frags); wr_u32(buf + 13, payload_bytes); } /*---------------------------------------------------------------------------*/ /* Clocks */ /*---------------------------------------------------------------------------*/ static double now_mono(void) { struct timespec ts; clock_gettime(CLOCK_MONOTONIC, &ts); return (double)ts.tv_sec + (double)ts.tv_nsec * 1e-9; } static double now_wall(void) { struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); return (double)ts.tv_sec + (double)ts.tv_nsec * 1e-9; } static void sleep_s(double s) { struct timespec ts; if (s <= 0.0) { return; } ts.tv_sec = (time_t)s; ts.tv_nsec = (long)((s - (double)ts.tv_sec) * 1e9); (void)nanosleep(&ts, NULL); } /*---------------------------------------------------------------------------*/ /* Internal state */ /*---------------------------------------------------------------------------*/ typedef struct { int active; uint32_t counter; uint8_t type; uint16_t total_fragments; uint16_t received_fragments; uint8_t mask[UDPS_FRAG_MASK_BYTES]; uint8_t *payload; size_t payload_cap; uint32_t chunk_size; /**< Learned from fragment 0; fixes placement. */ uint32_t assembled_bytes;/**< Highest byte written; the true payload size. */ double first_seen; } udps_slot_t; struct udps_client { udps_client_config_t cfg; /* Owned copies: the caller's strings need not outlive the create call. */ char server_addr[128]; char mcast_group[128]; char iface_addr[128]; struct sockaddr_in server_sa; int udp_fd; /**< Unicast receive socket, or joined multicast socket. */ int tcp_fd; /**< Multicast control channel; -1 in unicast mode. */ int connected; int ever_connected; double last_data; /**< Monotonic; drives the silence timeout. */ double last_keepalive; double disconnect_t; /**< Monotonic; 0 before the first attempt. */ udps_slot_t slots[UDPS_MAX_SLOTS]; udps_signal_t *sigs; uint32_t num_sigs; size_t sigs_cap; uint8_t publish_mode; double *valbuf; /**< Arena holding one frame's doubles. */ size_t valbuf_cap; udps_signal_values_t *vals; size_t vals_cap; uint8_t *rxbuf; uint32_t last_counter; int have_counter; udps_stats_t stats; char err[256]; udps_config_cb on_config; udps_data_cb on_data; udps_event_cb on_event; void *user; }; static void emit_event(udps_client_t *c, udps_event_t ev, const char *detail) { if (c->on_event != NULL) { c->on_event(ev, detail, c->user); } } /** Records an error, reports it, and always returns -1 for tail calls. */ static int fail(udps_client_t *c, const char *fmt, ...) { va_list ap; va_start(ap, fmt); vsnprintf(c->err, sizeof c->err, fmt, ap); va_end(ap); emit_event(c, UDPS_EVENT_ERROR, c->err); return -1; } /** Grows @p *buf to at least @p need bytes, preserving nothing. */ static int ensure_cap(void **buf, size_t *cap, size_t need) { void *p; if (*cap >= need) { return 0; } p = realloc(*buf, need); if (p == NULL) { return -1; } *buf = p; *cap = need; return 0; } /*---------------------------------------------------------------------------*/ /* Stateless helpers (public) */ /*---------------------------------------------------------------------------*/ uint32_t udps_signal_num_elements(const udps_signal_t *s) { uint32_t r, c, n; if (s == NULL) { return 0u; } r = (s->num_rows == 0u) ? 1u : s->num_rows; c = (s->num_cols == 0u) ? 1u : s->num_cols; if (r > UDPS_MAX_ELEMENTS || c > UDPS_MAX_ELEMENTS) { return UDPS_MAX_ELEMENTS; } n = r * c; return (n > UDPS_MAX_ELEMENTS) ? UDPS_MAX_ELEMENTS : n; } const char *udps_type_name(uint8_t tc) { switch (tc) { case UDPS_T_UINT8: return "uint8"; case UDPS_T_INT8: return "int8"; case UDPS_T_UINT16: return "uint16"; case UDPS_T_INT16: return "int16"; case UDPS_T_UINT32: return "uint32"; case UDPS_T_INT32: return "int32"; case UDPS_T_UINT64: return "uint64"; case UDPS_T_INT64: return "int64"; case UDPS_T_FLOAT32: return "float32"; case UDPS_T_FLOAT64: return "float64"; default: return "unknown"; } } int udps_parse_header(const void *buf, size_t len, udps_header_t *out) { const uint8_t *b = (const uint8_t *)buf; if (b == NULL || out == NULL || len < UDPS_HEADER_SIZE) { return -1; } out->magic = rd_u32(b); if (out->magic != UDPS_MAGIC) { return -1; } out->type = b[4]; out->counter = rd_u32(b + 5); out->fragment_idx = rd_u16(b + 9); out->total_fragments = rd_u16(b + 11); out->payload_bytes = rd_u32(b + 13); return 0; } int udps_parse_config(const void *payload, size_t len, udps_signal_t *sigs, uint32_t max_signals, uint32_t *num_signals, uint8_t *publish_mode) { const uint8_t *p = (const uint8_t *)payload; uint32_t n, i; size_t off; if (p == NULL || sigs == NULL || num_signals == NULL || len < 4u) { return -1; } n = rd_u32(p); /* A CONFIG cannot describe more signals than its payload can hold. */ if (n > (len - 4u) / UDPS_SIGNAL_DESC_SIZE || n > max_signals) { return -1; } off = 4u; for (i = 0u; i < n; i++) { const uint8_t *d = p + off; udps_signal_t *s = &sigs[i]; memcpy(s->name, d, 64); s->name[64] = '\0'; s->type_code = d[64]; s->quant_type = d[65]; s->num_dimensions = d[66]; s->num_rows = rd_u32(d + 67); s->num_cols = rd_u32(d + 71); s->range_min = rd_f64(d + 75); s->range_max = rd_f64(d + 83); s->time_mode = d[91]; s->sampling_rate = rd_f64(d + 92); s->time_signal_idx = rd_u32(d + 100); memcpy(s->unit, d + 104, 32); s->unit[32] = '\0'; off += UDPS_SIGNAL_DESC_SIZE; } *num_signals = n; if (publish_mode != NULL) { /* Trailing byte, absent in streams from older producers. */ *publish_mode = (off < len) ? p[off] : (uint8_t)UDPS_PUBLISH_STRICT; } return 0; } double udps_frame_value(const udps_frame_t *f, uint32_t sig, uint32_t sample, uint32_t elem) { const udps_signal_values_t *v; uint32_t idx; if (f == NULL || sig >= f->num_signals) { return 0.0; } v = &f->values[sig]; /* Accumulated scalars hold one value per batch slot; everything else holds * one value per element and repeats across slots. */ idx = (v->count == f->num_samples && udps_signal_num_elements(&f->signals[sig]) == 1u) ? sample : elem; return (idx < v->count) ? v->values[idx] : 0.0; } double udps_frame_element_time(const udps_frame_t *f, uint32_t sig, uint32_t elem) { const udps_signal_t *s; uint32_t n; double dt; if (f == NULL || sig >= f->num_signals) { return 0.0; } s = &f->signals[sig]; n = udps_signal_num_elements(s); if (n <= 1u || s->sampling_rate <= 0.0 || (s->time_mode != UDPS_TIME_FIRST_SAMPLE && s->time_mode != UDPS_TIME_LAST_SAMPLE)) { return f->recv_time; } if (elem >= n) { elem = n - 1u; } dt = 1.0 / s->sampling_rate; /* Anchor on arrival == the newest element, and walk backwards. */ return f->recv_time - (double)(n - 1u - elem) * dt; } /*---------------------------------------------------------------------------*/ /* Payload decoding */ /*---------------------------------------------------------------------------*/ static size_t raw_type_size(uint8_t tc) { switch (tc) { case UDPS_T_UINT8: case UDPS_T_INT8: return 1u; case UDPS_T_UINT16: case UDPS_T_INT16: return 2u; case UDPS_T_UINT32: case UDPS_T_INT32: case UDPS_T_FLOAT32: return 4u; case UDPS_T_UINT64: case UDPS_T_INT64: case UDPS_T_FLOAT64: return 8u; default: return 0u; } } static size_t quant_size(uint8_t qt) { switch (qt) { case UDPS_QUANT_UINT8: case UDPS_QUANT_INT8: return 1u; case UDPS_QUANT_UINT16: case UDPS_QUANT_INT16: return 2u; default: return 0u; } } static double read_raw(const uint8_t *p, uint8_t tc) { switch (tc) { case UDPS_T_UINT8: return (double)p[0]; case UDPS_T_INT8: return (double)(int8_t)p[0]; case UDPS_T_UINT16: return (double)rd_u16(p); case UDPS_T_INT16: return (double)(int16_t)rd_u16(p); case UDPS_T_UINT32: return (double)rd_u32(p); case UDPS_T_INT32: return (double)(int32_t)rd_u32(p); case UDPS_T_UINT64: return (double)rd_u64(p); case UDPS_T_INT64: return (double)(int64_t)rd_u64(p); case UDPS_T_FLOAT32: return (double)rd_f32(p); case UDPS_T_FLOAT64: return rd_f64(p); default: return 0.0; } } /** Expands a quantised integer back onto the signal's physical range. */ static double dequantise(uint8_t qt, uint16_t raw, double lo, double hi) { double span = hi - lo; switch (qt) { case UDPS_QUANT_UINT8: return lo + ((double)(uint8_t)raw / 255.0) * span; case UDPS_QUANT_INT8: return lo + (((double)(int8_t)(uint8_t)raw + 127.0) / 254.0) * span; case UDPS_QUANT_UINT16: return lo + ((double)raw / 65535.0) * span; case UDPS_QUANT_INT16: return lo + (((double)(int16_t)raw + 32767.0) / 65534.0) * span; default: return 0.0; } } /** Reads @p n consecutive elements of @p s, advancing @p off. */ static int parse_elems(const uint8_t *pl, size_t len, size_t *off, uint32_t n, const udps_signal_t *s, double *out) { int quantised = (s->quant_type != UDPS_QUANT_NONE); size_t sz = quantised ? quant_size(s->quant_type) : raw_type_size(s->type_code); size_t base = *off; uint32_t i; if (sz == 0u || base > len) { return -1; } /* Division rather than multiplication: no overflow on a crafted count. */ if ((size_t)n > (len - base) / sz) { return -1; } for (i = 0u; i < n; i++) { const uint8_t *p = pl + base + (size_t)i * sz; if (quantised) { uint16_t raw = (sz == 1u) ? (uint16_t)p[0] : rd_u16(p); out[i] = dequantise(s->quant_type, raw, s->range_min, s->range_max); } else { out[i] = read_raw(p, s->type_code); } } *off = base + (size_t)n * sz; return 0; } static int decode_config(udps_client_t *c, const uint8_t *pl, size_t len) { uint32_t claimed; if (len < 4u) { return fail(c, "CONFIG payload too short (%lu bytes)", (unsigned long)len); } claimed = rd_u32(pl); if (claimed > (len - 4u) / UDPS_SIGNAL_DESC_SIZE || claimed > UDPS_MAX_SIGNALS) { return fail(c, "CONFIG claims %lu signals, payload holds %lu", (unsigned long)claimed, (unsigned long)((len - 4u) / UDPS_SIGNAL_DESC_SIZE)); } if (ensure_cap((void **)&c->sigs, &c->sigs_cap, (claimed ? claimed : 1u) * sizeof(udps_signal_t)) != 0) { return fail(c, "out of memory for %lu signals", (unsigned long)claimed); } if (udps_parse_config(pl, len, c->sigs, claimed, &c->num_sigs, &c->publish_mode) != 0) { c->num_sigs = 0u; return fail(c, "malformed CONFIG payload"); } if (ensure_cap((void **)&c->vals, &c->vals_cap, (c->num_sigs ? c->num_sigs : 1u) * sizeof(udps_signal_values_t)) != 0) { c->num_sigs = 0u; return fail(c, "out of memory for signal value table"); } c->stats.config_updates++; if (c->on_config != NULL) { c->on_config(c->sigs, c->num_sigs, c->publish_mode, c->user); } return 0; } static int decode_data(udps_client_t *c, const uint8_t *pl, size_t len, uint32_t counter, double recv_time) { uint32_t nsamples = 1u; size_t off = 8u; size_t total = 0u; size_t written = 0u; uint32_t i; udps_frame_t frame; if (c->num_sigs == 0u) { return 0; /* DATA before CONFIG: nothing to decode against. */ } if (len < 8u) { return fail(c, "DATA payload too short (%lu bytes)", (unsigned long)len); } if (c->publish_mode == UDPS_PUBLISH_ACCUMULATE) { if (len < 12u) { return fail(c, "accumulate DATA payload missing sample count"); } nsamples = rd_u32(pl + 8); off = 12u; if (nsamples == 0u) { return 0; } if (nsamples > UDPS_MAX_ELEMENTS) { return fail(c, "accumulate sample count %lu out of range", (unsigned long)nsamples); } } for (i = 0u; i < c->num_sigs; i++) { uint32_t n = udps_signal_num_elements(&c->sigs[i]); total += (n == 1u) ? nsamples : n; } if (ensure_cap((void **)&c->valbuf, &c->valbuf_cap, (total ? total : 1u) * sizeof(double)) != 0) { return fail(c, "out of memory for %lu decoded values", (unsigned long)total); } for (i = 0u; i < c->num_sigs; i++) { uint32_t n = udps_signal_num_elements(&c->sigs[i]); uint32_t count = (n == 1u) ? nsamples : n; if (parse_elems(pl, len, &off, count, &c->sigs[i], c->valbuf + written) != 0) { return fail(c, "DATA payload truncated at signal '%s'", c->sigs[i].name); } c->vals[i].values = c->valbuf + written; c->vals[i].count = count; written += count; } if (c->have_counter && counter > c->last_counter + 1u) { c->stats.counter_gaps += counter - c->last_counter - 1u; } c->last_counter = counter; c->have_counter = 1; c->stats.frames_delivered++; if (c->on_data != NULL) { frame.counter = counter; frame.hrt = rd_u64(pl); frame.recv_time = recv_time; frame.publish_mode = c->publish_mode; frame.num_samples = nsamples; frame.num_signals = c->num_sigs; frame.signals = c->sigs; frame.values = c->vals; c->on_data(&frame, c->user); } return 0; } /*---------------------------------------------------------------------------*/ /* Fragment reassembly */ /*---------------------------------------------------------------------------*/ static void slot_reset_all(udps_client_t *c) { int i; for (i = 0; i < UDPS_MAX_SLOTS; i++) { c->slots[i].active = 0; } } static void gc_slots(udps_client_t *c, double now) { int i; for (i = 0; i < UDPS_MAX_SLOTS; i++) { if (c->slots[i].active && (now - c->slots[i].first_seen) > UDPS_SLOT_STALE_S) { c->slots[i].active = 0; c->stats.fragments_dropped += c->slots[i].received_fragments; } } } static void deliver_payload(udps_client_t *c, uint8_t type, const uint8_t *pl, size_t len, uint32_t counter, double recv_time) { if (type == UDPS_PKT_CONFIG) { (void)decode_config(c, pl, len); } else { (void)decode_data(c, pl, len, counter, recv_time); } } /** * @brief Files one fragment of a multi-fragment update. * * Fragments carry no offset, only an index, so placement relies on every * fragment but the last being the same size — learned from fragment 0. */ static void place_fragment(udps_client_t *c, const udps_header_t *h, const uint8_t *payload, size_t payload_bytes, double recv_time) { int slot = -1; int i; udps_slot_t *s; size_t byte_idx; uint8_t bit; size_t offset; if (h->fragment_idx >= h->total_fragments || h->total_fragments > UDPS_MAX_FRAGMENTS) { c->stats.fragments_dropped++; return; } for (i = 0; i < UDPS_MAX_SLOTS; i++) { if (c->slots[i].active && c->slots[i].counter == h->counter && c->slots[i].type == h->type) { slot = i; break; } } if (slot < 0) { double oldest = 0.0; for (i = 0; i < UDPS_MAX_SLOTS; i++) { if (!c->slots[i].active) { slot = i; break; } if (slot < 0 || c->slots[i].first_seen < oldest) { oldest = c->slots[i].first_seen; slot = i; } } s = &c->slots[slot]; if (s->active) { /* All slots busy: the oldest update is never going to complete. */ c->stats.fragments_dropped += s->received_fragments; } if (ensure_cap((void **)&s->payload, &s->payload_cap, c->cfg.max_packet_bytes) != 0) { (void)fail(c, "out of memory for a %lu byte reassembly buffer", (unsigned long)c->cfg.max_packet_bytes); s->active = 0; return; } s->active = 1; s->counter = h->counter; s->type = h->type; s->total_fragments = h->total_fragments; s->received_fragments = 0u; s->chunk_size = 0u; s->assembled_bytes = 0u; s->first_seen = recv_time; memset(s->mask, 0, sizeof s->mask); } s = &c->slots[slot]; byte_idx = (size_t)h->fragment_idx / 8u; bit = (uint8_t)(1u << (h->fragment_idx % 8u)); if ((s->mask[byte_idx] & bit) != 0u) { c->stats.fragments_dropped++; /* duplicate */ return; } if (s->chunk_size == 0u) { if (h->fragment_idx != 0u) { /* Fragment 0 was lost, so no offset can be computed for this one. */ c->stats.fragments_dropped++; return; } s->chunk_size = (uint32_t)payload_bytes; } offset = (size_t)h->fragment_idx * s->chunk_size; if (offset + payload_bytes > s->payload_cap) { c->stats.fragments_dropped++; return; } if (payload_bytes > 0u) { memcpy(s->payload + offset, payload, payload_bytes); } if (offset + payload_bytes > s->assembled_bytes) { s->assembled_bytes = (uint32_t)(offset + payload_bytes); } s->mask[byte_idx] |= bit; s->received_fragments++; if (s->received_fragments >= s->total_fragments) { s->active = 0; deliver_payload(c, s->type, s->payload, s->assembled_bytes, s->counter, recv_time); } } /** Validates one datagram (or TCP frame) and routes it. */ static void handle_packet(udps_client_t *c, const uint8_t *buf, size_t len) { udps_header_t h; double recv_time = now_wall(); if (udps_parse_header(buf, len, &h) != 0) { return; /* Not ours: stray traffic on a shared multicast port. */ } if (h.type != UDPS_PKT_DATA && h.type != UDPS_PKT_CONFIG) { return; } if ((size_t)h.payload_bytes + UDPS_HEADER_SIZE > len) { return; /* truncated */ } c->stats.packets_received++; c->stats.bytes_received += len; if (h.total_fragments <= 1u) { deliver_payload(c, h.type, buf + UDPS_HEADER_SIZE, h.payload_bytes, h.counter, recv_time); } else { place_fragment(c, &h, buf + UDPS_HEADER_SIZE, h.payload_bytes, recv_time); } } /*---------------------------------------------------------------------------*/ /* Transport */ /*---------------------------------------------------------------------------*/ /** Resolves a dotted quad, falling back to a name lookup. */ static int resolve_ipv4(const char *host, struct in_addr *out) { struct addrinfo hints; struct addrinfo *res = NULL; if (inet_pton(AF_INET, host, out) == 1) { return 0; } memset(&hints, 0, sizeof hints); hints.ai_family = AF_INET; hints.ai_socktype = SOCK_DGRAM; if (getaddrinfo(host, NULL, &hints, &res) != 0 || res == NULL) { return -1; } *out = ((struct sockaddr_in *)(void *)res->ai_addr)->sin_addr; freeaddrinfo(res); return 0; } static void set_nonblocking(int fd) { int flags = fcntl(fd, F_GETFL, 0); if (flags >= 0) { (void)fcntl(fd, F_SETFL, flags | O_NONBLOCK); } } /** Raises SO_RCVBUF; a small kernel buffer silently drops burst traffic. */ static void set_recv_buffer(int fd, uint32_t bytes) { int v = (int)bytes; if (v > 0) { (void)setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &v, (socklen_t)sizeof v); } } static int send_control(udps_client_t *c, uint8_t type) { uint8_t pkt[UDPS_HEADER_SIZE]; ssize_t n; build_header(pkt, type, 0u, 0u, 1u, 0u); if (c->tcp_fd >= 0) { n = send(c->tcp_fd, pkt, sizeof pkt, 0); } else if (c->udp_fd >= 0) { n = sendto(c->udp_fd, pkt, sizeof pkt, 0, (struct sockaddr *)&c->server_sa, (socklen_t)sizeof c->server_sa); } else { return -1; } return (n == (ssize_t)sizeof pkt) ? 0 : -1; } static void close_sockets(udps_client_t *c) { if (c->udp_fd >= 0) { (void)close(c->udp_fd); c->udp_fd = -1; } if (c->tcp_fd >= 0) { (void)close(c->tcp_fd); c->tcp_fd = -1; } } static int connect_unicast(udps_client_t *c) { struct sockaddr_in local; c->udp_fd = socket(AF_INET, SOCK_DGRAM, 0); if (c->udp_fd < 0) { return fail(c, "socket() failed: %s", strerror(errno)); } set_recv_buffer(c->udp_fd, c->cfg.recv_buffer_bytes); memset(&local, 0, sizeof local); local.sin_family = AF_INET; local.sin_addr.s_addr = htonl(INADDR_ANY); local.sin_port = 0; /* ephemeral: the server replies to this port */ if (bind(c->udp_fd, (struct sockaddr *)&local, (socklen_t)sizeof local) != 0) { int e = errno; close_sockets(c); return fail(c, "bind() failed: %s", strerror(e)); } set_nonblocking(c->udp_fd); if (send_control(c, UDPS_PKT_CONNECT) != 0) { int e = errno; close_sockets(c); return fail(c, "CONNECT to %s:%u failed: %s", c->server_addr, (unsigned)c->cfg.server_port, strerror(e)); } return 0; } static int connect_multicast(udps_client_t *c) { struct sockaddr_in local; struct ip_mreq mreq; struct timeval tv; int on = 1; /* Join before announcing: the server multicasts CONFIG the moment it sees * CONNECT, and a group we have not joined yet drops it in the kernel. */ c->udp_fd = socket(AF_INET, SOCK_DGRAM, 0); if (c->udp_fd < 0) { return fail(c, "socket() failed: %s", strerror(errno)); } (void)setsockopt(c->udp_fd, SOL_SOCKET, SO_REUSEADDR, &on, (socklen_t)sizeof on); set_recv_buffer(c->udp_fd, c->cfg.recv_buffer_bytes); memset(&local, 0, sizeof local); local.sin_family = AF_INET; local.sin_addr.s_addr = htonl(INADDR_ANY); local.sin_port = htons(c->cfg.data_port); if (bind(c->udp_fd, (struct sockaddr *)&local, (socklen_t)sizeof local) != 0) { int e = errno; close_sockets(c); return fail(c, "bind() on data port %u failed: %s", (unsigned)c->cfg.data_port, strerror(e)); } memset(&mreq, 0, sizeof mreq); if (resolve_ipv4(c->mcast_group, &mreq.imr_multiaddr) != 0) { close_sockets(c); return fail(c, "bad multicast group '%s'", c->mcast_group); } if (c->iface_addr[0] != '\0') { if (resolve_ipv4(c->iface_addr, &mreq.imr_interface) != 0) { close_sockets(c); return fail(c, "bad interface address '%s'", c->iface_addr); } } else { mreq.imr_interface.s_addr = htonl(INADDR_ANY); } if (setsockopt(c->udp_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, (socklen_t)sizeof mreq) != 0) { int e = errno; close_sockets(c); return fail(c, "joining %s failed: %s", c->mcast_group, strerror(e)); } set_nonblocking(c->udp_fd); /* Control channel: CONNECT out, CONFIG back. */ c->tcp_fd = socket(AF_INET, SOCK_STREAM, 0); if (c->tcp_fd < 0) { int e = errno; close_sockets(c); return fail(c, "TCP socket() failed: %s", strerror(e)); } 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)); } /* Bound wait so a half-received control frame cannot wedge the poll loop. */ tv.tv_sec = 1; tv.tv_usec = 0; (void)setsockopt(c->tcp_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, (socklen_t)sizeof tv); if (send_control(c, UDPS_PKT_CONNECT) != 0) { int e = errno; close_sockets(c); return fail(c, "CONNECT over TCP 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); if (rc != 0) { c->disconnect_t = now_mono(); return rc; } c->connected = 1; c->last_data = now_mono(); c->last_keepalive = c->last_data; c->have_counter = 0; slot_reset_all(c); if (c->ever_connected) { c->stats.reconnects++; } c->ever_connected = 1; emit_event(c, UDPS_EVENT_CONNECTED, c->server_addr); return 0; } static void do_disconnect(udps_client_t *c, const char *why) { if (!c->connected) { return; } (void)send_control(c, UDPS_PKT_DISCONNECT); close_sockets(c); c->connected = 0; c->disconnect_t = now_mono(); slot_reset_all(c); emit_event(c, UDPS_EVENT_DISCONNECTED, why); } /** Reads exactly @p n bytes from the control connection. */ static int read_exact_tcp(udps_client_t *c, uint8_t *dst, size_t n) { size_t got = 0u; while (got < n) { ssize_t r = recv(c->tcp_fd, dst + got, n - got, 0); if (r == 0) { return -1; /* orderly close */ } if (r < 0) { if (errno == EINTR) { continue; } return -1; } got += (size_t)r; } return 0; } /** Reads one framed UDPS packet off the TCP control connection. */ static int read_tcp_frame(udps_client_t *c) { udps_header_t h; if (read_exact_tcp(c, c->rxbuf, UDPS_HEADER_SIZE) != 0) { return -1; } if (udps_parse_header(c->rxbuf, UDPS_HEADER_SIZE, &h) != 0) { return -1; /* desynchronised: the stream cannot be resynced */ } if (h.payload_bytes > (uint32_t)(UDPS_RX_BUF_BYTES - UDPS_HEADER_SIZE)) { return -1; } if (h.payload_bytes > 0u && read_exact_tcp(c, c->rxbuf + UDPS_HEADER_SIZE, h.payload_bytes) != 0) { return -1; } handle_packet(c, c->rxbuf, UDPS_HEADER_SIZE + h.payload_bytes); return 0; } /*---------------------------------------------------------------------------*/ /* Public client API */ /*---------------------------------------------------------------------------*/ void udps_client_config_init(udps_client_config_t *cfg) { if (cfg == NULL) { return; } memset(cfg, 0, sizeof *cfg); cfg->silence_timeout_s = 1.0; cfg->reconnect_delay_s = 2.0; cfg->keepalive_interval_s = 15.0; /* server evicts silent clients at 30 s */ cfg->recv_buffer_bytes = 4u * 1024u * 1024u; cfg->max_packet_bytes = 1u * 1024u * 1024u; } static void copy_str(char *dst, size_t cap, const char *src) { if (src == NULL) { dst[0] = '\0'; return; } strncpy(dst, src, cap - 1u); dst[cap - 1u] = '\0'; } udps_client_t *udps_client_create(const udps_client_config_t *cfg) { udps_client_t *c; if (cfg == NULL || cfg->server_addr == NULL || cfg->server_port == 0u) { return NULL; } c = (udps_client_t *)calloc(1u, sizeof *c); if (c == NULL) { 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); /* 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; if (c->cfg.max_packet_bytes == 0u) { c->cfg.max_packet_bytes = 1u * 1024u * 1024u; } if (c->cfg.data_port == 0u) { c->cfg.data_port = (uint16_t)(cfg->server_port + 1u); } memset(&c->server_sa, 0, sizeof c->server_sa); c->server_sa.sin_family = AF_INET; c->server_sa.sin_port = htons(cfg->server_port); if (resolve_ipv4(c->server_addr, &c->server_sa.sin_addr) != 0) { free(c); return NULL; } c->rxbuf = (uint8_t *)malloc(UDPS_RX_BUF_BYTES); if (c->rxbuf == NULL) { free(c); return NULL; } strcpy(c->err, "no error"); return c; } void udps_client_destroy(udps_client_t *c) { int i; if (c == NULL) { return; } do_disconnect(c, "closed by application"); close_sockets(c); for (i = 0; i < UDPS_MAX_SLOTS; i++) { free(c->slots[i].payload); } free(c->sigs); free(c->valbuf); free(c->vals); free(c->rxbuf); free(c); } void udps_client_set_callbacks(udps_client_t *c, udps_config_cb on_config, udps_data_cb on_data, udps_event_cb on_event, void *user) { if (c == NULL) { return; } c->on_config = on_config; c->on_data = on_data; c->on_event = on_event; c->user = user; } int udps_client_is_connected(const udps_client_t *c) { return (c != NULL) && c->connected; } const udps_signal_t *udps_client_signals(const udps_client_t *c, uint32_t *num_signals) { if (c == NULL) { if (num_signals != NULL) { *num_signals = 0u; } return NULL; } if (num_signals != NULL) { *num_signals = c->num_sigs; } return (c->num_sigs > 0u) ? c->sigs : NULL; } uint8_t udps_client_publish_mode(const udps_client_t *c) { return (c != NULL) ? c->publish_mode : (uint8_t)UDPS_PUBLISH_STRICT; } void udps_client_stats(const udps_client_t *c, udps_stats_t *out) { if (c != NULL && out != NULL) { *out = c->stats; } } const char *udps_client_last_error(const udps_client_t *c) { return (c != NULL) ? c->err : "invalid client"; } /** Clamps @p budget so a pending deadline is not slept through. */ static double clamp_deadline(double budget, double interval, double elapsed) { double left; if (interval <= 0.0) { return budget; } left = interval - elapsed; if (left < 0.0) { left = 0.0; } return (left < budget) ? left : budget; } int udps_client_poll(udps_client_t *c, int timeout_ms) { double now; double budget; fd_set rset; struct timeval tv; int maxfd; int nready; int processed = 0; if (c == NULL) { return -1; } if (!c->connected) { now = now_mono(); if (c->disconnect_t > 0.0) { double wait = c->cfg.reconnect_delay_s - (now - c->disconnect_t); if (wait > 0.0) { /* Idle out the retry delay rather than hammering the server. */ double cap = (timeout_ms < 0) ? wait : (double)timeout_ms / 1000.0; sleep_s((wait < cap) ? wait : cap); return 0; } } if (do_connect(c) != 0) { return -1; } } now = now_mono(); budget = (timeout_ms < 0) ? 1.0 : (double)timeout_ms / 1000.0; /* Only unicast sends keepalives; clamping on them in multicast mode would * spin on a deadline that never gets refreshed. */ if (c->tcp_fd < 0) { budget = clamp_deadline(budget, c->cfg.keepalive_interval_s, now - c->last_keepalive); } budget = clamp_deadline(budget, c->cfg.silence_timeout_s, now - c->last_data); FD_ZERO(&rset); maxfd = -1; if (c->udp_fd >= 0 && c->udp_fd < FD_SETSIZE) { FD_SET(c->udp_fd, &rset); maxfd = c->udp_fd; } if (c->tcp_fd >= 0 && c->tcp_fd < FD_SETSIZE) { FD_SET(c->tcp_fd, &rset); if (c->tcp_fd > maxfd) { maxfd = c->tcp_fd; } } if (maxfd < 0) { do_disconnect(c, "no usable socket"); return fail(c, "socket descriptor outside FD_SETSIZE"); } tv.tv_sec = (time_t)budget; tv.tv_usec = (suseconds_t)((budget - (double)tv.tv_sec) * 1e6); nready = select(maxfd + 1, &rset, NULL, NULL, &tv); if (nready < 0) { if (errno == EINTR) { return 0; } do_disconnect(c, "select failed"); return fail(c, "select() failed: %s", strerror(errno)); } if (nready > 0 && c->tcp_fd >= 0 && FD_ISSET(c->tcp_fd, &rset)) { if (read_tcp_frame(c) != 0) { do_disconnect(c, "control connection lost"); return fail(c, "TCP control connection lost"); } c->last_data = now_mono(); processed++; } if (nready > 0 && c->udp_fd >= 0 && FD_ISSET(c->udp_fd, &rset)) { int drained = 0; while (drained < UDPS_DRAIN_LIMIT) { ssize_t n = recv(c->udp_fd, c->rxbuf, UDPS_RX_BUF_BYTES, 0); if (n < 0) { if (errno == EINTR) { continue; } if (errno == EAGAIN || errno == EWOULDBLOCK) { break; /* socket drained */ } do_disconnect(c, "receive failed"); return fail(c, "recv() failed: %s", strerror(errno)); } drained++; c->last_data = now_mono(); if ((size_t)n >= UDPS_HEADER_SIZE) { handle_packet(c, c->rxbuf, (size_t)n); } } processed += drained; } now = now_mono(); if (c->cfg.keepalive_interval_s > 0.0 && c->tcp_fd < 0 && (now - c->last_keepalive) >= c->cfg.keepalive_interval_s) { /* ACK, not CONNECT: it refreshes the server's last-seen without making * it resend CONFIG. */ (void)send_control(c, UDPS_PKT_ACK); c->last_keepalive = now; } gc_slots(c, now_wall()); if (c->cfg.silence_timeout_s > 0.0 && (now - c->last_data) >= c->cfg.silence_timeout_s) { do_disconnect(c, "server went silent"); return fail(c, "no data for %.3f s", c->cfg.silence_timeout_s); } return processed; }