espnow.c

// Copyright 2021 Espressif Systems (Shanghai) PTE LTD
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at

//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/param.h>

#include "freertos/FreeRTOS.h"
#include "freertos/event_groups.h"
#include "freertos/task.h"

#include "esp_wifi.h"
#include "esp_now.h"
#include "esp_log.h"
#include "espnow.h"
#include "esp_crc.h"

#define SEND_CB_OK                      BIT0
#define SEND_CB_FAIL                    BIT1

#define ESPNOW_MSG_CACHE                32

#ifndef CONFIG_ESPNOW_VERSION
#define ESPNOW_VERSION                  2
#else
#define ESPNOW_VERSION                  CONFIG_ESPNOW_VERSION
#endif

/* Event source task related definitions */
ESP_EVENT_DEFINE_BASE(ESP_EVENT_ESPNOW);

typedef struct {
    uint8_t type;
    uint8_t group_id[6];
    uint8_t addrs_num;
    uint8_t addrs_list[0][6];
} espnow_group_info_t;
typedef struct {
    uint16_t frame_head;
    uint16_t duration;
    uint8_t destination_address[6];
    uint8_t source_address[6];
    uint8_t broadcast_address[6];
    uint16_t sequence_control;

    uint8_t category_code;
    uint8_t organization_identifier[3]; // 0x18fe34
    uint8_t random_values[4];
    struct {
        uint8_t element_id;                 // 0xdd
        uint8_t lenght;                     //
        uint8_t organization_identifier[3]; // 0x18fe34
        uint8_t type;                       // 4
        uint8_t version;
        uint8_t body[0];
    } vendor_specific_content;
} __attribute__((packed)) espnow_frame_format_t;

typedef struct {
    uint8_t type    : 4;
    uint8_t version : 2;
    uint8_t         : 2;
    uint8_t size;
    espnow_frame_head_t frame_head;
    uint8_t dest_addr[6];
    uint8_t src_addr[6];
    uint8_t payload[0];
} __attribute__((packed)) espnow_data_t;

/**
 * @brief Receive data packet temporarily store in queue
 */
typedef struct {
    wifi_pkt_rx_ctrl_t rx_ctrl; /**< metadata header */
    espnow_data_t data;
} espnow_pkt_t;

static const char *TAG                  = "espnow";
static bool g_set_channel_flag          = true;
static espnow_config_t *g_espnow_config = NULL;
static EventGroupHandle_t g_event_group = NULL;
static xQueueHandle g_espnow_queue[ESPNOW_TYPE_MAX] = {NULL};
static struct {
    uint8_t type;
    uint16_t magic;
} __attribute__((packed)) g_msg_magic_cache[ESPNOW_MSG_CACHE] = {0};

static espnow_addr_t ESPNOW_ADDR_SELF       = {0};
const espnow_addr_t ESPNOW_ADDR_NONE        = {0};
const espnow_addr_t ESPNOW_ADDR_BROADCAST   = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0XFF};
const espnow_group_t ESPNOW_ADDR_GROUP_OTA  =  {'O', 'T', 'A', 0x0, 0x0, 0x0};
const espnow_group_t ESPNOW_ADDR_GROUP_PROV = {'P', 'R', 'O', 'V', 0x0, 0x0};
const espnow_group_t ESPNOW_ADDR_GROUP_SEC = {'S', 'E', 'C', 0x0, 0x0, 0x0};
static uint8_t g_msg_magic_cache_next = 0;
static espnow_frame_head_t g_espnow_frame_head_default = ESPNOW_FRAME_CONFIG_DEFAULT();

static wifi_country_t g_self_country = {0};
static SemaphoreHandle_t g_send_lock = NULL;

static bool queue_over_write(QueueHandle_t xQueue, const void *const pvItemToQueue, TickType_t xTicksToWait)
{
    if (!xQueue) {
        return false;
    }

    if (!uxQueueSpacesAvailable(xQueue)) {
        void *q_data = NULL;

        if (xQueueReceive(xQueue, &q_data,  0)) {
            ESP_FREE(q_data);
        }

        ESP_LOGW(TAG, "[%s, %d] espnow_queue is full", __func__, __LINE__);
    }

    return xQueueSend(xQueue, pvItemToQueue, xTicksToWait);
}

