/**
 * Copyright (C) 2011 Samsung Electronics Co., Ltd. All rights reserved.
 *
 * Mobile Communication Division,
 * Digital Media & Communications Business, Samsung Electronics Co., Ltd.
 *
 * This software and its documentation are confidential and proprietary
 * information of Samsung Electronics Co., Ltd.  No part of the software and
 * documents may be copied, reproduced, transmitted, translated, or reduced to
 * any electronic medium or machine-readable form without the prior written
 * consent of Samsung Electronics.
 *
 * Samsung Electronics makes no representations with respect to the contents,
 * and assumes no responsibility for any errors that might appear in the
 * software and documents. This publication and the contents hereof are subject
 * to change without notice.
 *
 */

/**
 * @file tz_hdcp2_crypto.c
 * @author
 * @date
 * @brief This file contains all the encryption-decryption function definitions used in HDCP authentication protocol.
 */

#include <sys/ioctl.h>

#include "tz_hdcp2.h"
#include "tz_hdcp2_crypto.h"

extern int g_secdrv_fd;
extern int g_crypt_dev_fd;

#if !defined(TRUE)
	#define TRUE	(1==1)
#endif /* !TRUE */

#if !defined(FALSE)
	#define FALSE	(1!=1)
#endif /* !FALSE */

#define EOL "\n"

/**
 * @fn int TZ_RSA_PKCS1_OAEP_SHA256(unsigned char *to, int tlen,const unsigned char *from, int flen, unsigned char *param, int plen);
 * @brief - This function is used for RSAES-OAEP encryption.
 * @param to - pointer to the encrypted output data.
 * @param tlen - length of output.
 * @param from - pointer to input data to be encrypted.
 * @param flen - length of input.
 * @param param - pointer to the parameters, if any.
 * @param plen - length of parameter.
 * @return int returns HDCP2_OK in case of success, else error code corresponding to the error
*/
int TZ_RSA_PKCS1_OAEP_SHA256(unsigned char *to, int tlen,
			const unsigned char *from, int flen, unsigned char *param, int plen);

/**
 * @fn int TZ_RSA_VERIFY_PKCS1_OAEP_SHA256(unsigned char *to, int tlen, const unsigned char *from, int flen, int num, const unsigned char *param, int plen)
 * @brief This function is used to unhash the decrypted data. It is called by TZ_RSA_OAEP_decrypt.
 * @param to - the pointer to the decrypted and unhashed data.
 * @param tlen - length of the data.
 * @param from - the pointer to the decrypted but hashed data
 * @param flen - length of from data.
 * @param num - length of HDCP2_SIZE_RECEIVER_PUBKEY.
 * @param param - pointer to parameter, if any.
 * @param plen - length of parameter.
 * @return int returns HDCP2_OK in case of success, else error code corresponding to the error
 */
int TZ_RSA_VERIFY_PKCS1_OAEP_SHA256(unsigned char *to, int tlen,
			const unsigned char *from, int flen, int num,
			const unsigned char *param, int plen);

/**
 * @fn void TZ_LOG_HEX(const char *title, const unsigned char *data, int length)
 * @brief This function is used for logging purposes in secure world.
 * @param title - String/Tag to print in logs
 * @param data - data to print in logs
 * @param length - length of the data.
 * @return void
 */

void TZ_LOG_HEX(const char *title, const unsigned char *data, int length)
{
#ifdef DEBUG
	int i = 0;
	char buffer[4096] = {0, };
	char dest_buffer[4096] = {0, };

	LOGD("%s [%d]\n", title, length);

	// print binary data
	for (i = 0; i < length; i++) {
		if (i % 16 == 0) {
			if (i != 0) LOGD("%s\n", buffer);
			buffer[0] = 0;
		}
		sprintf(dest_buffer, "%s%.2x ", buffer, data[i]);
		sprintf(buffer, "%s", dest_buffer);
	}

	sprintf(dest_buffer, "%s", buffer);
	sprintf(buffer, "%s", dest_buffer);
	LOGD("%s\n", buffer);
#endif /* DEBUG */
}

/**
 * @fn int TZ_AES_ECB_encrypt(uint8_t *key, uint32_t key_len, uint8_t *pt, uint32_t pt_len, uint8_t *ct, uint32_t *ct_len)
 * @brief This function is used for AES ecryption while pairing HDCP Transmitter and HDCP Receiver.
 * @param key - pointer to the key used in AES encryption
 * @param key_len - length of the key
 * @param pt - pointer to the data/information to be encrypted
 * @param pt_len - length of the data
 * @param ct - pointer to the encrypted output
 * @param ct_len - length of the encrypted output
 * @return int returns HDCP2_ERR_CRYPTO in case of failure else returns TLAPI_OK.
 */
