MLK-9708 arm: imx: add low power idle support for i.mx6sx

1. improve imx6q_set_int_mem_clk_lpm routine;
2. export rbc enable interface for cpuidle;
3. enable low power idle for i.MX6SX:

   WFI            -> first level idle;
   WAIT mode      -> second level idle;
   Low power idle -> third level idle, only when system is in low bus mode.

In low powe idle mode, below operations will be done:

   ARM power off;
   AHB freq lower to 3MHz;
   PERCLK freq lower to 6MHz;
   MMDC freq lower to 1MHz;

Anatop will be put into low power mode, and regular band-gap will
be off and low power band-gap will be enabled instead.

Also, in low power idle mode, 24MHz XTAL power will be off and 24MHz clk
source will be switched to RC-OSC to save power, this feature is only
enabled on i.MX6SX TO1.2.

Signed-off-by: Anson Huang <b20788@freescale.com>

9 files changed, 1169 insertions, 11 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index a5bc944a111e..8015762df4e0 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -32,6 +32,7 @@ ifeq ($(CONFIG_CPU_IDLE),y)
 obj-$(CONFIG_SOC_IMX5) += cpuidle-imx5.o
 obj-$(CONFIG_SOC_IMX6Q) += cpuidle-imx6q.o
 obj-$(CONFIG_SOC_IMX6SL) += cpuidle-imx6sl.o
+obj-$(CONFIG_SOC_IMX6SX) += cpuidle-imx6sx.o
 endif
 
 ifdef CONFIG_SND_IMX_SOC
@@ -118,7 +119,8 @@ AFLAGS_lpddr2_freq_imx6.o :=-Wa,-march=armv7-a
 obj-$(CONFIG_SOC_IMX6SL) += busfreq_lpddr2.o lpddr2_freq_imx6.o
 AFLAGS_lpddr2_freq_imx6sx.o :=-Wa,-march=armv7-a
 AFLAGS_ddr3_freq_imx6sx.o :=-Wa,-march=armv7-a
-obj-$(CONFIG_SOC_IMX6SX) += ddr3_freq_imx6sx.o lpddr2_freq_imx6sx.o
+AFLAGS_imx6sx_low_power_idle.o :=-Wa,-march=armv7-a
+obj-$(CONFIG_SOC_IMX6SX) += ddr3_freq_imx6sx.o lpddr2_freq_imx6sx.o imx6sx_low_power_idle.o
 endif
 
 
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index 4f6e4e81cb64..117bcd2daff8 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -140,11 +140,13 @@ void imx_anatop_init(void);
 void imx_anatop_pre_suspend(void);
 void imx_anatop_post_resume(void);
 int imx6q_set_lpm(enum mxc_cpu_pwr_mode mode);
-void imx6q_set_int_mem_clk_lpm(void);
+void imx6q_set_int_mem_clk_lpm(bool enable);
 void imx6sl_set_wait_clk(bool enter);
 void imx6_enet_mac_init(const char *compatible);
 int imx_mmdc_get_ddr_type(void);
 void imx6_busfreq_map_io(void);
+void imx6sx_low_power_idle(void);
+void imx6q_enable_rbc(bool enable);
 
 void imx_cpu_die(unsigned int cpu);
 int imx_cpu_kill(unsigned int cpu);
