diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 3054135..859eefa 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -21,4 +21,4 @@ jobs: fetch-depth: 100 - name: Meson build & test run: | - meson setup builddir && ninja -C builddir test + meson setup builddir -Dcsp:packet_padding_bytes=38 && ninja -C builddir test diff --git a/include/cblk/csp_if_cblk.h b/include/cblk/csp_if_cblk.h index 5030bad..370771d 100644 --- a/include/cblk/csp_if_cblk.h +++ b/include/cblk/csp_if_cblk.h @@ -20,7 +20,7 @@ typedef struct __attribute__((packed)) uint8_t reserved1 : 1; /* Byte 2 & 3*/ - uint16_t data_length :16; //! Data length in RS frame in bytes + uint16_t packet_length :16; //! Total CSP packet length in bytes including csp header and crypto overhead if encrypted } cblk_hdr_t; @@ -33,7 +33,10 @@ typedef struct __attribute__((packed)) #define CCSDS_FRAME_LEN 223 #define CBLK_DATA_LEN (CCSDS_FRAME_LEN-sizeof(cblk_hdr_t)) #define CRYPTO_PREAMP 16 /* crypto_secretbox_BOXZEROBYTES */ -#define CRYPTO_POSTAMP 32+9 /* crypto_secretbox_KEYBYTES + NOUNCE_SIZE */ +#define CRYPTO_POSTAMP (16+9) /* 16 zero fill + NONCE_SIZE */ +#define CRYPTO_MAC_SIZE 16 +/* ccsds frame index is 4 bits */ +#define CBLK_MAX_FRAMES_PER_PACKET 15 typedef struct { @@ -52,8 +55,7 @@ typedef struct { /* Variables for internal use */ uint8_t rx_packet_idx; uint8_t rx_frame_idx; - uint8_t packet_enc[CRYPTO_PREAMP+CSP_PACKET_PADDING_BYTES+CSP_BUFFER_SIZE+CRYPTO_POSTAMP]; - uint8_t packet_dec[CRYPTO_PREAMP+CSP_PACKET_PADDING_BYTES+CSP_BUFFER_SIZE+CRYPTO_POSTAMP]; + csp_packet_t* rx_packet; } csp_cblk_interface_data_t; diff --git a/include/crypto/crypto.h b/include/crypto/crypto.h index d3b6ad9..55b553e 100644 --- a/include/crypto/crypto.h +++ b/include/crypto/crypto.h @@ -5,11 +5,12 @@ #include #define CRYPTO_NUM_KEYS 3 +#define CSP_ID2_HEADER_SIZE 6 extern param_t tx_encrypt; extern param_t rx_decrypt; void crypto_key_generate(param_t * param, int idx); -int16_t crypto_decrypt(uint8_t * msg_out, uint8_t * ciphertext_in, uint16_t ciphertext_len, uint8_t crypto_key); -int16_t crypto_encrypt(uint8_t * msg_out, uint8_t * msg_in, uint16_t msg_len); +int16_t crypto_decrypt(csp_packet_t * packet, uint8_t crypto_key); +int16_t crypto_encrypt(csp_packet_t * packet); void crypto_init(); diff --git a/src/crypto/crypto.c b/src/crypto/crypto.c index 497ab3c..3fa03fd 100644 --- a/src/crypto/crypto.c +++ b/src/crypto/crypto.c @@ -3,9 +3,21 @@ #include #include #include +#include "csp/csp.h" #ifdef USE_TWEETNACL #include "tweetnacl.h" + +/* Required to link tweetnacl.c */ +void randombytes(unsigned char * a, unsigned long long c) { + // Note: Pseudo random since we are not initializing random! + unsigned int seed = csp_get_ms(); + while(c > 0) { + *a = rand_r(&seed) & 0xFF; + a++; + c--; + } +} #endif #ifdef USE_SODIUM @@ -16,6 +28,8 @@ #define NONCE_SIZE (sizeof(uint64_t) + sizeof(uint8_t)) +_Static_assert(CSP_PACKET_PADDING_BYTES >= crypto_secretbox_ZEROBYTES + CSP_ID2_HEADER_SIZE, "Not enough padding before csp packet for in-place encryption!"); + uint8_t _crypto_beforenm[CRYPTO_NUM_KEYS][crypto_secretbox_KEYBYTES]; void crypto_key_generate(param_t * param, int idx) { @@ -25,34 +39,71 @@ void crypto_key_generate(param_t * param, int idx) { param_get_data(&crypto_key3, _crypto_beforenm[2], sizeof(_crypto_beforenm[2])); } -/* -There is a 32-octet padding requirement on the plaintext buffer that you pass to crypto_box. -Internally, the NaCl implementation uses this space to avoid having to allocate memory or -use static memory that might involve a cache hit (see Bernstein's paper on cache timing -side-channel attacks for the juicy details). +/** + * @brief Decrypts a CSP packet payload in-place using NaCl/libsodium. + * + * This function verifies the Message Authentication Code (MAC) and decrypts the + * payload contained in the packet structure. It also performs replay protection + * by validating the received Nonce against a stored high-water mark. + * + * The function modifies the packet's data buffer directly, stripping the + * protocol overhead (MAC, padding, Nonce) to restore the original plaintext + * payload. + * + * **Memory Layout Transformation:** + * + * **Before:** + * @code + * [ MAC (16) ] [ Encrypted Payload (N) ] [ 0x00 (16) ] [ Nonce (8) ] [ TX ID (1) ] + * ^ + * frame_begin + * @endcode + * + * **After:** + * @code + * [ Headroom ] [ Plaintext Payload (N) ] [ Tailroom ] + * ^ + * frame_begin + * @endcode + * + * **Validation Steps:** + * 1. Checks if the packet is long enough to contain the necessary crypto overhead. + * 2. Extracts the Nonce from the end of the packet. + * 3. Prepends the required zerofill padding (`crypto_secretbox_BOXZEROBYTES`) to the start. + * 4. Performs authenticated decryption using `crypto_box_open_afternm`. + * 5. Verifies the Nonce counter is strictly greater than the last received Nonce for that group (Replay Protection). + * + * @param[in,out] packet Pointer to the CSP packet structure containing the encrypted frame. + * On success, `frame_begin` and `frame_length` are updated to point + * to the decrypted plaintext. + * @param[in] crypto_key The index of the pre-shared key to use for decryption (1-based index). + * + * @return + * - \b CSP_ERR_NONE: Decryption successful and Nonce is valid. + * - \b -1: Decryption failed (Authentication error, invalid key, packet too short, or replay detected). + */ +int16_t crypto_decrypt(csp_packet_t * packet, uint8_t crypto_key) { + + if(crypto_key == 0 || crypto_key > CRYPTO_NUM_KEYS) { + return -1; + } -Similarly, the crypto_box_open call requires 16 octets of zero padding before the start -of the actual ciphertext. This is used in a similar fashion. These padding octets are not -part of either the plaintext or the ciphertext, so if you are sending ciphertext across the -network, don't forget to remove them! -*/ -uint8_t decrypt_out[CSP_PACKET_PADDING_BYTES+CSP_BUFFER_SIZE+crypto_secretbox_ZEROBYTES]; -int16_t crypto_decrypt(uint8_t * msg_out, uint8_t * decrypt_in, uint16_t ciphertext_len, uint8_t crypto_key) { + if(packet->frame_length < NONCE_SIZE + crypto_secretbox_ZEROBYTES) { + return -1; + } - ciphertext_len = ciphertext_len - NONCE_SIZE; + packet->frame_length -= NONCE_SIZE; /* Receive nonce */ uint8_t decrypt_nonce[crypto_box_NONCEBYTES] = {}; - memcpy(&decrypt_nonce, &decrypt_in[crypto_secretbox_BOXZEROBYTES+ciphertext_len], NONCE_SIZE); + memcpy(&decrypt_nonce, &packet->frame_begin[packet->frame_length], NONCE_SIZE); /* Make room for zerofill at the beginning of message */ - memset(decrypt_in, 0, crypto_secretbox_BOXZEROBYTES); - - /* Make room for zerofill at the beginning of message */ - memset(decrypt_out, 0, crypto_secretbox_ZEROBYTES); + packet->frame_begin -= crypto_secretbox_BOXZEROBYTES; + memset(packet->frame_begin, 0, crypto_secretbox_BOXZEROBYTES); /* Decryption */ - if(crypto_box_open_afternm(decrypt_out, decrypt_in, ciphertext_len, decrypt_nonce, _crypto_beforenm[crypto_key-1]) != 0) { + if(crypto_box_open_afternm(packet->frame_begin, packet->frame_begin, packet->frame_length, decrypt_nonce, _crypto_beforenm[crypto_key-1]) != 0) { param_set_uint16(&crypto_fail_auth_count, param_get_uint16(&crypto_fail_auth_count) + 1); return -1; } @@ -60,51 +111,94 @@ int16_t crypto_decrypt(uint8_t * msg_out, uint8_t * decrypt_in, uint16_t ciphert /* Message successfully decrypted, check for valid nonce */ uint64_t nonce_counter; memcpy(&nonce_counter, decrypt_nonce, sizeof(uint64_t)); - uint8_t nounce_group = decrypt_nonce[sizeof(uint64_t)]; - uint64_t nonce_rx = param_get_uint64_array(&crypto_nonce_rx_count, nounce_group); + uint8_t nonce_group = decrypt_nonce[sizeof(uint64_t)]; + uint64_t nonce_rx = param_get_uint64_array(&crypto_nonce_rx_count, nonce_group); if(nonce_counter <= nonce_rx) { param_set_uint16(&crypto_fail_nonce_count, param_get_uint16(&crypto_fail_nonce_count) + 1); return -1; } - /* Copy encrypted data back to msgbuffer */ - memcpy(msg_out, &decrypt_out[crypto_secretbox_ZEROBYTES], ciphertext_len - crypto_secretbox_KEYBYTES); + /* Remove prepended MAC and Zero fill 16 bytes after message */ + packet->frame_length -= crypto_secretbox_ZEROBYTES; + packet->frame_begin += crypto_secretbox_ZEROBYTES; /* Update counter with received value so that next sent value is higher */ - param_set_uint64_array(&crypto_nonce_rx_count, nounce_group, nonce_counter); + param_set_uint64_array(&crypto_nonce_rx_count, nonce_group, nonce_counter); - /* Return useable length */ - return ciphertext_len - crypto_secretbox_KEYBYTES; + return CSP_ERR_NONE; } -uint8_t encrypt_in[crypto_secretbox_ZEROBYTES+CSP_PACKET_PADDING_BYTES+CSP_BUFFER_SIZE]; -int16_t crypto_encrypt(uint8_t * msg_out, uint8_t * msg_in, uint16_t msg_len) { +/** + * @brief Encrypts a CSP packet payload in-place using NaCl/libsodium. + * + * This function performs authenticated encryption on the data contained in the + * packet structure. It modifies the packet's data buffer directly, adjusting + * the `frame_begin` pointer and `frame_length` to account for the prepended + * Message Authentication Code (MAC) and the appended Nonce. + * + * **Memory Layout Transformation:** + * + * **Before:** + * @code + * [ Headroom ] [ Payload (N) ] [ Tailroom ] + * ^ + * frame_begin + * @endcode + * + * **After:** + * @code + * [ MAC (16) ] [ Encrypted Payload (N) ] [ 0x00 (16) ] [ Nonce (8) ] [ TX ID (1) ] + * ^ + * frame_begin + * @endcode + * + * @pre **Compile-time Check:** `CSP_PACKET_PADDING_BYTES` must be greater than + * `crypto_secretbox_ZEROBYTES + CSP_ID2_HEADER_SIZE`. This is enforced by a + * `_Static_assert` to ensure safe headroom padding. + * + * @param[in,out] packet Pointer to the CSP packet structure. The `frame_begin`, + * `frame_length`, and buffer contents will be modified. + * + * @return + * - \b CSP_ERR_NONE: Encryption successful. + * - \b CSP_ERR_INVAL: Packet buffer too small for prepending nonce and zerofill. + */ +int16_t crypto_encrypt(csp_packet_t * packet) { + + /* Check that there is enough space to postpend nonce and 16 byte zerofill */ + if(packet->length + NONCE_SIZE + crypto_secretbox_BOXZEROBYTES > CSP_BUFFER_SIZE) { + return CSP_ERR_INVAL; + } + /* Update and get transmit nonce */ uint64_t tx_nonce = param_get_uint64(&crypto_nonce_tx_count) + 1; param_set_uint64(&crypto_nonce_tx_count, tx_nonce); /* Pack nonce into 24-bytes format, expected by NaCl */ unsigned char nonce[crypto_box_NONCEBYTES] = {}; memcpy(nonce, &tx_nonce, sizeof(uint64_t)); + /* Add nonce ID to nonce */ nonce[sizeof(uint64_t)] = param_get_uint8(&crypto_nonce_tx_id); - /* Copy msg to new buffer, to make room for zerofill */ - memcpy(&encrypt_in[crypto_secretbox_ZEROBYTES], msg_in, msg_len); - /* Make room for zerofill at the beginning of message */ - memset(encrypt_in, 0, crypto_secretbox_ZEROBYTES); + uint8_t * padding_begin = packet->frame_begin - crypto_secretbox_ZEROBYTES; + memset(padding_begin, 0, crypto_secretbox_ZEROBYTES); - /* Make room for zerofill at the beginning of message */ - memset(msg_out, 0, crypto_secretbox_BOXZEROBYTES); + /* Encryption only returns -1 if mlen < 32 */ + crypto_box_afternm(padding_begin, padding_begin, packet->frame_length + crypto_secretbox_ZEROBYTES, nonce, _crypto_beforenm[param_get_uint8(&tx_encrypt)-1]); - if (crypto_box_afternm(msg_out, encrypt_in, crypto_secretbox_KEYBYTES + msg_len, nonce, _crypto_beforenm[param_get_uint8(&tx_encrypt)-1]) != 0) { - return -1; - } + /* Adjust packet pointers and length for the prepended MAC */ + packet->frame_begin -= crypto_secretbox_BOXZEROBYTES; + packet->frame_length += crypto_secretbox_BOXZEROBYTES; + + /* Zero out the 16 bytes between the end of the encrypted data and the nonce for backwards compatibility */ + memset(packet->frame_begin + packet->frame_length, 0, crypto_secretbox_BOXZEROBYTES); - /* Add nonce at the end of the packet */ - memcpy(&msg_out[crypto_secretbox_BOXZEROBYTES + msg_len + crypto_secretbox_KEYBYTES], nonce, NONCE_SIZE); + /* Add nonce at the end of the packet plus 16 bytes for backwards compatibility */ + memcpy(packet->frame_begin + (crypto_secretbox_BOXZEROBYTES + packet->frame_length), nonce, NONCE_SIZE); + packet->frame_length += NONCE_SIZE + crypto_secretbox_BOXZEROBYTES; - return msg_len + crypto_secretbox_KEYBYTES + NONCE_SIZE; + return CSP_ERR_NONE; } void crypto_init() { diff --git a/src/csp_if_cblk.c b/src/csp_if_cblk.c index c96fe87..c648c16 100644 --- a/src/csp_if_cblk.c +++ b/src/csp_if_cblk.c @@ -6,13 +6,18 @@ #include #include #include "crypto/crypto.h" +#include "csp/csp_buffer.h" #include uint8_t _cblk_rx_debug = 0; uint8_t _cblk_tx_debug = 0; +/* Calculate number of CCSDS frames required to send CSP packet of given size + * Returns 0 if size exceeds maximum allowed */ static uint8_t num_ccsds_from_csp(uint16_t framesize) { - + if(framesize > (CBLK_DATA_LEN * CBLK_MAX_FRAMES_PER_PACKET)) { + return 0; + } return (framesize+CBLK_DATA_LEN-1)/CBLK_DATA_LEN; } @@ -59,22 +64,36 @@ int csp_if_cblk_tx(csp_iface_t * iface, uint16_t via, csp_packet_t *packet, int csp_hex_dump("tx_frame", packet->frame_begin, packet->frame_length); } - uint16_t frame_length = packet->frame_length; - uint8_t* frame_begin = packet->frame_begin; - - ifdata->cblk_tx_lock(iface); if (param_get_uint8(&tx_encrypt)) { - frame_length = crypto_encrypt(ifdata->packet_enc, packet->frame_begin, packet->frame_length); - frame_begin = &ifdata->packet_enc[CRYPTO_PREAMP]; + + if(crypto_encrypt(packet) < 0) { + csp_buffer_free(packet); + if (_cblk_tx_debug >= 2) { + printf("Encryption fail: packet too large to encrypt\n"); + } + return CSP_ERR_INVAL; + } if (_cblk_tx_debug >= 3) { - csp_hex_dump("tx_enc", frame_begin, frame_length); + csp_hex_dump("tx_enc", packet->frame_begin, packet->frame_length); } } - uint16_t bytes_remain = frame_length; - for (int8_t frame_cnt = 0; frame_cnt < num_ccsds_from_csp(frame_length); frame_cnt++) { + uint16_t bytes_remain = packet->frame_length; + uint8_t num_frames = num_ccsds_from_csp(packet->frame_length); + + if(num_frames == 0) { + csp_buffer_free(packet); + if (_cblk_tx_debug >= 1) { + printf("Packet too large to send: %u bytes\n", packet->frame_length); + } + return CSP_ERR_INVAL; + } + + ifdata->cblk_tx_lock(iface); + + for (int8_t frame_cnt = 0; frame_cnt < num_frames; frame_cnt++) { cblk_frame_t * tx_ccsds_buf = ifdata->cblk_tx_buffer_get(iface); if (tx_ccsds_buf == NULL) { @@ -87,15 +106,15 @@ int csp_if_cblk_tx(csp_iface_t * iface, uint16_t via, csp_packet_t *packet, int tx_ccsds_buf->hdr.csp_packet_idx = iface->tx; tx_ccsds_buf->hdr.ccsds_frame_idx = frame_cnt; - tx_ccsds_buf->hdr.data_length = htobe16(frame_length); + tx_ccsds_buf->hdr.packet_length = htobe16(packet->frame_length); tx_ccsds_buf->hdr.nacl_crypto_key = param_get_uint8(&tx_encrypt); if (_cblk_tx_debug >= 1) { - printf("TX CCSDS header: %u %u %u\n", tx_ccsds_buf->hdr.csp_packet_idx, frame_cnt, frame_length); + printf("TX CCSDS header: %u %u %u\n", tx_ccsds_buf->hdr.csp_packet_idx, frame_cnt, packet->frame_length); } uint16_t segment_len = (CBLK_DATA_LEN < bytes_remain) ? CBLK_DATA_LEN : bytes_remain; - memcpy(tx_ccsds_buf->data, frame_begin+(frame_length-bytes_remain), segment_len); + memcpy(tx_ccsds_buf->data, packet->frame_begin+(packet->frame_length-bytes_remain), segment_len); bytes_remain -= segment_len; if (ifdata->cblk_tx_send(iface, tx_ccsds_buf) < 0) { @@ -113,17 +132,22 @@ int csp_if_cblk_tx(csp_iface_t * iface, uint16_t via, csp_packet_t *packet, int int csp_if_cblk_rx(csp_iface_t * iface, cblk_frame_t *frame, uint32_t len, uint8_t group) { - csp_cblk_interface_data_t * ifdata = iface->interface_data; + csp_cblk_interface_data_t * ifdata = iface->interface_data; - uint16_t frame_length = be16toh(frame->hdr.data_length); + uint16_t packet_length = be16toh(frame->hdr.packet_length); if (_cblk_rx_debug >= 3) { - printf("RX %p chain %u CCSDS header: %u %u %u\n", frame, group, frame->hdr.csp_packet_idx, frame->hdr.ccsds_frame_idx, frame_length); + printf("RX %p chain %u CCSDS header: %u %u %u\n", frame, group, frame->hdr.csp_packet_idx, frame->hdr.ccsds_frame_idx, packet_length); } - if (frame_length < 4 || (frame->hdr.nacl_crypto_key == 0 && frame_length > CSP_PACKET_PADDING_BYTES + CSP_BUFFER_SIZE) - || (frame->hdr.nacl_crypto_key > 0 && frame_length > CSP_PACKET_PADDING_BYTES + CSP_BUFFER_SIZE + CRYPTO_POSTAMP) - || frame->hdr.ccsds_frame_idx >= num_ccsds_from_csp(frame_length)) { + /* minimum header size in CSP version 1*/ + if (packet_length < 4 + /* invalid packet length */ + || (frame->hdr.nacl_crypto_key == 0 && packet_length > CSP_ID2_HEADER_SIZE + CSP_BUFFER_SIZE) + /* invalid encrypted packet length */ + || (frame->hdr.nacl_crypto_key > 0 && packet_length > CRYPTO_MAC_SIZE + CSP_ID2_HEADER_SIZE + CSP_BUFFER_SIZE) + /* invalid number of CCSDS frames */ + || frame->hdr.ccsds_frame_idx >= num_ccsds_from_csp(packet_length)) { /* This is triggered by dummybursts transmitted when opening channel, in case HW does not filter those */ return CSP_ERR_NONE; @@ -139,8 +163,14 @@ int csp_if_cblk_rx(csp_iface_t * iface, cblk_frame_t *frame, uint32_t len, uint8 /* Start handling a new packet */ ifdata->rx_frame_idx = frame->hdr.ccsds_frame_idx; ifdata->rx_packet_idx = frame->hdr.csp_packet_idx; + /* Setup rx packet moves frame_begin to fit csp header 4 or 6 bytes */ + csp_id_setup_rx(ifdata->rx_packet); + /* Adjust for crypto MAC if encrypted */ + if (frame->hdr.nacl_crypto_key > 0) { + ifdata->rx_packet->frame_begin -= CRYPTO_MAC_SIZE; + } - } else if (ifdata->rx_frame_idx+1 != frame->hdr.ccsds_frame_idx || ifdata->rx_packet_idx != frame->hdr.csp_packet_idx) { + } else if (ifdata->rx_frame_idx + 1 != frame->hdr.ccsds_frame_idx || ifdata->rx_packet_idx != frame->hdr.csp_packet_idx) { /* We are missing part of the received CSP frame */ if (_cblk_rx_debug >= 1) { @@ -155,73 +185,74 @@ int csp_if_cblk_rx(csp_iface_t * iface, cblk_frame_t *frame, uint32_t len, uint8 } uint16_t cblk_frame_len = CBLK_DATA_LEN; - if ((ifdata->rx_frame_idx+1)*CBLK_DATA_LEN > frame_length) { - cblk_frame_len = frame_length - ifdata->rx_frame_idx*CBLK_DATA_LEN; + if ((ifdata->rx_frame_idx + 1) * CBLK_DATA_LEN > packet_length) { + cblk_frame_len = packet_length - ifdata->rx_frame_idx * CBLK_DATA_LEN; + } + + /* Check for buffer overflow */ + uint8_t * buffer_end = ifdata->rx_packet->data + CSP_BUFFER_SIZE; + uint8_t * write_end = ifdata->rx_packet->frame_begin + ifdata->rx_packet->frame_length + cblk_frame_len; + if (write_end > buffer_end) { + iface->frame++; + return CSP_ERR_INVAL; } - memcpy(&ifdata->packet_dec[CRYPTO_PREAMP+ifdata->rx_frame_idx*CBLK_DATA_LEN], frame->data, cblk_frame_len); + memcpy(&ifdata->rx_packet->frame_begin[ifdata->rx_packet->frame_length], frame->data, cblk_frame_len); + ifdata->rx_packet->frame_length += cblk_frame_len; + - if (ifdata->rx_frame_idx+1 < num_ccsds_from_csp(frame_length)) { + if (ifdata->rx_frame_idx + 1 < num_ccsds_from_csp(packet_length)) { /* We are still waiting for the last CCSDS frame of the CSP packet */ return CSP_ERR_NONE; } - csp_packet_t* rx_packet = csp_buffer_get(frame_length); - csp_id_setup_rx(rx_packet); - if (frame->hdr.nacl_crypto_key > 0) { if (_cblk_rx_debug >= 4) { - csp_hex_dump("-rx_enc", &ifdata->packet_dec[CRYPTO_PREAMP], frame_length); + csp_hex_dump("-rx_enc", ifdata->rx_packet->frame_begin, packet_length); } - int16_t dec_frame_length = crypto_decrypt(rx_packet->frame_begin, ifdata->packet_dec, frame_length, frame->hdr.nacl_crypto_key); + int16_t decrypt_res = crypto_decrypt(ifdata->rx_packet, frame->hdr.nacl_crypto_key); - if (dec_frame_length < 0) { + if (decrypt_res < 0) { iface->autherr++; - csp_buffer_free(rx_packet); return CSP_ERR_HMAC; } - rx_packet->frame_length = dec_frame_length; - } else if (param_get_uint8(&rx_decrypt)) { iface->autherr++; - csp_buffer_free(rx_packet); return CSP_ERR_HMAC; - } else { - - memcpy(rx_packet->frame_begin, &ifdata->packet_dec[CRYPTO_PREAMP], frame_length); - rx_packet->frame_length = frame_length; } if (_cblk_rx_debug >= 5) { - csp_hex_dump("-rx_dec", rx_packet->frame_begin, rx_packet->frame_length); + csp_hex_dump("-rx_dec", ifdata->rx_packet->frame_begin, ifdata->rx_packet->frame_length); } /* Strip and parse CSP header */ - if (csp_id_strip(rx_packet) < 0) { + if (csp_id_strip(ifdata->rx_packet) < 0) { iface->frame++; - csp_buffer_free(rx_packet); return CSP_ERR_INVAL; } if (_cblk_rx_debug >= 4) { - csp_hex_dump("packet", rx_packet->data, rx_packet->length); + csp_hex_dump("packet", ifdata->rx_packet->data, ifdata->rx_packet->length); } - csp_qfifo_write(rx_packet, iface, NULL); + csp_qfifo_write(ifdata->rx_packet, iface, NULL); + ifdata->rx_packet = csp_buffer_get(0); + return CSP_ERR_NONE; } void csp_if_cblk_init(csp_iface_t * iface) { - csp_cblk_interface_data_t * ifdata = iface->interface_data; + csp_cblk_interface_data_t * ifdata = iface->interface_data; ifdata->rx_frame_idx = UINT8_MAX; ifdata->rx_packet_idx = UINT8_MAX; + ifdata->rx_packet = csp_buffer_get(0); if(ifdata->cblk_tx_lock == NULL || ifdata->cblk_tx_unlock == NULL) { printf("csp_if_cblk_init: lock function pointers must be set!\n");