Revert r45774 until I know why qemu is broken
authorTimo Kreuzer <timo.kreuzer@reactos.org>
Wed, 3 Mar 2010 16:30:56 +0000 (16:30 +0000)
committerTimo Kreuzer <timo.kreuzer@reactos.org>
Wed, 3 Mar 2010 16:30:56 +0000 (16:30 +0000)
svn path=/trunk/; revision=45792

reactos/ntoskrnl/include/internal/i386/asmmacro.S
reactos/ntoskrnl/include/internal/trap_x.h
reactos/ntoskrnl/ke/i386/cpu.c
reactos/ntoskrnl/ke/i386/trap.s
reactos/ntoskrnl/ke/i386/traphdlr.c

index 6ddb9d0..d8ca99d 100644 (file)
@@ -179,14 +179,6 @@ set_sane_segs:
         mov fs, ax
     endif
 
-#if DBG
-    /* Keep the frame chain intact */
-    mov eax, [esp + KTRAP_FRAME_EIP]
-    mov [esp + KTRAP_FRAME_DEBUGEIP], eax
-    mov [esp + KTRAP_FRAME_DEBUGEBP], ebp
-    mov ebp, esp
-#endif
-
     /* Set parameter 1 (ECX) to point to the frame */
     mov ecx, esp
 
@@ -195,157 +187,11 @@ set_sane_segs:
 
 ENDM
 
-MACRO(KiCallHandler, Handler)
-#if DBG
-    /* Use a call to get the return address for back traces */
-    call Handler
-#else
-    /* Use the faster jmp */
-    jmp Handler
-#endif
-    nop
-ENDM
-
 MACRO(TRAP_ENTRY, Trap, Flags)
 EXTERN @&Trap&Handler@4 :PROC
     PUBLIC _&Trap
     _&Trap:
     KiEnterTrap Flags
-    KiCallHandler @&Trap&Handler@4
-ENDM
-
-#define KI_RESTORE_EAX        HEX(001)
-#define KI_RESTORE_ECX_EDX    HEX(002)
-#define KI_RESTORE_FS         HEX(004)
-#define KI_RESTORE_SEGMENTS   HEX(008)
-#define KI_RESTORE_EFLAGS     HEX(010)
-#define KI_EXIT_SYSCALL       HEX(020)
-#define KI_EXIT_JMP           HEX(040)
-#define KI_EXIT_RET           HEX(080)
-#define KI_EXIT_IRET          HEX(100)
-#define KI_EDITED_FRAME       HEX(200)
-#define KI_RESTORE_VOLATILES  (KI_RESTORE_EAX OR KI_RESTORE_ECX_EDX)
-
-MACRO(KiTrapExitStub, Name, Flags)
-
-PUBLIC @&Name&@4
-@&Name&@4:
-
-    if (Flags AND KI_RESTORE_EFLAGS)
-
-        /* We will pop EFlags off the stack */
-        OffsetEsp = KTRAP_FRAME_EFLAGS
-
-    elseif (Flags AND KI_EXIT_IRET)
-
-        /* This is the IRET frame */
-        OffsetEsp = KTRAP_FRAME_EIP
-
-    else
-
-        OffsetEsp = 0
-
-    endif
-
-    if (Flags AND KI_EDITED_FRAME)
-
-        /* Load the requested ESP */
-        mov esp, [ecx + KTRAP_FRAME_TEMPESP]
-
-        /* Put return address on the new stack */
-        push [ecx + KTRAP_FRAME_EIP]
-
-        /* Put EFLAGS on the new stack */
-        push [ecx + KTRAP_FRAME_EFLAGS]
-
-    else
-
-        /* Point esp to an appropriate member of the frame */
-        lea esp, [ecx + OffsetEsp]
-
-    endif
-
-    /* Restore non volatiles */
-    mov ebx, [ecx + KTRAP_FRAME_EBX]
-    mov esi, [ecx + KTRAP_FRAME_ESI]
-    mov edi, [ecx + KTRAP_FRAME_EDI]
-    mov ebp, [ecx + KTRAP_FRAME_EBP]
-
-    if (Flags AND KI_RESTORE_EAX)
-
-        /* Restore eax */
-        mov eax, [ecx + KTRAP_FRAME_EAX]
-
-    endif
-
-    if (Flags AND KI_RESTORE_ECX_EDX)
-
-        /* Restore volatiles */
-        mov edx, [ecx + KTRAP_FRAME_EDX]
-        mov ecx, [ecx + KTRAP_FRAME_ECX]
-
-    elseif (Flags AND KI_EXIT_JMP)
-
-        /* Load return address into edx */
-        mov edx, [esp - OffsetEsp + KTRAP_FRAME_EIP]
-
-    elseif (Flags AND KI_EXIT_SYSCALL)
-
-        /* Set sysexit parameters */
-        mov edx, [esp - OffsetEsp + KTRAP_FRAME_EIP]
-        mov ecx, [esp - OffsetEsp + KTRAP_FRAME_ESP]
-
-        /* Keep interrupts disabled until the sti / sysexit */
-        and byte ptr [esp - OffsetEsp + KTRAP_FRAME_EFLAGS + 1], ~(EFLAGS_INTERRUPT_MASK >> 8)
-
-    endif
-
-    if (Flags AND KI_RESTORE_SEGMENTS)
-
-        /* Restore segments for user mode */
-        mov ds, [esp - OffsetEsp + KTRAP_FRAME_DS]
-        mov es, [esp - OffsetEsp + KTRAP_FRAME_ES]
-        mov gs, [esp - OffsetEsp + KTRAP_FRAME_GS]
-
-    endif
-
-    if ((Flags AND KI_RESTORE_FS) OR (Flags AND KI_RESTORE_SEGMENTS))
-
-        /* Restore user mode FS */
-        mov fs, [esp - OffsetEsp + KTRAP_FRAME_FS]
-
-    endif
-
-    if (Flags AND KI_RESTORE_EFLAGS)
-
-        /* Restore EFLAGS */
-        popf
-
-    endif
-
-    if (Flags AND KI_EXIT_SYSCALL)
-
-        /* Enable interrupts and return to user mode.
-           Both must follow directly after another to be "atomic". */
-        sti
-        sysexit
-
-    elseif (Flags AND KI_EXIT_IRET)
-
-        /* Return with iret */
-        iret
-
-    elseif (Flags AND KI_EXIT_JMP)
-
-        /* Return to kernel mode with a jmp */
-        jmp edx
-
-    elseif (Flags AND KI_EXIT_RET)
-
-        /* Return to kernel mode with a ret */
-        ret
-
-    endif
-
+    jmp @&Trap&Handler@4
 ENDM
 
index ccc98df..69ccde5 100644 (file)
@@ -8,8 +8,6 @@
 
 #pragma once
 
