mip_packet.hpp

Go to the documentation of this file.

#pragma once
 
#include "mip_field.hpp"
 
#include <mip/mip_packet.h>
 
#include <microstrain/serialization.hpp>
 
#include <assert.h>
#include <cstring>
 
 
namespace mip
{
 
 
class PacketView : public C::mip_packet_view
{
public:
    enum class Index : uint8_t
    {
        SYNC_1   = C::MIP_PACKET_INDEX_SYNC_1,
        SYNC_2   = C::MIP_PACKET_INDEX_SYNC_2,
        DESC_SET = C::MIP_PACKET_INDEX_DESC_SET,
        LENGTH   = C::MIP_PACKET_INDEX_LENGTH,
        PAYLOAD  = C::MIP_PACKET_INDEX_PAYLOAD
    };
 
    static constexpr uint8_t SYNC_1 = C::MIP_SYNC_1;
    static constexpr uint8_t SYNC_2 = C::MIP_SYNC_2;
 
    static constexpr size_t HEADER_LENGTH      = C::MIP_PACKET_HEADER_LENGTH;
    static constexpr size_t CHECKSUM_LENGTH    = C::MIP_PACKET_CHECKSUM_LENGTH;
    static constexpr size_t LENGTH_MIN         = C::MIP_PACKET_LENGTH_MIN;
    static constexpr size_t LENGTH_MAX         = C::MIP_PACKET_LENGTH_MAX;
    static constexpr size_t PAYLOAD_LENGTH_MIN = C::MIP_PACKET_PAYLOAD_LENGTH_MIN;
    static constexpr size_t PAYLOAD_LENGTH_MAX = C::MIP_PACKET_PAYLOAD_LENGTH_MAX;
 
    class FieldIterator;
 
    PacketView(uint8_t* buffer, size_t bufferSize, uint8_t descriptorSet) { C::mip_packet_create(this, buffer, bufferSize, descriptorSet); }
    PacketView(const uint8_t* buffer, size_t length) { C::mip_packet_from_buffer(this, const_cast<uint8_t*>(buffer), length); }
    PacketView(const C::mip_packet_view* other) { std::memcpy(static_cast<C::mip_packet_view*>(this), other, sizeof(*this)); }
    PacketView(const C::mip_packet_view& other) { std::memcpy(static_cast<C::mip_packet_view*>(this), &other, sizeof(*this)); }
 
    PacketView(microstrain::U8ArrayView buffer, uint8_t descriptorSet) { C::mip_packet_create(this, buffer.data(), buffer.size(), descriptorSet); }
 
    PacketView(microstrain::ConstU8ArrayView buffer) { C::mip_packet_from_buffer(this, const_cast<uint8_t*>(buffer.data()), buffer.size()); }
 
    //
    // C function wrappers
    //
 
    // General
    bool isSane() const { return C::mip_packet_is_sane(this); }    
    bool isValid() const { return C::mip_packet_is_valid(this); }  
    bool isEmpty() const { return C::mip_packet_is_empty(this); }  
    bool isData() const { return C::mip_packet_is_data(this); }    
 
    uint8_t descriptorSet()  const { return C::mip_packet_descriptor_set(this);  }  
 
    // Entire buffer
    microstrain::ConstU8ArrayView buffer() const { return {C::mip_packet_buffer  (this), C::mip_packet_buffer_length(this)}; } 
    microstrain::U8ArrayView      buffer_w()     { return {C::mip_packet_buffer_w(this), C::mip_packet_buffer_length(this)}; } 
    uint_least16_t                bufferLength()   const { return C::mip_packet_buffer_length(this);   }  
    int                           remainingSpace() const { return C::mip_packet_remaining_space(this); }  
 
    // Entire packet
    microstrain::ConstU8ArrayView data()    const { return {C::mip_packet_data(this), C::mip_packet_total_length(this)}; }  
    uint_least16_t                totalLength()    const { return C::mip_packet_total_length(this);    }  
 
    // Payload
    microstrain::ConstU8ArrayView payload() const { return {C::mip_packet_payload(this),   C::mip_packet_payload_length(this)}; }  
    microstrain::U8ArrayView      payload_w()     { return {C::mip_packet_payload_w(this), C::mip_packet_payload_length(this)}; }  
    uint8_t                       payloadLength()  const { return C::mip_packet_payload_length(this);  }  
 
