firmware: fw_base: Optimize trap handler for RV32 systems

On RV32 systems, we have two CSRs for M-mode status (MSTATUS and
MSTATUSH) when H-extension is implemented. This means we have to
save/restore MSTATUSH for RV32 systems only when H-extension is
implemented. The current _trap_handler() has extra instructions
(roughly 10) for conditional save/restore of MSTATUSH CSR.

These extra instructions in RV32 _trap_handler() can be avoided
if we create separate low-level trap handler for RV32 systems
having H-extension. This patch optimizes low-level trap handler
for RV32 systems accordingly.

Signed-off-by: Anup Patel <anup.patel@wdc.com>
Reviewed-by: Alistair Francis <alistair.francis@wdc.com>

diff --git a/firmware/fw_base.S b/firmware/fw_base.S
index 1d9b375..fb504e8 100644 (file)
--- a/firmware/fw_base.S
+++ b/firmware/fw_base.S
@@ -413,6 +413,14 @@ _start_warm:
 
        /* Setup trap handler */
        la      a4, _trap_handler
+#if __riscv_xlen == 32
+       csrr    a5, CSR_MISA
+       srli    a5, a5, ('H' - 'A')
+       andi    a5, a5, 0x1
+       beq     a5, zero, _skip_trap_handler_rv32_hyp
+       la      a4, _trap_handler_rv32_hyp
+_skip_trap_handler_rv32_hyp:
+#endif
        csrw    CSR_MTVEC, a4
 
        /* Initialize SBI runtime */
@@ -476,10 +484,7 @@ fw_platform_init:
        add     a0, a1, zero
        ret
 
-       .section .entry, "ax", %progbits
-       .align 3
-       .globl _trap_handler
-_trap_handler:
+.macro TRAP_SAVE_AND_SETUP_SP_T0
        /* Swap TP and MSCRATCH */
        csrrw   tp, CSR_MSCRATCH, tp
 
@@ -519,23 +524,23 @@ _trap_handler:
 
        /* Swap TP and MSCRATCH */
        csrrw   tp, CSR_MSCRATCH, tp
+.endm
 
+.macro TRAP_SAVE_MEPC_MSTATUS have_mstatush
        /* Save MEPC and MSTATUS CSRs */
        csrr    t0, CSR_MEPC
        REG_S   t0, SBI_TRAP_REGS_OFFSET(mepc)(sp)
        csrr    t0, CSR_MSTATUS
        REG_S   t0, SBI_TRAP_REGS_OFFSET(mstatus)(sp)
-       REG_S   zero, SBI_TRAP_REGS_OFFSET(mstatusH)(sp)
-#if __riscv_xlen == 32
-       csrr    t0, CSR_MISA
-       srli    t0, t0, ('H' - 'A')
-       andi    t0, t0, 0x1
-       beq     t0, zero, _skip_mstatush_save
+       .if \have_mstatush
        csrr    t0, CSR_MSTATUSH
        REG_S   t0, SBI_TRAP_REGS_OFFSET(mstatusH)(sp)
-_skip_mstatush_save:
-#endif
+       .else
+       REG_S   zero, SBI_TRAP_REGS_OFFSET(mstatusH)(sp)
+       .endif
+.endm
 
+.macro TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0
        /* Save all general regisers except SP and T0 */
        REG_S   zero, SBI_TRAP_REGS_OFFSET(zero)(sp)
        REG_S   ra, SBI_TRAP_REGS_OFFSET(ra)(sp)
@@ -567,11 +572,15 @@ _skip_mstatush_save:
        REG_S   t4, SBI_TRAP_REGS_OFFSET(t4)(sp)
        REG_S   t5, SBI_TRAP_REGS_OFFSET(t5)(sp)
        REG_S   t6, SBI_TRAP_REGS_OFFSET(t6)(sp)
+.endm
 
+.macro TRAP_CALL_C_ROUTINE
        /* Call C routine */
        add     a0, sp, zero
        call    sbi_trap_handler
+.endm
 
+.macro TRAP_RESTORE_GENERAL_REGS_EXCEPT_SP_T0
        /* Restore all general regisers except SP and T0 */
        REG_L   ra, SBI_TRAP_REGS_OFFSET(ra)(sp)
        REG_L   gp, SBI_TRAP_REGS_OFFSET(gp)(sp)
@@ -602,30 +611,70 @@ _skip_mstatush_save:
        REG_L   t4, SBI_TRAP_REGS_OFFSET(t4)(sp)
        REG_L   t5, SBI_TRAP_REGS_OFFSET(t5)(sp)
        REG_L   t6, SBI_TRAP_REGS_OFFSET(t6)(sp)
+.endm
 
+.macro TRAP_RESTORE_MEPC_MSTATUS have_mstatush
        /* Restore MEPC and MSTATUS CSRs */
        REG_L   t0, SBI_TRAP_REGS_OFFSET(mepc)(sp)
        csrw    CSR_MEPC, t0
        REG_L   t0, SBI_TRAP_REGS_OFFSET(mstatus)(sp)
        csrw    CSR_MSTATUS, t0
-#if __riscv_xlen == 32
-       csrr    t0, CSR_MISA
-       srli    t0, t0, ('H' - 'A')
-       andi    t0, t0, 0x1
-       beq     t0, zero, _skip_mstatush_restore
+       .if \have_mstatush
        REG_L   t0, SBI_TRAP_REGS_OFFSET(mstatusH)(sp)
        csrw    CSR_MSTATUSH, t0
-_skip_mstatush_restore:
-#endif
+       .endif
+.endm
 
+.macro TRAP_RESTORE_SP_T0
        /* Restore T0 */
        REG_L   t0, SBI_TRAP_REGS_OFFSET(t0)(sp)
 
        /* Restore SP */
        REG_L   sp, SBI_TRAP_REGS_OFFSET(sp)(sp)
+.endm
+
+       .section .entry, "ax", %progbits
+       .align 3
+       .globl _trap_handler
+_trap_handler:
+       TRAP_SAVE_AND_SETUP_SP_T0
+
+       TRAP_SAVE_MEPC_MSTATUS 0
+
+       TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0
+
+       TRAP_CALL_C_ROUTINE
+
+       TRAP_RESTORE_GENERAL_REGS_EXCEPT_SP_T0
+
+       TRAP_RESTORE_MEPC_MSTATUS 0
+
+       TRAP_RESTORE_SP_T0
 
        mret
 
+#if __riscv_xlen == 32
+       .section .entry, "ax", %progbits
+       .align 3
+       .globl _trap_handler_rv32_hyp
+_trap_handler_rv32_hyp:
+       TRAP_SAVE_AND_SETUP_SP_T0
+
+       TRAP_SAVE_MEPC_MSTATUS 1
+
+       TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0
+
+       TRAP_CALL_C_ROUTINE
+
+       TRAP_RESTORE_GENERAL_REGS_EXCEPT_SP_T0
+
+       TRAP_RESTORE_MEPC_MSTATUS 1
+
+       TRAP_RESTORE_SP_T0
+
+       mret
+#endif
+
        .section .entry, "ax", %progbits
        .align 3
        .globl _reset_regs