Add ACPI support to the arm64 mp code. We use the Multiple APIC Description

Table to find the CPUs to find the CPUs to start. Currently we assume PSCI,
however this assumption is shared with the FDT code.

Obtained from:	ABT Systems Ltd
Sponsored by:	The FreeBSD Foundation

1 files changed, 150 insertions, 18 deletions

diff --git a/sys/arm64/arm64/mp_machdep.c b/sys/arm64/arm64/mp_machdep.c
index a1081c390090..d055bfb97a11 100644
--- a/sys/arm64/arm64/mp_machdep.c
+++ b/sys/arm64/arm64/mp_machdep.c
@@ -28,6 +28,7 @@
  *
  */
 
+#include "opt_acpi.h"
 #include "opt_kstack_pages.h"
 #include "opt_platform.h"
 
@@ -59,6 +60,11 @@ __FBSDID("$FreeBSD$");
 #include <machine/vfp.h>
 #endif
 
+#ifdef DEV_ACPI
+#include <contrib/dev/acpica/include/acpi.h>
+#include <dev/acpica/acpivar.h>
+#endif
+
 #ifdef FDT
 #include <dev/ofw/openfirm.h>
 #include <dev/ofw/ofw_cpu.h>
@@ -106,14 +112,12 @@ struct pcb stoppcbs[MAXCPU];
 
 static device_t cpu_list[MAXCPU];
 
-#ifdef FDT
 /*
  * 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;
-#endif
 
 void mpentry(unsigned long cpuid);
 void init_secondary(uint64_t);
@@ -416,11 +420,9 @@ cpu_mp_probe(void)
 	return (1);
 }
 
-#ifdef FDT
-static boolean_t
-cpu_init_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
+static bool
+start_cpu(u_int id, uint64_t target_cpu)
 {
-	uint64_t target_cpu;
 	struct pcpu *pcpup;
 	vm_paddr_t pa;
 	u_int cpuid;
@@ -428,13 +430,13 @@ cpu_init_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
 
 	/* Check we are able to start this cpu */
 	if (id > mp_maxid)
-		return (0);
+		return (false);
 
 	KASSERT(id < MAXCPU, ("Too many CPUs"));
 
 	/* We are already running on cpu 0 */
 	if (id == cpu0)
