21 files changed, 1079 insertions, 189 deletions

diff --git a/arch/arm/mach-mvebu/Kconfig b/arch/arm/mach-mvebu/Kconfig
index 3f73eecbcfb0..6090b9eb00c8 100644
--- a/arch/arm/mach-mvebu/Kconfig
+++ b/arch/arm/mach-mvebu/Kconfig
@@ -3,15 +3,13 @@ config ARCH_MVEBU
         select ARCH_SUPPORTS_BIG_ENDIAN
         select CLKSRC_MMIO
         select GENERIC_IRQ_CHIP
-        select IRQ_DOMAIN
         select PINCTRL
         select PLAT_ORION
+        select SOC_BUS
         select MVEBU_MBUS
         select ZONE_DMA if ARM_LPAE
         select ARCH_REQUIRE_GPIOLIB
-        select MIGHT_HAVE_PCI
         select PCI_QUIRKS if PCI
-        select OF_ADDRESS_PCI
 
 if ARCH_MVEBU
 
@@ -38,7 +36,9 @@ config MACH_ARMADA_375
         select ARM_ERRATA_753970
         select ARM_GIC
         select ARMADA_375_CLK
-        select CPU_V7
+        select HAVE_ARM_SCU
+        select HAVE_ARM_TWD if SMP
+        select HAVE_SMP
         select MACH_MVEBU_V7
         select PINCTRL_ARMADA_375
         help
@@ -51,7 +51,9 @@ config MACH_ARMADA_38X
         select ARM_ERRATA_753970
         select ARM_GIC
         select ARMADA_38X_CLK
-        select CPU_V7
+        select HAVE_ARM_SCU
+        select HAVE_ARM_TWD if SMP
+        select HAVE_SMP
         select MACH_MVEBU_V7
         select PINCTRL_ARMADA_38X
         help
@@ -86,24 +88,15 @@ config MACH_KIRKWOOD
         select ARCH_REQUIRE_GPIOLIB
         select CPU_FEROCEON
         select KIRKWOOD_CLK
-        select OF_IRQ
         select ORION_IRQCHIP
         select ORION_TIMER
         select PCI
         select PCI_QUIRKS
         select PINCTRL_KIRKWOOD
-        select USE_OF
         help
           Say 'Y' here if you want your kernel to support boards based
           on the Marvell Kirkwood device tree.
 
-config MACH_T5325
-        bool "HP T5325 thin client"
-        depends on MACH_KIRKWOOD
-        help
-          Say 'Y' here if you want your kernel to support the
-          HP T5325 Thin client
-
 endmenu
 
 endif
diff --git a/arch/arm/mach-mvebu/Makefile b/arch/arm/mach-mvebu/Makefile
index a63e43b6b451..2ecb828e4a8b 100644
--- a/arch/arm/mach-mvebu/Makefile
+++ b/arch/arm/mach-mvebu/Makefile
@@ -2,12 +2,15 @@ ccflags-$(CONFIG_ARCH_MULTIPLATFORM) := -I$(srctree)/$(src)/include \
         -I$(srctree)/arch/arm/plat-orion/include
 
 AFLAGS_coherency_ll.o           := -Wa,-march=armv7-a
+CFLAGS_pmsu.o                   := -march=armv7-a
 
 obj-y                            += system-controller.o mvebu-soc-id.o
-obj-$(CONFIG_MACH_MVEBU_V7)      += board-v7.o
+
+ifeq ($(CONFIG_MACH_MVEBU_V7),y)
+obj-y                            += cpu-reset.o board-v7.o coherency.o coherency_ll.o pmsu.o
+obj-$(CONFIG_SMP)                += platsmp.o headsmp.o platsmp-a9.o headsmp-a9.o
+obj-$(CONFIG_HOTPLUG_CPU)        += hotplug.o
+endif
+
 obj-$(CONFIG_MACH_DOVE)          += dove.o
-obj-$(CONFIG_ARCH_MVEBU)         += coherency.o coherency_ll.o pmsu.o
-obj-$(CONFIG_SMP)                += platsmp.o headsmp.o
-obj-$(CONFIG_HOTPLUG_CPU)        += hotplug.o
 obj-$(CONFIG_MACH_KIRKWOOD)      += kirkwood.o kirkwood-pm.o
-obj-$(CONFIG_MACH_T5325)         += board-t5325.o
diff --git a/arch/arm/mach-mvebu/armada-370-xp.h b/arch/arm/mach-mvebu/armada-370-xp.h
index 237c86b83390..c3465f5b1250 100644
--- a/arch/arm/mach-mvebu/armada-370-xp.h
+++ b/arch/arm/mach-mvebu/armada-370-xp.h
@@ -20,8 +20,6 @@
 
 #define ARMADA_XP_MAX_CPUS 4
 
-void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq);
-void armada_xp_mpic_smp_cpu_init(void);
 void armada_xp_secondary_startup(void);
 extern struct smp_operations armada_xp_smp_ops;
 #endif
diff --git a/arch/arm/mach-mvebu/board-t5325.c b/arch/arm/mach-mvebu/board-t5325.c
deleted file mode 100644
index 65ace6db9f28..000000000000
--- a/arch/arm/mach-mvebu/board-t5325.c
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * HP T5325 Board Setup
- *
- * Copyright (C) 2014
- *
- * Andrew Lunn <andrew@lunn.ch>
- *
- * 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/i2c.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <sound/alc5623.h>
-#include "board.h"
-
-static struct platform_device hp_t5325_audio_device = {
-        .name           = "t5325-audio",
-        .id             = -1,
-};
-
-static struct alc5623_platform_data alc5621_data = {
-        .add_ctrl = 0x3700,
-        .jack_det_ctrl = 0x4810,
-};
-
-static struct i2c_board_info i2c_board_info[] __initdata = {
-        {
-                I2C_BOARD_INFO("alc5621", 0x1a),
-                .platform_data = &alc5621_data,
-        },
-};
-
-void __init t5325_init(void)
-{
-        i2c_register_board_info(0, i2c_board_info, ARRAY_SIZE(i2c_board_info));
-        platform_device_register(&hp_t5325_audio_device);
-}
diff --git a/arch/arm/mach-mvebu/board-v7.c b/arch/arm/mach-mvebu/board-v7.c
index 333fca8fdc41..594262b27f56 100644
--- a/arch/arm/mach-mvebu/board-v7.c
+++ b/arch/arm/mach-mvebu/board-v7.c
@@ -27,12 +27,30 @@
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
 #include <asm/mach/time.h>
+#include <asm/smp_scu.h>
 #include "armada-370-xp.h"
 #include "common.h"
 #include "coherency.h"
 #include "mvebu-soc-id.h"
 
 /*
+ * Enables the SCU when available. Obviously, this is only useful on
+ * Cortex-A based SOCs, not on PJ4B based ones.
+ */
+static void __init mvebu_scu_enable(void)
+{
+        void __iomem *scu_base;
+
+        struct device_node *np =
+                of_find_compatible_node(NULL, NULL, "arm,cortex-a9-scu");
+        if (np) {
+                scu_base = of_iomap(np, 0);
+                scu_enable(scu_base);
+                of_node_put(np);
+        }
+}
+
+/*
  * Early versions of Armada 375 SoC have a bug where the BootROM
  * leaves an external data abort pending. The kernel is hit by this
  * data abort as soon as it enters userspace, because it unmasks the
@@ -57,11 +75,10 @@ static void __init mvebu_timer_and_clk_init(void)
 {
         of_clk_init(NULL);
         clocksource_of_init();
+        mvebu_scu_enable();
         coherency_init();
-        BUG_ON(mvebu_mbus_dt_init());
-#ifdef CONFIG_CACHE_L2X0
+        BUG_ON(mvebu_mbus_dt_init(coherency_available()));
         l2x0_of_init(0, ~0UL);
-#endif
 
         if (of_machine_is_compatible("marvell,armada375"))
                 hook_fault_code(16 + 6, armada_375_external_abort_wa, SIGBUS, 0,
@@ -78,7 +95,7 @@ static void __init i2c_quirk(void)
          * mechanism. We can exit only if we are sure that we can
          * get the SoC revision and it is more recent than A0.
          */
