vfphw.S 8.29 KB
Newer Older
Linus Torvalds's avatar
Linus Torvalds committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
/*
 *  linux/arch/arm/vfp/vfphw.S
 *
 *  Copyright (C) 2004 ARM Limited.
 *  Written by Deep Blue Solutions Limited.
 *
 * 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.
 *
 * This code is called from the kernel's undefined instruction trap.
 * r9 holds the return address for successful handling.
 * lr holds the return address for unrecognised instructions.
 * r10 points at the start of the private FP workspace in the thread structure
 * sp points to a struct pt_regs (as defined in include/asm/proc/ptrace.h)
 */
#include <asm/thread_info.h>
#include <asm/vfpmacros.h>
19
#include <linux/kern_levels.h>
Linus Torvalds's avatar
Linus Torvalds committed
20
21
22
23
24
25
26
27
#include "../kernel/entry-header.S"

	.macro	DBGSTR, str
#ifdef DEBUG
	stmfd	sp!, {r0-r3, ip, lr}
	add	r0, pc, #4
	bl	printk
	b	1f
28
	.asciz  KERN_DEBUG "VFP: \str\n"
Linus Torvalds's avatar
Linus Torvalds committed
29
30
31
32
33
34
35
36
37
38
39
40
	.balign 4
1:	ldmfd	sp!, {r0-r3, ip, lr}
#endif
	.endm

	.macro  DBGSTR1, str, arg
#ifdef DEBUG
	stmfd	sp!, {r0-r3, ip, lr}
	mov	r1, \arg
	add	r0, pc, #4
	bl	printk
	b	1f
41
	.asciz  KERN_DEBUG "VFP: \str\n"
Linus Torvalds's avatar
Linus Torvalds committed
42
43
44
45
46
47
48
49
50
51
52
53
54
55
	.balign 4
1:	ldmfd	sp!, {r0-r3, ip, lr}
#endif
	.endm

	.macro  DBGSTR3, 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	printk
	b	1f
56
	.asciz  KERN_DEBUG "VFP: \str\n"
Linus Torvalds's avatar
Linus Torvalds committed
57
58
59
60
61
62
63
64
65
66
67
68
	.balign 4
1:	ldmfd	sp!, {r0-r3, ip, lr}
#endif
	.endm


@ VFP hardware support entry point.
@
@  r0  = faulted instruction
@  r2  = faulted PC+4
@  r9  = successful return
@  r10 = vfp_state union
69
@  r11 = CPU number
Linus Torvalds's avatar
Linus Torvalds committed
70
71
@  lr  = failure return

72
ENTRY(vfp_support_entry)
Linus Torvalds's avatar
Linus Torvalds committed
73
74
75
76
	DBGSTR3	"instr %08x pc %08x state %p", r0, r2, r10

	VFPFMRX	r1, FPEXC		@ Is the VFP enabled?
	DBGSTR1	"fpexc %08x", r1
77
	tst	r1, #FPEXC_EN
Linus Torvalds's avatar
Linus Torvalds committed
78
79
80
	bne	look_for_VFP_exceptions	@ VFP is already enabled

	DBGSTR1 "enable %x", r10
81
	ldr	r3, vfp_current_hw_state_address
82
	orr	r1, r1, #FPEXC_EN	@ user FPEXC has the enable bit set
