packet.c

#include "packet.h"
#include "commands.h"
#include "bitwise.h"

#include <string.h>


/* Combine metadata and data to form a wire-sendable packet.
 * Returns the size of the encoded packet
 */
ssize_t EncodePacket(
        uint8_t * packet,           /* Buffer to encode into */
        size_t packet_size,         /* Max buffer size */
        const struct metadata * m,  /* Metadata to encode */
        const uint8_t * data)       /* Data to encode */
{
	if (packet_size < (HDR_SIZE + CSUM_SIZE + m->data_len)) {
		return -1;
	}

	packet[BEGIN] = BEGIN_CHAR;
	packet[MTYPE_L] = LSByte16(m->msg_type);
	packet[MTYPE_H] = MSByte16(m->msg_type);
	packet[ID_L] = LSByte16(m->msg_id);
	packet[ID_H] = MSByte16(m->msg_id);
	packet[DLEN_L] = LSByte16(m->data_len);
	packet[DLEN_H] = MSByte16(m->data_len);
	memcpy(&packet[HDR_SIZE], data, m->data_len);

	packet[HDR_SIZE + m->data_len] = PacketChecksum(
			packet, HDR_SIZE + m->data_len + CSUM_SIZE);

	return m->data_len + HDR_SIZE + CSUM_SIZE;
}


/* Break apart packet, populating metadata. Data is copied
 * into the space pointed to by data.
 * Returns the size of the data.
 */
ssize_t DecodePacket(
        struct metadata * m,        /* Decoded metadata (includes data_len)*/
        uint8_t * data,             /* Data is copied into this buffer */
        size_t data_size,           /* Max buffer size */
        const uint8_t * packet,     /* Packet to decode */
        size_t packet_size)         /* Size of packet to decode */
{
	uint8_t checkSum;
	if (packet[BEGIN] != BEGIN_CHAR) {
		return -1;
	}

	if (packet_size < ((uint8_t) HDR_SIZE + CSUM_SIZE)) {
		return -2;
	}

	m->msg_type = BytesTo16(packet[MTYPE_L], packet[MTYPE_H]);
	m->msg_id = BytesTo16(packet[ID_L], packet[ID_H]);
	m->data_len =  BytesTo16(packet[DLEN_L], packet[DLEN_H]);

	if (packet_size < (HDR_SIZE + CSUM_SIZE + m->data_len)) {
		return -3;
	}

	if (data_size < m->data_len) {
		return -4;
	}

	checkSum = PacketChecksum(packet, HDR_SIZE + m->data_len + CSUM_SIZE);
	if (checkSum != packet[HDR_SIZE + m->data_len]) {
		return -5;
	}

	memcpy(data, &packet[HDR_SIZE], m->data_len);
	return m->data_len;
}

uint8_t PacketChecksum(const uint8_t * packet, size_t packet_size)
{
	uint8_t checkSum = 0;
	for(size_t i = 0; i < packet_size - CSUM_SIZE; i++){
		checkSum ^= packet[i];
	}	
	return checkSum;
}

size_t PacketSize(const struct metadata *m)
{
	return m->data_len + HDR_SIZE + CSUM_SIZE;
}