[ARM/tegra] suspend: add support for LP0 and LP1 suspend modes

in both LP0 and LP1, SDRAM is placed into self-refresh. in order to safely
perform this transition, the final shutdown procedure responsible for

  * turning off the MMU and L1 data cache
  * putting memory into self-refresh
  * setting the DDR pads to the lowest power state
  * and turning off PLLs

is copied into IRAM (at the address TEGRA_IRAM_BASE + SZ_4K) at the
start of the suspend process.

in LP1 mode (like LP2), the CPU is reset and executes the code specified
at the EVP reset vector. since SDRAM is in self-refresh, this code must
also be located in IRAM, and it must re-enable DRAM before restoring the
full context. in this implementation, it enables the CPU on PLLP, enables
PLLC and PLLM, restores the SCLK burst policy, and jumps to the LP2 reset
vector to restore the rest of the system (MMU, PLLX, coresite, etc.). the
LP2 reset vector is expected to be found in PMC_SCRATCH1, and is
initialized during system-bootup

in LP0 mode, the core voltage domain is also shutoff. as a result, all
of the volatile state in the core voltage domain (e.g., pinmux registers,
clock registers, etc.) must be saved to memory so that it can be restored
after the system resumes. a limited set of wakeups are available from LP0,
and the correct levels for the wakeups must be programmed into the PMC
wakepad configuration register prior to system shutdown. on resume, the
system resets into the boot ROM, and the boot ROM restores SDRAM and other
system state using values saved during kernel initialization in the PMC
scratch registers

for simplicity, the outer cache is shutdown for both LP0 and LP1; it
is possible to optimize the LP1 routine to bypass outer cache shutdown
and restart

v2 fixes from Vik Kasivajhula:
* restore PLLC during LP1 resume
* fix typo which set the CPU clock burst policy to PLLM, rather than PLLP

Change-Id: Icb1d2cbcbac8503369a10d16fd5c8b561af5a35a
Reviewed-on: http://git-master/r/1773
Reviewed-by: Gary King <gking@nvidia.com>
Tested-by: Gary King <gking@nvidia.com>

4 files changed, 385 insertions, 40 deletions

diff --git a/arch/arm/mach-tegra/board-nvodm.c b/arch/arm/mach-tegra/board-nvodm.c
index 19416f7da720..8d6a1b6b5b36 100644
--- a/arch/arm/mach-tegra/board-nvodm.c
+++ b/arch/arm/mach-tegra/board-nvodm.c
@@ -1320,10 +1320,26 @@ static void __init tegra_setup_suspend(void)
 	if (!w || !nr_wake)
 		goto do_register;
 
+	plat->wake_enb = 0;
+	plat->wake_low = 0;
+	plat->wake_high = 0;
+	plat->wake_any = 0;
+
 	while (nr_wake--) {
 		unsigned int pad = w->WakeupPadNumber;
 		if (pad < ARRAY_SIZE(wakepad_irq) && w->enable)
-			enable_irq_wake(wakepad_irq[w->WakeupPadNumber]);
+			enable_irq_wake(wakepad_irq[pad]);
+
+		if (w->enable) {
+			plat->wake_enb |= (1 << pad);
+
+			if (w->Polarity == NvOdmWakeupPadPolarity_Low)
+				plat->wake_low |= (1 << pad);
+			else if (w->Polarity == NvOdmWakeupPadPolarity_High)
+				plat->wake_high |= (1 << pad);
+			else if (w->Polarity == NvOdmWakeupPadPolarity_AnyEdge)
+				plat->wake_any |= (1 << pad);
+		}
 		w++;
 	}
 
diff --git a/arch/arm/mach-tegra/cortex_a9_save.S b/arch/arm/mach-tegra/cortex_a9_save.S
index 4456f68c67d8..6c80e8e28b79 100644
--- a/arch/arm/mach-tegra/cortex_a9_save.S
+++ b/arch/arm/mach-tegra/cortex_a9_save.S
@@ -42,9 +42,18 @@
 #define CONTEXT_SIZE_WORDS_SHIFT 7
 #define CONTEXT_SIZE_WORDS (1<<CONTEXT_SIZE_WORDS_SHIFT)
 
