[PATCH RFC] ARM: BCM5301X: Implement SMP support

@@ -15,6 +15,7 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "brcm,bcm4708-smp";
 
 		cpu at 0 {
 			device_type = "cpu";

@@ -36,6 +36,9 @@ obj-$(CONFIG_ARCH_BCM2835)	+= board_bcm2835.o
 
 # BCM5301X
 obj-$(CONFIG_ARCH_BCM_5301X)	+= bcm_5301x.o
+ifeq ($(CONFIG_SMP),y)
+obj-$(CONFIG_ARCH_BCM_5301X)	+= bcm5301x_smp.o bcm5301x_headsmp.o
+endif
 
 # BCM63XXx
 obj-$(CONFIG_ARCH_BCM_63XX)	:= bcm63xx.o

@@ -0,0 +1,108 @@
+/*
+ * Broadcom BCM470X / BCM5301X ARM platform code.
+ *
+ * Copyright 2003 - 2008 Broadcom Corporation
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+
+#include <asm/memory.h>
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+
+#define __virt_to_phys(x)	((x) - PAGE_OFFSET)
+
+/*
+ * v7_l1_cache_invalidate
+ *
+ * Invalidate contents of L1 cache without flushing its contents
+ * into outer cache and memory. This is needed when the contents
+ * of the cache are unpredictable after power-up.
+ *
+ * corrupts r0-r6
+ */
+ENTRY(v7_l1_cache_invalidate)
+	mov	r0, #0
+	mcr	p15, 2, r0, c0, c0, 0	@ set cache level to 1
+	mrc	p15, 1, r0, c0, c0, 0	@ read CLIDR
+
+	ldr	r1, =0x7fff
+	and	r2, r1, r0, lsr #13	@ get max # of index size
+
+	ldr	r1, =0x3ff
+	and	r3, r1, r0, lsr #3	@ NumWays - 1
+	add	r2, r2, #1		@ NumSets
+
+	and	r0, r0, #0x7
+	add	r0, r0, #4		@ SetShift
+
+	clz	r1, r3			@ WayShift
+	add	r4, r3, #1		@ NumWays
+1:	sub	r2, r2, #1		@ NumSets--
+	mov	r3, r4			@ Temp = NumWays
+2:	subs	r3, r3, #1		@ Temp--
+	mov	r5, r3, lsl r1
+	mov	r6, r2, lsl r0
+	orr	r5, r5, r6		@ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
+	mcr	p15, 0, r5, c7, c6, 2	@ Invalidate line
+	bgt	2b
+	cmp	r2, #0
+	bgt	1b
+	dsb
+	mov	r0,#0
+	mcr	p15,0,r0,c7,c5,0	/* Invalidate icache */
+	isb
+	mov	pc, lr
+ENDPROC(v7_l1_cache_invalidate)
+
+/*
+ * Platform 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.
+ */
+	__CPUINIT
+ENTRY(bcm5301x_secondary_startup)
+	/*
+	 * Get hardware CPU id of ours
+	 */
+	mrc	p15, 0, r0, c0, c0, 5
+	and	r0, r0, #15
+	/*
+	 * Wait on <pen_release> variable by physical address
+	 * to contain our hardware CPU id
+	 */
+#ifdef CONFIG_SPARSEMEM
+	ldr	r2, =(PAGE_OFFSET+SZ_128M)
+	ldr	r1, =pen_release
+	cmp	r1, r2
+	bge	1f
+	ldr	r2, =PAGE_OFFSET
+	sub	r6, r1, r2
+	b	2f
+1:
+	sub	r1, r1, r2
+	ldr	r2, =PHYS_OFFSET2
+	add	r6, r1, r2
+2:
+#else
+	ldr	r6, =__virt_to_phys(pen_release)
+#endif
+pen:	ldr	r7, [r6]
+	cmp	r7, r0
+	bne	pen
+	nop
+	/*
+	 * In case L1 cache has unpredictable contents at power-up
+	 * clean its contents without flushing.
+	 */
+	bl	v7_l1_cache_invalidate
+	nop
+	/*
+	 * we've been released from the holding pen: secondary_stack
+	 * should now contain the SVC stack for this core
+	 */
+	b	secondary_startup
+
+ENDPROC(bcm5301x_secondary_startup)
+	.ltorg

@@ -0,0 +1,185 @@
+/*
+ * Broadcom BCM470X / BCM5301X ARM platform code.
+ *
+ * Copyright (C) 2002 ARM Ltd.
+ * Copyright (C) 2015 Rafa? Mi?ecki <zajec5@gmail.com>
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+
+#include <asm/cacheflush.h>
+#include <asm/delay.h>
+#include <asm/smp_scu.h>
+
+#include <linux/clockchips.h>
+
+/*
+ * There is a 1KB LUT located at 0xFFFF0400-0xFFFFFFFF, and its first entry
+ * is where the secondary entry point needs to be written
+*/
+#define SOC_ROM_BASE_PA		0xffff0000
+#define SOC_ROM_LUT_OFF		0x400
+
+/* ENTRY in bcm5301x_headsmp.S */
+extern void bcm5301x_secondary_startup(void);
+
+static DEFINE_SPINLOCK(boot_lock);
+
+static void __cpuinit write_pen_release(int val)
+{
+	pen_release = val;
+	/* Make sure this store is visible to other CPUs */
+	smp_wmb();
+	__cpuc_flush_dcache_area((void *)&pen_release, sizeof(pen_release));
+	outer_clean_range(__pa(&pen_release), __pa(&pen_release + 1));
+}
+
+static void __init bcm5301x_smp_secondary_set_entry(void (*entry_point)(void))
+{
+	void __iomem *rombase = NULL;
+	phys_addr_t lut_pa;
+	u32 offset, mask;
+	u32 val;
+
+	mask = (1UL << PAGE_SHIFT) - 1;
+
+	lut_pa = SOC_ROM_BASE_PA & ~mask;
+	offset = SOC_ROM_BASE_PA &  mask;
+	offset += SOC_ROM_LUT_OFF;
+
+	rombase = ioremap(lut_pa, PAGE_SIZE);
+	if (!rombase)
+		return;
+	val = virt_to_phys(entry_point);
+
+	writel(val, rombase + offset);
+
+	smp_wmb();	/* probably not needed - io regs are not cached */
+	dsb_sev();	/* Exit WFI */
+	mb();
+
+	iounmap(rombase);
+}
+
+static void __init bcm5301x_smp_prepare_cpus(unsigned int max_cpus)
+{
+	void __iomem *scu_base;
+	unsigned int ncores;
+
+	if (!scu_a9_has_base()) {
+		pr_warn("Unknown SCU base\n");
+		return;
+	}
+
+	scu_base = ioremap((phys_addr_t)scu_a9_get_base(), SZ_256);
+	if (!scu_base) {
+		pr_err("Failed to remap SCU\n");
+		return;
+	}
+
+	ncores = scu_get_core_count(scu_base);
+	if (max_cpus > ncores) {
+		unsigned int i;
+
+		pr_warn("Possible CPU mask exceeds available cores, reducing to %u\n",
+			ncores);
+		for (i = ncores - 1; i < max_cpus; i++)
+			set_cpu_present(i, false);
+		max_cpus = ncores;
+	}
+
+	if (max_cpus > 1) {
+		/* nobody is to be released from the pen yet */
+		pen_release = -1;
+
+		/* Initialise the SCU */
+		scu_enable(scu_base);
+
+		/* Let CPUs know where to start */
+		bcm5301x_smp_secondary_set_entry(bcm5301x_secondary_startup);
+	}
+
+	iounmap(scu_base);
+}
+
+static void __cpuinit bcm5301x_smp_secondary_init(unsigned int cpu)
+{
+	trace_hardirqs_off();
+
+	/*
+	 * 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 __cpuinit bcm5301x_smp_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);
+
+	dsb_sev();
+
+	/*
+	 * Timeout set on purpose in jiffies so that on slow processors
+	 * that must also have low HZ it will wait longer.
+	 */
+	timeout = jiffies + (HZ * 10);
+
+	udelay(100);
+
+	/*
+	 * If the secondary CPU was waiting on WFE, it should
+	 * be already watching <pen_release>, or it could be
+	 * waiting in WFI, send it an IPI to be sure it wakes.
+	 */
+	if (pen_release != -1)
+		tick_broadcast(cpumask_of(cpu));
+
+	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;
+}
+
+static struct smp_operations bcm5301x_smp_ops __initdata = {
+	.smp_prepare_cpus	= bcm5301x_smp_prepare_cpus,
+	.smp_secondary_init	= bcm5301x_smp_secondary_init,
+	.smp_boot_secondary	= bcm5301x_smp_boot_secondary,
+};
+
+CPU_METHOD_OF_DECLARE(bcm5301x_smp, "brcm,bcm4708-smp",
+		      &bcm5301x_smp_ops);

@@ -6,3 +6,20 @@ Boards with the BCM4708 SoC shall have the following properties:
 Required root node property:
 
 compatible = "brcm,bcm4708";
+
+Optional sub-node properties:
+
+compatible = "brcm,bcm4708-sysram" SYSRAM for SMP bringup.
+		SMP-capable SoCs use part of the SYSRAM for storing
+		location of code to be executed by the extra cores.
+
+Example:
+
+	{
+		compatible = "brcm,bcm4708";
+
+		smp-sysram {
+			compatible = "brcm,bcm4708-sysram";
+			reg = <0xffff0000 0x1000>;
+		};
+	};

@@ -188,6 +188,7 @@ nodes to be present and contain the properties described below.
 			  can be one of:
 			    "allwinner,sun6i-a31"
 			    "arm,psci"
+			    "brcm,bcm4708-smp"
 			    "brcm,brahma-b15"
 			    "marvell,armada-375-smp"
 			    "marvell,armada-380-smp"

@@ -15,6 +15,7 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "brcm,bcm4708-smp";
 
 		cpu at 0 {
 			device_type = "cpu";

@@ -31,4 +32,8 @@
 		};
 	};
 
