#include <string.h>

#include "vk_data_struct.h"
#include "vk_interface.h"
#include "vk_error.h"
#include "vk_log.h"
#include "vk_utils.h"
#include "vk_table.h"
#include "vk_vault_manager.h"
#include "crypto/vk_crypto_aes.h"
#include "crypto/vk_crypto.h"

extern vk_device_info_t g_device_info;

int wrap_vault(target_t* target, uint8_t* data, uint32_t len, uint8_t* tag, uint32_t tag_len, uint8_t* random_iv)
{
	int ret = VK_ERR_GENERAL;
	uint32_t buf_len = 0;
	uint8_t* buf = NULL;

	if (target == NULL) {
		LOGE("%s: target is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (data == NULL) {
		LOGE("%s: data is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (len == 0 || len > MAX_VAULT_LEN) {
		LOGE("%s: Invalid len(%u)\n", __func__, len);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (tag == NULL) {
		LOGE("%s: tag is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (tag_len != AES_GCM_TAG_LEN) {
		LOGE("%s: Invalid tag size(%u)\n", __func__, tag_len);
		return VK_ERR_INVALID_ARGUMENT;
	}

	ret = target->platform.memory_alloc(&buf, len);
	if (ret != VK_SUCCESS || buf == NULL) {
		LOGE("%s: Failed memory alloc(%d)\n", __func__, ret);
		goto out;
	}

	ret = vk_crypto_aes_256_gcm_encrypt(data, len, buf, &buf_len, tag, tag_len, NULL, random_iv);
	if (ret != VK_SUCCESS) {
		LOGE("%s: Failed to encrypt with aes gcm(%d/%u/%u)\n", __func__, ret, len, buf_len);
		goto out;
	}

	if (len != buf_len) {
		LOGE("%s: Unexpected ciphertext length(%u/%u)\n", __func__, len, buf_len);
		ret = VK_ERR_WRAP_UNWRAP_OBJECT;
		goto out;
	}
	memcpy(data, buf, buf_len);

out:
	if (buf != NULL) {
		target->platform.memory_free(buf);
	}

	return ret;
}

int unwrap_vault(target_t* target, uint8_t* data, uint32_t len, uint8_t* tag, uint32_t tag_len, uint8_t* random_iv)
{
	int ret = VK_ERR_GENERAL;
	uint32_t buf_len = 0;
	uint8_t* buf = NULL;

	if (target == NULL) {
		LOGE("%s: target is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (data == NULL) {
		LOGE("%s: data is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (len == 0 || len > MAX_VAULT_LEN) {
		LOGE("%s: Invalid len(%u)\n", __func__, len);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (tag == NULL) {
		LOGE("%s: tag is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (tag_len != AES_GCM_TAG_LEN) {
		LOGE("%s: Invalid tag size(%u)\n", __func__, tag_len);
		return VK_ERR_INVALID_ARGUMENT;
	}

	ret = target->platform.memory_alloc(&buf, len);
	if (ret != VK_SUCCESS || buf == NULL) {
		LOGE("%s: Failed memory alloc(%d)\n", __func__, ret);
		goto out;
	}

	ret = vk_crypto_aes_256_gcm_decrypt(data, len, buf, &buf_len, tag, tag_len, NULL, random_iv);
	if (ret != VK_SUCCESS) {
		LOGE("%s: Failed to decrypt with aes gcm(%d/%u/%u)\n", __func__, ret, len, buf_len);
		goto out;
	}

	if (len != buf_len) {
		LOGE("%s: Unexpected plaintext length(%u/%u)\n", __func__, len, buf_len);
		ret = VK_ERR_WRAP_UNWRAP_OBJECT;
		goto out;
	}

	memcpy(data, buf, buf_len);

out:
	if (buf != NULL) {
		target->platform.memory_free(buf);
	}

	return ret;
}

int read_multi_blk_rpmb(target_t* target, const uint32_t vtab_index, const uint32_t jump_blk, const uint32_t read_size, uint8_t* pBuffer, const bool mirror)
{
	int ret = VK_ERR_GENERAL;

	uint32_t i;
	uint32_t read_cnt, read_cnt_extra;
	uint32_t start_index_rpmb;
	uint8_t* pTemp = NULL;

	if (target == NULL) {
		LOGE("%s: target is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (vtab_index >= NUM_OF_VAULTS) {
		LOGE("%s: Invalid table index(%d)\n", __func__, vtab_index);
		return VK_ERR_READ_VTAB;
	}

	if (jump_blk >= (MAX_VAULT_LEN / VAULT_SINGLE_BLOCK_LEN)) {
		LOGE("%s: Invalid block number(%d)\n", __func__, jump_blk);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (read_size > MAX_VAULT_LEN) {
		LOGE("%s: Invalid read size(%u)\n", __func__, read_size);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (pBuffer == NULL) {
		LOGE("%s: pBuffer is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (mirror) {
		start_index_rpmb = VTAB[vtab_index].start_index_rpmb_backup;
	} else {
		start_index_rpmb = VTAB[vtab_index].start_index_rpmb;		
	}

#ifdef VK_RPMB_TRANSFER_MLT_BLK
	read_cnt = (uint32_t)((float)read_size / (VAULT_SINGLE_BLOCK_LEN * RPMB_TRANSFER_MLT_BLK_READ));
	if (read_size != (read_cnt * RPMB_TRANSFER_MLT_BLK_READ * VAULT_SINGLE_BLOCK_LEN)) {
		read_cnt_extra = (uint32_t)(((float)(read_size - (read_cnt * RPMB_TRANSFER_MLT_BLK_READ * VAULT_SINGLE_BLOCK_LEN)) /  VAULT_SINGLE_BLOCK_LEN) + 0.999999);
	} else {
		read_cnt_extra = 0;
	}
#else
	read_cnt = (uint32_t)((float)read_size / (VAULT_SINGLE_BLOCK_LEN * RPMB_TRANSFER_MLT_BLK_READ) + 0.999999);
#endif

	pTemp = pBuffer;
	for (i = 0; i < read_cnt; i++) {
		ret = target->platform.read(VTAB[vtab_index].rpmb_partition,
			                        start_index_rpmb + jump_blk + (i * RPMB_TRANSFER_MLT_BLK_READ), pTemp, RPMB_TRANSFER_MLT_BLK_READ);
		if (ret != VK_SUCCESS) {
			LOGE("%s: Something went wrong while reading vault(%d/%d)\n", __func__, ret, RPMB_TRANSFER_MLT_BLK_READ);
			return ret;
		}
		pTemp += VAULT_SINGLE_BLOCK_LEN * RPMB_TRANSFER_MLT_BLK_READ;
	}
#ifdef VK_RPMB_TRANSFER_MLT_BLK
	if (read_cnt_extra) {
		ret = target->platform.read(VTAB[vtab_index].rpmb_partition,
				                    start_index_rpmb + jump_blk + (read_cnt * RPMB_TRANSFER_MLT_BLK_READ), pTemp, read_cnt_extra);
		if (ret != VK_SUCCESS) {
			LOGE("%s: Something went wrong while reading extra vault(%d/%d)\n", __func__, ret, RPMB_TRANSFER_MLT_BLK_READ);
			return ret;
		}
	}
#endif

	return VK_SUCCESS;
}

int read_unsheltered_vault(target_t* target, cmd_req_t* req, unsvault_t* vault)
{
	int ret = VK_ERR_GENERAL;

	if (target == NULL) {
		LOGE("%s: target is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req == NULL) {
		LOGE("%s: req is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (vault == NULL) {
		LOGE("%s: vault is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req->vtab_index >= NUM_OF_VAULTS) {
		LOGE("%s: Invalid table index(%d)\n", __func__, req->vtab_index);
		return VK_ERR_READ_VTAB;
	}

	if (req->vault_level == VAULT_LEVEL2) {
		if (g_device_info.rpmb_key_provisioning == 'F') {
			LOGE("%s: RPMB Key not programmed\n", __func__);
			return VK_ERR_RPMB_KEY_PROV;
		}

		ret = target->platform.read(VTAB[req->vtab_index].rpmb_partition,
			                        VTAB[req->vtab_index].start_index_rpmb, (uint8_t*)vault, VAULT_RPMB_BLOCK_UNIT);
		if (ret != VK_SUCCESS) {
			LOGE("%s: Something went wrong while reading vault(%d/L2)\n", __func__, ret);
			goto out;
		}

		if (!isAllZero(vault, sizeof(unsvault_t))) {
			ret = vk_crypto_check_integrity((uint8_t*)vault, HASHING_LEN, vault->hash);

			if (ret != VK_SUCCESS) {
				LOGE("%s: Failed to check integrity. Try to check mirror vault.(%d/L2)\n", __func__, ret);

				// read mirror
				ret = target->platform.read(VTAB[req->vtab_index].rpmb_partition,
					                        VTAB[req->vtab_index].start_index_rpmb_backup, (uint8_t*)vault, VAULT_RPMB_BLOCK_UNIT);
				if (ret != VK_SUCCESS) {
					LOGE("%s: Something went wrong while reading mirror vault(%d/L2)\n", __func__, ret);
					goto out;
				}

				if (!isAllZero(vault, sizeof(unsvault_t))) {

					ret = vk_crypto_check_integrity((uint8_t*)vault, HASHING_LEN, vault->hash);
					if (ret != VK_SUCCESS) {
						LOGE("%s: [mirror] Failed to check integrity.(%d/L2)\n", __func__, ret);
						goto out;
					}

					// Recovery vault from 'mirror' to 'origin'
					LOGI("%s: Recovery vault from mirror(L2)\n", __func__);
					ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
						                         VTAB[req->vtab_index].start_index_rpmb, (uint8_t*)vault, VAULT_RPMB_BLOCK_UNIT);
					if (ret != VK_SUCCESS) {
						LOGE("%s:[recovery] Something went wrong while writing vault(%d/L2)\n", __func__, ret);
						goto out;
					}
				} else {
					LOGE("%s: Vault is broken! :(\n", __func__);
					ret = VK_ERR_READ_VAULT;
					goto out;
				}
			}
		} else {
			LOGI("%s: Unshelterd vault is all zero(L2)\n", __func__);
			ret = VK_SUCCESS;
			goto out;
		}
	} else if (req->vault_level == VAULT_LEVEL1) {
		if (req->preload_size > req->entire_vault_size) {
			LOGE("%s: Invalid preload size(%d/L1)\n", __func__, req->preload_size);
			ret = VK_ERR_INVALID_ARGUMENT;
			goto out;
		}

		if (!isAllZero(req->preload, req->preload_size) && req->preload_size > 0) {
			ret = vk_crypto_check_integrity(req->preload, HASHING_LEN, req->preload + HASH_OFFSET);
			if (ret != VK_SUCCESS) {
				LOGE("%s: Failed to check integrity(%d/L1)\n", __func__, ret);
				goto out;
			}
		} else {
			LOGI("%s: Unshelterd vault is all zero(L1)\n", __func__);
			ret = VK_SUCCESS;
			goto out;
		}

		memcpy(vault, req->preload, sizeof(unsvault_t));
		ret = VK_SUCCESS;
	} else {
		LOGE("%s: Invalid vault_level type, something wrong in vtab(%d)\n", __func__, req->vault_level);
		ret = VK_ERR_INVALID_ARGUMENT;
		goto out;
	}

out:
	if (ret != VK_SUCCESS) {
		memset(vault, 0, sizeof(unsvault_t));
	}

	return ret;
}

int write_unsheltered_data(target_t* target, cmd_req_t* req, cmd_rsp_t* rsp, uint8_t* unsheltered_data)
{
	int ret = VK_ERR_GENERAL;
	unsvault_t* vault = NULL;
	bool enable_write_protection = true;
	uint8_t meta_fuse_blown = META_FUSE_BLOWN;
	uint8_t meta_fuse_not_blown = META_FUSE_NOT_BLOWN;

	if (target == NULL) {
		LOGE("%s: target is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req == NULL) {
		LOGE("%s: req is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (rsp == NULL) {
		LOGE("%s: rsp is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (unsheltered_data == NULL) {
		LOGE("%s: unsheltered_data is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req->vtab_index >= NUM_OF_VAULTS) {
		LOGE("%s: Invalid table index(%d)\n", __func__, req->vtab_index);
		return VK_ERR_READ_VTAB;
	}

	if (req->vault_level == VAULT_LEVEL2) {
		if (g_device_info.rpmb_key_provisioning == 'F') {
			LOGE("%s: RPMB Key not programmed\n", __func__);
			ret = VK_ERR_RPMB_KEY_PROV;
			goto out;
		}
	}

	ret = check_write_protection(target, req, &enable_write_protection);
	if (ret != VK_SUCCESS) {
		LOGE("%s: Failed to check write_protection(%d)\n", __func__, ret);
		return VK_ERR_GENERAL;
	}

	if (enable_write_protection) {
		LOGE("%s: Not allow to write. Enable write protection\n", __func__);
		return VK_ERR_PERMISSION_DENIED;
	}

	// Read stored unsheltered vault
	ret = target->platform.memory_alloc(&vault, sizeof(unsvault_t));
	if (ret != VK_SUCCESS || vault == NULL) {
		LOGE("%s: Failed memory alloc(%d)\n", __func__, ret);
		goto out;
	}

	ret = read_unsheltered_vault(target, req, vault);
	if (ret != VK_SUCCESS) {
		goto out;
	}

	memcpy(vault->unsheltered_data, unsheltered_data, MAX_UNSHELTERED_DATA_LEN);

	// Update client code, Hash
	vault->client_code = req->client_code;

	// set Meta data
	if (VTAB[req->vtab_index].one_time_writable) {
		memcpy(vault->meta + META_VAULT_WRITTEN, &meta_fuse_blown, sizeof(uint8_t));
	}

	if (req->vault_level == VAULT_LEVEL2) {
		memcpy(vault->meta + META_VAULT_USE_LEVEL2, &meta_fuse_blown, sizeof(uint8_t));
	}

	// Refresh Hash
	ret = vk_crypto_sha256((uint8_t*)vault, HASHING_LEN, vault->hash);
	if (ret != VK_SUCCESS) {
		LOGE("%s: Failed to hash vault(%d)\n", __func__, ret);
		goto out;
	}

	// Write unsheltered vault
	if (req->vault_level == VAULT_LEVEL2) {
		// write in mirror
		ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
			                         VTAB[req->vtab_index].start_index_rpmb_backup, (uint8_t*)vault, VAULT_RPMB_BLOCK_UNIT);
		if (ret != VK_SUCCESS) {
			LOGE("%s: Something went wrong while writing mirror vault(%d/L2)\n", __func__, ret);
			goto out;
		}

		// write in original
		ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
			                         VTAB[req->vtab_index].start_index_rpmb, (uint8_t*)vault, VAULT_RPMB_BLOCK_UNIT);
		if (ret != VK_SUCCESS) {
			LOGE("%s: Something went wrong while writing vault(%d/L2)\n", __func__, ret);
			goto out;
		}

		if (VTAB[req->vtab_index].update_steady == true) {
			rsp->req_steady.req_data_size = sizeof(unsvault_t);
			memcpy(rsp->req_steady.req_data, vault, rsp->req_steady.req_data_size);
		}
	} else if (req->vault_level == VAULT_LEVEL1) {
		rsp->req_steady.req_data_size = sizeof(unsvault_t);
		memcpy(rsp->req_steady.req_data, vault, rsp->req_steady.req_data_size);
	} else {
		LOGE("%s: Invalid vault_level type, something went wrong in vtab(%d/L1)\n", __func__, req->vault_level);
		ret = VK_ERR_INVALID_ARGUMENT;
		goto out;
	}

out:
	if (vault != NULL) {
		memset((uint8_t*)vault, 0, sizeof(unsvault_t));
		target->platform.memory_free(vault);
	}

	return ret;
}

int write_unsheltered_vault(target_t* target, cmd_req_t* req, cmd_rsp_t* rsp, unsvault_t* vault)
{
	int ret = VK_ERR_GENERAL;
	uint8_t meta_fuse_blown = META_FUSE_BLOWN;

	if (target == NULL) {
		LOGE("%s: target is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req == NULL) {
		LOGE("%s: req is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (rsp == NULL) {
		LOGE("%s: rsp is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (vault == NULL) {
		LOGE("%s: vault is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req->vtab_index >= NUM_OF_VAULTS) {
		LOGE("%s: Invalid table index(%d)\n", __func__, req->vtab_index);
		return VK_ERR_READ_VTAB;
	}

	if (req->vault_level == VAULT_LEVEL2) {
		if (g_device_info.rpmb_key_provisioning == 'F') {
			LOGE("%s: RPMB Key not programmed\n", __func__);
			return VK_ERR_RPMB_KEY_PROV;
		}
	}

	// Update client code, Hash
	vault->client_code = req->client_code;

	if (req->vault_level == VAULT_LEVEL2) {
		memcpy(vault->meta + META_VAULT_USE_LEVEL2, &meta_fuse_blown, sizeof(uint8_t));
	}

	// Refresh Hash
	ret = vk_crypto_sha256((uint8_t*)vault, HASHING_LEN, vault->hash);
	if (ret != VK_SUCCESS) {
		LOGE("%s: Failed to hash vault(%d)\n", __func__, ret);
		goto out;
	}

	// Write unsheltered vault
	if (req->vault_level == VAULT_LEVEL2) {
		// write in mirror
		ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
			                         VTAB[req->vtab_index].start_index_rpmb_backup, (uint8_t*)vault, VAULT_RPMB_BLOCK_UNIT);
		if (ret != VK_SUCCESS) {
			LOGE("%s: Something went wrong while writing mirror vault(%d/L2)\n", __func__, ret);
			goto out;
		}

		// write in original
		ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
			                         VTAB[req->vtab_index].start_index_rpmb, (uint8_t*)vault, VAULT_RPMB_BLOCK_UNIT);
		if (ret != VK_SUCCESS) {
			LOGE("%s: Something went wrong while writing vault(%d/L2)\n", __func__, ret);
			goto out;
		}

		if (VTAB[req->vtab_index].update_steady == true) {
			rsp->req_steady.req_data_size = sizeof(unsvault_t);
			memcpy(rsp->req_steady.req_data, vault, rsp->req_steady.req_data_size);
		}
	} else if (req->vault_level == VAULT_LEVEL1) {
		rsp->req_steady.req_data_size = sizeof(unsvault_t);
		memcpy(rsp->req_steady.req_data, vault, rsp->req_steady.req_data_size);
	} else {
		LOGE("%s: Invalid vault_level type, something went wrong in vtab(%d)\n", __func__, req->vault_level);
		ret = VK_ERR_INVALID_ARGUMENT;
		goto out;
	}

out:
	return ret;
}

int read_entire_vault(target_t* target, cmd_req_t* req, vault_t* entire_vault)
{
	int ret = VK_ERR_GENERAL;
	uint32_t sheltered_vault_size = 0;
	uint32_t sheltered_data_size = 0;
	uint32_t read_cnt, read_cnt_extra, write_cnt, write_cnt_extra;
	uint32_t i = 0;

	uint8_t aes_iv[AES_IV_LEN] = {0,};
	uint8_t gcm_tag[AES_GCM_TAG_LEN] = {0,};

	uint8_t* pTemp = NULL;
	uint8_t* pSheltered = NULL;

	if (target == NULL) {
		LOGE("%s: target is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req == NULL) {
		LOGE("%s: req is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (entire_vault == NULL) {
		LOGE("%s: entire_vault is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req->vtab_index >= NUM_OF_VAULTS) {
		LOGE("%s: Invalid table index(%d)\n", __func__, req->vtab_index);
		return VK_ERR_READ_VTAB;
	}

	read_cnt = read_cnt_extra = write_cnt = write_cnt_extra = 0;

	ret = read_unsheltered_vault(target, req, (unsvault_t*)entire_vault);
	if (ret != VK_SUCCESS) {
		goto out;
	}

	pSheltered = (uint8_t*)entire_vault + SHELTERED_ENTRY;

	memcpy(&sheltered_data_size, &entire_vault->unsheltered.sheltered_data_size, SHELTERED_VAULT_DATA_LEN_SIZE);
	memcpy(aes_iv, entire_vault->unsheltered.iv, AES_IV_LEN);
	memcpy(gcm_tag, entire_vault->unsheltered.tag, AES_GCM_TAG_LEN);

	if (sheltered_data_size > req->entire_vault_size - VAULT_LEN_EXPECT_SHELTERED_DATA) {
		LOGE("%s: Invalid stored data size(%d)\n", __func__, sheltered_data_size);
		ret = VK_ERR_READ_VAULT;
		goto out;
	}

	sheltered_vault_size = VAULT_KEY_LEN + VAULT_KEY_LEN + SHELTERED_RESERVED + sheltered_data_size;

	// Read SHELTERD VAULT
	if (req->vault_level == VAULT_LEVEL2) {

		pSheltered = (uint8_t*)entire_vault + SHELTERED_ENTRY;

#ifdef VK_RPMB_TRANSFER_MLT_BLK
		read_cnt = (uint32_t)((float)sheltered_vault_size / (VAULT_SINGLE_BLOCK_LEN * RPMB_TRANSFER_MLT_BLK_READ));
		if (sheltered_vault_size != (read_cnt * RPMB_TRANSFER_MLT_BLK_READ * VAULT_SINGLE_BLOCK_LEN)) {
			read_cnt_extra = (uint32_t)(((float)(sheltered_vault_size - (read_cnt * RPMB_TRANSFER_MLT_BLK_READ * VAULT_SINGLE_BLOCK_LEN)) /  VAULT_SINGLE_BLOCK_LEN) + 0.999999);
		} else {
			read_cnt_extra = 0;
		}
#else
		read_cnt = (uint32_t)((float)sheltered_vault_size / (VAULT_SINGLE_BLOCK_LEN * RPMB_TRANSFER_MLT_BLK_READ) + 0.999999);
#endif

		ret = read_multi_blk_rpmb(target, req->vtab_index, 1, sheltered_vault_size, pSheltered, false);
		if (ret != VK_SUCCESS)
			goto out;

		if (!isAllZero(pSheltered, (read_cnt * RPMB_TRANSFER_MLT_BLK_READ * VAULT_SINGLE_BLOCK_LEN + read_cnt_extra * VAULT_SINGLE_BLOCK_LEN))) {
			ret = unwrap_vault(target, pSheltered, sheltered_vault_size, gcm_tag, AES_GCM_TAG_LEN, aes_iv);
			if (ret != VK_SUCCESS) {
				LOGE("%s: Failed to unwrap vault(%d/L2)\n", __func__, ret);

				// Read mirror
				pSheltered = (uint8_t*)entire_vault + SHELTERED_ENTRY;

				ret = read_multi_blk_rpmb(target, req->vtab_index, 1, sheltered_vault_size, pSheltered, true);
				if (ret != VK_SUCCESS)
					goto out;

				ret = unwrap_vault(target, pSheltered, sheltered_vault_size, gcm_tag, AES_GCM_TAG_LEN, aes_iv);
				if (ret != VK_SUCCESS) {
					LOGE("%s: Failed to unwrap mirror vault(%d/L2)\n", __func__, ret);
					goto out;
				} else {
					// Restore vault from 'mirror' to 'origin'
					pTemp = (uint8_t*)entire_vault + SHELTERED_ENTRY;
#ifdef VK_RPMB_TRANSFER_MLT_BLK
					write_cnt = (uint32_t)((read_cnt * RPMB_TRANSFER_MLT_BLK_READ + read_cnt_extra) / RPMB_TRANSFER_MLT_BLK_WRITE);
					write_cnt_extra = read_cnt_extra;
#else
					write_cnt = read_cnt * RPMB_TRANSFER_MLT_BLK_READ + read_cnt_extra;
#endif
					for (i = 0; i < write_cnt; i++) {
						ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
							                         VTAB[req->vtab_index].start_index_rpmb + 1 + (i * RPMB_TRANSFER_MLT_BLK_WRITE), pTemp, RPMB_TRANSFER_MLT_BLK_WRITE);
						if (ret != VK_SUCCESS) {
							LOGE("%s: [Recovery] Something went wrong while writing vault(%d/L2/%d)\n", __func__, ret, RPMB_TRANSFER_MLT_BLK_WRITE);
							goto out;
						}
						pTemp += VAULT_SINGLE_BLOCK_LEN * RPMB_TRANSFER_MLT_BLK_WRITE;
					}
#ifdef VK_RPMB_TRANSFER_MLT_BLK
					if (write_cnt_extra) {
						ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
							                         VTAB[req->vtab_index].start_index_rpmb + 1 + (write_cnt * RPMB_TRANSFER_MLT_BLK_WRITE), pTemp, write_cnt_extra);
						if (ret != VK_SUCCESS) {
							LOGE("%s: [Recovery] Something went wrong while writing extra vault(%d/L2/%d)\n", __func__, ret, RPMB_TRANSFER_MLT_BLK_WRITE);
							goto out;
						}
					}
#endif
				}
			}
		} else {
			LOGI("%s: Sheltered vault is all zero(L2)\n", __func__);
			ret = VK_SUCCESS;
			goto out;
		}
	} else if (req->vault_level == VAULT_LEVEL1) {
		memcpy(pSheltered, req->preload + sizeof(unsvault_t), sheltered_vault_size);

		if (!isAllZero(pSheltered, sheltered_vault_size)) {
			ret = unwrap_vault(target, pSheltered, sheltered_vault_size, gcm_tag, AES_GCM_TAG_LEN, aes_iv);
			if (ret != VK_SUCCESS) {
				LOGE("%s: Failed to unwrap vault(%d/L1)\n", __func__, ret);
				goto out;
			}
		} else {
			LOGI("%s: Sheltered vault is all zero(L1)\n", __func__);
			ret = VK_SUCCESS;
			goto out;
		}
	} else {
		LOGE("%s: Invalid vault_level type, something went wrong in vtab(%d)\n", __func__, req->vault_level);
		ret = VK_ERR_INVALID_ARGUMENT;
		goto out;
	}

	// Vaild check
	if (entire_vault->unsheltered.sheltered_data_size > req->entire_vault_size - sizeof(unsvault_t)) {
		LOGE("%s: Invalid shletered data size(%d). Something went wrong.\n", __func__, entire_vault->unsheltered.sheltered_data_size);
		ret = VK_ERR_READ_VAULT;
		goto out;
	}

out:
	memset(aes_iv, 0, AES_IV_LEN);
	memset(gcm_tag, 0, AES_GCM_TAG_LEN);

	return ret;
}

int write_entire_vault(target_t* target, cmd_req_t* req, cmd_rsp_t* rsp, vault_t* entire_vault)
{
	int ret = VK_ERR_GENERAL;
	uint32_t sheltered_vault_size = 0;
	uint32_t sheltered_data_size = 0;
	uint32_t write_cnt, write_cnt_extra;
	uint32_t i = 0;
	bool enable_write_protection = true;
	uint8_t meta_fuse_blown = META_FUSE_BLOWN;

	uint8_t gcm_tag[AES_GCM_TAG_LEN] = {0,};

	uint8_t* pTemp = NULL;
	uint8_t* pSheltered = NULL;

	if (target == NULL) {
		LOGE("%s: target is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req == NULL) {
		LOGE("%s: req is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (rsp == NULL) {
		LOGE("%s: rsp is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (entire_vault == NULL) {
		LOGE("%s: entire_vault is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req->vtab_index >= NUM_OF_VAULTS) {
		LOGE("%s: Invalid table index(%d)\n", __func__, req->vtab_index);
		return VK_ERR_READ_VTAB;
	}

	if (req->vault_level == VAULT_LEVEL2) {
		if (g_device_info.rpmb_key_provisioning == 'F') {
			LOGE("%s: RPMB Key not programmed\n", __func__);
			return VK_ERR_RPMB_KEY_PROV;
		}
	}

	write_cnt = write_cnt_extra = 0;

	ret = check_write_protection(target, req, &enable_write_protection);
	if (ret != VK_SUCCESS) {
		LOGE("%s: Failed to check write_protection(%d)\n", __func__, ret);
		return VK_ERR_GENERAL;
	}

	if (enable_write_protection) {
		LOGE("%s: Not allow to write. Enable write protection\n", __func__);
		return VK_ERR_PERMISSION_DENIED;
	}

	sheltered_vault_size = req->entire_vault_size - sizeof(unsvault_t);

	pSheltered = (uint8_t*)entire_vault + SHELTERED_ENTRY;

	if (!isAllZero(entire_vault, VAULT_LEN_EXPECT_SHELTERED_DATA + entire_vault->unsheltered.sheltered_data_size)) {
		// Vaild check
		if (entire_vault->unsheltered.sheltered_data_size > req->entire_vault_size - VAULT_LEN_EXPECT_SHELTERED_DATA) {
			LOGE("%s: Invalid shletered data size(%d). Something went wrong.\n", __func__, entire_vault->unsheltered.sheltered_data_size);
			ret = VK_ERR_WRITE_VAULT;
			goto out;
		}

		memcpy(&sheltered_data_size, &(entire_vault->unsheltered.sheltered_data_size), SHELTERED_VAULT_DATA_LEN_SIZE);
		sheltered_vault_size = VAULT_KEY_LEN + VAULT_KEY_LEN + SHELTERED_RESERVED + sheltered_data_size;

		// Caution! : isAllZero((uint8_t*)entire_vault + VAULT_SINGLE_BLOCK_LEN ...
		if (isAllZero((uint8_t*)entire_vault + VAULT_SINGLE_BLOCK_LEN, sheltered_vault_size)) {
			sheltered_data_size = entire_vault->unsheltered.sheltered_data_size = 0;
			memcpy(&entire_vault->unsheltered.client_code, &(req->client_code), CLIENT_CODE_LEN);
			memset(entire_vault->unsheltered.iv, 0, AES_IV_LEN);
			memset(entire_vault->unsheltered.tag, 0, AES_GCM_TAG_LEN);

			sheltered_vault_size = req->entire_vault_size - sizeof(unsvault_t);
		} else {
			// Wrapping Sheltered vault
			ret = wrap_vault(target, pSheltered, sheltered_vault_size, gcm_tag, AES_GCM_TAG_LEN, req->random_iv);
			if (ret != VK_SUCCESS) {
				LOGE("%s: Failed to wrap vault(%d)\n", __func__, ret);
				goto out;
			}

			// Update  Client code, IV, GCM TAG
			memcpy(&entire_vault->unsheltered.client_code, &(req->client_code), CLIENT_CODE_LEN);
			memcpy(entire_vault->unsheltered.iv, req->random_iv, AES_IV_LEN);
			memcpy(entire_vault->unsheltered.tag, gcm_tag, AES_GCM_TAG_LEN);
		}

		if (req->vault_level == VAULT_LEVEL2) {
			memcpy(entire_vault->unsheltered.meta + META_VAULT_USE_LEVEL2, &meta_fuse_blown, sizeof(uint8_t));
		}

		// Refresh Hash
		ret = vk_crypto_sha256((uint8_t*)entire_vault + UNSHELTERED_ENTRY, HASHING_LEN, entire_vault->unsheltered.hash);
		if (ret != VK_SUCCESS) {
			LOGE("%s: Failed to hash vault(%d)\n", __func__, ret);
			goto out;
		}
	}

	// set Meta data
	if (VTAB[req->vtab_index].one_time_writable) {
		memcpy(entire_vault->unsheltered.meta + META_VAULT_WRITTEN, &meta_fuse_blown, sizeof(uint8_t));
	}

	// write entire vault
	if (req->vault_level == VAULT_LEVEL2) {
#ifdef VK_RPMB_TRANSFER_MLT_BLK
		write_cnt = (uint32_t)((float)(sheltered_vault_size + VAULT_SINGLE_BLOCK_LEN) / (VAULT_SINGLE_BLOCK_LEN * RPMB_TRANSFER_MLT_BLK_WRITE));
		if (VAULT_SINGLE_BLOCK_LEN + sheltered_vault_size != (write_cnt * RPMB_TRANSFER_MLT_BLK_WRITE * VAULT_SINGLE_BLOCK_LEN)) {
			write_cnt_extra = (uint32_t)(((float)(VAULT_SINGLE_BLOCK_LEN + sheltered_vault_size - (write_cnt * RPMB_TRANSFER_MLT_BLK_WRITE * VAULT_SINGLE_BLOCK_LEN)) /  VAULT_SINGLE_BLOCK_LEN) + 0.999999);
		} else {
			write_cnt_extra = 0;
		}
#else
		write_cnt = (uint32_t)((float)sheltered_vault_size / (VAULT_SINGLE_BLOCK_LEN * RPMB_TRANSFER_MLT_BLK_WRITE) + 0.999999) + 1;
#endif
		// write in mirror
		pTemp = (uint8_t*)entire_vault;
		for (i = 0; i < write_cnt; i++) {
			ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
				                         VTAB[req->vtab_index].start_index_rpmb_backup + (i * RPMB_TRANSFER_MLT_BLK_WRITE), pTemp, RPMB_TRANSFER_MLT_BLK_WRITE);
			if (ret != VK_SUCCESS) {
				LOGE("%s: Something went wrong while writing mirror vault(%d/L2/%d)\n", __func__, ret, RPMB_TRANSFER_MLT_BLK_WRITE);
				goto out;
			}
			pTemp += VAULT_SINGLE_BLOCK_LEN * RPMB_TRANSFER_MLT_BLK_WRITE;
		}
#ifdef VK_RPMB_TRANSFER_MLT_BLK
		if (write_cnt_extra) {
			ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
				                         VTAB[req->vtab_index].start_index_rpmb_backup + (write_cnt * RPMB_TRANSFER_MLT_BLK_WRITE), pTemp, write_cnt_extra);
			if (ret != VK_SUCCESS) {
				LOGE("%s: Something went wrong while writing extra mirror vault(%d/L2/%d)\n", __func__, ret, RPMB_TRANSFER_MLT_BLK_WRITE);
				goto out;
			}
		}
#endif
		// write in original
		pTemp = (uint8_t*)entire_vault;
		for (i = 0; i < write_cnt; i++) {
			ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
				                         VTAB[req->vtab_index].start_index_rpmb + (i * RPMB_TRANSFER_MLT_BLK_WRITE), pTemp, RPMB_TRANSFER_MLT_BLK_WRITE);
			if (ret != VK_SUCCESS) {
				LOGE("%s: Something went wrong while writing vault(%d/L2/%d)\n", __func__, ret, RPMB_TRANSFER_MLT_BLK_WRITE);
				goto out;
			}
			pTemp += VAULT_SINGLE_BLOCK_LEN * RPMB_TRANSFER_MLT_BLK_WRITE;
		}
#ifdef VK_RPMB_TRANSFER_MLT_BLK
		if (write_cnt_extra) {
			ret = target->platform.write(VTAB[req->vtab_index].rpmb_partition,
				                         VTAB[req->vtab_index].start_index_rpmb + (write_cnt * RPMB_TRANSFER_MLT_BLK_WRITE), pTemp, write_cnt_extra);
			if (ret != VK_SUCCESS) {
				LOGE("%s: Something went wrong while writing extra vault(%d/L2/%d)\n", __func__, ret, RPMB_TRANSFER_MLT_BLK_WRITE);
				goto out;
			}
		}
#endif
		if (VTAB[req->vtab_index].update_steady == true) {
			rsp->req_steady.req_data_size = sizeof(unsvault_t) + sheltered_vault_size;
			memcpy(rsp->req_steady.req_data, entire_vault, rsp->req_steady.req_data_size);
		}
	} else if (req->vault_level == VAULT_LEVEL1) {
		rsp->req_steady.req_data_size = sizeof(unsvault_t) + sheltered_vault_size;
		memcpy(rsp->req_steady.req_data, entire_vault, rsp->req_steady.req_data_size);
	} else {
		LOGE("%s: Invalid vault_level type, something went wrong in vtab(%d)\n", __func__, req->vault_level);
		ret = VK_ERR_INVALID_ARGUMENT;
		goto out;
	}

out:
	memset(gcm_tag, 0, AES_GCM_TAG_LEN);

	return ret;
}

// Only use for KG/RMM
int make_hmac(cmd_req_t* req, uint8_t* key, uint8_t* hmac)
{
	int ret = VK_ERR_GENERAL;
	uint8_t data[MAX_INPUT_DATA_LEN_FOR_HMAC] = {0,};
	size_t data_len = 0;

	if (req == NULL) {
		LOGE("%s: req is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (key == NULL) {
		LOGE("%s: key is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (hmac == NULL) {
		LOGE("%s: hmac is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req->unsheltered_len > 0 && req->unsheltered_len <= MAX_UNSHELTERED_DATA_LEN) {
		memcpy(data + data_len, req->unsheltered, req->unsheltered_len);
		data_len += req->unsheltered_len;
	}

	if (req->server_cert != NULL
	    && !isAllZero(req->server_cert, VAULT_KEY_LEN)
	    && data_len + VAULT_KEY_LEN <= MAX_INPUT_DATA_LEN_FOR_HMAC) {
		memcpy(data + data_len, req->server_cert, VAULT_KEY_LEN);
		data_len += VAULT_KEY_LEN;
	}

	if (!isAllZero(req->vault_nonce, VAULT_NONCE_LEN) && data_len + VAULT_NONCE_LEN <= MAX_INPUT_DATA_LEN_FOR_HMAC) {
		memcpy(data + data_len, req->vault_nonce, VAULT_NONCE_LEN);
		data_len += VAULT_NONCE_LEN;
	}

	ret = vk_crypto_hmac_sha256(hmac, data, data_len, key, VAULT_KEY_LEN);

	return ret;
}

int read_meta(target_t* target, cmd_req_t* req, uint32_t type, uint32_t* result)
{
	int ret = VK_ERR_GENERAL;
	unsvault_t* vault = NULL;
	uint8_t meta[VAULT_META_LEN] = {0,};

	if (target == NULL) {
		LOGE("%s: target is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req == NULL) {
		LOGE("%s: req is null\n", __func__);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (META_TYPE_MAX < type) {
		LOGE("%s: Invalid meta type(%d)\n", __func__, type);
		return VK_ERR_INVALID_ARGUMENT;
	}

	if (req->vtab_index >= NUM_OF_VAULTS) {
		LOGE("%s: Invalid table index(%d)\n", __func__, req->vtab_index);
		return VK_ERR_READ_VTAB;
	}

	ret = target->platform.memory_alloc(&vault, sizeof(unsvault_t));
	if (ret != VK_SUCCESS || vault == NULL) {
		LOGE("%s: Failed memory alloc(%d)\n", __func__, ret);
		goto out;
	}

	if (req->vault_level == VAULT_LEVEL2) {
		if (g_device_info.rpmb_key_provisioning == 'T') {
			ret = read_unsheltered_vault(target, req, vault);
			if (ret != VK_SUCCESS) {
				LOGE("%s: rpmb read failed(%d/L2) \n", __func__, ret);
				goto out;
			}
		} else {
			LOGE("%s: rpmb key not programmed(L2)\n", __func__);
			ret = VK_ERR_RPMB_KEY_PROV;
			goto out;
		}
	} else if (req->vault_level == VAULT_LEVEL1) {
		if (req->preload_size > req->entire_vault_size) {
			LOGE("%s: Invalid preload size(%d/L1)\n", __func__, req->preload_size);
			ret = VK_ERR_INVALID_ARGUMENT;
			goto out;
		}

		if (!isAllZero(req->preload + HASH_OFFSET, SHA256_DIGEST_LEN) && req->preload_size > 0) {
			ret = vk_crypto_check_integrity(req->preload, HASHING_LEN, req->preload + HASH_OFFSET);
			if (ret != VK_SUCCESS) {
				LOGE("%s: Failed to check integrity(%d/L1)\n", __func__, ret);
				goto out;
			}
		} else {
			if (type == META_CP_DATA_MIGRATION) {
				if (g_device_info.rpmb_key_provisioning == 'T') {
					req->vault_level = VAULT_LEVEL2;
					ret = read_unsheltered_vault(target, req, vault);
					req->vault_level = VAULT_LEVEL1;
					if (ret != VK_SUCCESS) {
						LOGE("%s: rpmb read failed(%d/L1)\n", __func__, ret);
						goto out;
					} else {
						memcpy(meta, vault->meta, VAULT_META_LEN);
						goto out;
					}
				} else {
					LOGE("%s: rpmb key not programmed(L1)\n", __func__);
					ret = VK_ERR_RPMB_KEY_PROV;
					goto out;
				}
			}
		}

		memcpy(vault, req->preload, sizeof(unsvault_t));
		ret = VK_SUCCESS;
	} else {
		LOGE("%s: Invalid vault_level type, something went wrong in vtab(%d)\n", __func__, req->vault_level);
		ret = VK_ERR_INVALID_ARGUMENT;
		goto out;
	}

	memcpy(meta, vault->meta, VAULT_META_LEN);

out:
	if (vault != NULL) {
		memset((uint8_t*)vault, 0, sizeof(unsvault_t));
		target->platform.memory_free(vault);
	}

	if (ret != VK_SUCCESS && ret != VK_ERR_RPMB_KEY_PROV) {
		LOGE("%s: Failed to get meta data(%d)\n", __func__, ret);
		return ret;
	}

	if (ret == VK_ERR_RPMB_KEY_PROV) {
		(*result) = META_FUSE_NOT_BLOWN;
	} else if (ret == VK_SUCCESS && meta[type] == META_FUSE_BLOWN) {
		(*result) = META_FUSE_BLOWN;
	} else if (ret == VK_SUCCESS && meta[type] == META_FUSE_NOT_BLOWN) {
		(*result) = META_FUSE_NOT_BLOWN;
	}

	return VK_SUCCESS;
}