Rework trap handling assembly code

sdk/xtdk/amd64/ketypes.h

@@ -381,14 +381,32 @@ typedef struct _KSWITCH_FRAME
 /* Trap frame definition */
 typedef struct _KTRAP_FRAME
 {
+    M128 Xmm0;
+    M128 Xmm1;
+    M128 Xmm2;
+    M128 Xmm3;
+    M128 Xmm4;
+    M128 Xmm5;
+    M128 Xmm6;
+    M128 Xmm7;
+    M128 Xmm8;
+    M128 Xmm9;
+    M128 Xmm10;
+    M128 Xmm11;
+    M128 Xmm12;
+    M128 Xmm13;
+    M128 Xmm14;
+    M128 Xmm15;
+    ULONG MxCsr;
+    ULONG PreviousMode;
+    ULONGLONG Cr2;
+    ULONGLONG Cr3;
     ULONGLONG Dr0;
     ULONGLONG Dr1;
     ULONGLONG Dr2;
     ULONGLONG Dr3;
     ULONGLONG Dr6;
     ULONGLONG Dr7;
-    ULONGLONG Cr2;
-    ULONGLONG Cr3;
     USHORT SegDs;
     USHORT SegEs;
     USHORT SegFs;
@@ -397,9 +415,6 @@ typedef struct _KTRAP_FRAME
     ULONGLONG Rbx;
     ULONGLONG Rcx;
     ULONGLONG Rdx;
-    ULONGLONG Rsi;
-    ULONGLONG Rdi;
-    ULONGLONG Rbp;
     ULONGLONG R8;
     ULONGLONG R9;
     ULONGLONG R10;
@@ -408,13 +423,16 @@ typedef struct _KTRAP_FRAME
     ULONGLONG R13;
     ULONGLONG R14;
     ULONGLONG R15;
+    ULONGLONG Rsi;
+    ULONGLONG Rdi;
+    ULONGLONG Rbp;
     ULONGLONG Vector;
     union {
         ULONGLONG ErrorCode;
         ULONGLONG ExceptionFrame;
     };
     ULONGLONG Rip;
-    ULONGLONG Cs;
+    ULONGLONG SegCs;
     ULONGLONG Flags;
     ULONGLONG Rsp;
     ULONGLONG SegSs;

sdk/xtdk/i686/ketypes.h

@@ -382,14 +382,15 @@ typedef struct _KSWITCH_FRAME
 /* Trap frame definition */
 typedef struct _KTRAP_FRAME
 {
+    ULONG PreviousMode;
+    ULONG Cr2;
+    ULONG Cr3;
     ULONG Dr0;
     ULONG Dr1;
     ULONG Dr2;
     ULONG Dr3;
     ULONG Dr6;
     ULONG Dr7;
-    ULONG Cr2;
-    ULONG Cr3;
     USHORT SegDs;
     USHORT SegEs;
     USHORT SegFs;
@@ -404,7 +405,7 @@ typedef struct _KTRAP_FRAME
     ULONG Vector;
     ULONG ErrorCode;
     ULONG Eip;
-    ULONG Cs;
+    ULONG SegCs;
     ULONG Flags;
     ULONG Esp;
     ULONG SegSs;

xtoskrnl/ar/amd64/archsup.S

@@ -2,10 +2,12 @@
  * PROJECT:         ExectOS
  * COPYRIGHT:       See COPYING.md in the top level directory
  * FILE:            xtoskrnl/ar/amd64/archsup.S
- * DESCRIPTION:     Provides AMD64 architecture features not implementable in C.
+ * DESCRIPTION:     Provides AMD64 architecture features not implementable in C
  * DEVELOPERS:      Rafal Kupiec <belliash@codingworkshop.eu.org>
  */
 
+#include <amd64/asmsup.h>
+
 .altmacro
 .text
 
@@ -32,6 +34,9 @@ ArpTrap\Vector:
     push $\Vector
 
     /* Push General Purpose Registers */
+    push %rbp
+    push %rdi
+    push %rsi
     push %r15
     push %r14
     push %r13
@@ -40,66 +45,117 @@ ArpTrap\Vector:
     push %r10
     push %r9
     push %r8
-    push %rbp
-    push %rdi
-    push %rsi
     push %rdx
     push %rcx
     push %rbx
     push %rax
 
-    /* Push Segments */
+    /* Reserve space for other registers and point RBP to the trap frame */
-    mov %gs, %ax
+    sub $(TRAP_FRAME_SIZE - TRAP_REGISTERS_SIZE), %rsp
-    push %ax
+    lea (%rsp), %rbp
-    mov %fs, %ax
-    push %ax
-    mov %es, %ax
-    push %ax
-    mov %ds, %ax
-    push %ax
 
-    /* Push Control Registers */
+    /* Store segment selectors */
-    mov %cr3, %rax
+    mov %gs, TrapSegGs(%rbp)
-    push %rax
+    mov %fs, TrapSegFs(%rbp)
-    mov %cr2, %rax
+    mov %es, TrapSegEs(%rbp)
-    push %rax
+    mov %ds, TrapSegDs(%rbp)
 
-    /* Push Debug Registers */
+    /* Store debug registers */
     mov %dr7, %rax
-    push %rax
+    mov %rax, TrapDr7(%rbp)
     mov %dr6, %rax
-    push %rax
+    mov %rax, TrapDr6(%rbp)
     mov %dr3, %rax
-    push %rax
+    mov %rax, TrapDr3(%rbp)
     mov %dr2, %rax
-    push %rax
+    mov %rax, TrapDr2(%rbp)
     mov %dr1, %rax
-    push %rax
+    mov %rax, TrapDr1(%rbp)
     mov %dr0, %rax
-    push %rax
+    mov %rax, TrapDr0(%rbp)
 
+    /* Store CR2 and CR3 */
+    mov %cr3, %rax
+    mov %rax, TrapCr3(%rbp)
+    mov %cr2, %rax
+    mov %rax, TrapCr2(%rbp)
+
+    /* Store MxCsr register */
+    stmxcsr TrapMxCsr(%rbp)
+
+    /* Store XMM registers */
+    movdqa %xmm15, TrapXmm15(%rbp)
+    movdqa %xmm14, TrapXmm14(%rbp)
+    movdqa %xmm13, TrapXmm13(%rbp)
+    movdqa %xmm12, TrapXmm12(%rbp)
+    movdqa %xmm11, TrapXmm11(%rbp)
+    movdqa %xmm10, TrapXmm10(%rbp)
+    movdqa %xmm9, TrapXmm9(%rbp)
+    movdqa %xmm8, TrapXmm8(%rbp)
+    movdqa %xmm7, TrapXmm7(%rbp)
+    movdqa %xmm6, TrapXmm6(%rbp)
+    movdqa %xmm5, TrapXmm5(%rbp)
+    movdqa %xmm4, TrapXmm4(%rbp)
+    movdqa %xmm3, TrapXmm3(%rbp)
+    movdqa %xmm2, TrapXmm2(%rbp)
+    movdqa %xmm1, TrapXmm1(%rbp)
+    movdqa %xmm0, TrapXmm0(%rbp)
+
+    /* Test previous mode and swap GS if needed */
+    movl $0, TrapPreviousMode(%ebp)
+    mov %cs, %ax
+    and $1, %al
+    mov %al, TrapPreviousMode(%rbp)
+    jz KernelMode$\Vector
+    swapgs
+
+KernelMode$\Vector:
     /* Push Frame Pointer, clear direction flag and pass to trap dispatcher */
     mov %rsp, %rcx
     cld
     call ArpDispatchTrap
 
-    /* Skip space occupied by Debug Registers */
+    /* Test previous mode and swapgs if needed */
-    add $(6 * 8), %rsp
+    testb $1, TrapPreviousMode(%rbp)
+    jz KernelModeReturn$\Vector
+    cli
+    swapgs
 
-    /* Skip space occupied by CR2 and CR3 */
+KernelModeReturn$\Vector:
-    add $(2 * 8), %rsp
+    /* Restore XMM registers */
+    movdqa TrapXmm0(%rbp), %xmm0
+    movdqa TrapXmm1(%rbp), %xmm1
+    movdqa TrapXmm2(%rbp), %xmm2
+    movdqa TrapXmm3(%rbp), %xmm3
+    movdqa TrapXmm4(%rbp), %xmm4
+    movdqa TrapXmm5(%rbp), %xmm5
+    movdqa TrapXmm6(%rbp), %xmm6
+    movdqa TrapXmm7(%rbp), %xmm7
+    movdqa TrapXmm8(%rbp), %xmm8
+    movdqa TrapXmm9(%rbp), %xmm9
+    movdqa TrapXmm10(%rbp), %xmm10
+    movdqa TrapXmm11(%rbp), %xmm11
+    movdqa TrapXmm12(%rbp), %xmm12
+    movdqa TrapXmm13(%rbp), %xmm13
+    movdqa TrapXmm14(%rbp), %xmm14
+    movdqa TrapXmm15(%rbp), %xmm15
 
-    /* Skip space occupied by Segments */
+    /* Load MxCsr register */
-    add $(4 * 2), %rsp
+    ldmxcsr TrapMxCsr(%rbp)
+
+    /* Restore segment selectors */
+    mov TrapSegDs(%rbp), %ds
+    mov TrapSegEs(%rbp), %es
+    mov TrapSegFs(%rbp), %fs
+
+    /* Free stack space */
+    add $(TRAP_FRAME_SIZE - TRAP_REGISTERS_SIZE), %rsp
 
     /* Pop General Purpose Registers */
     pop %rax
     pop %rbx
     pop %rcx
     pop %rdx
-    pop %rsi
-    pop %rdi
-    pop %rbp
     pop %r8
     pop %r9
     pop %r10
@@ -108,6 +164,9 @@ ArpTrap\Vector:
     pop %r13
     pop %r14
     pop %r15
+    pop %rsi
+    pop %rdi
+    pop %rbp
 
     /* Skip error code and vector number, then return */
     add $(2 * 8), %rsp

xtoskrnl/ar/i686/archsup.S

@@ -6,6 +6,8 @@
  * DEVELOPERS:      Rafal Kupiec <belliash@codingworkshop.eu.org>
  */
 
+#include <i686/asmsup.h>
+
 .altmacro
 .text
 
@@ -40,36 +42,45 @@ _ArpTrap\Vector:
     push %ebx
     push %eax
 
-    /* Push Segments */
+    /* Reserve space for other registers and point RBP to the trap frame */
-    mov %gs, %ax
+    sub $(TRAP_FRAME_SIZE - TRAP_REGISTERS_SIZE), %esp
-    push %ax
+    lea (%esp), %ebp
-    mov %fs, %ax
-    push %ax
-    mov %es, %ax
-    push %ax
-    mov %ds, %ax
-    push %ax
 
-    /* Push Control Registers */
+    /* Store segment selectors */
-    mov %cr3, %eax
+    mov %gs, TrapSegGs(%ebp)
-    push %eax
+    mov %fs, TrapSegFs(%ebp)
-    mov %cr2, %eax
+    mov %es, TrapSegEs(%ebp)
-    push %eax
+    mov %ds, TrapSegDs(%ebp)
 
-    /* Push Debug Registers */
+    /* Store debug registers */
     mov %dr7, %eax
-    push %eax
+    mov %eax, TrapDr7(%ebp)
     mov %dr6, %eax
-    push %eax
+    mov %eax, TrapDr6(%ebp)
     mov %dr3, %eax
-    push %eax
+    mov %eax, TrapDr3(%ebp)
     mov %dr2, %eax
-    push %eax
+    mov %eax, TrapDr2(%ebp)
     mov %dr1, %eax
-    push %eax
+    mov %eax, TrapDr1(%ebp)
     mov %dr0, %eax
-    push %eax
+    mov %eax, TrapDr0(%ebp)
 
+    /* Store CR2 and CR3 */
+    mov %cr3, %eax
+    mov %eax, TrapCr3(%ebp)
+    mov %cr2, %eax
+    mov %eax, TrapCr2(%ebp)
+
+    /* Test previous mode and swap GS if needed */
+    movl $0, TrapPreviousMode(%ebp)
+    mov %cs, %ax
+    and $1, %al
+    mov %al, TrapPreviousMode(%ebp)
+    jz KernelMode$\Vector
+    swapgs
+
+KernelMode$\Vector:
     /* Push Frame Pointer, clear direction flag and pass to trap dispatcher */
     push %esp
     cld
@@ -78,14 +89,20 @@ _ArpTrap\Vector:
     /* Clean up the stack */
     add $4, %esp
 
-    /* Skip space occupied by Debug Registers */
+    /* Test previous mode and swapgs if needed */
-    add $(6 * 4), %esp
+    testb $1, TrapPreviousMode(%ebp)
+    jz KernelModeReturn$\Vector
+    cli
+    swapgs
 
-    /* Skip space occupied by CR2 and CR3 */
+KernelModeReturn$\Vector:
-    add $(2 * 4), %esp
+    /* Restore segment selectors */
+    mov TrapSegDs(%ebp), %ds
+    mov TrapSegEs(%ebp), %es
+    mov TrapSegFs(%ebp), %fs
 
-    /* Skip space occupied by Segments */
+    /* Free stack space */
-    add $(4 * 2), %esp
+    add $(TRAP_FRAME_SIZE - TRAP_REGISTERS_SIZE), %esp
 
     /* Pop General Purpose Registers */
     pop %eax

xtoskrnl/includes/amd64/asmsup.h

@@ -0,0 +1,49 @@
+/**
+ * PROJECT:         ExectOS
+ * COPYRIGHT:       See COPYING.md in the top level directory
+ * FILE:            xtoskrnl/includes/amd64/asm.h
+ * DESCRIPTION:     AMD64 architecture assembly definitions
+ * DEVELOPERS:      Rafal Kupiec <belliash@codingworkshop.eu.org>
+ */
+
+#ifndef __XTOSKRNL_AMD64_ASMSUP_H
+#define __XTOSKRNL_AMD64_ASMSUP_H
+
+
+/* KTRAP_FRAME structure offsets */
+#define TrapXmm0                                0
+#define TrapXmm1                                16
+#define TrapXmm2                                32
+#define TrapXmm3                                48
+#define TrapXmm4                                64
+#define TrapXmm5                                80
+#define TrapXmm6                                96
+#define TrapXmm7                                112
+#define TrapXmm8                                128
+#define TrapXmm9                                144
+#define TrapXmm10                               160
+#define TrapXmm11                               176
+#define TrapXmm12                               192
+#define TrapXmm13                               208
+#define TrapXmm14                               224
+#define TrapXmm15                               240
+#define TrapMxCsr                               256
+#define TrapPreviousMode                        260
+#define TrapCr2                                 264
+#define TrapCr3                                 272
+#define TrapDr0                                 280
+#define TrapDr1                                 288
+#define TrapDr2                                 296
+#define TrapDr3                                 304
+#define TrapDr6                                 312
+#define TrapDr7                                 320
+#define TrapSegDs                               328
+#define TrapSegEs                               330
+#define TrapSegFs                               332
+#define TrapSegGs                               334
+
+/* KTRAP_FRAME length related definitions */
+#define TRAP_FRAME_SIZE                         512
+#define TRAP_REGISTERS_SIZE                     176
+
+#endif /* __XTOSKRNL_AMD64_ASMSUP_H */

xtoskrnl/includes/i686/asmsup.h

@@ -0,0 +1,32 @@
+/**
+ * PROJECT:         ExectOS
+ * COPYRIGHT:       See COPYING.md in the top level directory
+ * FILE:            xtoskrnl/includes/i686/asm.h
+ * DESCRIPTION:     i686 architecture assembly definitions
+ * DEVELOPERS:      Rafal Kupiec <belliash@codingworkshop.eu.org>
+ */
+
+#ifndef __XTOSKRNL_I686_ASMSUP_H
+#define __XTOSKRNL_I686_ASMSUP_H
+
+
+/* KTRAP_FRAME structure offsets */
+#define TrapPreviousMode                        0
+#define TrapCr2                                 4
+#define TrapCr3                                 8
+#define TrapDr0                                 12
+#define TrapDr1                                 16
+#define TrapDr2                                 20
+#define TrapDr3                                 24
+#define TrapDr6                                 28
+#define TrapDr7                                 32
+#define TrapSegDs                               36
+#define TrapSegEs                               38
+#define TrapSegFs                               40
+#define TrapSegGs                               42
+
+/* KTRAP_FRAME length related definitions */
+#define TRAP_FRAME_SIZE                         100
+#define TRAP_REGISTERS_SIZE                     56
+
+#endif /* __XTOSKRNL_AMD64_ASMSUP_H */