blob: 66dc2d03b7fc70c1130bd7b0a751dbeed6d53b17 [file] [log] [blame]
Linus Torvalds1da177e2005-04-16 15:20:36 -07001/*
2 * linux/arch/arm/vfp/vfphw.S
3 *
4 * Copyright (C) 2004 ARM Limited.
5 * Written by Deep Blue Solutions Limited.
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License version 2 as
9 * published by the Free Software Foundation.
10 *
11 * This code is called from the kernel's undefined instruction trap.
12 * r9 holds the return address for successful handling.
13 * lr holds the return address for unrecognised instructions.
14 * r10 points at the start of the private FP workspace in the thread structure
15 * sp points to a struct pt_regs (as defined in include/asm/proc/ptrace.h)
16 */
17#include <asm/thread_info.h>
18#include <asm/vfpmacros.h>
19#include "../kernel/entry-header.S"
20
21 .macro DBGSTR, str
22#ifdef DEBUG
23 stmfd sp!, {r0-r3, ip, lr}
24 add r0, pc, #4
25 bl printk
26 b 1f
27 .asciz "<7>VFP: \str\n"
28 .balign 4
291: ldmfd sp!, {r0-r3, ip, lr}
30#endif
31 .endm
32
33 .macro DBGSTR1, str, arg
34#ifdef DEBUG
35 stmfd sp!, {r0-r3, ip, lr}
36 mov r1, \arg
37 add r0, pc, #4
38 bl printk
39 b 1f
40 .asciz "<7>VFP: \str\n"
41 .balign 4
421: ldmfd sp!, {r0-r3, ip, lr}
43#endif
44 .endm
45
46 .macro DBGSTR3, str, arg1, arg2, arg3
47#ifdef DEBUG
48 stmfd sp!, {r0-r3, ip, lr}
49 mov r3, \arg3
50 mov r2, \arg2
51 mov r1, \arg1
52 add r0, pc, #4
53 bl printk
54 b 1f
55 .asciz "<7>VFP: \str\n"
56 .balign 4
571: ldmfd sp!, {r0-r3, ip, lr}
58#endif
59 .endm
60
61
62@ VFP hardware support entry point.
63@
64@ r0 = faulted instruction
65@ r2 = faulted PC+4
66@ r9 = successful return
67@ r10 = vfp_state union
Catalin Marinasc6428462007-01-24 18:47:08 +010068@ r11 = CPU number
Linus Torvalds1da177e2005-04-16 15:20:36 -070069@ lr = failure return
70
Catalin Marinas93ed3972008-08-28 11:22:32 +010071ENTRY(vfp_support_entry)
Linus Torvalds1da177e2005-04-16 15:20:36 -070072 DBGSTR3 "instr %08x pc %08x state %p", r0, r2, r10
73
74 VFPFMRX r1, FPEXC @ Is the VFP enabled?
75 DBGSTR1 "fpexc %08x", r1
Russell King228adef2007-07-18 09:37:10 +010076 tst r1, #FPEXC_EN
Linus Torvalds1da177e2005-04-16 15:20:36 -070077 bne look_for_VFP_exceptions @ VFP is already enabled
78
79 DBGSTR1 "enable %x", r10
80 ldr r3, last_VFP_context_address
Russell King228adef2007-07-18 09:37:10 +010081 orr r1, r1, #FPEXC_EN @ user FPEXC has the enable bit set
Catalin Marinasc6428462007-01-24 18:47:08 +010082 ldr r4, [r3, r11, lsl #2] @ last_VFP_context pointer
Russell King228adef2007-07-18 09:37:10 +010083 bic r5, r1, #FPEXC_EX @ make sure exceptions are disabled
Linus Torvalds1da177e2005-04-16 15:20:36 -070084 cmp r4, r10
85 beq check_for_exception @ we are returning to the same
86 @ process, so the registers are
87 @ still there. In this case, we do
88 @ not want to drop a pending exception.
89
90 VFPFMXR FPEXC, r5 @ enable VFP, disable any pending
91 @ exceptions, so we can get at the
92 @ rest of it
93
Catalin Marinasc6428462007-01-24 18:47:08 +010094#ifndef CONFIG_SMP
Linus Torvalds1da177e2005-04-16 15:20:36 -070095 @ Save out the current registers to the old thread state
Catalin Marinasc6428462007-01-24 18:47:08 +010096 @ No need for SMP since this is not done lazily
Linus Torvalds1da177e2005-04-16 15:20:36 -070097
98 DBGSTR1 "save old state %p", r4
99 cmp r4, #0
100 beq no_old_VFP_process
Catalin Marinas25ebee02007-09-25 15:22:24 +0100101 VFPFSTMIA r4, r5 @ save the working registers
Linus Torvalds1da177e2005-04-16 15:20:36 -0700102 VFPFMRX r5, FPSCR @ current status
Catalin Marinas85d69432009-05-30 14:00:18 +0100103#ifndef CONFIG_CPU_FEROCEON
Catalin Marinasc98929c2007-11-22 18:32:01 +0100104 tst r1, #FPEXC_EX @ is there additional state to save?
Catalin Marinas24b647a2008-11-06 13:23:08 +0000105 beq 1f
106 VFPFMRX r6, FPINST @ FPINST (only if FPEXC.EX is set)
107 tst r1, #FPEXC_FP2V @ is there an FPINST2 to read?
108 beq 1f
109 VFPFMRX r8, FPINST2 @ FPINST2 if needed (and present)
1101:
Catalin Marinas85d69432009-05-30 14:00:18 +0100111#endif
Linus Torvalds1da177e2005-04-16 15:20:36 -0700112 stmia r4, {r1, r5, r6, r8} @ save FPEXC, FPSCR, FPINST, FPINST2
113 @ and point r4 at the word at the
114 @ start of the register dump
Catalin Marinasc6428462007-01-24 18:47:08 +0100115#endif
Linus Torvalds1da177e2005-04-16 15:20:36 -0700116
117no_old_VFP_process:
118 DBGSTR1 "load state %p", r10
Catalin Marinasc6428462007-01-24 18:47:08 +0100119 str r10, [r3, r11, lsl #2] @ update the last_VFP_context pointer
Linus Torvalds1da177e2005-04-16 15:20:36 -0700120 @ Load the saved state back into the VFP
Catalin Marinas25ebee02007-09-25 15:22:24 +0100121 VFPFLDMIA r10, r5 @ reload the working registers while
Linus Torvalds1da177e2005-04-16 15:20:36 -0700122 @ FPEXC is in a safe state
Catalin Marinas80ed35472006-03-25 21:58:00 +0000123 ldmia r10, {r1, r5, r6, r8} @ load FPEXC, FPSCR, FPINST, FPINST2
Catalin Marinas85d69432009-05-30 14:00:18 +0100124#ifndef CONFIG_CPU_FEROCEON
Catalin Marinasc98929c2007-11-22 18:32:01 +0100125 tst r1, #FPEXC_EX @ is there additional state to restore?
Catalin Marinas24b647a2008-11-06 13:23:08 +0000126 beq 1f
127 VFPFMXR FPINST, r6 @ restore FPINST (only if FPEXC.EX is set)
128 tst r1, #FPEXC_FP2V @ is there an FPINST2 to write?
129 beq 1f
130 VFPFMXR FPINST2, r8 @ FPINST2 if needed (and present)
1311:
Catalin Marinas85d69432009-05-30 14:00:18 +0100132#endif
Linus Torvalds1da177e2005-04-16 15:20:36 -0700133 VFPFMXR FPSCR, r5 @ restore status
134
135check_for_exception:
Russell King228adef2007-07-18 09:37:10 +0100136 tst r1, #FPEXC_EX
Linus Torvalds1da177e2005-04-16 15:20:36 -0700137 bne process_exception @ might as well handle the pending
138 @ exception before retrying branch
139 @ out before setting an FPEXC that
140 @ stops us reading stuff
141 VFPFMXR FPEXC, r1 @ restore FPEXC last
142 sub r2, r2, #4
143 str r2, [sp, #S_PC] @ retry the instruction
George G. Davisf2255be2009-04-01 20:27:18 +0100144#ifdef CONFIG_PREEMPT
145 get_thread_info r10
146 ldr r4, [r10, #TI_PREEMPT] @ get preempt count
147 sub r11, r4, #1 @ decrement it
148 str r11, [r10, #TI_PREEMPT]
149#endif
Linus Torvalds1da177e2005-04-16 15:20:36 -0700150 mov pc, r9 @ we think we have handled things
151
152
153look_for_VFP_exceptions:
Catalin Marinasc98929c2007-11-22 18:32:01 +0100154 @ Check for synchronous or asynchronous exception
155 tst r1, #FPEXC_EX | FPEXC_DEX
Linus Torvalds1da177e2005-04-16 15:20:36 -0700156 bne process_exception
Catalin Marinasc98929c2007-11-22 18:32:01 +0100157 @ On some implementations of the VFP subarch 1, setting FPSCR.IXE
158 @ causes all the CDP instructions to be bounced synchronously without
159 @ setting the FPEXC.EX bit
Linus Torvalds1da177e2005-04-16 15:20:36 -0700160 VFPFMRX r5, FPSCR
Catalin Marinasc98929c2007-11-22 18:32:01 +0100161 tst r5, #FPSCR_IXE
Linus Torvalds1da177e2005-04-16 15:20:36 -0700162 bne process_exception
163
164 @ Fall into hand on to next handler - appropriate coproc instr
165 @ not recognised by VFP
166
167 DBGSTR "not VFP"
George G. Davisf2255be2009-04-01 20:27:18 +0100168#ifdef CONFIG_PREEMPT
169 get_thread_info r10
170 ldr r4, [r10, #TI_PREEMPT] @ get preempt count
171 sub r11, r4, #1 @ decrement it
172 str r11, [r10, #TI_PREEMPT]
173#endif
Linus Torvalds1da177e2005-04-16 15:20:36 -0700174 mov pc, lr
175
176process_exception:
177 DBGSTR "bounce"
Linus Torvalds1da177e2005-04-16 15:20:36 -0700178 mov r2, sp @ nothing stacked - regdump is at TOS
179 mov lr, r9 @ setup for a return to the user code.
180
181 @ Now call the C code to package up the bounce to the support code
182 @ r0 holds the trigger instruction
183 @ r1 holds the FPEXC value
184 @ r2 pointer to register dump
Catalin Marinasc98929c2007-11-22 18:32:01 +0100185 b VFP_bounce @ we have handled this - the support
Linus Torvalds1da177e2005-04-16 15:20:36 -0700186 @ code will raise an exception if
187 @ required. If not, the user code will
188 @ retry the faulted instruction
Catalin Marinas93ed3972008-08-28 11:22:32 +0100189ENDPROC(vfp_support_entry)
Linus Torvalds1da177e2005-04-16 15:20:36 -0700190
Catalin Marinas93ed3972008-08-28 11:22:32 +0100191ENTRY(vfp_save_state)
Catalin Marinasc6428462007-01-24 18:47:08 +0100192 @ Save the current VFP state
193 @ r0 - save location
194 @ r1 - FPEXC
195 DBGSTR1 "save VFP state %p", r0
Catalin Marinas25ebee02007-09-25 15:22:24 +0100196 VFPFSTMIA r0, r2 @ save the working registers
Catalin Marinasc6428462007-01-24 18:47:08 +0100197 VFPFMRX r2, FPSCR @ current status
Catalin Marinasc98929c2007-11-22 18:32:01 +0100198 tst r1, #FPEXC_EX @ is there additional state to save?
Catalin Marinas24b647a2008-11-06 13:23:08 +0000199 beq 1f
200 VFPFMRX r3, FPINST @ FPINST (only if FPEXC.EX is set)
201 tst r1, #FPEXC_FP2V @ is there an FPINST2 to read?
202 beq 1f
203 VFPFMRX r12, FPINST2 @ FPINST2 if needed (and present)
2041:
Catalin Marinasc6428462007-01-24 18:47:08 +0100205 stmia r0, {r1, r2, r3, r12} @ save FPEXC, FPSCR, FPINST, FPINST2
206 mov pc, lr
Catalin Marinas93ed3972008-08-28 11:22:32 +0100207ENDPROC(vfp_save_state)
Catalin Marinasc6428462007-01-24 18:47:08 +0100208
Linus Torvalds1da177e2005-04-16 15:20:36 -0700209last_VFP_context_address:
210 .word last_VFP_context
211
Catalin Marinas07f33a02009-07-24 12:32:57 +0100212 .macro tbl_branch, base, tmp, shift
213#ifdef CONFIG_THUMB2_KERNEL
214 adr \tmp, 1f
215 add \tmp, \tmp, \base, lsl \shift
216 mov pc, \tmp
217#else
218 add pc, pc, \base, lsl \shift
Linus Torvalds1da177e2005-04-16 15:20:36 -0700219 mov r0, r0
Catalin Marinas07f33a02009-07-24 12:32:57 +0100220#endif
2211:
222 .endm
223
224ENTRY(vfp_get_float)
225 tbl_branch r0, r3, #3
Linus Torvalds1da177e2005-04-16 15:20:36 -0700226 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
Catalin Marinas07f33a02009-07-24 12:32:57 +01002271: mrc p10, 0, r0, c\dr, c0, 0 @ fmrs r0, s0
Linus Torvalds1da177e2005-04-16 15:20:36 -0700228 mov pc, lr
Catalin Marinas07f33a02009-07-24 12:32:57 +0100229 .org 1b + 8
2301: mrc p10, 0, r0, c\dr, c0, 4 @ fmrs r0, s1
Linus Torvalds1da177e2005-04-16 15:20:36 -0700231 mov pc, lr
Catalin Marinas07f33a02009-07-24 12:32:57 +0100232 .org 1b + 8
Linus Torvalds1da177e2005-04-16 15:20:36 -0700233 .endr
Catalin Marinas93ed3972008-08-28 11:22:32 +0100234ENDPROC(vfp_get_float)
Linus Torvalds1da177e2005-04-16 15:20:36 -0700235
Catalin Marinas93ed3972008-08-28 11:22:32 +0100236ENTRY(vfp_put_float)
Catalin Marinas07f33a02009-07-24 12:32:57 +0100237 tbl_branch r1, r3, #3
Linus Torvalds1da177e2005-04-16 15:20:36 -0700238 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
Catalin Marinas07f33a02009-07-24 12:32:57 +01002391: mcr p10, 0, r0, c\dr, c0, 0 @ fmsr r0, s0
Linus Torvalds1da177e2005-04-16 15:20:36 -0700240 mov pc, lr
Catalin Marinas07f33a02009-07-24 12:32:57 +0100241 .org 1b + 8
2421: mcr p10, 0, r0, c\dr, c0, 4 @ fmsr r0, s1
Linus Torvalds1da177e2005-04-16 15:20:36 -0700243 mov pc, lr
Catalin Marinas07f33a02009-07-24 12:32:57 +0100244 .org 1b + 8
Linus Torvalds1da177e2005-04-16 15:20:36 -0700245 .endr
Catalin Marinas93ed3972008-08-28 11:22:32 +0100246ENDPROC(vfp_put_float)
Linus Torvalds1da177e2005-04-16 15:20:36 -0700247
Catalin Marinas93ed3972008-08-28 11:22:32 +0100248ENTRY(vfp_get_double)
Catalin Marinas07f33a02009-07-24 12:32:57 +0100249 tbl_branch r0, r3, #3
Linus Torvalds1da177e2005-04-16 15:20:36 -0700250 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
Catalin Marinas07f33a02009-07-24 12:32:57 +01002511: fmrrd r0, r1, d\dr
Linus Torvalds1da177e2005-04-16 15:20:36 -0700252 mov pc, lr
Catalin Marinas07f33a02009-07-24 12:32:57 +0100253 .org 1b + 8
Linus Torvalds1da177e2005-04-16 15:20:36 -0700254 .endr
Catalin Marinas25ebee02007-09-25 15:22:24 +0100255#ifdef CONFIG_VFPv3
256 @ d16 - d31 registers
257 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
Catalin Marinas07f33a02009-07-24 12:32:57 +01002581: mrrc p11, 3, r0, r1, c\dr @ fmrrd r0, r1, d\dr
Catalin Marinas25ebee02007-09-25 15:22:24 +0100259 mov pc, lr
Catalin Marinas07f33a02009-07-24 12:32:57 +0100260 .org 1b + 8
Catalin Marinas25ebee02007-09-25 15:22:24 +0100261 .endr
262#endif
Linus Torvalds1da177e2005-04-16 15:20:36 -0700263
Catalin Marinas25ebee02007-09-25 15:22:24 +0100264 @ virtual register 16 (or 32 if VFPv3) for compare with zero
Linus Torvalds1da177e2005-04-16 15:20:36 -0700265 mov r0, #0
266 mov r1, #0
267 mov pc, lr
Catalin Marinas93ed3972008-08-28 11:22:32 +0100268ENDPROC(vfp_get_double)
Linus Torvalds1da177e2005-04-16 15:20:36 -0700269
Catalin Marinas93ed3972008-08-28 11:22:32 +0100270ENTRY(vfp_put_double)
Catalin Marinas07f33a02009-07-24 12:32:57 +0100271 tbl_branch r2, r3, #3
Linus Torvalds1da177e2005-04-16 15:20:36 -0700272 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
Catalin Marinas07f33a02009-07-24 12:32:57 +01002731: fmdrr d\dr, r0, r1
Linus Torvalds1da177e2005-04-16 15:20:36 -0700274 mov pc, lr
Catalin Marinas07f33a02009-07-24 12:32:57 +0100275 .org 1b + 8
Linus Torvalds1da177e2005-04-16 15:20:36 -0700276 .endr
Catalin Marinas25ebee02007-09-25 15:22:24 +0100277#ifdef CONFIG_VFPv3
278 @ d16 - d31 registers
279 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
Catalin Marinas07f33a02009-07-24 12:32:57 +01002801: mcrr p11, 3, r1, r2, c\dr @ fmdrr r1, r2, d\dr
Catalin Marinas25ebee02007-09-25 15:22:24 +0100281 mov pc, lr
Catalin Marinas07f33a02009-07-24 12:32:57 +0100282 .org 1b + 8
Catalin Marinas25ebee02007-09-25 15:22:24 +0100283 .endr
284#endif
Catalin Marinas93ed3972008-08-28 11:22:32 +0100285ENDPROC(vfp_put_double)