1/*
2 * Copyright 2018-2020 NXP
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7
8#include <asm_macros.S>
9
10#include "bl31_data.h"
11#include "plat_psci.h"
12#include "platform_def.h"
13
14.global _getCoreData
15.global _setCoreData
16.global _getCoreState
17.global _setCoreState
18.global _init_global_data
19.global _get_global_data
20.global _set_global_data
21.global _initialize_psci
22.global _init_task_flags
23.global _set_task1_start
24.global _set_task1_done
25
26
27/* Function returns the specified data field value from the specified cpu
28 * core data area
29 * in:  x0 = core mask lsb
30 *	x1 = data field name/offset
31 * out: x0 = data value
32 * uses x0, x1, x2, [x13, x14, x15]
33 */
34func _getCoreData
35
36	/* generate a 0-based core number from the input mask */
37	clz   x2, x0
38	mov   x0, #63
39	sub   x0, x0, x2
40
41	/* x0 = core number (0-based) */
42	/* x1 = field offset */
43
44	/* determine if this is bootcore or secondary core */
45	cbnz  x0, 1f
46
47	/* get base address for bootcore data */
48	ldr  x2, =BC_PSCI_BASE
49	add  x2, x2, x1
50	b	2f
51
521:	/* get base address for secondary core data */
53
54	/* x0 = core number (0-based) */
55	/* x1 = field offset */
56
57	/* generate number of regions to offset */
58	mov   x2, #SEC_REGION_SIZE
59	mul   x2, x2, x0
60
61	/* x1 = field offset */
62	/* x2 = region offset */
63
64	/* generate the total offset to data element */
65	sub   x1, x2, x1
66
67	/* x1 = total offset to data element */
68
69	/* get the base address */
70	ldr   x2, =SECONDARY_TOP
71
72	/* apply offset to base addr */
73	sub   x2, x2, x1
742:
75	/* x2 = data element address */
76
77	dc   ivac, x2
78	dsb  sy
79	isb
80	/* read data */
81	ldr  x0, [x2]
82
83	ret
84endfunc _getCoreData
85
86
87/* Function returns the SoC-specific state of the specified cpu
88 * in:  x0 = core mask lsb
89 * out: x0 = data value
90 * uses x0, x1, x2, [x13, x14, x15]
91 */
92func _getCoreState
93
94	mov   x1, #CORE_STATE_DATA
95
96	/* generate a 0-based core number from the input mask */
97	clz   x2, x0
98	mov   x0, #63
99	sub   x0, x0, x2
100
101	/* x0 = core number (0-based) */
102	/* x1 = field offset */
103
104	/* determine if this is bootcore or secondary core */
105	cbnz  x0, 1f
106
107	/* get base address for bootcore data */
108	ldr  x2, =BC_PSCI_BASE
109	add  x2, x2, x1
110	b	2f
111
1121:	/* get base address for secondary core data */
113
114	/* x0 = core number (0-based) */
115	/* x1 = field offset */
116
117	/* generate number of regions to offset */
118	mov   x2, #SEC_REGION_SIZE
119	mul   x2, x2, x0
120
121	/* x1 = field offset */
122	/* x2 = region offset */
123
124	/* generate the total offset to data element */
125	sub   x1, x2, x1
126
127	/* x1 = total offset to data element */
128
129	/* get the base address */
130	ldr   x2, =SECONDARY_TOP
131
132	/* apply offset to base addr */
133	sub   x2, x2, x1
1342:
135	/* x2 = data element address */
136
137	dc   ivac, x2
138	dsb  sy
139	isb
140
141	/* read data */
142	ldr  x0, [x2]
143
144	ret
145endfunc _getCoreState
146
147
148/* Function writes the specified data value into the specified cpu
149 * core data area
150 * in:  x0 = core mask lsb
151 *	  x1 = data field offset
152 *	  x2 = data value to write/store
153 * out: none
154 * uses x0, x1, x2, x3, [x13, x14, x15]
155 */
156func _setCoreData
157	/* x0 = core mask */
158	/* x1 = field offset */
159	/* x2 = data value */
160
161	clz   x3, x0
162	mov   x0, #63
163	sub   x0, x0, x3
164
165	/* x0 = core number (0-based) */
166	/* x1 = field offset */
167	/* x2 = data value */
168
169	/* determine if this is bootcore or secondary core */
170	cbnz  x0, 1f
171
172	/* get base address for bootcore data */
173	ldr  x3, =BC_PSCI_BASE
174	add  x3, x3, x1
175	b	2f
176
1771:	/* get base address for secondary core data */
178
179	/* x0 = core number (0-based) */
180	/* x1 = field offset */
181	/* x2 = data value */
182
183	/* generate number of regions to offset */
184	mov   x3, #SEC_REGION_SIZE
185	mul   x3, x3, x0
186
187	/* x1 = field offset */
188	/* x2 = data value */
189	/* x3 = region offset */
190
191	/* generate the total offset to data element */
192	sub   x1, x3, x1
193
194	/* x1 = total offset to data element */
195	/* x2 = data value */
196
197	ldr   x3, =SECONDARY_TOP
198
199	/* apply offset to base addr */
200	sub   x3, x3, x1
201
2022:
203	/* x2 = data value */
204	/* x3 = data element address */
205
206	str   x2, [x3]
207
208	dc	cvac, x3
209	dsb   sy
210	isb
211	ret
212endfunc _setCoreData
213
214
215/* Function stores the specified core state
216 * in:  x0 = core mask lsb
217 *	x1 = data value to write/store
218 * out: none
219 * uses x0, x1, x2, x3, [x13, x14, x15]
220 */
221func _setCoreState
222	mov  x2, #CORE_STATE_DATA
223
224	clz   x3, x0
225	mov   x0, #63
226	sub   x0, x0, x3
227
228	/* x0 = core number (0-based) */
229	/* x1 = data value */
230	/* x2 = field offset */
231
232	/* determine if this is bootcore or secondary core */
233	cbnz  x0, 1f
234
235	/* get base address for bootcore data */
236	ldr  x3, =BC_PSCI_BASE
237	add  x3, x3, x2
238	b	2f
239
2401:	/* get base address for secondary core data */
241
242	/* x0 = core number (0-based) */
243	/* x1 = data value */
244	/* x2 = field offset */
245
246	/* generate number of regions to offset */
247	mov   x3, #SEC_REGION_SIZE
248	mul   x3, x3, x0
249
250	/* x1 = data value */
251	/* x2 = field offset */
252	/* x3 = region offset */
253
254	/* generate the total offset to data element */
255	sub   x2, x3, x2
256
257	/* x1 = data value */
258	/* x2 = total offset to data element */
259
260	ldr   x3, =SECONDARY_TOP
261
262	/* apply offset to base addr */
263	sub   x3, x3, x2
264
2652:
266	/* x1 = data value */
267	/* x3 = data element address */
268
269	str   x1, [x3]
270
271	dc	civac, x3
272	dsb   sy
273	isb
274	ret
275endfunc _setCoreState
276
277
278/* Function sets the task1 start
279 * in:  w0 = value to set flag to
280 * out: none
281 * uses x0, x1
282 */
283func _set_task1_start
284
285	ldr  x1, =SMC_TASK1_BASE
286
287	add  x1, x1, #TSK_START_OFFSET
288	str  w0, [x1]
289	dc   cvac, x1
290	dsb  sy
291	isb
292	ret
293endfunc _set_task1_start
294
295
296/* Function sets the state of the task 1 done flag
297 * in:  w0 = value to set flag to
298 * out: none
299 * uses x0, x1
300 */
301func _set_task1_done
302
303	ldr  x1, =SMC_TASK1_BASE
304
305	add  x1, x1, #TSK_DONE_OFFSET
306	str  w0, [x1]
307	dc   cvac, x1
308	dsb  sy
309	isb
310	ret
311endfunc _set_task1_done
312
313
314/* Function initializes the smc global data entries
315 * Note: the constant LAST_SMC_GLBL_OFFSET must reference the last entry in the
316 *	   smc global region
317 * in:  none
318 * out: none
319 * uses x0, x1, x2
320 */
321func _init_global_data
322
323	ldr  x1, =SMC_GLBL_BASE
324
325	/* x1 = SMC_GLBL_BASE */
326
327	mov x2, #LAST_SMC_GLBL_OFFSET
328	add x2, x2, x1
3291:
330	str  xzr, [x1]
331	dc   cvac, x1
332	cmp  x2, x1
333	add  x1, x1, #8
334	b.hi 1b
335
336	dsb  sy
337	isb
338	ret
339endfunc _init_global_data
340
341
342/* Function gets the value of the specified global data element
343 * in:  x0 = offset of data element
344 * out: x0 = requested data element
345 * uses x0, x1
346 */
347func _get_global_data
348
349	ldr  x1, =SMC_GLBL_BASE
350	add  x1, x1, x0
351	dc   ivac, x1
352	isb
353
354	ldr  x0, [x1]
355	ret
356endfunc _get_global_data
357
358
359/* Function sets the value of the specified global data element
360 * in:  x0 = offset of data element
361 *	  x1 = value to write
362 * out: none
363 * uses x0, x1, x2
364 */
365func _set_global_data
366
367	ldr  x2, =SMC_GLBL_BASE
368	add  x0, x0, x2
369	str  x1, [x0]
370	dc   cvac, x0
371
372	dsb  sy
373	isb
374	ret
375endfunc _set_global_data
376
377
378/* Function initializes the core data areas
379 * only executed by the boot core
380 * in:   none
381 * out:  none
382 * uses: x0, x1, x2, x3, x4, x5, x6, x7, [x13, x14, x15]
383 */
384func _initialize_psci
385	mov   x7, x30
386
387	/* initialize the bootcore psci data */
388	ldr   x5, =BC_PSCI_BASE
389	mov   x6, #CORE_RELEASED
390
391	str   x6,  [x5], #8
392	dc cvac, x5
393	str   xzr, [x5], #8
394	dc cvac, x5
395	str   xzr, [x5], #8
396	dc cvac, x5
397	str   xzr, [x5], #8
398	dc cvac, x5
399	str   xzr, [x5], #8
400	dc cvac, x5
401	str   xzr, [x5], #8
402	dc cvac, x5
403	str   xzr, [x5], #8
404	dc cvac, x5
405	str   xzr, [x5], #8
406	dc cvac, x5
407	str   xzr, [x5], #8
408	dc cvac, x5
409	str   xzr, [x5], #8
410	dc cvac, x5
411	str   xzr, [x5], #8
412	dc cvac, x5
413	str   xzr, [x5], #8
414	dc cvac, x5
415	str   xzr, [x5], #8
416	dc cvac, x5
417	str   xzr, [x5], #8
418	dc cvac, x5
419	str   xzr, [x5], #8
420	dc cvac, x5
421	str   xzr, [x5]
422	dc cvac, x5
423	dsb sy
424	isb
425
426	/* see if we have any secondary cores */
427	mov   x4, #PLATFORM_CORE_COUNT
428	sub   x4, x4, #1
429	cbz   x4, 3f
430
431	/* initialize the secondary core's psci data */
432	ldr  x5, =SECONDARY_TOP
433	/* core mask lsb for core 1 */
434	mov  x3, #2
435	sub  x5, x5, #SEC_REGION_SIZE
436
437	/* x3 = core1 mask lsb */
438	/* x4 = number of secondary cores */
439	/* x5 = core1 psci data base address */
4402:
441	/* set core state in x6 */
442	mov  x0, x3
443	mov  x6, #CORE_IN_RESET
444	bl   _soc_ck_disabled
445	cbz  x0, 1f
446	mov  x6, #CORE_DISABLED
4471:
448	add   x2, x5, #CORE_STATE_DATA
449	str   x6,  [x2]
450	dc cvac, x2
451	add   x2, x5, #SPSR_EL3_DATA
452	str   xzr, [x2]
453	dc cvac, x2
454	add   x2, x5, #CNTXT_ID_DATA
455	str   xzr, [x2]
456	dc cvac, x2
457	add   x2, x5, #START_ADDR_DATA
458	str   xzr, [x2]
459	dc cvac, x2
460	add   x2, x5, #LINK_REG_DATA
461	str   xzr, [x2]
462	dc cvac, x2
463	add   x2, x5, #GICC_CTLR_DATA
464	str   xzr, [x2]
465	dc cvac, x2
466	add   x2, x5, #ABORT_FLAG_DATA
467	str   xzr, [x2]
468	dc cvac, x2
469	add   x2, x5, #SCTLR_DATA
470	str   xzr, [x2]
471	dc cvac, x2
472	add   x2, x5, #CPUECTLR_DATA
473	str   xzr, [x2]
474	dc cvac, x2
475	add   x2, x5, #AUX_01_DATA
476	str   xzr, [x2]
477	dc cvac, x2
478	add   x2, x5, #AUX_02_DATA
479	str   xzr, [x2]
480	dc cvac, x2
481	add   x2, x5, #AUX_03_DATA
482	str   xzr, [x2]
483	dc cvac, x2
484	add   x2, x5, #AUX_04_DATA
485	str   xzr, [x2]
486	dc cvac, x2
487	add   x2, x5, #AUX_05_DATA
488	str   xzr, [x2]
489	dc cvac, x2
490	add   x2, x5, #SCR_EL3_DATA
491	str   xzr, [x2]
492	dc cvac, x2
493	add   x2, x5, #HCR_EL2_DATA
494	str   xzr, [x2]
495	dc cvac, x2
496	dsb sy
497	isb
498
499	sub   x4, x4, #1
500	cbz   x4, 3f
501
502	/* generate next core mask */
503	lsl  x3, x3, #1
504
505	/* decrement base address to next data area */
506	sub  x5, x5, #SEC_REGION_SIZE
507	b	2b
5083:
509	mov   x30, x7
510	ret
511endfunc _initialize_psci
512
513
514/* Function initializes the soc init task flags
515 * in:  none
516 * out: none
517 * uses x0, x1, [x13, x14, x15]
518 */
519func _init_task_flags
520
521	/* get the base address of the first task structure */
522	ldr  x0, =SMC_TASK1_BASE
523
524	/* x0 = task1 base address */
525
526	str  wzr, [x0, #TSK_START_OFFSET]
527	str  wzr, [x0, #TSK_DONE_OFFSET]
528	str  wzr, [x0, #TSK_CORE_OFFSET]
529	dc   cvac, x0
530
531	/* move to task2 structure */
532	add  x0, x0, #SMC_TASK_OFFSET
533
534	str  wzr, [x0, #TSK_START_OFFSET]
535	str  wzr, [x0, #TSK_DONE_OFFSET]
536	str  wzr, [x0, #TSK_CORE_OFFSET]
537	dc   cvac, x0
538
539	/* move to task3 structure */
540	add  x0, x0, #SMC_TASK_OFFSET
541
542	str  wzr, [x0, #TSK_START_OFFSET]
543	str  wzr, [x0, #TSK_DONE_OFFSET]
544	str  wzr, [x0, #TSK_CORE_OFFSET]
545	dc   cvac, x0
546
547	/* move to task4 structure */
548	add  x0, x0, #SMC_TASK_OFFSET
549
550	str  wzr, [x0, #TSK_START_OFFSET]
551	str  wzr, [x0, #TSK_DONE_OFFSET]
552	str  wzr, [x0, #TSK_CORE_OFFSET]
553	dc   cvac, x0
554
555	dsb  sy
556	isb
557	ret
558endfunc _init_task_flags
559