xref: /openbmc/linux/arch/arm/vfp/vfphw.S (revision a5b2c10c) 
1/* SPDX-License-Identifier: GPL-2.0-only */
2/*
3 *  linux/arch/arm/vfp/vfphw.S
4 *
5 *  Copyright (C) 2004 ARM Limited.
6 *  Written by Deep Blue Solutions Limited.
7 *
8 * This code is called from the kernel's undefined instruction trap.
9 * r9 holds the return address for successful handling.
10 * lr holds the return address for unrecognised instructions.
11 * r10 points at the start of the private FP workspace in the thread structure
12 * sp points to a struct pt_regs (as defined in include/asm/proc/ptrace.h)
13 */
14#include <linux/init.h>
15#include <linux/linkage.h>
16#include <asm/thread_info.h>
17#include <asm/vfpmacros.h>
18#include <linux/kern_levels.h>
19#include <asm/assembler.h>
20#include <asm/asm-offsets.h>
21
22	.macro	DBGSTR, str
23#ifdef DEBUG
24	stmfd	sp!, {r0-r3, ip, lr}
25	ldr	r0, =1f
26	bl	printk
27	ldmfd	sp!, {r0-r3, ip, lr}
28
29	.pushsection .rodata, "a"
301:	.ascii	KERN_DEBUG "VFP: \str\n"
31	.byte	0
32	.previous
33#endif
34	.endm
35
36	.macro  DBGSTR1, str, arg
37#ifdef DEBUG
38	stmfd	sp!, {r0-r3, ip, lr}
39	mov	r1, \arg
40	ldr	r0, =1f
41	bl	printk
42	ldmfd	sp!, {r0-r3, ip, lr}
43
44	.pushsection .rodata, "a"
451:	.ascii	KERN_DEBUG "VFP: \str\n"
46	.byte	0
47	.previous
48#endif
49	.endm
50
51	.macro  DBGSTR3, str, arg1, arg2, arg3
52#ifdef DEBUG
53	stmfd	sp!, {r0-r3, ip, lr}
54	mov	r3, \arg3
55	mov	r2, \arg2
56	mov	r1, \arg1
57	ldr	r0, =1f
58	bl	printk
59	ldmfd	sp!, {r0-r3, ip, lr}
60
61	.pushsection .rodata, "a"
621:	.ascii	KERN_DEBUG "VFP: \str\n"
63	.byte	0
64	.previous
65#endif
66	.endm
67
68
69@ VFP hardware support entry point.
70@
71@  r0  = instruction opcode (32-bit ARM or two 16-bit Thumb)
72@  r2  = PC value to resume execution after successful emulation
73@  r9  = normal "successful" return address
74@  r10 = vfp_state union
75@  r11 = CPU number
76@  lr  = unrecognised instruction return address
77@  IRQs enabled.
78ENTRY(vfp_support_entry)
79	DBGSTR3	"instr %08x pc %08x state %p", r0, r2, r10
80
81	.fpu	vfpv2
82	ldr	r3, [sp, #S_PSR]	@ Neither lazy restore nor FP exceptions
83	and	r3, r3, #MODE_MASK	@ are supported in kernel mode
84	teq	r3, #USR_MODE
85	bne	vfp_kmode_exception	@ Returns through lr
86
87	VFPFMRX	r1, FPEXC		@ Is the VFP enabled?
88	DBGSTR1	"fpexc %08x", r1
89	tst	r1, #FPEXC_EN
90	bne	look_for_VFP_exceptions	@ VFP is already enabled
91
92	DBGSTR1 "enable %x", r10
93	ldr	r3, vfp_current_hw_state_address
94	orr	r1, r1, #FPEXC_EN	@ user FPEXC has the enable bit set
95	ldr	r4, [r3, r11, lsl #2]	@ vfp_current_hw_state pointer
96	bic	r5, r1, #FPEXC_EX	@ make sure exceptions are disabled
97	cmp	r4, r10			@ this thread owns the hw context?
98#ifndef CONFIG_SMP
99	@ For UP, checking that this thread owns the hw context is
100	@ sufficient to determine that the hardware state is valid.
101	beq	vfp_hw_state_valid
102
103	@ On UP, we lazily save the VFP context.  As a different
104	@ thread wants ownership of the VFP hardware, save the old
105	@ state if there was a previous (valid) owner.
106
107	VFPFMXR	FPEXC, r5		@ enable VFP, disable any pending
108					@ exceptions, so we can get at the
109					@ rest of it
110
111	DBGSTR1	"save old state %p", r4
112	cmp	r4, #0			@ if the vfp_current_hw_state is NULL
113	beq	vfp_reload_hw		@ then the hw state needs reloading
114	VFPFSTMIA r4, r5		@ save the working registers
115	VFPFMRX	r5, FPSCR		@ current status
116#ifndef CONFIG_CPU_FEROCEON
117	tst	r1, #FPEXC_EX		@ is there additional state to save?
118	beq	1f
119	VFPFMRX	r6, FPINST		@ FPINST (only if FPEXC.EX is set)
120	tst	r1, #FPEXC_FP2V		@ is there an FPINST2 to read?
121	beq	1f
122	VFPFMRX	r8, FPINST2		@ FPINST2 if needed (and present)
1231:
124#endif
125	stmia	r4, {r1, r5, r6, r8}	@ save FPEXC, FPSCR, FPINST, FPINST2
126vfp_reload_hw:
127
128#else
129	@ For SMP, if this thread does not own the hw context, then we
130	@ need to reload it.  No need to save the old state as on SMP,
131	@ we always save the state when we switch away from a thread.
132	bne	vfp_reload_hw
133
134	@ This thread has ownership of the current hardware context.
135	@ However, it may have been migrated to another CPU, in which
136	@ case the saved state is newer than the hardware context.
137	@ Check this by looking at the CPU number which the state was
138	@ last loaded onto.
139	ldr	ip, [r10, #VFP_CPU]
140	teq	ip, r11
141	beq	vfp_hw_state_valid
142
143vfp_reload_hw:
144	@ We're loading this threads state into the VFP hardware. Update
145	@ the CPU number which contains the most up to date VFP context.
146	str	r11, [r10, #VFP_CPU]
147
148	VFPFMXR	FPEXC, r5		@ enable VFP, disable any pending
149					@ exceptions, so we can get at the
150					@ rest of it
151#endif
152
153	DBGSTR1	"load state %p", r10
154	str	r10, [r3, r11, lsl #2]	@ update the vfp_current_hw_state pointer
155					@ Load the saved state back into the VFP
156	VFPFLDMIA r10, r5		@ reload the working registers while
157					@ FPEXC is in a safe state
158	ldmia	r10, {r1, r5, r6, r8}	@ load FPEXC, FPSCR, FPINST, FPINST2
159#ifndef CONFIG_CPU_FEROCEON
160	tst	r1, #FPEXC_EX		@ is there additional state to restore?
161	beq	1f
162	VFPFMXR	FPINST, r6		@ restore FPINST (only if FPEXC.EX is set)
163	tst	r1, #FPEXC_FP2V		@ is there an FPINST2 to write?
164	beq	1f
165	VFPFMXR	FPINST2, r8		@ FPINST2 if needed (and present)
1661:
167#endif
168	VFPFMXR	FPSCR, r5		@ restore status
169
170@ The context stored in the VFP hardware is up to date with this thread
171vfp_hw_state_valid:
172	tst	r1, #FPEXC_EX
173	bne	process_exception	@ might as well handle the pending
174					@ exception before retrying branch
175					@ out before setting an FPEXC that
176					@ stops us reading stuff
177	VFPFMXR	FPEXC, r1		@ Restore FPEXC last
178	sub	r2, r2, #4		@ Retry current instruction - if Thumb
179	str	r2, [sp, #S_PC]		@ mode it's two 16-bit instructions,
180					@ else it's one 32-bit instruction, so
181					@ always subtract 4 from the following
182					@ instruction address.
183	dec_preempt_count_ti r10, r4
184	ret	r9			@ we think we have handled things
185
186
187look_for_VFP_exceptions:
188	@ Check for synchronous or asynchronous exception
189	tst	r1, #FPEXC_EX | FPEXC_DEX
190	bne	process_exception
191	@ On some implementations of the VFP subarch 1, setting FPSCR.IXE
192	@ causes all the CDP instructions to be bounced synchronously without
193	@ setting the FPEXC.EX bit
194	VFPFMRX	r5, FPSCR
195	tst	r5, #FPSCR_IXE
196	bne	process_exception
197
198	tst	r5, #FPSCR_LENGTH_MASK
199	beq	skip
200	orr	r1, r1, #FPEXC_DEX
201	b	process_exception
202skip:
203
204	@ Fall into hand on to next handler - appropriate coproc instr
205	@ not recognised by VFP
206
207	DBGSTR	"not VFP"
208	dec_preempt_count_ti r10, r4
209	ret	lr
210
211process_exception:
212	DBGSTR	"bounce"
213	mov	r2, sp			@ nothing stacked - regdump is at TOS
214	mov	lr, r9			@ setup for a return to the user code.
215
216	@ Now call the C code to package up the bounce to the support code
217	@   r0 holds the trigger instruction
218	@   r1 holds the FPEXC value
219	@   r2 pointer to register dump
220	b	VFP_bounce		@ we have handled this - the support
221					@ code will raise an exception if
222					@ required. If not, the user code will
223					@ retry the faulted instruction
224ENDPROC(vfp_support_entry)
225
226ENTRY(vfp_save_state)
227	@ Save the current VFP state
228	@ r0 - save location
229	@ r1 - FPEXC
230	DBGSTR1	"save VFP state %p", r0
231	VFPFSTMIA r0, r2		@ save the working registers
232	VFPFMRX	r2, FPSCR		@ current status
233	tst	r1, #FPEXC_EX		@ is there additional state to save?
234	beq	1f
235	VFPFMRX	r3, FPINST		@ FPINST (only if FPEXC.EX is set)
236	tst	r1, #FPEXC_FP2V		@ is there an FPINST2 to read?
237	beq	1f
238	VFPFMRX	r12, FPINST2		@ FPINST2 if needed (and present)
2391:
240	stmia	r0, {r1, r2, r3, r12}	@ save FPEXC, FPSCR, FPINST, FPINST2
241	ret	lr
242ENDPROC(vfp_save_state)
243
244	.align
245vfp_current_hw_state_address:
246	.word	vfp_current_hw_state
247
248	.macro	tbl_branch, base, tmp, shift
249#ifdef CONFIG_THUMB2_KERNEL
250	adr	\tmp, 1f
251	add	\tmp, \tmp, \base, lsl \shift
252	ret	\tmp
253#else
254	add	pc, pc, \base, lsl \shift
255	mov	r0, r0
256#endif
2571:
258	.endm
259
260ENTRY(vfp_get_float)
261	tbl_branch r0, r3, #3
262	.fpu	vfpv2
263	.irp	dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
2641:	vmov	r0, s\dr
265	ret	lr
266	.org	1b + 8
267	.endr
268	.irp	dr,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31
2691:	vmov	r0, s\dr
270	ret	lr
271	.org	1b + 8
272	.endr
273ENDPROC(vfp_get_float)
274
275ENTRY(vfp_put_float)
276	tbl_branch r1, r3, #3
277	.fpu	vfpv2
278	.irp	dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
2791:	vmov	s\dr, r0
280	ret	lr
281	.org	1b + 8
282	.endr
283	.irp	dr,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31
2841:	vmov	s\dr, r0
285	ret	lr
286	.org	1b + 8
287	.endr
288ENDPROC(vfp_put_float)
289
290ENTRY(vfp_get_double)
291	tbl_branch r0, r3, #3
292	.fpu	vfpv2
293	.irp	dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
2941:	vmov	r0, r1, d\dr
295	ret	lr
296	.org	1b + 8
297	.endr
298#ifdef CONFIG_VFPv3
299	@ d16 - d31 registers
300	.fpu	vfpv3
301	.irp	dr,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31
3021:	vmov	r0, r1, d\dr
303	ret	lr
304	.org	1b + 8
305	.endr
306#endif
307
308	@ virtual register 16 (or 32 if VFPv3) for compare with zero
309	mov	r0, #0
310	mov	r1, #0
311	ret	lr
312ENDPROC(vfp_get_double)
313
314ENTRY(vfp_put_double)
315	tbl_branch r2, r3, #3
316	.fpu	vfpv2
317	.irp	dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
3181:	vmov	d\dr, r0, r1
319	ret	lr
320	.org	1b + 8
321	.endr
322#ifdef CONFIG_VFPv3
323	.fpu	vfpv3
324	@ d16 - d31 registers
325	.irp	dr,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31
3261:	vmov	d\dr, r0, r1
327	ret	lr
328	.org	1b + 8
329	.endr
330#endif
331ENDPROC(vfp_put_double)
332