arm: imx: add low power idle for imx6ul

Add the low power idle support for i.MX6UL.

Signed-off-by: Jacky Bai <ping.bai@nxp.com>

6 files changed, 1151 insertions, 2 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index e6ce2ffda10a..519606aa8a08 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -24,7 +24,8 @@ obj-$(CONFIG_SOC_IMX6SL) += cpuidle-imx6sl.o imx6sl_low_power_idle.o
 obj-$(CONFIG_SOC_IMX6SLL) += cpuidle-imx6sx.o imx6sx_low_power_idle.o
 obj-$(CONFIG_SOC_IMX6SX) += cpuidle-imx6sx.o
 AFLAGS_imx6sx_low_power_idle.o :=-Wa,-march=armv7-a
-obj-$(CONFIG_SOC_IMX6UL) += cpuidle-imx6sx.o
+AFLAGS_imx6ul_low_power_idle.o :=-Wa,-march=armv7-a
+obj-$(CONFIG_SOC_IMX6UL) += cpuidle-imx6ul.o imx6ul_low_power_idle.o
 obj-$(CONFIG_SOC_IMX7ULP) += cpuidle-imx7ulp.o
 AFLAGS_imx7d_low_power_idle.o :=-Wa,-march=armv7-a
 obj-$(CONFIG_SOC_IMX7D_CA7) += cpuidle-imx7d.o imx7d_low_power_idle.o
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index 66e5f7f1c289..1e335503e919 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -113,6 +113,7 @@ void imx_mu_set_m4_run_mode(void);
 void imx_src_init(void);
 void imx_gpc_pre_suspend(bool arm_power_off);
 void imx_gpc_post_resume(void);
+void imx_gpc_switch_pupscr_clk(bool flag);
 void imx_gpc_mask_all(void);
 void imx_gpc_restore_all(void);
 void imx_gpc_hwirq_mask(unsigned int hwirq);
@@ -125,6 +126,7 @@ int imx6_set_lpm(enum mxc_cpu_pwr_mode mode);
 void imx6_set_int_mem_clk_lpm(bool enable);
 void imx6sl_low_power_idle(void);
 void imx6sx_low_power_idle(void);
+void imx6ul_low_power_idle(void);
 void imx7d_low_power_idle(void);
 #ifdef CONFIG_HAVE_IMX_MMDC
 int imx_mmdc_get_ddr_type(void);
