/*
 * SecBus - This file is part of SecBus
 * Copyright (C) 2012 - Telecom ParisTech
 * 
 * This file must be used under the terms of the CeCILL.
 * This source file is licensed as described in the file COPYING, which
 * you should have received as part of this distribution.  The terms
 * are also available at    
 * http://www.cecill.info/licences/Licence_CeCILL_V1.1-US.txt
 *
 */

#include "secbus_lib.h"
#include "hsm_reg.h"
#include "global.h"

#include "crypto/des.h"

#include "page_size.h"

void DES(uint32_t *in, uint32_t *out, uint32_t *key, bool cipher)
{
  if (cipher)
  {
    in[0] = key[2]^in[0];
    in[1] = key[3]^in[1];
  }
  else
  {
    in[0] = key[4]^in[0];
    in[1] = key[5]^in[1];
  }

  uint64_t kne[16][2];

  union convert_u
  {
    uint32_t word[2];
    uint8_t  byte[8];
  } convert_t;

  convert_t.word[0] = key[0];
  convert_t.word[1] = key[1]&0xFFFFFF; // k1 only 56bits

  deskey(kne, convert_t.byte, !cipher);

  convert_t.word[0] = in[0];
  convert_t.word[1] = in[1];
  des(kne, convert_t.byte);

  out[0] = convert_t.word[0];
  out[1] = convert_t.word[1];

  if (!cipher)
  {
    out[0] = key[2]^out[0];
    out[1] = key[3]^out[1];
  }
  else
  {
    out[0] = key[4]^out[0];
    out[1] = key[5]^out[1];
  }
}

template<typename T>
unsigned int _ctz(T n, unsigned int p)
{
  for (unsigned int i = 0; i<p; ++i )
  {
    if ( (1<<i)&n )
      return i;
  }
  return 0;
}

template<typename T>
unsigned int _clz(T n, unsigned int p)
{
  for (unsigned int i = 0; i<p; ++i )
  {
    if ( (1<<((p-1)-i))&n )
      return i;
  }
  return 0;
}

bool isPow2(unsigned int x)
{
  return ((x != 0) && !(x & (x - 1)));
}

bool fill_64(int be, addr_t address)
{
  unsigned int i = index(aligned(address, SIZE_64_IN_BYTES), SIZE_256_IN_BYTES);
  return (((be >> i*4) & 0xFF) == 0xFF);
}

bool inclusion(addr_t address, addr_t low_address, addr_t up_address)
{
  return (address >= low_address && address < up_address);
}
bool inferior(addr_t address, addr_t up_address)
{
  return (address <= up_address);
}
bool superior(addr_t address, addr_t low_address)
{
  return (address >= low_address);
}
addr_t aligned(addr_t address, int block_size_in_bytes)
{
  return (address - address % block_size_in_bytes);
}
unsigned int index(addr_t address, int block_size_in_bytes)
{
  return ((address % block_size_in_bytes)/sizeof(int));
}

addr_t getPSPEAddressFromPageSize(
  addr_t address,
  size_t size,
  addr_t mb_address,
  addr_t b_address)
{
//  return mb_address + aligned((address-b_address), pageSize[size]);
  return mb_address + ((uint32_t)((address-b_address)/pageSize[size]))*PSPE_SIZE_IN_BYTES*(pageSize[size]/pageSize[pageLevel-1]);
}

addr_t getPSPEBaseAddress(addr_t mb_address, size_t size)
{
  return mb_address;
}

addr_t getSPBaseAddress(addr_t mb_address, size_t size)
{
  uint32_t nb_pspe = size / (pageSize[pageLevel - 1]) + 1;

  return mb_address + nb_pspe * PSPE_SIZE_IN_BYTES;  
}

addr_t getMMacTBaseAddress(addr_t mb_address, size_t size)
{
  return getPSPEBaseAddress(mb_address, size) + (getSPBaseAddress(mb_address, size) - getPSPEBaseAddress(mb_address, size))*3/2;
}

addr_t getSPAddressFromIdx(uint32_t sp_idx, addr_t mb_address, size_t size)
{
  return getSPBaseAddress(mb_address, size) + sp_idx * SP_SIZE_IN_BYTES;
}

