- espnow_link, ctrl_msg and beat_sync: the reliable link, envelopes and beat-synced timing. - ctrl_msg: accept an envelope with the trailing save byte. - patterns_frame() and espnow_link_poll() split out of their tasks, so the WebAssembly simulator can drive one frame and one link pass from a timer. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
444 lines
13 KiB
C++
444 lines
13 KiB
C++
/*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*
|
|
* Port of led-driver/src/espnow_rx.py (the MicroPython driver) and the announce and ping parts of
|
|
* espnow_transport.py. Frames arrive in the ESP-NOW callback; beats are taken there, the rest is queued for
|
|
* the link task, which does the reliable-link work and calls ctrl_handle.
|
|
*/
|
|
#include "espnow_link.h"
|
|
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
|
|
#include "esp_check.h"
|
|
#include "esp_log.h"
|
|
#include "esp_now.h"
|
|
#include "esp_random.h"
|
|
#include "esp_timer.h"
|
|
#include "esp_wifi.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/queue.h"
|
|
#include "freertos/task.h"
|
|
|
|
#include "beat_sync.h"
|
|
#include "ctrl_msg.h"
|
|
#include "net.h"
|
|
#include "settings.h"
|
|
|
|
static const char *TAG = "espnow";
|
|
|
|
namespace {
|
|
|
|
constexpr uint8_t kMagic = 0x4C;
|
|
constexpr uint8_t kAnnounce = 0x01;
|
|
constexpr uint8_t kData = 0x20;
|
|
constexpr uint8_t kAck = 0x21;
|
|
constexpr uint8_t kDigest = 0x22;
|
|
constexpr uint8_t kResync = 0x23;
|
|
constexpr uint8_t kBeat = 0x25;
|
|
constexpr uint8_t kPingReq = 0x05;
|
|
constexpr uint8_t kPingRsp = 0x06;
|
|
constexpr uint8_t kFlagAck = 1;
|
|
constexpr uint8_t kFlagCommit = 2;
|
|
|
|
constexpr size_t kHdr = 9; // DATA header: [4C][20][flags][seq u16][ver u16][idx][cnt]
|
|
constexpr size_t kFragMax = 238; // bytes of the inner packet per DATA frame
|
|
constexpr int kMaxFrags = 32;
|
|
constexpr int kAsmSlots = 2; // messages being reassembled at once
|
|
constexpr int kDoneMax = 16; // recent messages kept for duplicate detection
|
|
constexpr int kInboxLen = 24;
|
|
constexpr int64_t kAsmTimeoutUs = 3000000;
|
|
constexpr int64_t kResyncGapUs = 3000000;
|
|
constexpr int64_t kAnnounceUs[3] = { 300000, 1300000, 2600000 };
|
|
// After the boot announces, repeat every 10 s: the radio's channel can hop while the client scans for its
|
|
// network, so one burst can be missed. The controller updates the device it already has, so repeats are harmless.
|
|
constexpr int64_t kAnnounceEveryUs = 10000000;
|
|
|
|
struct Frame {
|
|
uint8_t mac[6];
|
|
uint16_t len;
|
|
uint8_t data[250];
|
|
};
|
|
|
|
struct Asm {
|
|
bool used;
|
|
uint8_t mac[6];
|
|
uint16_t seq;
|
|
uint8_t cnt;
|
|
uint8_t flags;
|
|
uint16_t ver;
|
|
uint32_t mask;
|
|
uint16_t lens[kMaxFrags];
|
|
uint8_t *buf; // cnt * kFragMax
|
|
int64_t last_us;
|
|
};
|
|
|
|
struct Done {
|
|
uint8_t mac[6];
|
|
uint16_t seq;
|
|
};
|
|
|
|
QueueHandle_t s_inbox;
|
|
bool s_started;
|
|
espnow_stats_t s_st;
|
|
Asm s_asm[kAsmSlots];
|
|
Done s_done[kDoneMax];
|
|
int s_ndone;
|
|
uint16_t s_ver; // state version this driver has
|
|
bool s_resync_pending;
|
|
int64_t s_resync_at;
|
|
int64_t s_last_resync;
|
|
bool s_ping_pending;
|
|
int64_t s_ping_at;
|
|
uint8_t s_ping_mac[6];
|
|
uint32_t s_ping_id;
|
|
int s_announce_left;
|
|
int64_t s_announce_base;
|
|
int64_t s_next_announce;
|
|
|
|
// ---- sending ---------------------------------------------------------------------------------
|
|
|
|
esp_err_t send_to(const uint8_t mac[6], const uint8_t *data, size_t len)
|
|
{
|
|
if (!esp_now_is_peer_exist(mac)) {
|
|
esp_now_peer_info_t p = {};
|
|
memcpy(p.peer_addr, mac, 6);
|
|
p.ifidx = WIFI_IF_STA;
|
|
p.encrypt = false;
|
|
esp_err_t e = esp_now_add_peer(&p);
|
|
if (e != ESP_OK && e != ESP_ERR_ESPNOW_EXIST) {
|
|
s_st.send_fail++;
|
|
return e;
|
|
}
|
|
}
|
|
esp_err_t e = esp_now_send(mac, data, len);
|
|
if (e == ESP_OK) {
|
|
s_st.sent++;
|
|
} else {
|
|
s_st.send_fail++;
|
|
}
|
|
return e;
|
|
}
|
|
|
|
const uint8_t kBroadcast[6] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
|
|
|
|
void send_announce(void)
|
|
{
|
|
uint8_t pkt[250];
|
|
size_t n = ctrl_build_announce(pkt, sizeof(pkt));
|
|
if (n) {
|
|
send_to(kBroadcast, pkt, n);
|
|
}
|
|
}
|
|
|
|
void send_ack(const uint8_t mac[6], uint16_t seq, uint8_t cnt, uint32_t mask)
|
|
{
|
|
uint8_t p[10] = { kMagic, kAck, 0, (uint8_t)(seq & 0xFF), (uint8_t)(seq >> 8), cnt,
|
|
(uint8_t)(mask & 0xFF), (uint8_t)(mask >> 8), (uint8_t)(mask >> 16), (uint8_t)(mask >> 24) };
|
|
send_to(mac, p, sizeof(p));
|
|
s_st.acks++;
|
|
}
|
|
|
|
void send_resync(void)
|
|
{
|
|
if (!s_st.master_seen) return;
|
|
uint8_t p[4] = { kMagic, kResync, (uint8_t)(s_ver & 0xFF), (uint8_t)(s_ver >> 8) };
|
|
send_to(s_st.master, p, sizeof(p));
|
|
s_st.resyncs++;
|
|
s_last_resync = esp_timer_get_time();
|
|
}
|
|
|
|
void send_ping_rsp(void)
|
|
{
|
|
net_cfg_t n;
|
|
settings_load_net(&n);
|
|
size_t nl = strlen(n.hostname);
|
|
uint8_t p[250];
|
|
p[0] = kMagic;
|
|
p[1] = kPingRsp;
|
|
p[2] = s_ping_id & 0xFF;
|
|
p[3] = (s_ping_id >> 8) & 0xFF;
|
|
p[4] = (s_ping_id >> 16) & 0xFF;
|
|
p[5] = (s_ping_id >> 24) & 0xFF;
|
|
p[6] = (uint8_t)nl;
|
|
memcpy(p + 7, n.hostname, nl);
|
|
send_to(s_ping_mac, p, 7 + nl);
|
|
s_st.pings++;
|
|
}
|
|
|
|
// ---- receiving (link task) --------------------------------------------------------------------
|
|
|
|
void asm_free(Asm &a)
|
|
{
|
|
free(a.buf);
|
|
a.buf = nullptr;
|
|
a.used = false;
|
|
}
|
|
|
|
Asm *asm_find(const uint8_t mac[6], uint16_t seq)
|
|
{
|
|
for (auto &a : s_asm) {
|
|
if (a.used && a.seq == seq && memcmp(a.mac, mac, 6) == 0) return &a;
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
bool done_has(const uint8_t mac[6], uint16_t seq)
|
|
{
|
|
for (int i = 0; i < s_ndone; i++) {
|
|
if (s_done[i].seq == seq && memcmp(s_done[i].mac, mac, 6) == 0) return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void done_add(const uint8_t mac[6], uint16_t seq)
|
|
{
|
|
if (s_ndone == kDoneMax) {
|
|
memmove(&s_done[0], &s_done[1], sizeof(Done) * (kDoneMax - 1));
|
|
s_ndone--;
|
|
}
|
|
memcpy(s_done[s_ndone].mac, mac, 6);
|
|
s_done[s_ndone].seq = seq;
|
|
s_ndone++;
|
|
}
|
|
|
|
void on_data(const uint8_t mac[6], const uint8_t *data, size_t len, int64_t now)
|
|
{
|
|
if (len <= kHdr) return;
|
|
uint8_t flags = data[2];
|
|
uint16_t seq = (uint16_t)(data[3] | (data[4] << 8));
|
|
uint16_t ver = (uint16_t)(data[5] | (data[6] << 8));
|
|
uint8_t idx = data[7], cnt = data[8];
|
|
if (cnt == 0 || cnt > kMaxFrags || idx >= cnt) return;
|
|
memcpy(s_st.master, mac, 6);
|
|
s_st.master_seen = true;
|
|
uint32_t full = cnt < 32 ? ((1u << cnt) - 1) : 0xFFFFFFFFu;
|
|
if (done_has(mac, seq)) {
|
|
s_st.dups++;
|
|
if (flags & kFlagAck) send_ack(mac, seq, cnt, full); // our earlier ack was lost
|
|
return;
|
|
}
|
|
Asm *a = asm_find(mac, seq);
|
|
if (!a) {
|
|
// a free slot, else the one idle longest
|
|
Asm *pick = nullptr;
|
|
for (auto &s : s_asm) {
|
|
if (!s.used) { pick = &s; break; }
|
|
}
|
|
if (!pick) {
|
|
pick = &s_asm[0];
|
|
for (auto &s : s_asm) {
|
|
if (s.last_us < pick->last_us) pick = &s;
|
|
}
|
|
asm_free(*pick);
|
|
}
|
|
pick->buf = (uint8_t *)malloc((size_t)cnt * kFragMax);
|
|
if (!pick->buf) return;
|
|
pick->used = true;
|
|
memcpy(pick->mac, mac, 6);
|
|
pick->seq = seq;
|
|
pick->cnt = cnt;
|
|
pick->flags = flags;
|
|
pick->ver = ver;
|
|
pick->mask = 0;
|
|
memset(pick->lens, 0, sizeof(pick->lens));
|
|
a = pick;
|
|
}
|
|
size_t frag = len - kHdr;
|
|
if (frag > kFragMax) frag = kFragMax;
|
|
memcpy(a->buf + (size_t)idx * kFragMax, data + kHdr, frag);
|
|
a->lens[idx] = (uint16_t)frag;
|
|
a->mask |= 1u << idx;
|
|
a->last_us = now;
|
|
if (flags & kFlagAck) send_ack(mac, seq, cnt, a->mask);
|
|
if (a->mask != full) return;
|
|
|
|
// complete: the message is the fragments in order
|
|
size_t total = 0;
|
|
for (int i = 0; i < cnt; i++) total += a->lens[i];
|
|
uint8_t *msg = (uint8_t *)malloc(total ? total : 1);
|
|
if (msg) {
|
|
size_t off = 0;
|
|
for (int i = 0; i < cnt; i++) {
|
|
memcpy(msg + off, a->buf + (size_t)i * kFragMax, a->lens[i]);
|
|
off += a->lens[i];
|
|
}
|
|
done_add(mac, seq);
|
|
s_st.msgs++;
|
|
ctrl_handle(mac, msg, total);
|
|
free(msg);
|
|
}
|
|
if ((a->flags & kFlagCommit) && a->ver) {
|
|
s_ver = a->ver;
|
|
s_st.ver = a->ver;
|
|
s_resync_pending = false;
|
|
}
|
|
asm_free(*a);
|
|
}
|
|
|
|
void on_digest(const uint8_t mac[6], const uint8_t *data, size_t len, int64_t now)
|
|
{
|
|
if (len < 4) return;
|
|
memcpy(s_st.master, mac, 6);
|
|
s_st.master_seen = true;
|
|
uint16_t ver = (uint16_t)(data[2] | (data[3] << 8));
|
|
if (ver == 0 || ver == s_ver || s_resync_pending) return;
|
|
if (s_last_resync && now - s_last_resync < kResyncGapUs) return;
|
|
// spread the drivers' resync requests over a few hundred ms
|
|
s_resync_at = now + (int64_t)(esp_random() % 400) * 1000;
|
|
s_resync_pending = true;
|
|
}
|
|
|
|
void handle_frame(const Frame &f, int64_t now)
|
|
{
|
|
if (f.len < 2) return;
|
|
if (f.data[0] == kMagic) {
|
|
switch (f.data[1]) {
|
|
case kData:
|
|
on_data(f.mac, f.data, f.len, now);
|
|
return;
|
|
case kDigest:
|
|
on_digest(f.mac, f.data, f.len, now);
|
|
return;
|
|
case kPingReq:
|
|
if (f.len >= 6) {
|
|
s_ping_id = f.data[2] | (f.data[3] << 8) | (f.data[4] << 16) | ((uint32_t)f.data[5] << 24);
|
|
memcpy(s_ping_mac, f.mac, 6);
|
|
s_ping_at = now + (int64_t)(50 + esp_random() % 451) * 1000; // 50-500 ms, as the other drivers
|
|
s_ping_pending = true;
|
|
}
|
|
return;
|
|
case kAck:
|
|
case kResync:
|
|
case 0x24: // echo: bench only
|
|
case kPingRsp:
|
|
case kAnnounce:
|
|
return; // controller-bound messages, not for a driver
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
ctrl_handle(f.mac, f.data, f.len);
|
|
}
|
|
|
|
// The ESP-NOW receive callback: beats are taken here, anything else is queued
|
|
void on_recv(const esp_now_recv_info_t *info, const uint8_t *data, int len)
|
|
{
|
|
s_st.frames++;
|
|
if (len < 2 || len > 250) return;
|
|
if (data[0] == kMagic && data[1] == kBeat) {
|
|
beat_on_frame(data, (size_t)len);
|
|
s_st.beats++;
|
|
return;
|
|
}
|
|
Frame f;
|
|
memcpy(f.mac, info->src_addr, 6);
|
|
f.len = (uint16_t)len;
|
|
memcpy(f.data, data, len);
|
|
if (xQueueSend(s_inbox, &f, 0) != pdTRUE) {
|
|
s_st.dropped++;
|
|
}
|
|
}
|
|
|
|
// One pass of the link: up to a burst of received frames (waiting up to wait for the first), then the timed sends
|
|
// (announces, resync, ping reply) and expired reassemblies
|
|
void link_pass(TickType_t wait)
|
|
{
|
|
Frame f;
|
|
// at most a burst of frames per pass, then the other tasks run
|
|
if (xQueueReceive(s_inbox, &f, wait) == pdTRUE) {
|
|
for (int burst = 0; burst < 8; burst++) {
|
|
handle_frame(f, esp_timer_get_time());
|
|
if (xQueueReceive(s_inbox, &f, 0) != pdTRUE) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
if (wait) {
|
|
vTaskDelay(1);
|
|
}
|
|
int64_t now = esp_timer_get_time();
|
|
if (s_announce_left > 0 && now >= s_announce_base + kAnnounceUs[3 - s_announce_left]) {
|
|
s_announce_left--;
|
|
send_announce();
|
|
if (s_announce_left == 0) {
|
|
s_next_announce = now + kAnnounceEveryUs;
|
|
}
|
|
} else if (s_announce_left == 0 && now >= s_next_announce) {
|
|
s_next_announce = now + kAnnounceEveryUs;
|
|
send_announce();
|
|
}
|
|
if (s_resync_pending && now >= s_resync_at) {
|
|
s_resync_pending = false;
|
|
send_resync();
|
|
}
|
|
if (s_ping_pending && now >= s_ping_at) {
|
|
s_ping_pending = false;
|
|
send_ping_rsp();
|
|
}
|
|
for (auto &a : s_asm) {
|
|
if (a.used && now - a.last_us > kAsmTimeoutUs) asm_free(a);
|
|
}
|
|
}
|
|
|
|
void link_task(void *arg)
|
|
{
|
|
(void)arg;
|
|
for (;;) {
|
|
link_pass(pdMS_TO_TICKS(5));
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
esp_err_t espnow_link_start(void)
|
|
{
|
|
if (s_started) {
|
|
return ESP_ERR_INVALID_STATE;
|
|
}
|
|
net_cfg_t n;
|
|
settings_load_net(&n);
|
|
if (!n.espnow_enabled) {
|
|
ESP_LOGI(TAG, "ESP-NOW is off in settings");
|
|
return ESP_OK;
|
|
}
|
|
ctrl_init();
|
|
// ESP-NOW uses the radio's channel: the access point's, or the router's when the client is joined.
|
|
// It is not set here, since changing the channel while the client connects breaks the association.
|
|
uint8_t primary = n.espnow_channel;
|
|
wifi_second_chan_t second;
|
|
if (esp_wifi_get_channel(&primary, &second) == ESP_OK) {
|
|
s_st.channel = primary;
|
|
}
|
|
ESP_RETURN_ON_ERROR(esp_now_init(), TAG, "esp_now_init");
|
|
ESP_RETURN_ON_ERROR(esp_now_register_recv_cb(on_recv), TAG, "recv cb");
|
|
esp_now_peer_info_t bcast = {};
|
|
memcpy(bcast.peer_addr, kBroadcast, 6);
|
|
bcast.ifidx = WIFI_IF_STA;
|
|
esp_now_add_peer(&bcast); // it may already exist
|
|
|
|
s_inbox = xQueueCreate(kInboxLen, sizeof(Frame));
|
|
if (!s_inbox) {
|
|
return ESP_ERR_NO_MEM;
|
|
}
|
|
s_announce_left = 3;
|
|
s_announce_base = esp_timer_get_time();
|
|
s_st.running = true;
|
|
s_started = true;
|
|
xTaskCreate(link_task, "espnow_link", 6144, nullptr, 5, nullptr);
|
|
ESP_LOGI(TAG, "ESP-NOW on channel %u", n.espnow_channel);
|
|
return ESP_OK;
|
|
}
|
|
|
|
void espnow_link_poll(void)
|
|
{
|
|
if (s_started) {
|
|
link_pass(0);
|
|
}
|
|
}
|
|
|
|
void espnow_link_stats(espnow_stats_t *out)
|
|
{
|
|
*out = s_st;
|
|
}
|