arch/ppc64/kernel/vector.S

   1 #include <asm/ppc_asm.h>
   2 #include <asm/processor.h>
   3
   4 /*
   5  * The routines below are in assembler so we can closely control the
   6  * usage of floating-point registers.  These routines must be called
   7  * with preempt disabled.
   8  */
   9         .section ".toc","aw"
  10 fpzero:
  11         .tc     FD_0_0[TC],0
  12 fpone:
  13         .tc     FD_3ff00000_0[TC],0x3ff0000000000000    /* 1.0 */
  14 fphalf:
  15         .tc     FD_3fe00000_0[TC],0x3fe0000000000000    /* 0.5 */
  16
  17         .text
  18 /*
  19  * Internal routine to enable floating point and set FPSCR to 0.
  20  * Don't call it from C; it doesn't use the normal calling convention.
  21  */
  22 fpenable:
  23         mfmsr   r10
  24         ori     r11,r10,MSR_FP
  25         mtmsr   r11
  26         isync
  27         stfd    fr31,-8(r1)
  28         stfd    fr0,-16(r1)
  29         stfd    fr1,-24(r1)
  30         mffs    fr31
  31         lfd     fr1,fpzero@toc(r2)
  32         mtfsf   0xff,fr1
  33         blr
  34
  35 fpdisable:
  36         mtlr    r12
  37         mtfsf   0xff,fr31
  38         lfd     fr1,-24(r1)
  39         lfd     fr0,-16(r1)
  40         lfd     fr31,-8(r1)
  41         mtmsr   r10
  42         isync
  43         blr
  44
  45 /*
  46  * Vector add, floating point.
  47  */
  48 _GLOBAL(vaddfp)
  49         mflr    r12
  50         bl      fpenable
  51         li      r0,4
  52         mtctr   r0
  53         li      r6,0
  54 1:      lfsx    fr0,r4,r6
  55         lfsx    fr1,r5,r6
  56         fadds   fr0,fr0,fr1
  57         stfsx   fr0,r3,r6
  58         addi    r6,r6,4
  59         bdnz    1b
  60         b       fpdisable
  61
  62 /*
  63  * Vector subtract, floating point.
  64  */
  65 _GLOBAL(vsubfp)
  66         mflr    r12
  67         bl      fpenable
  68         li      r0,4
  69         mtctr   r0
  70         li      r6,0
  71 1:      lfsx    fr0,r4,r6
  72         lfsx    fr1,r5,r6
  73         fsubs   fr0,fr0,fr1
  74         stfsx   fr0,r3,r6
  75         addi    r6,r6,4
  76         bdnz    1b
  77         b       fpdisable
  78
  79 /*
  80  * Vector multiply and add, floating point.
  81  */
  82 _GLOBAL(vmaddfp)
  83         mflr    r12
  84         bl      fpenable
  85         stfd    fr2,-32(r1)
  86         li      r0,4
  87         mtctr   r0
  88         li      r7,0
  89 1:      lfsx    fr0,r4,r7
  90         lfsx    fr1,r5,r7
  91         lfsx    fr2,r6,r7
  92         fmadds  fr0,fr0,fr1,fr2
  93         stfsx   fr0,r3,r7
  94         addi    r7,r7,4
  95         bdnz    1b
  96         lfd     fr2,-32(r1)
  97         b       fpdisable
  98
  99 /*
 100  * Vector negative multiply and subtract, floating point.
 101  */
 102 _GLOBAL(vnmsubfp)
 103         mflr    r12
 104         bl      fpenable
 105         stfd    fr2,-32(r1)
 106         li      r0,4
 107         mtctr   r0
 108         li      r7,0
 109 1:      lfsx    fr0,r4,r7
 110         lfsx    fr1,r5,r7
 111         lfsx    fr2,r6,r7
 112         fnmsubs fr0,fr0,fr1,fr2
 113         stfsx   fr0,r3,r7
 114         addi    r7,r7,4
 115         bdnz    1b
 116         lfd     fr2,-32(r1)
 117         b       fpdisable
 118
 119 /*
 120  * Vector reciprocal estimate.  We just compute 1.0/x.
 121  * r3 -> destination, r4 -> source.
 122  */
 123 _GLOBAL(vrefp)
 124         mflr    r12
 125         bl      fpenable
 126         li      r0,4
 127         lfd     fr1,fpone@toc(r2)
 128         mtctr   r0
 129         li      r6,0
 130 1:      lfsx    fr0,r4,r6
 131         fdivs   fr0,fr1,fr0
 132         stfsx   fr0,r3,r6
 133         addi    r6,r6,4
 134         bdnz    1b
 135         b       fpdisable
 136
 137 /*
 138  * Vector reciprocal square-root estimate, floating point.
 139  * We use the frsqrte instruction for the initial estimate followed
 140  * by 2 iterations of Newton-Raphson to get sufficient accuracy.
 141  * r3 -> destination, r4 -> source.
 142  */
 143 _GLOBAL(vrsqrtefp)
 144         mflr    r12
 145         bl      fpenable
 146         stfd    fr2,-32(r1)
 147         stfd    fr3,-40(r1)
 148         stfd    fr4,-48(r1)
 149         stfd    fr5,-56(r1)
 150         li      r0,4
 151         lfd     fr4,fpone@toc(r2)
 152         lfd     fr5,fphalf@toc(r2)
 153         mtctr   r0
 154         li      r6,0
 155 1:      lfsx    fr0,r4,r6
 156         frsqrte fr1,fr0         /* r = frsqrte(s) */
 157         fmuls   fr3,fr1,fr0     /* r * s */
 158         fmuls   fr2,fr1,fr5     /* r * 0.5 */
 159         fnmsubs fr3,fr1,fr3,fr4 /* 1 - s * r * r */
 160         fmadds  fr1,fr2,fr3,fr1 /* r = r + 0.5 * r * (1 - s * r * r) */
 161         fmuls   fr3,fr1,fr0     /* r * s */
 162         fmuls   fr2,fr1,fr5     /* r * 0.5 */
 163         fnmsubs fr3,fr1,fr3,fr4 /* 1 - s * r * r */
 164         fmadds  fr1,fr2,fr3,fr1 /* r = r + 0.5 * r * (1 - s * r * r) */
 165         stfsx   fr1,r3,r6
 166         addi    r6,r6,4
 167         bdnz    1b
 168         lfd     fr5,-56(r1)
 169         lfd     fr4,-48(r1)
 170         lfd     fr3,-40(r1)
 171         lfd     fr2,-32(r1)
 172         b       fpdisable