[PATCH] CNS3xxx: add SMP support

[PATCH] CNS3xxx: add SMP support

[Imre Kaloz]

This patch adds support for SMP on the CNS3xxx SoCs.

Tested on a Gateworks Laguna board.

Signed-off-by: Imre Kaloz <kaloz@openwrt.org>
---
 arch/arm/Kconfig                   |    2 +-
 arch/arm/mach-cns3xxx/Kconfig      |    1 +
 arch/arm/mach-cns3xxx/Makefile     |    3 +
 arch/arm/mach-cns3xxx/headsmp.S    |   42 +++++++++
 arch/arm/mach-cns3xxx/hotplug.c    |  130 ++++++++++++++++++++++++++
 arch/arm/mach-cns3xxx/localtimer.c |   26 ++++++
 arch/arm/mach-cns3xxx/platsmp.c    |  176 ++++++++++++++++++++++++++++++++++++
 7 files changed, 379 insertions(+), 1 deletions(-)
 create mode 100644 arch/arm/mach-cns3xxx/headsmp.S
 create mode 100644 arch/arm/mach-cns3xxx/hotplug.c
 create mode 100644 arch/arm/mach-cns3xxx/localtimer.c
 create mode 100644 arch/arm/mach-cns3xxx/platsmp.c

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 9adc278..324509d 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1311,7 +1311,7 @@ config SMP
 	depends on REALVIEW_EB_ARM11MP || REALVIEW_EB_A9MP || \
 		 MACH_REALVIEW_PB11MP || MACH_REALVIEW_PBX || ARCH_OMAP4 || \
 		 ARCH_EXYNOS4 || ARCH_TEGRA || ARCH_U8500 || ARCH_VEXPRESS_CA9X4 || \
-		 ARCH_MSM_SCORPIONMP || ARCH_SHMOBILE
+		 ARCH_MSM_SCORPIONMP || ARCH_SHMOBILE || ARCH_CNS3XXX
 	select USE_GENERIC_SMP_HELPERS
 	select HAVE_ARM_SCU if !ARCH_MSM_SCORPIONMP
 	help
diff --git a/arch/arm/mach-cns3xxx/Kconfig b/arch/arm/mach-cns3xxx/Kconfig
index 29b13f2..a13f401 100644
--- a/arch/arm/mach-cns3xxx/Kconfig
+++ b/arch/arm/mach-cns3xxx/Kconfig
@@ -3,6 +3,7 @@ menu "CNS3XXX platform type"
 
 config MACH_CNS3420VB
 	bool "Support for CNS3420 Validation Board"
+	select HAVE_ARM_SCU if SMP
 	select MIGHT_HAVE_PCI
 	help
 	  Include support for the Cavium Networks CNS3420 MPCore Platform
diff --git a/arch/arm/mach-cns3xxx/Makefile b/arch/arm/mach-cns3xxx/Makefile
index 11033f1..4f78392 100644
--- a/arch/arm/mach-cns3xxx/Makefile
+++ b/arch/arm/mach-cns3xxx/Makefile
@@ -1,3 +1,6 @@
 obj-$(CONFIG_ARCH_CNS3XXX)		+= core.o pm.o devices.o
 obj-$(CONFIG_PCI)			+= pcie.o
 obj-$(CONFIG_MACH_CNS3420VB)		+= cns3420vb.o
