diff --git a/xtoskrnl/ar/amd64/archsup.S b/xtoskrnl/ar/amd64/archsup.S
index d91f8ea..481cd53 100644
--- a/xtoskrnl/ar/amd64/archsup.S
+++ b/xtoskrnl/ar/amd64/archsup.S
@@ -104,12 +104,20 @@ ArTrap\Vector:
     /* Test previous mode and swap GS if needed */
     movl $0, TrapPreviousMode(%rbp)
     mov %cs, %ax
-    and $1, %al
+    and $3, %al
     mov %al, TrapPreviousMode(%rbp)
     jz KernelMode$\Vector
     swapgs
+    jmp UserMode$\Vector
 
 KernelMode$\Vector:
+    /* Save kernel stack pointer (SS:RSP) */
+    movl %ss, %eax
+    mov %eax, TrapSegSs(%rbp)
+    lea TRAP_FRAME_SIZE(%rbp), %rax
+    mov %rax, TrapRsp(%rbp)
+
+UserMode$\Vector:
     /* Push Frame Pointer, clear direction flag and pass to trap dispatcher */
     mov %rsp, %rcx
     cld
diff --git a/xtoskrnl/includes/ar/amd64/asmsup.h b/xtoskrnl/includes/ar/amd64/asmsup.h
index 7e04ab6..6127077 100644
--- a/xtoskrnl/includes/ar/amd64/asmsup.h
+++ b/xtoskrnl/includes/ar/amd64/asmsup.h
@@ -56,6 +56,8 @@
 #define TrapSegEs                               330
 #define TrapSegFs                               332
 #define TrapSegGs                               334
+#define TrapRsp                                 496
+#define TrapSegSs                               504
 
 /* KTRAP_FRAME length related definitions */
 #define TRAP_FRAME_SIZE                         512