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

SecurityCtx_Ctrl::SecurityCtx_Ctrl(sc_module_name name,
				   const cache_param_s &pspe_param,
				   const cache_param_s &sp_param):
  sc_module(name),
  p_clk("clk"),
  p_resetn("resetn")
{
  pspe_cache_param = pspe_param;
  sp_cache_param   = sp_param;

  SC_METHOD(transition);
  dont_initialize();
  sensitive << p_clk.pos();

  SC_METHOD(genMoore);
  dont_initialize();
  sensitive << p_clk.neg();

  // cache instantiation even if they are disabled (we can enable both
  // of them with the SecBus IO after startup)
  pspe_cache = new Cache(pspe_param);
  sp_cache = new Cache(sp_param);

  current_sp   = SP_ALLOC();
  
  DEBUG(NONE, STD, "[SECBUS] Building SecurityCtx_Ctrl (%s)\n",
	basename());
}

SecurityCtx_Ctrl::~SecurityCtx_Ctrl()
{
  delete pspe_cache;
  delete sp_cache;

  SP_DEALLOC(current_sp);
}


void SecurityCtx_Ctrl::printPSPE()
{
  DEBUG(NONE, STD, "PSPE: %08x\n", (uint32_t)pspe_address_s.read());
  DEBUG(NONE, STD, "PSPE: %08x %08x\n", current_pspe[0], current_pspe[1]);
  DEBUG(NONE, STD, "PSPE: v=%u p=%u s=%u sp=%u mt=%x\n",
	(uint32_t)pspe_getValid(&current_pspe),
	(uint32_t)pspe_getProt(&current_pspe),
	(uint32_t)pspe_getSize(&current_pspe),
	(uint32_t)pspe_getSPIdx(&current_pspe),
	(uint32_t)(pspe_getMSAdd(&current_pspe)+pspe_getMSIdx(&current_pspe)*0x400));
}

void SecurityCtx_Ctrl::checkMPSPE()
{
  if (pspe_getValid(&current_pspe))
  {
    size_t psize = pspe_getSize(&current_pspe);

    if (psize == size_s.read())
    {
      // we've found the PSPE
      for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	ctx_m_pspe_s[i] = current_pspe[i];

      ctx_page_address_s = pspe_address_s.read();
      
      if (pspe_getProt(&current_pspe)) // no protection
      {
	for (uint32_t i=0;i<SP_SIZE_IN_WORDS;i++)
	  ctx_sp_s[i] = 0;

	fsm_s = FSM_IDLE;
      }
      else
      {
	round_s = io_status_e_sp;
	if (p_cache_sp.read())
	{
	  addr_t sp_address;
	  sp_address_s = sp_address = getSPAddressFromIdx(
	    pspe_getSPIdx(&current_pspe),
	    p_mb_address.read(),
	    p_size.read());

	  hit_s = sp_cache->lookup(sp_address, sp_victim_block);
	  
	  first_s = true;

	  fsm_s = FSM_HIT_SP;
	}
	else
	{
	  first_s = true;
	
	  sp_address_s = getSPAddressFromIdx(
	    pspe_getSPIdx(&current_pspe),
	    p_mb_address.read(),
	    p_size.read());

	  fsm_s = FSM_READ_SP;
	}
      }

      return;
    }
  }
   
  // each level has been tested, raise an interrupt
  if (size_s.read() == getPageLevel() - 1)
  {
    DEBUG(NONE, STD, "error MPSPE %x %x\n", pspe_address_s.read(), address_s.read());
    DEBUG(NONE, STD, "%x %x\n", current_pspe[0], current_pspe[1]);
    printPSPE();
    sc_stop();
    fsm_s = FSM_INTERRUPT;
  }
  else
  {
    size_s = size_s.read() + 1;
    addr_t pspe_address;
    pspe_address_s = pspe_address = getPSPEAddressFromPageSize(
      address_s.read(),
      size_s + 1,
      p_mb_address.read(),
      p_b_address.read());

    round_s = io_status_e_pspe;
    if (p_cache_pspe.read())
    {
      hit_s = pspe_cache->lookup(pspe_address, pspe_victim_block);

      fsm_s = FSM_HIT_PSPE;
    }
    else
    {
      fsm_s = FSM_READ_PSPE;
    }
  }   
}