/**< callback function of receiving ESPNOW data */
void espnow_recv_cb(const uint8_t *addr, const uint8_t *data, int size)
{
    espnow_data_t *espnow_data = (espnow_data_t *)data;
    wifi_promiscuous_pkt_t *promiscuous_pkt = (wifi_promiscuous_pkt_t *)(data - sizeof(wifi_pkt_rx_ctrl_t) - sizeof(espnow_frame_format_t));
    wifi_pkt_rx_ctrl_t *rx_ctrl = &promiscuous_pkt->rx_ctrl;

    ESP_LOG_BUFFER_HEXDUMP(TAG, data, size, ESP_LOG_DEBUG);
    ESP_LOGD(TAG, "[%s, %d], " MACSTR ", rssi: %d, size: %d, total: %d - %d, type: %d, addr: %02x, g_msg_magic_cache_next: %d",
             __func__, __LINE__, MAC2STR(addr), rx_ctrl->rssi, size, espnow_data->size, sizeof(espnow_data_t), espnow_data->type, addr[5], g_msg_magic_cache_next);

    /**< Filter ESP-NOW packets not generated by this project */
    if (espnow_data->version != ESPNOW_VERSION || size != espnow_data->size + sizeof(espnow_data_t)
            || ESPNOW_ADDR_IS_SELF(espnow_data->src_addr)) {
        ESP_LOGD(TAG, "Receive cb args error, recv_addr: "MACSTR", src_addr: " MACSTR ", data: %p, size: %d",
                 MAC2STR(addr), MAC2STR(espnow_data->src_addr), data, size);
        return ;
    }

    espnow_frame_head_t *frame_head = &espnow_data->frame_head;

    /**< Data does not need to be forwarded */
    if (!g_espnow_queue[espnow_data->type]
            && (!g_espnow_config->forward_enable || !frame_head->broadcast || !frame_head->forward_ttl)) {
        return ;
    }

    /**< Channel filtering */
    if (frame_head->filter_adjacent_channel && frame_head->channel != rx_ctrl->channel) {
        ESP_LOGD(TAG, "Filter adjacent channels, %d != %d", frame_head->channel, rx_ctrl->channel);
        return ;
    }

    /**< Rssi filtering */
    if (frame_head->filter_weak_signal && frame_head->forward_rssi > rx_ctrl->rssi) {
        ESP_LOGD(TAG, "Filter weak signal strength, %d > %d", frame_head->forward_rssi, rx_ctrl->rssi);
        return ;
    }

    for (size_t i = 0, index = g_msg_magic_cache_next; i < ESPNOW_MSG_CACHE;
            i++, index = (g_msg_magic_cache_next + i) % ESPNOW_MSG_CACHE) {
        if (g_msg_magic_cache[index].type == espnow_data->type
                && g_msg_magic_cache[index].magic == frame_head->magic) {
            return ;
        }
    }

    ESP_LOGD(TAG, "[%s, %d]: " MACSTR ", rssi: %d, channel: %d/%d, size: %d, %s, magic: 0x%x, ack: %d",
             __func__, __LINE__, MAC2STR(espnow_data->dest_addr), rx_ctrl->rssi, rx_ctrl->channel,
             rx_ctrl->secondary_channel, espnow_data->size, espnow_data->payload,
             frame_head->magic, espnow_data->frame_head.ack);

    if (espnow_data->type == ESPNOW_TYPE_ACK && ESPNOW_ADDR_IS_SELF(espnow_data->dest_addr)) {
        ESP_LOGD(TAG, ">[%s, %d]: broadcast: %d, dest_addr: " MACSTR, __func__, __LINE__, frame_head->broadcast,
                 MAC2STR(espnow_data->dest_addr));

        uint32_t *magic = ESP_MALLOC(sizeof(uint32_t));
        *magic = frame_head->magic;

        if (queue_over_write(g_espnow_queue[ESPNOW_TYPE_ACK], &magic, 0) != pdPASS) {
            ESP_LOGW(TAG, "[%s, %d] Send receive queue failed", __func__, __LINE__);
            ESP_FREE(magic);
            return ;
        };

        goto EXIT;
    }

    if (espnow_data->type == ESPNOW_TYPE_GROUP) {
        espnow_group_info_t *group_info = (espnow_group_info_t *) espnow_data->payload;
        bool set_group_flag = false;

        ESP_LOGD(TAG, ">1.2< group_id: " MACSTR ", dest_addr: " MACSTR,
                 MAC2STR(group_info->group_id), MAC2STR(espnow_data->dest_addr));
        ESP_LOGD(TAG, ">1.2< addr_mun: %d, dest_addr: " MACSTR, group_info->addrs_num,
                 MAC2STR(group_info->addrs_list[0]));

        if (group_info->addrs_num == 1 && ESPNOW_ADDR_IS_BROADCAST(group_info->addrs_list[0])) {
            set_group_flag = true;
        } else {
            for (size_t i = 0; i < group_info->addrs_num; i++) {
                if (ESPNOW_ADDR_IS_SELF(group_info->addrs_list[i])) {
                    set_group_flag = true;
                    break;
                }
            }
        }

        if (set_group_flag) {
            if (group_info->type) {
                espnow_add_group(group_info->group_id);
            } else {
                espnow_del_group(group_info->group_id);
            }
        }
    } else if (g_espnow_queue[espnow_data->type]) {
        if (!frame_head->group && frame_head->broadcast && !ESPNOW_ADDR_IS_BROADCAST(espnow_data->dest_addr)
                && !ESPNOW_ADDR_IS_SELF(espnow_data->dest_addr)) {
            goto FORWARD_DATA;
        } else if (frame_head->group && !espnow_is_my_group(espnow_data->dest_addr)) {
            ESP_LOGD(TAG, "[%s, %d]: group_num: %d, group_id: " MACSTR,
                     __func__, __LINE__, espnow_get_group_num(), MAC2STR(espnow_data->dest_addr));
            goto FORWARD_DATA;
        }

        espnow_pkt_t *q_data = ESP_MALLOC(sizeof(espnow_pkt_t) + espnow_data->size);
        memcpy(&q_data->rx_ctrl, rx_ctrl, sizeof(espnow_pkt_t));
        memcpy(&q_data->data, espnow_data, size);

        if (frame_head->channel && frame_head->channel != ESPNOW_CHANNEL_ALL) {
            q_data->rx_ctrl.channel = frame_head->channel;
        }

        if (queue_over_write(g_espnow_queue[espnow_data->type], &q_data, 0) != pdPASS) {
            ESP_LOGW(TAG, "[%s, %d] Send receive queue failed", __func__, __LINE__);
            ESP_FREE(q_data);
            return ;
        }
    }

FORWARD_DATA:

    if (g_espnow_config->forward_enable && frame_head->forward_ttl > 0 && frame_head->broadcast
            && frame_head->forward_rssi >= rx_ctrl->rssi && !ESPNOW_ADDR_IS_SELF(espnow_data->dest_addr)
            && !ESPNOW_ADDR_IS_SELF(espnow_data->src_addr)) {
        espnow_data_t *q_data = ESP_MALLOC(size);

        if (!q_data) {
            return ;
        }

        memcpy(q_data, espnow_data, size);

        if (frame_head->forward_ttl != ESPNOW_FORWARD_MAX_COUNT) {
            q_data->frame_head.forward_ttl--;
        }

        if (queue_over_write(g_espnow_queue[ESPNOW_TYPE_FORWARD], &q_data, 0) != pdPASS) {
            ESP_LOGW(TAG, "[%s, %d] Send receive queue failed", __func__, __LINE__);
            ESP_FREE(q_data);
            return ;
        }
    }

EXIT:
    g_msg_magic_cache_next = (g_msg_magic_cache_next + 1) % ESPNOW_MSG_CACHE;
    g_msg_magic_cache[g_msg_magic_cache_next].type  = espnow_data->type;
    g_msg_magic_cache[g_msg_magic_cache_next].magic = frame_head->magic;
}