diff --git a/arch/arm/mach-imx/cpuidle-imx6q.c b/arch/arm/mach-imx/cpuidle-imx6q.c
index d88448209354..b458490f59a2 100644
--- a/arch/arm/mach-imx/cpuidle-imx6q.c
+++ b/arch/arm/mach-imx/cpuidle-imx6q.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012 Freescale Semiconductor, Inc.
+ * Copyright (C) 2012-2014 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 version 2 as
@@ -66,7 +66,7 @@ static struct cpuidle_driver imx6q_cpuidle_driver = {
 int __init imx6q_cpuidle_init(void)
 {
 	/* Set INT_MEM_CLK_LPM bit to get a reliable WAIT mode support */
-	imx6q_set_int_mem_clk_lpm();
+	imx6q_set_int_mem_clk_lpm(true);
 
 	return cpuidle_register(&imx6q_cpuidle_driver, NULL);
 }
diff --git a/arch/arm/mach-imx/cpuidle-imx6sx.c b/arch/arm/mach-imx/cpuidle-imx6sx.c
new file mode 100644
index 000000000000..1d91375a0652
--- /dev/null
+++ b/arch/arm/mach-imx/cpuidle-imx6sx.c
@@ -0,0 +1,271 @@
+/*
+ * Copyright (C) 2014 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/cpuidle.h>
+#include <linux/cpu_pm.h>
+#include <linux/delay.h>
+#include <linux/genalloc.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <asm/cpuidle.h>
+#include <asm/fncpy.h>
+#include <asm/mach/map.h>
+#include <asm/proc-fns.h>
+#include <asm/suspend.h>
+#include <asm/tlb.h>
+
+#include "common.h"
+#include "cpuidle.h"
+#include "hardware.h"
+
+#define MX6_MAX_MMDC_IO_NUM	19
+
+#define PMU_LOW_PWR_CTRL	0x270
+#define XTALOSC24M_OSC_CONFIG0	0x2a0
+#define XTALOSC24M_OSC_CONFIG1	0x2b0
+#define XTALOSC24M_OSC_CONFIG2	0x2c0
+#define XTALOSC24M_OSC_CONFIG0_RC_OSC_PROG_CUR_SHIFT	24
+#define XTALOSC24M_OSC_CONFIG0_HYST_MINUS_MASK		0xf
+#define XTALOSC24M_OSC_CONFIG0_HYST_MINUS_SHIFT		16
+#define XTALOSC24M_OSC_CONFIG0_HYST_PLUS_MASK		0xf
+#define XTALOSC24M_OSC_CONFIG0_HYST_PLUS_SHIFT		12
+#define XTALOSC24M_OSC_CONFIG0_RC_OSC_PROG_SHIFT	4
+#define XTALOSC24M_OSC_CONFIG0_ENABLE_SHIFT		1
+#define XTALOSC24M_OSC_CONFIG0_START_SHIFT		0
+#define XTALOSC24M_OSC_CONFIG1_COUNT_RC_CUR_SHIFT	20
+#define XTALOSC24M_OSC_CONFIG1_COUNT_RC_TRG_SHIFT	0
+#define XTALOSC24M_OSC_CONFIG2_COUNT_1M_TRG_MASK	0xfff
+#define XTALOSC24M_OSC_CONFIG2_COUNT_1M_TRG_SHIFT	0
+
+static void __iomem *wfi_iram_base;
+static void __iomem *wfi_iram_base_phys;
+extern unsigned long iram_tlb_phys_addr;
+extern unsigned long mx6sx_lpm_wfi_start asm("mx6sx_lpm_wfi_start");
+extern unsigned long mx6sx_lpm_wfi_end asm("mx6sx_lpm_wfi_end");
+extern u32 low_bus_freq_mode;
+
+struct imx6_pm_base {
+	phys_addr_t pbase;
+	void __iomem *vbase;
+};
+
+static const u32 imx6sx_mmdc_io_offset[] __initconst = {
+	0x2ec, 0x2f0, 0x2f4, 0x2f8, /* DQM0 ~ DQM3 */
+	0x330, 0x334, 0x338, 0x33c, /* SDQS0 ~ SDQS3 */
+	0x60c, 0x610, 0x61c, 0x620, /* B0DS ~ B3DS */
+	0x5f8, 0x608, 0x310, 0x314, /* CTL, MODE, SODT0, SODT1 */
+	0x300, 0x2fc, 0x32c,	    /* CAS, RAS, SDCLK_0 */
+};
+
+struct imx6_cpuidle_pm_info {
+	phys_addr_t pbase; /* The physical address of pm_info. */
+	phys_addr_t resume_addr; /* The physical resume address for asm code */
+	u32 pm_info_size; /* Size of pm_info. */
+	u32 ttbr;
+	struct imx6_pm_base mmdc_base;
+	struct imx6_pm_base iomuxc_base;
+	struct imx6_pm_base ccm_base;
+	struct imx6_pm_base gpc_base;
+	struct imx6_pm_base l2_base;
+	struct imx6_pm_base anatop_base;
+	struct imx6_pm_base src_base;
+	u32 saved_diagnostic; /* To save disagnostic register */
+	u32 mmdc_io_num; /* Number of MMDC IOs which need saved/restored. */
+	u32 mmdc_io_val[MX6_MAX_MMDC_IO_NUM][2]; /* To save offset and value */
+} __aligned(8);
+
+static void (*imx6sx_wfi_in_iram_fn)(void __iomem *iram_vbase);
+
+static int imx6_idle_finish(unsigned long val)
+{
+	imx6sx_wfi_in_iram_fn(wfi_iram_base);
+
+	return 0;
+}
+
+static int imx6sx_enter_wait(struct cpuidle_device *dev,
+			    struct cpuidle_driver *drv, int index)
+{
+	imx6q_set_lpm(WAIT_UNCLOCKED);
+	if ((index == 1) || (!low_bus_freq_mode && index == 2)) {
+		cpu_do_idle();
+	} else {
+			/* Need to notify there is a cpu pm operation. */
+			cpu_pm_enter();
+			cpu_cluster_pm_enter();
+
+			cpu_suspend(0, imx6_idle_finish);
+
+			cpu_cluster_pm_exit();
+			cpu_pm_exit();
+			imx6q_enable_rbc(false);
+	}
+
+	imx6q_set_lpm(WAIT_CLOCKED);
+
+	return index;
+}
+
+static struct cpuidle_driver imx6sx_cpuidle_driver = {
+	.name = "imx6sx_cpuidle",
+	.owner = THIS_MODULE,
+	.states = {
+		/* WFI */
+		ARM_CPUIDLE_WFI_STATE,
+		/* WAIT MODE */
+		{
+			.exit_latency = 50,
+			.target_residency = 75,
+			.flags = CPUIDLE_FLAG_TIME_VALID,
+			.enter = imx6sx_enter_wait,
+			.name = "WAIT",
+			.desc = "Clock off",
+		},
+		/* LOW POWER IDLE */
+		{
+			/*
+			 * RBC 31us + ARM gating 93us + RBC clear 65us
+			 * + PLL2 relock 450us and some margin, here set
+			 * it to 650us.
+			 */
+			.exit_latency = 650,
+			.target_residency = 1000,
+			.flags = CPUIDLE_FLAG_TIME_VALID,
+			.enter = imx6sx_enter_wait,
+			.name = "LOW-POWER-IDLE",
+			.desc = "ARM power off",
+		},
+
+	},
+	.state_count = 3,
+	.safe_state_index = 0,
+};
+
+int __init imx6sx_cpuidle_init(void)
+{
+	struct imx6_cpuidle_pm_info *cpuidle_pm_info;
+	int i;
+	const u32 *mmdc_offset_array;
+	u32 wfi_code_size, val;
+
+	wfi_iram_base_phys = (void *)(iram_tlb_phys_addr + MX6_CPUIDLE_IRAM_ADDR_OFFSET);
+
+	/* Make sure wfi_iram_base is 8 byte aligned. */
+	if ((uintptr_t)(wfi_iram_base_phys) & (FNCPY_ALIGN - 1))
+		wfi_iram_base_phys += FNCPY_ALIGN - ((uintptr_t)wfi_iram_base_phys % (FNCPY_ALIGN));
+
+	wfi_iram_base = (void *)IMX_IO_P2V((unsigned long) wfi_iram_base_phys);
+
+	cpuidle_pm_info = wfi_iram_base;
+	cpuidle_pm_info->pbase = (phys_addr_t) wfi_iram_base_phys;
+	cpuidle_pm_info->pm_info_size = sizeof(*cpuidle_pm_info);
+	cpuidle_pm_info->resume_addr = virt_to_phys(v7_cpu_resume);
+	cpuidle_pm_info->mmdc_io_num = ARRAY_SIZE(imx6sx_mmdc_io_offset);
+	mmdc_offset_array = imx6sx_mmdc_io_offset;
+
+	cpuidle_pm_info->mmdc_base.pbase = MX6Q_MMDC_P0_BASE_ADDR;
+	cpuidle_pm_info->mmdc_base.vbase = (void __iomem *)IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR);
+
+	cpuidle_pm_info->ccm_base.pbase = MX6Q_CCM_BASE_ADDR;
+	cpuidle_pm_info->ccm_base.vbase = (void __iomem *)IMX_IO_P2V(MX6Q_CCM_BASE_ADDR);
+
+	cpuidle_pm_info->anatop_base.pbase = MX6Q_ANATOP_BASE_ADDR;
+	cpuidle_pm_info->anatop_base.vbase = (void __iomem *)IMX_IO_P2V(MX6Q_ANATOP_BASE_ADDR);
+
+	cpuidle_pm_info->gpc_base.pbase = MX6Q_GPC_BASE_ADDR;
+	cpuidle_pm_info->gpc_base.vbase = (void __iomem *)IMX_IO_P2V(MX6Q_GPC_BASE_ADDR);
+
+	cpuidle_pm_info->iomuxc_base.pbase = MX6Q_IOMUXC_BASE_ADDR;
+	cpuidle_pm_info->iomuxc_base.vbase = (void __iomem *)IMX_IO_P2V(MX6Q_IOMUXC_BASE_ADDR);
+
+	cpuidle_pm_info->l2_base.pbase = MX6Q_L2_BASE_ADDR;
+	cpuidle_pm_info->l2_base.vbase = (void __iomem *)IMX_IO_P2V(MX6Q_L2_BASE_ADDR);
+
+	cpuidle_pm_info->src_base.pbase = MX6Q_SRC_BASE_ADDR;
+	cpuidle_pm_info->src_base.vbase = (void __iomem *)IMX_IO_P2V(MX6Q_SRC_BASE_ADDR);
+
+	/* only save mmdc io offset, settings will be saved in asm code */
+	for (i = 0; i < cpuidle_pm_info->mmdc_io_num; i++)
+		cpuidle_pm_info->mmdc_io_val[i][0] = mmdc_offset_array[i];
+
+	/* code size should include cpuidle_pm_info size */
+	wfi_code_size = (&mx6sx_lpm_wfi_end -&mx6sx_lpm_wfi_start) *4 + sizeof(*cpuidle_pm_info);
+	imx6sx_wfi_in_iram_fn = (void *)fncpy(wfi_iram_base + sizeof(*cpuidle_pm_info),
+		&imx6sx_low_power_idle, wfi_code_size);
+
+	imx6q_set_int_mem_clk_lpm(true);
+
+	if (imx_get_soc_revision() >= IMX_CHIP_REVISION_1_2) {
+		/*
+		 * enable RC-OSC here, as it needs at least 4ms for RC-OSC to
+		 * be stable, low power idle flow can NOT endure this big
+		 * latency, so we make RC-OSC self-tuning enabled here.
+		 */
+		val = readl_relaxed(cpuidle_pm_info->anatop_base.vbase +
+			PMU_LOW_PWR_CTRL);
+		val |= 0x1;
+		writel_relaxed(val, cpuidle_pm_info->anatop_base.vbase +
+			PMU_LOW_PWR_CTRL);
+		/*
+		 * config RC-OSC freq
+		 * tune_enable = 1;tune_start = 1;hyst_plus = 0;hyst_minus = 0;
+		 * osc_prog = 0xa7;
+		 */
+		writel_relaxed(
+			0x4 << XTALOSC24M_OSC_CONFIG0_RC_OSC_PROG_CUR_SHIFT |
+			0xa7 << XTALOSC24M_OSC_CONFIG0_RC_OSC_PROG_SHIFT |
+			0x1 << XTALOSC24M_OSC_CONFIG0_ENABLE_SHIFT |
+			0x1 << XTALOSC24M_OSC_CONFIG0_START_SHIFT,
+			cpuidle_pm_info->anatop_base.vbase +
+			XTALOSC24M_OSC_CONFIG0);
+		/* set count_trg = 0x2dc */
+		writel_relaxed(
+			0x40 << XTALOSC24M_OSC_CONFIG1_COUNT_RC_CUR_SHIFT |
+			0x2dc << XTALOSC24M_OSC_CONFIG1_COUNT_RC_TRG_SHIFT,
+			cpuidle_pm_info->anatop_base.vbase +
+			XTALOSC24M_OSC_CONFIG1);
+		/* wait 4ms according to hardware design */
+		msleep(4);
+		/*
+		 * now add some hysteresis, hyst_plus=3, hyst_minus=3
+		 * (the minimum hysteresis that looks good is 2)
+		 */
+		val = readl_relaxed(cpuidle_pm_info->anatop_base.vbase +
+			XTALOSC24M_OSC_CONFIG0);
+		val &= ~((XTALOSC24M_OSC_CONFIG0_HYST_MINUS_MASK <<
+			XTALOSC24M_OSC_CONFIG0_HYST_MINUS_SHIFT) |
+			(XTALOSC24M_OSC_CONFIG0_HYST_PLUS_MASK <<
+			XTALOSC24M_OSC_CONFIG0_HYST_PLUS_SHIFT));
+		val |= (0x3 << XTALOSC24M_OSC_CONFIG0_HYST_MINUS_SHIFT) |
+			(0x3 << XTALOSC24M_OSC_CONFIG0_HYST_PLUS_SHIFT);
+		writel_relaxed(val, cpuidle_pm_info->anatop_base.vbase +
+			XTALOSC24M_OSC_CONFIG0);
+		/* set the count_1m_trg = 0x2d7 */
+		val = readl_relaxed(cpuidle_pm_info->anatop_base.vbase +
+			XTALOSC24M_OSC_CONFIG2);
+		val &= ~(XTALOSC24M_OSC_CONFIG2_COUNT_1M_TRG_MASK <<
+			XTALOSC24M_OSC_CONFIG2_COUNT_1M_TRG_SHIFT);
+		val |= 0x2d7 << XTALOSC24M_OSC_CONFIG2_COUNT_1M_TRG_SHIFT;
+		writel_relaxed(val, cpuidle_pm_info->anatop_base.vbase +
+			XTALOSC24M_OSC_CONFIG2);
+		/*
+		 * hardware design require to write XTALOSC24M_OSC_CONFIG0 or
+		 * XTALOSC24M_OSC_CONFIG1 to
+		 * make XTALOSC24M_OSC_CONFIG2 write work
+		 */
+		val = readl_relaxed(cpuidle_pm_info->anatop_base.vbase +
+			XTALOSC24M_OSC_CONFIG1);
+		writel_relaxed(val, cpuidle_pm_info->anatop_base.vbase +
+			XTALOSC24M_OSC_CONFIG1);
+	}
+
+	return cpuidle_register(&imx6sx_cpuidle_driver, NULL);
+}
diff --git a/arch/arm/mach-imx/cpuidle.h b/arch/arm/mach-imx/cpuidle.h
index 24e33670417c..07dd7759ba5e 100644
--- a/arch/arm/mach-imx/cpuidle.h
+++ b/arch/arm/mach-imx/cpuidle.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2012 Freescale Semiconductor, Inc.
+ * Copyright 2012-2014 Freescale Semiconductor, Inc.
  * Copyright 2012 Linaro Ltd.
  *
  * The code contained herein is licensed under the GNU General Public
