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