Merge pull request #417 from nasa/208-aos-fhecf-support

Donnie-Ice · web-flow · commit 40f2dfcf0140 · 2025-03-13T17:28:00.000-04:00
208 aos fhecf support
diff --git a/include/crypto.h b/include/crypto.h
@@ -59,6 +59,7 @@
 /*
 ** User Prototypes
 */
+uint8_t gf_mul(uint8_t a, uint8_t b);
 
 // Crypto Library Configuration functions
 extern int32_t Crypto_Config_CryptoLib(uint8_t key_type, uint8_t mc_type, uint8_t sa_type, uint8_t cryptography_type,
@@ -240,6 +241,7 @@ void           Crypto_Local_Config(void);
 void           Crypto_Local_Init(void);
 int32_t        Crypto_window(uint8_t *actual, uint8_t *expected, int length, int window);
 uint16_t       Crypto_Calc_FECF(const uint8_t *ingest, int len_ingest);
+uint16_t       Crypto_Calc_FHECF(uint8_t *data);
 void           Crypto_Calc_CRC_Init_Table(void);
 uint16_t       Crypto_Calc_CRC16(uint8_t *data, int size);
 int32_t        Crypto_Check_Anti_Replay(SecurityAssociation_t *sa_ptr, uint8_t *arsn, uint8_t *iv);
@@ -317,6 +319,7 @@ extern TM_FrameSecurityHeader_t tm_frame_sec_hdr; // Used to reduce bit math dup
 // exterm AOS_t aos_frame
 extern AOS_FramePrimaryHeader_t  aos_frame_pri_hdr;
 extern AOS_FrameSecurityHeader_t aos_frame_sec_hdr; // Used to reduce bit math duplication
+extern uint8_t                   parity[4]; // Used in FHECF calc
 
 // Global configuration structs
 extern CryptoConfig_t                        crypto_config;
@@ -357,4 +360,17 @@ extern uint8_t badFECF;
 extern uint32_t crc32Table[CRC32TBL_SIZE];
 extern uint16_t crc16Table[CRC16TBL_SIZE];
 
+// GF(2^4) field and logarithm tables
+static const uint8_t gf_exp[30] = {
+    1, 2, 4, 8, 3, 6, 12, 11, 5, 10, 7, 14, 15, 13, 9, 1,
+    2, 4, 8, 3, 6, 12, 11, 5, 10, 7, 14, 15, 13, 9
+};
+
+static const uint8_t gf_log[GF_SIZE] = {
+    0, 0, 1, 4, 2, 8, 5, 10, 3, 14, 9, 7, 6, 13, 11, 12
+};
+
+// Generator polynomial coefficients for g(x) = x^4 + a^3x^3 + ax^2 + a^3x + 1
+static const uint8_t gen_poly[RS_PARITY + 1] = {1, 8, 2, 8, 1};
+
 #endif // CRYPTO_H
diff --git a/include/crypto_config.h b/include/crypto_config.h
@@ -123,6 +123,7 @@
 #define REF_SIZE            250
 #define OCF_SIZE            4
 #define MAC_SIZE            16 /* bytes */
+#define FHECF_SIZE          2
 #define FECF_SIZE           2
 #define TC_SEGMENT_HDR_SIZE 1
 #define ECS_SIZE            4    /* bytes */
@@ -231,6 +232,13 @@
 #define TM_CADU_SIZE TM_FRAME_DATA_SIZE
 #endif
 
+// AOS Behavior Defines
+// FHECF Calculation
+#define RS_SYMS 10   // Total symbols in codeword
+#define RS_DATA 6    // Data symbols
+#define RS_PARITY 4  // Parity symbols
+#define GF_SIZE 16   // 2^4
+
 // Logic Behavior Defines
 #define CRYPTO_FALSE 0
 #define CRYPTO_TRUE  1
diff --git a/include/crypto_error.h b/include/crypto_error.h
@@ -142,6 +142,7 @@
 #define CRYPTO_LIB_ERR_SHPLF_LEN_GREATER_THAN_MAX_PAD_SIZE                  (-69)
 #define CRYPTO_LIB_ERR_INVALID_SVC_TYPE_WITH_ARSN                           (-70)
 #define CRYPTO_LIB_ERR_ARSN_LT_SHSNF                                        (-71)
+#define CRYPTO_LIB_ERR_INVALID_FHECF                                        (-72)
 
 #define CRYPTO_CORE_ERROR_CODES_MAX -71
 
diff --git a/src/core/crypto.c b/src/core/crypto.c
@@ -59,6 +59,8 @@ uint8_t badSPI  = 0;
 uint8_t badIV   = 0;
 uint8_t badMAC  = 0;
 uint8_t badFECF = 0;
+// FHECF
+uint8_t parity[RS_PARITY];
 //  CRC
 uint32_t crc32Table[CRC32TBL_SIZE];
 uint16_t crc16Table[CRC16TBL_SIZE];
@@ -402,6 +404,50 @@ uint16_t Crypto_Calc_CRC16(uint8_t *data, int size)
     return crc;
 }
 
