diff --git a/proxyclient/m1n1/proxy.py b/proxyclient/m1n1/proxy.py index 16ed89467..5d3f967e5 100644 --- a/proxyclient/m1n1/proxy.py +++ b/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) diff --git a/proxyclient/tools/linux.py b/proxyclient/tools/linux.py index afaaeb428..faaad1643 100755 --- a/proxyclient/tools/linux.py +++ b/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) diff --git a/src/payload.c b/src/payload.c index 5b1547fde..3c397908d 100644 --- a/src/payload.c +++ b/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; diff --git a/src/proxy.c b/src/proxy.c index 3f020c045..49f573ef4 100644 --- a/src/proxy.c +++ b/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]); diff --git a/src/proxy.h b/src/proxy.h index 1e8acdf06..01104e909 100644 --- a/src/proxy.h +++ b/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, diff --git a/src/smp.c b/src/smp.c index a0a41fdc5..26c59b7db 100644 --- a/src/smp.c +++ b/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) diff --git a/src/smp.h b/src/smp.h index 029697864..cf191a8bb 100644 --- a/src/smp.h +++ b/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) diff --git a/src/utils_asm.S b/src/utils_asm.S index 13af28d83..fa5c47cc2 100644 --- a/src/utils_asm.S +++ b/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