bb driver impl (not tested)

CMakeLists.txt

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

bbnow_driver.cpp

@@ -0,0 +1,352 @@
+#include "bbnow_driver.hpp"
+
+#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() {
+  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_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_priv.cpp

@@ -0,0 +1,360 @@
+#include "bbnow_priv.hpp"
+
+#include <cassert>
+#include <esp_log.h>
+
+const static char TAG[] = "bbnow_priv";
+
+size_t BBNowPrivPacket::serialize_size() const {
+    switch (this->packet_type) {
+        case BBNowPrivPacketType::NONE:
+            return -1;
+
+        case BBNowPrivPacketType::ACK:
+            return BBNowPrivPacket::ACK_PACKET_SIZE;
+
+        case BBNowPrivPacketType::DISCOVERY_RESPONSE:
+            return BBNowPrivPacket::DISCOVERY_RESPONSE_PACKET_SIZE;
+
+        case BBNowPrivPacketType::EXPANDER_MESSAGE: {
+            const auto& data =
+                std::get<BBNowPrivExpanderMessageData>(
+                    this->packet_data
+                );
+
+            return sizeof(BBNowPrivPacketType) +
+                   sizeof(uint16_t) +
+                   data.data.size();
+        }
+
+        case BBNowPrivPacketType::INFO_QUERY:
+            return BBNowPrivPacket::INFO_QUERY_PACKET_SIZE;
+            
+        default:
+            return -1;
+    }
+
+    return -1;
+}
+
+std::vector<uint8_t> BBNowPrivPacket::serialize() const {
+    auto buf = std::vector<uint8_t>();
+
+    if (this->serialize_size() == static_cast<size_t>(-1)) {
+        return buf;
+    }
+
+    this->serialize_into(buf);
+    return buf;
+}
+
+size_t BBNowPrivPacket::serialize_into(
+    std::vector<uint8_t>& buf
+) const {
+    size_t initial_size = buf.size();
+    size_t size = this->serialize_size();
+
+    if (size == static_cast<size_t>(-1)) {
+        return 0;
+    }
+
+    buf.reserve(initial_size + size);
+
+    // Push packet type.
+    static_assert(
+        sizeof(BBNowPrivPacketType) == 2,
+        "expect packet type to be 2 bytes"
+    );
+
+    uint16_t packet_type =
+        static_cast<uint16_t>(this->packet_type);
+
+    buf.push_back((packet_type >> 8) & 0xFF);
+    buf.push_back((packet_type >> 0) & 0xFF);
+
+    // Push sequence number.
+    static_assert(
+        sizeof(this->sequence) == 2,
+        "expect sequence number to be 2 bytes"
+    );
+
+    buf.push_back((this->sequence >> 8) & 0xFF);
+    buf.push_back((this->sequence >> 0) & 0xFF);
+
+    switch (this->packet_type) {
+        case BBNowPrivPacketType::NONE:
+            break;
+
+        case BBNowPrivPacketType::ACK:
+            break;
+
+        case BBNowPrivPacketType::DISCOVERY_RESPONSE: {
+            const auto& data =
+                std::get<BBNowPrivDiscoveryResponseData>(
+                    this->packet_data
+                );
+
+            static_assert(
+                sizeof(data.expantion_type) == 2,
+                "expect expansion type to be 2 bytes"
+            );
+
+            uint16_t expantion_type =
+                static_cast<uint16_t>(data.expantion_type);
+
+            buf.push_back((expantion_type >> 8) & 0xFF);
+            buf.push_back((expantion_type >> 0) & 0xFF);
+
+            break;
+        }
+
+        case BBNowPrivPacketType::EXPANDER_MESSAGE: {
+            const auto& data =
+                std::get<BBNowPrivExpanderMessageData>(
+                    this->packet_data
+                );
+
+            buf.insert(
+                buf.end(),
+                data.data.begin(),
+                data.data.end()
+            );
+
+            break;
+        }
+
+        case BBNowPrivPacketType::INFO_QUERY: {
+            const auto& data =
+                std::get<BBNowPrivInfoQueryData>(
+                    this->packet_data
+                );
+
+            static_assert(
+                sizeof(data) == 1,
+                "expect info query type to be 1 byte"
+            );
+            buf.push_back(data.battery_percentage);
+
+            break;
+        }
+    }
+
+    assert(buf.size() - initial_size == size);
+    return size;
+}
+
+std::optional<BBNowPrivPacket>
+BBNowPrivPacket::deserialize(
+    std::span<const uint8_t> bytes
+) {
+    // Packet type + sequence number.
+    static_assert(
+        sizeof(BBNowPrivPacketType) + sizeof(uint16_t) == 4,
+        "expect packet type and sequence number to be 4 bytes"
+    );
+
+    if (bytes.size() < 4) {
+        ESP_LOGW(
+            TAG,
+            "packet not long enough for priv packet"
+        );
+        return std::nullopt;
+    }
+
+    // Read packet type.
+    uint16_t packet_type =
+        ((uint16_t)bytes[0] << 8) |
+        ((uint16_t)bytes[1] << 0);
+
+    BBNowPrivPacket packet;
+
+    packet.packet_type =
+        static_cast<BBNowPrivPacketType>(packet_type);
+
+    if (packet.packet_type == BBNowPrivPacketType::NONE) {
+        ESP_LOGW(
+            TAG,
+            "BBNowPrivPacketType was NONE! discarding"
+        );
+        return std::nullopt;
+    }
+
+    if (packet.packet_type > BBNowPrivPacketType::MAX) {
+        ESP_LOGW(
+            TAG,
+            "BBNowPrivPacketType not in range! discarding"
+        );
+        return std::nullopt;
+    }
+
+    // Read sequence number.
+    packet.sequence =
+        ((uint16_t)bytes[2] << 8) |
+        ((uint16_t)bytes[3] << 0);
+
+    switch (packet.packet_type) {
+        case BBNowPrivPacketType::ACK: {
+            if (bytes.size() < ACK_PACKET_SIZE) {
+                ESP_LOGW(
+                    TAG,
+                    "ACK packet not long enough"
+                );
+                return std::nullopt;
+            }
+
+            if (bytes.size() > ACK_PACKET_SIZE) {
+                ESP_LOGW(
+                    TAG,
+                    "ACK packet longer than expected, "
+                    "decoding anyway"
+                );
+            }
+
+            packet.packet_data = BBNowPrivAckData{};
+            break;
+        }
+
+        case BBNowPrivPacketType::DISCOVERY_RESPONSE: {
+            if (bytes.size() < DISCOVERY_RESPONSE_PACKET_SIZE) {
+                ESP_LOGW(
+                    TAG,
+                    "discovery response packet not long enough"
+                );
+                return std::nullopt;
+            }
+
+            if (bytes.size() > DISCOVERY_RESPONSE_PACKET_SIZE) {
+                ESP_LOGW(
+                    TAG,
+                    "discovery response packet longer than expected, "
+                    "decoding anyway"
+                );
+            }
+
+            uint16_t expantion_type =
+                ((uint16_t)bytes[4] << 8) |
+                ((uint16_t)bytes[5] << 0);
+
+            auto data = BBNowPrivDiscoveryResponseData{
+                .expantion_type =
+                    static_cast<BlkBoxExpantion>(expantion_type)
+            };
+
+            if (data.expantion_type > BlkBoxExpantion::MAX) {
+                ESP_LOGW(
+                    TAG,
+                    "BlkBoxExpantion not in range! discarding"
+                );
+                return std::nullopt;
+            }
+
+            packet.packet_data = std::move(data);
+            break;
+        }
+
+        case BBNowPrivPacketType::EXPANDER_MESSAGE: {
+            // Everything after the 4-byte header is the message.
+            auto data = BBNowPrivExpanderMessageData{
+                .data = std::vector<uint8_t>(
+                    bytes.begin() + 4,
+                    bytes.end()
+                )
+            };
+
+            packet.packet_data = std::move(data);
+            break;
+        }
+
+        case BBNowPrivPacketType::INFO_QUERY: {
+            constexpr size_t INFO_QUERY_PACKET_SIZE =
+                sizeof(BBNowPrivPacketType) +
+                sizeof(uint16_t) +
+                sizeof(BBNowPrivInfoQueryData);
+
+            if (bytes.size() < INFO_QUERY_PACKET_SIZE) {
+                ESP_LOGW(
+                    TAG,
+                    "info query packet not long enough"
+                );
+                return std::nullopt;
+            }
+
+            if (bytes.size() > INFO_QUERY_PACKET_SIZE) {
+                ESP_LOGW(
+                    TAG,
+                    "info query packet longer than expected, "
+                    "decoding anyway"
+                );
+            }
+
+            BBNowPrivInfoQueryData data{
+                .battery_percentage = bytes[4]
+            };
+
+            packet.packet_data = data;
+            break;
+        }
+
+        case BBNowPrivPacketType::NONE:
+            return std::nullopt;
+    }
+
+    return packet;
+}
+
+BBNowPrivPacket BBNowPrivPacket::new_ack(
+    uint16_t sequence
+) {
+    BBNowPrivPacket packet;
+
+    packet.packet_type = BBNowPrivPacketType::ACK;
+    packet.sequence = sequence;
+    packet.packet_data = BBNowPrivAckData{};
+
+    return packet;
+}
+
+BBNowPrivPacket BBNowPrivPacket::new_discovery_response(
+    uint16_t sequence,
+    BBNowPrivDiscoveryResponseData data
+) {
+    BBNowPrivPacket packet;
+
+    packet.packet_type =
+        BBNowPrivPacketType::DISCOVERY_RESPONSE;
+
+    packet.sequence = sequence;
+    packet.packet_data = std::move(data);
+
+    return packet;
+}
+
+BBNowPrivPacket BBNowPrivPacket::new_expander_message(
+    uint16_t sequence,
+    BBNowPrivExpanderMessageData data
+) {
+    BBNowPrivPacket packet;
+
+    packet.packet_type =
+        BBNowPrivPacketType::EXPANDER_MESSAGE;
+
+    packet.sequence = sequence;
+    packet.packet_data = std::move(data);
+
+    return packet;
+}
+
+BBNowPrivPacket BBNowPrivPacket::new_info_query(
+    uint16_t sequence,
+    BBNowPrivInfoQueryData data
+) {
+    BBNowPrivPacket packet;
+
+    packet.packet_type =
+        BBNowPrivPacketType::INFO_QUERY;
+
+    packet.sequence = sequence;
+    packet.packet_data = std::move(data);
+
+    return packet;
+}

