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