209 lines
7.3 KiB
C
209 lines
7.3 KiB
C
/**
|
|
* @file c6_sync_espnow.c
|
|
* @brief ESP-NOW cross-node time-sync — ADR-110 D1 workaround.
|
|
*
|
|
* Same protocol as c6_timesync.c (TS_BEACON every 100 ms with leader epoch),
|
|
* but over ESP-NOW instead of 802.15.4 because the IDF v5.4 ieee802154 RX
|
|
* path doesn't deliver frames to user-space (see WITNESS-LOG-110 §D1).
|
|
*
|
|
* Frame layout (16 bytes payload, broadcast MAC FF:FF:FF:FF:FF:FF):
|
|
* [0..3] Magic 0x53454E50 ('SENP' — Sync via ESP-NOW)
|
|
* [4] Protocol ver 0x01
|
|
* [5] Leader flag 1 if sender claims leader
|
|
* [6..7] Reserved
|
|
* [8..15] Leader epoch µs (LE u64)
|
|
*/
|
|
|
|
#include "sdkconfig.h"
|
|
#include "c6_sync_espnow.h"
|
|
#include "esp_log.h"
|
|
#include "esp_now.h"
|
|
#include "esp_wifi.h"
|
|
#include "esp_mac.h"
|
|
#include "esp_timer.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/timers.h"
|
|
#include <string.h>
|
|
|
|
static const char *TAG = "c6_espnow";
|
|
|
|
#define BEACON_MAGIC 0x53454E50u /* 'SENP' little-endian */
|
|
#define BEACON_PROTO_VER 0x01
|
|
#define BEACON_PERIOD_MS 100
|
|
#define VALID_WINDOW_MS 3000
|
|
|
|
typedef struct __attribute__((packed)) {
|
|
uint32_t magic;
|
|
uint8_t proto_ver;
|
|
uint8_t leader_flag;
|
|
uint16_t _reserved;
|
|
uint64_t leader_epoch_us;
|
|
} espnow_beacon_t;
|
|
|
|
static const uint8_t s_broadcast_mac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
|
|
|
|
static uint64_t s_local_id = 0; /* 6-byte MAC packed into u64 */
|
|
static uint64_t s_leader_id = 0;
|
|
static int64_t s_offset_us = 0;
|
|
static uint64_t s_last_seen_us = 0;
|
|
static bool s_is_leader = false;
|
|
static TimerHandle_t s_beacon_timer = NULL;
|
|
|
|
static uint32_t s_tx_count = 0;
|
|
static uint32_t s_tx_fail = 0;
|
|
static uint32_t s_rx_count = 0;
|
|
static uint32_t s_rx_magic_match = 0;
|
|
|
|
static uint64_t mac6_to_u64(const uint8_t mac[6])
|
|
{
|
|
return ((uint64_t)mac[0] << 40) | ((uint64_t)mac[1] << 32) |
|
|
((uint64_t)mac[2] << 24) | ((uint64_t)mac[3] << 16) |
|
|
((uint64_t)mac[4] << 8) | (uint64_t)mac[5];
|
|
}
|
|
|
|
static void send_beacon(void)
|
|
{
|
|
espnow_beacon_t b = {
|
|
.magic = BEACON_MAGIC,
|
|
.proto_ver = BEACON_PROTO_VER,
|
|
.leader_flag = s_is_leader ? 1 : 0,
|
|
._reserved = 0,
|
|
.leader_epoch_us = (uint64_t)esp_timer_get_time(),
|
|
};
|
|
esp_err_t r = esp_now_send(s_broadcast_mac, (uint8_t *)&b, sizeof(b));
|
|
s_tx_count++;
|
|
if (r != ESP_OK) s_tx_fail++;
|
|
/* Diag log every 50 beacons. */
|
|
if ((s_tx_count % 50) == 1) {
|
|
ESP_LOGI(TAG, "tx#%lu (fail=%lu) rx#%lu (match=%lu) leader=%d offset_us=%lld",
|
|
(unsigned long)s_tx_count, (unsigned long)s_tx_fail,
|
|
(unsigned long)s_rx_count, (unsigned long)s_rx_magic_match,
|
|
(int)s_is_leader, (long long)s_offset_us);
|
|
}
|
|
}
|
|
|
|
/* IDF v5.4 ESP-NOW recv callback signature uses esp_now_recv_info_t.
|
|
* Falls back to the older signature on older IDF via ifdef. */
|
|
#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
|
|
static void on_recv(const esp_now_recv_info_t *info,
|
|
const uint8_t *data, int len)
|
|
{
|
|
const uint8_t *src_mac = info ? info->src_addr : NULL;
|
|
#else
|
|
static void on_recv(const uint8_t *src_mac, const uint8_t *data, int len)
|
|
{
|
|
#endif
|
|
s_rx_count++;
|
|
if (data == NULL || len < (int)sizeof(espnow_beacon_t)) return;
|
|
const espnow_beacon_t *b = (const espnow_beacon_t *)data;
|
|
if (b->magic != BEACON_MAGIC || b->proto_ver != BEACON_PROTO_VER) return;
|
|
s_rx_magic_match++;
|
|
uint64_t sender_id = src_mac ? mac6_to_u64(src_mac) : 0;
|
|
uint64_t now_us = (uint64_t)esp_timer_get_time();
|
|
|
|
/* Adopt sender as leader if it's claiming leadership AND its ID is
|
|
* lower than our current leader (or we have no leader). Lowest MAC
|
|
* wins — deterministic. */
|
|
if (b->leader_flag && (s_leader_id == 0 || sender_id < s_leader_id)) {
|
|
if (s_is_leader && sender_id < s_local_id) {
|
|
ESP_LOGI(TAG, "stepping down: heard lower-id leader %012llx (we are %012llx)",
|
|
(unsigned long long)sender_id, (unsigned long long)s_local_id);
|
|
s_is_leader = false;
|
|
}
|
|
s_leader_id = sender_id;
|
|
}
|
|
|
|
/* If accepted leader, compute offset from their epoch (only for non-leader). */
|
|
if (b->leader_flag && !s_is_leader && sender_id == s_leader_id) {
|
|
s_offset_us = (int64_t)b->leader_epoch_us - (int64_t)now_us;
|
|
s_last_seen_us = now_us;
|
|
}
|
|
}
|
|
|
|
static void on_send(const uint8_t *mac, esp_now_send_status_t status)
|
|
{
|
|
(void)mac;
|
|
if (status != ESP_NOW_SEND_SUCCESS) s_tx_fail++;
|
|
}
|
|
|
|
static void beacon_timer_cb(TimerHandle_t t)
|
|
{
|
|
(void)t;
|
|
uint64_t now = (uint64_t)esp_timer_get_time();
|
|
/* Promote self if no leader beacon for VALID_WINDOW_MS and we have lowest known id. */
|
|
if (!s_is_leader && (now - s_last_seen_us) > (VALID_WINDOW_MS * 1000ULL)) {
|
|
if (s_leader_id == 0 || s_local_id < s_leader_id) {
|
|
s_is_leader = true;
|
|
s_leader_id = s_local_id;
|
|
s_offset_us = 0;
|
|
ESP_LOGI(TAG, "promoting self to leader (no beacons for %u ms; local_id=%012llx)",
|
|
(unsigned)VALID_WINDOW_MS, (unsigned long long)s_local_id);
|
|
}
|
|
}
|
|
send_beacon();
|
|
}
|
|
|
|
esp_err_t c6_sync_espnow_init(void)
|
|
{
|
|
uint8_t mac[6];
|
|
esp_read_mac(mac, ESP_MAC_WIFI_STA);
|
|
s_local_id = mac6_to_u64(mac);
|
|
|
|
esp_err_t r = esp_now_init();
|
|
if (r != ESP_OK) {
|
|
ESP_LOGE(TAG, "esp_now_init failed: %s", esp_err_to_name(r));
|
|
return r;
|
|
}
|
|
esp_now_register_recv_cb(on_recv);
|
|
esp_now_register_send_cb(on_send);
|
|
|
|
/* Add broadcast peer so esp_now_send to FF:FF:FF:FF:FF:FF works. */
|
|
esp_now_peer_info_t peer = {0};
|
|
memcpy(peer.peer_addr, s_broadcast_mac, 6);
|
|
peer.channel = 0; /* current STA channel */
|
|
peer.ifidx = WIFI_IF_STA;
|
|
peer.encrypt = false;
|
|
r = esp_now_add_peer(&peer);
|
|
if (r != ESP_OK && r != ESP_ERR_ESPNOW_EXIST) {
|
|
ESP_LOGW(TAG, "esp_now_add_peer(broadcast) failed: %s", esp_err_to_name(r));
|
|
}
|
|
|
|
/* Start as candidate leader — will step down on receiving lower-id beacon. */
|
|
s_is_leader = true;
|
|
s_leader_id = s_local_id;
|
|
s_last_seen_us = (uint64_t)esp_timer_get_time();
|
|
|
|
s_beacon_timer = xTimerCreate("c6_espnow_beacon",
|
|
pdMS_TO_TICKS(BEACON_PERIOD_MS),
|
|
pdTRUE, NULL, beacon_timer_cb);
|
|
if (s_beacon_timer == NULL) {
|
|
ESP_LOGE(TAG, "xTimerCreate failed");
|
|
return ESP_ERR_NO_MEM;
|
|
}
|
|
xTimerStart(s_beacon_timer, 0);
|
|
|
|
ESP_LOGI(TAG, "init done: local_id=%012llx leader=yes(candidate) period=%ums",
|
|
(unsigned long long)s_local_id, (unsigned)BEACON_PERIOD_MS);
|
|
return ESP_OK;
|
|
}
|
|
|
|
uint64_t c6_sync_espnow_get_epoch_us(void)
|
|
{
|
|
return (uint64_t)((int64_t)esp_timer_get_time() + s_offset_us);
|
|
}
|
|
|
|
bool c6_sync_espnow_is_leader(void) { return s_is_leader; }
|
|
int64_t c6_sync_espnow_get_offset_us(void) { return s_offset_us; }
|
|
|
|
bool c6_sync_espnow_is_valid(void)
|
|
{
|
|
if (s_is_leader) return true;
|
|
uint64_t now = (uint64_t)esp_timer_get_time();
|
|
return (now - s_last_seen_us) < (VALID_WINDOW_MS * 1000ULL);
|
|
}
|
|
|
|
uint32_t c6_sync_espnow_tx_count(void) { return s_tx_count; }
|
|
uint32_t c6_sync_espnow_tx_fail(void) { return s_tx_fail; }
|
|
uint32_t c6_sync_espnow_rx_count(void) { return s_rx_count; }
|
|
uint32_t c6_sync_espnow_rx_magic_match(void) { return s_rx_magic_match; }
|