arch/powerpc/kernel/vector.S

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