size_t getPageLevel()
{
  return pageLevel;
}

size_t getLevelFromSize(size_t size)
{
  for (size_t i=0;i<pageLevel;i++)
  {
    if (size == pageSize[i])
      return i;
  }
  return -1;
}

size_t getPageSize(size_t level)
{
  return pageSize[level];
}

size_t getMMTSize(size_t size)
{
  size_t pspe_size = ((size + 0x1000 )/ (pageSize[pageLevel - 1]))*PSPE_SIZE_IN_BYTES;
  size_t mt_size = pspe_size*2;
  return mt_size;
//  return mt_size*3/2;
}

bool compare(uint32_t *value, uint32_t *reference, size_t size_in_bytes)
{
  return !memcmp(value, reference, size_in_bytes);
}

addr_t mt_getPageBaseAddress(addr_t address, size_t level)
{
  return address & (0xFFFFFFFF << _ctz(getPageSize(level), 32));
}

bool isMasterTree(addr_t address, addr_t mb_address, size_t size)
{
  addr_t begin = getPSPEBaseAddress(mb_address, size);

  return inclusion(address, begin, begin+getMMTSize(size));
}

addr_t mt_getParentLeavesAddress(addr_t leaf_address,
				 addr_t ht_address,
				 size_t level,
				 addr_t mb_address,
				 size_t size)
{
  uint32_t page_offset;
  if (isMasterTree(leaf_address, mb_address, size))
  {
    page_offset = leaf_address - getPSPEBaseAddress(mb_address, size);

    uint32_t return_addr = ((getMMTSize(size) >> 4) +
	    page_offset/HASH_ARITY + 
	    getMMacTBaseAddress(mb_address, size));

    return return_addr;
  }
  else
  {
    uint32_t mask = 0xFFFFFFFF >> (32 - _ctz(getPageSize(level), 32));
    page_offset = leaf_address & mask;

    return page_offset/HASH_ARITY + ht_address;
  }
}

addr_t ms_getMacAddress(addr_t address, addr_t page_address, size_t level)
{
  uint32_t mask = 0xFFFFFFFF >> (32 - _ctz(getPageSize(level), 32));
  uint32_t page_offset = address&mask;

  return page_address + page_offset/MS_SIZE_IN_BYTES*SIZE_64_IN_BYTES;
}

bool mt_isLastLevel(addr_t address, size_t level, addr_t mb_address, size_t size)
{
  addr_t page_start_address = mt_getPageBaseAddress(address, level);
  uint32_t offset = isMasterTree(address, mb_address, size) ? 
    address - getMMacTBaseAddress(mb_address, size) :
    address - page_start_address;

  return inclusion(offset, 0, 2*HASH_ARITY*SIZE_64_IN_BYTES);
}

addr_t mt_getParentAddress(addr_t node_address, size_t level, addr_t mb_address, size_t size)
{
  addr_t start_address;
  uint32_t index_64, index_256;

  if(isMasterTree(node_address, mb_address, size))
    start_address = getMMacTBaseAddress(mb_address, size);
  else
    start_address = mt_getPageBaseAddress(node_address, level);
  //64-bit block index
  index_64 = (node_address - start_address)/SIZE_64_IN_BYTES;
  //256-bit block index
  index_256 = index_64/HASH_ARITY;

  //last set
  if(mt_isLastLevel(node_address, level, mb_address, size)) 
  {
    return 0;
  }
  //next to last set
  else if (inclusion(index_64, 0x8, 0x20))
  {
    return (start_address + (index_256 - 2) * SIZE_64_IN_BYTES);
  }
  else
  {
    return (start_address + index_256 * SIZE_64_IN_BYTES);
  }
}

addr_t mt_getSetAddress(addr_t node_address, size_t level, addr_t mb_address, size_t size)
{
  return (mt_isLastLevel(node_address, level, mb_address, size)) ? 
    (node_address & 0xFFFFFFC7) :
    (node_address & 0xFFFFFFE7);
}

