mip_packet.c

Go to the documentation of this file.

 
#include "mip_packet.h"
#include "mip_offsets.h"
 
#include "definitions/descriptors.h"
 
#include <string.h>
#include <assert.h>
 
 
//
// Initialization
//
 
void mip_packet_from_buffer(mip_packet* packet, uint8_t* buffer, size_t length)
{
    assert(buffer != NULL);
 
    // Limit the length in case it's longer than a mip packer (or worse, longer than the buffer size field can hold)
    if( length > MIP_PACKET_LENGTH_MAX )
        length = MIP_PACKET_LENGTH_MAX;
 
    packet->_buffer        = buffer;
    packet->_buffer_length = (uint_least16_t)length;
}
 
void mip_packet_create(mip_packet* packet, uint8_t* buffer, size_t buffer_size, uint8_t descriptor_set)
{
    mip_packet_from_buffer(packet, buffer, buffer_size);
 
    if( buffer_size < MIP_PACKET_LENGTH_MIN )
    {
        assert(false); // Buffer too small!
        return;
    }
 
    packet->_buffer[MIP_INDEX_SYNC1]   = MIP_SYNC1;
    packet->_buffer[MIP_INDEX_SYNC2]   = MIP_SYNC2;
    packet->_buffer[MIP_INDEX_DESCSET] = descriptor_set;
    packet->_buffer[MIP_INDEX_LENGTH]  = 0;
}
 
 
 
//
// Accessors
//
 
uint8_t mip_packet_descriptor_set(const mip_packet* packet)
{
    return packet->_buffer[MIP_INDEX_DESCSET];
}
 
uint8_t mip_packet_payload_length(const mip_packet* packet)
{
    return packet->_buffer[MIP_INDEX_LENGTH];
}
 
uint_least16_t mip_packet_total_length(const mip_packet* packet)
{
    return mip_packet_payload_length(packet) + MIP_PACKET_LENGTH_MIN;
}
 
uint8_t* mip_packet_buffer(mip_packet* packet)
{
    return packet->_buffer;
}
 
const uint8_t* mip_packet_pointer(const mip_packet* packet)
{
    return packet->_buffer;
}
 
const uint8_t* mip_packet_payload(const mip_packet* packet)
{
    return packet->_buffer + MIP_INDEX_PAYLOAD;
}
 
uint16_t mip_packet_checksum_value(const mip_packet* packet)
{
    const uint_least16_t index = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH;
 
    return ((uint16_t)(packet->_buffer[index+0]) << 8) | (uint16_t)(packet->_buffer[index+1]);
}
 
uint16_t mip_packet_compute_checksum(const mip_packet* packet)
{
    uint8_t a = 0;
    uint8_t b = 0;
 
    // mip_packet_total_length always returns at least MIP_PACKET_LENGTH_MIN so this
    // subtraction is guaranteed to be safe.
    const uint_least16_t length = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH;
 
    for(uint_least16_t i=0; i<length; i++)
    {
        a += packet->_buffer[i];
        b += a;
    }
 
    return ((uint16_t)(a) << 8) | (uint16_t)(b);
}
 
 
bool mip_packet_is_sane(const mip_packet* packet)
{
    return packet->_buffer && (packet->_buffer_length >= MIP_PACKET_LENGTH_MIN) && (packet->_buffer_length >= mip_packet_payload_length(packet)+MIP_PACKET_LENGTH_MIN);
}
 
bool mip_packet_is_valid(const mip_packet* packet)
{
    if( !mip_packet_is_sane(packet) || (mip_packet_descriptor_set(packet) == 0x00) )
        return false;
 
    const uint16_t listed_checksum = mip_packet_checksum_value(packet);
    const uint16_t computed_checksum = mip_packet_compute_checksum(packet);
 
    return listed_checksum == computed_checksum;
}
 
bool mip_packet_is_empty(const mip_packet* packet)
{
    if( !mip_packet_is_sane(packet) )
        return true;
 
    return mip_packet_payload_length(packet) == 0;
}
 
 
uint_least16_t mip_packet_buffer_size(const mip_packet* packet)
{
    return packet->_buffer_length;
}
 
