1 files changed, 69 insertions, 44 deletions

diff --git a/src/arch/riscv/smp.c b/src/arch/riscv/smp.c
index 0a93763cb0..67dc13b8fc 100644
--- a/src/arch/riscv/smp.c
+++ b/src/arch/riscv/smp.c
@@ -7,68 +7,93 @@
 #include <console/console.h>
 #include <mcall.h>
 
+// made up value to sync hart state
+#define HART_SLEEPING 0x1
+#define HART_AWAKE    0x2
+
 void smp_pause(int working_hartid)
 {
-#define SYNCA (OTHER_HLS(working_hartid)->entry.sync_a)
-#define SYNCB (OTHER_HLS(working_hartid)->entry.sync_b)
-
 	int hartid = read_csr(mhartid);
 
+	// pause all harts which are not the working hart
 	if (hartid != working_hartid) {
-		/* waiting for work hart */
-		do {
-			barrier();
-		} while (atomic_read(&SYNCA) != 0x01234567);
-
-		clear_csr(mstatus, MSTATUS_MIE);
-		write_csr(mie, MIP_MSIP);
-
-		/* count how many cores enter the halt */
-		atomic_add(&SYNCB, 1);
+		clear_csr(mstatus, MSTATUS_MIE); // disable all interrupts
+		set_msip(hartid, 0); // clear pending interrupts
+		write_csr(mie, MIP_MSIP); // enable only IPI (for smp_resume)
+		barrier();
+		atomic_set(&HLS()->entry.sync_a, HART_SLEEPING); // mark the hart as sleeping.
 
+		// pause hart
 		do {
-			barrier();
-			__asm__ volatile ("wfi");
+			__asm__ volatile ("wfi"); // wait for interrupt
 		} while ((read_csr(mip) & MIP_MSIP) == 0);
-		set_msip(hartid, 0);
-		HLS()->entry.fn(HLS()->entry.arg);
-	} else {
-		/* Initialize the counter and
-		 * mark the work hart into smp_pause */
-		atomic_set(&SYNCB, 0);
-		atomic_set(&SYNCA, 0x01234567);
-
-		/* waiting for other Hart to enter the halt */
-		do {
-			barrier();
-		} while (atomic_read(&SYNCB) + 1 < CONFIG_MAX_CPUS);
 
-		/* initialize for the next call */
-		atomic_set(&SYNCA, 0);
-		atomic_set(&SYNCB, 0);
+		atomic_set(&HLS()->entry.sync_a, HART_AWAKE); // mark the hart as awake
+		HLS()->entry.fn(HLS()->entry.arg);
 	}
-#undef SYNCA
-#undef SYNCB
 }
 
+// must only be called by the WORKING_HARTID
 void smp_resume(void (*fn)(void *), void *arg)
 {
-	int hartid = read_csr(mhartid);
+	if (fn == NULL) {
+		printk(BIOS_ERR, "must pass a non-null function pointer\n");
+		return; // we can still boot with one hart
+	}
+
+	int working_hartid = read_csr(mhartid);
+
+	int hart_count = CONFIG_MAX_CPUS;
+	if (CONFIG(RISCV_GET_HART_COUNT_AT_RUNTIME))
+		hart_count = smp_get_hart_count();
 
-	if (fn == NULL)
-		die("must pass a non-null function pointer\n");
+	// check that all harts are present
 
-	for (int i = 0; i < CONFIG_MAX_CPUS; i++) {
-		OTHER_HLS(i)->entry.fn = fn;
-		OTHER_HLS(i)->entry.arg = arg;
+	u32 count_awake_harts = 0;
+	for (int i = 0; i < hart_count; i++) {
+		// The working hart never sleeps. It is a hard working hart.
+		if (i == working_hartid)
+			continue;
+
+		if (atomic_read(&OTHER_HLS(i)->entry.sync_a) != HART_SLEEPING) {
+			/*
+			 * we assmue here that the time between smp_pause and smp_resume
+			 * is enough for all harts to reach the smp_pause state.
+			 * But for some reason that was not the case for this hart ...
+			 */
+			printk(BIOS_ERR, "hart %d did not enter smp_pause\n", i);
+			OTHER_HLS(i)->enabled = 0; // disable hart
+		} else {
+			// hart is in wfi (wait for interrupt) state like it should be.
+
+			OTHER_HLS(i)->entry.fn = fn;
+			OTHER_HLS(i)->entry.arg = arg;
+			barrier();
+			set_msip(i, 1); // wake up hart
+		}
 	}
 
-	for (int i = 0; i < CONFIG_MAX_CPUS; i++)
-		if (i != hartid)
-			set_msip(i, 1);
+	printk(BIOS_DEBUG, "waiting for all harts to wake up...\n");
+	// confirm that all harts are wake
+	for (int i = 0; i < hart_count; i++) {
+		// The working hart never sleeps. It is a hard working hart.
+		if (i == working_hartid || !OTHER_HLS(i)->enabled)
+			continue;
 
-	if (HLS()->entry.fn == NULL)
-		die("entry fn not set\n");
+		// wait for hart to publish its waking state
+		while (atomic_read(&OTHER_HLS(i)->entry.sync_a) != HART_AWAKE)
+			;
+		count_awake_harts++;
+	}
+	printk(BIOS_DEBUG, "all harts up and running...\n");
 
-	HLS()->entry.fn(HLS()->entry.arg);
+	if ((hart_count - 1) != count_awake_harts) { // exclude working hart
+		/*
+		 * Apparently one or more harts did not reach smp_pause before smp_resume has
+		 * been called by the working hart. That should not happen and may indicate we
+		 * need a timeout of sorts to make sure we get all harts resumed.
+		 */
+		printk(BIOS_ERR, "some harts were too slow and could not resume\n");
+	}
+	fn(arg); // jump to fn with working hart
 }