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

[Nylon Chen]

Add rnmi_handler assembly wrapper and sbi_rnmi_trap_handler default
C handler. The default handler prints diagnostics and hangs the hart.

Co-developed-by: Zong Li <zong.li at sifive.com>
Signed-off-by: Zong Li <zong.li at sifive.com>
Suggested-by: Nick Hu <nick.hu at sifive.com>
Suggested-by: Samuel Holland <samuel.holland at sifive.com>
Signed-off-by: Nylon Chen <nylon.chen at sifive.com>
Signed-off-by: Yong-Xuan Wang <yongxuan.wang at sifive.com>
---
 firmware/fw_base.S     | 64 ++++++++++++++++++++++++++++++++++++++++++++++++++
 include/sbi/sbi_trap.h | 12 ++++++++--
 lib/sbi/sbi_hart.c     |  8 +++++++
 lib/sbi/sbi_trap.c     | 16 +++++++++++++
 4 files changed, 98 insertions(+), 2 deletions(-)

diff --git a/firmware/fw_base.S b/firmware/fw_base.S
index bce9e226..c2b9a5f8 100644
--- a/firmware/fw_base.S
+++ b/firmware/fw_base.S
@@ -643,6 +643,70 @@ memcmp:
 	REG_L	a0, SBI_TRAP_REGS_OFFSET(a0)(a0)
 .endm
 
+.macro RNMI_SAVE_REGS
+	/* Save all general registers except SP (already in MNSCRATCH) */
+	TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0
+	/* Also save t0 (TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0 doesn't save it) */
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(t0)(sp)
+.endm
+
+.macro RNMI_RESTORE_REGS
+	/* Restore all general registers except A0 and T0 */
+	TRAP_RESTORE_GENERAL_REGS_EXCEPT_A0_T0
+	/* Restore A0 and T0 */
+	TRAP_RESTORE_A0_T0
+.endm
+
+/* Smrnmi mnret instruction encoding (0x70200073) */
+.macro MNRET
+	.word 0x70200073
+.endm
+
+	.section .entry, "ax", %progbits
+	.align 4
+	.globl sbi_rnmi_vector
+sbi_rnmi_vector:
+	/* Swap SP with MNSCRATCH */
+	csrrw	sp, CSR_MNSCRATCH, sp
+
+	/* Allocate space for full trap registers structure */
+	addi	sp, sp, -(SBI_TRAP_REGS_SIZE)
+
+	/* Save all general registers */
+	RNMI_SAVE_REGS
+
+	/* Save original SP (currently in MNSCRATCH) */
+	csrr	t0, CSR_MNSCRATCH
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(sp)(sp)
+
+	/* Set up arguments for C RNMI handler */
+	add	a0, sp, zero
+
+	/* Save RNMI MNEPC/MNSTATUS into regs->mepc/mstatus. */
+	csrr	t0, CSR_MNEPC
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(mepc)(a0)
+	csrr	t0, CSR_MNSTATUS
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(mstatus)(a0)
+
+	/* Pass RNMI cause as second argument */
+	csrr	a1, CSR_MNCAUSE
+
+	/* Call the default C RNMI handler directly */
+	call	sbi_rnmi_trap_handler
+
+	/* Restore RNMI stack pointer for future RNMI entries. */
+	addi	t0, sp, SBI_TRAP_REGS_SIZE
+	csrw	CSR_MNSCRATCH, t0
+
+	/* Set A0 to point to saved registers for restore macros */
+	add	a0, sp, zero
+
+	/* Restore all general registers */
+	RNMI_RESTORE_REGS
+
+	/* Return from RNMI with mnret. */
+	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..afd95ad5 100644
--- a/include/sbi/sbi_trap.h
+++ b/include/sbi/sbi_trap.h
@@ -289,6 +289,14 @@ 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
+#endif /* !__ASSEMBLER__ */
+
+#endif /* __SBI_TRAP_H__ */
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index baebc438..7586ee09 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -839,6 +839,7 @@ int sbi_rnmi_vector_init(struct sbi_scratch *scratch, bool cold_boot)
 {
 	unsigned long rnmi_sp;
 	void *rnmi_ctx;
+	int ret;
 	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
 	const struct sbi_platform_operations *ops =
 		plat ? sbi_platform_ops(plat) : NULL;
@@ -871,6 +872,13 @@ int sbi_rnmi_vector_init(struct sbi_scratch *scratch, bool cold_boot)
 	rnmi_sp  = (unsigned long)rnmi_ctx + SBI_TRAP_REGS_SIZE;
 	csr_write(CSR_MNSCRATCH, rnmi_sp);
 
+	/* Ask the platform to program the RNMI trap vector. */
+	ret = ops->set_rnmi_trap_vector((uintptr_t)sbi_rnmi_vector, cold_boot);
+	if (ret == SBI_ENODEV)
+		return 0;
+	if (ret)
+		return ret;
+
 	/* Enable RNMI now that CSR_MNSCRATCH and the trap vector are set up. */
 	csr_set(CSR_MNSTATUS, MNSTATUS_NMIE);
 
diff --git a/lib/sbi/sbi_trap.c b/lib/sbi/sbi_trap.c
index f41db4d1..940b83d8 100644
--- a/lib/sbi/sbi_trap.c
+++ b/lib/sbi/sbi_trap.c
@@ -375,3 +375,19 @@ 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 sbi_rnmi_trap_handler(struct sbi_trap_regs *regs, unsigned long mncause)
+{
+	sbi_printf("mnepc   = 0x%016lx\n", regs->mepc);
+	sbi_printf("mncause = 0x%016lx\n", mncause);
+	sbi_printf("mnstatus= 0x%016lx\n", regs->mstatus);
+
+	sbi_hart_hang();
+}

-- 
2.43.7