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
|
/*
* Copyright (c) 2006-2020, RT-Thread Development Team
*
* SPDX-License-Identifier: Apache-2.0
*
* Date Author Notes
* 2020-01-15 bigmagic the first version
* 2020-08-10 SummerGift support clang compiler
* 2021-11-04 GuEe-GUI set sp with SP_ELx
* 2021-12-28 GuEe-GUI add smp support
*/
#include "rtconfig.h"
.section ".text.entrypoint","ax"
#define SECONDARY_STACK_SIZE 4096
.globl _start
.globl secondary_cpu_start
_start:
mrs x1, mpidr_el1
and x1, x1, #0xff
cbnz x1, cpu_idle /* If cpu id > 0, stop slave cores */
secondary_cpu_start:
#ifdef RT_USING_SMP
/* Read cpu mpidr_el1 */
mrs x1, mpidr_el1
/* Read cpu id */
ldr x0, =rt_cpu_mpidr_early /* BSP must be defined `rt_cpu_mpidr_early' table in smp */
mov x2, #0
cpu_id_confirm:
add x2, x2, #1 /* Next cpu id inc */
ldr x3, [x0], #8
cmp x3, #0
beq cpu_idle /* Mean that `rt_cpu_mpidr_early' table is end */
cmp x3, x1
bne cpu_id_confirm
/* Get cpu id success */
sub x0, x2, #1
msr tpidr_el1, x0 /* Save cpu id global */
cbz x0, cpu_setup /* Only go to cpu_setup when cpu id = 0 */
/* Set current cpu's stack top */
sub x0, x0, #1
mov x1, #SECONDARY_STACK_SIZE
adr x2, .secondary_cpu_stack_top
msub x1, x0, x1, x2
b cpu_check_el
#else
msr tpidr_el1, xzr
b cpu_setup
#endif /* RT_USING_SMP */
cpu_idle:
wfe
b cpu_idle
cpu_setup:
ldr x1, =_start
cpu_check_el:
mrs x0, CurrentEL /* CurrentEL Register. bit 2, 3. Others reserved */
and x0, x0, #12 /* Clear reserved bits */
/* Running at EL3? */
cmp x0, #12 /* EL3 value is 0b1100 */
bne cpu_not_in_el3
/* Should never be executed, just for completeness. (EL3) */
mov x2, #(1 << 0) /* EL0 and EL1 are in Non-Secure state */
orr x2, x2, #(1 << 4) /* RES1 */
orr x2, x2, #(1 << 5) /* RES1 */
orr x2, x2, #(1 << 7) /* SMC instructions are undefined at EL1 and above */
orr x2, x2, #(1 << 8) /* HVC instructions are enabled at EL1 and above */
orr x2, x2, #(1 << 10) /* The next lower level is AArch64 */
msr scr_el3, x2
/* Change execution level to EL2 */
mov x2, #0x3c9
msr spsr_el3, x2 /* 0b1111001001 */
adr x2, cpu_not_in_el3
msr elr_el3, x2
eret /* Exception Return: from EL3, continue from cpu_not_in_el3 */
cpu_not_in_el3: /* Running at EL2 or EL1 */
cmp x0, #4 /* EL1 = 0100 */
beq cpu_in_el1 /* Halt this core if running in El1 */
cpu_in_el2:
/* Enable CNTP for EL1 */
mrs x0, cnthctl_el2 /* Counter-timer Hypervisor Control register */
orr x0, x0, #3
msr cnthctl_el2, x0
msr cntvoff_el2, xzr
mov x0, #(1 << 31) /* Enable AArch64 in EL1 */
orr x0, x0, #(1 << 1) /* SWIO hardwired on Pi3 */
msr hcr_el2, x0
/* Change execution level to EL1 */
mov x2, #0x3c4
msr spsr_el2, x2 /* 0b1111000100 */
adr x2, cpu_in_el1
msr elr_el2, x2
eret
cpu_in_el1:
msr spsel, #1
mov sp, x1 /* Set sp in el1 */
/* Avoid trap from SIMD or float point instruction */
mov x1, #0x00300000 /* Don't trap any SIMD/FP instructions in both EL0 and EL1 */
msr cpacr_el1, x1
mrs x1, sctlr_el1
orr x1, x1, #(1 << 12) /* Enable Instruction */
bic x1, x1, #(3 << 3) /* Disable SP Alignment check */
bic x1, x1, #(1 << 1) /* Disable Alignment check */
msr sctlr_el1, x1
#ifdef RT_USING_SMP
ldr x1, =_start
cmp sp, x1
bne secondary_cpu_c_start
#endif /* RT_USING_SMP */
ldr x0, =__bss_start
ldr x1, =__bss_end
sub x2, x1, x0
mov x3, x1
cmp x2, #7
bls clean_bss_check
clean_bss_loop_quad:
str xzr, [x0], #8
sub x2, x3, x0
cmp x2, #7
bhi clean_bss_loop_quad
cmp x1, x0
bls jump_to_entry
clean_bss_loop_byte:
str xzr, [x0], #1
clean_bss_check:
cmp x1, x0
bhi clean_bss_loop_byte
jump_to_entry:
b rtthread_startup
b cpu_idle /* For failsafe, halt this core too */
#ifdef RT_USING_SMP
.align 12
.secondary_cpu_stack:
.space (SECONDARY_STACK_SIZE * (RT_CPUS_NR - 1))
.secondary_cpu_stack_top:
#endif
|
