[ion] 23628: - Detect NPX/FPU in simple assembly and XMMI/SSE2 by using CPU Feature flags, replacing the old asm+c mess. - Handle FPU/SSE saving in the CONTEXT<->KTRAP_FRAME routines directly, and using KiFlushNPXState assembly function. - Implement a naive Trap 7 (FPU Fault) handler in assembly that properly does most of the work required to handle an FPU fault. Not yet complete however since it almost ignores user-mode faults (just like trunk). FPU isn't done in ctx switching yet. - Remove all

Author: ion
Date: Mon Aug 21 03:53:07 2006
New Revision: 23628

URL: http://svn.reactos.org/svn/reactos?rev=23628&view=rev
Log:
- Detect NPX/FPU in simple assembly and XMMI/SSE2 by using CPU Feature flags, replacing the old asm+c mess.
- Handle FPU/SSE saving in the CONTEXT<->KTRAP_FRAME routines directly, and using KiFlushNPXState assembly function.
- Implement a naive Trap 7 (FPU Fault) handler in assembly that properly does most of the work required to handle an FPU fault. Not yet complete however since it almost ignores user-mode faults (just like trunk). FPU isn't done in ctx switching yet.
- Remove all the other .c code that dealt with FPU stuff and had become incompatible since more then a year ago.

Modified:
    trunk/reactos/include/ndk/asm.h
    trunk/reactos/ntoskrnl/include/internal/i386/fpu.h
    trunk/reactos/ntoskrnl/ke/i386/ctxswitch.S
    trunk/reactos/ntoskrnl/ke/i386/exp.c
    trunk/reactos/ntoskrnl/ke/i386/fpu.c
    trunk/reactos/ntoskrnl/ke/i386/kernel.c
    trunk/reactos/ntoskrnl/ke/i386/trap.s

Modified: trunk/reactos/include/ndk/asm.h
URL: http://svn.reactos.org/svn/reactos/trunk/reactos/include/ndk/asm.h?rev=23628...
==============================================================================
--- trunk/reactos/include/ndk/asm.h (original)
+++ trunk/reactos/include/ndk/asm.h Mon Aug 21 03:53:07 2006
@@ -382,9 +382,16 @@
 #define STATUS_INVALID_SYSTEM_SERVICE           0xC000001C
 #define STATUS_NO_CALLBACK_ACTIVE               0xC0000258
 #define APC_INDEX_MISMATCH                      0x01
+#define TRAP_CAUSE_UNKNOWN                      0x12
 #define IRQL_GT_ZERO_AT_SYSTEM_SERVICE          0x4A
 #define UNEXPECTED_KERNEL_MODE_TRAP             0x7F
 #define ATTEMPTED_SWITCH_FROM_DPC               0xB8
+
+//
+// IRQL Levels
+//
+#define PASSIVE_LEVEL                           0x0
+#define APC_LEVEL                               0x1
 #endif

//

Modified: trunk/reactos/ntoskrnl/include/internal/i386/fpu.h
URL: http://svn.reactos.org/svn/reactos/trunk/reactos/ntoskrnl/include/internal/i...
==============================================================================
--- trunk/reactos/ntoskrnl/include/internal/i386/fpu.h (original)
+++ trunk/reactos/ntoskrnl/include/internal/i386/fpu.h Mon Aug 21 03:53:07 2006
@@ -1,24 +1,5 @@
 #ifndef __NTOSKRNL_INCLUDE_INTERNAL_I386_FPU_H
 #define __NTOSKRNL_INCLUDE_INTERNAL_I386_FPU_H

-#include <internal/i386/ke.h>
-
-extern ULONG HardwareMathSupport;
-
-VOID
-KiCheckFPU(VOID);
-
-NTSTATUS
-KiHandleFpuFault(PKTRAP_FRAME Tf, ULONG ExceptionNr);
-
-VOID
-KiFxSaveAreaToFloatingSaveArea(FLOATING_SAVE_AREA *FloatingSaveArea, CONST PFX_SAVE_AREA FxSaveArea);
-
-BOOL
-KiContextToFxSaveArea(PFX_SAVE_AREA FxSaveArea, PCONTEXT Context);
-
-PFX_SAVE_AREA
-KiGetFpuState(PKTHREAD Thread);
-
 #endif /* __NTOSKRNL_INCLUDE_INTERNAL_I386_FPU_H */