/**< callback function of sending ESPNOW data */
void espnow_send_cb(const uint8_t *addr, esp_now_send_status_t status)
{
    if (!addr || !g_event_group) {
        ESP_LOGW(TAG, "Send cb args error, addr is NULL");
        return ;
    }

    if (status == ESP_NOW_SEND_SUCCESS) {
        xEventGroupSetBits(g_event_group, SEND_CB_OK);
    } else {
        xEventGroupSetBits(g_event_group, SEND_CB_FAIL);
    }
}

esp_err_t espnow_add_peer(const uint8_t *addr, const uint8_t *lmk)
{
    ESP_PARAM_CHECK(addr);

    /**< If peer exists, delete a peer from peer list */
    if (esp_now_is_peer_exist(addr)) {
        return ESP_OK;
    }

    esp_err_t ret = ESP_OK;
    esp_now_peer_info_t peer = {
        .ifidx = WIFI_IF_STA,
    };

    if (lmk) {
        peer.encrypt = true;
        memcpy(peer.lmk, lmk, ESP_NOW_KEY_LEN);
    }

    memcpy(peer.peer_addr, addr, 6);

    /**< Add a peer to peer list */
    ret = esp_now_add_peer(&peer);
    ESP_ERROR_RETURN(ret != ESP_OK, ret, "Add a peer to peer list fail");

    return ESP_OK;
}

esp_err_t espnow_del_peer(const uint8_t *addr)
{
    ESP_PARAM_CHECK(addr);

    esp_err_t ret = ESP_OK;

    /**< If peer exists, delete a peer from peer list */
    if (esp_now_is_peer_exist(addr) && !ESPNOW_ADDR_IS_BROADCAST(addr)) {
        ret = esp_now_del_peer(addr);
        ESP_ERROR_RETURN(ret != ESP_OK, ret, "esp_now_del_peer fail, ret: %d", ret);
    }

    return ESP_OK;
}

