Merge tag 'mvebu_everything_for_3.8' of git://git.infradead.org/users/jcooper/linux into late/mvebu

From Jason Cooper. Unfortunately this is a combined branch with all
mvebu code as one drop, something we normally try to avoid and instead
slice vendor topics across our branches. Hopefully we can avoid doing
this again for 3.9!

mvebu everything for v3.8
 - due to the complex interdependencies of the received pull requests
   I decided to keep this in one branch the way they recommended merging it
 - this was their first attempt at doing pull requests, we'll work on it
   with them

 - added SMP support for mvebu SoCs
 - added coherency fabric
 - added mdio and mvneta drivers
 - added mirabox board
 - added openblocks ax3-4 board
 - clock fixes and improvements
 - converted mv_xor driver to devicetree (extensive series in itself)

* tag 'mvebu_everything_for_3.8' of git://git.infradead.org/users/jcooper/linux: (85 commits)
  dma: mv_xor: fix error handling path
  dma: mv_xor: fix error checking of irq_of_parse_and_map()
  dma: mv_xor: use request_irq() instead of devm_request_irq()
  dma: mv_xor: clear the window override control registers
  arm: mvebu: fix address decoding armada_cfg_base() function
  ARM: mvebu: update defconfig with I2C and RTC support
  ARM: mvebu: Add SATA support for OpenBlocks AX3-4
  ARM: mvebu: Add support for the RTC in OpenBlocks AX3-4
  ARM: mvebu: Add support for I2C on OpenBlocks AX3-4
  ARM: mvebu: Add support for I2C controllers in Armada 370/XP
  arm: mvebu: Add hardware I/O Coherency support
  arm: plat-orion: Add coherency attribute when setup mbus target
  arm: dma mapping: Export a dma ops function arm_dma_set_mask
  arm: mvebu: Add SMP support for Armada XP
  arm: mm: Add support for PJ4B cpu and init routines
  arm: mvebu: Add IPI support via doorbells
  arm: mvebu: Add initial support for power managmement service unit
  arm: mvebu: Add support for coherency fabric in mach-mvebu
  arm: mvebu: update defconfig to include XOR driver
  arm: mvebu: update defconfig to include network driver
  ...

Signed-off-by: Olof Johansson <olof@lixom.net>

15 files changed, 658 insertions, 15 deletions

diff --git a/arch/arm/mach-mvebu/Kconfig b/arch/arm/mach-mvebu/Kconfig
index 416d46ef7ebd..c934e1d4933d 100644
--- a/arch/arm/mach-mvebu/Kconfig
+++ b/arch/arm/mach-mvebu/Kconfig
@@ -9,6 +9,10 @@ config ARCH_MVEBU
 	select PINCTRL
 	select PLAT_ORION
 	select SPARSE_IRQ
+	select CLKDEV_LOOKUP
+	select MVEBU_CLK_CORE
+	select MVEBU_CLK_CPU
+	select MVEBU_CLK_GATING
 
 if ARCH_MVEBU
 
@@ -17,7 +21,8 @@ menu "Marvell SOC with device tree"
 config MACH_ARMADA_370_XP
 	bool
 	select ARMADA_370_XP_TIMER
-	select CPU_V7
+	select HAVE_SMP
+	select CPU_PJ4B
 
 config MACH_ARMADA_370
 	bool "Marvell Armada 370 boards"
diff --git a/arch/arm/mach-mvebu/Makefile b/arch/arm/mach-mvebu/Makefile
index 57f996b6aa0e..5dcb369b58aa 100644
--- a/arch/arm/mach-mvebu/Makefile
+++ b/arch/arm/mach-mvebu/Makefile
@@ -2,4 +2,6 @@ ccflags-$(CONFIG_ARCH_MULTIPLATFORM) := -I$(srctree)/$(src)/include \
 	-I$(srctree)/arch/arm/plat-orion/include
 
 obj-y += system-controller.o
