diff --git a/inc/CPU.h b/inc/CPU.h
index 0729326..5843109 100644
--- a/inc/CPU.h
+++ b/inc/CPU.h
@@ -15,10 +15,11 @@
 #include "tlm_utils/simple_initiator_socket.h"
 
 #include "memory.h"
-#include "Instruction.h"
 #include "Execute.h"
 #include "Registers.h"
 #include "Log.h"
+#include "Instruction.h"
+#include "C_Instruction.h"
 
 using namespace sc_core;
 using namespace sc_dt;
@@ -47,6 +48,16 @@ private:
   Performance *perf;
   Log *log;
 
+  /**
+   * @brief Executes default ISA instruction
+   * @param  inst instruction to execute
+   * @return  true if PC is affected by instruction
+   */
+  bool process_base_instruction(Instruction &inst);
+
+  bool process_c_instruction(Instruction &inst);
+
+
   void CPU_thread(void);
 };
 
diff --git a/inc/C_Instruction.h b/inc/C_Instruction.h
new file mode 100644
index 0000000..225f017
--- /dev/null
+++ b/inc/C_Instruction.h
@@ -0,0 +1,382 @@
+/*!
+   \file Instruction.h
+   \brief Decode instructions part of the RISC-V
+   \author Màrius Montón
+   \date August 2018
+*/
+
+#ifndef C_INSTRUCTION__H
+#define C_INSTRUCTION__H
+
+#include "systemc"
+
+using namespace sc_core;
+using namespace sc_dt;
+using namespace std;
+
+typedef enum {
+OP_C_ADDI4SPN,
+OP_C_FLD,
+OP_C_LW,
+OP_C_FLW,
+OP_C_FSD,
+OP_C_SW,
+OP_C_FSW,
+
+OP_C_NOP,
+OP_C_ADDI,
+OP_C_JAL,
+OP_C_LI,
+OP_C_ADDI16SP,
+OP_C_LUI,
+OP_C_SRLI,
+OP_C_SRAI,
+OP_C_ANDI,
+OP_C_SUB,
+OP_C_XOR,
+OP_C_OR,
+OP_C_AND,
+OP_C_J,
+OP_C_BEQZ,
+OP_C_BNEZ,
+
+OP_C_SLLI,
+OP_C_FLDSP,
+OP_C_LWSP,
+OP_C_FLWSP,
+OP_C_JR,
+OP_C_MV,
+OP_C_EBREAK,
+OP_C_JALR,
+OP_C_ADD,
+OP_C_FSDSP,
+OP_C_SWSP,
+OP_C_FSWSP,
+
+OP_C_ERROR
+} op_C_Codes;
+
+
+typedef enum {
+  C_ADDI4SPN = 0b000,
+  C_FLD      = 0b001,
+  C_LW       = 0b010,
+  C_FLW      = 0b011,
+  C_FSD      = 0b101,
+  C_SW       = 0b110,
+  C_FSW      = 0b111,
+
+  C_ADDI     = 0b000,
+  C_JAL      = 0b001,
+  C_LI       = 0b010,
+  C_ADDI16SP = 0b011,
+  C_SRLI     = 0b100,
+  C_J        = 0b101,
+  C_BEQZ     = 0b110,
+  C_BNEZ     = 0b111,
+
+  C_SLLI     = 0b000,
+  C_FLDSP    = 0b001,
+  C_LWSP     = 0b010,
+  C_FLWSP    = 0b011,
+  C_JR       = 0b100,
+  C_FDSP     = 0b101,
+  C_SWSP     = 0b110,
+  C_FWWSP    = 0b111,
+} C_Codes;
+
+/**
+ * @brief Instruction decoding and fields access
+ */
+class C_Instruction{
+public:
+
+  /**
+   * @brief Constructor
+   * @param instr Instruction to decode
+   */
+  C_Instruction(sc_uint<32> instr);
+
+  /**
+   * @brief Access to opcode field
+   * @return return opcode field
+   */
+  inline int32_t opcode() {
+    return m_instr.range(1,0);
+  }
+
+  /**
+   * @brief Access to rd field
+   * @return rd field
+   */
+  inline int32_t get_rd() {
+    return m_instr.range(11, 7);
+  }
+
+  inline void set_rd(int32_t value) {
+    m_instr.range(11,7) = value;
+  }
+
+  inline int32_t get_rdp() {
+    return m_instr.range(4, 2);
+  }
+
+  /**
+   * @brief Access to rs1 field
+   * @return rs1 field
+   */
+  inline int32_t get_rs1() {
+    return m_instr.range(11, 7);
+  }
+
+  inline void set_rs1(int32_t value) {
+    m_instr.range(11,7) = value;
+  }
+
+
+  inline int32_t get_rs1p() {
+    return m_instr.range(9, 7);
+  }
+
+  /**
+   * @brief Access to rs2 field
+   * @return rs2 field
+   */
+  inline int32_t get_rs2() {
+    return m_instr.range(6, 2);
+  }
+
+  inline void set_rs2(int32_t value) {
+    m_instr.range(6,2) = value;
+  }
+
+
+  inline int32_t get_funct3() {
+    return m_instr.range(15, 13);
+  }
+
+  inline void set_funct3(int32_t value) {
+    m_instr.range(15,13) = value;
+  }
+
+  /**
+   * @brief Access to immediate field for I-type
+   * @return immediate_I field
+   */
+  inline int32_t get_imm_I() {
+    int32_t aux = 0;
+
+    aux = m_instr.range(31, 20);
+
+    /* sign extension (optimize) */
+    if (m_instr[31] == 1) {
+      aux |= (0b11111111111111111111) << 12;
+    }
+
+    return aux;
+  }
+
+  inline void set_imm_I(int32_t value) {
+    m_instr.range(31,20) = value;
+  }
+
+  /**
+   * @brief Access to immediate field for S-type
+   * @return immediate_S field
+   */
+  inline int32_t get_imm_S() {
+    int32_t aux = 0;
+
+    aux  = m_instr.range(31, 25) << 5;
+    aux |= m_instr.range(11,7);
+
+    if (m_instr[31] == 1) {
+      aux |= (0b11111111111111111111) << 12;
+    }
+
+    return aux;
+  }
+
+  inline void set_imm_S(int32_t value) {
+    sc_uint<32> aux = value;
+
+    m_instr.range(31,25) = aux.range(11,5);
+    m_instr.range(11,7) = aux.range(4,0);
+  }
+
+  /**
+   * @brief Access to immediate field for U-type
+   * @return immediate_U field
+   */
+  inline int32_t get_imm_U() {
+    return m_instr.range(31, 12);
+  }
+
+  inline void set_imm_U(int32_t value) {
+    m_instr.range(31,12) = (value << 12);
+  }
+
+  /**
+   * @brief Access to immediate field for B-type
+   * @return immediate_B field
+   */
+  inline int32_t get_imm_B() {
+    int32_t aux = 0;
+
+    aux |= m_instr[7] << 11;
+    aux |= m_instr.range(30, 25) << 5;
+    aux |= m_instr[31] << 12;
+    aux |= m_instr.range(11, 8) << 1;
+
+    if (m_instr[31] == 1) {
+      aux |= (0b11111111111111111111) << 12;
+    }
+
+    return aux;
+  }
+
+  inline void set_imm_B(int32_t value) {
+    sc_uint<32> aux = value;
+
+    m_instr[31] = aux[12];
+    m_instr.range(30,25) = aux.range(10,5);
+    m_instr.range(11,7) = aux.range(4,1);
+    m_instr[6] = aux[11];
+  }
+  /**
+   * @brief Access to immediate field for J-type
+   * @return immediate_J field
+   */
+  inline int32_t get_imm_J() {
+    int32_t aux = 0;
+
+    aux = m_instr[12] << 11;
+    aux |= m_instr[11] << 4;
+    aux |= m_instr[10] << 9;
+    aux |= m_instr[9] << 8;
+    aux |= m_instr[8] << 10;
+    aux |= m_instr[7] << 6;
+    aux |= m_instr[6] << 7;
+    aux |= m_instr.range(5,3) << 1;
+    aux |= m_instr[2] << 5;
+    
+    if (m_instr[12] == 1) {
+        aux |= 0b1111 << 12;
+    }
+
+    return aux;
+  }
+
+  inline void set_imm_J(int32_t value) {
+    sc_uint<32> aux = (value << 20);
+
+    m_instr[31] = aux[20];
+    m_instr.range(30,21) = aux.range(10,1);
+    m_instr[20] = aux[11];
+    m_instr.range(19,12) = aux.range(19,12);
+  }
+
+  inline int32_t get_imm_L() {
+      int32_t aux = 0;
+
+      aux = m_instr.range(12,10) << 3;
+      aux |= m_instr[6] << 2;
+      aux |= m_instr[5] << 6;
+
+      return aux;
+  }
+
+  inline int32_t get_imm_LWSP() {
+    int32_t aux = 0;
+
+    aux = m_instr[12] << 5;
+    aux |= m_instr.range(6,4) << 2;
+    aux |= m_instr.range(3,2) << 6;
+
+    return aux;
+  }
+
+  inline int32_t get_imm_ADDI() {
+    int32_t aux = 0;
+
+    aux = m_instr[12] << 5;
+    aux |= m_instr.range(6,2);
+
+    if (m_instr[12] == 1) {
+      aux |= 0b11111111111111111111111111 << 6;
+    }
+    return aux;
+  }
+
+  inline int32_t get_imm_ADDI4SPN() {
+    int32_t aux = 0;
+
+    aux = m_instr.range(12,11) << 4;
+    aux |= m_instr.range(10,7) << 6;
+    aux |= m_instr[6] << 2;
+    aux |= m_instr[5] << 3;
+
+    return aux;
+  }
+
+  inline int32_t get_imm_ADDI16SP() {
+    int32_t aux = 0;
+
+    aux = m_instr[12] << 9;
+    aux |= m_instr[6] << 4;
+    aux |= m_instr[5] << 6;
+    aux |= m_instr[4] << 8;
+    aux |= m_instr[3] << 7;
+    aux |= m_instr[2] << 5;
+
+    if (m_instr[12] == 1) {
+      aux |= 0b1111111111111111111111 << 10;
+    }
+    return aux;
+  }
+
+  inline int32_t get_imm_CSS() {
+    int32_t aux = 0;
+    aux = m_instr.range(12,9) << 2;
+    aux |= m_instr.range(8,7) << 6;
+
+    return aux;
+  }
+
+  inline int32_t get_imm_CB() {
+    int32_t aux = 0;
+
+    aux = m_instr[12] << 8;
+    aux |= m_instr[11] << 4;
+    aux |= m_instr[10] << 3;
+    aux |= m_instr[6] << 7;
+    aux |= m_instr[5] << 6;
+    aux |= m_instr[4] << 2;
+    aux |= m_instr[3] << 1;
+    aux |= m_instr[2] << 5;
+
+    if (m_instr[12] == 1) {
+      aux |= 0b11111111111111111111111 << 9;
+    }
+
+    return aux;
+  }
+
+  inline int32_t get_csr() {
+    return get_imm_I();
+  }
+
+  /**
+   * @brief Decodes opcode of instruction
+   * @return opcode of instruction
+   */
+  op_C_Codes decode();
+
+  inline void dump() {
+    cout << hex << "0x" << m_instr << dec << endl;
+  }
+private:
+  sc_uint<32> m_instr;
+};
+
+#endif
diff --git a/inc/Execute.h b/inc/Execute.h
index bf2a7a2..53de892 100644
--- a/inc/Execute.h
+++ b/inc/Execute.h
@@ -16,6 +16,7 @@
 
 #include "memory.h"
 #include "Instruction.h"