addr_t mt_getBlockIndex(addr_t address, size_t level, addr_t mb_address, size_t size)
{
  return (mt_isLastLevel(address, level, mb_address, size)) ? 
    (address & 0x38) >> 3 :
    (address & 0x18) >> 3;
}

addr_t mt_getBrotherAddress(addr_t node_address, size_t level, addr_t mb_address, size_t size){
  uint32_t temp = mt_isLastLevel(node_address, level, mb_address, size) ?
    HASH_ARITY + 2:
    HASH_ARITY;

  uint32_t index = (mt_getBlockIndex(node_address, level, mb_address, size) + 1) % temp;

  return (mt_getSetAddress(node_address, level, mb_address, size)
	  + (index * SIZE_64_IN_BYTES));
}



/*
addr_t get4kPageAddress(addr_t address)
{
  return (address & 0xFFFFF000);
}

addr_t get4mPageAddress(addr_t address)
{
  return (address & 0xFFC00000);
}

addr_t getFirstLevelAddress(addr_t base_address, addr_t address)
{
  //10 msb of address
  int offset = PSPE_SIZE_IN_BYTES * (address >> 22 & 0x3FF);
  return (base_address + offset);
}

addr_t getSecondLevelAddress(addr_t base_address, addr_t address)
{
  //20 msb of address
  int offset = PSPE_SIZE_IN_BYTES * (address >> 12 & 0xFFFFF);
  return (base_address + offset);
}

unsigned int getSPAddress(int SP_index)
{
  return (SP_BASE_ADDRESS + SP_SIZE_IN_BYTES * SP_index);
}

bool getValidBit(int* PSPE)
  //bit 0 of PSPE[0] 
{
  return (PSPE[0]&0x1);
}

void setValidBit(int* PSPE, bool V)
  //bit 0 of PSPE[0] 
{
  PSPE[0] = (PSPE[0] & 0xFFFFFFFE) | V;
}

PSPEtype_e getTypeBit(int* PSPE)
  //bit 2 of PSPE[0]
{
  return (PSPEtype_e)((PSPE[0]&0x4)>>2);
}

void setTypeBit(int* PSPE, PSPEtype_e T)
  //bit 2 of PSPE[0]
{
  PSPE[0] = (PSPE[0] & 0xFFFFFFFB) |(T << 2);
}
PageSize_e getSizeBit(int* PSPE)
  //bit 1 of PSPE[0]
{
  return (PageSize_e)((PSPE[0] & 0x2)>>1);
}
void setSizeBit(int* PSPE, PageSize_e PS)
  //bit 1 of PSPE[0]
{
  PSPE[0] = (PSPE[0] & 0xFFFFFFFD) | (PS << 1);
}

unsigned int getSPindex(int* PSPE)
  //bit 3 to 21 of PSPE[0]
{
  return ((PSPE[0] & 0x001FFFF8) >> 3);
}
void setSPindex(int* PSPE, int SP_index)
  //bit 3 to 21 of PSPE[0]
{
  PSPE[0] = (PSPE[0] & 0xFFE00007) | (SP_index << 3);
}

void getRHV(int* PSPE, int * RHV)
{
  for (unsigned int i = 0x0 ; i < SIZE_64_IN_WORDS; i++)
    RHV[i] =  (i == 0) ? (PSPE[i] & 0xFFFFFFF8) : PSPE[i] ;
}

void setRHV(int* PSPE, int *RHV)
{
  for (unsigned int i=0;i<SIZE_64_IN_WORDS;i++)
    PSPE[i] = (i == 0) ? ((PSPE[i] & 0x7) | (RHV[i] & 0xFFFFFFF8)) : RHV[i];
}

addr_t getBaseAddress(int* PSPE)
{
  return PSPE[1];
}
void setBaseAddress(int* PSPE, addr_t base_address)
{
  PSPE[1] = base_address;
}

confidentiality_mode_e getPM(int* SP)
  //First 2 bits of first SP word 
{
  return (confidentiality_mode_e)(SP[0] & 0x3);
}

integrity_mode_e getIM(int* SP)
  //Next 2 bits of first SP word
{
  return (integrity_mode_e)((SP[0] & 0xC)>>2);
}
void setPM(int* SP, confidentiality_mode_e PM)
  //First 2 bits of first SP word
{
  SP[0] = (SP[0] & 0xFFFFFFFC) | PM;
}

void setIM(int* SP, integrity_mode_e IM)
  //Next 2 bits of first SP word
{
  SP[0] = (SP[0] & 0xFFFFFFF3) | (IM << 2);
}

void getSK(int* SP, int* Key)
{
  unsigned int i;
  for ( i = 0x0 ; i < SP_SIZE ; i++)
    Key[i] = (i == 0) ? (SP[i] & 0xFFFFFFF0) : SP[i];
}

void setSK(int* SP, int *PSPE, int * Key)
{
  unsigned int i;
  for ( i = 0x0; i < SP_SIZE; i++)
    SP[i] = (i == 0) ? ((PSPE[i] & 0xF) + (Key[i] & 0xFFFFFFF0)) : Key[i];
}

addr_t getPageBaseAddress(addr_t address, PageSize_e ht_page_size)
{
  return (ht_page_size == SIZE_4K)? address & 0xFFFFF000 : address & 0xFFC00000; ; 
} 

bool isLastLevel(addr_t address, PageSize_e ht_page_size)
{
  addr_t page_start_address = getPageBaseAddress(address, ht_page_size);
  unsigned int offset = isMasterTree(address)? address - MHT_BASE_ADDRESS : address - page_start_address ;

  return inclusion(offset, 0, 2*HASH_ARITY*SIZE_64_IN_BYTES);	
}


addr_t getParentAddress(addr_t node_address, PageSize_e ht_page_size)
{
  addr_t start_address;
  unsigned int index_64, index_256;

  if(isMasterTree(node_address))	start_address = MHT_BASE_ADDRESS;
  else start_address = getPageBaseAddress(node_address, ht_page_size);
  //64-bit block index
  index_64 = (node_address - start_address)/SIZE_64_IN_BYTES;
  //256-bit block index
  index_256 = index_64/HASH_ARITY;

  //last set
  if(isLastLevel(node_address, ht_page_size)) return 0;
  //next to last set
  else if (inclusion(index_64, 0x8, 0x20)) return (start_address + (index_256 - 2) * SIZE_64_IN_BYTES);	
  else return (start_address + index_256 * SIZE_64_IN_BYTES);	    
} 

addr_t getParentLeavesAddress(addr_t leaf_address, addr_t ht_start_address, PageSize_e ht_page_size)
{
  unsigned int page_offset;
  if(isMasterTree(leaf_address))
  {
    page_offset = leaf_address - PSPE_BASE_ADDRESS;
    return (0x100000 + page_offset/HASH_ARITY + MHT_BASE_ADDRESS);
  }
  else 
  {
    page_offset = (ht_page_size == SIZE_4M)? (leaf_address & 0x003FFFFF) : (leaf_address & 0x00000FFF);
    return (page_offset/HASH_ARITY + ht_start_address);
  }
}

addr_t getSetAddress(addr_t node_address, PageSize_e ht_page_size)
{
  return (isLastLevel(node_address, ht_page_size) ? node_address & 0xFFFFFFC7 : node_address & 0xFFFFFFE7);
}

unsigned int getBlockIndex(addr_t address, PageSize_e ht_page_size)
{
  return (isLastLevel(address, ht_page_size) ? (address & 0x38) >> 3 : (address & 0x18) >> 3);
}

addr_t getMacAddress(addr_t address, addr_t page_address, PageSize_e page_size)
{
  unsigned int page_offset = (page_size == SIZE_4M)? (address & 0x003FFFFF) : (address & 0x00000FFF) ; 
  unsigned int mac_index = page_offset/MS_SIZE_IN_BYTES;
  return (page_address + mac_index * SIZE_64_IN_BYTES);   
} 

bool isMasterTree(addr_t address)
{
  return inclusion(address, PSPE_BASE_ADDRESS, MHT_END_ADDRESS);
}

bool isMasterBlock(addr_t address)
{
  return inclusion(address, PSPE_BASE_ADDRESS, SP_END_ADDRESS);
}

addr_t getBrotherAddress(addr_t node_address, PageSize_e ht_page_size)
{	
  unsigned int temp = isLastLevel(node_address, ht_page_size)? HASH_ARITY + 2 : HASH_ARITY;

  unsigned int index = (getBlockIndex(node_address,  ht_page_size) + 1) % temp;

  return (getSetAddress(node_address, ht_page_size) +  (index * SIZE_64_IN_BYTES));	
}

bool compare(int *value, int *reference, int size_in_bytes)
{
  return !memcmp(value, reference, size_in_bytes);
}
*/

