-/*
- * ReactOS kernel
- * Copyright (C) 1998, 1999, 2000, 2001 ReactOS Team
+/* $Id$
*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-/*
+ * COPYRIGHT: See COPYING in the top level directory
* PROJECT: ReactOS kernel
* FILE: ntoskrnl/ke/i386/fpu.c
* PURPOSE: Handles the FPU
- * PROGRAMMER: David Welch (welch@mcmail.com)
- * UPDATE HISTORY:
- * Created 22/05/98
+ *
+ * PROGRAMMERS: David Welch (welch@mcmail.com)
*/
/* INCLUDES *****************************************************************/
-#include <ddk/ntddk.h>
-#include <internal/ke.h>
-#include <internal/mm.h>
-#include <internal/ps.h>
-
+#include <roscfg.h>
+#include <ntoskrnl.h>
#define NDEBUG
#include <internal/debug.h>
+/* DEFINES *******************************************************************/
+
+/* x87 Status Word exception flags */
+#define X87_SW_IE (1<<0) /* Invalid Operation */
+#define X87_SW_DE (1<<1) /* Denormalized Operand */
+#define X87_SW_ZE (1<<2) /* Zero Devide */
+#define X87_SW_OE (1<<3) /* Overflow */
+#define X87_SW_UE (1<<4) /* Underflow */
+#define X87_SW_PE (1<<5) /* Precision */
+#define X87_SW_SE (1<<6) /* Stack Fault */
+
+#define X87_SW_ES (1<<7) /* Error Summary */
+
+/* MXCSR exception flags */
+#define MXCSR_IE (1<<0) /* Invalid Operation */
+#define MXCSR_DE (1<<1) /* Denormalized Operand */
+#define MXCSR_ZE (1<<2) /* Zero Devide */
+#define MXCSR_OE (1<<3) /* Overflow */
+#define MXCSR_UE (1<<4) /* Underflow */
+#define MXCSR_PE (1<<5) /* Precision */
+#define MXCSR_DAZ (1<<6) /* Denormals Are Zeros (P4 only) */
+
/* GLOBALS *******************************************************************/
-ULONG HardwareMathSupport;
+ULONG HardwareMathSupport = 0;
+static ULONG MxcsrFeatureMask = 0, XmmSupport = 0;
+ULONG FxsrSupport = 0; /* used by Ki386ContextSwitch for SMP */
/* FUNCTIONS *****************************************************************/
-VOID
+STATIC USHORT
+KiTagWordFnsaveToFxsave(USHORT TagWord)
+{
+ INT tmp;
+
+ /*
+ * Converts the tag-word. 11 (Empty) is converted into 0, everything else into 1
+ */
+ tmp = ~TagWord; /* Empty is now 00, any 2 bits containing 1 mean valid */
+ tmp = (tmp | (tmp >> 1)) & 0x5555; /* 0V0V0V0V0V0V0V0V */
+ tmp = (tmp | (tmp >> 1)) & 0x3333; /* 00VV00VV00VV00VV */
+ tmp = (tmp | (tmp >> 2)) & 0x0f0f; /* 0000VVVV0000VVVV */
+ tmp = (tmp | (tmp >> 4)) & 0x00ff; /* 00000000VVVVVVVV */
+
+ return tmp;
+}
+
+
+STATIC USHORT
+KiTagWordFxsaveToFnsave(PFXSAVE_FORMAT FxSave)
+{
+ USHORT TagWord = 0;
+ UCHAR Tag;
+ INT i;
+ struct FPREG { USHORT Significand[4]; USHORT Exponent; } *FpReg;
+
+ for (i = 0; i < 8; i++)
+ {
+ if (FxSave->TagWord & (1 << i)) /* valid */
+ {
+ FpReg = (struct FPREG *)(FxSave->RegisterArea + (i * 16));
+ switch (FpReg->Exponent & 0x00007fff)
+ {
+ case 0x0000:
+ if (FpReg->Significand[0] == 0 && FpReg->Significand[1] == 0 &&
+ FpReg->Significand[2] == 0 && FpReg->Significand[3] == 0)
+ Tag = 1; /* Zero */
+ else
+ Tag = 2; /* Special */
+ break;
+
+ case 0x7fff:
+ Tag = 2; /* Special */
+ break;
+
+ default:
+ if (FpReg->Significand[3] & 0x00008000)
+ Tag = 0; /* Valid */
+ else
+ Tag = 2; /* Special */
+ break;
+ }
+ }
+ else /* empty */
+ {
+ Tag = 3;
+ }
+ TagWord |= Tag << (i * 2);
+ }
+
+ return TagWord;
+}
+
+
+STATIC VOID
+KiFnsaveToFxsaveFormat(PFXSAVE_FORMAT FxSave, CONST PFNSAVE_FORMAT FnSave)
+{
+ INT i;
+
+ FxSave->ControlWord = (USHORT)FnSave->ControlWord;
+ FxSave->StatusWord = (USHORT)FnSave->StatusWord;
+ FxSave->TagWord = KiTagWordFnsaveToFxsave((USHORT)FnSave->TagWord);
+ FxSave->ErrorOpcode = (USHORT)(FnSave->ErrorSelector >> 16);
+ FxSave->ErrorOffset = FnSave->ErrorOffset;
+ FxSave->ErrorSelector = FnSave->ErrorSelector & 0x0000ffff;
+ FxSave->DataOffset = FnSave->DataOffset;
+ FxSave->DataSelector = FnSave->DataSelector & 0x0000ffff;
+ if (XmmSupport)
+ FxSave->MXCsr = 0x00001f80 & MxcsrFeatureMask;
+ else
+ FxSave->MXCsr = 0;
+ FxSave->MXCsrMask = MxcsrFeatureMask;
+ memset(FxSave->Reserved3, 0, sizeof(FxSave->Reserved3) +
+ sizeof(FxSave->Reserved4)); /* Don't zero Align16Byte because Context->ExtendedRegisters
+ is only 512 bytes, not 520 */
+ for (i = 0; i < 8; i++)
+ {
+ memcpy(FxSave->RegisterArea + (i * 16), FnSave->RegisterArea + (i * 10), 10);
+ memset(FxSave->RegisterArea + (i * 16) + 10, 0, 6);
+ }
+}
+
+STATIC VOID
+KiFxsaveToFnsaveFormat(PFNSAVE_FORMAT FnSave, CONST PFXSAVE_FORMAT FxSave)
+{
+ INT i;
+
+ FnSave->ControlWord = 0xffff0000 | FxSave->ControlWord;
+ FnSave->StatusWord = 0xffff0000 | FxSave->StatusWord;
+ FnSave->TagWord = 0xffff0000 | KiTagWordFxsaveToFnsave(FxSave);
+ FnSave->ErrorOffset = FxSave->ErrorOffset;
+ FnSave->ErrorSelector = FxSave->ErrorSelector & 0x0000ffff;
+ FnSave->ErrorSelector |= FxSave->ErrorOpcode << 16;
+ FnSave->DataOffset = FxSave->DataOffset;
+ FnSave->DataSelector = FxSave->DataSelector | 0xffff0000;
+ for (i = 0; i < 8; i++)
+ {
+ memcpy(FnSave->RegisterArea + (i * 10), FxSave->RegisterArea + (i * 16), 10);
+ }
+}
+
+
+STATIC VOID
+KiFloatingSaveAreaToFxSaveArea(PFX_SAVE_AREA FxSaveArea, CONST FLOATING_SAVE_AREA *FloatingSaveArea)
+{
+ if (FxsrSupport)
+ {
+ KiFnsaveToFxsaveFormat(&FxSaveArea->U.FxArea, (PFNSAVE_FORMAT)FloatingSaveArea);
+ }
+ else
+ {
+ memcpy(&FxSaveArea->U.FnArea, FloatingSaveArea, sizeof(FxSaveArea->U.FnArea));
+ }
+ FxSaveArea->NpxSavedCpu = 0;
+ FxSaveArea->Cr0NpxState = FloatingSaveArea->Cr0NpxState;
+}
+
+
+VOID
+KiFxSaveAreaToFloatingSaveArea(FLOATING_SAVE_AREA *FloatingSaveArea, CONST PFX_SAVE_AREA FxSaveArea)
+{
+ if (FxsrSupport)
+ {
+ KiFxsaveToFnsaveFormat((PFNSAVE_FORMAT)FloatingSaveArea, &FxSaveArea->U.FxArea);
+ }
+ else
+ {
+ memcpy(FloatingSaveArea, &FxSaveArea->U.FnArea, sizeof(FxSaveArea->U.FnArea));
+ }
+ FloatingSaveArea->Cr0NpxState = FxSaveArea->Cr0NpxState;
+}
+
+
+BOOL
+KiContextToFxSaveArea(PFX_SAVE_AREA FxSaveArea, PCONTEXT Context)
+{
+ BOOL FpuContextChanged = FALSE;
+
+ /* First of all convert the FLOATING_SAVE_AREA into the FX_SAVE_AREA */
+ if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) == CONTEXT_FLOATING_POINT)
+ {
+ KiFloatingSaveAreaToFxSaveArea(FxSaveArea, &Context->FloatSave);
+ FpuContextChanged = TRUE;
+ }
+
+ /* Now merge the FX_SAVE_AREA from the context with the destination area */
+ if ((Context->ContextFlags & CONTEXT_EXTENDED_REGISTERS) == CONTEXT_EXTENDED_REGISTERS)
+ {
+ if (FxsrSupport)
+ {
+ PFXSAVE_FORMAT src = (PFXSAVE_FORMAT)Context->ExtendedRegisters;
+ PFXSAVE_FORMAT dst = &FxSaveArea->U.FxArea;
+ dst->MXCsr = src->MXCsr & MxcsrFeatureMask;
+ memcpy(dst->Reserved3, src->Reserved3,
+ sizeof(src->Reserved3) + sizeof(src->Reserved4));
+
+ if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) != CONTEXT_FLOATING_POINT)
+ {
+ dst->ControlWord = src->ControlWord;
+ dst->StatusWord = src->StatusWord;
+ dst->TagWord = src->TagWord;
+ dst->ErrorOpcode = src->ErrorOpcode;
+ dst->ErrorOffset = src->ErrorOffset;
+ dst->ErrorSelector = src->ErrorSelector;
+ dst->DataOffset = src->DataOffset;
+ dst->DataSelector = src->DataSelector;
+ memcpy(dst->RegisterArea, src->RegisterArea, sizeof(src->RegisterArea));
+
+ FxSaveArea->NpxSavedCpu = 0;
+ FxSaveArea->Cr0NpxState = 0;
+ }
+ FpuContextChanged = TRUE;
+ }
+ }
+
+ return FpuContextChanged;
+}
+
+
+VOID INIT_FUNCTION
KiCheckFPU(VOID)
{
- unsigned short int status;
- int cr0;
-
- HardwareMathSupport = 0;
-
- __asm__("movl %%cr0, %0\n\t" : "=a" (cr0));
- /* Set NE and MP. */
- cr0 = cr0 | 0x22;
- /* Clear EM */
- cr0 = cr0 & (~0x4);
- __asm__("movl %0, %%cr0\n\t" : : "a" (cr0));
-
- __asm__("clts\n\t");
- __asm__("fninit\n\t");
- __asm__("fstsw %0\n\t" : "=a" (status));
- if (status != 0)
- {
- __asm__("movl %%cr0, %0\n\t" : "=a" (cr0));
- /* Set the EM flag in CR0 so any FPU instructions cause a trap. */
- cr0 = cr0 | 0x4;
- __asm__("movl %0, %%cr0\n\t" :
- : "a" (cr0));
- return;
- }
- /* fsetpm for i287, ignored by i387 */
- __asm__(".byte 0xDB, 0xE4\n\t");
- HardwareMathSupport = 1;
+ unsigned short int status;
+ int cr0;
+ ULONG Flags;
+ PKPRCB Prcb = KeGetCurrentPrcb();
+
+ Ke386SaveFlags(Flags);
+ Ke386DisableInterrupts();
+
+ HardwareMathSupport = 0;
+ FxsrSupport = 0;
+ XmmSupport = 0;
+
+ cr0 = Ke386GetCr0();
+ cr0 |= X86_CR0_NE | X86_CR0_MP;
+ cr0 &= ~(X86_CR0_EM | X86_CR0_TS);
+ Ke386SetCr0(cr0);
+
+#if defined(__GNUC__)
+ asm volatile("fninit\n\t");
+ asm volatile("fstsw %0\n\t" : "=a" (status));
+#elif defined(_MSC_VER)
+ __asm
+ {
+ fninit;
+ fstsw status
+ }
+#else
+#error Unknown compiler for inline assembler
+#endif
+
+ if (status != 0)
+ {
+ /* Set the EM flag in CR0 so any FPU instructions cause a trap. */
+ Ke386SetCr0(Ke386GetCr0() | X86_CR0_EM);
+ Ke386RestoreFlags(Flags);
+ return;
+ }
+
+ /* fsetpm for i287, ignored by i387 */
+#if defined(__GNUC__)
+ asm volatile(".byte 0xDB, 0xE4\n\t");
+#elif defined(_MSC_VER)
+ __asm _emit 0xDB __asm _emit 0xe4
+#else
+#error Unknown compiler for inline assembler
+#endif
+
+ HardwareMathSupport = 1;
+
+ /* check for and enable MMX/SSE support if possible */
+ if ((Prcb->FeatureBits & X86_FEATURE_FXSR) != 0)
+ {
+ BYTE DummyArea[sizeof(FX_SAVE_AREA) + 15];
+ PFX_SAVE_AREA FxSaveArea;
+
+ /* enable FXSR */
+ FxsrSupport = 1;
+
+ /* we need a 16 byte aligned FX_SAVE_AREA */
+ FxSaveArea = (PFX_SAVE_AREA)(((ULONG_PTR)DummyArea + 0xf) & (~0x0f));
+
+ Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSFXSR);
+ memset(&FxSaveArea->U.FxArea, 0, sizeof(FxSaveArea->U.FxArea));
+ asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
+ MxcsrFeatureMask = FxSaveArea->U.FxArea.MXCsrMask;
+ if (MxcsrFeatureMask == 0)
+ {
+ MxcsrFeatureMask = 0x0000ffbf;
+ }
+ }
+ /* FIXME: Check for SSE3 in Ke386CpuidFlags2! */
+ if (Prcb->FeatureBits & (X86_FEATURE_SSE | X86_FEATURE_SSE2))
+ {
+ Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSXMMEXCPT);
+
+ /* enable SSE */
+ XmmSupport = 1;
+ }
+
+ Ke386SetCr0(Ke386GetCr0() | X86_CR0_TS);
+ Ke386RestoreFlags(Flags);
+}
+
+
+PFX_SAVE_AREA
+KiGetFpuState(PKTHREAD Thread)
+{
+ PFX_SAVE_AREA FxSaveArea = NULL;
+ KIRQL OldIrql;
+ ULONG Cr0;
+
+ KeRaiseIrql(DISPATCH_LEVEL, &OldIrql);
+ if (Thread->NpxState & NPX_STATE_VALID)
+ {
+ FxSaveArea = (PFX_SAVE_AREA)((ULONG_PTR)Thread->InitialStack - sizeof (FX_SAVE_AREA));
+ if (Thread->NpxState & NPX_STATE_DIRTY)
+ {
+ ASSERT(KeGetCurrentPrcb()->NpxThread == Thread);
+
+ Cr0 = Ke386GetCr0();
+ asm volatile("clts");
+ if (FxsrSupport)
+ asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
+ else
+ {
+ asm volatile("fnsave %0" : : "m"(FxSaveArea->U.FnArea));
+ /* FPU state has to be reloaded because fnsave changes it. */
+ Cr0 |= X86_CR0_TS;
+ KeGetCurrentPrcb()->NpxThread = NULL;
+ }
+ Ke386SetCr0(Cr0);
+ Thread->NpxState = NPX_STATE_VALID;
+ }
+ }
+ KeLowerIrql(OldIrql);
+
+ return FxSaveArea;
+}
+
+
+NTSTATUS
+KiHandleFpuFault(PKTRAP_FRAME Tf, ULONG ExceptionNr)
+{
+ if (ExceptionNr == 7) /* device not present */
+ {
+ BOOL FpuInitialized = FALSE;
+ unsigned int cr0 = Ke386GetCr0();
+ PKTHREAD CurrentThread;
+ PFX_SAVE_AREA FxSaveArea;
+ KIRQL oldIrql;
+#ifndef CONFIG_SMP
+ PKTHREAD NpxThread;
+#endif
+
+ (void) cr0;
+ ASSERT((cr0 & X86_CR0_TS) == X86_CR0_TS);
+ ASSERT((Tf->Eflags & X86_EFLAGS_VM) == 0);
+ ASSERT((cr0 & X86_CR0_EM) == 0);
+
+ /* disable scheduler, clear TS in cr0 */
+ ASSERT_IRQL(DISPATCH_LEVEL);
+ KeRaiseIrql(DISPATCH_LEVEL, &oldIrql);
+ asm volatile("clts");
+
+ CurrentThread = KeGetCurrentThread();
+#ifndef CONFIG_SMP
+ NpxThread = KeGetCurrentPrcb()->NpxThread;
+#endif
+
+ ASSERT(CurrentThread != NULL);
+ DPRINT("Device not present exception happened! (Cr0 = 0x%x, NpxState = 0x%x)\n", cr0, CurrentThread->NpxState);
+
+#ifndef CONFIG_SMP
+ /* check if the current thread already owns the FPU */
+ if (NpxThread != CurrentThread) /* FIXME: maybe this could be an assertation */
+ {
+ /* save the FPU state into the owner's save area */
+ if (NpxThread != NULL)
+ {
+ KeGetCurrentPrcb()->NpxThread = NULL;
+ FxSaveArea = (PFX_SAVE_AREA)((ULONG_PTR)NpxThread->InitialStack - sizeof (FX_SAVE_AREA));
+ /* the fnsave might raise a delayed #MF exception */
+ if (FxsrSupport)
+ {
+ asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
+ }
+ else
+ {
+ asm volatile("fnsave %0" : : "m"(FxSaveArea->U.FnArea));
+ FpuInitialized = TRUE;
+ }
+ NpxThread->NpxState = NPX_STATE_VALID;
+ }
+#endif /* !CONFIG_SMP */
+
+ /* restore the state of the current thread */
+ ASSERT((CurrentThread->NpxState & NPX_STATE_DIRTY) == 0);
+ FxSaveArea = (PFX_SAVE_AREA)((ULONG_PTR)CurrentThread->InitialStack - sizeof (FX_SAVE_AREA));
+ if (CurrentThread->NpxState & NPX_STATE_VALID)
+ {
+ if (FxsrSupport)
+ {
+ FxSaveArea->U.FxArea.MXCsr &= MxcsrFeatureMask;
+ asm volatile("fxrstor %0" : : "m"(FxSaveArea->U.FxArea));
+ }
+ else
+ {
+ asm volatile("frstor %0" : : "m"(FxSaveArea->U.FnArea));
+ }
+ }
+ else /* NpxState & NPX_STATE_INVALID */
+ {
+ DPRINT("Setting up clean FPU state\n");
+ if (FxsrSupport)
+ {
+ memset(&FxSaveArea->U.FxArea, 0, sizeof(FxSaveArea->U.FxArea));
+ FxSaveArea->U.FxArea.ControlWord = 0x037f;
+ if (XmmSupport)
+ {
+ FxSaveArea->U.FxArea.MXCsr = 0x00001f80 & MxcsrFeatureMask;
+ }
+ asm volatile("fxrstor %0" : : "m"(FxSaveArea->U.FxArea));
+ }
+ else if (!FpuInitialized)
+ {
+ asm volatile("fninit");
+ }
+ }
+ KeGetCurrentPrcb()->NpxThread = CurrentThread;
+#ifndef CONFIG_SMP
+ }
+#endif
+
+ CurrentThread->NpxState |= NPX_STATE_DIRTY;
+ KeLowerIrql(oldIrql);
+ DPRINT("Device not present exception handled!\n");
+
+ return STATUS_SUCCESS;
+ }
+ else /* ExceptionNr == 16 || ExceptionNr == 19 */
+ {
+ EXCEPTION_RECORD Er;
+ KPROCESSOR_MODE PreviousMode;
+ PKTHREAD CurrentThread, NpxThread;
+ KIRQL OldIrql;
+ ULONG FpuEnvBuffer[7];
+ PFNSAVE_FORMAT FpuEnv = (PFNSAVE_FORMAT)FpuEnvBuffer;
+
+ ASSERT(ExceptionNr == 16 || ExceptionNr == 19); /* math fault or XMM fault*/
+
+ KeRaiseIrql(DISPATCH_LEVEL, &OldIrql);
+
+ NpxThread = KeGetCurrentPrcb()->NpxThread;
+ CurrentThread = KeGetCurrentThread();
+ if (NpxThread == NULL)
+ {
+ KeLowerIrql(OldIrql);
+ DPRINT("Math/Xmm fault ignored! (NpxThread == NULL)\n");
+ return STATUS_SUCCESS;
+ }
+ if (ExceptionNr == 16)
+ {
+ asm volatile("fnstenv %0" : : "m"(*FpuEnv));
+ asm volatile("fldenv %0" : : "m"(*FpuEnv)); /* Stupid x87... */
+ FpuEnv->StatusWord &= 0xffff;
+ }
+ KeLowerIrql(OldIrql);
+
+ PreviousMode = ((Tf->Cs & 0xffff) == USER_CS) ? (UserMode) : (KernelMode);
+ DPRINT("Math/Xmm fault happened! (PreviousMode = %s)\n",
+ (PreviousMode != KernelMode) ? ("UserMode") : ("KernelMode"));
+
+ ASSERT(NpxThread == CurrentThread); /* FIXME: Is not always true I think */
+
+ /* Get FPU/XMM state */
+ KeLowerIrql(OldIrql);
+
+ /* Determine exception code */
+ if (ExceptionNr == 16)
+ {
+ DPRINT("FpuStatusWord = 0x%04x\n", FpuStatusWord);
+
+ if (FpuEnv->StatusWord & X87_SW_IE)
+ Er.ExceptionCode = STATUS_FLOAT_INVALID_OPERATION;
+ else if (FpuEnv->StatusWord & X87_SW_DE)
+ Er.ExceptionCode = STATUS_FLOAT_DENORMAL_OPERAND;
+ else if (FpuEnv->StatusWord & X87_SW_ZE)
+ Er.ExceptionCode = STATUS_FLOAT_DIVIDE_BY_ZERO;
+ else if (FpuEnv->StatusWord & X87_SW_OE)
+ Er.ExceptionCode = STATUS_FLOAT_OVERFLOW;
+ else if (FpuEnv->StatusWord & X87_SW_UE)
+ Er.ExceptionCode = STATUS_FLOAT_UNDERFLOW;
+ else if (FpuEnv->StatusWord & X87_SW_PE)
+ Er.ExceptionCode = STATUS_FLOAT_INEXACT_RESULT;
+ else if (FpuEnv->StatusWord & X87_SW_SE)
+ Er.ExceptionCode = STATUS_FLOAT_STACK_CHECK;
+ else
+ ASSERT(0); /* not reached */
+ Er.ExceptionAddress = (PVOID)FpuEnv->ErrorOffset;
+ }
+ else /* ExceptionNr == 19 */
+ {
+ Er.ExceptionCode = STATUS_FLOAT_MULTIPLE_TRAPS;
+ Er.ExceptionAddress = (PVOID)Tf->Eip;
+ }
+
+ Er.ExceptionFlags = 0;
+ Er.ExceptionRecord = NULL;
+ Er.NumberParameters = 0;
+
+ /* Dispatch exception */
+ DPRINT("Dispatching exception (ExceptionCode = 0x%08x)\n", Er.ExceptionCode);
+ KiDispatchException(&Er, NULL, Tf, PreviousMode, TRUE);
+
+ DPRINT("Math-fault handled!\n");
+ return STATUS_SUCCESS;
+ }
+
+ return STATUS_UNSUCCESSFUL;
+}
+
+
+/* This is a rather naive implementation of Ke(Save/Restore)FloatingPointState
+ which will not work for WDM drivers. Please feel free to improve */
+
+NTSTATUS STDCALL
+KeSaveFloatingPointState(OUT PKFLOATING_SAVE Save)
+{
+ PFNSAVE_FORMAT FpState;
+
+ ASSERT_IRQL(DISPATCH_LEVEL);
+
+ /* check if we are doing software emulation */
+ if (!HardwareMathSupport)
+ {
+ return STATUS_ILLEGAL_FLOAT_CONTEXT;
+ }
+
+ FpState = ExAllocatePool(NonPagedPool, sizeof (FNSAVE_FORMAT));
+ if (NULL == FpState)
+ {
+ return STATUS_INSUFFICIENT_RESOURCES;
+ }
+ *((PVOID *) Save) = FpState;
+
+#if defined(__GNUC__)
+ asm volatile("fnsave %0\n\t" : "=m" (*FpState));
+#elif defined(_MSC_VER)
+ __asm mov eax, FpState;
+ __asm fsave [eax];
+#else
+#error Unknown compiler for inline assembler
+#endif
+
+ KeGetCurrentThread()->DispatcherHeader.NpxIrql = KeGetCurrentIrql();
+
+ return STATUS_SUCCESS;
+}
+
+
+NTSTATUS STDCALL
+KeRestoreFloatingPointState(IN PKFLOATING_SAVE Save)
+{
+ PFNSAVE_FORMAT FpState = *((PVOID *) Save);
+
+ if (KeGetCurrentThread()->DispatcherHeader.NpxIrql != KeGetCurrentIrql())
+ {
+ KEBUGCHECK(UNDEFINED_BUG_CODE);
+ }
+
+#if defined(__GNUC__)
+ asm volatile("fnclex\n\t");
+ asm volatile("frstor %0\n\t" : "=m" (*FpState));
+#elif defined(_MSC_VER)
+ __asm mov eax, FpState;
+ __asm frstor [eax];
+#else
+#error Unknown compiler for inline assembler
+#endif
+
+ ExFreePool(FpState);
+
+ return STATUS_SUCCESS;
}