ENGR00122161 Add imx23 low power mode

Add imx23 low power mode

Signed-off-by: Zhou Jingyu <Jingyu.Zhou@freescale.com>
Signed-off-by: Alejandro Gonzalez <alex.gonzalez@digi.com>

6 files changed, 1386 insertions, 1 deletions

diff --git a/arch/arm/mach-mx23/Makefile b/arch/arm/mach-mx23/Makefile
index 83bafa10ffa5..72c252d452ff 100644
--- a/arch/arm/mach-mx23/Makefile
+++ b/arch/arm/mach-mx23/Makefile
@@ -1,7 +1,7 @@
 #
 # Makefile for the linux kernel.
 #
-obj-y += pinctrl.o clock.o device.o serial.o power.o
+obj-y += pinctrl.o clock.o device.o serial.o power.o pm.o sleep.o
 
 # Board select
 obj-$(CONFIG_MACH_MX23EVK) += mx23evk.o mx23evk_pins.o
diff --git a/arch/arm/mach-mx23/clock.c b/arch/arm/mach-mx23/clock.c
index 97b005a5f9f0..da3d4556e45b 100644
--- a/arch/arm/mach-mx23/clock.c
+++ b/arch/arm/mach-mx23/clock.c
@@ -359,6 +359,68 @@ static struct clk lcdif_clk = {
 	.set_parent	= lcdif_set_parent,
 };
 
