Zicsr: implement register operand instructions

firmware/src/entry.S

@@ -1,27 +1,115 @@
 .global __entry
+.global __vector
+.global exception_stack_top
 
 .pushsection .text.entry
 .option push
 .option norvc
 __entry:
 __reset:        j fw_entry
-__exception:    j fw_exception_entry
 
 fw_entry:
     la sp, stack_top
     jal {firmware_main}
-fw_exception_entry:
-    mv tp, sp
-    la sp, exception_stack_top
-    jal {firmware_exception}
-    j .
 
 .option pop
 .popsection // .text.entry
 
+// ra+gp, 7 tN, 12 sN, 8 aN
+.set GP_REGS_SIZE,      ((2 + 7 + 12 + 8) * 4)
+.set CTL_REGS_SIZE,     (6 * 8)
+.set SAVED_FRAME_SIZE,  (GP_REGS_SIZE + CTL_REGS_SIZE)
+
 .pushsection .text.vectors, "ax"
-.global __vector
 __vector:
+    // Switch to exception stack
+    csrrw sp, mscratch, sp
+
+    // Store register state
+    addi sp, sp, -SAVED_FRAME_SIZE
+    sw ra,      0 * 4(sp)
+    sw gp,      1 * 4(sp)
+
+    sw t0,      2 * 4(sp)
+    sw t1,      3 * 4(sp)
+    sw t2,      4 * 4(sp)
+    sw t3,      5 * 4(sp)
+    sw t4,      6 * 4(sp)
+    sw t5,      7 * 4(sp)
+    sw t6,      8 * 4(sp)
+
+    sw s0,      9 * 4(sp)
+    sw s1,     10 * 4(sp)
+    sw s2,     11 * 4(sp)
+    sw s3,     12 * 4(sp)
+    sw s4,     13 * 4(sp)
+    sw s5,     14 * 4(sp)
+    sw s6,     15 * 4(sp)
+    sw s7,     16 * 4(sp)
+    sw s8,     17 * 4(sp)
+    sw s9,     18 * 4(sp)
+    sw s10,    19 * 4(sp)
+    sw s11,    20 * 4(sp)
+
+    sw a0,     21 * 4(sp)
+    sw a1,     22 * 4(sp)
+    sw a2,     23 * 4(sp)
+    sw a3,     24 * 4(sp)
+    sw a4,     25 * 4(sp)
+    sw a5,     26 * 4(sp)
+    sw a6,     27 * 4(sp)
+    sw a7,     28 * 4(sp)
+
+    csrr t0, mtval
+    csrr t1, mepc
+    csrr t2, mcause
+    csrr t3, mscratch
+
+    sw t0,     29 * 4(sp)
+    sw t1,     30 * 4(sp)
+    sw t2,     31 * 4(sp)
+    sw t3,     32 * 4(sp)
+
+    mv a0, sp
+    jal {firmware_exception}
+
+    // Restore state
+    lw ra,      0 * 4(sp)
+    lw gp,      1 * 4(sp)
+
+    lw t0,      2 * 4(sp)
+    lw t1,      3 * 4(sp)
+    lw t2,      4 * 4(sp)
+    lw t3,      5 * 4(sp)
+    lw t4,      6 * 4(sp)
+    lw t5,      7 * 4(sp)
+    lw t6,      8 * 4(sp)
+
+    lw s0,      9 * 4(sp)
+    lw s1,     10 * 4(sp)
+    lw s2,     11 * 4(sp)
+    lw s3,     12 * 4(sp)
+    lw s4,     13 * 4(sp)
+    lw s5,     14 * 4(sp)
+    lw s6,     15 * 4(sp)
+    lw s7,     16 * 4(sp)
+    lw s8,     17 * 4(sp)
+    lw s9,     18 * 4(sp)
+    lw s10,    19 * 4(sp)
+    lw s11,    20 * 4(sp)
+
+    lw a0,     21 * 4(sp)
+    lw a1,     22 * 4(sp)
+    lw a2,     23 * 4(sp)
+    lw a3,     24 * 4(sp)
+    lw a4,     25 * 4(sp)
+    lw a5,     26 * 4(sp)
+    lw a6,     27 * 4(sp)
+    lw a7,     28 * 4(sp)
+    addi sp, sp, SAVED_FRAME_SIZE
+
+    csrrw sp, mscratch, sp
+    mret
     j .
 .popsection // .text.vectors
 

