1/*
2 * Copyright (c) 2012 Linaro Limited.
3 *
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
17 */
18
19#include <linux/init.h>
20#include <linux/irqchip/arm-gic-v3.h>
21#include <linux/linkage.h>
22#include <asm/assembler.h>
23#include <asm/virt.h>
24
25#ifndef ZIMAGE
26/*
27 * For the kernel proper, we need to find out the CPU boot mode long after
28 * boot, so we need to store it in a writable variable.
29 *
30 * This is not in .bss, because we set it sufficiently early that the boot-time
31 * zeroing of .bss would clobber it.
32 */
33.data
34	.align	2
35ENTRY(__boot_cpu_mode)
36	.long	0
37.text
38
39	/*
40	 * Save the primary CPU boot mode. Requires 3 scratch registers.
41	 */
42	.macro	store_primary_cpu_mode	reg1, reg2, reg3
43	mrs	\reg1, cpsr
44	and	\reg1, \reg1, #MODE_MASK
45	adr	\reg2, .L__boot_cpu_mode_offset
46	ldr	\reg3, [\reg2]
47	str	\reg1, [\reg2, \reg3]
48	.endm
49
50	/*
51	 * Compare the current mode with the one saved on the primary CPU.
52	 * If they don't match, record that fact. The Z bit indicates
53	 * if there's a match or not.
54	 * Requires 3 additionnal scratch registers.
55	 */
56	.macro	compare_cpu_mode_with_primary mode, reg1, reg2, reg3
57	adr	\reg2, .L__boot_cpu_mode_offset
58	ldr	\reg3, [\reg2]
59	ldr	\reg1, [\reg2, \reg3]
60	cmp	\mode, \reg1		@ matches primary CPU boot mode?
61	orrne	\reg1, \reg1, #BOOT_CPU_MODE_MISMATCH
62	strne	\reg1, [\reg2, \reg3]	@ record what happened and give up
63	.endm
64
65#else	/* ZIMAGE */
66
67	.macro	store_primary_cpu_mode	reg1:req, reg2:req, reg3:req
68	.endm
69
70/*
71 * The zImage loader only runs on one CPU, so we don't bother with mult-CPU
72 * consistency checking:
73 */
74	.macro	compare_cpu_mode_with_primary mode, reg1, reg2, reg3
75	cmp	\mode, \mode
76	.endm
77
78#endif /* ZIMAGE */
79
80/*
81 * Hypervisor stub installation functions.
82 *
83 * These must be called with the MMU and D-cache off.
84 * They are not ABI compliant and are only intended to be called from the kernel
85 * entry points in head.S.
86 */
87@ Call this from the primary CPU
88ENTRY(__hyp_stub_install)
89	store_primary_cpu_mode	r4, r5, r6
90ENDPROC(__hyp_stub_install)
91
92	@ fall through...
93
94@ Secondary CPUs should call here
95ENTRY(__hyp_stub_install_secondary)
96	mrs	r4, cpsr
97	and	r4, r4, #MODE_MASK
98
99	/*
100	 * If the secondary has booted with a different mode, give up
101	 * immediately.
102	 */
103	compare_cpu_mode_with_primary	r4, r5, r6, r7
104	retne	lr
105
106	/*
107	 * Once we have given up on one CPU, we do not try to install the
108	 * stub hypervisor on the remaining ones: because the saved boot mode
109	 * is modified, it can't compare equal to the CPSR mode field any
110	 * more.
111	 *
112	 * Otherwise...
113	 */
114
115	cmp	r4, #HYP_MODE
116	retne	lr			@ give up if the CPU is not in HYP mode
117
118/*
119 * Configure HSCTLR to set correct exception endianness/instruction set
120 * state etc.
121 * Turn off all traps
122 * Eventually, CPU-specific code might be needed -- assume not for now
123 *
124 * This code relies on the "eret" instruction to synchronize the
125 * various coprocessor accesses. This is done when we switch to SVC
126 * (see safe_svcmode_maskall).
127 */
128	@ Now install the hypervisor stub:
129	W(adr)	r7, __hyp_stub_vectors
130	mcr	p15, 4, r7, c12, c0, 0	@ set hypervisor vector base (HVBAR)
131
132	@ Disable all traps, so we don't get any nasty surprise
133	mov	r7, #0
134	mcr	p15, 4, r7, c1, c1, 0	@ HCR
135	mcr	p15, 4, r7, c1, c1, 2	@ HCPTR
136	mcr	p15, 4, r7, c1, c1, 3	@ HSTR
137
138THUMB(	orr	r7, #(1 << 30)	)	@ HSCTLR.TE
139ARM_BE8(orr	r7, r7, #(1 << 25))     @ HSCTLR.EE
140	mcr	p15, 4, r7, c1, c0, 0	@ HSCTLR
141
142	mrc	p15, 4, r7, c1, c1, 1	@ HDCR
143	and	r7, #0x1f		@ Preserve HPMN
144	mcr	p15, 4, r7, c1, c1, 1	@ HDCR
145
146	@ Make sure NS-SVC is initialised appropriately
147	mrc	p15, 0, r7, c1, c0, 0	@ SCTLR
148	orr	r7, #(1 << 5)		@ CP15 barriers enabled
149	bic	r7, #(3 << 7)		@ Clear SED/ITD for v8 (RES0 for v7)
150	bic	r7, #(3 << 19)		@ WXN and UWXN disabled
151	mcr	p15, 0, r7, c1, c0, 0	@ SCTLR
152
153	mrc	p15, 0, r7, c0, c0, 0	@ MIDR
154	mcr	p15, 4, r7, c0, c0, 0	@ VPIDR
155
156	mrc	p15, 0, r7, c0, c0, 5	@ MPIDR
157	mcr	p15, 4, r7, c0, c0, 5	@ VMPIDR
158
159#if !defined(ZIMAGE) && defined(CONFIG_ARM_ARCH_TIMER)
160	@ make CNTP_* and CNTPCT accessible from PL1
161	mrc	p15, 0, r7, c0, c1, 1	@ ID_PFR1
162	lsr	r7, #16
163	and	r7, #0xf
164	cmp	r7, #1
165	bne	1f
166	mrc	p15, 4, r7, c14, c1, 0	@ CNTHCTL
167	orr	r7, r7, #3		@ PL1PCEN | PL1PCTEN
168	mcr	p15, 4, r7, c14, c1, 0	@ CNTHCTL
169	mov	r7, #0
170	mcrr	p15, 4, r7, r7, c14	@ CNTVOFF
171
172	@ Disable virtual timer in case it was counting
173	mrc	p15, 0, r7, c14, c3, 1	@ CNTV_CTL
174	bic	r7, #1			@ Clear ENABLE
175	mcr	p15, 0, r7, c14, c3, 1	@ CNTV_CTL
1761:
177#endif
178
179#ifdef CONFIG_ARM_GIC_V3
180	@ Check whether GICv3 system registers are available
181	mrc	p15, 0, r7, c0, c1, 1	@ ID_PFR1
182	ubfx	r7, r7, #28, #4
183	cmp	r7, #1
184	bne	2f
185
186	@ Enable system register accesses
187	mrc	p15, 4, r7, c12, c9, 5	@ ICC_HSRE
188	orr	r7, r7, #(ICC_SRE_EL2_ENABLE | ICC_SRE_EL2_SRE)
189	mcr	p15, 4, r7, c12, c9, 5	@ ICC_HSRE
190	isb
191
192	@ SRE bit could be forced to 0 by firmware.
193	@ Check whether it sticks before accessing any other sysreg
194	mrc	p15, 4, r7, c12, c9, 5	@ ICC_HSRE
195	tst	r7, #ICC_SRE_EL2_SRE
196	beq	2f
197	mov	r7, #0
198	mcr	p15, 4, r7, c12, c11, 0	@ ICH_HCR
1992:
200#endif
201
202	bx	lr			@ The boot CPU mode is left in r4.
203ENDPROC(__hyp_stub_install_secondary)
204
205__hyp_stub_do_trap:
206	teq	r0, #HVC_SET_VECTORS
207	bne	1f
208	mcr	p15, 4, r1, c12, c0, 0	@ set HVBAR
209	b	__hyp_stub_exit
210
2111:	teq	r0, #HVC_SOFT_RESTART
212	bne	1f
213	bx	r1
214
2151:	teq	r0, #HVC_RESET_VECTORS
216	beq	__hyp_stub_exit
217
218	ldr	r0, =HVC_STUB_ERR
219	__ERET
220
221__hyp_stub_exit:
222	mov	r0, #0
223	__ERET
224ENDPROC(__hyp_stub_do_trap)
225
226/*
227 * __hyp_set_vectors: Call this after boot to set the initial hypervisor
228 * vectors as part of hypervisor installation.  On an SMP system, this should
229 * be called on each CPU.
230 *
231 * r0 must be the physical address of the new vector table (which must lie in
232 * the bottom 4GB of physical address space.
233 *
234 * r0 must be 32-byte aligned.
235 *
236 * Before calling this, you must check that the stub hypervisor is installed
237 * everywhere, by waiting for any secondary CPUs to be brought up and then
238 * checking that BOOT_CPU_MODE_HAVE_HYP(__boot_cpu_mode) is true.
239 *
240 * If not, there is a pre-existing hypervisor, some CPUs failed to boot, or
241 * something else went wrong... in such cases, trying to install a new
242 * hypervisor is unlikely to work as desired.
243 *
244 * When you call into your shiny new hypervisor, sp_hyp will contain junk,
245 * so you will need to set that to something sensible at the new hypervisor's
246 * initialisation entry point.
247 */
248ENTRY(__hyp_set_vectors)
249	mov	r1, r0
250	mov	r0, #HVC_SET_VECTORS
251	__HVC(0)
252	ret	lr
253ENDPROC(__hyp_set_vectors)
254
255ENTRY(__hyp_soft_restart)
256	mov	r1, r0
257	mov	r0, #HVC_SOFT_RESTART
258	__HVC(0)
259	ret	lr
260ENDPROC(__hyp_soft_restart)
261
262ENTRY(__hyp_reset_vectors)
263	mov	r0, #HVC_RESET_VECTORS
264	__HVC(0)
265	ret	lr
266ENDPROC(__hyp_reset_vectors)
267
268#ifndef ZIMAGE
269.align 2
270.L__boot_cpu_mode_offset:
271	.long	__boot_cpu_mode - .
272#endif
273
274.align 5
275ENTRY(__hyp_stub_vectors)
276__hyp_stub_reset:	W(b)	.
277__hyp_stub_und:		W(b)	.
278__hyp_stub_svc:		W(b)	.
279__hyp_stub_pabort:	W(b)	.
280__hyp_stub_dabort:	W(b)	.
281__hyp_stub_trap:	W(b)	__hyp_stub_do_trap
282__hyp_stub_irq:		W(b)	.
283__hyp_stub_fiq:		W(b)	.
284ENDPROC(__hyp_stub_vectors)
285
286