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
|
/*
* Copyright (c) 2006-2020, RT-Thread Development Team
*
* SPDX-License-Identifier: Apache-2.0
*
* Date Author Notes
* 2020-01-15 bigmagic the first version
*/
.section ".text.entrypoint"
.set EL1_stack, __el1_stack
.global _start
// This symbol is set to 0x80000 in ld script. That is the address that raspi3's firmware
// loads 'kernel8.img' file in.
_start:
// read cpu id, stop slave cores
mrs x1, mpidr_el1 // MPIDR_EL1: Multi-Processor Affinity Register
and x1, x1, #3
cbz x1, .L__cpu_0 // .L prefix is the local label in ELF
// cpu id > 0, stop
// cpu id == 0 will also goto here after returned from entry() if possible
.L__current_cpu_idle:
wfe
b .L__current_cpu_idle
.L__cpu_0: // cpu id == 0
// set stack before our code
/* Define stack pointer for current exception level */
// ldr x2, =EL1_stack
// mov sp, x2
ldr x1, =_start
// set up EL1
mrs x0, CurrentEL // CurrentEL Register. bit 2, 3. Others reserved
and x0, x0, #12 // clear reserved bits
// running at EL3?
cmp x0, #12 // 1100b. So, EL3
bne .L__not_in_el3 // 11? !EL3 -> 5:
// should never be executed, just for completeness. (EL3)
mov x2, #0x5b1
msr scr_el3, x2 // SCR_ELn Secure Configuration Register
mov x2, #0x3c9
msr spsr_el3, x2 // SPSR_ELn. Saved Program Status Register. 1111001001
adr x2, .L__not_in_el3
msr elr_el3, x2
eret // Exception Return: from EL3, continue from .L__not_in_el3
// running at EL2 or EL1
.L__not_in_el3:
cmp x0, #4 // 0x04 0100 EL1
beq .L__in_el1 // EL1 -> 5:
// in EL2
msr sp_el1, x1 // Set sp of EL1 to _start
// 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
// enable AArch64 in EL1
mov x0, #(1 << 31) // AArch64
orr x0, x0, #(1 << 1) // SWIO hardwired on Pi3
msr hcr_el2, x0
mrs x0, hcr_el2
// change execution level to EL1
mov x2, #0x3c4
msr spsr_el2, x2 // 1111000100
adr x2, .L__in_el1
msr elr_el2, x2
eret // exception return. from EL2. continue from .L__in_el1
.L__in_el1:
mov sp, x1 // in EL1. Set sp to _start
// Set CPACR_EL1 (Architecture Feature Access Control Register) to 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)
bic x1, x1, #(3 << 3)
bic x1, x1, #(1 << 1)
msr sctlr_el1, x1
// clear bss
ldr x1, =__bss_start
ldr w2, =__bss_size
.L__clean_bss_loop:
cbz w2, .L__jump_to_entry
str xzr, [x1], #8
sub w2, w2, #1
cbnz w2, .L__clean_bss_loop
// jump to C code, should not return
.L__jump_to_entry:
bl entry
// for failsafe, halt this core too
b .L__current_cpu_idle
|
