[PATCH v3 1/3] lib: sbi: extract sbi_smrnmi_hart_init() and call on non-retentive resume

[Nia Su]

On non-retentive suspend resume, the hart never initializes the
Smrnmi CSRs the way hart_detect_features() does at boot, so a hart
waking from non-retentive suspend resumes with NMIs disabled and no
RNMI handler installed.

Extract the setup into sbi_smrnmi_hart_init() and call it from both
hart_detect_features() and sbi_hart_reinit().

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_platform.h |  5 ++++-
 lib/sbi/sbi_hart.c         | 55 +++++++++++++++++++++++++---------------------
 2 files changed, 34 insertions(+), 26 deletions(-)

diff --git a/include/sbi/sbi_platform.h b/include/sbi/sbi_platform.h
index 1e9a23c1252558879c683633ad000828fbfa8f3f..0a1e7eee0bae8eee963e5a4ebc0895b2bd10dd23 100644
--- a/include/sbi/sbi_platform.h
+++ b/include/sbi/sbi_platform.h
@@ -154,7 +154,10 @@ struct sbi_platform_operations {
 	/** platform specific pmp disable on current HART */
 	void (*pmp_disable)(unsigned int n);
 
-	/** platform specific Smrnmi handlers init on current HART */
+	/** platform specific Smrnmi handlers init on current HART.
+	 *  May be called more than once on the same HART (e.g. once
+	 *  during boot feature detection and again during hart
+	 *  re-init), so it must be safe to call repeatedly. */
 	void (*smrnmi_handlers_init)(void (*rnmi_handler)(void),
 			void (*rnme_handler)(void));
 
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index bee8855772d8bfe452a1f9f086ece2a7ab8bbe6b..18fac76ac070cb626ccb1cc959f4327de0e7c1d8 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -466,6 +466,33 @@ static int hart_mhpm_get_allowed_bits(void)
 	return num_bits;
 }
 
+static void sbi_smrnmi_hart_init(struct sbi_scratch *scratch)
+{
+	const struct sbi_platform *plat;
+	const struct sbi_platform_operations *ops;
+	extern void _trap_rnmi_handler(void);
+	extern void _trap_handler(void);
+
+	if (!sbi_hart_has_extension(scratch, SBI_HART_EXT_SMRNMI))
+		return;
+
+	plat = sbi_platform_thishart_ptr();
+	ops  = sbi_platform_ops(plat);
+
+	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);
+}
+
 static int hart_detect_features(struct sbi_scratch *scratch, bool cold_boot)
 {
 	struct sbi_trap_info trap = {0};
@@ -487,25 +514,7 @@ 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);
-	}
+	sbi_smrnmi_hart_init(scratch);
 
 #define __check_hpm_csr(__csr, __mask) 					  \
 	oldval = csr_read_allowed(__csr, &trap);			  \
@@ -690,15 +699,11 @@ __pmp_skip:
 
 int sbi_hart_reinit(struct sbi_scratch *scratch)
 {
-	int rc;
+	sbi_smrnmi_hart_init(scratch);
 
 	mstatus_init(scratch);
 
-	rc = fp_init(scratch);
-	if (rc)
-		return rc;
-
-	return 0;
+	return fp_init(scratch);
 }
 
 int sbi_hart_init(struct sbi_scratch *scratch, bool cold_boot)

-- 
2.43.7