Revert "ARM: tegra: remove usage of USE_TEGRA_CPU_SUSPEND"

This reverts commit e6d0e0ceec7cd1a7b8085eb31d2e70bc4d15684f.

Bug 967887

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

4 files changed, 211 insertions, 0 deletions

diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index 20e106499343..1882de0b31b6 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -97,6 +97,9 @@ struct suspend_context {
 };
 
 #ifdef CONFIG_PM_SLEEP
+#if USE_TEGRA_CPU_SUSPEND
+void *tegra_cpu_context;	/* non-cacheable page for CPU context */
+#endif
 phys_addr_t tegra_pgd_phys;	/* pgd used by hotplug & LP2 bootup */
 static pgd_t *tegra_pgd;
 static DEFINE_SPINLOCK(tegra_lp2_lock);
@@ -272,7 +275,52 @@ static __init int create_suspend_pgtable(void)
  */
 static __init int alloc_suspend_context(void)
 {
+#if USE_TEGRA_CPU_SUSPEND
+	pgprot_t prot = __pgprot_modify(pgprot_kernel, L_PTE_MT_MASK,
+					L_PTE_MT_BUFFERABLE | L_PTE_XN);
+	struct page *ctx_page;
+	unsigned long ctx_virt = 0;
+	pgd_t *pgd;
+	pmd_t *pmd;
+	pte_t *pte;
+
+	ctx_page = alloc_pages(GFP_KERNEL, 0);
+	if (IS_ERR_OR_NULL(ctx_page))
+		goto fail;
+
+	tegra_cpu_context = vm_map_ram(&ctx_page, 1, -1, prot);
+	if (IS_ERR_OR_NULL(tegra_cpu_context))
+		goto fail;
+
+	/* Add the context page to our private pgd. */
+	ctx_virt = (unsigned long)tegra_cpu_context;
+
+	pgd = tegra_pgd + pgd_index(ctx_virt);
+	if (!pgd_present(*pgd))
+		goto fail;
+	pmd = pmd_offset(pgd, ctx_virt);
+	if (!pmd_none(*pmd))
+		goto fail;
+	pte = pte_alloc_kernel(pmd, ctx_virt);
+	if (!pte)
+		goto fail;
+
+	set_pte_ext(pte, mk_pte(ctx_page, prot), 0);
+
+	outer_clean_range(__pa(pmd), __pa(pmd + 1));
+
 	return 0;
+
+fail:
+	if (ctx_page)
+		__free_page(ctx_page);
+	if (ctx_virt)
+		vm_unmap_ram((void*)ctx_virt, 1);
+	tegra_cpu_context = NULL;
+	return -ENOMEM;
+#else
+	return 0;
+#endif
 }
 
 /* ensures that sufficient time is passed for a register write to
diff --git a/arch/arm/mach-tegra/sleep-t2.S b/arch/arm/mach-tegra/sleep-t2.S
index 6289541da8e9..f70360628f34 100644
--- a/arch/arm/mach-tegra/sleep-t2.S
+++ b/arch/arm/mach-tegra/sleep-t2.S
@@ -229,8 +229,18 @@ ENTRY(tegra2_sleep_wfi)
 
 	bl	tegra_pen_unlock
 
+#if USE_TEGRA_CPU_SUSPEND
+	/* Enable the data cache and SMP coherency */
+	mrc	p15, 0, r10, c1, c0, 0
+	orr	r10, r10, #CR_C
+	dsb
+	mcr	p15, 0, r10, c1, c0, 0
+	isb
 	mcr	p15, 0, r11, c1, c0, 1	@ reenable coherency
 
+#else
+	mcr	p15, 0, r11, c1, c0, 1	@ reenable coherency
+#endif
 	/* Invalidate the TLBs & BTAC */
 	mov	r1, #0
 	mcr	p15, 0, r1, c8, c3, 0	@ invalidate shared TLBs
diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index e573aa023a11..003de98acd03 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -138,10 +138,35 @@ ENDPROC(tegra_cpu_exit_coherency)
  */
 	.align	L1_CACHE_SHIFT
 ENTRY(tegra_cpu_resume_phys)
