lib: sbi: Revert entry_count before doing hsm stop in hsm wait loop

Using hsm stop in hsm wait loop causes secondary harts to be stuck
forever in OpenSBI on RISC-V platforms where HSM hart hotplug is
available and all harts come-up at the same time during system
power-on.

For example, lets say we have two harts A and B on a RISC-V platform
with HSM hart hotplug which come-up at the same time during system
power-on. The hart A enters OpenSBI before hart B hence it becomes
the primary (or cold-boot) hart whereas hart B becomes the secondary
(or warm-boot) hart. The hart A follows the OpenSBI cold-boot path
and registers hsm device before hart B enters OpenSBI. The hart B
eventually enters OpenSBI and follows the OpenSBI warm-boot path
so it will increment it's own entry_count before entering hsm wait
loop where it sees hsm device and stops itself. Later as part of
the Linux boot-up sequence, hart A issues SBI HSM start call to
bring-up hart B but OpenSBI sees entry_count != init_count for
hart B in sbi_hsm_hart_start() hence hsm_device_hart_start() is
not called for hart B resulting in hart B stuck forever in OpenSBI.

To fix the above issue, revert entry_count before doing hsm stop
in hsm wait loop.

Fixes: d844dea ("lib: sbi: Use hsm stop for hsm wait")
Signed-off-by: Anup Patel <[email protected]>
Reviewed-by: Nick Hu <[email protected]>
Link: https://lore.kernel.org/r/[email protected]
Signed-off-by: Anup Patel <[email protected]>

Author: avpatel

include/sbi/sbi_init.h

@@ -16,6 +16,8 @@ struct sbi_scratch;
 
 void __noreturn sbi_init(struct sbi_scratch *scratch);
 
+void sbi_revert_entry_count(struct sbi_scratch *scratch);
+
 unsigned long sbi_entry_count(u32 hartindex);
 
 unsigned long sbi_init_count(u32 hartindex);

lib/sbi/sbi_hsm.c

@@ -176,8 +176,10 @@ static void sbi_hsm_hart_wait(struct sbi_scratch *scratch)
 		 * If the hsm_dev is ready and it support the hotplug, we can
 		 * use the hsm stop for more power saving
 		 */
-		if (hsm_device_has_hart_hotplug())
+		if (hsm_device_has_hart_hotplug()) {
+			sbi_revert_entry_count(scratch);
 			hsm_device_hart_stop();
+		}
 
 		wfi();
 	}

lib/sbi/sbi_init.c

@@ -579,6 +579,19 @@ void __noreturn sbi_init(struct sbi_scratch *scratch)
 		init_warmboot(scratch, hartid);
 }
 
+void sbi_revert_entry_count(struct sbi_scratch *scratch)
+{
+	unsigned long *entry_count, *init_count;
+
+	if (!entry_count_offset || !init_count_offset)
+		sbi_hart_hang();
+
+	entry_count = sbi_scratch_offset_ptr(scratch, entry_count_offset);
+	init_count = sbi_scratch_offset_ptr(scratch, init_count_offset);
+
+	*entry_count = *init_count;
+}
+
 unsigned long sbi_entry_count(u32 hartindex)
 {
 	struct sbi_scratch *scratch;