#include "rel.h" #include #include #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; }