/*
    This file is part of MutekH.
    
    MutekH is free software; you can redistribute it and/or modify it
    under the terms of the GNU Lesser General Public License as
    published by the Free Software Foundation; version 2.1 of the
    License.
    
    MutekH is distributed in the hope that it will be useful, but
    WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.
    
    You should have received a copy of the GNU Lesser General Public
    License along with MutekH; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
    02110-1301 USA.

    Copyright Jeremie Brunel <jeremie.brunel@telecom-paristech.fr> (c) 2013
*/

#include <libssm/libssm.h>
#include <mutek/mem_alloc.h>
#include <hexo/iospace.h>
#include <mutek/printk.h>
#include <stdbool.h>
#include <libssm/sb_hsm_types.h>
#include <device/driver.h>
#include <libssm/hsm.h>
#include <stdio.h>

struct sb_ssm_context_s
{
  paddr_t p_addr;
  size_t p_size;
  paddr_t mb_addr;

  paddr_t slave_ht_addr;
  paddr_t slave_mac_addr;

  struct device_hsm_s hsm_dev;
  struct sp_s *last_sp;
};

static struct sb_ssm_context_s *sb_ctx;

static inline paddr_t getSPAddress(uint32_t idx)
{
  paddr_t sp_ba = sb_ctx->mb_addr + (sb_ctx->p_size / 0x1000) * 8;

  return sp_ba + idx*64;
}

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 uint32_t getPSPEAddress(uint32_t addr, enum sb_page_size_e size)
{
  uint32_t offset = addr - sb_ctx->p_addr;

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

error_t sb_ssm_init(
  uint32_t ram_p_addr,
  size_t ram_p_size, 
  uint32_t mb_addr)
{
  sb_ctx = mem_alloc(sizeof(struct sb_ssm_context_s), mem_scope_default);

  sb_ctx->mb_addr = mb_addr;
  sb_ctx->p_addr = ram_p_addr;
  sb_ctx->p_size = ram_p_size;

  sb_ctx->slave_ht_addr = 0x81000400;
  sb_ctx->slave_mac_addr = 0x81000000;


  sb_ctx->last_sp = mem_alloc(sizeof(struct sp_s), mem_scope_default);
  memset(sb_ctx->last_sp, 0, sizeof(struct sp_s));

  return device_get_accessor_by_path(&sb_ctx->hsm_dev, NULL, "/fdt/secbus@0", DRIVER_CLASS_HSM);
}

static struct sp_s *sb_ssm_new_policy(bool_t c, bool_t i, enum hsm_prot_mode_e mode, uint16_t idx)
{
  uint32_t keyc[6];
  uint32_t keyi[6];
  uint_fast8_t j;
  for (j=0;j<6;j++)
  {
    keyc[j] = rand();
    keyi[j] = rand();
  }

  struct sp_s *sp = sb_ctx->last_sp;

  dev_hsm_lock_new_sp(&sb_ctx->hsm_dev, getSPAddress(idx), c, i, mode, keyc, keyi);
  dev_hsm_lock_delete_sp(&sb_ctx->hsm_dev, getSPAddress(idx+1));

  sp->c = c;
  sp->i = i;
  sp->mode = mode;
  sp->idx = idx;

  return sp;
}

struct sp_s* sb_ssm_get_policy(bool_t c, bool_t i, enum hsm_prot_mode_e mode, bool_t new)
{
  struct sp_s *sp = sb_ctx->last_sp;

  if (!new)
  {
    if (sp->c == c && sp->i == i && sp->mode == mode)
      return sp;
  }

  uint32_t sp_0[8];
  uint32_t idx = 0;
  while (1)
  {
    paddr_t sp_addr = getSPAddress(idx);
    if (dev_hsm_lock_read(&sb_ctx->hsm_dev, sp_addr, (uintptr_t)sp_0))
    {
      printk("cannot read sp %u at @%x\n", idx, sp_addr);
      return NULL;
    }
    
    if (sp_0[0]&0x1)
    {
      if (new)
	goto inc;

      uint8_t imode = (sp_0[0]>>1)&0x3;
      uint8_t cmode = (sp_0[0]>>3)&0x3;
      bool_t t_c = (cmode == 0)?false:true;
      bool_t t_i = (imode == 0)?false:true;
      enum hsm_prot_mode_e t_mode;

      if (imode == 2 || cmode == 2) // rw mode
      {
	t_mode = HSM_PROT_RW;
      }
      else
      {
	t_mode = HSM_PROT_RO;
      }

      if (c == t_c && i == t_i && t_mode == mode)
      {
	sp->c = t_c;
	sp->i = t_i;
	sp->mode = t_mode;
	sp->idx = idx;

	return sp;
      }	
    }
    else
    {
      break;
    }
  inc:
    idx++;
  }

  return sb_ssm_new_policy(c, i, mode, idx);  
}

#define GET_INTEGRITY_ADDR(type)					\
  ((type == HSM_PROT_RW) ? sb_ctx->slave_ht_addr : sb_ctx->slave_mac_addr)

#define ALIGN_INTEGRITY_ADDR(type, addr)		\
  ((type == HSM_PROT_RW) ? (addr - 0x400) : (addr))

#define ADD_INTEGRITY_ADDR(type, value); \
{                                        \
  if (type == HSM_PROT_RW)		 \
    sb_ctx->slave_ht_addr += value;	 \
  else                                   \
    sb_ctx->slave_mac_addr += value;	 \
}

#define ASSIGN_INTEGRITY_ADDR(type, value); \
{                                           \
  if (type == HSM_PROT_RW)		    \
    sb_ctx->slave_ht_addr = value;	    \
  else                                      \
    sb_ctx->slave_mac_addr = value;	    \
}

static bool_t __get_new_integrity_page(uint32_t *addr, enum hsm_prot_mode_e mode)
{
  uint32_t integrity_addr; 
  bool_t new_page = false;
  
  *addr = GET_INTEGRITY_ADDR(mode);
  integrity_addr = *addr;

  if (ALIGN_VALUE_LOW(integrity_addr, 0x1000) == ALIGN_INTEGRITY_ADDR(mode, integrity_addr))
    new_page = true;

  if (ALIGN_VALUE_LOW(integrity_addr, 0x1000) == ALIGN_VALUE_LOW(GET_INTEGRITY_ADDR(mode), 0x1000))
  {
    ADD_INTEGRITY_ADDR(mode, 0x1000);
  }

  if ((integrity_addr & 0xFFF) == 0xC00)
  {
    if (integrity_addr < GET_INTEGRITY_ADDR(mode))
    {
      uint32_t addr = ALIGN_VALUE_UP(GET_INTEGRITY_ADDR(mode), 0x1000);
      integrity_addr = (addr == GET_INTEGRITY_ADDR(mode)) ? addr + 0x1000 : addr;
      if (mode == HSM_PROT_RW)
        integrity_addr += 0x400;
    }
    else
    {
      if (mode == HSM_PROT_RW)
        integrity_addr += 0x800;
      else
        integrity_addr += 0x400;
    }
  }
  else
  {
    integrity_addr += 0x400;
  }
//  integrity_addr += 0x1000;

  ASSIGN_INTEGRITY_ADDR(mode, integrity_addr);

  return new_page;
}

size_t sb_ssm_new_protected_segment(uintptr_t start_address, size_t size, struct sp_s* sp)
{
  size_t protected_size = (size/0x1000)*0x1000;
  size_t i;
  uint32_t curr_addr, integrity_addr;
  bool_t is_new_page;
  bool_t no_prot;

  if (protected_size<size)
    protected_size += 0x1000;

  if (sp == NULL)
    no_prot = true;
  else
    no_prot = false;

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

    if (no_prot)
    {
      dev_hsm_lock_new_pspe(&sb_ctx->hsm_dev, getPSPEAddress(curr_addr, sb_p_size_4k),
			    0, 0, 0, 0, 0,
			    true, sb_p_size_4k);

    }
    else if (sp->i)
    {
      is_new_page = __get_new_integrity_page(&integrity_addr, sp->mode);
      uint8_t int_idx = (integrity_addr - (integrity_addr/0x1000)*0x1000)/0x400;

      dev_hsm_lock_new_pspe(&sb_ctx->hsm_dev, getPSPEAddress(curr_addr, sb_p_size_4k),
			    integrity_addr, int_idx, 0, 0, sp->idx,
			    false, sb_p_size_4k);
      if (is_new_page)
      {
	dev_hsm_lock_new_pspe(&sb_ctx->hsm_dev, getPSPEAddress(integrity_addr, sb_p_size_4k),
			      0, 0, 0, 0, 0, 0, sb_p_size_4k);
      }

      if (sp->mode == HSM_PROT_RW)
      {
	dev_hsm_lock_integrity(&sb_ctx->hsm_dev, curr_addr, is_new_page);
      }
    }
    else
    {
      dev_hsm_lock_new_pspe(&sb_ctx->hsm_dev, getPSPEAddress(curr_addr, sb_p_size_4k),
			    0, 0, 0, 0, sp->idx,
			    false, sb_p_size_4k);
    }
  }

  return protected_size;
}

error_t sb_ssm_cleanup_sp(struct sp_s *sp)
{
  // NOT IMPLEMENTED
  return 0;
}

error_t sb_ssm_cleanup_segment(uintptr_t start_address, size_t size)
{
  // NOT IMPLEMENTED
  return 0;
}

error_t sb_ssm_load_segment(uintptr_t src, uintptr_t dest, size_t size)
{
  size_t loaded_size = (size/0x40)*0x40;
  size_t i;

  if (loaded_size<size)
    loaded_size += 0x40;
  
  for (i=0;i<loaded_size/0x40;i++)
  {
    dev_hsm_lock_write(&sb_ctx->hsm_dev, src+i*0x40, dest+i*0x40);
  }

  return 0;
}

char *libssm_sptostr(char *str, struct sp_s *sp)
{
  sprintf(str, "idx=%d I=%s C=%s", sp->idx, (sp->mode==HSM_PROT_RO && sp->i)?"MAC":(sp->mode==HSM_PROT_RW && sp->i)?"HT":"0", (sp->mode==HSM_PROT_RO && sp->c)?"OTP":(sp->mode==HSM_PROT_RW && sp->c)?"BC":"0");
  return str;
}
