1 /* 2 * Copyright (C) 1999 Eddie C. Dost (ecd@atecom.com) 3 */ 4 5 #include <linux/types.h> 6 #include <linux/sched.h> 7 8 #include <asm/uaccess.h> 9 #include <asm/reg.h> 10 #include <asm/switch_to.h> 11 12 #include <asm/sfp-machine.h> 13 #include <math-emu/double.h> 14 15 #define FLOATFUNC(x) extern int x(void *, void *, void *, void *) 16 17 /* The instructions list which may be not implemented by a hardware FPU */ 18 FLOATFUNC(fre); 19 FLOATFUNC(frsqrtes); 20 FLOATFUNC(fsqrt); 21 FLOATFUNC(fsqrts); 22 FLOATFUNC(mtfsf); 23 FLOATFUNC(mtfsfi); 24 25 #ifdef CONFIG_MATH_EMULATION_HW_UNIMPLEMENTED 26 #undef FLOATFUNC(x) 27 #define FLOATFUNC(x) static inline int x(void *op1, void *op2, void *op3, \ 28 void *op4) { } 29 #endif 30 31 FLOATFUNC(fadd); 32 FLOATFUNC(fadds); 33 FLOATFUNC(fdiv); 34 FLOATFUNC(fdivs); 35 FLOATFUNC(fmul); 36 FLOATFUNC(fmuls); 37 FLOATFUNC(fsub); 38 FLOATFUNC(fsubs); 39 40 FLOATFUNC(fmadd); 41 FLOATFUNC(fmadds); 42 FLOATFUNC(fmsub); 43 FLOATFUNC(fmsubs); 44 FLOATFUNC(fnmadd); 45 FLOATFUNC(fnmadds); 46 FLOATFUNC(fnmsub); 47 FLOATFUNC(fnmsubs); 48 49 FLOATFUNC(fctiw); 50 FLOATFUNC(fctiwz); 51 FLOATFUNC(frsp); 52 53 FLOATFUNC(fcmpo); 54 FLOATFUNC(fcmpu); 55 56 FLOATFUNC(mcrfs); 57 FLOATFUNC(mffs); 58 FLOATFUNC(mtfsb0); 59 FLOATFUNC(mtfsb1); 60 61 FLOATFUNC(lfd); 62 FLOATFUNC(lfs); 63 64 FLOATFUNC(stfd); 65 FLOATFUNC(stfs); 66 FLOATFUNC(stfiwx); 67 68 FLOATFUNC(fabs); 69 FLOATFUNC(fmr); 70 FLOATFUNC(fnabs); 71 FLOATFUNC(fneg); 72 73 /* Optional */ 74 FLOATFUNC(fres); 75 FLOATFUNC(frsqrte); 76 FLOATFUNC(fsel); 77 78 79 #define OP31 0x1f /* 31 */ 80 #define LFS 0x30 /* 48 */ 81 #define LFSU 0x31 /* 49 */ 82 #define LFD 0x32 /* 50 */ 83 #define LFDU 0x33 /* 51 */ 84 #define STFS 0x34 /* 52 */ 85 #define STFSU 0x35 /* 53 */ 86 #define STFD 0x36 /* 54 */ 87 #define STFDU 0x37 /* 55 */ 88 #define OP59 0x3b /* 59 */ 89 #define OP63 0x3f /* 63 */ 90 91 /* Opcode 31: */ 92 /* X-Form: */ 93 #define LFSX 0x217 /* 535 */ 94 #define LFSUX 0x237 /* 567 */ 95 #define LFDX 0x257 /* 599 */ 96 #define LFDUX 0x277 /* 631 */ 97 #define STFSX 0x297 /* 663 */ 98 #define STFSUX 0x2b7 /* 695 */ 99 #define STFDX 0x2d7 /* 727 */ 100 #define STFDUX 0x2f7 /* 759 */ 101 #define STFIWX 0x3d7 /* 983 */ 102 103 /* Opcode 59: */ 104 /* A-Form: */ 105 #define FDIVS 0x012 /* 18 */ 106 #define FSUBS 0x014 /* 20 */ 107 #define FADDS 0x015 /* 21 */ 108 #define FSQRTS 0x016 /* 22 */ 109 #define FRES 0x018 /* 24 */ 110 #define FMULS 0x019 /* 25 */ 111 #define FRSQRTES 0x01a /* 26 */ 112 #define FMSUBS 0x01c /* 28 */ 113 #define FMADDS 0x01d /* 29 */ 114 #define FNMSUBS 0x01e /* 30 */ 115 #define FNMADDS 0x01f /* 31 */ 116 117 /* Opcode 63: */ 118 /* A-Form: */ 119 #define FDIV 0x012 /* 18 */ 120 #define FSUB 0x014 /* 20 */ 121 #define FADD 0x015 /* 21 */ 122 #define FSQRT 0x016 /* 22 */ 123 #define FSEL 0x017 /* 23 */ 124 #define FRE 0x018 /* 24 */ 125 #define FMUL 0x019 /* 25 */ 126 #define FRSQRTE 0x01a /* 26 */ 127 #define FMSUB 0x01c /* 28 */ 128 #define FMADD 0x01d /* 29 */ 129 #define FNMSUB 0x01e /* 30 */ 130 #define FNMADD 0x01f /* 31 */ 131 132 /* X-Form: */ 133 #define FCMPU 0x000 /* 0 */ 134 #define FRSP 0x00c /* 12 */ 135 #define FCTIW 0x00e /* 14 */ 136 #define FCTIWZ 0x00f /* 15 */ 137 #define FCMPO 0x020 /* 32 */ 138 #define MTFSB1 0x026 /* 38 */ 139 #define FNEG 0x028 /* 40 */ 140 #define MCRFS 0x040 /* 64 */ 141 #define MTFSB0 0x046 /* 70 */ 142 #define FMR 0x048 /* 72 */ 143 #define MTFSFI 0x086 /* 134 */ 144 #define FNABS 0x088 /* 136 */ 145 #define FABS 0x108 /* 264 */ 146 #define MFFS 0x247 /* 583 */ 147 #define MTFSF 0x2c7 /* 711 */ 148 149 150 #define AB 2 151 #define AC 3 152 #define ABC 4 153 #define D 5 154 #define DU 6 155 #define X 7 156 #define XA 8 157 #define XB 9 158 #define XCR 11 159 #define XCRB 12 160 #define XCRI 13 161 #define XCRL 16 162 #define XE 14 163 #define XEU 15 164 #define XFLB 10 165 166 static int 167 record_exception(struct pt_regs *regs, int eflag) 168 { 169 u32 fpscr; 170 171 fpscr = __FPU_FPSCR; 172 173 if (eflag) { 174 fpscr |= FPSCR_FX; 175 if (eflag & EFLAG_OVERFLOW) 176 fpscr |= FPSCR_OX; 177 if (eflag & EFLAG_UNDERFLOW) 178 fpscr |= FPSCR_UX; 179 if (eflag & EFLAG_DIVZERO) 180 fpscr |= FPSCR_ZX; 181 if (eflag & EFLAG_INEXACT) 182 fpscr |= FPSCR_XX; 183 if (eflag & EFLAG_INVALID) 184 fpscr |= FPSCR_VX; 185 if (eflag & EFLAG_VXSNAN) 186 fpscr |= FPSCR_VXSNAN; 187 if (eflag & EFLAG_VXISI) 188 fpscr |= FPSCR_VXISI; 189 if (eflag & EFLAG_VXIDI) 190 fpscr |= FPSCR_VXIDI; 191 if (eflag & EFLAG_VXZDZ) 192 fpscr |= FPSCR_VXZDZ; 193 if (eflag & EFLAG_VXIMZ) 194 fpscr |= FPSCR_VXIMZ; 195 if (eflag & EFLAG_VXVC) 196 fpscr |= FPSCR_VXVC; 197 if (eflag & EFLAG_VXSOFT) 198 fpscr |= FPSCR_VXSOFT; 199 if (eflag & EFLAG_VXSQRT) 200 fpscr |= FPSCR_VXSQRT; 201 if (eflag & EFLAG_VXCVI) 202 fpscr |= FPSCR_VXCVI; 203 } 204 205 // fpscr &= ~(FPSCR_VX); 206 if (fpscr & (FPSCR_VXSNAN | FPSCR_VXISI | FPSCR_VXIDI | 207 FPSCR_VXZDZ | FPSCR_VXIMZ | FPSCR_VXVC | 208 FPSCR_VXSOFT | FPSCR_VXSQRT | FPSCR_VXCVI)) 209 fpscr |= FPSCR_VX; 210 211 fpscr &= ~(FPSCR_FEX); 212 if (((fpscr & FPSCR_VX) && (fpscr & FPSCR_VE)) || 213 ((fpscr & FPSCR_OX) && (fpscr & FPSCR_OE)) || 214 ((fpscr & FPSCR_UX) && (fpscr & FPSCR_UE)) || 215 ((fpscr & FPSCR_ZX) && (fpscr & FPSCR_ZE)) || 216 ((fpscr & FPSCR_XX) && (fpscr & FPSCR_XE))) 217 fpscr |= FPSCR_FEX; 218 219 __FPU_FPSCR = fpscr; 220 221 return (fpscr & FPSCR_FEX) ? 1 : 0; 222 } 223 224 int 225 do_mathemu(struct pt_regs *regs) 226 { 227 void *op0 = 0, *op1 = 0, *op2 = 0, *op3 = 0; 228 unsigned long pc = regs->nip; 229 signed short sdisp; 230 u32 insn = 0; 231 int idx = 0; 232 int (*func)(void *, void *, void *, void *); 233 int type = 0; 234 int eflag, trap; 235 236 if (get_user(insn, (u32 *)pc)) 237 return -EFAULT; 238 239 switch (insn >> 26) { 240 case LFS: func = lfs; type = D; break; 241 case LFSU: func = lfs; type = DU; break; 242 case LFD: func = lfd; type = D; break; 243 case LFDU: func = lfd; type = DU; break; 244 case STFS: func = stfs; type = D; break; 245 case STFSU: func = stfs; type = DU; break; 246 case STFD: func = stfd; type = D; break; 247 case STFDU: func = stfd; type = DU; break; 248 249 case OP31: 250 switch ((insn >> 1) & 0x3ff) { 251 case LFSX: func = lfs; type = XE; break; 252 case LFSUX: func = lfs; type = XEU; break; 253 case LFDX: func = lfd; type = XE; break; 254 case LFDUX: func = lfd; type = XEU; break; 255 case STFSX: func = stfs; type = XE; break; 256 case STFSUX: func = stfs; type = XEU; break; 257 case STFDX: func = stfd; type = XE; break; 258 case STFDUX: func = stfd; type = XEU; break; 259 case STFIWX: func = stfiwx; type = XE; break; 260 default: 261 goto illegal; 262 } 263 break; 264 265 case OP59: 266 switch ((insn >> 1) & 0x1f) { 267 case FDIVS: func = fdivs; type = AB; break; 268 case FSUBS: func = fsubs; type = AB; break; 269 case FADDS: func = fadds; type = AB; break; 270 case FSQRTS: func = fsqrts; type = XB; break; 271 case FRES: func = fres; type = XB; break; 272 case FMULS: func = fmuls; type = AC; break; 273 case FRSQRTES: func = frsqrtes;type = XB; break; 274 case FMSUBS: func = fmsubs; type = ABC; break; 275 case FMADDS: func = fmadds; type = ABC; break; 276 case FNMSUBS: func = fnmsubs; type = ABC; break; 277 case FNMADDS: func = fnmadds; type = ABC; break; 278 default: 279 goto illegal; 280 } 281 break; 282 283 case OP63: 284 if (insn & 0x20) { 285 switch ((insn >> 1) & 0x1f) { 286 case FDIV: func = fdiv; type = AB; break; 287 case FSUB: func = fsub; type = AB; break; 288 case FADD: func = fadd; type = AB; break; 289 case FSQRT: func = fsqrt; type = XB; break; 290 case FRE: func = fre; type = XB; break; 291 case FSEL: func = fsel; type = ABC; break; 292 case FMUL: func = fmul; type = AC; break; 293 case FRSQRTE: func = frsqrte; type = XB; break; 294 case FMSUB: func = fmsub; type = ABC; break; 295 case FMADD: func = fmadd; type = ABC; break; 296 case FNMSUB: func = fnmsub; type = ABC; break; 297 case FNMADD: func = fnmadd; type = ABC; break; 298 default: 299 goto illegal; 300 } 301 break; 302 } 303 304 switch ((insn >> 1) & 0x3ff) { 305 case FCMPU: func = fcmpu; type = XCR; break; 306 case FRSP: func = frsp; type = XB; break; 307 case FCTIW: func = fctiw; type = XB; break; 308 case FCTIWZ: func = fctiwz; type = XB; break; 309 case FCMPO: func = fcmpo; type = XCR; break; 310 case MTFSB1: func = mtfsb1; type = XCRB; break; 311 case FNEG: func = fneg; type = XB; break; 312 case MCRFS: func = mcrfs; type = XCRL; break; 313 case MTFSB0: func = mtfsb0; type = XCRB; break; 314 case FMR: func = fmr; type = XB; break; 315 case MTFSFI: func = mtfsfi; type = XCRI; break; 316 case FNABS: func = fnabs; type = XB; break; 317 case FABS: func = fabs; type = XB; break; 318 case MFFS: func = mffs; type = X; break; 319 case MTFSF: func = mtfsf; type = XFLB; break; 320 default: 321 goto illegal; 322 } 323 break; 324 325 default: 326 goto illegal; 327 } 328 329 switch (type) { 330 case AB: 331 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 332 op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); 333 op2 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 334 break; 335 336 case AC: 337 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 338 op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); 339 op2 = (void *)¤t->thread.TS_FPR((insn >> 6) & 0x1f); 340 break; 341 342 case ABC: 343 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 344 op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); 345 op2 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 346 op3 = (void *)¤t->thread.TS_FPR((insn >> 6) & 0x1f); 347 break; 348 349 case D: 350 idx = (insn >> 16) & 0x1f; 351 sdisp = (insn & 0xffff); 352 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 353 op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); 354 break; 355 356 case DU: 357 idx = (insn >> 16) & 0x1f; 358 if (!idx) 359 goto illegal; 360 361 sdisp = (insn & 0xffff); 362 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 363 op1 = (void *)(regs->gpr[idx] + sdisp); 364 break; 365 366 case X: 367 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 368 break; 369 370 case XA: 371 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 372 op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); 373 break; 374 375 case XB: 376 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 377 op1 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 378 break; 379 380 case XE: 381 idx = (insn >> 16) & 0x1f; 382 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 383 op1 = (void *)((idx ? regs->gpr[idx] : 0) 384 + regs->gpr[(insn >> 11) & 0x1f]); 385 break; 386 387 case XEU: 388 idx = (insn >> 16) & 0x1f; 389 if (!idx) 390 goto illegal; 391 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 392 op1 = (void *)(regs->gpr[idx] 393 + regs->gpr[(insn >> 11) & 0x1f]); 394 break; 395 396 case XCR: 397 op0 = (void *)®s->ccr; 398 op1 = (void *)((insn >> 23) & 0x7); 399 op2 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); 400 op3 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 401 break; 402 403 case XCRL: 404 op0 = (void *)®s->ccr; 405 op1 = (void *)((insn >> 23) & 0x7); 406 op2 = (void *)((insn >> 18) & 0x7); 407 break; 408 409 case XCRB: 410 op0 = (void *)((insn >> 21) & 0x1f); 411 break; 412 413 case XCRI: 414 op0 = (void *)((insn >> 23) & 0x7); 415 op1 = (void *)((insn >> 12) & 0xf); 416 break; 417 418 case XFLB: 419 op0 = (void *)((insn >> 17) & 0xff); 420 op1 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 421 break; 422 423 default: 424 goto illegal; 425 } 426 427 /* 428 * If we support a HW FPU, we need to ensure the FP state 429 * is flushed into the thread_struct before attempting 430 * emulation 431 */ 432 flush_fp_to_thread(current); 433 434 eflag = func(op0, op1, op2, op3); 435 436 if (insn & 1) { 437 regs->ccr &= ~(0x0f000000); 438 regs->ccr |= (__FPU_FPSCR >> 4) & 0x0f000000; 439 } 440 441 trap = record_exception(regs, eflag); 442 if (trap) 443 return 1; 444 445 switch (type) { 446 case DU: 447 case XEU: 448 regs->gpr[idx] = (unsigned long)op1; 449 break; 450 451 default: 452 break; 453 } 454 455 regs->nip += 4; 456 return 0; 457 458 illegal: 459 return -ENOSYS; 460 } 461