21ff460d160a551a1abab49f35f2cb4e33d61db4
[linux-2.6.git] / arch / arm / mach-tegra / headsmp.S
1 /*
2  * arch/arm/mach-tegra/headsmp.S
3  *
4  * CPU initialization routines for Tegra SoCs
5  *
6  * Copyright (c) 2009-2011, NVIDIA Corporation.
7  * Copyright (c) 2011 Google, Inc.
8  * Author: Colin Cross <ccross@android.com>
9  *         Gary King <gking@nvidia.com>
10  *
11  * This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License.
14  *
15  * This program is distributed in the hope that it will be useful, but WITHOUT
16  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
17  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
18  * more details.
19  */
20
21 #include <linux/linkage.h>
22 #include <linux/init.h>
23
24 #include <asm/assembler.h>
25 #include <asm/cache.h>
26 #include <asm/page.h>
27
28 #include <mach/iomap.h>
29 #include <mach/io.h>
30
31 #include "asm_macros.h"
32 #include "reset.h"
33 #include "sleep.h"
34
35 #define DEBUG_CPU_RESET_HANDLER 0       /* Non-zero enables debug code */
36
37 #define PMC_SCRATCH41           0x140
38
39 #define RESET_DATA(x)           ((TEGRA_RESET_##x)*4)
40
41
42 #ifdef CONFIG_SMP
43 /*
44  *      tegra_secondary_startup
45  *
46  *       Initial secondary processor boot vector; jumps to kernel's
47  *       secondary_startup routine. Used for initial boot and hotplug
48  *       of secondary CPUs.
49  */
50 ENTRY(tegra_secondary_startup)
51         bl      __invalidate_cpu_state
52         b       secondary_startup
53 ENDPROC(tegra_secondary_startup)
54 #endif
55
56 #ifdef CONFIG_PM_SLEEP
57 /*
58  *      tegra_resume
59  *
60  *        CPU boot vector when restarting the a CPU following
61  *        an LP2 transition. Also branched to by LP0 and LP1 resume after
62  *        re-enabling sdram.
63  */
64 ENTRY(tegra_resume)
65         bl      __invalidate_cpu_state
66
67         cpu_id  r0
68         cmp     r0, #0                          @ CPU0?
69         bne     tegra_cpu_resume_phys           @ no
70
71 #ifndef CONFIG_ARCH_TEGRA_2x_SOC
72         @ Clear the flow controller flags for this CPU.
73         mov32   r2, TEGRA_FLOW_CTRL_BASE+8      @ CPU0 CSR
74         ldr     r1, [r2]
75         orr     r1, r1, #(1 << 15) | (1 << 14)  @ write to clear event & intr
76         movw    r0, #0x0FFD     @ enable, cluster_switch, immed, & bitmaps
77         bic     r1, r1, r0
78         str     r1, [r2]
79 #endif
80
81         /* enable SCU */
82         mov32   r0, TEGRA_ARM_PERIF_BASE
83         ldr     r1, [r0]
84         orr     r1, r1, #1
85         str     r1, [r0]
86
87 #ifdef CONFIG_TRUSTED_FOUNDATIONS
88         /* wake up (should have specified args?) */
89         bl      tegra_generic_smc
90 #endif
91
92         b       tegra_cpu_resume_phys
93 ENDPROC(tegra_resume)
94 #endif
95
96 /*
97  *      __invalidate_cpu_state
98  *
99  *        Invalidates volatile CPU state (SCU tags, caches, branch address
100  *        arrays, exclusive monitor, etc.) so that they can be safely enabled
101  *        instruction caching and branch predicition enabled
102  */
103 __invalidate_cpu_state:
104         clrex
105         mov     r0, #0
106         mcr     p15, 0, r0, c1, c0, 1   @ disable SMP, prefetch, broadcast
107         isb
108         mcr     p15, 0, r0, c7, c5, 0   @ invalidate BTAC, i-cache
109         mcr     p15, 0, r0, c7, c5, 6   @ invalidate branch pred array
110         mcr     p15, 0, r0, c8, c7, 0   @ invalidate unified TLB
111         dsb
112         isb
113
114         cpu_id  r0
115         cmp     r0, #0
116         mov32   r1, (TEGRA_ARM_PERIF_BASE + 0xC)
117         movne   r0, r0, lsl #2
118         movne   r2, #0xf
119         movne   r2, r2, lsl r0
120         strne   r2, [r1]                @ invalidate SCU tags for CPU
121
122         dsb
123         mov     r0, #0x1800
124         mcr     p15, 0, r0, c1, c0, 0   @ enable branch prediction, i-cache
125         isb
126         /* fall through */
127
128 /*
129  *      tegra_invalidate_l1
130  *
131  *        Invalidates the L1 data cache (no clean) during initial boot of a cpu
132  *
133  *        Corrupted registers: r0-r6
134  */
135 tegra_invalidate_l1:
136         mov     r0, #0
137         mcr     p15, 2, r0, c0, c0, 0
138         mrc     p15, 1, r0, c0, c0, 0
139
140         movw    r1, #0x7fff
141         and     r2, r1, r0, lsr #13
142
143         movw    r1, #0x3ff
144
145         and     r3, r1, r0, lsr #3      @ NumWays - 1
146         add     r2, r2, #1      @ NumSets
147
148         and     r0, r0, #0x7
149         add     r0, r0, #4      @ SetShift
150
151         clz     r1, r3          @ WayShift
152         add     r4, r3, #1      @ NumWays
153 1:      sub     r2, r2, #1      @ NumSets--
154         mov     r3, r4          @ Temp = NumWays
155 2:      subs    r3, r3, #1      @ Temp--
156         mov     r5, r3, lsl r1
157         mov     r6, r2, lsl r0
158         orr     r5, r5, r6      @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
159         mcr     p15, 0, r5, c7, c6, 2
160         bgt     2b
161         cmp     r2, #0
162         bgt     1b
163         dsb
164         isb
165         mov     pc, lr
166
167 /*
168  * __tegra_cpu_reset_handler_halt_failed:
169  *
170  * Alternate entry point for reset handler for cases where the
171  * WFI halt failed to take effect.
172  *
173  */
174         .align L1_CACHE_SHIFT
175 ENTRY(__tegra_cpu_reset_handler_start)
176
177 /*
178  * __tegra_cpu_reset_handler:
179  *
180  * Common handler for all CPU reset events.
181  *
182  * Register usage within the reset handler:
183  *
184  *      R7  = CPU present (to the OS) mask
185  *      R8  = CPU in LP1 state mask
186  *      R9  = CPU in LP2 state mask
187  *      R10 = CPU number
188  *      R11 = CPU mask
189  *      R12 = pointer to reset handler data
190  *
191  * NOTE: This code is copied to IRAM. All code and data accesses
192  *       must be position-independent.
193  */
194
195         .align L1_CACHE_SHIFT
196 ENTRY(__tegra_cpu_reset_handler)
197
198 #if DEBUG_CPU_RESET_HANDLER
199         b       .
200 #endif
201 #ifndef CONFIG_TRUSTED_FOUNDATIONS
202         cpsid   aif, 0x13                       @ SVC mode, interrupts disabled
203         mrc     p15, 0, r0, c0, c0, 0           @ read main ID register
204         and     r5, r0, #0x00f00000             @ variant
205         and     r6, r0, #0x0000000f             @ revision
206         orr     r6, r6, r5, lsr #20-4           @ combine variant and revision
207 #ifdef CONFIG_ARM_ERRATA_743622
208         teq     r6, #0x20                       @ present in r2p0
209         teqne   r6, #0x21                       @ present in r2p1
210         teqne   r6, #0x22                       @ present in r2p2
211         teqne   r6, #0x27                       @ present in r2p7
212         teqne   r6, #0x29                       @ present in r2p9
213         mrceq   p15, 0, r10, c15, c0, 1         @ read diagnostic register
214         orreq   r10, r10, #1 << 6               @ set bit #6
215         mcreq   p15, 0, r10, c15, c0, 1         @ write diagnostic register
216 #endif
217 #endif
218         mrc     p15, 0, r10, c0, c0, 5          @ MPIDR
219         and     r10, r10, #0x3                  @ R10 = CPU number
220         mov     r11, #1
221         mov     r11, r11, lsl r10               @ R11 = CPU mask
222         adr     r12, __tegra_cpu_reset_handler_data
223
224 #ifdef CONFIG_SMP
225         /* Does the OS know about this CPU? */
226         ldr     r7, [r12, #RESET_DATA(MASK_PRESENT)]
227         tst     r7, r11                         @ if !present
228         bleq    __die                           @ CPU not present (to OS)
229 #endif
230
231 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
232         /* If CPU1, don't let CPU0 reset CPU1 now that CPU1 is coming up. */
233         mov32   r6, TEGRA_PMC_BASE
234         mov     r0, #0
235         cmp     r10, #0
236         strne   r0, [r6, #PMC_SCRATCH41]
237 #endif
238
239 #ifdef CONFIG_PM_SLEEP
240         /* Waking up from LP1? */
241         ldr     r8, [r12, #RESET_DATA(MASK_LP1)]
242         tst     r8, r11                         @ if in_lp1
243         beq     __is_not_lp1
244         cmp     r10, #0
245         bne     __die                           @ only CPU0 can be here
246         ldr     lr, [r12, #RESET_DATA(STARTUP_LP1)]
247         cmp     lr, #0
248         bleq    __die                           @ no LP1 startup handler
249         bx      lr
250 __is_not_lp1:
251 #endif
252
253         /* Waking up from LP2? */
254         ldr     r9, [r12, #RESET_DATA(MASK_LP2)]
255         tst     r9, r11                         @ if in_lp2
256         beq     __is_not_lp2
257         ldr     lr, [r12, #RESET_DATA(STARTUP_LP2)]
258         cmp     lr, #0
259         bleq    __die                           @ no LP2 startup handler
260         bx      lr
261
262 __is_not_lp2:
263
264 #ifdef CONFIG_SMP
265         /* Can only be secondary boot (initial or hotplug) but CPU 0
266            cannot be here. */
267         cmp     r10, #0
268         bleq    __die                           @ CPU0 cannot be here
269         ldr     lr, [r12, #RESET_DATA(STARTUP_SECONDARY)]
270         cmp     lr, #0
271         bleq    __die                           @ no secondary startup handler
272         bx      lr
273 #endif
274
275 /*
276  * We don't know why the CPU reset. Just kill it.
277  * The LR register will contain the address we died at + 4.
278  */
279
280 __die:
281         sub     lr, lr, #4
282         mov32   r7, TEGRA_PMC_BASE
283         str     lr, [r7, #PMC_SCRATCH41]
284
285         mov32   r7, TEGRA_CLK_RESET_BASE
286
287 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
288         mov32   r0, 0x1111
289         mov     r1, r0, lsl r10
290         str     r1, [r7, #0x340]                @ CLK_RST_CPU_CMPLX_SET
291 #else
292         mov32   r6, TEGRA_FLOW_CTRL_BASE
293
294         cmp     r10, #0
295         moveq   r1, #FLOW_CTRL_HALT_CPU0_EVENTS
296         moveq   r2, #FLOW_CTRL_CPU0_CSR
297         movne   r1, r10, lsl #3
298         addne   r2, r1, #(FLOW_CTRL_CPU1_CSR-8)
299         addne   r1, r1, #(FLOW_CTRL_HALT_CPU1_EVENTS-8)
300
301         /* Clear CPU "event" and "interrupt" flags and power gate
302            it when halting but not before it is in the "WFI" state. */
303         ldr     r0, [r6, +r2]
304         orr     r0, r0, #FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG
305         orr     r0, r0, #FLOW_CTRL_CSR_ENABLE
306         str     r0, [r6, +r2]
307
308         /* Unconditionally halt this CPU */
309         mov     r0, #FLOW_CTRL_WAITEVENT
310         str     r0, [r6, +r1]
311         ldr     r0, [r6, +r1]                   @ memory barrier
312
313         dsb
314         isb
315         wfi                                     @ CPU should be power gated here
316
317         /* If the CPU didn't power gate above just kill it's clock. */
318
319         mov     r0, r11, lsl #8
320         str     r0, [r7, #348]                  @ CLK_CPU_CMPLX_SET
321 #endif
322         /* If the CPU still isn't dead, just spin here. */
323         b       .
324
325 ENDPROC(__tegra_cpu_reset_handler)
326         .align L1_CACHE_SHIFT
327         .type   __tegra_cpu_reset_handler_data, %object
328         .globl  __tegra_cpu_reset_handler_data
329 __tegra_cpu_reset_handler_data:
330         .rept   TEGRA_RESET_DATA_SIZE
331         .long   0
332         .endr
333         .size   __tegra_cpu_reset_handler_data, .-tegra_cpu_reset_handler_data
334         .align L1_CACHE_SHIFT
335 ENTRY(__tegra_cpu_reset_handler_end)