esp_err_t espnow_send(espnow_type_t type, const uint8_t *dest_addr, const void *data,
                      size_t size, const espnow_frame_head_t *data_head, TickType_t wait_ticks)
{
    ESP_PARAM_CHECK(dest_addr);
    ESP_PARAM_CHECK(data);
    ESP_PARAM_CHECK(type < ESPNOW_TYPE_MAX);
    ESP_PARAM_CHECK(size <= ESPNOW_DATA_LEN);
    ESP_ERROR_RETURN(!g_espnow_config, ESP_ERR_ESPNOW_NOT_INIT, "ESPNOW is not initialized");

    esp_err_t ret             = ESP_FAIL;
    TickType_t write_ticks    = 0;
    uint32_t start_ticks      = xTaskGetTickCount();
    uint8_t primary           = 0;
    wifi_second_chan_t second = 0;
    espnow_frame_head_t *frame_head = NULL;

    espnow_data_t *espnow_data = ESP_MALLOC(sizeof(espnow_data_t) + size);
    espnow_data->version = ESPNOW_VERSION;
    espnow_data->type = type;
    espnow_data->size = size;
    memcpy(espnow_data->dest_addr, dest_addr, 6);
    memcpy(espnow_data->src_addr, ESPNOW_ADDR_SELF, 6);
    memcpy(espnow_data->payload, data, size);

    if (data_head) {
        memcpy(&espnow_data->frame_head, data_head, sizeof(espnow_frame_head_t));
    } else {
        memcpy(&espnow_data->frame_head, &g_espnow_frame_head_default, sizeof(espnow_frame_head_t));
    }

    frame_head = &espnow_data->frame_head;

    if (!frame_head->magic) {
        frame_head->magic = esp_random();
    }

    if (!frame_head->broadcast && ESPNOW_ADDR_IS_BROADCAST(dest_addr)) {
        frame_head->broadcast = true;
    }

    if (frame_head->retransmit_count == 0) {
        frame_head->retransmit_count = 1;
    }

    ESP_LOGD(TAG, "[%s, %d] addr: " MACSTR", size: %d, count: %d, rssi: %d, data: %s, magic: 0x%x",
             __func__, __LINE__, MAC2STR(dest_addr), espnow_data->size, frame_head->retransmit_count,
             frame_head->forward_rssi, espnow_data->payload, frame_head->magic);

    /**< Wait for other tasks to be sent before send ESP-NOW data */
    if (xSemaphoreTake(g_send_lock, pdMS_TO_TICKS(wait_ticks)) != pdPASS) {
        ESP_FREE(espnow_data);
        return ESP_ERR_TIMEOUT;
    }

    if (g_set_channel_flag && frame_head->channel == ESPNOW_CHANNEL_ALL) {
        ret = esp_wifi_get_channel(&primary, &second);
        ESP_ERROR_GOTO(ret != ESP_OK, EXIT, "esp_wifi_get_channel, err_name: %s", esp_err_to_name(ret));
    } else if (frame_head->channel == 0) {
        ret = esp_wifi_get_channel(&primary, &second);
        ESP_ERROR_GOTO(ret != ESP_OK, EXIT, "esp_wifi_get_channel, err_name: %s", esp_err_to_name(ret));
        frame_head->channel = primary;
    }

    const uint8_t *addr = (frame_head->broadcast) ? ESPNOW_ADDR_BROADCAST : dest_addr;

    for (int count = 0; count < frame_head->retransmit_count; ++count) {
        for (int i = 0; i == 0
                || (g_set_channel_flag && frame_head->channel == ESPNOW_CHANNEL_ALL && i < g_self_country.nchan); ++i) {
            int retry_count = g_espnow_config->send_retry_num;

            if (g_set_channel_flag && frame_head->channel == ESPNOW_CHANNEL_ALL) {
                esp_wifi_set_channel(g_self_country.schan + i, WIFI_SECOND_CHAN_NONE);
            }

            xEventGroupClearBits(g_event_group, SEND_CB_OK | SEND_CB_FAIL);

            if (frame_head->ack) {
                count = 0;
            }

            do {
                ret = esp_now_send(addr, (uint8_t *)espnow_data, espnow_data->size + sizeof(espnow_data_t));

                if (frame_head->ack) {
                    uint32_t *ack_magic = NULL;

                    while (xQueueReceive(g_espnow_queue[ESPNOW_TYPE_ACK], &ack_magic,  0) == pdPASS) {
                        if (*ack_magic == frame_head->magic) {
                            espnow_data->frame_head.ack = false;
                            ret = ESP_OK;
                            ESP_FREE(ack_magic);
                            goto EXIT;
                        }

                        ESP_FREE(ack_magic);
                    }
                }

                if (ret == ESP_ERR_ESPNOW_NO_MEM) {
                    vTaskDelay(pdMS_TO_TICKS(10));
                }
            } while ((frame_head->ack && ++count < frame_head->retransmit_count) || (ret == ESP_ERR_ESPNOW_NO_MEM && --retry_count));

            ESP_ERROR_CONTINUE(ret != ESP_OK, "[%s, %d] <%s> esp_now_send, channel: %d",
                               __func__, __LINE__, esp_err_to_name(ret), g_self_country.schan + i);

            if (!frame_head->broadcast) {
                write_ticks = (wait_ticks == portMAX_DELAY) ? portMAX_DELAY :
                              xTaskGetTickCount() - start_ticks < wait_ticks ?
                              wait_ticks - (xTaskGetTickCount() - start_ticks) : 0;
                /**< Waiting send complete ack from mac layer */
                EventBits_t uxBits = xEventGroupWaitBits(g_event_group, SEND_CB_OK | SEND_CB_FAIL,
                                     pdTRUE, pdFALSE, MIN(write_ticks, g_espnow_config->send_max_timeout));

                ret = ESP_OK;
                ESP_ERROR_GOTO((uxBits & SEND_CB_OK) == SEND_CB_OK, EXIT, "");
            }
        }
    }

EXIT:

    if (g_set_channel_flag && frame_head->channel == ESPNOW_CHANNEL_ALL) {
        esp_wifi_set_channel(primary, second);
    }

    xSemaphoreGive(g_send_lock);

    if (frame_head->ack) {
        uint32_t *ack_magic = NULL;
        write_ticks = (wait_ticks == portMAX_DELAY) ? portMAX_DELAY :
                      xTaskGetTickCount() - start_ticks < wait_ticks ?
                      wait_ticks - (xTaskGetTickCount() - start_ticks) : 0;

        while (xQueueReceive(g_espnow_queue[ESPNOW_TYPE_ACK], &ack_magic, MIN(write_ticks,
                             g_espnow_config->send_max_timeout)) == pdPASS) {
            if (*ack_magic == frame_head->magic) {
                ESP_FREE(espnow_data);
                ESP_FREE(ack_magic);
                return ESP_OK;
            }

            ESP_FREE(ack_magic);
        }

        ret = ESP_ERR_WIFI_TIMEOUT;
    }

    ESP_FREE(espnow_data);

    return ret;
}