-        if (mvebu_get_soc_id(&rev, &dev) == 0 && dev > MV78XX0_A0_REV)
+        if (mvebu_get_soc_id(&dev, &rev) == 0 && rev > MV78XX0_A0_REV)
                 return;
 
         for_each_compatible_node(np, NULL, "marvell,mv78230-i2c") {
@@ -96,10 +113,66 @@ static void __init i2c_quirk(void)
         return;
 }
 
+#define A375_Z1_THERMAL_FIXUP_OFFSET 0xc
+
+static void __init thermal_quirk(void)
+{
+        struct device_node *np;
+        u32 dev, rev;
+
+        if (mvebu_get_soc_id(&dev, &rev) == 0 && rev > ARMADA_375_Z1_REV)
+                return;
+
+        for_each_compatible_node(np, NULL, "marvell,armada375-thermal") {
+                struct property *prop;
+                __be32 newval, *newprop, *oldprop;
+                int len;
+
+                /*
+                 * The register offset is at a wrong location. This quirk
+                 * creates a new reg property as a clone of the previous
+                 * one and corrects the offset.
+                 */
+                oldprop = (__be32 *)of_get_property(np, "reg", &len);
+                if (!oldprop)
+                        continue;
+
+                /* Create a duplicate of the 'reg' property */
+                prop = kzalloc(sizeof(*prop), GFP_KERNEL);
+                prop->length = len;
+                prop->name = kstrdup("reg", GFP_KERNEL);
+                prop->value = kzalloc(len, GFP_KERNEL);
+                memcpy(prop->value, oldprop, len);
+
+                /* Fixup the register offset of the second entry */
+                oldprop += 2;
+                newprop = (__be32 *)prop->value + 2;
+                newval = cpu_to_be32(be32_to_cpu(*oldprop) -
+                                     A375_Z1_THERMAL_FIXUP_OFFSET);
+                *newprop = newval;
+                of_update_property(np, prop);
+
+                /*
+                 * The thermal controller needs some quirk too, so let's change
+                 * the compatible string to reflect this.
+                 */
+                prop = kzalloc(sizeof(*prop), GFP_KERNEL);
+                prop->name = kstrdup("compatible", GFP_KERNEL);
+                prop->length = sizeof("marvell,armada375-z1-thermal");
+                prop->value = kstrdup("marvell,armada375-z1-thermal",
+                                                GFP_KERNEL);
+                of_update_property(np, prop);
+        }
+        return;
+}
+
 static void __init mvebu_dt_init(void)
 {
         if (of_machine_is_compatible("plathome,openblocks-ax3-4"))
                 i2c_quirk();
+        if (of_machine_is_compatible("marvell,a375-db"))
+                thermal_quirk();
+
         of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 }
 
@@ -109,6 +182,8 @@ static const char * const armada_370_xp_dt_compat[] = {
 };
 
 DT_MACHINE_START(ARMADA_370_XP_DT, "Marvell Armada 370/XP (Device Tree)")
+        .l2c_aux_val    = 0,
+        .l2c_aux_mask   = ~0,
         .smp            = smp_ops(armada_xp_smp_ops),
         .init_machine   = mvebu_dt_init,
         .init_time      = mvebu_timer_and_clk_init,
@@ -122,7 +197,10 @@ static const char * const armada_375_dt_compat[] = {
 };
 
 DT_MACHINE_START(ARMADA_375_DT, "Marvell Armada 375 (Device Tree)")
+        .l2c_aux_val    = 0,
+        .l2c_aux_mask   = ~0,
         .init_time      = mvebu_timer_and_clk_init,
+        .init_machine   = mvebu_dt_init,
         .restart        = mvebu_restart,
         .dt_compat      = armada_375_dt_compat,
 MACHINE_END
@@ -134,6 +212,8 @@ static const char * const armada_38x_dt_compat[] = {
 };
 
 DT_MACHINE_START(ARMADA_38X_DT, "Marvell Armada 380/385 (Device Tree)")
+        .l2c_aux_val    = 0,
+        .l2c_aux_mask   = ~0,
         .init_time      = mvebu_timer_and_clk_init,
         .restart        = mvebu_restart,
         .dt_compat      = armada_38x_dt_compat,
diff --git a/arch/arm/mach-mvebu/board.h b/arch/arm/mach-mvebu/board.h
index de7f0a191394..9c7bb4386f8b 100644
--- a/arch/arm/mach-mvebu/board.h
+++ b/arch/arm/mach-mvebu/board.h
@@ -13,10 +13,4 @@
 #ifndef __ARCH_MVEBU_BOARD_H
 #define __ARCH_MVEBU_BOARD_H
 
-#ifdef CONFIG_MACH_T5325
-void t5325_init(void);
-#else
-static inline void t5325_init(void) {};
-#endif
-
 #endif
diff --git a/arch/arm/mach-mvebu/coherency.c b/arch/arm/mach-mvebu/coherency.c
index 4e9d58148ca7..477202fd39cc 100644
--- a/arch/arm/mach-mvebu/coherency.c
+++ b/arch/arm/mach-mvebu/coherency.c
@@ -17,6 +17,8 @@
  * supplies basic routines for configuring and controlling hardware coherency
  */
 
+#define pr_fmt(fmt) "mvebu-coherency: " fmt
+
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/of_address.h>
@@ -24,13 +26,19 @@
 #include <linux/smp.h>
 #include <linux/dma-mapping.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/mbus.h>
+#include <linux/clk.h>
+#include <linux/pci.h>
 #include <asm/smp_plat.h>
 #include <asm/cacheflush.h>
+#include <asm/mach/map.h>
 #include "armada-370-xp.h"
 #include "coherency.h"
+#include "mvebu-soc-id.h"
 
 unsigned long coherency_phys_base;
-static void __iomem *coherency_base;
+void __iomem *coherency_base;
 static void __iomem *coherency_cpu_base;
 
 /* Coherency fabric registers */
@@ -38,27 +46,190 @@ static void __iomem *coherency_cpu_base;
 
 #define IO_SYNC_BARRIER_CTL_OFFSET                 0x0
 
+enum {
+        COHERENCY_FABRIC_TYPE_NONE,
+        COHERENCY_FABRIC_TYPE_ARMADA_370_XP,
+        COHERENCY_FABRIC_TYPE_ARMADA_375,
+        COHERENCY_FABRIC_TYPE_ARMADA_380,
+};
+
 static struct of_device_id of_coherency_table[] = {
-        {.compatible = "marvell,coherency-fabric"},
+        {.compatible = "marvell,coherency-fabric",
+         .data = (void *) COHERENCY_FABRIC_TYPE_ARMADA_370_XP },
+        {.compatible = "marvell,armada-375-coherency-fabric",
+         .data = (void *) COHERENCY_FABRIC_TYPE_ARMADA_375 },
+        {.compatible = "marvell,armada-380-coherency-fabric",
+         .data = (void *) COHERENCY_FABRIC_TYPE_ARMADA_380 },
         { /* end of list */ },
 };
 
-/* Function defined in coherency_ll.S */
-int ll_set_cpu_coherent(void __iomem *base_addr, unsigned int hw_cpu_id);
+/* Functions defined in coherency_ll.S */
+int ll_enable_coherency(void);
+void ll_add_cpu_to_smp_group(void);
 
