/*
 * 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 "mt_ctrl.h"

MT_Ctrl::MT_Ctrl(sc_module_name name)
  : sc_module(name)
{
  SC_METHOD(ps);
  sensitive << p_clk.pos();
  SC_METHOD(pc);
  sensitive << p_clk.neg();

  //allocation
  check_register_s = new uint32_t[LAST_HS_SIZE_IN_WORDS];
  rhv_s = new uint32_t[SIZE_64_IN_WORDS];
  hash_s = new uint32_t[SIZE_64_IN_WORDS];
  new_hash_s = new uint32_t[SIZE_64_IN_WORDS];
  reference_hash_s = new uint32_t[SIZE_64_IN_WORDS];
  restore_fifo = new  GenericFifo<addr_t>(LAST_HS_SIZE_IN_WORDS);
  synchronize_fifo = new GenericFifo<addr_t>(LAST_HS_SIZE_IN_WORDS);
  M = new uint32_t[(MAX_TREE_LEVEL + 1) * SIZE_64_IN_WORDS];

  DEBUG(NONE, STD, "[SECBUS] Building MT_Ctrl (%s)\n",
	basename());
}

void MT_Ctrl::ps()
{
  DEBUG(TRACE, FSM, "[%s]\t\tFSM I\t%u\n", basename(), (uint32_t)initialize_state.read());
  DEBUG(TRACE, FSM, "[%s]\t\tFSM C\t%u\n", basename(), (uint32_t)check_state.read());
  DEBUG(TRACE, FSM, "[%s]\t\tFSM U\t%u\n", basename(), (uint32_t)update_state.read());

  if (!p_resetn) 
  {
    cycle = 0;
    memset(rhv_s, 0, SIZE_64_IN_BYTES);
    memset(check_register_s, 0, LAST_HS_SIZE_IN_BYTES);
    memset(reference_hash_s, 0, SIZE_64_IN_BYTES);
    memset(hash_s, 0, SIZE_64_IN_BYTES);
    memset(new_hash_s, 0, SIZE_64_IN_BYTES);
    memset(reference_hash_s, 0, SIZE_64_IN_BYTES);
    memset(new_spspe_s, 0, PSPE_SIZE_IN_BYTES);
    memset(M, 0, (MAX_TREE_LEVEL + 1) * SIZE_64_IN_BYTES);
    update_state = HTU_IDLE;
    initialize_state = HTI_IDLE;
    hash_count = 0;
//    synchronize_address_s = PSPE_BASE_ADDRESS;
//    brother_address_s = PSPE_BASE_ADDRESS;
    check_state = HTC_IDLE;
    mb_address = 0; //PSPE_BASE_ADDRESS;
    state_s = 0;
    write_master_hv_s = false;
    return;
  }

  cycle++;

//-------------------------------------------------------- initialize_state ----------------------------------------------
//
  switch (initialize_state) 
  {
  case HTI_IDLE:
    state_s = 1;
    pspe_t m_pspe, s_pspe;

    write_master_hv_s = false;

    m_pspe[0] = sc_register_in.m_pspe[0].read();
    m_pspe[1] = sc_register_in.m_pspe[1].read();
    s_pspe[0] = sc_register_in.s_pspe[0].read();
    s_pspe[1] = sc_register_in.s_pspe[1].read();
/*    memcpy(m_pspe, sc_register_in.m_pspe, PSPE_SIZE_IN_BYTES);
    memcpy(s_pspe, sc_register_in.s_pspe, PSPE_SIZE_IN_BYTES);
*/
    if (cmd_in.enable && cmd_in.cmd == INITIALIZE)
    {
      is_init_s = true;

      level_s			= 0;
      brother_cpt_s 		= 0;
      set_address_s		= cmd_in.address;

      if (isMasterTree(cmd_in.address.read(),
		       p_mb_address.read(),
		       p_size.read()))
      {
	memset(check_register_s, 0, LAST_HS_SIZE_IN_BYTES);
	brother_address_s       = getMMacTBaseAddress(p_mb_address.read(), p_size.read());

	ht_start_address_s	= mt_getParentLeavesAddress(
	  cmd_in.address, 0x0, 0, p_mb_address.read(), p_size.read());
	ht_page_size_s		= getLevelFromSize(getMMTSize(p_size.read())>>2);
	master_tree_s		= true;
/*	DEBUG(NONE, STD, "CONTINUE :\n");
	DEBUG(NONE, STD, "bro: %x\n", getMMacTBaseAddress(p_mb_address.read(), p_size.read()));
	DEBUG(NONE, STD, "sta: %x\n", mt_getParentLeavesAddress(
		cmd_in.address, 0x0, 0, p_mb_address.read(), p_size.read()));
	DEBUG(NONE, STD, "psz: %x\n", getLevelFromSize(getMMTSize(p_size.read())>>2));
*/
//	brother_address_s       =  
      }
      else
      {
//	memcpy(new_spspe_s, s_pspe, PSPE_SIZE_IN_BYTES);
	new_spspe_s[0] = s_pspe[0];
	new_spspe_s[1] = s_pspe[1];

	memset(check_register_s, 0, LAST_HS_SIZE_IN_BYTES);
	brother_address_s	= mt_getPageBaseAddress(
	  pspe_getMSAdd(&m_pspe), pspe_getSize(&m_pspe));	

	ht_start_address_s	= pspe_getMSAdd(&m_pspe)+getPageSize(pspe_getSize(&m_pspe))/4*pspe_getMSIdx(&m_pspe);
	ht_page_size_s		= pspe_getSize(&m_pspe);
	master_tree_s		= false;
/*	DEBUG(NONE, STD, "##[MT] CONTINUE %x:\n", cmd_in.address.read());
	DEBUG(NONE, STD, "##[MT] bro: %x\n", mt_getPageBaseAddress(
	  pspe_getMSAdd(&m_pspe), pspe_getSize(&m_pspe)));
	DEBUG(NONE, STD, "##[MT] sta: %x\n", pspe_getMSAdd(&m_pspe)+getPageSize(pspe_getSize(&m_pspe))/4*pspe_getMSIdx(&m_pspe));
	DEBUG(NONE, STD, "##[MT] psz: %x\n", pspe_getSize(&m_pspe));
	DEBUG(NONE, STD, "##[MT] (%x:%x) -> %x %x\n", sc_register_in.s_pspe_address.read(), sc_register_in.s_page_address.read(), s_pspe[0], s_pspe[1]);
*/
/*	ht_start_address_s	= pspe_getMSAdd(&m_pspe);
	ht_page_size_s		= pspe_getSize(&m_pspe);
	master_tree_s		= false;
	brother_address_s	= mt_getPageBaseAddress(
	  pspe_getMSAdd(&m_pspe), pspe_getSize(&m_pspe));
	  memcpy(new_spspe_s, s_pspe, PSPE_SIZE_IN_BYTES);*/
	
      }

      initialize_state = I_START_RESTORE_BROTHER;
    }
    else if(cmd_in.enable && cmd_in.cmd == INITIALIZE_PAGE)
    {
      is_init_s = true;
      if (isMasterTree(cmd_in.address.read(),
		       p_mb_address.read(),
		       p_size.read()))
      {
	memset(check_register_s, 0, LAST_HS_SIZE_IN_BYTES);

	brother_address_s       = getMMacTBaseAddress(p_mb_address.read(), p_size.read());

	set_address_s 		= cmd_in.address;
	ht_start_address_s	= mt_getParentLeavesAddress(
	  cmd_in.address, 0x0, 0x0, p_mb_address.read(), p_size.read());

	ht_page_size_s		= getLevelFromSize(getMMTSize(p_size.read())>>2);
	master_tree_s		= true;
/*
	DEBUG(NONE, STD, "START:\n");
	DEBUG(NONE, STD, "bro: %x\n", getMMacTBaseAddress(p_mb_address.read(), p_size.read()));
	DEBUG(NONE, STD, "sta: %x\n", mt_getParentLeavesAddress(
		cmd_in.address, 0x0, 0x0, p_mb_address.read(), p_size.read()));
	DEBUG(NONE, STD, "psz :%x\n", getLevelFromSize(getMMTSize(p_size.read())>>2));
*/    }
      else
      {
	new_spspe_s[0] = s_pspe[0];
	new_spspe_s[1] = s_pspe[1];

	memset(check_register_s, 0, LAST_HS_SIZE_IN_BYTES);

	brother_address_s	= mt_getPageBaseAddress(
	  pspe_getMSAdd(&m_pspe), pspe_getSize(&m_pspe));	

	set_address_s 		= cmd_in.address;
	ht_start_address_s	= pspe_getMSAdd(&m_pspe)+getPageSize(pspe_getSize(&m_pspe))/4*pspe_getMSIdx(&m_pspe);
	
	ht_page_size_s		= pspe_getSize(&m_pspe);
	master_tree_s		= false;

/*	DEBUG(NONE, STD, "##[MT] START:\n");
	DEBUG(NONE, STD, "##[MT] bro: %x\n", mt_getPageBaseAddress(
	  pspe_getMSAdd(&m_pspe), pspe_getSize(&m_pspe)));
	DEBUG(NONE, STD, "##[MT] sta: %x\n", pspe_getMSAdd(&m_pspe)+getPageSize(pspe_getSize(&m_pspe))/4*pspe_getMSIdx(&m_pspe));
	DEBUG(NONE, STD, "##[MT] psz :%x\n", pspe_getSize(&m_pspe));
	DEBUG(NONE, STD, "##[MT] (%x:%x) -> s:(%x %x) m:(%x %x)\n", sc_register_in.s_pspe_address.read(), sc_register_in.s_page_address.read(), s_pspe[0], s_pspe[1], m_pspe[0], m_pspe[1]);
*/    } 
     
      level_s			= 0;
      brother_cpt_s 		= 0;
      initialize_state	        = I_START_WRITE_NULL_BROTHER;
    }
    break;

    //Initialise Last Set 

  case I_START_WRITE_NULL_BROTHER:
    initialize_state = I_WAIT_WRITE_NULL_BROTHER;
    break;	

  case I_WAIT_WRITE_NULL_BROTHER:
    if (memory_out_cmd.rspval)
    {
      if(brother_cpt_s == LAST_HASH_ARITY -1)
	initialize_state = I_START_READ_SET;
      else
      {
	brother_cpt_s 		= brother_cpt_s + 1;
	brother_address_s 	= brother_address_s + SIZE_64_IN_BYTES;
	initialize_state	= I_START_WRITE_NULL_BROTHER;
      }
    }
    break;

    //Check Last Set

  case I_START_RESTORE_BROTHER :
    initialize_state	= I_WAIT_RESTORE_BROTHER;
    break;

  case I_WAIT_RESTORE_BROTHER :
    if (hash_in_ack.rstack && hash_generator_out_cmd.rspval)
    {
//      DEBUG(NONE, STD, "restore brother %x %x %x\n", brother_address_s.read(), hash_in_rdata.data[0].read(), hash_in_rdata.data[1].read());
      check_register_s[brother_cpt_s * SIZE_64_IN_WORDS]     = hash_in_rdata.data[0];
      check_register_s[brother_cpt_s * SIZE_64_IN_WORDS + 1] = hash_in_rdata.data[1];

      hash_s[0] = hash_generator_in_data.data[0];
      hash_s[1] = hash_generator_in_data.data[1];

      if (brother_cpt_s == LAST_HASH_ARITY -1)
	//Last brother
	initialize_state	= I_START_CHECK_LAST_SET;
      else
      {
	brother_address_s	= brother_address_s + SIZE_64_IN_BYTES;
	brother_cpt_s 		= brother_cpt_s + 1;
	initialize_state	= I_START_RESTORE_BROTHER;
      }
    }
    break;

  case I_START_CHECK_LAST_SET :
    //pspe_getMAC(&new_spspe_s, &reference_hash_s);
    if (master_tree_s)
    {
      reference_hash_s[0] = master_sc_reg_in.pspe[0];
      reference_hash_s[1] = master_sc_reg_in.pspe[1];
    }
    else
    {
      reference_hash_s[0] = new_spspe_s[0]&0xFFFFFFF8;
      reference_hash_s[1] = new_spspe_s[1];
    }
    initialize_state	= I_WAIT_CHECK_LAST_SET;
    break;

  case I_WAIT_CHECK_LAST_SET :
    if (hash_generator_out_cmd.rspval)
    {
      if (master_tree_s)
	hash_s[0] = hash_generator_in_data.data[0];
      else
	hash_s[0] = hash_generator_in_data.data[0] & 0xFFFFFFF8;
      
      hash_s[1] = hash_generator_in_data.data[1];

      //Compare two Hashes
      if (compare(hash_s, reference_hash_s, SIZE_64_IN_BYTES))
	initialize_state	= I_START_READ_SET;
      else 
      {
	initialize_state	= I_INTEGRITY_ERROR;
      }
    }
    break;

    //Build HASH-TREE

  case I_START_READ_SET:
    w_parent_address_s	= mt_getParentLeavesAddress(set_address_s,
						    ht_start_address_s,
						    ht_page_size_s,
						    p_mb_address.read(),
						    p_size.read());
    initialize_state	= I_WAIT_IV_GENERATION;
    break;

  case I_START_IV_GENERATION :
    if (mt_isLastLevel(w_parent_address_s,
		       ht_page_size_s,
		       p_mb_address.read(),
		       p_size.read()))
    {
      level_s		= level_s + 1; 
      initialize_state	= I_WAIT_LAST_IV_GENERATION;
    }
    else
    {
      level_s		 = level_s + 1; 
      initialize_state	 = I_WAIT_IV_GENERATION;
      w_parent_address_s = mt_getParentAddress(w_parent_address_s, ht_page_size_s,
					       p_mb_address.read(), p_size.read());
    }
    break;

  case I_WAIT_IV_GENERATION:
    if (hash_generator_out_cmd.rspval)
    {
      M[level_s * SIZE_64_IN_WORDS]	= hash_generator_in_data.data[0].read();
      M[level_s * SIZE_64_IN_WORDS + 1]	= hash_generator_in_data.data[1].read();

      if (mt_isLastLevel(w_parent_address_s, ht_page_size_s,
			 p_mb_address.read(), p_size.read()))
      {
	if (master_tree_s && ((w_parent_address_s & 0xF) == 0)) // 0x1F
	{
	  initialize_state	= I_START_IV_GENERATION;
	}
	else if (!master_tree_s && ((w_parent_address_s & 0xF) == 0))
	  initialize_state	= I_START_IV_GENERATION;
	else if (memory_out_cmd.rspval)
	{
	  w_parent_address_s	= mt_getParentLeavesAddress(
	    set_address_s, ht_start_address_s, ht_page_size_s,
	    p_mb_address.read(), p_size.read());
	  level_s		= 0;
	  initialize_state	= I_START_UPDATE_LEAVE_PARENT;
	}
      }
      else if ((w_parent_address_s & 0x1F) == 0)
      {
	initialize_state	= I_START_IV_GENERATION;
      }
      else if (memory_out_cmd.rspval)
      {
/*	if (inclusion(set_address_s, 0x91000000, 0x91000080))
	  DEBUG(NONE, STD, "----> %x %x %x %x\n", hash_generator_in_data.data[0].read(),
	  hash_generator_in_data.data[1].read(), set_address_s.read(), w_parent_address_s.read());*/

	w_parent_address_s	= mt_getParentLeavesAddress(
	  set_address_s, ht_start_address_s, ht_page_size_s,
	  p_mb_address.read(), p_size.read());
	level_s			= 0;
	initialize_state	= I_START_UPDATE_LEAVE_PARENT;
      }
    }	
    break;

  case I_WAIT_LAST_IV_GENERATION:
    if (hash_generator_out_cmd.rspval)
    {
      M[level_s * SIZE_64_IN_WORDS]	= hash_generator_in_data.data[0].read();
      M[level_s * SIZE_64_IN_WORDS + 1]	= hash_generator_in_data.data[1].read();

      if (/*master_tree_s ||*/ ((w_parent_address_s & 0x3F) == 0))
      {
	w_parent_address_s	= mt_getParentLeavesAddress(
	  set_address_s, ht_start_address_s, ht_page_size_s,
	  p_mb_address.read(), p_size.read());
	level_s			= 0;
	initialize_state	= I_START_UPDATE_LEAVE_PARENT;
      }		
      else 
      {
	index_s			= 0;
	initialize_state	= I_START_UPDATE_ROOT_M;
      }
    }	
    break;

  case I_START_UPDATE_LEAVE_PARENT :
    initialize_state = I_WAIT_UPDATE_LEAVE_PARENT;
    break;

  case I_WAIT_UPDATE_LEAVE_PARENT :
    if (hash_generator_out_cmd.rspval)
    {
      M[0]     		= hash_generator_in_data.data[0].read();
      M[1]		= hash_generator_in_data.data[1].read();
      initialize_state	= I_START_UPDATE_PARENT_M;
    }
    break;

  case I_START_UPDATE_PARENT_M :
    initialize_state	= I_WAIT_UPDATE_PARENT_M;
    index_s		= mt_getBlockIndex(w_parent_address_s, ht_page_size_s,
					   p_mb_address.read(), p_size.read());
    level_s		= level_s + 1;
    break;

  case I_WAIT_UPDATE_PARENT_M :
    if (hash_generator_out_cmd.rspval && memory_out_cmd.rspval)
    {
      M[level_s * SIZE_64_IN_WORDS]	= hash_generator_in_data.data[0].read();
      M[level_s * SIZE_64_IN_WORDS + 1]	= hash_generator_in_data.data[1].read();
/*
      if (master_tree_s && (index_s == LAST_HASH_ARITY - 1))
      {
	initialize_state = HTI_IDLE;
	write_master_hv_s = true;
//	memcpy(MASTER_HV, M + level_s * SIZE_64_IN_WORDS, SIZE_64_IN_BYTES);	
}*/
      /*   else */
      if (mt_isLastLevel(w_parent_address_s, ht_page_size_s,
			      p_mb_address.read(), p_size.read()) &&
	       (index_s % SIZE_64_IN_WORDS == 1))
      {
	if (index_s ==  LAST_HASH_ARITY - 1)
	{
//	  pspe_mac_t tmp_mac = (M + level_s * SIZE_64_IN_WORDS);

	  new_spspe_s[0] = M[level_s*SIZE_64_IN_WORDS] & 0xFFFFFFF8;
	  new_spspe_s[1] = M[level_s*SIZE_64_IN_WORDS+1];

	  new_spspe_s[0] = (0xFFFFFFF8&new_spspe_s[0]) | (~0xFFFFFFF8 & ((pspe_valid_true) | ht_page_size_s.read()<<1));

/*	  pspe_setValid(&new_spspe_s, pspe_valid_true);
	  pspe_setSize(&new_spspe_s, (pspe_size_t)ht_page_size_s.read());
*/	  
	  //update PSPE
	  if (master_tree_s)
	  {
	    write_master_hv_s = true;

	    initialize_state = HTI_IDLE;
	  }
	  else
	    initialize_state = I_START_WRITE_PSPE;
	}
	else
	{
	  initialize_state = I_START_UPDATE_ROOT_M;
	  index_s	   = index_s + 1;
	}
      }
      else if (index_s == (HASH_ARITY - 1))
      {	
	w_parent_address_s = mt_getParentAddress(w_parent_address_s, ht_page_size_s,
						 p_mb_address.read(), p_size.read());
	initialize_state   = I_START_UPDATE_PARENT_M;
      }
      else
      {
	level_s		   = 0;
	set_address_s	   = set_address_s + HS_SIZE_IN_BYTES;
//	DEBUG(NONE, STD, "adv %x\n", set_address_s.read());
	initialize_state   = I_START_READ_SET;
      }
    }
    break;

  case I_START_UPDATE_ROOT_M:
    initialize_state = I_WAIT_UPDATE_ROOT_M;
    break;

  case I_WAIT_UPDATE_ROOT_M:
    if (hash_generator_out_cmd.rspval)
    {
      M[level_s * SIZE_64_IN_WORDS]	= hash_generator_in_data.data[0].read();
      M[level_s * SIZE_64_IN_WORDS + 1]	= hash_generator_in_data.data[1].read();

      if (index_s ==  LAST_HASH_ARITY - 1)
      {
	if (master_tree_s)
	{
	  write_master_hv_s = true;

	  initialize_state = HTI_IDLE;
	}
	else
	{
	  new_spspe_s[0] = M[level_s*SIZE_64_IN_WORDS] & 0xFFFFFFF8;
	  new_spspe_s[1] = M[level_s*SIZE_64_IN_WORDS + 1];

//	  pspe_setValid(&new_spspe_s, pspe_valid_true);
//	  pspe_setSize(&new_spspe_s, (pspe_size_t)ht_page_size_s.read());

	  new_spspe_s[0] = (0xFFFFFFF8&new_spspe_s[0]) | (~0xFFFFFFF8 & ((pspe_valid_true) | ht_page_size_s.read()<<1));
	  //update PSPE

	  initialize_state = I_START_WRITE_PSPE;
	}
      }
      else if (index_s == mt_getBlockIndex(w_parent_address_s, ht_page_size_s,
					   p_mb_address.read(), p_size.read()) - 1)
      {
	w_parent_address_s = mt_getParentLeavesAddress(
	  set_address_s, ht_start_address_s, ht_page_size_s,
	  p_mb_address.read(), p_size.read());
	level_s            = 0;
	initialize_state   = I_START_UPDATE_LEAVE_PARENT;
      }
      else
      {
	index_s		   = index_s + 1;
	initialize_state   = I_START_UPDATE_ROOT_M;
      }
    }
    break; 

  case I_START_WRITE_PSPE:
    initialize_state = I_WAIT_WRITE_PSPE;
    break;

  case I_WAIT_WRITE_PSPE:
    //PSPE written 
    if(sc_out_cmd.ack)
    {
      initialize_state = HTI_IDLE;
    }
    break;

  case I_INTEGRITY_ERROR :
    DEBUG(ERROR, STD, "----------------------------------------------------------------\n");
    DEBUG(ERROR, STD, "INTEGRITY VIOLATION DETECTED, ABORTING ...\n");
    DEBUG(ERROR, STD, "----------------------------------------------------------------\n");
    DEBUG(ERROR, STD, "ERROR : HASH INTEGRITY INITIALIZE OPERATION\n");
    DEBUG(ERROR, STD, "parent address : %x\n", (uint32_t)r_parent_address_s.read());
    DEBUG(ERROR, STD, "set address : %x\n", (uint32_t)set_address_s.read());
    for(unsigned int i=0;i<SIZE_64_IN_WORDS;i++)
      DEBUG(ERROR, STD, "hash : %x\n", hash_s[i]);
    for(unsigned int i=0;i<SIZE_64_IN_WORDS;i++)
      DEBUG(ERROR, STD, "reference hash : %x\n", reference_hash_s[i]);
    for(unsigned int i=0;i<LAST_HS_SIZE_IN_WORDS;i++)
      DEBUG(ERROR, STD, "set : %x\n", check_register_s[i]);
    //stop simulation
    sc_stop();
    initialize_state = I_WAIT_INTERRUPT;
    break;

  case I_WAIT_INTERRUPT:
    if (irq.ack.read())
    {
      initialize_state = HTI_IDLE;
    }
    break;

  default:
    break;
  }

