Add support for SMP. This uses the FDT data to find the CPUs to start on,

and psci to start them. I expect ACPI support to be added later.

This has been tested on qemu with 2 cpus as that is the current value of
MAXCPUS. This is expected to be increased in the future as FreeBSD has
been tested on 48 cores on the Cavium ThunderX hardware.

Partially based on a patch from Robin Randhawa from ARM.

Approved by:	ABT Systems Ltd
Relnotes:	yes
Sponsored by:	The FreeBSD Foundation
Differential Revision:	https://reviews.freebsd.org/D3024

7 files changed, 549 insertions, 10 deletions

diff --git a/sys/arm64/arm64/locore.S b/sys/arm64/arm64/locore.S
index c9a9c83..be7f0c7 100644
--- a/sys/arm64/arm64/locore.S
+++ b/sys/arm64/arm64/locore.S
@@ -72,6 +72,8 @@ _start:
 	msr	sctlr_el1, x2
 	isb
 
+	/* Set the context id */
+	msr	contextidr_el1, xzr
 
 	/* Get the virt -> phys offset */
 	bl	get_virt_delta
@@ -146,6 +148,46 @@ virtdone:
 .Lend:
 	.quad	_end
 
+#ifdef SMP
+/*
+ * mpentry(unsigned long)
+ *
+ * Called by a core when it is being brought online.
+ * The data in x0 is passed straight to init_secondary.
+ */
+ENTRY(mpentry)
+	/* Disable interrupts */
+	msr	daifset, #2
+
+	/* Drop to EL1 */
+	bl	drop_to_el1
+
+	/* Set the context id */
+	msr	contextidr_el1, x1
+
+	/* Load the kernel page table */
+	adr	x26, pagetable_l1_ttbr1
+	/* Load the identity page table */
+	adr	x27, pagetable_l1_ttbr0
+
+	/* Enable the mmu */
+	bl	start_mmu
+
+	/* Jump to the virtual address space */
+	ldr	x15, =mp_virtdone
+	br	x15
+
+mp_virtdone:
+	ldr	x4, =secondary_stacks
+	mov	x5, #(PAGE_SIZE * KSTACK_PAGES)
+	sub	x1, x0, #1
+	mul	x5, x1, x5
+	add	sp, x4, x5
+
+	b	init_secondary
+END(mpentry)
+#endif
+
 /*
  * If we are started in EL2, configure the required hypervisor
  * registers and drop to EL1.
diff --git a/sys/arm64/arm64/machdep.c b/sys/arm64/arm64/machdep.c
index 6137a6c..427c11a 100644
--- a/sys/arm64/arm64/machdep.c
+++ b/sys/arm64/arm64/machdep.c
@@ -119,6 +119,13 @@ cpu_startup(void *dummy)
 
 SYSINIT(cpu, SI_SUB_CPU, SI_ORDER_FIRST, cpu_startup, NULL);
 
+int
+cpu_idle_wakeup(int cpu)
+{
+
+	return (0);
+}
+
 void
 bzero(void *buf, size_t len)
 {
diff --git a/sys/arm64/arm64/mp_machdep.c b/sys/arm64/arm64/mp_machdep.c
new file mode 100644
index 0000000..15aa290
--- /dev/null
+++ b/sys/arm64/arm64/mp_machdep.c
@@ -0,0 +1,433 @@
+/*-
+ * Copyright (c) 2015 The FreeBSD Foundation
+ * All rights reserved.
+ *
+ * This software was developed by Andrew Turner under
+ * sponsorship from the FreeBSD Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ */
+
+#include "opt_platform.h"
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/cpu.h>
+#include <sys/kernel.h>
+#include <sys/malloc.h>
+#include <sys/module.h>
+#include <sys/mutex.h>
+#include <sys/proc.h>
+#include <sys/sched.h>
+#include <sys/smp.h>
+
+#include <vm/vm.h>
+#include <vm/pmap.h>
+#include <vm/vm_extern.h>
+#include <vm/vm_kern.h>
+
+#include <machine/intr.h>
+#include <machine/smp.h>
+#ifdef VFP
+#include <machine/vfp.h>
+#endif
+
+#ifdef FDT
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_cpu.h>
+#endif
+
+#include <dev/psci/psci.h>
+
+boolean_t ofw_cpu_reg(phandle_t node, u_int, cell_t *);
+
+extern struct pcpu __pcpu[];
+
+static enum {
+	CPUS_UNKNOWN,
+#ifdef FDT
+	CPUS_FDT,
+#endif
+} cpu_enum_method;
+
+static device_identify_t arm64_cpu_identify;
+static device_probe_t arm64_cpu_probe;
+static device_attach_t arm64_cpu_attach;
+
+static int ipi_handler(void *arg);
+
+struct mtx ap_boot_mtx;
+struct pcb stoppcbs[MAXCPU];
+
+#ifdef INVARIANTS
+static uint32_t cpu_reg[MAXCPU][2];
+#endif
+static device_t cpu_list[MAXCPU];
+
+void mpentry(unsigned long cpuid);
+void init_secondary(uint64_t);
+
+uint8_t secondary_stacks[MAXCPU - 1][PAGE_SIZE * KSTACK_PAGES] __aligned(16);
+
+/* # of Applications processors */
+volatile int mp_naps;
+/* Set to 1 once we're ready to let the APs out of the pen. */
+volatile int aps_ready = 0;
+
+/* Temporary variables for init_secondary()  */
+void *dpcpu[MAXCPU - 1];
+
+static device_method_t arm64_cpu_methods[] = {
+	/* Device interface */
+	DEVMETHOD(device_identify,	arm64_cpu_identify),
+	DEVMETHOD(device_probe,		arm64_cpu_probe),
+	DEVMETHOD(device_attach,	arm64_cpu_attach),
+
+	DEVMETHOD_END
+};
+
+static devclass_t arm64_cpu_devclass;
+static driver_t arm64_cpu_driver = {
+	"arm64_cpu",
+	arm64_cpu_methods,
+	0
+};
+
+DRIVER_MODULE(arm64_cpu, cpu, arm64_cpu_driver, arm64_cpu_devclass, 0, 0);
+
+static void
+arm64_cpu_identify(driver_t *driver, device_t parent)
+{
+
+	if (device_find_child(parent, "arm64_cpu", -1) != NULL)
+		return;
+	if (BUS_ADD_CHILD(parent, 0, "arm64_cpu", -1) == NULL)
+		device_printf(parent, "add child failed\n");
+}
+
+static int
+arm64_cpu_probe(device_t dev)
+{
+	u_int cpuid;
+
+	cpuid = device_get_unit(dev);
+	if (cpuid >= MAXCPU || cpuid > mp_maxid)
+		return (EINVAL);
+
+	return (0);
+}
+
+static int
+arm64_cpu_attach(device_t dev)
+{
+	const uint32_t *reg;
+	size_t reg_size;
+	u_int cpuid;
+	int i;
+
+	cpuid = device_get_unit(dev);
+
+	if (cpuid >= MAXCPU || cpuid > mp_maxid)
+		return (EINVAL);
+	KASSERT(cpu_list[cpuid] == NULL, ("Already have cpu %u", cpuid));
+
+	reg = cpu_get_cpuid(dev, &reg_size);
+	if (reg == NULL)
+		return (EINVAL);
+
+	device_printf(dev, "Found register:");
+	for (i = 0; i < reg_size; i++)
+		printf(" %x", reg[i]);
+	printf("\n");
+
+	/* Set the device to start it later */
+	cpu_list[cpuid] = dev;
+
+	return (0);
+}
+
+static void
+release_aps(void *dummy __unused)
+{
+	int i;
+
+	/* Setup the IPI handler */
+	for (i = 0; i < COUNT_IPI; i++)
+		arm_setup_ipihandler(ipi_handler, i);
+
+	atomic_store_rel_int(&aps_ready, 1);
+	/* Wake up the other CPUs */
+	__asm __volatile("sev");
+
+	printf("Release APs\n");
+
+	for (i = 0; i < 2000; i++) {
+		if (smp_started)
+			return;
+		DELAY(1000);
+	}
+
+	printf("AP's not started\n");
+}
+SYSINIT(start_aps, SI_SUB_SMP, SI_ORDER_FIRST, release_aps, NULL);
+
+void
+init_secondary(uint64_t cpu)
+{
+	struct pcpu *pcpup;
+	int i;
+
+	pcpup = &__pcpu[cpu];
+	/*
+	 * Set the pcpu pointer with a backup in tpidr_el1 to be
+	 * loaded when entering the kernel from userland.
+	 */
+	__asm __volatile(
+	    "mov x18, %0 \n"
+	    "msr tpidr_el1, %0" :: "r"(pcpup));
+
+	/*
+	 * pcpu_init() updates queue, so it should not be executed in parallel
+	 * on several cores
+	 */
+	while(mp_naps < (cpu - 1))
+		;
+
+	/* Signal our startup to BSP */
+	atomic_add_rel_32(&mp_naps, 1);
+
+	/* Spin until the BSP releases the APs */
+	while (!aps_ready)
+		__asm __volatile("wfe");
+
+	/* Initialize curthread */
+	KASSERT(PCPU_GET(idlethread) != NULL, ("no idle thread"));
+	pcpup->pc_curthread = pcpup->pc_idlethread;
+	pcpup->pc_curpcb = pcpup->pc_idlethread->td_pcb;
+
+	for (i = 0; i < COUNT_IPI; i++)
+		arm_unmask_ipi(i);
+
+	/* Start per-CPU event timers. */
+	cpu_initclocks_ap();
+
+#ifdef VFP
+	vfp_init();
+#endif
+
+	/* Configure the interrupt controller */
+	arm_init_secondary();
+
+	/* Enable interrupts */
+	intr_enable();
+
+	mtx_lock_spin(&ap_boot_mtx);
+
+	atomic_add_rel_32(&smp_cpus, 1);
+
+	if (smp_cpus == mp_ncpus) {
+		/* enable IPI's, tlb shootdown, freezes etc */
+		atomic_store_rel_int(&smp_started, 1);
+	}
+
+	mtx_unlock_spin(&ap_boot_mtx);
+
+	/* Enter the scheduler */
+	sched_throw(NULL);
+
+	panic("scheduler returned us to init_secondary");
+	/* NOTREACHED */
+}
+
+static int
+ipi_handler(void *arg)
+{
+	u_int cpu, ipi;
+
+	arg = (void *)((uintptr_t)arg & ~(1 << 16));
+	KASSERT((uintptr_t)arg < COUNT_IPI,
+	    ("Invalid IPI %ju", (uintptr_t)arg));
+
+	cpu = PCPU_GET(cpuid);
+	ipi = (uintptr_t)arg;
+
+	switch(ipi) {
+	case IPI_AST:
+		CTR0(KTR_SMP, "IPI_AST");
+		break;
+	case IPI_PREEMPT:
+		CTR1(KTR_SMP, "%s: IPI_PREEMPT", __func__);
+		sched_preempt(curthread);
+		break;
+	case IPI_RENDEZVOUS:
+		CTR0(KTR_SMP, "IPI_RENDEZVOUS");
+		smp_rendezvous_action();
+		break;
+	case IPI_STOP:
+	case IPI_STOP_HARD:
+		CTR0(KTR_SMP, (ipi == IPI_STOP) ? "IPI_STOP" : "IPI_STOP_HARD");
+		savectx(&stoppcbs[cpu]);
+
+		/* Indicate we are stopped */
+		CPU_SET_ATOMIC(cpu, &stopped_cpus);
+
+		/* Wait for restart */
+		while (!CPU_ISSET(cpu, &started_cpus))
+			cpu_spinwait();
+
+		CPU_CLR_ATOMIC(cpu, &started_cpus);
+		CPU_CLR_ATOMIC(cpu, &stopped_cpus);
+		CTR0(KTR_SMP, "IPI_STOP (restart)");
+		break;
+	case IPI_HARDCLOCK:
+		CTR1(KTR_SMP, "%s: IPI_HARDCLOCK", __func__);
+		hardclockintr();
+		break;
+	default:
+		panic("Unknown IPI %#0x on cpu %d", ipi, curcpu);
+	}
+
+	return (FILTER_HANDLED);
+}
+
+struct cpu_group *
+cpu_topo(void)
+{
+
+	return (smp_topo_none());
+}
+
+/* Determine if we running MP machine */
+int
+cpu_mp_probe(void)
+{
+
+	/* ARM64TODO: Read the u bit of mpidr_el1 to determine this */
+	return (1);
+}
+
+#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;
+	struct pcpu *pcpup;
+	vm_paddr_t pa;
+	int err;
+
+	/* Check we are able to start this cpu */
+	if (id > mp_maxid)
+		return (0);
+
+	KASSERT(id < MAXCPU, ("Too mant CPUs"));
+
+	KASSERT(addr_size == 1 || addr_size == 2, ("Invalid register size"));
+#ifdef INVARIANTS
+	cpu_reg[id][0] = reg[0];
+	if (addr_size == 2)
+		cpu_reg[id][1] = reg[1];
+#endif
+
+	/* We are already running on cpu 0 */
+	if (id == 0)
+		return (1);
+
+	CPU_SET(id, &all_cpus);
+
+	pcpup = &__pcpu[id];
+	pcpu_init(pcpup, id, sizeof(struct pcpu));
+
+	dpcpu[id - 1] = (void *)kmem_malloc(kernel_arena, DPCPU_SIZE,
+	    M_WAITOK | M_ZERO);
+	dpcpu_init(dpcpu[id - 1], id);
+
+	target_cpu = reg[0];
+	if (addr_size == 2) {
+		target_cpu <<= 32;
+		target_cpu |= reg[1];
+	}
+
+	printf("Starting CPU %u (%lx)\n", id, target_cpu);
+	pa = pmap_extract(kernel_pmap, (vm_offset_t)mpentry);
+
+	err = psci_cpu_on(target_cpu, pa, id);
+	if (err != PSCI_RETVAL_SUCCESS)
+		printf("Failed to start CPU %u\n", id);
+
+	return (1);
+}
+#endif
+
+/* Initialize and fire up non-boot processors */
+void
+cpu_mp_start(void)
+{
+
+	mtx_init(&ap_boot_mtx, "ap boot", NULL, MTX_SPIN);
+
+	CPU_SET(0, &all_cpus);
+
+	switch(cpu_enum_method) {
+#ifdef FDT
+	case CPUS_FDT:
+		ofw_cpu_early_foreach(cpu_init_fdt, true);
+		break;
+#endif
+	case CPUS_UNKNOWN:
+		break;
+	}
+}
+
+/* Introduce rest of cores to the world */
+void
+cpu_mp_announce(void)
+{
+}
+
+void
+cpu_mp_setmaxid(void)
+{
+#ifdef FDT
+	int cores;
+
+	cores = ofw_cpu_early_foreach(NULL, false);
+	if (cores > 0) {
+		cores = MIN(cores, MAXCPU);
+		if (bootverbose)
+			printf("Found %d CPUs in the device tree\n", cores);
+		mp_ncpus = cores;
+		mp_maxid = cores - 1;
+		cpu_enum_method = CPUS_FDT;
+		return;
+	}
+#endif
+
+	if (bootverbose)
+		printf("No CPU data, limiting to 1 core\n");
+	mp_ncpus = 1;
+	mp_maxid = 0;
+}
diff --git a/sys/arm64/arm64/swtch.S b/sys/arm64/arm64/swtch.S
index bf69b86..6774e76 100644
--- a/sys/arm64/arm64/swtch.S
+++ b/sys/arm64/arm64/swtch.S
@@ -30,6 +30,7 @@
  */
 
 #include "assym.s"