+obj-$(CONFIG_SMP)			+= platsmp.o headsmp.o
+obj-$(CONFIG_HOTPLUG_CPU)		+= hotplug.o
+obj-$(CONFIG_LOCAL_TIMERS)		+= localtimer.o
diff --git a/arch/arm/mach-cns3xxx/headsmp.S b/arch/arm/mach-cns3xxx/headsmp.S
new file mode 100644
index 0000000..456fd67
--- /dev/null
+++ b/arch/arm/mach-cns3xxx/headsmp.S
@@ -0,0 +1,42 @@
+/*
+ *  linux/arch/arm/mach-cns3xxx/headsmp.S
+ *
+ *  Cloned from linux/arch/arm/plat-versatile/headsmp.S
+ *
+ *  Copyright (c) 2003 ARM Limited
+ *  All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/linkage.h>
+#include <linux/init.h>
+
+	__INIT
+
+/*
+ * CNS3XXX specific entry point for secondary CPUs.  This provides
+ * a "holding pen" into which all secondary cores are held until we're
+ * ready for them to initialise.
+ */
+ENTRY(cns3xxx_secondary_startup)
+	mrc	p15, 0, r0, c0, c0, 5
+	and	r0, r0, #15
+	adr	r4, 1f
+	ldmia	r4, {r5, r6}
+	sub	r4, r4, r5
+	add	r6, r6, r4
+pen:	ldr	r7, [r6]
+	cmp	r7, r0
+	bne	pen
+
+	/*
+	 * we've been released from the holding pen: secondary_stack
+	 * should now contain the SVC stack for this core
+	 */
+	b	secondary_startup
+
+	.align
+1:	.long	.
+	.long	pen_release
diff --git a/arch/arm/mach-cns3xxx/hotplug.c b/arch/arm/mach-cns3xxx/hotplug.c
new file mode 100644
index 0000000..be0d499
--- /dev/null
+++ b/arch/arm/mach-cns3xxx/hotplug.c
@@ -0,0 +1,130 @@
+/* linux arch/arm/mach-cns3xxx/hotplug.c
+ *
+ *  Cloned from linux/arch/arm/mach-realview/hotplug.c
+ *
+ *  Copyright (C) 2002 ARM Ltd.
+ *  All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/smp.h>
+
+#include <asm/cacheflush.h>
+
+extern volatile int pen_release;
+
+static inline void cpu_enter_lowpower(void)
+{
+	unsigned int v;
+
+	flush_cache_all();
+	asm volatile(
+	"	mcr	p15, 0, %1, c7, c5, 0\n"
+	"	mcr	p15, 0, %1, c7, c10, 4\n"
+	/*
+	 * Turn off coherency
+	 */
+	"	mrc	p15, 0, %0, c1, c0, 1\n"
+	"	bic	%0, %0, %3\n"
+	"	mcr	p15, 0, %0, c1, c0, 1\n"
+	"	mrc	p15, 0, %0, c1, c0, 0\n"
+	"	bic	%0, %0, %2\n"
+	"	mcr	p15, 0, %0, c1, c0, 0\n"
+	  : "=&r" (v)
+	  : "r" (0), "Ir" (CR_C), "Ir" (0x40)
+	  : "cc");
+}
+
+static inline void cpu_leave_lowpower(void)
+{
+	unsigned int v;
+
+	asm volatile(
+	"mrc	p15, 0, %0, c1, c0, 0\n"
+	"	orr	%0, %0, %1\n"
+	"	mcr	p15, 0, %0, c1, c0, 0\n"
+	"	mrc	p15, 0, %0, c1, c0, 1\n"
+	"	orr	%0, %0, %2\n"
+	"	mcr	p15, 0, %0, c1, c0, 1\n"
+	  : "=&r" (v)
+	  : "Ir" (CR_C), "Ir" (0x40)
+	  : "cc");
+}
+
+static inline void platform_do_lowpower(unsigned int cpu, int *spurious)
+{
+	/*
+	 * there is no power-control hardware on this platform, so all
+	 * we can do is put the core into WFI; this is safe as the calling
+	 * code will have already disabled interrupts
+	 */
+	for (;;) {
+		/*
+		 * here's the WFI
+		 */
+		asm(".word	0xe320f003\n"
+		    :
+		    :
+		    : "memory", "cc");
+
+		if (pen_release == cpu) {
+			/*
+			 * OK, proper wakeup, we're done
+			 */
+			break;
+		}
+
+		/*
+		 * Getting here, means that we have come out of WFI without
+		 * having been woken up - this shouldn't happen
+		 *
+		 * Just note it happening - when we're woken, we can report
+		 * its occurrence.
+		 */
+		(*spurious)++;
+	}
+}
+
+int platform_cpu_kill(unsigned int cpu)
+{
+	return 1;
+}
+
+/*
+ * platform-specific code to shutdown a CPU
+ *
+ * Called with IRQs disabled
+ */
+void platform_cpu_die(unsigned int cpu)
+{
+	int spurious = 0;
+
+	/*
+	 * we're ready for shutdown now, so do it
+	 */
+	cpu_enter_lowpower();
+	platform_do_lowpower(cpu, &spurious);
+
+	/*
+	 * bring this CPU back into the world of cache
+	 * coherency, and then restore interrupts
+	 */
+	cpu_leave_lowpower();
+
+	if (spurious)
+		pr_warn("CPU%u: %u spurious wakeup calls\n", cpu, spurious);
+}
+
+int platform_cpu_disable(unsigned int cpu)
+{
+	/*
+	 * we don't allow CPU 0 to be shutdown (it is still too special
+	 * e.g. clock tick interrupts)
+	 */
+	return cpu == 0 ? -EPERM : 0;
+}
diff --git a/arch/arm/mach-cns3xxx/localtimer.c b/arch/arm/mach-cns3xxx/localtimer.c
new file mode 100644
index 0000000..5e1111c
--- /dev/null
+++ b/arch/arm/mach-cns3xxx/localtimer.c
@@ -0,0 +1,26 @@
+/* linux/arch/arm/mach-cns3xxx/localtimer.c
+ *
+ * Cloned from linux/arch/arm/mach-realview/localtimer.c
+ *
+ *  Copyright (C) 2002 ARM Ltd.
+ *  All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#include <linux/clockchips.h>
+
+#include <asm/irq.h>
+#include <asm/localtimer.h>
+
+/*
+ * Setup the local clock events for a CPU.
+ */
+int __cpuinit local_timer_setup(struct clock_event_device *evt)
+{
+	evt->irq = IRQ_LOCALTIMER;
+	twd_timer_setup(evt);
+	return 0;
+}
diff --git a/arch/arm/mach-cns3xxx/platsmp.c b/arch/arm/mach-cns3xxx/platsmp.c
new file mode 100644
index 0000000..f57fb5c
--- /dev/null
+++ b/arch/arm/mach-cns3xxx/platsmp.c
@@ -0,0 +1,176 @@
+/* linux/arch/arm/mach-cns3xxx/platsmp.c
+ *
+ * Copyright (C) 2011 Gateworks Corporation
+ *		Chris Lang <clang@gateworks.com>
+ *
+ * Cloned from linux/arch/arm/mach-vexpress/platsmp.c
+ *
+ *  Copyright (C) 2002 ARM Ltd.
+ *  All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/jiffies.h>
+#include <linux/smp.h>
+#include <linux/io.h>
+
+#include <asm/cacheflush.h>
+#include <asm/hardware/gic.h>
+#include <asm/smp_scu.h>
+#include <asm/unified.h>
+
+#include <mach/hardware.h>
+#include <mach/cns3xxx.h>
+
+extern void cns3xxx_secondary_startup(void);
+
+/*
+ * control for which core is the next to come out of the secondary
+ * boot "holding pen"
+ */
+
+volatile int __cpuinitdata pen_release = -1;
+
+/*
+ * Write pen_release in a way that is guaranteed to be visible to all
+ * observers, irrespective of whether they're taking part in coherency
+ * or not.  This is necessary for the hotplug code to work reliably.
+ */
+static void write_pen_release(int val)
+{
+	pen_release = val;
+	smp_wmb();
+	__cpuc_flush_dcache_area((void *)&pen_release, sizeof(pen_release));
+	outer_clean_range(__pa(&pen_release), __pa(&pen_release + 1));
+}
+
+static void __iomem *scu_base_addr(void)
+{
+	return (void __iomem *)(CNS3XXX_TC11MP_SCU_BASE_VIRT);
+}
+
+static DEFINE_SPINLOCK(boot_lock);
+
+void __cpuinit platform_secondary_init(unsigned int cpu)
+{
+	/*
+	 * if any interrupts are already enabled for the primary
+	 * core (e.g. timer irq), then they will not have been enabled
+	 * for us: do so
+	 */
+	gic_secondary_init(0);
+
+	/*
+	 * let the primary processor know we're out of the
+	 * pen, then head off into the C entry point
+	 */
+	write_pen_release(-1);
+
+	/*
+	 * Synchronise with the boot thread.
+	 */
+	spin_lock(&boot_lock);
+	spin_unlock(&boot_lock);
+}
+
+int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	unsigned long timeout;
+
+	/*
+	 * Set synchronisation state between this boot processor
+	 * and the secondary one
+	 */
+	spin_lock(&boot_lock);
+
+	/*
+	 * The secondary processor is waiting to be released from
+	 * the holding pen - release it, then wait for it to flag
+	 * that it has been released by resetting pen_release.
+	 *
+	 * Note that "pen_release" is the hardware CPU ID, whereas
+	 * "cpu" is Linux's internal ID.
+	 */
+	write_pen_release(cpu);
+
+	/*
+	 * Send the secondary CPU a soft interrupt, thereby causing
+	 * the boot monitor to read the system wide flags register,
+	 * and branch to the address found there.
+	 */
+	gic_raise_softirq(cpumask_of(cpu), 1);
+
+	timeout = jiffies + (1 * HZ);
+	while (time_before(jiffies, timeout)) {
+		smp_rmb();
+		if (pen_release == -1)
+			break;
+
+		udelay(10);
+	}
+
+	/*
+	 * now the secondary core is starting up let it run its
+	 * calibrations, then wait for it to finish
+	 */
+	spin_unlock(&boot_lock);
+
+	return pen_release != -1 ? -ENOSYS : 0;
+}
+
+/*
+ * Initialise the CPU possible map early - this describes the CPUs
+ * which may be present or become present in the system.
+ */
+
+void __init smp_init_cpus(void)
+{
+	void __iomem *scu_base = scu_base_addr();
+	unsigned int i, ncores;
+
+	ncores = scu_base ? scu_get_core_count(scu_base) : 1;
+
+	/* sanity check */
+	if (ncores > NR_CPUS) {
+		printk(KERN_WARNING
+		       "cns3xxx: no. of cores (%d) greater than configured "
+		       "maximum of %d - clipping\n",
+		       ncores, NR_CPUS);
+		ncores = NR_CPUS;
+	}
+
+	for (i = 0; i < ncores; i++)
+		set_cpu_possible(i, true);
+
+	set_smp_cross_call(gic_raise_softirq);
+}
+
+void __init platform_smp_prepare_cpus(unsigned int max_cpus)
+{
+	int i;
+
+	/*
+	 * Initialise the present map, which describes the set of CPUs
+	 * actually populated@the present time.
+	 */
+	for (i = 0; i < max_cpus; i++)
+		set_cpu_present(i, true);
+
+	scu_enable(scu_base_addr());
+
+	/*
+	 * Write the address of secondary startup into the
+	 * system-wide flags register. The boot monitor waits
+	 * until it receives a soft interrupt, and then the
+	 * secondary CPU branches to this address.
+	 */
+	__raw_writel(virt_to_phys(cns3xxx_secondary_startup),
+			(void __iomem *)(CNS3XXX_MISC_BASE_VIRT + 0x0600));
+}
-- 
1.7.1

