diff --git a/groundStation/src/backend/common/bitwise.h b/groundStation/src/backend/common/bitwise.h new file mode 100644 index 0000000000000000000000000000000000000000..54f1de43d824342485cd94c92561341e3db23252 --- /dev/null +++ b/groundStation/src/backend/common/bitwise.h @@ -0,0 +1,11 @@ +#ifndef _bitwise_h +#define _bitwise_h + +/* Bit shifting for endianness of 16-bit numbers */ +#define LSByte16(x) (x & 0xff) +#define MSByte16(x) ((x >> 8) & 0xff) + +/* Build a 16-bit integer out of two bytes */ +#define BytesTo16(lsb, msb) ((lsb & 0xff) | ((msb << 8) & 0xff)) + +#endif diff --git a/groundStation/src/backend/common/examples.c b/groundStation/src/backend/common/examples.c index 7d0ee50aef6df1b4c7ef349fd707521ee68213eb..036ddeb3c0443a88d42568cbcce6250f7e20ca83 100644 --- a/groundStation/src/backend/common/examples.c +++ b/groundStation/src/backend/common/examples.c @@ -4,6 +4,11 @@ #include <sys/types.h> + +/* This example is close to how the ground station will handle things. It + * shouldn't be too hard to adapt it to work on the quad side. + */ + static int next_msg_id = 0; int SendSetControl() diff --git a/groundStation/src/backend/common/packet.c b/groundStation/src/backend/common/packet.c new file mode 100644 index 0000000000000000000000000000000000000000..3d3c78cd3677eed25c8c30dedde2c2cd8d19122a --- /dev/null +++ b/groundStation/src/backend/common/packet.c @@ -0,0 +1,90 @@ +#include "packet.h" +#include "commands.h" +#include "bitwise.h" + +#include <string.h> + +enum PacketHeader { + BEGIN, + MTYPE_L, + MTYPE_H, + ID_L, + ID_H, + DLEN_L, + DLEN_H, + HDR_SIZE +}; + +enum ChecksumFormat { + CSUM_L, + CSUM_SIZE +}; + +/* 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); + + 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 */ +{ + if (packet[BEGIN] != BEGIN_CHAR) { + return -1; + } + + if (packet_size < ((uint8_t) HDR_SIZE + CSUM_SIZE)) { + return -1; + } + + 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 -1; + } + + if (data_size < m->data_len) { + return -1; + } + + memcpy(data, &packet[HDR_SIZE], m->data_len); + + /* Validate checksum */ + // TODO + // + return m->data_len; +} diff --git a/groundStation/src/backend/common/packet.h b/groundStation/src/backend/common/packet.h index 2763a8036d93cc6b867fb12386ef783b0475f649..83bc5936b98f17085f1483c993b79b3e5399b911 100644 --- a/groundStation/src/backend/common/packet.h +++ b/groundStation/src/backend/common/packet.h @@ -7,7 +7,7 @@ struct metadata { int msg_type; int msg_id; - int data_len; + size_t data_len; }; /* Combine metadata and data to form a wire-sendable packet. @@ -30,4 +30,7 @@ ssize_t DecodePacket( const uint8_t * packet, /* Packet to decode */ size_t packet_size); /* Size of packet to decode */ +/* Compute a checksum. Requires a packet and the entire packet length */ +uint8_t PacketChecksum(const uint8_t * packet, size_t); + #endif