Modified: trunk/reactos/ntoskrnl/ke/i386/ctxswitch.S
URL: http://svn.reactos.org/svn/reactos/trunk/reactos/ntoskrnl/ke/i386/ctxswitch....
==============================================================================
--- trunk/reactos/ntoskrnl/ke/i386/ctxswitch.S (original)
+++ trunk/reactos/ntoskrnl/ke/i386/ctxswitch.S Mon Aug 21 03:53:07 2006
@@ -15,15 +15,190 @@
 #include <ndk/asm.h>
 .intel_syntax noprefix

-#define Running 2 
-#define SIZEOF_TRAP_FRAME 0x8c
-#define APC_LEVEL 1
-
-/* GLOBALS ****************************************************************/  
-
-.extern _DispatcherDatabaseLock
+#define Running 2

/* FUNCTIONS ****************************************************************/
+
+.globl _KiIsNpxPresent@0
+.func KiIsNpxPresent@0
+_KiIsNpxPresent@0:
+
+    /* Save stack */
+    push ebp
+
+    /* Get CR0 and mask out FPU flags */
+    mov eax, cr0
+    and eax, ~(CR0_MP + CR0_TS + CR0_EM + CR0_ET)
+
+    /* Initialize the FPU and assume FALSE for return */
+    xor edx, edx
+    fninit
+
+    /* Save magic value on stack */
+    mov ecx, 0x42424242
+    push ecx
+
+    /* Setup stack for FPU store */
+    mov ebp ,esp
+    fnstsw [ebp]
+
+    /* Now check if our magic got cleared */
+    cmp byte ptr [ebp], 0
+    jnz NoFpu
+
+    /* Enable FPU, set return to TRUE */
+    or eax, CR0_ET
+    mov edx, 1
+
+    /* If this is a 486 or higher, enable INT 16 as well */
+    cmp dword ptr fs:KPCR_PRCB_CPU_TYPE, 3
+    jbe NoFpu
+    or eax, CR0_NE
+
+NoFpu:
+    /* Set emulation enabled during the first boot phase and set the CR0 */
+    or eax, (CR0_EM + CR0_TS)
+    mov cr0, eax
+
+    /* Restore stack */
+    pop eax
+    pop ebp
+
+    /* Return true or false */
+    mov eax, edx
+    ret
+.endfunc
+
+.globl _KiFlushNPXState@4
+.func KiFlushNPXState@4
+_KiFlushNPXState@4:
+
+    /* Save volatiles and disable interrupts */
+    push esi
+    push edi
+    push ebx
+    pushfd
+    cli
+
+    /* Save the PCR and get the current thread */
+    mov edi, fs:[KPCR_SELF]
+    mov esi, [edi+KPCR_CURRENT_THREAD]
+
+    /* Check if we're already loaded */
+    cmp byte ptr [esi+KTHREAD_NPX_STATE], NPX_STATE_LOADED
+    je IsValid
+
+    /* Check if we're supposed to get it */
+    cmp dword ptr [esp+20], 0
+    je Return
+
+#ifdef DBG
+    /* Assert Fxsr support */
+    test byte ptr _KeI386FxsrPresent, 1
+    jnz AssertOk
+    int 3
+AssertOk:
+#endif
+
+    /* Get CR0 and test if it's valid */
+    mov ebx, cr0
+    test bl, CR0_MP + CR0_TS + CR0_EM
+    jz Cr0OK
+
+    /* Enable fnsave to work */
+    and ebx, ~(CR0_MP + CR0_TS + CR0_EM)
+    mov cr0, ebx
+
+Cr0OK:
+    /* Check if we are the NPX Thread */
+    mov eax, [edi+KPCR_NPX_THREAD]
+    or eax, eax
+    jz DontSave
+
+    /* Check if it's not loaded */
+    cmp byte ptr [eax+KTHREAD_NPX_STATE], NPX_STATE_NOT_LOADED
+    jnz DontSave
+
+#ifdef DBG
+    /* We are the NPX Thread with an unloaded NPX State... this isn't normal! */
+    int 3
+#endif
+
+    /* Save the NPX State */
+    mov ecx, [eax+KTHREAD_INITIAL_STACK]
+    sub ecx, NPX_FRAME_LENGTH
+    fxsave [ecx]
+    mov byte ptr [eax+KTHREAD_NPX_STATE], NPX_STATE_NOT_LOADED
+
+DontSave:
+    /* Load the NPX State */
+    mov ecx, [esi+KTHREAD_INITIAL_STACK]
+    sub ecx, NPX_FRAME_LENGTH
+    fxrstor [ecx]
+
+    /* Get the CR0 state and destination */
+    mov edx, [ecx+FN_CR0_NPX_STATE]
+    mov ecx, [esp+20]
+    jmp DoneLoad
+
+IsValid:
+    /* We already have a valid state, flush it */
+    mov ebx, cr0
+    test bl, CR0_MP + CR0_TS + CR0_EM
+    jz Cr0OK2
+
+    /* Enable fnsave to work */
+    and ebx, ~(CR0_MP + CR0_TS + CR0_EM)
+    mov cr0, ebx
+
+Cr0OK2:
+    /* Get the kernel stack */
+    mov ecx, [esi+KTHREAD_INITIAL_STACK]
+    test byte ptr _KeI386FxsrPresent, 1
+    lea ecx, [ecx-NPX_FRAME_LENGTH]
+
+    /* Set the NPX State */
+    mov byte ptr [esi+KTHREAD_NPX_STATE], NPX_STATE_NOT_LOADED
+
+    /* Get Cr0 */
+    mov edx, [ecx+FN_CR0_NPX_STATE]
+    jz DoneLoad
+
+    /* Save the FX State */
+    fxsave [ecx]
+
+    /* Check if we also have to save it in the parameter */
+    mov ecx, [esp+20]
+    jecxz NoSave
+
+DoneLoad:
+    /* Save the Fn state in the parameter we got */
+    fnsave [ecx]
+    fwait
+
+NoSave:
+    /* Clear eax */
+    xor eax, eax
+
+    /* Add NPX State */
+    or ebx, NPX_STATE_NOT_LOADED
+
+    /* Clear the NPX thread */
+    mov [edi+KPCR_NPX_THREAD], eax
+
+    /* Add saved CR0 into NPX State, and set it */
+    or ebx, edx
+    mov cr0, ebx
+
+    /* Re-enable interrupts and return */
+Return:
+    popf
+    pop ebx
+    pop edi
+    pop esi
+    ret 4
+
+.endfunc