union convert_u
{
  uint64_t dword;
  uint32_t word[2];
} convert_t;

#define CONVERT(x)				\
  uint32_t *plop = (uint32_t*)x;		\
  convert_t.word[0] = plop[0];			\
  convert_t.word[1] = plop[1];

#define RETURN(x)				\
  *x[0] = convert_t.word[0];			\
  *x[1] = convert_t.word[1];

pspe_msadd_t pspe_getMSAdd(pspe_t *pspe)
{
  CONVERT(pspe);

  return PSPE_GET(&convert_t.dword, MSADD)*0x1000;
}

void pspe_setMSAdd(pspe_t *pspe, pspe_msadd_t msadd)
{
  CONVERT(pspe);

  PSPE_SET(&convert_t.dword, MSADD, msadd);

  RETURN(pspe);
}

pspe_msidx_t pspe_getMSIdx(pspe_t *pspe)
{
  CONVERT(pspe);

  return PSPE_GET(&convert_t.dword, MSIDX);
}

void pspe_setMSIdx(pspe_t *pspe, pspe_msidx_t msidx)
{
  CONVERT(pspe);

  PSPE_SET(&convert_t.dword, MSIDX, msidx);

  RETURN(pspe);
}

pspe_ivadd_t pspe_getIVAdd(pspe_t *pspe)
{
  CONVERT(pspe);

  return PSPE_GET(&convert_t.dword, IVADD);
}