//---------------------------------------------------- update_state ---------------------------------------------
//
  switch (update_state) 
  {
  case HTU_IDLE: 
    state_s = 3;
    
    pspe_t m_pspe, s_pspe;
    m_pspe[0] = sc_register_in.m_pspe[0].read();
    m_pspe[1] = sc_register_in.m_pspe[1].read();
    s_pspe[0] = sc_register_in.s_pspe[0].read();
    s_pspe[1] = sc_register_in.s_pspe[1].read();

    if(cmd_in.enable && cmd_in.cmd == UPDATE)
    {
      src_s = cmd_in.src;
      is_init_s = false;

      leaves_s = true;
      victim_address_s = cmd_in.address;
      set_address_s	= cmd_in.address;
				
      if (isMasterTree(cmd_in.address,
		       p_mb_address.read(),
		       p_size.read()))
      {
	ht_start_address_s	= getMMacTBaseAddress(p_mb_address.read(),
						      p_size.read());
	ht_page_size_s		= getLevelFromSize(getMMTSize(p_size.read())>>2);
	master_tree_s		= true;
//	memcpy(rhv_s, master_sc_reg_in.pspe, SIZE_64_IN_BYTES); 
	rhv_s[0] = master_sc_reg_in.pspe[0].read();
	rhv_s[1] = master_sc_reg_in.pspe[1].read();
//	DEBUG(NONE, STD, "##[MT] update MT %x %u\n", cmd_in.address.read(), cmd_in.src.read());
//	for (size_t i=0;i<SIZE_256_IN_WORDS;i++)
//	  DEBUG(NONE, STD, "##[MT] [%u] %x\n", i, register_1_in.data[i].read());

      }
      else //--
      {									
	ht_start_address_s	= pspe_getMSAdd(&m_pspe)+(getPageSize(pspe_getSize(&m_pspe))/4)*pspe_getMSIdx(&m_pspe);
	ht_page_size_s		= pspe_getSize(&m_pspe);
	master_tree_s		= false;
//	pspe_getMAC(&s_pspe, &rhv_s);
	rhv_s[0] = s_pspe[0]&0xFFFFFFF8;
	rhv_s[1] = s_pspe[1];
//	DEBUG(NONE, STD, "##[MT] update normal %x\n", cmd_in.address.read());
      }
				
      //Parent address to update
      update_state = U_START_PARENT_UPDATE;	
    }
    break;

  case U_START_PARENT_UPDATE:	//--
    update_state = U_WAIT_PARENT_UPDATE;
    break;

  case U_WAIT_PARENT_UPDATE:
    if (hash_generator_out_cmd.rspval)
    {
      new_hash_s[0] = hash_generator_in_data.data[0];
      new_hash_s[1] = hash_generator_in_data.data[1];

      if (leaves_s)//--
      {
	leaves_s           = false;
	w_parent_address_s = mt_getParentLeavesAddress(
	  victim_address_s, 
	  ht_start_address_s,
	  ht_page_size_s,
	  p_mb_address.read(),
	  p_size.read());
      }
      else
      {	
	w_parent_address_s = mt_getParentAddress(victim_address_s, 
						 ht_page_size_s,
						 p_mb_address.read(),
						 p_size.read());
      }

      update_state = U_START_WRITE_NEW_PARENT;
    }
    break;

  case U_START_WRITE_NEW_PARENT: //--
    restore_fifo->init();
    synchronize_fifo->init();
    update_state = U_WAIT_WRITE_NEW_PARENT;
    break;

  case U_WAIT_WRITE_NEW_PARENT :
    if (hash_in_ack.wack) //--
    {
      if (hash_in_ack.full)
	//Cache dirty threshold reached
      {
	victim_address_s = hash_in_ack.victim_address;
        
	pspe_t s_pspe;
	s_pspe[0] = sc_register_in.s_pspe[0].read();
	s_pspe[1] = sc_register_in.s_pspe[1].read();

	// reset Hit and brother counter
	hit_cpt_s     = 0;
	brother_cpt_s = 0;

	if (isMasterTree(hash_in_ack.victim_address,
			 p_mb_address.read(), p_size.read()))
	// address belongs to master tree
	{
//	  DEBUG(NONE, STD, "[MT] update change to %x\n", hash_in_ack.victim_address.read());
//	  memcpy(rhv_s, master_sc_reg_in.pspe, SIZE_64_IN_BYTES);
	  for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	    rhv_s[i] = master_sc_reg_in.pspe[i];

	  ht_start_address_s	= getMMacTBaseAddress(p_mb_address.read(),
						      p_size.read());
	  ht_page_size_s	= getLevelFromSize(getMMTSize(p_size.read())>>2);
	  master_tree_s		= true;

	  brother_address_s	= mt_getSetAddress(hash_in_ack.victim_address, 
						   getLevelFromSize(getMMTSize(p_size.read())>>2),
						   p_mb_address.read(),
						   p_size.read());
	  update_state		= U_START_READ_VICTIM_SET;
	}
	else
	{
	  if (getPSPEAddressFromPageSize(hash_in_ack.victim_address,
					 pspe_getSize(&s_pspe),
					 p_mb_address.read(),
					 p_b_address.read()) == (uint32_t)sc_register_in.s_pspe_address.read())
	  {
	    // we already have the spspe
	    rhv_s[0] = s_pspe[0]&0xFFFFFFF8;
	    rhv_s[1] = s_pspe[1];

	    ht_start_address_s = hash_in_ack.victim_address;
	    ht_page_size_s = pspe_getSize(&s_pspe);
	    master_tree_s = false;
	    brother_address_s = mt_getSetAddress(hash_in_ack.victim_address,
						 pspe_getSize(&s_pspe),
						 p_mb_address.read(),
						 p_size.read());

//	    DEBUG(NONE, STD, "[MT] update change to %x (%x %x) h\n", hash_in_ack.victim_address.read(), rhv_s[0], rhv_s[1]);
	    update_state = U_START_READ_VICTIM_SET;
	  }
	  else
	  {
	    update_state = U_START_READ_SLAVE_PSPE;
	  }
	}
      }
      else //--
      {
	update_state = HTU_IDLE;
      }
    }
    break;

  case U_START_READ_SLAVE_PSPE :
//    if (sc_out_cmd.
    update_state = U_WAIT_READ_SLAVE_PSPE;
    break;

  case U_WAIT_READ_SLAVE_PSPE :
    if (sc_out_cmd.ack && check_state == HTC_IDLE)
    {
      pspe_t s_pspe;
      s_pspe[0] = sc_register_in.s_pspe[0].read();
      s_pspe[1] = sc_register_in.s_pspe[1].read();
      
//      pspe_getMAC(&s_pspe, &rhv_s);
      rhv_s[0] = s_pspe[0]&0xFFFFFFF8;
      rhv_s[1] = s_pspe[1];
	
      ht_start_address_s = victim_address_s;
      ht_page_size_s	 = pspe_getSize(&s_pspe);
      brother_address_s	 = mt_getSetAddress(victim_address_s,
					   pspe_getSize(&s_pspe),
					   p_mb_address.read(),
					   p_size.read());		
      master_tree_s		= false;
      hit_cpt_s			= 0;
      brother_cpt_s		= 0;

//      DEBUG(NONE, STD, "[MT] update change to %x (%x %x) m\n", victim_address_s.read(), rhv_s[0], rhv_s[1]);

      update_state		= U_START_READ_VICTIM_SET;
    }
    break;

  case U_START_READ_VICTIM_SET :
    set_address_s	= mt_getSetAddress(victim_address_s, ht_page_size_s,
					   p_mb_address.read(), p_size.read());
    update_state	= U_WAIT_READ_VICTIM_HASH_SET;
    break;

  case U_WAIT_READ_VICTIM_HASH_SET :
    if (hash_in_ack.rack && hash_generator_out_cmd.rspval)
    {
      if (hash_in_ack.hit)
	//Increment Hit counter
	hit_cpt_s = hit_cpt_s + 1;
      if (hash_in_ack.dirty)
      {
	//store brother address in dirty fifo
	restore_fifo->simple_put(brother_address_s);
	synchronize_fifo->simple_put(brother_address_s);
      }

      check_register_s[brother_cpt_s * SIZE_64_IN_WORDS]     = hash_in_rdata.data[0];
      check_register_s[brother_cpt_s * SIZE_64_IN_WORDS + 1] = hash_in_rdata.data[1];

      hash_s[0] = hash_generator_in_data.data[0];
      hash_s[1] = hash_generator_in_data.data[1];

      if (mt_isLastLevel(
	    victim_address_s,
	    ht_page_size_s,
	    p_mb_address.read(),
	    p_size.read()))
      {
	if (brother_cpt_s == LAST_HASH_ARITY - 1)
	  //Last brother
	  update_state	    = U_START_NEW_HASH_GENERATION;
	else
	{
	  brother_address_s = brother_address_s + SIZE_64_IN_BYTES;
	  brother_cpt_s     = brother_cpt_s + 1;
	  update_state	    = U_START_READ_VICTIM_SET;
	}
      }
      else
      {
	if (brother_cpt_s == HASH_ARITY - 1)
	  update_state	    = U_START_NEW_HASH_GENERATION;
	else
	  //We continue to read brother
	{
	  brother_address_s = brother_address_s + SIZE_64_IN_BYTES;
	  brother_cpt_s     = brother_cpt_s + 1;
	  update_state	    = U_START_READ_VICTIM_SET;
	}
      }
    }
    break;

  case U_START_NEW_HASH_GENERATION:
    update_state = U_WAIT_NEW_HASH_GENERATION;
    break;

  case U_WAIT_NEW_HASH_GENERATION:
    if (hash_generator_out_cmd.rspval)
    {
      new_hash_s[0] = hash_generator_in_data.data[0];
      new_hash_s[1] = hash_generator_in_data.data[1];

      if (mt_isLastLevel(victim_address_s,
			 ht_page_size_s,
			 p_mb_address.read(),
			 p_size.read()))
      {
	if (hit_cpt_s == LAST_HASH_ARITY)
	  //Hit on all brothers
	{
	  synchronize_address_s	= synchronize_fifo->read();
	  update_state		= U_START_SYNCHRONIZE_DIRTY_VICTIM_BROTHERS;
	}
	else
	  //we must restore dirty adresses stored in fifo
	{
	  restore_address_s	= restore_fifo->read();
	  update_state		= U_START_RESTORE_DIRTY_VICTIM_BROTHERS;
	}
      }
      else
      {
	//We have read all brothers
	if (hit_cpt_s == HASH_ARITY)
	  //Hit on all brothers
	{
	  synchronize_address_s	= synchronize_fifo->read();
	  update_state		= U_START_SYNCHRONIZE_DIRTY_VICTIM_BROTHERS;
	}
	else
	  //we must restore dirty adresses stored in fifo
	{
	  restore_address_s	= restore_fifo->read();
	  update_state		= U_START_RESTORE_DIRTY_VICTIM_BROTHERS;
	}
      }
    }
    break;

  case U_START_RESTORE_DIRTY_VICTIM_BROTHERS : 
    //Decrease fifo read pointer
    restore_fifo->simple_get();
    index_s      = mt_getBlockIndex(
      restore_address_s,
      ht_page_size_s,
      p_mb_address.read(),
      p_size.read());
    update_state = U_WAIT_RESTORE_DIRTY_VICTIM_BROTHERS;
    break;

  case U_WAIT_RESTORE_DIRTY_VICTIM_BROTHERS :
    if (hash_in_ack.rstack)
    {
      check_register_s[index_s * SIZE_64_IN_WORDS]     = hash_in_rdata.data[0];
      check_register_s[index_s * SIZE_64_IN_WORDS + 1] = hash_in_rdata.data[1];

      if (mt_isLastLevel(victim_address_s,
			 ht_page_size_s,
			 p_mb_address.read(),
			 p_size.read()))
      {
	if (restore_fifo->empty() )
	  //whole dirty value restored
	{
	  r_parent_address_s = mt_getParentAddress(victim_address_s,
						   ht_page_size_s,
						   p_mb_address.read(),
						   p_size.read());
	  //Copy root hash value in reference value
	  memcpy(reference_hash_s, rhv_s, SIZE_64_IN_BYTES);
	  update_state = U_START_LAST_IC;
	}
	else
	  //We continue to restore
	{
	  restore_address_s = restore_fifo->read();
	  update_state	    = U_START_RESTORE_DIRTY_VICTIM_BROTHERS;
	}
      }
      else
      {
	if (restore_fifo->empty())
	  //whole dirty value restored
	{
	  r_parent_address_s = mt_getParentAddress(
	    victim_address_s,
	    ht_page_size_s,
	    p_mb_address.read(),
	    p_size.read());
	  update_state	     = U_START_READ_REFERENCE_PARENT;
	}
	else
	  //We continue to restore
	{
	  restore_address_s = restore_fifo->read();
	  update_state	    = U_START_RESTORE_DIRTY_VICTIM_BROTHERS;
	}
      }
    }
    break;

  case U_START_READ_REFERENCE_PARENT:
    update_state = U_WAIT_READ_REFERENCE_PARENT;
    index_s 	 = mt_getBlockIndex(
      r_parent_address_s,
      ht_page_size_s,
      p_mb_address.read(),
      p_size.read());
    break;

  case U_WAIT_READ_REFERENCE_PARENT :
    if (hash_in_ack.rack)
    {
      reference_hash_s[0] = hash_in_rdata.data[0];
      reference_hash_s[1] = hash_in_rdata.data[1];

      if (hash_in_ack.hit)
	//no need to restore brother
      {		
	if (hash_generator_out_cmd.rspval)
	{
	  hash_s[0] = hash_generator_in_data.data[0];
	  hash_s[1] = hash_generator_in_data.data[1];
	  //Compare two Hashes
	  if (compare(hash_s, reference_hash_s, SIZE_64_IN_BYTES))
	    //synchronize diry values stored in fifo
	  {
	    synchronize_address_s = synchronize_fifo->read();
	    update_state	  = U_START_SYNCHRONIZE_DIRTY_VICTIM_BROTHERS;
	  }
	  else 
	  {
	    update_state = U_INTEGRITY_ERROR;
	  }
	}
      }
      else
	//Cache Miss
      {
	//Copy reference_hash_s value in check_register_
	check_register_s[index_s * SIZE_64_IN_WORDS]	 = hash_in_rdata.data[0];
	check_register_s[index_s * SIZE_64_IN_WORDS + 1] = hash_in_rdata.data[1];

	brother_cpt_s	  = 0;
	brother_address_s = mt_getBrotherAddress(
	  r_parent_address_s,
	  ht_page_size_s,
	  p_mb_address.read(),
	  p_size.read());
	update_state	  = U_START_RESTORE_REFERENCE_PARENT_BROTHERS;
	set_address_s	  = mt_getSetAddress(
	  r_parent_address_s,
	  ht_page_size_s,
	  p_mb_address.read(),
	  p_size.read());
      }
    }
    break;

  case U_START_LAST_IC:
    update_state = U_WAIT_LAST_IC;
    break;

  case  U_WAIT_LAST_IC :
    if (hash_generator_out_cmd.rspval)
    {
      hash_s[0] = hash_generator_in_data.data[0];
      hash_s[1] = hash_generator_in_data.data[1];

      if (!master_tree_s)
      {
	//Last three validity, type and size bits must be masked before comparaison
	hash_s[0] = (hash_s[0] & 0xFFFFFFF8);
      }

      //Compare two Hashes
      if (compare(hash_s, reference_hash_s, SIZE_64_IN_BYTES))
      {
	synchronize_address_s = synchronize_fifo->read();
	update_state	      = U_START_SYNCHRONIZE_DIRTY_VICTIM_BROTHERS;
      }
      else
      {
	update_state = U_INTEGRITY_ERROR;
      }
    }
    break;


  case U_START_SYNCHRONIZE_DIRTY_VICTIM_BROTHERS :
    //Decrease fifo read pointer
    synchronize_fifo->simple_get();
    index_s      = mt_getBlockIndex(synchronize_address_s,
			       ht_page_size_s,
			       p_mb_address.read(),
			       p_size.read())*SIZE_64_IN_WORDS;
    update_state = U_WAIT_SYNCHRONIZE_DIRTY_VICTIM_BROTHERS;
    break;

  case U_WAIT_SYNCHRONIZE_DIRTY_VICTIM_BROTHERS :
    if (hash_in_ack.sack)
    {
      pspe_t s_pspe;
      s_pspe[0] = sc_register_in.s_pspe[0].read();
      s_pspe[1] = sc_register_in.s_pspe[1].read();
      
      if (mt_isLastLevel(victim_address_s,
			 ht_page_size_s,
			 p_mb_address.read(),
			 p_size.read()))
      {
	if (synchronize_fifo->empty())
	{
	  if (master_tree_s)
	  {
	    write_master_hv_s = true;

	    update_state = HTU_IDLE;	
	  }
	  else
	  {
	    //create new_spspe_s
	    new_spspe_s[0] = new_hash_s[0]&0xFFFFFFF8;
	    new_spspe_s[1] = new_hash_s[1];
	    new_spspe_s[0] = new_spspe_s[0] | (~0xFFFFFFF8 & ((pspe_getSize(&s_pspe)<<1) | pspe_getValid(&s_pspe)));
//	    pspe_setSize(&new_spspe_s, pspe_getSize(&s_pspe));
//	    pspe_setValid(&new_spspe_s, pspe_getValid(&s_pspe));
//	    pspe_setMAC(&new_spspe_s, &new_hash_s);

	    //update PSPE
	    update_state = U_START_WRITE_PSPE;
	  }
	}
	else
	  //Some dirty values are still not synchronized	
	{
	  synchronize_address_s	= synchronize_fifo->read();
	  update_state		= U_START_SYNCHRONIZE_DIRTY_VICTIM_BROTHERS;
	}
      }
      else
      {
	if (synchronize_fifo->empty())
	{
	  w_parent_address_s = mt_getParentAddress(
	    victim_address_s,
	    ht_page_size_s,
	    p_mb_address.read(),
	    p_size.read());
	  update_state	     = U_START_WRITE_NEW_PARENT;
	}
	else
	  //Some dirty values are still not synchronized	
	{
	  synchronize_address_s	= synchronize_fifo->read();
	  update_state		= U_START_SYNCHRONIZE_DIRTY_VICTIM_BROTHERS;
	}
      }
    }
    break;

  case U_START_WRITE_PSPE :
    update_state = U_WAIT_WRITE_PSPE;
    break;

  case U_WAIT_WRITE_PSPE :
    if(cmd_in.enable && cmd_in.cmd == UPDATE)
    {
      //store input in registers
      ht_start_address_s = getMMacTBaseAddress(p_mb_address.read(),
					       p_size.read());
      victim_address_s	 = cmd_in.address;
      master_tree_s	 = true;
      leaves_s		 = true;
      ht_page_size_s	 = getLevelFromSize(getMMTSize(p_size.read())>>2);
      set_address_s	 = cmd_in.address;
      //memcpy(rhv_s, master_sc_reg_in.pspe, SIZE_64_IN_BYTES);
      rhv_s[0] = master_sc_reg_in.pspe[0].read();
      rhv_s[1] = master_sc_reg_in.pspe[1].read();

      //Parent address to update
      update_state = U_START_PARENT_UPDATE;
    }
    else if (sc_out_cmd.ack.read())
    {
      update_state = HTU_IDLE;
    }
    break;

  case U_START_RESTORE_REFERENCE_PARENT_BROTHERS :
    index_s	 = mt_getBlockIndex(
      brother_address_s,
      ht_page_size_s,
      p_mb_address.read(),
      p_size.read());
    update_state = U_WAIT_RESTORE_REFERENCE_PARENT_BROTHERS;
    break;

  case U_WAIT_RESTORE_REFERENCE_PARENT_BROTHERS :
    if (hash_in_ack.rstack)
    {
      check_register_s[index_s * SIZE_64_IN_WORDS]     = hash_in_rdata.data[0];
      check_register_s[index_s * SIZE_64_IN_WORDS + 1] = hash_in_rdata.data[1];

      if (mt_isLastLevel(r_parent_address_s, 
			 ht_page_size_s,
			 p_mb_address.read(),
			 p_size.read()))
      {
	if (brother_cpt_s == (LAST_HASH_ARITY - 2))
	  //whole set restored
	{	
	  if (hash_generator_out_cmd.rspval)
	  {
	    hash_s[0] = hash_generator_in_data.data[0];
	    hash_s[1] = hash_generator_in_data.data[1];
	    //Compare two Hashes
	    if (compare(hash_s, reference_hash_s, SIZE_64_IN_BYTES))
	    {
	      //Copy root hash value in reference value
	      memcpy(reference_hash_s, rhv_s, SIZE_64_IN_BYTES);
	      update_state = U_START_LAST_IC;
	    }
	    else
	    {
	      update_state = U_INTEGRITY_ERROR;
	    }
	  }

	}
	else
	  //We continue to restore
	{
	  brother_cpt_s	    = brother_cpt_s + 1;
	  brother_address_s = mt_getBrotherAddress(brother_address_s,
						ht_page_size_s,
						p_mb_address.read(),
						p_size.read());
	  update_state	    = U_START_RESTORE_REFERENCE_PARENT_BROTHERS;
	}
      }
      else
      {
	if (brother_cpt_s == (HASH_ARITY - 2))
	  //we have restored the whole set
	{
	  if (hash_generator_out_cmd.rspval)
	  {
	    hash_s[0] = hash_generator_in_data.data[0];
	    hash_s[1] = hash_generator_in_data.data[1];
	    //Compare two Hashes
	    if (compare(hash_s, reference_hash_s, SIZE_64_IN_BYTES))
	    {
	      r_parent_address_s = mt_getParentAddress(
		r_parent_address_s, 
		ht_page_size_s,
		p_mb_address.read(),
		p_size.read());
	      update_state = U_START_READ_REFERENCE_PARENT;
	    }
	    else 
	    {
	      update_state = U_INTEGRITY_ERROR;
	    }
	  }
	}
	else
	  //We continue to restore brothers
	{
	  brother_cpt_s	    = brother_cpt_s + 1;
	  brother_address_s = mt_getBrotherAddress(
	    brother_address_s,
	    ht_page_size_s,
	    p_mb_address.read(),
	    p_size.read());
	  update_state	    = U_START_RESTORE_REFERENCE_PARENT_BROTHERS;
	}
      }
    }
    break;

  case U_INTEGRITY_ERROR :
    DEBUG(ERROR, STD, "----------------------------------------------------------------\n");
    DEBUG(ERROR, STD, "INTEGRITY VIOLATION DETECTED, ABORTING %u ...\n", cycle);
    DEBUG(ERROR, STD, "----------------------------------------------------------------\n"
	  "ERROR : HASH INTEGRITY WRITE OPERATION\n");
    DEBUG(ERROR, STD, "parent address : %x\n", (uint32_t)r_parent_address_s.read());
    DEBUG(ERROR, STD, "set address : %x\n", (uint32_t)set_address_s.read());
    for(unsigned int i=0;i<SIZE_64_IN_WORDS;i++)
      DEBUG(ERROR, STD, "hash : %x\n", hash_s[i]);
    for(unsigned int i=0;i<SIZE_64_IN_WORDS;i++)
      DEBUG(ERROR, STD, "reference hash : %x\n", reference_hash_s[i]);
    for(unsigned int i=0;i<LAST_HS_SIZE_IN_WORDS;i++)
      DEBUG(ERROR, STD, "set : %x\n", check_register_s[i]);
    //stop simulation
    sc_stop();

    update_state = U_WAIT_INTERRUPT;
    break;

  case U_WAIT_INTERRUPT:
    if (irq.ack.read())
    {
      update_state = HTU_IDLE;
    }
    break;

  default:
    break;
  }

