arm: tegra20: pm: rework secondary LP2

Use ARM cpu_suspend to save state during secondary LP2

Change-Id: Ie01e7ad2db4b11ecb190329c71a334216f7c4679
Signed-off-by: Prashant Gaikwad <pgaikwad@nvidia.com>

3 files changed, 12 insertions, 35 deletions

diff --git a/arch/arm/mach-tegra/cpuidle-t2.c b/arch/arm/mach-tegra/cpuidle-t2.c
index a58a84ed861e..ce7c4eeb68d0 100644
--- a/arch/arm/mach-tegra/cpuidle-t2.c
+++ b/arch/arm/mach-tegra/cpuidle-t2.c
@@ -40,6 +40,8 @@
 #include <linux/tick.h>
 #include <linux/cpu_pm.h>
 
+#include <asm/suspend.h>
+
 #include <mach/iomap.h>
 #include <mach/irqs.h>
 
@@ -280,7 +282,7 @@ static bool tegra2_idle_lp2_cpu_1(struct cpuidle_device *dev,
 
 	tegra_twd_suspend(&twd_context);
 
-	tegra2_sleep_wfi(PHYS_OFFSET - PAGE_OFFSET);
+	cpu_suspend(PHYS_OFFSET - PAGE_OFFSET, tegra2_finish_sleep_cpu_secondary);
 
 	tegra2_cpu_clear_resettable();
 
@@ -301,7 +303,6 @@ bool tegra2_idle_lp2(struct cpuidle_device *dev,
 	s64 request = ktime_to_us(tick_nohz_get_sleep_length());
 	bool last_cpu = tegra_set_cpu_in_lp2(dev->cpu);
 	bool entered_lp2 = false;
-
 	cpu_pm_enter();
 
 	if (dev->cpu == 0) {
@@ -325,6 +326,7 @@ bool tegra2_idle_lp2(struct cpuidle_device *dev,
 	cpu_pm_exit();
 	tegra_clear_cpu_in_lp2(dev->cpu);
 
+	cpu_pm_enter();
 	return entered_lp2;
 }
 
diff --git a/arch/arm/mach-tegra/sleep-t2.S b/arch/arm/mach-tegra/sleep-t2.S
index fc7b3db22318..79534964e718 100644
--- a/arch/arm/mach-tegra/sleep-t2.S
+++ b/arch/arm/mach-tegra/sleep-t2.S
@@ -190,16 +190,12 @@ ENTRY(tegra2_sleep_core_finish)
 ENDPROC(tegra2_sleep_core_finish)
 
 /*
- * tegra2_sleep_wfi(unsigned long v2p)
+ * tegra3_sleep_cpu_secondary_finish(unsigned long v2p)
+ *
+ * Enters WFI on secondary CPU by exiting coherency and saving CPU state.
  */
-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:
+ENTRY(tegra2_finish_sleep_cpu_secondary)
+	mov	r6, lr	@ save address of cpu_suspend_abort
 	mrc	p15, 0, r11, c1, c0, 1	@ save actlr before exiting coherency
 	bl	tegra_cpu_exit_coherency
 
@@ -209,10 +205,6 @@ tegra_sleep_cpu_save_finish:
 
 	bl	tegra_cpu_wfi
 
-	mov32	r0, TEGRA_PMC_VIRT + PMC_SCRATCH41
-	mov	r3, #CPU_NOT_RESETTABLE
-	str	r3, [r0]
-
 	/*
 	 * cpu may be reset while in wfi, which will return through
 	 * tegra_resume to tegra_cpu_resume_phys to tegra_cpu_resume
@@ -222,8 +214,6 @@ tegra_sleep_cpu_save_finish:
 	 *
 	 * r11 contains the original actlr
 	 */
-
-	mov	sp, r9			@ restore SP for aborted suspend
 	bl	tegra_pen_lock
 
 	mov32	r3, TEGRA_PMC_VIRT
@@ -250,24 +240,8 @@ tegra_sleep_cpu_save_finish:
 #else
 	bl	__cpuc_flush_kern_all
 #endif
-
-#ifdef CONFIG_CACHE_L2X0
-	cpu_id  r2
-	cmp r2, #0
-	bne no_l2_sync
-	/* Issue a PL310 cache sync operation */
-	dsb
-	mov32	r2, TEGRA_PL310_VIRT
-	movw	r1, 0x730	@ cache sync
-	add	r2, r2, r1
-	mov	r1, #0
-	str	r1, [r2]
-
-no_l2_sync:
-#endif
-
-	ldmfd	sp!, {r4 - r9, pc}
-ENDPROC(tegra2_sleep_wfi)
+	mov	pc, r6
+ENDPROC(tegra2_finish_sleep_cpu_secondary)
 
 /*
  * tegra2_tear_down_cpu
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
index e1e3eb8724e2..a9023664768c 100644
--- a/arch/arm/mach-tegra/sleep.h
+++ b/arch/arm/mach-tegra/sleep.h
@@ -158,6 +158,7 @@ void tegra2_cpu_clear_resettable(void);
 int tegra2_sleep_core_finish(unsigned long int);
 void tegra2_hotplug_shutdown(void);
 void tegra2_sleep_wfi(unsigned long v2p);
+int tegra2_finish_sleep_cpu_secondary(unsigned long int);
 #else
 extern void tegra3_iram_start;
 extern void tegra3_iram_end;