Merge tag 'mvebu-soc-3.16' of git://git.infradead.org/linux-mvebu into next/soc

Merge "ARM: mvebu: SoC changes for v3.16" from Jason Cooper:

mvebu SoC changes for v3.16

 - Armada 375/38x coherency support
 - Armada 375/38x SMP support
 - mvebu PMSU and CPU reset support
 - Armada 370/XP cpuidle support
 - kirkwood remove platform init of audio device
 - small fixes and cleanup for new SoC (375/38x)

Note:
 - due to complex deps, cpuidle changes Acked by appropriate maintainer for
   going though arm-soc tree.

* tag 'mvebu-soc-3.16' of git://git.infradead.org/linux-mvebu: (46 commits)
  ARM: mvebu: Fix pmsu compilation when ARMv6 is selected
  ARM: mvebu: conditionalize Armada 375 coherency workaround
  ARM: mvebu: conditionalize Armada 375 SMP workaround
  ARM: mvebu: add Armada 375 A0 revision definition
  ARM: mvebu: initialize mvebu-soc-id earlier
  ARM: mvebu: fix thermal quirk SoC revision check
  ARM: Kirkwood: t5325: Remove platform device to instantiate audio
  ARM: Kirkwood: Remove platform driver for codec
  ARM: mvebu: Add thermal quirk for the Armada 375 DB board
  ARM: mvebu: Select HAVE_ARM_TWD only if SMP is enabled
  ARM: mvebu: fix the name of the parameter used in mvebu_get_soc_id
  ARM: mvebu: remove unnecessary ifdef around l2x0_of_init
  ARM: mvebu: register the cpuidle driver for the Armada XP SoCs
  cpuidle: mvebu: Add initial CPU idle support for Armada 370/XP SoC
  ARM: mvebu: Register notifier callback for the cpuidle transition
  ARM: mvebu: refine which files are build in mach-mvebu
  ARM: mvebu: Add the PMSU related part of the cpu idle functions
  ARM: mvebu: Allow to power down L2 cache controller in idle mode
  ARM: mvebu: Low level function to disable HW coherency support
  ARM: mvebu: Split low level functions to manipulate HW coherency
  ...

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

33 files changed, 1178 insertions, 206 deletions

diff --git a/Documentation/devicetree/bindings/arm/armada-370-xp-pmsu.txt b/Documentation/devicetree/bindings/arm/armada-370-xp-pmsu.txt
index 926b4d6aae7e..26799ef562df 100644
--- a/Documentation/devicetree/bindings/arm/armada-370-xp-pmsu.txt
+++ b/Documentation/devicetree/bindings/arm/armada-370-xp-pmsu.txt
@@ -1,20 +1,21 @@
 Power Management Service Unit(PMSU)
 -----------------------------------
-Available on Marvell SOCs: Armada 370 and Armada XP
+Available on Marvell SOCs: Armada 370, Armada 38x and Armada XP
 
 Required properties:
 
-- compatible: "marvell,armada-370-xp-pmsu"
+- compatible: should be one of:
+  - "marvell,armada-370-pmsu" for Armada 370 or Armada XP
+  - "marvell,armada-380-pmsu" for Armada 38x
+  - "marvell,armada-370-xp-pmsu" was used for Armada 370/XP but is now
+    deprecated and will be removed
 
-- reg: Should contain PMSU registers location and length. First pair
-  for the per-CPU SW Reset Control registers, second pair for the
-  Power Management Service Unit.
+- reg: Should contain PMSU registers location and length.
 
 Example:
 
