Files
led-controller/main/rel.c
T
jimmyandClaude Sonnet 5.5 2543673bd8 feat: sequences v2, bridges, and sim-controller
Rework sequences (engine, seqplay, envelope, API, web UI), add bridge
support, add the host sim-controller and rig simulation tests, and
update docs, presets and project tooling config.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
2026-11-10 17:47:27 +13:00

452 lines
12 KiB
C

#include "rel.h"
#include <stdlib.h>
#include <string.h>
#include "esp_check.h"
#include "esp_log.h"
#include "esp_random.h"
#include "esp_timer.h"
#include "bench.h"
#include "espnow_tx.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "freertos/task.h"
// Reliable-link frames live inside the existing 0x4C wire protocol as extra message types
#define WIRE_MAGIC 0x4C
#define M_ANNOUNCE 0x01
#define M_DATA 0x20
#define M_ACK 0x21
#define M_DIGEST 0x22
#define M_RESYNC 0x23
#define M_ECHO 0x24
#define M_OTA_STATUS 0x26
#define M_PING_RSP 0x06
#define F_ACK 1
#define F_COMMIT 2
#define HDR 9
#define MAX_PENDING 32
#define MAX_TRIES 4
#define FIRST_RETRY_MS 60
#define DIGEST_PERIOD_US 1500000
#define MAX_PEERS 32
#define DONE_RING 16
static const char *TAG = "rel";
typedef struct {
bool used;
uint8_t mac[6];
uint16_t seq;
uint8_t cnt, flags, tries;
uint32_t mask;
uint16_t ver;
uint8_t *msg;
size_t len;
int64_t next_us;
} pend_t;
typedef struct {
uint8_t mac[6];
uint8_t len;
uint8_t data[96];
} rx_t;
typedef struct {
bool used;
uint8_t mac[6];
int64_t last_ack_us;
int fails;
} peer_t;
typedef struct {
bool used, ok;
uint8_t mac[6];
uint16_t seq;
} done_t;
static pend_t s_pend[MAX_PENDING];
static peer_t s_peer[MAX_PEERS];
static done_t s_done[DONE_RING]; // recent unicast outcomes, for rel_wait
static int s_done_next;
static SemaphoreHandle_t s_lock;
static QueueHandle_t s_rx;
static uint16_t s_seq;
static volatile uint16_t s_ver;
static rel_stats_t s_stats;
static rel_resync_cb_t s_resync_cb;
// Caller holds s_lock
static void record_done(const pend_t *p, bool ok)
{
done_t *d = &s_done[s_done_next];
s_done_next = (s_done_next + 1) % DONE_RING;
d->used = true;
d->ok = ok;
d->seq = p->seq;
memcpy(d->mac, p->mac, 6);
}
static uint32_t full_mask(int cnt)
{
return cnt >= 32 ? 0xFFFFFFFFu : ((1u << cnt) - 1);
}
static peer_t *peer_get(const uint8_t *mac, bool create)
{
peer_t *free_slot = NULL;
for (int i = 0; i < MAX_PEERS; i++) {
if (s_peer[i].used && memcmp(s_peer[i].mac, mac, 6) == 0) {
return &s_peer[i];
}
if (!s_peer[i].used && !free_slot) {
free_slot = &s_peer[i];
}
}
if (create && free_slot) {
memset(free_slot, 0, sizeof(*free_slot));
free_slot->used = true;
free_slot->last_ack_us = -1;
memcpy(free_slot->mac, mac, 6);
}
return create ? free_slot : NULL;
}
static esp_err_t send_frag(const uint8_t *mac, uint8_t flags, uint16_t seq, uint16_t ver, const uint8_t *msg,
size_t len, int idx, int cnt)
{
uint8_t frame[HDR + REL_FRAG_MAX];
size_t off = (size_t)idx * REL_FRAG_MAX;
size_t n = len - off > REL_FRAG_MAX ? REL_FRAG_MAX : len - off;
frame[0] = WIRE_MAGIC;
frame[1] = M_DATA;
frame[2] = flags;
frame[3] = seq & 255;
frame[4] = seq >> 8;
frame[5] = ver & 255;
frame[6] = ver >> 8;
frame[7] = idx;
frame[8] = cnt;
memcpy(frame + HDR, msg + off, n);
return espnow_tx_send(mac, frame, HDR + n);
}
esp_err_t rel_send(const uint8_t *mac, const void *data, size_t len, bool commit, int repeat)
{
return rel_send_seq(mac, data, len, commit, repeat, NULL);
}
esp_err_t rel_send_seq(const uint8_t *mac, const void *data, size_t len, bool commit, int repeat, uint16_t *seq_out)
{
if (len == 0) {
return ESP_ERR_INVALID_SIZE;
}
int cnt = (len + REL_FRAG_MAX - 1) / REL_FRAG_MAX;
if (cnt > REL_MAX_FRAGS) {
return ESP_ERR_INVALID_SIZE;
}
uint8_t flags = (mac ? F_ACK : 0) | (commit ? F_COMMIT : 0);
uint16_t ver = s_ver;
xSemaphoreTake(s_lock, portMAX_DELAY);
uint16_t seq = ++s_seq;
if (seq_out) {
*seq_out = seq;
}
pend_t *slot = NULL;
if (mac) {
for (int i = 0; i < MAX_PENDING && !slot; i++) {
slot = s_pend[i].used ? NULL : &s_pend[i];
}
if (!slot) {
xSemaphoreGive(s_lock);
return ESP_ERR_NO_MEM;
}
uint8_t *copy = malloc(len);
if (!copy) {
xSemaphoreGive(s_lock);
return ESP_ERR_NO_MEM;
}
memcpy(copy, data, len);
memset(slot, 0, sizeof(*slot));
slot->used = true;
memcpy(slot->mac, mac, 6);
slot->seq = seq;
slot->cnt = cnt;
slot->flags = flags;
slot->ver = ver;
slot->msg = copy;
slot->len = len;
slot->next_us = esp_timer_get_time() + FIRST_RETRY_MS * 1000;
}
xSemaphoreGive(s_lock);
esp_err_t ret = ESP_OK;
for (int r = 0; r < (mac ? 1 : (repeat < 1 ? 1 : repeat)) && ret == ESP_OK; r++) {
for (int i = 0; i < cnt && ret == ESP_OK; i++) {
ret = send_frag(mac, flags, seq, ver, data, len, i, cnt);
}
}
if (ret != ESP_OK && slot) { // queue full: caller retries the whole message under a new sequence
xSemaphoreTake(s_lock, portMAX_DELAY);
free(slot->msg);
slot->used = false;
xSemaphoreGive(s_lock);
}
return ret;
}
esp_err_t rel_wait(const uint8_t *mac, uint16_t seq, int timeout_ms)
{
int64_t until = esp_timer_get_time() + (int64_t)timeout_ms * 1000;
for (;;) {
xSemaphoreTake(s_lock, portMAX_DELAY);
for (int i = 0; i < DONE_RING; i++) {
const done_t *d = &s_done[i];
if (d->used && d->seq == seq && memcmp(d->mac, mac, 6) == 0) {
bool ok = d->ok;
xSemaphoreGive(s_lock);
return ok ? ESP_OK : ESP_FAIL;
}
}
xSemaphoreGive(s_lock);
if (esp_timer_get_time() >= until) {
return ESP_ERR_TIMEOUT;
}
vTaskDelay(pdMS_TO_TICKS(2));
}
}
esp_err_t rel_send_wait(const uint8_t *mac, const void *data, size_t len, int timeout_ms)
{
uint16_t seq;
esp_err_t err;
for (int t = 0; t < 50; t++) { // the radio queue is shared with everything else: wait for room
err = rel_send_seq(mac, data, len, false, 1, &seq);
if (err != ESP_ERR_NO_MEM) {
break;
}
vTaskDelay(pdMS_TO_TICKS(10));
}
return err == ESP_OK ? rel_wait(mac, seq, timeout_ms) : err;
}
void rel_bump_ver(void)
{
uint16_t v = s_ver + 1;
s_ver = v ? v : 1; // 0 means "no state" to drivers
}
uint16_t rel_ver(void)
{
return s_ver;
}
void rel_set_resync_cb(rel_resync_cb_t cb)
{
s_resync_cb = cb;
}
void rel_stats(rel_stats_t *out)
{
*out = s_stats;
}
int rel_peer_status(const uint8_t mac[6], int *fails)
{
int age = -1;
xSemaphoreTake(s_lock, portMAX_DELAY);
peer_t *p = peer_get(mac, false);
if (p) {
age = p->last_ack_us < 0 ? -1 : (int)((esp_timer_get_time() - p->last_ack_us) / 1000);
if (fails) {
*fails = p->fails;
}
} else if (fails) {
*fails = 0;
}
xSemaphoreGive(s_lock);
return age;
}
static rel_announce_cb_t s_announce_cb;
static rel_ota_status_cb_t s_ota_status_cb;
void rel_set_ota_status_cb(rel_ota_status_cb_t cb)
{
s_ota_status_cb = cb;
}
static rel_ping_cb_t s_ping_cb;
void rel_set_ping_cb(rel_ping_cb_t cb)
{
s_ping_cb = cb;
}
void rel_set_announce_cb(rel_announce_cb_t cb)
{
s_announce_cb = cb;
}
static void recv_cb(const uint8_t *src_mac, const uint8_t *data, int len)
{
int64_t now = esp_timer_get_time(); // before anything else: echoes are timing measurements
if (len < 3 || len > (int)sizeof(((rx_t *)0)->data)) {
return;
}
// A driver's boot hello is either a 0x4C ANNOUNCE or a small JSON object; everything else we
// care about is a link reply inside 0x4C
bool announce = (data[0] == WIRE_MAGIC && data[1] == M_ANNOUNCE) || data[0] == '{';
bool reply = data[0] == WIRE_MAGIC &&
(data[1] == M_ACK || data[1] == M_RESYNC || data[1] == M_ECHO || data[1] == M_OTA_STATUS ||
data[1] == M_PING_RSP);
if (!announce && !reply) {
return;
}
if (reply && espnow_tx_drop()) { // lossy-link test: this reply never arrived
return;
}
if (reply && data[1] == M_ECHO) {
bench_on_echo(src_mac, data[2] | (data[3] << 8), now);
return;
}
rx_t rx = { .len = len };
memcpy(rx.mac, src_mac, 6);
memcpy(rx.data, data, len);
xQueueSend(s_rx, &rx, 0); // dropping an ack only costs a retransmit
}
static void handle_rx(const rx_t *rx)
{
if (rx->data[0] == '{' || rx->data[1] == M_ANNOUNCE) {
if (s_announce_cb) {
s_announce_cb(rx->mac, rx->data, rx->len);
}
return;
}
if (rx->data[1] == M_RESYNC) {
s_stats.resyncs++;
if (s_resync_cb) {
s_resync_cb(rx->mac);
}
return;
}
if (rx->data[1] == M_OTA_STATUS) {
if (s_ota_status_cb) {
s_ota_status_cb(rx->mac, rx->data, rx->len);
}
return;
}
if (rx->data[1] == M_PING_RSP) {
// The driver is there: count it as heard, so group updates go to it again
xSemaphoreTake(s_lock, portMAX_DELAY);
peer_t *pe = peer_get(rx->mac, true);
if (pe) {
pe->last_ack_us = esp_timer_get_time();
pe->fails = 0;
}
xSemaphoreGive(s_lock);
if (s_ping_cb) {
s_ping_cb(rx->mac, rx->data, rx->len);
}
return;
}
if (rx->len < 10) { // [4C][21][0][seq u16][cnt][mask u32]
return;
}
uint16_t seq = rx->data[3] | (rx->data[4] << 8);
uint32_t mask = rx->data[6] | (rx->data[7] << 8) | (rx->data[8] << 16) | ((uint32_t)rx->data[9] << 24);
xSemaphoreTake(s_lock, portMAX_DELAY);
for (int i = 0; i < MAX_PENDING; i++) {
pend_t *p = &s_pend[i];
if (p->used && p->seq == seq && memcmp(p->mac, rx->mac, 6) == 0) {
p->mask |= mask;
if ((p->mask & full_mask(p->cnt)) == full_mask(p->cnt)) {
peer_t *pe = peer_get(p->mac, true);
if (pe) {
pe->last_ack_us = esp_timer_get_time();
pe->fails = 0;
}
s_stats.acked++;
record_done(p, true);
free(p->msg);
p->used = false;
}
break;
}
}
xSemaphoreGive(s_lock);
}
static void retry_pending(void)
{
int64_t now = esp_timer_get_time();
xSemaphoreTake(s_lock, portMAX_DELAY);
for (int i = 0; i < MAX_PENDING; i++) {
pend_t *p = &s_pend[i];
if (!p->used || now < p->next_us) {
continue;
}
if (p->tries >= MAX_TRIES) {
peer_t *pe = peer_get(p->mac, true);
if (pe) {
pe->fails++;
}
s_stats.ack_timeouts++;
record_done(p, false);
free(p->msg);
p->used = false;
continue;
}
for (int f = 0; f < p->cnt; f++) {
if (!(p->mask & (1u << f))) {
if (send_frag(p->mac, p->flags, p->seq, p->ver, p->msg, p->len, f, p->cnt) != ESP_OK) {
break; // queue full; try again next round
}
s_stats.retransmits++;
}
}
p->tries++;
p->next_us = now + ((int64_t)(FIRST_RETRY_MS << p->tries)) * 1000;
}
xSemaphoreGive(s_lock);
}
static void rel_task(void *arg)
{
int64_t next_digest = 0;
for (;;) {
rx_t rx;
if (xQueueReceive(s_rx, &rx, pdMS_TO_TICKS(20)) == pdTRUE) {
handle_rx(&rx);
continue; // drain before retrying
}
retry_pending();
int64_t now = esp_timer_get_time();
if (now >= next_digest) {
uint16_t v = s_ver;
uint8_t d[4] = { WIRE_MAGIC, M_DIGEST, v & 255, v >> 8 };
if (espnow_tx_send(NULL, d, sizeof(d)) == ESP_OK) {
s_stats.digests++;
}
next_digest = now + DIGEST_PERIOD_US;
}
}
}
esp_err_t rel_init(void)
{
s_lock = xSemaphoreCreateMutex();
s_rx = xQueueCreate(16, sizeof(rx_t));
ESP_RETURN_ON_FALSE(s_lock && s_rx, ESP_ERR_NO_MEM, TAG, "alloc");
s_seq = esp_random() & 0xFFFF; // drivers remember recent (mac, seq); a restart must not reuse them
s_ver = (esp_random() % 0x7FFE) + 1; // new epoch per boot, so a rebooted controller never matches stale drivers
ESP_RETURN_ON_ERROR(espnow_tx_set_rx_cb(recv_cb), TAG, "recv cb");
xTaskCreate(rel_task, "rel", 4096, NULL, 6, NULL);
ESP_LOGI(TAG, "reliable link up, state version %u", s_ver);
return ESP_OK;
}