Add initial support for SMP on Armada38x

- Add file sys/arm/mv/armada38x/armada38x_mp.c
- Set mp_maxid and mp_ncpus based on FDT unless SCU register indicates
  only one core
- Boot CPU1 in platform_mp_start_ap()
- IPI range defined

Obtained from:  Semihalf
Sponsored by:   Stormshield
Submitted by:   Michal Stanek <mst@semihalf.com>
Differential revision:  https://reviews.freebsd.org/D4426

4 files changed, 188 insertions, 1 deletions

diff --git a/sys/arm/mv/armada38x/armada38x_mp.c b/sys/arm/mv/armada38x/armada38x_mp.c
new file mode 100644
index 0000000..108a107
--- /dev/null
+++ b/sys/arm/mv/armada38x/armada38x_mp.c
@@ -0,0 +1,164 @@
+/*-
+ * Copyright (c) 2015 Semihalf.
+ * Copyright (c) 2015 Stormshield.
+ * All rights reserved.
+ *
+ * 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 <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/smp.h>
+
+#include <machine/smp.h>
+#include <machine/fdt.h>
+#include <machine/intr.h>
+
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <arm/mv/mvreg.h>
+
+int cpu_reset_deassert(void);
+
+int
+cpu_reset_deassert(void)
+{
+	bus_space_handle_t vaddr;
+	uint32_t reg;
+	int rv;
+
+	rv = bus_space_map(fdtbus_bs_tag, (bus_addr_t)MV_CPU_RESET_BASE,
+	    MV_CPU_RESET_REGS_LEN, 0, &vaddr);
+	if (rv != 0)
+		return (rv);
+
+	/* CPU1 is held at reset by default - clear assert bit to release it */
+	reg = bus_space_read_4(fdtbus_bs_tag, vaddr, CPU_RESET_OFFSET(1));
+	reg &= ~CPU_RESET_ASSERT;
+
+	bus_space_write_4(fdtbus_bs_tag, vaddr, CPU_RESET_OFFSET(1), reg);
+
+	bus_space_unmap(fdtbus_bs_tag, vaddr, MV_CPU_RESET_REGS_LEN);
+
+	return (0);
+}
+
+static int
+platform_cnt_cpus(void)
+{
+	bus_space_handle_t vaddr_scu;
+	phandle_t cpus_node, child;
+	char device_type[16];
+	int fdt_cpu_count = 0;
+	int reg_cpu_count = 0;
+	uint32_t val;
+	int rv;
+
+	cpus_node = OF_finddevice("/cpus");
+	if (cpus_node == -1) {
+		/* Default is one core */
+		mp_ncpus = 1;
+		return (0);
+	}
+
+	/* Get number of 'cpu' nodes from FDT */
+	for (child = OF_child(cpus_node); child != 0; child = OF_peer(child)) {
+		/* Check if child is a CPU */
+		memset(device_type, 0, sizeof(device_type));
+		rv = OF_getprop(child, "device_type", device_type,
+		    sizeof(device_type) - 1);
+		if (rv < 0)
+			continue;
+		if (strcmp(device_type, "cpu") != 0)
+			continue;
+
+		fdt_cpu_count++;
+	}
+
+	/* Get number of CPU cores from SCU register to cross-check with FDT */
+	rv = bus_space_map(fdtbus_bs_tag, (bus_addr_t)MV_SCU_BASE,
+	    MV_SCU_REGS_LEN, 0, &vaddr_scu);
+	if (rv != 0) {
+		/* Default is one core */
+		mp_ncpus = 1;
+		return (0);
+	}
+
+	val = bus_space_read_4(fdtbus_bs_tag, vaddr_scu, MV_SCU_REG_CONFIG);
+	bus_space_unmap(fdtbus_bs_tag, vaddr_scu, MV_SCU_REGS_LEN);
+        reg_cpu_count = (val & SCU_CFG_REG_NCPU_MASK) + 1;
+
+	/* Set mp_ncpus to number of cpus in FDT unless SOC contains only one */
+	mp_ncpus = min(reg_cpu_count, fdt_cpu_count);
+	/* mp_ncpus must be at least 1 */
+	mp_ncpus = max(1, mp_ncpus);
+
+	return (mp_ncpus);
+}
+
+void
+platform_mp_setmaxid(void)
+{
+
+	/* Armada38x family supports maximum 2 cores */
+	mp_ncpus = platform_cnt_cpus();
+	mp_maxid = 1;
+}
+
+int
+platform_mp_probe(void)
+{
+
+	return (mp_ncpus > 1);
+}
+
+void
+platform_mp_init_secondary(void)
+{
+
+	intr_pic_init_secondary();
+}
+
+void
+platform_mp_start_ap(void)
+{
+	int rv;
+
+	/* Write secondary entry address to PMSU register */
+	rv = pmsu_boot_secondary_cpu();
+	if (rv != 0)
+		return;
+
+	/* Release CPU1 from reset */
+	cpu_reset_deassert();
+}
+
+void
+platform_ipi_send(cpuset_t cpus, u_int ipi)
+{
+
+	pic_ipi_send(cpus, ipi);
+}
diff --git a/sys/arm/mv/armada38x/files.armada38x b/sys/arm/mv/armada38x/files.armada38x
index a446829..72066c7 100644
--- a/sys/arm/mv/armada38x/files.armada38x
+++ b/sys/arm/mv/armada38x/files.armada38x
@@ -1,4 +1,5 @@
 # $FreeBSD$
 
 arm/mv/armada38x/armada38x.c	standard
