/*
 * srv_cyber.c
 *
 *  Created on: 23 mai 2022
 *      Author:
 */
#include "srv_cyber.h"
#include "fsl_prince.h"
#include "fsl_debug_console.h"
//---------------------------------------------
// Define, macro
//---------------------------------------------


//---------------------------------------------
// Enum, struct, union
//---------------------------------------------
typedef uint8_t keyCode_t[PUF_GET_KEY_CODE_SIZE_FOR_KEY_SIZE(kPUF_KeySizeMax)];


//---------------------------------------------
// Type
//---------------------------------------------

//---------------------------------------------
// Const, var
//---------------------------------------------

flash_config_t	flashInstance = {0};
keyCode_t 		keyCode = {0};
puf_config_t	pufConfig = {0};
//---------------------------------------------
// Functions
//---------------------------------------------


bool GetKey(void);
uint32_t KeyCodeCheck(uint8_t * keycode, uint32_t * keytype, uint32_t * keyidx, uint32_t * keysize);
void LoadKeyCode(uint8_t ** keycode);
void StartPuf(void);
void StopPuf(void);

uint8_t 		activationCode[PUF_ACTIVATION_CODE_SIZE];
void StartPuf(void)
{
	uint8_t * 		pac;
	status_t 		status;

	PUF_GetDefaultConfig(&pufConfig);
	memset((void *)activationCode, 0, sizeof(activationCode));

	status = FFR_KeystoreGetAC(&flashInstance, (uint8_t *)activationCode);
	if (status != kStatus_Success)
	{
		//PRINTF("\r\nRead AC from CMPS failed\r\n");
		return;
	}
	pac = (uint8_t *)activationCode;

	/* Reinitialize PUF after enroll */
	status = PUF_Init(PUF, &pufConfig);
	if (status != kStatus_Success)
	{
		//PRINTF("\nError Initializing PUF!\r\n");
		return;
	}

	/* Start PUF by loading generated activation code */
	status = PUF_Start(PUF, pac, sizeof(activationCode));
	if (status != kStatus_Success)
	{
		//PRINTF("\nError during Start !\r\n");
		return;
	}
	return;
}

void StopPuf(void)
{
	PUF_Deinit(PUF, &pufConfig);
}

void LoadKeyCode(uint8_t ** keycode)
{
	uint32_t keyIdx = 0;// 0 <=> SBKEK ; 1<=>USERKEK
	status_t status;


	status = FFR_KeystoreGetKC(&flashInstance, keyCode, (ffr_key_type_t)keyIdx);
	if(status == kStatus_Success)
	{
		*keycode = keyCode;
		//PRINTF("\r\nGet Key Code successful\r\n");
	}
	else
	{
		//PRINTF("\r\nGet Key Code failed\r\n");
	}

	return;
}

uint32_t KeyCodeCheck(uint8_t * keycode, uint32_t * keytype, uint32_t * keyidx, uint32_t * keysize)
{
    *keytype = keycode[0];
    *keyidx = keycode[1];
    *keysize = keycode[3] == 0 ? 512 : 8 * keycode[3] ;

    if (keycode[0] >= 2)
    {
      return 1;
    }
    if (keycode[1] >= 16)
    {
    	return 2;
    }

    if (keycode[3] >= 64)
    {
    	return 3;
    }

    return 0;
}


bool GetKey(void)
{
    uint32_t	keyidx, keyslot, keycodesize, keysize, keytype, status;
    uint8_t *	keycode = NULL;
    status_t	result;

	LoadKeyCode(&keycode);
	if(keycode == NULL)
	{
		//PRINTF("\r\nError loading code");
		return false;
	}

	status = KeyCodeCheck(keycode, &keytype, &keyidx, &keysize);
	if(status != kStatus_Success)
	{
		//PRINTF("\r\nError in Key Code header");
		return false;
	}

	keycodesize = PUF_GET_KEY_CODE_SIZE_FOR_KEY_SIZE(keysize);

	keyslot = kPUF_KeySlot0; //send to AES ({1,2,3} send to PRINCE)
	result = PUF_GetHwKey(PUF, keycode, keycodesize, (puf_key_slot_t)keyslot, rand());
	if (result != kStatus_Success)
	{
		//PRINTF("\r\nError reconstructing key to HW bus!\r\n");
		return false;
	}
	else
	{
		return true;
	}
}




/*!
 * @fn void CYBER_Init (void)
 * @brief Initialize internal module
 */
