reactos/ntoskrnl/ke/i386/fpu.c

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