@@ -42,6 +42,12 @@ config CPU_IDLE_ZYNQ
 	help
 	  Select this to enable cpuidle on Xilinx Zynq processors.
 
+config CPU_IDLE_ARMADA_370_XP
+	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.
+
 endif
 
 config ARCH_NEEDS_CPU_IDLE_COUPLED

@@ -5,6 +5,7 @@
 obj-y += cpuidle.o driver.o governor.o sysfs.o governors/
 obj-$(CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED) += coupled.o
 
+obj-$(CONFIG_CPU_IDLE_ARMADA_370_XP) += cpuidle-armada-370-xp.o suspend-armada-370-xp.o
 obj-$(CONFIG_CPU_IDLE_CALXEDA) += cpuidle-calxeda.o
 obj-$(CONFIG_ARCH_KIRKWOOD) += cpuidle-kirkwood.o
 obj-$(CONFIG_CPU_IDLE_ZYNQ) += cpuidle-zynq.o

@@ -0,0 +1,132 @@
+/*
+ * Marvell Armada 370 and Armada XP SoC cpuidle driver
+ *
+ * Copyright (C) 2013 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/cpuidle.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/suspend.h>
+#include <asm/suspend.h>
+#include <linux/smp.h>
+#include <asm/cpuidle.h>
+#include <asm/smp_plat.h>
+#include <linux/armada-370-xp-pmsu.h>
+
+#define ARMADA_370_XP_MAX_STATES	3
+
+enum mv_pm_states {
+	WFI = 0,
+	MV_CPU_IDLE,
+	MV_CPU_DEEP_IDLE,
+	DISABLED,
+};
+
+extern void v7_flush_dcache_all(void);
+
+/* Functions defined in suspend-armada-370-xp.S */
+int armada_370_xp_cpu_resume(unsigned long);
+int armada_370_xp_cpu_suspend(unsigned long);
+
+int pm_support = WFI;
+static int __init pm_enable_setup(char *str)
+{
+	if (!strncmp(str, "wfi", 3))
+		pm_support = WFI;
+	else if (!strncmp(str, "idle", 4))
+		pm_support = MV_CPU_IDLE;
+	else if (!strncmp(str, "deepidle", 6))
+		pm_support = MV_CPU_DEEP_IDLE;
+	else if (!strncmp(str, "off", 3))
+		pm_support = DISABLED;
+
+	return 1;
+}
+__setup("pm_level=", pm_enable_setup);
+
+
+static int armada_370_xp_enter_idle(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv,
+				int index)
+{
+	bool deepidle = false;
+	unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
+
+	armada_370_xp_pmsu_set_start_addr(armada_370_xp_cpu_resume, hw_cpu);
+
+	if (index == MV_CPU_DEEP_IDLE)
+		deepidle = true;
+
+	cpu_suspend(deepidle, armada_370_xp_cpu_suspend);
+
+	cpu_init();
+
+	armada_370_xp_pmsu_idle_restore();
+
+	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,
+		.name			= "MV CPU DEEP IDLE",
+		.desc			= "CPU and L2 Fabric power down",
+	},
+	.state_count = ARMADA_370_XP_MAX_STATES,
+};
+
+static int __init armada_370_xp_cpuidle_init(void)
+{
+	if (!of_find_compatible_node(NULL, NULL, "marvell,armada-370-xp-pmsu"))
+		return -ENODEV;
+
+	if (!of_find_compatible_node(NULL, NULL, "marvell,coherency-fabric"))
+		return -ENODEV;
+
+	pr_info("Initializing Armada-XP CPU power management ");
+
+	armada_370_xp_pmsu_enable_l2_powerdown_onidle();
+
+	if (pm_support == WFI)
+		pr_info(" (WFI)\n");
+	else if (pm_support == MV_CPU_IDLE)
+		pr_info(" (CPU_IDLE)\n");
+	else if (pm_support == MV_CPU_DEEP_IDLE)
+		pr_info(" (CPU_DEEP_IDLE)\n");
+	else
+		pr_info(" (DISABLED)\n");
+
+	armada_370_xp_idle_driver.state_count = pm_support + 1;
+
+	return cpuidle_register(&armada_370_xp_idle_driver, NULL);
+}
+
+module_init(armada_370_xp_cpuidle_init);
+
+MODULE_LICENSE("GPL v2");

@@ -0,0 +1,91 @@
+/*
+ * CPU idle low level implementation for Marvell Armada 370 and Armada XP SoCs
+ *
+ * Copyright (C) 2013 Marvell
+ *
+ * Nadav Haklai <nadavh@marvell.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>
+
+
+/*
+* armadaxp_cpu_suspend: enter cpu deepIdle state
+* input:
+*/
+ENTRY(armada_370_xp_cpu_suspend)
+/* Save ARM registers */
+	stmfd	sp!, {r4 - r11, lr}	@ save registers on stack
+
+	bl armada_370_xp_pmsu_idle_prepare
+	/*
+	 * Invalidate L1 data cache. Even though only invalidate is
+	 * necessary exported flush API is used here. Doing clean
+	 * on already clean cache would be almost NOP.
+	 */
+	bl v7_flush_dcache_all
+
+	/*
+	 * Clear the SCTLR.C bit to prevent further data cache
+	 * allocation. Clearing SCTLR.C would make all the data accesses
+	 * strongly ordered and would not hit the cache.
+	 */
+	mrc	p15, 0, r0, c1, c0, 0
+	bic	r0, r0, #(1 << 2)	@ Disable the C bit
+	mcr	p15, 0, r0, c1, c0, 0
+	isb
+
+	bl v7_flush_dcache_all
+
+	/* Data memory barrier and Data sync barrier */
+	dsb
+	dmb
+
+	bl armada_370_xp_disable_snoop_ena
+
+	dsb				@ Data Synchronization Barrier
+
+/*
+ * ===================================
+ * == WFI instruction => Enter idle ==
+ * ===================================
+ */
+
+	wfi				@ wait for interrupt
+/*
+ * ===================================
+ * == Resume path for non-OFF modes ==
+ * ===================================
+ */
+
+/* Enable SnoopEna - Exclusive */
+	mov	r0, #1			@ r0!=0 means use virtual address
+	mov	r1, #0			@ Do not add CPU to SMP group
+	bl ll_set_cpu_coherent
+
+/* Re-enable C-bit if needed */
+	mrc	p15, 0, r0, c1, c0, 0
+	tst	r0, #(1 << 2)		@ Check C bit enabled?
+	orreq	r0, r0, #(1 << 2)	@ Enable the C bit if cleared
+	mcreq	p15, 0, r0, c1, c0, 0
+	isb
+
+	ldmfd	sp!, {r4 - r11, pc}	@ restore regs and return
+ENDPROC(armada_370_xp_cpu_suspend)
+
+/*
+* armada_370_xp_cpu_resume: exit cpu deepIdle state
+*/
+ENTRY(armada_370_xp_cpu_resume)
+	mov	r0, #0			@ r0==0 means use physical address
+	mov	r1, #1			@ Add CPU to SMP group
+	bl ll_set_cpu_coherent
+
+	/* Now branch to the common CPU resume function */
+	b	cpu_resume
+
+ENDPROC(armada_370_xp_cpu_resume)