powerpc/pm: support deep sleep feature on T104x

T104x has deep sleep feature, which can switch off most parts of
the SoC when it is in deep sleep mode. This way, it becomes more
energy-efficient.

The DDR controller will also be powered off in deep sleep. Therefore,
the last stage (the latter part of fsl_dp_enter_low) will run without DDR
access. This piece of code and related TLBs are prefetched in advance.

Due to the different initialization code between 32-bit and 64-bit, they
have seperate resume entry and precedure.

The feature supports 32-bit and 64-bit kernel mode.

Change-Id: I9b9b9188fdc67167030658b6cc1d0a1cbe7e2180
Signed-off-by: Wang Dongsheng <dongsheng.wang@freescale.com>
Signed-off-by: Chenhui Zhao <chenhui.zhao@freescale.com>
Reviewed-on: http://git.am.freescale.net:8181/10709
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Yang Li <LeoLi@freescale.com>
Reviewed-by: Jose Rivera <German.Rivera@freescale.com>

9 files changed, 827 insertions, 5 deletions

diff --git a/arch/powerpc/include/asm/fsl_pm.h b/arch/powerpc/include/asm/fsl_pm.h
index 03f35a4..d06adaa 100644
--- a/arch/powerpc/include/asm/fsl_pm.h
+++ b/arch/powerpc/include/asm/fsl_pm.h
@@ -12,6 +12,7 @@
 #define __PPC_FSL_PM_H
 #ifdef	__KERNEL__
 
+#ifndef __ASSEMBLY__
 #define E500_PM_PH10	1
 #define E500_PM_PH15	2
 #define E500_PM_PH20	3
@@ -33,5 +34,23 @@ struct fsl_pm_ops {
 };
 
 extern const struct fsl_pm_ops *qoriq_pm_ops;
+
+struct fsm_reg_vals;
+
+extern void fsl_dp_fsm_setup(void __iomem *dcsr_base, struct fsm_reg_vals *val);
+extern void fsl_dp_fsm_clean(void __iomem *dcsr_base, struct fsm_reg_vals *val);
+
+extern int fsl_dp_iomap(void);
+extern void fsl_dp_iounmap(void);
+
+extern int fsl_enter_epu_deepsleep(void);
+extern void fsl_dp_enter_low(void __iomem *ccsr_base, void __iomem *dcsr_base,
+			     void __iomem *pld_base, int pld_flag);
+extern void fsl_booke_deep_sleep_resume(void);
+#endif	/* __ASSEMBLY__ */
+
+#define T1040QDS_TETRA_FLAG	1
+#define T104xRDB_CPLD_FLAG	2
+
 #endif	/* __KERNEL__ */
 #endif  /* __PPC_FSL_PM_H */
diff --git a/arch/powerpc/include/asm/reg_booke.h b/arch/powerpc/include/asm/reg_booke.h
index 1353383..56d1f27 100644
--- a/arch/powerpc/include/asm/reg_booke.h
+++ b/arch/powerpc/include/asm/reg_booke.h
@@ -110,6 +110,7 @@
 #define SPRN_IVOR39	0x1B1	/* Interrupt Vector Offset Register 39 */
 #define SPRN_IVOR40	0x1B2	/* Interrupt Vector Offset Register 40 */
 #define SPRN_IVOR41	0x1B3	/* Interrupt Vector Offset Register 41 */
+#define SPRN_IVOR42	0x1B4	/* Interrupt Vector Offset Register 42 */
 #define SPRN_GIVOR2	0x1B8	/* Guest IVOR2 */
 #define SPRN_GIVOR3	0x1B9	/* Guest IVOR3 */
 #define SPRN_GIVOR4	0x1BA	/* Guest IVOR4 */
diff --git a/arch/powerpc/platforms/85xx/Kconfig b/arch/powerpc/platforms/85xx/Kconfig
index 2992c35..ae08633 100644
--- a/arch/powerpc/platforms/85xx/Kconfig
+++ b/arch/powerpc/platforms/85xx/Kconfig
@@ -295,3 +295,4 @@ config TQM85xx
 
 config FSL_QORIQ_PM
 	bool
+	select FSL_SLEEP_FSM
diff --git a/arch/powerpc/platforms/85xx/Makefile b/arch/powerpc/platforms/85xx/Makefile
index a82106d16..169766f 100644
--- a/arch/powerpc/platforms/85xx/Makefile
+++ b/arch/powerpc/platforms/85xx/Makefile
@@ -2,10 +2,8 @@
 # Makefile for the PowerPC 85xx linux kernel.
 #
 obj-$(CONFIG_SMP) += smp.o
-ifneq ($(CONFIG_PPC_E500MC),y)
 obj-$(CONFIG_SUSPEND)	+= sleep.o
-endif
-obj-$(FSL_QORIQ_PM)	  += qoriq_pm.o
+obj-$(CONFIG_FSL_QORIQ_PM) += qoriq_pm.o deepsleep.o
 
 obj-y += common.o
 
