uart_framing.hpp

#pragma once
 
#include <pb_decode.h>
 
#include <cstdint>
#include <vector>
 
#ifdef PLATFORMIO_BUILD
#include <power_frame_msg_platformio.h>
#else  // PLATFORMIO_BUILD
#include "proto/message_translation/power_frame_msg.hpp"
extern "C"
{
#include "proto/power_frame_msg.nanopb.h"
}
#endif  // PLATFORMIO_BUILD
 
const uint8_t START_END_FLAG_BYTE = 0x00;
 
namespace
{
    uint16_t crc16(const std::vector<uint8_t>& data, uint16_t length)
    {
        uint8_t x;
        uint16_t crc = 0xFFFF;
        int i        = 0;
 
        while (length--)
        {
            x = static_cast<uint8_t>(crc >> 8u) ^ data[i++];
            x ^= static_cast<uint8_t>(x >> 4u);
            crc = static_cast<uint16_t>((crc << 8u) ^ (x << 12u) ^ (x << 5u) ^ x);
        }
 
        return crc;
    }
 
    bool verifyLengthAndCrc(const TbotsProto_PowerFrame& frame)
    {
        uint16_t expected_length;
        std::vector<uint8_t> bytes;
        switch (frame.which_power_msg)
        {
            case TbotsProto_PowerFrame_power_control_tag:
                expected_length = TbotsProto_PowerPulseControl_size;
                bytes           = serializeToVector(frame.power_msg.power_control);
                break;
            case TbotsProto_PowerFrame_power_status_tag:
                expected_length = TbotsProto_PowerStatus_size;
                bytes           = serializeToVector(frame.power_msg.power_status);
                break;
            default:
                return false;
        }
        uint16_t expected_crc = crc16(bytes, static_cast<uint16_t>(frame.length));
        return frame.crc == expected_crc && frame.length == expected_length;
    }
 
    std::vector<uint8_t> cobsEncoding(const std::vector<uint8_t>& to_encode)
    {
        auto encoded = std::vector<uint8_t>();
        encoded.emplace_back(START_END_FLAG_BYTE);
 
        size_t overhead_location = 1;
        uint8_t overhead         = 0x01;
 
        for (auto byte : to_encode)
        {
            if (byte == START_END_FLAG_BYTE)
            {
                encoded.insert(encoded.begin() + overhead_location, overhead);
                overhead_location = encoded.size();
                overhead          = 0x01;
            }
            else
            {
                encoded.emplace_back(byte);
                if (++overhead == 0xFF)
                {
                    encoded.insert(encoded.begin() + overhead_location, overhead);
                    overhead_location = encoded.size();
                    overhead          = 0x01;
                }
            }
        }
        encoded.insert(encoded.begin() + overhead_location, overhead);
        encoded.emplace_back(START_END_FLAG_BYTE);
 
        return encoded;
    }
 
    bool cobsDecoding(const std::vector<uint8_t>& to_decode,
                      std::vector<uint8_t>& decoded)
    {
        if (to_decode.front() != START_END_FLAG_BYTE ||
            to_decode.back() != START_END_FLAG_BYTE)
        {
            return false;
        }
 
        uint16_t i = 1;
        while (i < to_decode.size())
        {
            uint8_t overhead = to_decode[i++];
            // Check that overhead does not point to past the end of the vector
            if (overhead + i > to_decode.size())
            {
                return false;
            }
            // Check that instances of the START_END_FLAG_BYTE are not in the middle of
            // the vector
            if (overhead == START_END_FLAG_BYTE && i != to_decode.size())
            {
                return false;
            }
            for (uint16_t j = 1; i < to_decode.size() && j < overhead; j++)
            {
                decoded.emplace_back(to_decode[i++]);
            }
            if (overhead < 0xFF && i != to_decode.size() - 1 &&
                overhead != START_END_FLAG_BYTE)
            {
                decoded.emplace_back(START_END_FLAG_BYTE);
            }
        }
 
        return true;
    }
}  // anonymous namespace
 
template <typename T>
TbotsProto_PowerFrame createUartFrame(const T& power_msg)
{
    TbotsProto_PowerFrame frame = TbotsProto_PowerFrame_init_default;
    auto buffer                 = serializeToVector(power_msg);
    frame.length                = static_cast<uint32_t>(buffer.size());
    frame.crc                   = crc16(buffer, static_cast<uint16_t>(frame.length));
    setPowerMsg(frame, power_msg);
    return frame;
}
 
std::vector<uint8_t> inline marshallUartPacket(const TbotsProto_PowerFrame& frame)
{
    auto bytes = serializeToVector(frame);
    return cobsEncoding(bytes);
}
 
bool inline unmarshalUartPacket(const std::vector<uint8_t>& data,
                                TbotsProto_PowerFrame& frame)
{
    std::vector<uint8_t> decoded;
    if (!cobsDecoding(data, decoded))
    {
        return false;
    }
    if (decoded.size() != TbotsProto_PowerFrame_size)
    {
        return false;
    }
    frame = TbotsProto_PowerFrame_init_default;
    pb_istream_t stream =
        pb_istream_from_buffer(static_cast<uint8_t*>(decoded.data()), decoded.size());
    if (!pb_decode(&stream, TbotsProto_PowerFrame_fields, &frame))
    {
        return false;
    }
    return verifyLengthAndCrc(frame);
}
template <typename T>
size_t getMarshalledSize(const T& power_msg)
{
    auto frame  = createUartFrame(power_msg);
    auto buffer = serializeToVector(frame);
    return cobsEncoding(buffer).size();
}