From: Alex Ionescu <aionescu@gmail.com>
Date: Tue, 13 Sep 2005 19:33:49 +0000 (+0000)
Subject: Some work-in-progress improvements and rethought of system-level code...notable impro... 
X-Git-Tag: ReactOS-0.2.8~583
X-Git-Url: https://git.reactos.org/?p=reactos.git;a=commitdiff_plain;h=7a5244c46d5f6d755aa09d8e61b9e05d314672a1

Some work-in-progress improvements and rethought of system-level code...notable improvemnts till now are: -The KTHREAD alterted flag is now properly set to FALSE during APC delivery during traps, -More support for PKEXCEPTION_FRAME has been added for future PPC/IA64/etc compatibility -Context to/from TrapFrame functions have been greatly fixed to allow for edited frames, special cases, exceptions and also have the necessary support for supporting sanitation based on PreviousMode - KDBG now shows the proper CS/SS at all times, previous hack was removed. - Systemcall code was optimized a bit and the trap code was made a lot more generic so that in can be made into macros (it currently isn't, for debugging purposes). - V86 Mode trap frame bias has now been added to make trap code a lot more generic and shareable in the future. -More support and fixes for future Debugging capabilities have been added. - Random other scalability and stability code. This is still being worked on, syscall.S looks nasty for now (visually)

svn path=/trunk/; revision=17838
---

diff --git a/reactos/lib/ntdll/ldr/startup.c b/reactos/lib/ntdll/ldr/startup.c
index 8e8c7c227ee..02347bc4563 100644
--- a/reactos/lib/ntdll/ldr/startup.c
+++ b/reactos/lib/ntdll/ldr/startup.c
@@ -256,13 +256,13 @@ LdrpInit(PCONTEXT Context,
    SYSTEM_BASIC_INFORMATION SystemInformation;
    NTSTATUS Status;
 
-   DPRINT("LdrpInit()\n");
+   DPRINT1("LdrpInit()\n");
    if (NtCurrentPeb()->Ldr == NULL || NtCurrentPeb()->Ldr->Initialized == FALSE)
      {
        Peb = (PPEB)(PEB_BASE);
-       DPRINT("Peb %x\n", Peb);
+       DPRINT1("Peb %x\n", Peb);
        ImageBase = Peb->ImageBaseAddress;
-       DPRINT("ImageBase %x\n", ImageBase);
+       DPRINT1("ImageBase %x\n", ImageBase);
        if (ImageBase <= (PVOID)0x1000)
          {
            DPRINT("ImageBase is null\n");
@@ -271,7 +271,7 @@ LdrpInit(PCONTEXT Context,
 
        /*  If MZ header exists  */
        PEDosHeader = (PIMAGE_DOS_HEADER) ImageBase;
-       DPRINT("PEDosHeader %x\n", PEDosHeader);
+       DPRINT1("PEDosHeader %x\n", PEDosHeader);
 
        if (PEDosHeader->e_magic != IMAGE_DOS_SIGNATURE ||
            PEDosHeader->e_lfanew == 0L ||
@@ -294,10 +294,12 @@ LdrpInit(PCONTEXT Context,
        NTHeaders = (PIMAGE_NT_HEADERS)((ULONG_PTR)ImageBase + PEDosHeader->e_lfanew);
 
        /* Get number of processors */
+       DPRINT1("Here\n");
        Status = ZwQuerySystemInformation(SystemBasicInformation,
 	                                 &SystemInformation,
 					 sizeof(SYSTEM_BASIC_INFORMATION),
 					 NULL);
+        DPRINT1("Here2\n");
        if (!NT_SUCCESS(Status))
          {
 	   ZwTerminateProcess(NtCurrentProcess(), Status);
diff --git a/reactos/lib/rtl/i386/exception.c b/reactos/lib/rtl/i386/exception.c
index c0d8be17335..c563d534cf3 100644
--- a/reactos/lib/rtl/i386/exception.c
+++ b/reactos/lib/rtl/i386/exception.c
@@ -165,7 +165,7 @@ RtlDispatchException(IN PEXCEPTION_RECORD ExceptionRecord,
     }
 
     /* Unhandled, return false */
-    DPRINT1("FALSE:(\n");
+    DPRINT1("FALSE\n");
     return FALSE;
 }
 
diff --git a/reactos/ntoskrnl/ex/init.c b/reactos/ntoskrnl/ex/init.c
index fdf708df666..ccd545af206 100644
--- a/reactos/ntoskrnl/ex/init.c
+++ b/reactos/ntoskrnl/ex/init.c
@@ -241,7 +241,6 @@ ExecuteRuntimeAsserts(VOID)
     ASSERT(FIELD_OFFSET(KPROCESS, DirectoryTableBase) == KPROCESS_DIRECTORY_TABLE_BASE);
     ASSERT(FIELD_OFFSET(KPROCESS, IopmOffset) == KPROCESS_IOPM_OFFSET);
     ASSERT(FIELD_OFFSET(KPROCESS, LdtDescriptor) == KPROCESS_LDT_DESCRIPTOR0);
-    ASSERT(FIELD_OFFSET(KTRAP_FRAME, Reserved9) == KTRAP_FRAME_RESERVED9);
     ASSERT(FIELD_OFFSET(KV86M_TRAP_FRAME, SavedExceptionStack) == TF_SAVED_EXCEPTION_STACK);
     ASSERT(FIELD_OFFSET(KV86M_TRAP_FRAME, regs) == TF_REGS);
     ASSERT(FIELD_OFFSET(KV86M_TRAP_FRAME, orig_ebp) == TF_ORIG_EBP);
diff --git a/reactos/ntoskrnl/include/internal/i386/ke.h b/reactos/ntoskrnl/include/internal/i386/ke.h
index 508a55b5717..7d16153242d 100644
--- a/reactos/ntoskrnl/include/internal/i386/ke.h
+++ b/reactos/ntoskrnl/include/internal/i386/ke.h
@@ -40,11 +40,15 @@
 #define X86_EXT_FEATURE_SSE3    0x00000001 /* SSE3 extension present */
 #define X86_EXT_FEATURE_3DNOW   0x40000000 /* 3DNOW! extension present */
 
+#define DR7_ACTIVE              0x00000055  /* If any of these bits are set, a Dr is active */
+
 /* Possible values for KTHREAD's NpxState */
 #define NPX_STATE_INVALID   0x01
 #define NPX_STATE_VALID     0x02
 #define NPX_STATE_DIRTY     0x04
 
+#define FRAME_EDITED        0xFFF8
+
 #ifndef __ASM__
 
 typedef struct _KIRQ_TRAPFRAME
diff --git a/reactos/ntoskrnl/include/internal/ke.h b/reactos/ntoskrnl/include/internal/ke.h
index 43eeccf0b23..a49b938599f 100644
--- a/reactos/ntoskrnl/include/internal/ke.h
+++ b/reactos/ntoskrnl/include/internal/ke.h
@@ -287,7 +287,10 @@ VOID inline FASTCALL KiSatisifyMultipleObjectWaits(PKWAIT_BLOCK WaitBlock);
 VOID FASTCALL KiWaitTest(PDISPATCHER_HEADER Object, KPRIORITY Increment);
 
 PULONG KeGetStackTopThread(struct _ETHREAD* Thread);
-BOOLEAN STDCALL KeContextToTrapFrame(PCONTEXT Context, PKEXCEPTION_FRAME ExeptionFrame, PKTRAP_FRAME TrapFrame);
+BOOLEAN STDCALL KeContextToTrapFrame(PCONTEXT Context,
+                                     PKEXCEPTION_FRAME ExeptionFrame,
+                                     PKTRAP_FRAME TrapFrame,
+                                     KPROCESSOR_MODE PreviousMode);
 VOID STDCALL KiDeliverApc(KPROCESSOR_MODE PreviousMode,
                   PVOID Reserved,
                   PKTRAP_FRAME TrapFrame);
@@ -368,8 +371,11 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
             PKTRAP_FRAME Tf,
 		    KPROCESSOR_MODE PreviousMode,
 		    BOOLEAN SearchFrames);
-VOID KeTrapFrameToContext(PKTRAP_FRAME TrapFrame,
-			  PCONTEXT Context);
+VOID
+NTAPI
+KeTrapFrameToContext(IN PKTRAP_FRAME TrapFrame,
+                     IN PKEXCEPTION_FRAME ExceptionFrame,
+                     IN OUT PCONTEXT Context);
 VOID
 KeApplicationProcessorInit(VOID);
 VOID
diff --git a/reactos/ntoskrnl/kd/wrappers/gdbstub.c b/reactos/ntoskrnl/kd/wrappers/gdbstub.c
index a5b6c30ae13..8e2142aed60 100644
--- a/reactos/ntoskrnl/kd/wrappers/gdbstub.c
+++ b/reactos/ntoskrnl/kd/wrappers/gdbstub.c
@@ -1321,7 +1321,7 @@ KdpGdbEnterDebuggerException(PEXCEPTION_RECORD ExceptionRecord,
 #error Unknown compiler for inline assembler
 #endif
 
-                KeContextToTrapFrame(Context, NULL, TrapFrame);
+                KeContextToTrapFrame(Context, NULL, TrapFrame, KernelMode);
                 return ((SigVal == 5) ? (kdContinue) : (kdHandleException));
                 break;
               }
@@ -1430,11 +1430,11 @@ GspBreakIn(PKINTERRUPT Interrupt,
 
   TrapFrame = PsGetCurrentThread()->Tcb.TrapFrame;
 
-  KeTrapFrameToContext (TrapFrame, &Context);
+  KeTrapFrameToContext (TrapFrame, NULL, &Context);
 
   KdpGdbEnterDebuggerException (NULL, &Context, TrapFrame);
 
-  KeContextToTrapFrame (&Context, NULL, TrapFrame);
+  KeContextToTrapFrame (&Context, NULL, TrapFrame, KernelMode);
 
   KeLowerIrql (OldIrql);
 
diff --git a/reactos/ntoskrnl/kdbg/kdb.c b/reactos/ntoskrnl/kdbg/kdb.c
index fd67c4e969c..f7d145627ae 100644
--- a/reactos/ntoskrnl/kdbg/kdb.c
+++ b/reactos/ntoskrnl/kdbg/kdb.c
@@ -15,10 +15,6 @@
 
 /* TYPES *********************************************************************/
 
-/* FIXME: NDK headers */
-#define TempEsp TempEip
-#define TempSegSs TempCs
-
 /* DEFINES *******************************************************************/
 
 #define KDB_STACK_SIZE                   (4096*3)
@@ -108,6 +104,24 @@ STATIC CONST PCHAR ExceptionNrToString[] =
    "SIMD Fault"
 };
 
+ULONG
+NTAPI
+KiSsFromTrapFrame(IN PKTRAP_FRAME TrapFrame);
+
+ULONG
+NTAPI
+KiEspFromTrapFrame(IN PKTRAP_FRAME TrapFrame);
+
+VOID
+NTAPI
+KiSsToTrapFrame(IN PKTRAP_FRAME TrapFrame,
+                IN ULONG Ss);
+
+VOID
+NTAPI
+KiEspToTrapFrame(IN PKTRAP_FRAME TrapFrame,
+                 IN ULONG Esp);
+
 /* FUNCTIONS *****************************************************************/
 
 STATIC VOID
@@ -124,22 +138,11 @@ KdbpTrapFrameToKdbTrapFrame(PKTRAP_FRAME TrapFrame, PKDB_KTRAP_FRAME KdbTrapFram
       "movl %%cr4, %3"    "\n\t"
       : "=r"(KdbTrapFrame->Cr0), "=r"(KdbTrapFrame->Cr2),
         "=r"(KdbTrapFrame->Cr3), "=r"(KdbTrapFrame->Cr4));
-   
-   if (TrapFrame->PreviousMode == KernelMode)
-   {
-      /* If the trapframe is a kmode one use the temp ss:esp */
-      KdbTrapFrame->Tf.Esp = (ULONG)TrapFrame->TempEsp;
-      KdbTrapFrame->Tf.Ss = (USHORT)((ULONG)TrapFrame->TempSegSs & 0xFFFF);
-   }
-   else
-   {
-      /* Otherwise use ss:esp pushed by the CPU */
-      /* FIXME: maybe change all trapframes to always put ss:esp into tempss:tempesp so we
-       *        can handle umode and kmode the same way */
-      KdbTrapFrame->Tf.Esp = TrapFrame->Esp;
-      KdbTrapFrame->Tf.Ss = TrapFrame->Ss;
-   }
-   
+
+    KdbTrapFrame->Tf.Esp = KiEspFromTrapFrame(TrapFrame);
+    KdbTrapFrame->Tf.Ss = (USHORT)(KiSsFromTrapFrame(TrapFrame) & 0xFFFF);
+
+
    /* FIXME: copy v86 registers if TrapFrame is a V86 trapframe */
 }
 
@@ -151,20 +154,8 @@ KdbpKdbTrapFrameToTrapFrame(PKDB_KTRAP_FRAME KdbTrapFrame, PKTRAP_FRAME TrapFram
    
    /* FIXME: write cr0, cr2, cr3 and cr4 (not needed atm) */
    
-   if (TrapFrame->PreviousMode == KernelMode)
-   {
-      /* If the trapframe is a kmode one write to the temp ss:esp */
-      TrapFrame->TempEsp = (PVOID)KdbTrapFrame->Tf.Esp;
-      TrapFrame->TempSegSs = (PVOID)(((ULONG)TrapFrame->TempSegSs & ~0xffff) | KdbTrapFrame->Tf.Ss);
-   }
-   else
-   {
-      /* Otherwise write to ss:esp pushed by the CPU */
-      /* FIXME: maybe change all trap-epilogs to always put temp ss:esp into ss:esp so we
-       *        can handle umode and kmode the same way */
-      TrapFrame->Esp = KdbTrapFrame->Tf.Esp;
-      TrapFrame->Ss = KdbTrapFrame->Tf.Ss;
-   }
+    KiSsToTrapFrame(TrapFrame, KdbTrapFrame->Tf.Ss);
+    KiEspToTrapFrame(TrapFrame, KdbTrapFrame->Tf.Esp);
    
    /* FIXME: copy v86 registers if TrapFrame is a V86 trapframe */
 }
diff --git a/reactos/ntoskrnl/kdbg/kdb_cli.c b/reactos/ntoskrnl/kdbg/kdb_cli.c
index 1e0334143e0..ca1ebc921d7 100644
--- a/reactos/ntoskrnl/kdbg/kdb_cli.c
+++ b/reactos/ntoskrnl/kdbg/kdb_cli.c
@@ -35,10 +35,6 @@
 
 /* DEFINES *******************************************************************/
 
-/* FIXME: NDK headers */
-#define TempEsp TempEip
-#define TempSegSs TempCs
-
 #define KEY_BS          8
 #define KEY_ESC         27
 #define KEY_DEL         127
diff --git a/reactos/ntoskrnl/ke/apc.c b/reactos/ntoskrnl/ke/apc.c
index dd7b39f188e..3bee737b6af 100644
--- a/reactos/ntoskrnl/ke/apc.c
+++ b/reactos/ntoskrnl/ke/apc.c
@@ -812,7 +812,7 @@ KiInitializeUserApc(IN PKEXCEPTION_FRAME ExceptionFrame,
 
     /* Save the full context */
     Context.ContextFlags = CONTEXT_FULL | CONTEXT_DEBUG_REGISTERS;
-    KeTrapFrameToContext(TrapFrame, &Context);
+    KeTrapFrameToContext(TrapFrame, ExceptionFrame, &Context);
 
     /* Protect with SEH */
     _SEH_TRY
diff --git a/reactos/ntoskrnl/ke/exception.c b/reactos/ntoskrnl/ke/exception.c
index 49ec4fcfed3..872df7240bc 100644
--- a/reactos/ntoskrnl/ke/exception.c
+++ b/reactos/ntoskrnl/ke/exception.c
@@ -28,7 +28,7 @@ KiContinuePreviousModeUser(IN PCONTEXT Context,
     Context = &LocalContext;
 
     /* Convert the context into Exception/Trap Frames */
-    KeContextToTrapFrame(&LocalContext, ExceptionFrame, TrapFrame);
+    KeContextToTrapFrame(&LocalContext, ExceptionFrame, TrapFrame, UserMode);
 }
 
 NTSTATUS
@@ -62,7 +62,7 @@ KiContinue(IN PCONTEXT Context,
         else
         {
             /* Convert the context into Exception/Trap Frames */
-            KeContextToTrapFrame(Context, ExceptionFrame, TrapFrame);
+            KeContextToTrapFrame(Context, ExceptionFrame, TrapFrame, KernelMode);
         }
     }
     _SEH_HANDLE
@@ -142,7 +142,7 @@ KiRaiseException(PEXCEPTION_RECORD ExceptionRecord,
     if (NT_SUCCESS(Status))
     {
         /* Convert the context record */
-        KeContextToTrapFrame(Context, ExceptionFrame, TrapFrame);
+        KeContextToTrapFrame(Context, ExceptionFrame, TrapFrame, PreviousMode);
 
         /* Dispatch the exception */
         KiDispatchException(ExceptionRecord,
diff --git a/reactos/ntoskrnl/ke/i386/ctxswitch.S b/reactos/ntoskrnl/ke/i386/ctxswitch.S
index 5fe5451dd47..e3d6f018b16 100644
--- a/reactos/ntoskrnl/ke/i386/ctxswitch.S
+++ b/reactos/ntoskrnl/ke/i386/ctxswitch.S
@@ -114,7 +114,7 @@ BadThread:
 .globl @KiSwapContextInternal@0
 @KiSwapContextInternal@0:
 #ifdef KDBG
-    jmp SaveTrapFrameForKDB
+    //jmp SaveTrapFrameForKDB
 SaveTrapFrameForKDB_Return:
 #endif
 
@@ -130,12 +130,14 @@ SaveTrapFrameForKDB_Return:
     /* Switching, disable interrupts now */
     cli
 
+    /* Save the initial stack in EAX */
+    mov eax, [edi+KTHREAD_INITIAL_STACK]
+
 #ifdef CONFIG_SMP
     /* Save FPU state if the thread has used it. */
     mov dword ptr [ebx+KPCR_NPX_THREAD], 0
     test byte ptr [edi+KTHREAD_NPX_STATE], NPX_STATE_DIRTY
     jz 3f
-    mov eax, [edi+KTHREAD_INITIAL_STACK]
     cmp dword ptr _FxsrSupport, 0
     je 1f
     fxsave [eax-SIZEOF_FX_SAVE_AREA]
@@ -149,8 +151,18 @@ SaveTrapFrameForKDB_Return:
     
     /* Save the stack pointer in this processors TSS */
     mov ebp, [ebx+KPCR_TSS]
+
+    /* Check if this isn't V86 Mode, so we can bias the Esp0 */
+    test dword ptr [eax - KTRAP_FRAME_SIZE + KTRAP_FRAME_EFLAGS], X86_EFLAGS_VM
+    jnz NoAdjust
+
+    /* Bias esp */
+    //sub dword ptr ss:[ebp+KTSS_ESP0], KTRAP_FRAME_V86_GS - KTRAP_FRAME_SS
+
+NoAdjust:
+    /* Push ESP0 Value */
     push ss:[ebp+KTSS_ESP0]
-    
+
     /* Check if address space switch is needed */
     mov eax, [esi+KTHREAD_APCSTATE_PROCESS]
     cmp eax, [edi+KTHREAD_APCSTATE_PROCESS]
diff --git a/reactos/ntoskrnl/ke/i386/exp.c b/reactos/ntoskrnl/ke/i386/exp.c
index 1e2e3b95840..04cec11df17 100644
--- a/reactos/ntoskrnl/ke/i386/exp.c
+++ b/reactos/ntoskrnl/ke/i386/exp.c
@@ -17,14 +17,18 @@
 
 /*
  * FIXMES:
+ *  - Put back VEH.
  *  - Clean up file.
- *  - Add more exception frame support for non-i386 compatibility.
+ *  - Sanitize some context fields.
  *  - Add PSEH handler when an exception occurs in an exception (KiCopyExceptionRecord).
  *  - Implement official stack trace functions (exported) and remove stuff here.
  *  - Forward exceptions to user-mode debugger.
- *  - Wrap Ki NTDLL callbacks in SEH.
  */
 
+VOID
+NTAPI
+Ki386AdjustEsp0(IN PKTRAP_FRAME TrapFrame);
+
 /* GLOBALS *****************************************************************/
 
 #define FLAG_IF (1<<9)
@@ -485,14 +489,14 @@ KiTrapHandler(PKTRAP_FRAME Tf, ULONG ExceptionNr)
    ASSERT(ExceptionNr != 14);
 
    /* Store the exception number in an unused field in the trap frame. */
-   Tf->DebugArgMark = (PVOID)ExceptionNr;
+   Tf->DebugArgMark = ExceptionNr;
 
    /* Use the address of the trap frame as approximation to the ring0 esp */
    Esp0 = (ULONG)&Tf->Eip;
 
    /* Get CR2 */
    cr2 = Ke386GetCr2();
-   Tf->DebugPointer = (PVOID)cr2;
+   Tf->DebugPointer = cr2;
 
    /*
     * If this was a V86 mode exception then handle it specially
@@ -566,21 +570,172 @@ KiTrapHandler(PKTRAP_FRAME Tf, ULONG ExceptionNr)
     }
 }
 
+ULONG
+NTAPI
+KiEspFromTrapFrame(IN PKTRAP_FRAME TrapFrame)
+{
+    /* Check if this is user-mode or V86 */
+    if ((TrapFrame->Cs & 1) || (TrapFrame->Eflags & X86_EFLAGS_VM))
+    {
+        /* Return it directly */
+        return TrapFrame->Esp;
+    }
+    else
+    {
+        /* Edited frame */
+        if (!(TrapFrame->Cs & FRAME_EDITED))
+        {
+            /* Return edited value */
+            return TrapFrame->TempEsp;
+        }
+        else
+        {
+            /* Virgin frame, calculate */
+            return (ULONG)&TrapFrame->Esp;
+        }
+    }
+}
+
+VOID
+NTAPI
+KiEspToTrapFrame(IN PKTRAP_FRAME TrapFrame,
+                 IN ULONG Esp)
+{
+    ULONG Previous = KiEspFromTrapFrame(TrapFrame);
+
+    /* Check if this is user-mode or V86 */
+    if ((TrapFrame->Cs & 1) || (TrapFrame->Eflags & X86_EFLAGS_VM))
+    {
+        /* Write it directly */
+        TrapFrame->Esp = Esp;
+    }
+    else
+    {
+        /* Don't allow ESP to be lowered, this is illegal */
+        if (Esp < Previous)
+        {
+            //KeBugCheck(SET_OF_INVALID_CONTEXT);
+        }
+
+        /* Create an edit frame, check if it was alrady */
+        if (!(TrapFrame->Cs & FRAME_EDITED))
+        {
+            /* Update the value */
+            TrapFrame->TempEsp = Esp;
+        }
+        else
+        {
+            /* Check if ESP changed */
+            if (Previous != Esp)
+            {
+                /* Save CS */
+                TrapFrame->TempCs = TrapFrame->Cs;
+                TrapFrame->Cs &= ~FRAME_EDITED;
+
+                /* Save ESP */
+                TrapFrame->TempEsp = Esp;
+            }
+        }
+    }
+}
+
+ULONG
+NTAPI
+KiSsFromTrapFrame(IN PKTRAP_FRAME TrapFrame)
+{
+    /* If this was V86 Mode */
+    if (TrapFrame->Eflags & X86_EFLAGS_VM)
+    {
+        /* Just return it */
+        return TrapFrame->Ss;
+    }
+    else if (TrapFrame->Cs & 1)
+    {
+        /* Usermode, return the User SS */
+        return TrapFrame->Ss | 3;
+    }
+    else
+    {
+        /* Kernel mode */
+        return KERNEL_DS;
+    }
+}
+
+VOID
+NTAPI
+KiSsToTrapFrame(IN PKTRAP_FRAME TrapFrame,
+                IN ULONG Ss)
+{
+    /* Remove the high-bits */
+    Ss &= 0xFFFF;
+
+    /* If this was V86 Mode */
+    if (TrapFrame->Eflags & X86_EFLAGS_VM)
+    {
+        /* Just write it */
+        TrapFrame->Ss = Ss;
+    }
+    else if (TrapFrame->Cs & 1)
+    {
+        /* Usermode, save the User SS */
+        TrapFrame->Ss = Ss | 3;
+    }
+}
+
 BOOLEAN
-STDCALL
-KeContextToTrapFrame(PCONTEXT Context,
-                     PKEXCEPTION_FRAME ExceptionFrame,
-                     PKTRAP_FRAME TrapFrame)
+NTAPI
+KeContextToTrapFrame(IN PCONTEXT Context,
+                     IN OUT PKEXCEPTION_FRAME ExceptionFrame,
+                     IN OUT PKTRAP_FRAME TrapFrame,
+                     IN KPROCESSOR_MODE PreviousMode)
 {
+    BOOLEAN V86Switch = FALSE;
+
     /* Start with the basic Registers */
     if ((Context->ContextFlags & CONTEXT_CONTROL) == CONTEXT_CONTROL)
     {
-        TrapFrame->Esp = Context->Esp;
-        TrapFrame->Ss = Context->SegSs;
-        TrapFrame->Cs = Context->SegCs;
-        TrapFrame->Eip = Context->Eip;
+        /* Check if we went through a V86 switch */
+        if ((Context->EFlags & X86_EFLAGS_VM) !=
+            (TrapFrame->Eflags & X86_EFLAGS_VM))
+        {
+            /* We did, remember this for later */
+            V86Switch = TRUE;
+        }
+
+        /* Copy EFLAGS. FIXME: Needs to be sanitized */
         TrapFrame->Eflags = Context->EFlags;
+
+        /* Copy EBP and EIP */
         TrapFrame->Ebp = Context->Ebp;
+        TrapFrame->Eip = Context->Eip;
+
+        /* Check if we were in V86 Mode */
+        if (TrapFrame->Eflags & X86_EFLAGS_VM)
+        {
+            /* Simply copy the CS value */
+            TrapFrame->Cs = Context->SegCs;
+        }
+        else
+        {
+            /* We weren't in V86, so sanitize the CS (FIXME!) */
+            TrapFrame->Cs = Context->SegCs;
+
+            /* Don't let it under 8, that's invalid */
+            if ((PreviousMode !=KernelMode) && (TrapFrame->Cs < 8))
+            {
+                /* Force it to User CS */
+                TrapFrame->Cs = USER_CS;
+            }
+        }
+
+        /* Handle SS Specially for validation */
+        KiSsToTrapFrame(TrapFrame, Context->SegSs);
+
+        /* Write ESP back; take into account Edited Trap Frames */
+        KiEspToTrapFrame(TrapFrame, Context->Esp);
+
+        /* Handle our V86 Bias if we went through a switch */
+        if (V86Switch) Ki386AdjustEsp0(TrapFrame);
     }
 
     /* Process the Integer Registers */
@@ -597,21 +752,62 @@ KeContextToTrapFrame(PCONTEXT Context,
     /* Process the Context Segments */
     if ((Context->ContextFlags & CONTEXT_SEGMENTS) == CONTEXT_SEGMENTS)
     {
-        TrapFrame->Ds = Context->SegDs;
-        TrapFrame->Es = Context->SegEs;
-        TrapFrame->Fs = Context->SegFs;
-        TrapFrame->Gs = Context->SegGs;
+        /* Check if we were in V86 Mode */
+        if (TrapFrame->Eflags & X86_EFLAGS_VM)
+        {
+            /* Copy the V86 Segments directlry */
+            TrapFrame->V86_Ds = Context->SegDs;
+            TrapFrame->V86_Es = Context->SegEs;
+            TrapFrame->V86_Fs = Context->SegFs;
+            TrapFrame->V86_Gs = Context->SegGs;
+        }
+        else if (!(TrapFrame->Cs & 1))
+        {
+            /* For user mode, write the values directly */
+            TrapFrame->Ds = USER_DS;
+            TrapFrame->Es = USER_DS;
+            TrapFrame->Fs = Context->SegFs;
+            KEBUGCHECK(0);
+            TrapFrame->Gs = 0;
+        }
+        else
+        {
+            /* For kernel-mode, return the values */
+            TrapFrame->Ds = Context->SegDs;
+            TrapFrame->Es = Context->SegEs;
+            TrapFrame->Fs = Context->SegFs;
+
+            /* Handle GS specially */
+            if (TrapFrame->Cs == USER_CS)
+            {
+                /* Don't use it, if user */
+                TrapFrame->Gs = 0;
+            }
+            else
+            {
+                /* Copy it if kernel */
+                TrapFrame->Gs = Context->SegGs;
+            }
+        }
     }
 
     /* Handle the Debug Registers */
     if ((Context->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;
+
+        /* Check if usermode */
+        if (PreviousMode != KernelMode)
+        {
+            /* Set the Debug Flag */
+            KeGetCurrentThread()->DebugActive = (Context->Dr7 & DR7_ACTIVE);
+        }
     }
 
     /* Handle FPU and Extended Registers */
@@ -619,51 +815,90 @@ KeContextToTrapFrame(PCONTEXT Context,
 }
 
 VOID
-KeTrapFrameToContext(PKTRAP_FRAME TrapFrame,
-		     PCONTEXT Context)
+NTAPI
+KeTrapFrameToContext(IN PKTRAP_FRAME TrapFrame,
+                     IN PKEXCEPTION_FRAME ExceptionFrame,
+                     IN OUT PCONTEXT Context)
 {
-   if ((Context->ContextFlags & CONTEXT_CONTROL) == CONTEXT_CONTROL)
-     {
-	Context->SegSs = TrapFrame->Ss;
-	Context->Esp = TrapFrame->Esp;
-	Context->SegCs = TrapFrame->Cs;
-	Context->Eip = TrapFrame->Eip;
-	Context->EFlags = TrapFrame->Eflags;
-	Context->Ebp = TrapFrame->Ebp;
-     }
-   if ((Context->ContextFlags & CONTEXT_INTEGER) == CONTEXT_INTEGER)
-     {
-	Context->Eax = TrapFrame->Eax;
-	Context->Ebx = TrapFrame->Ebx;
-	Context->Ecx = TrapFrame->Ecx;
-	/*
-	 * NOTE: In the trap frame which is built on entry to a system
-	 * call TrapFrame->Edx will actually hold the address of the
-	 * previous TrapFrame. I don't believe leaking this information
-	 * has security implications. Also EDX holds the address of the
-	 * arguments to the system call in progress so it isn't of much
-	 * interest to the debugger.
-	 */
-	Context->Edx = TrapFrame->Edx;
-	Context->Esi = TrapFrame->Esi;
-	Context->Edi = TrapFrame->Edi;
-     }
-   if ((Context->ContextFlags & CONTEXT_SEGMENTS) == CONTEXT_SEGMENTS)
-     {
-	Context->SegDs = TrapFrame->Ds;
-	Context->SegEs = TrapFrame->Es;
-	Context->SegFs = TrapFrame->Fs;
-	Context->SegGs = TrapFrame->Gs;
-     }
+    /* Start with the Control flags */
+    if ((Context->ContextFlags & CONTEXT_CONTROL) == CONTEXT_CONTROL)
+    {
+        /* EBP, EIP and EFLAGS */
+        Context->Ebp = TrapFrame->Ebp;
+        Context->Eip = TrapFrame->Eip;
+        Context->EFlags = TrapFrame->Eflags;
+
+        /* Return the correct CS */
+        if (!(TrapFrame->Cs & FRAME_EDITED) &&
+            !(TrapFrame->Eflags & X86_EFLAGS_VM))
+        {
+            /* Get it from the Temp location */
+            Context->SegCs = TrapFrame->TempCs & 0xFFFF;
+        }
+        else
+        {
+            /* Return it directly */
+            Context->SegCs = TrapFrame->Cs & 0xFFFF;
+        }
+
+        /* Get the Ss and ESP */
+        Context->SegSs = KiSsFromTrapFrame(TrapFrame);
+        Context->Esp = KiEspFromTrapFrame(TrapFrame);
+    }
+
+    /* Handle the Segments */
+    if ((Context->ContextFlags & CONTEXT_SEGMENTS) == CONTEXT_SEGMENTS)
+    {
+        /* Do V86 Mode first */
+        if (TrapFrame->Eflags & X86_EFLAGS_VM)
+        {
+            /* Return from the V86 location */
+            Context->SegGs = TrapFrame->V86_Gs & 0xFFFF;
+            Context->SegFs = TrapFrame->V86_Fs & 0xFFFF;
+            Context->SegEs = TrapFrame->V86_Es & 0xFFFF;
+            Context->SegDs = TrapFrame->V86_Ds & 0xFFFF;
+        }
+        else
+        {
+            /* Check if this was a Kernel Trap */
+            if (TrapFrame->Cs == KERNEL_CS)
+            {
+                /* Set valid selectors */
+                TrapFrame->Gs = 0;
+                TrapFrame->Fs = PCR_SELECTOR;
+                TrapFrame->Es = USER_DS;
+                TrapFrame->Ds = USER_DS;
+            }
+
+            /* Return the segments */
+            Context->SegGs = TrapFrame->Gs & 0xFFFF;
+            Context->SegFs = TrapFrame->Fs & 0xFFFF;
+            Context->SegEs = TrapFrame->Es & 0xFFFF;
+            Context->SegDs = TrapFrame->Ds & 0xFFFF;
+        }
+    }
+
+    /* Handle the simple registers */
+    if ((Context->ContextFlags & CONTEXT_INTEGER) == CONTEXT_INTEGER)
+    {
+        /* Return them directly */
+        Context->Eax = TrapFrame->Eax;
+        Context->Ebx = TrapFrame->Ebx;
+        Context->Ecx = TrapFrame->Ecx;
+        Context->Edx = TrapFrame->Edx;
+        Context->Esi = TrapFrame->Esi;
+        Context->Edi = TrapFrame->Edi;
+    }
+
    if ((Context->ContextFlags & CONTEXT_DEBUG_REGISTERS) == CONTEXT_DEBUG_REGISTERS)
-     {
+   {
 	/*
 	 * FIXME: Implement this case
 	 */
 	Context->ContextFlags &= (~CONTEXT_DEBUG_REGISTERS) | CONTEXT_i386;
      }
    if ((Context->ContextFlags & CONTEXT_FLOATING_POINT) == CONTEXT_FLOATING_POINT)
-     {
+   {
 	/*
 	 * FIXME: Implement this case
 	 *
@@ -676,7 +911,7 @@ KeTrapFrameToContext(PKTRAP_FRAME TrapFrame,
      }
 #if 0
    if ((Context->ContextFlags & CONTEXT_EXTENDED_REGISTERS) == CONTEXT_EXTENDED_REGISTERS)
-     {
+   {
 	/*
 	 * FIXME: Investigate this
 	 *
@@ -931,7 +1166,7 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
     ULONG_PTR Stack, NewStack;
     ULONG Size;
     BOOLEAN UserDispatch = FALSE;
-    DPRINT1("KiDispatchException() called\n");
+    DPRINT("KiDispatchException() called\n");
 
     /* Increase number of Exception Dispatches */
     KeGetCurrentPrcb()->KeExceptionDispatchCount++;
@@ -947,7 +1182,7 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
     }
 
     /* Get a Context */
-    KeTrapFrameToContext(TrapFrame, &Context);
+    KeTrapFrameToContext(TrapFrame, ExceptionFrame, &Context);
 
     /* Handle kernel-mode first, it's simpler */
     if (PreviousMode == KernelMode)
@@ -1017,7 +1252,7 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
                 /* Align context size and get stack pointer */
                 Size = (sizeof(CONTEXT) + 3) & ~3;
                 Stack = (Context.Esp & ~3) - Size;
-                DPRINT1("Stack: %lx\n", Stack);
+                DPRINT("Stack: %lx\n", Stack);
 
                 /* Probe stack and copy Context */
                 ProbeForWrite((PVOID)Stack, Size, sizeof(ULONG));
@@ -1028,7 +1263,7 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
                         (EXCEPTION_MAXIMUM_PARAMETERS - ExceptionRecord->NumberParameters) *
                         sizeof(ULONG) + 3) & ~3;
                 NewStack = Stack - Size;
-                DPRINT1("NewStack: %lx\n", NewStack);
+                DPRINT("NewStack: %lx\n", NewStack);
 
                 /* Probe stack and copy exception record. Don't forget to add the two params */
                 ProbeForWrite((PVOID)(NewStack - 2 * sizeof(ULONG_PTR)),
@@ -1041,7 +1276,7 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
                 *(PULONG_PTR)(NewStack - 2 * sizeof(ULONG_PTR)) = NewStack;
 
                 /* Set new Stack Pointer */
-                TrapFrame->Esp = NewStack - 2 * sizeof(ULONG_PTR);
+                KiEspToTrapFrame(TrapFrame, NewStack - 2 * sizeof(ULONG_PTR));
 
                 /* Set EIP to the User-mode Dispathcer */
                 TrapFrame->Eip = (ULONG)KeUserExceptionDispatcher;
@@ -1073,7 +1308,7 @@ KiDispatchException(PEXCEPTION_RECORD ExceptionRecord,
 
 Handled:
     /* Convert the context back into Trap/Exception Frames */
-    KeContextToTrapFrame(&Context, NULL, TrapFrame);
+    KeContextToTrapFrame(&Context, NULL, TrapFrame, PreviousMode);
     return;
 }
 
diff --git a/reactos/ntoskrnl/ke/i386/fpu.c b/reactos/ntoskrnl/ke/i386/fpu.c
index c26bb1cf3a4..a66badffac8 100644
--- a/reactos/ntoskrnl/ke/i386/fpu.c
+++ b/reactos/ntoskrnl/ke/i386/fpu.c
@@ -528,7 +528,7 @@ KiHandleFpuFault(PKTRAP_FRAME Tf, ULONG ExceptionNr)
 
       /* Fill the rest of the context */
       Context->ContextFlags = CONTEXT_FULL;
-      KeTrapFrameToContext(Tf, Context);
+      KeTrapFrameToContext(Tf, NULL, Context);
       Context->ContextFlags |= CONTEXT_FLOATING_POINT | CONTEXT_EXTENDED_REGISTERS;
 
       /* Determine exception code */
diff --git a/reactos/ntoskrnl/ke/i386/syscall.S b/reactos/ntoskrnl/ke/i386/syscall.S
index 1854acd4d96..ddc40ff29ec 100644
--- a/reactos/ntoskrnl/ke/i386/syscall.S
+++ b/reactos/ntoskrnl/ke/i386/syscall.S
@@ -1,13 +1,8 @@
-/* $Id$
- *
+/*
  * FILE:            ntoskrnl/ke/i386/syscall.S
  * COPYRIGHT:       See COPYING in the top level directory
  * PURPOSE:         System Call Handler
  * PROGRAMMER:      Alex Ionescu (alex@relsoft.net)
- * UPDATE HISTORY:
- *                  ??-??-??: Original Version - David Welch(?)
- *                  13-01-05: Complete rewrite, added support for SYSENTER, direct kmode syscalls
- *                            and re-wrote most of handler code. - Alex Ionescu
  */
 
 #include <roscfg.h>
@@ -16,275 +11,491 @@
 #include <ndk/i386/segment.h>
 
 #define UserMode  (1)
+#define STATUS_INVALID_SYSTEM_SERVICE 0xC000001C
 
-.globl KeReturnFromSystemCallWithHook
 .globl _KiServiceExit
 .globl _KiServiceExit2
 .globl _KiFastCallEntry
 .globl _KiSystemService
 .globl _KiDebugService
+.intel_syntax noprefix
+
+/*
+ * NOTE: I will create some macros for trap entry and exit,
+ * DR* register restoration, modified frame exit, etc, if GAS
+ * allows it/I find a way how. This would remove a lot of
+ * duplicated code in this file plus the other irq/trap asm files.
+ * I think this is similar to what NT does, if you look at teh
+ * Dr_kit*_a functions which look auto-generated.
+ */
+
+/*
+ * There are 3 main types of Trap Exits:
+ *
+ * - KiServiceExit
+ *     - Clear interrupt flag
+ *     - Common User APC Dispatching
+ *     - Common exit code; segments and volatiles are not restored
+ * You use this for System Call return, when volatiles are irrelevant.
+ * (NtContinue, NtRaiseException, KiCallUserMode and all System Call returns)
+ *
+ * - KiServiceExit2
+ *     - Clear interrupt flag
+ *     - Common User APC Dispatching
+ *     - Common exit code; the entire frame is restored.
+ * You use this when volatiles and other registers need to be restored.
+ * For example, if debugging is active (NtContinue, NtRaiseException).
+ *
+ * - Kei386EoiHelper
+ *     - Clear interrupt flag
+ *     - Common User APC Dispatching
+ *     - Common exit code; the entire frame is restored but *NOT* the Previous Mode
+ * You use this in the same context as KiServiceExit2, but when the Previous Mode
+ * should be tampered with. Clearly, as its name suggests, this routine is mostly
+ * useful for End Of Interrupts.
+ * Note that this routine is EXPORTED.
+ * Note that this routine must called by a JMP, not a CALL.
+ */
+
+/*
+ * The common exit code has 3 modes of operation:
+ *    - Whether or not to restore segments
+ *    - Whether or not to restore volatiles
+ *    - Whether or not to restore the previous mode
+ * All these are exemplified by the 3 trap exits shown above
+ */
+
+/*
+ * There is also common Debug Code present in the common exit code, which
+ * in turn calls common code to save the debug registers
+ */
+ 
+ /*
+  * FIXMEs:
+  *         - Fix Win32k Callbacks so we can optimize stack usage.
+  *         - Dig in trap code and see why we need to push/pop the segments,
+  *           which -shouldn't- be needed on syscalls; one of the things
+  *           missing for this to work is lazy loading in the GPF handler,
+  *           but there are other things to consider.
+  *         - Use macros and merge with trap.s nicely
+  */
+
+/*
+ * Entries will be discussed later.
+ */
+ 
+ /*** This file is a mess; it is being worked on. Please contact Alex:
+  *** alex@relsoft.net if you want to make any changes to it before this
+  *** message goes away
+  */
+
+/* FUNCTIONS ***************************************************************/
+
+BadStack:
+
+    /* Restore ESP0 stack */
+    mov ecx, [fs:KPCR_TSS]
+    mov esp, ss:[ecx+KTSS_ESP0]
+
+    /* FIXME: Generate Trap 6*/
+    int 3
 
 _KiFastCallEntry:
 
+// ==================== UNIQUE SYSENTER STUB. DO NOT DUPLICATE ============//
     /* Set FS to PCR */
-    movl $PCR_SELECTOR, %ecx
-    movw %cx, %fs
+    mov ecx, PCR_SELECTOR
+    mov fs, cx
+
+    /* Set DS/ES to Kernel Selector */
+    mov ecx, KERNEL_DS
+    mov ds, cx
+    mov es, cx
 
     /* Set the current stack to Kernel Stack */
-    movl %fs:KPCR_TSS, %ecx
-    movl %ss:KTSS_ESP0(%ecx), %ecx
-    movl %ecx, %esp
+    mov ecx, [fs:KPCR_TSS]
+    mov esp, ss:[ecx+KTSS_ESP0]
+    /*
+     * ^^^^^ Normally we should be able to use KTHREAD.InitialStack - sizeof
+     *       KTRAP_FRAME - sizeof(NPX_AREA) and that will give us exactly
+     *       our esp (ie: ebp == esp by the way the system is organized).
+     *       This didn't work until I added v86m trap frame bias, which was
+     *       needed anways. It works NICELY until we hit a Win32K Callback,
+     *       which also messes up a lea esp, [ebp+...] below. It seems the
+     *       stack for callbacks isn't properly set up (it's missing sizeof
+     *       KTRAP_FRAME), which is understandable since the whole code is
+     *       messed up and smashes new stacks instead of using the kernel
+     *       assigned one.
+     */
 
     /* Set up a fake INT Stack. */
-    pushl $USER_DS
-    pushl %edx                  /* Ring 3 SS:ESP */
-    pushfl
-    orl $X86_EFLAGS_IF, (%esp)  /* Re-enable IRQs in EFLAGS, to fake INT */
-    pushl $USER_CS
-    pushl $KUSER_SHARED_SYSCALL_RET
+    push USER_DS
+    push edx                            /* Ring 3 SS:ESP */
+    pushf                               /* Ring 3 EFLAGS */
+    push 2                              /* Ring 0 EFLAGS */
+    add edx, 8                          /* Skip user parameter list */
+    popf                                /* Set our EFLAGS */
+    or dword ptr [esp], X86_EFLAGS_IF   /* Re-enable IRQs in EFLAGS, to fake INT */
+    push USER_CS
+    push KUSER_SHARED_SYSCALL_RET
+
+    /* Setup the Trap Frame stack */
+    push 0
+    push ebp
+    push ebx
+    push esi
+    push edi
+    push TEB_SELECTOR
+
+    /* Save pointer to our PCR */
+    mov ebx, [fs:KPCR_SELF]
+
+    /* Get a pointer to the current thread */
+    mov esi, [ebx+KPCR_CURRENT_THREAD]
+
+    /* Set the exception handler chain terminator */
+    push [ebx+KPCR_EXCEPTION_LIST]
+    mov dword ptr [ebx+KPCR_EXCEPTION_LIST], -1
+
+    /* Push previous mode */
+    push UserMode
+
+.att_syntax
+    /* Save other registers */
+    sub $0xC, %esp                                                    // + 0x70
+    pushl $USER_DS                                                          // + 0x40
+    pushl $USER_DS                                                          // + 0x44
+    pushl $0                                                          // + 0x48
+    sub $0x30, %esp                                                    // + 0x70
+.intel_syntax noprefix
 
-    /* User Parameter List */
-    add $8, %edx
+    /* Write the previous mode */
+    mov byte ptr [esi+KTHREAD_PREVIOUS_MODE], UserMode
+
+    /* Go on the Kernel stack frame */
+    mov ebp, esp
+
+    /* Flush DR7 */
+    and dword ptr [ebp+KTRAP_FRAME_DR7], 0
+
+    /* Check if the thread was being debugged */
+    test byte ptr [esi+KTHREAD_DEBUG_ACTIVE], 0xFF
+
+    /* Jump to shared code or DR Save */
+    //jnz Dr_FastCallDrSave
+    jmp SharedCode
 
 _KiSystemService:
 
-    /*
-     * Construct a trap frame on the stack.
-     * The following are already on the stack.
-     */
-    // SS                                                                  + 0x0
-    // ESP                                                                 + 0x4
-    // EFLAGS                                                              + 0x8
-    // CS                                                                  + 0xC
-    // EIP                                                                 + 0x10
-    pushl $0                                                            // + 0x14
-    pushl %ebp                                                          // + 0x18
-    pushl %ebx                                                          // + 0x1C
-    pushl %esi                                                          // + 0x20
-    pushl %edi                                                          // + 0x24
-    pushl %fs                                                           // + 0x28
+// ==================== UNIQUE SYSCALL TRAP ENTRY DO NOT DUPLICATE ============//
+    /* Create a trap frame */
+    push 0
+    push ebp
+    push ebx
+    push esi
+    push edi
+    push fs
 
     /* Load PCR Selector into fs */
-    movw $PCR_SELECTOR, %bx
-    movw %bx, %fs
+    mov ebx, PCR_SELECTOR
+    mov fs, bx
+
+    /* Get a pointer to the current thread */
+    mov esi, [fs:KPCR_CURRENT_THREAD]
 
     /* Save the previous exception list */
-    pushl %fs:KPCR_EXCEPTION_LIST                                      // + 0x2C
+    push [fs:KPCR_EXCEPTION_LIST]
 
     /* Set the exception handler chain terminator */
-    movl $0xffffffff, %fs:KPCR_EXCEPTION_LIST
-
-    /* Get a pointer to the current thread */
-    movl %fs:KPCR_CURRENT_THREAD, %esi
+    mov dword ptr [fs:KPCR_EXCEPTION_LIST], -1
 
     /* Save the old previous mode */
-    pushl %ss:KTHREAD_PREVIOUS_MODE(%esi)                              // + 0x30
-
-    /* Set the new previous mode based on the saved CS selector */
-    movl 0x24(%esp), %ebx
-    andl $1, %ebx
-    movb %bl, %ss:KTHREAD_PREVIOUS_MODE(%esi)
-
+    push ss:[esi+KTHREAD_PREVIOUS_MODE]
+    
+.att_syntax
     /* Save other registers */
-    pushl %eax                                                         // + 0x34
-    pushl %ecx                                                         // + 0x38
-    pushl %edx                                                         // + 0x3C
+    sub $0xC, %esp                                                    // + 0x70
     pushl %ds                                                          // + 0x40
     pushl %es                                                          // + 0x44
     pushl %gs                                                          // + 0x48
-    sub $0x28, %esp                                                    // + 0x70
-
-#ifdef DBG
-    /* Trick gdb 6 into backtracing over the system call */
-    mov 0x6c(%esp), %ebx
-    pushl 4(%ebx)    /* DebugEIP */                                    // + 0x74
-#else
-    pushl 0x60(%esp) /* DebugEIP */                                    // + 0x74
-#endif
-    pushl %ebp       /* DebugEBP */                                    // + 0x78
+    sub $0x30, %esp                                                    // + 0x70
+.intel_syntax noprefix
 
-    /* Load the segment registers */
-    sti
-    movw $KERNEL_DS, %bx
-    movw %bx, %ds
-    movw %bx, %es
+    /* Set the new previous mode based on the saved CS selector */
+    mov ebx, [esp+0x6C]
+    and ebx, 1
+    mov byte ptr ss:[esi+KTHREAD_PREVIOUS_MODE], bl
+
+    /* Go on the Kernel stack frame */
+    mov ebp, esp
 
     /* Save the old trap frame pointer where EDX would be saved */
-    movl KTHREAD_TRAP_FRAME(%esi), %ebx
-    movl %ebx, KTRAP_FRAME_EDX(%esp)
+    mov ebx, [esi+KTHREAD_TRAP_FRAME]
+    mov [ebp+KTRAP_FRAME_EDX], ebx
+
+// ==================== COMMON DR SAVE CHECK.AND DEBUG FRAME SETUP ============//
+    /* Flush DR7 */
+    and dword ptr [ebp+KTRAP_FRAME_DR7], 0
 
-    /* Allocate new Kernel stack frame */
-    movl %esp,%ebp
+    /* Check if the thread was being debugged */
+    test byte ptr [esi+KTHREAD_DEBUG_ACTIVE], 0xFF
+    cld
+    //jnz Dr_kss_a
 
     /* Save a pointer to the trap frame in the TCB */
-    movl %ebp, KTHREAD_TRAP_FRAME(%esi)
+SharedCode:
+    mov [esi+KTHREAD_TRAP_FRAME], ebp
 
-CheckValidCall:
+    /* Get the Debug Trap Frame EBP/EIP */
+    mov ebx, [ebp+KTRAP_FRAME_EBP]
+    mov edi, [ebp+KTRAP_FRAME_EIP]
 
-#ifdef DBG
-    /*
-     * GDB thinks the function starts here and
-     * wants a standard prolog, so let's give it
-     */
-    pushl %ebp
-    movl %esp,%ebp
-    popl %ebp
-#endif
+    /* Write the debug data */
+    mov [ebp+KTRAP_FRAME_DEBUGPOINTER], edx
+    mov dword ptr [ebp+KTRAP_FRAME_DEBUGARGMARK], 0xBADB0D00
+    mov [ebp+KTRAP_FRAME_DEBUGEBP], ebx
+    mov [ebp+KTRAP_FRAME_DEBUGEIP], edi
+
+// ============= END OF COMMON DR SAVE CHECK.AND DEBUG FRAME SETUP ============//
+    /* Enable interrupts */
+    sti
+
+CheckValidCall:
 
     /*
      * Find out which table offset to use. Converts 0x1124 into 0x10.
      * The offset is related to the Table Index as such: Offset = TableIndex x 10
      */
-    movl %eax, %edi
-    shrl $8, %edi
-    andl $0x10, %edi
-    movl %edi, %ecx
+    mov edi, eax
+    shr edi, 8
+    and edi, 0x10
+    mov ecx, edi
 
     /* Now add the thread's base system table to the offset */
-    addl KTHREAD_SERVICE_TABLE(%esi), %edi
+    add edi, [esi+KTHREAD_SERVICE_TABLE]
 
     /* Get the true syscall ID and check it */
-    movl %eax, %ebx
-    andl $0x0FFF, %eax
-    cmpl 8(%edi), %eax
+    mov ebx, eax
+    and eax, 0xFFF
+    cmp eax, [edi+8]
 
     /* Invalid ID, try to load Win32K Table */
     jnb KiBBTUnexpectedRange
 
+#if 0 // <== Disabled for two reasons: We don't save TEB in 0x18, but KPCR.
+      // <== We don't have a KeGdiFlushUserBatch callback yet (needs to be
+      //     sent through the PsInitializeWin32Callouts structure)
+    /* Check if this was Win32K */
+    cmp ecx, 0x10
+    jnz NotWin32K
+
+    /* Get the TEB */
+    mov ecx, [fs:KPCR_TEB]
+
+    /* Check if we should flush the User Batch */
+    xor ebx, ebx
+    or ebx, [ecx+TEB_GDI_BATCH_COUNT]
+    jz NoWin32K
+
+    /* Flush it */
+    push edx
+    push eax
+    call [_KeGdiFlushUserBatch]
+    pop eax
+    pop edx
+#endif
+
+NotWin32K:
     /* Users's current stack frame pointer is source */
-    movl %edx, %esi
+    mov esi, edx
 
     /* Allocate room for argument list from kernel stack */
-    movl 12(%edi), %ecx
-    movb (%ecx, %eax), %cl
-    movzx %cl, %ecx
-
-    /* Allocate space on our stack */
-    subl %ecx, %esp
+    mov ebx, [edi+12]
+    xor ecx, ecx
+    mov cl, [eax+ebx]
 
     /* Get pointer to function */
-    movl (%edi), %edi
-    movl (%edi, %eax, 4), %eax
+    mov edi, [edi]
+    mov ebx, [edi+eax*4]
 
-    /* Copy the arguments from the user stack to our stack */
-    shr $2, %ecx
-    movl %esp, %edi
-    cld
+    /* Allocate space on our stack */
+    sub esp, ecx
+
+    /* 
+     * Copy the arguments from the user stack to our stack
+     * FIXME: This needs to be probed with MmSystemRangeStart
+     */
+    shr ecx, 2
+    mov edi, esp
     rep movsd
 
     /* Do the System Call */
-    call *%eax
-    movl %eax, KTRAP_FRAME_EAX(%ebp)
+    call ebx
 
     /* Deallocate the kernel stack frame  */
-    movl %ebp, %esp
+    mov esp, ebp
 
 KeReturnFromSystemCall:
 
     /* Get the Current Thread */
-    movl %fs:KPCR_CURRENT_THREAD, %esi
+    mov ecx, [fs:KPCR_CURRENT_THREAD]
 
     /* Restore the old trap frame pointer */
-    movl KTRAP_FRAME_EDX(%esp), %ebx
-    movl %ebx, KTHREAD_TRAP_FRAME(%esi)
+    mov edx, [ebp+KTRAP_FRAME_EDX]
+    mov [ecx+KTHREAD_TRAP_FRAME], edx
 
 _KiServiceExit:
-
-    /* Get the Current Thread */
+    /* Disable interrupts */
     cli
-    movl %fs:KPCR_CURRENT_THREAD, %esi
+
+// ================= COMMON USER-MODE APC DELIVERY CHECK ============//
+    /* Check for V86 mode */
+    test dword ptr [ebp+KTRAP_FRAME_EFLAGS], X86_EFLAGS_VM
+    jnz ApcLoop
 
     /* Deliver APCs only if we were called from user mode */
-    testb $1, KTRAP_FRAME_CS(%esp)
+    test byte ptr [ebp+KTRAP_FRAME_CS], 1
     je KiRosTrapReturn
 
+    /* Get the current thread */
+ApcLoop:
+    mov ebx, [fs:KPCR_CURRENT_THREAD]
+
+    /* Make it non-alerted */
+    mov byte ptr [ebx+KTHREAD_ALERTED], 0
+
     /* And only if any are actually pending */
-    cmpb $0, KTHREAD_PENDING_USER_APC(%esi)
+    cmp byte ptr [ebx+KTHREAD_PENDING_USER_APC], 0
     je KiRosTrapReturn
 
     /* Save pointer to Trap Frame */
-    movl %esp, %ebx
+    mov ebx, ebp
+
+// ================= PRESENT ONLY IF VOLATILES NEEDED ============//
+    /* Save some stuff that raising IRQL will kill */
+    mov [ebx+KTRAP_FRAME_EAX], eax
+    mov dword ptr [ebx+KTRAP_FRAME_FS], TEB_SELECTOR
+    mov dword ptr [ebx+KTRAP_FRAME_DS], USER_DS
+    mov dword ptr [ebx+KTRAP_FRAME_ES], USER_DS
+    mov dword ptr [ebx+KTRAP_FRAME_GS], 0
+// ============= END PRESENT ONLY IF VOLATILES NEEDED ============//
 
     /* Raise IRQL to APC_LEVEL */
-    movl $1, %ecx
+    mov ecx, 1
     call @KfRaiseIrql@4
 
     /* Save old IRQL */
-    pushl %eax
+    push eax
 
     /* Deliver APCs */
     sti
-    pushl %ebx
-    pushl $0
-    pushl $UserMode
+    push ebx
+    push 0
+    push UserMode
     call _KiDeliverApc@12
-    cli
 
     /* Return to old IRQL */
-    popl %ecx
+    pop ecx
     call @KfLowerIrql@4
+    
+    /* Restore EAX (only in volatile case) */
+    mov eax, [ebx+KTRAP_FRAME_EAX]
+    cli
+    jmp ApcLoop
+// ============== END COMMON USER-MODE APC DELIVERY CHECK ============//
 
 KiRosTrapReturn:
-
+// ========================= COMMON TRAP EXIT CODE ===================//
+    /* Restore exception list */
+    mov edx, [esp+KTRAP_FRAME_EXCEPTION_LIST]
+    mov [fs:KPCR_EXCEPTION_LIST], edx
+
+// ==================== ONLY IF PREVIOUS MODE NEEDED ==================//
+    /* Restore previous mode */
+    mov ecx, [esp+KTRAP_FRAME_PREVIOUS_MODE]
+    mov esi, [fs:KPCR_CURRENT_THREAD]
+    mov byte ptr [esi+KTHREAD_PREVIOUS_MODE], cl
+// ==================== END IF PREVIOUS MODE NEEDED ===================//
+
+    /* Check for V86 */
+    test dword ptr [esp+KTRAP_FRAME_EFLAGS], X86_EFLAGS_VM
+    jnz V86_Exit
+
+    /* Check if the frame was edited */
+    test word ptr [esp+KTRAP_FRAME_CS], FRAME_EDITED
+    jz EditedFrame
+
+// ==================== ONLY IF FULL RESTORE NEEDED ===================//
+    /* Check the old mode */
+    cmp word ptr [esp+KTRAP_FRAME_CS], USER_CS
+    bt word ptr [esp+KTRAP_FRAME_CS], 0
+    cmc
+    ja RestoreAll
+// ==================== END IF FULL RESTORE NEEDED ====================//
+
+ /* Skip debug information and unsaved registers */
+ //badbadbad
+    add esp, 0x30
+    pop gs
+    pop es
+    pop ds
+    add esp, 0x14
+//badbadbad
+
+    /* Restore FS */
+RestoreFs:
+    //lea esp, [ebp+KTRAP_FRAME_FS] <= BUG IN WIN32K CALLBACKS! STACK GETS SMASHED
+    pop fs
+
+CommonStackClean:
     /* Skip debug information and unsaved registers */
-    addl $0x30, %esp                                                   // + 0x48
-    popl %gs                                                           // + 0x44
-    popl %es                                                           // + 0x40
-    popl %ds                                                           // + 0x3C
-    popl %edx                                                          // + 0x38
-    popl %ecx                                                          // + 0x34
-    popl %eax                                                          // + 0x30
-
-    /* Restore the old previous mode */
-    popl %ebx                                                          // + 0x2C
-    movb %bl, %ss:KTHREAD_PREVIOUS_MODE(%esi)
-
-    /* Restore the old exception handler list */
-    popl %fs:KPCR_EXCEPTION_LIST                                       // + 0x28
-
-    /* Restore final registers from trap frame */
-    popl %fs                                                           // + 0x24
-    popl %edi                                                          // + 0x20
-    popl %esi                                                          // + 0x1C
-    popl %ebx                                                          // + 0x18
-    popl %ebp                                                          // + 0x14
-    add $4, %esp                                                       // + 0x10
+    //lea esp, [ebp+KTRAP_FRAME_EDI] <= BUG IN WIN32K CALLBACKS! STACK GETS SMASHED
+    pop edi
+    pop esi
+    pop ebx
+    pop ebp
+
+    /* Check for ABIOS */
+    cmp word ptr [esp+8], 0x80
+    ja AbiosExit
+
+AbiosReturn:
+    /* Pop error code */
+    add esp, 4
 
     /* Check if previous CS is from user-mode */
-    testl $1, 4(%esp)
+    test dword ptr [esp+4], 1
 
     /* It is, so use Fast Exit */
     jnz FastRet
 
-    /*
-     * Restore what the stub pushed, and return back to it.
-     * Note that we were CALLed, so the first thing on our stack is the ret EIP!
-     */
-    pop %edx                                                           // + 0x0C
-    pop %ecx                                                           // + 0x08
-    popf                                                               // + 0x04
-    jmp *%edx
+    /* Jump back to stub */
+    pop edx
+    pop ecx
+    popf
+    jmp edx
 
 IntRet:
 
     iret
 
 FastRet:
-
     /* Is SYSEXIT Supported/Wanted? */
-    cmpl $0, %ss:_KiFastSystemCallDisable
+    cmp dword ptr ss:[_KiFastSystemCallDisable], 0
+    jnz IntRet
+    test dword ptr [esp+8], X86_EFLAGS_TF
     jnz IntRet
 
     /* Restore FS to TIB */
-    mov $TEB_SELECTOR, %ecx
-    mov %ecx, %fs
-
+    mov ecx, TEB_SELECTOR
+    mov fs, ecx
+    
     /* We will be cleaning up the stack ourselves */
-    popl %edx                         /* New Ring 3 EIP */
-    add $0x4, %esp                    /* Skip Ring 3 DS */
-    andl $~X86_EFLAGS_IF, (%esp)      /* Remove IRQ hack from EFLAGS */
-    popfl                             /* Restore old EFLAGS */
-    popl %ecx                         /* Old Ring 3 SS:ESP */
+    pop edx                                 /* New Ring 3 EIP */
+    add esp, 4                              /* Skip Ring 3 DS */
+    and dword ptr [esp], ~X86_EFLAGS_IF     /* Remove IRQ hack from EFLAGS */
+    popf                                    /* Restore old EFLAGS */
+    pop ecx                                 /* Old Ring 3 SS:ESP */
 
     /*
      * At this point:
@@ -294,18 +505,72 @@ FastRet:
     sti
     sysexit
 
+AbiosExit:
+V86_Exit:
+    /* Not yet supported */
+    int 3
+
+RestoreAll:
+    /* Restore EAX */
+    mov eax, [esp+KTRAP_FRAME_EAX]
+
+    /* Skip registers */
+    add esp, 0x30
+
+    /* Restore segments and volatiles */
+    pop gs
+    pop es
+    pop ds
+    pop edx
+    pop ecx
+
+    /* Jump back to mainline code */
+    jmp RestoreFs
+
+EditedFrame:
+     /* Restore real CS value */
+     mov ebx, [esp+KTRAP_FRAME_TEMPCS]
+     mov [esp+KTRAP_FRAME_CS], ebx
+
+    /*
+     * If ESP was modified, then a special interrupt exit stack
+     * must be created to "update" ESP's value in a legal manner
+     */
+    mov ebx, [esp+KTRAP_FRAME_TEMPESP]
+    sub ebx, 0xC
+    mov [esp+KTRAP_FRAME_ERROR_CODE], ebx
+
+    /* Copy Interrupt Stack */
+    mov esi, [esp+KTRAP_FRAME_EFLAGS]
+    mov [ebx+8], esi
+    mov esi, [esp+KTRAP_FRAME_CS]
+    mov [ebx+4], esi
+    mov esi, [esp+KTRAP_FRAME_EIP]
+    mov [ebx], esi
+
+    /* Return */
+    add esp, KTRAP_FRAME_EDI
+    pop edi
+    pop esi
+    pop ebx
+    pop ebp
+    mov esp, [esp]
+    iret
+
 KiBBTUnexpectedRange:
 
     /* If this isn't a Win32K call, fail */
-    cmp $0x10, %ecx
+    cmp ecx, 0x10
     jne InvalidCall
 
     /* Set up Win32K Table */
-    pushl %edx
-    pushl %ebx
+    push edx
+    push ebx
     call _KiServiceCheck
-    popl %eax
-    popl %edx
+
+    /* FIXME: Handle failure */
+    pop eax
+    pop edx
 
     /* Try the Call again */
     jmp CheckValidCall
@@ -313,90 +578,145 @@ KiBBTUnexpectedRange:
 InvalidCall:
 
     /* Invalid System Call */
-    movl $0xC000001C, %eax /* STATUS_INVALID_SYSTEM_SERVICE */
-    movl %eax, KTRAP_FRAME_EAX(%ebp)
-    jmp _KiServiceExit
+    mov eax, STATUS_INVALID_SYSTEM_SERVICE
+    jmp KeReturnFromSystemCall
 
 _KiServiceExit2:
 
-    /* Get the Current Thread */
+    /* Disable interrupts */
     cli
-    movl %fs:KPCR_CURRENT_THREAD, %esi
+
+    /* Check for V86 mode */
+    test dword ptr [ebp+KTRAP_FRAME_EFLAGS], X86_EFLAGS_VM
+    jnz ApcLoop2
 
     /* Deliver APCs only if we were called from user mode */
-    testb $1, KTRAP_FRAME_CS(%esp)
-    je KiRosTrapReturn
+    test byte ptr [ebp+KTRAP_FRAME_CS], 1
+    je KiRosTrapReturn2
+
+    /* Get the current thread */
+ApcLoop2:
+    mov ebx, [fs:KPCR_CURRENT_THREAD]
+
+    /* Make it non-alerted */
+    mov byte ptr [ebx+KTHREAD_ALERTED], 0
 
     /* And only if any are actually pending */
-    cmpb $0, KTHREAD_PENDING_USER_APC(%esi)
-    je KiRosTrapReturn
+    cmp byte ptr [ebx+KTHREAD_PENDING_USER_APC], 0
+    je KiRosTrapReturn2
 
     /* Save pointer to Trap Frame */
-    movl %esp, %ebx
+    mov ebx, ebp
 
     /* Raise IRQL to APC_LEVEL */
-    movl $1, %ecx
+    mov ecx, 1
     call @KfRaiseIrql@4
 
     /* Save old IRQL */
-    pushl %eax
+    push eax
 
     /* Deliver APCs */
     sti
-    pushl %ebx
-    pushl $0
-    pushl $UserMode
+    push ebx
+    push 0
+    push UserMode
     call _KiDeliverApc@12
-    cli
 
     /* Return to old IRQL */
-    popl %ecx
+    pop ecx
     call @KfLowerIrql@4
+    cli
+    jmp ApcLoop2
+
+KiRosTrapReturn2:
+
+    /* Restore exception list */
+    mov edx, [esp+KTRAP_FRAME_EXCEPTION_LIST]
+    mov [fs:KPCR_EXCEPTION_LIST], edx
+
+    /* Restore previous mode */
+    mov ecx, [esp+KTRAP_FRAME_PREVIOUS_MODE]
+    mov esi, [fs:KPCR_CURRENT_THREAD]
+    mov byte ptr [esi+KTHREAD_PREVIOUS_MODE], cl
+
+    /* Check for V86 */
+    test dword ptr [esp+KTRAP_FRAME_EFLAGS], X86_EFLAGS_VM
+    jnz V86_Exit
 
-    /* Skip useless Debug Data */
-    addl $0x18, %esp                                                   // + 0x74
-
-    /* Restore Debug Registers */
-    popl %eax                                                          // + 0x5C
-    movl %eax, %dr0
-    popl %eax                                                          // + 0x58
-    movl %eax, %dr1
-    popl %eax                                                          // + 0x54
-    movl %eax, %dr2
-    popl %eax                                                          // + 0x50
-    movl %eax, %dr3
-    popl %eax                                                          // + 0x4C
-    movl %eax, %dr6
-    popl %eax                                                          // + 0x48
-    movl %eax, %dr7
-
-    /* Restore Registers */
-    popl %gs                                                           // + 0x44
-    popl %es                                                           // + 0x40
-    popl %ds                                                           // + 0x3C
-    popl %edx                                                          // + 0x38
-    popl %ecx                                                          // + 0x34
-    popl %eax                                                          // + 0x30
-
-    /* Restore the old previous mode */
-    popl %ebx                                                          // + 0x2C
-    movb %bl, %ss:KTHREAD_PREVIOUS_MODE(%esi)
-
-    /* Restore the old exception handler list */
-    popl %fs:KPCR_EXCEPTION_LIST                                       // + 0x28
-
-    /* Restore final registers from trap frame */
-    popl %fs                                                           // + 0x24
-    popl %edi                                                          // + 0x20
-    popl %esi                                                          // + 0x1C
-    popl %ebx                                                          // + 0x18
-    popl %ebp                                                          // + 0x14
-    add $4, %esp                                                       // + 0x10
-
-    /* Return to user-mode */
+    /* Check if the frame was edited */
+    test word ptr [esp+KTRAP_FRAME_CS], FRAME_EDITED
+    jz EditedFrame2
+
+    /* Restore volatiles */
+    mov edx, [esp+KTRAP_FRAME_EDX]
+    mov ecx, [esp+KTRAP_FRAME_ECX]
+    mov eax, [esp+KTRAP_FRAME_EAX]
+
+    /* Check if it was kernel */
+    cmp word ptr [ebp+KTRAP_FRAME_CS], KERNEL_CS
+    jz CommonStackClean2
+
+    /* Skip registers */
+    lea esp, [ebp+KTRAP_FRAME_GS]
+
+    /* Restore segments and volatiles */
+    pop gs
+    pop es
+    pop ds
+    lea esp, [ebp+KTRAP_FRAME_FS]
+    pop fs
+
+CommonStackClean2:
+    /* Skip debug information and unsaved registers */
+    lea esp, [ebp+KTRAP_FRAME_EDI]
+    pop edi
+    pop esi
+    pop ebx
+    pop ebp
+
+    /* Check for ABIOS */
+    cmp word ptr [esp+8], 0x80
+    ja AbiosExit
+
+    /* Pop error code and return */
+    add esp, 4
+    iret
+
+EditedFrame2:
+    /* Restore real CS value */
+    mov ebx, [esp+KTRAP_FRAME_TEMPCS]
+    mov [esp+KTRAP_FRAME_CS], ebx
+
+    /*
+     * If ESP was modified, then a special interrupt exit stack
+     * must be created to "update" ESP's value in a legal manner
+     */
+    mov ebx, [esp+KTRAP_FRAME_TEMPESP]
+    sub ebx, 0xC
+    mov [esp+KTRAP_FRAME_ERROR_CODE], ebx
+
+    /* Copy Interrupt Stack */
+    mov esi, [esp+KTRAP_FRAME_EFLAGS]
+    mov [ebx+8], esi
+    mov esi, [esp+KTRAP_FRAME_CS]
+    mov [ebx+4], esi
+    mov esi, [esp+KTRAP_FRAME_EIP]
+    mov [ebx], esi
+
+    /* Restore volatiles */
+    mov eax, [esp+KTRAP_FRAME_EAX]
+    mov edx, [esp+KTRAP_FRAME_EDX]
+    mov ecx, [esp+KTRAP_FRAME_ECX]
+
+    /* Return */
+    add esp, KTRAP_FRAME_EDI
+    pop edi
+    pop esi
+    pop ebx
+    pop ebp
+    mov esp, [esp]
     iret
 
-.intel_syntax noprefix
 _KiDebugService:
 
     /* Create the Trap Frame */
@@ -414,9 +734,8 @@ _KiDebugService:
     /* Save Exception List */
     push fs:[KPCR_EXCEPTION_LIST]
 
-    /* Use Old Previous Mode */
-    mov ebx, fs:[KPCR_CURRENT_THREAD]
-    push [ebx+KTHREAD_PREVIOUS_MODE]
+    /* Traps don't need the previous mode */
+    sub esp, 4
 
     /* Continue building the Trap Frame */
     push eax
@@ -425,39 +744,180 @@ _KiDebugService:
     push ds
     push es
     push gs
+    sub esp, 0x30
 
     /* Switch Segments to Kernel */
-    mov bx, KERNEL_DS
-    mov ds, bx
-    mov es, bx
+    mov ax, KERNEL_DS
+    mov ds, ax
+    mov es, ax
 
-    /* Save Debug Registers */
-    mov ebx, eax
-    mov eax, dr7
-    push eax
-    mov eax, dr6
-    push eax
-    mov eax, dr3
-    push eax
-    mov eax, dr2
-    push eax
-    mov eax, dr1
-    push eax
-    mov eax, dr0
-    push eax
-    mov eax, ebx
+    /* Set up frame */
+    mov ebp, esp
+
+    /* Check if this was from V86 Mode */
+    test dword ptr [ebp+KTRAP_FRAME_EFLAGS], X86_EFLAGS_VM
+    //jnz V86_kids
+
+    /* Get current thread */
+    mov ecx, [fs:KPCR_CURRENT_THREAD]
+    cld
+
+    /* Flush DR7 */
+    and dword ptr [ebp+KTRAP_FRAME_DR7], 0
 
-    /* Skip useless debug data */
-    sub esp, 0x18
+    /* Check if the thread was being debugged */
+    test byte ptr [ecx+KTHREAD_DEBUG_ACTIVE], 0xFF
+    //jnz Dr_kids
+
+    /* Get the Debug Trap Frame EBP/EIP */
+    mov ebx, [ebp+KTRAP_FRAME_EBP]
+    mov edi, [ebp+KTRAP_FRAME_EIP]
+
+    /* Write the debug data */
+    mov [ebp+KTRAP_FRAME_DEBUGPOINTER], edx
+    mov dword ptr [ebp+KTRAP_FRAME_DEBUGARGMARK], 0xBADB0D00
+    mov [ebp+KTRAP_FRAME_DEBUGEBP], ebx
+    mov [ebp+KTRAP_FRAME_DEBUGEIP], edi
 
     /* Call debug service dispatcher */
-    push edx
-    push ecx
-    push eax
+    push [ebp+KTRAP_FRAME_EDX]
+    push [ebp+KTRAP_FRAME_ECX]
+    push [ebp+KTRAP_FRAME_EAX]
     call _KdpServiceDispatcher@12
 
     /* Exit through common routine */
-    jmp _KiServiceExit2
+    jmp Kei386EoiHelper@0
+
+Kei386EoiHelper@0:
+
+    /* Disable interrupts */
+    cli
+
+    /* Check for V86 mode */
+    test dword ptr [ebp+KTRAP_FRAME_EFLAGS], X86_EFLAGS_VM
+    jnz ApcLoop3
+
+    /* Deliver APCs only if we were called from user mode */
+    test byte ptr [ebp+KTRAP_FRAME_CS], 1
+    je KiRosTrapReturn3
+
+    /* Get the current thread */
+ApcLoop3:
+    mov ebx, [fs:KPCR_CURRENT_THREAD]
+
+    /* Make it non-alerted */
+    mov byte ptr [ebx+KTHREAD_ALERTED], 0
+
+    /* And only if any are actually pending */
+    cmp byte ptr [ebx+KTHREAD_PENDING_USER_APC], 0
+    je KiRosTrapReturn3
+
+    /* Save pointer to Trap Frame */
+    mov ebx, ebp
+
+    /* Raise IRQL to APC_LEVEL */
+    mov ecx, 1
+    call @KfRaiseIrql@4
+
+    /* Save old IRQL */
+    push eax
+
+    /* Deliver APCs */
+    sti
+    push ebx
+    push 0
+    push UserMode
+    call _KiDeliverApc@12
+
+    /* Return to old IRQL */
+    pop ecx
+    call @KfLowerIrql@4
+    cli
+    jmp ApcLoop3
+
+KiRosTrapReturn3:
+
+    /* Restore exception list */
+    mov edx, [esp+KTRAP_FRAME_EXCEPTION_LIST]
+    mov [fs:KPCR_EXCEPTION_LIST], edx
+
+    /* Check for V86 */
+    test dword ptr [esp+KTRAP_FRAME_EFLAGS], X86_EFLAGS_VM
+    jnz V86_Exit
+
+    /* Check if the frame was edited */
+    test word ptr [esp+KTRAP_FRAME_CS], FRAME_EDITED
+    jz EditedFrame3
+
+    /* Restore volatiles */
+    mov edx, [esp+KTRAP_FRAME_EDX]
+    mov ecx, [esp+KTRAP_FRAME_ECX]
+    mov eax, [esp+KTRAP_FRAME_EAX]
+
+    /* Check if it was kernel */
+    cmp word ptr [ebp+KTRAP_FRAME_CS], KERNEL_CS
+    jz CommonStackClean3
+
+    /* Skip registers */
+    lea esp, [ebp+KTRAP_FRAME_GS]
+
+    /* Restore segments and volatiles */
+    pop gs
+    pop es
+    pop ds
+    lea esp, [ebp+KTRAP_FRAME_FS]
+    pop fs
+
+CommonStackClean3:
+    /* Skip debug information and unsaved registers */
+    lea esp, [ebp+KTRAP_FRAME_EDI]
+    pop edi
+    pop esi
+    pop ebx
+    pop ebp
+
+    /* Check for ABIOS */
+    cmp word ptr [esp+8], 0x80
+    ja AbiosExit
+
+    /* Pop error code and return */
+    add esp, 4
+    iret
+
+EditedFrame3:
+    /* Restore real CS value */
+    mov ebx, [esp+KTRAP_FRAME_TEMPCS]
+    mov [esp+KTRAP_FRAME_CS], ebx
+
+    /*
+     * If ESP was modified, then a special interrupt exit stack
+     * must be created to "update" ESP's value in a legal manner
+     */
+    mov ebx, [esp+KTRAP_FRAME_TEMPESP]
+    sub ebx, 0xC
+    mov [esp+KTRAP_FRAME_ERROR_CODE], ebx
+
+    /* Copy Interrupt Stack */
+    mov esi, [esp+KTRAP_FRAME_EFLAGS]
+    mov [ebx+8], esi
+    mov esi, [esp+KTRAP_FRAME_CS]
+    mov [ebx+4], esi
+    mov esi, [esp+KTRAP_FRAME_EIP]
+    mov [ebx], esi
+
+    /* Restore volatiles */
+    mov eax, [esp+KTRAP_FRAME_EAX]
+    mov edx, [esp+KTRAP_FRAME_EDX]
+    mov ecx, [esp+KTRAP_FRAME_ECX]
+
+    /* Return */
+    add esp, KTRAP_FRAME_EDI
+    pop edi
+    pop esi
+    pop ebx
+    pop ebp
+    mov esp, [esp]
+    iret
 
 .globl _NtRaiseException@12
 _NtRaiseException@12:
diff --git a/reactos/ntoskrnl/ke/i386/thread.c b/reactos/ntoskrnl/ke/i386/thread.c
index 2e69149a5d7..d0f92e16354 100644
--- a/reactos/ntoskrnl/ke/i386/thread.c
+++ b/reactos/ntoskrnl/ke/i386/thread.c
@@ -85,7 +85,7 @@ Ke386InitThreadWithContext(PKTHREAD Thread,
         TrapFrame = &InitFrame->TrapFrame;
 
         /* Set up a trap frame from the context. */
-        if (KeContextToTrapFrame(Context, NULL, TrapFrame))
+        if (KeContextToTrapFrame(Context, NULL, TrapFrame, UserMode))
         {
             Thread->NpxState = NPX_STATE_VALID;
         }
@@ -146,7 +146,9 @@ Ke386InitThreadWithContext(PKTHREAD Thread,
 
     /* And set up the Context Switch Frame */
     CtxSwitchFrame->RetEip = KiThreadStartup;
-    CtxSwitchFrame->Esp0 = (ULONG)Thread->InitialStack - sizeof(FX_SAVE_AREA);
+    CtxSwitchFrame->Esp0 = (ULONG_PTR)Thread->InitialStack -
+                                      sizeof(FX_SAVE_AREA) -
+                                      0x10;
     CtxSwitchFrame->ExceptionList = (PVOID)0xFFFFFFFF;
 
     /* Save back the new value of the kernel stack. */
diff --git a/reactos/ntoskrnl/ke/i386/trap.s b/reactos/ntoskrnl/ke/i386/trap.s
index b2c34b64ebb..952bdbc32fd 100644
--- a/reactos/ntoskrnl/ke/i386/trap.s
+++ b/reactos/ntoskrnl/ke/i386/trap.s
@@ -10,6 +10,7 @@
 
 #include <roscfg.h>
 #include <ndk/asm.h>
+#include <internal/i386/ke.h>
 #include <ndk/i386/segment.h>
 
 #define KernelMode 0
@@ -374,4 +375,37 @@ _KiCoprocessorError@0:
     /* Return to caller */
     ret
 
+.globl _Ki386AdjustEsp0@4
+_Ki386AdjustEsp0@4:
+
+    /* Get the current thread */
+    mov eax, [fs:KPCR_CURRENT_THREAD]
+
+    /* Get trap frame and stack */
+    mov edx, [esp+4]
+    mov eax, [eax+KTHREAD_INITIAL_STACK]
+
+    /* Check if V86 */
+    test dword ptr [edx+KTRAP_FRAME_EFLAGS], X86_EFLAGS_VM
+    jnz NoAdjust
+
+    /* Bias the stack */
+    sub eax, KTRAP_FRAME_V86_GS - KTRAP_FRAME_SS
+
+NoAdjust:
+    /* Skip FX Save Area */
+    sub eax, SIZEOF_FX_SAVE_AREA
+
+    /* Disable interrupts */
+    pushf
+    cli
+
+    /* Adjust ESP0 */
+    mov edx, [fs:KPCR_TSS]
+    mov ss:[edx+KTSS_ESP0], eax
+
+    /* Enable interrupts and return */
+    popf
+    ret 4
+
 /* EOF */
diff --git a/reactos/ntoskrnl/mm/i386/pfault.c b/reactos/ntoskrnl/mm/i386/pfault.c
index 0e1ad6af20a..08bcadcef6f 100644
--- a/reactos/ntoskrnl/mm/i386/pfault.c
+++ b/reactos/ntoskrnl/mm/i386/pfault.c
@@ -36,11 +36,11 @@ ULONG KiPageFaultHandler(PKTRAP_FRAME Tf, ULONG ExceptionNr)
    ASSERT(ExceptionNr == 14);
    
    /* Store the exception number in an unused field in the trap frame. */
-   Tf->DebugArgMark = (PVOID)14;
+   Tf->DebugArgMark = 14;
 
    /* get the faulting address */
    cr2 = Ke386GetCr2();
-   Tf->DebugPointer = (PVOID)cr2;
+   Tf->DebugPointer = cr2;
 
    /* it's safe to enable interrupts after cr2 has been saved */
    if (Tf->Eflags & (X86_EFLAGS_VM|X86_EFLAGS_IF))
diff --git a/reactos/ntoskrnl/ps/debug.c b/reactos/ntoskrnl/ps/debug.c
index 30d67610e84..9e994248f5e 100644
--- a/reactos/ntoskrnl/ps/debug.c
+++ b/reactos/ntoskrnl/ps/debug.c
@@ -22,6 +22,7 @@ typedef struct _GET_SET_CTX_CONTEXT {
     KAPC Apc;
     KEVENT Event;
     CONTEXT Context;
+    KPROCESSOR_MODE Mode;
 } GET_SET_CTX_CONTEXT, *PGET_SET_CTX_CONTEXT;
 
 
@@ -42,22 +43,24 @@ PspGetOrSetContextKernelRoutine(PKAPC Apc,
     PGET_SET_CTX_CONTEXT GetSetContext;
     PKEVENT Event;
     PCONTEXT Context;
+    KPROCESSOR_MODE Mode;
 
     /* Get the Context Structure */
     GetSetContext = CONTAINING_RECORD(Apc, GET_SET_CTX_CONTEXT, Apc);
     Context = &GetSetContext->Context;
     Event = &GetSetContext->Event;
+    Mode = GetSetContext->Mode;
 
     /* Check if it's a set or get */
     if (SystemArgument1) {
 
         /* Get the Context */
-        KeTrapFrameToContext(KeGetCurrentThread()->TrapFrame, Context);
+        KeTrapFrameToContext(KeGetCurrentThread()->TrapFrame, NULL, Context);
 
     } else {
 
         /* Set the Context */
-        KeContextToTrapFrame(Context, NULL, KeGetCurrentThread()->TrapFrame);
+        KeContextToTrapFrame(Context, NULL, KeGetCurrentThread()->TrapFrame, Mode);
     }
 
     /* Notify the Native API that we are done */
@@ -111,7 +114,7 @@ NtGetContextThread(IN HANDLE ThreadHandle,
              * I don't know if trying to get your own context makes much
              * sense but we can handle it more efficently.
              */
-            KeTrapFrameToContext(Thread->Tcb.TrapFrame, &GetSetContext.Context);
+            KeTrapFrameToContext(Thread->Tcb.TrapFrame, NULL, &GetSetContext.Context);
 
         } else {
 
@@ -120,6 +123,9 @@ NtGetContextThread(IN HANDLE ThreadHandle,
                               NotificationEvent,
                               FALSE);
 
+            /* Set the previous mode */
+            GetSetContext.Mode = PreviousMode;
+
             /* Initialize the APC */
             KeInitializeApc(&GetSetContext.Apc,
                             &Thread->Tcb,
@@ -221,7 +227,7 @@ NtSetContextThread(IN HANDLE ThreadHandle,
              * I don't know if trying to get your own context makes much
              * sense but we can handle it more efficently.
              */
-            KeContextToTrapFrame(&GetSetContext.Context, NULL, Thread->Tcb.TrapFrame);
+            KeContextToTrapFrame(&GetSetContext.Context, NULL, Thread->Tcb.TrapFrame, PreviousMode);
 
         } else {
 
@@ -230,6 +236,9 @@ NtSetContextThread(IN HANDLE ThreadHandle,
                               NotificationEvent,
                               FALSE);
 
+            /* Set the previous mode */
+            GetSetContext.Mode = PreviousMode;
+
             /* Initialize the APC */
             KeInitializeApc(&GetSetContext.Apc,
                             &Thread->Tcb,