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
|
/*
* Copyright (c) 2021, Jesse Buhagiar <jooster669@gmail.com>
*
* SPDX-License-Identifier: BSD-2-Clause
*/
.section .text.vector_table
// NOTE: This size must be a multiple of 16 bytes, to ensure that the stack pointer
// stays 16 byte aligned.
#define REGISTER_STATE_SIZE 272
#if REGISTER_STATE_SIZE % 16 != 0
# error "REGISTER_STATE_SIZE is not a multiple of 16 bytes!"
#endif
#define SPSR_EL1_SLOT (31 * 8)
#define ELR_EL1_SLOT (32 * 8)
#define SP_EL0_SLOT (33 * 8)
// Vector Table Entry macro. Each entry is aligned at 128 bytes, meaning we have
// at most that many instructions.
.macro table_entry label
.align 7
b \label
.endm
.macro unimplemented_entry
.align 7
wfe
b .
.endm
.extern exception_common
.extern handle_interrupt
//
// Save all register states to the current stack
// and enter the C++ exception handler
//
.macro save_current_context
// Allocate stack space for Trap Frame
sub sp, sp, #REGISTER_STATE_SIZE
stp x0, x1, [sp, #(0 * 0)]
stp x2, x3, [sp, #(2 * 8)]
stp x4, x5, [sp, #(4 * 8)]
stp x6, x7, [sp, #(6 * 8)]
stp x8, x9, [sp, #(8 * 8)]
stp x10, x11, [sp, #(10 * 8)]
stp x12, x13, [sp, #(12 * 8)]
stp x14, x15, [sp, #(14 * 8)]
stp x16, x17, [sp, #(16 * 8)]
stp x18, x19, [sp, #(18 * 8)]
stp x20, x21, [sp, #(20 * 8)]
stp x22, x23, [sp, #(22 * 8)]
stp x24, x25, [sp, #(24 * 8)]
stp x26, x27, [sp, #(26 * 8)]
stp x28, x29, [sp, #(28 * 8)]
str x30, [sp, #(30 * 8)]
// Let's save some special registers
mrs x0, spsr_el1
str x0, [sp, #SPSR_EL1_SLOT]
mrs x0, elr_el1
str x0, [sp, #ELR_EL1_SLOT]
mrs x0, sp_el0
str x0, [sp, #SP_EL0_SLOT]
// Set up TrapFrame struct on the stack
mov x0, sp
sub sp, sp, #16
str x0, [sp, #(1 * 8)]
str xzr, [sp, #(0 * 0)]
// Move stack pointer into first argument register
// and jump to the C++ exception handler
mov x0, sp
.endm
.macro restore_previous_context
// Remove TrapFrame from the stack
add sp, sp, #16
// Restore special registers first
ldr x0, [sp, #SPSR_EL1_SLOT]
msr spsr_el1, x0
ldr x0, [sp, #ELR_EL1_SLOT]
msr elr_el1, x0
ldr x0, [sp, #SP_EL0_SLOT]
msr sp_el0, x0
ldp x0, x1, [sp, #(0 * 0)]
ldp x2, x3, [sp, #(2 * 8)]
ldp x4, x5, [sp, #(4 * 8)]
ldp x6, x7, [sp, #(6 * 8)]
ldp x8, x9, [sp, #(8 * 8)]
ldp x10, x11, [sp, #(10 * 8)]
ldp x12, x13, [sp, #(12 * 8)]
ldp x14, x15, [sp, #(14 * 8)]
ldp x16, x17, [sp, #(16 * 8)]
ldp x18, x19, [sp, #(18 * 8)]
ldp x20, x21, [sp, #(20 * 8)]
ldp x22, x23, [sp, #(22 * 8)]
ldp x24, x25, [sp, #(24 * 8)]
ldp x26, x27, [sp, #(26 * 8)]
ldp x28, x29, [sp, #(28 * 8)]
ldr x30, [sp, #(30 * 8)]
add sp, sp, #REGISTER_STATE_SIZE
.endm
.global vector_table_el1
.weak vector_table_el1 // Vector table is weak in case someone wants to hook us in C++ land :^)
.type vector_table_el1, @object
// Vector table is 2KiB aligned (2^11)
.align 11
vector_table_el1:
// Exceptions taken from Current EL, with SP_EL0
table_entry synchronous_current_elsp_el0
table_entry irq_current_elsp_el0
table_entry fiq_current_elsp_el0
table_entry system_error_current_elsp_el0
// Exceptions taken from Current EL, with SP_ELx, x>0
table_entry synchronous_current_elsp_elx
table_entry irq_current_elsp_elx
table_entry fiq_current_elsp_elx
table_entry system_error_current_elsp_elx
// Exceptions from Lower EL, where causing application is in AArch64 mode
table_entry synchronous_lower_el
table_entry irq_lower_el
table_entry fiq_lower_el
table_entry system_error_lower_el
// Exceptions from Lower EL, where causing application is in AArch32 mode
unimplemented_entry
unimplemented_entry
unimplemented_entry
unimplemented_entry
synchronous_current_elsp_elx:
save_current_context
bl exception_common
restore_previous_context
eret
irq_current_elsp_elx:
save_current_context
bl handle_interrupt
restore_previous_context
eret
fiq_current_elsp_elx:
save_current_context
bl exception_common
restore_previous_context
eret
system_error_current_elsp_elx:
save_current_context
bl exception_common
restore_previous_context
eret
synchronous_current_elsp_el0:
save_current_context
bl exception_common
restore_previous_context
eret
irq_current_elsp_el0:
save_current_context
bl handle_interrupt
restore_previous_context
eret
fiq_current_elsp_el0:
save_current_context
bl exception_common
restore_previous_context
eret
system_error_current_elsp_el0:
save_current_context
bl exception_common
restore_previous_context
eret
synchronous_lower_el:
save_current_context
bl exception_common
restore_previous_context
eret
irq_lower_el:
save_current_context
bl handle_interrupt
restore_previous_context
eret
fiq_lower_el:
save_current_context
bl exception_common
restore_previous_context
eret
system_error_lower_el:
save_current_context
bl exception_common
restore_previous_context
eret
.global restore_context_and_eret
restore_context_and_eret:
mov x0, sp
bl exit_trap
restore_previous_context
eret
|
