ARM: tegra: Redesign Tegra CPU reset handling

- Add a single unified handler for all CPU resets that is copied to
  IRAM.
- Add state information to direct the flow of execution through the
  reset handler based on the reason a CPU was reset.
- Write the EVP CPU reset vector only once per cold/warm boot session.
- Prevent modification of the EVP CPU reset vector in Tegra3.

Bug 786290
Bug 790458

Change-Id: Ica6707f3514986ee914e73a2d9766a4e06ce2d29
Signed-off-by: Scott Williams <scwilliams@nvidia.com>
DW: Split into logical changes
Signed-off-by: Dan Willemsen <dwillemsen@nvidia.com>

Rebase-Id: R7b9859a83717e76c3c083bdde724bd5fef9ce089

1 files changed, 173 insertions, 19 deletions

diff --git a/arch/arm/mach-tegra/headsmp.S b/arch/arm/mach-tegra/headsmp.S
index c30e57cbcdef..5931b7fdc3a8 100644
--- a/arch/arm/mach-tegra/headsmp.S
+++ b/arch/arm/mach-tegra/headsmp.S
@@ -1,7 +1,7 @@
 /*
  * arch/arm/mach-tegra/headsmp.S
  *
- * SMP initialization routines for Tegra SoCs
+ * CPU initialization routines for Tegra SoCs
  *
  * Copyright (c) 2009-2011, NVIDIA Corporation.
  * Copyright (c) 2011 Google, Inc.
@@ -10,8 +10,7 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
+ * the Free Software Foundation; either version 2 of the License.
  *
  * This program is distributed in the hope that it will be useful, but WITHOUT
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
@@ -24,17 +23,29 @@
 
 #include <asm/assembler.h>
 #include <asm/cache.h>
+#include <asm/page.h>
 
 #include <mach/iomap.h>
+#include <mach/io.h>
 
 #include "asm_macros.h"
+#include "reset.h"
+#include "sleep.h"
+
+#define DEBUG_CPU_RESET_HANDLER	0	/* Non-zero enables debug code */
+
+#define PMC_SCRATCH41		0x140
+
+#define RESET_DATA(x)		((TEGRA_RESET_##x)*4)
+
 
 #ifdef CONFIG_SMP
 /*
  *	tegra_secondary_startup
  *
  *	 Initial secondary processor boot vector; jumps to kernel's
- *	 secondary_startup routine
+ *	 secondary_startup routine. Used for initial boot and hotplug
+ *	 of secondary CPUs.
  */
 ENTRY(tegra_secondary_startup)
 	bl	tegra_invalidate_l1
@@ -44,29 +55,20 @@ ENDPROC(tegra_secondary_startup)
 #endif
 
 #ifdef CONFIG_PM_SLEEP
-#ifdef CONFIG_SMP
-/*
- *	tegra_secondary_resume
- *
- *	  Secondary CPU boot vector when restarting a CPU following lp2 idle.
- */
-ENTRY(tegra_secondary_resume)
-	bl	tegra_invalidate_l1
-	bl	tegra_enable_coresite
-	b	cpu_resume
-ENDPROC(tegra_secondary_resume)
-#endif
-
 /*
  *	tegra_resume
  *
- *	  CPU boot vector when restarting the master CPU following
+ *	  CPU boot vector when restarting the a CPU following
  *	  an LP2 transition. Also branched to by LP0 and LP1 resume after
  *	  re-enabling sdram.
  */
 ENTRY(tegra_resume)
-	bl	tegra_invalidate_l1
 	bl	tegra_enable_coresite
+	bl	tegra_invalidate_l1
+
+	cpu_id	r0
+	cmp	r0, #0				@ CPU0?
+	bne	cpu_resume      		@ no
 
 #ifndef CONFIG_ARCH_TEGRA_2x_SOC
 	@ Clear the flow controller flags for this CPU.
@@ -132,3 +134,155 @@ tegra_enable_coresite:
 	mov32	r0, 0xC5ACCE55
 	mcr	p14, 0, r0, c7, c12, 6
 	mov	pc, lr
