120 lines
2.9 KiB
C++
120 lines
2.9 KiB
C++
#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;
|
|
};
|