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 16FLOATFUNC(fadd); 17FLOATFUNC(fadds); 18FLOATFUNC(fdiv); 19FLOATFUNC(fdivs); 20FLOATFUNC(fmul); 21FLOATFUNC(fmuls); 22FLOATFUNC(fsub); 23FLOATFUNC(fsubs); 24 25FLOATFUNC(fmadd); 26FLOATFUNC(fmadds); 27FLOATFUNC(fmsub); 28FLOATFUNC(fmsubs); 29FLOATFUNC(fnmadd); 30FLOATFUNC(fnmadds); 31FLOATFUNC(fnmsub); 32FLOATFUNC(fnmsubs); 33 34FLOATFUNC(fctiw); 35FLOATFUNC(fctiwz); 36FLOATFUNC(frsp); 37 38FLOATFUNC(fcmpo); 39FLOATFUNC(fcmpu); 40 41FLOATFUNC(mcrfs); 42FLOATFUNC(mffs); 43FLOATFUNC(mtfsb0); 44FLOATFUNC(mtfsb1); 45FLOATFUNC(mtfsf); 46FLOATFUNC(mtfsfi); 47 48FLOATFUNC(lfd); 49FLOATFUNC(lfs); 50 51FLOATFUNC(stfd); 52FLOATFUNC(stfs); 53FLOATFUNC(stfiwx); 54 55FLOATFUNC(fabs); 56FLOATFUNC(fmr); 57FLOATFUNC(fnabs); 58FLOATFUNC(fneg); 59 60/* Optional */ 61FLOATFUNC(fres); 62FLOATFUNC(frsqrte); 63FLOATFUNC(fsel); 64FLOATFUNC(fsqrt); 65FLOATFUNC(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 154static int 155record_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 213int 214do_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 487illegal: 488 return -ENOSYS; 489} 490