-armada-370-xp-pmsu@d0022000 {
-        compatible = "marvell,armada-370-xp-pmsu";
-        reg = <0xd0022100 0x430>,
-              <0xd0020800 0x20>;
+armada-370-xp-pmsu@22000 {
+        compatible = "marvell,armada-370-pmsu";
+        reg = <0x22000 0x1000>;
 };
 
diff --git a/Documentation/devicetree/bindings/arm/armada-cpu-reset.txt b/Documentation/devicetree/bindings/arm/armada-cpu-reset.txt
new file mode 100644
index 000000000000..b63a7b6ab998
--- /dev/null
+++ b/Documentation/devicetree/bindings/arm/armada-cpu-reset.txt
@@ -0,0 +1,14 @@
+Marvell Armada CPU reset controller
+===================================
+
+Required properties:
+
+- compatible: Should be "marvell,armada-370-cpu-reset".
+
+- reg: should be register base and length as documented in the
+  datasheet for the CPU reset registers
+
+cpurst: cpurst@20800 {
+       compatible = "marvell,armada-370-cpu-reset";
+       reg = <0x20800 0x20>;
+};
diff --git a/Documentation/devicetree/bindings/arm/coherency-fabric.txt b/Documentation/devicetree/bindings/arm/coherency-fabric.txt
index 17d8cd107559..8dd46617c889 100644
--- a/Documentation/devicetree/bindings/arm/coherency-fabric.txt
+++ b/Documentation/devicetree/bindings/arm/coherency-fabric.txt
@@ -1,16 +1,33 @@
 Coherency fabric
 ----------------
-Available on Marvell SOCs: Armada 370 and Armada XP
+Available on Marvell SOCs: Armada 370, Armada 375, Armada 38x and Armada XP
 
 Required properties:
 
-- compatible: "marvell,coherency-fabric"
+- compatible: the possible values are:
+
+ * "marvell,coherency-fabric", to be used for the coherency fabric of
+   the Armada 370 and Armada XP.
+
+ * "marvell,armada-375-coherency-fabric", for the Armada 375 coherency
+   fabric.
+
+ * "marvell,armada-380-coherency-fabric", for the Armada 38x coherency
+   fabric.
 
 - reg: Should contain coherency fabric registers location and
-  length. First pair for the coherency fabric registers, second pair
-  for the per-CPU fabric registers registers.
+  length.
+
+ * For "marvell,coherency-fabric", the first pair for the coherency
+   fabric registers, second pair for the per-CPU fabric registers.
 
-Example:
+ * For "marvell,armada-375-coherency-fabric", only one pair is needed
+   for the per-CPU fabric registers.
+
+ * For "marvell,armada-380-coherency-fabric", only one pair is needed
+   for the per-CPU fabric registers.
+
+Examples:
 
 coherency-fabric@d0020200 {
         compatible = "marvell,coherency-fabric";
@@ -19,3 +36,8 @@ coherency-fabric@d0020200 {
 
 };
 
+coherency-fabric@21810 {
+        compatible = "marvell,armada-375-coherency-fabric";
+        reg = <0x21810 0x1c>;
+};
+
diff --git a/Documentation/devicetree/bindings/arm/cpus.txt b/Documentation/devicetree/bindings/arm/cpus.txt
index 333f4aea3029..4bbcf4fb7583 100644
--- a/Documentation/devicetree/bindings/arm/cpus.txt
+++ b/Documentation/devicetree/bindings/arm/cpus.txt
@@ -185,6 +185,9 @@ nodes to be present and contain the properties described below.
                             "qcom,gcc-msm8660"
                             "qcom,kpss-acc-v1"
                             "qcom,kpss-acc-v2"
+                            "marvell,armada-375-smp"
+                            "marvell,armada-380-smp"
+                            "marvell,armada-xp-smp"
 
         - cpu-release-addr
                 Usage: required for systems that have an "enable-method"
diff --git a/arch/arm/mach-kirkwood/board-dt.c b/arch/arm/mach-kirkwood/board-dt.c
index 2801da49e2a3..ff18ff20f71f 100644
--- a/arch/arm/mach-kirkwood/board-dt.c
+++ b/arch/arm/mach-kirkwood/board-dt.c
@@ -195,7 +195,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();
diff --git a/arch/arm/mach-mvebu/Kconfig b/arch/arm/mach-mvebu/Kconfig
index 3f73eecbcfb0..d6b0a772a6dd 100644
--- a/arch/arm/mach-mvebu/Kconfig
+++ b/arch/arm/mach-mvebu/Kconfig
@@ -6,6 +6,7 @@ config ARCH_MVEBU
         select IRQ_DOMAIN
         select PINCTRL
         select PLAT_ORION
+        select SOC_BUS
         select MVEBU_MBUS
         select ZONE_DMA if ARM_LPAE
         select ARCH_REQUIRE_GPIOLIB
@@ -39,6 +40,9 @@ config MACH_ARMADA_375
         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
@@ -52,6 +56,9 @@ config MACH_ARMADA_38X
         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
@@ -97,13 +104,6 @@ config MACH_KIRKWOOD
           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..01cfce6ac20b 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);
 }
 
