/* * 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 #include #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; }