MLK-11357-2 ARM: imx: add cpuidle support for imx7d

Enable cpuidle for i.MX7D, total 3 level idle supported:

1. ARM WFI;
2. WAIT mode;
3. Low power idle with ARM/SCU platform power off.

Only when system in low bus freq mode, system is able to
enter low power idle, and only when both of 2 cores are
in low power idle, ARM/SCU platform will be powered off.

DDR will be put into low power mode when low power idle
is entered.

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

7 files changed, 976 insertions, 1 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 0f790c905a1f..6e3e68706be9 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -28,6 +28,8 @@ obj-$(CONFIG_SOC_IMX6Q) += cpuidle-imx6q.o
 obj-$(CONFIG_SOC_IMX6SL) += cpuidle-imx6sl.o
 obj-$(CONFIG_SOC_IMX6SX) += cpuidle-imx6sx.o
 obj-$(CONFIG_SOC_IMX6UL) += cpuidle-imx6sx.o
+AFLAGS_imx7d_low_power_idle.o :=-Wa,-march=armv7-a
+obj-$(CONFIG_SOC_IMX7D) += cpuidle-imx7d.o imx7d_low_power_idle.o
 endif
 
 ifdef CONFIG_SND_IMX_SOC
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index c3f67f14bbb8..3247493e912c 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -103,6 +103,8 @@ void imx_gpcv2_post_resume(void);
 void imx_gpcv2_set_core1_pdn_pup_by_software(bool pdn);
 unsigned int imx_gpcv2_is_mf_mix_off(void);
 void __init imx_gpcv2_check_dt(void);
+void imx_gpcv2_set_lpm_mode(enum mxc_cpu_pwr_mode mode);
+void imx_gpcv2_set_cpu_power_gate_in_idle(bool pdn);
 void imx_gpc_mask_all(void);
 void imx_gpc_restore_all(void);
 void imx_gpc_hwirq_mask(unsigned int hwirq);
@@ -119,6 +121,7 @@ int imx_ddrc_get_ddr_type(void);
 void imx_cpu_die(unsigned int cpu);
 int imx_cpu_kill(unsigned int cpu);
 void imx_busfreq_map_io(void);
+void imx7d_low_power_idle(void);
 
 #ifdef CONFIG_SUSPEND
 void v7_cpu_resume(void);
