[deliverable/linux.git] / arch / arm / mach-tegra / headsmp.S

#include <linux/linkage.h>
#include <linux/init.h>

#include <asm/cache.h>
#include <asm/asm-offsets.h>
#include <asm/hardware/cache-l2x0.h>

#include "flowctrl.h"
#include "iomap.h"
#include "reset.h"
#include "sleep.h"

#define APB_MISC_GP_HIDREV	0x804
#define PMC_SCRATCH41	0x140

#define RESET_DATA(x)	((TEGRA_RESET_##x)*4)

        .section ".text.head", "ax"
/*
 * Tegra specific entry point for secondary CPUs.
 *   The secondary kernel init calls v7_flush_dcache_all before it enables
 *   the L1; however, the L1 comes out of reset in an undefined state, so
 *   the clean + invalidate performed by v7_flush_dcache_all causes a bunch
 *   of cache lines with uninitialized data and uninitialized tags to get
 *   written out to memory, which does really unpleasant things to the main
 *   processor.  We fix this by performing an invalidate, rather than a
 *   clean + invalidate, before jumping into the kernel.
 */
ENTRY(v7_invalidate_l1)
        mov     r0, #0
        mcr     p15, 2, r0, c0, c0, 0
        mrc     p15, 1, r0, c0, c0, 0

        ldr     r1, =0x7fff
        and     r2, r1, r0, lsr #13

        ldr     r1, =0x3ff

        and     r3, r1, r0, lsr #3  @ NumWays - 1
        add     r2, r2, #1          @ NumSets

        and     r0, r0, #0x7
        add     r0, r0, #4          @ SetShift

        clz     r1, r3              @ WayShift
        add     r4, r3, #1          @ NumWays
1:      sub     r2, r2, #1          @ NumSets--
        mov     r3, r4              @ Temp = NumWays
2:      subs    r3, r3, #1          @ Temp--
        mov     r5, r3, lsl r1
        mov     r6, r2, lsl r0
        orr     r5, r5, r6          @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
        mcr     p15, 0, r5, c7, c6, 2
        bgt     2b
        cmp     r2, #0
        bgt     1b
        dsb
        isb
        mov     pc, lr
ENDPROC(v7_invalidate_l1)


ENTRY(tegra_secondary_startup)
        bl      v7_invalidate_l1
	/* Enable coresight */
	mov32	r0, 0xC5ACCE55
	mcr	p14, 0, r0, c7, c12, 6
        b       secondary_startup
ENDPROC(tegra_secondary_startup)

#ifdef CONFIG_PM_SLEEP
/*
 *	tegra_resume
 *
 *	  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	v7_invalidate_l1
	/* Enable coresight */
	mov32	r0, 0xC5ACCE55
	mcr	p14, 0, r0, c7, c12, 6

	cpu_id	r0
	cmp	r0, #0				@ CPU0?
	bne	cpu_resume			@ no