-//#define TRAP_DEBUG 1
-
 //
 // Unreachable code hint for GCC 4.5.x, older GCC versions, and MSVC
 //
 #define UNREACHABLE
 #endif
 
-//
-// Helper Code
-//
-BOOLEAN
-FORCEINLINE
-KiUserTrap(IN PKTRAP_FRAME TrapFrame)
-{
-    /* Anything else but Ring 0 is Ring 3 */
-    return (TrapFrame->SegCs & MODE_MASK);
-}
-
 //
 // Debug Macros
 //
@@ -90,20 +77,19 @@ KiFillTrapFrameDebug(IN PKTRAP_FRAME TrapFrame)
     TrapFrame->DbgArgPointer = TrapFrame->Edx;
     TrapFrame->DbgArgMark = 0xBADB0D00;
     TrapFrame->DbgEip = TrapFrame->Eip;
-    TrapFrame->DbgEbp = TrapFrame->Ebp;
-    TrapFrame->PreviousPreviousMode = -1;
+    TrapFrame->DbgEbp = TrapFrame->Ebp;   
 }
 
 VOID
 FORCEINLINE
 KiExitTrapDebugChecks(IN PKTRAP_FRAME TrapFrame,
-                      IN KTRAP_EXIT_SKIP_BITS SkipBits)
+                      IN KTRAP_STATE_BITS SkipBits)
 {
     /* Make sure interrupts are disabled */
     if (__readeflags() & EFLAGS_INTERRUPT_MASK)
     {
         DbgPrint("Exiting with interrupts enabled: %lx\n", __readeflags());
-        __debugbreak();
+        while (TRUE);
     }
     
     /* Make sure this is a real trap frame */
@@ -111,35 +97,35 @@ KiExitTrapDebugChecks(IN PKTRAP_FRAME TrapFrame,
     {
         DbgPrint("Exiting with an invalid trap frame? (No MAGIC in trap frame)\n");
         KiDumpTrapFrame(TrapFrame);
-        __debugbreak();
+        while (TRUE);
     }
     
     /* Make sure we're not in user-mode or something */
     if (Ke386GetFs() != KGDT_R0_PCR)
     {
         DbgPrint("Exiting with an invalid FS: %lx\n", Ke386GetFs());
-        __debugbreak();
+        while (TRUE);   
     }
     
     /* Make sure we have a valid SEH chain */
     if (KeGetPcr()->Tib.ExceptionList == 0)
     {
         DbgPrint("Exiting with NULL exception chain: %p\n", KeGetPcr()->Tib.ExceptionList);
-        __debugbreak();
+        while (TRUE);
     }
     
     /* Make sure we're restoring a valid SEH chain */
     if (TrapFrame->ExceptionList == 0)
     {
         DbgPrint("Entered a trap with a NULL exception chain: %p\n", TrapFrame->ExceptionList);
-        __debugbreak();
+        while (TRUE);
     }
     
     /* If we're ignoring previous mode, make sure caller doesn't actually want it */
     if ((SkipBits.SkipPreviousMode) && (TrapFrame->PreviousPreviousMode != -1))
     {
-        DbgPrint("Exiting a trap witout restoring previous mode, yet previous mode seems valid: %lx\n", TrapFrame->PreviousPreviousMode);
-        __debugbreak();
+        DbgPrint("Exiting a trap witout restoring previous mode, yet previous mode seems valid: %lx", TrapFrame->PreviousPreviousMode);
+        while (TRUE);
     }
 }
 
@@ -151,14 +137,14 @@ KiExitSystemCallDebugChecks(IN ULONG SystemCall,
     KIRQL OldIrql;
     
     /* Check if this was a user call */
-    if (KiUserTrap(TrapFrame))
+    if (KiUserMode(TrapFrame))
     {
         /* Make sure we are not returning with elevated IRQL */
         OldIrql = KeGetCurrentIrql();
         if (OldIrql != PASSIVE_LEVEL)
         {
             /* Forcibly put us in a sane state */
-            KeGetPcr()->Irql = PASSIVE_LEVEL;
+            KeGetPcr()->CurrentIrql = PASSIVE_LEVEL;
             _disable();
             
             /* Fail */
@@ -168,7 +154,7 @@ KiExitSystemCallDebugChecks(IN ULONG SystemCall,
                          0,
                          0);
         }
-#if 0
+        
         /* Make sure we're not attached and that APCs are not disabled */
         if ((KeGetCurrentThread()->ApcStateIndex != CurrentApcEnvironment) ||
             (KeGetCurrentThread()->CombinedApcDisable != 0))
@@ -180,7 +166,6 @@ KiExitSystemCallDebugChecks(IN ULONG SystemCall,
                          KeGetCurrentThread()->CombinedApcDisable,
                          0);
         }
-#endif
     }
 }
 #else
