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
|
/*
* File : vfp_entry_gcc.S
* This file is part of RT-Thread RTOS
* COPYRIGHT (C) 2006, RT-Thread Development Team
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* 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.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*
* Change Logs:
* Date Author Notes
* 2014-11-07 weety first version
*/
#include <rtconfig.h>
#ifdef RT_USING_VFP
#include "armv6.h"
#include "vfp.h"
//#define DEBUG
.macro PRINT, str
#ifdef DEBUG
stmfd sp!, {r0-r3, ip, lr}
add r0, pc, #4
bl rt_kprintf
b 1f
.asciz "VFP: \str\n"
.balign 4
1: ldmfd sp!, {r0-r3, ip, lr}
#endif
.endm
.macro PRINT1, str, arg
#ifdef DEBUG
stmfd sp!, {r0-r3, ip, lr}
mov r1, \arg
add r0, pc, #4
bl rt_kprintf
b 1f
.asciz "VFP: \str\n"
.balign 4
1: ldmfd sp!, {r0-r3, ip, lr}
#endif
.endm
.macro PRINT3, str, arg1, arg2, arg3
#ifdef DEBUG
stmfd sp!, {r0-r3, ip, lr}
mov r3, \arg3
mov r2, \arg2
mov r1, \arg1
add r0, pc, #4
bl rt_kprintf
b 1f
.asciz "VFP: \str\n"
.balign 4
1: ldmfd sp!, {r0-r3, ip, lr}
#endif
.endm
.macro get_vfpregs_offset, rd
ldr \rd, .vfp_offset
ldr \rd, [\rd]
.endm
.vfp_offset:
.word vfpregs_offset
.macro vfp_restore_working_reg, base, rd0
vldmia \base!, {d0-d15}
#ifdef RT_USING_VFPv3
vmrs \rd0, mvfr0
and \rd0, \rd0, #MVFR0_A_SIMD_MASK @ A_SIMD registers
cmp \rd0, #2 @ 0b0000 Not supported.
@ 0b0001 Supported, 16 ��64-bit registers.
@ 0b0010 Supported, 32 ��64-bit registers.
vldmiaeq \base!, {d16-d31}
addne \base, \base, #32*4 @ skip unused registers
#endif
.endm
.macro vfp_save_working_reg, base, rd0
vstmia \base!, {d0-d15} @ save the working registers
#ifdef RT_USING_VFPv3
vmrs \rd0, mvfr0
and \rd0, \rd0, #MVFR0_A_SIMD_MASK @ A_SIMD registers
cmp \rd0, #2 @ 0b0000 Not supported.
@ 0b0001 Supported, 16 ��64-bit registers.
@ 0b0010 Supported, 32 ��64-bit registers.
vstmiaeq \base!, {d16-d31}
addne \base, \base, #32*4 @ skip unused registers
#endif
.endm
.macro vfp_restore_state, base, fpexc_rd, rd0, rd1, rd2
ldmia \base, {\fpexc_rd, \rd0, \rd1, \rd2} @ load FPEXC, FPSCR, FPINST, FPINST2
tst \fpexc_rd, #FPEXC_EX @ vfp is in the exceptional state?
beq 1f
vmsr fpinst, \rd1 @ restore fpinst
tst \fpexc_rd, #FPEXC_FP2V @ FPINST2 instruction valid
beq 1f
vmsr fpinst2, \rd2 @ restore fpinst2
1:
vmsr fpscr, \rd0 @ restore fpscr
.endm
.macro vfp_save_state, base, fpexc_rd, rd0, rd1, rd2
vmrs \rd0, fpscr @ current status
tst \fpexc_rd, #FPEXC_EX @ vfp is in the exceptional state?
beq 1f
vmrs \rd1, fpinst @ get fpinst
tst \fpexc_rd, #FPEXC_FP2V @ FPINST2 instruction valid
beq 1f
vmrs \rd2, fpinst2 @ get fpinst2
1:
stmia \base, {\fpexc_rd, \rd0, \rd1, \rd2} @ save FPEXC, FPSCR, FPINST, FPINST2
.endm
/*
* VFP hardware support entry point.
* r0 = faulted instruction
* r2 = faulted PC+4
* r9 = successful return
* r10 = rt_thread structure
* lr = failure return
*/
.globl vfp_entry
vfp_entry:
ldr r1, =rt_interrupt_nest
ldr r1, [r1] @ get rt_interrupt_nest
cmp r1, #0 @ rt_interrupt_nest == 0?
bne irq_vfp_entry @ irq handler used VFP
get_vfpregs_offset r11
add r10, r10, r11 @ r10 = vfpregs
vmrs r1, fpexc
tst r1, #FPEXC_EN
bne __lookup_vfp_exceptions @ if the VFP already enabled, now checking vfp exceptions
ldr r3, last_vfp_context_address
orr r1, r1, #FPEXC_EN @ set VFP enable bit
ldr r4, [r3] @ get last_vfp_context pointer
bic r5, r1, #FPEXC_EX @ clear exceptions status
cmp r4, r10
beq __switch_to_the_same_thread @ switch to the same thread, checking pending exception.
vmsr fpexc, r5 @ enable VFP, clear any pending exceptions
/* Save the current VFP registers to the old thread context */
cmp r4, #0
beq __no_last_vfp_context
vfp_save_working_reg r4, r5 @ save the working registers
vfp_save_state r4, r1, r5, r6, r8 @ save vfp state registers
__no_last_vfp_context:
str r10, [r3] @ update the last_vfp_context pointer
vfp_restore_working_reg r10, r5 @ restore the working registers
vfp_restore_state r10, r1, r5, r6, r8 @ restore vfp state registers
__switch_to_the_same_thread:
tst r1, #FPEXC_EX
bne __do_exception
vmsr fpexc, r1 @ restore fpexc last
sub r2, r2, #4
str r2, [sp, #S_PC] @ retry the faulted instruction
PRINT1 "return instr=0x%08x", r2
mov pc, r9
__lookup_vfp_exceptions:
tst r1, #FPEXC_EX | FPEXC_DEX @ Check for synchronous or asynchronous exception
bne __do_exception
vmrs r5, fpscr
tst r5, #FPSCR_IXE
bne __do_exception
PRINT "__lookup_vfp_exceptions"
mov pc, lr
__do_exception:
PRINT "__do_exception"
push {lr}
mov r5, r1
bic r5, #FPEXC_EX @ clear exception
vmsr fpexc, r5
bl vfp_exception @ r0 = faulted instruction, r1 = fpexc
pop {pc}
@mov pc, lr
irq_vfp_entry:
vmrs r1, fpexc
tst r1, #FPEXC_EN
bne __lookup_vfp_exceptions @ if the VFP already enabled, now checking vfp exceptions
ldr r3, last_vfp_context_address
orr r1, r1, #FPEXC_EN @ set VFP enable bit
ldr r4, [r3] @ get last_vfp_context pointer
bic r5, r1, #FPEXC_EX @ clear exceptions status
vmsr fpexc, r5 @ enable VFP, clear any pending exceptions
/* Save the current VFP registers to the old thread context */
cmp r4, #0 @ last_vfp_context != NULL ?
beq __no_save_vfp_context
vfp_save_working_reg r4, r5 @ save the working registers
vfp_save_state r4, r1, r5, r6, r8 @ save vfp state registers
mov r4, #0
str r4, [r3] @ update the last_vfp_context pointer
@ last_vfp_context = NULL
__no_save_vfp_context:
sub r2, r2, #4
str r2, [sp, #S_PC] @ retry the faulted instruction
PRINT1 "return instr=0x%08x", r2
mov pc, r9
.align
last_vfp_context_address:
.word last_vfp_context
#endif
|