+	smp-sysram {
+		compatible = "brcm,bcm4708-sysram";
+		reg = <0xffff0000 0x1000>;
+	};
 };

@@ -36,6 +36,9 @@ obj-$(CONFIG_ARCH_BCM2835)	+= board_bcm2835.o
 
 # BCM5301X
 obj-$(CONFIG_ARCH_BCM_5301X)	+= bcm_5301x.o
+ifeq ($(CONFIG_SMP),y)
+obj-$(CONFIG_ARCH_BCM_5301X)	+= bcm5301x_smp.o bcm5301x_headsmp.o
+endif
 
 # BCM63XXx
 obj-$(CONFIG_ARCH_BCM_63XX)	:= bcm63xx.o

@@ -0,0 +1,46 @@
+/*
+ * Broadcom BCM470X / BCM5301X ARM platform code.
+ *
+ * Copyright (c) 2003 ARM Limited
+ * All Rights Reserved
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+#include <linux/linkage.h>
+
+/*
+ * BCM5301X specific entry point for secondary CPUs.
+ */
+ENTRY(bcm5301x_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
+
+	/*
+	 * In case L1 cache has unpredictable contents at power-up
+	 * clean its contents without flushing.
+	 */
+	/* bl	v7_l1_cache_invalidate */
+	bl      v7_invalidate_l1
+
+	mov	r0,#0
+	mcr	p15,0,r0,c7,c5,0	/* Invalidate icache */
+	dsb
+	isb
+
+	/*
+	 * we've been released from the holding pen: secondary_stack
+	 * should now contain the SVC stack for this core
+	 */
+	b	secondary_startup
+ENDPROC(bcm5301x_secondary_startup)
+
+	.align 2
+1:	.long	.
+	.long	pen_release

@@ -0,0 +1,160 @@
+/*
+ * Broadcom BCM470X / BCM5301X ARM platform code.
+ *
+ * Copyright (C) 2002 ARM Ltd.
+ * Copyright (C) 2015 Rafa? Mi?ecki <zajec5@gmail.com>
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+
+#include <asm/cacheflush.h>
+#include <asm/delay.h>
+#include <asm/smp_plat.h>
+#include <asm/smp_scu.h>
+
+#include <linux/clockchips.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#define SOC_ROM_LUT_OFF		0x400
+
+extern void bcm5301x_secondary_startup(void);
+
+static void __cpuinit write_pen_release(int val)
+{
+	pen_release = val;
+	smp_wmb();
+	sync_cache_w(&pen_release);
+}
+
+static DEFINE_SPINLOCK(boot_lock);
+
+static void __init bcm5301x_smp_secondary_set_entry(void (*entry_point)(void))
+{
+	void __iomem *sysram_base_addr = NULL;
+	struct device_node *node;
+
+	for_each_compatible_node(node, NULL, "brcm,bcm4708-sysram") {
+		if (!of_device_is_available(node))
+			continue;
+		sysram_base_addr = of_iomap(node, 0);
+		break;
+	}
+
+	if (!sysram_base_addr) {
+		pr_warn("Failed to map sysram\n");
+		return;
+	}
+
+	writel(virt_to_phys(entry_point), sysram_base_addr + SOC_ROM_LUT_OFF);
+
+	dsb_sev();	/* Exit WFI */
+	mb();		/* make sure write buffer is drained */
+
+	iounmap(sysram_base_addr);
+}
+
+static void __init bcm5301x_smp_prepare_cpus(unsigned int max_cpus)
+{
+	void __iomem *scu_base;
+
+	if (!scu_a9_has_base()) {
+		pr_warn("Unknown SCU base\n");
+		return;
+	}
+
+	scu_base = ioremap((phys_addr_t)scu_a9_get_base(), SZ_256);
+	if (!scu_base) {
+		pr_err("Failed to remap SCU\n");
+		return;
+	}
+
+	/* Initialise the SCU */
+	scu_enable(scu_base);
+
+	/* Let CPUs know where to start */
+	bcm5301x_smp_secondary_set_entry(bcm5301x_secondary_startup);
+
+	iounmap(scu_base);
+}
+
+static void __cpuinit bcm5301x_smp_secondary_init(unsigned int cpu)
+{
+	trace_hardirqs_off();
+
+	/*
+	 * 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 __cpuinit bcm5301x_smp_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_logical_map(cpu));
+
+	 /* Send the secondary CPU SEV */
+	dsb_sev();
+
+	udelay(100);
+
+	/*
+	 * 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 set on purpose in jiffies so that on slow processors
+	 * that must also have low HZ it will wait longer.
+	 */
+	timeout = jiffies + (HZ * 10);
+	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;
+}
+
+static struct smp_operations bcm5301x_smp_ops __initdata = {
+	.smp_prepare_cpus	= bcm5301x_smp_prepare_cpus,
+	.smp_secondary_init	= bcm5301x_smp_secondary_init,
+	.smp_boot_secondary	= bcm5301x_smp_boot_secondary,
+};
+
+CPU_METHOD_OF_DECLARE(bcm5301x_smp, "brcm,bcm4708-smp",
+		      &bcm5301x_smp_ops);