@@ -189,20 +174,334 @@ KiExitSystemCallDebugChecks(IN ULONG SystemCall,
 #define KiExitSystemCallDebugChecks(x, y)
 #endif
 
+//
+// Helper Code
+//
+BOOLEAN
+FORCEINLINE
+KiUserTrap(IN PKTRAP_FRAME TrapFrame)
+{
+    /* Anything else but Ring 0 is Ring 3 */
+    return (TrapFrame->SegCs & MODE_MASK);
+}
+
+//
+// "BOP" code used by VDM and V8086 Mode
+//
+VOID
+FORCEINLINE
+KiIssueBop(VOID)
+{
+    /* Invalid instruction that an invalid opcode handler must trap and handle */
+    asm volatile(".byte 0xC4\n.byte 0xC4\n");
+}
+
+VOID
+FORCEINLINE
+KiUserSystemCall(IN PKTRAP_FRAME TrapFrame)
+{
+    /*
+     * Kernel call or user call?
+     *
+     * This decision is made in inlined assembly because we need to patch
+     * the relative offset of the user-mode jump to point to the SYSEXIT
+     * routine if the CPU supports it. The only way to guarantee that a
+     * relative jnz/jz instruction is generated is to force it with the
+     * inline assembler.
+     */
+    asm volatile
+    (
+        "test $1, %0\n" /* MODE_MASK */
+        ".globl _KiSystemCallExitBranch\n_KiSystemCallExitBranch:\n"
+        "jnz _KiSystemCallExit\n"
+        :
+        : "r"(TrapFrame->SegCs)
+    );
+}
+
+//
+// Generates an Exit Epilog Stub for the given name
+//
+#define KI_FUNCTION_CALL            0x1
+#define KI_EDITED_FRAME             0x2
+#define KI_DIRECT_EXIT              0x4
+#define KI_FAST_SYSTEM_CALL_EXIT    0x8
+#define KI_SYSTEM_CALL_EXIT         0x10
+#define KI_SYSTEM_CALL_JUMP         0x20
+#define KiTrapExitStub(x, y)        VOID FORCEINLINE DECLSPEC_NORETURN x(IN PKTRAP_FRAME TrapFrame) { KiTrapExit(TrapFrame, y); UNREACHABLE; }
+#define KiTrapExitStub2(x, y)       VOID FORCEINLINE x(IN PKTRAP_FRAME TrapFrame) { KiTrapExit(TrapFrame, y); }
+
+//
+// How volatiles will be restored
+//
+#define KI_EAX_NO_VOLATILES         0x0
+#define KI_EAX_ONLY                 0x1
+#define KI_ALL_VOLATILES            0x2
+
+//
+// Exit mechanism to use
+//
+#define KI_EXIT_IRET                0x0
+#define KI_EXIT_SYSEXIT             0x1
+#define KI_EXIT_JMP                 0x2
+#define KI_EXIT_RET                 0x3
+
+//
+// Master Trap Epilog
+//
+VOID
+FORCEINLINE
+KiTrapExit(IN PKTRAP_FRAME TrapFrame,
+          IN ULONG Flags)
+{
+    ULONG FrameSize = FIELD_OFFSET(KTRAP_FRAME, Eip);
+    ULONG ExitMechanism = KI_EXIT_IRET, Volatiles = KI_ALL_VOLATILES, NonVolatiles = TRUE;
+    ULONG EcxField = FIELD_OFFSET(KTRAP_FRAME, Ecx), EdxField = FIELD_OFFSET(KTRAP_FRAME, Edx);
+    
+    /* System call exit needs a special label */
+    if (Flags & KI_SYSTEM_CALL_EXIT) __asm__ __volatile__
+    (
+        ".globl _KiSystemCallExit\n_KiSystemCallExit:\n"
+    );
+            
+    /* Start by making the trap frame equal to the stack */
+    __asm__ __volatile__
+    (
+        "movl %0, %%esp\n"
+        :
+        : "r"(TrapFrame)
+        : "%esp"
+    );
+        
+    /* Check what kind of trap frame this trap requires */
+    if (Flags & KI_FUNCTION_CALL)
+    {
+        /* These calls have an EIP on the stack they need */
+        ExitMechanism = KI_EXIT_RET;
+        Volatiles = FALSE;
+    }
+    else if (Flags & KI_EDITED_FRAME)
+    {
+        /* Edited frames store a new ESP in the error code field */
+        FrameSize = FIELD_OFFSET(KTRAP_FRAME, ErrCode);
+    }
+    else if (Flags & KI_DIRECT_EXIT)
+    {
+        /* Exits directly without restoring anything, interrupt frame on stack */
+        NonVolatiles = Volatiles = FALSE;
+    }
+    else if (Flags & KI_FAST_SYSTEM_CALL_EXIT)
+    {
+        /* We have a fake interrupt stack with a ring transition */
+        FrameSize = FIELD_OFFSET(KTRAP_FRAME, V86Es);
+        ExitMechanism = KI_EXIT_SYSEXIT;
+        
+        /* SYSEXIT wants EIP in EDX and ESP in ECX */
+        EcxField = FIELD_OFFSET(KTRAP_FRAME, HardwareEsp);
+        EdxField = FIELD_OFFSET(KTRAP_FRAME, Eip);
+    }
+    else if (Flags & KI_SYSTEM_CALL_EXIT)
+    {
+        /* Only restore EAX */
+        NonVolatiles = KI_EAX_ONLY;
+    }
+    else if (Flags & KI_SYSTEM_CALL_JUMP)
+    {
+        /* We have a fake interrupt stack with no ring transition */
+        FrameSize = FIELD_OFFSET(KTRAP_FRAME, HardwareEsp);
+        NonVolatiles = KI_EAX_ONLY;
+        ExitMechanism = KI_EXIT_JMP;
+    }
+    
+    /* Restore the non volatiles */
+    if (NonVolatiles) __asm__ __volatile__
+    (
+        "movl %c[b](%%esp), %%ebx\n"
+        "movl %c[s](%%esp), %%esi\n"
+        "movl %c[i](%%esp), %%edi\n"
+        "movl %c[p](%%esp), %%ebp\n"
+        :
+        : [b] "i"(FIELD_OFFSET(KTRAP_FRAME, Ebx)),
+          [s] "i"(FIELD_OFFSET(KTRAP_FRAME, Esi)),
+          [i] "i"(FIELD_OFFSET(KTRAP_FRAME, Edi)),
+          [p] "i"(FIELD_OFFSET(KTRAP_FRAME, Ebp))
+        : "%esp"
+    );
+    
+    /* Restore EAX if volatiles must be restored */
+    if (Volatiles) __asm__ __volatile__
+    (
+        "movl %c[a](%%esp), %%eax\n":: [a] "i"(FIELD_OFFSET(KTRAP_FRAME, Eax)) : "%esp"
+    );
+    
+    /* Restore the other volatiles if needed */
+    if (Volatiles == KI_ALL_VOLATILES) __asm__ __volatile__
+    (
+        "movl %c[c](%%esp), %%ecx\n"
+        "movl %c[d](%%esp), %%edx\n"
+        :
+        : [c] "i"(EcxField),
+          [d] "i"(EdxField)
+        : "%esp"
+    );
+    
+    /* Ring 0 system calls jump back to EDX */
+    if (Flags & KI_SYSTEM_CALL_JUMP) __asm__ __volatile__
+    (
+        "movl %c[d](%%esp), %%edx\n":: [d] "i"(FIELD_OFFSET(KTRAP_FRAME, Eip)) : "%esp"
+    );
+
+    /* Now destroy the trap frame on the stack */
+    __asm__ __volatile__ ("addl $%c[e],%%esp\n":: [e] "i"(FrameSize) : "%esp");
+    
+    /* Edited traps need to change to a new ESP */
+    if (Flags & KI_EDITED_FRAME) __asm__ __volatile__ ("movl (%%esp), %%esp\n":::"%esp");
+
+    /* Check the exit mechanism and apply it */
+    if (ExitMechanism == KI_EXIT_RET) __asm__ __volatile__("ret\n"::: "%esp");
+    else if (ExitMechanism == KI_EXIT_IRET) __asm__ __volatile__("iret\n"::: "%esp");
+    else if (ExitMechanism == KI_EXIT_JMP) __asm__ __volatile__("jmp *%%edx\n.globl _KiSystemCallExit2\n_KiSystemCallExit2:\n"::: "%esp");
+    else if (ExitMechanism == KI_EXIT_SYSEXIT) __asm__ __volatile__("sti\nsysexit\n"::: "%esp");   
+}
+
+//
+// All the specific trap epilog stubs
+//
+KiTrapExitStub (KiTrapReturn,              0);
+KiTrapExitStub (KiDirectTrapReturn,        KI_DIRECT_EXIT);
+KiTrapExitStub (KiCallReturn,              KI_FUNCTION_CALL);
+KiTrapExitStub (KiEditedTrapReturn,        KI_EDITED_FRAME);
+KiTrapExitStub2(KiSystemCallReturn,        KI_SYSTEM_CALL_JUMP);
+KiTrapExitStub (KiSystemCallSysExitReturn, KI_FAST_SYSTEM_CALL_EXIT);
+KiTrapExitStub (KiSystemCallTrapReturn,    KI_SYSTEM_CALL_EXIT);
+
 //
 // Generic Exit Routine
 //
-VOID FASTCALL DECLSPEC_NORETURN KiSystemCallReturn(IN PKTRAP_FRAME TrapFrame);
-VOID FASTCALL DECLSPEC_NORETURN KiSystemCallSysExitReturn(IN PKTRAP_FRAME TrapFrame);
-VOID FASTCALL DECLSPEC_NORETURN KiSystemCallTrapReturn(IN PKTRAP_FRAME TrapFrame);
-VOID FASTCALL DECLSPEC_NORETURN KiEditedTrapReturn(IN PKTRAP_FRAME TrapFrame);
-VOID FASTCALL DECLSPEC_NORETURN KiTrapReturn(IN PKTRAP_FRAME TrapFrame);
-VOID FASTCALL DECLSPEC_NORETURN KiTrapReturnNoSegments(IN PKTRAP_FRAME TrapFrame);
-
-typedef
 VOID
-(FASTCALL
-*FAST_SYSTEM_CALL_EXIT)(IN PKTRAP_FRAME TrapFrame) DECLSPEC_NORETURN;
+FORCEINLINE
+DECLSPEC_NORETURN
+KiExitTrap(IN PKTRAP_FRAME TrapFrame,
+           IN UCHAR Skip)
+{
+    KTRAP_EXIT_SKIP_BITS SkipBits = { .Bits = Skip };
+    PULONG ReturnStack;
+    
+    /* Debugging checks */
+    KiExitTrapDebugChecks(TrapFrame, SkipBits);
+
+    /* Restore the SEH handler chain */
+    KeGetPcr()->Tib.ExceptionList = TrapFrame->ExceptionList;
+    
+    /* Check if the previous mode must be restored */
+    if (__builtin_expect(!SkipBits.SkipPreviousMode, 0)) /* More INTS than SYSCALLs */
+    {
+        /* Restore it */
+        KeGetCurrentThread()->PreviousMode = TrapFrame->PreviousPreviousMode;
+    }
+
+    /* Check if there are active debug registers */
+    if (__builtin_expect(TrapFrame->Dr7 & ~DR7_RESERVED_MASK, 0))
+    {
+        /* Not handled yet */
+        DbgPrint("Need Hardware Breakpoint Support!\n");
+        DbgBreakPoint();
+        while (TRUE);
+    }
+    
+    /* Check if this was a V8086 trap */
+    if (__builtin_expect(TrapFrame->EFlags & EFLAGS_V86_MASK, 0)) KiTrapReturn(TrapFrame);
+
+    /* Check if the trap frame was edited */
+    if (__builtin_expect(!(TrapFrame->SegCs & FRAME_EDITED), 0))
+    {   
+        /*
+         * An edited trap frame happens when we need to modify CS and/or ESP but
+         * don't actually have a ring transition. This happens when a kernelmode
+         * caller wants to perform an NtContinue to another kernel address, such
+         * as in the case of SEH (basically, a longjmp), or to a user address.
+         *
+         * Therefore, the CPU never saved CS/ESP on the stack because we did not
+         * get a trap frame due to a ring transition (there was no interrupt).
+         * Even if we didn't want to restore CS to a new value, a problem occurs
+         * due to the fact a normal RET would not work if we restored ESP since
+         * RET would then try to read the result off the stack.
+         *
+         * The NT kernel solves this by adding 12 bytes of stack to the exiting
+         * trap frame, in which EFLAGS, CS, and EIP are stored, and then saving
+         * the ESP that's being requested into the ErrorCode field. It will then
+         * exit with an IRET. This fixes both issues, because it gives the stack
+         * some space where to hold the return address and then end up with the
+         * wanted stack, and it uses IRET which allows a new CS to be inputted.
+         *
+         */
+         
+        /* Set CS that is requested */
+        TrapFrame->SegCs = TrapFrame->TempSegCs;
+         
+        /* First make space on requested stack */
+        ReturnStack = (PULONG)(TrapFrame->TempEsp - 12);
+        TrapFrame->ErrCode = (ULONG_PTR)ReturnStack;
+         
+        /* Now copy IRET frame */
+        ReturnStack[0] = TrapFrame->Eip;
+        ReturnStack[1] = TrapFrame->SegCs;
+        ReturnStack[2] = TrapFrame->EFlags;
+        
+        /* Do special edited return */
+        KiEditedTrapReturn(TrapFrame);
+    }
+    
+    /* Check if this is a user trap */
+    if (__builtin_expect(KiUserTrap(TrapFrame), 1)) /* Ring 3 is where we spend time */
+    {
+        /* Check if segments should be restored */
+        if (!SkipBits.SkipSegments)
+        {
+            /* Restore segments */
+            Ke386SetGs(TrapFrame->SegGs);
+            Ke386SetEs(TrapFrame->SegEs);
+            Ke386SetDs(TrapFrame->SegDs);
+            Ke386SetFs(TrapFrame->SegFs);
+        }
+        
+        /* Always restore FS since it goes from KPCR to TEB */
+        Ke386SetFs(TrapFrame->SegFs);
+    }
+    
+    /* Check for system call -- a system call skips volatiles! */
+    if (__builtin_expect(SkipBits.SkipVolatiles, 0)) /* More INTs than SYSCALLs */
+    {
+        /* User or kernel call? */
+        KiUserSystemCall(TrapFrame);
+        
+        /* Restore EFLags */
+        __writeeflags(TrapFrame->EFlags);
+            
+        /* Call is kernel, so do a jump back since this wasn't a real INT */
+        KiSystemCallReturn(TrapFrame);
+
+        /* If we got here, this is SYSEXIT: are we stepping code? */
+        if (!(TrapFrame->EFlags & EFLAGS_TF))
+        {
+            /* Restore user FS */
+            Ke386SetFs(KGDT_R3_TEB | RPL_MASK);
+
+            /* Remove interrupt flag */
+            TrapFrame->EFlags &= ~EFLAGS_INTERRUPT_MASK;
+            __writeeflags(TrapFrame->EFlags);
+
+            /* Exit through SYSEXIT */
+            KiSystemCallSysExitReturn(TrapFrame);
+        }
+        
+        /* Exit through IRETD, either due to debugging or due to lack of SYSEXIT */
+        KiSystemCallTrapReturn(TrapFrame);
+    }
+    
+    /* Return from interrupt */
+    KiTrapReturn(TrapFrame);
+}
 
 //
 // Virtual 8086 Mode Optimized Trap Exit
@@ -218,9 +517,6 @@ KiExitV86Trap(IN PKTRAP_FRAME TrapFrame)
     Thread = KeGetCurrentThread();
     while (TRUE)
     {
-        /* Return if this isn't V86 mode anymore */
-        if (!(TrapFrame->EFlags & EFLAGS_V86_MASK)) KiEoiHelper(TrapFrame);;
-
         /* Turn off the alerted state for kernel mode */
         Thread->Alerted[KernelMode] = FALSE;
 
@@ -237,6 +533,9 @@ KiExitV86Trap(IN PKTRAP_FRAME TrapFrame)
         /* Restore IRQL and disable interrupts once again */
         KfLowerIrql(OldIrql);
         _disable();
+        
+        /* Return if this isn't V86 mode anymore */
+        if (__builtin_expect(TrapFrame->EFlags & EFLAGS_V86_MASK, 0)) return;
     }
      
     /* If we got here, we're still in a valid V8086 context, so quit it */
index ad8e21e..21d49c9 100644 (file)
@@ -995,8 +995,55 @@ KiLoadFastSyscallMachineSpecificRegisters(IN ULONG_PTR Context)
     return 0;
 }
 