void pspe_setIVAdd(pspe_t *pspe, pspe_ivadd_t ivadd)
{
  CONVERT(pspe);

  PSPE_SET(&convert_t.dword, IVADD, ivadd);

  RETURN(pspe);
}

pspe_ividx_t pspe_getIVIdx(pspe_t *pspe)
{
  CONVERT(pspe);

  return PSPE_GET(&convert_t.dword, IVIDX);
}

void pspe_setIVIdx(pspe_t *pspe, pspe_ividx_t ividx)
{
  CONVERT(pspe);

  PSPE_SET(&convert_t.dword, IVIDX, ividx);

  RETURN(pspe);
}

pspe_spidx_t pspe_getSPIdx(pspe_t *pspe)
{
  CONVERT(pspe);

  return PSPE_GET(&convert_t.dword, SPIDX);
}

void pspe_setSPIdx(pspe_t *pspe, pspe_spidx_t spidx)
{
  CONVERT(pspe);

  PSPE_SET(&convert_t.dword, SPIDX, spidx);

  RETURN(pspe);
}

pspe_size_t pspe_getSize(pspe_t *pspe)
{
  CONVERT(pspe);

  return (pspe_size_t)PSPE_GET(&convert_t.dword, SIZE);
}

void pspe_setSize(pspe_t *pspe, pspe_size_t size)
{
  CONVERT(pspe);

  PSPE_SET(&convert_t.dword, SIZE, size);

  RETURN(pspe);
}

pspe_prot_t pspe_getProt(pspe_t *pspe)
{
  CONVERT(pspe);

  return (pspe_prot_t)PSPE_GET(&convert_t.dword, PROT);
}

void pspe_setProt(pspe_t *pspe, pspe_prot_t prot)
{
  CONVERT(pspe);

  PSPE_SET(&convert_t.dword, PROT, prot);

  RETURN(pspe);
}

pspe_valid_t pspe_getValid(pspe_t *pspe)
{
  CONVERT(pspe);

  return (pspe_valid_t)PSPE_GET(&convert_t.dword, VALID);
}