void SecurityCtx_Ctrl::checkSPSPE()
{
  if (pspe_getValid(&current_pspe))
  {
    size_t psize = pspe_getSize(&current_pspe);

    if (psize == size_s.read())
    {
      // we've found the SPSPE
      for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	ctx_s_pspe_s[i] = current_pspe[i];

      ctx_s_pspe_address_s = pspe_address_s.read();

      pspe_t m_pspe;
      m_pspe[0] = ctx_m_pspe_s[0];
      m_pspe[1] = ctx_m_pspe_s[1];

      /** TODO : need to be optimized **/
      if (pspe_address_s.read() != getPSPEAddressFromPageSize(pspe_getMSAdd(&m_pspe),
							      pspe_getSize(&m_pspe),
							      p_mb_address.read(),
							      p_b_address.read()))
      {
	ctx_page_address_s = 0xFFFFFFFF;
      }
      
      fsm_s = FSM_IDLE;
      return;
    }

    if (size_s.read() == getPageLevel() - 1)
    {
      DEBUG(NONE, STD, "error\n");
      fsm_s = FSM_INTERRUPT;
    }
    else
    {
      size_s = size_s.read() + 1;
      addr_t pspe_address;
      pspe_address_s = pspe_address = getPSPEAddressFromPageSize(
	address_s.read(),
	size_s + 1,
	p_mb_address.read(),
	p_b_address.read());

      round_s = io_status_e_pspe;
      if (p_cache_pspe.read())
      {
 	hit_s = pspe_cache->lookup(pspe_address, pspe_victim_block);
	
	fsm_s = FSM_HIT_PSPE;
      }
      else
      {
	fsm_s = FSM_READ_PSPE;
      }
    }
  }
}

void SecurityCtx_Ctrl::checkSP()
{
  if (sp_getValid(&current_sp))
  {
    if (sp_getModeI(&current_sp) == sp_mode_i_mactree)
    {
      slave_s = true;

      uint32_t spspe_address = getPSPEAddressFromPageSize(pspe_getMSAdd(&current_pspe), 
						  pspe_getSize(&current_pspe),
						  p_mb_address.read(),
						  p_b_address.read());
      
      pspe_address_s = spspe_address;
      ctx_s_pspe_address_s = spspe_address;

      if (p_cache_pspe.read())
      {
	hit_s = pspe_cache->lookup(spspe_address, pspe_victim_block);

	fsm_s = FSM_HIT_PSPE;
      }
      else
      {
	fsm_s = FSM_READ_PSPE;
      }
    }
    else
    {
      fsm_s = FSM_IDLE;
    }
  }
  else
  {
    DEBUG(NONE, STD, "error sp %x\n", sp_address_s.read());
    printPSPE();
    DEBUG(NONE, STD, "request from %x\n", address_s.read());
    fsm_s = FSM_INTERRUPT;
  }
}