@@ -6,3 +6,27 @@ Boards with the BCM4708 SoC shall have the following properties:
 Required root node property:
 
 compatible = "brcm,bcm4708";
+
+Optional sub-node properties:
+
+compatible = "mmio-sram" for SRAM access with IO memory region
+		This is needed for SMP-capable SoCs which use part of
+		SRAM for storing location of code to be executed by the
+		extra cores.
+		SMP support requires another sub-node with compatible
+		property "brcm,bcm4708-sysram".
+
+Example:
+
+	sysram at ffff0000 {
+		compatible = "mmio-sram";
+		reg = <0xffff0000 0x10000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges = <0 0xffff0000 0x10000>;
+
+		smp-sysram at 0 {
+			compatible = "brcm,bcm4708-sysram";
+			reg = <0x0 0x1000>;
+		};
+	};

@@ -189,6 +189,7 @@ nodes to be present and contain the properties described below.
 			  can be one of:
 			    "allwinner,sun6i-a31"
 			    "arm,psci"
+			    "brcm,bcm4708-smp"
 			    "brcm,brahma-b15"
 			    "marvell,armada-375-smp"
 			    "marvell,armada-380-smp"

@@ -15,6 +15,7 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "brcm,bcm4708-smp";
 
 		cpu at 0 {
 			device_type = "cpu";

@@ -31,4 +32,16 @@
 		};
 	};
 
