1/*
2 * (C) Copyright 2014-2015 Freescale Semiconductor
3 *
4 * SPDX-License-Identifier:	GPL-2.0+
5 *
6 * Extracted from armv8/start.S
7 */
8
9#include <config.h>
10#include <linux/linkage.h>
11#include <asm/gic.h>
12#include <asm/macro.h>
13#ifdef CONFIG_MP
14#include <asm/arch/mp.h>
15#endif
16
17ENTRY(lowlevel_init)
18	mov	x29, lr			/* Save LR */
19
20#ifdef CONFIG_FSL_LSCH3
21
22	/* Set Wuo bit for RN-I 20 */
23#ifdef CONFIG_LS2080A
24	ldr	x0, =CCI_AUX_CONTROL_BASE(20)
25	ldr	x1, =0x00000010
26	bl	ccn504_set_aux
27#endif
28
29	/* Add fully-coherent masters to DVM domain */
30	ldr	x0, =CCI_MN_BASE
31	ldr	x1, =CCI_MN_RNF_NODEID_LIST
32	ldr	x2, =CCI_MN_DVM_DOMAIN_CTL_SET
33	bl	ccn504_add_masters_to_dvm
34
35	/* Set all RN-I ports to QoS of 15 */
36	ldr	x0, =CCI_S0_QOS_CONTROL_BASE(0)
37	ldr	x1, =0x00FF000C
38	bl	ccn504_set_qos
39	ldr	x0, =CCI_S1_QOS_CONTROL_BASE(0)
40	ldr	x1, =0x00FF000C
41	bl	ccn504_set_qos
42	ldr	x0, =CCI_S2_QOS_CONTROL_BASE(0)
43	ldr	x1, =0x00FF000C
44	bl	ccn504_set_qos
45
46	ldr	x0, =CCI_S0_QOS_CONTROL_BASE(2)
47	ldr	x1, =0x00FF000C
48	bl	ccn504_set_qos
49	ldr	x0, =CCI_S1_QOS_CONTROL_BASE(2)
50	ldr	x1, =0x00FF000C
51	bl	ccn504_set_qos
52	ldr	x0, =CCI_S2_QOS_CONTROL_BASE(2)
53	ldr	x1, =0x00FF000C
54	bl	ccn504_set_qos
55
56	ldr	x0, =CCI_S0_QOS_CONTROL_BASE(6)
57	ldr	x1, =0x00FF000C
58	bl	ccn504_set_qos
59	ldr	x0, =CCI_S1_QOS_CONTROL_BASE(6)
60	ldr	x1, =0x00FF000C
61	bl	ccn504_set_qos
62	ldr	x0, =CCI_S2_QOS_CONTROL_BASE(6)
63	ldr	x1, =0x00FF000C
64	bl	ccn504_set_qos
65
66	ldr	x0, =CCI_S0_QOS_CONTROL_BASE(12)
67	ldr	x1, =0x00FF000C
68	bl	ccn504_set_qos
69	ldr	x0, =CCI_S1_QOS_CONTROL_BASE(12)
70	ldr	x1, =0x00FF000C
71	bl	ccn504_set_qos
72	ldr	x0, =CCI_S2_QOS_CONTROL_BASE(12)
73	ldr	x1, =0x00FF000C
74	bl	ccn504_set_qos
75
76	ldr	x0, =CCI_S0_QOS_CONTROL_BASE(16)
77	ldr	x1, =0x00FF000C
78	bl	ccn504_set_qos
79	ldr	x0, =CCI_S1_QOS_CONTROL_BASE(16)
80	ldr	x1, =0x00FF000C
81	bl	ccn504_set_qos
82	ldr	x0, =CCI_S2_QOS_CONTROL_BASE(16)
83	ldr	x1, =0x00FF000C
84	bl	ccn504_set_qos
85
86	ldr	x0, =CCI_S0_QOS_CONTROL_BASE(20)
87	ldr	x1, =0x00FF000C
88	bl	ccn504_set_qos
89	ldr	x0, =CCI_S1_QOS_CONTROL_BASE(20)
90	ldr	x1, =0x00FF000C
91	bl	ccn504_set_qos
92	ldr	x0, =CCI_S2_QOS_CONTROL_BASE(20)
93	ldr	x1, =0x00FF000C
94	bl	ccn504_set_qos
95#endif
96
97	/* Set the SMMU page size in the sACR register */
98	ldr	x1, =SMMU_BASE
99	ldr	w0, [x1, #0x10]
100	orr	w0, w0, #1 << 16  /* set sACR.pagesize to indicate 64K page */
101	str	w0, [x1, #0x10]
102
103	/* Initialize GIC Secure Bank Status */
104#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
105	branch_if_slave x0, 1f
106	ldr	x0, =GICD_BASE
107	bl	gic_init_secure
1081:
109#ifdef CONFIG_GICV3
110	ldr	x0, =GICR_BASE
111	bl	gic_init_secure_percpu
112#elif defined(CONFIG_GICV2)
113	ldr	x0, =GICD_BASE
114	ldr	x1, =GICC_BASE
115	bl	gic_init_secure_percpu
116#endif
117#endif
118
119	branch_if_master x0, x1, 2f
120
121#if defined(CONFIG_MP) && defined(CONFIG_ARMV8_MULTIENTRY)
122	ldr	x0, =secondary_boot_func
123	blr	x0
124#endif
125
1262:
127#ifdef CONFIG_FSL_TZPC_BP147
128	/* Set Non Secure access for all devices protected via TZPC */
129	ldr	x1, =TZPCDECPROT_0_SET_BASE /* Decode Protection-0 Set Reg */
130	orr	w0, w0, #1 << 3 /* DCFG_RESET is accessible from NS world */
131	str	w0, [x1]
132
133	isb
134	dsb	sy
135#endif
136
137#ifdef CONFIG_FSL_TZASC_400
138	/* Set TZASC so that:
139	 * a. We use only Region0 whose global secure write/read is EN
140	 * b. We use only Region0 whose NSAID write/read is EN
141	 *
142	 * NOTE: As per the CCSR map doc, TZASC 3 and TZASC 4 are just
143	 * 	 placeholders.
144	 */
145	ldr	x1, =TZASC_GATE_KEEPER(0)
146	ldr	x0, [x1]		/* Filter 0 Gate Keeper Register */
147	orr	x0, x0, #1 << 0		/* Set open_request for Filter 0 */
148	str	x0, [x1]
149
150	ldr	x1, =TZASC_GATE_KEEPER(1)
151	ldr	x0, [x1]		/* Filter 0 Gate Keeper Register */
152	orr	x0, x0, #1 << 0		/* Set open_request for Filter 0 */
153	str	x0, [x1]
154
155	ldr	x1, =TZASC_REGION_ATTRIBUTES_0(0)
156	ldr	x0, [x1]		/* Region-0 Attributes Register */
157	orr	x0, x0, #1 << 31	/* Set Sec global write en, Bit[31] */
158	orr	x0, x0, #1 << 30	/* Set Sec global read en, Bit[30] */
159	str	x0, [x1]
160
161	ldr	x1, =TZASC_REGION_ATTRIBUTES_0(1)
162	ldr	x0, [x1]		/* Region-1 Attributes Register */
163	orr	x0, x0, #1 << 31	/* Set Sec global write en, Bit[31] */
164	orr	x0, x0, #1 << 30	/* Set Sec global read en, Bit[30] */
165	str	x0, [x1]
166
167	ldr	x1, =TZASC_REGION_ID_ACCESS_0(0)
168	ldr	w0, [x1]		/* Region-0 Access Register */
169	mov	w0, #0xFFFFFFFF		/* Set nsaid_wr_en and nsaid_rd_en */
170	str	w0, [x1]
171
172	ldr	x1, =TZASC_REGION_ID_ACCESS_0(1)
173	ldr	w0, [x1]		/* Region-1 Attributes Register */
174	mov	w0, #0xFFFFFFFF		/* Set nsaid_wr_en and nsaid_rd_en */
175	str	w0, [x1]
176
177	isb
178	dsb	sy
179#endif
180	mov	lr, x29			/* Restore LR */
181	ret
182ENDPROC(lowlevel_init)
183
184hnf_pstate_poll:
185	/* x0 has the desired status, return 0 for success, 1 for timeout
186	 * clobber x1, x2, x3, x4, x6, x7
187	 */
188	mov	x1, x0
189	mov	x7, #0			/* flag for timeout */
190	mrs	x3, cntpct_el0		/* read timer */
191	add	x3, x3, #1200		/* timeout after 100 microseconds */
192	mov	x0, #0x18
193	movk	x0, #0x420, lsl #16	/* HNF0_PSTATE_STATUS */
194	mov	w6, #8			/* HN-F node count */
1951:
196	ldr	x2, [x0]
197	cmp	x2, x1			/* check status */
198	b.eq	2f
199	mrs	x4, cntpct_el0
200	cmp	x4, x3
201	b.ls	1b
202	mov	x7, #1			/* timeout */
203	b	3f
2042:
205	add	x0, x0, #0x10000	/* move to next node */
206	subs	w6, w6, #1
207	cbnz	w6, 1b
2083:
209	mov	x0, x7
210	ret
211
212hnf_set_pstate:
213	/* x0 has the desired state, clobber x1, x2, x6 */
214	mov	x1, x0
215	/* power state to SFONLY */
216	mov	w6, #8			/* HN-F node count */
217	mov	x0, #0x10
218	movk	x0, #0x420, lsl #16	/* HNF0_PSTATE_REQ */
2191:	/* set pstate to sfonly */
220	ldr	x2, [x0]
221	and	x2, x2, #0xfffffffffffffffc	/* & HNFPSTAT_MASK */
222	orr	x2, x2, x1
223	str	x2, [x0]
224	add	x0, x0, #0x10000	/* move to next node */
225	subs	w6, w6, #1
226	cbnz	w6, 1b
227
228	ret
229
230ENTRY(__asm_flush_l3_cache)
231	/*
232	 * Return status in x0
233	 *    success 0
234	 *    tmeout 1 for setting SFONLY, 2 for FAM, 3 for both
235	 */
236	mov	x29, lr
237	mov	x8, #0
238
239	dsb	sy
240	mov	x0, #0x1		/* HNFPSTAT_SFONLY */
241	bl	hnf_set_pstate
242
243	mov	x0, #0x4		/* SFONLY status */
244	bl	hnf_pstate_poll
245	cbz	x0, 1f
246	mov	x8, #1			/* timeout */
2471:
248	dsb	sy
249	mov	x0, #0x3		/* HNFPSTAT_FAM */
250	bl	hnf_set_pstate
251
252	mov	x0, #0xc		/* FAM status */
253	bl	hnf_pstate_poll
254	cbz	x0, 1f
255	add	x8, x8, #0x2
2561:
257	mov	x0, x8
258	mov	lr, x29
259	ret
260ENDPROC(__asm_flush_l3_cache)
261
262#ifdef CONFIG_MP
263	/* Keep literals not used by the secondary boot code outside it */
264	.ltorg
265
266	/* Using 64 bit alignment since the spin table is accessed as data */
267	.align 4
268	.global secondary_boot_code
269	/* Secondary Boot Code starts here */
270secondary_boot_code:
271	.global __spin_table
272__spin_table:
273	.space CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_SIZE
274
275	.align 2
276ENTRY(secondary_boot_func)
277	/*
278	 * MPIDR_EL1 Fields:
279	 * MPIDR[1:0] = AFF0_CPUID <- Core ID (0,1)
280	 * MPIDR[7:2] = AFF0_RES
281	 * MPIDR[15:8] = AFF1_CLUSTERID <- Cluster ID (0,1,2,3)
282	 * MPIDR[23:16] = AFF2_CLUSTERID
283	 * MPIDR[24] = MT
284	 * MPIDR[29:25] = RES0
285	 * MPIDR[30] = U
286	 * MPIDR[31] = ME
287	 * MPIDR[39:32] = AFF3
288	 *
289	 * Linear Processor ID (LPID) calculation from MPIDR_EL1:
290	 * (We only use AFF0_CPUID and AFF1_CLUSTERID for now
291	 * until AFF2_CLUSTERID and AFF3 have non-zero values)
292	 *
293	 * LPID = MPIDR[15:8] | MPIDR[1:0]
294	 */
295	mrs	x0, mpidr_el1
296	ubfm	x1, x0, #8, #15
297	ubfm	x2, x0, #0, #1
298	orr	x10, x2, x1, lsl #2	/* x10 has LPID */
299	ubfm    x9, x0, #0, #15         /* x9 contains MPIDR[15:0] */
300	/*
301	 * offset of the spin table element for this core from start of spin
302	 * table (each elem is padded to 64 bytes)
303	 */
304	lsl	x1, x10, #6
305	ldr	x0, =__spin_table
306	/* physical address of this cpus spin table element */
307	add	x11, x1, x0
308
309	ldr	x0, =__real_cntfrq
310	ldr	x0, [x0]
311	msr	cntfrq_el0, x0	/* set with real frequency */
312	str	x9, [x11, #16]	/* LPID */
313	mov	x4, #1
314	str	x4, [x11, #8]	/* STATUS */
315	dsb	sy
316#if defined(CONFIG_GICV3)
317	gic_wait_for_interrupt_m x0
318#elif defined(CONFIG_GICV2)
319        ldr     x0, =GICC_BASE
320        gic_wait_for_interrupt_m x0, w1
321#endif
322
323	bl secondary_switch_to_el2
324#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
325	bl secondary_switch_to_el1
326#endif
327
328slave_cpu:
329	wfe
330	ldr	x0, [x11]
331	cbz	x0, slave_cpu
332#ifndef CONFIG_ARMV8_SWITCH_TO_EL1
333	mrs     x1, sctlr_el2
334#else
335	mrs     x1, sctlr_el1
336#endif
337	tbz     x1, #25, cpu_is_le
338	rev     x0, x0                  /* BE to LE conversion */
339cpu_is_le:
340	br	x0			/* branch to the given address */
341ENDPROC(secondary_boot_func)
342
343ENTRY(secondary_switch_to_el2)
344	switch_el x0, 1f, 0f, 0f
3450:	ret
3461:	armv8_switch_to_el2_m x0
347ENDPROC(secondary_switch_to_el2)
348
349ENTRY(secondary_switch_to_el1)
350	switch_el x0, 0f, 1f, 0f
3510:	ret
3521:	armv8_switch_to_el1_m x0, x1
353ENDPROC(secondary_switch_to_el1)
354
355	/* Ensure that the literals used by the secondary boot code are
356	 * assembled within it (this is required so that we can protect
357	 * this area with a single memreserve region
358	 */
359	.ltorg
360
361	/* 64 bit alignment for elements accessed as data */
362	.align 4
363	.global __real_cntfrq
364__real_cntfrq:
365	.quad COUNTER_FREQUENCY
366	.globl __secondary_boot_code_size
367	.type __secondary_boot_code_size, %object
368	/* Secondary Boot Code ends here */
369__secondary_boot_code_size:
370	.quad .-secondary_boot_code
371#endif
372