martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
a2efc142c3c20b9f14ad85dc41ccdf800faca52f
MARTe-Integrated-Components/Common/Client/c/udps_client.c
T
Martino FerrariandClaude Opus 4.6 7c5eb31a52 feat: standalone C/C++ UDPS client library 
Consuming a UDPStreamer feed so far meant either linking MARTe2 (UDPSClient)
or writing Go (Common/Client/go/udpsprotocol). Common/Client/c fills the gap
for plain C/C++ integrators: two files depending on nothing but libc and BSD
sockets, covering the whole receive path — CONNECT, fragment reassembly,
CONFIG/DATA decoding with dequantisation, keepalives and silence-triggered
reconnect. No threads are spawned; udps_client_poll() does all the work and
runs every callback, so it drops into an existing event loop unsynchronised.

Verified against run_udp_producer.sh at 1 Msps: unicast (120 MiB, no loss) and
multicast with 12-fragment cycles (116k datagrams, no loss).

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
2026-08-22 16:51:40 +02:00

1210 lines
39 KiB
C
Raw Blame History

/**
* @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 <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <netdb.h>
#include <netinet/in.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>
/*---------------------------------------------------------------------------*/
/* 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;
}
Reference in New Issue View Git Blame Copy Permalink