void pspe_setValid(pspe_t *pspe, pspe_valid_t valid)
{
  CONVERT(pspe);

  PSPE_SET(&convert_t.dword, VALID, valid);

  RETURN(pspe);
}

void pspe_getMAC(pspe_t *pspe, pspe_mac_t *mac)
{
  size_t i;
  for (i=0;i<MAC_SIZE_IN_WORDS;i++)
    *mac[i] = (i==0) ? (*pspe[0]&0xFFFFFFF8) : (*pspe[i]);
}

void pspe_setMAC(pspe_t *pspe, pspe_mac_t *mac)
{
  *pspe[0] = (*pspe[0]&0x1)|(*mac[0]&0xFFFFFFF8);
  *pspe[1] = *mac[1];
}

void sp_getKeyC(sp_t *sp, sp_key_c_t *key_c)
{
  size_t i;
  for (i=0;i<DES_KEY_SIZE_IN_WORDS;i++)
  {
    key_c[0][i] = (i == (DES_KEY_SIZE_IN_WORDS - 1)) ? (sp[0][2+i] & 0x00FFFFFF) : sp[0][2+i]; // DES-X 184-bits key 
  }
}

void sp_setKeyC(sp_t *sp, sp_key_c_t *key_c)
{
  size_t i;
  for (i=0;i<DES_KEY_SIZE_IN_WORDS;i++)
  {
    *sp[2+i] = (i == (DES_KEY_SIZE_IN_WORDS - 1)) ? ((*sp[2+i] & 0xFF000000) | *key_c[i]) : *key_c[i];
  }
}

void sp_getKeyI(sp_t *sp, sp_key_i_t *key_i)
{
  size_t i;
  for (i=0;i<DES_KEY_SIZE_IN_WORDS;i++)
  {
    key_i[0][i] = (i == (DES_KEY_SIZE_IN_WORDS - 1)) ? (sp[0][8+i] & 0x00FFFFFF) : sp[0][8+i]; // DES-X 184-bits key 
  }
}

void sp_setKeyI(sp_t *sp, sp_key_i_t *key_i)
{
  size_t i;
  for (i=0;i<DES_KEY_SIZE_IN_WORDS;i++)
  {
    *sp[8+i] = (i == (DES_KEY_SIZE_IN_WORDS - 1)) ? ((*sp[8+i] & 0xFF000000) | *key_i[i]) : *key_i[i];
  }
}

sp_mode_c_t sp_getModeC(sp_t *sp)
{
  return (sp_mode_c_t)SP_GET(sp[0], MODEC);
}

void sp_setModeC(sp_t *sp, sp_mode_c_t mode_c)
{
  SP_SET(sp[0], MODEC, mode_c);
}

sp_mode_i_t sp_getModeI(sp_t *sp)
{
  return (sp_mode_i_t)SP_GET(sp[0], MODEI);
}

void sp_setModeI(sp_t *sp, sp_mode_i_t mode_i)
{
  SP_SET(sp[0], MODEI, mode_i);
}

sp_valid_t sp_getValid(sp_t *sp)
{
  return (sp_valid_t)SP_GET(sp[0], VALID);
}

void sp_setValid(sp_t *sp, sp_valid_t valid)
{
  SP_SET(sp[0], VALID, valid);
}

addr_t io_config_getMBAddr(io_reg_32_t *config)
{
//  return (addr_t)IO_GET(config, CONFIG_MBADDR);
  return (addr_t)HSM_CFG_MBBA_GET(*config);
}

void io_config_setMBAddr(io_reg_32_t *config, addr_t mb_addr)
{
//  IO_SET(config, CONFIG_MBADDR, mb_addr);
  HSM_CFG_MBBA_SET(*config, mb_addr);
}

bool io_config_getEN(io_reg_32_t *config)
{
//  return (bool)IO_GET(config, CONFIG_EN);
  return (bool)HSM_CFG_EN_GET(*config);
}

void io_config_setEN(io_reg_32_t *config, bool enable)
{
//  IO_SET(config, CONFIG_EN, enable);
  if (enable)
    HSM_CFG_EN_SET(*config, ENABLE);
  else
    HSM_CFG_EN_SET(*config, DISABLE);
}

