[PATCH v4 3/3] firmware: fw_base.S: Add common NMI trap handler

[Nylon Chen <nylon.chen@sifive.com>]

Implement the firmware-level RNMI entry point and complete the RNMI
initialization:

fw_base.S - sbi_rnmi_vector assembly entry:
  Mirrors the existing _trap_handler pattern.  MNSCRATCH holds the
  per-hart scratch pointer (set by rnmi_vector_init), so the entry
  sequence is structurally identical to _trap_handler:

    csrrw sp, MNSCRATCH, sp      ; sp=scratch_ptr, MNSCRATCH=original_sp
    REG_S t0, TMP0(sp)           ; stash t0 in scratch tmp0 slot
    check MNSTATUS.MNPP          ; same bit layout as MSTATUS.MPP

  S/U-mode interrupted:
    frame allocated below scratch area (same as _trap_handler_s_mode)

  M-mode interrupted:
    frame allocated on the existing M-mode stack so no separate RNMI
    stack is required (same as _trap_handler_m_mode)

  After saving all registers, MNEPC/MNSTATUS are saved into the
  standard sbi_trap_regs mepc/mstatus fields so the C handler can
  inspect them with the same accessors used for normal traps.

  The handler is called as:
    sbi_rnmi_trap_handler(regs, mncause)

  On return registers are restored and mnret is executed.  The mnret
  opcode is emitted as a raw .word (0x70200073) because older toolchain
  versions do not recognise the mnemonic.

sbi_trap.c - sbi_rnmi_trap_handler default implementation:
  Prints mnepc, mncause, and mnstatus, then hangs the hart.  This is
  the correct behaviour for an unexpected NMI; a platform that can
  actually handle and resume from an NMI should override this function.

sbi_trap.h - declarations for sbi_rnmi_vector and sbi_rnmi_trap_handler.

sbi_hart.c - complete rnmi_vector_init():
  Adds the platform set_rnmi_trap_vector() call and MNSTATUS.NMIE
  enablement that were deferred from the previous patch.

Co-developed-by: Zong Li <zong.li@sifive.com>
Signed-off-by: Zong Li <zong.li@sifive.com>
Suggested-by: Nick Hu <nick.hu@sifive.com>
Suggested-by: Samuel Holland <samuel.holland@sifive.com>
Signed-off-by: Nylon Chen <nylon.chen@sifive.com>
Signed-off-by: Yong-Xuan Wang <yongxuan.wang@sifive.com>
---
 firmware/fw_base.S     | 141 +++++++++++++++++++++++++++++++++++++++++
 include/sbi/sbi_trap.h |  22 ++++++-
 lib/sbi/sbi_hart.c     |  34 ++++++++--
 lib/sbi/sbi_trap.c     |  17 +++++
 4 files changed, 208 insertions(+), 6 deletions(-)

diff --git a/firmware/fw_base.S b/firmware/fw_base.S
index bce9e226..641f314a 100644
--- a/firmware/fw_base.S
+++ b/firmware/fw_base.S
@@ -643,6 +643,147 @@ memcmp:
 	REG_L	a0, SBI_TRAP_REGS_OFFSET(a0)(a0)
 .endm
 
