Merge remote-tracking branch 'rv64cpu-import/master'

9 files changed, 1807 insertions, 0 deletions

diff --git a/rv64cpu/rtl/core/pipeline/ex_mem_reg.sv b/rv64cpu/rtl/core/pipeline/ex_mem_reg.sv
new file mode 100644
index 0000000..b8e5b9e
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/ex_mem_reg.sv
@@ -0,0 +1,281 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/ex_mem_reg.sv
+// Description: Execute to Memory Pipeline Register
+//              Latches results from the EX stage and bypassed control signals from ID/EX.
+//              Supports stall and flush operations.
+
+module ex_mem_reg
+    import rv64_pkg::*;
+(
+    input  logic                   clk,
+    input  logic                   rst,
+
+    // Pipeline Control
+    input  logic                   stall_en,
+    input  logic                   flush_en,
+
+    // ----------------------
+    // Inputs from EX Stage (computed results)
+    input  logic [XLEN-1:0]        ex_alu_result,
+    input  logic                   ex_branch_taken,
+    input  logic [XLEN-1:0]        ex_branch_target,
+
+    // ----------------------
+    // Bypassed Inputs from ID/EX Register
+
+    // Data/Operands
+    input  logic [XLEN-1:0]        ex_rs1_rdata,  // Needed for CSRRW/RS/RC
+    input  logic [XLEN-1:0]        ex_rs2_rdata,  // Needed for store data
+    input  logic [XLEN-1:0]        ex_fs1_rdata,
+    input  logic [XLEN-1:0]        ex_fs2_rdata,
+    input  logic [XLEN-1:0]        ex_fs3_rdata,
+    input  logic [4:0]             ex_rs1_addr,   // Serves as zimm for CSRRWI/SI/CI
+    input  logic [4:0]             ex_fs3_addr,
+    input  logic [XLEN-1:0]        ex_pc,         // PC passed along for exceptions/CSRs
+    input  logic [XLEN-1:0]        ex_imm,        // Imm passed for CSR writes
+    input  logic [INST_WIDTH-1:0]  ex_instr,
+    input  logic [2:0]             ex_instr_len,
+
+    // Memory Control
+    input  logic                   ex_is_load,
+    input  logic                   ex_is_store,
+    input  logic [2:0]             ex_mem_size,
+    input  logic                   ex_is_amo,
+    input  amo_op_t                ex_amo_op,
+    input  logic                   ex_is_fp,
+    input  logic                   ex_fp_reg_write_en,
+    input  logic                   ex_fp_store_data_sel,
+    input  fp_op_t                 ex_fp_op,
+    input  fp_fmt_t                ex_fp_fmt,
+    input  logic [2:0]             ex_fp_rm,
+    input  logic [XLEN-1:0]        ex_fp_result,
+    input  logic [4:0]             ex_fp_fflags,
+
+    // Writeback Control
+    input  logic                   ex_reg_write_en,
+    input  logic [4:0]             ex_rd_addr,
+    input  logic [1:0]             ex_wb_sel,
+
+    // System / Exception Control
+    input  logic                   ex_is_csr,
+    input  logic [2:0]             ex_csr_op,
+    input  logic                   ex_illegal_instr,
+    input  logic                   ex_instr_access_fault,
+    input  logic                   ex_instr_page_fault,
+    input  logic                   ex_is_ecall,
+    input  logic                   ex_is_ebreak,
+    input  logic                   ex_is_mret,
+    input  logic                   ex_is_sret,
+    input  logic                   ex_is_wfi,
+    input  logic                   ex_is_sfence_vma,
+    input  logic                   ex_is_fence,
+    input  logic                   ex_is_fence_i,
+
+
+    // ----------------------
+    // Outputs to MEM Stage
+
+    // Data/Operands
+    output logic [XLEN-1:0]        mem_alu_result,
+    output logic                   mem_branch_taken,
+    output logic [XLEN-1:0]        mem_branch_target,
+    output logic [XLEN-1:0]        mem_rs1_rdata,
+    output logic [XLEN-1:0]        mem_rs2_rdata,
+    output logic [XLEN-1:0]        mem_fs1_rdata,
+    output logic [XLEN-1:0]        mem_fs2_rdata,
+    output logic [XLEN-1:0]        mem_fs3_rdata,
+    output logic [4:0]             mem_rs1_addr,
+    output logic [4:0]             mem_fs3_addr,
+    output logic [XLEN-1:0]        mem_pc,
+    output logic [XLEN-1:0]        mem_imm,
+    output logic [INST_WIDTH-1:0]  mem_instr,
+    output logic [2:0]             mem_instr_len,
+
+    // Memory Control
+    output logic                   mem_is_load,
+    output logic                   mem_is_store,
+    output logic [2:0]             mem_mem_size,
+    output logic                   mem_is_amo,
+    output amo_op_t                mem_amo_op,
+    output logic                   mem_is_fp,
+    output logic                   mem_fp_reg_write_en,
+    output logic                   mem_fp_store_data_sel,
+    output fp_op_t                 mem_fp_op,
+    output fp_fmt_t                mem_fp_fmt,
+    output logic [2:0]             mem_fp_rm,
+    output logic [XLEN-1:0]        mem_fp_result,
+    output logic [4:0]             mem_fp_fflags,
+
+    // Writeback Control
+    output logic                   mem_reg_write_en,
+    output logic [4:0]             mem_rd_addr,
+    output logic [1:0]             mem_wb_sel,
+
+    // System / Exception Control
+    output logic                   mem_is_csr,
+    output logic [2:0]             mem_csr_op,
+    output logic                   mem_illegal_instr,
+    output logic                   mem_instr_access_fault,
+    output logic                   mem_instr_page_fault,
+    output logic                   mem_is_ecall,
+    output logic                   mem_is_ebreak,
+    output logic                   mem_is_mret,
+    output logic                   mem_is_sret,
+    output logic                   mem_is_wfi,
+    output logic                   mem_is_sfence_vma,
+    output logic                   mem_is_fence,
+    output logic                   mem_is_fence_i
+);
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            mem_instr        <= '0;
+            mem_instr_len      <= 3'd4;
+            mem_alu_result    <= '0;
+            mem_branch_taken  <= '0;
+            mem_branch_target <= '0;
+            mem_rs1_rdata     <= '0;
+            mem_rs2_rdata     <= '0;
+            mem_fs1_rdata     <= '0;
+            mem_fs2_rdata     <= '0;
+            mem_fs3_rdata     <= '0;
+            mem_rs1_addr      <= '0;
+            mem_fs3_addr      <= '0;
+            mem_pc            <= '0;
+            mem_imm           <= '0;
+
+            // Critical control signals must be zeroed out
+            mem_is_load       <= '0;
+            mem_is_store      <= '0;
+            mem_mem_size      <= '0;
+            mem_is_amo        <= '0;
+            mem_amo_op        <= AMO_NONE;
+            mem_is_fp         <= '0;
+            mem_fp_reg_write_en <= '0;
+            mem_fp_store_data_sel <= '0;
+            mem_fp_op         <= FP_OP_NONE;
+            mem_fp_fmt        <= FP_FMT_S;
+            mem_fp_rm         <= '0;
+            mem_fp_result     <= '0;
+            mem_fp_fflags     <= '0;
+            mem_reg_write_en  <= '0;
+            mem_rd_addr       <= '0;
+            mem_wb_sel        <= '0;
+
+            // Exceptions and system control
+            mem_is_csr             <= '0;
+            mem_csr_op             <= '0;
+            mem_illegal_instr      <= '0;
+            mem_instr_access_fault <= '0;
+            mem_instr_page_fault   <= '0;
+            mem_is_ecall           <= '0;
+            mem_is_ebreak          <= '0;
+            mem_is_mret            <= '0;
+            mem_is_sret            <= '0;
+            mem_is_wfi             <= '0;
+            mem_is_sfence_vma      <= '0;
+            mem_is_fence           <= '0;
+            mem_is_fence_i         <= '0;
+
+        end else if (flush_en) begin
+            mem_instr        <= '0;
+            mem_instr_len      <= 3'd4;
+            // Critical control signals must be zeroed out
+            mem_branch_taken  <= '0;
+            mem_is_load       <= '0;
+            mem_is_store      <= '0;
+            mem_mem_size      <= '0;
+            mem_is_amo        <= '0;
+            mem_amo_op        <= AMO_NONE;
+            mem_is_fp         <= '0;
+            mem_fp_reg_write_en <= '0;
+            mem_fp_store_data_sel <= '0;
+            mem_fp_op         <= FP_OP_NONE;
+            mem_fp_fmt        <= FP_FMT_S;
+            mem_fp_rm         <= '0;
+            mem_fp_result     <= '0;
+            mem_fp_fflags     <= '0;
+            mem_reg_write_en  <= '0;
+            mem_rd_addr       <= '0;
+            mem_wb_sel        <= '0;
+
+            // Exceptions and system control
+            mem_is_csr             <= '0;
+            mem_csr_op             <= '0;
+            mem_illegal_instr      <= '0;
+            mem_instr_access_fault <= '0;
+            mem_instr_page_fault   <= '0;
+            mem_is_ecall           <= '0;
+            mem_is_ebreak          <= '0;
+            mem_is_mret            <= '0;
+            mem_is_sret            <= '0;
+            mem_is_wfi             <= '0;
+            mem_is_sfence_vma      <= '0;
+            mem_is_fence           <= '0;
+            mem_is_fence_i         <= '0;
+        end else if (~stall_en) begin
+            mem_alu_result         <= ex_alu_result;
+            mem_branch_taken       <= ex_branch_taken;
+            mem_branch_target      <= ex_branch_target;
+            mem_rs1_rdata          <= ex_rs1_rdata;
+            mem_rs2_rdata          <= ex_rs2_rdata;
+            mem_fs1_rdata          <= ex_fs1_rdata;
+            mem_fs2_rdata          <= ex_fs2_rdata;
+            mem_fs3_rdata          <= ex_fs3_rdata;
+            mem_rs1_addr           <= ex_rs1_addr;
+            mem_fs3_addr           <= ex_fs3_addr;
+            mem_pc                 <= ex_pc;
+            mem_imm                <= ex_imm;
+            mem_instr              <= ex_instr;
+            mem_instr_len          <= ex_instr_len;
+
+            mem_is_load            <= ex_is_load;
+            mem_is_store           <= ex_is_store;
+            mem_mem_size           <= ex_mem_size;
+            mem_is_amo             <= ex_is_amo;
+            mem_amo_op             <= ex_amo_op;
+            mem_is_fp              <= ex_is_fp;
+            mem_fp_reg_write_en    <= ex_fp_reg_write_en;
+            mem_fp_store_data_sel  <= ex_fp_store_data_sel;
+            mem_fp_op              <= ex_fp_op;
+            mem_fp_fmt             <= ex_fp_fmt;
+            mem_fp_rm              <= ex_fp_rm;
+            mem_fp_result          <= ex_fp_result;
+            mem_fp_fflags          <= ex_fp_fflags;
+            mem_reg_write_en       <= ex_reg_write_en;
+            mem_rd_addr            <= ex_rd_addr;
+            mem_wb_sel             <= ex_wb_sel;
+
+            mem_is_csr             <= ex_is_csr;
+            mem_csr_op             <= ex_csr_op;
+            mem_illegal_instr      <= ex_illegal_instr;
+            mem_instr_access_fault <= ex_instr_access_fault;
+            mem_instr_page_fault   <= ex_instr_page_fault;
+            mem_is_ecall           <= ex_is_ecall;
+            mem_is_ebreak          <= ex_is_ebreak;
+            mem_is_mret            <= ex_is_mret;
+            mem_is_sret            <= ex_is_sret;
+            mem_is_wfi             <= ex_is_wfi;
+            mem_is_sfence_vma      <= ex_is_sfence_vma;
+            mem_is_fence           <= ex_is_fence;
+            mem_is_fence_i         <= ex_is_fence_i;
+        end
+        // If stall_en && !flush_en, maintain current state
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/ex_stage.sv b/rv64cpu/rtl/core/pipeline/ex_stage.sv
new file mode 100644
index 0000000..2758ffc
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/ex_stage.sv
@@ -0,0 +1,120 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/ex_stage.sv
+// Description: Execute Stage
+//              Performs ALU operations, branch/jump calculations, and generates memory addresses.
+
+module ex_stage
+    import rv64_pkg::*;
+(
+    // Input from ID/EX level pipeline register
+    input  logic [XLEN-1:0]    ex_pc,
+    input  logic [XLEN-1:0]    ex_rs1_rdata,
+    input  logic [XLEN-1:0]    ex_rs2_rdata,
+    input  logic [XLEN-1:0]    ex_imm,
+
+    // Execution Control Signals
+    input  alu_op_t            ex_alu_op,
+    input  logic               ex_alu_src1_sel,    // 0: rs1_rdata, 1: PC
+    input  logic               ex_alu_src2_sel,    // 0: rs2_rdata, 1: imm
+    input  logic               ex_is_branch,
+    input  branch_op_t         ex_branch_op,
+    input  logic               ex_is_jump,
+    input  logic               ex_is_word_op,
+
+    // Output to EX/MEM pipeline register & hazard unit
+    output logic [XLEN-1:0]    ex_alu_result,
+    output logic               ex_branch_taken,
+    output logic [XLEN-1:0]    ex_branch_target
+);
+
+    logic [XLEN-1:0]    alu_mux_a;
+    logic [XLEN-1:0]    alu_mux_b;
+    logic [XLEN-1:0]    alu_operand_a;
+    logic [XLEN-1:0]    alu_operand_b;
+    logic [XLEN-1:0]    alu_raw_result;
+    logic               branch_unit_taken;
+
+    // -------------
+    // ALU Input Multiplexers (MUX)
+    assign alu_mux_a = (ex_alu_src1_sel == 1'b1) ? ex_pc : ex_rs1_rdata;
+    assign alu_mux_b = (ex_alu_src2_sel == 1'b1) ? ex_imm : ex_rs2_rdata;
+    
+    // -------------
+    // RV64 Word Operation (32-bit) Pre-conditioning
+    // RV64 requires word operations (ADDW/SLLW/SRLW/SRAW) to operate on the 
+    // lower 32 bits and sign-extend the result.
+    always_comb begin
+        if (ex_is_word_op) begin
+            // Shift operations require specific 32-bit extension forms
+            if (ex_alu_op == ALU_SRA)
+                alu_operand_a = { {32{alu_mux_a[31]}}, alu_mux_a[31:0] }; // Sign extend for arithmetic shift
+            else if ((ex_alu_op == ALU_SRL) || (ex_alu_op == ALU_DIVU) || (ex_alu_op == ALU_REMU))
+                alu_operand_a = { 32'b0, alu_mux_a[31:0] };              // Zero extend for logical shift
+            else if ((ex_alu_op == ALU_DIV) || (ex_alu_op == ALU_REM))
+                alu_operand_a = { {32{alu_mux_a[31]}}, alu_mux_a[31:0] };
+            else
+                alu_operand_a = alu_mux_a;                               // ADDW/SUBW/SLLW don't care about upper bits
+            
+            // RV32 shift amount is strictly 5 bits (0-31), ignoring bit 5 which might be set in 64-bit registers
+            if (ex_alu_op == ALU_SLL || ex_alu_op == ALU_SRL || ex_alu_op == ALU_SRA)
+                alu_operand_b = { 59'b0, alu_mux_b[4:0] };
+            else if ((ex_alu_op == ALU_DIVU) || (ex_alu_op == ALU_REMU))
+                alu_operand_b = { 32'b0, alu_mux_b[31:0] };
+            else if ((ex_alu_op == ALU_DIV) || (ex_alu_op == ALU_REM))
+                alu_operand_b = { {32{alu_mux_b[31]}}, alu_mux_b[31:0] };
+            else
+                alu_operand_b = alu_mux_b;
+        end else begin
+            // 64-bit operations pass through unchanged
+            alu_operand_a = alu_mux_a;
+            alu_operand_b = alu_mux_b;
+        end
+    end
+
+    // --------------
+    // Instantiate the core computing engine ALU
+    integer_alu alu_inst (
+        .alu_op         (ex_alu_op),
+        .operand_a      (alu_operand_a),
+        .operand_b      (alu_operand_b),
+        .alu_result     (alu_raw_result)
+    );
+
+    // -------------
+    // Branch Unit
+    branch_unit bu_inst (
+        .is_branch    (ex_is_branch),
+        .branch_op    (ex_branch_op),
+        .operand_a    (ex_rs1_rdata),
+        .operand_b    (ex_rs2_rdata),
+        .branch_taken (branch_unit_taken)
+    );
+
+    // ------------- 
+    // 32-bit instruction result truncation and sign extension
+    assign ex_alu_result = ex_is_word_op ? 
+                           { {32{alu_raw_result[31]}}, alu_raw_result[31:0] } : 
+                           alu_raw_result;
+
+    // ------------- 
+    // Branch Target Calculation
+    assign ex_branch_target = alu_raw_result & ~64'b1;
+
+    assign ex_branch_taken  = ex_is_jump | (ex_is_branch & branch_unit_taken);
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/id_ex_reg.sv b/rv64cpu/rtl/core/pipeline/id_ex_reg.sv
new file mode 100644
index 0000000..777cc46
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/id_ex_reg.sv
@@ -0,0 +1,287 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/id_ex_reg.sv
+// Description: ID/EX Pipeline Register
+//              Latches control signals and data from ID stage and passes them to EX stage.
+//              Supports stall (freeze) and flush.
+
+module id_ex_reg
+    import rv64_pkg::*;
+(
+    input  logic                   clk,
+    input  logic                   rst,
+
+    // Pipeline Control
+    input  logic                   stall_en,
+    input  logic                   flush_en,
+
+
+    // Inputs from ID Stage
+
+    // Instruction payload
+    input  logic [INST_WIDTH-1:0]  id_instr,
+    input  logic [2:0]             id_instr_len,
+
+    // Data/Operands
+    input  logic [XLEN-1:0]        id_pc,
+    input  logic [XLEN-1:0]        id_rs1_rdata,
+    input  logic [XLEN-1:0]        id_rs2_rdata,
+    input  logic [XLEN-1:0]        id_fs1_rdata,
+    input  logic [XLEN-1:0]        id_fs2_rdata,
+    input  logic [XLEN-1:0]        id_fs3_rdata,
+    input  logic [4:0]             id_rs1_addr,
+    input  logic [4:0]             id_rs2_addr,
+    input  logic [4:0]             id_fs3_addr,
+    input  logic [XLEN-1:0]        id_imm,
+
+    // Execution Control
+    input  alu_op_t                id_alu_op,
+    input  logic                   id_alu_src1_sel,
+    input  logic                   id_alu_src2_sel,
+    input  logic                   id_is_word_op,
+    input  logic                   id_is_branch,
+    input  branch_op_t             id_branch_op,
+    input  logic                   id_is_jump,
+
+    // Memory & Writeback Control
+    input  logic                   id_is_load,
+    input  logic                   id_is_store,
+    input  logic [2:0]             id_mem_size,
+    input  logic                   id_is_amo,
+    input  amo_op_t                id_amo_op,
+    input  logic                   id_is_fp,
+    input  logic                   id_fp_reg_write_en,
+    input  logic                   id_fp_store_data_sel,
+    input  fp_op_t                 id_fp_op,
+    input  fp_fmt_t                id_fp_fmt,
+    input  logic [2:0]             id_fp_rm,
+    input  logic                   id_reg_write_en,
+    input  logic [4:0]             id_rd_addr,
+    input  logic [1:0]             id_wb_sel,
+
+    // System / Exception
+    input  logic                   id_is_csr,
+    input  logic [2:0]             id_csr_op,
+    input  logic                   id_illegal_instr,
+    input  logic                   id_instr_access_fault,
+    input  logic                   id_instr_page_fault,
+    input  logic                   id_is_ecall,
+    input  logic                   id_is_ebreak,
+    input  logic                   id_is_mret,
+    input  logic                   id_is_sret,
+    input  logic                   id_is_wfi,
+    input  logic                   id_is_sfence_vma,
+    input  logic                   id_is_fence,
+    input  logic                   id_is_fence_i,
+
+
+    // Outputs to EX Stage
+
+    // Instruction payload
+    output logic [INST_WIDTH-1:0]  ex_instr,
+    output logic [2:0]             ex_instr_len,
+
+    // Data/Operands
+    output logic [XLEN-1:0]        ex_pc,
+    output logic [XLEN-1:0]        ex_rs1_rdata,
+    output logic [XLEN-1:0]        ex_rs2_rdata,
+    output logic [XLEN-1:0]        ex_fs1_rdata,
+    output logic [XLEN-1:0]        ex_fs2_rdata,
+    output logic [XLEN-1:0]        ex_fs3_rdata,
+    output logic [4:0]             ex_rs1_addr,
+    output logic [4:0]             ex_rs2_addr,
+    output logic [4:0]             ex_fs3_addr,
+    output logic [XLEN-1:0]        ex_imm,
+
+    // Execution Control
+    output alu_op_t                ex_alu_op,
+    output logic                   ex_alu_src1_sel,
+    output logic                   ex_alu_src2_sel,
+    output logic                   ex_is_word_op,
+    output logic                   ex_is_branch,
+    output branch_op_t             ex_branch_op,
+    output logic                   ex_is_jump,
+
+    // Memory & Writeback Control
+    output logic                   ex_is_load,
+    output logic                   ex_is_store,
+    output logic [2:0]             ex_mem_size,
+    output logic                   ex_is_amo,
+    output amo_op_t                ex_amo_op,
+    output logic                   ex_is_fp,
+    output logic                   ex_fp_reg_write_en,
+    output logic                   ex_fp_store_data_sel,
+    output fp_op_t                 ex_fp_op,
+    output fp_fmt_t                ex_fp_fmt,
+    output logic [2:0]             ex_fp_rm,
+    output logic                   ex_reg_write_en,
+    output logic [4:0]             ex_rd_addr,
+    output logic [1:0]             ex_wb_sel,
+
+    // System / Exception
+    output logic                   ex_is_csr,
+    output logic [2:0]             ex_csr_op,
+    output logic                   ex_illegal_instr,
+    output logic                   ex_instr_access_fault,
+    output logic                   ex_instr_page_fault,
+    output logic                   ex_is_ecall,
+    output logic                   ex_is_ebreak,
+    output logic                   ex_is_mret,
+    output logic                   ex_is_sret,
+    output logic                   ex_is_wfi,
+    output logic                   ex_is_sfence_vma,
+    output logic                   ex_is_fence,
+    output logic                   ex_is_fence_i
+);
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            ex_instr         <= '0;
+            ex_instr_len     <= 3'd4;
+            ex_pc            <= '0;
+            ex_rs1_rdata     <= '0;
+            ex_rs2_rdata     <= '0;
+            ex_fs1_rdata     <= '0;
+            ex_fs2_rdata     <= '0;
+            ex_fs3_rdata     <= '0;
+            ex_rs1_addr      <= '0;
+            ex_rs2_addr      <= '0;
+            ex_fs3_addr      <= '0;
+            ex_imm           <= '0;
+            ex_alu_op        <= ALU_ADD; // NOP is effectively ADD x0, x0, 0
+            ex_alu_src1_sel  <= '0;
+            ex_alu_src2_sel  <= '0;
+            ex_is_word_op    <= '0;
+            ex_is_branch     <= '0;
+            ex_branch_op     <= BRANCH_EQ;
+            ex_is_jump       <= '0;
+            ex_is_load       <= '0;
+            ex_is_store      <= '0;
+            ex_mem_size      <= '0;
+            ex_is_amo        <= '0;
+            ex_amo_op        <= AMO_NONE;
+            ex_is_fp         <= '0;
+            ex_fp_reg_write_en <= '0;
+            ex_fp_store_data_sel <= '0;
+            ex_fp_op         <= FP_OP_NONE;
+            ex_fp_fmt        <= FP_FMT_S;
+            ex_fp_rm         <= '0;
+            ex_reg_write_en  <= '0; // VERY IMPORTANT: Do not write to regfile on NOP
+            ex_rd_addr       <= '0;
+            ex_wb_sel        <= '0;
+            ex_is_csr             <= '0;
+            ex_csr_op             <= '0;
+            ex_illegal_instr      <= '0;
+            ex_instr_access_fault <= '0;
+            ex_instr_page_fault   <= '0;
+            ex_is_ecall           <= '0;
+            ex_is_ebreak          <= '0;
+            ex_is_mret            <= '0;
+            ex_is_sret            <= '0;
+            ex_is_wfi             <= '0;
+            ex_is_sfence_vma      <= '0;
+            ex_is_fence           <= '0;
+            ex_is_fence_i         <= '0;
+        end else if (flush_en) begin
+            ex_instr         <= '0;
+            ex_instr_len     <= 3'd4;
+            ex_alu_op        <= ALU_ADD; // NOP is effectively ADD x0, x0, 0
+            ex_alu_src1_sel  <= '0;
+            ex_alu_src2_sel  <= '0;
+            ex_is_word_op    <= '0;
+            ex_is_branch     <= '0;
+            ex_branch_op     <= BRANCH_EQ;
+            ex_is_jump       <= '0;
+            ex_is_load       <= '0;
+            ex_is_store      <= '0;
+            ex_mem_size      <= '0;
+            ex_is_amo        <= '0;
+            ex_amo_op        <= AMO_NONE;
+            ex_is_fp         <= '0;
+            ex_fp_reg_write_en <= '0;
+            ex_fp_store_data_sel <= '0;
+            ex_fp_op         <= FP_OP_NONE;
+            ex_fp_fmt        <= FP_FMT_S;
+            ex_fp_rm         <= '0;
+            ex_reg_write_en  <= '0; // VERY IMPORTANT: Do not write to regfile on NOP
+            ex_rd_addr       <= '0;
+            ex_wb_sel        <= '0;
+            ex_is_csr             <= '0;
+            ex_csr_op             <= '0;
+            ex_illegal_instr      <= '0;
+            ex_instr_access_fault <= '0;
+            ex_instr_page_fault   <= '0;
+            ex_is_ecall           <= '0;
+            ex_is_ebreak          <= '0;
+            ex_is_mret            <= '0;
+            ex_is_sret            <= '0;
+            ex_is_wfi             <= '0;
+            ex_is_sfence_vma      <= '0;
+            ex_is_fence           <= '0;
+            ex_is_fence_i         <= '0;
+        end else if (~stall_en) begin
+            ex_instr         <= id_instr;
+            ex_instr_len     <= id_instr_len;
+            ex_pc            <= id_pc;
+            ex_rs1_rdata     <= id_rs1_rdata;
+            ex_rs2_rdata     <= id_rs2_rdata;
+            ex_fs1_rdata     <= id_fs1_rdata;
+            ex_fs2_rdata     <= id_fs2_rdata;
+            ex_fs3_rdata     <= id_fs3_rdata;
+            ex_rs1_addr      <= id_rs1_addr;
+            ex_rs2_addr      <= id_rs2_addr;
+            ex_fs3_addr      <= id_fs3_addr;
+            ex_imm           <= id_imm;
+            ex_alu_op        <= id_alu_op;
+            ex_alu_src1_sel  <= id_alu_src1_sel;
+            ex_alu_src2_sel  <= id_alu_src2_sel;
+            ex_is_word_op    <= id_is_word_op;
+            ex_is_branch     <= id_is_branch;
+            ex_branch_op     <= id_branch_op;
+            ex_is_jump       <= id_is_jump;
+            ex_is_load       <= id_is_load;
+            ex_is_store      <= id_is_store;
+            ex_mem_size      <= id_mem_size;
+            ex_is_amo        <= id_is_amo;
+            ex_amo_op        <= id_amo_op;
+            ex_is_fp         <= id_is_fp;
+            ex_fp_reg_write_en <= id_fp_reg_write_en;
+            ex_fp_store_data_sel <= id_fp_store_data_sel;
+            ex_fp_op         <= id_fp_op;
+            ex_fp_fmt        <= id_fp_fmt;
+            ex_fp_rm         <= id_fp_rm;
+            ex_reg_write_en  <= id_reg_write_en;
+            ex_rd_addr       <= id_rd_addr;
+            ex_wb_sel        <= id_wb_sel;
+            ex_is_csr             <= id_is_csr;
+            ex_csr_op             <= id_csr_op;
+            ex_illegal_instr      <= id_illegal_instr;
+            ex_instr_access_fault <= id_instr_access_fault;
+            ex_instr_page_fault   <= id_instr_page_fault;
+            ex_is_ecall           <= id_is_ecall;
+            ex_is_ebreak          <= id_is_ebreak;
+            ex_is_mret            <= id_is_mret;
+            ex_is_sret            <= id_is_sret;
+            ex_is_wfi             <= id_is_wfi;
+            ex_is_sfence_vma      <= id_is_sfence_vma;
+            ex_is_fence           <= id_is_fence;
+            ex_is_fence_i         <= id_is_fence_i;
+        end
+        // If stall_en && !flush_en, maintain current state
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/id_stage.sv b/rv64cpu/rtl/core/pipeline/id_stage.sv
new file mode 100644
index 0000000..1cf447a
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/id_stage.sv
@@ -0,0 +1,197 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/id_stage.sv
+// Description: Instruction Decode Stage, instantiate the Decoder and RegFile, extract control signals and operands
+
+module id_stage
+    import rv64_pkg::*;
+(
+    input  logic                   clk,
+    input  logic                   rst,
+
+    // Inputs from IF Stage (via IF/ID Pipeline Register)
+    input  logic [XLEN-1:0]        if_pc,        // PC of the current instruction
+    input  logic [INST_WIDTH-1:0]  if_instr,     // The 32-bit instruction fetched
+    input  logic [2:0]             if_instr_len,
+    input  logic [1:0]             csr_priv_mode, // Current privilege mode from CSR
+    input  logic                   csr_mstatus_tsr,
+    input  logic                   csr_mstatus_tw,
+    input  logic                   csr_mstatus_tvm,
+
+    // Write-back Inputs from WB Stage (to RegFile)
+    input  logic                   wb_reg_write_en, // Register write enable from WB stage
+    input  logic [4:0]             wb_rd_addr,     // Destination register address from WB
+    input  logic [XLEN-1:0]        wb_rd_wdata,    // Data to write back to register
+    input  logic                   wb_fp_write_en,
+    input  logic [4:0]             wb_fd_addr,
+    input  logic [XLEN-1:0]        wb_fd_wdata,
+    
+    // Outputs to EX Stage
+    // (Notice the 'id_' prefix to signify signals leaving the ID stage)
+    // Data/Operands
+    output logic [XLEN-1:0]        id_pc,          // Pass-through PC for branch/jump calculation
+    output logic [2:0]             id_instr_len,
+    output logic [XLEN-1:0]        id_rs1_rdata,   // Source register 1 data
+    output logic [XLEN-1:0]        id_rs2_rdata,   // Source register 2 data
+    output logic [XLEN-1:0]        id_fs1_rdata,
+    output logic [XLEN-1:0]        id_fs2_rdata,
+    output logic [XLEN-1:0]        id_fs3_rdata,
+    output logic [4:0]             id_rs1_addr,    // Rs1 index (for Forwarding & CSR zimm)
+    output logic [4:0]             id_rs2_addr,    // Rs2 index (for Forwarding)
+    output logic [4:0]             id_fs3_addr,
+    output logic [XLEN-1:0]        id_imm,         // Sign-extended generic immediate
+
+    // Execution Control (ALU & Branch)
+    output alu_op_t                id_alu_op,      // ALU operation code
+    output logic                   id_alu_src1_sel,// 0: rs1_rdata, 1: PC
+    output logic                   id_alu_src2_sel,// 0: rs2_rdata, 1: imm
+    output logic                   id_is_word_op,  // 1: RV64 32-bit operation (e.g. ADDW)
+    output logic                   id_is_branch,   // 1: B-type branch instruction
+    output branch_op_t             id_branch_op,   // Specific condition for branch (BEQ, BNE...)
+    output logic                   id_is_jump,     // 1: JAL or JALR
+
+    // Memory & Writeback Control (Passed down to MEM/WB stages eventually)
+    output logic                   id_is_load,     // 1: Load instruction
+    output logic                   id_is_store,    // 1: Store instruction
+    output logic [2:0]             id_mem_size,    // Memory access size (Byte/Half/Word/Double)
+    output logic                   id_is_amo,
+    output amo_op_t                id_amo_op,
+    output logic                   id_is_fp,
+    output logic                   id_fp_reg_write_en,
+    output logic                   id_fp_store_data_sel,
+    output fp_op_t                 id_fp_op,
+    output fp_fmt_t                id_fp_fmt,
+    output logic [2:0]             id_fp_rm,
+    output logic                   id_reg_write_en,// 1: Instruction will write to a register
+    output logic [4:0]             id_rd_addr,     // Destination register index
+    output logic [1:0]             id_wb_sel,      // Writeback source (0: ALU, 1: MEM, 2: PC+4)
+
+    // System / Exception
+    output logic                   id_is_csr,
+    output logic [2:0]             id_csr_op,
+    output logic                   id_illegal_instr,
+    output logic                   id_is_ecall,
+    output logic                   id_is_ebreak,
+    output logic                   id_is_mret,
+    output logic                   id_is_sret,
+    output logic                   id_is_wfi,
+    output logic                   id_is_sfence_vma,
+    output logic                   id_is_fence,
+    output logic                   id_is_fence_i
+);
+    // Internal Wires for connecting sub-modules
+    logic [4:0] rs1_addr;
+    logic [4:0] rs2_addr;
+    logic [4:0] fs3_addr;
+
+    // Pass-through PC and reg indices directly to next stage
+    assign id_pc = if_pc;
+    assign id_instr_len = if_instr_len;
+    assign id_rs1_addr = rs1_addr;
+    assign id_rs2_addr = rs2_addr;
+    assign fs3_addr = if_instr[31:27];
+    assign id_fs3_addr = fs3_addr;
+
+    // Instruction Decoder Instantiation
+
+    instruction_decoder u_decoder (
+        .instr          (if_instr),
+        .priv_mode      (csr_priv_mode),
+        .mstatus_tsr    (csr_mstatus_tsr),
+        .mstatus_tw     (csr_mstatus_tw),
+        .mstatus_tvm    (csr_mstatus_tvm),
+
+        // Outputs to RegFile read ports
+        .rs1_addr       (rs1_addr),
+        .rs2_addr       (rs2_addr),
+        .rd_addr        (id_rd_addr),
+
+        // Outputs to EX stage (ALU & execution control)
+        .alu_op         (id_alu_op),
+        .alu_src1_sel   (id_alu_src1_sel),
+        .alu_src2_sel   (id_alu_src2_sel),
+        .is_word_op     (id_is_word_op),
+        .imm            (id_imm),
+        
+        // Branch & Jump control
+        .is_branch      (id_is_branch),
+        .branch_op      (id_branch_op),
+        .is_jump        (id_is_jump),
+        
+        // Memory control
+        .is_load        (id_is_load),
+        .is_store       (id_is_store),
+        .mem_size       (id_mem_size),
+        .is_amo         (id_is_amo),
+        .amo_op         (id_amo_op),
+        .is_fp          (id_is_fp),
+        .fp_reg_write_en(id_fp_reg_write_en),
+        .fp_store_data_sel(id_fp_store_data_sel),
+        .fp_op          (id_fp_op),
+        .fp_fmt         (id_fp_fmt),
+        .fp_rm          (id_fp_rm),
+        
+        // Writeback control
+        .reg_write_en   (id_reg_write_en),
+        .wb_sel         (id_wb_sel),
+        
+        // System Control
+        .is_csr         (id_is_csr),
+        .csr_op         (id_csr_op),
+        .is_ecall       (id_is_ecall),
+        .is_ebreak      (id_is_ebreak),
+        .is_mret        (id_is_mret),
+        .is_sret        (id_is_sret),
+        .is_wfi         (id_is_wfi),
+        .is_sfence_vma  (id_is_sfence_vma),
+        .is_fence       (id_is_fence),
+        .is_fence_i     (id_is_fence_i),
+        .illegal_instr  (id_illegal_instr)
+    );
+
+    // Register File Instantiation
+    gpr_file u_regfile (
+        .clk            (clk),
+        .rst            (rst),
+        
+        // Read ports (from Decoder)
+        .rs1_addr       (rs1_addr),
+        .rs2_addr       (rs2_addr),
+        .rs1_rdata      (id_rs1_rdata),
+        .rs2_rdata      (id_rs2_rdata),
+        
+        // Write port (from WB stage)
+        .reg_write_en   (wb_reg_write_en),
+        .rd_addr        (wb_rd_addr),
+        .rd_wdata       (wb_rd_wdata)
+    );
+
+    fpr_file u_fp_regfile (
+        .clk            (clk),
+        .rst            (rst),
+        .fs1_addr       (rs1_addr),
+        .fs2_addr       (rs2_addr),
+        .fs3_addr       (fs3_addr),
+        .fs1_rdata      (id_fs1_rdata),
+        .fs2_rdata      (id_fs2_rdata),
+        .fs3_rdata      (id_fs3_rdata),
+        .fp_write_en    (wb_fp_write_en),
+        .fd_addr        (wb_fd_addr),
+        .fd_wdata       (wb_fd_wdata)
+    );
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/if_id_reg.sv b/rv64cpu/rtl/core/pipeline/if_id_reg.sv
new file mode 100644
index 0000000..0ea4fae
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/if_id_reg.sv
@@ -0,0 +1,72 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/if_id_reg.sv
+// Description: IF/ID Pipeline Register
+//              Latches PC and instruction from IF stage and passes them to ID stage.
+//              Supports stall (freeze) and flush (insert NOP).
+
+module if_id_reg
+    import rv64_pkg::*;
+(
+    input  logic                   clk,
+    input  logic                   rst,
+
+    // Pipeline Control from Hazard Unit
+    input  logic                   stall_en,   // 1: Freeze pipeline register state
+    input  logic                   flush_en,   // 1: Clear register (insert NOP)
+
+    // Inputs from IF Stage
+    input  logic [XLEN-1:0]        if_pc,      // PC from IF stage
+    input  logic [INST_WIDTH-1:0]  if_instr,   // Fetched instruction from IF stage
+    input  logic [2:0]             if_instr_len,
+    input  logic                   if_instr_access_fault,
+    input  logic                   if_instr_page_fault,
+
+    // Outputs to ID Stage
+    output logic [XLEN-1:0]        id_pc,      // Passed to ID stage
+    output logic [INST_WIDTH-1:0]  id_instr,   // Passed to ID stage
+    output logic [2:0]             id_instr_len,
+    output logic                   id_instr_access_fault,
+    output logic                   id_instr_page_fault
+);
+
+    localparam logic [31:0] NOP_INSTR = 32'h0000_0013;
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            id_pc                  <= '0;
+            id_instr               <= NOP_INSTR;
+            id_instr_len           <= 3'd4;
+            id_instr_access_fault <= 1'b0;
+            id_instr_page_fault   <= 1'b0;
+        end else if (flush_en) begin
+            id_pc                  <= '0;
+            id_instr               <= NOP_INSTR;
+            id_instr_len           <= 3'd4;
+            id_instr_access_fault <= 1'b0;
+            id_instr_page_fault   <= 1'b0;
+        end else if (~stall_en) begin
+            id_pc                  <= if_pc;
+            id_instr               <= if_instr;
+            id_instr_len           <= if_instr_len;
+            id_instr_access_fault <= if_instr_access_fault;
+            id_instr_page_fault   <= if_instr_page_fault;
+        end
+        // If stall_en && !flush_en, maintain current state
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/if_stage.sv b/rv64cpu/rtl/core/pipeline/if_stage.sv
new file mode 100644
index 0000000..c3d417b
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/if_stage.sv
@@ -0,0 +1,64 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/if_stage.sv
+// Description: Instruction Fetching Stage, includes PC registers and next-state PC calculation logic
+
+module if_stage 
+    import rv64_pkg::*;
+(
+    input logic                clk,
+    input logic                rst,
+
+    // Pipeline Control Signals (from Hazard Unit & Trap Controller)
+    input logic                stall_en,          // Freeze PC (stall), 0: Normal operation
+    input logic                pc_redirect_sel,   // Branch/Trap redirect request
+    input logic [XLEN-1:0]     pc_redirect_target,// Redirect target address
+    input logic                pc_redirect_is_trap, // 1: Redirect request comes from trap controller
+    input logic                if_instr_compressed,
+    
+    output logic [XLEN-1:0]    if_pc
+);
+
+    // PC Register
+    logic [XLEN-1:0] pc_reg;
+
+    // Combinational logic signal: the PC value to be written in the next clock cycle
+    logic [XLEN-1:0] next_pc;
+
+    always_comb begin
+        next_pc = pc_reg + (if_instr_compressed ? 64'd2 : 64'd4);
+    end
+
+    // Sequential Logic: Update PC Register
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            pc_reg <= 64'h0000_0000_8000_0000;
+        end else if (pc_redirect_sel && pc_redirect_is_trap) begin
+            pc_reg <= pc_redirect_target;
+        end else if (~stall_en) begin
+            if (pc_redirect_sel) begin
+                pc_reg <= pc_redirect_target;
+            end else begin
+                pc_reg <= next_pc;
+            end
+        end
+    end
+    
+    // Output wiring
+    assign if_pc = pc_reg;
+    
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/mem_stage.sv b/rv64cpu/rtl/core/pipeline/mem_stage.sv
new file mode 100644
index 0000000..d5a97dc
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/mem_stage.sv
@@ -0,0 +1,430 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/mem_stage.sv
+// Description: Memory Stage
+//              Latches results from the EX stage and bypassed control signals from ID/EX.
+//              Supports stall and flush operations.
+
+module mem_stage
+    import rv64_pkg::*;
+(
+    input  logic                   clk,
+    input  logic                   rst,
+
+    // Inputs from EX/MEM pipeline register
+    input  logic [XLEN-1:0]        mem_alu_result,    // Used as memory address or ALU result
+    input  logic                   mem_branch_taken,
+    input  logic [XLEN-1:0]        mem_branch_target,
+    input  logic [XLEN-1:0]        mem_rs1_rdata,
+    input  logic [XLEN-1:0]        mem_rs2_rdata,     // Store data
+    input  logic [XLEN-1:0]        mem_fs2_rdata,
+    input  logic [4:0]             mem_rs1_addr,
+    input  logic [XLEN-1:0]        mem_pc,
+    input  logic [XLEN-1:0]        mem_imm,
+    input  logic [INST_WIDTH-1:0]  mem_instr,
+    input  logic [2:0]             mem_instr_len,
+
+    // Memory Control
+    input  logic                   mem_is_load,
+    input  logic                   mem_is_store,
+    input  logic [2:0]             mem_mem_size,      // LB/LH/LW/LD/LBU/LHU/LWU
+    input  logic                   mem_is_amo,
+    input  amo_op_t                mem_amo_op,
+    input  logic                   mem_is_fp,
+    input  logic                   mem_fp_reg_write_en,
+    input  logic                   mem_fp_store_data_sel,
+    input  logic [XLEN-1:0]        mem_fp_result,
+    input  logic [4:0]             mem_fp_fflags,
+
+    // Writeback Control
+    input  logic                   mem_reg_write_en,
+    input  logic [4:0]             mem_rd_addr,
+    input  logic [1:0]             mem_wb_sel,
+
+    // Exceptions and System (bypassed through or generated)
+    input  logic                   mem_is_csr,
+    input  logic [2:0]             mem_csr_op,
+    input  logic                   mem_illegal_instr,
+    input  logic                   mem_instr_access_fault,
+    input  logic                   mem_instr_page_fault,
+    input  logic                   mem_is_ecall,
+    input  logic                   mem_is_ebreak,
+    input  logic                   mem_is_mret,
+    input  logic                   mem_is_sret,
+    input  logic                   mem_is_wfi,
+    input  logic                   mem_is_sfence_vma,
+    input  logic                   mem_is_fence,
+    input  logic                   mem_is_fence_i,
+
+    // Data Memory Interface (to Data Cache / Memory Controller)
+    output logic                   dmem_read_req,
+    output logic                   dmem_write_req,
+    output logic [XLEN-1:0]        dmem_addr,
+    output logic [XLEN-1:0]        dmem_wdata,
+    output logic [7:0]             dmem_wstrb,        // Write strobe depending on size
+    input  logic                   dmem_req_ready,    // Wait signal from D-Cache
+    input  logic                   dmem_rsp_valid,    // Data valid from D-Cache
+    input  logic [XLEN-1:0]        dmem_rdata,
+    input  logic                   dmem_load_access_fault,
+    input  logic                   dmem_store_access_fault,
+    input  logic                   dmem_load_page_fault,
+    input  logic                   dmem_store_page_fault,
+
+    // Outputs to MEM/WB Pipeline Register
+    output logic [XLEN-1:0]        mem_wb_alu_result,
+    output logic [XLEN-1:0]        mem_wb_mem_rdata,
+    output logic [XLEN-1:0]        mem_wb_fp_result,
+    output logic [4:0]             mem_wb_fp_fflags,
+    output logic [XLEN-1:0]        mem_wb_pc,
+    output logic [INST_WIDTH-1:0]  mem_wb_instr,
+    output logic [2:0]             mem_wb_instr_len,
+
+    output logic                   mem_wb_reg_write_en,
+    output logic                   mem_wb_fp_reg_write_en,
+    output logic [4:0]             mem_wb_rd_addr,
+    output logic [1:0]             mem_wb_wb_sel,
+
+    output logic                   mem_wb_is_csr,
+    output logic [2:0]             mem_wb_csr_op,
+    output logic [XLEN-1:0]        mem_wb_rs1_rdata,
+    output logic [4:0]             mem_wb_rs1_addr,
+    output logic [XLEN-1:0]        mem_wb_imm,
+
+    output logic                   mem_wb_illegal_instr,
+    output logic                   mem_wb_instr_access_fault,
+    output logic                   mem_wb_instr_page_fault,
+    output logic                   mem_wb_is_ecall,
+    output logic                   mem_wb_is_ebreak,
+    output logic                   mem_wb_is_mret,
+    output logic                   mem_wb_is_sret,
+    output logic                   mem_wb_load_fault,
+    output logic                   mem_wb_store_fault,
+    output logic                   mem_wb_load_page_fault,
+    output logic                   mem_wb_store_page_fault,
+    output logic [XLEN-1:0]        mem_wb_fault_addr,
+
+    // Control to Hazard Unit / Stall Logic
+    output logic                   mem_stall_req      // Stall pipeline if D-Cache doesn't respond
+);
+
+    logic [XLEN-1:0] load_data_aligned;
+    logic [XLEN-1:0] store_data_aligned;
+    logic [XLEN-1:0] amo_write_data;
+    logic [XLEN-1:0] amo_wb_data;
+    logic [XLEN-1:0] selected_store_data;
+    logic [XLEN-1:0] fp_load_data;
+    logic [7:0]      write_strobe;
+    logic            load_addr_misaligned;
+    logic            store_addr_misaligned;
+    logic            amo_is_lr;
+    logic            amo_is_sc;
+    logic            amo_is_word;
+    logic            amo_is_double;
+    logic            amo_reads_memory;
+    logic            amo_writes_memory;
+    logic            amo_store_access;
+    logic            amo_load_access;
+    logic            amo_addr_misaligned;
+    logic            amo_sc_success;
+    logic            amo_rmw_op;
+    logic            amo_rmw_read_phase;
+    logic            amo_rmw_write_phase;
+    logic [XLEN-1:0] amo_memory_rdata;
+    logic            amo_rmw_busy_q;
+    logic [XLEN-1:0] amo_rmw_old_data_q;
+    logic            reservation_valid_q;
+    logic [XLEN-1:0] reservation_addr_q;
+
+    function automatic logic addr_misaligned(
+        input logic [2:0]      size,
+        input logic [XLEN-1:0] addr
+    );
+        logic result;
+        case (size)
+            3'b000, 3'b100: result = 1'b0;         // Byte L/S
+            3'b001, 3'b101: result = addr[0];      // Halfword requires bit0 = 0
+            3'b010, 3'b110: result = |addr[1:0];   // Word requires bits[1:0] = 0
+            3'b011:          result = |addr[2:0];   // Doubleword requires bits[2:0] = 0
+            default:         result = 1'b0;
+        endcase
+        return result;
+    endfunction
+
+    assign amo_is_lr          = mem_is_amo && (mem_amo_op == AMO_LR);
+    assign amo_is_sc          = mem_is_amo && (mem_amo_op == AMO_SC);
+    assign amo_is_word        = mem_is_amo && (mem_mem_size == 3'b010);
+    assign amo_is_double      = mem_is_amo && (mem_mem_size == 3'b011);
+    assign amo_rmw_op         = mem_is_amo && !amo_is_lr && !amo_is_sc;
+    assign amo_rmw_read_phase = amo_rmw_op && !amo_rmw_busy_q;
+    assign amo_rmw_write_phase = amo_rmw_op && amo_rmw_busy_q;
+    assign amo_reads_memory   = amo_is_lr || amo_rmw_read_phase;
+    assign amo_store_access   = mem_is_amo && !amo_is_lr;
+    assign amo_load_access    = mem_is_amo && amo_is_lr;
+    assign amo_addr_misaligned = mem_is_amo && addr_misaligned(mem_mem_size, mem_alu_result);
+    assign amo_sc_success     = amo_is_sc && reservation_valid_q &&
+                                (reservation_addr_q == mem_alu_result) &&
+                                !amo_addr_misaligned && !dmem_store_access_fault &&
+                                !dmem_store_page_fault && !mem_instr_access_fault &&
+                                !mem_instr_page_fault;
+    assign amo_writes_memory  = (amo_is_sc && amo_sc_success) || amo_rmw_write_phase;
+    assign amo_memory_rdata   = amo_rmw_busy_q ? amo_rmw_old_data_q : dmem_rdata;
+
+    assign load_addr_misaligned  = mem_is_load && addr_misaligned(mem_mem_size, mem_alu_result);
+    assign store_addr_misaligned = (mem_is_store && addr_misaligned(mem_mem_size, mem_alu_result)) |
+                                   (amo_store_access && amo_addr_misaligned);
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            amo_rmw_busy_q      <= 1'b0;
+            amo_rmw_old_data_q  <= '0;
+            reservation_valid_q <= 1'b0;
+            reservation_addr_q  <= '0;
+        end else begin
+            if (amo_rmw_read_phase && dmem_read_req && dmem_rsp_valid && !dmem_load_access_fault &&
+                !dmem_load_page_fault && !mem_instr_access_fault && !mem_instr_page_fault) begin
+                amo_rmw_busy_q     <= 1'b1;
+                amo_rmw_old_data_q <= dmem_rdata;
+            end else if (amo_rmw_write_phase && (!dmem_write_req || dmem_req_ready)) begin
+                amo_rmw_busy_q     <= 1'b0;
+                amo_rmw_old_data_q <= '0;
+            end else if (!mem_is_amo) begin
+                amo_rmw_busy_q     <= 1'b0;
+                amo_rmw_old_data_q <= '0;
+            end
+
+            if (amo_is_lr && dmem_read_req && dmem_rsp_valid && !dmem_load_access_fault &&
+                !dmem_load_page_fault) begin
+                reservation_valid_q <= 1'b1;
+                reservation_addr_q  <= mem_alu_result;
+            end else if (amo_is_sc && dmem_req_ready) begin
+                reservation_valid_q <= 1'b0;
+                reservation_addr_q  <= '0;
+            end
+        end
+    end
+
+    // --------------------------
+    // Memory Request Logic
+    assign dmem_read_req  = (mem_is_load | amo_reads_memory) &
+                            ~(load_addr_misaligned | (amo_reads_memory & amo_addr_misaligned)) &
+                            ~dmem_load_access_fault & ~dmem_load_page_fault &
+                            ~mem_instr_access_fault & ~mem_instr_page_fault;
+    assign dmem_write_req = (mem_is_store | amo_writes_memory) & ~store_addr_misaligned & ~dmem_store_access_fault &
+                            ~dmem_store_page_fault & ~mem_instr_access_fault & ~mem_instr_page_fault;
+    assign dmem_addr      = mem_alu_result; // ALU result is the computed address
+
+    // --------------------------
+    // Stall generation: Stall if memory request is not yet ready or valid
+    // For a simple synchronous SRAM, this might be 0, but for cache it will depend on hits/misses
+    // --------------------------
+    always_comb begin
+        if (amo_rmw_read_phase && dmem_read_req) begin
+            mem_stall_req = 1'b1;
+        end else if ((dmem_read_req || dmem_write_req) && (!dmem_req_ready || (dmem_read_req && !dmem_rsp_valid))) begin
+            mem_stall_req = 1'b1;
+        end else begin
+            mem_stall_req = 1'b0;
+        end
+    end
+
+    // --------------------------
+    // Store Data Alignment and Strobe Generation
+    // --------------------------
+    always_comb begin
+        logic [2:0] align_offset;
+        store_data_aligned = '0;
+        write_strobe       = 8'b0000_0000;
+        selected_store_data = mem_fp_store_data_sel ? mem_fs2_rdata : mem_rs2_rdata;
+
+        // Mem address lower 3 bits determine the alignment within the 64-bit word
+        align_offset = mem_alu_result[2:0];
+
+        if (mem_is_store || amo_writes_memory) begin
+            case (mem_mem_size)
+                // SB: Store Byte
+                3'b000: begin
+                    store_data_aligned = {56'b0, selected_store_data[7:0]} << (align_offset * 8);
+                    write_strobe       = 8'b0000_0001 << align_offset;
+                end
+                // SH: Store Halfword
+                3'b001: begin
+                    store_data_aligned = {48'b0, selected_store_data[15:0]} << (align_offset * 8);
+                    write_strobe       = 8'b0000_0011 << align_offset;
+                end
+                // SW: Store Word
+                3'b010: begin
+                    store_data_aligned = {32'b0, amo_writes_memory ? amo_write_data[31:0] :
+                                          selected_store_data[31:0]} << (align_offset * 8);
+                    write_strobe       = 8'b0000_1111 << align_offset;
+                end
+                // SD: Store Doubleword
+                3'b011: begin
+                    store_data_aligned = amo_writes_memory ? amo_write_data : selected_store_data;
+                    write_strobe       = 8'b1111_1111;
+                end
+                default: begin
+                    store_data_aligned = '0;
+                    write_strobe       = 8'b0000_0000;
+                end
+            endcase
+        end
+    end
+
+    assign dmem_wdata = store_data_aligned;
+    assign dmem_wstrb = write_strobe;
+
+    // --------------------------
+    // Load Data Alignment and Sign/Zero Extension
+    // --------------------------
+    always_comb begin
+        logic [2:0] align_offset;
+        logic [XLEN-1:0] shifted_rdata;
+        load_data_aligned = '0;
+
+        align_offset = mem_alu_result[2:0];
+
+        // Shift data down so the target bytes are at the bottom
+        shifted_rdata = dmem_rdata >> (align_offset * 8);
+
+        case (mem_mem_size)
+            // LB: Load Byte (Sign extend)
+            3'b000: load_data_aligned = { {56{shifted_rdata[7]}}, shifted_rdata[7:0] };
+            // LH: Load Halfword (Sign extend)
+            3'b001: load_data_aligned = { {48{shifted_rdata[15]}}, shifted_rdata[15:0] };
+            // LW: Load Word (Sign extend)
+            3'b010: load_data_aligned = { {32{shifted_rdata[31]}}, shifted_rdata[31:0] };
+            // LD: Load Doubleword
+            3'b011: load_data_aligned = shifted_rdata;
+            // LBU: Load Byte Unsigned (Zero extend)
+            3'b100: load_data_aligned = { 56'b0, shifted_rdata[7:0] };
+            // LHU: Load Half Unsigned (Zero extend)
+            3'b101: load_data_aligned = { 48'b0, shifted_rdata[15:0] };
+            // LWU: Load Word Unsigned (Zero extend)
+            3'b110: load_data_aligned = { 32'b0, shifted_rdata[31:0] };
+            default: load_data_aligned = '0;
+        endcase
+    end
+
+    always_comb begin
+        fp_load_data = load_data_aligned;
+        if (mem_is_fp && (mem_mem_size == 3'b010)) begin
+            fp_load_data = {32'hFFFF_FFFF, load_data_aligned[31:0]};
+        end
+    end
+
+    always_comb begin
+        logic [2:0]        align_offset;
+        logic [XLEN-1:0]   shifted_rdata;
+        logic [31:0]       amo_old_word;
+        logic [31:0]       amo_rs2_word;
+        logic [31:0]       amo_result_word;
+        logic signed [31:0] amo_old_word_signed;
+        logic signed [31:0] amo_rs2_word_signed;
+        logic signed [XLEN-1:0] amo_old_double_signed;
+        logic signed [XLEN-1:0] amo_rs2_double_signed;
+
+        align_offset = mem_alu_result[2:0];
+        shifted_rdata = amo_memory_rdata >> (align_offset * 8);
+        amo_old_word = shifted_rdata[31:0];
+        amo_rs2_word = mem_rs2_rdata[31:0];
+        amo_old_word_signed = amo_old_word;
+        amo_rs2_word_signed = amo_rs2_word;
+        amo_old_double_signed = amo_memory_rdata;
+        amo_rs2_double_signed = mem_rs2_rdata;
+        amo_result_word = amo_old_word;
+        amo_write_data = mem_rs2_rdata;
+        amo_wb_data = '0;
+
+        if (amo_is_word) begin
+            unique case (mem_amo_op)
+                AMO_SWAP, AMO_SC: amo_result_word = amo_rs2_word;
+                AMO_ADD:          amo_result_word = amo_old_word + amo_rs2_word;
+                AMO_XOR:          amo_result_word = amo_old_word ^ amo_rs2_word;
+                AMO_AND:          amo_result_word = amo_old_word & amo_rs2_word;
+                AMO_OR:           amo_result_word = amo_old_word | amo_rs2_word;
+                AMO_MIN:          amo_result_word = (amo_old_word_signed < amo_rs2_word_signed) ?
+                                                     amo_old_word : amo_rs2_word;
+                AMO_MAX:          amo_result_word = (amo_old_word_signed > amo_rs2_word_signed) ?
+                                                     amo_old_word : amo_rs2_word;
+                AMO_MINU:         amo_result_word = (amo_old_word < amo_rs2_word) ? amo_old_word : amo_rs2_word;
+                AMO_MAXU:         amo_result_word = (amo_old_word > amo_rs2_word) ? amo_old_word : amo_rs2_word;
+                default:          amo_result_word = amo_old_word;
+            endcase
+            amo_write_data = {{32{amo_result_word[31]}}, amo_result_word};
+            amo_wb_data = amo_is_sc ? {63'b0, ~amo_sc_success} :
+                          {{32{amo_old_word[31]}}, amo_old_word};
+        end else if (amo_is_double) begin
+            unique case (mem_amo_op)
+                AMO_SWAP, AMO_SC: amo_write_data = mem_rs2_rdata;
+                AMO_ADD:          amo_write_data = amo_memory_rdata + mem_rs2_rdata;
+                AMO_XOR:          amo_write_data = amo_memory_rdata ^ mem_rs2_rdata;
+                AMO_AND:          amo_write_data = amo_memory_rdata & mem_rs2_rdata;
+                AMO_OR:           amo_write_data = amo_memory_rdata | mem_rs2_rdata;
+                AMO_MIN:          amo_write_data = (amo_old_double_signed < amo_rs2_double_signed) ?
+                                                   amo_memory_rdata : mem_rs2_rdata;
+                AMO_MAX:          amo_write_data = (amo_old_double_signed > amo_rs2_double_signed) ?
+                                                   amo_memory_rdata : mem_rs2_rdata;
+                AMO_MINU:         amo_write_data = (amo_memory_rdata < mem_rs2_rdata) ? amo_memory_rdata :
+                                                   mem_rs2_rdata;
+                AMO_MAXU:         amo_write_data = (amo_memory_rdata > mem_rs2_rdata) ? amo_memory_rdata :
+                                                   mem_rs2_rdata;
+                default:          amo_write_data = amo_memory_rdata;
+            endcase
+            amo_wb_data = amo_is_sc ? {63'b0, ~amo_sc_success} : amo_memory_rdata;
+        end
+    end
+
+    // --------------------------
+    // Pass Output Signals to WB Stage
+    // --------------------------
+    assign mem_wb_alu_result    = mem_alu_result;
+    assign mem_wb_mem_rdata     = mem_is_amo ? amo_wb_data : load_data_aligned;
+    assign mem_wb_fp_result     = mem_is_load ? fp_load_data : mem_fp_result;
+    assign mem_wb_fp_fflags     = mem_fp_fflags;
+    assign mem_wb_pc            = mem_pc;
+    assign mem_wb_instr         = mem_instr;
+    assign mem_wb_instr_len     = mem_instr_len;
+
+    assign mem_wb_reg_write_en  = mem_reg_write_en;
+    assign mem_wb_fp_reg_write_en = mem_fp_reg_write_en;
+    assign mem_wb_rd_addr       = mem_rd_addr;
+    assign mem_wb_wb_sel        = mem_wb_sel;
+
+    assign mem_wb_is_csr        = mem_is_csr;
+    assign mem_wb_csr_op        = mem_csr_op;
+    assign mem_wb_rs1_rdata     = mem_rs1_rdata;
+    assign mem_wb_rs1_addr      = mem_rs1_addr;
+    assign mem_wb_imm           = mem_imm;
+
+    assign mem_wb_illegal_instr      = mem_illegal_instr;
+    assign mem_wb_instr_access_fault = mem_instr_access_fault;
+    assign mem_wb_instr_page_fault   = mem_instr_page_fault;
+    assign mem_wb_is_ecall           = mem_is_ecall;
+    assign mem_wb_is_ebreak          = mem_is_ebreak;
+    assign mem_wb_is_mret            = mem_is_mret;
+    assign mem_wb_is_sret            = mem_is_sret;
+    assign mem_wb_load_fault         = load_addr_misaligned | (amo_load_access & amo_addr_misaligned) |
+                                       dmem_load_access_fault;
+    assign mem_wb_store_fault        = store_addr_misaligned | dmem_store_access_fault;
+    assign mem_wb_load_page_fault    = dmem_load_page_fault;
+    assign mem_wb_store_page_fault   = dmem_store_page_fault;
+    assign mem_wb_fault_addr         = (mem_instr_access_fault | mem_instr_page_fault) ? mem_pc :
+                                       ((dmem_load_page_fault | dmem_store_page_fault) ? mem_alu_result :
+                                       ((load_addr_misaligned | store_addr_misaligned |
+                                         dmem_load_access_fault | dmem_store_access_fault) ? mem_alu_result : '0));
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/mem_wb_reg.sv b/rv64cpu/rtl/core/pipeline/mem_wb_reg.sv
new file mode 100644
index 0000000..be0aec1
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/mem_wb_reg.sv
@@ -0,0 +1,188 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/mem_wb_reg.sv
+// Description: MEM/WB pipeline stage register (Memory Access to Write Back)
+//              Passes the final computation results, memory read data, and write-back control signals to the write-back stage
+
+module mem_wb_reg
+    import rv64_pkg::*;
+(
+    input  logic               clk,
+    input  logic               rst,
+
+    // -----------------------
+    // Control Signals from Hazard Unit
+    input  logic               stall_en,
+    input  logic               flush_en,
+
+    // -----------------------
+    // Inputs from MEM stage
+    // -----------------------
+    // Data and addresses
+    input  logic [XLEN-1:0]        mem_pc,
+    input  logic [XLEN-1:0]        mem_alu_result,
+    input  logic [XLEN-1:0]        mem_mem_rdata,
+    input  logic [XLEN-1:0]        mem_fp_result,
+    input  logic [4:0]             mem_fp_fflags,
+    input  logic [INST_WIDTH-1:0]  mem_instr,
+    input  logic [2:0]             mem_instr_len,
+
+    // Write back control
+    input  logic                   mem_reg_write_en,
+    input  logic                   mem_fp_reg_write_en,
+    input  logic [4:0]             mem_rd_addr,
+    input  logic [1:0]             mem_wb_sel,
+
+    // CSR & System
+    input  logic                   mem_is_csr,
+    input  logic [2:0]             mem_csr_op,
+    input  logic [XLEN-1:0]        mem_rs1_rdata,
+    input  logic [4:0]             mem_rs1_addr,
+    input  logic [XLEN-1:0]        mem_imm,
+    input  logic                   mem_illegal_instr,
+    input  logic                   mem_instr_access_fault,
+    input  logic                   mem_instr_page_fault,
+    input  logic                   mem_is_ecall,
+    input  logic                   mem_is_ebreak,
+    input  logic                   mem_is_mret,
+    input  logic                   mem_is_sret,
+    input  logic                   mem_load_fault,
+    input  logic                   mem_store_fault,
+    input  logic                   mem_load_page_fault,
+    input  logic                   mem_store_page_fault,
+    input  logic [XLEN-1:0]        mem_fault_addr,
+
+    // -----------------------
+    // Outputs to WB Stage
+    // -----------------------
+    // Data and address
+    output logic [XLEN-1:0]        wb_pc,
+    output logic [XLEN-1:0]        wb_alu_result,
+    output logic [XLEN-1:0]        wb_mem_rdata,
+    output logic [XLEN-1:0]        wb_fp_result,
+    output logic [4:0]             wb_fp_fflags,
+    output logic [INST_WIDTH-1:0]  wb_instr,
+    output logic [2:0]             wb_instr_len,
+
+    // Write back control
+    output logic                   wb_reg_write_en,
+    output logic                   wb_fp_reg_write_en,
+    output logic [4:0]             wb_rd_addr,
+    output logic [1:0]             wb_wb_sel,
+
+    // CSR & System
+    output logic                   wb_is_csr,
+    output logic [2:0]             wb_csr_op,
+    output logic [XLEN-1:0]        wb_rs1_rdata,
+    output logic [4:0]             wb_rs1_addr,
+    output logic [XLEN-1:0]        wb_imm,
+    output logic                   wb_illegal_instr,
+    output logic                   wb_instr_access_fault,
+    output logic                   wb_instr_page_fault,
+    output logic                   wb_is_ecall,
+    output logic                   wb_is_ebreak,
+    output logic                   wb_is_mret,
+    output logic                   wb_is_sret,
+    output logic                   wb_load_fault,
+    output logic                   wb_store_fault,
+    output logic                   wb_load_page_fault,
+    output logic                   wb_store_page_fault,
+    output logic [XLEN-1:0]        wb_fault_addr
+
+);
+    // Sequential Logic: State Update
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            wb_pc                  <= '0;
+            wb_alu_result          <= '0;
+            wb_mem_rdata           <= '0;
+            wb_fp_result           <= '0;
+            wb_fp_fflags           <= '0;
+            wb_instr               <= '0;
+            wb_instr_len           <= 3'd4;
+            wb_reg_write_en        <= '0;
+            wb_fp_reg_write_en     <= '0;
+            wb_rd_addr             <= '0;
+            wb_wb_sel              <= '0;
+            wb_is_csr              <= '0;
+            wb_csr_op              <= '0;
+            wb_rs1_rdata           <= '0;
+            wb_rs1_addr            <= '0;
+            wb_imm                 <= '0;
+            wb_illegal_instr       <= '0;
+            wb_instr_access_fault <= '0;
+            wb_instr_page_fault   <= '0;
+            wb_is_ecall           <= '0;
+            wb_is_ebreak          <= '0;
+            wb_is_mret            <= '0;
+            wb_is_sret            <= '0;
+            wb_load_fault         <= '0;
+            wb_store_fault        <= '0;
+            wb_load_page_fault    <= '0;
+            wb_store_page_fault   <= '0;
+            wb_fault_addr         <= '0;
+        end else if (flush_en) begin
+            // Target specific control signals to clear. Leave datapath elements intact
+            // to avoid extreme power consumption from massive flip-flop state changes array.
+            wb_reg_write_en       <= '0;
+            wb_fp_reg_write_en    <= '0;
+            wb_is_csr             <= '0;
+            wb_illegal_instr      <= '0;
+            wb_instr_access_fault <= '0;
+            wb_instr_page_fault   <= '0;
+            wb_is_ecall           <= '0;
+            wb_is_ebreak          <= '0;
+            wb_is_mret            <= '0;
+            wb_is_sret            <= '0;
+            wb_load_fault         <= '0;
+            wb_store_fault        <= '0;
+            wb_load_page_fault    <= '0;
+            wb_store_page_fault   <= '0;
+            wb_fault_addr         <= '0;
+        end else if (!stall_en) begin
+            wb_pc                  <= mem_pc;
+            wb_alu_result          <= mem_alu_result;
+            wb_mem_rdata           <= mem_mem_rdata;
+            wb_fp_result           <= mem_fp_result;
+            wb_fp_fflags           <= mem_fp_fflags;
+            wb_instr               <= mem_instr;
+            wb_instr_len           <= mem_instr_len;
+            wb_reg_write_en        <= mem_reg_write_en;
+            wb_fp_reg_write_en     <= mem_fp_reg_write_en;
+            wb_rd_addr             <= mem_rd_addr;
+            wb_wb_sel              <= mem_wb_sel;
+            wb_is_csr              <= mem_is_csr;
+            wb_csr_op              <= mem_csr_op;
+            wb_rs1_rdata           <= mem_rs1_rdata;
+            wb_rs1_addr            <= mem_rs1_addr;
+            wb_imm                 <= mem_imm;
+            wb_illegal_instr       <= mem_illegal_instr;
+            wb_instr_access_fault <= mem_instr_access_fault;
+            wb_instr_page_fault   <= mem_instr_page_fault;
+            wb_is_ecall           <= mem_is_ecall;
+            wb_is_ebreak          <= mem_is_ebreak;
+            wb_is_mret            <= mem_is_mret;
+            wb_is_sret            <= mem_is_sret;
+            wb_load_fault         <= mem_load_fault;
+            wb_store_fault        <= mem_store_fault;
+            wb_load_page_fault    <= mem_load_page_fault;
+            wb_store_page_fault   <= mem_store_page_fault;
+            wb_fault_addr         <= mem_fault_addr;
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/wb_stage.sv b/rv64cpu/rtl/core/pipeline/wb_stage.sv
new file mode 100644
index 0000000..b19cb67
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/wb_stage.sv
@@ -0,0 +1,168 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/wb_stage.sv
+// Description: Write Back Stage
+// Responsible for writing back to the general-purpose register file after multiplexing the final data (ALU result, memory read data, or PC+4)
+
+module wb_stage
+    import rv64_pkg::*;
+(
+    // Inputs from MEM/WB pipeline register
+    input  logic [XLEN-1:0]        mem_wb_alu_result,
+    input  logic [XLEN-1:0]        mem_wb_mem_rdata,
+    input  logic [XLEN-1:0]        mem_wb_fp_result,
+    input  logic [4:0]             mem_wb_fp_fflags,
+    input  logic [XLEN-1:0]        mem_wb_pc,
+    input  logic [XLEN-1:0]        mem_wb_pc_plus_4,
+    input  logic [INST_WIDTH-1:0]  mem_wb_instr,
+    
+    input  logic                   mem_wb_reg_write_en,
+    input  logic                   mem_wb_fp_reg_write_en,
+    input  logic [4:0]             mem_wb_rd_addr,
+    input  logic [1:0]             mem_wb_wb_sel,
+
+    input  logic                   mem_wb_is_csr,
+    input  logic [2:0]             mem_wb_csr_op,
+    input  logic [XLEN-1:0]        mem_wb_rs1_rdata,
+    input  logic [4:0]             mem_wb_rs1_addr,
+    input  logic [XLEN-1:0]        mem_wb_imm,
+    
+    input  logic                   mem_wb_illegal_instr,
+    input  logic                   mem_wb_is_ecall,
+    input  logic                   mem_wb_is_ebreak,
+    input  logic                   mem_wb_is_mret,
+    input  logic                   mem_wb_is_sret,
+    input  logic                   mem_wb_instr_access_fault,
+    input  logic                   mem_wb_instr_page_fault,
+    input  logic                   mem_wb_load_fault,
+    input  logic                   mem_wb_store_fault,
+    input  logic                   mem_wb_load_page_fault,
+    input  logic                   mem_wb_store_page_fault,
+    input  logic [XLEN-1:0]        mem_wb_fault_addr,
+
+    // Inputs from CSR Unit
+    input  logic [XLEN-1:0]        csr_rdata,         // Data read from CSR unit
+    input  logic                   csr_illegal_access,// CSR rejected the request
+
+    // Outputs to Register File
+    output logic [XLEN-1:0]        wb_rd_wdata,
+    output logic [4:0]             wb_rd_addr,
+    output logic                   wb_reg_write_en,
+    output logic [XLEN-1:0]        wb_fd_wdata,
+    output logic [4:0]             wb_fd_addr,
+    output logic                   wb_fp_write_en,
+    output logic [4:0]             wb_fp_fflags,
+    output logic                   wb_fp_state_dirty,
+    output logic                   wb_fp_fflags_valid,
+
+    // Outputs to CSR Unit
+    output logic                   csr_req,           // CSR request valid
+    output logic [2:0]             csr_op,            // CSR operation type
+    output logic [11:0]            csr_addr,          // CSR address
+    output logic [XLEN-1:0]        csr_rs1_data,      // Source data (from rs1_rdata or zimm)
+
+    // Outputs to Exception Handler (Trap Controller)
+    output logic                   trap_illegal_instr,
+    output logic                   trap_is_ecall,
+    output logic                   trap_is_ebreak,
+    output logic                   trap_is_mret,
+    output logic                   trap_is_sret,
+    output logic [XLEN-1:0]        trap_epc,
+    output logic                   trap_instr_access_fault,
+    output logic                   trap_instr_page_fault,
+    output logic                   trap_load_fault,
+    output logic                   trap_store_fault,
+    output logic                   trap_load_page_fault,
+    output logic                   trap_store_page_fault,
+    output logic [XLEN-1:0]        trap_bad_addr,
+    output logic [INST_WIDTH-1:0]  trap_bad_instr
+);
+
+    // --------------------------
+    // Exception/Trap Detection
+    // --------------------------
+    logic has_trap;
+    logic trap_blocker;
+    assign has_trap     = mem_wb_illegal_instr | mem_wb_is_ecall | mem_wb_is_ebreak |
+                           mem_wb_instr_access_fault | mem_wb_load_fault | mem_wb_store_fault |
+                           mem_wb_instr_page_fault | mem_wb_load_page_fault | mem_wb_store_page_fault;
+    assign trap_blocker = has_trap | csr_illegal_access;
+
+    // --------------------------
+    // CSR Signal Routing
+    // --------------------------
+    assign csr_req  = mem_wb_is_csr & ~has_trap;
+    assign csr_op   = mem_wb_csr_op;
+    assign csr_addr = mem_wb_imm[11:0];
+    
+    // For immediate CSR instructions (CSRRWI, CSRRSI, CSRRCI), 
+    // the source data is the zero-extended rs1_addr field.
+    // mem_wb_csr_op[2] is 1 for immediate variants.
+    assign csr_rs1_data = (mem_wb_csr_op[2]) ? {59'b0, mem_wb_rs1_addr} : mem_wb_rs1_rdata;
+
+    // --------------------------
+    // Data Multiplexer for Register File
+    // --------------------------
+    always_comb begin
+        if (mem_wb_is_csr) begin
+            // CSR reads write the previous CSR value to the destination register
+            wb_rd_wdata = csr_rdata;
+        end else begin
+            case (mem_wb_wb_sel)
+                2'b00: wb_rd_wdata = mem_wb_alu_result;
+                2'b01: wb_rd_wdata = mem_wb_mem_rdata;
+                2'b10: wb_rd_wdata = mem_wb_pc_plus_4;
+                default: wb_rd_wdata = '0;
+            endcase
+        end
+    end
+
+    // --------------------------
+    // Register File Write Control
+    // --------------------------
+    // We only write back if the instruction didn't trap.
+    assign wb_rd_addr      = mem_wb_rd_addr;
+    assign wb_reg_write_en = mem_wb_reg_write_en & ~trap_blocker;
+    assign wb_fd_addr      = mem_wb_rd_addr;
+    assign wb_fd_wdata     = mem_wb_fp_result;
+    assign wb_fp_write_en  = mem_wb_fp_reg_write_en & ~trap_blocker;
+    assign wb_fp_fflags    = mem_wb_fp_fflags;
+    assign wb_fp_state_dirty = wb_fp_write_en | ((|mem_wb_fp_fflags) & ~trap_blocker);
+    assign wb_fp_fflags_valid = (|mem_wb_fp_fflags) & ~trap_blocker;
+
+    // --------------------------
+    // Trap / Exception Signals
+    // --------------------------
+    assign trap_illegal_instr      = (mem_wb_illegal_instr &
+                                      ~mem_wb_instr_access_fault &
+                                      ~mem_wb_instr_page_fault) |
+                                     csr_illegal_access;
+    assign trap_is_ecall           = mem_wb_is_ecall;
+    assign trap_is_ebreak          = mem_wb_is_ebreak;
+    assign trap_is_mret            = mem_wb_is_mret;
+    assign trap_is_sret            = mem_wb_is_sret;
+    assign trap_epc                = mem_wb_pc;
+    assign trap_instr_access_fault = mem_wb_instr_access_fault;
+    assign trap_instr_page_fault   = mem_wb_instr_page_fault;
+    assign trap_load_fault         = mem_wb_load_fault;
+    assign trap_store_fault        = mem_wb_store_fault;
+    assign trap_load_page_fault    = mem_wb_load_page_fault;
+    assign trap_store_page_fault   = mem_wb_store_page_fault;
+    assign trap_bad_addr           = mem_wb_fault_addr;
+    assign trap_bad_instr          = mem_wb_instr;
+
+endmodule