diff --git a/include/sbi/sbi_hart.h b/include/sbi/sbi_hart.h index 32545f41..22d83275 100644 --- a/include/sbi/sbi_hart.h +++ b/include/sbi/sbi_hart.h @@ -69,6 +69,10 @@ enum sbi_hart_extensions { SBI_HART_EXT_SVADU, /** Hart has Smnpm extension */ SBI_HART_EXT_SMNPM, + /** HART has zicfilp extension */ + SBI_HART_EXT_ZICFILP, + /** HART has zicfiss extension */ + SBI_HART_EXT_ZICFISS, /** Maximum index of Hart extension */ SBI_HART_EXT_MAX, diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c index f7aed739..73f2c7b1 100644 --- a/lib/sbi/sbi_hart.c +++ b/lib/sbi/sbi_hart.c @@ -681,6 +681,8 @@ const struct sbi_hart_ext_data sbi_hart_ext[] = { __SBI_HART_EXT_DATA(svade, SBI_HART_EXT_SVADE), __SBI_HART_EXT_DATA(svadu, SBI_HART_EXT_SVADU), __SBI_HART_EXT_DATA(smnpm, SBI_HART_EXT_SMNPM), + __SBI_HART_EXT_DATA(zicfilp, SBI_HART_EXT_ZICFILP), + __SBI_HART_EXT_DATA(zicfiss, SBI_HART_EXT_ZICFISS), }; _Static_assert(SBI_HART_EXT_MAX == array_size(sbi_hart_ext), diff --git a/lib/sbi/sbi_trap.c b/lib/sbi/sbi_trap.c index b4f3a176..176e1740 100644 --- a/lib/sbi/sbi_trap.c +++ b/lib/sbi/sbi_trap.c @@ -103,6 +103,7 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs, const struct sbi_trap_info *trap) { ulong hstatus, vsstatus, prev_mode; + bool elp = false; #if __riscv_xlen == 32 bool prev_virt = (regs->mstatusH & MSTATUSH_MPV) ? true : false; #else @@ -116,6 +117,17 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs, if (prev_mode != PRV_S && prev_mode != PRV_U) return SBI_ENOTSUPP; + /* If hart support for zicfilp, clear MPELP because redirecting to VS or (H)S */ + if (sbi_hart_has_extension(sbi_scratch_thishart_ptr(), SBI_HART_EXT_ZICFILP)) { +#if __riscv_xlen == 32 + elp = regs->mstatusH & MSTATUSH_MPELP; + regs->mstatusH &= ~MSTATUSH_MPELP; +#else + elp = regs->mstatus & MSTATUS_MPELP; + regs->mstatus &= ~MSTATUS_MPELP; +#endif + } + /* If exceptions came from VS/VU-mode, redirect to VS-mode if * delegated in hedeleg */ @@ -169,6 +181,10 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs, /* Get VS-mode SSTATUS CSR */ vsstatus = csr_read(CSR_VSSTATUS); + /* If elp was set, set it back in vsstatus */ + if (elp) + vsstatus |= MSTATUS_SPELP; + /* Set SPP for VS-mode */ vsstatus &= ~SSTATUS_SPP; if (prev_mode == PRV_S) @@ -209,6 +225,10 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs, /* Clear SIE for S-mode */ regs->mstatus &= ~MSTATUS_SIE; + + /* If elp was set, set it back in mstatus */ + if (elp) + regs->mstatus |= MSTATUS_SPELP; } return 0;