/*
 * 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
 *
 */

Security_Ctrl::Security_Ctrl(sc_module_name name) :
  sc_module(name),
  p_clk("clk"),
  p_resetn("resetn")
{
  SC_METHOD(transition);
  dont_initialize();
  sensitive << p_clk.pos();

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

  current_sp = SP_ALLOC();
  key = KEY_ALLOC();

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

Security_Ctrl::~Security_Ctrl()
{
  SP_DEALLOC(current_sp);
  KEY_DEALLOC(key);
}

void Security_Ctrl::copySP()
{
  if (cmd.mb.read())
  {
    for (size_t i=0;i<SP_SIZE_IN_WORDS;i++)
      current_sp[i] = master_sp[i].read();

    if (inclusion(cmd.address.read(), 
		  getPSPEBaseAddress(p_mb_address.read(), p_size.read()),
		  getSPBaseAddress(p_mb_address.read(), p_size.read())))
    {
      current_sp[0] = 0x05;//0x05;
    }
    else
    {
      current_sp[0] = 0x15;// 0x15;
    }
  }
  else
  {
    for (size_t i=0;i<SP_SIZE_IN_WORDS;i++)
    {
      current_sp[i] = ctx_register.sp[i].read();
    }
  }

   sp_getKeyC(&current_sp, &key);
}

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

  if (!p_resetn.read())
  {
    fsm_read_s = R_IDLE;
    fsm_write_s = W_IDLE;

    m_cycle = 0;
    int_src_s = 0;

    return;
  }

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

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

  start_check_mac_s = false;
  start_up_mac_s = false;
  start_check_mactree_s = false;

  if (!secbus_enable.read())
  {
    for (uint32_t i=0;i<SP_SIZE_IN_WORDS;i++)
      master_sp[i] = master_ctx_init.sp[i].read();

    for (uint32_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
      master_pspe[i] = master_ctx_init.pspe[i].read();
  }
  else
  {
    if (master_update.enable.read())
    {
      for (uint32_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
      {
	master_pspe[i] = master_update.pspe[i].read();
      }
    }
  }

  /**
     FSM READ: handle READ command from input ctrl or securityctx_ctrl
  */
  switch (fsm_read_s.read())
  {
  case R_IDLE:
    if (!mt_ack.cack)
      break;

    if (cmd.cmd.read() == READ && cmd.cmdval.read())
    {
      fsm_write_s = W_SLEEP;
      mb_s = cmd.mb.read();
      bool cipher, integ;
      // read SP from ctx_register or master_sp depending on the
      // access type
      copySP();

      address_s = aligned(cmd.address.read(), SIZE_256_IN_BYTES);

      switch(sp_getModeC(&current_sp))
      {
      case sp_mode_c_none:
	is_cipher_s = cipher = false;
	break;

      case sp_mode_c_ctr:
	is_cipher_s = cipher = true;
	cmd_crypto_s = crypto_ctr;
	break;

      case sp_mode_c_cbc:
	is_cipher_s = cipher = true;
	#warning TODO : handle IV page
	cmd_crypto_s = crypto_cbc;
	break;

      default:
	is_cipher_s = cipher = false;
	DEBUG(ERROR, STD, "Unknown cryptographic ciphering mode\n");
	break;
      }

      switch(sp_getModeI(&current_sp))
      {
      case sp_mode_i_none:
	is_int_s = integ = false;
	break;

      case sp_mode_i_mac:
	is_int_s = integ = true;
        is_mac_s = true;
	break;

      case sp_mode_i_mactree:
	is_int_s = integ = true;
	is_mac_s = false;
	break;
	
      default:
	integ = false;
	is_mac_s = false;
	is_int_s = false;
	DEBUG(ERROR, STD, "Unknown cryptographic integrity mode\n");
	break;
      }

      // data are in the prefetch register
      if (r_register_ad.address.read() == 
	  aligned(cmd.address.read(), SIZE_256_IN_BYTES))
      {
	// if data are uncrypted, start decryption
	if (cipher)
	{
	  src_id_s  = 3;
	  dest_id_s = 0;
	  fsm_read_s = R_START_BLOCK_DEC;
	}
	// else rsp
	else
	{
	  fsm_read_s = R_UPDATE_REG_0;
	}
      }
      // if data are in the register 0
      else if (r_register_0.address.read() ==
	       aligned(cmd.address.read(), SIZE_256_IN_BYTES) && !cmd.mb.read())
      {
	fsm_read_s = R_IDLE;
	fsm_write_s = W_IDLE;
      }
      else
      {
	// if cipher block expected read in reg 1
	// and uncipher in reg 0
	if (cipher)
	{
	  int_src_s = 2;
	  src_id_s = 1;
	  dest_id_s = 0;
	}
	// if plain block expected read in reg 0
	else
	{
	  int_src_s = 2;
	  src_id_s = 0;
	  dest_id_s = 0;
	}
	fsm_read_s = R_START_READ;
      }
    }
    break;
    
  case R_START_READ:
    if (mem_rq.cmdack.read())
    {
      fsm_read_s = R_WAIT_READ;
    }
    break;

  case R_WAIT_READ:
    if (mem_rq.rspval.read())
    {
      if (is_cipher_s.read())
	fsm_read_s = R_START_BLOCK_DEC;
      else
      {
	if (is_int_s)
	{
	  if (is_mac_s)
	    start_check_mac_s = true;
	  else
	  {
//	    start_check_mactree_s = true;
	    if (mt_ack.uack || mt_ack.alap_ack)
	      fsm_read_s = R_START_CINT;
	    else
	      fsm_read_s = R_WAIT_FOR_CINT;
	    break;
	  }

//	  int_src_s = 2;
	}
	fsm_read_s = R_IDLE;
	fsm_write_s = W_IDLE;
      }
    }
    break;

  case R_WAIT_FOR_CINT:
    if (mt_ack.uack || mt_ack.alap_ack)
      fsm_read_s = R_START_CINT;
    break;

  case R_START_CINT:
    fsm_read_s = R_WAIT_CINT;
    break;
    
  case R_WAIT_CINT:
    if (mt_ack.cack)
    {
      fsm_read_s = R_IDLE;
      fsm_write_s = W_IDLE;
    }

  case R_UPDATE_REG_0:
    fsm_read_s = R_IDLE;
    fsm_write_s = W_IDLE;
    break;


  case R_START_BLOCK_DEC:
    if (crypto_cmd.cmdack.read())
    {
      fsm_read_s = R_WAIT_BLOCK_DEC;
    }
    break;

  case R_WAIT_BLOCK_DEC:
    if (crypto_cmd.rspval.read())
    {
      if (is_int_s)
      {
	if (is_mac_s)
	  start_check_mac_s = true;
	else
	{
	  fsm_read_s = R_START_CINT;
//	  start_check_mactree_s = true;
	  break;
	}

//	int_src_s = 2;
      }

      fsm_read_s = R_IDLE;
      fsm_write_s = W_IDLE;
    }
    break;

  default:
    break;
  }

  /**
     FSM WRITE: handle WRITE command from input ctrl or securityctx_ctrl

     For a write command occured after a normal memory access, datas
     are in the prefetch register, we use the byte enable to known
     which are the new data. The security policy is read from the
     context register.

     For a write command to the master block, datas are in the register 2,
     the security policy is read from the master_sp
  */
  switch (fsm_write_s.read())
  {
  case W_IDLE:
    if (!mt_ack.cack)
      break;

    if (cmd.cmd.read() == WRITE && cmd.cmdval.read())
    {
      fsm_read_s = R_SLEEP;
      mb_s = cmd.mb.read();

      bool cipher, integ, mac;

      // read SP from ctx_register or master_sp depending on the
      // access type
      copySP();

      address_s = cmd.address.read();
      crypto_size_s = 4;

      switch(sp_getModeC(&current_sp))
      {
      case sp_mode_c_none:
	is_cipher_s = cipher = false;
	break;

      case sp_mode_c_ctr:
	is_cipher_s = cipher = true;
	cmd_crypto_s = crypto_ctr;
	break;

      case sp_mode_c_cbc:
	is_cipher_s = cipher = true;
	#warning TODO : handle IV page
	cmd_crypto_s = crypto_cbc;
	break;

      default:
	is_cipher_s = cipher = false;
	DEBUG(ERROR, STD, "Unknown cryptographic ciphering mode\n");
	break;
      }

      switch(sp_getModeI(&current_sp))
      {
      case sp_mode_i_none:
	is_int_s = integ = false;
	is_mac_s = mac = false;
	break;

      case sp_mode_i_mac:
	is_int_s = integ = true;
        is_mac_s = mac = true;
	break;

      case sp_mode_i_mactree:
	is_int_s = integ = true;
	is_mac_s = mac = false;
	break;
	
      default:
	is_int_s = integ = false;
	is_mac_s = mac = false;
	DEBUG(ERROR, STD, "Unknown cryptographic integrity mode\n");
	break;
      }

      if (!cipher && !integ)
      {
	if (cmd.mb.read())
	{	
	  src_id_s = 2; // write reg 2
	  plen_s = 32 - _ctz(r_register_2.be.read(), 32) - _clz(r_register_2.be.read(), 32);
	}
	else
	{
	  src_id_s = 3; // write reg 4 (ad)
	  plen_s = 32 - _ctz(r_register_ad.be.read(), 32) - _clz(r_register_ad.be.read(), 32);
	}

	fsm_write_s = W_START_WRITE;
	break;
      }

      // encrypted data, write a full 256bits block
      plen_s = 32;
      address_s = aligned(cmd.address.read(), SIZE_256_IN_BYTES);

      if (!cipher && integ)
      {
	src_id_s = 0; // read in reg 0
	int_src_s = 0; // check reg 0

	if (mac)
	{
	  fsm_write_s = W_UPDATE_REG_0;
	}
	else
	{
	  fsm_write_s = W_START_READ;
	}
	break;
      }

      if (mac)
      {
	int_src_s = 1; // check integrity with reg 1
	src_id_s = 1; // encrypted data in reg 1
	dest_id_s = 0; // plain data in reg 0
	fsm_write_s = W_UPDATE_REG_0;
	break;
      }

      // already plain data in reg 0
      if (r_register_0.address.read() == aligned(cmd.address.read(), SIZE_256_IN_BYTES) && 
	  !cmd.mb.read() &&
	  r_register_0.be.read() == 0xFFFFFFFF)
      {
	int_src_s = 1; // update integrity with reg 1
	src_id_s = 1; // encrypted data in reg 1
	dest_id_s = 0; // plain data in reg 0
	fsm_write_s = W_UPDATE_REG_0;
      }
      else
      {
	// encrypted datas are in the register 1
	if (r_register_1.address.read() == aligned(cmd.address.read(), SIZE_256_IN_BYTES))
	{
	  int_src_s = 1; // update integrity with reg 1
	  src_id_s = 1; // encrypted data in reg 1
	  dest_id_s = 0; // plain data in reg 0
	  fsm_write_s = W_START_BLOCK_DEC;
	}
	else
	{
	  int_src_s = 1; // check integrity with reg 1
	  src_id_s = 1; // encrypted data in reg 1
	  dest_id_s = 0; // plain data in reg 0
	  fsm_write_s = W_START_READ;
	}
      }
    }
    break;

  case W_START_READ:
    if (mem_rq.cmdack.read())
    {
      fsm_write_s = W_WAIT_READ;
    }
    break;

  case W_WAIT_READ:
    if (mem_rq.rspval.read())
    {
      // check integrity from read data
      if (is_int_s)
      {
/*
	if (is_mac_s)
	  start_check_mac_s = true;
	else
	  start_check_mactree_s = true;
*/
	if (mt_ack.uack || mt_ack.alap_ack)
	  fsm_write_s = W_START_CINTEGRITY;
	else
	  fsm_write_s = W_WAIT_FOR_CINTEGRITY;
	break;
      }
      
      if (is_cipher_s.read())
      {
	fsm_write_s = W_START_BLOCK_DEC;
      }
      else
      {
	fsm_write_s = W_UPDATE_REG_0;
      }
    }
    break;

  case W_WAIT_FOR_CINTEGRITY:
    if (mt_ack.uack || mt_ack.alap_ack)
      fsm_write_s = W_START_CINTEGRITY;
    break;

  case W_START_CINTEGRITY:
    fsm_write_s = W_WAIT_CINTEGRITY;
    break;

  case W_WAIT_CINTEGRITY:
    if (mt_ack.cack)
    {
      if (is_cipher_s.read())
      {
	fsm_write_s = W_START_BLOCK_DEC;
      }
      else
      {
	fsm_write_s = W_UPDATE_REG_0;
      }
    }
    break;

  case W_START_BLOCK_DEC:
    if (crypto_cmd.cmdack.read())
    {
      fsm_write_s = W_WAIT_BLOCK_DEC;
    }
    break;

  case W_WAIT_BLOCK_DEC:
    if (crypto_cmd.rspval.read())
    {
      fsm_write_s = W_UPDATE_REG_0;
    }
    break;

  case W_UPDATE_REG_0:
    if (is_cipher_s.read())
    {
      fsm_write_s = W_START_BLOCK_ENC;
    }
    else
    {
      fsm_write_s = W_START_WRITE;
    }
    break;

  case W_START_BLOCK_ENC:
    if (crypto_cmd.cmdack.read())
    {
      fsm_write_s = W_WAIT_BLOCK_ENC;
    }
    break;

  case W_WAIT_BLOCK_ENC:
    if (crypto_cmd.rspval.read())
    {
      address_s = aligned(address_s.read(), SIZE_256_IN_BYTES);
      fsm_write_s = W_START_WRITE;
    }
    break;

  case W_START_WRITE:
    if (mem_rq.cmdack.read())
    {
      fsm_write_s = W_WAIT_WRITE;
    }
    break;

  case W_WAIT_WRITE:
    if (mem_rq.rspval.read())
    {
      // if integrity required, do update
      if (is_int_s)
      {
	if (is_mac_s)
	{
	  start_up_mac_s = true;
	  fsm_read_s = R_IDLE;
	  fsm_write_s = W_IDLE;
	}
	else
	  fsm_write_s = W_START_UINT;
      }
      else
      {
	fsm_read_s = R_IDLE;
	fsm_write_s = W_IDLE;
      }
    }
    break;

  case W_START_UINT:
    fsm_write_s = W_WAIT_UINT;
    break;

  case W_WAIT_UINT:
    if (mt_ack.alap_ack)
    {
      fsm_read_s = R_IDLE;
      fsm_write_s = W_IDLE;
    }
    break;

  default:
    break;
  }
}

void Security_Ctrl::genMealy()
{
  cmd.cmdack          = false;
  cmd.rspval          = false;
  crypto_cmd.cmdval   = false;
  crypto_cmd.enable   = false;
  crypto_cmd.direct_data = false;
  crypto_cmd.no_init  = false;
  mem_rq.cmdval       = false;
  mem_rq.direct_write = false;

  mt_cmd.enable = false;
  ms_cmd.enable = false;

  ack = false;

  w_register_0.enable = false;
  w_register_0.flush = false;

  for (size_t i=0;i<PSPE_SIZE_IN_WORDS;i++)
    master_ctx.pspe[i] = master_pspe[i];

  for (size_t i=0;i<SP_SIZE_IN_WORDS;i++)
    master_ctx.sp[i] = master_sp[i];

  if (start_check_mac_s)
  {
    ms_cmd.src = int_src_s;
    ms_cmd.enable = true;
    ms_cmd.cmd = CHECK;
    ms_cmd.address = aligned(address_s.read(), SIZE_256_IN_BYTES);
  }

  if (start_up_mac_s)
  {
    ms_cmd.src = int_src_s;
    ms_cmd.enable = true;
    ms_cmd.cmd = UPDATE;
    ms_cmd.address = aligned(address_s.read(), SIZE_256_IN_BYTES);
  }

  /**
     output generation for read state
  */
  switch (fsm_read_s.read())
  {
  case R_IDLE:
    if (mt_ack.cack)
    {
      cmd.cmdack = true;
      cmd.rspval = true;
      ack = true;
    }
    break;
    
  case R_START_READ:
    mem_rq.cmdval = true;
    mem_rq.cmd = READ;
    mem_rq.start_address = address_s.read();
    mem_rq.plen = 32;
    mem_rq.id = 1<<src_id_s.read() | 4;
    break;

  case R_START_CINT:
    mt_cmd.enable = true;
    mt_cmd.cmd = CHECK;
    mt_cmd.address = aligned(address_s.read(), SIZE_256_IN_BYTES);
    mt_cmd.src = int_src_s;
    break;

  case R_WAIT_READ:
    break;

  case R_UPDATE_REG_0:
    w_register_0.enable = true;
    w_register_0.address = address_s.read();
    if (mb_s.read())
      w_register_0.be = r_register_2.be.read();
    else
      w_register_0.be = r_register_ad.be.read();

    for (size_t i=0;i<SIZE_256_IN_WORDS;i++)
    {
      if (mb_s.read())
      {
        if ((r_register_2.be.read()>>(i*4))&0xF)
	{
	  uint32_t tmp = r_register_2.be.read()>>(i*4);
	  uint32_t mask = 0;
	  for (size_t j=0;j<4;j++)
	  {
	    if (tmp&(1<<j))
	      mask += 0xFF<<(j*8);
	  }
	  w_register_0.data[i] = (r_register_0.data[i] & (~mask)) | (r_register_2.data[i]);
	}
      }
      else     
      {
        if ((r_register_ad.be.read()>>(i*4))&0xF)
	{
	  uint32_t tmp = r_register_ad.be.read()>>(i*4);
	  uint32_t mask = 0;
	  for (size_t j=0;j<4;j++)
	  {
	    if (tmp&(1<<j))
	      mask += 0xFF<<(j*8);
	  }
	  w_register_0.data[i] = (r_register_0.data[i] & (~mask)) | (r_register_ad.data[i]);
	}
      }
    }
    break;

  case R_START_BLOCK_DEC:
    crypto_cmd.cmdval = true;
    crypto_cmd.enable = true;
    crypto_cmd.mode = cmd_crypto_s;
    for (uint32_t i=0;i<DES_KEY_SIZE_IN_WORDS;i++)
      crypto_cmd.key[i] = key[i];
    crypto_cmd.size = 4;
    crypto_cmd.cipher = false;
    crypto_cmd.id_src = src_id_s.read();
    crypto_cmd.id_dest = dest_id_s.read();
    crypto_cmd.reg_ready = true;
    crypto_cmd.address = address_s.read();
    break;

  case R_WAIT_BLOCK_DEC:
    break;

  default:
    break;
  }

  /**
     output generation for write state
  */
  switch (fsm_write_s.read())
  {
  case W_IDLE:
    if (mt_ack.cack)
    {
      cmd.cmdack = true;
      cmd.rspval = true;
      ack = true;
    }
    break;

  case W_START_READ:
    mem_rq.cmdval = true;
    mem_rq.cmd = READ;
    mem_rq.start_address = aligned(address_s.read(), SIZE_256_IN_BYTES);
    mem_rq.plen = 32;
    mem_rq.id = 1<<src_id_s.read() | 1<<(int_src_s.read());
    break;

  case W_WAIT_READ:
    break;

  case W_START_CINTEGRITY:
    mt_cmd.enable = true;
    mt_cmd.cmd = CHECK;
    mt_cmd.address = aligned(address_s.read(), SIZE_256_IN_BYTES);
    mt_cmd.src = int_src_s;
    break;

  case W_START_BLOCK_DEC:
    crypto_cmd.cmdval = true;
    crypto_cmd.enable = true;
    crypto_cmd.mode = cmd_crypto_s.read();
    crypto_cmd.size = crypto_size_s.read();
    for (uint32_t i=0;i<DES_KEY_SIZE_IN_WORDS;i++)
      crypto_cmd.key[i] = key[i];
    crypto_cmd.cipher = false;
    crypto_cmd.id_src = src_id_s.read();
    crypto_cmd.id_dest = dest_id_s.read();
    crypto_cmd.reg_ready = true;
    crypto_cmd.address = aligned(address_s.read(), SIZE_256_IN_BYTES);
    break;

  case W_WAIT_BLOCK_DEC:
    break;

  case W_UPDATE_REG_0:
    w_register_0.enable = true;
    w_register_0.address = address_s.read();

    if (mb_s.read())
      w_register_0.be = r_register_2.be.read();
    else
      w_register_0.be = r_register_ad.be.read();

    for (size_t i=0;i<SIZE_256_IN_WORDS;i++)
    {
      if (mb_s.read())
      {
        if ((r_register_2.be.read()>>(i*4))&0xF)
	{
	  uint32_t tmp = r_register_2.be.read()>>(i*4);
	  uint32_t mask = 0;
	  for (size_t j=0;j<4;j++)
	  {
	    if (tmp&(1<<j))
	      mask += 0xFF<<(j*8);
	  }
	  w_register_0.data[i] = (r_register_0.data[i] & (~mask)) | (r_register_2.data[i]);
	}
      }
      else     
      {
        if ((r_register_ad.be.read()>>(i*4))&0xF)
	{
	  uint32_t tmp = r_register_ad.be.read()>>(i*4);
	  uint32_t mask = 0;
	  for (size_t j=0;j<4;j++)
	  {
	    if (tmp&(1<<j))
	      mask += 0xFF<<(j*8);
	  }
	  w_register_0.data[i] = (r_register_0.data[i] & (~mask)) | (r_register_ad.data[i]);
	}
      }
    }
    break;

  case W_START_BLOCK_ENC:
    crypto_cmd.cmdval = true;
    crypto_cmd.mode = cmd_crypto_s.read();
    crypto_cmd.enable = true;
    crypto_cmd.size = crypto_size_s.read();
    crypto_cmd.cipher = true;
    for (uint32_t i=0;i<DES_KEY_SIZE_IN_WORDS;i++)
      crypto_cmd.key[i] = key[i];
    crypto_cmd.id_src = dest_id_s.read();
    crypto_cmd.id_dest = src_id_s.read();
    crypto_cmd.reg_ready = true;
    crypto_cmd.address = aligned(address_s.read(), SIZE_256_IN_BYTES);
    break;

  case W_WAIT_BLOCK_ENC:
    break;

  case W_START_WRITE:
    mem_rq.cmdval = true;
    mem_rq.cmd = WRITE;
    mem_rq.start_address = address_s.read();
    mem_rq.plen = plen_s.read();
    mem_rq.id = 1<<src_id_s.read();
    break;

  case W_WAIT_WRITE:
    break;

  case W_START_UINT:
    mt_cmd.enable = true;
    mt_cmd.cmd = UPDATE;
    mt_cmd.address = aligned(address_s.read(), SIZE_256_IN_BYTES);
    mt_cmd.src = int_src_s;
    break;

  case W_WAIT_UINT:
    break;

  default:
    break;
  }
}