-VOID FASTCALL DECLSPEC_NORETURN KiSystemCallSysExitReturn(IN PKTRAP_FRAME TrapFrame);
-extern PVOID KiFastCallExitHandler;
+VOID
+NTAPI
+KiDisableFastSyscallReturn(VOID)
+{
+    /* Was it applied? */
+    if (KiSystemCallExitAdjusted)
+    {        
+        /* Restore the original value */
+        KiSystemCallExitBranch[1] = KiSystemCallExitBranch[1] - KiSystemCallExitAdjusted;
+                
+        /* It's not adjusted anymore */
+        KiSystemCallExitAdjusted = FALSE;
+    }
+}
+
+VOID
+NTAPI
+KiEnableFastSyscallReturn(VOID)
+{
+    /* Check if the patch has already been done */
+    if ((KiSystemCallExitAdjusted == KiSystemCallExitAdjust) &&
+        (KiFastCallCopyDoneOnce))
+    {
+        return;
+    }
+    
+    /* Make sure the offset is within the distance of a Jxx SHORT */
+    if ((KiSystemCallExitBranch[1] - KiSystemCallExitAdjust) < 0x80)
+    {
+        /* Remove any existing code patch */
+        KiDisableFastSyscallReturn();
+        
+        /* We should have a JNZ there */
+        ASSERT(KiSystemCallExitBranch[0] == 0x75);
+
+        /* Do the patch */        
+        KiSystemCallExitAdjusted = KiSystemCallExitAdjust;
+        KiSystemCallExitBranch[1] -= KiSystemCallExitAdjusted;
+        
+        /* Remember that we've done it */
+        KiFastCallCopyDoneOnce = TRUE;
+    }
+    else
+    {
+        /* This shouldn't happen unless we've messed the macros up */
+        DPRINT1("Your compiled kernel is broken!\n");
+        DbgBreakPoint();
+    }
+}
 
 VOID
 NTAPI
