powerpods/main/esp_now_core.c

#include "esp_now_core.h"
#include "esp_now_proto.h"
#include "esp_err.h"
#include "esp_event.h"
#include "esp_log.h"
#include "esp_mac.h"
#include "esp_netif.h"
#include "esp_now.h"
#include "esp_wifi.h"
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h"
#include <stdio.h>
#include <string.h>

static const char *TAG = "[ESPNOW_CORE]";

static const uint8_t ESPNOW_BCAST[ESP_NOW_ETH_ALEN] = {0xff, 0xff, 0xff,
                                                       0xff, 0xff, 0xff};

static app_config_t s_config;
static uint8_t s_wifi_channel;
static uint8_t s_own_mac[ESP_NOW_ETH_ALEN];
static SemaphoreHandle_t s_send_done;
static SemaphoreHandle_t s_send_lock;
static bool s_send_cb_ready;
static volatile esp_now_send_status_t s_last_send_status;
static volatile bool s_last_send_ok;

#define ESPNOW_SEND_DONE_TIMEOUT_MS 500u
#define ESPNOW_SEND_MAX_ATTEMPTS 8u
#define ESPNOW_SEND_RETRY_DELAY_MS 10u
#define ESPNOW_NOMEM_RETRY_DELAY_MS 50u

#define ESPNOW_RELIABLE_MAX_ATTEMPTS 24u
#define ESPNOW_RELIABLE_NOMEM_DELAY_MS 50u

static uint8_t network_to_channel(uint8_t network) {
  if (network < 1 || network > 13) {
    return 1;
  }
  return network;
}

static void espnow_send_done_cb(const esp_now_send_info_t *tx_info,
                                esp_now_send_status_t status) {
  (void)tx_info;
  s_last_send_status = status;
  s_last_send_ok = (status == ESP_NOW_SEND_SUCCESS);
  if (s_send_done != NULL) {
    xSemaphoreGive(s_send_done);
  }
}

void esp_now_core_store_config(const app_config_t *config) {
  if (config == NULL) {
    return;
  }
  memset(&s_config, 0, sizeof(s_config));
  memcpy(&s_config, config, sizeof(s_config));
  s_wifi_channel = network_to_channel(config->network);
}

const app_config_t *esp_now_core_get_config(void) { return &s_config; }

bool esp_now_core_is_master(void) { return s_config.master; }

uint8_t esp_now_core_network(void) { return s_config.network; }

uint8_t esp_now_core_wifi_channel(void) { return s_wifi_channel; }

const uint8_t *esp_now_core_own_mac(void) { return s_own_mac; }

uint32_t esp_now_core_now_ms(void) {
  return (uint32_t)(xTaskGetTickCount() * portTICK_PERIOD_MS);
}

bool esp_now_core_mac_equal(const uint8_t *a, const uint8_t *b) {
  return memcmp(a, b, ESP_NOW_ETH_ALEN) == 0;
}

void esp_now_core_mac_to_str(const uint8_t *mac, char *out, size_t out_len) {
  snprintf(out, out_len, "%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1],
           mac[2], mac[3], mac[4], mac[5]);
}

esp_err_t esp_now_core_ensure_peer(const uint8_t *mac) {
  if (esp_now_is_peer_exist(mac)) {
    return ESP_OK;
  }

  esp_now_peer_info_t peer = {0};
  memcpy(peer.peer_addr, mac, ESP_NOW_ETH_ALEN);
  peer.channel = s_wifi_channel;
  peer.ifidx = WIFI_IF_STA;
  peer.encrypt = false;

  esp_err_t err = esp_now_add_peer(&peer);
  if (err != ESP_OK) {
    ESP_LOGW(TAG, "add peer failed: %s", esp_err_to_name(err));
  }
  return err;
}

esp_err_t esp_now_core_ensure_broadcast_peer(void) {
  return esp_now_core_ensure_peer(ESPNOW_BCAST);
}

static esp_err_t send_with_backpressure(const uint8_t *dest_mac,
                                        const alox_EspNowMessage *msg,
                                        uint32_t max_attempts,
                                        uint32_t nomem_delay_ms,
                                        uint32_t retry_delay_ms,
                                        bool quiet_retries) {
  if (s_send_lock == NULL ||
      xSemaphoreTake(s_send_lock, pdMS_TO_TICKS(5000)) != pdTRUE) {
    return ESP_ERR_TIMEOUT;
  }

  uint8_t buf[ESPNOW_PB_MAX_SIZE];
  size_t len = 0;
  esp_err_t result = ESP_FAIL;

  esp_err_t err = esp_now_proto_encode(msg, buf, sizeof(buf), &len);
  if (err != ESP_OK) {
    ESP_LOGW(TAG, "encode failed");
    result = err;
    goto out;
  }

  if (len > ESP_NOW_MAX_DATA_LEN) {
    ESP_LOGW(TAG, "encoded len %u > ESP-NOW max %u", (unsigned)len,
             (unsigned)ESP_NOW_MAX_DATA_LEN);
    result = ESP_ERR_INVALID_SIZE;
    goto out;
  }

  if (esp_now_core_ensure_peer(dest_mac) != ESP_OK) {
    result = ESP_FAIL;
    goto out;
  }

  for (uint32_t attempt = 0; attempt < max_attempts; attempt++) {
    if (s_send_cb_ready && s_send_done != NULL) {
      xSemaphoreTake(s_send_done, 0);
    }
    s_last_send_ok = false;

    err = esp_now_send(dest_mac, buf, len);
    if (err != ESP_OK) {
      const bool last = (attempt + 1 >= max_attempts);
      if (!quiet_retries || last) {
        ESP_LOGW(TAG, "send type=%u failed (attempt %lu/%lu): %s",
                 (unsigned)msg->type, (unsigned long)(attempt + 1),
                 (unsigned long)max_attempts, esp_err_to_name(err));
      }
      vTaskDelay(pdMS_TO_TICKS(err == ESP_ERR_ESPNOW_NO_MEM ? nomem_delay_ms
                                                             : retry_delay_ms));
      continue;
    }

    if (s_send_cb_ready && s_send_done != NULL) {
      if (xSemaphoreTake(s_send_done, pdMS_TO_TICKS(ESPNOW_SEND_DONE_TIMEOUT_MS)) !=
          pdTRUE) {
        const bool last = (attempt + 1 >= max_attempts);
        if (!quiet_retries || last) {
          ESP_LOGW(TAG, "send type=%u done timeout (attempt %lu/%lu)",
                   (unsigned)msg->type, (unsigned long)(attempt + 1),
                   (unsigned long)max_attempts);
        }
        vTaskDelay(pdMS_TO_TICKS(retry_delay_ms));
        continue;
      }
      if (!s_last_send_ok) {
        const bool last = (attempt + 1 >= max_attempts);
        if (!quiet_retries || last) {
          ESP_LOGW(TAG, "send type=%u peer status=%d (attempt %lu/%lu)",
                   (unsigned)msg->type, (int)s_last_send_status,
                   (unsigned long)(attempt + 1), (unsigned long)max_attempts);
        }
        vTaskDelay(pdMS_TO_TICKS(retry_delay_ms));
        continue;
      }
    }
    result = ESP_OK;
    goto out;
  }

out:
  xSemaphoreGive(s_send_lock);
  return result;
}