    // Checksum
    uint16_t checksumValue() const { return C::mip_packet_checksum_value(this); }     
    uint16_t computeChecksum() const { return C::mip_packet_compute_checksum(this); } 
 
    // Packet building
    bool addField(uint8_t fieldDescriptor, const uint8_t* payload, uint8_t payloadLength) { return C::mip_packet_add_field(this, fieldDescriptor, payload, payloadLength); }  
    Serializer createField(uint8_t fieldDescriptor, uint8_t length) { uint8_t* ptr; if(C::mip_packet_create_field(this, fieldDescriptor, length, &ptr) < 0) length =0; return Serializer{ptr, length}; }
 
    void finalize() { C::mip_packet_finalize(this); }  
 
    void reset(uint8_t descSet) { C::mip_packet_reset(this, descSet); }  
    void reset() { reset(descriptorSet()); }  
 
    //
    // C++ additions
    //
 
    uint8_t  dataAt(const size_t i) const { assert(i < totalLength()); return payload()[i];   }
    uint8_t& dataAt(const size_t i)       { assert(i < totalLength()); return payload_w()[i]; }
    uint8_t  dataAt(const Index i)  const { return dataAt(static_cast<size_t>(i)); }
    uint8_t& dataAt(const Index i)        { return dataAt(static_cast<size_t>(i)); }
 
    uint8_t  payloadAt(const size_t i) const { assert(i < payloadLength()); return                      C::mip_packet_payload(this)[i];  }  
    uint8_t& payloadAt(const size_t i)       { assert(i < payloadLength()); return const_cast<uint8_t&>(C::mip_packet_payload(this)[i]); }  
 
    bool addField(uint8_t fieldDescriptor, microstrain::ConstU8ArrayView payload) { return addField(fieldDescriptor, payload.data(), uint8_t(payload.size())); }
 
    bool addField(const FieldView& field) { return addField(field.fieldDescriptor(), field.payload()); }
 
 
    class AllocatedField : public Serializer
    {
    public:
        AllocatedField(mip::PacketView& packet, uint8_t* buffer, size_t space) : Serializer(buffer, space), m_packet(packet) {}
        //AllocatedField(const AllocatedField&) = delete;
        AllocatedField& operator=(const AllocatedField&) = delete;
 
        uint8_t* allocateOrCancel(size_t length)
        {
            uint8_t* ptr = getPtrAndAdvance(length);
            if(!ptr)
                cancel();
            return ptr;
        }
 
        bool commit()
        {
            assert(capacity() <= FieldView::PAYLOAD_LENGTH_MAX);
 
            bool ok = isOk();
 
            if(ok)
                ok &= C::mip_packet_update_last_field_length(&m_packet, basePointer(), (uint8_t) usedLength()) >= 0;
 
            if(!ok && basePointer())
                C::mip_packet_cancel_last_field(&m_packet, basePointer());
 
            return ok;
        }
 
        void cancel() { if(basePointer()) C::mip_packet_cancel_last_field(&m_packet, basePointer()); }
 
    private:
        PacketView& m_packet;
    };
 
    AllocatedField createField(uint8_t fieldDescriptor)
    {
        uint8_t* ptr;
        size_t max_size = std::max<int>(0, C::mip_packet_create_field(this, fieldDescriptor, 0, &ptr));
        return {*this, ptr, max_size};
    }
 
    //uint8_t  operator[](unsigned int index) const { return payloadAt(index); }
    //uint8_t& operator[](unsigned int index)       { return payloadAt(index); }
 
    //
    // Additional functions which have no C equivalent
    //
 
    FieldIterator begin() const { return firstField(); }
 
#if __cpp_range_based_for >= 201603
    // After 201603, for loops allow different classes for begin and end.
    // Using nullptr is simpler and more efficient than creating an end iterator.
    std::nullptr_t end() const { return nullptr; }
#else
    FieldIterator end() const { return FieldView(); }
#endif
 
    FieldView firstField() const { return {C::mip_field_first_from_packet(this)}; }
 
    template<class FieldType>
    bool addField(const FieldType& field, uint8_t fieldDescriptor=INVALID_FIELD_DESCRIPTOR)
    {
        if( fieldDescriptor == INVALID_FIELD_DESCRIPTOR )
            fieldDescriptor = FieldType::FIELD_DESCRIPTOR;
 
        AllocatedField buffer = createField(fieldDescriptor);
        buffer.insert(field);
        return buffer.commit();
    }
 