83
	ldr	r4, [r3, r11, lsl #2]	@ vfp_current_hw_state pointer
84
	bic	r5, r1, #FPEXC_EX	@ make sure exceptions are disabled
85
	cmp	r4, r10			@ this thread owns the hw context?
86
87
88
#ifndef CONFIG_SMP
	@ For UP, checking that this thread owns the hw context is
	@ sufficient to determine that the hardware state is valid.
89
	beq	vfp_hw_state_valid
Linus Torvalds's avatar
Linus Torvalds committed
90

91
92
93
94
	@ On UP, we lazily save the VFP context.  As a different
	@ thread wants ownership of the VFP hardware, save the old
	@ state if there was a previous (valid) owner.

Linus Torvalds's avatar
Linus Torvalds committed
95
96
97
98
99
	VFPFMXR	FPEXC, r5		@ enable VFP, disable any pending
					@ exceptions, so we can get at the
					@ rest of it

	DBGSTR1	"save old state %p", r4
100
101
	cmp	r4, #0			@ if the vfp_current_hw_state is NULL
	beq	vfp_reload_hw		@ then the hw state needs reloading
102
	VFPFSTMIA r4, r5		@ save the working registers
Linus Torvalds's avatar
Linus Torvalds committed
103
	VFPFMRX	r5, FPSCR		@ current status
104
#ifndef CONFIG_CPU_FEROCEON
105
	tst	r1, #FPEXC_EX		@ is there additional state to save?
106
107
108
109
110
111
	beq	1f
	VFPFMRX	r6, FPINST		@ FPINST (only if FPEXC.EX is set)
	tst	r1, #FPEXC_FP2V		@ is there an FPINST2 to read?
	beq	1f
	VFPFMRX	r8, FPINST2		@ FPINST2 if needed (and present)
1:
112
#endif
Linus Torvalds's avatar
Linus Torvalds committed
113
	stmia	r4, {r1, r5, r6, r8}	@ save FPEXC, FPSCR, FPINST, FPINST2
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
vfp_reload_hw:

#else
	@ For SMP, if this thread does not own the hw context, then we
	@ need to reload it.  No need to save the old state as on SMP,
	@ we always save the state when we switch away from a thread.
	bne	vfp_reload_hw

	@ This thread has ownership of the current hardware context.
	@ However, it may have been migrated to another CPU, in which
	@ case the saved state is newer than the hardware context.
	@ Check this by looking at the CPU number which the state was
	@ last loaded onto.
	ldr	ip, [r10, #VFP_CPU]
	teq	ip, r11
	beq	vfp_hw_state_valid

vfp_reload_hw:
	@ We're loading this threads state into the VFP hardware. Update
	@ the CPU number which contains the most up to date VFP context.
	str	r11, [r10, #VFP_CPU]

	VFPFMXR	FPEXC, r5		@ enable VFP, disable any pending
					@ exceptions, so we can get at the
					@ rest of it
139
#endif
Linus Torvalds's avatar
Linus Torvalds committed
140
141

	DBGSTR1	"load state %p", r10
142
	str	r10, [r3, r11, lsl #2]	@ update the vfp_current_hw_state pointer
Linus Torvalds's avatar
Linus Torvalds committed
143
					@ Load the saved state back into the VFP
144
	VFPFLDMIA r10, r5		@ reload the working registers while
Linus Torvalds's avatar
Linus Torvalds committed
145
					@ FPEXC is in a safe state
146
	ldmia	r10, {r1, r5, r6, r8}	@ load FPEXC, FPSCR, FPINST, FPINST2
147
#ifndef CONFIG_CPU_FEROCEON
148
	tst	r1, #FPEXC_EX		@ is there additional state to restore?
149
150
151
152
153
154
	beq	1f
	VFPFMXR	FPINST, r6		@ restore FPINST (only if FPEXC.EX is set)
	tst	r1, #FPEXC_FP2V		@ is there an FPINST2 to write?
	beq	1f
	VFPFMXR	FPINST2, r8		@ FPINST2 if needed (and present)
1:
155
#endif
Linus Torvalds's avatar
Linus Torvalds committed
156
157
	VFPFMXR	FPSCR, r5		@ restore status

158
159
@ The context stored in the VFP hardware is up to date with this thread
vfp_hw_state_valid:
160
	tst	r1, #FPEXC_EX
Linus Torvalds's avatar
Linus Torvalds committed
161
162
163
164
165
166
167
	bne	process_exception	@ might as well handle the pending
					@ exception before retrying branch
					@ out before setting an FPEXC that
					@ stops us reading stuff
	VFPFMXR	FPEXC, r1		@ restore FPEXC last
	sub	r2, r2, #4
	str	r2, [sp, #S_PC]		@ retry the instruction
168
169
170
171
172
173
#ifdef CONFIG_PREEMPT
	get_thread_info	r10
	ldr	r4, [r10, #TI_PREEMPT]	@ get preempt count
	sub	r11, r4, #1		@ decrement it
	str	r11, [r10, #TI_PREEMPT]
#endif
Linus Torvalds's avatar
Linus Torvalds committed
174
175
176
177
	mov	pc, r9			@ we think we have handled things


look_for_VFP_exceptions:
178
179
	@ Check for synchronous or asynchronous exception
	tst	r1, #FPEXC_EX | FPEXC_DEX
Linus Torvalds's avatar
Linus Torvalds committed
180
	bne	process_exception
181
182
183
	@ On some implementations of the VFP subarch 1, setting FPSCR.IXE
	@ causes all the CDP instructions to be bounced synchronously without
	@ setting the FPEXC.EX bit
Linus Torvalds's avatar
Linus Torvalds committed
184
	VFPFMRX	r5, FPSCR
185
	tst	r5, #FPSCR_IXE
Linus Torvalds's avatar
Linus Torvalds committed
186
187
188
189
190
191
	bne	process_exception

	@ Fall into hand on to next handler - appropriate coproc instr
	@ not recognised by VFP

	DBGSTR	"not VFP"
192
193
194
195
196
197
#ifdef CONFIG_PREEMPT
	get_thread_info	r10
	ldr	r4, [r10, #TI_PREEMPT]	@ get preempt count
	sub	r11, r4, #1		@ decrement it
	str	r11, [r10, #TI_PREEMPT]
#endif
Linus Torvalds's avatar
Linus Torvalds committed
198
199
200
201
202
203
204
205
206
207
208
	mov	pc, lr

process_exception:
	DBGSTR	"bounce"
	mov	r2, sp			@ nothing stacked - regdump is at TOS
	mov	lr, r9			@ setup for a return to the user code.

	@ Now call the C code to package up the bounce to the support code
	@   r0 holds the trigger instruction
	@   r1 holds the FPEXC value
	@   r2 pointer to register dump
209
	b	VFP_bounce		@ we have handled this - the support
Linus Torvalds's avatar
Linus Torvalds committed
210
211
212
					@ code will raise an exception if
					@ required. If not, the user code will
					@ retry the faulted instruction
213
ENDPROC(vfp_support_entry)
Linus Torvalds's avatar
Linus Torvalds committed
214

215
ENTRY(vfp_save_state)
216
217
218
219
	@ Save the current VFP state
	@ r0 - save location
	@ r1 - FPEXC
	DBGSTR1	"save VFP state %p", r0
220
	VFPFSTMIA r0, r2		@ save the working registers
221
	VFPFMRX	r2, FPSCR		@ current status
222
	tst	r1, #FPEXC_EX		@ is there additional state to save?
223
224
225
226
227
228
	beq	1f
	VFPFMRX	r3, FPINST		@ FPINST (only if FPEXC.EX is set)
	tst	r1, #FPEXC_FP2V		@ is there an FPINST2 to read?
	beq	1f
	VFPFMRX	r12, FPINST2		@ FPINST2 if needed (and present)
1:
229
230
	stmia	r0, {r1, r2, r3, r12}	@ save FPEXC, FPSCR, FPINST, FPINST2
	mov	pc, lr
231
ENDPROC(vfp_save_state)
232

233
	.align
234
235
vfp_current_hw_state_address:
	.word	vfp_current_hw_state
Linus Torvalds's avatar
Linus Torvalds committed
236

237
238
239
240
241
242
243
	.macro	tbl_branch, base, tmp, shift
#ifdef CONFIG_THUMB2_KERNEL
	adr	\tmp, 1f
	add	\tmp, \tmp, \base, lsl \shift
	mov	pc, \tmp
#else
	add	pc, pc, \base, lsl \shift
Linus Torvalds's avatar
Linus Torvalds committed
244
	mov	r0, r0
245
246
247
248
249
250
#endif
1:
	.endm

ENTRY(vfp_get_float)
	tbl_branch r0, r3, #3
Linus Torvalds's avatar
Linus Torvalds committed
251
	.irp	dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
252
1:	mrc	p10, 0, r0, c\dr, c0, 0	@ fmrs	r0, s0
Linus Torvalds's avatar
Linus Torvalds committed
253
	mov	pc, lr
254
255
	.org	1b + 8
1:	mrc	p10, 0, r0, c\dr, c0, 4	@ fmrs	r0, s1
Linus Torvalds's avatar
Linus Torvalds committed
256
	mov	pc, lr
257
	.org	1b + 8
Linus Torvalds's avatar
Linus Torvalds committed
258
	.endr
259
ENDPROC(vfp_get_float)
Linus Torvalds's avatar
Linus Torvalds committed
260

261
ENTRY(vfp_put_float)
262
	tbl_branch r1, r3, #3
Linus Torvalds's avatar
Linus Torvalds committed
263
	.irp	dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
264
1:	mcr	p10, 0, r0, c\dr, c0, 0	@ fmsr	r0, s0
Linus Torvalds's avatar
Linus Torvalds committed
265
	mov	pc, lr
266
267
	.org	1b + 8
1:	mcr	p10, 0, r0, c\dr, c0, 4	@ fmsr	r0, s1
Linus Torvalds's avatar
Linus Torvalds committed
268
	mov	pc, lr
269
	.org	1b + 8
Linus Torvalds's avatar
Linus Torvalds committed
270
	.endr
271
ENDPROC(vfp_put_float)
Linus Torvalds's avatar
Linus Torvalds committed
272

273
ENTRY(vfp_get_double)
274
	tbl_branch r0, r3, #3
Linus Torvalds's avatar
Linus Torvalds committed
275
	.irp	dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
276
1:	fmrrd	r0, r1, d\dr
Linus Torvalds's avatar
Linus Torvalds committed
277
	mov	pc, lr
278
	.org	1b + 8
Linus Torvalds's avatar
Linus Torvalds committed
279
	.endr
280
281
282
#ifdef CONFIG_VFPv3
	@ d16 - d31 registers
	.irp	dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
283
1:	mrrc	p11, 3, r0, r1, c\dr	@ fmrrd	r0, r1, d\dr
284
	mov	pc, lr
285
	.org	1b + 8
286
287
	.endr
#endif
Linus Torvalds's avatar
Linus Torvalds committed
288

289
	@ virtual register 16 (or 32 if VFPv3) for compare with zero
Linus Torvalds's avatar
Linus Torvalds committed
290
291
292
	mov	r0, #0
	mov	r1, #0
	mov	pc, lr
293
ENDPROC(vfp_get_double)
Linus Torvalds's avatar
Linus Torvalds committed
294

295
ENTRY(vfp_put_double)
296
	tbl_branch r2, r3, #3
Linus Torvalds's avatar
Linus Torvalds committed
297
	.irp	dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
298
1:	fmdrr	d\dr, r0, r1
Linus Torvalds's avatar
Linus Torvalds committed
299
	mov	pc, lr
300
	.org	1b + 8
Linus Torvalds's avatar
Linus Torvalds committed
301
	.endr
302
303
304
#ifdef CONFIG_VFPv3
	@ d16 - d31 registers
	.irp	dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
305
1:	mcrr	p11, 3, r0, r1, c\dr	@ fmdrr	r0, r1, d\dr
306
	mov	pc, lr
307
	.org	1b + 8
308
309
	.endr
#endif
310
ENDPROC(vfp_put_double)