#include "SCP03_transceive.h"
#include "SCP03_cmdGenerator.h"
#include "SCP03_tools.h"
#include "SCP03_rspParser.h"

#include "tz_debug.h"

static uint8_t Key_ENC[KEY_LEN_BYTE];
static uint8_t Key_MAC[KEY_LEN_BYTE];
static uint8_t Key_DEK[KEY_LEN_BYTE];

static uint8_t S_ENC[KEY_LEN_BYTE];
static uint8_t S_MAC[KEY_LEN_BYTE];
static uint8_t S_RMAC[KEY_LEN_BYTE];

static uint8_t MAC_Chaining_Value[16];

static uint8_t security_level;
static uint8_t security_level_status[4] = {0,};

static int encryptionCounter = 0;


SCPSTATUS openSession(uint8_t channelId, uint8_t kvn, uint8_t* KeyENC, uint8_t* KeyMAC, uint8_t* KeyDEK, uint8_t* AID, int AIDLength, uint8_t securityLevel) {
    uint8_t cpduData[APDU_DATA_SIZE_MAX] = {0,};
    uint8_t rpduData[MAX_RAPDU_DATA_SIZE] = {0,};
    uint8_t hostChallenge[8] = {0,};
    uint8_t cardChallenge[8] = {0,};
    uint8_t hostCryptogram[8] = {0,};
    uint8_t cardCryptogram[8] = {0,};
    uint8_t sequenceCounter[3] = {0,};
    ESESTATUS result = ESESTATUS_SUCCESS;
    SCPSTATUS verifyResult = SCP_SUCCESS;
    secEse_7816_cpdu_t cpdu;
    secEse_7816_rpdu_t rpdu;

    // parse security level
    security_level = securityLevel;
    security_level_status[0] = securityLevel & 0x01; // C-MAC
    security_level_status[1] = securityLevel & 0x02; // C-DECRYPTION
    security_level_status[2] = securityLevel & 0x10; // R-MAC
    security_level_status[3] = securityLevel & 0x20; // R-ENCRYPTION

    // copy root key
    memcpy (Key_ENC, KeyENC, KEY_LEN_BYTE);
    memcpy (Key_MAC, KeyMAC, KEY_LEN_BYTE);
    memcpy (Key_DEK, KeyDEK, KEY_LEN_BYTE);

    //save current logical channel
    //current_channelId = channelId;

    // init cpdu pdata for initialupdate
    cpdu.pdata = hostChallenge;
    hex_print_tag_debug ("Key_ENC", Key_ENC, KEY_LEN_BYTE);
    hex_print_tag_debug ("Key_MAC", Key_MAC, KEY_LEN_BYTE);
    hex_print_tag_debug ("Key_DEK", Key_DEK, KEY_LEN_BYTE);
    hex_print_tag_debug ("SL", security_level_status, 4);

    // get pdu datas for initialupdate
    verifyResult = getInitialUpdateCmd(&cpdu, kvn);
    if (verifyResult != SCP_SUCCESS) {
        LOGE("getInitialUpdateCmd Fail Error : %04x",verifyResult);
        return verifyResult;
    }

    // init rpdu pdata for initialupdate
    rpdu.pdata = rpduData;

    // send INITIALIZE UPDATE Command
    result = secEseTransmit(channelId, &cpdu, &rpdu);
    if (!(result == ESESTATUS_SUCCESS && (rpdu.sw1 == 0x90 && rpdu.sw2 == 0x00))) {
        LOGE("InitialUpdateCmd Fail");
        return SCP_INITIAL_UPDATE_FAIL;
    }
    // parse card Challenge from response PDU data field
    memcpy(cardChallenge, rpdu.pdata + 13, 8);
    hex_print_tag_debug ("cardChallenge", cardChallenge, 8);
    // parse card Cryptogram from response PDU data field
    memcpy(cardCryptogram, rpdu.pdata + 21, 8);
    hex_print_tag_debug ("cardCryptogram", cardCryptogram, 8);
    // parse sequence counter from response PDU data field
    memcpy(sequenceCounter, rpdu.pdata + 29, 3);
    hex_print_tag_debug ("sequenceCounter", sequenceCounter, 3);
    hex_print_tag_debug ("hostChallenge", hostChallenge, 8);

    // verify card challenge
    verifyResult = verifyCardChallenge(Key_ENC, cardChallenge, sequenceCounter, AID, AIDLength);
    if (verifyResult != SCP_SUCCESS) {
        LOGE("verifyCardChallenge Fail Error : %04x",verifyResult);
        return verifyResult;
    }
    // generate Session Key (S-MAC key)
    genSessionKey(Key_MAC, S_MAC, hostChallenge, cardChallenge, SMAC);
    hex_print_tag_debug ("S_MAC", S_MAC, KEY_LEN_BYTE);

    // verify card cryptogram
    verifyResult = verifyCardCryptogram(S_MAC, cardCryptogram, hostChallenge, cardChallenge);
    if (verifyResult != SCP_SUCCESS) {
        LOGE("verifyCardCryptogram Fail Error : %04x",verifyResult);
        return verifyResult;
    }

    // generate Host Cryptogram
    genHostCryptogram(S_MAC, hostCryptogram, hostChallenge, cardChallenge);
    hex_print_tag_debug ("hostCryptogram", hostCryptogram, 8);

    // init MAC chaining value & cpdu,rpdu
    memset(MAC_Chaining_Value, 0x00, 16);
    memset(&cpdu, 0x00, sizeof(secEse_7816_cpdu_t));
    memset(&rpdu, 0x00, sizeof(secEse_7816_rpdu_t));
    memset(rpduData, 0x00, MAX_RAPDU_DATA_SIZE);
    cpdu.pdata = cpduData;
    rpdu.pdata = rpduData;

    // generate EXTERNAL AUTHENTICATE Command
    getExternalAuthenticateCmd(S_MAC, &cpdu, security_level, hostCryptogram, MAC_Chaining_Value);
    hex_print_tag_debug ("ExternalAuthenticate cmd data", cpdu.pdata, 16);
    // send EXTERNAL AUTHENTICATE Command
    result = secEseTransmit(channelId, &cpdu, &rpdu);
    if (!(result == ESESTATUS_SUCCESS && (rpdu.sw1 == 0x90 && rpdu.sw2 == 0x00))) {
        LOGE("ExternalAuthenticate Fail");
        return SCP_EXTERNAL_AUTHENTICATE_FAIL;
    }

    // generate Session Key (S-ENC key)
    genSessionKey(Key_ENC, S_ENC, hostChallenge, cardChallenge, SENC);
    hex_print_tag_debug ("S_ENC", S_ENC, KEY_LEN_BYTE);

    // generate Session Key (S-RMAC key)
    genSessionKey(Key_MAC, S_RMAC, hostChallenge, cardChallenge, SRMAC);
    hex_print_tag_debug ("S_RMAC", S_RMAC, KEY_LEN_BYTE);

    encryptionCounter = 0;

    return SCP_SUCCESS;
}

