blk_box_common/blk_box_common.cpp

#include "blk_box_common.hpp"
#include <cassert>
#include <cstddef>
#include <cstring>
#include <optional>
#include <stdint.h>
#include <vector>
#include <esp_log.h>

const static char TAG[] = "blk_box_common";

size_t BlkBoxNowPubPacket::serialize_size() {
    switch (this->packet_type) {
        case BlkBoxNowPubPacketType::NONE:
            return -1;
        case BlkBoxNowPubPacketType::DISCOVERY:
            return BlkBoxNowPubPacket::DISCOVERY_PACKET_SIZE;
    }
    return -1;
}

std::vector<uint8_t> BlkBoxNowPubPacket::serialize() {
    size_t size = this->serialize_size();
    if (size == -1) {
        return std::vector<uint8_t>(0);
    }
    auto buf = std::vector<uint8_t>();
    buf.reserve(size);

    this->serialize_into(buf);
    return buf;
}

/// Serializes the packet into the given vector.
size_t BlkBoxNowPubPacket::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) == 1, "expect type field to be 1 byte");
    buf.push_back(static_cast<uint8_t>(this->packet_type));

    switch (this->packet_type) {
        case BlkBoxNowPubPacketType::NONE:
            break;
        case BlkBoxNowPubPacketType::DISCOVERY:
            static_assert(sizeof(this->packet_data.discovery) == 2, "expect discovery data to be 2 byte");
            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<BlkBoxNowPubPacket> BlkBoxNowPubPacket::deserialize(std::span<const uint8_t> bytes) {
    // first check for magic number and type
    if (bytes.size() < 5) {
        // not enough room for magic number and type
        return std::nullopt;
    }
    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!
    BlkBoxNowPubPacket packet;

    uint8_t packet_type = bytes[4];
    packet.packet_type = static_cast<BlkBoxNowPubPacketType>(packet_type);
    if (packet.packet_type > BlkBoxNowPubPacketType::MAX) {
        ESP_LOGW(TAG, "BlkBoxPubPacketType was NONE! discarding");
        return std::nullopt;
    }
    if (packet.packet_type > BlkBoxNowPubPacketType::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 BlkBoxNowPubPacketType::NONE:
            break;
        case BlkBoxNowPubPacketType::DISCOVERY:
            uint16_t device = ((uint16_t)bytes[5] << 8) | ((uint16_t)bytes[6]);
            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;
}

BlkBoxNowPubPacket BlkBoxNowPubPacket::new_discovery(BlkBoxNowPubDiscoveryData data) {
    BlkBoxNowPubPacket packet;
    packet.packet_type = BlkBoxNowPubPacketType::DISCOVERY;
    packet.packet_data.discovery = data;
    return packet;
}