65 lines
1.4 KiB
C++
65 lines
1.4 KiB
C++
#include "packet.h"
|
|
|
|
|
|
/* PUBLIC
|
|
----------------------------------------*/
|
|
// builds a packet from raw data and returns the error code
|
|
uint8_t Packet::read(uint8_t buf[], uint8_t size){
|
|
// 1. fail on invalid size
|
|
if( size < 1 ) return PKTREAD_EMPTY;
|
|
if( size > PROTO_SIZE ) return PKTREAD_OVERFLOW;
|
|
|
|
// 2. extract packet type
|
|
type = buf[0] == 0;
|
|
|
|
// 3. extract depending on type
|
|
return type ? read_discover(buf, size) : read_message(buf, size);
|
|
|
|
};
|
|
|
|
// writes the binary representation of the packet
|
|
uint8_t Packet::write(uint8_t buf[], uint8_t size){
|
|
/* TODO */
|
|
return 0;
|
|
};
|
|
|
|
|
|
/* PRIVATE
|
|
----------------------------------------*/
|
|
uint8_t Packet::read_discover(uint8_t buf[], uint8_t size){
|
|
// 1. fail on invalid size
|
|
if( size != DISCOVER_SIZE )
|
|
return PKTREAD_INVALID_DISCOVER_FORMAT;
|
|
|
|
// 2. extract fixed
|
|
dsc.opcode = buf[0];
|
|
dsc.wave = buf[1];
|
|
dsc.dist = buf[2];
|
|
|
|
return PKTREAD_OK;
|
|
};
|
|
|
|
uint8_t Packet::read_message(uint8_t buf[], uint8_t size){
|
|
// 1. fail on invalid size
|
|
if( size < MESSAGE_MIN_SIZE || size > MESSAGE_MAX_SIZE )
|
|
return PKTREAD_INVALID_MESSAGE_FORMAT;
|
|
|
|
// 2. extract fixed
|
|
msg.opcode = buf[0];
|
|
msg.dist = buf[1];
|
|
msg.ttl = buf[2];
|
|
msg.size = buf[3];
|
|
|
|
// 3. check message size
|
|
if( size - 4 - 1 != msg.size )
|
|
return PKTREAD_INVALID_MESSAGE_FORMAT;
|
|
|
|
// 4. extract message
|
|
strncpy(
|
|
(char*) msg.data,
|
|
(char*)buf+4,
|
|
msg.size
|
|
);
|
|
|
|
return PKTREAD_OK;
|
|
}; |