bool io_config_getInt(io_reg_32_t *config)
{
//  return (bool)IO_GET(config, CONFIG_INT);
  return (bool)HSM_CFG_IE_GET(*config);
}

void io_config_setInt(io_reg_32_t *config, bool enable)
{
//  IO_SET(config, CONFIG_INT, enable);
  if (enable)
    HSM_CFG_IE_SET(*config, ENABLE);
  else
    HSM_CFG_IE_SET(*config, DISABLE);
}

bool io_config_getPC(io_reg_32_t *config)
{
//  return (bool)IO_GET(config, CONFIG_PC);
  return (bool)HSM_CFG_PCE_GET(*config);
}

void io_config_setPC(io_reg_32_t *config, bool enable)
{
//  IO_SET(config, CONFIG_PC, enable);
  if (enable)
    HSM_CFG_PCE_SET(*config, ENABLE);
  else
    HSM_CFG_PCE_SET(*config, DISABLE);
}

bool io_config_getSPC(io_reg_32_t *config)
{
//  return (bool)IO_GET(config, CONFIG_SPC);
  return (bool)HSM_CFG_SPCE_GET(*config);
}

void io_config_setSPC(io_reg_32_t *config, bool enable)
{
//  IO_SET(config, CONFIG_SPC, enable);
  if (enable)
    HSM_CFG_SPCE_SET(*config, ENABLE);
  else
    HSM_CFG_SPCE_SET(*config, DISABLE);
}

bool io_config_getMACC(io_reg_32_t *config)
{
//  return (bool)IO_GET(config, CONFIG_MACC);
  return (bool)HSM_CFG_MSCE_GET(*config);
}

void io_config_setMACC(io_reg_32_t *config, bool enable)
{
//  IO_SET(config, CONFIG_MACC, enable);
  if (enable)
    HSM_CFG_MSCE_SET(*config, ENABLE);
  else
    HSM_CFG_MSCE_SET(*config, DISABLE);
}

bool io_config_getHMACC(io_reg_32_t *config)
{
//  return (bool)IO_GET(config, CONFIG_HMACC);
  return (bool)HSM_CFG_MTCE_GET(*config);
}

void io_config_setHMACC(io_reg_32_t *config, bool enable)
{
//  IO_SET(config, CONFIG_HMACC, enable);
  if (enable)
    HSM_CFG_MTCE_SET(*config, ENABLE);
  else
    HSM_CFG_MTCE_SET(*config, DISABLE);
}

addr_t io_config_getBRAM(io_reg_32_t *config)
{
//  return (addr_t)IO_GET(config, CONFIG_BRAM);
  return (addr_t)HSM_CFG_PADD_GET(*config);
}

void io_config_setBRAM(io_reg_32_t *config, addr_t b_ram_addr)
{
//  IO_SET(config, CONFIG_BRAM, b_ram_addr);
  HSM_CFG_PADD_SET(*config, b_ram_addr);
}

size_t io_config_getSRAM(io_reg_32_t *config)
{
//  return (size_t)IO_GET(config, CONFIG_SRAM);
  return (size_t)HSM_CFG_PSIZ_GET(*config);
}

void io_config_setSRAM(io_reg_32_t *config, size_t s_ram)
{
//  IO_SET(config, CONFIG_SRAM, s_ram);
  HSM_CFG_PSIZ_SET(*config, s_ram);
}

void io_status_setBusy(io_reg_32_t *status, bool busy)
{
//  IO_SET(status, STATUS_BUSY, busy);
  if (busy)
    HSM_STATUS_BUSY_SET(*status, BUSY);
  else
    HSM_STATUS_BUSY_SET(*status, IDLE);
}

bool io_status_getBusy(io_reg_32_t *status)
{
  return (bool)HSM_STATUS_BUSY_GET(*status);
//  return (bool)IO_GET(status, STATUS_BUSY);
}

