smp: Allow switching the boot (primary) CPU core

proxyclient/m1n1/proxy.py

@@ -572,6 +572,7 @@ class M1N1Proxy(Reloadable):
     P_SMP_CALL_SYNC_EL1 = 0x508
     P_SMP_CALL_EL0 = 0x509
     P_SMP_CALL_SYNC_EL0 = 0x50a
+    P_SMP_SWITCH_BOOT_CPU = 0x50b
 
     P_HEAPBLOCK_ALLOC = 0x600
     P_MALLOC = 0x601
@@ -1004,6 +1005,8 @@ def smp_call_sync_el0(self, cpu, addr, *args):
         if len(args) > 3:
             raise ValueError("Too many arguments")
         return self.request(self.P_SMP_CALL_SYNC_EL0, cpu, addr, *args)
+    def smp_switch_boot_cpu(self, cpu):
+        return self.request(self.P_SMP_SWITCH_BOOT_CPU, cpu)
 
     def heapblock_alloc(self, size):
         return self.request(self.P_HEAPBLOCK_ALLOC, size)

proxyclient/tools/linux.py

@@ -124,6 +124,10 @@
             u.msr("ACTLR_EL1", actlr, call=lambda addr, *args: p.smp_call_sync(i, addr & ~REGION_RX_EL1, *args))
     p.kboot_set_chosen("apple,tso", "")
 
+if p.smp_switch_boot_cpu(0) != 0:
+    print("Switching primary CPU to core 0 failed")
+    sys.exit(1)
+
 if p.kboot_prepare_dt(dtb_addr):
     print("DT prepare failed")
     sys.exit(1)

src/payload.c

@@ -342,6 +342,13 @@ int payload_run(void)
                 printf("Failed to kboot set %s='%s'\n", chosen[i], val);
         }
 
+        if (boot_cpu_idx != 0) {
+            int boot_cpu = boot_cpu_idx;
+            int index = smp_switch_boot_cpu(0);
+            if (boot_cpu != index)
+                printf("Switched boot CPU from %d to %d (%d)\n", boot_cpu, index, boot_cpu_idx);
+        }
+
         if (kboot_prepare_dt(fdt)) {
             printf("Failed to prepare FDT!\n");
             return -1;

src/proxy.c

@@ -374,6 +374,9 @@ int proxy_process(ProxyRequest *request, ProxyReply *reply)
                       request->args[3], request->args[4]);
             reply->retval = smp_wait(request->args[0]);
             break;
+        case P_SMP_SWITCH_BOOT_CPU:
+            reply->retval = smp_switch_boot_cpu(request->args[0]);
+            break;
 
         case P_HEAPBLOCK_ALLOC:
             reply->retval = (u64)heapblock_alloc(request->args[0]);

src/proxy.h

@@ -92,6 +92,7 @@ typedef enum {
     P_SMP_CALL_EL1_SYNC,
     P_SMP_CALL_EL0,
     P_SMP_CALL_EL0_SYNC,
+    P_SMP_SWITCH_BOOT_CPU,
 
     P_HEAPBLOCK_ALLOC = 0x600, // Heap and memory management ops
     P_MALLOC,

src/smp.c

@@ -5,6 +5,8 @@
 #include "aic.h"
 #include "aic_regs.h"
 #include "cpu_regs.h"
+#include "exception.h"
+#include "fb.h"
 #include "malloc.h"
 #include "memory.h"
 #include "pmgr.h"
@@ -108,21 +110,8 @@ void smp_secondary_prep_el3(void)
     return;
 }
 
-static void smp_start_cpu(int index, int die, int cluster, int core, u64 impl, u64 cpu_start_base)
+static void smp_prepare_cpu(int index)
 {
-    int i;
-
-    if (index >= MAX_CPUS)
-        return;
-
-    if (has_el3() && index >= MAX_EL3_CPUS)
-        return;
-
-    if (spin_table[index].flag)
-        return;
-
-    printf("Starting CPU %d (%d:%d:%d)... ", index, die, cluster, core);
-
     memset(&spin_table[index], 0, sizeof(struct spin_table));
 
     target_cpu = index;
@@ -139,6 +128,24 @@ static void smp_start_cpu(int index, int die, int cluster, int core, u64 impl, u
     dc_civac_range(&_reset_stack, sizeof(void *));
 
     sysop("dsb sy");
+}
+
+static void smp_start_cpu(int index, int die, int cluster, int core, u64 impl, u64 cpu_start_base)
+{
+    int i;
+
+    if (index >= MAX_CPUS)
+        return;
+
+    if (has_el3() && index >= MAX_EL3_CPUS)
+        return;
+
+    if (spin_table[index].flag)
+        return;
+
+    printf("Starting CPU %d (%d:%d:%d)... ", index, die, cluster, core);
+
+    smp_prepare_cpu(index);
 
     write64(impl, (u64)_vectors_start);
 
@@ -414,6 +421,93 @@ void smp_stop_secondaries(bool deep_sleep)
     }
 }
 
