- Fix KiDispatchException to unmask KI_EXCEPTION_INTERNAL when setting the exception...
[reactos.git] / reactos / ntoskrnl / ke / i386 / exp.c
index 56b0bb0..500a1db 100644 (file)
 #define NDEBUG
 #include <debug.h>
 
+/* GLOBALS *******************************************************************/
+
+/* DR Registers in the CONTEXT structure */
+UCHAR KiDebugRegisterContextOffsets[9] =
+{
+    FIELD_OFFSET(CONTEXT, Dr0),
+    FIELD_OFFSET(CONTEXT, Dr1),
+    FIELD_OFFSET(CONTEXT, Dr2),
+    FIELD_OFFSET(CONTEXT, Dr3),
+    0,
+    0,
+    FIELD_OFFSET(CONTEXT, Dr6),
+    FIELD_OFFSET(CONTEXT, Dr7),
+    0,
+};
+
+/* DR Registers in the KTRAP_FRAME structure */
+UCHAR KiDebugRegisterTrapOffsets[9] =
+{
+    FIELD_OFFSET(KTRAP_FRAME, Dr0),
+    FIELD_OFFSET(KTRAP_FRAME, Dr1),
+    FIELD_OFFSET(KTRAP_FRAME, Dr2),
+    FIELD_OFFSET(KTRAP_FRAME, Dr3),
+    0,
+    0,
+    FIELD_OFFSET(KTRAP_FRAME, Dr6),
+    FIELD_OFFSET(KTRAP_FRAME, Dr7),
+    0,
+};
+
 /* FUNCTIONS *****************************************************************/
 
+_SEH_DEFINE_LOCALS(KiCopyInfo)
+{
+    volatile EXCEPTION_RECORD SehExceptRecord;
+};
+
+_SEH_FILTER(KiCopyInformation)
+{
+    _SEH_ACCESS_LOCALS(KiCopyInfo);
+
+    /* Copy the exception records and return to the handler */
+    RtlCopyMemory((PVOID)&_SEH_VAR(SehExceptRecord),
+                  _SEH_GetExceptionPointers()->ExceptionRecord,
+                  sizeof(EXCEPTION_RECORD));
+    return EXCEPTION_EXECUTE_HANDLER;
+}
+
 VOID
 INIT_FUNCTION
 NTAPI
@@ -23,7 +69,7 @@ KeInitExceptions(VOID)
 {
     ULONG i;
     USHORT FlippedSelector;
-    extern KIDTENTRY KiIdt[];
+    extern KIDTENTRY KiIdt[MAXIMUM_IDTVECTOR];
 
     /* Loop the IDT */
     for (i = 0; i <= MAXIMUM_IDTVECTOR; i ++)
@@ -37,6 +83,101 @@ KeInitExceptions(VOID)
     }
 }
 
