[PATCH v2 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>
---
 lib/sbi/sbi_hart.c | 49 ++++++++++++++++++++++++++++++-------------------
 1 file changed, 30 insertions(+), 19 deletions(-)

diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index bee8855772d8bfe452a1f9f086ece2a7ab8bbe6b..d2b5be6a3ce68e6ab5c368e83b9cd0f09c4ca337 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);			  \
@@ -698,6 +707,8 @@ int sbi_hart_reinit(struct sbi_scratch *scratch)
 	if (rc)
 		return rc;
 
+	sbi_smrnmi_hart_init(scratch);
+
 	return 0;
 }
 

-- 
2.43.7