    template<class FieldType>
    static PacketView createFromField(microstrain::U8ArrayView packetBuffer, const FieldType& field, uint8_t fieldDescriptor=INVALID_FIELD_DESCRIPTOR)
    {
        if( fieldDescriptor == INVALID_FIELD_DESCRIPTOR )
            fieldDescriptor = FieldType::FIELD_DESCRIPTOR;
        PacketView packet(packetBuffer, FieldType::DESCRIPTOR_SET);
        packet.addField<FieldType>(field, fieldDescriptor);
        packet.finalize();
        return packet;
    }
 
 
    class FieldIterator
    {
    public:
        FieldIterator() {}
 
        FieldIterator(const FieldView& first) : mField(first) {}
 
        bool operator==(const FieldIterator& other) const {
            // Required to make invalid fields equivalent for range-based for loop
            if( !mField.isValid() && !other.mField.isValid() )
                return true;
            return (
                mField.descriptorSet()   == other.mField.descriptorSet()   &&
                mField.fieldDescriptor() == other.mField.fieldDescriptor() &&
                mField.payload()  == other.mField.payload()
            );
        }
        bool operator!=(const FieldIterator& other) const { return !(*this == other); }
 
        bool operator==(std::nullptr_t) const { return !mField.isValid(); }
        bool operator!=(std::nullptr_t) const { return mField.isValid(); }
 
        const FieldView& operator*() const { return mField; }
 
        FieldIterator& operator++() { mField.next(); return *this; }
 
    private:
        FieldView mField;
    };
 
    bool copyPacketTo(microstrain::U8ArrayView buffer) const
    {
        assert(isSane());
        microstrain::ConstU8ArrayView packet = this->data();
        if(packet.size() > buffer.size())
            return false;
        std::memcpy(buffer.data(), packet.data(), packet.size());
        return true;
    }
};
 
 
template<size_t BufferSize>
class SizedPacketBuf : public PacketView
{
    static_assert(BufferSize >= LENGTH_MIN, "BufferSize must be at least PacketView::LENGTH_MIN bytes");
 
public:
    explicit SizedPacketBuf(uint8_t descriptorSet=INVALID_DESCRIPTOR_SET) : PacketView(mData, sizeof(mData), descriptorSet) {}
 
    explicit SizedPacketBuf(microstrain::ConstU8ArrayView data) : PacketView(mData, sizeof(mData)) { copyFrom(data); }
 
    explicit SizedPacketBuf(const PacketView& packet) : PacketView(mData, sizeof(mData)) { copyFrom(packet); }
 
    SizedPacketBuf(const SizedPacketBuf& other) : PacketView(mData, sizeof(mData)) { copyFrom(other); }
 
 
    template<size_t OtherSize>
    explicit SizedPacketBuf(const SizedPacketBuf<OtherSize>& other) : PacketView(mData, sizeof(mData)) { copyFrom(other); };
 
    SizedPacketBuf& operator=(const SizedPacketBuf& other) { copyFrom(other); return *this; }
 
    template<size_t OtherSize>
    SizedPacketBuf& operator=(const SizedPacketBuf<OtherSize>& other) { copyFrom(other); return *this; }
 
    template<class FieldType>
    SizedPacketBuf(
        const FieldType& field,
        uint8_t fieldDescriptor=INVALID_FIELD_DESCRIPTOR,
        typename std::enable_if<std::is_class<FieldType>::value, void>::type* = nullptr
    ) : PacketView(mData, sizeof(mData))
    {
        createFromField<FieldType>({mData, sizeof(mData)}, field, fieldDescriptor);
    }
 
 
    PacketView ref() { return *this; }
 
    const PacketView& ref() const { return *this; }
 
    microstrain::ArrayView<uint8_t, BufferSize> buffer() { return {mData}; }
 
    void copyFrom(microstrain::ConstU8ArrayView data) { assert(data.size() <= sizeof(mData)); std::memcpy(mData, data.data(), data.size()); }
 
    void copyFrom(const PacketView& packet) { assert(packet.isSane()); copyFrom(packet.data()); }
 
private:
    uint8_t mData[BufferSize];
};
 
typedef SizedPacketBuf<PacketView::LENGTH_MAX> PacketBuf;
 
 
 
} // namespace mip