Google Git
Sign in
foss-eda-tools/third_party/shuttle/sky130/mpw-006/slot-032/a4827b322c80b1c40ddb4e969a1f58c7d30aa254^!/.
commit
a4827b322c80b1c40ddb4e969a1f58c7d30aa254
[log]
author
Charlie <charlie.david.smith@hotmail.co.uk>
Thu Jun 02 12:54:49 2022 +0100
committer
Charlie <charlie.david.smith@hotmail.co.uk>
Thu Jun 02 12:54:49 2022 +0100
tree
70ffcf21c880981268dd6d465d4a6c0c83606ca4
parent
e78d21187306967e0eaef7a1f77970daf925a0d0 [diff]
Added trap handling to core.

diff --git a/docs/Design/MemoryMap.txt b/docs/Design/MemoryMap.txt
index 81fff28..aa237f4 100644
--- a/docs/Design/MemoryMap.txt
+++ b/docs/Design/MemoryMap.txt

@@ -12,6 +12,7 @@
 				0x1081_0004: Relative Jump
 				0x1081_0008: Step
 				0x1081_0010: Current Instruction
+				0x1081_0020: Clear error
 				0x1081_1000-0x1081_101F: Register
 				0x1081_2000-0x1081_2FFF: CSRs			
 	0x11xx_xxxx: Slave 1 (Core 1 local memory)
@@ -25,6 +26,7 @@
 				0x1181_0004: Relative Jump
 				0x1181_0008: Step
 				0x1181_0010: Current Instruction
+				0x1181_0020: Clear error
 				0x1181_1000-0x1181_101F: Register
 				0x1181_2000-0x1181_2FFF: CSRs		
 	0x12xx_xxxx: Slave 2 (Video SRAM)

diff --git a/verilog/dv/coreArch/Makefile b/verilog/dv/coreArch/Makefile
index 2e9a0b1..0d2b4a8 100644
--- a/verilog/dv/coreArch/Makefile
+++ b/verilog/dv/coreArch/Makefile

@@ -36,7 +36,7 @@
 
 SIGNATURE_START=$(shell readelf -s coreArch.elf | grep begin_signature | awk '{print $$2}')
 SIGNATURE_END=$(shell readelf -s coreArch.elf | grep end_signature | awk '{print $$2}')
-CODE_END=$(shell readelf -s coreArch.elf | grep rvtest_code_end | awk '{print $$2}')
+CODE_END=$(shell readelf -s coreArch.elf | grep write_tohost | awk '{print $$2}')
 
 .SUFFIXES:
 all:  ${BLOCKS:=.lst} ${BLOCKS:=.disass} ${BLOCKS:=.vcd}

diff --git a/verilog/dv/coreArch/coreArch_tb.v b/verilog/dv/coreArch/coreArch_tb.v
index db77d9b..c83f4aa 100644
--- a/verilog/dv/coreArch/coreArch_tb.v
+++ b/verilog/dv/coreArch/coreArch_tb.v

@@ -32,16 +32,21 @@
 	wire memoryBusy = 1'b0;
 	
 	wire management_run = 1'b1;
+	wire management_trapEnable = 1'b1;
 	wire management_writeEnable = 1'b0;
 	wire[3:0] management_byteSelect = 4'b0000;
 	wire[15:0] management_address = 'b0;
 	wire[31:0] management_writeData = 'b0;
 	wire[31:0] management_readData;
 
+	wire eCall;
+	wire eBreak;
+
 	wire[1:0] probe_state;
+	wire[1:0] probe_env;
 	wire[31:0] probe_programCounter;
 	wire[6:0] probe_opcode;
-	wire[3:0] probe_errorCode;
+	wire[1:0] probe_errorCode;
 	wire probe_isBranch;
 	wire probe_takeBranch;
 	wire probe_isStore;
