236 lines
8.2 KiB
C++
236 lines
8.2 KiB
C++
#include "blk_box_drivers/bbnow_old.hpp"
|
|
#include "bbnow_pub.hpp"
|
|
#include "esp_now.h"
|
|
#include "esp_log.h"
|
|
#include "esp_mac.h"
|
|
#include <memory.h>
|
|
#include <cstring>
|
|
#include <span>
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "freertos/queue.h"
|
|
|
|
// TODO: remove this file and the hpp file associated with it.
|
|
|
|
const static char TAG[] = "bbnow";
|
|
|
|
#define BBNOW_QUEUE_LEN 16
|
|
#define BBNOW_TASK_STACK (4096)
|
|
#define BBNOW_TASK_PRIO 5
|
|
|
|
static QueueHandle_t g_bbnow_queue = NULL;
|
|
static TaskHandle_t g_bbnow_task_handle = NULL;
|
|
|
|
// Wire-format bytes of the pub magic number (big-endian), used to classify
|
|
// received packets without depending on host byte order of the uint32_t.
|
|
static const uint8_t k_pub_magic_bytes[4] = {
|
|
static_cast<uint8_t>((BBNowPubPacket::PUB_MAGIC_NUMBER >> 24) & 0xFF),
|
|
static_cast<uint8_t>((BBNowPubPacket::PUB_MAGIC_NUMBER >> 16) & 0xFF),
|
|
static_cast<uint8_t>((BBNowPubPacket::PUB_MAGIC_NUMBER >> 8) & 0xFF),
|
|
static_cast<uint8_t>((BBNowPubPacket::PUB_MAGIC_NUMBER >> 0) & 0xFF),
|
|
};
|
|
|
|
static void espnow_send_cb(const esp_now_send_info_t *tx_info, esp_now_send_status_t status)
|
|
{
|
|
if (tx_info == NULL) {
|
|
ESP_LOGE(TAG, "Send cb arg error");
|
|
return;
|
|
}
|
|
ESP_LOGI(TAG, "sending espnow packet of size: %d to " MACSTR, tx_info->data_len, MAC2STR(tx_info->des_addr));
|
|
}
|
|
|
|
static void espnow_recv_cb(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len)
|
|
{
|
|
uint8_t * src_addr = recv_info->src_addr;
|
|
uint8_t * des_addr = recv_info->des_addr;
|
|
|
|
if (src_addr == NULL || data == NULL || len <= 0) {
|
|
ESP_LOGE(TAG, "Receive cb arg error");
|
|
return;
|
|
}
|
|
|
|
ESP_LOGI(TAG, "got espnow packet of size: %d from " MACSTR " to " MACSTR, len, MAC2STR(src_addr), MAC2STR(des_addr));
|
|
|
|
// Hand the raw bytes to the queue; the bbnow task will classify and
|
|
// deserialize them. If the queue isn't up yet, just drop it.
|
|
if (g_bbnow_queue != NULL) {
|
|
bbnow_enqueue_received(src_addr, data, static_cast<size_t>(len));
|
|
}
|
|
}
|
|
|
|
esp_err_t bbnow_enqueue_received(const uint8_t* src, const uint8_t* data, size_t len) {
|
|
if (g_bbnow_queue == NULL) {
|
|
return ESP_ERR_INVALID_STATE;
|
|
}
|
|
if (data == NULL || len == 0 || len > ESP_NOW_MAX_DATA_LEN) {
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
BBNowQueueItem item{};
|
|
item.type = BBNowQueueItemType::RX;
|
|
item.len = len;
|
|
memcpy(item.payload, data, len);
|
|
if (src != NULL) {
|
|
memcpy(item.addr, src, ESP_NOW_ETH_ALEN);
|
|
} else {
|
|
memcpy(item.addr, BROADCAST_MAC, ESP_NOW_ETH_ALEN);
|
|
}
|
|
|
|
// Classify by sniffing the leading magic number so the handler knows
|
|
// whether to treat the payload as a pub or (future) priv packet.
|
|
if (len >= sizeof(k_pub_magic_bytes) &&
|
|
memcmp(data, k_pub_magic_bytes, sizeof(k_pub_magic_bytes)) == 0) {
|
|
item.kind = BBNowPacketKind::PUB;
|
|
} else {
|
|
item.kind = BBNowPacketKind::PRIV;
|
|
}
|
|
|
|
if (xQueueSend(g_bbnow_queue, &item, pdMS_TO_TICKS(100)) != pdTRUE) {
|
|
ESP_LOGE(TAG, "failed to enqueue received packet");
|
|
return ESP_FAIL;
|
|
}
|
|
return ESP_OK;
|
|
}
|
|
|
|
esp_err_t bbnow_send_pub(const BBNowPubPacket& pkt, const uint8_t* dest) {
|
|
if (dest == NULL) {
|
|
dest = BROADCAST_MAC;
|
|
}
|
|
if (g_bbnow_queue == NULL) {
|
|
ESP_LOGE(TAG, "bbnow not ready; call bbnow_start_task() first");
|
|
return ESP_ERR_INVALID_STATE;
|
|
}
|
|
|
|
auto buf = pkt.serialize();
|
|
if (buf.empty()) {
|
|
ESP_LOGE(TAG, "pub packet failed to serialize");
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
if (buf.size() > ESP_NOW_MAX_DATA_LEN) {
|
|
ESP_LOGE(TAG, "pub packet too large for esp_now (%u > %d)", buf.size(), ESP_NOW_MAX_DATA_LEN);
|
|
return ESP_ERR_INVALID_SIZE;
|
|
}
|
|
|
|
BBNowQueueItem item{};
|
|
item.type = BBNowQueueItemType::TX;
|
|
item.kind = BBNowPacketKind::PUB;
|
|
item.len = buf.size();
|
|
memcpy(item.payload, buf.data(), buf.size());
|
|
memcpy(item.addr, dest, ESP_NOW_ETH_ALEN);
|
|
|
|
if (xQueueSend(g_bbnow_queue, &item, pdMS_TO_TICKS(100)) != pdTRUE) {
|
|
ESP_LOGE(TAG, "failed to enqueue pub packet for tx");
|
|
return ESP_FAIL;
|
|
}
|
|
return ESP_OK;
|
|
}
|
|
|
|
esp_err_t bbnow_send_priv(const uint8_t* dest) {
|
|
// SCAFFOLD: BBNOW private packets are not implemented yet.
|
|
//
|
|
// Planned implementation mirrors `bbnow_send_pub`: serialize a
|
|
// `BBNowPrivPacket` (once defined in blk_box_common) into `item.payload`,
|
|
// set `item.kind = BBNowPacketKind::PRIV`, and enqueue a TX item. The
|
|
// task and RX handler below already route `PRIV` items correctly, so only
|
|
// this function and the `BBNowPacketKind::PRIV` branch of
|
|
// `bbnow_handle_received` need filling in.
|
|
(void)dest;
|
|
ESP_LOGE(TAG, "bbnow_send_priv: not implemented");
|
|
return ESP_ERR_NOT_SUPPORTED;
|
|
}
|
|
|
|
/// Handles a packet that arrived over esp_now. Routing on `item.kind` already
|
|
/// accounts for private packets even though their handling is still scaffolded.
|
|
static void bbnow_handle_received(const BBNowQueueItem& item) {
|
|
switch (item.kind) {
|
|
case BBNowPacketKind::PUB: {
|
|
auto pkt = BBNowPubPacket::deserialize(std::span<const uint8_t>(item.payload, item.len));
|
|
if (!pkt.has_value()) {
|
|
ESP_LOGW(TAG, "received pub packet failed to deserialize");
|
|
return;
|
|
}
|
|
ESP_LOGI(TAG, "received PUB packet: type=%d", static_cast<int>(pkt->packet_type));
|
|
if (pkt->packet_type == BBNowPubPacketType::DISCOVERY) {
|
|
ESP_LOGI(TAG, " DISCOVERY expantion=%d", static_cast<int>(std::get<BlkBoxNowPubDiscoveryData>(pkt->packet_data).expantion_type));
|
|
}
|
|
break;
|
|
}
|
|
case BBNowPacketKind::PRIV:
|
|
// SCAFFOLD: private packet handling not implemented yet.
|
|
ESP_LOGW(TAG, "received PRIV packet (%u bytes) -- handling not implemented", item.len);
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void bbnow_task(void* pv) {
|
|
(void)pv;
|
|
BBNowQueueItem item;
|
|
for (;;) {
|
|
if (xQueueReceive(g_bbnow_queue, &item, portMAX_DELAY) != pdTRUE) {
|
|
continue;
|
|
}
|
|
switch (item.type) {
|
|
case BBNowQueueItemType::TX: {
|
|
esp_err_t err = esp_now_send(item.addr, item.payload, item.len);
|
|
if (err != ESP_OK) {
|
|
ESP_LOGE(TAG, "esp_now_send failed: %s", esp_err_to_name(err));
|
|
}
|
|
break;
|
|
}
|
|
case BBNowQueueItemType::RX:
|
|
bbnow_handle_received(item);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void bbnow_start_task() {
|
|
if (g_bbnow_queue == NULL) {
|
|
g_bbnow_queue = xQueueCreate(BBNOW_QUEUE_LEN, sizeof(BBNowQueueItem));
|
|
if (g_bbnow_queue == NULL) {
|
|
ESP_LOGE(TAG, "failed to create bbnow queue");
|
|
return;
|
|
}
|
|
}
|
|
if (g_bbnow_task_handle == NULL) {
|
|
BaseType_t rc = xTaskCreate(bbnow_task, "bbnow_task", BBNOW_TASK_STACK, NULL, BBNOW_TASK_PRIO, &g_bbnow_task_handle);
|
|
if (rc != pdPASS) {
|
|
ESP_LOGE(TAG, "failed to create bbnow task");
|
|
} else {
|
|
ESP_LOGI(TAG, "bbnow task started");
|
|
}
|
|
}
|
|
}
|
|
|
|
void init_espnow(void) {
|
|
ESP_LOGI(TAG, "Intializing espnow...");
|
|
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) );
|
|
|
|
/* Set primary master key. */
|
|
// TODO: add provisions for encryption
|
|
// ESP_ERROR_CHECK( esp_now_set_pmk(ESPNOW_PMK));
|
|
|
|
// Add broadcast peer information to peer list, allowing us to send to it
|
|
esp_now_peer_info_t peer = {
|
|
.peer_addr = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF },
|
|
.lmk = {},
|
|
.channel = 0,
|
|
.ifidx = WIFI_IF_AP,
|
|
.encrypt = false,
|
|
.priv = NULL,
|
|
};
|
|
ESP_ERROR_CHECK( esp_now_add_peer(&peer) );
|
|
|
|
// Create the queue used to hand packets between callers, the esp_now
|
|
// callbacks, and the bbnow task. The task itself is started separately
|
|
// (see bbnow_start_task) so its lifecycle is explicit.
|
|
if (g_bbnow_queue == NULL) {
|
|
g_bbnow_queue = xQueueCreate(BBNOW_QUEUE_LEN, sizeof(BBNowQueueItem));
|
|
if (g_bbnow_queue == NULL) {
|
|
ESP_LOGE(TAG, "failed to create bbnow queue");
|
|
}
|
|
}
|
|
}
|