354726165d68acbb83b8af4277a0c11d01dd9878
[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 #include <arm64/monitor_macros.h>
29
30 /* called both for cold reset and boot_secondary */
31 FUNCTION(mon_init_cpu)
32         mrs     x4, currentel
33         cmp     x4, #MODE_EL(3)
34         b.ne    .               // error, if not EL3
35
36         /* initialize SCR to secure state */
37         mov     x3, #(MON_SCR_RESV1 | MON_SCR_64BIT)
38         msr     scr_el3, x3
39         isb
40
41         /* set vbar (with phys, to catch setup errors) */
42         adr     x3, _vector_el3
43         msr     vbar_el3, x3
44
45         /* enable I/D caches, disable MMU and alignment checks */
46         mrs     x4, sctlr_el3
47         bic     x4, x4, #(1 << 25)
48         orr     x4, x4, #(1 << 12)
49         orr     x4, x4, #(1 << 2)
50         bic     x4, x4, #((1 << 1) | (1 << 0))
51         msr     sctlr_el3, x4
52
53         /* set freq for arch general timer */
54         ldr     x0, =ARM_SYSTEM_COUNTER_FREQ
55         msr     cntfrq_el0, x0
56
57         /* enable the cycle count register */
58         mrs     x0, pmcr_el0
59         ubfx    x0, x0, #11, #5         // read PMCR.N field
60         mov     x4, #1
61         lsl     x0, x4, x0
62         sub     x0, x0, #1              // mask of event counters
63         orr     x0, x0, #0x80000000     // disable overflow intrs
64         msr     pmintenclr_el1, x0
65         msr     pmuserenr_el0, x4       // enable user mode access
66
67         /* mark per-CPU dist GROUP0 intrs non-secure */
68         ldr     x4, =ARM_GIC_DIST_BASE
69         mov     w3, #(~0)
70         str     w3, [x4, ARM_GIC_GICD_IGROUPR0]
71
72         /* enables GROUP0/GROUP1 intrs, signals GROUP0 with FIQ */
73         ldr     x4, =ARM_GIC_CPU_BASE
74         mov     w3, #((0xF << 4) | (0x1 << 3) | (0x3 << 0))
75         str     w3, [x4, ARM_GIC_GICC_CTLR]
76
77         /* init low pri mask, so NS can set its value */
78         mov     w3, #0xFF
79         str     w3, [x4, ARM_GIC_GICC_PMR]
80
81         /* disable copro traps to EL3 */
82         msr     cptr_el3, xzr
83
84         cpuidx  x12
85
86         /* setup per-cpu monitor stack (dividing up single 4K page) */
87         msr     spsel, #1
88         ldr     x3, =monitor_stack_top
89         lsl     x4, x12, #10    // each CPU gets a 1K stack
90         sub     x3, x3, x4
91         mov     sp, x3
92
93         ret
94
95 /*
96  * Return to address saved in __mon_cpu_return_addr, in
97  * AARCH32 SVC (non-secure) mode.
98  */
99 FUNCTION(mon_return_aarch32_ns)
100         /* load return address */
101         cpuidx  x1
102         adr     x2, __mon_cpu_return_addr
103         ldr     x2, [x2, x1, lsl #3]
104
105         msr     elr_el3, x2
106         mov     x2, #(MON_SCR_RESV1 | MON_SCR_32BIT | MON_SCR_NS_MODE)
107         msr     scr_el3, x2
108         mov     x2, #(MON_SPSR_EXC_MASKED | MODE_SVC)
109         msr     spsr_el3, x2
110
111         eret
112
113 /*
114  * Return to address saved in __mon_cpu_return_addr, in
115  * AARCH64 EL2 (non-secure) mode.
116  */
117 FUNCTION(mon_return_aarch64_ns)
118         /* load return address */
119         cpuidx  x1
120         adr     x2, __mon_cpu_return_addr
121         ldr     x2, [x2, x1, lsl #3]
122
123         msr     elr_el3, x2
124         mov     x2, #(MON_SCR_RESV1 | MON_SCR_64BIT | MON_SCR_NS_MODE)
125         msr     scr_el3, x2
126         mov     x2, #(MON_SPSR_EXC_MASKED | MODE_EL(2))
127         msr     spsr_el3, x2
128
129         eret
130
131 /*
132  * Routine to setup secondary CPU state and return, leaving
133  * the primary CPU to initialize the secureos.
134  */
135 FUNCTION(boot_secondary)
136         bl      mon_init_cpu
137         bl      mon_enable_mmu
138
139         /* reload vbar with virt addr */
140         adr     x0, _vector_el3
141         msr     vbar_el3, x0
142         isb
143
144         cpuidx  x0
145         bl      platform_psci_cpu_has_reset
146         b       mon_return_aarch64_ns
147
148 /* get the CPU ID */
149 FUNCTION(mon_get_cpu_id)
150         mrs     x0, midr_el1
151         ubfx    x0, x0, #4, #12
152         ret
153
154 .ltorg
155 .align 3
156 .global __mon_cpu_reset_vector
157 __mon_cpu_reset_vector:
158         b       boot_secondary
159
160 .ltorg
161 .align 3
162 .global __mon_cpu_return_addr
163 __mon_cpu_return_addr:
164         .rept MONCPUS
165         .quad 0
166         .endr
167
168 .ltorg
169 .align 3
170 .global __mon_cpu_return_mode
171 __mon_cpu_return_mode:
172         .rept MONCPUS
173         .quad 0
174         .endr