1
2
3
4
5
6#include <linux/init.h>
7#include <linux/irqchip/arm-gic-v3.h>
8#include <linux/linkage.h>
9#include <asm/assembler.h>
10#include <asm/virt.h>
11
12#ifndef ZIMAGE
13
14
15
16
17
18
19
20.data
21 .align 2
22ENTRY(__boot_cpu_mode)
23 .long 0
24.text
25
26
27
28
29 .macro store_primary_cpu_mode reg1, reg2, reg3
30 mrs \reg1, cpsr
31 and \reg1, \reg1,
32 adr \reg2, .L__boot_cpu_mode_offset
33 ldr \reg3, [\reg2]
34 str \reg1, [\reg2, \reg3]
35 .endm
36
37
38
39
40
41
42
43 .macro compare_cpu_mode_with_primary mode, reg1, reg2, reg3
44 adr \reg2, .L__boot_cpu_mode_offset
45 ldr \reg3, [\reg2]
46 ldr \reg1, [\reg2, \reg3]
47 cmp \mode, \reg1 @ matches primary CPU boot mode?
48 orrne \reg1, \reg1,
49 strne \reg1, [\reg2, \reg3] @ record what happened and give up
50 .endm
51
52#else
53
54 .macro store_primary_cpu_mode reg1:req, reg2:req, reg3:req
55 .endm
56
57
58
59
60
61 .macro compare_cpu_mode_with_primary mode, reg1, reg2, reg3
62 cmp \mode, \mode
63 .endm
64
65#endif
66
67
68
69
70
71
72
73
74@ Call this from the primary CPU
75ENTRY(__hyp_stub_install)
76 store_primary_cpu_mode r4, r5, r6
77ENDPROC(__hyp_stub_install)
78
79 @ fall through...
80
81@ Secondary CPUs should call here
82ENTRY(__hyp_stub_install_secondary)
83 mrs r4, cpsr
84 and r4, r4,
85
86
87
88
89
90 compare_cpu_mode_with_primary r4, r5, r6, r7
91 retne lr
92
93
94
95
96
97
98
99
100
101
102 cmp r4,
103 retne lr @ give up if the CPU is not in HYP mode
104
105
106
107
108
109
110
111
112
113
114
115 @ Now install the hypervisor stub:
116 W(adr) r7, __hyp_stub_vectors
117 mcr p15, 4, r7, c12, c0, 0 @ set hypervisor vector base (HVBAR)
118
119 @ Disable all traps, so we don't get any nasty surprise
120 mov r7,
121 mcr p15, 4, r7, c1, c1, 0 @ HCR
122 mcr p15, 4, r7, c1, c1, 2 @ HCPTR
123 mcr p15, 4, r7, c1, c1, 3 @ HSTR
124
125THUMB( orr r7,
126ARM_BE8(orr r7, r7,
127 mcr p15, 4, r7, c1, c0, 0 @ HSCTLR
128
129 mrc p15, 4, r7, c1, c1, 1 @ HDCR
130 and r7,
131 mcr p15, 4, r7, c1, c1, 1 @ HDCR
132
133 @ Make sure NS-SVC is initialised appropriately
134 mrc p15, 0, r7, c1, c0, 0 @ SCTLR
135 orr r7,
136 bic r7,
137 bic r7,
138 mcr p15, 0, r7, c1, c0, 0 @ SCTLR
139
140 mrc p15, 0, r7, c0, c0, 0 @ MIDR
141 mcr p15, 4, r7, c0, c0, 0 @ VPIDR
142
143 mrc p15, 0, r7, c0, c0, 5 @ MPIDR
144 mcr p15, 4, r7, c0, c0, 5 @ VMPIDR
145
146
147 @ make CNTP_* and CNTPCT accessible from PL1
148 mrc p15, 0, r7, c0, c1, 1 @ ID_PFR1
149 lsr r7,
150 and r7,
151 cmp r7,
152 bne 1f
153 mrc p15, 4, r7, c14, c1, 0 @ CNTHCTL
154 orr r7, r7,
155 mcr p15, 4, r7, c14, c1, 0 @ CNTHCTL
156 mov r7,
157 mcrr p15, 4, r7, r7, c14 @ CNTVOFF
158
159 @ Disable virtual timer in case it was counting
160 mrc p15, 0, r7, c14, c3, 1 @ CNTV_CTL
161 bic r7,
162 mcr p15, 0, r7, c14, c3, 1 @ CNTV_CTL
1631:
164#endif
165
166#ifdef CONFIG_ARM_GIC_V3
167 @ Check whether GICv3 system registers are available
168 mrc p15, 0, r7, c0, c1, 1 @ ID_PFR1
169 ubfx r7, r7,
170 teq r7,
171 beq 2f
172
173 @ Enable system register accesses
174 mrc p15, 4, r7, c12, c9, 5 @ ICC_HSRE
175 orr r7, r7,
176 mcr p15, 4, r7, c12, c9, 5 @ ICC_HSRE
177 isb
178
179 @ SRE bit could be forced to 0 by firmware.
180 @ Check whether it sticks before accessing any other sysreg
181 mrc p15, 4, r7, c12, c9, 5 @ ICC_HSRE
182 tst r7,
183 beq 2f
184 mov r7,
185 mcr p15, 4, r7, c12, c11, 0 @ ICH_HCR
1862:
187#endif
188
189 bx lr @ The boot CPU mode is left in r4.
190ENDPROC(__hyp_stub_install_secondary)
191
192__hyp_stub_do_trap:
193 teq r0,
194 bne 1f
195 mcr p15, 4, r1, c12, c0, 0 @ set HVBAR
196 b __hyp_stub_exit
197
1981: teq r0,
199 bne 1f
200 bx r1
201
2021: teq r0,
203 beq __hyp_stub_exit
204
205 ldr r0, =HVC_STUB_ERR
206 __ERET
207
208__hyp_stub_exit:
209 mov r0,
210 __ERET
211ENDPROC(__hyp_stub_do_trap)
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235ENTRY(__hyp_set_vectors)
236 mov r1, r0
237 mov r0,
238 __HVC(0)
239 ret lr
240ENDPROC(__hyp_set_vectors)
241
242ENTRY(__hyp_soft_restart)
243 mov r1, r0
244 mov r0,
245 __HVC(0)
246 ret lr
247ENDPROC(__hyp_soft_restart)
248
249ENTRY(__hyp_reset_vectors)
250 mov r0,
251 __HVC(0)
252 ret lr
253ENDPROC(__hyp_reset_vectors)
254
255#ifndef ZIMAGE
256.align 2
257.L__boot_cpu_mode_offset:
258 .long __boot_cpu_mode - .
259#endif
260
261.align 5
262ENTRY(__hyp_stub_vectors)
263__hyp_stub_reset: W(b) .
264__hyp_stub_und: W(b) .
265__hyp_stub_svc: W(b) .
266__hyp_stub_pabort: W(b) .
267__hyp_stub_dabort: W(b) .
268__hyp_stub_trap: W(b) __hyp_stub_do_trap
269__hyp_stub_irq: W(b) .
270__hyp_stub_fiq: W(b) .
271ENDPROC(__hyp_stub_vectors)
272
273