diff --git a/arch/arm/mach-imx/cpuidle-imx6ul.c b/arch/arm/mach-imx/cpuidle-imx6ul.c
new file mode 100644
index 000000000000..64e14f69fcca
--- /dev/null
+++ b/arch/arm/mach-imx/cpuidle-imx6ul.c
@@ -0,0 +1,320 @@
+/*
+ * Copyright (C) 2015-2016 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/busfreq-imx.h>
+#include <linux/cpuidle.h>
+#include <linux/cpu_pm.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/psci.h>
+#include <linux/regulator/consumer.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <asm/cpuidle.h>
+#include <asm/fncpy.h>
+#include <asm/proc-fns.h>
+#include <asm/suspend.h>
+
+#include <uapi/linux/psci.h>
+
+#include "common.h"
+#include "cpuidle.h"
+#include "hardware.h"
+
+#define MAX_MMDC_IO_NUM		14
+
+#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
+
+extern unsigned long iram_tlb_phys_addr;
+static void __iomem *wfi_iram_base;
+
+#ifdef CONFIG_CPU_FREQ
+static void __iomem *wfi_iram_base_phys;
+extern unsigned long mx6ul_lpm_wfi_start asm("mx6ul_lpm_wfi_start");
+extern unsigned long mx6ul_lpm_wfi_end asm("mx6ul_lpm_wfi_end");
+extern unsigned long mx6ull_lpm_wfi_start asm("mx6ull_lpm_wfi_start");
+extern unsigned long mx6ull_lpm_wfi_end asm("mx6ull_lpm_wfi_end");
+#endif
+
+struct imx6_pm_base {
+	phys_addr_t pbase;
+	void __iomem *vbase;
+};
+
+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 anatop_base;
+	struct imx6_pm_base src_base;
+	u32 mmdc_io_num; /* Number of MMDC IOs which need saved/restored. */
+	u32 mmdc_io_val[MAX_MMDC_IO_NUM][2]; /* To save offset and value */
+} __aligned(8);
+
+static const u32 imx6ul_mmdc_io_offset[] __initconst = {
+	0x244, 0x248, 0x24c, 0x250, /* DQM0, DQM1, RAS, CAS */
+	0x27c, 0x498, 0x4a4, 0x490, /* SDCLK0, GPR_B0DS-B1DS, GPR_ADDS */
+	0x280, 0x284, 0x260, 0x264, /* SDQS0~1, SODT0, SODT1 */
+	0x494, 0x4b0,	            /* MODE_CTL, MODE, */
+};
+
+static void (*imx6ul_wfi_in_iram_fn)(void __iomem *iram_vbase);
+
+#define MX6UL_POWERDWN_IDLE_PARAM	\
+	((1 << PSCI_0_2_POWER_STATE_ID_SHIFT) | \
+	 (1 << PSCI_0_2_POWER_STATE_AFFL_SHIFT) | \
+	 (PSCI_POWER_STATE_TYPE_POWER_DOWN << PSCI_0_2_POWER_STATE_TYPE_SHIFT))
+
+static int imx6ul_idle_finish(unsigned long val)
+{
+	if (psci_ops.cpu_suspend)
+		psci_ops.cpu_suspend(MX6UL_POWERDWN_IDLE_PARAM,
+				     __pa(cpu_resume));
+	else
+		imx6ul_wfi_in_iram_fn(wfi_iram_base);
+
+	return 0;
+}
+
+static int imx6ul_enter_wait(struct cpuidle_device *dev,
+			    struct cpuidle_driver *drv, int index)
+{
+	int mode = get_bus_freq_mode();
+
+	imx6_set_lpm(WAIT_UNCLOCKED);
+	if ((index == 1) || ((mode != BUS_FREQ_LOW) && index == 2)) {
+		cpu_do_idle();
+		index = 1;
+	} else {
+		/*
+		 * i.MX6UL TO1.0 ARM power up uses IPG/2048 as clock source,
+		 * from TO1.1, PGC_CPU_PUPSCR bit [5] is re-defined to switch
+		 * clock to IPG/32, enable this bit to speed up the ARM power
+		 * up process in low power idle case.
+		 */
+		if (cpu_is_imx6ul() && imx_get_soc_revision() >
+			IMX_CHIP_REVISION_1_0)
+			imx_gpc_switch_pupscr_clk(true);
+		/* Need to notify there is a cpu pm operation. */
+		cpu_pm_enter();
+		cpu_cluster_pm_enter();
+
+		cpu_suspend(0, imx6ul_idle_finish);
+
+		cpu_cluster_pm_exit();
+		cpu_pm_exit();
+		imx6_enable_rbc(false);
+
+		if (cpu_is_imx6ul() && imx_get_soc_revision() >
+			IMX_CHIP_REVISION_1_0)
+			imx_gpc_switch_pupscr_clk(false);
+	}
+
+	imx6_set_lpm(WAIT_CLOCKED);
+
+	return index;
+}
+
+static struct cpuidle_driver imx6ul_cpuidle_driver_v2 = {
+	.name = "imx6ul_cpuidle",
+	.owner = THIS_MODULE,
+	.states = {
+		/* WFI */
+		ARM_CPUIDLE_WFI_STATE,
+		/* WAIT */
+		{
+			.exit_latency = 50,
+			.target_residency = 75,
+			.enter = imx6ul_enter_wait,
+			.name = "WAIT",
+			.desc = "Clock off",
+		},
+		/* LOW POWER IDLE */
+		{
+			/*
+			 * RBC 130us + ARM gating 43us + RBC clear 65us
+			 * + PLL2 relock 450us and some margin, here set
+			 * it to 700us.
+			 */
+			.exit_latency = 700,
+			.target_residency = 1000,
+			.enter = imx6ul_enter_wait,
+			.name = "LOW-POWER-IDLE",
+			.desc = "ARM power off",
+		}
+	},
+	.state_count = 3,
+	.safe_state_index = 0,
+};
+
+static struct cpuidle_driver imx6ul_cpuidle_driver = {
+	.name = "imx6ul_cpuidle",
+	.owner = THIS_MODULE,
+	.states = {
+		/* WFI */
+		ARM_CPUIDLE_WFI_STATE,
+		/* WAIT */
+		{
+			.exit_latency = 50,
+			.target_residency = 75,
+			.enter = imx6ul_enter_wait,
+			.name = "WAIT",
+			.desc = "Clock off",
+		},
+		/* LOW POWER IDLE */
+		{
+			/*
+			 * RBC 130us + ARM gating 1370us + RBC clear 65us
+			 * + PLL2 relock 450us and some margin, here set
+			 * it to 2100us.
+			 */
+			.exit_latency = 2100,
+			.target_residency = 2500,
+			.enter = imx6ul_enter_wait,
+			.name = "LOW-POWER-IDLE",
+			.desc = "ARM power off",
+		}
+	},
+	.state_count = 3,
+	.safe_state_index = 0,
+};
+
+int __init imx6ul_cpuidle_init(void)
+{
+	void __iomem *anatop_base = (void __iomem *)IMX_IO_P2V(MX6Q_ANATOP_BASE_ADDR);
+	u32 val;
+#ifdef CONFIG_CPU_FREQ
+	struct imx6_cpuidle_pm_info *cpuidle_pm_info;
+	int i;
+	const u32 *mmdc_offset_array;
+	u32 wfi_code_size;
+
+	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(imx6ul_mmdc_io_offset);
+	mmdc_offset_array = imx6ul_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->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];
+
+	/* calculate the wfi code size */
+	wfi_code_size = (&mx6ul_lpm_wfi_end -&mx6ul_lpm_wfi_start) *4;
+
+	imx6ul_wfi_in_iram_fn = (void *)fncpy(wfi_iram_base + sizeof(*cpuidle_pm_info),
+		&imx6ul_low_power_idle, wfi_code_size);
+#endif
+
+	imx6_set_int_mem_clk_lpm(true);
+
+	/*
+	 * 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(anatop_base + PMU_LOW_PWR_CTRL);
+	val |= 0x1;
+	writel_relaxed(val, anatop_base + 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,
+		anatop_base + 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,
+		anatop_base + 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(anatop_base + 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, anatop_base  + XTALOSC24M_OSC_CONFIG0);
+	/* set the count_1m_trg = 0x2d7 */
+	val = readl_relaxed(anatop_base  + 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, anatop_base  + 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(anatop_base  + XTALOSC24M_OSC_CONFIG1);
+
+	/* ARM power up time is reduced since TO1.1 */
+	if (imx_get_soc_revision() > IMX_CHIP_REVISION_1_0)
+		return cpuidle_register(&imx6ul_cpuidle_driver_v2, NULL);
+	else
+		return cpuidle_register(&imx6ul_cpuidle_driver, NULL);
+}
diff --git a/arch/arm/mach-imx/cpuidle.h b/arch/arm/mach-imx/cpuidle.h
index 1d3c93f3f6b9..d011da14d4c6 100644
--- a/arch/arm/mach-imx/cpuidle.h
+++ b/arch/arm/mach-imx/cpuidle.h
@@ -9,6 +9,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);
+extern int imx6ul_cpuidle_init(void);
 extern int imx7d_cpuidle_init(void);
 extern int imx7d_enable_rcosc(void);
 extern int imx7ulp_cpuidle_init(void);