//------------------------------------------------------------- check_state -------------------------------------------
  switch (check_state) 
  {
  case HTC_IDLE : 
    state_s = 4;
  
    if(cmd_in.enable && cmd_in.cmd == CHECK)
    {
      src_s = cmd_in.src;

      if (isMasterTree(cmd_in.address,
		       p_mb_address.read(),
		       p_size.read()))
      {
	rhv_s[0] = master_sc_reg_in.pspe[0].read();
	rhv_s[1] = master_sc_reg_in.pspe[1].read();
	index_s = 0;
	set_address_s = cmd_in.address;
	ht_start_address_s = mt_getParentLeavesAddress(
	  getPSPEBaseAddress(p_mb_address.read(), p_size.read()),
	  0x0, 0x0, p_mb_address.read(), p_size.read());
	ht_page_size_s = getLevelFromSize(getMMTSize(p_size.read())>>2);
	master_tree_s = true;

//	DEBUG(NONE, STD, "##[MT] check MT : %08x %08x %08x %u %u\n", cmd_in.address.read(),
//	      rhv_s[0], rhv_s[1], cmd_in.src.read(), cycle);

      }
      else
      {	
	pspe_t m_pspe; 
	pspe_t s_pspe;

	m_pspe[0] = sc_register_in.m_pspe[0].read();
	m_pspe[1] = sc_register_in.m_pspe[1].read();
	s_pspe[0] = sc_register_in.s_pspe[0].read();
	s_pspe[1] = sc_register_in.s_pspe[1].read();

	index_s            = 0;
	set_address_s	 = cmd_in.address;
	ht_start_address_s = pspe_getMSAdd(&m_pspe)+(getPageSize(pspe_getSize(&m_pspe))/4)*pspe_getMSIdx(&m_pspe);
	ht_page_size_s	 = pspe_getSize(&m_pspe);
	master_tree_s	 = isMasterTree(cmd_in.address,
					p_mb_address.read(),
					p_size.read());
	
//	DEBUG(NONE, STD, "##[MT] check MT : %08x %08x %08x %08x %08x %08x %u\n", cmd_in.address.read(), pspe_getMSAdd(&m_pspe)+(getPageSize(pspe_getSize(&m_pspe))/4)*pspe_getMSIdx(&m_pspe), s_pspe[0], s_pspe[1], m_pspe[0], m_pspe[1], cmd_in.src.read());
//
	rhv_s[0] = s_pspe[0]&0xFFFFFFF8;
	rhv_s[1] = s_pspe[1];
//	pspe_getMAC(&s_pspe, &rhv_s); //ALAP_SPSPE : 0x25c3b78
      }

      //Parent address
      check_state = C_START_IV_GENERATION;
    }
    break;

  case C_START_IV_GENERATION :
    check_state = C_WAIT_HASH_GENERATION;
    break;

  case C_START_HASH_GENERATION :
    index_s     = index_s + 1;	
    check_state	= C_WAIT_HASH_GENERATION;
    break;

  case C_WAIT_HASH_GENERATION :
    if (hash_generator_out_cmd.rspval)
      //New value is available
    {
      hash_s[0] = hash_generator_in_data.data[0];
      hash_s[1] = hash_generator_in_data.data[1];

      bool ok;
      switch (cmd_in.src.read())
      {
      case 0:
	ok = fill_64(register_0_in.be, set_address_s + index_s * SIZE_64_IN_BYTES);
	break;
      case 1:
	ok = fill_64(register_1_in.be, set_address_s + index_s * SIZE_64_IN_BYTES);
//	DEBUG(NONE, STD, "%u %x\n", ok, register_1_in.be.read());
	break;
      case 2:
	ok = fill_64(register_2_in.be, set_address_s + index_s * SIZE_64_IN_BYTES);
	break;
      default:
	ok = false;
      break;
      }

      if (ok)
      {
	if (index_s == HASH_ARITY - 1)
	{
	  check_state	     = C_START_READ_REFERENCE_PARENT;
	  r_parent_address_s = mt_getParentLeavesAddress(
	    set_address_s, 
	    ht_start_address_s,
	    ht_page_size_s,
	    p_mb_address.read(),
	    p_size.read());
	}
	else
	  check_state = C_START_HASH_GENERATION;
      }
    }
    break;

  case  C_START_READ_REFERENCE_PARENT :
    check_state = C_WAIT_READ_REFERENCE_PARENT;
    index_s 	= mt_getBlockIndex(r_parent_address_s,
				   ht_page_size_s,
				   p_mb_address.read(),
				   p_size.read());
    break;

  case  C_START_CHECK_REFERENCE_PARENT :
    check_state = C_WAIT_READ_REFERENCE_PARENT;
    index_s 	= mt_getBlockIndex(r_parent_address_s,
				   ht_page_size_s,
				   p_mb_address.read(),
				   p_size.read());
    break;

  case C_WAIT_READ_REFERENCE_PARENT :
    if (hash_in_ack.rack)
    {
      reference_hash_s[0] = hash_in_rdata.data[0];
      reference_hash_s[1] = hash_in_rdata.data[1];

      if (hash_in_ack.hit)
	//Cache Hit: no need to restore brother
      {
	if (hash_generator_out_cmd.rspval)
	{
	  hash_s[0] = hash_generator_in_data.data[0];
	  hash_s[1] = hash_generator_in_data.data[1];
	  //Compare two Hashes
	  if (compare(hash_s, reference_hash_s, SIZE_64_IN_BYTES))
	  {
	    check_state = HTC_IDLE;
	  }
	  else
	  {
	    check_state = C_INTEGRITY_ERROR;
	  }
	}
      }
      else
	//Cache Miss
      {
	if (hash_generator_out_cmd.rspval)
	{
	  hash_s[0] = hash_generator_in_data.data[0];
	  hash_s[1] = hash_generator_in_data.data[1];

	  //Compare two Hashes
	  if (compare(hash_s, reference_hash_s, SIZE_64_IN_BYTES))
	  {
	    brother_cpt_s = 0;
	    //Copy reference hash in check_register_s
	    check_register_s[index_s * SIZE_64_IN_WORDS]     = hash_in_rdata.data[0];
	    check_register_s[index_s * SIZE_64_IN_WORDS + 1] = hash_in_rdata.data[1];
	    //Compute brother address
	    brother_address_s = mt_getBrotherAddress(
	      r_parent_address_s,
	      ht_page_size_s,
	      p_mb_address.read(),
	      p_size.read());
	    check_state = C_START_RESTORE_REFERENCE_PARENT_BROTHERS;
	  }
	  else 
	  {
	    check_state = C_INTEGRITY_ERROR;
	  }
	}
      }
    }
    break;

  case C_START_RESTORE_REFERENCE_PARENT_BROTHERS :
    index_s = mt_getBlockIndex(brother_address_s, 
			       ht_page_size_s,
			       p_mb_address.read(),
			       p_size.read())*SIZE_64_IN_WORDS;
    check_state = C_WAIT_RESTORE_REFERENCE_PARENT_BROTHERS;
    break;

  case C_WAIT_RESTORE_REFERENCE_PARENT_BROTHERS :
    if (hash_in_ack.rstack)
    {
      check_register_s[index_s]	    = hash_in_rdata.data[0];
      check_register_s[index_s + 1] = hash_in_rdata.data[1];

      if (mt_isLastLevel(r_parent_address_s, 
			 ht_page_size_s,
			 p_mb_address.read(),
			 p_size.read()))
      {
	if (brother_cpt_s == (LAST_HASH_ARITY - 2))
	  //whole set restored
	{
	  r_parent_address_s = mt_getParentAddress(r_parent_address_s,
						   ht_page_size_s,
						   p_mb_address.read(),
						   p_size.read());
	  set_address_s	     = mt_getSetAddress(r_parent_address_s,
						ht_page_size_s,
						p_mb_address.read(),
						p_size.read());
	  check_state	     = C_START_LAST_IC;
	}
	else
	  //Some brothers are still missing	
	{
	  brother_cpt_s	    = brother_cpt_s + 1;
	  brother_address_s = mt_getBrotherAddress(brother_address_s,
						ht_page_size_s,
						p_mb_address.read(),
						p_size.read());
	  check_state       = C_START_RESTORE_REFERENCE_PARENT_BROTHERS;
	}
      }
      else
      {
	if (brother_cpt_s == (HASH_ARITY - 2))
	  //whole set restored
	{
	  r_parent_address_s = mt_getParentAddress(
	    r_parent_address_s, 
	    ht_page_size_s,
	    p_mb_address.read(),
	    p_size.read());
	  set_address_s      = mt_getSetAddress(r_parent_address_s,
						ht_page_size_s,
						p_mb_address.read(),
						p_size.read());
	  check_state        = C_START_CHECK_REFERENCE_PARENT;
	}
	else
	  //Some brothers are still missing	
	{
	  brother_cpt_s	    = brother_cpt_s + 1;
	  brother_address_s = mt_getBrotherAddress(brother_address_s,
						ht_page_size_s,
						p_mb_address.read(),
						p_size.read());
	  check_state	    = C_START_RESTORE_REFERENCE_PARENT_BROTHERS;
	}
      }
    }
    break;

  case  C_START_LAST_IC :
    memcpy(reference_hash_s, rhv_s, SIZE_64_IN_BYTES);;
    check_state =  C_WAIT_LAST_IC;
    break;

  case  C_WAIT_LAST_IC :
    if (hash_generator_out_cmd.rspval)
    {
      hash_s[0] = hash_generator_in_data.data[0];
      hash_s[1] = hash_generator_in_data.data[1];
      if (!master_tree_s)
	//Last three validity, type and size bits must be masked before comparaison
	hash_s[0] = hash_s[0] & 0xFFFFFFF8;
      //Compare two Hashes
      if (compare(hash_s, reference_hash_s, SIZE_64_IN_BYTES))
      {
	check_state = HTC_IDLE;
      }
      else
	check_state = C_INTEGRITY_ERROR;
    }
    break;

  case C_INTEGRITY_ERROR :
    DEBUG(ERROR, STD, "----------------------------------------------------------------\n");
    DEBUG(ERROR, STD, "INTEGRITY VIOLATION DETECTED, ABORTING (cycle %u)...\n", cycle);
    DEBUG(ERROR, STD, "----------------------------------------------------------------\n");
    DEBUG(ERROR, STD, "ERROR : HASH INTEGRITY READ OPERATION\n");
    DEBUG(ERROR, STD, "parent address : %x\n", (uint32_t)r_parent_address_s.read());
    DEBUG(ERROR, STD, "set address : %x\n", (uint32_t)set_address_s.read());
    DEBUG(ERROR, STD, "pspe address : %x\n", (uint32_t)sc_register_in.s_pspe_address.read());
    for(unsigned int i=0;i<SIZE_64_IN_WORDS;i++)
      DEBUG(ERROR, STD, "hash : %x\n", hash_s[i]);
    for(unsigned int i=0;i<SIZE_64_IN_WORDS;i++)
      DEBUG(ERROR, STD, "reference hash : %x\n", reference_hash_s[i]);
    for(unsigned int i=0;i<LAST_HS_SIZE_IN_WORDS;i++)
      DEBUG(ERROR, STD, "set : %x\n", check_register_s[i]);
    //stop simulation
    sc_stop();

    check_state = C_WAIT_INTERRUPT;
    break;

  case C_WAIT_INTERRUPT:
    if (irq.ack.read())
    {
      check_state = HTC_IDLE;
    }
    break;

  default :
    break;
  }
}