@@ -123,6 +196,7 @@ static const char * const armada_375_dt_compat[] = {
 
 DT_MACHINE_START(ARMADA_375_DT, "Marvell Armada 375 (Device Tree)")
         .init_time      = mvebu_timer_and_clk_init,
+        .init_machine   = mvebu_dt_init,
         .restart        = mvebu_restart,
         .dt_compat      = armada_375_dt_compat,
 MACHINE_END
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..d5a975b6a590 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,17 @@
 #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 <asm/smp_plat.h>
 #include <asm/cacheflush.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 +44,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);
 }
@@ -121,42 +290,93 @@ static struct notifier_block mvebu_hwcc_platform_nb = {
         .notifier_call = mvebu_hwcc_platform_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();
+}
+
+static void __init armada_375_380_coherency_init(struct device_node *np)
+{
+        coherency_cpu_base = of_iomap(np, 0);
+}
+
+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_platform_nb);
+
         return 0;
 }
 
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..6828f9f157b0 100644
--- a/arch/arm/mach-mvebu/coherency_ll.S
+++ b/arch/arm/mach-mvebu/coherency_ll.S
@@ -21,38 +21,108 @@
 #define ARMADA_XP_CFB_CFG_REG_OFFSET 0x4
 
 #include <asm/assembler.h>
+#include <asm/cp15.h>
 
         .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
-ARM_BE8(rev     r1, r1)
+/* Returns with the coherency 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
 
-        /* Add CPU to SMP group - Atomic */
-        add     r3, r0, #ARMADA_XP_CFB_CTL_REG_OFFSET
+        /* use physical address of the coherency register */
+        adr     r1, 3f
+        ldr     r3, [r1]
+        ldr     r1, [r1, r3]
+        b       2f
 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
