bb driver impl (not tested)

CMakeLists.txt

@@ -1,5 +1,7 @@
 idf_component_register(
     SRCS "bbnow_driver.cpp" "bbnow_priv.cpp" "bbnow_pub.cpp"
     INCLUDE_DIRS "include" "."
-    # PRIV_REQUIRES 
+    REQUIRES
+        esp_wifi
+        freertos
 )

bbnow_driver.cpp

@@ -1,20 +1,352 @@
 #include "bbnow_driver.hpp"
 
-esp_err_t BBNowDriver::init()
-{
+#include "esp_log.h"
+#include "esp_now.h"
+#include "esp_wifi.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+
+#include <algorithm>
+#include <cstring>
+#include <new>
+#include <utility>
+#include <variant>
+
+namespace {
+
+[[maybe_unused]] const static char TAG[] = "bbnow_driver";
+constexpr size_t RX_QUEUE_LENGTH = 16;
+
+struct ReceivedPacket {
+  BBNowDriver::MacAddress source{};
+  std::variant<BBNowPubPacket, BBNowPrivPacket> packet;
+};
+
+QueueHandle_t pub_queue = nullptr;
+QueueHandle_t priv_queue = nullptr;
+
+constexpr bool is_broadcast(const BBNowDriver::MacAddress &mac) {
+  return mac == BBNowDriver::MacAddress{0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+}
+
+constexpr bool is_zero(const BBNowDriver::MacAddress &mac) {
+  return mac == BBNowDriver::MacAddress{};
+}
+
+constexpr BBNowDriver::MacAddress BROADCAST_MAC = {0xff, 0xff, 0xff,
+                                                   0xff, 0xff, 0xff};
+
+bool is_broadcast(const uint8_t *mac) {
+  return mac != nullptr &&
+         std::memcmp(mac, BROADCAST_MAC.data(), BROADCAST_MAC.size()) == 0;
+}
+
+bool is_valid_expander(BlkBoxExpantion type) {
+  return type != BlkBoxExpantion::NONE && type <= BlkBoxExpantion::MAX;
+}
+
+void destroy_queues() {
+  if (pub_queue != nullptr) {
+    ReceivedPacket *packet = nullptr;
+    while (xQueueReceive(pub_queue, &packet, 0) == pdTRUE) {
+      delete packet;
+    }
+    vQueueDelete(pub_queue);
+    pub_queue = nullptr;
+  }
+  if (priv_queue != nullptr) {
+    ReceivedPacket *packet = nullptr;
+    while (xQueueReceive(priv_queue, &packet, 0) == pdTRUE) {
+      delete packet;
+    }
+    vQueueDelete(priv_queue);
+    priv_queue = nullptr;
+  }
+}
+
+void espnow_receive_callback(
+  const esp_now_recv_info_t *receive_info,
+  const uint8_t *payload, int length
+) {
+  if (receive_info == nullptr || receive_info->src_addr == nullptr ||
+      receive_info->des_addr == nullptr || payload == nullptr || length <= 0 ||
+      static_cast<size_t>(length) > ESP_NOW_MAX_DATA_LEN) {
+    ESP_LOGW(TAG, "dropping ESP-NOW packet with invalid callback arguments");
+    return;
+  }
+
+  const size_t payload_length = static_cast<size_t>(length);
+  const bool is_pub = is_broadcast(receive_info->des_addr);
+  QueueHandle_t queue = is_pub ? pub_queue : priv_queue;
+  if (queue == nullptr) {
+    ESP_LOGW(TAG, "dropping ESP-NOW packet before driver initialization");
+    return;
+  }
+
+  ReceivedPacket *packet = nullptr;
+  if (is_pub) {
+    auto decoded = BBNowPubPacket::deserialize(
+        std::span<const uint8_t>(payload, payload_length));
+    if (!decoded.has_value()) {
+      ESP_LOGW(TAG, "dropping invalid public ESP-NOW packet");
+      return;
+    }
+
+    packet = new (std::nothrow) ReceivedPacket{
+        .source = {},
+        .packet = std::move(*decoded),
+    };
+  } else {
+    auto decoded = BBNowPrivPacket::deserialize(
+        std::span<const uint8_t>(payload, payload_length));
+    if (!decoded.has_value()) {
+      ESP_LOGW(TAG, "dropping invalid private ESP-NOW packet");
+      return;
+    }
+
+    packet = new (std::nothrow) ReceivedPacket{
+        .source = {},
+        .packet = std::move(*decoded),
+    };
+  }
+
+  if (packet == nullptr) {
+    ESP_LOGW(TAG, "dropping ESP-NOW packet: no memory for receive queue");
+    return;
+  }
+  std::memcpy(packet->source.data(), receive_info->src_addr,
+              packet->source.size());
+
+  // The callback cannot wait for application code to drain the queue.  A
+  // warning makes queue overflow visible while preserving the non-blocking
+  // callback contract.
+  ReceivedPacket *queued_packet = packet;
+  if (xQueueSend(queue, &queued_packet, 0) != pdTRUE) {
+    ESP_LOGW(TAG, "dropping ESP-NOW packet: receive queue is full");
+    delete packet;
+    return;
+  }
+}
+
+bool receive_packet(
+  QueueHandle_t queue, ReceivedPacket *&packet,
+  TickType_t max_delay
+) {
+  return queue != nullptr && xQueueReceive(queue, &packet, max_delay) == pdTRUE;
+}
+
+} // namespace
+
+bool BBNowDriver::initialized_ = false;
+std::vector<BBNowDriver::PairedExpander> BBNowDriver::paired_expanders;
+
+esp_err_t BBNowDriver::init() {
+  if (initialized_) {
+    return ESP_OK;
+  }
+
+  pub_queue = xQueueCreate(RX_QUEUE_LENGTH, sizeof(ReceivedPacket *));
+  priv_queue = xQueueCreate(RX_QUEUE_LENGTH, sizeof(ReceivedPacket *));
+  if (pub_queue == nullptr || priv_queue == nullptr) {
+    destroy_queues();
+    return ESP_ERR_NO_MEM;
+  }
+
+  esp_err_t err = esp_now_init();
+  if (err != ESP_OK) {
+    destroy_queues();
+    return err;
+  }
+
+  esp_now_peer_info_t broadcast_peer{};
+  std::memcpy(broadcast_peer.peer_addr, BROADCAST_MAC.data(),
+              BROADCAST_MAC.size());
+  broadcast_peer.channel = 0;
+  broadcast_peer.ifidx = WIFI_IF_STA;
+  broadcast_peer.encrypt = false;
+
+  err = esp_now_add_peer(&broadcast_peer);
+  if (err != ESP_OK) {
+    (void)esp_now_deinit();
+    destroy_queues();
+    return err;
+  }
+
+  err = esp_now_register_recv_cb(espnow_receive_callback);
+  if (err != ESP_OK) {
+    (void)esp_now_deinit();
+    destroy_queues();
+    return err;
+  }
+
+  initialized_ = true;
   return ESP_OK;
 }
 
-esp_err_t BBNowDriver::deinit()
-{
+esp_err_t BBNowDriver::deinit() {
+  if (!initialized_) {
+    return ESP_OK;
+  }
+
+  esp_err_t err = esp_now_deinit();
+  if (err != ESP_OK) {
+    return err;
+  }
+
+  initialized_ = false;
+  paired_expanders.clear();
+  destroy_queues();
   return ESP_OK;
 }
 
-esp_err_t BBNowDriver::send(
-    const MacAddress& destination,
-    const uint8_t* data,
-    size_t size
-)
-{
+esp_err_t BBNowDriver::send_pub(const BBNowPubPacket &packet) {
+  if (!initialized_) {
+    return ESP_ERR_INVALID_STATE;
+  }
+
+  const std::vector<uint8_t> payload = packet.serialize();
+  if (payload.empty()) {
+    return ESP_ERR_INVALID_ARG;
+  }
+  if (payload.size() > ESP_NOW_MAX_DATA_LEN) {
+    return ESP_ERR_INVALID_SIZE;
+  }
+
+  return esp_now_send(BROADCAST_MAC.data(), payload.data(), payload.size());
+}
+
+bool BBNowDriver::has_pub() {
+  return pub_queue != nullptr && uxQueueMessagesWaiting(pub_queue) != 0;
+}
+
+bool BBNowDriver::recv_pub(
+  MacAddress &source,
+  BBNowPubPacket &packet,
+  TickType_t max_delay
+) {
+  ReceivedPacket *received = nullptr;
+  if (receive_packet(pub_queue, received, max_delay)) {
+    source = received->source;
+    packet = std::move(std::get<BBNowPubPacket>(received->packet));
+    delete received;
+    return true;
+  }
+  return false;
+}
+
+esp_err_t BBNowDriver::send_priv(
+  const MacAddress &destination,                               
+  const BBNowPrivPacket &packet
+) {
+  if (!initialized_) {
+    return ESP_ERR_INVALID_STATE;
+  }
+  if (is_zero(destination)) {
+    return ESP_ERR_INVALID_ARG;
+  }
+
+  const std::vector<uint8_t> payload = packet.serialize();
+  if (payload.empty()) {
+    return ESP_ERR_INVALID_ARG;
+  }
+  if (payload.size() > ESP_NOW_MAX_DATA_LEN) {
+    return ESP_ERR_INVALID_SIZE;
+  }
+
+  return esp_now_send(destination.data(), payload.data(), payload.size());
+}
+
+bool BBNowDriver::has_priv() {
+  return priv_queue != nullptr && uxQueueMessagesWaiting(priv_queue) != 0;
+}
+
+bool BBNowDriver::recv_priv(
+  MacAddress &source,
+  BBNowPrivPacket &packet,
+  TickType_t max_delay
+) {
+  ReceivedPacket *received = nullptr;
+  if (receive_packet(priv_queue, received, max_delay)) {
+    source = received->source;
+    packet = std::move(std::get<BBNowPrivPacket>(received->packet));
+    delete received;
+    return true;
+  }
+  return false;
+}
+
+size_t BBNowDriver::send_discovery(BlkBoxExpantion type) {
+  if (!is_valid_expander(type)) {
+    return 0;
+  }
+
+  BlkBoxNowPubDiscoveryData data{.expantion_type = type};
+  BBNowPubPacket packet = BBNowPubPacket::new_discovery(data);
+  const size_t size = packet.serialize_size();
+  return send_pub(packet) == ESP_OK ? size : 0;
+}
+
+esp_err_t BBNowDriver::pair_expander(BlkBoxExpantion type,
+                                     const MacAddress &mac) {
+  if (!initialized_) {
+    return ESP_ERR_INVALID_STATE;
+  }
+  if (!is_valid_expander(type) || is_zero(mac) || is_broadcast(mac)) {
+    return ESP_ERR_INVALID_ARG;
+  }
+
+  for (auto &expander : paired_expanders) {
+    if (expander.mac == mac) {
+      expander.type = type;
+      return ESP_OK;
+    }
+  }
+
+  esp_now_peer_info_t peer{};
+  std::memcpy(peer.peer_addr, mac.data(), mac.size());
+  peer.channel = 0;
+  peer.ifidx = WIFI_IF_STA;
+  peer.encrypt = false;
+
+  esp_err_t err = esp_now_add_peer(&peer);
+  if (err != ESP_OK) {
+    return err;
+  }
+
+  paired_expanders.push_back(PairedExpander{
+      .mac = mac,
+      .name = {},
+      .type = type,
+  });
+
   return ESP_OK;
 }
+
+esp_err_t BBNowDriver::unpair_expander(const MacAddress &mac) {
+  if (!initialized_) {
+    return ESP_ERR_INVALID_STATE;
+  }
+  if (is_zero(mac) || is_broadcast(mac)) {
+    return ESP_ERR_INVALID_ARG;
+  }
+
+  auto it = std::find_if(
+      paired_expanders.begin(), paired_expanders.end(),
+      [&mac](const PairedExpander &expander) { return expander.mac == mac; });
+  if (it == paired_expanders.end()) {
+    return ESP_ERR_NOT_FOUND;
+  }
+
+  esp_err_t err = esp_now_del_peer(mac.data());
+  if (err != ESP_OK) {
+    return err;
+  }
+
+  paired_expanders.erase(it);
+  return ESP_OK;
+}
+
+std::span<const BBNowDriver::PairedExpander>
+BBNowDriver::get_paired_expanders() {
+  return paired_expanders;
+}

bbnow_pub.cpp

@@ -1,6 +1,7 @@
 #include "bbnow_pub.hpp"
 #include <cassert>
 #include <esp_log.h>
+#include <utility>
 
 const static char TAG[] = "bbnow_pub";
 
@@ -49,13 +50,16 @@ size_t BBNowPubPacket::serialize_into(std::vector<uint8_t>& buf) const {
     switch (this->packet_type) {
         case BBNowPubPacketType::NONE:
             break;
-        case BBNowPubPacketType::DISCOVERY:
-            static_assert(sizeof(this->packet_data.discovery) == 2, "expect discovery data to be 2 bytes");
-            uint16_t data = static_cast<uint16_t>(this->packet_data.discovery.expantion_type);
+        case BBNowPubPacketType::DISCOVERY: {
+            const auto& discovery =
+                std::get<BlkBoxNowPubDiscoveryData>(this->packet_data);
+            static_assert(sizeof(discovery) == 2, "expect discovery data to be 2 bytes");
+            uint16_t data = static_cast<uint16_t>(discovery.expantion_type);
             buf.push_back((data >> 8) & 0xFF);
             buf.push_back((data >> 0) & 0xFF);
             break;
         }
+    }
 
     assert(buf.size() - initial_size == size);
     return size;
@@ -88,7 +92,7 @@ std::optional<BBNowPubPacket> BBNowPubPacket::deserialize(std::span<const uint8_
     static_assert(sizeof(BBNowPubPacketType) == 2, "expect packet type to be 2 bytes");
     uint16_t packet_type = ((uint16_t)bytes[4] << 8) | ((uint16_t)bytes[5]);
     packet.packet_type = static_cast<BBNowPubPacketType>(packet_type);
-    if (packet.packet_type > BBNowPubPacketType::MAX) {
+    if (packet.packet_type == BBNowPubPacketType::NONE) {
         ESP_LOGW(TAG, "BlkBoxPubPacketType was NONE! discarding");
         return std::nullopt;
     }
@@ -111,21 +115,25 @@ std::optional<BBNowPubPacket> BBNowPubPacket::deserialize(std::span<const uint8_
     switch (packet.packet_type) {
         case BBNowPubPacketType::NONE:
             break;
-        case BBNowPubPacketType::DISCOVERY:
+        case BBNowPubPacketType::DISCOVERY: {
             uint16_t device = ((uint16_t)bytes[6] << 8) | ((uint16_t)bytes[7]);
-            packet.packet_data.discovery.expantion_type = static_cast<BlkBoxExpantion>(device);
-            if (packet.packet_data.discovery.expantion_type > BlkBoxExpantion::MAX) {
+            auto data = BlkBoxNowPubDiscoveryData{
+                .expantion_type = static_cast<BlkBoxExpantion>(device)
+            };
+            if (data.expantion_type > BlkBoxExpantion::MAX) {
                 ESP_LOGW(TAG, "BlkBoxExpantion not in range! discarding");
                 return std::nullopt;
             }
+            packet.packet_data = data;
             break;
         }
+    }
     return packet;
 }
 
 BBNowPubPacket BBNowPubPacket::new_discovery(BlkBoxNowPubDiscoveryData data) {
     BBNowPubPacket packet;
     packet.packet_type = BBNowPubPacketType::DISCOVERY;
-    packet.packet_data.discovery = data;
+    packet.packet_data = std::move(data);
     return packet;
 }

include/bbnow_driver.hpp

@@ -10,6 +10,7 @@
 #include <vector>
 
 #include "esp_err.h"
+#include "freertos/FreeRTOS.h"
 
 #include "bb_expantions.hpp"
 #include "bbnow_pub.hpp"
@@ -32,7 +33,11 @@ public:
 
     static esp_err_t send_pub(const BBNowPubPacket& packet);
     static bool has_pub();
-    static bool recv_pub(BBNowPubPacket& packet);
+    static bool recv_pub(
+        MacAddress& source,
+        BBNowPubPacket& packet,
+        TickType_t max_delay = 0
+    );
 
     static esp_err_t send_priv(
         const MacAddress& destination,
@@ -41,7 +46,8 @@ public:
     static bool has_priv();
     static bool recv_priv(
         MacAddress& source,
-        BBNowPrivPacket& packet
+        BBNowPrivPacket& packet,
+        TickType_t max_delay = 0
     );
 
     /// Broadcast a discovery packet.