//------------------------------------------------------------ pc ------------------------------------------------------------------
void MT_Ctrl::pc()
{
  hash_out_cmd.enable   = false;
  sc_out_cmd.enable     = false;
  sc_out_cmd.mt         = true;
  memory_out_cmd.cmdval = false;
  memory_out_cmd.id     = 4;
  memory_out_cmd.direct_write = false;
  memory_out_wdata.enable = false;

  master_sc_reg_out.enable = false;

  ack_out.cack     = false;
  ack_out.uack     = false;
  ack_out.alap_ack = false;
  ack_out.iack     = false;

  hash_generator_out_cmd.cmdval = false;
  hash_generator_out_cmd.enable = false;
  hash_generator_out_cmd.address = set_address_s;
  hash_generator_out_cmd.cipher = false;
  hash_generator_out_cmd.mode = crypto_mac;
  hash_generator_out_cmd.reg_ready = true;
  hash_generator_out_cmd.no_init = false;
  hash_generator_out_cmd.direct_data = true;
  hash_generator_out_data.enable = false;

  for (size_t i=0;i<DES_KEY_SIZE_IN_WORDS;i++)
  {
    hash_generator_out_cmd.key[i] = master_sc_reg_in.sp[8+i];
  }

  for (size_t i=0;i<IV_SIZE_IN_WORDS;i++)
    hash_generator_out_cmd.iv[i] = 0;

  // update master mac tree root node
  if (write_master_hv_s)
  {
    master_sc_reg_out.enable = true;
    for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
    {
      if (is_init_s)
      {
	master_sc_reg_out.pspe[i] = M[level_s*SIZE_64_IN_WORDS + i];
      }
      else
      {
	master_sc_reg_out.pspe[i] = new_hash_s[i];
      }
    }
  }

//-------------------------------------------------------- initialize_state
  switch (initialize_state) 
  {
  case HTI_IDLE :
    ack_out.iack                        = true;
    break;

  case I_START_WRITE_NULL_BROTHER  :
    memory_out_cmd.cmdval		= true;
    memory_out_cmd.cmd		        = WRITE;
    memory_out_cmd.direct_write         = true;
    memory_out_cmd.start_address	= brother_address_s;
    memory_out_cmd.plen 		= SIZE_64_IN_BYTES;
    memory_out_wdata.data[0]		= 0;
    memory_out_wdata.data[1]		= 0;
    memory_out_wdata.enable             = true;
    break;

  case I_START_RESTORE_BROTHER  :
    hash_out_cmd.enable	        	= true;
    hash_out_cmd.address		= brother_address_s;
    hash_out_cmd.cmd			= MTC_RESTORE;
    if (brother_cpt_s == 0)
    {
      hash_count++;
      hash_generator_out_cmd.enable = true;
      hash_generator_out_cmd.cmdval	= true;
      hash_generator_out_cmd.size	= 0;
      hash_generator_out_cmd.address	= brother_address_s;
    }
    else
    {
      hash_generator_out_cmd.enable = true;
      hash_generator_out_cmd.cmdval	= true;
      hash_generator_out_cmd.size	= SIZE_64_IN_WORDS;
      hash_generator_out_cmd.no_init      = true;
      hash_generator_out_cmd.iv[0]	= hash_s[0];
      hash_generator_out_cmd.iv[1]	= hash_s[1];
      hash_generator_out_data.data[0]	= check_register_s[(brother_cpt_s - 1) * SIZE_64_IN_WORDS];
      hash_generator_out_data.data[1]	= check_register_s[(brother_cpt_s - 1) * SIZE_64_IN_WORDS + 1];
      hash_generator_out_data.enable = true;
    }
    break;


  case I_START_CHECK_LAST_SET  :
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size	        = SIZE_64_IN_WORDS;
    hash_generator_out_cmd.no_init      = true;
    hash_generator_out_cmd.iv[0]	= hash_s[0];
    hash_generator_out_cmd.iv[1]	= hash_s[1];
    hash_generator_out_data.data[0]	= check_register_s[brother_cpt_s * SIZE_64_IN_WORDS];
    hash_generator_out_data.data[1]	= check_register_s[brother_cpt_s * SIZE_64_IN_WORDS + 1];
    hash_generator_out_data.enable = true;
    break;

  case  I_START_READ_SET :
    memory_out_cmd.cmdval		= true;
    memory_out_cmd.cmd   		= READ;
    memory_out_cmd.start_address	= set_address_s;
    memory_out_cmd.plen	        	= HS_SIZE_IN_BYTES;
    //start hash generation
    hash_count++;
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size	        = 0;
    hash_generator_out_cmd.address	= set_address_s;
    break;

  case  I_START_IV_GENERATION :
    //start hash generation
    hash_count++;
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size    	= 0;
    if (mt_isLastLevel(w_parent_address_s, ht_page_size_s,
		       p_mb_address.read(), p_size.read()))
    {
      hash_generator_out_cmd.address	= w_parent_address_s & 0xFFFFFF00;
    }
    else
    {
      hash_generator_out_cmd.address	= w_parent_address_s;
    }
    break;

  case  I_START_UPDATE_LEAVE_PARENT :
    //start hash generation
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size 	= HS_SIZE_IN_WORDS;
    hash_generator_out_cmd.address	= w_parent_address_s;
    hash_generator_out_cmd.no_init	= true;
    hash_generator_out_cmd.iv[0]	= M[0];
    hash_generator_out_cmd.iv[1]	= M[1];
    hash_generator_out_data.enable = true;
    for(unsigned int i=0;i<HS_SIZE_IN_WORDS;i++)
    {
      hash_generator_out_data.data[i]	= register_2_in.data[i];
    }
    break;

  case I_START_UPDATE_PARENT_M  :
    memory_out_cmd.cmdval		= true;
    memory_out_cmd.cmd		        = WRITE;
    memory_out_cmd.start_address	= w_parent_address_s;
    memory_out_cmd.plen		        = SIZE_64_IN_BYTES;
    memory_out_cmd.direct_write         = true;
    memory_out_wdata.enable             = true;
    memory_out_wdata.data[0]		= M[level_s * SIZE_64_IN_WORDS];
    memory_out_wdata.data[1]		= M[level_s * SIZE_64_IN_WORDS + 1];

    //update M[i+1]
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size	= SIZE_64_IN_WORDS;
    hash_generator_out_cmd.address	= w_parent_address_s;
    hash_generator_out_cmd.no_init	= true;
    hash_generator_out_cmd.iv[0]	= M[(level_s + 1) * SIZE_64_IN_WORDS];
    hash_generator_out_cmd.iv[1]	= M[(level_s + 1) * SIZE_64_IN_WORDS + 1];
    hash_generator_out_data.enable = true;
    hash_generator_out_data.data[0]	= M[level_s * SIZE_64_IN_WORDS];
    hash_generator_out_data.data[1]	= M[level_s * SIZE_64_IN_WORDS + 1];
    //
    hash_out_cmd.enable 		= true;
    hash_out_cmd.address		= w_parent_address_s;
    hash_out_cmd.cmd			= MTC_INVALIDATE;
    break;

  case I_START_UPDATE_ROOT_M :
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size 	= SIZE_64_IN_WORDS;
    hash_generator_out_cmd.address	= w_parent_address_s;
    hash_generator_out_cmd.no_init      = true;
    hash_generator_out_cmd.iv[0]	= M[level_s * SIZE_64_IN_WORDS];
    hash_generator_out_cmd.iv[1]	= M[level_s * SIZE_64_IN_WORDS + 1];
    hash_generator_out_data.enable = true;
    hash_generator_out_data.data[0]	= check_register_s[index_s * SIZE_64_IN_WORDS];
    hash_generator_out_data.data[1]	= check_register_s[index_s * SIZE_64_IN_WORDS + 1];
    break;

  case I_START_WRITE_PSPE :
    //Write in alap regsiter
    sc_out_cmd.enable	  		= true;
    sc_out_cmd.cmd	    		= CTX_CMD_UPDATE;
    sc_out_cmd.address		        = sc_register_in.s_pspe_address;
    for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
    {
      sc_out_wdata.data[i] 		= new_spspe_s[i];
    }
    break;

  case I_INTEGRITY_ERROR:
    irq.irq = true;
    irq.address = set_address_s.read();
    irq.type = io_status_e_ht;
    irq.op = io_status_e_store;
    break;

  default :
    break;
  }

//----------------------------------------------------- update_state
  switch (update_state) 
  {
  case HTU_IDLE : 
    ack_out.uack			= true;
    ack_out.alap_ack			= true;
    break;

  case U_START_PARENT_UPDATE:
    hash_count++;
    //start hash generation
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size 	= HS_SIZE_IN_WORDS;
    hash_generator_out_data.enable = true;
    for(size_t i=0;i<HS_SIZE_IN_WORDS;i++)
    {
      switch (src_s.read())
      {
      case 0:
	hash_generator_out_data.data[i]	= register_0_in.data[i];
      break;

      case 1:
	hash_generator_out_data.data[i]	= register_1_in.data[i];
	break;

      case 2:
	hash_generator_out_data.data[i]	= register_2_in.data[i];
	break;

      default:
	break;
      }
    }
    break;

  case U_START_WRITE_NEW_PARENT :
    hash_out_cmd.enable		        = true;
    hash_out_cmd.address		= w_parent_address_s;
    hash_out_cmd.cmd			= MTC_WRITE;
    hash_out_wdata.data[0]		= new_hash_s[0];
    hash_out_wdata.data[1]		= new_hash_s[1];
    break;

  case U_START_READ_SLAVE_PSPE :
    sc_out_cmd.enable		  	= true;
    sc_out_cmd.address   		= victim_address_s;
    sc_out_cmd.cmd			= CTX_CMD_READ;
    break;

  case U_WAIT_READ_SLAVE_PSPE :
    ack_out.alap_ack	                = true;
    break;

  case U_START_READ_VICTIM_SET :
    hash_out_cmd.enable   		= true;
    hash_out_cmd.address		= brother_address_s;
    hash_out_cmd.cmd			= MTC_READ;
    hash_generator_out_cmd.enable = true;
    if (brother_cpt_s == 0)
    {
      hash_count++;
      hash_generator_out_cmd.cmdval	= true;
      hash_generator_out_cmd.size	= 0;
      hash_generator_out_cmd.address	= brother_address_s;
    }
    else
    {
      hash_generator_out_cmd.cmdval	= true;
      hash_generator_out_cmd.size	= SIZE_64_IN_WORDS;
      hash_generator_out_cmd.no_init	= true;
      hash_generator_out_cmd.iv[0]	= hash_s[0];
      hash_generator_out_cmd.iv[1]	= hash_s[1];
      hash_generator_out_data.enable = true;
      hash_generator_out_data.data[0]	= check_register_s[(brother_cpt_s - 1) * SIZE_64_IN_WORDS];
      hash_generator_out_data.data[1]	= check_register_s[(brother_cpt_s - 1) * SIZE_64_IN_WORDS + 1];
    }
    break;

  case U_START_NEW_HASH_GENERATION:
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size  	= SIZE_64_IN_WORDS;
    hash_generator_out_cmd.no_init	= true;
    hash_generator_out_cmd.iv[0]	= hash_s[0];
    hash_generator_out_cmd.iv[1]	= hash_s[1];
    hash_generator_out_data.enable = true;
    hash_generator_out_data.data[0]	= check_register_s[brother_cpt_s * SIZE_64_IN_WORDS];
    hash_generator_out_data.data[1]	= check_register_s[brother_cpt_s * SIZE_64_IN_WORDS + 1];
    break;

  case U_START_RESTORE_DIRTY_VICTIM_BROTHERS :
    hash_out_cmd.enable 		= true;
    hash_out_cmd.address		= restore_address_s;
    hash_out_cmd.cmd			= MTC_RESTORE;
    break;

  case U_START_READ_REFERENCE_PARENT:
    hash_out_cmd.enable	        	= true;
    hash_out_cmd.address		= r_parent_address_s;
    hash_out_cmd.cmd			= MTC_READ;
    //start hash generation
    hash_count++;
    hash_generator_out_cmd.enable       = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size	        = HS_SIZE_IN_WORDS;
    hash_generator_out_data.enable = true;
    for(size_t i=0;i<HS_SIZE_IN_WORDS;i++)
      hash_generator_out_data.data[i]	= check_register_s[i];
    break;

  case U_START_LAST_IC:
    //start hash generation
    hash_count++;
    hash_generator_out_cmd.enable       = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size	        = LAST_HS_SIZE_IN_WORDS;
    hash_generator_out_data.enable = true;
    for(size_t i=0;i<LAST_HS_SIZE_IN_WORDS;i++)
      hash_generator_out_data.data[i]	= check_register_s[i];
    break;

  case U_START_WRITE_PSPE :
    sc_out_cmd.enable	  		= true;
    sc_out_cmd.cmd	    		= CTX_CMD_UPDATE;
    sc_out_cmd.address   		= sc_register_in.s_pspe_address;
    for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
      sc_out_wdata.data[i] 		= new_spspe_s[i];

    break;

  case U_WAIT_WRITE_PSPE :
    ack_out.alap_ack			= true;
    break;

  case U_START_SYNCHRONIZE_DIRTY_VICTIM_BROTHERS :
    hash_out_cmd.enable	        	= true;
    hash_out_cmd.address		= synchronize_address_s;
    hash_out_cmd.cmd			= MTC_SYNCHRONIZE;
    break;

  case U_START_RESTORE_REFERENCE_PARENT_BROTHERS :
    hash_out_cmd.enable		        = true;
    hash_out_cmd.address		= brother_address_s;
    hash_out_cmd.cmd			= MTC_RESTORE;
    break;

  case U_INTEGRITY_ERROR:
    irq.irq = true;
    irq.address = set_address_s.read();
    irq.type = io_status_e_ht;
    irq.op = io_status_e_store;
    break;

  default :
    break;
  }

//------------------------------------------------------- check_state
  switch (check_state) 
  {
  case HTC_IDLE : 
    ack_out.cack 			= true;
    break;

  case C_START_IV_GENERATION :
    hash_count++;
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size	        = 0;
    hash_generator_out_cmd.address	= set_address_s;
    break;

  case C_START_HASH_GENERATION :
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size	        = SIZE_64_IN_WORDS;
    hash_generator_out_cmd.no_init	= true;
    hash_generator_out_cmd.iv[0]	= hash_s[0];
    hash_generator_out_cmd.iv[1]	= hash_s[1];
    hash_generator_out_data.enable = true;
    switch (src_s.read())
    {
    case 0:
//      DEBUG(NONE, STD, "ref[%08x]: %x %x\n", register_0_in.address.read(), register_0_in.data[index_s * SIZE_64_IN_WORDS].read(), register_0_in.data[index_s * SIZE_64_IN_WORDS + 1].read());
      hash_generator_out_data.data[0]	= register_0_in.data[index_s * SIZE_64_IN_WORDS];
      hash_generator_out_data.data[1]	= register_0_in.data[index_s * SIZE_64_IN_WORDS + 1];
      break;

    case 1:
      hash_generator_out_data.data[0]	= register_1_in.data[index_s * SIZE_64_IN_WORDS];
      hash_generator_out_data.data[1]	= register_1_in.data[index_s * SIZE_64_IN_WORDS + 1];
      break;

    case 2:
      hash_generator_out_data.data[0]	= register_2_in.data[index_s * SIZE_64_IN_WORDS];
      hash_generator_out_data.data[1]	= register_2_in.data[index_s * SIZE_64_IN_WORDS + 1];
      break;

    default:
      break;
    }
    break;

  case C_START_READ_REFERENCE_PARENT:
    hash_out_cmd.enable 		= true;
    hash_out_cmd.address		= r_parent_address_s;
    hash_out_cmd.cmd			= MTC_READ; 
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size   	= SIZE_64_IN_WORDS;
    hash_generator_out_cmd.no_init	= true;
    hash_generator_out_cmd.iv[0]	= hash_s[0];
    hash_generator_out_cmd.iv[1]	= hash_s[1];
    hash_generator_out_data.enable = true;
    switch (src_s.read())
    {
    case 0:
//      DEBUG(NONE, STD, "ref[%08x]: %x %x\n", register_0_in.address.read(), register_0_in.data[index_s * SIZE_64_IN_WORDS].read(), register_0_in.data[index_s * SIZE_64_IN_WORDS + 1].read());
      hash_generator_out_data.data[0]	= register_0_in.data[index_s * SIZE_64_IN_WORDS];
      hash_generator_out_data.data[1]	= register_0_in.data[index_s * SIZE_64_IN_WORDS + 1];
      break;

    case 1:
      hash_generator_out_data.data[0]	= register_1_in.data[index_s * SIZE_64_IN_WORDS];
      hash_generator_out_data.data[1]	= register_1_in.data[index_s * SIZE_64_IN_WORDS + 1];
      break;

    case 2:
      hash_generator_out_data.data[0]	= register_2_in.data[index_s * SIZE_64_IN_WORDS];
      hash_generator_out_data.data[1]	= register_2_in.data[index_s * SIZE_64_IN_WORDS + 1];
      break;

    default:
      break;
    }
    break;

  case  C_START_CHECK_REFERENCE_PARENT :
    hash_out_cmd.enable 		= true;
    hash_out_cmd.address		= r_parent_address_s;
    hash_out_cmd.cmd			= MTC_READ; 
    //start hash generation
    hash_count++;
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size	        = HS_SIZE_IN_WORDS;
    hash_generator_out_data.enable = true;
    for(unsigned int i=0;i<HS_SIZE_IN_WORDS;i++)
      hash_generator_out_data.data[i]	= check_register_s[i];
    break;

  case  C_START_LAST_IC :
    //start hash generation
    hash_count++;
    hash_generator_out_cmd.enable = true;
    hash_generator_out_cmd.cmdval	= true;
    hash_generator_out_cmd.size 	= LAST_HS_SIZE_IN_WORDS;
    hash_generator_out_data.enable = true;
    for(unsigned int i=0;i<LAST_HS_SIZE_IN_WORDS;i++)
      hash_generator_out_data.data[i]	= check_register_s[i];
    break;

  case C_START_RESTORE_REFERENCE_PARENT_BROTHERS :
    hash_out_cmd.enable		        = true;
    hash_out_cmd.address		= brother_address_s;
    hash_out_cmd.cmd			= MTC_RESTORE;
    break;

  case C_INTEGRITY_ERROR:
    irq.irq = true;
    irq.address = set_address_s.read();
    irq.type = io_status_e_ht;
    irq.op = io_status_e_load;
    break;

  default:
    break;
  }
}

void MT_Ctrl::print_stats()
{
  DEBUG(STAT, STD, "- HASH OPERATION = %u\n", hash_count);
}

void MT_Ctrl::reset_stats()
{
  hash_count = 0;
}


