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
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
|
/*
<:copyright-BRCM:2019:DUAL/GPL:standard
Copyright (c) 2019 Broadcom
All Rights Reserved
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License, version 2, as published by
the Free Software Foundation (the "GPL").
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
A copy of the GPL is available at http://www.broadcom.com/licenses/GPLv2.php, or by
writing to the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
Boston, MA 02111-1307, USA.
:>
*/
#if defined(SPD_opteed)
#include <asm_macros.S>
.global save_atf_sysreg
.global restore_atf_sysreg
.macro read_sctlr reg
mrc p15, 0, \reg, c1, c0, 0
.endm
.macro write_sctlr reg
mcr p15, 0, \reg, c1, c0, 0
.endm
.macro read_scr reg
mrc p15, 0, \reg, c1, c1, 0
.endm
.macro write_scr reg
mcr p15, 0, \reg, c1, c1, 0
.endm
.macro write_ttbr0 reg
mcr p15, 0, \reg, c2, c0, 0
.endm
.macro read_ttbr0 reg
mrc p15, 0, \reg, c2, c0, 0
.endm
.macro write_ttbr1 reg
mcr p15, 0, \reg, c2, c0, 1
.endm
.macro read_ttbr1 reg
mrc p15, 0, \reg, c2, c0, 1
.endm
.macro write_ttbcr reg
mcr p15, 0, \reg, c2, c0, 2
.endm
.macro read_ttbcr reg
mrc p15, 0, \reg, c2, c0, 2
.endm
.macro write_dacr reg
mcr p15, 0, \reg, c3, c0, 0
.endm
.macro read_dacr reg
mrc p15, 0, \reg, c3, c0, 0
.endm
.macro read_vbar reg
mrc p15, 0, \reg, c12, c0, 0
.endm
.macro write_vbar reg
mcr p15, 0, \reg, c12, c0, 0
.endm
.macro write_mvbar reg
mcr p15, 0, \reg, c12, c0, 1
.endm
.macro read_mvbar reg
mrc p15, 0, \reg, c12, c0, 1
.endm
.macro write_mair0 reg
mcr p15, 0, \reg, c10, c2, 0
.endm
.macro read_mair0 reg
mrc p15, 0, \reg, c10, c2, 0
.endm
.macro write_mair1 reg
mcr p15, 0, \reg, c10, c2, 1
.endm
.macro read_mair1 reg
mrc p15, 0, \reg, c10, c2, 1
.endm
.macro read_cpsr reg
mrs \reg, cpsr
.endm
.macro write_cpsr reg
msr cpsr_fsxc, \reg
.endm
.macro invalidate_tlb reg
mov \reg, #0
mcr p15, 0, \reg, c8, c5, 0
mcr p15, 0, \reg, c8, c6, 0
mcr p15, 0, \reg, c8, c7, 0
isb
dsb
.endm
/*
*
*/
func save_atf_sysreg
push {r1, r4,r5}
mrc p15, 0, r1, c0, c0, 5
and r1, r1, #0xff
lsl r1, r1, #2
read_sctlr r4
bic r4, r4, #0x5
ldr r5, =atf_sctlr
add r5, r5, r1
str r4, [r5]
read_cpsr r4
ldr r5, =atf_cpsr
add r5, r5, r1
str r4, [r5]
read_scr r4
ldr r5, =atf_scr
str r4, [r5]
read_vbar r4
ldr r5, =atf_vbar
str r4, [r5]
read_mair0 r4
ldr r5, =atf_mair0
str r4, [r5]
read_mair1 r4
ldr r5, =atf_mair1
str r4, [r5]
read_dacr r4
ldr r5, =atf_dacr
str r4, [r5]
read_ttbcr r4
ldr r5, =atf_ttbcr
str r4, [r5]
read_mvbar r4
ldr r5, =atf_mvbar
str r4, [r5]
read_ttbr0 r4
ldr r5, =atf_ttbr0
str r4, [r5]
read_ttbr1 r4
ldr r5, =atf_ttbr1
str r4, [r5]
pop {r1, r4,r5}
bx lr
endfunc save_atf_sysreg
/*
*
*/
func restore_atf_sysreg
push {r1,r9,r10}
mrc p15, 0, r1, c0, c0, 5
and r1, r1, #0xff
lsl r1, r1, #2
ldr r9, =atf_sctlr
add r9, r9, r1
ldr r10, [r9]
write_sctlr r10
ldr r9, =atf_cpsr
add r9, r9, r1
ldr r10, [r9]
write_cpsr r10
ldr r9, =atf_scr
ldr r10, [r9]
write_scr r10
ldr r9, =atf_vbar
ldr r10, [r9]
write_vbar r10
ldr r9, =atf_mair0
ldr r10, [r9]
write_mair0 r10
ldr r9, =atf_mair1
ldr r10, [r9]
write_mair1 r10
ldr r9, =atf_dacr
ldr r10, [r9]
write_dacr r10
ldr r9, =atf_ttbcr
ldr r10, [r9]
write_ttbcr r10
ldr r9, =atf_mvbar
ldr r10, [r9]
write_mvbar r10
ldr r9, =atf_ttbr0
ldr r10, [r9]
write_ttbr0 r10
ldr r9, =atf_ttbr1
ldr r10, [r9]
write_ttbr1 r10
invalidate_tlb r10
pop {r1,r9,r10}
bx lr
endfunc restore_atf_sysreg
.section .data
atf_cpsr:
.word 0x1234
.word 0x1234
.word 0x1234
.word 0x1234
atf_sctlr:
.word 0x1234
.word 0x1234
.word 0x1234
.word 0x1234
atf_scr:
.word 0x1234
atf_vbar:
.word 0x1234
atf_mair0:
.word 0x1234
atf_mair1:
.word 0x1234
atf_dacr:
.word 0x1234
atf_ttbcr:
.word 0x1234
atf_mvbar:
.word 0x1234
atf_ttbr0:
.word 0x1234
atf_ttbr1:
.word 0x1234
#endif
|