void CYBER_Init(void)
{
	memset(&flashInstance, 0, sizeof(flash_config_t));
	FLASH_Init(&flashInstance);
	FFR_Init(&flashInstance);
//	HASHCRYPT_Init(HASHCRYPT);	//AGR test to init it before use PUF to send HW_KEY
}



/*!
 * @fn void CYBER_EncrypUsingHWUserKEK (void)
 * @brief Encrypt plaintext using AES ECB
 * @param plaintext data to encrypt
 * @param ciphertext encrypted data
 * @param size plaintext size
 * @warning size must be a multiple of 16bytes
 * @return true if OK, else false
 */
bool CYBER_EncrypUsingHWUserKEK(uint8_t *plaintext, uint8_t *ciphertext, size_t size)
{
	uint8_t keyAes[32]={0x2D , 0x65 , 0xA4 , 0xEA , 0x63 , 0x2F , 0xE3 , 0x98 , 0x1B , 0x87 , 0xB4 , 0xCB , 0x16 , 0x08 , 0xE5 , 0x49 , 0x17 , 0x25 , 0x6A , 0xEF , 0xC3 , 0x92 , 0x8E , 0xC4 , 0xCA , 0x4E , 0x42 , 0xFD , 0x6E , 0xE9 , 0x94 , 0x48};
	uint8_t output[64];
	status_t status;
	uint8_t	text[16]="essai";
	uint8_t	result[16]="KO";
    uint8_t cipherTest[16];
	hashcrypt_handle_t m_handle;
	const void * cipher2 = (const void*)ciphertext;



	m_handle.keyType = kHASHCRYPT_UserKey;
	status = HASHCRYPT_AES_SetKey(HASHCRYPT, &m_handle, keyAes, 32); // trick, this only configure AES in case of SecretKey AES 256b
	status = HASHCRYPT_AES_EncryptEcb(HASHCRYPT, &m_handle, text, cipherTest, 16);
	status = HASHCRYPT_AES_DecryptEcb(HASHCRYPT, &m_handle, cipherTest, result, 16);

	StartPuf();
	if( false == GetKey())
	{
		StopPuf();
		HASHCRYPT_Deinit(HASHCRYPT);
		return false;
	}

	m_handle.keyType = kHASHCRYPT_SecretKey;
	status = HASHCRYPT_AES_SetKey(HASHCRYPT, &m_handle, NULL, 32); // trick, this only configure AES in case of SecretKey AES 256b
	status = HASHCRYPT_AES_EncryptEcb(HASHCRYPT, &m_handle, plaintext, ciphertext, size);

	if( false == GetKey())
	{
		StopPuf();
		HASHCRYPT_Deinit(HASHCRYPT);
		return false;
	}

	m_handle.keyType = kHASHCRYPT_SecretKey;
	status = HASHCRYPT_AES_SetKey(HASHCRYPT, &m_handle, NULL, 32); // trick, this only configure AES in case of SecretKey AES 256b
	status = HASHCRYPT_AES_DecryptEcb(HASHCRYPT, &m_handle, ciphertext, result, 16);


	StopPuf();
//	HASHCRYPT_Deinit(HASHCRYPT);

	if(status == kStatus_Success)
	{
		return true;
	}else
	{
		return false;
	}

}

/*!
 * @fn void CYBER_DecrypUsingHWUserKEK (void)
 * @brief Decrypt cipher using AES ECB
 * @param ciphertext encrypted data
 * @param plaintext pointer to store decrypted data
 * @param size plaintext size
 * @warning size must be a multiple of 16bytes
 * @return true if OK, else false
 */
bool CYBER_DecrypUsingHWUserKEK(uint8_t *ciphertext, uint8_t *plaintext, size_t size)
{
	uint8_t keyAes[32];
	status_t status;
	hashcrypt_handle_t m_handle;


	StartPuf();
	GetKey();

	m_handle.keyType = kHASHCRYPT_SecretKey;
	status = HASHCRYPT_AES_SetKey(HASHCRYPT, &m_handle, keyAes, 32); // trick, this only configure AES in case of SecretKey
	status = HASHCRYPT_AES_DecryptEcb(HASHCRYPT, &m_handle, ciphertext, plaintext , size);
	StopPuf();
//	HASHCRYPT_Deinit(HASHCRYPT);

	if(status == kStatus_Success)
	{
		return true;
	}else
	{
		return false;
	}
}