Support boot CPU not being 0.

Right now, we assume the boot cpu is cpu0.  That is not true on m3 max,
where it is CPU 4.

To figure out which CPU is the boot CPU, we check to see which CPU
is running before we start any other CPUs, and record the MPIDR/idx.

Without this patch, four issues happen on m3 max:

1. We try to start the boot CPU again, crashing it
2. We skip the real CPU 0
3. We start m1n1 again on CPU0 when we boot it
4. We enable interrupts on CPU0 because we think it's the primary CPU.

Signed-off-by: Daniel Berlin <dberlin@dberlin.org>

src/exception.c

@@ -134,7 +134,7 @@ void exception_initialize(void)
     reg_clr(SYS_IMP_APL_UPMCR0, UPMCR0_IMODE_MASK);
     msr(SYS_IMP_APL_IPI_SR_EL1, IPI_SR_PENDING);
 
-    if (is_primary_core())
+    if (is_boot_cpu())
         msr(DAIF, 0 << 6); // Enable SError, IRQ and FIQ
     else
         msr(DAIF, 3 << 6); // Disable IRQ and FIQ

src/hv.c

@@ -111,9 +111,15 @@ static void hv_set_gxf_vbar(void)
 
 void hv_start(void *entry, u64 regs[4])
 {
+    if (boot_cpu_idx == -1) {
+        printf("Boot CPU has not been found, can't start hypervisor\n");
+        return;
+    }
+
     memset(hv_should_exit, 0, sizeof(hv_should_exit));
     memset(hv_started_cpus, 0, sizeof(hv_started_cpus));
-    hv_started_cpus[0] = 1;
+
+    hv_started_cpus[boot_cpu_idx] = true;
 
     msr(VBAR_EL1, _hv_vectors_start);
 
@@ -155,9 +161,12 @@ void hv_start(void *entry, u64 regs[4])
     udelay(200000);
     spin_lock(&bhl);
 
-    hv_started_cpus[0] = false;
+    hv_started_cpus[boot_cpu_idx] = false;
 
-    for (int i = 1; i < MAX_CPUS; i++) {
+    for (int i = 0; i < MAX_CPUS; i++) {
+        if (i == boot_cpu_idx) {
+            continue;
+        }
         hv_should_exit[i] = true;
         if (hv_started_cpus[i]) {
             printf("HV: Waiting for CPU %d to exit\n", i);

src/hv_wdt.c

@@ -129,9 +129,12 @@ void hv_wdt_breadcrumb(char c)
 
 void hv_wdt_init(void)
 {
-    int node = adt_path_offset(adt, "/cpus/cpu0");
+    char boot_cpu_name[32];
+
+    snprintf(boot_cpu_name, sizeof(boot_cpu_name), "/cpus/cpu%d", boot_cpu_idx);
+    int node = adt_path_offset(adt, boot_cpu_name);
     if (node < 0) {
-        printf("Error getting /cpus/cpu0 node\n");
+        printf("Error getting %s node\n", boot_cpu_name);
         return;
     }
 

src/iodev.c

@@ -150,7 +150,7 @@ void iodev_console_write(const void *buf, size_t length)
 {
     bool do_lock = mmu_active();
 
-    if (!do_lock && !is_primary_core()) {
+    if (!do_lock && !is_boot_cpu()) {
         if (length && iodevs[IODEV_UART]->usage & USAGE_CONSOLE) {
             iodevs[IODEV_UART]->ops->write(iodevs[IODEV_UART]->opaque, "*", 1);
             iodevs[IODEV_UART]->ops->write(iodevs[IODEV_UART]->opaque, buf, length);

src/payload.c

@@ -290,7 +290,9 @@ int payload_run(void)
         if (enable_tso) {
 
             do_enable_tso();
-            for (int i = 1; i < MAX_CPUS; i++) {
+            for (int i = 0; i < MAX_CPUS; i++) {
+                if (i == boot_cpu_idx)
+                    continue;
                 if (smp_is_alive(i)) {
                     smp_call0(i, do_enable_tso);
                     smp_wait(i);

src/smp.c

@@ -43,6 +43,8 @@ static u64 pmgr_reg;
 static u64 cpu_start_off;
 
 extern u8 _vectors_start[0];
+int boot_cpu_idx = -1;
+u64 boot_cpu_mpidr = 0;
 
 void smp_secondary_entry(void)
 {
@@ -238,17 +240,46 @@ void smp_start_secondaries(void)
         cpu_nodes[cpu_id] = node;
     }
 
-    for (int i = 1; i < MAX_CPUS; i++) {
-        int node = cpu_nodes[i];
+    /* The boot cpu id never changes once set */
+    if (boot_cpu_idx == -1) {
+        /* Figure out which CPU we are on by seeing which CPU is running */
 
-        if (!node)
+        /* This seems silly but it's what XNU does */
+        for (int i = 0; i < MAX_CPUS; i++) {
+            int cpu_node = cpu_nodes[i];
+            if (!cpu_node)
+                continue;
+            const char *state = adt_getprop(adt, cpu_node, "state", NULL);
+            if (!state)
+                continue;
+            if (strcmp(state, "running") == 0) {
+                boot_cpu_idx = i;
+                boot_cpu_mpidr = mrs(MPIDR_EL1);
+                break;
+            }
+        }
+    }
+
+    if (boot_cpu_idx == -1) {
+        printf(
+            "Could not find currently running CPU in cpu table, can't start other processors!\n");
+        return;
+    }
+
+    for (int i = 0; i < MAX_CPUS; i++) {
+
+        if (i == boot_cpu_idx)
+            continue;
+        int cpu_node = cpu_nodes[i];
+
+        if (!cpu_node)
             continue;
 
         u32 reg;
         u64 cpu_impl_reg[2];
-        if (ADT_GETPROP(adt, node, "reg", &reg) < 0)
+        if (ADT_GETPROP(adt, cpu_node, "reg", &reg) < 0)
             continue;
-        if (ADT_GETPROP_ARRAY(adt, node, "cpu-impl-reg", cpu_impl_reg) < 0)
+        if (ADT_GETPROP_ARRAY(adt, cpu_node, "cpu-impl-reg", cpu_impl_reg) < 0)
             continue;
 
         u8 core = FIELD_GET(CPU_REG_CORE, reg);
@@ -258,7 +289,7 @@ void smp_start_secondaries(void)
         smp_start_cpu(i, die, cluster, core, cpu_impl_reg[0], pmgr_reg + cpu_start_off);
     }
 
-    spin_table[0].mpidr = mrs(MPIDR_EL1) & 0xFFFFFF;
+    spin_table[boot_cpu_idx].mpidr = mrs(MPIDR_EL1) & 0xFFFFFF;
 }
 
 void smp_stop_secondaries(bool deep_sleep)
@@ -266,7 +297,7 @@ void smp_stop_secondaries(bool deep_sleep)
     printf("Stopping secondary CPUs...\n");
     smp_set_wfe_mode(true);
 
-    for (int i = 1; i < MAX_CPUS; i++) {
+    for (int i = 0; i < MAX_CPUS; i++) {
         int node = cpu_nodes[i];
 
         if (!node)
@@ -342,8 +373,8 @@ void smp_set_wfe_mode(bool new_mode)
     wfe_mode = new_mode;
     sysop("dsb sy");
 
-    for (int cpu = 1; cpu < MAX_CPUS; cpu++)
-        if (smp_is_alive(cpu))
+    for (int cpu = 0; cpu < MAX_CPUS; cpu++)
+        if (cpu != boot_cpu_idx && smp_is_alive(cpu))
             smp_send_ipi(cpu);
 
     sysop("sev");

src/smp.h

@@ -39,4 +39,5 @@ static inline int smp_id(void)
         return mrs(TPIDR_EL1);
 }
 
+extern int boot_cpu_idx;
 #endif

src/startup.c

@@ -106,7 +106,7 @@ void _start_c(void *boot_args, void *base)
 /* Secondary SMP core boot */
 void _cpu_reset_c(void *stack)
 {
-    if (mrs(MPIDR_EL1) & 0xffffff)
+    if (!is_boot_cpu())
         uart_puts("RVBAR entry on secondary CPU");
     else
         uart_puts("RVBAR entry on primary CPU");
@@ -118,7 +118,7 @@ void _cpu_reset_c(void *stack)
 
     exception_initialize();
 
-    if (mrs(MPIDR_EL1) & 0xffffff)
+    if (!is_boot_cpu())
         smp_secondary_entry();
     else
         m1n1_main();

src/utils.h

@@ -334,9 +334,11 @@ static inline int in_el2(void)
     return (mrs(CurrentEL) >> 2) == 2;
 }
 
-static inline int is_primary_core(void)
+extern int boot_cpu_idx;
+extern u64 boot_cpu_mpidr;
+static inline int is_boot_cpu(void)
 {
-    return mrs(MPIDR_EL1) == 0x80000000;
+    return boot_cpu_idx == -1 || boot_cpu_mpidr == mrs(MPIDR_EL1);
 }
 
 extern char _base[];