diff --git a/arch/arm/mach-imx/cpuidle-imx7d.c b/arch/arm/mach-imx/cpuidle-imx7d.c
new file mode 100644
index 000000000000..f92b76b19b14
--- /dev/null
+++ b/arch/arm/mach-imx/cpuidle-imx7d.c
@@ -0,0 +1,264 @@
+/*
+ * Copyright (C) 2015 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/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/cp15.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 XTALOSC24M_OSC_CONFIG0	0x10
+#define XTALOSC24M_OSC_CONFIG1	0x20
+#define XTALOSC24M_OSC_CONFIG2	0x30
+#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
+
+#define XTALOSC_CTRL_24M				0x0
+#define XTALOSC_CTRL_24M_RC_OSC_EN_SHIFT		13
+#define REG_SET						0x4
+
+static void __iomem *wfi_iram_base;
+static void __iomem *wfi_iram_base_phys;
+extern unsigned long iram_tlb_phys_addr;
+
+struct imx7_pm_base {
+	phys_addr_t pbase;
+	void __iomem *vbase;
+};
+
+struct imx7_cpuidle_pm_info {
+	phys_addr_t vbase; /* The virtual address of 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;
+	int last_cpu;
+	u32 ttbr;
+	u32 cpu1_wfi;
+	u32 lpi_enter;
+	u32 val;
+	u32 flag0;
+	u32 flag1;
+	struct imx7_pm_base ddrc_base;
+	struct imx7_pm_base ccm_base;
+	struct imx7_pm_base anatop_base;
+	struct imx7_pm_base src_base;
+} __aligned(8);
+
+static atomic_t master_lpi = ATOMIC_INIT(0);
+static atomic_t master_wait = ATOMIC_INIT(0);
+
+static void (*imx7d_wfi_in_iram_fn)(void __iomem *iram_vbase);
+static struct imx7_cpuidle_pm_info *cpuidle_pm_info;
+
+static int imx7d_idle_finish(unsigned long val)
+{
+	imx7d_wfi_in_iram_fn(wfi_iram_base);
+	return 0;
+}
+
+static int imx7d_enter_low_power_idle(struct cpuidle_device *dev,
+			    struct cpuidle_driver *drv, int index)
+{
+	int mode = get_bus_freq_mode();
+
+	if ((index == 1) || ((mode != BUS_FREQ_LOW) && index == 2)) {
+		if (atomic_inc_return(&master_wait) == num_online_cpus())
+			imx_gpcv2_set_lpm_mode(WAIT_UNCLOCKED);
+
+		cpu_do_idle();
+
+		atomic_dec(&master_wait);
+		imx_gpcv2_set_lpm_mode(WAIT_CLOCKED);
+	} else {
+		imx_gpcv2_set_lpm_mode(WAIT_UNCLOCKED);
+		cpu_pm_enter();
+
+		if (atomic_inc_return(&master_lpi) < num_online_cpus()) {
+			imx_set_cpu_jump(dev->cpu, ca7_cpu_resume);
+			/* initialize the last cpu id to invalid here */
+			cpuidle_pm_info->last_cpu = -1;
+			cpu_suspend(0, imx7d_idle_finish);
+		} else {
+			imx_gpcv2_set_cpu_power_gate_in_idle(true);
+			cpu_cluster_pm_enter();
+
+			cpuidle_pm_info->last_cpu = dev->cpu;
+			cpu_suspend(0, imx7d_idle_finish);
+
+			cpu_cluster_pm_exit();
+			imx_gpcv2_set_cpu_power_gate_in_idle(false);
+		}
+		atomic_dec(&master_lpi);
+
+		cpu_pm_exit();
+		imx_gpcv2_set_lpm_mode(WAIT_CLOCKED);
+	}
+
+	return index;
+}
+
+static struct cpuidle_driver imx7d_cpuidle_driver = {
+	.name = "imx7d_cpuidle",
+	.owner = THIS_MODULE,
+	.states = {
+		/* WFI */
+		ARM_CPUIDLE_WFI_STATE,
+		/* WAIT MODE */
+		{
+			.exit_latency = 50,
+			.target_residency = 75,
+			.flags = CPUIDLE_FLAG_TIMER_STOP,
+			.enter = imx7d_enter_low_power_idle,
+			.name = "WAIT",
+			.desc = "Clock off",
+		},
+		/* LOW POWER IDLE */
+		{
+			.exit_latency = 100,
+			.target_residency = 200,
+			.flags = CPUIDLE_FLAG_TIMER_STOP,
+			.enter = imx7d_enter_low_power_idle,
+			.name = "LOW-POWER-IDLE",
+			.desc = "ARM power off",
+		},
+	},
+	.state_count = 3,
+	.safe_state_index = 0,
+};
+
+int imx7d_enable_rcosc(void)
+{
+	void __iomem *anatop_base =
+		(void __iomem *)IMX_IO_P2V(MX7D_ANATOP_BASE_ADDR);
+	u32 val;
+
+	imx_gpcv2_set_lpm_mode(WAIT_CLOCKED);
+	/* set RC-OSC freq and turn it on */
+	writel_relaxed(0x1 << XTALOSC_CTRL_24M_RC_OSC_EN_SHIFT,
+		anatop_base + XTALOSC_CTRL_24M + REG_SET);
+	/*
+	 * 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 at least 4ms according to hardware design */
+	mdelay(6);
+	/*
+	 * 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);
+	writel_relaxed(val, anatop_base + XTALOSC24M_OSC_CONFIG1);
+
+	return 0;
+}
+
+int __init imx7d_cpuidle_init(void)
+{
+	wfi_iram_base_phys = (void *)(iram_tlb_phys_addr +
+		MX7_CPUIDLE_OCRAM_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->vbase = (phys_addr_t) 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(ca7_cpu_resume);
+	cpuidle_pm_info->cpu1_wfi = 0;
+	cpuidle_pm_info->lpi_enter = 0;
+	/* initialize the last cpu id to invalid here */
+	cpuidle_pm_info->last_cpu = -1;
+
+	cpuidle_pm_info->ddrc_base.pbase = MX7D_DDRC_BASE_ADDR;
+	cpuidle_pm_info->ddrc_base.vbase =
+		(void __iomem *)IMX_IO_P2V(MX7D_DDRC_BASE_ADDR);
+
+	cpuidle_pm_info->ccm_base.pbase = MX7D_CCM_BASE_ADDR;
+	cpuidle_pm_info->ccm_base.vbase =
+		(void __iomem *)IMX_IO_P2V(MX7D_CCM_BASE_ADDR);
+
+	cpuidle_pm_info->anatop_base.pbase = MX7D_ANATOP_BASE_ADDR;
+	cpuidle_pm_info->anatop_base.vbase =
+		(void __iomem *)IMX_IO_P2V(MX7D_ANATOP_BASE_ADDR);
+
+	cpuidle_pm_info->src_base.pbase = MX7D_SRC_BASE_ADDR;
+	cpuidle_pm_info->src_base.vbase =
+		(void __iomem *)IMX_IO_P2V(MX7D_SRC_BASE_ADDR);
+
+	imx7d_enable_rcosc();
+
+	/* code size should include cpuidle_pm_info size */
+	imx7d_wfi_in_iram_fn = (void *)fncpy(wfi_iram_base +
+		sizeof(*cpuidle_pm_info),
+		&imx7d_low_power_idle,
+		MX7_CPUIDLE_OCRAM_SIZE - sizeof(*cpuidle_pm_info));
+
+	return cpuidle_register(&imx7d_cpuidle_driver, NULL);
+}
diff --git a/arch/arm/mach-imx/cpuidle.h b/arch/arm/mach-imx/cpuidle.h
index f9140128ba05..6106fc9a0cd9 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-2015 Freescale Semiconductor, Inc.
  * Copyright 2012 Linaro Ltd.
  *
  * The code contained herein is licensed under the GNU General Public