+
+/*
+ * __tegra_cpu_reset_handler_halt_failed:
+ *
+ * Alternate entry point for reset handler for cases where the
+ * WFI halt failed to take effect.
+ *
+ */
+	.align L1_CACHE_SHIFT
+ENTRY(__tegra_cpu_reset_handler_start)
+
+/*
+ * __tegra_cpu_reset_handler:
+ *
+ * Common handler for all CPU reset events.
+ *
+ * Register usage within the reset handler:
+ *
+ *	R7  = CPU present (to the OS) mask
+ *	R8  = CPU in LP1 state mask
+ *	R9  = CPU in LP2 state mask
+ *	R10 = CPU number
+ *	R11 = CPU mask
+ *	R12 = pointer to reset handler data
+ *
+ * NOTE: This code is copied to IRAM. All code and data accesses
+ *	 must be position-independent.
+ */
+
+	.align L1_CACHE_SHIFT
+ENTRY(__tegra_cpu_reset_handler)
+
+#if DEBUG_CPU_RESET_HANDLER
+	mov32	r0, 0xC5ACCE55
+	mcr	p14, 0, r0, c7, c12, 6		@ Enable CoreSight access
+	b	.
+#endif
+	mrc	p15, 0, r10, c0, c0, 5		@ MPIDR
+	and	r10, r10, #0x3			@ R10 = CPU number
+	mov	r11, #1
+	mov	r11, r11, lsl r10  		@ R11 = CPU mask
+	adr	r12, __tegra_cpu_reset_handler_data
+
+#ifdef CONFIG_SMP
+	/* Does the OS know about this CPU? */
+	ldr	r7, [r12, #RESET_DATA(MASK_PRESENT)]
+	tst	r7, r11 			@ if !present
+	bleq	__die				@ CPU not present (to OS)
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+	/* Waking up from LP1? */
+	ldr	r8, [r12, #RESET_DATA(MASK_LP1)]
+	tst	r8, r11				@ if in_lp1
+	beq	__is_not_lp1
+	cmp	r10, #0
+	bne	__die				@ only CPU0 can be here
+	ldr	lr, [r12, #RESET_DATA(STARTUP_LP1)]
+	cmp	lr, #0
+	bleq	__die				@ no LP1 startup handler
+	bx	lr
+__is_not_lp1:
+#endif
+
+	/* Waking up from LP2? */
+	ldr	r9, [r12, #RESET_DATA(MASK_LP2)]
+	tst	r9, r11				@ if in_lp2
+	beq	__is_not_lp2
+#if defined(CONFIG_SMP) && defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	/* Tegra2 CPU1 LP2 wakeup uses the secondary startup handler */
+	cmp	r10, #1
+	bne	__is_not_lp2
+#endif
+	ldr	lr, [r12, #RESET_DATA(STARTUP_LP2)]
+	cmp	lr, #0
+	bleq	__die				@ no LP2 startup handler
+	bx	lr
+
+__is_not_lp2:
+
+#ifdef CONFIG_SMP
+	/* Can only be secondary boot (initial or hotplug) but CPU 0
+	   cannot be here. */
+	cmp	r10, #0
+	bleq	__die				@ CPU0 cannot be here
+	ldr	lr, [r12, #RESET_DATA(STARTUP_SECONDARY)]
+	cmp	lr, #0
+	bleq	__die				@ no secondary startup handler
+	bx	lr
+#endif
+
+/*
+ * We don't know why the CPU reset. Just kill it.
+ * The LR register will contain the address we died at + 4.
+ */
+
+__die:
+	sub	lr, lr, #4
+	mov32	r7, TEGRA_PMC_BASE
+	str	lr, [r7, #PMC_SCRATCH41]
+
+	mov32	r7, TEGRA_CLK_RESET_BASE
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	mov32	r0, 0x1111
+	mov	r1, r0, lsl r10
+	str	r1, [r7, #0x340]		@ CLK_RST_CPU_CMPLX_SET
+#else
+	mov32	r6, TEGRA_FLOW_CTRL_BASE
+
+	cmp	r10, #0
+	moveq	r1, #FLOW_CTRL_HALT_CPU0_EVENTS
+	moveq	r2, #FLOW_CTRL_CPU0_CSR
+	movne	r1, r10, lsl #3
+	addne	r2, r1, #(FLOW_CTRL_CPU1_CSR-8)
+	addne	r1, r1, #(FLOW_CTRL_HALT_CPU1_EVENTS-8)
+
+	/* Clear CPU "event" and "interrupt" flags and power gate
+	   it when halting but not before it is in the "WFI" state. */
+	ldr	r0, [r6, +r2]
+	orr	r0, r0, #FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG
+	orr	r0, r0, #FLOW_CTRL_CSR_ENABLE
+	str	r0, [r6, +r2]
+
+	/* Unconditionally halt this CPU */
+	mov	r0, #FLOW_CTRL_WAITEVENT
+	str	r0, [r6, +r1]
+	ldr	r0, [r6, +r1]			@ memory barrier
+
+	dsb
+	isb
+	wfi					@ CPU should be power gated here
+
+	/* If the CPU didn't power gate above just kill it's clock. */
+
+	mov	r0, r11, lsl #8
+	str	r0, [r7, #348]			@ CLK_CPU_CMPLX_SET
+#endif
+	/* If the CPU still isn't dead, just spin here. */
+	b	.
+
+ENDPROC(__tegra_cpu_reset_handler)
+	.align L1_CACHE_SHIFT
+	.type	__tegra_cpu_reset_handler_data, %object
+	.globl	__tegra_cpu_reset_handler_data
+__tegra_cpu_reset_handler_data:
+	.rept	TEGRA_RESET_DATA_SIZE
+	.long	0
+	.endr
+	.size	__tegra_cpu_reset_handler_data, .-tegra_cpu_reset_handler_data
+	.align L1_CACHE_SHIFT
+ENTRY(__tegra_cpu_reset_handler_end)