[arch][riscv] change secondary cpu bootstrap api

Instead of setting a counter of the number of secondaries to start, have
platform or target code pass in a list of harts to start instead. This
allows for there to be discontinuties in the layout of the cpu harts, or
in the case of some sifive based hardware, hart 0 is otherwise offline.

5 files changed, 125 insertions, 49 deletions

diff --git a/arch/riscv/include/arch/riscv.h b/arch/riscv/include/arch/riscv.h
index ac265f0d..48875575 100644
--- a/arch/riscv/include/arch/riscv.h
+++ b/arch/riscv/include/arch/riscv.h
@@ -230,7 +230,10 @@ static inline uint riscv_current_hart(void) {
 #endif
 }
 
-void riscv_set_secondary_count(int count);
+// Platform should pass in a list of secondary harts to start, not
+// including the boot hart. Will be trimmed to SMP_MAX_CPUS - 1.
+// Machine mode will always get all of the secondaries released (for now).
+void riscv_set_secondary_harts_to_start(const uint *harts, size_t count);
 
 void riscv_exception_entry(void);
 enum handler_return riscv_timer_exception(void);
diff --git a/arch/riscv/mp.c b/arch/riscv/mp.c
index 49b4bfaf..2fcf72b9 100644
--- a/arch/riscv/mp.c
+++ b/arch/riscv/mp.c
@@ -7,6 +7,8 @@
  * https://opensource.org/licenses/MIT
  */
 
+#include <arch/riscv.h>
+
 #include <lk/reg.h>
 #include <lk/compiler.h>
 #include <lk/debug.h>
@@ -14,6 +16,8 @@
 #include <lk/err.h>
 #include <lk/init.h>
 #include <lk/main.h>
+#include <stdlib.h>
+#include <string.h>
 
 #include <arch/atomic.h>
 #include <arch/ops.h>
@@ -27,13 +31,16 @@
 #define LOCAL_TRACE 0
 
 // mapping of cpu -> hart
-static int cpu_to_hart_map[SMP_MAX_CPUS];
+static uint cpu_to_hart_map[SMP_MAX_CPUS];
 
 // list of IPIs queued per cpu
 static volatile int ipi_data[SMP_MAX_CPUS];
 
 static spin_lock_t boot_cpu_lock = 1;
-volatile int secondaries_to_init = SMP_MAX_CPUS - 1;
+
+// list of cpus to boot, passed from platform
+static uint harts_to_boot[SMP_MAX_CPUS];
+static uint harts_to_boot_count = 0;
 
 // modified in start.S to save the physical address of _start as the first cpu boots
 uintptr_t _start_physical;
@@ -136,53 +143,56 @@ void riscv_secondary_entry(uint hart_id, uint __unused, uint cpu_id) {
             riscv_get_mvendorid(), riscv_get_marchid(),
             riscv_get_mimpid(), riscv_current_hart());
 
-    // atomic_add(&secondaries_to_init, -1);
-    // arch_mp_send_ipi(1 << 0, MP_IPI_GENERIC); // wake up hart0 to let it know this CPU has come up
-
     lk_secondary_cpu_entry();
 }
 