-int set_cpu_coherent(unsigned int hw_cpu_id, int smp_group_id)
+int set_cpu_coherent(void)
 {
         if (!coherency_base) {
-                pr_warn("Can't make CPU %d cache coherent.\n", hw_cpu_id);
+                pr_warn("Can't make current CPU cache coherent.\n");
                 pr_warn("Coherency fabric is not initialized\n");
                 return 1;
         }
 
-        return ll_set_cpu_coherent(coherency_base, hw_cpu_id);
+        ll_add_cpu_to_smp_group();
+        return ll_enable_coherency();
+}
+
+/*
+ * The below code implements the I/O coherency workaround on Armada
+ * 375. This workaround consists in using the two channels of the
+ * first XOR engine to trigger a XOR transaction that serves as the
+ * I/O coherency barrier.
+ */
+
+static void __iomem *xor_base, *xor_high_base;
+static dma_addr_t coherency_wa_buf_phys[CONFIG_NR_CPUS];
+static void *coherency_wa_buf[CONFIG_NR_CPUS];
+static bool coherency_wa_enabled;
+
+#define XOR_CONFIG(chan)            (0x10 + (chan * 4))
+#define XOR_ACTIVATION(chan)        (0x20 + (chan * 4))
+#define WINDOW_BAR_ENABLE(chan)     (0x240 + ((chan) << 2))
+#define WINDOW_BASE(w)              (0x250 + ((w) << 2))
+#define WINDOW_SIZE(w)              (0x270 + ((w) << 2))
+#define WINDOW_REMAP_HIGH(w)        (0x290 + ((w) << 2))
+#define WINDOW_OVERRIDE_CTRL(chan)  (0x2A0 + ((chan) << 2))
+#define XOR_DEST_POINTER(chan)      (0x2B0 + (chan * 4))
+#define XOR_BLOCK_SIZE(chan)        (0x2C0 + (chan * 4))
+#define XOR_INIT_VALUE_LOW           0x2E0
+#define XOR_INIT_VALUE_HIGH          0x2E4
+
+static inline void mvebu_hwcc_armada375_sync_io_barrier_wa(void)
+{
+        int idx = smp_processor_id();
+
+        /* Write '1' to the first word of the buffer */
+        writel(0x1, coherency_wa_buf[idx]);
+
+        /* Wait until the engine is idle */
+        while ((readl(xor_base + XOR_ACTIVATION(idx)) >> 4) & 0x3)
+                ;
+
+        dmb();
+
+        /* Trigger channel */
+        writel(0x1, xor_base + XOR_ACTIVATION(idx));
+
+        /* Poll the data until it is cleared by the XOR transaction */
+        while (readl(coherency_wa_buf[idx]))
+                ;
+}
+
+static void __init armada_375_coherency_init_wa(void)
+{
+        const struct mbus_dram_target_info *dram;
+        struct device_node *xor_node;
+        struct property *xor_status;
+        struct clk *xor_clk;
+        u32 win_enable = 0;
+        int i;
+
+        pr_warn("enabling coherency workaround for Armada 375 Z1, one XOR engine disabled\n");
+
+        /*
+         * Since the workaround uses one XOR engine, we grab a
+         * reference to its Device Tree node first.
+         */
+        xor_node = of_find_compatible_node(NULL, NULL, "marvell,orion-xor");
+        BUG_ON(!xor_node);
+
+        /*
+         * Then we mark it as disabled so that the real XOR driver
+         * will not use it.
+         */
+        xor_status = kzalloc(sizeof(struct property), GFP_KERNEL);
+        BUG_ON(!xor_status);
+
+        xor_status->value = kstrdup("disabled", GFP_KERNEL);
+        BUG_ON(!xor_status->value);
+
+        xor_status->length = 8;
+        xor_status->name = kstrdup("status", GFP_KERNEL);
+        BUG_ON(!xor_status->name);
+
+        of_update_property(xor_node, xor_status);
+
+        /*
+         * And we remap the registers, get the clock, and do the
+         * initial configuration of the XOR engine.
+         */
+        xor_base = of_iomap(xor_node, 0);
+        xor_high_base = of_iomap(xor_node, 1);
+
+        xor_clk = of_clk_get_by_name(xor_node, NULL);
+        BUG_ON(!xor_clk);
+
+        clk_prepare_enable(xor_clk);
+
+        dram = mv_mbus_dram_info();
+
+        for (i = 0; i < 8; i++) {
+                writel(0, xor_base + WINDOW_BASE(i));
+                writel(0, xor_base + WINDOW_SIZE(i));
+                if (i < 4)
+                        writel(0, xor_base + WINDOW_REMAP_HIGH(i));
+        }
+
+        for (i = 0; i < dram->num_cs; i++) {
+                const struct mbus_dram_window *cs = dram->cs + i;
+                writel((cs->base & 0xffff0000) |
+                       (cs->mbus_attr << 8) |
+                       dram->mbus_dram_target_id, xor_base + WINDOW_BASE(i));
+                writel((cs->size - 1) & 0xffff0000, xor_base + WINDOW_SIZE(i));
+
+                win_enable |= (1 << i);
+                win_enable |= 3 << (16 + (2 * i));
+        }
+
+        writel(win_enable, xor_base + WINDOW_BAR_ENABLE(0));
+        writel(win_enable, xor_base + WINDOW_BAR_ENABLE(1));
+        writel(0, xor_base + WINDOW_OVERRIDE_CTRL(0));
+        writel(0, xor_base + WINDOW_OVERRIDE_CTRL(1));
+
+        for (i = 0; i < CONFIG_NR_CPUS; i++) {
+                coherency_wa_buf[i] = kzalloc(PAGE_SIZE, GFP_KERNEL);
+                BUG_ON(!coherency_wa_buf[i]);
+
+                /*
+                 * We can't use the DMA mapping API, since we don't
+                 * have a valid 'struct device' pointer
+                 */
+                coherency_wa_buf_phys[i] =
+                        virt_to_phys(coherency_wa_buf[i]);
+                BUG_ON(!coherency_wa_buf_phys[i]);
+
+                /*
+                 * Configure the XOR engine for memset operation, with
+                 * a 128 bytes block size
+                 */
+                writel(0x444, xor_base + XOR_CONFIG(i));
+                writel(128, xor_base + XOR_BLOCK_SIZE(i));
+                writel(coherency_wa_buf_phys[i],
+                       xor_base + XOR_DEST_POINTER(i));
+        }
+
+        writel(0x0, xor_base + XOR_INIT_VALUE_LOW);
+        writel(0x0, xor_base + XOR_INIT_VALUE_HIGH);
+
+        coherency_wa_enabled = true;
 }
 
 static inline void mvebu_hwcc_sync_io_barrier(void)
 {
+        if (coherency_wa_enabled) {
+                mvebu_hwcc_armada375_sync_io_barrier_wa();
+                return;
+        }
+
         writel(0x1, coherency_cpu_base + IO_SYNC_BARRIER_CTL_OFFSET);
         while (readl(coherency_cpu_base + IO_SYNC_BARRIER_CTL_OFFSET) & 0x1);
 }
@@ -105,8 +276,8 @@ static struct dma_map_ops mvebu_hwcc_dma_ops = {
         .set_dma_mask           = arm_dma_set_mask,
 };
 