int TZ_AES_ECB_encrypt(uint8_t *key, uint32_t key_len, uint8_t *pt,
			uint32_t pt_len, uint8_t *ct, uint32_t *ct_len)
{
	TEE_Result result;
	TEE_OperationHandle operAESEnc = NULL;
	TEE_ObjectHandle objAESKey = NULL;
	TEE_Attribute attrsAESKey;
	int retrn = HDCP2_OK;

	LOGW("TZ_AES_ECB_encrypt(): Entry#\n\n");

	if (key == NULL || key_len != 16) {
		LOGE("TZ_AES_ECB_encrypt(): Error, Invalid key length\n\n");
		return HDCP2_ERR_CRYPTO;
	}

	if (pt == NULL || pt_len == 0) {
		LOGE("TZ_AES_ECB_encrypt(): Error, Invalid input data length\n\n");
		return HDCP2_ERR_CRYPTO;
	}

	if (ct == NULL) {
		LOGE("TZ_AES_ECB_encrypt(): Error, Invalid output data length\n\n");
		return HDCP2_ERR_CRYPTO;
	}

	result = TEE_AllocateOperation(&operAESEnc, TEE_ALG_AES_ECB_NOPAD, TEE_MODE_ENCRYPT, 8*key_len);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_AES_ECB_encrypt(): Error, TEE_AllocateOperation, result: %x\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	// Allocate Transient Object, create object attributes with AES Symmetric Key.
	result = TEE_AllocateTransientObject(TEE_TYPE_AES, 8*key_len, &objAESKey);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_AES_ECB_encrypt(): Error, TEE_AllocateTransientObject, result: %x\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	TEE_InitRefAttribute(&attrsAESKey, TEE_ATTR_SECRET_VALUE, key, key_len);
	result = TEE_PopulateTransientObject(objAESKey, &attrsAESKey, sizeof(attrsAESKey)/sizeof(TEE_Attribute));
	if (result != TEE_SUCCESS) {
		LOGE("TZ_AES_ECB_encrypt(): Error, TEE_PopulateTransientObject, result: %x\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	// Set Operation Key with Secret AES Symmetric Key
	result = TEE_SetOperationKey(operAESEnc, objAESKey);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_AES_ECB_encrypt(): Error, TEE_SetOperationKey, result: %x\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	// Init & Encrypt Plain to Cipher Text
	TEE_CipherInit(operAESEnc, NULL, 0);
	result = TEE_CipherDoFinal(operAESEnc, pt, pt_len, ct, ct_len);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_AES_ECB_encrypt(): Error, TEE_CipherDoFinal, result: %x\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

cleanup:
	TEE_FreeOperation(operAESEnc);
	TEE_CloseObject(objAESKey);

	if (retrn != HDCP2_OK)
		return -1;

	LOGW("TZ_AES_ECB_encrypt(): Exit#\n\n");
	return result;
}

/**
 * @fn int TZ_AES_ECB_decrypt(uint8_t *key, uint32_t key_len, uint8_t *ct, uint32_t ct_len, uint8_t *dt, uint32_t *dt_len)
 * @brief This function is used for AES deryption.
 * @param key - pointer to the key used in AES decryption
 * @param key_len - length of the key
 * @param pt - pointer to the encrypted data
 * @param pt_len - length of the data
 * @param ct - pointer to the decrypted output
 * @param ct_len - length of the decrypted output
 * @return int returns TLAPI_OK in case of success else returns corresponding error code.
 */
int TZ_AES_ECB_decrypt(uint8_t *key, uint32_t key_len, uint8_t *ct,
			uint32_t ct_len, uint8_t *dt, uint32_t *dt_len)
{
	TEE_Result result;
	TEE_OperationHandle operAESDec = NULL;
	TEE_ObjectHandle objAESKey = NULL;
	TEE_Attribute attrsAESKey;
	int retrn = HDCP2_OK;

	if (key == NULL || key_len != 16) {
		LOGE("TZ_AES_ECB_decrypt(): Error, Invalid key length\n\n");
		return HDCP2_ERR_CRYPTO;
	}

	if (ct == NULL || ct_len == 0) {
		LOGE("TZ_AES_ECB_decrypt(): Error, Invalid input data length\n\n");
		return HDCP2_ERR_CRYPTO;
	}

	if (dt == NULL) {
		LOGE("TZ_AES_ECB_decrypt(): Error, Invalid output data length\n\n");
		return HDCP2_ERR_CRYPTO;
	}

	result = TEE_AllocateOperation(&operAESDec, TEE_ALG_AES_ECB_NOPAD, TEE_MODE_DECRYPT, key_len*8);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_AES_ECB_decrypt(): Error, TEE_AllocateOperation, result: %x\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	// Allocate Transient Object, create object attributes with AES Symmetric Key.
	result = TEE_AllocateTransientObject(TEE_TYPE_AES, key_len*8, &objAESKey);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_AES_ECB_decrypt(): Error, TEE_AllocateTransientObject, result: %x\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	TEE_InitRefAttribute(&attrsAESKey, TEE_ATTR_SECRET_VALUE, key, key_len);

	result = TEE_PopulateTransientObject(objAESKey, &attrsAESKey, sizeof(attrsAESKey)/sizeof(TEE_Attribute));
	if (result != TEE_SUCCESS) {
		LOGE("TZ_AES_ECB_decrypt(): Error, TEE_PopulateTransientObject, result: %x\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	// Set Operation Key with Secret AES Symmetric Key
	result = TEE_SetOperationKey(operAESDec, objAESKey);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_AES_ECB_decrypt(): Error, TEE_SetOperationKey, result: %x\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	// Init & Encrypt Plain to Cipher Text
	TEE_CipherInit(operAESDec, NULL, 0);
	result = TEE_CipherDoFinal(operAESDec, ct, ct_len, dt, dt_len);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_AES_ECB_decrypt(): Error, TEE_CipherDoFinal, result: %x\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

cleanup:
	TEE_FreeOperation(operAESDec);
	TEE_CloseObject(objAESKey);
	// TEE_FreeTransientObject(objAESKey);
	if (retrn != HDCP2_OK)
		return -1;

	return result;
}

/**
 * @fn int TZ_Get_ContentKey(uint8_t *ks, uint8_t *lc128, uint8_t* pKey)
 * @brief This function is used to compute the Key used in AES module.
 * @param ks - session key.
 * @param lc128 - constant global key.
 * @param pKey - resultant output key
 * @return int returns HDCP2_OK in case of success.
 */
int TZ_Get_ContentKey(uint8_t *ks, uint8_t *lc128, uint8_t* pKey)
{
	int ret = HDCP2_OK;
	int i = 0;

	if (pKey == NULL || ks == NULL || lc128 == NULL) {
		LOGE("TZ_Get_ContentKey: HDCP2_ERR_NULL_POINTER\n\n");
		ret = HDCP2_ERR_INVALID_INPUT;
		goto err;
	}

	for (i = 0; i < 16; i++)
		pKey[i] = ks[i] ^ lc128[i];

err:
	return ret;
}

/**
 * @fn int TZ_rand(uint8_t *data, const int length)
 * @brief This function generates the pseudo - random number of length given as parameter.
 * @param data - type of pseudo random to be generated.
 * @param length - length of pseudo random to be generated.
 * @return int - returns length of generated pseudo-random number if successful else returns HDCP2_ERR_CRYPTO.
 */
int TZ_rand(uint8_t *data, const int length)
{
	TEE_GenerateRandom(data, (size_t) length);

	return length;
}

/**
 * @fn void TZ_IncreaseCtr(TZ_HDCP2_CTX *hdcp_ctx)
 * @brief This function is used to increase the counter in AES counter module.
 * @param hdcp_ctx - pointer to the HDCP context.
 * @return void
 */
void TZ_IncreaseCtr(TZ_HDCP2_CTX *hdcp_ctx)
{
	int i = 0;
	for (i = 7; i >= 0; i--) {
		hdcp_ctx->ctr[i] = hdcp_ctx->ctr[i] == MAX_INCREASE_COUNTER ? 0 : hdcp_ctx->ctr[i] + 1;
		if (hdcp_ctx->ctr[i] != 0)
			break;
	}
}

/**
 * @fn int TZ_Derivate_dkey(TZ_HDCP2_CTX *hdcp_ctx)
 * @brief This function is used for Key derivation.
 * @param hdcp_ctx - pointer to the HDCP context.
 * @return int returns HDCP2_OK in case of success else returns (HDCP2_ERR_TRUSTZONE_BASE - 61).
 */
int TZ_Derivate_dkey(TZ_HDCP2_CTX *hdcp_ctx)
{
	int i = 0;
	uint8_t key[16];
	uint8_t input[16] = {0, };
	size_t ct_len = 16;

	if (hdcp_ctx->version >= HDCP2_VERSION_2_2
		&& hdcp_ctx->transmitter_info.VERSION >= 0x02 && hdcp_ctx->receiver_info.VERSION >= 0x02) {
		//IV = rtx || (rrx XOR ctr)
		u8 temp[8] = {0};
		for(i = 7; i >= 0; i--)
			temp[i] = hdcp_ctx->rrx[i] ^ hdcp_ctx->ctr[i];

		TZ_LOG_HEX("hdcp_ctx->rrx", hdcp_ctx->rrx, sizeof(hdcp_ctx->rrx));
		TZ_LOG_HEX("hdcp_ctx->ctr", hdcp_ctx->ctr, sizeof(hdcp_ctx->ctr));
		TZ_LOG_HEX("temp", temp, sizeof(temp));
		TEE_MemMove(input, hdcp_ctx->rtx, sizeof(hdcp_ctx->rtx));
		TEE_MemMove(input + sizeof(hdcp_ctx->rtx), temp, sizeof(temp));
	} else {
		// input = rtx || ctr
		TEE_MemMove(input, hdcp_ctx->rtx, sizeof(hdcp_ctx->rtx));
		TEE_MemMove(input + 8, hdcp_ctx->ctr, sizeof(hdcp_ctx->ctr));
	}

	// key = km XOR rn
	TEE_MemMove(key, hdcp_ctx->pairing_info.km, sizeof(key));
	for (i = 8; i < 16; i++)
		key[i] ^= hdcp_ctx->rn[i - 8];

	TZ_AES_ECB_encrypt(key, 16, input, 16, hdcp_ctx->dkey, &ct_len);

	if (ct_len != 16) {
		return HDCP2_ERR_TRUSTZONE_BASE - 61;
	}

	TZ_IncreaseCtr(hdcp_ctx);

	return HDCP2_OK;
}

/**
 * @fn int TZ_SHA256(uint8_t *md, uint8_t *in, const int inlen)
 * @brief This is the hashing function used in cryptographic signature calculated on receiver certificate.
 * @param md - pointer to the key used in hash function.
 * @param in - pointer to the data to be hashed.
 * @param inlen - length of the data
 * @return int - returns 32 if success else returns HDCP2_ERR_CRYPTO.
 */
int TZ_SHA256(uint8_t *md, uint8_t *in, const int inlen)
{
	TEE_Result result;
	TEE_OperationHandle operDigest = NULL;
	size_t outlen = HDCP2_MESSAGE_DIGEST_SIZE; // SizeOf(Outlen) = 32 bytes( 32 * 8 = 256 bits) as SHA256 is used while calculating Message Digest
	unsigned char emptyDigest[] = {0xE3, 0xB0, 0xC4, 0x42, 0x98, 0xFC, 0x1C, 0x14, 0x9A, 0xFB, 0xF4, 0xC8,
									0x99, 0x6F, 0xB9, 0x24, 0x27, 0xAE, 0x41, 0xE4, 0x64, 0x9B, 0x93,
									0x4C, 0xA4, 0x95, 0x99, 0x1B, 0x78, 0x52, 0xB8, 0x55};

	if (md == NULL) {
		return HDCP2_ERR_CRYPTO;
	}

	if (in == NULL || inlen == 0) {
		TEE_MemMove(md, emptyDigest, sizeof(emptyDigest));
		return HDCP2_MESSAGE_DIGEST_SIZE;
	}

	result = TEE_AllocateOperation(&operDigest, TEE_ALG_SHA256, TEE_MODE_DIGEST, 0/*HDCP2_SIZE_RECEIVER_PUBKEY*/);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_SHA256 : error, TEE_AllocateOperation, ret=0x%08X, exit\n", result);
		return HDCP2_ERR_CRYPTO;
	}

	result = TEE_DigestDoFinal(operDigest, in, inlen, md, &outlen);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_SHA256 : error, TEE_DigestDoFinal ret=0x%08X, exit\n", result);
		TEE_FreeOperation(operDigest);
		return HDCP2_ERR_CRYPTO;
	}

	TEE_FreeOperation(operDigest);

	return HDCP2_MESSAGE_DIGEST_SIZE;
}

/**
 * @fn int TZ_HMAC_SHA256(uint8_t *md, uint8_t *key, const int keylen, uint8_t *in, const int inlen)
 * @brief This is hashing function used to calculate the hash value of the data given, *in, using hashing key *md.
 * @param md - output pointer
 * @param key - pointer to the key used in hash function.
 * @param keylen - length of the key
 * @param in - data to be hashed
 * @param inlen - length of the data
 * @return int returns HDCP2_ERR_CRYPTO
 */
int TZ_HMAC_SHA256(uint8_t *md, uint8_t *key, const int keylen, uint8_t *in,
			const int inlen)
{
	TEE_Result result;
	TEE_OperationHandle operHMAC_SHA256 = NULL;
	TEE_ObjectHandle objHMAC_SHA256Key = NULL;
	TEE_Attribute attrsHMAC_SHA256Key;
	int retrn = HDCP2_OK;
	size_t outlen = 32;

	result = TEE_AllocateOperation(&operHMAC_SHA256, TEE_ALG_HMAC_SHA256, TEE_MODE_MAC, keylen*8);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_HMAC_SHA256(): Error, TEE_AllocateOperation, result: %u\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	// Allocate Transient Object, create object attributes with Secret Key.
	result = TEE_AllocateTransientObject(TEE_TYPE_HMAC_SHA256, keylen*8, &objHMAC_SHA256Key);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_HMAC_SHA256(): Error, TEE_AllocateTransientObject, result: %u\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	TEE_InitRefAttribute(&attrsHMAC_SHA256Key, TEE_ATTR_SECRET_VALUE, key, keylen);

	result = TEE_PopulateTransientObject(objHMAC_SHA256Key, &attrsHMAC_SHA256Key, sizeof(attrsHMAC_SHA256Key)/sizeof(TEE_Attribute));
	if (result != TEE_SUCCESS) {
		LOGE("TZ_HMAC_SHA256(): Error, TEE_PopulateTransientObject, result: %u\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	// Set Operation Key with Derived Secret Key
	result = TEE_SetOperationKey(operHMAC_SHA256, objHMAC_SHA256Key);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_HMAC_SHA256(): Error, TEE_SetOperationKey, result: %u\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	}

	// Init & Compute HMAC_SHA256
	TEE_MACInit(operHMAC_SHA256, NULL, 0);

	result = TEE_MACComputeFinal(operHMAC_SHA256, in, inlen, md, &outlen);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_HMAC_SHA256(): Error, TEE_MACComputeFinal, result: %u\n", result);
		retrn = HDCP2_ERR_CRYPTO;
		goto cleanup;
	} else {
		LOGD("TZ_HMAC_SHA256(): Success, TEE_MACComputeFinal, result: %u\n", result);
	}

cleanup:
	TEE_FreeOperation(operHMAC_SHA256);
	TEE_FreeTransientObject(objHMAC_SHA256Key);
	if (retrn != HDCP2_OK)
		return -1;

	return 32;
}

/**
 * @fn int TZ_PKCS1_MGF1(unsigned char *mask, long len, const unsigned char *seed, long seedlen)
 * @brief This function is used for mask generation which uses TZ_SHA256 internally.
 * @param mask - pointer to the bits used for masking the data.
 * @param len - length of the mask.
 * @param seed - pointer to the extra bits used for seeding the input data.
 * @param seedlen - length of the seed number.
 * @return int returns length of the output
 */
int TZ_PKCS1_MGF1(unsigned char *mask, long len, const unsigned char *seed,
			long seedlen)
{
	long i, outlen = 0;
	unsigned char md[32];
	unsigned char buffer[128];
	int mdlen = 32;
	
	if (seedlen + 3 >= 128) {
		LOGE("TZ_PKCS1_MGF1: seedlen is invalid (%d)",seedlen);
		return HDCP2_ERR_INVALID_INPUT;
	}	
	TEE_MemFill(buffer, 0,128);

	for (i = 0; outlen < len; i++) {
		TEE_MemMove(buffer, seed, seedlen);
		buffer[seedlen] = (unsigned char) ((i >> 24) & 255);
		buffer[seedlen + 1] = (unsigned char) ((i >> 16) & 255);
		buffer[seedlen + 2] = (unsigned char) ((i >> 8)) & 255;
		buffer[seedlen + 3] = (unsigned char) (i & 255);

		if (outlen + mdlen <= len) {
			if (TZ_SHA256(mask + outlen, buffer, seedlen + 4) < 0) {
				return HDCP2_ERR_CRYPTO;
			}
			outlen += mdlen;
		} else {
			if (TZ_SHA256(md, buffer, seedlen + 4) < 0) {
				return HDCP2_ERR_CRYPTO;
			}
			TEE_MemMove(mask + outlen, md, len - outlen);
			outlen = len;
		}
	}

	return len;
}

/**
 * @fn int TZ_RSA_PKCS1_OAEP_SHA256(unsigned char *to, int tlen,const unsigned char *from, int flen, unsigned char *param, int plen);
 * @brief - This function is used for RSAES-OAEP encryption.
 * @param to - pointer to the encrypted output data.
 * @param tlen - length of output.
 * @param from - pointer to input data to be encrypted.
 * @param flen - length of input.
 * @param param - pointer to the parameters, if any.
 * @param plen - length of parameter.
 * @return int returns HDCP2_OK in case of success, else error code corresponding to the error
*/
int TZ_RSA_PKCS1_OAEP_SHA256(unsigned char *to, int tlen,
			const unsigned char *from, int flen, unsigned char *param, int plen)
{
	int i, digestlen = 32, emlen = tlen - 1;
	unsigned char *db, *seed;
	unsigned char dbmask[256] , seedmask[32] ;

	TEE_MemFill(dbmask,0,256);
	TEE_MemFill(seedmask,0,32);

	if (flen > emlen - 2 * digestlen - 1) {
		return HDCP2_ERR_CRYPTO;
	}

	if (emlen < 2 * digestlen + 1) {
		return HDCP2_ERR_CRYPTO;
	}

	to[0] = 0;
	seed = to + 1;
	db = to + digestlen + 1;

	if (TZ_SHA256(db, param, plen) < 0) {
		return HDCP2_ERR_CRYPTO;
	}

	TEE_MemFill(db + digestlen, 0, emlen - flen - 2 * digestlen - 1);
	db[emlen - flen - digestlen - 1] = 0x01;
	TEE_MemMove(db + emlen - flen - digestlen, from, (unsigned int) flen);

	if (TZ_rand(seed, digestlen) <= 0) {
		return HDCP2_ERR_CRYPTO;
	}

	if (TZ_PKCS1_MGF1(dbmask, emlen - digestlen, seed, digestlen) < 0) {
		return HDCP2_ERR_CRYPTO;
	}

	for (i = 0; i < emlen - digestlen; i++)
		db[i] ^= dbmask[i];

	if (TZ_PKCS1_MGF1(seedmask, digestlen, db, emlen - digestlen) < 0) {
		return HDCP2_ERR_CRYPTO;
	}

	for (i = 0; i < digestlen; i++)
		seed[i] ^= seedmask[i];

	return tlen;
}

/**
 * @fn int TZ_RSA_OAEP_encrypt(TZ_HDCP2_CTX *hdcp_ctx, HDCP2_KEY *hdcp2_key, uint8_t *in, uint8_t *out)
 * @brief This function is used for the encryption of data using RSAES-OAEP encryption scheme.
 * @param hdcp_ctx - pointer to the HDCP context.
 * @param hdcp2_key - pointer to the key used in the data encryption.
 * @param in - pointer to the data to be encrypted.
 * @param out - pointer to the output encrypted data.
 * @return int - returns length of the encrypted output.
 */
int TZ_RSA_OAEP_encrypt(TZ_HDCP2_CTX *hdcp_ctx, HDCP2_KEY *hdcp2_key,
			uint8_t *in, uint8_t *out)
{
	TEE_Result result;
	TEE_OperationHandle operKmEnc = NULL;
	TEE_ObjectHandle objRecvPubKey = NULL;
	TEE_Attribute attrsRecvPubKey[2];
	int retrn = HDCP2_OK;
	size_t outlen = HDCP2_SIZE_RECEIVER_PUBKEY;

	result = TEE_AllocateOperation(&operKmEnc, TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA256, TEE_MODE_ENCRYPT, 1024);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_RSA_OAEP_encrypt(): Error, TEE_AllocateOperation result: %x\n", result);
		retrn = HDCP2_ERR_OAEP_ENCRYPTION;
		goto cleanup;
	}

	// Allocate Transient Object, create object attributes with Receiver Public Key.
	result = TEE_AllocateTransientObject(TEE_TYPE_RSA_PUBLIC_KEY, 1024, &objRecvPubKey);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_RSA_OAEP_encrypt(): Error, TEE_AllocateTransientObject - objRecvPubKey, result: %x\n", result);
		retrn = HDCP2_ERR_OAEP_ENCRYPTION;
		goto cleanup;
	}
	// Initialize all those Attributes required to populate Transient Object
	TEE_InitRefAttribute(&attrsRecvPubKey[0], TEE_ATTR_RSA_MODULUS, hdcp2_key->cert.publickey_n, sizeof(hdcp2_key->cert.publickey_n));
	TEE_InitRefAttribute(&attrsRecvPubKey[1], TEE_ATTR_RSA_PUBLIC_EXPONENT, hdcp2_key->cert.publickey_e, sizeof(hdcp2_key->cert.publickey_e));

	result = TEE_PopulateTransientObject(objRecvPubKey, attrsRecvPubKey, sizeof(attrsRecvPubKey)/sizeof(TEE_Attribute));
	if (result != TEE_SUCCESS) {
		LOGE("TZ_RSA_OAEP_encrypt(): Error, TEE_PopulateTransientObject - objRecvPubKey, result: %x\n", result);
		retrn = HDCP2_ERR_OAEP_ENCRYPTION;
		goto cleanup;
	}

	// Set Operation Key with Receiver Public key
	result = TEE_SetOperationKey( operKmEnc, objRecvPubKey);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_RSA_OAEP_encrypt(): Error, TEE_SetOperationKey result: %u\n", result);
		retrn = HDCP2_ERR_OAEP_ENCRYPTION;
		goto cleanup;
	}

	result = TEE_AsymmetricEncrypt(operKmEnc, NULL, 0, hdcp_ctx->pairing_info.km, HDCP2_MASTER_KEY_SIZE, out, &outlen);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_RSA_OAEP_encrypt(): Error, TEE_AsymmetricEncrypt Encrypt MasterKey Km, result: %u\n", result);
		retrn = HDCP2_ERR_OAEP_ENCRYPTION;
		goto cleanup;
	} else {
		LOGD("TZ_RSA_OAEP_encrypt(): Success, TEE_AsymmetricEncrypt Encrypt MasterKey Km, result: %u\n", result);
	}

