firmware: fw_base: Improve exception stack setup in trap handler

Currently, the low-level trap handler (i.e. _trap_handler()) uses
branch instructions to conditionally setup exception stack based
on which mode trap occured.

This patch implements exception stack setup using xor instructions
which is faster with same number of instructions due to lack of
branch instructions.

The new exception stack setup approach can be best described by the
following pseudocode:

 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

Signed-off-by: Anup Patel <anup.patel@wdc.com>
Reviewed-by: Atish Patra <atish.patra@wdc.com>

diff --git a/firmware/fw_base.S b/firmware/fw_base.S
index b66ac41..0271d9a 100644 (file)
--- 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)