+#define EMC_CFG				0xc
+#define EMC_ADR_CFG			0x10
+#define EMC_REFRESH			0x70
+#define EMC_NOP				0xdc
+#define EMC_SELF_REF			0xe0
+#define EMC_REQ_CTRL			0x2b0
+#define EMC_EMC_STATUS			0x2b4
+
 #define PMC_CTRL			0x0
 #define PMC_CTRL_BFI_SHIFT		8
 #define PMC_CTRL_BFI_WIDTH		9
+#define PMC_SCRATCH1			0x54
 #define PMC_SCRATCH38			0x134
 
 #define CLK_RESET_CCLK_BURST		0x20
@@ -146,6 +155,14 @@
 	orr	\pa, \pa, \tmp
 .endm
 
+.macro emc_device_mask, rd, base
+	ldr	\rd, [\base, #EMC_ADR_CFG]
+	tst	\rd, #(0x3<<24)
+	moveq	\rd, #(0x1<<8)		@ just 1 device
+	movne	\rd, #(0x3<<8)		@ 2 devices
+.endm
+
+
 /*
  *	__cortex_a9_save(unsigned int mode)
  *
@@ -481,7 +498,10 @@ __tear_down_master:
 	 * cause the cache state to get invalidated (although LP1 & LP2
 	 * preserve the data in the L2, the control words (L2X0_CTRL,
 	 * L2X0_AUX_CTRL, etc.) need to be cleaned to L3 so that they
-	 * will be visible on reboot */
+	 * will be visible on reboot.  skip this for LP0, since the L2 cache
+	 * will be shutdown before we reach this point */
+	tst	sp, #TEGRA_POWER_EFFECT_LP0
+	bne	__l2_clean_done
 	mov32	r0, (TEGRA_ARM_PL310_BASE-IO_CPU_PHYS+IO_CPU_VIRT)
 	add	r3, r8, #(CONTEXT_SIZE_WORDS*4)
 	bic	r8, r8, #0x1f
@@ -498,6 +518,7 @@ __tear_down_master:
 13:	ldr	r1, [r0, #L2X0_CACHE_SYNC]
 	tst	r1, #1
 	bne	13b
+__l2_clean_done:
 #endif
 
 	tst	sp, #TEGRA_POWER_SDRAM_SELFREFRESH
@@ -532,7 +553,11 @@ __tear_down_master:
 	isb
 	mcr	p15, 0, r3, c2, c0, 0	@ TTB 0
 	isb
-	mov32	r3, (IO_IRAM_PHYS)	@ __tear_down_master_sdram, in IRAM
+	mov32	r2, __tegra_lp1_reset
+	mov32	r3, __tear_down_master_sdram
+	sub	r2, r3, r2
+	mov32	r3, (TEGRA_IRAM_CODE_AREA)
+	add	r3, r2, r3
 	movne	pc, r3
 ENDPROC(__tear_down_master)
 	.type	__tear_down_master_data, %object
@@ -541,17 +566,164 @@ __tear_down_master_data:
 	.size	__tear_down_master_data, . - __tear_down_master_data
 
 /*  START OF ROUTINES COPIED TO IRAM  */
-	.align 5
+/*
+ *	__tegra_lp1_reset
+ *
+ *	  reset vector for LP1 restore; copied into IRAM during suspend.
+ *	  brings the system back up to a safe starting point (SDRAM out of
+ *	  self-refresh, PLLC, PLLM and PLLP reenabled, CPU running on PLLP,
+ *	  system clock running on the same PLL that it suspended at), and
+ *	  jumps to tegra_lp2_startup to restore PLLX and virtual addressing.
+ *	  physical address of tegra_lp2_startup expected to be stored in
+ *	  PMC_SCRATCH1
+ */
+	.align L1_CACHE_SHIFT
+ENTRY(__tegra_lp1_reset)
+__tegra_lp1_reset:
+	/* the CPU and system bus are running at 32KHz and executing from
+	 * IRAM when this code is executed; immediately switch to CLKM and
+	 * enable PLLP. */
+	mov32	r0, TEGRA_CLK_RESET_BASE
+	mov	r1, #(1<<28)
+	str	r1, [r0, #CLK_RESET_SCLK_BURST]
+	str	r1, [r0, #CLK_RESET_CCLK_BURST]
+	mov	r1, #0
+	str	r1, [r0, #CLK_RESET_SCLK_DIVIDER]
+	str	r1, [r0, #CLK_RESET_CCLK_DIVIDER]
+
+	ldr	r1, [r0, #CLK_RESET_PLLM_BASE]
+	tst	r1, #(1<<30)
+	orreq	r1, r1, #(1<<30)
+	streq	r1, [r0, #CLK_RESET_PLLM_BASE]
+	ldr	r1, [r0, #CLK_RESET_PLLP_BASE]
+	tst	r1, #(1<<30)
+	orreq	r1, r1, #(1<<30)
+	streq	r1, [r0, #CLK_RESET_PLLP_BASE]
+	ldr	r1, [r0, #CLK_RESET_PLLC_BASE]
+	tst	r1, #(1<<30)
+	orreq	r1, r1, #(1<<30)
+	streq	r1, [r0, #CLK_RESET_PLLC_BASE]
+	mov32	r7, TEGRA_TMRUS_BASE
+	ldr	r1, [r7]
+
+	/* since the optimized settings are still in SDRAM, there is
+	 * no need to store them back into the IRAM-local __lp1_pad_area */
+	add	r2, pc, #__lp1_pad_area-(.+8)
+padload:ldmia	r2!, {r3-r4}
+	cmp	r3, #0
+	beq	padload_done
+	str	r4, [r3]
+	b	padload
+padload_done:
+	ldr	r2, [r7]
+	add	r2, r2, #0x4	@ 4uS delay for DRAM pad restoration
+	wait_until r2, r7, r3
+	add	r1, r1, #0xff	@ 255uS delay for PLL stabilization
+	wait_until r1, r7, r3
+
+	str	r4, [r0, #CLK_RESET_SCLK_BURST]
+	mov32	r4, ((1<<28) | (4))	@ burst policy is PLLP
+	str	r4, [r0, #CLK_RESET_CCLK_BURST]
+
+	mov32	r0, TEGRA_EMC_BASE
+	ldr	r1, [r0, #EMC_CFG]
+	bic	r1, r1, #(1<<31)	@ disable DRAM_CLK_STOP
+	str	r1, [r0, #EMC_CFG]
+
+	mov	r1, #0
+	str	r1, [r0, #EMC_SELF_REF]	@ take DRAM out of self refresh
+	mov	r1, #1
+	str	r1, [r0, #EMC_NOP]
+	str	r1, [r0, #EMC_NOP]
+	str	r1, [r0, #EMC_REFRESH]
+
+	emc_device_mask r1, r0
+
+exit_selfrefresh_loop:
+	ldr	r2, [r0, #EMC_EMC_STATUS]
+	ands	r2, r2, r1
+	bne	exit_selfrefresh_loop
+
+	mov	r1, #0
+	str	r1, [r0, #EMC_REQ_CTRL]
+
+	mov32	r0, TEGRA_PMC_BASE
+	ldr	r0, [r0, #PMC_SCRATCH1]
+	mov	pc, r0
+ENDPROC(__tegra_lp1_reset)
+
+/*
+ *	__tear_down_master_sdram
+ *
+ *	  disables MMU, data cache, and puts SDRAM into self-refresh.
+ *	  must execute from IRAM.
+ */
+	.align L1_CACHE_SHIFT
 __tear_down_master_sdram:
 	mrc	p15, 0, r3, c1, c0, 0
-	bic	r3, r3, #(1<<0)
-	mcr	p15, 0, r3, c1, c0, 0	@ disable MMU to prevent page misses
-	/* FIXME: save off DDR pad settings, put SDRAM in self refresh,
-	 * jump to tear_down_master_pll_cpu */
-	b	.
+	bic	r3, r3, #((1<<0) | (1<<2))	@ disable MMU and data-cache
+	mcr	p15, 0, r3, c1, c0, 0		@ to avoid page faults & races
+	dsb
+	isb
+
+	mov32	r1, TEGRA_EMC_BASE
+	mov	r2, #3
+	str	r2, [r1, #EMC_REQ_CTRL]		@ stall incoming DRAM requests
+
+emcidle:ldr	r2, [r1, #EMC_EMC_STATUS]
+	tst	r2, #4
+	beq	emcidle
+
+	mov	r2, #1
+	str	r2, [r1, #EMC_SELF_REF]
+
+	emc_device_mask r2, r1
+
+emcself:ldr	r3, [r1, #EMC_EMC_STATUS]
+	and	r3, r3, r2
+	cmp	r3, r2
+	bne	emcself				@ loop until DDR in self-refresh
+
+	add	r2, pc, #__lp1_pad_area-(.+8)
+
+padsave:ldm	r2, {r0-r1}
+	cmp	r0, #0
+	beq	padsave_done
+	ldr	r3, [r0]
+	str	r1, [r0]
+	str	r3, [r2, #4]
+	add	r2, r2, #8
+	b	padsave
+padsave_done:
+
+	ldr	r0, [r5, #CLK_RESET_SCLK_BURST]
+	str	r0, [r2, #4]
+	dsb
+	b	__tear_down_master_pll_cpu
 ENDPROC(__tear_down_master_sdram)
 
-	.align 5
+	.align	L1_CACHE_SHIFT
+	.type	__lp1_pad_area, %object
+__lp1_pad_area:
+	.word	TEGRA_APB_MISC_BASE + 0x8c8 /* XM2CFGCPADCTRL */
+	.word	0x8
+	.word	TEGRA_APB_MISC_BASE + 0x8cc /* XM2CFGDPADCTRL */
+	.word	0x8
+	.word	TEGRA_APB_MISC_BASE + 0x8d0 /* XM2CLKCFGPADCTRL */
+	.word	0x0
+	.word	TEGRA_APB_MISC_BASE + 0x8d4 /* XM2COMPPADCTRL */
+	.word	0x8
+	.word	TEGRA_APB_MISC_BASE + 0x8d8 /* XM2VTTGENPADCTRL */
+	.word	0x5500
+	.word	TEGRA_APB_MISC_BASE + 0x8e4 /* XM2CFGCPADCTRL2 */
+	.word	0x08080040
+	.word	TEGRA_APB_MISC_BASE + 0x8e8 /* XM2CFGDPADCTRL2 */
+	.word	0x0
+	.word	0x0	/* end of list */
+	.word	0x0	/* sclk_burst_policy */
+	.size	__lp1_pad_area, . - __lp1_pad_area
+
+	.align L1_CACHE_SHIFT
 __tear_down_master_pll_cpu:
 	ldr	r0, [r4, #PMC_CTRL]
 	bfi	r0, sp, #PMC_CTRL_BFI_SHIFT, #PMC_CTRL_BFI_WIDTH
@@ -575,9 +747,6 @@ __tear_down_master_pll_cpu:
 	mov	r0, #0
 	str	r0, [r5, #CLK_RESET_CCLK_DIVIDER]
 	str	r0, [r5, #CLK_RESET_SCLK_DIVIDER]
-	mov	r0, #0		/* burst policy = 32KHz */
-	str	r0, [r5, #CLK_RESET_SCLK_BURST]
-	str	r0, [r5, #CLK_RESET_SCLK_DIVIDER]
 
 	/* 2 us delay between changing sclk and disabling PLLs */
 	wait_for_us r1, r7, r9
@@ -585,8 +754,8 @@ __tear_down_master_pll_cpu:
 	wait_until r1, r7, r9
 
 	/* switch to CLKS */
+	mov	r0, #0	/* burst policy = 32KHz */
 	str	r0, [r5, #CLK_RESET_SCLK_BURST]
-	str	r0, [r5, #CLK_RESET_CCLK_BURST]
 
 	/* disable PLLP, PLLM, PLLC in LP0 and LP1 states */
 	ldr	r0, [r5, #CLK_RESET_PLLM_BASE]
@@ -641,8 +810,9 @@ __put_cpu_in_reset:
 	b	.
 ENDPROC(__put_cpu_in_reset)
 
-
-/*
- */
-
-/*  END OF ROUTINES COPIED TO IRAM  */
+/* dummy symbol for end of IRAM */
+	.align L1_CACHE_SHIFT
+ENTRY(__tegra_iram_end)
+__tegra_iram_end:
+	b	.
+ENDPROC(__tegra_iram_end)
diff --git a/arch/arm/mach-tegra/power.h b/arch/arm/mach-tegra/power.h
index 967ccf9adc8c..78f0c30990fd 100644
--- a/arch/arm/mach-tegra/power.h
+++ b/arch/arm/mach-tegra/power.h
@@ -23,6 +23,8 @@
 #ifndef __MACH_TEGRA_POWER_H
 #define __MACH_TEGRA_POWER_H
 
+#include <asm/page.h>
+
 #define TEGRA_POWER_SDRAM_SELFREFRESH	0x400 /* SDRAM is in self-refresh */
 
 #define TEGRA_POWER_PWRREQ_POLARITY	0x1   /* core power request polarity */
@@ -36,6 +38,9 @@
 #define TEGRA_POWER_PMC_SHIFT		8
 #define TEGRA_POWER_PMC_MASK		0x1ff
 
+/* layout of IRAM used for LP1 save & restore */
+#define TEGRA_IRAM_CODE_AREA		TEGRA_IRAM_BASE + SZ_4K
+#define TEGRA_IRAM_CODE_SIZE		SZ_4K
 
 #ifndef __ASSEMBLY__
 void tegra_lp2_set_trigger(unsigned long cycles);
@@ -47,6 +52,10 @@ struct tegra_suspend_platform_data {
 	unsigned long cpu_off_timer;	/* CPU power off time us, LP2/LP1 */
 	unsigned long core_timer;  /* core power good time in ticks,  LP0 */
 	unsigned long core_off_timer;	/* core power off time ticks, LP0 */
+	unsigned long wake_enb;    /* mask of enabled wake pads */
+	unsigned long wake_high;   /* high-level-triggered wake pads */
+	unsigned long wake_low;    /* low-level-triggered wake pads */
+	unsigned long wake_any;    /* any-edge-triggered wake pads */
 	bool dram_suspend;         /* platform supports DRAM self-refresh */
 	bool core_off;             /* platform supports core voltage off */
 	bool corereq_high;         /* Core power request active-high */
diff --git a/arch/arm/mach-tegra/suspend.c b/arch/arm/mach-tegra/suspend.c
index 9cdd805709d8..e99dda3a3b65 100644
--- a/arch/arm/mach-tegra/suspend.c
+++ b/arch/arm/mach-tegra/suspend.c
@@ -71,6 +71,11 @@ static void __iomem *evp_reset = IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE)+0x100;
 static void __iomem *tmrus = IO_ADDRESS(TEGRA_TMRUS_BASE);
 
 #define PMC_CTRL		0x0
+#define PMC_CTRL_LATCH_WAKEUPS	(1 << 5)
+#define PMC_WAKE_MASK		0xc
+#define PMC_WAKE_LEVEL		0x10
+
+#define PMC_SW_WAKE_STATUS	0x18
 #define PMC_COREPWRGOOD_TIMER	0x3c
 #define PMC_SCRATCH1		0x54
 #define PMC_CPUPWRGOOD_TIMER	0xc8
@@ -257,6 +262,131 @@ unsigned int tegra_suspend_lp2(unsigned int us)
 }
 
 #ifdef CONFIG_PM
+
+/* ensures that sufficient time is passed for a register write to
+ * serialize into the 32KHz domain */
+static void pmc_32kwritel(u32 val, unsigned long offs)
+{
+	writel(val, pmc + offs);
+	wmb();
+	udelay(130);
+}
+
+static void tegra_setup_wakepads(bool lp0_ok)
+{
+	u32 temp, status, lvl;
+
+	/* wakeup by interrupt, nothing to do here */
+	if (!lp0_ok)
+		return;
+
+	pmc_32kwritel(0, PMC_SW_WAKE_STATUS);
+	temp = readl(pmc + PMC_CTRL);
+	temp |= PMC_CTRL_LATCH_WAKEUPS;
+	pmc_32kwritel(0, PMC_CTRL);
+	temp &= ~PMC_CTRL_LATCH_WAKEUPS;
+	pmc_32kwritel(0, PMC_CTRL);
+
+	status = readl(pmc + PMC_SW_WAKE_STATUS);
+	lvl = readl(pmc + PMC_WAKE_LEVEL);
+
+	/* flip the wakeup trigger for any-edge triggered pads
+	 * which are currently asserting as wakeups */
+	status &= pdata->wake_any;
+	lvl &= ~pdata->wake_low;
+	lvl |= pdata->wake_high;
+	lvl ^= status;
+
+	writel(lvl, PMC_WAKE_LEVEL);
+	writel(pdata->wake_enb, PMC_WAKE_MASK);
+}
+
+extern void __tegra_lp1_reset(void);
+extern void __tegra_iram_end(void);
+
+static u8 *iram_save = NULL;
+static unsigned int iram_save_size = 0;
+static void __iomem *iram_code = IO_ADDRESS(TEGRA_IRAM_CODE_AREA);
+
+static void tegra_suspend_dram(bool lp0_ok)
+{
+	static unsigned long cpu_timer_32k = 0;
+	unsigned int lp2_timer;
+	unsigned int mode = TEGRA_POWER_SDRAM_SELFREFRESH;
+	unsigned long orig;
+
+	orig = readl(evp_reset);
+	/* copy the reset vector and SDRAM shutdown code into IRAM */
+	memcpy(iram_save, iram_code, iram_save_size);
+	memcpy(iram_code, (void *)__tegra_lp1_reset, iram_save_size);
+
+	if (!cpu_timer_32k) {
+		unsigned long long temp = 32768ull*pdata->cpu_timer + 999999;
+		do_div(temp, 1000000ul);
+		cpu_timer_32k = temp;
+	}
+
+	if (!lp0_ok) {
+		writel(TEGRA_IRAM_CODE_AREA, evp_reset);
+		NvRmPrivPowerSetState(s_hRmGlobal, NvRmPowerState_LP1);
+		lp2_timer = readl(pmc + PMC_CPUPWRGOOD_TIMER);
+		writel(cpu_timer_32k, pmc + PMC_CPUPWRGOOD_TIMER);
+		mode |= TEGRA_POWER_CPU_PWRREQ_OE;
+		if (pdata->core_off)
+			mode |= TEGRA_POWER_SYSCLK_OE;
+	} else {
+		NvRmPrivPowerSetState(s_hRmGlobal, NvRmPowerState_LP0);
+		set_power_timers(pdata->cpu_timer, pdata->cpu_off_timer);
+		mode |= TEGRA_POWER_SYSCLK_OE;
+		mode |= TEGRA_POWER_PWRREQ_OE;
+		mode |= TEGRA_POWER_EFFECT_LP0;
+	}
+
+	if (!pdata->corereq_high)
+		mode |= TEGRA_POWER_PWRREQ_POLARITY;
+	if (!pdata->sysclkreq_high)
+		mode |= TEGRA_POWER_SYSCLK_POLARITY;
+
+	/* for platforms where the core & CPU power requests are combined as
+	 * a single request to the PMU, transition from the current state to
+	 * the desired state by temporarily enabling both requests (if the
+	 * previous state does not match the new state)
+	 */
+
+	if (!pdata->separate_req) {
+		u32 reg = readl(pmc + PMC_CTRL);
+		reg |= ((mode & TEGRA_POWER_PMC_MASK) << TEGRA_POWER_PMC_SHIFT);
+		pmc_32kwritel(reg, PMC_CTRL);
+	}
+
+	tegra_setup_wakepads(lp0_ok);
+	suspend_cpu_complex();
+	flush_cache_all();
+	outer_shutdown();
+
+	__cortex_a9_save(mode);
+	restore_cpu_complex();
+
+	writel(orig, evp_reset);
+	outer_restart();
+
+	/* restore to CPU power request, assuming that the next transition
+	 * will be into LP2 */
+	if (lp0_ok && !pdata->separate_req) {
+		u32 reg = readl(pmc + PMC_CTRL);
+		mode &= TEGRA_POWER_PMC_MASK;
+		mode |= TEGRA_POWER_CPU_PWRREQ_OE;
+		reg |= (mode << TEGRA_POWER_PMC_SHIFT);
+		pmc_32kwritel(reg, PMC_CTRL);
+		reg &= ~(TEGRA_POWER_PWRREQ_OE << TEGRA_POWER_PMC_SHIFT);
+		writel(reg, pmc + PMC_CTRL);
+	} else if (!lp0_ok)
+		writel(lp2_timer, pmc + PMC_CPUPWRGOOD_TIMER);
+
+	memcpy(iram_code, iram_save, iram_save_size);
+	wmb();
+}
+
 static int tegra_suspend_prepare_late(void)
 {
 #ifdef CONFIG_TEGRA_NVRM
@@ -397,14 +527,17 @@ static int tegra_suspend_enter(suspend_state_t state)
 	int irq;
 
 	local_irq_save(flags);
-	tegra_irq_suspend();
-	tegra_dma_suspend();
-	tegra_pinmux_suspend();
-	tegra_gpio_suspend();
-	tegra_clk_suspend();
 
-	mc_data[0] = readl(mc + MC_SECURITY_START);
-	mc_data[1] = readl(mc + MC_SECURITY_SIZE);
+	if (pdata->core_off) {
+		tegra_irq_suspend();
+		tegra_dma_suspend();
+		tegra_pinmux_suspend();
+		tegra_gpio_suspend();
+		tegra_clk_suspend();
+
+		mc_data[0] = readl(mc + MC_SECURITY_START);
+		mc_data[1] = readl(mc + MC_SECURITY_SIZE);
+	}
 
 	for_each_irq_desc(irq, desc) {
 		if ((desc->status & IRQ_WAKEUP) &&
@@ -413,9 +546,12 @@ static int tegra_suspend_enter(suspend_state_t state)
 		}
 	}
 
-	/* lie about the power state so that the RM restarts DVFS */
-	NvRmPrivPowerSetState(s_hRmGlobal, NvRmPowerState_LP1);
-	tegra_suspend_lp2(0);
+	if (!pdata->dram_suspend || !iram_save) {
+		/* lie about the power state so that the RM restarts DVFS */
+		NvRmPrivPowerSetState(s_hRmGlobal, NvRmPowerState_LP1);
+		tegra_suspend_lp2(0);
+	} else
+		tegra_suspend_dram(pdata->core_off);
 
 	for_each_irq_desc(irq, desc) {
 		if ((desc->status & IRQ_WAKEUP) &&
@@ -424,14 +560,16 @@ static int tegra_suspend_enter(suspend_state_t state)
 		}
 	}
 
-	writel(mc_data[0], mc + MC_SECURITY_START);
-	writel(mc_data[1], mc + MC_SECURITY_SIZE);
-
-	tegra_clk_resume();
-	tegra_gpio_resume();
-	tegra_pinmux_resume();
-	tegra_dma_resume();
-	tegra_irq_resume();
+	if (pdata->core_off) {
+		writel(mc_data[0], mc + MC_SECURITY_START);
+		writel(mc_data[1], mc + MC_SECURITY_SIZE);
+
+		tegra_clk_resume();
+		tegra_gpio_resume();
+		tegra_pinmux_resume();
+		tegra_dma_resume();
+		tegra_irq_resume();
+	}
 
 	local_irq_restore(flags);
 
@@ -446,12 +584,24 @@ static struct platform_suspend_ops tegra_suspend_ops = {
 };
 #endif
 
+extern void __init lp0_suspend_init(void);
+
 void __init tegra_init_suspend(struct tegra_suspend_platform_data *plat)
 {
 	tegra_pclk = clk_get_sys(NULL, "pclk");
 	BUG_ON(!tegra_pclk);
 	pdata = plat;
 
+	iram_save_size = (unsigned long)__tegra_iram_end;
+	iram_save_size -= (unsigned long)__tegra_lp1_reset;
+
+	iram_save = kmalloc(iram_save_size, GFP_KERNEL);
+	if (!iram_save) {
+		pr_err("%s: unable to allocate memory for SDRAM self-refresh "
+		       "LP0/LP1 unavailable\n", __func__);
+	}
+	writel(virt_to_phys(tegra_lp2_startup), pmc + PMC_SCRATCH1);
+
 	if (pdata->core_off) {
 		u32 reg = 0, mode;
 
@@ -470,11 +620,11 @@ void __init tegra_init_suspend(struct tegra_suspend_platform_data *plat)
 
 		/* configure output inverters while the request is tristated */
 		reg |= (mode << TEGRA_POWER_PMC_SHIFT);
-		writel(reg, pmc + PMC_CTRL);
-		wmb();
-		udelay(2000); /* 32KHz domain delay */
+		pmc_32kwritel(reg, PMC_CTRL);
 		reg |= (TEGRA_POWER_SYSCLK_OE << TEGRA_POWER_PMC_SHIFT);
 		writel(reg, pmc + PMC_CTRL);
+
+		lp0_suspend_init();
 	}
 #ifdef CONFIG_PM
 	suspend_set_ops(&tegra_suspend_ops);