Support boot CPU not being 0.

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 */
+
+        /* 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 (!node)
+    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,15 +289,15 @@ 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)
 {
     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[];