void SecurityCtx_Ctrl::transition()
{
  m_cycle++;

  if (!p_resetn.read())
  {
    fsm_s = FSM_IDLE;

    m_cycle = 0;

    return;
  }

  DEBUG(TRACE, FSM, "[%s]\tFSM\t%u\n",
	basename(), (uint32_t)fsm_s.read());

  switch (fsm_s.read())
  {
  case FSM_IDLE:
    if (!ctx_cmd.enable.read())
      break;

    address_s = ctx_cmd.address.read();
    cmd_s = ctx_cmd.cmd.read();
    size_s = 0;
    slave_s = false;
    mt_s = ctx_cmd.mt.read();

//    flush_reg2_s = true;
    if (ctx_cmd.cmd.read() == CTX_CMD_READ)
    {
//      DEBUG(NONE, STD, "read ctx for %x (mt=%u)\n", ctx_cmd.address.read(), ctx_cmd.mt.read());
      addr_t pspe_address;
      pspe_address_s = pspe_address = getPSPEAddressFromPageSize(
	ctx_cmd.address.read(),
	0,
	p_mb_address.read(),
	p_b_address.read()
	);

      round_s = io_status_e_pspe;
    
      if (p_cache_pspe.read())
      {
	hit_s = pspe_cache->lookup(pspe_address, pspe_victim_block);
	
	fsm_s = FSM_HIT_PSPE;
      }
      else
      {
	fsm_s = FSM_READ_PSPE;
      }
    }
    else if (ctx_cmd.cmd.read() == CTX_CMD_UPDATE)
    {
      // address between PSPEBaseAddress and SPBaseAddress
      // it's  PSPE update
      if (inclusion(ctx_cmd.address.read(),
		    getPSPEBaseAddress(p_mb_address.read(),
				       p_size.read()),
		    getSPBaseAddress(p_mb_address.read(),
				     p_size.read())))
      {
	uint32_t idx = index(ctx_cmd.address.read(), SIZE_256_IN_BYTES);

	if (ctx_cmd.mt.read())
	{
	  for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	  {
	    current_pspe[i] = w_data.data[i].read();
	    pspe_write_data_s[i] = w_data.data[i].read();
	  }

	  if (ctx_cmd.address.read() == ctx_s_pspe_address_s.read())
	  {
	    for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	    {
	      ctx_s_pspe_s[i] = current_pspe[i];
	    }
	  }
	}
	else
	{
	  for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	  {
	    current_pspe[i] = r_register_0.data[idx+i].read();
	    pspe_write_data_s[i] = r_register_0.data[idx+i].read();
	  }
	}
	pspe_address_s = ctx_cmd.address.read();

	pspe_write_address_s = ctx_cmd.address.read();

  	if  (p_cache_pspe.read())
	{
	  hit_s = pspe_cache->lookup(ctx_cmd.address.read(), pspe_victim_block);
	  if (pspe_cache_param.write_policy == WRITE_THROUGH)
	  {
	    is_pspe_s = true;
	    fsm_s = FSM_FLUSH_REG2;
	  }
	  else
	  {
	    fsm_s = FSM_HIT_PSPE;
	  }
	}
	else
	{
	  is_pspe_s = true;
	  fsm_s = FSM_FLUSH_REG2;
	}
	  
      }
      else if (inclusion(ctx_cmd.address.read(),
			 getSPBaseAddress(p_mb_address.read(),
					  p_size.read()),
			 getMMacTBaseAddress(p_mb_address.read(),
					     p_size.read())))
      {
	sp_address_s = ctx_cmd.address.read();
	for (uint32_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
	  current_sp[i] = r_register_0.data[i].read();

	sp_write_address_s = ctx_cmd.address.read();
	for (uint32_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
	  sp_write_data_s[i] = r_register_0.data[i].read();
	
	if (p_cache_sp.read())
	{
	  hit_s = sp_cache->lookup(ctx_cmd.address.read(), sp_victim_block);

	  if (sp_cache_param.write_policy == WRITE_THROUGH)
	  {
	    is_pspe_s = false;
	    fsm_s = FSM_FLUSH_REG2;
	  }
	  else
	  {
	    fsm_s = FSM_HIT_SP;
	  }
	}
	else
	{
	  is_pspe_s = false;
	  fsm_s = FSM_FLUSH_REG2;
	}
      }
      else
      {
	DEBUG(NONE, STD, "Try to update PSPE/SP with a non valid address (%08x) at cycle=%u\n", (uint32_t)ctx_cmd.address.read(), m_cycle);
	round_s = io_status_e_pspe;
	fsm_s = FSM_INTERRUPT;
      }
    }
    else
    {
      DEBUG(ERROR, STD, "Cannot understand the SecurityCtx Controller command\n");
    }
    break;

  case FSM_FLUSH_REG2:
    if (is_pspe_s)
      fsm_s = FSM_WRITE_PSPE;
    else
      fsm_s = FSM_WRITE_SP;

    break;

  case FSM_HIT_PSPE:
    if (hit_s.read())
    {
      if (cmd_s.read() == CTX_CMD_READ || slave_s.read())
      {
	// hit, we retrieve the PSPE from the cache
	memcpy(current_pspe, pspe_cache->readCache(pspe_address_s.read()),
	       pspe_cache_param.block_size*sizeof(int));

	if (slave_s.read())
	{
	  ctx_s_pspe_address_s = pspe_address_s;
	  for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	    ctx_s_pspe_s[i] = current_pspe[i];

	  fsm_s = FSM_IDLE;
	}
	else
	{
	  if (mt_s.read())
	  {
	    checkSPSPE();
	  }
	  else
	  {
	    checkMPSPE();
	  }
	}
      }
      else if (cmd_s.read() == CTX_CMD_UPDATE)
      {
	pspe_cache->writeCache(pspe_address_s, current_pspe, 
			       pspe_cache_param.block_size);

	fsm_s = FSM_IDLE;
      }
    }
    else // miss
    {
      if (cmd_s.read() == CTX_CMD_READ || slave_s.read())
      {
	if (pspe_cache_param.write_policy == WRITE_THROUGH)
	{
	  fsm_s = FSM_READ_PSPE;
	}
	else
	{
	  if (pspe_victim_block->isDirty())
	  {
	    pspe_write_address_s = pspe_victim_block->getBaseAddress();
	    for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	      pspe_write_data_s[i] = pspe_victim_block->getData()[i];

	    is_pspe_s = true;
	    fsm_s = FSM_FLUSH_REG2;
	  }
	  else
	    fsm_s = FSM_READ_PSPE;
	}
      }
      else if (cmd_s.read() == CTX_CMD_UPDATE)
      {
	if (pspe_victim_block->isDirty())
	{
	  pspe_write_address_s = pspe_victim_block->getBaseAddress();
	  for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	    pspe_write_data_s[i] = pspe_victim_block->getData()[i];

	  is_pspe_s = true;
	  fsm_s = FSM_FLUSH_REG2;
	}
	else
	{
	  pspe_cache->replace(pspe_address_s.read(), current_pspe, true);

	  fsm_s = FSM_IDLE;
	}
      }
    }
    break;

  case FSM_READ_PSPE:
    if (sc_out_cmd.cmdack.read())
    {
      fsm_s = FSM_WREAD_PSPE;
    }
    break;

  case FSM_WREAD_PSPE:
    if (sc_out_cmd.rspval.read())
    {
      uint32_t idx = index(pspe_address_s.read(), SIZE_256_IN_BYTES);
      for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	current_pspe[i] = r_register_0.data[i+idx].read();

      if (p_cache_pspe.read())
	pspe_cache->replace(pspe_address_s, current_pspe, false);

      if (slave_s.read())
      {
	for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	  ctx_s_pspe_s[i] = r_register_0.data[idx+i];

	fsm_s = FSM_IDLE;
      }
      else
      {
	if (mt_s.read())
	{
	  checkSPSPE();
	}
	else
	{
	  checkMPSPE();
	}
      }
    }
    break;

  case FSM_HIT_SP:
    if (hit_s.read())
    {
      if (cmd_s.read() == CTX_CMD_READ)
      {
	if (first_s.read())
	  memcpy(current_sp, sp_cache->readCache(sp_address_s),
		 sp_cache_param.block_size*sizeof(int));
	else
	  memcpy(current_sp+SP_SIZE_IN_WORDS/2, sp_cache->readCache(sp_address_s),
		 sp_cache_param.block_size*sizeof(int));

	if (first_s.read())
	{
	  for (uint32_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
	  {
	    ctx_sp_s[i] = current_sp[i];
	  }

	  first_s = false;

	  sp_address_s = sp_address_s.read() + 32;
	  hit_s = sp_cache->lookup(sp_address_s.read()+32, sp_victim_block);

	  fsm_s = FSM_HIT_SP;
	}
	else
	{
	  for (uint32_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
	    ctx_sp_s[8+i] = current_sp[i];

	  checkSP();
	}
      }
      else if (cmd_s.read() == CTX_CMD_UPDATE)
      {
	uint32_t sp[SIZE_256_IN_WORDS];
	for (uint32_t i=0;i<SIZE_256_IN_WORDS;i++)
	  sp[i] = r_register_0.data[i];

	sp_cache->writeCache(sp_address_s.read(), sp,
			     sp_cache_param.block_size);
      }
    }
    else //miss
    {
      if (cmd_s.read() == CTX_CMD_READ)
      {
	if (sp_cache_param.write_policy == WRITE_THROUGH)
	  fsm_s = FSM_READ_SP;
	else
	{
	  if (sp_victim_block->isDirty())
	  {
	    sp_write_address_s = sp_victim_block->getBaseAddress();

	    for (uint32_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
	      sp_write_data_s[i] = sp_victim_block->getData()[i];

	    is_pspe_s = false;
	    fsm_s = FSM_FLUSH_REG2;
	  }
	  else
	    fsm_s = FSM_READ_SP;
	}
      }
      else if (cmd_s.read() == CTX_CMD_UPDATE)
      {
	if (sp_victim_block->isDirty())
	{
	  sp_write_address_s = sp_victim_block->getBaseAddress();
	  for (uint32_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
	    sp_write_data_s[i] = sp_victim_block->getData()[i];

	  is_pspe_s = false;
	  fsm_s = FSM_FLUSH_REG2;
	}
	else
	{
	  sp_cache->replace(sp_address_s.read(), current_sp, true);

	  fsm_s = FSM_IDLE;
	}
      }
    }
    break;

  case FSM_READ_SP:
    if (sc_out_cmd.cmdack.read())
    {
      fsm_s = FSM_WREAD_SP;
    }
    break;

  case FSM_WREAD_SP:
    if (sc_out_cmd.rspval.read())
    {
      uint32_t sp_to_cache[SP_SIZE_IN_WORDS/2];
      for (uint32_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
      {
	sp_to_cache[i] = r_register_0.data[i].read();
      }

      if (p_cache_sp.read())
	sp_cache->replace(sp_address_s, sp_to_cache, false);
      if (first_s.read())
      {
	for (uint32_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
	{
	  ctx_sp_s[i] = sp_to_cache[i];
	  current_sp[i] = sp_to_cache[i];
	}
	
	first_s = false;
	sp_address_s = sp_address_s.read() + 32;

	if (p_cache_sp.read())
	{
	  hit_s = sp_cache->lookup(sp_address_s.read()+32, sp_victim_block);

	  fsm_s = FSM_HIT_SP;
	}
	else
	{
	  fsm_s = FSM_READ_SP;
	}
      }
      else
      {
	for (uint32_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
	{
	  ctx_sp_s[i+8] = sp_to_cache[i];
	  current_sp[i+8] = sp_to_cache[i];
	}

	checkSP();
      }
    }      
    break;

  case FSM_WRITE_PSPE:
    if (sc_out_cmd.cmdack.read())
    {
      fsm_s = FSM_WWRITE_PSPE;
    }
    break;

  case FSM_WWRITE_PSPE:
    if (sc_out_cmd.rspval.read() || mt_s.read() || cmd_s.read() == CTX_CMD_UPDATE)
    {
      if (cmd_s.read() == CTX_CMD_READ)
	fsm_s = FSM_READ_PSPE;
      else
      {
/*	if (pspe_address_s == ctx_s_pspe_address_s.read())
	{
	  for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
	  {
	    ctx_s_pspe_s[i] = current_pspe[i];
	  }
	  }*/

	if (hit_s.read())
	{
	  pspe_cache->writeCache(pspe_address_s.read(), current_pspe,
				 pspe_cache_param.block_size);
	}
	else
	{
	  pspe_cache->replace(pspe_address_s.read(), current_pspe, true);
	}

	fsm_s = FSM_IDLE;
      }
    }
    break;

  case FSM_WRITE_SP:
    if (sc_out_cmd.cmdack.read())
    {
      fsm_s = FSM_WWRITE_SP;
    }
    break;

  case FSM_WWRITE_SP:
    if (sc_out_cmd.rspval.read() || cmd_s.read() == CTX_CMD_UPDATE)
    {
      if (cmd_s.read() == CTX_CMD_READ)
	fsm_s = FSM_READ_SP;
      else
      {
	if (hit_s.read())
	{
	  sp_cache->replace(sp_address_s.read(), current_sp,
			    sp_cache_param.block_size);
	}
	else
	{
	  sp_cache->replace(sp_address_s.read(), current_sp, true);
	}
	fsm_s = FSM_IDLE;
      }
    }
    break;

  case FSM_INTERRUPT:
    fsm_s = FSM_WAIT_INTERRUPT;
    break;

  case FSM_WAIT_INTERRUPT:
    if (irq.ack.read())
    {
      fsm_s = FSM_IDLE;
    }
    break;

  default:
    break;
  }
}

void SecurityCtx_Ctrl::genMoore()
{
  ctx_register.page_address = ctx_page_address_s.read();
  ctx_register.s_pspe_address = ctx_s_pspe_address_s.read();

  for (size_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
    ctx_register.sp[i]      = ctx_sp_s[i].read();
  
  for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
  {
    ctx_register.m_pspe[i]  = ctx_m_pspe_s[i].read();
    ctx_register.s_pspe[i]  = ctx_s_pspe_s[i].read();
  }

  w_register_2.enable = false;
  w_register_2.flush = false;
  
  sc_out_cmd.cmdval = false;
  ctx_cmd.ack = false;

  irq.irq = false;

  switch (fsm_s.read())
  {
  case FSM_IDLE:
    ctx_cmd.ack = true;
    break;

  case FSM_HIT_PSPE:
    break;

  case FSM_READ_PSPE:
    sc_out_cmd.address = aligned(pspe_address_s.read(), SIZE_256_IN_BYTES);
    sc_out_cmd.enable = true;
    sc_out_cmd.cmdval = true;
    sc_out_cmd.cmd    = READ;
    sc_out_cmd.be     = 0xFFFFFFFF;
    sc_out_cmd.mb     = true;
    break;

  case FSM_FLUSH_REG2:
    w_register_2.enable = true;
    w_register_2.flush  = true;
    break;

  case FSM_HIT_SP:
    break;

  case FSM_READ_SP:
    sc_out_cmd.address = sp_address_s.read();
    sc_out_cmd.enable = true;
    sc_out_cmd.cmdval = true;
    sc_out_cmd.cmd    = READ;
    sc_out_cmd.be     = 0xFFFFFFFF;
    sc_out_cmd.mb     = true;
    break;

  case FSM_WRITE_PSPE:
    w_register_2.enable = true;
    w_register_2.address = pspe_write_address_s.read();
    {
      uint32_t idx = index(pspe_write_address_s.read(), SIZE_256_IN_BYTES);
      w_register_2.be = 0xFF << (idx*4);
      for (uint32_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
      {
	w_register_2.data[idx+i] = pspe_write_data_s[i].read();
      }
    } 

    sc_out_cmd.address = pspe_write_address_s.read();
    sc_out_cmd.enable  = true;
    sc_out_cmd.cmdval  = true;
    sc_out_cmd.cmd     = WRITE;
    sc_out_cmd.be      = 0xFF;
    sc_out_cmd.mb      = true;
    break;

  case FSM_WRITE_SP:
    w_register_2.enable  = true;
    w_register_2.address = sp_write_address_s.read(); 
    w_register_2.be      = 0xFFFFFFFF;
    for (uint32_t i=0;i<SP_SIZE_IN_WORDS/2;i++)
      w_register_2.data[i] = sp_write_data_s[i].read();

    sc_out_cmd.address = sp_write_address_s.read();
    sc_out_cmd.enable  = true;
    sc_out_cmd.cmdval  = true;
    sc_out_cmd.cmd     = WRITE;
    sc_out_cmd.be      = 0xFFFFFFFF;
    sc_out_cmd.mb      = true;
    break;

  case FSM_INTERRUPT:
    irq.irq = true;
    irq.address = address_s.read();
    irq.type = round_s.read();
    irq.op = (cmd_s.read() == CTX_CMD_READ) ? io_status_e_load : io_status_e_store;
    break;

  default:
    break;
  }
}
