diff --git a/reactos/include/ndk/asm.h b/reactos/include/ndk/asm.h
index 0047dfec50e..f76279bf86c 100644
--- a/reactos/include/ndk/asm.h
+++ b/reactos/include/ndk/asm.h
@@ -430,6 +430,7 @@ Author:
 #define STATUS_ACCESS_VIOLATION                 0xC0000005
 #define STATUS_INVALID_SYSTEM_SERVICE           0xC000001C
 #define STATUS_NO_CALLBACK_ACTIVE               0xC0000258
+#define STATUS_CALLBACK_POP_STACK               0xC0000423
 #define STATUS_ARRAY_BOUNDS_EXCEEDED            0xC000008C
 #define STATUS_ILLEGAL_INSTRUCTION              0xC000001D
 #define STATUS_BREAKPOINT                       0x80000003
diff --git a/reactos/lib/rtl/i386/debug_asm.S b/reactos/lib/rtl/i386/debug_asm.S
index d288ccdbf43..4a858df6ed1 100644
--- a/reactos/lib/rtl/i386/debug_asm.S
+++ b/reactos/lib/rtl/i386/debug_asm.S
@@ -10,23 +10,35 @@
 
 /* GLOBALS ****************************************************************/
 
-.globl  _DbgBreakPoint@0
-.globl  _DbgBreakPointWithStatus@4
-.globl  _DbgUserBreakPoint@0
-.globl  _DebugService@20
+.globl _DbgBreakPoint@0
+.globl _DbgBreakPointWithStatus@4
+.globl _DbgUserBreakPoint@0
+.globl _DebugService@20
+.globl _DbgBreakPointNoBugCheck@0
 
 /* FUNCTIONS ***************************************************************/
 
+.func DbgBreakPointNoBugCheck@0
+_DbgBreakPointNoBugCheck@0:
+    int 3
+    ret
+.endfunc
+
+.func DbgBreakPoint@0
 _DbgBreakPoint@0:
 _DbgUserBreakPoint@0:
     int 3
     ret
- 
+.endfunc
+
+.func DbgBreakPointWithStatus@4
 _DbgBreakPointWithStatus@4:
     mov eax, [esp+4]
     int 3
     ret 4
+.endfunc
 
+.func DebugService@20
 _DebugService@20:
 
     /* Setup the stack */
@@ -58,4 +70,4 @@ _DebugService@20:
     /* Return */
     leave
     ret 20
-
+.endfunc
diff --git a/reactos/ntoskrnl/ke/i386/cpu.S b/reactos/ntoskrnl/ke/i386/cpu.S
index 494ca6bbf0a..ea925e4e182 100644
--- a/reactos/ntoskrnl/ke/i386/cpu.S
+++ b/reactos/ntoskrnl/ke/i386/cpu.S
@@ -11,30 +11,19 @@
 #include <asm.h>
 .intel_syntax noprefix
 
-/* GLOBALS ****************************************************************/  
-
-
 /* FUNCTIONS ****************************************************************/
 
-.global _DbgBreakPointNoBugCheck@0
-.func DbgBreakPointNoBugCheck@0
-_DbgBreakPointNoBugCheck@0:
-    int 3
-    ret
-.endfunc
-
 .globl _KeFlushCurrentTb@0
 .func KeFlushCurrentTb@0
 _KeFlushCurrentTb@0:
+
     /* Check for global page support */
     test byte ptr [_Ke386GlobalPagesEnabled], 0xff
     jz .L1
 
     /* Modifying the PSE, PGE or PAE Flag in CR4 causes the TLB to be flushed */
     mov eax, cr4
-.att_syntax /* Older binutils versions don't support ~ for .intel_syntax */
-    and $~CR4_PGE, %eax
-.intel_syntax noprefix
+    and eax, ~CR4_PGE
     mov cr4, eax
     or eax, CR4_PGE
     mov cr4, eax
@@ -47,25 +36,4 @@ _KeFlushCurrentTb@0:
     ret
 .endfunc
 
-.globl _KiCoprocessorError@0
-.func KiCoprocessorError@0
-_KiCoprocessorError@0:
 
-    /* Get the NPX Thread's Initial stack */
-    mov eax, [fs:KPCR_NPX_THREAD]
-    mov eax, [eax+KTHREAD_INITIAL_STACK]
-
-    /* Make space for the FPU Save area */
-    sub eax, SIZEOF_FX_SAVE_AREA
-
-    /* Set the CR0 State */
-    mov dword ptr [eax+FN_CR0_NPX_STATE], 8
-
-    /* Update it */
-    mov eax, cr0
-    or eax, 8
-    mov cr0, eax
-
-    /* Return to caller */
-    ret
-.endfunc
diff --git a/reactos/ntoskrnl/ke/i386/trap.s b/reactos/ntoskrnl/ke/i386/trap.s
index 1434d7dc4a6..115d83bcbea 100644
--- a/reactos/ntoskrnl/ke/i386/trap.s
+++ b/reactos/ntoskrnl/ke/i386/trap.s
@@ -55,6 +55,7 @@ GENERATE_IDT_STUBS                  /* INT 30-FF: UNEXPECTED INTERRUPTS     */
 /* And special system-defined software traps:                               */
 .globl _NtRaiseException@12
 .globl _NtContinue@8
+.globl _KiCoprocessorError@0
 
 /* Interrupt template entrypoints                                           */
 .globl _KiInterruptTemplate
