diff options
| -rw-r--r-- | machine/mtrap.c | 13 |
1 files changed, 6 insertions, 7 deletions
diff --git a/machine/mtrap.c b/machine/mtrap.c index a27908c..bf02fab 100644 --- a/machine/mtrap.c +++ b/machine/mtrap.c @@ -265,13 +265,12 @@ void redirect_trap(uintptr_t epc, uintptr_t mstatus) write_csr(scause, read_csr(mcause)); write_csr(mepc, read_csr(stvec)); - uintptr_t prev_priv = EXTRACT_FIELD(mstatus, MSTATUS_MPP); - uintptr_t prev_ie = EXTRACT_FIELD(mstatus, MSTATUS_MPIE); - mstatus = INSERT_FIELD(mstatus, MSTATUS_SPP, prev_priv); - mstatus = INSERT_FIELD(mstatus, MSTATUS_SPIE, prev_ie); - mstatus = INSERT_FIELD(mstatus, MSTATUS_MPP, PRV_S); - mstatus = INSERT_FIELD(mstatus, MSTATUS_MPIE, 0); - write_csr(mstatus, mstatus); + uintptr_t new_mstatus = mstatus & ~(MSTATUS_SPP | MSTATUS_SPIE | MSTATUS_MPIE); + uintptr_t mpp_s = MSTATUS_MPP & (MSTATUS_MPP >> 1); + new_mstatus |= (mstatus / (MSTATUS_MPIE / MSTATUS_SPIE)) & MSTATUS_SPIE; + new_mstatus |= (mstatus / (mpp_s / MSTATUS_SPP)) & MSTATUS_SPP; + new_mstatus |= mpp_s; + write_csr(mstatus, new_mstatus); extern void __redirect_trap(); return __redirect_trap(); |