@@ -1008,11 +1055,11 @@ KiRestoreFastSyscallReturnState(VOID)
         /* Check if it has been disabled */
         if (!KiFastSystemCallDisable)
         {
-            /* Do an IPI to enable it */
-            KeIpiGenericCall(KiLoadFastSyscallMachineSpecificRegisters, 0);
-
-            /* It's enabled, so use the proper exit stub */
-            KiFastCallExitHandler = KiSystemCallSysExitReturn;
+            /* KiSystemCallExit2 should come BEFORE KiSystemCallExit */
+            ASSERT(KiSystemCallExit2 < KiSystemCallExit);
+            
+            /* It's enabled, so we'll have to do a code patch */
+            KiSystemCallExitAdjust = KiSystemCallExit - KiSystemCallExit2;
         }
         else
         {
@@ -1020,6 +1067,16 @@ KiRestoreFastSyscallReturnState(VOID)
             KeFeatureBits &= ~KF_FAST_SYSCALL;
         }
     }
+    
+    /* Now check if all CPUs support fast system call, and the registry allows it */
+    if (KeFeatureBits & KF_FAST_SYSCALL)
+    {
+        /* Do an IPI to enable it */
+        KeIpiGenericCall(KiLoadFastSyscallMachineSpecificRegisters, 0);
+    }
+    
+    /* Perform the code patch that is required */
+    KiEnableFastSyscallReturn();
 }
 
 ULONG_PTR
index df7bc6a..5f24877 100644 (file)
@@ -120,18 +120,17 @@ _KiInterruptTemplateObject:
 PUBLIC _KiInterruptTemplateDispatch
 _KiInterruptTemplateDispatch:
 
-EXTERN @KiFastCallEntryHandler@8:PROC
-PUBLIC _KiFastCallEntry
-_KiFastCallEntry:
-    KiEnterTrap (KI_FAST_SYSTEM_CALL OR KI_NONVOLATILES_ONLY OR KI_DONT_SAVE_SEGS)
-    KiCallHandler @KiFastCallEntryHandler@8
-
-
 EXTERN @KiSystemServiceHandler@8:PROC
 PUBLIC _KiSystemService
 _KiSystemService:
     KiEnterTrap (KI_PUSH_FAKE_ERROR_CODE OR KI_NONVOLATILES_ONLY OR KI_DONT_SAVE_SEGS)
-    KiCallHandler @KiSystemServiceHandler@8
+    jmp @KiSystemServiceHandler@8
+
+EXTERN @KiFastCallEntryHandler@8:PROC
+PUBLIC _KiFastCallEntry
+_KiFastCallEntry:
+    KiEnterTrap (KI_FAST_SYSTEM_CALL OR KI_NONVOLATILES_ONLY OR KI_DONT_SAVE_SEGS)
+    jmp @KiFastCallEntryHandler@8
 
 PUBLIC _KiStartUnexpectedRange@0
 _KiStartUnexpectedRange@0:
@@ -144,15 +143,4 @@ PUBLIC _KiEndUnexpectedRange@0
 _KiEndUnexpectedRange@0:
     jmp _KiUnexpectedInterruptTail
 
