MLK-11300-05 ARM: imx: add pm support for imx6ul

Add basic pm suspend/resume support for i.MX6UL.

Signed-off-by: Bai Ping <b51503@freescale.com>

6 files changed, 395 insertions, 97 deletions

diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index 962eb48e89df..a790b97925be 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2014 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2015 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -93,6 +93,7 @@ void imx_smp_prepare(void);
 static inline void imx_scu_map_io(void) {}
 static inline void imx_smp_prepare(void) {}
 #endif
+void imx6_pm_map_io(void);
 void imx7_pm_map_io(void);
 void imx_src_init(void);
 void imx_gpc_pre_suspend(bool arm_power_off);
@@ -140,6 +141,7 @@ void imx6dl_pm_init(void);
 void imx6sl_pm_init(void);
 void imx6sx_pm_init(void);
 void imx6ul_pm_init(void);
+void imx6q_pm_set_ccm_base(void __iomem *base);
 
 #ifdef CONFIG_PM
 void imx51_pm_init(void);
diff --git a/arch/arm/mach-imx/hardware.h b/arch/arm/mach-imx/hardware.h
index 082e58953872..fe681cc89c93 100644
--- a/arch/arm/mach-imx/hardware.h
+++ b/arch/arm/mach-imx/hardware.h
@@ -109,13 +109,14 @@
 
 #include "mxc.h"
 
-#include "mx7.h"
 #include "mx3x.h"
 #include "mx31.h"
 #include "mx35.h"
 #include "mx2x.h"
 #include "mx21.h"
 #include "mx27.h"
+#include "mx6.h"
+#include "mx7.h"
 
 #define imx_map_entry(soc, name, _type)	{				\
 	.virtual = soc ## _IO_P2V(soc ## _ ## name ## _BASE_ADDR),	\
diff --git a/arch/arm/mach-imx/mach-imx6ul.c b/arch/arm/mach-imx/mach-imx6ul.c
index 58a2b88233e6..8540d5125f5f 100644
--- a/arch/arm/mach-imx/mach-imx6ul.c
+++ b/arch/arm/mach-imx/mach-imx6ul.c
@@ -73,6 +73,7 @@ static void __init imx6ul_init_machine(void)
 
 static void __init imx6ul_init_irq(void)
 {
+	imx_gpc_check_dt();
 	imx_init_revision_from_anatop();
 	imx_src_init();
 	irqchip_init();
@@ -87,12 +88,19 @@ static void __init imx6ul_init_late(void)
 		platform_device_register_simple("imx6q-cpufreq", -1, NULL, 0);
 }
 
+static void __init imx6ul_map_io(void)
+{
+	debug_ll_io_init();
+	imx6_pm_map_io();
+}
+
 static const char * const imx6ul_dt_compat[] __initconst = {
 	"fsl,imx6ul",
 	NULL,
 };
 
 DT_MACHINE_START(IMX6UL, "Freescale i.MX6 Ultralite (Device Tree)")
+	.map_io		= imx6ul_map_io,
 	.init_irq	= imx6ul_init_irq,
 	.init_machine	= imx6ul_init_machine,
 	.init_late	= imx6ul_init_late,
