tlk: 6/19 update
[3rdparty/ote_partner/tlk.git] / platform / tegra / common / boot_secondary.S
1 /*
2  * Copyright (c) 2012-2014, NVIDIA CORPORATION. All rights reserved
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining
5  * a copy of this software and associated documentation files
6  * (the "Software"), to deal in the Software without restriction,
7  * including without limitation the rights to use, copy, modify, merge,
8  * publish, distribute, sublicense, and/or sell copies of the Software,
9  * and to permit persons to whom the Software is furnished to do so,
10  * subject to the following conditions:
11  *
12  * The above copyright notice and this permission notice shall be
13  * included in all copies or substantial portions of the Software.
14  *
15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22  */
23 #include <platform/boot_secondary.h>
24
25 #define DEBUG_CPU_RESET_HANDLER         0
26 #define SCRATCH_WORDS                   8
27 #define CLK_RESET_CCLK_BURST            0x20
28 #define CLK_RESET_CCLK_DIVIDER          0x24
29 #define CLK_RESET_SCLK_BURST            0x28
30 #define CLK_RESET_SCLK_DIVIDER          0x2c
31
32 .globl invalidate_l1_v7
33 invalidate_l1_v7:
34         mov     r0, #0
35         mcr     p15, 2, r0, c0, c0, 0
36         mrc     p15, 1, r0, c0, c0, 0
37
38         movw    r1, #0x7fff
39         and     r2, r1, r0, lsr #13     @ get number of sets
40
41         movw    r1, #0x3ff
42
43         and     r3, r1, r0, lsr #3      @ NumWays - 1
44         add     r2, r2, #1              @ NumSets
45
46         and     r0, r0, #0x7
47         add     r0, r0, #4              @ SetShift
48
49         clz     r1, r3                  @ WayShift
50         add     r4, r3, #1              @ NumWays
51 1:      sub     r2, r2, #1              @ NumSets--
52         mov     r3, r4                  @ Temp = NumWays
53 2:      subs    r3, r3, #1              @ Temp--
54         mov     r5, r3, lsl r1
55         mov     r6, r2, lsl r0
56         orr     r5, r5, r6              @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
57         mcr     p15, 0, r5, c7, c6, 2
58         bgt     2b
59         cmp     r2, #0
60         bgt     1b
61         dsb
62         isb
63         mov     pc, lr
64
65 .globl _boot_secondary_cpu
66 _boot_secondary_cpu:
67         clrex
68
69         cpsid   aif, 0x13                       @ SVC mode, interrupts disabled
70 #if ARM_CPU_CORTEX_A9
71         mrc     p15, 0, r0, c0, c0, 0           @ read main ID register
72         and     r5, r0, #0x00f00000             @ variant
73         and     r6, r0, #0x0000000f             @ revision
74         orr     r6, r6, r5, lsr #20-4           @ combine variant and revision
75         /* ARM_ERRATA_743622 */
76         teq     r6, #0x20                       @ present in r2p0
77         teqne   r6, #0x21                       @ present in r2p1
78         teqne   r6, #0x22                       @ present in r2p2
79         teqne   r6, #0x27                       @ present in r2p7
80         teqne   r6, #0x29                       @ present in r2p9
81         mrceq   p15, 0, r10, c15, c0, 1         @ read diagnostic register
82         orreq   r10, r10, #1 << 6               @ set bit #6
83         mcreq   p15, 0, r10, c15, c0, 1         @ write diagnostic register
84 #endif
85 #if ARM_CPU_CORTEX_A15
86         mov     r0, #0
87         mcr     p15, 0, r0, c1, c0, 1           @ disable SMP, prefetch, broadcast
88         isb
89 #endif
90
91 #if DEBUG_CPU_RESET_HANDLER
92         b       .
93 #endif
94
95 #if ARM_CPU_CORTEX_A15
96         mrc     p15, 0x1, r0, c15, c0, 3        @ L2 prefetch control reg
97         tst     r0, #0x1000
98         orreq   r0, r0, #0x1000                 @ disable throttling
99         mcreq   p15, 0x1, r0, c15, c0, 3
100
101         mrc     p15, 0, r0, c0, c0, 0           @ main ID register
102         ubfx    r1, r0, #20, #4
103         cmp     r1, #0x3                        @ Compare to rev 3
104         mrcge   p15, 0x1, r0, c15, c0, 4        @ ACTLR2
105         orrge   r0, #(1<<31)                    @ Enable regional clock gates
106         mcrge   p15, 0x1, r0, c15, c0, 4        @ Rev 3 or higher
107
108 #if ARM_CLUSTER0_INIT_L2
109         /* This is only needed for cluster 0 with integrated L2 cache */
110         mrc     p15, 0, r0, c0, c0, 5
111         ubfx    r0, r0, #8, #4
112         tst     r0, #1
113         bne     __enable_i_cache
114         mrc     p15, 0x1, r0, c9, c0, 2
115         and     r1, r0, #7
116         cmp     r1, #2
117         beq     __enable_i_cache
118         bic     r0, r0, #7
119         orr     r0, r0, #2
120         mcr     p15, 0x1, r0, c9, c0, 2
121 __enable_i_cache:
122 #endif
123         mrc     p15, 0, r0, c1, c0, 0
124         orr     r0, r0, #0x1000
125         mcr     p15, 0, r0, c1, c0, 0           @ enable i-cache
126         b       cpu_context_restore
127 #endif
128
129 #if ARM_CPU_CORTEX_A9
130         mcr     p15, 0, r0, c7, c5, 0           @ invalidate BTAC, i-cache
131         mcr     p15, 0, r0, c7, c5, 6           @ invalidate branch pred array
132         mcr     p15, 0, r0, c8, c5, 0           @ invalidate instruction TLB
133         mcr     p15, 0, r0, c8, c6, 0           @ invalidate data TLB
134         mcr     p15, 0, r0, c8, c7, 0           @ invalidate unified TLB
135         dsb
136         isb
137 #endif
138
139 #if ARM_WITH_SCU
140         cpu_id  r0
141         cmp     r0, #0
142         ldrne   r1, =TEGRA_ARM_SCU_BASE
143         movne   r0, r0, lsl #2
144         movne   r2, #0xf
145         movne   r2, r2, lsl r0
146         strne   r2, [r1, #0xC]          @ invalidate SCU tags for CPU
147 #endif
148
149 #if ARM_CPU_CORTEX_A9
150         dsb
151         ldr     r0, =0x1000
152         mcr     p15, 0, r0, c1, c0, 0           @ enable i-cache
153         isb
154
155         bl      invalidate_l1_v7        @ invalidate D$
156 #endif
157
158 cpu_context_restore:
159         ldr     r0, =_cpu_context
160         ldr     r1, =VMEMBASE
161         sub     r0, r0, r1
162         adr     r1, _boot_secondary_phys_base
163         ldr     r1, [r1]
164
165         /* restore cpu context */
166         add     r0, r0, r1                      @ phys base of context
167         mov     r1, #1                          @ enable MMU
168         cpu_restore_context r0, r1, r2, r3
169
170         /* go virtual now */
171         ldr     r1, =go_virtual
172         mov     pc, r1
173 go_virtual:
174
175         cpu_id  r0
176         cmp     r0, #0                          @ CPU 0?
177         bne     1110f
178
179         ldr     r0, =svc_scratch_space
180         add     sp, r0, #(SCRATCH_WORDS * 4)    @ temporary svc_sp
181 #if ARM_WITH_SCU
182         ldr     r0, =cpu_enable_scu             @ enable SCU
183         blx     r0
184
185         ldr     r0, =cpu_enable_scu_access      @ r/w scu regs from all cpus
186         blx     r0
187 #endif
188         /* allow full access to CP10 and CP11 */
189         mrc     p15, 0, r0, c1, c0, 2           @ CPACR
190         orr     r0, r0, #0x00F00000
191         mcr     p15, 0, r0, c1, c0, 2           @ CPACR
192         isb
193
194         ldr     r0, =pm_early_init
195         blx     r0
196
197         ldr     r0, =cpu_power_down_mode
198         ldr     r0, [r0]
199         cmp     r0, #CPU_IN_LP0
200         ldreq   r0, =platform_restore_memory
201         blxeq   r0
202
203         /* enable user space perf counter access */
204         mrc     p15, 0, r0, c9, c12, 0
205         lsr     r0, #11
206         and     r0, r0, #0x1f
207         mov     r1, #1
208         lsl     r1, r1, r0
209         sub     r1, r1, #1
210         movt    r1, #0x8000
211         mcr     p15, 0, r1, c9, c14, 2
212         mov     r0, #1
213         mcr     p15, 0, r0, c9, c14, 0
214
215 1110:
216         ldr     r0, =platform_config_interrupts
217         blx     r0
218
219         /* enable SCR.FW */
220         mov     r0, #(1 << 4)           @ SCR.FW
221         mcr     p15, 0, r0, c1, c1, 0
222         isb
223
224         /* let normal world enable SMP, lock TLB, access CP10/11 */
225         mrc     p15, 0, r0, c1, c1, 2           @ NSACR
226         orr     r0, r0, #0x00000C00
227         orr     r0, r0, #0x00060000
228         mcr     p15, 0, r0, c1, c1, 2           @ NSACR
229
230 #if ARM_CPU_CORTEX_A9
231         /* normal world can write SMP, but not FW */
232         mrc     p15, 0, r0, c1, c0, 1
233         orr     r0, r0, #(0x1 << 0)             @ enable FW
234         mcr     p15, 0, r0, c1, c0, 1           @ ACTLR
235 #endif
236
237         /* mark banked CPU intrs (31:0) be non-secure */
238         ldr     r0, =cpu_gic_setup
239         blx     r0
240
241         /* goto monitor mode */
242         cpu_id  r0
243         cmp     r0, #0                          @ are we CPU #0?
244         ldreq   lr, =mon_stack_top
245         ldreq   r0, [lr]
246         cps     #0x16                           @ monitor mode
247         moveq   sp, r0                          @ restore mon_sp
248
249         /* NS world's entry point */
250         ldr     r0, =cpu_power_down_mode
251         ldr     r1, [r0]
252         cmp     r1, #CPU_IN_LP0
253         ldreq   lr, =_ns_resume_addr
254         moveq   r1, #0
255         streq   r1, [r0]
256         ldrne   lr, =_ns_addr_secondary_cpus
257         ldr     lr, [lr]
258         cmp     lr, #0
259         beq     .                               @ error: if NULL entry point
260
261         /* NS=1 */
262         mrc     p15, 0, r0, c1, c1, 0           @ SCR
263         orr     r0, r0, #0x1
264         mcr     p15, 0, r0, c1, c1, 0           @ NS=1 mode
265         isb
266
267         /*
268          * set shadow spsr_mon, with current reset CPSR state
269          * (e.g. intrs disabled) and changed the mode to SVC
270          */
271         mrs     r0, cpsr
272         bic     r0, #0x1f
273         orr     r0, #0x13
274         msr     spsr_cxfs, r0
275         movs    pc, lr
276
277 .globl boot_secondary_cpu_addr
278 boot_secondary_cpu_addr:        .word   _boot_secondary_cpu
279
280 .globl svc_scratch_space
281 svc_scratch_space:
282         .rept   SCRATCH_WORDS
283         .long   0
284         .endr
285
286 .globl _ns_resume_addr
287 _ns_resume_addr:
288         .word   0
289
290 .global _boot_secondary_phys_base
291 _boot_secondary_phys_base:
292         .int 0
293
294 /*
295  * LP1 resume code.  This code is executed out of TZRAM.  Note that DRAM
296  * is in self-refresh while this code is executing.
297  */
298 .globl _lp1_resume
299 _lp1_resume:
300         clrex
301
302         cpsid   aif, 0x13                       @ SVC mode, interrupts disabled
303 #if ARM_CPU_CORTEX_A9
304         mrc     p15, 0, r0, c0, c0, 0           @ read main ID register
305         and     r5, r0, #0x00f00000             @ variant
306         and     r6, r0, #0x0000000f             @ revision
307         orr     r6, r6, r5, lsr #20-4           @ combine variant and revision
308         /* ARM_ERRATA_743622 */
309         teq     r6, #0x20                       @ present in r2p0
310         teqne   r6, #0x21                       @ present in r2p1
311         teqne   r6, #0x22                       @ present in r2p2
312         teqne   r6, #0x27                       @ present in r2p7
313         teqne   r6, #0x29                       @ present in r2p9
314         mrceq   p15, 0, r10, c15, c0, 1         @ read diagnostic register
315         orreq   r10, r10, #1 << 6               @ set bit #6
316         mcreq   p15, 0, r10, c15, c0, 1         @ write diagnostic register
317 #endif
318 #if ARM_CPU_CORTEX_A15
319         mov     r0, #0
320         mcr     p15, 0, r0, c1, c0, 1           @ disable SMP, prefetch, broadcast
321         isb
322 #endif
323
324 #if DEBUG_CPU_RESET_HANDLER
325         b       .
326 #endif
327
328 #if ARM_CPU_CORTEX_A15
329         mrc     p15, 0x1, r0, c15, c0, 3        @ L2 prefetch control reg
330         tst     r0, #0x1000
331         orreq   r0, r0, #0x1000                 @ disable throttling
332         mcreq   p15, 0x1, r0, c15, c0, 3
333
334         mrc     p15, 0, r0, c0, c0, 0           @ main ID register
335         ubfx    r1, r0, #20, #4
336         cmp     r1, #0x3                        @ Compare to rev 3
337         mrcge   p15, 0x1, r0, c15, c0, 4        @ ACTLR2
338         orrge   r0, #(1<<31)                    @ Enable regional clock gates
339         mcrge   p15, 0x1, r0, c15, c0, 4        @ Rev 3 or higher
340
341 #if ARM_CLUSTER0_INIT_L2
342         /* set up L2 for cluster0 */
343         mrc     p15, 0, r0, c0, c0, 5
344         ubfx    r0, r0, #8, #4                  @ get cluster id
345         cmp     r0, #0                          @ C0 = 0, C1 = 1
346         bne     not_on_cluster0
347
348         mrc     p15, 0x1, r0, c9, c0, 0x2       @ read L2CTLR
349         bic     r0, r0, #0x7                    @ clear data ram latency field
350         orr     r0, r0, #0x2                    @ pipeline depth of 3
351         mcr     p15, 0x1, r0, c9, c0, 0x2       @ write L2CTLR
352
353 not_on_cluster0:
354 #endif
355 #endif
356
357         /* check if we should avoid switching to CLKM */
358         ldr     r0, =TZRAM_CPU_AVOID_CLKM_SWITCH
359         ldr     r0, [r0]
360         cmp     r0, #1
361         beq     avoid_clkm_switch
362
363         /* resuming from LP1 so handle 32KHz to CLKM/OSC clock switch */
364         ldr     r0, =TEGRA_CLK_RESET_BASE
365         mov     r1, #(1<<28)
366         str     r1, [r0, #CLK_RESET_SCLK_BURST]
367         str     r1, [r0, #CLK_RESET_CCLK_BURST]
368         mov     r1, #0
369         str     r1, [r0, #CLK_RESET_SCLK_DIVIDER]
370         str     r1, [r0, #CLK_RESET_CCLK_DIVIDER]
371
372 avoid_clkm_switch:
373         mov     r1, #0
374         ldr     r0, =TZRAM_CPU_AVOID_CLKM_SWITCH
375         str     r1, [r0]                @ clear the clkm switch flag
376
377 #if ARM_CPU_CORTEX_A15
378         mrc     p15, 0x1, r0, c15, c0, 3        @ L2 prefetch control reg
379         tst     r0, #0x1000
380         orreq   r0, r0, #0x1000                 @ disable throttling
381         mcreq   p15, 0x1, r0, c15, c0, 3
382
383         mrc     p15, 0, r0, c0, c0, 0           @ main ID register
384         ubfx    r1, r0, #20, #4
385         cmp     r1, #0x3                        @ Compare to rev 3
386         mrcge   p15, 0x1, r0, c15, c0, 4        @ ACTLR2
387         orrge   r0, #(1<<31)                    @ Enable regional clock gates
388         mcrge   p15, 0x1, r0, c15, c0, 4        @ Rev 3 or higher
389
390         /* This is only needed for cluster 0 with integrated L2 cache */
391         mrc     p15, 0, r0, c0, c0, 5
392         ubfx    r0, r0, #8, #4
393         tst     r0, #1
394         bne     lp1_enable_i_cache
395         mrc     p15, 0x1, r0, c9, c0, 2
396         and     r1, r0, #7
397         cmp     r1, #2
398         beq     lp1_enable_i_cache
399         bic     r0, r0, #7
400         orr     r0, r0, #2
401         mcr     p15, 0x1, r0, c9, c0, 2
402 lp1_enable_i_cache:
403         mrc     p15, 0, r0, c1, c0, 0
404         orr     r0, r0, #0x1000
405         mcr     p15, 0, r0, c1, c0, 0           @ enable i-cache
406         b       lp1_cpu_context_restore
407 #endif
408
409 #if ARM_CPU_CORTEX_A9
410         mcr     p15, 0, r0, c7, c5, 0           @ invalidate BTAC, i-cache
411         mcr     p15, 0, r0, c7, c5, 6           @ invalidate branch pred array
412         mcr     p15, 0, r0, c8, c5, 0           @ invalidate instruction TLB
413         mcr     p15, 0, r0, c8, c6, 0           @ invalidate data TLB
414         mcr     p15, 0, r0, c8, c7, 0           @ invalidate unified TLB
415         dsb
416         isb
417 #endif
418
419 #if ARM_WITH_SCU
420         cpu_id  r0
421         cmp     r0, #0
422         ldrne   r1, =TEGRA_ARM_SCU_BASE
423         movne   r0, r0, lsl #2
424         movne   r2, #0xf
425         movne   r2, r2, lsl r0
426         strne   r2, [r1, #0xC]                  @ invalidate SCU tags for CPU
427 #endif
428
429 #if ARM_CPU_CORTEX_A9
430         dsb
431         ldr     r0, =0x1000
432         mcr     p15, 0, r0, c1, c0, 0           @ enable i-cache
433         isb
434
435         /* invalidate dcache */
436         mov     r0, #0
437         mcr     p15, 2, r0, c0, c0, 0
438         mrc     p15, 1, r0, c0, c0, 0
439
440         movw    r1, #0x7fff
441         and     r2, r1, r0, lsr #13     @ get number of sets
442
443         movw    r1, #0x3ff
444
445         and     r3, r1, r0, lsr #3      @ NumWays - 1
446         add     r2, r2, #1              @ NumSets
447
448         and     r0, r0, #0x7
449         add     r0, r0, #4              @ SetShift
450
451         clz     r1, r3                  @ WayShift
452         add     r4, r3, #1              @ NumWays
453 1:      sub     r2, r2, #1              @ NumSets--
454         mov     r3, r4                  @ Temp = NumWays
455 2:      subs    r3, r3, #1              @ Temp--
456         mov     r5, r3, lsl r1
457         mov     r6, r2, lsl r0
458         orr     r5, r5, r6              @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
459         mcr     p15, 0, r5, c7, c6, 2
460         bgt     2b
461         cmp     r2, #0
462         bgt     1b
463         dsb
464         isb
465 #endif
466
467 lp1_cpu_context_restore:
468         /* restore cpu context */
469         ldr     r0, =TZRAM_CPU_CONTEXT          @ phys base of context
470         mov     r1, #0                          @ leave MMU disabled
471         cpu_restore_context r0, r1, r2, r3
472         isb
473
474         ldr     r0, =TZRAM_TEMP_SVC_SP          @ setup tempory svc sp
475         add     sp, r0, #(SCRATCH_WORDS * 4)
476
477 #if ARM_WITH_SCU
478         ldr     r3, =TEGRA_ARM_SCU_BASE         @ enable SCU
479         mov     r2, #1
480         str     r2, [r3]
481
482         ldr     r2, =0xf                        @ r/w scu regs from all cpus
483         str     r2, [r3, #0x50]
484         ldr     r2, =0xfff
485         str     r2, [r3, #0x54]
486 #endif
487         /* allow full access to CP10 and CP11 */
488         mrc     p15, 0, r0, c1, c0, 2           @ CPACR
489         orr     r0, r0, #0x00F00000
490         mcr     p15, 0, r0, c1, c0, 2           @ CPACR
491         isb
492
493         /* configure interrupts */
494         mov     r3, #0
495         mvn     r1, #0
496 interrupt_loop:
497         ldr     r2, =TEGRA_ARM_INT_DIST_BASE    @ should be better way to handle
498         add     r2, r2, #0x80                   @ add here
499         asr     r0, r3, #0x3
500         add     r3, r3, #0x20
501         cmp     r3, #0x320
502         add     r2, r0, r2
503         str     r1, [r2]
504         bne     interrupt_loop
505
506         /* restore reset vector */
507         ldr     r3, =TZRAM_BOOT_SECONDARY_CPU_ADDR
508         ldr     r2, [r3]
509         ldr     r3, =TEGRA_EXCEPTION_VECTORS_BASE
510         str     r2, [r3, #0x100]
511         dmb     sy
512         ldr     r3, [r3, #0x100]                @ dummy read to ensure write
513
514         /* mark banked CPU intrs (31:0) be non-secure */
515         ldr     r3, =TEGRA_ARM_INT_DIST_BASE
516         mvn     r2, #0x0
517         str     r2, [r3, #0x80]                 @ DIST_IRQ_SEC
518         ldr     r3, =TEGRA_ARM_INT_CPU_BASE
519         ldr     r2, =0xff
520         str     r2, [r3, #0x4]                  @ GIC_CPU_PRIMASK
521
522         /* restore monitor_vector base */
523         ldr     r3, =TZRAM_MVBAR
524         ldr     r3, [r3]
525         mcr     p15, 0, r3, c12, c0, 1
526
527         /* let normal world enable SMP, lock TLB, access CP10/11 */
528         mrc     p15, 0, r0, c1, c1, 2           @ NSACR
529         orr     r0, r0, #0x00000C00
530         orr     r0, r0, #0x00060000
531         mcr     p15, 0, r0, c1, c1, 2           @ NSACR
532
533 #if ARM_CPU_CORTEX_A9
534         /* normal world can write SMP, but not FW */
535         mrc     p15, 0, r0, c1, c0, 1
536         orr     r0, r0, #(0x1 << 0)             @ enable FW
537         mcr     p15, 0, r0, c1, c0, 1           @ ACTLR
538 #endif
539
540         /* goto monitor mode */
541         ldr     lr, =TZRAM_MON_STACK_TOP
542         ldr     r0, [lr]
543         cps     #0x16                           @ monitor mode
544         mov     sp, r0                          @ restore mon_sp
545
546         /* NS world's entry point */
547         ldr     lr, =TZRAM_NS_RESUME_ADDR
548         ldr     lr, [lr]
549         cmp     lr, #0
550         beq     .                               @ error: if NULL entry point
551
552         /* enable SCR.FW and NS=1 */
553         mrc     p15, 0, r0, c1, c1, 0           @ SCR
554         mov     r0, #(1 << 4) | 0x1             @ SCR.FW and NS=1
555         mcr     p15, 0, r0, c1, c1, 0
556         isb
557
558         /*
559          * set shadow spsr_mon, with current reset CPSR state
560          * (e.g. intrs disabled) and changed the mode to SVC
561          */
562         mrs     r0, cpsr
563         bic     r0, #0x1f
564         orr     r0, #0x13
565         msr     spsr_cxfs, r0
566         movs    pc, lr
567
568 .ltorg
569 .globl _lp1_resume_end
570 _lp1_resume_end:
571         b       .
572
573 .globl lp1_resume_addr
574 lp1_resume_addr:        .word   _lp1_resume