@@ -14,6 +14,7 @@
 extern int imx5_cpuidle_init(void);
 extern int imx6q_cpuidle_init(void);
 extern int imx6sl_cpuidle_init(void);
+extern int imx6sx_cpuidle_init(void);
 #else
 static inline int imx5_cpuidle_init(void)
 {
@@ -27,4 +28,8 @@ static inline int imx6sl_cpuidle_init(void)
 {
 	return 0;
 }
+static inline int imx6sx_cpuidle_init(void)
+{
+	return 0;
+}
 #endif
diff --git a/arch/arm/mach-imx/imx6sx_low_power_idle.S b/arch/arm/mach-imx/imx6sx_low_power_idle.S
new file mode 100644
index 000000000000..a53f920d5a86
--- /dev/null
+++ b/arch/arm/mach-imx/imx6sx_low_power_idle.S
@@ -0,0 +1,865 @@
+/*
+ * Copyright (C) 2014 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.
+ */
+
+#include <linux/linkage.h>
+
+#define PM_INFO_PBASE_OFFSET			0x0
+#define PM_INFO_RESUME_ADDR_OFFSET		0x4
+#define PM_INFO_PM_INFO_SIZE_OFFSET		0x8
+#define PM_INFO_PM_INFO_TTBR_OFFSET		0xc
+#define PM_INFO_MX6Q_MMDC_P_OFFSET		0x10
+#define PM_INFO_MX6Q_MMDC_V_OFFSET		0x14
+#define PM_INFO_MX6Q_IOMUXC_P_OFFSET		0x18
+#define PM_INFO_MX6Q_IOMUXC_V_OFFSET		0x1c
+#define PM_INFO_MX6Q_CCM_P_OFFSET		0x20
+#define PM_INFO_MX6Q_CCM_V_OFFSET		0x24
+#define PM_INFO_MX6Q_GPC_P_OFFSET		0x28
+#define PM_INFO_MX6Q_GPC_V_OFFSET		0x2c
+#define PM_INFO_MX6Q_L2_P_OFFSET		0x30
+#define PM_INFO_MX6Q_L2_V_OFFSET		0x34
+#define PM_INFO_MX6Q_ANATOP_P_OFFSET		0x38
+#define PM_INFO_MX6Q_ANATOP_V_OFFSET		0x3c
+#define PM_INFO_MX6Q_SRC_P_OFFSET		0x40
+#define PM_INFO_MX6Q_SRC_V_OFFSET		0x44
+#define PM_INFO_MX6Q_SAVED_DIAGNOSTIC_OFFSET	0x48
+#define PM_INFO_MMDC_IO_NUM_OFFSET		0x4c
+#define PM_INFO_MMDC_IO_VAL_OFFSET		0x50
+
+#define	MX6Q_MMDC_MAPSR		0x404
+#define MX6Q_MMDC_MPDGCTRL0	0x83c
+#define MX6Q_SRC_GPR1	0x20
+#define MX6Q_SRC_GPR2	0x24
+#define MX6Q_GPC_IMR1	0x08
+#define MX6Q_GPC_IMR2	0x0c
+#define MX6Q_GPC_IMR3	0x10
+#define MX6Q_GPC_IMR4	0x14
+#define MX6Q_CCM_CCR	0x0
+
+.globl mx6sx_lpm_wfi_start
+.globl mx6sx_lpm_wfi_end
+
+	.macro	pll_do_wait_lock
+1:
+	ldr	r7, [r10, r8]
+	ands	r7, #0x80000000
+	beq	1b
+
+	.endm
+
+	.macro	ccm_do_wait
+2:
+	ldr	r7, [r10, #0x48]
+	cmp	r7, #0x0
+	bne	2b
+
+	.endm
+
+	.macro	ccm_enter_idle
+
+	ldr	r10, [r0, #PM_INFO_MX6Q_CCM_V_OFFSET]
+
+	/* set ahb to 3MHz */
+	ldr	r7, [r10, #0x14]
+	orr	r7, r7, #0x1c00
+	str	r7, [r10, #0x14]
+
+	/* set perclk to 6MHz */
+	ldr	r7, [r10, #0x1c]
+	bic	r7, r7, #0x3f
+	orr	r7, r7, #0x3
+	str	r7, [r10, #0x1c]
+
+	/* set mmdc to 1MHz, periph2_clk2 need to be @8MHz */
+	ldr	r7, [r10, #0x14]
+	orr     r7, r7, #0x2
+	orr	r7, r7, #(0x7 << 3)
+	str	r7, [r10, #0x14]
+
+	ccm_do_wait
+
+	ldr	r10, [r0, #PM_INFO_MX6Q_ANATOP_V_OFFSET]
+
+	/* enable PLL1 bypass output */
+	ldr	r7, [r10]
+	orr	r7, r7, #0x12000
+	str	r7, [r10]
+
+	ldr	r10, [r0, #PM_INFO_MX6Q_CCM_V_OFFSET]
+
+	/* set pll1_sw to from pll1 main */
+	ldr	r7, [r10, #0xc]
+	bic	r7, r7, #0x4
+	str	r7, [r10, #0xc]
+
+	/* set step from osc */
+	ldr	r7, [r10, #0xc]
+	bic	r7, r7, #0x100
+	str	r7, [r10, #0xc]
+
+	/* set pll1_sw to from step */
+	ldr	r7, [r10, #0xc]
+	orr	r7, r7, #0x4
+	str	r7, [r10, #0xc]
+
+	ldr	r10, [r0, #PM_INFO_MX6Q_ANATOP_V_OFFSET]
+
+	/* Disable PLL1 bypass output */
+	ldr	r7, [r10]
+	bic	r7, r7, #0x12000
+	str	r7, [r10]
+
+	/*
+	 * disable pll2, suppose when system enter low
+	 * power idle mode, only 396MHz pfd needs pll2,
+	 * now we switch arm clock to OSC, we can disable
+	 * pll2 now, gate pll2_pfd2 first.
+	 */
+	ldr	r7, [r10, #0x100]
+	orr	r7, #0x800000
+	str	r7, [r10, #0x100]
+
+	ldr	r7, [r10, #0x30]
+	orr	r7, r7, #0x1000
+	bic	r7, r7, #0x2000
+	str	r7, [r10, #0x30]
+
+	.endm
+
+	.macro	ccm_exit_idle
+
+	cmp	r5, #0x0
+	ldreq	r10, [r0, #PM_INFO_MX6Q_ANATOP_V_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX6Q_ANATOP_P_OFFSET]
+
+	/* enable pll2 and pll2_pfd2 */
+	ldr	r7, [r10, #0x30]
+	bic	r7, r7, #0x1000
+	orr	r7, r7, #0x2000
+	str	r7, [r10, #0x30]
+
+	ldr	r8, =0x30
+	pll_do_wait_lock
+
+	ldr	r7, [r10, #0x100]
+	bic	r7, #0x800000
+	str	r7, [r10, #0x100]
+
+	/* enable PLL1 bypass output */
+	ldr	r7, [r10]
+	orr	r7, r7, #0x12000
+	str	r7, [r10]
+
+	cmp	r5, #0x0
+	ldreq	r10, [r0, #PM_INFO_MX6Q_CCM_V_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX6Q_CCM_P_OFFSET]
+
+	/* set perclk back to 24MHz */
+	ldr	r7, [r10, #0x1c]
+	bic	r7, r7, #0x3f
+	str	r7, [r10, #0x1c]
+
+	/* set mmdc back to 24MHz */
+	ldr	r7, [r10, #0x14]
+	bic	r7, r7, #0x7
+	bic	r7, r7, #(0x7 << 3)
+	str	r7, [r10, #0x14]
+
+	/* set ahb div back to 24MHz */
+	ldr	r7, [r10, #0x14]
+	bic	r7, r7, #0x1c00
+	str	r7, [r10, #0x14]
+
+	ccm_do_wait
+
+	/* set pll1_sw to from pll1 main */
+	ldr	r7, [r10, #0xc]
+	bic	r7, r7, #0x4
+	str	r7, [r10, #0xc]
+
+	/* set step from pll2_pfd2 */
+	ldr	r7, [r10, #0xc]
+	orr	r7, r7, #0x100
+	str	r7, [r10, #0xc]
+
+	/* set pll1_sw to from step */
+	ldr	r7, [r10, #0xc]
+	orr	r7, r7, #0x4
+	str	r7, [r10, #0xc]
+
+	cmp	r5, #0x0
+	ldreq	r10, [r0, #PM_INFO_MX6Q_ANATOP_V_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX6Q_ANATOP_P_OFFSET]
+
+	/* disable PLL1 bypass output */
+	ldr	r7, [r10]
+	bic	r7, r7, #0x12000
+	str	r7, [r10]
+
+	.endm
+
+	.macro	anatop_enter_idle
+
+	ldr	r10, [r0, #PM_INFO_MX6Q_ANATOP_V_OFFSET]
+
+	/*
+	 * check whether any PLL is enabled, as only when
+	 * there is no PLLs enabled, 2P5 and 1P1 can be
+	 * off and only enable weak ones.
+	 */
+
+	/* arm pll1 */
+	ldr	r7, [r10, #0]
+	ands	r7, r7, #(1 << 31)
+	bne	10f
+
+	/* sys pll2 */
+	ldr	r7, [r10, #0x30]
+	ands	r7, r7, #(1 << 31)
+	bne	10f
+
+	/* usb pll3 */
+	ldr	r7, [r10, #0x10]
+	ands	r7, r7, #(1 << 31)
+	bne	10f
+
+	/* audio pll4 */
+	ldr	r7, [r10, #0x70]
+	ands	r7, r7, #(1 << 31)
+	bne	10f
+
+	/* vidio pll5 */
+	ldr	r7, [r10, #0xa0]
+	ands	r7, r7, #(1 << 31)
+	bne	10f
+
+	/* enet pll6 */
+	ldr	r7, [r10, #0xe0]
+	ands	r7, r7, #(1 << 31)
+	bne	10f
+
+	/* usb host pll7 */
+	ldr	r7, [r10, #0x20]
+	ands	r7, r7, #(1 << 31)
+	bne	10f
+
+	/* enable weak 2P5 and turn off regular 2P5 */
+	ldr	r7, [r10, #0x130]
+	orr	r7, r7, #0x40000
+	str	r7, [r10, #0x130]
+	bic	r7, r7, #0x1
+	str	r7, [r10, #0x130]
+
+	/* enable weak 1p1 and turn off regular 1P1 */
+	ldr	r7, [r10, #0x110]
+	orr	r7, r7, #0x40000
+	str	r7, [r10, #0x110]
+	bic	r7, r7, #0x1
+	str	r7, [r10, #0x110]
+
+	/* check whether ARM LDO is bypassed */
+	ldr	r7, [r10, #0x140]
+	and	r7, r7, #0x1f
+	cmp	r7, #0x1f
+	bne	10f
+
+	/* low power band gap enable */
+	ldr	r7, [r10, #0x270]
+	orr	r7, r7, #0x20
+	str	r7, [r10, #0x270]
+
+	/* turn off the bias current from the regular bandgap */
+	ldr	r7, [r10, #0x270]
+	orr	r7, r7, #0x80
+	str	r7, [r10, #0x270]
+
+	/*
+	 * clear the REFTOP_SELFBIASOFF,
+	 * self-bias circuit of the band gap.
+	 * Per RM, should be cleared when
+	 * band gap is powered down.
+	 */
+	ldr	r7, [r10, #0x150]
+	bic	r7, r7, #0x8
+	str	r7, [r10, #0x150]
+
+	/* turn off regular bandgap */
+	ldr	r7, [r10, #0x150]
+	orr	r7, r7, #0x1
+	str	r7, [r10, #0x150]
+
+	/* only switch to RC-OSC clk after TO1.2 */
+	ldr	r7, [r10, #0x260]
+	and	r7, r7, #0x3
+	cmp	r7, #0x2
+	blt	10f
+
+	/* switch to RC-OSC */
+	ldr	r7, [r10, #0x270]
+	orr	r7, r7, #0x10
+	str	r7, [r10, #0x270]
+
+	/* turn off XTAL-OSC */
+	ldr	r7, [r10, #0x150]
+	orr	r7, r7, #0x40000000
+	str	r7, [r10, #0x150]
+10:
+	/* lower OSC current by 37.5% */
+	ldr	r7, [r10, #0x150]
+	orr	r7, r7, #0x6000
+	str	r7, [r10, #0x150]
+
+	.endm
+
+	.macro anatop_exit_idle
+
+	cmp	r5, #0x0
+	ldreq	r10, [r0, #PM_INFO_MX6Q_ANATOP_V_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX6Q_ANATOP_P_OFFSET]
+
+	/* increase OSC current to normal */
+	ldr	r7, [r10, #0x150]
+	bic	r7, r7, #0x6000
+	str	r7, [r10, #0x150]
+
+	/* only switch to RC-OSC after TO1.2 */
+	ldr	r7, [r10, #0x260]
+	and	r7, r7, #0x3
+	cmp	r7, #0x2
+	blt	15f
+
+	/* turn on XTAL-OSC and detector */
+	ldr	r7, [r10, #0x150]
+	bic	r7, r7, #0x40000000
+	orr	r7, r7, #0x10000
+	str	r7, [r10, #0x150]
+
+	/* wait for XTAL stable */
+14:
+	ldr	r7, [r10, #0x150]
+	ands	r7, r7, #0x8000
+	beq	14b
+
+	/* switch to XTAL-OSC */
+	ldr	r7, [r10, #0x270]
+	bic	r7, r7, #0x10
+	str	r7, [r10, #0x270]
+
+	/* turn off XTAL-OSC detector */
+	ldr	r7, [r10, #0x150]
+	bic	r7, r7, #0x10000
+	str	r7, [r10, #0x150]
+15:
+	/* check whether we need to enable 2P5/1P1 */
+	ldr	r7, [r10, #0x110]
+	ands	r7, r7, #0x40000
+	beq	11f
+
+	/* check whether ARM LDO is bypassed */
+	ldr	r7, [r10, #0x140]
+	and	r7, r7, #0x1f
+	cmp	r7, #0x1f
+	bne	12f
+
+	/* turn on regular bandgap and wait for stable */
+	ldr	r7, [r10, #0x150]
+	bic	r7, r7, #0x1
+	str	r7, [r10, #0x150]
+13:
+	ldr	r7, [r10, #0x150]
+	ands	r7, #0x80
+	beq	13b
+
+	/*
+	 * set the REFTOP_SELFBIASOFF,
+	 * self-bias circuit of the band gap.
+	 */
+	ldr     r7, [r10, #0x150]
+	orr     r7, r7, #0x8
+	str     r7, [r10, #0x150]
+
+	/* turn on the bias current from the regular bandgap */
+	ldr	r7, [r10, #0x270]
+	bic	r7, r7, #0x80
+	str	r7, [r10, #0x270]
+
+	/* low power band gap disable */
+	ldr	r7, [r10, #0x270]
+	bic	r7, r7, #0x20
+	str	r7, [r10, #0x270]
+12:
+	/* enable regular 2P5 and turn off weak 2P5 */
+	ldr	r7, [r10, #0x130]
+	orr	r7, r7, #0x1
+	str	r7, [r10, #0x130]
+
+	/* Ensure the 2P5 is up. */
+3:
+	ldr	r7, [r10, #0x130]
+	ands	r7, r7, #0x20000
+	beq	3b
+	ldr	r7, [r10, #0x130]
+	bic	r7, r7, #0x40000
+	str	r7, [r10, #0x130]
+
+	/* enable regular 1p1 and turn off weak 1P1 */
+	ldr	r7, [r10, #0x110]
+	orr	r7, r7, #0x1
+	str	r7, [r10, #0x110]
+4:
+	ldr	r7, [r10, #0x110]
+	ands	r7, r7, #0x20000
+	beq	4b
+	ldr	r7, [r10, #0x110]
+	bic	r7, r7, #0x40000
+	str	r7, [r10, #0x110]
+11:
+	.endm
+
+	.macro	disable_l1_dcache
+
+	/*
+	 * Flush all data from the L1 data cache before disabling
+	 * SCTLR.C bit.
+	 */
+	push	{r0 - r10, lr}
+	ldr	r7, =v7_flush_dcache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r10, lr}
+
+	/* disable d-cache */
+	mrc	p15, 0, r7, c1, c0, 0
+	bic	r7, r7, #(1 << 2)
+	mcr	p15, 0, r7, c1, c0, 0
+	dsb
+	isb
+
+	push	{r0 - r10, lr}
+	ldr	r7, =v7_flush_dcache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r10, lr}
+
+	.endm
+
+	.macro mmdc_enter_dvfs_mode
+
+	/* disable automatic power savings. */
+	ldr	r7, [r10, #MX6Q_MMDC_MAPSR]
+	orr	r7, r7, #0x1
+	str	r7, [r10, #MX6Q_MMDC_MAPSR]
+
+	/* disable power down timer */
+	ldr	r7, [r10, #0x4]
+	bic	r7, r7, #0xff00
+	str	r7, [r10, #0x4]
+
+	/* make the DDR explicitly enter self-refresh. */
+	ldr	r7, [r10, #MX6Q_MMDC_MAPSR]
+	orr	r7, r7, #(1 << 21)
+	str	r7, [r10, #MX6Q_MMDC_MAPSR]
+5:
+	ldr	r7, [r10, #MX6Q_MMDC_MAPSR]
+	ands	r7, r7, #(1 << 25)
+	beq	5b
+
+	.endm
+
+	.macro	resume_mmdc
+
+	/* restore MMDC IO */
+	cmp	r5, #0x0
+	ldreq	r10, [r0, #PM_INFO_MX6Q_IOMUXC_V_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX6Q_IOMUXC_P_OFFSET]
+
+	ldr	r6, [r0, #PM_INFO_MMDC_IO_NUM_OFFSET]
+	ldr	r7, =PM_INFO_MMDC_IO_VAL_OFFSET
+	add	r7, r7, r0
+6:
+	ldr	r8, [r7], #0x4
+	ldr	r9, [r7], #0x4
+	str	r9, [r10, r8]
+	subs	r6, r6, #0x1
+	bne	6b
+
+	cmp	r5, #0x0
+	ldreq	r10, [r0, #PM_INFO_MX6Q_MMDC_V_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX6Q_MMDC_P_OFFSET]
+
+	/* reset read FIFO, RST_RD_FIFO */
+	ldr	r7, =MX6Q_MMDC_MPDGCTRL0
+	ldr	r6, [r10, r7]
+	orr	r6, r6, #(1 << 31)
+	str	r6, [r10, r7]
+7:
+	ldr	r6, [r10, r7]
+	ands	r6, r6, #(1 << 31)
+	bne	7b
+
+	/* reset FIFO a second time */
+	ldr	r6, [r10, r7]
+	orr	r6, r6, #(1 << 31)
+	str	r6, [r10, r7]
+8:
+	ldr	r6, [r10, r7]
+	ands	r6, r6, #(1 << 31)
+	bne	8b
+
+	/* let DDR out of self-refresh */
+	ldr	r7, [r10, #MX6Q_MMDC_MAPSR]
+	bic	r7, r7, #(1 << 21)
+	str	r7, [r10, #MX6Q_MMDC_MAPSR]
+9:
+	ldr	r7, [r10, #MX6Q_MMDC_MAPSR]
+	ands	r7, r7, #(1 << 25)
+	bne	9b
+
+	/* enable power down timer */
+	ldr	r7, [r10, #0x4]
+	orr	r7, r7, #0x5500
+	str	r7, [r10, #0x4]
+
+	/* enable DDR auto power saving */
+	ldr	r7, [r10, #MX6Q_MMDC_MAPSR]
+	bic	r7, r7, #0x1
+	str	r7, [r10, #MX6Q_MMDC_MAPSR]
+
+	.endm
+
+	.macro	tlb_set_to_ocram
+
+	/* save ttbr */
+	mrc	p15, 0, r7, c2, c0, 1
+	str	r7, [r0, #PM_INFO_PM_INFO_TTBR_OFFSET]
+
+	/*
+	 * To ensure no page table walks occur in DDR, we
+	 * have a another page table stored in IRAM that only
+	 * contains entries pointing to IRAM, AIPS1 and AIPS2.
+	 * We need to set the TTBR1 to the new IRAM TLB.
+	 * Do the following steps:
+	 * 1. Flush the Branch Target Address Cache (BTAC)
+	 * 2. Set TTBR1 to point to IRAM page table.
+	 * 3. Disable page table walks in TTBR0 (PD0 = 1)
+	 * 4. Set TTBR0.N=1, implying 0-2G is translated by TTBR0
+	 *     and 2-4G is translated by TTBR1.
+	 */
+
+	ldr	r6, =iram_tlb_phys_addr
+	ldr	r7, [r6]
+
+	/* Flush the BTAC. */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
+
+	/* Disable Branch Prediction, Z bit in SCTLR. */
+	mrc	p15, 0, r6, c1, c0, 0
+	bic	r6, r6, #0x800
+	mcr	p15, 0, r6, c1, c0, 0
+
+	dsb
+	isb
+
+	/* Store the IRAM table in TTBR1 */
+	mcr	p15, 0, r7, c2, c0, 1
+
+	/* Read TTBCR and set PD0=1, N = 1 */
+	mrc	p15, 0, r6, c2, c0, 2
+	orr	r6, r6, #0x11
+	mcr	p15, 0, r6, c2, c0, 2
+
+	dsb
+	isb
+
+	/* flush the TLB */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c8, c3, 0
+
+	.endm
+
+	.macro	tlb_back_to_ddr
+
+	/* Restore the TTBCR */
+
+	dsb
+	isb
+
+	/* Read TTBCR and set PD0=0, N = 0 */
+	mrc	p15, 0, r6, c2, c0, 2
+	bic	r6, r6, #0x11
+	mcr	p15, 0, r6, c2, c0, 2
+
+	dsb
+	isb
+
+	/* flush the TLB */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c8, c3, 0
+
+	dsb
+	isb
+
+	/* Enable Branch Prediction, Z bit in SCTLR. */
+	mrc	p15, 0, r6, c1, c0, 0
+	orr	r6, r6, #0x800
+	mcr	p15, 0, r6, c1, c0, 0
+
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
+
+	/* restore ttbr */
+	ldr	r7, [r0, #PM_INFO_PM_INFO_TTBR_OFFSET]
+	mcr	p15, 0, r7, c2, c0, 1
+
+	.endm
+
+.extern iram_tlb_phys_addr
+
+/* imx6sx_low_power_idle */
+
+	.align 3
+ENTRY(imx6sx_low_power_idle)
+mx6sx_lpm_wfi_start:
+	push	{r4 - r10}
+
+	/* get necessary info from pm_info */
+	ldr	r1, [r0, #PM_INFO_PBASE_OFFSET]
+	ldr	r2, [r0, #PM_INFO_PM_INFO_SIZE_OFFSET]
+
+	/*
+	 * counting the resume address in iram
+	 * to set it in SRC register.
+	 */
+	ldr	r5, =imx6sx_low_power_idle
+	ldr     r6, =wakeup
+	sub	r6, r6, r5
+	add     r8, r1, r2
+	add	r3, r8, r6
+
+	/* store physical resume addr and pm_info address. */
+	ldr	r10, [r0, #PM_INFO_MX6Q_SRC_V_OFFSET]
+	str	r3, [r10, #0x20]
+	str	r1, [r10, #0x24]
+
+	/* save disagnostic register */
+	mrc	p15, 0, r7, c15, c0, 1
+	str	r7, [r0, #PM_INFO_MX6Q_SAVED_DIAGNOSTIC_OFFSET]
+
+	/* set ARM power to be gated */
+	ldr	r10, [r0, #PM_INFO_MX6Q_GPC_V_OFFSET]
+	ldr	r7, =0x1
+	str	r7, [r10, #0x2a0]
+
+	disable_l1_dcache
+
+#ifdef CONFIG_CACHE_L2X0
+	/* sync L2 */
+	ldr	r10, [r0, #PM_INFO_MX6Q_L2_V_OFFSET]
+
+	/* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+	ldr	r7, [r10, #0x730]
+	cmp	r7, #0x0
+	bne	wait_for_l2_to_idle
+
+	mov	r7, #0x0
+	str	r7, [r10, #0x730]
+	/* disable L2 */
+	str	r7, [r10, #0x100]
+
+	dsb
+	isb
+#endif
+
+	tlb_set_to_ocram
+
+	/* make sure MMDC in self-refresh */
+	ldr	r10, [r0, #PM_INFO_MX6Q_MMDC_V_OFFSET]
+	mmdc_enter_dvfs_mode
+
+	/* save DDR IO settings */
+	ldr     r10, [r0, #PM_INFO_MX6Q_IOMUXC_V_OFFSET]
+	ldr     r6, =0x0
+	ldr     r7, [r0, #PM_INFO_MMDC_IO_NUM_OFFSET]
+	ldr     r8, =PM_INFO_MMDC_IO_VAL_OFFSET
+	add     r8, r8, r0
+save_and_set_mmdc_io_lpm:
+	ldr	r9, [r8], #0x4
+	ldr	r5, [r10, r9]
+	str	r6, [r10, r9]
+	str	r5, [r8], #0x4
+	subs	r7, r7, #0x1
+	bne	save_and_set_mmdc_io_lpm
+
+	mov	r5, #0x0
+	ccm_enter_idle
+	anatop_enter_idle
+
+	/*
+	 * mask all GPC interrupts before
+	 * enabling the RBC counters to
+	 * avoid the counter starting too
+	 * early if an interupt is already
+	 * pending.
+	 */
+	ldr     r10, [r0, #PM_INFO_MX6Q_GPC_V_OFFSET]
+	ldr	r4, [r10, #MX6Q_GPC_IMR1]
+	ldr	r5, [r10, #MX6Q_GPC_IMR2]
+	ldr	r6, [r10, #MX6Q_GPC_IMR3]
+	ldr	r7, [r10, #MX6Q_GPC_IMR4]
+
+	ldr	r3, =0xffffffff
+	str	r3, [r10, #MX6Q_GPC_IMR1]
+	str	r3, [r10, #MX6Q_GPC_IMR2]
+	str	r3, [r10, #MX6Q_GPC_IMR3]
+	str	r3, [r10, #MX6Q_GPC_IMR4]
+
+	/*
+	 * enable the RBC bypass counter here
+	 * to hold off the interrupts. RBC counter
+	 * = 1 (30us). With this setting, the latency
+	 * from wakeup interrupt to ARM power up
+	 * is ~40uS.
+	 */
+	ldr     r10, [r0, #PM_INFO_MX6Q_CCM_V_OFFSET]
+	ldr	r3, [r10, #MX6Q_CCM_CCR]
+	bic	r3, r3, #(0x3f << 21)
+	orr	r3, r3, #(0x1 << 21)
+	str	r3, [r10, #MX6Q_CCM_CCR]
+
+	/* enable the counter. */
+	ldr	r3, [r10, #MX6Q_CCM_CCR]
+	orr	r3, r3, #(0x1 << 27)
+	str	r3, [r10, #MX6Q_CCM_CCR]
+
+	/* unmask all the GPC interrupts. */
+	ldr     r10, [r0, #PM_INFO_MX6Q_GPC_V_OFFSET]
+	str	r4, [r10, #MX6Q_GPC_IMR1]
+	str	r5, [r10, #MX6Q_GPC_IMR2]
+	str	r6, [r10, #MX6Q_GPC_IMR3]
+	str	r7, [r10, #MX6Q_GPC_IMR4]
+
+	/*
+	 * now delay for a short while (3usec)
+	 * ARM is at 1GHz at this point
+	 * so a short loop should be enough.
+	 * this delay is required to ensure that
+	 * the RBC counter can start counting in
+	 * case an interrupt is already pending
+	 * or in case an interrupt arrives just
+	 * as ARM is about to assert DSM_request.
+	 */
+	ldr	r4, =2000
+rbc_loop:
+	subs	r4, r4, #0x1
+	bne	rbc_loop
+
+	wfi
+
+	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
+
+	mov	r5, #0x0
+	anatop_exit_idle
+	ccm_exit_idle
+	resume_mmdc
+
+	/* clear ARM power gate setting */
+	ldr	r10, [r0, #PM_INFO_MX6Q_GPC_V_OFFSET]
+	ldr	r7, =0x0
+	str	r7, [r10, #0x2a0]
+
+	/* enable d-cache */
+	mrc	p15, 0, r7, c1, c0, 0
+	orr	r7, r7, #(1 << 2)
+	mcr	p15, 0, r7, c1, c0, 0
+
+#ifdef CONFIG_CACHE_L2X0
+	ldr	r10, [r0, #PM_INFO_MX6Q_L2_V_OFFSET]
+	mov	r7, #0x1
+	/* enable L2 */
+	str	r7, [r10, #0x100]
+#endif
+
+	tlb_back_to_ddr
+
+	/* Restore registers */
+	pop	{r4 - r10}
+	mov	pc, lr
+
+wakeup:
+
+	/* invalidate L1 I-cache first */
+	mov	r1, #0x0
+	mcr	p15, 0, r1, c7, c5, 0
+	mcr	p15, 0, r1, c7, c5, 0
+	mcr	p15, 0, r1, c7, c5, 6
+	/* enable the Icache and branch prediction */
+	mov	r1, #0x1800
+	mcr	p15, 0, r1, c1, c0, 0
+	isb
+	/* restore disagnostic register */
+	ldr	r7, [r0, #PM_INFO_MX6Q_SAVED_DIAGNOSTIC_OFFSET]
+	mcr	p15, 0, r7, c15, c0, 1
+
+	/* get physical resume address from pm_info. */
+	ldr	lr, [r0, #PM_INFO_RESUME_ADDR_OFFSET]
+	/* clear core0's entry and parameter */
+	ldr	r10, [r0, #PM_INFO_MX6Q_SRC_P_OFFSET]
+	mov	r7, #0x0
+	str	r7, [r10, #MX6Q_SRC_GPR1]
+	str	r7, [r10, #MX6Q_SRC_GPR2]
+
+	/* clear ARM power gate setting */
+	ldr	r10, [r0, #PM_INFO_MX6Q_GPC_P_OFFSET]
+	ldr	r7, =0x0
+	str	r7, [r10, #0x2a0]
+
+	mov	r5, #0x1
+	anatop_exit_idle
+	ccm_exit_idle
+	resume_mmdc
+
+	/* Restore registers */
+	mov	pc, lr
+	.ltorg
+mx6sx_lpm_wfi_end:
diff --git a/arch/arm/mach-imx/mach-imx6sx.c b/arch/arm/mach-imx/mach-imx6sx.c
index 6b60a74c7be2..3ec73b407cf2 100755
--- a/arch/arm/mach-imx/mach-imx6sx.c
+++ b/arch/arm/mach-imx/mach-imx6sx.c
@@ -20,6 +20,7 @@
 #include <asm/mach/map.h>
 
 #include "common.h"
+#include "cpuidle.h"
 
 static struct flexcan_platform_data flexcan_pdata[2];
 static int flexcan_en_gpio;
@@ -208,6 +209,8 @@ static void __init imx6sx_init_late(void)
 
 	if (of_machine_is_compatible("fsl,imx6sx-sdb"))
 		imx6sx_arm2_flexcan_fixup();
+
+	imx6sx_cpuidle_init();
 }
 
 static void __init imx6sx_map_io(void)
diff --git a/arch/arm/mach-imx/mx6.h b/arch/arm/mach-imx/mx6.h
index d4b516aff5d2..b4bfc28369bd 100644
--- a/arch/arm/mach-imx/mx6.h
+++ b/arch/arm/mach-imx/mx6.h
@@ -43,4 +43,5 @@
 #define MX6SL_WFI_IRAM_DATA_SIZE    100
 
 #define MX6_SUSPEND_IRAM_ADDR_OFFSET        0
+#define MX6_CPUIDLE_IRAM_ADDR_OFFSET        0x1000
 #endif
diff --git a/arch/arm/mach-imx/pm-imx6.c b/arch/arm/mach-imx/pm-imx6.c
index 5bff81ac2956..e191bc572582 100644
--- a/arch/arm/mach-imx/pm-imx6.c
+++ b/arch/arm/mach-imx/pm-imx6.c
@@ -235,15 +235,24 @@ void restore_ttbr1(unsigned long ttbr1)
 	);
 }
 
-void imx6q_set_int_mem_clk_lpm(void)
+void imx6q_set_int_mem_clk_lpm(bool enable)
 {
-	u32 val = readl_relaxed(ccm_base + CGPR);
-
-	val |= BM_CGPR_INT_MEM_CLK_LPM;
-	writel_relaxed(val, ccm_base + CGPR);
+	if ((cpu_is_imx6q() && imx_get_soc_revision() >
+		IMX_CHIP_REVISION_1_1) ||
+		(cpu_is_imx6dl() && imx_get_soc_revision() >
+		IMX_CHIP_REVISION_1_0) || cpu_is_imx6sx()) {
+		u32 val;
+
+		val = readl_relaxed(ccm_base + CGPR);
+		if (enable)
+			val |= BM_CGPR_INT_MEM_CLK_LPM;
+		else
+			val &= ~BM_CGPR_INT_MEM_CLK_LPM;
+		writel_relaxed(val, ccm_base + CGPR);
+	}
 }
 
-static void imx6q_enable_rbc(bool enable)
+void imx6q_enable_rbc(bool enable)
 {
 	u32 val;