esp_err_t espnow_send_group(const uint8_t addrs_list[][ESPNOW_ADDR_LEN], size_t addrs_num,
                            const uint8_t group_id[ESPNOW_ADDR_LEN], espnow_frame_head_t *data_head,
                            bool type, TickType_t wait_ticks)
{
    esp_err_t ret = ESP_OK;

    espnow_data_t *espnow_data = ESP_MALLOC(addrs_num > 35 ? ESPNOW_DATA_LEN : sizeof(espnow_data_t) + sizeof(espnow_group_info_t) + addrs_num * ESPNOW_ADDR_LEN);
    espnow_frame_head_t *frame_head = &espnow_data->frame_head;
    espnow_group_info_t *group_info = (espnow_group_info_t *) espnow_data->payload;

    uint8_t primary           = 0;
    wifi_second_chan_t second = 0;

    espnow_data->version = ESPNOW_VERSION;
    espnow_data->type = ESPNOW_TYPE_GROUP;
    memcpy(espnow_data->dest_addr, ESPNOW_ADDR_BROADCAST, ESPNOW_ADDR_LEN);
    memcpy(espnow_data->src_addr, ESPNOW_ADDR_SELF, ESPNOW_ADDR_LEN);

    if (data_head) {
        memcpy(frame_head, data_head, sizeof(espnow_frame_head_t));
    } else {
        memcpy(frame_head, &g_espnow_frame_head_default, sizeof(espnow_frame_head_t));
        frame_head->magic = esp_random();
    }

    if (g_set_channel_flag && (frame_head->channel == ESPNOW_CHANNEL_ALL || frame_head->channel == 0)) {
        ret = esp_wifi_get_channel(&primary, &second);
        ESP_ERROR_GOTO(ret != ESP_OK, EXIT, "esp_wifi_get_channel, err_name: %s", esp_err_to_name(ret));

        if (frame_head->channel == 0) {
            frame_head->channel = primary;
        }
    }

    group_info->type = type;
    group_info->addrs_num = addrs_num;
    memcpy(group_info->group_id, group_id, ESPNOW_ADDR_LEN);

    for (int i = 0; addrs_num > 0; ++i) {
        size_t send_addrs_num = (addrs_num > 32) ? 32 : addrs_num;
        addrs_num -= send_addrs_num;
        espnow_data->size = sizeof(espnow_group_info_t) + send_addrs_num * ESPNOW_ADDR_LEN;
        memcpy(group_info->addrs_list, addrs_list + i * 32, send_addrs_num * ESPNOW_ADDR_LEN);
        frame_head->magic += i;

        for (int count = 0; count < frame_head->retransmit_count; ++count) {
            for (int i = 0; i == 0
                    || (frame_head->channel == ESPNOW_CHANNEL_ALL && i < g_self_country.nchan && g_set_channel_flag); ++i) {

                if (frame_head->channel == ESPNOW_CHANNEL_ALL && g_set_channel_flag) {
                    esp_wifi_set_channel(g_self_country.schan + i, WIFI_SECOND_CHAN_NONE);
                }

                ret = esp_now_send(ESPNOW_ADDR_BROADCAST, (uint8_t *)espnow_data, espnow_data->size + sizeof(espnow_data_t));
                ESP_ERROR_CONTINUE(ret != ESP_OK, "[%s, %d] <%s> esp_now_send, channel: %d",
                                   __func__, __LINE__, esp_err_to_name(ret), g_self_country.schan + i);
            }
        }
    }

EXIT:

    if (frame_head->channel == ESPNOW_CHANNEL_ALL && g_set_channel_flag) {
        esp_wifi_set_channel(primary, second);
    }

    ESP_FREE(espnow_data);
    return ESP_OK;
}

