Orange Pi5 kernel

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