#include <stdbool.h>
#include <qsee_services.h>
#include <qsee_log.h>
#include <qsee_heap.h>
#include <qsee_core.h>
#include <qsee_stor.h>
#include <bksecapp_common.h>

#define UINT8_A uint8_t __attribute__ ((aligned(128)))

static UINT8_A rpmb_buffer[TZ_RPMB_BKSECAPP_BLOCK_SIZE] = { 0 };

qsee_stor_device_handle_t gDevHandle = 0;
qsee_stor_client_handle_t gClientHandle = 0;
qsee_stor_client_info_t gClientInfo;
qsee_stor_device_info_t device_info;
uint32_t num_sectors = 0;

int32_t bksecapp_rpmb_init(void)
{
	int32_t ret = QSEE_STOR_SUCCESS;

	ret = qsee_stor_device_init(QSEE_STOR_EMMC_RPMB, 0, &gDevHandle);
	if (QSEE_STOR_SUCCESS != ret) {
		qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to init RPMB device(%d)\n", __func__, ret);
		return ret;
	}

	ret = qsee_stor_device_get_info(&gDevHandle, &device_info);
	if (QSEE_STOR_SUCCESS != ret) {
		qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to get device info RPMB device(%d)\n", __func__, ret);
		return ret;
	}

	num_sectors = TZ_RPMB_BKSECAPP_BLOCK_SIZE/device_info.bytes_per_sector;
	if (TZ_RPMB_BKSECAPP_BLOCK_SIZE%device_info.bytes_per_sector)
		num_sectors += 1;

	ret = qsee_stor_open_partition(&gDevHandle, TZ_RPMB_BKSECAPP_PARITION_ID, &gClientHandle);
	if (QSEE_STOR_PARTI_NOT_FOUND_ERROR == ret) {
		qsee_log(QSEE_LOG_MSG_ERROR,"%s: There is no partition for bksecapp, add it.\n", __func__);
		qsee_log(QSEE_LOG_MSG_ERROR,"%s: Total RPMB sectors (%u), Available RPMB sectors (%u)\n", __func__,
			 device_info.total_sectors, device_info.available_sectors);
		if(num_sectors < device_info.available_sectors)
		{
			qsee_log(QSEE_LOG_MSG_ERROR,"%s: Total RPMB sectors (%u), Available RPMB sectors (%u)\n", __func__,
				 device_info.total_sectors, device_info.available_sectors);
			num_sectors = (num_sectors < device_info.available_sectors)
				 ? num_sectors : device_info.available_sectors;
		}

		ret = qsee_stor_add_partition(&gDevHandle, TZ_RPMB_BKSECAPP_PARITION_ID, num_sectors);
		if(QSEE_STOR_SUCCESS != ret)
		{
			qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to make new partition(%d)\n", __func__, ret);
			return ret;
		}

		ret = qsee_stor_open_partition(&gDevHandle, TZ_RPMB_BKSECAPP_PARITION_ID ,&gClientHandle);
		if ((QSEE_STOR_SUCCESS != ret) || (gClientHandle == 0))
		{
			qsee_log(QSEE_LOG_MSG_ERROR,"%s: Fail to open() return=0x%x", __func__, ret);
			return BKERR_RPMB_ADD_OPEN_FAILURE;
		}
		qsee_log(QSEE_LOG_MSG_ERROR,"%s: Successfully open partition", __func__);
	} else if (ret != QSEE_STOR_SUCCESS) {
		qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to open partition(%d)\n", __func__, ret);
		return ret;
	}

	return ret;
}

int32_t bksecapp_rpmb_read_to_buffer(void *buffer)
{
	int32_t ret = QSEE_STOR_SUCCESS;

	ret = bksecapp_rpmb_init();
	if(ret)
	{
		qsee_log(QSEE_LOG_MSG_ERROR,"%s: bksecapp_rpmb_init fail 0x%x", __func__, ret);
		return ret;
	}

	ret = qsee_stor_read_sectors(&gClientHandle, 0, num_sectors, buffer);
	if (ret != QSEE_STOR_SUCCESS) {
		qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to read data from rpmb(0x%08x) - try again\n", __func__, ret);
		ret = qsee_stor_read_sectors(&gClientHandle, 0, num_sectors, buffer);
		if (ret != QSEE_STOR_SUCCESS) {
			qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to read data from rpmb(0x%08x) - twice\n", __func__, ret);
			return ret;
		}
	}

	return ret;
}