cleanup:
	TEE_FreeOperation(operKmEnc);
	TEE_FreeTransientObject(objRecvPubKey);
	if (retrn != HDCP2_OK)
		return -1;

	return HDCP2_SIZE_RECEIVER_PUBKEY;
}

/**
 * @fn int TZ_RSA_OAEP_decrypt(TZ_HDCP2_CTX *hdcp_ctx, HDCP2_KEY *hdcp2_key, uint8_t *in, uint8_t *out)
 * @brief This function is used for the decryption of data using RSAES-OAEP scheme.
 * @param hdcp_ctx - pointer to the HDCP context.
 * @param hdcp2_key - pointer to the key used in the data decryption.
 * @param in - pointer to the data to be decrypted.
 * @param out - pointer to the decrypted data.
 * @return int returns HDCP2_OK in success else returns corresponding error code.
 */
int TZ_RSA_OAEP_decrypt(TZ_HDCP2_CTX *hdcp_ctx, HDCP2_KEY *hdcp2_key,
			uint8_t *in, uint8_t *out)
{
	TEE_Result result;
	TEE_OperationHandle operKmDec = NULL;
	TEE_ObjectHandle objRecvKeyPair = NULL;
	TEE_Attribute attrsRecvKeyPair[8];
	// As TEE_ATTR_RSA_PRIVATE_EXPONENT is mandatory attribute, so defining a private key here.
	uint8_t private_key[HDCP2_SIZE_RECEIVER_PUBKEY] = {0};
	int retrn = HDCP2_OK;
	size_t outlen = HDCP2_SIZE_RECEIVER_PUBKEY;

	// TEE_ATTR_RSA_PRIVATE_EXPONENT is mandatory attribute as per GP. 
	// We do not have private key exponent in our key set. 
	// But we can calculate private key exponent with p, q and public key exponent we have. 
	// The start of the process for calculating a private key exponent
	TEE_BigInt *p = (TEE_BigInt *)TEE_Malloc(sizeof(hdcp2_key->private_key.p) * sizeof(TEE_BigInt), 0);
	TEE_BigInt *q = (TEE_BigInt *)TEE_Malloc(sizeof(hdcp2_key->private_key.q) * sizeof(TEE_BigInt), 0);
	TEE_BigInt *e = (TEE_BigInt *)TEE_Malloc(sizeof(hdcp2_key->cert.publickey_e) * sizeof(TEE_BigInt), 0);	// public key exponent
	TEE_BigInt *d = (TEE_BigInt *)TEE_Malloc(HDCP2_SIZE_RECEIVER_PUBKEY * sizeof(TEE_BigInt), 0);		// private key exponent
	TEE_BigInt *euler_n = (TEE_BigInt *)TEE_Malloc((sizeof(hdcp2_key->private_key.p) * 2) * sizeof(TEE_BigInt), 0);
	TEE_BigInt *subVal = (TEE_BigInt *)TEE_Malloc(sizeof(int) * sizeof(TEE_BigInt), 0);
	TEE_BigInt *op1 = (TEE_BigInt *)TEE_Malloc(sizeof(hdcp2_key->private_key.p) * sizeof(TEE_BigInt), 0);
	TEE_BigInt *op2 = (TEE_BigInt *)TEE_Malloc(sizeof(hdcp2_key->private_key.q) * sizeof(TEE_BigInt), 0);

	TEE_BigIntInit(p, sizeof(hdcp2_key->private_key.p));
	TEE_BigIntInit(q, sizeof(hdcp2_key->private_key.q));
	TEE_BigIntInit(e, sizeof(hdcp2_key->cert.publickey_e));
	TEE_BigIntInit(d, HDCP2_SIZE_RECEIVER_PUBKEY);
	TEE_BigIntInit(euler_n, (sizeof(hdcp2_key->private_key.p) * 2));
	TEE_BigIntInit(subVal, sizeof(int));
	TEE_BigIntInit(op1, sizeof(hdcp2_key->private_key.p));
	TEE_BigIntInit(op2, sizeof(hdcp2_key->private_key.q));

	TEE_BigIntConvertFromOctetString(p, (uint8_t *) hdcp2_key->private_key.p, sizeof(hdcp2_key->private_key.p), 0);
	TEE_BigIntConvertFromOctetString(q, (uint8_t *) hdcp2_key->private_key.q, sizeof(hdcp2_key->private_key.q), 0);
	TEE_BigIntConvertFromOctetString(e, (uint8_t *) hdcp2_key->cert.publickey_e, sizeof(hdcp2_key->cert.publickey_e), 0);
	TEE_BigIntConvertFromS32(subVal, 1);	// subVal = 1

	TEE_BigIntSub(op1, p, subVal);		// op1 = p-1
	TEE_BigIntSub(op2, q, subVal);		// op2 = q-1
	TEE_BigIntMul(euler_n, op1, op2);	// eulur_n = (p-1)*(q-1)
	TEE_BigIntInvMod(d, e, euler_n);	// (d*e mod euler_n) is equal to 1
	TEE_BigIntConvertToOctetString(private_key, &outlen, d);
	// The end of the process for calculating a private key exponent

	result = TEE_AllocateOperation(&operKmDec, TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA256, TEE_MODE_DECRYPT, 1024);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_RSA_OAEP_decrypt: Error, TEE_AllocateOperation result: %u\n", result);
		retrn = HDCP2_ERR_OAEP_DECRYPTION;
		goto cleanup;
	}

	// Allocate Transient Object, create object attributes with Receiver Public/Private key Pair.
	result = TEE_AllocateTransientObject(TEE_TYPE_RSA_KEYPAIR, 1024, &objRecvKeyPair);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_RSA_OAEP_decrypt: Error, TEE_AllocateTransientObject - objRecvKeyPair, result: %u\n", result);
		retrn = HDCP2_ERR_OAEP_DECRYPTION;
		goto cleanup;
	}

	// Initialize all those Attributes required to populate Transient Object
	TEE_InitRefAttribute(&attrsRecvKeyPair[0], TEE_ATTR_RSA_MODULUS, hdcp2_key->cert.publickey_n, sizeof(hdcp2_key->cert.publickey_n));
	TEE_InitRefAttribute(&attrsRecvKeyPair[1], TEE_ATTR_RSA_PUBLIC_EXPONENT, hdcp2_key->cert.publickey_e, sizeof(hdcp2_key->cert.publickey_e));
	TEE_InitRefAttribute(&attrsRecvKeyPair[2], TEE_ATTR_RSA_PRIVATE_EXPONENT, private_key, 128);
	TEE_InitRefAttribute(&attrsRecvKeyPair[3], TEE_ATTR_RSA_PRIME1, (uint8_t *) hdcp2_key->private_key.p, sizeof(hdcp2_key->private_key.p));
	TEE_InitRefAttribute(&attrsRecvKeyPair[4], TEE_ATTR_RSA_PRIME2, (uint8_t *) hdcp2_key->private_key.q, sizeof(hdcp2_key->private_key.q));
	TEE_InitRefAttribute(&attrsRecvKeyPair[5], TEE_ATTR_RSA_EXPONENT1, (uint8_t *) hdcp2_key->private_key.dmp1, sizeof(hdcp2_key->private_key.dmp1));
	TEE_InitRefAttribute(&attrsRecvKeyPair[6], TEE_ATTR_RSA_EXPONENT2, (uint8_t *) hdcp2_key->private_key.dmq1, sizeof(hdcp2_key->private_key.dmq1));
	TEE_InitRefAttribute(&attrsRecvKeyPair[7], TEE_ATTR_RSA_COEFFICIENT, (uint8_t *) hdcp2_key->private_key.iqmp, sizeof(hdcp2_key->private_key.iqmp));

	result = TEE_PopulateTransientObject(objRecvKeyPair, attrsRecvKeyPair, sizeof(attrsRecvKeyPair)/sizeof(TEE_Attribute));
	if (result != TEE_SUCCESS) {
		LOGE("TZ_RSA_OAEP_decrypt: Error, TEE_PopulateTransientObject - objRecvKeyPair, result: %u\n", result);
		retrn = HDCP2_ERR_OAEP_DECRYPTION;
		goto cleanup;
	}

	// Set Operation Key with Receiver Public key
	result = TEE_SetOperationKey(operKmDec, objRecvKeyPair);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_RSA_OAEP_decrypt: Error, TEE_SetOperationKey result: %u\n", result);
		retrn = HDCP2_ERR_OAEP_DECRYPTION;
		goto cleanup;
	}

	result = TEE_AsymmetricDecrypt(operKmDec, NULL, 0, in, HDCP2_SIZE_RECEIVER_PUBKEY, out, &outlen);
	if (result != TEE_SUCCESS) {
		LOGE("TZ_RSA_OAEP_decrypt: Error, TEE_AsymmetricDecrypt result: %u\n", result);
		retrn = HDCP2_ERR_OAEP_DECRYPTION;
		goto cleanup;
	} else {
		LOGD("TZ_RSA_OAEP_decrypt: Success, TEE_AsymmetricDecrypt result: %u\n", result);
	}

