diff options
| -rw-r--r-- | firmware/fw_base.S | 47 |
1 files changed, 21 insertions, 26 deletions
diff --git a/firmware/fw_base.S b/firmware/fw_base.S index b66ac41..0271d9a 100644 --- a/firmware/fw_base.S +++ b/firmware/fw_base.S @@ -490,35 +490,30 @@ _trap_handler: /* Save T0 in scratch space */ REG_S t0, SBI_SCRATCH_TMP0_OFFSET(tp) - /* Check which mode we came from */ + /* + * Set T0 to appropriate exception stack + * + * Came_From_M_Mode = ((MSTATUS.MPP < PRV_M) ? 1 : 0) - 1; + * Exception_Stack = TP ^ (Came_From_M_Mode & (SP ^ TP)) + * + * Came_From_M_Mode = 0 ==> Exception_Stack = TP + * Came_From_M_Mode = -1 ==> Exception_Stack = SP + */ csrr t0, CSR_MSTATUS srl t0, t0, MSTATUS_MPP_SHIFT and t0, t0, PRV_M - xori t0, t0, PRV_M - beq t0, zero, _trap_handler_m_mode - - /* We came from S-mode or U-mode */ -_trap_handler_s_mode: - /* Set T0 to original SP */ - add t0, sp, zero - - /* Setup exception stack */ - add sp, tp, -(SBI_TRAP_REGS_SIZE) - - /* Jump to code common for all modes */ - j _trap_handler_all_mode - - /* We came from M-mode */ -_trap_handler_m_mode: - /* Set T0 to original SP */ - add t0, sp, zero - - /* Re-use current SP as exception stack */ - add sp, sp, -(SBI_TRAP_REGS_SIZE) - -_trap_handler_all_mode: - /* Save original SP (from T0) on stack */ - REG_S t0, SBI_TRAP_REGS_OFFSET(sp)(sp) + slti t0, t0, PRV_M + add t0, t0, -1 + xor sp, sp, tp + and t0, t0, sp + xor sp, sp, tp + xor t0, tp, t0 + + /* Save original SP on exception stack */ + REG_S sp, (SBI_TRAP_REGS_OFFSET(sp) - SBI_TRAP_REGS_SIZE)(t0) + + /* Set SP to exception stack and make room for trap registers */ + add sp, t0, -(SBI_TRAP_REGS_SIZE) /* Restore T0 from scratch space */ REG_L t0, SBI_SCRATCH_TMP0_OFFSET(tp) |