diff --git a/arch/arm/mach-imx/mx6.h b/arch/arm/mach-imx/mx6.h
new file mode 100644
index 000000000000..06b8135a9954
--- /dev/null
+++ b/arch/arm/mach-imx/mx6.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright 2004-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 version 2 as
+ *    * published by the Free Software Foundation.
+ *     */
+
+#ifndef __ASM_ARCH_MXC_IOMAP_H__
+#define __ASM_ARCH_MXC_IOMAP_H__
+
+#define MX6Q_IO_P2V(x)                  IMX_IO_P2V(x)
+#define MX6Q_IO_ADDRESS(x)              IOMEM(MX6Q_IO_P2V(x))
+
+#define MX6Q_L2_BASE_ADDR       0x00a02000
+#define MX6Q_L2_SIZE            0x1000
+#define MX6Q_IOMUXC_BASE_ADDR       0x020e0000
+#define MX6Q_IOMUXC_SIZE        0x4000
+#define MX6Q_SRC_BASE_ADDR      0x020d8000
+#define MX6Q_SRC_SIZE           0x4000
+#define MX6Q_CCM_BASE_ADDR      0x020c4000
+#define MX6Q_CCM_SIZE           0x4000
+#define MX6Q_ANATOP_BASE_ADDR       0x020c8000
+#define MX6Q_ANATOP_SIZE        0x1000
+#define MX6Q_GPC_BASE_ADDR      0x020dc000
+#define MX6Q_GPC_SIZE           0x4000
+#define MX6Q_SEMA4_BASE_ADDR	0x02290000
+#define MX6Q_SEMA4_SIZE		0x4000
+#define MX6Q_MMDC_P0_BASE_ADDR  0x021b0000
+#define MX6Q_MMDC_P0_SIZE       0x4000
+#define MX6Q_MMDC_P1_BASE_ADDR  0x021b4000
+#define MX6Q_MMDC_P1_SIZE       0x4000
+#define MX6Q_AIPS1_BASE_ADDR        0x02000000
+#define MX6Q_AIPS1_SIZE     0x100000
+#define MX6Q_AIPS2_BASE_ADDR        0x02100000
+#define MX6Q_AIPS2_SIZE     0x100000
+#define MX6Q_AIPS3_BASE_ADDR	0x02200000
+#define MX6Q_AIPS3_SIZE		0x100000
+
+#define MX6SX_IRAM_TLB_BASE_ADDR    0x008f8000
+#define MX6Q_IRAM_TLB_BASE_ADDR 0x00900000
+#define MX6Q_IRAM_TLB_SIZE      0x4000
+#define TT_ATTRIB_NON_CACHEABLE_1M  0x802
+#define MX6_SUSPEND_IRAM_DATA_SIZE  256
+#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 1515e498d348..7833d942df71 100644
--- a/arch/arm/mach-imx/pm-imx6.c
+++ b/arch/arm/mach-imx/pm-imx6.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2011-2014 Freescale Semiconductor, Inc.
+ * Copyright 2011-2015 Freescale Semiconductor, Inc.
  * Copyright 2011 Linaro Ltd.
  *
  * The code contained herein is licensed under the GNU General Public
@@ -19,11 +19,13 @@
 #include <linux/mfd/syscon/imx6q-iomuxc-gpr.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/of_fdt.h>
 #include <linux/of_platform.h>
 #include <linux/regmap.h>
 #include <linux/suspend.h>
 #include <asm/cacheflush.h>
 #include <asm/fncpy.h>
+#include <asm/mach/map.h>
 #include <asm/proc-fns.h>
 #include <asm/suspend.h>
 #include <asm/tlb.h>
@@ -64,6 +66,9 @@
 #define MX6Q_SUSPEND_OCRAM_SIZE		0x1000
 #define MX6_MAX_MMDC_IO_NUM		33
 
+extern unsigned long iram_tlb_base_addr;
+extern unsigned long iram_tlb_phys_addr;
+
 static void __iomem *ccm_base;
 static void __iomem *suspend_ocram_base;
 static void (*imx6_suspend_in_ocram_fn)(void __iomem *ocram_vbase);
@@ -195,6 +200,36 @@ static const struct imx6_pm_socdata imx6ul_pm_data __initconst = {
 	.mmdc_io_offset = imx6ul_mmdc_io_offset,
 };
 
+static struct map_desc iram_tlb_io_desc __initdata = {
+	/* .virtual and .pfn are run-time assigned */
+	.length     = SZ_1M,
+	.type       = MT_MEMORY_RWX_NONCACHED,
+};
+
+/*
+ * AIPS1 and AIPS2 is not used, because it will trigger a BUG_ON if
+ * lowlevel debug and earlyprintk are configured.
+ *
+ * it is because there is a vm conflict because UART1 is mapped early if
+ * AIPS1 is mapped using 1M size.
+ *
+ * Thus no use AIPS1 and AIPS2 to avoid kernel BUG_ON.
+ */
+static struct map_desc imx6_pm_io_desc[] __initdata = {
+	imx_map_entry(MX6Q, MMDC_P0, MT_DEVICE),
+	imx_map_entry(MX6Q, MMDC_P1, MT_DEVICE),
+	imx_map_entry(MX6Q, SRC, MT_DEVICE),
+	imx_map_entry(MX6Q, IOMUXC, MT_DEVICE),
+	imx_map_entry(MX6Q, CCM, MT_DEVICE),
+	imx_map_entry(MX6Q, ANATOP, MT_DEVICE),
+	imx_map_entry(MX6Q, GPC, MT_DEVICE),
+};
+
+static const char * const low_power_ocram_match[] __initconst = {
+	"fsl,lpm-sram",
+	NULL
+};
+
 /*
  * This structure is for passing necessary data for low level ocram
  * suspend code(arch/arm/mach-imx/suspend-imx6.S), if this struct
@@ -213,6 +248,8 @@ struct imx6_cpu_pm_info {
 	struct imx6_pm_base ccm_base;
 	struct imx6_pm_base gpc_base;
 	struct imx6_pm_base l2_base;
+	struct imx6_pm_base anatop_base;
+	u32 ttbr1; /* Store TTBR1 */
 	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);