-// platform can detect and set the number of cores to boot (optional)
-void riscv_set_secondary_count(int count) {
-    if (count > SMP_MAX_CPUS - 1) {
-        count = SMP_MAX_CPUS - 1;
-    }
-    secondaries_to_init = count;
+// platform hands the arch layer a list of harts to start, these will be
+// started later in riscv_boot_secondaries()
+void riscv_set_secondary_harts_to_start(const uint *harts, size_t count) {
+    harts_to_boot_count = MIN(count, SMP_MAX_CPUS);
+    memcpy(harts_to_boot, harts, harts_to_boot_count * sizeof(harts[0]));
 }
 
 // start any secondary cpus we are set to start. called on the boot processor
 void riscv_boot_secondaries(void) {
     // if theres nothing to start, abort here
-    if (secondaries_to_init == 0) {
+    if (harts_to_boot_count == 0) {
         dprintf(INFO, "RISCV: No secondary harts to start\n");
         return;
     }
 
-    lk_init_secondary_cpus(secondaries_to_init);
+    lk_init_secondary_cpus(harts_to_boot_count);
 
 #if RISCV_M_MODE
-    dprintf(INFO, "RISCV: Releasing %d secondary harts from purgatory\n", secondaries_to_init);
+    dprintf(INFO, "RISCV: Releasing all secondary harts from purgatory\n");
+
+    // For machine mode, we simply unblock all the trapped cpus in start.S by releasing
+    // the boot_cpu_lock. Via some mechanism or other, all of the cpus should have already entered
+    // start.S via the main entry point for this to work. This is the default behavior when
+    // running on QEMU, for example.
+    spin_unlock(&boot_cpu_lock);
 #else
-    uint boot_hart = riscv_current_hart();
-    dprintf(INFO, "RISCV: Going to try to start %d secondary harts\n", secondaries_to_init);
+    dprintf(INFO, "RISCV: Going to try to start %d secondary harts\n", harts_to_boot_count);
+
+    // May as well release the boot cpu lock now, since there's no reason to hold back secondaries we
+    // haven't started yet.
+    spin_unlock(&boot_cpu_lock);
 
     // use SBI HSM to boot the secondaries
-    // TODO: handle the range of harts we should consider, since they
-    // may not be zero based.
-    // Currently, starts from 0 and tries to start one extra core, with the idea
-    // that boot_hart should be one of them. This algorithm is somewhat broken, but
-    // works in the case of harts being 0-N and the boot hart being nonzero (but within [0...N]).
-    // Doesn't currently handle skipping cpus we shouldn't boot (like HART 0 on some machines)
-    for (uint i = 0; i <= (uint)secondaries_to_init; i++) {
+    uint boot_hart = riscv_current_hart();
+    for (uint i = 0; i < harts_to_boot_count; i++) {
         // skip the boot hart
-        if (i != boot_hart) {
-            dprintf(INFO, "RISCV: using SBI to start hart %u at %#lx\n", i, _start_physical);
-            sbi_boot_hart(i, _start_physical, 0);
+        if (harts_to_boot[i] != boot_hart) {
+            dprintf(INFO, "RISCV: using SBI to start hart %u at %#lx\n", harts_to_boot[i], _start_physical);
+            sbi_boot_hart(harts_to_boot[i], _start_physical, 0);
+
+            // Pause a little bit here to give the secondary an opportunity to start. Not strictly
+            // needed but helps the boot output flow without much of a boot penalty. Remove
+            // if it's a problem.
+            thread_sleep(50);
         }
     }
 #endif
-    /* release the secondary cpus */
-    spin_unlock(&boot_cpu_lock);
 }
 
 
diff --git a/arch/riscv/start.S b/arch/riscv/start.S
index 55d8a2c2..2394a62c 100644
--- a/arch/riscv/start.S
+++ b/arch/riscv/start.S
@@ -138,9 +138,8 @@ LOCAL_FUNCTION(secondary_trap)
 
     // bootstrap the secondary cpus
     jal     riscv_secondary_entry
-#else
-    j       hart_trap
 #endif
+    // fallthrough if either no SMP or riscv_secondary_entry returns
 END_FUNCTION(secondary_trap)
 
 LOCAL_FUNCTION(hart_trap)
diff --git a/platform/jh7110/platform.c b/platform/jh7110/platform.c
index 77bfe2a4..47c1eb8f 100644
--- a/platform/jh7110/platform.c
+++ b/platform/jh7110/platform.c
@@ -85,12 +85,23 @@ static void reserved_memory_callback(uint64_t base, uint64_t len, void *cookie)
     }
 }
 
+struct detected_cpus {
+    size_t count;
+
+    struct {
+        uint hart;
+    } cpu[SMP_MAX_CPUS];
+};
+
 static void cpucallback(uint64_t id, void *cookie) {
-    int *cpu_count = (int *)cookie;
+    struct detected_cpus *cpus = cookie;
 
-    LTRACEF("id %#llx cookie %p\n", id, cookie);
+    LTRACEF("hart %#llx\n", id);
 
-    (*cpu_count)++;
+    if (cpus->count < SMP_MAX_CPUS) {
+        cpus->cpu[cpus->count].hart = id;
+        cpus->count++;
+    }
 }
 
 struct pcie_detect_state {
@@ -112,8 +123,8 @@ void platform_early_init(void) {
 
     /* look for a flattened device tree in the second arg passed to us */
     bool found_mem = false;
-    int cpu_count = 0;
     struct reserved_memory_regions reserved = {0};
+    struct detected_cpus cpus = {0};
 
     const void *fdt = (void *)lk_boot_args[1];
 #if WITH_KERNEL_VM
@@ -126,7 +137,7 @@ void platform_early_init(void) {
         .reserved_memory = reserved_memory_callback,
         .reserved_memory_cookie = &reserved,
         .cpu = cpucallback,
-        .cpucookie = &cpu_count,
+        .cpucookie = &cpus,
         .pcie = pciecallback,
         .pciecookie = &pcie_state,
     };
@@ -163,12 +174,31 @@ void platform_early_init(void) {
     }
 #endif
 
-    if (cpu_count > 0) {
-        printf("FDT: found %d cpu%c\n", cpu_count, cpu_count == 1 ? ' ' : 's');
-        riscv_set_secondary_count(cpu_count - 1);
-    } else {
-        riscv_set_secondary_count(0);
+#if WITH_SMP
+    // TODO: refactor this code into libfdt
+    if (cpus.count > 0) {
+        printf("FDT: found %zu cpu%c\n", cpus.count, cpus.count == 1 ? ' ' : 's');
+        uint harts[SMP_MAX_CPUS - 1];
+
+        // copy from the detected cpu list to an array of harts, excluding the boot hart
+        size_t hart_index = 0;
+        for (size_t i = 0; i < cpus.count; i++) {
+            if (cpus.cpu[i].hart != riscv_current_hart()) {
+                harts[hart_index++] = cpus.cpu[i].hart;
+            }
+
+            // we can start MAX CPUS - 1 secondaries
+            if (hart_index >= SMP_MAX_CPUS - 1) {
+                break;
+            }
+        }
+
+        // tell the riscv layer how many cores we have to start
+        if (hart_index > 0) {
+            riscv_set_secondary_harts_to_start(harts, hart_index);
+        }
     }
+#endif
 
     LTRACEF("done scanning FDT\n");
 
diff --git a/platform/qemu-virt-riscv/platform.c b/platform/qemu-virt-riscv/platform.c
index 9fbfc371..9048707d 100644
--- a/platform/qemu-virt-riscv/platform.c
+++ b/platform/qemu-virt-riscv/platform.c
@@ -10,6 +10,7 @@
 #include <lk/main.h>
 #include <lk/reg.h>
 #include <lk/trace.h>
+#include <arch/riscv.h>
 #include <kernel/thread.h>
 #include <platform.h>
 #include <platform/interrupts.h>
@@ -92,12 +93,23 @@ static void reserved_memory_callback(uint64_t base, uint64_t len, void *cookie)
     }
 }
 
+struct detected_cpus {
+    size_t count;
+
+    struct {
+        uint hart;
+    } cpu[SMP_MAX_CPUS];
+};
+
 static void cpucallback(uint64_t id, void *cookie) {
-    int *cpu_count = (int *)cookie;
+    struct detected_cpus *cpus = cookie;
 
-    LTRACEF("id %#llx cookie %p\n", id, cookie);
+    LTRACEF("hart %#llx\n", id);
 
-    (*cpu_count)++;
+    if (cpus->count < SMP_MAX_CPUS) {
+        cpus->cpu[cpus->count].hart = id;
+        cpus->count++;
+    }
 }
 
 struct pcie_detect_state {
@@ -118,8 +130,8 @@ void platform_early_init(void) {
 
     /* look for a flattened device tree in the second arg passed to us */
     bool found_mem = false;
-    int cpu_count = 0;
     struct reserved_memory_regions reserved = {0};
+    struct detected_cpus cpus = {0};
 
     const void *fdt = (void *)lk_boot_args[1];
 #if WITH_KERNEL_VM
@@ -132,7 +144,7 @@ void platform_early_init(void) {
         .reserved_memory = reserved_memory_callback,
         .reserved_memory_cookie = &reserved,
         .cpu = cpucallback,
-        .cpucookie = &cpu_count,
+        .cpucookie = &cpus,
         .pcie = pciecallback,
         .pciecookie = &pcie_state,
     };
@@ -163,10 +175,32 @@ void platform_early_init(void) {
     }
 #endif
 
-    if (cpu_count > 0) {
-        printf("FDT: found %d cpu%c\n", cpu_count, cpu_count == 1 ? ' ' : 's');
-        riscv_set_secondary_count(cpu_count - 1);
+
+#if WITH_SMP
+    // TODO: refactor this code into libfdt
+    if (cpus.count > 0) {
+        printf("FDT: found %zu cpu%c\n", cpus.count, cpus.count == 1 ? ' ' : 's');
+        uint harts[SMP_MAX_CPUS - 1];
+
+        // copy from the detected cpu list to an array of harts, excluding the boot hart
+        size_t hart_index = 0;
+        for (size_t i = 0; i < cpus.count; i++) {
+            if (cpus.cpu[i].hart != riscv_current_hart()) {
+                harts[hart_index++] = cpus.cpu[i].hart;
+            }
+
+            // we can start MAX CPUS - 1 secondaries
+            if (hart_index >= SMP_MAX_CPUS - 1) {
+                break;
+            }
+        }
+
+        // tell the riscv layer how many cores we have to start
+        if (hart_index > 0) {
+            riscv_set_secondary_harts_to_start(harts, hart_index);
+        }
     }
+#endif
 
     LTRACEF("done scanning FDT\n");