#ifdef CONFIG_ARCH_TEGRA_3x_SOC
	/* Are we on Tegra20? */
	mov32	r6, TEGRA_APB_MISC_BASE
	ldr	r0, [r6, #APB_MISC_GP_HIDREV]
	and	r0, r0, #0xff00
	cmp	r0, #(0x20 << 8)
	beq	1f				@ Yes
	/* Clear the flow controller flags for this CPU. */
	mov32	r2, TEGRA_FLOW_CTRL_BASE + FLOW_CTRL_CPU0_CSR	@ CPU0 CSR
	ldr	r1, [r2]
	/* Clear event & intr flag */
	orr	r1, r1, \
		#FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG
	movw	r0, #0x0FFD	@ enable, cluster_switch, immed, & bitmaps
	bic	r1, r1, r0
	str	r1, [r2]
1:
#endif

#ifdef CONFIG_HAVE_ARM_SCU
	/* enable SCU */
	mov32	r0, TEGRA_ARM_PERIF_BASE
	ldr	r1, [r0]
	orr	r1, r1, #1
	str	r1, [r0]
#endif

	/* L2 cache resume & re-enable */
	l2_cache_resume r0, r1, r2, l2x0_saved_regs_addr

	b	cpu_resume
ENDPROC(tegra_resume)
#endif

#ifdef CONFIG_CACHE_L2X0
	.globl	l2x0_saved_regs_addr
l2x0_saved_regs_addr:
	.long	0
#endif

	.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)

	cpsid	aif, 0x13			@ SVC mode, interrupts disabled
	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_ARCH_TEGRA_2x_SOC
	/* Are we on Tegra20? */
	mov32	r6, TEGRA_APB_MISC_BASE
	ldr	r0, [r6, #APB_MISC_GP_HIDREV]
	and	r0, r0, #0xff00
	cmp	r0, #(0x20 << 8)
	bne	1f
	/* If not CPU0, don't let CPU0 reset CPU1 now that CPU1 is coming up. */
	mov32	r6, TEGRA_PMC_BASE
	mov	r0, #0
	cmp	r10, #0
	strne	r0, [r6, #PMC_SCRATCH41]
1:
#endif

	/* Waking up from LP2? */
	ldr	r9, [r12, #RESET_DATA(MASK_LP2)]
	tst	r9, r11				@ if in_lp2
	beq	__is_not_lp2
	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

	/* Are we on Tegra20? */
	mov32	r6, TEGRA_APB_MISC_BASE
	ldr	r0, [r6, #APB_MISC_GP_HIDREV]
	and	r0, r0, #0xff00
	cmp	r0, #(0x20 << 8)
	bne	1f

#ifdef CONFIG_ARCH_TEGRA_2x_SOC
	mov32	r0, 0x1111
	mov	r1, r0, lsl r10
	str	r1, [r7, #0x340]		@ CLK_RST_CPU_CMPLX_SET
#endif
1:
#ifdef CONFIG_ARCH_TEGRA_3x_SOC
	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
	.align L1_CACHE_SHIFT

ENTRY(__tegra_cpu_reset_handler_end)
Commit	Line	Data
1cea7326 CC	1	#include <linux/linkage.h>
	2	#include <linux/init.h>
	3
b36ab975	4	#include <asm/cache.h>
29a0e7be JL	5	#include <asm/asm-offsets.h>
29a0e7be JL	6	#include <asm/hardware/cache-l2x0.h>
b36ab975	7
b36ab975	8	#include "flowctrl.h"
2be39c07	9	#include "iomap.h"
b36ab975	10	#include "reset.h"
c2be5bfc	11	#include "sleep.h"
b36ab975 PDS	12
	13	#define APB_MISC_GP_HIDREV 0x804
	14	#define PMC_SCRATCH41 0x140
	15
	16	#define RESET_DATA(x) ((TEGRA_RESET_##x)*4)
	17
1cea7326	18	.section ".text.head", "ax"
1cea7326 CC	19	/*
	20	* Tegra specific entry point for secondary CPUs.
	21	* The secondary kernel init calls v7_flush_dcache_all before it enables
	22	* the L1; however, the L1 comes out of reset in an undefined state, so
	23	* the clean + invalidate performed by v7_flush_dcache_all causes a bunch
	24	* of cache lines with uninitialized data and uninitialized tags to get
	25	* written out to memory, which does really unpleasant things to the main
	26	* processor. We fix this by performing an invalidate, rather than a
	27	* clean + invalidate, before jumping into the kernel.
	28	*/
	29	ENTRY(v7_invalidate_l1)
	30	mov r0, #0
	31	mcr p15, 2, r0, c0, c0, 0
	32	mrc p15, 1, r0, c0, c0, 0
	33
	34	ldr r1, =0x7fff
	35	and r2, r1, r0, lsr #13
	36
	37	ldr r1, =0x3ff
	38
	39	and r3, r1, r0, lsr #3 @ NumWays - 1
	40	add r2, r2, #1 @ NumSets
	41
	42	and r0, r0, #0x7
	43	add r0, r0, #4 @ SetShift
	44
	45	clz r1, r3 @ WayShift
	46	add r4, r3, #1 @ NumWays
	47	1: sub r2, r2, #1 @ NumSets--
	48	mov r3, r4 @ Temp = NumWays
	49	2: subs r3, r3, #1 @ Temp--
	50	mov r5, r3, lsl r1
	51	mov r6, r2, lsl r0
	52	orr r5, r5, r6 @ Reg = (Temp<<WayShift)\|(NumSets<<SetShift)
	53	mcr p15, 0, r5, c7, c6, 2
	54	bgt 2b
	55	cmp r2, #0
	56	bgt 1b
	57	dsb
	58	isb
	59	mov pc, lr
	60	ENDPROC(v7_invalidate_l1)
	61
b36ab975	62
1cea7326	63	ENTRY(tegra_secondary_startup)
1cea7326	64	bl v7_invalidate_l1
b36ab975 PDS	65	/* Enable coresight */
	66	mov32 r0, 0xC5ACCE55
	67	mcr p14, 0, r0, c7, c12, 6
1cea7326 CC	68	b secondary_startup
1cea7326 CC	69	ENDPROC(tegra_secondary_startup)
b36ab975	70
d3f29365 JL	71	#ifdef CONFIG_PM_SLEEP
	72	/*
	73	* tegra_resume
	74	*
	75	* CPU boot vector when restarting the a CPU following
	76	* an LP2 transition. Also branched to by LP0 and LP1 resume after
	77	* re-enabling sdram.
	78	*/
	79	ENTRY(tegra_resume)
	80	bl v7_invalidate_l1
	81	/* Enable coresight */
	82	mov32 r0, 0xC5ACCE55
	83	mcr p14, 0, r0, c7, c12, 6
	84
	85	cpu_id r0
	86	cmp r0, #0 @ CPU0?
	87	bne cpu_resume @ no
	88
	89	#ifdef CONFIG_ARCH_TEGRA_3x_SOC
	90	/* Are we on Tegra20? */
	91	mov32 r6, TEGRA_APB_MISC_BASE
	92	ldr r0, [r6, #APB_MISC_GP_HIDREV]
	93	and r0, r0, #0xff00
	94	cmp r0, #(0x20 << 8)
	95	beq 1f @ Yes
	96	/* Clear the flow controller flags for this CPU. */
	97	mov32 r2, TEGRA_FLOW_CTRL_BASE + FLOW_CTRL_CPU0_CSR @ CPU0 CSR
	98	ldr r1, [r2]
	99	/* Clear event & intr flag */
	100	orr r1, r1, \
	101	#FLOW_CTRL_CSR_INTR_FLAG \| FLOW_CTRL_CSR_EVENT_FLAG
	102	movw r0, #0x0FFD @ enable, cluster_switch, immed, & bitmaps
	103	bic r1, r1, r0
	104	str r1, [r2]
	105	1:
	106	#endif
	107
	108	#ifdef CONFIG_HAVE_ARM_SCU
	109	/* enable SCU */
	110	mov32 r0, TEGRA_ARM_PERIF_BASE
	111	ldr r1, [r0]
	112	orr r1, r1, #1
	113	str r1, [r0]
	114	#endif
	115
29a0e7be JL	116	/* L2 cache resume & re-enable */
	117	l2_cache_resume r0, r1, r2, l2x0_saved_regs_addr
	118
d3f29365 JL	119	b cpu_resume
	120	ENDPROC(tegra_resume)
	121	#endif
	122
29a0e7be JL	123	#ifdef CONFIG_CACHE_L2X0
	124	.globl l2x0_saved_regs_addr
	125	l2x0_saved_regs_addr:
	126	.long 0
	127	#endif
	128
b36ab975 PDS	129	.align L1_CACHE_SHIFT
	130	ENTRY(__tegra_cpu_reset_handler_start)
	131
	132	/*
	133	* __tegra_cpu_reset_handler:
	134	*
	135	* Common handler for all CPU reset events.
	136	*
	137	* Register usage within the reset handler:
	138	*
	139	* R7 = CPU present (to the OS) mask
	140	* R8 = CPU in LP1 state mask
	141	* R9 = CPU in LP2 state mask
	142	* R10 = CPU number
	143	* R11 = CPU mask
	144	* R12 = pointer to reset handler data
	145	*
	146	* NOTE: This code is copied to IRAM. All code and data accesses
	147	* must be position-independent.
	148	*/
	149
	150	.align L1_CACHE_SHIFT
	151	ENTRY(__tegra_cpu_reset_handler)
	152
	153	cpsid aif, 0x13 @ SVC mode, interrupts disabled
	154	mrc p15, 0, r10, c0, c0, 5 @ MPIDR
	155	and r10, r10, #0x3 @ R10 = CPU number
	156	mov r11, #1
	157	mov r11, r11, lsl r10 @ R11 = CPU mask
	158	adr r12, __tegra_cpu_reset_handler_data
	159
	160	#ifdef CONFIG_SMP
	161	/* Does the OS know about this CPU? */
	162	ldr r7, [r12, #RESET_DATA(MASK_PRESENT)]
	163	tst r7, r11 @ if !present
	164	bleq __die @ CPU not present (to OS)
	165	#endif
	166
	167	#ifdef CONFIG_ARCH_TEGRA_2x_SOC
	168	/* Are we on Tegra20? */
	169	mov32 r6, TEGRA_APB_MISC_BASE
	170	ldr r0, [r6, #APB_MISC_GP_HIDREV]
	171	and r0, r0, #0xff00
	172	cmp r0, #(0x20 << 8)
	173	bne 1f
	174	/* If not CPU0, don't let CPU0 reset CPU1 now that CPU1 is coming up. */
	175	mov32 r6, TEGRA_PMC_BASE
	176	mov r0, #0
	177	cmp r10, #0
	178	strne r0, [r6, #PMC_SCRATCH41]
	179	1:
	180	#endif
	181
d3f29365 JL	182	/* Waking up from LP2? */
	183	ldr r9, [r12, #RESET_DATA(MASK_LP2)]
	184	tst r9, r11 @ if in_lp2
	185	beq __is_not_lp2
	186	ldr lr, [r12, #RESET_DATA(STARTUP_LP2)]
	187	cmp lr, #0
	188	bleq __die @ no LP2 startup handler
	189	bx lr
	190
	191	__is_not_lp2:
	192
b36ab975 PDS	193	#ifdef CONFIG_SMP
	194	/*
	195	* Can only be secondary boot (initial or hotplug) but CPU 0
	196	* cannot be here.
	197	*/
	198	cmp r10, #0
	199	bleq __die @ CPU0 cannot be here
	200	ldr lr, [r12, #RESET_DATA(STARTUP_SECONDARY)]
	201	cmp lr, #0
	202	bleq __die @ no secondary startup handler
	203	bx lr
	204	#endif
	205
	206	/*
	207	* We don't know why the CPU reset. Just kill it.
	208	* The LR register will contain the address we died at + 4.
	209	*/
	210
	211	__die:
	212	sub lr, lr, #4
	213	mov32 r7, TEGRA_PMC_BASE
	214	str lr, [r7, #PMC_SCRATCH41]
	215
	216	mov32 r7, TEGRA_CLK_RESET_BASE
	217
	218	/* Are we on Tegra20? */
	219	mov32 r6, TEGRA_APB_MISC_BASE
	220	ldr r0, [r6, #APB_MISC_GP_HIDREV]
	221	and r0, r0, #0xff00
	222	cmp r0, #(0x20 << 8)
	223	bne 1f
	224
	225	#ifdef CONFIG_ARCH_TEGRA_2x_SOC
	226	mov32 r0, 0x1111
	227	mov r1, r0, lsl r10
	228	str r1, [r7, #0x340] @ CLK_RST_CPU_CMPLX_SET
	229	#endif
	230	1:
86e51a2e PDS	231	#ifdef CONFIG_ARCH_TEGRA_3x_SOC
	232	mov32 r6, TEGRA_FLOW_CTRL_BASE
	233
	234	cmp r10, #0
	235	moveq r1, #FLOW_CTRL_HALT_CPU0_EVENTS
	236	moveq r2, #FLOW_CTRL_CPU0_CSR
	237	movne r1, r10, lsl #3
	238	addne r2, r1, #(FLOW_CTRL_CPU1_CSR-8)
	239	addne r1, r1, #(FLOW_CTRL_HALT_CPU1_EVENTS-8)
	240
	241	/* Clear CPU "event" and "interrupt" flags and power gate
	242	it when halting but not before it is in the "WFI" state. */
	243	ldr r0, [r6, +r2]
	244	orr r0, r0, #FLOW_CTRL_CSR_INTR_FLAG \| FLOW_CTRL_CSR_EVENT_FLAG
	245	orr r0, r0, #FLOW_CTRL_CSR_ENABLE
	246	str r0, [r6, +r2]
	247
	248	/* Unconditionally halt this CPU */
	249	mov r0, #FLOW_CTRL_WAITEVENT
	250	str r0, [r6, +r1]
	251	ldr r0, [r6, +r1] @ memory barrier
	252
	253	dsb
	254	isb
	255	wfi @ CPU should be power gated here
	256
	257	/* If the CPU didn't power gate above just kill it's clock. */
	258
	259	mov r0, r11, lsl #8
	260	str r0, [r7, #348] @ CLK_CPU_CMPLX_SET
	261	#endif
	262
b36ab975 PDS	263	/* If the CPU still isn't dead, just spin here. */
	264	b .
	265	ENDPROC(__tegra_cpu_reset_handler)
	266
	267	.align L1_CACHE_SHIFT
	268	.type __tegra_cpu_reset_handler_data, %object
	269	.globl __tegra_cpu_reset_handler_data
	270	__tegra_cpu_reset_handler_data:
	271	.rept TEGRA_RESET_DATA_SIZE
	272	.long 0
	273	.endr
	274	.align L1_CACHE_SHIFT
	275
	276	ENTRY(__tegra_cpu_reset_handler_end)