+uint8_t gf_mul(uint8_t a, uint8_t b) 
+{
+    if (a == 0 || b == 0) 
+    {
+        return 0;
+    }
+    else
+    {
+        return gf_exp[(gf_log[a] + gf_log[b]) % (GF_SIZE - 1)];
+    }
+}
+
+// Frame Header Error Control Field
+// Reference: CCSDS 732.0-B-4 (AOS Space Data Link Protocol) Section 4.1.2.6
+uint16_t Crypto_Calc_FHECF(uint8_t *data)
+{
+    uint8_t feedback = 0;
+    uint16_t result = 0;
+    int i = 0;
+    int j = 0;
+
+    // RS encoding
+    memset(parity, 0, RS_PARITY);
+    for (i = 0; i < RS_DATA; i++) 
+    {
+        feedback = data[i] ^ parity[0];
+        memmove(&parity[0], &parity[1], RS_PARITY - 1);
+        parity[RS_PARITY - 1] = 0;
+
+        for (j = 0; j < RS_PARITY; j++) 
+        {
+            parity[j] ^= gf_mul(feedback, gen_poly[j + 1]);
+        }
+    }
+#ifdef AOS_DEBUG
+    for (i = 0; i < RS_PARITY; i++)
+    {
+        printf("Parity[%d] = 0x%01x\n", i, parity[i]);
+    }
+#endif
+    result = (parity[0] << 12) | (parity[1] << 8) | (parity[2] << 4) | (parity[3] << 0);
+    return result;
+}
+
 /*
 ** Procedures Specifications
 */
diff --git a/src/core/crypto_aos.c b/src/core/crypto_aos.c
@@ -189,7 +189,23 @@ int32_t Crypto_AOS_ApplySecurity(uint8_t *pTfBuffer)
     // Detect if optional 2 byte FHEC is present
     if (current_managed_parameters_struct.aos_has_fhec == AOS_HAS_FHEC)
     {
-        idx += 2;
+        uint16_t recieved_fhecf = (((pTfBuffer[idx] << 8) & 0xFF00) | (pTfBuffer[idx + 1] & 0x00FF));
+#ifdef AOS_DEBUG
+        printf("Recieved FHECF: %04x\n", recieved_fhecf);
+        printf(KYEL "Calculating FHECF...\n" RESET);
+#endif
+        uint16_t calculated_fhecf = Crypto_Calc_FHECF(pTfBuffer);
+
+        if (recieved_fhecf != calculated_fhecf)
+        {
+            status = CRYPTO_LIB_ERR_INVALID_FHECF;
+            mc_if->mc_log(status);
+            return status;
+        }
+
+        pTfBuffer[idx] = (calculated_fhecf >> 8) & 0x00FF ;
+        pTfBuffer[idx+1] = (calculated_fhecf) & 0x00FF ;
+        idx = 8;
     }
 
     // Detect if optional variable length Insert Zone is present
@@ -327,6 +343,7 @@ int32_t Crypto_AOS_ApplySecurity(uint8_t *pTfBuffer)
      * ~~~Index currently at start of data field, AKA end of security header~~~
      **/
     data_loc = idx;
+    printf("IDX: %d", idx);
     // Calculate size of data to be encrypted
     pdu_len = current_managed_parameters_struct.max_frame_size - idx - sa_ptr->stmacf_len;
     // Check other managed parameter flags, subtract their lengths from data field if present
