feat: Complete CSR/privileged/exception interrupt path and fill in strict acceptance tests

- Refactor trap/CSR data path in the Core, unified with trap_ctrl interface to drive CSR update and return process
- Improve critical paths for exception/interrupt handling: ecall, illegal, load fault, external interrupt, mret/sret return
- Adjust pipeline stage registers with flush/stall coordination, fix residual control signal issues in trap scenarios
- Enhance decode and related pipeline module behavior consistency, fill in missing control signal propagation across registers
- Expand core integration testing, add trap/interrupt scenarios and CLI dispatch that can be executed per case
- Extend CSR unit test coverage: read/write semantics, privilege switching, delegation and nested returns, illegal access protection, etc.
- Register new core scenario cases in CTest to form a regression-capable P2 strict acceptance suite

Verification:

- Build passed
- Full CTest passed (22/22)

6 files changed, 434 insertions, 59 deletions

diff --git a/rv64cpu/tb/unit/tb_csr.cc b/rv64cpu/tb/unit/tb_csr.cc
index 95d6be7..9dce96f 100644
--- a/rv64cpu/tb/unit/tb_csr.cc
+++ b/rv64cpu/tb/unit/tb_csr.cc
@@ -17,6 +17,7 @@
 // File path: tb/unit/tb_csr.cc
 // Description: CSR unit testbench
 
+#include <array>
 #include <cassert>
 #include <cstdint>
 #include <iostream>
@@ -61,6 +62,13 @@ constexpr uint16_t CSR_FCSR      = 0x003;
 constexpr uint64_t EXC_INST_ILLEGAL = 2ULL;
 constexpr uint64_t EXC_ECALL_FROM_U = 8ULL;
 constexpr uint64_t EXC_ECALL_FROM_S = 9ULL;
+constexpr uint64_t EXC_LOAD_FAULT   = 5ULL;
+constexpr uint64_t EXC_STORE_FAULT  = 7ULL;
+
+constexpr uint64_t INTERRUPT_BIT    = 1ULL << 63;
+constexpr uint64_t INT_M_SOFTWARE   = 3ULL;
+constexpr uint64_t INT_M_TIMER      = 7ULL;
+constexpr uint64_t INT_M_EXTERNAL   = 11ULL;
 
 constexpr int MSTATUS_MIE_BIT  = 3;
 constexpr int MSTATUS_MPIE_BIT = 7;
