#include <string.h>
#include "QSEE_TZ_debug.h"
#include "qsee_stor.h"
#include "rpmb_storage.h"

#define TZ_RPMB_ARCOUNTER_PARITION_ID  0x8000F003

qsee_stor_device_handle_t gDevHandle = 0;
qsee_stor_client_handle_t gClientHandle = 0;
qsee_stor_client_info_t   gClientInfo;

int arcounter_rpmb_init()
{
  int ret = 0;//SUCCESS;
  uint32_t num_sectors = TZ_RPMB_ARCOUNTER_NUM_SECTOR_MAX;

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

  ret = qsee_stor_open_partition(&gDevHandle, TZ_RPMB_ARCOUNTER_PARITION_ID, &gClientHandle);
  if (QSEE_STOR_PARTI_NOT_FOUND_ERROR == ret) {
      qsee_stor_device_info_t device_info;

    TTY_LOG("%s: There is no partition for secure counter, add it.\n", __func__);
    ret = qsee_stor_device_get_info(&gDevHandle, &device_info);
	if (ret != QSEE_STOR_SUCCESS) {
		TTY_LOG("%s: Failed to get device info(%d)\n", __func__, ret);
		num_sectors = TZ_RPMB_ARCOUNTER_NUM_SECTOR_MAX;
	}
	else {
		TTY_LOG("%s: Total RPMB sectors (%u), Available RPMB sectors (%u)\n", __func__,
			 device_info.total_sectors, device_info.available_sectors);
		num_sectors = (TZ_RPMB_ARCOUNTER_NUM_SECTOR_MAX < device_info.available_sectors)
					 ? TZ_RPMB_ARCOUNTER_NUM_SECTOR_MAX : device_info.available_sectors;
	}

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

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

  }
  return ARCOUNTER_RPMB_SUCCESS;
}

int arcounter_rpmb_read(uint32_t sector_pos, uint8_t *data)
{
	int ret;

    ret = qsee_stor_read_sectors(&gClientHandle, sector_pos, 1, data);
    if (ret != QSEE_STOR_SUCCESS) {
        TTY_LOG("%s: Failed to read data from rpmb(0x%08x)\n", __func__, ret);
		return ARCOUNTER_RPMB_FAILURE;
    }

    return ARCOUNTER_RPMB_SUCCESS;
}

int arcounter_rpmb_write(uint32_t sector_pos, uint8_t *data)
{
    int ret;

    ret = qsee_stor_write_sectors(&gClientHandle, sector_pos, 1, data);
    if (ret != QSEE_STOR_SUCCESS) {
        TTY_LOG("%s: Failed to write data to rpmb(%d)\n", __func__, ret);
		return ARCOUNTER_RPMB_FAILURE;
    }

    return ARCOUNTER_RPMB_SUCCESS;
}


int arcounter_rpmb_set_flag(uint8_t flag)
{
  int ret;
  uint8_t curr_flag = 0x0;
  uint8_t buffer[TZ_RPMB_ARCOUNTER_BLOCK_SIZE] = {0,};

  TTY_LOG("arcounter flag : 0x%x", flag);
  ret = qsee_stor_read_sectors(&gClientHandle, 0, 1, buffer);
  if (ret != QSEE_STOR_SUCCESS) {
    TTY_LOG("%s: Failed to read data from rpmb(0x%08x)\n", __func__, ret);
    return ARCOUNTER_RPMB_FAILURE;
  }

  memcpy(&curr_flag, buffer+ENABLE_FLAG_OFFSET, ENABLE_FLAG_SIZE);
  if (curr_flag == flag) {
    TTY_LOG("flag is already set: %d", flag);
    return ARCOUNTER_RPMB_SUCCESS;
  }
  if (flag == 0x0) {
    memset(buffer, 0x0, TZ_RPMB_ARCOUNTER_BLOCK_SIZE);
  }
  memcpy(buffer+ENABLE_FLAG_OFFSET, &flag, ENABLE_FLAG_SIZE);

  ret = qsee_stor_write_sectors(&gClientHandle, 0, 1, buffer);
  if (QSEE_STOR_SUCCESS != ret) {
    TTY_LOG("Failed to write RPMB : 0x%x", ret);
    return ARCOUNTER_RPMB_FAILURE;
  }

  TTY_LOG("arcounter_rpmb_set_flag successful : %d", flag);
  return ARCOUNTER_RPMB_SUCCESS;
}

int arcounter_check_flag(uint8_t *flag)
{
  int ret;
  uint8_t buffer[TZ_RPMB_ARCOUNTER_BLOCK_SIZE] = {0,};

  ret = qsee_stor_read_sectors(&gClientHandle, 0, 1, buffer);
  if (ret != QSEE_STOR_SUCCESS) {
      TTY_LOG("%s: Failed to read data from rpmb(0x%08x)\n", __func__, ret);
      return ARCOUNTER_RPMB_FAILURE;
  }

  memcpy(flag, buffer+ENABLE_FLAG_OFFSET, ENABLE_FLAG_SIZE);
  TTY_LOG("arcounter flag : 0x%x", *flag);

  return ARCOUNTER_RPMB_SUCCESS;
}