+#if USE_TEGRA_CPU_SUSPEND
+#ifdef CONFIG_SMP
+	adr	r0, tegra_phys_sleep_sp
+	ALT_SMP(mrc p15, 0, r1, c0, c0, 5)
+	ALT_UP(mov r1, #0)
+	and	r1, r1, #15
+	ldr	r0, [r0, r1, lsl #2]		@ stack phys addr
+#else
+	ldr	r0, tegra_phys_sleep_sp		@ stack phys addr
+#endif
+	setmode	PSR_I_BIT | PSR_F_BIT | SVC_MODE, r1  @ set SVC, irqs off
+	@ 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
 	/* Use the standard cpu_resume. */
 	b	cpu_resume
+#endif
 ENDPROC(tegra_cpu_resume_phys)
 
+#if USE_TEGRA_CPU_SUSPEND
+tegra_phys_sleep_sp:
+	.rept	4
+	.long	0		@ preserve stack phys ptr here
+	.endr
+#endif
+
 /*
  * tegra_cpu_suspend
  *
@@ -160,6 +185,109 @@ ENDPROC(tegra_cpu_resume_phys)
 ENTRY(tegra_cpu_suspend)
 	mov	r9, lr
 	adr	lr, tegra_cpu_resume
+#if USE_TEGRA_CPU_SUSPEND
+	stmfd	sp!, {r4 - r11, lr}
+#ifdef MULTI_CPU
+	mov32	r10, processor
+	ldr	r5, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
+	ldr	ip, [r10, #CPU_DO_RESUME] @ virtual resume function
+#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, 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
+	bl	cpu_do_suspend
+#endif
+	dsb
+
+	/* Disable the data cache */
+	mrc	p15, 0, r10, c1, c0, 0
+	bic	r10, r10, #CR_C
+	dsb
+	mcr	p15, 0, r10, c1, c0, 0
+	isb
+
+	/* Flush data cache */
+#ifdef MULTI_CACHE
+	mov32	r10, cpu_cache
+	mov	lr, pc
+	ldr	pc, [r10, #CACHE_FLUSH_KERN_ALL]
+#else
+	bl	__cpuc_flush_kern_all
+#endif
+#ifdef CONFIG_CACHE_L2X0
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	cpu_id  r2
+	cmp r2, #0
+	bne no_l2_sync
+#endif
+	/* 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]
+#endif
+
+no_l2_sync:
+	/* Invalidate the TLBs & BTAC */
+	mov	r1, #0
+	mcr	p15, 0, r1, c8, c3, 0	@ invalidate shared TLBs
+	mcr	p15, 0, r1, c7, c1, 6	@ invalidate shared BTAC
+	dsb
+	isb
+
+	/* Turn off SMP coherency */
+	exit_smp r1, r2
+
+	/* Convert SP from virtual to physical address. */
+	movw	r1, #0xFFF
+	bic	r2, sp, r1		@ VA & 0xFFFFF000
+	mcr	p15, 0, r2, c7, c8, 0	@ V2PPRPC
+	mrc	p15, 0, r2, c7, c4, 0	@ PAR
+	bic	r2, r2, r1		@ PA & 0xFFFFF000
+	and	r0, sp, r1		@ VA & 0x00000FFF
+	orr	r2, r0, r2		@ (PA & 0xFFFFF000) | (VA & 0x00000FFF)
+
+	mov32	r3, tegra_phys_sleep_sp	@ per-CPU phys SP save area
+
+#ifdef CONFIG_SMP
+	ALT_SMP(mrc p15, 0, lr, c0, c0, 5)
+	ALT_UP(mov lr, #0)
+	and	lr, lr, #15
+#else
+	mov	lr, #0
+#endif
+
+	/* Save the normal PRRR value */
+	mrc	p15, 0, r0, c10, c2, 0	@ PRRR
+
+	/* Override all remappings to strongly ordered */
+	mov	r1, #0
+	mcr	p15, 0, r1, c10, c2, 0	@ PRRR
+	mcr	p15, 0, r1, c8, c7, 0	@ invalidate local TLBs
+	dsb
+	isb
+
+	/* Save the physical stack pointer */
+	str	r2, [r3, lr, lsl #2]	@ save phys SP
+
+	/* Restore the regular remappings */
+	mcr	p15, 0, r0, c10, c2, 0	@ PRRR
+	mcr	p15, 0, r1, c8, c7, 0	@ invalidate local TLBs
+	dsb
+	isb
+#else
 	/* Use the standard cpu_suspend. */
 	adr	r3, BSYM(tegra_finish_suspend)
 	b	__cpu_suspend
@@ -167,6 +295,7 @@ ENTRY(tegra_cpu_suspend)
 tegra_finish_suspend:
 	/* Turn off SMP coherency */
 	exit_smp r1, r6
+#endif
 	mov	pc, r9
 ENDPROC(tegra_cpu_suspend)
 
@@ -190,6 +319,18 @@ ENTRY(tegra_cpu_save)
 
 	mov	r7, sp			@ SP after reg save, before suspend
 
+#if USE_TEGRA_CPU_SUSPEND
+	cpu_id	r4
+	mov32	r5, tegra_cpu_context   @ address of non-cacheable context page
+	ldr	r5, [r5]		@ non-cacheable context save area
+	mov	r6, #0x400		@ size of one CPU context stack area
+	add	r4, r4, #1
+	smlabb	sp, r6, r4, r5		@ context area for this CPU
+	push_stack_token r4		@ debug check word
+	stmfd	sp!, {r7}		@ save the real stack pointer
+	push_stack_token r4		@ debug check word
+#endif
+
 	mov	r4, r12
 	mov	r8, r0
 	mov	r11, r2
@@ -235,6 +376,13 @@ tegra_cpu_resume:
 	dsb
 	isb
 
+#if USE_TEGRA_CPU_SUSPEND
+	pop_stack_token r4, r5		@ check stack debug token
+	ldmfd	sp!, {r0}		@ get the real stack pointer
+	pop_stack_token r4, r5		@ check stack debug token
+	mov	sp, r0			@ switch to the real stack pointer
+#endif
+
 	bl	cpu_init
 
 	pop_ctx_regs r1, r2		@ restore context registers
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
index 91bf73e6bbe8..b27dd48cebda 100644
--- a/arch/arm/mach-tegra/sleep.h
+++ b/arch/arm/mach-tegra/sleep.h
@@ -25,6 +25,11 @@
 
 #include <mach/iomap.h>
 
+#ifdef CONFIG_CACHE_L2X0
+#define USE_TEGRA_CPU_SUSPEND	1
+#else
+#define USE_TEGRA_CPU_SUSPEND	0
+#endif
 #ifndef CONFIG_TRUSTED_FOUNDATIONS
 /* FIXME: The code associated with this should be removed if our change to
    save the diagnostic regsiter in the CPU context is accepted. */