@@ -91,12 +99,13 @@ void drive_defaults(Vcsr_unit* dut) {
     dut->csr_op = 0;
     dut->csr_addr = 0;
     dut->csr_rs1_data = 0;
-    dut->trap_illegal_instr = 0;
-    dut->trap_is_ecall = 0;
-    dut->trap_is_ebreak = 0;
-    dut->trap_is_mret = 0;
-    dut->trap_is_sret = 0;
-    dut->trap_epc = 0;
+    dut->trap_ctrl_trigger = 0;
+    dut->trap_ctrl_to_s_mode = 0;
+    dut->trap_ctrl_mepc = 0;
+    dut->trap_ctrl_mcause = 0;
+    dut->trap_ctrl_mtval = 0;
+    dut->trap_ctrl_mret_en = 0;
+    dut->trap_ctrl_sret_en = 0;
 }
 
 void apply_reset(Vcsr_unit* dut) {
@@ -107,12 +116,45 @@ void apply_reset(Vcsr_unit* dut) {
 }
 
 void clear_trap_lines(Vcsr_unit* dut) {
-    dut->trap_illegal_instr = 0;
-    dut->trap_is_ecall = 0;
-    dut->trap_is_ebreak = 0;
-    dut->trap_is_mret = 0;
-    dut->trap_is_sret = 0;
-    dut->trap_epc = 0;
+    dut->trap_ctrl_trigger = 0;
+    dut->trap_ctrl_to_s_mode = 0;
+    dut->trap_ctrl_mepc = 0;
+    dut->trap_ctrl_mcause = 0;
+    dut->trap_ctrl_mtval = 0;
+    dut->trap_ctrl_mret_en = 0;
+    dut->trap_ctrl_sret_en = 0;
+}
+
+void trigger_trap(
+    Vcsr_unit* dut,
+    uint64_t mepc,
+    uint64_t mcause,
+    uint64_t mtval,
+    bool to_s_mode
+) {
+    dut->trap_ctrl_trigger = 1;
+    dut->trap_ctrl_mepc = mepc;
+    dut->trap_ctrl_mcause = mcause;
+    dut->trap_ctrl_mtval = mtval;
+    dut->trap_ctrl_to_s_mode = to_s_mode ? 1 : 0;
+    tick(dut);
+    dut->trap_ctrl_trigger = 0;
+    dut->trap_ctrl_mepc = 0;
+    dut->trap_ctrl_mcause = 0;
+    dut->trap_ctrl_mtval = 0;
+    dut->trap_ctrl_to_s_mode = 0;
+}
+
+void trigger_interrupt(Vcsr_unit* dut, uint64_t mepc, uint64_t int_cause) {
+    trigger_trap(dut, mepc, (INTERRUPT_BIT | int_cause), 0ULL, false);
+}
+
+template <typename Fn>
+void run_csr_scenario(Vcsr_unit* dut, const char* name, Fn&& body) {
+    clear_trap_lines(dut);
+    std::cout << "[RUN] CSR scenario: " << name << std::endl;
+    body(dut);
+    std::cout << "[PASS] CSR scenario: " << name << std::endl;
 }
 
 uint64_t read_csr(Vcsr_unit* dut, uint16_t addr) {
@@ -147,9 +189,33 @@ void force_priv_mode_via_mret(Vcsr_unit* dut, uint64_t encoded_mode) {
     mstatus &= ~(0x3ULL << MSTATUS_MPP_LO);
     mstatus |= (encoded_mode << MSTATUS_MPP_LO);
     csr_op(dut, CSR_MSTATUS, CSR_OP_WRITE, mstatus);
-    dut->trap_is_mret = 1;
+    dut->trap_ctrl_mret_en = 1;
     tick(dut);
-    dut->trap_is_mret = 0;
+    dut->trap_ctrl_mret_en = 0;
+}
+
+uint64_t arbitrate_trap_priority(
+    bool ext_external,
+    bool ext_software,
+    bool ext_timer,
+    bool has_sync_exception,
+    uint64_t sync_cause,
+    bool& chose_interrupt
+) {
+    if (ext_external) {
+        chose_interrupt = true;
+        return INTERRUPT_BIT | INT_M_EXTERNAL;
+    }
+    if (ext_software) {
+        chose_interrupt = true;
+        return INTERRUPT_BIT | INT_M_SOFTWARE;
+    }
+    if (ext_timer) {
+        chose_interrupt = true;
+        return INTERRUPT_BIT | INT_M_TIMER;
+    }
+    chose_interrupt = false;
+    return has_sync_exception ? sync_cause : 0ULL;
 }
 
 int main(int argc, char** argv) {
@@ -190,9 +256,9 @@ int main(int argc, char** argv) {
     std::cout << "[PASS] CSRRS/CSRRC honor masks and ignore zero data" << std::endl;
 
     // Read-only CSRs ignore write attempts
-    const uint64_t misa_before = read_csr(dut, CSR_MISA);
+    const uint64_t misa_ro_snapshot = read_csr(dut, CSR_MISA);
     csr_op(dut, CSR_MISA, CSR_OP_WRITE, 0ULL);
-    assert(read_csr(dut, CSR_MISA) == misa_before);
+    assert(read_csr(dut, CSR_MISA) == misa_ro_snapshot);
     std::cout << "[PASS] Read-only CSRs reject writes" << std::endl;
 
     // Canonical aliases for cycle/minstret
@@ -209,11 +275,7 @@ int main(int argc, char** argv) {
     uint64_t mstatus_cfg = read_csr(dut, CSR_MSTATUS) | (1ULL << MSTATUS_MIE_BIT);
     csr_op(dut, CSR_MSTATUS, CSR_OP_WRITE, mstatus_cfg);
     const uint64_t instret_pre_trap = read_csr(dut, CSR_MINSTRET);
-    dut->trap_illegal_instr = 1;
-    dut->trap_epc = 0xDEADULL;
-    tick(dut);
-    dut->trap_illegal_instr = 0;
-    dut->trap_epc = 0;
+    trigger_trap(dut, 0xDEADULL, EXC_INST_ILLEGAL, 0xDEADULL, false);
     assert(read_csr(dut, CSR_MINSTRET) == instret_pre_trap);
     advance_cycles(dut, 1);
     assert(read_csr(dut, CSR_MINSTRET) == instret_pre_trap + 1);
@@ -230,45 +292,37 @@ int main(int argc, char** argv) {
     std::cout << "[PASS] Illegal trap updates mepc/mcause/mtval/mstatus" << std::endl;
 
     // mret restore flow
-    dut->trap_is_mret = 1;
+    dut->trap_ctrl_mret_en = 1;
     tick(dut);
-    dut->trap_is_mret = 0;
-    const uint64_t mstatus_after_mret = read_csr(dut, CSR_MSTATUS);
-    assert(((mstatus_after_mret >> MSTATUS_MIE_BIT) & 1ULL) == 1ULL);
-    assert(((mstatus_after_mret >> MSTATUS_MPIE_BIT) & 1ULL) == 1ULL);
-    assert(((mstatus_after_mret >> MSTATUS_MPP_LO) & 0x3ULL) == PRIV_ENCODE_U);
+    dut->trap_ctrl_mret_en = 0;
+    const uint64_t mstatus_after_mret_reset = read_csr(dut, CSR_MSTATUS);
+    assert(((mstatus_after_mret_reset >> MSTATUS_MIE_BIT) & 1ULL) == 1ULL);
+    assert(((mstatus_after_mret_reset >> MSTATUS_MPIE_BIT) & 1ULL) == 1ULL);
+    assert(((mstatus_after_mret_reset >> MSTATUS_MPP_LO) & 0x3ULL) == PRIV_ENCODE_U);
     std::cout << "[PASS] mret restores interrupt enable bits" << std::endl;
 
     // sret restore flow
     uint64_t sstatus_cfg = (1ULL << SSTATUS_SIE_BIT) | (1ULL << SSTATUS_SPP_BIT);
     csr_op(dut, CSR_SSTATUS, CSR_OP_WRITE, sstatus_cfg);
-    dut->trap_is_sret = 1;
+    dut->trap_ctrl_sret_en = 1;
     tick(dut);
-    dut->trap_is_sret = 0;
-    const uint64_t sstatus_after_sret = read_csr(dut, CSR_SSTATUS);
-    assert(((sstatus_after_sret >> SSTATUS_SIE_BIT) & 1ULL) == 0ULL);
-    assert(((sstatus_after_sret >> SSTATUS_SPIE_BIT) & 1ULL) == 1ULL);
-    assert(((sstatus_after_sret >> SSTATUS_SPP_BIT) & 1ULL) == 0ULL);
+    dut->trap_ctrl_sret_en = 0;
+    const uint64_t sstatus_after_sret_reset = read_csr(dut, CSR_SSTATUS);
+    assert(((sstatus_after_sret_reset >> SSTATUS_SIE_BIT) & 1ULL) == 0ULL);
+    assert(((sstatus_after_sret_reset >> SSTATUS_SPIE_BIT) & 1ULL) == 1ULL);
+    assert(((sstatus_after_sret_reset >> SSTATUS_SPP_BIT) & 1ULL) == 0ULL);
     std::cout << "[PASS] sret updates supervisor status bits" << std::endl;
 
     // Ecall causes depend on current privilege mode
     force_priv_mode_via_mret(dut, PRIV_ENCODE_S);
-    dut->trap_is_ecall = 1;
-    dut->trap_epc = 0x600ULL;
-    tick(dut);
-    dut->trap_is_ecall = 0;
-    dut->trap_epc = 0;
+    trigger_trap(dut, 0x600ULL, EXC_ECALL_FROM_S, 0ULL, false);
     assert(read_csr(dut, CSR_MCAUSE) == EXC_ECALL_FROM_S);
     assert(read_csr(dut, CSR_MTVAL) == 0ULL);
     assert(read_csr(dut, CSR_MEPC) == 0x600ULL);
     assert(((read_csr(dut, CSR_MSTATUS) >> MSTATUS_MPP_LO) & 0x3ULL) == PRIV_ENCODE_S);
 
     force_priv_mode_via_mret(dut, PRIV_ENCODE_U);
-    dut->trap_is_ecall = 1;
-    dut->trap_epc = 0x700ULL;
-    tick(dut);
-    dut->trap_is_ecall = 0;
-    dut->trap_epc = 0;
+    trigger_trap(dut, 0x700ULL, EXC_ECALL_FROM_U, 0ULL, false);
     assert(read_csr(dut, CSR_MCAUSE) == EXC_ECALL_FROM_U);
     assert(read_csr(dut, CSR_MTVAL) == 0ULL);
     assert(read_csr(dut, CSR_MEPC) == 0x700ULL);
@@ -279,18 +333,16 @@ int main(int argc, char** argv) {
     csr_op(dut, CSR_MEDELEG, CSR_OP_WRITE, 0ULL);
     csr_op(dut, CSR_MEDELEG, CSR_OP_SET, (1ULL << EXC_ECALL_FROM_U));
     csr_op(dut, CSR_STVEC, CSR_OP_WRITE, 0x4000ULL);
+    assert(read_csr(dut, CSR_MEDELEG) & (1ULL << EXC_ECALL_FROM_U));
     force_priv_mode_via_mret(dut, PRIV_ENCODE_U);
-    dut->trap_is_ecall = 1;
-    dut->trap_epc = 0x900ULL;
-    tick(dut);
-    dut->trap_is_ecall = 0;
-    dut->trap_epc = 0;
+    assert(dut->csr_priv_mode == PRIV_ENCODE_U);
+    trigger_trap(dut, 0x900ULL, EXC_ECALL_FROM_U, 0ULL, true);
+    assert(dut->csr_trap_to_s_mode);
+    assert(dut->csr_trap_vector == 0x4000ULL);
     assert(read_csr(dut, CSR_SEPC) == 0x900ULL);
     assert(read_csr(dut, CSR_SCAUSE) == EXC_ECALL_FROM_U);
     assert(read_csr(dut, CSR_STVAL) == 0ULL);
     assert(dut->csr_priv_mode == PRIV_ENCODE_S);
-    assert(dut->csr_trap_to_s_mode);
-    assert(dut->csr_trap_vector == 0x4000ULL);
     std::cout << "[PASS] medeleg routes ecall to supervisor" << std::endl;
     force_priv_mode_via_mret(dut, PRIV_ENCODE_M);
 
@@ -326,6 +378,208 @@ int main(int argc, char** argv) {
     assert((read_csr(dut, CSR_FRM) & 0x7ULL) == ((0xABULL >> 5) & 0x7ULL));
     std::cout << "[PASS] FCSR/FM/FFLAGS read/write" << std::endl;
 
+    std::cout << "[INFO] Extending CSR trap coverage" << std::endl;
+
+    run_csr_scenario(dut, "interrupt_priority_matrix", [&](Vcsr_unit* target) {
+        struct PriorityScenario {
+            const char* name;
+            bool ext_external;
+            bool ext_software;
+            bool ext_timer;
+            bool sync_illegal;
+            uint64_t mepc;
+            uint64_t mtval;
+            uint64_t expected_cause;
+        };
+
+        const std::array<PriorityScenario, 5> suite = {{
+            {"external_over_others", true,  true,  true,  true,  0xA000ULL, 0x1111ULL, INTERRUPT_BIT | INT_M_EXTERNAL},
+            {"software_over_timer",  false, true,  true,  true,  0xA040ULL, 0x2222ULL, INTERRUPT_BIT | INT_M_SOFTWARE},
+            {"timer_over_sync",      false, false, true,  true,  0xA080ULL, 0x3333ULL, INTERRUPT_BIT | INT_M_TIMER},
+            {"interrupt_only",       true,  false, false, false, 0xA0C0ULL, 0x0ULL,     INTERRUPT_BIT | INT_M_EXTERNAL},
+            {"sync_fallback",        false, false, false, true,  0xA100ULL, 0x4444ULL, EXC_INST_ILLEGAL}
+        }};
+
+        for (const auto& scenario : suite) {
+            bool chose_interrupt = false;
+            const uint64_t chosen_cause = arbitrate_trap_priority(
+                scenario.ext_external,
+                scenario.ext_software,
+                scenario.ext_timer,
+                scenario.sync_illegal,
+                EXC_INST_ILLEGAL,
+                chose_interrupt
+            );
+            assert(chosen_cause == scenario.expected_cause);
+            trigger_trap(
+                target,
+                scenario.mepc,
+                chosen_cause,
+                scenario.sync_illegal ? scenario.mtval : 0ULL,
+                false
+            );
+            const uint64_t mcause = read_csr(target, CSR_MCAUSE);
+            assert(mcause == scenario.expected_cause);
+            assert(read_csr(target, CSR_MEPC) == scenario.mepc);
+            const bool mcause_is_interrupt = (mcause & INTERRUPT_BIT) != 0ULL;
+            assert(mcause_is_interrupt == chose_interrupt);
+            if (scenario.sync_illegal && !mcause_is_interrupt) {
+                assert(read_csr(target, CSR_MTVAL) == scenario.mtval);
+            }
+            assert(target->csr_priv_mode == PRIV_ENCODE_M);
+        }
+    });
+
+    run_csr_scenario(dut, "delegated_multitrap_context", [&](Vcsr_unit* target) {
+        csr_op(target, CSR_MEDELEG, CSR_OP_WRITE, 0ULL);
+        csr_op(target, CSR_MEDELEG, CSR_OP_SET, (1ULL << EXC_ECALL_FROM_U));
+        csr_op(target, CSR_STVEC, CSR_OP_WRITE, 0x4000ULL);
+        csr_op(target, CSR_MTVEC, CSR_OP_WRITE, 0x8000ULL);
+
+        force_priv_mode_via_mret(target, PRIV_ENCODE_S);
+        assert(target->csr_priv_mode == PRIV_ENCODE_S);
+
+        const uint64_t delegated_pc = 0x900ULL;
+        trigger_trap(target, delegated_pc, EXC_ECALL_FROM_U, 0ULL, true);
+        assert(target->csr_trap_to_s_mode);
+        assert(target->csr_trap_vector == 0x4000ULL);
+        assert(read_csr(target, CSR_SEPC) == delegated_pc);
+        assert(read_csr(target, CSR_SCAUSE) == EXC_ECALL_FROM_U);
+        assert(target->csr_priv_mode == PRIV_ENCODE_S);
+
+        const uint64_t machine_pc = 0xA00ULL;
+        trigger_trap(target, machine_pc, EXC_INST_ILLEGAL, 0ULL, false);
+        assert(!target->csr_trap_to_s_mode);
+        assert(target->csr_trap_vector == 0x8000ULL);
+        assert(read_csr(target, CSR_MEPC) == machine_pc);
+        assert(read_csr(target, CSR_SEPC) == delegated_pc);
+        assert(target->csr_priv_mode == PRIV_ENCODE_M);
+    });
+
+    run_csr_scenario(dut, "nested_return_sequence", [&](Vcsr_unit* target) {
+        force_priv_mode_via_mret(target, PRIV_ENCODE_U);
+        csr_op(target, CSR_SSTATUS, CSR_OP_WRITE, (1ULL << SSTATUS_SIE_BIT));
+
+        const uint64_t first_trap_pc = 0xB00ULL;
+        trigger_trap(target, first_trap_pc, EXC_ECALL_FROM_U, 0ULL, true);
+        uint64_t sstatus_snapshot = read_csr(target, CSR_SSTATUS);
+        assert(((sstatus_snapshot >> SSTATUS_SIE_BIT) & 1ULL) == 0ULL);
+        assert(((sstatus_snapshot >> SSTATUS_SPIE_BIT) & 1ULL) == 1ULL);
+        assert(((sstatus_snapshot >> SSTATUS_SPP_BIT) & 1ULL) == 0ULL);
+        assert(read_csr(target, CSR_SEPC) == first_trap_pc);
+
+        target->trap_ctrl_sret_en = 1;
+        tick(target);
+        target->trap_ctrl_sret_en = 0;
+        uint64_t sstatus_after_sret = read_csr(target, CSR_SSTATUS);
+        assert(((sstatus_after_sret >> SSTATUS_SIE_BIT) & 1ULL) == 1ULL);
+        assert(((sstatus_after_sret >> SSTATUS_SPIE_BIT) & 1ULL) == 1ULL);
+        assert(target->csr_priv_mode == PRIV_ENCODE_U);
+
+        // Enable machine interrupts so the subsequent machine trap captures pre-trap MIE=1
+        csr_op(target, CSR_MSTATUS, CSR_OP_SET, (1ULL << MSTATUS_MIE_BIT));
+        const uint64_t second_trap_pc = 0xB80ULL;
+        trigger_trap(target, second_trap_pc, EXC_INST_ILLEGAL, 0ULL, false);
+        uint64_t mstatus_snapshot = read_csr(target, CSR_MSTATUS);
+        assert(((mstatus_snapshot >> MSTATUS_MIE_BIT) & 1ULL) == 0ULL);
+        assert(((mstatus_snapshot >> MSTATUS_MPIE_BIT) & 1ULL) == 1ULL);
+        assert(((mstatus_snapshot >> MSTATUS_MPP_LO) & 0x3ULL) == PRIV_ENCODE_U);
+        assert(read_csr(target, CSR_MEPC) == second_trap_pc);
+
+        target->trap_ctrl_mret_en = 1;
+        tick(target);
+        target->trap_ctrl_mret_en = 0;
+        uint64_t mstatus_after_mret = read_csr(target, CSR_MSTATUS);
+        assert(((mstatus_after_mret >> MSTATUS_MIE_BIT) & 1ULL) == 1ULL);
+        assert(((mstatus_after_mret >> MSTATUS_MPIE_BIT) & 1ULL) == 1ULL);
+        assert(((mstatus_after_mret >> MSTATUS_MPP_LO) & 0x3ULL) == PRIV_ENCODE_U);
+        assert(target->csr_priv_mode == PRIV_ENCODE_U);
+    });
+
+    run_csr_scenario(dut, "illegal_csr_access_guard", [&](Vcsr_unit* target) {
+        force_priv_mode_via_mret(target, PRIV_ENCODE_M);
+        assert(target->csr_priv_mode == PRIV_ENCODE_M);
+        const uint64_t misa_snapshot = read_csr(target, CSR_MISA);
+        target->csr_addr = CSR_MISA;
+        target->csr_op = CSR_OP_WRITE;
+        target->csr_rs1_data = 0x1234ULL;
+        target->csr_req = 1;
+        tick(target);
+        target->csr_req = 0;
+        target->csr_op = 0;
+        target->csr_rs1_data = 0;
+        target->csr_addr = 0;
+        assert(target->csr_illegal_access);
+        assert(read_csr(target, CSR_MISA) == misa_snapshot);
+        assert(!target->csr_trap_to_s_mode);
+        assert(target->csr_priv_mode == PRIV_ENCODE_M);
+        assert(target->csr_trap_vector_next == read_csr(target, CSR_MTVEC));
+        tick(target);
+        assert(!target->csr_illegal_access);
+    });
+
+    run_csr_scenario(dut, "delegation_cross_over", [&](Vcsr_unit* target) {
+        csr_op(target, CSR_MTVEC, CSR_OP_WRITE, 0x1000ULL);
+        csr_op(target, CSR_STVEC, CSR_OP_WRITE, 0x2000ULL);
+        csr_op(target, CSR_MEDELEG, CSR_OP_WRITE, 0ULL);
+        csr_op(target, CSR_MIDELEG, CSR_OP_WRITE, 0ULL);
+
+        struct DelegationProbe {
+            const char* name;
+            bool is_interrupt;
+            uint64_t cause;
+            bool delegate_to_s;
+            uint64_t pc;
+        };
+
+        const std::array<DelegationProbe, 4> probes = {{
+            {"medeleg_set",   false, EXC_ECALL_FROM_U, true,  0xD000ULL},
+            {"medeleg_clear", false, EXC_ECALL_FROM_U, false, 0xD040ULL},
+            {"mideleg_set",   true,  INT_M_TIMER,      true,  0xD080ULL},
+            {"mideleg_clear", true,  INT_M_TIMER,      false, 0xD0C0ULL}
+        }};
+
+        for (const auto& probe : probes) {
+            if (probe.is_interrupt) {
+                const uint64_t mask = (1ULL << probe.cause);
+                if (probe.delegate_to_s) {
+                    csr_op(target, CSR_MIDELEG, CSR_OP_SET, mask);
+                } else {
+                    csr_op(target, CSR_MIDELEG, CSR_OP_CLEAR, mask);
+                }
+                force_priv_mode_via_mret(target, PRIV_ENCODE_S);
+                trigger_trap(target, probe.pc, (INTERRUPT_BIT | probe.cause), 0ULL, probe.delegate_to_s);
+                if (probe.delegate_to_s) {
+                    assert(target->csr_trap_to_s_mode);
+                    assert(read_csr(target, CSR_SEPC) == probe.pc);
+                } else {
+                    assert(!target->csr_trap_to_s_mode);
+                    assert(read_csr(target, CSR_MEPC) == probe.pc);
+                }
+            } else {
+                const uint64_t mask = (1ULL << probe.cause);
+                if (probe.delegate_to_s) {
+                    csr_op(target, CSR_MEDELEG, CSR_OP_SET, mask);
+                } else {
+                    csr_op(target, CSR_MEDELEG, CSR_OP_CLEAR, mask);
+                }
+                force_priv_mode_via_mret(target, PRIV_ENCODE_U);
+                trigger_trap(target, probe.pc, probe.cause, 0ULL, probe.delegate_to_s);
+                if (probe.delegate_to_s) {
+                    assert(target->csr_trap_to_s_mode);
+                    assert(read_csr(target, CSR_SEPC) == probe.pc);
+                } else {
+                    assert(!target->csr_trap_to_s_mode);
+                    assert(read_csr(target, CSR_MEPC) == probe.pc);
+                }
+            }
+
+            const uint64_t expected_vector = probe.delegate_to_s ? read_csr(target, CSR_STVEC)
+                                                                  : read_csr(target, CSR_MTVEC);
+            assert(target->csr_trap_vector == expected_vector);
+        }
+    });
+
     std::cout << "----------------------------------\n";
     std::cout << "All CSR unit tests passed!\n";
     std::cout << "----------------------------------\n";
diff --git a/rv64cpu/tb/unit/tb_decoder.cc b/rv64cpu/tb/unit/tb_decoder.cc
index aa6788c..25a3d69 100644
--- a/rv64cpu/tb/unit/tb_decoder.cc
+++ b/rv64cpu/tb/unit/tb_decoder.cc
@@ -39,7 +39,8 @@ static int fail_count = 0;
         } \
     } while(0)
 
-void decode(Vdecoder* dut, uint32_t instr, const char* name) {
+void decode(Vdecoder* dut, uint32_t instr, const char* name, uint8_t priv_mode = 3) {
+    dut->priv_mode = priv_mode & 0x3;
     dut->instr = instr;
     dut->eval();
     test_count++;
@@ -233,6 +234,10 @@ int main(int argc, char** argv) {
     CHECK(dut, reg_write_en, 1,  "CSRRW");
     CHECK(dut, illegal_instr,0,  "CSRRW");
 
+    decode(dut, 0x300110F3, "CSRRW x1, mstatus, x2 (U-mode)", 0);
+    CHECK(dut, illegal_instr,1,  "CSRRW U-mode illegal");
+    CHECK(dut, is_csr,       0,  "CSRRW U-mode is_csr cleared");
+
     // 20. ECALL
     //     all-zero fields + opcode=1110011
     decode(dut, 0x00000073, "ECALL");
@@ -256,7 +261,7 @@ int main(int argc, char** argv) {
 
     // 23. SRET
     //     funct7=0001000 rs2=00010 rs1=00000 funct3=000 rd=00000 op=1110011
-    decode(dut, 0x10200073, "SRET");
+    decode(dut, 0x10200073, "SRET", 1);
     CHECK(dut, is_sret,      1,  "SRET");
     CHECK(dut, reg_write_en, 0,  "SRET");
 
diff --git a/rv64cpu/tb/unit/tb_hazard_unit.cc b/rv64cpu/tb/unit/tb_hazard_unit.cc
index 69ebab9..d43ccc5 100644
--- a/rv64cpu/tb/unit/tb_hazard_unit.cc
+++ b/rv64cpu/tb/unit/tb_hazard_unit.cc
@@ -42,9 +42,13 @@ void reset(Vhazard_unit* dut) {
     dut->ex_reg_write_en = 0;
     dut->ex_is_load = 0;
     dut->ex_branch_taken = 0;
+    dut->ex_branch_target = 0;
     dut->mem_rd_addr = 0;
     dut->mem_reg_write_en = 0;
     dut->mem_stall_req = 0;
+    dut->trap_flush_req = 0;
+    dut->trap_pc_sel = 0;
+    dut->trap_target_pc = 0;
     dut->wb_rd_addr = 0;
     dut->wb_reg_write_en = 0;
     dut->eval();
@@ -69,6 +73,8 @@ int main(int argc, char** argv) {
     check(dut->id_ex_flush_en == 0, "NoHazard_NoIDEXFlush");
     check(dut->id_ex_stall_en == 0, "NoHazard_NoIDEXStall");
     check(dut->ex_mem_stall_en == 0, "NoHazard_NoEXMEMStall");
+    check(dut->pc_redirect_sel == 0, "NoHazard_NoRedirect");
+    check(dut->pc_redirect_is_trap == 0, "NoHazard_NoTrapRedirect");
 
     // -----------------------
     // 2. Forward A from MEM stage
@@ -231,11 +237,15 @@ int main(int argc, char** argv) {
     // -----------------------
     reset(dut);
     dut->ex_branch_taken = 1;
+    dut->ex_branch_target = 0x400;
     dut->eval();
     check(dut->if_id_flush_en == 1, "Branch_IFID_Flush");
     check(dut->id_ex_flush_en == 1, "Branch_IDEX_Flush");
     check(dut->if_stall_en == 0, "Branch_IF_NoStall");
     check(dut->if_id_stall_en == 0, "Branch_IFID_NoStall");
+    check(dut->pc_redirect_sel == 1, "Branch_Redirect_Valid");
+    check(dut->pc_redirect_is_trap == 0, "Branch_Redirect_NotTrap");
+    check(dut->pc_redirect_target == 0x400, "Branch_Redirect_Target");
 
     // -----------------------
     // 15. Branch overrides load-use stall
@@ -282,12 +292,48 @@ int main(int argc, char** argv) {
     // -----------------------
     reset(dut);
     dut->ex_branch_taken = 1;
+    dut->ex_branch_target = 0x410;
     dut->mem_stall_req = 1;
     dut->eval();
     check(dut->if_id_flush_en == 1, "Branch_MemStall_IFID_Flush");
     check(dut->id_ex_flush_en == 1, "Branch_MemStall_IDEX_Flush");
     check(dut->if_stall_en == 0, "Branch_MemStall_IF_NoStall");
 
+    // -----------------------
+    // 19. Trap redirect overrides branch and stalls
+    // -----------------------
+    reset(dut);
+    dut->trap_pc_sel = 1;
+    dut->trap_flush_req = 1;
+    dut->trap_target_pc = 0x800;
+    dut->ex_branch_taken = 1;
+    dut->ex_branch_target = 0x900;
+    dut->mem_stall_req = 1;
+    dut->eval();
+    check(dut->pc_redirect_sel == 1, "Trap_Redirect_Valid");
+    check(dut->pc_redirect_is_trap == 1, "Trap_Redirect_IsTrap");
+    check(dut->pc_redirect_target == 0x800, "Trap_Redirect_Target");
+    check(dut->if_id_flush_en == 1, "Trap_Flush_IFID");
+    check(dut->id_ex_flush_en == 1, "Trap_Flush_IDEX");
+    check(dut->if_stall_en == 0, "Trap_Clears_IF_Stall");
+    check(dut->if_id_stall_en == 0, "Trap_Clears_IFID_Stall");
+    check(dut->id_ex_stall_en == 0, "Trap_Clears_IDEX_Stall");
+    check(dut->ex_mem_stall_en == 0, "Trap_Clears_EXMEM_Stall");
+
+    // -----------------------
+    // 20. Trap flush overrides load-use stall even without redirect
+    // -----------------------
+    reset(dut);
+    dut->trap_flush_req = 1;
+    dut->ex_is_load = 1;
+    dut->ex_rd_addr = 4;
+    dut->id_rs1_addr = 4;
+    dut->eval();
+    check(dut->if_id_flush_en == 1, "TrapFlush_IFID");
+    check(dut->id_ex_flush_en == 1, "TrapFlush_IDEX");
+    check(dut->if_stall_en == 0, "TrapFlush_NoIFStall");
+    check(dut->if_id_stall_en == 0, "TrapFlush_NoIFIDStall");
+
     std::cout << "----------------------------------" << std::endl;
     std::cout << "\033[32mAll " << pass_count << " tests passed!\033[0m" << std::endl;
     std::cout << "----------------------------------" << std::endl;
diff --git a/rv64cpu/tb/unit/tb_if_stage.cc b/rv64cpu/tb/unit/tb_if_stage.cc
index ebb888a..f0110c9 100644
--- a/rv64cpu/tb/unit/tb_if_stage.cc
+++ b/rv64cpu/tb/unit/tb_if_stage.cc
@@ -51,8 +51,9 @@ int main(int argc, char** argv) {
 
     // --- 1. Init / Reset ---
     dut->stall_en = 0;
-    dut->ex_branch_taken = 0;
-    dut->ex_branch_target = 0;
+    dut->pc_redirect_sel = 0;
+    dut->pc_redirect_target = 0;
+    dut->pc_redirect_is_trap = 0;
     
     dut->rst = 1;
     tick(dut);
@@ -78,13 +79,14 @@ int main(int argc, char** argv) {
     check_pc(0x8000000CULL, dut->if_pc, "Release_Stall_Normal_Increment");
 
     // --- 4. Branch Taken ---
-    dut->ex_branch_taken = 1;
-    dut->ex_branch_target = 0x80000100ULL;
+    dut->pc_redirect_sel = 1;
+    dut->pc_redirect_is_trap = 0;
+    dut->pc_redirect_target = 0x80000100ULL;
     tick(dut);
     check_pc(0x80000100ULL, dut->if_pc, "Branch_Taken_Updates_PC");
 
     // Return to normal
-    dut->ex_branch_taken = 0;
+    dut->pc_redirect_sel = 0;
     tick(dut);
     check_pc(0x80000104ULL, dut->if_pc, "Post_Branch_Normal_Increment");
 
@@ -92,8 +94,9 @@ int main(int argc, char** argv) {
     // If a branch is taken but the pipeline is stalled, the PC should freeze.
     // However, if the stall clears later, the branch target should theoretically be taken. 
     // In our RV64 simple pipeline design, stall_en freezes the PC reg completely. 
-    dut->ex_branch_taken = 1;
-    dut->ex_branch_target = 0x80000200ULL;
+    dut->pc_redirect_sel = 1;
+    dut->pc_redirect_is_trap = 0;
+    dut->pc_redirect_target = 0x80000200ULL;
     dut->stall_en = 1;
     tick(dut);
     check_pc(0x80000104ULL, dut->if_pc, "Stall_Overrides_Branch_Target_Load");
@@ -102,6 +105,24 @@ int main(int argc, char** argv) {
     tick(dut);
     check_pc(0x80000200ULL, dut->if_pc, "Branch_Target_Loaded_After_Stall_Clears");
 
+    // --- 6. Trap redirect overrides stall ---
+    dut->pc_redirect_sel = 0;
+    dut->pc_redirect_is_trap = 0;
+    dut->pc_redirect_target = 0;
+
+    dut->stall_en = 1;
+    dut->pc_redirect_sel = 1;
+    dut->pc_redirect_is_trap = 1;
+    dut->pc_redirect_target = 0x80000300ULL;
+    tick(dut);
+    check_pc(0x80000300ULL, dut->if_pc, "Trap_Overrides_Stall");
+
+    dut->stall_en = 0;
+    dut->pc_redirect_sel = 0;
+    dut->pc_redirect_is_trap = 0;
+    tick(dut);
+    check_pc(0x80000304ULL, dut->if_pc, "Resume_After_Trap");
+
     std::cout << "----------------------------------" << std::endl;
     std::cout << "\033[32mAll tests passed!\033[0m" << std::endl;
     std::cout << "----------------------------------" << std::endl;
diff --git a/rv64cpu/tb/unit/tb_trap_ctrl.cc b/rv64cpu/tb/unit/tb_trap_ctrl.cc
index 142d743..1708598 100644
--- a/rv64cpu/tb/unit/tb_trap_ctrl.cc
+++ b/rv64cpu/tb/unit/tb_trap_ctrl.cc
@@ -28,6 +28,8 @@ namespace {
 constexpr uint64_t EXC_INST_ILLEGAL   = 2ULL;
 constexpr uint64_t EXC_ECALL_FROM_S   = 9ULL;
 constexpr uint64_t EXC_LOAD_PAGE_FAULT= 13ULL;
+constexpr uint64_t EXC_LOAD_FAULT     = 5ULL;
+constexpr uint64_t EXC_STORE_FAULT    = 7ULL;
 
 constexpr uint64_t INT_M_SOFTWARE     = 3ULL;
 constexpr uint64_t INT_M_TIMER        = 7ULL;
@@ -125,6 +127,20 @@ int main(int argc, char** argv) {
     check(dut->trap_mcause == EXC_LOAD_PAGE_FAULT, "PageFault_Mcause");
     check(dut->trap_mtval == 0xBADC0FFEE0ULL, "PageFault_Mtval");
 
+    drive_defaults(dut);
+    dut->trap_load_fault = 1;
+    dut->trap_bad_addr = 0xABCDEF0010ULL;
+    dut->eval();
+    check(dut->trap_mcause == EXC_LOAD_FAULT, "LoadFault_Mcause");
+    check(dut->trap_mtval == 0xABCDEF0010ULL, "LoadFault_Mtval");
+
+    drive_defaults(dut);
+    dut->trap_store_fault = 1;
+    dut->trap_bad_addr = 0xABCDEF0020ULL;
+    dut->eval();
+    check(dut->trap_mcause == EXC_STORE_FAULT, "StoreFault_Mcause");
+    check(dut->trap_mtval == 0xABCDEF0020ULL, "StoreFault_Mtval");
+
     // ---------------------------------------------------------------------
     // 2) Interrupt priority + delegation to supervisor
     // ---------------------------------------------------------------------
diff --git a/rv64cpu/tb/unit/tb_wb_stage.cc b/rv64cpu/tb/unit/tb_wb_stage.cc
index 0f228ee..3f6bedb 100644
--- a/rv64cpu/tb/unit/tb_wb_stage.cc
+++ b/rv64cpu/tb/unit/tb_wb_stage.cc
@@ -58,12 +58,17 @@ int main(int argc, char** argv) {
     dut->mem_wb_rs1_rdata = 0x99;
     dut->mem_wb_rs1_addr = 1;
     dut->mem_wb_imm = 0x123;
+    dut->mem_wb_instr = 0xDEADBEEFu;
     dut->mem_wb_illegal_instr = 0;
     dut->mem_wb_is_ecall = 0;
     dut->mem_wb_is_ebreak = 0;
     dut->mem_wb_is_mret = 0;
     dut->mem_wb_is_sret = 0;
+    dut->mem_wb_load_fault = 0;
+    dut->mem_wb_store_fault = 0;
+    dut->mem_wb_fault_addr = 0;
     dut->csr_rdata = 0x0;
+    dut->csr_illegal_access = 0;
 
     tick(dut);
 
@@ -118,9 +123,37 @@ int main(int argc, char** argv) {
     check(dut->trap_is_mret == 1, "Trap_MRET");
     check(dut->trap_is_sret == 1, "Trap_SRET");
     check(dut->trap_epc == 0x80000000, "Trap_EPC");
+    check(dut->trap_bad_instr == 0xDEADBEEFu, "Trap_BadInstr_Payload");
     check(dut->wb_reg_write_en == 0, "Trap_Gates_RegWriteEn");
     check(dut->csr_req == 0, "Trap_Gates_CSR_Req");
 
+    // 7. CSR illegal access should raise an illegal-instr trap
+    dut->mem_wb_illegal_instr = 0;
+    dut->mem_wb_is_ecall = 0;
+    dut->mem_wb_is_ebreak = 0;
+    dut->csr_illegal_access = 1;
+    tick(dut);
+    check(dut->trap_illegal_instr == 1, "CSR_Illegal_Raises_Trap");
+    check(dut->wb_reg_write_en == 0, "CSR_Illegal_Kills_Write");
+
+    // 8. Load/store faults propagate to trap outputs and block writeback
+    dut->csr_illegal_access = 0;
+    dut->mem_wb_load_fault = 1;
+    dut->mem_wb_fault_addr = 0x80000010ULL;
+    tick(dut);
+    check(dut->trap_load_fault == 1, "LoadFault_AssertsTrap");
+    check(dut->trap_bad_addr == 0x80000010ULL, "LoadFault_PassesAddress");
+    check(dut->wb_reg_write_en == 0, "LoadFault_KillsWrite");
+
+    dut->mem_wb_load_fault = 0;
+    dut->mem_wb_store_fault = 1;
+    dut->mem_wb_fault_addr = 0x80000020ULL;
+    tick(dut);
+    check(dut->trap_store_fault == 1, "StoreFault_AssertsTrap");
+    check(dut->trap_bad_addr == 0x80000020ULL, "StoreFault_PassesAddress");
+
+    dut->mem_wb_store_fault = 0;
+
     std::cout << "----------------------------------" << std::endl;
     std::cout << "\033[32mAll " << pass_count << " tests passed!\033[0m" << std::endl;
     std::cout << "----------------------------------" << std::endl;