bbnow_pub.cpp

@@ -1,10 +1,11 @@
 #include "bbnow_pub.hpp"
 #include <cassert>
 #include <esp_log.h>
+#include <utility>
 
 const static char TAG[] = "bbnow_pub";
 
-size_t BBNowPubPacket::serialize_size() {
+size_t BBNowPubPacket::serialize_size() const {
     switch (this->packet_type) {
         case BBNowPubPacketType::NONE:
             return -1;
@@ -14,7 +15,7 @@ size_t BBNowPubPacket::serialize_size() {
     return -1;
 }
 
-std::vector<uint8_t> BBNowPubPacket::serialize() {
+std::vector<uint8_t> BBNowPubPacket::serialize() const {
     auto buf = std::vector<uint8_t>();
 
     if (this->serialize_size() == -1) {
@@ -25,7 +26,7 @@ std::vector<uint8_t> BBNowPubPacket::serialize() {
     return buf;
 }
 
-size_t BBNowPubPacket::serialize_into(std::vector<uint8_t>& buf) {
+size_t BBNowPubPacket::serialize_into(std::vector<uint8_t>& buf) const {
     size_t initial_size = buf.size();
     size_t size = this->serialize_size();
     if (size == -1) {
@@ -49,13 +50,16 @@ size_t BBNowPubPacket::serialize_into(std::vector<uint8_t>& buf) {
     switch (this->packet_type) {
         case BBNowPubPacketType::NONE:
             break;
-        case BBNowPubPacketType::DISCOVERY:
+        case BBNowPubPacketType::DISCOVERY: {
-            static_assert(sizeof(this->packet_data.discovery) == 2, "expect discovery data to be 2 bytes");
+            const auto& discovery =
-            uint16_t data = static_cast<uint16_t>(this->packet_data.discovery.expantion_type);
+                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);
+            auto data = BlkBoxNowPubDiscoveryData{
-            if (packet.packet_data.discovery.expantion_type > BlkBoxExpantion::MAX) {
+                .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/bb_expantions.hpp

@@ -5,6 +5,8 @@
 
 #include <stdint.h>
 
+#define EXPANDER_DEVICE_NAME_MAX_LEN 32
+
 /// The expantion device ids in the BLK_BOX ecosystem.
 enum class BlkBoxExpantion: uint16_t {
     NONE = 0,

include/bbnow_driver.hpp

@@ -0,0 +1,80 @@
+/// Generic code for driving a BBNOW interface.
+
+#ifndef _BBNOW_DRIVER_HPP
+#define _BBNOW_DRIVER_HPP
+
+#include <array>
+#include <cstdint>
+#include <span>
+#include <string>
+#include <vector>
+
+#include "esp_err.h"
+#include "freertos/FreeRTOS.h"
+
+#include "bb_expantions.hpp"
+#include "bbnow_pub.hpp"
+#include "bbnow_priv.hpp"
+
+class BBNowDriver {
+public:
+    using MacAddress = std::array<uint8_t, 6>;
+
+    struct PairedExpander {
+        MacAddress mac;
+        std::string name;
+        BlkBoxExpantion type;
+    };
+
+    static constexpr uint8_t DEFAULT_CHANNEL = 6;
+
+    static esp_err_t init();
+    static esp_err_t deinit();
+
+    static esp_err_t send_pub(const BBNowPubPacket& packet);
+    static bool has_pub();
+    static bool recv_pub(
+        MacAddress& source,
+        BBNowPubPacket& packet,
+        TickType_t max_delay = 0
+    );
+
+    static esp_err_t send_priv(
+        const MacAddress& destination,
+        const BBNowPrivPacket& packet
+    );
+    static bool has_priv();
+    static bool recv_priv(
+        MacAddress& source,
+        BBNowPrivPacket& packet,
+        TickType_t max_delay = 0
+    );
+
+    /// Broadcast a discovery packet.
+    static size_t send_discovery(
+        BlkBoxExpantion type
+    );
+
+    static esp_err_t pair_expander(
+        BlkBoxExpantion type,
+        const MacAddress& mac
+    );
+
+    static esp_err_t unpair_expander(
+        const MacAddress& mac
+    );
+
+    static std::span<const PairedExpander> get_paired_expanders();
+
+private:
+    static constexpr MacAddress BROADCAST_MAC = {
+        0xff, 0xff, 0xff,
+        0xff, 0xff, 0xff
+    };
+
+    static bool initialized_;
+
+    static std::vector<PairedExpander> paired_expanders;
+};
+
+#endif /* _BBNOW_DRIVER_HPP */

include/bbnow_priv.hpp

@@ -4,16 +4,143 @@
 #define _BBNOW_PRIV_H
 
 #include <stdint.h>
+#include <vector>
+#include <optional>
+#include <span>
+#include <variant>
 
-#define EXPANDER_DEVICE_NAME_MAX_LEN 32
+#include "bb_expantions.hpp"
 
-/// The packet type for `BBNowPubPacket`s.
+///////////////////////////////////////
+// ACK                              //
+/////////////////////////////////////
+/// The data associated with the `ACK` type.
+struct BBNowPrivAckData {
+};
+
+///////////////////////////////////////
+// DISCOVERY RESPONSE               //
+/////////////////////////////////////
+/// The data associated with the `DISCOVERY_RESPONSE` type.
+struct BBNowPrivDiscoveryResponseData {
+    BlkBoxExpantion expantion_type{};
+};
+
+///////////////////////////////////////
+// EXPANDER MESSAGE                 //
+/////////////////////////////////////
+/// The data associated with the `EXPANDER_MESSAGE` type.
+struct BBNowPrivExpanderMessageData {
+    // TODO: this should get flattened into a concrete type, an enum over the expander types.
+    std::vector<uint8_t> data;
+};
+
+///////////////////////////////////////
+// INFO QUERY                       //
+/////////////////////////////////////
+/// The data associated with the `INFO_QUERY` type.
+struct BBNowPrivInfoQueryData {
+    uint8_t battery_percentage;
+    // TODO: add other status fields that may be useful to query.
+};
+
+///////////////////////////////////////
+// PRIV PACKET                      //
+/////////////////////////////////////
+/// The packet type for `BBNowPrivPacket`s.
 enum class BBNowPrivPacketType: uint16_t {
     NONE = 0,
     ACK = 1,
+    // TODO: we will need to add more here for handshaking in the future.
     DISCOVERY_RESPONSE = 2,
     EXPANDER_MESSAGE = 3,
-    MAX = 3,
+    INFO_QUERY = 4,
+    MAX = 4,
+};
+
+/// The data field for the packet.
+using BBNowPrivPacketData = std::variant<
+    std::monostate,
+    BBNowPrivAckData,
+    BBNowPrivDiscoveryResponseData,
+    BBNowPrivExpanderMessageData,
+    BBNowPrivInfoQueryData
+>;
+
+struct BBNowPrivPacket {
+    /// ACK packet size.
+    const static size_t ACK_PACKET_SIZE = 4;
+    static_assert(
+        ACK_PACKET_SIZE == sizeof(BBNowPrivPacketType) + sizeof(uint16_t),
+        "ACK packet size should match it's data size"
+    );
+
+    /// Discovery response packet size.
+    const static size_t DISCOVERY_RESPONSE_PACKET_SIZE = 6;
+    static_assert(
+        DISCOVERY_RESPONSE_PACKET_SIZE == sizeof(BBNowPrivPacketType) +
+        sizeof(uint16_t) +
+        sizeof(BBNowPrivDiscoveryResponseData),
+        "Discovery response packet size should match it's data size"
+    );
+
+    /// Info query packet size.
+    const static size_t INFO_QUERY_PACKET_SIZE = 5;
+    static_assert(
+        INFO_QUERY_PACKET_SIZE ==
+            sizeof(BBNowPrivPacketType) +
+            sizeof(uint16_t) +
+            sizeof(BBNowPrivInfoQueryData),
+        "Info query packet size should match its data size"
+    );
+
+    /// The type of packet this is.
+    BBNowPrivPacketType packet_type{};
+
+    /// Sequence number associated with this packet.
+    uint16_t sequence{};
+
+    /// The data held by the packet.
+    BBNowPrivPacketData packet_data{};
+
+    /// The size of the serialized packet (in bytes).
+    size_t serialize_size() const;
+
+    /// Serializes the packet to a vector.
+    std::vector<uint8_t> serialize() const;
+
+    /// Serializes the packet into the given vector.
+    ///
+    /// Returns the number of bytes written into the buffer.
+    size_t serialize_into(std::vector<uint8_t>& buf) const;
+
+    /// Deserializes a packet from the given bytes.
+    static std::optional<BBNowPrivPacket>
+    deserialize(std::span<const uint8_t> bytes);
+
+    /// Constructs an empty packet.
+    BBNowPrivPacket() = default;
+
+    /// Constructs a new `ACK` packet.
+    static BBNowPrivPacket new_ack(uint16_t sequence);
+
+    /// Constructs a new `DISCOVERY_RESPONSE` packet.
+    static BBNowPrivPacket new_discovery_response(
+        uint16_t sequence,
+        BBNowPrivDiscoveryResponseData data
+    );
+
+    /// Constructs a new `EXPANDER_MESSAGE` packet.
+    static BBNowPrivPacket new_expander_message(
+        uint16_t sequence,
+        BBNowPrivExpanderMessageData data
+    );
+
+    /// Constructs a new `INFO_QUERY` packet.
+    static BBNowPrivPacket new_info_query(
+        uint16_t sequence, 
+        BBNowPrivInfoQueryData data
+    );
 };
 
 #endif /* _BBNOW_PRIV_H */

include/bbnow_pub.hpp

@@ -7,6 +7,7 @@
 #include <vector>
 #include <optional>
 #include <span>
+#include <variant>
 
 #include "bb_expantions.hpp"
 
@@ -30,12 +31,14 @@ enum class BBNowPubPacketType: uint16_t {
     MAX = 1,
 };
 
-/// The data field for the packet
+/// The data field for the packet.
-union BlkBoxNowPubPacketData {
+using BlkBoxNowPubPacketData = std::variant<
-    BlkBoxNowPubDiscoveryData discovery;
+    std::monostate,
-};
+    BlkBoxNowPubDiscoveryData
+>;
 
 struct BBNowPubPacket {
+    // "bbnp" in ascii
     const static uint32_t PUB_MAGIC_NUMBER = 0x6262'6E70;
     // DISCOVERY packet size
     const static size_t DISCOVERY_PACKET_SIZE = sizeof(PUB_MAGIC_NUMBER) + sizeof(BBNowPubPacketType) + sizeof(BlkBoxNowPubDiscoveryData);
@@ -47,13 +50,13 @@ struct BBNowPubPacket {
     BlkBoxNowPubPacketData packet_data{};
 
     /// The size of the serialized packet (in bytes).
-    size_t serialize_size();
+    size_t serialize_size() const;
     /// Serializes the packet to a vector.
-    std::vector<uint8_t> serialize();
+    std::vector<uint8_t> serialize() const;
     /// Serializes the packet into the given vector.
     /// 
     /// Returns the number of bytes written into the buffer.
-    size_t serialize_into(std::vector<uint8_t>& buf);
+    size_t serialize_into(std::vector<uint8_t>& buf) const;
     /// Deserializes a packet from the given bytes.
     static std::optional<BBNowPubPacket> deserialize(std::span<const uint8_t> bytes);