@@ -911,6 +928,7 @@ int32_t Crypto_AOS_ProcessSecurity(uint8_t *p_ingest, uint16_t len_ingest, uint8
     SecurityAssociation_t *sa_ptr          = NULL;
     uint8_t                sa_service_type = -1;
     uint8_t                spi             = -1;
+    uint8_t                aos_hdr_len      = 6;
 
     // Bit math to give concise access to values in the ingest
     aos_frame_pri_hdr.tfvn = ((uint8_t)p_ingest[0] & 0xC0) >> 6;
@@ -921,7 +939,7 @@ int32_t Crypto_AOS_ProcessSecurity(uint8_t *p_ingest, uint16_t len_ingest, uint8
     printf(KYEL "\n----- Crypto_AOS_ProcessSecurity START -----\n" RESET);
 #endif
 
-    if (len_ingest < 6) // Frame length doesn't even have enough bytes for header -- error out.
+    if (len_ingest < aos_hdr_len) // Frame length doesn't even have enough bytes for header -- error out.
     {
         status = CRYPTO_LIB_ERR_INPUT_FRAME_TOO_SHORT_FOR_AOS_STANDARD;
         mc_if->mc_log(status);
@@ -974,7 +992,24 @@ int32_t Crypto_AOS_ProcessSecurity(uint8_t *p_ingest, uint16_t len_ingest, uint8
     byte_idx = 6;
     if (current_managed_parameters_struct.aos_has_fhec == AOS_HAS_FHEC)
     {
+        uint16_t recieved_fhecf = (((p_ingest[aos_hdr_len] << 8) & 0xFF00) | (p_ingest[aos_hdr_len + 1] & 0x00FF));
+#ifdef AOS_DEBUG
+        printf("Recieved FHECF: %04x\n", recieved_fhecf);
+        printf(KYEL "Calculating FHECF...\n" RESET);
+#endif
+        uint16_t calculated_fhecf = Crypto_Calc_FHECF(p_ingest);
+
+        if (recieved_fhecf != calculated_fhecf)
+        {
+            status = CRYPTO_LIB_ERR_INVALID_FHECF;
+            mc_if->mc_log(status);
+            return status;
+        }
+
+        p_ingest[byte_idx] = (calculated_fhecf >> 8) & 0x00FF ;
+        p_ingest[byte_idx+1] = (calculated_fhecf) & 0x00FF ;
         byte_idx = 8;
+        aos_hdr_len = byte_idx;
     }
 
     // Determine if Insert Zone exists, increment past it if so
@@ -1105,7 +1140,6 @@ int32_t Crypto_AOS_ProcessSecurity(uint8_t *p_ingest, uint16_t len_ingest, uint8
 #ifdef FECF_DEBUG
                 printf(KYEL "FECF CALC MATCHES! - GOOD\n" RESET);
 #endif
-                ;
             }
         }
     }
@@ -1134,18 +1168,18 @@ int32_t Crypto_AOS_ProcessSecurity(uint8_t *p_ingest, uint16_t len_ingest, uint8
         return status;
     }
 
-    // Copy over AOS Primary Header (6 bytes)
-    memcpy(p_new_dec_frame, &p_ingest[0], 6);
+    // Copy over AOS Primary Header (6-8 bytes)
+    memcpy(p_new_dec_frame, &p_ingest[0], aos_hdr_len);
 
     // Copy over insert zone data, if it exists
     if (current_managed_parameters_struct.aos_has_iz == AOS_HAS_IZ)
     {
-        memcpy(p_new_dec_frame + 6, &p_ingest[6], current_managed_parameters_struct.aos_iz_len);
+        memcpy(p_new_dec_frame + aos_hdr_len, &p_ingest[aos_hdr_len], current_managed_parameters_struct.aos_iz_len);
 #ifdef AOS_DEBUG
         printf("Copied over the following:\n\t");
         for (int i = 0; i < current_managed_parameters_struct.aos_iz_len; i++)
         {
-            printf("%02X", p_ingest[6 + i]);
+            printf("%02X", p_ingest[aos_hdr_len + i]);
         }
         printf("\n");
 #endif
diff --git a/src/core/crypto_error.c b/src/core/crypto_error.c
@@ -92,7 +92,8 @@ char *crypto_enum_errlist_core[] = {(char *)"CRYPTO_LIB_SUCCESS",
                                     (char *)"CRYPTO_LIB_ERR_STMACF_LEN_GREATER_THAN_MAX_MAC_SIZE",
                                     (char *)"CRYPTO_LIB_ERR_SHPLF_LEN_GREATER_THAN_MAX_PAD_SIZE",
                                     (char *)"CRYPTO_LIB_ERR_INVALID_SVC_TYPE_WITH_ARSN",
-                                    (char *)"CRYPTO_LIB_ERR_ARSN_LT_SHSNF"};
+                                    (char *)"CRYPTO_LIB_ERR_ARSN_LT_SHSNF",
+                                    (char *)"CRYPTO_LIB_ERR_INVALID_FHECF"};
 
 char *crypto_enum_errlist_config[] = {
     (char *)"CRYPTO_CONFIGURATION_NOT_COMPLETE",
diff --git a/test/unit/ut_aos_apply.c b/test/unit/ut_aos_apply.c
diff --git a/test/unit/ut_aos_process.c b/test/unit/ut_aos_process.c