firmware/src/main.rs

@@ -24,25 +24,52 @@ impl fmt::Write for D {
 }
 
 unsafe extern "C" fn firmware_main() -> ! {
+    unsafe extern "C" {
+        static __vector: u8;
+        static exception_stack_top: u8;
+    }
+
+    let vector_address = (&raw const __vector).addr();
+    core::arch::asm!("csrw mtvec, {val}", val = in(reg) vector_address);
+    let exception_stack_address = (&raw const exception_stack_top).addr();
+    core::arch::asm!("csrw mscratch, {val}", val = in(reg) exception_stack_address);
+
     use fmt::Write;
 
     let mut d = D;
-    for i in 0..10 {
+
-        write!(d, "Hello {i}").ok();
+    writeln!(d, "Set mtvec={vector_address:#x}").ok();
-        write!(d, "\n").ok();
+
+    for i in 0..20 {
+        writeln!(d, "Hello {i:#x}").ok();
     }
 
     loop {
         // TODO pause/system/fence instruction not implemented
         // core::hint::spin_loop();
-        core::arch::asm!("ebreak");
+        core::arch::asm!("ecall");
     }
 }
 
-unsafe extern "C" fn firmware_exception() {
+#[repr(C)]
+struct ExceptionFrame {
+    regs: [u32; 29],
+    mtval: u32,
+    mepc: u32,
+    mcause: u32,
+    mscratch: u32,
+}
+
+unsafe extern "C" fn firmware_exception(frame: &ExceptionFrame) {
     use fmt::Write;
     let mut d = D;
     writeln!(d, "In exception handler").ok();
+    writeln!(
+        d,
+        "mepc={:x}, mcause={:x}, mscratch={:x}",
+        frame.mepc, frame.mcause, frame.mscratch
+    )
+    .ok();
 }
 
 global_asm!(

src/core/rvx0_control.sv

@@ -33,7 +33,6 @@ module rvx0_control (
         end
     end
 
-    // TODO handle pslverr
     always @(negedge clk_i) begin
         if (~rst_i) begin
             case (control_state_o)
@@ -98,7 +97,6 @@ module rvx0_control (
             end
             CSTATE_EXEC: begin
                 if (exec_stall_i) begin
-                    $display("control: exec_stall_i @ %x", pc_value_i);
                     control_state_o <= CSTATE_FETCH;
                 end else if (exec_ready_i) begin
                     control_state_o <= CSTATE_WRITE;
@@ -115,8 +113,6 @@ module rvx0_control (
                     apb_master.pwrite <= 1;
                     apb_master.penable <= 1;
 
-                    $display("WRITE %x -> %x", memory_write_i, memory_address_i);
-
                     if (apb_master.pslverr) begin
                         apb_master.penable <= 0;
                         fault_memory_access_o <= 1;

src/core/rvx0_core.sv

@@ -20,6 +20,12 @@ module rvx0_core #(
 
     wire [31:0]     csr_mtvec;
     wire [31:0]     csr_mepc;
+    wire            csr_clk;
+    wire            csr_write;
+    wire [11:0]     csr_index;
+    wire [31:0]     csr_read_value;
+    wire            undefined_csr;
+    wire            csr_write_fault;
 
     // Exception generation
 
@@ -66,11 +72,14 @@ module rvx0_core #(
     wire [4:0]      rd;
     wire            v1src;
     wire [1:0]      v2src;
-    wire [1:0]      vdsrc;
+    wire [2:0]      vdsrc;
     wire [31:0]     imm;
     wire [3:0]      aluop;
     wire            write_pc;
     wire [2:0]      funct3;
+    wire            instruction_multi_cycle;
+    wire            instruction_zicsr;
+    wire            exception_return;
 
     // Into ALU
 
@@ -93,6 +102,22 @@ module rvx0_core #(
     wire [31:0] memory_write_output;
     wire        memory_access_partial;
 
+    // "Slow" datapath test
+
+    logic       regfile_clk;
+    logic       advance_pc;
+
+    // Zicsr extension
+
+    logic       zicsr_clk_gate;
+    wire        zicsr_clk;
+    wire        zicsr_exec_ready;
+    wire        zicsr_regfile_clk;
+    wire        zicsr_instruction_fault;
+    wire [31:0] zicsr_read_value;
+    wire [31:0] zicsr_write_value;
+    wire        zicsr_write;
+
     rvx0_pc #(
         .RESET_VECTOR               (RESET_VECTOR)
     ) pc (
@@ -129,7 +154,11 @@ module rvx0_core #(
         .conditional_o              (conditional),
 
         .raise_ecall_o              (raise_ecall),
-        .raise_ebreak_o             (raise_ebreak)
+        .raise_ebreak_o             (raise_ebreak),
+        .exception_return_o         (exception_return),
+
+        .instruction_multi_cycle_o  (instruction_multi_cycle),
+        .instruction_zicsr_o        (instruction_zicsr)
     );
 
     rvx0_regfile regfile (
@@ -195,13 +224,18 @@ module rvx0_core #(
         .rst_i                      (rst_i),
 
         .mepc_i                     (mepc_write_value),
-        .mepc_write_i               (fault),
+        .mepc_write_i               (csr_write_fault),
         .mcause_i                   (mcause_write_value),
-        .mcause_write_i             (fault),
+        .mcause_write_i             (csr_write_fault),
         .mtval_i                    (mtval_write_value),
-        .mtval_write_i              (fault),
+        .mtval_write_i              (csr_write_fault),
+        .mtvec_o                    (csr_mtvec),
 
-        .mtvec_o                    (csr_mtvec)
+        .csr_index_i                (csr_index),
+        .csr_write_value_i          (zicsr_write_value),
+        .csr_read_value_o           (csr_read_value),
+        .csr_write_i                (csr_write),
+        .undefined_csr_o            (undefined_csr)
     );
 
     rvx0_fault_encoder fault_encoder (
@@ -227,7 +261,30 @@ module rvx0_core #(
         .mepc_write_value_o         (mepc_write_value)
     );
 
-    assign fault_instruction = fault_instruction_decode || (fault_unsupported_size && (need_memory_read || need_memory_write));
+    rvx0_zicsr zicsr (
+        .clk_i                      (zicsr_clk),
+        .rst_i                      (rst_i),
+
+        .funct3_i                   (funct3),
+        .rs1_value_i                (rs1_value),
+        .rd_i                       (rd),
+        .imm_i                      (imm),
+
+        .exec_ready_o               (zicsr_exec_ready),
+        .regfile_clk_o              (zicsr_regfile_clk),
+
+        .csr_index_o                (csr_index),
+        .csr_read_output_o          (zicsr_read_value),
+        .csr_write_output_o         (zicsr_write_value),
+        .csr_value_i                (csr_read_value),
+        .csr_write_o                (zicsr_write),
+        .undefined_csr_i            (undefined_csr),
+        .instruction_fault_o        (zicsr_instruction_fault)
+    );
+
+    assign csr_write = (zicsr_write && zicsr_clk_gate) && state_exec;
+
+    assign fault_instruction = fault_instruction_decode || (fault_unsupported_size && (need_memory_read || need_memory_write)) || (zicsr_instruction_fault && instruction_zicsr);
     assign fault_memory_align = fault_address_align && (need_memory_write || need_memory_read);
 
     assign memory_write_input = rs2_value;
@@ -236,12 +293,20 @@ module rvx0_core #(
     assign alu_value1 = v1src == V1SRC_ZERO ? 0 : rs1_value;
     assign state_exec = control_state == CSTATE_EXEC;
 
-    assign pc_clk = (state_exec || rst_i) && clk_i;
+    assign pc_clk = ((state_exec && (advance_pc || fault)) || rst_i) && clk_i;
+
+    assign zicsr_clk = ((state_exec && zicsr_clk_gate) || rst_i) && clk_i;
 
     assign write_rd = (vdsrc != RDSRC_NONE) && state_exec && ~fault;
 
     assign write_pc_and_taken = ((conditional ? branch_taken : 1) && write_pc) || fault;
 
+    assign pc_write_value = fault ? csr_mtvec : alu_output;
+
+    assign zicsr_clk_gate = instruction_zicsr && state_exec;
+    assign exec_stall = fault && state_exec;
+    assign csr_write_fault = fault && state_exec;
+
     // Value mux
     always_comb begin
         case (vdsrc)
@@ -249,6 +314,7 @@ module rvx0_core #(
         RDSRC_ALU:  rd_write_value = alu_output;
         RDSRC_MEM:  rd_write_value = memory_read_output;
         RDSRC_NONE: rd_write_value = 0;
+        RDSRC_CSR:  rd_write_value = zicsr_read_value;
         endcase
 
         case (v2src)
@@ -256,34 +322,59 @@ module rvx0_core #(
         V2SRC_PC:   alu_value2 = pc_value;
         V2SRC_RS2:  alu_value2 = rs2_value;
         endcase
-
-        if (fault) begin
-            pc_write_value = csr_mtvec;
-        end else begin
-            pc_write_value = alu_output;
-        end
     end
 
     always @(posedge clk_i) begin
+        advance_pc = 0;
+
         case (control_state)
         CSTATE_EXEC: begin
             exec_ready = 0;
-            exec_stall = 0;
+            regfile_clk = 0;
 
-            if (fault) begin
-                $display("Take exception mcause=%x, mepc=%x, mtval=%x to %x", mcause_write_value, mepc_write_value, mtval_write_value, csr_mtvec);
-                $display("Instruction: %x", instruction);
-`ifdef RVX0_CRASH_ON_EXCEPTION
-                regfile.dump_state;
-                $finish;
-`endif
-                exec_stall = 1;
-            end else begin
 `ifdef RVX0_DEBUGGING
             $display("[%x] %x, rs1 (r%d)=%x, rs2 (r%d)=%x, v1src=%d, v2src=%d, vdsrc=%d", pc_value, instruction, rs1, rs1_value, rs2, rs2_value, v1src, v2src, vdsrc);
             $display("\talu_value1=%x, alu_value2=%x, imm=%x, rd_write_value=%x", alu_value1, alu_value2, imm, rd_write_value);
             $display("\tmri=%x, mro=%x, mwi=%x, mwo=%x", memory_read_input, memory_read_output, memory_write_input, memory_write_output);
 `endif
+
+            if (fault) begin
+                $display("Take exception mcause=%x, mepc=%x, mtval=%x to %x", mcause_write_value, mepc_write_value, mtval_write_value, csr_mtvec);
+                $display("Instruction: %x", instruction);
+                if (fault_instruction) begin
+                    $display("Instruction decode fault:");
+                    if (fault_instruction_decode) begin
+                        $display("\tfault_instruction_decode");
+                    end
+                    if (fault_unsupported_size && (need_memory_read || need_memory_write)) begin
+                        $display("\tfault_unsupported_size");
+                    end
+                    if (zicsr_instruction_fault && instruction_zicsr) begin
+                        $display("\tzicsr_instruction_fault");
+                    end
+                end
+                fault_encoder.dump_fault;
+`ifndef RVX0_CRASH_ON_EXCEPTION
+                if (raise_ebreak) begin
+`endif
+                    regfile.dump_state;
+                    csr.dump_state;
+                    $finish;
+`ifndef RVX0_CRASH_ON_EXCEPTION
+                end
+`endif
+            end else if (exception_return) begin
+                $error("TODO: exception return (mret/sret)");
+            end else if (instruction_zicsr) begin
+                regfile_clk = zicsr_regfile_clk;
+                advance_pc = zicsr_exec_ready;
+                exec_ready = zicsr_exec_ready;
+            end else if (instruction_multi_cycle) begin
+                // "Slow" instruction datapath, allowing multi-cycle
+                // instructions like mul/div, etc
+            end else begin
+                regfile_clk = 1;
+                advance_pc = 1;
                 exec_ready = 1;
             end
         end

src/core/rvx0_csr.sv

@@ -11,42 +11,89 @@ module rvx0_csr (
     input wire          mcause_write_i,
     input wire          mtval_write_i,
 
-    output wire [31:0]  misa_o,         // Hardwired
+    output wire [31:0]  mtvec_o,
-    output wire [31:0]  mvendorid_o,    // Hardwired
+
-    output wire [31:0]  marchid_o,      // Hardwired
+    // CSR access
-    output wire [31:0]  mimpid_o,       // Hardwired
+    input wire          csr_write_i,
-    output wire [31:0]  mhartid_o,      // Hardwired
+    input wire [11:0]   csr_index_i,
-    output reg [31:0]   mstatus_o,      // TODO
+    input wire [31:0]   csr_write_value_i,
-    output reg [31:0]   mstatush_o,     // TODO
+    output logic [31:0] csr_read_value_o,
-    output reg [31:0]   mtvec_o,
+    output logic        undefined_csr_o
-    output reg [31:0]   mepc_o,
-    output reg [31:0]   mcause_o,
-    output reg [31:0]   mtval_o         // TODO
 );
-    // Add extensions here :)
+    const logic [3:0] IDX_MISA = 0;
-    assign misa_o       = MISA_MXLEN_32;
+    const logic [3:0] IDX_MVENDORID = 1;
-    assign mvendorid_o  = 0;
+    const logic [3:0] IDX_MARCHID = 2;
-    assign marchid_o    = 0;
+    const logic [3:0] IDX_MIMPID = 3;
-    assign mimpid_o     = 0;
+    const logic [3:0] IDX_MHARTID = 4;
-    assign mhartid_o    = 0;
+    const logic [3:0] IDX_MSTATUS = 5;
+    const logic [3:0] IDX_MSTATUSH = 6;
+    const logic [3:0] IDX_MTVEC = 7;
+    const logic [3:0] IDX_MEPC = 8;
+    const logic [3:0] IDX_MCAUSE = 9;
+    const logic [3:0] IDX_MTVAL = 10;
+    const logic [3:0] IDX_MSCRATCH = 11;
+
+    logic [31:0] csr_values [0:15];
+    logic [3:0] selected_csr;
+    wire non_insn_write;
+
+    assign mtvec_o = csr_values[IDX_MTVEC];
+
+    assign non_insn_write = mepc_write_i || mtval_write_i || mcause_write_i;
+
+    assign csr_read_value_o = csr_values[selected_csr];
+
+    function void dump_state;
+        $display("CSR state:");
+        $display(
+            "\tmtvec=%x, mtval=%x, mepc=%x, mcause=%x",
+            csr_values[IDX_MTVEC], csr_values[IDX_MTVAL],
+            csr_values[IDX_MEPC], csr_values[IDX_MCAUSE]
+        );
+        $display("\tmscratch=%x", csr_values[IDX_MSCRATCH]);
+    endfunction
+
+    always_comb begin
+        undefined_csr_o = 0;
+        selected_csr = 0;
+
+        case (csr_index_i)
+        // Machine trap setup
+        'h305: selected_csr = IDX_MTVEC;
+        // Machine trap handling
+        'h340: selected_csr = IDX_MSCRATCH;
+        'h341: selected_csr = IDX_MEPC;
+        'h342: selected_csr = IDX_MCAUSE;
+        'h343: selected_csr = IDX_MTVAL;
+        default: undefined_csr_o = 1;
+        endcase
+    end
 
     always @(posedge clk_i) begin
         if (rst_i) begin
-            // TODO set from CPU
+            csr_values[IDX_MTVEC] <= 0;
-            mtvec_o = 'h00001004;
+            csr_values[IDX_MEPC] <= 0;
-            mepc_o = 0;
+            csr_values[IDX_MSTATUS] <= 0;
-            mstatus_o = 0;
+            csr_values[IDX_MSTATUSH] <= 0;
-            mstatush_o = 0;
+        end else begin
-        end
+            if (non_insn_write) begin
-
+                // Non-Zicsr writes take precedence (likely from an exception)
                 if (mepc_write_i) begin
-            mepc_o <= mepc_i;
+                    $display("CSR write mepc <= %x", mepc_i);
+                    csr_values[IDX_MEPC] <= mepc_i;
                 end
                 if (mcause_write_i) begin
-            mcause_o <= mcause_i;
+                    csr_values[IDX_MCAUSE] <= mcause_i;
                 end
                 if (mtval_write_i) begin
-            mtval_o <= mtval_i;
+                    csr_values[IDX_MTVAL] <= mtval_i;
+                end
+            end else if (csr_write_i && ~undefined_csr_o) begin
+                $display("CSR write: %d (%x) <= %x", selected_csr, csr_index_i, csr_write_value_i);
+                if (selected_csr >= IDX_MSTATUS) begin
+                    csr_values[selected_csr] <= csr_write_value_i;
+                end
+            end
         end
     end
 endmodule

src/core/rvx0_fault_encoder.sv

@@ -22,6 +22,32 @@ module rvx0_fault_encoder (
     output logic [31:0] mepc_write_value_o,
     output logic        fault_o
 );
+    function void dump_fault;
+        $display("Fault:");
+        if (fault_instruction_align_i) begin
+            $display("\tfault_instruction_align_i");
+        end
+        if (fault_instruction_fetch_i) begin
+            $display("\tfault_instruction_fetch_i");
+        end
+        if (fault_instruction_decode_i) begin
+            $display("\tfault_instruction_decode_i");
+        end
+        if (fault_memory_align_i) begin
+            $display("\tfault_memory_align_i");
+        end
+        if (fault_memory_access_i) begin
+            $display("\tfault_memory_access_i");
+        end
+        if (raise_ecall_i) begin
+            $display("\traise_ecall_i");
+        end
+        if (raise_ebreak_i) begin
+            $display("\traise_ebreak_i");
+        end
+
+    endfunction
+
     assign fault_o = fault_instruction_align_i ||
                      fault_instruction_fetch_i ||
                      fault_instruction_decode_i ||
@@ -67,5 +93,4 @@ module rvx0_fault_encoder (
             mcause_write_value_o = MCAUSE_INTR_EXT_MMODE | MCAUSE_INTERRUPT;
         end
     end
-
 endmodule

src/core/rvx0_zicsr.sv

@@ -0,0 +1,119 @@
+module rvx0_zicsr (
+    input wire          clk_i,
+    input wire          rst_i,
+
+    input wire [2:0]    funct3_i,
+    input wire [31:0]   rs1_value_i,
+    input wire [4:0]    rd_i,
+    input wire [31:0]   imm_i,
+    input wire [31:0]   csr_value_i,
+    input wire          undefined_csr_i,
+
+
+    output logic        exec_ready_o,
+    output logic        regfile_clk_o,
+
+    // CSR access
+    output logic        instruction_fault_o,
+    output logic [31:0] csr_read_output_o,
+    output logic [31:0] csr_write_output_o,
+    output logic [11:0] csr_index_o,
+    output logic        csr_write_o,
+    output logic        csr_clk_o
+);
+    logic           need_csr_read;
+    logic           need_csr_write;
+    logic           single_cycle;
+
+    logic [31:0]    value1;
+    logic [31:0]    value2;
+    logic [31:0]    rs1_value_latch;
+    logic [31:0]    csr_value_latch;
+    logic [1:0]     latch_state;
+
+    wire [31:0]     csrrs_output;
+    wire [31:0]     csrrc_output;
+
+    assign csr_index_o = imm_i[11:0];
+
+    assign csrrs_output = value1 | value2;
+    assign csrrc_output = value1 & ~value2;
+
+    assign exec_ready_o = single_cycle || latch_state == 2;
+    assign csr_write_o = single_cycle ? need_csr_write : latch_state == 2;
+    assign regfile_clk_o = single_cycle ? need_csr_read : latch_state == 2;
+
+    assign value1 = single_cycle ? csr_value_i : csr_value_latch;
+    assign value2 = single_cycle ? rs1_value_i : rs1_value_latch;
+    assign csr_read_output_o = single_cycle ? csr_value_i : csr_value_latch;
+
+    always_comb begin
+        instruction_fault_o = undefined_csr_i;
+        need_csr_read = 0;
+        single_cycle = 0;
+
+        csr_write_output_o = 0;
+
+        case (funct3_i)
+        3'b001: begin // csrrw
+            need_csr_read = rd_i != 0;
+            need_csr_write = 1;
+            csr_write_output_o = value2;
+        end
+        3'b010: begin // csrrs
+            need_csr_read = 1;
+            need_csr_write = rs1_value_i != 0;
+            csr_write_output_o = csrrs_output;
+        end
+        3'b011: begin // csrrc
+            need_csr_read = 1;
+            need_csr_write = rs1_value_i != 0;
+            csr_write_output_o = csrrc_output;
+        end
+        // TODO csrrwi, csrrsi, csrrci
+        default: instruction_fault_o = 1;
+        endcase
+
+        if ((need_csr_read && ~need_csr_write) || (~need_csr_read && need_csr_write)) begin
+            single_cycle = 1;
+        end
+    end
+
+    always @(posedge clk_i) begin
+        if (rst_i || instruction_fault_o) begin
+            latch_state <= 0;
+        end else begin
+            if (~need_csr_read && ~need_csr_write) begin
+                $error("TODO: no-op zicsr instruction?");
+            end
+
+            if (single_cycle) begin
+                latch_state <= 0;
+
+                if (need_csr_read) begin
+                    $display("CSR read: csr=%x, csr_read_output_o=%x", csr_index_o, csr_read_output_o);
+                end else begin
+                    $display("CSR write: csr=%x, csr_write_output_o=%x", csr_index_o, csr_write_output_o);
+                end
+            end else begin
+                // need_csr_read && need_csr_write
+                case (latch_state)
+                0: begin
+                    // Latch CSR and RS1 values
+                    csr_value_latch <= csr_value_i;
+                    rs1_value_latch <= rs1_value_i;
+                    $display("Latched values: csr_value_latch=%x, rs1_value_latch=%x", csr_value_latch, rs1_value_latch);
+                    latch_state <= 1;
+                end
+                1: begin
+                    // Writeback
+                    latch_state <= 2;
+                end
+                2: begin
+                    latch_state <= 0;
+                end
+                endcase
+            end
+        end
+    end
+endmodule

src/decoder/rvx0_decoder.sv

@@ -8,17 +8,19 @@ module rvx0_decoder (
     output logic        write_mem_o,
     output logic        conditional_o,
 
-    output logic        is_system_o,
-
     output logic        raise_ecall_o,
     output logic        raise_ebreak_o,
+    output logic        instruction_multi_cycle_o,
+
+    output logic        instruction_zicsr_o,
+    output logic        exception_return_o,
 
     output logic        instruction_fault_o,
     output logic [6:0]  opcode_o,
     output wire [31:0]  imm_o,
     output wire         v1src_o,
     output wire [1:0]   v2src_o,
-    output wire [1:0]   vdsrc_o,
+    output wire [2:0]   vdsrc_o,
     output wire [3:0]   aluop_o,
     output wire [2:0]   funct3_o,
     output wire [6:0]   funct7_o,
@@ -55,7 +57,7 @@ module rvx0_decoder (
     wire        base_write_mem;
     wire        base_v1src;
     wire [1:0]  base_v2src;
-    wire [1:0]  base_vdsrc;
+    wire [2:0]  base_vdsrc;
     wire        base_conditional;
     wire [3:0]  base_aluop;
     wire [31:0] base_imm;
@@ -85,16 +87,13 @@ module rvx0_decoder (
     );
 
     always_comb begin
-        is_system_o = 0;
-
-        case (opcode_o)
-        'b0001111: is_system_o = 1;
-        'b1110011: is_system_o = 1;
-        endcase
-
         raise_ecall_o = 0;
         raise_ebreak_o = 0;
 
+        instruction_multi_cycle_o = 0;
+        instruction_zicsr_o = 0;
+        exception_return_o = 0;
+
         write_pc_o = 0;
         write_mem_o = 0;
         conditional_o = 0;
@@ -106,14 +105,40 @@ module rvx0_decoder (
         vdsrc_o = RDSRC_NONE;
         aluop_o = ALUOP_ADD;
 
-        if (rs1_o == 0 && funct3_o == 0 && rd_o == 0 && opcode_o == 'b1110011) begin
+        if (opcode_o == 'b1110011) begin
+            case (funct3_o)
+            0: begin
+                if (rd_o == 0 && rs1_o == 0) begin
                     case (instruction_i[31:20])
                     0: raise_ecall_o = 1;
                     1: raise_ebreak_o = 1;
+                    'h302: if (rs2_o == 2) begin
+                        exception_return_o = 1;
+                    end else begin
+                        instruction_fault_o = 1;
+                    end
                     default: instruction_fault_o = 1;
                     endcase
+                end else begin
+                    instruction_fault_o = 1;
+                end
+            end
+            4: instruction_fault_o = 1;
+            default: begin
+                instruction_zicsr_o = 1;
+                instruction_multi_cycle_o = 1;
+                vdsrc_o = RDSRC_CSR;
+
+                imm_o = { 20'b0, instruction_i[31:20] };
+            end
+            endcase
+
+            if (instruction_fault_o) begin
+                $display("instruction_fault_o on SYSTEM");
+                $display("\timm=%b, rs2=%d, rs1=%d, funct3=%b, rd=%d", instruction_i[31:20], rs2_o, rs1_o, funct3_o, rd_o);
+            end
         end else if (rs1_o == 0 && funct3_o == 0 && opcode_o == 'b0001111) begin
-            // Nice
+            // Nothing
         end else begin
             instruction_fault_o = base_instruction_fault;
 

src/decoder/rvx0_decoder_i.sv

@@ -19,7 +19,7 @@ module rvx0_decoder_i (
     output wire [31:0]          imm_o,
     output wire                 v1src_o,
     output wire [1:0]           v2src_o,
-    output wire [1:0]           vdsrc_o,
+    output wire [2:0]           vdsrc_o,
     output wire [3:0]           aluop_o
 );
     always_comb begin

src/rvx0_defs.svh

@@ -1,13 +1,18 @@
 `ifndef __RVX0_DEFS_SVH
 `define __RVX0_DEFS_SVH
 
-`define RVX0_DEBUGGING  1
+// `define RVX0_DEBUGGING  1
 // `define RVX0_CRASH_ON_EXCEPTION 1
+`define RVX0_ZICSR      1
 
-const logic [1:0]   RDSRC_ALU = 0;
+const logic [2:0]   RDSRC_NONE  = 0;
-const logic [1:0]   RDSRC_MEM = 1;
+const logic [2:0]   RDSRC_ALU   = 1;
-const logic [1:0]   RDSRC_PC = 2;
+const logic [2:0]   RDSRC_MEM   = 2;
-const logic [1:0]   RDSRC_NONE = 3;
+const logic [2:0]   RDSRC_PC    = 3;
+const logic [2:0]   RDSRC_CSR   = 4;
+const logic [2:0]   RDSRC_MALU  = 5; // TODO will be used for M extension
+const logic [2:0]   RDSRC_RES2  = 6;
+const logic [2:0]   RDSRC_RES3  = 7;
 
 const logic         V1SRC_ZERO = 0;
 const logic         V1SRC_RS1 = 1;

src/rvx0_simple_ram.sv

@@ -10,10 +10,10 @@ module rvx0_simple_ram (
             if (apb_slave.penable) begin
                 if (apb_slave.pwrite) begin
                     words[apb_slave.paddr[22:2]] <= apb_slave.pwdata;
-                    $display("RAM write [%x] <= %x", apb_slave.paddr, apb_slave.pwdata);
+                    // $display("RAM write [%x] <= %x", apb_slave.paddr, apb_slave.pwdata);
                 end else begin
                     apb_slave.prdata <= words[apb_slave.paddr[22:2]];
-                    $display("RAM read %x <= [%x]", words[apb_slave.paddr[22:2]], apb_slave.paddr);
+                    // $display("RAM read %x <= [%x]", words[apb_slave.paddr[22:2]], apb_slave.paddr);
                 end
 
                 apb_slave.pready <= 1;