cleanup:
	TEE_FreeOperation(operKmDec);
	TEE_FreeTransientObject(objRecvKeyPair);
	TEE_Free(p);
	TEE_Free(q);
	TEE_Free(e);
	TEE_Free(d);
	TEE_Free(euler_n);
	TEE_Free(subVal);
	TEE_Free(op1);
	TEE_Free(op2);

	if (retrn != HDCP2_OK)
		return -1;

	return HDCP2_SIZE_RECEIVER_PUBKEY;
}

/**
 * @fn int TZ_SW_AES_CTR(u8 *in, u32 inlen, u8 *out, u32 *outlen, uint8_t *key, uint8_t *pP)
 * @brief This function is used to encrypt data using AES ctr scheme.
 * @param in - pointer to the input frame.
 * @param inlen - length of input data.
 * @param out - pointer to the output
 * @param outlen - length of encrypted output.
 * @param key - key used in the encryption.
 * @param pP - pointer to the (riv XORed with streamCtr) concatenated with inputCtr.
 * @return int
 */
int TZ_SW_AES_CTR(u8 *in, u32 inlen, u8 *out, u32 *outlen, uint8_t *key, uint8_t *pP)
{
	int carry = 0;
	int ret, i, blkidx;
	u8 ct[16] = {0};
	int blknum = (inlen+15)/16;
	u32 blklen = 16;

	*outlen = 0;

	for (blkidx=0; blkidx<blknum; blkidx++) {
		if ((ret=TZ_AES_ECB_encrypt(key, 16, pP, 16, ct, &blklen)) != HDCP2_OK)
			return HDCP2_ERR_TRUSTZONE_BASE-ret;

		if (blkidx == blknum - 1 && inlen % 16 != 0)
			blklen = inlen % 16;
		for (i=0; i<(int)blklen; i++)
			out[blkidx*16 + i] = ct[i] ^ in[blkidx*16 + i];

		*outlen += blklen;

		// increase counter
		carry = 0;
		for (i = 0; i < 8; i++) {
			if (pP[15 - i] + carry < 255) {
				pP[15 - i] += 1;
				break;
			}
			pP[15 - i] = 0;
			carry = 1;
		}
	}

	return HDCP2_OK;
}