/*++
  * KiThreadStartup

Modified: trunk/reactos/ntoskrnl/ke/i386/exp.c
URL: http://svn.reactos.org/svn/reactos/trunk/reactos/ntoskrnl/ke/i386/exp.c?rev=...
==============================================================================
--- trunk/reactos/ntoskrnl/ke/i386/exp.c (original)
+++ trunk/reactos/ntoskrnl/ke/i386/exp.c Mon Aug 21 03:53:07 2006
@@ -28,10 +28,18 @@
 #pragma alloc_text(INIT, KeInitExceptions)
 #endif

+#define SIZE_OF_FX_REGISTERS 32
+
 VOID
 NTAPI
 Ki386AdjustEsp0(
     IN PKTRAP_FRAME TrapFrame
+);
+
+VOID
+NTAPI
+KiFlushNPXState(
+    IN FLOATING_SAVE_AREA *SaveArea
 );

extern KIDTENTRY KiIdt[];
@@ -467,10 +475,10 @@
  */
 {
    ULONG_PTR cr2;
-   NTSTATUS Status;
    ULONG Esp0;

ASSERT(ExceptionNr != 14);
+   ASSERT((ExceptionNr != 7 && ExceptionNr != 16 && ExceptionNr != 19));

/* Use the address of the trap frame as approximation to the ring0 esp */
    Esp0 = (ULONG)&Tf->Eip;
@@ -526,18 +534,6 @@
      }

