#include "GPCmd.h"

#include "sec_apdu.h"

#include "SCP03_transceive.h"

#include "SCP03_crypto.h"

#include "tz_log.h"



void genKeyDataField(uint8_t* keyDataField, uint8_t* key, int keyLength) {

	uint8_t encrypted_key[KEY_LEN_BYTE] = {0,};

	uint8_t inputData[16] = {0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01};

	uint8_t encryptedInputData[16] = {0,};

	uint8_t iv[16] = {0,};

	//uint8_t keyCheckValue[3] = {0,};



	requestSensitiveDataEncryption(key, keyLength, encrypted_key);



	keyDataField[0] = 0x88;

	keyDataField[1] = KEY_LEN_BYTE+1;

	keyDataField[2] = KEY_LEN_BYTE;

	memcpy(keyDataField+3, encrypted_key, KEY_LEN_BYTE);

	keyDataField[3+KEY_LEN_BYTE] = 0x03;



	scp03_aes_encrypt (key, inputData, 16, encryptedInputData, iv, NO_PADDING);



	memcpy(keyDataField+3+KEY_LEN_BYTE+1,encryptedInputData,3);

}



SCPSTATUS sendPutKeyCmd(uint8_t channelId, uint8_t kvn, uint8_t* key_enc, uint8_t* key_mac, uint8_t* key_dek, uint8_t add) {

    SCPSTATUS status = 0;

	secEse_7816_cpdu_t cpdu;

	secEse_7816_rpdu_t rpdu;

	int keyDataFieldLength = 7 + KEY_LEN_BYTE;



	uint8_t CPDU_DATA[MAX_PLAIN_TEXT_SIZE] = {0,};

	uint8_t RPDU_DATA[MAX_RSP_CIPHERED_TEXT_SIZE] = {0,};



	uint8_t KeyENCDataField [7 + KEY_LEN_BYTE];

	uint8_t KeyMACDataField [7 + KEY_LEN_BYTE];

	uint8_t KeyDEKDataField [7 + KEY_LEN_BYTE];



	memset(&cpdu,0, sizeof(secEse_7816_cpdu_t));

	memset(&rpdu,0, sizeof(secEse_7816_rpdu_t));



	cpdu.cla = 0x80;

	cpdu.cpdu_type = 0x00;

	cpdu.ins = 0xD8;

	cpdu.lc = 1+ (keyDataFieldLength*3);

	cpdu.le = 0x00;

	cpdu.le_type = 0x01;

    if (add == 1) {

        cpdu.p1 = 0x00;

    } else {

	    cpdu.p1 = kvn;

    }

	cpdu.p2 = 0x81;

	cpdu.pdata = CPDU_DATA;

	rpdu.pdata = RPDU_DATA;



	CPDU_DATA[0] = kvn;



	genKeyDataField(KeyENCDataField, key_enc, KEY_LEN_BYTE);

	genKeyDataField(KeyMACDataField, key_mac, KEY_LEN_BYTE);

	genKeyDataField(KeyDEKDataField, key_dek, KEY_LEN_BYTE);



	memcpy(CPDU_DATA+1, KeyENCDataField, keyDataFieldLength);

	memcpy(CPDU_DATA+1+keyDataFieldLength, KeyMACDataField, keyDataFieldLength);

	memcpy(CPDU_DATA+1+keyDataFieldLength+keyDataFieldLength, KeyDEKDataField, keyDataFieldLength);



    status = apduTransceive (channelId, &cpdu, &rpdu);

    if (status != SCP_SUCCESS) {

        LOGE("sendPutKeyCmd() apduTransceive Failed. sw1-sw2 : 0x%x%x",rpdu.sw1, rpdu.sw2);

    }



	if (rpdu.sw1 == 0x90 && rpdu.sw2 == 00) {

		return SCP_SUCCESS;

	} else if (rpdu.sw1 == 0x65 && rpdu.sw2 == 0x81) {

		return SCP_PUTKEY_MEMORY_FAILURE;

	} else if (rpdu.sw1 == 0x6A && rpdu.sw2 == 0x84) {

	    return SCP_NOT_ENOUGH_MEMORY;

	} else if (rpdu.sw1 == 0x6A && rpdu.sw2 == 0x88) {

		return SCP_REFERENCE_DATA_NOT_FOUND;

	} else if (rpdu.sw1 == 0x94 && rpdu.sw2 == 0x84) {

		return SCP_PUTKEY_ALGO_NOT_SUPPORT;

	} else if (rpdu.sw1 == 0x94 && rpdu.sw2 == 0x85) {

		return SCP_PUTKEY_INVALID_KEY_CHECK_VALUE;

	} else {

        LOGE("sendPutKeyCmd() apduTransceive Failed. sw1-sw2 : 0x%x%x",rpdu.sw1, rpdu.sw2);

		return SCP_FAIL;

	}

}



SCPSTATUS sendStoreDataCmd(uint8_t channelId, uint8_t p1, uint8_t p2, uint8_t dataLength, uint8_t* data, uint8_t* responseData) {

    SCPSTATUS status = 0;

	secEse_7816_cpdu_t cpdu;

	secEse_7816_rpdu_t rpdu;



	uint8_t CPDU_DATA[MAX_PLAIN_TEXT_SIZE] = {0,};

	uint8_t RPDU_DATA[MAX_RSP_CIPHERED_TEXT_SIZE] = {0,};



	memset(&cpdu,0, sizeof(secEse_7816_cpdu_t));

	memset(&rpdu,0, sizeof(secEse_7816_rpdu_t));



	cpdu.cla = 0x80;

	cpdu.cpdu_type = 0x00;

	cpdu.ins = 0xE2;

	cpdu.lc = dataLength;

	cpdu.le = 0x00;

	cpdu.le_type = 0x01;

	cpdu.p1 = p1;

	cpdu.p2 = p2;



	memcpy(CPDU_DATA, data, dataLength);

	cpdu.pdata = CPDU_DATA;

	rpdu.pdata = RPDU_DATA;



	status = apduTransceive (channelId, &cpdu, &rpdu);

    if (status != SCP_SUCCESS) {

        LOGE("sendStoreDataCmd() apduTransceive Failed. sw1-sw2 : 0x%x%x", rpdu.sw1, rpdu.sw2);

    }



	if (rpdu.sw1 == 0x90 && rpdu.sw2 == 00) {

	    if (rpdu.len > 0 && rpdu.len <= MAX_RSP_CIPHERED_TEXT_SIZE) {

    		memcpy(responseData, RPDU_DATA, rpdu.len);

	    }

	    return SCP_SUCCESS;

	} else if ((rpdu.sw1 == 0x6A && rpdu.sw2 == 0x89) ||

	        (rpdu.sw1 == 0x63 && rpdu.sw2 == 0x81) ) {

		return SCP_FILE_ALREADY_EXIST;

	} else if (rpdu.sw1 == 0x6A && rpdu.sw2 == 0x80) {

		return SCP_INCORRECT_VALUE;

	} else if (rpdu.sw1 == 0x6A && rpdu.sw2 == 0x84) {

		return SCP_NOT_ENOUGH_MEMORY;

	} else if (rpdu.sw1 == 0x6A && rpdu.sw2 == 0x88) {

		return SCP_REFERENCE_DATA_NOT_FOUND;

	} else {

        LOGE("sendStoreDataCmd() apduTransceive Failed. sw1-sw2 : 0x%x%x",rpdu.sw1, rpdu.sw2);

		return SCP_FAIL;

	}

}