/**
 * @fn int TZ_Cipher_AES_CTR_Encrypt_HW(u8 *in, u32 inlen, u8 *out, u32 *outlen, uint8_t *key, uint8_t *pP)
 * @brief This function encrypts the data using AES encryption in CTR mode
 * @param in - pointer to input data to be encrypted
 * @param inlen - length of input data to be encrypted
 * @param out- pointer to output encrypted data stream is to be stored
 * @param outlen - length of output data
 * @param key - pointer to the key
 * @param pP - pointer to the (riv XORed with streamCtr) concatenated with inputCtr.
 * @return int - HDCP2_OK in case of success, else error code corresponding to the error
 */
int TZ_Cipher_AES_CTR_Encrypt_HW(u8 *pt, u32 pt_len, u8 *ct, u32 *ct_len, uint8_t *key,
			uint8_t *pP)
{
	int ret = TEE_SUCCESS;
	struct crypto_info info;
	struct crypt_oper cop;
	TEE_MemFill((void*)&info, 0, sizeof(struct crypto_info));
	TEE_MemFill((void *)&cop, 0, sizeof(struct crypt_oper));

	info.mode = ACE_AES_CTR | ACE_MODE_ENC; /* AES + CTR + ENCRYPT */
	info.keylen = 16;
	info.ivlen = 16;

	if (key == NULL || pP == NULL) {
		LOGE("TZ_Cipher_AES_CTR_Encrypt_HW: Error key or pP invalid\n");
		return HDCP2_ERR;
	}

	TEE_MemMove(info.key, key, info.keylen);
	TEE_MemMove(info.iv, pP, info.ivlen);

	if (g_crypt_dev_fd <= 0) {
		LOGE("CRYPTO DEV is not open, crypto dev fd = %d", g_crypt_dev_fd);
		return HDCP2_ERR;
	}

	ret = ioctl(g_crypt_dev_fd, CRYPT_FUNC_INIT_CONTEXT, (unsigned long)&info);
	if (ret != TEE_SUCCESS) {
		LOGE("TZ_Cipher_AES_CTR_Encrypt_HW: CRYPT_FUNC_INIT_CONTEXT failed with ret val: %d , errno= %d (%s)\n", ret, errno, strerror(errno));
		return HDCP2_ERR;
	}

	cop.src_addr = (char*)pt;
	cop.dst_addr = (char*)ct;
	cop.len = pt_len;
	cop.out_len =ct_len;
	cop.final = 1;

	ret = ioctl(g_crypt_dev_fd, CRYPT_FUNC_OPERATION, (unsigned long)&cop);
	if (ret != TEE_SUCCESS) {
		LOGE("TZ_Cipher_AES_CTR_Encrypt_HW: CRYPT_FUNC_OPERATION failed with ret val: %d and errno: %d (%s)\n", ret, errno, strerror(errno));
		return HDCP2_ERR;
	}

	return ret;
}

