#include "SCP03_tools.h"
#include "dk_log.h"
#include <TZ_Vendor_tl.h>
#define LOGE DK_LOG_ERR

SCPSTATUS verifyCardChallenge(uint8_t* Key_ENC, uint8_t* cardChallenge, uint8_t* sequenceCounter, uint8_t* AID, int AIDLength) {
    int i;
    uint8_t cCardChallenge[16] = {0,};

    genCardChallenge(Key_ENC, sequenceCounter, AID, AIDLength, cCardChallenge);
    for (i = 0 ; i < 8 ; i++) {
        if (cardChallenge[i] != cCardChallenge[i]) {
            return SCP_VERIFY_CARD_CHALLENGE_FAIL;
        }

    }
    return SCP_SUCCESS;
}

SCPSTATUS verifyCardCryptogram(uint8_t* S_MAC, uint8_t* cardCryptogram, uint8_t* hostChallenge, uint8_t* cardChallenge) {
    int i;
    uint8_t cCardCryptogram[16] = {0,};

    genCardCryptogram(S_MAC, hostChallenge, cardChallenge, cCardCryptogram);
    for (i = 0 ; i < 8 ; i++) {
        if (cardCryptogram[i] != cCardCryptogram[i]) {
            return SCP_VERIFY_CARD_CRYPTOGRAM_FAIL;
        }
    }

    return SCP_SUCCESS;
}

void genFinalApdu(p_secEse_7816_cpdu_t pCpdu, uint8_t* C_MAC) {
    memcpy(pCpdu->pdata + pCpdu->lc, C_MAC, 8);
    pCpdu->lc = pCpdu->lc+8;
}

void genICV(uint8_t* ICV, uint8_t* S_ENC, int encryptionCounter, int ICV_type) {
    uint8_t inputData[16] = {0,};
    uint8_t IV[16] = {0,};

    inputData[15] = encryptionCounter & 0x000000ff;
    inputData[14] = (encryptionCounter>>8) & 0x000000ff;
    inputData[13] = (encryptionCounter>>16) & 0x000000ff;
    inputData[12] = (encryptionCounter>>24) & 0x000000ff;

    if (ICV_type == ICVRSP) {
        inputData[0] = 0x80;
    }

    scp03_aes_encrypt (S_ENC, inputData, 16, ICV, IV, NO_PADDING);
}

void genEncSensitiveData(uint8_t* Key_DEK, uint8_t* inputData, int inputDataLength, uint8_t* outputData) {
    uint8_t ICV[16] = {0,};

    if ((inputDataLength%16) != 0) {
        scp03_aes_encrypt (Key_DEK, inputData, inputDataLength, outputData, ICV, PADDING);
    } else {
        scp03_aes_encrypt (Key_DEK, inputData, inputDataLength, outputData, ICV, NO_PADDING);
    }
}

void genApduC_MAC(uint8_t* S_MAC, uint8_t* MCV, p_secEse_7816_cpdu_t pCpdu, uint8_t* C_MAC) {
    uint8_t outCMAC[16];
    int cmdBufferSize = 21 + pCpdu->lc;

    uint8_t* cmdBuffer = NULL;
    cmdBuffer = (uint8_t*) TZ_malloc (cmdBufferSize);
    if ( NULL == cmdBuffer )
    {
        LOGE("memory is not allocated");
        return;
    }

    memcpy(cmdBuffer, MCV, 16);
    cmdBuffer[16] = pCpdu->cla;
    cmdBuffer[17] = pCpdu->ins;
    cmdBuffer[18] = pCpdu->p1;
    cmdBuffer[19] = pCpdu->p2;
    cmdBuffer[20] = pCpdu->lc+8;
    memcpy(cmdBuffer+21, pCpdu->pdata, pCpdu->lc);

    SCP03_AES_CMAC (S_MAC, cmdBuffer, cmdBufferSize, outCMAC);

    memcpy(C_MAC, outCMAC, 8);
    memcpy(MCV, outCMAC, 16);

    //pCpdu->lc = pCpdu->lc + 8;
    TZ_free(cmdBuffer);
}

void genApduR_MAC(uint8_t* S_RMAC, uint8_t* MCV, p_secEse_7816_rpdu_t pRpdu, uint8_t* R_MAC) {
    uint8_t outCMAC[16];
    uint16_t ciphered_RDF_Length;
    int rspBufferSize;
    uint8_t* rspBuffer = NULL;

    if ((pRpdu->len-8) < 0) {
        LOGE("Invaild Response Length");
        return;
    }

    ciphered_RDF_Length = pRpdu->len-8;
    rspBufferSize = 16 + ciphered_RDF_Length + 2;

    rspBuffer = (uint8_t*) TZ_malloc (rspBufferSize);
    if ( NULL == rspBuffer )
    {
        LOGE("memory is not allocated");
        return;
    }

    memcpy(rspBuffer, MCV, 16);
    memcpy(rspBuffer+16, pRpdu->pdata, ciphered_RDF_Length);
    memcpy(rspBuffer+16+ciphered_RDF_Length, &pRpdu->sw1, 1);
    memcpy(rspBuffer+16+ciphered_RDF_Length+1, &pRpdu->sw2, 1);

    SCP03_AES_CMAC (S_RMAC, rspBuffer, rspBufferSize, outCMAC);
    memcpy(R_MAC, outCMAC, 8);

    //pCpdu->lc = pCpdu->lc + 8;
    TZ_free(rspBuffer);
}