+#include "C_Instruction.h"
 #include "Registers.h"
 #include "Log.h"
 
@@ -43,7 +44,7 @@ public:
   void LUI(Instruction &inst);
   void AUIPC(Instruction &inst);
 
-  void JAL(Instruction &inst);
+  void JAL(Instruction &inst, bool c_extension = false, int m_rd = 1);
   void JALR(Instruction &inst);
 
   void BEQ(Instruction &inst);
@@ -55,7 +56,7 @@ public:
 
   void LB(Instruction &inst);
   void LH(Instruction &inst);
-  void LW(Instruction &inst);
+  void LW(Instruction &inst, bool c_extension = false);
   void LBU(Instruction &inst);
   void LHU(Instruction &inst);
 
@@ -65,7 +66,7 @@ public:
   void SBU(Instruction &inst);
   void SHU(Instruction &inst);
 
-  void ADDI(Instruction &inst);
+  void ADDI(Instruction &inst, bool c_extension = false);
   void SLTI(Instruction &inst);
   void SLTIU(Instruction &inst);
   void XORI(Instruction &inst);
@@ -87,6 +88,9 @@ public:
   void OR(Instruction &inst);
   void AND(Instruction &inst);
 
+  void FENCE(Instruction &inst);
+  void ECALL(Instruction &inst);
+
   void CSRRW(Instruction &inst);
   void CSRRS(Instruction &inst);
   void CSRRC(Instruction &inst);
@@ -94,6 +98,19 @@ public:
   void CSRRSI(Instruction &inst);
   void CSRRCI(Instruction &inst);
 
+  void MRET(Instruction &inst);
+
+  /* C Extensions */
+  void C_JR(Instruction &inst);
+  void C_MV(Instruction &inst);
+  void C_LWSP(Instruction &inst);
+  void C_ADDI4SPN(Instruction &inst);
+  void C_ADDI16SP(Instruction &inst);
+  void C_SWSP(Instruction &inst);
+  void C_BEQZ(Instruction &inst);
+  void C_BNEZ(Instruction &inst);
+  void C_LI(Instruction &inst);
+
   void NOP(Instruction &inst);
 
 private:
diff --git a/inc/Instruction.h b/inc/Instruction.h
index 44c75ba..aa1e386 100644
--- a/inc/Instruction.h
+++ b/inc/Instruction.h
@@ -14,6 +14,23 @@ using namespace sc_core;
 using namespace sc_dt;
 using namespace std;
 
+typedef enum {
+  BASE_EXTENSION,
+  M_EXTENSION,
+  A_EXTENSION,
+  F_EXTENSION,
+  D_EXTENSION,
+  Q_EXTENSION,
+  L_EXTENSION,
+  C_EXTENSION,
+  R_EXTENSION,
+  J_EXTENSION,
+  P_EXTENSION,
+  V_EXTENSION,
+  N_EXTENSION,
+  UNKNOWN_EXTENSION
+} extension_t;
+
 typedef enum {
 OP_LUI,
 OP_AUIPC,
@@ -58,6 +75,21 @@ OP_SRA,
 OP_OR,
 OP_AND,
 
+OP_FENCE,
+OP_ECALL,
+OP_EBREAK,
+
+OP_CSRRW,
+OP_CSRRS,
+OP_CSRRC,
+OP_CSRRWI,
+OP_CSRRSI,
+OP_CSRRCI,
+
+OP_URET,
+OP_SRET,
+OP_MRET,
+
 OP_ERROR
 } opCodes;
 
@@ -116,19 +148,34 @@ typedef enum {
   SRA_F7  = 0b0100000,
   OR_F    = 0b110,
   AND_F   = 0b111,
+
+  FENCE   = 0b0001111,
+  ECALL   = 0b1110011,
+  ECALL_F = 0b000000000000,
+  EBREAK_F= 0b000000000001,
+  URET_F  = 0b000000000010,
+  SRET_F  = 0b000100000010,
+  MRET_F  = 0b001100000010,
+  ECALL_F3= 0b000,
+  CSRRW   = 0b001,
+  CSRRS   = 0b010,
+  CSRRC   = 0b011,
+  CSRRWI  = 0b101,
+  CSRRSI  = 0b110,
+  CSRRCI  = 0b111,
 } Codes;
 
 /**
  * @brief Instruction decoding and fields access
  */
