ARM: tegra: rethink the cpu suspend-resume code path

The current kernel methodology expects that tegra_cpu_suspend
is actually the last function in the entire suspend sequence.

In order to achieve this, the code needs to be remodelled a
bit so that we actually execute native cpu_suspend at the end
of the suspend sequence. This allows us to leverage all the
cpu_suspend code developed by ARM in the upstream kernels.

Bug 934368

Change-Id: I94172d7adaa54c10043c479a57b270925d85a16b
Signed-off-by: Varun Wadekar <vwadekar@nvidia.com>
Reviewed-on: http://git-master/r/84481
Reviewed-by: Simone Willett <swillett@nvidia.com>
Tested-by: Simone Willett <swillett@nvidia.com>

8 files changed, 43 insertions, 165 deletions

diff --git a/arch/arm/mach-tegra/cpuidle-t2.c b/arch/arm/mach-tegra/cpuidle-t2.c
index e5ff7c61f24c..d7b787bcac3b 100644
--- a/arch/arm/mach-tegra/cpuidle-t2.c
+++ b/arch/arm/mach-tegra/cpuidle-t2.c
@@ -40,6 +40,7 @@
 #include <linux/tick.h>
 
 #include <asm/cpu_pm.h>
+#include <asm/suspend.h>
 
 #include <mach/iomap.h>
 #include <mach/irqs.h>
diff --git a/arch/arm/mach-tegra/cpuidle-t3.c b/arch/arm/mach-tegra/cpuidle-t3.c
index c6a50a542d8c..9a19a8a4260c 100644
--- a/arch/arm/mach-tegra/cpuidle-t3.c
+++ b/arch/arm/mach-tegra/cpuidle-t3.c
@@ -43,6 +43,7 @@
 #include <asm/cpu_pm.h>
 #include <asm/hardware/gic.h>
 #include <asm/localtimer.h>
+#include <asm/suspend.h>
 
 #include <mach/iomap.h>
 #include <mach/irqs.h>
@@ -377,7 +378,7 @@ static void tegra3_idle_enter_lp2_cpu_n(struct cpuidle_device *dev,
 	tegra_cpu_wake_by_time[dev->cpu] = ktime_to_us(entry_time) + request;
 	smp_wmb();
 
-	tegra3_sleep_cpu_secondary(PLAT_PHYS_OFFSET - PAGE_OFFSET);
+	cpu_suspend(0, tegra3_sleep_cpu_secondary_finish);
 
 	tegra_cpu_wake_by_time[dev->cpu] = LLONG_MAX;
 
diff --git a/arch/arm/mach-tegra/headsmp.S b/arch/arm/mach-tegra/headsmp.S
index 4a9f03ffd972..219b7641a40f 100644
--- a/arch/arm/mach-tegra/headsmp.S
+++ b/arch/arm/mach-tegra/headsmp.S
@@ -66,7 +66,7 @@ ENTRY(tegra_resume)
 
 	cpu_id	r0
 	cmp	r0, #0				@ CPU0?
-	bne	tegra_cpu_resume_phys		@ no
+	bne	cpu_resume			@ no
 
 #ifndef CONFIG_ARCH_TEGRA_2x_SOC
 	@ Clear the flow controller flags for this CPU.
@@ -92,7 +92,7 @@ ENTRY(tegra_resume)
 	bl	tegra_generic_smc
 #endif
 
-	b	tegra_cpu_resume_phys
+	b	cpu_resume
 ENDPROC(tegra_resume)
 #endif
 
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index 07025b418298..e0a33c1da395 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -52,6 +52,7 @@
 #include <asm/pgalloc.h>
 #include <asm/pgtable.h>
 #include <asm/tlbflush.h>
+#include <asm/suspend.h>
 
 #include <mach/clk.h>
 #include <mach/iomap.h>
@@ -261,18 +262,6 @@ static __init int create_suspend_pgtable(void)
 	return 0;
 }
 