-static int mvebu_hwcc_platform_notifier(struct notifier_block *nb,
-                                       unsigned long event, void *__dev)
+static int mvebu_hwcc_notifier(struct notifier_block *nb,
+                               unsigned long event, void *__dev)
 {
         struct device *dev = __dev;
 
@@ -117,47 +288,148 @@ static int mvebu_hwcc_platform_notifier(struct notifier_block *nb,
         return NOTIFY_OK;
 }
 
-static struct notifier_block mvebu_hwcc_platform_nb = {
-        .notifier_call = mvebu_hwcc_platform_notifier,
+static struct notifier_block mvebu_hwcc_nb = {
+        .notifier_call = mvebu_hwcc_notifier,
 };
 
-int __init coherency_init(void)
+static void __init armada_370_coherency_init(struct device_node *np)
+{
+        struct resource res;
+
+        of_address_to_resource(np, 0, &res);
+        coherency_phys_base = res.start;
+        /*
+         * Ensure secondary CPUs will see the updated value,
+         * which they read before they join the coherency
+         * fabric, and therefore before they are coherent with
+         * the boot CPU cache.
+         */
+        sync_cache_w(&coherency_phys_base);
+        coherency_base = of_iomap(np, 0);
+        coherency_cpu_base = of_iomap(np, 1);
+        set_cpu_coherent();
+}
+
+/*
+ * This ioremap hook is used on Armada 375/38x to ensure that PCIe
+ * memory areas are mapped as MT_UNCACHED instead of MT_DEVICE. This
+ * is needed as a workaround for a deadlock issue between the PCIe
+ * interface and the cache controller.
+ */
+static void __iomem *
+armada_pcie_wa_ioremap_caller(phys_addr_t phys_addr, size_t size,
+                              unsigned int mtype, void *caller)
+{
+        struct resource pcie_mem;
+
+        mvebu_mbus_get_pcie_mem_aperture(&pcie_mem);
+
+        if (pcie_mem.start <= phys_addr && (phys_addr + size) <= pcie_mem.end)
+                mtype = MT_UNCACHED;
+
+        return __arm_ioremap_caller(phys_addr, size, mtype, caller);
+}
+
+static void __init armada_375_380_coherency_init(struct device_node *np)
+{
+        struct device_node *cache_dn;
+
+        coherency_cpu_base = of_iomap(np, 0);
+        arch_ioremap_caller = armada_pcie_wa_ioremap_caller;
+
+        /*
+         * Add the PL310 property "arm,io-coherent". This makes sure the
+         * outer sync operation is not used, which allows to
+         * workaround the system erratum that causes deadlocks when
+         * doing PCIe in an SMP situation on Armada 375 and Armada
+         * 38x.
+         */
+        for_each_compatible_node(cache_dn, NULL, "arm,pl310-cache") {
+                struct property *p;
+
+                p = kzalloc(sizeof(*p), GFP_KERNEL);
+                p->name = kstrdup("arm,io-coherent", GFP_KERNEL);
+                of_add_property(cache_dn, p);
+        }
+}
+
+static int coherency_type(void)
 {
         struct device_node *np;
+        const struct of_device_id *match;
 
-        np = of_find_matching_node(NULL, of_coherency_table);
+        np = of_find_matching_node_and_match(NULL, of_coherency_table, &match);
         if (np) {
-                struct resource res;
-                pr_info("Initializing Coherency fabric\n");
-                of_address_to_resource(np, 0, &res);
-                coherency_phys_base = res.start;
-                /*
-                 * Ensure secondary CPUs will see the updated value,
-                 * which they read before they join the coherency
-                 * fabric, and therefore before they are coherent with
-                 * the boot CPU cache.
-                 */
-                sync_cache_w(&coherency_phys_base);
-                coherency_base = of_iomap(np, 0);
-                coherency_cpu_base = of_iomap(np, 1);
-                set_cpu_coherent(cpu_logical_map(smp_processor_id()), 0);
-                of_node_put(np);
+                int type = (int) match->data;
+
+                /* Armada 370/XP coherency works in both UP and SMP */
+                if (type == COHERENCY_FABRIC_TYPE_ARMADA_370_XP)
+                        return type;
+
+                /* Armada 375 coherency works only on SMP */
+                else if (type == COHERENCY_FABRIC_TYPE_ARMADA_375 && is_smp())
+                        return type;
+
+                /* Armada 380 coherency works only on SMP */
+                else if (type == COHERENCY_FABRIC_TYPE_ARMADA_380 && is_smp())
+                        return type;
         }
 
-        return 0;
+        return COHERENCY_FABRIC_TYPE_NONE;
 }
 
-static int __init coherency_late_init(void)
+int coherency_available(void)
+{
+        return coherency_type() != COHERENCY_FABRIC_TYPE_NONE;
+}
+
+int __init coherency_init(void)
 {
+        int type = coherency_type();
         struct device_node *np;
 
         np = of_find_matching_node(NULL, of_coherency_table);
-        if (np) {
-                bus_register_notifier(&platform_bus_type,
-                                      &mvebu_hwcc_platform_nb);
-                of_node_put(np);
+
+        if (type == COHERENCY_FABRIC_TYPE_ARMADA_370_XP)
+                armada_370_coherency_init(np);
+        else if (type == COHERENCY_FABRIC_TYPE_ARMADA_375 ||
+                 type == COHERENCY_FABRIC_TYPE_ARMADA_380)
+                armada_375_380_coherency_init(np);
+
+        return 0;
+}
+
+static int __init coherency_late_init(void)
+{
+        int type = coherency_type();
+
+        if (type == COHERENCY_FABRIC_TYPE_NONE)
+                return 0;
+
+        if (type == COHERENCY_FABRIC_TYPE_ARMADA_375) {
+                u32 dev, rev;
+
+                if (mvebu_get_soc_id(&dev, &rev) == 0 &&
+                    rev == ARMADA_375_Z1_REV)
+                        armada_375_coherency_init_wa();
         }
+
+        bus_register_notifier(&platform_bus_type,
+                              &mvebu_hwcc_nb);
+
         return 0;
 }
 
 postcore_initcall(coherency_late_init);
+
+#if IS_ENABLED(CONFIG_PCI)
+static int __init coherency_pci_init(void)
+{
+        if (coherency_available())
+                bus_register_notifier(&pci_bus_type,
+                                       &mvebu_hwcc_nb);
+        return 0;
+}
+
+arch_initcall(coherency_pci_init);
+#endif
diff --git a/arch/arm/mach-mvebu/coherency.h b/arch/arm/mach-mvebu/coherency.h
index 760226c41353..54cb7607b526 100644
--- a/arch/arm/mach-mvebu/coherency.h
+++ b/arch/arm/mach-mvebu/coherency.h
@@ -15,8 +15,9 @@
 #define __MACH_370_XP_COHERENCY_H
 
 extern unsigned long coherency_phys_base;
+int set_cpu_coherent(void);
 
-int set_cpu_coherent(unsigned int cpu_id, int smp_group_id);
 int coherency_init(void);
+int coherency_available(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
index ee7598fe75db..510c29e079ca 100644
--- a/arch/arm/mach-mvebu/coherency_ll.S
+++ b/arch/arm/mach-mvebu/coherency_ll.S
@@ -21,38 +21,129 @@
 #define ARMADA_XP_CFB_CFG_REG_OFFSET 0x4
 
 #include <asm/assembler.h>
+#include <asm/cp15.h>
 
         .text
+/* Returns the coherency base address in r1 (r0 is untouched) */
+ENTRY(ll_get_coherency_base)
+        mrc     p15, 0, r1, c1, c0, 0
+        tst     r1, #CR_M @ Check MMU bit enabled
+        bne     1f
+
+        /*
+         * MMU is disabled, use the physical address of the coherency
+         * base address.
+         */
+        adr     r1, 3f
+        ldr     r3, [r1]
+        ldr     r1, [r1, r3]
+        b       2f
+1:
+        /*
+         * MMU is enabled, use the virtual address of the coherency
+         * base address.
+         */
+        ldr     r1, =coherency_base
+        ldr     r1, [r1]
+2:
+        mov     pc, lr
+ENDPROC(ll_get_coherency_base)
+
 /*
- * r0: Coherency fabric base register address
- * r1: HW CPU id
+ * Returns the coherency CPU mask in r3 (r0 is untouched). This
+ * coherency CPU mask can be used with the coherency fabric
+ * configuration and control registers. Note that the mask is already
+ * endian-swapped as appropriate so that the calling functions do not
+ * have to care about endianness issues while accessing the coherency
+ * fabric registers
  */
-ENTRY(ll_set_cpu_coherent)
-        /* Create bit by cpu index */
-        mov     r3, #(1 << 24)
-        lsl     r1, r3, r1
-ARM_BE8(rev     r1, r1)
-
-        /* Add CPU to SMP group - Atomic */
-        add     r3, r0, #ARMADA_XP_CFB_CTL_REG_OFFSET
-1:
-        ldrex   r2, [r3]
-        orr     r2, r2, r1
-        strex   r0, r2, [r3]
-        cmp     r0, #0
-        bne 1b
-
-        /* Enable coherency on CPU - Atomic */
-        add     r3, r3, #ARMADA_XP_CFB_CFG_REG_OFFSET
+ENTRY(ll_get_coherency_cpumask)
+        mrc     15, 0, r3, cr0, cr0, 5
+        and     r3, r3, #15
+        mov     r2, #(1 << 24)
+        lsl     r3, r2, r3
+ARM_BE8(rev     r3, r3)
+        mov     pc, lr
+ENDPROC(ll_get_coherency_cpumask)
+
+/*
+ * ll_add_cpu_to_smp_group(), ll_enable_coherency() and
+ * ll_disable_coherency() use the strex/ldrex instructions while the
+ * MMU can be disabled. The Armada XP SoC has an exclusive monitor
+ * that tracks transactions to Device and/or SO memory and thanks to
+ * that, exclusive transactions are functional even when the MMU is
+ * disabled.
+ */
+
+ENTRY(ll_add_cpu_to_smp_group)
+        /*
+         * As r0 is not modified by ll_get_coherency_base() and
+         * ll_get_coherency_cpumask(), we use it to temporarly save lr
+         * and avoid it being modified by the branch and link
+         * calls. This function is used very early in the secondary
+         * CPU boot, and no stack is available at this point.
+         */
+        mov     r0, lr
+        bl      ll_get_coherency_base
+        bl      ll_get_coherency_cpumask
+        mov     lr, r0
+        add     r0, r1, #ARMADA_XP_CFB_CFG_REG_OFFSET
 1:
-        ldrex   r2, [r3]
-        orr     r2, r2, r1
-        strex   r0, r2, [r3]
-        cmp     r0, #0
-        bne 1b
+        ldrex   r2, [r0]
+        orr     r2, r2, r3
+        strex   r1, r2, [r0]
+        cmp     r1, #0
+        bne     1b
+        mov     pc, lr
+ENDPROC(ll_add_cpu_to_smp_group)
 
+ENTRY(ll_enable_coherency)
+        /*
+         * As r0 is not modified by ll_get_coherency_base() and
+         * ll_get_coherency_cpumask(), we use it to temporarly save lr
+         * and avoid it being modified by the branch and link
+         * calls. This function is used very early in the secondary
+         * CPU boot, and no stack is available at this point.
+         */
+        mov r0, lr
+        bl      ll_get_coherency_base
+        bl      ll_get_coherency_cpumask
+        mov lr, r0
+        add     r0, r1, #ARMADA_XP_CFB_CTL_REG_OFFSET
+1:
+        ldrex   r2, [r0]
+        orr     r2, r2, r3
+        strex   r1, r2, [r0]
+        cmp     r1, #0
+        bne     1b
         dsb
-
         mov     r0, #0
         mov     pc, lr
-ENDPROC(ll_set_cpu_coherent)
+ENDPROC(ll_enable_coherency)
+
+ENTRY(ll_disable_coherency)
+        /*
+         * As r0 is not modified by ll_get_coherency_base() and
+         * ll_get_coherency_cpumask(), we use it to temporarly save lr
+         * and avoid it being modified by the branch and link
+         * calls. This function is used very early in the secondary
+         * CPU boot, and no stack is available at this point.
+         */
+        mov     r0, lr
+        bl      ll_get_coherency_base
+        bl      ll_get_coherency_cpumask
+        mov     lr, r0
+        add     r0, r1, #ARMADA_XP_CFB_CTL_REG_OFFSET
+1:
+        ldrex   r2, [r0]
+        bic     r2, r2, r3
+        strex   r1, r2, [r0]
+        cmp     r1, #0
+        bne     1b
+        dsb
+        mov     pc, lr
+ENDPROC(ll_disable_coherency)
+
+        .align 2
+3:
+        .long   coherency_phys_base - .
diff --git a/arch/arm/mach-mvebu/common.h b/arch/arm/mach-mvebu/common.h
index 55449c487c9e..b67fb7a10d8b 100644
--- a/arch/arm/mach-mvebu/common.h
+++ b/arch/arm/mach-mvebu/common.h
@@ -18,6 +18,9 @@
 #include <linux/reboot.h>
 
 void mvebu_restart(enum reboot_mode mode, const char *cmd);
+int mvebu_cpu_reset_deassert(int cpu);
+void mvebu_pmsu_set_cpu_boot_addr(int hw_cpu, void *boot_addr);
+void mvebu_system_controller_set_cpu_boot_addr(void *boot_addr);
 
 void armada_xp_cpu_die(unsigned int cpu);
 
diff --git a/arch/arm/mach-mvebu/cpu-reset.c b/arch/arm/mach-mvebu/cpu-reset.c
new file mode 100644
index 000000000000..4a8f9eebebea
--- /dev/null
+++ b/arch/arm/mach-mvebu/cpu-reset.c
@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 2014 Marvell
+ *
+ * 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.
+ */
+
+#define pr_fmt(fmt) "mvebu-cpureset: " fmt
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/of_address.h>
+#include <linux/io.h>
+#include <linux/resource.h>
+#include "armada-370-xp.h"
+
+static void __iomem *cpu_reset_base;
+static size_t cpu_reset_size;
+
+#define CPU_RESET_OFFSET(cpu) (cpu * 0x8)
+#define CPU_RESET_ASSERT      BIT(0)
+
+int mvebu_cpu_reset_deassert(int cpu)
+{
+        u32 reg;
+
+        if (!cpu_reset_base)
+                return -ENODEV;
+
+        if (CPU_RESET_OFFSET(cpu) >= cpu_reset_size)
+                return -EINVAL;
+
+        reg = readl(cpu_reset_base + CPU_RESET_OFFSET(cpu));
+        reg &= ~CPU_RESET_ASSERT;
+        writel(reg, cpu_reset_base + CPU_RESET_OFFSET(cpu));
+
+        return 0;
+}
+
+static int mvebu_cpu_reset_map(struct device_node *np, int res_idx)
+{
+        struct resource res;
+
+        if (of_address_to_resource(np, res_idx, &res)) {
+                pr_err("unable to get resource\n");
+                return -ENOENT;
+        }
+
+        if (!request_mem_region(res.start, resource_size(&res),
+                                np->full_name)) {
+                pr_err("unable to request region\n");
+                return -EBUSY;
+        }
+
+        cpu_reset_base = ioremap(res.start, resource_size(&res));
+        if (!cpu_reset_base) {
+                pr_err("unable to map registers\n");
+                release_mem_region(res.start, resource_size(&res));
+                return -ENOMEM;
+        }
+
+        cpu_reset_size = resource_size(&res);
+
+        return 0;
+}
+
+int __init mvebu_cpu_reset_init(void)
+{
+        struct device_node *np;
+        int res_idx;
+        int ret;
+
+        np = of_find_compatible_node(NULL, NULL,
+                                     "marvell,armada-370-cpu-reset");
+        if (np) {
+                res_idx = 0;
+        } else {
+                /*
+                 * This code is kept for backward compatibility with
+                 * old Device Trees.
+                 */
+                np = of_find_compatible_node(NULL, NULL,
+                                             "marvell,armada-370-xp-pmsu");
+                if (np) {
+                        pr_warn(FW_WARN "deprecated pmsu binding\n");
+                        res_idx = 1;
+                }
+        }
+
+        /* No reset node found */
+        if (!np)
+                return -ENODEV;
+
+        ret = mvebu_cpu_reset_map(np, res_idx);
+        of_node_put(np);
+
+        return ret;
+}
+
+early_initcall(mvebu_cpu_reset_init);
diff --git a/arch/arm/mach-mvebu/dove.c b/arch/arm/mach-mvebu/dove.c
index 5e5a43624237..b50464ec1130 100644
--- a/arch/arm/mach-mvebu/dove.c
+++ b/arch/arm/mach-mvebu/dove.c
@@ -23,7 +23,7 @@ static void __init dove_init(void)
 #ifdef CONFIG_CACHE_TAUROS2
         tauros2_init(0);
 #endif
-        BUG_ON(mvebu_mbus_dt_init());
+        BUG_ON(mvebu_mbus_dt_init(false));
         of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 }
 
diff --git a/arch/arm/mach-mvebu/headsmp-a9.S b/arch/arm/mach-mvebu/headsmp-a9.S
new file mode 100644
index 000000000000..5925366bc03c
--- /dev/null
+++ b/arch/arm/mach-mvebu/headsmp-a9.S
@@ -0,0 +1,34 @@
+/*
+ * SMP support: Entry point for secondary CPUs of Marvell EBU
+ * Cortex-A9 based SOCs (Armada 375 and Armada 38x).
+ *
+ * Copyright (C) 2014 Marvell
+ *
+ * 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/linkage.h>
+#include <linux/init.h>
+
+        __CPUINIT
+#define CPU_RESUME_ADDR_REG 0xf10182d4
+
+.global armada_375_smp_cpu1_enable_code_start
+.global armada_375_smp_cpu1_enable_code_end
+
+armada_375_smp_cpu1_enable_code_start:
+        ldr     r0, [pc, #4]
+        ldr     r1, [r0]
+        mov     pc, r1
+        .word   CPU_RESUME_ADDR_REG
+armada_375_smp_cpu1_enable_code_end:
+
+ENTRY(mvebu_cortex_a9_secondary_startup)
+        bl      v7_invalidate_l1
+        b       secondary_startup
+ENDPROC(mvebu_cortex_a9_secondary_startup)
diff --git a/arch/arm/mach-mvebu/headsmp.S b/arch/arm/mach-mvebu/headsmp.S
index 3dd80df428f7..2c4032e368ba 100644
--- a/arch/arm/mach-mvebu/headsmp.S
+++ b/arch/arm/mach-mvebu/headsmp.S
@@ -31,21 +31,10 @@
 ENTRY(armada_xp_secondary_startup)
  ARM_BE8(setend be )                    @ go BE8 if entered LE
 
-        /* Get coherency fabric base physical address */
-        adr     r0, 1f
-        ldr     r1, [r0]
-        ldr     r0, [r0, r1]
+        bl      ll_add_cpu_to_smp_group
 
-        /* Read CPU id */
-        mrc     p15, 0, r1, c0, c0, 5
-        and     r1, r1, #0xF
+        bl      ll_enable_coherency
 
-        /* Add CPU to coherency fabric */
-        bl      ll_set_cpu_coherent
         b       secondary_startup
 
 ENDPROC(armada_xp_secondary_startup)
-
-        .align 2
-1:
-        .long   coherency_phys_base - .
diff --git a/arch/arm/mach-mvebu/kirkwood.c b/arch/arm/mach-mvebu/kirkwood.c
index 120207fc36f1..46f105913c84 100644
--- a/arch/arm/mach-mvebu/kirkwood.c
+++ b/arch/arm/mach-mvebu/kirkwood.c
@@ -169,7 +169,7 @@ static void __init kirkwood_dt_init(void)
 {
         kirkwood_disable_mbus_error_propagation();
 
-        BUG_ON(mvebu_mbus_dt_init());
+        BUG_ON(mvebu_mbus_dt_init(false));
 
 #ifdef CONFIG_CACHE_FEROCEON_L2
         feroceon_of_init();
@@ -180,9 +180,6 @@ static void __init kirkwood_dt_init(void)
         kirkwood_pm_init();
         kirkwood_dt_eth_fixup();
 
-        if (of_machine_is_compatible("hp,t5325"))
-                t5325_init();
-
         of_platform_populate(NULL, of_default_bus_match_table, auxdata, NULL);
 }
 
diff --git a/arch/arm/mach-mvebu/mvebu-soc-id.c b/arch/arm/mach-mvebu/mvebu-soc-id.c
index 09520e19b78e..d0f35b4d4a23 100644
--- a/arch/arm/mach-mvebu/mvebu-soc-id.c
+++ b/arch/arm/mach-mvebu/mvebu-soc-id.c
@@ -23,6 +23,8 @@
 #include <linux/kernel.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/sys_soc.h>
 #include "mvebu-soc-id.h"
 
 #define PCIE_DEV_ID_OFF         0x0
@@ -127,5 +129,33 @@ clk_err:
 
         return ret;
 }
-core_initcall(mvebu_soc_id_init);
+early_initcall(mvebu_soc_id_init);
 
+static int __init mvebu_soc_device(void)
+{
+        struct soc_device_attribute *soc_dev_attr;
+        struct soc_device *soc_dev;
+
+        /* Also protects against running on non-mvebu systems */
+        if (!is_id_valid)
+                return 0;
+
+        soc_dev_attr = kzalloc(sizeof(*soc_dev_attr), GFP_KERNEL);
+        if (!soc_dev_attr)
+                return -ENOMEM;
+
+        soc_dev_attr->family = kasprintf(GFP_KERNEL, "Marvell");
+        soc_dev_attr->revision = kasprintf(GFP_KERNEL, "%X", soc_rev);
+        soc_dev_attr->soc_id = kasprintf(GFP_KERNEL, "%X", soc_dev_id);
+
+        soc_dev = soc_device_register(soc_dev_attr);
+        if (IS_ERR(soc_dev)) {
+                kfree(soc_dev_attr->family);
+                kfree(soc_dev_attr->revision);
+                kfree(soc_dev_attr->soc_id);
+                kfree(soc_dev_attr);
+        }
+
+        return 0;
+}
+postcore_initcall(mvebu_soc_device);
diff --git a/arch/arm/mach-mvebu/mvebu-soc-id.h b/arch/arm/mach-mvebu/mvebu-soc-id.h
index 31654252fe35..c16bb68ca81f 100644
--- a/arch/arm/mach-mvebu/mvebu-soc-id.h
+++ b/arch/arm/mach-mvebu/mvebu-soc-id.h
@@ -20,6 +20,10 @@
 #define MV78XX0_A0_REV      0x1
 #define MV78XX0_B0_REV      0x2
 
+/* Armada 375 */
+#define ARMADA_375_Z1_REV   0x0
+#define ARMADA_375_A0_REV   0x3
+
 #ifdef CONFIG_ARCH_MVEBU
 int mvebu_get_soc_id(u32 *dev, u32 *rev);
 #else
diff --git a/arch/arm/mach-mvebu/platsmp-a9.c b/arch/arm/mach-mvebu/platsmp-a9.c
new file mode 100644
index 000000000000..96c2c59e34b6
--- /dev/null
+++ b/arch/arm/mach-mvebu/platsmp-a9.c
@@ -0,0 +1,102 @@
+/*
+ * Symmetric Multi Processing (SMP) support for Marvell EBU Cortex-A9
+ * based SOCs (Armada 375/38x).
+ *
+ * Copyright (C) 2014 Marvell
+ *
+ * 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/init.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/smp.h>
+#include <linux/mbus.h>
+#include <asm/smp_scu.h>
+#include <asm/smp_plat.h>
+#include "common.h"
+#include "mvebu-soc-id.h"
+#include "pmsu.h"
+
+#define CRYPT0_ENG_ID   41
+#define CRYPT0_ENG_ATTR 0x1
+#define SRAM_PHYS_BASE  0xFFFF0000
+
+#define BOOTROM_BASE    0xFFF00000
+#define BOOTROM_SIZE    0x100000
+
+extern unsigned char armada_375_smp_cpu1_enable_code_end;
+extern unsigned char armada_375_smp_cpu1_enable_code_start;
+
+void armada_375_smp_cpu1_enable_wa(void)
+{
+        void __iomem *sram_virt_base;
+
+        mvebu_mbus_del_window(BOOTROM_BASE, BOOTROM_SIZE);
+        mvebu_mbus_add_window_by_id(CRYPT0_ENG_ID, CRYPT0_ENG_ATTR,
+                                SRAM_PHYS_BASE, SZ_64K);
+        sram_virt_base = ioremap(SRAM_PHYS_BASE, SZ_64K);
+
+        memcpy(sram_virt_base, &armada_375_smp_cpu1_enable_code_start,
+               &armada_375_smp_cpu1_enable_code_end
+               - &armada_375_smp_cpu1_enable_code_start);
+}
+
+extern void mvebu_cortex_a9_secondary_startup(void);
+
+static int __cpuinit mvebu_cortex_a9_boot_secondary(unsigned int cpu,
+                                                    struct task_struct *idle)
+{
+        int ret, hw_cpu;
+
+        pr_info("Booting CPU %d\n", cpu);
+
+        /*
+         * 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.
+         */
+        hw_cpu = cpu_logical_map(cpu);
+
+        if (of_machine_is_compatible("marvell,armada375")) {
+                u32 dev, rev;
+
+                if (mvebu_get_soc_id(&dev, &rev) == 0 &&
+                    rev == ARMADA_375_Z1_REV)
+                        armada_375_smp_cpu1_enable_wa();
+
+                mvebu_system_controller_set_cpu_boot_addr(mvebu_cortex_a9_secondary_startup);
+        }
+        else {
+                mvebu_pmsu_set_cpu_boot_addr(hw_cpu,
+                                             mvebu_cortex_a9_secondary_startup);
+        }
+
+        smp_wmb();
+        ret = mvebu_cpu_reset_deassert(hw_cpu);
+        if (ret) {
+                pr_err("Could not start the secondary CPU: %d\n", ret);
+                return ret;
+        }
+        arch_send_wakeup_ipi_mask(cpumask_of(cpu));
+
+        return 0;
+}
+
+static struct smp_operations mvebu_cortex_a9_smp_ops __initdata = {
+        .smp_boot_secondary     = mvebu_cortex_a9_boot_secondary,
+#ifdef CONFIG_HOTPLUG_CPU
+        .cpu_die                = armada_xp_cpu_die,
+#endif
+};
+
+CPU_METHOD_OF_DECLARE(mvebu_armada_375_smp, "marvell,armada-375-smp",
+                      &mvebu_cortex_a9_smp_ops);
+CPU_METHOD_OF_DECLARE(mvebu_armada_380_smp, "marvell,armada-380-smp",
+                      &mvebu_cortex_a9_smp_ops);
diff --git a/arch/arm/mach-mvebu/platsmp.c b/arch/arm/mach-mvebu/platsmp.c
index a6da03f5b24e..88b976b31719 100644
--- a/arch/arm/mach-mvebu/platsmp.c
+++ b/arch/arm/mach-mvebu/platsmp.c
@@ -70,16 +70,19 @@ static void __init set_secondary_cpus_clock(void)
         }
 }
 