/*
-    * Try to handle device-not-present, math-fault and xmm-fault exceptions.
-    */
-   if (ExceptionNr == 7 || ExceptionNr == 16 || ExceptionNr == 19)
-     {
-       Status = KiHandleFpuFault(Tf, ExceptionNr);
-       if (NT_SUCCESS(Status))
-         {
-           return(0);
-         }
-     }
-
-   /*
     * Handle user exceptions differently
     */
    if ((Tf->SegCs & 0xFFFF) == (KGDT_R3_CODE | RPL_MASK))
@@ -660,6 +656,23 @@
         /* Usermode, save the User SS */
         TrapFrame->HardwareSegSs = Ss | RPL_MASK;
     }
+}
+
+USHORT
+NTAPI
+KiTagWordFnsaveToFxsave(USHORT TagWord)
+{
+    INT FxTagWord = ~TagWord; 
+
+    /* 
+     * Empty is now 00, any 2 bits containing 1 mean valid
+     * Now convert the rest (11->0 and the rest to 1)
+     */
+    FxTagWord = (FxTagWord | (FxTagWord >> 1)) & 0x5555; /* 0V0V0V0V0V0V0V0V */
+    FxTagWord = (FxTagWord | (FxTagWord >> 1)) & 0x3333; /* 00VV00VV00VV00VV */
+    FxTagWord = (FxTagWord | (FxTagWord >> 2)) & 0x0f0f; /* 0000VVVV0000VVVV */
+    FxTagWord = (FxTagWord | (FxTagWord >> 4)) & 0x00ff; /* 00000000VVVVVVVV */
+    return FxTagWord;
 }

VOID
@@ -671,7 +684,7 @@
                      IN KPROCESSOR_MODE PreviousMode)
 {
     PFX_SAVE_AREA FxSaveArea;
-    //ULONG i; Future Use
+    ULONG i;
     BOOLEAN V86Switch = FALSE;

/* Start with the basic Registers */
@@ -784,7 +797,21 @@
         /* Check if NPX is present */
         if (KeI386NpxPresent)
         {
-            /* Future use */
+            /* Flush the NPX State */
+            KiFlushNPXState(NULL);
+
+            /* Copy the FX State */
+            RtlCopyMemory(&FxSaveArea->U.FxArea,
+                          &Context->ExtendedRegisters[0],
+                          MAXIMUM_SUPPORTED_EXTENSION);
+
+            /* Remove reserved bits from MXCSR */
+            FxSaveArea->U.FxArea.MXCsr &= ~0xFFBF;
+
+            /* Mask out any invalid flags */
+            FxSaveArea->Cr0NpxState &= ~(CR0_EM | CR0_MP | CR0_TS);
+
+            /* FIXME: Check if this is a VDM app */
         }
     }

