3 files changed, 35 insertions, 68 deletions

diff --git a/sys/arm64/arm64/machdep.c b/sys/arm64/arm64/machdep.c
index b48c833d6a23c..739257bed1dbd 100644
--- a/sys/arm64/arm64/machdep.c
+++ b/sys/arm64/arm64/machdep.c
@@ -661,6 +661,7 @@ cpu_pcpu_init(struct pcpu *pcpu, int cpuid, size_t size)
 {
 
 	pcpu->pc_acpi_id = 0xffffffff;
+	pcpu->pc_mpidr = 0xffffffff;
 }
 
 void
diff --git a/sys/arm64/arm64/mp_machdep.c b/sys/arm64/arm64/mp_machdep.c
index 8c8ceafe18e95..352a2fe7813dc 100644
--- a/sys/arm64/arm64/mp_machdep.c
+++ b/sys/arm64/arm64/mp_machdep.c
@@ -125,12 +125,9 @@ static void ipi_stop(void *);
 
 struct pcb stoppcbs[MAXCPU];
 
-/*
- * Not all systems boot from the first CPU in the device tree. To work around
- * this we need to find which CPU we have booted from so when we later
- * enable the secondary CPUs we skip this one.
- */
-static int cpu0 = -1;
+#ifdef FDT
+static u_int fdt_cpuid;
+#endif
 
 void mpentry(unsigned long cpuid);
 void init_secondary(uint64_t);
@@ -432,36 +429,23 @@ cpu_mp_probe(void)
 }
 
 static bool
-start_cpu(u_int id, uint64_t target_cpu)
+start_cpu(u_int cpuid, uint64_t target_cpu)
 {
 	struct pcpu *pcpup;
 	vm_paddr_t pa;
-	u_int cpuid;
 	int err, naps;
 
 	/* Check we are able to start this cpu */
-	if (id > mp_maxid)
+	if (cpuid > mp_maxid)
 		return (false);
 
-	KASSERT(id < MAXCPU, ("Too many CPUs"));
-
-	/* We are already running on cpu 0 */
-	if (id == cpu0)
-		return (true);
-
-	/*
-	 * Rotate the CPU IDs to put the boot CPU as CPU 0. We keep the other
-	 * CPUs ordered as they are likely grouped into clusters so it can be
-	 * useful to keep that property, e.g. for the GICv3 driver to send
-	 * an IPI to all CPUs in the cluster.
-	 */
-	cpuid = id;
-	if (cpuid < cpu0)
-		cpuid += mp_maxid + 1;
-	cpuid -= cpu0;
+	KASSERT(cpuid < MAXCPU, ("Too many CPUs"));
+	KASSERT(__pcpu[0].pc_mpidr != (target_cpu & CPU_AFF_MASK),
+	    ("Start_cpu() was called on the boot CPU"));
 
 	pcpup = &__pcpu[cpuid];
 	pcpu_init(pcpup, cpuid, sizeof(struct pcpu));
+	pcpup->pc_mpidr = target_cpu & CPU_AFF_MASK;
 
 	dpcpu[cpuid - 1] = (void *)kmem_malloc(DPCPU_SIZE, M_WAITOK | M_ZERO);
 	dpcpu_init(dpcpu[cpuid - 1], cpuid);
@@ -483,7 +467,7 @@ start_cpu(u_int id, uint64_t target_cpu)
 		KASSERT(err == PSCI_MISSING ||
 		    (mp_quirks & MP_QUIRK_CPULIST) == MP_QUIRK_CPULIST,
 		    ("Failed to start CPU %u (%lx), error %d\n",
-		    id, target_cpu, err));
+		    cpuid, target_cpu, err));
 
 		pcpu_destroy(pcpup);
 		kmem_free((vm_offset_t)dpcpu[cpuid - 1], DPCPU_SIZE);
@@ -492,9 +476,6 @@ start_cpu(u_int id, uint64_t target_cpu)
 		bootstacks[cpuid] = NULL;
 		mp_ncpus--;
 
-		/* Notify the user that the CPU failed to start */
-		printf("Failed to start CPU %u (%lx), error %d\n",
-		    id, target_cpu, err);
 	} else {
 		/* Wait for the AP to switch to its boot stack. */
 		while (atomic_load_int(&aps_started) < naps + 1)
@@ -518,6 +499,13 @@ madt_handler(ACPI_SUBTABLE_HEADER *entry, void *arg)
 		intr = (ACPI_MADT_GENERIC_INTERRUPT *)entry;
 		cpuid = arg;
 		id = *cpuid;
+
+		/* Skip the boot CPU, but save its ACPI id. */
+		if (__pcpu[0].pc_mpidr == (intr->ArmMpidr & CPU_AFF_MASK)) {
+			__pcpu[0].pc_acpi_id = intr->Uid;
+			break;
+		}
+
 		start_cpu(id, intr->ArmMpidr);
 		__pcpu[id].pc_acpi_id = intr->Uid;
 		(*cpuid)++;
@@ -543,8 +531,8 @@ cpu_init_acpi(void)
 		printf("Unable to map the MADT, not starting APs\n");
 		return;
 	}