-static void armada_xp_secondary_init(unsigned int cpu)
-{
-        armada_xp_mpic_smp_cpu_init();
-}
-
 static int armada_xp_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
+        int ret, hw_cpu;
+
         pr_info("Booting CPU %d\n", cpu);
 
-        armada_xp_boot_cpu(cpu, armada_xp_secondary_startup);
+        hw_cpu = cpu_logical_map(cpu);
+        mvebu_pmsu_set_cpu_boot_addr(hw_cpu, armada_xp_secondary_startup);
+        ret = mvebu_cpu_reset_deassert(hw_cpu);
+        if (ret) {
+                pr_warn("unable to boot CPU: %d\n", ret);
+                return ret;
+        }
 
         return 0;
 }
@@ -90,8 +93,6 @@ static void __init armada_xp_smp_init_cpus(void)
 
         if (ncores == 0 || ncores > ARMADA_XP_MAX_CPUS)
                 panic("Invalid number of CPUs in DT\n");
-
-        set_smp_cross_call(armada_mpic_send_doorbell);
 }
 
 static void __init armada_xp_smp_prepare_cpus(unsigned int max_cpus)
@@ -102,7 +103,7 @@ static 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);
+        set_cpu_coherent();
 
         /*
          * In order to boot the secondary CPUs we need to ensure
@@ -124,9 +125,11 @@ static void __init armada_xp_smp_prepare_cpus(unsigned int max_cpus)
 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
 };
+
+CPU_METHOD_OF_DECLARE(armada_xp_smp, "marvell,armada-xp-smp",
+                      &armada_xp_smp_ops);
diff --git a/arch/arm/mach-mvebu/pmsu.c b/arch/arm/mach-mvebu/pmsu.c
index d71ef53107c4..53a55c8520bf 100644
--- a/arch/arm/mach-mvebu/pmsu.c
+++ b/arch/arm/mach-mvebu/pmsu.c
@@ -16,62 +16,283 @@
  * other SOC units
  */
 
