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
|
/*
* Copyright (c) 2006-2020, RT-Thread Development Team
*
* SPDX-License-Identifier: Apache-2.0
*
* Date Author Notes
* 2021-06-29 Wayne the first version
*/
/* GICv2 - Distributor Registers */
#define GICD_CTLR 0x0000
#define GICD_TYPER 0x0004
#define GICD_IIDR 0x0008
#define GICD_STATUSR 0x0010
#define GICD_SETSPI_NSR 0x0040
#define GICD_CLRSPI_NSR 0x0048
#define GICD_SETSPI_SR 0x0050
#define GICD_CLRSPI_SR 0x0058
#define GICD_SEIR 0x0068
#define GICD_IGROUPRn 0x0080
#define GICD_ISENABLERn 0x0100
#define GICD_ICENABLERn 0x0180
#define GICD_ISPENDRn 0x0200
#define GICD_ICPENDRn 0x0280
#define GICD_ISACTIVERn 0x0300
#define GICD_ICACTIVERn 0x0380
#define GICD_IPRIORITYRn 0x0400
#define GICD_ITARGETSRn 0x0800
#define GICD_ICFGR 0x0c00
#define GICD_IGROUPMODRn 0x0d00
#define GICD_NSACRn 0x0e00
#define GICD_SGIR 0x0f00
#define GICD_CPENDSGIRn 0x0f10
#define GICD_SPENDSGIRn 0x0f20
#define GICD_IROUTERn 0x6000
/* GICv2 - CPU Interface Memory Mapped Registers */
#define GICC_CTLR 0x0000
#define GICC_PMR 0x0004
#define GICC_BPR 0x0008
#define GICC_IAR 0x000C
#define GICC_EOIR 0x0010
#define GICC_RPR 0x0014
#define GICC_HPPIR 0x0018
#define GICC_ABPR 0x001c
#define GICC_AIAR 0x0020
#define GICC_AEOIR 0x0024
#define GICC_AHPPIR 0x0028
#define GICC_APRn 0x00d0
#define GICC_NSAPRn 0x00e0
#define GICC_IIDR 0x00fc
#define GICC_DIR 0x1000
.section ".text.entrypoint"
.global _start
_start:
/* Give execution address for secondary CPU */
adr x20, .
mov sp, x20
/*=============================================================*/
/* Enable the SMP bit. */
/*=============================================================*/
mrs x0, S3_1_C15_C2_1
orr x0, x0, #(1<<6)
msr S3_1_C15_C2_1, x0
/*=============================================================*/
/* Read CPU id */
/* Primary core(id=0): Help Secondary core leaving. */
/* Secondary core(id>0): Notice 'Ready' to Primary core. */
/*=============================================================*/
/* MPIDR_EL1: Multi-Processor Affinity Register */
mrs x1, mpidr_el1
and x1, x1, #3
cbz x1, .L__cpu_0
.L__current_cpu_idle:
/*=============================================================*/
/* Secondary CPU notification */
/*=============================================================*/
wfe /* Wait for Primary CPU's notification */
mov x0, #0x48 /* if (*(0x40460048)==0) */
movk x0, #0x4046, LSL #16 /* goto L__current_cpu_idle */
ldr x1, [x0] /* else */
cmp x1, #0 /* *(0x4046004C)=_start */
b.eq .L__current_cpu_idle /* goto L__cpus_trans_state */
add x0, x0, #4
str w1, [x0]
b .L__cpus_trans_state
.L__cpu_0:
// *(0x4046004C) = 0
mov x0, #0x4C
movk x0, #0x4046, LSL #16
mov x1, xzr
str w1, [x0]
mov x10, #0x1000 /* 4096 times looping */
.L__cpu_0_loop:
sub x10, x10, #0x1
cmp x10, #0
b.eq .L__cpus_trans_state
/*=============================================================*/
/* Help CPU-1 to leave IBR. */
/*=============================================================*/
mov x0, #0x48 /* *(0x40460048) = _start */
movk x0, #0x4046, LSL #16
mov x1, x20
str w1, [x0]
sev /* Wakeup Secondary CPU */
add x0, x0, #4 /* if(*(0x4046004C)!=_start) */
ldr w2, [x0] /* goto L__cpu_0_loop */
cmp x1, x2
bne .L__cpu_0_loop
.L__cpus_trans_state:
/*=============================================================*/
/* Initialize Gtimer. Set frequency to 12MHz. */
/*=============================================================*/
mov x0, #0x1B00
movk x0, #0xB7, LSL #16
msr CNTFRQ_EL0, x0
/*=============================================================*/
/* Enable GICv2. */
/* Assign all IRQs to secure group. */
/*=============================================================*/
/* Route to secure Group */
mov x0, #0x1000
movk x0, #0x5080, LSL #16
mov w9, #0x3
str w9, [x0, GICD_CTLR]
ldr w9, [x0, GICD_TYPER]
and w10, w9, #0x1f
cbz w10, 1f
add x11, x0, GICD_IGROUPRn
mov w9, #0
str w9, [x11], #0x04
0: str w9, [x11], #0x04
sub w10, w10, #0x1
cbnz w10, 0b
mov x1, #0x2000
movk x1, #0x5080, LSL #16
mov w0, #3
str w0, [x1]
mov w0, #1 << 7
str w0, [x1, #4]
1:
mov x0, #0x1000
movk x0, #0x5080, LSL #16
mov x1, #0x2000
movk x1, #0x5080, LSL #16
mov w9, #0
str w9, [x0, GICD_IGROUPRn]
mov w9, #0x1
str w9, [x0, GICD_ISENABLERn]
mov w9, #0x1e7
str w9, [x1, GICC_CTLR]
mov w9, #0x1 << 7
str w9, [x1, GICC_PMR]
/*=============================================================*/
/* Enable FP/SIMD at EL1 */
/*=============================================================*/
mov x0, #(3 << 20)
msr cpacr_el1, x0 /* Enable FP/SIMD at EL1 */
/*=============================================================*/
/* Initialize sctlr_el1 */
/*=============================================================*/
mov x0, xzr
orr x0, x0, #(1 << 29) /* Enable LSMAOE at EL1 */
orr x0, x0, #(1 << 28) /* Enable nTLSMD at EL1 */
orr x0, x0, #(1 << 23) /* Enable SPAN at EL1 */
orr x0, x0, #(1 << 22) /* Enable EIS at EL1 */
orr x0, x0, #(1 << 20) /* Enable TSCXT at EL1 */
orr x0, x0, #(1 << 11) /* Enable EOS at EL1 */
msr sctlr_el1, x0
/*=============================================================*/
/* Initialize scr_el3 */
/*=============================================================*/
mov x0, xzr
/* RW, Lower levels are all AArch32. */
orr x0, x0, #(1 << 9) /* Enable SIF */
orr x0, x0, #(1 << 8) /* Enable HCE */
orr x0, x0, #(1 << 7) /* Enable SMD */
orr x0, x0, #(1 << 5) /* RES1[5:4] */
orr x0, x0, #(1 << 4)
/* Disable FIQ routing */
/* Disable IRQ routing */
/* Disable NS */
msr scr_el3, x0
/*=============================================================*/
/* Initialize spsr_el3 */
/*=============================================================*/
mov x0, xzr
mov x0, #0b00011 /* AARCH32_SVC */
orr x0, x0, #(1 << 8) /* Enable SError and External Abort. */
orr x0, x0, #(1 << 7) /* IRQ interrupt Process state mask. */
orr x0, x0, #(1 << 6) /* FIQ interrupt Process state mask. */
orr x0, x0, #(1 << 4) /* FIQ interrupt Process state mask. */
msr spsr_el3, x0
/*=============================================================*/
/* Initialize elr_el3 */
/* Jump to Secure AARCH32_SVC from EL3. */
/*=============================================================*/
adr x0, .aarch32_code /* Exception return to aarch32_code */
msr elr_el3, x0
eret
.aarch32_code:
|