+ULONG
+FASTCALL
+KiUpdateDr7(IN ULONG Dr7)
+{
+    ULONG DebugMask = KeGetCurrentThread()->DispatcherHeader.DebugActive;
+
+    /* Check if debugging is enabled */
+    if (DebugMask & DR_MASK(DR7_OVERRIDE_V))
+    {
+        /* Sanity checks */
+        ASSERT((DebugMask & DR_REG_MASK) != 0);
+        ASSERT((Dr7 & ~DR7_RESERVED_MASK) == DR7_OVERRIDE_MASK);
+        return 0;
+    }
+
+    /* Return DR7 itself */
+    return Dr7;
+}
+
+BOOLEAN
+FASTCALL
+KiRecordDr7(OUT PULONG Dr7Ptr,
+            OUT PULONG DrMask)
+{
+    ULONG NewMask, Mask;
+    UCHAR Result;
+
+    /* Check if the caller gave us a mask */
+    if (!DrMask)
+    {
+        /* He didn't use the one from the thread */
+        Mask = KeGetCurrentThread()->DispatcherHeader.DebugActive;
+    }
+    else
+    {
+        /* He did, read it */
+        Mask = *DrMask;
+    }
+
+    /* Sanity check */
+    ASSERT((*Dr7Ptr & DR7_RESERVED_MASK) == 0);
+
+    /* Check if DR7 is empty */
+    NewMask = Mask;
+    if (!(*Dr7Ptr))
+    {
+        /* Assume failure */
+        Result = FALSE;
+
+        /* Check the DR mask */
+        NewMask &= ~(DR_MASK(7));
+        if (NewMask & DR_REG_MASK)
+        {
+            /* Set the active mask */
+            NewMask |= DR_MASK(DR7_OVERRIDE_V);
+
+            /* Set DR7 override */
+            *DrMask = DR7_OVERRIDE_MASK;
+        }
+        else
+        {
+            /* Sanity check */
+            ASSERT(NewMask == 0);
+        }
+    }
+    else
+    {
+        /* Check if we have a mask or not */
+        Result = NewMask ? TRUE: FALSE;
+
+        /* Update the mask to disable debugging */
+        NewMask &= ~(DR_MASK(DR7_OVERRIDE_V));
+        NewMask |= DR_MASK(7);
+    }
+
+    /* Check if caller wants the new mask */
+    if (DrMask)
+    {
+        /* Update it */
+        *DrMask = NewMask;
+    }
+    else
+    {
+        /* Check if the mask changed */
+        if (Mask != NewMask)
+        {
+            /* Update it */
+            KeGetCurrentThread()->DispatcherHeader.DebugActive = NewMask;
+        }
+    }
+
+    /* Return the result */
+    return Result;
+}
+
 ULONG
 NTAPI
 KiEspFromTrapFrame(IN PKTRAP_FRAME TrapFrame)