+        /* use virtual address of the coherency register */
+        ldr     r1, =coherency_base
+        ldr     r1, [r1]
+2:
+        mov     pc, lr
+ENDPROC(ll_get_coherency_base)
+
+/* Returns with the CPU ID in r3 (r0 is untouched)*/
+ENTRY(ll_get_cpuid)
+        mrc     15, 0, r3, cr0, cr0, 5
+        and     r3, r3, #15
+        mov     r2, #(1 << 24)
+        lsl     r3, r2, r3
+ARM_BE8(rev     r1, r1)
+        mov     pc, lr
+ENDPROC(ll_get_cpuid)
+
+/* ll_add_cpu_to_smp_group, ll_enable_coherency and
+ * ll_disable_coherency use strex/ldrex whereas MMU can be off. The
+ * Armada XP SoC has an exclusive monitor that can track transactions
+ * to Device and/or SO and as such also when MMU is disabled the
+ * exclusive transactions will be functional
+ */
+
+ENTRY(ll_add_cpu_to_smp_group)
+        /*
+         * r0 being untouched in ll_get_coherency_base and
+         * ll_get_cpuid, we can use it to save lr modifing it with the
+         * following bl
+         */
+        mov r0, lr
+        bl      ll_get_coherency_base
+        bl      ll_get_cpuid
+        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)
+        /*
+         * r0 being untouched in ll_get_coherency_base and
+         * ll_get_cpuid, we can use it to save lr modifing it with the
+         * following bl
+         */
+        mov r0, lr
+        bl      ll_get_coherency_base
+        bl      ll_get_cpuid
+        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)
+        /*
+         * r0 being untouched in ll_get_coherency_base and
+         * ll_get_cpuid, we can use it to save lr modifing it with the
+         * following bl
+         */
+        mov r0, lr
+        bl      ll_get_coherency_base
+        bl      ll_get_cpuid
+        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 f3d4cf53f746..e9119a99a5f3 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
@@ -116,5 +118,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);
diff --git a/drivers/bus/mvebu-mbus.c b/drivers/bus/mvebu-mbus.c
index 293e2e0a0a87..ff02fc90fc21 100644
--- a/drivers/bus/mvebu-mbus.c
+++ b/drivers/bus/mvebu-mbus.c
@@ -694,7 +694,6 @@ static int __init mvebu_mbus_common_init(struct mvebu_mbus_state *mbus,
                                          phys_addr_t sdramwins_phys_base,
                                          size_t sdramwins_size)
 {
-        struct device_node *np;
         int win;
 
         mbus->mbuswins_base = ioremap(mbuswins_phys_base, mbuswins_size);
@@ -707,12 +706,6 @@ static int __init mvebu_mbus_common_init(struct mvebu_mbus_state *mbus,
                 return -ENOMEM;
         }
 
-        np = of_find_compatible_node(NULL, NULL, "marvell,coherency-fabric");
-        if (np) {
-                mbus->hw_io_coherency = 1;
-                of_node_put(np);
-        }
-
         for (win = 0; win < mbus->soc->num_wins; win++)
                 mvebu_mbus_disable_window(mbus, win);
 
@@ -882,7 +875,7 @@ static void __init mvebu_mbus_get_pcie_resources(struct device_node *np,
         }
 }
 
-int __init mvebu_mbus_dt_init(void)
+int __init mvebu_mbus_dt_init(bool is_coherent)
 {
         struct resource mbuswins_res, sdramwins_res;
         struct device_node *np, *controller;
@@ -920,6 +913,8 @@ int __init mvebu_mbus_dt_init(void)
                 return -EINVAL;
         }
 
+        mbus_state.hw_io_coherency = is_coherent;
+
         /* Get optional pcie-{mem,io}-aperture properties */
         mvebu_mbus_get_pcie_resources(np, &mbus_state.pcie_mem_aperture,
                                           &mbus_state.pcie_io_aperture);
diff --git a/drivers/cpuidle/Kconfig.arm b/drivers/cpuidle/Kconfig.arm
index 97ccc31dbdd8..5bb94780d377 100644
--- a/drivers/cpuidle/Kconfig.arm
+++ b/drivers/cpuidle/Kconfig.arm
@@ -1,6 +1,11 @@
 #
 # ARM CPU Idle drivers
 #
+config ARM_ARMADA_370_XP_CPUIDLE
+        bool "CPU Idle Driver for Armada 370/XP family processors"
+        depends on ARCH_MVEBU
+        help
+          Select this to enable cpuidle on Armada 370/XP processors.
 
 config ARM_BIG_LITTLE_CPUIDLE
         bool "Support for ARM big.LITTLE processors"
diff --git a/drivers/cpuidle/Makefile b/drivers/cpuidle/Makefile
index f71ae1b373c5..9902d052bd87 100644
--- a/drivers/cpuidle/Makefile
+++ b/drivers/cpuidle/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED) += coupled.o
 
 ##################################################################################
 # ARM SoC drivers
+obj-$(CONFIG_ARM_ARMADA_370_XP_CPUIDLE) += cpuidle-armada-370-xp.o
 obj-$(CONFIG_ARM_BIG_LITTLE_CPUIDLE)    += cpuidle-big_little.o
 obj-$(CONFIG_ARM_HIGHBANK_CPUIDLE)      += cpuidle-calxeda.o
 obj-$(CONFIG_ARM_KIRKWOOD_CPUIDLE)      += cpuidle-kirkwood.o