-
-	cpuid = 0;
+	/* Boot CPU is always 0 */
+	cpuid = 1;
 	acpi_walk_subtables(madt + 1, (char *)madt + madt->Header.Length,
 	    madt_handler, &cpuid);
 
@@ -569,16 +557,21 @@ cpu_init_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
 		target_cpu |= reg[1];
 	}
 
-	if (!start_cpu(id, target_cpu))
+	/* Skip boot CPU */
+	if (__pcpu[0].pc_mpidr == (target_cpu & CPU_AFF_MASK))
+		return (TRUE);
+
+	if (!start_cpu(fdt_cpuid, target_cpu))
 		return (FALSE);
+	fdt_cpuid++;
 
 	/* Try to read the numa node of this cpu */
 	if (vm_ndomains == 1 ||
 	    OF_getencprop(node, "numa-node-id", &domain, sizeof(domain)) <= 0)
 		domain = 0;
-	__pcpu[id].pc_domain = domain;
+	__pcpu[fdt_cpuid].pc_domain = domain;
 	if (domain < MAXMEMDOM)
-		CPU_SET(id, &cpuset_domain[domain]);
+		CPU_SET(fdt_cpuid, &cpuset_domain[domain]);
 
 	return (TRUE);
 }
@@ -590,18 +583,19 @@ cpu_mp_start(void)
 {
 #ifdef FDT
 	phandle_t node;
-	int i;
 #endif
+	int i;
 
 	mtx_init(&ap_boot_mtx, "ap boot", NULL, MTX_SPIN);
 
+	/* CPU 0 is always boot CPU. */
 	CPU_SET(0, &all_cpus);
+	__pcpu[0].pc_mpidr = READ_SPECIALREG(mpidr_el1) & CPU_AFF_MASK;
 
 	switch(arm64_bus_method) {
 #ifdef DEV_ACPI
 	case ARM64_BUS_ACPI:
 		mp_quirks = MP_QUIRK_CPULIST;
-		KASSERT(cpu0 >= 0, ("Current CPU was not found"));
 		cpu_init_acpi();
 		break;
 #endif
@@ -614,7 +608,7 @@ cpu_mp_start(void)
 				mp_quirks = fdt_quirks[i].quirks;
 			}
 		}
-		KASSERT(cpu0 >= 0, ("Current CPU was not found"));
+		fdt_cpuid = 1;
 		ofw_cpu_early_foreach(cpu_init_fdt, true);
 		break;
 #endif
@@ -635,16 +629,10 @@ cpu_count_acpi_handler(ACPI_SUBTABLE_HEADER *entry, void *arg)
 {
 	ACPI_MADT_GENERIC_INTERRUPT *intr;
 	u_int *cores = arg;
-	uint64_t mpidr_reg;
 
 	switch(entry->Type) {
 	case ACPI_MADT_TYPE_GENERIC_INTERRUPT:
 		intr = (ACPI_MADT_GENERIC_INTERRUPT *)entry;
-		if (cpu0 < 0) {
-			mpidr_reg = READ_SPECIALREG(mpidr_el1);
-			if ((mpidr_reg & 0xff00fffffful) == intr->ArmMpidr)
-				cpu0 = *cores;
-		}
 		(*cores)++;
 		break;
 	default:
@@ -679,29 +667,6 @@ cpu_count_acpi(void)
 }
 #endif
 
-#ifdef FDT
-static boolean_t
-cpu_find_cpu0_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
-{
-	uint64_t mpidr_fdt, mpidr_reg;
-
-	if (cpu0 < 0) {
-		mpidr_fdt = reg[0];
-		if (addr_size == 2) {
-			mpidr_fdt <<= 32;
-			mpidr_fdt |= reg[1];
-		}
-
-		mpidr_reg = READ_SPECIALREG(mpidr_el1);
-
-		if ((mpidr_reg & 0xff00fffffful) == mpidr_fdt)
-			cpu0 = id;
-	}
-
-	return (TRUE);
-}
-#endif
-
 void
 cpu_mp_setmaxid(void)
 {
@@ -726,7 +691,7 @@ cpu_mp_setmaxid(void)
 #endif
 #ifdef FDT
 	case ARM64_BUS_FDT:
-		cores = ofw_cpu_early_foreach(cpu_find_cpu0_fdt, false);
+		cores = ofw_cpu_early_foreach(NULL, false);
 		if (cores > 0) {
 			cores = MIN(cores, MAXCPU);
 			if (bootverbose)
diff --git a/sys/arm64/include/pcpu.h b/sys/arm64/include/pcpu.h
index c13de37c71415..a520cfc7df56e 100644
--- a/sys/arm64/include/pcpu.h
+++ b/sys/arm64/include/pcpu.h
@@ -48,7 +48,8 @@ struct debug_monitor_state;
 	struct pmap *pc_curpmap;					\
 	struct pmap *pc_curvmpmap;					\
 	u_int	pc_bcast_tlbi_workaround;				\
-	char __pad[205]
+	u_int	pc_mpidr;	/* stored MPIDR value */		\
+	char __pad[201]
 
 #ifdef _KERNEL