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