diff --git a/drivers/cpuidle/cpuidle-armada-370-xp.c b/drivers/cpuidle/cpuidle-armada-370-xp.c
new file mode 100644
index 000000000000..28587d0f3947
--- /dev/null
+++ b/drivers/cpuidle/cpuidle-armada-370-xp.c
@@ -0,0 +1,93 @@
+/*
+ * Marvell Armada 370 and Armada XP SoC cpuidle driver
+ *
+ * Copyright (C) 2014 Marvell
+ *
+ * Nadav Haklai <nadavh@marvell.com>
+ * 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.
+ *
+ * Maintainer: Gregory CLEMENT <gregory.clement@free-electrons.com>
+ */
+
+#include <linux/cpu_pm.h>
+#include <linux/cpuidle.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/suspend.h>
+#include <linux/platform_device.h>
+#include <asm/cpuidle.h>
+
+#define ARMADA_370_XP_MAX_STATES        3
+#define ARMADA_370_XP_FLAG_DEEP_IDLE    0x10000
+
+static int (*armada_370_xp_cpu_suspend)(int);
+
+static int armada_370_xp_enter_idle(struct cpuidle_device *dev,
+                                struct cpuidle_driver *drv,
+                                int index)
+{
+        int ret;
+        bool deepidle = false;
+        cpu_pm_enter();
+
+        if (drv->states[index].flags & ARMADA_370_XP_FLAG_DEEP_IDLE)
+                deepidle = true;
+
+        ret = armada_370_xp_cpu_suspend(deepidle);
+        if (ret)
+                return ret;
+
+        cpu_pm_exit();
+
+        return index;
+}
+
+static struct cpuidle_driver armada_370_xp_idle_driver = {
+        .name                   = "armada_370_xp_idle",
+        .states[0]              = ARM_CPUIDLE_WFI_STATE,
+        .states[1]              = {
+                .enter                  = armada_370_xp_enter_idle,
+                .exit_latency           = 10,
+                .power_usage            = 50,
+                .target_residency       = 100,
+                .flags                  = CPUIDLE_FLAG_TIME_VALID,
+                .name                   = "MV CPU IDLE",
+                .desc                   = "CPU power down",
+        },
+        .states[2]              = {
+                .enter                  = armada_370_xp_enter_idle,
+                .exit_latency           = 100,
+                .power_usage            = 5,
+                .target_residency       = 1000,
+                .flags                  = CPUIDLE_FLAG_TIME_VALID |
+                                                ARMADA_370_XP_FLAG_DEEP_IDLE,
+                .name                   = "MV CPU DEEP IDLE",
+                .desc                   = "CPU and L2 Fabric power down",
+        },
+        .state_count = ARMADA_370_XP_MAX_STATES,
+};
+
+static int armada_370_xp_cpuidle_probe(struct platform_device *pdev)
+{
+
+        armada_370_xp_cpu_suspend = (void *)(pdev->dev.platform_data);
+        return cpuidle_register(&armada_370_xp_idle_driver, NULL);
+}
+
+static struct platform_driver armada_370_xp_cpuidle_plat_driver = {
+        .driver = {
+                .name = "cpuidle-armada-370-xp",
+                .owner = THIS_MODULE,
+        },
+        .probe = armada_370_xp_cpuidle_probe,
+};
+
+module_platform_driver(armada_370_xp_cpuidle_plat_driver);
+
+MODULE_AUTHOR("Gregory CLEMENT <gregory.clement@free-electrons.com>");
+MODULE_DESCRIPTION("Armada 370/XP cpu idle driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/irqchip/irq-armada-370-xp.c b/drivers/irqchip/irq-armada-370-xp.c
index 3899ba7821c5..c887e6eebc41 100644
--- a/drivers/irqchip/irq-armada-370-xp.c
+++ b/drivers/irqchip/irq-armada-370-xp.c
@@ -19,6 +19,7 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/irqchip/chained_irq.h>
+#include <linux/cpu.h>
 #include <linux/io.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
@@ -310,7 +311,8 @@ static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
 }
 
 #ifdef CONFIG_SMP