/**
 * @fn int TZ_Cipher_AES_CTR_Decrypt_HW(u8 *in, u32 inlen, u8 *out, u32 *outlen, uint8_t *key, uint8_t *pP)
 * @brief This function decrypts the data using AES decryption in CTR mode
 * @param in - pointer to input data to be decrypted
 * @param inlen - length of input data to be decrypted
 * @param out- pointer to output decrypted data stream is to be stored
 * @param outlen - length of output data
 * @param key - pointer to the key
 * @param pP - pointer to the (riv XORed with streamCtr) concatenated with inputCtr.
 * @return int - HDCP2_OK in case of success, else error code corresponding to the error
 */
int TZ_Cipher_AES_CTR_Decrypt_HW(u8 *ct, u32 ct_len, u8 *pt, u32 *pt_len, uint8_t *key, uint8_t *pP)
{
	return TZ_Cipher_AES_CTR_Encrypt_HW(ct, ct_len, pt, pt_len, key, pP);
}

int TZ_RSA_VERIFY_PKCS1_OAEP_SHA256(unsigned char *to, int tlen,
			const unsigned char *from, int flen, int num,
			const unsigned char *param, int plen)
{
	int i = -1, digestlen = 32, dblen = -1, mlen = -1;
	const unsigned char *maskeddb;
	int lzero = -1;
	unsigned char db[256] , seed[32], phash[32];
	unsigned char *padded_from;
	int bad = 0;

	TEE_MemFill(db,0,256);
	TEE_MemFill(seed,0,32);
	TEE_MemFill(phash,0,32);

	if (flen < 0)
		return HDCP2_ERR_TRUSTZONE_BASE-1;

	if (--num < 2 * digestlen + 1)
		return HDCP2_ERR_TRUSTZONE_BASE-1;

	lzero = num - flen;
	if (lzero < 0) {
		lzero = 0;
		flen = num;
	}

	dblen = num - digestlen;
	if (dblen > (MAX_HASH_BUFFER - digestlen) / 2) {
		return HDCP2_ERR_TRUSTZONE_BASE-1;
	}
	padded_from = db + dblen;
	if (lzero > MAX_HASH_BUFFER - dblen || flen > MAX_HASH_BUFFER - dblen) {
		return HDCP2_ERR_TRUSTZONE_BASE-1;
	}
	TEE_MemFill(padded_from, 0, lzero);
	//TEE_MemMove(padded_fTEE_MemMovelzero, from, flen);
	bad = 0;
	TEE_MemMove(padded_from, from + 1, flen);

	maskeddb = padded_from + digestlen;
	if (TZ_PKCS1_MGF1(seed, digestlen, maskeddb, dblen) < 0)
		return HDCP2_ERR_TRUSTZONE_BASE-2;

	for (i = 0; i < digestlen; i++)
		seed[i] ^= padded_from[i];

	if (TZ_PKCS1_MGF1(db, dblen, seed, digestlen) < 0)
		return HDCP2_ERR_TRUSTZONE_BASE-3;

	for (i = 0; i < dblen; i++)
		db[i] ^= maskeddb[i];

	if (TZ_SHA256(phash, (u8 *)param, plen) < 0)
		return HDCP2_ERR_TRUSTZONE_BASE-4;

	if (TEE_MemCompare(db, phash, digestlen) != 0 || bad) {
		return HDCP2_ERR_TRUSTZONE_BASE-5;
	} else {
		for (i = digestlen; i < dblen; i++)
			if (db[i] != 0x00)
				break;
		if (i == dblen || db[i] != 0x01) {
			return HDCP2_ERR_TRUSTZONE_BASE-6;
		} else {
			mlen = dblen - ++i;
			if (tlen < mlen || mlen > HDCP2_SIZE_RECEIVER_PUBKEY) {
				return HDCP2_ERR_TRUSTZONE_BASE-7;
			} else {
				TEE_MemMove(to, db + i, mlen);
			}
		}
	}

	return HDCP2_OK;
}