int32_t bksecapp_rpmb_read(rpmb_data_t *rsp_buffer)
{
	int32_t ret = QSEE_STOR_SUCCESS;
	rpmb_data_t* temp_read_buffer = (rpmb_data_t*)&rpmb_buffer;

	ret = bksecapp_rpmb_read_to_buffer(temp_read_buffer);
	if (ret != QSEE_STOR_SUCCESS) {
		qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to read data from rpmb to local buffer(0x%08x)\n", __func__, ret);
		return ret;
	}

	memcpy(rsp_buffer,temp_read_buffer,TZ_RPMB_BKSECAPP_BLOCK_SIZE);

	qsee_log(QSEE_LOG_MSG_ERROR,"%s: bksecapp_rpmb_read success", __func__, ret);
	return ret;
}

int32_t bksecapp_rpmb_write(rpmb_data_t *req_buffer)
{
	int32_t ret = QSEE_STOR_SUCCESS;
	rpmb_data_t* temp_read_buffer = (rpmb_data_t*)&rpmb_buffer;

	ret = bksecapp_rpmb_read_to_buffer(temp_read_buffer);
	if(ret)
	{
		qsee_log(QSEE_LOG_MSG_ERROR,"%s: bksecapp_rpmb_read fail 0x%x", __func__, ret);
		return ret;
	}

	if (req_buffer->rpmb_cmd == TZ_RPMB_BKSECAPP_CMD_COREDATA)
	{
		if(req_buffer->store_size == TZ_RPMB_BKSECAPP_COREDATA_SIZE)
		{
			memcpy(&temp_read_buffer->CMdata,&req_buffer->CMdata,TZ_RPMB_BKSECAPP_COREDATA_SIZE);
			ret = qsee_stor_write_sectors(&gClientHandle, 0, num_sectors,(void *)temp_read_buffer);
			if(ret != QSEE_STOR_SUCCESS)
			{
				qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to write data to rpmb(0x%x) - try again\n", __func__, ret);
				ret = qsee_stor_write_sectors(&gClientHandle, 0, num_sectors,(void *)temp_read_buffer);
				if (ret != QSEE_STOR_SUCCESS) {
					qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to write data to rpmb(%x) twice\n", __func__, ret);
					return ret;
				}
				return ret;
			}
		}
		else
		{
			qsee_log(QSEE_LOG_MSG_ERROR,"%s: Invalid transfer size expect:%d size:%d" \
				,__func__,TZ_RPMB_BKSECAPP_COREDATA_SIZE,req_buffer->store_size);
			return BKERR_RPMB_MISMATCH_STORE_SIZE;
		}
	} 
	else if (req_buffer->rpmb_cmd == TZ_RPMB_BKSECAPP_CMD_RP_DATA)
	{
		if(req_buffer->store_size == TZ_RPMB_BKSECAPP_RPDATA_SIZE)
		{
			memcpy(&temp_read_buffer->RPdata,&req_buffer->RPdata,TZ_RPMB_BKSECAPP_RPDATA_SIZE);
			ret = qsee_stor_write_sectors(&gClientHandle, 0, num_sectors,(void *)temp_read_buffer);
			if(ret != QSEE_STOR_SUCCESS)
			{
				qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to write data to rpmb(0x%x) - try again\n", __func__, ret);
				ret = qsee_stor_write_sectors(&gClientHandle, 0, num_sectors,(void *)temp_read_buffer);
				if (ret != QSEE_STOR_SUCCESS) {
					qsee_log(QSEE_LOG_MSG_ERROR,"%s: Failed to write data to rpmb(%x) twice\n", __func__, ret);
					return ret;
				}
				return ret;
			}
		}
		else
		{
			qsee_log(QSEE_LOG_MSG_ERROR,"%s: Invalid transfer size expect:%d size:%d" \
				,__func__,TZ_RPMB_BKSECAPP_RPDATA_SIZE,req_buffer->store_size);
			return BKERR_RPMB_MISMATCH_STORE_SIZE;
		}
	}
	else 
	{
		return BKERR_RPMB_NOT_FOUND_CMD;
	}

	qsee_log(QSEE_LOG_MSG_ERROR,"%s: bksecapp_rpmb_write success", __func__, ret);
	return ret;
}