reactos/lib/fast486/common.inl

   1 /*
   2  * Fast486 386/486 CPU Emulation Library
   3  * common.inl
   4  *
   5  * Copyright (C) 2014 Aleksandar Andrejevic <theflash AT sdf DOT lonestar DOT org>
   6  *
   7  * This program is free software; you can redistribute it and/or
   8  * modify it under the terms of the GNU General Public License
   9  * as published by the Free Software Foundation; either version 2
  10  * of the License, or (at your option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, write to the Free Software
  19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20  */
  21
  22 #include "common.h"
  23 #include "fpu.h"
  24
  25 /* PUBLIC FUNCTIONS ***********************************************************/
  26
  27 #if defined (__GNUC__)
  28     #define CountLeadingZeros64(x) __builtin_clzll(x)
  29 #elif (_MSC_VER >= 1500) && defined(_WIN64)
  30     #define CountLeadingZeros64(x) __lzcnt64(x)
  31 #elif (_MSC_VER >= 1500)
  32     #define CountLeadingZeros64(x) ((x) > 0xFFFFFFFFULL) ? __lzcnt((x) >> 32) \
  33                                                          : (__lzcnt(x) + 32)
  34 #else
  35     static FORCEINLINE
  36     ULONG CountLeadingZeros64(ULONGLONG Value)
  37     {
  38         ULONG Count = 0;
  39         ULONGLONG Mask = 1ULL << 63;
  40
  41         while (!(Value & Mask))
  42         {
  43             Count++;
  44             Mask >>= 1;
  45         }
  46
  47         return Count;
  48     }
  49 #endif
  50
  51 FORCEINLINE
  52 INT
  53 Fast486GetCurrentPrivLevel(PFAST486_STATE State)
  54 {
  55     /* Return the CPL, or 3 if we're in virtual 8086 mode */
  56     return (!State->Flags.Vm) ? State->Cpl : 3;
  57 }
  58
  59 FORCEINLINE
  60 ULONG
  61 Fast486GetPageTableEntry(PFAST486_STATE State,
  62                          ULONG VirtualAddress,
  63                          BOOLEAN MarkAsDirty)
  64 {
  65     ULONG PdeIndex = GET_ADDR_PDE(VirtualAddress);
  66     ULONG PteIndex = GET_ADDR_PTE(VirtualAddress);
  67     FAST486_PAGE_DIR DirectoryEntry;
  68     FAST486_PAGE_TABLE TableEntry;
  69     ULONG PageDirectory = State->ControlRegisters[FAST486_REG_CR3];
  70
  71     if ((State->Tlb != NULL)
  72         && (State->Tlb[VirtualAddress >> 12] != INVALID_TLB_FIELD))
  73     {
  74         /* Return the cached entry */
  75         return State->Tlb[VirtualAddress >> 12];
  76     }
  77
  78     /* Read the directory entry */
  79     State->MemReadCallback(State,
  80                            PageDirectory + PdeIndex * sizeof(ULONG),
  81                            &DirectoryEntry.Value,
  82                            sizeof(DirectoryEntry));
  83
  84     /* Make sure it is present */
  85     if (!DirectoryEntry.Present) return 0;
  86
  87     /* Was the directory entry accessed before? */
  88     if (!DirectoryEntry.Accessed)
  89     {
  90         /* Well, it is now */
  91         DirectoryEntry.Accessed = TRUE;
  92
  93         /* Write back the directory entry */
  94         State->MemWriteCallback(State,
  95                                 PageDirectory + PdeIndex * sizeof(ULONG),
  96                                 &DirectoryEntry.Value,
  97                                 sizeof(DirectoryEntry));
  98     }
  99
 100     /* Read the table entry */
 101     State->MemReadCallback(State,
 102                            (DirectoryEntry.TableAddress << 12)
 103                            + PteIndex * sizeof(ULONG),
 104                            &TableEntry.Value,
 105                            sizeof(TableEntry));
 106
 107     /* Make sure it is present */
 108     if (!TableEntry.Present) return 0;
 109
 110     if (MarkAsDirty) TableEntry.Dirty = TRUE;
 111
 112     /* Was the table entry accessed before? */
 113     if (!TableEntry.Accessed)
 114     {
 115         /* Well, it is now */
 116         TableEntry.Accessed = TRUE;
 117
 118         /* Write back the table entry */
 119         State->MemWriteCallback(State,
 120                                 (DirectoryEntry.TableAddress << 12)
 121                                 + PteIndex * sizeof(ULONG),
 122                                 &TableEntry.Value,
 123                                 sizeof(TableEntry));
 124     }
 125
 126     /*
 127      * The resulting permissions depend on the permissions
 128      * in the page directory table too
 129      */
 130     TableEntry.Writeable &= DirectoryEntry.Writeable;
 131     TableEntry.Usermode &= DirectoryEntry.Usermode;
 132
 133     if (State->Tlb != NULL)
 134     {
 135         /* Set the TLB entry */
 136         State->Tlb[VirtualAddress >> 12] = TableEntry.Value;
 137     }
 138
 139     /* Return the table entry */
 140     return TableEntry.Value;
 141 }
 142
 143 FORCEINLINE
 144 BOOLEAN
 145 Fast486ReadLinearMemory(PFAST486_STATE State,
 146                         ULONG LinearAddress,
 147                         PVOID Buffer,
 148                         ULONG Size)
 149 {
 150     /* Check if paging is enabled */
 151     if (State->ControlRegisters[FAST486_REG_CR0] & FAST486_CR0_PG)
 152     {
 153         ULONG Page;
 154         FAST486_PAGE_TABLE TableEntry;
 155         INT Cpl = Fast486GetCurrentPrivLevel(State);
 156         ULONG BufferOffset = 0;
 157
 158         for (Page = PAGE_ALIGN(LinearAddress);
 159              Page <= PAGE_ALIGN(LinearAddress + Size - 1);
 160              Page += PAGE_SIZE)
 161         {
 162             ULONG PageOffset = 0, PageLength = PAGE_SIZE;
 163
 164             /* Get the table entry */
 165             TableEntry.Value = Fast486GetPageTableEntry(State, Page, FALSE);
 166
 167             if (!TableEntry.Present || (!TableEntry.Usermode && (Cpl > 0)))
 168             {
 169                 /* Exception */
 170                 Fast486ExceptionWithErrorCode(State,
 171                                               FAST486_EXCEPTION_PF,
 172                                               TableEntry.Value & 0x07);
 173                 return FALSE;
 174             }
 175
 176             /* Check if this is the first page */
 177             if (Page == PAGE_ALIGN(LinearAddress))
 178             {
 179                 /* Start reading from the offset from the beginning of the page */
 180                 PageOffset = PAGE_OFFSET(LinearAddress);
 181                 PageLength -= PageOffset;
 182             }
 183
 184             /* Check if this is the last page */
 185             if (Page == PAGE_ALIGN(LinearAddress + Size - 1))
 186             {
 187                 /* Read only a part of the page */
 188                 PageLength = PAGE_OFFSET(LinearAddress + Size - 1) - PageOffset + 1;
 189             }
 190
 191             /* Read the memory */
 192             State->MemReadCallback(State,
 193                                    (TableEntry.Address << 12) | PageOffset,
 194                                    (PVOID)((ULONG_PTR)Buffer + BufferOffset),
 195                                    PageLength);
 196
 197             BufferOffset += PageLength;
 198         }
 199     }
 200     else
 201     {
 202         /* Read the memory */
 203         State->MemReadCallback(State, LinearAddress, Buffer, Size);
 204     }
 205
 206     return TRUE;
 207 }
 208
 209 FORCEINLINE
 210 BOOLEAN
 211 Fast486WriteLinearMemory(PFAST486_STATE State,
 212                          ULONG LinearAddress,
 213                          PVOID Buffer,
 214                          ULONG Size)
 215 {
 216     /* Check if paging is enabled */
 217     if (State->ControlRegisters[FAST486_REG_CR0] & FAST486_CR0_PG)
 218     {
 219         ULONG Page;
 220         FAST486_PAGE_TABLE TableEntry;
 221         INT Cpl = Fast486GetCurrentPrivLevel(State);
 222         ULONG BufferOffset = 0;
 223
 224         for (Page = PAGE_ALIGN(LinearAddress);
 225              Page <= PAGE_ALIGN(LinearAddress + Size - 1);
 226              Page += PAGE_SIZE)
 227         {
 228             ULONG PageOffset = 0, PageLength = PAGE_SIZE;
 229
 230             /* Get the table entry */
 231             TableEntry.Value = Fast486GetPageTableEntry(State, Page, TRUE);
 232
 233             if ((!TableEntry.Present || (!TableEntry.Usermode && (Cpl > 0)))
 234                 || ((State->ControlRegisters[FAST486_REG_CR0] & FAST486_CR0_WP)
 235                 && !TableEntry.Writeable))
 236             {
 237                 /* Exception */
 238                 Fast486ExceptionWithErrorCode(State,
 239                                               FAST486_EXCEPTION_PF,
 240                                               TableEntry.Value & 0x07);
 241                 return FALSE;
 242             }
 243
 244             /* Check if this is the first page */
 245             if (Page == PAGE_ALIGN(LinearAddress))
 246             {
 247                 /* Start writing from the offset from the beginning of the page */
 248                 PageOffset = PAGE_OFFSET(LinearAddress);
 249                 PageLength -= PageOffset;
 250             }
 251
 252             /* Check if this is the last page */
 253             if (Page == PAGE_ALIGN(LinearAddress + Size - 1))
 254             {
 255                 /* Write only a part of the page */
 256                 PageLength = PAGE_OFFSET(LinearAddress + Size - 1) - PageOffset + 1;
 257             }
 258
 259             /* Write the memory */
 260             State->MemWriteCallback(State,
 261                                     (TableEntry.Address << 12) | PageOffset,
 262                                     (PVOID)((ULONG_PTR)Buffer + BufferOffset),
 263                                     PageLength);
 264
 265             BufferOffset += PageLength;
 266         }
 267     }
 268     else
 269     {
 270         /* Write the memory */
 271         State->MemWriteCallback(State, LinearAddress, Buffer, Size);
 272     }
 273
 274     return TRUE;
 275 }
 276
 277 FORCEINLINE
 278 VOID
 279 Fast486Exception(PFAST486_STATE State,
 280                  FAST486_EXCEPTIONS ExceptionCode)
 281 {
 282     /* Call the internal function */
 283     Fast486ExceptionWithErrorCode(State, ExceptionCode, 0);
 284 }
 285
 286 FORCEINLINE
 287 BOOLEAN
 288 Fast486StackPush(PFAST486_STATE State,
 289                  ULONG Value)
 290 {
 291     BOOLEAN Size = State->SegmentRegs[FAST486_REG_CS].Size;
 292
 293     /* The OPSIZE prefix toggles the size */
 294     if (State->PrefixFlags & FAST486_PREFIX_OPSIZE) Size = !Size;
 295
 296     if (Size)
 297     {
 298         /* 32-bit size */
 299
 300         /* Check if ESP is between 1 and 3 */
 301         if (State->GeneralRegs[FAST486_REG_ESP].Long >= 1
 302             && State->GeneralRegs[FAST486_REG_ESP].Long <= 3)
 303         {
 304             Fast486Exception(State, FAST486_EXCEPTION_SS);
 305             return FALSE;
 306         }
 307
 308         /* Subtract ESP by 4 */
 309         State->GeneralRegs[FAST486_REG_ESP].Long -= sizeof(ULONG);
 310
 311         /* Store the value in SS:ESP */
 312         return Fast486WriteMemory(State,
 313                                   FAST486_REG_SS,
 314                                   State->GeneralRegs[FAST486_REG_ESP].Long,
 315                                   &Value,
 316                                   sizeof(ULONG));
 317     }
 318     else
 319     {
 320         /* 16-bit size */
 321         USHORT ShortValue = LOWORD(Value);
 322
 323         /* Check if SP is 1 */
 324         if (State->GeneralRegs[FAST486_REG_ESP].LowWord == 1)
 325         {
 326             Fast486Exception(State, FAST486_EXCEPTION_SS);
 327             return FALSE;
 328         }
 329
 330         /* Subtract SP by 2 */
 331         State->GeneralRegs[FAST486_REG_ESP].LowWord -= sizeof(USHORT);
 332
 333         /* Store the value in SS:SP */
 334         return Fast486WriteMemory(State,
 335                                   FAST486_REG_SS,
 336                                   State->GeneralRegs[FAST486_REG_ESP].LowWord,
 337                                   &ShortValue,
 338                                   sizeof(USHORT));
 339     }
 340 }
 341
 342 FORCEINLINE
 343 BOOLEAN
 344 Fast486StackPop(PFAST486_STATE State,
 345                 PULONG Value)
 346 {
 347     BOOLEAN Size = State->SegmentRegs[FAST486_REG_CS].Size;
 348
 349     /* The OPSIZE prefix toggles the size */
 350     TOGGLE_OPSIZE(Size);
 351
 352     if (Size)
 353     {
 354         /* 32-bit size */
 355         ULONG LongValue;
 356
 357         /* Check if ESP is 0xFFFFFFFF */
 358         if (State->GeneralRegs[FAST486_REG_ESP].Long == 0xFFFFFFFF)
 359         {
 360             Fast486Exception(State, FAST486_EXCEPTION_SS);
 361             return FALSE;
 362         }
 363
 364         /* Read the value from SS:ESP */
 365         if (!Fast486ReadMemory(State,
 366                                FAST486_REG_SS,
 367                                State->GeneralRegs[FAST486_REG_ESP].Long,
 368                                FALSE,
 369                                &LongValue,
 370                                sizeof(LongValue)))
 371         {
 372             /* An exception occurred */
 373             return FALSE;
 374         }
 375
 376         /* Increment ESP by 4 */
 377         State->GeneralRegs[FAST486_REG_ESP].Long += sizeof(ULONG);
 378
 379         /* Store the value in the result */
 380         *Value = LongValue;
 381     }
 382     else
 383     {
 384         /* 16-bit size */
 385         USHORT ShortValue;
 386
 387         /* Check if SP is 0xFFFF */
 388         if (State->GeneralRegs[FAST486_REG_ESP].LowWord == 0xFFFF)
 389         {
 390             Fast486Exception(State, FAST486_EXCEPTION_SS);
 391             return FALSE;
 392         }
 393
 394         /* Read the value from SS:SP */
 395         if (!Fast486ReadMemory(State,
 396                                FAST486_REG_SS,
 397                                State->GeneralRegs[FAST486_REG_ESP].LowWord,
 398                                FALSE,
 399                                &ShortValue,
 400                                sizeof(ShortValue)))
 401         {
 402             /* An exception occurred */
 403             return FALSE;
 404         }
 405
 406         /* Increment SP by 2 */
 407         State->GeneralRegs[FAST486_REG_ESP].LowWord += sizeof(USHORT);
 408
 409         /* Store the value in the result */
 410         *Value = ShortValue;
 411     }
 412
 413     return TRUE;
 414 }
 415
 416 FORCEINLINE
 417 BOOLEAN
 418 Fast486LoadSegment(PFAST486_STATE State,
 419                    FAST486_SEG_REGS Segment,
 420                    USHORT Selector)
 421 {
 422     PFAST486_SEG_REG CachedDescriptor;
 423     FAST486_GDT_ENTRY GdtEntry;
 424
 425     ASSERT(Segment < FAST486_NUM_SEG_REGS);
 426
 427     /* Get the cached descriptor */
 428     CachedDescriptor = &State->SegmentRegs[Segment];
 429
 430     /* Check for protected mode */
 431     if ((State->ControlRegisters[FAST486_REG_CR0] & FAST486_CR0_PE) && !State->Flags.Vm)
 432     {
 433         if (!(Selector & SEGMENT_TABLE_INDICATOR))
 434         {
 435             /* Make sure the GDT contains the entry */
 436             if (GET_SEGMENT_INDEX(Selector) >= (State->Gdtr.Size + 1))
 437             {
 438                 Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_GP, Selector);
 439                 return FALSE;
 440             }
 441
 442             /* Read the GDT */
 443             if (!Fast486ReadLinearMemory(State,
 444                                          State->Gdtr.Address
 445                                          + GET_SEGMENT_INDEX(Selector),
 446                                          &GdtEntry,
 447                                          sizeof(GdtEntry)))
 448             {
 449                 /* Exception occurred */
 450                 return FALSE;
 451             }
 452         }
 453         else
 454         {
 455             /* Make sure the LDT contains the entry */
 456             if (GET_SEGMENT_INDEX(Selector) >= (State->Ldtr.Limit + 1))
 457             {
 458                 Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_GP, Selector);
 459                 return FALSE;
 460             }
 461
 462             /* Read the LDT */
 463             if (!Fast486ReadLinearMemory(State,
 464                                          State->Ldtr.Base
 465                                          + GET_SEGMENT_INDEX(Selector),
 466                                          &GdtEntry,
 467                                          sizeof(GdtEntry)))
 468             {
 469                 /* Exception occurred */
 470                 return FALSE;
 471             }
 472         }
 473
 474         if (Segment == FAST486_REG_SS)
 475         {
 476             /* Loading the stack segment */
 477
 478             if (GET_SEGMENT_INDEX(Selector) == 0)
 479             {
 480                 Fast486Exception(State, FAST486_EXCEPTION_GP);
 481                 return FALSE;
 482             }
 483
 484             if (!GdtEntry.SystemType)
 485             {
 486                 /* This is a special descriptor */
 487                 Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_GP, Selector);
 488                 return FALSE;
 489             }
 490
 491             if (GdtEntry.Executable || !GdtEntry.ReadWrite)
 492             {
 493                 Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_GP, Selector);
 494                 return FALSE;
 495             }
 496
 497             if ((GET_SEGMENT_RPL(Selector) != Fast486GetCurrentPrivLevel(State))
 498                 || (GET_SEGMENT_RPL(Selector) != GdtEntry.Dpl))
 499             {
 500                 Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_GP, Selector);
 501                 return FALSE;
 502             }
 503
 504             if (!GdtEntry.Present)
 505             {
 506                 Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_SS, Selector);
 507                 return FALSE;
 508             }
 509         }
 510         else if (Segment == FAST486_REG_CS)
 511         {
 512             /* Loading the code segment */
 513
 514             if (GET_SEGMENT_INDEX(Selector) == 0)
 515             {
 516                 Fast486Exception(State, FAST486_EXCEPTION_GP);
 517                 return FALSE;
 518             }
 519
 520             if (!GdtEntry.SystemType)
 521             {
 522                 // TODO: Call/interrupt/task gates NOT IMPLEMENTED!
 523                 UNIMPLEMENTED;
 524             }
 525             else
 526             {
 527                 if (!GdtEntry.Present)
 528                 {
 529                     Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_NP, Selector);
 530                     return FALSE;
 531                 }
 532
 533                 if (!GdtEntry.Executable)
 534                 {
 535                     Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_GP, Selector);
 536                     return FALSE;
 537                 }
 538
 539                 if (GdtEntry.DirConf)
 540                 {
 541                     /* Conforming Code Segment */
 542
 543                     if (GdtEntry.Dpl > Fast486GetCurrentPrivLevel(State))
 544                     {
 545                         /* Must be accessed from lower-privileged code */
 546                         Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_GP, Selector);
 547                         return FALSE;
 548                     }
 549                 }
 550                 else
 551                 {
 552                     /* Regular code segment */
 553
 554                     if ((GET_SEGMENT_RPL(Selector) > Fast486GetCurrentPrivLevel(State))
 555                         || (Fast486GetCurrentPrivLevel(State) != GdtEntry.Dpl))
 556                     {
 557                         Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_GP, Selector);
 558                         return FALSE;
 559                     }
 560                 }
 561
 562                 /* Update CPL */
 563                 State->Cpl = GET_SEGMENT_RPL(Selector);
 564             }
 565         }
 566         else
 567         {
 568             /* Loading a data segment */
 569
 570             if (GET_SEGMENT_INDEX(Selector) != 0)
 571             {
 572                 if (!GdtEntry.SystemType)
 573                 {
 574                     /* This is a special descriptor */
 575                     Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_GP, Selector);
 576                     return FALSE;
 577                 }
 578
 579                 if ((GET_SEGMENT_RPL(Selector) > GdtEntry.Dpl)
 580                     || (Fast486GetCurrentPrivLevel(State) > GdtEntry.Dpl))
 581                 {
 582                     Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_GP, Selector);
 583                     return FALSE;
 584                 }
 585
 586                 if (!GdtEntry.Present)
 587                 {
 588                     Fast486ExceptionWithErrorCode(State, FAST486_EXCEPTION_NP, Selector);
 589                     return FALSE;
 590                 }
 591             }
 592             else
 593             {
 594                 /* This is a NULL selector */
 595                 RtlZeroMemory(&GdtEntry, sizeof(GdtEntry));
 596             }
 597         }
 598
 599         /* Update the cache entry */
 600         CachedDescriptor->Selector = Selector;
 601         CachedDescriptor->Base = GdtEntry.Base | (GdtEntry.BaseMid << 16) | (GdtEntry.BaseHigh << 24);
 602         CachedDescriptor->Limit = GdtEntry.Limit | (GdtEntry.LimitHigh << 16);
 603         CachedDescriptor->Accessed = GdtEntry.Accessed;
 604         CachedDescriptor->ReadWrite = GdtEntry.ReadWrite;
 605         CachedDescriptor->DirConf = GdtEntry.DirConf;
 606         CachedDescriptor->Executable = GdtEntry.Executable;
 607         CachedDescriptor->SystemType = GdtEntry.SystemType;
 608         CachedDescriptor->Rpl = GET_SEGMENT_RPL(Selector);
 609         CachedDescriptor->Dpl = GdtEntry.Dpl;
 610         CachedDescriptor->Present = GdtEntry.Present;
 611         CachedDescriptor->Size = GdtEntry.Size;
 612
 613         /* Check for page granularity */
 614         if (GdtEntry.Granularity) CachedDescriptor->Limit <<= 12;
 615     }
 616     else
 617     {
 618         /* Update the selector and base */
 619         CachedDescriptor->Selector = Selector;
 620         CachedDescriptor->Base = Selector << 4;
 621     }
 622
 623     return TRUE;
 624 }
 625
 626 FORCEINLINE
 627 BOOLEAN
 628 Fast486FetchByte(PFAST486_STATE State,
 629                  PUCHAR Data)
 630 {
 631     PFAST486_SEG_REG CachedDescriptor;
 632
 633     /* Get the cached descriptor of CS */
 634     CachedDescriptor = &State->SegmentRegs[FAST486_REG_CS];
 635
 636     /* Read from memory */
 637     if (!Fast486ReadMemory(State,
 638                            FAST486_REG_CS,
 639                            (CachedDescriptor->Size) ? State->InstPtr.Long
 640                                                     : State->InstPtr.LowWord,
 641                            TRUE,
 642                            Data,
 643                            sizeof(UCHAR)))
 644     {
 645         /* Exception occurred during instruction fetch */
 646         return FALSE;
 647     }
 648
 649     /* Advance the instruction pointer */
 650     if (CachedDescriptor->Size) State->InstPtr.Long++;
 651     else State->InstPtr.LowWord++;
 652
 653     return TRUE;
 654 }
 655
 656 FORCEINLINE
 657 BOOLEAN
 658 Fast486FetchWord(PFAST486_STATE State,
 659                  PUSHORT Data)
 660 {
 661     PFAST486_SEG_REG CachedDescriptor;
 662
 663     /* Get the cached descriptor of CS */
 664     CachedDescriptor = &State->SegmentRegs[FAST486_REG_CS];
 665
 666     /* Read from memory */
 667     // FIXME: Fix byte order on big-endian machines
 668     if (!Fast486ReadMemory(State,
 669                            FAST486_REG_CS,
 670                            (CachedDescriptor->Size) ? State->InstPtr.Long
 671                                                     : State->InstPtr.LowWord,
 672                            TRUE,
 673                            Data,
 674                            sizeof(USHORT)))
 675     {
 676         /* Exception occurred during instruction fetch */
 677         return FALSE;
 678     }
 679
 680     /* Advance the instruction pointer */
 681     if (CachedDescriptor->Size) State->InstPtr.Long += sizeof(USHORT);
 682     else State->InstPtr.LowWord += sizeof(USHORT);
 683
 684     return TRUE;
 685 }
 686
 687 FORCEINLINE
 688 BOOLEAN
 689 Fast486FetchDword(PFAST486_STATE State,
 690                   PULONG Data)
 691 {
 692     PFAST486_SEG_REG CachedDescriptor;
 693
 694     /* Get the cached descriptor of CS */
 695     CachedDescriptor = &State->SegmentRegs[FAST486_REG_CS];
 696
 697     /* Read from memory */
 698     // FIXME: Fix byte order on big-endian machines
 699     if (!Fast486ReadMemory(State,
 700                            FAST486_REG_CS,
 701                            (CachedDescriptor->Size) ? State->InstPtr.Long
 702                                                     : State->InstPtr.LowWord,
 703                            TRUE,
 704                            Data,
 705                            sizeof(ULONG)))
 706     {
 707         /* Exception occurred during instruction fetch */
 708         return FALSE;
 709     }
 710
 711     /* Advance the instruction pointer */
 712     if (CachedDescriptor->Size) State->InstPtr.Long += sizeof(ULONG);
 713     else State->InstPtr.LowWord += sizeof(ULONG);
 714
 715     return TRUE;
 716 }
 717
 718 FORCEINLINE
 719 BOOLEAN
 720 Fast486CalculateParity(UCHAR Number)
 721 {
 722     // See http://graphics.stanford.edu/~seander/bithacks.html#ParityLookupTable too...
 723     return (0x9669 >> ((Number & 0x0F) ^ (Number >> 4))) & 1;
 724 }
 725
 726 FORCEINLINE
 727 BOOLEAN
 728 Fast486ParseModRegRm(PFAST486_STATE State,
 729                      BOOLEAN AddressSize,
 730                      PFAST486_MOD_REG_RM ModRegRm)
 731 {
 732     UCHAR ModRmByte, Mode, RegMem;
 733
 734     /* Fetch the MOD REG R/M byte */
 735     if (!Fast486FetchByte(State, &ModRmByte))
 736     {
 737         /* Exception occurred */
 738         return FALSE;
 739     }
 740
 741     /* Unpack the mode and R/M */
 742     Mode = ModRmByte >> 6;
 743     RegMem = ModRmByte & 0x07;
 744
 745     /* Set the register operand */
 746     ModRegRm->Register = (ModRmByte >> 3) & 0x07;
 747
 748     /* Check the mode */
 749     if ((ModRmByte >> 6) == 3)
 750     {
 751         /* The second operand is also a register */
 752         ModRegRm->Memory = FALSE;
 753         ModRegRm->SecondRegister = RegMem;
 754
 755         /* Done parsing */
 756         return TRUE;
 757     }
 758
 759     /* The second operand is memory */
 760     ModRegRm->Memory = TRUE;
 761
 762     if (AddressSize)
 763     {
 764         if (RegMem == FAST486_REG_ESP)
 765         {
 766             UCHAR SibByte;
 767             ULONG Scale, Index, Base;
 768
 769             /* Fetch the SIB byte */
 770             if (!Fast486FetchByte(State, &SibByte))
 771             {
 772                 /* Exception occurred */
 773                 return FALSE;
 774             }
 775
 776             /* Unpack the scale, index and base */
 777             Scale = 1 << (SibByte >> 6);
 778             Index = (SibByte >> 3) & 0x07;
 779             if (Index != FAST486_REG_ESP) Index = State->GeneralRegs[Index].Long;
 780             else Index = 0;
 781
 782             if (((SibByte & 0x07) != FAST486_REG_EBP) || (Mode != 0))
 783             {
 784                 /* Use the register a base */
 785                 Base = State->GeneralRegs[SibByte & 0x07].Long;
 786             }
 787             else
 788             {
 789                 /* Fetch the base */
 790                 if (!Fast486FetchDword(State, &Base))
 791                 {
 792                     /* Exception occurred */
 793                     return FALSE;
 794                 }
 795             }
 796
 797             if ((SibByte & 0x07) == FAST486_REG_ESP)
 798             {
 799                 /* Check if there is no segment override */
 800                 if (!(State->PrefixFlags & FAST486_PREFIX_SEG))
 801                 {
 802                     /* Add a SS: prefix */
 803                     State->PrefixFlags |= FAST486_PREFIX_SEG;
 804                     State->SegmentOverride = FAST486_REG_SS;
 805                 }
 806             }
 807
 808             /* Calculate the address */
 809             ModRegRm->MemoryAddress = Base + Index * Scale;
 810         }
 811         else if (RegMem == FAST486_REG_EBP)
 812         {
 813             if (Mode) ModRegRm->MemoryAddress = State->GeneralRegs[FAST486_REG_EBP].Long;
 814             else ModRegRm->MemoryAddress = 0;
 815         }
 816         else
 817         {
 818             /* Get the base from the register */
 819             ModRegRm->MemoryAddress = State->GeneralRegs[RegMem].Long;
 820         }
 821
 822         /* Check if there is no segment override */
 823         if (!(State->PrefixFlags & FAST486_PREFIX_SEG))
 824         {
 825             /* Check if the default segment should be SS */
 826             if ((RegMem == FAST486_REG_EBP) && Mode)
 827             {
 828                 /* Add a SS: prefix */
 829                 State->PrefixFlags |= FAST486_PREFIX_SEG;
 830                 State->SegmentOverride = FAST486_REG_SS;
 831             }
 832         }
 833
 834         if (Mode == 1)
 835         {
 836             CHAR Offset;
 837
 838             /* Fetch the byte */
 839             if (!Fast486FetchByte(State, (PUCHAR)&Offset))
 840             {
 841                 /* Exception occurred */
 842                 return FALSE;
 843             }
 844
 845             /* Add the signed offset to the address */
 846             ModRegRm->MemoryAddress += (LONG)Offset;
 847         }
 848         else if ((Mode == 2) || ((Mode == 0) && (RegMem == FAST486_REG_EBP)))
 849         {
 850             LONG Offset;
 851
 852             /* Fetch the dword */
 853             if (!Fast486FetchDword(State, (PULONG)&Offset))
 854             {
 855                 /* Exception occurred */
 856                 return FALSE;
 857             }
 858
 859             /* Add the signed offset to the address */
 860             ModRegRm->MemoryAddress += Offset;
 861         }
 862     }
 863     else
 864     {
 865         /* Check the operand */
 866         switch (RegMem)
 867         {
 868             case 0:
 869             {
 870                 /* [BX + SI] */
 871                 ModRegRm->MemoryAddress = State->GeneralRegs[FAST486_REG_EBX].LowWord
 872                                            + State->GeneralRegs[FAST486_REG_ESI].LowWord;
 873
 874                 break;
 875             }
 876
 877             case 1:
 878             {
 879                 /* [BX + DI] */
 880                 ModRegRm->MemoryAddress = State->GeneralRegs[FAST486_REG_EBX].LowWord
 881                                            + State->GeneralRegs[FAST486_REG_EDI].LowWord;
 882
 883                 break;
 884             }
 885
 886             case 2:
 887             {
 888                 /* SS:[BP + SI] */
 889                 ModRegRm->MemoryAddress = State->GeneralRegs[FAST486_REG_EBP].LowWord
 890                                            + State->GeneralRegs[FAST486_REG_ESI].LowWord;
 891
 892                 break;
 893             }
 894
 895             case 3:
 896             {
 897                 /* SS:[BP + DI] */
 898                 ModRegRm->MemoryAddress = State->GeneralRegs[FAST486_REG_EBP].LowWord
 899                                            + State->GeneralRegs[FAST486_REG_EDI].LowWord;
 900
 901                 break;
 902             }
 903
 904             case 4:
 905             {
 906                 /* [SI] */
 907                 ModRegRm->MemoryAddress = State->GeneralRegs[FAST486_REG_ESI].LowWord;
 908
 909                 break;
 910             }
 911
 912             case 5:
 913             {
 914                 /* [DI] */
 915                 ModRegRm->MemoryAddress = State->GeneralRegs[FAST486_REG_EDI].LowWord;
 916
 917                 break;
 918             }
 919
 920             case 6:
 921             {
 922                 if (Mode)
 923                 {
 924                     /* [BP] */
 925                     ModRegRm->MemoryAddress = State->GeneralRegs[FAST486_REG_EBP].LowWord;
 926                 }
 927                 else
 928                 {
 929                     /* [constant] (added later) */
 930                     ModRegRm->MemoryAddress = 0;
 931                 }
 932
 933                 break;
 934             }
 935
 936             case 7:
 937             {
 938                 /* [BX] */
 939                 ModRegRm->MemoryAddress = State->GeneralRegs[FAST486_REG_EBX].LowWord;
 940
 941                 break;
 942             }
 943         }
 944
 945         /* Check if there is no segment override */
 946         if (!(State->PrefixFlags & FAST486_PREFIX_SEG))
 947         {
 948             /* Check if the default segment should be SS */
 949             if ((RegMem == 2) || (RegMem == 3) || ((RegMem == 6) && Mode))
 950             {
 951                 /* Add a SS: prefix */
 952                 State->PrefixFlags |= FAST486_PREFIX_SEG;
 953                 State->SegmentOverride = FAST486_REG_SS;
 954             }
 955         }
 956
 957         if (Mode == 1)
 958         {
 959             CHAR Offset;
 960
 961             /* Fetch the byte */
 962             if (!Fast486FetchByte(State, (PUCHAR)&Offset))
 963             {
 964                 /* Exception occurred */
 965                 return FALSE;
 966             }
 967
 968             /* Add the signed offset to the address */
 969             ModRegRm->MemoryAddress += (LONG)Offset;
 970         }
 971         else if ((Mode == 2) || ((Mode == 0) && (RegMem == 6)))
 972         {
 973             SHORT Offset;
 974
 975             /* Fetch the word */
 976             if (!Fast486FetchWord(State, (PUSHORT)&Offset))
 977             {
 978                 /* Exception occurred */
 979                 return FALSE;
 980             }
 981
 982             /* Add the signed offset to the address */
 983             ModRegRm->MemoryAddress += (LONG)Offset;
 984         }
 985
 986         /* Clear the top 16 bits */
 987         ModRegRm->MemoryAddress &= 0x0000FFFF;
 988     }
 989
 990     return TRUE;
 991 }
 992
 993 FORCEINLINE
 994 BOOLEAN
 995 Fast486ReadModrmByteOperands(PFAST486_STATE State,
 996                              PFAST486_MOD_REG_RM ModRegRm,
 997                              PUCHAR RegValue,
 998                              PUCHAR RmValue)
 999 {
1000     FAST486_SEG_REGS Segment = FAST486_REG_DS;
1001
1002     if (RegValue)
1003     {
1004         /* Get the register value */
1005         if (ModRegRm->Register & 0x04)
1006         {
1007             /* AH, CH, DH, BH */
1008             *RegValue = State->GeneralRegs[ModRegRm->Register & 0x03].HighByte;
1009         }
1010         else
1011         {
1012             /* AL, CL, DL, BL */
1013             *RegValue = State->GeneralRegs[ModRegRm->Register & 0x03].LowByte;
1014         }
1015     }
1016
1017     if (RmValue)
1018     {
1019         if (!ModRegRm->Memory)
1020         {
1021             /* Get the second register value */
1022             if (ModRegRm->SecondRegister & 0x04)
1023             {
1024                 /* AH, CH, DH, BH */
1025                 *RmValue = State->GeneralRegs[ModRegRm->SecondRegister & 0x03].HighByte;
1026             }
1027             else
1028             {
1029                 /* AL, CL, DL, BL */
1030                 *RmValue = State->GeneralRegs[ModRegRm->SecondRegister & 0x03].LowByte;
1031             }
1032         }
1033         else
1034         {
1035             /* Check for the segment override */
1036             if (State->PrefixFlags & FAST486_PREFIX_SEG)
1037             {
1038                 /* Use the override segment instead */
1039                 Segment = State->SegmentOverride;
1040             }
1041
1042             /* Read memory */
1043             if (!Fast486ReadMemory(State,
1044                                    Segment,
1045                                    ModRegRm->MemoryAddress,
1046                                    FALSE,
1047                                    RmValue,
1048                                    sizeof(UCHAR)))
1049             {
1050                 /* Exception occurred */
1051                 return FALSE;
1052             }
1053         }
1054     }
1055
1056     return TRUE;
1057 }
1058
1059 FORCEINLINE
1060 BOOLEAN
1061 Fast486ReadModrmWordOperands(PFAST486_STATE State,
1062                              PFAST486_MOD_REG_RM ModRegRm,
1063                              PUSHORT RegValue,
1064                              PUSHORT RmValue)
1065 {
1066     FAST486_SEG_REGS Segment = FAST486_REG_DS;
1067
1068     if (RegValue)
1069     {
1070         /* Get the register value */
1071         *RegValue = State->GeneralRegs[ModRegRm->Register].LowWord;
1072     }
1073
1074     if (RmValue)
1075     {
1076         if (!ModRegRm->Memory)
1077         {
1078             /* Get the second register value */
1079             *RmValue = State->GeneralRegs[ModRegRm->SecondRegister].LowWord;
1080         }
1081         else
1082         {
1083             /* Check for the segment override */
1084             if (State->PrefixFlags & FAST486_PREFIX_SEG)
1085             {
1086                 /* Use the override segment instead */
1087                 Segment = State->SegmentOverride;
1088             }
1089
1090             /* Read memory */
1091             if (!Fast486ReadMemory(State,
1092                                    Segment,
1093                                    ModRegRm->MemoryAddress,
1094                                    FALSE,
1095                                    RmValue,
1096                                    sizeof(USHORT)))
1097             {
1098                 /* Exception occurred */
1099                 return FALSE;
1100             }
1101         }
1102     }
1103
1104     return TRUE;
1105 }
1106
1107 FORCEINLINE
1108 BOOLEAN
1109 Fast486ReadModrmDwordOperands(PFAST486_STATE State,
1110                               PFAST486_MOD_REG_RM ModRegRm,
1111                               PULONG RegValue,
1112                               PULONG RmValue)
1113 {
1114     FAST486_SEG_REGS Segment = FAST486_REG_DS;
1115
1116     if (RegValue)
1117     {
1118         /* Get the register value */
1119         *RegValue = State->GeneralRegs[ModRegRm->Register].Long;
1120     }
1121
1122     if (RmValue)
1123     {
1124         if (!ModRegRm->Memory)
1125         {
1126             /* Get the second register value */
1127             *RmValue = State->GeneralRegs[ModRegRm->SecondRegister].Long;
1128         }
1129         else
1130         {
1131             /* Check for the segment override */
1132             if (State->PrefixFlags & FAST486_PREFIX_SEG)
1133             {
1134                 /* Use the override segment instead */
1135                 Segment = State->SegmentOverride;
1136             }
1137
1138             /* Read memory */
1139             if (!Fast486ReadMemory(State,
1140                                    Segment,
1141                                    ModRegRm->MemoryAddress,
1142                                    FALSE,
1143                                    RmValue,
1144                                    sizeof(ULONG)))
1145             {
1146                 /* Exception occurred */
1147                 return FALSE;
1148             }
1149         }
1150     }
1151
1152     return TRUE;
1153 }
1154
1155 FORCEINLINE
1156 BOOLEAN
1157 Fast486WriteModrmByteOperands(PFAST486_STATE State,
1158                               PFAST486_MOD_REG_RM ModRegRm,
1159                               BOOLEAN WriteRegister,
1160                               UCHAR Value)
1161 {
1162     FAST486_SEG_REGS Segment = FAST486_REG_DS;
1163
1164     if (WriteRegister)
1165     {
1166         /* Store the value in the register */
1167         if (ModRegRm->Register & 0x04)
1168         {
1169             /* AH, CH, DH, BH */
1170             State->GeneralRegs[ModRegRm->Register & 0x03].HighByte = Value;
1171         }
1172         else
1173         {
1174             /* AL, CL, DL, BL */
1175             State->GeneralRegs[ModRegRm->Register & 0x03].LowByte = Value;
1176         }
1177     }
1178     else
1179     {
1180         if (!ModRegRm->Memory)
1181         {
1182             /* Store the value in the second register */
1183             if (ModRegRm->SecondRegister & 0x04)
1184             {
1185                 /* AH, CH, DH, BH */
1186                 State->GeneralRegs[ModRegRm->SecondRegister & 0x03].HighByte = Value;
1187             }
1188             else
1189             {
1190                 /* AL, CL, DL, BL */
1191                 State->GeneralRegs[ModRegRm->SecondRegister & 0x03].LowByte = Value;
1192             }
1193         }
1194         else
1195         {
1196             /* Check for the segment override */
1197             if (State->PrefixFlags & FAST486_PREFIX_SEG)
1198             {
1199                 /* Use the override segment instead */
1200                 Segment = State->SegmentOverride;
1201             }
1202
1203             /* Write memory */
1204             if (!Fast486WriteMemory(State,
1205                                     Segment,
1206                                     ModRegRm->MemoryAddress,
1207                                     &Value,
1208                                     sizeof(UCHAR)))
1209             {
1210                 /* Exception occurred */
1211                 return FALSE;
1212             }
1213         }
1214     }
1215
1216     return TRUE;
1217 }
1218
1219 FORCEINLINE
1220 BOOLEAN
1221 Fast486WriteModrmWordOperands(PFAST486_STATE State,
1222                               PFAST486_MOD_REG_RM ModRegRm,
1223                               BOOLEAN WriteRegister,
1224                               USHORT Value)
1225 {
1226     FAST486_SEG_REGS Segment = FAST486_REG_DS;
1227
1228     if (WriteRegister)
1229     {
1230         /* Store the value in the register */
1231         State->GeneralRegs[ModRegRm->Register].LowWord = Value;
1232     }
1233     else
1234     {
1235         if (!ModRegRm->Memory)
1236         {
1237             /* Store the value in the second register */
1238             State->GeneralRegs[ModRegRm->SecondRegister].LowWord = Value;
1239         }
1240         else
1241         {
1242             /* Check for the segment override */
1243             if (State->PrefixFlags & FAST486_PREFIX_SEG)
1244             {
1245                 /* Use the override segment instead */
1246                 Segment = State->SegmentOverride;
1247             }
1248
1249             /* Write memory */
1250             if (!Fast486WriteMemory(State,
1251                                     Segment,
1252                                     ModRegRm->MemoryAddress,
1253                                     &Value,
1254                                     sizeof(USHORT)))
1255             {
1256                 /* Exception occurred */
1257                 return FALSE;
1258             }
1259         }
1260     }
1261
1262     return TRUE;
1263 }
1264
1265 FORCEINLINE
1266 BOOLEAN
1267 Fast486WriteModrmDwordOperands(PFAST486_STATE State,
1268                                PFAST486_MOD_REG_RM ModRegRm,
1269                                BOOLEAN WriteRegister,
1270                                ULONG Value)
1271 {
1272     FAST486_SEG_REGS Segment = FAST486_REG_DS;
1273
1274     if (WriteRegister)
1275     {
1276         /* Store the value in the register */
1277         State->GeneralRegs[ModRegRm->Register].Long = Value;
1278     }
1279     else
1280     {
1281         if (!ModRegRm->Memory)
1282         {
1283             /* Store the value in the second register */
1284             State->GeneralRegs[ModRegRm->SecondRegister].Long = Value;
1285         }
1286         else
1287         {
1288             /* Check for the segment override */
1289             if (State->PrefixFlags & FAST486_PREFIX_SEG)
1290             {
1291                 /* Use the override segment instead */
1292                 Segment = State->SegmentOverride;
1293             }
1294
1295             /* Write memory */
1296             if (!Fast486WriteMemory(State,
1297                                     Segment,
1298                                     ModRegRm->MemoryAddress,
1299                                     &Value,
1300                                     sizeof(ULONG)))
1301             {
1302                 /* Exception occurred */
1303                 return FALSE;
1304             }
1305         }
1306     }
1307
1308     return TRUE;
1309 }
1310
1311 #ifndef FAST486_NO_FPU
1312
1313 FORCEINLINE
1314 VOID
1315 Fast486FpuNormalize(PFAST486_STATE State, PFAST486_FPU_DATA_REG Data)
1316 {
1317     UINT LeadingZeros;
1318
1319     if (FPU_IS_NORMALIZED(Data)) return;
1320     if (FPU_IS_ZERO(Data))
1321     {
1322         Data->Exponent = 0;
1323         return;
1324     }
1325
1326     LeadingZeros = CountLeadingZeros64(Data->Mantissa);
1327
1328     if (LeadingZeros < Data->Exponent)
1329     {
1330         Data->Mantissa <<= LeadingZeros;
1331         Data->Exponent -= LeadingZeros;
1332     }
1333     else
1334     {
1335         /* Make it denormalized */
1336         Data->Mantissa <<= Data->Exponent - 1;
1337         Data->Exponent = 1;
1338
1339         /* Underflow */
1340         State->FpuStatus.Ue = TRUE;
1341     }
1342 }
1343
1344 FORCEINLINE
1345 USHORT
1346 Fast486GetValueTag(PFAST486_FPU_DATA_REG Data)
1347 {
1348     if (FPU_IS_ZERO(Data)) return FPU_TAG_ZERO;
1349     else if (FPU_IS_NAN(Data)) return FPU_TAG_SPECIAL;
1350     else return FPU_TAG_VALID;
1351 }
1352
1353 FORCEINLINE
1354 VOID
1355 Fast486FpuPush(PFAST486_STATE State,
1356                PFAST486_FPU_DATA_REG Data)
1357 {
1358     State->FpuStatus.Top--;
1359
1360     if (FPU_GET_TAG(0) == FPU_TAG_EMPTY)
1361     {
1362         FPU_ST(0) = *Data;
1363         FPU_SET_TAG(0, Fast486GetValueTag(Data));
1364     }
1365     else State->FpuStatus.Ie = TRUE;
1366 }
1367
1368 FORCEINLINE
1369 VOID
1370 Fast486FpuPop(PFAST486_STATE State)
1371 {
1372     if (FPU_GET_TAG(0) != FPU_TAG_EMPTY)
1373     {
1374         FPU_SET_TAG(0, FPU_TAG_EMPTY);
1375         State->FpuStatus.Top++;
1376     }
1377     else State->FpuStatus.Ie = TRUE;
1378 }
1379
1380 #endif
1381
1382 /* EOF */