-		return (1);
+		return (true);
 
 	/*
 	 * Rotate the CPU IDs to put the boot CPU as CPU 0. We keep the other
@@ -454,12 +456,6 @@ cpu_init_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
 	    M_WAITOK | M_ZERO);
 	dpcpu_init(dpcpu[cpuid - 1], cpuid);
 
-	target_cpu = reg[0];
-	if (addr_size == 2) {
-		target_cpu <<= 32;
-		target_cpu |= reg[1];
-	}
-
 	printf("Starting CPU %u (%lx)\n", cpuid, target_cpu);
 	pa = pmap_extract(kernel_pmap, (vm_offset_t)mpentry);
 
@@ -482,7 +478,67 @@ cpu_init_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
 	} else
 		CPU_SET(cpuid, &all_cpus);
 
-	return (1);
+	return (true);
+}
+
+#ifdef DEV_ACPI
+static void
+madt_handler(ACPI_SUBTABLE_HEADER *entry, void *arg)
+{
+	ACPI_MADT_GENERIC_INTERRUPT *intr;
+	u_int *cpuid;
+
+	switch(entry->Type) {
+	case ACPI_MADT_TYPE_GENERIC_INTERRUPT:
+		intr = (ACPI_MADT_GENERIC_INTERRUPT *)entry;
+		cpuid = arg;
+
+		start_cpu((*cpuid), intr->ArmMpidr);
+		(*cpuid)++;
+		break;
+	default:
+		break;
+	}
+}
+
+static void
+cpu_init_acpi(void)
+{
+	ACPI_TABLE_MADT *madt;
+	vm_paddr_t physaddr;
+	u_int cpuid;
+
+	physaddr = acpi_find_table(ACPI_SIG_MADT);
+	if (physaddr == 0)
+		return;
+
+	madt = acpi_map_table(physaddr, ACPI_SIG_MADT);
+	if (madt == NULL) {
+		printf("Unable to map the MADT, not starting APs\n");
+		return;
+	}
+
+	cpuid = 0;
+	acpi_walk_subtables(madt + 1, (char *)madt + madt->Header.Length,
+	    madt_handler, &cpuid);
+
+	acpi_unmap_table(madt);
+}
+#endif
+
+#ifdef FDT
+static boolean_t
+cpu_init_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
+{
+	uint64_t target_cpu;
+
+	target_cpu = reg[0];
+	if (addr_size == 2) {
+		target_cpu <<= 32;
+		target_cpu |= reg[1];
+	}
+
+	return (start_cpu(id, target_cpu) ? TRUE : FALSE);
 }
 #endif
 
@@ -496,6 +552,12 @@ cpu_mp_start(void)
 	CPU_SET(0, &all_cpus);
 
 	switch(arm64_bus_method) {
+#ifdef DEV_ACPI
+	case ARM64_BUS_ACPI:
+		KASSERT(cpu0 >= 0, ("Current CPU was not found"));
+		cpu_init_acpi();
+		break;
+#endif
 #ifdef FDT
 	case ARM64_BUS_FDT:
 		KASSERT(cpu0 >= 0, ("Current CPU was not found"));
@@ -513,6 +575,56 @@ cpu_mp_announce(void)
 {
 }
 
+#ifdef DEV_ACPI
+static void
+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:
+		break;
+	}
+}
+
+static u_int
+cpu_count_acpi(void)
+{
+	ACPI_TABLE_MADT *madt;
+	vm_paddr_t physaddr;
+	u_int cores;
+
+	physaddr = acpi_find_table(ACPI_SIG_MADT);
+	if (physaddr == 0)
+		return (0);
+
+	madt = acpi_map_table(physaddr, ACPI_SIG_MADT);
+	if (madt == NULL) {
+		printf("Unable to map the MADT, not starting APs\n");
+		return (0);
+	}
+
+	cores = 0;
+	acpi_walk_subtables(madt + 1, (char *)madt + madt->Header.Length,
+	    cpu_count_acpi_handler, &cores);
+
+	acpi_unmap_table(madt);
+
+	return (cores);
+}
+#endif
+
 #ifdef FDT
 static boolean_t
 cpu_find_cpu0_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
@@ -539,10 +651,27 @@ cpu_find_cpu0_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
 void
 cpu_mp_setmaxid(void)
 {
-#ifdef FDT
+#if defined(DEV_ACPI) || defined(FDT)
 	int cores;
+#endif
 
-	if (arm64_bus_method == ARM64_BUS_FDT) {
+	switch(arm64_bus_method) {
+#ifdef DEV_ACPI
+	case ARM64_BUS_ACPI:
+		cores = cpu_count_acpi();
+		if (cores > 0) {
+			cores = MIN(cores, MAXCPU);
+			if (bootverbose)
+				printf("Found %d CPUs in the ACPI tables\n",
+				    cores);
+			mp_ncpus = cores;
+			mp_maxid = cores - 1;
+			return;
+		}
+		break;
+#endif
+#ifdef FDT
+	case ARM64_BUS_FDT:
 		cores = ofw_cpu_early_foreach(cpu_find_cpu0_fdt, false);
 		if (cores > 0) {
 			cores = MIN(cores, MAXCPU);
@@ -553,8 +682,11 @@ cpu_mp_setmaxid(void)
 			mp_maxid = cores - 1;
 			return;
 		}
-	}
+		break;
 #endif
+	default:
+		break;
+	}
 
 	if (bootverbose)
 		printf("No CPU data, limiting to 1 core\n");