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