#include "SCP03_rspParser.h"

#include "tz_log.h"



SCPSTATUS verifyRMAC(uint8_t* MCV, uint8_t* S_RMAC, p_secEse_7816_rpdu_t pRpdu) {

    uint8_t R_MAC[8] = {0,};

    uint8_t parsed_R_MAC[8] = {0,};

    int i;



    if (pRpdu->len < 8) {

        LOGE("Invaild Response Length");

        return SCP_RESPONSE_LENGTH_INVALID;

    }



    genApduR_MAC(S_RMAC, MCV, pRpdu, R_MAC); // calculate R-MAC

    memcpy(parsed_R_MAC, pRpdu->pdata + (pRpdu->len-8), 8);

    for (i = 0 ; i < 8 ; i++) { // compare calulated R-MAC and received R-MAC

        if (R_MAC[i] != parsed_R_MAC[i]) {

            return SCP_VERIFY_RMAC_FAIL;

        }

    }



    return SCP_SUCCESS;

}



void decryptRspDataField(uint8_t* S_ENC, int encryptionCounter, p_secEse_7816_rpdu_t pRpdu) {

    uint8_t encrypted_data[MAX_RSP_CIPHERED_TEXT_SIZE] = {0,};

    uint8_t plain_text[MAX_RSP_CIPHERED_TEXT_SIZE] = {0,};

    uint8_t ICV_RSP[16] = {0,};

    int plain_text_length = 0;



    if (pRpdu->len == 0 || pRpdu->len > 240 || (((pRpdu->len) % 16) != 0) ) {

        LOGE("Invaild Response Length");

        return;

    }

    memcpy(encrypted_data, pRpdu->pdata, pRpdu->len);

    genICV(ICV_RSP, S_ENC, encryptionCounter, ICVRSP); // generate ICV for decryption

    scp03_aes_decrypt (S_ENC, encrypted_data, pRpdu->len, plain_text, &plain_text_length, ICV_RSP, PADDING); // decrypt response data

    pRpdu->len = plain_text_length;

    memset(pRpdu->pdata, 0x00, MAX_RSP_CIPHERED_TEXT_SIZE);

    memcpy(pRpdu->pdata, plain_text, plain_text_length);

}