diff --git a/arch/powerpc/platforms/85xx/deepsleep.c b/arch/powerpc/platforms/85xx/deepsleep.c
new file mode 100644
index 0000000..4a41020
--- /dev/null
+++ b/arch/powerpc/platforms/85xx/deepsleep.c
@@ -0,0 +1,182 @@
+/*
+ * Support deep sleep feature for T104x
+ *
+ * Copyright 2014 Freescale Semiconductor Inc.
+ *
+ * Author: Chenhui Zhao <chenhui.zhao@freescale.com>
+ *
+ * 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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/of_platform.h>
+#include <linux/of_address.h>
+#include <sysdev/fsl_soc.h>
+#include <asm/machdep.h>
+#include <asm/fsl_pm.h>
+
+#define SIZE_1MB	0x100000
+#define SIZE_2MB	0x200000
+
+#define CCSR_SCFG_DPSLPCR	0xfc000
+#define CCSR_SCFG_DPSLPCR_WDRR_EN	0x1
+#define CCSR_SCFG_SPARECR2	0xfc504
+#define CCSR_SCFG_SPARECR3	0xfc508
+
+#define CCSR_GPIO1_GPDIR	0x130000
+#define CCSR_GPIO1_GPODR	0x130004
+#define CCSR_GPIO1_GPDAT	0x130008
+#define CCSR_GPIO1_GPDIR_29	0x4
+
+/* 128 bytes buffer for restoring data broke by DDR training initialization */
+#define DDR_BUF_SIZE	128
+static u8 ddr_buff[DDR_BUF_SIZE] __aligned(64);
+
+static void *dcsr_base, *ccsr_base, *pld_base;
+static int pld_flag;
+
+int fsl_dp_iomap(void)
+{
+	struct device_node *np;
+	int ret = 0;
+	phys_addr_t ccsr_phy_addr, dcsr_phy_addr;
+
+	ccsr_phy_addr = get_immrbase();
+	if (ccsr_phy_addr == -1) {
+		pr_err("%s: Can't get the address of CCSR\n", __func__);
+		ret = -EINVAL;
+		goto ccsr_err;
+	}
+	ccsr_base = ioremap(ccsr_phy_addr, SIZE_2MB);
+	if (!ccsr_base) {
+		ret = -ENOMEM;
+		goto ccsr_err;
+	}
+
+	dcsr_phy_addr = get_dcsrbase();
+	if (dcsr_phy_addr == -1) {
+		pr_err("%s: Can't get the address of DCSR\n", __func__);
+		ret = -EINVAL;
+		goto dcsr_err;
+	}
+	dcsr_base = ioremap(dcsr_phy_addr, SIZE_1MB);
+	if (!dcsr_base) {
+		ret = -ENOMEM;
+		goto dcsr_err;
+	}
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,tetra-fpga");
+	if (np) {
+		pld_flag = T1040QDS_TETRA_FLAG;
+	} else {
+		np = of_find_compatible_node(NULL, NULL, "fsl,t104xrdb-cpld");
+		if (np) {
+			pld_flag = T104xRDB_CPLD_FLAG;
+		} else {
+			pr_err("%s: Can't find the FPGA/CPLD node\n",
+					__func__);
+			ret = -EINVAL;
+			goto pld_err;
+		}
+	}
+	pld_base = of_iomap(np, 0);
+	of_node_put(np);
+
+	return 0;
+
+pld_err:
+	iounmap(dcsr_base);
+dcsr_err:
+	iounmap(ccsr_base);
+ccsr_err:
+	ccsr_base = NULL;
+	dcsr_base = NULL;
+	pld_base = NULL;
+	return ret;
+}
+
+void fsl_dp_iounmap(void)
+{
+	if (dcsr_base) {
+		iounmap(dcsr_base);
+		dcsr_base = NULL;
+	}
+
+	if (ccsr_base) {
+		iounmap(ccsr_base);
+		ccsr_base = NULL;
+	}
+
+	if (pld_base) {
+		iounmap(pld_base);
+		pld_base = NULL;
+	}
+}
+
+static void fsl_dp_ddr_save(void *ccsr_base)
+{
+	u32 ddr_buff_addr;
+
+	/*
+	 * DDR training initialization will break 128 bytes at the beginning
+	 * of DDR, therefore, save them so that the bootloader will restore
+	 * them. Assume that DDR is mapped to the address space started with
+	 * CONFIG_PAGE_OFFSET.
+	 */
+	memcpy(ddr_buff, (void *)CONFIG_PAGE_OFFSET, DDR_BUF_SIZE);
+
+	/* assume ddr_buff is in the physical address space of 4GB */
+	ddr_buff_addr = (u32)(__pa(ddr_buff) & 0xffffffff);
+
+	/*
+	 * the bootloader will restore the first 128 bytes of DDR from
+	 * the location indicated by the register SPARECR3
+	 */
+	out_be32(ccsr_base + CCSR_SCFG_SPARECR3, ddr_buff_addr);
+}
+
+static void fsl_dp_set_resume_pointer(void *ccsr_base)
+{
+	u32 resume_addr;
+
+	/* the bootloader will finally jump to this address to return kernel */
+#ifdef CONFIG_PPC32
+	resume_addr = (u32)(__pa(fsl_booke_deep_sleep_resume));
+#else
+	resume_addr = (u32)(__pa(*(u64 *)fsl_booke_deep_sleep_resume)
+			    & 0xffffffff);
+#endif
+
+	/* use the register SPARECR2 to save the resume address */
+	out_be32(ccsr_base + CCSR_SCFG_SPARECR2, resume_addr);
+
+}
+
+int fsl_enter_epu_deepsleep(void)
+{
+	fsl_dp_ddr_save(ccsr_base);
+
+	fsl_dp_set_resume_pointer(ccsr_base);
+
+	/*  enable Warm Device Reset request. */
+	setbits32(ccsr_base + CCSR_SCFG_DPSLPCR, CCSR_SCFG_DPSLPCR_WDRR_EN);
+
+	/* set GPIO1_29 as an output pin (not open-drain), and output 0 */
+	clrbits32(ccsr_base + CCSR_GPIO1_GPDAT, CCSR_GPIO1_GPDIR_29);
+	clrbits32(ccsr_base + CCSR_GPIO1_GPODR, CCSR_GPIO1_GPDIR_29);
+	setbits32(ccsr_base + CCSR_GPIO1_GPDIR, CCSR_GPIO1_GPDIR_29);
+
+	fsl_dp_fsm_setup(dcsr_base, NULL);
+
+	fsl_dp_enter_low(ccsr_base, dcsr_base, pld_base, pld_flag);
+
+	/* disable Warm Device Reset request */
+	clrbits32(ccsr_base + CCSR_SCFG_DPSLPCR, CCSR_SCFG_DPSLPCR_WDRR_EN);
+
+	fsl_dp_fsm_clean(dcsr_base, NULL);
+
+	return 0;
+}
diff --git a/arch/powerpc/platforms/85xx/qoriq_pm.c b/arch/powerpc/platforms/85xx/qoriq_pm.c
index 65c298b..b6718f0 100644
--- a/arch/powerpc/platforms/85xx/qoriq_pm.c
+++ b/arch/powerpc/platforms/85xx/qoriq_pm.c
@@ -18,6 +18,9 @@
 #include <asm/fsl_pm.h>
 
 #define FSL_SLEEP		0x1
+#define FSL_DEEP_SLEEP		0x2
+
+int (*fsl_enter_deepsleep)(void);
 
 /* specify the sleep state of the present platform */
 unsigned int sleep_pm_state;
