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
|
/*
* Copyright (c) 2018, Arm Limited. All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include <asm_macros.S>
.globl tftf_vector
vector_base tftf_vector
/*
* Current EL with SP0 : 0x0 - 0x200.
*/
vector_entry SynchronousExceptionSP0
b SynchronousExceptionSP0
check_vector_size SynchronousExceptionSP0
vector_entry IrqSP0
b IrqSP0
check_vector_size IrqSP0
vector_entry FiqSP0
b FiqSP0
check_vector_size FiqSP0
vector_entry SErrorSP0
b SErrorSP0
check_vector_size SErrorSP0
/*
* Current EL with SPx : 0x200 - 0x400.
*/
vector_entry SynchronousExceptionSPx
b SynchronousExceptionSPx
check_vector_size SynchronousExceptionSPx
vector_entry IrqSPx
/*
* TODO: Investigate whether the Trusted Firmware-A code for context
* save/restore could be reused
*/
stp x29, x30, [sp, #-0x10]!
bl save_regs
bl tftf_irq_handler_dispatcher
bl restore_regs
ldp x29, x30, [sp], #0x10
eret
check_vector_size IrqSPx
vector_entry FiqSPx
b FiqSPx
check_vector_size FiqSPx
vector_entry SErrorSPx
b SErrorSPx
check_vector_size SErrorSPx
/*
* Lower EL using AArch64 : 0x400 - 0x600.
*/
vector_entry SynchronousExceptionA64
b SynchronousExceptionA64
check_vector_size SynchronousExceptionA64
vector_entry IrqA64
b IrqA64
check_vector_size IrqA64
vector_entry FiqA64
b FiqA64
check_vector_size FiqA64
vector_entry SErrorA64
b SErrorA64
check_vector_size SErrorA64
/*
* Lower EL using AArch32 : 0x600 - 0x800.
*/
vector_entry SynchronousExceptionA32
b SynchronousExceptionA32
check_vector_size SynchronousExceptionA32
vector_entry IrqA32
b IrqA32
check_vector_size IrqA32
vector_entry FiqA32
b FiqA32
check_vector_size FiqA32
vector_entry SErrorA32
b SErrorA32
check_vector_size SErrorA32
// Note: Exceptions will always be from the same EL, so no need to save spsr
func save_regs
sub sp, sp, #0x100
stp x0, x1, [sp, #0x0]
stp x2, x3, [sp, #0x10]
stp x4, x5, [sp, #0x20]
stp x6, x7, [sp, #0x30]
stp x8, x9, [sp, #0x40]
stp x10, x11, [sp, #0x50]
stp x12, x13, [sp, #0x60]
stp x14, x15, [sp, #0x70]
stp x16, x17, [sp, #0x80]
stp x18, x19, [sp, #0x90]
stp x20, x21, [sp, #0xa0]
stp x22, x23, [sp, #0xb0]
stp x24, x25, [sp, #0xc0]
stp x26, x27, [sp, #0xd0]
mrs x0, sp_el0
stp x28, x0, [sp, #0xe0]
str x0, [sp, #0xf0]
ret
endfunc save_regs
// Note: Exceptions will always be from the same EL, so no need to restore spsr
func restore_regs
ldr x9, [sp, #0xf0]
ldp x28, x9, [sp, #0xe0]
msr sp_el0, x9
ldp x26, x27, [sp, #0xd0]
ldp x24, x25, [sp, #0xc0]
ldp x22, x23, [sp, #0xb0]
ldp x20, x21, [sp, #0xa0]
ldp x18, x19, [sp, #0x90]
ldp x16, x17, [sp, #0x80]
ldp x14, x15, [sp, #0x70]
ldp x12, x13, [sp, #0x60]
ldp x10, x11, [sp, #0x50]
ldp x8, x9, [sp, #0x40]
ldp x6, x7, [sp, #0x30]
ldp x4, x5, [sp, #0x20]
ldp x2, x3, [sp, #0x10]
ldp x0, x1, [sp, #0x0]
add sp, sp, #0x100
ret
endfunc restore_regs
|