Intravision/reactos
1
0
Fork 0
mirror of https://github.com/reactos/reactos.git synced 2025-08-05 21:53:06 +00:00
Code Issues Projects Releases Packages Wiki Activity

Change indentation - make it a bit more readable and same style everywhere.

Browse source 
KiCheckFPU: Change calculation of DummyArea/FxSaveArea (avoid conditional)
KeSaveFloatingPointState: Allocate NonPagedPool for the saved state because function can be called only at IRQL >= DISPATCH

svn path=/trunk/; revision=18286
This commit is contained in:
Gregor Anich 2005-10-05 23:30:39 +00:00
parent 7d6d579a40
commit 2446ae0fd7
1 changed files with 286 additions and 299 deletions
Download patch file Download diff file Expand all files Collapse all files

585
reactos/ntoskrnl/ke/i386/fpu.c
View file

@ -48,235 +48,227 @@ ULONG FxsrSupport = 0; /* used by Ki386ContextSwitch for SMP */
STATIC USHORT STATIC USHORT
KiTagWordFnsaveToFxsave(USHORT TagWord) KiTagWordFnsaveToFxsave(USHORT TagWord)
{ {
INT tmp; INT tmp;
/* /*
* Converts the tag-word. 11 (Empty) is converted into 0, everything else into 1 * 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 = ~TagWord; /* Empty is now 00, any 2 bits containing 1 mean valid */
tmp = (tmp | (tmp >> 1)) & 0x5555; /* 0V0V0V0V0V0V0V0V */ tmp = (tmp | (tmp >> 1)) & 0x5555; /* 0V0V0V0V0V0V0V0V */
tmp = (tmp | (tmp >> 1)) & 0x3333; /* 00VV00VV00VV00VV */ tmp = (tmp | (tmp >> 1)) & 0x3333; /* 00VV00VV00VV00VV */
tmp = (tmp | (tmp >> 2)) & 0x0f0f; /* 0000VVVV0000VVVV */ tmp = (tmp | (tmp >> 2)) & 0x0f0f; /* 0000VVVV0000VVVV */
tmp = (tmp | (tmp >> 4)) & 0x00ff; /* 00000000VVVVVVVV */ tmp = (tmp | (tmp >> 4)) & 0x00ff; /* 00000000VVVVVVVV */
return tmp; return tmp;
} }
STATIC USHORT STATIC USHORT
KiTagWordFxsaveToFnsave(PFXSAVE_FORMAT FxSave) KiTagWordFxsaveToFnsave(PFXSAVE_FORMAT FxSave)
{ {
USHORT TagWord = 0; USHORT TagWord = 0;
UCHAR Tag; UCHAR Tag;
INT i; INT i;
struct FPREG { USHORT Significand[4]; USHORT Exponent; } *FpReg; struct FPREG { USHORT Significand[4]; USHORT Exponent; } *FpReg;
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
{ {
if (FxSave->TagWord & (1 << i)) /* valid */ if (FxSave->TagWord & (1 << i)) /* valid */
{ {
FpReg = (struct FPREG *)(FxSave->RegisterArea + (i * 16)); FpReg = (struct FPREG *)(FxSave->RegisterArea + (i * 16));
switch (FpReg->Exponent & 0x00007fff) switch (FpReg->Exponent & 0x00007fff)
{ {
case 0x0000: case 0x0000:
if (FpReg->Significand[0] == 0 && FpReg->Significand[1] == 0 && if (FpReg->Significand[0] == 0 && FpReg->Significand[1] == 0 &&
FpReg->Significand[2] == 0 && FpReg->Significand[3] == 0) FpReg->Significand[2] == 0 && FpReg->Significand[3] == 0)
{
Tag = 1; /* Zero */ Tag = 1; /* Zero */
}
else else
{
Tag = 2; /* Special */ Tag = 2; /* Special */
}
break; break;
case 0x7fff: case 0x7fff:
Tag = 2; /* Special */ Tag = 2; /* Special */
break; break;
default: default:
if (FpReg->Significand[3] & 0x00008000) if (FpReg->Significand[3] & 0x00008000)
{
Tag = 0; /* Valid */ Tag = 0; /* Valid */
}
else else
{
Tag = 2; /* Special */ Tag = 2; /* Special */
}
break; break;
} }
} }
else /* empty */ else /* empty */
{ {
Tag = 3; Tag = 3;
} }
TagWord |= Tag << (i * 2); TagWord |= Tag << (i * 2);
} }
return TagWord; return TagWord;
} }
STATIC VOID STATIC VOID
KiFnsaveToFxsaveFormat(PFXSAVE_FORMAT FxSave, CONST PFNSAVE_FORMAT FnSave) KiFnsaveToFxsaveFormat(PFXSAVE_FORMAT FxSave, CONST PFNSAVE_FORMAT FnSave)
{ {
INT i; INT i;
FxSave->ControlWord = (USHORT)FnSave->ControlWord; FxSave->ControlWord = (USHORT)FnSave->ControlWord;
FxSave->StatusWord = (USHORT)FnSave->StatusWord; FxSave->StatusWord = (USHORT)FnSave->StatusWord;
FxSave->TagWord = KiTagWordFnsaveToFxsave((USHORT)FnSave->TagWord); FxSave->TagWord = KiTagWordFnsaveToFxsave((USHORT)FnSave->TagWord);
FxSave->ErrorOpcode = (USHORT)(FnSave->ErrorSelector >> 16); FxSave->ErrorOpcode = (USHORT)(FnSave->ErrorSelector >> 16);
FxSave->ErrorOffset = FnSave->ErrorOffset; FxSave->ErrorOffset = FnSave->ErrorOffset;
FxSave->ErrorSelector = FnSave->ErrorSelector & 0x0000ffff; FxSave->ErrorSelector = FnSave->ErrorSelector & 0x0000ffff;
FxSave->DataOffset = FnSave->DataOffset; FxSave->DataOffset = FnSave->DataOffset;
FxSave->DataSelector = FnSave->DataSelector & 0x0000ffff; FxSave->DataSelector = FnSave->DataSelector & 0x0000ffff;
if (XmmSupport) if (XmmSupport)
FxSave->MXCsr = 0x00001f80 & MxcsrFeatureMask; FxSave->MXCsr = 0x00001f80 & MxcsrFeatureMask;
else else
FxSave->MXCsr = 0; FxSave->MXCsr = 0;
FxSave->MXCsrMask = MxcsrFeatureMask; FxSave->MXCsrMask = MxcsrFeatureMask;
memset(FxSave->Reserved3, 0, sizeof(FxSave->Reserved3) + memset(FxSave->Reserved3, 0, sizeof(FxSave->Reserved3) +
sizeof(FxSave->Reserved4)); /* XXX - doesnt zero Align16Byte because sizeof(FxSave->Reserved4)); /* Don't zero Align16Byte because Context->ExtendedRegisters
Context->ExtendedRegisters is only 512 bytes, not 520 */ is only 512 bytes, not 520 */
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
{ {
memcpy(FxSave->RegisterArea + (i * 16), FnSave->RegisterArea + (i * 10), 10); memcpy(FxSave->RegisterArea + (i * 16), FnSave->RegisterArea + (i * 10), 10);
memset(FxSave->RegisterArea + (i * 16) + 10, 0, 6); memset(FxSave->RegisterArea + (i * 16) + 10, 0, 6);
} }
} }
STATIC VOID STATIC VOID
KiFxsaveToFnsaveFormat(PFNSAVE_FORMAT FnSave, CONST PFXSAVE_FORMAT FxSave) KiFxsaveToFnsaveFormat(PFNSAVE_FORMAT FnSave, CONST PFXSAVE_FORMAT FxSave)
{ {
INT i; INT i;
FnSave->ControlWord = 0xffff0000 | FxSave->ControlWord; FnSave->ControlWord = 0xffff0000 | FxSave->ControlWord;
FnSave->StatusWord = 0xffff0000 | FxSave->StatusWord; FnSave->StatusWord = 0xffff0000 | FxSave->StatusWord;
FnSave->TagWord = 0xffff0000 | KiTagWordFxsaveToFnsave(FxSave); FnSave->TagWord = 0xffff0000 | KiTagWordFxsaveToFnsave(FxSave);
FnSave->ErrorOffset = FxSave->ErrorOffset; FnSave->ErrorOffset = FxSave->ErrorOffset;
FnSave->ErrorSelector = FxSave->ErrorSelector & 0x0000ffff; FnSave->ErrorSelector = FxSave->ErrorSelector & 0x0000ffff;
FnSave->ErrorSelector |= FxSave->ErrorOpcode << 16; FnSave->ErrorSelector |= FxSave->ErrorOpcode << 16;
FnSave->DataOffset = FxSave->DataOffset; FnSave->DataOffset = FxSave->DataOffset;
FnSave->DataSelector = FxSave->DataSelector | 0xffff0000; FnSave->DataSelector = FxSave->DataSelector | 0xffff0000;
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
{ {
memcpy(FnSave->RegisterArea + (i * 10), FxSave->RegisterArea + (i * 16), 10); memcpy(FnSave->RegisterArea + (i * 10), FxSave->RegisterArea + (i * 16), 10);
} }
} }
VOID VOID
KiFloatingSaveAreaToFxSaveArea(PFX_SAVE_AREA FxSaveArea, CONST FLOATING_SAVE_AREA *FloatingSaveArea) KiFloatingSaveAreaToFxSaveArea(PFX_SAVE_AREA FxSaveArea, CONST FLOATING_SAVE_AREA *FloatingSaveArea)
{ {
if (FxsrSupport) if (FxsrSupport)
{ {
KiFnsaveToFxsaveFormat(&FxSaveArea->U.FxArea, (PFNSAVE_FORMAT)FloatingSaveArea); KiFnsaveToFxsaveFormat(&FxSaveArea->U.FxArea, (PFNSAVE_FORMAT)FloatingSaveArea);
} }
else else
{ {
memcpy(&FxSaveArea->U.FnArea, FloatingSaveArea, sizeof(FxSaveArea->U.FnArea)); memcpy(&FxSaveArea->U.FnArea, FloatingSaveArea, sizeof(FxSaveArea->U.FnArea));
} }
FxSaveArea->NpxSavedCpu = 0; FxSaveArea->NpxSavedCpu = 0;
FxSaveArea->Cr0NpxState = FloatingSaveArea->Cr0NpxState; FxSaveArea->Cr0NpxState = FloatingSaveArea->Cr0NpxState;
} }
BOOL BOOL
KiContextToFxSaveArea(PFX_SAVE_AREA FxSaveArea, PCONTEXT Context) KiContextToFxSaveArea(PFX_SAVE_AREA FxSaveArea, PCONTEXT Context)
{ {
BOOL FpuContextChanged = FALSE; BOOL FpuContextChanged = FALSE;
/* First of all convert the FLOATING_SAVE_AREA into the FX_SAVE_AREA */ /* First of all convert the FLOATING_SAVE_AREA into the FX_SAVE_AREA */
if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) == CONTEXT_FLOATING_POINT) if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) == CONTEXT_FLOATING_POINT)
{ {
KiFloatingSaveAreaToFxSaveArea(FxSaveArea, &Context->FloatSave); KiFloatingSaveAreaToFxSaveArea(FxSaveArea, &Context->FloatSave);
FpuContextChanged = TRUE; FpuContextChanged = TRUE;
} }
/* Now merge the FX_SAVE_AREA from the context with the destination area */ /* Now merge the FX_SAVE_AREA from the context with the destination area */
if ((Context->ContextFlags & CONTEXT_EXTENDED_REGISTERS) == CONTEXT_EXTENDED_REGISTERS) if ((Context->ContextFlags & CONTEXT_EXTENDED_REGISTERS) == CONTEXT_EXTENDED_REGISTERS)
{ {
if (FxsrSupport) if (FxsrSupport)
{ {
PFXSAVE_FORMAT src = (PFXSAVE_FORMAT)Context->ExtendedRegisters; PFXSAVE_FORMAT src = (PFXSAVE_FORMAT)Context->ExtendedRegisters;
PFXSAVE_FORMAT dst = &FxSaveArea->U.FxArea; PFXSAVE_FORMAT dst = &FxSaveArea->U.FxArea;
dst->MXCsr = src->MXCsr & MxcsrFeatureMask; dst->MXCsr = src->MXCsr & MxcsrFeatureMask;
memcpy(dst->Reserved3, src->Reserved3, memcpy(dst->Reserved3, src->Reserved3,
sizeof(src->Reserved3) + sizeof(src->Reserved4)); sizeof(src->Reserved3) + sizeof(src->Reserved4));
if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) != CONTEXT_FLOATING_POINT) if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) != CONTEXT_FLOATING_POINT)
{ {
dst->ControlWord = src->ControlWord; dst->ControlWord = src->ControlWord;
dst->StatusWord = src->StatusWord; dst->StatusWord = src->StatusWord;
dst->TagWord = src->TagWord; dst->TagWord = src->TagWord;
dst->ErrorOpcode = src->ErrorOpcode; dst->ErrorOpcode = src->ErrorOpcode;
dst->ErrorOffset = src->ErrorOffset; dst->ErrorOffset = src->ErrorOffset;
dst->ErrorSelector = src->ErrorSelector; dst->ErrorSelector = src->ErrorSelector;
dst->DataOffset = src->DataOffset; dst->DataOffset = src->DataOffset;
dst->DataSelector = src->DataSelector; dst->DataSelector = src->DataSelector;
memcpy(dst->RegisterArea, src->RegisterArea, sizeof(src->RegisterArea)); memcpy(dst->RegisterArea, src->RegisterArea, sizeof(src->RegisterArea));
FxSaveArea->NpxSavedCpu = 0; FxSaveArea->NpxSavedCpu = 0;
FxSaveArea->Cr0NpxState = 0; FxSaveArea->Cr0NpxState = 0;
} }
FpuContextChanged = TRUE; FpuContextChanged = TRUE;
} }
} }
return FpuContextChanged; return FpuContextChanged;
} }
VOID INIT_FUNCTION VOID INIT_FUNCTION
KiCheckFPU(VOID) KiCheckFPU(VOID)
{ {
unsigned short int status; unsigned short int status;
int cr0; int cr0;
ULONG Flags; ULONG Flags;
PKPRCB Prcb = KeGetCurrentPrcb(); PKPRCB Prcb = KeGetCurrentPrcb();
Ke386SaveFlags(Flags); Ke386SaveFlags(Flags);
Ke386DisableInterrupts(); Ke386DisableInterrupts();
HardwareMathSupport = 0; HardwareMathSupport = 0;
FxsrSupport = 0; FxsrSupport = 0;
XmmSupport = 0; XmmSupport = 0;
cr0 = Ke386GetCr0(); cr0 = Ke386GetCr0();
cr0 |= X86_CR0_NE | X86_CR0_MP; cr0 |= X86_CR0_NE | X86_CR0_MP;
cr0 &= ~(X86_CR0_EM | X86_CR0_TS); cr0 &= ~(X86_CR0_EM | X86_CR0_TS);
Ke386SetCr0(cr0); Ke386SetCr0(cr0);
#if defined(__GNUC__) #if defined(__GNUC__)
asm volatile("fninit\n\t"); asm volatile("fninit\n\t");
asm volatile("fstsw %0\n\t" : "=a" (status)); asm volatile("fstsw %0\n\t" : "=a" (status));
#elif defined(_MSC_VER) #elif defined(_MSC_VER)
__asm __asm
{ {
fninit; fninit;
fstsw status fstsw status
} }
#else #else
#error Unknown compiler for inline assembler #error Unknown compiler for inline assembler
#endif #endif
if (status != 0) if (status != 0)
{ {
/* Set the EM flag in CR0 so any FPU instructions cause a trap. */ /* Set the EM flag in CR0 so any FPU instructions cause a trap. */
Ke386SetCr0(Ke386GetCr0() | X86_CR0_EM); Ke386SetCr0(Ke386GetCr0() | X86_CR0_EM);
Ke386RestoreFlags(Flags); Ke386RestoreFlags(Flags);
return; return;
} }
/* fsetpm for i287, ignored by i387 */ /* fsetpm for i287, ignored by i387 */
#if defined(__GNUC__) #if defined(__GNUC__)
asm volatile(".byte 0xDB, 0xE4\n\t"); asm volatile(".byte 0xDB, 0xE4\n\t");
#elif defined(_MSC_VER) #elif defined(_MSC_VER)
__asm _emit 0xDB __asm _emit 0xe4 __asm _emit 0xDB __asm _emit 0xe4
#else #else
#error Unknown compiler for inline assembler #error Unknown compiler for inline assembler
#endif #endif
HardwareMathSupport = 1; HardwareMathSupport = 1;
/* check for and enable MMX/SSE support if possible */ /* check for and enable MMX/SSE support if possible */
if ((Prcb->FeatureBits & X86_FEATURE_FXSR) != 0) if ((Prcb->FeatureBits & X86_FEATURE_FXSR) != 0)
{ {
BYTE DummyArea[sizeof(FX_SAVE_AREA) + 15]; BYTE DummyArea[sizeof(FX_SAVE_AREA) + 15];
PFX_SAVE_AREA FxSaveArea; PFX_SAVE_AREA FxSaveArea;
@ -284,32 +276,28 @@ KiCheckFPU(VOID)
FxsrSupport = 1; FxsrSupport = 1;
/* we need a 16 byte aligned FX_SAVE_AREA */ /* we need a 16 byte aligned FX_SAVE_AREA */
FxSaveArea = (PFX_SAVE_AREA)DummyArea; FxSaveArea = (PFX_SAVE_AREA)(((ULONG_PTR)DummyArea + 0xf) & (~0x0f));
if ((ULONG_PTR)FxSaveArea & 0x0f)
{
FxSaveArea = (PFX_SAVE_AREA)(((ULONG_PTR)FxSaveArea + 0x10) & (~0x0f));
}
Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSFXSR); Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSFXSR);
memset(&FxSaveArea->U.FxArea, 0, sizeof(FxSaveArea->U.FxArea)); memset(&FxSaveArea->U.FxArea, 0, sizeof(FxSaveArea->U.FxArea));
asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea)); asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
MxcsrFeatureMask = FxSaveArea->U.FxArea.MXCsrMask; MxcsrFeatureMask = FxSaveArea->U.FxArea.MXCsrMask;
if (MxcsrFeatureMask == 0) if (MxcsrFeatureMask == 0)
{ {
MxcsrFeatureMask = 0x0000ffbf; MxcsrFeatureMask = 0x0000ffbf;
} }
} }
/* FIXME: Check for SSE3 in Ke386CpuidFlags2! */ /* FIXME: Check for SSE3 in Ke386CpuidFlags2! */
if (Prcb->FeatureBits & (X86_FEATURE_SSE | X86_FEATURE_SSE2)) if (Prcb->FeatureBits & (X86_FEATURE_SSE | X86_FEATURE_SSE2))
{ {
Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSXMMEXCPT); Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSXMMEXCPT);
/* enable SSE */ /* enable SSE */
XmmSupport = 1; XmmSupport = 1;
} }
Ke386SetCr0(Ke386GetCr0() | X86_CR0_TS); Ke386SetCr0(Ke386GetCr0() | X86_CR0_TS);
Ke386RestoreFlags(Flags); Ke386RestoreFlags(Flags);
} }
/* This is a rather naive implementation of Ke(Save/Restore)FloatingPointState /* This is a rather naive implementation of Ke(Save/Restore)FloatingPointState
@ -320,261 +308,260 @@ KiCheckFPU(VOID)
NTSTATUS STDCALL NTSTATUS STDCALL
KeSaveFloatingPointState(OUT PKFLOATING_SAVE Save) KeSaveFloatingPointState(OUT PKFLOATING_SAVE Save)
{ {
char *FpState; char *FpState;
ASSERT_IRQL(DISPATCH_LEVEL); /* FIXME: is this removed for non-debug builds? I hope not! */ ASSERT_IRQL(DISPATCH_LEVEL);
/* check if we are doing software emulation */ /* check if we are doing software emulation */
if (!HardwareMathSupport) if (!HardwareMathSupport)
{ {
return STATUS_ILLEGAL_FLOAT_CONTEXT; return STATUS_ILLEGAL_FLOAT_CONTEXT;
} }
FpState = ExAllocatePool(PagedPool, FPU_STATE_SIZE); FpState = ExAllocatePool(NonPagedPool, FPU_STATE_SIZE);
if (NULL == FpState) if (NULL == FpState)
{ {
return STATUS_INSUFFICIENT_RESOURCES; return STATUS_INSUFFICIENT_RESOURCES;
} }
*((PVOID *) Save) = FpState; *((PVOID *) Save) = FpState;
#if defined(__GNUC__) #if defined(__GNUC__)
asm volatile("fsave %0\n\t" : "=m" (*FpState)); asm volatile("fsave %0\n\t" : "=m" (*FpState));
#elif defined(_MSC_VER) #elif defined(_MSC_VER)
__asm mov eax, FpState; __asm mov eax, FpState;
__asm fsave [eax]; __asm fsave [eax];
#else #else
#error Unknown compiler for inline assembler #error Unknown compiler for inline assembler
#endif #endif
KeGetCurrentThread()->NpxIrql = KeGetCurrentIrql(); KeGetCurrentThread()->NpxIrql = KeGetCurrentIrql();
return STATUS_SUCCESS; return STATUS_SUCCESS;
} }
NTSTATUS STDCALL NTSTATUS STDCALL
KeRestoreFloatingPointState(IN PKFLOATING_SAVE Save) KeRestoreFloatingPointState(IN PKFLOATING_SAVE Save)
{ {
char *FpState = *((PVOID *) Save); char *FpState = *((PVOID *) Save);
if (KeGetCurrentThread()->NpxIrql != KeGetCurrentIrql()) if (KeGetCurrentThread()->NpxIrql != KeGetCurrentIrql())
{ {
KEBUGCHECK(UNDEFINED_BUG_CODE); KEBUGCHECK(UNDEFINED_BUG_CODE);
} }
#if defined(__GNUC__) #if defined(__GNUC__)
__asm__("frstor %0\n\t" : "=m" (*FpState)); __asm__("frstor %0\n\t" : "=m" (*FpState));
#elif defined(_MSC_VER) #elif defined(_MSC_VER)
__asm mov eax, FpState; __asm mov eax, FpState;
__asm frstor [eax]; __asm frstor [eax];
#else #else
#error Unknown compiler for inline assembler #error Unknown compiler for inline assembler
#endif #endif
ExFreePool(FpState); ExFreePool(FpState);
return STATUS_SUCCESS; return STATUS_SUCCESS;
} }
NTSTATUS NTSTATUS
KiHandleFpuFault(PKTRAP_FRAME Tf, ULONG ExceptionNr) KiHandleFpuFault(PKTRAP_FRAME Tf, ULONG ExceptionNr)
{ {
if (ExceptionNr == 7) /* device not present */ if (ExceptionNr == 7) /* device not present */
{ {
BOOL FpuInitialized = FALSE; BOOL FpuInitialized = FALSE;
unsigned int cr0 = Ke386GetCr0(); unsigned int cr0 = Ke386GetCr0();
PKTHREAD CurrentThread; PKTHREAD CurrentThread;
PFX_SAVE_AREA FxSaveArea; PFX_SAVE_AREA FxSaveArea;
KIRQL oldIrql; KIRQL oldIrql;
#ifndef CONFIG_SMP #ifndef CONFIG_SMP
PKTHREAD NpxThread; PKTHREAD NpxThread;
#endif #endif
(void) cr0; (void) cr0;
ASSERT((cr0 & X86_CR0_TS) == X86_CR0_TS); ASSERT((cr0 & X86_CR0_TS) == X86_CR0_TS);
ASSERT((Tf->Eflags & X86_EFLAGS_VM) == 0); ASSERT((Tf->Eflags & X86_EFLAGS_VM) == 0);
ASSERT((cr0 & X86_CR0_EM) == 0); ASSERT((cr0 & X86_CR0_EM) == 0);
/* disable scheduler, clear TS in cr0 */ /* disable scheduler, clear TS in cr0 */
ASSERT_IRQL(DISPATCH_LEVEL); ASSERT_IRQL(DISPATCH_LEVEL);
KeRaiseIrql(DISPATCH_LEVEL, &oldIrql); KeRaiseIrql(DISPATCH_LEVEL, &oldIrql);
asm volatile("clts"); asm volatile("clts");
CurrentThread = KeGetCurrentThread(); CurrentThread = KeGetCurrentThread();
#ifndef CONFIG_SMP #ifndef CONFIG_SMP
NpxThread = KeGetCurrentPrcb()->NpxThread; NpxThread = KeGetCurrentPrcb()->NpxThread;
#endif #endif
ASSERT(CurrentThread != NULL); ASSERT(CurrentThread != NULL);
DPRINT("Device not present exception happened! (Cr0 = 0x%x, NpxState = 0x%x)\n", cr0, CurrentThread->NpxState); DPRINT("Device not present exception happened! (Cr0 = 0x%x, NpxState = 0x%x)\n", cr0, CurrentThread->NpxState);
#ifndef CONFIG_SMP #ifndef CONFIG_SMP
/* check if the current thread already owns the FPU */ /* check if the current thread already owns the FPU */
if (NpxThread != CurrentThread) /* FIXME: maybe this could be an assertation */ if (NpxThread != CurrentThread) /* FIXME: maybe this could be an assertation */
{ {
/* save the FPU state into the owner's save area */ /* save the FPU state into the owner's save area */
if (NpxThread != NULL) if (NpxThread != NULL)
{ {
KeGetCurrentPrcb()->NpxThread = NULL; KeGetCurrentPrcb()->NpxThread = NULL;
FxSaveArea = (PFX_SAVE_AREA)((char *)NpxThread->InitialStack - sizeof (FX_SAVE_AREA)); FxSaveArea = (PFX_SAVE_AREA)((char *)NpxThread->InitialStack - sizeof (FX_SAVE_AREA));
/* the fnsave might raise a delayed #MF exception */ /* the fnsave might raise a delayed #MF exception */
if (FxsrSupport) if (FxsrSupport)
{ {
asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea)); asm volatile("fxsave %0" : : "m"(FxSaveArea->U.FxArea));
} }
else else
{ {
asm volatile("fnsave %0" : : "m"(FxSaveArea->U.FnArea)); asm volatile("fnsave %0" : : "m"(FxSaveArea->U.FnArea));
FpuInitialized = TRUE; FpuInitialized = TRUE;
} }
NpxThread->NpxState = NPX_STATE_VALID; NpxThread->NpxState = NPX_STATE_VALID;
} }
#endif /* !CONFIG_SMP */ #endif /* !CONFIG_SMP */
/* restore the state of the current thread */ /* restore the state of the current thread */
ASSERT((CurrentThread->NpxState & NPX_STATE_DIRTY) == 0); ASSERT((CurrentThread->NpxState & NPX_STATE_DIRTY) == 0);
FxSaveArea = (PFX_SAVE_AREA)((char *)CurrentThread->InitialStack - sizeof (FX_SAVE_AREA)); FxSaveArea = (PFX_SAVE_AREA)((char *)CurrentThread->InitialStack - sizeof (FX_SAVE_AREA));
if (CurrentThread->NpxState & NPX_STATE_VALID) if (CurrentThread->NpxState & NPX_STATE_VALID)
{ {
if (FxsrSupport) if (FxsrSupport)
{ {
FxSaveArea->U.FxArea.MXCsr &= MxcsrFeatureMask; FxSaveArea->U.FxArea.MXCsr &= MxcsrFeatureMask;
asm volatile("fxrstor %0" : : "m"(FxSaveArea->U.FxArea)); asm volatile("fxrstor %0" : : "m"(FxSaveArea->U.FxArea));
} }
else else
{ {
asm volatile("frstor %0" : : "m"(FxSaveArea->U.FnArea)); asm volatile("frstor %0" : : "m"(FxSaveArea->U.FnArea));
} }
} }
else /* NpxState & NPX_STATE_INVALID */ else /* NpxState & NPX_STATE_INVALID */
{ {
DPRINT("Setting up clean FPU state\n"); DPRINT("Setting up clean FPU state\n");
if (FxsrSupport) if (FxsrSupport)
{ {
memset(&FxSaveArea->U.FxArea, 0, sizeof(FxSaveArea->U.FxArea)); memset(&FxSaveArea->U.FxArea, 0, sizeof(FxSaveArea->U.FxArea));
FxSaveArea->U.FxArea.ControlWord = 0x037f; FxSaveArea->U.FxArea.ControlWord = 0x037f;
if (XmmSupport) if (XmmSupport)
{ {
FxSaveArea->U.FxArea.MXCsr = 0x00001f80 & MxcsrFeatureMask; FxSaveArea->U.FxArea.MXCsr = 0x00001f80 & MxcsrFeatureMask;
} }
asm volatile("fxrstor %0" : : "m"(FxSaveArea->U.FxArea)); asm volatile("fxrstor %0" : : "m"(FxSaveArea->U.FxArea));
} }
else if (!FpuInitialized) else if (!FpuInitialized)
{ {
asm volatile("finit"); asm volatile("finit");
} }
} }
KeGetCurrentPrcb()->NpxThread = CurrentThread; KeGetCurrentPrcb()->NpxThread = CurrentThread;
#ifndef CONFIG_SMP #ifndef CONFIG_SMP
} }
#endif #endif
CurrentThread->NpxState |= NPX_STATE_DIRTY; CurrentThread->NpxState |= NPX_STATE_DIRTY;
KeLowerIrql(oldIrql); KeLowerIrql(oldIrql);
DPRINT("Device not present exception handled!\n"); DPRINT("Device not present exception handled!\n");
return STATUS_SUCCESS; return STATUS_SUCCESS;
} }
else /* ExceptionNr == 16 || ExceptionNr == 19 */ else /* ExceptionNr == 16 || ExceptionNr == 19 */
{ {
EXCEPTION_RECORD Er; EXCEPTION_RECORD Er;
UCHAR DummyContext[sizeof(CONTEXT) + 16]; UCHAR DummyContext[sizeof(CONTEXT) + 16];
PCONTEXT Context; PCONTEXT Context;
KPROCESSOR_MODE PreviousMode; KPROCESSOR_MODE PreviousMode;
PKTHREAD CurrentThread, NpxThread; PKTHREAD CurrentThread, NpxThread;
KIRQL oldIrql; KIRQL oldIrql;
ASSERT(ExceptionNr == 16 || ExceptionNr == 19); /* math fault or XMM fault*/ ASSERT(ExceptionNr == 16 || ExceptionNr == 19); /* math fault or XMM fault*/
KeRaiseIrql(DISPATCH_LEVEL, &oldIrql); KeRaiseIrql(DISPATCH_LEVEL, &oldIrql);
NpxThread = KeGetCurrentPrcb()->NpxThread; NpxThread = KeGetCurrentPrcb()->NpxThread;
CurrentThread = KeGetCurrentThread(); CurrentThread = KeGetCurrentThread();
if (NpxThread == NULL) if (NpxThread == NULL)
{ {
KeLowerIrql(oldIrql); KeLowerIrql(oldIrql);
DPRINT1("!!! Math/Xmm fault ignored! (NpxThread == NULL)\n"); DPRINT1("!!! Math/Xmm fault ignored! (NpxThread == NULL)\n");
return STATUS_SUCCESS; return STATUS_SUCCESS;
} }
PreviousMode = ((Tf->Cs & 0xffff) == USER_CS) ? (UserMode) : (KernelMode); PreviousMode = ((Tf->Cs & 0xffff) == USER_CS) ? (UserMode) : (KernelMode);
DPRINT("Math/Xmm fault happened! (PreviousMode = %s)\n", DPRINT("Math/Xmm fault happened! (PreviousMode = %s)\n",
(PreviousMode != KernelMode) ? ("UserMode") : ("KernelMode")); (PreviousMode != KernelMode) ? ("UserMode") : ("KernelMode"));
ASSERT(NpxThread == CurrentThread); /* FIXME: Is not always true I think */ ASSERT(NpxThread == CurrentThread); /* FIXME: Is not always true I think */
/* For fxsave we have to align Context->ExtendedRegisters on 16 bytes */ /* For fxsave we have to align Context->ExtendedRegisters on 16 bytes */
Context = (PCONTEXT)DummyContext; Context = (PCONTEXT)DummyContext;
Context = (PCONTEXT)((ULONG_PTR)Context + 0x10 - ((ULONG_PTR)Context->ExtendedRegisters & 0x0f)); Context = (PCONTEXT)((ULONG_PTR)Context + 0x10 - ((ULONG_PTR)Context->ExtendedRegisters & 0x0f));
/* Get FPU/XMM state */ /* Get FPU/XMM state */
Context->FloatSave.Cr0NpxState = 0; Context->FloatSave.Cr0NpxState = 0;
if (FxsrSupport) if (FxsrSupport)
{ {
PFXSAVE_FORMAT FxSave = (PFXSAVE_FORMAT)Context->ExtendedRegisters; PFXSAVE_FORMAT FxSave = (PFXSAVE_FORMAT)Context->ExtendedRegisters;
FxSave->MXCsrMask = MxcsrFeatureMask; FxSave->MXCsrMask = MxcsrFeatureMask;
memset(FxSave->RegisterArea, 0, sizeof(FxSave->RegisterArea) + memset(FxSave->RegisterArea, 0, sizeof(FxSave->RegisterArea) +
sizeof(FxSave->Reserved3) + sizeof(FxSave->Reserved4)); sizeof(FxSave->Reserved3) + sizeof(FxSave->Reserved4));
asm volatile("fxsave %0" : : "m"(*FxSave)); asm volatile("fxsave %0" : : "m"(*FxSave));
KeLowerIrql(oldIrql); KeLowerIrql(oldIrql);
KiFxsaveToFnsaveFormat((PFNSAVE_FORMAT)&Context->FloatSave, FxSave); KiFxsaveToFnsaveFormat((PFNSAVE_FORMAT)&Context->FloatSave, FxSave);
} }
else else
{ {
PFNSAVE_FORMAT FnSave = (PFNSAVE_FORMAT)&Context->FloatSave; PFNSAVE_FORMAT FnSave = (PFNSAVE_FORMAT)&Context->FloatSave;
asm volatile("fnsave %0" : : "m"(*FnSave)); asm volatile("fnsave %0" : : "m"(*FnSave));
KeLowerIrql(oldIrql); KeLowerIrql(oldIrql);
KiFnsaveToFxsaveFormat((PFXSAVE_FORMAT)Context->ExtendedRegisters, FnSave); KiFnsaveToFxsaveFormat((PFXSAVE_FORMAT)Context->ExtendedRegisters, FnSave);
} }
/* Fill the rest of the context */ /* Fill the rest of the context */
Context->ContextFlags = CONTEXT_FULL; Context->ContextFlags = CONTEXT_FULL;
KeTrapFrameToContext(Tf, NULL, Context); KeTrapFrameToContext(Tf, NULL, Context);
Context->ContextFlags |= CONTEXT_FLOATING_POINT | CONTEXT_EXTENDED_REGISTERS; Context->ContextFlags |= CONTEXT_FLOATING_POINT | CONTEXT_EXTENDED_REGISTERS;
/* Determine exception code */ /* Determine exception code */
if (ExceptionNr == 16) if (ExceptionNr == 16)
{ {
USHORT FpuStatusWord = Context->FloatSave.StatusWord & 0xffff; USHORT FpuStatusWord = Context->FloatSave.StatusWord & 0xffff;
DPRINT("FpuStatusWord = 0x%04x\n", FpuStatusWord); DPRINT("FpuStatusWord = 0x%04x\n", FpuStatusWord);
if (FpuStatusWord & X87_SW_IE) if (FpuStatusWord & X87_SW_IE)
Er.ExceptionCode = STATUS_FLOAT_INVALID_OPERATION; Er.ExceptionCode = STATUS_FLOAT_INVALID_OPERATION;
else if (FpuStatusWord & X87_SW_DE) else if (FpuStatusWord & X87_SW_DE)
Er.ExceptionCode = STATUS_FLOAT_DENORMAL_OPERAND; Er.ExceptionCode = STATUS_FLOAT_DENORMAL_OPERAND;
else if (FpuStatusWord & X87_SW_ZE) else if (FpuStatusWord & X87_SW_ZE)
Er.ExceptionCode = STATUS_FLOAT_DIVIDE_BY_ZERO; Er.ExceptionCode = STATUS_FLOAT_DIVIDE_BY_ZERO;
else if (FpuStatusWord & X87_SW_OE) else if (FpuStatusWord & X87_SW_OE)
Er.ExceptionCode = STATUS_FLOAT_OVERFLOW; Er.ExceptionCode = STATUS_FLOAT_OVERFLOW;
else if (FpuStatusWord & X87_SW_UE) else if (FpuStatusWord & X87_SW_UE)
Er.ExceptionCode = STATUS_FLOAT_UNDERFLOW; Er.ExceptionCode = STATUS_FLOAT_UNDERFLOW;
else if (FpuStatusWord & X87_SW_PE) else if (FpuStatusWord & X87_SW_PE)
Er.ExceptionCode = STATUS_FLOAT_INEXACT_RESULT; Er.ExceptionCode = STATUS_FLOAT_INEXACT_RESULT;
else if (FpuStatusWord & X87_SW_SE) else if (FpuStatusWord & X87_SW_SE)
Er.ExceptionCode = STATUS_FLOAT_STACK_CHECK; Er.ExceptionCode = STATUS_FLOAT_STACK_CHECK;
else else
ASSERT(0); /* not reached */ ASSERT(0); /* not reached */
/* FIXME: is this the right way to get the correct EIP of the faulting instruction? */ /* FIXME: is this the right way to get the correct EIP of the faulting instruction? */
Er.ExceptionAddress = (PVOID)Context->FloatSave.ErrorOffset; Er.ExceptionAddress = (PVOID)Context->FloatSave.ErrorOffset;
} }
else /* ExceptionNr == 19 */ else /* ExceptionNr == 19 */
{ {
/* FIXME: When should we use STATUS_FLOAT_MULTIPLE_FAULTS? */ /* FIXME: When should we use STATUS_FLOAT_MULTIPLE_FAULTS? */
Er.ExceptionCode = STATUS_FLOAT_MULTIPLE_TRAPS; Er.ExceptionCode = STATUS_FLOAT_MULTIPLE_TRAPS;
Er.ExceptionAddress = (PVOID)Tf->Eip; Er.ExceptionAddress = (PVOID)Tf->Eip;
} }
Er.ExceptionFlags = 0; Er.ExceptionFlags = 0;
Er.ExceptionRecord = NULL; Er.ExceptionRecord = NULL;
Er.NumberParameters = 0; Er.NumberParameters = 0;
/* Dispatch exception */ /* Dispatch exception */
DPRINT("Dispatching exception (ExceptionCode = 0x%08x)\n", Er.ExceptionCode); DPRINT("Dispatching exception (ExceptionCode = 0x%08x)\n", Er.ExceptionCode);
KiDispatchException(&Er, NULL, Tf, PreviousMode, TRUE); KiDispatchException(&Er, NULL, Tf, PreviousMode, TRUE);
DPRINT("Math-fault handled!\n"); DPRINT("Math-fault handled!\n");
return STATUS_SUCCESS; return STATUS_SUCCESS;
} }
return STATUS_UNSUCCESSFUL; return STATUS_UNSUCCESSFUL;
} }