esp_err_t espnow_recv(espnow_type_t type,  uint8_t *src_addr, void *data,
                      size_t *size, wifi_pkt_rx_ctrl_t *rx_ctrl, TickType_t wait_ticks)
{
    ESP_PARAM_CHECK(data);
    ESP_ERROR_RETURN(!g_espnow_config, ESP_ERR_ESPNOW_NOT_INIT, "ESPNOW is not initialized");
    ESP_ERROR_RETURN(!g_espnow_queue[type], ESP_ERR_INVALID_STATE, "The receive queue size is not configured");

    esp_err_t ret        = 0;
    espnow_pkt_t *q_data = NULL;

    /**
     * @brief Receive data packet from special queue
     */
    xQueueHandle espnow_queue = g_espnow_queue[type];

    if (xQueueReceive(espnow_queue, &q_data, wait_ticks) != pdPASS) {
        ESP_LOGD(TAG, "Read queue: %d timeout", type);
        return ESP_ERR_TIMEOUT;
    }

    espnow_data_t *espnow_data = &q_data->data;
    espnow_frame_head_t *frame_head = &espnow_data->frame_head;

    ESP_LOGD(TAG, "[%s, %d], " MACSTR ", magic: 0x%04x, size: %d, %s", __func__, __LINE__, MAC2STR(espnow_data->src_addr),
             frame_head->magic, espnow_data->size, espnow_data->payload);

    if (frame_head->ack) {
        espnow_data_t *ack_data = ESP_CALLOC(1, sizeof(espnow_data_t));
        ack_data->type  = ESPNOW_TYPE_ACK;
        ack_data->size  = 0;
        memcpy(&ack_data->frame_head, frame_head, sizeof(espnow_frame_head_t));
        memcpy(ack_data->src_addr, ESPNOW_ADDR_SELF, 6);
        memcpy(ack_data->dest_addr, espnow_data->src_addr, 6);

        if (queue_over_write(g_espnow_queue[ESPNOW_TYPE_FORWARD], &ack_data, MIN(wait_ticks, g_espnow_config->send_max_timeout)) != pdPASS) {
            ESP_LOGW(TAG, "[%s, %d] Send receive queue failed", __func__, __LINE__);
            ESP_FREE(ack_data);
            ret = ESP_FAIL;
        }
    }

    if (ret == ESP_OK) {
        *size = espnow_data->size;
        memcpy(data, espnow_data->payload, espnow_data->size);
        memcpy(src_addr, espnow_data->src_addr, 6);

        if (rx_ctrl) {
            *rx_ctrl = q_data->rx_ctrl;
        }
    }

    ESP_FREE(q_data);

    return ret;
}