+/* Smrnmi mnret instruction encoding (0x70200073) */
+.macro MNRET
+	.word 0x70200073
+.endm
+
+	.align 4
+	.section .entry, "ax", %progbits
+	.globl sbi_rnmi_vector
+sbi_rnmi_vector:
+	/* Swap SP with MNSCRATCH. After: sp=scratch_ptr, MNSCRATCH=original_sp */
+	csrrw	sp, CSR_MNSCRATCH, sp
+
+	/* Save T0 in scratch tmp0 slot */
+	REG_S	t0, SBI_SCRATCH_TMP0_OFFSET(sp)
+
+	/* Check MNSTATUS.MNPP to determine interrupted privilege mode */
+	csrr	t0, CSR_MNSTATUS
+	srl	t0, t0, MSTATUS_MPP_SHIFT
+	and	t0, t0, PRV_M
+	xori	t0, t0, PRV_M
+	beq	t0, zero, _sbi_rnmi_m_mode
+
+	/* Interrupted S/U-mode: use scratch area as exception stack */
+_sbi_rnmi_s_mode:
+	/* Load original SP from MNSCRATCH into T0 */
+	csrr	t0, CSR_MNSCRATCH
+	/* Restore MNSCRATCH = scratch_ptr for future RNMI entries */
+	csrw	CSR_MNSCRATCH, sp
+	/* Allocate exception frame just below scratch area */
+	addi	sp, sp, -(SBI_TRAP_REGS_SIZE)
+	j	_sbi_rnmi_save_regs
+
+	/* Interrupted M-mode: reuse existing M-mode stack */
+_sbi_rnmi_m_mode:
+	/* Load original SP from MNSCRATCH into T0 */
+	csrr	t0, CSR_MNSCRATCH
+	/* Restore MNSCRATCH = scratch_ptr for future RNMI entries */
+	csrw	CSR_MNSCRATCH, sp
+	/* Allocate exception frame on the existing M-mode stack */
+	addi	sp, t0, -(SBI_TRAP_REGS_SIZE)
+
+_sbi_rnmi_save_regs:
+	/* sp = exception frame, t0 = original SP, MNSCRATCH = scratch_ptr */
+
+	/* Save original SP (from T0) */
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(sp)(sp)
+
+	/* Reload saved T0 from scratch tmp0, then save to exception frame */
+	csrr	t0, CSR_MNSCRATCH
+	REG_L	t0, SBI_SCRATCH_TMP0_OFFSET(t0)
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(t0)(sp)
+
+	/* Save all general registers except SP and T0 (already saved) */
+	REG_S	zero, SBI_TRAP_REGS_OFFSET(zero)(sp)
+	REG_S	ra,   SBI_TRAP_REGS_OFFSET(ra)(sp)
+	REG_S	gp,   SBI_TRAP_REGS_OFFSET(gp)(sp)
+	REG_S	tp,   SBI_TRAP_REGS_OFFSET(tp)(sp)
+	REG_S	t1,   SBI_TRAP_REGS_OFFSET(t1)(sp)
+	REG_S	t2,   SBI_TRAP_REGS_OFFSET(t2)(sp)
+	REG_S	s0,   SBI_TRAP_REGS_OFFSET(s0)(sp)
+	REG_S	s1,   SBI_TRAP_REGS_OFFSET(s1)(sp)
+	REG_S	a0,   SBI_TRAP_REGS_OFFSET(a0)(sp)
+	REG_S	a1,   SBI_TRAP_REGS_OFFSET(a1)(sp)
+	REG_S	a2,   SBI_TRAP_REGS_OFFSET(a2)(sp)
+	REG_S	a3,   SBI_TRAP_REGS_OFFSET(a3)(sp)
+	REG_S	a4,   SBI_TRAP_REGS_OFFSET(a4)(sp)
+	REG_S	a5,   SBI_TRAP_REGS_OFFSET(a5)(sp)
+	REG_S	a6,   SBI_TRAP_REGS_OFFSET(a6)(sp)
+	REG_S	a7,   SBI_TRAP_REGS_OFFSET(a7)(sp)
+	REG_S	s2,   SBI_TRAP_REGS_OFFSET(s2)(sp)
+	REG_S	s3,   SBI_TRAP_REGS_OFFSET(s3)(sp)
+	REG_S	s4,   SBI_TRAP_REGS_OFFSET(s4)(sp)
+	REG_S	s5,   SBI_TRAP_REGS_OFFSET(s5)(sp)
+	REG_S	s6,   SBI_TRAP_REGS_OFFSET(s6)(sp)
+	REG_S	s7,   SBI_TRAP_REGS_OFFSET(s7)(sp)
+	REG_S	s8,   SBI_TRAP_REGS_OFFSET(s8)(sp)
+	REG_S	s9,   SBI_TRAP_REGS_OFFSET(s9)(sp)
+	REG_S	s10,  SBI_TRAP_REGS_OFFSET(s10)(sp)
+	REG_S	s11,  SBI_TRAP_REGS_OFFSET(s11)(sp)
+	REG_S	t3,   SBI_TRAP_REGS_OFFSET(t3)(sp)
+	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)
+
+	/* Save MNEPC into regs->mepc, MNSTATUS into regs->mstatus */
+	csrr	t0, CSR_MNEPC
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(mepc)(sp)
+	csrr	t0, CSR_MNSTATUS
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(mstatus)(sp)
+
+	/* Call C RNMI handler: a0=regs, a1=mncause */
+	add	a0, sp, zero
+	csrr	a1, CSR_MNCAUSE
+	call	sbi_rnmi_trap_handler
+
+	/* Restore MNEPC and MNSTATUS from exception frame */
+	REG_L	t0, SBI_TRAP_REGS_OFFSET(mepc)(sp)
+	csrw	CSR_MNEPC, t0
+	REG_L	t0, SBI_TRAP_REGS_OFFSET(mstatus)(sp)
+	csrw	CSR_MNSTATUS, t0
+
+	/* Restore all general registers except SP and T0 */
+	REG_L	ra,  SBI_TRAP_REGS_OFFSET(ra)(sp)
+	REG_L	gp,  SBI_TRAP_REGS_OFFSET(gp)(sp)
+	REG_L	tp,  SBI_TRAP_REGS_OFFSET(tp)(sp)
+	REG_L	t1,  SBI_TRAP_REGS_OFFSET(t1)(sp)
+	REG_L	t2,  SBI_TRAP_REGS_OFFSET(t2)(sp)
+	REG_L	s0,  SBI_TRAP_REGS_OFFSET(s0)(sp)
+	REG_L	s1,  SBI_TRAP_REGS_OFFSET(s1)(sp)
+	REG_L	a0,  SBI_TRAP_REGS_OFFSET(a0)(sp)
+	REG_L	a1,  SBI_TRAP_REGS_OFFSET(a1)(sp)
+	REG_L	a2,  SBI_TRAP_REGS_OFFSET(a2)(sp)
+	REG_L	a3,  SBI_TRAP_REGS_OFFSET(a3)(sp)
+	REG_L	a4,  SBI_TRAP_REGS_OFFSET(a4)(sp)
+	REG_L	a5,  SBI_TRAP_REGS_OFFSET(a5)(sp)
+	REG_L	a6,  SBI_TRAP_REGS_OFFSET(a6)(sp)
+	REG_L	a7,  SBI_TRAP_REGS_OFFSET(a7)(sp)
+	REG_L	s2,  SBI_TRAP_REGS_OFFSET(s2)(sp)
+	REG_L	s3,  SBI_TRAP_REGS_OFFSET(s3)(sp)
+	REG_L	s4,  SBI_TRAP_REGS_OFFSET(s4)(sp)
+	REG_L	s5,  SBI_TRAP_REGS_OFFSET(s5)(sp)
+	REG_L	s6,  SBI_TRAP_REGS_OFFSET(s6)(sp)
+	REG_L	s7,  SBI_TRAP_REGS_OFFSET(s7)(sp)
+	REG_L	s8,  SBI_TRAP_REGS_OFFSET(s8)(sp)
+	REG_L	s9,  SBI_TRAP_REGS_OFFSET(s9)(sp)
+	REG_L	s10, SBI_TRAP_REGS_OFFSET(s10)(sp)
+	REG_L	s11, SBI_TRAP_REGS_OFFSET(s11)(sp)
+	REG_L	t3,  SBI_TRAP_REGS_OFFSET(t3)(sp)
+	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)
+
+	/* Restore T0 */
+	REG_L	t0, SBI_TRAP_REGS_OFFSET(t0)(sp)
+
+	/* Restore SP */
+	REG_L	sp, SBI_TRAP_REGS_OFFSET(sp)(sp)
+
+	/* Return from RNMI */
+	MNRET
+
 	.section .entry, "ax", %progbits
 	.align 3
 	.globl _trap_handler
