ARM: ux500: pm: Added context and pm

These files are primarily needed by suspend
and cpuidle.

Change-Id: I1cfccc33d6e412dfcaaf1793ce19ea27c4b3b724
Signed-off-by: Rickard Andersson <rickard.andersson@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/32068
Reviewed-by: Jonas ABERG <jonas.aberg@stericsson.com>
Tested-by: Jonas ABERG <jonas.aberg@stericsson.com>

18 files changed, 2992 insertions, 23 deletions

diff --git a/arch/arm/configs/u8500_defconfig b/arch/arm/configs/u8500_defconfig
index bd70193b0a2..652b537bf51 100755
--- a/arch/arm/configs/u8500_defconfig
+++ b/arch/arm/configs/u8500_defconfig
@@ -36,6 +36,8 @@ CONFIG_DISPLAY_AV8100_TERTIARY=y
 CONFIG_DISPLAY_AV8100_TRIPPLE_BUFFER=y
 CONFIG_DBX500_PRCMU_QOS_POWER=y
 CONFIG_DBX500_PRCMU_DEBUG=y
+# CONFIG_UX500_DEBUG_NO_LAUTERBACH is not set
+CONFIG_UX500_CONTEXT=y
 CONFIG_NO_HZ=y
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_SMP=y
diff --git a/arch/arm/mach-ux500/Makefile b/arch/arm/mach-ux500/Makefile
index 54ff7fca12c..8d5d620ef4f 100644
--- a/arch/arm/mach-ux500/Makefile
+++ b/arch/arm/mach-ux500/Makefile
@@ -4,7 +4,8 @@
 
 obj-y				:= clock.o cpu.o devices.o dcache.o \
 				   devices-common.o id.o pins.o \
-				   usb.o reboot_reasons.o timer.o
+				   usb.o reboot_reasons.o timer.o \
+				   uart-db8500.o
 obj-y				+= pm/
 
 obj-$(CONFIG_REGULATOR)			+= regulator-ux500.o
diff --git a/arch/arm/mach-ux500/devices-common.c b/arch/arm/mach-ux500/devices-common.c
index b01af37020c..4d705af245e 100644
--- a/arch/arm/mach-ux500/devices-common.c
+++ b/arch/arm/mach-ux500/devices-common.c
@@ -21,8 +21,8 @@
 #include <video/mcde_fb.h>
 
 #include <mach/hardware.h>
+#include <mach/pm.h>
 
-#include "pm/pm.h"
 #include "devices-common.h"
 
 struct amba_device *
diff --git a/arch/arm/mach-ux500/devices-db5500.c b/arch/arm/mach-ux500/devices-db5500.c
index ee6050263f5..bbbe6420ffc 100644
--- a/arch/arm/mach-ux500/devices-db5500.c
+++ b/arch/arm/mach-ux500/devices-db5500.c
@@ -21,6 +21,7 @@
 #include <mach/db5500-regs.h>
 
 #include <mach/prcmu.h>
+#include <mach/pm.h>
 
 #define GPIO_DATA(_name, first, num)					\
 	{								\
@@ -28,6 +29,8 @@
 		.first_gpio	= first,				\
 		.first_irq	= NOMADIK_GPIO_TO_IRQ(first),		\
 		.num_gpio	= num,					\
+		.get_secondary_status = ux500_pm_gpio_read_wake_up_status, \
+		.set_ioforce	= ux500_pm_prcmu_set_ioforce,		\
 	}
 
 #define GPIO_RESOURCE(block)						\
diff --git a/arch/arm/mach-ux500/devices-db8500.c b/arch/arm/mach-ux500/devices-db8500.c
index 86e371f444b..0353a14d02a 100644
--- a/arch/arm/mach-ux500/devices-db8500.c
+++ b/arch/arm/mach-ux500/devices-db8500.c
@@ -24,12 +24,12 @@
 #include <mach/devices.h>
 #include <mach/hardware.h>
 #include <mach/setup.h>
+#include <mach/pm.h>
 #include <video/mcde.h>
 #include <linux/mfd/dbx500-prcmu.h>
 #include <mach/hsi.h>
 #include <mach/ste-dma40-db8500.h>
 #include <trace/stm.h>
-#include "pm/pm.h"
 
 #include "pins-db8500.h"
 
@@ -39,6 +39,8 @@
 		.first_gpio	= first,				\
 		.first_irq	= NOMADIK_GPIO_TO_IRQ(first),		\
 		.num_gpio	= num,					\
+		.get_secondary_status = ux500_pm_gpio_read_wake_up_status, \
+		.set_ioforce	= ux500_pm_prcmu_set_ioforce,		\
 		.supports_sleepmode = true,				\
 	}
 
diff --git a/arch/arm/mach-ux500/dma-db8500.c b/arch/arm/mach-ux500/dma-db8500.c
index e17eb7a3d8a..022d8d293eb 100644
--- a/arch/arm/mach-ux500/dma-db8500.c
+++ b/arch/arm/mach-ux500/dma-db8500.c
@@ -15,8 +15,10 @@
 #include <mach/hsi.h>
 #include <mach/setup.h>
 #include <mach/ste-dma40-db8500.h>
+#include <mach/pm.h>
+#include <mach/context.h>
+
 
