From 0345dc589bbddfdd28b4efb0881ced311fe9f287 Mon Sep 17 00:00:00 2001
From: Thomas Kolb <cfr34k-git@tkolb.de>
Date: Sun, 3 Nov 2024 14:34:03 +0100
Subject: [PATCH] l2/packet_structs: add function to encode a complete packet

---
 impl/src/layer2/packet_structs.c | 37 ++++++++++++++++++++++++++++++++
 impl/src/layer2/packet_structs.h | 13 +++++++++++
 2 files changed, 50 insertions(+)

diff --git a/impl/src/layer2/packet_structs.c b/impl/src/layer2/packet_structs.c
index 1e91e60..3bd0dae 100644
--- a/impl/src/layer2/packet_structs.c
+++ b/impl/src/layer2/packet_structs.c
@@ -8,9 +8,13 @@
 #include <assert.h>
 #include <string.h>
 
+#include <liquid/liquid.h>
+
 #define LOGGER_MODULE_NAME "l2ps"
 #include "logger.h"
 
+#include "config.h"
+
 #include "packet_structs.h"
 
 
@@ -129,3 +133,36 @@ void layer2_dump_packet_header(int level, const layer2_packet_header_t *header)
 	}
 
 }
+
+
+size_t layer2_encode_packet(
+		const layer2_packet_header_t *header,
+		uint8_t *payload, size_t payload_len,
+		uint8_t *data_out, size_t data_out_len)
+{
+	unsigned int crc_size = crc_sizeof_key(PAYLOAD_CRC_SCHEME);
+
+	if(data_out_len < LAYER2_PACKET_HEADER_ENCODED_SIZE_MAX) {
+		return 0;
+	}
+
+	size_t packet_size = layer2_encode_packet_header(header, data_out);
+
+	if(data_out_len < packet_size + crc_size + payload_len) {
+		return 0;
+	}
+
+	// add the payload data
+	if(payload) {
+		memcpy(data_out + packet_size, payload, payload_len);
+	}
+
+	packet_size += payload_len;
+
+	// calculate CRC of everything and append it to the packet
+	crc_append_key(PAYLOAD_CRC_SCHEME, data_out, packet_size);
+
+	packet_size += crc_size;
+
+	return packet_size;
+}
diff --git a/impl/src/layer2/packet_structs.h b/impl/src/layer2/packet_structs.h
index fedd698..3c50545 100644
--- a/impl/src/layer2/packet_structs.h
+++ b/impl/src/layer2/packet_structs.h
@@ -74,6 +74,19 @@ bool layer2_decode_packet_header(const uint8_t *encoded, size_t encoded_len, lay
  */
 void layer2_dump_packet_header(int level, const layer2_packet_header_t *header);
 
+/*!\brief Encode a complete packet (with header and CRC)
+ * \param header       The header structure to encode.
+ * \param payload      The payload data to encode.
+ * \param payload_len  The length of the payload data.
+ * \param data_out     The buffer for the encoded packet data.
+ * \param data_out_len The size of the output buffer.
+ * \returns The number of bytes written to the output buffer or 0 if not enough space was available.
+ */
+size_t layer2_encode_packet(
+		const layer2_packet_header_t *header,
+		uint8_t *payload, size_t payload_len,
+		uint8_t *data_out, size_t data_out_len);
+
 /*!\brief Get a string representation of the given message type.
  */
 const char* layer2_msg_type_to_string(layer2_message_type_t type);