diff --git a/include/sbi/sbi_trap.h b/include/sbi/sbi_trap.h
index 731a0c98..6a56e64e 100644
--- a/include/sbi/sbi_trap.h
+++ b/include/sbi/sbi_trap.h
@@ -289,6 +289,24 @@ static inline void sbi_trap_set_context(struct sbi_scratch *scratch,
 
 struct sbi_trap_context *sbi_trap_handler(struct sbi_trap_context *tcntx);
 
-#endif
+/**
+ * Common firmware RNMI trap handler entry in M-mode.
+ *
+ * Platforms program their RNMI trap vector to this address using the
+ * set_rnmi_trap_vector platform operation.
+ */
+void sbi_rnmi_vector(void);
 
-#endif
+/**
+ * Default RNMI trap handler called from sbi_rnmi_vector.
+ *
+ * This is the default handler for Resumable Non-Maskable Interrupts (RNMI).
+ * It prints diagnostic information and hangs the hart. Platforms that can
+ * handle and resume from an NMI should override this function.
+ */
+void __attribute__((weak)) sbi_rnmi_trap_handler(struct sbi_trap_regs *regs,
+						  unsigned long mncause);
+
+#endif /* !__ASSEMBLER__ */
+
+#endif /* __SBI_TRAP_H__ */
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index eca6662f..519d29a0 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -766,21 +766,47 @@ int sbi_hart_init(struct sbi_scratch *scratch, bool cold_boot)
 	return rnmi_vector_init(scratch, cold_boot);
 }
 