-class Instruction{
+class Instruction {
 public:
 
   /**
    * @brief Constructor
    * @param instr Instruction to decode
    */
-  Instruction(sc_int<32> instr);
+  Instruction(sc_uint<32> instr);
 
   /**
    * @brief Access to opcode field
@@ -143,47 +190,66 @@ public:
    * @brief Access to rd field
    * @return rd field
    */
-  inline int32_t rd() {
+  inline int32_t get_rd() {
     return m_instr.range(11, 7);
   }
 
+  inline void set_rd(int32_t value) {
+    m_instr.range(11,7) = value;
+  }
+
   /**
    * @brief Access to funct3 field
    * @return funct3 field
    */
-  inline int32_t funct3() {
+  inline int32_t get_funct3() {
     return m_instr.range(14, 12);
   }
 
+  inline void set_funct3(int32_t value) {
+    m_instr.range(14,12) = value;
+  }
+
   /**
    * @brief Access to rs1 field
    * @return rs1 field
    */
-  inline int32_t rs1() {
+  inline int32_t get_rs1() {
     return m_instr.range(19, 15);
   }
 
+  inline void set_rs1(int32_t value) {
+    m_instr.range(19,15) = value;
+  }
+
   /**
    * @brief Access to rs2 field
    * @return rs2 field
    */
-  inline int32_t rs2() {
+  inline int32_t get_rs2() {
     return m_instr.range(24, 20);
   }
 
+  inline void set_rs2(int32_t value) {
+    m_instr.range(24,10) = value;
+  }
   /**
    * @brief Access to funct7 field
    * @return funct7 field
    */
-  inline int32_t funct7() {
+  inline int32_t get_funct7() {
     return m_instr.range(31, 25);
   }
 
+  inline void set_func7(int32_t value) {
+    m_instr.range(31,25) = value;
+  }
+
   /**
    * @brief Access to immediate field for I-type
    * @return immediate_I field
    */
-  inline int32_t imm_I() {
+  inline int32_t get_imm_I() {
     int32_t aux = 0;
 
     aux = m_instr.range(31, 20);
@@ -196,11 +262,15 @@ public:
     return aux;
   }
 
+  inline void set_imm_I(int32_t value) {
+    m_instr.range(31,20) = value;
+  }
+
   /**
    * @brief Access to immediate field for S-type
    * @return immediate_S field
    */
-  inline int32_t imm_S() {
+  inline int32_t get_imm_S() {
     int32_t aux = 0;
 
     aux  = m_instr.range(31, 25) << 5;
@@ -213,19 +283,30 @@ public:
     return aux;
   }
 
+  inline void set_imm_S(int32_t value) {
+    sc_uint<32> aux = value;
+
+    m_instr.range(31,25) = aux.range(11,5);
+    m_instr.range(11,7) = aux.range(4,0);
+  }
+
   /**
    * @brief Access to immediate field for U-type
    * @return immediate_U field
    */
-  inline int32_t imm_U() {
+  inline int32_t get_imm_U() {
     return m_instr.range(31, 12);
   }
 
+  inline void set_imm_U(int32_t value) {
+    m_instr.range(31,12) = (value << 12);
+  }
+
   /**
    * @brief Access to immediate field for B-type
    * @return immediate_B field
    */
-  inline int32_t imm_B() {
+  inline int32_t get_imm_B() {
     int32_t aux = 0;
 
     aux |= m_instr[7] << 11;
@@ -240,11 +321,20 @@ public:
     return aux;
   }
 
+  inline void set_imm_B(int32_t value) {
+    sc_uint<32> aux = value;
+
+    m_instr[31] = aux[12];
+    m_instr.range(30,25) = aux.range(10,5);
+    m_instr.range(11,7) = aux.range(4,1);
+    m_instr[6] = aux[11];
+  }
+
   /**
    * @brief Access to immediate field for J-type
    * @return immediate_J field
    */
-  inline int32_t imm_J() {
+  inline int32_t get_imm_J() {
     int32_t aux = 0;
 
     aux = m_instr[31] << 20;
@@ -256,11 +346,25 @@ public:
     if (m_instr[31] == 1) {
       aux |= (0b111111111111) << 20;
     }
+
     return aux;
   }
 
-  inline int32_t csr() {
-    return imm_I();
+  inline void set_imm_J(int32_t value) {
+    sc_uint<32> aux = (value << 20);
+
+    m_instr[31] = aux[20];
+    m_instr.range(30,21) = aux.range(10,1);
+    m_instr[20] = aux[11];
+    m_instr.range(19,12) = aux.range(19,12);
+  }
+
+  inline int32_t get_csr() {
+    int32_t aux = 0;
+
+    aux = m_instr.range(31, 20);
+
+    return aux;
   }
 
   /**
@@ -269,11 +373,23 @@ public:
    */
   opCodes decode();
 
+  /**
+   * @brief returns what instruction extension
+   * @return extension
+   */
+  extension_t check_extension();
+
+
+  uint32_t getInstr() {
+    return m_instr;
+  }
+
+
   inline void dump() {
     cout << hex << "0x" << m_instr << dec << endl;
   }
 private:
-  sc_int<32> m_instr;
+  sc_uint<32> m_instr;
 };
 
 #endif
diff --git a/inc/Memory.h b/inc/Memory.h
index 7245a61..9f9f5b9 100644
--- a/inc/Memory.h
+++ b/inc/Memory.h
@@ -48,14 +48,12 @@ public:
   // *********************************************
   // TLM-2 forward DMI method
   // *********************************************
-
   virtual bool get_direct_mem_ptr(tlm::tlm_generic_payload& trans,
                                   tlm::tlm_dmi& dmi_data);
 
   // *********************************************
   // TLM-2 debug transport method
   // *********************************************
-
   virtual unsigned int transport_dbg(tlm::tlm_generic_payload& trans);
 
 private:
diff --git a/inc/Registers.h b/inc/Registers.h
index 4467108..575f006 100644
--- a/inc/Registers.h
+++ b/inc/Registers.h
@@ -14,6 +14,7 @@
 #include "tlm.h"
 
 #include "Performance.h"
+#include "Memory.h"
 
 using namespace sc_core;
 using namespace sc_dt;
@@ -126,8 +127,13 @@ public:
   /**
    * Increments PC couunter to next address
    */
-  inline void incPC() {
-    register_PC += 4;
+  inline void incPC(bool C_ext=false) {
+    if (C_ext == true) {
+      register_PC += 2;
+    } else {
+      register_PC += 4;
+    }
+
   }
 
   /**
diff --git a/src/CPU.cpp b/src/CPU.cpp
index 70af7ab..fd10f0f 100644
--- a/src/CPU.cpp
+++ b/src/CPU.cpp
@@ -1,4 +1,3 @@
-
 #include "CPU.h"
 
 SC_HAS_PROCESS(CPU);
@@ -22,6 +21,242 @@ CPU::~CPU() {
   cout << "*********************************************" << endl;
 }
 
+bool CPU::process_c_instruction(Instruction &inst) {
+  bool PC_not_affected = true;
+
+  C_Instruction c_inst(inst.getInstr());
+
+  switch(c_inst.decode()) {
+    case OP_C_ADDI4SPN:
+      exec->C_ADDI4SPN(inst);
+      break;
+    case OP_C_LW:
+      exec->LW(inst, true);
+      break;
+    case OP_C_ADDI:
+      exec->ADDI(inst, true);
+      break;
+    case OP_C_JAL:
+      exec->JAL(inst, true, 1);
+      PC_not_affected = false;
+      break;
+    case OP_C_J:
+      exec->JAL(inst, true, 0);
+      PC_not_affected = false;
+      break;
+    case OP_C_LI:
+      exec->C_LI(inst);
+      break;
+    case OP_C_LWSP:
+      exec->C_LWSP(inst);
+      break;
+    case OP_C_JR:
+      exec->C_JR(inst);
+      PC_not_affected = false;
+      break;
+    case OP_C_MV:
+      exec->C_MV(inst);
+      break;
+    case OP_C_SWSP:
+      exec->C_SWSP(inst);
+      break;
+    case OP_C_ADDI16SP:
+      exec->C_ADDI16SP(inst);
+      break;
+    case OP_C_BEQZ:
+      exec->C_BEQZ(inst);
+      PC_not_affected = false;
+      break;
+    case OP_C_BNEZ:
+      exec->C_BNEZ(inst);
+      PC_not_affected = false;
+      break;
+    default:
+      std::cout << "C instruction not implemented yet" << endl;
+      inst.dump();
+      exec->NOP(inst);
+      //sc_stop();
+      break;
+
+  }
+
+  return PC_not_affected;
+}
+
+bool CPU::process_base_instruction(Instruction &inst) {
+  bool PC_not_affected = true;
+
+  switch(inst.decode()) {
+    case OP_LUI:
+      exec->LUI(inst);
+      break;
+    case OP_AUIPC:
+      exec->AUIPC(inst);
+      break;
+    case OP_JAL:
+      exec->JAL(inst);
+      PC_not_affected = false;
+      break;
+    case OP_JALR:
+      exec->JALR(inst);
+      PC_not_affected = false;
+      break;
+    case OP_BEQ:
+      exec->BEQ(inst);
+      PC_not_affected = false;
+      break;
+    case OP_BNE:
+      exec->BNE(inst);
+      PC_not_affected = false;
+      break;
+    case OP_BLT:
+      exec->BLT(inst);
+      PC_not_affected = false;
+      break;
+    case OP_BGE:
+      exec->BGE(inst);
+      PC_not_affected = false;
+      break;
+    case OP_BLTU:
+      exec->BLTU(inst);
+      PC_not_affected = false;
+      break;
+    case OP_BGEU:
+      exec->BGEU(inst);
+      PC_not_affected = false;
+      break;
+    case OP_LB:
+      exec->LB(inst);
+      break;
+    case OP_LH:
+      exec->LB(inst);
+      break;
+    case OP_LW:
+      exec->LW(inst);
+      break;
+    case OP_LBU:
+      exec->LBU(inst);
+      break;
+    case OP_LHU:
+      exec->LHU(inst);
+      break;
+    case OP_SB:
+      exec->SB(inst);
+      break;
+    case OP_SH:
+      exec->SH(inst);
+      break;
+    case OP_SW:
+      exec->SW(inst);
+      break;
+    case OP_ADDI:
+      exec->ADDI(inst);
+      break;
+    case OP_SLTI:
+      exec->SLTI(inst);
+      break;
+    case OP_SLTIU:
+      exec->SLTIU(inst);
+      break;
+    case OP_XORI:
+      exec->XORI(inst);
+      break;
+    case OP_ORI:
+      exec->ORI(inst);
+      break;
+    case OP_ANDI:
+      exec->ANDI(inst);
+      break;
+    case OP_SLLI:
+      exec->SLLI(inst);
+      break;
+    case OP_SRLI:
+      exec->SRLI(inst);
+      break;
+    case OP_SRAI:
+      exec->SRAI(inst);
+      break;
+    case OP_ADD:
+      exec->ADD(inst);
+      break;
+    case OP_SUB:
+      exec->SUB(inst);
+      break;
+    case OP_SLL:
+      exec->SLL(inst);
+      break;
+    case OP_SLT:
+      exec->SLT(inst);
+      break;
+    case OP_SLTU:
+      exec->SLTU(inst);
+      break;
+    case OP_XOR:
+      exec->XOR(inst);
+      break;
+    case OP_SRL:
+      exec->SRL(inst);
+      break;
+    case OP_SRA:
+      exec->SRA(inst);
+      break;
+    case OP_OR:
+      exec->OR(inst);
+      break;
+    case OP_AND:
+      exec->AND(inst);
+      break;
+#if 0
+    case OP_CSRRW:
+      exec->CSRRW(inst);
+      break;
+    case OP_CSRRS:
+      exec->CSRRS(inst);
+      break;
+    case OP_CSRRC:
+      exec->CSRRC(inst);
+      break;
+#endif
+    case OP_FENCE:
+      exec->FENCE(inst);
+      break;
+    case OP_ECALL:
+      exec->ECALL(inst);
+      break;
+    case OP_CSRRW:
+      exec->CSRRW(inst);
+      break;
+    case OP_CSRRS:
+      exec->CSRRS(inst);
+      break;
+    case OP_CSRRC:
+      exec->CSRRC(inst);
+      break;
+    case OP_CSRRWI:
+      exec->CSRRWI(inst);
+      break;
+    case OP_CSRRSI:
+      exec->CSRRSI(inst);
+      break;
+    case OP_CSRRCI:
+      exec->CSRRCI(inst);
+      break;
+
+    case OP_MRET:
+      exec->MRET(inst);
+      PC_not_affected = false;
+      break;
+    default:
+      std::cout << "Wrong instruction" << endl;
+      inst.dump();
+      exec->NOP(inst);
+      //sc_stop();
+      break;
+  }
+
+  return PC_not_affected;
+}
+
 /**
  * main thread for CPU simulation
  * @brief CPU mai thread
@@ -29,9 +264,10 @@ CPU::~CPU() {
 void CPU::CPU_thread(void) {
 
   tlm::tlm_generic_payload* trans = new tlm::tlm_generic_payload;
-  int32_t INSTR;
+  uint32_t INSTR;
   sc_time delay = SC_ZERO_TIME;
-  bool PC_not_affected = true;
+  bool PC_not_affected;
+  bool incPCby2 = false;
 
   trans->set_command( tlm::TLM_READ_COMMAND );
   trans->set_data_ptr( reinterpret_cast<unsigned char*>(&INSTR) );
@@ -53,152 +289,32 @@ void CPU::CPU_thread(void) {
       if ( trans->is_response_error() ) {
         SC_REPORT_ERROR("CPU base", "Read memory");
       } else {
-        log->SC_log(Log::INFO) << "PC: " << hex << register_bank->getPC()
-              << dec << endl;
+        log->SC_log(Log::INFO) << "PC: 0x" << hex
+              << register_bank->getPC() << ". ";
+
         Instruction inst(INSTR);
 
-        PC_not_affected = true;
-        switch(inst.decode()) {
-          case OP_LUI:
-            exec->LUI(inst);
+        /* check what type of instruction is and execute it */
+        switch(inst.check_extension()) {
+          case BASE_EXTENSION:
+            PC_not_affected = process_base_instruction(inst);
+            incPCby2 = false;
             break;
-          case OP_AUIPC:
-            exec->AUIPC(inst);
+          case C_EXTENSION:
+            PC_not_affected = process_c_instruction(inst);
+            incPCby2 = true;
             break;
-          case OP_JAL:
-            exec->JAL(inst);
-            PC_not_affected = false;
-            break;
-          case OP_JALR:
-            exec->JALR(inst);
-            PC_not_affected = false;
-            break;
-          case OP_BEQ:
-            exec->BEQ(inst);
-            PC_not_affected = false;
-            break;
-          case OP_BNE:
-            exec->BNE(inst);
-            PC_not_affected = false;
-            break;
-          case OP_BLT:
-            exec->BLT(inst);
-            PC_not_affected = false;
-            break;
-          case OP_BGE:
-            exec->BGE(inst);
-            PC_not_affected = false;
-            break;
-          case OP_BLTU:
-            exec->BLTU(inst);
-            PC_not_affected = false;
-            break;
-          case OP_BGEU:
-            exec->BGEU(inst);
-            PC_not_affected = false;
-            break;
-          case OP_LB:
-            exec->LB(inst);
-            break;
-          case OP_LH:
-            exec->LB(inst);
-            break;
-          case OP_LW:
-            exec->LW(inst);
-            break;
-          case OP_LBU:
-            exec->LBU(inst);
-            break;
-          case OP_LHU:
-            exec->LHU(inst);
-            break;
-          case OP_SB:
-            exec->SB(inst);
-            break;
-          case OP_SH:
-            exec->SH(inst);
-            break;
-          case OP_SW:
-            exec->SW(inst);
-            break;
-          case OP_ADDI:
-            exec->ADDI(inst);
-            break;
-          case OP_SLTI:
-            exec->SLTI(inst);
-            break;
-          case OP_SLTIU:
-            exec->SLTIU(inst);
-            break;
-          case OP_XORI:
-            exec->XORI(inst);
-            break;
-          case OP_ORI:
-            exec->ORI(inst);
-            break;
-          case OP_ANDI:
-            exec->ANDI(inst);
-            break;
-          case OP_SLLI:
-            exec->SLLI(inst);
-            break;
-          case OP_SRLI:
-            exec->SRLI(inst);
-            break;
-          case OP_SRAI:
-            exec->SRAI(inst);
-            break;
-          case OP_ADD:
-            exec->ADD(inst);
-            break;
-          case OP_SUB:
-            exec->SUB(inst);
-            break;
-          case OP_SLL:
-            exec->SLL(inst);
-            break;
-          case OP_SLT:
-            exec->SLT(inst);
-            break;
-          case OP_SLTU:
-            exec->SLTU(inst);
-            break;
-          case OP_XOR:
-            exec->XOR(inst);
-            break;
-          case OP_SRL:
-            exec->SRL(inst);
-            break;
-          case OP_SRA:
-            exec->SRA(inst);
-            break;
-          case OP_OR:
-            exec->OR(inst);
-            break;
-          case OP_AND:
-            exec->AND(inst);
-            break;
-#if 0
-          case OP_CSRRW:
-            exec->CSRRW(inst);
-            break;
-          case OP_CSRRS:
-            exec->CSRRS(inst);
-            break;
-          case OP_CSRRC:
-            exec->CSRRC(inst);
-            break;
-#endif
           default:
-            cout << endl << "Instruction not implemented: ";
+            std::cout << "Extension not implemented yet" << std::endl;
             inst.dump();
             exec->NOP(inst);
+          } // switch (inst.check_extension())
         }
+
         perf->instructionsInc();
 
         if (PC_not_affected == true) {
-          register_bank->incPC();
+          register_bank->incPC(incPCby2);
         }
-      }
   } // while(1)
 } // CPU_thread
diff --git a/src/C_Instruction.cpp b/src/C_Instruction.cpp
new file mode 100644
index 0000000..5385630
--- /dev/null
+++ b/src/C_Instruction.cpp
@@ -0,0 +1,109 @@
+#include "C_Instruction.h"
+
+
+C_Instruction::C_Instruction(sc_uint<32> instr) {
+  m_instr = instr;
+}
+
+op_C_Codes C_Instruction::decode() {
+
+  switch (opcode()) {
+
+    case 0b00:
+      switch(get_funct3()) {
+        case C_ADDI4SPN:
+          return OP_C_ADDI4SPN;
+          break;
+        case C_FLD:
+          return OP_C_FLD;
+          break;
+        case C_LW:
+          return OP_C_LW;
+          break;
+        case C_FLW:
+          return OP_C_FLW;
+          break;
+        case C_FSD:
+          return OP_C_FSD;
+          break;
+        case C_SW:
+          return OP_C_SW;
+          break;
+        case C_FSW:
+          return OP_C_FSW;
+          break;
+        default:
+          return OP_C_ERROR;
+          break;
+      }
+      break;
+
+    case 0b01:
+      switch(get_funct3()) {
+        case C_ADDI:
+          return OP_C_ADDI;
+          break;
+        case C_JAL:
+          return OP_C_JAL;
+          break;
+        case C_LI:
+          return OP_C_LI;
+          break;
+        case C_ADDI16SP:
+          return OP_C_ADDI16SP;
+          break;
+        case C_SRLI:
+          return OP_C_SRLI;
+          break;
+        case C_J:
+          return OP_C_J;
+          break;
+        case C_BEQZ:
+          return OP_C_BEQZ;
+          break;
+        case C_BNEZ:
+          return OP_C_BNEZ;
+          break;
+        default:
+          return OP_C_ERROR;
+          break;
+      }
+      break;
+
+    case 0b10:
+      switch(get_funct3()) {
+        case C_SLLI:
+        case C_FLDSP:
+        case C_LWSP:
+          return OP_C_LWSP;
+          break;
+        case C_FLWSP:
+          return OP_C_FLWSP;
+          break;
+        case C_JR:
+          if (m_instr.range(6,2) == 0) {
+            return OP_C_JR;
+          } else {
+            return OP_C_MV;
+          }
+          break;
+        case C_FDSP:
+          break;
+        case C_SWSP:
+          return OP_C_SWSP;
+          break;
+        case C_FWWSP:
+        default:
+          return OP_C_ERROR;
+          break;
+      }
+      break;
+
+    default:
+      return OP_C_ERROR;
+      break;
+
+  }
+
+  return OP_C_ERROR;
+}
diff --git a/src/Execute.cpp b/src/Execute.cpp
index f10e524..25b4435 100644
--- a/src/Execute.cpp
+++ b/src/Execute.cpp
@@ -14,10 +14,11 @@ void Execute::LUI(Instruction &inst) {
   int rd;
   uint32_t imm = 0;
 
-  rd = inst.rd();
-  imm = inst.imm_U() << 12;
+  rd = inst.get_rd();
+  imm = inst.get_imm_U() << 12;
   regs->setValue(rd, imm);
-  log->SC_log(Log::INFO) << "LUI R" << rd << " <- 0x" << hex << imm << endl;
+  log->SC_log(Log::INFO) << dec << "LUI x"
+          << rd << " <- 0x" << hex << imm << endl;
 
 }
 
@@ -26,35 +27,49 @@ void Execute::AUIPC(Instruction &inst) {
   uint32_t imm = 0;
   int new_pc;
 
-  rd = inst.rd();
-  imm = inst.imm_U() << 12;
+  rd = inst.get_rd();
+  imm = inst.get_imm_U() << 12;
   new_pc = regs->getPC() + imm;
 
-  regs->setPC(new_pc);
   regs->setValue(rd, new_pc);
 
-  log->SC_log(Log::INFO) << "AUIPC R" << rd << " + PC -> PC (" << new_pc << ")" << endl;
+  log->SC_log(Log::INFO) << dec << "AUIPC x"
+          << rd << " <- " << imm << " + PC (0x" << hex
+          << new_pc << ")" << endl;
 }
 
-void Execute::JAL(Instruction &inst) {
+void Execute::JAL(Instruction &inst, bool c_extension, int m_rd) {
   int32_t mem_addr = 0;
   int rd;
   int new_pc, old_pc;
 
-  rd = inst.rd();
-  mem_addr = inst.imm_J();
+  if (c_extension == false) {
+    rd = inst.get_rd();
+    mem_addr = inst.get_imm_J();
+    old_pc = regs->getPC();
+    new_pc = old_pc + mem_addr;
 
-  old_pc = regs->getPC();
+    regs->setPC(new_pc);
 
+    old_pc = old_pc + 4;
+    regs->setValue(rd, old_pc);
+  } else {
+    C_Instruction c_inst(inst.getInstr());
 
-  new_pc = old_pc + mem_addr;
-  regs->setPC(new_pc);
+    rd = m_rd;
+    mem_addr = c_inst.get_imm_J();
+    old_pc = regs->getPC();
 
-  old_pc = old_pc + 4;
-  regs->setValue(rd, old_pc);
+    new_pc = old_pc + mem_addr;
+    regs->setPC(new_pc);
 
-  log->SC_log(Log::INFO) << dec << "JAL: R" << rd << " <- 0x" << hex << old_pc
-          << dec << " PC + " << mem_addr << " -> PC (0x"
+    old_pc = old_pc + 2;
+    regs->setValue(rd, old_pc);
+  }
+
+  log->SC_log(Log::INFO) << dec << "JAL: x"
+          << rd << " <- 0x" << hex << old_pc << dec
+          << ". PC + " << mem_addr << " -> PC (0x"
           << hex << new_pc << ")" << endl;
 }
 
@@ -63,18 +78,18 @@ void Execute::JALR(Instruction &inst) {
   int rd, rs1;
   int new_pc, old_pc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  mem_addr = inst.imm_I();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  mem_addr = inst.get_imm_I();
 
   old_pc = regs->getPC();
   regs->setValue(rd, old_pc + 4);
 
-
   new_pc = (regs->getValue(rs1) + mem_addr) & 0xFFFFFFFE;
   regs->setPC(new_pc);
 
-  log->SC_log(Log::INFO) << "JALR: R" << dec << rd << " <- 0x" << hex << old_pc + 4
+  log->SC_log(Log::INFO) << dec << "JALR: x"
+          << rd << " <- 0x" << hex << old_pc + 4
           << " PC <- 0x" << hex << new_pc << endl;
 }
 
@@ -82,18 +97,22 @@ void Execute::BEQ(Instruction &inst) {
   int rs1, rs2;
   int new_pc = 0;
 
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   if (regs->getValue(rs1) == regs->getValue(rs2)) {
-    new_pc = regs->getPC() + inst.imm_B();
+    new_pc = regs->getPC() + inst.get_imm_B();
     regs->setPC(new_pc);
+    std::cout << "HERE new_pc" << new_pc << std::endl;
   } else {
     regs->incPC();
+    new_pc = regs->getPC();
   }
 
-  log->SC_log(Log::INFO) << "BEQ R" << rs1 << " == R" << rs2  << "? -> PC (" << new_pc << ")" << endl;
-
+  log->SC_log(Log::INFO) << "BEQ x" << dec
+          << rs1 << "(" << regs->getValue(rs1) << ") == x"
+          << rs2 << "(" << regs->getValue(rs2) << ")? -> PC (0x"
+          << hex << new_pc << ")" << dec << endl;
 }
 
 void Execute::BNE(Instruction &inst) {
@@ -101,93 +120,105 @@ void Execute::BNE(Instruction &inst) {
   int new_pc = 0;
   uint32_t val1, val2;
 
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   val1 = regs->getValue(rs1);
   val2 = regs->getValue(rs2);
 
   if (val1 != val2) {
-    new_pc = regs->getPC() + inst.imm_B();
+    new_pc = regs->getPC() + inst.get_imm_B();
     regs->setPC(new_pc);
   } else {
     regs->incPC();
+    new_pc = regs->getPC();
   }
 
-  log->SC_log(Log::INFO) << "BNE: R" << rs1 << "(" <<  val1
-          << ") == R"  << rs2  << "(" << val2 << ")? -> PC ("
-          << new_pc << ")" << endl;
+  log->SC_log(Log::INFO) << "BNE: x" << dec
+          << rs1 << "(" <<  val1 << ") == x"
+          << rs2  << "(" << val2 << ")? -> PC (0x"
+          << hex << new_pc << ")" << dec << endl;
 }
 
 void Execute::BLT(Instruction &inst) {
   int rs1, rs2;
   int new_pc = 0;
 
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   if ((int32_t)regs->getValue(rs1) < (int32_t)regs->getValue(rs2)) {
-    new_pc = regs->getPC() + inst.imm_B();
+    new_pc = regs->getPC() + inst.get_imm_B();
     regs->setPC(new_pc);
   } else {
     regs->incPC();
   }
 
-  log->SC_log(Log::INFO) << "BLT R" << rs1 << " < R" << rs2  << "? -> PC (" << new_pc << ")" << endl;
+  log->SC_log(Log::INFO) << "BLT x" << dec
+          << rs1 << "(" << (int32_t)regs->getValue(rs1) << ") < x"
+          << rs2 << "(" << (int32_t)regs->getValue(rs2) << ")? -> PC (0x"
+          << hex << new_pc << ")" << dec << endl;
 }
 
 void Execute::BGE(Instruction &inst) {
   int rs1, rs2;
   int new_pc = 0;
 
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   if ((int32_t)regs->getValue(rs1) >= (int32_t)regs->getValue(rs2)) {
-    new_pc = regs->getPC() + inst.imm_B();
+    new_pc = regs->getPC() + inst.get_imm_B();
     regs->setPC(new_pc);
   } else {
     regs->incPC();
   }
 
-  log->SC_log(Log::INFO) << "BGE R" << rs1 << "(" <<
-          (int32_t)regs->getValue(rs1) << ") > R" <<
-          rs2  << "(" << (int32_t)regs->getValue(rs2)
-          << ")? -> PC (" << new_pc << ")" << endl;
+  log->SC_log(Log::INFO) << "BGE x" << dec
+          << rs1 << "(" << (int32_t)regs->getValue(rs1) << ") > x"
+          << rs2 << "(" << (int32_t)regs->getValue(rs2) << ")? -> PC (0x"
+          << hex << new_pc << ")" << dec << endl;
 }
 
 void Execute::BLTU(Instruction &inst) {
   int rs1, rs2;
   int new_pc = 0;
 
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   if (regs->getValue(rs1) < regs->getValue(rs2)) {
-    new_pc = regs->getPC() + inst.imm_B();
+    new_pc = regs->getPC() + inst.get_imm_B();
     regs->setPC(new_pc);
   } else {
     regs->incPC();
+    new_pc = regs->getPC();
   }
 
-  log->SC_log(Log::INFO) << "BLTU R" << rs1 << " < R" << rs2  << "? -> PC (" << new_pc << ")" << endl;
+  log->SC_log(Log::INFO) << "BLTU x"
+          << rs1 << "(" << regs->getValue(rs1) << ") < x"
+          << rs2 << "(" << regs->getValue(rs2) << ")? -> PC (0x"
+          << hex << new_pc << ")" << dec << endl;
 }
 
 void Execute::BGEU(Instruction &inst) {
   int rs1, rs2;
   int new_pc = 0;
 
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   if (regs->getValue(rs1) >= regs->getValue(rs2)) {
-    new_pc = regs->getPC() + inst.imm_B();
+    new_pc = regs->getPC() + inst.get_imm_B();
     regs->setPC(new_pc);
   } else {
     regs->incPC();
   }
 
-  log->SC_log(Log::INFO) << "BGEU R" << rs1 << " > R" << rs2  << "? -> PC (" << new_pc << ")" << endl;
+  log->SC_log(Log::INFO) << "BGEU x" << dec
+          << rs1 << "(" << regs->getValue(rs1) << ") > x"
+          << rs2 << "(" << regs->getValue(rs2) << ")? -> PC (0x"
+          << hex << new_pc << ")" << dec << endl;
 }
 
 void Execute::LB(Instruction &inst) {
@@ -196,16 +227,17 @@ void Execute::LB(Instruction &inst) {
   int32_t imm = 0;
   int8_t data;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  imm = inst.get_imm_I();
 
   mem_addr = imm + regs->getValue(rs1);
   data = readDataMem(mem_addr, 1);
   regs->setValue(rd, data);
 
-  log->SC_log(Log::INFO) << "LB: R" << rs1 << " + " << imm << " (@0x"
-          << hex <<mem_addr << dec << ") -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "LB: x"
+          << rs1 << " + " << imm << " (@0x"
+          << hex << mem_addr << dec << ") -> x" << rd << endl;
 }
 
 void Execute::LH(Instruction &inst) {
@@ -214,34 +246,44 @@ void Execute::LH(Instruction &inst) {
   int32_t imm = 0;
   int16_t data;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  imm = inst.get_imm_I();
 
   mem_addr = imm + regs->getValue(rs1);
   data = readDataMem(mem_addr, 2);
   regs->setValue(rd, data);
 
-  log->SC_log(Log::INFO) << "LH: R" << rs1 << " + " << imm << " (@0x"
-          << hex <<mem_addr << dec << ") -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "LH: x"
+          << rs1 << " + " << imm << " (@0x"
+          << hex << mem_addr << dec << ") -> x" << rd << endl;
 }
 
-void Execute::LW(Instruction &inst) {
+void Execute::LW(Instruction &inst, bool c_extension) {
   uint32_t mem_addr = 0;
   int rd, rs1;
   int32_t imm = 0;
   uint32_t data;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  if (c_extension == false) {
+    rd = inst.get_rd();
+    rs1 = inst.get_rs1();
+    imm = inst.get_imm_I();
+  } else {
+    C_Instruction c_inst(inst.getInstr());
+
+    rd = c_inst.get_rdp();
+    rs1 = c_inst.get_rs1p();
+    imm = c_inst.get_imm_L();
+  }
 
   mem_addr = imm + regs->getValue(rs1);
   data = readDataMem(mem_addr, 4);
   regs->setValue(rd, data);
 
-  log->SC_log(Log::INFO) << "LW: R" << rs1 << " + " << imm << " (@0x"
-          << hex <<mem_addr << dec << ") -> R" << rd << endl;
+  log->SC_log(Log::INFO) << dec << "C.LW: x"
+          << rs1 << " + " << imm << " (@0x" << hex
+          << mem_addr << dec << ") -> x" << rd << endl;
 }
 
 void Execute::LBU(Instruction &inst) {
@@ -250,16 +292,17 @@ void Execute::LBU(Instruction &inst) {
   int32_t imm = 0;
   uint8_t data;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  imm = inst.get_imm_I();
 
   mem_addr = imm + regs->getValue(rs1);
   data = readDataMem(mem_addr, 1);
   regs->setValue(rd, data);
 
-  log->SC_log(Log::INFO) << "LBU: R" << rs1 << " + " << imm << " (@0x"
-          << hex <<mem_addr << dec << ") -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "LBU: x"
+          << rs1 << " + " << imm << " (@0x"
+          << hex <<mem_addr << dec << ") -> x" << rd << endl;
 }
 
 void Execute::LHU(Instruction &inst) {
@@ -268,16 +311,17 @@ void Execute::LHU(Instruction &inst) {
   int32_t imm = 0;
   uint16_t data;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  imm = inst.get_imm_I();
 
   mem_addr = imm + regs->getValue(rs1);
   data = readDataMem(mem_addr, 2);
   regs->setValue(rd, data);
 
-  log->SC_log(Log::INFO) << "LHU: R" << rs1 << " + " << imm << " (@0x"
-          << hex <<mem_addr << dec << ") -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "LHU: x"
+          << rs1 << " + " << imm << " (@0x"
+          << hex <<mem_addr << dec << ") -> x" << rd << endl;
 }
 
 void Execute::SB(Instruction &inst) {
@@ -286,17 +330,18 @@ void Execute::SB(Instruction &inst) {
   int32_t imm = 0;
   uint32_t data;
 
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
-  imm = inst.imm_S();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
+  imm = inst.get_imm_S();
 
   mem_addr = imm + regs->getValue(rs1);
   data = regs->getValue(rs2);
 
   writeDataMem(mem_addr, data, 1);
 
-  log->SC_log(Log::INFO) << "SB: R" << rs2 << " -> R" << rs1 << " + "
-          << imm << " (@0x" << hex <<mem_addr << dec << ")" <<  endl;
+  log->SC_log(Log::INFO) << "SB: x"
+          << rs2 << " -> x" << rs1 << " + " << imm
+          << " (@0x" << hex <<mem_addr << dec << ")" << endl;
 }
 
 void Execute::SH(Instruction &inst) {
@@ -305,17 +350,19 @@ void Execute::SH(Instruction &inst) {
   int32_t imm = 0;
   uint32_t data;
 
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
-  imm = inst.imm_S();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
+  imm = inst.get_imm_S();
 
   mem_addr = imm + regs->getValue(rs1);
   data = regs->getValue(rs2);
 
   writeDataMem(mem_addr, data, 2);
 
-  log->SC_log(Log::INFO) << "SH: R" << rs2 << " -> R" << rs1 << " + "
-          << imm << " (@0x" << hex <<mem_addr << dec << ")" <<  endl;
+  log->SC_log(Log::INFO) << "SH: x"
+          << rs2 << " -> x"
+          << rs1 << " + " << imm << " (@0x" << hex
+          << mem_addr << dec << ")" << endl;
 }
 
 void Execute::SW(Instruction &inst) {
@@ -324,51 +371,66 @@ void Execute::SW(Instruction &inst) {
   int32_t imm = 0;
   uint32_t data;
 
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
-  imm = inst.imm_S();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
+  imm = inst.get_imm_S();
 
   mem_addr = imm + regs->getValue(rs1);
   data = regs->getValue(rs2);
 
   writeDataMem(mem_addr, data, 4);
 
-  log->SC_log(Log::INFO) << "SW: R" << dec << rs2 << "(0x" << hex << data
-          << ") -> R" << dec << rs1 << " + " << imm
-          << " (@0x" << hex << mem_addr << dec << ")" <<  endl;
+  log->SC_log(Log::INFO) << dec << "SW: x"
+          << rs2 << "(0x" << hex << data << ") -> x" << dec
+          << rs1 << " + " << imm << " (@0x" << hex
+          << mem_addr << dec << ")" << endl;
 }
 
-void Execute::ADDI(Instruction &inst) {
+void Execute::ADDI(Instruction &inst, bool c_extension) {
   int rd, rs1;
   int32_t imm = 0;
   int32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  if (c_extension == false) {
+    rd = inst.get_rd();
+    rs1 = inst.get_rs1();
+    imm = inst.get_imm_I();
+  } else {
+    C_Instruction c_inst(inst.getInstr());
+
+    rd = c_inst.get_rd();
+    rs1 = rd;
+    imm = c_inst.get_imm_ADDI();
+  }
 
   calc = regs->getValue(rs1) + imm;
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << dec << "ADDI: R" << rs1 << " + " << imm << " -> R" << rd << endl;
+  log->SC_log(Log::INFO) << dec << "ADDI: x"
+          << rs1 << " + " << imm << " -> x"
+          << rd  << "(" << calc << ")"<< endl;
 }
 
 void Execute::SLTI(Instruction &inst) {
   int rd, rs1;
   int32_t imm;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  imm = inst.get_imm_I();
 
   if (regs->getValue(rs1) < imm) {
     regs->setValue(rd, 1);
-    log->SC_log(Log::INFO) << "SLTI: R" << rs1 << " < " << imm
-          << " => " << "1 -> R" << rd << endl;
+    log->SC_log(Log::INFO) << "SLTI: x"
+            << rs1 << " < "
+            << imm << " => " << "1 -> x"
+            << rd  << endl;
   } else {
     regs->setValue(rd, 0);
-    log->SC_log(Log::INFO) << "SLTI: R" << rs1 << " < " << imm
-          << " => " << "0 -> R" << rd << endl;
+    log->SC_log(Log::INFO) << "SLTI: x"
+            << rs1 << " < "
+            << imm << " => " << "0 -> x"
+            << rd  << endl;
   }
 }
 
@@ -376,18 +438,22 @@ void Execute::SLTIU(Instruction &inst) {
   int rd, rs1;
   int32_t imm;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  imm = inst.get_imm_I();
 
   if ((uint32_t) regs->getValue(rs1) < (uint32_t)imm) {
     regs->setValue(rd, 1);
-    log->SC_log(Log::INFO) << "SLTIU: R" << rs1 << " < " << imm
-          << " => " << "1 -> R" << rd << endl;
+    log->SC_log(Log::INFO) << "SLTIU: x"
+            << rs1 << " < "
+            << imm << " => " << "1 -> x"
+            << rd  << endl;
   } else {
     regs->setValue(rd, 0);
-    log->SC_log(Log::INFO) << "SLTIU: R" << rs1 << " < " << imm
-          << " => " << "0 -> R" << rd << endl;
+    log->SC_log(Log::INFO) << "SLTIU: x"
+            << rs1 << " < "
+            << imm << " => " << "0 -> x"
+            << rd  << endl;
   }
 }
 
@@ -396,15 +462,17 @@ void Execute::XORI(Instruction &inst) {
   int32_t imm;
   uint32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  imm = inst.get_imm_I();
 
   calc = regs->getValue(rs1) ^ imm;
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "XORI: R" << rs1 << " XOR " << imm
-          << "-> R" << rd << endl;
+  log->SC_log(Log::INFO) << "XORI: x"
+          << rs1 << " XOR "
+          << imm << "-> x"
+          << rd  << endl;
 }
 
 void Execute::ORI(Instruction &inst) {
@@ -412,15 +480,17 @@ void Execute::ORI(Instruction &inst) {
   int32_t imm;
   uint32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  imm = inst.get_imm_I();
 
   calc = regs->getValue(rs1) | imm;
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "ORI: R" << rs1 << " OR " << imm
-          << "-> R" << rd << endl;
+  log->SC_log(Log::INFO) << "ORI: x"
+          << rs1 << " OR "
+          << imm << "-> x"
+          << rd  << endl;
 }
 
 void Execute::ANDI(Instruction &inst) {
@@ -428,15 +498,17 @@ void Execute::ANDI(Instruction &inst) {
   int32_t imm;
   uint32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  imm = inst.imm_I();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  imm = inst.get_imm_I();
 
   calc = regs->getValue(rs1) & imm;
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "ANDI: R" << rs1 << " AND " << imm
-          << " -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "ANDI: x"
+          << rs1 << " AND "
+          << imm << " -> x"
+          << rd  << endl;
 }
 
 void Execute::SLLI(Instruction &inst) {
@@ -444,16 +516,18 @@ void Execute::SLLI(Instruction &inst) {
   uint32_t shift;
   uint32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   shift = rs2 & 0x1F;
 
   calc = ((uint32_t)regs->getValue(rs1)) << shift;
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "SLLI: R" << rs1 << " << " << shift << " -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "SLLI: x"
+          << rs1 << " << " << shift << " -> x"
+          << rd  << endl;
 }
 
 void Execute::SRLI(Instruction &inst) {
@@ -461,16 +535,18 @@ void Execute::SRLI(Instruction &inst) {
   uint32_t shift;
   uint32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   shift = rs2 & 0x1F;
 
   calc = ((uint32_t)regs->getValue(rs1)) >> shift;
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "SRLI: R" << rs1 << " >> " << shift << " -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "SRLI: x"
+          << rs1 << " >> " << shift << " -> x"
+          << rd  << endl;
 }
 
 void Execute::SRAI(Instruction &inst) {
@@ -478,44 +554,53 @@ void Execute::SRAI(Instruction &inst) {
   uint32_t shift;
   int32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   shift = rs2 & 0x1F;
 
   calc = regs->getValue(rs1) >> shift;
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "SRAI: R" << rs1 << " >> " << shift << " -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "SRAI: x"
+          << rs1 << " >> " << shift << " -> x"
+          << rd  << endl;
 }
 
 void Execute::ADD(Instruction &inst) {
   int rd, rs1, rs2;
   uint32_t calc;
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   calc = regs->getValue(rs1) + regs->getValue(rs2);
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "ADD: R" << rs1 << " + R" << rs2 << " -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "ADD: x"
+          << rs1 << " + x"
+          << rs2 << " -> x"
+          << rd  << endl;
 }
 
 void Execute::SUB(Instruction &inst) {
   int rd, rs1, rs2;
   uint32_t calc;
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   calc = regs->getValue(rs1) - regs->getValue(rs2);
   regs->setValue(rd, calc);
 
   /* Can insert some arbitrary execution time */
-  wait(sc_time(10, SC_NS));
-  log->SC_log(Log::INFO) << "SUB: R" << rs1 << " - R" << rs2 << " -> R" << rd << endl;
+  //wait(sc_time(10, SC_NS));
+
+  log->SC_log(Log::INFO) << "SUB: x"
+          << rs1 << " - x"
+          << rs2 << " -> x"
+          << rd  << endl;
 }
 
 void Execute::SLL(Instruction &inst) {
@@ -523,90 +608,97 @@ void Execute::SLL(Instruction &inst) {
   uint32_t shift;
   uint32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   shift = regs->getValue(rs2) & 0x1F;
 
   calc = ((uint32_t)regs->getValue(rs1)) << shift;
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "SLL: R" << rs1 << " << " << shift << " -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "SLL: x"
+          << rs1 << " << " << shift << " -> x"
+          << rd  << endl;
 }
 
-
-/** */
 void Execute::SLT(Instruction &inst) {
   int rd, rs1, rs2;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   if (regs->getValue(rs1) < regs->getValue(rs2)) {
     regs->setValue(rd, 1);
-    log->SC_log(Log::INFO) << "SLT: R" << rs1 << " < R" << rs2
-          << " => " << "1 -> R" << rd << endl;
+    log->SC_log(Log::INFO) << "SLT: x"
+            << rs1 << " < x"
+            << rs2 << " => " << "1 -> x"
+            << rd << endl;
   } else {
     regs->setValue(rd, 0);
-    log->SC_log(Log::INFO) << "SLT: R" << rs1 << " < R" << rs2
-          << " => " << "0 -> R" << rd << endl;
+    log->SC_log(Log::INFO) << "SLT: x"
+            << rs1 << " < x"
+            << rs2 << " => " << "0 -> x"
+            << rd  << endl;
   }
 }
 
-
 void Execute::SLTU(Instruction &inst) {
   int rd, rs1, rs2;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   if ( (uint32_t)regs->getValue(rs1) < (uint32_t)regs->getValue(rs2)) {
     regs->setValue(rd, 1);
-    log->SC_log(Log::INFO) << "SLTU: R" << rs1 << " < R" << rs2
-          << " => " << "1 -> R" << rd << endl;
+    log->SC_log(Log::INFO) << "SLTU: x"
+            << rs1 << " < x"
+            << rs2 << " => " << "1 -> x"
+            << rd << endl;
   } else {
     regs->setValue(rd, 0);
-    log->SC_log(Log::INFO) << "SLTU: R" << rs1 << " < R" << rs2
-          << " => " << "0 -> R" << rd << endl;
+    log->SC_log(Log::INFO) << "SLTU: x"
+            << rs1 << " < x"
+            << rs2 << " => " << "0 -> x"
+            << rd  << endl;
   }
 }
 
-
 void Execute::XOR(Instruction &inst) {
   int rd, rs1, rs2;
   uint32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   calc = regs->getValue(rs1) ^ regs->getValue(rs2);
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "XOR: R" << rs1 << " XOR R" << rs2
-          << "-> R" << rd << endl;
+  log->SC_log(Log::INFO) << "XOR: x"
+          << rs1 << " XOR x"
+          << rs2 << "-> x"
+          << rd  << endl;
 }
 
-
-
 void Execute::SRL(Instruction &inst) {
   int rd, rs1, rs2;
   uint32_t shift;
   uint32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   shift = regs->getValue(rs2) & 0x1F;
 
   calc = ((uint32_t)regs->getValue(rs1)) >> shift;
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "SRL: R" << rs1 << " >> " << shift << " -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "SRL: x"
+          << rs1 << " >> " << shift << " -> x" << rd << endl;
 }
 
 void Execute::SRA(Instruction &inst) {
@@ -614,48 +706,62 @@ void Execute::SRA(Instruction &inst) {
   uint32_t shift;
   int32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   shift = regs->getValue(rs2) & 0x1F;
 
   calc = regs->getValue(rs1) >> shift;
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "SRA: R" << rs1 << " >> " << shift << " -> R" << rd << endl;
+  log->SC_log(Log::INFO) << "SRA: x"
+          << rs1 << " >> " << shift << " -> x" << rd << endl;
 }
 
-
 void Execute::OR(Instruction &inst) {
   int rd, rs1, rs2;
   uint32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   calc = regs->getValue(rs1) | regs->getValue(rs2);
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "OR: R" << rs1 << " OR R" << rs2
-          << "-> R" << rd << endl;
+  log->SC_log(Log::INFO) << "OR: x" << rs1 << " OR x" << rs2
+          << "-> x" << rd << endl;
 }
 
-
 void Execute::AND(Instruction &inst) {
   int rd, rs1, rs2;
   uint32_t calc;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  rs2 = inst.rs2();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  rs2 = inst.get_rs2();
 
   calc = regs->getValue(rs1) & regs->getValue(rs2);
   regs->setValue(rd, calc);
 
-  log->SC_log(Log::INFO) << "AND: R" << rs1 << " AND R" << rs2
-          << "-> R" << rd << endl;
+  log->SC_log(Log::INFO) << "AND: x" << rs1 << " AND x" << rs2
+          << "-> x" << rd << endl;
+}
+
+void Execute::FENCE(Instruction &inst) {
+  log->SC_log(Log::INFO) << "FENCE" << endl;
+}
+
+void Execute::ECALL(Instruction &inst) {
+
+  log->SC_log(Log::INFO) << "ECALL" << endl;
+  std::cout << "ECALL Instruction called, stopping simulation" << endl;
+  regs->dump();
+  cout << "Simulation time " << sc_time_stamp() << endl;
+  perf->dump();
+
+  SC_REPORT_ERROR("Execute", "ECALL");
 }
 
 void Execute::CSRRW(Instruction &inst) {
@@ -663,22 +769,21 @@ void Execute::CSRRW(Instruction &inst) {
   int csr;
   uint32_t aux;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  csr = inst.csr();
-
-  if (rd == 0) {
-    return;
-  }
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  csr = inst.get_csr();
 
   /* These operations must be atomical */
-  aux = regs->getCSR(csr);
-  regs->setValue(rd, aux);
+  if (rd != 0) {
+    aux = regs->getCSR(csr);
+    regs->setValue(rd, aux);
+  }
   aux = regs->getValue(rs1);
   regs->setCSR(csr, aux);
 
-  log->SC_log(Log::INFO) << "CSRRW: CSR #" << csr << " -> R" << rd
-          << ". R" << rs1 << "-> CSR #" << csr << endl;
+  log->SC_log(Log::INFO) << "CSRRW: CSR #"
+          << csr << " -> x" << rd
+          << ". x" << rs1 << "-> CSR #" << csr << endl;
 }
 
 void Execute::CSRRS(Instruction &inst) {
@@ -686,9 +791,9 @@ void Execute::CSRRS(Instruction &inst) {
   int csr;
   uint32_t bitmask, aux;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  csr = inst.csr();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  csr = inst.get_csr();
 
   if (rd == 0) {
     return;
@@ -702,8 +807,9 @@ void Execute::CSRRS(Instruction &inst) {
   aux = aux | bitmask;
   regs->setCSR(csr, aux);
 
-  log->SC_log(Log::INFO) << "CSRRS: CSR #" << csr << " -> R" << rd
-          << ". R" << rs1 << " & CSR #" << csr << endl;
+  log->SC_log(Log::INFO) << "CSRRS: CSR #"
+          << csr << " -> x" << rd
+          << ". x" << rs1 << " & CSR #" << csr << endl;
 }
 
 void Execute::CSRRC(Instruction &inst) {
@@ -711,9 +817,9 @@ void Execute::CSRRC(Instruction &inst) {
   int csr;
   uint32_t bitmask, aux;
 
-  rd = inst.rd();
-  rs1 = inst.rs1();
-  csr = inst.csr();
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  csr = inst.get_csr();
 
   if (rd == 0) {
     return;
@@ -727,10 +833,287 @@ void Execute::CSRRC(Instruction &inst) {
   aux = aux & ~bitmask;
   regs->setCSR(csr, aux);
 
-  log->SC_log(Log::INFO) << "CSRRC: CSR #" << csr << " -> R" << rd
-          << ". R" << rs1 << " & CSR #" << csr << endl;
+  log->SC_log(Log::INFO) << "CSRRC: CSR #"
+          << csr << " -> x" << rd
+          << ". x" << rs1 << " & CSR #" << csr << endl;
 }
 
+void Execute::CSRRWI(Instruction &inst) {
+  int rd, rs1;
+  int csr;
+  uint32_t aux;
+
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  csr = inst.get_csr();
+
+
+  /* These operations must be atomical */
+  if (rd != 0) {
+    aux = regs->getCSR(csr);
+    regs->setValue(rd, aux);
+  }
+  aux = rs1;
+  regs->setCSR(csr, aux);
+
+  log->SC_log(Log::INFO) << "CSRRWI: CSR #"
+          << csr << " -> x" << rd
+          << ". x" << rs1 << "-> CSR #" << csr << endl;
+}
+
+void Execute::CSRRSI(Instruction &inst) {
+  int rd, rs1;
+  int csr;
+  uint32_t bitmask, aux;
+
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  csr = inst.get_csr();
+
+  if (rs1 == 0) {
+    return;
+  }
+
+  /* These operations must be atomical */
+  aux = regs->getCSR(csr);
+  regs->setValue(rd, aux);
+
+  bitmask = rs1;
+  aux = aux | bitmask;
+  regs->setCSR(csr, aux);
+
+  log->SC_log(Log::INFO) << "CSRRSI: CSR #"
+          << csr << " -> x" << rd
+          << ". x" << rs1 << " & CSR #" << csr << endl;
+}
+
+void Execute::CSRRCI(Instruction &inst) {
+  int rd, rs1;
+  int csr;
+  uint32_t bitmask, aux;
+
+  rd = inst.get_rd();
+  rs1 = inst.get_rs1();
+  csr = inst.get_csr();
+
+  if (rs1 == 0) {
+    return;
+  }
+
+  /* These operations must be atomical */
+  aux = regs->getCSR(csr);
+  regs->setValue(rd, aux);
+
+  bitmask = rs1;
+  aux = aux & ~bitmask;
+  regs->setCSR(csr, aux);
+
+  log->SC_log(Log::INFO) << "CSRRCI: CSR #"
+          << csr << " -> x" << rd
+          << ". x" << rs1 << " & CSR #" << csr << endl;
+}
+
+void Execute::MRET(Instruction &inst) {
+  uint32_t new_pc = 0;
+
+  new_pc = regs->getCSR(0x341);
+  regs->setPC(new_pc);
+
+  log->SC_log(Log::INFO) << "MRET: PC <- 0x" << hex << new_pc << endl;
+}
+
+void Execute::C_JR(Instruction &inst) {
+  uint32_t mem_addr = 0;
+  int rs1;
+  int new_pc;
+
+  C_Instruction c_inst(inst.getInstr());
+
+  rs1 = c_inst.get_rs1();
+  mem_addr = 0;
+
+  std::cout << "rs1 :" << rs1 << std::endl;
+
+  new_pc = (regs->getValue(rs1) + mem_addr) & 0xFFFFFFFE;
+  regs->setPC(new_pc);
+
+  log->SC_log(Log::INFO) << "JR: PC <- 0x" << hex << new_pc << endl;
+}
+
+void Execute::C_MV(Instruction &inst) {
+  int rd, rs1, rs2;
+  uint32_t calc;
+
+  C_Instruction c_inst(inst.getInstr());
+
+  rd = c_inst.get_rd();
+  rs1 = 0;
+  rs2 = c_inst.get_rs2();
+
+  calc = regs->getValue(rs1) + regs->getValue(rs2);
+  regs->setValue(rd, calc);
+
+  log->SC_log(Log::INFO) << "MV: x" << dec
+          << rs1 << "(" << regs->getValue(rs1) << ") + x"
+          << rs2 << "(" << regs->getValue(rs2) << ") -> x"
+          << rd  << "(" << calc << ")" << endl;
+}
+
+void Execute::C_LWSP(Instruction &inst) {
+  uint32_t mem_addr = 0;
+  int rd, rs1;
+  int32_t imm = 0;
+  uint32_t data;
+
+  // lw rd, offset[7:2](x2)
+  C_Instruction c_inst(inst.getInstr());
+
+  rd = c_inst.get_rd();
+  rs1 = 2;
+  imm = c_inst.get_imm_LWSP();
+
+  mem_addr = imm + regs->getValue(rs1);
+  data = readDataMem(mem_addr, 4);
+
+  regs->setValue(rd, data);
+
+  log->SC_log(Log::INFO) << "C.LWSP: x" << dec
+          << rs1 << "(0x" << hex << regs->getValue(rs1) << ") + "
+          << dec << imm << " (@0x" << hex << mem_addr << dec << ") -> x"
+          << rd << "(" << hex << data << ")"<< dec << endl;
+}
+
+void Execute::C_ADDI4SPN(Instruction  &inst) {
+  int rd, rs1;
+  int32_t imm = 0;
+  int32_t calc;
+
+  C_Instruction c_inst(inst.getInstr());
+
+  rd = c_inst.get_rdp();
+  rs1 = 2;
+  imm = c_inst.get_imm_ADDI4SPN();
+
+  calc = regs->getValue(rs1) + imm;
+  regs->setValue(rd, calc);
+
+  log->SC_log(Log::INFO) << dec << "ADDI4SPN: x"
+          << rs1 << "(0x" << hex << regs->getValue(rs1) << ") + "
+          << dec << imm << " -> x"
+          << rd << "(0x" << hex << calc << ")" << endl;
+}
+
+void Execute::C_ADDI16SP(Instruction &inst) {
+  // addi x2, x2, nzimm[9:4]
+  int rd, rs1;
+  int32_t imm = 0;
+  int32_t calc;
+
+  C_Instruction c_inst(inst.getInstr());
+
+  rd = 2;
+  rs1 = 2;
+  imm = c_inst.get_imm_ADDI16SP();
+
+  calc = regs->getValue(rs1) + imm;
+  regs->setValue(rd, calc);
+
+  log->SC_log(Log::INFO) << dec << "ADDI16SP: x"
+          << rs1 << " + "
+          << dec << imm << " -> x"
+          << rd  << "(0x" << hex << calc << ")" << endl;
+}
+
+void Execute::C_SWSP(Instruction &inst) {
+  // sw rs2, offset(x2)
+  uint32_t mem_addr = 0;
+  int rs1, rs2;
+  int32_t imm = 0;
+  uint32_t data;
+
+  C_Instruction c_inst(inst.getInstr());
+
+  rs1 = 2;
+  rs2 = c_inst.get_rs2();
+  imm = c_inst.get_imm_CSS();
+
+  mem_addr = imm + regs->getValue(rs1);
+  data = regs->getValue(rs2);
+
+  writeDataMem(mem_addr, data, 4);
+
+  log->SC_log(Log::INFO) << dec << "SWSP: x"
+          << rs2 << "(0x" << hex << data << ") -> x" << dec
+          << rs1 << " + " << imm << " (@0x" << hex
+          << mem_addr << dec << ")" << endl;
+}
+
+void Execute::C_BEQZ(Instruction &inst) {
+  int rs1;
+  int new_pc = 0;
+  uint32_t val1;
+  C_Instruction c_inst(inst.getInstr());
+
+  rs1 = c_inst.get_rs1p();
+  val1 = regs->getValue(rs1);
+
+  if (val1 == 0) {
+    new_pc = regs->getPC() + c_inst.get_imm_CB();
+    regs->setPC(new_pc);
+  } else {
+    regs->incPC(true); //PC <- PC + 2
+    new_pc = regs->getPC();
+  }
+
+  log->SC_log(Log::INFO) << "C.BEQZ: x" << dec
+          << rs1 << "(" <<  val1 << ") == 0? -> PC (0x"
+          << hex << new_pc << ")" << dec << endl;
+}
+
+void Execute::C_BNEZ(Instruction &inst) {
+  int rs1;
+  int new_pc = 0;
+  uint32_t val1;
+  C_Instruction c_inst(inst.getInstr());
+
+  rs1 = c_inst.get_rs1p();
+  val1 = regs->getValue(rs1);
+
+  if (val1 != 0) {
+    new_pc = regs->getPC() + c_inst.get_imm_CB();
+    regs->setPC(new_pc);
+  } else {
+    regs->incPC(true); //PC <- PC +2
+    new_pc = regs->getPC();
+  }
+
+  log->SC_log(Log::INFO) << "C.BNEZ: x" << dec
+          << rs1 << "(" <<  val1 << ") != 0? -> PC (0x"
+          << hex << new_pc << ")" << dec << endl;
+}
+
+void Execute::C_LI(Instruction &inst) {
+
+  int rd, rs1;
+  int32_t imm = 0;
+  int32_t calc;
+
+  C_Instruction c_inst(inst.getInstr());
+
+  rd = c_inst.get_rd();
+  rs1 = 0;
+  imm = c_inst.get_imm_ADDI();
+
+  calc = regs->getValue(rs1) + imm;
+  regs->setValue(rd, calc);
+
+  log->SC_log(Log::INFO) << dec << "LI: x"
+          << rs1 << "(" << regs->getValue(rs1) << ") + "
+          << imm << " -> x" << rd  << "(" << calc << ")" << endl;
+}
+
+
+
 void Execute::NOP(Instruction &inst) {
   cout << endl;
   regs->dump();
@@ -762,6 +1145,9 @@ uint32_t Execute::readDataMem(uint32_t addr, int size) {
 
   data_bus->b_transport( trans, delay);
 
+  if ( trans.is_response_error() ) {
+    SC_REPORT_ERROR("Memory", "Read memory");
+  }
   return data;
 }
 
diff --git a/src/Instruction.cpp b/src/Instruction.cpp
index 456c602..503eec8 100644
--- a/src/Instruction.cpp
+++ b/src/Instruction.cpp
@@ -1,7 +1,7 @@
 #include "Instruction.h"
 
 
-Instruction::Instruction(sc_int<32> instr) {
+Instruction::Instruction(sc_uint<32> instr) {
   m_instr = instr;
 }
 
@@ -16,7 +16,7 @@ opCodes Instruction::decode() {
     case JALR:
         return OP_JALR;
     case BEQ:
-      switch(funct3()) {
+      switch(get_funct3()) {
         case BEQ_F:
           return OP_BEQ;
         case BNE_F:
@@ -32,7 +32,7 @@ opCodes Instruction::decode() {
       }
       return OP_ERROR;
     case LB:
-      switch(funct3()) {
+      switch(get_funct3()) {
         case LB_F:
           return OP_LB;
         case LH_F:
@@ -46,7 +46,7 @@ opCodes Instruction::decode() {
       }
       return OP_ERROR;
     case SB:
-      switch(funct3()) {
+      switch(get_funct3()) {
         case SB_F:
           return OP_SB;
         case SH_F:
@@ -56,7 +56,7 @@ opCodes Instruction::decode() {
       }
       return OP_ERROR;
     case ADDI:
-      switch(funct3()) {
+      switch(get_funct3()) {
         case ADDI_F:
           return OP_ADDI;
         case SLTI_F:
@@ -72,7 +72,7 @@ opCodes Instruction::decode() {
         case SLLI_F:
           return OP_SLLI;
         case SRLI_F:
-          switch(funct7()) {
+          switch(get_funct7()) {
             case SRLI_F7:
               return OP_SRLI;
             case SRAI_F7:
@@ -82,9 +82,9 @@ opCodes Instruction::decode() {
       }
       return OP_ERROR;
     case ADD: {
-      switch(funct3()) {
+      switch(get_funct3()) {
         case ADD_F:
-          switch (funct7()) {
+          switch (get_funct7()) {
             case ADD_F7:
               return OP_ADD;
             case SUB_F7:
@@ -100,7 +100,7 @@ opCodes Instruction::decode() {
         case XOR_F:
           return OP_XOR;
         case SRL_F:
-          switch(funct7()) {
+          switch(get_funct7()) {
             case SRL_F7:
               return OP_SRL;
             case SRA_F7:
@@ -112,8 +112,64 @@ opCodes Instruction::decode() {
           return OP_AND;
       }
     } /* ADD */
-      return OP_ERROR;
+    case FENCE:
+      return OP_FENCE;
+    case ECALL: {
+      switch (get_funct3()) {
+        case ECALL_F3:
+          switch(get_csr()) {
+            case ECALL_F:
+              return OP_ECALL;
+            case EBREAK_F:
+                return OP_EBREAK;
+            case URET_F:
+              return OP_URET;
+            case SRET_F:
+              return OP_SRET;
+            case MRET_F:
+              return OP_MRET;
+          }
+          break;
+        case CSRRW:
+          return OP_CSRRW;
+          break;
+        case CSRRS:
+          return OP_CSRRS;
+          break;
+        case CSRRC:
+          return OP_CSRRC;
+          break;
+        case CSRRWI:
+          return OP_CSRRWI;
+          break;
+        case CSRRSI:
+          return OP_CSRRSI;
+          break;
+        case CSRRCI:
+          return OP_CSRRCI;
+          break;
+      }
+    }
     default:
       return OP_ERROR;
   }
 }
+
+
+extension_t Instruction::check_extension() {
+  if (m_instr.range(1,0) == 0b11) {
+    return BASE_EXTENSION;
+  } else if (m_instr.range(1,0) == 0b00) {
+    return C_EXTENSION;
+  } else if (m_instr.range(1,0) == 0b01) {
+    return C_EXTENSION;
+  } else if (m_instr.range(1,0) == 0b10) {
+    return C_EXTENSION;
+  } else if (m_instr.range(6,0) == 0b0110011) {
+    return M_EXTENSION;
+  } else if (m_instr.range(6,0) == 0b0101111) {
+    return A_EXTENSION;
+  } else {
+    return UNKNOWN_EXTENSION;
+  }
+}
diff --git a/src/Registers.cpp b/src/Registers.cpp
index caceb43..c5821c1 100644
--- a/src/Registers.cpp
+++ b/src/Registers.cpp
@@ -2,10 +2,12 @@
 
 Registers::Registers() {
 
-  memset(register_bank, 0, sizeof(int32_t)*32); // 32 registers of 32 bits each
+  memset(register_bank, 0, sizeof(uint32_t)*32); // 32 registers of 32 bits each
+  memset(CSR, 0, sizeof(uint32_t)*4096);
   perf = Performance::getInstance();
 
-  register_bank[sp] = 1024-1;  // SP points to end of memory
+  //register_bank[sp] = 1024-1;  // SP points to end of memory
+  register_bank[sp] = Memory::SIZE-4;
   register_PC = 0x10000;       // default _start address
 }
 
diff --git a/tests/C/func3/Makefile b/tests/C/func3/Makefile
new file mode 100644
index 0000000..d1ee44b
--- /dev/null
+++ b/tests/C/func3/Makefile
@@ -0,0 +1,52 @@
+TARGET   = func3
+
+TARGET_ARCH=riscv32
+
+CC       = riscv32-unknown-linux-gnu-gcc
+# compiling flags here
+#CFLAGS = -Wall -I. -O0 -nostdlib -march=rv32i -mabi=ilp32 --entry main 
+CFLAGS = -Wall -I. -O0 -static
+#CFLAGS = -Wall -I. -O0 -Xlinker --gc-sections -lgcc -lc -static --specs=nano.specs
+
+
+LINKER   = riscv32-unknown-linux-gnu-gcc
+# linking flags here
+LFLAGS   = -I. --entry main -L/opt/riscv/riscv32-unknown-elf/lib/
+LIBS   = $(EXTRA_LIBS)
+
+
+# change these to proper directories where each file should be
+SRCDIR   = ./
+OBJDIR   = .
+BINDIR   = ./
+INCDIR = -I.
+LIBDIR = -L.
+
+
+SOURCES  := $(wildcard $(SRCDIR)/*.c)
+INCLUDES := $(wildcard $(INCDIR)/*.h)
+OBJECTS  := $(SOURCES:$(SRCDIR)/%.c=$(OBJDIR)/%.o)
+rm       = rm -f
+
+
+$(BINDIR)/$(TARGET): $(OBJECTS)
+#	$(LINKER) $(OBJECTS) $(LFLAGS) $(LIBS) $(LIBDIR) -o $@
+	riscv32-unknown-linux-gnu-objdump -d $< > dump
+	objcopy -Oihex $< $(TARGET).hex
+#	@echo "Linking complete!"
+
+$(OBJECTS): $(OBJDIR)/%.o : $(SRCDIR)/%.c
+	@echo "Compiling "$<" ..."
+#	$(CC) $(CFLAGS) $(INCDIR) -c $< -o $@
+	$(CC) $(CFLAGS) $(INCDIR) $< -o $@
+	@echo "Done!"
+
+.PHONY: clean
+clean:
+	@$(rm) $(OBJECTS) *.hex dump
+	@echo "Cleanup complete!"
+
+.PHONY: remove
+remove: clean
+	@$(rm) $(BINDIR)/$(TARGET)
+	@echo "Executable removed!"
diff --git a/tests/C/func3/func3.c b/tests/C/func3/func3.c
new file mode 100644
index 0000000..b3e7b8d
--- /dev/null
+++ b/tests/C/func3/func3.c
@@ -0,0 +1,36 @@
+#include <string.h>
+
+#define TRACE (*(unsigned char *)0x40000000)
+
+void print(char *msg) {
+  int i = 0;
+  while(msg[i] != '\0') {
+    TRACE = msg[i];
+    i++;
+  }
+}
+
+int func1(int a, int* b) {
+  return a - (*b);
+}
+
+
+void main(void) {
+  int x1, x2, x3;
+  int aux[5] = {0};
+  int aux2[5];
+
+  x1 = 6;
+  x2 = 7;
+
+  x3 = func1(x1, &x2);
+
+  if (x3 == (6-7)) {
+    print("OK\n");
+  } else {
+    print("ERROR\n");
+  }
+
+
+  memcpy(aux, aux2, 5);
+}