+#define pr_fmt(fmt) "mvebu-pmsu: " fmt
+
+#include <linux/cpu_pm.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/of_address.h>
 #include <linux/io.h>
+#include <linux/platform_device.h>
 #include <linux/smp.h>
+#include <linux/resource.h>
+#include <asm/cacheflush.h>
+#include <asm/cp15.h>
 #include <asm/smp_plat.h>
-#include "pmsu.h"
+#include <asm/suspend.h>
+#include <asm/tlbflush.h>
+#include "common.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)
+#define PMSU_BASE_OFFSET    0x100
+#define PMSU_REG_SIZE       0x1000
+
+/* PMSU MP registers */
+#define PMSU_CONTROL_AND_CONFIG(cpu)        ((cpu * 0x100) + 0x104)
+#define PMSU_CONTROL_AND_CONFIG_DFS_REQ         BIT(18)
+#define PMSU_CONTROL_AND_CONFIG_PWDDN_REQ       BIT(16)
+#define PMSU_CONTROL_AND_CONFIG_L2_PWDDN        BIT(20)
+
+#define PMSU_CPU_POWER_DOWN_CONTROL(cpu)    ((cpu * 0x100) + 0x108)
+
+#define PMSU_CPU_POWER_DOWN_DIS_SNP_Q_SKIP      BIT(0)
+
+#define PMSU_STATUS_AND_MASK(cpu)           ((cpu * 0x100) + 0x10c)
+#define PMSU_STATUS_AND_MASK_CPU_IDLE_WAIT      BIT(16)
+#define PMSU_STATUS_AND_MASK_SNP_Q_EMPTY_WAIT   BIT(17)
+#define PMSU_STATUS_AND_MASK_IRQ_WAKEUP         BIT(20)
+#define PMSU_STATUS_AND_MASK_FIQ_WAKEUP         BIT(21)
+#define PMSU_STATUS_AND_MASK_DBG_WAKEUP         BIT(22)
+#define PMSU_STATUS_AND_MASK_IRQ_MASK           BIT(24)
+#define PMSU_STATUS_AND_MASK_FIQ_MASK           BIT(25)
+
+#define PMSU_BOOT_ADDR_REDIRECT_OFFSET(cpu) ((cpu * 0x100) + 0x124)
+
+/* PMSU fabric registers */
+#define L2C_NFABRIC_PM_CTL                  0x4
+#define L2C_NFABRIC_PM_CTL_PWR_DOWN             BIT(20)
+
+extern void ll_disable_coherency(void);
+extern void ll_enable_coherency(void);
+
+static struct platform_device armada_xp_cpuidle_device = {
+        .name = "cpuidle-armada-370-xp",
+};
 
 static struct of_device_id of_pmsu_table[] = {
-        {.compatible = "marvell,armada-370-xp-pmsu"},
+        { .compatible = "marvell,armada-370-pmsu", },
+        { .compatible = "marvell,armada-370-xp-pmsu", },
+        { .compatible = "marvell,armada-380-pmsu", },
         { /* end of list */ },
 };
 
