diff --git a/impl/src/layer2/layer2_structs.c b/impl/src/layer2/layer2_structs.c new file mode 100644 index 0000000..8131be0 --- /dev/null +++ b/impl/src/layer2/layer2_structs.c @@ -0,0 +1,50 @@ +#include +#include + +#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; +} diff --git a/impl/src/layer2/layer2_structs.h b/impl/src/layer2/layer2_structs.h index 1c042c6..8a15eb0 100644 --- a/impl/src/layer2/layer2_structs.h +++ b/impl/src/layer2/layer2_structs.h @@ -2,6 +2,7 @@ #define LAYER2_STRUCTS_H #include +#include #include /* 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 {