+static struct clk cpu_clk, h_clk;
+static int clkseq_set_parent(struct clk *clk, struct clk *parent)
+{
+	int ret = -EINVAL;
+	int shift = 8;
+
+	/* bypass? */
+	if (parent == &ref_xtal_clk)
+		shift = 4;
+
+	if (clk->bypass_reg) {
+		u32 hbus_val, cpu_val;
+
+		if (clk == &cpu_clk && shift == 4) {
+			hbus_val = __raw_readl(CLKCTRL_BASE_ADDR +
+					HW_CLKCTRL_HBUS);
+			cpu_val = __raw_readl(CLKCTRL_BASE_ADDR +
+					HW_CLKCTRL_CPU);
+
+			hbus_val &= ~(BM_CLKCTRL_HBUS_DIV_FRAC_EN |
+				      BM_CLKCTRL_HBUS_DIV);
+			hbus_val |= 1;
+
+			cpu_val &= ~BM_CLKCTRL_CPU_DIV_CPU;
+			cpu_val |= 1;
+
+			__raw_writel(1 << clk->bypass_bits,
+					clk->bypass_reg + shift);
+
+			__raw_writel(hbus_val,
+					CLKCTRL_BASE_ADDR + HW_CLKCTRL_HBUS);
+			__raw_writel(cpu_val,
+					CLKCTRL_BASE_ADDR + HW_CLKCTRL_CPU);
+			/* h_clk.rate = 0; */
+		} else if (clk == &cpu_clk && shift == 8) {
+			hbus_val = __raw_readl(CLKCTRL_BASE_ADDR +
+							HW_CLKCTRL_HBUS);
+			cpu_val = __raw_readl(CLKCTRL_BASE_ADDR +
+							HW_CLKCTRL_CPU);
+			hbus_val &= ~(BM_CLKCTRL_HBUS_DIV_FRAC_EN |
+				      BM_CLKCTRL_HBUS_DIV);
+			hbus_val |= 2;
+			cpu_val &= ~BM_CLKCTRL_CPU_DIV_CPU;
+			cpu_val |= 2;
+
+			__raw_writel(hbus_val,
+				CLKCTRL_BASE_ADDR + HW_CLKCTRL_HBUS);
+			__raw_writel(cpu_val,
+				CLKCTRL_BASE_ADDR + HW_CLKCTRL_CPU);
+			/*	h_clk.rate = 0; */
+
+			__raw_writel(1 << clk->bypass_bits,
+					clk->bypass_reg + shift);
+		} else
+			__raw_writel(1 << clk->bypass_bits,
+					clk->bypass_reg + shift);
+		ret = 0;
+	}
+
+	return ret;
+}
+
 static unsigned long cpu_get_rate(struct clk *clk)
 {
 	unsigned long reg;
@@ -373,7 +435,15 @@ static unsigned long cpu_get_rate(struct clk *clk)
 
 static struct clk cpu_clk = {
 	.parent = &ref_cpu_clk,
+	.scale_reg	= CLKCTRL_BASE_ADDR + HW_CLKCTRL_FRAC,
+	.scale_bits	= 0,
+	.bypass_reg	= CLKCTRL_BASE_ADDR + HW_CLKCTRL_CLKSEQ,
+	.bypass_bits	= 7,
+	.busy_reg	= CLKCTRL_BASE_ADDR + HW_CLKCTRL_CPU,
+	.busy_bits	= 28,
+/*	.flags		= RATE_PROPAGATES | ENABLED,*/
 	.get_rate = cpu_get_rate,
+	.set_parent = clkseq_set_parent,
 };
 
 static unsigned long uart_get_rate(struct clk *clk)
diff --git a/arch/arm/mach-mx23/include/mach/mx23.h b/arch/arm/mach-mx23/include/mach/mx23.h
index ae7f43c0744f..f5df35f98f4c 100644
--- a/arch/arm/mach-mx23/include/mach/mx23.h
+++ b/arch/arm/mach-mx23/include/mach/mx23.h
@@ -88,4 +88,5 @@
 #define MXS_LL_UART_PADDR DUART_PHYS_ADDR
 #define MXS_LL_UART_VADDR MX23_SOC_IO_ADDRESS(DUART_PHYS_ADDR)
 #endif
+
 #endif /* __ASM_ARCH_MACH_MX23_H__ */
diff --git a/arch/arm/mach-mx23/pm.c b/arch/arm/mach-mx23/pm.c
new file mode 100644
index 000000000000..e66984162da7
--- /dev/null
+++ b/arch/arm/mach-mx23/pm.c
@@ -0,0 +1,638 @@
+/*
+ * Copyright (C) 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * 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.
+ */
+
+
+#include <linux/suspend.h>
+#include <linux/rtc.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kthread.h>
+
+#include <asm/cacheflush.h>
+#include <asm/mach-types.h>
+
+#include <asm/mach/time.h>
+
+#include <mach/hardware.h>
+#include <mach/dma.h>
+#include <mach/regs-rtc.h>
+#include "regs-clkctrl.h"
+#include "regs-pinctrl.h"
+#include <mach/regs-power.h>
+#include <mach/regs-pwm.h>
+#include <mach/regs-rtc.h>
+#include <mach/../../regs-icoll.h>
+#include "regs-dram.h"
+
+#include "sleep.h"
+
+#define PENDING_IRQ_RETRY 100
+static void *saved_sram;
+static int saved_sleep_state;
+
+#define WAIT_DC_OK_CYCLES 24000
+#define WAIT_CYCLE(n) for (i = 0; i < n; i++);
+#define LOWER_VDDIO 10
+#define LOWER_VDDA 9
+#define LOWER_VDDD 0xa
+#define MAX_POWEROFF_CODE_SIZE (6 * 1024)
+#define REGS_CLKCTRL_BASE IO_ADDRESS(CLKCTRL_PHYS_ADDR)
+
+static void mx23_standby(void)
+{
+	int i;
+	u32 reg_vddd, reg_vdda, reg_vddio;
+	/* DDR EnterSelfrefreshMode */
+	__raw_writel(
+	BM_DRAM_CTL08_SREFRESH | __raw_readl(IO_ADDRESS(DRAM_PHYS_ADDR)
+		+ HW_DRAM_CTL08),
+	IO_ADDRESS(DRAM_PHYS_ADDR) + HW_DRAM_CTL08);
+
+	/* Gating EMI CLock */
+	__raw_writel(BM_CLKCTRL_EMI_CLKGATE |
+			__raw_readl(REGS_CLKCTRL_BASE + HW_CLKCTRL_EMI),
+			REGS_CLKCTRL_BASE + HW_CLKCTRL_EMI);
+#if 0
+	/* Disable PLL */
+	__raw_writel(BM_CLKCTRL_PLLCTRL0_POWER,
+		REGS_CLKCTRL_BASE + HW_CLKCTRL_PLLCTRL0_CLR);
+#endif
+
+	/* Reduce the VDDIO (3.050 volt) */
+	reg_vddio = __raw_readl(REGS_POWER_BASE + HW_POWER_VDDIOCTRL);
+	__raw_writel(reg_vddio | BM_POWER_VDDIOCTRL_BO_OFFSET,
+		REGS_POWER_BASE + HW_POWER_VDDIOCTRL);
+	__raw_writel((__raw_readl(REGS_POWER_BASE + HW_POWER_VDDIOCTRL) &
+		~BM_POWER_VDDIOCTRL_TRG) | LOWER_VDDIO,
+		REGS_POWER_BASE + HW_POWER_VDDIOCTRL);
+	WAIT_CYCLE(WAIT_DC_OK_CYCLES)
+
+	while (!(__raw_readl(REGS_POWER_BASE + HW_POWER_STS) &
+		BM_POWER_STS_DC_OK))
+		;
+
+	/* Reduce VDDA 1.725volt */
+	reg_vdda = __raw_readl(REGS_POWER_BASE + HW_POWER_VDDACTRL);
+	__raw_writel(reg_vdda | BM_POWER_VDDACTRL_BO_OFFSET,
+		REGS_POWER_BASE + HW_POWER_VDDACTRL);
+	__raw_writel((__raw_readl(REGS_POWER_BASE + HW_POWER_VDDACTRL) &
+		~BM_POWER_VDDACTRL_TRG) | LOWER_VDDA,
+		REGS_POWER_BASE + HW_POWER_VDDACTRL);
+	WAIT_CYCLE(WAIT_DC_OK_CYCLES)
+
+	/* wait for DC_OK */
+	while (!(__raw_readl(REGS_POWER_BASE + HW_POWER_STS) &
+		BM_POWER_STS_DC_OK))
+		;
+
+	/* Reduce VDDD 1.000 volt */
+	reg_vddd = __raw_readl(REGS_POWER_BASE + HW_POWER_VDDDCTRL);
+	__raw_writel(reg_vddd | BM_POWER_VDDDCTRL_BO_OFFSET,
+		REGS_POWER_BASE + HW_POWER_VDDDCTRL);
+	__raw_writel((__raw_readl(REGS_POWER_BASE + HW_POWER_VDDDCTRL) &
+		~BM_POWER_VDDDCTRL_TRG) | LOWER_VDDD,
+		REGS_POWER_BASE + HW_POWER_VDDDCTRL);
+	WAIT_CYCLE(WAIT_DC_OK_CYCLES)
+
+	while (!(__raw_readl(REGS_POWER_BASE + HW_POWER_STS) &
+		BM_POWER_STS_DC_OK))
+		;
+
+	/* optimize the DCDC loop gain */
+	__raw_writel((__raw_readl(REGS_POWER_BASE + HW_POWER_LOOPCTRL) &
+		~BM_POWER_LOOPCTRL_EN_RCSCALE),
+		REGS_POWER_BASE + HW_POWER_LOOPCTRL);
+	__raw_writel((__raw_readl(REGS_POWER_BASE + HW_POWER_LOOPCTRL) &
+		~BM_POWER_LOOPCTRL_DC_R) |
+		(2<<BP_POWER_LOOPCTRL_DC_R),
+		REGS_POWER_BASE + HW_POWER_LOOPCTRL);
+
+	/* half the fets */
+	__raw_writel(BM_POWER_MINPWR_HALF_FETS,
+		REGS_POWER_BASE + HW_POWER_MINPWR_SET);
+	__raw_writel(BM_POWER_MINPWR_DOUBLE_FETS,
+		REGS_POWER_BASE + HW_POWER_MINPWR_CLR);
+
+	__raw_writel(BM_POWER_LOOPCTRL_CM_HYST_THRESH,
+			REGS_POWER_BASE + HW_POWER_LOOPCTRL_CLR);
+	__raw_writel(BM_POWER_LOOPCTRL_EN_CM_HYST,
+			REGS_POWER_BASE + HW_POWER_LOOPCTRL_CLR);
+	__raw_writel(BM_POWER_LOOPCTRL_EN_DF_HYST,
+			REGS_POWER_BASE + HW_POWER_LOOPCTRL_CLR);
+
+
+	/* enable PFM */
+	__raw_writel(BM_POWER_LOOPCTRL_HYST_SIGN,
+		REGS_POWER_BASE + HW_POWER_LOOPCTRL_SET);
+	__raw_writel(BM_POWER_MINPWR_EN_DC_PFM,
+		REGS_POWER_BASE + HW_POWER_MINPWR_SET);
+
+	__raw_writel(BM_CLKCTRL_CPU_INTERRUPT_WAIT,
+		REGS_CLKCTRL_BASE + HW_CLKCTRL_CPU_SET);
+	/* Power off ... */
+	asm("mcr     p15, 0, r2, c7, c0, 4");
+	__raw_writel(BM_CLKCTRL_CPU_INTERRUPT_WAIT,
+			REGS_CLKCTRL_BASE + HW_CLKCTRL_CPU_CLR);
+
+	/* restore the DCDC parameter */
+
+	__raw_writel(BM_POWER_MINPWR_EN_DC_PFM,
+			REGS_POWER_BASE + HW_POWER_MINPWR_CLR);
+	__raw_writel(BM_POWER_LOOPCTRL_HYST_SIGN,
+			REGS_POWER_BASE + HW_POWER_LOOPCTRL_CLR);
+	__raw_writel(BM_POWER_LOOPCTRL_EN_DF_HYST,
+			REGS_POWER_BASE + HW_POWER_LOOPCTRL_SET);
+	__raw_writel(BM_POWER_LOOPCTRL_EN_CM_HYST,
+			REGS_POWER_BASE + HW_POWER_LOOPCTRL_SET);
+	__raw_writel(BM_POWER_LOOPCTRL_CM_HYST_THRESH,
+			REGS_POWER_BASE + HW_POWER_LOOPCTRL_SET);
+
+	__raw_writel((__raw_readl(REGS_POWER_BASE + HW_POWER_LOOPCTRL) &
+		~BM_POWER_LOOPCTRL_DC_R) |
+		(2<<BP_POWER_LOOPCTRL_DC_R),
+		REGS_POWER_BASE + HW_POWER_LOOPCTRL);
+	__raw_writel((__raw_readl(REGS_POWER_BASE + HW_POWER_LOOPCTRL) &
+		~BM_POWER_LOOPCTRL_EN_RCSCALE) |
+		(3 << BP_POWER_LOOPCTRL_EN_RCSCALE),
+		REGS_POWER_BASE + HW_POWER_LOOPCTRL);
+
+	/* double the fets */
+	__raw_writel(BM_POWER_MINPWR_HALF_FETS,
+		REGS_POWER_BASE + HW_POWER_MINPWR_CLR);
+	__raw_writel(BM_POWER_MINPWR_DOUBLE_FETS,
+		REGS_POWER_BASE + HW_POWER_MINPWR_SET);
+
+
+	/* Restore VDDD */
+	__raw_writel(reg_vddd, REGS_POWER_BASE + HW_POWER_VDDDCTRL);
+
+	WAIT_CYCLE(WAIT_DC_OK_CYCLES)
+	while (!(__raw_readl(REGS_POWER_BASE + HW_POWER_STS) &
+		BM_POWER_STS_DC_OK))
+		;
+
+	__raw_writel(reg_vdda, REGS_POWER_BASE + HW_POWER_VDDACTRL);
+	WAIT_CYCLE(WAIT_DC_OK_CYCLES)
+	while (!(__raw_readl(REGS_POWER_BASE + HW_POWER_STS) &
+		BM_POWER_STS_DC_OK))
+		;
+
+	__raw_writel(reg_vddio, REGS_POWER_BASE + HW_POWER_VDDIOCTRL);
+	WAIT_CYCLE(WAIT_DC_OK_CYCLES)
+	while (!(__raw_readl(REGS_POWER_BASE + HW_POWER_STS) &
+		BM_POWER_STS_DC_OK))
+		;
+
+
+	/* Enable PLL */
+	__raw_writel(BM_CLKCTRL_PLLCTRL0_POWER,
+		REGS_CLKCTRL_BASE + HW_CLKCTRL_PLLCTRL0_SET);
+	/* Ungating EMI CLock */
+	__raw_writel(~BM_CLKCTRL_EMI_CLKGATE &
+			__raw_readl(REGS_CLKCTRL_BASE + HW_CLKCTRL_EMI),
+			REGS_CLKCTRL_BASE + HW_CLKCTRL_EMI);
+
+	/* LeaveSelfrefreshMode */
+	__raw_writel(
+		(~BM_DRAM_CTL08_SREFRESH) &
+			__raw_readl(IO_ADDRESS(DRAM_PHYS_ADDR)
+			+ HW_DRAM_CTL08),
+		IO_ADDRESS(DRAM_PHYS_ADDR) + HW_DRAM_CTL08);
+}
+
+static inline void do_standby(void)
+{
+	void (*mx23_cpu_standby_ptr) (void);
+	struct clk *cpu_clk;
+	struct clk *osc_clk;
+	struct clk *pll_clk;
+	struct clk *hbus_clk;
+	struct clk *cpu_parent = NULL;
+	int cpu_rate = 0;
+	int hbus_rate = 0;
+	u32 reg_clkctrl_clkseq, reg_clkctrl_xtal;
+	unsigned long iram_phy_addr;
+	void *iram_virtual_addr;
+
+	/*
+	 * 1) switch clock domains from PLL to 24MHz
+	 * 2) lower voltage (TODO)
+	 * 3) switch EMI to 24MHz and turn PLL off (done in sleep.S)
+	 */
+
+
+	/* make sure SRAM copy gets physically written into SDRAM.
+	 * SDRAM will be placed into self-refresh during power down
+	 */
+	flush_cache_all();
+	iram_virtual_addr = iram_alloc(MAX_POWEROFF_CODE_SIZE, &iram_phy_addr);
+	if (iram_virtual_addr == NULL) {
+		pr_info("can not get iram for suspend\n");
+		return;
+	}
+
+	/* copy suspend function into SRAM */
+	memcpy(iram_virtual_addr, mx23_standby,
+			MAX_POWEROFF_CODE_SIZE);
+
+	/* now switch the CPU to ref_xtal */
+	cpu_clk = clk_get(NULL, "cpu");
+	osc_clk = clk_get(NULL, "ref_xtal");
+	pll_clk = clk_get(NULL, "pll.0");
+	hbus_clk = clk_get(NULL, "h");
+
+	if (!IS_ERR(cpu_clk) && !IS_ERR(osc_clk)) {
+		cpu_rate = clk_get_rate(cpu_clk);
+		cpu_parent = clk_get_parent(cpu_clk);
+		hbus_rate = clk_get_rate(hbus_clk);
+		clk_set_parent(cpu_clk, osc_clk);
+	}
+
+	local_fiq_disable();
+
+	__raw_writel(BM_POWER_CTRL_ENIRQ_PSWITCH,
+		REGS_POWER_BASE + HW_POWER_CTRL_SET);
+
+	reg_clkctrl_clkseq = __raw_readl(REGS_CLKCTRL_BASE +
+		HW_CLKCTRL_CLKSEQ);
+
+	__raw_writel(BM_CLKCTRL_CLKSEQ_BYPASS_ETM |
+		BM_CLKCTRL_CLKSEQ_BYPASS_SSP |
+		BM_CLKCTRL_CLKSEQ_BYPASS_GPMI |
+		BM_CLKCTRL_CLKSEQ_BYPASS_IR |
+		BM_CLKCTRL_CLKSEQ_BYPASS_PIX|
+		BM_CLKCTRL_CLKSEQ_BYPASS_SAIF,
+		REGS_CLKCTRL_BASE + HW_CLKCTRL_CLKSEQ_SET);
+
+	reg_clkctrl_xtal = __raw_readl(REGS_CLKCTRL_BASE + HW_CLKCTRL_XTAL);
+
+	__raw_writel(reg_clkctrl_xtal | BM_CLKCTRL_XTAL_FILT_CLK24M_GATE |
+		BM_CLKCTRL_XTAL_PWM_CLK24M_GATE |
+		BM_CLKCTRL_XTAL_DRI_CLK24M_GATE,
+		REGS_CLKCTRL_BASE + HW_CLKCTRL_XTAL);
+
+	/* do suspend */
+	mx23_cpu_standby_ptr = iram_virtual_addr;
+	mx23_cpu_standby_ptr();
+
+	__raw_writel(reg_clkctrl_clkseq, REGS_CLKCTRL_BASE + HW_CLKCTRL_CLKSEQ);
+	__raw_writel(reg_clkctrl_xtal, REGS_CLKCTRL_BASE + HW_CLKCTRL_XTAL);
+
+	saved_sleep_state = 0;  /* waking from standby */
+	__raw_writel(BM_POWER_CTRL_PSWITCH_IRQ,
+		REGS_POWER_BASE + HW_POWER_CTRL_CLR);
+	mxs_nomatch_resume_timer();
+
+	local_fiq_enable();
+
+	if (cpu_parent) {
+		clk_set_parent(cpu_clk, cpu_parent);
+		clk_set_rate(cpu_clk, cpu_rate);
+		clk_set_rate(hbus_clk, hbus_rate);
+	}
+
+	clk_put(hbus_clk);
+	clk_put(pll_clk);
+	clk_put(osc_clk);
+	clk_put(cpu_clk);
+
+	iram_free(iram_phy_addr, MAX_POWEROFF_CODE_SIZE);
+}
+
+static u32 clk_regs[] = {
+		HW_CLKCTRL_PLLCTRL0,
+		HW_CLKCTRL_XTAL,
+		HW_CLKCTRL_PIX,
+		HW_CLKCTRL_SSP,
+		HW_CLKCTRL_GPMI,
+		HW_CLKCTRL_FRAC,
+		HW_CLKCTRL_CLKSEQ,
+};
+
+static noinline void do_mem(void)
+{
+	unsigned long iram_phy_addr;
+	void *iram_virtual_addr;
+	void (*mx23_cpu_suspend_ptr) (u32);
+	struct sleep_data saved_context;
+	int i;
+	struct clk *cpu_clk;
+	struct clk *osc_clk;
+	struct clk *pll_clk;
+	struct clk *hbus_clk;
+	int cpu_rate = 0;
+	int hbus_rate = 0;
+
+	saved_context.fingerprint = SLEEP_DATA_FINGERPRINT;
+
+	saved_context.old_c00 = __raw_readl(0xC0000000);
+	saved_context.old_c04 = __raw_readl(0xC0000004);
+	__raw_writel((u32)&saved_context, (void *)0xC0000000);
+
+	iram_virtual_addr = iram_alloc(MAX_POWEROFF_CODE_SIZE, &iram_phy_addr);
+	if (iram_virtual_addr == NULL) {
+		pr_info("can not get iram for suspend\n");
+		return;
+	}
+
+	local_irq_disable();
+	local_fiq_disable();
+
+	/* mxs_dma_suspend(); */
+	mxs_nomatch_suspend_timer();
+
+	/* clocks */
+	for (i = 0; i < ARRAY_SIZE(clk_regs); i++)
+		saved_context.clks[i] =
+				__raw_readl(clk_regs[i]);
+
+	/* interrupt collector */
+	/*
+	saved_context.icoll_ctrl =
+		__raw_readl(REGS_ICOLL_BASE + HW_ICOLL_CTRL);
+	if (machine_is_mx23()) {
+#ifdef CONFIG_MACH_MX23
+		for (i = 0; i < 16; i++)
+			saved_context.icoll.prio[i] =
+			__raw_readl(REGS_ICOLL_BASE + HW_ICOLL_PRIORITYn(i));
+#endif
+	} else if (machine_is_mx23()) {
+#ifdef CONFIG_MACH_STMP378X
+		for (i = 0; i < 128; i++)
+			saved_context.icoll.intr[i] =
+			__raw_readl(REGS_ICOLL_BASE + HW_ICOLL_INTERRUPTn(i));
+#endif
+	}
+	*/
+
+	/* save pinmux state */
+	for (i = 0; i < 0x100; i++)
+		saved_context.pinmux[i] =
+			__raw_readl(IO_ADDRESS(PINCTRL_PHYS_ADDR) + (i<<4));
+
+	cpu_clk = clk_get(NULL, "cpu");
+	osc_clk = clk_get(NULL, "osc_24M");
+	pll_clk = clk_get(NULL, "pll");
+	hbus_clk = clk_get(NULL, "hclk");
+
+	cpu_rate = clk_get_rate(cpu_clk);
+	hbus_rate = clk_get_rate(hbus_clk);
+
+
+	/* set the PERSISTENT_SLEEP_BIT for bootloader */
+	__raw_writel(1 << 10,
+		IO_ADDRESS(RTC_PHYS_ADDR) + HW_RTC_PERSISTENT1_SET);
+	/* XXX: temp */
+
+	/*
+	 * make sure SRAM copy gets physically written into SDRAM.
+	 * SDRAM will be placed into self-refresh during power down
+	 */
+	flush_cache_all();
+
+	/*copy suspend function into SRAM */
+	memcpy(iram_virtual_addr, mx23_cpu_suspend,
+		MAX_POWEROFF_CODE_SIZE);
+
+	/* do suspend */
+	mx23_cpu_suspend_ptr = (void *)MX23_OCRAM_BASE;
+	mx23_cpu_suspend_ptr(0);
+
+	saved_sleep_state = 1;	/* waking from non-standby state */
+
+
+	/* clocks */
+	for (i = 0; i < ARRAY_SIZE(clk_regs); i++)
+		__raw_writel(saved_context.clks[i],
+			clk_regs[i]);
+
+	/* interrupt collector */
+/*
+	__raw_writel(saved_context.icoll_ctrl, REGS_ICOLL_BASE + HW_ICOLL_CTRL);
+	if (machine_is_mx23()) {
+#ifdef CONFIG_MACH_MX23
+		for (i = 0; i < 16; i++)
+			__raw_writel(saved_context.icoll.prio[i],
+			REGS_ICOLL_BASE + HW_ICOLL_PRIORITYn(i));
+#endif
+	} else if (machine_is_mx23()) {
+#ifdef CONFIG_MACH_STMP378X
+		for (i = 0; i < 128; i++)
+			__raw_writel(saved_context.icoll.intr[i],
+			REGS_ICOLL_BASE + HW_ICOLL_INTERRUPTn(i));
+#endif
+	}
+*/
+
+	/* restore pinmux state */
+	for (i = 0; i < 0x100; i++)
+		__raw_writel(saved_context.pinmux[i],
+				IO_ADDRESS(PINCTRL_PHYS_ADDR) + (i<<4));
+
+	clk_set_rate(cpu_clk, cpu_rate);
+	clk_set_rate(hbus_clk, hbus_rate);
+
+	__raw_writel(saved_context.old_c00, 0xC0000000);
+	__raw_writel(saved_context.old_c04, 0xC0000004);
+
+	clk_put(hbus_clk);
+	clk_put(pll_clk);
+	clk_put(osc_clk);
+	clk_put(cpu_clk);
+
+	mxs_nomatch_resume_timer();
+	/* mxs_dma_resume(); */
+
+	iram_free(iram_phy_addr, MAX_POWEROFF_CODE_SIZE);
+	local_fiq_enable();
+	local_irq_enable();
+}
+
+static int mx23_pm_enter(suspend_state_t state)
+{
+	switch (state) {
+	case PM_SUSPEND_STANDBY:
+		do_standby();
+		break;
+	case PM_SUSPEND_MEM:
+		do_mem();
+		break;
+	}
+	return 0;
+}
+
+static int mx23_pm_valid(suspend_state_t state)
+{
+	return (state == PM_SUSPEND_STANDBY) ||
+	       (state == PM_SUSPEND_MEM);
+}
+
+static suspend_state_t saved_state;
+
+static int mx23_pm_begin(suspend_state_t state)
+{
+	mxs_nomatch_suspend_timer();
+	saved_state = state;
+	return 0;
+}
+
+static void mx23_pm_end(void)
+{
+	/*XXX: Nothing to do */
+}
+
+suspend_state_t mx23_pm_get_target(void)
+{
+	return saved_state;
+}
+EXPORT_SYMBOL(mx23_pm_get_target);
+
+/**
+ * mx23_pm_get_sleep_state - get sleep state we waking from
+ *
+ * returns boolean: 0 if waking up from standby, 1 otherwise
+ */
+int mx23_pm_sleep_was_deep(void)
+{
+	return saved_sleep_state;
+}
+EXPORT_SYMBOL(mx23_pm_sleep_was_deep);
+
+static struct platform_suspend_ops mx23_suspend_ops = {
+	.enter	= mx23_pm_enter,
+	.valid	= mx23_pm_valid,
+	.begin	= mx23_pm_begin,
+	.end	= mx23_pm_end,
+};
+
+void mx23_pm_idle(void)
+{
+	local_irq_disable();
+	local_fiq_disable();
+	if (need_resched()) {
+		local_fiq_enable();
+		local_irq_enable();
+		return;
+	}
+
+	__raw_writel(1<<12, REGS_CLKCTRL_BASE + HW_CLKCTRL_CPU_SET);
+	__asm__ __volatile__ ("mcr	p15, 0, r0, c7, c0, 4");
+
+	local_fiq_enable();
+	local_irq_enable();
+}
+
+static void mx23_pm_power_off(void)
+{
+	__raw_writel((0x3e77 << 16) | 1, REGS_POWER_BASE + HW_POWER_RESET);
+}
+
+struct mx23_pswitch_state {
+	int dev_running;
+};
+
+static DECLARE_COMPLETION(suspend_request);
+
+static int suspend_thread_fn(void *data)
+{
+	while (1) {
+		wait_for_completion_interruptible(&suspend_request);
+		pm_suspend(PM_SUSPEND_STANDBY);
+	}
+	return 0;
+}
+
+static struct mx23_pswitch_state pswitch_state = {
+	.dev_running = 0,
+};
+
+static irqreturn_t pswitch_interrupt(int irq, void *dev)
+{
+	int pin_value, i;
+
+	/* check if irq by pswitch */
+	if (!(__raw_readl(REGS_POWER_BASE + HW_POWER_CTRL) &
+		BM_POWER_CTRL_PSWITCH_IRQ))
+		return IRQ_HANDLED;
+	for (i = 0; i < 3000; i++) {
+		pin_value = __raw_readl(REGS_POWER_BASE + HW_POWER_STS) &
+			BF_POWER_STS_PSWITCH(0x1);
+		if (pin_value == 0)
+			break;
+		mdelay(1);
+	}
+	if (i < 3000) {
+		pr_info("pswitch goto suspend\n");
+		complete(&suspend_request);
+	} else {
+		pr_info("release pswitch to power down\n");
+		for (i = 0; i < 5000; i++) {
+			pin_value = __raw_readl(REGS_POWER_BASE + HW_POWER_STS)
+				& BF_POWER_STS_PSWITCH(0x1);
+			if (pin_value == 0)
+				break;
+			mdelay(1);
+		}
+		pr_info("pswitch power down\n");
+		mx23_pm_power_off();
+	}
+	__raw_writel(BM_POWER_CTRL_PSWITCH_IRQ,
+		REGS_POWER_BASE + HW_POWER_CTRL_CLR);
+	return IRQ_HANDLED;
+}
+
+static struct irqaction pswitch_irq = {
+	.name		= "pswitch",
+	.flags		= IRQF_DISABLED | IRQF_SHARED,
+	.handler	= pswitch_interrupt,
+	.dev_id		= &pswitch_state,
+};
+
+static void init_pswitch(void)
+{
+	kthread_run(suspend_thread_fn, NULL, "pswitch");
+	__raw_writel(BM_POWER_CTRL_PSWITCH_IRQ,
+		REGS_POWER_BASE + HW_POWER_CTRL_CLR);
+	__raw_writel(BM_POWER_CTRL_POLARITY_PSWITCH |
+		BM_POWER_CTRL_ENIRQ_PSWITCH,
+		REGS_POWER_BASE + HW_POWER_CTRL_SET);
+	__raw_writel(BM_POWER_CTRL_PSWITCH_IRQ,
+		REGS_POWER_BASE + HW_POWER_CTRL_CLR);
+	setup_irq(IRQ_VDD5V, &pswitch_irq);
+}
+
+static int __init mx23_pm_init(void)
+{
+	saved_sram = kmalloc(0x4000, GFP_ATOMIC);
+	if (!saved_sram) {
+		printk(KERN_ERR
+		 "PM Suspend: can't allocate memory to save portion of SRAM\n");
+		return -ENOMEM;
+	}
+
+	pm_power_off = mx23_pm_power_off;
+	pm_idle = mx23_pm_idle;
+	suspend_set_ops(&mx23_suspend_ops);
+	init_pswitch();
+	return 0;
+}
+
+late_initcall(mx23_pm_init);
diff --git a/arch/arm/mach-mx23/sleep.S b/arch/arm/mach-mx23/sleep.S
new file mode 100644
index 000000000000..9cc9c9d01908
--- /dev/null
+++ b/arch/arm/mach-mx23/sleep.S
@@ -0,0 +1,552 @@
+/*
+ * Copyright (C) 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * 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.
+ */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+#include <mach/hardware.h>
+#include <asm/system.h>
+#include <asm/pgtable-hwdef.h>
+#include <mach/hardware.h>
+#include <mach/regs-power.h>
+#include "regs-clkctrl.h"
+#include "sleep.h"
+
+#define HW_CLKCTRL_CPU_ADDR \
+	(MX23_SOC_IO_ADDRESS(CLKCTRL_PHYS_ADDR) + HW_CLKCTRL_CPU)
+#define HW_POWER_MINPWR_ADDR \
+	(MX23_SOC_IO_ADDRESS(POWER_PHYS_ADDR) + HW_POWER_MINPWR)
+#define HW_POWER_RESET_ADDR \
+	(MX23_SOC_IO_ADDRESS(POWER_PHYS_ADDR) + HW_POWER_RESET)
+
+#define HW_DRAM_CTL06	MX23_SOC_IO_ADDRESS(0x800E0018)
+#define HW_DRAM_CTL08	MX23_SOC_IO_ADDRESS(0x800E0020)
+#define HW_EMI_STAT	MX23_SOC_IO_ADDRESS(0x80020010)
+#define HW_RTC_PERSISTENT0 \
+			MX23_SOC_IO_ADDRESS(0x8005C060)
+
+#define PHYS_RAM_START		0x40000000
+
+.global cpu_arm926_switch_mm
+
+		.text
+
+.align 8
+ENTRY(mx23_cpu_standby)
+	@ save registers on stack
+	stmfd	sp!, {r0 - r9, lr}
+
+	adr	r9, __mx23_temp_stack
+
+	@ clean cache
+	ldr	r1, __mx23_flush_cache_addr
+	mov	lr, pc
+	mov	pc, r1
+
+	@ put DRAM into self refresh
+	mov	r0, #(HW_DRAM_CTL08 & 0x000000FF)
+	orr	r0, r0, #(HW_DRAM_CTL08 & 0x0000FF00)
+	orr	r0, r0, #(HW_DRAM_CTL08 & 0x00FF0000)
+	orr	r0, r0, #(HW_DRAM_CTL08 & 0xFF000000)
+	ldr	r1, [r0]
+	orr	r1, r1, #(1 << 8)
+	str	r1, [r0]
+	@ wait for it to actually happen
+	mov	r0, #(HW_EMI_STAT & 0x000000FF)
+	orr	r0, r0, #(HW_EMI_STAT & 0x0000FF00)
+	orr	r0, r0, #(HW_EMI_STAT & 0x00FF0000)
+	orr	r0, r0, #(HW_EMI_STAT & 0xFF000000)
+1:	ldr	r1, [r0]
+	teq	r1, #(1 << 1)
+	beq	1b
+	nop
+	nop
+	nop
+	nop
+	nop
+
+#ifdef CONFIG_STMP378X_RAM_FREQ_SCALING
+	@ RAM to clk from xtal
+	mov	lr, pc
+	b	mx23_ram_save_timings
+	mov	lr, pc
+	b	mx23_ram_24M_set_timings
+
+	mov	r0, #(HW_CLKCTRL_CLKSEQ_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_CLKCTRL_CLKSEQ_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_CLKCTRL_CLKSEQ_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_CLKCTRL_CLKSEQ_ADDR & 0xFF000000)
+	ldr	r4, [r0]
+	mov	r1, #(1<<6)
+	str	r1, [r0, #4]
+1:	ldr	r1, [r0]
+	tst	r1, #BM_CLKCTRL_EMI_BUSY_REF_XTAL
+	bne	1b
+
+	@ save RAM divisors
+	mov	r0, #(HW_CLKCTRL_FRAC_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_CLKCTRL_FRAC_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_CLKCTRL_FRAC_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_CLKCTRL_FRAC_ADDR & 0xFF000000)
+	ldr	r8, [r0]
+	and	r8, r8, #(0x3F << 8)
+	lsr	r8, r8, #8
+	mov	r0, #(HW_CLKCTRL_EMI_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_CLKCTRL_EMI_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_CLKCTRL_EMI_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_CLKCTRL_EMI_ADDR & 0xFF000000)
+	ldr	r7, [r0]
+	and	r7, r7, #0x3F
+
+	@ shut the PLL down
+	mov	r0, #(HW_CLKCTRL_PLLCTRL0_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_CLKCTRL_PLLCTRL0_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_CLKCTRL_PLLCTRL0_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_CLKCTRL_PLLCTRL0_ADDR & 0xFF000000)
+	mov	r1, #(1<<16)
+	str	r1, [r0, #0x08]	@ clear
+
+	@ set vddd to minimum
+	mov	r0, #(HW_POWER_VDDDCTRL_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_POWER_VDDDCTRL_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_POWER_VDDDCTRL_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_POWER_VDDDCTRL_ADDR & 0xFF000000)
+	ldr	r6, [r0]
+	bic	r1, r6, #0xFF
+	bic	r1, r1, #0x30
+	orr	r1, r1, #0xa
+	str	r1, [r0]
+	/* now wait 1000 us = 24000 cycles */
+	mov	r0, #24 << 10
+3:	sub	r0, r0, #1
+	cmp	r0, #0
+	bne	3b
+	nop
+#endif
+
+	@ do enter standby
+	mov	r0, #(HW_CLKCTRL_CPU_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_CLKCTRL_CPU_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_CLKCTRL_CPU_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_CLKCTRL_CPU_ADDR & 0xFF000000)
+	mov	r1, #(1<<12)
+	str	r1, [r0, #4]
+	mov	r2, #0
+	mcr	p15, 0, r2, c7, c0, 4
+	nop
+
+	@ sleeping now...
+
+	@ remove INTERRUPT_WAIT bit
+	str	r1, [r0, #8]
+	nop
+	nop
+	nop
+
+#ifdef CONFIG_STMP378X_RAM_FREQ_SCALING
+	@ restore vddd
+	mov	r0, #(HW_POWER_VDDDCTRL_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_POWER_VDDDCTRL_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_POWER_VDDDCTRL_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_POWER_VDDDCTRL_ADDR & 0xFF000000)
+	ldr	r1, [r0]
+	str	r6, [r0]
+	/* now wait 1000 us = 24000 cycles */
+	mov	r0, #24 << 10
+12:	sub	r0, r0, #1
+	cmp	r0, #0
+	bne	12b
+	nop
+
+	@ put the PLL back up
+	mov	r0, #(HW_CLKCTRL_PLLCTRL0_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_CLKCTRL_PLLCTRL0_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_CLKCTRL_PLLCTRL0_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_CLKCTRL_PLLCTRL0_ADDR & 0xFF000000)
+	mov	r1, #(1<<16)
+	str	r1, [r0, #0x04]	@ set
+	/* now wait 10 us = 240 cycles */
+	mov	r0, #240
+11:	sub	r0, r0, #1
+	cmp	r0, #0
+	bne	11b
+	nop
+
+	@ set divisors and switch EMI back to PLL
+	mov	lr, pc
+	b	mx23_ram_restore_timings
+	mov	lr, pc
+	b	__mx23_emi_set_values
+
+	mov	r0, #(HW_CLKCTRL_CLKSEQ_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_CLKCTRL_CLKSEQ_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_CLKCTRL_CLKSEQ_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_CLKCTRL_CLKSEQ_ADDR & 0xFF000000)
+	mov	r1, #(1<<6)
+	str	r1, [r0, #8]
+
+	mov	r0, #(HW_CLKCTRL_EMI_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_CLKCTRL_EMI_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_CLKCTRL_EMI_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_CLKCTRL_EMI_ADDR & 0xFF000000)
+	ldr	r1, [r0]
+	bic	r1, #BM_CLKCTRL_EMI_DCC_RESYNC_ENABLE
+	str	r1, [r0]
+#endif
+
+	@ restore normal DRAM mode
+	mov	r0, #(HW_DRAM_CTL08 & 0x000000FF)
+	orr	r0, r0, #(HW_DRAM_CTL08 & 0x0000FF00)
+	orr	r0, r0, #(HW_DRAM_CTL08 & 0x00FF0000)
+	orr	r0, r0, #(HW_DRAM_CTL08 & 0xFF000000)
+	ldr	r1, [r0]
+	bic	r1, r1, #(1 << 8)
+	str	r1, [r0]
+	@ wait for it to actually happen
+	mov	r0, #(HW_EMI_STAT & 0x000000FF)
+	orr	r0, r0, #(HW_EMI_STAT & 0x0000FF00)
+	orr	r0, r0, #(HW_EMI_STAT & 0x00FF0000)
+	orr	r0, r0, #(HW_EMI_STAT & 0xFF000000)
+102:	ldr	r1, [r0]
+	tst	r1, #(1 << 1)
+	bne	102b
+
+	nop
+	nop
+	nop
+
+	@ restore regs and return
+	ldmfd   sp!, {r0 - r9, pc}
+
+	.space	0x100
+__mx23_temp_stack:
+	.word	0
+
+#ifdef CONFIG_STMP378X_RAM_FREQ_SCALING
+#include "emi.inc"
+#endif
+
+__mx23_flush_cache_addr:
+	.word	arm926_flush_kern_cache_all
+
+ENTRY(mx23_standby_alloc_sz)
+	.word	. - mx23_cpu_standby
+
+ENTRY(mx23_cpu_suspend)
+	@ save registers on stack
+	stmfd	sp!, {r1 - r12, lr}
+
+	@ save context
+	mov	r0, #0xd3	@ SVC, Interrupts disabled
+	msr	cpsr, r0
+	mov	r1, #0xC0000000
+	ldr	r1, [r1]
+	mrc	p15, 0, r0, c1, c0, 0
+	str	r0, [r1, #MMUCTL_OFFS]
+	mrc	p15, 0, r0, c15, c1, 0
+	str	r0, [r1, #MMUCPACCESS_OFS]
+	mrc	p15, 0, r0, c2, c0, 0
+	str	r0, [r1, #MMUTTB_OFFS]
+	mrc	p15, 0, r0, c3, c0, 0
+	str	r0, [r1, #MMUDOMAIN_OFFS]
+	mrc	p15, 0, r0, c13, c0, 0
+	str	r0, [r1, #MMUPID_OFFS]
+
+	str	sp, [r1, #SVC_SP_OFFS]
+	mrs	r0, spsr
+	str	r0, [r1, #SVC_SPSR_OFFS]
+
+	add	r2, r1, #FIQ_SPSR_OFFS
+	mov	r0, #0xd1	@ FIQ, Interrupts disabled
+	msr	cpsr, r0
+	mrs	r3, spsr
+	stmia	r2!, {r3, r8-r12, sp, lr}
+
+	add	r2, r1, #ABT_SPSR_OFFS
+	mov	r0, #0xd7	@ ABT, Interrupts disabled
+	msr	cpsr, r0
+	mrs	r3, spsr
+	stmia	r2!, {r3, sp, lr}
+
+	add	r2, r1, #IRQ_SPSR_OFFS
+	mov	r0, #0xd2	@ IRQ, Interrupts disabled
+	msr	cpsr, r0
+	mrs	r3, spsr
+	stmia	r2!, {r3, sp, lr}
+
+	add	r2, r1, #UND_SPSR_OFFS
+	mov	r0, #0xdb	@ UND, Interrupts disabled
+	msr	cpsr, r0
+	mrs	r3, spsr
+	stmia	r2!, {r3, sp, lr}
+
+	add	r2, r1, #SYS_SP_OFFS
+	mov	r0, #0xdf	@ SYS, Interrupts disabled
+	msr	cpsr, r0
+	stmia	r2!, {sp, lr}
+
+	add	r2, r1, #SVC_R8_OFFS
+	mov	r0, #0xd3	@ Back to SVC, Interrupts disabled
+	msr	cpsr, r0
+
+	@ save entry point
+	sub	r1, r1, #(0xC0000000 - PHYS_RAM_START)
+	mov	r0, #0xC0000000
+	str	r1, [r0]
+	ldr	r1, __mx23_resume_point
+	sub	r1, r1, #(0xC0000000 - PHYS_RAM_START)
+	str	r1, [r0, #4]
+	mov	r0, #0
+
+	@ clean cache
+	ldr	r1, __mx23_flush_cache_addr2
+	mov	lr, pc
+	mov	pc, r1
+
+	@ enable internal xtal
+	mov	r2, #(HW_POWER_MINPWR_ADDR & 0x000000FF)
+	orr	r2, r2, #(HW_POWER_MINPWR_ADDR & 0x0000FF00)
+	orr	r2, r2, #(HW_POWER_MINPWR_ADDR & 0x00FF0000)
+	orr	r2, r2, #(HW_POWER_MINPWR_ADDR & 0xFF000000)
+	ldr	r1, [r2]
+	orr	r1, r1, #(1<<9)
+	str	r1, [r2]
+	orr	r1, r1, #(1<<8)
+	str	r1, [r2]
+
+	@ enable RTC/RAM clocks
+	mov	r0, #(HW_RTC_PERSISTENT0 & 0x000000FF)
+	orr	r0, r0, #(HW_RTC_PERSISTENT0 & 0x0000FF00)
+	orr	r0, r0, #(HW_RTC_PERSISTENT0 & 0x00FF0000)
+	orr	r0, r0, #(HW_RTC_PERSISTENT0 & 0xFF000000)
+	mov	r1, #((1<<4)|(1<<5)|1)
+	str	r1, [r0, #4]
+
+	@ put DRAM into self refresh
+	mov	r0, #(HW_DRAM_CTL08 & 0x000000FF)
+	orr	r0, r0, #(HW_DRAM_CTL08 & 0x0000FF00)
+	orr	r0, r0, #(HW_DRAM_CTL08 & 0x00FF0000)
+	orr	r0, r0, #(HW_DRAM_CTL08 & 0xFF000000)
+	ldr	r1, [r0]
+	orr	r1, r1, #(1 << 8)
+	str	r1, [r0]
+	@ wait for it to actually happen
+	mov	r0, #(HW_EMI_STAT & 0x000000FF)
+	orr	r0, r0, #(HW_EMI_STAT & 0x0000FF00)
+	orr	r0, r0, #(HW_EMI_STAT & 0x00FF0000)
+	orr	r0, r0, #(HW_EMI_STAT & 0xFF000000)
+1:	ldr	r1, [r0]
+	teq	r1, #(1 << 1)
+	beq	1b
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	@ power off RAM
+	mov	r0, #(HW_DRAM_CTL06 & 0x000000FF)
+	orr	r0, r0, #(HW_DRAM_CTL06 & 0x0000FF00)
+	orr	r0, r0, #(HW_DRAM_CTL06 & 0x00FF0000)
+	orr	r0, r0, #(HW_DRAM_CTL06 & 0xFF000000)
+	ldr	r1, [r0]
+	orr	r1, r1, #(1<<24)
+	str	r1, [r0]
+	nop
+	nop
+	nop
+	nop
+
+	@ do enter sleep
+	mov	r0, #(HW_POWER_RESET_ADDR & 0x000000FF)
+	orr	r0, r0, #(HW_POWER_RESET_ADDR & 0x0000FF00)
+	orr	r0, r0, #(HW_POWER_RESET_ADDR & 0x00FF0000)
+	orr	r0, r0, #(HW_POWER_RESET_ADDR & 0xFF000000)
+	mov	r1, #0xFF000000
+	orr	r1, r1, #0x00FF0000
+	str	r1, [r0, #8]
+	mov	r1, #0x3E000000
+	orr	r1, r1, #0x00770000
+	str	r1, [r0, #4]
+	mov	r1, #2
+	str	r1, [r0, #8]
+	mov	r1, #1
+	str	r1, [r0, #4]
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	@ sleeping now...
+
+__restore_context:
+	mov	r0, #0
+	mcr	p15, 0, r0, c7, c10, 4	@ Drain write buffer
+	mcr	p15, 0, r0, c8, c7, 0	@ Invalidate TLBs
+	mcr	p15, 0, r0, c7, c7, 0	@ Invalidate I & D cache
+	nop
+	nop
+
+	mov	r0, #0xd3
+	msr	cpsr, r0
+
+	bl	__create_temp_page_tables
+	mov	r3, r4
+
+	mov	r1, #PHYS_RAM_START
+	ldr	r1, [r1]
+	ldr	r2, [r1, #MMUDOMAIN_OFFS]
+	ldr	r4, [r1, #MMUCPACCESS_OFS]
+	ldr	r5, [r1, #MMUPID_OFFS]
+	ldr	r6, =__resume_after_mmu
+	ldr	r7, [r1, #MMUCTL_OFFS]
+	ldr	r8, [r1, #MMUTTB_OFFS]
+	add	r1, r1, #(0xC0000000 - PHYS_RAM_START)
+	mov	r0, #0
+@	mcr	p15, 0, r4, c15, c1, 0	@ cpaccess
+	mcr	p15, 0, r5, c13, c0, 0	@ pid
+	mcr	p15, 0, r2, c3, c0, 0	@ domain
+	mcr	p15, 0, r3, c2, c0, 0	@ ttb
+	b	1f
+	.align 5
+1:	mov	r0, r0
+	mcr     p15, 0, r7, c1, c0, 0	@ mmuctl
+	nop
+	mrc	p15, 0, r0, c3, c0, 0	@ read id
+	mov	r0, r0
+	mov	r0, r0
+	sub	pc, r6, r5, lsr #32
+	nop
+	nop
+	nop
+__resume_after_mmu:
+	mov	r0, #0
+	mcr	p15, 0, r0, c8, c7, 0	@ Invalidate TLBs
+	mcr	p15, 0, r0, c7, c7, 0	@ Invalidate I & D cache
+
+	mov	r0, r8
+	bl	cpu_arm926_switch_mm
+
+	mov	r0, #0xd1 @FIQ, Interrupts disabled
+	ldr	r2, [r1, #FIQ_SPSR_OFFS]
+	add	r3, r1, #FIQ_R8_OFFS
+	msr	cpsr, r0
+	msr	spsr, r2
+	ldmia	r3!, {r8-r12, sp, lr}
+
+	mov	r0, #0xd7 @ABT, Interrupts disabled
+	ldr	r2, [r1, #ABT_SPSR_OFFS]
+	add	r3, r1, #ABT_SP_OFFS
+	msr	cpsr, r0
+	msr	spsr, r2
+	ldmia	r3!, {sp, lr}
+
+	mov	r0, #0xd2 @IRQ, Interrupts disabled
+	ldr	r2, [r1, #IRQ_SPSR_OFFS]
+	add	r3, r1, #IRQ_SP_OFFS
+	msr	cpsr, r0
+	msr	spsr, r2
+	ldmia	r3!, {sp, lr}
+
+	mov	r0, #0xdb @UND, Interrupts disabled
+	ldr	r2, [r1, #UND_SPSR_OFFS]
+	add	r3, r1, #UND_SP_OFFS
+	msr	cpsr, r0
+	msr	spsr, r2
+	ldmia	r3!, {sp, lr}
+
+	mov	r0, #0xdf @SYS, Interrupts disabled
+	add	r3, r1, #SYS_SP_OFFS
+	msr	cpsr, r0
+	ldmia	r3!, {sp, lr}
+
+	mov	r0, #0xd3 @SVC, interrupts disabled
+	ldr	r2, [r1, #SVC_SPSR_OFFS]
+	ldr	r3, [r1, #SVC_SP_OFFS]
+	msr	cpsr, r0
+	msr	spsr, r2
+	mov	sp, r3
+
+#if 0
+	@ select CPU bypass, will be cleared afterwards
+	ldr	r0, =HW_CLKCTRL_CLKSEQ_ADDR
+	ldr	r2, =HW_CLKCTRL_HBUS_ADDR
+	ldr	r4, =HW_CLKCTRL_CPU_ADDR
+	mov	r1, #(1<<7)
+	ldr	r3, [r2]
+	bic	r3, r3, #BM_CLKCTRL_HBUS_DIV
+	orr	r3, r3, #1
+	ldr	r5, [r4]
+	bic	r5, r5, #BM_CLKCTRL_CPU_DIV_CPU
+	orr	r5, r5, #1
+	str	r1, [r0, #4]
+	str	r3, [r2]
+	str	r5, [r4]
+#endif
+	@ restore regs and return
+	ldmfd   sp!, {r1 - r12, lr}
+	mov	pc, lr
+
+__mx23_flush_cache_addr2:
+	.word	arm926_flush_kern_cache_all
+__mx23_resume_point:
+	.word	__restore_context
+ENTRY(mx23_s2ram_alloc_sz)
+	.word	. - mx23_cpu_suspend
+
+__create_temp_page_tables:
+	ldr	r4, =(__temp_ttb - 0xC0000000 + PHYS_RAM_START)
+
+	/*
+	 * Clear the 16K level 1 swapper page table
+	 */
+	mov	r0, r4
+	mov	r3, #0
+	add	r6, r0, #0x4000
+1:	str	r3, [r0], #4
+	str	r3, [r0], #4
+	str	r3, [r0], #4
+	str	r3, [r0], #4
+	teq	r0, r6
+	bne	1b
+
+	/*
+	 * Create identity mapping for the area close to where we are to
+	 * cater for the MMU enable.
+	 */
+	mov	r6, pc, lsr #20			@ kind of where we are
+	ldr	r7, =\
+	(PMD_TYPE_SECT | PMD_SECT_BUFFERABLE | PMD_SECT_CACHEABLE\
+	| PMD_BIT4 | PMD_SECT_AP_WRITE | PMD_SECT_AP_READ)
+
+	orr	r3, r7, r6, lsl #20		@ flags + kernel base
+	str	r3, [r4, r6, lsl #2]		@ identity mapping
+
+	mov	r6, r6, lsl #20
+	add	r6, r6, #(0xC0000000-PHYS_RAM_START)
+	str	r3, [r4, r6, lsr #18]
+
+	mov	pc, lr
+	.ltorg
+
+	.section ".sdata", "a"
+	.align 14
+__temp_ttb:
+	.space 0x8000
diff --git a/arch/arm/mach-mx23/sleep.h b/arch/arm/mach-mx23/sleep.h
new file mode 100644
index 000000000000..de219ec0b851
--- /dev/null
+++ b/arch/arm/mach-mx23/sleep.h
@@ -0,0 +1,124 @@
+/*
+ * Copyright (C) 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * 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 __PM_H__
+#define __PM_H__
+
+#include "regs-clkctrl.h"
+
+#define MMUTTB1_MASK		0x00003FE0
+#define MMUTTBC_MASK		0xFFFFFFFC
+
+#define LINK_OFFS	0x08
+#define MMUCTL_OFFS	0x0C
+#define MMUAUXCTL_OFFS	0x10
+#define MMUCPACCESS_OFS	0x14
+#define MMUTTB_OFFS	0x18
+#define MMUPID_OFFS	0x1C
+#define MMUDOMAIN_OFFS	0x20
+#define SVC_R8_OFFS	0x2C
+#define SVC_SP_OFFS	0x40
+#define SVC_SPSR_OFFS	0x44
+#define FIQ_SPSR_OFFS	0x48
+#define FIQ_R8_OFFS	0x4C
+#define FIQ_SP_OFFS	0x60
+#define ABT_R8_OFFS	0x68
+#define ABT_SPSR_OFFS	0x7C
+#define ABT_SP_OFFS	0x80
+#define IRQ_R8_OFFS	0x88
+#define IRQ_SPSR_OFFS	0x9C
+#define IRQ_SP_OFFS	0xA0
+#define UND_SPSR_OFFS	0xA8
+#define UND_SP_OFFS	0xAC
+#define SYS_SPSR_OFFS	0xB4
+#define SYS_SP_OFFS	0xB8
+
+#ifndef __ASSEMBLER__
+#define SLEEP_DATA_FINGERPRINT 0xdeadbeef
+struct sleep_data {
+	u32		fingerprint;
+	u32		wake_addr;
+	u32		link_addr;
+	u32		mmuctl;
+	u32		mmuauxctl;
+	u32		mmucpaccess;
+	u32		mmuttb;
+	u32		mmupid;
+	u32		mmudomain;
+	u32		svc_r6;
+	u32		svc_r7;
+	u32		svc_r8;
+	u32		svc_r9;
+	u32		svc_r10;
+	u32		svc_r11;
+	u32		svc_r12;
+	u32		svc_sp;
+	u32		svc_spsr;
+	u32		fiq_spsr;
+	u32		fiq_r8;
+	u32		fiq_r9;
+	u32		fiq_r10;
+	u32		fiq_r11;
+	u32		fiq_r12;
+	u32		fiq_sp;
+	u32		fiq_lr;
+	u32		abt_r8;
+	u32		abt_r9;
+	u32		abt_r10;
+	u32		abt_r11;
+	u32		abt_r12;
+	u32		abt_spsr;
+	u32		abt_sp;
+	u32		abt_lr;
+	u32		irq_r8;
+	u32		irq_r9;
+	u32		irq_r10;
+	u32		irq_r11;
+	u32		irq_r12;
+	u32		irq_spsr;
+	u32		irq_sp;
+	u32		irq_lr;
+	u32		und_spsr;
+	u32		und_sp;
+	u32		und_lr;
+	u32		sys_spsr;
+	u32		sys_sp;
+	u32		sys_lr;
+	u32		pinmux[0x100];
+	u32		icoll_ctrl;
+	union {
+		u32		prio[0x10];
+		u32		intr[0x80];
+	} icoll;
+	u32		clks[16];
+	u32		old_c00;
+	u32		old_c04;
+};
+
+extern int mx23_s2ram_alloc_sz;
+void mx23_cpu_suspend(void);
+extern int mx23_standby_alloc_sz;
+void mx23_cpu_standby(void);
+void mxs_nomatch_suspend_timer(void);
+void mxs_nomatch_resume_timer(void);
+
+void *iram_alloc(unsigned int size, unsigned long *dma_addr);
+void iram_free(unsigned long addr, unsigned int size);
+#endif /* __ASSEMBLER__ */
+#endif /* __PM_H__ */