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
|
/*
* 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
*/
.section ".text.entrypoint","ax"
.globl _start
_start:
/* Read cpu id */
mrs x1, mpidr_el1
and x1, x1, #3
cbz x1, cpu_setup /* If cpu id > 0, stop slave cores */
cpu_idle:
wfe
b cpu_idle
cpu_setup:
ldr x1, =_start
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 x0, #(1 << 0) /* EL0 and EL1 are in Non-Secure state */
orr x0, x0, #(1 << 4) /* RES1 */
orr x0, x0, #(1 << 5) /* RES1 */
orr x0, x0, #(1 << 7) /* SMC instructions are undefined at EL1 and above */
orr x0, x0, #(1 << 8) /* HVC instructions are enabled at EL1 and above */
orr x0, x0, #(1 << 10) /* The next lower level is AArch64 */
msr scr_el3, x2
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:
msr sp_el1, x1
/* 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:
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
ldr x1, =__bss_start
ldr w2, =__bss_size
clean_bss_loop:
cbz w2, jump_to_entry
str xzr, [x1], #8
sub w2, w2, #1
cbnz w2, clean_bss_loop
jump_to_entry:
b rtthread_startup
b cpu_idle /* For failsafe, halt this core too */
|
