tlk: 5/22 update
[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         /* mark per-CPU dist GROUP0 intrs non-secure */
58         ldr     x4, =ARM_GIC_DIST_BASE
59         mov     w3, #(~0)
60         str     w3, [x4, ARM_GIC_GICD_IGROUPR0]
61
62         /* enables GROUP0/GROUP1 intrs, signals GROUP0 with FIQ */
63         ldr     x4, =ARM_GIC_CPU_BASE
64         mov     w3, #((0xF << 4) | (0x1 << 3) | (0x3 << 0))
65         str     w3, [x4, ARM_GIC_GICC_CTLR]
66
67         /* init low pri mask, so NS can set its value */
68         mov     w3, #0xFF
69         str     w3, [x4, ARM_GIC_GICC_PMR]
70
71         /* disable copro traps to EL3 */
72         msr     cptr_el3, xzr
73
74         cpuidx  x12
75
76         /* setup per-cpu monitor stack (dividing up single 4K page) */
77         msr     spsel, #1
78         ldr     x3, =monitor_stack_top
79         lsl     x4, x12, #10    // each CPU gets a 1K stack
80         sub     x3, x3, x4
81         mov     sp, x3
82
83         ret
84
85 /*
86  * Return to address saved in __mon_cpu_return_addr, in
87  * AARCH32 SVC (non-secure) mode.
88  */
89 FUNCTION(mon_return_aarch32_ns)
90         /* load return address */
91         cpuidx  x1
92         adr     x2, __mon_cpu_return_addr
93         ldr     x2, [x2, x1, lsl #3]
94
95         msr     elr_el3, x2
96         mov     x2, #(MON_SCR_RESV1 | MON_SCR_32BIT | MON_SCR_NS_MODE)
97         msr     scr_el3, x2
98         mov     x2, #(MON_SPSR_EXC_MASKED | MODE_SVC)
99         msr     spsr_el3, x2
100
101         eret
102
103 /*
104  * Return to address saved in __mon_cpu_return_addr, in
105  * AARCH64 EL2 (non-secure) mode.
106  */
107 FUNCTION(mon_return_aarch64_ns)
108         /* load return address */
109         cpuidx  x1
110         adr     x2, __mon_cpu_return_addr
111         ldr     x2, [x2, x1, lsl #3]
112
113         msr     elr_el3, x2
114         mov     x2, #(MON_SCR_RESV1 | MON_SCR_64BIT | MON_SCR_NS_MODE)
115         msr     scr_el3, x2
116         mov     x2, #(MON_SPSR_EXC_MASKED | MODE_EL(2))
117         msr     spsr_el3, x2
118
119         eret
120
121 /*
122  * Routine to setup secondary CPU state and return, leaving
123  * the primary CPU to initialize the secureos.
124  */
125 FUNCTION(boot_secondary)
126         bl      mon_init_cpu
127         bl      mon_enable_mmu
128
129         /* reload vbar with virt addr */
130         adr     x0, _vector_el3
131         msr     vbar_el3, x0
132         isb
133
134         cpuidx  x0
135         bl      platform_psci_cpu_has_reset
136         b       mon_return_aarch64_ns
137
138 /* get the CPU ID */
139 FUNCTION(mon_get_cpu_id)
140         mrs     x0, midr_el1
141         ubfx    x0, x0, #4, #12
142         ret
143
144 .ltorg
145 .align 3
146 .global __mon_cpu_reset_vector
147 __mon_cpu_reset_vector:
148         b       boot_secondary
149
150 .ltorg
151 .align 3
152 .global __mon_cpu_return_addr
153 __mon_cpu_return_addr:
154         .rept MONCPUS
155         .quad 0
156         .endr
157
158 .ltorg
159 .align 3
160 .global __mon_cpu_return_mode
161 __mon_cpu_return_mode:
162         .rept MONCPUS
163         .quad 0
164         .endr