linux/arch/powerpc/math-emu/math.c
<<
>>
Prefs
   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 *)&current->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 *)&current->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 *)&current->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 *)&current->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 *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 264                op1 = (void *)&current->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 *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 362                op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
 363                op2 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
 364                break;
 365
 366        case AC:
 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 >>  6) & 0x1f);
 370                break;
 371
 372        case ABC:
 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 >> 11) & 0x1f);
 376                op3 = (void *)&current->thread.TS_FPR((insn >>  6) & 0x1f);
 377                break;
 378
 379        case D:
 380                idx = (insn >> 16) & 0x1f;
 381                sdisp = (insn & 0xffff);
 382                op0 = (void *)&current->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 *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 393                op1 = (void *)(regs->gpr[idx] + sdisp);
 394                break;
 395
 396        case X:
 397                op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 398                break;
 399
 400        case XA:
 401                op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 402                op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
 403                break;
 404
 405        case XB:
 406                op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
 407                op1 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
 408                break;
 409
 410        case XE:
 411                idx = (insn >> 16) & 0x1f;
 412                op0 = (void *)&current->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 *)&current->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 *)&regs->ccr;
 433                op1 = (void *)((insn >> 23) & 0x7);
 434                op2 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
 435                op3 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
 436                break;
 437
 438        case XCRL:
 439                op0 = (void *)&regs->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 *)&current->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