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 bool s_send_cb_ready;

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;
  (void)status;
  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);
}

esp_err_t esp_now_core_send_wait(const uint8_t *dest_mac,
                                 const alox_EspNowMessage *msg) {
  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");
    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);
    return ESP_ERR_INVALID_SIZE;
  }

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

  if (s_send_cb_ready && s_send_done != NULL) {
    xSemaphoreTake(s_send_done, 0);
  }

  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));
    return err;
  }

  if (s_send_cb_ready && s_send_done != NULL) {
    if (xSemaphoreTake(s_send_done, pdMS_TO_TICKS(50)) != pdTRUE) {
      ESP_LOGW(TAG, "send type=%u done timeout", (unsigned)msg->type);
    }
  }
  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();
  if (s_send_done != 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)");
  }
}