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

SecurityController::SecurityController(sc_module_name name, ofstream * file): sc_module(name)
{
  _secbus_debug(cout, TRACE, "BUILDING SECURITY CONTROLLER ..."<<"\n");

  SC_METHOD(ps);
  sensitive << clk.pos();
  SC_METHOD(pc);
  sensitive << clk.neg()<< register_0_in.data[0]
    << register_0_in.data[1]<< register_0_in.data[2]
    << register_0_in.data[3]<< register_0_in.data[4]
    << register_0_in.data[5]<< register_0_in.data[6]
    << register_0_in.data[7];

  output = file;

  plaintext_s = new int [SP_SIZE];
  ciphertext_s = new int [SIZE_64_IN_WORDS];
  mask_s = new int [SIZE_64_IN_WORDS];

  _secbus_debug(cout, TRACE, "SECURITY CONTROLLER BUILT"<<"\n");
}

void SecurityController::pc()
{
  in.rack			= false;
  mb_out_ack.rack		= false;
  in.wack			= false;
  mb_out_ack.wack		= false;
  out_cmd.enable		= false;
  register_0_out.flush		= false;
  register_0_out.enable		= false;
  register_1_out.enable		= false;
  register_1_out.flush		= false;
  mac_cmd_out.enable		= false;
  hash_cmd_out.enable		= false;
  cb_out_cmd.enable		= false;
  cb_out_cmd.enable		= false;


  switch (sc_read_state)
  {
    case R_IDLE :
      in.rack = true;
      break;

    case R_START_MASK_GENERATION:
      //Start mask generation
      cb_out_cmd.enable		= true;
      cb_out_cmd.cmode		= OTP;
      cb_out_cmd.address	= block_address_s;
      cb_out_cmd.key		= sc_register.sp;
      //write input register
      if (write_register_0_s)
      {
        register_0_out.enable	= true;
        register_0_out.address	= block_address_s - SIZE_64_IN_BYTES;
        register_0_out.be	= 0xFF<<(index_s - SIZE_64_IN_WORDS)*4;
        register_0_out.data[index_s - SIZE_64_IN_WORDS]		= plaintext_s[0];
        register_0_out.data[index_s - SIZE_64_IN_WORDS + 1]	= plaintext_s[1];
      }
      break;


    case R_START_BLOCK_DEC:
      //write input register
      if (write_register_0_s)
      {
        register_0_out.enable	= true;
        register_0_out.address	= block_address_s - SIZE_64_IN_BYTES;
        register_0_out.be	= 0xFF<<(index_s - SIZE_64_IN_WORDS)*4;
        register_0_out.data[index_s - SIZE_64_IN_WORDS]		= plaintext_s[0];
        register_0_out.data[index_s - SIZE_64_IN_WORDS + 1]	= plaintext_s[1];
      }
      //start deciphering
      cb_out_cmd.enable		= true;
      cb_out_cmd.cmode		= BC;
      cb_out_cmd.key		= sc_register.sp;
      cb_out_data.data[0]	= register_1_in.data[index_s];
      cb_out_data.data[1]	= register_1_in.data[index_s+1];
      break;

    case R_WRITE_64_TO_REGISTER_0:
      //Write 64 bits to R0
      register_0_out.enable	= true;
      register_0_out.address	= block_address_s;
      register_0_out.be		= 0xFF<<index_s*4;
      register_0_out.data[index_s]	= plaintext_s[0];
      register_0_out.data[index_s + 1]	= plaintext_s[1];
      break;


    case R_WRITE_256_TO_REGISTER_0:
      //write 256 bits to R0
      register_0_out.enable	= true;
      register_0_out.address	= block_address_s;
      register_0_out.be		= 0xFFFFFFFF;
      for(unsigned int i=0;i<SIZE_256_IN_WORDS;i++)
        register_0_out.data[i]	= register_1_in.data[i];
      break;


    case R_START_READ_256:
      //To memory
      out_cmd.id		= memory_cmd_id_s;
      out_cmd.cmd		= READ;
      out_cmd.enable		= true;
      out_cmd.start_address	= set_address_s;
      out_cmd.plen		= SIZE_256_IN_BYTES;
      if (getPM(sc_register.sp) == OTP)
      {
        //Start mask generation
        cb_out_cmd.enable	= true;
        cb_out_cmd.cmode	= OTP;
        cb_out_cmd.address	= block_address_s;
        cb_out_cmd.key		= sc_register.sp;
      }
      break;

    case R_START_READ_IMMEDIATE:
      out_cmd.cmd = IMMEDIATE;
      out_cmd.enable = true;
      break;

    default :
      break;
  }

  switch (sc_write_state)
  {

    case W_IDLE:
      in.wack = true;
      break;

    case W_START_READ_256:
      //To memory
      out_cmd.id		= memory_cmd_id_s;
      out_cmd.cmd		= READ;
      out_cmd.enable		= true;
      out_cmd.start_address	= set_address_s;
      out_cmd.plen		= SIZE_256_IN_BYTES;
      break;

    case W_START_READ_64:
      //To memory
      out_cmd.id		= memory_cmd_id_s;
      out_cmd.cmd		= READ;
      out_cmd.enable		= true;
      out_cmd.start_address	= block_address_s;
      out_cmd.plen		= SIZE_64_IN_BYTES;
      break;

    case W_START_MASK_GENERATION:
      //Start mask generation
      cb_out_cmd.enable		= true;
      cb_out_cmd.address	= block_address_s;
      cb_out_cmd.key		= sc_register.sp;
      cb_out_cmd.cmode		= OTP;
      break;

    case W_START_BLOCK_DEC:
      //start deciphering
      cb_out_cmd.enable		= true;
      cb_out_data.data[0]	= register_1_in.data[index_s];
      cb_out_data.data[1]	= register_1_in.data[index_s+1];
      cb_out_cmd.key		= sc_register.sp;
      cb_out_cmd.cmode		= BC;
      break;

    case W_START_BLOCK_ENC:
      //start ciphering
      cb_out_cmd.enable		= true;
      cb_out_data.data[0]	= register_0_in.data[index_s];
      cb_out_data.data[1]	= register_0_in.data[index_s+1];
      cb_out_cmd.key		= sc_register.sp;
      cb_out_cmd.cmode		= BC;
      break;

    case W_WRITE_64_TO_REGISTER_0:
      //Write 64 bits to R0
      register_0_out.enable	= true;
      register_0_out.address	= block_address_s;
      register_0_out.be		= be;
      register_0_out.data[index_s]	= plaintext_s[0];
      register_0_out.data[index_s + 1]	= plaintext_s[1];
      break;

    case W_WRITE_64_TO_REGISTER_1:
      //Write 64 bits to R0
      register_1_out.enable	= true;
      register_1_out.address	= block_address_s;
      register_1_out.be		= register_0_in.be & (0xFF<<index_s*4);
      register_1_out.data[index_s]	= ciphertext_s[0];
      register_1_out.data[index_s + 1]	= ciphertext_s[1];
      break;

    case W_WRITE_256_TO_REGISTER_1:
      //Write 256 bits to R1 according to BE signal
      register_1_out.enable	= true;
      register_1_out.address	= block_address_s;
      register_1_out.be		= register_0_in.be;
      for(unsigned int i=0;i<SIZE_256_IN_WORDS;i++)
        register_1_out.data[i]	= register_0_in.data[i];
      break;

    case W_START_WRITE_TO_MEMORY:
      //To memory
      out_cmd.id		= memory_cmd_id_s;
      out_cmd.cmd		= WRITE;
      out_cmd.enable		= true;
      out_cmd.start_address	= start_address_s;
      out_cmd.plen		= plen_s;
      break;

    case W_START_IU:
      //Start integrity updating
      if (getIM(sc_register.sp) == HASH_TREE)
      {
        hash_cmd_out.enable	= true;
        hash_cmd_out.address	= set_address_s;
        hash_cmd_out.cmd	= UPDATE;
      }
      else
      {
        mac_cmd_out.enable	= true;
        mac_cmd_out.address	= set_address_s;
        mac_cmd_out.cmd		= UPDATE;
      }
      break;

    default :
      break;

  }

  switch (mbsc_read_state)
  {
    case MBR_IDLE:
      mb_out_ack.rack 		= true;
      mb_out_rdata.data[0] 	= plaintext_s[0];
      mb_out_rdata.data[1] 	= plaintext_s[1];
      mb_out_rdata.data[2] 	= plaintext_s[2];
      mb_out_rdata.data[3] 	= plaintext_s[3];
      break;

    case MBR_START_READ_256 :
      //Read 256 bits from memory
      out_cmd.id		= memory_cmd_id_s;
      out_cmd.cmd		= READ;
      out_cmd.enable		= true;
      out_cmd.start_address	= set_address_s;
      out_cmd.plen		= SIZE_256_IN_BYTES;
      break;

    case MBR_START_BLOCK_DEC ://5
      //start deciphering
      cb_out_cmd.enable		= true;
      cb_out_data.data[0]	= register_1_in.data[index_s];
      cb_out_data.data[1]	= register_1_in.data[index_s + 1];
      cb_out_cmd.key		= MASTER_KEY;
      cb_out_cmd.cmode		= BC;

      break;

    default:
      break;
  }

  switch (mbsc_write_state)
  {
    case MBW_IDLE :
      mb_out_ack.wack = true;
      break;

    case MBW_START_BLOCK_ENC:
      //start ciphering
      cb_out_cmd.enable		= true;
      cb_out_data.data[0]	= plaintext_s[index_s % SP_SIZE];
      cb_out_data.data[1]	= plaintext_s[1 + index_s % SP_SIZE];
      cb_out_cmd.key		= MASTER_KEY;
      cb_out_cmd.cmode		= BC;
      break;

    case MBW_START_READ_256:
      //Read 256 bits from memory
      out_cmd.id		= memory_cmd_id_s;
      out_cmd.cmd		= READ;
      out_cmd.enable		= true;
      out_cmd.start_address	= set_address_s;
      out_cmd.plen		= SIZE_256_IN_BYTES;
      break;

    case MBW_WRITE_64_TO_REGISTER_1:
      //Write 64 bits to R1
      register_1_out.enable	= true;
      register_1_out.address	= block_address_s;
      register_1_out.be		= 0xFF<<index_s*4;
      if (pspe_s) {
        register_1_out.data[index_s]		= plaintext_s[0];
        register_1_out.data[index_s + 1]	= plaintext_s[1];}
      else {
        register_1_out.data[index_s]		= ciphertext_s[0];
        register_1_out.data[index_s + 1]	= ciphertext_s[1];}
      break;

    case MBW_START_WRITE_TO_MEMORY:
      //write memory
      out_cmd.id		= memory_cmd_id_s;
      out_cmd.cmd		= WRITE;
      out_cmd.enable		= true;
      out_cmd.start_address	= start_address_s;
      out_cmd.plen		= plen_s;
      break;

    case MBW_START_HT_IU:
      //Start updating HT
      hash_cmd_out.enable	= true;
      hash_cmd_out.address	= set_address_s;
      hash_cmd_out.cmd		= UPDATE;
      break;

    default:
      break;
  }

  if (start_ht_update)
  {
    hash_cmd_out.enable 	= true;
    hash_cmd_out.address 	= set_address_s;
    hash_cmd_out.cmd 		= INITIALIZE; 
  }

  //start ht ic
  if (start_ht_ic_s)
  {
    hash_cmd_out.enable		= true;
    hash_cmd_out.address	= set_address_s;
    hash_cmd_out.cmd		= CHECK;
  }
  //start mac ic
  if (start_mac_ic_s)
  {
    mac_cmd_out.enable		= true;
    mac_cmd_out.address		= set_address_s;
    mac_cmd_out.cmd		= CHECK;
  }

  if (flush_register_1_s)
  {
    register_1_out.enable	= true;
    register_1_out.flush	= true;
  }
  if (flush_register_0_s)
  {
    register_0_out.enable	= true;
    register_0_out.flush	= true;
  }
  if (write_register_1_s)
  {
    register_1_out.enable	= true;
    register_1_out.address	= block_address_s - SIZE_64_IN_BYTES;
    register_1_out.be		= be;
    register_1_out.data[index_s - SIZE_64_IN_WORDS]	= ciphertext_s[0];
    register_1_out.data[index_s - SIZE_64_IN_WORDS + 1]	= ciphertext_s[1];
  }

}