int mip_packet_remaining_space(const mip_packet* packet)
{
    return mip_packet_buffer_size(packet) - mip_packet_total_length(packet);
}
 
bool mip_packet_is_data(const mip_packet* packet)
{
    return mip_is_data_descriptor_set(mip_packet_descriptor_set(packet));
}
 
//
// Packet Building
//
 
bool mip_packet_add_field(mip_packet* packet, uint8_t field_descriptor, const uint8_t* payload, uint8_t payload_length)
{
    uint8_t* payload_buffer;
    int remaining = mip_packet_alloc_field(packet, field_descriptor, payload_length, &payload_buffer);
    if( remaining < 0 )
        return false;
 
    memcpy(payload_buffer, payload, payload_length);
 
    return true;
}
 
int mip_packet_alloc_field(mip_packet* packet, uint8_t field_descriptor, uint8_t payload_length, uint8_t** const payload_ptr_out)
{
    assert(payload_ptr_out != NULL);
    assert( payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX );
 
    const int remaining = mip_packet_remaining_space(packet);
 
    const uint8_t field_length = MIP_FIELD_HEADER_LENGTH + payload_length;
 
    *payload_ptr_out = NULL;
 
    if( field_length <= remaining )
    {
        const uint_least16_t field_index = MIP_HEADER_LENGTH + mip_packet_payload_length(packet);
 
        packet->_buffer[MIP_INDEX_LENGTH] += field_length;
 
        packet->_buffer[field_index+MIP_INDEX_FIELD_LEN]  = field_length;
        packet->_buffer[field_index+MIP_INDEX_FIELD_DESC] = field_descriptor;
 
        *payload_ptr_out = &packet->_buffer[field_index + MIP_INDEX_FIELD_PAYLOAD];
    }
 
    return remaining - field_length;
}
 
int mip_packet_realloc_last_field(mip_packet* packet, uint8_t* payload_ptr, uint8_t new_payload_length)
{
    assert(payload_ptr != NULL);
    assert( new_payload_length <= MIP_FIELD_PAYLOAD_LENGTH_MAX );
 
    uint8_t* field_ptr = payload_ptr - MIP_INDEX_FIELD_PAYLOAD;
    const uint8_t old_field_length = field_ptr[MIP_INDEX_FIELD_LEN];
    const uint8_t new_field_length = new_payload_length + MIP_FIELD_HEADER_LENGTH;
 
    const int delta_length = new_field_length - old_field_length;
 
    const int remaining = mip_packet_remaining_space(packet) - delta_length;
 
    if( remaining >= 0 )
    {
        field_ptr[MIP_INDEX_FIELD_LEN] = new_field_length;
        packet->_buffer[MIP_INDEX_LENGTH] += (int8_t)delta_length;
    }
 
    return remaining;
}
 
int mip_packet_cancel_last_field(mip_packet* packet, uint8_t* payload_ptr)
{
    assert(payload_ptr != NULL);
 
    uint8_t* field_ptr = payload_ptr - MIP_INDEX_FIELD_PAYLOAD;
    const uint8_t old_field_length = field_ptr[MIP_INDEX_FIELD_LEN];
 
    packet->_buffer[MIP_INDEX_LENGTH] -= old_field_length;
 
    return mip_packet_remaining_space(packet);
}
 
void mip_packet_finalize(mip_packet* packet)
{
    uint16_t checksum = mip_packet_compute_checksum(packet);
    uint_least16_t length = mip_packet_total_length(packet) - MIP_CHECKSUM_LENGTH;
 
    packet->_buffer[length+0] = checksum >> 8;
    packet->_buffer[length+1] = checksum & 0xFF;
}
 
 
void mip_packet_reset(mip_packet* packet, uint8_t descriptor_set)
{
    mip_packet_create(packet, mip_packet_buffer(packet), mip_packet_buffer_size(packet), descriptor_set);
}