-
-/* EXIT CODE *****************************************************************/
-
-KiTrapExitStub KiSystemCallReturn,        (KI_RESTORE_EAX OR KI_RESTORE_EFLAGS OR KI_EXIT_JMP)
-KiTrapExitStub KiSystemCallSysExitReturn, (KI_RESTORE_EAX OR KI_RESTORE_FS OR KI_RESTORE_EFLAGS OR KI_EXIT_SYSCALL)
-KiTrapExitStub KiSystemCallTrapReturn,    (KI_RESTORE_EAX OR KI_RESTORE_FS OR KI_EXIT_IRET)
-
-KiTrapExitStub KiEditedTrapReturn,        (KI_RESTORE_VOLATILES OR KI_RESTORE_EFLAGS OR KI_EDITED_FRAME OR KI_EXIT_RET)
-KiTrapExitStub KiTrapReturn,              (KI_RESTORE_VOLATILES OR KI_RESTORE_SEGMENTS OR KI_EXIT_IRET)
-KiTrapExitStub KiTrapReturnNoSegments,    (KI_RESTORE_VOLATILES OR KI_EXIT_IRET)
-
 END
index 44b15c3..6c77764 100644 (file)
@@ -45,8 +45,6 @@ UCHAR KiTrapIoTable[] =
     0x6F,                      /* OUTS                                 */    
 };
 
-FAST_SYSTEM_CALL_EXIT KiFastCallExitHandler = KiSystemCallTrapReturn;
-
 BOOLEAN
 FORCEINLINE
 KiVdmTrap(IN PKTRAP_FRAME TrapFrame)
@@ -64,62 +62,21 @@ KiV86Trap(IN PKTRAP_FRAME TrapFrame)
     return ((TrapFrame->EFlags & EFLAGS_V86_MASK) != 0);
 }
 
-BOOLEAN
-FORCEINLINE
-KeIsFrameEdited(IN PKTRAP_FRAME TrapFrame)
-{
-    /* An edited frame changes esp. It is marked by clearing the bits
-       defined by FRAME_EDITED in the SegCs field of the trap frame */
-    return ((TrapFrame->SegCs & FRAME_EDITED) == 0);
-}
-
 /* TRAP EXIT CODE *************************************************************/
 
 VOID
-FORCEINLINE
-KiCommonExit(IN PKTRAP_FRAME TrapFrame, const ULONG Flags)
+FASTCALL
+DECLSPEC_NORETURN
+KiEoiHelper(IN PKTRAP_FRAME TrapFrame)
 {
     /* Disable interrupts until we return */
     _disable();
-
+    
     /* Check for APC delivery */
     KiCheckForApcDelivery(TrapFrame);
-
-    /* Debugging checks */
-    KiExitTrapDebugChecks(TrapFrame, Flags);
-
-    /* Restore the SEH handler chain */
-    KeGetPcr()->Tib.ExceptionList = TrapFrame->ExceptionList;
-
-    /* Check if there are active debug registers */
-    if (__builtin_expect(TrapFrame->Dr7 & ~DR7_RESERVED_MASK, 0))
-    {
-        /* Not handled yet */
-        DbgPrint("Need Hardware Breakpoint Support!\n");
-        DbgBreakPoint();
-        while (TRUE);
-    }
-}
-
-VOID
-FASTCALL
-DECLSPEC_NORETURN
-KiEoiHelper(IN PKTRAP_FRAME TrapFrame)
-{
-    /* Common trap exit code */
-    KiCommonExit(TrapFrame, 0);
-
-    /* Check if this was a V8086 trap */
-    if (TrapFrame->EFlags & EFLAGS_V86_MASK) KiTrapReturnNoSegments(TrapFrame);
-
-    /* Check for user mode exit */
-    if (TrapFrame->SegCs & MODE_MASK) KiTrapReturn(TrapFrame);
-
-    /* Check for edited frame */
-    if (KeIsFrameEdited(TrapFrame)) KiEditedTrapReturn(TrapFrame);
-
-    /* Exit the trap to kernel mode */
-    KiTrapReturnNoSegments(TrapFrame);
+    
+    /* Now exit the trap for real */
+    KiExitTrap(TrapFrame, KTE_SKIP_PM_BIT);
 }
 
 VOID
@@ -128,36 +85,17 @@ DECLSPEC_NORETURN
 KiServiceExit(IN PKTRAP_FRAME TrapFrame,
               IN NTSTATUS Status)
 {
-    ASSERT((TrapFrame->EFlags & EFLAGS_V86_MASK) == 0);
-    ASSERT(!KeIsFrameEdited(TrapFrame));
-
+    /* Disable interrupts until we return */
+    _disable();
+    
+    /* Check for APC delivery */
+    KiCheckForApcDelivery(TrapFrame);
+    
     /* Copy the status into EAX */
     TrapFrame->Eax = Status;
-
-    /* Common trap exit code */
-    KiCommonExit(TrapFrame, 0);
     
-    /* Restore previous mode */
-    KeGetCurrentThread()->PreviousMode = TrapFrame->PreviousPreviousMode;
-
-    /* Check for user mode exit */
-    if (TrapFrame->SegCs & MODE_MASK)
-    {
-        /* Check if we were single stepping */
-        if (TrapFrame->EFlags & EFLAGS_TF)
-        {
-            /* Must use the IRET handler */
-            KiSystemCallTrapReturn(TrapFrame);
-        }
-        else
-        {
-            /* We can use the sysexit handler */
-            KiFastCallExitHandler(TrapFrame);
-        }
-    }
-
-    /* Exit to kernel mode */
-    KiSystemCallReturn(TrapFrame);
+    /* Now exit the trap for real */
+    KiExitTrap(TrapFrame, KTE_SKIP_SEG_BIT | KTE_SKIP_VOL_BIT);
 }
 
 VOID
@@ -165,23 +103,14 @@ FASTCALL
 DECLSPEC_NORETURN
 KiServiceExit2(IN PKTRAP_FRAME TrapFrame)
 {
-    /* Common trap exit code */
-    KiCommonExit(TrapFrame, 0);
-
-    /* Restore previous mode */
-    KeGetCurrentThread()->PreviousMode = TrapFrame->PreviousPreviousMode;
-
-    /* Check if this was a V8086 trap */
-    if (TrapFrame->EFlags & EFLAGS_V86_MASK) KiTrapReturnNoSegments(TrapFrame);
-
-    /* Check for user mode exit */
-    if (TrapFrame->SegCs & MODE_MASK) KiTrapReturn(TrapFrame);
-
-    /* Check for edited frame */
-    if (KeIsFrameEdited(TrapFrame)) KiEditedTrapReturn(TrapFrame);
-
-    /* Exit the trap to kernel mode */
-    KiTrapReturnNoSegments(TrapFrame);
+    /* Disable interrupts until we return */
+    _disable();
+    
+    /* Check for APC delivery */
+    KiCheckForApcDelivery(TrapFrame);
+    
+    /* Now exit the trap for real */
+    KiExitTrap(TrapFrame, 0);
 }
 
 /* TRAP HANDLERS **************************************************************/