@@ -66,6 +71,9 @@
 		$display("CODE_END: 0x%h", 32'h`CODE_END);
 
 		wait((probe_programCounter == 32'h`CODE_END) || (|probe_errorCode));
+		//wait(eBreak || (|probe_errorCode));
+
+		#50
 
 		if (|probe_errorCode) begin
 			$display("%c[1;31m",27);
@@ -162,6 +170,8 @@
 	assign VDD1V8 = power2;
 	assign VSS = 1'b0;
 
+	localparam CORE_EXTENSIONS = 26'b00_0000_0000_0000_0001_0000_0000;
+
 	RV32ICore core(
 		.vccd1 (VDD1V8),	// User area 1 1.8V supply
 		.vssd1 (VSS),		// User area 1 digital ground
@@ -175,12 +185,21 @@
 		.memoryDataRead(memoryDataRead),
 		.memoryBusy(memoryBusy),
 		.management_run(management_run),
+		.management_trapEnable(management_trapEnable),
 		.management_writeEnable(management_writeEnable),
 		.management_byteSelect(management_byteSelect),
 		.management_address(management_address),
 		.management_writeData(management_writeData),
 		.management_readData(management_readData),
+		.coreIndex(8'h00),
+		.manufacturerID(11'h000),
+		.partID(16'hCD55),
+		.versionID(4'h0),
+		.extensions(CORE_EXTENSIONS),
+		.eCall(eCall),
+		.eBreak(eBreak),
 		.probe_state(probe_state),
+		.probe_env(probe_env),
 		.probe_programCounter(probe_programCounter),
 		.probe_opcode(probe_opcode),
 		.probe_errorCode(probe_errorCode),

diff --git a/verilog/dv/coreArch/riscof/ExperiarSoC/ExperiarSoC_isa.yaml b/verilog/dv/coreArch/riscof/ExperiarSoC/ExperiarSoC_isa.yaml
index c985fd4..432e02d 100644
--- a/verilog/dv/coreArch/riscof/ExperiarSoC/ExperiarSoC_isa.yaml
+++ b/verilog/dv/coreArch/riscof/ExperiarSoC/ExperiarSoC_isa.yaml

@@ -1,26 +1,27 @@
 hart_ids: [0]
 hart0:
-  ISA: RV32IZicsr
+  ISA: RV32I_Zicsr
   physical_addr_sz: 32
   User_Spec_Version: '2.3'
   supported_xlen: [32]
+  hw_data_misaligned_support: false
   misa:
-   reset-val: 0x40000100
-   rv32:
-     accessible: true
-     mxl:
-       implemented: true
-       type:
-           warl:
+    reset-val: 0x40000100
+    rv32:
+      accessible: true
+      mxl:
+        implemented: true
+        type:
+            warl:
               dependency_fields: []
               legal:
                 - mxl[1:0] in [0x1]
               wr_illegal:
                 - Unchanged
-     extensions:
-       implemented: true
-       type:
-           warl:
+      extensions:
+        implemented: true
+        type:
+            warl:
               dependency_fields: []
               legal:
                 - extensions[25:0] bitmask [0x0001104, 0x0000000]

diff --git a/verilog/dv/coreArch/riscof/ExperiarSoC/env/model_test.h b/verilog/dv/coreArch/riscof/ExperiarSoC/env/model_test.h
index a55076e..d7be858 100644
--- a/verilog/dv/coreArch/riscof/ExperiarSoC/env/model_test.h
+++ b/verilog/dv/coreArch/riscof/ExperiarSoC/env/model_test.h

@@ -11,11 +11,11 @@
         .word 4;
 
 //RV_COMPLIANCE_HALT
-#define RVMODEL_HALT                                              \
-  li x1, 1;                                                                   \
-  write_tohost:                                                               \
-    sw x1, tohost, t5;                                                        \
-    j write_tohost;
+#define RVMODEL_HALT   \
+	li x1, 1;          \
+	write_tohost:      \
+	sw x1, tohost, t5; \
+	j write_tohost;
 
 #define RVMODEL_BOOT
 

diff --git a/verilog/dv/coreArch/runArchTest.py b/verilog/dv/coreArch/runArchTest.py
index eb7f177..0046f5a 100644
--- a/verilog/dv/coreArch/runArchTest.py
+++ b/verilog/dv/coreArch/runArchTest.py

@@ -10,17 +10,21 @@
 	process.wait()
 	if process.stdout != None:
 		for line in process.stdout:
-			print(str(line).replace("\\n", ""))
+			print(line.decode("utf-8"), end="")
 	return process.returncode
 
 def RunArchTest(elfFilePath:str, signatureFilePath:str):
 	# Run the simulator
 	print(f"Running simulation")
-	makeFilePath = os.path.dirname(os.path.abspath(__file__))
-	shutil.copy2(os.path.join(makeFilePath, "coreArch_tb.v"), os.getcwd())
-	shutil.copy2(os.path.join(makeFilePath, "Makefile"), os.getcwd())
-	RunShellCommand(["make"])
 
+	try:
+		makeFilePath = os.path.dirname(os.path.abspath(__file__))
+		shutil.copy2(os.path.join(makeFilePath, "coreArch_tb.v"), os.getcwd())
+		shutil.copy2(os.path.join(makeFilePath, "Makefile"), os.getcwd())
+		RunShellCommand(["make"])
+	except Exception as e:
+		print(e)
+		
 	print(f"Simulation completed")
 
 def main():

diff --git a/verilog/includes/includes.rtl.caravel_user_project b/verilog/includes/includes.rtl.caravel_user_project
index 66bdffb..5a9380f 100644
--- a/verilog/includes/includes.rtl.caravel_user_project
+++ b/verilog/includes/includes.rtl.caravel_user_project

@@ -12,6 +12,7 @@
 -v $(USER_PROJECT_VERILOG)/rtl/ExperiarCore/CSR/CSR_DataRegister.v 
 -v $(USER_PROJECT_VERILOG)/rtl/ExperiarCore/CSR/CSR_ReadRegister.v 
 -v $(USER_PROJECT_VERILOG)/rtl/ExperiarCore/CSR/CSR_TimerRegister.v 
+-v $(USER_PROJECT_VERILOG)/rtl/ExperiarCore/CSR/Traps/Traps.v 
 -v $(USER_PROJECT_VERILOG)/rtl/ExperiarCore/JTAG.v 
 -v $(USER_PROJECT_VERILOG)/rtl/ExperiarCore/JTAGRegister.v 
 -v $(USER_PROJECT_VERILOG)/rtl/ExperiarCore/Memory/LocalMemoryInterface.v 

diff --git a/verilog/rtl/ExperiarCore/CSR/CSR.v b/verilog/rtl/ExperiarCore/CSR/CSR.v
index 0e714d9..3003313 100644
--- a/verilog/rtl/ExperiarCore/CSR/CSR.v
+++ b/verilog/rtl/ExperiarCore/CSR/CSR.v

@@ -15,7 +15,28 @@
 		input wire[15:0] partID,
 		input wire[3:0] versionID,
 		input wire[25:0] extensions,
-		input wire instructionCompleted
+		input wire instructionCompleted,
+
+		// System trap interface
+		input wire[1:0] coreState,
+		input wire[31:0] programCounter,
+		input wire[31:0] currentInstruction,
+		input wire isLoad,
+		input wire isStore,
+		input wire isMachineTimerInterrupt,
+		input wire isMachineExternalInterrupt,
+		input wire isMachineSoftwareInterrupt,
+		input wire isAddressMisaligned,
+		input wire isAccessFault,
+		input wire isInvalidInstruction,
+		input wire isEBREAK,
+		input wire isECALL,
+		input wire isAddressBreakpoint,
+		input wire[15:0] userInterrupts,
+		input wire trapReturn,
+		output wire inTrap,
+		output wire[31:0] trapVector,
+		output wire[31:0] trapReturnVector
 	);
 
 	// Cylce
@@ -128,6 +149,38 @@
 		.csrRequestOutput(misaRequestOutput),
 		.value({ 2'b01, 4'b0, extensions }));
 
+	// Trap handling
+	wire[31:0] trapsReadData;
+	wire trapsRequestOutput;
+	Traps traps(
+		.clk(clk),
+		.rst(rst),
+		.csrWriteEnable(csrWriteEnable),
+		.csrReadEnable(csrReadEnable),
+		.csrAddress(csrAddress),
+		.csrWriteData(csrWriteData),
+		.csrReadData(trapsReadData),
+		.requestOutput(trapsRequestOutput),
+		.coreState(coreState),
+		.programCounter(programCounter),
+		.currentInstruction(currentInstruction),
+		.isLoad(isLoad),
+		.isStore(isStore),
+		.isMachineTimerInterrupt(isMachineTimerInterrupt),
+		.isMachineExternalInterrupt(isMachineExternalInterrupt),
+		.isMachineSoftwareInterrupt(isMachineSoftwareInterrupt),
+		.isAddressMisaligned(isAddressMisaligned),
+		.isAccessFault(isAccessFault),
+		.isInvalidInstruction(isInvalidInstruction),
+		.isEBREAK(isEBREAK),
+		.isECALL(isECALL),
+		.isAddressBreakpoint(isAddressBreakpoint),
+		.userInterrupts(userInterrupts),
+		.trapReturn(trapReturn),
+		.inTrap(inTrap),
+		.trapVector(trapVector),
+		.trapReturnVector(trapReturnVector));
+
 	always @(*) begin
 		case (1'b1)
 			// Timers
@@ -143,6 +196,9 @@
 			coreIDRequestOutput: csrReadData <= coreIDReadData;
 			misaRequestOutput: csrReadData <= misaReadData;
 
+			// Traps
+			trapsRequestOutput: csrReadData <= trapsReadData;
+
 			default: csrReadData <= 32'b0;
 		endcase
 	end

diff --git a/verilog/rtl/ExperiarCore/CSR/CSR_ConfigurationRegister.v b/verilog/rtl/ExperiarCore/CSR/CSR_ConfigurationRegister.v
index 973c079..8b77762 100644
--- a/verilog/rtl/ExperiarCore/CSR/CSR_ConfigurationRegister.v
+++ b/verilog/rtl/ExperiarCore/CSR/CSR_ConfigurationRegister.v

@@ -10,7 +10,7 @@
 		input wire csrReadEnable,
 		input wire[11:0] csrAddress,
 		input wire[31:0] csrWriteData,
-		output reg[31:0] csrReadData,
+		output wire[31:0] csrReadData,
 		output wire csrRequestOutput,
 
 		// System interface

diff --git a/verilog/rtl/ExperiarCore/CSR/CSR_DataRegister.v b/verilog/rtl/ExperiarCore/CSR/CSR_DataRegister.v
index e69de29..8cfce0b 100644
--- a/verilog/rtl/ExperiarCore/CSR/CSR_DataRegister.v
+++ b/verilog/rtl/ExperiarCore/CSR/CSR_DataRegister.v

@@ -0,0 +1,31 @@
+module CSR_DataRegister #(
+		parameter ADDRESS = 12'h000
+	)(
+		input wire clk,
+		input wire rst,
+
+		// CSR interface
+		input wire csrWriteEnable,
+		input wire csrReadEnable,
+		input wire[11:0] csrAddress,
+		input wire[31:0] csrWriteData,
+		output wire[31:0] csrReadData,
+		output wire csrRequestOutput,
+
+		// System interface
+		input wire[31:0] readData,
+		output wire readDataEnable,
+		output wire[31:0] writeData,
+		output wire writeDataEnable
+	);
+	
+	wire csrEnabled = csrAddress == ADDRESS;
+
+	assign csrReadData = csrEnabled && csrReadEnable ? readData : 32'b0;
+	assign readDataEnable = csrEnabled && csrReadEnable;
+	assign writeData = csrWriteData;
+	assign writeDataEnable = csrEnabled && csrWriteEnable;
+
+	assign csrRequestOutput = csrEnabled && csrReadEnable;
+	
+endmodule
\ No newline at end of file

diff --git a/verilog/rtl/ExperiarCore/CSR/Traps/Traps.v b/verilog/rtl/ExperiarCore/CSR/Traps/Traps.v
new file mode 100644
index 0000000..b116d6c
--- /dev/null
+++ b/verilog/rtl/ExperiarCore/CSR/Traps/Traps.v

@@ -0,0 +1,347 @@
+module Traps (
+		input wire clk,
+		input wire rst,
+
+		// CSR interface
+		input wire csrWriteEnable,
+		input wire csrReadEnable,
+		input wire[11:0] csrAddress,
+		input wire[31:0] csrWriteData,
+		output reg[31:0] csrReadData,
+		output wire requestOutput,
+
+		// System interface
+		input wire[1:0] coreState,
+		input wire[31:0] programCounter,
+		input wire[31:0] currentInstruction,
+		input wire isLoad,
+		input wire isStore,
+		input wire isMachineTimerInterrupt,
+		input wire isMachineExternalInterrupt,
+		input wire isMachineSoftwareInterrupt,
+		input wire isAddressMisaligned,
+		input wire isAccessFault,
+		input wire isInvalidInstruction,
+		input wire isEBREAK,
+		input wire isECALL,
+		input wire isAddressBreakpoint,
+		input wire[15:0] userInterrupts,
+		input wire trapReturn,
+		output wire inTrap,
+		output wire[31:0] trapVector,
+		output wire[31:0] trapReturnVector
+	);
+	
+	localparam CORE_STATE_HALT 	 	 = 2'b00;
+	localparam CORE_STATE_FETCH   	 = 2'b10;
+	localparam CORE_STATE_EXECUTE 	 = 2'b11;
+
+	reg machineInterruptEnable;
+	reg machinePreviousInterruptEnable;
+	wire[31:0] mieValue;
+	reg[31:0] mtvecValue;
+	reg[31:0] mepcValue;
+	reg[31:0] mcauseValue;
+	reg[31:0] mtvalValue;
+	reg[31:0] mipValue;
+
+	wire[11:0] systemInterrupts = { isMachineExternalInterrupt, 1'b0, 1'b0, 1'b0, 
+									isMachineTimerInterrupt, 1'b0, 1'b0, 1'b0, 
+									isMachineSoftwareInterrupt, 1'b0, 1'b0, 1'b0 };
+	wire[31:0] pendingInterrupts = { userInterrupts, 4'b0000, systemInterrupts } & mieValue;
+
+	wire misalignedInstructionFetch = isAddressMisaligned && (coreState == CORE_STATE_FETCH);
+
+	reg[30:0] trapCause;
+	always @(*) begin
+		if (isInterrupt) begin
+			case (1'b1)
+				isMachineSoftwareInterrupt: trapCause <= 30'd3;
+				isMachineTimerInterrupt: trapCause <= 30'd7;
+				|userInterrupts: trapCause <= 30'd8;
+				isMachineExternalInterrupt: trapCause <= 30'd11;
+				default: trapCause <= 30'b0;
+			endcase
+		end else begin
+			case (1'b1)
+				isAddressBreakpoint && (coreState == CORE_STATE_FETCH): trapCause <= 30'd3;
+				isAccessFault && (coreState == CORE_STATE_FETCH): trapCause <= 30'd1;
+				isInvalidInstruction: trapCause <= 30'd2;
+				misalignedInstructionFetch: trapCause <= 30'd0;
+				isECALL: trapCause <= 30'd11;
+				isEBREAK: trapCause <= 30'd3;
+				isAddressBreakpoint && (coreState == CORE_STATE_EXECUTE) && (isLoad || isStore): trapCause <= 30'd3;
+				isAddressMisaligned && (coreState == CORE_STATE_EXECUTE) && isStore: trapCause <= 30'd6;
+				isAddressMisaligned && (coreState == CORE_STATE_EXECUTE) && isLoad: trapCause <= 30'd4;
+				isAccessFault && (coreState == CORE_STATE_EXECUTE) && isStore: trapCause <= 30'd7;
+				isAccessFault && (coreState == CORE_STATE_EXECUTE) && isLoad: trapCause <= 30'd5;				
+				default: trapCause <= 30'b0;
+			endcase
+		end		
+	end
+
+	// Misaligned instruction fetch sets trap cause to zero, so needs to be triggered specifically
+	wire isException = |trapCause || misalignedInstructionFetch;
+	wire isInterrupt = |pendingInterrupts;
+	wire isBreakPoint = isEBREAK || isAddressBreakpoint;
+	wire isTrap = isException || isInterrupt;
+
+	assign inTrap = isTrap;
+
+	wire[31:0] mipLoadValue = machineInterruptEnable ? pendingInterrupts : 1'b0;
+
+	reg[31:0] mtvalLoadValue;
+	always @(*) begin
+		 if (isTrap) begin
+			 case (1'b1)
+				 isBreakPoint: mtvalLoadValue <= programCounter;
+				 isAddressMisaligned: mtvalLoadValue <= programCounter;
+				 isInvalidInstruction: mtvalLoadValue <= currentInstruction;
+				 default: mtvalLoadValue <= 32'b0;
+			 endcase			
+		 end else begin
+			 mtvalLoadValue <= 32'b0;
+		 end 
+	end
+
+	// mstatus
+	wire[31:0] mstatusReadData;
+	wire mstatusRequestOutput;
+	wire mstatusReadDataEnable_nc;
+	wire[31:0] mstatusWriteData;
+	wire mstatusWriteDataEnable;
+	CSR_DataRegister #(.ADDRESS(12'h300)) mstatus(
+		.clk(clk),
+		.rst(rst),
+		.csrWriteEnable(csrWriteEnable),
+		.csrReadEnable(csrReadEnable),
+		.csrAddress(csrAddress),
+		.csrWriteData(csrWriteData),
+		.csrReadData(mstatusReadData),
+		.csrRequestOutput(mstatusRequestOutput),
+		.readData( { 25'b0, machinePreviousInterruptEnable, 3'b0, machineInterruptEnable, 2'b0 } ),
+		.readDataEnable(mstatusReadDataEnable_nc),
+		.writeData(mstatusWriteData),
+		.writeDataEnable(mstatusWriteDataEnable));
+
+	always @(posedge clk) begin
+		if (rst) begin
+			machineInterruptEnable <= 1'b0;
+			machinePreviousInterruptEnable <= 1'b0;
+		end
+		else begin
+			if (trapReturn) begin
+				machineInterruptEnable <= machinePreviousInterruptEnable;
+				machinePreviousInterruptEnable <= 1'b0;
+			end else if (isTrap) begin
+				machineInterruptEnable <= 1'b0;
+				machinePreviousInterruptEnable <= machineInterruptEnable;
+			end else if (mstatusWriteDataEnable) begin
+				machineInterruptEnable <=  mstatusWriteData[3];
+				machinePreviousInterruptEnable <=  mstatusWriteData[7];
+			end
+		end
+	end
+
+	// mie
+	wire[31:0] mieReadData;
+	wire mieRequestOutput;
+	CSR_ConfigurationRegister #(.ADDRESS(12'h304), .DEFAULT(32'b0)) mie(
+		.clk(clk),
+		.rst(rst),
+		.csrWriteEnable(csrWriteEnable),
+		.csrReadEnable(csrReadEnable),
+		.csrAddress(csrAddress),
+		.csrWriteData(csrWriteData),
+		.csrReadData(mieReadData),
+		.csrRequestOutput(mieRequestOutput),
+		.value(mieValue));
+
+	// mtvec
+	// In theory this is a WARL register, so only legal values can be read,
+	// This means that some bits should always be zero, but 
+	wire[31:0] mtvecValueValid = mtvecValue;//{ mtvecValue[31:4] , 2'b00, 1'b0, mtvecValue[0] }; 
+	wire mtvecRequestOutput;
+	wire[31:0] mtvecReadData;
+	wire mtvecReadDataEnable_nc;
+	wire[31:0] mtvecWriteData;
+	wire mtvecWriteDataEnable;
+	CSR_DataRegister #(.ADDRESS(12'h305)) mtvec(
+		.clk(clk),
+		.rst(rst),
+		.csrWriteEnable(csrWriteEnable),
+		.csrReadEnable(csrReadEnable),
+		.csrAddress(csrAddress),
+		.csrWriteData(csrWriteData),
+		.csrReadData(mtvecReadData),
+		.csrRequestOutput(mtvecRequestOutput),
+		.readData(mtvecValueValid),
+		.readDataEnable(mtvecReadDataEnable_nc),
+		.writeData(mtvecWriteData),
+		.writeDataEnable(mtvecWriteDataEnable));
+
+	always @(posedge clk) begin
+		if (rst) mtvecValue <= 32'b0;
+		else begin
+			if (mtvecWriteDataEnable) mtvecValue <= mtvecWriteData;
+		end
+	end
+
+	wire[31:0] trapVectorBase = { mtvecValueValid[31:2], 2'b00 };
+	wire[1:0] trapVectorMode = mtvecValueValid[1:0];
+
+	assign trapVector = (trapVectorMode == 2'b01) && isInterrupt ? trapVectorBase + {mcauseValue[29:0], 2'b00} : trapVectorBase;
+
+	// mscratch
+	wire[31:0] mscratchReadData;
+	wire mscratchRequestOutput;
+	wire[31:0] mscratchValue_nc;
+	CSR_ConfigurationRegister #(.ADDRESS(12'h340), .DEFAULT(32'b0)) mscratch (
+		.clk(clk),
+		.rst(rst),
+		.csrWriteEnable(csrWriteEnable),
+		.csrReadEnable(csrReadEnable),
+		.csrAddress(csrAddress),
+		.csrWriteData(csrWriteData),
+		.csrReadData(mscratchReadData),
+		.csrRequestOutput(mscratchRequestOutput),
+		.value(mscratchValue_nc));
+
+	// mepc
+	wire[31:0] mepcReadData;
+	wire mepcRequestOutput;
+	wire mepcReadDataEnable_nc;
+	wire[31:0] mepcWriteData;
+	wire mepcWriteDataEnable;
+	CSR_DataRegister #(.ADDRESS(12'h341)) mepc(
+		.clk(clk),
+		.rst(rst),
+		.csrWriteEnable(csrWriteEnable),
+		.csrReadEnable(csrReadEnable),
+		.csrAddress(csrAddress),
+		.csrWriteData(csrWriteData),
+		.csrReadData(mepcReadData),
+		.csrRequestOutput(mepcRequestOutput),
+		.readData({ mepcValue[31:1], 1'b0 }),
+		.readDataEnable(mepcReadDataEnable_nc),
+		.writeData(mepcWriteData),
+		.writeDataEnable(mepcWriteDataEnable));
+
+	always @(posedge clk) begin
+		if (rst) mepcValue <= 32'b0;
+		else begin
+			if (isTrap) mepcValue <= programCounter;
+			else if (mepcWriteDataEnable) mepcValue <= mepcWriteData;
+		end
+	end
+
+	assign trapReturnVector = mepcValue;
+
+	// mcause
+	wire[31:0] mcauseReadData;
+	wire mcauseRequestOutput;
+	wire mcauseReadDataEnable_nc;
+	wire[31:0] mcauseWriteData;
+	wire mcauseWriteDataEnable;
+	CSR_DataRegister #(.ADDRESS(12'h342)) mcause(
+		.clk(clk),
+		.rst(rst),
+		.csrWriteEnable(csrWriteEnable),
+		.csrReadEnable(csrReadEnable),
+		.csrAddress(csrAddress),
+		.csrWriteData(csrWriteData),
+		.csrReadData(mcauseReadData),
+		.csrRequestOutput(mcauseRequestOutput),
+		.readData(mcauseValue),
+		.readDataEnable(mcauseReadDataEnable_nc),
+		.writeData(mcauseWriteData),
+		.writeDataEnable(mcauseWriteDataEnable));
+
+	always @(posedge clk) begin
+		if (rst) mcauseValue <= 32'b0;
+		else begin
+			if (isTrap) mcauseValue <= { isInterrupt, trapCause };
+			else if (mcauseWriteDataEnable) mcauseValue <= mcauseWriteData;
+		end
+	end
+
+	// mtval
+	wire[31:0] mtvalReadData;
+	wire mtvalRequestOutput;
+	wire mtvalReadDataEnable_nc;
+	wire[31:0] mtvalWriteData;
+	wire mtvalWriteDataEnable;
+	CSR_DataRegister #(.ADDRESS(12'h343)) mtval(
+		.clk(clk),
+		.rst(rst),
+		.csrWriteEnable(csrWriteEnable),
+		.csrReadEnable(csrReadEnable),
+		.csrAddress(csrAddress),
+		.csrWriteData(csrWriteData),
+		.csrReadData(mtvalReadData),
+		.csrRequestOutput(mtvalRequestOutput),
+		.readData(mtvalValue),
+		.readDataEnable(mtvalReadDataEnable_nc),
+		.writeData(mtvalWriteData),
+		.writeDataEnable(mtvalWriteDataEnable));
+
+	always @(posedge clk) begin
+		if (rst) mtvalValue <= 32'b0;
+		else begin
+			if (isTrap) mtvalValue <= mtvalLoadValue;
+			else if (mtvalWriteDataEnable) mtvalValue <= mtvalWriteData;
+		end
+	end
+
+	// mip
+	wire[31:0] mipReadData;
+	wire mipRequestOutput;
+	wire mipReadDataEnable_nc;
+	wire[31:0] mipWriteData;
+	wire mipWriteDataEnable;
+	CSR_DataRegister #(.ADDRESS(12'h344)) mip(
+		.clk(clk),
+		.rst(rst),
+		.csrWriteEnable(csrWriteEnable),
+		.csrReadEnable(csrReadEnable),
+		.csrAddress(csrAddress),
+		.csrWriteData(csrWriteData),
+		.csrReadData(mipReadData),
+		.csrRequestOutput(mipRequestOutput),
+		.readData(mipValue),
+		.readDataEnable(mipReadDataEnable_nc),
+		.writeData(mipWriteData),
+		.writeDataEnable(mipWriteDataEnable));
+
+	always @(posedge clk) begin
+		if (rst) mipValue <= 32'b0;
+		else begin
+			if (isTrap && isInterrupt) mipValue <= mipLoadValue;
+			else if (mipWriteDataEnable) mipValue <= mipWriteData;
+		end
+	end
+
+	assign requestOutput = mstatusRequestOutput 
+						|| mieRequestOutput 
+						|| mtvecRequestOutput 
+						|| mscratchRequestOutput 
+						|| mepcRequestOutput
+						|| mcauseRequestOutput
+						|| mtvalRequestOutput
+						|| mipRequestOutput;
+
+	always @(*) begin
+		case (1'b1)
+			mstatusRequestOutput: csrReadData <= mstatusReadData;
+			mieRequestOutput: csrReadData <= mieReadData;
+			mtvecRequestOutput: csrReadData <= mtvecReadData;
+			mscratchRequestOutput: csrReadData <= mscratchReadData;
+			mepcRequestOutput: csrReadData <= mepcReadData;
+			mcauseRequestOutput: csrReadData <= mcauseReadData;
+			mtvalRequestOutput: csrReadData <= mtvalReadData;
+			mipRequestOutput: csrReadData <= mipReadData;
+			default: csrReadData <= 32'b0;
+		endcase
+	end
+
+endmodule
\ No newline at end of file

diff --git a/verilog/rtl/ExperiarCore/CoreManagement.v b/verilog/rtl/ExperiarCore/CoreManagement.v
index 4130065..6b1748b 100644
--- a/verilog/rtl/ExperiarCore/CoreManagement.v
+++ b/verilog/rtl/ExperiarCore/CoreManagement.v

@@ -4,6 +4,7 @@
 
 		// Interface to core
 		output wire management_run,
+		output wire management_trapEnable,
 		output wire management_writeEnable,
 		output wire[3:0] management_byteSelect,
 		output wire[15:0] management_address,
@@ -11,7 +12,12 @@
 		input wire[31:0] management_readData,
 
 		// Core state
-		input wire[3:0] core_errorCode,
+		input wire[1:0] core_env,
+		input wire[1:0] core_errorCode,
+
+		// Address breakpoint
+		output wire isAddressBreakpoint,
+		input wire[31:0] coreAddress,
 
 		// Interface from jtag
 		input wire jtag_management_writeEnable,
@@ -31,6 +37,10 @@
 		output wire wb_management_busy
 	);
 
+	// TODO instruction breakpoints
+	assign isAddressBreakpoint = 0;
+	//assign isAddressBreakpoint = coreAddress == instructionBreakpointAddress;
+
 	// Master select
 	wire jtagSelect = jtag_management_writeEnable || jtag_management_readEnable;
 	wire wbRequest = wb_management_writeEnable || wb_management_readEnable;
@@ -55,11 +65,12 @@
 
 	// Registers
 	// Control register: Default 0x0
-	// b00: Run
-	wire control;
+	// b00: run
+	// b01: trapEnable
+	wire[1:0] control;
 	wire[31:0] controlOutputData;
 	wire controlOutputRequest;
-	ConfigurationRegister #(.WIDTH(1), .ADDRESS(12'h000), .DEFAULT(1'b0)) controlRegister(
+	ConfigurationRegister #(.WIDTH(2), .ADDRESS(12'h000), .DEFAULT(2'b0)) controlRegister(
 		.clk(clk),
 		.rst(rst),
 		.enable(registerEnable),
@@ -96,12 +107,13 @@
 		.writeData(stateWriteData_nc),
 		.writeData_en(stateWriteDataEnable_nc),
 		.writeData_busy(1'b0),
-		.readData({ management_run, core_errorCode }),
+		.readData({ management_run, core_env, core_errorCode }),
 		.readData_en(stateReadDataEnable_nc),
 		.readData_busy(1'b0));
 
 	// Core
-	assign management_run = control;
+	assign management_run = control[0];
+	assign management_trapEnable = control[1];
 	assign management_writeEnable = coreEnable && peripheralBus_we;
 	assign management_byteSelect = peripheralBus_byteSelect;
 	assign management_address = peripheralBus_address[15:0];

diff --git a/verilog/rtl/ExperiarCore/ExperiarCore_top.v b/verilog/rtl/ExperiarCore/ExperiarCore_top.v
index 12ea0d4..aaa6519 100644
--- a/verilog/rtl/ExperiarCore/ExperiarCore_top.v
+++ b/verilog/rtl/ExperiarCore/ExperiarCore_top.v

@@ -18,6 +18,9 @@
 		input wire jtag_tdi,
 		output wire jtag_tdo,
 
+		// Interrupts
+		//input wire[15:0] userInterrupts,
+
 		// Wishbone master interface from core
 		output wire core_wb_cyc_o,
 		output wire core_wb_stb_o,
@@ -59,9 +62,10 @@
 
 		// Logic probes
 		output wire[1:0] probe_state,
+		output wire[1:0] probe_env,
 		output wire[31:0] probe_programCounter,
 		output wire[6:0] probe_opcode,
-		output wire[3:0] probe_errorCode,
+		output wire[1:0] probe_errorCode,
 		output wire probe_isBranch,
 		output wire probe_takeBranch,
 		output wire probe_isStore,
@@ -96,7 +100,46 @@
 	localparam CORE_EXTENSIONS_SHORT = 14'b00_0001_0000_0000;
 	localparam CORE_VERSION = 8'h00;
 
+	// Core
+	// Memory interface from core
+	wire coreMemoryWriteEnable;
+	wire coreMemoryReadEnable;
+	wire[31:0] coreMemoryAddress;
+	wire[3:0] coreMemoryByteSelect;
+	wire[31:0] coreMemoryDataWrite;
+	wire[31:0] coreMemoryDataRead;
+	wire coreMemoryBusy;
+
+	// Memory interface from core to local memory
+	wire coreLocalMemoryWriteEnable;
+	wire coreLocalMemoryReadEnable;
+	wire[23:0] coreLocalMemoryAddress;
+	wire[3:0] coreLocalMemoryByteSelect;
+	wire[31:0] coreLocalMemoryDataWrite;
+	wire[31:0] coreLocalMemoryDataRead;
+	wire coreLocalMemoryBusy;
+
+	// Memory interface from core to wb
+	wire coreWBWriteEnable;
+	wire coreWBReadEnable;
+	wire[27:0] coreWBAddress;
+	wire[3:0] coreWBByteSelect;
+	wire[31:0] coreWBDataWrite;
+	wire[31:0] coreWBDataRead;
+	wire coreWBBusy;
+
+	// Memory interface from wb to local memory
+	wire wbLocalMemoryWriteEnable;
+	wire wbLocalMemoryReadEnable;
+	wire[23:0] wbLocalMemoryAddress;
+	wire[3:0] wbLocalMemoryByteSelect;
+	wire[31:0] wbLocalMemoryDataWrite;
+	wire[31:0] wbLocalMemoryDataRead;
+	wire wbLocalMemoryBusy;
+
+	// Core management
 	wire management_run;
+	wire management_trapEnable;
 	wire management_writeEnable;
 	wire[3:0] management_byteSelect;
 	wire[15:0] management_address;
@@ -137,15 +180,20 @@
 		.management_readData(jtag_management_readData),
 		.probe_jtagInstruction(probe_jtagInstruction));
 
+	wire isAddressBreakpoint;
 	CoreManagement coreManagement(
 		.clk(wb_clk_i),
 		.rst(wb_rst_i),
 		.management_run(management_run),
+		.management_trapEnable(management_trapEnable),
 		.management_writeEnable(management_writeEnable),
 		.management_byteSelect(management_byteSelect),
 		.management_address(management_address),
 		.management_writeData(management_writeData),
 		.management_readData(management_readData),
+		.isAddressBreakpoint(isAddressBreakpoint),
+		.coreAddress(coreMemoryAddress),
+		.core_env(probe_env),
 		.core_errorCode(probe_errorCode),
 		.jtag_management_writeEnable(jtag_management_writeEnable),
 		.jtag_management_readEnable(jtag_management_readEnable),
@@ -162,42 +210,7 @@
 		.wb_management_busy(wb_management_busy));
 
 	// Core
-	// Memory interface from core
-	wire coreMemoryWriteEnable;
-	wire coreMemoryReadEnable;
-	wire[31:0] coreMemoryAddress;
-	wire[3:0] coreMemoryByteSelect;
-	wire[31:0] coreMemoryDataWrite;
-	wire[31:0] coreMemoryDataRead;
-	wire coreMemoryBusy;
-
-	// Memory interface from core to local memory
-	wire coreLocalMemoryWriteEnable;
-	wire coreLocalMemoryReadEnable;
-	wire[23:0] coreLocalMemoryAddress;
-	wire[3:0] coreLocalMemoryByteSelect;
-	wire[31:0] coreLocalMemoryDataWrite;
-	wire[31:0] coreLocalMemoryDataRead;
-	wire coreLocalMemoryBusy;
-
-	// Memory interface from core to wb
-	wire coreWBWriteEnable;
-	wire coreWBReadEnable;
-	wire[27:0] coreWBAddress;
-	wire[3:0] coreWBByteSelect;
-	wire[31:0] coreWBDataWrite;
-	wire[31:0] coreWBDataRead;
-	wire coreWBBusy;
-
-	// Memory interface from wb to local memory
-	wire wbLocalMemoryWriteEnable;
-	wire wbLocalMemoryReadEnable;
-	wire[23:0] wbLocalMemoryAddress;
-	wire[3:0] wbLocalMemoryByteSelect;
-	wire[31:0] wbLocalMemoryDataWrite;
-	wire[31:0] wbLocalMemoryDataRead;
-	wire wbLocalMemoryBusy;
-
+	wire[15:0] userInterrupts = 16'b0;
 	RV32ICore core(
 `ifdef USE_POWER_PINS
 		.vccd1(vccd1),	// User area 1 1.8V power
@@ -213,6 +226,7 @@
 		.memoryDataRead(coreMemoryDataRead),
 		.memoryBusy(coreMemoryBusy),
 		.management_run(management_run),
+		.management_trapEnable(management_trapEnable),
 		.management_writeEnable(management_writeEnable),
 		.management_byteSelect(management_byteSelect),
 		.management_address(management_address),
@@ -223,7 +237,12 @@
 		.partID(partID),
 		.versionID(versionID),
 		.extensions(CORE_EXTENSIONS),
+		.eCall(eCall),
+		.eBreak(eBreak),
+		.isAddressBreakpoint(isAddressBreakpoint),
+		.userInterrupts(userInterrupts),
 		.probe_state(probe_state),
+		.probe_env(probe_env),
 		.probe_programCounter(probe_programCounter),
 		.probe_opcode(probe_opcode),
 		.probe_errorCode(probe_errorCode),

diff --git a/verilog/rtl/ExperiarCore/RV32ICore.v b/verilog/rtl/ExperiarCore/RV32ICore.v
index 4017da8..3fb2c65 100644
--- a/verilog/rtl/ExperiarCore/RV32ICore.v
+++ b/verilog/rtl/ExperiarCore/RV32ICore.v

@@ -18,6 +18,7 @@
 
 		// Management interface
 		input wire management_run,
+		input wire management_trapEnable,
 		input wire management_writeEnable,
 		input wire[3:0] management_byteSelect,
 		input wire[15:0] management_address,
@@ -31,11 +32,20 @@
 		input wire[3:0] versionID,
 		input wire[25:0] extensions,
 
+		// System commands
+		output wire eCall,
+		output wire eBreak,
+
+		// Traps
+		input wire isAddressBreakpoint,
+		input wire[15:0] userInterrupts,
+
 		// Logic probes
 		output wire[1:0] probe_state,
+		output wire[1:0] probe_env,
 		output wire[31:0] probe_programCounter,
 		output wire[6:0] probe_opcode,
-		output wire[3:0] probe_errorCode,
+		output wire[1:0] probe_errorCode,
 		output wire probe_isBranch,
 		output wire probe_takeBranch,
 		output wire probe_isStore,
@@ -43,13 +53,13 @@
 		output wire probe_isCompressed
     );
 
-	localparam STATE_HALT 	 = 2'b00;
-	localparam STATE_FETCH   = 2'b10;
-	localparam STATE_EXECUTE = 2'b11;
+	localparam STATE_HALT 	 	= 2'b00;
+	localparam STATE_FETCH   	= 2'b10;
+	localparam STATE_EXECUTE 	= 2'b11;
 
 	// System registers
 	reg[1:0] state = STATE_HALT;
-	reg[3:0] currentError = 4'b0;
+	reg[1:0] currentError = 2'b0;
 	reg[31:0] programCounter = 32'b0;
 	reg[31:0] currentInstruction = 32'b0;
 	reg[31:0] registers [0:31];
@@ -61,6 +71,7 @@
 
 	wire management_selectProgramCounter      = (management_address[15:14] == MANAGMENT_ADDRESS_SYSTEM) && (management_address[13:4] == 10'h000);
 	wire management_selectInstructionRegister = (management_address[15:14] == MANAGMENT_ADDRESS_SYSTEM) && (management_address[13:4] == 10'h001);
+	wire management_selectClearError		  = (management_address[15:14] == MANAGMENT_ADDRESS_SYSTEM) && (management_address[13:4] == 10'h002);
 	wire management_selectRegister            = (management_address[15:14] == MANAGMENT_ADDRESS_REGISTERS) && (management_address[13:7] == 7'h00);
 	wire management_selectCSR 				  = management_address[15:14] == MANAGMENT_ADDRESS_CSR;
 
@@ -69,6 +80,7 @@
 	wire management_writeProgramCounter_set = management_writeProgramCounter && (management_address[3:0] == 4'h0);
 	wire management_writeProgramCounter_jump = management_writeProgramCounter && (management_address[3:0] == 4'h4);
 	wire management_writeProgramCounter_step = management_writeProgramCounter && (management_address[3:0] == 4'h8);
+	wire management_writeClearError = management_writeValid && management_selectClearError;
 	wire management_writeRegister = management_writeValid && management_selectRegister;
 	wire management_writeCSR = management_writeValid && management_selectCSR;
 
@@ -103,33 +115,20 @@
 		management_byteSelect[0] ? management_dataOut[7:0]   : 8'h00
 	};
 
-	// CSR
-	// Core interface
+	// CSR interface
 	wire coreCSRWrite;
 	wire coreCSRRead;
 	wire[11:0] coreCSRIndex = currentInstruction[31:20];
 	reg[32:0] coreCSRWriteData;
-
-	// Combined interface
-	wire csrWriteEnable = management_writeCSR || (management_run && coreCSRWrite);
-	wire csrReadEnable = management_readCSR || (management_run && coreCSRRead);
+	wire csrWriteEnable = management_writeCSR || (management_run && coreCSRWrite && (state == STATE_EXECUTE));
+	wire csrReadEnable = management_readCSR || (management_run && coreCSRRead && (state == STATE_EXECUTE));
 	wire[11:0] csrAddress = !management_run ? management_csrIndex : coreCSRIndex;
 	wire[31:0] csrWriteData = !management_run ? management_writeData : coreCSRWriteData;
 	wire[31:0] csrReadData;
-	CSR csr(
-		.clk(clk),
-		.rst(rst),
-		.csrWriteEnable(csrWriteEnable),
-		.csrReadEnable(csrReadEnable),
-		.csrAddress(csrAddress),
-		.csrWriteData(csrWriteData),
-		.csrReadData(csrReadData),
-		.coreIndex(coreIndex),
-		.manufacturerID(manufacturerID),
-		.partID(partID),
-		.versionID(versionID),
-		.extensions(extensions),
-		.instructionCompleted(instructionCompleted));
+
+	wire[31:0] trapVector;
+	wire[31:0] trapReturnVector;
+	wire inTrap;
 
 	// Immediate Decode
 	wire[31:0] imm_I = {currentInstruction[31] ? 21'h1F_FFFF : 21'h00_0000, currentInstruction[30:25], currentInstruction[24:21], currentInstruction[20]};
@@ -171,9 +170,11 @@
 	wire isCSRRC = isCSR && (funct3[1:0] == 2'b11);
 	wire isECALL  = isSystem && (currentInstruction[31:7] == 25'b0000000000000000000000000);
 	wire isEBREAK = isSystem && (currentInstruction[31:7] == 25'b0000000000010000000000000);
+	wire isRET    = isSystem && (currentInstruction[31:7] == 25'b0011000000100000000000000);
+
+	wire validSystemCommand = isCSR || isECALL || isEBREAK || isRET;
 
 	reg invalidInstruction;
-
 	always @(*) begin
 		case ({ isLUI, isAUIPC, isJAL, isJALR, isBranch, isLoad, isStore, isALUImm, isALU, isFENCE, isSystem })
 			'b00000000001: invalidInstruction <= 1'b0;
@@ -186,11 +187,13 @@
 			'b00010000000: invalidInstruction <= 1'b0;
 			'b00100000000: invalidInstruction <= 1'b0;
 			'b01000000000: invalidInstruction <= 1'b0;
-			'b10000000000: invalidInstruction <= 1'b0;
+			'b10000000000: invalidInstruction <= validSystemCommand;
 			default: invalidInstruction <= 1'b1;
 		endcase
 	end
 
+	wire isInvalidInstruction = invalidInstruction && state == STATE_EXECUTE;
+
 	// Integer Registers
 	wire[31:0] rs1 = |rs1Index ? registers[rs1Index] : 32'b0;
 	wire[31:0] rs2 = |rs2Index ? registers[rs2Index] : 32'b0;
@@ -236,9 +239,10 @@
 										  isJALR		  		   ? imm_I :
 										  (isBranch && takeBranch) ? imm_B :
 																     32'b0;
-	wire[31:0] nextProgramCounterFull = nextProgramCounterBase + nextProgramCounterOffset;
+	wire[31:0] nextProgramCounterWord = nextProgramCounterBase + nextProgramCounterOffset;
 	wire[31:0] nextProgramCounterCompressed = programCounterLink; // TODO: Need to implement compressed branch and jump instructions
-	wire[31:0] nextProgramCounter = isCompressed ? nextProgramCounterCompressed : nextProgramCounterFull;
+	wire[31:0] nextProgramCounterFull = isCompressed ? nextProgramCounterCompressed : nextProgramCounterWord;
+	wire[31:0] nextProgramCounter = { nextProgramCounterFull[31:1] , 1'b0};
 
 	// ALU	
 	wire aluAlt = funct7 == 7'b0100000 && (isALU || isALUImmShift);
@@ -278,7 +282,7 @@
 	assign coreCSRWrite = isCSRRW || (isCSR && |rs1Index);
 	assign coreCSRRead = isCSRRC || isCSRRS || (isCSR && |rdIndex);
 
-	wire csrRS1Data = isCSRIMM ? { 27'b0, rs1Index} : rs1;
+	wire[31:0] csrRS1Data = isCSRIMM ? { 27'b0, rs1Index} : rs1;
 
 	always @(*) begin
 		if (isCSR) begin			
@@ -340,6 +344,7 @@
 	wire[6:0] loadStoreByteMask = {3'b0, baseByteMask} << targetMemoryAddress[1:0];
 	wire loadStoreByteMaskValid = |(loadStoreByteMask[3:0]);
 	wire addressMissaligned = |loadStoreByteMask[6:4];
+	wire isAddressMisaligned = addressMissaligned && ((state == STATE_FETCH) || ((state == STATE_EXECUTE) && (isLoad || isStore)));
 	wire shouldLoad  = loadStoreByteMaskValid && !addressMissaligned && ((state == STATE_FETCH) || ((state == STATE_EXECUTE) && isLoad));
 	wire shouldStore = loadStoreByteMaskValid && !addressMissaligned && ((state == STATE_EXECUTE) && isStore);
 	assign memoryAddress = shouldLoad || shouldStore ? { targetMemoryAddress[31:2], 2'b00 } : 32'b0;
@@ -424,13 +429,14 @@
 	wire memoryWriteDone = shouldStore ? !memoryBusy : 1'b0;
 
 	// Register Write
-	wire integerRegisterWriteEn = isLUI || isAUIPC || isJAL || isJALR || isALU || isALUImm || isLoad;
+	wire integerRegisterWriteEn = isLUI || isAUIPC || isJAL || isJALR || isALU || isALUImm || isLoad || csrReadEnable;
 	wire[31:0] integerRegisterWriteData = isLUI 			  ? imm_U			   :
 										  isAUIPC 			  ? aluAPlusB 		   :
 										  isJAL 			  ? programCounterLink :
 										  isJALR 			  ? programCounterLink :
 										  isLoad 			  ? loadData		   :
 										  (isALU || isALUImm) ? aluValue 		   :
+										  csrReadEnable		  ? csrReadData 	   :
 																32'b0;
 
 	wire progressExecute = isStore ? memoryWriteDone :
@@ -445,47 +451,100 @@
 			programCounter <= 32'b0;
 			currentInstruction <= 32'b0;
 		end else begin
-			if (!(|currentError)) begin
-				case (state)
-					STATE_HALT: begin
-						if (management_allowInstruction) state <= STATE_FETCH;
-						else begin
-							if (management_writeProgramCounter_set) programCounter <= { management_writeData[31:1] , 1'b0};
-							else if (management_writeProgramCounter_jump) programCounter <= { management_jumpTarget[31:1] , 1'b0};
-							else if (management_writeRegister) registers[management_registerIndex] <= management_writeData;
-						end
+			case (state)
+				STATE_HALT: begin
+					if (management_allowInstruction && !(|currentError)) state <= STATE_FETCH;
+					else begin
+						if (management_writeProgramCounter_set) programCounter <= { management_writeData[31:1] , 1'b0};
+						else if (management_writeProgramCounter_jump) programCounter <= { management_jumpTarget[31:1] , 1'b0};
+						else if (management_writeRegister) registers[management_registerIndex] <= management_writeData;
+						else if (management_writeClearError) currentError <= 2'b0;
 					end
+				end
 
-					STATE_FETCH: begin
-						if (memoryReadReady) begin
+				STATE_FETCH: begin
+					if (|currentError) begin
+						state <= STATE_HALT;
+					end else begin
+						if (inTrap) programCounter <= trapVector;
+						else if (memoryReadReady) begin
 							currentInstruction <= dataIn;
 							state <= STATE_EXECUTE;
 						end
 					end
+				end
 
-					STATE_EXECUTE: begin
-						if (addressMissaligned || invalidInstruction) begin
-							currentError <= { 1'b0, 1'b0, addressMissaligned, invalidInstruction };
-						end else begin
-							if (progressExecute) begin
-								if (integerRegisterWriteEn && |rdIndex) registers[rdIndex] <= integerRegisterWriteData;
+				STATE_EXECUTE: begin
+					if (!management_trapEnable && (isAddressMisaligned || isInvalidInstruction)) begin
+						currentError <= { isAddressMisaligned, isInvalidInstruction };
+						state <= STATE_HALT;
+					end else begin
+						if (progressExecute) begin
+							if (integerRegisterWriteEn && |rdIndex) registers[rdIndex] <= integerRegisterWriteData;
 
-								programCounter <= { nextProgramCounter[31:1] , 1'b0};
-
-								if (management_allowInstruction) state <= STATE_FETCH;
-								else state <= STATE_HALT;
+							if (management_trapEnable) begin
+								if (inTrap) programCounter <= trapVector;
+								else if (isRET) programCounter <= trapReturnVector;
+								else programCounter <= nextProgramCounter;
+							end else begin
+								programCounter <= nextProgramCounter;
 							end
+							
+							if (management_allowInstruction) state <= STATE_FETCH;
+							else state <= STATE_HALT;
 						end
 					end
+				end
 
-					default: state <= STATE_HALT;
-				endcase
-			end
+				default: state <= STATE_HALT;
+			endcase
 		end
 	end
 
+	// System commands 
+	assign eCall = isECALL && (state == STATE_EXECUTE);
+	assign eBreak = isEBREAK && (state == STATE_EXECUTE);
+	wire trapReturn = isRET && (state == STATE_EXECUTE);
+
+	// CSRs
+	wire instructionCompleted = (state == STATE_EXECUTE) && progressExecute;
+	CSR csr(
+		.clk(clk),
+		.rst(rst),
+		.csrWriteEnable(csrWriteEnable),
+		.csrReadEnable(csrReadEnable),
+		.csrAddress(csrAddress),
+		.csrWriteData(csrWriteData),
+		.csrReadData(csrReadData),
+		.coreIndex(coreIndex),
+		.manufacturerID(manufacturerID),
+		.partID(partID),
+		.versionID(versionID),
+		.extensions(extensions),
+		.instructionCompleted(instructionCompleted),
+		.coreState(state),
+		.programCounter(programCounter),
+		.currentInstruction(currentInstruction),
+		.isLoad(isLoad),
+		.isStore(isStore),
+		.isMachineTimerInterrupt(isMachineTimerInterrupt),
+		.isMachineExternalInterrupt(isMachineExternalInterrupt),
+		.isMachineSoftwareInterrupt(isMachineSoftwareInterrupt),
+		.isAddressMisaligned(isAddressMisaligned),
+		.isAccessFault(isAccessFault),
+		.isInvalidInstruction(isInvalidInstruction),
+		.isEBREAK(eBreak),
+		.isECALL(eCall),
+		.isAddressBreakpoint(isAddressBreakpoint),
+		.userInterrupts(userInterrupts),
+		.trapReturn(trapReturn),
+		.inTrap(inTrap),
+		.trapVector(trapVector),
+		.trapReturnVector(trapReturnVector));
+
 	// Debug
 	assign probe_state = state;
+	assign probe_env = { eCall, eBreak };
 	assign probe_programCounter = programCounter;
 	assign probe_opcode = opcode;
 	assign probe_errorCode = currentError;

diff --git a/verilog/rtl/ExperiarSoC/ExperiarSoC_top.v b/verilog/rtl/ExperiarSoC/ExperiarSoC_top.v
index 1521abc..ff06ba0 100644
--- a/verilog/rtl/ExperiarSoC/ExperiarSoC_top.v
+++ b/verilog/rtl/ExperiarSoC/ExperiarSoC_top.v

@@ -339,9 +339,10 @@
 
 	// Logic probes
 	wire[1:0] probe_core0_state;
+	wire[1:0] probe_core0_env;
 	wire[31:0] probe_core0_programCounter;
 	wire[6:0] probe_core0_opcode;
-	wire[3:0] probe_core0_errorCode;
+	wire[1:0] probe_core0_errorCode;
 	wire probe_core0_isBranch;
 	wire probe_core0_takeBranch;
 	wire probe_core0_isStore;
@@ -351,6 +352,7 @@
 
 	wire[54:0] probe_core0 = {
 		probe_core0_state,
+		probe_core0_env,
 		probe_core0_programCounter,
 		probe_core0_opcode,
 		probe_core0_errorCode,
@@ -409,6 +411,7 @@
 		.addr1(core0SRAM_addr1),
 		.dout1(core0SRAM_dout1),
 		.probe_state(probe_core0_state),
+		.probe_env(probe_core0_env),
 		.probe_programCounter(probe_core0_programCounter),
 		.probe_opcode(probe_core0_opcode),
 		.probe_errorCode(probe_core0_errorCode),
@@ -485,9 +488,10 @@
 
 	// Logic probes
 	wire[1:0] probe_core1_state;
+	wire[1:0] probe_core1_env;
 	wire[31:0] probe_core1_programCounter;
 	wire[6:0] probe_core1_opcode;
-	wire[3:0] probe_core1_errorCode;
+	wire[1:0] probe_core1_errorCode;
 	wire probe_core1_isBranch;
 	wire probe_core1_takeBranch;
 	wire probe_core1_isStore;
@@ -497,6 +501,7 @@
 
 	wire[54:0] probe_core1 = {
 		probe_core1_state,
+		probe_core1_env,
 		probe_core1_programCounter,
 		probe_core1_opcode,
 		probe_core1_errorCode,
@@ -555,6 +560,7 @@
 		.addr1(core1SRAM_addr1),
 		.dout1(core1SRAM_dout1),
 		.probe_state(probe_core1_state),
+		.probe_env(probe_core1_env),
 		.probe_programCounter(probe_core1_programCounter),
 		.probe_opcode(probe_core1_opcode),
 		.probe_errorCode(probe_core1_errorCode),