[deliverable/linux.git] / arch / powerpc / kernel / vector.S

#include <asm/processor.h>
#include <asm/ppc_asm.h>
#include <asm/reg.h>
#include <asm/asm-offsets.h>
#include <asm/cputable.h>
#include <asm/thread_info.h>
#include <asm/page.h>
#include <asm/ptrace.h>

/*
 * load_up_altivec(unused, unused, tsk)
 * Disable VMX for the task which had it previously,
 * and save its vector registers in its thread_struct.
 * Enables the VMX for use in the kernel on return.
 * On SMP we know the VMX is free, since we give it up every
 * switch (ie, no lazy save of the vector registers).
 */
_GLOBAL(load_up_altivec)
	mfmsr	r5			/* grab the current MSR */
	oris	r5,r5,MSR_VEC@h
	MTMSRD(r5)			/* enable use of AltiVec now */
	isync

/*
 * For SMP, we don't do lazy VMX switching because it just gets too
 * horrendously complex, especially when a task switches from one CPU
 * to another.  Instead we call giveup_altvec in switch_to.
 * VRSAVE isn't dealt with here, that is done in the normal context
 * switch code. Note that we could rely on vrsave value to eventually
 * avoid saving all of the VREGs here...
 */
#ifndef CONFIG_SMP
	LOAD_REG_ADDRBASE(r3, last_task_used_altivec)
	toreal(r3)
	PPC_LL	r4,ADDROFF(last_task_used_altivec)(r3)
	PPC_LCMPI	0,r4,0
	beq	1f

	/* Save VMX state to last_task_used_altivec's THREAD struct */
	toreal(r4)
	addi	r4,r4,THREAD
	SAVE_32VRS(0,r5,r4)
	mfvscr	vr0
	li	r10,THREAD_VSCR
	stvx	vr0,r10,r4
	/* Disable VMX for last_task_used_altivec */
	PPC_LL	r5,PT_REGS(r4)
	toreal(r5)
	PPC_LL	r4,_MSR-STACK_FRAME_OVERHEAD(r5)
	lis	r10,MSR_VEC@h
	andc	r4,r4,r10
	PPC_STL	r4,_MSR-STACK_FRAME_OVERHEAD(r5)
1:
#endif /* CONFIG_SMP */

	/* Hack: if we get an altivec unavailable trap with VRSAVE
	 * set to all zeros, we assume this is a broken application
	 * that fails to set it properly, and thus we switch it to
	 * all 1's
	 */
	mfspr	r4,SPRN_VRSAVE
	cmpwi	0,r4,0
	bne+	1f
	li	r4,-1
	mtspr	SPRN_VRSAVE,r4
1:
	/* enable use of VMX after return */
#ifdef CONFIG_PPC32
	mfspr	r5,SPRN_SPRG_THREAD		/* current task's THREAD (phys) */
	oris	r9,r9,MSR_VEC@h
#else
	ld	r4,PACACURRENT(r13)
	addi	r5,r4,THREAD		/* Get THREAD */
	oris	r12,r12,MSR_VEC@h
	std	r12,_MSR(r1)
#endif
	li	r4,1
	li	r10,THREAD_VSCR
	stw	r4,THREAD_USED_VR(r5)
	lvx	vr0,r10,r5
	mtvscr	vr0
	REST_32VRS(0,r4,r5)
#ifndef CONFIG_SMP
	/* Update last_task_used_altivec to 'current' */
	subi	r4,r5,THREAD		/* Back to 'current' */
	fromreal(r4)
	PPC_STL	r4,ADDROFF(last_task_used_altivec)(r3)
#endif /* CONFIG_SMP */
	/* restore registers and return */
	blr

/*
 * giveup_altivec(tsk)
 * Disable VMX for the task given as the argument,
 * and save the vector registers in its thread_struct.
 * Enables the VMX for use in the kernel on return.
 */
_GLOBAL(giveup_altivec)
	mfmsr	r5
	oris	r5,r5,MSR_VEC@h
	SYNC
	MTMSRD(r5)			/* enable use of VMX now */
	isync
	PPC_LCMPI	0,r3,0
	beqlr-				/* if no previous owner, done */
	addi	r3,r3,THREAD		/* want THREAD of task */
	PPC_LL	r5,PT_REGS(r3)
	PPC_LCMPI	0,r5,0
	SAVE_32VRS(0,r4,r3)
	mfvscr	vr0
	li	r4,THREAD_VSCR
	stvx	vr0,r4,r3
	beq	1f
	PPC_LL	r4,_MSR-STACK_FRAME_OVERHEAD(r5)