@@ -1472,6 +1473,28 @@ _KiSystemFatalException:
     ret
 .endfunc
 
+.func KiCoprocessorError@0
+_KiCoprocessorError@0:
+
+    /* Get the NPX Thread's Initial stack */
+    mov eax, [fs:KPCR_NPX_THREAD]
+    mov eax, [eax+KTHREAD_INITIAL_STACK]
+
+    /* Make space for the FPU Save area */
+    sub eax, SIZEOF_FX_SAVE_AREA
+
+    /* Set the CR0 State */
+    mov dword ptr [eax+FN_CR0_NPX_STATE], 8
+
+    /* Update it */
+    mov eax, cr0
+    or eax, 8
+    mov cr0, eax
+
+    /* Return to caller */
+    ret
+.endfunc
+
 /* UNEXPECTED INTERRUPT HANDLERS **********************************************/
 
 .globl _KiStartUnexpectedRange@0
diff --git a/reactos/ntoskrnl/ke/i386/usercall_asm.S b/reactos/ntoskrnl/ke/i386/usercall_asm.S
index 512cad0eed4..475234a7e7d 100644
--- a/reactos/ntoskrnl/ke/i386/usercall_asm.S
+++ b/reactos/ntoskrnl/ke/i386/usercall_asm.S
@@ -127,9 +127,7 @@ StackOk:
     mov [ebx+KTHREAD_CALLBACK_STACK], esp
 
     /* Align stack on 16-byte boundary */
-.att_syntax /* Older binutils versions don't support ~ for intel_syntax */
-    and $~15,%esp
-.intel_syntax noprefix
+    and esp, ~15
     mov edi, esp
 
     /* Set destination and origin NPX Areas */
@@ -178,13 +176,16 @@ DontBias:
     lea esi, [edx+KTRAP_FRAME_FS]
     rep movsd
 
-    /* VMWARE Hack because ES/DS sometimes gets smashed when returning to User-Mode. Investigate! */
-    mov dword ptr [esp+KTRAP_FRAME_DS], KGDT_R3_DATA + RPL_MASK
-    mov dword ptr [esp+KTRAP_FRAME_ES], KGDT_R3_DATA + RPL_MASK
+    /* Copy DR7 */
+    mov edi, [edx+KTRAP_FRAME_DR7]
+    test edi, ~DR7_RESERVED_MASK
+    mov [esp+KTRAP_FRAME_DR7], edi
 
-    /* FIXME: Copy debug registers if needed */
+    /* Check if we need to save debug registers */
+    jnz SaveDebug
 
     /* Get user-mode dispatcher address and set it as EIP */
+SetEip:
     mov eax, _KeUserCallbackDispatcher
     mov [esp+KTRAP_FRAME_EIP], eax
 
@@ -205,6 +206,15 @@ DontBias:
     /* Exit to user-mode */
     jmp _KiServiceExit
 
+SaveDebug:
+
+    /* Copy all 5 DRs */
+    mov ecx, 5
+    lea edi, [esp+KTRAP_FRAME_DR0]
+    lea esi, [edx+KTRAP_FRAME_DR0]
+    rep movsd
+    jmp SetEip
+
 GrowFailed:
     /* Restore registers */
     pop edi
@@ -294,11 +304,19 @@ _NtCallbackReturn@12:
     mov edx, [esi+FN_CR0_NPX_STATE]
     mov [ebx+FN_CR0_NPX_STATE], edx
 
-    /* Get saved trap frame and clear DR7 */
+    /* Check if we failed in user mode */
+    cmp ebp, STATUS_CALLBACK_POP_STACK
     mov edi, [ecx+CBSTACK_TRAP_FRAME]
+    jz UserFault
+
+CheckDebug:
+
+    /* Clear DR7 */
     and dword ptr [edi+KTRAP_FRAME_DR7], 0
 
-    /* FIXME: Restore debug regs */
+    /* Check if debugging was active */
+    test byte ptr [eax+KTHREAD_DEBUG_ACTIVE], 0xFF
+    jnz RestoreDebug
 
     /* Get TSS */
     mov edx, fs:[KPCR_TSS]
@@ -336,12 +354,41 @@ V86Ret:
     add esp, 8
     jmp edx
 
+UserFault:
+    /* Set size to copy */
+    mov ecx, (KTRAP_FRAME_V86_ES - KTRAP_FRAME_FS) / 4
+
+    /* Check if this was V86 mode */
+    mov esi, [eax+KTHREAD_TRAP_FRAME]
+    test dword ptr [esi+KTRAP_FRAME_EFLAGS], EFLAGS_V86_MASK
+
+    /* Save EDI and load destination */
+    mov edx, edi
+    lea edi, [edi+KTRAP_FRAME_FS]
+    jz NotV86
+    add ecx, 16 / 4
+
+NotV86:
+    /* Set source and copy */
+    lea esi, [esi+KTRAP_FRAME_FS]
+    rep movsd
+
+    /* Restore ECX and ECX */
+    mov ecx, [eax+KTHREAD_CALLBACK_STACK]
+    mov edi, edx
+    jmp CheckDebug
+
+RestoreDebug:
+
+    /* Not yet supported */
+    int 3
+    jmp $
+
 NoStack:
 
     /* Return failure */
     mov eax, STATUS_NO_CALLBACK_ACTIVE
     ret 12
-
 .endfunc
 
 /*++
@@ -450,5 +497,4 @@ V86Switch:
     pop edi
     pop esi
     ret 8
-
 .endfunc