diff --git a/inc/Registers.h b/inc/Registers.h
index 5f0d8bb..dcb6a06 100644
--- a/inc/Registers.h
+++ b/inc/Registers.h
@@ -197,7 +197,7 @@ public:
 	 * @param  reg_num register number
 	 * @return         register value
 	 */
-	uint32_t getValue(int reg_num);
+	uint32_t getValue(int reg_num) const;
 
 	/**
 	 * Returns PC value
@@ -220,9 +220,12 @@ public:
 		} else {
 			register_PC += 4;
 		}
-
 	}
 
+    inline void incPCby2() {
+        register_PC += 2;
+    }
+
 	/**
 	 * @brief Get CSR value
 	 * @param csr CSR number to access
@@ -255,10 +258,8 @@ private:
 	/**
 	 * CSR registers (4096 maximum)
 	 */
-	//uint32_t CSR[4096];
 	std::unordered_map<unsigned int, uint32_t> CSR{0};
 
-
 	Performance *perf;
 
 	void initCSR();
diff --git a/src/CPU.cpp b/src/CPU.cpp
index ed3f038..0925299 100644
--- a/src/CPU.cpp
+++ b/src/CPU.cpp
@@ -114,8 +114,6 @@ bool CPU::cpu_process_IRQ() {
 }
 
 bool CPU::CPU_step() {
-
-	bool incPCby2 = false;
 	bool PC_not_affected = false;
 
 	/* Get new PC value */
@@ -142,7 +140,6 @@ bool CPU::CPU_step() {
 	}
 
 	perf->codeMemoryRead();
-
 	log->SC_log(Log::INFO) << "PC: 0x" << std::hex << register_bank->getPC()
 			<< ". ";
 
@@ -153,20 +150,28 @@ bool CPU::CPU_step() {
 	switch (inst->check_extension()) {
 	[[likely]] case BASE_EXTENSION:
 		PC_not_affected = exec->process_instruction(inst, &breakpoint);
-		incPCby2 = false;
+        if (PC_not_affected) {
+            register_bank->incPC();
+        }
 		break;
 	case C_EXTENSION:
 		PC_not_affected = c_inst->process_instruction(inst, &breakpoint);
-		incPCby2 = true;
+        if (PC_not_affected) {
+            register_bank->incPCby2();
+        }
 		break;
 	case M_EXTENSION:
 		PC_not_affected = m_inst->process_instruction(inst);
-		incPCby2 = false;
+        if (PC_not_affected) {
+            register_bank->incPC();
+        }
 		break;
 	case A_EXTENSION:
 		PC_not_affected = a_inst->process_instruction(inst);
-		incPCby2 = false;
-		break;
+        if (PC_not_affected) {
+            register_bank->incPC();
+        }
+        break;
 	[[unlikely]] default:
 		std::cout << "Extension not implemented yet" << std::endl;
 		inst->dump();
@@ -179,10 +184,6 @@ bool CPU::CPU_step() {
 
 	perf->instructionsInc();
 
-	if (PC_not_affected) {
-		register_bank->incPC(incPCby2);
-	}
-
 	return breakpoint;
 }
 
diff --git a/src/C_extension.cpp b/src/C_extension.cpp
index cbd75bc..5b5d681 100644
--- a/src/C_extension.cpp
+++ b/src/C_extension.cpp
@@ -325,7 +325,7 @@ bool C_extension::Exec_C_BEQZ() {
 		new_pc = static_cast<int32_t>(regs->getPC()) + get_imm_CB();
 		regs->setPC(new_pc);
 	} else {
-		regs->incPC(true); //PC <- PC + 2
+		regs->incPCby2();
 		new_pc = static_cast<int32_t>(regs->getPC());
 	}
 
@@ -350,7 +350,7 @@ bool C_extension::Exec_C_BNEZ() {
 		new_pc = static_cast<int32_t>(regs->getPC()) + get_imm_CB();
 		regs->setPC(new_pc);
 	} else {
-		regs->incPC(true); //PC <- PC +2
+		regs->incPCby2(); //PC <- PC +2
 		new_pc = static_cast<int32_t>(regs->getPC());
 	}
 
diff --git a/src/Registers.cpp b/src/Registers.cpp
index 94e9d77..ce46e5e 100644
--- a/src/Registers.cpp
+++ b/src/Registers.cpp
@@ -9,14 +9,9 @@
 #include "Registers.h"
 
 Registers::Registers() {
-
 	perf = Performance::getInstance();
 
 	initCSR();
-
-	//std::cout << "Memory size: 0x" << std::hex << Memory::SIZE << std::endl;
-	//std::cout << "SP address: 0x" << std::hex << (0x10000000 / 4) - 1 << std::endl;
-
 	register_bank[sp] = Memory::SIZE - 4; // default stack at the end of the memory
 	register_PC = 0x80000000;       // default _start address
 }
@@ -108,7 +103,7 @@ void Registers::setValue(int reg_num, int32_t value) {
 	}
 }
 
-uint32_t Registers::getValue(int reg_num) {
+uint32_t Registers::getValue(int reg_num) const {
 	if ((reg_num >= 0) && (reg_num < 32)) {
 		perf->registerRead();
 		return register_bank[reg_num];
@@ -125,7 +120,7 @@ void Registers::setPC(uint32_t new_pc) {
 	register_PC = new_pc;
 }
 
-uint32_t Registers::getCSR(int csr) {
+uint32_t Registers::getCSR(const int csr)  {
 	uint32_t ret_value;
 
 	switch (csr) {
@@ -156,14 +151,14 @@ uint32_t Registers::getCSR(int csr) {
 				>> 32 & 0x00000000FFFFFFFF);
 		break;
 	[[likely]] default:
-		ret_value = CSR[csr];
+        ret_value = CSR[csr];
 		break;
 	}
 	return ret_value;
 }
 
 void Registers::setCSR(int csr, uint32_t value) {
-	/* @FIXME: rv32mi-p-ma_fetch tests doesn't allow MISA to writable,
+	/* @FIXME: rv32mi-p-ma_fetch tests doesn't allow MISA to be writable,
 	 * but Volume II: Privileged Architecture v1.10 says MISA is writable (?)
 	 */
 	if (csr != CSR_MISA) {