-#include "pm/pm.h"
 
 static struct resource dma40_resources[] = {
 	[0] = {
@@ -231,6 +233,54 @@ static struct stedma40_platform_data dma40_plat_data = {
 	.use_esram_lcla = false,
 };
 
+#ifdef CONFIG_UX500_CONTEXT
+#define D40_DREG_GCC		0x000
+#define D40_DREG_LCPA		0x020
+#define D40_DREG_LCLA		0x024
+
+static void __iomem *base;
+
+static int dma_context_notifier_call(struct notifier_block *this,
+				     unsigned long event, void *data)
+{
+	static unsigned long lcpa;
+	static unsigned long lcla;
+	static unsigned long gcc;
+
+	switch (event) {
+	case CONTEXT_APE_SAVE:
+		lcla = readl(base + D40_DREG_LCLA);
+		lcpa = readl(base + D40_DREG_LCPA);
+		gcc = readl(base + D40_DREG_GCC);
+		break;
+
+	case CONTEXT_APE_RESTORE:
+		writel(gcc, base + D40_DREG_GCC);
+		writel(lcpa, base + D40_DREG_LCPA);
+		writel(lcla, base + D40_DREG_LCLA);
+		break;
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block dma_context_notifier = {
+	.notifier_call = dma_context_notifier_call,
+};
+
+static void dma_context_notifier_init(void)
+{
+	base = ioremap(dma40_resources[0].start, resource_size(&dma40_resources[0]));
+	if (WARN_ON(!base))
+		return;
+
+	WARN_ON(context_ape_notifier_register(&dma_context_notifier));
+}
+#else
+static void dma_context_notifier_init(void)
+{
+}
+#endif
+
 static struct platform_device dma40_device = {
 	.dev = {
 		.platform_data = &dma40_plat_data,
@@ -252,4 +302,5 @@ void __init db8500_dma_init(void)
 	if (ret)
 		dev_err(&dma40_device.dev, "unable to register device: %d\n", ret);
 
+	dma_context_notifier_init();
 }
diff --git a/arch/arm/mach-ux500/include/mach/context.h b/arch/arm/mach-ux500/include/mach/context.h
new file mode 100644
index 00000000000..22b56351284
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/context.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>
+ *         Rickard Andersson <rickard.andersson@stericsson.com> for
+ *         ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+#ifndef CONTEXT_H
+#define CONTEXT_H
+
+#include <linux/notifier.h>
+
+#ifdef CONFIG_UX500_CONTEXT
+
+/* Defines to be with
+ * context_ape_notifier_register
+ */
+#define CONTEXT_APE_SAVE 0 /* APE save */
+#define CONTEXT_APE_RESTORE 1 /* APE restore */
+
+/* Defines to be with
+ * context_arm_notifier_register
+ */
+#define CONTEXT_ARM_CORE_SAVE 0 /* Called for each ARM core */
+#define CONTEXT_ARM_CORE_RESTORE 1 /* Called for each ARM core */
+#define CONTEXT_ARM_COMMON_SAVE 2 /* Called when ARM common is saved */
+#define CONTEXT_ARM_COMMON_RESTORE 3 /* Called when ARM common is restored */
+
+int context_ape_notifier_register(struct notifier_block *nb);
+int context_ape_notifier_unregister(struct notifier_block *nb);
+
+int context_arm_notifier_register(struct notifier_block *nb);
+int context_arm_notifier_unregister(struct notifier_block *nb);
+
+void context_vape_save(void);
+void context_vape_restore(void);
+
+void context_gpio_save(void);
+void context_gpio_restore(void);
+void context_gpio_restore_mux(void);
+void context_gpio_mux_safe_switch(bool begin);
+
+void context_gic_dist_disable_unneeded_irqs(void);
+
+void context_varm_save_common(void);
+void context_varm_restore_common(void);
+
+void context_varm_save_core(void);
+void context_varm_restore_core(void);
+
+void context_save_cpu_registers(void);
+void context_restore_cpu_registers(void);
+
+void context_save_to_sram_and_wfi(bool cleanL2cache);
+
+void context_clean_l1_cache_all(void);
+void context_save_arm_registers(u32 **backup_stack);
+void context_restore_arm_registers(u32 **backup_stack);
+
+void context_save_cp15_registers(u32 **backup_stack);
+void context_restore_cp15_registers(u32 **backup_stack);
+
+void context_save_to_sram_and_wfi_internal(u32 backup_sram_storage,
+					   bool cleanL2cache);
+
+/* DB specific functions in either context-db8500 or context-db5500 */
+void u8500_context_save_icn(void);
+void u8500_context_restore_icn(void);
+void u8500_context_init(void);
+
+void u5500_context_save_icn(void);
+void u5500_context_restore_icn(void);
+void u5500_context_init(void);
+
+#else
+
+static inline void context_varm_save_core(void) {}
+static inline void context_save_cpu_registers(void) {}
+static inline void context_save_to_sram_and_wfi(bool cleanL2cache) {}
+static inline void context_restore_cpu_registers(void) {}
+static inline void context_varm_restore_core(void) {}
+
+#endif
+
+#endif
diff --git a/arch/arm/mach-ux500/include/mach/pm.h b/arch/arm/mach-ux500/include/mach/pm.h
new file mode 100644
index 00000000000..8b58bd52e18
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/pm.h
@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com> for
+ *         ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#ifndef PM_COMMON_H
+#define PM_COMMON_H
+
+#ifdef CONFIG_PM
+
+enum prcmu_idle_stat {
+	SLEEP_OK		= 0xf3,
+	DEEP_SLEEP_OK		= 0xf6,
+	IDLE_OK			= 0xf0,
+	DEEPIDLE_OK		= 0xe3,
+	PRCMU2ARMPENDINGIT_ER	= 0x91,
+	ARMPENDINGIT_ER		= 0x93,
+};
+
+/**
+ * ux500_pm_gic_decouple()
+ *
+ * Decouple GIC from the interrupt bus.
+ */
+void ux500_pm_gic_decouple(void);
+
+/**
+ * ux500_pm_gic_recouple()
+ *
+ * Recouple GIC with the interrupt bus.
+ */
+void ux500_pm_gic_recouple(void);
+
+/**
+ * ux500_pm_gic_pending_interrupt()
+ *
+ * returns true, if there are pending interrupts.
+ */
+bool ux500_pm_gic_pending_interrupt(void);
+
+/**
+ * ux500_pm_prcmu_pending_interrupt()
+ *
+ * returns true, if there are pending interrupts.
+ */
+bool ux500_pm_prcmu_pending_interrupt(void);
+
+/**
+ * ux500_pm_prcmu_set_ioforce()
+ *
+ * @enable: Enable/disable
+ *
+ * Enable/disable the gpio-ring
+ */
+void ux500_pm_prcmu_set_ioforce(bool enable);
+
+/**
+ * ux500_pm_prcmu_copy_gic_settings()
+ *
+ * This function copies all the gic interrupt settings to the prcmu.
+ * This is needed for the system to catch interrupts in ApIdle
+ */
+void ux500_pm_prcmu_copy_gic_settings(void);
+
+/**
+ * ux500_pm_gpio_save_wake_up_status()
+ *
+ * This function is called when the prcmu has woken the ARM
+ * but before ioforce is disabled.
+ */
+void ux500_pm_gpio_save_wake_up_status(void);
+
+/**
+ * ux500_pm_gpio_read_wake_up_status()
+ *
+ * @bank_number: The gpio bank.
+ *
+ * Returns the WKS register settings for given bank number.
+ * The WKS register is cleared when ioforce is released therefore
+ * this function is needed.
+ */
+u32 ux500_pm_gpio_read_wake_up_status(unsigned int bank_number);
+
+/**
+ * ux500_pm_other_cpu_wfi()
+ *
+ * Returns true if the other CPU is in WFI.
+ */
+bool ux500_pm_other_cpu_wfi(void);
+
+/**
+ * ux500_pm_prcmu_idle_stat()
+ *
+ * Returns the status of the last prcmu idle/sleep
+ */
+enum prcmu_idle_stat ux500_pm_prcmu_idle_stat(void);
+
+struct dev_power_domain;
+extern struct dev_power_domain ux500_dev_power_domain;
+extern struct dev_power_domain ux500_amba_dev_power_domain;
+
+#else
+u32 ux500_pm_gpio_read_wake_up_status(unsigned int bank_number)
+{
+	return 0;
+}
+
+/**
+ * ux500_pm_prcmu_set_ioforce()
+ *
+ * @enable: Enable/disable
+ *
+ * Enable/disable the gpio-ring
+ */
+static inline void ux500_pm_prcmu_set_ioforce(bool enable) { }
+
+#endif
+
+#endif
diff --git a/arch/arm/mach-ux500/pm/Kconfig b/arch/arm/mach-ux500/pm/Kconfig
index 5e126e2f954..096452d2919 100644
--- a/arch/arm/mach-ux500/pm/Kconfig
+++ b/arch/arm/mach-ux500/pm/Kconfig
@@ -4,3 +4,9 @@ config DBX500_PRCMU_QOS_POWER
 	default y
 	help
 	  Add support for PRCMU power Quality of Service
+
+config UX500_CONTEXT
+	bool "Context save/restore support for UX500"
+	depends on (UX500_SOC_DB8500 || UX500_SOC_DB5500) && PM
+	help
+	  This is needed for ApSleep and deeper sleep states.
diff --git a/arch/arm/mach-ux500/pm/Makefile b/arch/arm/mach-ux500/pm/Makefile
index 531ba964acb..34307594d14 100644
--- a/arch/arm/mach-ux500/pm/Makefile
+++ b/arch/arm/mach-ux500/pm/Makefile
@@ -1,6 +1,7 @@
 #
 # Power save related files
 #
-obj-y := runtime.o no_suspend.o
+obj-y := pm.o runtime.o no_suspend.o
 
 obj-$(CONFIG_DBX500_PRCMU_QOS_POWER) += prcmu-qos-power.o
+obj-$(CONFIG_UX500_CONTEXT) 		+= context.o context_arm.o context-db8500.o context-db5500.o
diff --git a/arch/arm/mach-ux500/pm/context-db5500.c b/arch/arm/mach-ux500/pm/context-db5500.c
new file mode 100644
index 00000000000..8075bd9e113
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/context-db5500.c
@@ -0,0 +1,404 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>,
+ *         Rickard Andersson <rickard.andersson@stericsson.com>,
+ *         Sundar Iyer <sundar.iyer@stericsson.com>,
+ *         ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#include <linux/io.h>
+
+#include <mach/hardware.h>
+#include <mach/context.h>
+
+/* These registers are DB5500 specific */
+#define NODE_HIBW1_ESRAM_IN_0_PRIORITY		0x0
+#define NODE_HIBW1_ESRAM_IN_1_PRIORITY		0x4
+
+#define NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT	0x18
+#define NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT	0x1C
+#define NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT	0x20
+
+#define NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT	0x24
+#define NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT	0x28
+#define NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT	0x2C
+
+#define NODE_HIBW1_DDR_IN_0_PRIORITY		0x400
+#define NODE_HIBW1_DDR_IN_1_PRIORITY		0x404
+#define NODE_HIBW1_DDR_IN_2_PRIORITY		0x408
+
+#define NODE_HIBW1_DDR_IN_0_LIMIT		0x424
+#define NODE_HIBW1_DDR_IN_1_LIMIT		0x428
+#define NODE_HIBW1_DDR_IN_2_LIMIT		0x42C
+
+#define NODE_HIBW1_DDR_OUT_0_PRIORITY		0x430
+
+#define NODE_HIBW2_ESRAM_IN_0_PRIORITY		0x800
+#define NODE_HIBW2_ESRAM_IN_1_PRIORITY		0x804
+
+#define NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT	0x818
+#define NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT	0x81C
+#define NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT	0x820
+
+#define NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT	0x824
+#define NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT	0x828
+#define NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT	0x82C
+
+#define NODE_HIBW2_DDR_IN_0_PRIORITY		0xC00
+#define NODE_HIBW2_DDR_IN_1_PRIORITY		0xC04
+#define NODE_HIBW2_DDR_IN_2_PRIORITY		0xC08
+#define NODE_HIBW2_DDR_IN_3_PRIORITY		0xC0C
+
+#define NODE_HIBW2_DDR_IN_0_LIMIT		0xC30
+#define NODE_HIBW2_DDR_IN_1_LIMIT		0xC34
+#define NODE_HIBW2_DDR_IN_2_LIMIT		0xC38
+#define NODE_HIBW2_DDR_IN_3_LIMIT		0xC3C
+
+#define NODE_HIBW2_DDR_OUT_0_PRIORITY		0xC40
+
+#define NODE_ESRAM0_IN_0_PRIORITY		0x1000
+#define NODE_ESRAM0_IN_1_PRIORITY		0x1004
+#define NODE_ESRAM0_IN_2_PRIORITY		0x1008
+
+#define NODE_ESRAM0_IN_0_LIMIT			0x1024
+#define NODE_ESRAM0_IN_1_LIMIT			0x1028
+#define NODE_ESRAM0_IN_2_LIMIT			0x102C
+#define NODE_ESRAM0_OUT_0_PRIORITY		0x1030
+
+#define NODE_ESRAM1_2_IN_0_PRIORITY		0x1400
+#define NODE_ESRAM1_2_IN_1_PRIORITY		0x1404
+#define NODE_ESRAM1_2_IN_2_PRIORITY		0x1408
+
+#define NODE_ESRAM1_2_IN_0_ARB_1_LIMIT		0x1424
+#define NODE_ESRAM1_2_IN_1_ARB_1_LIMIT		0x1428
+#define NODE_ESRAM1_2_IN_2_ARB_1_LIMIT		0x142C
+#define NODE_ESRAM1_2_OUT_0_PRIORITY		0x1430
+
+#define NODE_ESRAM3_4_IN_0_PRIORITY		0x1800
+#define NODE_ESRAM3_4_IN_1_PRIORITY		0x1804
+#define NODE_ESRAM3_4_IN_2_PRIORITY		0x1808
+
+#define NODE_ESRAM3_4_IN_0_ARB_1_LIMIT		0x1824
+#define NODE_ESRAM3_4_IN_1_ARB_1_LIMIT		0x1828
+#define NODE_ESRAM3_4_IN_2_ARB_1_LIMIT		0x182C
+#define NODE_ESRAM3_4_OUT_0_PRIORITY		0x1830
+
+/*
+ * Save ICN (Interconnect or Interconnect nodes) configuration registers
+ * TODO: This can be optimized, for example if we have
+ * a static ICN configuration.
+ */
+
+static struct {
+	void __iomem *base;
+	u32 hibw1_esram_in_pri[2];
+	u32 hibw1_esram_in0_arb[3];
+	u32 hibw1_esram_in1_arb[3];
+	u32 hibw1_ddr_in_prio[3];
+	u32 hibw1_ddr_in_limit[3];
+	u32 hibw1_ddr_out_prio_reg;
+
+	/* HiBw2 node registers */
+	u32 hibw2_esram_in_pri[2];
+	u32 hibw2_esram_in0_arblimit[3];
+	u32 hibw2_esram_in1_arblimit[3];
+	u32 hibw2_ddr_in_prio[4];
+	u32 hibw2_ddr_in_limit[4];
+	u32 hibw2_ddr_out_prio_reg;
+
+	/* ESRAM node registers */
+	u32 esram_in_prio[3];
+	u32 esram_in_lim[3];
+	u32 esram_out_prio_reg;
+
+	u32 esram12_in_prio[3];
+	u32 esram12_in_arb_lim[3];
+	u32 esram12_out_prio_reg;
+
+	u32 esram34_in_prio[3];
+	u32 esram34_in_arb_lim[3];
+	u32 esram34_out_prio;
+} context_icn;
+
+
+void u5500_context_save_icn(void)
+{
+	/* hibw1 */
+	context_icn.hibw1_esram_in_pri[0] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_0_PRIORITY);
+	context_icn.hibw1_esram_in_pri[1] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_1_PRIORITY);
+
+	context_icn.hibw1_esram_in0_arb[0] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT);
+	context_icn.hibw1_esram_in0_arb[1] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT);
+	context_icn.hibw1_esram_in0_arb[2] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT);
+
+	context_icn.hibw1_esram_in1_arb[0] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT);
+	context_icn.hibw1_esram_in1_arb[1] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT);
+	context_icn.hibw1_esram_in1_arb[2] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT);
+
+	context_icn.hibw1_ddr_in_prio[0] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_0_PRIORITY);
+	context_icn.hibw1_ddr_in_prio[1] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_1_PRIORITY);
+	context_icn.hibw1_ddr_in_prio[2] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_2_PRIORITY);
+
+	context_icn.hibw1_ddr_in_limit[0] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_0_LIMIT);
+	context_icn.hibw1_ddr_in_limit[1] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_1_LIMIT);
+	context_icn.hibw1_ddr_in_limit[2] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_2_LIMIT);
+
+	context_icn.hibw1_ddr_out_prio_reg =
+		readl(context_icn.base + NODE_HIBW1_DDR_OUT_0_PRIORITY);
+
+	/* hibw2 */
+	context_icn.hibw2_esram_in_pri[0] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_0_PRIORITY);
+	context_icn.hibw2_esram_in_pri[1] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_1_PRIORITY);
+
+	context_icn.hibw2_esram_in0_arblimit[0] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT);
+	context_icn.hibw2_esram_in0_arblimit[1] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT);
+	context_icn.hibw2_esram_in0_arblimit[2] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT);
+
+	context_icn.hibw2_esram_in1_arblimit[0] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT);
+	context_icn.hibw2_esram_in1_arblimit[1] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT);
+	context_icn.hibw2_esram_in1_arblimit[2] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT);
+
+	context_icn.hibw2_ddr_in_prio[0] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_0_PRIORITY);
+	context_icn.hibw2_ddr_in_prio[1] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_1_PRIORITY);
+	context_icn.hibw2_ddr_in_prio[2] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_2_PRIORITY);
+	context_icn.hibw2_ddr_in_prio[3] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_3_PRIORITY);
+
+	context_icn.hibw2_ddr_in_limit[0] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_0_LIMIT);
+	context_icn.hibw2_ddr_in_limit[1] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_1_LIMIT);
+	context_icn.hibw2_ddr_in_limit[2] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_2_LIMIT);
+	context_icn.hibw2_ddr_in_limit[3] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_3_LIMIT);
+
+	context_icn.hibw2_ddr_out_prio_reg =
+		readl(context_icn.base + NODE_HIBW2_DDR_OUT_0_PRIORITY);
+
+	/* ESRAM0 */
+	context_icn.esram_in_prio[0] =
+		readl(context_icn.base + NODE_ESRAM0_IN_0_PRIORITY);
+	context_icn.esram_in_prio[1] =
+		readl(context_icn.base + NODE_ESRAM0_IN_1_PRIORITY);
+	context_icn.esram_in_prio[2] =
+		readl(context_icn.base + NODE_ESRAM0_IN_2_PRIORITY);
+
+	context_icn.esram_in_lim[0] =
+		readl(context_icn.base + NODE_ESRAM0_IN_0_LIMIT);
+	context_icn.esram_in_lim[1] =
+		readl(context_icn.base + NODE_ESRAM0_IN_1_LIMIT);
+	context_icn.esram_in_lim[2] =
+		readl(context_icn.base + NODE_ESRAM0_IN_2_LIMIT);
+
+	context_icn.esram_out_prio_reg =
+		readl(context_icn.base + NODE_ESRAM0_OUT_0_PRIORITY);
+
+	/* ESRAM1-2 */
+	context_icn.esram12_in_prio[0] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_0_PRIORITY);
+	context_icn.esram12_in_prio[1] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_1_PRIORITY);
+	context_icn.esram12_in_prio[2] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_2_PRIORITY);
+
+	context_icn.esram12_in_arb_lim[0] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_0_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[1] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_1_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[2] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_2_ARB_1_LIMIT);
+
+	context_icn.esram12_out_prio_reg =
+		readl(context_icn.base + NODE_ESRAM1_2_OUT_0_PRIORITY);
+
+	/* ESRAM3-4 */
+	context_icn.esram34_in_prio[0] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_0_PRIORITY);
+	context_icn.esram34_in_prio[1] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_1_PRIORITY);
+	context_icn.esram34_in_prio[2] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_2_PRIORITY);
+
+	context_icn.esram34_in_arb_lim[0] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_0_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[1] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_1_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[2] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_2_ARB_1_LIMIT);
+
+	context_icn.esram34_out_prio =
+		readl(context_icn.base + NODE_ESRAM3_4_OUT_0_PRIORITY);
+}
+
+/*
+ * Restore ICN configuration registers
+ */
+void u5500_context_restore_icn(void)
+{
+
+	/* hibw1 */
+	writel(context_icn.hibw1_esram_in_pri[0],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_0_PRIORITY);
+	writel(context_icn.hibw1_esram_in_pri[1],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_1_PRIORITY);
+
+	writel(context_icn.hibw1_esram_in0_arb[0],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT);
+	writel(context_icn.hibw1_esram_in0_arb[1],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT);
+	writel(context_icn.hibw1_esram_in0_arb[2],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT);
+
+	writel(context_icn.hibw1_esram_in1_arb[0],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT);
+	writel(context_icn.hibw1_esram_in1_arb[1],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT);
+	writel(context_icn.hibw1_esram_in1_arb[2],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT);
+
+	writel(context_icn.hibw1_ddr_in_prio[0],
+	       context_icn.base + NODE_HIBW1_DDR_IN_0_PRIORITY);
+	writel(context_icn.hibw1_ddr_in_prio[1],
+	       context_icn.base + NODE_HIBW1_DDR_IN_1_PRIORITY);
+	writel(context_icn.hibw1_ddr_in_prio[2],
+	       context_icn.base + NODE_HIBW1_DDR_IN_2_PRIORITY);
+
+	writel(context_icn.hibw1_ddr_in_limit[0],
+	       context_icn.base + NODE_HIBW1_DDR_IN_0_LIMIT);
+	writel(context_icn.hibw1_ddr_in_limit[1],
+	       context_icn.base + NODE_HIBW1_DDR_IN_1_LIMIT);
+	writel(context_icn.hibw1_ddr_in_limit[2],
+	       context_icn.base + NODE_HIBW1_DDR_IN_2_LIMIT);
+
+	writel(context_icn.hibw1_ddr_out_prio_reg,
+	       context_icn.base + NODE_HIBW1_DDR_OUT_0_PRIORITY);
+
+	/* hibw2 */
+	writel(context_icn.hibw2_esram_in_pri[0],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_0_PRIORITY);
+	writel(context_icn.hibw2_esram_in_pri[1],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_1_PRIORITY);
+
+	writel(context_icn.hibw2_esram_in0_arblimit[0],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT);
+	writel(context_icn.hibw2_esram_in0_arblimit[1],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT);
+	writel(context_icn.hibw2_esram_in0_arblimit[2],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT);
+
+	writel(context_icn.hibw2_esram_in1_arblimit[0],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT);
+	writel(context_icn.hibw2_esram_in1_arblimit[1],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT);
+	writel(context_icn.hibw2_esram_in1_arblimit[2],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT);
+
+	writel(context_icn.hibw2_ddr_in_prio[0],
+	       context_icn.base + NODE_HIBW2_DDR_IN_0_PRIORITY);
+	writel(context_icn.hibw2_ddr_in_prio[1],
+	       context_icn.base + NODE_HIBW2_DDR_IN_1_PRIORITY);
+	writel(context_icn.hibw2_ddr_in_prio[2],
+	       context_icn.base + NODE_HIBW2_DDR_IN_2_PRIORITY);
+	writel(context_icn.hibw2_ddr_in_prio[3],
+	       context_icn.base + NODE_HIBW2_DDR_IN_3_PRIORITY);
+
+	writel(context_icn.hibw2_ddr_in_limit[0],
+	       context_icn.base + NODE_HIBW2_DDR_IN_0_LIMIT);
+	writel(context_icn.hibw2_ddr_in_limit[1],
+	       context_icn.base + NODE_HIBW2_DDR_IN_1_LIMIT);
+	writel(context_icn.hibw2_ddr_in_limit[2],
+	       context_icn.base + NODE_HIBW2_DDR_IN_2_LIMIT);
+	writel(context_icn.hibw2_ddr_in_limit[3],
+	       context_icn.base + NODE_HIBW2_DDR_IN_3_LIMIT);
+
+	writel(context_icn.hibw2_ddr_out_prio_reg,
+	       context_icn.base + NODE_HIBW2_DDR_OUT_0_PRIORITY);
+
+	/* ESRAM0 */
+	writel(context_icn.esram_in_prio[0],
+	       context_icn.base + NODE_ESRAM0_IN_0_PRIORITY);
+	writel(context_icn.esram_in_prio[1],
+	       context_icn.base + NODE_ESRAM0_IN_1_PRIORITY);
+	writel(context_icn.esram_in_prio[2],
+	       context_icn.base + NODE_ESRAM0_IN_2_PRIORITY);
+
+	writel(context_icn.esram_in_lim[0],
+	       context_icn.base + NODE_ESRAM0_IN_0_LIMIT);
+	writel(context_icn.esram_in_lim[1],
+	       context_icn.base + NODE_ESRAM0_IN_1_LIMIT);
+	writel(context_icn.esram_in_lim[2],
+	       context_icn.base + NODE_ESRAM0_IN_2_LIMIT);
+
+	writel(context_icn.esram_out_prio_reg,
+	       context_icn.base + NODE_ESRAM0_OUT_0_PRIORITY);
+
+	/* ESRAM1-2 */
+	writel(context_icn.esram12_in_prio[0],
+	       context_icn.base + NODE_ESRAM1_2_IN_0_PRIORITY);
+	writel(context_icn.esram12_in_prio[1],
+	       context_icn.base + NODE_ESRAM1_2_IN_1_PRIORITY);
+	writel(context_icn.esram12_in_prio[2],
+	       context_icn.base + NODE_ESRAM1_2_IN_2_PRIORITY);
+
+	writel(context_icn.esram12_in_arb_lim[0],
+	       context_icn.base + NODE_ESRAM1_2_IN_0_ARB_1_LIMIT);
+	writel(context_icn.esram12_in_arb_lim[1],
+	       context_icn.base + NODE_ESRAM1_2_IN_1_ARB_1_LIMIT);
+	writel(context_icn.esram12_in_arb_lim[2],
+	       context_icn.base + NODE_ESRAM1_2_IN_2_ARB_1_LIMIT);
+
+	writel(context_icn.esram12_out_prio_reg,
+	       context_icn.base + NODE_ESRAM1_2_OUT_0_PRIORITY);
+
+	/* ESRAM3-4 */
+	writel(context_icn.esram34_in_prio[0],
+	       context_icn.base + NODE_ESRAM3_4_IN_0_PRIORITY);
+	writel(context_icn.esram34_in_prio[1],
+	       context_icn.base + NODE_ESRAM3_4_IN_1_PRIORITY);
+	writel(context_icn.esram34_in_prio[2],
+	       context_icn.base + NODE_ESRAM3_4_IN_2_PRIORITY);
+
+	writel(context_icn.esram34_in_arb_lim[0],
+	       context_icn.base + NODE_ESRAM3_4_IN_0_ARB_1_LIMIT);
+	writel(context_icn.esram34_in_arb_lim[1],
+	       context_icn.base + NODE_ESRAM3_4_IN_1_ARB_1_LIMIT);
+	writel(context_icn.esram34_in_arb_lim[2],
+	       context_icn.base + NODE_ESRAM3_4_IN_2_ARB_1_LIMIT);
+
+	writel(context_icn.esram34_out_prio,
+	       context_icn.base + NODE_ESRAM3_4_OUT_0_PRIORITY);
+
+}
+
+void u5500_context_init(void)
+{
+	context_icn.base = ioremap(U5500_ICN_BASE, SZ_8K);
+}
diff --git a/arch/arm/mach-ux500/pm/context-db8500.c b/arch/arm/mach-ux500/pm/context-db8500.c
new file mode 100644
index 00000000000..3ba73e51a6d
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/context-db8500.c
@@ -0,0 +1,456 @@
+/*
+ * Copyright (C) STMicroelectronics 2009
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Sundar Iyer for ST-Ericsson
+ *
+ */
+
+#include <linux/io.h>
+
+#include <mach/hardware.h>
+#include <mach/context.h>
+
+/*
+ * ST-Interconnect context
+ */
+
+/* priority, bw limiter register offsets */
+#define NODE_HIBW1_ESRAM_IN_0_PRIORITY		0x00
+#define NODE_HIBW1_ESRAM_IN_1_PRIORITY		0x04
+#define NODE_HIBW1_ESRAM_IN_2_PRIORITY		0x08
+#define NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT	0x24
+#define NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT	0x28
+#define NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT	0x2C
+#define NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT	0x30
+#define NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT	0x34
+#define NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT	0x38
+#define NODE_HIBW1_ESRAM_IN_2_ARB_1_LIMIT	0x3C
+#define NODE_HIBW1_ESRAM_IN_2_ARB_2_LIMIT	0x40
+#define NODE_HIBW1_ESRAM_IN_2_ARB_3_LIMIT	0x44
+#define NODE_HIBW1_DDR_IN_0_PRIORITY		0x400
+#define NODE_HIBW1_DDR_IN_1_PRIORITY		0x404
+#define NODE_HIBW1_DDR_IN_2_PRIORITY		0x408
+#define NODE_HIBW1_DDR_IN_0_LIMIT		0x424
+#define NODE_HIBW1_DDR_IN_1_LIMIT		0x428
+#define NODE_HIBW1_DDR_IN_2_LIMIT		0x42C
+#define NODE_HIBW1_DDR_OUT_0_PRIORITY		0x430
+#define NODE_HIBW2_ESRAM_IN_0_PRIORITY		0x800
+#define NODE_HIBW2_ESRAM_IN_1_PRIORITY		0x804
+#define NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT	0x818
+#define NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT	0x81C
+#define NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT	0x820
+#define NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT	0x824
+#define NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT	0x828
+#define NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT	0x82C
+#define NODE_HIBW2_DDR_IN_0_PRIORITY		0xC00
+#define NODE_HIBW2_DDR_IN_1_PRIORITY		0xC04
+#define NODE_HIBW2_DDR_IN_2_PRIORITY		0xC08
+
+#define NODE_HIBW2_DDR_IN_0_LIMIT		0xC24
+#define NODE_HIBW2_DDR_IN_1_LIMIT		0xC28
+#define NODE_HIBW2_DDR_IN_2_LIMIT		0xC2C
+#define NODE_HIBW2_DDR_OUT_0_PRIORITY		0xC30
+
+/*
+ * Note the following addresses are presented in
+ * db8500 design spec v3.1 and v3.3, table 10.
+ * But their addresses are not the same as in the
+ * description. The addresses in the description
+ * of each registers are correct.
+ * NODE_HIBW2_DDR_IN_3_LIMIT is only present in v1.
+ *
+ * Faulty registers addresses in table 10:
+ * NODE_HIBW2_DDR_IN_2_LIMIT 0xC38
+ * NODE_HIBW2_DDR_IN_3_LIMIT 0xC3C
+ * NODE_HIBW2_DDR_OUT_0_PRIORITY 0xC40
+ */
+
+#define NODE_ESRAM0_IN_0_PRIORITY		0x1000
+#define NODE_ESRAM0_IN_1_PRIORITY		0x1004
+#define NODE_ESRAM0_IN_2_PRIORITY		0x1008
+#define NODE_ESRAM0_IN_3_PRIORITY		0x100C
+#define NODE_ESRAM0_IN_0_LIMIT			0x1030
+#define NODE_ESRAM0_IN_1_LIMIT			0x1034
+#define NODE_ESRAM0_IN_2_LIMIT			0x1038
+#define NODE_ESRAM0_IN_3_LIMIT			0x103C
+/* common */
+#define NODE_ESRAM1_2_IN_0_PRIORITY		0x1400
+#define NODE_ESRAM1_2_IN_1_PRIORITY		0x1404
+#define NODE_ESRAM1_2_IN_2_PRIORITY		0x1408
+#define NODE_ESRAM1_2_IN_3_PRIORITY		0x140C
+#define NODE_ESRAM1_2_IN_0_ARB_1_LIMIT		0x1430
+#define NODE_ESRAM1_2_IN_0_ARB_2_LIMIT		0x1434
+#define NODE_ESRAM1_2_IN_1_ARB_1_LIMIT		0x1438
+#define NODE_ESRAM1_2_IN_1_ARB_2_LIMIT		0x143C
+#define NODE_ESRAM1_2_IN_2_ARB_1_LIMIT		0x1440
+#define NODE_ESRAM1_2_IN_2_ARB_2_LIMIT		0x1444
+#define NODE_ESRAM1_2_IN_3_ARB_1_LIMIT		0x1448
+#define NODE_ESRAM1_2_IN_3_ARB_2_LIMIT		0x144C
+
+#define NODE_ESRAM3_4_IN_0_PRIORITY		0x1800
+#define NODE_ESRAM3_4_IN_1_PRIORITY		0x1804
+#define NODE_ESRAM3_4_IN_2_PRIORITY		0x1808
+#define NODE_ESRAM3_4_IN_3_PRIORITY		0x180C
+#define NODE_ESRAM3_4_IN_0_ARB_1_LIMIT		0x1830
+#define NODE_ESRAM3_4_IN_0_ARB_2_LIMIT		0x1834
+#define NODE_ESRAM3_4_IN_1_ARB_1_LIMIT		0x1838
+#define NODE_ESRAM3_4_IN_1_ARB_2_LIMIT		0x183C
+#define NODE_ESRAM3_4_IN_2_ARB_1_LIMIT		0x1840
+#define NODE_ESRAM3_4_IN_2_ARB_2_LIMIT		0x1844
+#define NODE_ESRAM3_4_IN_3_ARB_1_LIMIT		0x1848
+#define NODE_ESRAM3_4_IN_3_ARB_2_LIMIT		0x184C
+
+static struct {
+	void __iomem *base;
+	u32 hibw1_esram_in_pri[3];
+	u32 hibw1_esram_in0_arb[3];
+	u32 hibw1_esram_in1_arb[3];
+	u32 hibw1_esram_in2_arb[3];
+	u32 hibw1_ddr_in_prio[3];
+	u32 hibw1_ddr_in_limit[3];
+	u32 hibw1_ddr_out_prio;
+
+	/* HiBw2 node registers */
+	u32 hibw2_esram_in_pri[2];
+	u32 hibw2_esram_in0_arblimit[3];
+	u32 hibw2_esram_in1_arblimit[3];
+	u32 hibw2_ddr_in_prio[4];
+	u32 hibw2_ddr_in_limit[4];
+	u32 hibw2_ddr_out_prio;
+
+	/* ESRAM node registers */
+	u32 esram_in_prio[4];
+	u32 esram_in_lim[4];
+	u32 esram0_in_prio[4];
+	u32 esram0_in_lim[4];
+	u32 esram12_in_prio[4];
+	u32 esram12_in_arb_lim[8];
+	u32 esram34_in_prio[4];
+	u32 esram34_in_arb_lim[8];
+} context_icn;
+
+/**
+ * u8500_context_save_icn() - save ICN context
+ *
+ */
+void u8500_context_save_icn(void)
+{
+	void __iomem *b = context_icn.base;
+
+	context_icn.hibw1_esram_in_pri[0] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_0_PRIORITY);
+	context_icn.hibw1_esram_in_pri[1] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_1_PRIORITY);
+	context_icn.hibw1_esram_in_pri[2] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_2_PRIORITY);
+
+	context_icn.hibw1_esram_in0_arb[0] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT);
+	context_icn.hibw1_esram_in0_arb[1] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT);
+	context_icn.hibw1_esram_in0_arb[2] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT);
+
+	context_icn.hibw1_esram_in1_arb[0] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT);
+	context_icn.hibw1_esram_in1_arb[1] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT);
+	context_icn.hibw1_esram_in1_arb[2] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT);
+
+	context_icn.hibw1_esram_in2_arb[0] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_2_ARB_1_LIMIT);
+	context_icn.hibw1_esram_in2_arb[1] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_2_ARB_2_LIMIT);
+	context_icn.hibw1_esram_in2_arb[2] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_2_ARB_3_LIMIT);
+
+	context_icn.hibw1_ddr_in_prio[0] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_0_PRIORITY);
+	context_icn.hibw1_ddr_in_prio[1] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_1_PRIORITY);
+	context_icn.hibw1_ddr_in_prio[2] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_2_PRIORITY);
+
+	context_icn.hibw1_ddr_in_limit[0] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_0_LIMIT);
+	context_icn.hibw1_ddr_in_limit[1] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_1_LIMIT);
+	context_icn.hibw1_ddr_in_limit[2] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_2_LIMIT);
+
+	context_icn.hibw1_ddr_out_prio =
+		readl_relaxed(b + NODE_HIBW1_DDR_OUT_0_PRIORITY);
+
+	context_icn.hibw2_esram_in_pri[0] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_0_PRIORITY);
+	context_icn.hibw2_esram_in_pri[1] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_1_PRIORITY);
+
+	context_icn.hibw2_esram_in0_arblimit[0] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT);
+	context_icn.hibw2_esram_in0_arblimit[1] =
+		 readl_relaxed(b + NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT);
+	context_icn.hibw2_esram_in0_arblimit[2] =
+		 readl_relaxed(b + NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT);
+
+	context_icn.hibw2_esram_in1_arblimit[0] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT);
+	context_icn.hibw2_esram_in1_arblimit[1] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT);
+	context_icn.hibw2_esram_in1_arblimit[2] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT);
+
+	context_icn.hibw2_ddr_in_prio[0] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_0_PRIORITY);
+	context_icn.hibw2_ddr_in_prio[1] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_1_PRIORITY);
+	context_icn.hibw2_ddr_in_prio[2] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_2_PRIORITY);
+
+	context_icn.hibw2_ddr_in_limit[0] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_0_LIMIT);
+	context_icn.hibw2_ddr_in_limit[1] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_1_LIMIT);
+
+	context_icn.hibw2_ddr_in_limit[2] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_2_LIMIT);
+
+	context_icn.hibw2_ddr_out_prio =
+		readl_relaxed(b + NODE_HIBW2_DDR_OUT_0_PRIORITY);
+
+	context_icn.esram0_in_prio[0] =
+		readl_relaxed(b + NODE_ESRAM0_IN_0_PRIORITY);
+	context_icn.esram0_in_prio[1] =
+		readl_relaxed(b + NODE_ESRAM0_IN_1_PRIORITY);
+	context_icn.esram0_in_prio[2] =
+		readl_relaxed(b + NODE_ESRAM0_IN_2_PRIORITY);
+	context_icn.esram0_in_prio[3] =
+		readl_relaxed(b + NODE_ESRAM0_IN_3_PRIORITY);
+
+	context_icn.esram0_in_lim[0] =
+		readl_relaxed(b + NODE_ESRAM0_IN_0_LIMIT);
+	context_icn.esram0_in_lim[1] =
+		readl_relaxed(b + NODE_ESRAM0_IN_1_LIMIT);
+	context_icn.esram0_in_lim[2] =
+		readl_relaxed(b + NODE_ESRAM0_IN_2_LIMIT);
+	context_icn.esram0_in_lim[3] =
+		readl_relaxed(b + NODE_ESRAM0_IN_3_LIMIT);
+
+	context_icn.esram12_in_prio[0] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_0_PRIORITY);
+	context_icn.esram12_in_prio[1] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_1_PRIORITY);
+	context_icn.esram12_in_prio[2] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_2_PRIORITY);
+	context_icn.esram12_in_prio[3] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_3_PRIORITY);
+
+	context_icn.esram12_in_arb_lim[0] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_0_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[1] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_0_ARB_2_LIMIT);
+	context_icn.esram12_in_arb_lim[2] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_1_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[3] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_1_ARB_2_LIMIT);
+	context_icn.esram12_in_arb_lim[4] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_2_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[5] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_2_ARB_2_LIMIT);
+	context_icn.esram12_in_arb_lim[6] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_3_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[7] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_3_ARB_2_LIMIT);
+
+	context_icn.esram34_in_prio[0] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_0_PRIORITY);
+	context_icn.esram34_in_prio[1] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_1_PRIORITY);
+	context_icn.esram34_in_prio[2] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_2_PRIORITY);
+	context_icn.esram34_in_prio[3] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_3_PRIORITY);
+
+	context_icn.esram34_in_arb_lim[0] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_0_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[1] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_0_ARB_2_LIMIT);
+	context_icn.esram34_in_arb_lim[2] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_1_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[3] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_1_ARB_2_LIMIT);
+	context_icn.esram34_in_arb_lim[4] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_2_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[5] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_2_ARB_2_LIMIT);
+	context_icn.esram34_in_arb_lim[6] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_3_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[7] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_3_ARB_2_LIMIT);
+}
+
+/**
+ * u8500_context_restore_icn() - restore ICN context
+ *
+ */
+void u8500_context_restore_icn(void)
+{
+	void __iomem *b = context_icn.base;
+
+	writel_relaxed(context_icn.hibw1_esram_in_pri[0],
+		       b + NODE_HIBW1_ESRAM_IN_0_PRIORITY);
+	writel_relaxed(context_icn.hibw1_esram_in_pri[1],
+		       b + NODE_HIBW1_ESRAM_IN_1_PRIORITY);
+	writel_relaxed(context_icn.hibw1_esram_in_pri[2],
+		       b + NODE_HIBW1_ESRAM_IN_2_PRIORITY);
+
+	writel_relaxed(context_icn.hibw1_esram_in0_arb[0],
+		       b + NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in0_arb[1],
+		       b + NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in0_arb[2],
+		       b + NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT);
+
+	writel_relaxed(context_icn.hibw1_esram_in1_arb[0],
+		       b + NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in1_arb[1],
+		       b + NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in1_arb[2],
+		       b + NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT);
+
+	writel_relaxed(context_icn.hibw1_esram_in2_arb[0],
+		       b + NODE_HIBW1_ESRAM_IN_2_ARB_1_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in2_arb[1],
+		       b + NODE_HIBW1_ESRAM_IN_2_ARB_2_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in2_arb[2],
+		       b + NODE_HIBW1_ESRAM_IN_2_ARB_3_LIMIT);
+
+	writel_relaxed(context_icn.hibw1_ddr_in_prio[0],
+		       b + NODE_HIBW1_DDR_IN_0_PRIORITY);
+	writel_relaxed(context_icn.hibw1_ddr_in_prio[1],
+		       b + NODE_HIBW1_DDR_IN_1_PRIORITY);
+	writel_relaxed(context_icn.hibw1_ddr_in_prio[2],
+		       b + NODE_HIBW1_DDR_IN_2_PRIORITY);
+
+	writel_relaxed(context_icn.hibw1_ddr_in_limit[0],
+		       b + NODE_HIBW1_DDR_IN_0_LIMIT);
+	writel_relaxed(context_icn.hibw1_ddr_in_limit[1],
+		       b + NODE_HIBW1_DDR_IN_1_LIMIT);
+	writel_relaxed(context_icn.hibw1_ddr_in_limit[2],
+		       b + NODE_HIBW1_DDR_IN_2_LIMIT);
+
+	writel_relaxed(context_icn.hibw1_ddr_out_prio,
+		       b + NODE_HIBW1_DDR_OUT_0_PRIORITY);
+
+	writel_relaxed(context_icn.hibw2_esram_in_pri[0],
+		       b + NODE_HIBW2_ESRAM_IN_0_PRIORITY);
+	writel_relaxed(context_icn.hibw2_esram_in_pri[1],
+		       b + NODE_HIBW2_ESRAM_IN_1_PRIORITY);
+
+	writel_relaxed(context_icn.hibw2_esram_in0_arblimit[0],
+		       b + NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT);
+	writel_relaxed(context_icn.hibw2_esram_in0_arblimit[1],
+		       b + NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT);
+	writel_relaxed(context_icn.hibw2_esram_in0_arblimit[2],
+		       b + NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT);
+
+	writel_relaxed(context_icn.hibw2_esram_in1_arblimit[0],
+		       b + NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT);
+	writel_relaxed(context_icn.hibw2_esram_in1_arblimit[1],
+		       b + NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT);
+	writel_relaxed(context_icn.hibw2_esram_in1_arblimit[2],
+		       b + NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT);
+
+	writel_relaxed(context_icn.hibw2_ddr_in_prio[0],
+		       b + NODE_HIBW2_DDR_IN_0_PRIORITY);
+	writel_relaxed(context_icn.hibw2_ddr_in_prio[1],
+		       b + NODE_HIBW2_DDR_IN_1_PRIORITY);
+	writel_relaxed(context_icn.hibw2_ddr_in_prio[2],
+		       b + NODE_HIBW2_DDR_IN_2_PRIORITY);
+	writel_relaxed(context_icn.hibw2_ddr_in_limit[0],
+		       b + NODE_HIBW2_DDR_IN_0_LIMIT);
+	writel_relaxed(context_icn.hibw2_ddr_in_limit[1],
+		       b + NODE_HIBW2_DDR_IN_1_LIMIT);
+	writel_relaxed(context_icn.hibw2_ddr_in_limit[2],
+		       b + NODE_HIBW2_DDR_IN_2_LIMIT);
+	writel_relaxed(context_icn.hibw2_ddr_out_prio,
+		       b + NODE_HIBW2_DDR_OUT_0_PRIORITY);
+
+	writel_relaxed(context_icn.esram0_in_prio[0],
+		       b + NODE_ESRAM0_IN_0_PRIORITY);
+	writel_relaxed(context_icn.esram0_in_prio[1],
+		       b + NODE_ESRAM0_IN_1_PRIORITY);
+	writel_relaxed(context_icn.esram0_in_prio[2],
+		       b + NODE_ESRAM0_IN_2_PRIORITY);
+	writel_relaxed(context_icn.esram0_in_prio[3],
+		       b + NODE_ESRAM0_IN_3_PRIORITY);
+
+	writel_relaxed(context_icn.esram0_in_lim[0],
+		       b + NODE_ESRAM0_IN_0_LIMIT);
+	writel_relaxed(context_icn.esram0_in_lim[1],
+		       b + NODE_ESRAM0_IN_1_LIMIT);
+	writel_relaxed(context_icn.esram0_in_lim[2],
+		       b + NODE_ESRAM0_IN_2_LIMIT);
+	writel_relaxed(context_icn.esram0_in_lim[3],
+		       b + NODE_ESRAM0_IN_3_LIMIT);
+
+	writel_relaxed(context_icn.esram12_in_prio[0],
+		       b + NODE_ESRAM1_2_IN_0_PRIORITY);
+	writel_relaxed(context_icn.esram12_in_prio[1],
+		       b + NODE_ESRAM1_2_IN_1_PRIORITY);
+	writel_relaxed(context_icn.esram12_in_prio[2],
+		       b + NODE_ESRAM1_2_IN_2_PRIORITY);
+	writel_relaxed(context_icn.esram12_in_prio[3],
+		       b + NODE_ESRAM1_2_IN_3_PRIORITY);
+
+	writel_relaxed(context_icn.esram12_in_arb_lim[0],
+		       b + NODE_ESRAM1_2_IN_0_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[1],
+		       b + NODE_ESRAM1_2_IN_0_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[2],
+		       b + NODE_ESRAM1_2_IN_1_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[3],
+		       b + NODE_ESRAM1_2_IN_1_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[4],
+		       b + NODE_ESRAM1_2_IN_2_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[5],
+		       b + NODE_ESRAM1_2_IN_2_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[6],
+		       b + NODE_ESRAM1_2_IN_3_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[7],
+		       b + NODE_ESRAM1_2_IN_3_ARB_2_LIMIT);
+
+	writel_relaxed(context_icn.esram34_in_prio[0],
+		       b + NODE_ESRAM3_4_IN_0_PRIORITY);
+	writel_relaxed(context_icn.esram34_in_prio[1],
+		       b + NODE_ESRAM3_4_IN_1_PRIORITY);
+	writel_relaxed(context_icn.esram34_in_prio[2],
+		       b + NODE_ESRAM3_4_IN_2_PRIORITY);
+	writel_relaxed(context_icn.esram34_in_prio[3],
+		       b + NODE_ESRAM3_4_IN_3_PRIORITY);
+
+	writel_relaxed(context_icn.esram34_in_arb_lim[0],
+		       b + NODE_ESRAM3_4_IN_0_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[1],
+		       b + NODE_ESRAM3_4_IN_0_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[2],
+		       b + NODE_ESRAM3_4_IN_1_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[3],
+		       b + NODE_ESRAM3_4_IN_1_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[4],
+		       b + NODE_ESRAM3_4_IN_2_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[5],
+		       b + NODE_ESRAM3_4_IN_2_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[6],
+		       b + NODE_ESRAM3_4_IN_3_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[7],
+		       b + NODE_ESRAM3_4_IN_3_ARB_2_LIMIT);
+}
+
+void u8500_context_init(void)
+{
+	context_icn.base = ioremap(U8500_ICN_BASE, SZ_8K);
+}
diff --git a/arch/arm/mach-ux500/pm/context.c b/arch/arm/mach-ux500/pm/context.c
new file mode 100644
index 00000000000..ad794a53d35
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/context.c
@@ -0,0 +1,987 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>,
+ *         Rickard Andersson <rickard.andersson@stericsson.com>,
+ *         Jonas Aaberg <jonas.aberg@stericsson.com>,
+ *         Sundar Iyer for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/smp.h>
+#include <linux/percpu.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/notifier.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/gpio/nomadik.h>
+
+#include <mach/hardware.h>
+#include <mach/irqs.h>
+#include <mach/pm.h>
+#include <mach/context.h>
+
+#include <asm/hardware/gic.h>
+
+#include "scu.h"
+#include "../product.h"
+
+#define GPIO_NUM_BANKS 9
+#define GPIO_NUM_SAVE_REGISTERS 7
+
+/*
+ * TODO:
+ * - Use the "UX500*"-macros instead where possible
+ */
+
+#define U8500_BACKUPRAM_SIZE SZ_64K
+
+#define U8500_PUBLIC_BOOT_ROM_BASE (U8500_BOOT_ROM_BASE + 0x17000)
+#define U5500_PUBLIC_BOOT_ROM_BASE (U5500_BOOT_ROM_BASE + 0x18000)
+
+/*
+ * Special dedicated addresses in backup RAM.  The 5500 addresses are identical
+ * to the 8500 ones.
+ */
+#define U8500_EXT_RAM_LOC_BACKUPRAM_ADDR		0x80151FDC
+#define U8500_CPU0_CP15_CR_BACKUPRAM_ADDR		0x80151F80
+#define U8500_CPU1_CP15_CR_BACKUPRAM_ADDR		0x80151FA0
+
+#define U8500_CPU0_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR	0x80151FD8
+#define U8500_CPU1_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR	0x80151FE0
+
+#define GIC_DIST_ENABLE_NS 0x0
+
+/* 32 interrupts fits in 4 bytes */
+#define GIC_DIST_ENABLE_SET_COMMON_NUM ((DBX500_NR_INTERNAL_IRQS - \
+					 IRQ_SHPI_START) / 32)
+#define GIC_DIST_ENABLE_SET_CPU_NUM (IRQ_SHPI_START / 32)
+#define GIC_DIST_ENABLE_SET_SPI0 GIC_DIST_ENABLE_SET
+#define GIC_DIST_ENABLE_SET_SPI32 (GIC_DIST_ENABLE_SET + IRQ_SHPI_START / 8)
+
+#define GIC_DIST_ENABLE_CLEAR_0 GIC_DIST_ENABLE_CLEAR
+#define GIC_DIST_ENABLE_CLEAR_32 (GIC_DIST_ENABLE_CLEAR + 4)
+#define GIC_DIST_ENABLE_CLEAR_64 (GIC_DIST_ENABLE_CLEAR + 8)
+#define GIC_DIST_ENABLE_CLEAR_96 (GIC_DIST_ENABLE_CLEAR + 12)
+#define GIC_DIST_ENABLE_CLEAR_128 (GIC_DIST_ENABLE_CLEAR + 16)
+
+#define GIC_DIST_PRI_COMMON_NUM ((DBX500_NR_INTERNAL_IRQS - IRQ_SHPI_START) / 4)
+#define GIC_DIST_PRI_CPU_NUM (IRQ_SHPI_START / 4)
+#define GIC_DIST_PRI_SPI0 GIC_DIST_PRI
+#define GIC_DIST_PRI_SPI32 (GIC_DIST_PRI + IRQ_SHPI_START)
+
+#define GIC_DIST_SPI_TARGET_COMMON_NUM ((DBX500_NR_INTERNAL_IRQS - \
+					 IRQ_SHPI_START) / 4)
+#define GIC_DIST_SPI_TARGET_CPU_NUM (IRQ_SHPI_START / 4)
+#define GIC_DIST_SPI_TARGET_SPI0 GIC_DIST_TARGET
+#define GIC_DIST_SPI_TARGET_SPI32 (GIC_DIST_TARGET + IRQ_SHPI_START)
+
+/* 16 interrupts per 4 bytes */
+#define GIC_DIST_CONFIG_COMMON_NUM ((DBX500_NR_INTERNAL_IRQS - IRQ_SHPI_START) \
+				    / 16)
+#define GIC_DIST_CONFIG_CPU_NUM (IRQ_SHPI_START / 16)
+#define GIC_DIST_CONFIG_SPI0 GIC_DIST_CONFIG
+#define GIC_DIST_CONFIG_SPI32 (GIC_DIST_CONFIG + IRQ_SHPI_START / 4)
+
+/* TODO! Move STM reg offsets to suitable place */
+#define STM_CR_OFFSET	0x00
+#define STM_MMC_OFFSET	0x08
+#define STM_TER_OFFSET	0x10
+
+#define U5500_PRCMU_DBG_PWRCTRL		(U5500_PRCMU_BASE + 0x4AC)
+#define PRCMU_DBG_PWRCTRL_A9DBGCLKEN	(1 << 4)
+
+#define TPIU_PORT_SIZE 0x4
+#define TPIU_TRIGGER_COUNTER 0x104
+#define TPIU_TRIGGER_MULTIPLIER 0x108
+#define TPIU_CURRENT_TEST_PATTERN 0x204
+#define TPIU_TEST_PATTERN_REPEAT 0x208
+#define TPIU_FORMATTER 0x304
+#define TPIU_FORMATTER_SYNC 0x308
+#define TPIU_LOCK_ACCESS_REGISTER 0xFB0
+
+#define TPIU_UNLOCK_CODE 0xc5acce55
+
+#define SCU_FILTER_STARTADDR	0x40
+#define SCU_FILTER_ENDADDR	0x44
+#define SCU_ACCESS_CTRL_SAC	0x50
+
+/*
+ * Periph clock cluster context
+ */
+#define PRCC_BCK_EN		0x00
+#define PRCC_KCK_EN		0x08
+#define PRCC_BCK_STATUS		0x10
+#define PRCC_KCK_STATUS		0x14
+
+/* The context of the Trace Port Interface Unit (TPIU) */
+static struct {
+	void __iomem *base;
+	u32 port_size;
+	u32 trigger_counter;
+	u32 trigger_multiplier;
+	u32 current_test_pattern;
+	u32 test_pattern_repeat;
+	u32 formatter;
+	u32 formatter_sync;
+} context_tpiu;
+
+static struct {
+	void __iomem *base;
+	u32 cr;
+	u32 mmc;
+	u32 ter;
+} context_stm_ape;
+
+struct context_gic_cpu {
+	void __iomem *base;
+	u32 ctrl;
+	u32 primask;
+	u32 binpoint;
+};
+static DEFINE_PER_CPU(struct context_gic_cpu, context_gic_cpu);
+
+static struct {
+	void __iomem *base;
+	u32 ns;
+	u32 enable_set[GIC_DIST_ENABLE_SET_COMMON_NUM]; /* IRQ 32 to 160 */
+	u32 priority_level[GIC_DIST_PRI_COMMON_NUM];
+	u32 spi_target[GIC_DIST_SPI_TARGET_COMMON_NUM];
+	u32 config[GIC_DIST_CONFIG_COMMON_NUM];
+} context_gic_dist_common;
+
+struct context_gic_dist_cpu {
+	void __iomem *base;
+	u32 enable_set[GIC_DIST_ENABLE_SET_CPU_NUM]; /* IRQ 0 to 31 */
+	u32 priority_level[GIC_DIST_PRI_CPU_NUM];
+	u32 spi_target[GIC_DIST_SPI_TARGET_CPU_NUM];
+	u32 config[GIC_DIST_CONFIG_CPU_NUM];
+};
+static DEFINE_PER_CPU(struct context_gic_dist_cpu, context_gic_dist_cpu);
+
+static struct {
+	void __iomem *base;
+	u32 ctrl;
+	u32 cpu_pwrstatus;
+	u32 inv_all_nonsecure;
+	u32 filter_start_addr;
+	u32 filter_end_addr;
+	u32 access_ctrl_sac;
+} context_scu;
+
+#define UX500_NR_PRCC_BANKS 5
+static struct {
+	void __iomem *base;
+	struct clk *clk;
+	u32 bus_clk;
+	u32 kern_clk;
+} context_prcc[UX500_NR_PRCC_BANKS];
+
+static u32 backup_sram_storage[NR_CPUS] = {
+	IO_ADDRESS(U8500_CPU0_CP15_CR_BACKUPRAM_ADDR),
+	IO_ADDRESS(U8500_CPU1_CP15_CR_BACKUPRAM_ADDR),
+};
+
+static u32 gpio_bankaddr[GPIO_NUM_BANKS] = {IO_ADDRESS(U8500_GPIOBANK0_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK1_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK2_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK3_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK4_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK5_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK6_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK7_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK8_BASE)
+};
+
+static u32 gpio_save[GPIO_NUM_BANKS][GPIO_NUM_SAVE_REGISTERS];
+
+/*
+ * Stacks and stack pointers
+ */
+static DEFINE_PER_CPU(u32[128], varm_registers_backup_stack);
+static DEFINE_PER_CPU(u32 *, varm_registers_pointer);
+
+static DEFINE_PER_CPU(u32[128], varm_cp15_backup_stack);
+static DEFINE_PER_CPU(u32 *, varm_cp15_pointer);
+
+
+static ATOMIC_NOTIFIER_HEAD(context_ape_notifier_list);
+static ATOMIC_NOTIFIER_HEAD(context_arm_notifier_list);
+
+/*
+ * Register a simple callback for handling vape context save/restore
+ */
+int context_ape_notifier_register(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_register(&context_ape_notifier_list, nb);
+}
+EXPORT_SYMBOL(context_ape_notifier_register);
+
+/*
+ * Remove a previously registered callback
+ */
+int context_ape_notifier_unregister(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_unregister(&context_ape_notifier_list,
+						    nb);
+}
+EXPORT_SYMBOL(context_ape_notifier_unregister);
+
+/*
+ * Register a simple callback for handling varm context save/restore
+ */
+int context_arm_notifier_register(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_register(&context_arm_notifier_list, nb);
+}
+EXPORT_SYMBOL(context_arm_notifier_register);
+
+/*
+ * Remove a previously registered callback
+ */
+int context_arm_notifier_unregister(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_unregister(&context_arm_notifier_list,
+						    nb);
+}
+EXPORT_SYMBOL(context_arm_notifier_unregister);
+
+static void save_prcc(void)
+{
+	int i;
+
+	for (i = 0; i < UX500_NR_PRCC_BANKS; i++) {
+		clk_enable(context_prcc[i].clk);
+
+		context_prcc[i].bus_clk =
+			readl(context_prcc[i].base + PRCC_BCK_STATUS);
+		context_prcc[i].kern_clk =
+			readl(context_prcc[i].base + PRCC_KCK_STATUS);
+
+		clk_disable(context_prcc[i].clk);
+	}
+}
+
+static void restore_prcc(void)
+{
+	int i;
+
+	for (i = 0; i < UX500_NR_PRCC_BANKS; i++) {
+		clk_enable(context_prcc[i].clk);
+
+		writel(context_prcc[i].bus_clk,
+		       context_prcc[i].base + PRCC_BCK_EN);
+		writel(context_prcc[i].kern_clk,
+		       context_prcc[i].base + PRCC_KCK_EN);
+
+		clk_disable(context_prcc[i].clk);
+	}
+}
+
+static void save_stm_ape(void)
+{
+	/*
+	 * TODO: Check with PRCMU developers how STM is handled by PRCMU
+	 * firmware. According to DB5500 design spec there is a "flush"
+	 * mechanism supposed to be used by the PRCMU before power down,
+	 * PRCMU fw might save/restore the following three registers
+	 * at the same time.
+	 */
+	context_stm_ape.cr  = readl(context_stm_ape.base +
+				    STM_CR_OFFSET);
+	context_stm_ape.mmc = readl(context_stm_ape.base +
+				    STM_MMC_OFFSET);
+	context_stm_ape.ter = readl(context_stm_ape.base +
+				    STM_TER_OFFSET);
+}
+
+static void restore_stm_ape(void)
+{
+	writel(context_stm_ape.ter,
+	       context_stm_ape.base + STM_TER_OFFSET);
+	writel(context_stm_ape.mmc,
+	       context_stm_ape.base + STM_MMC_OFFSET);
+	writel(context_stm_ape.cr,
+	       context_stm_ape.base + STM_CR_OFFSET);
+}
+
+static bool tpiu_clocked(void)
+{
+#ifdef CONFIG_UX500_DEBUG_NO_LAUTERBACH
+	return false;
+#else
+	if (cpu_is_u5500())
+		return readl_relaxed(__io_address(U5500_PRCMU_DBG_PWRCTRL))
+			& PRCMU_DBG_PWRCTRL_A9DBGCLKEN;
+
+	if (cpu_is_u8500())
+		return ux500_jtag_enabled();
+
+	return true;
+#endif
+}
+
+/*
+ * Save the context of the Trace Port Interface Unit (TPIU).
+ * Saving/restoring is needed for the PTM tracing to work together
+ * with the sleep states ApSleep and ApDeepSleep.
+ */
+static void save_tpiu(void)
+{
+	if (!tpiu_clocked())
+		return;
+
+	context_tpiu.port_size		  = readl(context_tpiu.base +
+						  TPIU_PORT_SIZE);
+	context_tpiu.trigger_counter	  = readl(context_tpiu.base +
+						  TPIU_TRIGGER_COUNTER);
+	context_tpiu.trigger_multiplier   = readl(context_tpiu.base +
+						TPIU_TRIGGER_MULTIPLIER);
+	context_tpiu.current_test_pattern = readl(context_tpiu.base +
+						  TPIU_CURRENT_TEST_PATTERN);
+	context_tpiu.test_pattern_repeat  = readl(context_tpiu.base +
+						 TPIU_TEST_PATTERN_REPEAT);
+	context_tpiu.formatter		  = readl(context_tpiu.base +
+						  TPIU_FORMATTER);
+	context_tpiu.formatter_sync	  = readl(context_tpiu.base +
+						  TPIU_FORMATTER_SYNC);
+}
+
+/*
+ * Restore the context of the Trace Port Interface Unit (TPIU).
+ * Saving/restoring is needed for the PTM tracing to work together
+ * with the sleep states ApSleep and ApDeepSleep.
+ */
+static void restore_tpiu(void)
+{
+	if (!tpiu_clocked())
+		return;
+
+	writel(TPIU_UNLOCK_CODE,
+	       context_tpiu.base + TPIU_LOCK_ACCESS_REGISTER);
+
+	writel(context_tpiu.port_size,
+	       context_tpiu.base + TPIU_PORT_SIZE);
+	writel(context_tpiu.trigger_counter,
+	       context_tpiu.base + TPIU_TRIGGER_COUNTER);
+	writel(context_tpiu.trigger_multiplier,
+	       context_tpiu.base + TPIU_TRIGGER_MULTIPLIER);
+	writel(context_tpiu.current_test_pattern,
+	       context_tpiu.base + TPIU_CURRENT_TEST_PATTERN);
+	writel(context_tpiu.test_pattern_repeat,
+	       context_tpiu.base + TPIU_TEST_PATTERN_REPEAT);
+	writel(context_tpiu.formatter,
+	       context_tpiu.base + TPIU_FORMATTER);
+	writel(context_tpiu.formatter_sync,
+	       context_tpiu.base + TPIU_FORMATTER_SYNC);
+}
+
+/*
+ * Save GIC CPU IF registers
+ *
+ * This is per cpu so it needs to be called for each one.
+ */
+
+static void save_gic_if_cpu(struct context_gic_cpu *c_gic_cpu)
+{
+	c_gic_cpu->ctrl     = readl_relaxed(c_gic_cpu->base + GIC_CPU_CTRL);
+	c_gic_cpu->primask  = readl_relaxed(c_gic_cpu->base + GIC_CPU_PRIMASK);
+	c_gic_cpu->binpoint = readl_relaxed(c_gic_cpu->base + GIC_CPU_BINPOINT);
+}
+
+/*
+ * Restore GIC CPU IF registers
+ *
+ * This is per cpu so it needs to be called for each one.
+ */
+static void restore_gic_if_cpu(struct context_gic_cpu *c_gic_cpu)
+{
+	writel_relaxed(c_gic_cpu->ctrl,     c_gic_cpu->base + GIC_CPU_CTRL);
+	writel_relaxed(c_gic_cpu->primask,  c_gic_cpu->base + GIC_CPU_PRIMASK);
+	writel_relaxed(c_gic_cpu->binpoint, c_gic_cpu->base + GIC_CPU_BINPOINT);
+}
+
+/*
+ * Save GIC Distributor Common registers
+ *
+ * This context is common. Only one CPU needs to call.
+ *
+ * Save SPI (Shared Peripheral Interrupt) settings, IRQ 32-159.
+ */
+
+static void save_gic_dist_common(void)
+{
+	int i;
+
+	context_gic_dist_common.ns = readl_relaxed(context_gic_dist_common.base
+					   + GIC_DIST_ENABLE_NS);
+
+	for (i = 0; i < GIC_DIST_ENABLE_SET_COMMON_NUM; i++)
+		context_gic_dist_common.enable_set[i] =
+			readl_relaxed(context_gic_dist_common.base +
+			      GIC_DIST_ENABLE_SET_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_PRI_COMMON_NUM; i++)
+		context_gic_dist_common.priority_level[i] =
+			readl_relaxed(context_gic_dist_common.base +
+			      GIC_DIST_PRI_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_SPI_TARGET_COMMON_NUM; i++)
+		context_gic_dist_common.spi_target[i] =
+			readl_relaxed(context_gic_dist_common.base +
+			      GIC_DIST_SPI_TARGET_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_CONFIG_COMMON_NUM; i++)
+		context_gic_dist_common.config[i] =
+			readl_relaxed(context_gic_dist_common.base +
+			      GIC_DIST_CONFIG_SPI32 +  i * 4);
+}
+
+/*
+ * Restore GIC Distributor Common registers
+ *
+ * This context is common. Only one CPU needs to call.
+ *
+ * Save SPI (Shared Peripheral Interrupt) settings, IRQ 32-159.
+ */
+static void restore_gic_dist_common(void)
+{
+
+	int i;
+
+	for (i = 0; i < GIC_DIST_CONFIG_COMMON_NUM; i++)
+		writel_relaxed(context_gic_dist_common.config[i],
+		       context_gic_dist_common.base +
+		       GIC_DIST_CONFIG_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_SPI_TARGET_COMMON_NUM; i++)
+		writel_relaxed(context_gic_dist_common.spi_target[i],
+		       context_gic_dist_common.base +
+		       GIC_DIST_SPI_TARGET_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_PRI_COMMON_NUM; i++)
+		writel_relaxed(context_gic_dist_common.priority_level[i],
+			context_gic_dist_common.base +
+		       GIC_DIST_PRI_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_ENABLE_SET_COMMON_NUM; i++)
+		writel_relaxed(context_gic_dist_common.enable_set[i],
+		       context_gic_dist_common.base +
+		       GIC_DIST_ENABLE_SET_SPI32 +  i * 4);
+
+	writel_relaxed(context_gic_dist_common.ns,
+	       context_gic_dist_common.base + GIC_DIST_ENABLE_NS);
+}
+
+
+
+/*
+ * Save GIC Dist CPU registers
+ *
+ * This needs to be called by all cpu:s which will not call
+ * save_gic_dist_common(). Only the registers of the GIC which are
+ * banked will be saved.
+ */
+static void save_gic_dist_cpu(struct context_gic_dist_cpu *c_gic)
+{
+	int i;
+
+	for (i = 0; i < GIC_DIST_ENABLE_SET_CPU_NUM; i++)
+		c_gic->enable_set[i] =
+			readl_relaxed(c_gic->base +
+			      GIC_DIST_ENABLE_SET_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_PRI_CPU_NUM; i++)
+		c_gic->priority_level[i] =
+			readl_relaxed(c_gic->base +
+			      GIC_DIST_PRI_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_SPI_TARGET_CPU_NUM; i++)
+		c_gic->spi_target[i] =
+			readl_relaxed(c_gic->base +
+			      GIC_DIST_SPI_TARGET_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_CONFIG_CPU_NUM; i++)
+		c_gic->config[i] =
+			readl_relaxed(c_gic->base +
+			      GIC_DIST_CONFIG_SPI0 +  i * 4);
+}
+
+/*
+ * Restore GIC Dist CPU registers
+ *
+ * This needs to be called by all cpu:s which will not call
+ * restore_gic_dist_common(). Only the registers of the GIC which are
+ * banked will be saved.
+ */
+static void restore_gic_dist_cpu(struct context_gic_dist_cpu *c_gic)
+{
+
+	int i;
+
+	for (i = 0; i < GIC_DIST_CONFIG_CPU_NUM; i++)
+		writel_relaxed(c_gic->config[i],
+		       c_gic->base +
+		       GIC_DIST_CONFIG_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_SPI_TARGET_CPU_NUM; i++)
+		writel_relaxed(c_gic->spi_target[i],
+		       c_gic->base +
+		       GIC_DIST_SPI_TARGET_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_PRI_CPU_NUM; i++)
+		writel_relaxed(c_gic->priority_level[i],
+			c_gic->base +
+		       GIC_DIST_PRI_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_ENABLE_SET_CPU_NUM; i++)
+		writel_relaxed(c_gic->enable_set[i],
+		       c_gic->base +
+		       GIC_DIST_ENABLE_SET_SPI0 +  i * 4);
+}
+
+/*
+ * Disable interrupts that are not necessary
+ * to have turned on during ApDeepSleep.
+ */
+void context_gic_dist_disable_unneeded_irqs(void)
+{
+
+	writel(0xffffffff,
+	       context_gic_dist_common.base +
+	       GIC_DIST_ENABLE_CLEAR_0);
+
+	writel(0xffffffff,
+	       context_gic_dist_common.base +
+	       GIC_DIST_ENABLE_CLEAR_32);
+
+	/* Leave PRCMU IRQ 0 and 1 enabled */
+	writel(0xffff3fff,
+	       context_gic_dist_common.base +
+	       GIC_DIST_ENABLE_CLEAR_64);
+
+	writel(0xffffffff,
+	       context_gic_dist_common.base +
+	       GIC_DIST_ENABLE_CLEAR_96);
+
+	writel(0xffffffff,
+	       context_gic_dist_common.base +
+	       GIC_DIST_ENABLE_CLEAR_128);
+
+}
+
+static void save_scu(void)
+{
+	context_scu.ctrl =
+		readl_relaxed(context_scu.base + SCU_CTRL);
+	context_scu.cpu_pwrstatus =
+		readl_relaxed(context_scu.base + SCU_CPU_STATUS);
+	context_scu.inv_all_nonsecure =
+		readl_relaxed(context_scu.base + SCU_INVALIDATE);
+	context_scu.filter_start_addr =
+		readl_relaxed(context_scu.base + SCU_FILTER_STARTADDR);
+	context_scu.filter_end_addr	=
+		readl_relaxed(context_scu.base + SCU_FILTER_ENDADDR);
+	context_scu.access_ctrl_sac	=
+		readl_relaxed(context_scu.base + SCU_ACCESS_CTRL_SAC);
+}
+
+static void restore_scu(void)
+{
+	writel_relaxed(context_scu.ctrl,
+	       context_scu.base + SCU_CTRL);
+	writel_relaxed(context_scu.cpu_pwrstatus,
+	       context_scu.base + SCU_CPU_STATUS);
+	writel_relaxed(context_scu.inv_all_nonsecure,
+	       context_scu.base + SCU_INVALIDATE);
+	writel_relaxed(context_scu.filter_start_addr,
+	       context_scu.base + SCU_FILTER_STARTADDR);
+	writel_relaxed(context_scu.filter_end_addr,
+	       context_scu.base + SCU_FILTER_ENDADDR);
+	writel_relaxed(context_scu.access_ctrl_sac,
+	       context_scu.base + SCU_ACCESS_CTRL_SAC);
+}
+
+/*
+ * Save VAPE context
+ */
+void context_vape_save(void)
+{
+	atomic_notifier_call_chain(&context_ape_notifier_list,
+				   CONTEXT_APE_SAVE, NULL);
+
+	if (cpu_is_u5500())
+		u5500_context_save_icn();
+	if (cpu_is_u8500())
+		u8500_context_save_icn();
+
+	save_stm_ape();
+
+	save_tpiu();
+
+	save_prcc();
+
+}
+
+/*
+ * Restore VAPE context
+ */
+void context_vape_restore(void)
+{
+	restore_prcc();
+
+	restore_tpiu();
+
+	restore_stm_ape();
+
+	if (cpu_is_u5500())
+		u5500_context_restore_icn();
+	if (cpu_is_u8500())
+		u8500_context_restore_icn();
+
+	atomic_notifier_call_chain(&context_ape_notifier_list,
+				   CONTEXT_APE_RESTORE, NULL);
+}
+
+/*
+ * Save GPIO registers that might be modified
+ * for power save reasons.
+ */
+void context_gpio_save(void)
+{
+	int i;
+
+	for (i = 0; i < GPIO_NUM_BANKS; i++) {
+		gpio_save[i][0] = readl(gpio_bankaddr[i] + NMK_GPIO_AFSLA);
+		gpio_save[i][1] = readl(gpio_bankaddr[i] + NMK_GPIO_AFSLB);
+		gpio_save[i][2] = readl(gpio_bankaddr[i] + NMK_GPIO_PDIS);
+		gpio_save[i][3] = readl(gpio_bankaddr[i] + NMK_GPIO_DIR);
+		gpio_save[i][4] = readl(gpio_bankaddr[i] + NMK_GPIO_DAT);
+		gpio_save[i][6] = readl(gpio_bankaddr[i] + NMK_GPIO_SLPC);
+	}
+}
+
+/*
+ * Restore GPIO registers that might be modified
+ * for power save reasons.
+ */
+void context_gpio_restore(void)
+{
+	int i;
+	u32 output_state;
+	u32 pull_up;
+	u32 pull_down;
+	u32 pull;
+
+	for (i = 0; i < GPIO_NUM_BANKS; i++) {
+		writel(gpio_save[i][2], gpio_bankaddr[i] + NMK_GPIO_PDIS);
+
+		writel(gpio_save[i][3], gpio_bankaddr[i] + NMK_GPIO_DIR);
+
+		/* Set the high outputs. outpute_state = GPIO_DIR & GPIO_DAT */
+		output_state = gpio_save[i][3] & gpio_save[i][4];
+		writel(output_state, gpio_bankaddr[i] + NMK_GPIO_DATS);
+
+		/*
+		 * Set the low outputs.
+		 * outpute_state = ~(GPIO_DIR & GPIO_DAT) & GPIO_DIR
+		 */
+		output_state = ~(gpio_save[i][3] & gpio_save[i][4]) &
+			gpio_save[i][3];
+		writel(output_state, gpio_bankaddr[i] + NMK_GPIO_DATC);
+
+		/*
+		 * Restore pull up/down.
+		 * Only write pull up/down settings on inputs where
+		 * PDIS is not set.
+		 * pull = (~GPIO_DIR & ~GPIO_PDIS)
+		 */
+		pull = (~gpio_save[i][3] & ~gpio_save[i][2]);
+		nmk_gpio_read_pull(i, &pull_up);
+
+		pull_down = pull & ~pull_up;
+		pull_up = pull & pull_up;
+		/* Set pull ups */
+		writel(pull_up, gpio_bankaddr[i] + NMK_GPIO_DATS);
+		/* Set pull downs */
+		writel(pull_down, gpio_bankaddr[i] + NMK_GPIO_DATC);
+
+		writel(gpio_save[i][6], gpio_bankaddr[i] + NMK_GPIO_SLPC);
+
+	}
+
+}
+
+/*
+ * Restore GPIO mux registers that might be modified
+ * for power save reasons.
+ */
+void context_gpio_restore_mux(void)
+{
+	int i;
+
+	/* Change mux settings */
+	for (i = 0; i < GPIO_NUM_BANKS; i++) {
+		writel(gpio_save[i][0], gpio_bankaddr[i] + NMK_GPIO_AFSLA);
+		writel(gpio_save[i][1], gpio_bankaddr[i] + NMK_GPIO_AFSLB);
+	}
+}
+
+/*
+ * Safe sequence used to switch IOs between GPIO and Alternate-C mode:
+ *  - Save SLPM registers (Not done.)
+ *  - Set SLPM=0 for the IOs you want to switch. (We assume that all
+ *    SLPM registers already are 0 except for the ones that wants to
+ *    have the mux connected in sleep (e.g modem STM)).
+ *  - Configure the GPIO registers for the IOs that are being switched
+ *  - Set IOFORCE=1
+ *  - Modify the AFLSA/B registers for the IOs that are being switched
+ *  - Set IOFORCE=0
+ *  - Restore SLPM registers (Not done.)
+ *  - Any spurious wake up event during switch sequence to be ignored
+ *    and cleared
+ */
+void context_gpio_mux_safe_switch(bool begin)
+{
+	int i;
+
+	static u32 rwimsc[GPIO_NUM_BANKS];
+	static u32 fwimsc[GPIO_NUM_BANKS];
+
+	if (begin) {
+		for (i = 0; i < GPIO_NUM_BANKS; i++) {
+			/* Save registers */
+			rwimsc[i] = readl(gpio_bankaddr[i] + NMK_GPIO_RWIMSC);
+			fwimsc[i] = readl(gpio_bankaddr[i] + NMK_GPIO_FWIMSC);
+
+			/* Prevent spurious wakeups */
+			writel(0, gpio_bankaddr[i] + NMK_GPIO_RWIMSC);
+			writel(0, gpio_bankaddr[i] + NMK_GPIO_FWIMSC);
+		}
+
+		ux500_pm_prcmu_set_ioforce(true);
+	} else {
+		ux500_pm_prcmu_set_ioforce(false);
+
+		/* Restore wake up settings */
+		for (i = 0; i < GPIO_NUM_BANKS; i++) {
+			writel(rwimsc[i], gpio_bankaddr[i] + NMK_GPIO_RWIMSC);
+			writel(fwimsc[i], gpio_bankaddr[i] + NMK_GPIO_FWIMSC);
+		}
+
+	}
+
+}
+
+/*
+ * Save common
+ *
+ * This function must be called once for all cores before going to deep sleep.
+ */
+void context_varm_save_common(void)
+{
+	atomic_notifier_call_chain(&context_arm_notifier_list,
+				   CONTEXT_ARM_COMMON_SAVE, NULL);
+
+	/* Save common parts */
+	save_gic_dist_common();
+	save_scu();
+}
+
+/*
+ * Restore common
+ *
+ * This function must be called once for all cores when waking up from deep
+ * sleep.
+ */
+void context_varm_restore_common(void)
+{
+	/* Restore common parts */
+	restore_scu();
+	restore_gic_dist_common();
+
+	atomic_notifier_call_chain(&context_arm_notifier_list,
+				   CONTEXT_ARM_COMMON_RESTORE, NULL);
+}
+
+/*
+ * Save core
+ *
+ * This function must be called once for each cpu core before going to deep
+ * sleep.
+ */
+void context_varm_save_core(void)
+{
+
+	int cpu = smp_processor_id();
+
+	atomic_notifier_call_chain(&context_arm_notifier_list,
+				   CONTEXT_ARM_CORE_SAVE, NULL);
+
+	per_cpu(varm_cp15_pointer, cpu) = per_cpu(varm_cp15_backup_stack, cpu);
+
+	/* Save core */
+	save_gic_if_cpu(&per_cpu(context_gic_cpu, cpu));
+	save_gic_dist_cpu(&per_cpu(context_gic_dist_cpu, cpu));
+	context_save_cp15_registers(&per_cpu(varm_cp15_pointer, cpu));
+}
+
+/*
+ * Restore core
+ *
+ * This function must be called once for each cpu core when waking up from
+ * deep sleep.
+ */
+void context_varm_restore_core(void)
+{
+	int cpu = smp_processor_id();
+
+	/* Restore core */
+	context_restore_cp15_registers(&per_cpu(varm_cp15_pointer, cpu));
+	restore_gic_dist_cpu(&per_cpu(context_gic_dist_cpu, cpu));
+	restore_gic_if_cpu(&per_cpu(context_gic_cpu, cpu));
+
+	atomic_notifier_call_chain(&context_arm_notifier_list,
+				   CONTEXT_ARM_CORE_RESTORE, NULL);
+
+}
+
+/*
+ * Save CPU registers
+ *
+ * This function saves ARM registers.
+ */
+void context_save_cpu_registers(void)
+{
+	int cpu = smp_processor_id();
+
+	per_cpu(varm_registers_pointer, cpu) =
+		per_cpu(varm_registers_backup_stack, cpu);
+	context_save_arm_registers(&per_cpu(varm_registers_pointer, cpu));
+}
+
+/*
+ * Restore CPU registers
+ *
+ * This function restores ARM registers.
+ */
+void context_restore_cpu_registers(void)
+{
+	int cpu = smp_processor_id();
+
+	context_restore_arm_registers(&per_cpu(varm_registers_pointer, cpu));
+}
+
+/*
+ * This function stores CP15 registers related to cache and mmu
+ * in backup SRAM. It also stores stack pointer, CPSR
+ * and return address for the PC in backup SRAM and
+ * does wait for interrupt.
+ */
+void context_save_to_sram_and_wfi(bool cleanL2cache)
+{
+	int cpu = smp_processor_id();
+
+	context_save_to_sram_and_wfi_internal(backup_sram_storage[cpu],
+					      cleanL2cache);
+}
+
+static int __init context_init(void)
+{
+	int i;
+	void __iomem *ux500_backup_ptr;
+
+	/* allocate backup pointer for RAM data */
+	ux500_backup_ptr = (void *)__get_free_pages(GFP_KERNEL,
+				  get_order(U8500_BACKUPRAM_SIZE));
+
+	if (!ux500_backup_ptr) {
+		pr_warning("context: could not allocate backup memory\n");
+		return -ENOMEM;
+	}
+
+	/*
+	 * ROM code addresses to store backup contents,
+	 * pass the physical address of back up to ROM code
+	 */
+	writel(virt_to_phys(ux500_backup_ptr),
+	       IO_ADDRESS(U8500_EXT_RAM_LOC_BACKUPRAM_ADDR));
+
+
+	if (cpu_is_u5500()) {
+		writel(IO_ADDRESS(U5500_PUBLIC_BOOT_ROM_BASE),
+		       IO_ADDRESS(U8500_CPU0_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR));
+
+		writel(IO_ADDRESS(U5500_PUBLIC_BOOT_ROM_BASE),
+		       IO_ADDRESS(U8500_CPU1_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR));
+
+		context_tpiu.base = ioremap(U5500_TPIU_BASE, SZ_4K);
+		context_stm_ape.base = ioremap(U5500_STM_REG_BASE, SZ_4K);
+		context_scu.base = ioremap(U5500_SCU_BASE, SZ_4K);
+
+		context_prcc[0].base = ioremap(U5500_CLKRST1_BASE, SZ_4K);
+		context_prcc[1].base = ioremap(U5500_CLKRST2_BASE, SZ_4K);
+		context_prcc[2].base = ioremap(U5500_CLKRST3_BASE, SZ_4K);
+		context_prcc[3].base = ioremap(U5500_CLKRST5_BASE, SZ_4K);
+		context_prcc[4].base = ioremap(U5500_CLKRST6_BASE, SZ_4K);
+
+		context_gic_dist_common.base = ioremap(U5500_GIC_DIST_BASE, SZ_4K);
+		per_cpu(context_gic_cpu, 0).base = ioremap(U5500_GIC_CPU_BASE, SZ_4K);
+	} else if (cpu_is_u8500()) {
+		/* Give logical address to backup RAM. For both CPUs */
+		writel(IO_ADDRESS(U8500_PUBLIC_BOOT_ROM_BASE),
+		       IO_ADDRESS(U8500_CPU0_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR));
+
+		writel(IO_ADDRESS(U8500_PUBLIC_BOOT_ROM_BASE),
+		       IO_ADDRESS(U8500_CPU1_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR));
+
+		context_tpiu.base = ioremap(U8500_TPIU_BASE, SZ_4K);
+		context_stm_ape.base = ioremap(U8500_STM_REG_BASE, SZ_4K);
+		context_scu.base = ioremap(U8500_SCU_BASE, SZ_4K);
+
+		/* PERIPH4 is always on, so no need saving prcc */
+		context_prcc[0].base = ioremap(U8500_CLKRST1_BASE, SZ_4K);
+		context_prcc[1].base = ioremap(U8500_CLKRST2_BASE, SZ_4K);
+		context_prcc[2].base = ioremap(U8500_CLKRST3_BASE, SZ_4K);
+		context_prcc[3].base = ioremap(U8500_CLKRST5_BASE, SZ_4K);
+		context_prcc[4].base = ioremap(U8500_CLKRST6_BASE, SZ_4K);
+
+		context_gic_dist_common.base = ioremap(U8500_GIC_DIST_BASE, SZ_4K);
+		per_cpu(context_gic_cpu, 0).base = ioremap(U8500_GIC_CPU_BASE, SZ_4K);
+	}
+
+	per_cpu(context_gic_dist_cpu, 0).base = context_gic_dist_common.base;
+
+	for (i = 1; i < num_possible_cpus(); i++) {
+		per_cpu(context_gic_cpu, i).base
+			= per_cpu(context_gic_cpu, 0).base;
+		per_cpu(context_gic_dist_cpu, i).base
+			= per_cpu(context_gic_dist_cpu, 0).base;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(context_prcc); i++) {
+		const int clusters[] = {1, 2, 3, 5, 6};
+		char clkname[10];
+
+		snprintf(clkname, sizeof(clkname), "PERIPH%d", clusters[i]);
+
+		context_prcc[i].clk = clk_get_sys(clkname, NULL);
+		BUG_ON(IS_ERR(context_prcc[i].clk));
+	}
+
+	if (cpu_is_u8500()) {
+		u8500_context_init();
+	} else if (cpu_is_u5500()) {
+		u5500_context_init();
+	} else {
+		printk(KERN_ERR "context: unknown hardware!\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+subsys_initcall(context_init);
diff --git a/arch/arm/mach-ux500/pm/context_arm.S b/arch/arm/mach-ux500/pm/context_arm.S
new file mode 100644
index 00000000000..55e2accc85f
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/context_arm.S
@@ -0,0 +1,385 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>
+ *         Rickard Andersson <rickard.andersson@stericsson.com> for
+ *         ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#include <linux/linkage.h>
+#include <mach/hardware.h>
+#include <asm/hardware/cache-l2x0.h>
+
+/*
+ * Save and increment macro
+ */
+.macro SAVE_AND_INCREMENT FROM_REG TO_REG
+	str \FROM_REG, [\TO_REG], #+4
+.endm
+
+/*
+ * Decrement and restore macro
+ */
+.macro DECREMENT_AND_RESTORE FROM_REG TO_REG
+	ldr \TO_REG, [\FROM_REG, #-4]!
+.endm
+
+/*
+ * Save ARM registers
+ *
+ * This function must be called in supervisor mode.
+ *
+ * r0 = address to backup stack pointer
+ *
+ * Backup stack operations:
+ * + {sp, lr}^
+ * + cpsr
+ * + {r3, r8-r14} (FIQ mode: r3=spsr)
+ * + {r3, r13, r14} (IRQ mode: r3=spsr)
+ * + {r3, r13, r14} (abort mode: r3=spsr)
+ * + {r3, r13, r14} (undef mode: r3=spsr)
+ */
+	.align
+	.section ".text", "ax"
+ENTRY(context_save_arm_registers)
+	stmfd sp!, {r1, r2, r3, lr}		@ Save on stack
+	ldr r1, [r0]				@ Read backup stack pointer
+
+	stmia r1, {sp, lr}^			@ Store user mode sp and lr
+	                                        @ registers
+	add r1, r1, #8				@ Update backup pointer (not
+	                                        @ done in previous instruction)
+
+	mrs r2, cpsr				@ Get CPSR
+	SAVE_AND_INCREMENT r2 r1		@ Save CPSR register
+	orr r2, r2, #0xc0			@ Disable FIQ and IRQ
+	bic r2, r2, #0x1f			@ Setup r2 to change mode
+
+	@ The suffix to CPSR refers to which field(s) of the CPSR is
+	@ rereferenced (you can specify one or more). Defined fields are:
+	@
+	@ c - control
+	@ x - extension
+	@ s - status
+	@ f - flags
+
+	orr r3, r2, #0x11			@ Save FIQ mode registers
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3, r8-r14}
+
+	orr r3, r2, #0x12			@ Save IRQ mode registers
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3, r13, r14}
+
+	orr r3, r2, #0x17			@ Save abort mode registers +
+	                                        @ common mode registers
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3, r13, r14}
+
+	orr r3, r2, #0x1B			@ Save undef mode registers
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3, r13, r14}
+
+	orr r3, r2, #0x13			@ Return to supervisor mode
+	msr cpsr_cxsf, r3
+
+	str r1, [r0]				@ Write backup stack pointer
+	ldmfd sp!, {r1, r2, r3, pc}		@ Restore registers and return
+
+
+
+/*
+ * Restore ARM registers
+ *
+ * This function must be called in supervisor mode.
+ *
+ * r0 = address to backup stack pointer
+ *
+ * Backup stack operations:
+ * - {r3, r13, r14} (undef mode: spsr=r3)
+ * - {r3, r13, r14} (abort mode: spsr=r3)
+ * - {r3, r13, r14} (IRQ mode: spsr=r3)
+ * - {r3, r8-r14} (FIQ mode: spsr=r3)
+ * - cpsr
+ * - {sp, lr}^
+ */
+	.align
+	.section ".text", "ax"
+ENTRY(context_restore_arm_registers)
+	stmfd sp!, {r1, r2, r3, lr}		@ Save on stack
+	ldr r1, [r0]				@ Read backup stack pointer
+
+	mrs r2, cpsr				@ Get CPSR
+	orr r2, r2, #0xc0			@ Disable FIQ and IRQ
+	bic r2, r2, #0x1f			@ Setup r2 to change mode
+
+	orr r3, r2, #0x1b			@ Restore undef mode registers
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3, r13, r14}
+	msr spsr_cxsf, r3
+
+	orr r3, r2, #0x17			@ Restore abort mode registers
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3, r13, r14}
+	msr spsr_cxsf, r3
+
+	orr r3, r2, #0x12			@ Restore IRQ mode registers
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3, r13, r14}
+	msr spsr_cxsf, r3
+
+	orr r3, r2, #0x11			@ Restore FIQ mode registers
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3, r8-r14}
+	msr spsr_cxsf, r3
+
+	DECREMENT_AND_RESTORE r1 r3		@ Restore cpsr register
+	msr cpsr_cxsf, r3
+
+	ldmdb r1, {sp, lr}^			@ Restore sp and lr registers
+	sub r1, r1, #8				@ Update backup pointer (not
+	                                        @ done in previous instruction)
+
+	str r1, [r0]				@ Write backup stack pointer
+	ldmfd sp!, {r1, r2, r3, pc}		@ Restore registers and return
+
+
+
+/*
+ * Save CP15 registers
+ *
+ * This function must be called in supervisor mode.
+ *
+ * r0 = address to backup stack pointer
+ *
+ * TTBR0, TTBR1, TTBRC, DACR CP15 registers are restored by boot ROM from SRAM.
+ */
+	.align 4
+	.section ".text", "ax"
+ENTRY(context_save_cp15_registers)
+	stmfd sp!, {r1, r2, r3, lr}		@ Save on stack (r3 is saved due
+						@ to 8 byte aligned stack)
+	ldr r1, [r0]				@ Read backup stack pointer
+
+	mrc p15, 0, r2, c12, c0, 0		@ Read Non-secure Vector Base
+	                                        @ Address Register
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 0, r2, c10, c2, 0		@ Access primary memory region
+	                                        @ remap register
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 0, r2, c10, c2, 1		@ Access normal memory region
+	                                        @ remap register
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 0, r2, c13, c0, 1		@ Read Context ID Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c13, c0, 2		@ Read Thread ID registers,
+	                                        @ this register is both user
+	                                        @ and privileged R/W accessible
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c13, c0, 3		@ Read Thread ID registers,
+	                                        @ this register is user
+	                                        @ read-only and privileged R/W
+	                                        @ accessible.
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c13, c0, 4		@ Read Thread ID registers,
+	                                        @ this register is privileged
+	                                        @ R/W accessible only.
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 2, r2, c0, c0, 0		@ Cache Size Selection Register
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 0, r2, c9, c12, 0		@ Read PMNC Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c12, 1		@ Read PMCNTENSET Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c12, 5		@ Read PMSELR Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c13, 0		@ Read PMCCNTR Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c13, 1		@ Read PMXEVTYPER Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c14, 0		@ Read PMUSERENR Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c14, 1		@ Read PMINTENSET Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c14, 2		@ Read PMINTENCLR Register
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 0, r2, c1, c0, 2		@ Read CPACR Register
+	SAVE_AND_INCREMENT r2 r1
+
+	str r1, [r0]				@ Write backup stack pointer
+	ldmfd sp!, {r1, r2, r3, pc}		@ Restore registers and return
+
+
+
+/*
+ * Restore CP15 registers
+ *
+ * This function must be called in supervisor mode.
+ *
+ * r0 = address to backup stack pointer
+ */
+	.align 4
+	.section ".text", "ax"
+ENTRY(context_restore_cp15_registers)
+	stmfd sp!, {r1, r2, r3, lr}		@ Save on stack (r3 is saved due
+						@ to 8 byte aligned stack)
+	ldr r1, [r0]				@ Read backup stack pointer
+
+	DECREMENT_AND_RESTORE r1 r2		@ Write CPACR register
+	mcr p15, 0, r2, c1, c0, 2
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c14, 2		@ Write PMINTENCLR Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c14, 1		@ Write PMINTENSET Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c14, 0		@ Write PMUSERENR Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c13, 1		@ Write PMXEVTYPER Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c13, 0		@ Write PMCCNTR Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c12, 5		@ Write PMSELR Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c12, 1		@ Write PMCNTENSET Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c12, 0		@ Write PMNC Register
+
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 2, r2, c0, c0, 0		@ Cache Size Selection Register
+
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c13, c0, 4		@ Write Thread ID registers,
+	                                        @ this register is privileged
+	                                        @ R/W accessible only
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c13, c0, 3		@ Write Thread ID registers,
+	                                        @ this register is user
+	                                        @ read-only and privileged R/W
+	                                        @ accessible
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c13, c0, 2		@ Write Thread ID registers,
+	                                        @ this register is both user
+	                                        @ and privileged R/W accessible
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c13, c0, 1		@ Write Context ID Register
+
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c10, c2, 1		@ Access normal memory region
+	                                        @ remap register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c10, c2, 0		@ Access primary memory region
+	                                        @ remap register
+
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c12, c0, 0		@ Write Non-secure Vector Base
+	                                        @ Address Register
+
+	str r1, [r0]				@ Write backup stack pointer
+	ldmfd sp!, {r1, r2, r3, pc}		@ Restore registers and return
+
+
+/*
+ * L1 cache clean function. Commit 'dirty' data from L1
+ * to L2 cache.
+ *
+ * r0, r1, r2,  used locally
+ *
+ */
+	.align 4
+	.section ".text", "ax"
+ENTRY(context_clean_l1_cache_all)
+
+	mov	r0, #0				@ swith to cache level 0
+	                                        @ (L1 cache)
+	mcr	p15, 2, r0, c0, c0, 0		@ select current cache level
+	                                        @ in cssr
+
+	dmb
+	mov     r1, #0				@ r1 = way index
+wayLoopL1clean:
+	mov     r0, #0				@ r0 = line index
+lineLoopL1clean:
+	mov     r2, r1, lsl #30			@ TODO: OK to hard-code
+	                                        @ SoC-specific L1 cache details?
+	add     r2, r0, lsl #5
+	mcr     p15, 0, r2, c7, c10, 2		@ Clean cache by set/way
+	add     r0, r0, #1
+	cmp     r0, #256			@ TODO: Ok with hard-coded
+	                                        @ set/way sizes or do we have to
+	                                        @ read them from ARM regs? Is it
+	                                        @ set correctly in silicon?
+	bne     lineLoopL1clean
+	add     r1, r1, #1
+	cmp     r1, #4				@ TODO: Ditto, sizes...
+	bne     wayLoopL1clean
+
+	dsb
+	isb
+	mov	pc, lr
+
+ENDPROC(context_clean_l1_cache_all)
+
+/*
+ * Last saves to backup RAM, cache clean and WFI
+ *
+ * r0 = address to backup_sram_storage base adress
+ * r1 = indicate whether also L2 cache should be cleaned
+ */
+	.align 4
+	.section ".text", "ax"
+ENTRY(context_save_to_sram_and_wfi_internal)
+
+	stmfd sp!, {r2-r12, lr}			@ save on stack.
+
+	mrc p15, 0, r2, c1, c0, 0		@ read cp15 system control
+	                                        @ register
+	str r2, [r0, #0x00]
+	mrc p15, 0, r2, c2, c0, 0		@ read cp15 ttb0 register
+	str r2, [r0, #0x04]
+	mrc p15, 0, r2, c2, c0, 1		@ read cp15 ttb1 register
+	str r2, [r0, #0x08]
+	mrc p15, 0, r2, c2, c0, 2		@ read cp15 ttb control register
+	str r2, [r0, #0x0C]
+	mrc p15, 0, r2, c3, c0, 0		@ read domain access control
+	                                        @ register
+	str r2, [r0, #0x10]
+
+	ldr r2, =return_here
+	str r2, [r0, #0x14]			@ save program counter restore
+	                                        @ value to backup_sram_storage
+	mrs r2, cpsr
+	str r2, [r0, #0x18]			@ save cpsr to
+	                                        @ backup_sram_storage
+	str sp, [r0, #0x1c]			@ save sp to backup_sram_storage
+
+	mov r4, r1				@ Set r4 = cleanL2cache, r1
+	                                        @ will be destroyed by
+	                                        @ v7_clean_l1_cache_all
+
+	bl context_clean_l1_cache_all		@ Commit all dirty data in L1
+	                                        @ cache to L2 without
+	                                        @ invalidating
+
+	dsb					@ data synchronization barrier
+	isb					@ instruction synchronization
+	                                        @ barrier
+	wfi					@ wait for interrupt
+
+return_here:					@ both cores return here
+						@ now we are out deep sleep
+	                                        @ with all the context lost
+	                                        @ except pc, sp and cpsr
+
+	ldmfd sp!, {r2-r12, pc}			@ restore from stack
+
diff --git a/arch/arm/mach-ux500/pm/pm.c b/arch/arm/mach-ux500/pm/pm.c
new file mode 100644
index 00000000000..d986d1024d0
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/pm.c
@@ -0,0 +1,231 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com> for
+ *         ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#include <linux/io.h>
+#include <linux/percpu.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
+
+#include <asm/hardware/gic.h>
+#include <asm/processor.h>
+
+#include <mach/hardware.h>
+#include <mach/pm.h>
+
+#define STABILIZATION_TIME 30 /* us */
+#define GIC_FREEZE_DELAY 1 /* us */
+
+#define PRCM_ARM_WFI_STANDBY_CPU0_WFI 0x8
+#define PRCM_ARM_WFI_STANDBY_CPU1_WFI 0x10
+
+/* Dual A9 core interrupt management unit registers */
+#define PRCM_A9_MASK_REQ	(_PRCMU_BASE + 0x328)
+#define PRCM_A9_MASK_REQ_PRCM_A9_MASK_REQ	0x1
+#define PRCM_A9_MASK_ACK	(_PRCMU_BASE + 0x32c)
+
+#define PRCM_ARMITMSK31TO0	(_PRCMU_BASE + 0x11c)
+#define PRCM_ARMITMSK63TO32	(_PRCMU_BASE + 0x120)
+#define PRCM_ARMITMSK95TO64	(_PRCMU_BASE + 0x124)
+#define PRCM_ARMITMSK127TO96	(_PRCMU_BASE + 0x128)
+#define PRCM_POWER_STATE_VAL	(_PRCMU_BASE + 0x25C)
+#define PRCM_ARMITVAL31TO0	(_PRCMU_BASE + 0x260)
+#define PRCM_ARMITVAL63TO32	(_PRCMU_BASE + 0x264)
+#define PRCM_ARMITVAL95TO64	(_PRCMU_BASE + 0x268)
+#define PRCM_ARMITVAL127TO96	(_PRCMU_BASE + 0x26C)
+
+/* ARM WFI Standby signal register */
+#define PRCM_ARM_WFI_STANDBY    (_PRCMU_BASE + 0x130)
+
+/* IO force */
+#define PRCM_IOCR		(_PRCMU_BASE + 0x310)
+#define PRCM_IOCR_IOFORCE			0x1
+
+static u32 u8500_gpio_banks[] = {U8500_GPIOBANK0_BASE,
+				 U8500_GPIOBANK1_BASE,
+				 U8500_GPIOBANK2_BASE,
+				 U8500_GPIOBANK3_BASE,
+				 U8500_GPIOBANK4_BASE,
+				 U8500_GPIOBANK5_BASE,
+				 U8500_GPIOBANK6_BASE,
+				 U8500_GPIOBANK7_BASE,
+				 U8500_GPIOBANK8_BASE};
+
+static u32 u5500_gpio_banks[] = {U5500_GPIOBANK0_BASE,
+				 U5500_GPIOBANK1_BASE,
+				 U5500_GPIOBANK2_BASE,
+				 U5500_GPIOBANK3_BASE,
+				 U5500_GPIOBANK4_BASE,
+				 U5500_GPIOBANK5_BASE,
+				 U5500_GPIOBANK6_BASE,
+				 U5500_GPIOBANK7_BASE};
+
+static u32 ux500_gpio_wks[ARRAY_SIZE(u8500_gpio_banks)];
+
+inline int ux500_pm_arm_on_ext_clk(bool leave_arm_pll_on)
+{
+	return 0;
+}
+
+/* Decouple GIC from the interrupt bus */
+void ux500_pm_gic_decouple(void)
+{
+	writel(readl(PRCM_A9_MASK_REQ) | PRCM_A9_MASK_REQ_PRCM_A9_MASK_REQ,
+		PRCM_A9_MASK_REQ);
+
+	while (!readl(PRCM_A9_MASK_REQ))
+		cpu_relax();
+
+	/* TODO: Use the ack bit when possible */
+	udelay(GIC_FREEZE_DELAY); /* Wait for the GIC to freeze */
+}
+
+/* Recouple GIC with the interrupt bus */
+void ux500_pm_gic_recouple(void)
+{
+	writel((readl(PRCM_A9_MASK_REQ) & ~PRCM_A9_MASK_REQ_PRCM_A9_MASK_REQ),
+	       PRCM_A9_MASK_REQ);
+
+	/* TODO: Use the ack bit when possible */
+}
+
+#define GIC_NUMBER_REGS 5
+bool ux500_pm_gic_pending_interrupt(void)
+{
+	u32 pr; /* Pending register */
+	u32 er; /* Enable register */
+	int i;
+
+	/* 5 registers. STI & PPI not skipped */
+	for (i = 0; i < GIC_NUMBER_REGS; i++) {
+
+		pr = readl_relaxed(__io_address(U8500_GIC_DIST_BASE) +
+				   GIC_DIST_PENDING_SET + i * 4);
+		er = readl_relaxed(__io_address(U8500_GIC_DIST_BASE) +
+				   GIC_DIST_ENABLE_SET + i * 4);
+
+		if (pr & er)
+			return true; /* There is a pending interrupt */
+	}
+	return false;
+}
+
+#define GIC_NUMBER_SPI_REGS 4
+bool ux500_pm_prcmu_pending_interrupt(void)
+{
+	u32 it;
+	u32 im;
+	int i;
+
+	for (i = 0; i < GIC_NUMBER_SPI_REGS; i++) { /* There are 4 registers */
+
+		it = readl(PRCM_ARMITVAL31TO0 + i * 4);
+		im = readl(PRCM_ARMITMSK31TO0 + i * 4);
+
+		if (it & im)
+			return true; /* There is a pending interrupt */
+	}
+
+	return false;
+}
+
+void ux500_pm_prcmu_set_ioforce(bool enable)
+{
+	if (enable)
+		writel(readl(PRCM_IOCR) | PRCM_IOCR_IOFORCE, PRCM_IOCR);
+	else
+		writel(readl(PRCM_IOCR) & ~PRCM_IOCR_IOFORCE, PRCM_IOCR);
+}
+
+void ux500_pm_prcmu_copy_gic_settings(void)
+{
+	u32 er; /* Enable register */
+	int i;
+
+	for (i = 0; i < GIC_NUMBER_SPI_REGS; i++) { /* 4*32 SPI interrupts */
+		/* +1 due to skip STI and PPI */
+		er = readl_relaxed(__io_address(U8500_GIC_DIST_BASE) +
+			   GIC_DIST_ENABLE_SET + (i + 1) * 4);
+		writel(er, PRCM_ARMITMSK31TO0 + i * 4);
+	}
+}
+
+void ux500_pm_gpio_save_wake_up_status(void)
+{
+	int num_banks;
+	u32 *banks;
+	int i;
+
+	if (cpu_is_u5500()) {
+		num_banks = ARRAY_SIZE(u5500_gpio_banks);
+		banks = u5500_gpio_banks;
+	} else {
+		num_banks = ARRAY_SIZE(u8500_gpio_banks);
+		banks = u8500_gpio_banks;
+	}
+
+	nmk_gpio_clocks_enable();
+
+	for (i = 0; i < num_banks; i++)
+		ux500_gpio_wks[i] = readl(__io_address(banks[i]) + NMK_GPIO_WKS);
+
+	nmk_gpio_clocks_disable();
+}
+
+u32 ux500_pm_gpio_read_wake_up_status(unsigned int bank_num)
+{
+	if (WARN_ON(cpu_is_u5500() && bank_num >=
+		    ARRAY_SIZE(u5500_gpio_banks)))
+		return 0;
+
+	if (WARN_ON(cpu_is_u8500() && bank_num >=
+		    ARRAY_SIZE(u8500_gpio_banks)))
+		return 0;
+
+	return ux500_gpio_wks[bank_num];
+}
+
+/* Check if the other CPU is in WFI */
+bool ux500_pm_other_cpu_wfi(void)
+{
+	if (smp_processor_id()) {
+		/* We are CPU 1 => check if CPU0 is in WFI */
+		if (readl(PRCM_ARM_WFI_STANDBY) &
+		    PRCM_ARM_WFI_STANDBY_CPU0_WFI)
+			return true;
+	} else {
+		/* We are CPU 0 => check if CPU1 is in WFI */
+		if (readl(PRCM_ARM_WFI_STANDBY) &
+		    PRCM_ARM_WFI_STANDBY_CPU1_WFI)
+			return true;
+	}
+
+	return false;
+}
+
+/* PRCM_ACK_MB0_AP_PWRSTTR_STATUS */
+#define DB8500_PRCMU_STATUS_REGISTER	0x801b8e08
+#define DB5500_PRCMU_STATUS_REGISTER	0x80168f38
+
+enum prcmu_idle_stat ux500_pm_prcmu_idle_stat(void)
+{
+	u32 val;
+	void __iomem *prcmu_status_reg;
+
+	if (cpu_is_u8500())
+		prcmu_status_reg = __io_address(DB8500_PRCMU_STATUS_REGISTER);
+	else if (cpu_is_u5500())
+		prcmu_status_reg = __io_address(DB5500_PRCMU_STATUS_REGISTER);
+	else
+		ux500_unknown_soc();
+
+	val = readl(prcmu_status_reg) & 0xff;
+
+	return (enum prcmu_idle_stat)val;
+}
diff --git a/arch/arm/mach-ux500/pm/pm.h b/arch/arm/mach-ux500/pm/pm.h
deleted file mode 100644
index af95d7b2325..00000000000
--- a/arch/arm/mach-ux500/pm/pm.h
+++ /dev/null
@@ -1,18 +0,0 @@
-/*
- * Copyright (C) ST-Ericsson SA 2010
- * Author: Rickard Andersson <rickard.andersson@stericsson.com> for
- *         ST-Ericsson.
- * License terms: GNU General Public License (GPL) version 2
- *
- */
-
-#ifndef PM_COMMON_H
-#define PM_COMMON_H
-
-#ifdef CONFIG_PM
-struct dev_power_domain;
-extern struct dev_power_domain ux500_dev_power_domain;
-extern struct dev_power_domain ux500_amba_dev_power_domain;
-#endif
-
-#endif
diff --git a/arch/arm/mach-ux500/pm/scu.h b/arch/arm/mach-ux500/pm/scu.h
new file mode 100644
index 00000000000..a09e86a9d3c
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/scu.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) 2009 ST-Ericsson SA
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef __ASMARM_ARCH_SCU_H
+#define __ASMARM_ARCH_SCU_H
+
+#include <mach/hardware.h>
+
+#define SCU_BASE	U8500_SCU_BASE
+/*
+ *  * SCU registers
+ *   */
+#define SCU_CTRL        0x00
+#define SCU_CONFIG      0x04
+#define SCU_CPU_STATUS      0x08
+#define SCU_INVALIDATE      0x0c
+#define SCU_FPGA_REVISION   0x10
+
+#endif
diff --git a/arch/arm/mach-ux500/uart-db8500.c b/arch/arm/mach-ux500/uart-db8500.c
new file mode 100644
index 00000000000..fad9b9a13df
--- /dev/null
+++ b/arch/arm/mach-ux500/uart-db8500.c
@@ -0,0 +1,225 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Arun R Murthy <arun.murthy@stericsson.com>,
+ *	   Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/amba/serial.h>
+#include <mach/setup.h>
+#include <mach/hardware.h>
+#include <mach/context.h>
+
+#ifdef CONFIG_UX500_CONTEXT
+
+static struct {
+	struct clk *uart_clk;
+	void __iomem *base;
+	/* dr */
+	/* rsr_err */
+	u32 dma_wm;
+	u32 timeout;
+	/* fr */
+	u32 lcrh_rx;
+	u32 ilpr;
+	u32 ibrd;
+	u32 fbrd;
+	u32 lcrh_tx;
+	u32 cr;
+	u32 ifls;
+	u32 imsc;
+	/* ris */
+	/* mis */
+	/* icr */
+	u32 dmacr;
+	u32 xfcr;
+	u32 xon1;
+	u32 xon2;
+	u32 xoff1;
+	u32 xoff2;
+	/* itcr */
+	/* itip */
+	/* itop */
+	/* tdr */
+	u32 abcr;
+	/* absr */
+	/* abfmt */
+	/* abdr */
+	/* abdfr */
+	/* abmr */
+	u32 abimsc;
+	/* abris */
+	/* abmis */
+	/* abicr */
+	/* id_product_h_xy */
+	/* id_provider */
+	/* periphid0 */
+	/* periphid1 */
+	/* periphid2 */
+	/* periphid3 */
+	/* pcellid0 */
+	/* pcellid1 */
+	/* pcellid2 */
+	/* pcellid3 */
+} context_uart;
+
+static void save_uart(void)
+{
+	void __iomem *membase;
+
+	membase = context_uart.base;
+
+	clk_enable(context_uart.uart_clk);
+
+	context_uart.dma_wm = readl_relaxed(membase + ST_UART011_DMAWM);
+	context_uart.timeout = readl_relaxed(membase + ST_UART011_TIMEOUT);
+	context_uart.lcrh_rx = readl_relaxed(membase + ST_UART011_LCRH_RX);
+	context_uart.ilpr = readl_relaxed(membase + UART01x_ILPR);
+	context_uart.ibrd = readl_relaxed(membase + UART011_IBRD);
+	context_uart.fbrd = readl_relaxed(membase + UART011_FBRD);
+	context_uart.lcrh_tx = readl_relaxed(membase + ST_UART011_LCRH_TX);
+	context_uart.cr = readl_relaxed(membase + UART011_CR);
+	context_uart.ifls = readl_relaxed(membase + UART011_IFLS);
+	context_uart.imsc = readl_relaxed(membase + UART011_IMSC);
+	context_uart.dmacr = readl_relaxed(membase + UART011_DMACR);
+	context_uart.xfcr = readl_relaxed(membase + ST_UART011_XFCR);
+	context_uart.xon1 = readl_relaxed(membase + ST_UART011_XON1);
+	context_uart.xon2 = readl_relaxed(membase + ST_UART011_XON2);
+	context_uart.xoff1 = readl_relaxed(membase + ST_UART011_XOFF1);
+	context_uart.xoff2 = readl_relaxed(membase + ST_UART011_XOFF2);
+	context_uart.abcr = readl_relaxed(membase + ST_UART011_ABCR);
+	context_uart.abimsc = readl_relaxed(membase + ST_UART011_ABIMSC);
+
+	clk_disable(context_uart.uart_clk);
+}
+
+static void restore_uart(void)
+{
+	int cnt;
+	int retries = 100;
+	unsigned int cr;
+	void __iomem *membase;
+	u16 dummy;
+	bool show_warn = false;
+
+	membase = context_uart.base;
+	clk_enable(context_uart.uart_clk);
+
+	writew_relaxed(context_uart.ifls, membase + UART011_IFLS);
+	cr = UART01x_CR_UARTEN | UART011_CR_TXE | UART011_CR_LBE;
+
+	writew_relaxed(cr, membase + UART011_CR);
+	writew_relaxed(0, membase + UART011_FBRD);
+	writew_relaxed(1, membase + UART011_IBRD);
+	writew_relaxed(0, membase + ST_UART011_LCRH_RX);
+	if (context_uart.lcrh_tx != ST_UART011_LCRH_RX) {
+		int i;
+		/*
+		 * Wait 10 PCLKs before writing LCRH_TX register,
+		 * to get this delay write read only register 10 times
+		 */
+		for (i = 0; i < 10; ++i)
+			dummy = readw(membase + ST_UART011_LCRH_RX);
+		writew_relaxed(0, membase + ST_UART011_LCRH_TX);
+	}
+	writew(0, membase + UART01x_DR);
+	do {
+		if (!(readw(membase + UART01x_FR) & UART01x_FR_BUSY))
+			break;
+		cpu_relax();
+	} while (retries-- > 0);
+	if (retries < 0)
+		/*
+		 * We can't print out a warning here since the uart is
+		 * not fully restored. Do it later.
+		 */
+		show_warn = true;
+
+	writel_relaxed(context_uart.dma_wm, membase + ST_UART011_DMAWM);
+	writel_relaxed(context_uart.timeout, membase + ST_UART011_TIMEOUT);
+	writel_relaxed(context_uart.lcrh_rx, membase + ST_UART011_LCRH_RX);
+	writel_relaxed(context_uart.ilpr, membase + UART01x_ILPR);
+	writel_relaxed(context_uart.ibrd, membase + UART011_IBRD);
+	writel_relaxed(context_uart.fbrd, membase + UART011_FBRD);
+	/*
+	 * Wait 10 PCLKs before writing LCRH_TX register,
+	 * to get this delay write read only register 10-3
+	 * times, as already there are 3 writes after
+	 * ST_UART011_LCRH_RX
+	 */
+	for (cnt = 0; cnt < 7; cnt++)
+		dummy = readw(membase + ST_UART011_LCRH_RX);
+
+	writel_relaxed(context_uart.lcrh_tx, membase + ST_UART011_LCRH_TX);
+	writel_relaxed(context_uart.ifls, membase + UART011_IFLS);
+	writel_relaxed(context_uart.dmacr, membase + UART011_DMACR);
+	writel_relaxed(context_uart.xfcr, membase + ST_UART011_XFCR);
+	writel_relaxed(context_uart.xon1, membase + ST_UART011_XON1);
+	writel_relaxed(context_uart.xon2, membase + ST_UART011_XON2);
+	writel_relaxed(context_uart.xoff1, membase + ST_UART011_XOFF1);
+	writel_relaxed(context_uart.xoff2, membase + ST_UART011_XOFF2);
+	writel_relaxed(context_uart.abcr, membase + ST_UART011_ABCR);
+	writel_relaxed(context_uart.abimsc, membase + ST_UART011_ABIMSC);
+	writel_relaxed(context_uart.cr, membase + UART011_CR);
+	writel(context_uart.imsc, membase + UART011_IMSC);
+
+	clk_disable(context_uart.uart_clk);
+
+	if (show_warn)
+		pr_warning("%s:uart tx busy\n", __func__);
+}
+
+static int uart_context_notifier_call(struct notifier_block *this,
+				     unsigned long event, void *data)
+{
+	switch (event) {
+	case CONTEXT_APE_SAVE:
+		save_uart();
+		break;
+
+	case CONTEXT_APE_RESTORE:
+		restore_uart();
+		break;
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block uart_context_notifier = {
+	.notifier_call = uart_context_notifier_call,
+};
+
+#define __UART_BASE(soc, x)	soc##_UART##x##_BASE
+#define UART_BASE(soc, x)	__UART_BASE(soc, x)
+
+static int __init uart_context_notifier_init(void)
+{
+	unsigned long base;
+	static const char clkname[] __initconst
+		= "uart" __stringify(CONFIG_UX500_DEBUG_UART);
+
+	if (cpu_is_u8500())
+		base = UART_BASE(U8500, CONFIG_UX500_DEBUG_UART);
+	else if (cpu_is_u5500())
+		base = UART_BASE(U5500, CONFIG_UX500_DEBUG_UART);
+	else
+		ux500_unknown_soc();
+
+	context_uart.base = ioremap(base, SZ_4K);
+	context_uart.uart_clk = clk_get_sys(clkname, NULL);
+
+	if (IS_ERR(context_uart.uart_clk)) {
+		pr_err("%s:unable to get clk-uart%d\n", __func__,
+		       CONFIG_UX500_DEBUG_UART);
+		return -EINVAL;
+	}
+
+	return WARN_ON(context_ape_notifier_register(&uart_context_notifier));
+}
+arch_initcall(uart_context_notifier_init);
+#endif