#ifdef CONFIG_VSX
BEGIN_FTR_SECTION
	lis	r3,(MSR_VEC|MSR_VSX)@h
FTR_SECTION_ELSE
	lis	r3,MSR_VEC@h
ALT_FTR_SECTION_END_IFSET(CPU_FTR_VSX)
#else
	lis	r3,MSR_VEC@h
#endif
	andc	r4,r4,r3		/* disable FP for previous task */
	PPC_STL	r4,_MSR-STACK_FRAME_OVERHEAD(r5)
1:
#ifndef CONFIG_SMP
	li	r5,0
	LOAD_REG_ADDRBASE(r4,last_task_used_altivec)
	PPC_STL	r5,ADDROFF(last_task_used_altivec)(r4)
#endif /* CONFIG_SMP */
	blr

#ifdef CONFIG_VSX

#ifdef CONFIG_PPC32
#error This asm code isn't ready for 32-bit kernels
#endif

/*
 * load_up_vsx(unused, unused, tsk)
 * Disable VSX for the task which had it previously,
 * and save its vector registers in its thread_struct.
 * Reuse the fp and vsx saves, but first check to see if they have
 * been saved already.
 */
_GLOBAL(load_up_vsx)
/* Load FP and VSX registers if they haven't been done yet */
	andi.	r5,r12,MSR_FP
	beql+	load_up_fpu		/* skip if already loaded */
	andis.	r5,r12,MSR_VEC@h
	beql+	load_up_altivec		/* skip if already loaded */

#ifndef CONFIG_SMP
	ld	r3,last_task_used_vsx@got(r2)
	ld	r4,0(r3)
	cmpdi	0,r4,0
	beq	1f
	/* Disable VSX for last_task_used_vsx */
	addi	r4,r4,THREAD
	ld	r5,PT_REGS(r4)
	ld	r4,_MSR-STACK_FRAME_OVERHEAD(r5)
	lis	r6,MSR_VSX@h
	andc	r6,r4,r6
	std	r6,_MSR-STACK_FRAME_OVERHEAD(r5)
1:
#endif /* CONFIG_SMP */
	ld	r4,PACACURRENT(r13)
	addi	r4,r4,THREAD		/* Get THREAD */
	li	r6,1
	stw	r6,THREAD_USED_VSR(r4) /* ... also set thread used vsr */
	/* enable use of VSX after return */
	oris	r12,r12,MSR_VSX@h
	std	r12,_MSR(r1)
#ifndef CONFIG_SMP
	/* Update last_task_used_vsx to 'current' */
	ld	r4,PACACURRENT(r13)
	std	r4,0(r3)
#endif /* CONFIG_SMP */
	b	fast_exception_return

/*
 * __giveup_vsx(tsk)
 * Disable VSX for the task given as the argument.
 * Does NOT save vsx registers.
 * Enables the VSX for use in the kernel on return.
 */
_GLOBAL(__giveup_vsx)
	mfmsr	r5
	oris	r5,r5,MSR_VSX@h
	mtmsrd	r5			/* enable use of VSX now */
	isync

	cmpdi	0,r3,0
	beqlr-				/* if no previous owner, done */
	addi	r3,r3,THREAD		/* want THREAD of task */
	ld	r5,PT_REGS(r3)
	cmpdi	0,r5,0
	beq	1f
	ld	r4,_MSR-STACK_FRAME_OVERHEAD(r5)
	lis	r3,MSR_VSX@h
	andc	r4,r4,r3		/* disable VSX for previous task */
	std	r4,_MSR-STACK_FRAME_OVERHEAD(r5)
1:
#ifndef CONFIG_SMP
	li	r5,0
	ld	r4,last_task_used_vsx@got(r2)
	std	r5,0(r4)
#endif /* CONFIG_SMP */
	blr

#endif /* CONFIG_VSX */


/*
 * The routines below are in assembler so we can closely control the
 * usage of floating-point registers.  These routines must be called
 * with preempt disabled.
 */
#ifdef CONFIG_PPC32
	.data
fpzero:
	.long	0
fpone:
	.long	0x3f800000	/* 1.0 in single-precision FP */
fphalf:
	.long	0x3f000000	/* 0.5 in single-precision FP */