void io_status_setErrT(io_reg_32_t *status, io_status_errt_t errt)
{
//  IO_SET(status, STATUS_ERRT, errt);
  switch(errt)
  {
  case io_status_e_none:
    HSM_STATUS_ERRT_SET(*status, E_NONE);
    break;
  case io_status_e_pspe:
    HSM_STATUS_ERRT_SET(*status, E_PSPE);
    break;
  case io_status_e_sp:
    HSM_STATUS_ERRT_SET(*status, E_SP);
    break;
  case io_status_e_mac:
    HSM_STATUS_ERRT_SET(*status, E_MAC);
    break;
  case io_status_e_ht:
    HSM_STATUS_ERRT_SET(*status, E_HT);
    break;
  }
}

io_status_errt_t io_status_getErrT(io_reg_32_t *status)
{
//  return (io_status_errt_t)IO_GET(status, STATUS_ERRT);
  return (io_status_errt_t)HSM_STATUS_ERRT_GET(*status);
}

void io_status_setErrC(io_reg_32_t *status, io_status_errc_t errc)
{
//  IO_SET(status, STATUS_ERRC, errc);
  switch(errc)
  {
  case io_status_e_load:
    HSM_STATUS_ERRC_SET(*status, LOAD);
    break;
  case io_status_e_store:
    HSM_STATUS_ERRC_SET(*status, STORE);
    break;
  }
}

io_status_errc_t io_status_getErrC(io_reg_32_t *status)
{
//  return (io_status_errc_t)IO_GET(status, STATUS_ERRC);
  return (io_status_errc_t)HSM_STATUS_ERRC_GET(*status);
}

void io_status_setErrA(io_reg_32_t *status, addr_t erra)
{
//  IO_SET(status, STATUS_ERRA, erra);
  HSM_STATUS_ERRA_SET(*status, erra);
}

addr_t io_status_getErrA(io_reg_32_t *status)
{
//  return (addr_t)IO_GET(status, STATUS_ERRA);
  return (addr_t)HSM_STATUS_ERRA_GET(*status);
}

void io_cmd_setCmd(io_reg_32_t *cmd, io_cmd_cmd_t cmd_val)
{
//  IO_SET(cmd, CMD_CMD, cmd_val);
  switch(cmd_val)
  {
  case io_cmd_none:
    HSM_AGRWCMD_CMD_SET(*cmd, NONE);
    break;
  case io_cmd_load:
    HSM_AGRWCMD_CMD_SET(*cmd, READ);
    break;
  case io_cmd_store:
    HSM_AGRWCMD_CMD_SET(*cmd, WRITE);
    break;
  case io_cmd_init:
    HSM_AGRWCMD_CMD_SET(*cmd, INIT);
    break;
  case io_cmd_continue:
    HSM_AGRWCMD_CMD_SET(*cmd, CONTINUE);
    break;
  }
}

io_cmd_cmd_t io_cmd_getCmd(io_reg_32_t *cmd)
{
//  return (io_cmd_cmd_t)IO_GET(cmd, CMD_CMD);
  return (io_cmd_cmd_t)HSM_AGRWCMD_CMD_GET(*cmd);
}

void printCacheParam(const cache_param_s &cache)
{
  DEBUG(NONE, STD, "[SECBUS] Cache Properties -- %s --\n", cache.name);
  DEBUG(NONE, STD, "[SECBUS] \t| * Set Number %u\n", cache.set_number);
  DEBUG(NONE, STD, "[SECBUS] \t| * Associativity %u\n", cache.associativity);
  DEBUG(NONE, STD, "[SECBUS] \t| * Block Size %ubits\n", cache.block_size*32);
  DEBUG(NONE, STD, "[SECBUS] \t| * Policy %s\n", 
	(cache.write_policy == WRITE_THROUGH) ? "WRITE_THROUGH" : "WRITE_BACK");
}

void printWithUnit(uint32_t number)
{
  if (number>=0x100000)
  {
    DEBUG(NONE, STD, "%uMBytes", number/0x100000);
  }
  else if (number>=0x400)
  {
    DEBUG(NONE, STD, "%uKBytes", number/0x400);
  }
  else
  {
    DEBUG(NONE, STD, "%uBytes", number);
  }
}