-obj-$(CONFIG_MACH_ARMADA_370_XP) += armada-370-xp.o irq-armada-370-xp.o addr-map.o
+obj-$(CONFIG_MACH_ARMADA_370_XP) += armada-370-xp.o irq-armada-370-xp.o addr-map.o coherency.o coherency_ll.o pmsu.o
+obj-$(CONFIG_SMP)                += platsmp.o headsmp.o
+obj-$(CONFIG_HOTPLUG_CPU)        += hotplug.o
diff --git a/arch/arm/mach-mvebu/addr-map.c b/arch/arm/mach-mvebu/addr-map.c
index fe454a4430be..ab9b3bd4fef5 100644
--- a/arch/arm/mach-mvebu/addr-map.c
+++ b/arch/arm/mach-mvebu/addr-map.c
@@ -78,7 +78,7 @@ armada_cfg_base(const struct orion_addr_map_cfg *cfg, int win)
 	if (win < 8)
 		offset = (win << 4);
 	else
-		offset = ARMADA_WINDOW_8_PLUS_OFFSET + (win << 3);
+		offset = ARMADA_WINDOW_8_PLUS_OFFSET + ((win - 8) << 3);
 
 	return cfg->bridge_virt_base + offset;
 }
@@ -108,6 +108,9 @@ static int __init armada_setup_cpu_mbus(void)
 
 	addr_map_cfg.bridge_virt_base = mbus_unit_addr_decoding_base;
 
+	if (of_find_compatible_node(NULL, NULL, "marvell,coherency-fabric"))
+		addr_map_cfg.hw_io_coherency = 1;
+
 	/*
 	 * Disable, clear and configure windows.
 	 */
diff --git a/arch/arm/mach-mvebu/armada-370-xp.c b/arch/arm/mach-mvebu/armada-370-xp.c
index 49d791548ad6..7434b5e36197 100644
--- a/arch/arm/mach-mvebu/armada-370-xp.c
+++ b/arch/arm/mach-mvebu/armada-370-xp.c
@@ -17,11 +17,14 @@
 #include <linux/of_platform.h>
 #include <linux/io.h>
 #include <linux/time-armada-370-xp.h>
+#include <linux/clk/mvebu.h>
+#include <linux/dma-mapping.h>
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
 #include <asm/mach/time.h>
 #include "armada-370-xp.h"
 #include "common.h"
