diff --git a/include/crypto.h b/include/crypto.h index a153b774..95c96ea3 100644 --- a/include/crypto.h +++ b/include/crypto.h @@ -102,6 +102,44 @@ extern int32_t Crypto_TC_ProcessSecurity(uint8_t* ingest, int *len_ingest, TC_t* extern int32_t Crypto_TC_ApplySecurity_Cam(const uint8_t* p_in_frame, const uint16_t in_frame_length, uint8_t** pp_enc_frame, uint16_t* p_enc_frame_len, char* cam_cookies); extern int32_t Crypto_TC_ProcessSecurity_Cam(uint8_t* ingest, int *len_ingest, TC_t* tc_sdls_processed_frame, char* cam_cookies); + +int32_t Crypto_TC_Get_SA_Service_Type(uint8_t* sa_service_type, SecurityAssociation_t* sa_ptr); +int32_t Crypto_TC_Parse_Check_FECF(uint8_t* ingest, int* len_ingest, TC_t* tc_sdls_processed_frame); +int32_t Crypto_TC_Nontransmitted_IV_Increment(SecurityAssociation_t* sa_ptr, TC_t* tc_sdls_processed_frame); +int32_t Crypto_TC_Nontransmitted_SN_Increment(SecurityAssociation_t* sa_ptr, TC_t* tc_sdls_processed_frame); +int32_t Crypto_TC_Check_ACS_Keylen(crypto_key_t* akp, SecurityAssociation_t* sa_ptr); +int32_t Crypto_TC_Check_ECS_Keylen(crypto_key_t* ekp, SecurityAssociation_t* sa_ptr); +void Crypto_TC_Safe_Free_Ptr(uint8_t* ptr); +int32_t Crypto_TC_Do_Decrypt(uint8_t sa_service_type, uint8_t ecs_is_aead_algorithm, crypto_key_t* ekp, SecurityAssociation_t* sa_ptr, uint8_t* aad, TC_t* tc_sdls_processed_frame, uint8_t* ingest, uint16_t tc_enc_payload_start_index, uint16_t aad_len, char* cam_cookies, crypto_key_t* akp, uint8_t segment_hdr_len); +int32_t Crypto_TC_Process_Sanity_Check(int* len_ingest); +int32_t Crypto_TC_Prep_AAD(TC_t* tc_sdls_processed_frame, uint8_t fecf_len, uint8_t sa_service_type, uint8_t ecs_is_aead_algorithm, uint16_t* aad_len, SecurityAssociation_t* sa_ptr, uint8_t segment_hdr_len, uint8_t* ingest, uint8_t** aad); +int32_t Crypto_TC_Get_Keys(crypto_key_t** ekp, crypto_key_t** akp, SecurityAssociation_t* sa_ptr); +int32_t Crypto_TC_Check_IV_ARSN(SecurityAssociation_t* sa_ptr,TC_t* tc_sdls_processed_frame); +uint32_t Crypto_TC_Sanity_Validations(TC_t* tc_sdls_processed_frame, SecurityAssociation_t** sa_ptr); +void Crypto_TC_Get_Ciper_Mode_TCP(uint8_t sa_service_type, uint32_t* encryption_cipher, uint8_t* ecs_is_aead_algorithm, SecurityAssociation_t* sa_ptr); +int32_t Crypto_TC_Get_Ciper_Mode_TCA(uint8_t sa_service_type, uint32_t* encryption_cipher, uint8_t* ecs_is_aead_algorithm, SecurityAssociation_t* sa_ptr); +void Crypto_TC_Calc_Lengths(uint8_t* fecf_len, uint8_t* segment_hdr_len); +void Crypto_TC_Set_Segment_Header(TC_t* tc_sdls_processed_frame, uint8_t* ingest, int* byte_idx); +int32_t Crypto_TC_Check_CMD_Frame_Flag(uint8_t header_cc); +int32_t Crypto_TC_Validate_SA_Service_Type(uint8_t sa_service_type); +int32_t Crypto_TC_Handle_Enc_Padding(uint8_t sa_service_type, uint32_t* pkcs_padding, uint16_t* p_enc_frame_len, uint16_t* new_enc_frame_header_field_length, uint16_t tf_payload_len, SecurityAssociation_t* sa_ptr); +int32_t Crypto_TC_Frame_Validation(uint16_t* p_enc_frame_len); +int32_t Crypto_TC_Accio_Buffer(uint8_t** p_new_enc_frame, uint16_t* p_enc_frame_len); +int32_t Crypto_TC_ACS_Algo_Check(SecurityAssociation_t* sa_ptr); +int32_t Crypto_TC_Check_IV_Setup(SecurityAssociation_t* sa_ptr, uint8_t* p_new_enc_frame, uint16_t *index); +int32_t Crypto_TC_Do_Encrypt_PLAINTEXT(uint8_t sa_service_type, SecurityAssociation_t* sa_ptr, uint16_t* mac_loc, uint16_t tf_payload_len, uint8_t segment_hdr_len, uint8_t* p_new_enc_frame, crypto_key_t* ekp, uint8_t** aad, uint8_t ecs_is_aead_algorithm, uint16_t *index_p, const uint8_t* p_in_frame, char* cam_cookies, uint32_t pkcs_padding); +void Crypto_TC_Do_Encrypt_NONPLAINTEXT(uint8_t sa_service_type, SecurityAssociation_t* sa_ptr); +int32_t Crypto_TC_Do_Encrypt(uint8_t sa_service_type, SecurityAssociation_t* sa_ptr, uint16_t* mac_loc, uint16_t tf_payload_len, uint8_t segment_hdr_len, uint8_t* p_new_enc_frame, crypto_key_t* ekp, uint8_t** aad, uint8_t ecs_is_aead_algorithm, uint16_t *index_p, const uint8_t* p_in_frame, char* cam_cookies, uint32_t pkcs_padding, uint16_t new_enc_frame_header_field_length, uint16_t* new_fecf); +int32_t Crypto_TC_Check_Init_Setup(uint16_t in_frame_length); +int32_t Crypto_TC_Sanity_Setup(const uint8_t* p_in_frame, const uint16_t in_frame_length); +int32_t Crytpo_TC_Validate_TC_Temp_Header(const uint16_t in_frame_length, TC_FramePrimaryHeader_t temp_tc_header, const uint8_t* p_in_frame, uint8_t* map_id, uint8_t* segmentation_hdr, SecurityAssociation_t** sa_ptr); +int32_t Crypto_TC_Finalize_Frame_Setup(uint8_t sa_service_type, uint32_t* pkcs_padding, uint16_t* p_enc_frame_len, uint16_t* new_enc_frame_header_field_length, uint16_t tf_payload_len, SecurityAssociation_t** sa_ptr, uint8_t** p_new_enc_frame); +void Crypto_TC_Handle_Padding(uint32_t pkcs_padding, SecurityAssociation_t* sa_ptr, uint8_t* p_new_enc_frame, uint16_t* index); +int32_t Crypto_TC_Set_IV(SecurityAssociation_t* sa_ptr, uint8_t* p_new_enc_frame, uint16_t* index); + + + + // Telemetry (TM) extern int32_t Crypto_TM_ApplySecurity(uint8_t* pTfBuffer); extern int32_t Crypto_TM_ProcessSecurity(uint8_t* p_ingest, uint16_t len_ingest, uint8_t** pp_processed_frame, uint16_t *p_decrypted_length); @@ -109,6 +147,7 @@ extern int32_t Crypto_TM_ProcessSecurity(uint8_t* p_ingest, uint16_t len_ingest, extern int32_t Crypto_AOS_ApplySecurity(uint8_t* pTfBuffer); extern int32_t Crypto_AOS_ProcessSecurity(uint8_t* p_ingest, uint16_t len_ingest, uint8_t** pp_processed_frame, uint16_t* p_decrypted_length); + // Crypo Error Support Functions extern char* Crypto_Get_Error_Code_Enum_String(int32_t crypto_error_code); @@ -138,6 +177,10 @@ int32_t Crypto_Check_Anti_Replay(SecurityAssociation_t *sa_ptr, uint8_t *arsn, u int32_t Crypto_Get_ECS_Algo_Keylen(uint8_t algo); int32_t Crypto_Get_ACS_Algo_Keylen(uint8_t algo); +int32_t Crypto_Check_Anti_Replay_Verify_Pointers(SecurityAssociation_t* sa_ptr, uint8_t* arsn, uint8_t* iv); +int32_t Crypto_Check_Anti_Replay_ARSNW(SecurityAssociation_t* sa_ptr, uint8_t* arsn, int8_t* arsn_valid); +int32_t Crypto_Check_Anti_Replay_GCM(SecurityAssociation_t* sa_ptr, uint8_t* iv, int8_t* iv_valid); + // Key Management Functions int32_t Crypto_Key_OTAR(void); int32_t Crypto_Key_update(uint8_t state); diff --git a/include/crypto_error.h b/include/crypto_error.h index 7d29a751..63e0c71d 100644 --- a/include/crypto_error.h +++ b/include/crypto_error.h @@ -120,6 +120,7 @@ #define CRYPTO_LIB_ERR_MC_INIT (-48) #define CRYPTO_LIB_ERR_INPUT_FRAME_TOO_SHORT_FOR_AOS_STANDARD (-49) #define CRYPTO_LIB_ERR_TC_ENUM_USED_FOR_AOS_CONFIG (-50) +#define CRYPTO_LIB_ERR_INVALID_SA_SERVICE_TYPE (-51) extern char *crypto_enum_errlist_core[]; extern char *crypto_enum_errlist_config[]; diff --git a/src/core/crypto.c b/src/core/crypto.c index 528ece0c..c9633d6e 100644 --- a/src/core/crypto.c +++ b/src/core/crypto.c @@ -806,30 +806,45 @@ int32_t Crypto_Process_Extended_Procedure_Pdu(TC_t* tc_sdls_processed_frame, uin return status; } // End Process SDLS PDU -/* -** @brief: Check IVs and ARSNs to ensure within valid positive window if applicable -*/ -int32_t Crypto_Check_Anti_Replay(SecurityAssociation_t* sa_ptr, uint8_t* arsn, uint8_t* iv) + +/** + * @brief Function: Crypto_Check_Anti_Replay_Verify_Pointers + * Sanity Check, validates pointers, verifies non-null + * @param sa_ptr: SecurityAssociation_t* + * @param arsn: uint8_t* + * @param iv: uint8_t* + **/ +int32_t Crypto_Check_Anti_Replay_Verify_Pointers(SecurityAssociation_t* sa_ptr, uint8_t* arsn, uint8_t* iv) { int32_t status = CRYPTO_LIB_SUCCESS; - int8_t IV_VALID = -1; - int8_t ARSN_VALID = -1; - - // Check for NULL pointers if (sa_ptr == NULL) // #177 - Modification made per suggestion of 'Spicydll' - prevents null dereference { - return CRYPTO_LIB_ERR_NULL_SA; + status = CRYPTO_LIB_ERR_NULL_SA; + return status; } if (arsn == NULL && sa_ptr->arsn_len > 0) { - return CRYPTO_LIB_ERR_NULL_ARSN; + status = CRYPTO_LIB_ERR_NULL_ARSN; + return status; } if (iv == NULL && sa_ptr->shivf_len > 0 && crypto_config.cryptography_type != CRYPTOGRAPHY_TYPE_KMCCRYPTO) { - return CRYPTO_LIB_ERR_NULL_IV; + status = CRYPTO_LIB_ERR_NULL_IV; + return status; } - - // If sequence number field is greater than zero, check for replay + return status; +} + +/** + * @brief Function: Crypto_Check_Anti_Replay_ARSNW + * Sanity Check, validates ARSN within window + * @param sa_ptr: SecurityAssociation_t* + * @param arsn: uint8_t* + * @param arsn_valid: uint8_t* + **/ +int32_t Crypto_Check_Anti_Replay_ARSNW(SecurityAssociation_t* sa_ptr, uint8_t* arsn, int8_t* arsn_valid) +{ + int32_t status = CRYPTO_LIB_SUCCESS; if (sa_ptr->shsnf_len > 0) { // Check Sequence Number is in ARSNW @@ -855,11 +870,23 @@ int32_t Crypto_Check_Anti_Replay(SecurityAssociation_t* sa_ptr, uint8_t* arsn, u // Valid ARSN received, increment stored value else { - ARSN_VALID = CRYPTO_TRUE; + *arsn_valid = CRYPTO_TRUE; // memcpy(sa_ptr->arsn, arsn, sa_ptr->arsn_len); } } - // If IV is greater than zero and using GCM, check for replay + return status; +} + +/** + * @brief Function: Crypto_Check_Anti_Replay_GCM + * Sanity Check, validates IV within window + * @param sa_ptr: SecurityAssociation_t* + * @param iv: uint8_t* + * @param iv_valid: uint8_t* + **/ +int32_t Crypto_Check_Anti_Replay_GCM(SecurityAssociation_t* sa_ptr, uint8_t* iv, int8_t* iv_valid) +{ + int32_t status = CRYPTO_LIB_SUCCESS; if ((sa_ptr->iv_len > 0) && (sa_ptr->ecs == CRYPTO_CIPHER_AES256_GCM)) { // Check IV is in ARSNW @@ -893,20 +920,46 @@ int32_t Crypto_Check_Anti_Replay(SecurityAssociation_t* sa_ptr, uint8_t* arsn, u // Valid IV received, increment stored value else { - IV_VALID = CRYPTO_TRUE; + *iv_valid = CRYPTO_TRUE; // memcpy(sa_ptr->iv, iv, sa_ptr->iv_len); } } - // IV length is greater than zero, but not using an incrementing IV as in GCM - // we can't verify this internally as Crpytolib doesn't track previous IVs - // or generate random ones - // else{} + return status; +} + +/** + * @brief Function: Crypto_Check_Anti_Replay + * Verifies data within window. + * @param sa_ptr: SecurityAssociation_t* + * @param arsn: uint8_t* + * @param iv: uint8_t* + **/ +int32_t Crypto_Check_Anti_Replay(SecurityAssociation_t* sa_ptr, uint8_t* arsn, uint8_t* iv) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + int8_t iv_valid = -1; + int8_t arsn_valid = -1; + + // Check for NULL pointers + status = Crypto_Check_Anti_Replay_Verify_Pointers(sa_ptr, arsn, iv); + + // If sequence number field is greater than zero, check for replay + if(status == CRYPTO_LIB_SUCCESS) + { + status = Crypto_Check_Anti_Replay_ARSNW(sa_ptr, arsn, &arsn_valid); + } + + // If IV is greater than zero and using GCM, check for replay + if(status == CRYPTO_LIB_SUCCESS) + { + status = Crypto_Check_Anti_Replay_GCM(sa_ptr, iv, &iv_valid); + } // For GCM specifically, if have a valid IV... - if ((sa_ptr->ecs == CRYPTO_CIPHER_AES256_GCM) && (IV_VALID == CRYPTO_TRUE)) + if ((sa_ptr->ecs == CRYPTO_CIPHER_AES256_GCM) && (iv_valid == CRYPTO_TRUE)) { // Using ARSN? Need to be valid to increment both - if (sa_ptr->arsn_len > 0 && ARSN_VALID == CRYPTO_TRUE) + if (sa_ptr->arsn_len > 0 && arsn_valid == CRYPTO_TRUE) { memcpy(sa_ptr->iv, iv, sa_ptr->iv_len); memcpy(sa_ptr->arsn, arsn, sa_ptr->arsn_len); @@ -919,18 +972,25 @@ int32_t Crypto_Check_Anti_Replay(SecurityAssociation_t* sa_ptr, uint8_t* arsn, u } // If not GCM, and ARSN is valid - can incrmeent it - if (sa_ptr->ecs != CRYPTO_CIPHER_AES256_GCM && ARSN_VALID == CRYPTO_TRUE) + if (sa_ptr->ecs != CRYPTO_CIPHER_AES256_GCM && arsn_valid == CRYPTO_TRUE) { memcpy(sa_ptr->arsn, arsn, sa_ptr->arsn_len); } + if(status != CRYPTO_LIB_SUCCESS) + { + // Log error if it happened + mc_if->mc_log(status); + } + return status; } -/* -** @brief: For a given algorithm, return the associated key length in bytes -** @param: algo -*/ +/** +* @brief: Function: Crypto_Get_ECS_Algo_Keylen +* For a given algorithm, return the associated key length in bytes +* @param algo: uint8_t +**/ int32_t Crypto_Get_ECS_Algo_Keylen(uint8_t algo) { int32_t retval = -1; @@ -953,10 +1013,11 @@ int32_t Crypto_Get_ECS_Algo_Keylen(uint8_t algo) return retval; } -/* -** @brief: For a given algorithm, return the associated key length in bytes -** @param: algo -*/ +/** +* @brief: Function: Crypto_Get_ACS_Algo_Keylen +* For a given algorithm, return the associated key length in bytes +* @param algo: uint8_t +**/ int32_t Crypto_Get_ACS_Algo_Keylen(uint8_t algo) { int32_t retval = -1; @@ -979,6 +1040,11 @@ int32_t Crypto_Get_ACS_Algo_Keylen(uint8_t algo) return retval; } +/** +* @brief: Function: Crypto_Get_Security_Header_Length +* Return Security Header Length +* @param sa_ptr: SecurityAssociation_t* +**/ int32_t Crypto_Get_Security_Header_Length(SecurityAssociation_t* sa_ptr) { /* Narrator's Note: Leaving this here for future work @@ -1002,12 +1068,17 @@ int32_t Crypto_Get_Security_Header_Length(SecurityAssociation_t* sa_ptr) return securityHeaderLength; } +/** +* @brief: Function: Crypto_Get_Security_Trailer_Length +* Return Security Trailer Length +* @param sa_ptr: SecurityAssociation_t* +**/ int32_t Crypto_Get_Security_Trailer_Length(SecurityAssociation_t* sa_ptr) { if (!sa_ptr) { #ifdef DEBUG - printf(KRED "Get_Trailer_Header_Length passed Null SA!\n" RESET); + printf(KRED "Get_Trailer_Trailer_Length passed Null SA!\n" RESET); #endif return CRYPTO_LIB_ERR_NULL_SA; } @@ -1017,4 +1088,4 @@ int32_t Crypto_Get_Security_Trailer_Length(SecurityAssociation_t* sa_ptr) return securityTrailerLength; -} \ No newline at end of file +} diff --git a/src/core/crypto_error.c b/src/core/crypto_error.c index f049de06..4ced95a9 100644 --- a/src/core/crypto_error.c +++ b/src/core/crypto_error.c @@ -72,6 +72,7 @@ char *crypto_enum_errlist_core[] = (char*) "CRYPTO_LIB_ERR_MC_INIT", (char*) "CRYPTO_LIB_ERR_INPUT_FRAME_TOO_SHORT_FOR_AOS_STANDARD", (char*) "CRYPTO_LIB_ERR_TC_ENUM_USED_FOR_AOS_CONFIG", + (char*) "CRYPTO_LIB_ERR_INVALID_SA_SERVICE_TYPE", }; char *crypto_enum_errlist_config[] = @@ -133,6 +134,34 @@ char *crypto_enum_errlist_crypto_cam[] = (char*) "CAM_KEYTAB_FILE_KINIT_FAILURE", }; +/* +** @brief: Helper Function. Get specific error code, given code, allowable max, and valid string expansion +** @param: int32_t, int32_t, char* + * @return: char* +*/ +char* Crypto_Get_Crypto_Error_Code_String(int32_t crypto_error_code, int32_t crypto_error_code_max, char* valid_output_string) +{ + if(crypto_error_code < crypto_error_code_max) + { + return CRYPTO_UNDEFINED_ERROR; + } + return valid_output_string; +} + +/* +** @brief: Helper Function. Get specific error code, given code, allowable max, and valid string expansion +** @param: int32_t, int32_t, char* + * @return: char* +*/ +char* Crypto_Get_Error_Code_String(int32_t crypto_error_code, int32_t crypto_error_code_max, char* valid_output_string) +{ + if(crypto_error_code > crypto_error_code_max) + { + return CRYPTO_UNDEFINED_ERROR; + } + return valid_output_string; +} + /* ** @brief: For a given crypto error code, return the associated error code enum string ** @param: int32_t @@ -140,92 +169,34 @@ char *crypto_enum_errlist_crypto_cam[] = */ char* Crypto_Get_Error_Code_Enum_String(int32_t crypto_error_code) { + char* return_string = CRYPTO_UNDEFINED_ERROR; if(crypto_error_code >= 600) // CAM Error Codes { - if(crypto_error_code > 610) - { - return CRYPTO_UNDEFINED_ERROR; - } - else - { - return crypto_enum_errlist_crypto_cam[crypto_error_code % 600]; - } - + return_string = Crypto_Get_Error_Code_String(crypto_error_code, 610, crypto_enum_errlist_crypto_cam[crypto_error_code % 600]); } else if(crypto_error_code >= 500) // KMC Error Codes { - if(crypto_error_code > 515) - { - return CRYPTO_UNDEFINED_ERROR; - } - else - { - return crypto_enum_errlist_crypto_kmc[crypto_error_code % 500]; - } + return_string = Crypto_Get_Error_Code_String(crypto_error_code, 515, crypto_enum_errlist_crypto_kmc[crypto_error_code % 500]); } else if(crypto_error_code >= 400) // Crypto Interface Error Codes { - if(crypto_error_code > 402) - { - return CRYPTO_UNDEFINED_ERROR; - } - else - { - return crypto_enum_errlist_crypto_if[crypto_error_code % 400]; - } - + return_string = Crypto_Get_Error_Code_String(crypto_error_code, 402, crypto_enum_errlist_crypto_if[crypto_error_code % 400]); } else if(crypto_error_code >= 300) // SADB MariadDB Error Codes { - if(crypto_error_code > 303) - { - return CRYPTO_UNDEFINED_ERROR; - } - else - { - return crypto_enum_errlist_sa_mariadb[crypto_error_code % 300]; - } - + return_string = Crypto_Get_Error_Code_String(crypto_error_code, 303, crypto_enum_errlist_sa_mariadb[crypto_error_code % 300]); } else if(crypto_error_code >= 200) // SADB Interface Error Codes { - if(crypto_error_code > 201) - { - return CRYPTO_UNDEFINED_ERROR; - } - else - { - return crypto_enum_errlist_sa_if[crypto_error_code % 200]; - } + return_string = Crypto_Get_Error_Code_String(crypto_error_code, 201, crypto_enum_errlist_sa_if[crypto_error_code % 200]); } else if(crypto_error_code >= 100) // Configuration Error Codes { - if(crypto_error_code > 103) - { - return CRYPTO_UNDEFINED_ERROR; - } - else - { - return crypto_enum_errlist_config[crypto_error_code % 100]; - } - } - else if(crypto_error_code > 0) // Unused Error Codes 1-100 - { - return CRYPTO_UNDEFINED_ERROR; + return_string = Crypto_Get_Error_Code_String(crypto_error_code, 103, crypto_enum_errlist_config[crypto_error_code % 100]); } else if(crypto_error_code <= 0) // Cryptolib Core Error Codes { - if(crypto_error_code < -45) - { - return CRYPTO_UNDEFINED_ERROR; - } - else - { - return crypto_enum_errlist_core[(crypto_error_code * (-1))]; - } - } - else - { - return CRYPTO_UNDEFINED_ERROR; + return_string = Crypto_Get_Crypto_Error_Code_String(crypto_error_code, -51, crypto_enum_errlist_core[(crypto_error_code * (-1))]); } + return return_string; } \ No newline at end of file diff --git a/src/core/crypto_tc.c b/src/core/crypto_tc.c index 7fbec841..66854bcc 100644 --- a/src/core/crypto_tc.c +++ b/src/core/crypto_tc.c @@ -28,129 +28,93 @@ static int32_t crypto_tc_validate_sa(SecurityAssociation_t* sa); static int32_t crypto_handle_incrementing_nontransmitted_counter(uint8_t* dest, uint8_t* src, int src_full_len, int transmitted_len, int window); /** - * @brief Function: Crypto_TC_ApplySecurity - * Applies Security to incoming frame. Encryption, Authentication, and Authenticated Encryption - * @param p_in_frame: uint8* - * @param in_frame_length: uint16 - * @param pp_in_frame: uint8_t** - * @param p_enc_frame_len: uint16 - * @return int32: Success/Failure - **/ -int32_t Crypto_TC_ApplySecurity(const uint8_t* p_in_frame, const uint16_t in_frame_length, uint8_t** pp_in_frame, - uint16_t* p_enc_frame_len) -{ - // Passthrough to maintain original function signature when CAM isn't used. - return Crypto_TC_ApplySecurity_Cam(p_in_frame, in_frame_length, pp_in_frame, p_enc_frame_len, NULL); -} -/** - * @brief Function: Crypto_TC_ApplySecurity_Cam - * Applies Security to incoming frame. Encryption, Authentication, and Authenticated Encryption - * @param p_in_frame: uint8* - * @param in_frame_length: uint16 - * @param pp_in_frame: uint8_t** - * @param p_enc_frame_len: uint16 - * @param cam_cookies: char* - * @return int32: Success/Failure + * @brief Function: Crypto_TC_Get_SA_Service_Type + * Determines the SA service type + * @param sa_service_type: uint8* + * @param sa_ptr: SecurityAssociation_t* + * @return int32: ENUM - Service type **/ -int32_t Crypto_TC_ApplySecurity_Cam(const uint8_t* p_in_frame, const uint16_t in_frame_length, uint8_t** pp_in_frame, - uint16_t* p_enc_frame_len, char* cam_cookies) +int32_t Crypto_TC_Get_SA_Service_Type(uint8_t* sa_service_type, SecurityAssociation_t* sa_ptr) { - // Local Variables int32_t status = CRYPTO_LIB_SUCCESS; - TC_FramePrimaryHeader_t temp_tc_header; - SecurityAssociation_t* sa_ptr = NULL; - uint8_t* p_new_enc_frame = NULL; - uint8_t sa_service_type = -1; - uint16_t mac_loc = 0; - uint16_t tf_payload_len = 0x0000; - uint16_t new_fecf = 0x0000; - uint8_t* aad = NULL; - uint16_t new_enc_frame_header_field_length = 0; - uint32_t encryption_cipher = 0; - uint8_t ecs_is_aead_algorithm; - int i; - uint32_t pkcs_padding = 0; - crypto_key_t* ekp = NULL; -#ifdef DEBUG - printf(KYEL "\n----- Crypto_TC_ApplySecurity START -----\n" RESET); -#endif - - if (p_in_frame == NULL) + if ((sa_ptr->est == 0) && (sa_ptr->ast == 0)) { - status = CRYPTO_LIB_ERR_NULL_BUFFER; - printf(KRED "Error: Input Buffer NULL! \n" RESET); - mc_if->mc_log(status); - return status; // Just return here, nothing can be done. + *sa_service_type = SA_PLAINTEXT; } - -#ifdef DEBUG - printf("%d TF Bytes received\n", in_frame_length); - printf("DEBUG - "); - for (i = 0; i < in_frame_length; i++) + else if ((sa_ptr->est == 0) && (sa_ptr->ast == 1)) { - printf("%02X", ((uint8_t*)&*p_in_frame)[i]); + *sa_service_type = SA_AUTHENTICATION; } - printf("\nPrinted %d bytes\n", in_frame_length); -#else - // TODO - Find another way to know this and remove this argument - uint16_t tmp = in_frame_length; - tmp = tmp; -#endif - - if ((crypto_config.init_status == UNITIALIZED) || (mc_if == NULL) || (sa_if == NULL)) + else if ((sa_ptr->est == 1) && (sa_ptr->ast == 0)) { - printf(KRED "ERROR: CryptoLib Configuration Not Set! -- CRYPTO_LIB_ERR_NO_CONFIG, Will Exit\n" RESET); - status = CRYPTO_LIB_ERR_NO_CONFIG; - // Can't mc_log since it's not configured - return status; // return immediately so a NULL crypto_config is not dereferenced later + *sa_service_type = SA_ENCRYPTION; } - - if (in_frame_length < 5) // Frame length doesn't have enough bytes for TC TF header -- error out. + else if ((sa_ptr->est == 1) && (sa_ptr->ast == 1)) { - status = CRYPTO_LIB_ERR_INPUT_FRAME_TOO_SHORT_FOR_TC_STANDARD; - mc_if->mc_log(status); - return status; + *sa_service_type = SA_AUTHENTICATED_ENCRYPTION; } - - // Primary Header - temp_tc_header.tfvn = ((uint8_t)p_in_frame[0] & 0xC0) >> 6; - temp_tc_header.bypass = ((uint8_t)p_in_frame[0] & 0x20) >> 5; - temp_tc_header.cc = ((uint8_t)p_in_frame[0] & 0x10) >> 4; - temp_tc_header.spare = ((uint8_t)p_in_frame[0] & 0x0C) >> 2; - temp_tc_header.scid = ((uint8_t)p_in_frame[0] & 0x03) << 8; - temp_tc_header.scid = temp_tc_header.scid | (uint8_t)p_in_frame[1]; - temp_tc_header.vcid = ((uint8_t)p_in_frame[2] & 0xFC) >> 2 & crypto_config.vcid_bitmask; - temp_tc_header.fl = ((uint8_t)p_in_frame[2] & 0x03) << 8; - temp_tc_header.fl = temp_tc_header.fl | (uint8_t)p_in_frame[3]; - temp_tc_header.fsn = (uint8_t)p_in_frame[4]; - - if (in_frame_length < temp_tc_header.fl + 1) // Specified frame length larger than provided frame! + else { - status = CRYPTO_LIB_ERR_INPUT_FRAME_LENGTH_SHORTER_THAN_FRAME_HEADERS_LENGTH; + // Probably unnecessary check + // Leaving for now as it would be cleaner in SA to have an association enum returned I believe + printf(KRED "Error: SA Service Type is not defined! \n" RESET); + status = CRYPTO_LIB_ERROR; mc_if->mc_log(status); return status; } + return status; +} - // Lookup-retrieve managed parameters for frame via gvcid: - status = Crypto_Get_Managed_Parameters_For_Gvcid(temp_tc_header.tfvn, temp_tc_header.scid, temp_tc_header.vcid, - gvcid_managed_parameters, ¤t_managed_parameters); - if (status != CRYPTO_LIB_SUCCESS) +/** + * @brief Function: Crypto_TC_Get_Ciper_Mode_TCA + * Validates Cipher Mode + * @param sa_service_type: uint8_t + * @param encryption_cipher: uint32_t* + * @param ecs_is_aead_algorithm: uint8_t* + * @param sa_ptr: SecurityAssociation_t* + * @return int32: Cipher Mode or Error Enum + **/ +int32_t Crypto_TC_Get_Ciper_Mode_TCA(uint8_t sa_service_type, uint32_t* encryption_cipher, uint8_t* ecs_is_aead_algorithm, SecurityAssociation_t* sa_ptr) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + + if (sa_service_type != SA_PLAINTEXT) { - mc_if->mc_log(status); - return status; - } // Unable to get necessary Managed Parameters for TC TF -- return with error. + if (sa_ptr->ecs != CRYPTO_CIPHER_NONE) + { + *encryption_cipher = sa_ptr->ecs; +#ifdef TC_DEBUG + printf(KYEL "SA Encryption Cipher: %d\n", *encryption_cipher); +#endif + } + // If no pointer, must not be using ECS at all + else + { + *encryption_cipher = CRYPTO_CIPHER_NONE; + } + *ecs_is_aead_algorithm = Crypto_Is_AEAD_Algorithm(*encryption_cipher); + } - uint8_t segmentation_hdr = 0x00; - uint8_t map_id = 0; - if (current_managed_parameters->has_segmentation_hdr == TC_HAS_SEGMENT_HDRS) + if (*encryption_cipher == CRYPTO_CIPHER_NONE && sa_ptr->est == 1) { - segmentation_hdr = p_in_frame[5]; - map_id = segmentation_hdr & 0x3F; + status = CRYPTO_LIB_ERR_NO_ECS_SET_FOR_ENCRYPTION_MODE; + mc_if->mc_log(status); } + return status; +} - // Check if command frame flag set - if ((temp_tc_header.cc == 1) && (status == CRYPTO_LIB_SUCCESS)) +/** + * @brief Function: Crypto_TC_Check_CMD_Frame_Flag + * Validates the Command Frame Flag + * @param header_cc: uint8_t + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Check_CMD_Frame_Flag(uint8_t header_cc) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + + if ((header_cc == 1) && (status == CRYPTO_LIB_SUCCESS)) { /* ** CCSDS 232.0-B-3 @@ -162,655 +126,1559 @@ int32_t Crypto_TC_ApplySecurity_Cam(const uint8_t* p_in_frame, const uint16_t in #endif status = CRYPTO_LIB_ERR_INVALID_CC_FLAG; mc_if->mc_log(status); - return status; } + return status; +} + +/** + * @brief Function: Crypto_TC_Validate_SA_Service_Type + * Validates the SA service type + * @param sa_service_type: uint8_t + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Validate_SA_Service_Type(uint8_t sa_service_type) +{ + int32_t status = CRYPTO_LIB_SUCCESS; - if (status == CRYPTO_LIB_SUCCESS) + if ((sa_service_type != SA_PLAINTEXT) && (sa_service_type != SA_AUTHENTICATED_ENCRYPTION) && (sa_service_type != SA_ENCRYPTION) && (sa_service_type != SA_AUTHENTICATION)) { - status = sa_if->sa_get_operational_sa_from_gvcid(temp_tc_header.tfvn, temp_tc_header.scid, - temp_tc_header.vcid, map_id, &sa_ptr); - // If unable to get operational SA, can return - if (status != CRYPTO_LIB_SUCCESS) - { - mc_if->mc_log(status); - return status; - } + printf(KRED "Unknown SA Service Type Detected!\n" RESET); + status = CRYPTO_LIB_ERR_INVALID_SA_SERVICE_TYPE; + } + return status; +} - // Try to assure SA is sane - status = crypto_tc_validate_sa(sa_ptr); - if (status != CRYPTO_LIB_SUCCESS) +/** + * @brief Function: Crypto_TC_Handle_Enc_Padding + * Handles Padding as necessary, returns success/failure + * @param sa_service_type: uint8_t + * @param pkcs_padding: uint32_t* + * @param p_enc_frame_len: uint16_t* + * @param new_enc_frame_header_field_length: uint16_t* + * @param tf_payload_len: uint16_t + * @param sa_ptr: SecurityAssociation_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Handle_Enc_Padding(uint8_t sa_service_type, uint32_t* pkcs_padding, uint16_t* p_enc_frame_len, uint16_t* new_enc_frame_header_field_length, uint16_t tf_payload_len, SecurityAssociation_t* sa_ptr) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + if (sa_service_type == SA_ENCRYPTION) + { + // Handle Padding, if necessary + if (sa_ptr->ecs == CRYPTO_CIPHER_AES256_CBC) { - mc_if->mc_log(status); - return status; + *pkcs_padding = tf_payload_len % TC_BLOCK_SIZE; // Block Sizes of 16 + + *pkcs_padding = TC_BLOCK_SIZE - *pkcs_padding; // Could potentially need 16 bytes of padding. + + *p_enc_frame_len += *pkcs_padding; // Add the necessary padding to the frame_len + new pad length field + + *new_enc_frame_header_field_length = (*p_enc_frame_len) - 1; +#ifdef DEBUG + + printf("SHPLF_LEN: %d\n", sa_ptr->shplf_len); + printf("Padding Needed: %d\n", *pkcs_padding); + printf("Previous data_len: %d\n", tf_payload_len); + printf("New data_len: %d\n", (tf_payload_len + *pkcs_padding)); + printf("New enc_frame_len: %d\n", (*p_enc_frame_len)); +#endif + // Don't Exceed Max Frame Size! 1024 + if (*p_enc_frame_len > TC_MAX_FRAME_SIZE) + { + status = CRYPTO_LIB_ERR_TC_FRAME_SIZE_EXCEEDS_SPEC_LIMIT; + mc_if->mc_log(status); + } } + } + return status; +} -#ifdef SA_DEBUG - printf(KYEL "DEBUG - Printing SA Entry for current frame.\n" RESET); - Crypto_saPrint(sa_ptr); +/** + * @brief Function: Crypto_TC_Frame_Validation + * Frame validation - sanity check + * @param p_enc_frame_len: uint16_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Frame_Validation(uint16_t* p_enc_frame_len) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + if (*p_enc_frame_len > current_managed_parameters->max_frame_size) + { +#ifdef DEBUG + printf("Managed length is: %d\n", current_managed_parameters->max_frame_size); + printf("New enc frame length will be: %d\n", *p_enc_frame_len); #endif + printf(KRED "Error: New frame would violate maximum tc frame managed parameter! \n" RESET); + status = CRYPTO_LIB_ERR_TC_FRAME_SIZE_EXCEEDS_MANAGED_PARAM_MAX_LIMIT; + mc_if->mc_log(status); + printf("STATUS=%d\n", status); + return status; + } + // Ensure the frame to be created will not violate spec max length + if ((*p_enc_frame_len > 1024) && status == CRYPTO_LIB_SUCCESS) + { + printf(KRED "Error: New frame would violate specification max TC frame size! \n" RESET); + status = CRYPTO_LIB_ERR_TC_FRAME_SIZE_EXCEEDS_SPEC_LIMIT; + mc_if->mc_log(status); + } + return status; +} + +/** + * TODO: Should this be pre-allocated in the library? + * @brief Function: Crypto_TC_Accio_Buffer + * Buffer creation for KMC + * @param p_new_enc_frame: uint8_t** + * @param p_enc_frame_len: uint16_t* + * @return int32: Creates Buffer, Returns Success/Failure + **/ +int32_t Crypto_TC_Accio_Buffer(uint8_t** p_new_enc_frame, uint16_t* p_enc_frame_len) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + *p_new_enc_frame = (uint8_t*)malloc((*p_enc_frame_len) * sizeof(uint8_t)); + if (!p_new_enc_frame) + { + printf(KRED "Error: Malloc for encrypted output buffer failed! \n" RESET); + status = CRYPTO_LIB_ERROR; + mc_if->mc_log(status); + return status; + } + memset(*p_new_enc_frame, 0, *p_enc_frame_len); + return status; +} - // Determine SA Service Type - if ((sa_ptr->est == 0) && (sa_ptr->ast == 0)) +/** + * @brief Function: Crypto_TC_ACS_Algo_Check + * Verifies ACS Algorithm - Sanity Check + * @param sa_ptr: SecurityAssociation_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_ACS_Algo_Check(SecurityAssociation_t* sa_ptr) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + if ((sa_ptr->est == 0) && (sa_ptr->ast == 1)) + { + if (sa_ptr->acs_len != 0) { - sa_service_type = SA_PLAINTEXT; + if ((sa_ptr->acs == CRYPTO_MAC_CMAC_AES256 || sa_ptr->acs == CRYPTO_MAC_HMAC_SHA256 || sa_ptr->acs == CRYPTO_MAC_HMAC_SHA512) && + sa_ptr->iv_len > 0) + { + status = CRYPTO_LIB_ERR_IV_NOT_SUPPORTED_FOR_ACS_ALGO; + mc_if->mc_log(status); + } } - else if ((sa_ptr->est == 0) && (sa_ptr->ast == 1)) + } + return status; +} + +/** + * @brief Function: Crypto_TC_Check_IV_Setup + * Verifies IV - Sanity Check + * @param sa_ptr: SecurityAssociation_t* + * @param p_new_enc_frame: uint8_t* + * @param index: uint16_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Check_IV_Setup(SecurityAssociation_t* sa_ptr, uint8_t* p_new_enc_frame, uint16_t *index) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + int i; + uint16_t index_temp = *index; + if (crypto_config.iv_type == IV_INTERNAL) + { + // Start index from the transmitted portion + for (i = sa_ptr->iv_len - sa_ptr->shivf_len; i < sa_ptr->iv_len; i++) { - sa_service_type = SA_AUTHENTICATION; + *(p_new_enc_frame + index_temp) = *(sa_ptr->iv + i); + index_temp++; } - else if ((sa_ptr->est == 1) && (sa_ptr->ast == 0)) + } + // IV is NULL / IV_CRYPTO_MODULE + else + { + // Transmitted length > 0, AND using KMC_CRYPTO + if ((sa_ptr->shivf_len > 0) && (crypto_config.cryptography_type == CRYPTOGRAPHY_TYPE_KMCCRYPTO)) { - sa_service_type = SA_ENCRYPTION; + index_temp += sa_ptr->iv_len - (sa_ptr->iv_len - sa_ptr->shivf_len); } - else if ((sa_ptr->est == 1) && (sa_ptr->ast == 1)) + else if (sa_ptr->shivf_len == 0) { - sa_service_type = SA_AUTHENTICATED_ENCRYPTION; + // IV isn't being used, so don't care if it's Null } else { - // Probably unnecessary check - // Leaving for now as it would be cleaner in SA to have an association enum returned I believe - printf(KRED "Error: SA Service Type is not defined! \n" RESET); - status = CRYPTO_LIB_ERROR; + status = CRYPTO_LIB_ERR_NULL_IV; mc_if->mc_log(status); return status; } + } + *index = index_temp; + return status; +} - // Determine Algorithm cipher & mode. // TODO - Parse authentication_cipher, and handle AEAD cases properly - if (sa_service_type != SA_PLAINTEXT) + +/** + * @brief Function: Crypto_TC_Do_Encrypt_PLAINTEXT + * Handles Plaintext TC Encryption + * @param sa_service_type: uint8_t + * @param sa_ptr: SecurityAssociation_t* + * @param mac_loc: uint16_t* + * @param tf_payload_len: uint16_t + * @param segment_hdr_len: uint8_t + * @param p_new_enc_frame: uint8_t* + * @param ekp: crypto_key_t* + * @param aad: uint8_t** + * @param ecs_is_aead_algorithm: uint8_t + * @param index_p: uint16_t* + * @param p_in_frame: const uint8_t* + * @param cam_cookies: char* + * @param pkcs_padding:uint32_t + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Do_Encrypt_PLAINTEXT(uint8_t sa_service_type, SecurityAssociation_t* sa_ptr, uint16_t* mac_loc, uint16_t tf_payload_len, uint8_t segment_hdr_len, uint8_t* p_new_enc_frame, crypto_key_t* ekp, uint8_t** aad, uint8_t ecs_is_aead_algorithm, uint16_t *index_p, const uint8_t* p_in_frame, char* cam_cookies, uint32_t pkcs_padding) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + uint16_t index = *index_p; + if (sa_service_type != SA_PLAINTEXT) + { + uint8_t* mac_ptr = NULL; + uint16_t aad_len = 0; + + if (sa_service_type == SA_AUTHENTICATED_ENCRYPTION || sa_service_type == SA_AUTHENTICATION) { - if (sa_ptr->ecs != CRYPTO_CIPHER_NONE) + *mac_loc = TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN + sa_ptr->shivf_len + sa_ptr->shsnf_len + + sa_ptr->shplf_len + tf_payload_len; +#ifdef MAC_DEBUG + printf(KYEL "MAC location is: %d\n" RESET, *mac_loc); + printf(KYEL "MAC size is: %d\n" RESET, sa_ptr->stmacf_len); +#endif + mac_ptr = &p_new_enc_frame[*mac_loc]; + + // Prepare the Header AAD (CCSDS 335.0-B-1 4.2.3.2.2.3) + aad_len = TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN + sa_ptr->shivf_len + + sa_ptr->shsnf_len + sa_ptr->shplf_len; + if (sa_service_type == SA_AUTHENTICATION) // auth only, we authenticate the payload as part of the AEAD encrypt call here { - encryption_cipher = sa_ptr->ecs; + aad_len += tf_payload_len; + } #ifdef TC_DEBUG - printf(KYEL "SA Encryption Cipher: %d\n", encryption_cipher); + printf("Calculated AAD Length: %d\n", aad_len); #endif - } - // If no pointer, must not be using ECS at all - else + if (sa_ptr->abm_len < aad_len) { - encryption_cipher = CRYPTO_CIPHER_NONE; + status = CRYPTO_LIB_ERR_ABM_TOO_SHORT_FOR_AAD; + mc_if->mc_log(status); + return status; } - ecs_is_aead_algorithm = Crypto_Is_AEAD_Algorithm(encryption_cipher); - } - - if (encryption_cipher == CRYPTO_CIPHER_NONE && sa_ptr->est == 1) - { - status = CRYPTO_LIB_ERR_NO_ECS_SET_FOR_ENCRYPTION_MODE; - mc_if->mc_log(status); - return status; + *aad = Crypto_Prepare_TC_AAD(p_new_enc_frame, aad_len, sa_ptr->abm); } #ifdef TC_DEBUG - switch (sa_service_type) - { - case SA_PLAINTEXT: - printf(KBLU "Creating a TC - CLEAR!\n" RESET); - break; - case SA_AUTHENTICATION: - printf(KBLU "Creating a TC - AUTHENTICATED!\n" RESET); - break; - case SA_ENCRYPTION: - printf(KBLU "Creating a TC - ENCRYPTED!\n" RESET); - break; - case SA_AUTHENTICATED_ENCRYPTION: - printf(KBLU "Creating a TC - AUTHENTICATED ENCRYPTION!\n" RESET); - break; - } + printf("Encrypted bytes output_loc is %d\n", index); + printf("Input bytes input_loc is %d\n", TC_FRAME_HEADER_SIZE + segment_hdr_len); #endif - // Determine if segment header exists - uint8_t segment_hdr_len = TC_SEGMENT_HDR_SIZE; - if (current_managed_parameters->has_segmentation_hdr == TC_NO_SEGMENT_HDRS) + /* Get Key */ + ekp = key_if->get_key(sa_ptr->ekid); + if (ekp == NULL) { - segment_hdr_len = 0; + status = CRYPTO_LIB_ERR_KEY_ID_ERROR; + mc_if->mc_log(status); + return status; } - // Determine if FECF exists - uint8_t fecf_len = FECF_SIZE; - if (current_managed_parameters->has_fecf == TC_NO_FECF) + if (ecs_is_aead_algorithm == CRYPTO_TRUE) { - fecf_len = 0; - } - - // Calculate tf_payload length here to be used in other logic - tf_payload_len = temp_tc_header.fl - TC_FRAME_HEADER_SIZE - segment_hdr_len - fecf_len + 1; - - /** - * A note on plaintext: Take a permissive approach to allow the lengths of fields that aren't going to be used. - * The 355.0-B-2 (July 2022) says the following in $4.2.2.4: - * 'It is possible to create a ‘clear mode’ SA using one of the defined service types by - specifying the algorithm as a ‘no-op’ function (no actual cryptographic operation to - be performed). Such an SA might be used, for example, during development - testing of other aspects of data link processing before cryptographic capabilities are - available for integrated testing.In this scenario, the Security Header and Trailer - field lengths are kept constant across all supported configurations. For security - reasons, the use of such an SA is not recommended in normal operation.' - */ - - // Calculate frame lengths based on SA fields - *p_enc_frame_len = temp_tc_header.fl + 1 + 2 + sa_ptr->shivf_len + sa_ptr->shsnf_len + sa_ptr->shplf_len + sa_ptr->stmacf_len; - new_enc_frame_header_field_length = (*p_enc_frame_len) - 1; + // Check that key length to be used ets the algorithm requirement + if ((int32_t)ekp->key_len != Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs)) + { + Crypto_TC_Safe_Free_Ptr(*aad); + status = CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; + mc_if->mc_log(status); + return status; + } - if (sa_service_type == SA_ENCRYPTION) + status = cryptography_if->cryptography_aead_encrypt(&p_new_enc_frame[index], // ciphertext output + (size_t)tf_payload_len, // length of data + (uint8_t*)(p_in_frame + TC_FRAME_HEADER_SIZE + segment_hdr_len), // plaintext input + (size_t)tf_payload_len, // in data length + &(ekp->value[0]), // Key + Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs), // Length of key derived from sa_ptr key_ref + sa_ptr, // SA (for key reference) + sa_ptr->iv, // IV + sa_ptr->iv_len, // IV Length + mac_ptr, // tag output + sa_ptr->stmacf_len, // tag size + *aad, // AAD Input + aad_len, // Length of AAD + (sa_ptr->est == 1), + (sa_ptr->ast == 1), + (sa_ptr->ast == 1), + &sa_ptr->ecs, // encryption cipher + &sa_ptr->acs, // authentication cipher + cam_cookies); + } + else // non aead algorithm { - // Handle Padding, if necessary - if (sa_ptr->ecs == CRYPTO_CIPHER_AES256_CBC) + // TODO - implement non-AEAD algorithm logic + if (sa_service_type == SA_ENCRYPTION) { - pkcs_padding = tf_payload_len % TC_BLOCK_SIZE; // Block Sizes of 16 - - pkcs_padding = TC_BLOCK_SIZE - pkcs_padding; // Could potentially need 16 bytes of padding. - - *p_enc_frame_len += pkcs_padding; // Add the necessary padding to the frame_len + new pad length field + // Check that key length to be used ets the algorithm requirement + if ((int32_t)ekp->key_len != Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs)) + { + Crypto_TC_Safe_Free_Ptr(*aad); + return CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; + } - new_enc_frame_header_field_length = (*p_enc_frame_len) - 1; -#ifdef DEBUG + status = cryptography_if->cryptography_encrypt(&p_new_enc_frame[index], // ciphertext output + (size_t)tf_payload_len, + &p_new_enc_frame[index], // length of data + //(uint8_t*)(p_in_frame + TC_FRAME_HEADER_SIZE + segment_hdr_len), // plaintext input + (size_t)tf_payload_len, // in data length + // new_frame_length, + &(ekp->value[0]), // Key + Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs), // Length of key derived from sa_ptr key_ref + sa_ptr, // SA (for key reference) + sa_ptr->iv, // IV + sa_ptr->iv_len, // IV Length + &sa_ptr->ecs, // encryption cipher + pkcs_padding, + cam_cookies); + } - printf("SHPLF_LEN: %d\n", sa_ptr->shplf_len); - printf("Padding Needed: %d\n", pkcs_padding); - printf("Previous data_len: %d\n", tf_payload_len); - printf("New data_len: %d\n", (tf_payload_len + pkcs_padding)); - printf("New enc_frame_len: %d\n", (*p_enc_frame_len)); -#endif - // Don't Exceed Max Frame Size! 1024 - if (*p_enc_frame_len > TC_MAX_FRAME_SIZE) + if (sa_service_type == SA_AUTHENTICATION) + { + /* Get Key */ + crypto_key_t* akp = NULL; + akp = key_if->get_key(sa_ptr->akid); + if (akp == NULL) { - status = CRYPTO_LIB_ERR_TC_FRAME_SIZE_EXCEEDS_SPEC_LIMIT; - mc_if->mc_log(status); - return status; + return CRYPTO_LIB_ERR_KEY_ID_ERROR; } - } - } - if (sa_service_type != (SA_PLAINTEXT || SA_AUTHENTICATED_ENCRYPTION || SA_ENCRYPTION || SA_AUTHENTICATION)) - { - printf(KRED "Unknown SA Service Type Detected!" RESET); - } + // Check that key length to be used ets the algorithm requirement + if ((int32_t)akp->key_len != Crypto_Get_ACS_Algo_Keylen(sa_ptr->acs)) + { + Crypto_TC_Safe_Free_Ptr(*aad); + return CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; + } - // Ensure the frame to be created will not violate managed parameter maximum length - if (*p_enc_frame_len > current_managed_parameters->max_frame_size) - { -#ifdef DEBUG - printf("Managed length is: %d\n", current_managed_parameters->max_frame_size); - printf("New enc frame length will be: %d\n", *p_enc_frame_len); -#endif - printf(KRED "Error: New frame would violate maximum tc frame managed parameter! \n" RESET); - status = CRYPTO_LIB_ERR_TC_FRAME_SIZE_EXCEEDS_MANAGED_PARAM_MAX_LIMIT; - mc_if->mc_log(status); - return status; - } - // Ensure the frame to be created will not violate spec max length - if (*p_enc_frame_len > 1024) + status = cryptography_if->cryptography_authenticate(&p_new_enc_frame[index], // ciphertext output + (size_t)tf_payload_len, // length of data + (uint8_t*)(p_in_frame + TC_FRAME_HEADER_SIZE + segment_hdr_len), // plaintext input + (size_t)tf_payload_len, // in data length + &(akp->value[0]), // Key + Crypto_Get_ACS_Algo_Keylen(sa_ptr->acs), + sa_ptr, // SA (for key reference) + sa_ptr->iv, // IV + sa_ptr->iv_len, // IV Length + mac_ptr, // tag output + sa_ptr->stmacf_len, // tag size + *aad, // AAD Input + aad_len, // Length of AAD + sa_ptr->ecs, // encryption cipher + sa_ptr->acs, // authentication cipher + cam_cookies); + } + + } + *index_p = index; + if (status != CRYPTO_LIB_SUCCESS) { - printf(KRED "Error: New frame would violate specification max TC frame size! \n" RESET); - status = CRYPTO_LIB_ERR_TC_FRAME_SIZE_EXCEEDS_SPEC_LIMIT; + Crypto_TC_Safe_Free_Ptr(*aad); mc_if->mc_log(status); - return status; + return status; // Cryptography IF call failed, return. } + } + return status; +} - // Accio buffer - p_new_enc_frame = (uint8_t*)malloc((*p_enc_frame_len) * sizeof(uint8_t)); - if (!p_new_enc_frame) +/** + * @brief Function: Crypto_TC_Do_Encrypt_NONPLAINTEXT + * Handles NON-Plaintext TC Encryption + * @param sa_service_type: uint8_t + * @param sa_ptr: SecurityAssociation_t* + * @return int32: Success/Failure + **/ +void Crypto_TC_Do_Encrypt_NONPLAINTEXT(uint8_t sa_service_type, SecurityAssociation_t* sa_ptr) +{ + if (sa_service_type != SA_PLAINTEXT) + { +#ifdef INCREMENT + if (crypto_config.crypto_increment_nontransmitted_iv == SA_INCREMENT_NONTRANSMITTED_IV_TRUE) { - printf(KRED "Error: Malloc for encrypted output buffer failed! \n" RESET); - status = CRYPTO_LIB_ERROR; - mc_if->mc_log(status); - return status; + if (sa_ptr->shivf_len > 0 && sa_ptr->iv_len != 0) + { + Crypto_increment(sa_ptr->iv, sa_ptr->iv_len); + } } - memset(p_new_enc_frame, 0, *p_enc_frame_len); - -#ifdef TC_DEBUG - printf(KYEL "DEBUG - Total TC Buffer to be malloced is: %d bytes\n" RESET, *p_enc_frame_len); - printf(KYEL "\tlen of TF\t = %d\n" RESET, temp_tc_header.fl); - printf(KYEL "\tsegment hdr len\t = %d\n" RESET, segment_hdr_len); - printf(KYEL "\tspi len\t\t = 2\n" RESET); - printf(KYEL "\tshivf_len\t = %d\n" RESET, sa_ptr->shivf_len); - printf(KYEL "\tiv_len\t\t = %d\n" RESET, sa_ptr->iv_len); - printf(KYEL "\tshsnf_len\t = %d\n" RESET, sa_ptr->shsnf_len); - printf(KYEL "\tshplf len\t = %d\n" RESET, sa_ptr->shplf_len); - printf(KYEL "\tarsn_len\t = %d\n" RESET, sa_ptr->arsn_len); - printf(KYEL "\tstmacf_len\t = %d\n" RESET, sa_ptr->stmacf_len); -#endif - - // Copy original TF header, w/ segment header if applicable - memcpy(p_new_enc_frame, p_in_frame, TC_FRAME_HEADER_SIZE + segment_hdr_len); - - // Set new TF Header length - // Recall: Length field is one minus total length per spec - *(p_new_enc_frame + 2) = - ((*(p_new_enc_frame + 2) & 0xFC) | (((new_enc_frame_header_field_length) & (0x0300)) >> 8)); - *(p_new_enc_frame + 3) = ((new_enc_frame_header_field_length) & (0x00FF)); - -#ifdef TC_DEBUG - printf(KYEL "Printing updated TF Header:\n\t"); - for (i = 0; i < TC_FRAME_HEADER_SIZE; i++) + else // SA_INCREMENT_NONTRANSMITTED_IV_FALSE { - printf("%02X",*(p_new_enc_frame + i)); + // Only increment the transmitted portion + if (sa_ptr->shivf_len > 0 && sa_ptr->iv_len != 0) + { + Crypto_increment(sa_ptr->iv + (sa_ptr->iv_len - sa_ptr->shivf_len), sa_ptr->shivf_len); + } } - // Recall: The buffer length is 1 greater than the field value set in the TCTF - printf("\n\tLength set to 0x%02X\n" RESET, new_enc_frame_header_field_length); -#endif - - /* - ** Start variable length fields - */ - uint16_t index = TC_FRAME_HEADER_SIZE; // Frame header is 5 bytes - - if (current_managed_parameters->has_segmentation_hdr == TC_HAS_SEGMENT_HDRS) + if (sa_ptr->shsnf_len > 0) { - index++; // Add 1 byte to index because segmentation header used for this gvcid. + Crypto_increment(sa_ptr->arsn, sa_ptr->arsn_len); } - /* - ** Begin Security Header Fields - ** Reference CCSDS SDLP 3550b1 4.1.1.1.3 - */ - // Set SPI - *(p_new_enc_frame + index) = ((sa_ptr->spi & 0xFF00) >> 8); - *(p_new_enc_frame + index + 1) = (sa_ptr->spi & 0x00FF); - index += 2; - - // Set initialization vector if specified #ifdef SA_DEBUG - if (sa_ptr->shivf_len > 0 && sa_ptr->iv != NULL) + int i = 0; + if (sa_ptr->iv_len > 0) { - printf(KYEL "Using IV value:\n\t"); + printf(KYEL "Next IV value is:\n\t"); for (i = 0; i < sa_ptr->iv_len; i++) { printf("%02x", *(sa_ptr->iv + i)); } printf("\n" RESET); - printf(KYEL "Transmitted IV value:\n\t"); + printf(KYEL "Next transmitted IV value is:\n\t"); for (i = sa_ptr->iv_len - sa_ptr->shivf_len; i < sa_ptr->iv_len; i++) { printf("%02x", *(sa_ptr->iv + i)); } printf("\n" RESET); } + printf(KYEL "Next ARSN value is:\n\t"); + for (i = 0; i < sa_ptr->arsn_len; i++) + { + printf("%02x", *(sa_ptr->arsn + i)); + } + printf("\n" RESET); + printf(KYEL "Next transmitted ARSN value is:\n\t"); + for (i = sa_ptr->arsn_len - sa_ptr->shsnf_len; i < sa_ptr->arsn_len; i++) + { + printf("%02x", *(sa_ptr->arsn + i)); + } + printf("\n" RESET); +#endif #endif + } +} + +/** + * @brief Function: Crypto_TC_Do_Encrypt + * Starts TC Encryption - Handles Plaintext and NON Plaintext + * @param sa_service_type: uint8_t + * @param sa_ptr: SecurityAssociation_t* + * @param mac_loc: uint16_t* + * @param tf_payload_len: uint16_t + * @param segment_hdr_len: uint8_t + * @param p_new_enc_frame: uint8_t* + * @param ekp: crypto_key_t* + * @param aad: uint8_t** + * @param ecs_is_aead_algorithm: uint8_t + * @param index_p: uint16_t* + * @param p_in_frame: const uint8_t* + * @param cam_cookies: char* + * @param pkcs_padding:uint32_t + * @param new_enc_frame_header_field_length: uint16_t + * @param new_fecf: uint16_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Do_Encrypt(uint8_t sa_service_type, SecurityAssociation_t* sa_ptr, uint16_t* mac_loc, uint16_t tf_payload_len, uint8_t segment_hdr_len, uint8_t* p_new_enc_frame, crypto_key_t* ekp, uint8_t** aad, uint8_t ecs_is_aead_algorithm, uint16_t *index_p, const uint8_t* p_in_frame, char* cam_cookies, uint32_t pkcs_padding, uint16_t new_enc_frame_header_field_length, uint16_t* new_fecf) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + uint16_t index = *index_p; + status = Crypto_TC_Do_Encrypt_PLAINTEXT(sa_service_type, sa_ptr, mac_loc, tf_payload_len, segment_hdr_len, p_new_enc_frame, ekp, aad, ecs_is_aead_algorithm, index_p, p_in_frame, cam_cookies, pkcs_padding); + if (status != CRYPTO_LIB_SUCCESS) + { + Crypto_TC_Safe_Free_Ptr(*aad); + mc_if->mc_log(status); + return status; + } - // if(sa_service_type != SA_PLAINTEXT) - //{ - // return CRYPTO_LIB_ERR_NULL_CIPHERS; - // } + //TODO: Status? + Crypto_TC_Do_Encrypt_NONPLAINTEXT(sa_service_type, sa_ptr); - if ((sa_ptr->est == 0) && (sa_ptr->ast == 1)) - { - if (sa_ptr->acs_len != 0) - { - if ((sa_ptr->acs == CRYPTO_MAC_CMAC_AES256 || sa_ptr->acs == CRYPTO_MAC_HMAC_SHA256 || sa_ptr->acs == CRYPTO_MAC_HMAC_SHA512) && - sa_ptr->iv_len > 0) - { - status = CRYPTO_LIB_ERR_IV_NOT_SUPPORTED_FOR_ACS_ALGO; - mc_if->mc_log(status); - return status; - } - } - } + /* + ** End Authentication / Encryption + */ - if (crypto_config.iv_type == IV_INTERNAL) + // Only calculate & insert FECF if CryptoLib is configured to do so & gvcid includes FECF. + if (current_managed_parameters->has_fecf == TC_HAS_FECF) + { +#ifdef FECF_DEBUG + printf(KCYN "Calcing FECF over %d bytes\n" RESET, new_enc_frame_header_field_length - 1); +#endif + if (crypto_config.crypto_create_fecf == CRYPTO_TC_CREATE_FECF_TRUE) { - // Start index from the transmitted portion - for (i = sa_ptr->iv_len - sa_ptr->shivf_len; i < sa_ptr->iv_len; i++) - { - *(p_new_enc_frame + index) = *(sa_ptr->iv + i); - index++; - } + *new_fecf = Crypto_Calc_FECF(p_new_enc_frame, new_enc_frame_header_field_length - 1); + *(p_new_enc_frame + new_enc_frame_header_field_length - 1) = (uint8_t)((*new_fecf & 0xFF00) >> 8); + *(p_new_enc_frame + new_enc_frame_header_field_length) = (uint8_t)(*new_fecf & 0x00FF); } - // IV is NULL / IV_CRYPTO_MODULE - else + else // CRYPTO_TC_CREATE_FECF_FALSE { - // Transmitted length > 0, AND using KMC_CRYPTO - if ((sa_ptr->shivf_len > 0) && (crypto_config.cryptography_type == CRYPTOGRAPHY_TYPE_KMCCRYPTO)) - { - index += sa_ptr->iv_len - (sa_ptr->iv_len - sa_ptr->shivf_len); - } - else if (sa_ptr->shivf_len == 0) - { - // IV isn't being used, so don't care if it's Null - } - else - { - status = CRYPTO_LIB_ERR_NULL_IV; - mc_if->mc_log(status); - return status; - } + *(p_new_enc_frame + new_enc_frame_header_field_length - 1) = (uint8_t)0x00; + *(p_new_enc_frame + new_enc_frame_header_field_length) = (uint8_t)0x00; } + index += 2; + } + *index_p = index; + return status; +} - // Set anti-replay sequence number if specified - /* - ** See also: 4.1.1.4.2 - ** 4.1.1.4.4 If authentication or authenticated encryption is not selected - ** for an SA, the Sequence Number field shall be zero octets in length. - ** Reference CCSDS 3550b1 - */ - for (i = sa_ptr->arsn_len - sa_ptr->shsnf_len; i < sa_ptr->arsn_len; i++) - { - // Copy in ARSN from SA - *(p_new_enc_frame + index) = *(sa_ptr->arsn + i); - index++; - } +/** + * @brief Function: Crypto_TC_Check_Init_Setup + * TC Init Setup Sanity Check + * @param in_frame_length: uint16_t + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Check_Init_Setup(uint16_t in_frame_length) +{ + int32_t status = CRYPTO_LIB_SUCCESS; - // Set security header padding if specified - /* - ** 4.2.3.4 h) if the algorithm and mode selected for the SA require the use of - ** fill padding, place the number of fill bytes used into the Pad Length field - ** of the Security Header - Reference CCSDS 3550b1 - */ - // TODO: Revisit this - // TODO: Likely SA API Call - /* 4.1.1.5.2 The Pad Length field shall contain the count of fill bytes used in the - ** cryptographic process, consisting of an integral number of octets. - CCSDS 3550b1 - */ - // TODO: Set this depending on crypto cipher used + if ((crypto_config.init_status == UNITIALIZED) || (mc_if == NULL) || (sa_if == NULL)) + { + printf(KRED "ERROR: CryptoLib Configuration Not Set! -- CRYPTO_LIB_ERR_NO_CONFIG, Will Exit\n" RESET); + status = CRYPTO_LIB_ERR_NO_CONFIG; + // Can't mc_log since it's not configured + return status; // return immediately so a NULL crypto_config is not dereferenced later + } - if (pkcs_padding) - { - uint8_t hex_padding[3] = {0}; // TODO: Create #Define for the 3 - pkcs_padding = pkcs_padding & 0x00FFFFFF; // Truncate to be maxiumum of 3 bytes in size + if (in_frame_length < 5) // Frame length doesn't have enough bytes for TC TF header -- error out. + { + status = CRYPTO_LIB_ERR_INPUT_FRAME_TOO_SHORT_FOR_TC_STANDARD; + mc_if->mc_log(status); + return status; + } + + return status; +} + +/** + * @brief Function: Crypto_TC_Sanity_Setup + * TC Setup Sanity Check - Calls Init_Setup + * @param p_in_frame: const uint8_t* + * @param in_frame_length: const uint16_t + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Sanity_Setup(const uint8_t* p_in_frame, const uint16_t in_frame_length) +{ + uint32_t status = CRYPTO_LIB_SUCCESS; + if (p_in_frame == NULL) + { + status = CRYPTO_LIB_ERR_NULL_BUFFER; + printf(KRED "Error: Input Buffer NULL! \n" RESET); + mc_if->mc_log(status); + return status; // Just return here, nothing can be done. + } + +#ifdef DEBUG + int i; + printf("%d TF Bytes received\n", in_frame_length); + printf("DEBUG - "); + for (i = 0; i < in_frame_length; i++) + { + printf("%02X", ((uint8_t*)&*p_in_frame)[i]); + } + printf("\nPrinted %d bytes\n", in_frame_length); +#else + // TODO - Find another way to know this and remove this argument + uint16_t tmp = in_frame_length; + tmp = tmp; +#endif + + status = Crypto_TC_Check_Init_Setup(in_frame_length); + if (status != CRYPTO_LIB_SUCCESS) + { + // No Logging - as MC might not be initialized + return status; + } + return status; +} + +/** + * @brief Function: Crytpo_TC_Validate_TC_Temp_Header + * TC Temp Header Validation - Sanity Check + * @param in_frame_length: const uint16_t + * @param temp_tc_header: TC_FramePrimaryHeader_t + * @param p_in_frame: const uint8_t* + * @param map_id: uint8_t* + * @param segmentation_hdr: uint8_t* + * @param sa_ptr: SecurityAssociation_t** + * @return int32: Success/Failure + **/ +int32_t Crytpo_TC_Validate_TC_Temp_Header(const uint16_t in_frame_length, TC_FramePrimaryHeader_t temp_tc_header, const uint8_t* p_in_frame, uint8_t* map_id, uint8_t* segmentation_hdr, SecurityAssociation_t** sa_ptr) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + if (in_frame_length < temp_tc_header.fl + 1) // Specified frame length larger than provided frame! + { + status = CRYPTO_LIB_ERR_INPUT_FRAME_LENGTH_SHORTER_THAN_FRAME_HEADERS_LENGTH; + mc_if->mc_log(status); + return status; + } + + // Lookup-retrieve managed parameters for frame via gvcid: + status = Crypto_Get_Managed_Parameters_For_Gvcid(temp_tc_header.tfvn, temp_tc_header.scid, temp_tc_header.vcid, + gvcid_managed_parameters, ¤t_managed_parameters); + if (status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } // Unable to get necessary Managed Parameters for TC TF -- return with error. + + if (current_managed_parameters->has_segmentation_hdr == TC_HAS_SEGMENT_HDRS) + { + *segmentation_hdr = p_in_frame[5]; + *map_id = *segmentation_hdr & 0x3F; + } + + // Check if command frame flag set + status = Crypto_TC_Check_CMD_Frame_Flag(temp_tc_header.cc); + if (status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } + + status = sa_if->sa_get_operational_sa_from_gvcid(temp_tc_header.tfvn, temp_tc_header.scid, + temp_tc_header.vcid, *map_id, sa_ptr); + // If unable to get operational SA, can return + if (status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } + + // Try to assure SA is sane + status = crypto_tc_validate_sa(*sa_ptr); + if (status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } + + return status; +} + + /** + * @brief Function: Crypto_TC_Finalize_Frame_Setup + * Handles validation and setup of TC Frame + * @param sa_service_type: uint8_t + * @param pkcs_padding: uint32_t* + * @param p_enc_frame_len: uint16_t* + * @param new_enc_frame_header_field_length: uint16_t* + * @param tf_payload_len: uint16_t + * @param sa_ptr: SecurityAssociation_t** + * @param p_new_enc_frame: uint8_t** + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Finalize_Frame_Setup(uint8_t sa_service_type, uint32_t* pkcs_padding, uint16_t* p_enc_frame_len, uint16_t* new_enc_frame_header_field_length, uint16_t tf_payload_len, SecurityAssociation_t** sa_ptr, uint8_t** p_new_enc_frame) +{ + uint32_t status = CRYPTO_LIB_SUCCESS; + status = Crypto_TC_Handle_Enc_Padding(sa_service_type, pkcs_padding, p_enc_frame_len, new_enc_frame_header_field_length, tf_payload_len, *sa_ptr); + if(status == CRYPTO_LIB_SUCCESS) + { + status = Crypto_TC_Validate_SA_Service_Type(sa_service_type); + } + + if(status == CRYPTO_LIB_SUCCESS) + { + // Ensure the frame to be created will not violate managed parameter maximum length + status = Crypto_TC_Frame_Validation(p_enc_frame_len); + } + + if(status == CRYPTO_LIB_SUCCESS) + { + // Accio buffer + status = Crypto_TC_Accio_Buffer(p_new_enc_frame, p_enc_frame_len); + } + + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + } + + return status; +} + +/** + * @brief Function: Crypto_TC_Handle_Padding + * Handles Frame Padding if necessary: Depends on KMC vs Internal vs Algorithm etc + * @param pkcs_padding: uint32_t + * @param sa_ptr: SecurityAssociation_t* + * @param p_new_enc_frame: uint8_t* + * @param index: uint16_t* + * @return int32: Success/Failure + **/ +void Crypto_TC_Handle_Padding(uint32_t pkcs_padding, SecurityAssociation_t* sa_ptr, uint8_t* p_new_enc_frame, uint16_t* index) +{ + int i = 0; + uint16_t temp_index = *index; + if (pkcs_padding) + { + uint8_t hex_padding[3] = {0}; // TODO: Create #Define for the 3 + pkcs_padding = pkcs_padding & 0x00FFFFFF; // Truncate to be maxiumum of 3 bytes in size + + // Byte Magic + hex_padding[0] = (pkcs_padding >> 16) & 0xFF; + hex_padding[1] = (pkcs_padding >> 8) & 0xFF; + hex_padding[2] = (pkcs_padding)&0xFF; - // Byte Magic - hex_padding[0] = (pkcs_padding >> 16) & 0xFF; - hex_padding[1] = (pkcs_padding >> 8) & 0xFF; - hex_padding[2] = (pkcs_padding)&0xFF; + uint8_t padding_start = 0; + padding_start = 3 - sa_ptr->shplf_len; + + for (i = 0; i < sa_ptr->shplf_len; i++) + { + *(p_new_enc_frame + temp_index) = hex_padding[padding_start++]; + temp_index++; + } + *index = temp_index; + } +} - uint8_t padding_start = 0; - padding_start = 3 - sa_ptr->shplf_len; +/** + * @brief Function: Crypto_TC_Set_IV + * Performs validation and setup of IV + * @param sa_ptr: SecurityAssociation_t* + * @param p_new_enc_frame: uint8_t* + * @param index: uint16_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Set_IV(SecurityAssociation_t* sa_ptr, uint8_t* p_new_enc_frame, uint16_t* index) +{ + uint32_t status = CRYPTO_LIB_SUCCESS; + #ifdef SA_DEBUG + if (sa_ptr->shivf_len > 0 && sa_ptr->iv != NULL) + { + int i = 0; + printf(KYEL "Using IV value:\n\t"); + for (i = 0; i < sa_ptr->iv_len; i++) + { + printf("%02x", *(sa_ptr->iv + i)); + } + printf("\n" RESET); + printf(KYEL "Transmitted IV value:\n\t"); + for (i = sa_ptr->iv_len - sa_ptr->shivf_len; i < sa_ptr->iv_len; i++) + { + printf("%02x", *(sa_ptr->iv + i)); + } + printf("\n" RESET); + } +#endif + status = Crypto_TC_ACS_Algo_Check(sa_ptr); + if(status == CRYPTO_LIB_SUCCESS) + { + status = Crypto_TC_Check_IV_Setup(sa_ptr, p_new_enc_frame, index); + } + + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + } + return status; +} + +/** + * @brief Function: Crypto_TC_ApplySecurity + * Applies Security to incoming frame. Encryption, Authentication, and Authenticated Encryption + * @param p_in_frame: uint8* + * @param in_frame_length: uint16 + * @param pp_in_frame: uint8_t** + * @param p_enc_frame_len: uint16 + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_ApplySecurity(const uint8_t* p_in_frame, const uint16_t in_frame_length, uint8_t** pp_in_frame, + uint16_t* p_enc_frame_len) +{ + // Passthrough to maintain original function signature when CAM isn't used. + return Crypto_TC_ApplySecurity_Cam(p_in_frame, in_frame_length, pp_in_frame, p_enc_frame_len, NULL); +} +/** + * @brief Function: Crypto_TC_ApplySecurity_Cam + * Applies Security to incoming frame. Encryption, Authentication, and Authenticated Encryption + * @param p_in_frame: uint8* + * @param in_frame_length: uint16 + * @param pp_in_frame: uint8_t** + * @param p_enc_frame_len: uint16 + * @param cam_cookies: char* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_ApplySecurity_Cam(const uint8_t* p_in_frame, const uint16_t in_frame_length, uint8_t** pp_in_frame, + uint16_t* p_enc_frame_len, char* cam_cookies) +{ + // Local Variables + int32_t status = CRYPTO_LIB_SUCCESS; + TC_FramePrimaryHeader_t temp_tc_header; + SecurityAssociation_t* sa_ptr = NULL; + uint8_t* p_new_enc_frame = NULL; + uint8_t sa_service_type = -1; + uint16_t mac_loc = 0; + uint16_t tf_payload_len = 0x0000; + uint16_t new_fecf = 0x0000; + uint8_t* aad = NULL; + uint16_t new_enc_frame_header_field_length = 0; + uint32_t encryption_cipher = 0; + uint8_t ecs_is_aead_algorithm; + int i; + uint32_t pkcs_padding = 0; + crypto_key_t* ekp = NULL; + uint8_t map_id = 0; + uint8_t segmentation_hdr = 0x00; + +#ifdef DEBUG + printf(KYEL "\n----- Crypto_TC_ApplySecurity START -----\n" RESET); +#endif + + status = Crypto_TC_Sanity_Setup(p_in_frame, in_frame_length); + if (status != CRYPTO_LIB_SUCCESS) + { + // Logging handled inside sanity setup functionality + return status; + } + + // Primary Header + temp_tc_header.tfvn = ((uint8_t)p_in_frame[0] & 0xC0) >> 6; + temp_tc_header.bypass = ((uint8_t)p_in_frame[0] & 0x20) >> 5; + temp_tc_header.cc = ((uint8_t)p_in_frame[0] & 0x10) >> 4; + temp_tc_header.spare = ((uint8_t)p_in_frame[0] & 0x0C) >> 2; + temp_tc_header.scid = ((uint8_t)p_in_frame[0] & 0x03) << 8; + temp_tc_header.scid = temp_tc_header.scid | (uint8_t)p_in_frame[1]; + temp_tc_header.vcid = ((uint8_t)p_in_frame[2] & 0xFC) >> 2 & crypto_config.vcid_bitmask; + temp_tc_header.fl = ((uint8_t)p_in_frame[2] & 0x03) << 8; + temp_tc_header.fl = temp_tc_header.fl | (uint8_t)p_in_frame[3]; + temp_tc_header.fsn = (uint8_t)p_in_frame[4]; + + status = Crytpo_TC_Validate_TC_Temp_Header(in_frame_length, temp_tc_header, p_in_frame, &map_id, &segmentation_hdr, &sa_ptr); + if (status != CRYPTO_LIB_SUCCESS) + { + return status; + } + + +#ifdef SA_DEBUG + printf(KYEL "DEBUG - Printing SA Entry for current frame.\n" RESET); + Crypto_saPrint(sa_ptr); +#endif + + // Determine SA Service Type + status = Crypto_TC_Get_SA_Service_Type(&sa_service_type, sa_ptr); + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } + + // Determine Algorithm cipher & mode. // TODO - Parse authentication_cipher, and handle AEAD cases properly + status = Crypto_TC_Get_Ciper_Mode_TCA(sa_service_type, &encryption_cipher, &ecs_is_aead_algorithm, sa_ptr); + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } + +#ifdef TC_DEBUG + switch (sa_service_type) + { + case SA_PLAINTEXT: + printf(KBLU "Creating a TC - CLEAR!\n" RESET); + break; + case SA_AUTHENTICATION: + printf(KBLU "Creating a TC - AUTHENTICATED!\n" RESET); + break; + case SA_ENCRYPTION: + printf(KBLU "Creating a TC - ENCRYPTED!\n" RESET); + break; + case SA_AUTHENTICATED_ENCRYPTION: + printf(KBLU "Creating a TC - AUTHENTICATED ENCRYPTION!\n" RESET); + break; + } +#endif + + // Determine if segment header exists and FECF exists + uint8_t segment_hdr_len = TC_SEGMENT_HDR_SIZE; + uint8_t fecf_len = FECF_SIZE; + + Crypto_TC_Calc_Lengths(&fecf_len, &segment_hdr_len); + + // Calculate tf_payload length here to be used in other logic + tf_payload_len = temp_tc_header.fl - TC_FRAME_HEADER_SIZE - segment_hdr_len - fecf_len + 1; + + /** + * A note on plaintext: Take a permissive approach to allow the lengths of fields that aren't going to be used. + * The 355.0-B-2 (July 2022) says the following in $4.2.2.4: + * 'It is possible to create a ‘clear mode’ SA using one of the defined service types by + specifying the algorithm as a ‘no-op’ function (no actual cryptographic operation to + be performed). Such an SA might be used, for example, during development + testing of other aspects of data link processing before cryptographic capabilities are + available for integrated testing.In this scenario, the Security Header and Trailer + field lengths are kept constant across all supported configurations. For security + reasons, the use of such an SA is not recommended in normal operation.' + */ + + // Calculate frame lengths based on SA fields + *p_enc_frame_len = temp_tc_header.fl + 1 + 2 + sa_ptr->shivf_len + sa_ptr->shsnf_len + sa_ptr->shplf_len + sa_ptr->stmacf_len; + new_enc_frame_header_field_length = (*p_enc_frame_len) - 1; + + // Finalize frame setup + status = Crypto_TC_Finalize_Frame_Setup(sa_service_type, &pkcs_padding, p_enc_frame_len, &new_enc_frame_header_field_length, tf_payload_len, &sa_ptr, &p_new_enc_frame); + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } + + +#ifdef TC_DEBUG + printf(KYEL "DEBUG - Total TC Buffer to be malloced is: %d bytes\n" RESET, *p_enc_frame_len); + printf(KYEL "\tlen of TF\t = %d\n" RESET, temp_tc_header.fl); + printf(KYEL "\tsegment hdr len\t = %d\n" RESET, segment_hdr_len); + printf(KYEL "\tspi len\t\t = 2\n" RESET); + printf(KYEL "\tshivf_len\t = %d\n" RESET, sa_ptr->shivf_len); + printf(KYEL "\tiv_len\t\t = %d\n" RESET, sa_ptr->iv_len); + printf(KYEL "\tshsnf_len\t = %d\n" RESET, sa_ptr->shsnf_len); + printf(KYEL "\tshplf len\t = %d\n" RESET, sa_ptr->shplf_len); + printf(KYEL "\tarsn_len\t = %d\n" RESET, sa_ptr->arsn_len); + printf(KYEL "\tstmacf_len\t = %d\n" RESET, sa_ptr->stmacf_len); +#endif + + // Copy original TF header, w/ segment header if applicable + memcpy(p_new_enc_frame, p_in_frame, TC_FRAME_HEADER_SIZE + segment_hdr_len); + + // Set new TF Header length + // Recall: Length field is one minus total length per spec + *(p_new_enc_frame + 2) = + ((*(p_new_enc_frame + 2) & 0xFC) | (((new_enc_frame_header_field_length) & (0x0300)) >> 8)); + *(p_new_enc_frame + 3) = ((new_enc_frame_header_field_length) & (0x00FF)); + +#ifdef TC_DEBUG + printf(KYEL "Printing updated TF Header:\n\t"); + for (i = 0; i < TC_FRAME_HEADER_SIZE; i++) + { + printf("%02X",*(p_new_enc_frame + i)); + } + // Recall: The buffer length is 1 greater than the field value set in the TCTF + printf("\n\tLength set to 0x%02X\n" RESET, new_enc_frame_header_field_length); +#endif + + /* + ** Start variable length fields + */ + uint16_t index = TC_FRAME_HEADER_SIZE; // Frame header is 5 bytes + + if (current_managed_parameters->has_segmentation_hdr == TC_HAS_SEGMENT_HDRS) + { + index++; // Add 1 byte to index because segmentation header used for this gvcid. + } + + /* + ** Begin Security Header Fields + ** Reference CCSDS SDLP 3550b1 4.1.1.1.3 + */ + // Set SPI + *(p_new_enc_frame + index) = ((sa_ptr->spi & 0xFF00) >> 8); + *(p_new_enc_frame + index + 1) = (sa_ptr->spi & 0x00FF); + index += 2; + + // Set initialization vector if specified + status = Crypto_TC_Set_IV(sa_ptr, p_new_enc_frame, &index); + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } + + // Set anti-replay sequence number if specified + /* + ** See also: 4.1.1.4.2 + ** 4.1.1.4.4 If authentication or authenticated encryption is not selected + ** for an SA, the Sequence Number field shall be zero octets in length. + ** Reference CCSDS 3550b1 + */ + for (i = sa_ptr->arsn_len - sa_ptr->shsnf_len; i < sa_ptr->arsn_len; i++) + { + // Copy in ARSN from SA + *(p_new_enc_frame + index) = *(sa_ptr->arsn + i); + index++; + } + + // Set security header padding if specified + /* + ** 4.2.3.4 h) if the algorithm and mode selected for the SA require the use of + ** fill padding, place the number of fill bytes used into the Pad Length field + ** of the Security Header - Reference CCSDS 3550b1 + */ + // TODO: Revisit this + // TODO: Likely SA API Call + /* 4.1.1.5.2 The Pad Length field shall contain the count of fill bytes used in the + ** cryptographic process, consisting of an integral number of octets. - CCSDS 3550b1 + */ + // TODO: Set this depending on crypto cipher used + Crypto_TC_Handle_Padding(pkcs_padding, sa_ptr, p_new_enc_frame, &index); + + /* + ** End Security Header Fields + */ + + // Copy in original TF data - except FECF + // Will be over-written if using encryption later + // tf_payload_len = temp_tc_header.fl - TC_FRAME_HEADER_SIZE - segment_hdr_len - fecf_len + 1; + + memcpy((p_new_enc_frame + index), (p_in_frame + TC_FRAME_HEADER_SIZE + segment_hdr_len), tf_payload_len); + index += tf_payload_len; + for (uint32_t i = 0; i < pkcs_padding; i++) + { + /* 4.1.1.5.2 The Pad Length field shall contain the count of fill bytes used in the + ** cryptographic process, consisting of an integral number of octets. - CCSDS 3550b1 + */ + // TODO: Set this depending on crypto cipher used + *(p_new_enc_frame + index + i) = (uint8_t)pkcs_padding; // How much padding is needed? + // index++; + } + index -= tf_payload_len; + tf_payload_len += pkcs_padding; + + /* + ** Begin Authentication / Encryption + */ + + status = Crypto_TC_Do_Encrypt(sa_service_type, sa_ptr, &mac_loc, tf_payload_len, segment_hdr_len, p_new_enc_frame, ekp, &aad, ecs_is_aead_algorithm, &index, p_in_frame, cam_cookies, pkcs_padding, new_enc_frame_header_field_length, &new_fecf); + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } + + + +#ifdef TC_DEBUG + printf(KYEL "Printing new TC Frame of length %d:\n\t", *p_enc_frame_len); + for (i = 0; i <*p_enc_frame_len; i++) + { + printf("%02X",*(p_new_enc_frame + i)); + } + printf("\n\tThe returned length is: %d\n" RESET, new_enc_frame_header_field_length); +#endif + + *pp_in_frame = p_new_enc_frame; + + status = sa_if->sa_save_sa(sa_ptr); + +#ifdef DEBUG + printf(KYEL "----- Crypto_TC_ApplySecurity END -----\n" RESET); +#endif + Crypto_TC_Safe_Free_Ptr(aad); + mc_if->mc_log(status); + return status; +} + +/** + * @brief Function: Crypto_TC_ProcessSecurity + * Performs Authenticated decryption, decryption, and authentication + * @param ingest: uint8_t* + * @param len_ingest: int* + * @param tc_sdls_processed_frame: TC_t* + * @return int32: Success/Failure +**/ +int32_t Crypto_TC_ProcessSecurity(uint8_t* ingest, int* len_ingest, TC_t* tc_sdls_processed_frame) +{ + // Pass-through to maintain original function signature when CAM isn't used. + return Crypto_TC_ProcessSecurity_Cam(ingest, len_ingest, tc_sdls_processed_frame, NULL); +} + +/** + * @brief Function: Crypto_TC_Parse_Check_FECF + * Parses and validates frame FECF + * @param ingest: uint8_t* + * @param len_ingest: int* + * @param tc_sdls_processed_frame: TC_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Parse_Check_FECF(uint8_t* ingest, int* len_ingest, TC_t* tc_sdls_processed_frame) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + if (current_managed_parameters->has_fecf == TC_HAS_FECF) + { + tc_sdls_processed_frame->tc_sec_trailer.fecf = (((ingest[tc_sdls_processed_frame->tc_header.fl - 1] << 8) & 0xFF00) | + (ingest[tc_sdls_processed_frame->tc_header.fl] & 0x00FF)); + + if (crypto_config.crypto_check_fecf == TC_CHECK_FECF_TRUE) + { + uint16_t received_fecf = tc_sdls_processed_frame->tc_sec_trailer.fecf; + // Calculate our own + uint16_t calculated_fecf = Crypto_Calc_FECF(ingest, *len_ingest - 2); + // Compare + if (received_fecf != calculated_fecf) + { +#ifdef DEBUG + printf("Received FECF is 0x%04X\n", received_fecf); + printf("Calculated FECF is 0x%04X\n", calculated_fecf); + printf("FECF was Calced over %d bytes\n", *len_ingest - 2); +#endif + status = CRYPTO_LIB_ERR_INVALID_FECF; + mc_if->mc_log(status); + } + } + } + return status; +} + +/** + * @brief Function: Crypto_TC_Nontransmitted_IV_Increment + * Handles increment of Nontransmitted portion of IV + * @param sa_ptr: SecurityAssociation_t* + * @param tc_sdls_processed_frame: TC_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Nontransmitted_IV_Increment(SecurityAssociation_t* sa_ptr, TC_t* tc_sdls_processed_frame) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + + if (sa_ptr->shivf_len < sa_ptr->iv_len && + crypto_config.ignore_anti_replay == TC_IGNORE_ANTI_REPLAY_FALSE && + crypto_config.crypto_increment_nontransmitted_iv == SA_INCREMENT_NONTRANSMITTED_IV_TRUE) + { + status = crypto_handle_incrementing_nontransmitted_counter(tc_sdls_processed_frame->tc_sec_header.iv, sa_ptr->iv, sa_ptr->iv_len, sa_ptr->shivf_len, sa_ptr->arsnw); + if (status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } + } + else // Not checking IV ARSNW or only non-transmitted portion is static; Note, non-transmitted IV in SA must match frame or will fail MAC check. + { + // Retrieve non-transmitted portion of IV from SA (if applicable) + memcpy(tc_sdls_processed_frame->tc_sec_header.iv, sa_ptr->iv, sa_ptr->iv_len - sa_ptr->shivf_len); + } + return status; +} + +/** + * @brief Function: Crypto_TC_Nontransmitted_SN_Increment + * Handles increment of Nontransmitted portion of SN + * @param sa_ptr: SecurityAssociation_t* + * @param tc_sdls_processed_frame: TC_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Nontransmitted_SN_Increment(SecurityAssociation_t* sa_ptr, TC_t* tc_sdls_processed_frame) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + if (sa_ptr->shsnf_len < sa_ptr->arsn_len && + crypto_config.ignore_anti_replay == TC_IGNORE_ANTI_REPLAY_FALSE) + { + status = crypto_handle_incrementing_nontransmitted_counter(tc_sdls_processed_frame->tc_sec_header.sn, sa_ptr->arsn, sa_ptr->arsn_len, sa_ptr->shsnf_len, sa_ptr->arsnw); + if (status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + } + } + else // Not checking ARSN in ARSNW + { + // Parse non-transmitted portion of ARSN from SA + memcpy(tc_sdls_processed_frame->tc_sec_header.sn, sa_ptr->arsn, sa_ptr->arsn_len - sa_ptr->shsnf_len); + } + return status; +} + +/** + * @brief Function: Crypto_TC_Check_ACS_Keylen + * Validates ACS Keylength + * @param akp: crypto_key_t* + * @param sa_ptr: SecurityAssociation_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Check_ACS_Keylen(crypto_key_t* akp, SecurityAssociation_t* sa_ptr) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + if ((int32_t)akp->key_len != Crypto_Get_ACS_Algo_Keylen(sa_ptr->acs)) + { + status = CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; + mc_if->mc_log(status); + } + return status; +} + +/** + * @brief Function: Crypto_TC_Check_ECS_Keylen + * Validates ECS Keylength + * @param ekp: crypto_key_t* + * @param sa_ptr: SecurityAssociation_t* + * @return int32: Success/Failure + **/ +int32_t Crypto_TC_Check_ECS_Keylen(crypto_key_t* ekp, SecurityAssociation_t* sa_ptr) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + if ((int32_t)ekp->key_len != Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs)) + { + status = CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; + mc_if->mc_log(status); + } + return status; +} + +/** + * @brief Function: Crypto_TC_Safe_Free_Ptr + * Pointer Safe Free + * @param ptr: uint8_t* + **/ +void Crypto_TC_Safe_Free_Ptr(uint8_t* ptr) +{ + if (!ptr) free(ptr); +} + +/** + * @brief Function: Crypto_TC_Do_Decrypt + * Handles Frame Decryption + * @param sa_service_type: uint8_t + * @param ecs_is_aead_algorithm: uint8_t + * @param ekp: crypto_key_t* + * @param sa_ptr: SecurityAssociation_t* + * @param aad: uint8_t* + * @param tc_sdls_processed_frame: TC_t* + * @param ingest: uint8_t* + * @param tc_enc_payload_start_index: uint16_t + * @param aad_len: uint16_t + * @param cam_cookies: char* + * @param akp: crypto_key_t* + * @param segment_hdr_len: uint8_t + * @return int32_t: Success/Failure + **/ +int32_t Crypto_TC_Do_Decrypt(uint8_t sa_service_type, uint8_t ecs_is_aead_algorithm, crypto_key_t* ekp, SecurityAssociation_t* sa_ptr, uint8_t* aad, TC_t* tc_sdls_processed_frame, uint8_t* ingest, uint16_t tc_enc_payload_start_index, uint16_t aad_len, char* cam_cookies, crypto_key_t* akp, uint8_t segment_hdr_len) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + + if (sa_service_type != SA_PLAINTEXT && ecs_is_aead_algorithm == CRYPTO_TRUE) + { + // Check that key length to be used meets the algorithm requirement + + status = Crypto_TC_Check_ECS_Keylen(ekp, sa_ptr); + if(status!= CRYPTO_LIB_SUCCESS){ + Crypto_TC_Safe_Free_Ptr(aad); + return status; + } + + status = cryptography_if->cryptography_aead_decrypt( + tc_sdls_processed_frame->tc_pdu, // plaintext output + (size_t)(tc_sdls_processed_frame->tc_pdu_len), // length of data + &(ingest[tc_enc_payload_start_index]), // ciphertext input + (size_t)(tc_sdls_processed_frame->tc_pdu_len), // in data length + &(ekp->value[0]), // Key + Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs), // + sa_ptr, // SA for key reference + tc_sdls_processed_frame->tc_sec_header.iv, // IV + sa_ptr->iv_len, // IV Length + tc_sdls_processed_frame->tc_sec_trailer.mac, // Frame Expected Tag + sa_ptr->stmacf_len, // tag size + aad, // additional authenticated data + aad_len, // length of AAD + (sa_ptr->est), // Decryption Bool + (sa_ptr->ast), // Authentication Bool + (sa_ptr->ast), // AAD Bool + &sa_ptr->ecs, // encryption cipher + &sa_ptr->acs, // authentication cipher + cam_cookies // + ); + } + else if (sa_service_type != SA_PLAINTEXT && ecs_is_aead_algorithm == CRYPTO_FALSE) // Non aead algorithm + { + // TODO - implement non-AEAD algorithm logic + if (sa_service_type == SA_AUTHENTICATION || sa_service_type == SA_AUTHENTICATED_ENCRYPTION) + { + // Check that key length to be used ets the algorithm requirement + status = Crypto_TC_Check_ACS_Keylen(akp, sa_ptr); + if(status!= CRYPTO_LIB_SUCCESS) + { + Crypto_TC_Safe_Free_Ptr(aad); + return status; + } + + status = cryptography_if->cryptography_validate_authentication( + tc_sdls_processed_frame->tc_pdu, // plaintext output + (size_t)(tc_sdls_processed_frame->tc_pdu_len), // length of data + &(ingest[tc_enc_payload_start_index]), // ciphertext input + (size_t)(tc_sdls_processed_frame->tc_pdu_len), // in data length + &(akp->value[0]), // Key + Crypto_Get_ACS_Algo_Keylen(sa_ptr->acs), // + sa_ptr, // SA for key reference + tc_sdls_processed_frame->tc_sec_header.iv, // IV + sa_ptr->iv_len, // IV Length + tc_sdls_processed_frame->tc_sec_trailer.mac, // Frame Expected Tag + sa_ptr->stmacf_len, // tag size + aad, // additional authenticated data + aad_len, // length of AAD + CRYPTO_CIPHER_NONE, // encryption cipher + sa_ptr->acs, // authentication cipher + cam_cookies // + ); + } + if (sa_service_type == SA_ENCRYPTION || sa_service_type == SA_AUTHENTICATED_ENCRYPTION) + { + // Check that key length to be used emets the algorithm requirement + if ((int32_t)ekp->key_len != Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs)) + { + Crypto_TC_Safe_Free_Ptr(aad); + status = CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; + mc_if->mc_log(status); + return status; + } + + status = cryptography_if->cryptography_decrypt( + tc_sdls_processed_frame->tc_pdu, // plaintext output + (size_t)(tc_sdls_processed_frame->tc_pdu_len), // length of data + &(ingest[tc_enc_payload_start_index]), // ciphertext input + (size_t)(tc_sdls_processed_frame->tc_pdu_len), // in data length + &(ekp->value[0]), // Key + Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs), // + sa_ptr, // SA for key reference + tc_sdls_processed_frame->tc_sec_header.iv, // IV + sa_ptr->iv_len, // IV Length + &sa_ptr->ecs, // encryption cipher + &sa_ptr->acs, // authentication cipher + cam_cookies // + ); - for (i = 0; i < sa_ptr->shplf_len; i++) + // Handle Padding Removal + if (sa_ptr->shplf_len != 0) { - *(p_new_enc_frame + index) = hex_padding[padding_start++]; - index++; + int padding_location = TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN + sa_ptr->shivf_len + + sa_ptr->shsnf_len; + uint16_t padding_amount = 0; + // Get Padding Amount from ingest frame + padding_amount = (int)ingest[padding_location]; + // Remove Padding from final decrypted portion + tc_sdls_processed_frame->tc_pdu_len -= padding_amount; } } + } + else if (sa_service_type == SA_PLAINTEXT) + { + memcpy(tc_sdls_processed_frame->tc_pdu, &(ingest[tc_enc_payload_start_index]), + tc_sdls_processed_frame->tc_pdu_len); + } + return status; +} - /* - ** End Security Header Fields - */ +/** + * @brief Function: Crypto_TC_Process_Sanity_Check + * Validates Input Frame Length + * @param len_ingest: int* + * @return int32_t: Success/Failure + **/ +int32_t Crypto_TC_Process_Sanity_Check(int* len_ingest) +{ + int32_t status = CRYPTO_LIB_SUCCESS; - // Copy in original TF data - except FECF - // Will be over-written if using encryption later - // tf_payload_len = temp_tc_header.fl - TC_FRAME_HEADER_SIZE - segment_hdr_len - fecf_len + 1; +#ifdef DEBUG + printf(KYEL "\n----- Crypto_TC_ProcessSecurity START -----\n" RESET); +#endif - memcpy((p_new_enc_frame + index), (p_in_frame + TC_FRAME_HEADER_SIZE + segment_hdr_len), tf_payload_len); - index += tf_payload_len; - for (uint32_t i = 0; i < pkcs_padding; i++) - { - /* 4.1.1.5.2 The Pad Length field shall contain the count of fill bytes used in the - ** cryptographic process, consisting of an integral number of octets. - CCSDS 3550b1 - */ - // TODO: Set this depending on crypto cipher used - *(p_new_enc_frame + index + i) = (uint8_t)pkcs_padding; // How much padding is needed? - // index++; - } - index -= tf_payload_len; - tf_payload_len += pkcs_padding; + if ((mc_if == NULL) || (crypto_config.init_status == UNITIALIZED)) + { + printf(KRED "ERROR: CryptoLib Configuration Not Set! -- CRYPTO_LIB_ERR_NO_CONFIG, Will Exit\n" RESET); + status = CRYPTO_LIB_ERR_NO_CONFIG; + mc_if->mc_log(status); + } + if ((*len_ingest < 5) && (status == CRYPTO_LIB_SUCCESS)) // Frame length doesn't even have enough bytes for header -- error out. + { + status = CRYPTO_LIB_ERR_INPUT_FRAME_TOO_SHORT_FOR_TC_STANDARD; + mc_if->mc_log(status); + } + return status; +} - /* - ** Begin Authentication / Encryption - */ +/** + * @brief Function: Crypto_TC_Prep_AAD + * Validates and Prepares AAD as necessary + * @param tc_sdls_processed_frame: TC_t* + * @param fecf_len: uint8_t + * @param sa_service_type: uint8_t + * @param ecs_is_aead_algorithm: uint8_t + * @param aad_len: uint16_t* + * @param sa_ptr: SecurityAssociation_t* + * @param segment_hdr_len: uint8_t + * @param ingest: uint8_t* + * @param aad: uint8_t** + * @return int32_t: Success/Failure + **/ +int32_t Crypto_TC_Prep_AAD(TC_t* tc_sdls_processed_frame, uint8_t fecf_len, uint8_t sa_service_type, uint8_t ecs_is_aead_algorithm, uint16_t* aad_len, SecurityAssociation_t* sa_ptr, uint8_t segment_hdr_len, uint8_t* ingest, uint8_t** aad) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + uint16_t aad_len_temp = *aad_len; - if (sa_service_type != SA_PLAINTEXT) + if ((sa_service_type == SA_AUTHENTICATION) || (sa_service_type == SA_AUTHENTICATED_ENCRYPTION)) { - uint8_t* mac_ptr = NULL; - uint16_t aad_len = 0; + uint16_t tc_mac_start_index = tc_sdls_processed_frame->tc_header.fl + 1 - fecf_len - sa_ptr->stmacf_len; - if (sa_service_type == SA_AUTHENTICATED_ENCRYPTION || sa_service_type == SA_AUTHENTICATION) - { - mac_loc = TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN + sa_ptr->shivf_len + sa_ptr->shsnf_len + - sa_ptr->shplf_len + tf_payload_len; -#ifdef MAC_DEBUG - printf(KYEL "MAC location is: %d\n" RESET, mac_loc); - printf(KYEL "MAC size is: %d\n" RESET, sa_ptr->stmacf_len); + // Parse the received MAC + memcpy((tc_sdls_processed_frame->tc_sec_trailer.mac), + &(ingest[tc_mac_start_index]), sa_ptr->stmacf_len); +#ifdef DEBUG + printf("MAC Parsed from Frame:\n"); + Crypto_hexprint(tc_sdls_processed_frame->tc_sec_trailer.mac, sa_ptr->stmacf_len); #endif - mac_ptr = &p_new_enc_frame[mac_loc]; + aad_len_temp = tc_mac_start_index; - // Prepare the Header AAD (CCSDS 335.0-B-1 4.2.3.2.2.3) - aad_len = TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN + sa_ptr->shivf_len + - sa_ptr->shsnf_len + sa_ptr->shplf_len; - if (sa_service_type == SA_AUTHENTICATION) // auth only, we authenticate the payload as part of the AEAD encrypt call here - { - aad_len += tf_payload_len; - } -#ifdef TC_DEBUG - printf("Calculated AAD Length: %d\n", aad_len); -#endif - if (sa_ptr->abm_len < aad_len) - { - status = CRYPTO_LIB_ERR_ABM_TOO_SHORT_FOR_AAD; - mc_if->mc_log(status); - return status; - } - aad = Crypto_Prepare_TC_AAD(p_new_enc_frame, aad_len, sa_ptr->abm); - } -#ifdef TC_DEBUG - printf("Encrypted bytes output_loc is %d\n", index); - printf("Input bytes input_loc is %d\n", TC_FRAME_HEADER_SIZE + segment_hdr_len); -#endif + if ((sa_service_type == SA_AUTHENTICATED_ENCRYPTION) && (ecs_is_aead_algorithm == CRYPTO_TRUE)) + { + aad_len_temp = TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN + sa_ptr->shivf_len + sa_ptr->shsnf_len + + sa_ptr->shplf_len; + } + if (sa_ptr->abm_len < aad_len_temp) + { + status = CRYPTO_LIB_ERR_ABM_TOO_SHORT_FOR_AAD; + mc_if->mc_log(status); + return status; + } + *aad = Crypto_Prepare_TC_AAD(ingest, aad_len_temp, sa_ptr->abm); + *aad_len = aad_len_temp; + aad = aad; + } + return status; +} - /* Get Key */ - ekp = key_if->get_key(sa_ptr->ekid); - if (ekp == NULL) - { - status = CRYPTO_LIB_ERR_KEY_ID_ERROR; - mc_if->mc_log(status); - return status; - } +/** + * @TODO: Possible Duplication + * @brief Function: Crypto_TC_Get_Keys + * Retreives EKP/AKP as necessary + * @param ekp: crypto_key_t** + * @param akp: crypto_key_t** + * @param sa_ptr: SecurityAssociation_t* + * @return int32_t: Success/Failure + **/ +int32_t Crypto_TC_Get_Keys(crypto_key_t** ekp, crypto_key_t** akp, SecurityAssociation_t* sa_ptr) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + *ekp = key_if->get_key(sa_ptr->ekid); + *akp = key_if->get_key(sa_ptr->akid); - if (ecs_is_aead_algorithm == CRYPTO_TRUE) - { - // Check that key length to be used ets the algorithm requirement - if ((int32_t)ekp->key_len != Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs)) - { - if (!aad) free(aad); - status = CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; - mc_if->mc_log(status); - return status; - } + if (ekp == NULL) + { + status = CRYPTO_LIB_ERR_KEY_ID_ERROR; + mc_if->mc_log(status); + } + + if ((akp == NULL) && (status == CRYPTO_LIB_SUCCESS)) + { + status = CRYPTO_LIB_ERR_KEY_ID_ERROR; + mc_if->mc_log(status); + } + return status; +} - status = cryptography_if->cryptography_aead_encrypt(&p_new_enc_frame[index], // ciphertext output - (size_t)tf_payload_len, // length of data - (uint8_t*)(p_in_frame + TC_FRAME_HEADER_SIZE + segment_hdr_len), // plaintext input - (size_t)tf_payload_len, // in data length - &(ekp->value[0]), // Key - Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs), // Length of key derived from sa_ptr key_ref - sa_ptr, // SA (for key reference) - sa_ptr->iv, // IV - sa_ptr->iv_len, // IV Length - mac_ptr, // tag output - sa_ptr->stmacf_len, // tag size - aad, // AAD Input - aad_len, // Length of AAD - (sa_ptr->est == 1), - (sa_ptr->ast == 1), - (sa_ptr->ast == 1), - &sa_ptr->ecs, // encryption cipher - &sa_ptr->acs, // authentication cipher - cam_cookies); - } - else // non aead algorithm - { - // TODO - implement non-AEAD algorithm logic - if (sa_service_type == SA_ENCRYPTION) - { - // Check that key length to be used ets the algorithm requirement - if ((int32_t)ekp->key_len != Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs)) - { - if (!aad) free(aad); - return CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; - } - - status = cryptography_if->cryptography_encrypt(&p_new_enc_frame[index], // ciphertext output - (size_t)tf_payload_len, - &p_new_enc_frame[index], // length of data - //(uint8_t*)(p_in_frame + TC_FRAME_HEADER_SIZE + segment_hdr_len), // plaintext input - (size_t)tf_payload_len, // in data length - // new_frame_length, - &(ekp->value[0]), // Key - Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs), // Length of key derived from sa_ptr key_ref - sa_ptr, // SA (for key reference) - sa_ptr->iv, // IV - sa_ptr->iv_len, // IV Length - &sa_ptr->ecs, // encryption cipher - pkcs_padding, - cam_cookies); - } +/** + * @brief Function: Crypto_TC_Check_IV_ARSN + * Checks and validates Anti Replay + * @param sa_ptr: SecurityAssociation_t* + * @param tc_sdls_processed_frame: TC_t* + * @return int32_t: Success/Failure + **/ +int32_t Crypto_TC_Check_IV_ARSN(SecurityAssociation_t* sa_ptr,TC_t* tc_sdls_processed_frame) +{ + int32_t status = CRYPTO_LIB_SUCCESS; - if (sa_service_type == SA_AUTHENTICATION) - { - /* Get Key */ - crypto_key_t* akp = NULL; - akp = key_if->get_key(sa_ptr->akid); - if (akp == NULL) - { - return CRYPTO_LIB_ERR_KEY_ID_ERROR; - } - - // Check that key length to be used ets the algorithm requirement - if ((int32_t)akp->key_len != Crypto_Get_ACS_Algo_Keylen(sa_ptr->acs)) - { - if (!aad) free(aad); - return CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; - } - - status = cryptography_if->cryptography_authenticate(&p_new_enc_frame[index], // ciphertext output - (size_t)tf_payload_len, // length of data - (uint8_t*)(p_in_frame + TC_FRAME_HEADER_SIZE + segment_hdr_len), // plaintext input - (size_t)tf_payload_len, // in data length - &(akp->value[0]), // Key - Crypto_Get_ACS_Algo_Keylen(sa_ptr->acs), - sa_ptr, // SA (for key reference) - sa_ptr->iv, // IV - sa_ptr->iv_len, // IV Length - mac_ptr, // tag output - sa_ptr->stmacf_len, // tag size - aad, // AAD Input - aad_len, // Length of AAD - sa_ptr->ecs, // encryption cipher - sa_ptr->acs, // authentication cipher - cam_cookies); - } - } + if (crypto_config.ignore_anti_replay == TC_IGNORE_ANTI_REPLAY_FALSE && status == CRYPTO_LIB_SUCCESS) + { + status = Crypto_Check_Anti_Replay(sa_ptr, tc_sdls_processed_frame->tc_sec_header.sn, tc_sdls_processed_frame->tc_sec_header.iv); + + if (status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + } + if(status == CRYPTO_LIB_SUCCESS) // else + { + // Only save the SA (IV/ARSN) if checking the anti-replay counter; Otherwise we don't update. + status = sa_if->sa_save_sa(sa_ptr); if (status != CRYPTO_LIB_SUCCESS) { - if (!aad) free(aad); mc_if->mc_log(status); - return status; // Cryptography IF call failed, return. } - } - if (sa_service_type != SA_PLAINTEXT) + } + } + else + { + if (crypto_config.sa_type == SA_TYPE_MARIADB) { -#ifdef INCREMENT - if (crypto_config.crypto_increment_nontransmitted_iv == SA_INCREMENT_NONTRANSMITTED_IV_TRUE) - { - if (sa_ptr->shivf_len > 0 && sa_ptr->iv_len != 0) - { - Crypto_increment(sa_ptr->iv, sa_ptr->iv_len); - } - } - else // SA_INCREMENT_NONTRANSMITTED_IV_FALSE - { - // Only increment the transmitted portion - if (sa_ptr->shivf_len > 0 && sa_ptr->iv_len != 0) - { - Crypto_increment(sa_ptr->iv + (sa_ptr->iv_len - sa_ptr->shivf_len), sa_ptr->shivf_len); - } - } - if (sa_ptr->shsnf_len > 0) - { - Crypto_increment(sa_ptr->arsn, sa_ptr->arsn_len); - } - -#ifdef SA_DEBUG - if (sa_ptr->iv_len > 0) - { - printf(KYEL "Next IV value is:\n\t"); - for (i = 0; i < sa_ptr->iv_len; i++) - { - printf("%02x", *(sa_ptr->iv + i)); - } - printf("\n" RESET); - printf(KYEL "Next transmitted IV value is:\n\t"); - for (i = sa_ptr->iv_len - sa_ptr->shivf_len; i < sa_ptr->iv_len; i++) - { - printf("%02x", *(sa_ptr->iv + i)); - } - printf("\n" RESET); - } - printf(KYEL "Next ARSN value is:\n\t"); - for (i = 0; i < sa_ptr->arsn_len; i++) - { - printf("%02x", *(sa_ptr->arsn + i)); - } - printf("\n" RESET); - printf(KYEL "Next transmitted ARSN value is:\n\t"); - for (i = sa_ptr->arsn_len - sa_ptr->shsnf_len; i < sa_ptr->arsn_len; i++) - { - printf("%02x", *(sa_ptr->arsn + i)); - } - printf("\n" RESET); -#endif -#endif + if (sa_ptr->ek_ref != NULL) + free(sa_ptr->ek_ref); + free(sa_ptr); } - /* - ** End Authentication / Encryption - */ + } + return status; +} - // Only calculate & insert FECF if CryptoLib is configured to do so & gvcid includes FECF. - if (current_managed_parameters->has_fecf == TC_HAS_FECF) - { -#ifdef FECF_DEBUG - printf(KCYN "Calcing FECF over %d bytes\n" RESET, new_enc_frame_header_field_length - 1); -#endif - if (crypto_config.crypto_create_fecf == CRYPTO_TC_CREATE_FECF_TRUE) - { - new_fecf = Crypto_Calc_FECF(p_new_enc_frame, new_enc_frame_header_field_length - 1); - *(p_new_enc_frame + new_enc_frame_header_field_length - 1) = (uint8_t)((new_fecf & 0xFF00) >> 8); - *(p_new_enc_frame + new_enc_frame_header_field_length) = (uint8_t)(new_fecf & 0x00FF); - } - else // CRYPTO_TC_CREATE_FECF_FALSE - { - *(p_new_enc_frame + new_enc_frame_header_field_length - 1) = (uint8_t)0x00; - *(p_new_enc_frame + new_enc_frame_header_field_length) = (uint8_t)0x00; - } - index += 2; - } +/** + * @brief Function: Crypto_TC_Sanity_Validations + * Checks and validates SA as best as possible + * @param tc_sdls_processed_frame: TC_t* + * @param sa_ptr: SecurityAssociation_t* + * @return int32_t: Success/Failure + **/ +uint32_t Crypto_TC_Sanity_Validations(TC_t* tc_sdls_processed_frame, SecurityAssociation_t** sa_ptr) +{ + uint32_t status = CRYPTO_LIB_SUCCESS; -#ifdef TC_DEBUG - printf(KYEL "Printing new TC Frame of length %d:\n\t", *p_enc_frame_len); - for (i = 0; i <*p_enc_frame_len; i++) - { - printf("%02X",*(p_new_enc_frame + i)); - } - printf("\n\tThe returned length is: %d\n" RESET, new_enc_frame_header_field_length); -#endif + status = sa_if->sa_get_from_spi(tc_sdls_processed_frame->tc_sec_header.spi, sa_ptr); + // If no valid SPI, return + if(status == CRYPTO_LIB_SUCCESS) + { + // Try to assure SA is sane + status = crypto_tc_validate_sa(*sa_ptr); + } + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + } + + return status; +} - *pp_in_frame = p_new_enc_frame; +/** + * @brief Function: Crypto_TC_Get_Ciper_Mode_TCP + * Retrieves TC Process cipher mode + * @param sa_service_type: uint8_t + * @param encryption_cipher: uint32_t* + * @param ecs_is_aead_algorithm: uint8_t* + * @param sa_ptr:SecurityAssociation_t* + **/ +void Crypto_TC_Get_Ciper_Mode_TCP(uint8_t sa_service_type, uint32_t* encryption_cipher, uint8_t* ecs_is_aead_algorithm, SecurityAssociation_t* sa_ptr) +{ + if (sa_service_type != SA_PLAINTEXT) + { + *encryption_cipher = sa_ptr->ecs; + *ecs_is_aead_algorithm = Crypto_Is_AEAD_Algorithm(*encryption_cipher); } +} - status = sa_if->sa_save_sa(sa_ptr); + /** + * @brief Function: Crypto_TC_Calc_Lengths + * Sets fecf and segment header lengths as necessary + * @param fecf_len: uint8_t * + * @param segment_hdr_len: uint8_t* + **/ +void Crypto_TC_Calc_Lengths(uint8_t* fecf_len, uint8_t* segment_hdr_len) +{ + if (current_managed_parameters->has_fecf == TC_NO_FECF) + { + *fecf_len = 0; + } -#ifdef DEBUG - printf(KYEL "----- Crypto_TC_ApplySecurity END -----\n" RESET); -#endif - if (!aad) free(aad); - mc_if->mc_log(status); - return status; + if (current_managed_parameters->has_segmentation_hdr == TC_NO_SEGMENT_HDRS) + { + *segment_hdr_len = 0; + } } -/** - * @brief Function: Crypto_TC_ProcessSecurity - * Performs Authenticated decryption, decryption, and authentication - * @param ingest: uint8_t* - * @param len_ingest: int* - * @param tc_sdls_processed_frame: TC_t* - * @return int32: Success/Failure -**/ -int32_t Crypto_TC_ProcessSecurity(uint8_t* ingest, int* len_ingest, TC_t* tc_sdls_processed_frame) + /** + * @brief Function: Crypto_TC_Set_Segment_Header + * Sets up TC Segment Header as necessary + * @param tc_sdls_processed_frame: TC_t* + * @param ingest: uint8_t* + * @param byte_idx: int* + **/ +void Crypto_TC_Set_Segment_Header(TC_t* tc_sdls_processed_frame, uint8_t* ingest, int* byte_idx) { - // Pass-through to maintain original function signature when CAM isn't used. - return Crypto_TC_ProcessSecurity_Cam(ingest, len_ingest, tc_sdls_processed_frame, NULL); + int byte_idx_tmp = *byte_idx; + if (current_managed_parameters->has_segmentation_hdr == TC_HAS_SEGMENT_HDRS) + { + tc_sdls_processed_frame->tc_sec_header.sh = (uint8_t)ingest[*byte_idx]; + byte_idx_tmp++; + } + *byte_idx = byte_idx_tmp; } /** @@ -825,6 +1693,7 @@ int32_t Crypto_TC_ProcessSecurity_Cam(uint8_t* ingest, int* len_ingest, TC_t* tc // Loads the ingest frame into the global tc_frame while performing decryption { // Local Variables + cam_cookies = cam_cookies; int32_t status = CRYPTO_LIB_SUCCESS; SecurityAssociation_t* sa_ptr = NULL; uint8_t sa_service_type = -1; @@ -833,27 +1702,16 @@ int32_t Crypto_TC_ProcessSecurity_Cam(uint8_t* ingest, int* len_ingest, TC_t* tc uint32_t encryption_cipher; uint8_t ecs_is_aead_algorithm = -1; crypto_key_t* ekp = NULL; + crypto_key_t* akp = NULL; - if ((mc_if == NULL) || (crypto_config.init_status == UNITIALIZED)) - { - printf(KRED "ERROR: CryptoLib Configuration Not Set! -- CRYPTO_LIB_ERR_NO_CONFIG, Will Exit\n" RESET); - status = CRYPTO_LIB_ERR_NO_CONFIG; - mc_if->mc_log(status); - return status; - } - -#ifdef DEBUG - printf(KYEL "\n----- Crypto_TC_ProcessSecurity START -----\n" RESET); -#endif + int byte_idx = 0; - if (*len_ingest < 5) // Frame length doesn't even have enough bytes for header -- error out. + status = Crypto_TC_Process_Sanity_Check(len_ingest); + if (status != CRYPTO_LIB_SUCCESS) { - status = CRYPTO_LIB_ERR_INPUT_FRAME_TOO_SHORT_FOR_TC_STANDARD; - mc_if->mc_log(status); return status; - } + } - int byte_idx = 0; // Primary Header tc_sdls_processed_frame->tc_header.tfvn = ((uint8_t)ingest[byte_idx] & 0xC0) >> 6; tc_sdls_processed_frame->tc_header.bypass = ((uint8_t)ingest[byte_idx] & 0x20) >> 5; @@ -888,33 +1746,21 @@ int32_t Crypto_TC_ProcessSecurity_Cam(uint8_t* ingest, int* len_ingest, TC_t* tc mc_if->mc_log(status); return status; } // Unable to get necessary Managed Parameters for TC TF -- return with error. + // Segment Header - if (current_managed_parameters->has_segmentation_hdr == TC_HAS_SEGMENT_HDRS) - { - tc_sdls_processed_frame->tc_sec_header.sh = (uint8_t)ingest[byte_idx]; - byte_idx++; - } + Crypto_TC_Set_Segment_Header(tc_sdls_processed_frame, ingest, &byte_idx); + // Security Header tc_sdls_processed_frame->tc_sec_header.spi = ((uint8_t)ingest[byte_idx] << 8) | (uint8_t)ingest[byte_idx + 1]; byte_idx += 2; -#ifdef TC_DEBUG - printf("vcid = %d \n", tc_sdls_processed_frame->tc_header.vcid); - printf("spi = %d \n", tc_sdls_processed_frame->tc_sec_header.spi); -#endif - status = sa_if->sa_get_from_spi(tc_sdls_processed_frame->tc_sec_header.spi, &sa_ptr); - // If no valid SPI, return - if (status != CRYPTO_LIB_SUCCESS) - { - mc_if->mc_log(status); - return status; - } - // Try to assure SA is sane - status = crypto_tc_validate_sa(sa_ptr); - if (status != CRYPTO_LIB_SUCCESS) - { - mc_if->mc_log(status); - return status; - } + +#ifdef TC_DEBUG + printf("vcid = %d \n", tc_sdls_processed_frame->tc_header.vcid); + printf("spi = %d \n", tc_sdls_processed_frame->tc_sec_header.spi); +#endif + + status = Crypto_TC_Sanity_Validations(tc_sdls_processed_frame, &sa_ptr); + // Allocate the necessary byte arrays within the security header + trailer given the SA //tc_sdls_processed_frame->tc_sec_header.iv = calloc(1, sa_ptr->iv_len); //tc_sdls_processed_frame->tc_sec_header.sn = calloc(1, sa_ptr->arsn_len); @@ -928,37 +1774,11 @@ int32_t Crypto_TC_ProcessSecurity_Cam(uint8_t* ingest, int* len_ingest, TC_t* tc tc_sdls_processed_frame->tc_sec_trailer.mac_field_len = sa_ptr->stmacf_len; // Determine SA Service Type - if ((sa_ptr->est == 0) && (sa_ptr->ast == 0)) - { - sa_service_type = SA_PLAINTEXT; - } - else if ((sa_ptr->est == 0) && (sa_ptr->ast == 1)) - { - sa_service_type = SA_AUTHENTICATION; - } - else if ((sa_ptr->est == 1) && (sa_ptr->ast == 0)) - { - sa_service_type = SA_ENCRYPTION; - } - else if ((sa_ptr->est == 1) && (sa_ptr->ast == 1)) - { - sa_service_type = SA_AUTHENTICATED_ENCRYPTION; - } - else - { - // Probably unnecessary check - // Leaving for now as it would be cleaner in SA to have an association enum returned I believe - printf(KRED "Error: SA Service Type is not defined! \n" RESET); - status = CRYPTO_LIB_ERROR; - mc_if->mc_log(status); - return status; - } + Crypto_TC_Get_SA_Service_Type(&sa_service_type, sa_ptr); + // Determine Algorithm cipher & mode. // TODO - Parse authentication_cipher, and handle AEAD cases properly - if (sa_service_type != SA_PLAINTEXT) - { - encryption_cipher = sa_ptr->ecs; - ecs_is_aead_algorithm = Crypto_Is_AEAD_Algorithm(encryption_cipher); - } + Crypto_TC_Get_Ciper_Mode_TCP(sa_service_type, &encryption_cipher, &ecs_is_aead_algorithm, sa_ptr); + #ifdef TC_DEBUG switch (sa_service_type) { @@ -979,63 +1799,23 @@ int32_t Crypto_TC_ProcessSecurity_Cam(uint8_t* ingest, int* len_ingest, TC_t* tc // TODO: Calculate lengths when needed uint8_t fecf_len = FECF_SIZE; - if (current_managed_parameters->has_fecf == TC_NO_FECF) - { - fecf_len = 0; - } - uint8_t segment_hdr_len = TC_SEGMENT_HDR_SIZE; - if (current_managed_parameters->has_segmentation_hdr == TC_NO_SEGMENT_HDRS) - { - segment_hdr_len = 0; - } + + Crypto_TC_Calc_Lengths(&fecf_len, &segment_hdr_len); // Parse & Check FECF - if (current_managed_parameters->has_fecf == TC_HAS_FECF) - { - tc_sdls_processed_frame->tc_sec_trailer.fecf = (((ingest[tc_sdls_processed_frame->tc_header.fl - 1] << 8) & 0xFF00) | - (ingest[tc_sdls_processed_frame->tc_header.fl] & 0x00FF)); + Crypto_TC_Parse_Check_FECF(ingest, len_ingest, tc_sdls_processed_frame); - if (crypto_config.crypto_check_fecf == TC_CHECK_FECF_TRUE) - { - uint16_t received_fecf = tc_sdls_processed_frame->tc_sec_trailer.fecf; - // Calculate our own - uint16_t calculated_fecf = Crypto_Calc_FECF(ingest, *len_ingest - 2); - // Compare - if (received_fecf != calculated_fecf) - { -#ifdef DEBUG - printf("Received FECF is 0x%04X\n", received_fecf); - printf("Calculated FECF is 0x%04X\n", calculated_fecf); - printf("FECF was Calced over %d bytes\n", *len_ingest - 2); -#endif - status = CRYPTO_LIB_ERR_INVALID_FECF; - mc_if->mc_log(status); - return status; - } - } - } // Parse transmitted portion of IV from received frame (Will be Whole IV if iv_len==shivf_len) memcpy((tc_sdls_processed_frame->tc_sec_header.iv + (sa_ptr->iv_len - sa_ptr->shivf_len)), &(ingest[TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN]), sa_ptr->shivf_len); // Handle non-transmitted IV increment case (transmitted-portion roll-over) - if (sa_ptr->shivf_len < sa_ptr->iv_len && - crypto_config.ignore_anti_replay == TC_IGNORE_ANTI_REPLAY_FALSE && - crypto_config.crypto_increment_nontransmitted_iv == SA_INCREMENT_NONTRANSMITTED_IV_TRUE) - { - status = crypto_handle_incrementing_nontransmitted_counter(tc_sdls_processed_frame->tc_sec_header.iv, sa_ptr->iv, sa_ptr->iv_len, sa_ptr->shivf_len, sa_ptr->arsnw); - if (status != CRYPTO_LIB_SUCCESS) - { - mc_if->mc_log(status); - return status; - } - } - else // Not checking IV ARSNW or only non-transmitted portion is static; Note, non-transmitted IV in SA must match frame or will fail MAC check. + status = Crypto_TC_Nontransmitted_IV_Increment(sa_ptr, tc_sdls_processed_frame); + if (status != CRYPTO_LIB_SUCCESS) { - // Retrieve non-transmitted portion of IV from SA (if applicable) - memcpy(tc_sdls_processed_frame->tc_sec_header.iv, sa_ptr->iv, sa_ptr->iv_len - sa_ptr->shivf_len); + return status; } #ifdef DEBUG @@ -1048,20 +1828,10 @@ int32_t Crypto_TC_ProcessSecurity_Cam(uint8_t* ingest, int* len_ingest, TC_t* tc &(ingest[TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN + sa_ptr->shivf_len]), sa_ptr->shsnf_len); // Handle non-transmitted SN increment case (transmitted-portion roll-over) - if (sa_ptr->shsnf_len < sa_ptr->arsn_len && - crypto_config.ignore_anti_replay == TC_IGNORE_ANTI_REPLAY_FALSE) - { - status = crypto_handle_incrementing_nontransmitted_counter(tc_sdls_processed_frame->tc_sec_header.sn, sa_ptr->arsn, sa_ptr->arsn_len, sa_ptr->shsnf_len, sa_ptr->arsnw); - if (status != CRYPTO_LIB_SUCCESS) - { - mc_if->mc_log(status); - return status; - } - } - else // Not checking ARSN in ARSNW + status = Crypto_TC_Nontransmitted_SN_Increment(sa_ptr, tc_sdls_processed_frame); + if (status != CRYPTO_LIB_SUCCESS) { - // Parse non-transmitted portion of ARSN from SA - memcpy(tc_sdls_processed_frame->tc_sec_header.sn, sa_ptr->arsn, sa_ptr->arsn_len - sa_ptr->shsnf_len); + return status; } #ifdef DEBUG @@ -1076,31 +1846,14 @@ int32_t Crypto_TC_ProcessSecurity_Cam(uint8_t* ingest, int* len_ingest, TC_t* tc sa_ptr->shplf_len); // Parse MAC, prepare AAD - if ((sa_service_type == SA_AUTHENTICATION) || (sa_service_type == SA_AUTHENTICATED_ENCRYPTION)) - { - uint16_t tc_mac_start_index = tc_sdls_processed_frame->tc_header.fl + 1 - fecf_len - sa_ptr->stmacf_len; + status = Crypto_TC_Prep_AAD(tc_sdls_processed_frame, fecf_len, sa_service_type, ecs_is_aead_algorithm, &aad_len, sa_ptr, segment_hdr_len, ingest, &aad); - // Parse the received MAC - memcpy((tc_sdls_processed_frame->tc_sec_trailer.mac), - &(ingest[tc_mac_start_index]), sa_ptr->stmacf_len); -#ifdef DEBUG - printf("MAC Parsed from Frame:\n"); - Crypto_hexprint(tc_sdls_processed_frame->tc_sec_trailer.mac, sa_ptr->stmacf_len); -#endif - aad_len = tc_mac_start_index; - if ((sa_service_type == SA_AUTHENTICATED_ENCRYPTION) && (ecs_is_aead_algorithm == CRYPTO_TRUE)) - { - aad_len = TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN + sa_ptr->shivf_len + sa_ptr->shsnf_len + - sa_ptr->shplf_len; - } - if (sa_ptr->abm_len < aad_len) - { - status = CRYPTO_LIB_ERR_ABM_TOO_SHORT_FOR_AAD; - mc_if->mc_log(status); - return status; - } - aad = Crypto_Prepare_TC_AAD(ingest, aad_len, sa_ptr->abm); + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; } + uint16_t tc_enc_payload_start_index = TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN + sa_ptr->shivf_len + sa_ptr->shsnf_len + sa_ptr->shplf_len; @@ -1121,178 +1874,38 @@ int32_t Crypto_TC_ProcessSecurity_Cam(uint8_t* ingest, int* len_ingest, TC_t* tc #endif /* Get Key */ - ekp = key_if->get_key(sa_ptr->ekid); - if (ekp == NULL) - { - status = CRYPTO_LIB_ERR_KEY_ID_ERROR; - mc_if->mc_log(status); - return status; - } - - crypto_key_t* akp = NULL; - akp = key_if->get_key(sa_ptr->akid); - if (akp == NULL) + status = Crypto_TC_Get_Keys(&ekp, &akp, sa_ptr); + if (status != CRYPTO_LIB_SUCCESS) { - status = CRYPTO_LIB_ERR_KEY_ID_ERROR; mc_if->mc_log(status); - return status; - } - - if (sa_service_type != SA_PLAINTEXT && ecs_is_aead_algorithm == CRYPTO_TRUE) - { - // Check that key length to be used ets the algorithm requirement - if ((int32_t)ekp->key_len != Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs)) - { - if (!aad) free(aad); - status = CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; - mc_if->mc_log(status); - return status; - } - - status = cryptography_if->cryptography_aead_decrypt( - tc_sdls_processed_frame->tc_pdu, // plaintext output - (size_t)(tc_sdls_processed_frame->tc_pdu_len), // length of data - &(ingest[tc_enc_payload_start_index]), // ciphertext input - (size_t)(tc_sdls_processed_frame->tc_pdu_len), // in data length - &(ekp->value[0]), // Key - Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs), // - sa_ptr, // SA for key reference - tc_sdls_processed_frame->tc_sec_header.iv, // IV - sa_ptr->iv_len, // IV Length - tc_sdls_processed_frame->tc_sec_trailer.mac, // Frame Expected Tag - sa_ptr->stmacf_len, // tag size - aad, // additional authenticated data - aad_len, // length of AAD - (sa_ptr->est), // Decryption Bool - (sa_ptr->ast), // Authentication Bool - (sa_ptr->ast), // AAD Bool - &sa_ptr->ecs, // encryption cipher - &sa_ptr->acs, // authentication cipher - cam_cookies // - ); - } - else if (sa_service_type != SA_PLAINTEXT && ecs_is_aead_algorithm == CRYPTO_FALSE) // Non aead algorithm - { - // TODO - implement non-AEAD algorithm logic - if (sa_service_type == SA_AUTHENTICATION || sa_service_type == SA_AUTHENTICATED_ENCRYPTION) - { - // Check that key length to be used ets the algorithm requirement - if ((int32_t)akp->key_len != Crypto_Get_ACS_Algo_Keylen(sa_ptr->acs)) - { - if (!aad) free(aad); - status = CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; - mc_if->mc_log(status); - return status; - } - - status = cryptography_if->cryptography_validate_authentication( - tc_sdls_processed_frame->tc_pdu, // plaintext output - (size_t)(tc_sdls_processed_frame->tc_pdu_len), // length of data - &(ingest[tc_enc_payload_start_index]), // ciphertext input - (size_t)(tc_sdls_processed_frame->tc_pdu_len), // in data length - &(akp->value[0]), // Key - Crypto_Get_ACS_Algo_Keylen(sa_ptr->acs), // - sa_ptr, // SA for key reference - tc_sdls_processed_frame->tc_sec_header.iv, // IV - sa_ptr->iv_len, // IV Length - tc_sdls_processed_frame->tc_sec_trailer.mac, // Frame Expected Tag - sa_ptr->stmacf_len, // tag size - aad, // additional authenticated data - aad_len, // length of AAD - CRYPTO_CIPHER_NONE, // encryption cipher - sa_ptr->acs, // authentication cipher - cam_cookies // - ); - } - if (sa_service_type == SA_ENCRYPTION || sa_service_type == SA_AUTHENTICATED_ENCRYPTION) - { - // Check that key length to be used emets the algorithm requirement - if ((int32_t)ekp->key_len != Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs)) - { - if (!aad) free(aad); - status = CRYPTO_LIB_ERR_KEY_LENGTH_ERROR; - mc_if->mc_log(status); - return status; - } - - status = cryptography_if->cryptography_decrypt( - tc_sdls_processed_frame->tc_pdu, // plaintext output - (size_t)(tc_sdls_processed_frame->tc_pdu_len), // length of data - &(ingest[tc_enc_payload_start_index]), // ciphertext input - (size_t)(tc_sdls_processed_frame->tc_pdu_len), // in data length - &(ekp->value[0]), // Key - Crypto_Get_ECS_Algo_Keylen(sa_ptr->ecs), // - sa_ptr, // SA for key reference - tc_sdls_processed_frame->tc_sec_header.iv, // IV - sa_ptr->iv_len, // IV Length - &sa_ptr->ecs, // encryption cipher - &sa_ptr->acs, // authentication cipher - cam_cookies // - ); - - // Handle Padding Removal - if (sa_ptr->shplf_len != 0) - { - int padding_location = TC_FRAME_HEADER_SIZE + segment_hdr_len + SPI_LEN + sa_ptr->shivf_len + - sa_ptr->shsnf_len; - uint16_t padding_amount = 0; - // Get Padding Amount from ingest frame - padding_amount = (int)ingest[padding_location]; - // Remove Padding from final decrypted portion - tc_sdls_processed_frame->tc_pdu_len -= padding_amount; - } - } - } - else if (sa_service_type == SA_PLAINTEXT) - { - memcpy(tc_sdls_processed_frame->tc_pdu, &(ingest[tc_enc_payload_start_index]), - tc_sdls_processed_frame->tc_pdu_len); + return status; } + status = Crypto_TC_Do_Decrypt(sa_service_type, ecs_is_aead_algorithm, ekp, sa_ptr, aad, tc_sdls_processed_frame, ingest, tc_enc_payload_start_index, aad_len, cam_cookies, akp, segment_hdr_len); if (status != CRYPTO_LIB_SUCCESS) { - if (!aad) free(aad); + Crypto_TC_Safe_Free_Ptr(aad); mc_if->mc_log(status); return status; // Cryptography IF call failed, return. } // Now that MAC has been verified, check IV & ARSN if applicable - if (crypto_config.ignore_anti_replay == TC_IGNORE_ANTI_REPLAY_FALSE && status == CRYPTO_LIB_SUCCESS) - { - status = Crypto_Check_Anti_Replay(sa_ptr, tc_sdls_processed_frame->tc_sec_header.sn, tc_sdls_processed_frame->tc_sec_header.iv); - - if (status != CRYPTO_LIB_SUCCESS) - { - if (!aad) free(aad); - mc_if->mc_log(status); - return status; - } - - // Only save the SA (IV/ARSN) if checking the anti-replay counter; Otherwise we don't update. - status = sa_if->sa_save_sa(sa_ptr); - if (status != CRYPTO_LIB_SUCCESS) - { - if (!aad) free(aad); - mc_if->mc_log(status); - return status; - } - } - else + status = Crypto_TC_Check_IV_ARSN(sa_ptr, tc_sdls_processed_frame); + if (status != CRYPTO_LIB_SUCCESS) { - if (crypto_config.sa_type == SA_TYPE_MARIADB) - { - if (sa_ptr->ek_ref != NULL) - free(sa_ptr->ek_ref); - free(sa_ptr); - } + Crypto_TC_Safe_Free_Ptr(aad); + mc_if->mc_log(status); + return status; // Cryptography IF call failed, return. } - + // Extended PDU processing, if applicable if (status == CRYPTO_LIB_SUCCESS && crypto_config.process_sdls_pdus == TC_PROCESS_SDLS_PDUS_TRUE) { status = Crypto_Process_Extended_Procedure_Pdu(tc_sdls_processed_frame, ingest); } - if (!aad) free(aad); + + Crypto_TC_Safe_Free_Ptr(aad); + mc_if->mc_log(status); return status; } @@ -1371,6 +1984,7 @@ uint8_t* Crypto_Prepare_TC_AAD(uint8_t* buffer, uint16_t len_aad, uint8_t* abm_b } /** + * TODO: Single Return * @brief Function: crypto_tc_validate_sa * Helper function to assist with ensuring sane SA configurations * @param sa: SecurityAssociation_t* @@ -1402,6 +2016,16 @@ static int32_t crypto_tc_validate_sa(SecurityAssociation_t* sa) return CRYPTO_LIB_SUCCESS; } +/** + * @brief Function: crypto_handle_incrementing_nontransmitted_counter + * Handles incrementing of nontransmitted counter + * @param dest: uint8_t* + * @param src: uint8_t* + * @param src_full_len: int + * @param transmitted_len: int + * @param window: int + * @return int32: Success/Failure +**/ static int32_t crypto_handle_incrementing_nontransmitted_counter(uint8_t* dest, uint8_t* src, int src_full_len, int transmitted_len, int window) { int32_t status = CRYPTO_LIB_SUCCESS; diff --git a/src/crypto/kmc/base64url.c b/src/crypto/kmc/base64url.c index 0092a2df..dd2acaed 100644 --- a/src/crypto/kmc/base64url.c +++ b/src/crypto/kmc/base64url.c @@ -167,6 +167,40 @@ void base64urlEncode(const void* input, size_t inputLen, char_t* output, } +void base64urlDecode_rempadding(size_t inputLen, uint32_t value, size_t *n, uint8_t* p) +{ + //All trailing pad characters are omitted in Base64url + if((inputLen % 4) == 2) + { + //The last block contains only 1 byte + if(p != NULL) + { + //Decode the last byte + p[*n] = (value >> 4) & 0xFF; + } + + //Adjust the length of the decoded data + *n = *n + 1; + } + else if((inputLen % 4) == 3) + { + //The last block contains only 2 bytes + if(p != NULL) + { + //Decode the last two bytes + p[*n] = (value >> 10) & 0xFF; + p[*n + 1] = (value >> 2) & 0xFF; + } + + //Adjust the length of the decoded data + *n = *n + 2; + } + else + { + //No pad characters in this case + } +} + /** * @brief Base64url decoding algorithm * @param[in] input Base64url-encoded string @@ -252,36 +286,7 @@ int32_t base64urlDecode(const char_t* input, size_t inputLen, void* output, //Check status code if(!error) { - //All trailing pad characters are omitted in Base64url - if((inputLen % 4) == 2) - { - //The last block contains only 1 byte - if(p != NULL) - { - //Decode the last byte - p[n] = (value >> 4) & 0xFF; - } - - //Adjust the length of the decoded data - n++; - } - else if((inputLen % 4) == 3) - { - //The last block contains only 2 bytes - if(p != NULL) - { - //Decode the last two bytes - p[n] = (value >> 10) & 0xFF; - p[n + 1] = (value >> 2) & 0xFF; - } - - //Adjust the length of the decoded data - n += 2; - } - else - { - //No pad characters in this case - } + base64urlDecode_rempadding(inputLen,value, &n, p); } //Total number of bytes that have been written diff --git a/src/crypto/libgcrypt/cryptography_interface_libgcrypt.template.c b/src/crypto/libgcrypt/cryptography_interface_libgcrypt.template.c index 7b8ea32f..90b467d6 100644 --- a/src/crypto/libgcrypt/cryptography_interface_libgcrypt.template.c +++ b/src/crypto/libgcrypt/cryptography_interface_libgcrypt.template.c @@ -509,64 +509,54 @@ static int32_t cryptography_encrypt(uint8_t* data_out, size_t len_data_out, return status; } -static int32_t cryptography_aead_encrypt(uint8_t* data_out, size_t len_data_out, - uint8_t* data_in, size_t len_data_in, - uint8_t* key, uint32_t len_key, - SecurityAssociation_t* sa_ptr, // For key index or key references (when key not passed in explicitly via key param) - uint8_t* iv, uint32_t iv_len, - uint8_t* mac, uint32_t mac_size, - uint8_t* aad, uint32_t aad_len, - uint8_t encrypt_bool, uint8_t authenticate_bool, - uint8_t aad_bool, uint8_t* ecs, uint8_t* acs, char* cam_cookies) +int32_t cryptography_verify_ecs_enum_algo(uint8_t* ecs, int32_t* algo, int32_t* mode) { - gcry_error_t gcry_error = GPG_ERR_NO_ERROR; - gcry_cipher_hd_t tmp_hd; int32_t status = CRYPTO_LIB_SUCCESS; - uint8_t* key_ptr = key; - - // Fix warning - acs = acs; - cam_cookies = cam_cookies; - - sa_ptr = sa_ptr; // Unused in this implementation - - // Select correct libgcrypt ecs enum - int32_t algo = -1; if (ecs != NULL) { - algo = cryptography_get_ecs_algo(*ecs); - if (algo == CRYPTO_LIB_ERR_UNSUPPORTED_ECS) + *algo = cryptography_get_ecs_algo(*ecs); + if (*algo == CRYPTO_LIB_ERR_UNSUPPORTED_ECS) { - return CRYPTO_LIB_ERR_UNSUPPORTED_ECS; + status = CRYPTO_LIB_ERR_UNSUPPORTED_ECS; + return status; } } else { - return CRYPTO_LIB_ERR_NULL_ECS_PTR; + status = CRYPTO_LIB_ERR_NULL_ECS_PTR; + return status; } // Verify the mode to accompany the ecs enum - int32_t mode = -1; - mode = cryptography_get_ecs_mode(*ecs); - if (mode == CRYPTO_LIB_ERR_UNSUPPORTED_ECS_MODE) return CRYPTO_LIB_ERR_UNSUPPORTED_ECS_MODE; - - // TODO: Get Flag Functionality + *mode = cryptography_get_ecs_mode(*ecs); + if (*mode == CRYPTO_LIB_ERR_UNSUPPORTED_ECS_MODE) + { + status = CRYPTO_LIB_ERR_UNSUPPORTED_ECS_MODE; + return status; + } + return status; +} + +int32_t cryptography_gcry_setup(int32_t mode, int32_t algo, gcry_cipher_hd_t* tmp_hd, uint8_t* key_ptr, uint32_t len_key, uint8_t* iv, uint32_t iv_len, gcry_error_t* gcry_error) +{ + int32_t status = CRYPTO_LIB_SUCCESS; + if(mode == CRYPTO_CIPHER_AES256_CBC_MAC) { - gcry_error = gcry_cipher_open(&(tmp_hd), algo, mode, GCRY_CIPHER_CBC_MAC); + *gcry_error = gcry_cipher_open(tmp_hd, algo, mode, GCRY_CIPHER_CBC_MAC); } else { - gcry_error = gcry_cipher_open(&(tmp_hd), algo, mode, GCRY_CIPHER_NONE); + *gcry_error = gcry_cipher_open(tmp_hd, algo, mode, GCRY_CIPHER_NONE); } - if ((gcry_error & GPG_ERR_CODE_MASK) != GPG_ERR_NO_ERROR) + if ((*gcry_error & GPG_ERR_CODE_MASK) != GPG_ERR_NO_ERROR) { - printf(KRED "ERROR: gcry_cipher_open error code %d\n" RESET, gcry_error & GPG_ERR_CODE_MASK); - printf(KRED "Failure: %s/%s\n", gcry_strsource(gcry_error), gcry_strerror(gcry_error)); + printf(KRED "ERROR: gcry_cipher_open error code %d\n" RESET, *gcry_error & GPG_ERR_CODE_MASK); + printf(KRED "Failure: %s/%s\n", gcry_strsource(*gcry_error), gcry_strerror(*gcry_error)); status = CRYPTO_LIB_ERR_LIBGCRYPT_ERROR; return status; } - gcry_error = gcry_cipher_setkey(tmp_hd, key_ptr, len_key); + *gcry_error = gcry_cipher_setkey(*tmp_hd, key_ptr, len_key); #ifdef SA_DEBUG uint32_t i; printf(KYEL "AEAD MAC: Printing Key:\n\t"); @@ -577,23 +567,64 @@ static int32_t cryptography_aead_encrypt(uint8_t* data_out, size_t len_data_out, printf("\n"); #endif - if ((gcry_error & GPG_ERR_CODE_MASK) != GPG_ERR_NO_ERROR) + if ((*gcry_error & GPG_ERR_CODE_MASK) != GPG_ERR_NO_ERROR) { - printf(KRED "ERROR: gcry_cipher_setkey error code %d\n" RESET, gcry_error & GPG_ERR_CODE_MASK); - printf(KRED "Failure: %s/%s\n", gcry_strsource(gcry_error), gcry_strerror(gcry_error)); + printf(KRED "ERROR: gcry_cipher_setkey error code %d\n" RESET, *gcry_error & GPG_ERR_CODE_MASK); + printf(KRED "Failure: %s/%s\n", gcry_strsource(*gcry_error), gcry_strerror(*gcry_error)); status = CRYPTO_LIB_ERR_LIBGCRYPT_ERROR; - gcry_cipher_close(tmp_hd); + gcry_cipher_close(*tmp_hd); return status; } - gcry_error = gcry_cipher_setiv(tmp_hd, iv, iv_len); - if ((gcry_error & GPG_ERR_CODE_MASK) != GPG_ERR_NO_ERROR) + *gcry_error = gcry_cipher_setiv(*tmp_hd, iv, iv_len); + if ((*gcry_error & GPG_ERR_CODE_MASK) != GPG_ERR_NO_ERROR) { - printf(KRED "ERROR: gcry_cipher_setiv error code %d\n" RESET, gcry_error & GPG_ERR_CODE_MASK); - printf(KRED "Failure: %s/%s\n", gcry_strsource(gcry_error), gcry_strerror(gcry_error)); + printf(KRED "ERROR: gcry_cipher_setiv error code %d\n" RESET, *gcry_error & GPG_ERR_CODE_MASK); + printf(KRED "Failure: %s/%s\n", gcry_strsource(*gcry_error), gcry_strerror(*gcry_error)); status = CRYPTO_LIB_ERR_LIBGCRYPT_ERROR; - gcry_cipher_close(tmp_hd); + gcry_cipher_close(*tmp_hd); return status; } + return status; +} + +static int32_t cryptography_aead_encrypt(uint8_t* data_out, size_t len_data_out, + uint8_t* data_in, size_t len_data_in, + uint8_t* key, uint32_t len_key, + SecurityAssociation_t* sa_ptr, // For key index or key references (when key not passed in explicitly via key param) + uint8_t* iv, uint32_t iv_len, + uint8_t* mac, uint32_t mac_size, + uint8_t* aad, uint32_t aad_len, + uint8_t encrypt_bool, uint8_t authenticate_bool, + uint8_t aad_bool, uint8_t* ecs, uint8_t* acs, char* cam_cookies) +{ + gcry_error_t gcry_error = GPG_ERR_NO_ERROR; + gcry_cipher_hd_t tmp_hd = 0; + int32_t status = CRYPTO_LIB_SUCCESS; + uint8_t* key_ptr = key; + + // Fix warning + acs = acs; + cam_cookies = cam_cookies; + + sa_ptr = sa_ptr; // Unused in this implementation + + // Select correct libgcrypt ecs enum + int32_t algo = -1; + int32_t mode = -1; + status = cryptography_verify_ecs_enum_algo(ecs, &algo, &mode); + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } + + // TODO: Get Flag Functionality + status = cryptography_gcry_setup(mode, algo, &tmp_hd, key_ptr, len_key, iv, iv_len, &gcry_error); + if(status != CRYPTO_LIB_SUCCESS) + { + mc_if->mc_log(status); + return status; + } #ifdef DEBUG size_t j; @@ -680,6 +711,7 @@ static int32_t cryptography_aead_encrypt(uint8_t* data_out, size_t len_data_out, } #ifdef MAC_DEBUG + uint32_t i = 0; printf("MAC = 0x"); for (i = 0; i < mac_size; i++) { diff --git a/src/sa/internal/sa_interface_inmemory.template.c b/src/sa/internal/sa_interface_inmemory.template.c index 15764669..7e129e5c 100644 --- a/src/sa/internal/sa_interface_inmemory.template.c +++ b/src/sa/internal/sa_interface_inmemory.template.c @@ -459,24 +459,10 @@ static int32_t sa_get_from_spi(uint16_t spi, SecurityAssociation_t** security_as return status; } -/** - * @brief Function: sa_get_operational_sa_from_gvcid - * @param tfvn: uint8 - * @param scid: uint16 - * @param vcid: uint16 - * @param mapid: uint8 // tc only - * @param security_association: SecurityAssociation_t** - * @return int32: Success/Failure - **/ -static int32_t sa_get_operational_sa_from_gvcid(uint8_t tfvn, uint16_t scid, uint16_t vcid, uint8_t mapid, - SecurityAssociation_t** security_association) +int32_t sa_get_operational_sa_from_gvcid_find_iv(uint8_t tfvn, uint16_t scid, uint16_t vcid, uint8_t mapid, SecurityAssociation_t** security_association) { int32_t status = CRYPTO_LIB_ERR_NO_OPERATIONAL_SA; - int i; - if (sa == NULL) - { - return CRYPTO_LIB_ERR_NO_INIT; - } + int i = 0; for (i = 0; i < NUM_SA; i++) { @@ -493,12 +479,14 @@ static int32_t sa_get_operational_sa_from_gvcid(uint8_t tfvn, uint16_t scid, uin // Must have IV if using libgcrypt and auth/enc if (sa[i].iv == NULL && (sa[i].ast == 1 || sa[i].est == 1) && crypto_config.cryptography_type != CRYPTOGRAPHY_TYPE_KMCCRYPTO) { - return CRYPTO_LIB_ERR_NULL_IV; + status = CRYPTO_LIB_ERR_NULL_IV; + return status; } // Must have ABM if doing authentication if (sa[i].abm == NULL && sa[i].ast) { - return CRYPTO_LIB_ERR_NULL_ABM; + status = CRYPTO_LIB_ERR_NULL_ABM; + return status; } #ifdef SA_DEBUG @@ -512,84 +500,179 @@ static int32_t sa_get_operational_sa_from_gvcid(uint8_t tfvn, uint16_t scid, uin break; } } + return status; +} - // If not a success, attempt to generate a meaningful error code - if (status != CRYPTO_LIB_SUCCESS) - { -#ifdef SA_DEBUG - printf(KRED "Error - Making best attempt at a useful error code:\n\t" RESET); -#endif - for (i = 0; i < NUM_SA; i++) - { - // Could possibly have more than one field mismatched, - // ordering so the 'most accurate' SA's error is returned - // (determined by matching header fields L to R) - if ((sa[i].gvcid_blk.tfvn != tfvn) && (sa[i].gvcid_blk.scid == scid) && +void sa_mismatched_tfvn_error(int * i_p, int32_t* status, uint8_t tfvn, uint16_t scid, uint16_t vcid, uint8_t mapid) +{ + int i = *i_p; + if ((sa[i].gvcid_blk.tfvn != tfvn) && (sa[i].gvcid_blk.scid == scid) && (sa[i].gvcid_blk.vcid == vcid) && (sa[i].gvcid_blk.mapid == mapid && sa[i].sa_state == SA_OPERATIONAL)) - { + { #ifdef SA_DEBUG - printf(KRED "An operational SA was found - but mismatched tfvn.\n" RESET); - printf(KRED "SA is %d\n", i); - printf(KRED "Incoming tfvn is %d\n", tfvn); - printf(KRED "SA tfvn is %d\n", sa[i].gvcid_blk.tfvn); + printf(KRED "An operational SA was found - but mismatched tfvn.\n" RESET); + printf(KRED "SA is %d\n", i); + printf(KRED "Incoming tfvn is %d\n", tfvn); + printf(KRED "SA tfvn is %d\n", sa[i].gvcid_blk.tfvn); #endif - status = CRYPTO_LIB_ERR_INVALID_TFVN; - } - else if ((sa[i].gvcid_blk.tfvn == tfvn) && (sa[i].gvcid_blk.scid != scid) && + *status = CRYPTO_LIB_ERR_INVALID_TFVN; + } + *i_p = i; +} + +void sa_mismatched_scid(int* i_p, int32_t* status, uint8_t tfvn, uint16_t scid, uint16_t vcid, uint8_t mapid) +{ + int i = *i_p; + if ((sa[i].gvcid_blk.tfvn == tfvn) && (sa[i].gvcid_blk.scid != scid) && (sa[i].gvcid_blk.vcid == vcid) && (sa[i].gvcid_blk.mapid == mapid && sa[i].sa_state == SA_OPERATIONAL)) - { + { #ifdef SA_DEBUG - printf(KRED "An operational SA was found - but mismatched scid.\n" RESET); - printf(KRED "SA is %d\n", i); - printf(KRED "SCID is %d\n", scid); - printf(KRED "gvcid_blk SCID is %d\n", sa[i].gvcid_blk.scid); + printf(KRED "An operational SA was found - but mismatched scid.\n" RESET); + printf(KRED "SA is %d\n", i); + printf(KRED "SCID is %d\n", scid); + printf(KRED "gvcid_blk SCID is %d\n", sa[i].gvcid_blk.scid); #endif - status = CRYPTO_LIB_ERR_INVALID_SCID; - } - else if ((sa[i].gvcid_blk.tfvn == tfvn) && (sa[i].gvcid_blk.scid == scid) && + *status = CRYPTO_LIB_ERR_INVALID_SCID; + } + *i_p = i; +} + +void sa_mismatched_vcid(int* i_p, int32_t* status, uint8_t tfvn, uint16_t scid, uint16_t vcid, uint8_t mapid) +{ + int i = *i_p; + if ((sa[i].gvcid_blk.tfvn == tfvn) && (sa[i].gvcid_blk.scid == scid) && (sa[i].gvcid_blk.vcid != vcid) && (sa[i].gvcid_blk.mapid == mapid && sa[i].sa_state == SA_OPERATIONAL)) - { + { #ifdef SA_DEBUG - printf(KRED "An operational SA was found - but mismatched vcid.\n" RESET); - printf(KRED "SA is %d\n", i); + printf(KRED "An operational SA was found - but mismatched vcid.\n" RESET); + printf(KRED "SA is %d\n", i); #endif - status = CRYPTO_LIB_ERR_INVALID_VCID; - } - else if ((sa[i].gvcid_blk.tfvn == tfvn) && (sa[i].gvcid_blk.scid == scid) && + *status = CRYPTO_LIB_ERR_INVALID_VCID; + } + *i_p = i; +} + +void sa_mismatched_mapid(int* i_p, int32_t* status, uint8_t tfvn, uint16_t scid, uint16_t vcid, uint8_t mapid) +{ + int i = *i_p; + if ((sa[i].gvcid_blk.tfvn == tfvn) && (sa[i].gvcid_blk.scid == scid) && (sa[i].gvcid_blk.vcid == vcid) && (sa[i].gvcid_blk.mapid != mapid && sa[i].sa_state == SA_OPERATIONAL)) - { + { #ifdef SA_DEBUG - printf(KRED "An operational SA was found - but mismatched mapid.\n" RESET); + printf(KRED "An operational SA was found - but mismatched mapid.\n" RESET); #endif - status = CRYPTO_LIB_ERR_INVALID_MAPID; - } - else if ((sa[i].gvcid_blk.tfvn == tfvn) && (sa[i].gvcid_blk.scid == scid) && - (sa[i].gvcid_blk.vcid == vcid) && - (sa[i].gvcid_blk.mapid == mapid && sa[i].sa_state != SA_OPERATIONAL)) - { + *status = CRYPTO_LIB_ERR_INVALID_MAPID; + } + *i_p = i; +} + +void sa_non_operational_sa(int* i_p, int32_t* status, uint8_t tfvn, uint16_t scid, uint16_t vcid, uint8_t mapid) +{ + int i = *i_p; + if ((sa[i].gvcid_blk.tfvn == tfvn) && (sa[i].gvcid_blk.scid == scid) && + (sa[i].gvcid_blk.vcid == vcid) && + (sa[i].gvcid_blk.mapid == mapid && sa[i].sa_state != SA_OPERATIONAL)) + { +#ifdef SA_DEBUG + printf(KRED "A valid but non-operational SA was found: SPI: %d.\n" RESET, sa[i].spi); +#endif + *status = CRYPTO_LIB_ERR_NO_OPERATIONAL_SA; + } + *i_p = i; +} + +void sa_debug_block(uint8_t tfvn, uint16_t scid, uint16_t vcid, uint8_t mapid) +{ +// Detailed debug block +#ifdef SA_DEBUG + printf(KYEL "Incoming frame parameters:\n" RESET); + printf(KYEL "\ttfvn %02X\n" RESET, tfvn); + printf(KYEL "\tscid %d\n" RESET, scid); + printf(KYEL "\tvcid %d\n" RESET, vcid); + printf(KYEL "\tmapid %02X\n" RESET, mapid); +#endif +// Ignore Unused Variables + (void) tfvn; + (void) scid; + (void) vcid; + (void) mapid; +} + +int32_t sa_get_operational_sa_from_gvcid_generate_error(int32_t* status, uint8_t tfvn, uint16_t scid, uint16_t vcid, uint8_t mapid) +{ + int i = 0; + + if (*status != CRYPTO_LIB_SUCCESS) + { #ifdef SA_DEBUG - printf(KRED "A valid but non-operational SA was found: SPI: %d.\n" RESET, sa[i].spi); + printf(KRED "Error - Making best attempt at a useful error code:\n\t" RESET); #endif - status = CRYPTO_LIB_ERR_NO_OPERATIONAL_SA; + for (i = 0; i < NUM_SA; i++) + { + // Could possibly have more than one field mismatched, + // ordering so the 'most accurate' SA's error is returned + // (determined by matching header fields L to R) + sa_mismatched_tfvn_error(&i, status, tfvn, scid, vcid, mapid); + if(status != CRYPTO_LIB_SUCCESS) + { + sa_debug_block(tfvn, scid, vcid, mapid); + return *status; } - else + sa_mismatched_scid(&i, status, tfvn, scid, vcid, mapid); + if(status != CRYPTO_LIB_SUCCESS) { - // Don't set status, could overwrite useful error message above + sa_debug_block(tfvn, scid, vcid, mapid); + return *status; + } + sa_mismatched_vcid(&i, status, tfvn, scid, vcid, mapid); + if(status != CRYPTO_LIB_SUCCESS) + { + sa_debug_block(tfvn, scid, vcid, mapid); + return *status; + } + sa_mismatched_mapid(&i, status, tfvn, scid, vcid, mapid); + if(status != CRYPTO_LIB_SUCCESS) + { + sa_debug_block(tfvn, scid, vcid, mapid); + return *status; + } + sa_non_operational_sa(&i, status, tfvn, scid, vcid, mapid); + if(status != CRYPTO_LIB_SUCCESS) + { + sa_debug_block(tfvn, scid, vcid, mapid); + return *status; } } - // Detailed debug block -#ifdef SA_DEBUG - printf(KYEL "Incoming frame parameters:\n" RESET); - printf(KYEL "\ttfvn %02X\n" RESET, tfvn); - printf(KYEL "\tscid %d\n" RESET, scid); - printf(KYEL "\tvcid %d\n" RESET, vcid); - printf(KYEL "\tmapid %02X\n" RESET, mapid); -#endif } + return *status; +} + +/** + * @brief Function: sa_get_operational_sa_from_gvcid + * @param tfvn: uint8 + * @param scid: uint16 + * @param vcid: uint16 + * @param mapid: uint8 // tc only + * @param security_association: SecurityAssociation_t** + * @return int32: Success/Failure + **/ +static int32_t sa_get_operational_sa_from_gvcid(uint8_t tfvn, uint16_t scid, uint16_t vcid, uint8_t mapid, + SecurityAssociation_t** security_association) +{ + int32_t status = CRYPTO_LIB_ERR_NO_OPERATIONAL_SA; + if (sa == NULL) + { + return CRYPTO_LIB_ERR_NO_INIT; + } + + status = sa_get_operational_sa_from_gvcid_find_iv(tfvn, scid, vcid, mapid, security_association); + + // If not a success, attempt to generate a meaningful error code + status = sa_get_operational_sa_from_gvcid_generate_error(&status, tfvn, scid, vcid, mapid); return status; } diff --git a/support/standalone/standalone.c b/support/standalone/standalone.c index 32d905d2..b7db0f82 100644 --- a/support/standalone/standalone.c +++ b/support/standalone/standalone.c @@ -428,6 +428,101 @@ void crypto_standalone_tm_frame(uint8_t* in_data, uint16_t in_length, uint8_t* o memcpy(out_data, &in_data[header_length], in_length - header_length - trailer_length); } +void crypto_standalone_tm_debug_recv(int32_t status, int tm_process_len, uint8_t* tm_process_in) +{ + if (tm_debug == 1) + { + printf("crypto_standalone_tm_process - received[%d]: 0x", tm_process_len); + for (int i = 0; i < status; i++) + { + printf("%02x", tm_process_in[i]); + } + printf("\n"); + } +} + +void crypto_standalone_tm_debug_process(uint8_t* tm_process_in) +{ + if (tm_debug == 1) + { + printf("Printing first bytes of Tf Pri Hdr:\n\t"); + for (int i = 0; i < 6; i++) + { + printf("%02X", *(tm_process_in + 4 + i)); + } + printf("\n"); + printf("Processing frame WITH ASM...\n"); + } +} + +void crypto_standalone_spp_telem_or_idle(int32_t* status_p, uint8_t* tm_ptr, uint16_t* spp_len_p, udp_info_t* tm_sock, struct sockaddr_in fwd_addr, int* tm_process_len_p) +{ + int32_t status = *status_p; + uint16_t spp_len = *spp_len_p; + int tm_process_len = *tm_process_len_p; + + if ((tm_ptr[0] == 0x08) || ((tm_ptr[0] == 0x03) && tm_ptr[1] == 0xff)) + { + spp_len = (((0xFFFF & tm_ptr[4]) << 8) | tm_ptr[5]) + 7; +#ifdef CRYPTO_STANDALONE_TM_PROCESS_DEBUG + printf("crypto_standalone_tm_process - SPP[%d]: 0x", spp_len); + for (int i = 0; i < spp_len; i++) + { + printf("%02x", tm_ptr[i]); + } + printf("\n"); +#endif + // Send all SPP telemetry packets + if (tm_ptr[0] == 0x08) + { + status = sendto(tm_sock->sockfd, tm_ptr, spp_len, 0, (struct sockaddr*)&fwd_addr, sizeof(fwd_addr)); + } + // Only send idle packets if configured to do so + else + { +#ifdef CRYPTO_STANDALONE_DISCARD_IDLE_PACKETS + // Don't forward idle packets + status = spp_len; +#else + status = sendto(tm_sock->sockfd, tm_ptr, spp_len, 0, (struct sockaddr*)&fwd_addr, sizeof(fwd_addr)); +#endif + } + + // Check status + if ((status == -1) || (status != spp_len)) + { + printf("crypto_standalone_tm_process - Reply error %d \n", status); + } + tm_ptr = &tm_ptr[spp_len]; + tm_process_len = tm_process_len - spp_len; + } + else if (tm_ptr[0] == 0xff && tm_ptr[1] == 0x48) + { + // Idle Frame + // Idle Frame is entire length of remaining data +#ifdef CRYPTO_STANDALONE_DISCARD_IDLE_FRAMES + // Don't forward idle frame + status = spp_len; +#else + status = sendto(tm_sock->sockfd, tm_ptr, tm_process_len, 0, (struct sockaddr*)&fwd_addr, sizeof(fwd_addr)); + if ((status == -1) || (status != spp_len)) + { + printf("crypto_standalone_tm_process - Reply error %d \n", status); + } + tm_ptr = &tm_ptr[spp_len]; +#endif + tm_process_len = 0; + } + else + { + printf("crypto_standalone_tm_process - SPP loop error, expected idle packet or frame! tm_ptr = 0x%02x%02x \n", tm_ptr[0], tm_ptr[1]); + tm_process_len = 0; + } + *status_p = status; + *spp_len_p = spp_len; + *tm_process_len_p = tm_process_len; +} + void* crypto_standalone_tm_process(void* sock) { int32_t status = CRYPTO_LIB_SUCCESS; @@ -459,29 +554,13 @@ void* crypto_standalone_tm_process(void* sock) if (status != -1) { tm_process_len = status; - if (tm_debug == 1) - { - printf("crypto_standalone_tm_process - received[%d]: 0x", tm_process_len); - for (int i = 0; i < status; i++) - { - printf("%02x", tm_process_in[i]); - } - printf("\n"); - } + /* Recieve */ + crypto_standalone_tm_debug_recv(status, tm_process_len, tm_process_in); /* Process */ #ifdef TM_CADU_HAS_ASM // Process Security skipping prepended ASM - if (tm_debug == 1) - { - printf("Printing first bytes of Tf Pri Hdr:\n\t"); - for (int i = 0; i < 6; i++) - { - printf("%02X", *(tm_process_in + 4 + i)); - } - printf("\n"); - printf("Processing frame WITH ASM...\n"); - } - // Account for ASM length + crypto_standalone_tm_debug_process(tm_process_in); + status = Crypto_TM_ProcessSecurity(tm_process_in + 4, (const uint16_t)tm_process_len - 4, &tm_ptr, &tm_out_len); #else if (tm_debug == 1) @@ -540,63 +619,8 @@ void* crypto_standalone_tm_process(void* sock) while (tm_process_len > 5) { // SPP Telemetry OR SPP Idle Packet - if ((tm_ptr[0] == 0x08) || ((tm_ptr[0] == 0x03) && tm_ptr[1] == 0xff)) - { - spp_len = (((0xFFFF & tm_ptr[4]) << 8) | tm_ptr[5]) + 7; -#ifdef CRYPTO_STANDALONE_TM_PROCESS_DEBUG - printf("crypto_standalone_tm_process - SPP[%d]: 0x", spp_len); - for (int i = 0; i < spp_len; i++) - { - printf("%02x", tm_ptr[i]); - } - printf("\n"); -#endif - // Send all SPP telemetry packets - if (tm_ptr[0] == 0x08) - { - status = sendto(tm_sock->sockfd, tm_ptr, spp_len, 0, (struct sockaddr*)&fwd_addr, sizeof(fwd_addr)); - } - // Only send idle packets if configured to do so - else - { -#ifdef CRYPTO_STANDALONE_DISCARD_IDLE_PACKETS - // Don't forward idle packets - status = spp_len; -#else - status = sendto(tm_sock->sockfd, tm_ptr, spp_len, 0, (struct sockaddr*)&fwd_addr, sizeof(fwd_addr)); -#endif - } + crypto_standalone_spp_telem_or_idle(&status, tm_ptr, &spp_len, tm_sock, fwd_addr, &tm_process_len); - // Check status - if ((status == -1) || (status != spp_len)) - { - printf("crypto_standalone_tm_process - Reply error %d \n", status); - } - tm_ptr = &tm_ptr[spp_len]; - tm_process_len = tm_process_len - spp_len; - } - else if (tm_ptr[0] == 0xff && tm_ptr[1] == 0x48) - { - // Idle Frame - // Idle Frame is entire length of remaining data -#ifdef CRYPTO_STANDALONE_DISCARD_IDLE_FRAMES - // Don't forward idle frame - status = spp_len; -#else - status = sendto(tm_sock->sockfd, tm_ptr, tm_process_len, 0, (struct sockaddr*)&fwd_addr, sizeof(fwd_addr)); - if ((status == -1) || (status != spp_len)) - { - printf("crypto_standalone_tm_process - Reply error %d \n", status); - } - tm_ptr = &tm_ptr[spp_len]; -#endif - tm_process_len = 0; - } - else - { - printf("crypto_standalone_tm_process - SPP loop error, expected idle packet or frame! tm_ptr = 0x%02x%02x \n", tm_ptr[0], tm_ptr[1]); - tm_process_len = 0; - } } } else