#define LDCONST(fr, name)	\
	lis	r11,name@ha;	\
	lfs	fr,name@l(r11)
#else

	.section ".toc","aw"
fpzero:
	.tc	FD_0_0[TC],0
fpone:
	.tc	FD_3ff00000_0[TC],0x3ff0000000000000	/* 1.0 */
fphalf:
	.tc	FD_3fe00000_0[TC],0x3fe0000000000000	/* 0.5 */

#define LDCONST(fr, name)	\
	lfd	fr,name@toc(r2)
#endif

	.text
/*
 * Internal routine to enable floating point and set FPSCR to 0.
 * Don't call it from C; it doesn't use the normal calling convention.
 */
fpenable:
#ifdef CONFIG_PPC32
	stwu	r1,-64(r1)
#else
	stdu	r1,-64(r1)
#endif
	mfmsr	r10
	ori	r11,r10,MSR_FP
	mtmsr	r11
	isync
	stfd	fr0,24(r1)
	stfd	fr1,16(r1)
	stfd	fr31,8(r1)
	LDCONST(fr1, fpzero)
	mffs	fr31
	MTFSF_L(fr1)
	blr

fpdisable:
	mtlr	r12
	MTFSF_L(fr31)
	lfd	fr31,8(r1)
	lfd	fr1,16(r1)
	lfd	fr0,24(r1)
	mtmsr	r10
	isync
	addi	r1,r1,64
	blr

/*
 * Vector add, floating point.
 */
_GLOBAL(vaddfp)
	mflr	r12
	bl	fpenable
	li	r0,4
	mtctr	r0
	li	r6,0
1:	lfsx	fr0,r4,r6
	lfsx	fr1,r5,r6
	fadds	fr0,fr0,fr1
	stfsx	fr0,r3,r6
	addi	r6,r6,4
	bdnz	1b
	b	fpdisable

/*
 * Vector subtract, floating point.
 */
_GLOBAL(vsubfp)
	mflr	r12
	bl	fpenable
	li	r0,4
	mtctr	r0
	li	r6,0
1:	lfsx	fr0,r4,r6
	lfsx	fr1,r5,r6
	fsubs	fr0,fr0,fr1
	stfsx	fr0,r3,r6
	addi	r6,r6,4
	bdnz	1b
	b	fpdisable

/*
 * Vector multiply and add, floating point.
 */
_GLOBAL(vmaddfp)
	mflr	r12
	bl	fpenable
	stfd	fr2,32(r1)
	li	r0,4
	mtctr	r0
	li	r7,0
1:	lfsx	fr0,r4,r7
	lfsx	fr1,r5,r7
	lfsx	fr2,r6,r7
	fmadds	fr0,fr0,fr2,fr1
	stfsx	fr0,r3,r7
	addi	r7,r7,4
	bdnz	1b
	lfd	fr2,32(r1)
	b	fpdisable

/*
 * Vector negative multiply and subtract, floating point.
 */
_GLOBAL(vnmsubfp)
	mflr	r12
	bl	fpenable
	stfd	fr2,32(r1)
	li	r0,4
	mtctr	r0
	li	r7,0
1:	lfsx	fr0,r4,r7
	lfsx	fr1,r5,r7
	lfsx	fr2,r6,r7
	fnmsubs	fr0,fr0,fr2,fr1
	stfsx	fr0,r3,r7
	addi	r7,r7,4
	bdnz	1b
	lfd	fr2,32(r1)
	b	fpdisable

/*
 * Vector reciprocal estimate.  We just compute 1.0/x.
 * r3 -> destination, r4 -> source.
 */
_GLOBAL(vrefp)
	mflr	r12
	bl	fpenable
	li	r0,4
	LDCONST(fr1, fpone)
	mtctr	r0
	li	r6,0
1:	lfsx	fr0,r4,r6
	fdivs	fr0,fr1,fr0
	stfsx	fr0,r3,r6
	addi	r6,r6,4
	bdnz	1b
	b	fpdisable

/*
 * Vector reciprocal square-root estimate, floating point.
 * We use the frsqrte instruction for the initial estimate followed
 * by 2 iterations of Newton-Raphson to get sufficient accuracy.
 * r3 -> destination, r4 -> source.
 */
_GLOBAL(vrsqrtefp)
	mflr	r12
	bl	fpenable
	stfd	fr2,32(r1)
	stfd	fr3,40(r1)
	stfd	fr4,48(r1)
	stfd	fr5,56(r1)
	li	r0,4
	LDCONST(fr4, fpone)
	LDCONST(fr5, fphalf)
	mtctr	r0
	li	r6,0
