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