ENGR00216076-1: PM: Add Power Management driver for Vybrid

System could run into STOP and LPRun modes. When system was working in STOP mode,
pressing SW1 button or inserting or removing SD card could wake up it.

Signed-off-by: Alison Wang <b18965@freescale.com>

10 files changed, 1076 insertions, 14 deletions

diff --git a/arch/arm/mach-mvf/Makefile b/arch/arm/mach-mvf/Makefile
index c103e4dc3078..e25fb28b7216 100644
--- a/arch/arm/mach-mvf/Makefile
+++ b/arch/arm/mach-mvf/Makefile
@@ -3,8 +3,8 @@
 #
 
 # Object file lists.
-obj-y   := cpu.o mm.o system.o devices.o dummy_gpio.o irq.o bus_freq.o mvf_fec.o usb_dr.o usb_dr2.o
+obj-y   := cpu.o mm.o system.o devices.o dummy_gpio.o irq.o bus_freq.o mvf_fec.o usb_dr.o usb_dr2.o pm.o
 obj-y += l2switch.o
 
-obj-$(CONFIG_ARCH_MVFA5) += clock.o
+obj-$(CONFIG_ARCH_MVFA5) += clock.o mvf_suspend.o
 obj-$(CONFIG_MACH_MVFA5_TWR_VF700) += board-twr-vf700.o
diff --git a/arch/arm/mach-mvf/board-twr-vf700.c b/arch/arm/mach-mvf/board-twr-vf700.c
index 03cc6ad51788..5903f7ae0244 100644
--- a/arch/arm/mach-mvf/board-twr-vf700.c
+++ b/arch/arm/mach-mvf/board-twr-vf700.c
@@ -72,6 +72,7 @@
 #include <mach/mipi_dsi.h>
 #include <mach/mipi_csi2.h>
 #include <mach/fsl_l2_switch.h>
+#include <mach/mxc.h>
 #include <asm/irq.h>
 #include <asm/setup.h>
 #include <asm/mach-types.h>
@@ -364,22 +365,20 @@ static void spi_device_init(void)
 				ARRAY_SIZE(mvf_spi_board_info));
 }
 
-#if 1
-static void vf700_suspend_enter(void)
+static void vf600_suspend_enter(void)
 {
 	/* suspend preparation */
 }
 
-static void vf700_suspend_exit(void)
+static void vf600_suspend_exit(void)
 {
 	/* resmue resore */
 }
