[PATCH 1/2] lib: sbi: Fix Smrnmi init and non-retentive suspend handling

[Nia Su]

From: Nylon Chen <nylon.chen at sifive.com>

Two fixes for the Smrnmi extension infrastructure:

1. Extract Smrnmi CSR setup into sbi_smrnmi_hart_init() and call it
   from both hart_detect_features() (early, before trap-based probing)
   and sbi_hart_reinit() (covers non-retentive suspend resume via
   init_warm_resume()).  This removes the need for save/restore of
   CSR_MNSCRATCH and MNSTATUS in sbi_hsm_data.

2. Change smrnmi_handlers_init callback return type from void to int.
   Allow NULL callback for platforms that do not need to program a
   vendor-specific NMI vector register.  Only set MNSTATUS.NMIE after
   the callback succeeds.

Fixes: 2d211fe6f9d5 ("lib: sbi: hart: Detect and enable Smrnmi before trap-based feature detection")

Signed-off-by: Nylon Chen <nylon.chen at sifive.com>
Co-developed-by: Nia Su <nia.su at sifive.com>
Signed-off-by: Nia Su <nia.su at sifive.com>
---
 include/sbi/sbi_hart.h     |  1 +
 include/sbi/sbi_platform.h |  2 +-
 lib/sbi/sbi_hart.c         | 55 +++++++++++++++++++++++++++++-----------------
 3 files changed, 37 insertions(+), 21 deletions(-)

diff --git a/include/sbi/sbi_hart.h b/include/sbi/sbi_hart.h
index 543393bba41526017cd95596348e26eaa897850a..03f12717572f402d77e4e78f8d29c93a5fa0343d 100644
--- a/include/sbi/sbi_hart.h
+++ b/include/sbi/sbi_hart.h
@@ -132,6 +132,7 @@ extern unsigned long hart_features_offset;
 
 struct sbi_scratch;
 
+int sbi_smrnmi_hart_init(struct sbi_scratch *scratch);
 int sbi_hart_reinit(struct sbi_scratch *scratch);
 int sbi_hart_init(struct sbi_scratch *scratch, bool cold_boot);
 
diff --git a/include/sbi/sbi_platform.h b/include/sbi/sbi_platform.h
index 1e9a23c1252558879c683633ad000828fbfa8f3f..d55805defafb0a7935d0d95653aefcc622078f69 100644
--- a/include/sbi/sbi_platform.h
+++ b/include/sbi/sbi_platform.h
@@ -155,7 +155,7 @@ struct sbi_platform_operations {
 	void (*pmp_disable)(unsigned int n);
 
 	/** platform specific Smrnmi handlers init on current HART */
-	void (*smrnmi_handlers_init)(void (*rnmi_handler)(void),
+	int (*smrnmi_handlers_init)(void (*rnmi_handler)(void),
 			void (*rnme_handler)(void));
 
 	/** platform specific Smrnmi NMI handler.
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index bee8855772d8bfe452a1f9f086ece2a7ab8bbe6b..287d7ba7c4e110d77b3cb68929a6309e15ccd7fe 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -466,6 +466,37 @@ static int hart_mhpm_get_allowed_bits(void)
 	return num_bits;
 }
 
+int sbi_smrnmi_hart_init(struct sbi_scratch *scratch)
+{
+	extern void _trap_rnmi_handler(void);
+	extern void _trap_handler(void);
+	const struct sbi_platform *plat;
+	const struct sbi_platform_operations *ops;
+	int ret;
+
+	if (!sbi_hart_has_extension(scratch, SBI_HART_EXT_SMRNMI))
+		return 0;
+
+	plat = sbi_platform_thishart_ptr();
+	ops  = plat ? sbi_platform_ops(plat) : NULL;
+
+	/*
+	 * Platforms with fixed or mtvec-based NMI vectors need no
+	 * vendor register programming; NULL callback is valid.
+	 */
+	if (ops && ops->smrnmi_handlers_init) {
+		ret = ops->smrnmi_handlers_init(_trap_rnmi_handler,
+						_trap_handler);
+		if (ret)
+			return ret;
+	}
+
+	csr_write(CSR_MNSCRATCH, scratch);
+	csr_set(CSR_MNSTATUS, MNSTATUS_NMIE);
+
+	return 0;
+}
+
 static int hart_detect_features(struct sbi_scratch *scratch, bool cold_boot)
 {
 	struct sbi_trap_info trap = {0};
@@ -487,25 +518,9 @@ static int hart_detect_features(struct sbi_scratch *scratch, bool cold_boot)
 	if (rc)
 		return rc;
 
-	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SMRNMI)) {
-		const struct sbi_platform *plat = sbi_platform_thishart_ptr();
-		const struct sbi_platform_operations *ops = sbi_platform_ops(plat);
-		extern void _trap_rnmi_handler(void);
-		extern void _trap_handler(void);
-
-		if (!ops || !ops->smrnmi_handlers_init)
-			sbi_panic("Smrnmi detected, but platform lacks smrnmi_handlers_init callback\n");
-
-		/* Reuse _trap_handler for the RNME slot since RNME is taken
-		 * as a regular M-mode trap with NMIE=0. */
-		ops->smrnmi_handlers_init(_trap_rnmi_handler, _trap_handler);
-
-		/* Initialize MNSCRATCH for the RNMI handler */
-		csr_write(CSR_MNSCRATCH, scratch);
-
-		/* Enable NMIs */
-		csr_set(CSR_MNSTATUS, MNSTATUS_NMIE);
-	}
+	rc = sbi_smrnmi_hart_init(scratch);
+	if (rc)
+		return rc;
 
 #define __check_hpm_csr(__csr, __mask) 					  \
 	oldval = csr_read_allowed(__csr, &trap);			  \
@@ -698,7 +713,7 @@ int sbi_hart_reinit(struct sbi_scratch *scratch)
 	if (rc)
 		return rc;
 
-	return 0;
+	return sbi_smrnmi_hart_init(scratch);
 }
 
 int sbi_hart_init(struct sbi_scratch *scratch, bool cold_boot)

-- 
2.43.7