First version
[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 .globl flush_dcache_all
295 flush_dcache_all:
296         stmfd   sp!, {r4-r5, r7-r8, r9-r11, lr}
297         mrs     r8, cpsr                        @ save the old interrupt state
298         cpsid   iaf                             @ cpsid iaf, interrupts disabled
299
300         dmb
301         mrc     p15, 0, r2, c1, c0, 0
302         bic     r2, r2, #(1 << 2)               @ disable D-cache
303         dsb
304         mcr     p15, 0, r2, c1, c0, 0
305         dmb                                     @ ensure ordering with previous memory accesses
306         mrc     p15, 1, r0, c0, c0, 1           @ read clidr
307         ands    r3, r0, #0x7000000              @ extract loc from clidr
308         mov     r3, r3, lsr #23                 @ left align loc bit field
309         beq     1005f                           @ if loc is 0, then no need to clean
310         mov     r10, #0                         @ start clean at cache level 0
311 1001:
312         add     r2, r10, r10, lsr #1            @ work out 3x current cache level
313         mov     r1, r0, lsr r2                  @ extract cache type bits from clidr
314         and     r1, r1, #7                      @ mask of the bits for current cache only
315         cmp     r1, #2                          @ see what cache we have at this level
316         blt     1004f                           @ skip if no cache, or just i-cache
317         mrs     r9, cpsr                        @ save/disable irqs to make ccsr&csidr reads atomic
318         cpsid   i
319         mcr     p15, 2, r10, c0, c0, 0          @ select current cache level in cssr
320         isb                                     @ isb to sych the new cssr&csidr
321         mrc     p15, 1, r1, c0, c0, 0           @ read the new csidr
322         msr     cpsr_c, r9
323         and     r2, r1, #7                      @ extract the length of the cache lines
324         add     r2, r2, #4                      @ add 4 (line length offset)
325         ldr     r4, =0x3ff
326         ands    r4, r4, r1, lsr #3              @ find maximum number on the way size
327         clz     r5, r4                          @ find bit position of way size increment
328         ldr     r7, =0x7fff
329         ands    r7, r7, r1, lsr #13             @ extract max number of the index size
330 1002:
331         mov     r9, r4                          @ create working copy of max way size
332 1003:
333         orr     r11, r10, r9, lsl r5            @ factor way and cache number into r11
334         orr     r11, r11, r7, lsl r2            @ factor index number into r11
335         mcr     p15, 0, r11, c7, c14, 2         @ flush by set/way
336         subs    r9, r9, #1                      @ decrement the way
337         bge     1003b
338         subs    r7, r7, #1                      @ decrement the index
339         bge     1002b
340 1004:
341         add     r10, r10, #2                    @ increment cache number
342         cmp     r3, r10
343         bgt     1001b
344 1005:
345         mov     r10, #0                         @ swith back to cache level 0
346         mcr     p15, 2, r10, c0, c0, 0          @ select current cache level in cssr
347         dsb
348         isb
349
350         msr     cpsr, r8                        @ restore previous intr state
351         ldmfd   sp!, {r4-r5, r7-r8, r9-r11, lr}
352         mov     pc, lr
353
354 /*
355  * LP1 resume code.  This code is executed out of TZRAM.  Note that DRAM
356  * is in self-refresh while this code is executing.
357  */
358 .globl _lp1_resume
359 _lp1_resume:
360         clrex
361
362         cpsid   aif, 0x13                       @ SVC mode, interrupts disabled
363 #if ARM_CPU_CORTEX_A9
364         mrc     p15, 0, r0, c0, c0, 0           @ read main ID register
365         and     r5, r0, #0x00f00000             @ variant
366         and     r6, r0, #0x0000000f             @ revision
367         orr     r6, r6, r5, lsr #20-4           @ combine variant and revision
368         /* ARM_ERRATA_743622 */
369         teq     r6, #0x20                       @ present in r2p0
370         teqne   r6, #0x21                       @ present in r2p1
371         teqne   r6, #0x22                       @ present in r2p2
372         teqne   r6, #0x27                       @ present in r2p7
373         teqne   r6, #0x29                       @ present in r2p9
374         mrceq   p15, 0, r10, c15, c0, 1         @ read diagnostic register
375         orreq   r10, r10, #1 << 6               @ set bit #6
376         mcreq   p15, 0, r10, c15, c0, 1         @ write diagnostic register
377 #endif
378 #if ARM_CPU_CORTEX_A15
379         mov     r0, #0
380         mcr     p15, 0, r0, c1, c0, 1           @ disable SMP, prefetch, broadcast
381         isb
382 #endif
383
384 #if DEBUG_CPU_RESET_HANDLER
385         b       .
386 #endif
387
388 #if ARM_CPU_CORTEX_A15
389         mrc     p15, 0x1, r0, c15, c0, 3        @ L2 prefetch control reg
390         tst     r0, #0x1000
391         orreq   r0, r0, #0x1000                 @ disable throttling
392         mcreq   p15, 0x1, r0, c15, c0, 3
393
394         mrc     p15, 0, r0, c0, c0, 0           @ main ID register
395         ubfx    r1, r0, #20, #4
396         cmp     r1, #0x3                        @ Compare to rev 3
397         mrcge   p15, 0x1, r0, c15, c0, 4        @ ACTLR2
398         orrge   r0, #(1<<31)                    @ Enable regional clock gates
399         mcrge   p15, 0x1, r0, c15, c0, 4        @ Rev 3 or higher
400
401 #if ARM_CLUSTER0_INIT_L2
402         /* set up L2 for cluster0 */
403         mrc     p15, 0, r0, c0, c0, 5
404         ubfx    r0, r0, #8, #4                  @ get cluster id
405         cmp     r0, #0                          @ C0 = 0, C1 = 1
406         bne     not_on_cluster0
407
408         mrc     p15, 0x1, r0, c9, c0, 0x2       @ read L2CTLR
409         bic     r0, r0, #0x7                    @ clear data ram latency field
410         orr     r0, r0, #0x2                    @ pipeline depth of 3
411         mcr     p15, 0x1, r0, c9, c0, 0x2       @ write L2CTLR
412
413 not_on_cluster0:
414 #endif
415 #endif
416
417         /* check if we should avoid switching to CLKM */
418         ldr     r0, =TZRAM_CPU_AVOID_CLKM_SWITCH
419         ldr     r0, [r0]
420         cmp     r0, #1
421         beq     avoid_clkm_switch
422
423         /* resuming from LP1 so handle 32KHz to CLKM/OSC clock switch */
424         ldr     r0, =TEGRA_CLK_RESET_BASE
425         mov     r1, #(1<<28)
426         str     r1, [r0, #CLK_RESET_SCLK_BURST]
427         str     r1, [r0, #CLK_RESET_CCLK_BURST]
428         mov     r1, #0
429         str     r1, [r0, #CLK_RESET_SCLK_DIVIDER]
430         str     r1, [r0, #CLK_RESET_CCLK_DIVIDER]
431
432 avoid_clkm_switch:
433         mov     r1, #0
434         ldr     r0, =TZRAM_CPU_AVOID_CLKM_SWITCH
435         str     r1, [r0]                @ clear the clkm switch flag
436
437 #if ARM_CPU_CORTEX_A15
438         mrc     p15, 0x1, r0, c15, c0, 3        @ L2 prefetch control reg
439         tst     r0, #0x1000
440         orreq   r0, r0, #0x1000                 @ disable throttling
441         mcreq   p15, 0x1, r0, c15, c0, 3
442
443         mrc     p15, 0, r0, c0, c0, 0           @ main ID register
444         ubfx    r1, r0, #20, #4
445         cmp     r1, #0x3                        @ Compare to rev 3
446         mrcge   p15, 0x1, r0, c15, c0, 4        @ ACTLR2
447         orrge   r0, #(1<<31)                    @ Enable regional clock gates
448         mcrge   p15, 0x1, r0, c15, c0, 4        @ Rev 3 or higher
449
450         /* This is only needed for cluster 0 with integrated L2 cache */
451         mrc     p15, 0, r0, c0, c0, 5
452         ubfx    r0, r0, #8, #4
453         tst     r0, #1
454         bne     lp1_enable_i_cache
455         mrc     p15, 0x1, r0, c9, c0, 2
456         and     r1, r0, #7
457         cmp     r1, #2
458         beq     lp1_enable_i_cache
459         bic     r0, r0, #7
460         orr     r0, r0, #2
461         mcr     p15, 0x1, r0, c9, c0, 2
462 lp1_enable_i_cache:
463         mrc     p15, 0, r0, c1, c0, 0
464         orr     r0, r0, #0x1000
465         mcr     p15, 0, r0, c1, c0, 0           @ enable i-cache
466         b       lp1_cpu_context_restore
467 #endif
468
469 #if ARM_CPU_CORTEX_A9
470         mcr     p15, 0, r0, c7, c5, 0           @ invalidate BTAC, i-cache
471         mcr     p15, 0, r0, c7, c5, 6           @ invalidate branch pred array
472         mcr     p15, 0, r0, c8, c5, 0           @ invalidate instruction TLB
473         mcr     p15, 0, r0, c8, c6, 0           @ invalidate data TLB
474         mcr     p15, 0, r0, c8, c7, 0           @ invalidate unified TLB
475         dsb
476         isb
477 #endif
478
479 #if ARM_WITH_SCU
480         cpu_id  r0
481         cmp     r0, #0
482         ldrne   r1, =TEGRA_ARM_SCU_BASE
483         movne   r0, r0, lsl #2
484         movne   r2, #0xf
485         movne   r2, r2, lsl r0
486         strne   r2, [r1, #0xC]                  @ invalidate SCU tags for CPU
487 #endif
488
489 #if ARM_CPU_CORTEX_A9
490         dsb
491         ldr     r0, =0x1000
492         mcr     p15, 0, r0, c1, c0, 0           @ enable i-cache
493         isb
494
495         /* invalidate dcache */
496         mov     r0, #0
497         mcr     p15, 2, r0, c0, c0, 0
498         mrc     p15, 1, r0, c0, c0, 0
499
500         movw    r1, #0x7fff
501         and     r2, r1, r0, lsr #13     @ get number of sets
502
503         movw    r1, #0x3ff
504
505         and     r3, r1, r0, lsr #3      @ NumWays - 1
506         add     r2, r2, #1              @ NumSets
507
508         and     r0, r0, #0x7
509         add     r0, r0, #4              @ SetShift
510
511         clz     r1, r3                  @ WayShift
512         add     r4, r3, #1              @ NumWays
513 1:      sub     r2, r2, #1              @ NumSets--
514         mov     r3, r4                  @ Temp = NumWays
515 2:      subs    r3, r3, #1              @ Temp--
516         mov     r5, r3, lsl r1
517         mov     r6, r2, lsl r0
518         orr     r5, r5, r6              @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
519         mcr     p15, 0, r5, c7, c6, 2
520         bgt     2b
521         cmp     r2, #0
522         bgt     1b
523         dsb
524         isb
525 #endif
526
527 lp1_cpu_context_restore:
528         /* restore cpu context */
529         ldr     r0, =TZRAM_CPU_CONTEXT          @ phys base of context
530         mov     r1, #0                          @ leave MMU disabled
531         cpu_restore_context r0, r1, r2, r3
532         isb
533
534         ldr     r0, =TZRAM_TEMP_SVC_SP          @ setup tempory svc sp
535         add     sp, r0, #(SCRATCH_WORDS * 4)
536
537 #if ARM_WITH_SCU
538         ldr     r3, =TEGRA_ARM_SCU_BASE         @ enable SCU
539         mov     r2, #1
540         str     r2, [r3]
541
542         ldr     r2, =0xf                        @ r/w scu regs from all cpus
543         str     r2, [r3, #0x50]
544         ldr     r2, =0xfff
545         str     r2, [r3, #0x54]
546 #endif
547         /* allow full access to CP10 and CP11 */
548         mrc     p15, 0, r0, c1, c0, 2           @ CPACR
549         orr     r0, r0, #0x00F00000
550         mcr     p15, 0, r0, c1, c0, 2           @ CPACR
551         isb
552
553         /* configure interrupts */
554         mov     r3, #0
555         mvn     r1, #0
556 interrupt_loop:
557         ldr     r2, =TEGRA_ARM_INT_DIST_BASE    @ should be better way to handle
558         add     r2, r2, #0x80                   @ add here
559         asr     r0, r3, #0x3
560         add     r3, r3, #0x20
561         cmp     r3, #0x320
562         add     r2, r0, r2
563         str     r1, [r2]
564         bne     interrupt_loop
565
566         /* restore reset vector */
567         ldr     r3, =TZRAM_BOOT_SECONDARY_CPU_ADDR
568         ldr     r2, [r3]
569         ldr     r3, =TEGRA_EXCEPTION_VECTORS_BASE
570         str     r2, [r3, #0x100]
571         dmb     sy
572         ldr     r3, [r3, #0x100]                @ dummy read to ensure write
573
574         /* mark banked CPU intrs (31:0) be non-secure */
575         ldr     r3, =TEGRA_ARM_INT_DIST_BASE
576         mvn     r2, #0x0
577         str     r2, [r3, #0x80]                 @ DIST_IRQ_SEC
578         ldr     r3, =TEGRA_ARM_INT_CPU_BASE
579         ldr     r2, =0xff
580         str     r2, [r3, #0x4]                  @ GIC_CPU_PRIMASK
581
582         /* restore monitor_vector base */
583         ldr     r3, =TZRAM_MVBAR
584         ldr     r3, [r3]
585         mcr     p15, 0, r3, c12, c0, 1
586
587         /* let normal world enable SMP, lock TLB, access CP10/11 */
588         mrc     p15, 0, r0, c1, c1, 2           @ NSACR
589         orr     r0, r0, #0x00000C00
590         orr     r0, r0, #0x00060000
591         mcr     p15, 0, r0, c1, c1, 2           @ NSACR
592
593 #if ARM_CPU_CORTEX_A9
594         /* normal world can write SMP, but not FW */
595         mrc     p15, 0, r0, c1, c0, 1
596         orr     r0, r0, #(0x1 << 0)             @ enable FW
597         mcr     p15, 0, r0, c1, c0, 1           @ ACTLR
598 #endif
599
600         /* goto monitor mode */
601         ldr     lr, =TZRAM_MON_STACK_TOP
602         ldr     r0, [lr]
603         cps     #0x16                           @ monitor mode
604         mov     sp, r0                          @ restore mon_sp
605
606         /* NS world's entry point */
607         ldr     lr, =TZRAM_NS_RESUME_ADDR
608         ldr     lr, [lr]
609         cmp     lr, #0
610         beq     .                               @ error: if NULL entry point
611
612         /* enable SCR.FW and NS=1 */
613         mrc     p15, 0, r0, c1, c1, 0           @ SCR
614         mov     r0, #(1 << 4) | 0x1             @ SCR.FW and NS=1
615         mcr     p15, 0, r0, c1, c1, 0
616         isb
617
618         /*
619          * set shadow spsr_mon, with current reset CPSR state
620          * (e.g. intrs disabled) and changed the mode to SVC
621          */
622         mrs     r0, cpsr
623         bic     r0, #0x1f
624         orr     r0, #0x13
625         msr     spsr_cxfs, r0
626         movs    pc, lr
627
628 .ltorg
629 .globl _lp1_resume_end
630 _lp1_resume_end:
631         b       .
632
633 .globl lp1_resume_addr
634 lp1_resume_addr:        .word   _lp1_resume