void SecurityController::ps()
{	
  _secbus_debug(*output, TRACE, dec<<"SECURITY CONTROLLER READ STATE: "<<sc_read_state<<"\n");
  _secbus_debug(*output, TRACE, dec<<"SECURITY CONTROLLER WRITE STATE: "<<sc_write_state<<"\n");
  _secbus_debug(*output, TRACE, dec<<"SECURITY CONTROLLER MB READ STATE: "<<mbsc_read_state<<"\n");
  _secbus_debug(*output, TRACE, dec<<"SECURITY CONTROLLER MB WRITE STATE: "<<mbsc_write_state<<"\n");


  if (!reset) 
  {
    address_in_s	= 0xFFFFFFFF;
    plen_s		= 0xFFFFFFFF;
    block_address_s 	= 0xFFFFFFFF;
    set_address_s	= 0xFFFFFFFF;
    start_address_s 	= 0xFFFFFFFF;
    be			= 0;
    memset(plaintext_s, 0, SIZE_64_IN_BYTES);
    memset(mask_s, 0, SIZE_64_IN_BYTES);
    sc_read_state	= R_IDLE;
    sc_write_state	= W_IDLE;
    mbsc_read_state	= MBR_IDLE;
    mbsc_write_state	= MBW_IDLE;
    return;
  }

  write_register_0_s = false;
  write_register_1_s = false;

  start_ht_ic_s = false;
  start_mac_ic_s = false;

  start_ht_update = false;

  flush_register_1_s = false;
  flush_register_0_s = false;

  integrity_idle_v	= hash_ack_in.cack && hash_ack_in.uack && mac_ack_in.cack && mac_ack_in.uack && hash_ack_in.alap_ack;
  mb_integrity_idle_v	= hash_ack_in.cack && hash_ack_in.alap_ack && mac_ack_in.cack && mac_ack_in.uack;

  switch (sc_read_state)
  {
    case R_IDLE ://0
      if(in.enable && in.cmd == READ)
      {
        address_in_s    = in.address;
        //get address of 64-bit block
        block_address_s = aligned(in.address, SIZE_256_IN_BYTES);
        //get address of 256-bit block
        set_address_s   = aligned(in.address, SIZE_256_IN_BYTES);
        index_s         = 0;


        if (getPM(sc_register.sp) == NO_CONFIDENTIALITY && getIM(sc_register.sp) == NO_INTEGRITY)
        {
          memory_cmd_id_s	= 0;

          if (integrity_idle_v)
            //integrity blocks are idle
	    sc_read_state = R_START_READ_256;
	  else
	    sc_read_state = R_WAIT_FOR_READ_256;
/*            sc_read_state = R_START_READ_IMMEDIATE;
          else
	  sc_read_state = R_WAIT_FOR_READ_IMMEDIATE;*/
        }
        else
        {

          if (register_1_in.address == aligned(in.address, SIZE_256_IN_BYTES) && register_1_in.be == 0xFFFFFFFF)
            //data is in out buffer
          {
            switch (getPM(sc_register.sp))
            {
              case OTP :
                //data is in out buffer
                sc_read_state = R_START_MASK_GENERATION;
                break;

              case BC :
                //data is in out buffer
                sc_read_state = R_START_BLOCK_DEC;
                break;

              case NO_CONFIDENTIALITY :
                sc_read_state = R_WRITE_256_TO_REGISTER_0;
                break;

              default :
                cout<<"CONFIDENTIALITY MODE NOT AVAILABLE";
                break;
            }
          }
          else
          {
            flush_register_1_s = true;
            if (getIM(sc_register.sp) == NO_INTEGRITY)
              memory_cmd_id_s	= 1;
            else 
              memory_cmd_id_s = 6;

            if (integrity_idle_v)
              //integrity blocks are idle
              sc_read_state = R_START_READ_256;
            else
              sc_read_state = R_WAIT_FOR_READ_256;
          }
        }
      }
      break;

    case R_START_READ_IMMEDIATE:
      sc_read_state = R_WAIT_READ_IMMEDIATE;
      break;

    case R_WAIT_FOR_READ_IMMEDIATE:
      if (integrity_idle_v)
        sc_read_state = R_START_READ_IMMEDIATE;
      break;

    case R_WAIT_READ_IMMEDIATE:
      if (in_ack.rack)
        sc_read_state = R_IDLE;
      break;

    case R_WAIT_FOR_READ_256:
      if (integrity_idle_v)
        //integrity blocks are idle
        sc_read_state = R_START_READ_256;
      break;

    case R_START_READ_256:
//cout << "start_read : " <<dec<< m <<endl;
      switch (getIM(sc_register.sp))
      {
        case MAC :
          start_mac_ic_s = true;
          break;

        case HASH_TREE :
          start_ht_ic_s = true;
          break;

        case NO_INTEGRITY :
          break;

        default :
          cout<<"INTEGRITY MODE NOT AVAILABLE";
          break;
      }
      switch (getPM(sc_register.sp))
      {
        case OTP :
          sc_read_state = R_WAIT_MASK_GENERATION;
          break;

        case BC :
          sc_read_state = R_WAIT_FOR_BLOCK_DEC;
          break;

        case NO_CONFIDENTIALITY :
          sc_read_state = R_WAIT_READ_256;
          break;

        default :
          cout<<"CONFIDENTIALITY MODE NOT AVAILABLE";
          break;
      }
      break;

    case R_WAIT_READ_256:
//cout << "----- wait_read : " <<dec<< m <<endl;
      if(in_ack.rack)
      {
        if (getIM(sc_register.sp) == NO_INTEGRITY){

//cout << "-------------- NI : " << dec<<m<< endl;
          sc_read_state = R_IDLE;
        }else{                  

//cout << "-------------- reg0 : " << dec<<m<< endl; 
          sc_read_state = R_WRITE_256_TO_REGISTER_0;}

      }
      break;


    case R_START_MASK_GENERATION:
      sc_read_state = R_WAIT_MASK_GENERATION;
      break;

    case R_WAIT_MASK_GENERATION :
      if (cb_in_ack)
      {
        if (fill_64(register_1_in.be, block_address_s))
        {
          //xor2 operation
          plaintext_s[0] = register_1_in.data[index_s]	^ cb_in_data.data[0];
          plaintext_s[1] = register_1_in.data[index_s+1]	^ cb_in_data.data[1];

          if((block_address_s + SIZE_64_IN_BYTES) % SIZE_256_IN_BYTES == 0)
            sc_read_state = R_WRITE_64_TO_REGISTER_0;
          else
          {
            write_register_0_s  = true;
            block_address_s     = block_address_s + SIZE_64_IN_BYTES;
            index_s             = index_s + SIZE_64_IN_WORDS;
            sc_read_state       = R_START_MASK_GENERATION;
          }	
        }
      }
      break;

    case R_WAIT_FOR_BLOCK_DEC:
      if (fill_64(register_1_in.be, block_address_s))
        sc_read_state = R_START_BLOCK_DEC;
      break;


    case R_START_BLOCK_DEC:
      sc_read_state = R_WAIT_BLOCK_DEC;
      break;

    case R_WAIT_BLOCK_DEC:
      if (cb_in_ack)
      {
        plaintext_s[0] = cb_in_data.data[0];
        plaintext_s[1] = cb_in_data.data[1];

        if((block_address_s + SIZE_64_IN_BYTES) % SIZE_256_IN_BYTES == 0)
          sc_read_state = R_WRITE_64_TO_REGISTER_0;
        else
        {
          write_register_0_s		= true;
          index_s					= index_s + SIZE_64_IN_WORDS;
          block_address_s			= block_address_s + SIZE_64_IN_BYTES;
          if (fill_64(register_1_in.be, block_address_s + SIZE_64_IN_BYTES))
            sc_read_state		= R_START_BLOCK_DEC;
          else
            sc_read_state		= R_WAIT_FOR_BLOCK_DEC;
        }
      }
      break;

    case R_WRITE_64_TO_REGISTER_0:
      sc_read_state = R_IDLE;
      break;

    case R_WRITE_256_TO_REGISTER_0:
      sc_read_state = R_IDLE;
      break;

    default :
      break;
  }
  switch (sc_write_state)
  {
    case W_IDLE :

      if(in.enable && in.cmd == WRITE)
      {
        address_in_s	= in.address;
        plen_s		= in.end_address - in.address + 4;
        //index of 64-bit block in 256-bit block 
        index_s			= index(aligned(in.address, SIZE_64_IN_BYTES), SIZE_256_IN_BYTES);
        //get address of 64-bit block
        block_address_s	= aligned(in.address, SIZE_64_IN_BYTES);
        //get address of 256-bit block
        set_address_s	= aligned(in.address, SIZE_256_IN_BYTES);



        if (getIM(sc_register.sp) == NO_INTEGRITY)
        {
          switch (getPM(sc_register.sp))
          {
            case OTP :
              start_address_s	= in.address;
              end_address_s	= in.end_address + 4;
              memory_cmd_id_s	= 7;

              if (register_1_in.address != aligned(in.address, SIZE_256_IN_BYTES))
                flush_register_1_s = true;

              sc_write_state = W_START_MASK_GENERATION;

              break;

            case BC :

              start_address_s	= aligned(in.address, SIZE_64_IN_BYTES);
              end_address_s	= aligned(in.end_address, SIZE_64_IN_BYTES) + SIZE_64_IN_BYTES;
              memory_cmd_id_s	= 1;

              if (register_1_in.address == aligned(in.address, SIZE_256_IN_BYTES))
              { 
                if (fill_64(register_0_in.be, in.address))
                  //no RMW
                  sc_write_state = W_START_BLOCK_ENC;
                else if (fill_64(register_1_in.be, in.address))
                  //relevant block is in R1
                  sc_write_state = W_START_BLOCK_DEC;
                else
                  //relevant block is not in register
                {
                  if (integrity_idle_v)
                    //integrity blocks are idle
                    sc_write_state = W_START_READ_64;
                  else
                    sc_write_state = W_WAIT_FOR_READ_64;
                }
              }
              else
              {
                flush_register_1_s = true;

                if (fill_64(register_0_in.be, in.address))
                  // no need to flush before writing
                  sc_write_state = W_START_BLOCK_ENC;
                else
                  //read relevant block from memory
                {
                  if (integrity_idle_v)
                    //integrity blocks are idle
                    sc_write_state = W_START_READ_64;
                  else
                    sc_write_state = W_WAIT_FOR_READ_64;
                }
              }
              break;

            case NO_CONFIDENTIALITY :

              start_address_s		= in.address;
              end_address_s	  	= in.end_address;
              memory_cmd_id_s		= 0;

              if (integrity_idle_v)
                //integrity blocks are idle
                sc_write_state = W_START_WRITE_TO_MEMORY;
              else
                sc_write_state = W_WAIT_FOR_WRITE_TO_MEMORY;	
              break;

            default :
              cout<<"CONFIDENTIALITY MODE NOT AVAILABLE";
              break;
          }
        }
        else
          //Integrity 
        {

          memory_cmd_id_s = 6;    
          switch (getPM(sc_register.sp))
          {
            case OTP :
              start_address_s	= in.address;
              end_address_s	= in.end_address + 4;

              if ((register_0_in.be == 0xFFFFFFFF) && (in.end_address - in.address + 4 == SIZE_256_IN_BYTES))
                sc_write_state = W_START_MASK_GENERATION;
              else
              {
                if (register_1_in.address == aligned(in.address, SIZE_256_IN_BYTES))
                  sc_write_state = W_START_MASK_GENERATION;
                else
                {
                  flush_register_1_s  = true;

                  if (integrity_idle_v)
                    //integrity blocks are idle
                    sc_write_state = W_START_READ_256;
                  else
                    sc_write_state = W_WAIT_FOR_READ_256;
                }
              }
              break;

            case BC :
              start_address_s	= aligned(in.address, SIZE_64_IN_BYTES);
              end_address_s	= aligned(in.end_address, SIZE_64_IN_BYTES) + SIZE_64_IN_BYTES;

              if ((register_0_in.be == 0xFFFFFFFF) && (in.end_address - in.address + 4 == SIZE_256_IN_BYTES))
                //Write of 256 bits
                sc_write_state = W_START_BLOCK_ENC;
              else
              {
                if (register_1_in.address == aligned(in.address, SIZE_256_IN_BYTES))
                {
                  if (fill_64(register_0_in.be, in.address))
                    sc_write_state = W_START_BLOCK_ENC;
                  else
                    sc_write_state = W_START_BLOCK_DEC;
                }
                else
                {
                  flush_register_1_s  = true;

                  if (integrity_idle_v)
                    //integrity blocks are idle
                    sc_write_state = W_START_READ_256;
                  else
                    sc_write_state = W_WAIT_FOR_READ_256;
                }
              }
              break;

            case NO_CONFIDENTIALITY :

              start_address_s		= in.address;
              end_address_s		= in.end_address;

              if ((register_0_in.be == 0xFFFFFFFF) && (in.end_address - in.address + 4 == SIZE_256_IN_BYTES))
                //Write of 256 bits
                sc_write_state = W_WRITE_256_TO_REGISTER_1;
              else
              {
                if (register_1_in.address == aligned(in.address, SIZE_256_IN_BYTES))
                  sc_write_state = W_WRITE_256_TO_REGISTER_1;
                else
                {
                  flush_register_1_s  = true;

                  if (integrity_idle_v)
                    //integrity blocks are idle
                    sc_write_state = W_START_READ_256;
                  else
                    sc_write_state = W_WAIT_FOR_READ_256;
                }
              }
              break;

            default :
              cout<<"CONFIDENTIALITY MODE NOT AVAILABLE";
              break;
          }
        }
      }
      break;

    case W_WAIT_FOR_READ_64:
      if (integrity_idle_v)
        //integrity blocks are idle
        sc_write_state = W_START_READ_64;
      break;

    case W_START_READ_64:
      sc_write_state = W_WAIT_READ_64;
      break;

    case W_WAIT_READ_64:
      if(in_ack.rack)
        sc_write_state = W_START_BLOCK_DEC;
      break;


    case W_WAIT_FOR_READ_256:
      if (integrity_idle_v)
        //integrity blocks are idle
        sc_write_state = W_START_READ_256;
      break;

    case W_START_READ_256:
      sc_write_state = W_WAIT_READ_256;

      switch (getIM(sc_register.sp))
      {
        case MAC :
          start_mac_ic_s = true;
          break;

        case HASH_TREE :
          start_ht_ic_s = true;
          break;

        case NO_INTEGRITY :
          break;

        default :
          cout<<"INTEGRITY MODE NOT AVAILABLE";
          break;
      }
      break;

    case W_WAIT_READ_256:
      if(in_ack.rack)
      {

        switch (getPM(sc_register.sp))
        {
          case OTP :
            sc_write_state = W_START_MASK_GENERATION;
            break;

          case BC :
            if (fill_64(register_0_in.be, address_in_s))
              sc_write_state = W_START_BLOCK_ENC;
            else
              sc_write_state = W_START_BLOCK_DEC;
            break;

          case NO_CONFIDENTIALITY :
            sc_write_state = W_WRITE_256_TO_REGISTER_1;
            break;

          default :
            cout<<"CONFIDENTIALITY MODE NOT AVAILABLE";
            break;
        }
      }
      break;

    case W_START_MASK_GENERATION:
      sc_write_state = W_WAIT_MASK_GENERATION;
      break;

    case W_WAIT_MASK_GENERATION ://5
      if (cb_in_ack)
      {
        //xor2 operation
        ciphertext_s[0] = register_0_in.data[index_s]	^ cb_in_data.data[0];
        ciphertext_s[1] = register_0_in.data[index_s+1]	^ cb_in_data.data[1];

        if((block_address_s + SIZE_64_IN_BYTES) >= end_address_s)
          sc_write_state = W_WRITE_64_TO_REGISTER_1;
        else
        {	
          write_register_1_s	= true;
          block_address_s		= block_address_s + SIZE_64_IN_BYTES;
          be					      = register_0_in.be & (0xFF<<(index_s*4));
          index_s				    = index_s + SIZE_64_IN_WORDS;
          sc_write_state		= W_START_MASK_GENERATION;
        }	
      }
      break;


    case W_START_BLOCK_DEC:
      sc_write_state = W_WAIT_BLOCK_DEC;
      break;

    case W_WAIT_BLOCK_DEC:
      if (cb_in_ack)
      {
        plaintext_s[0] = cb_in_data.data[0];
        plaintext_s[1] = cb_in_data.data[1];
        //Compute BE signal before writing in R0
        setBe(block_address_s);
        sc_write_state = W_WRITE_64_TO_REGISTER_0;
      }
      break;

    case W_WRITE_64_TO_REGISTER_0:
      sc_write_state = W_START_BLOCK_ENC;
      break;

    case W_START_BLOCK_ENC:
      sc_write_state = W_WAIT_BLOCK_ENC;
      break;

    case W_WAIT_BLOCK_ENC:
      if (cb_in_ack)
      {
        ciphertext_s[0] = cb_in_data.data[0];
        ciphertext_s[1] = cb_in_data.data[1];

        plen_s = end_address_s - start_address_s;	

        if (block_address_s + SIZE_64_IN_BYTES == end_address_s)
          //whole packet processed
          sc_write_state = W_WRITE_64_TO_REGISTER_1;
        else
        {
          block_address_s		= block_address_s + SIZE_64_IN_BYTES;
          index_s				= index_s + SIZE_64_IN_WORDS;
          be					= 0xFF<<index_s*4;
          write_register_1_s	= true;

          if (getIM(sc_register.sp) == NO_INTEGRITY)
          {
            if (fill_64(register_0_in.be, block_address_s + SIZE_64_IN_BYTES))

              sc_write_state = W_START_BLOCK_ENC;

            else if (register_1_in.address == aligned(block_address_s + SIZE_64_IN_BYTES, SIZE_256_IN_BYTES) 
                && fill_64(register_1_in.be, block_address_s + SIZE_64_IN_BYTES))

              sc_write_state = W_START_BLOCK_DEC;

            else
            {
              if (integrity_idle_v)
                //integrity blocks are idle
                sc_write_state = W_START_READ_64;
              else
                sc_write_state = W_WAIT_FOR_READ_64;
            }
          }
          else
          {
            if (fill_64(register_0_in.be, block_address_s + SIZE_64_IN_BYTES))
              sc_write_state = W_START_BLOCK_ENC;
            else
              sc_write_state = W_START_BLOCK_DEC;
          }
        }
      }
      break;

    case W_WRITE_64_TO_REGISTER_1:
      if (integrity_idle_v)
        //ic completed
        sc_write_state = W_START_WRITE_TO_MEMORY;
      else
        sc_write_state = W_WAIT_FOR_WRITE_TO_MEMORY;
      break;

    case W_WRITE_256_TO_REGISTER_1:
      if (integrity_idle_v)
        //ic completed
        sc_write_state = W_START_WRITE_TO_MEMORY;
      else
        sc_write_state = W_WAIT_FOR_WRITE_TO_MEMORY;
      break;

    case W_START_WRITE_TO_MEMORY:
      sc_write_state = W_WAIT_WRITE_TO_MEMORY;
      break;

    case W_WAIT_FOR_WRITE_TO_MEMORY:
      if (integrity_idle_v)
        //ic completed
        sc_write_state = W_START_WRITE_TO_MEMORY;
      break;

    case W_WAIT_WRITE_TO_MEMORY:
      if (in_ack.wack)
      {
        if (getIM(sc_register.sp)== NO_INTEGRITY)
          sc_write_state = W_IDLE;
        else
          sc_write_state = W_START_IU;
      }
      break;

    case W_START_IU:
      sc_write_state = W_IDLE;
      break;

    case W_WAIT_IU:
      if(hash_ack_in.uack && mac_ack_in.uack)
        sc_write_state = W_IDLE;
      break;

    default :
      break;

  }

  /////////////////////// MB ///////////////////////

  switch (mbsc_read_state)
  {
    case MBR_IDLE ://0
      if(mb_in_cmd.enable && mb_in_cmd.cmd == READ)
      {
        address_in_s     = mb_in_cmd.start_address;
        //get address of 64-bit block
        block_address_s  =  mb_in_cmd.start_address;
        //get address of 256-bit block
        set_address_s    = aligned(mb_in_cmd.start_address, HS_SIZE_IN_BYTES);
        //pspe access or not
        pspe_s           = inclusion(mb_in_cmd.start_address, PSPE_BASE_ADDRESS, PSPE_END_ADDRESS);
        //index of 64-bit block in 256-bit block 
        index_s          = index(mb_in_cmd.start_address, HS_SIZE_IN_BYTES);

        memory_cmd_id_s	= 6;

        if (inclusion(mb_in_cmd.start_address, PSPE_BASE_ADDRESS, PSPE_END_ADDRESS))
          //PSPE
        {	
          if (register_1_in.address == aligned(mb_in_cmd.start_address, HS_SIZE_IN_BYTES))
            //data is in out buffer
          {
            plaintext_s[0] = register_1_in.data[index(mb_in_cmd.start_address, HS_SIZE_IN_BYTES)];
            plaintext_s[1] = register_1_in.data[index(mb_in_cmd.start_address, HS_SIZE_IN_BYTES) + 1];
          }
          else
          {
            flush_register_1_s	= true;

            if (integrity_idle_v || mb_integrity_idle_v)
              //integrity blocks are idle or ht_controller is writing pspe
              mbsc_read_state = MBR_START_READ_256;
            else
              mbsc_read_state = MBR_WAIT_FOR_READ_256;
          }
        }
        else
        {
          if (register_1_in.address == aligned(mb_in_cmd.start_address, HS_SIZE_IN_BYTES))
            mbsc_read_state = MBR_START_BLOCK_DEC;
          else
          {
            flush_register_1_s	= true;

            if (integrity_idle_v)
              //integrity blocks are idle
              mbsc_read_state = MBR_START_READ_256;
            else
              mbsc_read_state = MBR_WAIT_FOR_READ_256;
          }
        }
      }
      break;

    case MBR_WAIT_FOR_READ_256 :
      if (integrity_idle_v || mb_integrity_idle_v)
        //integrity blocks are idle
        mbsc_read_state = MBR_START_READ_256;
      break;

    case MBR_START_READ_256 :
      start_ht_ic_s = true;
      if (pspe_s)
        mbsc_read_state = MBR_WAIT_READ_256;
      else
        mbsc_read_state = MBR_WAIT_FOR_BLOCK_DEC;
      break;

    case MBR_WAIT_READ_256 :
      if (in_ack.rack)	
      {
        if (fill_64(register_1_in.be, block_address_s))
        {
          plaintext_s[0]  = register_1_in.data[index_s];
          plaintext_s[1]  = register_1_in.data[index_s + 1];
          mbsc_read_state   = MBR_IDLE;
        }
      }
      break;

    case MBR_WAIT_FOR_BLOCK_DEC:
      if (fill_64(register_1_in.be, block_address_s))
        mbsc_read_state = MBR_START_BLOCK_DEC;
      break;

    case MBR_START_BLOCK_DEC :
      mbsc_read_state = MBR_WAIT_BLOCK_DEC;
      break;

    case MBR_WAIT_BLOCK_DEC :
      if (cb_in_ack)
      {	
        plaintext_s[index_s % SP_SIZE]			= cb_in_data.data[0];
        plaintext_s[1 + (index_s % SP_SIZE)]	= cb_in_data.data[1];

        if((block_address_s + SIZE_64_IN_BYTES) % SP_SIZE_IN_BYTES == 0)	
          mbsc_read_state = MBR_IDLE;
        else
        {
          index_s				= index_s + SIZE_64_IN_WORDS;
          block_address_s		= block_address_s + SIZE_64_IN_BYTES;
          if (fill_64(register_1_in.be, block_address_s + SIZE_64_IN_BYTES))
            mbsc_read_state		= MBR_START_BLOCK_DEC;
          else
            mbsc_read_state		= MBR_WAIT_FOR_BLOCK_DEC;

        }
      }
      break;

    default :
      break;
  }

  switch (mbsc_write_state)
  {
    case MBW_IDLE :
      if(mb_in_cmd.enable && mb_in_cmd.cmd == WRITE)
      { 
        address_in_s = mb_in_cmd.start_address;

        plaintext_s[0] = mb_in_wdata.data[0];
        plaintext_s[1] = mb_in_wdata.data[1];
        plaintext_s[2] = mb_in_wdata.data[2];
        plaintext_s[3] = mb_in_wdata.data[3];

        start_address_s	= mb_in_cmd.start_address;

        //get address of 64-bit block
        block_address_s		= aligned(mb_in_cmd.start_address, SIZE_64_IN_BYTES);
        set_address_s		= aligned(mb_in_cmd.start_address, SIZE_256_IN_BYTES);
        //pspe access or not
        pspe_s              = inclusion(mb_in_cmd.start_address, PSPE_BASE_ADDRESS, PSPE_END_ADDRESS);
        //index of 64-bit block in 256-bit block
        index_s             = index(mb_in_cmd.start_address, SIZE_256_IN_BYTES);

        memory_cmd_id_s	= 6;

        if (inclusion(mb_in_cmd.start_address, PSPE_BASE_ADDRESS, PSPE_END_ADDRESS))
          //pspe
        {
          plen_s			= PSPE_SIZE_IN_BYTES;
          end_address_s	= mb_in_cmd.start_address + PSPE_SIZE_IN_BYTES;

          if (register_1_in.address == aligned(mb_in_cmd.start_address, SIZE_256_IN_BYTES))
            mbsc_write_state = MBW_WRITE_64_TO_REGISTER_1;
          else
          {
            flush_register_1_s	= true;

            if (integrity_idle_v || mb_integrity_idle_v)
              //integrity blocks are idle
              mbsc_write_state = MBW_START_READ_256;
            else
              mbsc_write_state = MBW_WAIT_FOR_READ_256;
          }
        }
        else
        {
          plen_s			= SP_SIZE_IN_BYTES;
          end_address_s	= mb_in_cmd.start_address + SP_SIZE_IN_BYTES;

          if (register_1_in.address == aligned(mb_in_cmd.start_address, SIZE_256_IN_BYTES))
            mbsc_write_state = MBW_START_BLOCK_ENC;
          else
          {
            flush_register_1_s	= true;

            if (integrity_idle_v || mb_integrity_idle_v)
              //integrity blocks are idle
              mbsc_write_state = MBW_START_READ_256;
            else
              mbsc_write_state = MBW_WAIT_FOR_READ_256;
          }
        }
      }
      break;

    case MBW_START_BLOCK_ENC:
      mbsc_write_state = MBW_WAIT_BLOCK_ENC;
      break;

    case MBW_WAIT_BLOCK_ENC:
      if (cb_in_ack)
      {
        ciphertext_s[0] = cb_in_data.data[0];
        ciphertext_s[1] = cb_in_data.data[1];

        if((block_address_s + SIZE_64_IN_BYTES) % SP_SIZE_IN_BYTES == 0)
          mbsc_write_state = MBW_WRITE_64_TO_REGISTER_1;
        else
        {
          write_register_1_s	= true;
          be					= 0xFF<<index_s*4;
          block_address_s		= block_address_s + SIZE_64_IN_BYTES;
          index_s				= index_s + SIZE_64_IN_WORDS;
          mbsc_write_state	= MBW_START_BLOCK_ENC;
        }
      }
      break;

    case MBW_WAIT_FOR_READ_256:
      if (integrity_idle_v || mb_integrity_idle_v)
        //integrity blocks are idle
        mbsc_write_state = MBW_START_READ_256;
      break;

    case MBW_START_READ_256 :
      start_ht_ic_s = true;
      //start_ht_update = true;
      mbsc_write_state = MBW_WAIT_READ_256;
      break;

    case MBW_WAIT_READ_256:
      if (in_ack.rack)	
      {
        if (pspe_s)
          mbsc_write_state = MBW_WRITE_64_TO_REGISTER_1;
        else
          mbsc_write_state = MBW_START_BLOCK_ENC;
      }
      break;

    case MBW_WRITE_64_TO_REGISTER_1:
      if (integrity_idle_v || mb_integrity_idle_v)
        //write memory
        mbsc_write_state = MBW_START_WRITE_TO_MEMORY;
      else 
        mbsc_write_state = MBW_WAIT_FOR_WRITE_TO_MEMORY;
      break;

    case MBW_WAIT_FOR_WRITE_TO_MEMORY:
      if (integrity_idle_v || mb_integrity_idle_v)
        //write memory
        mbsc_write_state = MBW_START_WRITE_TO_MEMORY;
      break;

    case MBW_START_WRITE_TO_MEMORY:
      mbsc_write_state = MBW_WAIT_WRITE_TO_MEMORY;
      break;

    case MBW_WAIT_WRITE_TO_MEMORY:
      if (in_ack.wack)
        mbsc_write_state = MBW_START_HT_IU;
      break;

    case MBW_START_HT_IU:
      mbsc_write_state = MBW_IDLE;
      break;

    case MBW_WAIT_HT_IU:
      if(hash_ack_in.uack)
        mbsc_write_state = MBW_IDLE;
      break;
  }
}
void SecurityController::setBe(addr_t address)
{
  unsigned int i = index(address, SIZE_256_IN_BYTES);
  be = (~register_0_in.be & (0xFF<<i*4));
}