esp_err_t esp_now_core_send_wait(const uint8_t *dest_mac,
                                 const alox_EspNowMessage *msg) {
  return send_with_backpressure(dest_mac, msg, ESPNOW_SEND_MAX_ATTEMPTS,
                                ESPNOW_NOMEM_RETRY_DELAY_MS,
                                ESPNOW_SEND_RETRY_DELAY_MS, false);
}

esp_err_t esp_now_core_send_reliable(const uint8_t *dest_mac,
                                     const alox_EspNowMessage *msg) {
  return send_with_backpressure(dest_mac, msg, ESPNOW_RELIABLE_MAX_ATTEMPTS,
                                ESPNOW_RELIABLE_NOMEM_DELAY_MS,
                                ESPNOW_SEND_RETRY_DELAY_MS, true);
}

esp_err_t esp_now_core_send_fast(const uint8_t *dest_mac,
                                 const alox_EspNowMessage *msg) {
  if (s_send_lock == NULL ||
      xSemaphoreTake(s_send_lock, pdMS_TO_TICKS(5000)) != pdTRUE) {
    return ESP_ERR_TIMEOUT;
  }

  uint8_t buf[ESPNOW_PB_MAX_SIZE];
  size_t len = 0;

  esp_err_t err = esp_now_proto_encode(msg, buf, sizeof(buf), &len);
  if (err != ESP_OK) {
    ESP_LOGW(TAG, "encode failed");
    xSemaphoreGive(s_send_lock);
    return err;
  }

  if (len > ESP_NOW_MAX_DATA_LEN) {
    ESP_LOGW(TAG, "encoded len %u > ESP-NOW max %u", (unsigned)len,
             (unsigned)ESP_NOW_MAX_DATA_LEN);
    xSemaphoreGive(s_send_lock);
    return ESP_ERR_INVALID_SIZE;
  }

  if (esp_now_core_ensure_peer(dest_mac) != ESP_OK) {
    xSemaphoreGive(s_send_lock);
    return ESP_FAIL;
  }

  err = esp_now_send(dest_mac, buf, len);
  if (err != ESP_OK) {
    ESP_LOGW(TAG, "send type=%u failed: %s", (unsigned)msg->type,
             esp_err_to_name(err));
  }
  xSemaphoreGive(s_send_lock);
  return err;
}

esp_err_t esp_now_core_send(const uint8_t *dest_mac,
                            const alox_EspNowMessage *msg) {
  return esp_now_core_send_wait(dest_mac, msg);
}

esp_err_t esp_now_core_init_radio(uint8_t channel) {
  ESP_ERROR_CHECK(esp_netif_init());
  ESP_ERROR_CHECK(esp_event_loop_create_default());

  wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
  ESP_ERROR_CHECK(esp_wifi_init(&cfg));

  wifi_config_t wifi_config = {0};
  wifi_config.sta.channel = channel;
  wifi_config.sta.scan_method = WIFI_ALL_CHANNEL_SCAN;
  wifi_config.sta.sort_method = WIFI_CONNECT_AP_BY_SIGNAL;

  ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
  ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
  ESP_ERROR_CHECK(esp_wifi_start());
  ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));
  ESP_ERROR_CHECK(esp_wifi_set_channel(channel, WIFI_SECOND_CHAN_NONE));

  ESP_ERROR_CHECK(esp_read_mac(s_own_mac, ESP_MAC_WIFI_STA));
  s_wifi_channel = channel;
  return ESP_OK;
}

void esp_now_core_init_send_done(void) {
  s_send_done = xSemaphoreCreateBinary();
  s_send_lock = xSemaphoreCreateMutex();
  if (s_send_done != NULL && s_send_lock != NULL &&
      esp_now_register_send_cb(espnow_send_done_cb) == ESP_OK) {
    s_send_cb_ready = true;
  } else {
    ESP_LOGW(TAG, "send-done callback unavailable (OTA may drop packets)");
  }
}