ARM: tegra: copy native ARM code behavior for cpu suspend-resume

The cpu suspend-resume code now duplicates the non-tegra
part from the native ARM code.

Bug 934368

Change-Id: I100c8de8e107d1baebb6ec30a1f6f77bca8f44aa
Signed-off-by: Varun Wadekar <vwadekar@nvidia.com>
Reviewed-on: http://git-master/r/83098

1 files changed, 29 insertions, 41 deletions

diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index 38e5f69a3437..c4628e9691bb 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -161,33 +161,22 @@ ENTRY(tegra_cpu_resume_phys)
 	ldr	r0, tegra_phys_sleep_sp		@ stack phys addr
 #endif
 	setmode	PSR_I_BIT | PSR_F_BIT | SVC_MODE, r1  @ set SVC, irqs off
-#ifdef MULTI_CPU
 	@ load v:p, stack, resume fn
   ARM(	ldmia	r0!, {r1, sp, pc}	)
 THUMB(	ldmia	r0!, {r1, r2, r3}	)
 THUMB(	mov	sp, r2			)
 THUMB(	bx	r3			)
 #else
-	@ load v:p, stack, return fn
-  ARM(	ldmia	r0!, {r1, sp, lr}	)
-THUMB(	ldmia	r0!, {r1, r2, lr}	)
-THUMB(	mov	sp, r2			)
-	b	cpu_do_resume
-#endif
-#else
 	/* Use the standard cpu_resume. */
 	b	cpu_resume
 #endif
 ENDPROC(tegra_cpu_resume_phys)
 
 #if USE_TEGRA_CPU_SUSPEND
-	.align	L1_CACHE_SHIFT
-	.globl	tegra_phys_sleep_sp
 tegra_phys_sleep_sp:
-	.rept	4
+	.rept	CONFIG_NR_CPUS
 	.long	0		@ preserve stack phys ptr here
 	.endr
-	.align	L1_CACHE_SHIFT  @ nothing else must be in this cache line
 #endif
 
 /*
@@ -199,34 +188,35 @@ tegra_phys_sleep_sp:
  *
  * Input:
  *	r1 = v:p offset
- *	r3 = virtual return function
+ *	lr = return to the caller of this function
  * Output:
  *	sp is decremented to allocate space for CPU state on stack
- * r0-r3,ip,lr corrupted
+ * r0-r3,r8,r9,ip,lr corrupted
  */
 	.align	L1_CACHE_SHIFT
 ENTRY(tegra_cpu_suspend)
-#if USE_TEGRA_CPU_SUSPEND
-	stmfd	sp!, {r3}
-	stmfd	sp!, {r4 - r11}
 	mov	r9, lr
+	adr	lr, tegra_cpu_resume
+#if USE_TEGRA_CPU_SUSPEND
+	stmfd	sp!, {r4 - r11, lr}
 #ifdef MULTI_CPU
 	mov32	r10, processor
-	mov	r2, sp			@ current virtual SP
-	ldr	r0, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
+	ldr	r5, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
 	ldr	ip, [r10, #CPU_DO_RESUME] @ virtual resume function
-	sub	sp, sp, r0		@ allocate CPU state on stack
-	mov	r0, sp			@ save pointer
+#else
+	mov32	r5, cpu_suspend_size
+	mov32	ip, cpu_do_resume
+#endif
+	mov	r6, sp			@ current virtual SP
+	sub	sp, sp, r5		@ allocate CPU state on stack
+	mov	r0, sp			@ save pointer to CPU save block
 	add	ip, ip, r1		@ convert resume fn to phys
-	stmfd	sp!, {r1, r2, ip}	@ save v:p, virt SP, phys resume fn
+	stmfd	sp!, {r1, r6, ip}	@ save v:p, virt SP, phys resume fn
+
+#ifdef MULTI_CPU
 	mov	lr, pc
 	ldr	pc, [r10, #CPU_DO_SUSPEND] @ save CPU state
 #else
-	mov	r2, sp			@ current virtual SP
-	mov32	r0, cpu_suspend_size
-	sub	sp, sp, r0		@ allocate CPU state on stack
-	mov	r0, sp			@ save pointer
-	stmfd	sp!, {r1, r2, ip}	@ save v:p, virt SP, phys resume fn
 	bl	cpu_do_suspend
 #endif
 	dsb
@@ -309,15 +299,16 @@ no_l2_sync:
 	mcr	p15, 0, r1, c8, c7, 0	@ invalidate local TLBs
 	dsb
 	isb
-
-	mov	pc, r9
 #else
 	/* Use the standard cpu_suspend. */
-	mov	r8, lr
-	bl	cpu_suspend
-	exit_smp r0, r2
-	mov	pc, r8
+	adr	r3, BSYM(tegra_finish_suspend)
+	b	__cpu_suspend
+
+tegra_finish_suspend:
+	/* Turn off SMP coherency */
+	exit_smp r1, r6
 #endif
+	mov	pc, r9
 ENDPROC(tegra_cpu_suspend)
 
 /*
@@ -326,21 +317,18 @@ ENDPROC(tegra_cpu_suspend)
  * Input:
  *	r0 = v:p offset
  *	r12 = return to the caller of this function
- *	lr = resume address
  * 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-r10
+ * Corrupts r1, r3-r11
  */
 
 ENTRY(tegra_cpu_save)
 	push_ctx_regs r1		@ save context registers
 
-	adr	r3, tegra_cpu_resume
-
 	mov	r7, sp			@ SP after reg save, before suspend
 
 #if USE_TEGRA_CPU_SUSPEND
@@ -356,12 +344,12 @@ ENTRY(tegra_cpu_save)
 #endif
 
 	mov	r4, r12
-	mov	r5, r0
-	mov	r6, r2
+	mov	r8, r0
+	mov	r11, r2
 	mov	r1, r0
 	bl	tegra_cpu_suspend
-	mov	r0, r5
-	mov	r2, r6
+	mov	r0, r8
+	mov	r2, r11
 	mov	pc, r4
 ENDPROC(tegra_cpu_save)