-void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq)
+static void armada_mpic_send_doorbell(const struct cpumask *mask,
+                                      unsigned int irq)
 {
         int cpu;
         unsigned long map = 0;
@@ -330,7 +332,7 @@ void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq)
                 ARMADA_370_XP_SW_TRIG_INT_OFFS);
 }
 
-void armada_xp_mpic_smp_cpu_init(void)
+static void armada_xp_mpic_smp_cpu_init(void)
 {
         /* Clear pending IPIs */
         writel(0, per_cpu_int_base + ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS);
@@ -342,6 +344,20 @@ void armada_xp_mpic_smp_cpu_init(void)
         /* Unmask IPI interrupt */
         writel(0, per_cpu_int_base + ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
 }
+
+static int armada_xp_mpic_secondary_init(struct notifier_block *nfb,
+                                         unsigned long action, void *hcpu)
+{
+        if (action == CPU_STARTING || action == CPU_STARTING_FROZEN)
+                armada_xp_mpic_smp_cpu_init();
+        return NOTIFY_OK;
+}
+
+static struct notifier_block armada_370_xp_mpic_cpu_notifier = {
+        .notifier_call = armada_xp_mpic_secondary_init,
+        .priority = 100,
+};
+
 #endif /* CONFIG_SMP */
 
 static struct irq_domain_ops armada_370_xp_mpic_irq_ops = {
@@ -497,6 +513,10 @@ static int __init armada_370_xp_mpic_of_init(struct device_node *node,
         if (parent_irq <= 0) {
                 irq_set_default_host(armada_370_xp_mpic_domain);
                 set_handle_irq(armada_370_xp_handle_irq);
+#ifdef CONFIG_SMP
+                set_smp_cross_call(armada_mpic_send_doorbell);
+                register_cpu_notifier(&armada_370_xp_mpic_cpu_notifier);
+#endif
         } else {
                 irq_set_chained_handler(parent_irq,
                                         armada_370_xp_mpic_handle_cascade_irq);
diff --git a/drivers/irqchip/irq-orion.c b/drivers/irqchip/irq-orion.c
index e25f246cd2fb..34d18b48bb78 100644
--- a/drivers/irqchip/irq-orion.c
+++ b/drivers/irqchip/irq-orion.c
@@ -42,7 +42,7 @@ __exception_irq_entry orion_handle_irq(struct pt_regs *regs)
                 u32 stat = readl_relaxed(gc->reg_base + ORION_IRQ_CAUSE) &
                         gc->mask_cache;
                 while (stat) {
-                        u32 hwirq = ffs(stat) - 1;
+                        u32 hwirq = __fls(stat);
                         u32 irq = irq_find_mapping(orion_irq_domain,
                                                    gc->irq_base + hwirq);
                         handle_IRQ(irq, regs);
@@ -117,7 +117,7 @@ static void orion_bridge_irq_handler(unsigned int irq, struct irq_desc *desc)
                    gc->mask_cache;
 
         while (stat) {
-                u32 hwirq = ffs(stat) - 1;
+                u32 hwirq = __fls(stat);
 
                 generic_handle_irq(irq_find_mapping(d, gc->irq_base + hwirq));
                 stat &= ~(1 << hwirq);
diff --git a/include/linux/mbus.h b/include/linux/mbus.h
index 345b8c53b897..550c88fb0267 100644
--- a/include/linux/mbus.h
+++ b/include/linux/mbus.h
@@ -73,6 +73,6 @@ int mvebu_mbus_del_window(phys_addr_t base, size_t size);
 int mvebu_mbus_init(const char *soc, phys_addr_t mbus_phys_base,
                     size_t mbus_size, phys_addr_t sdram_phys_base,
                     size_t sdram_size);
-int mvebu_mbus_dt_init(void);
+int mvebu_mbus_dt_init(bool is_coherent);
 
 #endif /* __LINUX_MBUS_H */