+	sysram at ffff0000 {
+		compatible = "mmio-sram";
+		reg = <0xffff0000 0x10000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges = <0 0xffff0000 0x10000>;
+
+		smp-sysram at 0 {
+			compatible = "brcm,bcm4708-sysram";
+			reg = <0x0 0x1000>;
+		};
+	};
 };

@@ -36,6 +36,9 @@ obj-$(CONFIG_ARCH_BCM2835)	+= board_bcm2835.o
 
 # BCM5301X
 obj-$(CONFIG_ARCH_BCM_5301X)	+= bcm_5301x.o
+ifeq ($(CONFIG_SMP),y)
+obj-$(CONFIG_ARCH_BCM_5301X)	+= bcm5301x_smp.o bcm5301x_headsmp.o
+endif
 
 # BCM63XXx
 obj-$(CONFIG_ARCH_BCM_63XX)	:= bcm63xx.o

@@ -0,0 +1,45 @@
+/*
+ * Broadcom BCM470X / BCM5301X ARM platform code.
+ *
+ * Copyright (c) 2003 ARM Limited
+ * All Rights Reserved
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+#include <linux/linkage.h>
+
+/*
+ * BCM5301X specific entry point for secondary CPUs.
+ */
+ENTRY(bcm5301x_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
+
+	/*
+	 * In case L1 cache has unpredictable contents at power-up
+	 * clean its contents without flushing.
+	 */
+	bl      v7_invalidate_l1
+
+	mov	r0, #0
+	mcr	p15, 0, r0, c7, c5, 0	/* Invalidate icache */
+	dsb
+	isb
+
+	/*
+	 * we've been released from the holding pen: secondary_stack
+	 * should now contain the SVC stack for this core
+	 */
+	b	secondary_startup
+ENDPROC(bcm5301x_secondary_startup)
+
+	.align 2
+1:	.long	.
+	.long	pen_release

@@ -0,0 +1,158 @@
+/*
+ * Broadcom BCM470X / BCM5301X ARM platform code.
+ *
+ * Copyright (C) 2002 ARM Ltd.
+ * Copyright (C) 2015 Rafa? Mi?ecki <zajec5@gmail.com>
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+
+#include <asm/cacheflush.h>
+#include <asm/delay.h>
+#include <asm/smp_plat.h>
+#include <asm/smp_scu.h>
+
+#include <linux/clockchips.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#define SOC_ROM_LUT_OFF		0x400
+
+extern void bcm5301x_secondary_startup(void);
+
+static void __cpuinit write_pen_release(int val)
+{
+	pen_release = val;
+	smp_wmb();
+	sync_cache_w(&pen_release);
+}
+
+static DEFINE_SPINLOCK(boot_lock);
+
+static void __init bcm5301x_smp_secondary_set_entry(void (*entry_point)(void))
+{
+	void __iomem *sysram_base_addr = NULL;
+	struct device_node *node;
+
+	node = of_find_compatible_node(NULL, NULL, "brcm,bcm4708-sysram");
+	if (!of_device_is_available(node))
+		return;
+
+	sysram_base_addr = of_iomap(node, 0);
+	if (!sysram_base_addr) {
+		pr_warn("Failed to map sysram\n");
+		return;
+	}
+
+	writel(virt_to_phys(entry_point), sysram_base_addr + SOC_ROM_LUT_OFF);
+
+	dsb_sev();	/* Exit WFI */
+	mb();		/* make sure write buffer is drained */
+
+	iounmap(sysram_base_addr);
+}
+
+static void __init bcm5301x_smp_prepare_cpus(unsigned int max_cpus)
+{
+	void __iomem *scu_base;
+
+	if (!scu_a9_has_base()) {
+		pr_warn("Unknown SCU base\n");
+		return;
+	}
+
+	scu_base = ioremap((phys_addr_t)scu_a9_get_base(), SZ_256);
+	if (!scu_base) {
+		pr_err("Failed to remap SCU\n");
+		return;
+	}
+
+	/* Initialise the SCU */
+	scu_enable(scu_base);
+
+	/* Let CPUs know where to start */
+	bcm5301x_smp_secondary_set_entry(bcm5301x_secondary_startup);
+
+	iounmap(scu_base);
+}
+
+static void __cpuinit bcm5301x_smp_secondary_init(unsigned int cpu)
+{
+	trace_hardirqs_off();
+
+	/*
+	 * 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 __cpuinit bcm5301x_smp_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_logical_map(cpu));
+
+	 /* Send the secondary CPU SEV */
+	dsb_sev();
+
+	udelay(100);
+
+	/*
+	 * 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 set on purpose in jiffies so that on slow processors
+	 * that must also have low HZ it will wait longer.
+	 */
+	timeout = jiffies + (HZ * 10);
+	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;
+}
+
+static struct smp_operations bcm5301x_smp_ops __initdata = {
+	.smp_prepare_cpus	= bcm5301x_smp_prepare_cpus,
+	.smp_secondary_init	= bcm5301x_smp_secondary_init,
+	.smp_boot_secondary	= bcm5301x_smp_boot_secondary,
+};
+
+CPU_METHOD_OF_DECLARE(bcm5301x_smp, "brcm,bcm4708-smp",
+		      &bcm5301x_smp_ops);