#include "sb_hsm_drv.h"
#include "hsm_reg.h"

extern uint32_t cpu_mem_read32(uintptr_t addr);
extern void cpu_mem_write32(uintptr_t addr, uint32_t data);

static uintptr_t sb_addr = NULL;

#ifdef VERIFIED_MODE
static inline sb_hsm_err_t __check_pspe_addr(addr_t pspe_addr)
{
  uint32_t reg_config = cpu_mem_read32(sb_addr + HSM_CFG_ADDR);

  if (!reg_config)
    return SB_NINIT;

  addr_t mb_addr = (HSM_CFG_MBBA_GET(reg_config)<<24); // get mb addr
  // mb size occupied by pspe = protected_size / size of smallest page * 0x8
  size_t pspe_mb_size = ((HSM_CFG_PSIZ_GET(reg_config) + 1)*0x1000000/0x1000*0x8;

  if (mb_addr > pspe_addr || pspe_addr >= (mb_addr + pspe_mb_size))
    return SB_EADDR;

  return SB_OK;
}
#endif

static inline void __sb_wait_idle()
{
  while (HSM_STATUS_BUSY_GET(cpu_mem_read32(sb_addr + HSM_STATUS_ADDR)) & HSM_STATUS_BUSY_BUSY)
    ;
}

inline reg_t sb_hsm_getstatus()
{
  return cpu_mem_read32(sb_addr + HSM_STATUS_ADDR);
}

sb_hsm_err_t sb_hsm_init_drv(uintptr_t io_addr)
{
  sb_addr = io_addr;

  return SB_OK;
}

sb_hsm_err_t sb_hsm_init(addr_t mb_addr, 
			 addr_t ram_p_addr, 
			 size_t ram_p_size, 
			 uint32_t key_c[6], 
			 uint32_t key_i[6])
{
  if (sb_addr == NULL)
    return SB_NDRV;

  size_t i;
  // write confidentiality key for MB into the sp register
  for (i=0;i<6;i++)
    cpu_mem_write32(sb_addr + HSM_MIK_ADDR + 4*(i+2), key_c[i]);
  
  // write intgrity key into for MB the sp register
  for (i=0;i<6;i++)
    cpu_mem_write32(sb_addr + HSM_MCK_ADDR + 4*i, key_i[i]);

  uint32_t reg_config = 0;
  HSM_CFG_MBBA_SET(reg_config, (mb_addr>>24));
  // enable HSM
  HSM_CFG_EN_SET(reg_config, ENABLE);
  // disable interrupt
  HSM_CFG_IE_SET(reg_config, DISABLE);
  // IV not implemented yet
  HSM_CFG_IVE_SET(reg_config, DISABLE);
  // all caches enable
  HSM_CFG_PCE_SET(reg_config, ENABLE);
  HSM_CFG_SPCE_SET(reg_config, ENABLE);
  HSM_CFG_MSCE_SET(reg_config, ENABLE);
  HSM_CFG_MTCE_SET(reg_config, ENABLE);
  // IV not implemented yet
  HSM_CFG_IVCE_SET(reg_config, DISABLE);
  
  HSM_CFG_PADD_SET(reg_config, (ram_p_addr>>24));
  HSM_CFG_PSIZ_SET(reg_config, ram_p_size);

  // write the config register, HSM is now enable
  cpu_mem_write32(sb_addr + HSM_CFG_ADDR, reg_config);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_enable()
{
  if (sb_addr == NULL)
    return SB_NDRV;

  uint32_t reg_config = cpu_mem_read32(sb_addr + HSM_CFG_ADDR);

  if (!reg_config)
    return SB_NINIT;

  if (HSM_CFG_EN_GET(reg_config) == HSM_CFG_EN_ENABLE)
    return SB_ALREADY;

  HSM_CFG_EN_SET(reg_config, ENABLE);

  cpu_mem_write32(sb_addr + HSM_CFG_ADDR, reg_config);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_disable()
{
  if (sb_addr == NULL)
    return SB_NDRV;

  uint32_t reg_config = cpu_mem_read32(sb_addr + HSM_CFG_ADDR);

  if (!reg_config)
    return SB_NINIT;

  if (HSM_CFG_EN_GET(reg_config) == HSM_CFG_EN_DISABLE)
    return SB_ALREADY;

  HSM_CFG_EN_SET(reg_config, DISABLE);

  cpu_mem_write32(sb_addr + HSM_CFG_ADDR, reg_config);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_enable_int()
{
  if (sb_addr == NULL)
    return SB_NDRV;

  uint32_t reg_config = cpu_mem_read32(sb_addr + HSM_CFG_ADDR);

  if (!reg_config)
    return SB_NINIT;

  if (HSM_CFG_IE_GET(reg_config) == HSM_CFG_IE_ENABLE)
    return SB_ALREADY;

  HSM_CFG_IE_SET(reg_config, ENABLE);

  cpu_mem_write32(sb_addr + HSM_CFG_ADDR, reg_config);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_disable_int()
{
  if (sb_addr == NULL)
    return SB_NDRV;

  uint32_t reg_config = cpu_mem_read32(sb_addr + HSM_CFG_ADDR);

  if (!reg_config)
    return SB_NINIT;

  if (HSM_CFG_IE_GET(reg_config) == HSM_CFG_IE_DISABLE)
    return SB_ALREADY;

  HSM_CFG_IE_SET(reg_config, DISABLE);

  cpu_mem_write32(sb_addr + HSM_CFG_ADDR, reg_config);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_pspe_init(addr_t pspe_address, 
			      addr_t integrity_page,
			      size_t integrity_idx,
			      addr_t iv_page,
			      size_t iv_idx,
			      size_t sp_idx, 
			      bool_t unprotected, 
			      enum sb_page_size_e size)
{
  uint32_t pspe[2] = {0, 0};
  size_t block_idx = (pspe_address % 32)/sizeof(uint32_t);

  if (sb_addr == NULL)
    return SB_NDRV;

#ifdef VERIFIED_MODE
  if (__check_pspe_addr(pspe_address))
    return SB_EADDR;
#endif

  pspe[0] = 0x1; // set pspe valid
  pspe[0] |= size<<1;

  if (unprotected)
    pspe[0] |= 1<<3; // set no protection
  else
    pspe[0] |= sp_idx<<4; // set sp_idx

  pspe[0] |= (iv_idx)<<20;
  pspe[0] |= (iv_page/0x1000)<<22;
  pspe[1] |= (iv_page/0x1000)>>10;

  pspe[1] |= (integrity_idx)<<10;
  pspe[1] |= (integrity_page/0x1000)<<12;

  // write pspe into the group block
  cpu_mem_write32(sb_addr + HSM_AGRWDATA_ADDR + 4*block_idx, pspe[0]);
  cpu_mem_write32(sb_addr + HSM_AGRWDATA_ADDR + 4*(block_idx+1), pspe[1]);
  
  // write pspe addr
  cpu_mem_write32(sb_addr + HSM_AGRWADD_ADDR, pspe_address);
  // write cmd
  cpu_mem_write32(sb_addr + HSM_AGRWCMD_ADDR, HSM_AGRWCMD_CMD_WRITE);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_pspe_delete(addr_t pspe_address)
{
  size_t block_idx = (pspe_address % 32)/sizeof(uint32_t);

  if (sb_addr == NULL)
    return SB_NDRV;

#ifdef VERIFIED_MODE
  if (__check_pspe_addr(pspe_address))
    return SB_EADDR;
#endif

  // write blank pspe (valid bit = 0)
  cpu_mem_write32(sb_addr + HSM_AGRWDATA_ADDR + 4*block_idx, 0);
  cpu_mem_write32(sb_addr + HSM_AGRWDATA_ADDR + 4*(block_idx+1), 0);

  // write pspe addr
  cpu_mem_write32(sb_addr + HSM_AGRWADD_ADDR, pspe_address);
  // write cmd
  cpu_mem_write32(sb_addr + HSM_AGRWCMD_ADDR, HSM_AGRWCMD_CMD_WRITE);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_sp_init(addr_t sp_addr,
			    enum sb_confidentiality_mode_e c,
			    enum sb_integrity_mode_e i,
			    uint32_t key_c[6],
			    uint32_t key_i[6])
{
  uint32_t sp_header;
  size_t j;
  sb_hsm_err_t err;

  if (sb_addr == NULL)
    return SB_NDRV;

  sp_header = 0x1; // validity bit
  sp_header |= i<<1; // integrity type
  sp_header |= c<<3; // confidentiality type

  /* we have to disable the interruption : 1 sp need 2 operations to
     be write. As we don't want to be interrupt between those 2
     operations, we have to disable it for the first operation */
  err = sb_hsm_disable_int();

  // write the first part of the sp (header + confidentiality key)
  cpu_mem_write32(sb_addr + HSM_AGRWDATA_ADDR, sp_header);
  for (j=0;j<6;j++)
    cpu_mem_write32(sb_addr + HSM_AGRWDATA_ADDR + 4*(2+j), key_c[j]);

  // write the sp address
  cpu_mem_write32(sb_addr + HSM_AGRWADD_ADDR, sp_addr);
  // write the command
  cpu_mem_write32(sb_addr + HSM_AGRWCMD_ADDR, HSM_AGRWCMD_CMD_WRITE);

  // wait HSM 
  __sb_wait_idle();

  // restore previous interrupt state
  if (err == SB_OK)
    sb_hsm_enable_int();

  // write second part of the SP
  for (j=0;j<6;j++)
    cpu_mem_write32(sb_addr + HSM_AGRWDATA_ADDR + 4*j, key_i[j]);

  // write the sp address +0x20 (second part)
  cpu_mem_write32(sb_addr + HSM_AGRWADD_ADDR, sp_addr + 0x20);
  // write the command
  cpu_mem_write32(sb_addr + HSM_AGRWCMD_ADDR, HSM_AGRWCMD_CMD_WRITE);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_sp_delete(addr_t sp_addr)
{
  size_t i;

  if (sb_addr == NULL)
    return SB_NDRV;

  // write blank pspe (only the first part of the sp is deleted)
  for (i=0;i<8;i++)
    cpu_mem_write32(sb_addr + HSM_AGRWDATA_ADDR + 4*i, 0x0);

  // write sp address
  cpu_mem_write32(sb_addr + HSM_AGRWADD_ADDR, sp_addr);
  // write command
  cpu_mem_write32(sb_addr + HSM_AGRWCMD_ADDR, HSM_AGRWCMD_CMD_WRITE);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_init_page_integrity(addr_t address_page, bool new)
{
  if (sb_addr == NULL)
    return SB_NDRV;

  /* address is the address of the protected page, the HSM fetch in
     first, the security context relies to the page and can find the
     integrity page */
  cpu_mem_write32(sb_addr + HSM_AGRWADD_ADDR, address_page);
  // write cmd : if it's a new page, write SB_INIT_CMD else SB_CONTINUE_CMD
  cpu_mem_write32(sb_addr + HSM_AGRWCMD_ADDR, (new) ? HSM_AGRWCMD_CMD_INIT : HSM_AGRWCMD_CMD_CONTINUE);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_writeGroup(addr_t dest, uintptr_t src)
{
  sb_hsm_err_t err = sb_hsm_prewriteGroup(src);

  if (err)
    return err;

  return sb_hsm_completedwriteGroup(dest);
}

sb_hsm_err_t sb_hsm_completedwriteGroup(addr_t dest)
{
  if (sb_addr == NULL)
    return SB_NDRV;

  // write the address block
  cpu_mem_write32(sb_addr + HSM_AGRWADD_ADDR, dest);
  // store the block
  cpu_mem_write32(sb_addr + HSM_AGRWCMD_ADDR, HSM_AGRWCMD_CMD_WRITE);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_prewriteGroup(uintptr_t src)
{
  size_t i;
  volatile uint32_t *my_src = (volatile uint32_t*)src;

  if (sb_addr == NULL)
    return SB_NDRV;

  // write the block data
  for (i=0;i<8;i++)
    cpu_mem_write32(sb_addr + HSM_AGRWDATA_ADDR + 4*i, my_src[i]);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_prereadGroup(addr_t src)
{
  if (sb_addr == NULL)
    return SB_NDRV;

  cpu_mem_write32(sb_addr + HSM_AGRWADD_ADDR, src);
  cpu_mem_write32(sb_addr + HSM_AGRWCMD_ADDR, HSM_AGRWCMD_CMD_READ);

  return SB_OK;
}

sb_hsm_err_t sb_hsm_completedreadGroup(uintptr_t dest)
{
  size_t i;
  volatile uint32_t *my_dest = (volatile uint32_t*)dest;

  for (i=0;i<8;i++)
  {
    my_dest[i] = cpu_mem_read32(sb_addr + HSM_AGRWDATA_ADDR + 4*i);
  }

  return SB_OK;
}