+#include "coherency.h"
 
 static struct map_desc armada_370_xp_io_desc[] __initdata = {
 	{
@@ -37,27 +40,45 @@ void __init armada_370_xp_map_io(void)
 	iotable_init(armada_370_xp_io_desc, ARRAY_SIZE(armada_370_xp_io_desc));
 }
 
+void __init armada_370_xp_timer_and_clk_init(void)
+{
+	mvebu_clocks_init();
+	armada_370_xp_timer_init();
+}
+
+void __init armada_370_xp_init_early(void)
+{
+	/*
+	 * Some Armada 370/XP devices allocate their coherent buffers
+	 * from atomic context. Increase size of atomic coherent pool
+	 * to make sure such the allocations won't fail.
+	 */
+	init_dma_coherent_pool_size(SZ_1M);
+}
+
 struct sys_timer armada_370_xp_timer = {
-	.init		= armada_370_xp_timer_init,
+	.init		= armada_370_xp_timer_and_clk_init,
 };
 
 static void __init armada_370_xp_dt_init(void)
 {
 	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
+	coherency_init();
 }
 
-static const char * const armada_370_xp_dt_board_dt_compat[] = {
-	"marvell,a370-db",
-	"marvell,axp-db",
+static const char * const armada_370_xp_dt_compat[] = {
+	"marvell,armada-370-xp",
 	NULL,
 };
 
-DT_MACHINE_START(ARMADA_XP_DT, "Marvell Aramada 370/XP (Device Tree)")
+DT_MACHINE_START(ARMADA_XP_DT, "Marvell Armada 370/XP (Device Tree)")
+	.smp		= smp_ops(armada_xp_smp_ops),
 	.init_machine	= armada_370_xp_dt_init,
 	.map_io		= armada_370_xp_map_io,
+	.init_early	= armada_370_xp_init_early,
 	.init_irq	= armada_370_xp_init_irq,
 	.handle_irq     = armada_370_xp_handle_irq,
 	.timer		= &armada_370_xp_timer,
 	.restart	= mvebu_restart,
-	.dt_compat	= armada_370_xp_dt_board_dt_compat,
+	.dt_compat	= armada_370_xp_dt_compat,
 MACHINE_END
diff --git a/arch/arm/mach-mvebu/armada-370-xp.h b/arch/arm/mach-mvebu/armada-370-xp.h
index aac9bebc6b03..c6a7d74fddfe 100644
--- a/arch/arm/mach-mvebu/armada-370-xp.h
+++ b/arch/arm/mach-mvebu/armada-370-xp.h
@@ -19,4 +19,11 @@
 #define ARMADA_370_XP_REGS_VIRT_BASE	IOMEM(0xfeb00000)
 #define ARMADA_370_XP_REGS_SIZE		SZ_1M
 
+#ifdef CONFIG_SMP
+#include <linux/cpumask.h>
+
+void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq);
+void armada_xp_mpic_smp_cpu_init(void);
+#endif
+
 #endif /* __MACH_ARMADA_370_XP_H */
diff --git a/arch/arm/mach-mvebu/coherency.c b/arch/arm/mach-mvebu/coherency.c
new file mode 100644
index 000000000000..8278960066c3
--- /dev/null
+++ b/arch/arm/mach-mvebu/coherency.c
@@ -0,0 +1,155 @@
+/*
+ * Coherency fabric (Aurora) support for Armada 370 and XP platforms.
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * Yehuda Yitschak <yehuday@marvell.com>
+ * Gregory Clement <gregory.clement@free-electrons.com>
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ *
+ * The Armada 370 and Armada XP SOCs have a coherency fabric which is
+ * responsible for ensuring hardware coherency between all CPUs and between
+ * CPUs and I/O masters. This file initializes the coherency fabric and
+ * supplies basic routines for configuring and controlling hardware coherency
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/of_address.h>
+#include <linux/io.h>
+#include <linux/smp.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <asm/smp_plat.h>
+#include "armada-370-xp.h"
+
+/*
+ * Some functions in this file are called very early during SMP
+ * initialization. At that time the device tree framework is not yet
+ * ready, and it is not possible to get the register address to
+ * ioremap it. That's why the pointer below is given with an initial
+ * value matching its virtual mapping
+ */
+static void __iomem *coherency_base = ARMADA_370_XP_REGS_VIRT_BASE + 0x20200;
+static void __iomem *coherency_cpu_base;
+
+/* Coherency fabric registers */
+#define COHERENCY_FABRIC_CFG_OFFSET		   0x4
+
+#define IO_SYNC_BARRIER_CTL_OFFSET		   0x0
+
+static struct of_device_id of_coherency_table[] = {
+	{.compatible = "marvell,coherency-fabric"},
+	{ /* end of list */ },
+};
+
+#ifdef CONFIG_SMP
+int coherency_get_cpu_count(void)
+{
+	int reg, cnt;
+
+	reg = readl(coherency_base + COHERENCY_FABRIC_CFG_OFFSET);
+	cnt = (reg & 0xF) + 1;
+
+	return cnt;
+}
+#endif
+
+/* Function defined in coherency_ll.S */
+int ll_set_cpu_coherent(void __iomem *base_addr, unsigned int hw_cpu_id);
+
+int set_cpu_coherent(unsigned int hw_cpu_id, int smp_group_id)
+{
+	if (!coherency_base) {
+		pr_warn("Can't make CPU %d cache coherent.\n", hw_cpu_id);
+		pr_warn("Coherency fabric is not initialized\n");
+		return 1;
+	}
+
+	return ll_set_cpu_coherent(coherency_base, hw_cpu_id);
+}
+
+static inline void mvebu_hwcc_sync_io_barrier(void)
+{
+	writel(0x1, coherency_cpu_base + IO_SYNC_BARRIER_CTL_OFFSET);
+	while (readl(coherency_cpu_base + IO_SYNC_BARRIER_CTL_OFFSET) & 0x1);
+}
+
+static dma_addr_t mvebu_hwcc_dma_map_page(struct device *dev, struct page *page,
+				  unsigned long offset, size_t size,
+				  enum dma_data_direction dir,
+				  struct dma_attrs *attrs)
+{
+	if (dir != DMA_TO_DEVICE)
+		mvebu_hwcc_sync_io_barrier();
+	return pfn_to_dma(dev, page_to_pfn(page)) + offset;
+}
+
+
+static void mvebu_hwcc_dma_unmap_page(struct device *dev, dma_addr_t dma_handle,
+			      size_t size, enum dma_data_direction dir,
+			      struct dma_attrs *attrs)
+{
+	if (dir != DMA_TO_DEVICE)
+		mvebu_hwcc_sync_io_barrier();
+}
+
+static void mvebu_hwcc_dma_sync(struct device *dev, dma_addr_t dma_handle,
+			size_t size, enum dma_data_direction dir)
+{
+	if (dir != DMA_TO_DEVICE)
+		mvebu_hwcc_sync_io_barrier();
+}
+
+static struct dma_map_ops mvebu_hwcc_dma_ops = {
+	.alloc			= arm_dma_alloc,
+	.free			= arm_dma_free,
+	.mmap			= arm_dma_mmap,
+	.map_page		= mvebu_hwcc_dma_map_page,
+	.unmap_page		= mvebu_hwcc_dma_unmap_page,
+	.get_sgtable		= arm_dma_get_sgtable,
+	.map_sg			= arm_dma_map_sg,
+	.unmap_sg		= arm_dma_unmap_sg,
+	.sync_single_for_cpu	= mvebu_hwcc_dma_sync,
+	.sync_single_for_device	= mvebu_hwcc_dma_sync,
+	.sync_sg_for_cpu	= arm_dma_sync_sg_for_cpu,
+	.sync_sg_for_device	= arm_dma_sync_sg_for_device,
+	.set_dma_mask		= arm_dma_set_mask,
+};
+
+static int mvebu_hwcc_platform_notifier(struct notifier_block *nb,
+				       unsigned long event, void *__dev)
+{
+	struct device *dev = __dev;
+
+	if (event != BUS_NOTIFY_ADD_DEVICE)
+		return NOTIFY_DONE;
+	set_dma_ops(dev, &mvebu_hwcc_dma_ops);
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block mvebu_hwcc_platform_nb = {
+	.notifier_call = mvebu_hwcc_platform_notifier,
+};
+
+int __init coherency_init(void)
+{
+	struct device_node *np;
+
+	np = of_find_matching_node(NULL, of_coherency_table);
+	if (np) {
+		pr_info("Initializing Coherency fabric\n");
+		coherency_base = of_iomap(np, 0);
+		coherency_cpu_base = of_iomap(np, 1);
+		set_cpu_coherent(cpu_logical_map(smp_processor_id()), 0);
+		bus_register_notifier(&platform_bus_type,
+					&mvebu_hwcc_platform_nb);
+	}
+
+	return 0;
+}
diff --git a/arch/arm/mach-mvebu/coherency.h b/arch/arm/mach-mvebu/coherency.h
new file mode 100644
index 000000000000..2f428137f6fe
--- /dev/null
+++ b/arch/arm/mach-mvebu/coherency.h
@@ -0,0 +1,24 @@
+/*
+ * arch/arm/mach-mvebu/include/mach/coherency.h
+ *
+ *
+ * Coherency fabric (Aurora) support for Armada 370 and XP platforms.
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#ifndef __MACH_370_XP_COHERENCY_H
+#define __MACH_370_XP_COHERENCY_H
+
+#ifdef CONFIG_SMP
+int coherency_get_cpu_count(void);
+#endif
+
+int set_cpu_coherent(int cpu_id, int smp_group_id);
+int coherency_init(void);
+
+#endif	/* __MACH_370_XP_COHERENCY_H */
diff --git a/arch/arm/mach-mvebu/coherency_ll.S b/arch/arm/mach-mvebu/coherency_ll.S
new file mode 100644
index 000000000000..53e8391192cd
--- /dev/null
+++ b/arch/arm/mach-mvebu/coherency_ll.S
@@ -0,0 +1,49 @@
+/*
+ * Coherency fabric: low level functions
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ *
+ * This file implements the assembly function to add a CPU to the
+ * coherency fabric. This function is called by each of the secondary
+ * CPUs during their early boot in an SMP kernel, this why this
+ * function have to callable from assembly. It can also be called by a
+ * primary CPU from C code during its boot.
+ */
+
+#include <linux/linkage.h>
+#define ARMADA_XP_CFB_CTL_REG_OFFSET 0x0
+#define ARMADA_XP_CFB_CFG_REG_OFFSET 0x4
+
+	.text
+/*
+ * r0: Coherency fabric base register address
+ * r1: HW CPU id
+ */
+ENTRY(ll_set_cpu_coherent)
+	/* Create bit by cpu index */
+	mov	r3, #(1 << 24)
+	lsl	r1, r3, r1
+
+	/* Add CPU to SMP group - Atomic */
+	add	r3, r0, #ARMADA_XP_CFB_CTL_REG_OFFSET
+	ldr	r2, [r3]
+	orr	r2, r2, r1
+	str	r2, [r3]
+
+	/* Enable coherency on CPU - Atomic */
+	add	r3, r0, #ARMADA_XP_CFB_CFG_REG_OFFSET
+	ldr	r2, [r3]
+	orr	r2, r2, r1
+	str	r2, [r3]
+
+	dsb
+
+	mov	r0, #0
+	mov	pc, lr
+ENDPROC(ll_set_cpu_coherent)
diff --git a/arch/arm/mach-mvebu/common.h b/arch/arm/mach-mvebu/common.h
index 02f89eaa25fe..aa27bc2ffb60 100644
--- a/arch/arm/mach-mvebu/common.h
+++ b/arch/arm/mach-mvebu/common.h
@@ -20,4 +20,9 @@ void mvebu_restart(char mode, const char *cmd);
 void armada_370_xp_init_irq(void);
 void armada_370_xp_handle_irq(struct pt_regs *regs);
 
+void armada_xp_cpu_die(unsigned int cpu);
+int armada_370_xp_coherency_init(void);
+int armada_370_xp_pmsu_init(void);
+void armada_xp_secondary_startup(void);
+extern struct smp_operations armada_xp_smp_ops;
 #endif
diff --git a/arch/arm/mach-mvebu/headsmp.S b/arch/arm/mach-mvebu/headsmp.S
new file mode 100644
index 000000000000..a06e0ede8c08
--- /dev/null
+++ b/arch/arm/mach-mvebu/headsmp.S
@@ -0,0 +1,49 @@
+/*
+ * SMP support: Entry point for secondary CPUs
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * Yehuda Yitschak <yehuday@marvell.com>
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ *
+ * This file implements the assembly entry point for secondary CPUs in
+ * an SMP kernel. The only thing we need to do is to add the CPU to
+ * the coherency fabric by writing to 2 registers. Currently the base
+ * register addresses are hard coded due to the early initialisation
+ * problems.
+ */
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+
+/*
+ * At this stage the secondary CPUs don't have acces yet to the MMU, so
+ * we have to provide physical addresses
+ */
+#define ARMADA_XP_CFB_BASE	     0xD0020200
+
+	__CPUINIT
+
+/*
+ * Armada XP specific entry point for secondary CPUs.
+ * We add the CPU to the coherency fabric and then jump to secondary
+ * startup
+ */
+ENTRY(armada_xp_secondary_startup)
+
+	/* Read CPU id */
+	mrc     p15, 0, r1, c0, c0, 5
+	and     r1, r1, #0xF
+
+	/* Add CPU to coherency fabric */
+	ldr     r0, =ARMADA_XP_CFB_BASE
+
+	bl	ll_set_cpu_coherent
+	b	secondary_startup
+
+ENDPROC(armada_xp_secondary_startup)
diff --git a/arch/arm/mach-mvebu/hotplug.c b/arch/arm/mach-mvebu/hotplug.c
new file mode 100644
index 000000000000..b228b6a80c85
--- /dev/null
+++ b/arch/arm/mach-mvebu/hotplug.c
@@ -0,0 +1,30 @@
+/*
+ * Symmetric Multi Processing (SMP) support for Armada XP
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * Lior Amsalem <alior@marvell.com>
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/smp.h>
+#include <asm/proc-fns.h>
+
+/*
+ * platform-specific code to shutdown a CPU
+ *
+ * Called with IRQs disabled
+ */
+void __ref armada_xp_cpu_die(unsigned int cpu)
+{
+	cpu_do_idle();
+
+	/* We should never return from idle */
+	panic("mvebu: cpu %d unexpectedly exit from shutdown\n", cpu);
+}
diff --git a/arch/arm/mach-mvebu/irq-armada-370-xp.c b/arch/arm/mach-mvebu/irq-armada-370-xp.c
index 5f5f9394b6b2..549b6846f940 100644
--- a/arch/arm/mach-mvebu/irq-armada-370-xp.c
+++ b/arch/arm/mach-mvebu/irq-armada-370-xp.c
@@ -24,6 +24,7 @@
 #include <linux/irqdomain.h>
 #include <asm/mach/arch.h>
 #include <asm/exception.h>
+#include <asm/smp_plat.h>
 
 /* Interrupt Controller Registers Map */
 #define ARMADA_370_XP_INT_SET_MASK_OFFS		(0x48)
@@ -35,6 +36,12 @@
 
 #define ARMADA_370_XP_CPU_INTACK_OFFS		(0x44)
 
+#define ARMADA_370_XP_SW_TRIG_INT_OFFS           (0x4)
+#define ARMADA_370_XP_IN_DRBEL_MSK_OFFS          (0xc)
+#define ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS        (0x8)
+
+#define ACTIVE_DOORBELLS			(8)
+
 static void __iomem *per_cpu_int_base;
 static void __iomem *main_int_base;
 static struct irq_domain *armada_370_xp_mpic_domain;
@@ -51,11 +58,22 @@ static void armada_370_xp_irq_unmask(struct irq_data *d)
 	       per_cpu_int_base + ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
 }
 
+#ifdef CONFIG_SMP
+static int armada_xp_set_affinity(struct irq_data *d,
+				  const struct cpumask *mask_val, bool force)
+{
+	return 0;
+}
+#endif
+
 static struct irq_chip armada_370_xp_irq_chip = {
 	.name		= "armada_370_xp_irq",
 	.irq_mask       = armada_370_xp_irq_mask,
 	.irq_mask_ack   = armada_370_xp_irq_mask,
 	.irq_unmask     = armada_370_xp_irq_unmask,
+#ifdef CONFIG_SMP
+	.irq_set_affinity = armada_xp_set_affinity,
+#endif
 };
 
 static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
@@ -72,6 +90,41 @@ static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
 	return 0;
 }
 