static void espnow_forward_task(void *arg)
{
    esp_err_t ret        = 0;
    espnow_data_t *espnow_data = NULL;
    uint8_t primary           = 0;
    wifi_second_chan_t second = 0;

    while (g_espnow_config) {
        if (xQueueReceive(g_espnow_queue[ESPNOW_TYPE_FORWARD], &espnow_data, g_espnow_config->send_max_timeout) == pdFAIL) {
            continue;
        }

        ESP_LOGD(TAG, "[%s, %d], " MACSTR ", size: %d, %s", __func__, __LINE__,
                 MAC2STR(espnow_data->src_addr), espnow_data->size, espnow_data->payload);

        espnow_frame_head_t *frame_head = &espnow_data->frame_head;
        const uint8_t *dest_addr = (frame_head->broadcast) ? ESPNOW_ADDR_BROADCAST : espnow_data->dest_addr;

        /**< Wait for other tasks to be sent before send ESP-NOW data */
        if (xSemaphoreTake(g_send_lock, portMAX_DELAY) != pdPASS) {
            ESP_LOGW(TAG, "Wait Sem fail");
            ESP_FREE(espnow_data);
            continue;
        }

        if (frame_head->channel && g_set_channel_flag && g_espnow_config->forward_switch_channel) {
            ESP_ERROR_CHECK(esp_wifi_get_channel(&primary, &second));
        }

        ESP_LOGD(TAG, "[%s, %d], " MACSTR ", total: %d, type: %d, magic: 0x%x", __func__, __LINE__,
                 MAC2STR(espnow_data->src_addr),  espnow_data->size, espnow_data->type, frame_head->magic);

        for (int count = 0; !count || count < frame_head->retransmit_count; ++count) {
            for (int i = 0;  i == 0 || (frame_head->channel == ESPNOW_CHANNEL_ALL && i < g_self_country.nchan && g_set_channel_flag && g_espnow_config->forward_switch_channel); ++i) {
                uint8_t retry_count = g_espnow_config->send_retry_num;

                if (frame_head->channel && g_set_channel_flag && g_espnow_config->forward_switch_channel) {
                    esp_wifi_set_channel(g_self_country.schan + i, WIFI_SECOND_CHAN_NONE);
                }

                do {
                    ret = esp_now_send(dest_addr, (uint8_t *)espnow_data, sizeof(espnow_data_t) + espnow_data->size);

                    if (ret == ESP_ERR_ESPNOW_NO_MEM) {
                        vTaskDelay(pdMS_TO_TICKS(10));
                    }
                } while (ret == ESP_ERR_ESPNOW_NO_MEM && --retry_count);

                ESP_ERROR_CONTINUE(ret != ESP_OK, "[%s, %d] <%s> esp_now_send, channel: %d",
                                   __func__, __LINE__, esp_err_to_name(ret), g_self_country.schan + i);
            }
        }

        ESP_LOGD(TAG, "[%s, %d], " MACSTR ", size: %d, %s", __func__, __LINE__, MAC2STR(espnow_data->src_addr), espnow_data->size, espnow_data->payload);

        if (frame_head->channel && g_set_channel_flag && g_espnow_config->forward_switch_channel) {
            esp_wifi_set_channel(primary, second);
        }

        xSemaphoreGive(g_send_lock);
        ESP_FREE(espnow_data);
    }

    vTaskDelete(NULL);
}

static void wifi_event_handler(void *arg, esp_event_base_t event_base,
                               int32_t event_id, void *event_data)
{
    static bool s_ap_staconnected_flag = false;
    static bool s_sta_connected_flag   = false;

    switch (event_id) {
        case WIFI_EVENT_AP_STACONNECTED: {
            wifi_event_ap_stadisconnected_t *event = (wifi_event_ap_stadisconnected_t *) event_data;
            ESP_LOGI(TAG, "station "MACSTR" join, AID=%d", MAC2STR(event->mac), event->aid);
            s_ap_staconnected_flag = true;
            break;
        }

        case WIFI_EVENT_AP_STADISCONNECTED: {
            wifi_event_ap_stadisconnected_t* event = (wifi_event_ap_stadisconnected_t*) event_data;
            ESP_LOGI(TAG, "station "MACSTR" leave, AID=%d", MAC2STR(event->mac), event->aid);
            s_ap_staconnected_flag = false;
            break;
        }

        case WIFI_EVENT_STA_CONNECTED:{
            wifi_event_sta_connected_t *event = (wifi_event_sta_connected_t *)event_data;
            ESP_LOGI(TAG, "Connected to %s (BSSID: "MACSTR", Channel: %d)", event->ssid,
            MAC2STR(event->bssid), event->channel);
            s_sta_connected_flag = true;
            break;
        }

        case WIFI_EVENT_STA_DISCONNECTED: {
            ESP_LOGI(TAG, "sta disconnect");
            s_sta_connected_flag = false;
            break;
        }

    default:
        break;
    }

    wifi_mode_t mode = WIFI_MODE_NULL;
    esp_wifi_get_mode(&mode);

    if (mode == WIFI_MODE_APSTA || s_ap_staconnected_flag == true || s_sta_connected_flag == true) {
        g_set_channel_flag = false;
    } else {
        g_set_channel_flag = true;
    }
}

