arch/powerpc/math-emu/math.c

   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 *)&current->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 *)&current->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 *)&current->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 *)&current->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 *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 268                 op1 = (void *)&current->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 *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 368                 op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
 369                 op2 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
 370                 break;
 371
 372         case AC:
 373                 op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 374                 op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
 375                 op2 = (void *)&current->thread.TS_FPR((insn >>  6) & 0x1f);
 376                 break;
 377
 378         case ABC:
 379                 op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 380                 op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
 381                 op2 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
 382                 op3 = (void *)&current->thread.TS_FPR((insn >>  6) & 0x1f);
 383                 break;
 384
 385         case D:
 386                 idx = (insn >> 16) & 0x1f;
 387                 sdisp = (insn & 0xffff);
 388                 op0 = (void *)&current->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 *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 399                 op1 = (void *)(regs->gpr[idx] + sdisp);
 400                 break;
 401
 402         case X:
 403                 op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 404                 break;
 405
 406         case XA:
 407                 op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 408                 op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
 409                 break;
 410
 411         case XB:
 412                 op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 413                 op1 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
 414                 break;
 415
 416         case XE:
 417                 idx = (insn >> 16) & 0x1f;
 418                 op0 = (void *)&current->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 *)&current->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 *)&regs->ccr;
 439                 op1 = (void *)((insn >> 23) & 0x7);
 440                 op2 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
 441                 op3 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
 442                 break;
 443
 444         case XCRL:
 445                 op0 = (void *)&regs->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 *)&current->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 }