arch/powerpc/math-emu/math_efp.c

   1 /*
   2  * arch/powerpc/math-emu/math_efp.c
   3  *
   4  * Copyright (C) 2006-2008, 2010 Freescale Semiconductor, Inc.
   5  *
   6  * Author: Ebony Zhu,   <ebony.zhu@freescale.com>
   7  *         Yu Liu,      <yu.liu@freescale.com>
   8  *
   9  * Derived from arch/alpha/math-emu/math.c
  10  *              arch/powerpc/math-emu/math.c
  11  *
  12  * Description:
  13  * This file is the exception handler to make E500 SPE instructions
  14  * fully comply with IEEE-754 floating point standard.
  15  *
  16  * This program is free software; you can redistribute it and/or
  17  * modify it under the terms of the GNU General Public License
  18  * as published by the Free Software Foundation; either version
  19  * 2 of the License, or (at your option) any later version.
  20  */
  21
  22 #include <linux/types.h>
  23
  24 #include <asm/uaccess.h>
  25 #include <asm/reg.h>
  26
  27 #define FP_EX_BOOKE_E500_SPE
  28 #include <asm/sfp-machine.h>
  29
  30 #include <math-emu/soft-fp.h>
  31 #include <math-emu/single.h>
  32 #include <math-emu/double.h>
  33
  34 #define EFAPU           0x4
  35
  36 #define VCT             0x4
  37 #define SPFP            0x6
  38 #define DPFP            0x7
  39
  40 #define EFSADD          0x2c0
  41 #define EFSSUB          0x2c1
  42 #define EFSABS          0x2c4
  43 #define EFSNABS         0x2c5
  44 #define EFSNEG          0x2c6
  45 #define EFSMUL          0x2c8
  46 #define EFSDIV          0x2c9
  47 #define EFSCMPGT        0x2cc
  48 #define EFSCMPLT        0x2cd
  49 #define EFSCMPEQ        0x2ce
  50 #define EFSCFD          0x2cf
  51 #define EFSCFSI         0x2d1
  52 #define EFSCTUI         0x2d4
  53 #define EFSCTSI         0x2d5
  54 #define EFSCTUF         0x2d6
  55 #define EFSCTSF         0x2d7
  56 #define EFSCTUIZ        0x2d8
  57 #define EFSCTSIZ        0x2da
  58
  59 #define EVFSADD         0x280
  60 #define EVFSSUB         0x281
  61 #define EVFSABS         0x284
  62 #define EVFSNABS        0x285
  63 #define EVFSNEG         0x286
  64 #define EVFSMUL         0x288
  65 #define EVFSDIV         0x289
  66 #define EVFSCMPGT       0x28c
  67 #define EVFSCMPLT       0x28d
  68 #define EVFSCMPEQ       0x28e
  69 #define EVFSCTUI        0x294
  70 #define EVFSCTSI        0x295
  71 #define EVFSCTUF        0x296
  72 #define EVFSCTSF        0x297
  73 #define EVFSCTUIZ       0x298
  74 #define EVFSCTSIZ       0x29a
  75
  76 #define EFDADD          0x2e0
  77 #define EFDSUB          0x2e1
  78 #define EFDABS          0x2e4
  79 #define EFDNABS         0x2e5
  80 #define EFDNEG          0x2e6
  81 #define EFDMUL          0x2e8
  82 #define EFDDIV          0x2e9
  83 #define EFDCTUIDZ       0x2ea
  84 #define EFDCTSIDZ       0x2eb
  85 #define EFDCMPGT        0x2ec
  86 #define EFDCMPLT        0x2ed
  87 #define EFDCMPEQ        0x2ee
  88 #define EFDCFS          0x2ef
  89 #define EFDCTUI         0x2f4
  90 #define EFDCTSI         0x2f5
  91 #define EFDCTUF         0x2f6
  92 #define EFDCTSF         0x2f7
  93 #define EFDCTUIZ        0x2f8
  94 #define EFDCTSIZ        0x2fa
  95
  96 #define AB      2
  97 #define XA      3
  98 #define XB      4
  99 #define XCR     5
 100 #define NOTYPE  0
 101
 102 #define SIGN_BIT_S      (1UL << 31)
 103 #define SIGN_BIT_D      (1ULL << 63)
 104 #define FP_EX_MASK      (FP_EX_INEXACT | FP_EX_INVALID | FP_EX_DIVZERO | \
 105                         FP_EX_UNDERFLOW | FP_EX_OVERFLOW)
 106
 107 static int have_e500_cpu_a005_erratum;
 108
 109 union dw_union {
 110         u64 dp[1];
 111         u32 wp[2];
 112 };
 113
 114 static unsigned long insn_type(unsigned long speinsn)
 115 {
 116         unsigned long ret = NOTYPE;
 117
 118         switch (speinsn & 0x7ff) {
 119         case EFSABS:    ret = XA;       break;
 120         case EFSADD:    ret = AB;       break;
 121         case EFSCFD:    ret = XB;       break;
 122         case EFSCMPEQ:  ret = XCR;      break;
 123         case EFSCMPGT:  ret = XCR;      break;
 124         case EFSCMPLT:  ret = XCR;      break;
 125         case EFSCTSF:   ret = XB;       break;
 126         case EFSCTSI:   ret = XB;       break;
 127         case EFSCTSIZ:  ret = XB;       break;
 128         case EFSCTUF:   ret = XB;       break;
 129         case EFSCTUI:   ret = XB;       break;
 130         case EFSCTUIZ:  ret = XB;       break;
 131         case EFSDIV:    ret = AB;       break;
 132         case EFSMUL:    ret = AB;       break;
 133         case EFSNABS:   ret = XA;       break;
 134         case EFSNEG:    ret = XA;       break;
 135         case EFSSUB:    ret = AB;       break;
 136         case EFSCFSI:   ret = XB;       break;
 137
 138         case EVFSABS:   ret = XA;       break;
 139         case EVFSADD:   ret = AB;       break;
 140         case EVFSCMPEQ: ret = XCR;      break;
 141         case EVFSCMPGT: ret = XCR;      break;
 142         case EVFSCMPLT: ret = XCR;      break;
 143         case EVFSCTSF:  ret = XB;       break;
 144         case EVFSCTSI:  ret = XB;       break;
 145         case EVFSCTSIZ: ret = XB;       break;
 146         case EVFSCTUF:  ret = XB;       break;
 147         case EVFSCTUI:  ret = XB;       break;
 148         case EVFSCTUIZ: ret = XB;       break;
 149         case EVFSDIV:   ret = AB;       break;
 150         case EVFSMUL:   ret = AB;       break;
 151         case EVFSNABS:  ret = XA;       break;
 152         case EVFSNEG:   ret = XA;       break;
 153         case EVFSSUB:   ret = AB;       break;
 154
 155         case EFDABS:    ret = XA;       break;
 156         case EFDADD:    ret = AB;       break;
 157         case EFDCFS:    ret = XB;       break;
 158         case EFDCMPEQ:  ret = XCR;      break;
 159         case EFDCMPGT:  ret = XCR;      break;
 160         case EFDCMPLT:  ret = XCR;      break;
 161         case EFDCTSF:   ret = XB;       break;
 162         case EFDCTSI:   ret = XB;       break;
 163         case EFDCTSIDZ: ret = XB;       break;
 164         case EFDCTSIZ:  ret = XB;       break;
 165         case EFDCTUF:   ret = XB;       break;
 166         case EFDCTUI:   ret = XB;       break;
 167         case EFDCTUIDZ: ret = XB;       break;
 168         case EFDCTUIZ:  ret = XB;       break;
 169         case EFDDIV:    ret = AB;       break;
 170         case EFDMUL:    ret = AB;       break;
 171         case EFDNABS:   ret = XA;       break;
 172         case EFDNEG:    ret = XA;       break;
 173         case EFDSUB:    ret = AB;       break;
 174
 175         default:
 176                 printk(KERN_ERR "\nOoops! SPE instruction no type found.");
 177                 printk(KERN_ERR "\ninst code: %08lx\n", speinsn);
 178         }
 179
 180         return ret;
 181 }
 182
 183 int do_spe_mathemu(struct pt_regs *regs)
 184 {
 185         FP_DECL_EX;
 186         int IR, cmp;
 187
 188         unsigned long type, func, fc, fa, fb, src, speinsn;
 189         union dw_union vc, va, vb;
 190
 191         if (get_user(speinsn, (unsigned int __user *) regs->nip))
 192                 return -EFAULT;
 193         if ((speinsn >> 26) != EFAPU)
 194                 return -EINVAL;         /* not an spe instruction */
 195
 196         type = insn_type(speinsn);
 197         if (type == NOTYPE)
 198                 return -ENOSYS;
 199
 200         func = speinsn & 0x7ff;
 201         fc = (speinsn >> 21) & 0x1f;
 202         fa = (speinsn >> 16) & 0x1f;
 203         fb = (speinsn >> 11) & 0x1f;
 204         src = (speinsn >> 5) & 0x7;
 205
 206         vc.wp[0] = current->thread.evr[fc];
 207         vc.wp[1] = regs->gpr[fc];
 208         va.wp[0] = current->thread.evr[fa];
 209         va.wp[1] = regs->gpr[fa];
 210         vb.wp[0] = current->thread.evr[fb];
 211         vb.wp[1] = regs->gpr[fb];
 212
 213         __FPU_FPSCR = mfspr(SPRN_SPEFSCR);
 214
 215         pr_debug("speinsn:%08lx spefscr:%08lx\n", speinsn, __FPU_FPSCR);
 216         pr_debug("vc: %08x  %08x\n", vc.wp[0], vc.wp[1]);
 217         pr_debug("va: %08x  %08x\n", va.wp[0], va.wp[1]);
 218         pr_debug("vb: %08x  %08x\n", vb.wp[0], vb.wp[1]);
 219
 220         switch (src) {
 221         case SPFP: {
 222                 FP_DECL_S(SA); FP_DECL_S(SB); FP_DECL_S(SR);
 223
 224                 switch (type) {
 225                 case AB:
 226                 case XCR:
 227                         FP_UNPACK_SP(SA, va.wp + 1);
 228                 case XB:
 229                         FP_UNPACK_SP(SB, vb.wp + 1);
 230                         break;
 231                 case XA:
 232                         FP_UNPACK_SP(SA, va.wp + 1);
 233                         break;
 234                 }
 235
 236                 pr_debug("SA: %ld %08lx %ld (%ld)\n", SA_s, SA_f, SA_e, SA_c);
 237                 pr_debug("SB: %ld %08lx %ld (%ld)\n", SB_s, SB_f, SB_e, SB_c);
 238
 239                 switch (func) {
 240                 case EFSABS:
 241                         vc.wp[1] = va.wp[1] & ~SIGN_BIT_S;
 242                         goto update_regs;
 243
 244                 case EFSNABS:
 245                         vc.wp[1] = va.wp[1] | SIGN_BIT_S;
 246                         goto update_regs;
 247
 248                 case EFSNEG:
 249                         vc.wp[1] = va.wp[1] ^ SIGN_BIT_S;
 250                         goto update_regs;
 251
 252                 case EFSADD:
 253                         FP_ADD_S(SR, SA, SB);
 254                         goto pack_s;
 255
 256                 case EFSSUB:
 257                         FP_SUB_S(SR, SA, SB);
 258                         goto pack_s;
 259
 260                 case EFSMUL:
 261                         FP_MUL_S(SR, SA, SB);
 262                         goto pack_s;
 263
 264                 case EFSDIV:
 265                         FP_DIV_S(SR, SA, SB);
 266                         goto pack_s;
 267
 268                 case EFSCMPEQ:
 269                         cmp = 0;
 270                         goto cmp_s;
 271
 272                 case EFSCMPGT:
 273                         cmp = 1;
 274                         goto cmp_s;
 275
 276                 case EFSCMPLT:
 277                         cmp = -1;
 278                         goto cmp_s;
 279
 280                 case EFSCTSF:
 281                 case EFSCTUF:
 282                         if (!((vb.wp[1] >> 23) == 0xff && ((vb.wp[1] & 0x7fffff) > 0))) {
 283                                 /* NaN */
 284                                 if (((vb.wp[1] >> 23) & 0xff) == 0) {
 285                                         /* denorm */
 286                                         vc.wp[1] = 0x0;
 287                                 } else if ((vb.wp[1] >> 31) == 0) {
 288                                         /* positive normal */
 289                                         vc.wp[1] = (func == EFSCTSF) ?
 290                                                 0x7fffffff : 0xffffffff;
 291                                 } else { /* negative normal */
 292                                         vc.wp[1] = (func == EFSCTSF) ?
 293                                                 0x80000000 : 0x0;
 294                                 }
 295                         } else { /* rB is NaN */
 296                                 vc.wp[1] = 0x0;
 297                         }
 298                         goto update_regs;
 299
 300                 case EFSCFD: {
 301                         FP_DECL_D(DB);
 302                         FP_CLEAR_EXCEPTIONS;
 303                         FP_UNPACK_DP(DB, vb.dp);
 304
 305                         pr_debug("DB: %ld %08lx %08lx %ld (%ld)\n",
 306                                         DB_s, DB_f1, DB_f0, DB_e, DB_c);
 307
 308                         FP_CONV(S, D, 1, 2, SR, DB);
 309                         goto pack_s;
 310                 }
 311
 312                 case EFSCTSI:
 313                 case EFSCTSIZ:
 314                 case EFSCTUI:
 315                 case EFSCTUIZ:
 316                         if (func & 0x4) {
 317                                 _FP_ROUND(1, SB);
 318                         } else {
 319                                 _FP_ROUND_ZERO(1, SB);
 320                         }
 321                         FP_TO_INT_S(vc.wp[1], SB, 32,
 322                                         (((func & 0x3) != 0) || SB_s));
 323                         goto update_regs;
 324
 325                 default:
 326                         goto illegal;
 327                 }
 328                 break;
 329
 330 pack_s:
 331                 pr_debug("SR: %ld %08lx %ld (%ld)\n", SR_s, SR_f, SR_e, SR_c);
 332
 333                 FP_PACK_SP(vc.wp + 1, SR);
 334                 goto update_regs;
 335
 336 cmp_s:
 337                 FP_CMP_S(IR, SA, SB, 3);
 338                 if (IR == 3 && (FP_ISSIGNAN_S(SA) || FP_ISSIGNAN_S(SB)))
 339                         FP_SET_EXCEPTION(FP_EX_INVALID);
 340                 if (IR == cmp) {
 341                         IR = 0x4;
 342                 } else {
 343                         IR = 0;
 344                 }
 345                 goto update_ccr;
 346         }
 347
 348         case DPFP: {
 349                 FP_DECL_D(DA); FP_DECL_D(DB); FP_DECL_D(DR);
 350
 351                 switch (type) {
 352                 case AB:
 353                 case XCR:
 354                         FP_UNPACK_DP(DA, va.dp);
 355                 case XB:
 356                         FP_UNPACK_DP(DB, vb.dp);
 357                         break;
 358                 case XA:
 359                         FP_UNPACK_DP(DA, va.dp);
 360                         break;
 361                 }
 362
 363                 pr_debug("DA: %ld %08lx %08lx %ld (%ld)\n",
 364                                 DA_s, DA_f1, DA_f0, DA_e, DA_c);
 365                 pr_debug("DB: %ld %08lx %08lx %ld (%ld)\n",
 366                                 DB_s, DB_f1, DB_f0, DB_e, DB_c);
 367
 368                 switch (func) {
 369                 case EFDABS:
 370                         vc.dp[0] = va.dp[0] & ~SIGN_BIT_D;
 371                         goto update_regs;
 372
 373                 case EFDNABS:
 374                         vc.dp[0] = va.dp[0] | SIGN_BIT_D;
 375                         goto update_regs;
 376
 377                 case EFDNEG:
 378                         vc.dp[0] = va.dp[0] ^ SIGN_BIT_D;
 379                         goto update_regs;
 380
 381                 case EFDADD:
 382                         FP_ADD_D(DR, DA, DB);
 383                         goto pack_d;
 384
 385                 case EFDSUB:
 386                         FP_SUB_D(DR, DA, DB);
 387                         goto pack_d;
 388
 389                 case EFDMUL:
 390                         FP_MUL_D(DR, DA, DB);
 391                         goto pack_d;
 392
 393                 case EFDDIV:
 394                         FP_DIV_D(DR, DA, DB);
 395                         goto pack_d;
 396
 397                 case EFDCMPEQ:
 398                         cmp = 0;
 399                         goto cmp_d;
 400
 401                 case EFDCMPGT:
 402                         cmp = 1;
 403                         goto cmp_d;
 404
 405                 case EFDCMPLT:
 406                         cmp = -1;
 407                         goto cmp_d;
 408
 409                 case EFDCTSF:
 410                 case EFDCTUF:
 411                         if (!((vb.wp[0] >> 20) == 0x7ff &&
 412                            ((vb.wp[0] & 0xfffff) > 0 || (vb.wp[1] > 0)))) {
 413                                 /* not a NaN */
 414                                 if (((vb.wp[0] >> 20) & 0x7ff) == 0) {
 415                                         /* denorm */
 416                                         vc.wp[1] = 0x0;
 417                                 } else if ((vb.wp[0] >> 31) == 0) {
 418                                         /* positive normal */
 419                                         vc.wp[1] = (func == EFDCTSF) ?
 420                                                 0x7fffffff : 0xffffffff;
 421                                 } else { /* negative normal */
 422                                         vc.wp[1] = (func == EFDCTSF) ?
 423                                                 0x80000000 : 0x0;
 424                                 }
 425                         } else { /* NaN */
 426                                 vc.wp[1] = 0x0;
 427                         }
 428                         goto update_regs;
 429
 430                 case EFDCFS: {
 431                         FP_DECL_S(SB);
 432                         FP_CLEAR_EXCEPTIONS;
 433                         FP_UNPACK_SP(SB, vb.wp + 1);
 434
 435                         pr_debug("SB: %ld %08lx %ld (%ld)\n",
 436                                         SB_s, SB_f, SB_e, SB_c);
 437
 438                         FP_CONV(D, S, 2, 1, DR, SB);
 439                         goto pack_d;
 440                 }
 441
 442                 case EFDCTUIDZ:
 443                 case EFDCTSIDZ:
 444                         _FP_ROUND_ZERO(2, DB);
 445                         FP_TO_INT_D(vc.dp[0], DB, 64, ((func & 0x1) == 0));
 446                         goto update_regs;
 447
 448                 case EFDCTUI:
 449                 case EFDCTSI:
 450                 case EFDCTUIZ:
 451                 case EFDCTSIZ:
 452                         if (func & 0x4) {
 453                                 _FP_ROUND(2, DB);
 454                         } else {
 455                                 _FP_ROUND_ZERO(2, DB);
 456                         }
 457                         FP_TO_INT_D(vc.wp[1], DB, 32,
 458                                         (((func & 0x3) != 0) || DB_s));
 459                         goto update_regs;
 460
 461                 default:
 462                         goto illegal;
 463                 }
 464                 break;
 465
 466 pack_d:
 467                 pr_debug("DR: %ld %08lx %08lx %ld (%ld)\n",
 468                                 DR_s, DR_f1, DR_f0, DR_e, DR_c);
 469
 470                 FP_PACK_DP(vc.dp, DR);
 471                 goto update_regs;
 472
 473 cmp_d:
 474                 FP_CMP_D(IR, DA, DB, 3);
 475                 if (IR == 3 && (FP_ISSIGNAN_D(DA) || FP_ISSIGNAN_D(DB)))
 476                         FP_SET_EXCEPTION(FP_EX_INVALID);
 477                 if (IR == cmp) {
 478                         IR = 0x4;
 479                 } else {
 480                         IR = 0;
 481                 }
 482                 goto update_ccr;
 483
 484         }
 485
 486         case VCT: {
 487                 FP_DECL_S(SA0); FP_DECL_S(SB0); FP_DECL_S(SR0);
 488                 FP_DECL_S(SA1); FP_DECL_S(SB1); FP_DECL_S(SR1);
 489                 int IR0, IR1;
 490
 491                 switch (type) {
 492                 case AB:
 493                 case XCR:
 494                         FP_UNPACK_SP(SA0, va.wp);
 495                         FP_UNPACK_SP(SA1, va.wp + 1);
 496                 case XB:
 497                         FP_UNPACK_SP(SB0, vb.wp);
 498                         FP_UNPACK_SP(SB1, vb.wp + 1);
 499                         break;
 500                 case XA:
 501                         FP_UNPACK_SP(SA0, va.wp);
 502                         FP_UNPACK_SP(SA1, va.wp + 1);
 503                         break;
 504                 }
 505
 506                 pr_debug("SA0: %ld %08lx %ld (%ld)\n",
 507                                 SA0_s, SA0_f, SA0_e, SA0_c);
 508                 pr_debug("SA1: %ld %08lx %ld (%ld)\n",
 509                                 SA1_s, SA1_f, SA1_e, SA1_c);
 510                 pr_debug("SB0: %ld %08lx %ld (%ld)\n",
 511                                 SB0_s, SB0_f, SB0_e, SB0_c);
 512                 pr_debug("SB1: %ld %08lx %ld (%ld)\n",
 513                                 SB1_s, SB1_f, SB1_e, SB1_c);
 514
 515                 switch (func) {
 516                 case EVFSABS:
 517                         vc.wp[0] = va.wp[0] & ~SIGN_BIT_S;
 518                         vc.wp[1] = va.wp[1] & ~SIGN_BIT_S;
 519                         goto update_regs;
 520
 521                 case EVFSNABS:
 522                         vc.wp[0] = va.wp[0] | SIGN_BIT_S;
 523                         vc.wp[1] = va.wp[1] | SIGN_BIT_S;
 524                         goto update_regs;
 525
 526                 case EVFSNEG:
 527                         vc.wp[0] = va.wp[0] ^ SIGN_BIT_S;
 528                         vc.wp[1] = va.wp[1] ^ SIGN_BIT_S;
 529                         goto update_regs;
 530
 531                 case EVFSADD:
 532                         FP_ADD_S(SR0, SA0, SB0);
 533                         FP_ADD_S(SR1, SA1, SB1);
 534                         goto pack_vs;
 535
 536                 case EVFSSUB:
 537                         FP_SUB_S(SR0, SA0, SB0);
 538                         FP_SUB_S(SR1, SA1, SB1);
 539                         goto pack_vs;
 540
 541                 case EVFSMUL:
 542                         FP_MUL_S(SR0, SA0, SB0);
 543                         FP_MUL_S(SR1, SA1, SB1);
 544                         goto pack_vs;
 545
 546                 case EVFSDIV:
 547                         FP_DIV_S(SR0, SA0, SB0);
 548                         FP_DIV_S(SR1, SA1, SB1);
 549                         goto pack_vs;
 550
 551                 case EVFSCMPEQ:
 552                         cmp = 0;
 553                         goto cmp_vs;
 554
 555                 case EVFSCMPGT:
 556                         cmp = 1;
 557                         goto cmp_vs;
 558
 559                 case EVFSCMPLT:
 560                         cmp = -1;
 561                         goto cmp_vs;
 562
 563                 case EVFSCTSF:
 564                         __asm__ __volatile__ ("mtspr 512, %4\n"
 565                                 "efsctsf %0, %2\n"
 566                                 "efsctsf %1, %3\n"
 567                                 : "=r" (vc.wp[0]), "=r" (vc.wp[1])
 568                                 : "r" (vb.wp[0]), "r" (vb.wp[1]), "r" (0));
 569                         goto update_regs;
 570
 571                 case EVFSCTUF:
 572                         __asm__ __volatile__ ("mtspr 512, %4\n"
 573                                 "efsctuf %0, %2\n"
 574                                 "efsctuf %1, %3\n"
 575                                 : "=r" (vc.wp[0]), "=r" (vc.wp[1])
 576                                 : "r" (vb.wp[0]), "r" (vb.wp[1]), "r" (0));
 577                         goto update_regs;
 578
 579                 case EVFSCTUI:
 580                 case EVFSCTSI:
 581                 case EVFSCTUIZ:
 582                 case EVFSCTSIZ:
 583                         if (func & 0x4) {
 584                                 _FP_ROUND(1, SB0);
 585                                 _FP_ROUND(1, SB1);
 586                         } else {
 587                                 _FP_ROUND_ZERO(1, SB0);
 588                                 _FP_ROUND_ZERO(1, SB1);
 589                         }
 590                         FP_TO_INT_S(vc.wp[0], SB0, 32,
 591                                         (((func & 0x3) != 0) || SB0_s));
 592                         FP_TO_INT_S(vc.wp[1], SB1, 32,
 593                                         (((func & 0x3) != 0) || SB1_s));
 594                         goto update_regs;
 595
 596                 default:
 597                         goto illegal;
 598                 }
 599                 break;
 600
 601 pack_vs:
 602                 pr_debug("SR0: %ld %08lx %ld (%ld)\n",
 603                                 SR0_s, SR0_f, SR0_e, SR0_c);
 604                 pr_debug("SR1: %ld %08lx %ld (%ld)\n",
 605                                 SR1_s, SR1_f, SR1_e, SR1_c);
 606
 607                 FP_PACK_SP(vc.wp, SR0);
 608                 FP_PACK_SP(vc.wp + 1, SR1);
 609                 goto update_regs;
 610
 611 cmp_vs:
 612                 {
 613                         int ch, cl;
 614
 615                         FP_CMP_S(IR0, SA0, SB0, 3);
 616                         FP_CMP_S(IR1, SA1, SB1, 3);
 617                         if (IR0 == 3 && (FP_ISSIGNAN_S(SA0) || FP_ISSIGNAN_S(SB0)))
 618                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 619                         if (IR1 == 3 && (FP_ISSIGNAN_S(SA1) || FP_ISSIGNAN_S(SB1)))
 620                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 621                         ch = (IR0 == cmp) ? 1 : 0;
 622                         cl = (IR1 == cmp) ? 1 : 0;
 623                         IR = (ch << 3) | (cl << 2) | ((ch | cl) << 1) |
 624                                 ((ch & cl) << 0);
 625                         goto update_ccr;
 626                 }
 627         }
 628         default:
 629                 return -EINVAL;
 630         }
 631
 632 update_ccr:
 633         regs->ccr &= ~(15 << ((7 - ((speinsn >> 23) & 0x7)) << 2));
 634         regs->ccr |= (IR << ((7 - ((speinsn >> 23) & 0x7)) << 2));
 635
 636 update_regs:
 637         __FPU_FPSCR &= ~FP_EX_MASK;
 638         __FPU_FPSCR |= (FP_CUR_EXCEPTIONS & FP_EX_MASK);
 639         mtspr(SPRN_SPEFSCR, __FPU_FPSCR);
 640
 641         current->thread.evr[fc] = vc.wp[0];
 642         regs->gpr[fc] = vc.wp[1];
 643
 644         pr_debug("ccr = %08lx\n", regs->ccr);
 645         pr_debug("cur exceptions = %08x spefscr = %08lx\n",
 646                         FP_CUR_EXCEPTIONS, __FPU_FPSCR);
 647         pr_debug("vc: %08x  %08x\n", vc.wp[0], vc.wp[1]);
 648         pr_debug("va: %08x  %08x\n", va.wp[0], va.wp[1]);
 649         pr_debug("vb: %08x  %08x\n", vb.wp[0], vb.wp[1]);
 650
 651         return 0;
 652
 653 illegal:
 654         if (have_e500_cpu_a005_erratum) {
 655                 /* according to e500 cpu a005 erratum, reissue efp inst */
 656                 regs->nip -= 4;
 657                 pr_debug("re-issue efp inst: %08lx\n", speinsn);
 658                 return 0;
 659         }
 660
 661         printk(KERN_ERR "\nOoops! IEEE-754 compliance handler encountered un-supported instruction.\ninst code: %08lx\n", speinsn);
 662         return -ENOSYS;
 663 }
 664
 665 int speround_handler(struct pt_regs *regs)
 666 {
 667         union dw_union fgpr;
 668         int s_lo, s_hi;
 669         unsigned long speinsn, type, fc;
 670
 671         if (get_user(speinsn, (unsigned int __user *) regs->nip))
 672                 return -EFAULT;
 673         if ((speinsn >> 26) != 4)
 674                 return -EINVAL;         /* not an spe instruction */
 675
 676         type = insn_type(speinsn & 0x7ff);
 677         if (type == XCR) return -ENOSYS;
 678
 679         __FPU_FPSCR = mfspr(SPRN_SPEFSCR);
 680         pr_debug("speinsn:%08lx spefscr:%08lx\n", speinsn, __FPU_FPSCR);
 681
 682         /* No need to round if the result is exact */
 683         if (!(__FPU_FPSCR & FP_EX_INEXACT))
 684                 return 0;
 685
 686         fc = (speinsn >> 21) & 0x1f;
 687         s_lo = regs->gpr[fc] & SIGN_BIT_S;
 688         s_hi = current->thread.evr[fc] & SIGN_BIT_S;
 689         fgpr.wp[0] = current->thread.evr[fc];
 690         fgpr.wp[1] = regs->gpr[fc];
 691
 692         pr_debug("round fgpr: %08x  %08x\n", fgpr.wp[0], fgpr.wp[1]);
 693
 694         switch ((speinsn >> 5) & 0x7) {
 695         /* Since SPE instructions on E500 core can handle round to nearest
 696          * and round toward zero with IEEE-754 complied, we just need
 697          * to handle round toward +Inf and round toward -Inf by software.
 698          */
 699         case SPFP:
 700                 if ((FP_ROUNDMODE) == FP_RND_PINF) {
 701                         if (!s_lo) fgpr.wp[1]++; /* Z > 0, choose Z1 */
 702                 } else { /* round to -Inf */
 703                         if (s_lo) fgpr.wp[1]++; /* Z < 0, choose Z2 */
 704                 }
 705                 break;
 706
 707         case DPFP:
 708                 if (FP_ROUNDMODE == FP_RND_PINF) {
 709                         if (!s_hi) fgpr.dp[0]++; /* Z > 0, choose Z1 */
 710                 } else { /* round to -Inf */
 711                         if (s_hi) fgpr.dp[0]++; /* Z < 0, choose Z2 */
 712                 }
 713                 break;
 714
 715         case VCT:
 716                 if (FP_ROUNDMODE == FP_RND_PINF) {
 717                         if (!s_lo) fgpr.wp[1]++; /* Z_low > 0, choose Z1 */
 718                         if (!s_hi) fgpr.wp[0]++; /* Z_high word > 0, choose Z1 */
 719                 } else { /* round to -Inf */
 720                         if (s_lo) fgpr.wp[1]++; /* Z_low < 0, choose Z2 */
 721                         if (s_hi) fgpr.wp[0]++; /* Z_high < 0, choose Z2 */
 722                 }
 723                 break;
 724
 725         default:
 726                 return -EINVAL;
 727         }
 728
 729         current->thread.evr[fc] = fgpr.wp[0];
 730         regs->gpr[fc] = fgpr.wp[1];
 731
 732         pr_debug("  to fgpr: %08x  %08x\n", fgpr.wp[0], fgpr.wp[1]);
 733
 734         return 0;
 735 }
 736
 737 int __init spe_mathemu_init(void)
 738 {
 739         u32 pvr, maj, min;
 740
 741         pvr = mfspr(SPRN_PVR);
 742
 743         if ((PVR_VER(pvr) == PVR_VER_E500V1) ||
 744             (PVR_VER(pvr) == PVR_VER_E500V2)) {
 745                 maj = PVR_MAJ(pvr);
 746                 min = PVR_MIN(pvr);
 747
 748                 /*
 749                  * E500 revision below 1.1, 2.3, 3.1, 4.1, 5.1
 750                  * need cpu a005 errata workaround
 751                  */
 752                 switch (maj) {
 753                 case 1:
 754                         if (min < 1)
 755                                 have_e500_cpu_a005_erratum = 1;
 756                         break;
 757                 case 2:
 758                         if (min < 3)
 759                                 have_e500_cpu_a005_erratum = 1;
 760                         break;
 761                 case 3:
 762                 case 4:
 763                 case 5:
 764                         if (min < 1)
 765                                 have_e500_cpu_a005_erratum = 1;
 766                         break;
 767                 default:
 768                         break;
 769                 }
 770         }
 771
 772         return 0;
 773 }
 774
 775 module_init(spe_mathemu_init);