+#ifdef CONFIG_SMP
+void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq)
+{
+	int cpu;
+	unsigned long map = 0;
+
+	/* Convert our logical CPU mask into a physical one. */
+	for_each_cpu(cpu, mask)
+		map |= 1 << cpu_logical_map(cpu);
+
+	/*
+	 * Ensure that stores to Normal memory are visible to the
+	 * other CPUs before issuing the IPI.
+	 */
+	dsb();
+
+	/* submit softirq */
+	writel((map << 8) | irq, main_int_base +
+		ARMADA_370_XP_SW_TRIG_INT_OFFS);
+}
+
+void armada_xp_mpic_smp_cpu_init(void)
+{
+	/* Clear pending IPIs */
+	writel(0, per_cpu_int_base + ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS);
+
+	/* Enable first 8 IPIs */
+	writel((1 << ACTIVE_DOORBELLS) - 1, per_cpu_int_base +
+		ARMADA_370_XP_IN_DRBEL_MSK_OFFS);
+
+	/* Unmask IPI interrupt */
+	writel(0, per_cpu_int_base + ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
+}
+#endif /* CONFIG_SMP */
+
 static struct irq_domain_ops armada_370_xp_mpic_irq_ops = {
 	.map = armada_370_xp_mpic_irq_map,
 	.xlate = irq_domain_xlate_onecell,
@@ -91,13 +144,18 @@ static int __init armada_370_xp_mpic_of_init(struct device_node *node,
 	control = readl(main_int_base + ARMADA_370_XP_INT_CONTROL);
 
 	armada_370_xp_mpic_domain =
-	    irq_domain_add_linear(node, (control >> 2) & 0x3ff,
-				  &armada_370_xp_mpic_irq_ops, NULL);
+		irq_domain_add_linear(node, (control >> 2) & 0x3ff,
+				&armada_370_xp_mpic_irq_ops, NULL);
 
 	if (!armada_370_xp_mpic_domain)
 		panic("Unable to add Armada_370_Xp MPIC irq domain (DT)\n");
 
 	irq_set_default_host(armada_370_xp_mpic_domain);
+
+#ifdef CONFIG_SMP
+	armada_xp_mpic_smp_cpu_init();
+#endif
+
 	return 0;
 }
 
@@ -111,14 +169,36 @@ asmlinkage void __exception_irq_entry armada_370_xp_handle_irq(struct pt_regs
 					ARMADA_370_XP_CPU_INTACK_OFFS);
 		irqnr = irqstat & 0x3FF;
 
-		if (irqnr < 1023) {
-			irqnr =
-			    irq_find_mapping(armada_370_xp_mpic_domain, irqnr);
+		if (irqnr > 1022)
+			break;
+
+		if (irqnr >= 8) {
+			irqnr =	irq_find_mapping(armada_370_xp_mpic_domain,
+					irqnr);
 			handle_IRQ(irqnr, regs);
 			continue;
 		}
+#ifdef CONFIG_SMP
+		/* IPI Handling */
+		if (irqnr == 0) {
+			u32 ipimask, ipinr;
+
+			ipimask = readl_relaxed(per_cpu_int_base +
+						ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS)
+				& 0xFF;
+
+			writel(0x0, per_cpu_int_base +
+				ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS);
+
+			/* Handle all pending doorbells */
+			for (ipinr = 0; ipinr < ACTIVE_DOORBELLS; ipinr++) {
+				if (ipimask & (0x1 << ipinr))
+					handle_IPI(ipinr, regs);
+			}
+			continue;
+		}
+#endif
 
-		break;
 	} while (1);
 }
 
diff --git a/arch/arm/mach-mvebu/platsmp.c b/arch/arm/mach-mvebu/platsmp.c
new file mode 100644
index 000000000000..fe16aaf7c19c
--- /dev/null
+++ b/arch/arm/mach-mvebu/platsmp.c
@@ -0,0 +1,122 @@
+/*
+ * Symmetric Multi Processing (SMP) support for Armada XP
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * Lior Amsalem <alior@marvell.com>
+ * Yehuda Yitschak <yehuday@marvell.com>
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ *
+ * The Armada XP SoC has 4 ARMv7 PJ4B CPUs running in full HW coherency
+ * This file implements the routines for preparing the SMP infrastructure
+ * and waking up the secondary CPUs
+ */
+
+#include <linux/init.h>
+#include <linux/smp.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <asm/cacheflush.h>
+#include <asm/smp_plat.h>
+#include "common.h"
+#include "armada-370-xp.h"
+#include "pmsu.h"
+#include "coherency.h"
+
+void __init set_secondary_cpus_clock(void)
+{
+	int thiscpu;
+	unsigned long rate;
+	struct clk *cpu_clk = NULL;
+	struct device_node *np = NULL;
+
+	thiscpu = smp_processor_id();
+	for_each_node_by_type(np, "cpu") {
+		int err;
+		int cpu;
+
+		err = of_property_read_u32(np, "reg", &cpu);
+		if (WARN_ON(err))
+			return;
+
+		if (cpu == thiscpu) {
+			cpu_clk = of_clk_get(np, 0);
+			break;
+		}
+	}
+	if (WARN_ON(IS_ERR(cpu_clk)))
+		return;
+	clk_prepare_enable(cpu_clk);
+	rate = clk_get_rate(cpu_clk);
+
+	/* set all the other CPU clk to the same rate than the boot CPU */
+	for_each_node_by_type(np, "cpu") {
+		int err;
+		int cpu;
+
+		err = of_property_read_u32(np, "reg", &cpu);
+		if (WARN_ON(err))
+			return;
+
+		if (cpu != thiscpu) {
+			cpu_clk = of_clk_get(np, 0);
+			clk_set_rate(cpu_clk, rate);
+		}
+	}
+}
+
+static void __cpuinit armada_xp_secondary_init(unsigned int cpu)
+{
+	armada_xp_mpic_smp_cpu_init();
+}
+
+static int __cpuinit armada_xp_boot_secondary(unsigned int cpu,
+					      struct task_struct *idle)
+{
+	pr_info("Booting CPU %d\n", cpu);
+
+	armada_xp_boot_cpu(cpu, armada_xp_secondary_startup);
+
+	return 0;
+}
+
+static void __init armada_xp_smp_init_cpus(void)
+{
+	unsigned int i, ncores;
+	ncores = coherency_get_cpu_count();
+
+	/* Limit possible CPUs to defconfig */
+	if (ncores > nr_cpu_ids) {
+		pr_warn("SMP: %d CPUs physically present. Only %d configured.",
+			ncores, nr_cpu_ids);
+		pr_warn("Clipping CPU count to %d\n", nr_cpu_ids);
+		ncores = nr_cpu_ids;
+	}
+
+	for (i = 0; i < ncores; i++)
+		set_cpu_possible(i, true);
+
+	set_smp_cross_call(armada_mpic_send_doorbell);
+}
+
+void __init armada_xp_smp_prepare_cpus(unsigned int max_cpus)
+{
+	set_secondary_cpus_clock();
+	flush_cache_all();
+	set_cpu_coherent(cpu_logical_map(smp_processor_id()), 0);
+}
+
+struct smp_operations armada_xp_smp_ops __initdata = {
+	.smp_init_cpus		= armada_xp_smp_init_cpus,
+	.smp_prepare_cpus	= armada_xp_smp_prepare_cpus,
+	.smp_secondary_init	= armada_xp_secondary_init,
+	.smp_boot_secondary	= armada_xp_boot_secondary,
+#ifdef CONFIG_HOTPLUG_CPU
+	.cpu_die		= armada_xp_cpu_die,
+#endif
+};
diff --git a/arch/arm/mach-mvebu/pmsu.c b/arch/arm/mach-mvebu/pmsu.c
new file mode 100644
index 000000000000..3cc4bef6401c
--- /dev/null
+++ b/arch/arm/mach-mvebu/pmsu.c
@@ -0,0 +1,75 @@
+/*
+ * Power Management Service Unit(PMSU) support for Armada 370/XP platforms.
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * Yehuda Yitschak <yehuday@marvell.com>
+ * Gregory Clement <gregory.clement@free-electrons.com>
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ *
+ * The Armada 370 and Armada XP SOCs have a power management service
+ * unit which is responsible for powering down and waking up CPUs and
+ * other SOC units
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/of_address.h>
+#include <linux/io.h>
+#include <linux/smp.h>
+#include <asm/smp_plat.h>
+
+static void __iomem *pmsu_mp_base;
+static void __iomem *pmsu_reset_base;
+
+#define PMSU_BOOT_ADDR_REDIRECT_OFFSET(cpu)	((cpu * 0x100) + 0x24)
+#define PMSU_RESET_CTL_OFFSET(cpu)		(cpu * 0x8)
+
+static struct of_device_id of_pmsu_table[] = {
+	{.compatible = "marvell,armada-370-xp-pmsu"},
+	{ /* end of list */ },
+};
+
+#ifdef CONFIG_SMP
+int armada_xp_boot_cpu(unsigned int cpu_id, void *boot_addr)
+{
+	int reg, hw_cpu;
+
+	if (!pmsu_mp_base || !pmsu_reset_base) {
+		pr_warn("Can't boot CPU. PMSU is uninitialized\n");
+		return 1;
+	}
+
+	hw_cpu = cpu_logical_map(cpu_id);
+
+	writel(virt_to_phys(boot_addr), pmsu_mp_base +
+			PMSU_BOOT_ADDR_REDIRECT_OFFSET(hw_cpu));
+
+	/* Release CPU from reset by clearing reset bit*/
+	reg = readl(pmsu_reset_base + PMSU_RESET_CTL_OFFSET(hw_cpu));
+	reg &= (~0x1);
+	writel(reg, pmsu_reset_base + PMSU_RESET_CTL_OFFSET(hw_cpu));
+
+	return 0;
+}
+#endif
+
+int __init armada_370_xp_pmsu_init(void)
+{
+	struct device_node *np;
+
+	np = of_find_matching_node(NULL, of_pmsu_table);
+	if (np) {
+		pr_info("Initializing Power Management Service Unit\n");
+		pmsu_mp_base = of_iomap(np, 0);
+		pmsu_reset_base = of_iomap(np, 1);
+	}
+
+	return 0;
+}
+
+early_initcall(armada_370_xp_pmsu_init);
diff --git a/arch/arm/mach-mvebu/pmsu.h b/arch/arm/mach-mvebu/pmsu.h
new file mode 100644
index 000000000000..07a737c6b95d
--- /dev/null
+++ b/arch/arm/mach-mvebu/pmsu.h
@@ -0,0 +1,16 @@
+/*
+ * Power Management Service Unit (PMSU) support for Armada 370/XP platforms.
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#ifndef __MACH_MVEBU_PMSU_H
+#define __MACH_MVEBU_PMSU_H
+
+int armada_xp_boot_cpu(unsigned int cpu_id, void *phys_addr);
+
+#endif	/* __MACH_370_XP_PMSU_H */