@@ -27,6 +30,7 @@ static unsigned int sleep_modes;
 static int qoriq_suspend_enter(suspend_state_t state)
 {
 	int ret = 0;
+	int cpu;
 
 	switch (state) {
 	case PM_SUSPEND_STANDBY:
@@ -38,6 +42,17 @@ static int qoriq_suspend_enter(suspend_state_t state)
 
 		break;
 
+	case PM_SUSPEND_MEM:
+
+		cpu = smp_processor_id();
+		qoriq_pm_ops->irq_mask(cpu);
+
+		ret = fsl_enter_deepsleep();
+
+		qoriq_pm_ops->irq_unmask(cpu);
+
+		break;
+
 	default:
 		ret = -EINVAL;
 
@@ -51,12 +66,30 @@ static int qoriq_suspend_valid(suspend_state_t state)
 	if (state == PM_SUSPEND_STANDBY && (sleep_modes & FSL_SLEEP))
 		return 1;
 
+	if (state == PM_SUSPEND_MEM && (sleep_modes & FSL_DEEP_SLEEP))
+		return 1;
+
 	return 0;
 }
 
+static int qoriq_suspend_begin(suspend_state_t state)
+{
+	if (state == PM_SUSPEND_MEM)
+		return fsl_dp_iomap();
+
+	return 0;
+}
+
+static void qoriq_suspend_end(void)
+{
+	fsl_dp_iounmap();
+}
+
 static const struct platform_suspend_ops qoriq_suspend_ops = {
 	.valid = qoriq_suspend_valid,
 	.enter = qoriq_suspend_enter,
+	.begin = qoriq_suspend_begin,
+	.end = qoriq_suspend_end,
 };
 
 static int __init qoriq_suspend_init(void)
@@ -70,6 +103,12 @@ static int __init qoriq_suspend_init(void)
 	if (np)
 		sleep_pm_state = PLAT_PM_LPM20;
 
+	np = of_find_compatible_node(NULL, NULL, "fsl,t1040-rcpm");
+	if (np) {
+		fsl_enter_deepsleep = fsl_enter_epu_deepsleep;
+		sleep_modes |= FSL_DEEP_SLEEP;
+	}
+
 	suspend_set_ops(&qoriq_suspend_ops);
 
 	return 0;
diff --git a/arch/powerpc/platforms/85xx/sleep.S b/arch/powerpc/platforms/85xx/sleep.S
index b272f0c..cf4da3c 100644
--- a/arch/powerpc/platforms/85xx/sleep.S
+++ b/arch/powerpc/platforms/85xx/sleep.S
@@ -1,9 +1,9 @@
 /*
- * Enter and leave deep sleep/sleep state on MPC85xx
+ * Enter and leave deep sleep/sleep state
  *
  * Author: Scott Wood <scottwood@freescale.com>
  *
- * Copyright (C) 2006-2012 Freescale Semiconductor, Inc. All rights reserved.
+ * Copyright (C) 2006-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 version 2 as published
@@ -14,7 +14,193 @@
 #include <asm/ppc_asm.h>
 #include <asm/reg.h>
 #include <asm/asm-offsets.h>
+#include <asm/fsl_pm.h>
+#include <asm/mmu.h>
 
+/*
+ * the number of bytes occupied by one register
+ * the value of 8 is compatible with both 32-bit and 64-bit registers
+ */
+#define STRIDE_SIZE		8
+
+/* GPR0 - GPR31 */
+#define BOOKE_GPR0_OFF		0x0000
+#define BOOKE_GPR_COUNT		32
+/* IVOR0 - IVOR42 */
+#define BOOKE_IVOR0_OFF	   (BOOKE_GPR0_OFF + BOOKE_GPR_COUNT * STRIDE_SIZE)
+#define BOOKE_IVOR_COUNT	43
+/* SPRG0 - SPRG9 */
+#define BOOKE_SPRG0_OFF	   (BOOKE_IVOR0_OFF + BOOKE_IVOR_COUNT * STRIDE_SIZE)
+#define BOOKE_SPRG_COUNT	10
+/* IVPR */
+#define BOOKE_IVPR_OFF	   (BOOKE_SPRG0_OFF + BOOKE_SPRG_COUNT * STRIDE_SIZE)
+
+#define BOOKE_LR_OFF		(BOOKE_IVPR_OFF + STRIDE_SIZE)
+#define BOOKE_MSR_OFF		(BOOKE_LR_OFF + STRIDE_SIZE)
+#define BOOKE_TBU_OFF		(BOOKE_MSR_OFF + STRIDE_SIZE)
+#define BOOKE_TBL_OFF		(BOOKE_TBU_OFF + STRIDE_SIZE)
+#define BOOKE_EPCR_OFF		(BOOKE_TBL_OFF + STRIDE_SIZE)
+#define BOOKE_HID0_OFF		(BOOKE_EPCR_OFF + STRIDE_SIZE)
+#define BOOKE_PIR_OFF		(BOOKE_HID0_OFF + STRIDE_SIZE)
+#define BOOKE_PID0_OFF		(BOOKE_PIR_OFF + STRIDE_SIZE)
+#define BOOKE_BUCSR_OFF		(BOOKE_PID0_OFF + STRIDE_SIZE)
+
+#define BUFFER_SIZE		(BOOKE_BUCSR_OFF + STRIDE_SIZE)
+
+#undef SAVE_GPR
+#define SAVE_GPR(gpr, offset) \
+	PPC_STL gpr, offset(r10)
+
+#define RESTORE_GPR(gpr, offset) \
+	PPC_LL gpr, offset(r10)
+
+#define SAVE_SPR(spr, offset) \
+	mfspr	r0, spr ;\
+	PPC_STL	r0, offset(r10)
+
+#define RESTORE_SPR(spr, offset) \
+	PPC_LL	r0, offset(r10) ;\
+	mtspr	spr, r0
+
+#define SAVE_ALL_GPR \
+	SAVE_GPR(r1, BOOKE_GPR0_OFF + STRIDE_SIZE * 1) ;\
+	SAVE_GPR(r2, BOOKE_GPR0_OFF + STRIDE_SIZE * 2) ;\
+	SAVE_GPR(r13, BOOKE_GPR0_OFF + STRIDE_SIZE * 13) ;\
+	SAVE_GPR(r14, BOOKE_GPR0_OFF + STRIDE_SIZE * 14) ;\
+	SAVE_GPR(r15, BOOKE_GPR0_OFF + STRIDE_SIZE * 15) ;\
+	SAVE_GPR(r16, BOOKE_GPR0_OFF + STRIDE_SIZE * 16) ;\
+	SAVE_GPR(r17, BOOKE_GPR0_OFF + STRIDE_SIZE * 17) ;\
+	SAVE_GPR(r18, BOOKE_GPR0_OFF + STRIDE_SIZE * 18) ;\
+	SAVE_GPR(r19, BOOKE_GPR0_OFF + STRIDE_SIZE * 19) ;\
+	SAVE_GPR(r20, BOOKE_GPR0_OFF + STRIDE_SIZE * 20) ;\
+	SAVE_GPR(r21, BOOKE_GPR0_OFF + STRIDE_SIZE * 21) ;\
+	SAVE_GPR(r22, BOOKE_GPR0_OFF + STRIDE_SIZE * 22) ;\
+	SAVE_GPR(r23, BOOKE_GPR0_OFF + STRIDE_SIZE * 23) ;\
+	SAVE_GPR(r24, BOOKE_GPR0_OFF + STRIDE_SIZE * 24) ;\
+	SAVE_GPR(r25, BOOKE_GPR0_OFF + STRIDE_SIZE * 25) ;\
+	SAVE_GPR(r26, BOOKE_GPR0_OFF + STRIDE_SIZE * 26) ;\
+	SAVE_GPR(r27, BOOKE_GPR0_OFF + STRIDE_SIZE * 27) ;\
+	SAVE_GPR(r28, BOOKE_GPR0_OFF + STRIDE_SIZE * 28) ;\
+	SAVE_GPR(r29, BOOKE_GPR0_OFF + STRIDE_SIZE * 29) ;\
+	SAVE_GPR(r30, BOOKE_GPR0_OFF + STRIDE_SIZE * 30) ;\
+	SAVE_GPR(r31, BOOKE_GPR0_OFF + STRIDE_SIZE * 31)
+
+#define RESTORE_ALL_GPR \
+	RESTORE_GPR(r1, BOOKE_GPR0_OFF + STRIDE_SIZE * 1) ;\
+	RESTORE_GPR(r2, BOOKE_GPR0_OFF + STRIDE_SIZE * 2) ;\
+	RESTORE_GPR(r13, BOOKE_GPR0_OFF + STRIDE_SIZE * 13) ;\
+	RESTORE_GPR(r14, BOOKE_GPR0_OFF + STRIDE_SIZE * 14) ;\
+	RESTORE_GPR(r15, BOOKE_GPR0_OFF + STRIDE_SIZE * 15) ;\
+	RESTORE_GPR(r16, BOOKE_GPR0_OFF + STRIDE_SIZE * 16) ;\
+	RESTORE_GPR(r17, BOOKE_GPR0_OFF + STRIDE_SIZE * 17) ;\
+	RESTORE_GPR(r18, BOOKE_GPR0_OFF + STRIDE_SIZE * 18) ;\
+	RESTORE_GPR(r19, BOOKE_GPR0_OFF + STRIDE_SIZE * 19) ;\
+	RESTORE_GPR(r20, BOOKE_GPR0_OFF + STRIDE_SIZE * 20) ;\
+	RESTORE_GPR(r21, BOOKE_GPR0_OFF + STRIDE_SIZE * 21) ;\
+	RESTORE_GPR(r22, BOOKE_GPR0_OFF + STRIDE_SIZE * 22) ;\
+	RESTORE_GPR(r23, BOOKE_GPR0_OFF + STRIDE_SIZE * 23) ;\
+	RESTORE_GPR(r24, BOOKE_GPR0_OFF + STRIDE_SIZE * 24) ;\
+	RESTORE_GPR(r25, BOOKE_GPR0_OFF + STRIDE_SIZE * 25) ;\
+	RESTORE_GPR(r26, BOOKE_GPR0_OFF + STRIDE_SIZE * 26) ;\
+	RESTORE_GPR(r27, BOOKE_GPR0_OFF + STRIDE_SIZE * 27) ;\
+	RESTORE_GPR(r28, BOOKE_GPR0_OFF + STRIDE_SIZE * 28) ;\
+	RESTORE_GPR(r29, BOOKE_GPR0_OFF + STRIDE_SIZE * 29) ;\
+	RESTORE_GPR(r30, BOOKE_GPR0_OFF + STRIDE_SIZE * 30) ;\
+	RESTORE_GPR(r31, BOOKE_GPR0_OFF + STRIDE_SIZE * 31)
+
+#define SAVE_ALL_SPRG \
+	SAVE_SPR(SPRN_SPRG0, BOOKE_SPRG0_OFF + STRIDE_SIZE * 0) ;\
+	SAVE_SPR(SPRN_SPRG1, BOOKE_SPRG0_OFF + STRIDE_SIZE * 1) ;\
+	SAVE_SPR(SPRN_SPRG2, BOOKE_SPRG0_OFF + STRIDE_SIZE * 2) ;\
+	SAVE_SPR(SPRN_SPRG3, BOOKE_SPRG0_OFF + STRIDE_SIZE * 3) ;\
+	SAVE_SPR(SPRN_SPRG4, BOOKE_SPRG0_OFF + STRIDE_SIZE * 4) ;\
+	SAVE_SPR(SPRN_SPRG5, BOOKE_SPRG0_OFF + STRIDE_SIZE * 5) ;\
+	SAVE_SPR(SPRN_SPRG6, BOOKE_SPRG0_OFF + STRIDE_SIZE * 6) ;\
+	SAVE_SPR(SPRN_SPRG7, BOOKE_SPRG0_OFF + STRIDE_SIZE * 7) ;\
+	SAVE_SPR(SPRN_SPRG8, BOOKE_SPRG0_OFF + STRIDE_SIZE * 8) ;\
+	SAVE_SPR(SPRN_SPRG9, BOOKE_SPRG0_OFF + STRIDE_SIZE * 9)
+
+#define RESTORE_ALL_SPRG \
+	RESTORE_SPR(SPRN_SPRG0, BOOKE_SPRG0_OFF + STRIDE_SIZE * 0) ;\
+	RESTORE_SPR(SPRN_SPRG1, BOOKE_SPRG0_OFF + STRIDE_SIZE * 1) ;\
+	RESTORE_SPR(SPRN_SPRG2, BOOKE_SPRG0_OFF + STRIDE_SIZE * 2) ;\
+	RESTORE_SPR(SPRN_SPRG3, BOOKE_SPRG0_OFF + STRIDE_SIZE * 3) ;\
+	RESTORE_SPR(SPRN_SPRG4, BOOKE_SPRG0_OFF + STRIDE_SIZE * 4) ;\
+	RESTORE_SPR(SPRN_SPRG5, BOOKE_SPRG0_OFF + STRIDE_SIZE * 5) ;\
+	RESTORE_SPR(SPRN_SPRG6, BOOKE_SPRG0_OFF + STRIDE_SIZE * 6) ;\
+	RESTORE_SPR(SPRN_SPRG7, BOOKE_SPRG0_OFF + STRIDE_SIZE * 7) ;\
+	RESTORE_SPR(SPRN_SPRG8, BOOKE_SPRG0_OFF + STRIDE_SIZE * 8) ;\
+	RESTORE_SPR(SPRN_SPRG9, BOOKE_SPRG0_OFF + STRIDE_SIZE * 9)
+
+#define SAVE_ALL_IVOR \
+	SAVE_SPR(SPRN_IVOR0, BOOKE_IVOR0_OFF + STRIDE_SIZE * 0) ;\
+	SAVE_SPR(SPRN_IVOR1, BOOKE_IVOR0_OFF + STRIDE_SIZE * 1) ;\
+	SAVE_SPR(SPRN_IVOR2, BOOKE_IVOR0_OFF + STRIDE_SIZE * 2) ;\
+	SAVE_SPR(SPRN_IVOR3, BOOKE_IVOR0_OFF + STRIDE_SIZE * 3) ;\
+	SAVE_SPR(SPRN_IVOR4, BOOKE_IVOR0_OFF + STRIDE_SIZE * 4) ;\
+	SAVE_SPR(SPRN_IVOR5, BOOKE_IVOR0_OFF + STRIDE_SIZE * 5) ;\
+	SAVE_SPR(SPRN_IVOR6, BOOKE_IVOR0_OFF + STRIDE_SIZE * 6) ;\
+	SAVE_SPR(SPRN_IVOR7, BOOKE_IVOR0_OFF + STRIDE_SIZE * 7) ;\
+	SAVE_SPR(SPRN_IVOR8, BOOKE_IVOR0_OFF + STRIDE_SIZE * 8) ;\
+	SAVE_SPR(SPRN_IVOR9, BOOKE_IVOR0_OFF + STRIDE_SIZE * 9) ;\
+	SAVE_SPR(SPRN_IVOR10, BOOKE_IVOR0_OFF + STRIDE_SIZE * 10) ;\
+	SAVE_SPR(SPRN_IVOR11, BOOKE_IVOR0_OFF + STRIDE_SIZE * 11) ;\
+	SAVE_SPR(SPRN_IVOR12, BOOKE_IVOR0_OFF + STRIDE_SIZE * 12) ;\
+	SAVE_SPR(SPRN_IVOR13, BOOKE_IVOR0_OFF + STRIDE_SIZE * 13) ;\
+	SAVE_SPR(SPRN_IVOR14, BOOKE_IVOR0_OFF + STRIDE_SIZE * 14) ;\
+	SAVE_SPR(SPRN_IVOR15, BOOKE_IVOR0_OFF + STRIDE_SIZE * 15) ;\
+	SAVE_SPR(SPRN_IVOR35, BOOKE_IVOR0_OFF + STRIDE_SIZE * 35) ;\
+	SAVE_SPR(SPRN_IVOR36, BOOKE_IVOR0_OFF + STRIDE_SIZE * 36) ;\
+	SAVE_SPR(SPRN_IVOR37, BOOKE_IVOR0_OFF + STRIDE_SIZE * 37) ;\
+	SAVE_SPR(SPRN_IVOR38, BOOKE_IVOR0_OFF + STRIDE_SIZE * 38) ;\
+	SAVE_SPR(SPRN_IVOR39, BOOKE_IVOR0_OFF + STRIDE_SIZE * 39) ;\
+	SAVE_SPR(SPRN_IVOR40, BOOKE_IVOR0_OFF + STRIDE_SIZE * 40) ;\
+	SAVE_SPR(SPRN_IVOR41, BOOKE_IVOR0_OFF + STRIDE_SIZE * 41)
+
+#define RESTORE_ALL_IVOR \
+	RESTORE_SPR(SPRN_IVOR0, BOOKE_IVOR0_OFF + STRIDE_SIZE * 0) ;\
+	RESTORE_SPR(SPRN_IVOR1, BOOKE_IVOR0_OFF + STRIDE_SIZE * 1) ;\
+	RESTORE_SPR(SPRN_IVOR2, BOOKE_IVOR0_OFF + STRIDE_SIZE * 2) ;\
+	RESTORE_SPR(SPRN_IVOR3, BOOKE_IVOR0_OFF + STRIDE_SIZE * 3) ;\
+	RESTORE_SPR(SPRN_IVOR4, BOOKE_IVOR0_OFF + STRIDE_SIZE * 4) ;\
+	RESTORE_SPR(SPRN_IVOR5, BOOKE_IVOR0_OFF + STRIDE_SIZE * 5) ;\
+	RESTORE_SPR(SPRN_IVOR6, BOOKE_IVOR0_OFF + STRIDE_SIZE * 6) ;\
+	RESTORE_SPR(SPRN_IVOR7, BOOKE_IVOR0_OFF + STRIDE_SIZE * 7) ;\
+	RESTORE_SPR(SPRN_IVOR8, BOOKE_IVOR0_OFF + STRIDE_SIZE * 8) ;\
+	RESTORE_SPR(SPRN_IVOR9, BOOKE_IVOR0_OFF + STRIDE_SIZE * 9) ;\
+	RESTORE_SPR(SPRN_IVOR10, BOOKE_IVOR0_OFF + STRIDE_SIZE * 10) ;\
+	RESTORE_SPR(SPRN_IVOR11, BOOKE_IVOR0_OFF + STRIDE_SIZE * 11) ;\
+	RESTORE_SPR(SPRN_IVOR12, BOOKE_IVOR0_OFF + STRIDE_SIZE * 12) ;\
+	RESTORE_SPR(SPRN_IVOR13, BOOKE_IVOR0_OFF + STRIDE_SIZE * 13) ;\
+	RESTORE_SPR(SPRN_IVOR14, BOOKE_IVOR0_OFF + STRIDE_SIZE * 14) ;\
+	RESTORE_SPR(SPRN_IVOR15, BOOKE_IVOR0_OFF + STRIDE_SIZE * 15) ;\
+	RESTORE_SPR(SPRN_IVOR35, BOOKE_IVOR0_OFF + STRIDE_SIZE * 35) ;\
+	RESTORE_SPR(SPRN_IVOR36, BOOKE_IVOR0_OFF + STRIDE_SIZE * 36) ;\
+	RESTORE_SPR(SPRN_IVOR37, BOOKE_IVOR0_OFF + STRIDE_SIZE * 37) ;\
+	RESTORE_SPR(SPRN_IVOR38, BOOKE_IVOR0_OFF + STRIDE_SIZE * 38) ;\
+	RESTORE_SPR(SPRN_IVOR39, BOOKE_IVOR0_OFF + STRIDE_SIZE * 39) ;\
+	RESTORE_SPR(SPRN_IVOR40, BOOKE_IVOR0_OFF + STRIDE_SIZE * 40) ;\
+	RESTORE_SPR(SPRN_IVOR41, BOOKE_IVOR0_OFF + STRIDE_SIZE * 41)
+
+/* reset time base to prevent from overflow */
+#define DELAY(count)		\
+	li	r3, count;	\
+	li	r4, 0;		\
+	mtspr	SPRN_TBWL, r4;	\
+101:	mfspr	r4, SPRN_TBRL;	\
+	cmpw	r4, r3;		\
+	blt	101b
+
+#define FSL_DIS_ALL_IRQ		\
+	mfmsr	r8;			\
+	rlwinm	r8, r8, 0, ~MSR_CE;	\
+	rlwinm	r8, r8, 0, ~MSR_ME;	\
+	rlwinm	r8, r8, 0, ~MSR_EE;	\
+	rlwinm	r8, r8, 0, ~MSR_DE;	\
+	mtmsr	r8;			\
+	isync
+
+#ifndef CONFIG_PPC_E500MC
 #define SS_TB		0x00
 #define SS_HID		0x08 /* 2 HIDs */
 #define SS_IAC		0x10 /* 2 IACs */
@@ -607,3 +793,367 @@ mpc85xx_deep_resume:
 	mtdec	r3
 
 	blr
+
+#else /* CONFIG_PPC_E500MC */
+
+	.section .data
+	.align	6
+regs_buffer:
+	.space BUFFER_SIZE
+
+	.section .text
+/*
+ * Save CPU registers
+ * r3 : the base address of the buffer which stores the values of registers
+ */
+e5500_cpu_state_save:
+	/* store the base address to r10 */
+	mr	r10, r3
+
+	SAVE_ALL_GPR
+	SAVE_ALL_SPRG
+	SAVE_ALL_IVOR
+
+	SAVE_SPR(SPRN_IVPR, BOOKE_IVPR_OFF)
+	SAVE_SPR(SPRN_PID0, BOOKE_PID0_OFF)
+	SAVE_SPR(SPRN_EPCR, BOOKE_EPCR_OFF)
+	SAVE_SPR(SPRN_HID0, BOOKE_HID0_OFF)
+	SAVE_SPR(SPRN_PIR, BOOKE_PIR_OFF)
+	SAVE_SPR(SPRN_BUCSR, BOOKE_BUCSR_OFF)
+1:
+	mfspr	r5, SPRN_TBRU
+	mfspr	r4, SPRN_TBRL
+	SAVE_GPR(r5, BOOKE_TBU_OFF)
+	SAVE_GPR(r4, BOOKE_TBL_OFF)
+	mfspr	r3, SPRN_TBRU
+	cmpw	r3, r5
+	bne	1b
+
+	blr
+
+/*
+ * Restore CPU registers
+ * r3 : the base address of the buffer which stores the values of registers
+ */
+e5500_cpu_state_restore:
+	/* store the base address to r10 */
+	mr	r10, r3
+
+	RESTORE_ALL_GPR
+	RESTORE_ALL_SPRG
+	RESTORE_ALL_IVOR
+
+	RESTORE_SPR(SPRN_IVPR, BOOKE_IVPR_OFF)
+	RESTORE_SPR(SPRN_PID0, BOOKE_PID0_OFF)
+	RESTORE_SPR(SPRN_EPCR, BOOKE_EPCR_OFF)
+	RESTORE_SPR(SPRN_HID0, BOOKE_HID0_OFF)
+	RESTORE_SPR(SPRN_PIR, BOOKE_PIR_OFF)
+	RESTORE_SPR(SPRN_BUCSR, BOOKE_BUCSR_OFF)
+
+	li	r0, 0
+	mtspr	SPRN_TBWL, r0
+	RESTORE_SPR(SPRN_TBWU, BOOKE_TBU_OFF)
+	RESTORE_SPR(SPRN_TBWL, BOOKE_TBL_OFF)
+
+	blr
+
+#define CPC_CPCCSR0		0x0
+#define CPC_CPCCSR0_CPCFL	0x800
+
+/*
+ * Flush the CPC cache.
+ * r3 : the base address of CPC
+ */
+flush_cpc_cache:
+	lwz	r6, CPC_CPCCSR0(r3)
+	ori	r6, r6, CPC_CPCCSR0_CPCFL
+	stw	r6, CPC_CPCCSR0(r3)
+	sync
+
+	/* Wait until completing the flush */
+1:	lwz	r6, CPC_CPCCSR0(r3)
+	andi.	r6, r6, CPC_CPCCSR0_CPCFL
+	bne	1b
+
+	blr
+
+/*
+ * the last stage to enter deep sleep
+ *
+ */
+	.align 6
+_GLOBAL(fsl_dp_enter_low)
+deepsleep_start:
+	LOAD_REG_ADDR(r9, buf_tmp)
+	/* save the return address and MSR */
+	mflr	r8
+	PPC_STL r8, 0(r9)
+	mfmsr	r8
+	PPC_STL r8, 8(r9)
+	mfspr	r8, SPRN_TCR
+	PPC_STL r8, 16(r9)
+	li	r8, 0
+	mtspr	SPRN_TCR, r8
+
+	/* save the parameters */
+	PPC_STL	r3, 32(r9)
+	PPC_STL	r4, 40(r9)
+	PPC_STL	r5, 48(r9)
+	PPC_STL	r6, 56(r9)
+
+	LOAD_REG_ADDR(r3, regs_buffer)
+	bl	e5500_cpu_state_save
+
+	/* restore the parameters */
+	LOAD_REG_ADDR(r9, buf_tmp)
+	PPC_LL	r31, 32(r9)
+	PPC_LL	r30, 40(r9)
+	PPC_LL	r29, 48(r9)
+	PPC_LL	r28, 56(r9)
+
+	/* flush caches inside CPU */
+	LOAD_REG_ADDR(r3, cur_cpu_spec)
+	PPC_LL	r3, 0(r3)
+	PPC_LL	r3, CPU_FLUSH_CACHES(r3)
+	PPC_LCMPI  0, r3, 0
+	beq	6f
+#ifdef CONFIG_PPC64
+	PPC_LL	r3, 0(r3)
+#endif
+	mtctr	r3
+	bctrl
+6:
+	/* Flush the CPC cache */
+#define CPC_OFFSET	0x10000
+	mr	r3, r31
+	addis	r3, r3, CPC_OFFSET@h
+	bl	flush_cpc_cache
+
+	/* prefecth TLB */
+#define CCSR_GPIO1_GPDAT	0x130008
+#define CCSR_GPIO1_GPDAT_29	0x4
+	LOAD_REG_IMMEDIATE(r11, CCSR_GPIO1_GPDAT)
+	add	r11, r31, r11
+	lwz	r10, 0(r11)
+
+#define CCSR_RCPM_PCPH15SETR	0xe20b4
+#define CCSR_RCPM_PCPH15SETR_CORE0	0x1
+	LOAD_REG_IMMEDIATE(r12, CCSR_RCPM_PCPH15SETR)
+	add	r12, r31, r12
+	lwz	r10, 0(r12)
+
+#define CCSR_DDR_SDRAM_CFG_2	0x8114
+#define CCSR_DDR_SDRAM_CFG_2_FRC_SR	0x80000000
+	LOAD_REG_IMMEDIATE(r13, CCSR_DDR_SDRAM_CFG_2)
+	add	r13, r31, r13
+	lwz	r10, 0(r13)
+
+#define	DCSR_EPU_EPGCR		0x000
+#define DCSR_EPU_EPGCR_GCE	0x80000000
+	li	r14, DCSR_EPU_EPGCR
+	add	r14, r30, r14
+	lwz	r10, 0(r14)
+
+#define	DCSR_EPU_EPECR15	0x33C
+#define DCSR_EPU_EPECR15_IC0	0x80000000
+	li	r15, DCSR_EPU_EPECR15
+	add	r15, r30, r15
+	lwz	r10, 0(r15)
+
+#define CCSR_SCFG_QMIFRSTCR		0xfc40c
+#define CCSR_SCFG_QMIFRSTCR_QMIFRST	0x80000000
+	LOAD_REG_IMMEDIATE(r16, CCSR_SCFG_QMIFRSTCR)
+	add	r16, r31, r16
+	lwz	r10, 0(r16)
+
+/*
+ * There are two kind of register maps, one for T1040QDS and
+ * the other for T104xRDB.
+ */
+#define T104XRDB_CPLD_MISCCSR		0x17
+#define T104XRDB_CPLD_MISCCSR_SLEEPEN	0x40
+#define T1040QDS_QIXIS_PWR_CTL2		0x21
+#define T1040QDS_QIXIS_PWR_CTL2_PCTL	0x2
+	li	r3, T1040QDS_QIXIS_PWR_CTL2
+	PPC_LCMPI  0, r28, T1040QDS_TETRA_FLAG
+	beq	20f
+	li	r3, T104XRDB_CPLD_MISCCSR
+20:	add	r29, r29, r3
+	lbz	r10, 0(r29)
+	sync
+
+	LOAD_REG_ADDR(r8, deepsleep_start)
+	LOAD_REG_ADDR(r9, deepsleep_end)
+
+	/* prefecth code to cache so that executing code after disable DDR */
+1:	icbtls	2, 0, r8
+	addi	r8, r8, 64
+	cmpw	r8, r9
+	blt	1b
+	sync
+
+	FSL_DIS_ALL_IRQ
+
+	/*
+	 * Place DDR controller in self refresh mode.
+	 * From here on, can't access DDR any more.
+	 */
+	lwz	r10, 0(r13)
+	oris	r10, r10, CCSR_DDR_SDRAM_CFG_2_FRC_SR@h
+	stw	r10, 0(r13)
+	lwz	r10, 0(r13)
+	sync
+
+	DELAY(500)
+
+	/*
+	 * Enable deep sleep signals by write external CPLD/FPGA register.
+	 * The bootloader will disable them when wakeup from deep sleep.
+	 */
+	lbz	r10, 0(r29)
+	li	r3, T1040QDS_QIXIS_PWR_CTL2_PCTL
+	PPC_LCMPI  0, r28, T1040QDS_TETRA_FLAG
+	beq	22f
+	li	r3, T104XRDB_CPLD_MISCCSR_SLEEPEN
+22:	or	r10, r10, r3
+	stb	r10, 0(r29)
+	lbz	r10, 0(r29)
+	sync
+
+	/*
+	 * Set GPIO1_29 to lock the signal MCKE down during deep sleep.
+	 * The bootloader will clear it when wakeup.
+	 */
+	lwz	r10, 0(r11)
+	ori	r10, r10, CCSR_GPIO1_GPDAT_29
+	stw	r10, 0(r11)
+	lwz	r10, 0(r11)
+
+	DELAY(100)
+
+	/* Reset QMan system bus interface */
+	lwz	r10, 0(r16)
+	oris	r10, r10, CCSR_SCFG_QMIFRSTCR_QMIFRST@h
+	stw	r10, 0(r16)
+	lwz	r10, 0(r16)
+
+	/* Enable all EPU Counters */
+	li	r10, 0
+	oris	r10, r10, DCSR_EPU_EPGCR_GCE@h
+	stw	r10, 0(r14)
+	lwz	r10, 0(r14)
+
+	/* Enable SCU15 to trigger on RCPM Concentrator 0 */
+	lwz	r10, 0(r15)
+	oris	r10, r10, DCSR_EPU_EPECR15_IC0@h
+	stw	r10, 0(r15)
+	lwz	r10, 0(r15)
+
+	/* put Core0 in PH15 mode, trigger EPU FSM */
+	lwz	r10, 0(r12)
+	ori	r10, r10, CCSR_RCPM_PCPH15SETR_CORE0
+	stw	r10, 0(r12)
+2:
+	b 2b
+
+	/*
+	 * Leave some space to prevent prefeching instruction
+	 * beyond deepsleep_end. The space also can be used as heap.
+	 */
+buf_tmp:
+	.space 128
+	.align 6
+deepsleep_end:
+
+	.align 12
+#ifdef CONFIG_PPC32
+_GLOBAL(fsl_booke_deep_sleep_resume)
+	/* disable interrupts */
+	FSL_DIS_ALL_IRQ
+
+#define ENTRY_MAPPING_BOOT_SETUP
+#include <../../kernel/fsl_booke_entry_mapping.S>
+#undef ENTRY_MAPPING_BOOT_SETUP
+
+	li	r3, 0
+	mfspr   r4, SPRN_PIR
+	bl	call_setup_cpu
+
+	/* Load each CAM entry */
+	LOAD_REG_ADDR(r3, tlbcam_index)
+	lwz	r3, 0(r3)
+	mtctr	r3
+	li	r0, 0
+3:	mr	r3, r0
+	bl	loadcam_entry
+	addi	r0, r0, 1
+	bdnz	3b
+
+	/* restore cpu registers */
+	LOAD_REG_ADDR(r3, regs_buffer)
+	bl	e5500_cpu_state_restore
+
+	/* restore return address */
+	LOAD_REG_ADDR(r3, buf_tmp)
+	ld	r4, 0(r3)
+	mtlr	r4
+	ld	r4, 8(r3)
+	mtmsr	r4
+
+	blr
+
+#else /* CONFIG_PPC32 */
+
+_GLOBAL(fsl_booke_deep_sleep_resume)
+	/* disable interrupts */
+	FSL_DIS_ALL_IRQ
+
+	/* switch to 64-bit mode */
+	bl	.enable_64b_mode
+
+	/* set TOC pointer */
+	bl	.relative_toc
+
+	/* setup initial TLBs, switch to kernel space ... */
+	bl	.start_initialization_book3e
+
+	/* address space changed, set TOC pointer again */
+	bl	.relative_toc
+
+	/* call a cpu state restore handler */
+	LOAD_REG_ADDR(r23, cur_cpu_spec)
+	ld	r23,0(r23)
+	ld	r23,CPU_SPEC_RESTORE(r23)
+	cmpdi	0,r23,0
+	beq	1f
+	ld	r23,0(r23)
+	mtctr	r23
+	bctrl
+1:
+	LOAD_REG_ADDR(r3, regs_buffer)
+	bl	e5500_cpu_state_restore
+
+	/* Load each CAM entry */
+	LOAD_REG_ADDR(r3, tlbcam_index)
+	lwz	r3, 0(r3)
+	mtctr	r3
+	li	r0, 0
+3:	mr	r3, r0
+	bl	loadcam_entry
+	addi	r0, r0, 1
+	bdnz	3b
+
+	/* restore return address */
+	LOAD_REG_ADDR(r3, buf_tmp)
+	ld	r4, 16(r3)
+	mtspr	SPRN_TCR, r4
+	ld	r4, 0(r3)
+	mtlr	r4
+	ld	r4, 8(r3)
+	mtmsr	r4
+
+	blr
+
+#endif /* CONFIG_PPC32 */
+
+#endif
diff --git a/arch/powerpc/sysdev/fsl_soc.c b/arch/powerpc/sysdev/fsl_soc.c
index 228cf91..6d5683b 100644
--- a/arch/powerpc/sysdev/fsl_soc.c
+++ b/arch/powerpc/sysdev/fsl_soc.c
@@ -46,6 +46,37 @@ extern void init_fcc_ioports(struct fs_platform_info*);
 extern void init_fec_ioports(struct fs_platform_info*);
 extern void init_smc_ioports(struct fs_uart_platform_info*);
 static phys_addr_t immrbase = -1;
+static phys_addr_t dcsrbase = -1;
+
+phys_addr_t get_dcsrbase(void)
+{
+	struct device_node *np;
+	const __be32 *prop;
+	int size;
+	u32 naddr;
+
+	if (dcsrbase != -1)
+		return dcsrbase;
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,dcsr");
+	if (!np)
+		return -1;
+
+	prop = of_get_property(np, "#address-cells", &size);
+	if (prop && size == 4)
+		naddr = be32_to_cpup(prop);
+	else
+		naddr = 2;
+
+	prop = of_get_property(np, "ranges", NULL);
+	if (prop)
+		dcsrbase = of_translate_address(np, prop + naddr);
+
+	of_node_put(np);
+
+	return dcsrbase;
+}
+EXPORT_SYMBOL(get_dcsrbase);
 
 phys_addr_t get_immrbase(void)
 {
diff --git a/arch/powerpc/sysdev/fsl_soc.h b/arch/powerpc/sysdev/fsl_soc.h
index 99fa2e5..a158c18 100644
--- a/arch/powerpc/sysdev/fsl_soc.h
+++ b/arch/powerpc/sysdev/fsl_soc.h
@@ -6,6 +6,7 @@
 
 struct spi_device;
 
+extern phys_addr_t get_dcsrbase(void);
 extern phys_addr_t get_immrbase(void);
 #if defined(CONFIG_CPM2) || defined(CONFIG_QUICC_ENGINE) || defined(CONFIG_8xx)
 extern u32 get_brgfreq(void);