@@ -653,7 +582,10 @@ KiTrap06Handler(IN PKTRAP_FRAME TrapFrame)
         _disable();
         
         /* Do a quick V86 exit if possible */
-        KiExitV86Trap(TrapFrame);
+        if (__builtin_expect(TrapFrame->EFlags & EFLAGS_V86_MASK, 1)) KiExitV86Trap(TrapFrame);
+        
+        /* Exit trap the slow way */
+        KiEoiHelper(TrapFrame);
     }
 
     /* Save trap frame */
@@ -910,7 +842,10 @@ KiTrap0DHandler(IN PKTRAP_FRAME TrapFrame)
         _disable();
         
         /* Do a quick V86 exit if possible */
-        KiExitV86Trap(TrapFrame);
+        if (__builtin_expect(TrapFrame->EFlags & EFLAGS_V86_MASK, 1)) KiExitV86Trap(TrapFrame);
+        
+        /* Exit trap the slow way */
+        KiEoiHelper(TrapFrame);
     }
     
     /* Save trap frame */
@@ -974,7 +909,7 @@ KiTrap0DHandler(IN PKTRAP_FRAME TrapFrame)
                      (((Instructions[i + 2] & 0x38) == 0x10) ||        // LLDT
                       (Instructions[i + 2] == 0x18))) ||               // LTR
                     ((Instructions[i + 1] == 0x01) &&              // LGDT or LIDT or LMSW
-                     (((Instructions[i + 2] & 0x38) == 0x10) ||        // LGDT
+                     (((Instructions[i + 2] & 0x38) == 0x10) ||        // LLGT
                       (Instructions[i + 2] == 0x18) ||                 // LIDT
                       (Instructions[i + 2] == 0x30))) ||               // LMSW
                     (Instructions[i + 1] == 0x08) ||               // INVD
@@ -986,7 +921,6 @@ KiTrap0DHandler(IN PKTRAP_FRAME TrapFrame)
                     (Instructions[i + 1] == 0x24) ||               // MOV YYY, DR
                     (Instructions[i + 1] == 0x30) ||               // WRMSR
                     (Instructions[i + 1] == 0x33))                 // RDPMC
-                    // INVLPG, INVLPGA, SYSRET
                 {
                     /* These are all privileged */
                     Privileged = TRUE;
@@ -1059,7 +993,7 @@ KiTrap0DHandler(IN PKTRAP_FRAME TrapFrame)
      * a POP <SEG>, which could cause an invalid segment if someone had messed
      * with the segment values.
      *
-     * Another case is a bogus SS, which would hit a GPF when doing the iret.
+     * Another case is a bogus SS, which would hit a GPF when doing the ired.
      * This could only be done through a buggy or malicious driver, or perhaps
      * the kernel debugger.
      *
@@ -1133,14 +1067,9 @@ KiTrap0DHandler(IN PKTRAP_FRAME TrapFrame)
         /* Fix it */
         TrapFrame->SegEs = (KGDT_R3_DATA | RPL_MASK);
      }
-     else
-     {
-         /* Whatever it is, we can't handle it */
-         KiSystemFatalException(EXCEPTION_GP_FAULT, TrapFrame);
-     }
-
-    /* Return to where we came from */
-    KiTrapReturn(TrapFrame);
+     
+     /* Do a direct trap exit: restore volatiles only */
+     KiExitTrap(TrapFrame, KTE_SKIP_PM_BIT | KTE_SKIP_SEG_BIT);
 }
 
 VOID
@@ -1247,7 +1176,7 @@ KiTrap0EHandler(IN PKTRAP_FRAME TrapFrame)
                                  Cr2,
                                  TrapFrame);
     }
-
+    
     /* Only other choice is an in-page error, with 3 parameters */
     KiDispatchExceptionFromTrapFrame(STATUS_IN_PAGE_ERROR,
                                      TrapFrame->Eip,
@@ -1448,89 +1377,55 @@ KiDebugServiceHandler(IN PKTRAP_FRAME TrapFrame)
 }
 
 VOID
-FORCEINLINE
+FASTCALL
 DECLSPEC_NORETURN