@@ -176,6 +317,8 @@ KeContextToTrapFrame(IN PCONTEXT Context,
     ULONG i;
     BOOLEAN V86Switch = FALSE;
     KIRQL OldIrql = APC_LEVEL;
+    ULONG DrMask = 0;
+    PVOID SafeDr;
 
     /* Do this at APC_LEVEL */
     if (KeGetCurrentIrql() < APC_LEVEL) KeRaiseIrql(APC_LEVEL, &OldIrql);
@@ -191,8 +334,8 @@ KeContextToTrapFrame(IN PCONTEXT Context,
             V86Switch = TRUE;
         }
 
-        /* Copy EFLAGS. FIXME: Needs to be sanitized */
-        TrapFrame->EFlags = Context->EFlags;
+        /* Copy EFLAGS and sanitize them*/
+        TrapFrame->EFlags = Ke386SanitizeFlags(Context->EFlags, PreviousMode);
 
         /* Copy EBP and EIP */
         TrapFrame->Ebp = Context->Ebp;
@@ -206,14 +349,14 @@ KeContextToTrapFrame(IN PCONTEXT Context,
         }
         else
         {
-            /* We weren't in V86, so sanitize the CS (FIXME!) */
-            TrapFrame->SegCs = Context->SegCs;
+            /* We weren't in V86, so sanitize the CS */
+            TrapFrame->SegCs = Ke386SanitizeSeg(Context->SegCs, PreviousMode);
 
             /* Don't let it under 8, that's invalid */
             if ((PreviousMode != KernelMode) && (TrapFrame->SegCs < 8))
             {
                 /* Force it to User CS */
-                TrapFrame->SegCs = (KGDT_R3_CODE | RPL_MASK);
+                TrapFrame->SegCs = KGDT_R3_CODE | RPL_MASK;
             }
         }
 
@@ -245,7 +388,7 @@ KeContextToTrapFrame(IN PCONTEXT Context,
         /* Check if we were in V86 Mode */
         if (TrapFrame->EFlags & EFLAGS_V86_MASK)
         {
-            /* Copy the V86 Segments directlry */
+            /* Copy the V86 Segments directly */
             TrapFrame->V86Ds = Context->SegDs;
             TrapFrame->V86Es = Context->SegEs;
             TrapFrame->V86Fs = Context->SegFs;
@@ -256,12 +399,12 @@ KeContextToTrapFrame(IN PCONTEXT Context,
             /* For kernel mode, write the standard values */
             TrapFrame->SegDs = KGDT_R3_DATA | RPL_MASK;
             TrapFrame->SegEs = KGDT_R3_DATA | RPL_MASK;
-            TrapFrame->SegFs = Context->SegFs;
+            TrapFrame->SegFs = Ke386SanitizeSeg(Context->SegFs, PreviousMode);
             TrapFrame->SegGs = 0;
         }
         else
         {
-            /* For user mode, return the values directlry */
+            /* For user mode, return the values directly */
             TrapFrame->SegDs = Context->SegDs;
             TrapFrame->SegEs = Context->SegEs;
             TrapFrame->SegFs = Context->SegFs;
@@ -304,7 +447,13 @@ KeContextToTrapFrame(IN PCONTEXT Context,
             /* Mask out any invalid flags */
             FxSaveArea->Cr0NpxState &= ~(CR0_EM | CR0_MP | CR0_TS);
 
-            /* FIXME: Check if this is a VDM app */
+            /* Check if this is a VDM app */
+            if (PsGetCurrentProcess()->VdmObjects)
+            {
+                /* Allow the EM flag */
+                FxSaveArea->Cr0NpxState |= Context->FloatSave.Cr0NpxState &
+                                           (CR0_EM | CR0_MP);
+            }
         }
     }
 
@@ -357,16 +506,40 @@ KeContextToTrapFrame(IN PCONTEXT Context,
             }
             else
             {
-                /* Just dump the Fn state in */
-                RtlCopyMemory(&FxSaveArea->U.FnArea,
-                              &Context->FloatSave,
-                              sizeof(FNSAVE_FORMAT));
+                /* Copy the structure */
+                FxSaveArea->U.FnArea.ControlWord = Context->FloatSave.
+                                                   ControlWord;
+                FxSaveArea->U.FnArea.StatusWord = Context->FloatSave.
+                                                  StatusWord;
+                FxSaveArea->U.FnArea.TagWord = Context->FloatSave.TagWord;
+                FxSaveArea->U.FnArea.ErrorOffset = Context->FloatSave.
+                                                   ErrorOffset;
+                FxSaveArea->U.FnArea.ErrorSelector = Context->FloatSave.
+                                                     ErrorSelector;
+                FxSaveArea->U.FnArea.DataOffset = Context->FloatSave.
+                                                  DataOffset;
+                FxSaveArea->U.FnArea.DataSelector = Context->FloatSave.
+                                                    DataSelector;
+
+                /* Loop registers */
+                for (i = 0; i < SIZE_OF_80387_REGISTERS; i++)
+                {
+                    /* Copy registers */
+                    FxSaveArea->U.FnArea.RegisterArea[i] =
+                        Context->FloatSave.RegisterArea[i];
+                }
             }
 
             /* Mask out any invalid flags */
             FxSaveArea->Cr0NpxState &= ~(CR0_EM | CR0_MP | CR0_TS);
 
-            /* FIXME: Check if this is a VDM app */
+            /* Check if this is a VDM app */
+            if (PsGetCurrentProcess()->VdmObjects)
+            {
+                /* Allow the EM flag */
+                FxSaveArea->Cr0NpxState |= Context->FloatSave.Cr0NpxState &
+                    (CR0_EM | CR0_MP);
+            }
         }
         else
         {
@@ -378,23 +551,38 @@ KeContextToTrapFrame(IN PCONTEXT Context,
     /* Handle the Debug Registers */
     if ((ContextFlags & CONTEXT_DEBUG_REGISTERS) == CONTEXT_DEBUG_REGISTERS)
     {
-        /* FIXME: All these should be sanitized */
-        TrapFrame->Dr0 = Context->Dr0;
-        TrapFrame->Dr1 = Context->Dr1;
-        TrapFrame->Dr2 = Context->Dr2;
-        TrapFrame->Dr3 = Context->Dr3;
-        TrapFrame->Dr6 = Context->Dr6;
-        TrapFrame->Dr7 = Context->Dr7;
+        /* Loop DR registers */
+        for (i = 0; i < 4; i++)
+        {
+            /* Sanitize the context DR Address */
+            SafeDr = Ke386SanitizeDr(KiDrFromContext(i, Context), PreviousMode);
+
+            /* Save it in the trap frame */
+            *KiDrFromTrapFrame(i, TrapFrame) = SafeDr;
+
+            /* Check if this DR address is active and add it in the DR mask */
+            if (SafeDr) DrMask |= DR_MASK(i);
+        }
+
+        /* Now save and sanitize DR6 */
+        TrapFrame->Dr6 = Context->Dr6 & DR6_LEGAL;
+        if (TrapFrame->Dr6) DrMask |= DR_MASK(6);
+
+        /* Save and sanitize DR7 */
+        TrapFrame->Dr7 = Context->Dr7 & DR7_LEGAL;
+        KiRecordDr7(&TrapFrame->Dr7, &DrMask);
 
-        /* Check if usermode */
+        /* If we're in user-mode */
         if (PreviousMode != KernelMode)
         {
-            /* Set the Debug Flag */
-            KeGetCurrentThread()->DispatcherHeader.DebugActive =
-                (Context->Dr7 & DR7_ACTIVE);
+            /* FIXME: Save the mask */
+            //KeGetCurrentThread()->DispatcherHeader.DebugActive = DrMask;
         }
     }
 
+    /* Check if thread has IOPL and force it enabled if so */
+    if (KeGetCurrentThread()->Iopl) TrapFrame->EFlags |= 0x3000;
+
     /* Restore IRQL */
     if (OldIrql < APC_LEVEL) KeLowerIrql(OldIrql);
 }
@@ -413,6 +601,7 @@ KeTrapFrameToContext(IN PKTRAP_FRAME TrapFrame,
     } FloatSaveBuffer;
     FLOATING_SAVE_AREA *FloatSaveArea;
     KIRQL OldIrql = APC_LEVEL;
+    ULONG i;
 
     /* Do this at APC_LEVEL */
     if (KeGetCurrentIrql() < APC_LEVEL) KeRaiseIrql(APC_LEVEL, &OldIrql);
@@ -534,10 +723,23 @@ KeTrapFrameToContext(IN PKTRAP_FRAME TrapFrame,
                 KiFlushNPXState(NULL);
             }
 
-            /* Copy into the Context */
-            RtlCopyMemory(&Context->FloatSave,
-                          FloatSaveArea,
-                          sizeof(FNSAVE_FORMAT));
+            /* Copy structure */
+            Context->FloatSave.ControlWord = FloatSaveArea->ControlWord;
+            Context->FloatSave.StatusWord = FloatSaveArea->StatusWord;
+            Context->FloatSave.TagWord = FloatSaveArea->TagWord;
+            Context->FloatSave.ErrorOffset = FloatSaveArea->ErrorOffset;
+            Context->FloatSave.ErrorSelector = FloatSaveArea->ErrorSelector;
+            Context->FloatSave.DataOffset = FloatSaveArea->DataOffset;
+            Context->FloatSave.DataSelector = FloatSaveArea->DataSelector;
+            Context->FloatSave.Cr0NpxState = FxSaveArea->Cr0NpxState;
+
+            /* Loop registers */
+            for (i = 0; i < SIZE_OF_80387_REGISTERS; i++)
+            {
+                /* Copy them */
+                Context->FloatSave.RegisterArea[i] =
+                    FloatSaveArea->RegisterArea[i];
+            }
          }
          else
          {
@@ -550,24 +752,26 @@ KeTrapFrameToContext(IN PKTRAP_FRAME TrapFrame,
     if ((Context->ContextFlags & CONTEXT_DEBUG_REGISTERS) ==
         CONTEXT_DEBUG_REGISTERS)
     {
-        /* Copy the debug registers */
-        Context->Dr0 = TrapFrame->Dr0;
-        Context->Dr1 = TrapFrame->Dr1;
-        Context->Dr2 = TrapFrame->Dr2;
-        Context->Dr3 = TrapFrame->Dr3;
-        Context->Dr6 = TrapFrame->Dr6;
-
-        /* For user-mode, only set DR7 if a debugger is active */
-        if (((TrapFrame->SegCs & MODE_MASK) ||
-            (TrapFrame->EFlags & EFLAGS_V86_MASK)) &&
-            (KeGetCurrentThread()->DispatcherHeader.DebugActive))
+        /* Make sure DR7 is valid */
+        if (TrapFrame->Dr7 & ~DR7_RESERVED_MASK)
         {
-            /* Copy it over */
-            Context->Dr7 = TrapFrame->Dr7;
+            /* Copy the debug registers */
+            Context->Dr0 = TrapFrame->Dr0;
+            Context->Dr1 = TrapFrame->Dr1;
+            Context->Dr2 = TrapFrame->Dr2;
+            Context->Dr3 = TrapFrame->Dr3;
+            Context->Dr6 = TrapFrame->Dr6;
+
+            /* Update DR7 */
+            //Context->Dr7 = KiUpdateDr7(TrapFrame->Dr7);
         }
         else
         {
-            /* Clear it */
+            /* Otherwise clear DR registers */
+            Context->Dr0 =
+            Context->Dr1 =
+            Context->Dr3 =
+            Context->Dr6 =
             Context->Dr7 = 0;
         }
     }
@@ -578,17 +782,17 @@ KeTrapFrameToContext(IN PKTRAP_FRAME TrapFrame,
 
 VOID
 NTAPI
-KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
-                    PKEXCEPTION_FRAME ExceptionFrame,
-                    PKTRAP_FRAME TrapFrame,
-                    KPROCESSOR_MODE PreviousMode,
-                    BOOLEAN FirstChance)
+KiDispatchException(IN PEXCEPTION_RECORD ExceptionRecord,
+                    IN PKEXCEPTION_FRAME ExceptionFrame,
+                    IN PKTRAP_FRAME TrapFrame,
+                    IN KPROCESSOR_MODE PreviousMode,
+                    IN BOOLEAN FirstChance)
 {
     CONTEXT Context;
-    KD_CONTINUE_TYPE Action;
     ULONG_PTR Stack, NewStack;
     ULONG Size;
-    BOOLEAN UserDispatch = FALSE;
+    EXCEPTION_RECORD LocalExceptRecord;
+    _SEH_DECLARE_LOCALS(KiCopyInfo);
 
     /* Increase number of Exception Dispatches */
     KeGetCurrentPrcb()->KeExceptionDispatchCount++;
@@ -596,17 +800,34 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
     /* Set the context flags */
     Context.ContextFlags = CONTEXT_FULL | CONTEXT_DEBUG_REGISTERS;
 
-    /* Check if User Mode */
-    if (PreviousMode == UserMode)
+    /* Check if User Mode or if the debugger isenabled */
+    if ((PreviousMode == UserMode) || (KdDebuggerEnabled))
     {
         /* Add the FPU Flag */
         Context.ContextFlags |= CONTEXT_FLOATING_POINT;
-        if (KeI386FxsrPresent) Context.ContextFlags |= CONTEXT_EXTENDED_REGISTERS;
+
+        /* Check for NPX Support */
+        if (KeI386FxsrPresent)
+        {
+            /* Save those too */
+            Context.ContextFlags |= CONTEXT_EXTENDED_REGISTERS;
+        }
     }
 
     /* Get a Context */
     KeTrapFrameToContext(TrapFrame, ExceptionFrame, &Context);
 
+    /* Fix up EIP */
+    if (ExceptionRecord->ExceptionCode == STATUS_BREAKPOINT)
+    {
+        /* Decrement EIP by one */
+        Context.Eip--;
+    }
+
+    /* Sanity check */
+    ASSERT(!((PreviousMode == KernelMode) &&
+             (Context.EFlags & EFLAGS_V86_MASK)));
+
     /* Handle kernel-mode first, it's simpler */
     if (PreviousMode == KernelMode)
     {
@@ -614,42 +835,42 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
         if (FirstChance == TRUE)
         {
             /* Break into the debugger for the first time */
-            Action = KdpEnterDebuggerException(ExceptionRecord,
-                                               PreviousMode,
-                                               &Context,
-                                               TrapFrame,
-                                               TRUE,
-                                               TRUE);
-
-            /* If the debugger said continue, then continue */
-            if (Action == kdContinue) goto Handled;
-
-            /* If the Debugger couldn't handle it, dispatch the exception */
-            if (RtlDispatchException(ExceptionRecord, &Context))
+            if (KiDebugRoutine(TrapFrame,
+                               ExceptionFrame,
+                               ExceptionRecord,
+                               &Context,
+                               PreviousMode,
+                               FALSE))
             {
-                /* It was handled by an exception handler, continue */
+                /* Exception was handled */
                 goto Handled;
             }
+
+            /* HACK: GDB Entry */
+            if (KdpCallGdb(TrapFrame, ExceptionRecord, &Context)) goto Handled;
+
+            /* If the Debugger couldn't handle it, dispatch the exception */
+            if (RtlDispatchException(ExceptionRecord, &Context)) goto Handled;
         }
 
         /* This is a second-chance exception, only for the debugger */
-        Action = KdpEnterDebuggerException(ExceptionRecord,
-                                           PreviousMode,
-                                           &Context,
-                                           TrapFrame,
-                                           FALSE,
-                                           FALSE);
-
-        /* If the debugger said continue, then continue */
-        if (Action == kdContinue) goto Handled;
+        if (KiDebugRoutine(TrapFrame,
+                           ExceptionFrame,
+                           ExceptionRecord,
+                           &Context,
+                           PreviousMode,
+                           TRUE))
+        {
+            /* Exception was handled */
+            goto Handled;
+        }
 
         /* Third strike; you're out */
-        KEBUGCHECKWITHTF(KMODE_EXCEPTION_NOT_HANDLED,
-                         ExceptionRecord->ExceptionCode,
-                         (ULONG_PTR)ExceptionRecord->ExceptionAddress,
-                         ExceptionRecord->ExceptionInformation[0],
-                         ExceptionRecord->ExceptionInformation[1],
-                         TrapFrame);
+        KeBugCheckEx(KMODE_EXCEPTION_NOT_HANDLED,
+                     ExceptionRecord->ExceptionCode,
+                     (ULONG_PTR)ExceptionRecord->ExceptionAddress,
+                     ExceptionRecord->ExceptionInformation[0],
+                     ExceptionRecord->ExceptionInformation[1]);
     }
     else
     {
@@ -657,38 +878,59 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
         if (FirstChance)
         {
             /* Enter Debugger if available */
-            Action = KdpEnterDebuggerException(ExceptionRecord,
-                                               PreviousMode,
-                                               &Context,
-                                               TrapFrame,
-                                               TRUE,
-                                               TRUE);
+            if (PsGetCurrentProcess()->DebugPort)
+            {
+                /* FIXME : TODO */
+                //KEBUGCHECK(0);
+            }
+            else if (KiDebugRoutine(TrapFrame,
+                                    ExceptionFrame,
+                                    ExceptionRecord,
+                                    &Context,
+                                    PreviousMode,
+                                    FALSE))
+            {
+                /* Exception was handled */
+                goto Handled;
+            }
 
-            /* Exit if we're continuing */
-            if (Action == kdContinue) goto Handled;
+            /* HACK: GDB Entry */
+            if (KdpCallGdb(TrapFrame, ExceptionRecord, &Context)) goto Handled;
 
-            /* FIXME: Forward exception to user mode debugger */
+            /* Forward exception to user mode debugger */
+            if (DbgkForwardException(ExceptionRecord, TRUE, FALSE)) goto Exit;
 
             /* Set up the user-stack */
+DispatchToUser:
             _SEH_TRY
             {
+                /* Make sure we have a valid SS and that this isn't V86 mode */
+                if ((TrapFrame->HardwareSegSs != (KGDT_R3_DATA | RPL_MASK)) ||
+                    (TrapFrame->EFlags & EFLAGS_V86_MASK))
+                {
+                    /* Raise an exception instead */
+                    LocalExceptRecord.ExceptionCode = STATUS_ACCESS_VIOLATION;
+                    LocalExceptRecord.ExceptionFlags = 0;
+                    LocalExceptRecord.NumberParameters = 0;
+                    RtlRaiseException(&LocalExceptRecord);
+                }
+
                 /* Align context size and get stack pointer */
                 Size = (sizeof(CONTEXT) + 3) & ~3;
                 Stack = (Context.Esp & ~3) - Size;
-                DPRINT("Stack: %lx\n", Stack);
 
                 /* Probe stack and copy Context */
                 ProbeForWrite((PVOID)Stack, Size, sizeof(ULONG));
                 RtlCopyMemory((PVOID)Stack, &Context, sizeof(CONTEXT));
 
                 /* Align exception record size and get stack pointer */
-                Size = (sizeof(EXCEPTION_RECORD) - 
-                        (EXCEPTION_MAXIMUM_PARAMETERS - ExceptionRecord->NumberParameters) *
-                        sizeof(ULONG) + 3) & ~3;
+                Size = (sizeof(EXCEPTION_RECORD) -
+                       (EXCEPTION_MAXIMUM_PARAMETERS -
+                        ExceptionRecord->NumberParameters) *
+                       sizeof(ULONG) + 3) & ~3;
                 NewStack = Stack - Size;
-                DPRINT("NewStack: %lx\n", NewStack);
 
-                /* Probe stack and copy exception record. Don't forget to add the two params */
+                /* Probe stack and copy exception record */
                 ProbeForWrite((PVOID)(NewStack - 2 * sizeof(ULONG_PTR)),
                               Size +  2 * sizeof(ULONG_PTR),
                               sizeof(ULONG));
@@ -699,43 +941,72 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
                 *(PULONG_PTR)(NewStack - 2 * sizeof(ULONG_PTR)) = NewStack;
 
                 /* Set new Stack Pointer */
+                KiSsToTrapFrame(TrapFrame, KGDT_R3_DATA);
                 KiEspToTrapFrame(TrapFrame, NewStack - 2 * sizeof(ULONG_PTR));
 
-                /* Set EIP to the User-mode Dispathcer */
+                /* Force correct segments */
+                TrapFrame->SegCs = Ke386SanitizeSeg(KGDT_R3_CODE, PreviousMode);
+                TrapFrame->SegDs = Ke386SanitizeSeg(KGDT_R3_DATA, PreviousMode);
+                TrapFrame->SegEs = Ke386SanitizeSeg(KGDT_R3_DATA, PreviousMode);
+                TrapFrame->SegFs = Ke386SanitizeSeg(KGDT_R3_TEB, PreviousMode);
+                TrapFrame->SegGs = 0;
+
+                /* Set EIP to the User-mode Dispatcher */
                 TrapFrame->Eip = (ULONG)KeUserExceptionDispatcher;
-                UserDispatch = TRUE;
                 _SEH_LEAVE;
             }
-            _SEH_HANDLE
+            _SEH_EXCEPT(KiCopyInformation)
             {
-                /* Do second-chance */
+                /* Check if we got a stack overflow and raise that instead */
+                if (_SEH_VAR(SehExceptRecord).ExceptionCode ==
+                    STATUS_STACK_OVERFLOW)
+                {
+                    /* Copy the exception address and record */
+                    _SEH_VAR(SehExceptRecord).ExceptionAddress =
+                        ExceptionRecord->ExceptionAddress;
+                    RtlCopyMemory(ExceptionRecord,
+                                  (PVOID)&_SEH_VAR(SehExceptRecord),
+                                  sizeof(EXCEPTION_RECORD));
+
+                    /* Do the exception again */
+                    goto DispatchToUser;
+                }
             }
             _SEH_END;
-        }
 
-        /* If we dispatch to user, return now */
-        if (UserDispatch) return;
+            /* Dispatch exception to user-mode */
+            return;
+        }
 
-        /* FIXME: Forward the exception to the debugger for 2nd chance */
+        /* Try second chance */
+        if (DbgkForwardException(ExceptionRecord, TRUE, FALSE))
+        {
+            /* Handled, get out */
+            goto Exit;
+        }
+        else if (DbgkForwardException(ExceptionRecord, FALSE, TRUE))
+        {
+            /* Handled, get out */
+            goto Exit;
+        }
 
-        /* 3rd strike, kill the thread */
-        DPRINT1("Unhandled UserMode exception, terminating thread\n");
-        ZwTerminateThread(NtCurrentThread(), ExceptionRecord->ExceptionCode);
-        KEBUGCHECKWITHTF(KMODE_EXCEPTION_NOT_HANDLED,
-                         ExceptionRecord->ExceptionCode,
-                         (ULONG_PTR)ExceptionRecord->ExceptionAddress,
-                         ExceptionRecord->ExceptionInformation[0],
-                         ExceptionRecord->ExceptionInformation[1],
-                         TrapFrame);
+        /* 3rd strike, kill the process */
+        ZwTerminateProcess(NtCurrentProcess(), ExceptionRecord->ExceptionCode);
+        KeBugCheckEx(KMODE_EXCEPTION_NOT_HANDLED,
+                     ExceptionRecord->ExceptionCode,
+                     (ULONG_PTR)ExceptionRecord->ExceptionAddress,
+                     ExceptionRecord->ExceptionInformation[0],
+                     ExceptionRecord->ExceptionInformation[1]);
     }
 
 Handled:
     /* Convert the context back into Trap/Exception Frames */
     KeContextToTrapFrame(&Context,
-                         NULL,
+                         ExceptionFrame,
                          TrapFrame,
                          Context.ContextFlags,
                          PreviousMode);
+Exit:
     return;
 }
 
@@ -746,27 +1017,32 @@ NTSTATUS
 NTAPI
 KeRaiseUserException(IN NTSTATUS ExceptionCode)
 {
+    NTSTATUS Status = STATUS_SUCCESS;
     ULONG OldEip;
-    PKTHREAD Thread = KeGetCurrentThread();
+    PTEB Teb = KeGetCurrentThread()->Teb;
+    PKTRAP_FRAME TrapFrame = KeGetCurrentThread()->TrapFrame;
 
     /* Make sure we can access the TEB */
     _SEH_TRY
     {
-        Thread->Teb->ExceptionCode = ExceptionCode;
+        /* Set the exception code */
+        Teb->ExceptionCode = ExceptionCode;
     }
     _SEH_HANDLE
     {
-        return(ExceptionCode);
+        /* Save exception code */
+        Status = ExceptionCode;
     }
     _SEH_END;
+    if (!NT_SUCCESS(Status)) return Status;
 
     /* Get the old EIP */
-    OldEip = Thread->TrapFrame->Eip;
+    OldEip = TrapFrame->Eip;
 
     /* Change it to the user-mode dispatcher */
-    Thread->TrapFrame->Eip = (ULONG_PTR)KeRaiseUserExceptionDispatcher;
+    TrapFrame->Eip = (ULONG_PTR)KeRaiseUserExceptionDispatcher;
 
     /* Return the old EIP */
-    return((NTSTATUS)OldEip);
+    return (NTSTATUS)OldEip;
 }