@@ -29,6 +30,10 @@ static inline int imx6sx_cpuidle_init(void)
 {
 	return 0;
 }
+static inline int imx6ul_cpuidle_init(void)
+{
+	return 0;
+}
 static inline int imx7d_cpuidle_init(void)
 {
 	return 0;
diff --git a/arch/arm/mach-imx/imx6ul_low_power_idle.S b/arch/arm/mach-imx/imx6ul_low_power_idle.S
new file mode 100644
index 000000000000..26bb83da1a7d
--- /dev/null
+++ b/arch/arm/mach-imx/imx6ul_low_power_idle.S
@@ -0,0 +1,821 @@
+/*
+ * Copyright (C) 2015 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_ANATOP_P_OFFSET		0x30
+#define PM_INFO_MX6Q_ANATOP_V_OFFSET		0x34
+#define PM_INFO_MX6Q_SRC_P_OFFSET		0x38
+#define PM_INFO_MX6Q_SRC_V_OFFSET		0x3c
+#define PM_INFO_MMDC_IO_NUM_OFFSET		0x40
+#define PM_INFO_MMDC_IO_VAL_OFFSET		0x44
+
+#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 mx6ul_lpm_wfi_start
+.globl mx6ul_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]
+
+	/* bypass PLL1 output to OSC */
+	ldr	r7, [r10]
+	orr	r7, r7, #(0x1 << 16)
+	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]
+
+	/* Unbypass PLL1 */
+	ldr	r7, [r10]
+	bic	r7, r7, #(0x1 << 16)
+	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]
+
+	/* 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]
+
+	/* disconnect vdd_high_in and vdd_snvs_in */
+	ldr	r7, [r10, #0x150]
+	orr	r7, r7, #0x1000
+	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]
+
+	/* 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
+
+/* imx6ul_low_power_idle */
+
+	.align 3
+ENTRY(imx6ul_low_power_idle)
+mx6ul_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, =imx6ul_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]
+
+	/* 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
+
+	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
+	 * = 4 (120us). With this setting, the latency
+	 * from wakeup interrupt to ARM power up
+	 * is ~130uS.
+	 */
+	ldr     r10, [r0, #PM_INFO_MX6Q_CCM_V_OFFSET]
+	ldr	r3, [r10, #MX6Q_CCM_CCR]
+	bic	r3, r3, #(0x3f << 21)
+	orr	r3, r3, #(0x4 << 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 24MHz 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, =50
+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
+
+	/* clear ARM power gate setting */
+	ldr	r10, [r0, #PM_INFO_MX6Q_GPC_V_OFFSET]
+	ldr	r7, =0x0
+	str	r7, [r10, #0x2a0]
+
+	resume_mmdc
+	/* enable d-cache */
+	mrc	p15, 0, r7, c1, c0, 0
+	orr	r7, r7, #(1 << 2)
+	mcr	p15, 0, r7, c1, c0, 0
+
+	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
+
+	/* 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
+mx6ul_lpm_wfi_end:
diff --git a/arch/arm/mach-imx/mach-imx6ul.c b/arch/arm/mach-imx/mach-imx6ul.c
index 075bb43be034..3e37a4a1de9b 100644
--- a/arch/arm/mach-imx/mach-imx6ul.c
+++ b/arch/arm/mach-imx/mach-imx6ul.c
@@ -54,7 +54,7 @@ static void __init imx6ul_init_irq(void)
 
 static void __init imx6ul_init_late(void)
 {
-	imx6sx_cpuidle_init();
+	imx6ul_cpuidle_init();
 
 	if (IS_ENABLED(CONFIG_ARM_IMX6Q_CPUFREQ))
 		platform_device_register_simple("imx6q-cpufreq", -1, NULL, 0);