+#include "opt_sched.h"
 
 #include <machine/asm.h>
 
@@ -39,10 +40,6 @@ __FBSDID("$FreeBSD$");
  * void cpu_throw(struct thread *old, struct thread *new)
  */
 ENTRY(cpu_throw)
-#ifdef SMP
-#error cpu_throw needs to be ported to support SMP
-#endif
-
 #ifdef VFP
 	/* Backup the new thread pointer around a call to C code */
 	mov	x19, x1
@@ -100,10 +97,6 @@ END(cpu_throw)
  * x3 to x7, x16 and x17 are caller saved
  */
 ENTRY(cpu_switch)
-#ifdef SMP
-#error cpu_switch needs to be ported to support SMP
-#endif
-
 	/* Store the new curthread */
 	str	x1, [x18, #PC_CURTHREAD]
 	/* And the new pcb */
@@ -167,7 +160,15 @@ ENTRY(cpu_switch)
 	/* Release the old thread */
 	str	x2, [x0, #TD_LOCK]
 #if defined(SCHED_ULE) && defined(SMP)
-#error We may need to wait for the lock here
+	/* Read the value in blocked_lock */
+	ldr	x0, =_C_LABEL(blocked_lock)
+	ldr	x1, [x0]
+	/* Load curthread */
+	ldr	x2, [x18, #PC_CURTHREAD]
+1:
+	ldr	x3, [x2, #TD_LOCK]
+	cmp	x3, x1
+	b.eq	1b
 #endif
 
 	/* Restore the registers */
diff --git a/sys/arm64/conf/GENERIC b/sys/arm64/conf/GENERIC
index 4e84ac7..1bd12b8 100644
--- a/sys/arm64/conf/GENERIC
+++ b/sys/arm64/conf/GENERIC
@@ -69,6 +69,7 @@ options 	VFP			# Floating-point support
 options 	RACCT			# Resource accounting framework
 options 	RACCT_DEFAULT_TO_DISABLED # Set kern.racct.enable=0 by default
 options 	RCTL			# Resource limits
+options 	SMP
 
 # Debugging support.  Always need this:
 options 	KDB			# Enable kernel debugger support.
diff --git a/sys/arm64/include/smp.h b/sys/arm64/include/smp.h
index da23dbe..0f56396 100644
--- a/sys/arm64/include/smp.h
+++ b/sys/arm64/include/smp.h
@@ -1 +1,55 @@
-/* $FreeBSD$ */
+/*-
+ * Copyright (c) 2014 Andrew Turner <andrew@FreeBSD.org>
+ * Copyright (c) 2015 The FreeBSD Foundation
+ * All rights reserved.
+ *
+ * This software was developed by Andrew Turner under sponsorship from
+ * the FreeBSD Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef	_MACHINE_SMP_H_
+#define	_MACHINE_SMP_H_
+
+#include <machine/pcb.h>
+
+enum {
+	IPI_AST,
+	IPI_PREEMPT,
+	IPI_RENDEZVOUS,
+	IPI_STOP,
+	IPI_STOP_HARD,
+	IPI_HARDCLOCK,
+	COUNT_IPI,
+};
+
+void	ipi_all_but_self(u_int ipi);
+void	ipi_cpu(int cpu, u_int ipi);
+void	ipi_selected(cpuset_t cpus, u_int ipi);
+
+/* global data in mp_machdep.c */
+extern struct pcb               stoppcbs[];
+
+#endif /* !_MACHINE_SMP_H_ */
diff --git a/sys/conf/files.arm64 b/sys/conf/files.arm64
index d4301ac..d63594e 100644
--- a/sys/conf/files.arm64
+++ b/sys/conf/files.arm64
@@ -35,6 +35,7 @@ arm64/arm64/locore.S		standard	no-obj
 arm64/arm64/machdep.c		standard
 arm64/arm64/mem.c		standard
 arm64/arm64/minidump_machdep.c	standard
+arm64/arm64/mp_machdep.c	optional	smp
 arm64/arm64/nexus.c		standard
 arm64/arm64/pic_if.m		standard
 arm64/arm64/pmap.c		standard