From a6464a28b85b70e7febfeed2f7798ee0e65dbd7f Mon Sep 17 00:00:00 2001 From: Thomas Kolb Date: Thu, 18 Jul 2024 22:56:42 +0200 Subject: [PATCH] Layer 2 received packet handling --- impl/src/layer2/layer2_rx.c | 66 ++++++++++++++++++++++++++++++++ impl/src/layer2/layer2_rx.h | 38 ++++++++++++++++++ impl/src/layer2/layer2_tx.c | 9 +++-- impl/src/layer2/layer2_tx.h | 12 ++++-- impl/src/layer2/packet_structs.c | 15 ++++++-- impl/src/layer2/packet_structs.h | 6 +++ impl/src/main.c | 36 ++++++++++------- impl/src/results.h | 1 + 8 files changed, 159 insertions(+), 24 deletions(-) create mode 100644 impl/src/layer2/layer2_rx.c create mode 100644 impl/src/layer2/layer2_rx.h diff --git a/impl/src/layer2/layer2_rx.c b/impl/src/layer2/layer2_rx.c new file mode 100644 index 0000000..c145b64 --- /dev/null +++ b/impl/src/layer2/layer2_rx.c @@ -0,0 +1,66 @@ +#include +#include +#include + +#include + +#include "layer2_rx.h" + +#include "packet_structs.h" + +#include "config.h" + +result_t layer2_rx_init(layer2_rx_t *ctx, int tun_fd) +{ + ctx->last_acked_seq = 0; + ctx->tun_fd = tun_fd; + return OK; +} + + +void layer2_rx_destroy(layer2_rx_t *ctx) +{ + (void)ctx; + // nothing to do here so far +} + + +result_t layer2_rx_handle_packet(layer2_rx_t *ctx, const uint8_t *buf, size_t buf_len) +{ + // check the CRC + size_t packet_size = buf_len - crc_sizeof_key(PAYLOAD_CRC_SCHEME); + + if(!crc_check_key(PAYLOAD_CRC_SCHEME, (unsigned char*)buf, packet_size)) { + LOG(LVL_ERR, "payload CRC check failed!"); + return ERR_INTEGRITY; + } + + // decode the header + layer2_packet_header_t header; + + if(!layer2_decode_packet_header(buf, buf_len, &header)) { + LOG(LVL_ERR, "Header could not be decoded!"); + return ERR_INTEGRITY; + } + + ctx->last_acked_seq = header.rx_seq_nr; + + size_t header_size = layer2_get_encoded_header_size(&header); + + // extract the payload and forward it to the tun device + const uint8_t *payload = buf + header_size; + size_t payload_len = packet_size - header_size; + + int ret = write(ctx->tun_fd, payload, payload_len); + if(ret < 0) { + LOG(LVL_ERR, "write: %s", strerror(errno)); + } + + return OK; +} + + +uint8_t layer2_rx_get_last_acked_seq(const layer2_rx_t *ctx) +{ + return ctx->last_acked_seq; +} diff --git a/impl/src/layer2/layer2_rx.h b/impl/src/layer2/layer2_rx.h new file mode 100644 index 0000000..8e305dc --- /dev/null +++ b/impl/src/layer2/layer2_rx.h @@ -0,0 +1,38 @@ +#ifndef LAYER2_RX_H +#define LAYER2_RX_H + +#include + +typedef struct { + uint8_t last_acked_seq; + + int tun_fd; +} layer2_rx_t; + + +/*!\brief Initialize the layer 2 receiver context. + * + * \param ctx The receiver context to initialize. + * \param tun_fd The TUN device to read packets from. + * \returns OK if everything worked or a fitting error code. + */ +result_t layer2_rx_init(layer2_rx_t *ctx, int tun_fd); + +/*!\brief Destroy the given layer 2 receiver context. + */ +void layer2_rx_destroy(layer2_rx_t *ctx); + +/*!\brief Handle a received packet. + * + * \param ctx The receiver context. + * \param buf Where to write the encoded packet data. + * \param buf_len Space available in the buffer. + * \returns A result code from the packet handling procedure. + */ +result_t layer2_rx_handle_packet(layer2_rx_t *ctx, const uint8_t *buf, size_t buf_len); + +/*!\brief Return the sequence number expected next by the other side. + */ +uint8_t layer2_rx_get_last_acked_seq(const layer2_rx_t *ctx); + +#endif // LAYER2_RX_H diff --git a/impl/src/layer2/layer2_tx.c b/impl/src/layer2/layer2_tx.c index 5288ae2..2dfe0a9 100644 --- a/impl/src/layer2/layer2_tx.c +++ b/impl/src/layer2/layer2_tx.c @@ -14,11 +14,12 @@ #define SEQ_NR_MASK 0xF -result_t layer2_tx_init(layer2_tx_t *ctx) +result_t layer2_tx_init(layer2_tx_t *ctx, int tun_fd) { packet_queue_init(&ctx->packet_queue); ctx->next_packet_index = 0; ctx->next_seq_nr = 0; + ctx->tun_fd = tun_fd; return OK; } @@ -29,11 +30,11 @@ void layer2_tx_destroy(layer2_tx_t *ctx) } -result_t layer2_tx_fill_packet_queue(layer2_tx_t *ctx, int tun_fd) +result_t layer2_tx_fill_packet_queue(layer2_tx_t *ctx) { struct pollfd pfd = {0}; - pfd.fd = tun_fd; + pfd.fd = ctx->tun_fd; pfd.events = POLLIN; layer2_packet_header_t header; @@ -59,7 +60,7 @@ result_t layer2_tx_fill_packet_queue(layer2_tx_t *ctx, int tun_fd) header.tx_seq_nr = ctx->next_seq_nr; uint8_t packetbuf[2048]; - ret = read(tun_fd, packetbuf, sizeof(packetbuf)); + ret = read(ctx->tun_fd, packetbuf, sizeof(packetbuf)); if(ret < 0) { LOG(LVL_ERR, "read: %s", strerror(errno)); return ERR_SYSCALL; diff --git a/impl/src/layer2/layer2_tx.h b/impl/src/layer2/layer2_tx.h index 6685060..58c21f5 100644 --- a/impl/src/layer2/layer2_tx.h +++ b/impl/src/layer2/layer2_tx.h @@ -7,14 +7,21 @@ typedef struct { packet_queue_t packet_queue; + size_t next_packet_index; uint8_t next_seq_nr; + + int tun_fd; } layer2_tx_t; /*!\brief Initialize the layer 2 transmitter context. + * + * \param ctx The transmitter context to initialize. + * \param tun_fd The TUN device to read packets from. + * \returns OK if everything worked or a fitting error code. */ -result_t layer2_tx_init(layer2_tx_t *ctx); +result_t layer2_tx_init(layer2_tx_t *ctx, int tun_fd); /*!\brief Destroy the given layer 2 transmitter context. */ @@ -22,9 +29,8 @@ void layer2_tx_destroy(layer2_tx_t *ctx); /*!\brief Fill the packet queue from the given TUN device. * \param ctx The transmitter context. - * \param tun_fd File descriptor of the TUN device to read packets from. */ -result_t layer2_tx_fill_packet_queue(layer2_tx_t *ctx, int tun_fd); +result_t layer2_tx_fill_packet_queue(layer2_tx_t *ctx); /*!\brief Encode the next packet for transmission. * diff --git a/impl/src/layer2/packet_structs.c b/impl/src/layer2/packet_structs.c index 9396300..6915688 100644 --- a/impl/src/layer2/packet_structs.c +++ b/impl/src/layer2/packet_structs.c @@ -33,6 +33,15 @@ size_t layer2_encode_packet_header(const layer2_packet_header_t *header, uint8_t } +size_t layer2_get_encoded_header_size(const layer2_packet_header_t *header) +{ + assert(header->src_addr.length <= 4 && header->src_addr.length != 0); + assert(header->dst_addr.length <= 4 && header->dst_addr.length != 0); + + return 2 + 2 * header->src_addr.length + 2 * header->dst_addr.length; +} + + bool layer2_decode_packet_header(const uint8_t *encoded, size_t encoded_len, layer2_packet_header_t *header) { // check if there are enough bytes for the minimum header size: @@ -42,11 +51,11 @@ bool layer2_decode_packet_header(const uint8_t *encoded, size_t encoded_len, lay header->msg_type = (encoded[0] >> 5) & 0x7; header->tx_request = (encoded[0] & 0x10) != 0; - header->src_addr.length = (encoded[0] >> 2) & 0x3 + 1; - header->dst_addr.length = (encoded[0] >> 0) & 0x3 + 1; + header->src_addr.length = ((encoded[0] >> 2) & 0x3) + 1; + header->dst_addr.length = ((encoded[0] >> 0) & 0x3) + 1; // check for the actually needed size - if(encoded_len < (2 + 2*header->src_addr.length + 2*header->dst_addr.length)) { + if(encoded_len < (2U + 2*header->src_addr.length + 2*header->dst_addr.length)) { return false; } diff --git a/impl/src/layer2/packet_structs.h b/impl/src/layer2/packet_structs.h index e1fa842..4147b07 100644 --- a/impl/src/layer2/packet_structs.h +++ b/impl/src/layer2/packet_structs.h @@ -47,6 +47,12 @@ typedef struct layer2_packet_header_s { */ size_t layer2_encode_packet_header(const layer2_packet_header_t *header, uint8_t *encoded); +/*!\brief Get the size of a layer2 packet header in encoded form. + * \param header The header structure. + * \returns The number of encoded bytes. + */ +size_t layer2_get_encoded_header_size(const layer2_packet_header_t *header); + /*!\brief Decode a layer2 packet header from the received form. * \param encoded A byte array containing the received header. * \param encoded_len Length (in bytes) of the given encoded data. diff --git a/impl/src/main.c b/impl/src/main.c index ee915f2..92e9fdf 100644 --- a/impl/src/main.c +++ b/impl/src/main.c @@ -21,8 +21,10 @@ #include "layer1/tx.h" #include "layer1/rx.h" -#include "layer2/tundev.h" #include "layer2/layer2_tx.h" +#include "layer2/layer2_rx.h" + +#include "layer2/tundev.h" #include "sdr/sdr.h" @@ -43,6 +45,8 @@ static double next_tx_switch_time = 0.0; static rx_stats_t m_rx_stats; +static layer2_rx_t l2rx; + static void signal_handler(int signal, siginfo_t *info, void *ctx) { (void)signal; @@ -93,22 +97,24 @@ void cb_rx(rx_evt_t evt, const struct layer1_rx_s *rx, uint8_t *packet_data, siz //hexdump(packet_data, packet_len < 64 ? packet_len : 64); //LOG(LVL_INFO, "===================================="); - m_rx_stats.successful_decodes++; - block_tx_for(TX_SWITCH_BACKOFF_END_OF_PACKET_MS); - unsigned int data_size = packet_len - crc_sizeof_key(PAYLOAD_CRC_SCHEME); + result_t result = layer2_rx_handle_packet(&l2rx, packet_data, packet_len); + switch(result) { + case OK: + m_rx_stats.successful_decodes++; + break; - // FIXME: move to layer 2 - if(!crc_check_key(PAYLOAD_CRC_SCHEME, packet_data, data_size)) { - LOG(LVL_WARN, "payload CRC check failed!"); - break; + case ERR_INTEGRITY: + LOG(LVL_ERR, "Packet could not be decoded by Layer 2."); + m_rx_stats.failed_decodes++; + break; + + default: // all other errors + LOG(LVL_ERR, "layer2_rx_handle_packet() returned error code %u.", result); + break; } - ret = write(m_tunfd, packet_data, data_size); - if(ret < 0) { - LOG(LVL_ERR, "write: %s", strerror(errno)); - } break; case RX_EVT_PREAMBLE_FOUND: @@ -179,7 +185,8 @@ int main(int argc, char **argv) RESULT_CHECK(layer1_tx_init(&tx)); RESULT_CHECK(layer1_rx_init(&rx, cb_rx)); - RESULT_CHECK(layer2_tx_init(&l2tx)); + RESULT_CHECK(layer2_tx_init(&l2tx, m_tunfd)); + RESULT_CHECK(layer2_rx_init(&l2rx, m_tunfd)); // ** Set up signal handling @@ -217,7 +224,7 @@ int main(int argc, char **argv) double now = get_hires_time(); // fill the TX queue from the TUN device - layer2_tx_fill_packet_queue(&l2tx, m_tunfd); + layer2_tx_fill_packet_queue(&l2tx); if((now > next_tx_switch_time) && (on_air || !layer1_rx_is_busy(&rx))) { if(layer2_tx_can_transmit(&l2tx)) { @@ -355,6 +362,7 @@ int main(int argc, char **argv) // ** Cleanup ** layer2_tx_destroy(&l2tx); + layer2_rx_destroy(&l2rx); layer1_tx_shutdown(&tx); layer1_rx_shutdown(&rx); diff --git a/impl/src/results.h b/impl/src/results.h index faa686f..6606521 100644 --- a/impl/src/results.h +++ b/impl/src/results.h @@ -13,6 +13,7 @@ typedef enum { ERR_SYSCALL, // a syscall failed. Use errno to determine the cause. ERR_SOAPY, // an error occurred in the SoapySDR library. ERR_SDR, // an error occurred in the SDR interface. + ERR_INTEGRITY, // an integrity check failed (e.g. CRC of received packet is wrong) } result_t; #ifdef DEBUG_LIQUID