+/* Forward declaration for the RNMI vector entry point in fw_base.S */
+extern void sbi_rnmi_vector(void);
+
 static int rnmi_vector_init(struct sbi_scratch *scratch, bool cold_boot)
 {
+	int ret;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+	const struct sbi_platform_operations *ops =
+		plat ? sbi_platform_ops(plat) : NULL;
+
 	/* If the hart does not support Smrnmi then nothing to do. */
 	if (!sbi_hart_has_extension(scratch, SBI_HART_EXT_SMRNMI))
 		return 0;
 
+	/* Platform MUST provide RNMI vector hook if hart supports Smrnmi. */
+	if (!ops || !ops->set_rnmi_trap_vector) {
+		sbi_printf("%s: platform does not support Smrnmi\n", __func__);
+		return SBI_ENOTSUPP;
+	}
+
 	/*
 	 * Set MNSCRATCH to the scratch pointer (same role as MSCRATCH).
-	 * The RNMI handler (to be added in the next patch) will check
-	 * MNSTATUS.MNPP to determine whether to use the scratch area
-	 * (S/U-mode interrupted) or reuse the existing M-mode stack
-	 * (M-mode interrupted).
+	 * The RNMI handler swaps SP/MNSCRATCH on entry, detects the
+	 * interrupted mode via MNSTATUS.MNPP, and then either reuses the
+	 * existing M-mode stack (M-mode interrupted) or uses the scratch
+	 * area as a stack base (S/U-mode interrupted) -- mirroring what
+	 * _trap_handler already does for nested M-mode traps.
 	 */
 	csr_write(CSR_MNSCRATCH, (unsigned long)scratch);
 
+	/* Ask the platform to program the RNMI trap vector. */
+	ret = ops->set_rnmi_trap_vector((unsigned long)sbi_rnmi_vector,
+					cold_boot);
+	if (ret == SBI_ENODEV)
+		return 0;
+	if (ret)
+		return ret;
+
+	/* Enable RNMI now that MNSCRATCH and the trap vector are set up. */
+	csr_set(CSR_MNSTATUS, MNSTATUS_NMIE);
+
 	return 0;
 }
 
diff --git a/lib/sbi/sbi_trap.c b/lib/sbi/sbi_trap.c
index f41db4d1..38be122a 100644
--- a/lib/sbi/sbi_trap.c
+++ b/lib/sbi/sbi_trap.c
@@ -375,3 +375,20 @@ trap_done:
 	sbi_trap_set_context(scratch, tcntx->prev_context);
 	return tcntx;
 }
+
+/**
+ * Default RNMI trap handler.
+ *
+ * This is the default handler for Resumable Non-Maskable Interrupts (RNMI).
+ * It is invoked directly from the firmware RNMI entry (rnmi_handler)
+ * and prints diagnostic information before hanging the hart.
+ */
+void __attribute__((weak)) sbi_rnmi_trap_handler(struct sbi_trap_regs *regs,
+						  unsigned long mncause)
+{
+	sbi_printf("mnepc    = 0x%" PRILX "\n", regs->mepc);
+	sbi_printf("mncause  = 0x%" PRILX "\n", mncause);
+	sbi_printf("mnstatus = 0x%" PRILX "\n", regs->mstatus);
+
+	sbi_hart_hang();
+}
-- 
2.43.7


-- 
opensbi mailing list
opensbi@lists.infradead.org
http://lists.infradead.org/mailman/listinfo/opensbi