#include "al/sec.h"
#include "sb_hsm_drv.h"
#include "macro.h"
#include "stdio.h"
#include "memory_footprint.h"
#include "ssm.h"
#include "stdlib.h"
#include "sb_hsm_external.h"

__attribute__((section (".external"))) const struct hsm_op_s hsm_op = {
  .enable = sb_hsm_enable,
  .disable = sb_hsm_disable,
  .enable_int = sb_hsm_enable_int,
  .disable_int = sb_hsm_disable_int,
  .pspe_init = sb_hsm_pspe_init,
  .pspe_delete = sb_hsm_pspe_delete,
  .sp_init = sb_hsm_sp_init,
  .sp_delete = sb_hsm_sp_delete,
  .init_page_integrity = sb_hsm_init_page_integrity,
  .writeGroup = sb_hsm_writeGroup,
  .prewriteGroup = sb_hsm_prewriteGroup,
  .completedwriteGroup = sb_hsm_completedwriteGroup,
  .prereadGroup = sb_hsm_prereadGroup,
  .completedreadGroup = sb_hsm_completedreadGroup,
  .getstatus = sb_hsm_getstatus
};

extern struct memory_footprint_s *memory_footprint;

static struct ssm_context_s *ssm_ctx = NULL;

static inline uint32_t pow(uint32_t a, uint32_t b)
{
  size_t i;
  uint32_t res = 1;

  for (i=0;i<b;i++)
    res *= a;

  return res;
}

static inline addr_t getPSPEAddress(addr_t addr, enum sb_page_size_e size)
{
  uint32_t offset = addr - ssm_ctx->ram_p_addr;

  return ssm_ctx->mb_addr + (offset / (0x1000*(pow(0x10,3-size))))*0x8;
}

static inline void ssm_wait()
{
  while(sb_hsm_getstatus()&0x1)
	 ;
}

void ssm_init(addr_t mb_addr, addr_t ram_p_addr, size_t ram_p_size)
{
  if (ssm_ctx == NULL)
    ssm_ctx = mem_get_block(sizeof(struct ssm_context_s));

  ssm_ctx->mb_addr = mb_addr;
  ssm_ctx->ram_p_addr = ram_p_addr;
  ssm_ctx->ram_p_size = (ram_p_size+1)*0x1000000;
  ssm_ctx->sp_idx = 0;

  uint32_t c_key[6] = {0x12345678, 0x9abcdef0, 0x23456789, 0x3a2c1af8, 0x8796413d, 0x9a4b68ce};
  uint32_t i_key[6] = {0x664171cd, 0xcca54bc6, 0xc1b8ea18, 0x78fbca98, 0x6a4fb648, 0x6469aebc};

  sb_hsm_init(mb_addr, ram_p_addr, ram_p_size, c_key, i_key);
  
  ssm_wait();
  sb_hsm_init_page_integrity(0x91000000, true);
  ssm_wait();
  sb_hsm_init_page_integrity(0x91010000, false);
  ssm_wait();
  sb_hsm_init_page_integrity(0x91020000, false);
  ssm_wait();

}

void ssm_del_sp(size_t idx)
{
  paddr_t sp_addr = ssm_ctx->mb_addr + (ssm_ctx->ram_p_size/0x1000)*0x8 + idx*64;

  sb_hsm_sp_delete(sp_addr);

  ssm_wait();
}

void ssm_add_sp(size_t idx, bool with_integrity, bool with_confidentiality, enum sp_mode_e mode)
{
  uint32_t ckey[6] = {1,2,3,4,5,6};
  uint32_t ikey[6] = {7,8,9,10,11,12};

  addr_t sp_addr = ssm_ctx->mb_addr + (ssm_ctx->ram_p_size/0x1000)*0x8 + idx*64;

  if (mode == sp_rw)
  {
    ssm_wait();

    enum sb_confidentiality_mode_e _c = (with_confidentiality) ? sb_sp_c_cbc : sb_sp_c_none;
    enum sb_integrity_mode_e _i = (with_integrity) ? sb_sp_i_mact : sb_sp_i_none;

    sb_hsm_sp_init(sp_addr, _c, _i, ckey, ikey);		   

    ssm_wait();
  }
  else
  {
    ssm_wait();

    sb_hsm_sp_init(sp_addr, (with_confidentiality) ? sb_sp_c_otp : sb_sp_c_none, 
		   (with_integrity) ? sb_sp_i_mac : sb_sp_i_none,
		   ckey, ikey);		   
    ssm_wait();
  }

  ssm_del_sp(idx+1);
}

uint32_t ssm_get_sp(size_t idx)
{
  addr_t sp_addr = ssm_ctx->mb_addr + (ssm_ctx->ram_p_size/0x1000)*0x8 + idx*64;

  ssm_wait();

  sb_hsm_prereadGroup(sp_addr);

  ssm_wait();

  uint32_t ret[8];
  sb_hsm_completedreadGroup((uintptr_t)&ret);

  return ret[0];
}