-KiSystemCall(IN PKTRAP_FRAME TrapFrame,
-                       IN PVOID Arguments)
+KiSystemCall(IN ULONG SystemCallNumber,
+             IN PVOID Arguments)
 {
     PKTHREAD Thread;
+    PKTRAP_FRAME TrapFrame;
     PKSERVICE_TABLE_DESCRIPTOR DescriptorTable;
     ULONG Id, Offset, StackBytes, Result;
     PVOID Handler;
-    ULONG SystemCallNumber = TrapFrame->Eax;
-
-    /* Get the current thread */
-    Thread = KeGetCurrentThread();
-
-    /* Set debug header */
-    KiFillTrapFrameDebug(TrapFrame);
-
-    /* Chain trap frames */
-    TrapFrame->Edx = (ULONG_PTR)Thread->TrapFrame;
-
-    /* No error code */
-    TrapFrame->ErrCode = 0;
-
-    /* Save previous mode */
-    TrapFrame->PreviousPreviousMode = Thread->PreviousMode;
-
-    /* Save the SEH chain and terminate it for now */    
-    TrapFrame->ExceptionList = KeGetPcr()->Tib.ExceptionList;
-    KeGetPcr()->Tib.ExceptionList = EXCEPTION_CHAIN_END;
-
-    /* Clear DR7 and check for debugging */
-    TrapFrame->Dr7 = 0;
-    if (__builtin_expect(Thread->DispatcherHeader.DebugActive & 0xFF, 0))
+    
+    /* Loop because we might need to try this twice in case of a GUI call */
+    while (TRUE)
     {
-        UNIMPLEMENTED;
-        while (TRUE);
-    }
-
-    /* Set thread fields */
-    Thread->TrapFrame = TrapFrame;
-    Thread->PreviousMode = KiUserTrap(TrapFrame);
-
-    /* Enable interrupts */
-    _enable();
-
-    /* Decode the system call number */
-    Offset = (SystemCallNumber >> SERVICE_TABLE_SHIFT) & SERVICE_TABLE_MASK;
-    Id = SystemCallNumber & SERVICE_NUMBER_MASK;
-
-    /* Get descriptor table */
-    DescriptorTable = (PVOID)((ULONG_PTR)Thread->ServiceTable + Offset);
+        /* Decode the system call number */
+        Offset = (SystemCallNumber >> SERVICE_TABLE_SHIFT) & SERVICE_TABLE_MASK;
+        Id = SystemCallNumber & SERVICE_NUMBER_MASK;
+    
+        /* Get current thread, trap frame, and descriptor table */
+        Thread = KeGetCurrentThread();
+        TrapFrame = Thread->TrapFrame;
+        DescriptorTable = (PVOID)((ULONG_PTR)Thread->ServiceTable + Offset);
 
-    /* Validate the system call number */
-    if (__builtin_expect(Id >= DescriptorTable->Limit, 0))
-    {
-        /* Check if this is a GUI call */
-        if (!(Offset & SERVICE_TABLE_TEST))
+        /* Validate the system call number */
+        if (__builtin_expect(Id >= DescriptorTable->Limit, 0))
         {
-            /* Fail the call */
-            Result = STATUS_INVALID_SYSTEM_SERVICE;
-            goto ExitCall;
-        }
+            /* Check if this is a GUI call */
+            if (__builtin_expect(!(Offset & SERVICE_TABLE_TEST), 0))
+            {
+                /* Fail the call */
+                Result = STATUS_INVALID_SYSTEM_SERVICE;
+                goto ExitCall;
+            }
 
-        /* Convert us to a GUI thread -- must wrap in ASM to get new EBP */        
-        Result = KiConvertToGuiThread();
-        if (!NT_SUCCESS(Result))
-        {
-            /* Set the last error and fail */
-            //SetLastWin32Error(RtlNtStatusToDosError(Result));
-            goto ExitCall;
+            /* Convert us to a GUI thread -- must wrap in ASM to get new EBP */        
+            Result = KiConvertToGuiThread();
+            if (__builtin_expect(!NT_SUCCESS(Result), 0))
+            {
+                /* Figure out how we should fail to the user */
+                UNIMPLEMENTED;
+                while (TRUE);
+            }
+            
+            /* Try the call again */
+            continue;
         }
         
-        /* Reload trap frame and descriptor table pointer from new stack */
-        TrapFrame = *(volatile PVOID*)&Thread->TrapFrame;
-        DescriptorTable = (PVOID)(*(volatile ULONG_PTR*)&Thread->ServiceTable + Offset);
-
-        /* Validate the system call number again */
-        if (Id >= DescriptorTable->Limit)
-        {
-            /* Fail the call */
-            Result = STATUS_INVALID_SYSTEM_SERVICE;
-            goto ExitCall;
-        }
+        /* If we made it here, the call is good */
+        break;
     }
     
     /* Check if this is a GUI call */
@@ -1573,13 +1468,45 @@ ExitCall:
 }
 
 VOID
-FASTCALL
+FORCEINLINE
 DECLSPEC_NORETURN
-KiSystemServiceHandler(IN PKTRAP_FRAME TrapFrame,
-                       IN PVOID Arguments)
+KiSystemCallHandler(IN PKTRAP_FRAME TrapFrame,
+                    IN ULONG ServiceNumber,
+                    IN PVOID Arguments,
+                    IN PKTHREAD Thread,
+                    IN KPROCESSOR_MODE PreviousMode,
+                    IN KPROCESSOR_MODE PreviousPreviousMode,
+                    IN USHORT SegFs)
 {
-    /* Call the shared handler (inline) */
-    KiSystemCall(TrapFrame, Arguments);
+    /* No error code */
+    TrapFrame->ErrCode = 0;
+    
+    /* Save previous mode and FS segment */
+    TrapFrame->PreviousPreviousMode = PreviousPreviousMode;
+    TrapFrame->SegFs = SegFs;
+        
+    /* Save the SEH chain and terminate it for now */    
+    TrapFrame->ExceptionList = KeGetPcr()->Tib.ExceptionList;
+    KeGetPcr()->Tib.ExceptionList = EXCEPTION_CHAIN_END;
+        
+    /* Clear DR7 and check for debugging */
+    TrapFrame->Dr7 = 0;
+    if (__builtin_expect(Thread->DispatcherHeader.DebugActive & 0xFF, 0))
+    {
+        UNIMPLEMENTED;
+        while (TRUE);
+    }
+
+    /* Set thread fields */
+    Thread->TrapFrame = TrapFrame;
+    Thread->PreviousMode = PreviousMode;
+    
+    /* Set debug header */
+    KiFillTrapFrameDebug(TrapFrame);
+    
+    /* Enable interrupts and make the call */
+    _enable();
+    KiSystemCall(ServiceNumber, Arguments);   
 }
 
 VOID
@@ -1588,20 +1515,54 @@ DECLSPEC_NORETURN
 KiFastCallEntryHandler(IN PKTRAP_FRAME TrapFrame,
                        IN PVOID Arguments)
 {
+    PKTHREAD Thread;
+
     /* Set up a fake INT Stack and enable interrupts */
     TrapFrame->HardwareSegSs = KGDT_R3_DATA | RPL_MASK;
     TrapFrame->HardwareEsp = (ULONG_PTR)Arguments;
     TrapFrame->EFlags = __readeflags() | EFLAGS_INTERRUPT_MASK;
     TrapFrame->SegCs = KGDT_R3_CODE | RPL_MASK;
     TrapFrame->Eip = SharedUserData->SystemCallReturn;
-    TrapFrame->SegFs = KGDT_R3_TEB | RPL_MASK;
     __writeeflags(0x2);
     
-     /* Arguments are actually 2 frames down (because of the double indirection) */
+    /* Get the current thread */
+    Thread = KeGetCurrentThread();
+    
+    /* Arguments are actually 2 frames down (because of the double indirection) */
     Arguments = (PVOID)(TrapFrame->HardwareEsp + 8);
 
     /* Call the shared handler (inline) */
-    KiSystemCall(TrapFrame, Arguments);
+    KiSystemCallHandler(TrapFrame,
+                        TrapFrame->Eax,
+                        Arguments,
+                        Thread,
+                        UserMode,
+                        Thread->PreviousMode,
+                        KGDT_R3_TEB | RPL_MASK);
+}
+
+VOID
+FASTCALL
+DECLSPEC_NORETURN
+KiSystemServiceHandler(IN PKTRAP_FRAME TrapFrame,
+                       IN PVOID Arguments)
+{
+    PKTHREAD Thread;
+
+    /* Get the current thread */
+    Thread = KeGetCurrentThread();
+    
+    /* Chain trap frames */
+    TrapFrame->Edx = (ULONG_PTR)Thread->TrapFrame;
+    
+    /* Call the shared handler (inline) */
+    KiSystemCallHandler(TrapFrame,
+                        TrapFrame->Eax,
+                        Arguments,
+                        Thread,
+                        KiUserTrap(TrapFrame),
+                        Thread->PreviousMode,
+                        TrapFrame->SegFs);
 }
 
 /*