vfphw.S 7.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285
  1. /*
  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. .macro DBGSTR, str
  21. #ifdef DEBUG
  22. stmfd sp!, {r0-r3, ip, lr}
  23. add r0, pc, #4
  24. bl printk
  25. b 1f
  26. .asciz "<7>VFP: \str\n"
  27. .balign 4
  28. 1: ldmfd sp!, {r0-r3, ip, lr}
  29. #endif
  30. .endm
  31. .macro DBGSTR1, str, arg
  32. #ifdef DEBUG
  33. stmfd sp!, {r0-r3, ip, lr}
  34. mov r1, \arg
  35. add r0, pc, #4
  36. bl printk
  37. b 1f
  38. .asciz "<7>VFP: \str\n"
  39. .balign 4
  40. 1: ldmfd sp!, {r0-r3, ip, lr}
  41. #endif
  42. .endm
  43. .macro DBGSTR3, str, arg1, arg2, arg3
  44. #ifdef DEBUG
  45. stmfd sp!, {r0-r3, ip, lr}
  46. mov r3, \arg3
  47. mov r2, \arg2
  48. mov r1, \arg1
  49. add r0, pc, #4
  50. bl printk
  51. b 1f
  52. .asciz "<7>VFP: \str\n"
  53. .balign 4
  54. 1: ldmfd sp!, {r0-r3, ip, lr}
  55. #endif
  56. .endm
  57. @ VFP hardware support entry point.
  58. @
  59. @ r0 = faulted instruction
  60. @ r2 = faulted PC+4
  61. @ r9 = successful return
  62. @ r10 = vfp_state union
  63. @ r11 = CPU number
  64. @ lr = failure return
  65. ENTRY(vfp_support_entry)
  66. DBGSTR3 "instr %08x pc %08x state %p", r0, r2, r10
  67. VFPFMRX r1, FPEXC @ Is the VFP enabled?
  68. DBGSTR1 "fpexc %08x", r1
  69. tst r1, #FPEXC_EN
  70. bne look_for_VFP_exceptions @ VFP is already enabled
  71. DBGSTR1 "enable %x", r10
  72. ldr r3, last_VFP_context_address
  73. orr r1, r1, #FPEXC_EN @ user FPEXC has the enable bit set
  74. ldr r4, [r3, r11, lsl #2] @ last_VFP_context pointer
  75. bic r5, r1, #FPEXC_EX @ make sure exceptions are disabled
  76. cmp r4, r10
  77. beq check_for_exception @ we are returning to the same
  78. @ process, so the registers are
  79. @ still there. In this case, we do
  80. @ not want to drop a pending exception.
  81. VFPFMXR FPEXC, r5 @ enable VFP, disable any pending
  82. @ exceptions, so we can get at the
  83. @ rest of it
  84. #ifndef CONFIG_SMP
  85. @ Save out the current registers to the old thread state
  86. @ No need for SMP since this is not done lazily
  87. DBGSTR1 "save old state %p", r4
  88. cmp r4, #0
  89. beq no_old_VFP_process
  90. VFPFSTMIA r4, r5 @ save the working registers
  91. VFPFMRX r5, FPSCR @ current status
  92. #ifndef CONFIG_CPU_FEROCEON
  93. tst r1, #FPEXC_EX @ is there additional state to save?
  94. beq 1f
  95. VFPFMRX r6, FPINST @ FPINST (only if FPEXC.EX is set)
  96. tst r1, #FPEXC_FP2V @ is there an FPINST2 to read?
  97. beq 1f
  98. VFPFMRX r8, FPINST2 @ FPINST2 if needed (and present)
  99. 1:
  100. #endif
  101. stmia r4, {r1, r5, r6, r8} @ save FPEXC, FPSCR, FPINST, FPINST2
  102. @ and point r4 at the word at the
  103. @ start of the register dump
  104. #endif
  105. no_old_VFP_process:
  106. DBGSTR1 "load state %p", r10
  107. str r10, [r3, r11, lsl #2] @ update the last_VFP_context pointer
  108. @ Load the saved state back into the VFP
  109. VFPFLDMIA r10, r5 @ reload the working registers while
  110. @ FPEXC is in a safe state
  111. ldmia r10, {r1, r5, r6, r8} @ load FPEXC, FPSCR, FPINST, FPINST2
  112. #ifndef CONFIG_CPU_FEROCEON
  113. tst r1, #FPEXC_EX @ is there additional state to restore?
  114. beq 1f
  115. VFPFMXR FPINST, r6 @ restore FPINST (only if FPEXC.EX is set)
  116. tst r1, #FPEXC_FP2V @ is there an FPINST2 to write?
  117. beq 1f
  118. VFPFMXR FPINST2, r8 @ FPINST2 if needed (and present)
  119. 1:
  120. #endif
  121. VFPFMXR FPSCR, r5 @ restore status
  122. check_for_exception:
  123. tst r1, #FPEXC_EX
  124. bne process_exception @ might as well handle the pending
  125. @ exception before retrying branch
  126. @ out before setting an FPEXC that
  127. @ stops us reading stuff
  128. VFPFMXR FPEXC, r1 @ restore FPEXC last
  129. sub r2, r2, #4
  130. str r2, [sp, #S_PC] @ retry the instruction
  131. #ifdef CONFIG_PREEMPT
  132. get_thread_info r10
  133. ldr r4, [r10, #TI_PREEMPT] @ get preempt count
  134. sub r11, r4, #1 @ decrement it
  135. str r11, [r10, #TI_PREEMPT]
  136. #endif
  137. mov pc, r9 @ we think we have handled things
  138. look_for_VFP_exceptions:
  139. @ Check for synchronous or asynchronous exception
  140. tst r1, #FPEXC_EX | FPEXC_DEX
  141. bne process_exception
  142. @ On some implementations of the VFP subarch 1, setting FPSCR.IXE
  143. @ causes all the CDP instructions to be bounced synchronously without
  144. @ setting the FPEXC.EX bit
  145. VFPFMRX r5, FPSCR
  146. tst r5, #FPSCR_IXE
  147. bne process_exception
  148. @ Fall into hand on to next handler - appropriate coproc instr
  149. @ not recognised by VFP
  150. DBGSTR "not VFP"
  151. #ifdef CONFIG_PREEMPT
  152. get_thread_info r10
  153. ldr r4, [r10, #TI_PREEMPT] @ get preempt count
  154. sub r11, r4, #1 @ decrement it
  155. str r11, [r10, #TI_PREEMPT]
  156. #endif
  157. mov pc, lr
  158. process_exception:
  159. DBGSTR "bounce"
  160. mov r2, sp @ nothing stacked - regdump is at TOS
  161. mov lr, r9 @ setup for a return to the user code.
  162. @ Now call the C code to package up the bounce to the support code
  163. @ r0 holds the trigger instruction
  164. @ r1 holds the FPEXC value
  165. @ r2 pointer to register dump
  166. b VFP_bounce @ we have handled this - the support
  167. @ code will raise an exception if
  168. @ required. If not, the user code will
  169. @ retry the faulted instruction
  170. ENDPROC(vfp_support_entry)
  171. ENTRY(vfp_save_state)
  172. @ Save the current VFP state
  173. @ r0 - save location
  174. @ r1 - FPEXC
  175. DBGSTR1 "save VFP state %p", r0
  176. VFPFSTMIA r0, r2 @ save the working registers
  177. VFPFMRX r2, FPSCR @ current status
  178. tst r1, #FPEXC_EX @ is there additional state to save?
  179. beq 1f
  180. VFPFMRX r3, FPINST @ FPINST (only if FPEXC.EX is set)
  181. tst r1, #FPEXC_FP2V @ is there an FPINST2 to read?
  182. beq 1f
  183. VFPFMRX r12, FPINST2 @ FPINST2 if needed (and present)
  184. 1:
  185. stmia r0, {r1, r2, r3, r12} @ save FPEXC, FPSCR, FPINST, FPINST2
  186. mov pc, lr
  187. ENDPROC(vfp_save_state)
  188. last_VFP_context_address:
  189. .word last_VFP_context
  190. .macro tbl_branch, base, tmp, shift
  191. #ifdef CONFIG_THUMB2_KERNEL
  192. adr \tmp, 1f
  193. add \tmp, \tmp, \base, lsl \shift
  194. mov pc, \tmp
  195. #else
  196. add pc, pc, \base, lsl \shift
  197. mov r0, r0
  198. #endif
  199. 1:
  200. .endm
  201. ENTRY(vfp_get_float)
  202. tbl_branch r0, r3, #3
  203. .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
  204. 1: mrc p10, 0, r0, c\dr, c0, 0 @ fmrs r0, s0
  205. mov pc, lr
  206. .org 1b + 8
  207. 1: mrc p10, 0, r0, c\dr, c0, 4 @ fmrs r0, s1
  208. mov pc, lr
  209. .org 1b + 8
  210. .endr
  211. ENDPROC(vfp_get_float)
  212. ENTRY(vfp_put_float)
  213. tbl_branch r1, r3, #3
  214. .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
  215. 1: mcr p10, 0, r0, c\dr, c0, 0 @ fmsr r0, s0
  216. mov pc, lr
  217. .org 1b + 8
  218. 1: mcr p10, 0, r0, c\dr, c0, 4 @ fmsr r0, s1
  219. mov pc, lr
  220. .org 1b + 8
  221. .endr
  222. ENDPROC(vfp_put_float)
  223. ENTRY(vfp_get_double)
  224. tbl_branch r0, r3, #3
  225. .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
  226. 1: fmrrd r0, r1, d\dr
  227. mov pc, lr
  228. .org 1b + 8
  229. .endr
  230. #ifdef CONFIG_VFPv3
  231. @ d16 - d31 registers
  232. .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
  233. 1: mrrc p11, 3, r0, r1, c\dr @ fmrrd r0, r1, d\dr
  234. mov pc, lr
  235. .org 1b + 8
  236. .endr
  237. #endif
  238. @ virtual register 16 (or 32 if VFPv3) for compare with zero
  239. mov r0, #0
  240. mov r1, #0
  241. mov pc, lr
  242. ENDPROC(vfp_get_double)
  243. ENTRY(vfp_put_double)
  244. tbl_branch r2, r3, #3
  245. .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
  246. 1: fmdrr d\dr, r0, r1
  247. mov pc, lr
  248. .org 1b + 8
  249. .endr
  250. #ifdef CONFIG_VFPv3
  251. @ d16 - d31 registers
  252. .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
  253. 1: mcrr p11, 3, r1, r2, c\dr @ fmdrr r1, r2, d\dr
  254. mov pc, lr
  255. .org 1b + 8
  256. .endr
  257. #endif
  258. ENDPROC(vfp_put_double)