bool get_new_mac_page(uint32_t *return_addr)
{
  uint32_t integrity_addr; 
  bool new_page = false;
  
  *return_addr = memory_footprint->slave_mac_addr;
  integrity_addr = *return_addr;

  if (ALIGN_VALUE_DOWN(integrity_addr, 0x1000) == integrity_addr)
    new_page = true;

  if (ALIGN_VALUE_DOWN(integrity_addr, 0x1000) == ALIGN_VALUE_DOWN(memory_footprint->slave_ht_addr, 0x1000))
  {
    memory_footprint->slave_ht_addr += 0x1000;
  }

  if ((integrity_addr & 0xFFF) == 0xC00)
  {
    if (integrity_addr < memory_footprint->slave_ht_addr)
    {
      uint32_t addr = ALIGN_VALUE_UP(memory_footprint->slave_ht_addr, 0x1000);
      integrity_addr = (addr == memory_footprint->slave_ht_addr) ? addr + 0x1000 : addr;
    }
    else
    {
      integrity_addr += 0x400;
    }
  }
  else
  {
    integrity_addr += 0x400;
  }

  memory_footprint->slave_mac_addr = integrity_addr;

  return new_page;
}

bool get_new_ht_page(uint32_t *return_addr)
{
  uint32_t integrity_addr; 
  bool new_page = false;
  
  *return_addr = memory_footprint->slave_ht_addr;
  integrity_addr = *return_addr;

  if (ALIGN_VALUE_DOWN(integrity_addr, 0x1000) == (integrity_addr-0x400))
    new_page = true;

  if (ALIGN_VALUE_DOWN(integrity_addr, 0x1000) == ALIGN_VALUE_DOWN(memory_footprint->slave_mac_addr, 0x1000))
  {
    memory_footprint->slave_mac_addr += 0x1000;
  }

  if ((integrity_addr & 0xFFF) == 0xC00)
  {
    if (integrity_addr < memory_footprint->slave_mac_addr)
    {
      uint32_t addr = ALIGN_VALUE_UP(memory_footprint->slave_mac_addr, 0x1000);
      integrity_addr = (addr == memory_footprint->slave_mac_addr) ? addr + 0x1000 : addr;
      integrity_addr += 0x400;
    }
    else
    {
      integrity_addr += 0x800;
    }
  }
  else
  {
    integrity_addr += 0x400;
  }

  memory_footprint->slave_ht_addr = integrity_addr;

  return new_page;
}

void ssm_create_segment(uint32_t sp, uint32_t start_address, size_t size)
{
  size_t i;
  uint32_t curr_addr;
  uint32_t integrity_addr;
  bool is_new_page;

  uint32_t sp_header = ssm_get_sp(sp);

  for (i=0;i<size/0x1000;i++)
  {
    curr_addr = start_address+i*0x1000;

    if ((sp_header & 0x4) == 0x4) // HT
    {
      is_new_page = get_new_ht_page(&integrity_addr);

      sb_hsm_pspe_init(getPSPEAddress(curr_addr, sb_p_size_4k), 
		       integrity_addr, (integrity_addr - (integrity_addr/0x1000)*0x1000)/0x400, 
		       0, 0, sp, ((sp_header&0x1E)==0)?true:false, sb_p_size_4k);
      ssm_wait();
		       
      if (is_new_page)
      {
	sb_hsm_pspe_init(getPSPEAddress(integrity_addr, sb_p_size_4k), 0, 0, 0, 0, 0, 0, sb_p_size_4k); 
	ssm_wait();
      }

      sb_hsm_init_page_integrity(curr_addr, is_new_page);
      ssm_wait();
    }
    else if ((sp_header & 0x2) == 0x2) // MAC
    {
      is_new_page = get_new_mac_page(&integrity_addr);

      sb_hsm_pspe_init(getPSPEAddress(curr_addr, sb_p_size_4k),
		       integrity_addr, (integrity_addr - (integrity_addr/0x1000)*0x1000)/0x400,
		       0, 0, sp, ((sp_header&0x1E)==0)?true:false, sb_p_size_4k);
      ssm_wait();

      if (is_new_page)
      {
	sb_hsm_pspe_init(getPSPEAddress(integrity_addr, sb_p_size_4k), 0, 0, 0, 0, 0, 0, sb_p_size_4k); 
	ssm_wait();
      }
    }
    else // no integrity
    {
      sb_hsm_pspe_init(getPSPEAddress(curr_addr, sb_p_size_4k), 
		       0, 0, 0, 0, sp, ((sp_header&0x1E)==0)?true:false, sb_p_size_4k);

      ssm_wait();
    }
  }
}
