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
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
|
/*
* Copyright (c) 2013-2014 embedded brains GmbH. All rights reserved.
*
* embedded brains GmbH
* Dornierstr. 4
* 82178 Puchheim
* Germany
* <rtems@embedded-brains.de>
*
* The license and distribution terms for this file may be
* found in the file LICENSE in this distribution or at
* http://www.rtems.org/license/LICENSE.
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <rtems/asm.h>
#include <rtems/score/cpu.h>
#define FRAME_OFFSET_R4 0
#define FRAME_OFFSET_R5 4
#define FRAME_OFFSET_R6 8
#define FRAME_OFFSET_R7 12
#define FRAME_OFFSET_R8 16
#define FRAME_OFFSET_R9 20
#define FRAME_OFFSET_R10 24
#define FRAME_OFFSET_R11 28
#define FRAME_OFFSET_LR 32
#ifdef ARM_MULTILIB_VFP
#define FRAME_OFFSET_D8 40
#define FRAME_OFFSET_D9 48
#define FRAME_OFFSET_D10 56
#define FRAME_OFFSET_D11 64
#define FRAME_OFFSET_D12 72
#define FRAME_OFFSET_D13 80
#define FRAME_OFFSET_D14 88
#define FRAME_OFFSET_D15 96
#define FRAME_SIZE (FRAME_OFFSET_D15 + 8)
#else
#define FRAME_SIZE (FRAME_OFFSET_LR + 4)
#endif
.section .text
FUNCTION_THUMB_ENTRY(_CPU_Context_validate)
/* Save */
sub sp, sp, #FRAME_SIZE
mov r1, r4
str r1, [sp, #FRAME_OFFSET_R4]
mov r1, r5
str r1, [sp, #FRAME_OFFSET_R5]
mov r1, r6
str r1, [sp, #FRAME_OFFSET_R6]
mov r1, r7
str r1, [sp, #FRAME_OFFSET_R7]
mov r1, r8
str r1, [sp, #FRAME_OFFSET_R8]
mov r1, r9
str r1, [sp, #FRAME_OFFSET_R9]
mov r1, r10
str r1, [sp, #FRAME_OFFSET_R10]
mov r1, r11
str r1, [sp, #FRAME_OFFSET_R11]
mov r1, lr
str r1, [sp, #FRAME_OFFSET_LR]
#ifdef ARM_MULTILIB_VFP
vstr d8, [sp, #FRAME_OFFSET_D8]
vstr d9, [sp, #FRAME_OFFSET_D9]
vstr d10, [sp, #FRAME_OFFSET_D10]
vstr d11, [sp, #FRAME_OFFSET_D11]
vstr d12, [sp, #FRAME_OFFSET_D12]
vstr d13, [sp, #FRAME_OFFSET_D13]
vstr d14, [sp, #FRAME_OFFSET_D14]
vstr d15, [sp, #FRAME_OFFSET_D15]
#endif
/* Fill */
/* R1 is used for temporary values */
mov r1, r0
/* R2 contains the stack pointer */
mov r2, sp
.macro fill_register reg
add r1, r1, #1
mov \reg, r1
.endm
#ifdef ARM_MULTILIB_VFP
/* R3 contains the FPSCR */
vmrs r3, FPSCR
movs r4, #0x001f
#ifdef ARM_MULTILIB_ARCH_V7M
movt r4, #0xf000
#else
movt r4, #0xf800
#endif
bic r3, r3, r4
and r4, r4, r0
orr r3, r3, r4
vmsr FPSCR, r3
#else
fill_register r3
#endif
fill_register r4
fill_register r5
fill_register r6
fill_register r7
fill_register r8
fill_register r9
fill_register r10
fill_register r11
fill_register r12
fill_register lr
#ifdef ARM_MULTILIB_VFP
.macro fill_vfp_register reg
add r1, r1, #1
vmov \reg, r1, r1
.endm
fill_vfp_register d0
fill_vfp_register d1
fill_vfp_register d2
fill_vfp_register d3
fill_vfp_register d4
fill_vfp_register d5
fill_vfp_register d6
fill_vfp_register d7
fill_vfp_register d8
fill_vfp_register d9
fill_vfp_register d10
fill_vfp_register d11
fill_vfp_register d12
fill_vfp_register d13
fill_vfp_register d14
fill_vfp_register d15
#ifdef ARM_MULTILIB_VFP_D32
fill_vfp_register d16
fill_vfp_register d17
fill_vfp_register d18
fill_vfp_register d19
fill_vfp_register d20
fill_vfp_register d21
fill_vfp_register d22
fill_vfp_register d23
fill_vfp_register d24
fill_vfp_register d25
fill_vfp_register d26
fill_vfp_register d27
fill_vfp_register d28
fill_vfp_register d29
fill_vfp_register d30
fill_vfp_register d31
#endif /* ARM_MULTILIB_VFP_D32 */
#endif /* ARM_MULTILIB_VFP */
/* Check */
check:
.macro check_register reg
add r1, r1, #1
cmp \reg, r1
bne restore
.endm
cmp r2, sp
bne restore
mov r1, r0
#ifndef ARM_MULTILIB_VFP
check_register r3
#endif
check_register r4
check_register r5
check_register r6
check_register r7
check_register r8
check_register r9
check_register r10
check_register r11
check_register r12
check_register lr
#ifdef ARM_MULTILIB_VFP
b check_vfp
#endif
b check
/* Restore */
restore:
ldr r1, [sp, #FRAME_OFFSET_R4]
mov r4, r1
ldr r1, [sp, #FRAME_OFFSET_R5]
mov r5, r1
ldr r1, [sp, #FRAME_OFFSET_R6]
mov r6, r1
ldr r1, [sp, #FRAME_OFFSET_R7]
mov r7, r1
ldr r1, [sp, #FRAME_OFFSET_R8]
mov r8, r1
ldr r1, [sp, #FRAME_OFFSET_R9]
mov r9, r1
ldr r1, [sp, #FRAME_OFFSET_R10]
mov r10, r1
ldr r1, [sp, #FRAME_OFFSET_R11]
mov r11, r1
ldr r1, [sp, #FRAME_OFFSET_LR]
mov lr, r1
#ifdef ARM_MULTILIB_VFP
vldr d8, [sp, #FRAME_OFFSET_D8]
vldr d9, [sp, #FRAME_OFFSET_D9]
vldr d10, [sp, #FRAME_OFFSET_D10]
vldr d11, [sp, #FRAME_OFFSET_D11]
vldr d12, [sp, #FRAME_OFFSET_D12]
vldr d13, [sp, #FRAME_OFFSET_D13]
vldr d14, [sp, #FRAME_OFFSET_D14]
vldr d15, [sp, #FRAME_OFFSET_D15]
#endif
add sp, sp, #FRAME_SIZE
bx lr
FUNCTION_END(_CPU_Context_validate)
#ifdef ARM_MULTILIB_VFP
check_vfp:
.macro check_vfp_register reg
add r1, r1, #1
vmov r4, r5, \reg
cmp r4, r5
bne 1f
cmp r1, r4
bne 1f
b 2f
1:
b restore
2:
.endm
vmrs r4, FPSCR
cmp r4, r3
bne restore
check_vfp_register d0
check_vfp_register d1
check_vfp_register d2
check_vfp_register d3
check_vfp_register d4
check_vfp_register d5
check_vfp_register d6
check_vfp_register d7
check_vfp_register d8
check_vfp_register d9
check_vfp_register d10
check_vfp_register d11
check_vfp_register d12
check_vfp_register d13
check_vfp_register d14
check_vfp_register d15
#ifdef ARM_MULTILIB_VFP_D32
check_vfp_register d16
check_vfp_register d17
check_vfp_register d18
check_vfp_register d19
check_vfp_register d20
check_vfp_register d21
check_vfp_register d22
check_vfp_register d23
check_vfp_register d24
check_vfp_register d25
check_vfp_register d26
check_vfp_register d27
check_vfp_register d28
check_vfp_register d29
check_vfp_register d30
check_vfp_register d31
#endif /* ARM_MULTILIB_VFP_D32 */
/* Restore r4 and r5 */
mov r1, r0
fill_register r4
fill_register r5
b check
#endif /* ARM_MULTILIB_VFP */
|
