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
|
.equ _core_id_mask, 0b11
.section .text.boot
.macro init_core coreid
// set vector address.
ldr r0, =vector
mcr p15, 0, r0, c12, c0, 0
cps #0x12 // Setup sp in IRQ mode.
ldr sp, =irq_stack_core\coreid
cps #0x11 // Setup sp in FIQ mode.
ldr sp, =fiq_stack_core\coreid
cps #0x1B // Setup sp in UNDEF mode.
ldr sp, =undefined_stack_core\coreid
cps #0x17 // Setup sp in ABORT mode.
ldr sp, =data_stack_core\coreid
cps #0x1f // Setup sp in USR/SYS mode.
ldr sp, =sys_stack_core\coreid
cps #0x13 // Setup sp in SVC mode.
ldr sp, =svc_stack_core\coreid
.endm
.macro core_stacks coreid
.space 4096
undefined_stack_core\coreid:
.space 4096
svc_stack_core\coreid:
.space 4096
data_stack_core\coreid:
.space 4096
irq_stack_core\coreid:
.space 4096
fiq_stack_core\coreid:
.space 4096
sys_stack_core\coreid:
.endm
.global _start
_start:
reset:
cpsid aif
// Exit Hypervisor Mode
mrs r0, cpsr
and r1, r0, #0x1F
cmp r1, #0x1A
bne 1f
bic r0, r0, #0x1f
orr r0, r0, #0x13
msr spsr_cxsf, r0
add r0, pc, #4
msr ELR_hyp, r0
eret
1:
// disable core0,1,2.
mrc p15, #0, r1, c0, c0, #5
and r1, r1, #3
cmp r1, #1
beq runcore1
cmp r1, #2
beq runcore2
cmp r1, #3
bge runcore3
init_core 0
// Clear out bss.
ldr r4, =__bss_start
ldr r9, =__bss_end
mov r5, #0
mov r6, #0
mov r7, #0
mov r8, #0
b 2f
1: // store multiple at r4.
stmia r4!, {{r5-r8}}
2: // If we are still below bss_end, loop.
cmp r4, r9
blo 1b
ldr r3, =_start_rust
blx r3
runcore1:
init_core 1
mov r0, #1
b _start_other_core
runcore2:
init_core 2
mov r0, #2
b _start_other_core
runcore3:
init_core 3
mov r0, #3
b _start_other_core
undefined:
.global io_halt
io_halt:
wfi
b io_halt
.align 5
vector:
ldr pc, reset_handler
ldr pc, undefined_handler
ldr pc, svc_handler
ldr pc, prefetch_handler
ldr pc, data_handler
ldr pc, unused_handler
ldr pc, irq_handler
ldr pc, fiq_handler
reset_handler: .word reset
undefined_handler: .word undefined
svc_handler: .word svc
prefetch_handler: .word prefetch
data_handler: .word data
unused_handler: .word io_halt
irq_handler: .word irq
fiq_handler: .word fiq
.section .bss.sysstacks
.align 4
core_stacks 0
core_stacks 1
core_stacks 2
core_stacks 3
|