SCPSTATUS apduTransceive (uint8_t channelId, p_secEse_7816_cpdu_t pCpdu, p_secEse_7816_rpdu_t pRpdu) {
    ESESTATUS result = ESESTATUS_SUCCESS;
    SCPSTATUS verifyResult = 0;
    uint8_t ICV[16] = {0,};
    uint8_t encrypted_data[MAX_CIPHERED_TEXT_SIZE] = {0,};

    if (!(security_level_status[0] == 0 &&
        security_level_status[1] == 0 &&
        security_level_status[2] == 0 &&
        security_level_status[3] == 0)) {
        // Global platform CLA for Secure messaging
        pCpdu->cla = pCpdu->cla | 0x04;
    }

    if ((security_level_status[1] & 0x02) == 0x02) { // C-DECRYPTION security level enabled and data is exist, remove LC != 0 check logic for FRA 20.06.17
        encryptionCounter = encryptionCounter+1;
        // generate ICV for data encryption
        genICV(ICV, S_ENC, encryptionCounter, ICVCMD);
    }

    // generate SCP03 command (encryption and MACing)
    getSCP03Cmd(pCpdu, encrypted_data, S_ENC, S_MAC, ICV, MAC_Chaining_Value, security_level_status);

    // transceive to ISO
    result = secEseTransmit(channelId, pCpdu, pRpdu);
    if ((pRpdu->sw1 == 0x90 && pRpdu->sw2 == 0x00) || pRpdu->sw1 == 0x62 || pRpdu->sw1 == 0x63) { // status byte is 9000 or 62xx or 63xx
        if ((security_level_status[2] & 0x10) == 0x10) { // RMAC shall exist
            // verify R-MAC
            verifyResult = verifyRMAC(MAC_Chaining_Value, S_RMAC, pRpdu);
            if (verifyResult != SCP_SUCCESS) {
                LOGE("VERIFY RMAC FAILED");
                return verifyResult;
            } else {
                LOGD("VERIFY RMAC OK");
            }
            pRpdu->len -= 8;
        }
        if (((security_level_status[3] & 0x20) == 0x20) && (pRpdu->len != 0)) { // R-ENCRYPTION security level enabled and response data is exist
            decryptRspDataField(S_ENC, encryptionCounter, pRpdu); // decrypt Data field
        }
    }

    return result;
}

void requestSensitiveDataEncryption(uint8_t* sensitiveData, int dataLength, uint8_t* encrypted_data) {
    genEncSensitiveData(Key_DEK, sensitiveData, dataLength, encrypted_data);
    return;
}