@@ -418,42 +455,108 @@ static const struct platform_suspend_ops imx6q_pm_ops = {
 	.valid = imx6q_pm_valid,
 };
 
-static int __init imx6_pm_get_base(struct imx6_pm_base *base,
-				const char *compat)
+static int __init imx6_dt_find_lpsram(unsigned long node, const char *uname,
+				      int depth, void *data)
 {
-	struct device_node *node;
-	struct resource res;
-	int ret = 0;
+	unsigned long lpram_addr;
+	const __be32 *prop = of_get_flat_dt_prop(node, "reg", NULL);
+
+	if (of_flat_dt_match(node, low_power_ocram_match)) {
+		if (!prop)
+			return -EINVAL;
+
+		lpram_addr = be32_to_cpup(prop);
+
+		/* We need to create a 1M page table entry. */
+		iram_tlb_io_desc.virtual = IMX_IO_P2V(lpram_addr & 0xFFF00000);
+		iram_tlb_io_desc.pfn = __phys_to_pfn(lpram_addr & 0xFFF00000);
+		iram_tlb_phys_addr = lpram_addr;
+		iram_tlb_base_addr = IMX_IO_P2V(lpram_addr);
 
-	node = of_find_compatible_node(NULL, NULL, compat);
-	if (!node) {
-		ret = -ENODEV;
-		goto out;
+		iotable_init(&iram_tlb_io_desc, 1);
 	}
 
-	ret = of_address_to_resource(node, 0, &res);
-	if (ret)
-		goto put_node;
+	return 0;
+}
 
-	base->pbase = res.start;
-	base->vbase = ioremap(res.start, resource_size(&res));
-	if (!base->vbase)
-		ret = -ENOMEM;
+void __init imx6_pm_map_io(void)
+{
+	unsigned long i;
 
-put_node:
-	of_node_put(node);
-out:
-	return ret;
+	pr_info("pm_map_io init\n\n");
+
+	iotable_init(imx6_pm_io_desc, ARRAY_SIZE(imx6_pm_io_desc));
+
+	/*
+	 * Get the address of IRAM or OCRAM to be used by the low
+	 * power code from the device tree.
+	 */
+	WARN_ON(of_scan_flat_dt(imx6_dt_find_lpsram, NULL));
+
+	/* Return if no IRAM space is allocated for suspend/resume code. */
+	if (!iram_tlb_base_addr) {
+		pr_warn("No IRAM/OCRAM memory allocated for suspend/resume \
+			 code. Please ensure device tree has an entry for \
+			 fsl,lpm-sram.\n");
+		return;
+	}
+
+	/* Set all entries to 0. */
+	memset((void *)iram_tlb_base_addr, 0, MX6Q_IRAM_TLB_SIZE);
+
+	/*
+	 * Make sure the IRAM virtual address has a mapping in the IRAM
+	 * page table.
+	 *
+	 * Only use the top 11 bits [31-20] when storing the physical
+	 * address in the page table as only these bits are required
+	 * for 1M mapping.
+	 */
+	i = ((iram_tlb_base_addr >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(iram_tlb_phys_addr & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
+
+	/*
+	 * Make sure the AIPS1 virtual address has a mapping in the
+	 * IRAM page table.
+	 */
+	i = ((IMX_IO_P2V(MX6Q_AIPS1_BASE_ADDR) >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(MX6Q_AIPS1_BASE_ADDR & 0xFFF00000) |
+		TT_ATTRIB_NON_CACHEABLE_1M;
+
+	/*
+	 * Make sure the AIPS2 virtual address has a mapping in the
+	 * IRAM page table.
+	 */
+	i = ((IMX_IO_P2V(MX6Q_AIPS2_BASE_ADDR) >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(MX6Q_AIPS2_BASE_ADDR & 0xFFF00000) |
+		TT_ATTRIB_NON_CACHEABLE_1M;
+
+	/*
+	 * Make sure the AIPS3 virtual address has a mapping
+	 * in the IRAM page table.
+	 */
+	i = ((IMX_IO_P2V(MX6Q_AIPS3_BASE_ADDR) >> 20) << 2) / 4;
+		*((unsigned long *)iram_tlb_base_addr + i) =
+		(MX6Q_AIPS3_BASE_ADDR & 0xFFF00000) |
+		TT_ATTRIB_NON_CACHEABLE_1M;
+
+	/*
+	 * Make sure the L2 controller virtual address has a mapping
+	 * in the IRAM page table.
+	 */
+	i = ((IMX_IO_P2V(MX6Q_L2_BASE_ADDR) >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(MX6Q_L2_BASE_ADDR & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
 }
 
 static int __init imx6q_suspend_init(const struct imx6_pm_socdata *socdata)
 {
-	phys_addr_t ocram_pbase;
 	struct device_node *node;
-	struct platform_device *pdev;
 	struct imx6_cpu_pm_info *pm_info;
-	struct gen_pool *ocram_pool;
-	unsigned long ocram_base;
+	unsigned long iram_paddr;
 	int i, ret = 0;
 	const u32 *mmdc_offset_array;
 
@@ -464,41 +567,24 @@ static int __init imx6q_suspend_init(const struct imx6_pm_socdata *socdata)
 		return -EINVAL;
 	}
 
-	node = of_find_compatible_node(NULL, NULL, "mmio-sram");
-	if (!node) {
-		pr_warn("%s: failed to find ocram node!\n", __func__);
-		return -ENODEV;
-	}
-
-	pdev = of_find_device_by_node(node);
-	if (!pdev) {
-		pr_warn("%s: failed to find ocram device!\n", __func__);
-		ret = -ENODEV;
-		goto put_node;
-	}
-
-	ocram_pool = gen_pool_get(&pdev->dev, NULL);
-	if (!ocram_pool) {
-		pr_warn("%s: ocram pool unavailable!\n", __func__);
-		ret = -ENODEV;
-		goto put_node;
-	}
-
-	ocram_base = gen_pool_alloc(ocram_pool, MX6Q_SUSPEND_OCRAM_SIZE);
-	if (!ocram_base) {
-		pr_warn("%s: unable to alloc ocram!\n", __func__);
-		ret = -ENOMEM;
-		goto put_node;
-	}
+	/*
+	 * 16KB is allocated for IRAM TLB, but only up 8k is for kernel TLB,
+	 * The lower 8K is not used, so use the lower 8K for IRAM code and
+	 * pm_info.
+	 *
+	 */
+	iram_paddr = iram_tlb_phys_addr + MX6_SUSPEND_IRAM_ADDR_OFFSET;
 
-	ocram_pbase = gen_pool_virt_to_phys(ocram_pool, ocram_base);
+	/* Make sure iram_paddr is 8 byte aligned. */
+	if ((uintptr_t)(iram_paddr) & (FNCPY_ALIGN - 1))
+		iram_paddr += FNCPY_ALIGN - iram_paddr % (FNCPY_ALIGN);
 
-	suspend_ocram_base = __arm_ioremap_exec(ocram_pbase,
-		MX6Q_SUSPEND_OCRAM_SIZE, false);
+	/* Get the virtual address of the suspend code. */
+	suspend_ocram_base = (void *)IMX_IO_P2V(iram_paddr);
 
 	memset(suspend_ocram_base, 0, sizeof(*pm_info));
 	pm_info = suspend_ocram_base;
-	pm_info->pbase = ocram_pbase;
+	pm_info->pbase = iram_paddr;
 	pm_info->resume_addr = virt_to_phys(v7_cpu_resume);
 	pm_info->pm_info_size = sizeof(*pm_info);
 
@@ -506,40 +592,33 @@ static int __init imx6q_suspend_init(const struct imx6_pm_socdata *socdata)
 	 * ccm physical address is not used by asm code currently,
 	 * so get ccm virtual address directly.
 	 */
-	pm_info->ccm_base.vbase = ccm_base;
+	pm_info->ccm_base.pbase = MX6Q_CCM_BASE_ADDR;
+	pm_info->ccm_base.vbase = (void __iomem *)
+				   IMX_IO_P2V(MX6Q_CCM_BASE_ADDR);
 
-	ret = imx6_pm_get_base(&pm_info->mmdc_base, socdata->mmdc_compat);
-	if (ret) {
-		pr_warn("%s: failed to get mmdc base %d!\n", __func__, ret);
-		goto put_node;
-	}
+	pm_info->mmdc_base.pbase = MX6Q_MMDC_P0_BASE_ADDR;
+	pm_info->mmdc_base.vbase = (void __iomem *)
+				    IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR);
 
-	ret = imx6_pm_get_base(&pm_info->src_base, socdata->src_compat);
-	if (ret) {
-		pr_warn("%s: failed to get src base %d!\n", __func__, ret);
-		goto src_map_failed;
-	}
+	pm_info->src_base.pbase = MX6Q_SRC_BASE_ADDR;
+	pm_info->src_base.vbase = (void __iomem *)
+				   IMX_IO_P2V(MX6Q_SRC_BASE_ADDR);
 
-	ret = imx6_pm_get_base(&pm_info->iomuxc_base, socdata->iomuxc_compat);
-	if (ret) {
-		pr_warn("%s: failed to get iomuxc base %d!\n", __func__, ret);
-		goto iomuxc_map_failed;
-	}
+	pm_info->iomuxc_base.pbase = MX6Q_IOMUXC_BASE_ADDR;
+	pm_info->iomuxc_base.vbase = (void __iomem *)
+				      IMX_IO_P2V(MX6Q_IOMUXC_BASE_ADDR);
 
-	ret = imx6_pm_get_base(&pm_info->gpc_base, socdata->gpc_compat);
-	if (ret) {
-		pr_warn("%s: failed to get gpc base %d!\n", __func__, ret);
-		goto gpc_map_failed;
-	}
+	pm_info->gpc_base.pbase = MX6Q_GPC_BASE_ADDR;
+	pm_info->gpc_base.vbase = (void __iomem *)
+				   IMX_IO_P2V(MX6Q_GPC_BASE_ADDR);
 
-	if (socdata->pl310_compat) {
-		ret = imx6_pm_get_base(&pm_info->l2_base, socdata->pl310_compat);
-		if (ret) {
-			pr_warn("%s: failed to get pl310-cache base %d!\n",
-				__func__, ret);
-			goto pl310_cache_map_failed;
-		}
-	}
+	pm_info->l2_base.pbase = MX6Q_L2_BASE_ADDR;
+	pm_info->l2_base.vbase = (void __iomem *)
+				  IMX_IO_P2V(MX6Q_L2_BASE_ADDR);
+
+	pm_info->anatop_base.pbase = MX6Q_ANATOP_BASE_ADDR;
+	pm_info->anatop_base.vbase = (void __iomem *)
+				  IMX_IO_P2V(MX6Q_ANATOP_BASE_ADDR);
 
 	pm_info->ddr_type = imx_mmdc_get_ddr_type();
 	pm_info->mmdc_io_num = socdata->mmdc_io_num;
@@ -560,14 +639,6 @@ static int __init imx6q_suspend_init(const struct imx6_pm_socdata *socdata)
 
 	goto put_node;
 
-pl310_cache_map_failed:
-	iounmap(pm_info->gpc_base.vbase);
-gpc_map_failed:
-	iounmap(pm_info->iomuxc_base.vbase);
-iomuxc_map_failed:
-	iounmap(pm_info->src_base.vbase);
-src_map_failed:
-	iounmap(pm_info->mmdc_base.vbase);
 put_node:
 	of_node_put(node);
 
diff --git a/arch/arm/mach-imx/suspend-imx6.S b/arch/arm/mach-imx/suspend-imx6.S
index 76ee2ceec8d5..85bde115f128 100644
--- a/arch/arm/mach-imx/suspend-imx6.S
+++ b/arch/arm/mach-imx/suspend-imx6.S
@@ -1,5 +1,5 @@
 /*
- * Copyright 2014 Freescale Semiconductor, Inc.
+ * Copyright 2014-2015 Freescale Semiconductor, Inc.
  *
  * The code contained herein is licensed under the GNU General Public
  * License. You may obtain a copy of the GNU General Public License
@@ -59,8 +59,11 @@
 #define PM_INFO_MX6Q_GPC_V_OFFSET		0x34
 #define PM_INFO_MX6Q_L2_P_OFFSET		0x38
 #define PM_INFO_MX6Q_L2_V_OFFSET		0x3C
-#define PM_INFO_MMDC_IO_NUM_OFFSET		0x40
-#define PM_INFO_MMDC_IO_VAL_OFFSET		0x44
+#define PM_INFO_MX6Q_ANATOP_P_OFFSET		0x40
+#define PM_INFO_MX6Q_ANATOP_V_OFFSET		0x44
+#define	PM_INFO_MX6Q_TTBR1_V_OFFSET		0x48
+#define PM_INFO_MMDC_IO_NUM_OFFSET		0x4c
+#define PM_INFO_MMDC_IO_VAL_OFFSET		0x50
 
 #define MX6Q_SRC_GPR1	0x20
 #define MX6Q_SRC_GPR2	0x24
@@ -74,6 +77,45 @@
 
 	.align 3
 
+       /* Check if the cpu is cortex-a7 */
+       .macro is_cortex_a7
+
+       /* Read the primary cpu number is MPIDR */
+       mrc     p15, 0, r5, c0, c0, 0
+       ldr     r6, =0xfff0
+       and     r5, r5, r6
+       ldr     r6, =0xc070
+       cmp     r5, r6
+
+       .endm
+
+       .macro  disable_l1_cache
+
+       /*
+        * 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  sync_l2_cache
 
 	/* sync L2 cache to drain L2's buffers to DRAM. */
@@ -151,6 +193,111 @@
 
 	.endm
 
+	.macro store_ttbr1
+
+	/* Store TTBR1 to pm_info->ttbr1 */
+	mrc	p15, 0, r7, c2, c0, 1
+	str	r7, [r0, #PM_INFO_MX6Q_TTBR1_V_OFFSET]
+
+	/* 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	r6, [r6]
+	dsb
+	isb
+
+	/* Store the IRAM table in TTBR1 */
+	mcr	p15, 0, r6, 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
+
+	/* Disable L1 data cache. */
+	mrc	p15, 0, r6, c1, c0, 0
+	bic	r6, r6, #0x4
+	mcr	p15, 0, r6, c1, c0, 0
+
+	dsb
+	isb
+
+	is_cortex_a7
+	beq	14f
+
+#ifdef CONFIG_CACHE_L2X0
+	ldr	r8, [r0, #PM_INFO_MX6Q_L2_V_OFFSET]
+	mov	r6, #0x0
+	str	r6, [r8, #0x100]
+
+	dsb
+	isb
+#endif
+14:
+	.endm
+
+	.macro restore_ttbr1
+
+	is_cortex_a7
+	beq	15f
+
+#ifdef CONFIG_CACHE_L2X0
+	/* Enable L2. */
+	ldr	r8, [r0, #PM_INFO_MX6Q_L2_V_OFFSET]
+	ldr	r7, =0x1
+	str	r7, [r8, #0x100]
+#endif
+
+15:
+	/* Enable L1 data cache. */
+	mrc	p15, 0, r6, c1, c0, 0
+	orr	r6, r6, #0x4
+	mcr	p15, 0, r6, c1, c0, 0
+
+	dsb
+	isb
+
+	/* Restore TTBCR */
+	/* 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 TTBR1, get the origin ttbr1 from pm info */
+	ldr	r7, [r0, #PM_INFO_MX6Q_TTBR1_V_OFFSET]
+	mcr	p15, 0, r7, c2, c0, 1
+
+	.endm
+
+
 ENTRY(imx6_suspend)
 	ldr	r1, [r0, #PM_INFO_PBASE_OFFSET]
 	ldr	r2, [r0, #PM_INFO_RESUME_ADDR_OFFSET]
@@ -185,8 +332,22 @@ ENTRY(imx6_suspend)
 	str	r9, [r11, #MX6Q_SRC_GPR1]
 	str	r1, [r11, #MX6Q_SRC_GPR2]
 
+	/*
+	 * Check if the cpu is Cortex-A7, for Cortex-A7
+	 * the cache implementation is not the same as
+	 * Cortex-A9, so the cache maintenance operation
+	 * is different.
+	 */
+	is_cortex_a7
+	beq	a7_dache_flush
+
 	/* need to sync L2 cache before DSM. */
 	sync_l2_cache
+	b	ttbr_store
+a7_dache_flush:
+	disable_l1_cache
+ttbr_store:
+	store_ttbr1
 
 	ldr	r11, [r0, #PM_INFO_MX6Q_MMDC_V_OFFSET]
 	/*
@@ -305,6 +466,7 @@ rbc_loop:
 	 */
 	mov	r5, #0x0
 	resume_mmdc
+	restore_ttbr1
 
 	/* return to suspend finish */
 	ret	lr
@@ -342,8 +504,11 @@ ENDPROC(imx6_suspend)
 
 ENTRY(v7_cpu_resume)
 	bl	v7_invalidate_l1
+	is_cortex_a7
+	beq	done
 #ifdef CONFIG_CACHE_L2X0
 	bl	l2c310_early_resume
 #endif
+done:
 	b	cpu_resume
 ENDPROC(v7_cpu_resume)