[PATCH] CNS3xxx: add SMP support

[Russell King - ARM Linux]

On Mon, Jul 18, 2011 at 09:55:01PM +0200, Imre Kaloz wrote:
> diff --git a/arch/arm/mach-cns3xxx/Kconfig b/arch/arm/mach-cns3xxx/Kconfig
> index 29b13f2..a13f401 100644
> --- a/arch/arm/mach-cns3xxx/Kconfig
> +++ b/arch/arm/mach-cns3xxx/Kconfig
> @@ -3,6 +3,7 @@ menu "CNS3XXX platform type"
>  
>  config MACH_CNS3420VB
>  	bool "Support for CNS3420 Validation Board"
> +	select HAVE_ARM_SCU if SMP

This is not required:

config SMP
        select HAVE_ARM_SCU if !ARCH_MSM_SCORPIONMP

So you get the ARM SCU if you enable SMP anyway (if you're not scorpionmp).

> diff --git a/arch/arm/mach-cns3xxx/headsmp.S b/arch/arm/mach-cns3xxx/headsmp.S
> new file mode 100644
> index 0000000..456fd67
> --- /dev/null
> +++ b/arch/arm/mach-cns3xxx/headsmp.S
> @@ -0,0 +1,42 @@
> +/*
> + *  linux/arch/arm/mach-cns3xxx/headsmp.S
> + *
> + *  Cloned from linux/arch/arm/plat-versatile/headsmp.S
> + *
> + *  Copyright (c) 2003 ARM Limited
> + *  All Rights Reserved
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + */
> +#include <linux/linkage.h>
> +#include <linux/init.h>
> +
> +	__INIT

This should (in all likely) be __CPUINIT so that secondary CPUs can
re-enter this on hot-plug, or if you boot with max_cpus= lower than
the number of CPUs you have and you later hotplug them in.

> diff --git a/arch/arm/mach-cns3xxx/hotplug.c b/arch/arm/mach-cns3xxx/hotplug.c
> new file mode 100644
> index 0000000..be0d499
> --- /dev/null
> +++ b/arch/arm/mach-cns3xxx/hotplug.c
> @@ -0,0 +1,130 @@
> +/* linux arch/arm/mach-cns3xxx/hotplug.c
> + *
> + *  Cloned from linux/arch/arm/mach-realview/hotplug.c
> + *
> + *  Copyright (C) 2002 ARM Ltd.
> + *  All Rights Reserved
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> +*/
> +
> +#include <linux/kernel.h>
> +#include <linux/errno.h>
> +#include <linux/smp.h>
> +
> +#include <asm/cacheflush.h>
> +
> +extern volatile int pen_release;
> +
> +static inline void cpu_enter_lowpower(void)
> +{
> +	unsigned int v;
> +
> +	flush_cache_all();
> +	asm volatile(
> +	"	mcr	p15, 0, %1, c7, c5, 0\n"
> +	"	mcr	p15, 0, %1, c7, c10, 4\n"
> +	/*
> +	 * Turn off coherency
> +	 */
> +	"	mrc	p15, 0, %0, c1, c0, 1\n"
> +	"	bic	%0, %0, %3\n"
> +	"	mcr	p15, 0, %0, c1, c0, 1\n"
> +	"	mrc	p15, 0, %0, c1, c0, 0\n"
> +	"	bic	%0, %0, %2\n"
> +	"	mcr	p15, 0, %0, c1, c0, 0\n"
> +	  : "=&r" (v)
> +	  : "r" (0), "Ir" (CR_C), "Ir" (0x40)
> +	  : "cc");
> +}
> +
> +static inline void cpu_leave_lowpower(void)
> +{
> +	unsigned int v;
> +
> +	asm volatile(
> +	"mrc	p15, 0, %0, c1, c0, 0\n"
> +	"	orr	%0, %0, %1\n"
> +	"	mcr	p15, 0, %0, c1, c0, 0\n"
> +	"	mrc	p15, 0, %0, c1, c0, 1\n"
> +	"	orr	%0, %0, %2\n"
> +	"	mcr	p15, 0, %0, c1, c0, 1\n"
> +	  : "=&r" (v)
> +	  : "Ir" (CR_C), "Ir" (0x40)
> +	  : "cc");
> +}
> +
> +static inline void platform_do_lowpower(unsigned int cpu, int *spurious)
> +{
> +	/*
> +	 * there is no power-control hardware on this platform, so all
> +	 * we can do is put the core into WFI; this is safe as the calling
> +	 * code will have already disabled interrupts
> +	 */
> +	for (;;) {
> +		/*
> +		 * here's the WFI
> +		 */
> +		asm(".word	0xe320f003\n"
> +		    :
> +		    :
> +		    : "memory", "cc");
> +
> +		if (pen_release == cpu) {
> +			/*
> +			 * OK, proper wakeup, we're done
> +			 */
> +			break;
> +		}
> +
> +		/*
> +		 * Getting here, means that we have come out of WFI without
> +		 * having been woken up - this shouldn't happen
> +		 *
> +		 * Just note it happening - when we're woken, we can report
> +		 * its occurrence.
> +		 */
> +		(*spurious)++;
> +	}
> +}
> +
> +int platform_cpu_kill(unsigned int cpu)
> +{
> +	return 1;
> +}
> +
> +/*
> + * platform-specific code to shutdown a CPU
> + *
> + * Called with IRQs disabled
> + */
> +void platform_cpu_die(unsigned int cpu)
> +{
> +	int spurious = 0;
> +
> +	/*
> +	 * we're ready for shutdown now, so do it
> +	 */
> +	cpu_enter_lowpower();
> +	platform_do_lowpower(cpu, &spurious);
> +
> +	/*
> +	 * bring this CPU back into the world of cache
> +	 * coherency, and then restore interrupts
> +	 */
> +	cpu_leave_lowpower();
> +
> +	if (spurious)
> +		pr_warn("CPU%u: %u spurious wakeup calls\n", cpu, spurious);
> +}
> +
> +int platform_cpu_disable(unsigned int cpu)
> +{
> +	/*
> +	 * we don't allow CPU 0 to be shutdown (it is still too special
> +	 * e.g. clock tick interrupts)
> +	 */
> +	return cpu == 0 ? -EPERM : 0;
> +}

Have you thought about whether that's the best method?  Can you put a
secondary CPU back into reset or power down?  If so, it may be a good
idea to do that, rather than just copying the Realview code.

> +static void __iomem *scu_base_addr(void)
> +{
> +	return (void __iomem *)(CNS3XXX_TC11MP_SCU_BASE_VIRT);
> +}

If it's at a fixed address then don't do this.  Use a macro directly.
Even better - make sure your macros like CNS3XXX_TC11MP_SCU_BASE_VIRT
are correctly typed in the first place and avoid littering code with
casts.

> +	/*
> +	 * Send the secondary CPU a soft interrupt, thereby causing
> +	 * the boot monitor to read the system wide flags register,
> +	 * and branch to the address found there.
> +	 */
> +	gic_raise_softirq(cpumask_of(cpu), 1);

Will IPI0 do?  If so, please use IPI0.

> +void __init platform_smp_prepare_cpus(unsigned int max_cpus)
> +{
> +	int i;
> +
> +	/*
> +	 * Initialise the present map, which describes the set of CPUs
> +	 * actually populated at the present time.
> +	 */
> +	for (i = 0; i < max_cpus; i++)
> +		set_cpu_present(i, true);
> +
> +	scu_enable(scu_base_addr());
> +
> +	/*
> +	 * Write the address of secondary startup into the
> +	 * system-wide flags register. The boot monitor waits
> +	 * until it receives a soft interrupt, and then the
> +	 * secondary CPU branches to this address.
> +	 */
> +	__raw_writel(virt_to_phys(cns3xxx_secondary_startup),
> +			(void __iomem *)(CNS3XXX_MISC_BASE_VIRT + 0x0600));

Best move this into your boot_secondary() function so that hotplug works
after boot.

> +}

The above will change at the next merge window - if your CPUs are 0..N
then there'll be no need to touch the cpu present array.  So the only
thing left should be the call to scu_enable() here.

[PATCH] CNS3xxx: Add SMP support

[Krzysztof Hałasa]

Signed-off-by: Krzysztof Ha?asa <khalasa@piap.pl>

--- a/arch/arm/mach-cns3xxx/Kconfig
+++ b/arch/arm/mach-cns3xxx/Kconfig
@@ -3,6 +3,9 @@ menuconfig ARCH_CNS3XXX
 	bool "Cavium Networks CNS3XXX family"
 	depends on ARCH_MULTI_V6
 	select ARM_GIC
+	select HAVE_ARM_SCU if SMP
+	select HAVE_ARM_TWD
+	select HAVE_SMP
 	help
 	  Support for Cavium Networks CNS3XXX platform.
 
--- a/arch/arm/mach-cns3xxx/Makefile
+++ b/arch/arm/mach-cns3xxx/Makefile
@@ -4,3 +4,4 @@ cns3xxx-y				+= core.o pm.o
 cns3xxx-$(CONFIG_ATAGS)			+= devices.o
 cns3xxx-$(CONFIG_PCI)			+= pcie.o
 cns3xxx-$(CONFIG_MACH_CNS3420VB)	+= cns3420vb.o
+cns3xxx-$(CONFIG_SMP)			+= platsmp.o headsmp.o
--- a/arch/arm/mach-cns3xxx/cns3420vb.c
+++ b/arch/arm/mach-cns3xxx/cns3420vb.c
@@ -251,5 +251,6 @@ MACHINE_START(CNS3420VB, "Cavium Networks CNS3420 Validation Board")
 	.init_time	= cns3xxx_timer_init,
 	.init_machine	= cns3420_init,
 	.init_late      = cns3xxx_pcie_init_late,
+	.smp		= smp_ops(cns3xxx_smp_ops),
 	.restart	= cns3xxx_restart,
 MACHINE_END
--- a/arch/arm/mach-cns3xxx/core.c
+++ b/arch/arm/mach-cns3xxx/core.c
@@ -17,6 +17,7 @@
 #include <linux/platform_device.h>
 #include <linux/usb/ehci_pdriver.h>
 #include <linux/usb/ohci_pdriver.h>
+#include <asm/smp_twd.h>
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
 #include <asm/mach/time.h>
@@ -26,6 +27,8 @@
 #include "core.h"
 #include "pm.h"
 
+#define IRQ_LOCALTIMER 29
+
 static struct map_desc cns3xxx_io_desc[] __initdata = {
 	{
 		.virtual	= CNS3XXX_TC11MP_SCU_BASE_VIRT,
@@ -198,6 +201,15 @@ static struct irqaction cns3xxx_timer_irq = {
 	.handler	= cns3xxx_timer_interrupt,
 };
 
+static void __init cns3xxx_init_twd(void)
+{
+	static DEFINE_TWD_LOCAL_TIMER(cns3xx_twd_local_timer,
+		CNS3XXX_TC11MP_TWD_BASE,
+		IRQ_LOCALTIMER);
+
+	twd_local_timer_register(&cns3xx_twd_local_timer);
+}
+
 /*
  * Set up the clock source and clock events devices
  */
@@ -251,6 +263,7 @@ static void __init __cns3xxx_timer_init(unsigned int timer_irq)
 	setup_irq(timer_irq, &cns3xxx_timer_irq);
 
 	cns3xxx_clockevents_init(timer_irq);
+	cns3xxx_init_twd();
 }
 
 void __init cns3xxx_timer_init(void)
--- a/arch/arm/mach-cns3xxx/core.h
+++ b/arch/arm/mach-cns3xxx/core.h
@@ -13,6 +13,7 @@
 
 #include <linux/reboot.h>
 
+extern struct smp_operations cns3xxx_smp_ops;
 extern void cns3xxx_timer_init(void);
 
 #ifdef CONFIG_CACHE_L2X0
--- /dev/null
+++ b/arch/arm/mach-cns3xxx/headsmp.S
@@ -0,0 +1,39 @@
+/*
+ *  Cloned from linux/arch/arm/mach-exynos/headsmp.S
+ *
+ *  Copyright (c) 2003 ARM Limited
+ *  All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/linkage.h>
+#include <linux/init.h>
+
+/*
+ * CNS3XXX specific entry point for secondary CPUs.  This provides
+ * a "holding pen" into which all secondary cores are held until we're
+ * ready for them to initialise.
+ */
+ENTRY(cns3xxx_secondary_startup)
+	mrc	p15, 0, r0, c0, c0, 5
+	and	r0, r0, #15
+	adr	r4, 1f
+	ldmia	r4, {r5, r6}
+	sub	r4, r4, r5
+	add	r6, r6, r4
+pen:	ldr	r7, [r6]
+	cmp	r7, r0
+	bne	pen
+
+	/*
+	 * we've been released from the holding pen: secondary_stack
+	 * should now contain the SVC stack for this core
+	 */
+	b	secondary_startup
+ENDPROC(cns3xxx_secondary_startup)
+
+	.align 2
+1:	.long	.
+	.long	pen_release
--- /dev/null
+++ b/arch/arm/mach-cns3xxx/platsmp.c
@@ -0,0 +1,151 @@
+/*
+ *  linux/arch/arm/mach-cns3xxx/platsmp.c
+ *
+ *  Copyright (C) 2002 ARM Ltd.
+ *  Copyright 2012 Gateworks Corporation
+ *         Chris Lang <clang@gateworks.com>
+ *         Tim Harvey <tharvey@gateworks.com>
+ *
+ *  All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/jiffies.h>
+#include <linux/smp.h>
+#include <linux/io.h>
+#include <asm/cacheflush.h>
+#include <asm/smp_scu.h>
+#include <asm/unified.h>
+#include "cns3xxx.h"
+
+extern void cns3xxx_secondary_startup(void);
+
+#define SCU_CPU_STATUS 0x08
+#define SCU_BASE ((void __iomem *)CNS3XXX_TC11MP_SCU_BASE_VIRT)
+
+/*
+ * Write pen_release in a way that is guaranteed to be visible to all
+ * observers, irrespective of whether they're taking part in coherency
+ * or not.  This is necessary for the hotplug code to work reliably.
+ */
+static void write_pen_release(int val)
+{
+	pen_release = val;
+	smp_wmb();
+	__cpuc_flush_dcache_area((void *)&pen_release, sizeof(pen_release));
+	outer_clean_range(__pa(&pen_release), __pa(&pen_release + 1));
+}
+
+static DEFINE_SPINLOCK(boot_lock);
+
+static void cns3xxx_secondary_init(unsigned int cpu)
+{
+	/*
+	 * let the primary processor know we're out of the
+	 * pen, then head off into the C entry point
+	 */
+	write_pen_release(-1);
+
+	/*
+	 * Synchronise with the boot thread.
+	 */
+	spin_lock(&boot_lock);
+	spin_unlock(&boot_lock);
+}
+
+static int cns3xxx_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	unsigned long timeout;
+
+	/*
+	 * Set synchronisation state between this boot processor
+	 * and the secondary one
+	 */
+	spin_lock(&boot_lock);
+
+	/*
+	 * The secondary processor is waiting to be released from
+	 * the holding pen - release it, then wait for it to flag
+	 * that it has been released by resetting pen_release.
+	 *
+	 * Note that "pen_release" is the hardware CPU ID, whereas
+	 * "cpu" is Linux's internal ID.
+	 */
+	write_pen_release(cpu);
+
+	/*
+	 * Send the secondary CPU a soft interrupt, thereby causing
+	 * the boot monitor to read the system wide flags register,
+	 * and branch to the address found there.
+	 */
+	arch_send_wakeup_ipi_mask(cpumask_of(cpu));
+
+	timeout = jiffies + (1 * HZ);
+	while (time_before(jiffies, timeout)) {
+		smp_rmb();
+		if (pen_release == -1)
+			break;
+
+		udelay(10);
+	}
+
+	/*
+	 * now the secondary core is starting up let it run its
+	 * calibrations, then wait for it to finish
+	 */
+	spin_unlock(&boot_lock);
+
+	return pen_release != -1 ? -ENOSYS : 0;
+}
+
+/*
+ * Initialise the CPU possible map early - this describes the CPUs
+ * which may be present or become present in the system.
+ */
+static void __init cns3xxx_smp_init_cpus(void)
+{
+	unsigned int i, ncores;
+	unsigned int status;
+
+	/* SCU_CPU_STATUS must be examined instead of SCU_CONFIGURATION
+	 * used in scu_get_core_count
+	 */
+	status = __raw_readl(SCU_BASE + SCU_CPU_STATUS);
+	for (i = 0; i < NR_CPUS + 1; i++) {
+		if (((status >> (i * 2)) & 0x3) == 0)
+			set_cpu_possible(i, true);
+		else
+			break;
+	}
+	ncores = i;
+}
+
+static void __init cns3xxx_smp_prepare_cpus(unsigned int max_cpus)
+{
+	/*
+	 * enable SCU
+	 */
+	scu_enable(SCU_BASE);
+
+	/*
+	 * Write the address of secondary startup into the
+	 * system-wide flags register. The boot monitor waits
+	 * until it receives a soft interrupt, and then the
+	 * secondary CPU branches to this address.
+	 */
+	__raw_writel(virt_to_phys(cns3xxx_secondary_startup),
+			(void __iomem *)(CNS3XXX_MISC_BASE_VIRT + 0x0600));
+}
+
+struct smp_operations cns3xxx_smp_ops __initdata = {
+	.smp_init_cpus      = cns3xxx_smp_init_cpus,
+	.smp_prepare_cpus   = cns3xxx_smp_prepare_cpus,
+	.smp_secondary_init = cns3xxx_secondary_init,
+	.smp_boot_secondary = cns3xxx_boot_secondary,
+};

-- 
Krzysztof Halasa

Industrial Research Institute for Automation and Measurements PIAP
Al. Jerozolimskie 202, 02-486 Warsaw, Poland