1:	lfsx	fr0,r4,r6
	frsqrte	fr1,fr0		/* r = frsqrte(s) */
	fmuls	fr3,fr1,fr0	/* r * s */
	fmuls	fr2,fr1,fr5	/* r * 0.5 */
	fnmsubs	fr3,fr1,fr3,fr4	/* 1 - s * r * r */
	fmadds	fr1,fr2,fr3,fr1	/* r = r + 0.5 * r * (1 - s * r * r) */
	fmuls	fr3,fr1,fr0	/* r * s */
	fmuls	fr2,fr1,fr5	/* r * 0.5 */
	fnmsubs	fr3,fr1,fr3,fr4	/* 1 - s * r * r */
	fmadds	fr1,fr2,fr3,fr1	/* r = r + 0.5 * r * (1 - s * r * r) */
	stfsx	fr1,r3,r6
	addi	r6,r6,4
	bdnz	1b
	lfd	fr5,56(r1)
	lfd	fr4,48(r1)
	lfd	fr3,40(r1)
	lfd	fr2,32(r1)
	b	fpdisable
Commit	Line	Data
e821ea70	1	#include <asm/processor.h>
14cf11af	2	#include <asm/ppc_asm.h>
b3b8dc6c	3	#include <asm/reg.h>
e821ea70 BH	4	#include <asm/asm-offsets.h>
	5	#include <asm/cputable.h>
	6	#include <asm/thread_info.h>
	7	#include <asm/page.h>
46f52210	8	#include <asm/ptrace.h>
e821ea70 BH	9
	10	/*
	11	* load_up_altivec(unused, unused, tsk)
	12	* Disable VMX for the task which had it previously,
	13	* and save its vector registers in its thread_struct.
	14	* Enables the VMX for use in the kernel on return.
	15	* On SMP we know the VMX is free, since we give it up every
	16	* switch (ie, no lazy save of the vector registers).
	17	*/
	18	_GLOBAL(load_up_altivec)
	19	mfmsr r5 /* grab the current MSR */
	20	oris r5,r5,MSR_VEC@h
	21	MTMSRD(r5) /* enable use of AltiVec now */
	22	isync
	23
	24	/*
	25	* For SMP, we don't do lazy VMX switching because it just gets too
	26	* horrendously complex, especially when a task switches from one CPU
	27	* to another. Instead we call giveup_altvec in switch_to.
	28	* VRSAVE isn't dealt with here, that is done in the normal context
	29	* switch code. Note that we could rely on vrsave value to eventually
	30	* avoid saving all of the VREGs here...
	31	*/
	32	#ifndef CONFIG_SMP
	33	LOAD_REG_ADDRBASE(r3, last_task_used_altivec)
	34	toreal(r3)
	35	PPC_LL r4,ADDROFF(last_task_used_altivec)(r3)
	36	PPC_LCMPI 0,r4,0
	37	beq 1f
	38
	39	/* Save VMX state to last_task_used_altivec's THREAD struct */
	40	toreal(r4)
	41	addi r4,r4,THREAD
	42	SAVE_32VRS(0,r5,r4)
	43	mfvscr vr0
	44	li r10,THREAD_VSCR
	45	stvx vr0,r10,r4
	46	/* Disable VMX for last_task_used_altivec */
	47	PPC_LL r5,PT_REGS(r4)
	48	toreal(r5)
	49	PPC_LL r4,_MSR-STACK_FRAME_OVERHEAD(r5)
	50	lis r10,MSR_VEC@h
	51	andc r4,r4,r10
	52	PPC_STL r4,_MSR-STACK_FRAME_OVERHEAD(r5)
	53	1:
	54	#endif /* CONFIG_SMP */
	55
	56	/* Hack: if we get an altivec unavailable trap with VRSAVE
	57	* set to all zeros, we assume this is a broken application
	58	* that fails to set it properly, and thus we switch it to
	59	* all 1's
	60	*/
	61	mfspr r4,SPRN_VRSAVE
e090aa80	62	cmpwi 0,r4,0
e821ea70 BH	63	bne+ 1f
	64	li r4,-1
	65	mtspr SPRN_VRSAVE,r4
	66	1:
	67	/* enable use of VMX after return */
	68	#ifdef CONFIG_PPC32