esp_err_t espnow_init(const espnow_config_t *config)
{
    if (g_espnow_config) {
        return ESP_OK;
    }

    g_espnow_config = ESP_MALLOC(sizeof(espnow_config_t));
    memcpy(g_espnow_config, config, sizeof(espnow_config_t));

    /**< Event group for espnow sent cb */
    g_event_group = xEventGroupCreate();
    ESP_ERROR_RETURN(!g_event_group, ESP_FAIL, "Create event group fail");
    
    g_send_lock = xSemaphoreCreateMutex();
    ESP_ERROR_RETURN(!g_send_lock, ESP_FAIL, "Create send semaphore mutex fail");

    ESP_ERROR_CHECK(esp_event_handler_register(WIFI_EVENT, ESP_EVENT_ANY_ID, wifi_event_handler, NULL));

    /**< Create ESPNOW_TYPE_MAX queue to distinguish data and temporarily store */
    uint8_t *qsize = (uint8_t *)&config->qsize;

    for (int i = 0; i < ESPNOW_TYPE_MAX; ++i) {
        if (qsize[i] != 0) {
            g_espnow_queue[i] = xQueueCreate(qsize[i], sizeof(espnow_data_t *));
            ESP_ERROR_RETURN(!g_espnow_queue[i], ESP_FAIL, "Create espnow debug queue fail");
        }
    }

    /**< Initialize ESPNOW function */
    ESP_ERROR_CHECK(esp_now_init());
    ESP_ERROR_CHECK(esp_now_register_send_cb(espnow_send_cb));
    ESP_ERROR_CHECK(esp_now_register_recv_cb(espnow_recv_cb));
    ESP_ERROR_CHECK(esp_now_set_pmk(config->pmk));

    espnow_add_peer(ESPNOW_ADDR_BROADCAST, NULL);

    esp_wifi_get_country(&g_self_country);
    esp_wifi_get_mac(ESP_IF_WIFI_STA, ESPNOW_ADDR_SELF);
    ESP_LOGI(TAG, "mac: " MACSTR ", version: %d", MAC2STR(ESPNOW_ADDR_SELF), ESPNOW_VERSION);

    if(config->forward_enable) {
        ESP_LOGI(TAG, "Enable data forwarding");
        xTaskCreate(espnow_forward_task, "espnow_forward", 3072, NULL, tskIDLE_PRIORITY, NULL);
    }

    return ESP_OK;
}

esp_err_t espnow_deinit(void)
{
    ESP_ERROR_RETURN(!g_espnow_config, ESP_ERR_ESPNOW_NOT_INIT, "ESPNOW is not initialized");

    for (int i = 0; i < ESPNOW_TYPE_MAX; ++i) {
        uint8_t *tmp_data = NULL;
        xQueueHandle tmp_queue = g_espnow_queue[i];

        g_espnow_queue[i] = NULL;

        while (tmp_queue && xQueueReceive(tmp_queue, &tmp_data, 0)) {
            ESP_FREE(tmp_data);
        }

        vQueueDelete(tmp_queue);
    }

    vEventGroupDelete(g_event_group);
    g_event_group = NULL;

    /**< De-initialize ESPNOW function */
    ESP_ERROR_CHECK(esp_now_unregister_recv_cb());
    ESP_ERROR_CHECK(esp_now_unregister_send_cb());
    ESP_ERROR_CHECK(esp_now_deinit());

    ESP_FREE(g_espnow_config);
    g_espnow_config = NULL;

    return ESP_OK;
}

esp_err_t espnow_set_qsize(espnow_type_t type, size_t size)
{
    uint8_t *qsize = (uint8_t *)&g_espnow_config->qsize;

    if (qsize[type] && qsize[type] == size) {
        return ESP_OK;
    }

    if(g_espnow_queue[type]) {
        uint8_t *tmp = NULL;

        while (xQueueReceive(g_espnow_queue[type], &tmp, 0)) {
            ESP_FREE(tmp);
        }
        
        vQueueDelete(g_espnow_queue[type]);
        g_espnow_queue[type] = NULL;
    }

    if (size) {
        g_espnow_queue[type] = xQueueCreate(size, sizeof(espnow_data_t *));
        ESP_ERROR_RETURN(!g_espnow_queue[type], ESP_FAIL, "Create espnow %d queue fail", type);
    }

    return ESP_OK;
}

esp_err_t espnow_get_qsize(espnow_type_t type, size_t *size)
{
    uint8_t *qsize = (uint8_t *)&g_espnow_config->qsize;

    *size = qsize[type];

    return ESP_OK;
}