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

This reverts commit f31ca2d9e0580b58dc51fde31fc8ace190dd253b.

Bug 967887

Change-Id: I3fe975f7a6939cace5e208947bcb82e09008c0ac
Signed-off-by: Sang-Hun Lee <sanlee@nvidia.com>
Reviewed-on: http://git-master/r/96787
Reviewed-by: Automatic_Commit_Validation_User
Reviewed-by: Varun Wadekar <vwadekar@nvidia.com>

8 files changed, 165 insertions, 43 deletions

diff --git a/arch/arm/mach-tegra/cpuidle-t2.c b/arch/arm/mach-tegra/cpuidle-t2.c
index d7b787bcac3b..e5ff7c61f24c 100644
--- a/arch/arm/mach-tegra/cpuidle-t2.c
+++ b/arch/arm/mach-tegra/cpuidle-t2.c
@@ -40,7 +40,6 @@
 #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 9a19a8a4260c..c6a50a542d8c 100644
--- a/arch/arm/mach-tegra/cpuidle-t3.c
+++ b/arch/arm/mach-tegra/cpuidle-t3.c
@@ -43,7 +43,6 @@
 #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>
@@ -378,7 +377,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();
 
-	cpu_suspend(0, tegra3_sleep_cpu_secondary_finish);
+	tegra3_sleep_cpu_secondary(PLAT_PHYS_OFFSET - PAGE_OFFSET);
 
 	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 219b7641a40f..4a9f03ffd972 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	cpu_resume			@ no
+	bne	tegra_cpu_resume_phys		@ 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	cpu_resume
+	b	tegra_cpu_resume_phys
 ENDPROC(tegra_resume)
 #endif
 
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index e53effb9b669..20e106499343 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -54,7 +54,6 @@
 #include <asm/pgalloc.h>
 #include <asm/pgtable.h>
 #include <asm/tlbflush.h>
-#include <asm/suspend.h>
 
 #include <mach/clk.h>
 #include <mach/iomap.h>
@@ -264,6 +263,18 @@ 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)
@@ -496,9 +507,9 @@ static void tegra_sleep_core(enum tegra_suspend_mode mode,
 	}
 #endif
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
-	cpu_suspend(v2p, tegra2_sleep_core_finish);
+	tegra2_sleep_core(v2p);
 #else
-	cpu_suspend(v2p, tegra3_sleep_core_finish);
+	tegra3_sleep_core(v2p);
 #endif
 }
 
@@ -509,7 +520,7 @@ static inline void tegra_sleep_cpu(unsigned long v2p)
 			  (TEGRA_RESET_HANDLER_BASE +
 			   tegra_cpu_reset_handler_offset));
 #endif
-	cpu_suspend(v2p, tegra_sleep_cpu_finish);
+	tegra_sleep_cpu_save(v2p);
 }
 
 unsigned int tegra_idle_lp2_last(unsigned int sleep_time, unsigned int flags)
@@ -1036,6 +1047,13 @@ 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 fc7b3db22318..6289541da8e9 100644
--- a/arch/arm/mach-tegra/sleep-t2.S
+++ b/arch/arm/mach-tegra/sleep-t2.S
@@ -179,29 +179,25 @@ 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_finish)
-	bl	tegra_cpu_exit_coherency
+ENTRY(tegra2_sleep_core)
+	mov	r12, pc 		@ return here is via r12
+	b	tegra_cpu_save
 	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_finish)
+ENDPROC(tegra2_sleep_core)
 
 /*
  * tegra2_sleep_wfi(unsigned long v2p)
  */
 ENTRY(tegra2_sleep_wfi)
-	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
+	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
 
 	mov32	r0, TEGRA_PMC_VIRT + PMC_SCRATCH41
 	mov	r3, #CPU_RESETTABLE
@@ -223,7 +219,7 @@ tegra_sleep_cpu_save_finish:
 	 * r11 contains the original actlr
 	 */
 
-	mov	sp, r9			@ restore SP for aborted suspend
+	mov	sp, r7			@ restore SP for aborted suspend
 	bl	tegra_pen_lock
 
 	mov32	r3, TEGRA_PMC_VIRT
@@ -266,7 +262,8 @@ tegra_sleep_cpu_save_finish:
 no_l2_sync:
 #endif
 
