[ros-diffs] [sir_richard] 45014: Trap handlers in C patch 6 of X: [NTOS]: Implement Trap 7 and 16 in C. These are the last two FPU/NPX traps. Not really good with x86 FPU stuff, so there might be some mistakes to look over later. [NTOS]: Fixed a bug in Trap 19 handler (forgot to write CR0 back).

Author: sir_richard
Date: Sat Jan  9 16:07:44 2010
New Revision: 45014

URL: http://svn.reactos.org/svn/reactos?rev=45014&view=rev
Log:
Trap handlers in C patch 6 of X:
    [NTOS]: Implement Trap 7 and 16 in C. These are the last two FPU/NPX traps. Not really
good with x86 FPU stuff, so there might be some mistakes to look over later.
    [NTOS]: Fixed a bug in Trap 19 handler (forgot to write CR0 back).

Modified:
    trunk/reactos/ntoskrnl/include/internal/i386/intrin_i.h
    trunk/reactos/ntoskrnl/include/internal/trap_x.h
    trunk/reactos/ntoskrnl/ke/i386/trap.s
    trunk/reactos/ntoskrnl/ke/i386/traphdlr.c

Modified: trunk/reactos/ntoskrnl/include/internal/i386/intrin_i.h
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/ntoskrnl/include/internal/…
==============================================================================
--- trunk/reactos/ntoskrnl/include/internal/i386/intrin_i.h [iso-8859-1] (original)
+++ trunk/reactos/ntoskrnl/include/internal/i386/intrin_i.h [iso-8859-1] Sat Jan  9
16:07:44 2010
@@ -27,7 +27,21 @@
     {
         __asm__ __volatile__ ("fnsave %0\n wait\n" : :
"m"(SaveArea));
     }
-
+}
+
+FORCEINLINE
+VOID
+Ke386LoadFpuState(IN PFX_SAVE_AREA SaveArea)
+{
+    extern ULONG KeI386FxsrPresent;
+    if (KeI386FxsrPresent)
+    {
+        __asm__ __volatile__ ("fxrstor %0\n" : "=m"(SaveArea) : );
+    }
+    else
+    {
+        __asm__ __volatile__ (".globl _FrRestore\n _FrRestore: \n frstor %0\n
wait\n" : "=m"(SaveArea) : );
+    }
 }
 
 FORCEINLINE

Modified: trunk/reactos/ntoskrnl/include/internal/trap_x.h
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/ntoskrnl/include/internal/…
==============================================================================
--- trunk/reactos/ntoskrnl/include/internal/trap_x.h [iso-8859-1] (original)
+++ trunk/reactos/ntoskrnl/include/internal/trap_x.h [iso-8859-1] Sat Jan  9 16:07:44
2010
@@ -137,6 +137,13 @@
             ((KiUserTrap(TrapFrame)) &&
(PsGetCurrentProcess()->VdmObjects)));
 }
 
+PFX_SAVE_AREA
+FORCEINLINE
+KiGetThreadNpxArea(IN PKTHREAD Thread)
+{
+    return (PFX_SAVE_AREA)((ULONG_PTR)Thread->InitialStack - sizeof(FX_SAVE_AREA));
+}
+
 VOID
 FORCEINLINE
 KiTrapFrameFromPushaStack(IN PKTRAP_FRAME TrapFrame)

Modified: trunk/reactos/ntoskrnl/ke/i386/trap.s
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/ntoskrnl/ke/i386/trap.s?re…
==============================================================================
--- trunk/reactos/ntoskrnl/ke/i386/trap.s [iso-8859-1] (original)
+++ trunk/reactos/ntoskrnl/ke/i386/trap.s [iso-8859-1] Sat Jan  9 16:07:44 2010
@@ -693,317 +693,7 @@
 GENERATE_TRAP_HANDLER KiTrap4, 1
 GENERATE_TRAP_HANDLER KiTrap5, 1
 GENERATE_TRAP_HANDLER KiTrap6, 1
