From 748eee73a12b9263bf80a9ca9e8a40ce57a8da6d Mon Sep 17 00:00:00 2001 From: Andrew Waterman Date: Sat, 16 Jul 2016 16:16:45 -0700 Subject: Improve trap redirection code --- machine/mtrap.c | 13 ++++++------- 1 file 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(); -- cgit v1.1