-#ifdef CONFIG_SMP
-int armada_xp_boot_cpu(unsigned int cpu_id, void *boot_addr)
+void mvebu_pmsu_set_cpu_boot_addr(int hw_cpu, void *boot_addr)
 {
-        int reg, hw_cpu;
+        writel(virt_to_phys(boot_addr), pmsu_mp_base +
+                PMSU_BOOT_ADDR_REDIRECT_OFFSET(hw_cpu));
+}
+
+static int __init armada_370_xp_pmsu_init(void)
+{
+        struct device_node *np;
+        struct resource res;
+        int ret = 0;
+
+        np = of_find_matching_node(NULL, of_pmsu_table);
+        if (!np)
+                return 0;
+
+        pr_info("Initializing Power Management Service Unit\n");
 
-        if (!pmsu_mp_base || !pmsu_reset_base) {
-                pr_warn("Can't boot CPU. PMSU is uninitialized\n");
-                return 1;
+        if (of_address_to_resource(np, 0, &res)) {
+                pr_err("unable to get resource\n");
+                ret = -ENOENT;
+                goto out;
         }
 
-        hw_cpu = cpu_logical_map(cpu_id);
+        if (of_device_is_compatible(np, "marvell,armada-370-xp-pmsu")) {
+                pr_warn(FW_WARN "deprecated pmsu binding\n");
+                res.start = res.start - PMSU_BASE_OFFSET;
+                res.end = res.start + PMSU_REG_SIZE - 1;
+        }
 
-        writel(virt_to_phys(boot_addr), pmsu_mp_base +
-                        PMSU_BOOT_ADDR_REDIRECT_OFFSET(hw_cpu));
+        if (!request_mem_region(res.start, resource_size(&res),
+                                np->full_name)) {
+                pr_err("unable to request region\n");
+                ret = -EBUSY;
+                goto out;
+        }
+
+        pmsu_mp_base = ioremap(res.start, resource_size(&res));
+        if (!pmsu_mp_base) {
+                pr_err("unable to map registers\n");
+                release_mem_region(res.start, resource_size(&res));
+                ret = -ENOMEM;
+                goto out;
+        }
+
+ out:
+        of_node_put(np);
+        return ret;
+}
+
+static void armada_370_xp_pmsu_enable_l2_powerdown_onidle(void)
+{
+        u32 reg;
+
+        if (pmsu_mp_base == NULL)
+                return;
+
+        /* Enable L2 & Fabric powerdown in Deep-Idle mode - Fabric */
+        reg = readl(pmsu_mp_base + L2C_NFABRIC_PM_CTL);
+        reg |= L2C_NFABRIC_PM_CTL_PWR_DOWN;
+        writel(reg, pmsu_mp_base + L2C_NFABRIC_PM_CTL);
+}
+
+static void armada_370_xp_cpu_resume(void)
+{
+        asm volatile("bl    ll_add_cpu_to_smp_group\n\t"
+                     "bl    ll_enable_coherency\n\t"
+                     "b     cpu_resume\n\t");
+}
+
+/* No locking is needed because we only access per-CPU registers */
+void armada_370_xp_pmsu_idle_prepare(bool deepidle)
+{
+        unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
+        u32 reg;
+
+        if (pmsu_mp_base == NULL)
+                return;
 
-        /* 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));
+        /*
+         * Adjust the PMSU configuration to wait for WFI signal, enable
+         * IRQ and FIQ as wakeup events, set wait for snoop queue empty
+         * indication and mask IRQ and FIQ from CPU
+         */
+        reg = readl(pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
+        reg |= PMSU_STATUS_AND_MASK_CPU_IDLE_WAIT    |
+               PMSU_STATUS_AND_MASK_IRQ_WAKEUP       |
+               PMSU_STATUS_AND_MASK_FIQ_WAKEUP       |
+               PMSU_STATUS_AND_MASK_SNP_Q_EMPTY_WAIT |
+               PMSU_STATUS_AND_MASK_IRQ_MASK         |
+               PMSU_STATUS_AND_MASK_FIQ_MASK;
+        writel(reg, pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
+
+        reg = readl(pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
+        /* ask HW to power down the L2 Cache if needed */
+        if (deepidle)
+                reg |= PMSU_CONTROL_AND_CONFIG_L2_PWDDN;
+
+        /* request power down */
+        reg |= PMSU_CONTROL_AND_CONFIG_PWDDN_REQ;
+        writel(reg, pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
+
+        /* Disable snoop disable by HW - SW is taking care of it */
+        reg = readl(pmsu_mp_base + PMSU_CPU_POWER_DOWN_CONTROL(hw_cpu));
+        reg |= PMSU_CPU_POWER_DOWN_DIS_SNP_Q_SKIP;
+        writel(reg, pmsu_mp_base + PMSU_CPU_POWER_DOWN_CONTROL(hw_cpu));
+}
+
+static noinline int do_armada_370_xp_cpu_suspend(unsigned long deepidle)
+{
+        armada_370_xp_pmsu_idle_prepare(deepidle);
+
+        v7_exit_coherency_flush(all);
+
+        ll_disable_coherency();
+
+        dsb();
+
+        wfi();
+
+        /* If we are here, wfi failed. As processors run out of
+         * coherency for some time, tlbs might be stale, so flush them
+         */
+        local_flush_tlb_all();
+
+        ll_enable_coherency();
+
+        /* Test the CR_C bit and set it if it was cleared */
+        asm volatile(
+        "mrc    p15, 0, %0, c1, c0, 0 \n\t"
+        "tst    %0, #(1 << 2) \n\t"
+        "orreq  %0, %0, #(1 << 2) \n\t"
+        "mcreq  p15, 0, %0, c1, c0, 0 \n\t"
+        "isb    "
+        : : "r" (0));
+
+        pr_warn("Failed to suspend the system\n");
 
         return 0;
 }
-#endif
 
-static int __init armada_370_xp_pmsu_init(void)
+static int armada_370_xp_cpu_suspend(unsigned long deepidle)
+{
+        return cpu_suspend(deepidle, do_armada_370_xp_cpu_suspend);
+}
+
+/* No locking is needed because we only access per-CPU registers */
+static noinline void armada_370_xp_pmsu_idle_restore(void)
+{
+        unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
+        u32 reg;
+
+        if (pmsu_mp_base == NULL)
+                return;
+
+        /* cancel ask HW to power down the L2 Cache if possible */
+        reg = readl(pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
+        reg &= ~PMSU_CONTROL_AND_CONFIG_L2_PWDDN;
+        writel(reg, pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
+
+        /* cancel Enable wakeup events and mask interrupts */
+        reg = readl(pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
+        reg &= ~(PMSU_STATUS_AND_MASK_IRQ_WAKEUP | PMSU_STATUS_AND_MASK_FIQ_WAKEUP);
+        reg &= ~PMSU_STATUS_AND_MASK_CPU_IDLE_WAIT;
+        reg &= ~PMSU_STATUS_AND_MASK_SNP_Q_EMPTY_WAIT;
+        reg &= ~(PMSU_STATUS_AND_MASK_IRQ_MASK | PMSU_STATUS_AND_MASK_FIQ_MASK);
+        writel(reg, pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
+}
+
+static int armada_370_xp_cpu_pm_notify(struct notifier_block *self,
+                                    unsigned long action, void *hcpu)
+{
+        if (action == CPU_PM_ENTER) {
+                unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
+                mvebu_pmsu_set_cpu_boot_addr(hw_cpu, armada_370_xp_cpu_resume);
+        } else if (action == CPU_PM_EXIT) {
+                armada_370_xp_pmsu_idle_restore();
+        }
+
+        return NOTIFY_OK;
+}
+
+static struct notifier_block armada_370_xp_cpu_pm_notifier = {
+        .notifier_call = armada_370_xp_cpu_pm_notify,
+};
+
+int __init armada_370_xp_cpu_pm_init(void)
 {
         struct device_node *np;
 
+        /*
+         * Check that all the requirements are available to enable
+         * cpuidle. So far, it is only supported on Armada XP, cpuidle
+         * needs the coherency fabric and the PMSU enabled
+         */
+
+        if (!of_machine_is_compatible("marvell,armadaxp"))
+                return 0;
+
+        np = of_find_compatible_node(NULL, NULL, "marvell,coherency-fabric");
+        if (!np)
+                return 0;
+        of_node_put(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);
-                of_node_put(np);
-        }
+        if (!np)
+                return 0;
+        of_node_put(np);
+
+        armada_370_xp_pmsu_enable_l2_powerdown_onidle();
+        armada_xp_cpuidle_device.dev.platform_data = armada_370_xp_cpu_suspend;
+        platform_device_register(&armada_xp_cpuidle_device);
+        cpu_pm_register_notifier(&armada_370_xp_cpu_pm_notifier);
 
         return 0;
 }
 
+arch_initcall(armada_370_xp_cpu_pm_init);
 early_initcall(armada_370_xp_pmsu_init);
diff --git a/arch/arm/mach-mvebu/system-controller.c b/arch/arm/mach-mvebu/system-controller.c
index 614ba6832ff3..0c5524ac75b7 100644
--- a/arch/arm/mach-mvebu/system-controller.c
+++ b/arch/arm/mach-mvebu/system-controller.c
@@ -37,6 +37,8 @@ struct mvebu_system_controller {
 
         u32 rstoutn_mask_reset_out_en;
         u32 system_soft_reset;
+
+        u32 resume_boot_addr;
 };
 static struct mvebu_system_controller *mvebu_sc;
 
@@ -52,6 +54,7 @@ static const struct mvebu_system_controller armada_375_system_controller = {
         .system_soft_reset_offset = 0x58,
         .rstoutn_mask_reset_out_en = 0x1,
         .system_soft_reset = 0x1,
+        .resume_boot_addr = 0xd4,
 };
 
 static const struct mvebu_system_controller orion_system_controller = {
@@ -98,6 +101,16 @@ void mvebu_restart(enum reboot_mode mode, const char *cmd)
                 ;
 }
 
+#ifdef CONFIG_SMP
+void mvebu_system_controller_set_cpu_boot_addr(void *boot_addr)
+{
+        BUG_ON(system_controller_base == NULL);
+        BUG_ON(mvebu_sc->resume_boot_addr == 0);
+        writel(virt_to_phys(boot_addr), system_controller_base +
+               mvebu_sc->resume_boot_addr);
+}
+#endif
+
 static int __init mvebu_system_controller_init(void)
 {
         const struct of_device_id *match;
@@ -114,4 +127,4 @@ static int __init mvebu_system_controller_init(void)
         return 0;
 }
 
-arch_initcall(mvebu_system_controller_init);
+early_initcall(mvebu_system_controller_init);