ee43eb78	69	mfspr r5,SPRN_SPRG_THREAD /* current task's THREAD (phys) */
e821ea70 BH	70	oris r9,r9,MSR_VEC@h
	71	#else
	72	ld r4,PACACURRENT(r13)
	73	addi r5,r4,THREAD /* Get THREAD */
	74	oris r12,r12,MSR_VEC@h
	75	std r12,_MSR(r1)
	76	#endif
	77	li r4,1
	78	li r10,THREAD_VSCR
	79	stw r4,THREAD_USED_VR(r5)
	80	lvx vr0,r10,r5
	81	mtvscr vr0
	82	REST_32VRS(0,r4,r5)
	83	#ifndef CONFIG_SMP
0115cb54	84	/* Update last_task_used_altivec to 'current' */
e821ea70 BH	85	subi r4,r5,THREAD /* Back to 'current' */
e821ea70 BH	86	fromreal(r4)
0115cb54	87	PPC_STL r4,ADDROFF(last_task_used_altivec)(r3)
e821ea70 BH	88	#endif /* CONFIG_SMP */
	89	/* restore registers and return */
	90	blr
	91
	92	/*
	93	* giveup_altivec(tsk)
	94	* Disable VMX for the task given as the argument,
	95	* and save the vector registers in its thread_struct.
	96	* Enables the VMX for use in the kernel on return.
	97	*/
	98	_GLOBAL(giveup_altivec)
	99	mfmsr r5
	100	oris r5,r5,MSR_VEC@h
	101	SYNC
	102	MTMSRD(r5) /* enable use of VMX now */
	103	isync
	104	PPC_LCMPI 0,r3,0
	105	beqlr- /* if no previous owner, done */
	106	addi r3,r3,THREAD /* want THREAD of task */
	107	PPC_LL r5,PT_REGS(r3)
	108	PPC_LCMPI 0,r5,0
	109	SAVE_32VRS(0,r4,r3)
	110	mfvscr vr0
	111	li r4,THREAD_VSCR
	112	stvx vr0,r4,r3
	113	beq 1f
	114	PPC_LL r4,_MSR-STACK_FRAME_OVERHEAD(r5)
	115	#ifdef CONFIG_VSX
	116	BEGIN_FTR_SECTION
	117	lis r3,(MSR_VEC\|MSR_VSX)@h
	118	FTR_SECTION_ELSE
	119	lis r3,MSR_VEC@h
	120	ALT_FTR_SECTION_END_IFSET(CPU_FTR_VSX)
	121	#else
	122	lis r3,MSR_VEC@h
	123	#endif
	124	andc r4,r4,r3 /* disable FP for previous task */
	125	PPC_STL r4,_MSR-STACK_FRAME_OVERHEAD(r5)
	126	1:
	127	#ifndef CONFIG_SMP
	128	li r5,0
	129	LOAD_REG_ADDRBASE(r4,last_task_used_altivec)
	130	PPC_STL r5,ADDROFF(last_task_used_altivec)(r4)
	131	#endif /* CONFIG_SMP */
	132	blr
	133
	134	#ifdef CONFIG_VSX
	135
	136	#ifdef CONFIG_PPC32
	137	#error This asm code isn't ready for 32-bit kernels
	138	#endif
	139
	140	/*
	141	* load_up_vsx(unused, unused, tsk)
	142	* Disable VSX for the task which had it previously,
	143	* and save its vector registers in its thread_struct.
	144	* Reuse the fp and vsx saves, but first check to see if they have
	145	* been saved already.
	146	*/
	147	_GLOBAL(load_up_vsx)
	148	/* Load FP and VSX registers if they haven't been done yet */
	149	andi. r5,r12,MSR_FP
	150	beql+ load_up_fpu /* skip if already loaded */
	151	andis. r5,r12,MSR_VEC@h