@@ -15,6 +15,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 imx7d_cpuidle_init(void);
 #else
 static inline int imx5_cpuidle_init(void)
 {
@@ -32,4 +33,8 @@ static inline int imx6sx_cpuidle_init(void)
 {
 	return 0;
 }
+static inline int imx7d_cpuidle_init(void)
+{
+	return 0;
+}
 #endif
diff --git a/arch/arm/mach-imx/imx7d_low_power_idle.S b/arch/arm/mach-imx/imx7d_low_power_idle.S
new file mode 100644
index 000000000000..070b457c8fc1
--- /dev/null
+++ b/arch/arm/mach-imx/imx7d_low_power_idle.S
@@ -0,0 +1,697 @@
+/*
+ * 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_VBASE_OFFSET			0x0
+#define PM_INFO_PBASE_OFFSET			0x4
+#define PM_INFO_RESUME_ADDR_OFFSET		0x8
+#define PM_INFO_PM_INFO_SIZE_OFFSET		0xc
+#define PM_INFO_PM_INFO_LAST_CPU_OFFSET		0x10
+#define PM_INFO_PM_INFO_TTBR_OFFSET		0x14
+#define PM_INFO_PM_INFO_CPU1_IN_WFI_OFFSET	0x18
+#define PM_INFO_PM_INFO_LPI_ENTER_OFFSET	0x1c
+#define PM_INFO_VAL_OFFSET			0x20
+#define PM_INFO_FLAG0_OFFSET			0x24
+#define PM_INFO_FLAG1_OFFSET			0x28
+#define PM_INFO_MX7D_DDRC_P_OFFSET		0x2c
+#define PM_INFO_MX7D_DDRC_V_OFFSET		0x30
+#define PM_INFO_MX7D_CCM_P_OFFSET		0x34
+#define PM_INFO_MX7D_CCM_V_OFFSET		0x38
+#define PM_INFO_MX7D_ANATOP_P_OFFSET		0x3c
+#define PM_INFO_MX7D_ANATOP_V_OFFSET		0x40
+#define PM_INFO_MX7D_SRC_P_OFFSET		0x44
+#define PM_INFO_MX7D_SRC_V_OFFSET		0x48
+
+#define MX7D_SRC_GPR1	0x74
+#define MX7D_SRC_GPR2	0x78
+#define MX7D_SRC_GPR3	0x7c
+#define MX7D_SRC_GPR4	0x80
+#define DDRC_STAT	0x4
+#define DDRC_PWRCTL	0x30
+#define DDRC_PSTAT	0x3fc
+
+/*
+ * imx_pen_lock
+ *
+ * The reference link of Peterson's algorithm:
+ * http://en.wikipedia.org/wiki/Peterson's_algorithm
+ *
+ * val1 = r1 = !turn (inverted from Peterson's algorithm)
+ * on cpu 0:
+ * r2 = flag[0] (in flag0)
+ * r3 = flag[1] (in flag1)
+ * on cpu1:
+ * r2 = flag[1] (in flag1)
+ * r3 = flag[0] (in flag0)
+ *
+ */
+	.macro	imx_pen_lock
+
+	mov	r8, r0
+	mrc	p15, 0, r5, c0, c0, 5
+	and	r5, r5, #3
+	add	r6, r8, #PM_INFO_VAL_OFFSET
+	cmp	r5, #0
+	addeq	r7, r8, #PM_INFO_FLAG0_OFFSET
+	addeq	r8, r8, #PM_INFO_FLAG1_OFFSET
+	addne	r7, r8, #PM_INFO_FLAG1_OFFSET
+	addne	r8, r8, #PM_INFO_FLAG0_OFFSET
+
+	mov	r9, #1
+	str	r9, [r7]
+	dsb
+	str	r5, [r6]
+1:
+	dsb
+	ldr	r9, [r8]
+	cmp	r9, #1
+	ldreq	r9, [r6]
+	cmpeq	r9, r5
+	beq	1b
+
+	.endm
+
+	.macro	imx_pen_unlock
+
+	dsb
+	mrc	p15, 0, r6, c0, c0, 5
+	and	r6, r6, #3
+	cmp	r6, #0
+	addeq	r7, r0, #PM_INFO_FLAG0_OFFSET
+	addne	r7, r0, #PM_INFO_FLAG1_OFFSET
+	mov	r9, #0
+	str	r9, [r7]
+
+	.endm
+
+	.macro  disable_l1_dcache
+
+	push	{r0 - r12, lr}
+	ldr	r7, =v7_flush_dcache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r12, 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 - r12, lr}
+	ldr	r7, =v7_flush_dcache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r12, lr}
+
+#ifdef CONFIG_SMP
+	clrex
+
+	/* Turn off SMP bit. */
+	mrc	p15, 0, r8, c1, c0, 1
+	bic	r8, r8, #0x40
+	mcr	p15, 0, r8, c1, c0, 1
+
+	isb
+	dsb
+#endif
+
+	.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.
+	 */
+
+	/* 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
+
+	/* Flush the BTAC. */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
+
+	ldr	r6, =iram_tlb_phys_addr
+	ldr	r7, [r6]
+
+	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
+
+	/* 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
+
+	/* 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
+
+	/* r10 must be DDRC base address */
+	.macro ddrc_enter_self_refresh
+
+	ldr	r10, [r0, #PM_INFO_MX7D_DDRC_V_OFFSET]
+
+	/* let DDR out of self-refresh */
+	ldr	r7, =0x0
+	str	r7, [r10, #DDRC_PWRCTL]
+
+	/* wait rw port_busy clear */
+	ldr	r6, =(0x1 << 16)
+	orr	r6, r6, #0x1
+2:
+	ldr	r7, [r10, #DDRC_PSTAT]
+	ands	r7, r7, r6
+	bne	2b
+
+	/* enter self-refresh bit 5 */
+	ldr	r7, =(0x1 << 5)
+	str	r7, [r10, #DDRC_PWRCTL]
+
+	/* wait until self-refresh mode entered */
+3:
+	ldr	r7, [r10, #DDRC_STAT]
+	and	r7, r7, #0x3
+	cmp	r7, #0x3
+	bne	3b
+4:
+	ldr	r7, [r10, #DDRC_STAT]
+	ands	r7, r7, #0x20
+	beq	4b
+
+	/* disable dram clk */
+	ldr	r7, [r10, #DDRC_PWRCTL]
+	orr	r7, r7, #(1 << 3)
+	str	r7, [r10, #DDRC_PWRCTL]
+
+	.endm
+
+	/* r10 must be DDRC base address */
+	.macro ddrc_exit_self_refresh
+
+	cmp	r5, #0x1
+	ldreq	r10, [r0, #PM_INFO_MX7D_DDRC_P_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX7D_DDRC_V_OFFSET]
+
+	/* let DDR out of self-refresh */
+	ldr	r7, =0x0
+	str	r7, [r10, #DDRC_PWRCTL]
+
+	/* wait until self-refresh mode exited */
+5:
+	ldr	r7, [r10, #DDRC_STAT]
+	and	r7, r7, #0x3
+	cmp	r7, #0x3
+	beq	5b
+
+	/* enable auto self-refresh */
+	ldr	r7, [r10, #DDRC_PWRCTL]
+	orr	r7, r7, #(1 << 0)
+	str	r7, [r10, #DDRC_PWRCTL]
+
+	.endm
+
+	.macro	pll_do_wait_lock
+6:
+	ldr	r7, [r10, r8]
+	ands	r7, #0x80000000
+	beq	6b
+
+	.endm
+
+	.macro ccm_enter_idle
+
+	ldr	r10, [r0, #PM_INFO_MX7D_ANATOP_V_OFFSET]
+
+	/* ungate pfd1 332m for lower axi */
+	ldr	r7, =0x8000
+	str	r7, [r10, #0xc8]
+
+	ldr	r10, [r0, #PM_INFO_MX7D_CCM_V_OFFSET]
+
+	/* switch ARM CLK to OSC */
+	ldr	r8, =0x8000
+	ldr	r7, [r10, r8]
+	bic	r7, r7, #0x7000000
+	str	r7, [r10, r8]
+
+	/* lower AXI clk from 24MHz to 3MHz */
+	ldr	r8, =0x8800
+	ldr	r7, [r10, r8]
+	orr	r7, r7, #0x7
+	str	r7, [r10, r8]
+
+	/* lower AHB clk from 24MHz to 3MHz */
+	ldr	r8, =0x9000
+	ldr	r7, [r10, r8]
+	orr	r7, r7, #0x7
+	str	r7, [r10, r8]
+
+	/* gate dram clk */
+	ldr	r8, =0x9880
+	ldr	r7, [r10, r8]
+	bic	r7, r7, #0x10000000
+	str	r7, [r10, r8]
+
+	ldr	r10, [r0, #PM_INFO_MX7D_ANATOP_V_OFFSET]
+
+	/* gate pfd1 332m */
+	ldr	r7, =0x8000
+	str	r7, [r10, #0xc4]
+
+	/* gate system pll pfd div 1 */
+	ldr	r7, =0x10
+	str	r7, [r10, #0xb4]
+	/* power down ARM, 480 and DRAM PLL */
+	ldr	r7, =0x1000
+	str	r7, [r10, #0x64]
+	str	r7, [r10, #0xb4]
+	ldr	r7, =0x100000
+	str	r7, [r10, #0x74]
+
+	.endm
+
+	.macro ccm_exit_idle
+
+	cmp	r5, #0x1
+	ldreq	r10, [r0, #PM_INFO_MX7D_ANATOP_P_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX7D_ANATOP_V_OFFSET]
+
+	/* power up ARM, 480 and DRAM PLL */
+	ldr	r7, =0x1000
+	str	r7, [r10, #0x68]
+	ldr	r8, =0x60
+	pll_do_wait_lock
+
+	ldr	r7, =0x1000
+	str	r7, [r10, #0xb8]
+	ldr	r8, =0xb0
+	pll_do_wait_lock
+
+	ldr	r7, =0x100000
+	str	r7, [r10, #0x78]
+	ldr	r8, =0x70
+	pll_do_wait_lock
+
+	/* ungate pfd1 332m for lower axi */
+	ldr	r7, =0x8000
+	str	r7, [r10, #0xc8]
+
+	/* ungate system pll pfd div 1 */
+	ldr	r7, =0x10
+	str	r7, [r10, #0xb8]
+
+	cmp	r5, #0x1
+	ldreq	r10, [r0, #PM_INFO_MX7D_CCM_P_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX7D_CCM_V_OFFSET]
+
+	/* switch ARM CLK to PLL */
+	ldr	r8, =0x8000
+	ldr	r7, [r10, r8]
+	orr	r7, r7, #0x1000000
+	str	r7, [r10, r8]
+
+	/* restore AXI clk from 3MHz to 24MHz */
+	ldr	r8, =0x8800
+	ldr	r7, [r10, r8]
+	bic	r7, r7, #0x7
+	str	r7, [r10, r8]
+
+	/* restore AHB clk from 3MHz to 24MHz */
+	ldr	r8, =0x9000
+	ldr	r7, [r10, r8]
+	bic	r7, r7, #0x7
+	str	r7, [r10, r8]
+
+	/* ungate dram clk */
+	ldr	r8, =0x9880
+	ldr	r7, [r10, r8]
+	orr	r7, r7, #0x10000000
+	str	r7, [r10, r8]
+
+	cmp	r5, #0x1
+	ldreq	r10, [r0, #PM_INFO_MX7D_ANATOP_P_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX7D_ANATOP_V_OFFSET]
+
+	/* gate pfd1 332m for lower axi */
+	ldr	r7, =0x8000
+	str	r7, [r10, #0xc4]
+
+	.endm
+
+	.macro anatop_enter_idle
+
+	ldr	r10, [r0, #PM_INFO_MX7D_ANATOP_V_OFFSET]
+
+	/* XTAL to RC-OSC switch */
+	ldr	r7, [r10]
+	orr	r7, r7, #0x1000
+	str	r7, [r10]
+	/* power down XTAL */
+	ldr	r7, [r10]
+	orr	r7, r7, #0x1
+	str	r7, [r10]
+
+	/* enable weak 1P0A */
+	ldr	r7, [r10, #0x200]
+	orr	r7, r7, #0x40000
+	str	r7, [r10, #0x200]
+
+	/* disable LDO 1P0A */
+	ldr	r7, [r10, #0x200]
+	bic	r7, r7, #0x1
+	str	r7, [r10, #0x200]
+
+	/* disable LDO 1P0D */
+	ldr	r7, [r10, #0x210]
+	bic	r7, r7, #0x1
+	str	r7, [r10, #0x210]
+
+	/* disable LDO 1P2 */
+	ldr	r7, [r10, #0x220]
+	bic	r7, r7, #0x1
+	str	r7, [r10, #0x220]
+
+	/* switch to low power bandgap */
+	ldr	r7, [r10, #0x270]
+	orr	r7, r7, #0x400
+	str	r7, [r10, #0x270]
+	/* power down normal bandgap */
+	orr	r7, r7, #0x1
+	str	r7, [r10, #0x270]
+
+	.endm
+
+	.macro anatop_exit_idle
+
+	cmp	r5, #0x1
+	ldreq	r10, [r0, #PM_INFO_MX7D_ANATOP_P_OFFSET]
+	ldrne	r10, [r0, #PM_INFO_MX7D_ANATOP_V_OFFSET]
+
+	/* power on normal bandgap */
+	ldr	r7, [r10, #0x270]
+	bic	r7, r7, #0x1
+	str	r7, [r10, #0x270]
+	/* switch to normal bandgap */
+	bic	r7, r7, #0x400
+	str	r7, [r10, #0x270]
+
+	/* enable LDO 1P2 */
+	ldr	r7, [r10, #0x220]
+	orr	r7, r7, #0x1
+	str	r7, [r10, #0x220]
+7:
+	ldr	r7, [r10, #0x220]
+	ands	r7, #0x20000
+	beq	7b
+
+	/* enable LDO 1P0D */
+	ldr	r7, [r10, #0x210]
+	orr	r7, r7, #0x1
+	str	r7, [r10, #0x210]
+8:
+	ldr	r7, [r10, #0x210]
+	ands	r7, #0x20000
+	beq	8b
+
+	/* enable LDO 1P0A */
+	ldr	r7, [r10, #0x200]
+	orr	r7, r7, #0x1
+	str	r7, [r10, #0x200]
+9:
+	ldr	r7, [r10, #0x200]
+	ands	r7, #0x20000
+	beq	9b
+	/* disable weak 1P0A */
+	ldr	r7, [r10, #0x200]
+	bic	r7, r7, #0x40000
+	str	r7, [r10, #0x200]
+
+	/* power up XTAL and wait */
+	ldr	r7, [r10]
+	bic	r7, r7, #0x1
+	str	r7, [r10]
+10:
+	ldr	r7, [r10]
+	ands	r7, r7, #0x4
+	beq	10b
+	/* RC-OSC to XTAL switch */
+	ldr	r7, [r10]
+	bic	r7, r7, #0x1000
+	str	r7, [r10]
+
+	.endm
+
+.extern iram_tlb_phys_addr
+
+	.align 3
+ENTRY(imx7d_low_power_idle)
+	push	{r0 - r12}
+
+	/* 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, =imx7d_low_power_idle
+	ldr     r6, =wakeup
+	sub	r6, r6, r5
+	add     r8, r1, r2
+	add	r3, r8, r6
+
+	/* r11 is cpu id */
+	mrc	p15, 0, r11, c0, c0, 5
+	and	r11, r11, #3
+	cmp	r11, #0x0
+	ldreq	r6, =MX7D_SRC_GPR1
+	ldreq	r7, =MX7D_SRC_GPR2
+	ldrne	r6, =MX7D_SRC_GPR3
+	ldrne	r7, =MX7D_SRC_GPR4
+	/* store physical resume addr and pm_info address. */
+	ldr	r10, [r0, #PM_INFO_MX7D_SRC_V_OFFSET]
+	str	r3, [r10, r6]
+	str	r1, [r10, r7]
+
+	disable_l1_dcache
+
+	tlb_set_to_ocram
+
+	imx_pen_lock
+
+	ldr	r6, [r0, #PM_INFO_PM_INFO_LAST_CPU_OFFSET]
+	cmp	r11, r6
+	bne	first_cpu
+last_cpu:
+	ldr	r7, [r0, #PM_INFO_PM_INFO_CPU1_IN_WFI_OFFSET]
+	cmp	r7, #0x0
+	beq	lpi_enter_done
+
+	ddrc_enter_self_refresh
+	ccm_enter_idle
+	anatop_enter_idle
+	/* set low power idle enter flag */
+	ldr	r7, =0x1
+	str	r7, [r0, #PM_INFO_PM_INFO_LPI_ENTER_OFFSET]
+	b	lpi_enter_done
+first_cpu:
+	/* set first cpu wfi flag */
+	ldr	r7, =0x1
+	str	r7, [r0, #PM_INFO_PM_INFO_CPU1_IN_WFI_OFFSET]
+lpi_enter_done:
+
+	imx_pen_unlock
+
+	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
+
+	imx_pen_lock
+
+	ldr	r6, [r0, #PM_INFO_PM_INFO_LAST_CPU_OFFSET]
+	cmp	r11, r6
+	beq	do_exit_wfi
+
+	/* clear first cpu wfi flag */
+	ldr	r7, =0x0
+	str	r7, [r0, #PM_INFO_PM_INFO_CPU1_IN_WFI_OFFSET]
+do_exit_wfi:
+	ldr	r7, [r0, #PM_INFO_PM_INFO_LPI_ENTER_OFFSET]
+	cmp	r7, #0
+	beq	skip_lpi_flow
+
+	ldr	r5, =0x0
+	anatop_exit_idle
+	ccm_exit_idle
+	ddrc_exit_self_refresh
+	/* clear lpi enter flag */
+	ldr	r7, =0x0
+	str	r7, [r0, #PM_INFO_PM_INFO_LPI_ENTER_OFFSET]
+skip_lpi_flow:
+
+	imx_pen_unlock
+
+	tlb_back_to_ddr
+
+#ifdef CONFIG_SMP
+	/* Turn on SMP bit. */
+	mrc	p15, 0, r7, c1, c0, 1
+	orr	r7, r7, #0x40
+	mcr	p15, 0, r7, c1, c0, 1
+
+	isb
+#endif
+
+	/* enable d-cache */
+	mrc	p15, 0, r7, c1, c0, 0
+	orr	r7, r7, #(1 << 2)
+	mcr	p15, 0, r7, c1, c0, 0
+	dsb
+	isb
+
+	/* Restore registers */
+	pop	{r0 - r12}
+	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
+
+	/* r11 is cpu id */
+	mrc	p15, 0, r11, c0, c0, 5
+	and	r11, r11, #3
+	cmp	r11, #0x0
+
+	imx_pen_lock
+
+	ldr	r6, [r0, #PM_INFO_PM_INFO_LAST_CPU_OFFSET]
+	cmp	r11, r6
+	beq	do_exit_lp_idle
+
+	/* clear first wfi flag */
+	ldr	r7, =0x0
+	str	r7, [r0, #PM_INFO_PM_INFO_CPU1_IN_WFI_OFFSET]
+do_exit_lp_idle:
+	ldr	r7, [r0, #PM_INFO_PM_INFO_LPI_ENTER_OFFSET]
+	cmp	r7, #0
+	beq	wakeup_skip_lpi_flow
+
+	ldr	r5, =0x1
+	anatop_exit_idle
+	ccm_exit_idle
+	ddrc_exit_self_refresh
+	/* clear lpi enter flag */
+	ldr	r7, =0x0
+	str	r7, [r0, #PM_INFO_PM_INFO_LPI_ENTER_OFFSET]
+wakeup_skip_lpi_flow:
+
+	imx_pen_unlock
+
+	/* get physical resume address from pm_info. */
+	ldr	lr, [r0, #PM_INFO_RESUME_ADDR_OFFSET]
+
+	/* Restore registers */
+	mov	pc, lr
+	.ltorg
+ENDPROC(imx7d_low_power_idle)
diff --git a/arch/arm/mach-imx/mach-imx7d.c b/arch/arm/mach-imx/mach-imx7d.c
index e3a33f15ad3e..3b9796aabcbd 100644
--- a/arch/arm/mach-imx/mach-imx7d.c
+++ b/arch/arm/mach-imx/mach-imx7d.c
@@ -17,6 +17,7 @@
 #include <asm/mach/map.h>
 
 #include "common.h"
+#include "cpuidle.h"
 
 static int ar8031_phy_fixup(struct phy_device *dev)
 {
@@ -111,6 +112,7 @@ static void __init imx7d_init_late(void)
 	if (IS_ENABLED(CONFIG_ARM_IMX7D_CPUFREQ)) {
 		platform_device_register_simple("imx7d-cpufreq", -1, NULL, 0);
 	}
+	imx7d_cpuidle_init();
 }
 
 static void __init imx7d_map_io(void)
diff --git a/arch/arm/mach-imx/src.c b/arch/arm/mach-imx/src.c
index f09821356e51..2bde676cecc7 100644
--- a/arch/arm/mach-imx/src.c
+++ b/arch/arm/mach-imx/src.c
@@ -117,6 +117,7 @@ void imx_enable_cpu(int cpu, bool enable)
 
 void imx_set_cpu_jump(int cpu, void *jump_addr)
 {
+	spin_lock(&src_lock);
 	cpu = cpu_logical_map(cpu);
 	if (cpu_is_imx7d())
 		writel_relaxed(virt_to_phys(jump_addr),
@@ -124,6 +125,7 @@ void imx_set_cpu_jump(int cpu, void *jump_addr)
 	else
 		writel_relaxed(virt_to_phys(jump_addr),
 			src_base + SRC_GPR1 + cpu * 8);
+	spin_unlock(&src_lock);
 }
 
 u32 imx_get_cpu_arg(int cpu)