[PATCH] firmware: fw_base: Optimize trap handler for RV32 systems

[Alistair Francis]

On Sun, 2020-11-22 at 12:02 +0530, Anup Patel wrote:
> 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) to 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 at wdc.com>

Reviewed-by: Alistair Francis <alistair.francis at wdc.com>

Alistair

> ---
>  firmware/fw_base.S | 87 ++++++++++++++++++++++++++++++++++++--------
> --
>  1 file changed, 68 insertions(+), 19 deletions(-)
> 
> diff --git a/firmware/fw_base.S b/firmware/fw_base.S
> index 1d9b375..fb504e8 100644
> --- 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