From 16f7ae52422fcdb8b4e86134537bddaaedc83268 Mon Sep 17 00:00:00 2001 From: Thomas Kolb 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 #include +#include + #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);