-	ldmfd	sp!, {r4 - r9, pc}
+	pop_ctx_regs r0, r1		@ restore context registers
+	mov	pc, lr
 ENDPROC(tegra2_sleep_wfi)
 
 /*
diff --git a/arch/arm/mach-tegra/sleep-t3.S b/arch/arm/mach-tegra/sleep-t3.S
index 6d58585a4903..4417da33de38 100644
--- a/arch/arm/mach-tegra/sleep-t3.S
+++ b/arch/arm/mach-tegra/sleep-t3.S
@@ -201,13 +201,14 @@ ENDPROC(tegra3_cpu_reset)
 #ifdef CONFIG_PM_SLEEP
 
 /*
- * tegra3_sleep_core_finish(unsigned long int)
+ * tegra3_sleep_core(unsigned long v2p)
  *
  * enters suspend in LP0 or LP1 by turning off the mmu and jumping to
  * tegra3_tear_down_core in IRAM
  */
-ENTRY(tegra3_sleep_core_finish)
-	bl	tegra_cpu_exit_coherency
+ENTRY(tegra3_sleep_core)
+	mov	r12, pc 		@ return here is via r12
+	b	tegra_cpu_save
 
 	/* preload all the address literals that are needed for the
 	 * CPU power-gating process, to avoid loads from SDRAM (which are
@@ -226,23 +227,22 @@ ENTRY(tegra3_sleep_core_finish)
 	mov32	r2, TEGRA_IRAM_CODE_AREA
 	add	r1, r1, r2
 	b	tegra_turn_off_mmu
-ENDPROC(tegra3_sleep_core_finish)
+ENDPROC(tegra3_sleep_core)
 
 /*
- * tegra3_sleep_cpu_secondary_finish(unsigned long v2p)
+ * tegra3_sleep_cpu_secondary(unsigned long v2p)
  *
  * Enters LP2 on secondary CPU by exiting coherency and powergating the CPU.
  */
-ENTRY(tegra3_sleep_cpu_secondary_finish)
-	mov	r6, lr
-	bl	tegra_cpu_exit_coherency
+ENTRY(tegra3_sleep_cpu_secondary)
+	mov	r12, pc 			@ return here is via r12
+	b	tegra_cpu_save
 
 	/* Powergate this CPU. */
 	mov	r0, #0				@ power mode flags (!hotplug)
 	bl	tegra3_cpu_reset
-	mov	r0, #1				@ never return here
-	mov	pc, r6
-ENDPROC(tegra3_sleep_cpu_secondary_finish)
+	b	.				@ should never get here
+ENDPROC(tegra3_sleep_cpu_secondary)
 
 /*
  * tegra3_tear_down_cpu
diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index 18b8799ea328..e573aa023a11 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -131,13 +131,84 @@ ENDPROC(tegra_cpu_exit_coherency)
 
 #ifdef CONFIG_PM_SLEEP
 /*
- * tegra_sleep_cpu_finish(unsigned long int)
+ * 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)
  *
  * enters suspend in LP2 by turning off the mmu and jumping to
  * tegra?_tear_down_cpu
  */
-ENTRY(tegra_sleep_cpu_finish)
-	bl	tegra_cpu_exit_coherency
+ENTRY(tegra_sleep_cpu_save)
+	mov	r12, pc 		@ return here is via r12
+	b	tegra_cpu_save
 
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	mov32	r1, tegra2_tear_down_cpu
@@ -146,7 +217,28 @@ ENTRY(tegra_sleep_cpu_finish)
 #endif
 	add	r1, r1, r0
 	b	tegra_turn_off_mmu
-ENDPROC(tegra_sleep_cpu_finish)
+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
 
 /*
  * tegra_turn_off_mmu
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
index 59298f1efbe9..91bf73e6bbe8 100644
--- a/arch/arm/mach-tegra/sleep.h
+++ b/arch/arm/mach-tegra/sleep.h
@@ -123,6 +123,24 @@
 #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) +	\
@@ -145,9 +163,8 @@ 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);
-int tegra_sleep_cpu_finish(unsigned long v2p);
+void tegra_sleep_cpu_save(unsigned long v2p);
 void tegra_resume(void);
-void tegra_cpu_resume(void);
 
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 extern void tegra2_iram_start;
@@ -156,14 +173,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);
-int tegra2_sleep_core_finish(unsigned long int);
+void tegra2_sleep_core(unsigned long v2p);
 void tegra2_hotplug_shutdown(void);
 void tegra2_sleep_wfi(unsigned long v2p);
 #else
 extern void tegra3_iram_start;
 extern void tegra3_iram_end;
-int tegra3_sleep_core_finish(unsigned long int);
-int tegra3_sleep_cpu_secondary_finish(unsigned long int);
+void tegra3_sleep_core(unsigned long v2p);
+void tegra3_sleep_cpu_secondary(unsigned long v2p);
 void tegra3_hotplug_shutdown(void);
 #endif