arch/powerpc/math-emu/math_efp.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * arch/powerpc/math-emu/math_efp.c
   4  *
   5  * Copyright (C) 2006-2008, 2010 Freescale Semiconductor, Inc.
   6  *
   7  * Author: Ebony Zhu,   <ebony.zhu@freescale.com>
   8  *         Yu Liu,      <yu.liu@freescale.com>
   9  *
  10  * Derived from arch/alpha/math-emu/math.c
  11  *              arch/powerpc/math-emu/math.c
  12  *
  13  * Description:
  14  * This file is the exception handler to make E500 SPE instructions
  15  * fully comply with IEEE-754 floating point standard.
  16  */
  17
  18 #include <linux/types.h>
  19 #include <linux/prctl.h>
  20 #include <linux/module.h>
  21
  22 #include <linux/uaccess.h>
  23 #include <asm/reg.h>
  24
  25 #define FP_EX_BOOKE_E500_SPE
  26 #include <asm/sfp-machine.h>
  27
  28 #include <math-emu/soft-fp.h>
  29 #include <math-emu/single.h>
  30 #include <math-emu/double.h>
  31
  32 #define EFAPU           0x4
  33
  34 #define VCT             0x4
  35 #define SPFP            0x6
  36 #define DPFP            0x7
  37
  38 #define EFSADD          0x2c0
  39 #define EFSSUB          0x2c1
  40 #define EFSABS          0x2c4
  41 #define EFSNABS         0x2c5
  42 #define EFSNEG          0x2c6
  43 #define EFSMUL          0x2c8
  44 #define EFSDIV          0x2c9
  45 #define EFSCMPGT        0x2cc
  46 #define EFSCMPLT        0x2cd
  47 #define EFSCMPEQ        0x2ce
  48 #define EFSCFD          0x2cf
  49 #define EFSCFSI         0x2d1
  50 #define EFSCTUI         0x2d4
  51 #define EFSCTSI         0x2d5
  52 #define EFSCTUF         0x2d6
  53 #define EFSCTSF         0x2d7
  54 #define EFSCTUIZ        0x2d8
  55 #define EFSCTSIZ        0x2da
  56
  57 #define EVFSADD         0x280
  58 #define EVFSSUB         0x281
  59 #define EVFSABS         0x284
  60 #define EVFSNABS        0x285
  61 #define EVFSNEG         0x286
  62 #define EVFSMUL         0x288
  63 #define EVFSDIV         0x289
  64 #define EVFSCMPGT       0x28c
  65 #define EVFSCMPLT       0x28d
  66 #define EVFSCMPEQ       0x28e
  67 #define EVFSCTUI        0x294
  68 #define EVFSCTSI        0x295
  69 #define EVFSCTUF        0x296
  70 #define EVFSCTSF        0x297
  71 #define EVFSCTUIZ       0x298
  72 #define EVFSCTSIZ       0x29a
  73
  74 #define EFDADD          0x2e0
  75 #define EFDSUB          0x2e1
  76 #define EFDABS          0x2e4
  77 #define EFDNABS         0x2e5
  78 #define EFDNEG          0x2e6
  79 #define EFDMUL          0x2e8
  80 #define EFDDIV          0x2e9
  81 #define EFDCTUIDZ       0x2ea
  82 #define EFDCTSIDZ       0x2eb
  83 #define EFDCMPGT        0x2ec
  84 #define EFDCMPLT        0x2ed
  85 #define EFDCMPEQ        0x2ee
  86 #define EFDCFS          0x2ef
  87 #define EFDCTUI         0x2f4
  88 #define EFDCTSI         0x2f5
  89 #define EFDCTUF         0x2f6
  90 #define EFDCTSF         0x2f7
  91 #define EFDCTUIZ        0x2f8
  92 #define EFDCTSIZ        0x2fa
  93
  94 #define AB      2
  95 #define XA      3
  96 #define XB      4
  97 #define XCR     5
  98 #define NOTYPE  0
  99
 100 #define SIGN_BIT_S      (1UL << 31)
 101 #define SIGN_BIT_D      (1ULL << 63)
 102 #define FP_EX_MASK      (FP_EX_INEXACT | FP_EX_INVALID | FP_EX_DIVZERO | \
 103                         FP_EX_UNDERFLOW | FP_EX_OVERFLOW)
 104
 105 static int have_e500_cpu_a005_erratum;
 106
 107 union dw_union {
 108         u64 dp[1];
 109         u32 wp[2];
 110 };
 111
 112 static unsigned long insn_type(unsigned long speinsn)
 113 {
 114         unsigned long ret = NOTYPE;
 115
 116         switch (speinsn & 0x7ff) {
 117         case EFSABS:    ret = XA;       break;
 118         case EFSADD:    ret = AB;       break;
 119         case EFSCFD:    ret = XB;       break;
 120         case EFSCMPEQ:  ret = XCR;      break;
 121         case EFSCMPGT:  ret = XCR;      break;
 122         case EFSCMPLT:  ret = XCR;      break;
 123         case EFSCTSF:   ret = XB;       break;
 124         case EFSCTSI:   ret = XB;       break;
 125         case EFSCTSIZ:  ret = XB;       break;
 126         case EFSCTUF:   ret = XB;       break;
 127         case EFSCTUI:   ret = XB;       break;
 128         case EFSCTUIZ:  ret = XB;       break;
 129         case EFSDIV:    ret = AB;       break;
 130         case EFSMUL:    ret = AB;       break;
 131         case EFSNABS:   ret = XA;       break;
 132         case EFSNEG:    ret = XA;       break;
 133         case EFSSUB:    ret = AB;       break;
 134         case EFSCFSI:   ret = XB;       break;
 135
 136         case EVFSABS:   ret = XA;       break;
 137         case EVFSADD:   ret = AB;       break;
 138         case EVFSCMPEQ: ret = XCR;      break;
 139         case EVFSCMPGT: ret = XCR;      break;
 140         case EVFSCMPLT: ret = XCR;      break;
 141         case EVFSCTSF:  ret = XB;       break;
 142         case EVFSCTSI:  ret = XB;       break;
 143         case EVFSCTSIZ: ret = XB;       break;
 144         case EVFSCTUF:  ret = XB;       break;
 145         case EVFSCTUI:  ret = XB;       break;
 146         case EVFSCTUIZ: ret = XB;       break;
 147         case EVFSDIV:   ret = AB;       break;
 148         case EVFSMUL:   ret = AB;       break;
 149         case EVFSNABS:  ret = XA;       break;
 150         case EVFSNEG:   ret = XA;       break;
 151         case EVFSSUB:   ret = AB;       break;
 152
 153         case EFDABS:    ret = XA;       break;
 154         case EFDADD:    ret = AB;       break;
 155         case EFDCFS:    ret = XB;       break;
 156         case EFDCMPEQ:  ret = XCR;      break;
 157         case EFDCMPGT:  ret = XCR;      break;
 158         case EFDCMPLT:  ret = XCR;      break;
 159         case EFDCTSF:   ret = XB;       break;
 160         case EFDCTSI:   ret = XB;       break;
 161         case EFDCTSIDZ: ret = XB;       break;
 162         case EFDCTSIZ:  ret = XB;       break;
 163         case EFDCTUF:   ret = XB;       break;
 164         case EFDCTUI:   ret = XB;       break;
 165         case EFDCTUIDZ: ret = XB;       break;
 166         case EFDCTUIZ:  ret = XB;       break;
 167         case EFDDIV:    ret = AB;       break;
 168         case EFDMUL:    ret = AB;       break;
 169         case EFDNABS:   ret = XA;       break;
 170         case EFDNEG:    ret = XA;       break;
 171         case EFDSUB:    ret = AB;       break;
 172         }
 173
 174         return ret;
 175 }
 176
 177 int do_spe_mathemu(struct pt_regs *regs)
 178 {
 179         FP_DECL_EX;
 180         int IR, cmp;
 181
 182         unsigned long type, func, fc, fa, fb, src, speinsn;
 183         union dw_union vc, va, vb;
 184
 185         if (get_user(speinsn, (unsigned int __user *) regs->nip))
 186                 return -EFAULT;
 187         if ((speinsn >> 26) != EFAPU)
 188                 return -EINVAL;         /* not an spe instruction */
 189
 190         type = insn_type(speinsn);
 191         if (type == NOTYPE)
 192                 goto illegal;
 193
 194         func = speinsn & 0x7ff;
 195         fc = (speinsn >> 21) & 0x1f;
 196         fa = (speinsn >> 16) & 0x1f;
 197         fb = (speinsn >> 11) & 0x1f;
 198         src = (speinsn >> 5) & 0x7;
 199
 200         vc.wp[0] = current->thread.evr[fc];
 201         vc.wp[1] = regs->gpr[fc];
 202         va.wp[0] = current->thread.evr[fa];
 203         va.wp[1] = regs->gpr[fa];
 204         vb.wp[0] = current->thread.evr[fb];
 205         vb.wp[1] = regs->gpr[fb];
 206
 207         __FPU_FPSCR = mfspr(SPRN_SPEFSCR);
 208
 209         pr_debug("speinsn:%08lx spefscr:%08lx\n", speinsn, __FPU_FPSCR);
 210         pr_debug("vc: %08x  %08x\n", vc.wp[0], vc.wp[1]);
 211         pr_debug("va: %08x  %08x\n", va.wp[0], va.wp[1]);
 212         pr_debug("vb: %08x  %08x\n", vb.wp[0], vb.wp[1]);
 213
 214         switch (src) {
 215         case SPFP: {
 216                 FP_DECL_S(SA); FP_DECL_S(SB); FP_DECL_S(SR);
 217
 218                 switch (type) {
 219                 case AB:
 220                 case XCR:
 221                         FP_UNPACK_SP(SA, va.wp + 1);
 222                 case XB:
 223                         FP_UNPACK_SP(SB, vb.wp + 1);
 224                         break;
 225                 case XA:
 226                         FP_UNPACK_SP(SA, va.wp + 1);
 227                         break;
 228                 }
 229
 230                 pr_debug("SA: %ld %08lx %ld (%ld)\n", SA_s, SA_f, SA_e, SA_c);
 231                 pr_debug("SB: %ld %08lx %ld (%ld)\n", SB_s, SB_f, SB_e, SB_c);
 232
 233                 switch (func) {
 234                 case EFSABS:
 235                         vc.wp[1] = va.wp[1] & ~SIGN_BIT_S;
 236                         goto update_regs;
 237
 238                 case EFSNABS:
 239                         vc.wp[1] = va.wp[1] | SIGN_BIT_S;
 240                         goto update_regs;
 241
 242                 case EFSNEG:
 243                         vc.wp[1] = va.wp[1] ^ SIGN_BIT_S;
 244                         goto update_regs;
 245
 246                 case EFSADD:
 247                         FP_ADD_S(SR, SA, SB);
 248                         goto pack_s;
 249
 250                 case EFSSUB:
 251                         FP_SUB_S(SR, SA, SB);
 252                         goto pack_s;
 253
 254                 case EFSMUL:
 255                         FP_MUL_S(SR, SA, SB);
 256                         goto pack_s;
 257
 258                 case EFSDIV:
 259                         FP_DIV_S(SR, SA, SB);
 260                         goto pack_s;
 261
 262                 case EFSCMPEQ:
 263                         cmp = 0;
 264                         goto cmp_s;
 265
 266                 case EFSCMPGT:
 267                         cmp = 1;
 268                         goto cmp_s;
 269
 270                 case EFSCMPLT:
 271                         cmp = -1;
 272                         goto cmp_s;
 273
 274                 case EFSCTSF:
 275                 case EFSCTUF:
 276                         if (SB_c == FP_CLS_NAN) {
 277                                 vc.wp[1] = 0;
 278                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 279                         } else {
 280                                 SB_e += (func == EFSCTSF ? 31 : 32);
 281                                 FP_TO_INT_ROUND_S(vc.wp[1], SB, 32,
 282                                                 (func == EFSCTSF));
 283                         }
 284                         goto update_regs;
 285
 286                 case EFSCFD: {
 287                         FP_DECL_D(DB);
 288                         FP_CLEAR_EXCEPTIONS;
 289                         FP_UNPACK_DP(DB, vb.dp);
 290
 291                         pr_debug("DB: %ld %08lx %08lx %ld (%ld)\n",
 292                                         DB_s, DB_f1, DB_f0, DB_e, DB_c);
 293
 294                         FP_CONV(S, D, 1, 2, SR, DB);
 295                         goto pack_s;
 296                 }
 297
 298                 case EFSCTSI:
 299                 case EFSCTUI:
 300                         if (SB_c == FP_CLS_NAN) {
 301                                 vc.wp[1] = 0;
 302                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 303                         } else {
 304                                 FP_TO_INT_ROUND_S(vc.wp[1], SB, 32,
 305                                                 ((func & 0x3) != 0));
 306                         }
 307                         goto update_regs;
 308
 309                 case EFSCTSIZ:
 310                 case EFSCTUIZ:
 311                         if (SB_c == FP_CLS_NAN) {
 312                                 vc.wp[1] = 0;
 313                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 314                         } else {
 315                                 FP_TO_INT_S(vc.wp[1], SB, 32,
 316                                                 ((func & 0x3) != 0));
 317                         }
 318                         goto update_regs;
 319
 320                 default:
 321                         goto illegal;
 322                 }
 323                 break;
 324
 325 pack_s:
 326                 pr_debug("SR: %ld %08lx %ld (%ld)\n", SR_s, SR_f, SR_e, SR_c);
 327
 328                 FP_PACK_SP(vc.wp + 1, SR);
 329                 goto update_regs;
 330
 331 cmp_s:
 332                 FP_CMP_S(IR, SA, SB, 3);
 333                 if (IR == 3 && (FP_ISSIGNAN_S(SA) || FP_ISSIGNAN_S(SB)))
 334                         FP_SET_EXCEPTION(FP_EX_INVALID);
 335                 if (IR == cmp) {
 336                         IR = 0x4;
 337                 } else {
 338                         IR = 0;
 339                 }
 340                 goto update_ccr;
 341         }
 342
 343         case DPFP: {
 344                 FP_DECL_D(DA); FP_DECL_D(DB); FP_DECL_D(DR);
 345
 346                 switch (type) {
 347                 case AB:
 348                 case XCR:
 349                         FP_UNPACK_DP(DA, va.dp);
 350                 case XB:
 351                         FP_UNPACK_DP(DB, vb.dp);
 352                         break;
 353                 case XA:
 354                         FP_UNPACK_DP(DA, va.dp);
 355                         break;
 356                 }
 357
 358                 pr_debug("DA: %ld %08lx %08lx %ld (%ld)\n",
 359                                 DA_s, DA_f1, DA_f0, DA_e, DA_c);
 360                 pr_debug("DB: %ld %08lx %08lx %ld (%ld)\n",
 361                                 DB_s, DB_f1, DB_f0, DB_e, DB_c);
 362
 363                 switch (func) {
 364                 case EFDABS:
 365                         vc.dp[0] = va.dp[0] & ~SIGN_BIT_D;
 366                         goto update_regs;
 367
 368                 case EFDNABS:
 369                         vc.dp[0] = va.dp[0] | SIGN_BIT_D;
 370                         goto update_regs;
 371
 372                 case EFDNEG:
 373                         vc.dp[0] = va.dp[0] ^ SIGN_BIT_D;
 374                         goto update_regs;
 375
 376                 case EFDADD:
 377                         FP_ADD_D(DR, DA, DB);
 378                         goto pack_d;
 379
 380                 case EFDSUB:
 381                         FP_SUB_D(DR, DA, DB);
 382                         goto pack_d;
 383
 384                 case EFDMUL:
 385                         FP_MUL_D(DR, DA, DB);
 386                         goto pack_d;
 387
 388                 case EFDDIV:
 389                         FP_DIV_D(DR, DA, DB);
 390                         goto pack_d;
 391
 392                 case EFDCMPEQ:
 393                         cmp = 0;
 394                         goto cmp_d;
 395
 396                 case EFDCMPGT:
 397                         cmp = 1;
 398                         goto cmp_d;
 399
 400                 case EFDCMPLT:
 401                         cmp = -1;
 402                         goto cmp_d;
 403
 404                 case EFDCTSF:
 405                 case EFDCTUF:
 406                         if (DB_c == FP_CLS_NAN) {
 407                                 vc.wp[1] = 0;
 408                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 409                         } else {
 410                                 DB_e += (func == EFDCTSF ? 31 : 32);
 411                                 FP_TO_INT_ROUND_D(vc.wp[1], DB, 32,
 412                                                 (func == EFDCTSF));
 413                         }
 414                         goto update_regs;
 415
 416                 case EFDCFS: {
 417                         FP_DECL_S(SB);
 418                         FP_CLEAR_EXCEPTIONS;
 419                         FP_UNPACK_SP(SB, vb.wp + 1);
 420
 421                         pr_debug("SB: %ld %08lx %ld (%ld)\n",
 422                                         SB_s, SB_f, SB_e, SB_c);
 423
 424                         FP_CONV(D, S, 2, 1, DR, SB);
 425                         goto pack_d;
 426                 }
 427
 428                 case EFDCTUIDZ:
 429                 case EFDCTSIDZ:
 430                         if (DB_c == FP_CLS_NAN) {
 431                                 vc.dp[0] = 0;
 432                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 433                         } else {
 434                                 FP_TO_INT_D(vc.dp[0], DB, 64,
 435                                                 ((func & 0x1) == 0));
 436                         }
 437                         goto update_regs;
 438
 439                 case EFDCTUI:
 440                 case EFDCTSI:
 441                         if (DB_c == FP_CLS_NAN) {
 442                                 vc.wp[1] = 0;
 443                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 444                         } else {
 445                                 FP_TO_INT_ROUND_D(vc.wp[1], DB, 32,
 446                                                 ((func & 0x3) != 0));
 447                         }
 448                         goto update_regs;
 449
 450                 case EFDCTUIZ:
 451                 case EFDCTSIZ:
 452                         if (DB_c == FP_CLS_NAN) {
 453                                 vc.wp[1] = 0;
 454                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 455                         } else {
 456                                 FP_TO_INT_D(vc.wp[1], DB, 32,
 457                                                 ((func & 0x3) != 0));
 458                         }
 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 EVFSCTUF:
 564                 case EVFSCTSF:
 565                         if (SB0_c == FP_CLS_NAN) {
 566                                 vc.wp[0] = 0;
 567                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 568                         } else {
 569                                 SB0_e += (func == EVFSCTSF ? 31 : 32);
 570                                 FP_TO_INT_ROUND_S(vc.wp[0], SB0, 32,
 571                                                 (func == EVFSCTSF));
 572                         }
 573                         if (SB1_c == FP_CLS_NAN) {
 574                                 vc.wp[1] = 0;
 575                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 576                         } else {
 577                                 SB1_e += (func == EVFSCTSF ? 31 : 32);
 578                                 FP_TO_INT_ROUND_S(vc.wp[1], SB1, 32,
 579                                                 (func == EVFSCTSF));
 580                         }
 581                         goto update_regs;
 582
 583                 case EVFSCTUI:
 584                 case EVFSCTSI:
 585                         if (SB0_c == FP_CLS_NAN) {
 586                                 vc.wp[0] = 0;
 587                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 588                         } else {
 589                                 FP_TO_INT_ROUND_S(vc.wp[0], SB0, 32,
 590                                                 ((func & 0x3) != 0));
 591                         }
 592                         if (SB1_c == FP_CLS_NAN) {
 593                                 vc.wp[1] = 0;
 594                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 595                         } else {
 596                                 FP_TO_INT_ROUND_S(vc.wp[1], SB1, 32,
 597                                                 ((func & 0x3) != 0));
 598                         }
 599                         goto update_regs;
 600
 601                 case EVFSCTUIZ:
 602                 case EVFSCTSIZ:
 603                         if (SB0_c == FP_CLS_NAN) {
 604                                 vc.wp[0] = 0;
 605                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 606                         } else {
 607                                 FP_TO_INT_S(vc.wp[0], SB0, 32,
 608                                                 ((func & 0x3) != 0));
 609                         }
 610                         if (SB1_c == FP_CLS_NAN) {
 611                                 vc.wp[1] = 0;
 612                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 613                         } else {
 614                                 FP_TO_INT_S(vc.wp[1], SB1, 32,
 615                                                 ((func & 0x3) != 0));
 616                         }
 617                         goto update_regs;
 618
 619                 default:
 620                         goto illegal;
 621                 }
 622                 break;
 623
 624 pack_vs:
 625                 pr_debug("SR0: %ld %08lx %ld (%ld)\n",
 626                                 SR0_s, SR0_f, SR0_e, SR0_c);
 627                 pr_debug("SR1: %ld %08lx %ld (%ld)\n",
 628                                 SR1_s, SR1_f, SR1_e, SR1_c);
 629
 630                 FP_PACK_SP(vc.wp, SR0);
 631                 FP_PACK_SP(vc.wp + 1, SR1);
 632                 goto update_regs;
 633
 634 cmp_vs:
 635                 {
 636                         int ch, cl;
 637
 638                         FP_CMP_S(IR0, SA0, SB0, 3);
 639                         FP_CMP_S(IR1, SA1, SB1, 3);
 640                         if (IR0 == 3 && (FP_ISSIGNAN_S(SA0) || FP_ISSIGNAN_S(SB0)))
 641                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 642                         if (IR1 == 3 && (FP_ISSIGNAN_S(SA1) || FP_ISSIGNAN_S(SB1)))
 643                                 FP_SET_EXCEPTION(FP_EX_INVALID);
 644                         ch = (IR0 == cmp) ? 1 : 0;
 645                         cl = (IR1 == cmp) ? 1 : 0;
 646                         IR = (ch << 3) | (cl << 2) | ((ch | cl) << 1) |
 647                                 ((ch & cl) << 0);
 648                         goto update_ccr;
 649                 }
 650         }
 651         default:
 652                 return -EINVAL;
 653         }
 654
 655 update_ccr:
 656         regs->ccr &= ~(15 << ((7 - ((speinsn >> 23) & 0x7)) << 2));
 657         regs->ccr |= (IR << ((7 - ((speinsn >> 23) & 0x7)) << 2));
 658
 659 update_regs:
 660         /*
 661          * If the "invalid" exception sticky bit was set by the
 662          * processor for non-finite input, but was not set before the
 663          * instruction being emulated, clear it.  Likewise for the
 664          * "underflow" bit, which may have been set by the processor
 665          * for exact underflow, not just inexact underflow when the
 666          * flag should be set for IEEE 754 semantics.  Other sticky
 667          * exceptions will only be set by the processor when they are
 668          * correct according to IEEE 754 semantics, and we must not
 669          * clear sticky bits that were already set before the emulated
 670          * instruction as they represent the user-visible sticky
 671          * exception status.  "inexact" traps to kernel are not
 672          * required for IEEE semantics and are not enabled by default,
 673          * so the "inexact" sticky bit may have been set by a previous
 674          * instruction without the kernel being aware of it.
 675          */
 676         __FPU_FPSCR
 677           &= ~(FP_EX_INVALID | FP_EX_UNDERFLOW) | current->thread.spefscr_last;
 678         __FPU_FPSCR |= (FP_CUR_EXCEPTIONS & FP_EX_MASK);
 679         mtspr(SPRN_SPEFSCR, __FPU_FPSCR);
 680         current->thread.spefscr_last = __FPU_FPSCR;
 681
 682         current->thread.evr[fc] = vc.wp[0];
 683         regs->gpr[fc] = vc.wp[1];
 684
 685         pr_debug("ccr = %08lx\n", regs->ccr);
 686         pr_debug("cur exceptions = %08x spefscr = %08lx\n",
 687                         FP_CUR_EXCEPTIONS, __FPU_FPSCR);
 688         pr_debug("vc: %08x  %08x\n", vc.wp[0], vc.wp[1]);
 689         pr_debug("va: %08x  %08x\n", va.wp[0], va.wp[1]);
 690         pr_debug("vb: %08x  %08x\n", vb.wp[0], vb.wp[1]);
 691
 692         if (current->thread.fpexc_mode & PR_FP_EXC_SW_ENABLE) {
 693                 if ((FP_CUR_EXCEPTIONS & FP_EX_DIVZERO)
 694                     && (current->thread.fpexc_mode & PR_FP_EXC_DIV))
 695                         return 1;
 696                 if ((FP_CUR_EXCEPTIONS & FP_EX_OVERFLOW)
 697                     && (current->thread.fpexc_mode & PR_FP_EXC_OVF))
 698                         return 1;
 699                 if ((FP_CUR_EXCEPTIONS & FP_EX_UNDERFLOW)
 700                     && (current->thread.fpexc_mode & PR_FP_EXC_UND))
 701                         return 1;
 702                 if ((FP_CUR_EXCEPTIONS & FP_EX_INEXACT)
 703                     && (current->thread.fpexc_mode & PR_FP_EXC_RES))
 704                         return 1;
 705                 if ((FP_CUR_EXCEPTIONS & FP_EX_INVALID)
 706                     && (current->thread.fpexc_mode & PR_FP_EXC_INV))
 707                         return 1;
 708         }
 709         return 0;
 710
 711 illegal:
 712         if (have_e500_cpu_a005_erratum) {
 713                 /* according to e500 cpu a005 erratum, reissue efp inst */
 714                 regs->nip -= 4;
 715                 pr_debug("re-issue efp inst: %08lx\n", speinsn);
 716                 return 0;
 717         }
 718
 719         printk(KERN_ERR "\nOoops! IEEE-754 compliance handler encountered un-supported instruction.\ninst code: %08lx\n", speinsn);
 720         return -ENOSYS;
 721 }
 722
 723 int speround_handler(struct pt_regs *regs)
 724 {
 725         union dw_union fgpr;
 726         int s_lo, s_hi;
 727         int lo_inexact, hi_inexact;
 728         int fp_result;
 729         unsigned long speinsn, type, fb, fc, fptype, func;
 730
 731         if (get_user(speinsn, (unsigned int __user *) regs->nip))
 732                 return -EFAULT;
 733         if ((speinsn >> 26) != 4)
 734                 return -EINVAL;         /* not an spe instruction */
 735
 736         func = speinsn & 0x7ff;
 737         type = insn_type(func);
 738         if (type == XCR) return -ENOSYS;
 739
 740         __FPU_FPSCR = mfspr(SPRN_SPEFSCR);
 741         pr_debug("speinsn:%08lx spefscr:%08lx\n", speinsn, __FPU_FPSCR);
 742
 743         fptype = (speinsn >> 5) & 0x7;
 744
 745         /* No need to round if the result is exact */
 746         lo_inexact = __FPU_FPSCR & (SPEFSCR_FG | SPEFSCR_FX);
 747         hi_inexact = __FPU_FPSCR & (SPEFSCR_FGH | SPEFSCR_FXH);
 748         if (!(lo_inexact || (hi_inexact && fptype == VCT)))
 749                 return 0;
 750
 751         fc = (speinsn >> 21) & 0x1f;
 752         s_lo = regs->gpr[fc] & SIGN_BIT_S;
 753         s_hi = current->thread.evr[fc] & SIGN_BIT_S;
 754         fgpr.wp[0] = current->thread.evr[fc];
 755         fgpr.wp[1] = regs->gpr[fc];
 756
 757         fb = (speinsn >> 11) & 0x1f;
 758         switch (func) {
 759         case EFSCTUIZ:
 760         case EFSCTSIZ:
 761         case EVFSCTUIZ:
 762         case EVFSCTSIZ:
 763         case EFDCTUIDZ:
 764         case EFDCTSIDZ:
 765         case EFDCTUIZ:
 766         case EFDCTSIZ:
 767                 /*
 768                  * These instructions always round to zero,
 769                  * independent of the rounding mode.
 770                  */
 771                 return 0;
 772
 773         case EFSCTUI:
 774         case EFSCTUF:
 775         case EVFSCTUI:
 776         case EVFSCTUF:
 777         case EFDCTUI:
 778         case EFDCTUF:
 779                 fp_result = 0;
 780                 s_lo = 0;
 781                 s_hi = 0;
 782                 break;
 783
 784         case EFSCTSI:
 785         case EFSCTSF:
 786                 fp_result = 0;
 787                 /* Recover the sign of a zero result if possible.  */
 788                 if (fgpr.wp[1] == 0)
 789                         s_lo = regs->gpr[fb] & SIGN_BIT_S;
 790                 break;
 791
 792         case EVFSCTSI:
 793         case EVFSCTSF:
 794                 fp_result = 0;
 795                 /* Recover the sign of a zero result if possible.  */
 796                 if (fgpr.wp[1] == 0)
 797                         s_lo = regs->gpr[fb] & SIGN_BIT_S;
 798                 if (fgpr.wp[0] == 0)
 799                         s_hi = current->thread.evr[fb] & SIGN_BIT_S;
 800                 break;
 801
 802         case EFDCTSI:
 803         case EFDCTSF:
 804                 fp_result = 0;
 805                 s_hi = s_lo;
 806                 /* Recover the sign of a zero result if possible.  */
 807                 if (fgpr.wp[1] == 0)
 808                         s_hi = current->thread.evr[fb] & SIGN_BIT_S;
 809                 break;
 810
 811         default:
 812                 fp_result = 1;
 813                 break;
 814         }
 815
 816         pr_debug("round fgpr: %08x  %08x\n", fgpr.wp[0], fgpr.wp[1]);
 817
 818         switch (fptype) {
 819         /* Since SPE instructions on E500 core can handle round to nearest
 820          * and round toward zero with IEEE-754 complied, we just need
 821          * to handle round toward +Inf and round toward -Inf by software.
 822          */
 823         case SPFP:
 824                 if ((FP_ROUNDMODE) == FP_RND_PINF) {
 825                         if (!s_lo) fgpr.wp[1]++; /* Z > 0, choose Z1 */
 826                 } else { /* round to -Inf */
 827                         if (s_lo) {
 828                                 if (fp_result)
 829                                         fgpr.wp[1]++; /* Z < 0, choose Z2 */
 830                                 else
 831                                         fgpr.wp[1]--; /* Z < 0, choose Z2 */
 832                         }
 833                 }
 834                 break;
 835
 836         case DPFP:
 837                 if (FP_ROUNDMODE == FP_RND_PINF) {
 838                         if (!s_hi) {
 839                                 if (fp_result)
 840                                         fgpr.dp[0]++; /* Z > 0, choose Z1 */
 841                                 else
 842                                         fgpr.wp[1]++; /* Z > 0, choose Z1 */
 843                         }
 844                 } else { /* round to -Inf */
 845                         if (s_hi) {
 846                                 if (fp_result)
 847                                         fgpr.dp[0]++; /* Z < 0, choose Z2 */
 848                                 else
 849                                         fgpr.wp[1]--; /* Z < 0, choose Z2 */
 850                         }
 851                 }
 852                 break;
 853
 854         case VCT:
 855                 if (FP_ROUNDMODE == FP_RND_PINF) {
 856                         if (lo_inexact && !s_lo)
 857                                 fgpr.wp[1]++; /* Z_low > 0, choose Z1 */
 858                         if (hi_inexact && !s_hi)
 859                                 fgpr.wp[0]++; /* Z_high word > 0, choose Z1 */
 860                 } else { /* round to -Inf */
 861                         if (lo_inexact && s_lo) {
 862                                 if (fp_result)
 863                                         fgpr.wp[1]++; /* Z_low < 0, choose Z2 */
 864                                 else
 865                                         fgpr.wp[1]--; /* Z_low < 0, choose Z2 */
 866                         }
 867                         if (hi_inexact && s_hi) {
 868                                 if (fp_result)
 869                                         fgpr.wp[0]++; /* Z_high < 0, choose Z2 */
 870                                 else
 871                                         fgpr.wp[0]--; /* Z_high < 0, choose Z2 */
 872                         }
 873                 }
 874                 break;
 875
 876         default:
 877                 return -EINVAL;
 878         }
 879
 880         current->thread.evr[fc] = fgpr.wp[0];
 881         regs->gpr[fc] = fgpr.wp[1];
 882
 883         pr_debug("  to fgpr: %08x  %08x\n", fgpr.wp[0], fgpr.wp[1]);
 884
 885         if (current->thread.fpexc_mode & PR_FP_EXC_SW_ENABLE)
 886                 return (current->thread.fpexc_mode & PR_FP_EXC_RES) ? 1 : 0;
 887         return 0;
 888 }
 889
 890 int __init spe_mathemu_init(void)
 891 {
 892         u32 pvr, maj, min;
 893
 894         pvr = mfspr(SPRN_PVR);
 895
 896         if ((PVR_VER(pvr) == PVR_VER_E500V1) ||
 897             (PVR_VER(pvr) == PVR_VER_E500V2)) {
 898                 maj = PVR_MAJ(pvr);
 899                 min = PVR_MIN(pvr);
 900
 901                 /*
 902                  * E500 revision below 1.1, 2.3, 3.1, 4.1, 5.1
 903                  * need cpu a005 errata workaround
 904                  */
 905                 switch (maj) {
 906                 case 1:
 907                         if (min < 1)
 908                                 have_e500_cpu_a005_erratum = 1;
 909                         break;
 910                 case 2:
 911                         if (min < 3)
 912                                 have_e500_cpu_a005_erratum = 1;
 913                         break;
 914                 case 3:
 915                 case 4:
 916                 case 5:
 917                         if (min < 1)
 918                                 have_e500_cpu_a005_erratum = 1;
 919                         break;
 920                 default:
 921                         break;
 922                 }
 923         }
 924
 925         return 0;
 926 }
 927
 928 module_init(spe_mathemu_init);