release/src-rt-6.x.4708/linux/linux-2.6.36/arch/powerpc/math-emu/mtfsf.c

   1 #include <linux/types.h>
   2 #include <linux/errno.h>
   3 #include <asm/uaccess.h>
   4
   5 #include <asm/sfp-machine.h>
   6 #include <math-emu/soft-fp.h>
   7
   8 int
   9 mtfsf(unsigned int FM, u32 *frB)
  10 {
  11         u32 mask;
  12         u32 fpscr;
  13
  14         if (FM == 0)
  15                 return 0;
  16
  17         if (FM == 0xff)
  18                 mask = 0x9fffffff;
  19         else {
  20                 mask = 0;
  21                 if (FM & (1 << 0))
  22                         mask |= 0x90000000;
  23                 if (FM & (1 << 1))
  24                         mask |= 0x0f000000;
  25                 if (FM & (1 << 2))
  26                         mask |= 0x00f00000;
  27                 if (FM & (1 << 3))
  28                         mask |= 0x000f0000;
  29                 if (FM & (1 << 4))
  30                         mask |= 0x0000f000;
  31                 if (FM & (1 << 5))
  32                         mask |= 0x00000f00;
  33                 if (FM & (1 << 6))
  34                         mask |= 0x000000f0;
  35                 if (FM & (1 << 7))
  36                         mask |= 0x0000000f;
  37         }
  38
  39         __FPU_FPSCR &= ~(mask);
  40         __FPU_FPSCR |= (frB[1] & mask);
  41
  42         __FPU_FPSCR &= ~(FPSCR_VX);
  43         if (__FPU_FPSCR & (FPSCR_VXSNAN | FPSCR_VXISI | FPSCR_VXIDI |
  44                      FPSCR_VXZDZ | FPSCR_VXIMZ | FPSCR_VXVC |
  45                      FPSCR_VXSOFT | FPSCR_VXSQRT | FPSCR_VXCVI))
  46                 __FPU_FPSCR |= FPSCR_VX;
  47
  48         fpscr = __FPU_FPSCR;
  49         fpscr &= ~(FPSCR_FEX);
  50         if (((fpscr & FPSCR_VX) && (fpscr & FPSCR_VE)) ||
  51             ((fpscr & FPSCR_OX) && (fpscr & FPSCR_OE)) ||
  52             ((fpscr & FPSCR_UX) && (fpscr & FPSCR_UE)) ||
  53             ((fpscr & FPSCR_ZX) && (fpscr & FPSCR_ZE)) ||
  54             ((fpscr & FPSCR_XX) && (fpscr & FPSCR_XE)))
  55                 fpscr |= FPSCR_FEX;
  56         __FPU_FPSCR = fpscr;
  57
  58 #ifdef DEBUG
  59         printk("%s: %02x %p: %08lx\n", __func__, FM, frB, __FPU_FPSCR);
  60 #endif
  61
  62         return 0;
  63 }