-
-.func KiTrap7
-TRAP_FIXUPS kit7_a, kit7_t, DoFixupV86, DoNotFixupAbios
-_KiTrap7:
-    /* Push error code */
-    push 0
-
-    /* Enter trap */
-    TRAP_PROLOG kit7_a, kit7_t
-
-    /* Get the current thread and stack */
-StartTrapHandle:
-    mov eax, PCR[KPCR_CURRENT_THREAD]
-    mov ecx, [eax+KTHREAD_INITIAL_STACK]
-    sub ecx, NPX_FRAME_LENGTH
-
-    /* Check if emulation is enabled */
-    test byte 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, PCR[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 [ecx]
-
-AfterRestore:
-    /* Set state loaded */
-    mov byte ptr [eax+KTHREAD_NPX_STATE], NPX_STATE_LOADED
-    mov PCR[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
-
-KernelNpx:
-
-    /* Set delayed error */
-    or dword ptr [ecx+FN_CR0_NPX_STATE], CR0_TS
-
-    /* Check if this happened during restore */
-    cmp dword ptr [ebp+KTRAP_FRAME_EIP], offset FrRestore
-    jnz UserNpx
-
-    /* Skip instruction and dispatch the exception */
-    add dword ptr [ebp+KTRAP_FRAME_EIP], 3
-    jmp _Kei386EoiHelper@0
-
-IsLoaded:
-    /* Check if TS is set */
-    test bl, CR0_TS
-    jnz TsSetOnLoadedState
-
-HandleNpxFault:
-    /* Check if the trap came from V86 mode */
-    test dword ptr [ebp+KTRAP_FRAME_EFLAGS], EFLAGS_V86_MASK
-    jnz V86Npx
-
-    /* Check if it came from kernel mode */
-    test byte ptr [ebp+KTRAP_FRAME_CS], MODE_MASK
-    jz KernelNpx
-
-    /* Check if it came from a VDM */
-    cmp word ptr [ebp+KTRAP_FRAME_CS], KGDT_R3_CODE + RPL_MASK
-    jne V86Npx
-
-UserNpx:
-    /* Get the current thread */
-    mov eax, PCR[KPCR_CURRENT_THREAD]
-
-    /* Check NPX state */
-    cmp byte ptr [eax+KTHREAD_NPX_STATE], NPX_STATE_NOT_LOADED
-
-    /* Get the NPX save area */
-    mov ecx, [eax+KTHREAD_INITIAL_STACK]
-    lea ecx, [ecx-NPX_FRAME_LENGTH]
-    jz NoSaveRestore
-
-HandleUserNpx:
-
-    /* Set new CR0 */
-    mov ebx, cr0
-    and ebx, ~(CR0_MP + CR0_EM + CR0_TS)
-    mov cr0, ebx
-
-    /* Check if we have FX support */
-    test byte ptr _KeI386FxsrPresent, 1
-    jz FnSave2
-
-    /* Save the state */
-    fxsave [ecx]
-    jmp MakeCr0Dirty
-FnSave2:
-    fnsave [ecx]
-    wait
-
-MakeCr0Dirty:
-    /* Make CR0 state not loaded */
-    or ebx, NPX_STATE_NOT_LOADED
-    or ebx, [ecx+FN_CR0_NPX_STATE]
-    mov cr0, ebx
-
-    /* Update NPX state */
-    mov byte ptr [eax+KTHREAD_NPX_STATE], NPX_STATE_NOT_LOADED
-    mov dword ptr PCR[KPCR_NPX_THREAD], 0
-
-NoSaveRestore:
-    /* Clear the TS bit and re-enable interrupts */
-    and dword ptr [ecx+FN_CR0_NPX_STATE], ~CR0_TS
-    sti
-
-    /* Check if we have FX support */
-    test byte ptr _KeI386FxsrPresent, 1
-    jz FnError
-
-    /* Get error offset, control and status words */
-    mov ebx, [ecx+FX_ERROR_OFFSET]
-    movzx eax, word ptr [ecx+FX_CONTROL_WORD]
-    movzx edx, word ptr [ecx+FX_STATUS_WORD]
-
-    /* Get the faulting opcode */
-    mov esi, [ecx+FX_DATA_OFFSET]
-    jmp CheckError
-
-FnError:
-    /* Get error offset, control and status words */
-    mov ebx, [ecx+FP_ERROR_OFFSET]
-    movzx eax, word ptr [ecx+FP_CONTROL_WORD]
-    movzx edx, word ptr [ecx+FP_STATUS_WORD]
-
-    /* Get the faulting opcode */
-    mov esi, [ecx+FP_DATA_OFFSET]
-
-CheckError:
-    /* Mask exceptions */
-    and eax, 0x3F
-    not eax
-    and eax, edx
-
-    /* Check if what's left is invalid */
-    test al, 1
-    jz ValidNpxOpcode
-
-    /* Check if it was a stack fault */
-    test al, 64
-    jnz InvalidStack
-
-    /* Raise exception */
-    mov eax, STATUS_FLOAT_INVALID_OPERATION
-    jmp _DispatchOneParamZero
-
-InvalidStack:
-
-    /* Raise exception */
-    mov eax, STATUS_FLOAT_STACK_CHECK
-    jmp _DispatchTwoParamZero
-
-ValidNpxOpcode:
-
-    /* Check for divide by 0 */
-    test al, 4
-    jz 1f
-
-    /* Raise exception */
-    mov eax, STATUS_FLOAT_DIVIDE_BY_ZERO
-    jmp _DispatchOneParamZero
-
-1:
-    /* Check for denormal */
-    test al, 2
-    jz 1f
-
-    /* Raise exception */
-    mov eax, STATUS_FLOAT_INVALID_OPERATION
-    jmp _DispatchOneParamZero
-
-1:
-    /* Check for overflow */
-    test al, 8
-    jz 1f
-
-    /* Raise exception */
-    mov eax, STATUS_FLOAT_OVERFLOW
-    jmp _DispatchOneParamZero
-
-1:
-    /* Check for underflow */
-    test al, 16
-    jz 1f
-
-    /* Raise exception */
-    mov eax, STATUS_FLOAT_UNDERFLOW
-    jmp _DispatchOneParamZero
-
-1:
-    /* Check for precision fault */
-    test al, 32
-    jz UnexpectedNpx
-
-    /* Raise exception */
-    mov eax, STATUS_FLOAT_INEXACT_RESULT
-    jmp _DispatchOneParamZero
-
-UnexpectedNpx:
-
-    /* Strange result, bugcheck the OS */
-    sti
-    push ebp
-    push 0
-    push 0
-    push eax
-    push 1
-    push TRAP_CAUSE_UNKNOWN
-    call _KeBugCheckWithTf@24
-
-V86Npx:
-    /* Check if this is a VDM */
-    mov eax, PCR[KPCR_CURRENT_THREAD]
-    mov ebx, [eax+KTHREAD_APCSTATE_PROCESS]
-    cmp dword ptr [ebx+EPROCESS_VDM_OBJECTS], 0
-    jz HandleUserNpx
-
-    /* V86 NPX not handled */
-    UNHANDLED_V86_PATH
-
-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 UnexpectedNpx
-
-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
-
-BogusTrap:
-    /* Cause a bugcheck */
-    push 0
-    push 0
-    push ebx
-    push 2
-    push TRAP_CAUSE_UNKNOWN
-    call _KeBugCheckEx@20
-.endfunc
-
+GENERATE_TRAP_HANDLER KiTrap7, 1
 GENERATE_TRAP_HANDLER KiTrap8, 0
 GENERATE_TRAP_HANDLER KiTrap9, 1
 GENERATE_TRAP_HANDLER KiTrap10, 0
@@ -1496,33 +1186,7 @@
 
 GENERATE_TRAP_HANDLER KiTrap14, 0
 GENERATE_TRAP_HANDLER KiTrap0F, 1
-
-.func KiTrap16
-TRAP_FIXUPS kit10_a, kit10_t, DoFixupV86, DoNotFixupAbios
-_KiTrap16:
-    /* Push error code */
-    push 0
-
-    /* Enter trap */
-    TRAP_PROLOG kit10_a, kit10_t
-
-    /* Check if this is the NPX Thread */
-    mov eax, PCR[KPCR_CURRENT_THREAD]
-    cmp eax, PCR[KPCR_NPX_THREAD]
-
-    /* Get the initial stack and NPX frame */
-    mov ecx, [eax+KTHREAD_INITIAL_STACK]
-    lea ecx, [ecx-NPX_FRAME_LENGTH]
-
-    /* If this is a valid fault, handle it */
-    jz HandleNpxFault
-
-    /* Otherwise, re-enable interrupts and set delayed error */
-    sti
-    or dword ptr [ecx+FN_CR0_NPX_STATE], CR0_TS
-    jmp _Kei386EoiHelper@0
-.endfunc
-
+GENERATE_TRAP_HANDLER KiTrap16, 1
 GENERATE_TRAP_HANDLER KiTrap17, 1
 GENERATE_TRAP_HANDLER KiTrap19, 1
 

Modified: trunk/reactos/ntoskrnl/ke/i386/traphdlr.c
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/ntoskrnl/ke/i386/traphdlr.…
==============================================================================
--- trunk/reactos/ntoskrnl/ke/i386/traphdlr.c [iso-8859-1] (original)
+++ trunk/reactos/ntoskrnl/ke/i386/traphdlr.c [iso-8859-1] Sat Jan  9 16:07:44 2010
@@ -249,6 +249,163 @@
                                      Parameter2,
                                      Parameter3,
                                      TrapFrame); 
+}
+
+VOID
+FASTCALL
+KiNpxHandler(IN PKTRAP_FRAME TrapFrame,
+             IN PKTHREAD Thread,
+             IN PFX_SAVE_AREA SaveArea)
+{
+    ULONG Cr0, Mask, Error, ErrorOffset, DataOffset;
+    extern VOID FrRestore(VOID);
+    
+    /* Check for VDM trap */
+    ASSERT((KiVdmTrap(TrapFrame)) == FALSE);
+
+    /* Check for kernel trap */
+    if (!KiUserTrap(TrapFrame))
+    {
+        /* Kernel might've tripped a delayed error */
+        SaveArea->Cr0NpxState |= CR0_TS;
+        
+        /* Only valid if it happened during a restore */
+        if ((PVOID)TrapFrame->Eip == FrRestore)
+        {
+            /* It did, so just skip the instruction */
+            TrapFrame->Eip += 3; /* sizeof(FRSTOR) */
+            KiEoiHelper(TrapFrame);
+        }
+    }
+
+    /* User or kernel trap -- get ready to issue an exception */
+    if (Thread->NpxState == NPX_STATE_NOT_LOADED)
+    {
+        /* Update CR0 */
+        Cr0 = __readcr0();
+        Cr0 &= ~(CR0_MP | CR0_EM | CR0_TS);
+        __writecr0(Cr0);
+
+        /* Save FPU state */
+        Ke386SaveFpuState(SaveArea);
+
+        /* Mark CR0 state dirty */
+        Cr0 |= NPX_STATE_NOT_LOADED;
+        Cr0 |= SaveArea->Cr0NpxState;
+        __writecr0(Cr0);
+
+        /* Update NPX state */
+        Thread->NpxState = NPX_STATE_NOT_LOADED;
+        KeGetCurrentPrcb()->NpxThread = NULL;
+    }
+
+    /* Clear the TS bit and re-enable interrupts */
+    SaveArea->Cr0NpxState &= ~CR0_TS;
+    _enable();
+    
+    /* Check if we should get the FN or FX error */
+    if (KeI386FxsrPresent)
+    {
+        /* Get it from FX */
+        Mask = SaveArea->U.FxArea.ControlWord;
+        Error = SaveArea->U.FxArea.StatusWord;
+        
+        /* Get the FPU exception address too */
+        ErrorOffset = SaveArea->U.FxArea.ErrorOffset;
+        DataOffset = SaveArea->U.FxArea.DataOffset;
+    }
+    else
+    {
+        /* Get it from FN */
+        Mask = SaveArea->U.FnArea.ControlWord;
+        Error = SaveArea->U.FnArea.StatusWord;
+        
+        /* Get the FPU exception address too */
+        ErrorOffset = SaveArea->U.FnArea.ErrorOffset;
+        DataOffset = SaveArea->U.FnArea.DataOffset;
+    }
+
+    /* Get legal exceptions that software should handle */
+    Error &= (FSW_INVALID_OPERATION |
+              FSW_DENORMAL |
+              FSW_ZERO_DIVIDE |
+              FSW_OVERFLOW |
+              FSW_UNDERFLOW |
+              FSW_PRECISION);
+    Error &= ~Mask;
+    
+    if (Error & FSW_STACK_FAULT)
+    {
+        /* Issue stack check fault */
+        KiDispatchException2Args(STATUS_FLOAT_STACK_CHECK,
+                                 ErrorOffset,
+                                 0,
+                                 DataOffset,
+                                 TrapFrame);
+    }
+    
+    /* Check for invalid operation */
+    if (Error & FSW_INVALID_OPERATION)
+    {
+        /* Issue fault */
+        KiDispatchException1Args(STATUS_FLOAT_INVALID_OPERATION,
+                                 ErrorOffset,
+                                 0,
+                                 TrapFrame);
+    }
+    
+    /* Check for divide by zero */
+    if (Error & FSW_ZERO_DIVIDE)
+    {
+        /* Issue fault */
+        KiDispatchException1Args(STATUS_FLOAT_DIVIDE_BY_ZERO,
+                                 ErrorOffset,
+                                 0,
+                                 TrapFrame);
+    }
+    
+    /* Check for denormal */
+    if (Error & FSW_DENORMAL)
+    {
+        /* Issue fault */
+        KiDispatchException1Args(STATUS_FLOAT_INVALID_OPERATION,
+                                 ErrorOffset,
+                                 0,
+                                 TrapFrame);
+    }
+    
+    /* Check for overflow */
+    if (Error & FSW_OVERFLOW)
+    {
+        /* Issue fault */
+        KiDispatchException1Args(STATUS_FLOAT_OVERFLOW,
+                                 ErrorOffset,
+                                 0,
+                                 TrapFrame);
+    }
+    
+    /* Check for underflow */
+    if (Error & FSW_UNDERFLOW)
+    {
+        /* Issue fault */
+        KiDispatchException1Args(STATUS_FLOAT_UNDERFLOW,
+                                 ErrorOffset,
+                                 0,
+                                 TrapFrame);
+    }
+
+    /* Check for precision fault */
+    if (Error & FSW_PRECISION)
+    {
+        /* Issue fault */
+        KiDispatchException1Args(STATUS_FLOAT_INEXACT_RESULT,
+                                 ErrorOffset,
+                                 0,
+                                 TrapFrame);
+    }
+    
+    /* Unknown FPU fault */
+    KeBugCheckWithTf(TRAP_CAUSE_UNKNOWN, 1, Error, 0, 0, TrapFrame);
 }
 
 VOID
