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

24 files changed, 9643 insertions, 0 deletions

diff --git a/rv64cpu/rtl/core/execute/branch_unit.sv b/rv64cpu/rtl/core/execute/branch_unit.sv
new file mode 100644
index 0000000..c9662ce
--- /dev/null
+++ b/rv64cpu/rtl/core/execute/branch_unit.sv
@@ -0,0 +1,52 @@
+//     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/branch_unit.sv
+// Description: Branch Unit, responsible for determining whether conditional jump instructions are effective
+
+module branch_unit 
+    import rv64_pkg::*;
+(
+    input   logic              is_branch,      // Indicates whether the current instruction is a branch instruction
+    input   logic [2:0]        branch_op,      // Specific branch types func3
+    input   logic [XLEN-1:0]   operand_a,      // operand 1
+    input   logic [XLEN-1:0]   operand_b,      // operand 2
+    output  logic              branch_taken    // Branch taken signal
+);
+
+    always_comb begin
+        branch_taken = 1'b0;
+        
+        if (is_branch) begin
+            case (branch_op)
+                // 3'b000: BEQ (Branch if Equal)
+                BRANCH_EQ: branch_taken = (operand_a == operand_b);
+                // 3'b001: BNE (Branch if Not Equal)
+                BRANCH_NE: branch_taken = (operand_a != operand_b);
+                // 3'b100: BLT (Branch if Less Than, Signed)
+                BRANCH_LT: branch_taken = ($signed(operand_a) < $signed(operand_b));
+                // 3'b101: BGE (Branch if Greater Than or Equal, Signed)
+                BRANCH_GE: branch_taken = ($signed(operand_a) >= $signed(operand_b));
+                // 3'b110: BLTU (Branch if Less Than, Unsigned)
+                BRANCH_LTU: branch_taken = (operand_a < operand_b);
+                // 3'b111: BGEU (Branch if Greater Than or Equal, Unsigned)
+                BRANCH_GEU: branch_taken = (operand_a >= operand_b);
+                default: branch_taken = 1'b0;
+            endcase
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/execute/compressed_decoder.sv b/rv64cpu/rtl/core/execute/compressed_decoder.sv
new file mode 100644
index 0000000..ccbb043
--- /dev/null
+++ b/rv64cpu/rtl/core/execute/compressed_decoder.sv
@@ -0,0 +1,234 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+// File path: rtl/core/compressed_decoder.sv
+// Description: RV64C/Zca decompressor. Produces an equivalent 32-bit instruction
+//              for the existing RV64 pipeline and flags reserved encodings.
+
+module compressed_decoder
+    import rv64_pkg::*;
+(
+    input  logic [15:0] compressed_instr,
+    output logic [31:0] expanded_instr,
+    output logic        illegal_instr
+);
+
+    logic [1:0] quadrant;
+    logic [2:0] funct3;
+    logic [4:0] rd_rs1;
+    logic [4:0] rs2;
+    logic [4:0] rd_prime;
+    logic [4:0] rs1_prime;
+    logic [4:0] rs2_prime;
+
+    assign quadrant  = compressed_instr[1:0];
+    assign funct3    = compressed_instr[15:13];
+    assign rd_rs1    = compressed_instr[11:7];
+    assign rs2       = compressed_instr[6:2];
+    assign rd_prime  = {2'b01, compressed_instr[4:2]};
+    assign rs1_prime = {2'b01, compressed_instr[9:7]};
+    assign rs2_prime = {2'b01, compressed_instr[4:2]};
+
+    function automatic logic [31:0] r_instr(
+        input logic [6:0] funct7,
+        input logic [4:0] rs2_i,
+        input logic [4:0] rs1_i,
+        input logic [2:0] funct3_i,
+        input logic [4:0] rd_i,
+        input logic [6:0] opcode_i
+    );
+        return {funct7, rs2_i, rs1_i, funct3_i, rd_i, opcode_i};
+    endfunction
+
+    function automatic logic [31:0] i_instr(
+        input logic [11:0] imm_i,
+        input logic [4:0]  rs1_i,
+        input logic [2:0]  funct3_i,
+        input logic [4:0]  rd_i,
+        input logic [6:0]  opcode_i
+    );
+        return {imm_i, rs1_i, funct3_i, rd_i, opcode_i};
+    endfunction
+
+    function automatic logic [31:0] s_instr(
+        input logic [11:0] imm_i,
+        input logic [4:0]  rs2_i,
+        input logic [4:0]  rs1_i,
+        input logic [2:0]  funct3_i,
+        input logic [6:0]  opcode_i
+    );
+        return {imm_i[11:5], rs2_i, rs1_i, funct3_i, imm_i[4:0], opcode_i};
+    endfunction
+
+    function automatic logic [31:0] b_instr(
+        input logic [12:0] imm_i,
+        input logic [4:0]  rs2_i,
+        input logic [4:0]  rs1_i,
+        input logic [2:0]  funct3_i
+    );
+        return {imm_i[12], imm_i[10:5], rs2_i, rs1_i, funct3_i,
+                imm_i[4:1], imm_i[11], OPCODE_BRANCH};
+    endfunction
+
+    function automatic logic [31:0] u_instr(
+        input logic [19:0] imm_i,
+        input logic [4:0]  rd_i,
+        input logic [6:0]  opcode_i
+    );
+        return {imm_i, rd_i, opcode_i};
+    endfunction
+
+    function automatic logic [31:0] j_instr(
+        input logic [20:0] imm_i,
+        input logic [4:0]  rd_i
+    );
+        return {imm_i[20], imm_i[10:1], imm_i[11], imm_i[19:12], rd_i, OPCODE_JAL};
+    endfunction
+
+    always_comb begin
+        logic [11:0] ci_imm;
+        logic [11:0] addi4spn_imm;
+        logic [11:0] lw_imm;
+        logic [11:0] ld_imm;
+        logic [11:0] lwsp_imm;
+        logic [11:0] ldsp_imm;
+        logic [11:0] swsp_imm;
+        logic [11:0] sdsp_imm;
+        logic [12:0] branch_imm;
+        logic [20:0] jump_imm;
+        logic [5:0]  shamt;
+        logic [19:0] lui_imm;
+
+        ci_imm       = {{6{compressed_instr[12]}}, compressed_instr[12], compressed_instr[6:2]};
+        addi4spn_imm = {2'b00, compressed_instr[10:7], compressed_instr[12:11],
+                        compressed_instr[5], compressed_instr[6], 2'b00};
+        lw_imm       = {5'b00000, compressed_instr[5], compressed_instr[12:10],
+                        compressed_instr[6], 2'b00};
+        ld_imm       = {4'b0000, compressed_instr[6:5], compressed_instr[12:10], 3'b000};
+        lwsp_imm     = {4'b0000, compressed_instr[3:2], compressed_instr[12],
+                        compressed_instr[6:4], 2'b00};
+        ldsp_imm     = {3'b000, compressed_instr[4:2], compressed_instr[12],
+                        compressed_instr[6:5], 3'b000};
+        swsp_imm     = {4'b0000, compressed_instr[8:7], compressed_instr[12:9], 2'b00};
+        sdsp_imm     = {3'b000, compressed_instr[9:7], compressed_instr[12:10], 3'b000};
+        branch_imm   = {{4{compressed_instr[12]}}, compressed_instr[12],
+                        compressed_instr[6:5], compressed_instr[2],
+                        compressed_instr[11:10], compressed_instr[4:3], 1'b0};
+        jump_imm     = {{9{compressed_instr[12]}}, compressed_instr[12],
+                        compressed_instr[8], compressed_instr[10:9], compressed_instr[6],
+                        compressed_instr[7], compressed_instr[2], compressed_instr[11],
+                        compressed_instr[5:3], 1'b0};
+        shamt        = {compressed_instr[12], compressed_instr[6:2]};
+        lui_imm      = {{14{compressed_instr[12]}}, compressed_instr[12], compressed_instr[6:2]};
+
+        expanded_instr = 32'h0000_0013;
+        illegal_instr  = 1'b0;
+
+        unique case (quadrant)
+            2'b00: begin
+                unique case (funct3)
+                    3'b000: begin // C.ADDI4SPN
+                        expanded_instr = i_instr(addi4spn_imm, 5'd2, 3'b000, rd_prime, OPCODE_OP_IMM);
+                        illegal_instr  = (addi4spn_imm == 12'b0);
+                    end
+                    3'b001: expanded_instr = i_instr(ld_imm, rs1_prime, 3'b011, rd_prime, OPCODE_LOAD_FP);
+                    3'b010: expanded_instr = i_instr(lw_imm, rs1_prime, 3'b010, rd_prime, OPCODE_LOAD);
+                    3'b011: expanded_instr = i_instr(ld_imm, rs1_prime, 3'b011, rd_prime, OPCODE_LOAD);
+                    3'b101: expanded_instr = s_instr(ld_imm, rs2_prime, rs1_prime, 3'b011, OPCODE_STORE_FP);
+                    3'b110: expanded_instr = s_instr(lw_imm, rs2_prime, rs1_prime, 3'b010, OPCODE_STORE);
+                    3'b111: expanded_instr = s_instr(ld_imm, rs2_prime, rs1_prime, 3'b011, OPCODE_STORE);
+                    default: illegal_instr = 1'b1;
+                endcase
+            end
+
+            2'b01: begin
+                unique case (funct3)
+                    3'b000: expanded_instr = i_instr(ci_imm, rd_rs1, 3'b000, rd_rs1, OPCODE_OP_IMM);
+                    3'b001: expanded_instr = i_instr(ci_imm, rd_rs1, 3'b000, rd_rs1, OPCODE_OP_IMM_32);
+                    3'b010: expanded_instr = i_instr(ci_imm, 5'd0, 3'b000, rd_rs1, OPCODE_OP_IMM);
+                    3'b011: begin
+                        if (rd_rs1 == 5'd2) begin
+                            expanded_instr = i_instr({{2{compressed_instr[12]}}, compressed_instr[12],
+                                                       compressed_instr[4:3], compressed_instr[5],
+                                                       compressed_instr[2], compressed_instr[6], 4'b0000},
+                                                      5'd2, 3'b000, 5'd2, OPCODE_OP_IMM);
+                            illegal_instr = (compressed_instr[12:2] == 11'b0);
+                        end else begin
+                            expanded_instr = u_instr(lui_imm, rd_rs1, OPCODE_LUI);
+                            illegal_instr = (rd_rs1 == 5'd0) || (lui_imm == 20'b0);
+                        end
+                    end
+                    3'b100: begin
+                        unique case (compressed_instr[11:10])
+                            2'b00: begin
+                                expanded_instr = i_instr({6'b000000, shamt}, rs1_prime, 3'b101, rs1_prime, OPCODE_OP_IMM);
+                                illegal_instr = (shamt == 6'b0);
+                            end
+                            2'b01: begin
+                                expanded_instr = i_instr({6'b010000, shamt}, rs1_prime, 3'b101, rs1_prime, OPCODE_OP_IMM);
+                                illegal_instr = (shamt == 6'b0);
+                            end
+                            2'b10: expanded_instr = i_instr(ci_imm, rs1_prime, 3'b111, rs1_prime, OPCODE_OP_IMM);
+                            2'b11: begin
+                                unique case ({compressed_instr[12], compressed_instr[6:5]})
+                                    3'b000: expanded_instr = r_instr(7'b0100000, rs2_prime, rs1_prime, 3'b000, rs1_prime, OPCODE_OP);
+                                    3'b001: expanded_instr = r_instr(7'b0000000, rs2_prime, rs1_prime, 3'b100, rs1_prime, OPCODE_OP);
+                                    3'b010: expanded_instr = r_instr(7'b0000000, rs2_prime, rs1_prime, 3'b110, rs1_prime, OPCODE_OP);
+                                    3'b011: expanded_instr = r_instr(7'b0000000, rs2_prime, rs1_prime, 3'b111, rs1_prime, OPCODE_OP);
+                                    3'b100: expanded_instr = r_instr(7'b0100000, rs2_prime, rs1_prime, 3'b000, rs1_prime, OPCODE_OP_32);
+                                    3'b101: expanded_instr = r_instr(7'b0000000, rs2_prime, rs1_prime, 3'b000, rs1_prime, OPCODE_OP_32);
+                                    default: illegal_instr = 1'b1;
+                                endcase
+                            end
+                        endcase
+                    end
+                    3'b101: expanded_instr = j_instr(jump_imm, 5'd0);
+                    3'b110: expanded_instr = b_instr(branch_imm, 5'd0, rs1_prime, 3'b000);
+                    3'b111: expanded_instr = b_instr(branch_imm, 5'd0, rs1_prime, 3'b001);
+                    default: illegal_instr = 1'b1;
+                endcase
+            end
+
+            2'b10: begin
+                unique case (funct3)
+                    3'b000: begin
+                        expanded_instr = i_instr({6'b000000, shamt}, rd_rs1, 3'b001, rd_rs1, OPCODE_OP_IMM);
+                        illegal_instr = (shamt == 6'b0);
+                    end
+                    3'b001: begin
+                        expanded_instr = i_instr(ldsp_imm, 5'd2, 3'b011, rd_rs1, OPCODE_LOAD_FP);
+                    end
+                    3'b010: begin
+                        expanded_instr = i_instr(lwsp_imm, 5'd2, 3'b010, rd_rs1, OPCODE_LOAD);
+                        illegal_instr = (rd_rs1 == 5'd0);
+                    end
+                    3'b011: begin
+                        expanded_instr = i_instr(ldsp_imm, 5'd2, 3'b011, rd_rs1, OPCODE_LOAD);
+                        illegal_instr = (rd_rs1 == 5'd0);
+                    end
+                    3'b100: begin
+                        if (!compressed_instr[12] && (rs2 == 5'd0)) begin
+                            expanded_instr = i_instr(12'b0, rd_rs1, 3'b000, 5'd0, OPCODE_JALR);
+                            illegal_instr = (rd_rs1 == 5'd0);
+                        end else if (!compressed_instr[12]) begin
+                            expanded_instr = r_instr(7'b0000000, rs2, 5'd0, 3'b000, rd_rs1, OPCODE_OP);
+                        end else if (compressed_instr[12] && (rd_rs1 == 5'd0) && (rs2 == 5'd0)) begin
+                            expanded_instr = 32'h0010_0073; // EBREAK
+                        end else if (compressed_instr[12] && (rs2 == 5'd0)) begin
+                            expanded_instr = i_instr(12'b0, rd_rs1, 3'b000, 5'd1, OPCODE_JALR);
+                        end else begin
+                            expanded_instr = r_instr(7'b0000000, rs2, rd_rs1, 3'b000, rd_rs1, OPCODE_OP);
+                        end
+                    end
+                    3'b101: expanded_instr = s_instr(sdsp_imm, rs2, 5'd2, 3'b011, OPCODE_STORE_FP);
+                    3'b110: expanded_instr = s_instr(swsp_imm, rs2, 5'd2, 3'b010, OPCODE_STORE);
+                    3'b111: expanded_instr = s_instr(sdsp_imm, rs2, 5'd2, 3'b011, OPCODE_STORE);
+                    default: illegal_instr = 1'b1;
+                endcase
+            end
+
+            default: illegal_instr = 1'b1;
+        endcase
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/execute/hazard_unit.sv b/rv64cpu/rtl/core/execute/hazard_unit.sv
new file mode 100644
index 0000000..ee90356
--- /dev/null
+++ b/rv64cpu/rtl/core/execute/hazard_unit.sv
@@ -0,0 +1,200 @@
+//     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/hazard_unit.sv
+// Description: Hazard Control Unit
+// Handles data hazards (forwarding, Load-Use stalls), control hazards
+// (branch misprediction flushes), and memory stalls (D-Cache misses).
+
+module hazard_unit
+    import rv64_pkg::*;
+(
+    // -----------------------
+    // Inputs from ID Stage (for Load-Use detection)
+    // -----------------------
+    input  logic [4:0]  id_rs1_addr,
+    input  logic [4:0]  id_rs2_addr,
+
+    // -----------------------
+    // Inputs from EX Stage (ID/EX register outputs)
+    // -----------------------
+    input  logic [4:0]  ex_rs1_addr,
+    input  logic [4:0]  ex_rs2_addr,
+    input  logic [4:0]  ex_rd_addr,
+    input  logic        ex_reg_write_en,
+    input  logic        ex_is_load,
+    input  logic        ex_is_csr,
+    input  logic        ex_branch_taken,
+    input  logic [XLEN-1:0] ex_branch_target,
+
+    // -----------------------
+    // Inputs from MEM Stage (EX/MEM register outputs)
+    // -----------------------
+    input  logic [4:0]  mem_rd_addr,
+    input  logic        mem_reg_write_en,
+    input  logic        mem_result_late,
+    input  logic        mem_is_csr,
+    input  logic        mem_stall_req,       // D-Cache miss or memory busy
+
+    // -----------------------
+    // Trap Controller Inputs
+    // -----------------------
+    input  logic        trap_flush_req,
+    input  logic        trap_pc_sel,
+    input  logic [XLEN-1:0] trap_target_pc,
+
+    // -----------------------
+    // Inputs from WB Stage (MEM/WB register outputs)
+    // -----------------------
+    input  logic [4:0]  wb_rd_addr,
+    input  logic        wb_reg_write_en,
+
+    // -----------------------
+    // Outputs: EX-stage operand forwarding mux selects
+    // -----------------------
+    // 00: no forward (use regfile), 01: forward from MEM, 10: forward from WB
+    output logic [1:0]  forward_a_sel,
+    output logic [1:0]  forward_b_sel,
+
+    // -----------------------
+    // Outputs: Pipeline stall and flush controls
+    // -----------------------
+    output logic        if_stall_en,
+    output logic        if_id_stall_en,
+    output logic        id_ex_stall_en,
+    output logic        ex_mem_stall_en,
+    output logic        if_id_flush_en,
+    output logic        id_ex_flush_en,
+
+    // -----------------------
+    // Outputs: IF-stage PC redirect controls
+    // -----------------------
+    output logic        pc_redirect_sel,
+    output logic [XLEN-1:0] pc_redirect_target,
+    output logic        pc_redirect_is_trap
+);
+
+    // -----------------------
+    // Data Forwarding (EX-stage operand bypass)
+    // -----------------------
+    // Forwarding resolves RAW hazards by routing results from
+    // MEM or WB back to the EX ALU inputs. We compare EX-stage
+    // source register addresses (the instruction currently in EX)
+    // against the destination registers of instructions in MEM/WB.
+    //
+    // Priority: MEM > WB (MEM result is more recent).
+
+    // Operand A (ex_rs1_addr)
+    always_comb begin
+        if (mem_reg_write_en && !mem_result_late && (mem_rd_addr != 5'b0) && (mem_rd_addr == ex_rs1_addr)) begin
+            forward_a_sel = 2'b01; // Forward from MEM stage
+        end else if (wb_reg_write_en && (wb_rd_addr != 5'b0) && (wb_rd_addr == ex_rs1_addr)) begin
+            forward_a_sel = 2'b10; // Forward from WB stage
+        end else begin
+            forward_a_sel = 2'b00; // No forwarding
+        end
+    end
+
+    // Operand B (ex_rs2_addr)
+    always_comb begin
+        if (mem_reg_write_en && !mem_result_late && (mem_rd_addr != 5'b0) && (mem_rd_addr == ex_rs2_addr)) begin
+            forward_b_sel = 2'b01; // Forward from MEM stage
+        end else if (wb_reg_write_en && (wb_rd_addr != 5'b0) && (wb_rd_addr == ex_rs2_addr)) begin
+            forward_b_sel = 2'b10; // Forward from WB stage
+        end else begin
+            forward_b_sel = 2'b00; // No forwarding
+        end
+    end
+
+    // -----------------------
+    // Load-Use Hazard Detection
+    // -----------------------
+    // If the instruction in EX is a load, and the instruction in
+    // ID reads from the load's destination, we must stall for 1
+    // cycle because the load data is not available until MEM.
+    logic load_use_stall;
+    assign load_use_stall = ex_is_load && (ex_rd_addr != 5'b0) &&
+                            ((ex_rd_addr == id_rs1_addr) || (ex_rd_addr == id_rs2_addr));
+
+    logic csr_use_stall;
+    assign csr_use_stall = ((ex_is_csr && ex_reg_write_en && (ex_rd_addr != 5'b0) &&
+                             ((ex_rd_addr == id_rs1_addr) || (ex_rd_addr == id_rs2_addr))) ||
+                            (mem_is_csr && mem_reg_write_en && (mem_rd_addr != 5'b0) &&
+                             ((mem_rd_addr == id_rs1_addr) || (mem_rd_addr == id_rs2_addr))));
+
+    // -----------------------
+    // PC Redirect Selection (Trap has top priority)
+    // -----------------------
+    always_comb begin
+        pc_redirect_sel      = 1'b0;
+        pc_redirect_target   = '0;
+        pc_redirect_is_trap  = 1'b0;
+
+        if (trap_pc_sel) begin
+            pc_redirect_sel     = 1'b1;
+            pc_redirect_target  = trap_target_pc;
+            pc_redirect_is_trap = 1'b1;
+        end else if (ex_branch_taken) begin
+            pc_redirect_sel     = 1'b1;
+            pc_redirect_target  = ex_branch_target;
+        end
+    end
+
+    // -----------------------
+    // Stall and Flush Signal Generation
+    // -----------------------
+    always_comb begin
+        // Defaults: no stalls, no flushes
+        if_stall_en     = 1'b0;
+        if_id_stall_en  = 1'b0;
+        id_ex_stall_en  = 1'b0;
+        ex_mem_stall_en = 1'b0;
+        if_id_flush_en  = 1'b0;
+        id_ex_flush_en  = 1'b0;
+
+        if (trap_flush_req) begin
+            if_id_flush_en = 1'b1;
+            id_ex_flush_en = 1'b1;
+        end else begin
+            // (1) Memory stall (D-Cache miss): freeze the entire pipeline
+            if (mem_stall_req) begin
+                if_stall_en     = 1'b1;
+                if_id_stall_en  = 1'b1;
+                id_ex_stall_en  = 1'b1;
+                ex_mem_stall_en = 1'b1;
+            end
+
+            // (2) Value-use stall: freeze IF and IF/ID, insert bubble into ID/EX.
+            // Loads and CSR reads both produce their register result after EX.
+            if ((load_use_stall || csr_use_stall) && !mem_stall_req) begin
+                if_stall_en    = 1'b1;
+                if_id_stall_en = 1'b1;
+                id_ex_flush_en = 1'b1;
+            end
+
+            // (3) Branch/jump misprediction: flush IF/ID and ID/EX
+            // Branch flush overrides load-use stall (the stalled instruction is invalid anyway)
+            if (ex_branch_taken) begin
+                if_id_flush_en = 1'b1;
+                id_ex_flush_en = 1'b1;
+                // Cancel load-use stall if branch is also taken
+                if_stall_en    = 1'b0;
+                if_id_stall_en = 1'b0;
+            end
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/execute/instruction_decoder.sv b/rv64cpu/rtl/core/execute/instruction_decoder.sv
new file mode 100644
index 0000000..14d9559
--- /dev/null
+++ b/rv64cpu/rtl/core/execute/instruction_decoder.sv
@@ -0,0 +1,672 @@
+//     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/decoder.sv
+// Description: Instruction decoder for RV64IMAFD base ISA + Zicsr + Zifencei
+//              Translates 32-bit instructions into control signals for the pipeline.
+//              C is handled by the IF-stage decompressor before this decoder.
+
+module instruction_decoder
+    import rv64_pkg::*;
+(
+    input  logic [31:0]       instr,          // 32-bit instruction
+    input  logic [1:0]        priv_mode,      // Current privilege mode from CSR
+    input  logic              mstatus_tsr,    // Trap SRET in S-mode
+    input  logic              mstatus_tw,     // Trap WFI in S/U-mode
+    input  logic              mstatus_tvm,    // Trap supervisor memory-management ops in S-mode
+
+    // Register File
+    output logic [4:0]        rs1_addr,       // Source register 1 address
+    output logic [4:0]        rs2_addr,       // Source register 2 address
+    output logic [4:0]        rd_addr,        // Destination register address
+    output logic              reg_write_en,      // Register file write enable
+
+    // ALU Control
+    output alu_op_t           alu_op,         // ALU operation select
+    output logic              alu_src1_sel,   // 0 = rs1,  1 = PC
+    output logic              alu_src2_sel,   // 0 = rs2,  1 = immediate
+
+    // Immediate
+    output logic [XLEN-1:0]   imm,            // Sign-extended immediate
+
+    // Memory Access
+    output logic              is_load,        // MEM stage: read from memory
+    output logic              is_store,       // MEM stage: write to memory
+    output logic [2:0]        mem_size,       // funct3 → byte/half/word/double + sign
+    output logic              is_amo,         // A extension atomic memory operation
+    output amo_op_t           amo_op,         // LR/SC/AMO operation selector
+    output logic              is_fp,
+    output logic              fp_reg_write_en,
+    output logic              fp_store_data_sel,
+    output fp_op_t            fp_op,
+    output fp_fmt_t           fp_fmt,
+    output logic [2:0]        fp_rm,
+
+    // Branch / Jump
+    output logic              is_branch,      // EX: conditional branch
+    output logic              is_jump,        // EX: unconditional jump (JAL/JALR)
+    output logic [2:0]        branch_op,      // funct3 → BEQ/BNE/BLT/BGE/BLTU/BGEU
+
+    // Writeback Source
+    output logic [1:0]        wb_sel,         // 0 = ALU result, 1 = MEM data, 2 = PC+4
+
+    // RV64 Word Operations
+    output logic              is_word_op,     // EX: operate on lower 32 bits, sext result
+
+    // System / Privilege
+    output logic              is_csr,         // CSR instruction
+    output logic [2:0]        csr_op,         // funct3 → CSRRW/CSRRS/CSRRC/CSRRWI/CSRRSI/CSRRCI
+    output logic              is_ecall,       // ECALL
+    output logic              is_ebreak,      // EBREAK
+    output logic              is_mret,        // MRET
+    output logic              is_sret,        // SRET
+    output logic              is_wfi,         // WFI
+    output logic              is_sfence_vma,  // SFENCE.VMA
+    output logic              is_fence,       // FENCE
+    output logic              is_fence_i,     // FENCE.I (Zifencei)
+
+    // Error
+    output logic              illegal_instr   // Unrecognised or malformed instruction
+);
+
+    // ============================
+    // Instruction Field Extraction
+    // ============================
+    typedef enum logic [1:0] {
+        PRIV_MODE_U = 2'b00,
+        PRIV_MODE_S = 2'b01,
+        PRIV_MODE_M = 2'b11
+    } priv_mode_t;
+
+    logic [6:0] opcode;
+    logic [2:0] funct3;
+    logic [6:0] funct7;
+    logic [4:0] amo_funct5;
+
+    assign opcode   = instr[6:0];
+    assign rd_addr  = instr[11:7];
+    assign funct3   = instr[14:12];
+    assign rs1_addr = (opcode == OPCODE_LUI) ? 5'b00000 : instr[19:15];
+    assign rs2_addr = instr[24:20];
+    assign funct7   = instr[31:25];
+    assign amo_funct5 = instr[31:27];
+
+    // ============================
+    // Immediate Generation
+    // ============================
+    logic [XLEN-1:0] imm_i, imm_s, imm_b, imm_u, imm_j;
+    logic [11:0]     csr_addr_field;
+
+    // I-type:  instr[31:20]
+    assign imm_i = {{52{instr[31]}}, instr[31:20]};
+
+    // S-type:  {instr[31:25], instr[11:7]}
+    assign imm_s = {{52{instr[31]}}, instr[31:25], instr[11:7]};
+
+    // B-type:  {instr[31], instr[7], instr[30:25], instr[11:8], 1'b0}
+    assign imm_b = {{51{instr[31]}}, instr[31], instr[7], instr[30:25], instr[11:8], 1'b0};
+
+    // U-type:  {instr[31:12], 12'b0}
+    assign imm_u = {{32{instr[31]}}, instr[31:12], 12'b0};
+
+    // J-type:  {instr[31], instr[19:12], instr[20], instr[30:21], 1'b0}
+    assign imm_j = {{43{instr[31]}}, instr[31], instr[19:12], instr[20], instr[30:21], 1'b0};
+    assign csr_addr_field = instr[31:20];
+
+    function automatic int priv_rank(logic [1:0] mode);
+        case (mode)
+            PRIV_MODE_U: return 0;
+            PRIV_MODE_S: return 1;
+            2'b10:       return 2; // Reserved/H-mode, treat as higher than S
+            default:     return 3; // Machine mode (or anything else) highest
+        endcase
+    endfunction
+
+    function automatic logic priv_ge(logic [1:0] cur, logic [1:0] req);
+        return priv_rank(cur) >= priv_rank(req);
+    endfunction
+
+    function automatic logic csr_access_allowed(logic [1:0] cur, logic [11:0] addr);
+        logic [1:0] req_priv;
+        req_priv = addr[9:8];
+        return priv_ge(cur, req_priv);
+    endfunction
+
+    // ============================
+    // Main Decode Logic
+    // ============================
+    always_comb begin
+        // Default values (all signals inactive)
+        alu_op        = ALU_ADD;
+        alu_src1_sel  = 1'b0;   // rs1
+        alu_src2_sel  = 1'b0;   // rs2
+        reg_write_en  = 1'b0;
+        imm           = '0;
+        is_load       = 1'b0;
+        is_store      = 1'b0;
+        mem_size      = 3'b0;
+        is_amo        = 1'b0;
+        amo_op        = AMO_NONE;
+        is_fp         = 1'b0;
+        fp_reg_write_en  = 1'b0;
+        fp_store_data_sel = 1'b0;
+        fp_op         = FP_OP_NONE;
+        fp_fmt        = FP_FMT_S;
+        fp_rm         = funct3;
+        is_branch     = 1'b0;
+        is_jump       = 1'b0;
+        branch_op     = 3'b0;
+        wb_sel        = 2'b00;  // ALU result
+        is_word_op    = 1'b0;
+        is_csr        = 1'b0;
+        csr_op        = 3'b0;
+        is_ecall      = 1'b0;
+        is_ebreak     = 1'b0;
+        is_mret       = 1'b0;
+        is_sret       = 1'b0;
+        is_wfi        = 1'b0;
+        is_sfence_vma = 1'b0;
+        is_fence      = 1'b0;
+        is_fence_i    = 1'b0;
+        illegal_instr = 1'b0;
+
+        case (opcode)
+
+            // LUI — rd = imm_u
+
+            OPCODE_LUI: begin
+                reg_write_en = 1'b1;
+                alu_src1_sel = 1'b0;    // doesn't matter,  ALU_ADD(0, imm_u)
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_u;
+                alu_op       = ALU_ADD; // result = 0 + imm_u  (rs1 forced to x0 externally or use alu_src1_sel trick)
+                // NOTE: EX stage should feed 0 to ALU operand 1 when alu_src1_sel==0 and opcode==LUI.
+                // Alternative: add a dedicated alu_src1_sel=2 for "zero". For now, the pipeline
+                // can detect LUI by opcode and zero out the operand.
+            end
+
+            // AUIPC — rd = PC + imm_u
+
+
+            OPCODE_AUIPC: begin
+                reg_write_en = 1'b1;
+                alu_src1_sel = 1'b1;    // PC
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_u;
+                alu_op       = ALU_ADD; // result = PC + imm_u
+            end
+
+            // -----------------
+            // JAL — rd = PC+4;  PC = PC + imm_j
+            OPCODE_JAL: begin
+                reg_write_en = 1'b1;
+                is_jump      = 1'b1;
+                alu_src1_sel = 1'b1;    // PC
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_j;
+                alu_op       = ALU_ADD; // target = PC + imm_j
+                wb_sel       = 2'b10;   // writeback PC+4
+            end
+
+            // ------------------
+            // JALR — rd = PC+4;  PC = (rs1 + imm_i) & ~1
+
+            OPCODE_JALR: begin
+                reg_write_en = 1'b1;
+                is_jump      = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_i;
+                alu_op       = ALU_ADD; // target = rs1 + imm_i  (LSB cleared by EX stage)
+                wb_sel       = 2'b10;   // writeback PC+4
+            end
+
+            // -----------------
+            // Branch — compare rs1, rs2;  if taken, PC = PC + imm_b
+            OPCODE_BRANCH: begin
+                reg_write_en = 1'b0;    // branches never write rd
+                is_branch    = 1'b1;
+                alu_src1_sel = 1'b1;    // PC  (ALU computes branch target)
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_b;
+                alu_op       = ALU_ADD; // branch target = PC + imm_b
+                branch_op    = funct3;  // BEQ/BNE/BLT/BGE/BLTU/BGEU
+            end
+
+            // -----------------
+            // Load — rd = Mem[rs1 + imm_i]
+            OPCODE_LOAD: begin
+                reg_write_en = 1'b1;
+                is_load      = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_i;
+                alu_op       = ALU_ADD; // effective address = rs1 + imm_i
+                mem_size     = funct3;  // LB/LH/LW/LD/LBU/LHU/LWU
+                wb_sel       = 2'b01;   // writeback from memory
+            end
+
+            // -----------------
+            // Store — Mem[rs1 + imm_s] = rs2
+            OPCODE_STORE: begin
+                reg_write_en = 1'b0;
+                is_store     = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b1;    // immediate  (NOT rs2 — rs2 is store data)
+                imm          = imm_s;
+                alu_op       = ALU_ADD; // effective address = rs1 + imm_s
+                mem_size     = funct3;  // SB/SH/SW/SD
+            end
+
+            // -----------------
+            // OP-IMM (I-type ALU) — rd = rs1 OP imm
+            // ADDI, SLTI, SLTIU, XORI, ORI, ANDI, SLLI, SRLI, SRAI
+            OPCODE_OP_IMM: begin
+                reg_write_en = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_i;
+
+                case (funct3)
+                    3'b000:  alu_op = ALU_ADD;                              // ADDI
+                    3'b010:  alu_op = ALU_SLT;                              // SLTI
+                    3'b011:  alu_op = ALU_SLTU;                             // SLTIU
+                    3'b100:  alu_op = ALU_XOR;                              // XORI
+                    3'b110:  alu_op = ALU_OR;                               // ORI
+                    3'b111:  alu_op = ALU_AND;                              // ANDI
+                    3'b001:  alu_op = ALU_SLL;                              // SLLI
+                    3'b101:  alu_op = (funct7[5]) ? ALU_SRA : ALU_SRL;      // SRAI / SRLI
+                    default: alu_op = ALU_ADD;
+                endcase
+            end
+
+            // -----------------
+            // OP-IMM-32 (I-type ALU, 32-bit word variants for RV64)
+            // ADDIW, SLLIW, SRLIW, SRAIW
+            OPCODE_OP_IMM_32: begin
+                reg_write_en = 1'b1;
+                is_word_op   = 1'b1;    // EX stage: operate on lower 32 bits, sext result
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_i;
+
+                case (funct3)
+                    3'b000:  alu_op = ALU_ADD;                              // ADDIW
+                    3'b001:  alu_op = ALU_SLL;                              // SLLIW
+                    3'b101:  alu_op = (funct7[5]) ? ALU_SRA : ALU_SRL;      // SRAIW / SRLIW
+                    default: begin
+                        alu_op = ALU_ADD;
+                        illegal_instr = 1'b1;
+                    end
+                endcase
+            end
+
+            // -----------------
+            // OP (R-type ALU) — rd = rs1 OP rs2
+            // ADD, SUB, SLL, SLT, SLTU, XOR, SRL, SRA, OR, AND
+            OPCODE_OP: begin
+                reg_write_en = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b0;    // rs2
+
+                if (funct7 == 7'b0000001) begin
+                    case (funct3)
+                        3'b000:  alu_op = ALU_MUL;
+                        3'b001:  alu_op = ALU_MULH;
+                        3'b010:  alu_op = ALU_MULHSU;
+                        3'b011:  alu_op = ALU_MULHU;
+                        3'b100:  alu_op = ALU_DIV;
+                        3'b101:  alu_op = ALU_DIVU;
+                        3'b110:  alu_op = ALU_REM;
+                        3'b111:  alu_op = ALU_REMU;
+                        default: illegal_instr = 1'b1;
+                    endcase
+                end else begin
+                    case (funct3)
+                        3'b000:  alu_op = (funct7[5]) ? ALU_SUB : ALU_ADD;  // SUB / ADD
+                        3'b001:  alu_op = ALU_SLL;                          // SLL
+                        3'b010:  alu_op = ALU_SLT;                          // SLT
+                        3'b011:  alu_op = ALU_SLTU;                         // SLTU
+                        3'b100:  alu_op = ALU_XOR;                          // XOR
+                        3'b101:  alu_op = (funct7[5]) ? ALU_SRA : ALU_SRL;  // SRA / SRL
+                        3'b110:  alu_op = ALU_OR;                           // OR
+                        3'b111:  alu_op = ALU_AND;                          // AND
+                        default: alu_op = ALU_ADD;
+                    endcase
+                end
+            end
+
+            // -----------------
+            // OP-32 (R-type ALU, 32-bit word variants for RV64)
+            // ADDW, SUBW, SLLW, SRLW, SRAW
+            OPCODE_OP_32: begin
+                reg_write_en = 1'b1;
+                is_word_op   = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b0;    // rs2
+
+                if (funct7 == 7'b0000001) begin
+                    case (funct3)
+                        3'b000:  alu_op = ALU_MUL;
+                        3'b100:  alu_op = ALU_DIV;
+                        3'b101:  alu_op = ALU_DIVU;
+                        3'b110:  alu_op = ALU_REM;
+                        3'b111:  alu_op = ALU_REMU;
+                        default: illegal_instr = 1'b1;
+                    endcase
+                end else begin
+                    case (funct3)
+                        3'b000:  alu_op = (funct7[5]) ? ALU_SUB : ALU_ADD;  // SUBW / ADDW
+                        3'b001:  alu_op = ALU_SLL;                          // SLLW
+                        3'b101:  alu_op = (funct7[5]) ? ALU_SRA : ALU_SRL;  // SRAW / SRLW
+                        default: begin
+                            alu_op = ALU_ADD;
+                            illegal_instr = 1'b1;
+                        end
+                    endcase
+                end
+            end
+
+            // -----------------
+            // MISC-MEM — FENCE / FENCE.I
+            OPCODE_MISC_MEM: begin
+                case (funct3)
+                    3'b000:  is_fence   = 1'b1;  // FENCE  (NOP in single-core in-order)
+                    3'b001:  is_fence_i = 1'b1;  // FENCE.I — flush I-Cache
+                    default: illegal_instr = 1'b1;
+                endcase
+            end
+
+            // -----------------
+            // SYSTEM — ECALL / EBREAK / xRET / WFI / SFENCE.VMA / CSR*
+            OPCODE_SYSTEM: begin
+                if (funct3 == 3'b000) begin
+                    // Privileged instructions (non-CSR)
+                    reg_write_en = 1'b0;
+                    case ({funct7, instr[24:20]})    // {funct7, rs2}
+                        12'b0000000_00000: is_ecall      = 1'b1;   // ECALL
+                        12'b0000000_00001: is_ebreak     = 1'b1;   // EBREAK
+                        12'b0011000_00010: begin                  // MRET
+                            if (priv_mode == PRIV_MODE_M)
+                                is_mret = 1'b1;
+                            else
+                                illegal_instr = 1'b1;
+                        end
+                        12'b0001000_00010: begin                  // SRET
+                            if (!priv_ge(priv_mode, PRIV_MODE_S))
+                                illegal_instr = 1'b1;
+                            else if ((priv_mode == PRIV_MODE_S) && mstatus_tsr)
+                                illegal_instr = 1'b1;
+                            else
+                                is_sret = 1'b1;
+                        end
+                        12'b0001000_00101: begin                  // WFI
+                            if ((priv_mode != PRIV_MODE_M) && mstatus_tw)
+                                illegal_instr = 1'b1;
+                            else
+                                is_wfi = 1'b1;
+                        end
+                        default: begin
+                            if (funct7 == 7'b0001001) begin
+                                if (!priv_ge(priv_mode, PRIV_MODE_S))
+                                    illegal_instr = 1'b1;
+                                else if ((priv_mode == PRIV_MODE_S) && mstatus_tvm)
+                                    illegal_instr = 1'b1;
+                                else
+                                    is_sfence_vma = 1'b1;           // SFENCE.VMA
+                            end else begin
+                                illegal_instr = 1'b1;
+                            end
+                        end
+                    endcase
+                end else begin
+                    // CSR instructions
+                    // funct3: 001=CSRRW, 010=CSRRS, 011=CSRRC,
+                    //         101=CSRRWI, 110=CSRRSI, 111=CSRRCI
+                    reg_write_en = 1'b1;
+                    is_csr       = 1'b1;
+                    csr_op       = funct3;
+                    imm          = imm_i;   // CSR address = instr[31:20] (reuse imm_i path)
+                    if (!csr_access_allowed(priv_mode, csr_addr_field)) begin
+                        reg_write_en  = 1'b0;
+                        is_csr        = 1'b0;
+                        illegal_instr = 1'b1;
+                    end
+                end
+            end
+
+            // -----------------
+            // AMO — A extension (LR/SC/AMO*)
+            OPCODE_AMO: begin
+                reg_write_en = 1'b1;
+                is_amo       = 1'b1;
+                alu_src1_sel = 1'b0;
+                alu_src2_sel = 1'b1;
+                imm          = '0;
+                alu_op       = ALU_ADD;
+                wb_sel       = 2'b01;
+
+                unique case (funct3)
+                    3'b010:  mem_size = 3'b010; // AMO*.W
+                    3'b011:  mem_size = 3'b011; // AMO*.D
+                    default: illegal_instr = 1'b1;
+                endcase
+
+                unique case (amo_funct5)
+                    5'b00010: begin
+                        amo_op = AMO_LR;
+                        if (rs2_addr != 5'd0) begin
+                            illegal_instr = 1'b1;
+                        end
+                    end
+                    5'b00011: amo_op = AMO_SC;
+                    5'b00001: amo_op = AMO_SWAP;
+                    5'b00000: amo_op = AMO_ADD;
+                    5'b00100: amo_op = AMO_XOR;
+                    5'b01100: amo_op = AMO_AND;
+                    5'b01000: amo_op = AMO_OR;
+                    5'b10000: amo_op = AMO_MIN;
+                    5'b10100: amo_op = AMO_MAX;
+                    5'b11000: amo_op = AMO_MINU;
+                    5'b11100: amo_op = AMO_MAXU;
+                    default: begin
+                        amo_op        = AMO_NONE;
+                        illegal_instr = 1'b1;
+                    end
+                endcase
+
+                if (illegal_instr) begin
+                    reg_write_en = 1'b0;
+                    is_amo       = 1'b0;
+                    amo_op       = AMO_NONE;
+                end
+            end
+
+            // -----------------
+            // Floating-Point — F/D extensions
+            OPCODE_LOAD_FP: begin
+                is_fp           = 1'b1;
+                is_load         = 1'b1;
+                fp_reg_write_en = 1'b1;
+                alu_src1_sel    = 1'b0;
+                alu_src2_sel    = 1'b1;
+                imm             = imm_i;
+                alu_op          = ALU_ADD;
+                wb_sel          = 2'b01;
+                unique case (funct3)
+                    3'b010: begin
+                        mem_size = 3'b010; // FLW
+                        fp_fmt   = FP_FMT_S;
+                    end
+                    3'b011: begin
+                        mem_size = 3'b011; // FLD
+                        fp_fmt   = FP_FMT_D;
+                    end
+                    default: begin
+                        is_fp           = 1'b0;
+                        fp_reg_write_en = 1'b0;
+                        illegal_instr   = 1'b1;
+                    end
+                endcase
+            end
+
+            OPCODE_STORE_FP: begin
+                is_fp             = 1'b1;
+                is_store          = 1'b1;
+                fp_store_data_sel = 1'b1;
+                alu_src1_sel      = 1'b0;
+                alu_src2_sel      = 1'b1;
+                imm               = imm_s;
+                alu_op            = ALU_ADD;
+                unique case (funct3)
+                    3'b010: begin
+                        mem_size = 3'b010; // FSW
+                        fp_fmt   = FP_FMT_S;
+                    end
+                    3'b011: begin
+                        mem_size = 3'b011; // FSD
+                        fp_fmt   = FP_FMT_D;
+                    end
+                    default: begin
+                        is_fp             = 1'b0;
+                        fp_store_data_sel = 1'b0;
+                        illegal_instr     = 1'b1;
+                    end
+                endcase
+            end
+
+            OPCODE_MADD,
+            OPCODE_MSUB,
+            OPCODE_NMSUB,
+            OPCODE_NMADD: begin
+                is_fp           = 1'b1;
+                fp_reg_write_en = 1'b1;
+                fp_rm           = funct3;
+                unique case (instr[26:25])
+                    2'b00: fp_fmt = FP_FMT_S;
+                    2'b01: fp_fmt = FP_FMT_D;
+                    default: illegal_instr = 1'b1;
+                endcase
+                unique case (opcode)
+                    OPCODE_MADD:  fp_op = FP_OP_MADD;
+                    OPCODE_MSUB:  fp_op = FP_OP_MSUB;
+                    OPCODE_NMSUB: fp_op = FP_OP_NMSUB;
+                    default:      fp_op = FP_OP_NMADD;
+                endcase
+                if (illegal_instr) begin
+                    is_fp           = 1'b0;
+                    fp_reg_write_en = 1'b0;
+                    fp_op           = FP_OP_NONE;
+                end
+            end
+
+            OPCODE_OP_FP: begin
+                is_fp = 1'b1;
+                fp_rm = funct3;
+                unique case (instr[26:25])
+                    2'b00: fp_fmt = FP_FMT_S;
+                    2'b01: fp_fmt = FP_FMT_D;
+                    default: illegal_instr = 1'b1;
+                endcase
+
+                unique case (funct7)
+                    7'b0000000, 7'b0000001: begin fp_op = FP_OP_ADD; fp_reg_write_en = 1'b1; end
+                    7'b0000100, 7'b0000101: begin fp_op = FP_OP_SUB; fp_reg_write_en = 1'b1; end
+                    7'b0001000, 7'b0001001: begin fp_op = FP_OP_MUL; fp_reg_write_en = 1'b1; end
+                    7'b0001100, 7'b0001101: begin fp_op = FP_OP_DIV; fp_reg_write_en = 1'b1; end
+                    7'b0101100, 7'b0101101: begin
+                        fp_op = FP_OP_SQRT;
+                        fp_reg_write_en = 1'b1;
+                        if (rs2_addr != 5'd0) illegal_instr = 1'b1;
+                    end
+                    7'b0010000, 7'b0010001: begin
+                        fp_reg_write_en = 1'b1;
+                        unique case (funct3)
+                            3'b000: fp_op = FP_OP_SGNJ;
+                            3'b001: fp_op = FP_OP_SGNJN;
+                            3'b010: fp_op = FP_OP_SGNJX;
+                            default: illegal_instr = 1'b1;
+                        endcase
+                    end
+                    7'b0010100, 7'b0010101: begin
+                        fp_reg_write_en = 1'b1;
+                        unique case (funct3)
+                            3'b000: fp_op = FP_OP_MIN;
+                            3'b001: fp_op = FP_OP_MAX;
+                            default: illegal_instr = 1'b1;
+                        endcase
+                    end
+                    7'b0100000, 7'b0100001: begin
+                        fp_op = FP_OP_CVT_F2F;
+                        fp_reg_write_en = 1'b1;
+                    end
+                    7'b1010000, 7'b1010001: begin
+                        reg_write_en = 1'b1;
+                        unique case (funct3)
+                            3'b010: fp_op = FP_OP_EQ;
+                            3'b001: fp_op = FP_OP_LT;
+                            3'b000: fp_op = FP_OP_LE;
+                            default: illegal_instr = 1'b1;
+                        endcase
+                    end
+                    7'b1100000, 7'b1100001: begin
+                        fp_op = FP_OP_CVT_F2I;
+                        reg_write_en = 1'b1;
+                    end
+                    7'b1101000, 7'b1101001: begin
+                        fp_op = FP_OP_CVT_I2F;
+                        fp_reg_write_en = 1'b1;
+                    end
+                    7'b1110000, 7'b1110001: begin
+                        if (rs2_addr == 5'd0 && funct3 == 3'b000) begin
+                            fp_op = FP_OP_MV_F2X;
+                            reg_write_en = 1'b1;
+                        end else if (rs2_addr == 5'd0 && funct3 == 3'b001) begin
+                            fp_op = FP_OP_CLASS;
+                            reg_write_en = 1'b1;
+                        end else begin
+                            illegal_instr = 1'b1;
+                        end
+                    end
+                    7'b1111000, 7'b1111001: begin
+                        if (rs2_addr == 5'd0 && funct3 == 3'b000) begin
+                            fp_op = FP_OP_MV_X2F;
+                            fp_reg_write_en = 1'b1;
+                        end else begin
+                            illegal_instr = 1'b1;
+                        end
+                    end
+                    default: illegal_instr = 1'b1;
+                endcase
+
+                if (illegal_instr) begin
+                    is_fp           = 1'b0;
+                    fp_reg_write_en = 1'b0;
+                    reg_write_en    = 1'b0;
+                    fp_op           = FP_OP_NONE;
+                end
+            end
+
+            // -----------------
+            // Unrecognised opcode
+            default: begin
+                illegal_instr = 1'b1;
+            end
+
+        endcase
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/execute/integer_alu.sv b/rv64cpu/rtl/core/execute/integer_alu.sv
new file mode 100644
index 0000000..833f902
--- /dev/null
+++ b/rv64cpu/rtl/core/execute/integer_alu.sv
@@ -0,0 +1,123 @@
+//     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/alu.sv
+// Description: 64-bit Arithmetic Logic Unit (ALU)
+
+module integer_alu
+    import rv64_pkg::*;
+(
+    input  alu_op_t          alu_op,     // ALU operation select (from decoder)
+    input  logic [XLEN-1:0]  operand_a,    // Operand 1 (register rs1 value or PC)
+    input  logic [XLEN-1:0]  operand_b,    // Operand 2 (register rs2 value or immediate)
+
+    output logic [XLEN-1:0]  alu_result     // ALU result
+);
+
+    // Shift amount: RV64I uses lower 6 bits of rs2 (shamt[5:0])
+    logic [5:0] shamt;
+    assign shamt = operand_b[5:0];
+
+    logic signed [XLEN-1:0] operand_a_signed;
+    logic signed [XLEN-1:0] operand_b_signed;
+    logic signed [127:0]    mul_signed_signed;
+    logic signed [127:0]    mul_signed_unsigned;
+    logic [127:0]           mul_unsigned_unsigned;
+
+    assign operand_a_signed     = operand_a;
+    assign operand_b_signed     = operand_b;
+    assign mul_signed_signed    = operand_a_signed * operand_b_signed;
+    assign mul_signed_unsigned  = operand_a_signed * $signed({1'b0, operand_b});
+    assign mul_unsigned_unsigned = operand_a * operand_b;
+
+    always_comb begin
+        alu_result = '0;
+
+        case (alu_op)
+            ALU_ADD:  alu_result = operand_a + operand_b;
+            ALU_SUB:  alu_result = operand_a - operand_b;
+
+            // Logical left shift: the shift amount is the lower 6 bits of rs2
+            ALU_SLL:  alu_result = operand_a << shamt;
+
+            // Set less than (signed)
+            ALU_SLT:  alu_result = {63'b0, $signed(operand_a) < $signed(operand_b)};
+
+            // Set less than (unsigned)
+            ALU_SLTU: alu_result = {63'b0, operand_a < operand_b};
+
+            ALU_XOR:  alu_result = operand_a ^ operand_b;
+
+            // Logical right shift: the shift amount is the lower 6 bits of rs2
+            ALU_SRL:  alu_result = operand_a >> shamt;
+
+            // Arithmetic right shift: the shift amount is the lower 6 bits of rs2
+            ALU_SRA:  alu_result = $unsigned($signed(operand_a) >>> shamt);
+
+            ALU_OR:   alu_result = operand_a | operand_b;
+            ALU_AND:  alu_result = operand_a & operand_b;
+
+            ALU_MUL: begin
+                alu_result = mul_signed_signed[XLEN-1:0];
+            end
+
+            ALU_MULH: begin
+                alu_result = mul_signed_signed[127:64];
+            end
+
+            ALU_MULHSU: begin
+                alu_result = mul_signed_unsigned[127:64];
+            end
+
+            ALU_MULHU: begin
+                alu_result = mul_unsigned_unsigned[127:64];
+            end
+
+            ALU_DIV: begin
+                if (operand_b == '0) begin
+                    alu_result = {XLEN{1'b1}};
+                end else if ((operand_a == 64'h8000_0000_0000_0000) &&
+                             (operand_b == 64'hFFFF_FFFF_FFFF_FFFF)) begin
+                    alu_result = operand_a;
+                end else begin
+                    alu_result = $unsigned(operand_a_signed / operand_b_signed);
+                end
+            end
+
+            ALU_DIVU: begin
+                alu_result = (operand_b == '0) ? {XLEN{1'b1}} : (operand_a / operand_b);
+            end
+
+            ALU_REM: begin
+                if (operand_b == '0) begin
+                    alu_result = operand_a;
+                end else if ((operand_a == 64'h8000_0000_0000_0000) &&
+                             (operand_b == 64'hFFFF_FFFF_FFFF_FFFF)) begin
+                    alu_result = '0;
+                end else begin
+                    alu_result = $unsigned(operand_a_signed % operand_b_signed);
+                end
+            end
+
+            ALU_REMU: begin
+                alu_result = (operand_b == '0) ? operand_a : (operand_a % operand_b);
+            end
+
+            default:  alu_result = '0;
+        endcase
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/fpu/fpu.sv b/rv64cpu/rtl/core/fpu/fpu.sv
new file mode 100644
index 0000000..9e9494d
--- /dev/null
+++ b/rv64cpu/rtl/core/fpu/fpu.sv
@@ -0,0 +1,2392 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+// File path: rtl/core/fpu/fpu_unit.sv
+// Description: RV64F/RV64D floating-point execution unit.
+
+module fpu
+    import rv64_pkg::*;
+(
+    input  fp_op_t            fp_op,
+    input  fp_fmt_t           fp_fmt,
+    input  logic [2:0]        rm,
+    input  logic [2:0]        funct3,
+    input  logic [4:0]        rs2_addr,
+    input  logic [XLEN-1:0]   fs1_data,
+    input  logic [XLEN-1:0]   fs2_data,
+    input  logic [XLEN-1:0]   fs3_data,
+    input  logic [XLEN-1:0]   int_rs1_data,
+
+    output logic [XLEN-1:0]   fp_result,
+    output logic [XLEN-1:0]   int_result,
+    output logic [4:0]        fflags,
+    output logic              illegal_instr
+);
+
+    localparam logic [31:0] CANONICAL_NAN_S = 32'h7FC0_0000;
+    localparam logic [63:0] CANONICAL_NAN_D = 64'h7FF8_0000_0000_0000;
+
+    logic [31:0] s1_raw;
+    logic [31:0] s2_raw;
+    logic [31:0] s3_raw;
+    logic [31:0] s1;
+    logic [31:0] s2;
+    logic [31:0] s3;
+    logic [63:0] d1;
+    logic [63:0] d2;
+    logic [63:0] d3;
+
+    assign s1_raw = fs1_data[31:0];
+    assign s2_raw = fs2_data[31:0];
+    assign s3_raw = fs3_data[31:0];
+    assign s1 = (fs1_data[63:32] == 32'hFFFF_FFFF) ? s1_raw : CANONICAL_NAN_S;
+    assign s2 = (fs2_data[63:32] == 32'hFFFF_FFFF) ? s2_raw : CANONICAL_NAN_S;
+    assign s3 = (fs3_data[63:32] == 32'hFFFF_FFFF) ? s3_raw : CANONICAL_NAN_S;
+    assign d1 = fs1_data;
+    assign d2 = fs2_data;
+    assign d3 = fs3_data;
+
+    function automatic logic is_nan_s(input logic [31:0] value);
+        return (&value[30:23]) && (|value[22:0]);
+    endfunction
+
+    function automatic logic is_snan_s(input logic [31:0] value);
+        return is_nan_s(value) && (value[22] == 1'b0);
+    endfunction
+
+    function automatic logic is_nan_d(input logic [63:0] value);
+        return (&value[62:52]) && (|value[51:0]);
+    endfunction
+
+    function automatic logic is_snan_d(input logic [63:0] value);
+        return is_nan_d(value) && (value[51] == 1'b0);
+    endfunction
+
+    function automatic logic [9:0] class_s(input logic [31:0] value);
+        logic sign;
+        logic [7:0] exp;
+        logic [22:0] frac;
+        sign = value[31];
+        exp = value[30:23];
+        frac = value[22:0];
+        class_s = '0;
+        if (&exp && (frac != '0)) begin
+            class_s[is_snan_s(value) ? 8 : 9] = 1'b1;
+        end else if (sign && (&exp) && (frac == '0)) begin
+            class_s[0] = 1'b1;
+        end else if (sign && (exp != '0)) begin
+            class_s[1] = 1'b1;
+        end else if (sign && (exp == '0) && (frac != '0)) begin
+            class_s[2] = 1'b1;
+        end else if (sign && (exp == '0) && (frac == '0)) begin
+            class_s[3] = 1'b1;
+        end else if (!sign && (exp == '0) && (frac == '0)) begin
+            class_s[4] = 1'b1;
+        end else if (!sign && (exp == '0) && (frac != '0)) begin
+            class_s[5] = 1'b1;
+        end else if (!sign && (exp != '0) && !(&exp)) begin
+            class_s[6] = 1'b1;
+        end else if (!sign && (&exp) && (frac == '0)) begin
+            class_s[7] = 1'b1;
+        end
+    endfunction
+
+    function automatic logic [9:0] class_d(input logic [63:0] value);
+        logic sign;
+        logic [10:0] exp;
+        logic [51:0] frac;
+        sign = value[63];
+        exp = value[62:52];
+        frac = value[51:0];
+        class_d = '0;
+        if (&exp && (frac != '0)) begin
+            class_d[is_snan_d(value) ? 8 : 9] = 1'b1;
+        end else if (sign && (&exp) && (frac == '0)) begin
+            class_d[0] = 1'b1;
+        end else if (sign && (exp != '0)) begin
+            class_d[1] = 1'b1;
+        end else if (sign && (exp == '0) && (frac != '0)) begin
+            class_d[2] = 1'b1;
+        end else if (sign && (exp == '0) && (frac == '0)) begin
+            class_d[3] = 1'b1;
+        end else if (!sign && (exp == '0) && (frac == '0)) begin
+            class_d[4] = 1'b1;
+        end else if (!sign && (exp == '0) && (frac != '0)) begin
+            class_d[5] = 1'b1;
+        end else if (!sign && (exp != '0) && !(&exp)) begin
+            class_d[6] = 1'b1;
+        end else if (!sign && (&exp) && (frac == '0)) begin
+            class_d[7] = 1'b1;
+        end
+    endfunction
+
+    function automatic logic less_s(input logic [31:0] lhs, input logic [31:0] rhs);
+        logic lhs_neg;
+        logic rhs_neg;
+        lhs_neg = lhs[31];
+        rhs_neg = rhs[31];
+        if ((lhs[30:0] == '0) && (rhs[30:0] == '0)) begin
+            return 1'b0;
+        end
+        if (lhs_neg != rhs_neg) begin
+            return lhs_neg;
+        end
+        return lhs_neg ? (lhs[30:0] > rhs[30:0]) : (lhs[30:0] < rhs[30:0]);
+    endfunction
+
+    function automatic logic less_d(input logic [63:0] lhs, input logic [63:0] rhs);
+        logic lhs_neg;
+        logic rhs_neg;
+        lhs_neg = lhs[63];
+        rhs_neg = rhs[63];
+        if ((lhs[62:0] == '0) && (rhs[62:0] == '0)) begin
+            return 1'b0;
+        end
+        if (lhs_neg != rhs_neg) begin
+            return lhs_neg;
+        end
+        return lhs_neg ? (lhs[62:0] > rhs[62:0]) : (lhs[62:0] < rhs[62:0]);
+    endfunction
+
+    function automatic logic equal_s(input logic [31:0] lhs, input logic [31:0] rhs);
+        if ((lhs[30:0] == '0) && (rhs[30:0] == '0)) begin
+            return 1'b1;
+        end
+        return lhs == rhs;
+    endfunction
+
+    function automatic logic equal_d(input logic [63:0] lhs, input logic [63:0] rhs);
+        if ((lhs[62:0] == '0) && (rhs[62:0] == '0)) begin
+            return 1'b1;
+        end
+        return lhs == rhs;
+    endfunction
+
+    function automatic logic [2:0] effective_rm(input logic [2:0] rm_value);
+        effective_rm = (rm_value <= 3'b100) ? rm_value : 3'b000;
+    endfunction
+
+    function automatic logic round_increment(
+        input logic [2:0]      round_mode,
+        input logic            sign,
+        input logic [127:0]    quotient,
+        input logic [127:0]    remainder,
+        input logic [127:0]    half
+    );
+        logic greater_than_half;
+        logic equal_to_half;
+
+        greater_than_half = remainder > half;
+        equal_to_half     = remainder == half;
+
+        unique case (effective_rm(round_mode))
+            3'b000: round_increment = greater_than_half || (equal_to_half && quotient[0]); // RNE
+            3'b001: round_increment = 1'b0;                                                // RTZ
+            3'b010: round_increment = sign && (remainder != '0);                           // RDN
+            3'b011: round_increment = !sign && (remainder != '0);                          // RUP
+            3'b100: round_increment = greater_than_half || equal_to_half;                  // RMM
+            default: round_increment = 1'b0;
+        endcase
+    endfunction
+
+    function automatic logic [63:0] invalid_integer_result(
+        input logic is_signed,
+        input logic is_long,
+        input logic sign,
+        input logic is_nan
+    );
+        if (is_signed) begin
+            if (sign && !is_nan) begin
+                invalid_integer_result = is_long ? 64'h8000_0000_0000_0000 : 64'hFFFF_FFFF_8000_0000;
+            end else begin
+                invalid_integer_result = is_long ? 64'h7FFF_FFFF_FFFF_FFFF : 64'h0000_0000_7FFF_FFFF;
+            end
+        end else begin
+            if (sign && !is_nan) begin
+                invalid_integer_result = '0;
+            end else begin
+                invalid_integer_result = 64'hFFFF_FFFF_FFFF_FFFF;
+            end
+        end
+    endfunction
+
+    function automatic logic [63:0] cvt_s_to_int(
+        input logic [31:0] value,
+        input logic [1:0]  int_fmt,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [7:0] exp;
+        logic [22:0] frac;
+        logic [23:0] significand;
+        logic signed [12:0] unbiased_exp;
+        logic is_signed;
+        logic is_long;
+        logic invalid;
+        logic inexact;
+        logic [127:0] magnitude;
+        logic [127:0] quotient;
+        logic [127:0] remainder;
+        logic [127:0] divisor;
+        logic [127:0] half;
+        logic [127:0] rounded_magnitude;
+        logic [63:0] positive_limit;
+        logic [63:0] negative_limit;
+        logic [63:0] unsigned_limit;
+        logic [63:0] integer_value;
+        int shift;
+
+        sign          = value[31];
+        exp           = value[30:23];
+        frac          = value[22:0];
+        significand   = (exp == '0) ? {1'b0, frac} : {1'b1, frac};
+        unbiased_exp  = $signed({5'b0, exp}) - 13'sd127;
+        is_signed     = (int_fmt == 2'd0) || (int_fmt == 2'd2);
+        is_long       = (int_fmt == 2'd2) || (int_fmt == 2'd3);
+        invalid       = 1'b0;
+        inexact       = 1'b0;
+        magnitude     = '0;
+        quotient      = '0;
+        remainder     = '0;
+        divisor       = '0;
+        half          = '0;
+        rounded_magnitude = '0;
+        positive_limit = is_long ? 64'h7FFF_FFFF_FFFF_FFFF : 64'h0000_0000_7FFF_FFFF;
+        negative_limit = is_long ? 64'h8000_0000_0000_0000 : 64'h0000_0000_8000_0000;
+        unsigned_limit = is_long ? 64'hFFFF_FFFF_FFFF_FFFF : 64'h0000_0000_FFFF_FFFF;
+        integer_value  = '0;
+        flags          = '0;
+
+        if (is_nan_s(value) || (&exp)) begin
+            invalid = 1'b1;
+        end else if (significand == '0) begin
+            rounded_magnitude = '0;
+        end else begin
+            if (unbiased_exp >= 13'sd23) begin
+                shift = int'(unbiased_exp) - 23;
+                if (shift >= 104) begin
+                    invalid = 1'b1;
+                end else begin
+                    rounded_magnitude = {104'b0, significand} << shift;
+                end
+            end else begin
+                shift = 23 - int'(unbiased_exp);
+                if (shift > 24) begin
+                    quotient  = '0;
+                    remainder = {104'b0, significand};
+                    divisor   = '0;
+                    half      = '0;
+                    inexact   = remainder != '0;
+                    unique case (effective_rm(round_mode))
+                        3'b010:  rounded_magnitude = (sign && inexact) ? 128'd1 : 128'd0;
+                        3'b011:  rounded_magnitude = (!sign && inexact) ? 128'd1 : 128'd0;
+                        default: rounded_magnitude = '0;
+                    endcase
+                end else begin
+                    divisor   = 128'd1 << shift;
+                    quotient  = {104'b0, significand} >> shift;
+                    remainder = {104'b0, significand} & (divisor - 128'd1);
+                    half      = divisor >> 1;
+                    inexact   = (remainder != '0);
+                    rounded_magnitude = quotient +
+                                        (round_increment(round_mode, sign, quotient, remainder, half) ? 128'd1 : 128'd0);
+                end
+            end
+        end
+
+        if (!invalid) begin
+            if (is_signed) begin
+                if ((!sign && (rounded_magnitude > {64'b0, positive_limit})) ||
+                    (sign && (rounded_magnitude > {64'b0, negative_limit}))) begin
+                    invalid = 1'b1;
+                end
+            end else begin
+                if ((sign && (rounded_magnitude != '0)) ||
+                    (rounded_magnitude > {64'b0, unsigned_limit})) begin
+                    invalid = 1'b1;
+                end
+            end
+        end
+
+        if (invalid) begin
+            flags[4] = 1'b1;
+            return invalid_integer_result(is_signed, is_long, sign, is_nan_s(value));
+        end
+
+        if (inexact) begin
+            flags[0] = 1'b1;
+        end
+
+        integer_value = rounded_magnitude[63:0];
+        if (sign) begin
+            integer_value = (~integer_value) + 64'd1;
+        end
+
+        if (!is_long) begin
+            return {{32{integer_value[31]}}, integer_value[31:0]};
+        end
+
+        return integer_value;
+    endfunction
+
+    function automatic logic [63:0] cvt_d_to_int(
+        input logic [63:0] value,
+        input logic [1:0]  int_fmt,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [10:0] exp;
+        logic [51:0] frac;
+        logic [52:0] significand;
+        logic signed [12:0] unbiased_exp;
+        logic is_signed;
+        logic is_long;
+        logic invalid;
+        logic inexact;
+        logic [127:0] quotient;
+        logic [127:0] remainder;
+        logic [127:0] divisor;
+        logic [127:0] half;
+        logic [127:0] rounded_magnitude;
+        logic [63:0] positive_limit;
+        logic [63:0] negative_limit;
+        logic [63:0] unsigned_limit;
+        logic [63:0] integer_value;
+        int shift;
+
+        sign          = value[63];
+        exp           = value[62:52];
+        frac          = value[51:0];
+        significand   = (exp == '0) ? {1'b0, frac} : {1'b1, frac};
+        unbiased_exp  = $signed({2'b0, exp}) - 13'sd1023;
+        is_signed     = (int_fmt == 2'd0) || (int_fmt == 2'd2);
+        is_long       = (int_fmt == 2'd2) || (int_fmt == 2'd3);
+        invalid       = 1'b0;
+        inexact       = 1'b0;
+        quotient      = '0;
+        remainder     = '0;
+        divisor       = '0;
+        half          = '0;
+        rounded_magnitude = '0;
+        positive_limit = is_long ? 64'h7FFF_FFFF_FFFF_FFFF : 64'h0000_0000_7FFF_FFFF;
+        negative_limit = is_long ? 64'h8000_0000_0000_0000 : 64'h0000_0000_8000_0000;
+        unsigned_limit = is_long ? 64'hFFFF_FFFF_FFFF_FFFF : 64'h0000_0000_FFFF_FFFF;
+        integer_value  = '0;
+        flags          = '0;
+
+        if (is_nan_d(value) || (&exp)) begin
+            invalid = 1'b1;
+        end else if (significand == '0) begin
+            rounded_magnitude = '0;
+        end else begin
+            if (unbiased_exp >= 13'sd52) begin
+                shift = int'(unbiased_exp) - 52;
+                if (shift >= 75) begin
+                    invalid = 1'b1;
+                end else begin
+                    rounded_magnitude = {75'b0, significand} << shift;
+                end
+            end else begin
+                shift = 52 - int'(unbiased_exp);
+                if (shift > 53) begin
+                    quotient  = '0;
+                    remainder = {75'b0, significand};
+                    inexact   = remainder != '0;
+                    unique case (effective_rm(round_mode))
+                        3'b010:  rounded_magnitude = (sign && inexact) ? 128'd1 : 128'd0;
+                        3'b011:  rounded_magnitude = (!sign && inexact) ? 128'd1 : 128'd0;
+                        default: rounded_magnitude = '0;
+                    endcase
+                end else begin
+                    divisor   = 128'd1 << shift;
+                    quotient  = {75'b0, significand} >> shift;
+                    remainder = {75'b0, significand} & (divisor - 128'd1);
+                    half      = divisor >> 1;
+                    inexact   = (remainder != '0);
+                    rounded_magnitude = quotient +
+                                        (round_increment(round_mode, sign, quotient, remainder, half) ? 128'd1 : 128'd0);
+                end
+            end
+        end
+
+        if (!invalid) begin
+            if (is_signed) begin
+                if ((!sign && (rounded_magnitude > {64'b0, positive_limit})) ||
+                    (sign && (rounded_magnitude > {64'b0, negative_limit}))) begin
+                    invalid = 1'b1;
+                end
+            end else begin
+                if ((sign && (rounded_magnitude != '0)) ||
+                    (rounded_magnitude > {64'b0, unsigned_limit})) begin
+                    invalid = 1'b1;
+                end
+            end
+        end
+
+        if (invalid) begin
+            flags[4] = 1'b1;
+            return invalid_integer_result(is_signed, is_long, sign, is_nan_d(value));
+        end
+
+        if (inexact) begin
+            flags[0] = 1'b1;
+        end
+
+        integer_value = rounded_magnitude[63:0];
+        if (sign) begin
+            integer_value = (~integer_value) + 64'd1;
+        end
+
+        if (!is_long) begin
+            return {{32{integer_value[31]}}, integer_value[31:0]};
+        end
+
+        return integer_value;
+    endfunction
+
+    function automatic int highest_bit_64(input logic [63:0] value);
+        highest_bit_64 = -1;
+        for (int bit_idx = 0; bit_idx < 64; bit_idx++) begin
+            if (value[bit_idx]) begin
+                highest_bit_64 = bit_idx;
+            end
+        end
+    endfunction
+
+    function automatic int highest_bit_53(input logic [52:0] value);
+        highest_bit_53 = -1;
+        for (int bit_idx = 0; bit_idx < 53; bit_idx++) begin
+            if (value[bit_idx]) begin
+                highest_bit_53 = bit_idx;
+            end
+        end
+    endfunction
+
+    function automatic logic [31:0] overflow_s_result(input logic sign, input logic [2:0] round_mode);
+        unique case (effective_rm(round_mode))
+            3'b001:  overflow_s_result = {sign, 8'hFE, 23'h7F_FFFF};
+            3'b010:  overflow_s_result = sign ? 32'hFF80_0000 : 32'h7F7F_FFFF;
+            3'b011:  overflow_s_result = sign ? 32'hFF7F_FFFF : 32'h7F80_0000;
+            default: overflow_s_result = {sign, 8'hFF, 23'b0};
+        endcase
+    endfunction
+
+    function automatic logic [63:0] overflow_d_result(input logic sign, input logic [2:0] round_mode);
+        unique case (effective_rm(round_mode))
+            3'b001:  overflow_d_result = {sign, 11'h7FE, 52'hF_FFFF_FFFF_FFFF};
+            3'b010:  overflow_d_result = sign ? 64'hFFF0_0000_0000_0000 : 64'h7FEF_FFFF_FFFF_FFFF;
+            3'b011:  overflow_d_result = sign ? 64'hFFEF_FFFF_FFFF_FFFF : 64'h7FF0_0000_0000_0000;
+            default: overflow_d_result = {sign, 11'h7FF, 52'b0};
+        endcase
+    endfunction
+
+    function automatic logic [27:0] shift_right_jam_28(
+        input logic [27:0] value,
+        input int          shift
+    );
+        logic sticky;
+
+        if (shift <= 0) begin
+            return value;
+        end
+        if (shift >= 28) begin
+            return (value != '0) ? 28'd1 : 28'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 28; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        return (value >> shift) | (sticky ? 28'd1 : 28'd0);
+    endfunction
+
+    function automatic logic shifted_out_28(
+        input logic [27:0] value,
+        input int          shift
+    );
+        logic sticky;
+
+        if (shift <= 0) begin
+            return 1'b0;
+        end
+        if (shift >= 28) begin
+            return value != '0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 28; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        return sticky;
+    endfunction
+
+    function automatic logic [27:0] shift_right_jam_48(
+        input logic [47:0] value,
+        input int          shift
+    );
+        logic sticky;
+        logic [27:0] shifted;
+
+        if (shift <= 0) begin
+            return value[27:0];
+        end
+        if (shift >= 48) begin
+            return (value != '0) ? 28'd1 : 28'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 48; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        shifted = 28'((value >> shift));
+        return shifted | (sticky ? 28'd1 : 28'd0);
+    endfunction
+
+    function automatic logic [56:0] shift_right_jam_57(
+        input logic [56:0] value,
+        input int          shift
+    );
+        logic sticky;
+
+        if (shift <= 0) begin
+            return value;
+        end
+        if (shift >= 57) begin
+            return (value != '0) ? 57'd1 : 57'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 57; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        return (value >> shift) | (sticky ? 57'd1 : 57'd0);
+    endfunction
+
+    function automatic logic [64:0] shift_right_jam_65(
+        input logic [64:0] value,
+        input int          shift
+    );
+        logic sticky;
+
+        if (shift <= 0) begin
+            return value;
+        end
+        if (shift >= 65) begin
+            return (value != '0) ? 65'd1 : 65'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 65; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        return (value >> shift) | (sticky ? 65'd1 : 65'd0);
+    endfunction
+
+    function automatic logic [127:0] shift_right_jam_128(
+        input logic [127:0] value,
+        input int           shift
+    );
+        logic sticky;
+
+        if (shift <= 0) begin
+            return value;
+        end
+        if (shift >= 128) begin
+            return (value != '0) ? 128'd1 : 128'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 128; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        return (value >> shift) | (sticky ? 128'd1 : 128'd0);
+    endfunction
+
+    function automatic logic [56:0] shift_right_jam_128_to_57(
+        input logic [127:0] value,
+        input int           shift
+    );
+        logic sticky;
+        logic [56:0] shifted;
+
+        if (shift <= 0) begin
+            return value[56:0];
+        end
+        if (shift >= 128) begin
+            return (value != '0) ? 57'd1 : 57'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 128; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        shifted = 57'(value >> shift);
+        return shifted | (sticky ? 57'd1 : 57'd0);
+    endfunction
+
+    function automatic logic [31:0] round_pack_s(
+        input logic                   sign,
+        input logic signed [10:0]     unbiased_exp,
+        input logic [27:0]            significand,
+        input logic                   inexact_before_round,
+        input logic [2:0]             round_mode,
+        output logic [4:0]            flags
+    );
+        logic signed [10:0] result_unbiased_exp;
+        logic [27:0] result_sig;
+        logic round_any;
+        logic round_up;
+        logic [24:0] rounded_sig;
+        logic [7:0] result_exp;
+        logic [22:0] result_frac;
+        logic tiny_before_round;
+        int subnormal_shift;
+
+        flags = '0;
+        result_unbiased_exp = unbiased_exp;
+        result_sig = significand;
+        result_sig[0] = result_sig[0] | inexact_before_round;
+        tiny_before_round = 1'b0;
+
+        if (result_unbiased_exp < -11'sd126) begin
+            tiny_before_round = 1'b1;
+            subnormal_shift = -126 - int'(result_unbiased_exp);
+            result_sig = shift_right_jam_28(result_sig, subnormal_shift);
+            result_unbiased_exp = -11'sd126;
+        end
+
+        round_any = |result_sig[2:0];
+        unique case (effective_rm(round_mode))
+            3'b000:  round_up = result_sig[2] && (result_sig[1] || result_sig[0] || result_sig[3]);
+            3'b001:  round_up = 1'b0;
+            3'b010:  round_up = sign && round_any;
+            3'b011:  round_up = !sign && round_any;
+            3'b100:  round_up = result_sig[2];
+            default: round_up = 1'b0;
+        endcase
+
+        rounded_sig = {1'b0, result_sig[26:3]} + (round_up ? 25'd1 : 25'd0);
+        if (round_any) begin
+            flags[0] = 1'b1;
+        end
+        if (tiny_before_round && flags[0]) begin
+            flags[1] = 1'b1;
+        end
+
+        if (rounded_sig[24]) begin
+            rounded_sig = rounded_sig >> 1;
+            result_unbiased_exp = result_unbiased_exp + 11'sd1;
+        end
+
+        if (result_unbiased_exp > 11'sd127) begin
+            flags[2] = 1'b1;
+            flags[0] = 1'b1;
+            return overflow_s_result(sign, round_mode);
+        end
+
+        if ((result_unbiased_exp == -11'sd126) && !rounded_sig[23]) begin
+            result_exp = 8'b0;
+            result_frac = rounded_sig[22:0];
+            if (flags[0]) begin
+                flags[1] = 1'b1;
+            end
+        end else begin
+            result_exp = 8'(result_unbiased_exp + 11'sd127);
+            result_frac = rounded_sig[22:0];
+        end
+
+        return {sign, result_exp, result_frac};
+    endfunction
+
+    function automatic logic [63:0] round_pack_d(
+        input logic                   sign,
+        input logic signed [12:0]     unbiased_exp,
+        input logic [56:0]            significand,
+        input logic                   inexact_before_round,
+        input logic [2:0]             round_mode,
+        output logic [4:0]            flags
+    );
+        logic signed [12:0] result_unbiased_exp;
+        logic [56:0] result_sig;
+        logic round_any;
+        logic round_up;
+        logic [53:0] rounded_sig;
+        logic [10:0] result_exp;
+        logic [51:0] result_frac;
+        logic tiny_before_round;
+        int subnormal_shift;
+
+        flags = '0;
+        result_unbiased_exp = unbiased_exp;
+        result_sig = significand;
+        result_sig[0] = result_sig[0] | inexact_before_round;
+        tiny_before_round = 1'b0;
+
+        if (result_unbiased_exp < -13'sd1022) begin
+            tiny_before_round = 1'b1;
+            subnormal_shift = -1022 - int'(result_unbiased_exp);
+            result_sig = shift_right_jam_57(result_sig, subnormal_shift);
+            result_unbiased_exp = -13'sd1022;
+        end
+
+        round_any = |result_sig[2:0];
+        unique case (effective_rm(round_mode))
+            3'b000:  round_up = result_sig[2] && (result_sig[1] || result_sig[0] || result_sig[3]);
+            3'b001:  round_up = 1'b0;
+            3'b010:  round_up = sign && round_any;
+            3'b011:  round_up = !sign && round_any;
+            3'b100:  round_up = result_sig[2];
+            default: round_up = 1'b0;
+        endcase
+
+        rounded_sig = {1'b0, result_sig[55:3]} + (round_up ? 54'd1 : 54'd0);
+        if (round_any) begin
+            flags[0] = 1'b1;
+        end
+        if (tiny_before_round && flags[0]) begin
+            flags[1] = 1'b1;
+        end
+
+        if (rounded_sig[53]) begin
+            rounded_sig = rounded_sig >> 1;
+            result_unbiased_exp = result_unbiased_exp + 13'sd1;
+        end
+
+        if (result_unbiased_exp > 13'sd1023) begin
+            flags[2] = 1'b1;
+            flags[0] = 1'b1;
+            return overflow_d_result(sign, round_mode);
+        end
+
+        if ((result_unbiased_exp == -13'sd1022) && !rounded_sig[52]) begin
+            result_exp = 11'b0;
+            result_frac = rounded_sig[51:0];
+            if (flags[0]) begin
+                flags[1] = 1'b1;
+            end
+        end else begin
+            result_exp = 11'(result_unbiased_exp + 13'sd1023);
+            result_frac = rounded_sig[51:0];
+        end
+
+        return {sign, result_exp, result_frac};
+    endfunction
+
+    function automatic logic [31:0] fp_addsub_s(
+        input logic [31:0] lhs,
+        input logic [31:0] rhs,
+        input logic        subtract,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic lhs_sign;
+        logic rhs_sign;
+        logic result_sign;
+        logic [7:0] lhs_exp;
+        logic [7:0] rhs_exp;
+        logic [22:0] lhs_frac;
+        logic [22:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [10:0] lhs_unbiased_exp;
+        logic signed [10:0] rhs_unbiased_exp;
+        logic signed [10:0] result_unbiased_exp;
+        logic [27:0] lhs_sig;
+        logic [27:0] rhs_sig;
+        logic [27:0] result_sig;
+        logic [27:0] larger_sig;
+        logic [27:0] smaller_sig;
+        logic signed [10:0] larger_exp;
+        logic signs_match;
+        logic alignment_inexact;
+        int exp_diff;
+
+        flags = '0;
+        lhs_sign = lhs[31];
+        rhs_sign = rhs[31] ^ subtract;
+        lhs_exp = lhs[30:23];
+        rhs_exp = rhs[30:23];
+        lhs_frac = lhs[22:0];
+        rhs_frac = rhs[22:0];
+        lhs_is_zero = (lhs[30:0] == '0);
+        rhs_is_zero = (rhs[30:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+        signs_match = lhs_sign == rhs_sign;
+        alignment_inexact = 1'b0;
+
+        if (is_nan_s(lhs) || is_nan_s(rhs)) begin
+            flags[4] = is_snan_s(lhs) || is_snan_s(rhs);
+            return CANONICAL_NAN_S;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            if (lhs_is_inf && rhs_is_inf && !signs_match) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_S;
+            end
+            return lhs_is_inf ? {lhs_sign, 8'hFF, 23'b0} : {rhs_sign, 8'hFF, 23'b0};
+        end
+
+        if (lhs_is_zero && rhs_is_zero) begin
+            result_sign = signs_match ? lhs_sign : (effective_rm(round_mode) == 3'b010);
+            return {result_sign, 31'b0};
+        end
+        if (lhs_is_zero) begin
+            return {rhs_sign, rhs[30:0]};
+        end
+        if (rhs_is_zero) begin
+            return lhs;
+        end
+
+        lhs_unbiased_exp = (lhs_exp == '0) ? -11'sd126 : ($signed({3'b0, lhs_exp}) - 11'sd127);
+        rhs_unbiased_exp = (rhs_exp == '0) ? -11'sd126 : ($signed({3'b0, rhs_exp}) - 11'sd127);
+        lhs_sig = {1'b0, (lhs_exp == '0) ? 1'b0 : 1'b1, lhs_frac, 3'b000};
+        rhs_sig = {1'b0, (rhs_exp == '0) ? 1'b0 : 1'b1, rhs_frac, 3'b000};
+
+        if ((lhs_unbiased_exp > rhs_unbiased_exp) ||
+            ((lhs_unbiased_exp == rhs_unbiased_exp) && (lhs_sig >= rhs_sig))) begin
+            exp_diff = int'(lhs_unbiased_exp) - int'(rhs_unbiased_exp);
+            larger_sig = lhs_sig;
+            alignment_inexact = shifted_out_28(rhs_sig, exp_diff);
+            smaller_sig = shift_right_jam_28(rhs_sig, exp_diff);
+            larger_exp = lhs_unbiased_exp;
+            result_sign = lhs_sign;
+        end else begin
+            exp_diff = int'(rhs_unbiased_exp) - int'(lhs_unbiased_exp);
+            larger_sig = rhs_sig;
+            alignment_inexact = shifted_out_28(lhs_sig, exp_diff);
+            smaller_sig = shift_right_jam_28(lhs_sig, exp_diff);
+            larger_exp = rhs_unbiased_exp;
+            result_sign = rhs_sign;
+        end
+
+        result_unbiased_exp = larger_exp;
+        if (signs_match) begin
+            result_sig = larger_sig + smaller_sig;
+            if (result_sig[27]) begin
+                result_sig = shift_right_jam_28(result_sig, 1);
+                result_unbiased_exp = result_unbiased_exp + 11'sd1;
+            end
+        end else begin
+            result_sig = larger_sig - smaller_sig;
+            if (result_sig == '0) begin
+                result_sign = effective_rm(round_mode) == 3'b010;
+                return {result_sign, 31'b0};
+            end
+            while (!result_sig[26] && (result_unbiased_exp > -11'sd126)) begin
+                result_sig = result_sig << 1;
+                result_unbiased_exp = result_unbiased_exp - 11'sd1;
+            end
+        end
+
+        return round_pack_s(result_sign, result_unbiased_exp, result_sig, alignment_inexact, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] fp_addsub_d(
+        input logic [63:0] lhs,
+        input logic [63:0] rhs,
+        input logic        subtract,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic lhs_sign;
+        logic rhs_sign;
+        logic result_sign;
+        logic [10:0] lhs_exp;
+        logic [10:0] rhs_exp;
+        logic [51:0] lhs_frac;
+        logic [51:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [12:0] lhs_unbiased_exp;
+        logic signed [12:0] rhs_unbiased_exp;
+        logic signed [12:0] result_unbiased_exp;
+        logic [56:0] lhs_sig;
+        logic [56:0] rhs_sig;
+        logic [56:0] result_sig;
+        logic [56:0] larger_sig;
+        logic [56:0] smaller_sig;
+        logic signed [12:0] larger_exp;
+        logic signs_match;
+        int exp_diff;
+
+        flags = '0;
+        lhs_sign = lhs[63];
+        rhs_sign = rhs[63] ^ subtract;
+        lhs_exp = lhs[62:52];
+        rhs_exp = rhs[62:52];
+        lhs_frac = lhs[51:0];
+        rhs_frac = rhs[51:0];
+        lhs_is_zero = (lhs[62:0] == '0);
+        rhs_is_zero = (rhs[62:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+        signs_match = lhs_sign == rhs_sign;
+
+        if (is_nan_d(lhs) || is_nan_d(rhs)) begin
+            flags[4] = is_snan_d(lhs) || is_snan_d(rhs);
+            return CANONICAL_NAN_D;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            if (lhs_is_inf && rhs_is_inf && !signs_match) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_D;
+            end
+            return lhs_is_inf ? {lhs_sign, 11'h7FF, 52'b0} : {rhs_sign, 11'h7FF, 52'b0};
+        end
+
+        if (lhs_is_zero && rhs_is_zero) begin
+            result_sign = signs_match ? lhs_sign : (effective_rm(round_mode) == 3'b010);
+            return {result_sign, 63'b0};
+        end
+        if (lhs_is_zero) begin
+            return {rhs_sign, rhs[62:0]};
+        end
+        if (rhs_is_zero) begin
+            return lhs;
+        end
+
+        lhs_unbiased_exp = (lhs_exp == '0) ? -13'sd1022 : ($signed({2'b0, lhs_exp}) - 13'sd1023);
+        rhs_unbiased_exp = (rhs_exp == '0) ? -13'sd1022 : ($signed({2'b0, rhs_exp}) - 13'sd1023);
+        lhs_sig = {1'b0, (lhs_exp == '0) ? 1'b0 : 1'b1, lhs_frac, 3'b000};
+        rhs_sig = {1'b0, (rhs_exp == '0) ? 1'b0 : 1'b1, rhs_frac, 3'b000};
+
+        if ((lhs_unbiased_exp > rhs_unbiased_exp) ||
+            ((lhs_unbiased_exp == rhs_unbiased_exp) && (lhs_sig >= rhs_sig))) begin
+            exp_diff = int'(lhs_unbiased_exp) - int'(rhs_unbiased_exp);
+            larger_sig = lhs_sig;
+            smaller_sig = shift_right_jam_57(rhs_sig, exp_diff);
+            larger_exp = lhs_unbiased_exp;
+            result_sign = lhs_sign;
+        end else begin
+            exp_diff = int'(rhs_unbiased_exp) - int'(lhs_unbiased_exp);
+            larger_sig = rhs_sig;
+            smaller_sig = shift_right_jam_57(lhs_sig, exp_diff);
+            larger_exp = rhs_unbiased_exp;
+            result_sign = rhs_sign;
+        end
+
+        result_unbiased_exp = larger_exp;
+        if (signs_match) begin
+            result_sig = larger_sig + smaller_sig;
+            if (result_sig[56]) begin
+                result_sig = shift_right_jam_57(result_sig, 1);
+                result_unbiased_exp = result_unbiased_exp + 13'sd1;
+            end
+        end else begin
+            result_sig = larger_sig - smaller_sig;
+            if (result_sig == '0) begin
+                result_sign = effective_rm(round_mode) == 3'b010;
+                return {result_sign, 63'b0};
+            end
+            while (!result_sig[55] && (result_unbiased_exp > -13'sd1022)) begin
+                result_sig = result_sig << 1;
+                result_unbiased_exp = result_unbiased_exp - 13'sd1;
+            end
+        end
+
+        return round_pack_d(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] fp_mul_d(
+        input logic [63:0] lhs,
+        input logic [63:0] rhs,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic result_sign;
+        logic [10:0] lhs_exp;
+        logic [10:0] rhs_exp;
+        logic [51:0] lhs_frac;
+        logic [51:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [12:0] lhs_unbiased_exp;
+        logic signed [12:0] rhs_unbiased_exp;
+        logic signed [12:0] result_unbiased_exp;
+        logic [52:0] lhs_sig;
+        logic [52:0] rhs_sig;
+        logic [105:0] product;
+        logic [56:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+
+        flags = '0;
+        result_sign = lhs[63] ^ rhs[63];
+        lhs_exp = lhs[62:52];
+        rhs_exp = rhs[62:52];
+        lhs_frac = lhs[51:0];
+        rhs_frac = rhs[51:0];
+        lhs_is_zero = (lhs[62:0] == '0);
+        rhs_is_zero = (rhs[62:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+
+        if (is_nan_d(lhs) || is_nan_d(rhs)) begin
+            flags[4] = is_snan_d(lhs) || is_snan_d(rhs);
+            return CANONICAL_NAN_D;
+        end
+
+        if ((lhs_is_inf && rhs_is_zero) || (rhs_is_inf && lhs_is_zero)) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_D;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            return {result_sign, 11'h7FF, 52'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_zero) begin
+            return {result_sign, 63'b0};
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_53({1'b0, lhs_frac});
+            lhs_unbiased_exp = 13'(lhs_msb - 1074);
+            lhs_sig = {1'b0, lhs_frac} << (52 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({2'b0, lhs_exp}) - 13'sd1023;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_53({1'b0, rhs_frac});
+            rhs_unbiased_exp = 13'(rhs_msb - 1074);
+            rhs_sig = {1'b0, rhs_frac} << (52 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({2'b0, rhs_exp}) - 13'sd1023;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        product = lhs_sig * rhs_sig;
+        if (product[105]) begin
+            result_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp + 13'sd1;
+            result_sig = shift_right_jam_128_to_57({22'b0, product}, 50);
+        end else begin
+            result_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp;
+            result_sig = shift_right_jam_128_to_57({22'b0, product}, 49);
+        end
+
+        return round_pack_d(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [31:0] fp_mul_s(
+        input logic [31:0] lhs,
+        input logic [31:0] rhs,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic result_sign;
+        logic [7:0] lhs_exp;
+        logic [7:0] rhs_exp;
+        logic [22:0] lhs_frac;
+        logic [22:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [10:0] lhs_unbiased_exp;
+        logic signed [10:0] rhs_unbiased_exp;
+        logic signed [10:0] result_unbiased_exp;
+        logic [23:0] lhs_sig;
+        logic [23:0] rhs_sig;
+        logic [47:0] product;
+        logic [27:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+
+        flags = '0;
+        result_sign = lhs[31] ^ rhs[31];
+        lhs_exp = lhs[30:23];
+        rhs_exp = rhs[30:23];
+        lhs_frac = lhs[22:0];
+        rhs_frac = rhs[22:0];
+        lhs_is_zero = (lhs[30:0] == '0);
+        rhs_is_zero = (rhs[30:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+
+        if (is_nan_s(lhs) || is_nan_s(rhs)) begin
+            flags[4] = is_snan_s(lhs) || is_snan_s(rhs);
+            return CANONICAL_NAN_S;
+        end
+
+        if ((lhs_is_inf && rhs_is_zero) || (rhs_is_inf && lhs_is_zero)) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_S;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            return {result_sign, 8'hFF, 23'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_zero) begin
+            return {result_sign, 31'b0};
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_64({41'b0, lhs_frac});
+            lhs_unbiased_exp = 11'(lhs_msb - 149);
+            lhs_sig = {1'b0, lhs_frac} << (23 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({3'b0, lhs_exp}) - 11'sd127;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_64({41'b0, rhs_frac});
+            rhs_unbiased_exp = 11'(rhs_msb - 149);
+            rhs_sig = {1'b0, rhs_frac} << (23 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({3'b0, rhs_exp}) - 11'sd127;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        product = lhs_sig * rhs_sig;
+        if (product[47]) begin
+            result_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp + 11'sd1;
+            result_sig = shift_right_jam_48(product, 21);
+        end else begin
+            result_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp;
+            result_sig = shift_right_jam_48(product, 20);
+        end
+
+        return round_pack_s(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [31:0] fp_div_s(
+        input logic [31:0] lhs,
+        input logic [31:0] rhs,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic result_sign;
+        logic [7:0] lhs_exp;
+        logic [7:0] rhs_exp;
+        logic [22:0] lhs_frac;
+        logic [22:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [10:0] lhs_unbiased_exp;
+        logic signed [10:0] rhs_unbiased_exp;
+        logic signed [10:0] result_unbiased_exp;
+        logic [23:0] lhs_sig;
+        logic [23:0] rhs_sig;
+        logic [63:0] dividend;
+        logic [63:0] quotient;
+        logic [23:0] remainder;
+        logic [27:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+
+        flags = '0;
+        result_sign = lhs[31] ^ rhs[31];
+        lhs_exp = lhs[30:23];
+        rhs_exp = rhs[30:23];
+        lhs_frac = lhs[22:0];
+        rhs_frac = rhs[22:0];
+        lhs_is_zero = (lhs[30:0] == '0);
+        rhs_is_zero = (rhs[30:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+
+        if (is_nan_s(lhs) || is_nan_s(rhs)) begin
+            flags[4] = is_snan_s(lhs) || is_snan_s(rhs);
+            return CANONICAL_NAN_S;
+        end
+
+        if ((lhs_is_zero && rhs_is_zero) || (lhs_is_inf && rhs_is_inf)) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_S;
+        end
+
+        if (lhs_is_inf || rhs_is_zero) begin
+            if (rhs_is_zero && !lhs_is_zero && !lhs_is_inf) begin
+                flags[3] = 1'b1;
+            end
+            return {result_sign, 8'hFF, 23'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_inf) begin
+            return {result_sign, 31'b0};
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_64({41'b0, lhs_frac});
+            lhs_unbiased_exp = 11'(lhs_msb - 149);
+            lhs_sig = {1'b0, lhs_frac} << (23 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({3'b0, lhs_exp}) - 11'sd127;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_64({41'b0, rhs_frac});
+            rhs_unbiased_exp = 11'(rhs_msb - 149);
+            rhs_sig = {1'b0, rhs_frac} << (23 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({3'b0, rhs_exp}) - 11'sd127;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        if (lhs_sig >= rhs_sig) begin
+            result_unbiased_exp = lhs_unbiased_exp - rhs_unbiased_exp;
+            dividend = {40'b0, lhs_sig} << 26;
+        end else begin
+            result_unbiased_exp = lhs_unbiased_exp - rhs_unbiased_exp - 11'sd1;
+            dividend = {40'b0, lhs_sig} << 27;
+        end
+
+        quotient = dividend / {40'b0, rhs_sig};
+        remainder = 24'(dividend % {40'b0, rhs_sig});
+        result_sig = 28'(quotient);
+        if (remainder != '0) begin
+            result_sig[0] = 1'b1;
+        end
+
+        return round_pack_s(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] fp_div_d(
+        input logic [63:0] lhs,
+        input logic [63:0] rhs,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic result_sign;
+        logic [10:0] lhs_exp;
+        logic [10:0] rhs_exp;
+        logic [51:0] lhs_frac;
+        logic [51:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [12:0] lhs_unbiased_exp;
+        logic signed [12:0] rhs_unbiased_exp;
+        logic signed [12:0] result_unbiased_exp;
+        logic [52:0] lhs_sig;
+        logic [52:0] rhs_sig;
+        logic [127:0] dividend;
+        logic [127:0] quotient;
+        logic [52:0] remainder;
+        logic [56:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+
+        flags = '0;
+        result_sign = lhs[63] ^ rhs[63];
+        lhs_exp = lhs[62:52];
+        rhs_exp = rhs[62:52];
+        lhs_frac = lhs[51:0];
+        rhs_frac = rhs[51:0];
+        lhs_is_zero = (lhs[62:0] == '0);
+        rhs_is_zero = (rhs[62:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+
+        if (is_nan_d(lhs) || is_nan_d(rhs)) begin
+            flags[4] = is_snan_d(lhs) || is_snan_d(rhs);
+            return CANONICAL_NAN_D;
+        end
+
+        if ((lhs_is_zero && rhs_is_zero) || (lhs_is_inf && rhs_is_inf)) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_D;
+        end
+
+        if (lhs_is_inf || rhs_is_zero) begin
+            if (rhs_is_zero && !lhs_is_zero && !lhs_is_inf) begin
+                flags[3] = 1'b1;
+            end
+            return {result_sign, 11'h7FF, 52'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_inf) begin
+            return {result_sign, 63'b0};
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_53({1'b0, lhs_frac});
+            lhs_unbiased_exp = 13'(lhs_msb - 1074);
+            lhs_sig = {1'b0, lhs_frac} << (52 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({2'b0, lhs_exp}) - 13'sd1023;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_53({1'b0, rhs_frac});
+            rhs_unbiased_exp = 13'(rhs_msb - 1074);
+            rhs_sig = {1'b0, rhs_frac} << (52 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({2'b0, rhs_exp}) - 13'sd1023;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        if (lhs_sig >= rhs_sig) begin
+            result_unbiased_exp = lhs_unbiased_exp - rhs_unbiased_exp;
+            dividend = {75'b0, lhs_sig} << 55;
+        end else begin
+            result_unbiased_exp = lhs_unbiased_exp - rhs_unbiased_exp - 13'sd1;
+            dividend = {75'b0, lhs_sig} << 56;
+        end
+
+        quotient = dividend / {75'b0, rhs_sig};
+        remainder = 53'(dividend % {75'b0, rhs_sig});
+        result_sig = 57'(quotient);
+        if (remainder != '0) begin
+            result_sig[0] = 1'b1;
+        end
+
+        return round_pack_d(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [31:0] fp_sqrt_s(
+        input logic [31:0] value,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [7:0] exp;
+        logic [22:0] frac;
+        logic signed [10:0] unbiased_exp;
+        logic signed [10:0] result_unbiased_exp;
+        logic [23:0] sig;
+        logic [63:0] radicand;
+        logic [63:0] trial;
+        logic [63:0] root;
+        logic [63:0] remainder;
+        logic [27:0] result_sig;
+        int sig_msb;
+
+        flags = '0;
+        sign = value[31];
+        exp = value[30:23];
+        frac = value[22:0];
+
+        if (is_nan_s(value)) begin
+            flags[4] = is_snan_s(value);
+            return CANONICAL_NAN_S;
+        end
+
+        if ((&exp) && (frac == '0)) begin
+            if (sign) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_S;
+            end
+            return value;
+        end
+
+        if (value[30:0] == '0) begin
+            return value;
+        end
+
+        if (sign) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_S;
+        end
+
+        if (exp == '0) begin
+            sig_msb = highest_bit_64({41'b0, frac});
+            unbiased_exp = 11'(sig_msb - 149);
+            sig = {1'b0, frac} << (23 - sig_msb);
+        end else begin
+            unbiased_exp = $signed({3'b0, exp}) - 11'sd127;
+            sig = {1'b1, frac};
+        end
+
+        if (unbiased_exp[0]) begin
+            radicand = {39'b0, sig, 1'b0} << 29;
+            result_unbiased_exp = (unbiased_exp - 11'sd1) >>> 1;
+        end else begin
+            radicand = {40'b0, sig} << 29;
+            result_unbiased_exp = unbiased_exp >>> 1;
+        end
+
+        root = '0;
+        for (int bit_idx = 26; bit_idx >= 0; bit_idx--) begin
+            trial = root | (64'd1 << bit_idx);
+            if ((trial * trial) <= radicand) begin
+                root = trial;
+            end
+        end
+
+        remainder = radicand - (root * root);
+        result_sig = 28'(root);
+        if (remainder != '0) begin
+            result_sig[0] = 1'b1;
+        end
+
+        return round_pack_s(1'b0, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] fp_sqrt_d(
+        input logic [63:0] value,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [10:0] exp;
+        logic [51:0] frac;
+        logic signed [12:0] unbiased_exp;
+        logic signed [12:0] result_unbiased_exp;
+        logic [52:0] sig;
+        logic [127:0] radicand;
+        logic [127:0] trial;
+        logic [127:0] root;
+        logic [127:0] remainder;
+        logic [56:0] result_sig;
+        int sig_msb;
+
+        flags = '0;
+        sign = value[63];
+        exp = value[62:52];
+        frac = value[51:0];
+
+        if (is_nan_d(value)) begin
+            flags[4] = is_snan_d(value);
+            return CANONICAL_NAN_D;
+        end
+
+        if ((&exp) && (frac == '0)) begin
+            if (sign) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_D;
+            end
+            return value;
+        end
+
+        if (value[62:0] == '0) begin
+            return value;
+        end
+
+        if (sign) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_D;
+        end
+
+        if (exp == '0) begin
+            sig_msb = highest_bit_53({1'b0, frac});
+            unbiased_exp = 13'(sig_msb - 1074);
+            sig = {1'b0, frac} << (52 - sig_msb);
+        end else begin
+            unbiased_exp = $signed({2'b0, exp}) - 13'sd1023;
+            sig = {1'b1, frac};
+        end
+
+        if (unbiased_exp[0]) begin
+            radicand = {74'b0, sig, 1'b0} << 58;
+            result_unbiased_exp = (unbiased_exp - 13'sd1) >>> 1;
+        end else begin
+            radicand = {75'b0, sig} << 58;
+            result_unbiased_exp = unbiased_exp >>> 1;
+        end
+
+        root = '0;
+        for (int bit_idx = 55; bit_idx >= 0; bit_idx--) begin
+            trial = root | (128'd1 << bit_idx);
+            if ((trial * trial) <= radicand) begin
+                root = trial;
+            end
+        end
+
+        remainder = radicand - (root * root);
+        result_sig = 57'(root);
+        if (remainder != '0) begin
+            result_sig[0] = 1'b1;
+        end
+
+        return round_pack_d(1'b0, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [31:0] fp_fma_s(
+        input logic [31:0] mul_lhs,
+        input logic [31:0] mul_rhs,
+        input logic [31:0] addend,
+        input logic        negate_product,
+        input logic        negate_addend,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic lhs_sign;
+        logic rhs_sign;
+        logic addend_sign;
+        logic product_sign;
+        logic result_sign;
+        logic [7:0] lhs_exp;
+        logic [7:0] rhs_exp;
+        logic [7:0] addend_exp;
+        logic [22:0] lhs_frac;
+        logic [22:0] rhs_frac;
+        logic [22:0] addend_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic addend_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic addend_is_inf;
+        logic invalid_multiply;
+        logic signed [10:0] lhs_unbiased_exp;
+        logic signed [10:0] rhs_unbiased_exp;
+        logic signed [10:0] addend_unbiased_exp;
+        logic signed [10:0] product_unbiased_exp;
+        logic signed [10:0] common_unbiased_exp;
+        logic signed [10:0] result_unbiased_exp;
+        logic [23:0] lhs_sig;
+        logic [23:0] rhs_sig;
+        logic [23:0] addend_sig;
+        logic [47:0] product;
+        logic [47:0] normalized_product;
+        logic [64:0] product_ext;
+        logic [64:0] addend_ext;
+        logic [64:0] aligned_product;
+        logic [64:0] aligned_addend;
+        logic [64:0] result_ext;
+        logic [64:0] rounded_source;
+        logic [27:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+        int addend_msb;
+        int exp_diff;
+
+        flags = '0;
+        lhs_sign = mul_lhs[31];
+        rhs_sign = mul_rhs[31];
+        addend_sign = addend[31] ^ negate_addend;
+        product_sign = lhs_sign ^ rhs_sign ^ negate_product;
+        lhs_exp = mul_lhs[30:23];
+        rhs_exp = mul_rhs[30:23];
+        addend_exp = addend[30:23];
+        lhs_frac = mul_lhs[22:0];
+        rhs_frac = mul_rhs[22:0];
+        addend_frac = addend[22:0];
+        lhs_is_zero = (mul_lhs[30:0] == '0);
+        rhs_is_zero = (mul_rhs[30:0] == '0);
+        addend_is_zero = (addend[30:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+        addend_is_inf = (&addend_exp) && (addend_frac == '0);
+        invalid_multiply = (lhs_is_inf && rhs_is_zero) || (rhs_is_inf && lhs_is_zero);
+
+        if (is_nan_s(mul_lhs) || is_nan_s(mul_rhs) || is_nan_s(addend) || invalid_multiply) begin
+            flags[4] = invalid_multiply || is_snan_s(mul_lhs) || is_snan_s(mul_rhs) || is_snan_s(addend);
+            return CANONICAL_NAN_S;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            if (addend_is_inf && (product_sign != addend_sign)) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_S;
+            end
+            return {product_sign, 8'hFF, 23'b0};
+        end
+
+        if (addend_is_inf) begin
+            return {addend_sign, 8'hFF, 23'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_zero) begin
+            if (addend_is_zero) begin
+                result_sign = (product_sign == addend_sign) ? product_sign : (effective_rm(round_mode) == 3'b010);
+                return {result_sign, 31'b0};
+            end
+            return {addend_sign, addend[30:0]};
+        end
+
+        if (addend_is_zero) begin
+            return fp_mul_s({mul_lhs[31] ^ negate_product, mul_lhs[30:0]}, mul_rhs, round_mode, flags);
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_64({41'b0, lhs_frac});
+            lhs_unbiased_exp = 11'(lhs_msb - 149);
+            lhs_sig = {1'b0, lhs_frac} << (23 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({3'b0, lhs_exp}) - 11'sd127;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_64({41'b0, rhs_frac});
+            rhs_unbiased_exp = 11'(rhs_msb - 149);
+            rhs_sig = {1'b0, rhs_frac} << (23 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({3'b0, rhs_exp}) - 11'sd127;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        if (addend_exp == '0) begin
+            addend_msb = highest_bit_64({41'b0, addend_frac});
+            addend_unbiased_exp = 11'(addend_msb - 149);
+            addend_sig = {1'b0, addend_frac} << (23 - addend_msb);
+        end else begin
+            addend_unbiased_exp = $signed({3'b0, addend_exp}) - 11'sd127;
+            addend_sig = {1'b1, addend_frac};
+        end
+
+        product = lhs_sig * rhs_sig;
+        if (product[47]) begin
+            product_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp + 11'sd1;
+            normalized_product = product;
+        end else begin
+            product_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp;
+            normalized_product = product << 1;
+        end
+
+        product_ext = {4'b0, normalized_product, 13'b0};
+        addend_ext = {4'b0, addend_sig, 37'b0};
+
+        if (product_unbiased_exp >= addend_unbiased_exp) begin
+            common_unbiased_exp = product_unbiased_exp;
+            exp_diff = int'(product_unbiased_exp) - int'(addend_unbiased_exp);
+            aligned_product = product_ext;
+            aligned_addend = shift_right_jam_65(addend_ext, exp_diff);
+        end else begin
+            common_unbiased_exp = addend_unbiased_exp;
+            exp_diff = int'(addend_unbiased_exp) - int'(product_unbiased_exp);
+            aligned_product = shift_right_jam_65(product_ext, exp_diff);
+            aligned_addend = addend_ext;
+        end
+
+        result_unbiased_exp = common_unbiased_exp;
+        if (product_sign == addend_sign) begin
+            result_ext = aligned_product + aligned_addend;
+            result_sign = product_sign;
+            if (result_ext[61]) begin
+                result_ext = shift_right_jam_65(result_ext, 1);
+                result_unbiased_exp = result_unbiased_exp + 11'sd1;
+            end
+        end else if (aligned_product >= aligned_addend) begin
+            result_ext = aligned_product - aligned_addend;
+            result_sign = product_sign;
+        end else begin
+            result_ext = aligned_addend - aligned_product;
+            result_sign = addend_sign;
+        end
+
+        if (result_ext == '0) begin
+            result_sign = effective_rm(round_mode) == 3'b010;
+            return {result_sign, 31'b0};
+        end
+
+        while (!result_ext[60] && (result_unbiased_exp > -11'sd126)) begin
+            result_ext = result_ext << 1;
+            result_unbiased_exp = result_unbiased_exp - 11'sd1;
+        end
+
+        rounded_source = shift_right_jam_65(result_ext, 34);
+        result_sig = rounded_source[27:0];
+        return round_pack_s(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] fp_fma_d(
+        input logic [63:0] mul_lhs,
+        input logic [63:0] mul_rhs,
+        input logic [63:0] addend,
+        input logic        negate_product,
+        input logic        negate_addend,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic lhs_sign;
+        logic rhs_sign;
+        logic addend_sign;
+        logic product_sign;
+        logic result_sign;
+        logic [10:0] lhs_exp;
+        logic [10:0] rhs_exp;
+        logic [10:0] addend_exp;
+        logic [51:0] lhs_frac;
+        logic [51:0] rhs_frac;
+        logic [51:0] addend_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic addend_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic addend_is_inf;
+        logic invalid_multiply;
+        logic signed [12:0] lhs_unbiased_exp;
+        logic signed [12:0] rhs_unbiased_exp;
+        logic signed [12:0] addend_unbiased_exp;
+        logic signed [12:0] product_unbiased_exp;
+        logic signed [12:0] common_unbiased_exp;
+        logic signed [12:0] result_unbiased_exp;
+        logic [52:0] lhs_sig;
+        logic [52:0] rhs_sig;
+        logic [52:0] addend_sig;
+        logic [105:0] product;
+        logic [105:0] normalized_product;
+        logic [127:0] product_ext;
+        logic [127:0] addend_ext;
+        logic [127:0] aligned_product;
+        logic [127:0] aligned_addend;
+        logic [127:0] result_ext;
+        logic [127:0] rounded_source;
+        logic [56:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+        int addend_msb;
+        int exp_diff;
+
+        flags = '0;
+        lhs_sign = mul_lhs[63];
+        rhs_sign = mul_rhs[63];
+        addend_sign = addend[63] ^ negate_addend;
+        product_sign = lhs_sign ^ rhs_sign ^ negate_product;
+        lhs_exp = mul_lhs[62:52];
+        rhs_exp = mul_rhs[62:52];
+        addend_exp = addend[62:52];
+        lhs_frac = mul_lhs[51:0];
+        rhs_frac = mul_rhs[51:0];
+        addend_frac = addend[51:0];
+        lhs_is_zero = (mul_lhs[62:0] == '0);
+        rhs_is_zero = (mul_rhs[62:0] == '0);
+        addend_is_zero = (addend[62:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+        addend_is_inf = (&addend_exp) && (addend_frac == '0);
+        invalid_multiply = (lhs_is_inf && rhs_is_zero) || (rhs_is_inf && lhs_is_zero);
+
+        if (is_nan_d(mul_lhs) || is_nan_d(mul_rhs) || is_nan_d(addend) || invalid_multiply) begin
+            flags[4] = invalid_multiply || is_snan_d(mul_lhs) || is_snan_d(mul_rhs) || is_snan_d(addend);
+            return CANONICAL_NAN_D;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            if (addend_is_inf && (product_sign != addend_sign)) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_D;
+            end
+            return {product_sign, 11'h7FF, 52'b0};
+        end
+
+        if (addend_is_inf) begin
+            return {addend_sign, 11'h7FF, 52'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_zero) begin
+            if (addend_is_zero) begin
+                result_sign = (product_sign == addend_sign) ? product_sign : (effective_rm(round_mode) == 3'b010);
+                return {result_sign, 63'b0};
+            end
+            return {addend_sign, addend[62:0]};
+        end
+
+        if (addend_is_zero) begin
+            return fp_mul_d({mul_lhs[63] ^ negate_product, mul_lhs[62:0]}, mul_rhs, round_mode, flags);
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_53({1'b0, lhs_frac});
+            lhs_unbiased_exp = 13'(lhs_msb - 1074);
+            lhs_sig = {1'b0, lhs_frac} << (52 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({2'b0, lhs_exp}) - 13'sd1023;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_53({1'b0, rhs_frac});
+            rhs_unbiased_exp = 13'(rhs_msb - 1074);
+            rhs_sig = {1'b0, rhs_frac} << (52 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({2'b0, rhs_exp}) - 13'sd1023;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        if (addend_exp == '0) begin
+            addend_msb = highest_bit_53({1'b0, addend_frac});
+            addend_unbiased_exp = 13'(addend_msb - 1074);
+            addend_sig = {1'b0, addend_frac} << (52 - addend_msb);
+        end else begin
+            addend_unbiased_exp = $signed({2'b0, addend_exp}) - 13'sd1023;
+            addend_sig = {1'b1, addend_frac};
+        end
+
+        product = lhs_sig * rhs_sig;
+        if (product[105]) begin
+            product_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp + 13'sd1;
+            normalized_product = product;
+        end else begin
+            product_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp;
+            normalized_product = product << 1;
+        end
+
+        product_ext = {9'b0, normalized_product, 13'b0};
+        addend_ext = {9'b0, addend_sig, 66'b0};
+
+        if (product_unbiased_exp >= addend_unbiased_exp) begin
+            common_unbiased_exp = product_unbiased_exp;
+            exp_diff = int'(product_unbiased_exp) - int'(addend_unbiased_exp);
+            aligned_product = product_ext;
+            aligned_addend = shift_right_jam_128(addend_ext, exp_diff);
+        end else begin
+            common_unbiased_exp = addend_unbiased_exp;
+            exp_diff = int'(addend_unbiased_exp) - int'(product_unbiased_exp);
+            aligned_product = shift_right_jam_128(product_ext, exp_diff);
+            aligned_addend = addend_ext;
+        end
+
+        result_unbiased_exp = common_unbiased_exp;
+        if (product_sign == addend_sign) begin
+            result_ext = aligned_product + aligned_addend;
+            result_sign = product_sign;
+            if (result_ext[119]) begin
+                result_ext = shift_right_jam_128(result_ext, 1);
+                result_unbiased_exp = result_unbiased_exp + 13'sd1;
+            end
+        end else if (aligned_product >= aligned_addend) begin
+            result_ext = aligned_product - aligned_addend;
+            result_sign = product_sign;
+        end else begin
+            result_ext = aligned_addend - aligned_product;
+            result_sign = addend_sign;
+        end
+
+        if (result_ext == '0) begin
+            result_sign = effective_rm(round_mode) == 3'b010;
+            return {result_sign, 63'b0};
+        end
+
+        while (!result_ext[118] && (result_unbiased_exp > -13'sd1022)) begin
+            result_ext = result_ext << 1;
+            result_unbiased_exp = result_unbiased_exp - 13'sd1;
+        end
+
+        rounded_source = shift_right_jam_128(result_ext, 63);
+        result_sig = rounded_source[56:0];
+        return round_pack_d(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] pack_uint_to_s(
+        input logic        sign,
+        input logic [63:0] magnitude,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        int msb_index;
+        int shift;
+        logic [127:0] quotient;
+        logic [127:0] remainder;
+        logic [127:0] divisor;
+        logic [127:0] half;
+        logic [24:0] rounded_sig;
+        logic [7:0] exp_field;
+        logic [31:0] result_s;
+
+        flags = '0;
+        if (magnitude == '0) begin
+            return {32'hFFFF_FFFF, sign, 31'b0};
+        end
+
+        msb_index = highest_bit_64(magnitude);
+        exp_field = 8'(msb_index + 127);
+        if (msb_index <= 23) begin
+            rounded_sig = {1'b0, magnitude[23:0] << (23 - msb_index)};
+        end else begin
+            shift     = msb_index - 23;
+            divisor   = 128'd1 << shift;
+            quotient  = {64'b0, magnitude} >> shift;
+            remainder = {64'b0, magnitude} & (divisor - 128'd1);
+            half      = divisor >> 1;
+            rounded_sig = {1'b0, quotient[23:0]} +
+                          (round_increment(round_mode, sign, quotient, remainder, half) ? 25'd1 : 25'd0);
+            if (remainder != '0) begin
+                flags[0] = 1'b1;
+            end
+            if (rounded_sig[24]) begin
+                rounded_sig = rounded_sig >> 1;
+                exp_field = exp_field + 8'd1;
+            end
+        end
+
+        result_s = {sign, exp_field, rounded_sig[22:0]};
+        return {32'hFFFF_FFFF, result_s};
+    endfunction
+
+    function automatic logic [63:0] pack_uint_to_d(
+        input logic        sign,
+        input logic [63:0] magnitude,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        int msb_index;
+        int shift;
+        logic [127:0] quotient;
+        logic [127:0] remainder;
+        logic [127:0] divisor;
+        logic [127:0] half;
+        logic [53:0] rounded_sig;
+        logic [10:0] exp_field;
+
+        flags = '0;
+        if (magnitude == '0) begin
+            return {sign, 63'b0};
+        end
+
+        msb_index = highest_bit_64(magnitude);
+        exp_field = 11'(msb_index + 1023);
+        if (msb_index <= 52) begin
+            rounded_sig = {1'b0, magnitude[52:0] << (52 - msb_index)};
+        end else begin
+            shift     = msb_index - 52;
+            divisor   = 128'd1 << shift;
+            quotient  = {64'b0, magnitude} >> shift;
+            remainder = {64'b0, magnitude} & (divisor - 128'd1);
+            half      = divisor >> 1;
+            rounded_sig = {1'b0, quotient[52:0]} +
+                          (round_increment(round_mode, sign, quotient, remainder, half) ? 54'd1 : 54'd0);
+            if (remainder != '0) begin
+                flags[0] = 1'b1;
+            end
+            if (rounded_sig[53]) begin
+                rounded_sig = rounded_sig >> 1;
+                exp_field = exp_field + 11'd1;
+            end
+        end
+
+        return {sign, exp_field, rounded_sig[51:0]};
+    endfunction
+
+    function automatic logic [63:0] cvt_int_to_fp(
+        input logic [63:0] int_value,
+        input logic [1:0]  int_fmt,
+        input logic        to_double,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic is_signed;
+        logic is_long;
+        logic sign;
+        logic [63:0] source_value;
+        logic [63:0] magnitude;
+
+        is_signed    = (int_fmt == 2'd0) || (int_fmt == 2'd2);
+        is_long      = (int_fmt == 2'd2) || (int_fmt == 2'd3);
+        source_value = is_long ? int_value :
+                       (is_signed ? {{32{int_value[31]}}, int_value[31:0]} : {32'b0, int_value[31:0]});
+        sign         = is_signed && source_value[63];
+        magnitude    = sign ? ((~source_value) + 64'd1) : source_value;
+
+        if (to_double) begin
+            return pack_uint_to_d(sign, magnitude, round_mode, flags);
+        end
+
+        return pack_uint_to_s(sign, magnitude, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] cvt_s_to_d(
+        input logic [31:0] value,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [7:0] exp;
+        logic [22:0] frac;
+        int frac_msb;
+        logic [10:0] exp_d;
+        logic [51:0] frac_d;
+        logic [22:0] subnormal_tail_s;
+
+        flags = '0;
+        sign  = value[31];
+        exp   = value[30:23];
+        frac  = value[22:0];
+
+        if (is_nan_s(value)) begin
+            flags[4] = is_snan_s(value);
+            return CANONICAL_NAN_D;
+        end
+        if (&exp) begin
+            return {sign, 11'h7FF, 52'b0};
+        end
+        if ((exp == '0) && (frac == '0)) begin
+            return {sign, 63'b0};
+        end
+        if (exp == '0) begin
+            frac_msb = -1;
+            for (int bit_idx = 0; bit_idx < 23; bit_idx++) begin
+                if (frac[bit_idx]) begin
+                    frac_msb = bit_idx;
+                end
+            end
+            exp_d = 11'(frac_msb - 149 + 1023);
+            subnormal_tail_s = frac ^ (23'(1) << frac_msb);
+            frac_d = {29'b0, subnormal_tail_s} << (52 - frac_msb);
+            return {sign, exp_d, frac_d};
+        end
+
+        exp_d = 11'(int'(exp) - 127 + 1023);
+        frac_d = {frac, 29'b0};
+        return {sign, exp_d, frac_d};
+    endfunction
+
+    function automatic logic [63:0] cvt_d_to_s(
+        input logic [63:0] value,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [10:0] exp;
+        logic [51:0] frac;
+        logic [52:0] significand;
+        logic signed [13:0] unbiased_exp;
+        int frac_msb;
+        int shift;
+        logic [127:0] quotient;
+        logic [127:0] remainder;
+        logic [127:0] divisor;
+        logic [127:0] half;
+        logic [24:0] rounded_sig;
+        logic [7:0] exp_s;
+        logic [31:0] result_s;
+
+        flags = '0;
+        sign  = value[63];
+        exp   = value[62:52];
+        frac  = value[51:0];
+
+        if (is_nan_d(value)) begin
+            flags[4] = is_snan_d(value);
+            return {32'hFFFF_FFFF, CANONICAL_NAN_S};
+        end
+        if (&exp) begin
+            return {32'hFFFF_FFFF, sign, 8'hFF, 23'b0};
+        end
+        if ((exp == '0) && (frac == '0)) begin
+            return {32'hFFFF_FFFF, sign, 31'b0};
+        end
+
+        if (exp == '0) begin
+            frac_msb = highest_bit_53({1'b0, frac});
+            unbiased_exp = 14'(frac_msb - 1074);
+            significand = {1'b0, frac} << (52 - frac_msb);
+        end else begin
+            unbiased_exp = $signed({3'b0, exp}) - 14'sd1023;
+            significand = {1'b1, frac};
+        end
+
+        if (unbiased_exp > 14'sd127) begin
+            flags[2] = 1'b1;
+            flags[0] = 1'b1;
+            return {32'hFFFF_FFFF, overflow_s_result(sign, round_mode)};
+        end
+
+        if (unbiased_exp >= -14'sd126) begin
+            exp_s = 8'(unbiased_exp + 14'sd127);
+            shift = 29;
+            divisor = 128'd1 << shift;
+            quotient = {75'b0, significand} >> shift;
+            remainder = {75'b0, significand} & (divisor - 128'd1);
+            half = divisor >> 1;
+            rounded_sig = {1'b0, quotient[23:0]} +
+                          (round_increment(round_mode, sign, quotient, remainder, half) ? 25'd1 : 25'd0);
+            if (remainder != '0) begin
+                flags[0] = 1'b1;
+            end
+            if (rounded_sig[24]) begin
+                rounded_sig = rounded_sig >> 1;
+                exp_s = exp_s + 8'd1;
+                if (exp_s == 8'hFF) begin
+                    flags[2] = 1'b1;
+                    flags[0] = 1'b1;
+                    return {32'hFFFF_FFFF, overflow_s_result(sign, round_mode)};
+                end
+            end
+            result_s = {sign, exp_s, rounded_sig[22:0]};
+            return {32'hFFFF_FFFF, result_s};
+        end
+
+        shift = 29 + ((-126) - int'(unbiased_exp));
+        if (shift >= 128) begin
+            quotient = '0;
+            remainder = {75'b0, significand};
+            rounded_sig = '0;
+            if (remainder != '0) begin
+                flags[1] = 1'b1;
+                flags[0] = 1'b1;
+                unique case (effective_rm(round_mode))
+                    3'b010:  rounded_sig = sign ? 25'd1 : 25'd0;
+                    3'b011:  rounded_sig = !sign ? 25'd1 : 25'd0;
+                    default: rounded_sig = '0;
+                endcase
+            end
+        end else begin
+            divisor = 128'd1 << shift;
+            quotient = {75'b0, significand} >> shift;
+            remainder = {75'b0, significand} & (divisor - 128'd1);
+            half = divisor >> 1;
+            rounded_sig = {1'b0, quotient[23:0]} +
+                          (round_increment(round_mode, sign, quotient, remainder, half) ? 25'd1 : 25'd0);
+            if (remainder != '0) begin
+                flags[1] = 1'b1;
+                flags[0] = 1'b1;
+            end
+        end
+
+        if (rounded_sig[23]) begin
+            result_s = {sign, 8'd1, 23'b0};
+        end else begin
+            result_s = {sign, 8'b0, rounded_sig[22:0]};
+        end
+        return {32'hFFFF_FFFF, result_s};
+    endfunction
+
+    always_comb begin
+        logic invalid_s;
+        logic invalid_d;
+        logic cmp_value;
+        logic [31:0] result_s;
+        logic [63:0] result_d;
+        logic [63:0] result_int;
+        logic [4:0] result_flags;
+
+        fp_result     = '0;
+        int_result    = '0;
+        fflags        = '0;
+        illegal_instr = 1'b0;
+        invalid_s     = is_nan_s(s1) || is_nan_s(s2);
+        invalid_d     = is_nan_d(d1) || is_nan_d(d2);
+        cmp_value     = 1'b0;
+        result_s      = '0;
+        result_d      = '0;
+        result_int    = '0;
+        result_flags  = '0;
+
+        unique case (fp_op)
+            FP_OP_NONE: begin
+                fp_result  = '0;
+                int_result = '0;
+                fflags     = '0;
+            end
+
+            FP_OP_ADD, FP_OP_SUB: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    result_s = fp_addsub_s(s1, s2, fp_op == FP_OP_SUB, rm, result_flags);
+                    fp_result = {32'hFFFF_FFFF, result_s};
+                    fflags = result_flags;
+                end else begin
+                    result_d = fp_addsub_d(d1, d2, fp_op == FP_OP_SUB, rm, result_flags);
+                    fp_result = result_d;
+                    fflags = result_flags;
+                end
+            end
+
+            FP_OP_MUL: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    result_s = fp_mul_s(s1, s2, rm, result_flags);
+                    fp_result = {32'hFFFF_FFFF, result_s};
+                    fflags = result_flags;
+                end else begin
+                    result_d = fp_mul_d(d1, d2, rm, result_flags);
+                    fp_result = result_d;
+                    fflags = result_flags;
+                end
+            end
+
+            FP_OP_DIV: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    result_s = fp_div_s(s1, s2, rm, result_flags);
+                    fp_result = {32'hFFFF_FFFF, result_s};
+                    fflags = result_flags;
+                end else begin
+                    result_d = fp_div_d(d1, d2, rm, result_flags);
+                    fp_result = result_d;
+                    fflags = result_flags;
+                end
+            end
+
+            FP_OP_SQRT: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    result_s = fp_sqrt_s(s1, rm, result_flags);
+                    fp_result = {32'hFFFF_FFFF, result_s};
+                    fflags = result_flags;
+                end else begin
+                    result_d = fp_sqrt_d(d1, rm, result_flags);
+                    fp_result = result_d;
+                    fflags = result_flags;
+                end
+            end
+
+            FP_OP_MADD, FP_OP_MSUB, FP_OP_NMSUB, FP_OP_NMADD: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    unique case (fp_op)
+                        FP_OP_MADD:  result_s = fp_fma_s(s1, s2, s3, 1'b0, 1'b0, rm, result_flags);
+                        FP_OP_MSUB:  result_s = fp_fma_s(s1, s2, s3, 1'b0, 1'b1, rm, result_flags);
+                        FP_OP_NMSUB: result_s = fp_fma_s(s1, s2, s3, 1'b1, 1'b0, rm, result_flags);
+                        default:     result_s = fp_fma_s(s1, s2, s3, 1'b1, 1'b1, rm, result_flags);
+                    endcase
+                    fp_result = {32'hFFFF_FFFF, result_s};
+                    fflags = result_flags;
+                end else begin
+                    unique case (fp_op)
+                        FP_OP_MADD:  result_d = fp_fma_d(d1, d2, d3, 1'b0, 1'b0, rm, result_flags);
+                        FP_OP_MSUB:  result_d = fp_fma_d(d1, d2, d3, 1'b0, 1'b1, rm, result_flags);
+                        FP_OP_NMSUB: result_d = fp_fma_d(d1, d2, d3, 1'b1, 1'b0, rm, result_flags);
+                        default:     result_d = fp_fma_d(d1, d2, d3, 1'b1, 1'b1, rm, result_flags);
+                    endcase
+                    fp_result = result_d;
+                    fflags = result_flags;
+                end
+            end
+
+            FP_OP_SGNJ, FP_OP_SGNJN, FP_OP_SGNJX: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    unique case (fp_op)
+                        FP_OP_SGNJ:  fp_result = {32'hFFFF_FFFF, s2[31],  s1[30:0]};
+                        FP_OP_SGNJN: fp_result = {32'hFFFF_FFFF, ~s2[31], s1[30:0]};
+                        default:     fp_result = {32'hFFFF_FFFF, s1[31] ^ s2[31], s1[30:0]};
+                    endcase
+                end else begin
+                    unique case (fp_op)
+                        FP_OP_SGNJ:  fp_result = {d2[63],  d1[62:0]};
+                        FP_OP_SGNJN: fp_result = {~d2[63], d1[62:0]};
+                        default:     fp_result = {d1[63] ^ d2[63], d1[62:0]};
+                    endcase
+                end
+            end
+
+            FP_OP_MV_X2F: begin
+                fp_result = (fp_fmt == FP_FMT_S) ? {32'hFFFF_FFFF, int_rs1_data[31:0]} : int_rs1_data;
+            end
+
+            FP_OP_MV_F2X: begin
+                int_result = (fp_fmt == FP_FMT_S) ? {{32{s1_raw[31]}}, s1_raw} : d1;
+            end
+
+            FP_OP_CLASS: begin
+                int_result = (fp_fmt == FP_FMT_S) ? {54'b0, class_s(s1)} : {54'b0, class_d(d1)};
+            end
+
+            FP_OP_EQ, FP_OP_LT, FP_OP_LE: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    if (invalid_s) begin
+                        fflags[4] = is_snan_s(s1) || is_snan_s(s2) || (fp_op != FP_OP_EQ);
+                        cmp_value = 1'b0;
+                    end else begin
+                        cmp_value = (fp_op == FP_OP_EQ) ? equal_s(s1, s2) :
+                                    (fp_op == FP_OP_LT) ? less_s(s1, s2) :
+                                                          (less_s(s1, s2) || equal_s(s1, s2));
+                    end
+                end else begin
+                    if (invalid_d) begin
+                        fflags[4] = is_snan_d(d1) || is_snan_d(d2) || (fp_op != FP_OP_EQ);
+                        cmp_value = 1'b0;
+                    end else begin
+                        cmp_value = (fp_op == FP_OP_EQ) ? equal_d(d1, d2) :
+                                    (fp_op == FP_OP_LT) ? less_d(d1, d2) :
+                                                          (less_d(d1, d2) || equal_d(d1, d2));
+                    end
+                end
+                int_result = {63'b0, cmp_value};
+            end
+
+            FP_OP_MIN, FP_OP_MAX: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    if (is_nan_s(s1) && is_nan_s(s2)) begin
+                        fp_result = {32'hFFFF_FFFF, CANONICAL_NAN_S};
+                        fflags[4] = is_snan_s(s1) || is_snan_s(s2);
+                    end else if (is_nan_s(s1)) begin
+                        fp_result = {32'hFFFF_FFFF, s2};
+                        fflags[4] = is_snan_s(s1);
+                    end else if (is_nan_s(s2)) begin
+                        fp_result = {32'hFFFF_FFFF, s1};
+                        fflags[4] = is_snan_s(s2);
+                    end else if ((s1[30:0] == '0) && (s2[30:0] == '0)) begin
+                        fp_result = {32'hFFFF_FFFF, (fp_op == FP_OP_MIN) ? {s1[31] | s2[31], 31'b0} :
+                                                                     {s1[31] & s2[31], 31'b0}};
+                    end else if (fp_op == FP_OP_MIN) begin
+                        fp_result = {32'hFFFF_FFFF, less_s(s1, s2) ? s1 : s2};
+                    end else begin
+                        fp_result = {32'hFFFF_FFFF, less_s(s1, s2) ? s2 : s1};
+                    end
+                end else begin
+                    if (is_nan_d(d1) && is_nan_d(d2)) begin
+                        fp_result = CANONICAL_NAN_D;
+                        fflags[4] = is_snan_d(d1) || is_snan_d(d2);
+                    end else if (is_nan_d(d1)) begin
+                        fp_result = d2;
+                        fflags[4] = is_snan_d(d1);
+                    end else if (is_nan_d(d2)) begin
+                        fp_result = d1;
+                        fflags[4] = is_snan_d(d2);
+                    end else if ((d1[62:0] == '0) && (d2[62:0] == '0)) begin
+                        fp_result = (fp_op == FP_OP_MIN) ? {d1[63] | d2[63], 63'b0} :
+                                                           {d1[63] & d2[63], 63'b0};
+                    end else if (fp_op == FP_OP_MIN) begin
+                        fp_result = less_d(d1, d2) ? d1 : d2;
+                    end else begin
+                        fp_result = less_d(d1, d2) ? d2 : d1;
+                    end
+                end
+            end
+
+            FP_OP_CVT_F2I: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    result_int = cvt_s_to_int(s1, rs2_addr[1:0], rm, result_flags);
+                end else begin
+                    result_int = cvt_d_to_int(d1, rs2_addr[1:0], rm, result_flags);
+                end
+                int_result = result_int;
+                fflags = result_flags;
+            end
+
+            FP_OP_CVT_I2F: begin
+                result_d = cvt_int_to_fp(int_rs1_data, rs2_addr[1:0], fp_fmt == FP_FMT_D, rm, result_flags);
+                fp_result = result_d;
+                fflags = result_flags;
+            end
+
+            FP_OP_CVT_F2F: begin
+                if (fp_fmt == FP_FMT_D) begin
+                    result_d = cvt_s_to_d(s1, result_flags);
+                    fp_result = result_d;
+                    if (is_nan_s(s1)) begin
+                        result_flags[4] = is_snan_s(s1);
+                    end
+                end else begin
+                    result_d = cvt_d_to_s(d1, rm, result_flags);
+                    fp_result = result_d;
+                    if (is_nan_d(d1)) begin
+                        result_flags[4] = is_snan_d(d1);
+                    end
+                end
+                fflags = result_flags;
+            end
+
+            default: begin
+                fp_result = (fp_fmt == FP_FMT_S) ? {32'hFFFF_FFFF, CANONICAL_NAN_S} : CANONICAL_NAN_D;
+                int_result = '0;
+                fflags[4] = 1'b1;
+            end
+        endcase
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/mmu/mmu.sv b/rv64cpu/rtl/core/mmu/mmu.sv
new file mode 100644
index 0000000..60c4afa
--- /dev/null
+++ b/rv64cpu/rtl/core/mmu/mmu.sv
@@ -0,0 +1,359 @@
+//     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/mmu/mmu_top.sv
+// Description: MMU top-level integrating iTLB + dTLB + shared PTW
+
+module mmu
+	import rv64_pkg::*;
+#(
+	parameter int ITLB_ENTRIES = 16,
+	parameter int DTLB_ENTRIES = 32,
+	parameter int ASID_WIDTH = 16
+)(
+	input  logic                  clk,
+	input  logic                  rst,
+
+	// CSR/context inputs
+	input  logic [XLEN-1:0]       satp_value,
+	input  logic [1:0]            priv_mode,
+	input  logic                  sum,
+	input  logic                  mxr,
+
+	// i-side translation request
+	input  logic                  i_req_valid,
+	output logic                  i_req_ready,
+	input  logic [XLEN-1:0]       i_req_vaddr,
+	output logic                  i_resp_valid,
+	output logic                  i_resp_hit,
+	output logic                  i_resp_page_fault,
+	output logic [XLEN-1:0]       i_resp_vaddr,
+	output logic [XLEN-1:0]       i_resp_paddr,
+
+	// d-side translation request
+	input  logic                  d_req_valid,
+	output logic                  d_req_ready,
+	input  logic [XLEN-1:0]       d_req_vaddr,
+	input  logic                  d_req_is_store,
+	output logic                  d_resp_valid,
+	output logic                  d_resp_hit,
+	output logic                  d_resp_page_fault,
+	output logic [XLEN-1:0]       d_resp_paddr,
+
+	// sfence / invalidation
+	input  logic                  flush_all,
+	input  logic                  flush_asid_valid,
+	input  logic [ASID_WIDTH-1:0] flush_asid,
+	input  logic                  flush_vpn_valid,
+	input  logic [44:0]           flush_vpn,
+
+	// PTW memory interface
+	output logic                  ptw_mem_req_valid,
+	input  logic                  ptw_mem_req_ready,
+	output logic [XLEN-1:0]       ptw_mem_req_addr,
+	input  logic                  ptw_mem_resp_valid,
+	input  logic [XLEN-1:0]       ptw_mem_resp_data
+);
+
+	localparam logic [3:0] SATP_MODE_BARE = 4'd0;
+
+	typedef enum logic [0:0] {
+		PTW_SRC_I = 1'b0,
+		PTW_SRC_D = 1'b1
+	} ptw_src_t;
+
+	logic [ASID_WIDTH-1:0] req_asid;
+	logic [3:0]            req_mode;
+
+	logic                  i_tlb_resp_valid;
+	logic                  i_tlb_resp_hit;
+	logic [XLEN-1:0]       i_tlb_resp_paddr;
+	logic                  i_tlb_resp_page_fault;
+	logic                  i_tlb_resp_need_ptw;
+
+	logic                  d_tlb_resp_valid;
+	logic                  d_tlb_resp_hit;
+	logic [XLEN-1:0]       d_tlb_resp_paddr;
+	logic                  d_tlb_resp_page_fault;
+	logic                  d_tlb_resp_need_ptw;
+
+	logic                  i_pending_q;
+	logic                  d_pending_q;
+	logic [XLEN-1:0]       i_pending_vaddr_q;
+	logic [XLEN-1:0]       d_pending_vaddr_q;
+	logic [1:0]            i_pending_priv_q;
+	logic [1:0]            d_pending_priv_q;
+	logic                  i_pending_sum_q;
+	logic                  d_pending_sum_q;
+	logic                  i_pending_mxr_q;
+	logic                  d_pending_mxr_q;
+	logic [ASID_WIDTH-1:0] i_pending_asid_q;
+	logic [ASID_WIDTH-1:0] d_pending_asid_q;
+	logic [3:0]            i_pending_mode_q;
+	logic [3:0]            d_pending_mode_q;
+	logic                  d_pending_is_store_q;
+
+	logic                  ptw_req_valid;
+	logic                  ptw_req_ready;
+	logic                  ptw_resp_valid;
+	logic                  ptw_resp_fault;
+	logic [XLEN-1:0]       ptw_resp_paddr;
+
+	logic                  ptw_refill_valid;
+	logic [44:0]           ptw_refill_vpn;
+	logic [43:0]           ptw_refill_ppn;
+	logic [2:0]            ptw_refill_level;
+	logic [3:0]            ptw_refill_mode;
+	logic [ASID_WIDTH-1:0] ptw_refill_asid;
+	logic                  ptw_refill_g;
+	logic                  ptw_refill_u;
+	logic                  ptw_refill_r;
+	logic                  ptw_refill_w;
+	logic                  ptw_refill_x;
+	logic                  ptw_refill_a;
+	logic                  ptw_refill_d;
+
+	logic                  ptw_busy_q;
+	ptw_src_t              ptw_owner_q;
+	ptw_src_t              ptw_owner_next;
+
+	logic                  i_accept;
+	logic                  d_accept;
+	logic                  i_fast_resp_valid;
+	logic                  d_fast_resp_valid;
+	logic                  i_ptw_resp_sel;
+	logic                  d_ptw_resp_sel;
+
+	logic                  i_refill_valid;
+	logic                  d_refill_valid;
+
+	assign req_mode = satp_value[63:60];
+	assign req_asid = satp_value[59 -: ASID_WIDTH];
+
+	assign i_req_ready = !i_pending_q;
+	assign d_req_ready = !d_pending_q;
+	assign i_accept = i_req_valid && i_req_ready;
+	assign d_accept = d_req_valid && d_req_ready;
+
+	assign i_fast_resp_valid = i_accept && (i_tlb_resp_hit || i_tlb_resp_page_fault);
+	assign d_fast_resp_valid = d_accept && (d_tlb_resp_hit || d_tlb_resp_page_fault);
+
+	assign i_ptw_resp_sel = ptw_resp_valid && (ptw_owner_q == PTW_SRC_I);
+	assign d_ptw_resp_sel = ptw_resp_valid && (ptw_owner_q == PTW_SRC_D);
+
+	assign i_resp_valid = i_fast_resp_valid || i_ptw_resp_sel;
+	assign i_resp_hit = i_fast_resp_valid ? i_tlb_resp_hit : (i_ptw_resp_sel && !ptw_resp_fault);
+	assign i_resp_page_fault = i_fast_resp_valid ? i_tlb_resp_page_fault : (i_ptw_resp_sel && ptw_resp_fault);
+	assign i_resp_vaddr = i_fast_resp_valid ? i_req_vaddr : i_pending_vaddr_q;
+	assign i_resp_paddr = i_fast_resp_valid ? i_tlb_resp_paddr : ptw_resp_paddr;
+
+	assign d_resp_valid = d_fast_resp_valid || d_ptw_resp_sel;
+	assign d_resp_hit = d_fast_resp_valid ? d_tlb_resp_hit : (d_ptw_resp_sel && !ptw_resp_fault);
+	assign d_resp_page_fault = d_fast_resp_valid ? d_tlb_resp_page_fault : (d_ptw_resp_sel && ptw_resp_fault);
+	assign d_resp_paddr = d_fast_resp_valid ? d_tlb_resp_paddr : ptw_resp_paddr;
+
+	always_comb begin
+		if (d_pending_q) begin
+			ptw_owner_next = PTW_SRC_D;
+		end else begin
+			ptw_owner_next = PTW_SRC_I;
+		end
+	end
+
+	assign ptw_req_valid = !ptw_busy_q && (d_pending_q || i_pending_q);
+
+	assign i_refill_valid = ptw_refill_valid && (ptw_owner_q == PTW_SRC_I);
+	assign d_refill_valid = ptw_refill_valid && (ptw_owner_q == PTW_SRC_D);
+
+	always_ff @(posedge clk) begin
+		if (rst) begin
+			i_pending_q <= 1'b0;
+			d_pending_q <= 1'b0;
+			ptw_busy_q <= 1'b0;
+			ptw_owner_q <= PTW_SRC_I;
+			i_pending_vaddr_q <= '0;
+			d_pending_vaddr_q <= '0;
+			i_pending_priv_q <= '0;
+			d_pending_priv_q <= '0;
+			i_pending_sum_q <= 1'b0;
+			d_pending_sum_q <= 1'b0;
+			i_pending_mxr_q <= 1'b0;
+			d_pending_mxr_q <= 1'b0;
+			i_pending_asid_q <= '0;
+			d_pending_asid_q <= '0;
+			i_pending_mode_q <= SATP_MODE_BARE;
+			d_pending_mode_q <= SATP_MODE_BARE;
+			d_pending_is_store_q <= 1'b0;
+		end else begin
+			if (i_accept && i_tlb_resp_need_ptw) begin
+				i_pending_q <= 1'b1;
+				i_pending_vaddr_q <= i_req_vaddr;
+				i_pending_priv_q <= priv_mode;
+				i_pending_sum_q <= sum;
+				i_pending_mxr_q <= mxr;
+				i_pending_asid_q <= req_asid;
+				i_pending_mode_q <= req_mode;
+			end
+
+			if (d_accept && d_tlb_resp_need_ptw) begin
+				d_pending_q <= 1'b1;
+				d_pending_vaddr_q <= d_req_vaddr;
+				d_pending_priv_q <= priv_mode;
+				d_pending_sum_q <= sum;
+				d_pending_mxr_q <= mxr;
+				d_pending_asid_q <= req_asid;
+				d_pending_mode_q <= req_mode;
+				d_pending_is_store_q <= d_req_is_store;
+			end
+
+			if (ptw_req_valid && ptw_req_ready) begin
+				ptw_busy_q <= 1'b1;
+				ptw_owner_q <= ptw_owner_next;
+			end
+
+			if (ptw_resp_valid) begin
+				ptw_busy_q <= 1'b0;
+				if (ptw_owner_q == PTW_SRC_I) begin
+					i_pending_q <= 1'b0;
+				end else begin
+					d_pending_q <= 1'b0;
+				end
+			end
+		end
+	end
+
+	tlb #(
+		.ENTRIES(ITLB_ENTRIES),
+		.ASID_WIDTH(ASID_WIDTH)
+	) u_itlb (
+		.clk              (clk),
+		.rst              (rst),
+		.req_valid        (i_accept),
+		.req_vaddr        (i_req_vaddr),
+		.req_asid         (req_asid),
+		.req_mode         (req_mode),
+		.req_priv         (priv_mode),
+		.req_is_fetch     (1'b1),
+		.req_is_store     (1'b0),
+		.req_sum          (sum),
+		.req_mxr          (mxr),
+		.resp_valid       (i_tlb_resp_valid),
+		.resp_hit         (i_tlb_resp_hit),
+		.resp_paddr       (i_tlb_resp_paddr),
+		.resp_page_fault  (i_tlb_resp_page_fault),
+		.resp_need_ptw    (i_tlb_resp_need_ptw),
+		.refill_valid     (i_refill_valid),
+		.refill_vpn       (ptw_refill_vpn),
+		.refill_ppn       (ptw_refill_ppn),
+		.refill_level     (ptw_refill_level),
+		.refill_mode      (ptw_refill_mode),
+		.refill_asid      (ptw_refill_asid),
+		.refill_g         (ptw_refill_g),
+		.refill_u         (ptw_refill_u),
+		.refill_r         (ptw_refill_r),
+		.refill_w         (ptw_refill_w),
+		.refill_x         (ptw_refill_x),
+		.refill_a         (ptw_refill_a),
+		.refill_d         (ptw_refill_d),
+		.flush_all        (flush_all),
+		.flush_asid_valid (flush_asid_valid),
+		.flush_asid       (flush_asid),
+		.flush_vpn_valid  (flush_vpn_valid),
+		.flush_vpn        (flush_vpn),
+		.flush_mode       (req_mode)
+	);
+
+	tlb #(
+		.ENTRIES(DTLB_ENTRIES),
+		.ASID_WIDTH(ASID_WIDTH)
+	) u_dtlb (
+		.clk              (clk),
+		.rst              (rst),
+		.req_valid        (d_accept),
+		.req_vaddr        (d_req_vaddr),
+		.req_asid         (req_asid),
+		.req_mode         (req_mode),
+		.req_priv         (priv_mode),
+		.req_is_fetch     (1'b0),
+		.req_is_store     (d_req_is_store),
+		.req_sum          (sum),
+		.req_mxr          (mxr),
+		.resp_valid       (d_tlb_resp_valid),
+		.resp_hit         (d_tlb_resp_hit),
+		.resp_paddr       (d_tlb_resp_paddr),
+		.resp_page_fault  (d_tlb_resp_page_fault),
+		.resp_need_ptw    (d_tlb_resp_need_ptw),
+		.refill_valid     (d_refill_valid),
+		.refill_vpn       (ptw_refill_vpn),
+		.refill_ppn       (ptw_refill_ppn),
+		.refill_level     (ptw_refill_level),
+		.refill_mode      (ptw_refill_mode),
+		.refill_asid      (ptw_refill_asid),
+		.refill_g         (ptw_refill_g),
+		.refill_u         (ptw_refill_u),
+		.refill_r         (ptw_refill_r),
+		.refill_w         (ptw_refill_w),
+		.refill_x         (ptw_refill_x),
+		.refill_a         (ptw_refill_a),
+		.refill_d         (ptw_refill_d),
+		.flush_all        (flush_all),
+		.flush_asid_valid (flush_asid_valid),
+		.flush_asid       (flush_asid),
+		.flush_vpn_valid  (flush_vpn_valid),
+		.flush_vpn        (flush_vpn),
+		.flush_mode       (req_mode)
+	);
+
+	ptw #(
+		.ASID_WIDTH(ASID_WIDTH)
+	) u_ptw (
+		.clk           (clk),
+		.rst           (rst),
+		.req_valid     (ptw_req_valid),
+		.req_ready     (ptw_req_ready),
+		.req_vaddr     ((ptw_owner_next == PTW_SRC_D) ? d_pending_vaddr_q : i_pending_vaddr_q),
+		.req_asid      ((ptw_owner_next == PTW_SRC_D) ? d_pending_asid_q : i_pending_asid_q),
+		.req_mode      ((ptw_owner_next == PTW_SRC_D) ? d_pending_mode_q : i_pending_mode_q),
+		.req_priv      ((ptw_owner_next == PTW_SRC_D) ? d_pending_priv_q : i_pending_priv_q),
+		.req_is_fetch  (ptw_owner_next == PTW_SRC_I),
+		.req_is_store  ((ptw_owner_next == PTW_SRC_D) ? d_pending_is_store_q : 1'b0),
+		.req_sum       ((ptw_owner_next == PTW_SRC_D) ? d_pending_sum_q : i_pending_sum_q),
+		.req_mxr       ((ptw_owner_next == PTW_SRC_D) ? d_pending_mxr_q : i_pending_mxr_q),
+		.satp_value    (satp_value),
+		.mem_req_valid (ptw_mem_req_valid),
+		.mem_req_ready (ptw_mem_req_ready),
+		.mem_req_addr  (ptw_mem_req_addr),
+		.mem_resp_valid(ptw_mem_resp_valid),
+		.mem_resp_data (ptw_mem_resp_data),
+		.resp_valid    (ptw_resp_valid),
+		.resp_fault    (ptw_resp_fault),
+		.resp_paddr    (ptw_resp_paddr),
+		.refill_valid  (ptw_refill_valid),
+		.refill_vpn    (ptw_refill_vpn),
+		.refill_ppn    (ptw_refill_ppn),
+		.refill_level  (ptw_refill_level),
+		.refill_mode   (ptw_refill_mode),
+		.refill_asid   (ptw_refill_asid),
+		.refill_g      (ptw_refill_g),
+		.refill_u      (ptw_refill_u),
+		.refill_r      (ptw_refill_r),
+		.refill_w      (ptw_refill_w),
+		.refill_x      (ptw_refill_x),
+		.refill_a      (ptw_refill_a),
+		.refill_d      (ptw_refill_d)
+	);
+
+endmodule
diff --git a/rv64cpu/rtl/core/mmu/ptw.sv b/rv64cpu/rtl/core/mmu/ptw.sv
new file mode 100644
index 0000000..6447b5a
--- /dev/null
+++ b/rv64cpu/rtl/core/mmu/ptw.sv
@@ -0,0 +1,416 @@
+//     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/mmu/ptw.sv
+// Description: Page-table walker for Sv39. Sv48/Sv57/Sv64 are explicit stubs.
+
+module ptw
+	import rv64_pkg::*;
+#(
+	parameter int ASID_WIDTH = 16
+)(
+	input  logic                   clk,
+	input  logic                   rst,
+
+	input  logic                   req_valid,
+	output logic                   req_ready,
+	input  logic [XLEN-1:0]        req_vaddr,
+	input  logic [ASID_WIDTH-1:0]  req_asid,
+	input  logic [3:0]             req_mode,
+	input  logic [1:0]             req_priv,
+	input  logic                   req_is_fetch,
+	input  logic                   req_is_store,
+	input  logic                   req_sum,
+	input  logic                   req_mxr,
+	input  logic [XLEN-1:0]        satp_value,
+
+	output logic                   mem_req_valid,
+	input  logic                   mem_req_ready,
+	output logic [XLEN-1:0]        mem_req_addr,
+	input  logic                   mem_resp_valid,
+	input  logic [XLEN-1:0]        mem_resp_data,
+
+	output logic                   resp_valid,
+	output logic                   resp_fault,
+	output logic [XLEN-1:0]        resp_paddr,
+
+	output logic                   refill_valid,
+	output logic [44:0]            refill_vpn,
+	output logic [43:0]            refill_ppn,
+	output logic [2:0]             refill_level,
+	output logic [3:0]             refill_mode,
+	output logic [ASID_WIDTH-1:0]  refill_asid,
+	output logic                   refill_g,
+	output logic                   refill_u,
+	output logic                   refill_r,
+	output logic                   refill_w,
+	output logic                   refill_x,
+	output logic                   refill_a,
+	output logic                   refill_d
+);
+
+	localparam logic [3:0] SATP_MODE_BARE = 4'd0;
+	localparam logic [3:0] SATP_MODE_SV39 = 4'd8;
+	localparam logic [3:0] SATP_MODE_SV48 = 4'd9;
+	localparam logic [3:0] SATP_MODE_SV57 = 4'd10;
+	localparam logic [3:0] SATP_MODE_SV64 = 4'd11; // Reserved/future stub.
+
+	typedef enum logic [2:0] {
+		PTW_IDLE,
+		PTW_ISSUE,
+		PTW_WAIT,
+		PTW_DONE
+	} ptw_state_t;
+
+	ptw_state_t state_q;
+
+	logic [XLEN-1:0]        req_vaddr_q;
+	logic [ASID_WIDTH-1:0]  req_asid_q;
+	logic [3:0]             req_mode_q;
+	logic [1:0]             req_priv_q;
+	logic                   req_is_fetch_q;
+	logic                   req_is_store_q;
+	logic                   req_sum_q;
+	logic                   req_mxr_q;
+
+	logic [43:0]            table_ppn_q;
+	logic [2:0]             level_q;
+
+	logic                   pte_v;
+	logic                   pte_r;
+	logic                   pte_w;
+	logic                   pte_x;
+	logic                   pte_u;
+	logic                   pte_g;
+	logic                   pte_a;
+	logic                   pte_d;
+	logic [43:0]            pte_ppn;
+
+	logic                   leaf_pte;
+	logic                   bad_pte;
+	logic                   superpage_misaligned;
+
+	logic [44:0]            req_vpn_q;
+	logic [XLEN-1:0]        next_paddr;
+
+	logic                   resp_fault_q;
+	logic [XLEN-1:0]        resp_paddr_q;
+	logic                   refill_valid_q;
+
+	logic [44:0]            refill_vpn_q;
+	logic [43:0]            refill_ppn_q;
+	logic [2:0]             refill_level_q;
+	logic [3:0]             refill_mode_q;
+	logic [ASID_WIDTH-1:0]  refill_asid_q;
+	logic                   refill_g_q;
+	logic                   refill_u_q;
+	logic                   refill_r_q;
+	logic                   refill_w_q;
+	logic                   refill_x_q;
+	logic                   refill_a_q;
+	logic                   refill_d_q;
+
+	function automatic logic is_supported_mode(input logic [3:0] mode);
+		return (mode == SATP_MODE_SV39);
+	endfunction
+
+	function automatic int vpn_stride(input logic [3:0] mode);
+		case (mode)
+			SATP_MODE_SV39: vpn_stride = 9;
+			default:        vpn_stride = 0;
+		endcase
+	endfunction
+
+	function automatic logic [44:0] vaddr_to_vpn(
+		input logic [XLEN-1:0] vaddr,
+		input logic [3:0] mode
+	);
+		logic [44:0] vpn;
+		begin
+			vpn = '0;
+			case (mode)
+				SATP_MODE_SV39: vpn[26:0] = vaddr[38:12];
+				default:         vpn = '0;
+			endcase
+			return vpn;
+		end
+	endfunction
+
+	function automatic logic [9:0] vpn_index(
+		input logic [XLEN-1:0] vaddr,
+		input logic [3:0] mode,
+		input logic [2:0] level
+	);
+		logic [9:0] idx;
+		begin
+			idx = '0;
+			case (mode)
+				SATP_MODE_SV39: idx = {1'b0, vaddr[12 + level * 9 +: 9]};
+				default: idx = '0;
+			endcase
+			return idx;
+		end
+	endfunction
+
+	function automatic logic vaddr_is_canonical(
+		input logic [XLEN-1:0] vaddr,
+		input logic [3:0] mode
+	);
+		case (mode)
+			SATP_MODE_SV39: vaddr_is_canonical = (vaddr[63:39] == {25{vaddr[38]}});
+			default:        vaddr_is_canonical = 1'b1;
+		endcase
+	endfunction
+
+	function automatic logic [5:0] page_shift(
+		input logic [3:0] mode,
+		input logic [2:0] level
+	);
+		int stride;
+		begin
+			stride = vpn_stride(mode);
+			return 6'(12 + level * stride);
+		end
+	endfunction
+
+	function automatic logic allow_access(
+		input logic is_fetch,
+		input logic is_store,
+		input logic [1:0] priv,
+		input logic sum,
+		input logic mxr,
+		input logic u,
+		input logic r,
+		input logic w,
+		input logic x,
+		input logic a,
+		input logic d
+	);
+		logic is_load;
+		logic readable;
+		logic priv_u;
+		logic priv_ok;
+		begin
+			is_load = ~is_fetch && ~is_store;
+			readable = r || (mxr && x);
+			priv_u = (priv == 2'b00);
+
+			if (u) begin
+				if (priv_u) begin
+					priv_ok = 1'b1;
+				end else begin
+					priv_ok = ~is_fetch && sum;
+				end
+			end else begin
+				priv_ok = ~priv_u;
+			end
+
+			return a && priv_ok && (
+				(is_fetch && x) ||
+				(is_store && w && d) ||
+				(is_load && readable)
+			);
+		end
+	endfunction
+
+	assign pte_v = mem_resp_data[0];
+	assign pte_r = mem_resp_data[1];
+	assign pte_w = mem_resp_data[2];
+	assign pte_x = mem_resp_data[3];
+	assign pte_u = mem_resp_data[4];
+	assign pte_g = mem_resp_data[5];
+	assign pte_a = mem_resp_data[6];
+	assign pte_d = mem_resp_data[7];
+	assign pte_ppn = mem_resp_data[53:10];
+
+	assign leaf_pte = pte_r || pte_x;
+	assign bad_pte = (!pte_v) || (pte_w && !pte_r);
+
+	always_comb begin
+		int stride;
+		int lower_bits;
+		logic [63:0] ppn_mask;
+		stride = vpn_stride(req_mode_q);
+		lower_bits = level_q * stride;
+		ppn_mask = '0;
+		superpage_misaligned = 1'b0;
+		if (leaf_pte && (lower_bits > 0)) begin
+			if (lower_bits >= 44) begin
+				ppn_mask = 64'h0000_0FFF_FFFF_FFFF;
+			end else begin
+				ppn_mask = (64'd1 << lower_bits) - 1;
+			end
+			superpage_misaligned = |({20'b0, pte_ppn} & ppn_mask);
+		end
+	end
+
+	always_comb begin
+		logic [5:0] shift;
+		logic [XLEN-1:0] low_mask;
+		int shift_index;
+		shift = page_shift(req_mode_q, level_q);
+		shift_index = int'(shift);
+		if (shift_index >= XLEN) begin
+			low_mask = {XLEN{1'b1}};
+		end else begin
+			low_mask = ({{(XLEN-1){1'b0}}, 1'b1} << shift_index) - 1;
+		end
+		next_paddr = (({{(XLEN-56){1'b0}}, pte_ppn, 12'b0}) & ~low_mask) | (req_vaddr_q & low_mask);
+	end
+
+	assign req_ready = (state_q == PTW_IDLE);
+	assign mem_req_valid = (state_q == PTW_ISSUE);
+	assign mem_req_addr = {{(XLEN-56){1'b0}}, table_ppn_q, 12'b0} +
+						 ({{(XLEN-10){1'b0}}, vpn_index(req_vaddr_q, req_mode_q, level_q)} << 3);
+
+	assign resp_valid = (state_q == PTW_DONE);
+	assign resp_fault = resp_fault_q;
+	assign resp_paddr = resp_paddr_q;
+
+	assign refill_valid = refill_valid_q && (state_q == PTW_DONE);
+	assign refill_vpn = refill_vpn_q;
+	assign refill_ppn = refill_ppn_q;
+	assign refill_level = refill_level_q;
+	assign refill_mode = refill_mode_q;
+	assign refill_asid = refill_asid_q;
+	assign refill_g = refill_g_q;
+	assign refill_u = refill_u_q;
+	assign refill_r = refill_r_q;
+	assign refill_w = refill_w_q;
+	assign refill_x = refill_x_q;
+	assign refill_a = refill_a_q;
+	assign refill_d = refill_d_q;
+
+	always_ff @(posedge clk) begin
+		if (rst) begin
+			state_q <= PTW_IDLE;
+			req_vaddr_q <= '0;
+			req_asid_q <= '0;
+			req_mode_q <= SATP_MODE_BARE;
+			req_priv_q <= '0;
+			req_is_fetch_q <= 1'b0;
+			req_is_store_q <= 1'b0;
+			req_sum_q <= 1'b0;
+			req_mxr_q <= 1'b0;
+			table_ppn_q <= '0;
+			level_q <= '0;
+			req_vpn_q <= '0;
+			resp_fault_q <= 1'b0;
+			resp_paddr_q <= '0;
+			refill_valid_q <= 1'b0;
+			refill_vpn_q <= '0;
+			refill_ppn_q <= '0;
+			refill_level_q <= '0;
+			refill_mode_q <= SATP_MODE_BARE;
+			refill_asid_q <= '0;
+			refill_g_q <= 1'b0;
+			refill_u_q <= 1'b0;
+			refill_r_q <= 1'b0;
+			refill_w_q <= 1'b0;
+			refill_x_q <= 1'b0;
+			refill_a_q <= 1'b0;
+			refill_d_q <= 1'b0;
+		end else begin
+			case (state_q)
+				PTW_IDLE: begin
+					refill_valid_q <= 1'b0;
+					if (req_valid) begin
+						req_vaddr_q <= req_vaddr;
+						req_asid_q <= req_asid;
+						req_mode_q <= req_mode;
+						req_priv_q <= req_priv;
+						req_is_fetch_q <= req_is_fetch;
+						req_is_store_q <= req_is_store;
+						req_sum_q <= req_sum;
+						req_mxr_q <= req_mxr;
+						req_vpn_q <= vaddr_to_vpn(req_vaddr, req_mode);
+
+						if (!is_supported_mode(req_mode) || !vaddr_is_canonical(req_vaddr, req_mode)) begin
+							resp_fault_q <= 1'b1;
+							resp_paddr_q <= '0;
+							state_q <= PTW_DONE;
+						end else begin
+							table_ppn_q <= satp_value[43:0];
+							case (req_mode)
+								SATP_MODE_SV39: level_q <= 3'd2;
+								default:        level_q <= '0;
+							endcase
+							state_q <= PTW_ISSUE;
+						end
+					end
+				end
+
+				PTW_ISSUE: begin
+					if (mem_req_ready) begin
+						state_q <= PTW_WAIT;
+					end
+				end
+
+				PTW_WAIT: begin
+					if (mem_resp_valid) begin
+						if (bad_pte) begin
+							resp_fault_q <= 1'b1;
+							resp_paddr_q <= '0;
+							refill_valid_q <= 1'b0;
+							state_q <= PTW_DONE;
+						end else if (leaf_pte) begin
+							if (superpage_misaligned ||
+								!allow_access(req_is_fetch_q, req_is_store_q, req_priv_q, req_sum_q, req_mxr_q,
+												  pte_u, pte_r, pte_w, pte_x, pte_a, pte_d)) begin
+								resp_fault_q <= 1'b1;
+								resp_paddr_q <= '0;
+								refill_valid_q <= 1'b0;
+							end else begin
+								resp_fault_q <= 1'b0;
+								resp_paddr_q <= next_paddr;
+								refill_valid_q <= 1'b1;
+								refill_vpn_q <= req_vpn_q;
+								refill_ppn_q <= pte_ppn;
+								refill_level_q <= level_q;
+								refill_mode_q <= req_mode_q;
+								refill_asid_q <= req_asid_q;
+								refill_g_q <= pte_g;
+								refill_u_q <= pte_u;
+								refill_r_q <= pte_r;
+								refill_w_q <= pte_w;
+								refill_x_q <= pte_x;
+								refill_a_q <= pte_a;
+								refill_d_q <= pte_d;
+							end
+							state_q <= PTW_DONE;
+						end else if (level_q == 0) begin
+							resp_fault_q <= 1'b1;
+							resp_paddr_q <= '0;
+							refill_valid_q <= 1'b0;
+							state_q <= PTW_DONE;
+						end else begin
+							table_ppn_q <= pte_ppn;
+							level_q <= level_q - 1'b1;
+							state_q <= PTW_ISSUE;
+						end
+					end
+				end
+
+				PTW_DONE: begin
+					state_q <= PTW_IDLE;
+				end
+
+				default: state_q <= PTW_IDLE;
+			endcase
+		end
+	end
+
+endmodule
diff --git a/rv64cpu/rtl/core/mmu/tlb.sv b/rv64cpu/rtl/core/mmu/tlb.sv
new file mode 100644
index 0000000..06eb947
--- /dev/null
+++ b/rv64cpu/rtl/core/mmu/tlb.sv
@@ -0,0 +1,382 @@
+//     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/mmu/tlb.sv
+// Description: Parameterized L1 TLB (Bare + Sv39, higher modes stubbed)
+
+module tlb
+	import rv64_pkg::*;
+#(
+	parameter int ENTRIES = 32,
+	parameter int ASID_WIDTH = 16
+)(
+	input  logic                   clk,
+	input  logic                   rst,
+
+	// Translation request
+	input  logic                   req_valid,
+	input  logic [XLEN-1:0]        req_vaddr,
+	input  logic [ASID_WIDTH-1:0]  req_asid,
+	input  logic [3:0]             req_mode,       // satp.MODE
+	input  logic [1:0]             req_priv,       // 0=U,1=S,3=M
+	input  logic                   req_is_fetch,
+	input  logic                   req_is_store,
+	input  logic                   req_sum,
+	input  logic                   req_mxr,
+
+	// Translation response
+	output logic                   resp_valid,
+	output logic                   resp_hit,
+	output logic [XLEN-1:0]        resp_paddr,
+	output logic                   resp_page_fault,
+	output logic                   resp_need_ptw,
+
+	// Refill from PTW
+	input  logic                   refill_valid,
+	input  logic [44:0]            refill_vpn,     // normalized VPN, max Sv57 width
+	input  logic [43:0]            refill_ppn,
+	input  logic [2:0]             refill_level,   // 0=4KB leaf, 1=next superpage ...
+	input  logic [3:0]             refill_mode,
+	input  logic [ASID_WIDTH-1:0]  refill_asid,
+	input  logic                   refill_g,
+	input  logic                   refill_u,
+	input  logic                   refill_r,
+	input  logic                   refill_w,
+	input  logic                   refill_x,
+	input  logic                   refill_a,
+	input  logic                   refill_d,
+
+	// sfence / invalidation hooks
+	input  logic                   flush_all,
+	input  logic                   flush_asid_valid,
+	input  logic [ASID_WIDTH-1:0]  flush_asid,
+	input  logic                   flush_vpn_valid,
+	input  logic [44:0]            flush_vpn,
+	input  logic [3:0]             flush_mode
+);
+
+	localparam logic [3:0] SATP_MODE_BARE = 4'd0;
+	localparam logic [3:0] SATP_MODE_SV39 = 4'd8;
+	localparam logic [3:0] SATP_MODE_SV48 = 4'd9;
+	localparam logic [3:0] SATP_MODE_SV57 = 4'd10;
+	localparam logic [3:0] SATP_MODE_SV64 = 4'd11; // Reserved/future stub.
+
+	typedef struct packed {
+		logic                   valid;
+		logic [3:0]             mode;
+		logic [ASID_WIDTH-1:0]  asid;
+		logic [44:0]            vpn;
+		logic [43:0]            ppn;
+		logic [2:0]             level;
+		logic                   g;
+		logic                   u;
+		logic                   r;
+		logic                   w;
+		logic                   x;
+		logic                   a;
+		logic                   d;
+	} tlb_entry_t;
+
+	tlb_entry_t entries [0:ENTRIES-1];
+	logic [$clog2(ENTRIES)-1:0] repl_ptr;
+
+	logic [44:0] req_vpn;
+	logic [43:0] hit_ppn;
+	logic [2:0]  hit_level;
+	logic [3:0]  hit_mode;
+	logic        hit_u, hit_r, hit_w, hit_x, hit_a, hit_d;
+	logic        hit_found;
+	logic [5:0]  shift;
+	logic [XLEN-1:0] low_mask;
+	logic        req_priv_u;
+	logic [XLEN-1:0] hit_ppn_xlen;
+	int unsigned shift_index;
+
+	function automatic logic is_supported_mode(input logic [3:0] mode);
+		return (mode == SATP_MODE_BARE) || (mode == SATP_MODE_SV39);
+	endfunction
+
+	function automatic int vpn_width(input logic [3:0] mode);
+		case (mode)
+			SATP_MODE_SV39: vpn_width = 27;
+			default:        vpn_width = 0;
+		endcase
+	endfunction
+
+	function automatic int vpn_stride(input logic [3:0] mode);
+		case (mode)
+			SATP_MODE_SV39: vpn_stride = 9;
+			default:        vpn_stride = 0;
+		endcase
+	endfunction
+
+	function automatic logic [44:0] make_vpn_mask(
+		input logic [3:0] mode,
+		input logic [2:0] level
+	);
+		logic [44:0] mask;
+		int width;
+		int stride;
+		int ignore;
+		int i;
+		begin
+			mask = '0;
+			width = vpn_width(mode);
+			stride = vpn_stride(mode);
+			ignore = level * stride;
+
+			if (width > 0) begin
+				for (i = ignore; i < width; i++) begin
+					mask[i] = 1'b1;
+				end
+			end
+
+			make_vpn_mask = mask;
+		end
+	endfunction
+
+	function automatic logic [44:0] vaddr_to_vpn(
+		input logic [XLEN-1:0] vaddr,
+		input logic [3:0] mode
+	);
+		logic [44:0] vpn;
+		begin
+			vpn = '0;
+			case (mode)
+				SATP_MODE_SV39: vpn[26:0] = vaddr[38:12];
+				default:         vpn = '0;
+			endcase
+			vaddr_to_vpn = vpn;
+		end
+	endfunction
+
+	function automatic logic vaddr_is_canonical(
+		input logic [XLEN-1:0] vaddr,
+		input logic [3:0] mode
+	);
+		case (mode)
+			SATP_MODE_SV39: vaddr_is_canonical = (vaddr[63:39] == {25{vaddr[38]}});
+			default:        vaddr_is_canonical = 1'b1;
+		endcase
+	endfunction
+
+	function automatic logic [5:0] page_shift(
+		input logic [3:0] mode,
+		input logic [2:0] level
+	);
+		int stride;
+		begin
+			stride = vpn_stride(mode);
+			page_shift = 6'(12 + level * stride);
+		end
+	endfunction
+
+	function automatic logic entry_match(
+		input tlb_entry_t e,
+		input logic [44:0] vpn,
+		input logic [ASID_WIDTH-1:0] asid,
+		input logic [3:0] mode
+	);
+		logic [44:0] mask;
+		begin
+			mask = make_vpn_mask(e.mode, e.level);
+			entry_match = e.valid && (e.mode == mode) && (e.g || (e.asid == asid)) &&
+						  ((((e.vpn ^ vpn) & mask) == '0));
+		end
+	endfunction
+
+	function automatic logic allow_access(
+		input logic is_fetch,
+		input logic is_store,
+		input logic priv_u,
+		input logic sum,
+		input logic mxr,
+		input logic u,
+		input logic r,
+		input logic w,
+		input logic x,
+		input logic a,
+		input logic d
+	);
+		logic is_load;
+		logic readable;
+		logic priv_ok;
+		begin
+			is_load = ~is_fetch && ~is_store;
+			readable = r || (mxr && x);
+
+			// U=1 page: only U-mode unless SUM permits S-mode data access
+			// U=0 page: S/M-mode only
+			if (u) begin
+				if (priv_u) begin
+					priv_ok = 1'b1;
+				end else begin
+					priv_ok = ~is_fetch && sum;
+				end
+			end else begin
+				priv_ok = ~priv_u;
+			end
+
+			allow_access = a && priv_ok && (
+				(is_fetch && x) ||
+				(is_store && w && d) ||
+				(is_load  && readable)
+			);
+		end
+	endfunction
+
+	integer k;
+	always_comb begin
+		req_vpn = vaddr_to_vpn(req_vaddr, req_mode);
+
+		resp_valid = req_valid;
+		resp_hit = 1'b0;
+		resp_paddr = req_vaddr;
+		resp_page_fault = 1'b0;
+		resp_need_ptw = 1'b0;
+
+		shift = '0;
+		low_mask = '0;
+		req_priv_u = 1'b0;
+		hit_ppn_xlen = '0;
+		shift_index = 0;
+
+		hit_found = 1'b0;
+		hit_ppn = '0;
+		hit_level = '0;
+		hit_mode = '0;
+		hit_u = 1'b0;
+		hit_r = 1'b0;
+		hit_w = 1'b0;
+		hit_x = 1'b0;
+		hit_a = 1'b0;
+		hit_d = 1'b0;
+
+		if (req_valid) begin
+			if (!is_supported_mode(req_mode) || !vaddr_is_canonical(req_vaddr, req_mode)) begin
+				resp_page_fault = 1'b1;
+			end else if (req_mode == SATP_MODE_BARE || req_priv == 2'b11) begin
+				// Bare mode or M-mode bypasses translation.
+				resp_hit = 1'b1;
+				resp_paddr = req_vaddr;
+			end else begin
+				for (k = 0; k < ENTRIES; k++) begin
+					if (!hit_found && entry_match(entries[k], req_vpn, req_asid, req_mode)) begin
+						hit_found = 1'b1;
+						hit_ppn = entries[k].ppn;
+						hit_level = entries[k].level;
+						hit_mode = entries[k].mode;
+						hit_u = entries[k].u;
+						hit_r = entries[k].r;
+						hit_w = entries[k].w;
+						hit_x = entries[k].x;
+						hit_a = entries[k].a;
+						hit_d = entries[k].d;
+					end
+				end
+
+				if (hit_found) begin
+					shift = page_shift(hit_mode, hit_level);
+					shift_index = int'(shift);
+					if (shift_index >= XLEN) begin
+						low_mask = {XLEN{1'b1}};
+					end else begin
+						low_mask = ({{(XLEN-1){1'b0}}, 1'b1} << shift_index) - 1;
+					end
+
+					hit_ppn_xlen = {{(XLEN-56){1'b0}}, hit_ppn, 12'b0};
+
+					req_priv_u = (req_priv == 2'b00);
+					if (!allow_access(req_is_fetch, req_is_store, req_priv_u, req_sum, req_mxr,
+									  hit_u, hit_r, hit_w, hit_x, hit_a, hit_d)) begin
+						resp_page_fault = 1'b1;
+					end else begin
+						resp_hit = 1'b1;
+						resp_paddr = (hit_ppn_xlen & ~low_mask) | (req_vaddr & low_mask);
+					end
+				end else begin
+					resp_need_ptw = 1'b1;
+				end
+			end
+		end
+	end
+
+	integer i;
+	always_ff @(posedge clk) begin
+		if (rst) begin
+			repl_ptr <= '0;
+			for (i = 0; i < ENTRIES; i++) begin
+				entries[i].valid <= 1'b0;
+				entries[i].mode <= SATP_MODE_BARE;
+				entries[i].asid <= '0;
+				entries[i].vpn <= '0;
+				entries[i].ppn <= '0;
+				entries[i].level <= '0;
+				entries[i].g <= 1'b0;
+				entries[i].u <= 1'b0;
+				entries[i].r <= 1'b0;
+				entries[i].w <= 1'b0;
+				entries[i].x <= 1'b0;
+				entries[i].a <= 1'b0;
+				entries[i].d <= 1'b0;
+			end
+		end else begin
+			if (flush_all) begin
+				for (i = 0; i < ENTRIES; i++) begin
+					entries[i].valid <= 1'b0;
+				end
+			end else if (flush_asid_valid || flush_vpn_valid) begin
+				for (i = 0; i < ENTRIES; i++) begin
+					logic asid_match;
+					logic vpn_match;
+					logic mode_match;
+					logic [44:0] mask;
+
+					asid_match = !flush_asid_valid || (!entries[i].g && (entries[i].asid == flush_asid));
+					mode_match = !flush_vpn_valid || (entries[i].mode == flush_mode);
+					if (flush_vpn_valid) begin
+						mask = make_vpn_mask(entries[i].mode, entries[i].level);
+						vpn_match = (((entries[i].vpn ^ flush_vpn) & mask) == '0);
+					end else begin
+						vpn_match = 1'b1;
+					end
+
+					if (entries[i].valid && asid_match && mode_match && vpn_match) begin
+						entries[i].valid <= 1'b0;
+					end
+				end
+			end
+
+			if (refill_valid && is_supported_mode(refill_mode) && (refill_mode != SATP_MODE_BARE)) begin
+				entries[repl_ptr].valid <= 1'b1;
+				entries[repl_ptr].mode <= refill_mode;
+				entries[repl_ptr].asid <= refill_asid;
+				entries[repl_ptr].vpn <= refill_vpn;
+				entries[repl_ptr].ppn <= refill_ppn;
+				entries[repl_ptr].level <= refill_level;
+				entries[repl_ptr].g <= refill_g;
+				entries[repl_ptr].u <= refill_u;
+				entries[repl_ptr].r <= refill_r;
+				entries[repl_ptr].w <= refill_w;
+				entries[repl_ptr].x <= refill_x;
+				entries[repl_ptr].a <= refill_a;
+				entries[repl_ptr].d <= refill_d;
+				repl_ptr <= repl_ptr + 1'b1;
+			end
+		end
+	end
+
+endmodule
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
diff --git a/rv64cpu/rtl/core/privileged/csr_unit.sv b/rv64cpu/rtl/core/privileged/csr_unit.sv
new file mode 100644
index 0000000..a25e17a
--- /dev/null
+++ b/rv64cpu/rtl/core/privileged/csr_unit.sv
@@ -0,0 +1,1221 @@
+//     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/csr_unit.sv
+
+module csr_unit
+	import rv64_pkg::*;
+	import csr_pkg::*;
+#(
+	parameter logic [XLEN-1:0] HART_ID         = '0,
+	parameter logic [XLEN-1:0] VENDOR_ID       = '0,
+	parameter logic [XLEN-1:0] ARCH_ID         = '0,
+	parameter logic [XLEN-1:0] IMPL_ID         = '0,
+	parameter logic [XLEN-1:0] MISA_VALUE      = 64'h8000_0000_0014_11ad,
+	parameter int              PHYS_ADDR_WIDTH = 56,
+	parameter int              PMP_GRANULARITY = 0
+)(
+	input  logic              clk,
+	input  logic              rst,
+	input  logic              csr_req,
+	input  logic [2:0]        csr_op,
+	input  logic [11:0]       csr_addr,
+	input  logic [XLEN-1:0]   csr_rs1_data,
+	input  logic              trap_ctrl_trigger,
+	input  logic              trap_ctrl_to_s_mode,
+	input  logic [XLEN-1:0]   trap_ctrl_mepc,
+	input  logic [XLEN-1:0]   trap_ctrl_mcause,
+	input  logic [XLEN-1:0]   trap_ctrl_mtval,
+	input  logic              trap_ctrl_mret_en,
+	input  logic              trap_ctrl_sret_en,
+	input  logic              m_timer_interrupt,
+	input  logic              m_software_interrupt,
+	input  logic              m_external_interrupt,
+	input  logic              s_timer_interrupt,
+	input  logic              s_software_interrupt,
+	input  logic              s_external_interrupt,
+	input  logic              fp_state_dirty,
+	input  logic              fp_fflags_valid,
+	input  logic [4:0]        fp_fflags,
+	output logic [XLEN-1:0]   csr_rdata,
+	output logic [XLEN-1:0]   csr_mstatus,
+	output logic [XLEN-1:0]   csr_sstatus,
+	output logic [XLEN-1:0]   csr_medeleg,
+	output logic [XLEN-1:0]   csr_mideleg,
+	output logic [XLEN-1:0]   csr_mie,
+	output logic [XLEN-1:0]   csr_sie,
+	output logic [XLEN-1:0]   csr_mip,
+	output logic [XLEN-1:0]   csr_sip,
+	output logic [XLEN-1:0]   csr_mtvec,
+	output logic [XLEN-1:0]   csr_stvec,
+	output logic [XLEN-1:0]   csr_mepc,
+	output logic [XLEN-1:0]   csr_sepc,
+	output logic [XLEN-1:0]   csr_satp,
+	output logic [2:0]        csr_frm,
+	output logic [1:0]        csr_priv_mode,
+	output logic [XLEN-1:0]   csr_trap_vector,
+	output logic              csr_trap_to_s_mode,
+	output logic [XLEN-1:0]   csr_trap_vector_next,
+	output logic              csr_trap_to_s_mode_next,
+	output pmpcfg_entry_t     csr_pmpcfg_entries [PMP_ENTRY_COUNT],
+	output pmpaddr_entry_t    csr_pmpaddr_entries [PMP_ENTRY_COUNT],
+	output logic              csr_illegal_access
+);
+
+	typedef enum logic [1:0] {
+		PRIV_MODE_U = 2'b00,
+		PRIV_MODE_S = 2'b01,
+		PRIV_MODE_M = 2'b11
+	} priv_mode_t;
+
+	localparam int MSTATUS_MIE_BIT  = 3;
+	localparam int MSTATUS_MPIE_BIT = 7;
+	localparam int MSTATUS_VS_LO    = 9;
+	localparam int MSTATUS_VS_HI    = 10;
+	localparam int MSTATUS_MPP_LO   = 11;
+	localparam int MSTATUS_MPP_HI   = 12;
+	localparam int MSTATUS_FS_LO    = 13;
+	localparam int MSTATUS_FS_HI    = 14;
+	localparam int MSTATUS_XS_LO    = 15;
+	localparam int MSTATUS_XS_HI    = 16;
+	localparam int MSTATUS_MPRV_BIT = 17;
+	localparam int MSTATUS_SUM_BIT  = 18;
+	localparam int MSTATUS_MXR_BIT  = 19;
+	localparam int MSTATUS_TVM_BIT  = 20;
+	localparam int MSTATUS_TW_BIT   = 21;
+	localparam int MSTATUS_TSR_BIT  = 22;
+	localparam int MSTATUS_UXL_LO   = 32;
+	localparam int MSTATUS_UXL_HI   = 33;
+	localparam int MSTATUS_SXL_LO   = 34;
+	localparam int MSTATUS_SXL_HI   = 35;
+	localparam int MSTATUS_SD_BIT   = 63;
+	localparam logic [1:0] STATUS_XLEN_64 = 2'b10;
+	localparam int SSTATUS_SIE_BIT  = 1;
+	localparam int SSTATUS_SPIE_BIT = 5;
+	localparam int SSTATUS_SPP_BIT  = 8;
+	localparam int SSTATUS_VS_LO    = MSTATUS_VS_LO;
+	localparam int SSTATUS_VS_HI    = MSTATUS_VS_HI;
+	localparam int SSTATUS_FS_LO    = MSTATUS_FS_LO;
+	localparam int SSTATUS_FS_HI    = MSTATUS_FS_HI;
+	localparam int SSTATUS_XS_LO    = MSTATUS_XS_LO;
+	localparam int SSTATUS_XS_HI    = MSTATUS_XS_HI;
+	localparam int SSTATUS_SUM_BIT  = MSTATUS_SUM_BIT;
+	localparam int SSTATUS_MXR_BIT  = MSTATUS_MXR_BIT;
+	localparam int SSTATUS_UXL_LO   = MSTATUS_UXL_LO;
+	localparam int SSTATUS_UXL_HI   = MSTATUS_UXL_HI;
+	localparam int SSTATUS_SD_BIT   = MSTATUS_SD_BIT;
+	localparam int SATP_MODE_LO     = 60;
+	localparam int SATP_MODE_HI     = 63;
+	localparam int SATP_ASID_LO     = 44;
+	localparam int SATP_ASID_HI     = 59;
+	localparam int SATP_PPN_LO      = 0;
+	localparam int SATP_PPN_HI      = 43;
+	localparam logic [3:0] SATP_MODE_BARE = 4'd0;
+	localparam logic [3:0] SATP_MODE_SV39 = 4'd8;
+	localparam logic [3:0] SATP_MODE_SV48 = 4'd9;  // Stub: unsupported until Sv48 is implemented.
+	localparam logic [3:0] SATP_MODE_SV57 = 4'd10; // Stub: unsupported until Sv57 is implemented.
+	localparam logic [3:0] SATP_MODE_SV64 = 4'd11; // Reserved/future stub.
+	localparam int MCOUNTINHIBIT_CY_BIT = 0;
+	localparam int MCOUNTINHIBIT_IR_BIT = 2;
+	localparam int COUNTEREN_CY_BIT = 0;
+	localparam int COUNTEREN_TM_BIT = 1;
+	localparam int COUNTEREN_IR_BIT = 2;
+	localparam int HPM_COUNTER_COUNT = 29;
+	localparam int INTERRUPT_S_SOFTWARE_BIT = int'(INTERRUPT_S_SOFTWARE);
+	localparam int INTERRUPT_S_TIMER_BIT    = int'(INTERRUPT_S_TIMER);
+	localparam int INTERRUPT_S_EXTERNAL_BIT = int'(INTERRUPT_S_EXTERNAL);
+	localparam int INTERRUPT_M_SOFTWARE_BIT = int'(INTERRUPT_M_SOFTWARE);
+	localparam int INTERRUPT_M_TIMER_BIT    = int'(INTERRUPT_M_TIMER);
+	localparam int INTERRUPT_M_EXTERNAL_BIT = int'(INTERRUPT_M_EXTERNAL);
+	localparam logic [XLEN-1:0] S_INTERRUPT_MASK =
+		(64'h1 << INTERRUPT_S_SOFTWARE) |
+		(64'h1 << INTERRUPT_S_TIMER) |
+		(64'h1 << INTERRUPT_S_EXTERNAL);
+	localparam logic [XLEN-1:0] M_INTERRUPT_MASK =
+		(64'h1 << INTERRUPT_M_SOFTWARE) |
+		(64'h1 << INTERRUPT_M_TIMER) |
+		(64'h1 << INTERRUPT_M_EXTERNAL);
+	localparam logic [XLEN-1:0] INTERRUPT_MASK = S_INTERRUPT_MASK | M_INTERRUPT_MASK;
+
+	// Machine-mode CSRs
+	logic [XLEN-1:0] mstatus_reg;
+	logic [XLEN-1:0] medeleg_reg;
+	logic [XLEN-1:0] mideleg_reg;
+	logic [XLEN-1:0] mie_reg;
+	logic [XLEN-1:0] mtvec_reg;
+	logic [XLEN-1:0] mcounteren_reg;
+	logic [XLEN-1:0] mscratch_reg;
+	logic [XLEN-1:0] mepc_reg;
+	logic [XLEN-1:0] mcause_reg;
+	logic [XLEN-1:0] mtval_reg;
+	logic [XLEN-1:0] mip_reg;
+	logic [XLEN-1:0] mtinst_reg;
+	logic [XLEN-1:0] mtval2_reg;
+	logic [XLEN-1:0] menvcfg_reg;
+	logic [XLEN-1:0] mstateen_reg [4];
+	pmpcfg_entry_t  pmpcfg_entries [PMP_ENTRY_COUNT];
+	pmpaddr_entry_t pmpaddr_entries [PMP_ENTRY_COUNT];
+
+	// Supervisor-mode CSRs
+	logic [XLEN-1:0] sstatus_reg;
+	logic [XLEN-1:0] stvec_reg;
+	logic [XLEN-1:0] scounteren_reg;
+	logic [XLEN-1:0] senvcfg_reg;
+	logic [XLEN-1:0] sstateen_reg [4];
+	logic [XLEN-1:0] sscratch_reg;
+	logic [XLEN-1:0] sepc_reg;
+	logic [XLEN-1:0] scause_reg;
+	logic [XLEN-1:0] stval_reg;
+	logic [XLEN-1:0] satp_reg;
+	logic [XLEN-1:0] hstatus_reg;
+	logic [XLEN-1:0] hedeleg_reg;
+	logic [XLEN-1:0] hideleg_reg;
+	logic [XLEN-1:0] hie_reg;
+	logic [XLEN-1:0] htimedelta_reg;
+	logic [XLEN-1:0] hcounteren_reg;
+	logic [XLEN-1:0] hgeie_reg;
+	logic [XLEN-1:0] henvcfg_reg;
+	logic [XLEN-1:0] hstateen_reg [4];
+	logic [XLEN-1:0] htval_reg;
+	logic [XLEN-1:0] hip_reg;
+	logic [XLEN-1:0] hvip_reg;
+	logic [XLEN-1:0] htinst_reg;
+	logic [XLEN-1:0] hgatp_reg;
+	logic [XLEN-1:0] vsstatus_reg;
+	logic [XLEN-1:0] vsie_reg;
+	logic [XLEN-1:0] vstvec_reg;
+	logic [XLEN-1:0] vsscratch_reg;
+	logic [XLEN-1:0] vsepc_reg;
+	logic [XLEN-1:0] vscause_reg;
+	logic [XLEN-1:0] vstval_reg;
+	logic [XLEN-1:0] vsip_reg;
+	logic [XLEN-1:0] vsatp_reg;
+
+	// Counters
+	logic [XLEN-1:0] mcycle_reg;
+	logic [XLEN-1:0] minstret_reg;
+	logic [XLEN-1:0] time_reg;
+	logic [XLEN-1:0] mcountinhibit_reg;
+	logic [XLEN-1:0] mhpmcounter_reg [HPM_COUNTER_COUNT];
+
+	// Floating-point CSRs
+	logic [4:0]       fflags_reg;
+	logic [2:0]       frm_reg;
+
+	priv_mode_t      priv_mode_q;
+
+	logic [11:0]     csr_addr_canonical;
+	logic            is_write_cmd;
+	logic            is_set_cmd;
+	logic            is_clear_cmd;
+	logic            has_non_zero_data;
+	logic            csr_write_en;
+	logic [XLEN-1:0] trap_vector_target;
+	logic            csr_addr_supported;
+	logic            csr_privilege_illegal;
+	logic            csr_write_illegal;
+	logic            csr_stateen_illegal;
+	logic [XLEN-1:0] csr_read_data;
+	logic [XLEN-1:0] mip_value;
+	logic [XLEN-1:0] sip_value;
+	logic [XLEN-1:0] sie_value;
+	logic [XLEN-1:0] trap_vector_q;
+	logic            trap_to_s_q;
+	logic            trap_taken;
+	logic            trap_taken_to_s_mode;
+	logic [XLEN-1:0] trap_taken_mepc;
+	logic [XLEN-1:0] trap_taken_mcause;
+	logic [XLEN-1:0] trap_taken_mtval;
+
+	// Canonicalize addresses, decode operations, and prepare CSR/trap metadata.
+	assign csr_addr_canonical = canonical_addr(csr_addr);
+	assign is_write_cmd       = (csr_op == 3'b001) || (csr_op == 3'b101);
+	assign is_set_cmd         = (csr_op == 3'b010) || (csr_op == 3'b110);
+	assign is_clear_cmd       = (csr_op == 3'b011) || (csr_op == 3'b111);
+	assign has_non_zero_data  = |csr_rs1_data;
+	assign csr_addr_supported = csr_is_supported(csr_addr_canonical);
+	assign csr_write_en       = csr_req & csr_addr_supported & !csr_privilege_illegal &
+	                            !csr_write_illegal & !csr_stateen_illegal &
+	                            (is_write_cmd | ((is_set_cmd | is_clear_cmd) & has_non_zero_data));
+	assign trap_taken               = trap_ctrl_trigger;
+	assign trap_taken_to_s_mode     = trap_ctrl_to_s_mode;
+	assign trap_taken_mepc          = trap_ctrl_mepc;
+	assign trap_taken_mcause        = trap_ctrl_mcause;
+	assign trap_taken_mtval         = trap_ctrl_mtval;
+
+	assign trap_vector_target      = trap_taken_to_s_mode ? stvec_reg : mtvec_reg;
+	assign csr_read_data           = read_csr(csr_addr_canonical);
+	assign csr_rdata               = csr_read_data;
+	assign mip_value               = compose_mip();
+	assign sip_value               = mip_value & S_INTERRUPT_MASK;
+	assign sie_value               = mie_reg & S_INTERRUPT_MASK;
+	assign csr_mstatus             = mstatus_reg;
+	assign csr_sstatus             = sstatus_reg;
+	assign csr_medeleg             = medeleg_reg;
+	assign csr_mideleg             = mideleg_reg;
+	assign csr_mie                 = mie_reg;
+	assign csr_sie                 = sie_value;
+	assign csr_mip                 = mip_value;
+	assign csr_sip                 = sip_value;
+	assign csr_mtvec               = mtvec_reg;
+	assign csr_stvec               = stvec_reg;
+	assign csr_mepc                = mepc_reg;
+	assign csr_sepc                = sepc_reg;
+	assign csr_satp                = satp_reg;
+	assign csr_frm                 = frm_reg;
+	assign csr_priv_mode           = priv_mode_q;
+	assign csr_trap_vector         = trap_vector_q;
+	assign csr_trap_to_s_mode      = trap_to_s_q;
+	assign csr_trap_vector_next    = trap_vector_target;
+	assign csr_trap_to_s_mode_next = trap_taken_to_s_mode;
+	assign csr_write_illegal       = csr_req && csr_is_read_only_by_encoding(csr_addr) &&
+	                                 (is_write_cmd || ((is_set_cmd || is_clear_cmd) && has_non_zero_data));
+	assign csr_privilege_illegal   = csr_req && csr_addr_supported &&
+	                                 !csr_privilege_allows_access(csr_addr, csr_addr_canonical);
+	assign csr_stateen_illegal     = csr_req && csr_access_blocked_by_stateen(csr_addr, csr_addr_canonical);
+	assign csr_illegal_access      = csr_req && (!csr_addr_supported || csr_privilege_illegal ||
+	                                             csr_write_illegal || csr_stateen_illegal);
+
+	for (genvar pmp_entry_idx = 0; pmp_entry_idx < PMP_ENTRY_COUNT; pmp_entry_idx++) begin : gen_pmp_outputs
+		assign csr_pmpcfg_entries[pmp_entry_idx] = pmpcfg_entries[pmp_entry_idx];
+		assign csr_pmpaddr_entries[pmp_entry_idx] = pmpaddr_entries[pmp_entry_idx];
+	end
+
+	// Main CSR state machine: counts cycles, processes CSR writes, and reacts to traps/returns.
+	always_ff @(posedge clk) begin
+		if (rst) begin
+			mstatus_reg      <= reset_mstatus_value();
+			medeleg_reg      <= legalize_medeleg('0);
+			mideleg_reg      <= '0;
+			mie_reg          <= '0;
+			mtvec_reg        <= '0;
+			mcounteren_reg   <= '0;
+			mscratch_reg     <= '0;
+			mepc_reg         <= '0;
+			mcause_reg       <= '0;
+			mtval_reg        <= '0;
+			mip_reg          <= '0;
+			mtinst_reg       <= '0;
+			mtval2_reg       <= '0;
+			menvcfg_reg      <= '0;
+			for (int unsigned stateen_idx = 0; stateen_idx < 4; stateen_idx++) begin
+				mstateen_reg[stateen_idx] <= '0;
+				sstateen_reg[stateen_idx] <= '0;
+				hstateen_reg[stateen_idx] <= '0;
+			end
+			for (int unsigned pmp_entry_idx = 0; pmp_entry_idx < PMP_ENTRY_COUNT; pmp_entry_idx++) begin
+				pmpcfg_entries[pmp_entry_idx] <= '0;
+				pmpaddr_entries[pmp_entry_idx] <= '0;
+			end
+			sstatus_reg      <= reset_sstatus_value();
+			stvec_reg        <= '0;
+			scounteren_reg   <= '0;
+			senvcfg_reg      <= '0;
+			sscratch_reg     <= '0;
+			sepc_reg         <= '0;
+			scause_reg       <= '0;
+			stval_reg        <= '0;
+			satp_reg         <= '0;
+			hstatus_reg      <= '0;
+			hedeleg_reg      <= '0;
+			hideleg_reg      <= '0;
+			hie_reg          <= '0;
+			htimedelta_reg   <= '0;
+			hcounteren_reg   <= '0;
+			hgeie_reg        <= '0;
+			henvcfg_reg      <= '0;
+			htval_reg        <= '0;
+			hip_reg          <= '0;
+			hvip_reg         <= '0;
+			htinst_reg       <= '0;
+			hgatp_reg        <= '0;
+			vsstatus_reg     <= reset_sstatus_value();
+			vsie_reg         <= '0;
+			vstvec_reg       <= '0;
+			vsscratch_reg    <= '0;
+			vsepc_reg        <= '0;
+			vscause_reg      <= '0;
+			vstval_reg       <= '0;
+			vsip_reg         <= '0;
+			vsatp_reg        <= '0;
+			mcycle_reg         <= '0;
+			minstret_reg       <= '0;
+			time_reg           <= '0;
+			mcountinhibit_reg  <= '0;
+			for (int unsigned hpm_idx = 0; hpm_idx < HPM_COUNTER_COUNT; hpm_idx++) begin
+				mhpmcounter_reg[hpm_idx] <= '0;
+			end
+			fflags_reg       <= '0;
+			frm_reg          <= '0;
+			priv_mode_q      <= PRIV_MODE_M;
+			trap_vector_q    <= '0;
+			trap_to_s_q      <= 1'b0;
+		end else begin
+			if (!mcountinhibit_reg[MCOUNTINHIBIT_CY_BIT]) begin
+				mcycle_reg <= mcycle_reg + 64'd1;
+			end
+			time_reg <= time_reg + 64'd1;
+			if (!trap_taken && !mcountinhibit_reg[MCOUNTINHIBIT_IR_BIT]) begin
+				minstret_reg <= minstret_reg + 64'd1;
+			end
+
+			if (csr_write_en && csr_is_writable(csr_addr_canonical)) begin
+				if (csr_is_pmpcfg(csr_addr_canonical)) begin
+					write_pmpcfg_csr(csr_addr_canonical, csr_write_next(read_csr(csr_addr_canonical), csr_rs1_data, csr_op));
+				end else if (csr_is_pmpaddr(csr_addr_canonical)) begin
+					write_pmpaddr_csr(csr_addr_canonical, csr_write_next(read_csr(csr_addr_canonical), csr_rs1_data, csr_op));
+				end else if (csr_is_mhpmcounter(csr_addr_canonical)) begin
+					mhpmcounter_reg[hpmcounter_index(csr_addr_canonical)] <=
+						csr_write_next(read_csr(csr_addr_canonical), csr_rs1_data, csr_op);
+				end else begin
+					unique case (csr_addr_canonical)
+						CSR_MSTATUS: begin
+							logic [XLEN-1:0] next_data;
+							logic [XLEN-1:0] legal_data;
+							next_data    = csr_write_next(mstatus_reg, csr_rs1_data, csr_op);
+							legal_data   = legalize_mstatus(next_data);
+							mstatus_reg  <= legal_data;
+							sstatus_reg  <= mirror_mstatus_to_sstatus(sstatus_reg, legal_data);
+						end
+						CSR_MEDELEG:    medeleg_reg    <= legalize_medeleg(csr_write_next(medeleg_reg, csr_rs1_data, csr_op));
+						CSR_MIDELEG:    mideleg_reg    <= legalize_mideleg(csr_write_next(mideleg_reg, csr_rs1_data, csr_op));
+						CSR_MIE:        mie_reg        <= legalize_interrupt_csr(csr_write_next(mie_reg, csr_rs1_data, csr_op));
+						CSR_MTVEC:      mtvec_reg      <= csr_write_next(mtvec_reg, csr_rs1_data, csr_op);
+						CSR_MSCRATCH:   mscratch_reg   <= csr_write_next(mscratch_reg, csr_rs1_data, csr_op);
+						CSR_MEPC:       mepc_reg       <= csr_write_next(mepc_reg, csr_rs1_data, csr_op);
+						CSR_MCAUSE:     mcause_reg     <= csr_write_next(mcause_reg, csr_rs1_data, csr_op);
+						CSR_MTVAL:      mtval_reg      <= csr_write_next(mtval_reg, csr_rs1_data, csr_op);
+						CSR_MIP:        mip_reg        <= legalize_interrupt_csr(csr_write_next(mip_reg, csr_rs1_data, csr_op));
+						CSR_MTINST:     mtinst_reg     <= '0;
+						CSR_MTVAL2:     mtval2_reg     <= csr_write_next(mtval2_reg, csr_rs1_data, csr_op);
+						CSR_MCOUNTEREN: mcounteren_reg <= legalize_counteren(csr_write_next(mcounteren_reg, csr_rs1_data, csr_op));
+						CSR_MCOUNTINHIBIT: mcountinhibit_reg <= legalize_mcountinhibit(csr_write_next(mcountinhibit_reg, csr_rs1_data, csr_op));
+						CSR_MENVCFG:    menvcfg_reg    <= legalize_menvcfg(csr_write_next(menvcfg_reg, csr_rs1_data, csr_op));
+						CSR_MSTATEEN0:  mstateen_reg[0] <= legalize_mstateen(0, csr_write_next(mstateen_reg[0], csr_rs1_data, csr_op));
+						CSR_MSTATEEN1:  mstateen_reg[1] <= legalize_mstateen(1, csr_write_next(mstateen_reg[1], csr_rs1_data, csr_op));
+						CSR_MSTATEEN2:  mstateen_reg[2] <= legalize_mstateen(2, csr_write_next(mstateen_reg[2], csr_rs1_data, csr_op));
+						CSR_MSTATEEN3:  mstateen_reg[3] <= legalize_mstateen(3, csr_write_next(mstateen_reg[3], csr_rs1_data, csr_op));
+						CSR_SSTATUS: begin
+							logic [XLEN-1:0] next_sstatus;
+							next_sstatus = csr_write_next(sstatus_reg, csr_rs1_data, csr_op);
+							sstatus_reg  <= legalize_sstatus(next_sstatus);
+							mstatus_reg  <= apply_sstatus_to_mstatus(mstatus_reg, next_sstatus);
+						end
+						CSR_SIE:        mie_reg        <= (mie_reg & ~S_INTERRUPT_MASK) |
+						                                  (legalize_interrupt_csr(csr_write_next(sie_value, csr_rs1_data, csr_op)) &
+						                                   S_INTERRUPT_MASK);
+						CSR_STVEC:      stvec_reg      <= csr_write_next(stvec_reg, csr_rs1_data, csr_op);
+						CSR_SCOUNTEREN: scounteren_reg <= legalize_counteren(csr_write_next(scounteren_reg, csr_rs1_data, csr_op));
+						CSR_SENVCFG:    senvcfg_reg    <= legalize_senvcfg(csr_write_next(senvcfg_reg, csr_rs1_data, csr_op));
+						CSR_SSTATEEN0:  sstateen_reg[0] <= legalize_sstateen(0, csr_write_next(sstateen_reg[0], csr_rs1_data, csr_op));
+						CSR_SSTATEEN1:  sstateen_reg[1] <= legalize_sstateen(1, csr_write_next(sstateen_reg[1], csr_rs1_data, csr_op));
+						CSR_SSTATEEN2:  sstateen_reg[2] <= legalize_sstateen(2, csr_write_next(sstateen_reg[2], csr_rs1_data, csr_op));
+						CSR_SSTATEEN3:  sstateen_reg[3] <= legalize_sstateen(3, csr_write_next(sstateen_reg[3], csr_rs1_data, csr_op));
+						CSR_SSCRATCH:   sscratch_reg   <= csr_write_next(sscratch_reg, csr_rs1_data, csr_op);
+						CSR_SEPC:       sepc_reg       <= csr_write_next(sepc_reg, csr_rs1_data, csr_op);
+						CSR_SCAUSE:     scause_reg     <= csr_write_next(scause_reg, csr_rs1_data, csr_op);
+						CSR_STVAL:      stval_reg      <= csr_write_next(stval_reg, csr_rs1_data, csr_op);
+						CSR_SIP:        mip_reg        <= (mip_reg & ~S_INTERRUPT_MASK) |
+						                                  (legalize_interrupt_csr(csr_write_next(sip_value, csr_rs1_data, csr_op)) &
+						                                   S_INTERRUPT_MASK);
+						CSR_SATP: begin
+							logic [XLEN-1:0] next_satp;
+							next_satp = csr_write_next(satp_reg, csr_rs1_data, csr_op);
+							satp_reg  <= legalize_satp(next_satp, satp_reg);
+						end
+						CSR_HSTATUS:    hstatus_reg    <= legalize_hstatus(csr_write_next(hstatus_reg, csr_rs1_data, csr_op));
+						CSR_HEDELEG:    hedeleg_reg    <= legalize_medeleg(csr_write_next(hedeleg_reg, csr_rs1_data, csr_op));
+						CSR_HIDELEG:    hideleg_reg    <= legalize_mideleg(csr_write_next(hideleg_reg, csr_rs1_data, csr_op));
+						CSR_HIE:        hie_reg        <= '0;
+						CSR_HTIMEDELTA: htimedelta_reg <= csr_write_next(htimedelta_reg, csr_rs1_data, csr_op);
+						CSR_HCOUNTEREN: hcounteren_reg <= legalize_counteren(csr_write_next(hcounteren_reg, csr_rs1_data, csr_op));
+						CSR_HGEIE:      hgeie_reg      <= '0;
+						CSR_HENVCFG:    henvcfg_reg    <= legalize_menvcfg(csr_write_next(henvcfg_reg, csr_rs1_data, csr_op));
+						CSR_HSTATEEN0:  hstateen_reg[0] <= legalize_mstateen(0, csr_write_next(hstateen_reg[0], csr_rs1_data, csr_op));
+						CSR_HSTATEEN1:  hstateen_reg[1] <= legalize_mstateen(1, csr_write_next(hstateen_reg[1], csr_rs1_data, csr_op));
+						CSR_HSTATEEN2:  hstateen_reg[2] <= legalize_mstateen(2, csr_write_next(hstateen_reg[2], csr_rs1_data, csr_op));
+						CSR_HSTATEEN3:  hstateen_reg[3] <= legalize_mstateen(3, csr_write_next(hstateen_reg[3], csr_rs1_data, csr_op));
+						CSR_HTVAL:      htval_reg      <= csr_write_next(htval_reg, csr_rs1_data, csr_op);
+						CSR_HIP:        hip_reg        <= '0;
+						CSR_HVIP:       hvip_reg       <= '0;
+						CSR_HTINST:     htinst_reg     <= '0;
+						CSR_HGATP:      hgatp_reg      <= legalize_hgatp(csr_write_next(hgatp_reg, csr_rs1_data, csr_op), hgatp_reg);
+						CSR_VSSTATUS:   vsstatus_reg   <= legalize_sstatus(csr_write_next(vsstatus_reg, csr_rs1_data, csr_op));
+						CSR_VSIE:       vsie_reg       <= '0;
+						CSR_VSTVEC:     vstvec_reg     <= csr_write_next(vstvec_reg, csr_rs1_data, csr_op);
+						CSR_VSSCRATCH:  vsscratch_reg  <= csr_write_next(vsscratch_reg, csr_rs1_data, csr_op);
+						CSR_VSEPC:      vsepc_reg      <= csr_write_next(vsepc_reg, csr_rs1_data, csr_op);
+						CSR_VSCAUSE:    vscause_reg    <= csr_write_next(vscause_reg, csr_rs1_data, csr_op);
+						CSR_VSTVAL:     vstval_reg     <= csr_write_next(vstval_reg, csr_rs1_data, csr_op);
+						CSR_VSIP:       vsip_reg       <= '0;
+						CSR_VSATP: begin
+							logic [XLEN-1:0] next_vsatp;
+							next_vsatp = csr_write_next(vsatp_reg, csr_rs1_data, csr_op);
+							vsatp_reg  <= legalize_satp(next_vsatp, vsatp_reg);
+						end
+						CSR_MCYCLE:     mcycle_reg     <= csr_write_next(mcycle_reg, csr_rs1_data, csr_op);
+						CSR_MINSTRET:   minstret_reg   <= csr_write_next(minstret_reg, csr_rs1_data, csr_op);
+						CSR_FFLAGS: begin
+							logic [XLEN-1:0] next_flags;
+							next_flags  = csr_write_next({{(XLEN-5){1'b0}}, fflags_reg}, csr_rs1_data, csr_op);
+							fflags_reg  <= next_flags[4:0];
+							mstatus_reg <= set_mstatus_fs_dirty(mstatus_reg);
+							sstatus_reg <= set_sstatus_fs_dirty(sstatus_reg);
+						end
+						CSR_FRM: begin
+							logic [XLEN-1:0] next_frm;
+							next_frm    = csr_write_next({{(XLEN-3){1'b0}}, frm_reg}, csr_rs1_data, csr_op);
+							frm_reg     <= next_frm[2:0];
+							mstatus_reg <= set_mstatus_fs_dirty(mstatus_reg);
+							sstatus_reg <= set_sstatus_fs_dirty(sstatus_reg);
+						end
+						CSR_FCSR: begin
+							logic [XLEN-1:0] next_fcsr;
+							next_fcsr   = csr_write_next(compose_fcsr(), csr_rs1_data, csr_op);
+							fflags_reg  <= next_fcsr[4:0];
+							frm_reg     <= next_fcsr[7:5];
+							mstatus_reg <= set_mstatus_fs_dirty(mstatus_reg);
+							sstatus_reg <= set_sstatus_fs_dirty(sstatus_reg);
+						end
+						default: ;
+					endcase
+				end
+			end
+
+			if (fp_state_dirty) begin
+				mstatus_reg <= set_mstatus_fs_dirty(mstatus_reg);
+				sstatus_reg <= set_sstatus_fs_dirty(sstatus_reg);
+			end
+
+			if (fp_fflags_valid) begin
+				fflags_reg <= fflags_reg | fp_fflags;
+			end
+
+			if (trap_taken) begin
+				trap_vector_q <= trap_vector_target;
+				trap_to_s_q   <= trap_taken_to_s_mode;
+				// Update privilege-specific CSRs based on whether the trap was delegated.
+				if (trap_taken_to_s_mode) begin
+					logic [XLEN-1:0] next_sstatus;
+					next_sstatus = sstatus_reg;
+					next_sstatus[SSTATUS_SPIE_BIT] = sstatus_reg[SSTATUS_SIE_BIT];
+					next_sstatus[SSTATUS_SIE_BIT]  = 1'b0;
+					next_sstatus[SSTATUS_SPP_BIT]  = (priv_mode_q == PRIV_MODE_S);
+					sstatus_reg <= next_sstatus;
+					mstatus_reg <= apply_sstatus_to_mstatus(mstatus_reg, next_sstatus);
+					sepc_reg    <= trap_taken_mepc;
+					scause_reg  <= trap_taken_mcause;
+					stval_reg   <= trap_taken_mtval;
+					priv_mode_q <= PRIV_MODE_S;
+				end else begin
+					logic [XLEN-1:0] next_mstatus;
+					next_mstatus = mstatus_reg;
+					next_mstatus[MSTATUS_MPIE_BIT]              = mstatus_reg[MSTATUS_MIE_BIT];
+					next_mstatus[MSTATUS_MIE_BIT]               = 1'b0;
+					next_mstatus[MSTATUS_MPP_HI:MSTATUS_MPP_LO] = encode_priv_mode(priv_mode_q);
+					mstatus_reg <= legalize_mstatus(next_mstatus);
+					sstatus_reg <= mirror_mstatus_to_sstatus(sstatus_reg, legalize_mstatus(next_mstatus));
+					mepc_reg    <= trap_taken_mepc;
+					mcause_reg  <= trap_taken_mcause;
+					mtval_reg   <= trap_taken_mtval;
+					priv_mode_q <= PRIV_MODE_M;
+				end
+			end else begin
+				if (trap_ctrl_mret_en) begin
+					priv_mode_t return_mode;
+					logic [XLEN-1:0] next_mstatus;
+					return_mode  = decode_priv_mode(mstatus_reg[MSTATUS_MPP_HI:MSTATUS_MPP_LO]);
+					next_mstatus = mstatus_reg;
+					next_mstatus[MSTATUS_MIE_BIT]               = mstatus_reg[MSTATUS_MPIE_BIT];
+					next_mstatus[MSTATUS_MPIE_BIT]              = 1'b1;
+					next_mstatus[MSTATUS_MPP_HI:MSTATUS_MPP_LO] = encode_priv_mode(PRIV_MODE_U);
+					if (return_mode != PRIV_MODE_M) begin
+						next_mstatus[MSTATUS_MPRV_BIT] = 1'b0;
+					end
+					mstatus_reg <= legalize_mstatus(next_mstatus);
+					sstatus_reg <= mirror_mstatus_to_sstatus(sstatus_reg, legalize_mstatus(next_mstatus));
+					priv_mode_q <= return_mode;
+				end
+
+				if (trap_ctrl_sret_en) begin
+					logic [XLEN-1:0] next_sstatus;
+					logic [XLEN-1:0] next_mstatus;
+					next_sstatus = sstatus_reg;
+					next_sstatus[SSTATUS_SIE_BIT]  = sstatus_reg[SSTATUS_SPIE_BIT];
+					next_sstatus[SSTATUS_SPIE_BIT] = 1'b1;
+					next_sstatus[SSTATUS_SPP_BIT]  = 1'b0;
+					next_mstatus = apply_sstatus_to_mstatus(mstatus_reg, next_sstatus);
+					next_mstatus[MSTATUS_MPRV_BIT] = 1'b0;
+					sstatus_reg <= legalize_sstatus(next_sstatus);
+					mstatus_reg <= legalize_mstatus(next_mstatus);
+					priv_mode_q <= (sstatus_reg[SSTATUS_SPP_BIT]) ? PRIV_MODE_S : PRIV_MODE_U;
+				end
+			end
+		end
+	end
+
+	// Map CSR aliases (cycle/instret) to their canonical machine addresses.
+	function automatic logic [11:0] canonical_addr(input logic [11:0] addr);
+		case (addr)
+			CSR_CYCLE:   canonical_addr = CSR_MCYCLE;
+			CSR_INSTRET: canonical_addr = CSR_MINSTRET;
+			default:     canonical_addr = addr;
+		endcase
+	endfunction
+
+	function automatic bit csr_is_pmpcfg(input logic [11:0] addr);
+		csr_is_pmpcfg = (addr >= CSR_PMPCFG0) && (addr <= CSR_PMPCFG15) && (addr[0] == 1'b0);
+	endfunction
+
+	function automatic bit csr_is_pmpaddr(input logic [11:0] addr);
+		csr_is_pmpaddr = (addr >= CSR_PMPADDR0) && (addr <= CSR_PMPADDR63);
+	endfunction
+
+	function automatic bit csr_is_mhpmcounter(input logic [11:0] addr);
+		csr_is_mhpmcounter = (addr >= CSR_MHPMCOUNTER3) && (addr <= CSR_MHPMCOUNTER31);
+	endfunction
+
+	function automatic bit csr_is_hpmcounter(input logic [11:0] addr);
+		csr_is_hpmcounter = (addr >= CSR_HPMCOUNTER3) && (addr <= CSR_HPMCOUNTER31);
+	endfunction
+
+	function automatic bit csr_is_mhpmevent(input logic [11:0] addr);
+		csr_is_mhpmevent = (addr >= CSR_MHPMEVENT3) && (addr <= CSR_MHPMEVENT31);
+	endfunction
+
+	function automatic bit csr_is_counter(input logic [11:0] addr);
+		return (addr == CSR_CYCLE) || (addr == CSR_TIME) || (addr == CSR_INSTRET) || csr_is_hpmcounter(addr);
+	endfunction
+
+	function automatic int unsigned counter_index(input logic [11:0] addr);
+		if (csr_is_hpmcounter(addr)) begin
+			return int'(addr) - int'(CSR_CYCLE);
+		end
+		case (addr)
+			CSR_CYCLE:   return COUNTEREN_CY_BIT;
+			CSR_TIME:    return COUNTEREN_TM_BIT;
+			CSR_INSTRET: return COUNTEREN_IR_BIT;
+			default:     return COUNTEREN_CY_BIT;
+		endcase
+	endfunction
+
+	function automatic int unsigned hpmcounter_index(input logic [11:0] addr);
+		if (csr_is_hpmcounter(addr)) begin
+			return int'(addr) - int'(CSR_HPMCOUNTER3);
+		end
+		return int'(addr) - int'(CSR_MHPMCOUNTER3);
+	endfunction
+
+	function automatic int unsigned pmpcfg_csr_base_entry(input logic [11:0] addr);
+		return (int'(addr) - int'(CSR_PMPCFG0)) * 4;
+	endfunction
+
+	function automatic int unsigned pmpaddr_csr_entry(input logic [11:0] addr);
+		return int'(addr) - int'(CSR_PMPADDR0);
+	endfunction
+
+	function automatic logic [XLEN-1:0] pmpaddr_writable_mask();
+		logic [XLEN-1:0] mask;
+		mask = '0;
+		for (int bit_idx = 0; bit_idx < (PHYS_ADDR_WIDTH - 2); bit_idx++) begin
+			mask[bit_idx] = 1'b1;
+		end
+		return mask;
+	endfunction
+
+	function automatic bit pmpcfg_byte_is_legal(input pmpcfg_entry_t cfg);
+		logic [1:0] address_matching;
+		address_matching = cfg[PMPCFG_A_HI:PMPCFG_A_LO];
+		if (!cfg[PMPCFG_R_BIT] && cfg[PMPCFG_W_BIT]) begin
+			return 1'b0;
+		end
+		if ((PMP_GRANULARITY >= 2) && (address_matching == PMP_MODE_NA4)) begin
+			return 1'b0;
+		end
+		return 1'b1;
+	endfunction
+
+	function automatic pmpcfg_entry_t legalize_pmpcfg_byte(input pmpcfg_entry_t cfg);
+		pmpcfg_entry_t legal_cfg;
+		legal_cfg = cfg;
+		legal_cfg[6:5] = 2'b00;
+		return legal_cfg;
+	endfunction
+
+	function automatic bit pmpaddr_is_locked(input int unsigned entry_idx);
+		bit locked;
+		locked = pmpcfg_entries[entry_idx][PMPCFG_L_BIT];
+		if (entry_idx < (PMP_ENTRY_COUNT - 1)) begin
+			locked |= pmpcfg_entries[entry_idx + 1][PMPCFG_L_BIT] &&
+			          (pmpcfg_entries[entry_idx + 1][PMPCFG_A_HI:PMPCFG_A_LO] == PMP_MODE_TOR);
+		end
+		return locked;
+	endfunction
+
+	function automatic logic [XLEN-1:0] read_pmpcfg_csr(input logic [11:0] addr);
+		logic [XLEN-1:0] data;
+		int unsigned base_entry;
+		data = '0;
+		base_entry = pmpcfg_csr_base_entry(addr);
+		for (int byte_idx = 0; byte_idx < PMP_CFG_ENTRIES_PER_RV64_CSR; byte_idx++) begin
+			if ((base_entry + byte_idx) < PMP_ENTRY_COUNT) begin
+				data[(byte_idx * 8) +: 8] = pmpcfg_entries[base_entry + byte_idx];
+			end
+		end
+		return data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] read_pmpaddr_csr(input logic [11:0] addr);
+		return pmpaddr_entries[pmpaddr_csr_entry(addr)] & pmpaddr_writable_mask();
+	endfunction
+
+	task automatic write_pmpcfg_csr(input logic [11:0] addr, input logic [XLEN-1:0] write_data);
+		int unsigned base_entry;
+		pmpcfg_entry_t next_cfg;
+		base_entry = pmpcfg_csr_base_entry(addr);
+		for (int byte_idx = 0; byte_idx < PMP_CFG_ENTRIES_PER_RV64_CSR; byte_idx++) begin
+			if ((base_entry + byte_idx) < PMP_ENTRY_COUNT) begin
+				next_cfg = legalize_pmpcfg_byte(write_data[(byte_idx * 8) +: 8]);
+				if (!pmpcfg_entries[base_entry + byte_idx][PMPCFG_L_BIT] && pmpcfg_byte_is_legal(next_cfg)) begin
+					pmpcfg_entries[base_entry + byte_idx] <= next_cfg;
+				end
+			end
+		end
+	endtask
+
+	task automatic write_pmpaddr_csr(input logic [11:0] addr, input logic [XLEN-1:0] write_data);
+		int unsigned entry_idx;
+		entry_idx = pmpaddr_csr_entry(addr);
+		if (!pmpaddr_is_locked(entry_idx)) begin
+			pmpaddr_entries[entry_idx] <= write_data & pmpaddr_writable_mask();
+		end
+	endtask
+
+	// Apply the CSR op semantics (write/set/clear) for a single register.
+	function automatic logic [XLEN-1:0] csr_write_next(
+		input logic [XLEN-1:0] current_data,
+		input logic [XLEN-1:0] write_data,
+		input logic [2:0]      op_code
+	);
+		logic [XLEN-1:0] next_data;
+		unique case (op_code)
+			3'b001, 3'b101: next_data = write_data;                     // CSRRW / CSRRWI
+			3'b010, 3'b110: next_data = current_data | write_data;      // CSRRS / CSRRSI
+			3'b011, 3'b111: next_data = current_data & ~write_data;     // CSRRC / CSRRCI
+			default:         next_data = current_data;
+		endcase
+		return next_data;
+	endfunction
+
+	function automatic bit csr_is_writable(input logic [11:0] addr);
+		case (addr)
+			CSR_MVENDORID,
+			CSR_MARCHID,
+			CSR_MIMPID,
+			CSR_MHARTID,
+			CSR_MCONFIGPTR,
+			CSR_TIME: csr_is_writable = 1'b0;
+			default:  csr_is_writable = 1'b1;
+		endcase
+	endfunction
+
+	function automatic bit csr_is_read_only_by_encoding(input logic [11:0] addr);
+		return addr[11:10] == 2'b11;
+	endfunction
+
+	function automatic bit csr_is_supported(input logic [11:0] addr);
+		bit supported;
+		if (csr_is_pmpcfg(addr) || csr_is_pmpaddr(addr) ||
+		    csr_is_mhpmcounter(addr) || csr_is_hpmcounter(addr) || csr_is_mhpmevent(addr)) begin
+			supported = 1'b1;
+		end else begin
+			unique case (addr)
+				CSR_MSTATUS,
+				CSR_MISA,
+				CSR_MEDELEG,
+				CSR_MIDELEG,
+				CSR_MIE,
+				CSR_MTVEC,
+				CSR_MSCRATCH,
+				CSR_MEPC,
+				CSR_MCAUSE,
+				CSR_MTVAL,
+				CSR_MIP,
+				CSR_MTINST,
+				CSR_MTVAL2,
+				CSR_MCOUNTEREN,
+				CSR_MCOUNTINHIBIT,
+				CSR_MENVCFG,
+				CSR_MSTATEEN0,
+				CSR_MSTATEEN1,
+				CSR_MSTATEEN2,
+				CSR_MSTATEEN3,
+				CSR_MVENDORID,
+				CSR_MARCHID,
+				CSR_MIMPID,
+				CSR_MHARTID,
+				CSR_MCONFIGPTR,
+				CSR_SSTATUS,
+				CSR_SIE,
+				CSR_STVEC,
+				CSR_SCOUNTEREN,
+				CSR_SENVCFG,
+				CSR_SSTATEEN0,
+				CSR_SSTATEEN1,
+				CSR_SSTATEEN2,
+				CSR_SSTATEEN3,
+				CSR_SSCRATCH,
+				CSR_SEPC,
+				CSR_SCAUSE,
+				CSR_STVAL,
+				CSR_SIP,
+				CSR_SATP,
+				CSR_HSTATUS,
+				CSR_HEDELEG,
+				CSR_HIDELEG,
+				CSR_HIE,
+				CSR_HTIMEDELTA,
+				CSR_HCOUNTEREN,
+				CSR_HGEIE,
+				CSR_HENVCFG,
+				CSR_HSTATEEN0,
+				CSR_HSTATEEN1,
+				CSR_HSTATEEN2,
+				CSR_HSTATEEN3,
+				CSR_HTVAL,
+				CSR_HIP,
+				CSR_HVIP,
+				CSR_HTINST,
+				CSR_HGATP,
+				CSR_HGEIP,
+				CSR_VSSTATUS,
+				CSR_VSIE,
+				CSR_VSTVEC,
+				CSR_VSSCRATCH,
+				CSR_VSEPC,
+				CSR_VSCAUSE,
+				CSR_VSTVAL,
+				CSR_VSIP,
+				CSR_VSATP,
+				CSR_MCYCLE,
+				CSR_MINSTRET,
+				CSR_CYCLE,
+				CSR_INSTRET,
+				CSR_TIME,
+				CSR_FFLAGS,
+				CSR_FRM,
+				CSR_FCSR:
+					supported = 1'b1;
+				default:
+					supported = 1'b0;
+			endcase
+		end
+		return supported;
+	endfunction
+
+	function automatic bit csr_privilege_allows_access(
+		input logic [11:0] requested_addr,
+		input logic [11:0] canonicalized_addr
+	);
+		logic [1:0] minimum_privilege;
+		minimum_privilege = requested_addr[9:8];
+		if (!csr_is_supported(canonicalized_addr)) begin
+			return 1'b0;
+		end
+		if (minimum_privilege == 2'b10) begin
+			return 1'b0;
+		end
+		return encode_priv_mode(priv_mode_q) >= minimum_privilege;
+	endfunction
+
+	function automatic bit csr_access_blocked_by_stateen(
+		input logic [11:0] requested_addr,
+		input logic [11:0] canonicalized_addr
+	);
+		int unsigned current_counter_index;
+		if (csr_is_counter(requested_addr) && (priv_mode_q != PRIV_MODE_M)) begin
+			current_counter_index = counter_index(requested_addr);
+			if (!mcounteren_reg[current_counter_index]) begin
+				return 1'b1;
+			end
+			if ((priv_mode_q == PRIV_MODE_U) && !scounteren_reg[current_counter_index]) begin
+				return 1'b1;
+			end
+		end
+		if ((canonicalized_addr == CSR_SENVCFG) && (priv_mode_q == PRIV_MODE_S)) begin
+			return !mstateen_reg[0][STATEEN0_ENVCFG_BIT];
+		end
+		return 1'b0;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_mstatus(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = data;
+		if (legal_data[MSTATUS_MPP_HI:MSTATUS_MPP_LO] == 2'b10) begin
+			legal_data[MSTATUS_MPP_HI:MSTATUS_MPP_LO] = encode_priv_mode(PRIV_MODE_U);
+		end
+		legal_data[MSTATUS_XS_HI:MSTATUS_XS_LO] = 2'b00;
+		legal_data[MSTATUS_UXL_HI:MSTATUS_UXL_LO] = STATUS_XLEN_64;
+		legal_data[MSTATUS_SXL_HI:MSTATUS_SXL_LO] = STATUS_XLEN_64;
+		legal_data[MSTATUS_SD_BIT] = (legal_data[MSTATUS_FS_HI:MSTATUS_FS_LO] == 2'b11) ||
+		                             (legal_data[MSTATUS_XS_HI:MSTATUS_XS_LO] == 2'b11) ||
+		                             (legal_data[MSTATUS_VS_HI:MSTATUS_VS_LO] == 2'b11);
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_sstatus(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[SSTATUS_SIE_BIT]                  = data[SSTATUS_SIE_BIT];
+		legal_data[SSTATUS_SPIE_BIT]                 = data[SSTATUS_SPIE_BIT];
+		legal_data[SSTATUS_SPP_BIT]                  = data[SSTATUS_SPP_BIT];
+		legal_data[SSTATUS_VS_HI:SSTATUS_VS_LO]      = data[SSTATUS_VS_HI:SSTATUS_VS_LO];
+		legal_data[SSTATUS_FS_HI:SSTATUS_FS_LO]      = data[SSTATUS_FS_HI:SSTATUS_FS_LO];
+		legal_data[SSTATUS_SUM_BIT]                  = data[SSTATUS_SUM_BIT];
+		legal_data[SSTATUS_MXR_BIT]                  = data[SSTATUS_MXR_BIT];
+		legal_data[SSTATUS_UXL_HI:SSTATUS_UXL_LO]    = STATUS_XLEN_64;
+		legal_data[SSTATUS_SD_BIT]                   = (legal_data[SSTATUS_FS_HI:SSTATUS_FS_LO] == 2'b11) ||
+		                                               (legal_data[SSTATUS_XS_HI:SSTATUS_XS_LO] == 2'b11) ||
+		                                               (legal_data[SSTATUS_VS_HI:SSTATUS_VS_LO] == 2'b11);
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] reset_mstatus_value();
+		return legalize_mstatus('0);
+	endfunction
+
+	function automatic logic [XLEN-1:0] reset_sstatus_value();
+		return legalize_sstatus('0);
+	endfunction
+
+	function automatic logic [XLEN-1:0] set_mstatus_fs_dirty(input logic [XLEN-1:0] current);
+		logic [XLEN-1:0] next_data;
+		next_data = current;
+		next_data[MSTATUS_FS_HI:MSTATUS_FS_LO] = 2'b11;
+		return legalize_mstatus(next_data);
+	endfunction
+
+	function automatic logic [XLEN-1:0] set_sstatus_fs_dirty(input logic [XLEN-1:0] current);
+		logic [XLEN-1:0] next_data;
+		next_data = current;
+		next_data[SSTATUS_FS_HI:SSTATUS_FS_LO] = 2'b11;
+		return legalize_sstatus(next_data);
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_medeleg(input logic [XLEN-1:0] data);
+		return (data & MEDELEG_WRITABLE_MASK) | MEDELEG_READ_ONLY_ONE_MASK;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_mideleg(input logic [XLEN-1:0] data);
+		return data & MIDELEG_WRITABLE_MASK;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_interrupt_csr(input logic [XLEN-1:0] data);
+		return data & INTERRUPT_MASK;
+	endfunction
+
+	function automatic logic [XLEN-1:0] compose_mip();
+		logic [XLEN-1:0] pending;
+		pending = legalize_interrupt_csr(mip_reg);
+		pending[INTERRUPT_M_SOFTWARE_BIT] |= m_software_interrupt;
+		pending[INTERRUPT_M_TIMER_BIT]    |= m_timer_interrupt;
+		pending[INTERRUPT_M_EXTERNAL_BIT] |= m_external_interrupt;
+		pending[INTERRUPT_S_SOFTWARE_BIT] |= s_software_interrupt;
+		pending[INTERRUPT_S_TIMER_BIT]    |= s_timer_interrupt;
+		pending[INTERRUPT_S_EXTERNAL_BIT] |= s_external_interrupt;
+		return pending;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_mcountinhibit(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[MCOUNTINHIBIT_CY_BIT] = data[MCOUNTINHIBIT_CY_BIT];
+		legal_data[MCOUNTINHIBIT_IR_BIT] = data[MCOUNTINHIBIT_IR_BIT];
+		legal_data[31:3] = data[31:3];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_counteren(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[COUNTEREN_CY_BIT] = data[COUNTEREN_CY_BIT];
+		legal_data[COUNTEREN_TM_BIT] = data[COUNTEREN_TM_BIT];
+		legal_data[COUNTEREN_IR_BIT] = data[COUNTEREN_IR_BIT];
+		legal_data[31:3] = data[31:3];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_senvcfg(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[ENVCFG_FIOM_BIT] = data[ENVCFG_FIOM_BIT];
+		legal_data[ENVCFG_CBIE_HI:ENVCFG_CBIE_LO] =
+			menvcfg_reg[ENVCFG_CBIE_HI:ENVCFG_CBIE_LO] & data[ENVCFG_CBIE_HI:ENVCFG_CBIE_LO];
+		legal_data[ENVCFG_CBCFE_BIT] = menvcfg_reg[ENVCFG_CBCFE_BIT] & data[ENVCFG_CBCFE_BIT];
+		legal_data[ENVCFG_CBZE_BIT] = menvcfg_reg[ENVCFG_CBZE_BIT] & data[ENVCFG_CBZE_BIT];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_menvcfg(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[ENVCFG_FIOM_BIT] = data[ENVCFG_FIOM_BIT];
+		legal_data[ENVCFG_CBIE_HI:ENVCFG_CBIE_LO] = data[ENVCFG_CBIE_HI:ENVCFG_CBIE_LO];
+		legal_data[ENVCFG_CBCFE_BIT] = data[ENVCFG_CBCFE_BIT];
+		legal_data[ENVCFG_CBZE_BIT] = data[ENVCFG_CBZE_BIT];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_hstatus(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[6]  = data[6];   // VTSR
+		legal_data[7]  = data[7];   // VTW
+		legal_data[8]  = data[8];   // VTVM
+		legal_data[12] = data[12];  // SPVP
+		legal_data[14] = data[14];  // SPV
+		legal_data[16:15] = data[16:15]; // GVA and VSBE are WARL here.
+		legal_data[33:32] = STATUS_XLEN_64; // VSXL
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_mstateen(input int unsigned index, input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		if (index == 0) begin
+			legal_data[STATEEN0_ENVCFG_BIT] = data[STATEEN0_ENVCFG_BIT];
+		end
+		legal_data[STATEEN_SE0_BIT] = data[STATEEN_SE0_BIT];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_sstateen(input int unsigned index, input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		if (index == 0) begin
+			legal_data[STATEEN0_ENVCFG_BIT] = data[STATEEN0_ENVCFG_BIT] & mstateen_reg[0][STATEEN0_ENVCFG_BIT];
+		end
+		legal_data[STATEEN_SE0_BIT] = data[STATEEN_SE0_BIT] & mstateen_reg[index][STATEEN_SE0_BIT];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_satp(
+		input logic [XLEN-1:0] data,
+		input logic [XLEN-1:0] current
+	);
+		logic [XLEN-1:0] legal_data;
+		logic [3:0] mode;
+		mode = data[SATP_MODE_HI:SATP_MODE_LO];
+		legal_data = current;
+
+		unique case (mode)
+			SATP_MODE_BARE: begin
+				legal_data = '0;
+			end
+			SATP_MODE_SV39: begin
+				legal_data = '0;
+				legal_data[SATP_MODE_HI:SATP_MODE_LO] = SATP_MODE_SV39;
+				legal_data[SATP_ASID_HI:SATP_ASID_LO] = data[SATP_ASID_HI:SATP_ASID_LO];
+				legal_data[SATP_PPN_HI:SATP_PPN_LO]   = data[SATP_PPN_HI:SATP_PPN_LO];
+			end
+			SATP_MODE_SV48,
+			SATP_MODE_SV57,
+			SATP_MODE_SV64: begin
+				legal_data = current;
+			end
+			default: begin
+				legal_data = current;
+			end
+		endcase
+
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_hgatp(
+		input logic [XLEN-1:0] data,
+		input logic [XLEN-1:0] current
+	);
+		logic [XLEN-1:0] legal_data;
+		logic [3:0] mode;
+		mode = data[SATP_MODE_HI:SATP_MODE_LO];
+		legal_data = current;
+
+		unique case (mode)
+			SATP_MODE_BARE: legal_data = '0;
+			SATP_MODE_SV39: legal_data = data;
+			SATP_MODE_SV48,
+			SATP_MODE_SV57,
+			SATP_MODE_SV64: legal_data = current;
+			default: legal_data = current;
+		endcase
+
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] apply_sstatus_to_mstatus(
+		input logic [XLEN-1:0] mstatus_cur,
+		input logic [XLEN-1:0] sstatus_data
+	);
+		logic [XLEN-1:0] next_data;
+		next_data = mstatus_cur;
+		next_data[SSTATUS_SIE_BIT]                  = sstatus_data[SSTATUS_SIE_BIT];
+		next_data[SSTATUS_SPIE_BIT]                 = sstatus_data[SSTATUS_SPIE_BIT];
+		next_data[SSTATUS_SPP_BIT]                  = sstatus_data[SSTATUS_SPP_BIT];
+		next_data[SSTATUS_VS_HI:SSTATUS_VS_LO]      = sstatus_data[SSTATUS_VS_HI:SSTATUS_VS_LO];
+		next_data[SSTATUS_FS_HI:SSTATUS_FS_LO]      = sstatus_data[SSTATUS_FS_HI:SSTATUS_FS_LO];
+		next_data[SSTATUS_SUM_BIT]                  = sstatus_data[SSTATUS_SUM_BIT];
+		next_data[SSTATUS_MXR_BIT]                  = sstatus_data[SSTATUS_MXR_BIT];
+		next_data[MSTATUS_UXL_HI:MSTATUS_UXL_LO]    = STATUS_XLEN_64;
+		return legalize_mstatus(next_data);
+	endfunction
+
+	function automatic logic [XLEN-1:0] mirror_mstatus_to_sstatus(
+		input logic [XLEN-1:0] sstatus_cur,
+		input logic [XLEN-1:0] mstatus_data
+	);
+		logic [XLEN-1:0] next_data;
+		next_data = legalize_sstatus(sstatus_cur);
+		next_data[SSTATUS_SIE_BIT]                  = mstatus_data[SSTATUS_SIE_BIT];
+		next_data[SSTATUS_SPIE_BIT]                 = mstatus_data[SSTATUS_SPIE_BIT];
+		next_data[SSTATUS_SPP_BIT]                  = mstatus_data[SSTATUS_SPP_BIT];
+		next_data[SSTATUS_VS_HI:SSTATUS_VS_LO]      = mstatus_data[MSTATUS_VS_HI:MSTATUS_VS_LO];
+		next_data[SSTATUS_FS_HI:SSTATUS_FS_LO]      = mstatus_data[SSTATUS_FS_HI:SSTATUS_FS_LO];
+		next_data[SSTATUS_SUM_BIT]                  = mstatus_data[SSTATUS_SUM_BIT];
+		next_data[SSTATUS_MXR_BIT]                  = mstatus_data[MSTATUS_MXR_BIT];
+		next_data[SSTATUS_UXL_HI:SSTATUS_UXL_LO]    = STATUS_XLEN_64;
+		next_data[SSTATUS_SD_BIT]                   = mstatus_data[MSTATUS_SD_BIT];
+		return legalize_sstatus(next_data);
+	endfunction
+
+	// Pack the discrete FP status bits into the architectural FCSR layout.
+	function automatic logic [XLEN-1:0] compose_fcsr();
+		logic [XLEN-1:0] value;
+		value = '0;
+		value[4:0] = fflags_reg;
+		value[7:5] = frm_reg;
+		return value;
+	endfunction
+
+	function automatic priv_mode_t decode_priv_mode(input logic [1:0] encoded);
+		case (encoded)
+			2'b00: decode_priv_mode = PRIV_MODE_U;
+			2'b01: decode_priv_mode = PRIV_MODE_S;
+			default: decode_priv_mode = PRIV_MODE_M;
+		endcase
+	endfunction
+
+	function automatic logic [1:0] encode_priv_mode(input priv_mode_t mode);
+		case (mode)
+			PRIV_MODE_U: encode_priv_mode = 2'b00;
+			PRIV_MODE_S: encode_priv_mode = 2'b01;
+			default:     encode_priv_mode = 2'b11;
+		endcase
+	endfunction
+
+	function automatic logic [XLEN-1:0] read_csr(input logic [11:0] addr);
+		logic [XLEN-1:0] data_out;
+		if (csr_is_pmpcfg(addr)) begin
+			data_out = read_pmpcfg_csr(addr);
+		end else if (csr_is_pmpaddr(addr)) begin
+			data_out = read_pmpaddr_csr(addr);
+		end else if (csr_is_mhpmcounter(addr) || csr_is_hpmcounter(addr)) begin
+			data_out = mhpmcounter_reg[hpmcounter_index(addr)];
+		end else if (csr_is_mhpmevent(addr)) begin
+			data_out = '0;
+		end else begin
+			unique case (addr)
+				CSR_FFLAGS:    data_out = {{(XLEN-5){1'b0}}, fflags_reg};
+				CSR_FRM:       data_out = {{(XLEN-3){1'b0}}, frm_reg};
+				CSR_FCSR:      data_out = compose_fcsr();
+				CSR_MSTATUS:   data_out = mstatus_reg;
+				CSR_MISA:      data_out = MISA_VALUE;
+				CSR_MEDELEG:   data_out = medeleg_reg;
+				CSR_MIDELEG:   data_out = mideleg_reg;
+				CSR_MIE:       data_out = mie_reg;
+				CSR_MTVEC:     data_out = mtvec_reg;
+				CSR_MSCRATCH:  data_out = mscratch_reg;
+				CSR_MEPC:      data_out = mepc_reg;
+				CSR_MCAUSE:    data_out = mcause_reg;
+				CSR_MTVAL:     data_out = mtval_reg;
+				CSR_MIP:       data_out = mip_value;
+				CSR_MTINST:    data_out = mtinst_reg;
+				CSR_MTVAL2:    data_out = mtval2_reg;
+				CSR_MCOUNTEREN: data_out = mcounteren_reg;
+				CSR_MCOUNTINHIBIT: data_out = mcountinhibit_reg;
+				CSR_MENVCFG:   data_out = menvcfg_reg;
+				CSR_MSTATEEN0: data_out = mstateen_reg[0];
+				CSR_MSTATEEN1: data_out = mstateen_reg[1];
+				CSR_MSTATEEN2: data_out = mstateen_reg[2];
+				CSR_MSTATEEN3: data_out = mstateen_reg[3];
+				CSR_MVENDORID: data_out = VENDOR_ID;
+				CSR_MARCHID:   data_out = ARCH_ID;
+				CSR_MIMPID:    data_out = IMPL_ID;
+				CSR_MHARTID:   data_out = HART_ID;
+				CSR_MCONFIGPTR: data_out = '0;
+				CSR_SSTATUS:   data_out = sstatus_reg;
+				CSR_SIE:       data_out = sie_value;
+				CSR_STVEC:     data_out = stvec_reg;
+				CSR_SCOUNTEREN: data_out = scounteren_reg;
+				CSR_SENVCFG:   data_out = legalize_senvcfg(senvcfg_reg);
+				CSR_SSTATEEN0: data_out = sstateen_reg[0];
+				CSR_SSTATEEN1: data_out = sstateen_reg[1];
+				CSR_SSTATEEN2: data_out = sstateen_reg[2];
+				CSR_SSTATEEN3: data_out = sstateen_reg[3];
+				CSR_SSCRATCH:  data_out = sscratch_reg;
+				CSR_SEPC:      data_out = sepc_reg;
+				CSR_SCAUSE:    data_out = scause_reg;
+				CSR_STVAL:     data_out = stval_reg;
+				CSR_SIP:       data_out = sip_value;
+				CSR_SATP:      data_out = satp_reg;
+				CSR_HSTATUS:   data_out = hstatus_reg;
+				CSR_HEDELEG:   data_out = hedeleg_reg;
+				CSR_HIDELEG:   data_out = hideleg_reg;
+				CSR_HIE:       data_out = hie_reg;
+				CSR_HTIMEDELTA: data_out = htimedelta_reg;
+				CSR_HCOUNTEREN: data_out = hcounteren_reg;
+				CSR_HGEIE:     data_out = hgeie_reg;
+				CSR_HENVCFG:   data_out = henvcfg_reg;
+				CSR_HSTATEEN0: data_out = hstateen_reg[0];
+				CSR_HSTATEEN1: data_out = hstateen_reg[1];
+				CSR_HSTATEEN2: data_out = hstateen_reg[2];
+				CSR_HSTATEEN3: data_out = hstateen_reg[3];
+				CSR_HTVAL:     data_out = htval_reg;
+				CSR_HIP:       data_out = hip_reg;
+				CSR_HVIP:      data_out = hvip_reg;
+				CSR_HTINST:    data_out = htinst_reg;
+				CSR_HGATP:     data_out = hgatp_reg;
+				CSR_HGEIP:     data_out = '0;
+				CSR_VSSTATUS:  data_out = vsstatus_reg;
+				CSR_VSIE:      data_out = vsie_reg;
+				CSR_VSTVEC:    data_out = vstvec_reg;
+				CSR_VSSCRATCH: data_out = vsscratch_reg;
+				CSR_VSEPC:     data_out = vsepc_reg;
+				CSR_VSCAUSE:   data_out = vscause_reg;
+				CSR_VSTVAL:    data_out = vstval_reg;
+				CSR_VSIP:      data_out = vsip_reg;
+				CSR_VSATP:     data_out = vsatp_reg;
+				CSR_MCYCLE:    data_out = mcycle_reg;
+				CSR_MINSTRET:  data_out = minstret_reg;
+				CSR_CYCLE:     data_out = mcycle_reg;
+				CSR_INSTRET:   data_out = minstret_reg;
+				CSR_TIME:       data_out = time_reg;
+				default:        data_out = '0;
+			endcase
+		end
+		return data_out;
+	endfunction
+
+endmodule
diff --git a/rv64cpu/rtl/core/privileged/pmp_checker.sv b/rv64cpu/rtl/core/privileged/pmp_checker.sv
new file mode 100644
index 0000000..361e507
--- /dev/null
+++ b/rv64cpu/rtl/core/privileged/pmp_checker.sv
@@ -0,0 +1,181 @@
+//     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/pmp_checker.sv
+// Description: RISC-V Physical Memory Protection access checker.
+
+module pmp_checker
+    import rv64_pkg::*;
+    import csr_pkg::*;
+#(
+    parameter int PHYS_ADDR_WIDTH = 56
+)(
+    input  logic              req_valid,
+    input  logic [XLEN-1:0]   req_paddr,
+    input  logic [3:0]        req_size,
+    input  logic              req_is_fetch,
+    input  logic              req_is_store,
+    input  logic [1:0]        priv_mode,
+    input  pmpcfg_entry_t     pmpcfg_entries [PMP_ENTRY_COUNT],
+    input  pmpaddr_entry_t    pmpaddr_entries [PMP_ENTRY_COUNT],
+    output logic              access_fault
+);
+
+    localparam logic [1:0] PRIV_MODE_M = 2'b11;
+
+    typedef struct packed {
+        logic valid;
+        logic [XLEN-1:0] lower_bound;
+        logic [XLEN-1:0] upper_bound;
+    } pmp_region_t;
+
+    function automatic logic [XLEN-1:0] pmpaddr_to_paddr(input pmpaddr_entry_t pmpaddr);
+        logic [XLEN-1:0] paddr;
+        paddr = pmpaddr << 2;
+        if (PHYS_ADDR_WIDTH < XLEN) begin
+            paddr[XLEN-1:PHYS_ADDR_WIDTH] = '0;
+        end
+        return paddr;
+    endfunction
+
+    function automatic int unsigned napot_trailing_ones(input pmpaddr_entry_t pmpaddr);
+        int unsigned ones;
+        ones = 0;
+        for (int bit_idx = 0; bit_idx < (PHYS_ADDR_WIDTH - 2); bit_idx++) begin
+            if (pmpaddr[bit_idx]) begin
+                ones++;
+            end else begin
+                break;
+            end
+        end
+        return ones;
+    endfunction
+
+    function automatic pmp_region_t pmp_region(input int unsigned entry_idx);
+        pmp_region_t region;
+        logic [1:0] address_matching;
+        int unsigned ones;
+        logic [XLEN-1:0] napot_mask;
+        logic [XLEN-1:0] napot_base;
+
+        region.valid = 1'b0;
+        region.lower_bound = '0;
+        region.upper_bound = '0;
+        address_matching = pmpcfg_entries[entry_idx][PMPCFG_A_HI:PMPCFG_A_LO];
+
+        unique case (address_matching)
+            PMP_MODE_OFF: begin
+                region.valid = 1'b0;
+            end
+            PMP_MODE_TOR: begin
+                region.valid = 1'b1;
+                region.lower_bound = (entry_idx == 0) ? '0 : pmpaddr_to_paddr(pmpaddr_entries[entry_idx - 1]);
+                region.upper_bound = pmpaddr_to_paddr(pmpaddr_entries[entry_idx]);
+                if (region.upper_bound <= region.lower_bound) begin
+                    region.valid = 1'b0;
+                end
+            end
+            PMP_MODE_NA4: begin
+                region.valid = 1'b1;
+                region.lower_bound = pmpaddr_to_paddr(pmpaddr_entries[entry_idx]);
+                region.upper_bound = region.lower_bound + 64'd4;
+            end
+            PMP_MODE_NAPOT: begin
+                region.valid = 1'b1;
+                ones = napot_trailing_ones(pmpaddr_entries[entry_idx]);
+                napot_mask = (64'd1 << (ones + 1)) - 64'd1;
+                napot_base = (pmpaddr_entries[entry_idx] & ~napot_mask) << 2;
+                if (PHYS_ADDR_WIDTH < XLEN) begin
+                    napot_base[XLEN-1:PHYS_ADDR_WIDTH] = '0;
+                end
+                region.lower_bound = napot_base;
+                region.upper_bound = napot_base + (64'd1 << (ones + 3));
+            end
+            default: begin
+                region.valid = 1'b0;
+            end
+        endcase
+
+        return region;
+    endfunction
+
+    function automatic logic ranges_overlap(
+        input logic [XLEN-1:0] access_lower,
+        input logic [XLEN-1:0] access_upper,
+        input logic [XLEN-1:0] region_lower,
+        input logic [XLEN-1:0] region_upper
+    );
+        return (access_lower < region_upper) && (access_upper > region_lower);
+    endfunction
+
+    function automatic logic range_contains(
+        input logic [XLEN-1:0] access_lower,
+        input logic [XLEN-1:0] access_upper,
+        input logic [XLEN-1:0] region_lower,
+        input logic [XLEN-1:0] region_upper
+    );
+        return (access_lower >= region_lower) && (access_upper <= region_upper);
+    endfunction
+
+    function automatic logic permissions_allow(
+        input pmpcfg_entry_t cfg,
+        input logic is_fetch,
+        input logic is_store
+    );
+        if (is_fetch) begin
+            return cfg[PMPCFG_X_BIT];
+        end
+        if (is_store) begin
+            return cfg[PMPCFG_W_BIT];
+        end
+        return cfg[PMPCFG_R_BIT];
+    endfunction
+
+    always_comb begin
+        logic [XLEN-1:0] access_lower;
+        logic [XLEN-1:0] access_upper;
+        logic [3:0] normalized_size;
+        logic matched;
+        pmp_region_t region;
+
+        normalized_size = (req_size == 4'd0) ? 4'd1 : req_size;
+        access_lower = req_paddr;
+        access_upper = req_paddr + {{(XLEN-4){1'b0}}, normalized_size};
+        matched = 1'b0;
+        region = '0;
+        access_fault = 1'b0;
+
+        if (req_valid) begin
+            for (int unsigned entry_idx = 0; entry_idx < PMP_ENTRY_COUNT; entry_idx++) begin
+                region = pmp_region(entry_idx);
+                if (!matched && region.valid &&
+                    ranges_overlap(access_lower, access_upper, region.lower_bound, region.upper_bound)) begin
+                    matched = 1'b1;
+                    if (!range_contains(access_lower, access_upper, region.lower_bound, region.upper_bound)) begin
+                        access_fault = 1'b1;
+                    end else if ((priv_mode != PRIV_MODE_M) || pmpcfg_entries[entry_idx][PMPCFG_L_BIT]) begin
+                        access_fault = !permissions_allow(pmpcfg_entries[entry_idx], req_is_fetch, req_is_store);
+                    end
+                end
+            end
+
+            if (!matched && (priv_mode != PRIV_MODE_M)) begin
+                access_fault = 1'b1;
+            end
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/privileged/trap_controller.sv b/rv64cpu/rtl/core/privileged/trap_controller.sv
new file mode 100644
index 0000000..246b392
--- /dev/null
+++ b/rv64cpu/rtl/core/privileged/trap_controller.sv
@@ -0,0 +1,279 @@
+//     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/trap_ctrl.sv
+// Description: Privileged-level Trap Controller (Exception and Interrupt Handling)
+//              Detects exceptions/interrupts, triggers context saving, and generates global pipeline flush and PC jump signals
+
+module trap_controller
+    import rv64_pkg::*;
+    import csr_pkg::*;
+(
+    // Submission signals from the WB stage (guarantee of precise exceptions)
+    input  logic [XLEN-1:0]        wb_pc,
+    input  logic                  trap_illegal_instr,
+    input  logic                  trap_is_ecall,
+    input  logic                  trap_is_ebreak,
+    input  logic                  trap_is_mret,
+    input  logic                  trap_is_sret,
+    input  logic                  trap_instr_access_fault,
+    input  logic                  trap_load_fault,
+    input  logic                  trap_store_fault,
+    input  logic                  trap_instr_page_fault,
+    input  logic                  trap_load_page_fault,
+    input  logic                  trap_store_page_fault,
+    input  logic [XLEN-1:0]       trap_bad_addr,
+    input  logic [INST_WIDTH-1:0] trap_bad_instr,
+
+    // Status read from CSR Unit
+    input  logic                  csr_mstatus_mie,    // Global interrupt enable (MIE)
+    input  logic [XLEN-1:0]       csr_mie,            // Machine interrupt enable CSR
+    input  logic [XLEN-1:0]       csr_sie,            // Supervisor interrupt enable CSR view
+    input  logic [XLEN-1:0]       csr_mip,            // Machine interrupt pending CSR
+    input  logic [XLEN-1:0]       csr_sip,            // Supervisor interrupt pending CSR view
+    input  logic [XLEN-1:0]       csr_mstatus,        // Machine status CSR
+    input  logic [XLEN-1:0]       csr_mtvec,          // Machine trap vector
+    input  logic [XLEN-1:0]       csr_stvec,          // Supervisor trap vector
+    input  logic [XLEN-1:0]       csr_mepc,           // Machine exception PC (for MRET)
+    input  logic [XLEN-1:0]       csr_sepc,           // Supervisor exception PC (for SRET)
+    input  logic [XLEN-1:0]       csr_medeleg,        // Exception delegation mask
+    input  logic [XLEN-1:0]       csr_mideleg,        // Interrupt delegation mask
+    input  logic [1:0]            csr_priv_mode,      // Current privilege level
+
+    // Status update signal sent to the CSR Unit
+    output logic                  trap_trigger,       // Trigger exception/interrupt context save
+    output logic [XLEN-1:0]       trap_mepc,          // Saved PC (MEPC/SEPC selected by trap_to_s_mode)
+    output logic [XLEN-1:0]       trap_mcause,        // Cause code (bit63 indicates interrupt)
+    output logic [XLEN-1:0]       trap_mtval,         // Additional information saved to mtval/stval
+    output logic                  trap_to_s_mode,     // 1: delegated to Supervisor mode
+    output logic                  mret_en,            // Trigger MRET context restore
+    output logic                  sret_en,            // Trigger SRET context restore
+
+    // Control flow interference sent to the Hazard Unit and IF stage
+    output logic                  trap_flush_req,     // Global pipeline flush request
+    output logic                  trap_pc_sel,        // Forcibly take over PC MUX (0: normal, 1: Trap/Mret/Sret)
+    output logic [XLEN-1:0]       trap_next_pc        // Target address for forced jump
+);
+
+    typedef enum logic [1:0] {
+        PRIV_MODE_U = 2'b00,
+        PRIV_MODE_S = 2'b01,
+        PRIV_MODE_M = 2'b11
+    } priv_mode_t;
+
+    localparam logic [XLEN-1:0] INTERRUPT_BIT = {1'b1, {(XLEN-1){1'b0}}};
+    localparam int INTERRUPT_M_SOFTWARE_BIT = int'(INTERRUPT_M_SOFTWARE);
+    localparam int INTERRUPT_M_TIMER_BIT    = int'(INTERRUPT_M_TIMER);
+    localparam int INTERRUPT_M_EXTERNAL_BIT = int'(INTERRUPT_M_EXTERNAL);
+    localparam int INTERRUPT_S_SOFTWARE_BIT = int'(INTERRUPT_S_SOFTWARE);
+    localparam int INTERRUPT_S_TIMER_BIT    = int'(INTERRUPT_S_TIMER);
+    localparam int INTERRUPT_S_EXTERNAL_BIT = int'(INTERRUPT_S_EXTERNAL);
+    localparam int MSTATUS_SIE_BIT = 1;
+    localparam int MSTATUS_TSR_BIT = 22;
+
+    logic                 has_sync_exception;
+    logic [XLEN-1:0]      sync_cause;
+    logic [XLEN-1:0]      sync_mtval;
+    logic [XLEN-1:0]      illegal_instr_value;
+    logic                 sret_tsr_illegal;
+
+    assign illegal_instr_value = {{(XLEN-INST_WIDTH){1'b0}}, trap_bad_instr};
+    assign sret_tsr_illegal    = trap_is_sret &&
+                                  (priv_mode_t'(csr_priv_mode) == PRIV_MODE_S) &&
+                                  csr_mstatus[MSTATUS_TSR_BIT];
+
+    always_comb begin
+        has_sync_exception = 1'b0;
+        sync_cause         = '0;
+        sync_mtval         = '0;
+
+        if (trap_instr_access_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_INST_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end else if (trap_instr_page_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_INSTR_PAGE_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end else if (trap_illegal_instr || sret_tsr_illegal) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_INST_ILLEGAL;
+            sync_mtval         = illegal_instr_value;
+        end else if (trap_is_ebreak) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_BREAKPOINT;
+            sync_mtval         = wb_pc;
+        end else if (trap_is_ecall) begin
+            has_sync_exception = 1'b1;
+            case (priv_mode_t'(csr_priv_mode))
+                PRIV_MODE_U: sync_cause = EXC_ECALL_FROM_U;
+                PRIV_MODE_S: sync_cause = EXC_ECALL_FROM_S;
+                default:     sync_cause = EXC_ECALL_FROM_M;
+            endcase
+            sync_mtval = '0;
+        end else if (trap_load_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_LOAD_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end else if (trap_store_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_STORE_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end else if (trap_load_page_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_LOAD_PAGE_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end else if (trap_store_page_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_STORE_PAGE_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end
+    end
+
+    // Interrupt Detection - Asynchronous, gated by mie and privilege global-enable rules.
+    logic               has_interrupt;
+    logic [XLEN-1:0]    interrupt_cause;
+    logic               machine_interrupts_globally_enabled;
+    logic               supervisor_interrupts_globally_enabled;
+    logic               interrupt_targets_s_mode;
+
+    assign machine_interrupts_globally_enabled = (priv_mode_t'(csr_priv_mode) != PRIV_MODE_M) ||
+                                                 csr_mstatus_mie;
+    assign supervisor_interrupts_globally_enabled = (priv_mode_t'(csr_priv_mode) == PRIV_MODE_U) ||
+                                                    ((priv_mode_t'(csr_priv_mode) == PRIV_MODE_S) &&
+                                                     csr_mstatus[MSTATUS_SIE_BIT]);
+
+    function automatic logic interrupt_pending_enabled(
+        input logic [XLEN-1:0] pending,
+        input logic [XLEN-1:0] enable,
+        input int              cause
+    );
+        interrupt_pending_enabled = pending[cause[5:0]] && enable[cause[5:0]];
+    endfunction
+
+    task automatic take_interrupt(
+        input  logic [XLEN-1:0] cause,
+        input  logic            to_s_mode,
+        output logic            taken,
+        output logic [XLEN-1:0] taken_cause,
+        output logic            taken_targets_s_mode
+    );
+        taken                = 1'b1;
+        taken_cause          = INTERRUPT_BIT | cause;
+        taken_targets_s_mode = to_s_mode;
+    endtask
+
+    function automatic logic s_interrupt_cause_delegated(input int cause);
+        s_interrupt_cause_delegated = csr_mideleg[cause[5:0]];
+    endfunction
+
+    function automatic logic s_interrupt_cause_takeable(input int cause);
+        if (s_interrupt_cause_delegated(cause)) begin
+            s_interrupt_cause_takeable = supervisor_interrupts_globally_enabled &&
+                                         interrupt_pending_enabled(csr_sip, csr_sie, cause);
+        end else begin
+            s_interrupt_cause_takeable = machine_interrupts_globally_enabled &&
+                                         interrupt_pending_enabled(csr_mip, csr_mie, cause);
+        end
+    endfunction
+
+    always_comb begin
+        has_interrupt             = 1'b0;
+        interrupt_cause           = '0;
+        interrupt_targets_s_mode  = 1'b0;
+
+        if (machine_interrupts_globally_enabled &&
+            interrupt_pending_enabled(csr_mip, csr_mie, INTERRUPT_M_EXTERNAL_BIT)) begin
+            take_interrupt(INTERRUPT_M_EXTERNAL, 1'b0, has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end else if (machine_interrupts_globally_enabled &&
+                     interrupt_pending_enabled(csr_mip, csr_mie, INTERRUPT_M_SOFTWARE_BIT)) begin
+            take_interrupt(INTERRUPT_M_SOFTWARE, 1'b0, has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end else if (machine_interrupts_globally_enabled &&
+                     interrupt_pending_enabled(csr_mip, csr_mie, INTERRUPT_M_TIMER_BIT)) begin
+            take_interrupt(INTERRUPT_M_TIMER, 1'b0, has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end else if (s_interrupt_cause_takeable(INTERRUPT_S_EXTERNAL_BIT)) begin
+            take_interrupt(INTERRUPT_S_EXTERNAL, s_interrupt_cause_delegated(INTERRUPT_S_EXTERNAL_BIT),
+                           has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end else if (s_interrupt_cause_takeable(INTERRUPT_S_SOFTWARE_BIT)) begin
+            take_interrupt(INTERRUPT_S_SOFTWARE, s_interrupt_cause_delegated(INTERRUPT_S_SOFTWARE_BIT),
+                           has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end else if (s_interrupt_cause_takeable(INTERRUPT_S_TIMER_BIT)) begin
+            take_interrupt(INTERRUPT_S_TIMER, s_interrupt_cause_delegated(INTERRUPT_S_TIMER_BIT),
+                           has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end
+    end 
+
+    logic [XLEN-1:0] trap_cause_value;
+    logic [XLEN-1:0] trap_mtval_value;
+    logic            trap_is_interrupt;
+
+    assign trap_is_interrupt = has_interrupt;
+    assign trap_cause_value  = has_interrupt ? interrupt_cause : sync_cause;
+    assign trap_mtval_value  = has_interrupt ? '0 : sync_mtval;
+
+    assign trap_trigger = has_interrupt | has_sync_exception;
+
+    logic [5:0]      trap_cause_index;
+    logic            delegate_to_s;
+    logic [XLEN-1:0] deleg_mask;
+    logic            machine_interrupt_cause;
+
+    assign trap_cause_index = trap_cause_value[5:0];
+    assign deleg_mask       = trap_is_interrupt ? csr_mideleg : csr_medeleg;
+    assign machine_interrupt_cause = trap_is_interrupt &&
+                                     ((trap_cause_index == INTERRUPT_M_SOFTWARE_BIT[5:0]) ||
+                                      (trap_cause_index == INTERRUPT_M_TIMER_BIT[5:0]) ||
+                                      (trap_cause_index == INTERRUPT_M_EXTERNAL_BIT[5:0]));
+    assign delegate_to_s    = trap_trigger && (priv_mode_t'(csr_priv_mode) != PRIV_MODE_M) &&
+                              ((trap_is_interrupt && interrupt_targets_s_mode) ||
+                               (!trap_is_interrupt && deleg_mask[trap_cause_index] && !machine_interrupt_cause));
+    assign trap_to_s_mode   = delegate_to_s;
+
+    assign trap_mcause = trap_trigger ? trap_cause_value : '0;
+    assign trap_mtval  = trap_trigger ? trap_mtval_value : '0;
+    assign trap_mepc   = trap_trigger ? wb_pc : '0;
+
+    assign mret_en = trap_is_mret & ~trap_trigger;
+    assign sret_en = trap_is_sret & ~trap_trigger;
+
+    assign trap_flush_req = trap_trigger | mret_en | sret_en;
+    assign trap_pc_sel    = trap_flush_req;
+
+    logic [XLEN-1:0] trap_vector_base;
+    logic [1:0]      trap_vector_mode;
+    logic [XLEN-1:0] trap_vector_aligned;
+
+    assign trap_vector_base    = delegate_to_s ? csr_stvec : csr_mtvec;
+    assign trap_vector_mode    = trap_vector_base[1:0];
+    assign trap_vector_aligned = {trap_vector_base[XLEN-1:2], 2'b00};
+
+    always_comb begin
+        if (trap_trigger) begin
+            if ((trap_vector_mode == 2'b01) && trap_is_interrupt) begin
+                trap_next_pc = trap_vector_aligned + (({{(XLEN-6){1'b0}}, trap_cause_index}) << 2);
+            end else begin
+                trap_next_pc = trap_vector_aligned;
+            end
+        end else if (mret_en) begin
+            trap_next_pc = csr_mepc;
+        end else if (sret_en) begin
+            trap_next_pc = csr_sepc;
+        end else begin
+            trap_next_pc = '0;
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/registers/fpr_file.sv b/rv64cpu/rtl/core/registers/fpr_file.sv
new file mode 100644
index 0000000..7522b5f
--- /dev/null
+++ b/rv64cpu/rtl/core/registers/fpr_file.sv
@@ -0,0 +1,41 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+// File path: rtl/core/fp_regfile.sv
+// Description: 32-entry 64-bit floating-point register file for RV64F/RV64D.
+
+module fpr_file
+    import rv64_pkg::*;
+(
+    input  logic            clk,
+    input  logic            rst,
+
+    input  logic [4:0]      fs1_addr,
+    input  logic [4:0]      fs2_addr,
+    input  logic [4:0]      fs3_addr,
+    output logic [XLEN-1:0] fs1_rdata,
+    output logic [XLEN-1:0] fs2_rdata,
+    output logic [XLEN-1:0] fs3_rdata,
+
+    input  logic            fp_write_en,
+    input  logic [4:0]      fd_addr,
+    input  logic [XLEN-1:0] fd_wdata
+);
+
+    logic [XLEN-1:0] regs [0:31];
+
+    assign fs1_rdata = (fp_write_en && (fs1_addr == fd_addr)) ? fd_wdata : regs[fs1_addr];
+    assign fs2_rdata = (fp_write_en && (fs2_addr == fd_addr)) ? fd_wdata : regs[fs2_addr];
+    assign fs3_rdata = (fp_write_en && (fs3_addr == fd_addr)) ? fd_wdata : regs[fs3_addr];
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            for (int unsigned reg_idx = 0; reg_idx < 32; reg_idx++) begin
+                regs[reg_idx] <= '0;
+            end
+        end else if (fp_write_en) begin
+            regs[fd_addr] <= fd_wdata;
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/registers/gpr_file.sv b/rv64cpu/rtl/core/registers/gpr_file.sv
new file mode 100644
index 0000000..4ddf200
--- /dev/null
+++ b/rv64cpu/rtl/core/registers/gpr_file.sv
@@ -0,0 +1,67 @@
+//     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/regfile.sv
+// Description: 64-bit Register File
+
+module gpr_file
+    import rv64_pkg::*;
+(
+    input  logic                      clk,  // Clock signal
+    input  logic                      rst,  // Reset signal active high
+
+    // Read port 1
+    input  logic [4:0]                rs1_addr, // Read address 1
+    output logic [XLEN-1:0]           rs1_rdata,  // Read data 1
+
+    // Read port 2
+    input  logic [4:0]                rs2_addr, // Read address 2
+    output logic [XLEN-1:0]           rs2_rdata,  // Read data 2
+
+    // Write port
+    input  logic [4:0]                rd_addr,  // Write address
+    input  logic [XLEN-1:0]           rd_wdata,   // Write data
+    input  logic                      reg_write_en // Write enable signal
+);
+
+    // Register file storage
+    logic [XLEN-1:0] regs [0:31];
+
+    // Read Logic (combinational logic)
+    // Internal bypass: If reading the same register as currently being written, 
+    // forward the write data immediately.
+    assign rs1_rdata = (rs1_addr == 5'b0) ? '0 : 
+                       ((rs1_addr == rd_addr) && reg_write_en) ? rd_wdata : 
+                       regs[rs1_addr];
+                       
+    assign rs2_rdata = (rs2_addr == 5'b0) ? '0 : 
+                       ((rs2_addr == rd_addr) && reg_write_en) ? rd_wdata : 
+                       regs[rs2_addr];
+
+    // Write Logic (sequential logic)
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            for (int i = 0; i < 32; i++) begin
+                regs[i] <= '0;
+            end
+        end else begin
+            if (reg_write_en && rd_addr != 5'b0) begin
+                regs[rd_addr] <= rd_wdata;
+            end
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/rv64_core.sv b/rv64cpu/rtl/core/rv64_core.sv
new file mode 100644
index 0000000..630fc3d
--- /dev/null
+++ b/rv64cpu/rtl/core/rv64_core.sv
@@ -0,0 +1,1217 @@
+//     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/richard_core.sv
+// Description: Richard CPU core top-level wrapper (Core Top)
+// Assembles a 5-stage pipeline, 4 inter-stage registers, and Hazard Unit
+
+module rv64_core
+    import rv64_pkg::*;
+    import csr_pkg::*;
+(
+    input  logic        clk,
+    input  logic        rst,
+
+    // Interface 1: Instruction Memory (I-Mem)
+    output logic [XLEN-1:0] imem_addr,
+    input  logic [31:0]     imem_rdata,
+
+    // Interface 2: Data Memory (D-Mem)
+    output logic        dmem_read_en,
+    output logic        dmem_write_en,
+    output logic [63:0] dmem_addr,
+    output logic [63:0] dmem_wdata,
+    output logic [2:0]  dmem_size,
+    input  logic [63:0] dmem_rdata,
+
+    // Interface 3: External Interrupt Sources
+    input  logic        m_timer_interrupt,
+    input  logic        m_software_interrupt,
+    input  logic        m_external_interrupt,
+    input  logic        s_timer_interrupt,
+    input  logic        s_software_interrupt,
+    input  logic        s_external_interrupt
+);
+    localparam int CSR_MSTATUS_MIE_BIT = 3;
+    localparam int CSR_MSTATUS_SUM_BIT = 18;
+    localparam int CSR_MSTATUS_MXR_BIT = 19;
+    localparam int CSR_MSTATUS_TVM_BIT = 20;
+    localparam int CSR_MSTATUS_TW_BIT  = 21;
+    localparam int CSR_MSTATUS_TSR_BIT = 22;
+
+    // -----------------------
+    // Internal Wire Declarations
+    // -----------------------
+
+    // (1) Hazard Unit Controls
+    logic [1:0]        hu_forward_a_sel, hu_forward_b_sel;
+    logic              hu_if_stall_en, hu_if_id_stall_en, hu_id_ex_stall_en, hu_ex_mem_stall_en;
+    logic              hu_if_id_flush_en, hu_id_ex_flush_en;
+    logic              hu_pc_redirect_sel, hu_pc_redirect_is_trap;
+    logic [XLEN-1:0]   hu_pc_redirect_target;
+
+    // (2) IF Stage Outputs
+    logic [XLEN-1:0]   if_pc_out;
+    logic [INST_WIDTH-1:0] if_instr_to_decode;
+    logic [INST_WIDTH-1:0] if_compressed_expanded_instr;
+    logic              if_instr_compressed;
+    logic [2:0]        if_instr_len;
+    logic              if_compressed_illegal;
+    // imem interface (ports)
+
+    // (3) IF/ID Pipeline Register Outputs
+    logic [XLEN-1:0]   if_id_pc;
+    logic [31:0]       if_id_instr;
+    logic [2:0]        if_id_instr_len;
+
+    // (4) ID Stage Outputs
+    logic [XLEN-1:0]   id_pc_out;
+    logic [2:0]        id_instr_len;
+    logic [XLEN-1:0]   id_rs1_rdata, id_rs2_rdata;
+    logic [XLEN-1:0]   id_fs1_rdata, id_fs2_rdata, id_fs3_rdata;
+    logic [4:0]        id_rs1_addr, id_rs2_addr;
+    logic [4:0]        id_fs3_addr;
+    logic [XLEN-1:0]   id_imm;
+    alu_op_t           id_alu_op;
+    logic              id_alu_src1_sel, id_alu_src2_sel;
+    logic              id_is_word_op, id_is_branch, id_is_jump;
+    branch_op_t        id_branch_op;
+    logic              id_is_load, id_is_store;
+    logic [2:0]        id_mem_size;
+    logic              id_is_amo;
+    amo_op_t           id_amo_op;
+    logic              id_is_fp, id_fp_reg_write_en, id_fp_store_data_sel;
+    fp_op_t            id_fp_op;
+    fp_fmt_t           id_fp_fmt;
+    logic [2:0]        id_fp_rm;
+    logic              id_reg_write_en;
+    logic [4:0]        id_rd_addr;
+    logic [1:0]        id_wb_sel;
+    logic              id_is_csr;
+    logic [2:0]        id_csr_op;
+    logic              id_illegal_instr, id_is_ecall, id_is_ebreak, id_is_mret, id_is_sret;
+    logic              id_is_wfi, id_is_sfence_vma, id_is_fence, id_is_fence_i;
+
+    // (5) ID/EX Pipeline Register Outputs
+    logic [XLEN-1:0]   ex_pc_in;
+    logic [INST_WIDTH-1:0] ex_instr_in;
+    logic [2:0]        ex_instr_len_in;
+    logic [XLEN-1:0]   ex_rs1_rdata_in, ex_rs2_rdata_in;
+    logic [XLEN-1:0]   ex_fs1_rdata_in, ex_fs2_rdata_in, ex_fs3_rdata_in;
+    logic [4:0]        ex_rs1_addr_in, ex_rs2_addr_in;
+    logic [4:0]        ex_fs3_addr_in;
+    logic [XLEN-1:0]   ex_imm_in;
+    alu_op_t           ex_alu_op_in;
+    logic              ex_alu_src1_sel_in, ex_alu_src2_sel_in;
+    logic              ex_is_word_op_in, ex_is_branch_in, ex_is_jump_in;
+    branch_op_t        ex_branch_op_in;
+    logic              ex_is_load_in, ex_is_store_in;
+    logic [2:0]        ex_mem_size_in;
+    logic              ex_is_amo_in;
+    amo_op_t           ex_amo_op_in;
+    logic              ex_is_fp_in, ex_fp_reg_write_en_in, ex_fp_store_data_sel_in;
+    fp_op_t            ex_fp_op_in;
+    fp_fmt_t           ex_fp_fmt_in;
+    logic [2:0]        ex_fp_rm_in;
+    logic              ex_reg_write_en_in;
+    logic [4:0]        ex_rd_addr_in;
+    logic [1:0]        ex_wb_sel_in;
+    logic              ex_is_csr_in;
+    logic [2:0]        ex_csr_op_in;
+    logic              ex_illegal_instr_in, ex_is_ecall_in, ex_is_ebreak_in, ex_is_mret_in, ex_is_sret_in;
+    logic              ex_is_wfi_in, ex_is_sfence_vma_in, ex_is_fence_in, ex_is_fence_i_in;
+
+    // (6) EX Stage Outputs (and Forwarding Muxes)
+    logic [XLEN-1:0]   ex_rs1_rdata_fwd, ex_rs2_rdata_fwd;
+    logic [XLEN-1:0]   ex_fs1_rdata_fwd, ex_fs2_rdata_fwd, ex_fs3_rdata_fwd;
+    logic [XLEN-1:0]   mem_forward_data;
+    logic [XLEN-1:0]   ex_alu_result_out;
+    logic [XLEN-1:0]   ex_result_to_mem;
+    logic [XLEN-1:0]   ex_fp_result_out;
+    logic [XLEN-1:0]   ex_fp_int_result_out;
+    logic [4:0]        ex_fp_fflags_out;
+    logic              ex_fp_illegal_out;
+    logic              ex_branch_taken_out;
+    logic [XLEN-1:0]   ex_branch_target_out;
+    logic              ex_redirect_taken;
+    logic [XLEN-1:0]   ex_redirect_target;
+
+    // (7) EX/MEM Pipeline Register Outputs
+    logic [XLEN-1:0]   mem_pc_in;
+    logic [INST_WIDTH-1:0] mem_instr_in;
+    logic [2:0]        mem_instr_len_in;
+    logic [XLEN-1:0]   mem_alu_result_in;
+    logic              mem_branch_taken_in;
+    logic [XLEN-1:0]   mem_branch_target_in;
+    logic [XLEN-1:0]   mem_rs1_rdata_in, mem_rs2_rdata_in;
+    logic [XLEN-1:0]   mem_fs1_rdata_in, mem_fs2_rdata_in, mem_fs3_rdata_in;
+    logic [4:0]        mem_rs1_addr_in;
+    logic [4:0]        mem_fs3_addr_in;
+    logic [XLEN-1:0]   mem_imm_in;
+    logic              mem_is_load_in, mem_is_store_in;
+    logic [2:0]        mem_mem_size_in;
+    logic              mem_is_amo_in;
+    amo_op_t           mem_amo_op_in;
+    logic              mem_is_fp_in, mem_fp_reg_write_en_in, mem_fp_store_data_sel_in;
+    fp_op_t            mem_fp_op_in;
+    fp_fmt_t           mem_fp_fmt_in;
+    logic [2:0]        mem_fp_rm_in;
+    logic [XLEN-1:0]   mem_fp_result_in;
+    logic [4:0]        mem_fp_fflags_in;
+    logic              mem_reg_write_en_in;
+    logic [4:0]        mem_rd_addr_in;
+    logic [1:0]        mem_wb_sel_in;
+    logic              mem_is_csr_in;
+    logic [2:0]        mem_csr_op_in;
+    logic              mem_illegal_instr_in, mem_is_ecall_in, mem_is_ebreak_in, mem_is_mret_in, mem_is_sret_in;
+    logic              mem_is_wfi_in, mem_is_sfence_vma_in, mem_is_fence_in, mem_is_fence_i_in;
+
+    // (8) MEM Stage Outputs (to MEM/WB Register)
+    logic [XLEN-1:0]   mw_reg_alu_result_in;
+    logic [XLEN-1:0]   mw_reg_mem_rdata_in;
+    logic [XLEN-1:0]   mw_reg_fp_result_in;
+    logic [4:0]        mw_reg_fp_fflags_in;
+    logic [XLEN-1:0]   mw_reg_pc_in;
+    logic [INST_WIDTH-1:0] mw_reg_instr_in;
+    logic [2:0]        mw_reg_instr_len_in;
+    logic [7:0]        mw_reg_dmem_wstrb_in;
+    logic              mw_reg_reg_write_en_in;
+    logic              mw_reg_fp_reg_write_en_in;
+    logic [4:0]        mw_reg_rd_addr_in;
+    logic [1:0]        mw_reg_wb_sel_in;
+    logic              mw_reg_is_csr_in;
+    logic [2:0]        mw_reg_csr_op_in;
+    logic [XLEN-1:0]   mw_reg_rs1_rdata_in;
+    logic [4:0]        mw_reg_rs1_addr_in;
+    logic [XLEN-1:0]   mw_reg_imm_in;
+    logic              mw_reg_illegal_instr_in, mw_reg_is_ecall_in, mw_reg_is_ebreak_in, mw_reg_is_mret_in, mw_reg_is_sret_in;
+    logic              mw_reg_load_fault_in, mw_reg_store_fault_in;
+    logic              mw_reg_load_page_fault_in, mw_reg_store_page_fault_in;
+    logic [XLEN-1:0]   mw_reg_fault_addr_in;
+    logic              mem_stall_req;
+
+    // (9) MEM/WB Pipeline Register Outputs
+    logic [XLEN-1:0]   wb_pc_in;
+    logic [XLEN-1:0]   wb_alu_result_in;
+    logic [XLEN-1:0]   wb_mem_rdata_in;
+    logic [XLEN-1:0]   wb_fp_result_in;
+    logic [4:0]        wb_fp_fflags_in;
+    logic [INST_WIDTH-1:0] wb_instr_in;
+    logic [2:0]        wb_instr_len_in;
+    logic              wb_reg_write_en_in;
+    logic              wb_fp_reg_write_en_in;
+    logic [4:0]        wb_rd_addr_in;
+    logic [1:0]        wb_wb_sel_in;
+    logic              wb_is_csr_in;
+    logic [2:0]        wb_csr_op_in;
+    logic [XLEN-1:0]   wb_rs1_rdata_in;
+    logic [4:0]        wb_rs1_addr_in;
+    logic [XLEN-1:0]   wb_imm_in;
+    logic              wb_illegal_instr_in, wb_is_ecall_in, wb_is_ebreak_in, wb_is_mret_in, wb_is_sret_in;
+    logic              wb_load_fault, wb_store_fault;
+    logic              wb_load_page_fault, wb_store_page_fault;
+    logic [XLEN-1:0]   wb_fault_addr_in;
+
+    // (10) WB Stage Outputs (to RegFile & CSR Unit)
+    logic [XLEN-1:0]   wb_rd_wdata;
+    logic [4:0]        wb_rd_addr;
+    logic              wb_reg_write_en_out;
+    logic [XLEN-1:0]   wb_fd_wdata;
+    logic [4:0]        wb_fd_addr;
+    logic              wb_fp_write_en;
+    logic [4:0]        wb_fp_fflags;
+    logic              wb_fp_state_dirty;
+    logic              wb_fp_fflags_valid;
+    logic              csr_req;
+    logic [2:0]        csr_op;
+    logic [11:0]       csr_addr;
+    logic [XLEN-1:0]   csr_rs1_data;
+    logic [XLEN-1:0]   csr_rdata; // Result from CSR unit
+    logic [1:0]        csr_priv_mode_cur;
+    logic [XLEN-1:0]   csr_mtvec_base;
+    logic [XLEN-1:0]   csr_stvec_base;
+    logic [XLEN-1:0]   csr_mstatus_value;
+    logic [XLEN-1:0]   csr_sstatus_value;
+    logic [XLEN-1:0]   csr_medeleg_value;
+    logic [XLEN-1:0]   csr_mideleg_value;
+    logic [XLEN-1:0]   csr_mie_value;
+    logic [XLEN-1:0]   csr_sie_value;
+    logic [XLEN-1:0]   csr_mip_value;
+    logic [XLEN-1:0]   csr_sip_value;
+    logic [XLEN-1:0]   csr_mepc_value;
+    logic [XLEN-1:0]   csr_sepc_value;
+    logic [XLEN-1:0]   csr_satp_value;
+    logic [2:0]        csr_frm_value;
+    logic [XLEN-1:0]   csr_trap_vector_value;
+    logic              csr_trap_to_s_mode;
+    logic              csr_illegal_access;
+    pmpcfg_entry_t     csr_pmpcfg_entries [PMP_ENTRY_COUNT];
+    pmpaddr_entry_t    csr_pmpaddr_entries [PMP_ENTRY_COUNT];
+
+    // (11) Exception / Trap bus
+    logic              trap_illegal_instr, trap_is_ecall, trap_is_ebreak, trap_is_mret, trap_is_sret;
+    logic [XLEN-1:0]   trap_epc;
+    logic              trap_instr_access_fault;
+
+    // (12) Trap Controller + interrupt wiring
+    logic              trap_load_fault, trap_store_fault;
+    logic              trap_instr_page_fault, trap_load_page_fault, trap_store_page_fault;
+    logic [XLEN-1:0]   trap_bad_addr;
+    logic [INST_WIDTH-1:0] trap_bad_instr;
+    logic              trap_flush_req;
+    logic              trap_pc_sel;
+    logic [XLEN-1:0]   trap_next_pc;
+    logic              trap_pc_sel_raw;
+    logic [XLEN-1:0]   trap_next_pc_raw;
+    logic              trap_ctrl_trigger;
+    logic [XLEN-1:0]   trap_ctrl_mepc;
+    logic [XLEN-1:0]   trap_ctrl_mcause;
+    logic [XLEN-1:0]   trap_ctrl_mtval;
+    logic              trap_ctrl_to_s_mode;
+    logic              trap_ctrl_mret_en;
+    logic              trap_ctrl_sret_en;
+    logic              csr_mstatus_mie;
+    logic              csr_mstatus_sum;
+    logic              csr_mstatus_mxr;
+    logic              if_instr_access_fault;
+    logic              if_instr_page_fault;
+    logic              if_id_instr_access_fault;
+    logic              if_id_instr_page_fault;
+    logic              ex_instr_access_fault_in;
+    logic              ex_instr_page_fault_in;
+    logic              mem_instr_access_fault_in;
+    logic              mem_instr_page_fault_in;
+    logic              mw_reg_instr_access_fault_in;
+    logic              mw_reg_instr_page_fault_in;
+    logic              wb_instr_access_fault;
+    logic              wb_instr_page_fault;
+    logic              dmem_access_fault;
+    logic              dmem_load_access_fault;
+    logic              dmem_store_access_fault;
+    logic              dmem_load_page_fault;
+    logic              dmem_store_page_fault;
+    logic [3:0]        dmem_access_size;
+
+    logic              mmu_i_req_valid;
+    logic              mmu_i_resp_valid;
+    logic              mmu_i_resp_page_fault;
+    logic [XLEN-1:0]   mmu_i_resp_vaddr;
+    logic [XLEN-1:0]   mmu_i_resp_paddr;
+    logic              mmu_d_req_valid;
+    logic              mmu_d_resp_valid;
+    logic              mmu_d_resp_page_fault;
+    logic [XLEN-1:0]   mmu_d_resp_paddr;
+    logic              ptw_mem_req_valid;
+    logic              ptw_mem_req_ready;
+    logic [XLEN-1:0]   ptw_mem_req_addr;
+    logic              ptw_mem_resp_valid;
+    logic [XLEN-1:0]   ptw_mem_resp_data;
+    logic              ptw_mem_resp_valid_q;
+    logic [XLEN-1:0]   ptw_mem_resp_data_q;
+    logic              raw_dmem_read_en;
+    logic              raw_dmem_write_en;
+    logic [XLEN-1:0]   raw_dmem_addr;
+    logic [XLEN-1:0]   raw_dmem_wdata;
+    logic [7:0]        raw_dmem_wstrb;
+    logic              data_access_needs_translation;
+    logic              data_translation_ready;
+    logic              data_translation_page_fault;
+    logic              data_memory_port_available;
+    logic              if_translation_ready;
+    logic              if_translation_page_fault;
+    logic              mmu_stall_req;
+    logic              dmem_req_is_store;
+
+    assign csr_mstatus_mie        = csr_mstatus_value[CSR_MSTATUS_MIE_BIT];
+    assign csr_mstatus_sum        = csr_mstatus_value[CSR_MSTATUS_SUM_BIT];
+    assign csr_mstatus_mxr        = csr_mstatus_value[CSR_MSTATUS_MXR_BIT];
+    assign ex_redirect_taken       = ex_branch_taken_out | ex_is_fence_i_in;
+    assign ex_redirect_target      = ex_is_fence_i_in ?
+                                     (ex_pc_in + {{(XLEN-3){1'b0}}, ex_instr_len_in}) :
+                                     ex_branch_target_out;
+
+    always_comb begin
+        unique case (mem_mem_size_in)
+            3'b000, 3'b100: dmem_access_size = 4'd1;
+            3'b001, 3'b101: dmem_access_size = 4'd2;
+            3'b010, 3'b110: dmem_access_size = 4'd4;
+            3'b011:         dmem_access_size = 4'd8;
+            default:        dmem_access_size = 4'd1;
+        endcase
+    end
+
+    assign mmu_i_req_valid = !rst;
+    assign dmem_req_is_store = mem_is_store_in | (mem_is_amo_in && (mem_amo_op_in != AMO_LR));
+
+    assign mmu_d_req_valid = (mem_is_load_in | mem_is_store_in | mem_is_amo_in) &
+                              ~mem_instr_access_fault_in & ~mem_instr_page_fault_in;
+
+    assign if_translation_ready      = mmu_i_resp_valid & (mmu_i_resp_vaddr == if_pc_out);
+    assign if_translation_page_fault = if_translation_ready & mmu_i_resp_page_fault;
+    assign if_instr_page_fault       = if_translation_page_fault;
+
+    assign data_access_needs_translation = mmu_d_req_valid;
+    assign data_translation_ready        = !data_access_needs_translation | mmu_d_resp_valid;
+    assign data_translation_page_fault   = data_access_needs_translation & mmu_d_resp_valid &
+                                           mmu_d_resp_page_fault;
+    assign data_memory_port_available    = !ptw_mem_req_valid;
+    assign dmem_load_page_fault          = (mem_is_load_in | (mem_is_amo_in && (mem_amo_op_in == AMO_LR))) &
+                                           data_translation_page_fault;
+    assign dmem_store_page_fault         = dmem_req_is_store & data_translation_page_fault;
+    assign mmu_stall_req                 = !if_translation_ready;
+
+    assign ptw_mem_req_ready  = 1'b1;
+    assign ptw_mem_resp_valid = ptw_mem_resp_valid_q;
+    assign ptw_mem_resp_data  = ptw_mem_resp_data_q;
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            ptw_mem_resp_valid_q <= 1'b0;
+            ptw_mem_resp_data_q  <= '0;
+        end else begin
+            ptw_mem_resp_valid_q <= ptw_mem_req_valid & ptw_mem_req_ready;
+            if (ptw_mem_req_valid & ptw_mem_req_ready) begin
+                ptw_mem_resp_data_q <= dmem_rdata;
+            end
+        end
+    end
+
+    mmu u_mmu_top (
+        .clk                  (clk),
+        .rst                  (rst),
+        .satp_value           (csr_satp_value),
+        .priv_mode            (csr_priv_mode_cur),
+        .sum                  (csr_mstatus_sum),
+        .mxr                  (csr_mstatus_mxr),
+        .i_req_valid          (mmu_i_req_valid),
+        .i_req_ready          (),
+        .i_req_vaddr          (if_pc_out),
+        .i_resp_valid         (mmu_i_resp_valid),
+        .i_resp_hit           (),
+        .i_resp_page_fault    (mmu_i_resp_page_fault),
+        .i_resp_vaddr         (mmu_i_resp_vaddr),
+        .i_resp_paddr         (mmu_i_resp_paddr),
+        .d_req_valid          (mmu_d_req_valid),
+        .d_req_ready          (),
+        .d_req_vaddr          (mem_alu_result_in),
+        .d_req_is_store       (dmem_req_is_store),
+        .d_resp_valid         (mmu_d_resp_valid),
+        .d_resp_hit           (),
+        .d_resp_page_fault    (mmu_d_resp_page_fault),
+        .d_resp_paddr         (mmu_d_resp_paddr),
+        .flush_all            (mem_is_sfence_vma_in),
+        .flush_asid_valid     (1'b0),
+        .flush_asid           ('0),
+        .flush_vpn_valid      (1'b0),
+        .flush_vpn            ('0),
+        .ptw_mem_req_valid    (ptw_mem_req_valid),
+        .ptw_mem_req_ready    (ptw_mem_req_ready),
+        .ptw_mem_req_addr     (ptw_mem_req_addr),
+        .ptw_mem_resp_valid   (ptw_mem_resp_valid),
+        .ptw_mem_resp_data    (ptw_mem_resp_data)
+    );
+
+    // ---------------------------------------------------------
+    // (A) Hazard Unit Implementation
+    // ---------------------------------------------------------
+    hazard_unit u_hazard_unit (
+        .id_rs1_addr        (id_rs1_addr),
+        .id_rs2_addr        (id_rs2_addr),
+        .ex_rs1_addr        (ex_rs1_addr_in),
+        .ex_rs2_addr        (ex_rs2_addr_in),
+        .ex_rd_addr         (ex_rd_addr_in),
+        .ex_reg_write_en    (ex_reg_write_en_in),
+        .ex_is_load         (ex_is_load_in | ex_is_amo_in),
+        .ex_is_csr          (ex_is_csr_in),
+        .ex_branch_taken    (ex_redirect_taken),
+        .ex_branch_target   (ex_redirect_target),
+        .mem_rd_addr        (mem_rd_addr_in),
+        .mem_reg_write_en   (mem_reg_write_en_in),
+        .mem_result_late    (mem_wb_sel_in == 2'b01),
+        .mem_is_csr         (mem_is_csr_in),
+        .mem_stall_req      (mem_stall_req | mmu_stall_req),
+        .trap_flush_req     (trap_flush_req),
+        .trap_pc_sel        (trap_pc_sel),
+        .trap_target_pc     (trap_next_pc),
+        .wb_rd_addr         (wb_rd_addr_in),
+        .wb_reg_write_en    (wb_reg_write_en_out),
+        .forward_a_sel      (hu_forward_a_sel),
+        .forward_b_sel      (hu_forward_b_sel),
+        .if_stall_en        (hu_if_stall_en),
+        .if_id_stall_en     (hu_if_id_stall_en),
+        .id_ex_stall_en     (hu_id_ex_stall_en),
+        .ex_mem_stall_en    (hu_ex_mem_stall_en),
+        .if_id_flush_en     (hu_if_id_flush_en),
+        .id_ex_flush_en     (hu_id_ex_flush_en),
+        .pc_redirect_sel    (hu_pc_redirect_sel),
+        .pc_redirect_target (hu_pc_redirect_target),
+        .pc_redirect_is_trap(hu_pc_redirect_is_trap)
+    );
+
+    // ---------------------------------------------------------
+    // (B) IF Stage - Instruction Fetch
+    // ---------------------------------------------------------
+    if_stage u_if_stage (
+        .clk                (clk),
+        .rst                (rst),
+        .stall_en           (hu_if_stall_en),
+        .pc_redirect_sel    (hu_pc_redirect_sel),
+        .pc_redirect_target (hu_pc_redirect_target),
+        .pc_redirect_is_trap(hu_pc_redirect_is_trap),
+        .if_instr_compressed(if_instr_compressed),
+        .if_pc              (if_pc_out)
+    );
+
+    assign imem_addr = (if_translation_ready && !mmu_i_resp_page_fault) ? mmu_i_resp_paddr : if_pc_out;
+
+    assign if_instr_compressed = (imem_rdata[1:0] != 2'b11);
+    assign if_instr_len        = if_instr_compressed ? 3'd2 : 3'd4;
+
+    compressed_decoder u_compressed_decoder (
+        .compressed_instr (imem_rdata[15:0]),
+        .expanded_instr   (if_compressed_expanded_instr),
+        .illegal_instr    (if_compressed_illegal)
+    );
+
+    always_comb begin
+        if_instr_to_decode = imem_rdata;
+        if (if_instr_compressed) begin
+            if_instr_to_decode = if_compressed_illegal ? 32'h0000_0000 : if_compressed_expanded_instr;
+        end
+    end
+
+    pmp_checker u_pmp_checker_if (
+        .req_valid       (if_translation_ready && !mmu_i_resp_page_fault),
+        .req_paddr       (mmu_i_resp_paddr),
+        .req_size        (4'd4),
+        .req_is_fetch    (1'b1),
+        .req_is_store    (1'b0),
+        .priv_mode       (csr_priv_mode_cur),
+        .pmpcfg_entries  (csr_pmpcfg_entries),
+        .pmpaddr_entries (csr_pmpaddr_entries),
+        .access_fault    (if_instr_access_fault)
+    );
+
+    if_id_reg u_if_id_reg (
+        .clk                (clk),
+        .rst                (rst),
+        .stall_en           (hu_if_id_stall_en),
+        .flush_en           (hu_if_id_flush_en),
+        .if_pc                  (if_pc_out),
+        .if_instr               (if_instr_to_decode),
+        .if_instr_len           (if_instr_len),
+        .if_instr_access_fault  (if_instr_access_fault),
+        .if_instr_page_fault    (if_instr_page_fault),
+        .id_pc                  (if_id_pc),
+        .id_instr               (if_id_instr),
+        .id_instr_len           (if_id_instr_len),
+        .id_instr_access_fault  (if_id_instr_access_fault),
+        .id_instr_page_fault    (if_id_instr_page_fault)
+    );
+
+    // ---------------------------------------------------------
+    // (C) ID Stage - Instruction Decode & RegFile
+    // ---------------------------------------------------------
+    id_stage u_id_stage (
+        .clk                (clk),
+        .rst                (rst),
+        .if_pc              (if_id_pc),
+        .if_instr           (if_id_instr),
+        .if_instr_len       (if_id_instr_len),
+        .csr_priv_mode      (csr_priv_mode_cur),
+        .csr_mstatus_tsr    (csr_mstatus_value[CSR_MSTATUS_TSR_BIT]),
+        .csr_mstatus_tw     (csr_mstatus_value[CSR_MSTATUS_TW_BIT]),
+        .csr_mstatus_tvm    (csr_mstatus_value[CSR_MSTATUS_TVM_BIT]),
+        .wb_reg_write_en    (wb_reg_write_en_out),
+        .wb_rd_addr         (wb_rd_addr),
+        .wb_rd_wdata        (wb_rd_wdata),
+        .wb_fp_write_en     (wb_fp_write_en),
+        .wb_fd_addr         (wb_fd_addr),
+        .wb_fd_wdata        (wb_fd_wdata),
+        .id_pc              (id_pc_out),
+        .id_instr_len       (id_instr_len),
+        .id_rs1_rdata       (id_rs1_rdata),
+        .id_rs2_rdata       (id_rs2_rdata),
+        .id_fs1_rdata       (id_fs1_rdata),
+        .id_fs2_rdata       (id_fs2_rdata),
+        .id_fs3_rdata       (id_fs3_rdata),
+        .id_rs1_addr        (id_rs1_addr),
+        .id_rs2_addr        (id_rs2_addr),
+        .id_fs3_addr        (id_fs3_addr),
+        .id_imm             (id_imm),
+        .id_alu_op          (id_alu_op),
+        .id_alu_src1_sel    (id_alu_src1_sel),
+        .id_alu_src2_sel    (id_alu_src2_sel),
+        .id_is_word_op      (id_is_word_op),
+        .id_is_branch       (id_is_branch),
+        .id_branch_op       (id_branch_op),
+        .id_is_jump         (id_is_jump),
+        .id_is_load         (id_is_load),
+        .id_is_store        (id_is_store),
+        .id_mem_size        (id_mem_size),
+        .id_is_amo          (id_is_amo),
+        .id_amo_op          (id_amo_op),
+        .id_is_fp           (id_is_fp),
+        .id_fp_reg_write_en (id_fp_reg_write_en),
+        .id_fp_store_data_sel(id_fp_store_data_sel),
+        .id_fp_op           (id_fp_op),
+        .id_fp_fmt          (id_fp_fmt),
+        .id_fp_rm           (id_fp_rm),
+        .id_reg_write_en    (id_reg_write_en),
+        .id_rd_addr         (id_rd_addr),
+        .id_wb_sel          (id_wb_sel),
+        .id_is_csr          (id_is_csr),
+        .id_csr_op          (id_csr_op),
+        .id_illegal_instr   (id_illegal_instr),
+        .id_is_ecall        (id_is_ecall),
+        .id_is_ebreak       (id_is_ebreak),
+        .id_is_mret         (id_is_mret),
+        .id_is_sret         (id_is_sret),
+        .id_is_wfi          (id_is_wfi),
+        .id_is_sfence_vma   (id_is_sfence_vma),
+        .id_is_fence        (id_is_fence),
+        .id_is_fence_i      (id_is_fence_i)
+    );
+
+    id_ex_reg u_id_ex_reg (
+        .clk                (clk),
+        .rst                (rst),
+        .stall_en           (hu_id_ex_stall_en),
+        .flush_en           (hu_id_ex_flush_en),
+        .id_instr           (if_id_instr),
+        .id_instr_len       (id_instr_len),
+        .id_pc              (id_pc_out),
+        .id_rs1_rdata       (id_rs1_rdata),
+        .id_rs2_rdata       (id_rs2_rdata),
+        .id_fs1_rdata       (id_fs1_rdata),
+        .id_fs2_rdata       (id_fs2_rdata),
+        .id_fs3_rdata       (id_fs3_rdata),
+        .id_rs1_addr        (id_rs1_addr),
+        .id_rs2_addr        (id_rs2_addr),
+        .id_fs3_addr        (id_fs3_addr),
+        .id_imm             (id_imm),
+        .id_alu_op          (id_alu_op),
+        .id_alu_src1_sel    (id_alu_src1_sel),
+        .id_alu_src2_sel    (id_alu_src2_sel),
+        .id_is_word_op      (id_is_word_op),
+        .id_is_branch       (id_is_branch),
+        .id_branch_op       (id_branch_op),
+        .id_is_jump         (id_is_jump),
+        .id_is_load         (id_is_load),
+        .id_is_store        (id_is_store),
+        .id_mem_size        (id_mem_size),
+        .id_is_amo          (id_is_amo),
+        .id_amo_op          (id_amo_op),
+        .id_is_fp           (id_is_fp),
+        .id_fp_reg_write_en (id_fp_reg_write_en),
+        .id_fp_store_data_sel(id_fp_store_data_sel),
+        .id_fp_op           (id_fp_op),
+        .id_fp_fmt          (id_fp_fmt),
+        .id_fp_rm           (id_fp_rm),
+        .id_reg_write_en    (id_reg_write_en),
+        .id_rd_addr         (id_rd_addr),
+        .id_wb_sel          (id_wb_sel),
+        .id_is_csr              (id_is_csr),
+        .id_csr_op              (id_csr_op),
+        .id_illegal_instr       (id_illegal_instr),
+        .id_instr_access_fault  (if_id_instr_access_fault),
+        .id_instr_page_fault    (if_id_instr_page_fault),
+        .id_is_ecall            (id_is_ecall),
+        .id_is_ebreak       (id_is_ebreak),
+        .id_is_mret         (id_is_mret),
+        .id_is_sret         (id_is_sret),
+        .id_is_wfi          (id_is_wfi),
+        .id_is_sfence_vma   (id_is_sfence_vma),
+        .id_is_fence        (id_is_fence),
+        .id_is_fence_i      (id_is_fence_i),
+        .ex_pc              (ex_pc_in),
+        .ex_instr           (ex_instr_in),
+        .ex_instr_len       (ex_instr_len_in),
+        .ex_rs1_rdata       (ex_rs1_rdata_in),
+        .ex_rs2_rdata       (ex_rs2_rdata_in),
+        .ex_fs1_rdata       (ex_fs1_rdata_in),
+        .ex_fs2_rdata       (ex_fs2_rdata_in),
+        .ex_fs3_rdata       (ex_fs3_rdata_in),
+        .ex_rs1_addr        (ex_rs1_addr_in),
+        .ex_rs2_addr        (ex_rs2_addr_in),
+        .ex_fs3_addr        (ex_fs3_addr_in),
+        .ex_imm             (ex_imm_in),
+        .ex_alu_op          (ex_alu_op_in),
+        .ex_alu_src1_sel    (ex_alu_src1_sel_in),
+        .ex_alu_src2_sel    (ex_alu_src2_sel_in),
+        .ex_is_word_op      (ex_is_word_op_in),
+        .ex_is_branch       (ex_is_branch_in),
+        .ex_branch_op       (ex_branch_op_in),
+        .ex_is_jump         (ex_is_jump_in),
+        .ex_is_load         (ex_is_load_in),
+        .ex_is_store        (ex_is_store_in),
+        .ex_mem_size        (ex_mem_size_in),
+        .ex_is_amo          (ex_is_amo_in),
+        .ex_amo_op          (ex_amo_op_in),
+        .ex_is_fp           (ex_is_fp_in),
+        .ex_fp_reg_write_en (ex_fp_reg_write_en_in),
+        .ex_fp_store_data_sel(ex_fp_store_data_sel_in),
+        .ex_fp_op           (ex_fp_op_in),
+        .ex_fp_fmt          (ex_fp_fmt_in),
+        .ex_fp_rm           (ex_fp_rm_in),
+        .ex_reg_write_en    (ex_reg_write_en_in),
+        .ex_rd_addr         (ex_rd_addr_in),
+        .ex_wb_sel          (ex_wb_sel_in),
+        .ex_is_csr              (ex_is_csr_in),
+        .ex_csr_op              (ex_csr_op_in),
+        .ex_illegal_instr       (ex_illegal_instr_in),
+        .ex_instr_access_fault  (ex_instr_access_fault_in),
+        .ex_instr_page_fault    (ex_instr_page_fault_in),
+        .ex_is_ecall            (ex_is_ecall_in),
+        .ex_is_ebreak       (ex_is_ebreak_in),
+        .ex_is_mret         (ex_is_mret_in),
+        .ex_is_sret         (ex_is_sret_in),
+        .ex_is_wfi          (ex_is_wfi_in),
+        .ex_is_sfence_vma   (ex_is_sfence_vma_in),
+        .ex_is_fence        (ex_is_fence_in),
+        .ex_is_fence_i      (ex_is_fence_i_in)
+    );
+
+    // ---------------------------------------------------------
+    // (D) EX Stage - Execution & Forwarding
+    // ---------------------------------------------------------
+
+    always_comb begin
+        unique case (mem_wb_sel_in)
+            2'b00:   mem_forward_data = mem_alu_result_in;
+            2'b01:   mem_forward_data = mw_reg_mem_rdata_in;
+            2'b10:   mem_forward_data = mem_pc_in + 64'd4;
+            default: mem_forward_data = '0;
+        endcase
+    end
+    
+    // Operand A Forwarding Mux
+    always_comb begin
+        case (hu_forward_a_sel)
+            2'b01:   ex_rs1_rdata_fwd = mem_forward_data;  // From MEM
+            2'b10:   ex_rs1_rdata_fwd = wb_rd_wdata;       // From WB
+            default: ex_rs1_rdata_fwd = ex_rs1_rdata_in;   // From RegFile
+        endcase
+    end
+
+    // Operand B Forwarding Mux
+    always_comb begin
+        case (hu_forward_b_sel)
+            2'b01:   ex_rs2_rdata_fwd = mem_forward_data;  // From MEM
+            2'b10:   ex_rs2_rdata_fwd = wb_rd_wdata;       // From WB
+            default: ex_rs2_rdata_fwd = ex_rs2_rdata_in;   // From RegFile
+        endcase
+    end
+
+    always_comb begin
+        if (mem_fp_reg_write_en_in && (mem_rd_addr_in == ex_rs1_addr_in)) begin
+            ex_fs1_rdata_fwd = mw_reg_fp_result_in;
+        end else if (wb_fp_write_en && (wb_fd_addr == ex_rs1_addr_in)) begin
+            ex_fs1_rdata_fwd = wb_fd_wdata;
+        end else begin
+            ex_fs1_rdata_fwd = ex_fs1_rdata_in;
+        end
+    end
+
+    always_comb begin
+        if (mem_fp_reg_write_en_in && (mem_rd_addr_in == ex_rs2_addr_in)) begin
+            ex_fs2_rdata_fwd = mw_reg_fp_result_in;
+        end else if (wb_fp_write_en && (wb_fd_addr == ex_rs2_addr_in)) begin
+            ex_fs2_rdata_fwd = wb_fd_wdata;
+        end else begin
+            ex_fs2_rdata_fwd = ex_fs2_rdata_in;
+        end
+    end
+
+    always_comb begin
+        if (mem_fp_reg_write_en_in && (mem_rd_addr_in == ex_fs3_addr_in)) begin
+            ex_fs3_rdata_fwd = mw_reg_fp_result_in;
+        end else if (wb_fp_write_en && (wb_fd_addr == ex_fs3_addr_in)) begin
+            ex_fs3_rdata_fwd = wb_fd_wdata;
+        end else begin
+            ex_fs3_rdata_fwd = ex_fs3_rdata_in;
+        end
+    end
+
+    ex_stage u_ex_stage (
+        .ex_pc              (ex_pc_in),
+        .ex_rs1_rdata       (ex_rs1_rdata_fwd),
+        .ex_rs2_rdata       (ex_rs2_rdata_fwd),
+        .ex_imm             (ex_imm_in),
+        .ex_alu_op          (ex_alu_op_in),
+        .ex_alu_src1_sel    (ex_alu_src1_sel_in),
+        .ex_alu_src2_sel    (ex_alu_src2_sel_in),
+        .ex_is_branch       (ex_is_branch_in),
+        .ex_branch_op       (ex_branch_op_in),
+        .ex_is_jump         (ex_is_jump_in),
+        .ex_is_word_op      (ex_is_word_op_in),
+        .ex_alu_result      (ex_alu_result_out),
+        .ex_branch_taken    (ex_branch_taken_out),
+        .ex_branch_target   (ex_branch_target_out)
+    );
+
+    fpu u_fpu_unit (
+        .fp_op          (ex_fp_op_in),
+        .fp_fmt         (ex_fp_fmt_in),
+        .rm             ((ex_fp_rm_in == 3'b111) ? csr_frm_value : ex_fp_rm_in),
+        .funct3         (ex_instr_in[14:12]),
+        .rs2_addr       (ex_rs2_addr_in),
+        .fs1_data       (ex_fs1_rdata_fwd),
+        .fs2_data       (ex_fs2_rdata_fwd),
+        .fs3_data       (ex_fs3_rdata_fwd),
+        .int_rs1_data   (ex_rs1_rdata_fwd),
+        .fp_result      (ex_fp_result_out),
+        .int_result     (ex_fp_int_result_out),
+        .fflags         (ex_fp_fflags_out),
+        .illegal_instr  (ex_fp_illegal_out)
+    );
+
+    assign ex_result_to_mem = (ex_is_fp_in && ex_reg_write_en_in) ?
+                              ex_fp_int_result_out : ex_alu_result_out;
+
+    ex_mem_reg u_ex_mem_reg (
+        .clk                (clk),
+        .rst                (rst),
+        .stall_en           (hu_ex_mem_stall_en),
+        .flush_en           (trap_flush_req),
+        .ex_alu_result      (ex_result_to_mem),
+        .ex_branch_taken    (ex_branch_taken_out),
+        .ex_branch_target   (ex_branch_target_out),
+        .ex_rs1_rdata       (ex_rs1_rdata_fwd),
+        .ex_rs2_rdata       (ex_rs2_rdata_fwd),
+        .ex_fs1_rdata       (ex_fs1_rdata_fwd),
+        .ex_fs2_rdata       (ex_fs2_rdata_fwd),
+        .ex_fs3_rdata       (ex_fs3_rdata_fwd),
+        .ex_rs1_addr        (ex_rs1_addr_in),
+        .ex_fs3_addr        (ex_fs3_addr_in),
+        .ex_pc              (ex_pc_in),
+        .ex_imm             (ex_imm_in),
+        .ex_instr           (ex_instr_in),
+        .ex_instr_len       (ex_instr_len_in),
+        .ex_is_load         (ex_is_load_in),
+        .ex_is_store        (ex_is_store_in),
+        .ex_mem_size        (ex_mem_size_in),
+        .ex_is_amo          (ex_is_amo_in),
+        .ex_amo_op          (ex_amo_op_in),
+        .ex_is_fp           (ex_is_fp_in),
+        .ex_fp_reg_write_en (ex_fp_reg_write_en_in),
+        .ex_fp_store_data_sel(ex_fp_store_data_sel_in),
+        .ex_fp_op           (ex_fp_op_in),
+        .ex_fp_fmt          (ex_fp_fmt_in),
+        .ex_fp_rm           (ex_fp_rm_in),
+        .ex_fp_result       (ex_fp_result_out),
+        .ex_fp_fflags       (ex_fp_fflags_out),
+        .ex_reg_write_en    (ex_reg_write_en_in),
+        .ex_rd_addr         (ex_rd_addr_in),
+        .ex_wb_sel          (ex_wb_sel_in),
+        .ex_is_csr          (ex_is_csr_in),
+        .ex_csr_op          (ex_csr_op_in),
+        .ex_illegal_instr   (ex_illegal_instr_in | (ex_is_fp_in & ex_fp_illegal_out)),
+        .ex_instr_access_fault(ex_instr_access_fault_in),
+        .ex_instr_page_fault(ex_instr_page_fault_in),
+        .ex_is_ecall        (ex_is_ecall_in),
+        .ex_is_ebreak       (ex_is_ebreak_in),
+        .ex_is_mret         (ex_is_mret_in),
+        .ex_is_sret         (ex_is_sret_in),
+        .ex_is_wfi          (ex_is_wfi_in),
+        .ex_is_sfence_vma   (ex_is_sfence_vma_in),
+        .ex_is_fence        (ex_is_fence_in),
+        .ex_is_fence_i      (ex_is_fence_i_in),
+        .mem_alu_result      (mem_alu_result_in),
+        .mem_branch_taken    (mem_branch_taken_in),
+        .mem_branch_target   (mem_branch_target_in),
+        .mem_rs1_rdata       (mem_rs1_rdata_in),
+        .mem_rs2_rdata       (mem_rs2_rdata_in),
+        .mem_fs1_rdata       (mem_fs1_rdata_in),
+        .mem_fs2_rdata       (mem_fs2_rdata_in),
+        .mem_fs3_rdata       (mem_fs3_rdata_in),
+        .mem_rs1_addr        (mem_rs1_addr_in),
+        .mem_fs3_addr        (mem_fs3_addr_in),
+        .mem_pc              (mem_pc_in),
+        .mem_imm             (mem_imm_in),
+        .mem_instr           (mem_instr_in),
+        .mem_instr_len       (mem_instr_len_in),
+        .mem_is_load         (mem_is_load_in),
+        .mem_is_store        (mem_is_store_in),
+        .mem_mem_size        (mem_mem_size_in),
+        .mem_is_amo          (mem_is_amo_in),
+        .mem_amo_op          (mem_amo_op_in),
+        .mem_is_fp           (mem_is_fp_in),
+        .mem_fp_reg_write_en (mem_fp_reg_write_en_in),
+        .mem_fp_store_data_sel(mem_fp_store_data_sel_in),
+        .mem_fp_op           (mem_fp_op_in),
+        .mem_fp_fmt          (mem_fp_fmt_in),
+        .mem_fp_rm           (mem_fp_rm_in),
+        .mem_fp_result       (mem_fp_result_in),
+        .mem_fp_fflags       (mem_fp_fflags_in),
+        .mem_reg_write_en    (mem_reg_write_en_in),
+        .mem_rd_addr         (mem_rd_addr_in),
+        .mem_wb_sel          (mem_wb_sel_in),
+        .mem_is_csr          (mem_is_csr_in),
+        .mem_csr_op          (mem_csr_op_in),
+        .mem_illegal_instr   (mem_illegal_instr_in),
+        .mem_instr_access_fault(mem_instr_access_fault_in),
+        .mem_instr_page_fault(mem_instr_page_fault_in),
+        .mem_is_ecall        (mem_is_ecall_in),
+        .mem_is_ebreak       (mem_is_ebreak_in),
+        .mem_is_mret         (mem_is_mret_in),
+        .mem_is_sret         (mem_is_sret_in),
+        .mem_is_wfi          (mem_is_wfi_in),
+        .mem_is_sfence_vma   (mem_is_sfence_vma_in),
+        .mem_is_fence        (mem_is_fence_in),
+        .mem_is_fence_i      (mem_is_fence_i_in)
+    );
+
+    // ---------------------------------------------------------
+    // (E) MEM Stage - Memory Access
+    // ---------------------------------------------------------
+    pmp_checker u_pmp_checker_dmem (
+        .req_valid       (data_access_needs_translation && mmu_d_resp_valid && !mmu_d_resp_page_fault),
+        .req_paddr       (mmu_d_resp_paddr),
+        .req_size        (dmem_access_size),
+        .req_is_fetch    (1'b0),
+        .req_is_store    (dmem_req_is_store),
+        .priv_mode       (csr_priv_mode_cur),
+        .pmpcfg_entries  (csr_pmpcfg_entries),
+        .pmpaddr_entries (csr_pmpaddr_entries),
+        .access_fault    (dmem_access_fault)
+    );
+
+    assign dmem_load_access_fault  = dmem_access_fault &
+                                     (mem_is_load_in | (mem_is_amo_in && (mem_amo_op_in == AMO_LR)));
+    assign dmem_store_access_fault = dmem_access_fault & dmem_req_is_store;
+
+    mem_stage u_mem_stage (
+        .clk                (clk),
+        .rst                (rst),
+        .mem_alu_result      (mem_alu_result_in),
+        .mem_branch_taken    (mem_branch_taken_in),
+        .mem_branch_target   (mem_branch_target_in),
+        .mem_rs1_rdata       (mem_rs1_rdata_in),
+        .mem_rs2_rdata       (mem_rs2_rdata_in),
+        .mem_fs2_rdata       (mem_fs2_rdata_in),
+        .mem_rs1_addr        (mem_rs1_addr_in),
+        .mem_pc              (mem_pc_in),
+        .mem_imm             (mem_imm_in),
+        .mem_instr           (mem_instr_in),
+        .mem_instr_len       (mem_instr_len_in),
+        .mem_is_load         (mem_is_load_in),
+        .mem_is_store        (mem_is_store_in),
+        .mem_mem_size        (mem_mem_size_in),
+        .mem_is_amo          (mem_is_amo_in),
+        .mem_amo_op          (mem_amo_op_in),
+        .mem_is_fp           (mem_is_fp_in),
+        .mem_fp_reg_write_en (mem_fp_reg_write_en_in),
+        .mem_fp_store_data_sel(mem_fp_store_data_sel_in),
+        .mem_fp_result       (mem_fp_result_in),
+        .mem_fp_fflags       (mem_fp_fflags_in),
+        .mem_reg_write_en    (mem_reg_write_en_in),
+        .mem_rd_addr         (mem_rd_addr_in),
+        .mem_wb_sel          (mem_wb_sel_in),
+        .mem_is_csr          (mem_is_csr_in),
+        .mem_csr_op          (mem_csr_op_in),
+        .mem_illegal_instr   (mem_illegal_instr_in),
+        .mem_instr_access_fault(mem_instr_access_fault_in),
+        .mem_instr_page_fault(mem_instr_page_fault_in),
+        .mem_is_ecall        (mem_is_ecall_in),
+        .mem_is_ebreak       (mem_is_ebreak_in),
+        .mem_is_mret         (mem_is_mret_in),
+        .mem_is_sret         (mem_is_sret_in),
+        .mem_is_wfi          (mem_is_wfi_in),
+        .mem_is_sfence_vma   (mem_is_sfence_vma_in),
+        .mem_is_fence        (mem_is_fence_in),
+        .mem_is_fence_i      (mem_is_fence_i_in),
+        .dmem_read_req       (raw_dmem_read_en),
+        .dmem_write_req      (raw_dmem_write_en),
+        .dmem_addr           (raw_dmem_addr),
+        .dmem_wdata          (raw_dmem_wdata),
+        .dmem_wstrb          (raw_dmem_wstrb),
+        .dmem_req_ready      (data_translation_ready && !data_translation_page_fault &&
+                              data_memory_port_available),
+        .dmem_rsp_valid      (data_translation_ready && !data_translation_page_fault &&
+                              data_memory_port_available),
+        .dmem_rdata          (dmem_rdata),
+        .dmem_load_access_fault(dmem_load_access_fault),
+        .dmem_store_access_fault(dmem_store_access_fault),
+        .dmem_load_page_fault(dmem_load_page_fault),
+        .dmem_store_page_fault(dmem_store_page_fault),
+        .mem_wb_alu_result   (mw_reg_alu_result_in),
+        .mem_wb_mem_rdata    (mw_reg_mem_rdata_in),
+        .mem_wb_fp_result    (mw_reg_fp_result_in),
+        .mem_wb_fp_fflags    (mw_reg_fp_fflags_in),
+        .mem_wb_pc           (mw_reg_pc_in),
+        .mem_wb_instr        (mw_reg_instr_in),
+        .mem_wb_instr_len    (mw_reg_instr_len_in),
+        .mem_wb_reg_write_en (mw_reg_reg_write_en_in),
+        .mem_wb_fp_reg_write_en(mw_reg_fp_reg_write_en_in),
+        .mem_wb_rd_addr      (mw_reg_rd_addr_in),
+        .mem_wb_wb_sel       (mw_reg_wb_sel_in),
+        .mem_wb_is_csr       (mw_reg_is_csr_in),
+        .mem_wb_csr_op       (mw_reg_csr_op_in),
+        .mem_wb_rs1_rdata    (mw_reg_rs1_rdata_in),
+        .mem_wb_rs1_addr     (mw_reg_rs1_addr_in),
+        .mem_wb_imm          (mw_reg_imm_in),
+        .mem_wb_illegal_instr(mw_reg_illegal_instr_in),
+        .mem_wb_instr_access_fault(mw_reg_instr_access_fault_in),
+        .mem_wb_instr_page_fault(mw_reg_instr_page_fault_in),
+        .mem_wb_is_ecall     (mw_reg_is_ecall_in),
+        .mem_wb_is_ebreak    (mw_reg_is_ebreak_in),
+        .mem_wb_is_mret      (mw_reg_is_mret_in),
+        .mem_wb_is_sret      (mw_reg_is_sret_in),
+        .mem_wb_load_fault   (mw_reg_load_fault_in),
+        .mem_wb_store_fault  (mw_reg_store_fault_in),
+        .mem_wb_load_page_fault(mw_reg_load_page_fault_in),
+        .mem_wb_store_page_fault(mw_reg_store_page_fault_in),
+        .mem_wb_fault_addr   (mw_reg_fault_addr_in),
+        .mem_stall_req       (mem_stall_req)
+    );
+
+    always_comb begin
+        if (ptw_mem_req_valid) begin
+            dmem_read_en  = 1'b1;
+            dmem_write_en = 1'b0;
+            dmem_addr     = ptw_mem_req_addr;
+            dmem_wdata    = '0;
+            dmem_size     = 3'b011;
+        end else begin
+            dmem_read_en  = raw_dmem_read_en && data_translation_ready &&
+                            !data_translation_page_fault;
+            dmem_write_en = raw_dmem_write_en && data_translation_ready &&
+                            !data_translation_page_fault;
+            dmem_addr     = data_access_needs_translation ? mmu_d_resp_paddr : raw_dmem_addr;
+            dmem_wdata    = raw_dmem_wdata;
+            dmem_size     = mem_mem_size_in;
+        end
+    end
+
+    mem_wb_reg u_mem_wb_reg (
+        .clk                 (clk),
+        .rst                 (rst),
+        .stall_en            (1'b0),
+        .flush_en            (trap_flush_req | mem_stall_req),
+        .mem_pc              (mw_reg_pc_in),
+        .mem_alu_result      (mw_reg_alu_result_in),
+        .mem_mem_rdata       (mw_reg_mem_rdata_in),
+        .mem_fp_result       (mw_reg_fp_result_in),
+        .mem_fp_fflags       (mw_reg_fp_fflags_in),
+        .mem_instr           (mw_reg_instr_in),
+        .mem_instr_len       (mw_reg_instr_len_in),
+        .mem_reg_write_en    (mw_reg_reg_write_en_in),
+        .mem_fp_reg_write_en (mw_reg_fp_reg_write_en_in),
+        .mem_rd_addr         (mw_reg_rd_addr_in),
+        .mem_wb_sel          (mw_reg_wb_sel_in),
+        .mem_is_csr          (mw_reg_is_csr_in),
+        .mem_csr_op          (mw_reg_csr_op_in),
+        .mem_rs1_rdata       (mw_reg_rs1_rdata_in),
+        .mem_rs1_addr        (mw_reg_rs1_addr_in),
+        .mem_imm             (mw_reg_imm_in),
+        .mem_illegal_instr   (mw_reg_illegal_instr_in),
+        .mem_instr_access_fault(mw_reg_instr_access_fault_in),
+        .mem_instr_page_fault(mw_reg_instr_page_fault_in),
+        .mem_is_ecall        (mw_reg_is_ecall_in),
+        .mem_is_ebreak       (mw_reg_is_ebreak_in),
+        .mem_is_mret         (mw_reg_is_mret_in),
+        .mem_is_sret         (mw_reg_is_sret_in),
+        .mem_load_fault      (mw_reg_load_fault_in),
+        .mem_store_fault     (mw_reg_store_fault_in),
+        .mem_load_page_fault (mw_reg_load_page_fault_in),
+        .mem_store_page_fault(mw_reg_store_page_fault_in),
+        .mem_fault_addr      (mw_reg_fault_addr_in),
+        .wb_pc               (wb_pc_in),
+        .wb_alu_result       (wb_alu_result_in),
+        .wb_mem_rdata        (wb_mem_rdata_in),
+        .wb_fp_result        (wb_fp_result_in),
+        .wb_fp_fflags        (wb_fp_fflags_in),
+        .wb_instr            (wb_instr_in),
+        .wb_instr_len        (wb_instr_len_in),
+        .wb_reg_write_en     (wb_reg_write_en_in),
+        .wb_fp_reg_write_en  (wb_fp_reg_write_en_in),
+        .wb_rd_addr          (wb_rd_addr_in),
+        .wb_wb_sel           (wb_wb_sel_in),
+        .wb_is_csr           (wb_is_csr_in),
+        .wb_csr_op           (wb_csr_op_in),
+        .wb_rs1_rdata        (wb_rs1_rdata_in),
+        .wb_rs1_addr         (wb_rs1_addr_in),
+        .wb_imm              (wb_imm_in),
+        .wb_illegal_instr    (wb_illegal_instr_in),
+        .wb_instr_access_fault(wb_instr_access_fault),
+        .wb_instr_page_fault (wb_instr_page_fault),
+        .wb_is_ecall         (wb_is_ecall_in),
+        .wb_is_ebreak        (wb_is_ebreak_in),
+        .wb_is_mret          (wb_is_mret_in),
+        .wb_is_sret          (wb_is_sret_in),
+        .wb_load_fault       (wb_load_fault),
+        .wb_store_fault      (wb_store_fault),
+        .wb_load_page_fault  (wb_load_page_fault),
+        .wb_store_page_fault (wb_store_page_fault),
+        .wb_fault_addr       (wb_fault_addr_in)
+    );
+
+    // ---------------------------------------------------------
+    // (F) WB Stage - Write Back & System Interaction
+    // ---------------------------------------------------------
+    wb_stage u_wb_stage (
+        .mem_wb_alu_result  (wb_alu_result_in),
+        .mem_wb_mem_rdata   (wb_mem_rdata_in),
+        .mem_wb_fp_result   (wb_fp_result_in),
+        .mem_wb_fp_fflags   (wb_fp_fflags_in),
+        .mem_wb_pc          (wb_pc_in),
+        .mem_wb_pc_plus_4   (wb_pc_in + {{(XLEN-3){1'b0}}, wb_instr_len_in}),
+        .mem_wb_reg_write_en(wb_reg_write_en_in),
+        .mem_wb_fp_reg_write_en(wb_fp_reg_write_en_in),
+        .mem_wb_rd_addr     (wb_rd_addr_in),
+        .mem_wb_wb_sel      (wb_wb_sel_in),
+        .mem_wb_is_csr      (wb_is_csr_in),
+        .mem_wb_csr_op      (wb_csr_op_in),
+        .mem_wb_rs1_rdata   (wb_rs1_rdata_in),
+        .mem_wb_rs1_addr    (wb_rs1_addr_in),
+        .mem_wb_imm         (wb_imm_in),
+        .mem_wb_instr       (wb_instr_in),
+        .mem_wb_illegal_instr(wb_illegal_instr_in),
+        .mem_wb_instr_access_fault(wb_instr_access_fault),
+        .mem_wb_instr_page_fault(wb_instr_page_fault),
+        .mem_wb_is_ecall    (wb_is_ecall_in),
+        .mem_wb_is_ebreak   (wb_is_ebreak_in),
+        .mem_wb_is_mret     (wb_is_mret_in),
+        .mem_wb_is_sret     (wb_is_sret_in),
+        .mem_wb_load_fault  (wb_load_fault),
+        .mem_wb_store_fault (wb_store_fault),
+        .mem_wb_load_page_fault(wb_load_page_fault),
+        .mem_wb_store_page_fault(wb_store_page_fault),
+        .mem_wb_fault_addr  (wb_fault_addr_in),
+        .csr_rdata          (csr_rdata),
+        .csr_illegal_access (csr_illegal_access),
+        .wb_rd_wdata        (wb_rd_wdata),
+        .wb_rd_addr         (wb_rd_addr),
+        .wb_reg_write_en    (wb_reg_write_en_out),
+        .wb_fd_wdata        (wb_fd_wdata),
+        .wb_fd_addr         (wb_fd_addr),
+        .wb_fp_write_en     (wb_fp_write_en),
+        .wb_fp_fflags       (wb_fp_fflags),
+        .wb_fp_state_dirty  (wb_fp_state_dirty),
+        .wb_fp_fflags_valid (wb_fp_fflags_valid),
+        .csr_req            (csr_req),
+        .csr_op             (csr_op),
+        .csr_addr           (csr_addr),
+        .csr_rs1_data       (csr_rs1_data),
+        .trap_illegal_instr (trap_illegal_instr),
+        .trap_is_ecall      (trap_is_ecall),
+        .trap_is_ebreak     (trap_is_ebreak),
+        .trap_is_mret       (trap_is_mret),
+        .trap_is_sret       (trap_is_sret),
+        .trap_epc           (trap_epc),
+        .trap_instr_access_fault(trap_instr_access_fault),
+        .trap_instr_page_fault(trap_instr_page_fault),
+        .trap_load_fault    (trap_load_fault),
+        .trap_store_fault   (trap_store_fault),
+        .trap_load_page_fault(trap_load_page_fault),
+        .trap_store_page_fault(trap_store_page_fault),
+        .trap_bad_addr      (trap_bad_addr),
+        .trap_bad_instr     (trap_bad_instr)
+    );
+
+    csr_unit u_csr_unit (
+        .clk                (clk),
+        .rst                (rst),
+        .csr_req            (csr_req),
+        .csr_op             (csr_op),
+        .csr_addr           (csr_addr),
+        .csr_rs1_data       (csr_rs1_data),
+        .trap_ctrl_trigger  (trap_ctrl_trigger),
+        .trap_ctrl_to_s_mode(trap_ctrl_to_s_mode),
+        .trap_ctrl_mepc     (trap_ctrl_mepc),
+        .trap_ctrl_mcause   (trap_ctrl_mcause),
+        .trap_ctrl_mtval    (trap_ctrl_mtval),
+        .trap_ctrl_mret_en  (trap_ctrl_mret_en),
+        .trap_ctrl_sret_en  (trap_ctrl_sret_en),
+        .m_timer_interrupt(m_timer_interrupt),
+        .m_software_interrupt(m_software_interrupt),
+        .m_external_interrupt(m_external_interrupt),
+        .s_timer_interrupt  (s_timer_interrupt),
+        .s_software_interrupt(s_software_interrupt),
+        .s_external_interrupt(s_external_interrupt),
+        .fp_state_dirty     (wb_fp_state_dirty),
+        .fp_fflags_valid    (wb_fp_fflags_valid),
+        .fp_fflags          (wb_fp_fflags),
+        .csr_rdata          (csr_rdata),
+        .csr_mstatus        (csr_mstatus_value),
+        .csr_sstatus        (csr_sstatus_value),
+        .csr_medeleg        (csr_medeleg_value),
+        .csr_mideleg        (csr_mideleg_value),
+        .csr_mie            (csr_mie_value),
+        .csr_sie            (csr_sie_value),
+        .csr_mip            (csr_mip_value),
+        .csr_sip            (csr_sip_value),
+        .csr_mtvec          (csr_mtvec_base),
+        .csr_stvec          (csr_stvec_base),
+        .csr_mepc           (csr_mepc_value),
+        .csr_sepc           (csr_sepc_value),
+        .csr_satp           (csr_satp_value),
+        .csr_frm            (csr_frm_value),
+        .csr_priv_mode          (csr_priv_mode_cur),
+        .csr_trap_vector        (),
+        .csr_trap_to_s_mode     (),
+        .csr_trap_vector_next   (csr_trap_vector_value),
+        .csr_trap_to_s_mode_next(csr_trap_to_s_mode),
+        .csr_pmpcfg_entries     (csr_pmpcfg_entries),
+        .csr_pmpaddr_entries    (csr_pmpaddr_entries),
+        .csr_illegal_access     (csr_illegal_access)
+    );
+
+    trap_controller u_trap_ctrl (
+        .wb_pc               (wb_pc_in),
+        .trap_illegal_instr  (trap_illegal_instr),
+        .trap_is_ecall       (trap_is_ecall),
+        .trap_is_ebreak      (trap_is_ebreak),
+        .trap_is_mret        (trap_is_mret),
+        .trap_is_sret        (trap_is_sret),
+        .trap_instr_access_fault(trap_instr_access_fault),
+        .trap_load_fault     (trap_load_fault),
+        .trap_store_fault    (trap_store_fault),
+        .trap_instr_page_fault(trap_instr_page_fault),
+        .trap_load_page_fault(trap_load_page_fault),
+        .trap_store_page_fault(trap_store_page_fault),
+        .trap_bad_addr       (trap_bad_addr),
+        .trap_bad_instr      (trap_bad_instr),
+        .csr_mstatus_mie     (csr_mstatus_mie),
+        .csr_mie             (csr_mie_value),
+        .csr_sie             (csr_sie_value),
+        .csr_mip             (csr_mip_value),
+        .csr_sip             (csr_sip_value),
+        .csr_mstatus         (csr_mstatus_value),
+        .csr_mtvec           (csr_mtvec_base),
+        .csr_stvec           (csr_stvec_base),
+        .csr_mepc            (csr_mepc_value),
+        .csr_sepc            (csr_sepc_value),
+        .csr_medeleg         (csr_medeleg_value),
+        .csr_mideleg         (csr_mideleg_value),
+        .csr_priv_mode       (csr_priv_mode_cur),
+        .trap_trigger        (trap_ctrl_trigger),
+        .trap_mepc           (trap_ctrl_mepc),
+        .trap_mcause         (trap_ctrl_mcause),
+        .trap_mtval          (trap_ctrl_mtval),
+        .trap_to_s_mode      (trap_ctrl_to_s_mode),
+        .mret_en             (trap_ctrl_mret_en),
+        .sret_en             (trap_ctrl_sret_en),
+        .trap_flush_req      (trap_flush_req),
+        .trap_pc_sel         (trap_pc_sel_raw),
+        .trap_next_pc        (trap_next_pc_raw)
+    );
+
+    assign trap_pc_sel = trap_pc_sel_raw;
+    assign trap_next_pc = trap_next_pc_raw;
+
+endmodule