vfphw.S 6.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233
  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. .globl vfp_support_entry
  66. vfp_support_entry:
  67. DBGSTR3 "instr %08x pc %08x state %p", r0, r2, r10
  68. VFPFMRX r1, FPEXC @ Is the VFP enabled?
  69. DBGSTR1 "fpexc %08x", r1
  70. tst r1, #FPEXC_ENABLE
  71. bne look_for_VFP_exceptions @ VFP is already enabled
  72. DBGSTR1 "enable %x", r10
  73. ldr r3, last_VFP_context_address
  74. orr r1, r1, #FPEXC_ENABLE @ user FPEXC has the enable bit set
  75. ldr r4, [r3, r11, lsl #2] @ last_VFP_context pointer
  76. bic r5, r1, #FPEXC_EXCEPTION @ make sure exceptions are disabled
  77. cmp r4, r10
  78. beq check_for_exception @ we are returning to the same
  79. @ process, so the registers are
  80. @ still there. In this case, we do
  81. @ not want to drop a pending exception.
  82. VFPFMXR FPEXC, r5 @ enable VFP, disable any pending
  83. @ exceptions, so we can get at the
  84. @ rest of it
  85. #ifndef CONFIG_SMP
  86. @ Save out the current registers to the old thread state
  87. @ No need for SMP since this is not done lazily
  88. DBGSTR1 "save old state %p", r4
  89. cmp r4, #0
  90. beq no_old_VFP_process
  91. VFPFMRX r5, FPSCR @ current status
  92. VFPFMRX r6, FPINST @ FPINST (always there, rev0 onwards)
  93. tst r1, #FPEXC_FPV2 @ is there an FPINST2 to read?
  94. VFPFMRX r8, FPINST2, NE @ FPINST2 if needed - avoids reading
  95. @ nonexistant reg on rev0
  96. VFPFSTMIA r4 @ save the working registers
  97. stmia r4, {r1, r5, r6, r8} @ save FPEXC, FPSCR, FPINST, FPINST2
  98. @ and point r4 at the word at the
  99. @ start of the register dump
  100. #endif
  101. no_old_VFP_process:
  102. DBGSTR1 "load state %p", r10
  103. str r10, [r3, r11, lsl #2] @ update the last_VFP_context pointer
  104. @ Load the saved state back into the VFP
  105. VFPFLDMIA r10 @ reload the working registers while
  106. @ FPEXC is in a safe state
  107. ldmia r10, {r1, r5, r6, r8} @ load FPEXC, FPSCR, FPINST, FPINST2
  108. tst r1, #FPEXC_FPV2 @ is there an FPINST2 to write?
  109. VFPFMXR FPINST2, r8, NE @ FPINST2 if needed - avoids writing
  110. @ nonexistant reg on rev0
  111. VFPFMXR FPINST, r6
  112. VFPFMXR FPSCR, r5 @ restore status
  113. check_for_exception:
  114. tst r1, #FPEXC_EXCEPTION
  115. bne process_exception @ might as well handle the pending
  116. @ exception before retrying branch
  117. @ out before setting an FPEXC that
  118. @ stops us reading stuff
  119. VFPFMXR FPEXC, r1 @ restore FPEXC last
  120. sub r2, r2, #4
  121. str r2, [sp, #S_PC] @ retry the instruction
  122. mov pc, r9 @ we think we have handled things
  123. look_for_VFP_exceptions:
  124. tst r1, #FPEXC_EXCEPTION
  125. bne process_exception
  126. VFPFMRX r5, FPSCR
  127. tst r5, #FPSCR_IXE @ IXE doesn't set FPEXC_EXCEPTION !
  128. bne process_exception
  129. @ Fall into hand on to next handler - appropriate coproc instr
  130. @ not recognised by VFP
  131. DBGSTR "not VFP"
  132. mov pc, lr
  133. process_exception:
  134. DBGSTR "bounce"
  135. sub r2, r2, #4
  136. str r2, [sp, #S_PC] @ retry the instruction on exit from
  137. @ the imprecise exception handling in
  138. @ the support code
  139. mov r2, sp @ nothing stacked - regdump is at TOS
  140. mov lr, r9 @ setup for a return to the user code.
  141. @ Now call the C code to package up the bounce to the support code
  142. @ r0 holds the trigger instruction
  143. @ r1 holds the FPEXC value
  144. @ r2 pointer to register dump
  145. b VFP9_bounce @ we have handled this - the support
  146. @ code will raise an exception if
  147. @ required. If not, the user code will
  148. @ retry the faulted instruction
  149. #ifdef CONFIG_SMP
  150. .globl vfp_save_state
  151. .type vfp_save_state, %function
  152. vfp_save_state:
  153. @ Save the current VFP state
  154. @ r0 - save location
  155. @ r1 - FPEXC
  156. DBGSTR1 "save VFP state %p", r0
  157. VFPFMRX r2, FPSCR @ current status
  158. VFPFMRX r3, FPINST @ FPINST (always there, rev0 onwards)
  159. tst r1, #FPEXC_FPV2 @ is there an FPINST2 to read?
  160. VFPFMRX r12, FPINST2, NE @ FPINST2 if needed - avoids reading
  161. @ nonexistant reg on rev0
  162. VFPFSTMIA r0 @ save the working registers
  163. stmia r0, {r1, r2, r3, r12} @ save FPEXC, FPSCR, FPINST, FPINST2
  164. mov pc, lr
  165. #endif
  166. last_VFP_context_address:
  167. .word last_VFP_context
  168. .globl vfp_get_float
  169. vfp_get_float:
  170. add pc, pc, r0, lsl #3
  171. mov r0, r0
  172. .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
  173. mrc p10, 0, r0, c\dr, c0, 0 @ fmrs r0, s0
  174. mov pc, lr
  175. mrc p10, 0, r0, c\dr, c0, 4 @ fmrs r0, s1
  176. mov pc, lr
  177. .endr
  178. .globl vfp_put_float
  179. vfp_put_float:
  180. add pc, pc, r1, lsl #3
  181. mov r0, r0
  182. .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
  183. mcr p10, 0, r0, c\dr, c0, 0 @ fmsr r0, s0
  184. mov pc, lr
  185. mcr p10, 0, r0, c\dr, c0, 4 @ fmsr r0, s1
  186. mov pc, lr
  187. .endr
  188. .globl vfp_get_double
  189. vfp_get_double:
  190. add pc, pc, r0, lsl #3
  191. mov r0, r0
  192. .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
  193. fmrrd r0, r1, d\dr
  194. mov pc, lr
  195. .endr
  196. @ virtual register 16 for compare with zero
  197. mov r0, #0
  198. mov r1, #0
  199. mov pc, lr
  200. .globl vfp_put_double
  201. vfp_put_double:
  202. add pc, pc, r2, lsl #3
  203. mov r0, r0
  204. .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
  205. fmdrr d\dr, r0, r1
  206. mov pc, lr
  207. .endr