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