[PATCH v2 5/9] lib: sbi: Initialize as much as possible on non-coldboot HART

[Xiang W]

Separate sbi_hsm_hart_wait from sbi_hsm_init to allow the secondary
Hart to perform more initialization.

Signed-off-by: Xiang W <wxjstz at 126.com>
---
 include/sbi/sbi_hsm.h |  3 ++-
 lib/sbi/sbi_hsm.c     | 49 ++++++++++++++++++-------------------------
 lib/sbi/sbi_init.c    | 15 ++++++-------
 3 files changed, 28 insertions(+), 39 deletions(-)

diff --git a/include/sbi/sbi_hsm.h b/include/sbi/sbi_hsm.h
index 38f90516..3be08654 100644
--- a/include/sbi/sbi_hsm.h
+++ b/include/sbi/sbi_hsm.h
@@ -63,7 +63,8 @@ const struct sbi_hsm_device *sbi_hsm_get_device(void);
 
 void sbi_hsm_set_device(const struct sbi_hsm_device *dev);
 
-int sbi_hsm_init(struct sbi_scratch *scratch, bool cold_boot);
+int sbi_hsm_init(struct sbi_scratch *scratch);
+void sbi_hsm_hart_wait(struct sbi_scratch *scratch);
 void __noreturn sbi_hsm_exit(struct sbi_scratch *scratch);
 
 int sbi_hsm_hart_start(struct sbi_scratch *scratch,
diff --git a/lib/sbi/sbi_hsm.c b/lib/sbi/sbi_hsm.c
index 557ab131..f32212d8 100644
--- a/lib/sbi/sbi_hsm.c
+++ b/lib/sbi/sbi_hsm.c
@@ -159,7 +159,7 @@ void __noreturn sbi_hsm_hart_start_finish(struct sbi_scratch *scratch,
 	sbi_hart_switch_mode(hartid, next_arg1, next_addr, next_mode, false);
 }
 
-static void sbi_hsm_hart_wait(struct sbi_scratch *scratch)
+void sbi_hsm_hart_wait(struct sbi_scratch *scratch)
 {
 	unsigned long saved_mie;
 	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
@@ -183,14 +183,10 @@ static void sbi_hsm_hart_wait(struct sbi_scratch *scratch)
 
 		wfi();
 	}
+	sbi_ipi_raw_clear();
 
 	/* Restore MIE CSR */
 	csr_write(CSR_MIE, saved_mie);
-
-	/*
-	 * No need to clear IPI here because the sbi_ipi_init() will
-	 * clear it for current HART.
-	 */
 }
 
 const struct sbi_hsm_device *sbi_hsm_get_device(void)
@@ -247,34 +243,29 @@ static void hsm_device_hart_resume(void)
 		hsm_dev->hart_resume();
 }
 
-int sbi_hsm_init(struct sbi_scratch *scratch, bool cold_boot)
+int sbi_hsm_init(struct sbi_scratch *scratch)
 {
 	struct sbi_scratch *rscratch;
 	struct sbi_hsm_data *hdata;
 
-	if (cold_boot) {
-		hart_data_offset = sbi_scratch_alloc_offset(sizeof(*hdata));
-		if (!hart_data_offset)
-			return SBI_ENOMEM;
-
-		/* Initialize hart state data for every hart */
-		sbi_for_each_hartindex(i) {
-			rscratch = sbi_hartindex_to_scratch(i);
-			if (!rscratch)
-				continue;
-
-			hdata = sbi_scratch_offset_ptr(rscratch,
-						       hart_data_offset);
-			ATOMIC_INIT(&hdata->state,
-				    (i == current_hartindex()) ?
-				    SBI_HSM_STATE_START_PENDING :
-				    SBI_HSM_STATE_STOPPED);
-			ATOMIC_INIT(&hdata->start_ticket, 0);
-		}
-	} else {
-		sbi_hsm_hart_wait(scratch);
+	hart_data_offset = sbi_scratch_alloc_offset(sizeof(*hdata));
+	if (!hart_data_offset)
+		return SBI_ENOMEM;
+
+	/* Initialize hart state data for every hart */
+	sbi_for_each_hartindex(i) {
+		rscratch = sbi_hartindex_to_scratch(i);
+		if (!rscratch)
+			continue;
+
+		hdata = sbi_scratch_offset_ptr(rscratch,
+					       hart_data_offset);
+		ATOMIC_INIT(&hdata->state,
+			    (i == current_hartindex()) ?
+			    SBI_HSM_STATE_START_PENDING :
+			    SBI_HSM_STATE_STOPPED);
+		ATOMIC_INIT(&hdata->start_ticket, 0);
 	}
-
 	return 0;
 }
 
diff --git a/lib/sbi/sbi_init.c b/lib/sbi/sbi_init.c
index 4c55b3dc..5c017cda 100644
--- a/lib/sbi/sbi_init.c
+++ b/lib/sbi/sbi_init.c
@@ -223,6 +223,10 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 	if (rc)
 		sbi_hart_hang();
 
+	rc = sbi_hsm_init(scratch);
+	if (rc)
+		sbi_hart_hang();
+
 	entry_count_offset = sbi_scratch_alloc_offset(__SIZEOF_POINTER__);
 	if (!entry_count_offset)
 		sbi_hart_hang();
@@ -234,10 +238,6 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 	count = sbi_scratch_offset_ptr(scratch, entry_count_offset);
 	(*count)++;
 
-	rc = sbi_hsm_init(scratch, true);
-	if (rc)
-		sbi_hart_hang();
-
 	/*
 	 * All non-coldboot HARTs do HSM initialization (i.e. enter HSM state
 	 * machine) at the start of the warmboot path so it is wasteful to
@@ -400,11 +400,6 @@ static void __noreturn init_warm_startup(struct sbi_scratch *scratch,
 	count = sbi_scratch_offset_ptr(scratch, entry_count_offset);
 	(*count)++;
 
-	/* Note: This has to be first thing in warmboot init sequence */
-	rc = sbi_hsm_init(scratch, false);
-	if (rc)
-		sbi_hart_hang();
-
 	rc = sbi_platform_early_init(plat, false);
 	if (rc)
 		sbi_hart_hang();
@@ -457,6 +452,8 @@ static void __noreturn init_warm_startup(struct sbi_scratch *scratch,
 	if (rc)
 		sbi_hart_hang();
 
+	sbi_hsm_hart_wait(scratch);
+
 	count = sbi_scratch_offset_ptr(scratch, init_count_offset);
 	(*count)++;
 
-- 
2.47.2