152	beql+ load_up_altivec /* skip if already loaded */
153
154	#ifndef CONFIG_SMP
155	ld r3,last_task_used_vsx@got(r2)
156	ld r4,0(r3)
157	cmpdi 0,r4,0
158	beq 1f
159	/* Disable VSX for last_task_used_vsx */
160	addi r4,r4,THREAD
161	ld r5,PT_REGS(r4)
162	ld r4,_MSR-STACK_FRAME_OVERHEAD(r5)
163	lis r6,MSR_VSX@h
164	andc r6,r4,r6
165	std r6,_MSR-STACK_FRAME_OVERHEAD(r5)
166	1:
167	#endif /* CONFIG_SMP */
168	ld r4,PACACURRENT(r13)
169	addi r4,r4,THREAD /* Get THREAD */
170	li r6,1
171	stw r6,THREAD_USED_VSR(r4) /* ... also set thread used vsr */
172	/* enable use of VSX after return */
173	oris r12,r12,MSR_VSX@h
174	std r12,_MSR(r1)
175	#ifndef CONFIG_SMP
0115cb54	176	/* Update last_task_used_vsx to 'current' */
e821ea70 BH	177	ld r4,PACACURRENT(r13)
	178	std r4,0(r3)
	179	#endif /* CONFIG_SMP */
	180	b fast_exception_return
	181
	182	/*
	183	* __giveup_vsx(tsk)
	184	* Disable VSX for the task given as the argument.
	185	* Does NOT save vsx registers.
	186	* Enables the VSX for use in the kernel on return.
	187	*/
	188	_GLOBAL(__giveup_vsx)
	189	mfmsr r5
	190	oris r5,r5,MSR_VSX@h
	191	mtmsrd r5 /* enable use of VSX now */
	192	isync
	193
	194	cmpdi 0,r3,0
	195	beqlr- /* if no previous owner, done */
	196	addi r3,r3,THREAD /* want THREAD of task */
	197	ld r5,PT_REGS(r3)
	198	cmpdi 0,r5,0
	199	beq 1f
	200	ld r4,_MSR-STACK_FRAME_OVERHEAD(r5)
	201	lis r3,MSR_VSX@h
	202	andc r4,r4,r3 /* disable VSX for previous task */
	203	std r4,_MSR-STACK_FRAME_OVERHEAD(r5)
	204	1:
	205	#ifndef CONFIG_SMP
	206	li r5,0
	207	ld r4,last_task_used_vsx@got(r2)
	208	std r5,0(r4)
	209	#endif /* CONFIG_SMP */
	210	blr
	211
	212	#endif /* CONFIG_VSX */
	213
14cf11af PM	214
	215	/*
	216	* The routines below are in assembler so we can closely control the
	217	* usage of floating-point registers. These routines must be called
	218	* with preempt disabled.
	219	*/
	220	#ifdef CONFIG_PPC32
	221	.data
	222	fpzero:
	223	.long 0
	224	fpone:
	225	.long 0x3f800000 /* 1.0 in single-precision FP */
	226	fphalf:
	227	.long 0x3f000000 /* 0.5 in single-precision FP */
	228
	229	#define LDCONST(fr, name) \
	230	lis r11,name@ha; \
	231	lfs fr,name@l(r11)
	232	#else
	233
	234	.section ".toc","aw"
	235	fpzero:
	236	.tc FD_0_0[TC],0
	237	fpone:
	238	.tc FD_3ff00000_0[TC],0x3ff0000000000000 /* 1.0 */
	239	fphalf:
	240	.tc FD_3fe00000_0[TC],0x3fe0000000000000 /* 0.5 */
	241
	242	#define LDCONST(fr, name) \
	243	lfd fr,name@toc(r2)
	244	#endif
	245
	246	.text
	247	/*
	248	* Internal routine to enable floating point and set FPSCR to 0.
	249	* Don't call it from C; it doesn't use the normal calling convention.
	250	*/
	251	fpenable:
	252	#ifdef CONFIG_PPC32
	253	stwu r1,-64(r1)
	254	#else
	255	stdu r1,-64(r1)
	256	#endif
	257	mfmsr r10
	258	ori r11,r10,MSR_FP
	259	mtmsr r11
	260	isync
	261	stfd fr0,24(r1)
	262	stfd fr1,16(r1)
	263	stfd fr31,8(r1)
	264	LDCONST(fr1, fpzero)
	265	mffs fr31
3a2c48cf	266	MTFSF_L(fr1)
14cf11af PM	267	blr
	268
	269	fpdisable:
	270	mtlr r12
