git.sesse.net Git - pistorm/blob - m68kfpu.c

   1 #include <math.h>
   2 #include <stdio.h>
   3 #include <stdarg.h>
   4
   5 extern void exit(int);
   6
   7 static void fatalerror(char *format, ...) {
   8       va_list ap;
   9       va_start(ap,format);
  10       vfprintf(stderr,format,ap);  // JFF: fixed. Was using fprintf and arguments were wrong
  11       va_end(ap);
  12       exit(1);
  13 }
  14
  15 #define FPCC_N                  0x08000000
  16 #define FPCC_Z                  0x04000000
  17 #define FPCC_I                  0x02000000
  18 #define FPCC_NAN                0x01000000
  19
  20 #define DOUBLE_INFINITY                                 (unsigned long long)(0x7ff0000000000000)
  21 #define DOUBLE_EXPONENT                                 (unsigned long long)(0x7ff0000000000000)
  22 #define DOUBLE_MANTISSA                                 (unsigned long long)(0x000fffffffffffff)
  23
  24 extern flag floatx80_is_nan( floatx80 a );
  25
  26 // masks for packed dwords, positive k-factor
  27 static uint32 pkmask2[18] =
  28 {
  29         0xffffffff, 0, 0xf0000000, 0xff000000, 0xfff00000, 0xffff0000,
  30         0xfffff000, 0xffffff00, 0xfffffff0, 0xffffffff,
  31         0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff,
  32         0xffffffff, 0xffffffff, 0xffffffff
  33 };
  34
  35 static uint32 pkmask3[18] =
  36 {
  37         0xffffffff, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  38         0xf0000000, 0xff000000, 0xfff00000, 0xffff0000,
  39         0xfffff000, 0xffffff00, 0xfffffff0, 0xffffffff,
  40 };
  41
  42 static inline double fx80_to_double(floatx80 fx)
  43 {
  44         uint64 d;
  45         double *foo;
  46
  47         foo = (double *)&d;
  48
  49         d = floatx80_to_float64(fx);
  50
  51         return *foo;
  52 }
  53
  54 static inline floatx80 double_to_fx80(double in)
  55 {
  56         uint64 *d;
  57
  58         d = (uint64 *)&in;
  59
  60         return float64_to_floatx80(*d);
  61 }
  62
  63 static inline floatx80 load_extended_float80(uint32 ea)
  64 {
  65         uint32 d1,d2;
  66         uint16 d3;
  67         floatx80 fp;
  68
  69         d3 = m68ki_read_16(ea);
  70         d1 = m68ki_read_32(ea+4);
  71         d2 = m68ki_read_32(ea+8);
  72
  73         fp.high = d3;
  74         fp.low = ((uint64)d1<<32) | (d2 & 0xffffffff);
  75
  76         return fp;
  77 }
  78
  79 static inline void store_extended_float80(uint32 ea, floatx80 fpr)
  80 {
  81         m68ki_write_16(ea+0, fpr.high);
  82         m68ki_write_16(ea+2, 0);
  83         m68ki_write_32(ea+4, (fpr.low>>32)&0xffffffff);
  84         m68ki_write_32(ea+8, fpr.low&0xffffffff);
  85 }
  86
  87 static inline floatx80 load_pack_float80(uint32 ea)
  88 {
  89         uint32 dw1, dw2, dw3;
  90         floatx80 result;
  91         double tmp;
  92         char str[128], *ch;
  93
  94         dw1 = m68ki_read_32(ea);
  95         dw2 = m68ki_read_32(ea+4);
  96         dw3 = m68ki_read_32(ea+8);
  97
  98         ch = &str[0];
  99         if (dw1 & 0x80000000)   // mantissa sign
 100         {
 101                 *ch++ = '-';
 102         }
 103         *ch++ = (char)((dw1 & 0xf) + '0');
 104         *ch++ = '.';
 105         *ch++ = (char)(((dw2 >> 28) & 0xf) + '0');
 106         *ch++ = (char)(((dw2 >> 24) & 0xf) + '0');
 107         *ch++ = (char)(((dw2 >> 20) & 0xf) + '0');
 108         *ch++ = (char)(((dw2 >> 16) & 0xf) + '0');
 109         *ch++ = (char)(((dw2 >> 12) & 0xf) + '0');
 110         *ch++ = (char)(((dw2 >> 8)  & 0xf) + '0');
 111         *ch++ = (char)(((dw2 >> 4)  & 0xf) + '0');
 112         *ch++ = (char)(((dw2 >> 0)  & 0xf) + '0');
 113         *ch++ = (char)(((dw3 >> 28) & 0xf) + '0');
 114         *ch++ = (char)(((dw3 >> 24) & 0xf) + '0');
 115         *ch++ = (char)(((dw3 >> 20) & 0xf) + '0');
 116         *ch++ = (char)(((dw3 >> 16) & 0xf) + '0');
 117         *ch++ = (char)(((dw3 >> 12) & 0xf) + '0');
 118         *ch++ = (char)(((dw3 >> 8)  & 0xf) + '0');
 119         *ch++ = (char)(((dw3 >> 4)  & 0xf) + '0');
 120         *ch++ = (char)(((dw3 >> 0)  & 0xf) + '0');
 121         *ch++ = 'E';
 122         if (dw1 & 0x40000000)   // exponent sign
 123         {
 124                 *ch++ = '-';
 125         }
 126         *ch++ = (char)(((dw1 >> 24) & 0xf) + '0');
 127         *ch++ = (char)(((dw1 >> 20) & 0xf) + '0');
 128         *ch++ = (char)(((dw1 >> 16) & 0xf) + '0');
 129         *ch = '\0';
 130
 131         sscanf(str, "%le", &tmp);
 132
 133         result = double_to_fx80(tmp);
 134
 135         return result;
 136 }
 137
 138 static inline void store_pack_float80(uint32 ea, int k, floatx80 fpr)
 139 {
 140         uint32 dw1, dw2, dw3;
 141         char str[128], *ch;
 142         int i, j, exp;
 143
 144         dw1 = dw2 = dw3 = 0;
 145         ch = &str[0];
 146
 147         sprintf(str, "%.16e", fx80_to_double(fpr));
 148
 149         if (*ch == '-')
 150         {
 151                 ch++;
 152                 dw1 = 0x80000000;
 153         }
 154
 155         if (*ch == '+')
 156         {
 157                 ch++;
 158         }
 159
 160         dw1 |= (*ch++ - '0');
 161
 162         if (*ch == '.')
 163         {
 164                 ch++;
 165         }
 166
 167         // handle negative k-factor here
 168         if ((k <= 0) && (k >= -13))
 169         {
 170                 exp = 0;
 171                 for (i = 0; i < 3; i++)
 172                 {
 173                         if (ch[18+i] >= '0' && ch[18+i] <= '9')
 174                         {
 175                                 exp = (exp << 4) | (ch[18+i] - '0');
 176                         }
 177                 }
 178
 179                 if (ch[17] == '-')
 180                 {
 181                         exp = -exp;
 182                 }
 183
 184                 k = -k;
 185                 // last digit is (k + exponent - 1)
 186                 k += (exp - 1);
 187
 188                 // round up the last significant mantissa digit
 189                 if (ch[k+1] >= '5')
 190                 {
 191                         ch[k]++;
 192                 }
 193
 194                 // zero out the rest of the mantissa digits
 195                 for (j = (k+1); j < 16; j++)
 196                 {
 197                         ch[j] = '0';
 198                 }
 199
 200                 // now zero out K to avoid tripping the positive K detection below
 201                 k = 0;
 202         }
 203
 204         // crack 8 digits of the mantissa
 205         for (i = 0; i < 8; i++)
 206         {
 207                 dw2 <<= 4;
 208                 if (*ch >= '0' && *ch <= '9')
 209                 {
 210                         dw2 |= *ch++ - '0';
 211                 }
 212         }
 213
 214         // next 8 digits of the mantissa
 215         for (i = 0; i < 8; i++)
 216         {
 217                 dw3 <<= 4;
 218                 if (*ch >= '0' && *ch <= '9')
 219                 dw3 |= *ch++ - '0';
 220         }
 221
 222         // handle masking if k is positive
 223         if (k >= 1)
 224         {
 225                 if (k <= 17)
 226                 {
 227                         dw2 &= pkmask2[k];
 228                         dw3 &= pkmask3[k];
 229                 }
 230                 else
 231                 {
 232                         dw2 &= pkmask2[17];
 233                         dw3 &= pkmask3[17];
 234 //                      m68ki_cpu.fpcr |=  (need to set OPERR bit)
 235                 }
 236         }
 237
 238         // finally, crack the exponent
 239         if (*ch == 'e' || *ch == 'E')
 240         {
 241                 ch++;
 242                 if (*ch == '-')
 243                 {
 244                         ch++;
 245                         dw1 |= 0x40000000;
 246                 }
 247
 248                 if (*ch == '+')
 249                 {
 250                         ch++;
 251                 }
 252
 253                 j = 0;
 254                 for (i = 0; i < 3; i++)
 255                 {
 256                         if (*ch >= '0' && *ch <= '9')
 257                         {
 258                                 j = (j << 4) | (*ch++ - '0');
 259                         }
 260                 }
 261
 262                 dw1 |= (j << 16);
 263         }
 264
 265         m68ki_write_32(ea, dw1);
 266         m68ki_write_32(ea+4, dw2);
 267         m68ki_write_32(ea+8, dw3);
 268 }
 269
 270 static inline void SET_CONDITION_CODES(floatx80 reg)
 271 {
 272         REG_FPSR &= ~(FPCC_N|FPCC_Z|FPCC_I|FPCC_NAN);
 273
 274         // sign flag
 275         if (reg.high & 0x8000)
 276         {
 277                 REG_FPSR |= FPCC_N;
 278         }
 279
 280         // zero flag
 281         if (((reg.high & 0x7fff) == 0) && ((reg.low<<1) == 0))
 282         {
 283                 REG_FPSR |= FPCC_Z;
 284         }
 285
 286         // infinity flag
 287         if (((reg.high & 0x7fff) == 0x7fff) && ((reg.low<<1) == 0))
 288         {
 289                 REG_FPSR |= FPCC_I;
 290         }
 291
 292         // NaN flag
 293         if (floatx80_is_nan(reg))
 294         {
 295                 REG_FPSR |= FPCC_NAN;
 296         }
 297 }
 298
 299 static inline int TEST_CONDITION(int condition)
 300 {
 301         int n = (REG_FPSR & FPCC_N) != 0;
 302         int z = (REG_FPSR & FPCC_Z) != 0;
 303         int nan = (REG_FPSR & FPCC_NAN) != 0;
 304         int r = 0;
 305         switch (condition)
 306         {
 307                 case 0x10:
 308                 case 0x00:              return 0;                                       // False
 309
 310                 case 0x11:
 311                 case 0x01:              return (z);                                     // Equal
 312
 313                 case 0x12:
 314                 case 0x02:              return (!(nan || z || n));                      // Greater Than
 315
 316                 case 0x13:
 317                 case 0x03:              return (z || !(nan || n));                      // Greater or Equal
 318
 319                 case 0x14:
 320                 case 0x04:              return (n && !(nan || z));                      // Less Than
 321
 322                 case 0x15:
 323                 case 0x05:              return (z || (n && !nan));                      // Less Than or Equal
 324
 325                 case 0x16:
 326                 case 0x06:              return !nan && !z;
 327
 328                 case 0x17:
 329                 case 0x07:              return !nan;
 330
 331                 case 0x18:
 332                 case 0x08:              return nan;
 333
 334                 case 0x19:
 335                 case 0x09:              return nan || z;
 336
 337                 case 0x1a:
 338                 case 0x0a:              return (nan || !(n || z));                      // Not Less Than or Equal
 339
 340                 case 0x1b:
 341                 case 0x0b:              return (nan || z || !n);                        // Not Less Than
 342
 343                 case 0x1c:
 344                 case 0x0c:              return (nan || (n && !z));                      // Not Greater or Equal Than
 345
 346                 case 0x1d:
 347                 case 0x0d:              return (nan || z || n);                         // Not Greater Than
 348
 349                 case 0x1e:
 350                 case 0x0e:              return (!z);                                    // Not Equal
 351
 352                 case 0x1f:
 353                 case 0x0f:              return 1;                                       // True
 354
 355                 default:                fatalerror("M68kFPU: test_condition: unhandled condition %02X\n", condition);
 356         }
 357
 358         return r;
 359 }
 360
 361 static uint8 READ_EA_8(int ea)
 362 {
 363         int mode = (ea >> 3) & 0x7;
 364         int reg = (ea & 0x7);
 365
 366         switch (mode)
 367         {
 368                 case 0:         // Dn
 369                 {
 370                         return REG_D[reg];
 371                 }
 372                 case 2:         // (An)
 373                 {
 374                         uint32 ea = REG_A[reg];
 375                         return m68ki_read_8(ea);
 376                 }
 377                 case 5:         // (d16, An)
 378                 {
 379                         uint32 ea = EA_AY_DI_8();
 380                         return m68ki_read_8(ea);
 381                 }
 382                 case 6:         // (An) + (Xn) + d8
 383                 {
 384                         uint32 ea = EA_AY_IX_8();
 385                         return m68ki_read_8(ea);
 386                 }
 387                 case 7:
 388                 {
 389                         switch (reg)
 390                         {
 391                                 case 0:         // (xxx).W
 392                                 {
 393                                         uint32 ea = (uint32)OPER_I_16();
 394                                         return m68ki_read_8(ea);
 395                                 }
 396                                 case 1:         // (xxx).L
 397                                 {
 398                                         uint32 d1 = OPER_I_16();
 399                                         uint32 d2 = OPER_I_16();
 400                                         uint32 ea = (d1 << 16) | d2;
 401                                         return m68ki_read_8(ea);
 402                                 }
 403                                 case 4:         // #<data>
 404                                 {
 405                                         return  OPER_I_8();
 406                                 }
 407                                 default:        fatalerror("M68kFPU: READ_EA_8: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 408                         }
 409                         break;
 410                 }
 411                 default:        fatalerror("M68kFPU: READ_EA_8: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 412         }
 413
 414         return 0;
 415 }
 416
 417 static uint16 READ_EA_16(int ea)
 418 {
 419         int mode = (ea >> 3) & 0x7;
 420         int reg = (ea & 0x7);
 421
 422         switch (mode)
 423         {
 424                 case 0:         // Dn
 425                 {
 426                         return (uint16)(REG_D[reg]);
 427                 }
 428                 case 2:         // (An)
 429                 {
 430                         uint32 ea = REG_A[reg];
 431                         return m68ki_read_16(ea);
 432                 }
 433                 case 5:         // (d16, An)
 434                 {
 435                         uint32 ea = EA_AY_DI_16();
 436                         return m68ki_read_16(ea);
 437                 }
 438                 case 6:         // (An) + (Xn) + d8
 439                 {
 440                         uint32 ea = EA_AY_IX_16();
 441                         return m68ki_read_16(ea);
 442                 }
 443                 case 7:
 444                 {
 445                         switch (reg)
 446                         {
 447                                 case 0:         // (xxx).W
 448                                 {
 449                                         uint32 ea = (uint32)OPER_I_16();
 450                                         return m68ki_read_16(ea);
 451                                 }
 452                                 case 1:         // (xxx).L
 453                                 {
 454                                         uint32 d1 = OPER_I_16();
 455                                         uint32 d2 = OPER_I_16();
 456                                         uint32 ea = (d1 << 16) | d2;
 457                                         return m68ki_read_16(ea);
 458                                 }
 459                                 case 4:         // #<data>
 460                                 {
 461                                         return OPER_I_16();
 462                                 }
 463
 464                                 default:        fatalerror("M68kFPU: READ_EA_16: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 465                         }
 466                         break;
 467                 }
 468                 default:        fatalerror("M68kFPU: READ_EA_16: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 469         }
 470
 471         return 0;
 472 }
 473
 474 static uint32 READ_EA_32(int ea)
 475 {
 476         int mode = (ea >> 3) & 0x7;
 477         int reg = (ea & 0x7);
 478
 479         switch (mode)
 480         {
 481                 case 0:         // Dn
 482                 {
 483                         return REG_D[reg];
 484                 }
 485                 case 2:         // (An)
 486                 {
 487                         uint32 ea = REG_A[reg];
 488                         return m68ki_read_32(ea);
 489                 }
 490                 case 3:         // (An)+
 491                 {
 492                         uint32 ea = EA_AY_PI_32();
 493                         return m68ki_read_32(ea);
 494                 }
 495                 case 5:         // (d16, An)
 496                 {
 497                         uint32 ea = EA_AY_DI_32();
 498                         return m68ki_read_32(ea);
 499                 }
 500                 case 6:         // (An) + (Xn) + d8
 501                 {
 502                         uint32 ea = EA_AY_IX_32();
 503                         return m68ki_read_32(ea);
 504                 }
 505                 case 7:
 506                 {
 507                         switch (reg)
 508                         {
 509                                 case 0:         // (xxx).W
 510                                 {
 511                                         uint32 ea = (uint32)OPER_I_16();
 512                                         return m68ki_read_32(ea);
 513                                 }
 514                                 case 1:         // (xxx).L
 515                                 {
 516                                         uint32 d1 = OPER_I_16();
 517                                         uint32 d2 = OPER_I_16();
 518                                         uint32 ea = (d1 << 16) | d2;
 519                                         return m68ki_read_32(ea);
 520                                 }
 521                                 case 2:         // (d16, PC)
 522                                 {
 523                                         uint32 ea = EA_PCDI_32();
 524                                         return m68ki_read_32(ea);
 525                                 }
 526                                 case 4:         // #<data>
 527                                 {
 528                                         return  OPER_I_32();
 529                                 }
 530                                 default:        fatalerror("M68kFPU: READ_EA_32: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 531                         }
 532                         break;
 533                 }
 534                 default:        fatalerror("M68kFPU: READ_EA_32: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 535         }
 536         return 0;
 537 }
 538
 539 static uint64 READ_EA_64(int ea)
 540 {
 541         int mode = (ea >> 3) & 0x7;
 542         int reg = (ea & 0x7);
 543         uint32 h1, h2;
 544
 545         switch (mode)
 546         {
 547                 case 2:         // (An)
 548                 {
 549                         uint32 ea = REG_A[reg];
 550                         h1 = m68ki_read_32(ea+0);
 551                         h2 = m68ki_read_32(ea+4);
 552                         return  (uint64)(h1) << 32 | (uint64)(h2);
 553                 }
 554                 case 3:         // (An)+
 555                 {
 556                         uint32 ea = REG_A[reg];
 557                         REG_A[reg] += 8;
 558                         h1 = m68ki_read_32(ea+0);
 559                         h2 = m68ki_read_32(ea+4);
 560                         return  (uint64)(h1) << 32 | (uint64)(h2);
 561                 }
 562                 case 5:         // (d16, An)
 563                 {
 564                         uint32 ea = EA_AY_DI_32();
 565                         h1 = m68ki_read_32(ea+0);
 566                         h2 = m68ki_read_32(ea+4);
 567                         return  (uint64)(h1) << 32 | (uint64)(h2);
 568                 }
 569                 case 7:
 570                 {
 571                         switch (reg)
 572                         {
 573                                 case 4:         // #<data>
 574                                 {
 575                                         h1 = OPER_I_32();
 576                                         h2 = OPER_I_32();
 577                                         return  (uint64)(h1) << 32 | (uint64)(h2);
 578                                 }
 579                                 case 2:         // (d16, PC)
 580                                 {
 581                                         uint32 ea = EA_PCDI_32();
 582                                         h1 = m68ki_read_32(ea+0);
 583                                         h2 = m68ki_read_32(ea+4);
 584                                         return  (uint64)(h1) << 32 | (uint64)(h2);
 585                                 }
 586                                 default:        fatalerror("M68kFPU: READ_EA_64: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 587                         }
 588                         break;
 589                 }
 590                 default:        fatalerror("M68kFPU: READ_EA_64: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 591         }
 592
 593         return 0;
 594 }
 595
 596
 597 static floatx80 READ_EA_FPE(int mode, int reg, uint32 di_mode_ea)
 598 {
 599         floatx80 fpr;
 600
 601         switch (mode)
 602         {
 603                 case 2:         // (An)
 604                 {
 605                         uint32 ea = REG_A[reg];
 606                         fpr = load_extended_float80(ea);
 607                         break;
 608                 }
 609
 610                 case 3:         // (An)+
 611                 {
 612                         uint32 ea = REG_A[reg];
 613                         REG_A[reg] += 12;
 614                         fpr = load_extended_float80(ea);
 615                         break;
 616                 }
 617       case 5:           // (d16, An)  (added by JFF)
 618                 {
 619                   fpr = load_extended_float80(di_mode_ea);
 620                 break;
 621
 622                 }
 623                 case 7: // extended modes
 624                 {
 625                         switch (reg)
 626                         {
 627                                 case 2: // (d16, PC)
 628                                         {
 629                                                 uint32 ea = EA_PCDI_32();
 630                                                 fpr = load_extended_float80(ea);
 631                                         }
 632                                         break;
 633
 634                                 case 3: // (d16,PC,Dx.w)
 635                                         {
 636                                                 uint32 ea = EA_PCIX_32();
 637                                                 fpr = load_extended_float80(ea);
 638                                         }
 639                                         break;
 640                         case 4: // immediate (JFF)
 641                                 {
 642                                   uint32 ea = REG_PC;
 643                                   fpr = load_extended_float80(ea);
 644                                   REG_PC += 12;
 645                                 }
 646                                 break;
 647                                 default:
 648                                         fatalerror("M68kFPU: READ_EA_FPE: unhandled mode %d, reg %d, at %08X\n", mode, reg, REG_PC);
 649                                         break;
 650                         }
 651                 }
 652                 break;
 653
 654                 default:        fatalerror("M68kFPU: READ_EA_FPE: unhandled mode %d, reg %d, at %08X\n", mode, reg, REG_PC); break;
 655         }
 656
 657         return fpr;
 658 }
 659
 660 static floatx80 READ_EA_PACK(int ea)
 661 {
 662         floatx80 fpr;
 663         int mode = (ea >> 3) & 0x7;
 664         int reg = (ea & 0x7);
 665
 666         switch (mode)
 667         {
 668                 case 2:         // (An)
 669                 {
 670                         uint32 ea = REG_A[reg];
 671                         fpr = load_pack_float80(ea);
 672                         break;
 673                 }
 674
 675                 case 3:         // (An)+
 676                 {
 677                         uint32 ea = REG_A[reg];
 678                         REG_A[reg] += 12;
 679                         fpr = load_pack_float80(ea);
 680                         break;
 681                 }
 682
 683                 case 7: // extended modes
 684                 {
 685                         switch (reg)
 686                         {
 687                                 case 3: // (d16,PC,Dx.w)
 688                                         {
 689                                                 uint32 ea = EA_PCIX_32();
 690                                                 fpr = load_pack_float80(ea);
 691                                         }
 692                                         break;
 693
 694                                 default:
 695                                         fatalerror("M68kFPU: READ_EA_PACK: unhandled mode %d, reg %d, at %08X\n", mode, reg, REG_PC);
 696                                         break;
 697                         }
 698                 }
 699                 break;
 700
 701                 default:        fatalerror("M68kFPU: READ_EA_PACK: unhandled mode %d, reg %d, at %08X\n", mode, reg, REG_PC); break;
 702         }
 703
 704         return fpr;
 705 }
 706
 707 static void WRITE_EA_8(int ea, uint8 data)
 708 {
 709         int mode = (ea >> 3) & 0x7;
 710         int reg = (ea & 0x7);
 711
 712         switch (mode)
 713         {
 714                 case 0:         // Dn
 715                 {
 716                         REG_D[reg] = data;
 717                         break;
 718                 }
 719                 case 2:         // (An)
 720                 {
 721                         uint32 ea = REG_A[reg];
 722                         m68ki_write_8(ea, data);
 723                         break;
 724                 }
 725                 case 3:         // (An)+
 726                 {
 727                         uint32 ea = EA_AY_PI_8();
 728                         m68ki_write_8(ea, data);
 729                         break;
 730                 }
 731                 case 4:         // -(An)
 732                 {
 733                         uint32 ea = EA_AY_PD_8();
 734                         m68ki_write_8(ea, data);
 735                         break;
 736                 }
 737                 case 5:         // (d16, An)
 738                 {
 739                         uint32 ea = EA_AY_DI_8();
 740                         m68ki_write_8(ea, data);
 741                         break;
 742                 }
 743                 case 6:         // (An) + (Xn) + d8
 744                 {
 745                         uint32 ea = EA_AY_IX_8();
 746                         m68ki_write_8(ea, data);
 747                         break;
 748                 }
 749                 case 7:
 750                 {
 751                         switch (reg)
 752                         {
 753                                 case 1:         // (xxx).B
 754                                 {
 755                                         uint32 d1 = OPER_I_16();
 756                                         uint32 d2 = OPER_I_16();
 757                                         uint32 ea = (d1 << 16) | d2;
 758                                         m68ki_write_8(ea, data);
 759                                         break;
 760                                 }
 761                                 case 2:         // (d16, PC)
 762                                 {
 763                                         uint32 ea = EA_PCDI_16();
 764                                         m68ki_write_8(ea, data);
 765                                         break;
 766                                 }
 767                                 default:        fatalerror("M68kFPU: WRITE_EA_8: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 768                         }
 769                         break;
 770                 }
 771                 default:        fatalerror("M68kFPU: WRITE_EA_8: unhandled mode %d, reg %d, data %08X at %08X\n", mode, reg, data, REG_PC);
 772         }
 773 }
 774
 775 static void WRITE_EA_16(int ea, uint16 data)
 776 {
 777         int mode = (ea >> 3) & 0x7;
 778         int reg = (ea & 0x7);
 779
 780         switch (mode)
 781         {
 782                 case 0:         // Dn
 783                 {
 784                         REG_D[reg] = data;
 785                         break;
 786                 }
 787                 case 2:         // (An)
 788                 {
 789                         uint32 ea = REG_A[reg];
 790                         m68ki_write_16(ea, data);
 791                         break;
 792                 }
 793                 case 3:         // (An)+
 794                 {
 795                         uint32 ea = EA_AY_PI_16();
 796                         m68ki_write_16(ea, data);
 797                         break;
 798                 }
 799                 case 4:         // -(An)
 800                 {
 801                         uint32 ea = EA_AY_PD_16();
 802                         m68ki_write_16(ea, data);
 803                         break;
 804                 }
 805                 case 5:         // (d16, An)
 806                 {
 807                         uint32 ea = EA_AY_DI_16();
 808                         m68ki_write_16(ea, data);
 809                         break;
 810                 }
 811                 case 6:         // (An) + (Xn) + d8
 812                 {
 813                         uint32 ea = EA_AY_IX_16();
 814                         m68ki_write_16(ea, data);
 815                         break;
 816                 }
 817                 case 7:
 818                 {
 819                         switch (reg)
 820                         {
 821                                 case 1:         // (xxx).W
 822                                 {
 823                                         uint32 d1 = OPER_I_16();
 824                                         uint32 d2 = OPER_I_16();
 825                                         uint32 ea = (d1 << 16) | d2;
 826                                         m68ki_write_16(ea, data);
 827                                         break;
 828                                 }
 829                                 case 2:         // (d16, PC)
 830                                 {
 831                                         uint32 ea = EA_PCDI_16();
 832                                         m68ki_write_16(ea, data);
 833                                         break;
 834                                 }
 835                                 default:        fatalerror("M68kFPU: WRITE_EA_16: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 836                         }
 837                         break;
 838                 }
 839                 default:        fatalerror("M68kFPU: WRITE_EA_16: unhandled mode %d, reg %d, data %08X at %08X\n", mode, reg, data, REG_PC);
 840         }
 841 }
 842
 843 static void WRITE_EA_32(int ea, uint32 data)
 844 {
 845         int mode = (ea >> 3) & 0x7;
 846         int reg = (ea & 0x7);
 847
 848         switch (mode)
 849         {
 850                 case 0:         // Dn
 851                 {
 852                         REG_D[reg] = data;
 853                         break;
 854                 }
 855                 case 1:         // An
 856                 {
 857                         REG_A[reg] = data;
 858                         break;
 859                 }
 860                 case 2:         // (An)
 861                 {
 862                         uint32 ea = REG_A[reg];
 863                         m68ki_write_32(ea, data);
 864                         break;
 865                 }
 866                 case 3:         // (An)+
 867                 {
 868                         uint32 ea = EA_AY_PI_32();
 869                         m68ki_write_32(ea, data);
 870                         break;
 871                 }
 872                 case 4:         // -(An)
 873                 {
 874                         uint32 ea = EA_AY_PD_32();
 875                         m68ki_write_32(ea, data);
 876                         break;
 877                 }
 878                 case 5:         // (d16, An)
 879                 {
 880                         uint32 ea = EA_AY_DI_32();
 881                         m68ki_write_32(ea, data);
 882                         break;
 883                 }
 884                 case 6:         // (An) + (Xn) + d8
 885                 {
 886                         uint32 ea = EA_AY_IX_32();
 887                         m68ki_write_32(ea, data);
 888                         break;
 889                 }
 890                 case 7:
 891                 {
 892                         switch (reg)
 893                         {
 894                                 case 1:         // (xxx).L
 895                                 {
 896                                         uint32 d1 = OPER_I_16();
 897                                         uint32 d2 = OPER_I_16();
 898                                         uint32 ea = (d1 << 16) | d2;
 899                                         m68ki_write_32(ea, data);
 900                                         break;
 901                                 }
 902                                 case 2:         // (d16, PC)
 903                                 {
 904                                         uint32 ea = EA_PCDI_32();
 905                                         m68ki_write_32(ea, data);
 906                                         break;
 907                                 }
 908                                 default:        fatalerror("M68kFPU: WRITE_EA_32: unhandled mode %d, reg %d at %08X\n", mode, reg, REG_PC);
 909                         }
 910                         break;
 911                 }
 912                 default:        fatalerror("M68kFPU: WRITE_EA_32: unhandled mode %d, reg %d, data %08X at %08X\n", mode, reg, data, REG_PC);
 913         }
 914 }
 915
 916 static void WRITE_EA_64(int ea, uint64 data)
 917 {
 918         int mode = (ea >> 3) & 0x7;
 919         int reg = (ea & 0x7);
 920
 921         switch (mode)
 922         {
 923                 case 2:         // (An)
 924                 {
 925                         uint32 ea = REG_A[reg];
 926                         m68ki_write_32(ea, (uint32)(data >> 32));
 927                         m68ki_write_32(ea+4, (uint32)(data));
 928                         break;
 929                 }
 930                 case 4:         // -(An)
 931                 {
 932                         uint32 ea;
 933                         REG_A[reg] -= 8;
 934                         ea = REG_A[reg];
 935                         m68ki_write_32(ea+0, (uint32)(data >> 32));
 936                         m68ki_write_32(ea+4, (uint32)(data));
 937                         break;
 938                 }
 939                 case 5:         // (d16, An)
 940                 {
 941                         uint32 ea = EA_AY_DI_32();
 942                         m68ki_write_32(ea+0, (uint32)(data >> 32));
 943                         m68ki_write_32(ea+4, (uint32)(data));
 944                         break;
 945                 }
 946                 default:        fatalerror("M68kFPU: WRITE_EA_64: unhandled mode %d, reg %d, data %08X%08X at %08X\n", mode, reg, (uint32)(data >> 32), (uint32)(data), REG_PC);
 947         }
 948 }
 949
 950 static void WRITE_EA_FPE(int mode, int reg, floatx80 fpr, uint32 di_mode_ea)
 951 {
 952
 953
 954         switch (mode)
 955         {
 956                 case 2:         // (An)
 957                 {
 958                         uint32 ea;
 959                         ea = REG_A[reg];
 960                         store_extended_float80(ea, fpr);
 961                         break;
 962                 }
 963
 964                 case 3:         // (An)+
 965                 {
 966                         uint32 ea;
 967                         ea = REG_A[reg];
 968                         store_extended_float80(ea, fpr);
 969                         REG_A[reg] += 12;
 970                         break;
 971                 }
 972
 973                 case 4:         // -(An)
 974                 {
 975                         uint32 ea;
 976                         REG_A[reg] -= 12;
 977                         ea = REG_A[reg];
 978                         store_extended_float80(ea, fpr);
 979                         break;
 980                 }
 981           case 5:               // (d16, An)  (added by JFF)
 982                 {
 983                   // EA_AY_DI_32() should not be done here because fmovem would increase
 984                   // PC each time, reading incorrect displacement & advancing PC too much
 985                   // uint32 ea = EA_AY_DI_32();
 986                   store_extended_float80(di_mode_ea, fpr);
 987                  break;
 988
 989                 }
 990                 case 7:
 991                 {
 992                         switch (reg)
 993                         {
 994                                 default:        fatalerror("M68kFPU: WRITE_EA_FPE: unhandled mode %d, reg %d, at %08X\n", mode, reg, REG_PC);
 995                         }
 996                         break;
 997                 }
 998                 default:        fatalerror("M68kFPU: WRITE_EA_FPE: unhandled mode %d, reg %d, at %08X\n", mode, reg, REG_PC);
 999         }
1000 }
1001
1002 static void WRITE_EA_PACK(int ea, int k, floatx80 fpr)
1003 {
1004         int mode = (ea >> 3) & 0x7;
1005         int reg = (ea & 0x7);
1006
1007         switch (mode)
1008         {
1009                 case 2:         // (An)
1010                 {
1011                         uint32 ea;
1012                         ea = REG_A[reg];
1013                         store_pack_float80(ea, k, fpr);
1014                         break;
1015                 }
1016
1017                 case 3:         // (An)+
1018                 {
1019                         uint32 ea;
1020                         ea = REG_A[reg];
1021                         store_pack_float80(ea, k, fpr);
1022                         REG_A[reg] += 12;
1023                         break;
1024                 }
1025
1026                 case 4:         // -(An)
1027                 {
1028                         uint32 ea;
1029                         REG_A[reg] -= 12;
1030                         ea = REG_A[reg];
1031                         store_pack_float80(ea, k, fpr);
1032                         break;
1033                 }
1034
1035                 case 7:
1036                 {
1037                         switch (reg)
1038                         {
1039                                 default:        fatalerror("M68kFPU: WRITE_EA_PACK: unhandled mode %d, reg %d, at %08X\n", mode, reg, REG_PC);
1040                         }
1041                 }
1042                 break;
1043                 default:        fatalerror("M68kFPU: WRITE_EA_PACK: unhandled mode %d, reg %d, at %08X\n", mode, reg, REG_PC);
1044         }
1045 }
1046
1047
1048 static void fpgen_rm_reg(uint16 w2)
1049 {
1050         int ea = REG_IR & 0x3f;
1051         int rm = (w2 >> 14) & 0x1;
1052         int src = (w2 >> 10) & 0x7;
1053         int dst = (w2 >>  7) & 0x7;
1054         int opmode = w2 & 0x7f;
1055         floatx80 source;
1056
1057         // fmovecr #$f, fp0     f200 5c0f
1058
1059         if (rm)
1060         {
1061                 switch (src)
1062                 {
1063                         case 0:         // Long-Word Integer
1064                         {
1065                                 sint32 d = READ_EA_32(ea);
1066                                 source = int32_to_floatx80(d);
1067                                 break;
1068                         }
1069                         case 1:         // Single-precision Real
1070                         {
1071                                 uint32 d = READ_EA_32(ea);
1072                                 source = float32_to_floatx80(d);
1073                                 break;
1074                         }
1075                         case 2:         // Extended-precision Real
1076                         {
1077                         int imode = (ea >> 3) & 0x7;
1078                         int reg = (ea & 0x7);
1079                         uint32 di_mode_ea = imode == 5 ? (REG_A[reg]+MAKE_INT_16(m68ki_read_imm_16())) : 0;
1080                         source = READ_EA_FPE(imode,reg,di_mode_ea);
1081                                 break;
1082                         }
1083                         case 3:         // Packed-decimal Real
1084                         {
1085                                 source = READ_EA_PACK(ea);
1086                                 break;
1087                         }
1088                         case 4:         // Word Integer
1089                         {
1090                                 sint16 d = READ_EA_16(ea);
1091                                 source = int32_to_floatx80((sint32)d);
1092                                 break;
1093                         }
1094                         case 5:         // Double-precision Real
1095                         {
1096                                 uint64 d = READ_EA_64(ea);
1097
1098                                 source = float64_to_floatx80(d);
1099                                 break;
1100                         }
1101                         case 6:         // Byte Integer
1102                         {
1103                                 sint8 d = READ_EA_8(ea);
1104                                 source = int32_to_floatx80((sint32)d);
1105                                 break;
1106                         }
1107                         case 7:         // FMOVECR load from constant ROM
1108                         {
1109                                 switch (w2 & 0x7f)
1110                                 {
1111                                         case 0x0:       // Pi
1112                                                 source.high = 0x4000;
1113                                                 source.low = U64(0xc90fdaa22168c235);
1114                                                 break;
1115
1116                                         case 0xb:       // log10(2)
1117                                                 source.high = 0x3ffd;
1118                                                 source.low = U64(0x9a209a84fbcff798);
1119                                                 break;
1120
1121                                         case 0xc:       // e
1122                                                 source.high = 0x4000;
1123                                                 source.low = U64(0xadf85458a2bb4a9b);
1124                                                 break;
1125
1126                                         case 0xd:       // log2(e)
1127                                                 source.high = 0x3fff;
1128                                                 source.low = U64(0xb8aa3b295c17f0bc);
1129                                                 break;
1130
1131                                         case 0xe:       // log10(e)
1132                                                 source.high = 0x3ffd;
1133                                                 source.low = U64(0xde5bd8a937287195);
1134                                                 break;
1135
1136                                         case 0xf:       // 0.0
1137                                                 source = int32_to_floatx80((sint32)0);
1138                                                 break;
1139
1140                                         case 0x30:      // ln(2)
1141                                                 source.high = 0x3ffe;
1142                                                 source.low = U64(0xb17217f7d1cf79ac);
1143                                                 break;
1144
1145                                         case 0x31:      // ln(10)
1146                                                 source.high = 0x4000;
1147                                                 source.low = U64(0x935d8dddaaa8ac17);
1148                                                 break;
1149
1150                                         case 0x32:      // 1 (or 100?  manuals are unclear, but 1 would make more sense)
1151                                                 source = int32_to_floatx80((sint32)1);
1152                                                 break;
1153
1154                                         case 0x33:      // 10^1
1155                                                 source = int32_to_floatx80((sint32)10);
1156                                                 break;
1157
1158                                         case 0x34:      // 10^2
1159                                                 source = int32_to_floatx80((sint32)10*10);
1160                                                 break;
1161
1162                                         default:
1163                                                 fatalerror("fmove_rm_reg: unknown constant ROM offset %x at %08x\n", w2&0x7f, REG_PC-4);
1164                                                 break;
1165                                 }
1166
1167                                 // handle it right here, the usual opmode bits aren't valid in the FMOVECR case
1168                                 REG_FP[dst] = source;
1169                         SET_CONDITION_CODES(REG_FP[dst]); // JFF when destination is a register, we HAVE to update FPCR
1170                                 USE_CYCLES(4);
1171                                 return;
1172                         }
1173                         default:        fatalerror("fmove_rm_reg: invalid source specifier %x at %08X\n", src, REG_PC-4);
1174                 }
1175         }
1176         else
1177         {
1178                 source = REG_FP[src];
1179         }
1180
1181
1182
1183         switch (opmode)
1184         {
1185                 case 0x00:              // FMOVE
1186                 {
1187                         REG_FP[dst] = source;
1188                     SET_CONDITION_CODES(REG_FP[dst]);  // JFF needs update condition codes
1189                         USE_CYCLES(4);
1190                         break;
1191                 }
1192                 case 0x01:              // Fsint
1193                 {
1194                         sint32 temp;
1195                         temp = floatx80_to_int32(source);
1196                         REG_FP[dst] = int32_to_floatx80(temp);
1197                         SET_CONDITION_CODES(REG_FP[dst]);  // JFF needs update condition codes
1198                         break;
1199                 }
1200                 case 0x03:              // FsintRZ
1201                 {
1202                         sint32 temp;
1203                         temp = floatx80_to_int32_round_to_zero(source);
1204                         REG_FP[dst] = int32_to_floatx80(temp);
1205                         SET_CONDITION_CODES(REG_FP[dst]);  // JFF needs update condition codes
1206                         break;
1207                 }
1208                 case 0x04:              // FSQRT
1209                 {
1210                         REG_FP[dst] = floatx80_sqrt(source);
1211                         SET_CONDITION_CODES(REG_FP[dst]);
1212                         USE_CYCLES(109);
1213                         break;
1214                 }
1215                 case 0x18:              // FABS
1216                 {
1217                         REG_FP[dst] = source;
1218                         REG_FP[dst].high &= 0x7fff;
1219                         SET_CONDITION_CODES(REG_FP[dst]);
1220                         USE_CYCLES(3);
1221                         break;
1222                 }
1223                 case 0x1a:              // FNEG
1224                 {
1225                         REG_FP[dst] = source;
1226                         REG_FP[dst].high ^= 0x8000;
1227                         SET_CONDITION_CODES(REG_FP[dst]);
1228                         USE_CYCLES(3);
1229                         break;
1230                 }
1231                 case 0x1e:              // FGETEXP
1232                 {
1233                         sint16 temp;
1234                         temp = source.high;     // get the exponent
1235                         temp -= 0x3fff; // take off the bias
1236                         REG_FP[dst] = double_to_fx80((double)temp);
1237                         SET_CONDITION_CODES(REG_FP[dst]);
1238                         USE_CYCLES(6);
1239                         break;
1240                 }
1241             case 0x60:          // FSDIVS (JFF) (source has already been converted to floatx80)
1242                 case 0x20:              // FDIV
1243                 {
1244                         REG_FP[dst] = floatx80_div(REG_FP[dst], source);
1245                     SET_CONDITION_CODES(REG_FP[dst]); // JFF
1246                         USE_CYCLES(43);
1247                         break;
1248                 }
1249                 case 0x22:              // FADD
1250                 {
1251                         REG_FP[dst] = floatx80_add(REG_FP[dst], source);
1252                         SET_CONDITION_CODES(REG_FP[dst]);
1253                         USE_CYCLES(9);
1254                         break;
1255                 }
1256                 case 0x63:              // FSMULS (JFF) (source has already been converted to floatx80)
1257                 case 0x23:              // FMUL
1258                 {
1259                         REG_FP[dst] = floatx80_mul(REG_FP[dst], source);
1260                         SET_CONDITION_CODES(REG_FP[dst]);
1261                         USE_CYCLES(11);
1262                         break;
1263                 }
1264                 case 0x25:              // FREM
1265                 {
1266                         REG_FP[dst] = floatx80_rem(REG_FP[dst], source);
1267                         SET_CONDITION_CODES(REG_FP[dst]);
1268                         USE_CYCLES(43); // guess
1269                         break;
1270                 }
1271                 case 0x28:              // FSUB
1272                 {
1273                         REG_FP[dst] = floatx80_sub(REG_FP[dst], source);
1274                         SET_CONDITION_CODES(REG_FP[dst]);
1275                         USE_CYCLES(9);
1276                         break;
1277                 }
1278                 case 0x38:              // FCMP
1279                 {
1280                         floatx80 res;
1281                         res = floatx80_sub(REG_FP[dst], source);
1282                         SET_CONDITION_CODES(res);
1283                         USE_CYCLES(7);
1284                         break;
1285                 }
1286                 case 0x3a:              // FTST
1287                 {
1288                         floatx80 res;
1289                         res = source;
1290                         SET_CONDITION_CODES(res);
1291                         USE_CYCLES(7);
1292                         break;
1293                 }
1294
1295                 default:        fatalerror("fpgen_rm_reg: unimplemented opmode %02X at %08X\n", opmode, REG_PC-4);
1296         }
1297 }
1298
1299 static void fmove_reg_mem(uint16 w2)
1300 {
1301         int ea = REG_IR & 0x3f;
1302         int src = (w2 >>  7) & 0x7;
1303         int dst = (w2 >> 10) & 0x7;
1304         int k = (w2 & 0x7f);
1305
1306         switch (dst)
1307         {
1308                 case 0:         // Long-Word Integer
1309                 {
1310                         sint32 d = (sint32)floatx80_to_int32(REG_FP[src]);
1311                         WRITE_EA_32(ea, d);
1312                         break;
1313                 }
1314                 case 1:         // Single-precision Real
1315                 {
1316                         uint32 d = floatx80_to_float32(REG_FP[src]);
1317                         WRITE_EA_32(ea, d);
1318                         break;
1319                 }
1320                 case 2:         // Extended-precision Real
1321                 {
1322                         int mode = (ea >> 3) & 0x7;
1323                         int reg = (ea & 0x7);
1324                         uint32 di_mode_ea = mode == 5 ? (REG_A[reg]+MAKE_INT_16(m68ki_read_imm_16())) : 0;
1325                         WRITE_EA_FPE(mode, reg, REG_FP[src], di_mode_ea);
1326                         break;
1327                 }
1328                 case 3:         // Packed-decimal Real with Static K-factor
1329                 {
1330                         // sign-extend k
1331                         k = (k & 0x40) ? (k | 0xffffff80) : (k & 0x7f);
1332                         WRITE_EA_PACK(ea, k, REG_FP[src]);
1333                         break;
1334                 }
1335                 case 4:         // Word Integer
1336                 {
1337                         WRITE_EA_16(ea, (sint16)floatx80_to_int32(REG_FP[src]));
1338                         break;
1339                 }
1340                 case 5:         // Double-precision Real
1341                 {
1342                         uint64 d;
1343
1344                         d = floatx80_to_float64(REG_FP[src]);
1345
1346                         WRITE_EA_64(ea, d);
1347                         break;
1348                 }
1349                 case 6:         // Byte Integer
1350                 {
1351                         WRITE_EA_8(ea, (sint8)floatx80_to_int32(REG_FP[src]));
1352                         break;
1353                 }
1354                 case 7:         // Packed-decimal Real with Dynamic K-factor
1355                 {
1356                         WRITE_EA_PACK(ea, REG_D[k>>4], REG_FP[src]);
1357                         break;
1358                 }
1359         }
1360
1361         USE_CYCLES(12);
1362 }
1363
1364 static void fmove_fpcr(uint16 w2)
1365 {
1366         int ea = REG_IR & 0x3f;
1367         int dir = (w2 >> 13) & 0x1;
1368         int reg = (w2 >> 10) & 0x7;
1369
1370         if (dir)        // From system control reg to <ea>
1371         {
1372                 if (reg & 4) WRITE_EA_32(ea, REG_FPCR);
1373                 if (reg & 2) WRITE_EA_32(ea, REG_FPSR);
1374                 if (reg & 1) WRITE_EA_32(ea, REG_FPIAR);
1375         }
1376         else            // From <ea> to system control reg
1377         {
1378       if (reg & 4)
1379                 {
1380                   REG_FPCR = READ_EA_32(ea);
1381                   // JFF: need to update rounding mode from softfloat module
1382                   float_rounding_mode = (REG_FPCR >> 4) & 0x3;
1383                 }
1384                 if (reg & 2) REG_FPSR = READ_EA_32(ea);
1385                 if (reg & 1) REG_FPIAR = READ_EA_32(ea);
1386         }
1387
1388         USE_CYCLES(10);
1389 }
1390
1391 static void fmovem(uint16 w2)
1392 {
1393         int i;
1394         int ea = REG_IR & 0x3f;
1395         int dir = (w2 >> 13) & 0x1;
1396         int mode = (w2 >> 11) & 0x3;
1397         int reglist = w2 & 0xff;
1398
1399         if (dir)        // From FP regs to mem
1400         {
1401                 switch (mode)
1402         {
1403                 case 2:         // (JFF): Static register list, postincrement or control addressing mode.
1404             {
1405               int imode = (ea >> 3) & 0x7;
1406               int reg = (ea & 0x7);
1407               int di_mode = imode == 5;
1408               uint32 di_mode_ea = di_mode ? (REG_A[reg]+MAKE_INT_16(m68ki_read_imm_16())) : 0;
1409               for (i=0; i < 8; i++)
1410                         {
1411                           if (reglist & (1 << i))
1412                             {
1413                               WRITE_EA_FPE(imode,reg, REG_FP[7-i],di_mode_ea);
1414                               USE_CYCLES(2);
1415                               if (di_mode)
1416                                 {
1417                                   di_mode_ea += 12;
1418                                 }
1419                         }
1420                         }
1421               break;
1422                  }
1423                         case 0:         // Static register list, predecrement addressing mode
1424                         {
1425               int imode = (ea >> 3) & 0x7;
1426               int reg = (ea & 0x7);
1427               // the "di_mode_ea" parameter kludge is required here else WRITE_EA_FPE would have
1428               // to call EA_AY_DI_32() (that advances PC & reads displacement) each time
1429               // when the proper behaviour is 1) read once, 2) increment ea for each matching register
1430               // this forces to pre-read the mode (named "imode") so we can decide to read displacement, only once
1431               int di_mode = imode == 5;
1432               uint32 di_mode_ea =  di_mode ? (REG_A[reg]+MAKE_INT_16(m68ki_read_imm_16())) : 0;
1433                                 for (i=0; i < 8; i++)
1434                                 {
1435                                         if (reglist & (1 << i))
1436                                         {
1437                                             WRITE_EA_FPE(imode,reg, REG_FP[i],di_mode_ea);
1438                                                 USE_CYCLES(2);
1439                                             if (di_mode)
1440                                                 {
1441                                                   di_mode_ea += 12;
1442                                                 }
1443                                         }
1444                                 }
1445                                 break;
1446                         }
1447
1448                         default:        fatalerror("040fpu0: FMOVEM: mode %d unimplemented at %08X\n", mode, REG_PC-4);
1449                 }
1450         }
1451         else            // From mem to FP regs
1452         {
1453                 switch (mode)
1454                 {
1455                         case 2:         // Static register list, postincrement addressing mode
1456                         {
1457                       int imode = (ea >> 3) & 0x7;
1458                       int reg = (ea & 0x7);
1459                       int di_mode = imode == 5;
1460                       uint32 di_mode_ea = di_mode ? (REG_A[reg]+MAKE_INT_16(m68ki_read_imm_16())) : 0;
1461                                 for (i=0; i < 8; i++)
1462                                 {
1463                                         if (reglist & (1 << i))
1464                                         {
1465                                                 REG_FP[7-i] = READ_EA_FPE(imode,reg,di_mode_ea);
1466                                                 USE_CYCLES(2);
1467                                             if (di_mode)
1468                                                 {
1469                                                   di_mode_ea += 12;
1470                                                 }
1471                                         }
1472                                 }
1473                                 break;
1474                         }
1475
1476                         default:        fatalerror("040fpu0: FMOVEM: mode %d unimplemented at %08X\n", mode, REG_PC-4);
1477                 }
1478         }
1479 }
1480
1481 static void fscc()
1482 {
1483   // added by JFF, this seems to work properly now
1484   int condition = OPER_I_16() & 0x3f;
1485
1486   int cc = TEST_CONDITION(condition);
1487   int mode = (REG_IR & 0x38) >> 3;
1488   int v = (cc ? 0xff : 0x00);
1489
1490   switch (mode)
1491   {
1492   case 0:  // fscc Dx
1493     {
1494       // If the specified floating-point condition is true, sets the byte integer operand at
1495       // the destination to TRUE (all ones); otherwise, sets the byte to FALSE (all zeros).
1496
1497       REG_D[REG_IR & 7] = (REG_D[REG_IR & 7] & 0xFFFFFF00) | v;
1498       break;
1499     }
1500     case 5: // (disp,Ax)
1501     {
1502     int reg = REG_IR & 7;
1503     uint32 ea = REG_A[reg]+MAKE_INT_16(m68ki_read_imm_16());
1504     m68ki_write_8(ea,v);
1505     break;
1506     }
1507
1508   default:
1509     {
1510       // unimplemented see fpu_uae.cpp around line 1300
1511       fatalerror("040fpu0: fscc: mode %d not implemented at %08X\n", mode, REG_PC-4);
1512     }
1513     }
1514   USE_CYCLES(7);  // JFF unsure of the number of cycles!!
1515 }
1516 static void fbcc16(void)
1517 {
1518         sint32 offset;
1519         int condition = REG_IR & 0x3f;
1520
1521         offset = (sint16)(OPER_I_16());
1522
1523         // TODO: condition and jump!!!
1524         if (TEST_CONDITION(condition))
1525         {
1526                 m68ki_trace_t0();                          /* auto-disable (see m68kcpu.h) */
1527                 m68ki_branch_16(offset-2);
1528         }
1529
1530         USE_CYCLES(7);
1531         }
1532
1533 static void fbcc32(void)
1534 {
1535         sint32 offset;
1536         int condition = REG_IR & 0x3f;
1537
1538         offset = OPER_I_32();
1539
1540         // TODO: condition and jump!!!
1541         if (TEST_CONDITION(condition))
1542         {
1543                 m68ki_trace_t0();                          /* auto-disable (see m68kcpu.h) */
1544                 m68ki_branch_32(offset-4);
1545         }
1546
1547         USE_CYCLES(7);
1548 }
1549
1550
1551 void m68040_fpu_op0()
1552 {
1553         m68ki_cpu.fpu_just_reset = 0;
1554
1555         switch ((REG_IR >> 6) & 0x3)
1556         {
1557                 case 0:
1558                 {
1559                         uint16 w2 = OPER_I_16();
1560                         switch ((w2 >> 13) & 0x7)
1561                         {
1562                                 case 0x0:       // FPU ALU FP, FP
1563                                 case 0x2:       // FPU ALU ea, FP
1564                                 {
1565                                         fpgen_rm_reg(w2);
1566                                         break;
1567                                 }
1568
1569                                 case 0x3:       // FMOVE FP, ea
1570                                 {
1571                                         fmove_reg_mem(w2);
1572                                         break;
1573                                 }
1574
1575                                 case 0x4:       // FMOVEM ea, FPCR
1576                                 case 0x5:       // FMOVEM FPCR, ea
1577                                 {
1578                                         fmove_fpcr(w2);
1579                                         break;
1580                                 }
1581
1582                                 case 0x6:       // FMOVEM ea, list
1583                                 case 0x7:       // FMOVEM list, ea
1584                                 {
1585                                         fmovem(w2);
1586                                         break;
1587                                 }
1588
1589                                 default:        fatalerror("M68kFPU: unimplemented subop %d at %08X\n", (w2 >> 13) & 0x7, REG_PC-4);
1590                         }
1591                         break;
1592                 }
1593
1594             case 1:           // FScc (JFF)
1595                 {
1596                   fscc();
1597                   break;
1598                 }
1599                 case 2:         // FBcc disp16
1600                 {
1601                         fbcc16();
1602                         break;
1603                 }
1604                 case 3:         // FBcc disp32
1605                 {
1606                         fbcc32();
1607                         break;
1608                 }
1609
1610       default:  fatalerror("M68kFPU: unimplemented main op %d at %08X\n", (m68ki_cpu.ir >> 6) & 0x3,  REG_PC-4);
1611         }
1612 }
1613
1614 static void perform_fsave(uint32 addr, int inc)
1615 {
1616         if (inc)
1617         {
1618                 // 68881 IDLE, version 0x1f
1619                 m68ki_write_32(addr, 0x1f180000);
1620                 m68ki_write_32(addr+4, 0);
1621                 m68ki_write_32(addr+8, 0);
1622                 m68ki_write_32(addr+12, 0);
1623                 m68ki_write_32(addr+16, 0);
1624                 m68ki_write_32(addr+20, 0);
1625                 m68ki_write_32(addr+24, 0x70000000);
1626         }
1627         else
1628         {
1629                 m68ki_write_32(addr, 0x70000000);
1630                 m68ki_write_32(addr-4, 0);
1631                 m68ki_write_32(addr-8, 0);
1632                 m68ki_write_32(addr-12, 0);
1633                 m68ki_write_32(addr-16, 0);
1634                 m68ki_write_32(addr-20, 0);
1635                 m68ki_write_32(addr-24, 0x1f180000);
1636         }
1637 }
1638
1639 // FRESTORE on a NULL frame reboots the FPU - all registers to NaN, the 3 status regs to 0
1640 static void do_frestore_null()
1641 {
1642         int i;
1643
1644         REG_FPCR = 0;
1645         REG_FPSR = 0;
1646         REG_FPIAR = 0;
1647         for (i = 0; i < 8; i++)
1648         {
1649                 REG_FP[i].high = 0x7fff;
1650                 REG_FP[i].low = U64(0xffffffffffffffff);
1651         }
1652
1653         // Mac IIci at 408458e6 wants an FSAVE of a just-restored NULL frame to also be NULL
1654         // The PRM says it's possible to generate a NULL frame, but not how/when/why.  (need the 68881/68882 manual!)
1655         m68ki_cpu.fpu_just_reset = 1;
1656 }
1657
1658 void m68040_fpu_op1()
1659 {
1660         int ea = REG_IR & 0x3f;
1661         int mode = (ea >> 3) & 0x7;
1662         int reg = (ea & 0x7);
1663         uint32 addr, temp;
1664
1665         switch ((REG_IR >> 6) & 0x3)
1666         {
1667                 case 0:         // FSAVE <ea>
1668                 {
1669                         switch (mode)
1670                         {
1671                                 case 3: // (An)+
1672                                         addr = EA_AY_PI_32();
1673
1674                                         if (m68ki_cpu.fpu_just_reset)
1675                                         {
1676                                                 m68ki_write_32(addr, 0);
1677                                         }
1678                                         else
1679                                         {
1680                                                 // we normally generate an IDLE frame
1681                                                 REG_A[reg] += 6*4;
1682                                                 perform_fsave(addr, 1);
1683                                         }
1684                                         break;
1685
1686                                 case 4: // -(An)
1687                                         addr = EA_AY_PD_32();
1688
1689                                         if (m68ki_cpu.fpu_just_reset)
1690                                         {
1691                                                 m68ki_write_32(addr, 0);
1692                                         }
1693                                         else
1694                                         {
1695                                                 // we normally generate an IDLE frame
1696                                                 REG_A[reg] -= 6*4;
1697                                                 perform_fsave(addr, 0);
1698                                         }
1699                                         break;
1700
1701                                 default:
1702                                         fatalerror("M68kFPU: FSAVE unhandled mode %d reg %d at %x\n", mode, reg, REG_PC);
1703                         }
1704                         break;
1705                 }
1706                 break;
1707
1708                 case 1:         // FRESTORE <ea>
1709                 {
1710                         switch (mode)
1711                         {
1712                                 case 2: // (An)
1713                                         addr = REG_A[reg];
1714                                         temp = m68ki_read_32(addr);
1715
1716                                         // check for NULL frame
1717                                         if (temp & 0xff000000)
1718                                         {
1719                                                 // we don't handle non-NULL frames and there's no pre/post inc/dec to do here
1720                                                 m68ki_cpu.fpu_just_reset = 0;
1721                                         }
1722                                         else
1723                                         {
1724                                                 do_frestore_null();
1725                                         }
1726                                         break;
1727
1728                                 case 3: // (An)+
1729                                         addr = EA_AY_PI_32();
1730                                         temp = m68ki_read_32(addr);
1731
1732                                         // check for NULL frame
1733                                         if (temp & 0xff000000)
1734                                         {
1735                                                 m68ki_cpu.fpu_just_reset = 0;
1736
1737                                                 // how about an IDLE frame?
1738                                                 if ((temp & 0x00ff0000) == 0x00180000)
1739                                                 {
1740                                                         REG_A[reg] += 6*4;
1741                                                 } // check UNIMP
1742                                                 else if ((temp & 0x00ff0000) == 0x00380000)
1743                                                 {
1744                                                         REG_A[reg] += 14*4;
1745                                                 } // check BUSY
1746                                                 else if ((temp & 0x00ff0000) == 0x00b40000)
1747                                                 {
1748                                                         REG_A[reg] += 45*4;
1749                                                 }
1750                                         }
1751                                         else
1752                                         {
1753                                                 do_frestore_null();
1754                                         }
1755                                         break;
1756
1757                                 default:
1758                                         fatalerror("M68kFPU: FRESTORE unhandled mode %d reg %d at %x\n", mode, reg, REG_PC);
1759                         }
1760                         break;
1761                 }
1762                 break;
1763
1764                 default:        fatalerror("m68040_fpu_op1: unimplemented op %d at %08X\n", (REG_IR >> 6) & 0x3, REG_PC-2);
1765         }
1766 }
1767
1768
1769