Implement packet.c

groundStation/src/backend/common/bitwise.h

+#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

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()

groundStation/src/backend/common/packet.c

+#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;
+}

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