@@ -799,11 +826,58 @@
         /* Check if NPX is present */
         if (KeI386NpxPresent)
         {
-            /* Future use */
+            /* Flush the NPX State */
+            KiFlushNPXState(NULL);
+
+            /* Check if we have Fxsr support */
+            if (KeI386FxsrPresent)
+            {
+                /* Convert the Fn Floating Point state to Fx */
+                FxSaveArea->U.FxArea.ControlWord =
+                    (USHORT)Context->FloatSave.ControlWord;
+                FxSaveArea->U.FxArea.StatusWord =
+                    (USHORT)Context->FloatSave.StatusWord;
+                FxSaveArea->U.FxArea.TagWord =
+                    KiTagWordFnsaveToFxsave((USHORT)Context->FloatSave.TagWord);
+                FxSaveArea->U.FxArea.ErrorOpcode =
+                    (USHORT)(Context->FloatSave.ErrorSelector >> 16);
+                FxSaveArea->U.FxArea.ErrorOffset =
+                    Context->FloatSave.ErrorOffset;
+                FxSaveArea->U.FxArea.ErrorSelector =
+                    Context->FloatSave.ErrorSelector & 0xFFFF;
+                FxSaveArea->U.FxArea.DataOffset =
+                    Context->FloatSave.DataOffset;
+                FxSaveArea->U.FxArea.DataSelector =
+                    Context->FloatSave.DataSelector & 0xFFFF;
+
+                /* Clear out the Register Area */
+                RtlZeroMemory(&FxSaveArea->U.FxArea.RegisterArea[0], SIZE_OF_FX_REGISTERS);
+
+                /* Loop the 8 floating point registers */
+                for (i = 0; i < 8; i++)
+                {
+                    /* Copy from Fn to Fx */
+                    RtlCopyMemory(FxSaveArea->U.FxArea.RegisterArea + (i * 16),
+                                  Context->FloatSave.RegisterArea + (i * 10),
+                                  10);
+                }
+            }
+            else
+            {
+                /* Just dump the Fn state in */
+                RtlCopyMemory(&FxSaveArea->U.FnArea,
+                              &Context->FloatSave,
+                              sizeof(FNSAVE_FORMAT));
+            }
+
+            /* Mask out any invalid flags */
+            FxSaveArea->Cr0NpxState &= ~(CR0_EM | CR0_MP | CR0_TS);
+
+            /* FIXME: Check if this is a VDM app */
         }
         else
         {
-            /* Future use */
+            /* FIXME: Handle FPU Emulation */
         }
     }

@@ -826,9 +900,6 @@
                 (Context->Dr7 & DR7_ACTIVE);
         }
     }
-
-    /* Handle FPU and Extended Registers */
-    KiContextToFxSaveArea((PFX_SAVE_AREA)(TrapFrame + 1), Context);
 }

VOID
@@ -837,7 +908,13 @@
                      IN PKEXCEPTION_FRAME ExceptionFrame,
                      IN OUT PCONTEXT Context)
 {
-    PFX_SAVE_AREA FxSaveArea = NULL;
+    PFX_SAVE_AREA FxSaveArea;
+    struct _AlignHack
+    {
+        UCHAR Hack[15];
+        FLOATING_SAVE_AREA UnalignedArea;
+    } FloatSaveBuffer;
+    FLOATING_SAVE_AREA *FloatSaveArea;

/* Start with the Control flags */
     if ((Context->ContextFlags & CONTEXT_CONTROL) == CONTEXT_CONTROL)
@@ -920,19 +997,13 @@
         /* Make sure NPX is present */
         if (KeI386NpxPresent)
         {
-            /* Future use */
-        }
-
-        /* Old code */
-        FxSaveArea = KiGetFpuState(KeGetCurrentThread());
-        if (FxSaveArea != NULL)
-        {
-            memcpy(Context->ExtendedRegisters, &FxSaveArea->U.FxArea,
-                   min(sizeof (Context->ExtendedRegisters), sizeof (FxSaveArea->U.FxArea)) );
-        }
-        else
-        {
-            Context->ContextFlags &= (~CONTEXT_EXTENDED_REGISTERS) | CONTEXT_i386;
+            /* Flush the NPX State */
+            KiFlushNPXState(NULL);
+
+            /* Copy the registers */
+            RtlCopyMemory(&Context->ExtendedRegisters[0],
+                          &FxSaveArea->U.FxArea,
+                          MAXIMUM_SUPPORTED_EXTENSION);
         }
     }

@@ -946,24 +1017,34 @@

/* Make sure we have an NPX */
         if (KeI386NpxPresent)