+arm/mv/armada38x/armada38x_mp.c	optional smp
 arm/mv/armada38x/rtc.c		standard
diff --git a/sys/arm/mv/armada38x/std.armada38x b/sys/arm/mv/armada38x/std.armada38x
index 161eebb..732fd90 100644
--- a/sys/arm/mv/armada38x/std.armada38x
+++ b/sys/arm/mv/armada38x/std.armada38x
@@ -8,3 +8,5 @@ makeoptions	CONF_CFLAGS="-march=armv7a"
 makeoptions	KERNVIRTADDR=0xc0000000
 
 options         KERNVIRTADDR=0xc0000000
+options		IPI_IRQ_START=0
+options		IPI_IRQ_END=15
diff --git a/sys/arm/mv/mvreg.h b/sys/arm/mv/mvreg.h
index fa807a0..1190fde 100644
--- a/sys/arm/mv/mvreg.h
+++ b/sys/arm/mv/mvreg.h
@@ -465,8 +465,28 @@
 #if defined(SOC_MV_ARMADA38X)
 #define	MV_SCU_BASE		(MV_BASE + 0xc000)
 #define	MV_SCU_REGS_LEN		0x100
-#define	MV_SCU_REG_CTRL		0
+#define	MV_SCU_REG_CTRL		0x00
+#define	MV_SCU_REG_CONFIG	0x04
 #define	MV_SCU_ENABLE		1
 #endif
 
+/*
+ * PMSU
+ */
+#if defined(SOC_MV_ARMADA38X)
+#define	MV_PMSU_BASE		(MV_BASE + 0x22000)
+#define	MV_PMSU_REGS_LEN	0x1000
+#define	PMSU_BOOT_ADDR_REDIRECT_OFFSET(cpu)	(((cpu) * 0x100) + 0x124)
+#endif
+
+/*
+ * CPU RESET
+ */
+#if defined(SOC_MV_ARMADA38X)
+#define	MV_CPU_RESET_BASE	(MV_BASE + 0x20800)
+#define	MV_CPU_RESET_REGS_LEN	0x8
+#define	CPU_RESET_OFFSET(cpu)	((cpu) * 0x8)
+#define	CPU_RESET_ASSERT	0x1
+#endif
+
 #endif /* _MVREG_H_ */