ux500-pm: "ApDeepSleep" low power state support

This patch adds the "ApDeepSleep" low power state for the
U8500 platform. The ApDeepSleep is the lowest power state,
both the core/ape turned off. This low power mode can now be
entered/exited through the standard linux PM framework

echo mem > /sys/power/state will put the core into the deep sleep
state. The system can be woken up from USB insertion, console,
RTC events back to the execute mode.

This patch
 - cleans existing non-working deepsleep code from PRCMU f/w file
 - cleans redundant code for cpu hotplug
 - re-orgs the platform pm.c file for supporting
   uart2/gic/icn/prcc peripheral contexts
 - fixes FIDO_IR_ER: 258539, make UART2 console as a wakeup event
 - incorporates review comments from gerrit

The "ApSleep" power state can now be entered through the
echo standby > /sys/power/state command.

FIDO_IR_ER: 258539, add deep sleep support
FIDO_IR_ER: 258539, make UART2 console as a wakeup event

Signed-off-by: Sundar R Iyer <sundar.iyer@stericsson.com>
Acked-By: Biju C Das <biju.das@stericsson.com>
Signed-off-by: Mian Yousaf Kaukab <mian.yousaf.kaukab@stericsson.com>
Change-Id: I1ebb7fc193ec7dcea765c300277042464868493c
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/2355
Reviewed-by: Jonas ABERG <jonas.aberg@stericsson.com>

10 files changed, 1549 insertions, 537 deletions

diff --git a/arch/arm/mach-ux500/Makefile b/arch/arm/mach-ux500/Makefile
index 880a680bcf0..645657ccac7 100755
--- a/arch/arm/mach-ux500/Makefile
+++ b/arch/arm/mach-ux500/Makefile
@@ -22,7 +22,7 @@ obj-$(CONFIG_MACH_U5500_SIMULATOR)	+= board-u5500.o
 obj-$(CONFIG_SMP)			+= platsmp.o headsmp.o localtimer.o
 obj-$(CONFIG_U8500_CPUIDLE) 		+= cpuidle.o
 obj-$(CONFIG_U8500_CPUFREQ) 		+= cpufreq.o
-obj-$(CONFIG_U8500_PM)			+= pm.o
+obj-$(CONFIG_U8500_PM)			+= pm.o savecontext.o
 obj-$(CONFIG_SENSORS1P_MOP)            	+= sensors1p.o
 obj-$(CONFIG_USB)            		+= musb_db8500.o
 
diff --git a/arch/arm/mach-ux500/clock.c b/arch/arm/mach-ux500/clock.c
index 3e900ebea1a..30f08044061 100755
--- a/arch/arm/mach-ux500/clock.c
+++ b/arch/arm/mach-ux500/clock.c
@@ -676,10 +676,6 @@ static void u8500_amba_clk_enable(void)
 	writel(0xBF, IO_ADDRESS(U8500_PER3_BASE + 0xF000 + 0x04));
 	writel(~0x0 & ~(1 << 6), IO_ADDRESS(U8500_PER3_BASE + 0xF000 + 0x0C));
 
