First version
[3rdparty/ote_partner/tlk.git] / lib / monitor / arm64 / monitor_cpu.S
1 /*
2  * Copyright (c) 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 <config.h>
24 #include <asm.h>
25 #include <arch/arm.h>
26 #include <arm64/asm.h>
27 #include <psci.h>
28
29 /* called both for cold reset and boot_secondary */
30 FUNCTION(mon_init_cpu)
31         mrs     x4, currentel
32         cmp     x4, #MODE_EL(3)
33         b.ne    .               // error, if not EL3
34
35         /* initialize SCR to secure state */
36         mov     x3, #MON_INIT_EL3_SCR
37         msr     scr_el3, x3
38         isb
39
40         /* set vbar (with phys, to catch setup errors) */
41         adr     x3, _vector_el3
42         msr     vbar_el3, x3
43
44         /* enable I/D caches, disable MMU and alignment checks */
45         mrs     x4, sctlr_el3
46         bic     x4, x4, #(1 << 25)
47         orr     x4, x4, #(1 << 12)
48         orr     x4, x4, #(1 << 2)
49         bic     x4, x4, #((1 << 1) | (1 << 0))
50         msr     sctlr_el3, x4
51
52         /* set freq for arch general timer */
53         ldr     x0, =ARM_SYSTEM_COUNTER_FREQ
54         msr     cntfrq_el0, x0
55
56         /* mark per-CPU dist GROUP0 intrs non-secure */
57         ldr     x4, =ARM_GIC_DIST_BASE
58         mov     w3, #(~0)
59         str     w3, [x4, ARM_GIC_GICD_IGROUPR0]
60
61         /* enables GROUP0/GROUP1 intrs, signals GROUP0 with FIQ */
62         ldr     x4, =ARM_GIC_CPU_BASE
63         mov     w3, #((0xF << 4) | (0x1 << 3) | (0x3 << 0))
64         str     w3, [x4, ARM_GIC_GICC_CTLR]
65
66         /* init low pri mask, so NS can set its value */
67         mov     w3, #0xFF
68         str     w3, [x4, ARM_GIC_GICC_PMR]
69
70         /* disable copro traps to EL3 */
71         msr     cptr_el3, xzr
72
73         cpuidx  x12
74
75         /* setup per-cpu monitor stack (dividing up single 4K page) */
76         msr     spsel, #1
77         ldr     x3, =monitor_stack_top
78         lsl     x4, x12, #10    // each CPU gets a 1K stack
79         sub     x3, x3, x4
80         mov     sp, x3
81
82         ret
83
84 /*
85  * Return to address saved in __mon_cpu_return_addr, which
86  * will be the BL during cold reset or address in NS world
87  * during PSCI CPU transistions.
88  */
89 FUNCTION(mon_init_return)
90         /* load per-cpu return address */
91         cpuidx  x12
92         adr     x5, __mon_cpu_return_addr
93         ldr     x3, [x5, x12, lsl #3]
94         msr     elr_el3, x3
95
96         mov     x3, #MON_INIT_EL3_SCR
97         orr     x3, x3, #1              // return NS=1
98         msr     scr_el3, x3
99         isb
100
101         /* go back non-secure in EL2 */
102         mov     x3, #MON_INIT_EL2_SPSR_AARCH64
103         msr     spsr_el3, x3
104         eret
105
106 /*
107  * Routine to setup secondary CPU state and return, leaving
108  * the primary CPU to initialize the secureos.
109  */
110 FUNCTION(boot_secondary)
111         bl      mon_init_cpu
112         bl      mon_enable_mmu
113
114         /* reload vbar with virt addr */
115         adr     x0, _vector_el3
116         msr     vbar_el3, x0
117         isb
118
119         cpuidx  x0
120         bl      platform_psci_cpu_has_reset
121         b       mon_init_return
122
123 .ltorg
124 .align 3
125 .global __mon_cpu_reset_vector
126 __mon_cpu_reset_vector:
127         b       boot_secondary
128
129 .ltorg
130 .align 3
131 .global __mon_cpu_return_addr
132 __mon_cpu_return_addr:
133         .rept MONCPUS
134         .quad 0
135         .endr