#include <asm/ppc_asm.h>
#include <asm/processor.h>
/*
* 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.
*/
.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 */
.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:
mfmsr r10
ori r11,r10,MSR_FP
mtmsr r11
isync
stfd fr31,-8(r1)
stfd fr0,-16(r1)
stfd fr1,-24(r1)
mffs fr31
lfd fr1,fpzero@toc(r2)
mtfsf 0xff,fr1
blr
fpdisable:
mtlr r12
mtfsf 0xff,fr31
lfd fr1,-24(r1)
lfd fr0,-16(r1)
lfd fr31,-8(r1)
mtmsr r10
isync
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
lfd fr1,fpone@toc(r2)
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
lfd fr4,fpone@toc(r2)
lfd fr5,fphalf@toc(r2)
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
