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