@@ -385,6 +542,115 @@
                              TrapFrame->Eip,
                              TrapFrame);
     
+}
+
+VOID
+FASTCALL
+KiTrap7Handler(IN PKTRAP_FRAME TrapFrame)
+{
+    PKTHREAD Thread, NpxThread;
+    PFX_SAVE_AREA SaveArea, NpxSaveArea;
+    ULONG Cr0;
+    
+    /* Save trap frame */
+    KiEnterTrap(TrapFrame);
+
+    /* Try to handle NPX delay load */
+    while (TRUE)
+    {
+        /* Get the current thread */
+        Thread = KeGetCurrentThread();
+
+        /* Get the NPX frame */
+        SaveArea = KiGetThreadNpxArea(Thread);
+
+        /* Check if emulation is enabled */
+        if (SaveArea->Cr0NpxState & CR0_EM)
+        {
+            /* Not implemented */
+            UNIMPLEMENTED;
+            while (TRUE);
+        }
+    
+        /* Save CR0 and check NPX state */
+        Cr0 = __readcr0();
+        if (Thread->NpxState != NPX_STATE_LOADED)
+        {
+            /* Update CR0 */
+            Cr0 &= ~(CR0_MP | CR0_EM | CR0_TS);
+            __writecr0(Cr0);
+        
+            /* Get the NPX thread */
+            NpxThread = KeGetCurrentPrcb()->NpxThread;
+            if (NpxThread)
+            {
+                /* Get the NPX frame */
+                NpxSaveArea = KiGetThreadNpxArea(NpxThread);
+                
+                /* Save FPU state */
+                Ke386SaveFpuState(NpxSaveArea);
+
+                /* Update NPX state */
+                Thread->NpxState = NPX_STATE_NOT_LOADED;
+           }
+       
+            /* Load FPU state */
+            Ke386LoadFpuState(SaveArea);
+        
+            /* Update NPX state */
+            Thread->NpxState = NPX_STATE_LOADED;
+            KeGetCurrentPrcb()->NpxThread = Thread;
+        
+            /* Enable interrupts */
+            _enable();
+        
+            /* Check if CR0 needs to be reloaded due to context switch */
+            if (!SaveArea->Cr0NpxState) KiEoiHelper(TrapFrame);
+        
+            /* Otherwise, we need to reload CR0, disable interrupts */
+            _disable();
+        
+            /* Reload CR0 */
+            Cr0 = __readcr0();
+            Cr0 |= SaveArea->Cr0NpxState;
+            __writecr0(Cr0);
+        
+            /* Now restore interrupts and check for TS */
+            _enable();
+            if (Cr0 & CR0_TS) KiEoiHelper(TrapFrame);
+        
+            /* We're still here -- clear TS and try again */
+            __writecr0(__readcr0() &~ CR0_TS);
+            _disable();
+        }
+        else
+        {
+            /* This is an actual fault, not a lack of FPU state */
+            break;
+        }
+    }
+    
+    /* TS should not be set */
+    if (Cr0 & CR0_TS)
+    {
+        /*
+         * If it's incorrectly set, then maybe the state is actually still valid
+         * but we could've lock track of that due to a BIOS call.
+         * Make sure MP is still set, which should verify the theory.
+         */
+        if (Cr0 & CR0_MP)
+        {
+            /* Indeed, the state is actually still valid, so clear TS */
+            __writecr0(__readcr0() &~ CR0_TS);
+            KiEoiHelper(TrapFrame);
+        }
+        
+        /* Otherwise, something strange is going on */
+        KeBugCheckWithTf(TRAP_CAUSE_UNKNOWN, 2, Cr0, 0, 0, TrapFrame);
+    }
+    
+    /* It's not a delayed load, so process this trap as an NPX fault */
+    KiNpxHandler(TrapFrame, Thread, SaveArea);
 }
 
 VOID
