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 "sfp-machine.h" 12 #include "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_VXSNAN) 172 fpscr |= FPSCR_VXSNAN; 173 if (eflag & EFLAG_VXISI) 174 fpscr |= FPSCR_VXISI; 175 if (eflag & EFLAG_VXIDI) 176 fpscr |= FPSCR_VXIDI; 177 if (eflag & EFLAG_VXZDZ) 178 fpscr |= FPSCR_VXZDZ; 179 if (eflag & EFLAG_VXIMZ) 180 fpscr |= FPSCR_VXIMZ; 181 if (eflag & EFLAG_VXVC) 182 fpscr |= FPSCR_VXVC; 183 if (eflag & EFLAG_VXSOFT) 184 fpscr |= FPSCR_VXSOFT; 185 if (eflag & EFLAG_VXSQRT) 186 fpscr |= FPSCR_VXSQRT; 187 if (eflag & EFLAG_VXCVI) 188 fpscr |= FPSCR_VXCVI; 189 } 190 191 fpscr &= ~(FPSCR_VX); 192 if (fpscr & (FPSCR_VXSNAN | FPSCR_VXISI | FPSCR_VXIDI | 193 FPSCR_VXZDZ | FPSCR_VXIMZ | FPSCR_VXVC | 194 FPSCR_VXSOFT | FPSCR_VXSQRT | FPSCR_VXCVI)) 195 fpscr |= FPSCR_VX; 196 197 fpscr &= ~(FPSCR_FEX); 198 if (((fpscr & FPSCR_VX) && (fpscr & FPSCR_VE)) || 199 ((fpscr & FPSCR_OX) && (fpscr & FPSCR_OE)) || 200 ((fpscr & FPSCR_UX) && (fpscr & FPSCR_UE)) || 201 ((fpscr & FPSCR_ZX) && (fpscr & FPSCR_ZE)) || 202 ((fpscr & FPSCR_XX) && (fpscr & FPSCR_XE))) 203 fpscr |= FPSCR_FEX; 204 205 __FPU_FPSCR = fpscr; 206 207 return (fpscr & FPSCR_FEX) ? 1 : 0; 208 } 209 #endif /* CONFIG_MATH_EMULATION */ 210 211 int 212 do_mathemu(struct pt_regs *regs) 213 { 214 void *op0 = 0, *op1 = 0, *op2 = 0, *op3 = 0; 215 unsigned long pc = regs->nip; 216 signed short sdisp; 217 u32 insn = 0; 218 int idx = 0; 219 #ifdef CONFIG_MATH_EMULATION 220 int (*func)(void *, void *, void *, void *); 221 int type = 0; 222 int eflag, trap; 223 #endif 224 225 if (get_user(insn, (u32 *)pc)) 226 return -EFAULT; 227 228 #ifndef CONFIG_MATH_EMULATION 229 switch (insn >> 26) { 230 case LFD: 231 idx = (insn >> 16) & 0x1f; 232 sdisp = (insn & 0xffff); 233 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 234 op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); 235 lfd(op0, op1, op2, op3); 236 break; 237 case LFDU: 238 idx = (insn >> 16) & 0x1f; 239 sdisp = (insn & 0xffff); 240 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 241 op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); 242 lfd(op0, op1, op2, op3); 243 regs->gpr[idx] = (unsigned long)op1; 244 break; 245 case STFD: 246 idx = (insn >> 16) & 0x1f; 247 sdisp = (insn & 0xffff); 248 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 249 op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); 250 stfd(op0, op1, op2, op3); 251 break; 252 case STFDU: 253 idx = (insn >> 16) & 0x1f; 254 sdisp = (insn & 0xffff); 255 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 256 op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); 257 stfd(op0, op1, op2, op3); 258 regs->gpr[idx] = (unsigned long)op1; 259 break; 260 case OP63: 261 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 262 op1 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 263 fmr(op0, op1, op2, op3); 264 break; 265 default: 266 goto illegal; 267 } 268 #else /* CONFIG_MATH_EMULATION */ 269 switch (insn >> 26) { 270 case LFS: func = lfs; type = D; break; 271 case LFSU: func = lfs; type = DU; break; 272 case LFD: func = lfd; type = D; break; 273 case LFDU: func = lfd; type = DU; break; 274 case STFS: func = stfs; type = D; break; 275 case STFSU: func = stfs; type = DU; break; 276 case STFD: func = stfd; type = D; break; 277 case STFDU: func = stfd; type = DU; break; 278 279 case OP31: 280 switch ((insn >> 1) & 0x3ff) { 281 case LFSX: func = lfs; type = XE; break; 282 case LFSUX: func = lfs; type = XEU; break; 283 case LFDX: func = lfd; type = XE; break; 284 case LFDUX: func = lfd; type = XEU; break; 285 case STFSX: func = stfs; type = XE; break; 286 case STFSUX: func = stfs; type = XEU; break; 287 case STFDX: func = stfd; type = XE; break; 288 case STFDUX: func = stfd; type = XEU; break; 289 case STFIWX: func = stfiwx; type = XE; break; 290 default: 291 goto illegal; 292 } 293 break; 294 295 case OP59: 296 switch ((insn >> 1) & 0x1f) { 297 case FDIVS: func = fdivs; type = AB; break; 298 case FSUBS: func = fsubs; type = AB; break; 299 case FADDS: func = fadds; type = AB; break; 300 case FSQRTS: func = fsqrts; type = AB; break; 301 case FRES: func = fres; type = AB; break; 302 case FMULS: func = fmuls; type = AC; break; 303 case FMSUBS: func = fmsubs; type = ABC; break; 304 case FMADDS: func = fmadds; type = ABC; break; 305 case FNMSUBS: func = fnmsubs; type = ABC; break; 306 case FNMADDS: func = fnmadds; type = ABC; break; 307 default: 308 goto illegal; 309 } 310 break; 311 312 case OP63: 313 if (insn & 0x20) { 314 switch ((insn >> 1) & 0x1f) { 315 case FDIV: func = fdiv; type = AB; break; 316 case FSUB: func = fsub; type = AB; break; 317 case FADD: func = fadd; type = AB; break; 318 case FSQRT: func = fsqrt; type = AB; break; 319 case FSEL: func = fsel; type = ABC; break; 320 case FMUL: func = fmul; type = AC; break; 321 case FRSQRTE: func = frsqrte; type = AB; break; 322 case FMSUB: func = fmsub; type = ABC; break; 323 case FMADD: func = fmadd; type = ABC; break; 324 case FNMSUB: func = fnmsub; type = ABC; break; 325 case FNMADD: func = fnmadd; type = ABC; break; 326 default: 327 goto illegal; 328 } 329 break; 330 } 331 332 switch ((insn >> 1) & 0x3ff) { 333 case FCMPU: func = fcmpu; type = XCR; break; 334 case FRSP: func = frsp; type = XB; break; 335 case FCTIW: func = fctiw; type = XB; break; 336 case FCTIWZ: func = fctiwz; type = XB; break; 337 case FCMPO: func = fcmpo; type = XCR; break; 338 case MTFSB1: func = mtfsb1; type = XCRB; break; 339 case FNEG: func = fneg; type = XB; break; 340 case MCRFS: func = mcrfs; type = XCRL; break; 341 case MTFSB0: func = mtfsb0; type = XCRB; break; 342 case FMR: func = fmr; type = XB; break; 343 case MTFSFI: func = mtfsfi; type = XCRI; break; 344 case FNABS: func = fnabs; type = XB; break; 345 case FABS: func = fabs; type = XB; break; 346 case MFFS: func = mffs; type = X; break; 347 case MTFSF: func = mtfsf; type = XFLB; break; 348 default: 349 goto illegal; 350 } 351 break; 352 353 default: 354 goto illegal; 355 } 356 357 switch (type) { 358 case AB: 359 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 360 op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); 361 op2 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 362 break; 363 364 case AC: 365 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 366 op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); 367 op2 = (void *)¤t->thread.TS_FPR((insn >> 6) & 0x1f); 368 break; 369 370 case ABC: 371 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 372 op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); 373 op2 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 374 op3 = (void *)¤t->thread.TS_FPR((insn >> 6) & 0x1f); 375 break; 376 377 case D: 378 idx = (insn >> 16) & 0x1f; 379 sdisp = (insn & 0xffff); 380 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 381 op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); 382 break; 383 384 case DU: 385 idx = (insn >> 16) & 0x1f; 386 if (!idx) 387 goto illegal; 388 389 sdisp = (insn & 0xffff); 390 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 391 op1 = (void *)(regs->gpr[idx] + sdisp); 392 break; 393 394 case X: 395 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 396 break; 397 398 case XA: 399 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 400 op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); 401 break; 402 403 case XB: 404 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 405 op1 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 406 break; 407 408 case XE: 409 idx = (insn >> 16) & 0x1f; 410 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 411 if (!idx) { 412 if (((insn >> 1) & 0x3ff) == STFIWX) 413 op1 = (void *)(regs->gpr[(insn >> 11) & 0x1f]); 414 else 415 goto illegal; 416 } else { 417 op1 = (void *)(regs->gpr[idx] + regs->gpr[(insn >> 11) & 0x1f]); 418 } 419 420 break; 421 422 case XEU: 423 idx = (insn >> 16) & 0x1f; 424 op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); 425 op1 = (void *)((idx ? regs->gpr[idx] : 0) 426 + regs->gpr[(insn >> 11) & 0x1f]); 427 break; 428 429 case XCR: 430 op0 = (void *)®s->ccr; 431 op1 = (void *)((insn >> 23) & 0x7); 432 op2 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); 433 op3 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 434 break; 435 436 case XCRL: 437 op0 = (void *)®s->ccr; 438 op1 = (void *)((insn >> 23) & 0x7); 439 op2 = (void *)((insn >> 18) & 0x7); 440 break; 441 442 case XCRB: 443 op0 = (void *)((insn >> 21) & 0x1f); 444 break; 445 446 case XCRI: 447 op0 = (void *)((insn >> 23) & 0x7); 448 op1 = (void *)((insn >> 12) & 0xf); 449 break; 450 451 case XFLB: 452 op0 = (void *)((insn >> 17) & 0xff); 453 op1 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); 454 break; 455 456 default: 457 goto illegal; 458 } 459 460 eflag = func(op0, op1, op2, op3); 461 462 if (insn & 1) { 463 regs->ccr &= ~(0x0f000000); 464 regs->ccr |= (__FPU_FPSCR >> 4) & 0x0f000000; 465 } 466 467 trap = record_exception(regs, eflag); 468 if (trap) 469 return 1; 470 471 switch (type) { 472 case DU: 473 case XEU: 474 regs->gpr[idx] = (unsigned long)op1; 475 break; 476 477 default: 478 break; 479 } 480 #endif /* CONFIG_MATH_EMULATION */ 481 482 regs->nip += 4; 483 return 0; 484 485 illegal: 486 return -ENOSYS; 487 } 488