-static const struct pm_platform_data mvf_vf700_pm_data __initconst = {
+static const struct pm_platform_data mvf_vf600_pm_data __initconst = {
 	.name = "mvf_pm",
-	.suspend_enter = vf700_suspend_enter,
-	.suspend_exit = vf700_suspend_exit,
+	.suspend_enter = vf600_suspend_enter,
+	.suspend_exit = vf600_suspend_exit,
 };
-#endif
 
 static struct mvf_dcu_platform_data mvf_dcu_pdata = {
 	.mode_str	= "480x272",
@@ -458,6 +457,8 @@ static void __init mvf_board_init(void)
 
 	mvf_add_snvs_rtc();
 
+	mvf_add_pm_imx(0, &mvf_vf600_pm_data);
+
 	mvf_add_sdhci_esdhc_imx(1, &mvfa5_sd1_data);
 
 	mvf_add_imx_i2c(0, &mvf600_i2c_data);
diff --git a/arch/arm/mach-mvf/cpu.c b/arch/arm/mach-mvf/cpu.c
index 2254ac5974c5..4c6d018cd076 100644
--- a/arch/arm/mach-mvf/cpu.c
+++ b/arch/arm/mach-mvf/cpu.c
@@ -81,8 +81,7 @@ EXPORT_SYMBOL(mvf_revision);
 
 static int __init post_cpu_init(void)
 {
-
-	/*iram_init(MVF_IRAM_BASE_ADDR, MVF_IRAM_SIZE);*/
+	iram_init(MVF_IRAM_BASE_ADDR, MVF_IRAM_SIZE);
 
 	/* Move wait routine into iRAM */
 	ccm_base = MVF_IO_ADDRESS(MVF_CCM_BASE_ADDR);
diff --git a/arch/arm/mach-mvf/crm_regs.h b/arch/arm/mach-mvf/crm_regs.h
index 78604eb96447..57fac5bbe973 100644
--- a/arch/arm/mach-mvf/crm_regs.h
+++ b/arch/arm/mach-mvf/crm_regs.h
@@ -50,6 +50,7 @@
 #define PFD_480_BASE_ADDR		(MXC_PLL_BASE + 0xF0)
 #define PFD_528_BASE_ADDR		(MXC_PLL_BASE + 0x100)
 #define PFD_528SYS_BASE_ADDR		(MXC_PLL_BASE + 0x2B0)
+#define ANADIG_2P5_ADDR			(MXC_PLL_BASE + 0x130)
 #define ANADIG_MISC0_REG		(MXC_PLL_BASE + 0x150)
 #define ANADIG_MISC1_REG		(MXC_PLL_BASE + 0x160)
 #define PLL_PFD_480_USB1		(MXC_PLL_BASE + 0xF0)
diff --git a/arch/arm/mach-mvf/irq.c b/arch/arm/mach-mvf/irq.c
index c3e7b9291faa..89c7d382173f 100644
--- a/arch/arm/mach-mvf/irq.c
+++ b/arch/arm/mach-mvf/irq.c
@@ -36,9 +36,9 @@ static int mvf_gic_irq_set_wake(struct irq_data *d, unsigned int enable)
 
 	if (enable) {
 		gpc_wake_irq[d->irq / 32 - 1] |= 1 << (d->irq % 32);
-		printk(KERN_INFO "add wake up source irq %d\n", d->irq);
+		printk(KERN_DEBUG "add wake up source irq %d\n", d->irq);
 	} else {
-		printk(KERN_INFO "remove wake up source irq %d\n", d->irq);
+		printk(KERN_DEBUG "remove wake up source irq %d\n", d->irq);
 		gpc_wake_irq[d->irq / 32 - 1] &= ~(1 << (d->irq % 32));
 	}
 	return 0;
@@ -48,6 +48,7 @@ void mvf_init_irq(void)
 	unsigned int i;
 	void __iomem *int_router_base =
 				MVF_IO_ADDRESS(MVF_MSCM_INT_ROUTER_BASE);
+	struct irq_desc *desc;
 
 	/* start offset if private timer irq id, which is 29.
 	 * ID table:
@@ -60,6 +61,11 @@ void mvf_init_irq(void)
 	gic_init(0, 27, MVF_IO_ADDRESS(MVF_INTD_BASE_ADDR),
 		MVF_IO_ADDRESS(MVF_SCUGIC_BASE_ADDR + 0x100));
 
+	for (i = MXC_INT_START; i <= MXC_INT_END; i++) {
+		desc = irq_to_desc(i);
+		desc->irq_data.chip->irq_set_wake = mvf_gic_irq_set_wake;
+	}
+
 	mvf_register_gpios();
 
 	for (i = 0; i < 112; i++)
diff --git a/arch/arm/mach-mvf/mvf_suspend.S b/arch/arm/mach-mvf/mvf_suspend.S
new file mode 100644
index 000000000000..ce9205162538
--- /dev/null
+++ b/arch/arm/mach-mvf/mvf_suspend.S
@@ -0,0 +1,484 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * 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.
+ */
+
+#include <linux/linkage.h>
+#include <mach/hardware.h>
+#include <asm/memory.h>
+#include <mach/mvf.h>
+#include "regs-src.h"
+#include "crm_regs.h"
+
+#define PERIPBASE_VIRT_OFFSET	0xb2000000
+#define TTRBIT_MASK 		0xffffc000
+#define TABLE_INDEX_MASK 	0xfff00000
+#define TABLE_ENTRY 		0x00000c02
+#define CACHE_DISABLE_MASK 	0xfffffffb
+#define IRAM_SUSPEND_SIZE 	(1 << 15)
+
+/*************************************************************
+mvf_suspend:
+
+Suspend the processor (eg, wait for interrupt).
+
+r1: iram_paddr
+r2: suspend_iram_base
+*************************************************************/
+
+	.macro	mvf_stop_mode_enter
+
+	ldr	r3, =MVF_ANATOP_BASE_ADDR
+	add	r3, r3, #PERIPBASE_VIRT_OFFSET
+	add	r3, r3, #0x20000
+
+	/* pll7 disable */
+	ldr	r4, [r3, #0x20]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0x20]
+
+	/* pll3 disable */
+	ldr	r4, [r3, #0x10]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0x10]
+
+	/* pll4 disable */
+	ldr	r4, [r3, #0x70]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0x70]
+
+	/* pll6 disable */
+	ldr	r4, [r3, #0xa0]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0xa0]
+
+	/* pll5 disable */
+	ldr	r4, [r3, #0xe0]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0xe0]
+
+	/* pll1 disable */
+	ldr	r4, [r3, #0x270]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0x270]
+
+	/* stop mode is masked to Anatop */
+	ldr	r3, =MVF_CCM_BASE_ADDR
+	add	r3, r3, #PERIPBASE_VIRT_OFFSET
+	add	r3, r3, #0x20000
+
+	ldr	r4, [r3, #0x2c]
+	bic	r4, r4, #0x100
+	str	r4, [r3, #0x2c]
+
+	ldr	r3, =MVF_GPC_BASE_ADDR
+	add	r3, r3, #PERIPBASE_VIRT_OFFSET
+	add	r3, r3, #0x20000
+
+	/* ensure power domain 0 */
+	ldr	r4, [r3, #0x0]
+	bic	r4, r4, #0x01
+	str	r4, [r3, #0x0]
+
+	/* enable deep sleep for memories */
+	ldr	r4, [r3, #0x0]
+	orr	r4, r4, #0x80
+	str	r4, [r3, #0x0]
+
+	/* disable well bias */
+	ldr	r4, [r3, #0x0]
+	bic	r4, r4, #0x10
+	str	r4, [r3, #0x0]
+
+	/* turn off HPREG in stop mode */
+	ldr	r4, [r3, #0x0]
+	orr	r4, r4, #0x08
+	str	r4, [r3, #0x0]
+
+	/* gpc_lpmr set stop mode */
+	ldr	r4, =0x02
+	str	r4, [r3, #0x40]
+
+	.endm
+
+	.macro	mvf_lpstop_mode_enter
+
+	ldr	r3, =MVF_ANATOP_BASE_ADDR
+	add	r3, r3, #PERIPBASE_VIRT_OFFSET
+	add	r3, r3, #0x20000
+
+	/* pll7 disable */
+	ldr	r4, [r3, #0x20]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0x20]
+
+	/* pll3 disable */
+	ldr	r4, [r3, #0x10]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0x10]
+
+	/* pll4 disable */
+	ldr	r4, [r3, #0x70]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0x70]
+
+	/* pll6 disable */
+	ldr	r4, [r3, #0xa0]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0xa0]
+
+	/* pll5 disable */
+	ldr	r4, [r3, #0xe0]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0xe0]
+
+	/* pll1 disable */
+	ldr	r4, [r3, #0x270]
+	bic	r4, r4, #0x2000
+	str	r4, [r3, #0x270]
+
+	ldr	r3, =MVF_CCM_BASE_ADDR
+	add	r3, r3, #PERIPBASE_VIRT_OFFSET
+	add	r3, r3, #0x20000
+
+	ldr	r4, [r3, #0x2c]
+	bic	r4, r4, #0x100
+	str	r4, [r3, #0x2c]
+
+	ldr	r3, =MVF_GPC_BASE_ADDR
+	add	r3, r3, #PERIPBASE_VIRT_OFFSET
+	add	r3, r3, #0x20000
+
+	/* enable deep sleep for memories */
+	ldr	r4, [r3, #0x0]
+	orr	r4, r4, #0x40
+	str	r4, [r3, #0x0]
+
+	/* enable LPSTOP3 */
+	ldr	r4, [r3, #0x0]
+	bic	r4, r4, #0x04
+	bic	r4, r4, #0x02
+	str	r4, [r3, #0x0]
+
+	/* ensure power domain 1 */
+	ldr	r4, [r3, #0x0]
+	orr	r4, r4, #0x01
+	str	r4, [r3, #0x0]
+
+	/* gpc_lpmr set low-power stop mode */
+	ldr	r4, =0x02
+	str	r4, [r3, #0x40]
+
+	.endm
+
+/******************************************************************
+Invalidate l1 dcache, r0-r4, r6, r7 used
+******************************************************************/
+	.macro	invalidate_l1_dcache
+
+	mov 	r0, #0
+	mcr 	p15, 2, r0, c0, c0, 0
+	mrc 	p15, 1, r0, c0, c0, 0
+
+	ldr 	r1, =0x7fff
+	and 	r2, r1, r0, lsr #13
+
+	ldr 	r1, =0x3ff
+
+	and 	r3, r1, r0, lsr #3	@ NumWays - 1
+	add 	r2, r2, #1		@ NumSets
+
+	and 	r0, r0, #0x7
+	add 	r0, r0, #4		@ SetShift
+
+	clz 	r1, r3			@ WayShift
+	add 	r4, r3, #1		@ NumWays
+1:
+	sub 	r2, r2, #1		@ NumSets--
+	mov 	r3, r4			@ Temp = NumWays
+2:
+	subs	r3, r3, #1		@ Temp--
+	mov 	r7, r3, lsl r1
+	mov 	r6, r2, lsl r0
+	orr 	r7, r7, r6
+	mcr 	p15, 0, r7, c7, c6, 2
+	bgt 	2b
+	cmp 	r2, #0
+	bgt 	1b
+	dsb
+	isb
+
+	.endm
+
+/******************************************************************
+Flush and disable L1 dcache
+******************************************************************/
+	.macro	flush_disable_l1_dcache
+
+	/*
+	 * Flush all data from the L1 data cache before disabling
+	 * SCTLR.C bit.
+	 */
+	push	{r0-r12, lr}
+	ldr r0, =v7_flush_dcache_all
+	mov lr, pc
+	mov pc, r0
+	pop {r0-r12, lr}
+
+	/*
+	 * Clear the SCTLR.C bit to prevent further data cache
+	 * allocation. Clearing SCTLR.C would make all the data accesses
+	 * strongly ordered and would not hit the cache.
+	 */
+	mrc p15, 0, r0, c1, c0, 0
+	bic r0, r0, #(1 << 2)		@ Disable the C bit
+	mcr p15, 0, r0, c1, c0, 0
+	isb
+
+	/*
+	 * Invalidate L1 data cache. Even though only invalidate is
+	 * necessary exported flush API is used here. Doing clean
+	 * on already clean cache would be almost NOP.
+	 */
+	push	{r0-r12, lr}
+	ldr r0, =v7_flush_dcache_all
+	mov lr, pc
+	mov pc, r0
+	pop {r0-r12, lr}
+
+	/*
+	 * Execute an ISB instruction to ensure that all of the
+	 * CP15 register changes have been committed.
+	 */
+	isb
+
+	/*
+	 * Execute a barrier instruction to ensure that all cache,
+	 * TLB and branch predictor maintenance operations issued
+	 * by any CPU in the cluster have completed.
+	 */
+	dsb
+	dmb
+
+	.endm
+
+ENTRY(mvf_suspend)
+	stmfd	sp!, {r0-r12}     @ Save registers
+/*************************************************************
+suspend mode entry
+*************************************************************/
+	mov	r11, r0
+
+	cmp	r0, #0x1
+	bne	dormant
+
+	/* Need to flush and disable L1 dcache*/
+	flush_disable_l1_dcache
+
+	wfi
+
+	nop
+	nop
+	nop
+	nop
+
+	/* Invalidate L1 I-cache first */
+	mov r1, #0x0
+	mcr p15, 0, r1, c7, c5, 0 @ Invalidate I-Cache
+
+	/* Need to invalidate L1 dcache, as the power is dropped */
+	invalidate_l1_dcache
+
+	/* Enable L1 dcache first */
+	mrc p15, 0, r0, c1, c0, 0
+	orr r0, r0, #(1 << 2)	@ Disable the C bit
+	mcr p15, 0, r0, c1, c0, 0
+
+/***********************************************************
+never run to here
+************************************************************/
+	b 	out	/* exit standby */
+	/* Place the literal pool here so that literals are
+	within 16KB range */
+	.ltorg
+
+/************************************************************
+dormant entry, data save in stack
+************************************************************/
+dormant:
+
+/************************************************************
+saved register and context as below:
+	sp
+	spsr
+	lr
+	CPACR
+	TTBR0
+	TTBR1
+	TTBCR
+	DACR
+	PRRR
+	NMRR
+	ACTLR
+	Context ID
+	User r/w thread ID
+	Secure or NS VBAR
+	CPSR
+	SCTLR
+************************************************************/
+	/* stack is from the tail of iram_suspend base */
+	mov	r0, r2 		/* get suspend_iram_base */
+	add	r0, r0, #IRAM_SUSPEND_SIZE
+
+	mov	r4, r11			@ Store state entered
+	stmfd	r0!, {r4}
+
+	mov	r4, sp			@ Store sp
+	mrs	r5, spsr		@ Store spsr
+	mov	r6, lr			@ Store lr
+	stmfd	r0!, {r4-r6}
+
+	/* c1 and c2 registers */
+	mrc	p15, 0, r4, c1, c0, 2	@ CPACR
+	mrc	p15, 0, r5, c2, c0, 0	@ TTBR0
+	mrc	p15, 0, r6, c2, c0, 1	@ TTBR1
+	mrc	p15, 0, r7, c2, c0, 2	@ TTBCR
+	stmfd	r0!, {r4-r7}
+
+	/* c3 and c10 registers */
+	mrc	p15, 0, r4, c3, c0, 0	@ DACR
+	mrc	p15, 0, r5, c10, c2, 0	@ PRRR
+	mrc	p15, 0, r6, c10, c2, 1	@ NMRR
+	mrc	p15, 0, r7, c1, c0, 1	@ ACTLR
+	stmfd	r0!,{r4-r7}
+
+	/* c12, c13 and CPSR registers */
+	mrc	p15, 0, r4, c13, c0, 1	@ Context ID
+	mrc	p15, 0, r5, c13, c0, 2	@ User r/w thread ID
+	mrc	p15, 0, r6, c12, c0, 0	@ Secure or NS VBAR
+	mrs	r7, cpsr		@ Store CPSR
+	stmfd	r0!, {r4-r7}
+
+	/* c1 control register */
+	mrc	p15, 0, r4, c1, c0, 0	@ SCTLR
+	stmfd	r0!, {r4}
+
+	/* Need to flush and disable L1 dcache*/
+	flush_disable_l1_dcache
+
+	/* Make sure TLBs are primed */
+	ldr	r1,  =MVF_IOMUXC_BASE_ADDR
+	add	r1, r1, #PERIPBASE_VIRT_OFFSET
+	add	r1, r1, #0x20000
+	ldr	r0, [r1]
+	ldr	r1, =MVF_SRC_BASE_ADDR
+	add	r1, r1, #PERIPBASE_VIRT_OFFSET
+	add	r1, r1, #0x20000
+	ldr	r0, [r1]
+	ldr	r1, =MVF_CCM_BASE_ADDR
+	add	r1, r1, #PERIPBASE_VIRT_OFFSET
+	add	r1, r1, #0x20000
+	ldr	r0, [r1]
+	ldr	r1, =MVF_GPC_BASE_ADDR
+	add	r1, r1, #PERIPBASE_VIRT_OFFSET
+	add	r1, r1, #0x20000
+	ldr	r0, [r1]
+	ldr	r1, =MVF_CCM_BASE_ADDR
+	add	r1, r1, #PERIPBASE_VIRT_OFFSET
+	add	r1, r1, #0x20000
+	ldr	r0, [r1]
+
+	/* Do a DSB to drain the buffers */
+	dsb
+
+	mvf_stop_mode_enter
+
+/****************************************************************
+execute a wfi instruction to let SOC go into stop mode.
+****************************************************************/
+	wfi
+
+	nop
+	nop
+	nop
+	nop
+
+/****************************************************************
+if go here, means there is a wakeup irq pending, we should resume
+system immediately.
+****************************************************************/
+	mov	r0, r2 		/* get suspend_iram_base */
+	add	r0, r0, #IRAM_SUSPEND_SIZE
+
+	ldmea	r0!, {r11}	@ standby or mem
+
+	/* mask all the GPC interrupts */
+	ldr	r3, =MVF_GPC_BASE_ADDR
+	add	r3, r3, #PERIPBASE_VIRT_OFFSET
+	add	r3, r3, #0x20000
+	ldr	r4, =0xffffffff
+	str	r4, [r3, #0x44]
+	str	r4, [r3, #0x48]
+	str	r4, [r3, #0x4c]
+	str	r4, [r3, #0x50]
+
+	/* pll2 enable */
+	ldr	r3, =MVF_ANATOP_BASE_ADDR
+	add	r3, r3, #PERIPBASE_VIRT_OFFSET
+	add	r3, r3, #0x20000
+
+	ldr	r4, [r3, #0x30]
+	orr	r4, r4, #0x2000
+	str	r4, [r3, #0x30]
+
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	mrc	p15, 0, r1, c1, c0, 0
+	orr	r1, r1, #(1 << 2)	@ Enable the C bit
+	mcr	p15, 0, r1, c1, c0, 0
+
+	b 	out
+
+/************************************************
+return back to mvf_suspend_enter for suspend
+*************************************************/
+out:
+	ldmfd	sp!, {r0-r12}
+	mov	pc, lr
+
+	.type	mvf_do_suspend, #object
+ENTRY(mvf_do_suspend)
+	.word	mvf_suspend
+	.size	mvf_suspend, . - mvf_suspend
diff --git a/arch/arm/mach-mvf/pm.c b/arch/arm/mach-mvf/pm.c
new file mode 100644
index 000000000000..0e62caf372da
--- /dev/null
+++ b/arch/arm/mach-mvf/pm.c
@@ -0,0 +1,312 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * This 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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/suspend.h>
+#include <linux/iram_alloc.h>
+#include <linux/interrupt.h>
+#include <asm/tlb.h>
+#include <asm/mach/map.h>
+#include <mach/hardware.h>
+#include <asm/hardware/gic.h>
+#include <mach/mxc_uart.h>
+#include "crm_regs.h"
+#include "regs-anadig.h"
+#include "regs-pm.h"
+#include "regs-src.h"
+
+static struct clk *cpu_clk;
+static struct clk *ipg_clk;
+static struct clk *periph_clk;
+
+extern void mvf_suspend(suspend_state_t state);
+
+static struct device *pm_dev;
+static void __iomem *scu_base;
+static void __iomem *gpc_base;
+static void __iomem *src_base;
+static void __iomem *anadig_base;
+
+static void *suspend_iram_base;
+static void (*suspend_in_iram)(suspend_state_t state,
+	unsigned long iram_paddr, unsigned long suspend_iram_base) = NULL;
+static unsigned long iram_paddr, cpaddr;
+
+static u32 ccm_ccr, ccm_clpcr, ccm_ccsr, scu_ctrl;
+static u32 gpc_imr[4], gpc_pgcr, gpc_lpmr;
+static u32 ccm_anadig_pfd528, ccm_anadig_pfd480_usb0, ccm_anadig_2p5;
+static int g_state;
+
+static void mvf_suspend_store(void)
+{
+	/* save some settings before suspend */
+	ccm_ccr = __raw_readl(MXC_CCM_CCR);
+	ccm_clpcr = __raw_readl(MXC_CCM_CLPCR);
+	ccm_ccsr = __raw_readl(MXC_CCM_CCSR);
+	ccm_anadig_pfd528 = __raw_readl(PFD_528_BASE_ADDR);
+	ccm_anadig_pfd480_usb0 = __raw_readl(PFD_480_BASE_ADDR);
+	ccm_anadig_2p5 = __raw_readl(ANADIG_2P5_ADDR);
+
+	scu_ctrl = __raw_readl(scu_base + SCU_CTRL_OFFSET);
+	gpc_imr[0] = __raw_readl(gpc_base + GPC_IMR1_OFFSET);
+	gpc_imr[1] = __raw_readl(gpc_base + GPC_IMR2_OFFSET);
+	gpc_imr[2] = __raw_readl(gpc_base + GPC_IMR3_OFFSET);
+	gpc_imr[3] = __raw_readl(gpc_base + GPC_IMR4_OFFSET);
+	gpc_pgcr = __raw_readl(gpc_base + GPC_PGCR_OFFSET);
+	gpc_lpmr = __raw_readl(gpc_base + GPC_LPMR_OFFSET);
+}
+
+static void mvf_suspend_restore(void)
+{
+	/* restore settings after suspend */
+	__raw_writel(ccm_anadig_2p5, ANADIG_2P5_ADDR);
+
+	udelay(50);
+
+	__raw_writel(ccm_ccr, MXC_CCM_CCR);
+	__raw_writel(ccm_clpcr, MXC_CCM_CLPCR);
+	__raw_writel(ccm_anadig_pfd528, PFD_528_BASE_ADDR);
+	__raw_writel(ccm_anadig_pfd480_usb0, PFD_480_BASE_ADDR);
+
+	__raw_writel(scu_ctrl, scu_base + SCU_CTRL_OFFSET);
+	__raw_writel(gpc_imr[0], gpc_base + GPC_IMR1_OFFSET);
+	__raw_writel(gpc_imr[1], gpc_base + GPC_IMR2_OFFSET);
+	__raw_writel(gpc_imr[2], gpc_base + GPC_IMR3_OFFSET);
+	__raw_writel(gpc_imr[3], gpc_base + GPC_IMR4_OFFSET);
+	__raw_writel(gpc_pgcr, gpc_base + GPC_PGCR_OFFSET);
+	__raw_writel(gpc_lpmr, gpc_base + GPC_LPMR_OFFSET);
+
+	/* enable PLLs */
+	__raw_writel(__raw_readl(PLL3_480_USB1_BASE_ADDR) | ANADIG_PLL_ENABLE,
+		PLL3_480_USB1_BASE_ADDR);
+	__raw_writel(__raw_readl(PLL3_480_USB2_BASE_ADDR) | ANADIG_PLL_ENABLE,
+		PLL3_480_USB2_BASE_ADDR);
+	__raw_writel(__raw_readl(PLL4_AUDIO_BASE_ADDR) | ANADIG_PLL_ENABLE,
+		PLL4_AUDIO_BASE_ADDR);
+	__raw_writel(__raw_readl(PLL6_VIDEO_BASE_ADDR) | ANADIG_PLL_ENABLE,
+		PLL6_VIDEO_BASE_ADDR);
+	__raw_writel(__raw_readl(PLL5_ENET_BASE_ADDR) | ANADIG_PLL_ENABLE,
+		PLL5_ENET_BASE_ADDR);
+	__raw_writel(__raw_readl(PLL1_SYS_BASE_ADDR) | ANADIG_PLL_ENABLE,
+		PLL1_SYS_BASE_ADDR);
+
+	/* restore system clock */
+	__raw_writel(ccm_ccsr, MXC_CCM_CCSR);
+}
+
+static void uart_reinit(unsigned long int clkspeed, unsigned long int baud)
+{
+	void __iomem *membase;
+	u8 tmp;
+	u16 sbr, brfa;
+
+	membase = MVF_IO_ADDRESS(MVF_UART1_BASE_ADDR);
+
+	__raw_writeb(0, membase + MXC_UARTMODEM);
+
+	/* make sure the transmitter and receiver are
+	 * disabled while changing settings */
+	tmp = __raw_readb(membase + MXC_UARTCR2);
+	tmp &= ~MXC_UARTCR2_RE;
+	tmp &= ~MXC_UARTCR2_TE;
+	__raw_writeb(tmp, membase + MXC_UARTCR2);
+
+	__raw_writeb(0, membase + MXC_UARTCR1);
+
+	sbr = (u16) ((clkspeed * 1000) / (baud * 16));
+	brfa = ((clkspeed * 1000) / baud) - (sbr * 16);
+
+	tmp = __raw_readb(membase + MXC_UARTBDH);
+	tmp &= ~MXC_UARTBDH_SBR_MASK;
+	tmp |= ((sbr & 0x1f00) >> 8);
+	__raw_writeb(tmp, membase + MXC_UARTBDH);
+	tmp = sbr & 0x00ff;
+	__raw_writeb(tmp, membase + MXC_UARTBDL);
+
+	tmp = __raw_readb(membase + MXC_UARTCR4);
+	tmp &= ~MXC_UARTCR4_BRFA_MASK;
+	tmp |= (brfa & MXC_UARTCR4_BRFA_MASK);
+	__raw_writeb(tmp , membase + MXC_UARTCR4);
+
+	tmp = __raw_readb(membase + MXC_UARTCR2);
+	tmp |= MXC_UARTCR2_RE;
+	tmp |= MXC_UARTCR2_TE;
+	__raw_writeb(tmp, membase + MXC_UARTCR2);
+}
+
+static int mvf_suspend_enter(suspend_state_t state)
+{
+	struct gic_dist_state gds;
+	struct gic_cpu_state gcs;
+	bool arm_pg = false;
+
+	mvf_suspend_store();
+
+	switch (state) {
+	case PM_SUSPEND_MEM:
+		mvf_cpu_lp_set(LOW_POWER_STOP);
+		g_state = PM_SUSPEND_MEM;
+		arm_pg = true;
+		break;
+	case PM_SUSPEND_STANDBY:
+		mvf_cpu_lp_set(LOW_POWER_RUN);
+		g_state = PM_SUSPEND_STANDBY;
+		arm_pg = true;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (state == PM_SUSPEND_MEM || state == PM_SUSPEND_STANDBY) {
+		local_flush_tlb_all();
+		flush_cache_all();
+
+		if (arm_pg) {
+			/* preserve GIC state */
+			save_gic_dist_state(0, &gds);
+			save_gic_cpu_state(0, &gcs);
+		}
+
+		suspend_in_iram(state, (unsigned long)iram_paddr,
+			(unsigned long)suspend_iram_base);
+
+		/* reconfigure UART1 when using 24MHz system clock */
+		uart_reinit(4000, 115200);
+
+		printk(KERN_DEBUG "Read GPC_PGSR register: %x\n",
+			__raw_readl(gpc_base + GPC_PGSR_OFFSET));
+
+		/* mask all interrupts */
+		__raw_writel(0xffffffff, gpc_base + GPC_IMR1_OFFSET);
+		__raw_writel(0xffffffff, gpc_base + GPC_IMR2_OFFSET);
+		__raw_writel(0xffffffff, gpc_base + GPC_IMR3_OFFSET);
+		__raw_writel(0xffffffff, gpc_base + GPC_IMR4_OFFSET);
+
+		udelay(80);
+
+		if (arm_pg) {
+			/* restore GIC registers */
+			restore_gic_dist_state(0, &gds);
+			restore_gic_cpu_state(0, &gcs);
+		}
+
+		mvf_suspend_restore();
+
+		/* reconfigure UART1 when using 396MHz system clock */
+		uart_reinit(66000, 115200);
+
+		__raw_writel(BM_ANADIG_ANA_MISC0_STOP_MODE_CONFIG,
+			ANADIG_MISC0_REG);
+	} else
+		cpu_do_idle();
+
+	return 0;
+}
+
+static void mvf_suspend_finish(void)
+{
+	if (g_state == PM_SUSPEND_MEM)
+		free_irq(MVF_INT_WKPU0, NULL);
+}
+
+static int mvf_pm_valid(suspend_state_t state)
+{
+	return (state > PM_SUSPEND_ON && state <= PM_SUSPEND_MAX);
+}
+
+const struct platform_suspend_ops mvf_suspend_ops = {
+	.valid = mvf_pm_valid,
+	.enter = mvf_suspend_enter,
+	.finish = mvf_suspend_finish,
+};
+
+static int __devinit mvf_pm_probe(struct platform_device *pdev)
+{
+	pm_dev = &pdev->dev;
+
+	return 0;
+}
+
+static struct platform_driver mvf_pm_driver = {
+	.driver = {
+		   .name = "imx_pm",
+	},
+	.probe = mvf_pm_probe,
+};
+
+static int __init pm_init(void)
+{
+	scu_base = MVF_IO_ADDRESS(MVF_SCUGIC_BASE_ADDR);
+	gpc_base = MVF_GPC_BASE;
+	anadig_base = MXC_PLL_BASE;
+	src_base = MVF_IO_ADDRESS(MVF_SRC_BASE_ADDR);
+
+	pr_info("Static Power Management for Freescale Vybrid\n");
+
+	if (platform_driver_register(&mvf_pm_driver) != 0) {
+		printk(KERN_ERR "mvf_pm_driver register failed\n");
+		return -ENODEV;
+	}
+
+	suspend_set_ops(&mvf_suspend_ops);
+
+	/* move suspend routine into sram */
+	cpaddr = (unsigned long)iram_alloc(SZ_32K, &iram_paddr);
+
+	/* need to remap the area here since we want the memory region
+	 * to be executable */
+	suspend_iram_base = __arm_ioremap(iram_paddr, SZ_32K,
+		MT_MEMORY_NONCACHED);
+	printk(KERN_DEBUG "cpaddr = %x suspend_iram_base=%x iram_paddr %x\n",
+		(unsigned int)cpaddr, (unsigned int)suspend_iram_base,
+		(unsigned int)iram_paddr);
+
+	/* need to run the suspend code from sram */
+	memcpy((void *)cpaddr, mvf_suspend, SZ_32K);
+
+	suspend_in_iram = (void *)suspend_iram_base;
+
+	cpu_clk = clk_get(NULL, "cpu_clk");
+	if (IS_ERR(cpu_clk)) {
+		printk(KERN_DEBUG "%s: failed to get cpu_clk\n", __func__);
+		return PTR_ERR(cpu_clk);
+	}
+	ipg_clk = clk_get(NULL, "ipg_clk");
+	if (IS_ERR(ipg_clk)) {
+		printk(KERN_DEBUG "%s: failed to get ipg_clk\n", __func__);
+		return PTR_ERR(ipg_clk);
+	}
+	periph_clk = clk_get(NULL, "periph_clk");
+	if (IS_ERR(periph_clk)) {
+		printk(KERN_DEBUG "%s: failed to get periph_clk\n", __func__);
+		return PTR_ERR(periph_clk);
+	}
+
+	printk(KERN_INFO "PM driver module loaded\n");
+	return 0;
+}
+
+static void __exit pm_cleanup(void)
+{
+	platform_driver_unregister(&mvf_pm_driver);
+}
+
+module_init(pm_init);
+module_exit(pm_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("PM driver");
+MODULE_LICENSE("GPL");
diff --git a/arch/arm/mach-mvf/regs-pm.h b/arch/arm/mach-mvf/regs-pm.h
new file mode 100644
index 000000000000..c54bdd346709
--- /dev/null
+++ b/arch/arm/mach-mvf/regs-pm.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * This 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 __ARCH_ARM_MACH_MVF_REGS_PM_H__
+#define __ARCH_ARM_MACH_MVF_REGS_PM_H__
+
+/* GPC registers */
+#define GPC_PGCR_OFFSET			(0x00)
+#define GPC_PGCR_DS_STOP		(0x000000080)
+#define GPC_PGCR_DS_LPSTOP		(0x000000040)
+#define GPC_PGCR_WB_STOP		(0x000000010)
+#define GPC_PGCR_HP_OFF			(0x000000008)
+#define GPC_PGCR_PG_48K			(0x000000004)
+#define GPC_PGCR_PG_16K			(0x000000002)
+#define GPC_PGCR_PG_PD1			(0x000000001)
+
+#define GPC_PGSR_OFFSET			(0x0c)
+
+#define GPC_LPMR_OFFSET			(0x40)
+#define GPC_LPMR_CLPCR_RUN		(0x00000000)
+#define GPC_LPMR_CLPCR_WAIT		(0x00000001)
+#define GPC_LPMR_CLPCR_STOP		(0x00000002)
+
+#define GPC_IMR1_OFFSET			(0x44)
+#define GPC_IMR2_OFFSET			(0x48)
+#define GPC_IMR3_OFFSET			(0x4c)
+#define GPC_IMR4_OFFSET			(0x50)
+#define GPC_ISR1_OFFSET			(0x54)
+#define GPC_ISR2_OFFSET			(0x58)
+#define GPC_ISR3_OFFSET			(0x5c)
+#define GPC_ISR4_OFFSET			(0x60)
+
+/* VREG registers */
+#define VREG_CTRL_OFFSET		(0x00)
+#define VREG_CTRL_PORPU			(0x00010000)
+#define VREG_CTRL_HVDMASK		(0x00000001)
+#define VREG_STAT_OFFSET		(0x04)
+
+/* WKPU registers */
+#define WKPU_NSR_OFFSET			(0x00)
+
+#define WKPU_NCR_OFFSET			(0x08)
+#define WKPU_NCR_NLOCK0			(0x80000000)
+#define WKPU_NCR_NWRE0			(0x10000000)
+#define WKPU_NCR_NREE0			(0x04000000)
+#define WKPU_NCR_NFEE0			(0x02000000)
+#define WKPU_NCR_NFE0			(0x01000000)
+
+#define WKPU_WISR_OFFSET		(0x14)
+
+#define WKPU_IRER_OFFSET		(0x18)
+
+#define WKPU_WRER_OFFSET		(0x1c)
+
+#define WKPU_WIREER_OFFSET		(0x28)
+
+#define WKPU_WIFEER_OFFSET		(0x2c)
+
+#define WKPU_WIFER_OFFSET		(0x30)
+
+#define WKPU_WIPUER_OFFSET		(0x34)
+
+#define RISING_EDGE_ENABLED		(0x9e)
+#define FALLING_EDGE_ENABLED		(0xfe)
+
+/* SCU registers */
+#define SCU_CTRL_OFFSET			(0x00)
+
+
+#endif				/* __ARCH_ARM_MACH_MVF_REGS_PM_H__ */
diff --git a/arch/arm/mach-mvf/regs-src.h b/arch/arm/mach-mvf/regs-src.h
new file mode 100644
index 000000000000..b93e99d92da1
--- /dev/null
+++ b/arch/arm/mach-mvf/regs-src.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc,
+ *
+ * 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.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef _SRC_REGISTER_HEADER_
+#define _SRC_REGISTER_HEADER_
+
+#define SRC_SCR_OFFSET		0x000
+#define SRC_SBMR_OFFSET		0x004
+#define SRC_SRSR_OFFSET		0x008
+#define SRC_SECR_OFFSET		0x00c
+#define SRC_SISR_OFFSET		0x014
+#define SRC_SIMR_OFFSET		0x018
+#define SRC_SBMR2_OFFSET	0x01c
+#define SRC_GPR0_OFFSET		0x020
+#define SRC_GPR1_OFFSET		0x024
+#define SRC_GPR2_OFFSET		0x028
+#define SRC_GPR3_OFFSET		0x02c
+#define SRC_GPR4_OFFSET		0x030
+#define SRC_HAB0_OFFSET		0x034
+#define SRC_HAB1_OFFSET		0x038
+#define SRC_HAB2_OFFSET		0x03c
+#define SRC_HAB3_OFFSET		0x040
+#define SRC_HAB4_OFFSET		0x044
+#define SRC_HAB5_OFFSET		0x048
+#define SRC_MISC0_OFFSET	0x04c
+#define SRC_MISC1_OFFSET	0x050
+#define SRC_MISC2_OFFSET	0x054
+#define SRC_MISC3_OFFSET	0x058
+
+#endif
diff --git a/arch/arm/mach-mvf/system.c b/arch/arm/mach-mvf/system.c
index 764c608cbf42..989c75a02ed1 100644
--- a/arch/arm/mach-mvf/system.c
+++ b/arch/arm/mach-mvf/system.c
@@ -21,7 +21,8 @@
 #include <linux/delay.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
-#include <linux/pmic_external.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
 #include <asm/io.h>
 #include <mach/hardware.h>
 #include <mach/clock.h>
@@ -29,6 +30,142 @@
 #include <asm/system.h>
 #include "crm_regs.h"
 #include "regs-anadig.h"
+#include "regs-pm.h"
+
+#define SW1_WAKEUP_PIN		38
+#define	SW1_PORT1_PCR6_ADDR	0x4004a018
+
+static void __iomem *gpc_base = MVF_GPC_BASE;
+
+void gpc_set_wakeup(void)
+{
+	__raw_writel(0xffffffff, gpc_base + GPC_IMR1_OFFSET);
+	__raw_writel(0xffffffff, gpc_base + GPC_IMR2_OFFSET);
+	/* unmask WKPU0 interrupt */
+	__raw_writel(0xefffffff, gpc_base + GPC_IMR3_OFFSET);
+	/* unmask GPIO4 interrupt */
+	__raw_writel(0xffff7fff, gpc_base + GPC_IMR4_OFFSET);
+
+	return;
+}
+
+void enable_wkpu(u32 source, u32 rise_fall)
+{
+	__raw_writel(1 << source, MVF_WKPU_BASE + WKPU_IRER_OFFSET);
+	__raw_writel(1 << source, MVF_WKPU_BASE + WKPU_WRER_OFFSET);
+
+	if (rise_fall == RISING_EDGE_ENABLED)
+		__raw_writel(1 << source, MVF_WKPU_BASE + WKPU_WIREER_OFFSET);
+	else
+		__raw_writel(1 << source, MVF_WKPU_BASE + WKPU_WIFEER_OFFSET);
+}
+
+static irqreturn_t wkpu_irq(int irq, void *dev_id)
+{
+	u32 wisr;
+
+	wisr = __raw_readl(MVF_WKPU_BASE + WKPU_WISR_OFFSET);
+	if (wisr)
+		__raw_writel(wisr, MVF_WKPU_BASE + WKPU_WISR_OFFSET);
+
+	return IRQ_NONE;
+}
+
+/* set cpu multiple modes before WFI instruction */
+void mvf_cpu_lp_set(enum mvf_cpu_pwr_mode mode)
+{
+	u32 ccm_ccsr, ccm_clpcr, ccm_ccr;
+	u32 tmp;
+	int ret;
+
+	if ((mode == LOW_POWER_STOP) || (mode == STOP_MODE)) {
+		/* config SW1 for waking up system */
+		gpio_request_one(SW1_WAKEUP_PIN, GPIOF_IN, "SW1 wakeup");
+		gpio_set_value(SW1_WAKEUP_PIN, 0);
+		/* PORT1_PCR6 IRQC interrupt/dma request disabled */
+		tmp = __raw_readl(MVF_IO_ADDRESS(SW1_PORT1_PCR6_ADDR));
+		tmp &= ~0x000f0000;
+		__raw_writel(tmp, MVF_IO_ADDRESS(SW1_PORT1_PCR6_ADDR));
+
+		ret = request_irq(MVF_INT_WKPU0, wkpu_irq,
+			IRQF_DISABLED, "wkpu irq", NULL);
+		if (ret)
+			printk(KERN_ERR "Request wkpu IRQ failed\n");
+
+		/* enable WKPU interrupt */
+		enable_wkpu(11, FALLING_EDGE_ENABLED);
+	}
+
+	ccm_ccr = __raw_readl(MXC_CCM_CCR);
+	ccm_ccsr = __raw_readl(MXC_CCM_CCSR);
+
+	switch (mode) {
+	case WAIT_MODE:
+		break;
+	case LOW_POWER_RUN:
+		ccm_ccr |= MXC_CCM_CCR_FIRC_EN;
+		__raw_writel(ccm_ccr, MXC_CCM_CCR);
+		ccm_ccsr &= ~MXC_CCM_CCSR_FAST_CLK_SEL_MASK;
+		ccm_ccsr &= ~MXC_CCM_CCSR_SLOW_CLK_SEL_MASK;
+		__raw_writel(ccm_ccsr, MXC_CCM_CCSR);
+
+		/* switch system clock to FIRC 24MHz */
+		ccm_ccsr &= ~MXC_CCM_CCSR_SYS_CLK_SEL_MASK;
+		__raw_writel(ccm_ccsr, MXC_CCM_CCSR);
+
+		__raw_writel(__raw_readl(PLL3_480_USB1_BASE_ADDR) &
+			~ANADIG_PLL_ENABLE, PLL3_480_USB1_BASE_ADDR);
+		__raw_writel(__raw_readl(PLL3_480_USB2_BASE_ADDR) &
+			~ANADIG_PLL_ENABLE, PLL3_480_USB2_BASE_ADDR);
+		__raw_writel(__raw_readl(PLL4_AUDIO_BASE_ADDR) &
+			~ANADIG_PLL_ENABLE, PLL4_AUDIO_BASE_ADDR);
+		__raw_writel(__raw_readl(PLL6_VIDEO_BASE_ADDR) &
+			~ANADIG_PLL_ENABLE, PLL6_VIDEO_BASE_ADDR);
+		__raw_writel(__raw_readl(PLL5_ENET_BASE_ADDR) &
+			~ANADIG_PLL_ENABLE, PLL5_ENET_BASE_ADDR);
+		__raw_writel(__raw_readl(PLL1_SYS_BASE_ADDR) &
+			~ANADIG_PLL_ENABLE, PLL1_SYS_BASE_ADDR);
+		break;
+	case STOP_MODE:
+	case LOW_POWER_STOP:
+		gpc_set_wakeup();
+
+		/* unmask UART1 in low-power mode */
+		__raw_writel(0xfffeffff, MXC_CCM_CCGR0);
+		/* unmask WKUP and GPC in low-power mode */
+		__raw_writel(0xfeefffff, MXC_CCM_CCGR4);
+		/* unmask DDRC in low-power mode */
+		__raw_writel(0xefffffff, MXC_CCM_CCGR6);
+
+		ccm_ccr |= MXC_CCM_CCR_FIRC_EN;
+		__raw_writel(ccm_ccr, MXC_CCM_CCR);
+		ccm_ccsr &= ~MXC_CCM_CCSR_FAST_CLK_SEL_MASK;
+		ccm_ccsr &= ~MXC_CCM_CCSR_SLOW_CLK_SEL_MASK;
+		__raw_writel(ccm_ccsr, MXC_CCM_CCSR);
+
+		/* switch system clock to FIRC 24MHz */
+		ccm_ccsr &= ~MXC_CCM_CCSR_SYS_CLK_SEL_MASK;
+		__raw_writel(ccm_ccsr, MXC_CCM_CCSR);
+
+		/* on-chip oscillator will not be
+		 * powered down in stop mode */
+		ccm_clpcr = __raw_readl(MXC_CCM_CLPCR);
+		ccm_clpcr &= ~MXC_CCM_CLPCR_SBYOS;
+		ccm_clpcr &= ~MXC_CCM_CLPCR_M_CORE1_WFI;
+		ccm_clpcr &= ~MXC_CCM_CLPCR_M_CORE0_WFI;
+		ccm_clpcr &= ~MXC_CCM_CLPCR_ARM_CLK_LPM;
+		__raw_writel(ccm_clpcr, MXC_CCM_CLPCR);
+
+		/* mask stop mode to Anatop */
+		__raw_writel(ccm_clpcr & ~MXC_CCM_CLPCR_ANATOP_STOP_MODE,
+			MXC_CCM_CLPCR);
+
+		break;
+	default:
+		printk(KERN_WARNING "UNKNOW cpu power mode: %d\n", mode);
+		return;
+	}
+}
 
 void arch_idle(void)
 {