@@ -581,6 +847,34 @@
     KiSystemFatalException(EXCEPTION_RESERVED_TRAP, TrapFrame);
 }
 
+
+VOID
+FASTCALL
+KiTrap16Handler(IN PKTRAP_FRAME TrapFrame)
+{
+    PKTHREAD Thread;
+    PFX_SAVE_AREA SaveArea;
+    
+    /* Save trap frame */
+    KiEnterTrap(TrapFrame);
+
+    /* Check if this is the NPX thrad */
+    Thread = KeGetCurrentThread();
+    SaveArea = KiGetThreadNpxArea(Thread);
+    if (Thread != KeGetCurrentPrcb()->NpxThread)
+    {
+        /* It isn't, enable interrupts and set delayed error */
+        _enable();
+        SaveArea->Cr0NpxState |= CR0_TS;
+        
+        /* End trap */
+        KiEoiHelper(TrapFrame);
+    }
+    
+    /* Otherwise, proceed with NPX fault handling */
+    KiNpxHandler(TrapFrame, Thread, SaveArea);
+}
+
 VOID
 FASTCALL
 KiTrap17Handler(IN PKTRAP_FRAME TrapFrame)
@@ -613,7 +907,7 @@
     }
 
     /* Get the NPX frame */
-    SaveArea = (PFX_SAVE_AREA)((ULONG_PTR)Thread->InitialStack -
sizeof(FX_SAVE_AREA));
+    SaveArea = KiGetThreadNpxArea(Thread);
 
     /* Check for VDM trap */
     ASSERT((KiVdmTrap(TrapFrame)) == FALSE);
@@ -636,6 +930,7 @@
     /* Mark CR0 state dirty */
     Cr0 |= NPX_STATE_NOT_LOADED;
     Cr0 |= SaveArea->Cr0NpxState;
+     __writecr0(Cr0);
     
     /* Update NPX state */
     Thread->NpxState = NPX_STATE_NOT_LOADED;