inplace encryption inside csp packet

Author: edvardxyz

include/crypto/crypto.h

@@ -12,4 +12,5 @@ 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_encrypt_inplace(csp_packet_t * packet);
 void crypto_init();

src/crypto/crypto.c

@@ -13,9 +13,12 @@
 #endif
 
 #include "crypto/crypto_param.h"
+#define CSP_ID2_HEADER_SIZE 6
 
 #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) {
@@ -107,6 +110,79 @@ int16_t crypto_encrypt(uint8_t * msg_out, uint8_t * msg_in, uint16_t msg_len) {
     return msg_len + crypto_secretbox_KEYBYTES + NONCE_SIZE;
 }
 
+/**
+ * @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 headless 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_inplace(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);
+
+    /* Make room for zerofill at the beginning of message */
+    uint8_t * padding_begin = packet->frame_begin - crypto_secretbox_ZEROBYTES;
+    memset(padding_begin, 0, crypto_secretbox_ZEROBYTES);
+
+    /* 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]);
+
+    /* Adjust packet pointers and length for the prepended MAC */
+    packet->frame_begin -= crypto_secretbox_MACBYTES;
+    packet->frame_length += crypto_secretbox_MACBYTES;
+
+    /* 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 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 CSP_ERR_NONE;
+}
+
 void crypto_init() {
 
     crypto_key_generate(NULL, -1);

src/csp_if_cblk.c

@@ -59,23 +59,28 @@ 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_inplace(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);
+
+    for (int8_t frame_cnt = 0; frame_cnt < num_frames; frame_cnt++) {
 
+        ifdata->cblk_tx_lock(iface);
         cblk_frame_t * tx_ccsds_buf = ifdata->cblk_tx_buffer_get(iface);
         if (tx_ccsds_buf == NULL) {
             ifdata->cblk_tx_unlock(iface);
@@ -87,15 +92,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.data_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) {