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 } 64