opportunistic-xbee/packet.cpp

#include "packet.h"


/* PUBLIC
----------------------------------------*/
// builds a packet from raw data and returns the error code
uint8_t Packet::read(uint8_t* buf, const size_t size){
	// 1. fail on invalid size
	if( size < 1          ) return PKTREAD_EMPTY;
	if( size > PROTO_SIZE ) return PKTREAD_OVERFLOW;  

	// 2. extract packet type
	opcode = buf[0];

	// 3. extract depending on type
	return (opcode == 0) ? read_discover(buf, size) : read_message(buf, size);

};

// writes the binary representation of the packet
size_t Packet::write(uint8_t* buf){
	if( opcode == 0 )
		return write_discover(buf);

	return write_message(buf);
};

/* GETTERS / SETTERS
----------------------------------------*/
uint8_t Packet::getOpcode()           { return opcode; }
void Packet::setOpcode(uint8_t value) { opcode = value; dsc.opcode = value; msg.opcode = value; }

uint8_t Packet::getWave()           { return dsc.wave; }
void Packet::setWave(uint8_t value) { dsc.wave = value; }

uint8_t Packet::getSender()           { return msg.sender; }
void Packet::setSender(uint8_t value) { msg.sender = value; }

uint8_t Packet::getDist()           { return (opcode == 0) ? dsc.dist : msg.dist; }
void Packet::setDist(uint8_t value) { if(opcode == 0) dsc.dist = value; else msg.dist = value; }

uint8_t Packet::getTTL()           { return msg.ttl; }
void Packet::setTTL(uint8_t value) { msg.ttl = value; }

uint8_t Packet::getSize()           { return msg.size; }

uint8_t* Packet::getData(){ return msg.data; }
void Packet::setData(uint8_t *buffer) {
	if( strlen(buffer) >= MESSAGE_MAX_PAYLOAD )
		return;

	msg.size = strlen(buffer);

	strncpy(msg.data, buffer, MESSAGE_MAX_PAYLOAD);
}



/* PRIVATE
----------------------------------------*/
uint8_t Packet::read_discover(uint8_t *buf, const size_t size){
	// 1. fail on invalid size
	if( size != DISCOVER_SIZE )
		return PKTREAD_INVALID_DISCOVER_FORMAT;

	dsc.opcode = buf[0];
	dsc.wave   = buf[1];
	dsc.dist   = buf[2];

	return PKTREAD_OK;
};


size_t Packet::write_discover(uint8_t *buf){
	buf[0] = dsc.opcode;
	buf[1] = dsc.wave;
	buf[2] = dsc.dist;

	return 3;
};


uint8_t Packet::read_message(uint8_t *buf, const size_t size){
	// 1. fail on invalid size
	if( size < PROTO_MIN_SIZE || size > PROTO_SIZE )
		return PKTREAD_INVALID_MESSAGE_FORMAT;

	// 2. fill values
	msg.opcode = buf[0];
	msg.sender = buf[1];
	msg.dist   = buf[2];
	msg.ttl    = buf[3];
	msg.size   = buf[4];

	// 3. check message size
	if( size - 5 != msg.size )
		return PKTREAD_INVALID_MESSAGE_FORMAT;

  // 4. memset data
  //memset(msg.data,0,MESSAGE_MAX_PAYLOAD);  

	// 5. extract message
	strncpy(msg.data, buf+5, msg.size);
  memset(msg.data+msg.size,0,MESSAGE_MAX_PAYLOAD-msg.size);

	return PKTREAD_OK;
};

size_t Packet::write_message(uint8_t *buf){
	buf[0] = msg.opcode;
	buf[1] = msg.sender;
	buf[2] = msg.dist;
	buf[3] = msg.ttl;
	buf[4] = msg.size;

	strncpy(buf+5, msg.data, MESSAGE_MAX_PAYLOAD);

	return 5 + msg.size;
};