+extern void cpu_reset(void) __attribute__((noreturn));
+extern void *smp_switch_boot_cpu_entry(int cpu_index);
+extern int smp_switch_boot_cpu_exit(void *prev_stack, u64 saved_sp);
+
+static u64 switch_boot_cpu_init_new(int cpu_index, int old_index, u64 saved_sp)
+{
+    int i;
+    void *prev_stack;
+
+    // wait for the previous previous boot CPU to be available as secondary
+    for (i = 0; i < 100; i++) {
+        sysop("dmb ld");
+        if (spin_table[old_index].flag)
+            break;
+        udelay(1000);
+    }
+
+    if (i >= 100)
+        printf("Previous boot CPU %d failed to start as secondary!\n", old_index);
+    else
+        printf("  Started.\n");
+
+    // restore _reset_stacks from secondary init of previous boot cpu
+    _reset_stack = dummy_stack + DUMMY_STACK_SIZE;
+    _reset_stack_el1 = dummy_stack_el1 + DUMMY_STACK_SIZE;
+
+    prev_stack = secondary_stacks[cpu_index];
+    secondary_stacks[cpu_index] = dummy_stack;
+
+    // setup current CPU as boot CPU
+    if (in_el2())
+        msr(TPIDR_EL2, boot_cpu_idx);
+    else
+        msr(TPIDR_EL1, boot_cpu_idx);
+
+    // clear spin table of new boot CPU
+    memset(&spin_table[boot_cpu_idx], 0, sizeof(struct spin_table));
+    spin_table[boot_cpu_idx].mpidr = mrs(MPIDR_EL1) & 0xFFFFFF;
+
+    exception_initialize();
+
+    mmu_init();
+    fb_set_active(true);
+
+    smp_switch_boot_cpu_exit(prev_stack, saved_sp);
+    __builtin_unreachable();
+    return -1;
+}
+
+void smp_do_switch_boot_cpu(int cpu_index, u64 saved_sp)
+{
+    int old_index = boot_cpu_idx;
+
+    printf("Switching boot CPU from %d to %d\n", old_index, cpu_index);
+
+    // disable frame buffer until the new CPU has called mmu_init()
+    fb_set_active(false);
+
+    smp_call3(cpu_index, switch_boot_cpu_init_new, cpu_index, old_index, saved_sp);
+
+    // switch the boot CPU so the old boot CPU resets as secondary
+    boot_cpu_idx = cpu_index;
+    boot_cpu_mpidr = spin_table[cpu_index].mpidr;
+    smp_prepare_cpu(old_index);
+
+    cpu_reset();
+    __builtin_unreachable();
+}
+
+int smp_switch_boot_cpu(int cpu_index)
+{
+    if (cpu_index == boot_cpu_idx)
+        return cpu_index;
+
+    if (!smp_is_alive(cpu_index)) {
+        printf("Trying to switch to offline CPU %d\n", cpu_index);
+        return -1;
+    }
+
+    // Call asm helper function to capture callee saved state and pass `sp` to the new boot CPU.
+    // This function will return on the new boot CPU.
+    void *old_stack = smp_switch_boot_cpu_entry(cpu_index);
+    free(old_stack);
+
+    return cpu_index;
+}
+
 void smp_send_ipi(int cpu)
 {
     if (cpu >= MAX_CPUS)

src/smp.h

@@ -19,6 +19,8 @@ void smp_secondary_prep_el3(void);
 void smp_start_secondaries(void);
 void smp_stop_secondaries(bool deep_sleep);
 
+int smp_switch_boot_cpu(int cpu_index);
+
 #define smp_call0(i, f)          smp_call4(i, f, 0, 0, 0, 0)
 #define smp_call1(i, f, a)       smp_call4(i, f, a, 0, 0, 0)
 #define smp_call2(i, f, a, b)    smp_call4(i, f, a, b, 0, 0)

src/utils_asm.S

@@ -172,3 +172,36 @@ _deep_wfi_helper:
     ldp x30, x0, [sp], #16
 
     ret
+
+.extern smp_do_switch_boot_cpu
+
+.globl smp_switch_boot_cpu_entry
+.type smp_switch_boot_cpu_entry, @function
+smp_switch_boot_cpu_entry:
+    stp x29, x30, [sp, #-16]!
+    mov x29, sp
+    stp x27, x28, [sp, #-16]!
+    stp x25, x26, [sp, #-16]!
+    stp x23, x24, [sp, #-16]!
+    stp x21, x22, [sp, #-16]!
+    stp x19, x20, [sp, #-16]!
+    stp x17, x18, [sp, #-16]!
+
+    mov x1, sp
+    bl smp_do_switch_boot_cpu
+    // unreachable
+    b .
+
+.globl smp_switch_boot_cpu_exit
+.type smp_switch_boot_cpu_exit, @function
+smp_switch_boot_cpu_exit:
+    mov sp, x1
+    ldp x17, x18, [sp], #16
+    ldp x19, x20, [sp], #16
+    ldp x21, x22, [sp], #16
+    ldp x23, x24, [sp], #16
+    ldp x25, x26, [sp], #16
+    ldp x27, x28, [sp], #16
+    ldp x29, x30, [sp], #16
+    // passes argument in x0 as return value
+    ret // looks like a return from smp_switch_boot_cpu_entry on the new boot cpu