diff --git a/src/exception.c b/src/exception.c index 7f476d3aa..7092ece22 100644 --- a/src/exception.c +++ b/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 diff --git a/src/hv.c b/src/hv.c index ca1be9124..5179bf7b1 100644 --- a/src/hv.c +++ b/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); diff --git a/src/hv_wdt.c b/src/hv_wdt.c index f472a7797..d78c38584 100644 --- a/src/hv_wdt.c +++ b/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; } diff --git a/src/iodev.c b/src/iodev.c index 32b58317c..7c03b9c51 100644 --- a/src/iodev.c +++ b/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); diff --git a/src/payload.c b/src/payload.c index 1bc7d2de3..0f49cf573 100644 --- a/src/payload.c +++ b/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); diff --git a/src/smp.c b/src/smp.c index 729571c36..fe5b08a36 100644 --- a/src/smp.c +++ b/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", ®) < 0) + if (ADT_GETPROP(adt, cpu_node, "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"); diff --git a/src/smp.h b/src/smp.h index eaed0a92d..691caec1a 100644 --- a/src/smp.h +++ b/src/smp.h @@ -39,4 +39,5 @@ static inline int smp_id(void) return mrs(TPIDR_EL1); } +extern int boot_cpu_idx; #endif diff --git a/src/startup.c b/src/startup.c index 84f189454..5473f3707 100644 --- a/src/startup.c +++ b/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(); diff --git a/src/utils.h b/src/utils.h index dc4bb8d68..de17fe34a 100644 --- a/src/utils.h +++ b/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[];