Implement layer 2 header packing and unpacking

impl/src/layer2/layer2_structs.c

@@ -0,0 +1,50 @@
+#include <assert.h>
+#include <string.h>
+
+#include "layer2_structs.h"
+
+
+size_t layer2_encode_packet_header(const layer2_packet_header_t *header, uint8_t *encoded)
+{
+	assert(header->src_addr_len < 4);
+	assert(header->dst_addr_len < 4);
+	assert(header->tx_seq_nr < 16);
+	assert(header->rx_seq_nr < 16);
+
+	size_t encoded_size = 2;
+
+	encoded[0] =
+		((uint8_t)header->msg_type & 0x7) << 5
+		| ((uint8_t)header->tx_request & 0x1) << 4
+		| (header->src_addr_len & 0x3) << 2
+		| (header->dst_addr_len & 0x3) << 0;
+
+	encoded[1] =
+		(header->tx_seq_nr & 0xF) << 4
+		| (header->rx_seq_nr & 0xF) << 0;
+
+	memcpy(encoded + encoded_size, header->src_addr, 2 * header->src_addr_len);
+	encoded_size += 2 * header->src_addr_len;
+
+	memcpy(encoded + encoded_size, header->dst_addr, 2 * header->dst_addr_len);
+	encoded_size += 2 * header->dst_addr_len;
+
+	return encoded_size;
+}
+
+
+bool layer2_decode_packet_header(const uint8_t *encoded, layer2_packet_header_t *header)
+{
+	header->msg_type = (encoded[0] >> 5) & 0x7;
+	header->tx_request = (encoded[0] & 0x10) != 0;
+	header->src_addr_len = (encoded[0] >> 2) & 0x3;
+	header->dst_addr_len = (encoded[0] >> 0) & 0x3;
+
+	header->tx_seq_nr = (encoded[1] >> 4) & 0xF;
+	header->tx_seq_nr = (encoded[1] >> 0) & 0xF;
+
+	memcpy(header->src_addr, encoded + 2, 2 * header->src_addr_len);
+	memcpy(header->dst_addr, encoded + 2 + 2 * header->src_addr_len, 2 * header->dst_addr_len);
+
+	return true;
+}

impl/src/layer2/layer2_structs.h

@@ -2,6 +2,7 @@
 #define LAYER2_STRUCTS_H
 
 #include <stdint.h>
+#include <stddef.h>
 #include <stdbool.h>
 
 /* Common Link-layer Header */
@@ -25,6 +26,20 @@ typedef struct layer2_packet_header_s {
 	uint8_t dst_addr[8];            //!< destination HAM-64 address
 } layer2_packet_header_t;
 
+/*!\brief Encode a layer2 packet header to the transmitted form.
+ * \param header   The header structure to encode.
+ * \param encoded  A byte array where the encoded header will be stored. Must have space for 18 bytes.
+ * \returns        The number of encoded bytes. Zero if an error occurred.
+ */
+size_t layer2_encode_packet_header(const layer2_packet_header_t *header, uint8_t *encoded);
+
+/*!\brief Decode a layer2 packet header from the received form.
+ * \param encoded  A byte array containing the received header.
+ * \param header   The header structure where the decoded information is stored.
+ * \returns        Whether the header was decoded successfully.
+ */
+bool layer2_decode_packet_header(const uint8_t *encoded, layer2_packet_header_t *header);
+
 /* Data Packet Structs */
 
 typedef enum {