-/*
- * alloc_suspend_context
- *
- * Allocate a non-cacheable page to hold the CPU contexts.
- * The standard ARM CPU context save functions don't work if there's
- * an external L2 cache controller (like a PL310) in system.
- */
-static __init int alloc_suspend_context(void)
-{
-	return 0;
-}
-
 /* 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)
@@ -505,9 +494,9 @@ static void tegra_sleep_core(enum tegra_suspend_mode mode,
 	}
 #endif
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
-	tegra2_sleep_core(v2p);
+	cpu_suspend(v2p, tegra2_sleep_core_finish);
 #else
-	tegra3_sleep_core(v2p);
+	cpu_suspend(v2p, tegra3_sleep_core_finish);
 #endif
 }
 
@@ -518,7 +507,7 @@ static inline void tegra_sleep_cpu(unsigned long v2p)
 			  (TEGRA_RESET_HANDLER_BASE +
 			   tegra_cpu_reset_handler_offset));
 #endif
-	tegra_sleep_cpu_save(v2p);
+	cpu_suspend(v2p, tegra_sleep_cpu_finish);
 }
 
 unsigned int tegra_idle_lp2_last(unsigned int sleep_time, unsigned int flags)
@@ -1043,13 +1032,6 @@ void __init tegra_init_suspend(struct tegra_suspend_platform_data *plat)
 		goto fail;
 	}
 
-	if (alloc_suspend_context() < 0) {
-		pr_err("%s: CPU context alloc failed -- LP0/LP1/LP2 unavailable\n",
-				__func__);
-		plat->suspend_mode = TEGRA_SUSPEND_NONE;
-		goto fail;
-	}
-
 	if ((tegra_get_chipid() == TEGRA_CHIPID_TEGRA3) &&
 	    (tegra_get_revision() == TEGRA_REVISION_A01) &&
 	    (plat->suspend_mode == TEGRA_SUSPEND_LP0)) {
diff --git a/arch/arm/mach-tegra/sleep-t2.S b/arch/arm/mach-tegra/sleep-t2.S
index 6289541da8e9..fc7b3db22318 100644
--- a/arch/arm/mach-tegra/sleep-t2.S
+++ b/arch/arm/mach-tegra/sleep-t2.S
@@ -179,25 +179,29 @@ ENDPROC(tegra2_cpu_is_resettable_soon)
  * enters suspend in LP0 or LP1 by turning off the mmu and jumping to
  * tegra2_tear_down_core in IRAM
  */
-ENTRY(tegra2_sleep_core)
-	mov	r12, pc 		@ return here is via r12
-	b	tegra_cpu_save
+ENTRY(tegra2_sleep_core_finish)
+	bl	tegra_cpu_exit_coherency
 	mov32	r1, tegra2_tear_down_core
 	mov32	r2, tegra2_iram_start
 	sub	r1, r1, r2
 	mov32	r2, TEGRA_IRAM_CODE_AREA
 	add	r1, r1, r2
 	b	tegra_turn_off_mmu
-ENDPROC(tegra2_sleep_core)
+ENDPROC(tegra2_sleep_core_finish)
 
 /*
  * tegra2_sleep_wfi(unsigned long v2p)
  */
 ENTRY(tegra2_sleep_wfi)
-	mrc	p15, 0, r2, c1, c0, 1	@ save actlr before exiting coherency
-	mov	r12, pc 		@ return here is via r12
-	b	tegra_cpu_save
-	mov	r11, r2
+	stmfd	sp!, {r4 - r9, lr}
+	mov	r9, sp			@ save sp for aborted suspend
+	adr	r1, BSYM(tegra_sleep_cpu_save_finish)
+	bl	cpu_suspend
+	ldmfd	sp!, {r4 - r9, pc}
+
+tegra_sleep_cpu_save_finish:
+	mrc	p15, 0, r11, c1, c0, 1	@ save actlr before exiting coherency
+	bl	tegra_cpu_exit_coherency
 
 	mov32	r0, TEGRA_PMC_VIRT + PMC_SCRATCH41
 	mov	r3, #CPU_RESETTABLE
@@ -219,7 +223,7 @@ ENTRY(tegra2_sleep_wfi)
 	 * r11 contains the original actlr
 	 */
 
-	mov	sp, r7			@ restore SP for aborted suspend
+	mov	sp, r9			@ restore SP for aborted suspend
 	bl	tegra_pen_lock
 
 	mov32	r3, TEGRA_PMC_VIRT
@@ -262,8 +266,7 @@ ENTRY(tegra2_sleep_wfi)
 no_l2_sync:
 #endif
 
-	pop_ctx_regs r0, r1		@ restore context registers
-	mov	pc, lr
+	ldmfd	sp!, {r4 - r9, pc}
 ENDPROC(tegra2_sleep_wfi)
 
 /*
diff --git a/arch/arm/mach-tegra/sleep-t3.S b/arch/arm/mach-tegra/sleep-t3.S
index 4417da33de38..6d58585a4903 100644
--- a/arch/arm/mach-tegra/sleep-t3.S
+++ b/arch/arm/mach-tegra/sleep-t3.S
@@ -201,14 +201,13 @@ ENDPROC(tegra3_cpu_reset)
 #ifdef CONFIG_PM_SLEEP
 
 /*
- * tegra3_sleep_core(unsigned long v2p)
+ * tegra3_sleep_core_finish(unsigned long int)
  *
  * enters suspend in LP0 or LP1 by turning off the mmu and jumping to
  * tegra3_tear_down_core in IRAM
  */
