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