diff --git a/src/core/crypto.c b/src/core/crypto.c index 0821a5e9..7149e23f 100644 --- a/src/core/crypto.c +++ b/src/core/crypto.c @@ -789,31 +789,31 @@ int32_t Crypto_Process_Extended_Procedure_Pdu(TC_t *tc_sdls_processed_frame, uin if (status == CRYPTO_LIB_SUCCESS) { - if (crypto_config.has_pus_hdr == TC_HAS_PUS_HDR) + // Check for speciic App ID for EPs - the CryptoLib Apid in this case + if ((tc_sdls_processed_frame->tc_pdu[0] == 0x18) && (tc_sdls_processed_frame->tc_pdu[1] == 0x80)) { - // Check for speciic App ID for EPs - the CryptoLib Apid in this case - if ((tc_sdls_processed_frame->tc_pdu[0] == 0x18) && (tc_sdls_processed_frame->tc_pdu[1] == 0x80)) - { #ifdef CRYPTO_EPROC - // Check validity of SAs used for EP - if(valid_ep_sa == CRYPTO_TRUE) - { + // Check validity of SAs used for EP + if(valid_ep_sa == CRYPTO_TRUE) + { #ifdef DEBUG - printf(KGRN "Received SDLS command: " RESET); + printf(KGRN "Received SDLS command: " RESET); #endif - // CCSDS Header - sdls_frame.hdr.pvn = (tc_sdls_processed_frame->tc_pdu[0] & 0xE0) >> 5; - sdls_frame.hdr.type = (tc_sdls_processed_frame->tc_pdu[0] & 0x10) >> 4; - sdls_frame.hdr.shdr = (tc_sdls_processed_frame->tc_pdu[0] & 0x08) >> 3; - sdls_frame.hdr.appID = - ((tc_sdls_processed_frame->tc_pdu[0] & 0x07) << 8) | tc_sdls_processed_frame->tc_pdu[1]; - sdls_frame.hdr.seq = (tc_sdls_processed_frame->tc_pdu[2] & 0xC0) >> 6; - sdls_frame.hdr.pktid = - ((tc_sdls_processed_frame->tc_pdu[2] & 0x3F) << 8) | tc_sdls_processed_frame->tc_pdu[3]; - sdls_frame.hdr.pkt_length = - (tc_sdls_processed_frame->tc_pdu[4] << 8) | tc_sdls_processed_frame->tc_pdu[5]; - - // CCSDS PUS + // CCSDS Header + sdls_frame.hdr.pvn = (tc_sdls_processed_frame->tc_pdu[0] & 0xE0) >> 5; + sdls_frame.hdr.type = (tc_sdls_processed_frame->tc_pdu[0] & 0x10) >> 4; + sdls_frame.hdr.shdr = (tc_sdls_processed_frame->tc_pdu[0] & 0x08) >> 3; + sdls_frame.hdr.appID = + ((tc_sdls_processed_frame->tc_pdu[0] & 0x07) << 8) | tc_sdls_processed_frame->tc_pdu[1]; + sdls_frame.hdr.seq = (tc_sdls_processed_frame->tc_pdu[2] & 0xC0) >> 6; + sdls_frame.hdr.pktid = + ((tc_sdls_processed_frame->tc_pdu[2] & 0x3F) << 8) | tc_sdls_processed_frame->tc_pdu[3]; + sdls_frame.hdr.pkt_length = + (tc_sdls_processed_frame->tc_pdu[4] << 8) | tc_sdls_processed_frame->tc_pdu[5]; + + if (crypto_config.has_pus_hdr == TC_HAS_PUS_HDR) + { + // If ECSS PUS Header is being used sdls_frame.pus.shf = (tc_sdls_processed_frame->tc_pdu[6] & 0x80) >> 7; sdls_frame.pus.pusv = (tc_sdls_processed_frame->tc_pdu[6] & 0x70) >> 4; sdls_frame.pus.ack = (tc_sdls_processed_frame->tc_pdu[6] & 0x0F); @@ -821,38 +821,38 @@ int32_t Crypto_Process_Extended_Procedure_Pdu(TC_t *tc_sdls_processed_frame, uin sdls_frame.pus.sst = tc_sdls_processed_frame->tc_pdu[8]; sdls_frame.pus.sid = (tc_sdls_processed_frame->tc_pdu[9] & 0xF0) >> 4; sdls_frame.pus.spare = (tc_sdls_processed_frame->tc_pdu[9] & 0x0F); - - // SDLS TLV PDU - sdls_frame.pdu.hdr.type = (tc_sdls_processed_frame->tc_pdu[10] & 0x80) >> 7; - sdls_frame.pdu.hdr.uf = (tc_sdls_processed_frame->tc_pdu[10] & 0x40) >> 6; - sdls_frame.pdu.hdr.sg = (tc_sdls_processed_frame->tc_pdu[10] & 0x30) >> 4; - sdls_frame.pdu.hdr.pid = (tc_sdls_processed_frame->tc_pdu[10] & 0x0F); - sdls_frame.pdu.hdr.pdu_len = - (tc_sdls_processed_frame->tc_pdu[11] << 8) | tc_sdls_processed_frame->tc_pdu[12]; - for (int x = 13; x < (13 + sdls_frame.hdr.pkt_length); x++) - { - sdls_frame.pdu.data[x - 13] = tc_sdls_processed_frame->tc_pdu[x]; - } - -#ifdef CCSDS_DEBUG - Crypto_ccsdsPrint(&sdls_frame); -#endif - - // Determine type of PDU - status = Crypto_PDU(ingest, tc_sdls_processed_frame); } - // Received EP PDU on invalid SA - else + + // SDLS TLV PDU + sdls_frame.pdu.hdr.type = (tc_sdls_processed_frame->tc_pdu[10] & 0x80) >> 7; + sdls_frame.pdu.hdr.uf = (tc_sdls_processed_frame->tc_pdu[10] & 0x40) >> 6; + sdls_frame.pdu.hdr.sg = (tc_sdls_processed_frame->tc_pdu[10] & 0x30) >> 4; + sdls_frame.pdu.hdr.pid = (tc_sdls_processed_frame->tc_pdu[10] & 0x0F); + sdls_frame.pdu.hdr.pdu_len = + (tc_sdls_processed_frame->tc_pdu[11] << 8) | tc_sdls_processed_frame->tc_pdu[12]; + for (int x = 13; x < (13 + sdls_frame.hdr.pkt_length); x++) { - printf(KRED "Received EP PDU on invalid SA! SPI %d\n" RESET, tc_sdls_processed_frame->tc_sec_header.spi); - status = CRYPTO_LIB_ERR_SDLS_EP_WRONG_SPI; + sdls_frame.pdu.data[x - 13] = tc_sdls_processed_frame->tc_pdu[x]; } - #else // Received an EP command without EPs being built - valid_ep_sa = valid_ep_sa; // Suppress build error - status = CRYPTO_LIB_ERR_SDLS_EP_NOT_BUILT; - #endif //CRYPTO_EPROC +#ifdef CCSDS_DEBUG + Crypto_ccsdsPrint(&sdls_frame); +#endif + + // Determine type of PDU + status = Crypto_PDU(ingest, tc_sdls_processed_frame); + } + // Received EP PDU on invalid SA + else + { + printf(KRED "Received EP PDU on invalid SA! SPI %d\n" RESET, tc_sdls_processed_frame->tc_sec_header.spi); + status = CRYPTO_LIB_ERR_SDLS_EP_WRONG_SPI; } + + #else // Received an EP command without EPs being built + valid_ep_sa = valid_ep_sa; // Suppress build error + status = CRYPTO_LIB_ERR_SDLS_EP_NOT_BUILT; + #endif //CRYPTO_EPROC } else if (tc_sdls_processed_frame->tc_header.vcid == TC_SDLS_EP_VCID) // TC SDLS PDU with no packet layer { @@ -892,11 +892,7 @@ int32_t Crypto_Process_Extended_Procedure_Pdu(TC_t *tc_sdls_processed_frame, uin status = CRYPTO_LIB_ERR_SDLS_EP_NOT_BUILT; #endif //CRYPTO_EPROC } - else - { - // TODO - Process SDLS PDU with Packet Layer without PUS_HDR - } - } + } return status; } // End Process SDLS PDU