-	/* enable AMBA configuration clock ONLY */
-	writel(~0x0, IO_ADDRESS(U8500_PER6_BASE + 0xF000 + 0x04));
-	writel(~0x0, IO_ADDRESS(U8500_PER6_BASE + 0xF000 + 0x0C));
-
 	for (i = 0; i < ARRAY_SIZE(u8500_boot_clk); i++) {
 		boot_clks[i] = clk_get_sys(u8500_boot_clk[i], NULL);
 		clk_enable(boot_clks[i]);
diff --git a/arch/arm/mach-ux500/cpu-db8500.c b/arch/arm/mach-ux500/cpu-db8500.c
index f8bf7ac8e0a..992c0a1e90a 100644
--- a/arch/arm/mach-ux500/cpu-db8500.c
+++ b/arch/arm/mach-ux500/cpu-db8500.c
@@ -29,6 +29,9 @@ static struct map_desc u8500_io_desc[] __initdata = {
 	__IO_DEV_DESC(U8500_PRCMU_BASE, SZ_4K),
 	__IO_DEV_DESC(U8500_PRCMU_TCDM_BASE, SZ_4K),
 	__IO_DEV_DESC(U8500_ASIC_ID_BASE, SZ_4K),
+	__IO_DEV_DESC(U8500_ICN_BASE, SZ_8K),
+	{IO_ADDRESS(U8500_BACKUPRAM0_BASE),
+		__phys_to_pfn(U8500_BACKUPRAM0_BASE), SZ_8K, MT_BACKUP_RAM},
 };
 
 static struct map_desc u8500_ed_io_desc[] __initdata = {
diff --git a/arch/arm/mach-ux500/hotplug.c b/arch/arm/mach-ux500/hotplug.c
index f0c9471519c..65fad7251ee 100755
--- a/arch/arm/mach-ux500/hotplug.c
+++ b/arch/arm/mach-ux500/hotplug.c
@@ -13,81 +13,40 @@
 #include <linux/smp.h>
 #include <linux/completion.h>
 
-extern volatile int pen_release;
-
-static DECLARE_COMPLETION(cpu_killed);
-
-static inline void cpu_enter_lowpower(void)
-{
-	unsigned int v;
-	asm volatile("bl v7_flush_kern_cache_all\n"
-	"	mcr	p15, 0, %1, c7, c5, 0\n"
-	"	dsb\n"
-	/*
-	 * Turn off coherency
-	 */
-	"	mrc	p15, 0, %0, c1, c0, 1\n"
-	"	bic	%0, %0, #0x40\n"
-	"	mcr	p15, 0, %0, c1, c0, 1\n"
-	"	mrc	p15, 0, %0, c1, c0, 0\n"
-	"	bic	%0, %0, #0x04\n"
-	"	mcr	p15, 0, %0, c1, c0, 0\n"
-	  : "=&r" (v)
-	  : "r" (0)
-	  : "cc");
+#include <asm/cacheflush.h>
 
-}
+#include "pm.h"
 
-static inline void cpu_leave_lowpower(void)
-{
-	unsigned int v;
+extern volatile int pen_release;
 
-	asm volatile("mrc p15, 0, %0, c1, c0, 0\n"
-	"	orr	%0, %0, #0x04\n"
-	"	mcr	p15, 0, %0, c1, c0, 0\n"
-	"	mrc	p15, 0, %0, c1, c0, 1\n"
-	"	orr	%0, %0, #0x40\n"
-	"	mcr	p15, 0, %0, c1, c0, 1\n"
-	  : "=&r" (v)
-	  :
-	  : "cc");
-}
+static DECLARE_COMPLETION(cpu_killed);
 
 static inline void platform_do_lowpower(unsigned int cpu)
 {
+	flush_cache_all();
+
 	/*
-	 * there is no power-control hardware on this platform, so all
-	 * we can do is put the core into WFI; this is safe as the calling
-	 * code will have already disabled interrupts
+	 * backup cpu specific context
+	 * one woken upm we wud have restored
+	 * our cpu specific context
 	 */
-	for (;;) {
+	/* for SVP builds. SVP doesnt incorporate
+	 * CONFIG_PM yet!
+	 */
+#ifdef CONFIG_PM
+	ux500_cpu_context_deepsleep(cpu);
+#else
+	__asm__ __volatile__("dsb\n\t" "wfi\n\t" \
+			: : : "memory");
+#endif
 
+	for (;;) {
 		/*
-		 * here's the WFI
+		 * whiling around till some one releases
+		 * the holding pen; if true, v r done!
 		 */
-		asm(".word	0xe320f003\n"
-		    :
-		    :
-		    : "memory", "cc");
-
-		if (pen_release == cpu) {
-			/*
-			 * OK, proper wakeup, we're done
-			 */
+		if (pen_release == cpu)
 			break;
-		}
-
-		/*
-		 * getting here, means that we have come out of WFI without
-		 * having been woken up - this shouldn't happen
-		 *
-		 * The trouble is, letting people know about this is not really
-		 * possible, since we are currently running incoherently, and
-		 * therefore cannot safely call printk() or anything else
-		 */
-#ifdef DEBUG
-		printk("CPU%u: spurious wakeup call\n", cpu);
-#endif
 	}
 }
 
@@ -116,19 +75,7 @@ void platform_cpu_die(unsigned int cpu)
 	printk(KERN_NOTICE "CPU%u: shutdown\n", cpu);
 	complete(&cpu_killed);
 
-	/*
-	 * we're ready for shutdown now, so do it
-	 */
-	/* FIXME : skipping this for a temp hack */
-	/* cpu_enter_lowpower(); */
 	platform_do_lowpower(cpu);
-
-	/*
-	 * bring this CPU back into the world of cache
-	 * coherency, and then restore interrupts
-	 */
-	/* FIXME : skipping this for a temp hack */
-	/* cpu_leave_lowpower(); */
 }
 
 int mach_cpu_disable(unsigned int cpu)
diff --git a/arch/arm/mach-ux500/include/mach/prcmu-regs.h b/arch/arm/mach-ux500/include/mach/prcmu-regs.h
index 1b1c2287b63..b852663a270 100755
--- a/arch/arm/mach-ux500/include/mach/prcmu-regs.h
+++ b/arch/arm/mach-ux500/include/mach/prcmu-regs.h
@@ -40,7 +40,8 @@
 #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_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)
diff --git a/arch/arm/mach-ux500/pm.c b/arch/arm/mach-ux500/pm.c
index ef2aefa51ea..509c2c59c02 100755
--- a/arch/arm/mach-ux500/pm.c
+++ b/arch/arm/mach-ux500/pm.c
@@ -9,16 +9,18 @@
 
 /* U8500 platform suspend/resume support */
 
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/suspend.h>
-#include <linux/errno.h>
-#include <linux/time.h>
+#include "pm.h"
 
-#include <asm/memory.h>
-#include <asm/system.h>
+#define U8500_EXT_BACKUPRAM_ADDR	(0x80151FDC)
+#define U8500_CPU0_PUBLIC_BACKUP	(0x80151FD8)
+#define U8500_CPU1_PUBLIC_BACKUP	(0x80151FE0)
+#define U8500_BACKUPRAM_SIZE		(64 * 1024)
 
-#include <mach/prcmu-fw-api.h>
+#define ENABLE	(1)
+#define DISABLE	(0)
+
+/* pointers for backups/contexts to be saved */
+dma_addr_t *arm_cntxt_cpu0, *arm_cntxt_cpu1, *ux500_backup_ptr;
 
 static int u8500_pm_begin(suspend_state_t state)
 {
@@ -30,25 +32,725 @@ static int u8500_pm_prepare(void)
 	return 0;
 }
 
+/* GPIO context */
+static struct ux500_gpio_regs gpio_context[UX500_NR_GPIO_BANKS];
+
+/* gpio bank addrs */
+uint32_t gpio_banks[] = {
+	GPIO_BANK_BASE(0),
+	GPIO_BANK_BASE(1),
+	GPIO_BANK_BASE(2),
+	GPIO_BANK_BASE(3),
+	GPIO_BANK_BASE(4),
+	GPIO_BANK_BASE(5),
+	GPIO_BANK_BASE(6),
+	GPIO_BANK_BASE(7),
+	GPIO_BANK_BASE(8),
+};
+
+/**
+ * ux500_save_gpio_context() - save GPIO periph context
+ *
+ */
+static void ux500_save_gpio_context(void)
+{
+	uint32_t i, base;
+
+	for (i = 0; i < UX500_NR_GPIO_BANKS; i++) {
+
+		base = gpio_banks[i];
+
+		gpio_context[i].gpio_dat = readl(base + NMK_GPIO_DAT);
+		gpio_context[i].gpio_dat_set = readl(base + NMK_GPIO_DATS);
+		gpio_context[i].gpio_dat_clr = readl(base + NMK_GPIO_DATC);
+		gpio_context[i].gpio_pdis = readl(base + NMK_GPIO_PDIS);
+		gpio_context[i].gpio_dir = readl(base + NMK_GPIO_DIR);
+		gpio_context[i].gpio_dir_set = readl(base + NMK_GPIO_DIRS);
+		gpio_context[i].gpio_dir_clr = readl(base + NMK_GPIO_DIRC);
+		gpio_context[i].gpio_slpm = readl(base + NMK_GPIO_SLPC);
+		gpio_context[i].gpio_altfunc_a = readl(base + NMK_GPIO_AFSLA);
+		gpio_context[i].gpio_altfunc_b = readl(base + NMK_GPIO_AFSLB);
+		gpio_context[i].gpio_rimsc = readl(base + NMK_GPIO_RIMSC);
+		gpio_context[i].gpio_fsmsc = readl(base + NMK_GPIO_FWIMSC);
+		gpio_context[i].gpio_int_status = readl(base + NMK_GPIO_IS);
+		gpio_context[i].gpio_int_clr = readl(base + NMK_GPIO_IC);
+		gpio_context[i].gpio_rwmsc = readl(base + NMK_GPIO_RWIMSC);
+		gpio_context[i].gpio_fwmsc = readl(base + NMK_GPIO_FWIMSC);
+		gpio_context[i].gpio_wakeup_status = readl(base + NMK_GPIO_WKS);
+	}
+
+	return;
+}
+
+/**
+ * ux500_restore_gpio_context() - restore GPIO periph context
+ *
+ */
+static void ux500_restore_gpio_context(void)
+{
+	uint32_t i, base;
+
+	for (i = 0; i < UX500_NR_GPIO_BANKS; i++) {
+
+		base = gpio_banks[i];
+
+		writel(gpio_context[i].gpio_dat, base + NMK_GPIO_DAT);
+		writel(gpio_context[i].gpio_dat_set, base + NMK_GPIO_DATS);
+		writel(gpio_context[i].gpio_dat_clr, base + NMK_GPIO_DATC);
+		writel(gpio_context[i].gpio_pdis, base + NMK_GPIO_PDIS);
+		writel(gpio_context[i].gpio_dir, base + NMK_GPIO_DIR);
+		writel(gpio_context[i].gpio_dir_set, base + NMK_GPIO_DIRS);
+		writel(gpio_context[i].gpio_dir_clr, base + NMK_GPIO_DIRC);
+		writel(gpio_context[i].gpio_slpm, base + NMK_GPIO_SLPC);
+		writel(gpio_context[i].gpio_altfunc_a, base + NMK_GPIO_AFSLA);
+		writel(gpio_context[i].gpio_altfunc_b, base + NMK_GPIO_AFSLB);
+		writel(gpio_context[i].gpio_rimsc, base + NMK_GPIO_RIMSC);
+		writel(gpio_context[i].gpio_fsmsc, base + NMK_GPIO_FWIMSC);
+		writel(gpio_context[i].gpio_int_status, base + NMK_GPIO_IS);
+		writel(gpio_context[i].gpio_int_clr, base + NMK_GPIO_IC);
+		writel(gpio_context[i].gpio_rwmsc, base + NMK_GPIO_RWIMSC);
+		writel(gpio_context[i].gpio_fwmsc, base + NMK_GPIO_FWIMSC);
+		writel(gpio_context[i].gpio_wakeup_status, base + NMK_GPIO_WKS);
+	}
+
+	return;
+}
+
+/* GIC context */
+/* local copy of max irqs */
+static uint32_t ux500_max_irq;
+
+/**
+ * ux500_disable_gic_interrupts() - disable all GIC interrupts
+ *
+ */
+static inline void ux500_disable_gic_interrupts(void)
+{
+	uint32_t base = IO_ADDRESS(U8500_GIC_DIST_BASE);
+
+	/* disable all interrupts in the gic */
+	writel(0xffffffff, base + GIC_DIST_ENABLE_CLEAR + 0x4);
+	writel(0xffffffff, base + GIC_DIST_ENABLE_CLEAR + 0x8);
+	writel(0xffffffff, base + GIC_DIST_ENABLE_CLEAR + 0xc);
+	writel(0xffffffff, base + GIC_DIST_ENABLE_CLEAR + 0x10);
+
+	return;
+}
+
+/**
+ * ux500_save_gic_context() - save GIC context
+ *
+ * this function saves the gic context and also disables
+ * all gic interrupts further.
+ *
+ */
+void ux500_save_gic_context(void)
+{
+	uint32_t i, j;
+	uint32_t base = IO_ADDRESS(U8500_GIC_DIST_BASE);
+
+	for (i = 32, j = 0; i < ux500_max_irq; i += 16)
+		gic_dist_config[j++] =
+			readl(base + GIC_DIST_CONFIG + (i * 4)/16);
+
+	for (i = 32, j = 0; i < ux500_max_irq; i += 4)
+		gic_dist_target[j++] =
+			readl(base + GIC_DIST_TARGET + (i * 4)/4);
+
+	for (i = 0, j = 0; i < ux500_max_irq; i += 32)
+		gic_enable_set_reg[j++] =
+			readl(base + GIC_DIST_ENABLE_SET + (i * 4)/32);
+
+	ux500_disable_gic_interrupts();
+
+	return;
+}
+
+/**
+ * ux500_restore_gic_context() - restore GIC context
+ *
+ */
+void ux500_restore_gic_context(void)
+{
+	uint32_t i, j;
+	uint32_t base = IO_ADDRESS(U8500_GIC_DIST_BASE);
+
+	writel(0, base + GIC_DIST_CTRL);
+
+	/* read back the register values */
+	for (i = 32, j = 0; i < ux500_max_irq; i += 16)
+		writel(gic_dist_config[j++],
+				base + GIC_DIST_CONFIG + (i * 4)/16);
+
+	for (i = 32, j = 0; i < ux500_max_irq; i += 4)
+		writel(gic_dist_target[j++],
+				base + GIC_DIST_TARGET + (i * 4)/4);
+
+	for (i = 0, j = 0; i < ux500_max_irq; i += 32)
+		writel(gic_enable_set_reg[j++],
+				base + GIC_DIST_ENABLE_SET + (i * 4)/32);
+
+	/*
+	 * enables the GIC controller after we restore the
+	 * interrupt configuration
+	 */
+	writel(1, base + GIC_DIST_CTRL);
+
+	return;
+}
+
+/* PRCC context */
+static struct ux500_prcc_contxt prcc_contxt[UX500_NR_PRCC_BANKS];
+#define PERIPH_PRCC_BASE_OFFSET		(0xf000)
+#define PERIPH5_PRCC_BASE_OFFSET	(0x1f000)
+
+/* prcc bank addrs */
+uint32_t prcc_banks[] = {
+	PRCC_BANK_BASE(1),
+	PRCC_BANK_BASE(2),
+	PRCC_BANK_BASE(3),
+	PRCC_BANK_BASE(5),
+	PRCC_BANK_BASE(6),
+};
+
+/**
+ * ux500_save_prcc_context() - save periph clock context
+ *
+ */
+void ux500_save_prcc_context(void)
+{
+	uint32_t i, base;
+
+	for (i = 0; i < UX500_NR_PRCC_BANKS; i++) {
+		base = prcc_banks[i];
+		/* periph 5 has a different layout */
+		if (base == PRCC_BANK_BASE(5))
+			base = prcc_banks[i] + PERIPH5_PRCC_BASE_OFFSET;
+		else
+			base = prcc_banks[i] + PERIPH_PRCC_BASE_OFFSET;
+
+		prcc_contxt[i].periph_bus_clk = readl(base + PRCC_BCK_STATUS);
+		prcc_contxt[i].periph_kern_clk = readl(base + PRCC_KCK_STATUS);
+	}
+	return;
+}
+
+/**
+ * ux500_restore_prcc_context() - restore periph clock context
+ *
+ */
+void ux500_restore_prcc_context(void)
+{
+	uint32_t i, base;
+
+	for (i = 0; i < UX500_NR_PRCC_BANKS; i++) {
+		base = prcc_banks[i];
+		/* periph 5 has a different layout */
+		if (base == PRCC_BANK_BASE(5))
+			base = prcc_banks[i] + PERIPH5_PRCC_BASE_OFFSET;
+		else
+			base = prcc_banks[i] + PERIPH_PRCC_BASE_OFFSET;
+
+		writel(prcc_contxt[i].periph_bus_clk, base + PRCC_BCK_EN);
+		writel(prcc_contxt[i].periph_kern_clk, base + PRCC_KCK_EN);
+	}
+
+	return;
+}
+
+/* UART2(console) context */
+static void __iomem *uart_backup_base, *uart_register_base;
+
+/**
+ * ux500_save_uart_context() - save uart2 (console)  context
+ *
+ */
+void ux500_save_uart_context(void)
+{
+	/* FIXME : blind memcpy must be replaced */
+	memcpy(uart_backup_base, uart_register_base, SZ_4K);
+	return;
+}
+
+/**
+ * ux500_restore_uart_context() - restore uart2 (console)  context
+ *
+ */
+void ux500_restore_uart_context(void)
+{
+	memcpy(uart_register_base, uart_backup_base, SZ_4K);
+	return;
+}
+
+/* ST ICN context */
+static struct ux500_interconnect_contxt icn_contxt;
+
+/**
+ * ux500_save_icn_context() - save ICN context
+ *
+ */
+void ux500_save_icn_context(void)
+{
+	uint32_t base = IO_ADDRESS(U8500_ICN_BASE);
+
+	icn_contxt.ux500_hibw1_esram_in_pri_regs[0] =
+		readb(base + NODE_HIBW1_ESRAM_IN_0_PRIORITY_REG);
+	icn_contxt.ux500_hibw1_esram_in_pri_regs[1] =
+		readb(base + NODE_HIBW1_ESRAM_IN_1_PRIORITY_REG);
+	icn_contxt.ux500_hibw1_esram_in_pri_regs[2] =
+		readb(base + NODE_HIBW1_ESRAM_IN_2_PRIORITY_REG);
+
+	icn_contxt.ux500_hibw1_esram_in0_arb_regs[0] =
+		readb(base + NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_hibw1_esram_in0_arb_regs[1] =
+		readb(base + NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_hibw1_esram_in0_arb_regs[2] =
+		readb(base + NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT_REG);
+
+	icn_contxt.ux500_hibw1_esram_in1_arb_regs[0] =
+		readb(base + NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_hibw1_esram_in1_arb_regs[1] =
+		readb(base + NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_hibw1_esram_in1_arb_regs[2] =
+		readb(base + NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT_REG);
+
+	icn_contxt.ux500_hibw1_esram_in2_arb_regs[0] =
+		readb(base + NODE_HIBW1_ESRAM_IN_2_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_hibw1_esram_in2_arb_regs[1] =
+		readb(base + NODE_HIBW1_ESRAM_IN_2_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_hibw1_esram_in2_arb_regs[2] =
+		readb(base + NODE_HIBW1_ESRAM_IN_2_ARB_3_LIMIT_REG);
+
+	icn_contxt.ux500_hibw1_ddr_in_prio_regs[0] =
+		readb(base + NODE_HIBW1_DDR_IN_0_PRIORITY_REG);
+	icn_contxt.ux500_hibw1_ddr_in_prio_regs[1] =
+		readb(base + NODE_HIBW1_DDR_IN_1_PRIORITY_REG);
+	icn_contxt.ux500_hibw1_ddr_in_prio_regs[2] =
+		readb(base + NODE_HIBW1_DDR_IN_2_PRIORITY_REG);
+
+	icn_contxt.ux500_hibw1_ddr_in_limit_regs[0] =
+		readb(base + NODE_HIBW1_DDR_IN_0_LIMIT_REG);
+	icn_contxt.ux500_hibw1_ddr_in_limit_regs[1] =
+		readb(base + NODE_HIBW1_DDR_IN_1_LIMIT_REG);
+	icn_contxt.ux500_hibw1_ddr_in_limit_regs[2] =
+		readb(base + NODE_HIBW1_DDR_IN_2_LIMIT_REG);
+
+	icn_contxt.ux500_hibw1_ddr_out_prio_reg =
+		readb(base + NODE_HIBW1_DDR_OUT_0_PRIORITY_REG);
+
+	icn_contxt.ux500_hibw2_esram_in_pri_regs[0] =
+		readb(base + NODE_HIBW2_ESRAM_IN_0_PRIORITY_REG);
+	icn_contxt.ux500_hibw2_esram_in_pri_regs[1] =
+		readb(base + NODE_HIBW2_ESRAM_IN_1_PRIORITY_REG);
+
+	icn_contxt.ux500_hibw2_esram_in0_arblimit_regs[0] =
+		readb(base + NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_hibw2_esram_in0_arblimit_regs[1] =
+		 readb(base + NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_hibw2_esram_in0_arblimit_regs[2] =
+		 readb(base + NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT_REG);
+
+	icn_contxt.ux500_hibw2_esram_in1_arblimit_regs[0] =
+		readb(base + NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_hibw2_esram_in1_arblimit_regs[1] =
+		readb(base + NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_hibw2_esram_in1_arblimit_regs[2] =
+		readb(base + NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT_REG);
+
+	icn_contxt.ux500_hibw2_ddr_in_prio_regs[0] =
+		readb(base + NODE_HIBW2_DDR_IN_0_PRIORITY_REG);
+	icn_contxt.ux500_hibw2_ddr_in_prio_regs[1] =
+		readb(base + NODE_HIBW2_DDR_IN_1_PRIORITY_REG);
+	icn_contxt.ux500_hibw2_ddr_in_prio_regs[2] =
+		readb(base + NODE_HIBW2_DDR_IN_2_PRIORITY_REG);
+	icn_contxt.ux500_hibw2_ddr_in_prio_regs[3] =
+		readb(base + NODE_HIBW2_DDR_IN_3_PRIORITY_REG);
+
+	icn_contxt.ux500_hibw2_ddr_in_limit_regs[0] =
+		readb(base + NODE_HIBW2_DDR_IN_0_LIMIT_REG);
+	icn_contxt.ux500_hibw2_ddr_in_limit_regs[1] =
+		readb(base + NODE_HIBW2_DDR_IN_1_LIMIT_REG);
+
+	icn_contxt.ux500_esram12_in_prio_regs[0] =
+		readb(base + NODE_ESRAM1_2_IN_0_PRIORITY_REG);
+	icn_contxt.ux500_esram12_in_prio_regs[1] =
+		readb(base + NODE_ESRAM1_2_IN_1_PRIORITY_REG);
+	icn_contxt.ux500_esram12_in_prio_regs[2] =
+		readb(base + NODE_ESRAM1_2_IN_2_PRIORITY_REG);
+	icn_contxt.ux500_esram12_in_prio_regs[3] =
+		readb(base + NODE_ESRAM1_2_IN_3_PRIORITY_REG);
+
+	icn_contxt.ux500_esram12_in_arb_lim_regs[0] =
+		readb(base + NODE_ESRAM1_2_IN_0_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_esram12_in_arb_lim_regs[1] =
+		readb(base + NODE_ESRAM1_2_IN_0_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_esram12_in_arb_lim_regs[2] =
+		readb(base + NODE_ESRAM1_2_IN_1_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_esram12_in_arb_lim_regs[3] =
+		readb(base + NODE_ESRAM1_2_IN_1_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_esram12_in_arb_lim_regs[4] =
+		readb(base + NODE_ESRAM1_2_IN_2_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_esram12_in_arb_lim_regs[5] =
+		readb(base + NODE_ESRAM1_2_IN_2_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_esram12_in_arb_lim_regs[6] =
+		readb(base + NODE_ESRAM1_2_IN_3_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_esram12_in_arb_lim_regs[7] =
+		readb(base + NODE_ESRAM1_2_IN_3_ARB_2_LIMIT_REG);
+
+	icn_contxt.ux500_esram34_in_prio_regs[0] =
+		readb(base + NODE_ESRAM3_4_IN_0_PRIORITY_REG);
+	icn_contxt.ux500_esram34_in_prio_regs[1] =
+		readb(base + NODE_ESRAM3_4_IN_1_PRIORITY_REG);
+	icn_contxt.ux500_esram34_in_prio_regs[2] =
+		readb(base + NODE_ESRAM3_4_IN_2_PRIORITY_REG);
+	icn_contxt.ux500_esram34_in_prio_regs[3] =
+		readb(base + NODE_ESRAM3_4_IN_3_PRIORITY_REG);
+
+	icn_contxt.ux500_esram34_in_arb_lim_regs[0] =
+		readb(base + NODE_ESRAM3_4_IN_0_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_esram34_in_arb_lim_regs[1] =
+		readb(base + NODE_ESRAM3_4_IN_0_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_esram34_in_arb_lim_regs[2] =
+		readb(base + NODE_ESRAM3_4_IN_1_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_esram34_in_arb_lim_regs[3] =
+		readb(base + NODE_ESRAM3_4_IN_1_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_esram34_in_arb_lim_regs[4] =
+		readb(base + NODE_ESRAM3_4_IN_2_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_esram34_in_arb_lim_regs[5] =
+		readb(base + NODE_ESRAM3_4_IN_2_ARB_2_LIMIT_REG);
+	icn_contxt.ux500_esram34_in_arb_lim_regs[6] =
+		readb(base + NODE_ESRAM3_4_IN_3_ARB_1_LIMIT_REG);
+	icn_contxt.ux500_esram34_in_arb_lim_regs[7] =
+		readb(base + NODE_ESRAM3_4_IN_3_ARB_2_LIMIT_REG);
+
+	return;
+}
+
+/**
+ * ux500_restore_icn_context() - restore ICN context
+ *
+ */
+void ux500_restore_icn_context(void)
+{
+	uint32_t base = IO_ADDRESS(U8500_ICN_BASE);
+
+	writel(icn_contxt.ux500_hibw1_esram_in_pri_regs[0],
+		base + NODE_HIBW1_ESRAM_IN_0_PRIORITY_REG);
+	writel(icn_contxt.ux500_hibw1_esram_in_pri_regs[1],
+		base + NODE_HIBW1_ESRAM_IN_1_PRIORITY_REG);
+	writel(icn_contxt.ux500_hibw1_esram_in_pri_regs[2],
+		base + NODE_HIBW1_ESRAM_IN_2_PRIORITY_REG);
+
+	writel(icn_contxt.ux500_hibw1_esram_in0_arb_regs[0],
+		base + NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw1_esram_in0_arb_regs[1],
+		base + NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw1_esram_in0_arb_regs[2],
+		base + NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT_REG);
+
+	writel(icn_contxt.ux500_hibw1_esram_in1_arb_regs[0],
+		base + NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw1_esram_in1_arb_regs[1],
+		base + NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw1_esram_in1_arb_regs[2],
+		base + NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT_REG);
+
+	writel(icn_contxt.ux500_hibw1_esram_in2_arb_regs[0],
+		base + NODE_HIBW1_ESRAM_IN_2_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw1_esram_in2_arb_regs[1],
+		base + NODE_HIBW1_ESRAM_IN_2_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw1_esram_in2_arb_regs[2],
+		base + NODE_HIBW1_ESRAM_IN_2_ARB_3_LIMIT_REG);
+
+	writel(icn_contxt.ux500_hibw1_ddr_in_prio_regs[0],
+		base + NODE_HIBW1_DDR_IN_0_PRIORITY_REG);
+	writel(icn_contxt.ux500_hibw1_ddr_in_prio_regs[1],
+		base + NODE_HIBW1_DDR_IN_1_PRIORITY_REG);
+	writel(icn_contxt.ux500_hibw1_ddr_in_prio_regs[2],
+		base + NODE_HIBW1_DDR_IN_2_PRIORITY_REG);
+
+	writel(icn_contxt.ux500_hibw1_ddr_in_limit_regs[0],
+		base + NODE_HIBW1_DDR_IN_0_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw1_ddr_in_limit_regs[1],
+		base + NODE_HIBW1_DDR_IN_1_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw1_ddr_in_limit_regs[2],
+		base + NODE_HIBW1_DDR_IN_2_LIMIT_REG);
+
+	writel(icn_contxt.ux500_hibw1_ddr_out_prio_reg,
+		base + NODE_HIBW1_DDR_OUT_0_PRIORITY_REG);
+
+	writel(icn_contxt.ux500_hibw2_esram_in_pri_regs[0],
+		base + NODE_HIBW2_ESRAM_IN_0_PRIORITY_REG);
+	writel(icn_contxt.ux500_hibw2_esram_in_pri_regs[1],
+		base + NODE_HIBW2_ESRAM_IN_1_PRIORITY_REG);
+
+	writel(icn_contxt.ux500_hibw2_esram_in0_arblimit_regs[0],
+		base + NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw2_esram_in0_arblimit_regs[1],
+		base + NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw2_esram_in0_arblimit_regs[2],
+		base + NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT_REG);
+
+	writel(icn_contxt.ux500_hibw2_esram_in1_arblimit_regs[0],
+		base + NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw2_esram_in1_arblimit_regs[1],
+		base + NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw2_esram_in1_arblimit_regs[2],
+		base + NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT_REG);
+
+	writel(icn_contxt.ux500_hibw2_ddr_in_prio_regs[0],
+		base + NODE_HIBW2_DDR_IN_0_PRIORITY_REG);
+	writel(icn_contxt.ux500_hibw2_ddr_in_prio_regs[1],
+		base + NODE_HIBW2_DDR_IN_1_PRIORITY_REG);
+	writel(icn_contxt.ux500_hibw2_ddr_in_prio_regs[2],
+		base + NODE_HIBW2_DDR_IN_2_PRIORITY_REG);
+	writel(icn_contxt.ux500_hibw2_ddr_in_prio_regs[3],
+		base + NODE_HIBW2_DDR_IN_3_PRIORITY_REG);
+
+	writel(icn_contxt.ux500_hibw2_ddr_in_limit_regs[0],
+		base + NODE_HIBW2_DDR_IN_0_LIMIT_REG);
+	writel(icn_contxt.ux500_hibw2_ddr_in_limit_regs[1],
+		base + NODE_HIBW2_DDR_IN_1_LIMIT_REG);
+
+	writel(icn_contxt.ux500_esram12_in_prio_regs[0],
+		base + NODE_ESRAM1_2_IN_0_PRIORITY_REG);
+	writel(icn_contxt.ux500_esram12_in_prio_regs[1],
+		base + NODE_ESRAM1_2_IN_1_PRIORITY_REG);
+	writel(icn_contxt.ux500_esram12_in_prio_regs[2],
+		base + NODE_ESRAM1_2_IN_2_PRIORITY_REG);
+	writel(icn_contxt.ux500_esram12_in_prio_regs[3],
+		base + NODE_ESRAM1_2_IN_3_PRIORITY_REG);
+
+	writel(icn_contxt.ux500_esram12_in_arb_lim_regs[0],
+		base + NODE_ESRAM1_2_IN_0_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_esram12_in_arb_lim_regs[1],
+		base + NODE_ESRAM1_2_IN_0_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_esram12_in_arb_lim_regs[2],
+		base + NODE_ESRAM1_2_IN_1_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_esram12_in_arb_lim_regs[3],
+		base + NODE_ESRAM1_2_IN_1_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_esram12_in_arb_lim_regs[4],
+		base + NODE_ESRAM1_2_IN_2_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_esram12_in_arb_lim_regs[5],
+		base + NODE_ESRAM1_2_IN_2_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_esram12_in_arb_lim_regs[6],
+		base + NODE_ESRAM1_2_IN_3_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_esram12_in_arb_lim_regs[7],
+		base + NODE_ESRAM1_2_IN_3_ARB_2_LIMIT_REG);
+
+	writel(icn_contxt.ux500_esram34_in_prio_regs[0],
+		base + NODE_ESRAM3_4_IN_0_PRIORITY_REG);
+	writel(icn_contxt.ux500_esram34_in_prio_regs[1],
+		base + NODE_ESRAM3_4_IN_1_PRIORITY_REG);
+	writel(icn_contxt.ux500_esram34_in_prio_regs[2],
+		base + NODE_ESRAM3_4_IN_2_PRIORITY_REG);
+	writel(icn_contxt.ux500_esram34_in_prio_regs[3],
+		base + NODE_ESRAM3_4_IN_3_PRIORITY_REG);
+
+	writel(icn_contxt.ux500_esram34_in_arb_lim_regs[0],
+		base + NODE_ESRAM3_4_IN_0_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_esram34_in_arb_lim_regs[1],
+		base + NODE_ESRAM3_4_IN_0_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_esram34_in_arb_lim_regs[2],
+		base + NODE_ESRAM3_4_IN_1_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_esram34_in_arb_lim_regs[3],
+		base + NODE_ESRAM3_4_IN_1_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_esram34_in_arb_lim_regs[4],
+		base + NODE_ESRAM3_4_IN_2_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_esram34_in_arb_lim_regs[5],
+		base + NODE_ESRAM3_4_IN_2_ARB_2_LIMIT_REG);
+	writel(icn_contxt.ux500_esram34_in_arb_lim_regs[6],
+		base + NODE_ESRAM3_4_IN_3_ARB_1_LIMIT_REG);
+	writel(icn_contxt.ux500_esram34_in_arb_lim_regs[7],
+		base + NODE_ESRAM3_4_IN_3_ARB_2_LIMIT_REG);
+
+	return;
+}
+
+/* SCU context */
+static struct ux500_scu_context scu_contxt;
+
+/**
+ * ux500_save_scu_context() - save SCU context
+ *
+ */
+void ux500_save_scu_context(void)
+{
+	uint32_t base = IO_ADDRESS(UX500_SCU_BASE);
+
+	scu_contxt.scu_ctrl 		= readl(base + SCU_CTRL);
+	scu_contxt.scu_cpu_pwrstatus 	= readl(base + SCU_CPU_STATUS);
+	scu_contxt.scu_inv_all_nonsecure = readl(base + SCU_INVALIDATE);
+	scu_contxt.scu_filter_start_addr = readl(base + SCU_FILTER_STARTADDR);
+	scu_contxt.scu_filter_end_addr	= readl(base + SCU_FILTER_ENDADDR);
+	scu_contxt.scu_access_ctrl_sac	= readl(base + SCU_ACCESS_CTRL_SAC);
+
+	return;
+}
+
+/**
+ * ux500_restore_scu_context() - restore SCU context
+ *
+ */
+void ux500_restore_scu_context(void)
+{
+	uint32_t base = IO_ADDRESS(UX500_SCU_BASE);
+
+	writel(scu_contxt.scu_ctrl, 		base + SCU_CTRL);
+	writel(scu_contxt.scu_cpu_pwrstatus, 	base + SCU_CPU_STATUS);
+	writel(scu_contxt.scu_inv_all_nonsecure, base + SCU_INVALIDATE);
+	writel(scu_contxt.scu_filter_start_addr, base + SCU_FILTER_STARTADDR);
+	writel(scu_contxt.scu_filter_end_addr, 	base + SCU_FILTER_ENDADDR);
+	writel(scu_contxt.scu_access_ctrl_sac, 	base + SCU_ACCESS_CTRL_SAC);
+
+	return;
+}
+
+/**
+ * ux500_save_peripheral_context() - save all peripheral context
+ *
+ */
+static void ux500_save_peripheral_context(void)
+{
+	/* save the inter-connect per */
+	ux500_save_icn_context();
+	/* save uart2 per */
+	ux500_save_uart_context();
+	/* save gpio per */
+	ux500_save_gpio_context();
+	/* save periph clock configs */
+	ux500_save_prcc_context();
+	/* save the scu config */
+	ux500_save_scu_context();
+
+	return;
+}
+
+/**
+ * ux500_restore_peripheral_context() - restore peripheral context
+ *
+ */
+static void ux500_restore_peripheral_context(void)
+{
+	/* restore scu */
+	ux500_restore_scu_context();
+	/* restore inter-connct */
+	ux500_restore_icn_context();
+	/* restore periph clock */
+	ux500_restore_prcc_context();
+	/* restore gpio per */
+	ux500_restore_gpio_context();
+	/* restore uart2 */
+	ux500_restore_uart_context();
+
+	return;
+}
+
+/**
+ * ux500_save_core_context() - save core(gic/..) context
+ *
+ */
+static void ux500_save_core_context(void)
+{
+	ux500_save_gic_context();
+	return;
+}
+
+/*
+ * FIXME : replace this hack for console gpio
+ * 	   this config makes the gpio29 pins pull up
+ *	   by deflt
+ */
+#define CONSOLE_GPIO_HACK	(0x60000000)
+
 static int u8500_pm_enter(suspend_state_t state)
 {
-	int ret;
+	uint32_t ret = 0;
+	uint32_t cpu = smp_processor_id();
+
+	/* FIXME : replace these with generic APIs. this is
+	 * 	   a hack!!
+	 */
+	writel(readl(GPIO_BK0_DAT) | CONSOLE_GPIO_HACK, GPIO_BK0_DAT);
+
+	/* configure the prcm for a sleep/deep sleep wakeup */
+	prcmu_configure_wakeup_events(
+			(PRCMU_WAKEUPBY_MODEM |
+				PRCMU_WAKEUPBY_ARMITMGMT |
+				PRCMU_WAKEUPBY_APE4500INT |
+				PRCMU_WAKEUPBY_GPIOS |
+				PRCMU_WAKEUPBY_RTCRTT),
+			0x0, LOW_POWER_WAKEUP);
+
+	/*
+	 * enforce the IOCR; make all gpios input be default
+	 * we will have no console now onwards
+	 */
+	writel(ENABLE, PRCMU_IOCR);
+
+	/* save peripheral context */
+	ux500_save_peripheral_context();
 
 	switch (state) {
 	case PM_SUSPEND_STANDBY:
-		prcmu_apply_ap_state_transition(APEXECUTE_TO_APIDLE,
-				DDR_PWR_STATE_UNCHANGED, 0);
-		ret = 0;
+
+		/*
+		 * NOTE: due to a bug on the HW, the PRCM_A9_MASK_ACK
+		 *	 doesnt work. as a result, reading the register
+		 * 	 wont help and we skip it for now. enable the
+		 *	 mask for now.
+		 */
+		writel(ENABLE, PRCM_A9_MASK_REQ);
+
+		/* request the PRCM for the sleep */
+		prcmu_apply_ap_state_transition(APEXECUTE_TO_APSLEEP,
+			DDR_PWR_STATE_UNCHANGED, 0);
+
 		break;
 	case PM_SUSPEND_MEM:
-		prcmu_apply_ap_state_transition(APEXECUTE_TO_APSLEEP,
-				DDR_PWR_STATE_UNCHANGED, 0);
-		ret = 0;
+
+		/* 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_BACKUPRAM_ADDR));
+		writel(IO_ADDRESS(U8500_BACKUPRAM0_BASE),
+			IO_ADDRESS(U8500_CPU0_PUBLIC_BACKUP));
+		writel(IO_ADDRESS(U8500_BACKUPRAM0_BASE),
+			IO_ADDRESS(U8500_CPU1_PUBLIC_BACKUP));
+
+		/* core context to be saved */
+		ux500_save_core_context();
+
+		/* final context saving, and then into deep sleep */
+		ux500_cpu_context_deepsleep(cpu);
+
+		/* gone into deep sleep */
+
+		/* woken up; restore contexts */
+		ux500_restore_gic_context();
+
 		break;
 	default:
 		ret = -EINVAL;
 	}
 
+	ux500_restore_peripheral_context();
+
+	/* disable ioforce */
+	writel(DISABLE, PRCMU_IOCR);
+
+	/* debug print:  */
+	if (readb(PRCM_ACK_MB0_AP_PWRST_STATUS) == DEEPSLEEP_TO_EXECUTEOK)
+		printk(KERN_INFO
+			"ux500: Wakeup from ApDeepSleep successful\n");
+	else if (readb(PRCM_ACK_MB0_AP_PWRST_STATUS) == SLEEP_TO_EXECUTEOK)
+		printk(KERN_INFO
+			"ux500: Wakeup from ApSleep successful\n");
+	else
+		printk(KERN_INFO
+			"ux500: Wakeup Status=0x%x\n",
+				readb(PRCM_ACK_MB0_AP_PWRST_STATUS));
+
 	return ret;
 }
 
@@ -70,9 +772,63 @@ static struct platform_suspend_ops u8500_pm_ops = {
 	.valid = u8500_pm_valid,
 };
 
+#define GIC_SUPPORTED_INT_MASK	(0x1f)
+/* FIXME : remove this later when board files have such defines */
+#define MOP500_UART2RX_GPIO	(29)
+
 static int __init u8500_pm_init(void)
 {
+	uint32_t base;
+
+	/* find out the total interrupts on this platform */
+	base = IO_ADDRESS(U8500_GIC_DIST_BASE);
+
+	/* find how many interrupts are supported */
+	ux500_max_irq = readl(base + GIC_DIST_CTR) & GIC_SUPPORTED_INT_MASK;
+	ux500_max_irq = (ux500_max_irq + 1) * 32;
+
+	/* GIC supports maximum 1020 interrupts */
+	if (ux500_max_irq > max(1020, NR_IRQS))
+		ux500_max_irq = max(1020, NR_IRQS);
+
+	/* allocate backup pointers for CPU0/1 */
+	arm_cntxt_cpu1 = kzalloc(SZ_1K, GFP_KERNEL);
+	if (!arm_cntxt_cpu1) {
+		printk(KERN_WARNING
+			 "ux500-pm: backup ptr allocation failed\n");
+		return -ENOMEM;
+	}
+
+	arm_cntxt_cpu0 = kzalloc(SZ_1K, GFP_KERNEL);
+	if (!arm_cntxt_cpu0) {
+		printk(KERN_WARNING
+			"ux500-pm: backup ptr allocation failed\n");
+		return -ENOMEM;
+	}
+
+	/* allocate backup pointers for UART context */
+	uart_register_base = ioremap(U8500_UART2_BASE, SZ_4K);
+	if (!uart_register_base)
+		printk(KERN_WARNING "u8500-pm: uart register base NULL\n");
+
+	uart_backup_base = kzalloc(SZ_4K, GFP_KERNEL);
+	if (!uart_backup_base)
+		printk(KERN_WARNING "u8500-pm: uart register backup NULL\n");
+
+	/* make the UART2-Rx/GPIO29 as a wakeup event */
+	set_irq_type(GPIO_TO_IRQ(MOP500_UART2RX_GPIO), IRQ_TYPE_EDGE_BOTH);
+	set_irq_wake(GPIO_TO_IRQ(MOP500_UART2RX_GPIO), ENABLE);
+
+	/* allocate backup pointer for RAM data */
+	ux500_backup_ptr = (void *)__get_free_pages(GFP_KERNEL,
+					get_order(U8500_BACKUPRAM_SIZE));
+	if (!ux500_backup_ptr) {
+		printk(KERN_WARNING "ux500-pm: couldnt allocate backup ptr\n");
+		return -ENOMEM;
+	}
+
 	suspend_set_ops(&u8500_pm_ops);
+
 	return 0;
 }
 
diff --git a/arch/arm/mach-ux500/pm.h b/arch/arm/mach-ux500/pm.h
new file mode 100644
index 00000000000..72b4fcec795
--- /dev/null
+++ b/arch/arm/mach-ux500/pm.h
@@ -0,0 +1,208 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Sundar Iyer <sundar.iyer@stericsson.com>
+ *
+ * power management generic file
+ */
+
+#ifndef _PM_H_
+#define _PM_H_
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/suspend.h>
+#include <linux/errno.h>
+#include <linux/time.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/memory.h>
+
+#include <asm/system.h>
+
+#include <mach/scu.h>
+#include <mach/prcmu-regs.h>
+#include <mach/prcmu-fw-api.h>
+#include <mach/prcmu-fw-defs_v1.h>
+
+#include "prcmu-fw_v1.h"
+
+/* Peripheral Context Save/Restore */
+
+/*
+ * GPIO periph
+ */
+struct ux500_gpio_regs {
+	uint32_t gpio_dat;
+	uint32_t gpio_dat_set;
+	uint32_t gpio_dat_clr;
+	uint32_t gpio_pdis;
+	uint32_t gpio_dir;
+	uint32_t gpio_dir_set;
+	uint32_t gpio_dir_clr;
+	uint32_t gpio_slpm;
+	uint32_t gpio_altfunc_a;
+	uint32_t gpio_altfunc_b;
+	uint32_t gpio_rimsc;
+	uint32_t gpio_fsmsc;
+	uint32_t gpio_int_status;
+	uint32_t gpio_int_clr;
+	uint32_t gpio_rwmsc;
+	uint32_t gpio_fwmsc;
+	uint32_t gpio_wakeup_status;
+};
+
+#define UX500_NR_GPIO_BANKS	(8)
+#define GPIO_BANK_BASE(x) IO_ADDRESS(GPIO_BANK##x##_BASE)
+
+/*
+ * A9 sub system GIC context
+ */
+#define UX500_GIC_REG_SET	(100)
+
+static uint32_t gic_enable_set_reg[UX500_GIC_REG_SET];
+static uint32_t gic_dist_config[UX500_GIC_REG_SET];
+static uint32_t gic_dist_target[UX500_GIC_REG_SET];
+
+/*
+ * Periph clock cluster context
+ */
+#define PRCC_BCK_EN		0x00
+#define PRCC_KCK_EN		0x08
+#define PRCC_BCK_STATUS		0x10
+#define PRCC_KCK_STATUS		0x14
+
+#define UX500_NR_PRCC_BANKS	(5)
+#define PRCC_BANK_BASE(x) IO_ADDRESS(U8500_PER##x##_BASE)
+
+struct ux500_prcc_contxt {
+	uint32_t periph_bus_clk;
+	uint32_t periph_kern_clk;
+};
+
+/*
+ * ST-Interconnect context
+ */
+/* priority, bw limiter register offsets */
+#define NODE_HIBW1_ESRAM_IN_0_PRIORITY_REG       0x00
+#define NODE_HIBW1_ESRAM_IN_1_PRIORITY_REG       0x04
+#define NODE_HIBW1_ESRAM_IN_2_PRIORITY_REG       0x08
+#define NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT_REG    0x24
+#define NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT_REG    0x28
+#define NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT_REG    0x2C
+#define NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT_REG    0x30
+#define NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT_REG    0x34
+#define NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT_REG    0x38
+#define NODE_HIBW1_ESRAM_IN_2_ARB_1_LIMIT_REG    0x3C
+#define NODE_HIBW1_ESRAM_IN_2_ARB_2_LIMIT_REG    0x40
+#define NODE_HIBW1_ESRAM_IN_2_ARB_3_LIMIT_REG    0x44
+#define NODE_HIBW1_DDR_IN_0_PRIORITY_REG         0x400
+#define NODE_HIBW1_DDR_IN_1_PRIORITY_REG         0x404
+#define NODE_HIBW1_DDR_IN_2_PRIORITY_REG         0x408
+#define NODE_HIBW1_DDR_IN_0_LIMIT_REG            0x424
+#define NODE_HIBW1_DDR_IN_1_LIMIT_REG            0x428
+#define NODE_HIBW1_DDR_IN_2_LIMIT_REG            0x42C
+#define NODE_HIBW1_DDR_OUT_0_PRIORITY_REG        0x430
+#define NODE_HIBW2_ESRAM_IN_0_PRIORITY_REG       0x800
+#define NODE_HIBW2_ESRAM_IN_1_PRIORITY_REG       0x804
+#define NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT_REG    0x818
+#define NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT_REG    0x81C
+#define NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT_REG    0x820
+#define NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT_REG    0x824
+#define NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT_REG    0x828
+#define NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT_REG    0x82C
+#define NODE_HIBW2_DDR_IN_0_PRIORITY_REG         0xC00
+#define NODE_HIBW2_DDR_IN_1_PRIORITY_REG         0xC04
+#define NODE_HIBW2_DDR_IN_2_PRIORITY_REG         0xC08
+#define NODE_HIBW2_DDR_IN_3_PRIORITY_REG         0xC0C
+#define NODE_HIBW2_DDR_IN_0_LIMIT_REG            0xC30
+#define NODE_HIBW2_DDR_IN_1_LIMIT_REG            0xC34
+#define NODE_ESRAM1_2_IN_0_PRIORITY_REG          0x1400
+#define NODE_ESRAM1_2_IN_1_PRIORITY_REG          0x1404
+#define NODE_ESRAM1_2_IN_2_PRIORITY_REG          0x1408
+#define NODE_ESRAM1_2_IN_3_PRIORITY_REG          0x140C
+#define NODE_ESRAM1_2_IN_0_ARB_1_LIMIT_REG       0x1430
+#define NODE_ESRAM1_2_IN_0_ARB_2_LIMIT_REG       0x1434
+#define NODE_ESRAM1_2_IN_1_ARB_1_LIMIT_REG       0x1438
+#define NODE_ESRAM1_2_IN_1_ARB_2_LIMIT_REG       0x143C
+#define NODE_ESRAM1_2_IN_2_ARB_1_LIMIT_REG       0x1440
+#define NODE_ESRAM1_2_IN_2_ARB_2_LIMIT_REG       0x1444
+#define NODE_ESRAM1_2_IN_3_ARB_1_LIMIT_REG       0x1448
+#define NODE_ESRAM1_2_IN_3_ARB_2_LIMIT_REG       0x144C
+#define NODE_ESRAM3_4_IN_0_PRIORITY_REG          0x1800
+#define NODE_ESRAM3_4_IN_1_PRIORITY_REG          0x1804
+#define NODE_ESRAM3_4_IN_2_PRIORITY_REG          0x1808
+#define NODE_ESRAM3_4_IN_3_PRIORITY_REG          0x180C
+#define NODE_ESRAM3_4_IN_0_ARB_1_LIMIT_REG       0x1830
+#define NODE_ESRAM3_4_IN_0_ARB_2_LIMIT_REG       0x1834
+#define NODE_ESRAM3_4_IN_1_ARB_1_LIMIT_REG       0x1838
+#define NODE_ESRAM3_4_IN_1_ARB_2_LIMIT_REG       0x183C
+#define NODE_ESRAM3_4_IN_2_ARB_1_LIMIT_REG       0x1840
+#define NODE_ESRAM3_4_IN_2_ARB_2_LIMIT_REG       0x1844
+#define NODE_ESRAM3_4_IN_3_ARB_1_LIMIT_REG       0x1848
+#define NODE_ESRAM3_4_IN_3_ARB_2_LIMIT_REG       0x184C
+
+struct ux500_interconnect_contxt {
+	uint32_t ux500_hibw1_esram_in_pri_regs[3];
+	uint32_t ux500_hibw1_esram_in0_arb_regs[3];
+	uint32_t ux500_hibw1_esram_in1_arb_regs[3];
+	uint32_t ux500_hibw1_esram_in2_arb_regs[3];
+	uint32_t ux500_hibw1_ddr_in_prio_regs[3];
+	uint32_t ux500_hibw1_ddr_in_limit_regs[3];
+	uint32_t ux500_hibw1_ddr_out_prio_reg;
+
+	/* HiBw2 node registers */
+	uint32_t ux500_hibw2_esram_in_pri_regs[2];
+	uint32_t ux500_hibw2_esram_in0_arblimit_regs[3];
+	uint32_t ux500_hibw2_esram_in1_arblimit_regs[3];
+	uint32_t ux500_hibw2_ddr_in_prio_regs[4];
+	uint32_t ux500_hibw2_ddr_in_limit_regs[4];
+	uint32_t ux500_hibw2_ddr_out_prio_reg;
+
+	/* ESRAM node registers */
+	uint32_t ux500_esram_in_prio_regs[4];
+	uint32_t ux500_esram_in_lim_regs[4];
+	uint32_t ux500_esram12_in_prio_regs[4];
+	uint32_t ux500_esram12_in_arb_lim_regs[8];
+	uint32_t ux500_esram34_in_prio_regs[4];
+	uint32_t ux500_esram34_in_arb_lim_regs[8];
+};
+
+/*
+ * SCU context
+ */
+#define SCU_FILTER_STARTADDR	0x40
+#define SCU_FILTER_ENDADDR	0x44
+#define SCU_ACCESS_CTRL_SAC	0x50
+
+struct ux500_scu_context {
+	uint32_t scu_ctrl;
+	uint32_t scu_cpu_pwrstatus;
+	uint32_t scu_inv_all_nonsecure;
+	uint32_t scu_filter_start_addr;
+	uint32_t scu_filter_end_addr;
+	uint32_t scu_access_ctrl_sac;
+};
+
+/* helpers to save/restore contexts */
+void ux500_save_gic_context(void);
+void ux500_restore_gic_context(void);
+
+void ux500_save_uart_context(void);
+void ux500_restore_uart_context(void);
+
+void ux500_save_prcc_context(void);
+void ux500_restore_prcc_context(void);
+
+void ux500_save_icn_context(void);
+void ux500_restore_icn_context(void);
+
+void ux500_save_scu_context(void);
+void ux500_restore_scu_context(void);
+
+void ux500_cpu_context_deepsleep(uint8_t cpu);
+
+#endif /* _PM_H */
diff --git a/arch/arm/mach-ux500/prcmu-fw.c b/arch/arm/mach-ux500/prcmu-fw.c
index ef39dceb4f7..bdce3c9c6af 100755
--- a/arch/arm/mach-ux500/prcmu-fw.c
+++ b/arch/arm/mach-ux500/prcmu-fw.c
@@ -29,8 +29,6 @@
 	(printk(KERN_ALERT NAME ": " format , ## arg)) : \
 		({do {} while (0); })
 
-void __iomem *rtc_register_base ;
-
 struct hw_usage{
 	char b2r2:1;
 	char mcde:1;
@@ -747,222 +745,6 @@ int prcmu_set_irq_wakeup(uint32_t irq)
 }
 EXPORT_SYMBOL(prcmu_set_irq_wakeup);
 
-/* FIXME : Make these generic enough to pass backup
- *	   addresses for re-using the same functions
- *	   for sleep/deep sleep modes
- */
-/* Context Backups for ApSleep */
-unsigned int PRCC_backup[15];
-
-#define _PRCC_CLK_RST1_BASE 	IO_ADDRESS(U8500_PER1_BASE + 0xF000)
-#define _PRCC_CLK_RST2_BASE 	IO_ADDRESS(U8500_PER2_BASE + 0xF000)
-#define _PRCC_CLK_RST3_BASE 	IO_ADDRESS(U8500_PER3_BASE + 0xF000)
-#define _PRCC_CLK_RST5_BASE 	IO_ADDRESS(U8500_PER5_BASE + 0x1F000)
-#define _PRCC_CLK_RST6_BASE 	IO_ADDRESS(U8500_PER6_BASE + 0xF000)
-
-void pm_save_config_PRCC(void)
-{
-	uint8_t i = 0;
-
-	PRCC_backup[i++] = readl(_PRCC_CLK_RST1_BASE + 0x10);
-	PRCC_backup[i++] = readl(_PRCC_CLK_RST1_BASE + 0x14);
-	PRCC_backup[i++] = readl(_PRCC_CLK_RST2_BASE + 0x10);
-	PRCC_backup[i++] = readl(_PRCC_CLK_RST2_BASE + 0x14);
-	PRCC_backup[i++] = readl(_PRCC_CLK_RST3_BASE + 0x10);
-	PRCC_backup[i++] = readl(_PRCC_CLK_RST3_BASE + 0x14);
-	PRCC_backup[i++] = readl(_PRCC_CLK_RST5_BASE + 0x10);
-	PRCC_backup[i++] = readl(_PRCC_CLK_RST5_BASE + 0x14);
-	PRCC_backup[i++] = readl(_PRCC_CLK_RST6_BASE + 0x10);
-	PRCC_backup[i++] = readl(_PRCC_CLK_RST6_BASE + 0x14);
-}
-
-void pm_restore_config_PRCC(void)
-{
-	uint8_t i = 0;
-
-	writel(PRCC_backup[i++], _PRCC_CLK_RST1_BASE + 0x0);
-	writel(PRCC_backup[i++], _PRCC_CLK_RST1_BASE + 0x8);
-	writel(PRCC_backup[i++], _PRCC_CLK_RST2_BASE + 0x0);
-	writel(PRCC_backup[i++], _PRCC_CLK_RST2_BASE + 0x8);
-	writel(PRCC_backup[i++], _PRCC_CLK_RST3_BASE + 0x0);
-	writel(PRCC_backup[i++], _PRCC_CLK_RST3_BASE + 0x8);
-	writel(PRCC_backup[i++], _PRCC_CLK_RST5_BASE + 0x0);
-	writel(PRCC_backup[i++], _PRCC_CLK_RST5_BASE + 0x8);
-	writel(PRCC_backup[i++], _PRCC_CLK_RST6_BASE + 0x0);
-	writel(PRCC_backup[i++], _PRCC_CLK_RST6_BASE + 0x8);
-}
-
-void __iomem *uart_backup_base, *uart_register_base;
-
-/* TODO : return success value to let decide to proceed
- * 	  for low power request or not
- */
-void pm_save_config_UART(void)
-{
-	uart_register_base = ioremap(U8500_UART2_BASE, SZ_4K);
-	if (!uart_register_base) {
-		printk(KERN_WARNING
-		 "u8500-prcm : cannot allocate uart register base\n");
-	}
-
-	uart_backup_base = kzalloc(SZ_4K, GFP_KERNEL);
-	if (!uart_backup_base) {
-		printk(KERN_WARNING
-		 "u8500-prcm : cannot allocate uart register backup base\n");
-	}
-
-	/* Copy UART config */
-	memcpy(uart_backup_base, uart_register_base, SZ_4K);
-}
-
-void __iomem *ab8500_backup_base, *ab8500_register_base;
-
-void pm_restore_config_UART(void)
-{
-	memcpy(uart_register_base, uart_backup_base, SZ_4K);
-}
-
-/* Deep Sleep context save */
-
-/* public context backup */
-/* TODO : remove hard coded backup RAM addresses */
-void a9ss_save_public_config(void)
-{
-	int val;
-
-	/* this happens for CPU0 only as of now */
-	/* CP15 SCTLR */
-	__asm__ __volatile__(
-			"mrc p15, 0, %0, c1, c1, 2"
-			: "=r" (val)
-			:
-			: "cc");
-	writel(val, IO_ADDRESS(0x80151F80));
-
-	/* CP15 TTBR0 */
-	__asm__ __volatile__(
-			"mrc p15, 0, %0, c2, c0, 0"
-			: "=r" (val)
-			:
-			: "cc");
-	writel(val, IO_ADDRESS(0x80151F84));
-
-	/* CP15 TTBR1 */
-	__asm__ __volatile__(
-			"mrc p15, 0, %0, c2, c0, 1"
-			: "=r" (val)
-			:
-			: "cc");
-	writel(val, IO_ADDRESS(0x80151F88));
-
-	/* CP15 TTBCR */
-	__asm__ __volatile__(
-			"mrc p15, 0, %0, c2, c0, 2"
-			: "=r" (val)
-			:
-			: "cc");
-	writel(val, IO_ADDRESS(0x80151F8C));
-
-	/* CP15 DACR */
-	__asm__ __volatile__(
-			"mrc p15, 0, %0, c3, c0, 0"
-			: "=r" (val)
-			:
-			: "cc");
-	writel(val, IO_ADDRESS(0x80151F90));
-
-	/* CPSR */
-	__asm__ __volatile__(
-			"mrs %0, cpsr"
-			: "=r" (val)
-			:
-			: "cc");
-	writel(val, IO_ADDRESS(0x80151F98));
-
-	return;
-}
-
-void a9ss_restore_public_config(void)
-{
-	int val;
-
-	/* CPSR */
-	val = readl(IO_ADDRESS(0x80151F98));
-	__asm__ __volatile__(
-			"msr cpsr, %0"
-			:
-			: "r" (val));
-
-	/* CP15 DACR */
-	val = readl(IO_ADDRESS(0x80151F90));
-	__asm__ __volatile__(
-			"mcr p15, 0, %0, c3, c0, 0"
-			:
-			: "r" (val));
-
-	/* CP15 TTBCR */
-	val = readl(IO_ADDRESS(0x80151F8C));
-	__asm__ __volatile__(
-			"mcr p15, 0, %0, c2, c0, 2"
-			:
-			: "r" (val));
-
-	/* CP15 TTBR1 */
-	val = readl(IO_ADDRESS(0x80151F88));
-	__asm__ __volatile__(
-			"mcr p15, 0, %0, c2, c0, 1"
-			:
-			: "r" (val));
-
-	/* CP15 TTBR0 */
-	val = readl(IO_ADDRESS(0x80151F84));
-	__asm__ __volatile__(
-			"mcr p15, 0, %0, c2, c0, 0"
-			:
-			: "r" (val));
-
-	/* CP15 SCTLR */
-	val = readl(IO_ADDRESS(0x80151F80));
-	__asm__ __volatile__(
-			"mrc p15, 0, %0, c1, c1, 2"
-			:
-			: "r" (val));
-
-	return;
-}
-
-/* ARM SCU backup */
-#include <mach/scu.h>
-unsigned int scu_backup_config[5];
-
-void a9ss_save_scu_config(void)
-{
-	scu_backup_config[0] = __raw_readl(IO_ADDRESS(U8500_SCU_BASE)
-							+ SCU_CTRL);
-	scu_backup_config[1] = __raw_readl(IO_ADDRESS(U8500_SCU_BASE)
-							+ SCU_CONFIG);
-	scu_backup_config[2] = __raw_readl(IO_ADDRESS(U8500_SCU_BASE)
-							+ SCU_CPU_STATUS);
-	scu_backup_config[3] = __raw_readl(IO_ADDRESS(U8500_SCU_BASE)
-							+ SCU_INVALIDATE);
-
-	return;
-}
-
-void a9ss_restore_scu_config(void)
-{
-	__raw_writel(scu_backup_config[0], IO_ADDRESS(U8500_SCU_BASE)
-							+ SCU_CTRL);
-	__raw_writel(scu_backup_config[1], IO_ADDRESS(U8500_SCU_BASE)
-							+ SCU_CONFIG);
-	__raw_writel(scu_backup_config[2], IO_ADDRESS(U8500_SCU_BASE)
-							+ SCU_CPU_STATUS);
-	__raw_writel(scu_backup_config[3], IO_ADDRESS(U8500_SCU_BASE)
-							+ SCU_INVALIDATE);
-
-	return;
-}
-
 /* FIXME : get these from platform/header files instead */
 /* GIC BAse Address */
 #define GIC_BASE_ADDR           IO_ADDRESS(0xA0411000)
@@ -975,9 +757,6 @@ void a9ss_restore_scu_config(void)
 #define DIST_ENABLE_CLEAR       (GIC_BASE_ADDR + 0x180)
 #define DIST_ACTIVE_BIT         (GIC_BASE_ADDR + 0x304)
 
-#define PRCM_DEBUG_NOPWRDOWN_VAL        IO_ADDRESS(0x80157194)
-#define PRCM_POWER_STATE_VAL            IO_ADDRESS(0x8015725C)
-
 /**
  * prcmu_apply_ap_state_transition - PRCMU State Transition function
  * @transition: Transition to be requested to move to new AP power mode
@@ -997,6 +776,7 @@ void a9ss_restore_scu_config(void)
  * not support it. This also assumes that the non-boot cpu's are in wfi
  * and not wfe.
  */
+
 int prcmu_apply_ap_state_transition(enum ap_pwrst_trans_t transition,
 					enum ddr_pwrst_t ddr_state_req,
 					int _4500_fifo_wakeup)
@@ -1038,118 +818,6 @@ int prcmu_apply_ap_state_transition(enum ap_pwrst_trans_t transition,
 	}
 
 	switch (transition) {
-	case APEXECUTE_TO_APSLEEP:
-
-		/* PROGRAM THE ITMASK* registers for a Sleep */
-		writel(0x00000000, PRCM_ARMITMSK31TO0);
-		writel(0x00000100, PRCM_ARMITMSK63TO32);
-		writel(0x00000000, PRCM_ARMITMSK95TO64);
-		writel(0x00000000, PRCM_ARMITMSK127TO96);
-
-		/* probe the GIC if its already frozen */
-		tmp = readl(PRCM_A9_MASK_ACK);
-		if (tmp & 0x00000001)
-			printk(KERN_WARNING "GIC is already frozen\n");
-
-		/* write to GIC freeze */
-		tmp = readl(PRCM_A9_MASK_REQ);
-		tmp |= 0x01;
-		writel(tmp, PRCM_A9_MASK_REQ);
-
-		/* we wait for a random timeout. as of now the
-		 * the GIC freeze isnt ACKed; so wait
-		 */
-		timeout = 100;
-		while (timeout--)
-			cpu_relax();
-
-		prcmu_configure_wakeup_events(0xFFFFFFFF, 0x0,
-				LOW_POWER_WAKEUP);
-
-		spin_lock(&req_mb0_lock);
-
-		/* CREATE MAILBOX FOR EXECUTE TO IDLE POWER TRANSITION */
-		/* Write PwrStTrH=0 header to request a Power state xsition */
-		writeb(0x0, PRCM_MBOX_HEADER_REQ_MB0);
-
-		/* write request to the MBOX0 */
-		writeb(APEXECUTE_TO_APSLEEP, PRCM_REQ_MB0_PWRSTTRH_APPWRST);
-		writeb(OFF, PRCM_REQ_MB0_PWRSTTRH_APPLLST);
-		writeb(ON, PRCM_REQ_MB0_PWRSTTRH_ULPCLKST);
-		writeb(0x56, PRCM_REQ_MB0_PWRSTTRH_BYTEFILL);
-
-		/* As per the sync logic, we are supposed to be the final CPU.
-		 * If the other CPU isnt in wfi, better exit by putting
-		 * ourselves in wfi
-		 */
-		if (smp_processor_id()) {
-			if (!(readl(PRCM_ARM_WFI_STANDBY) & 0x8)) {
-				spin_unlock(&req_mb0_lock);
-				__asm__ __volatile__(
-					"dsb\n\t" "wfi\n\t" : : : "memory");
-				return 0;
-			}
-		} else /* running on CPU0, check for CPU1 WFI standby */ {
-			if (!(readl(PRCM_ARM_WFI_STANDBY) & 0x10)) {
-				spin_unlock(&req_mb0_lock);
-				__asm__ __volatile__(
-					"dsb\n\t" "wfi\n\t" : : : "memory");
-				return 0;
-			}
-		}
-
-		/* Enable RTC-INT in the GIC */
-		if (!(readl(PRCM_ARMITMSK31TO0) & 0x40000))
-			writel((readl(PRCM_ARMITMSK31TO0) | 0x40000),
-						PRCM_ARMITMSK31TO0);
-
-
-		/* FIXME : later on, the ARM should not request a Idle if one
-		 *         of the ITSTATUS0/5 are still alive!!!
-		 */
-		if (readl(PRCM_ITSTATUS0) == 0x80) {
-			printk(KERN_WARNING "PRCM_ITSTATUS0 Not cleared\n");
-			spin_unlock(&req_mb0_lock);
-			__asm__ __volatile__(
-				"dsb\n\t" "wfi\n\t" : : : "memory");
-			return -EBUSY;
-		}
-
-		/*
-		 *
-		 * REQUEST POWER XSITION
-		 *
-		 */
-
-		/* clear ACK MB0 */
-		writeb(0x0, PRCM_ACK_MB0);
-
-		_wait_for_req_complete(REQ_MB0);
-
-		spin_unlock(&req_mb0_lock);
-
-		printk(KERN_INFO "u8500-prcm : To ApSleep\n");
-
-		/* Context Saving beings */
-
-		pm_save_config_PRCC();
-
-		pm_save_config_UART();
-
-		/* here comes the wfi */
-		__asm__ __volatile__(
-				"dsb\n\t" "wfi\n\t" : : : "memory");
-
-		/* Restore PRCC Configs */
-		pm_restore_config_PRCC();
-
-		/* Restore UART Configs */
-		pm_restore_config_UART();
-
-		if (readb(PRCM_ACK_MB0_AP_PWRST_STATUS) == 0xF3)
-			printk(KERN_INFO "u8500-prcm : Woken from Sleep OK\n");
-
-		break;
 	case APEXECUTE_TO_APIDLE:
 		/* Copy the current GIC set enable config as wakeup */
 		for (val = 0; val < 4; val++) {
@@ -1159,20 +827,6 @@ int prcmu_apply_ap_state_transition(enum ap_pwrst_trans_t transition,
 
 		prcmu_configure_wakeup_events(((1 << 17) | (1 << 5)),
 						0x0, EXE_WAKEUP);
-#if 0
-		/* SIGNAL MAILBOX */
-		/* set the MBOX_CPU_SET bit to set an IT to xP70 */
-		writel(1 << 0, PRCM_MBOX_CPU_SET);
-
-		/* wait for corresponding MB0X_CPU_VAL bit to be cleared */
-		while ((readl(PRCM_MBOX_CPU_VAL) & (1 << 0)) && timeout--)
-			cpu_relax();
-		if (!timeout) {
-			printk(KERN_INFO
-			 "Timeout in prcmu_configure_wakeup_events!!\n");
-			return -EBUSY;
-		}
-#endif
 		spin_lock(&req_mb0_lock);
 
 		/* CREATE MAILBOX FOR EXECUTE TO IDLE POWER TRANSITION */
@@ -1247,111 +901,27 @@ int prcmu_apply_ap_state_transition(enum ap_pwrst_trans_t transition,
 		prcmu_configure_wakeup_events((1 << 5), 0x0, EXE_WAKEUP);
 
 		break;
-	case APEXECUTE_TO_APDEEPSLEEP:
-		/* PROGRAM THE ITMASK* registers for deep sleep  */
-		writel(0x00000000, PRCM_ARMITMSK31TO0);
-		writel(0x00000100, PRCM_ARMITMSK63TO32);
-		writel(0x00000000, PRCM_ARMITMSK95TO64);
-		writel(0x00000000, PRCM_ARMITMSK127TO96);
-
-		/* we skip the GIC freeze due to the FIQ being
-		 * not handled by the ARM later on
-		 */
-		prcmu_configure_wakeup_events(((1 << 17) | (1 << 5)),
-						0x0, LOW_POWER_WAKEUP);
 
-		spin_lock(&req_mb0_lock);
-
-		/* CREATE MAILBOX FOR EXECUTE TO IDLE POWER TRANSITION */
+	case APEXECUTE_TO_APSLEEP:
+	case APEXECUTE_TO_APDEEPSLEEP:
 		/* Write PwrStTrH=0 header to request a Power state xsition */
 		writeb(0x0, PRCM_MBOX_HEADER_REQ_MB0);
 
 		/* write request to the MBOX0 */
-		writeb(APEXECUTE_TO_APDEEPSLEEP, PRCM_REQ_MB0_PWRSTTRH_APPWRST);
-		writeb(OFF, PRCM_REQ_MB0_PWRSTTRH_APPLLST);
-		writeb(ON, PRCM_REQ_MB0_PWRSTTRH_ULPCLKST);
-		writeb(0x56, PRCM_REQ_MB0_PWRSTTRH_BYTEFILL);
-
-		/* As per the sync logic, we are supposed to be the final CPU.
-		 * If the other CPU isnt in wfi, better exit by putting
-		 * ourselves in wfi
-		 */
-		if (smp_processor_id()) {
-			if (!(readl(PRCM_ARM_WFI_STANDBY) & 0x8)) {
-				spin_unlock(&req_mb0_lock);
-				__asm__ __volatile__(
-					"dsb\n\t" "wfi\n\t" : : : "memory");
-				return 0;
-			}
-		} else /* running on CPU0, check for CPU1 WFI standby */ {
-			if (!(readl(PRCM_ARM_WFI_STANDBY) & 0x10)) {
-				spin_unlock(&req_mb0_lock);
-				__asm__ __volatile__(
-					"dsb\n\t" "wfi\n\t" : : : "memory");
-				return 0;
-			}
-		}
-
-		/* Enable RTC-INT in the GIC */
-		if (!(readl(PRCM_ARMITMSK31TO0) & 0x40000))
-			writel((readl(PRCM_ARMITMSK31TO0) | 0x40000),
-						PRCM_ARMITMSK31TO0);
-
-
-		/* FIXME : later on, the ARM should not request a Idle if one
-		 *         of the ITSTATUS0/5 are still alive!!!
-		 */
-		if (readl(PRCM_ITSTATUS0) == 0x80) {
-			printk(KERN_WARNING "PRCM_ITSTATUS0 Not cleared\n");
-			spin_unlock(&req_mb0_lock);
-			__asm__ __volatile__(
-				"dsb\n\t" "wfi\n\t" : : : "memory");
-			return -EBUSY;
-		}
-
-		/*
-		 *
-		 * REQUEST POWER XSITION
-		 *
-		 */
-
-		/* clear ACK MB0 */
-		writeb(0x0, PRCM_ACK_MB0);
-
-		_wait_for_req_complete(REQ_MB0);
-		spin_unlock(&req_mb0_lock);
-
-		printk(KERN_INFO "u8500-prcm : To Sleep\n");
+		writeb(transition, PRCM_REQ_MB0_PWRSTTRH_APPWRST);
+		writeb(0x1, PRCM_REQ_MB0_PWRSTTRH_APPLLST);
+		writeb(0x1, PRCM_REQ_MB0_PWRSTTRH_ULPCLKST);
 
-		/* Restore PRCC configs */
-		pm_save_config_PRCC();
-
-		pm_save_config_UART();
-
-		a9ss_save_public_config();
-
-		/* here comes the wfi */
-		__asm__ __volatile__(
-			"dsb\n\t" "wfi\n\t" : : : "memory");
-
-		/* Restore PRCC Configs */
-		pm_restore_config_PRCC();
-
-		/* Restore UART Configs */
-		pm_restore_config_UART();
-
-		if (readb(PRCM_ACK_MB0_AP_PWRST_STATUS) == 0xF3)
-			printk(KERN_INFO "u8500-prcm : Woken from Sleep OK\n");
+		/* Execute PRCMU command */
+		writel(1, PRCM_MBOX_CPU_SET);
 
-		break;
-	case APBOOT_TO_APEXECUTE:
+		__asm__ __volatile__("dsb\n\t" "wfi\n\t" : : : "memory");
 		break;
 	default:
 		ret = -EINVAL;
 	}
 	return ret;
 
-
 }
 EXPORT_SYMBOL(prcmu_apply_ap_state_transition);
 
diff --git a/arch/arm/mach-ux500/prcmu-fw_v1.h b/arch/arm/mach-ux500/prcmu-fw_v1.h
index c25846305ff..05d203298cc 100755
--- a/arch/arm/mach-ux500/prcmu-fw_v1.h
+++ b/arch/arm/mach-ux500/prcmu-fw_v1.h
@@ -12,6 +12,30 @@
 #include <mach/prcmu-fw-api.h>
 #include <mach/prcmu-fw-defs_v1.h>
 
+/* Define for GPIO configuration */
+#define GPIO_BANK0_BASE         (U8500_PER1_BASE + 0xE000)
+#define GPIO_BANK1_BASE         (U8500_PER1_BASE + 0xE000 + 0x80)
+#define GPIO_BANK2_BASE         (U8500_PER3_BASE + 0xE000)
+#define GPIO_BANK3_BASE         (U8500_PER3_BASE + 0xE000 + 0x80)
+#define GPIO_BANK4_BASE         (U8500_PER3_BASE + 0xE000 + 0x100)
+#define GPIO_BANK5_BASE         (U8500_PER3_BASE + 0xE000 + 0x180)
+#define GPIO_BANK6_BASE         (U8500_PER2_BASE + 0xE000)
+#define GPIO_BANK7_BASE         (U8500_PER2_BASE + 0xE000 + 0x80)
+#define GPIO_BANK8_BASE         (U8500_PER5_BASE + 0x1E000)
+
+
+#define _GPIO_BK0_BASE IO_ADDRESS(GPIO_BANK0_BASE)
+
+#define GPIO_BK0_DAT (_GPIO_BK0_BASE)
+#define GPIO_BK0_DATS (_GPIO_BK0_BASE + 0x4)
+#define GPIO_BK0_DATC (_GPIO_BK0_BASE + 0x8)
+#define GPIO_BK0_DIR (_GPIO_BK0_BASE + 0x10)
+#define GPIO_BK0_FSLA (_GPIO_BK0_BASE + 0x20)
+#define GPIO_BK0_FSLB (_GPIO_BK0_BASE + 0x24)
+#define GPIO_BK0_RWMSC (_GPIO_BK0_BASE + 0x50)
+#define GPIO_BK0_FWMSC (_GPIO_BK0_BASE + 0x54)
+
+
 #define _PRCMU_TCDM_BASE     IO_ADDRESS(U8500_PRCMU_TCDM_BASE)
 #define PRCM_BOOT_STATUS    (_PRCMU_TCDM_BASE + 0xFFF)
 #define PRCM_ROMCODE_A2P    (_PRCMU_TCDM_BASE + 0xFFE)
@@ -144,9 +168,14 @@ enum mailbox_t {
 	REQ_MB5 = 5,	/* Uses XP70_IT_EVENT_17 */
 };
 
-/* Union declaration */
-
+/* PRCMU Wakeup defines */
+#define	PRCMU_WAKEUPBY_MODEM		(0x1 << 5)
+#define PRCMU_WAKEUPBY_ARMITMGMT	(0x1 << 17)
+#define PRCMU_WAKEUPBY_APE4500INT	(0x1 << 7)
+#define PRCMU_WAKEUPBY_GPIOS		(0xff800000)
+#define PRCMU_WAKEUPBY_RTCRTT		(0x3)
 
+/* Union declaration */
 
 /* ARM to XP70 mailbox definition */
 union req_mb0_t {
diff --git a/arch/arm/mach-ux500/savecontext.S b/arch/arm/mach-ux500/savecontext.S
new file mode 100644
index 00000000000..bcb679017f2
--- /dev/null
+++ b/arch/arm/mach-ux500/savecontext.S
@@ -0,0 +1,502 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Sundar Iyer <sundar.iyer@stericsson.com>
+ * Author: David Paris <david.paris-nonst@stericsson.com>
+ *
+ * ux500 core context/save for low power modes
+ */
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+#include <mach/hardware.h>
+#include <asm/hardware/cache-l2x0.h>
+
+
+/* Define for DeepSleep specific case */
+#define _BACKUPRAM1_BASE 		IO_ADDRESS(U8500_BACKUPRAM1_BASE)
+
+#define BACKUPRAMCPU0	(_BACKUPRAM1_BASE + 0xF80)
+#define BACKUPRAMCPU1	(_BACKUPRAM1_BASE + 0xFA0)
+
+#define UX500L2CCBASE	IO_ADDRESS(UX500_L2CC_BASE)
+
+ENTRY(ux500_cpu_context_deepsleep)
+	.code 32
+	.extern prcmu_apply_ap_state_transition
+
+	push {r0-r4, lr}
+
+	mov r2, r0
+	cmp r0, #0
+	bne load_cpu1_ctxt
+
+	ldr r0, =BACKUPRAMCPU0
+	b common_ctxt
+
+load_cpu1_ctxt:
+	ldr r0, =BACKUPRAMCPU1
+
+common_ctxt:
+	/* Save SystemControl Register */
+	mrc p15, 0, r1, c1, c0, 0
+	str r1,[r0,#0x00]
+
+	/* Save TTBR0 */
+	mrc p15, 0, r1, c2, c0, 0
+	str r1,[r0,#0x04]
+
+	/* Save TTBR1 */
+	mrc p15, 0, r1, c2, c0, 1
+	str	r1,[r0,#0x08]
+
+	/* save TTBRC */
+	mrc p15, 0, r1, c2, c0, 2
+	str r1,[r0,#0x0C]
+
+	/* Save DACR */
+	mrc p15, 0, r1, c3, c0, 0
+	str r1,[r0,#0x10]
+
+	cmp r2, #0
+	bne store_ret_addr_for_cpu1
+
+	ldr r1, =_return_from_deepsleep_cpu0
+	str r1,[r0,#0x14]
+	b common_continue
+
+store_ret_addr_for_cpu1:
+	ldr r1, =_return_from_deepsleep_cpu1
+	str r1,[r0,#0x14]
+
+common_continue:
+	/* save CPSR */
+	mrs r1, cpsr
+	str r1,[r0,#0x18]
+
+	/* save Current stack (Must not be used after) */
+	str sp,[r0,#0x1C]
+
+	/* Save ARM Context */
+	mov r0, r2
+	bl save_processor_context
+
+	cmp r2, #0
+	bne _cpu1_wait_deepsleep
+
+	stmfd sp!, {r0, r1, r2}
+	bl ux500_flush_all
+	ldmfd sp!, {r0, r1, r2}
+
+	mov r0, #0x4	/* APEXECUTE_TO_APDEEPSLEEP */
+	mov r1, #0x0	/* DDR_PWR_STATE_UNCHANGED */
+	mov r2, #0x0	/* INTR_NOT_AS_WAKEUP */
+
+	bl prcmu_apply_ap_state_transition
+
+	b _return_from_deepsleep_cpu0
+
+_cpu1_wait_deepsleep:
+	dsb
+	wfi
+	b _return_from_deepsleep_cpu1
+
+_return_from_deepsleep_cpu0:
+	mov r0, #0
+
+	b _exit_deeps
+
+_return_from_deepsleep_cpu1:
+	mov r0, #1
+
+_exit_deeps:
+	bl restore_processor_context
+
+	pop {r0-r4, pc}
+ENDPROC(ux500_cpu_context_deepsleep)
+
+ENTRY(save_processor_context)
+	.extern arm_cntxt_cpu0
+	.extern arm_cntxt_cpu1
+
+	.code 32
+
+	push {r0-r3, lr}
+
+	/* load the backup pointer accordingly */
+	cmp r0, #0
+	bne save_cpu1_ctxt
+	ldr r1, =arm_cntxt_cpu0
+	b save_ctxt
+
+save_cpu1_ctxt:
+	ldr r1, =arm_cntxt_cpu1
+
+save_ctxt:
+	ldr r1, [r1]
+
+	mrs r2, cpsr
+	str r2, [r1], #+4
+
+	/* clear up all mode bits which change with the mode. */
+	bic r2, r2, #0xf
+
+	/*
+	 * enter all modes and save banked registers
+	 * suffix to CPSR : c-control, f-flags, s-status, x-xtension
+	 */
+
+	/*
+	 * Enter FIQ mode, Interrupts disabled
+	 * Save: r8-r14 and spsr
+	 * Assume: r2 depicts the processor in Supervisor mode
+	 */
+	orr r3, r2, #0x1
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3,r8-r14}
+
+	/*
+	 * Enter IRQ mode, Interrupts disabled
+	 * Save: r13,r14 and spsr
+	 */
+	orr r3, r2, #0x2
+	msr cpsr_cxsf, r3 /* Enter IRQ mode with IRQ/FIQ disable */
+	mrs r3, spsr
+	stmia r1!, {r3,r13,r14}
+
+	/*
+	 * Enter Abort mode, irq/fiq disabled
+	 * Save: r13,r14 and sps
+	 */
+	orr r3, r2, #0x7
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3, r4-r14}
+
+	/*
+	 * Enter Undef mode, irq/fiq disable
+	 * Save: r13,r14 and spsr
+	 */
+	orr r3, r2, #0xB
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3,r13,r14}
+
+	/* go back to the SVC mode now */
+	orr r3, r2, #0x3
+	msr cpsr_cxsf, r3
+
+	/* in SVC mode, save all CP15 configs */
+
+	/* Non-secure Vector Base Address Register */
+	mrc p15, 0, r2, c12, c0, 0
+	str r2, [r1], #4
+
+	/* Primary Region Remap reg */
+	mrc p15, 0, r2, c10, c2, 0
+	str r2, [r1], #4
+
+	mrc p15, 0, r2, c10, c2, 1
+	str r2, [r1], #4
+
+	/* Context ID reg */
+	mrc p15, 0, r2, c13, c0, 1
+	str r2, [r1], #4
+
+	/* Thread ID  registers */
+	mrc p15, 0, r2, c13, c0, 2
+	str r2, [r1], #4
+
+	mrc p15, 0, r2, c13, c0, 3
+	str r2, [r1], #4
+
+	mrc p15, 0, r2, c13, c0, 4
+	str r2, [r1], #4
+
+	/* Cache Size Selection Register */
+	mrc p15, 2, r2, c0, c0,  0
+	str r2, [r1], #4
+
+	/* PMNC */
+	mrc p15, 0, r2, c9, c12, 0
+	str r2, [r1], #4
+
+	/* PMCNTENSET register */
+	mrc p15, 0, r2, c9, c12, 1
+	str r2, [r1], #4
+
+	/* PMSELR register */
+	mrc p15, 0, r2, c9, c12, 5
+	str r2, [r1], #4
+
+	/* PMCCNTR register */
+	mrc p15, 0, r2, c9, c13, 0
+	str r2, [r1], #4
+
+	/* PMXEVTYPER register */
+	mrc p15, 0, r2, c9, c13, 1
+	str r2, [r1], #4
+
+	/* PMUSERENR register */
+	mrc p15, 0, r2, c9, c14, 0
+	str r2, [r1], #4
+
+	/* PMINTENSET register */
+	mrc p15, 0, r2, c9, c14, 1
+	str r2, [r1], #4
+
+	/* PMINTENCLR register */
+	mrc p15, 0, r2, c9, c14, 2
+	str r2, [r1], #4
+
+	/* CPACR register */
+	mrc p15, 0, r2, c1, c0, 2
+	str r2, [r1], #4
+
+	cmp r0, #0
+	bne backup_ptr_cpu1
+
+	ldr r0, =arm_cntxt_cpu0
+	b update_backup_ptr
+
+backup_ptr_cpu1:
+	ldr r0, =arm_cntxt_cpu1
+
+update_backup_ptr:
+	str r1, [r0]
+
+	pop {r0-r3,pc}
+ENDPROC(save_processor_context)
+
+
+ENTRY (restore_processor_context)
+	.code 32
+	.extern arm_cntxt_cpu0
+	.extern arm_cntxt_cpu1
+
+	push {r0-r3, lr}
+
+	cmp r0, #0
+	bne restore_cpu1_cntxt
+	ldr r1, =arm_cntxt_cpu0
+	b restore_cntxt
+
+restore_cpu1_cntxt:
+	ldr r1, =arm_cntxt_cpu1
+
+restore_cntxt:
+	ldr r1, [r1]
+
+	/* restore the CP15 configs */
+	sub r1, r1, #4
+
+	/* CPACR register */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c1, c0, 2
+
+	/* PMINTENCLR register */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c9, c14, 2
+
+	/* PMINTENSET register */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c9, c14, 1
+
+	/* PMUSERENR register */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c9, c14, 0
+
+	/* PMXEVTYPER register */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c9, c13, 1
+
+	/* PMCCNTR register */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c9, c13, 0
+
+	/* PMSELR register */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c9, c12, 5
+
+	/* PMCNTENSET register */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c9, c12, 1
+
+	/* PMNC register */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c9, c12, 0
+
+	/* Cache Size Selection register */
+	ldr r2, [r1], #-4
+	mcr p15, 2, r2, c0, c0,  0
+
+	/* Thread IDs registers */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c13, c0, 4
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c13, c0, 3
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c13, c0, 2
+
+	/* Context ID register */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c13, c0, 1
+
+	/* memory region map registers */
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c10, c2, 1
+	ldr r2, [r1], #-4
+	mcr p15, 0, r2, c10, c2, 0
+
+	/* Non-secure Vector Base Address register */
+	ldr r2, [r1]
+	mcr p15, 0, r2, c12, c0, 0
+
+	/* backup the value of cpsr */
+	mrs r2, cpsr
+	orr r2, r2, #0xC0
+	bic r2, r2, #0xf
+
+        /*
+         * Enter Undef mode, irq/fiq disabled
+         * restore: r13,r14 and spsr
+         */
+	orr r3, r2, #0xB
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3,r13,r14}
+	msr spsr_cxsf, r3
+
+        /*
+         * Enter Abort mode, irq/fiq disabled
+         * restore: r13,r14 and spsr
+         */
+	orr r3, r2, #0x7
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3, r4-r14}
+	msr spsr_cxsf, r3
+
+        /*
+         * Enter IRQ mode, irq disabled
+         * restore: r13,r14 and spsr
+         */
+	orr r3, r2, #0x2
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3,r13,r14}
+	msr spsr_cxsf, r3
+
+        /*
+         * Enter FIQ mode, irq disabled
+         * restore: r13,r14 and spsr
+         */
+	orr r3, r2, #0x1
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3,r8-r14}
+	msr spsr_cxsf, r3
+
+	/*
+         * Enter SVC mode, irq disabled
+         * restore: cpsr
+         */
+	orr r3, r2, #0x3
+	msr cpsr_cxsf, r3
+
+	ldr r2, [r1, #-4]!
+	msr cpsr, r2
+	msr cpsr_cxsf, r3
+
+	cmp r0, #0
+	bne backup_ptr_restore_cpu1
+	ldr r0, =arm_cntxt_cpu0
+	b update_backup_restore_ptr
+
+backup_ptr_restore_cpu1:
+	ldr r0, =arm_cntxt_cpu1
+
+update_backup_restore_ptr:
+	sub r1, r1, #4
+	str r1, [r0]
+
+	pop {r0-r3, pc}
+ENDPROC(restore_processor_context)
+
+ENTRY(ux500_flush_all)
+	.code 32
+
+	/*
+	 *  Flush the entire cache system.
+	 *  The data cache flush is now achieved using atomic clean/invalidates
+	 *  working outwards from L1 cache. This is done using Set/Way based
+	 *  cache maintainance instructions.
+	 *  The instruction cache can still be invalidated back to the point of
+	 *  unification in a single instruction.
+	 *
+	 */
+	stmfd   sp!, {r4-r5, r7, r9-r11, lr}
+	mrc     p15, 1, r0, c0, c0, 1           @ read clidr
+        ands    r3, r0, #0x7000000              @ extract loc from clidr
+        mov     r3, r3, lsr #23                 @ left align loc bit field
+        beq     finished                        @ if loc is 0, no need to clean
+        mov     r10, #0                         @ start clean at cache level 0
+loop1:
+        add     r2, r10, r10, lsr #1            @ work 3x current cache level
+        mov     r1, r0, lsr r2                  @ extract cache type from clidr
+        and     r1, r1, #7                      @ mask bits for current cache
+        cmp     r1, #2                          @ see cache at this level
+        blt     skip                            @ skip if no cache, or i-cache
+        mcr     p15, 2, r10, c0, c0, 0          @ select current cache level
+        isb                                     @ isb to sych new cssr&csidr
+        mrc     p15, 1, r1, c0, c0, 0           @ read the new csidr
+        and     r2, r1, #7                      @ extract length of cache lines
+        add     r2, r2, #4                      @ add 4 (line length offset)
+        ldr     r4, =0x3ff
+        ands    r4, r4, r1, lsr #3              @ find maximum on the way size
+        clz     r5, r4                          @ bit position of way size inc
+        ldr     r7, =0x7fff
+        ands    r7, r7, r1, lsr #13             @ max number of the index size
+loop2:
+        mov     r9, r4                          @ create copy of max way size
+loop3:
+	orr     r11, r10, r9, lsl r5		@ way and cache number into r11
+	orr     r11, r11, r7, lsl r2		@ way and cache number into r11
+        mcr     p15, 0, r11, c7, c14, 2         @ clean & invalidate by set/way
+        subs    r9, r9, #1                      @ decrement the way
+        bge     loop3
+        subs    r7, r7, #1                      @ decrement the index
+        bge     loop2
+skip:
+        add     r10, r10, #2                    @ increment cache number
+        cmp     r3, r10
+        bgt     loop1
+finished:
+        mov     r10, #0                         @ swith back to cache level 0
+        mcr     p15, 2, r10, c0, c0, 0          @ current cache level in cssr
+        dsb
+        isb
+
+	/* L2 cache cleaning */
+
+	ldr r0, =UX500L2CCBASE
+	ldr r2, [r0, #L2X0_AUX_CTRL]
+
+	ldr r2, =0xff
+	str r2, [r0, #L2X0_CLEAN_WAY]
+	2:
+	ldr r3, [r0, #L2X0_CLEAN_WAY]
+	cmp r3, #0
+	bne 2b
+
+	ldr r2, =0xff
+	str r2, [r0, #L2X0_INV_WAY]
+	1:
+	ldr r3, [r0, #L2X0_INV_WAY]
+	cmp r3, #0
+	bne 1b
+
+	mcr p15, 0, r0, c7, c5, 6		@ flush BTAC/BTB
+	isb
+
+	ldmfd sp!, {r4-r5,r7,r9-r11, pc}
+
+ENDPROC(ux500_flush_all)