-        {
-            /* Future use */
-        }
-        else
-        {
-            /* Future Use */
-        }
-
-        /* Old code */
-        FxSaveArea = KiGetFpuState(KeGetCurrentThread());
-        if (FxSaveArea != NULL)
-        {
-            KiFxSaveAreaToFloatingSaveArea(&Context->FloatSave, FxSaveArea);
-        }
-        else
-        {
-            Context->ContextFlags &= (~CONTEXT_FLOATING_POINT) | CONTEXT_i386;
-        }
+         {
+            /* Check if we have Fxsr support */
+            if (KeI386FxsrPresent)
+            {
+                /* Align the floating area to 16-bytes */
+                FloatSaveArea = (FLOATING_SAVE_AREA*)
+                                ((ULONG_PTR)&FloatSaveBuffer.UnalignedArea &~ 0xF);
+
+                /* Get the State */
+                KiFlushNPXState(FloatSaveArea);
+            }
+            else
+            {
+                /* We don't, use the FN area and flush the NPX State */
+                FloatSaveArea = (FLOATING_SAVE_AREA*)&FxSaveArea->U.FnArea;
+                KiFlushNPXState(NULL);
+            }
+
+            /* Copy into the Context */
+            RtlCopyMemory(&Context->FloatSave,
+                          &FxSaveArea->U.FnArea,
+                          sizeof(FNSAVE_FORMAT));
+         }
+         else
+         {
+            /* FIXME: Handle Emulation */
+             Context->ContextFlags &= (~CONTEXT_FLOATING_POINT) | CONTEXT_i386;
+         }
     }

/* Handle debug registers */

Modified: trunk/reactos/ntoskrnl/ke/i386/fpu.c
URL: http://svn.reactos.org/svn/reactos/trunk/reactos/ntoskrnl/ke/i386/fpu.c?rev=...
==============================================================================
--- trunk/reactos/ntoskrnl/ke/i386/fpu.c (original)
+++ trunk/reactos/ntoskrnl/ke/i386/fpu.c Mon Aug 21 03:53:07 2006
@@ -14,462 +14,7 @@
 #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 *******************************************************************/
-
-extern ULONG KeI386NpxPresent;
-extern ULONG KeI386XMMIPresent;
-extern ULONG KeI386FxsrPresent;
-
-static ULONG MxcsrFeatureMask = 0;
-
 /* FUNCTIONS *****************************************************************/
