132 lines
4.6 KiB
C++
132 lines
4.6 KiB
C++
#include "bbnow_pub.hpp"
|
|
#include <cassert>
|
|
#include <esp_log.h>
|
|
|
|
const static char TAG[] = "bbnow_pub";
|
|
|
|
size_t BBNowPubPacket::serialize_size() {
|
|
switch (this->packet_type) {
|
|
case BBNowPubPacketType::NONE:
|
|
return -1;
|
|
case BBNowPubPacketType::DISCOVERY:
|
|
return BBNowPubPacket::DISCOVERY_PACKET_SIZE;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
std::vector<uint8_t> BBNowPubPacket::serialize() {
|
|
auto buf = std::vector<uint8_t>();
|
|
|
|
if (this->serialize_size() == -1) {
|
|
return buf;
|
|
}
|
|
|
|
this->serialize_into(buf);
|
|
return buf;
|
|
}
|
|
|
|
size_t BBNowPubPacket::serialize_into(std::vector<uint8_t>& buf) {
|
|
size_t initial_size = buf.size();
|
|
size_t size = this->serialize_size();
|
|
if (size == -1) {
|
|
return 0;
|
|
}
|
|
buf.reserve(initial_size + size);
|
|
|
|
// push magic number
|
|
static_assert(sizeof(PUB_MAGIC_NUMBER) == 4, "expect magic number to be 4 bytes");
|
|
buf.push_back((PUB_MAGIC_NUMBER >> 24) & 0xFF);
|
|
buf.push_back((PUB_MAGIC_NUMBER >> 16) & 0xFF);
|
|
buf.push_back((PUB_MAGIC_NUMBER >> 8) & 0xFF);
|
|
buf.push_back((PUB_MAGIC_NUMBER >> 0) & 0xFF);
|
|
|
|
// push packet type
|
|
static_assert(sizeof(this->packet_type) == 2, "expect type field 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);
|
|
|
|
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);
|
|
buf.push_back((data >> 8) & 0xFF);
|
|
buf.push_back((data >> 0) & 0xFF);
|
|
break;
|
|
}
|
|
|
|
assert(buf.size() - initial_size == size);
|
|
return size;
|
|
}
|
|
|
|
/// Deserializes a packet from the given bytes.
|
|
std::optional<BBNowPubPacket> BBNowPubPacket::deserialize(std::span<const uint8_t> bytes) {
|
|
// first check for magic number and type
|
|
static_assert(sizeof(PUB_MAGIC_NUMBER) + sizeof(BBNowPubPacketType) == 6, "expect magic and type to be 6 bytes");
|
|
if (bytes.size() < 6) {
|
|
// not enough room for magic number and type
|
|
return std::nullopt;
|
|
}
|
|
|
|
static_assert(sizeof(PUB_MAGIC_NUMBER) == 4, "expect magic number to be 4 bytes");
|
|
uint32_t magic_number =
|
|
(uint32_t)(bytes[0] << 24) |
|
|
(uint32_t)(bytes[1] << 16) |
|
|
(uint32_t)(bytes[2] << 8) |
|
|
(uint32_t)(bytes[3] << 0);
|
|
|
|
if (magic_number != PUB_MAGIC_NUMBER) {
|
|
ESP_LOGD(TAG, "packet missing magic number. discarding");
|
|
return std::nullopt;
|
|
}
|
|
|
|
// magic number matches! this is a blk_box pub packet!
|
|
BBNowPubPacket packet;
|
|
|
|
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) {
|
|
ESP_LOGW(TAG, "BlkBoxPubPacketType was NONE! discarding");
|
|
return std::nullopt;
|
|
}
|
|
if (packet.packet_type > BBNowPubPacketType::MAX) {
|
|
ESP_LOGW(TAG, "BlkBoxPubPacketType not in range! discarding");
|
|
return std::nullopt;
|
|
}
|
|
|
|
// right now, the size is dependent only on the `packet_type` field, so we can
|
|
// check to see if the packet is big enough now.
|
|
size_t expected_size = packet.serialize_size();
|
|
if (bytes.size() < expected_size) {
|
|
ESP_LOGW(TAG, "packet not long enough for pub packet of type %d", packet_type);
|
|
return std::nullopt;
|
|
}
|
|
if (bytes.size() > expected_size) {
|
|
ESP_LOGW(TAG, "pub packet longer than expected for packet of type %d. decoding anyway", packet_type);
|
|
}
|
|
|
|
switch (packet.packet_type) {
|
|
case BBNowPubPacketType::NONE:
|
|
break;
|
|
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) {
|
|
ESP_LOGW(TAG, "BlkBoxExpantion not in range! discarding");
|
|
return std::nullopt;
|
|
}
|
|
break;
|
|
}
|
|
return packet;
|
|
}
|
|
|
|
BBNowPubPacket BBNowPubPacket::new_discovery(BlkBoxNowPubDiscoveryData data) {
|
|
BBNowPubPacket packet;
|
|
packet.packet_type = BBNowPubPacketType::DISCOVERY;
|
|
packet.packet_data.discovery = data;
|
|
return packet;
|
|
}
|