martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
feature/udpscope
MARTe-Integrated-Components/Common/Client/c/udps_client.c
T
Martino FerrariandClaude Opus 4.6 deabd257e5 fix(udps): stop packets being dated from an earlier time base 
Reported as samples sporadically carrying a previous packet's timestamp:
holes on one side of the stream and collisions on the other, in both the
Go and the MARTe2 receiver. That it appeared in both is what located it
-- the shared cause is upstream of either client. Four independent
defects, all of which end in a packet's values being placed at a time
that is not theirs.

Reassembly slot exhaustion (the "Reassembly slots full; evicting oldest"
flood). Chunk size was learnt only from fragment 0, so an out-of-order
burst destroyed a packet whose bytes had all arrived and left the slot
occupied until the 2 s GC. Slots were keyed on the counter alone, but
DATA and CONFIG number independently, so equal counters merged the two
streams. The 32-byte received-mask covered 256 of the 512 fragments the
client accepts, so a duplicate above 255 was counted as new and the
packet was delivered with a hole of stale bytes in it. And one datagram
was read per Execute(), which cannot drain a fast producer. Fixed with a
pendingTail deferral, (counter, type) keying, a 64-byte mask, a
256-datagram drain, counter-age slot reclamation, and a 1 Hz aggregated
warning in place of the per-eviction flood.

UDPStreamer dropping whole Accumulate batches. EventSem::ResetWait is
Reset-then-Wait, so a Post() landing while the sender thread was inside
ServiceClients()/SendData() was destroyed by the next Reset. The batch
was then skipped with dataReady false, readyFill was never cleared, and
the following flush overwrote it: an entire run of RT cycles never
reached the wire. The record of pending work now lives in the buffers
rather than in the semaphore edge, which also removes up to
UDPS_DATA_WAIT_MS of latency; genuine backpressure overwrites are
counted and reported. Against the unfixed code the new test sees
2999/3000 batches never consumed.

Period inflation after loss. Accumulated scalars carry no SamplingRate,
so the receiver derives dt from the sender-clock gap -- but dividing it
by the previous packet's sample count is only right while nothing is
lost. One loss doubles the reported period, which spreads a batch a full
batch past its own end and into the range the next packet claims. That
is the hole and the collision, exactly. Inferring the cycle count from
the estimate's own period is not a way out: it has a stable fixed point
wherever gap/dt is an integer, so a real rate change locks it at the old
one for good (AccumDtGTest.FollowsSustainedRateChange).

The packet counter removes the ambiguity, so all three receivers now
order on it: a DATA packet that does not advance the counter is dropped
rather than delivered, because its values are older than data already
handed over. Ordering is on the signed difference so it survives the
uint32 wrap, and the sequence resets on reconnect, where the producer's
counter restarts independently of ours. The loss count that falls out of
the same delta feeds the period estimate as cycles = prevN * (1 + lost),
which reduces exactly to gap/prevN when nothing is lost and therefore
still tracks a genuine rate change. UDPSClient::AcceptDataCounter (C++),
udpsprotocol.SequenceGate (Go), decode_data (C).

The C client's existing gap counter was wrap-unsafe and let a stale
packet rewind last_counter, which made every subsequent gap wrong; it
uses the same code now. Docs/Protocol.md gains an Ordering DATA section
stating the requirement for any receiver, including ones outside this
repository.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
2026-09-02 01:14:40 +02:00

1231 lines
40 KiB
C
Raw Permalink 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;
uint32_t lost = 0u;
udps_frame_t frame;
if (c->num_sigs == 0u) {
return 0; /* DATA before CONFIG: nothing to decode against. */
}
/* Order the sequence before spending anything on the payload.
*
* Reassembly completes in arrival order, not counter order, so a packet
* delayed or duplicated on the wire surfaces after a newer one has already
* been delivered. Its samples carry an older time base: they land on top
* of data the consumer already has and leave the span they should have
* filled empty. Nothing in the payload distinguishes such a packet from a
* good one, only the counter does.
*
* The counter is a wrapping uint32, so it is ordered by the signed
* difference; comparing the values directly would call the first packet
* after the wrap stale and reject the stream from then on. */
if (c->have_counter) {
int32_t delta = (int32_t)(counter - c->last_counter);
if (delta <= 0) {
c->stats.stale_packets++;
return 0;
}
lost = (uint32_t)delta - 1u;
c->stats.counter_gaps += lost;
}
c->last_counter = counter;
c->have_counter = 1;
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;
}
c->stats.frames_delivered++;
if (c->on_data != NULL) {
frame.counter = counter;
frame.lost = lost;
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