-
-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, 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 (KeI386XMMIPresent)
-        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, 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, FLOATING_SAVE_AREA *FloatingSaveArea)
-{
-    if (KeI386FxsrPresent)
-    {
-        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 (KeI386FxsrPresent)
-    {
-        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 (KeI386FxsrPresent)
-        {
-            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;
-    ULONG Flags;
-    PKPRCB Prcb = KeGetCurrentPrcb();
-
-    Ke386SaveFlags(Flags);
-    Ke386DisableInterrupts();
-
-    KeI386NpxPresent = 0;
-    KeI386FxsrPresent = 0;
-    KeI386XMMIPresent = 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
-
-    KeI386NpxPresent = 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 */
-        KeI386FxsrPresent = 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 */
-        KeI386XMMIPresent = 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_LOADED)
-    {
-        FxSaveArea = (PFX_SAVE_AREA)((ULONG_PTR)Thread->InitialStack - sizeof (FX_SAVE_AREA));
-        if (Thread->NpxState & NPX_STATE_NOT_LOADED)
-        {
-            ASSERT(KeGetCurrentPrcb()->NpxThread == Thread);
-
-            Cr0 = Ke386GetCr0();
-            asm volatile("clts");
-            if (KeI386FxsrPresent)
-                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_LOADED;
-        }
-    }
-    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;
-
-        (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();
-        ASSERT(CurrentThread != NULL);
-
-        CurrentThread->NpxState |= NPX_STATE_NOT_LOADED;
-        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->SegCs & 0xffff) == (KGDT_R3_CODE | RPL_MASK)) ? (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 */

Modified: trunk/reactos/ntoskrnl/ke/i386/kernel.c
URL: http://svn.reactos.org/svn/reactos/trunk/reactos/ntoskrnl/ke/i386/kernel.c?r...
==============================================================================
--- trunk/reactos/ntoskrnl/ke/i386/kernel.c (original)
+++ trunk/reactos/ntoskrnl/ke/i386/kernel.c Mon Aug 21 03:53:07 2006
@@ -31,6 +31,7 @@
 BOOLEAN Ke386GlobalPagesEnabled = FALSE;
 ULONG KiFastSystemCallDisable = 1;
 ULONG KeI386NpxPresent = 0;
+ULONG MxcsrFeatureMask = 0;
 ULONG KeI386XMMIPresent = 0;
 ULONG KeI386FxsrPresent = 0;
 extern PVOID Ki386InitialStackArray[MAXIMUM_PROCESSORS];
@@ -47,7 +48,46 @@
 #pragma alloc_text(INIT, Ki386SetProcessorFeatures)
 #endif

+BOOLEAN
+NTAPI
+KiIsNpxPresent(
+    VOID
+);
+
 /* FUNCTIONS *****************************************************************/
+
+VOID
+INIT_FUNCTION
+NTAPI
+KiCheckFPU(VOID)
+{
+    PKPRCB Prcb = KeGetCurrentPrcb();
+
+    /* Check for a math co-processor (NPX) */
+    KeI386NpxPresent = KiIsNpxPresent();
+
+    /* Check for and enable MMX/SSE support if possible */
+    KeI386FxsrPresent = Prcb->FeatureBits & X86_FEATURE_FXSR ? TRUE : FALSE;
+
+    /* Check for SSE (2 and 3 should have this set too) */
+    KeI386XMMIPresent = Prcb->FeatureBits & X86_FEATURE_SSE ? TRUE : FALSE;
+
+    /* Check if Fxsr was found */
+    if (KeI386FxsrPresent)
+    {
+        /* Enable it. FIXME: Send an IPI */
+        Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSFXSR);
+
+        /* Check if XMM was found too */
+        if (KeI386XMMIPresent)
+        {
+            /* Enable it: FIXME: Send an IPI. */
+            Ke386SetCr4(Ke386GetCr4() | X86_CR4_OSXMMEXCPT);
+
+            /* FIXME: Implement and enable XMM Page Zeroing for Mm */
+        }
+    }
+}

static VOID INIT_FUNCTION
 Ki386GetCpuId(VOID)

Modified: trunk/reactos/ntoskrnl/ke/i386/trap.s
URL: http://svn.reactos.org/svn/reactos/trunk/reactos/ntoskrnl/ke/i386/trap.s?rev...
==============================================================================
--- trunk/reactos/ntoskrnl/ke/i386/trap.s (original)
+++ trunk/reactos/ntoskrnl/ke/i386/trap.s Mon Aug 21 03:53:07 2006
@@ -819,6 +819,7 @@
     jne _Kei386EoiHelper@0
     jmp _KiV86Complete

+.func KiTrap7
 _KiTrap7:
     /* Push error code */
     push 0
@@ -826,18 +827,134 @@
     /* Enter trap */
     TRAP_PROLOG(7)

-    /* Call the C exception handler */
-    push 7
-    push ebp
-    call _KiTrapHandler
-    add esp, 8
-
-    /* Check for v86 recovery */
-    cmp eax, 1
-
-    /* Return to caller */
-    jne _Kei386EoiHelper@0
-    jmp _KiV86Complete
+    /* Get the current thread and stack */
+StartTrapHandle:
+    mov eax, [fs:KPCR_CURRENT_THREAD]
+    mov ecx, [eax+KTHREAD_INITIAL_STACK]
+    sub ecx, NPX_FRAME_LENGTH
+
+    /* Check if emulation is enabled */
+    test dword ptr [ecx+FN_CR0_NPX_STATE], CR0_EM
+    jnz EmulationEnabled
+
+CheckState:
+    /* Check if the NPX state is loaded */
+    cmp byte ptr [eax+KTHREAD_NPX_STATE], NPX_STATE_LOADED
+    mov ebx, cr0
+    jz IsLoaded
+
+    /* Remove flags */
+    and ebx, ~(CR0_MP + CR0_TS + CR0_EM)
+    mov cr0, ebx
+
+    /* Check the NPX thread */
+    mov edx, [fs:KPCR_NPX_THREAD]
+    or edx, edx
+    jz NoNpxThread
+
+    /* Get the NPX Stack */
+    mov esi, [edx+KTHREAD_INITIAL_STACK]
+    sub esi, NPX_FRAME_LENGTH
+
+    /* Check if we have FXSR and check which operand to use */
+    test byte ptr _KeI386FxsrPresent, 1
+    jz FnSave
+    fxsave [esi]
+    jmp AfterSave
+
+FnSave:
+    fnsave [esi]
+
+AfterSave:
+    /* Set the thread's state to dirty */
+    mov byte ptr [edx+KTHREAD_NPX_STATE], NPX_STATE_NOT_LOADED
+
+NoNpxThread:
+    /* Check if we have FXSR and choose which operand to use */
+    test byte ptr _KeI386FxsrPresent, 1
+    jz FrRestore
+    fxrstor [ecx]
+    jmp AfterRestore
+
+FrRestore:
+    frstor [esi]
+
+AfterRestore:
+    /* Set state loaded */
+    mov byte ptr [eax+KTHREAD_NPX_STATE], NPX_STATE_LOADED
+    mov [fs:KPCR_NPX_THREAD], eax
+
+    /* Enable interrupts to happen now */
+    sti
+    nop
+
+    /* Check if CR0 needs to be reloaded due to a context switch */
+    cmp dword ptr [ecx+FN_CR0_NPX_STATE], 0
+    jz _Kei386EoiHelper@0
+
+    /* We have to reload CR0... disable interrupts */
+    cli
+
+    /* Get CR0 and update it */
+    mov ebx, cr0
+    or ebx, [ecx+FN_CR0_NPX_STATE]
+    mov cr0, ebx
+
+    /* Restore interrupts and check if TS is back on */
+    sti
+    test bl, CR0_TS
+    jz _Kei386EoiHelper@0
+
+    /* Clear TS, and loop handling again */
+    clts
+    cli
+    jmp StartTrapHandle
+
+IsLoaded:
+    /* Check if TS is set */
+    test bl, CR0_TS
+    jnz TsSetOnLoadedState
+
+    /* Check if the trap came from user-mode */
+    int 3
+
+EmulationEnabled:
+    /* Did this come from kernel-mode? */
+    cmp word ptr [ebp+KTRAP_FRAME_CS], KGDT_R0_CODE
+    jz CheckState
+
+    /* It came from user-mode, so this would only be valid inside a VDM */
+    /* Since we don't actually have VDMs in ROS, bugcheck. */
+    jmp BogusTrap2
+
+TsSetOnLoadedState:
+    /* TS shouldn't be set, unless this we don't have a Math Processor */
+    test bl, CR0_MP
+    jnz BogusTrap
+
+    /* Strange that we got a trap at all, but ignore and continue */
+    clts
+    jmp _Kei386EoiHelper@0
+
+BogusTrap2:
+    /* Cause a bugcheck */
+    sti
+    push 0
+    push 0
+    push eax
+    push 1
+    push TRAP_CAUSE_UNKNOWN
+    call _KeBugCheckEx@20
+
+BogusTrap:
+    /* Cause a bugcheck */
+    push 0
+    push 0
+    push ebx
+    push 2
+    push TRAP_CAUSE_UNKNOWN
+    call _KeBugCheckEx@20
+.endfunc

.globl _KiTrap8
 _KiTrap8:
@@ -864,6 +981,7 @@
     jne _Kei386EoiHelper@0
     jmp _KiV86Complete

+#if 1
 _KiTrap10:
     /* Enter trap */
     TRAP_PROLOG(10)
@@ -880,6 +998,7 @@
     /* Return to caller */
     jne _Kei386EoiHelper@0
     jmp _KiV86Complete
+#endif

_KiTrap11:
     /* Enter trap */
@@ -915,6 +1034,7 @@
     jne _Kei386EoiHelper@0
     jmp _KiV86Complete

+#if 1
 _KiTrap13:
     /* Enter trap */
     TRAP_PROLOG(13)
@@ -931,6 +1051,7 @@
     /* Return to caller */
     jne _Kei386EoiHelper@0
     jmp _KiV86Complete
+#endif

_KiTrap14:
     /* Enter trap */