-ENTRY(tegra3_sleep_core)
-	mov	r12, pc 		@ return here is via r12
-	b	tegra_cpu_save
+ENTRY(tegra3_sleep_core_finish)
+	bl	tegra_cpu_exit_coherency
 
 	/* preload all the address literals that are needed for the
 	 * CPU power-gating process, to avoid loads from SDRAM (which are
@@ -227,22 +226,23 @@ ENTRY(tegra3_sleep_core)
 	mov32	r2, TEGRA_IRAM_CODE_AREA
 	add	r1, r1, r2
 	b	tegra_turn_off_mmu
-ENDPROC(tegra3_sleep_core)
+ENDPROC(tegra3_sleep_core_finish)
 
 /*
- * tegra3_sleep_cpu_secondary(unsigned long v2p)
+ * tegra3_sleep_cpu_secondary_finish(unsigned long v2p)
  *
  * Enters LP2 on secondary CPU by exiting coherency and powergating the CPU.
  */
-ENTRY(tegra3_sleep_cpu_secondary)
-	mov	r12, pc 			@ return here is via r12
-	b	tegra_cpu_save
+ENTRY(tegra3_sleep_cpu_secondary_finish)
+	mov	r6, lr
+	bl	tegra_cpu_exit_coherency
 
 	/* Powergate this CPU. */
 	mov	r0, #0				@ power mode flags (!hotplug)
 	bl	tegra3_cpu_reset
-	b	.				@ should never get here
-ENDPROC(tegra3_sleep_cpu_secondary)
+	mov	r0, #1				@ never return here
+	mov	pc, r6
+ENDPROC(tegra3_sleep_cpu_secondary_finish)
 
 /*
  * tegra3_tear_down_cpu
diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index e573aa023a11..18b8799ea328 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -131,84 +131,13 @@ ENDPROC(tegra_cpu_exit_coherency)
 
 #ifdef CONFIG_PM_SLEEP
 /*
- * Restore CPU state for a suspend
- *
- * NOTE: This is a copy of cpu_resume in arch/arm/sleep.S that has been
- *	 modified to work with an L2 cache.
- */
-	.align	L1_CACHE_SHIFT
-ENTRY(tegra_cpu_resume_phys)
-	/* Use the standard cpu_resume. */
-	b	cpu_resume
-ENDPROC(tegra_cpu_resume_phys)
-
-/*
- * tegra_cpu_suspend
- *
- * Save CPU suspend state
- * NOTE: This is a copy of cpu_suspend in arch/arm/sleep.S that has been
- *	 modified to work with an L2 cache.
- *
- * Input:
- *	r1 = v:p offset
- *	lr = return to the caller of this function
- * Output:
- *	sp is decremented to allocate space for CPU state on stack
- * r0-r3,r8,r9,ip,lr corrupted
- */
-	.align	L1_CACHE_SHIFT
-ENTRY(tegra_cpu_suspend)
-	mov	r9, lr
-	adr	lr, tegra_cpu_resume
-	/* Use the standard cpu_suspend. */
-	adr	r3, BSYM(tegra_finish_suspend)
-	b	__cpu_suspend
-
-tegra_finish_suspend:
-	/* Turn off SMP coherency */
-	exit_smp r1, r6
-	mov	pc, r9
-ENDPROC(tegra_cpu_suspend)
-
-/*
- * tegra_cpu_save
- *
- * Input:
- *	r0 = v:p offset
- *	r12 = return to the caller of this function
- * Output:
- *	r0 = v:p offset
- *	r7 = SP after saving the registers but before cpu_suspend, suitable
- *	     for restoring an aborted suspend
- *	sp = SP after tegra_cpu_suspend (the 'real' SP)
- * Saves r4-r11 on the stack
- * Corrupts r1, r3-r11
- */
-
-ENTRY(tegra_cpu_save)
-	push_ctx_regs r1		@ save context registers
-
-	mov	r7, sp			@ SP after reg save, before suspend
-
-	mov	r4, r12
-	mov	r8, r0
-	mov	r11, r2
-	mov	r1, r0
-	bl	tegra_cpu_suspend
-	mov	r0, r8
-	mov	r2, r11
-	mov	pc, r4
-ENDPROC(tegra_cpu_save)
-
-/*
- * tegra_sleep_cpu_save(unsigned long v2p)
+ * tegra_sleep_cpu_finish(unsigned long int)
  *
  * enters suspend in LP2 by turning off the mmu and jumping to
  * tegra?_tear_down_cpu
  */