3a2c48cf	271	MTFSF_L(fr31)
14cf11af PM	272	lfd fr31,8(r1)
	273	lfd fr1,16(r1)
	274	lfd fr0,24(r1)
	275	mtmsr r10
	276	isync
	277	addi r1,r1,64
	278	blr
	279
	280	/*
	281	* Vector add, floating point.
	282	*/
	283	_GLOBAL(vaddfp)
	284	mflr r12
	285	bl fpenable
	286	li r0,4
	287	mtctr r0
	288	li r6,0
	289	1: lfsx fr0,r4,r6
	290	lfsx fr1,r5,r6
	291	fadds fr0,fr0,fr1
	292	stfsx fr0,r3,r6
	293	addi r6,r6,4
	294	bdnz 1b
	295	b fpdisable
	296
	297	/*
	298	* Vector subtract, floating point.
	299	*/
	300	_GLOBAL(vsubfp)
	301	mflr r12
	302	bl fpenable
	303	li r0,4
	304	mtctr r0
	305	li r6,0
	306	1: lfsx fr0,r4,r6
	307	lfsx fr1,r5,r6
	308	fsubs fr0,fr0,fr1
	309	stfsx fr0,r3,r6
	310	addi r6,r6,4
	311	bdnz 1b
	312	b fpdisable
	313
	314	/*
	315	* Vector multiply and add, floating point.
	316	*/
	317	_GLOBAL(vmaddfp)
	318	mflr r12
	319	bl fpenable
	320	stfd fr2,32(r1)
	321	li r0,4
	322	mtctr r0
	323	li r7,0
	324	1: lfsx fr0,r4,r7
	325	lfsx fr1,r5,r7
	326	lfsx fr2,r6,r7
	327	fmadds fr0,fr0,fr2,fr1
	328	stfsx fr0,r3,r7
	329	addi r7,r7,4
	330	bdnz 1b
	331	lfd fr2,32(r1)
	332	b fpdisable
	333
	334	/*
	335	* Vector negative multiply and subtract, floating point.
336	*/
337	_GLOBAL(vnmsubfp)
338	mflr r12
339	bl fpenable
340	stfd fr2,32(r1)
341	li r0,4
342	mtctr r0
343	li r7,0
344	1: lfsx fr0,r4,r7
345	lfsx fr1,r5,r7
346	lfsx fr2,r6,r7
347	fnmsubs fr0,fr0,fr2,fr1
348	stfsx fr0,r3,r7
349	addi r7,r7,4
350	bdnz 1b
351	lfd fr2,32(r1)
352	b fpdisable
353
354	/*
355	* Vector reciprocal estimate. We just compute 1.0/x.
356	* r3 -> destination, r4 -> source.
357	*/
358	_GLOBAL(vrefp)
359	mflr r12
360	bl fpenable
361	li r0,4
362	LDCONST(fr1, fpone)
363	mtctr r0
364	li r6,0
365	1: lfsx fr0,r4,r6
366	fdivs fr0,fr1,fr0
367	stfsx fr0,r3,r6
368	addi r6,r6,4
369	bdnz 1b
370	b fpdisable
371
372	/*
373	* Vector reciprocal square-root estimate, floating point.
374	* We use the frsqrte instruction for the initial estimate followed
375	* by 2 iterations of Newton-Raphson to get sufficient accuracy.
376	* r3 -> destination, r4 -> source.
377	*/
378	_GLOBAL(vrsqrtefp)
379	mflr r12
380	bl fpenable
381	stfd fr2,32(r1)
382	stfd fr3,40(r1)
383	stfd fr4,48(r1)
384	stfd fr5,56(r1)
385	li r0,4
386	LDCONST(fr4, fpone)
387	LDCONST(fr5, fphalf)
388	mtctr r0
389	li r6,0
390	1: lfsx fr0,r4,r6
391	frsqrte fr1,fr0 /* r = frsqrte(s) */
392	fmuls fr3,fr1,fr0 /* r * s */
393	fmuls fr2,fr1,fr5 /* r * 0.5 */
394	fnmsubs fr3,fr1,fr3,fr4 /* 1 - s * r * r */
395	fmadds fr1,fr2,fr3,fr1 /* r = r + 0.5 * r * (1 - s * r * r) */
396	fmuls fr3,fr1,fr0 /* r * s */
397	fmuls fr2,fr1,fr5 /* r * 0.5 */
398	fnmsubs fr3,fr1,fr3,fr4 /* 1 - s * r * r */
399	fmadds fr1,fr2,fr3,fr1 /* r = r + 0.5 * r * (1 - s * r * r) */
400	stfsx fr1,r3,r6
401	addi r6,r6,4
402	bdnz 1b
403	lfd fr5,56(r1)
404	lfd fr4,48(r1)
405	lfd fr3,40(r1)
406	lfd fr2,32(r1)
407	b fpdisable