reactos/ntoskrnl/ke/i386/fpu.c

   1 /* $Id: fpu.c,v 1.18 2004/11/27 16:12:26 hbirr Exp $
   2  *
   3  *  ReactOS kernel
   4  *  Copyright (C) 1998, 1999, 2000, 2001 ReactOS Team
   5  *
   6  *  This program is free software; you can redistribute it and/or modify
   7  *  it under the terms of the GNU General Public License as published by
   8  *  the Free Software Foundation; either version 2 of the License, or
   9  *  (at your option) any later version.
  10  *
  11  *  This program is distributed in the hope that it will be useful,
  12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *  GNU General Public License for more details.
  15  *
  16  *  You should have received a copy of the GNU General Public License
  17  *  along with this program; if not, write to the Free Software
  18  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  19  */
  20 /*
  21  * PROJECT:         ReactOS kernel
  22  * FILE:            ntoskrnl/ke/i386/fpu.c
  23  * PURPOSE:         Handles the FPU
  24  * PROGRAMMER:      David Welch (welch@mcmail.com)
  25  * UPDATE HISTORY:
  26  *                  Created 22/05/98
  27  */
  28
  29 /* INCLUDES *****************************************************************/
  30
  31 #include <roscfg.h>
  32 #include <ntoskrnl.h>
  33 #define NDEBUG
  34 #include <internal/debug.h>
  35
  36 /* DEFINES *******************************************************************/
  37
  38 #define EXCEPTION_FLT_DENORMAL_OPERAND          (0xc000008dL)
  39 #define EXCEPTION_FLT_DIVIDE_BY_ZERO            (0xc000008eL)
  40 #define EXCEPTION_FLT_INEXACT_RESULT            (0xc000008fL)
  41 #define EXCEPTION_FLT_INVALID_OPERATION         (0xc0000090L)
  42 #define EXCEPTION_FLT_OVERFLOW                  (0xc0000091L)
  43 #define EXCEPTION_FLT_STACK_CHECK               (0xc0000092L)
  44 #define EXCEPTION_FLT_UNDERFLOW                 (0xc0000093L)
  45 #define EXCEPTION_FLT_MULTIPLE_FAULTS           (0xC00002B4L)
  46 #define EXCEPTION_FLT_MULTIPLE_TRAPS            (0xC00002B5L)
  47
  48 /* x87 Status Word exception flags */
  49 #define X87_SW_IE       (1<<0)   /* Invalid Operation */
  50 #define X87_SW_DE       (1<<1)   /* Denormalized Operand */
  51 #define X87_SW_ZE       (1<<2)   /* Zero Devide */
  52 #define X87_SW_OE       (1<<3)   /* Overflow */
  53 #define X87_SW_UE       (1<<4)   /* Underflow */
  54 #define X87_SW_PE       (1<<5)   /* Precision */
  55 #define X87_SW_SE       (1<<6)   /* Stack Fault */
  56
  57 #define X87_SW_ES       (1<<7)   /* Error Summary */
  58
  59 /* MXCSR exception flags */
  60 #define MXCSR_IE        (1<<0)   /* Invalid Operation */
  61 #define MXCSR_DE        (1<<1)   /* Denormalized Operand */
  62 #define MXCSR_ZE        (1<<2)   /* Zero Devide */
  63 #define MXCSR_OE        (1<<3)   /* Overflow */
  64 #define MXCSR_UE        (1<<4)   /* Underflow */
  65 #define MXCSR_PE        (1<<5)   /* Precision */
  66 #define MXCSR_DAZ       (1<<6)   /* Denormals Are Zeros (P4 only) */
  67
  68 /* GLOBALS *******************************************************************/
  69
  70 ULONG HardwareMathSupport = 0;
  71 static ULONG MxcsrFeatureMask = 0, XmmSupport = 0;
  72 ULONG FxsrSupport = 0; /* used by Ki386ContextSwitch for MP */
  73
  74 /* FUNCTIONS *****************************************************************/
  75
  76 STATIC USHORT
  77 KiTagWordFnsaveToFxsave(USHORT TagWord)
  78 {
  79   INT tmp;
  80
  81   /*
  82    * Converts the tag-word. 11 (Empty) is converted into 0, everything else into 1
  83    */
  84   tmp = ~TagWord; /* Empty is now 00, any 2 bits containing 1 mean valid */
  85   tmp = (tmp | (tmp >> 1)) & 0x5555; /* 0V0V0V0V0V0V0V0V */
  86   tmp = (tmp | (tmp >> 1)) & 0x3333; /* 00VV00VV00VV00VV */
  87   tmp = (tmp | (tmp >> 2)) & 0x0f0f; /* 0000VVVV0000VVVV */
  88   tmp = (tmp | (tmp >> 4)) & 0x00ff; /* 00000000VVVVVVVV */
  89
  90   return tmp;
  91 }
  92
  93 STATIC USHORT
  94 KiTagWordFxsaveToFnsave(PFXSAVE_FORMAT FxSave)
  95 {
  96   USHORT TagWord = 0;
  97   UCHAR Tag;
  98   INT i;
  99   struct FPREG { USHORT Significand[4]; USHORT Exponent; } *FpReg;
 100
 101   for (i = 0; i < 8; i++)
 102     {
 103       if (FxSave->TagWord & (1 << i)) /* valid */
 104         {
 105           FpReg = (struct FPREG *)(FxSave->RegisterArea + (i * 16));
 106           switch (FpReg->Exponent & 0x00007fff)
 107             {
 108               case 0x0000:
 109                 if (FpReg->Significand[0] == 0 && FpReg->Significand[1] == 0 &&
 110                     FpReg->Significand[2] == 0 && FpReg->Significand[3] == 0)
 111                   {
 112                     Tag = 1;  /* Zero */
 113                   }
 114                 else
 115                   {
 116                     Tag = 2;  /* Special */
 117                   }
 118                 break;
 119
 120               case 0x7fff:
 121                 Tag = 2;      /* Special */
 122                 break;
 123
 124               default:
 125                 if (FpReg->Significand[3] & 0x00008000)
 126                   {
 127                     Tag = 0;  /* Valid */
 128                   }
 129                 else
 130                   {
 131                     Tag = 2;  /* Special */
 132                   }
 133                 break;
 134             }
 135         }
 136       else /* empty */
 137         {
 138           Tag = 3;
 139         }
 140       TagWord |= Tag << (i * 2);
 141     }
 142
 143   return TagWord;
 144 }
 145
 146 STATIC VOID
 147 KiFnsaveToFxsaveFormat(PFXSAVE_FORMAT FxSave, CONST PFNSAVE_FORMAT FnSave)
 148 {
 149   INT i;
 150
 151   FxSave->ControlWord = (USHORT)FnSave->ControlWord;
 152   FxSave->StatusWord = (USHORT)FnSave->StatusWord;
 153   FxSave->TagWord = KiTagWordFnsaveToFxsave((USHORT)FnSave->TagWord);
 154   FxSave->ErrorOpcode = (USHORT)(FnSave->ErrorSelector >> 16);
 155   FxSave->ErrorOffset = FnSave->ErrorOffset;
 156   FxSave->ErrorSelector = FnSave->ErrorSelector & 0x0000ffff;
 157   FxSave->DataOffset = FnSave->DataOffset;
 158   FxSave->DataSelector = FnSave->DataSelector & 0x0000ffff;
 159   if (XmmSupport)
 160     FxSave->MXCsr = 0x00001f80 & MxcsrFeatureMask;
 161   else
 162     FxSave->MXCsr = 0;
 163   FxSave->MXCsrMask = MxcsrFeatureMask;
 164   memset(FxSave->Reserved3, 0, sizeof(FxSave->Reserved3) +
 165          sizeof(FxSave->Reserved4)); /* XXX - doesnt zero Align16Byte because
 166                                         Context->ExtendedRegisters is only 512 bytes, not 520 */
 167   for (i = 0; i < 8; i++)
 168     {
 169       memcpy(FxSave->RegisterArea + (i * 16), FnSave->RegisterArea + (i * 10), 10);
 170       memset(FxSave->RegisterArea + (i * 16) + 10, 0, 6);
 171     }
 172 }
 173
 174 STATIC VOID
 175 KiFxsaveToFnsaveFormat(PFNSAVE_FORMAT FnSave, CONST PFXSAVE_FORMAT FxSave)
 176 {
 177   INT i;
 178
 179   FnSave->ControlWord = 0xffff0000 | FxSave->ControlWord;
 180   FnSave->StatusWord = 0xffff0000 | FxSave->StatusWord;
 181   FnSave->TagWord = 0xffff0000 | KiTagWordFxsaveToFnsave(FxSave);
 182   FnSave->ErrorOffset = FxSave->ErrorOffset;
 183   FnSave->ErrorSelector = FxSave->ErrorSelector & 0x0000ffff;
 184   FnSave->ErrorSelector |= FxSave->ErrorOpcode << 16;
 185   FnSave->DataOffset = FxSave->DataOffset;
 186   FnSave->DataSelector = FxSave->DataSelector | 0xffff0000;
 187   for (i = 0; i < 8; i++)
 188     {
 189       memcpy(FnSave->RegisterArea + (i * 10), FxSave->RegisterArea + (i * 16), 10);
 190     }
 191 }
 192
 193 VOID
 194 KiFloatingSaveAreaToFxSaveArea(PFX_SAVE_AREA FxSaveArea, CONST FLOATING_SAVE_AREA *FloatingSaveArea)
 195 {
 196   if (FxsrSupport)
 197   {
 198     KiFnsaveToFxsaveFormat(&FxSaveArea->U.FxArea, (PFNSAVE_FORMAT)FloatingSaveArea);
 199   }
 200   else
 201   {
 202     memcpy(&FxSaveArea->U.FnArea, FloatingSaveArea, sizeof(FxSaveArea->U.FnArea));
 203   }
 204   FxSaveArea->NpxSavedCpu = 0;
 205   FxSaveArea->Cr0NpxState = FloatingSaveArea->Cr0NpxState;
 206 }
 207
 208 BOOL
 209 KiContextToFxSaveArea(PFX_SAVE_AREA FxSaveArea, PCONTEXT Context)
 210 {
 211   BOOL FpuContextChanged = FALSE;
 212
 213   /* First of all convert the FLOATING_SAVE_AREA into the FX_SAVE_AREA */
 214   if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) == CONTEXT_FLOATING_POINT)
 215     {
 216       KiFloatingSaveAreaToFxSaveArea(FxSaveArea, &Context->FloatSave);
 217       FpuContextChanged = TRUE;
 218     }
 219
 220   /* Now merge the FX_SAVE_AREA from the context with the destination area */
 221   if ((Context->ContextFlags & CONTEXT_EXTENDED_REGISTERS) == CONTEXT_EXTENDED_REGISTERS)
 222     {
 223       if (FxsrSupport)
 224         {
 225           PFXSAVE_FORMAT src = (PFXSAVE_FORMAT)Context->ExtendedRegisters;
 226           PFXSAVE_FORMAT dst = &FxSaveArea->U.FxArea;
 227           dst->MXCsr = src->MXCsr & MxcsrFeatureMask;
 228           memcpy(dst->Reserved3, src->Reserved3,
 229                  sizeof(src->Reserved3) + sizeof(src->Reserved4));
 230
 231           if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) != CONTEXT_FLOATING_POINT)
 232             {
 233               dst->ControlWord = src->ControlWord;
 234               dst->StatusWord = src->StatusWord;
 235               dst->TagWord = src->TagWord;
 236               dst->ErrorOpcode = src->ErrorOpcode;
 237               dst->ErrorOffset = src->ErrorOffset;
 238               dst->ErrorSelector = src->ErrorSelector;
 239               dst->DataOffset = src->DataOffset;
 240               dst->DataSelector = src->DataSelector;
 241               memcpy(dst->RegisterArea, src->RegisterArea, sizeof(src->RegisterArea));
 242
 243               FxSaveArea->NpxSavedCpu = 0;
 244               FxSaveArea->Cr0NpxState = 0;
 245             }
 246           FpuContextChanged = TRUE;
 247         }
 248     }
 249
 250   return FpuContextChanged;
 251 }
 252
 253 VOID INIT_FUNCTION
 254 KiCheckFPU(VOID)
 255 {
 256    unsigned short int status;
 257    int cr0;
 258    ULONG Flags;
 259    PKPCR Pcr = KeGetCurrentKPCR();
 260
 261    Ke386SaveFlags(Flags);
 262    Ke386DisableInterrupts();
 263
 264    HardwareMathSupport = 0;
 265    FxsrSupport = 0;
 266    XmmSupport = 0;
 267
 268    cr0 = Ke386GetCr0();
 269    cr0 |= X86_CR0_NE | X86_CR0_MP;
 270    cr0 &= ~(X86_CR0_EM | X86_CR0_TS);
 271    Ke386SetCr0(cr0);
 272
 273 #if defined(__GNUC__)
 274    asm volatile("fninit\n\t");
 275    asm volatile("fstsw %0\n\t" : "=a" (status));
 276 #elif defined(_MSC_VER)
 277    __asm
 278    {
 279            fninit;
 280            fstsw status
 281    }
 282 #else
 283 #error Unknown compiler for inline assembler
 284 #endif
 285
 286    if (status != 0)
 287      {
 288         /* Set the EM flag in CR0 so any FPU instructions cause a trap. */
 289         Ke386SetCr0(Ke386GetCr0() | X86_CR0_EM);
 290         Ke386RestoreFlags(Flags);
 291         return;
 292      }
 293
 294    /* fsetpm for i287, ignored by i387 */
 295 #if defined(__GNUC__)
 296    asm volatile(".byte 0xDB, 0xE4\n\t");
 297 #elif defined(_MSC_VER)
 298    __asm _emit 0xDB __asm _emit 0xe4
 299 #else
 300 #error Unknown compiler for inline assembler
 301 #endif
 302
 303    HardwareMathSupport = 1;
 304
 305    /* check for and enable MMX/SSE support if possible */
 306    if ((Pcr->PrcbData.FeatureBits & X86_FEATURE_FXSR) != 0)
 307      {
 308         BYTE DummyArea[sizeof(FX_SAVE_AREA) + 15];
 309         PFX_SAVE_AREA FxSaveArea;
 310
 311         /* enable FXSR */
 312         FxsrSupport = 1;
 313
 314         /* we need a 16 byte aligned FX_SAVE_AREA */
 315         FxSaveArea = (PFX_SAVE_AREA)DummyArea;
 316         if ((ULONG_PTR)FxSaveArea & 0x0f)
 317           {
 318             FxSaveArea = (PFX_SAVE_AREA)(((ULONG_PTR)FxSaveArea + 0x10) & (~0x0f));
 319           }
 320
 321         Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSFXSR);
 322         memset(&FxSaveArea->U.FxArea, 0, sizeof(FxSaveArea->U.FxArea));
 323         asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
 324         MxcsrFeatureMask = FxSaveArea->U.FxArea.MXCsrMask;
 325         if (MxcsrFeatureMask == 0)
 326           {
 327              MxcsrFeatureMask = 0x0000ffbf;
 328           }
 329      }
 330    /* FIXME: Check for SSE3 in Ke386CpuidFlags2! */
 331    if (Pcr->PrcbData.FeatureBits & (X86_FEATURE_SSE | X86_FEATURE_SSE2))
 332      {
 333         Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSXMMEXCPT);
 334
 335         /* enable SSE */
 336         XmmSupport = 1;
 337      }
 338
 339    Ke386SetCr0(Ke386GetCr0() | X86_CR0_TS);
 340    Ke386RestoreFlags(Flags);
 341 }
 342
 343 /* This is a rather naive implementation of Ke(Save/Restore)FloatingPointState
 344    which will not work for WDM drivers. Please feel free to improve */
 345
 346 #define FPU_STATE_SIZE 108
 347
 348 NTSTATUS STDCALL
 349 KeSaveFloatingPointState(OUT PKFLOATING_SAVE Save)
 350 {
 351   char *FpState;
 352
 353   ASSERT_IRQL(DISPATCH_LEVEL); /* FIXME: is this removed for non-debug builds? I hope not! */
 354
 355   /* check if we are doing software emulation */
 356   if (!HardwareMathSupport)
 357     {
 358       return STATUS_ILLEGAL_FLOAT_CONTEXT;
 359     }
 360
 361   FpState = ExAllocatePool(PagedPool, FPU_STATE_SIZE);
 362   if (NULL == FpState)
 363     {
 364       return STATUS_INSUFFICIENT_RESOURCES;
 365     }
 366   *((PVOID *) Save) = FpState;
 367
 368 #if defined(__GNUC__)
 369   asm volatile("fsave %0\n\t" : "=m" (*FpState));
 370 #elif defined(_MSC_VER)
 371   __asm mov eax, FpState;
 372   __asm fsave [eax];
 373 #else
 374 #error Unknown compiler for inline assembler
 375 #endif
 376
 377   KeGetCurrentThread()->NpxIrql = KeGetCurrentIrql();
 378
 379   return STATUS_SUCCESS;
 380 }
 381
 382 NTSTATUS STDCALL
 383 KeRestoreFloatingPointState(IN PKFLOATING_SAVE Save)
 384 {
 385   char *FpState = *((PVOID *) Save);
 386
 387   if (KeGetCurrentThread()->NpxIrql != KeGetCurrentIrql())
 388     {
 389       KEBUGCHECK(UNDEFINED_BUG_CODE);
 390     }
 391
 392 #if defined(__GNUC__)
 393   __asm__("frstor %0\n\t" : "=m" (*FpState));
 394 #elif defined(_MSC_VER)
 395   __asm mov eax, FpState;
 396   __asm frstor [eax];
 397 #else
 398 #error Unknown compiler for inline assembler
 399 #endif
 400
 401   ExFreePool(FpState);
 402
 403   return STATUS_SUCCESS;
 404 }
 405
 406 VOID
 407 KiClearFloatingPointState(BOOLEAN Save)
 408 {
 409    PKTHREAD CurrentThread;
 410    PFX_SAVE_AREA FxSaveArea;
 411
 412    ASSERT(KeGetCurrentIrql() >= DISPATCH_LEVEL);
 413
 414    CurrentThread = KeGetCurrentThread();
 415
 416    if (CurrentThread->NpxState & NPX_STATE_DIRTY)
 417    {
 418       if (Save)
 419       {
 420          FxSaveArea = (PFX_SAVE_AREA)((char *)CurrentThread->InitialStack - sizeof (FX_SAVE_AREA));
 421          if (FxsrSupport)
 422          {
 423             asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
 424          }
 425          else
 426          {
 427             asm volatile("fnsave %0" : : "m"(FxSaveArea->U.FnArea));
 428          }
 429          CurrentThread->NpxState = NPX_STATE_VALID;
 430       }
 431       else
 432       {
 433          CurrentThread->NpxState = NPX_STATE_INVALID;
 434       }
 435       Ke386SetCr0(Ke386GetCr0() | X86_CR0_TS);
 436    }
 437    if (KeGetCurrentKPCR()->PrcbData.NpxThread == CurrentThread)
 438    {
 439       KeGetCurrentKPCR()->PrcbData.NpxThread = NULL;
 440    }
 441 }
 442
 443
 444 NTSTATUS
 445 KiHandleFpuFault(PKTRAP_FRAME Tf, ULONG ExceptionNr)
 446 {
 447   if (ExceptionNr == 7) /* device not present */
 448     {
 449       BOOL FpuInitialized = FALSE;
 450       unsigned int cr0 = Ke386GetCr0();
 451       PKTHREAD CurrentThread;
 452       PFX_SAVE_AREA FxSaveArea;
 453       KIRQL oldIrql;
 454 #ifndef MP
 455       PKTHREAD NpxThread;
 456 #endif
 457
 458       (void) cr0;
 459       ASSERT((cr0 & X86_CR0_TS) == X86_CR0_TS);
 460       ASSERT((Tf->Eflags & X86_EFLAGS_VM) == 0);
 461       ASSERT((cr0 & X86_CR0_EM) == 0);
 462
 463       /* disable scheduler, clear TS in cr0 */
 464       ASSERT_IRQL(DISPATCH_LEVEL);
 465       KeRaiseIrql(DISPATCH_LEVEL, &oldIrql);
 466       asm volatile("clts");
 467
 468       CurrentThread = KeGetCurrentThread();
 469 #ifndef MP
 470       NpxThread = KeGetCurrentKPCR()->PrcbData.NpxThread;
 471 #endif
 472
 473       ASSERT(CurrentThread != NULL);
 474       DPRINT("Device not present exception happened! (Cr0 = 0x%x, NpxState = 0x%x)\n", cr0, CurrentThread->NpxState);
 475
 476 #ifndef MP
 477       /* check if the current thread already owns the FPU */
 478       if (NpxThread != CurrentThread) /* FIXME: maybe this could be an assertation */
 479         {
 480           /* save the FPU state into the owner's save area */
 481           if (NpxThread != NULL)
 482             {
 483               KeGetCurrentKPCR()->PrcbData.NpxThread = NULL;
 484               FxSaveArea = (PFX_SAVE_AREA)((char *)NpxThread->InitialStack - sizeof (FX_SAVE_AREA));
 485               /* the fnsave might raise a delayed #MF exception */
 486               if (FxsrSupport)
 487                 {
 488                   asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
 489                 }
 490               else
 491                 {
 492                   asm volatile("fnsave %0" : : "m"(FxSaveArea->U.FnArea));
 493                   FpuInitialized = TRUE;
 494                 }
 495               NpxThread->NpxState = NPX_STATE_VALID;
 496             }
 497 #endif /* !MP */
 498
 499           /* restore the state of the current thread */
 500           ASSERT((CurrentThread->NpxState & NPX_STATE_DIRTY) == 0);
 501           FxSaveArea = (PFX_SAVE_AREA)((char *)CurrentThread->InitialStack - sizeof (FX_SAVE_AREA));
 502           if (CurrentThread->NpxState & NPX_STATE_VALID)
 503             {
 504               if (FxsrSupport)
 505                 {
 506                   FxSaveArea->U.FxArea.MXCsr &= MxcsrFeatureMask;
 507                   asm volatile("fxrstor %0" : : "m"(FxSaveArea->U.FxArea));
 508                 }
 509               else
 510                 {
 511                   asm volatile("frstor %0" : : "m"(FxSaveArea->U.FnArea));
 512                 }
 513             }
 514           else /* NpxState & NPX_STATE_INVALID */
 515             {
 516               DPRINT("Setting up clean FPU state\n");
 517               if (FxsrSupport)
 518                 {
 519                   memset(&FxSaveArea->U.FxArea, 0, sizeof(FxSaveArea->U.FxArea));
 520                   FxSaveArea->U.FxArea.ControlWord = 0x037f;
 521                   if (XmmSupport)
 522                     {
 523                       FxSaveArea->U.FxArea.MXCsr = 0x00001f80 & MxcsrFeatureMask;
 524                     }
 525                   asm volatile("fxrstor %0" : : "m"(FxSaveArea->U.FxArea));
 526                 }
 527               else if (!FpuInitialized)
 528                 {
 529                   asm volatile("finit");
 530                 }
 531             }
 532           KeGetCurrentKPCR()->PrcbData.NpxThread = CurrentThread;
 533 #ifndef MP
 534         }
 535 #endif
 536
 537       CurrentThread->NpxState |= NPX_STATE_DIRTY;
 538       KeLowerIrql(oldIrql);
 539       DPRINT("Device not present exception handled!\n");
 540
 541       return STATUS_SUCCESS;
 542     }
 543   else /* ExceptionNr == 16 || ExceptionNr == 19 */
 544     {
 545       EXCEPTION_RECORD Er;
 546       UCHAR DummyContext[sizeof(CONTEXT) + 16];
 547       PCONTEXT Context;
 548       KPROCESSOR_MODE PreviousMode;
 549       PKTHREAD CurrentThread, NpxThread;
 550       KIRQL oldIrql;
 551
 552       ASSERT(ExceptionNr == 16 || ExceptionNr == 19); /* math fault or XMM fault*/
 553
 554       KeRaiseIrql(DISPATCH_LEVEL, &oldIrql);
 555
 556       NpxThread = KeGetCurrentKPCR()->PrcbData.NpxThread;
 557       CurrentThread = KeGetCurrentThread();
 558       if (NpxThread == NULL)
 559         {
 560           KeLowerIrql(oldIrql);
 561           DPRINT1("!!! Math/Xmm fault ignored! (NpxThread == NULL)\n");
 562           return STATUS_SUCCESS;
 563         }
 564
 565       PreviousMode = ((Tf->Cs & 0xffff) == USER_CS) ? (UserMode) : (KernelMode);
 566       DPRINT("Math/Xmm fault happened! (PreviousMode = %s)\n",
 567               (PreviousMode == UserMode) ? ("UserMode") : ("KernelMode"));
 568
 569       ASSERT(NpxThread == CurrentThread); /* FIXME: Is not always true I think */
 570
 571       /* For fxsave we have to align Context->ExtendedRegisters on 16 bytes */
 572       Context = (PCONTEXT)DummyContext;
 573       Context = (PCONTEXT)((ULONG_PTR)Context + 0x10 - ((ULONG_PTR)Context->ExtendedRegisters & 0x0f));
 574
 575       /* Get FPU/XMM state */
 576       Context->FloatSave.Cr0NpxState = 0;
 577       if (FxsrSupport)
 578         {
 579           PFXSAVE_FORMAT FxSave = (PFXSAVE_FORMAT)Context->ExtendedRegisters;
 580           FxSave->MXCsrMask = MxcsrFeatureMask;
 581           memset(FxSave->RegisterArea, 0, sizeof(FxSave->RegisterArea) +
 582                  sizeof(FxSave->Reserved3) + sizeof(FxSave->Reserved4));
 583           asm volatile("fxsave %0" : : "m"(*FxSave));
 584           KeLowerIrql(oldIrql);
 585           KiFxsaveToFnsaveFormat((PFNSAVE_FORMAT)&Context->FloatSave, FxSave);
 586         }
 587       else
 588         {
 589           PFNSAVE_FORMAT FnSave = (PFNSAVE_FORMAT)&Context->FloatSave;
 590           asm volatile("fnsave %0" : : "m"(*FnSave));
 591           KeLowerIrql(oldIrql);
 592           KiFnsaveToFxsaveFormat((PFXSAVE_FORMAT)Context->ExtendedRegisters, FnSave);
 593         }
 594
 595       /* Fill the rest of the context */
 596       Context->ContextFlags = CONTEXT_FULL;
 597       KeTrapFrameToContext(Tf, Context);
 598       Context->ContextFlags |= CONTEXT_FLOATING_POINT | CONTEXT_EXTENDED_REGISTERS;
 599
 600       /* Determine exception code */
 601       if (ExceptionNr == 16)
 602         {
 603           USHORT FpuStatusWord = Context->FloatSave.StatusWord & 0xffff;
 604           DPRINT("FpuStatusWord = 0x%04x\n", FpuStatusWord);
 605
 606           if (FpuStatusWord & X87_SW_IE)
 607             Er.ExceptionCode = EXCEPTION_FLT_INVALID_OPERATION;
 608           else if (FpuStatusWord & X87_SW_DE)
 609             Er.ExceptionCode = EXCEPTION_FLT_DENORMAL_OPERAND;
 610           else if (FpuStatusWord & X87_SW_ZE)
 611             Er.ExceptionCode = EXCEPTION_FLT_DIVIDE_BY_ZERO;
 612           else if (FpuStatusWord & X87_SW_OE)
 613             Er.ExceptionCode = EXCEPTION_FLT_OVERFLOW;
 614           else if (FpuStatusWord & X87_SW_UE)
 615             Er.ExceptionCode = EXCEPTION_FLT_UNDERFLOW;
 616           else if (FpuStatusWord & X87_SW_PE)
 617             Er.ExceptionCode = EXCEPTION_FLT_INEXACT_RESULT;
 618           else if (FpuStatusWord & X87_SW_SE)
 619             Er.ExceptionCode = EXCEPTION_FLT_STACK_CHECK;
 620           else
 621             ASSERT(0); /* not reached */
 622           /* FIXME: is this the right way to get the correct EIP of the faulting instruction? */
 623           Er.ExceptionAddress = (PVOID)Context->FloatSave.ErrorOffset;
 624         }
 625       else /* ExceptionNr == 19 */
 626         {
 627           /* FIXME: When should we use EXCEPTION_FLT_MULTIPLE_FAULTS? */
 628           Er.ExceptionCode = EXCEPTION_FLT_MULTIPLE_TRAPS;
 629           Er.ExceptionAddress = (PVOID)Tf->Eip;
 630         }
 631
 632       Er.ExceptionFlags = 0;
 633       Er.ExceptionRecord = NULL;
 634       Er.NumberParameters = 0;
 635
 636       /* Dispatch exception */
 637       DPRINT("Dispatching exception (ExceptionCode = 0x%08x)\n", Er.ExceptionCode);
 638       KiDispatchException(&Er, Context, Tf, PreviousMode, TRUE);
 639
 640       DPRINT("Math-fault handled!\n");
 641       return STATUS_SUCCESS;
 642     }
 643
 644   return STATUS_UNSUCCESSFUL;
 645 }
 646