-ENTRY(tegra_sleep_cpu_save)
-	mov	r12, pc 		@ return here is via r12
-	b	tegra_cpu_save
+ENTRY(tegra_sleep_cpu_finish)
+	bl	tegra_cpu_exit_coherency
 
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	mov32	r1, tegra2_tear_down_cpu
@@ -217,28 +146,7 @@ ENTRY(tegra_sleep_cpu_save)
 #endif
 	add	r1, r1, r0
 	b	tegra_turn_off_mmu
-ENDPROC(tegra_sleep_cpu_save)
-
-/*
- * tegra_cpu_resume
- *
- * reloads the volatile CPU state from the context area
- * initializes the processor mode stacks
- * the mmu should be on and the CPU should be coherent before this is called
- */
-	.align L1_CACHE_SHIFT
-tegra_cpu_resume:
-	mov	r0, #0
-	mcr	p15, 0, r0, c8, c3, 0	@ invalidate TLB
-	mcr	p15, 0, r0, c7, c5, 6	@ flush BTAC
-	mcr	p15, 0, r0, c7, c5, 0	@ flush instruction cache
-	dsb
-	isb
-
-	bl	cpu_init
-
-	pop_ctx_regs r1, r2		@ restore context registers
-	mov	pc, lr
+ENDPROC(tegra_sleep_cpu_finish)
 
 /*
  * tegra_turn_off_mmu
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
index 91bf73e6bbe8..59298f1efbe9 100644
--- a/arch/arm/mach-tegra/sleep.h
+++ b/arch/arm/mach-tegra/sleep.h
@@ -123,24 +123,6 @@
 #endif
 .endm
 
-.macro push_ctx_regs, tmp1
-	push_stack_token \tmp1		@ debug check word
-	stmfd	sp!, {r4 - r11, lr}
-#if USE_TEGRA_DIAG_REG_SAVE
-	mrc	p15, 0, r4, c15, c0, 1	@ read diagnostic register
-	stmfd	sp!, {r4}
-#endif
-.endm
-
-.macro pop_ctx_regs, tmp1, tmp2
-#if USE_TEGRA_DIAG_REG_SAVE
-	ldmfd	sp!, {r4}
-	mcr	p15, 0, r4, c15, c0, 1	@ write diagnostic register
-#endif
-	ldmfd	sp!, {r4 - r11, lr}
-	pop_stack_token \tmp1, \tmp2	@ debug stack debug token
-.endm
-
 #else	/* !defined(__ASSEMBLY__) */
 
 #define FLOW_CTRL_HALT_CPU(cpu)	(IO_ADDRESS(TEGRA_FLOW_CTRL_BASE) +	\
@@ -163,8 +145,9 @@ static inline void flowctrl_writel(unsigned long val, void __iomem *addr)
 void tegra_pen_lock(void);
 void tegra_pen_unlock(void);
 void tegra_cpu_wfi(void);
-void tegra_sleep_cpu_save(unsigned long v2p);
+int tegra_sleep_cpu_finish(unsigned long v2p);
 void tegra_resume(void);
+void tegra_cpu_resume(void);
 
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 extern void tegra2_iram_start;
@@ -173,14 +156,14 @@ int  tegra2_cpu_is_resettable_soon(void);
 void tegra2_cpu_reset(int cpu);
 void tegra2_cpu_set_resettable_soon(void);
 void tegra2_cpu_clear_resettable(void);
-void tegra2_sleep_core(unsigned long v2p);
+int tegra2_sleep_core_finish(unsigned long int);
 void tegra2_hotplug_shutdown(void);
 void tegra2_sleep_wfi(unsigned long v2p);
 #else
 extern void tegra3_iram_start;
 extern void tegra3_iram_end;
-void tegra3_sleep_core(unsigned long v2p);
-void tegra3_sleep_cpu_secondary(unsigned long v2p);
+int tegra3_sleep_core_finish(unsigned long int);
+int tegra3_sleep_cpu_secondary_finish(unsigned long int);
 void tegra3_hotplug_shutdown(void);
 #endif