[mirror_edk2.git] / MdeModulePkg / Universal / DebugSupportDxe / X64 / AsmFuncs.nasm

;/** @file\r
;  Low level x64 routines used by the debug support driver.\r
;\r
;  Copyright (c) 2007 - 2016, Intel Corporation. All rights reserved.<BR>\r
;  This program and the accompanying materials\r
;  are licensed and made available under the terms and conditions of the BSD License\r
;  which accompanies this distribution.  The full text of the license may be found at\r
;  http://opensource.org/licenses/bsd-license.php\r
;\r
;  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
;  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
;\r
;**/\r
\r
%define EXCPT64_DIVIDE_ERROR 0\r
%define EXCPT64_DEBUG 1\r
%define EXCPT64_NMI 2\r
%define EXCPT64_BREAKPOINT 3\r
%define EXCPT64_OVERFLOW 4\r
%define EXCPT64_BOUND 5\r
%define EXCPT64_INVALID_OPCODE 6\r
%define EXCPT64_DOUBLE_FAULT 8\r
%define EXCPT64_INVALID_TSS 10\r
%define EXCPT64_SEG_NOT_PRESENT 11\r
%define EXCPT64_STACK_FAULT 12\r
%define EXCPT64_GP_FAULT 13\r
%define EXCPT64_PAGE_FAULT 14\r
%define EXCPT64_FP_ERROR 16\r
%define EXCPT64_ALIGNMENT_CHECK 17\r
%define EXCPT64_MACHINE_CHECK 18\r
%define EXCPT64_SIMD 19\r
\r
%define FXSTOR_FLAG 0x1000000         ; bit cpuid 24 of feature flags\r
\r
;; The FXSTOR and FXRSTOR commands are used for saving and restoring the x87,\r
;; MMX, SSE, SSE2, etc registers.  The initialization of the debugsupport driver\r
;; MUST check the CPUID feature flags to see that these instructions are available\r
;; and fail to init if they are not.\r
\r
;; fxstor [rdi]\r
%macro FXSTOR_RDI 0\r
                         db 0xf, 0xae, 00000111y ; mod = 00, reg/op = 000, r/m = 111 = [rdi]\r
%endmacro\r
\r
;; fxrstor [rsi]\r
%macro FXRSTOR_RSI 0\r
                         db 0xf, 0xae, 00001110y ; mod = 00, reg/op = 001, r/m = 110 = [rsi]\r
%endmacro\r
\r
SECTION .data\r
\r
global ASM_PFX(OrigVector)\r
global ASM_PFX(InterruptEntryStub)\r
global ASM_PFX(StubSize)\r
global ASM_PFX(CommonIdtEntry)\r
global ASM_PFX(FxStorSupport)\r
extern ASM_PFX(InterruptDistrubutionHub)\r
\r
ASM_PFX(StubSize): dd InterruptEntryStubEnd - ASM_PFX(InterruptEntryStub)\r
AppRsp: dq 0x1111111111111111 ; ?\r
DebugRsp: dq 0x2222222222222222 ; ?\r
ExtraPush: dq 0x3333333333333333 ; ?\r
ExceptData: dq 0x4444444444444444 ; ?\r
Rflags: dq 0x5555555555555555 ; ?\r
ASM_PFX(OrigVector): dq 0x6666666666666666 ; ?\r
\r
;; The declarations below define the memory region that will be used for the debug stack.\r
;; The context record will be built by pushing register values onto this stack.\r
;; It is imparitive that alignment be carefully managed, since the FXSTOR and\r
;; FXRSTOR instructions will GP fault if their memory operand is not 16 byte aligned.\r
;;\r
;; The stub will switch stacks from the application stack to the debuger stack\r
;; and pushes the exception number.\r
;;\r
;; Then we building the context record on the stack. Since the stack grows down,\r
;; we push the fields of the context record from the back to the front.  There\r
;; are 336 bytes of stack used prior allocating the 512 bytes of stack to be\r
;; used as the memory buffer for the fxstor instruction. Therefore address of\r
;; the buffer used for the FXSTOR instruction is &Eax - 336 - 512, which\r
;; must be 16 byte aligned.\r
;;\r
;; We carefully locate the stack to make this happen.\r
;;\r
;; For reference, the context structure looks like this:\r
;;      struct {\r
;;        UINT64            ExceptionData;\r
;;        FX_SAVE_STATE_X64 FxSaveState;    // 512 bytes, must be 16 byte aligned\r
;;        UINT64            Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;\r
;;        UINT64            Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;\r
;;        UINT64            RFlags;\r
;;        UINT64            Ldtr, Tr;\r
;;        UINT64            Gdtr[2], Idtr[2];\r
;;        UINT64            Rip;\r
;;        UINT64            Gs, Fs, Es, Ds, Cs, Ss;\r
;;        UINT64            Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;\r
;;        UINT64            R8, R9, R10, R11, R12, R13, R14, R15;\r
;;      } SYSTEM_CONTEXT_X64;  // 64 bit system context record\r
\r
align           16\r
DebugStackEnd: db "DbgStkEnd >>>>>>"    ;; 16 byte long string - must be 16 bytes to preserve alignment\r
                times 0x1ffc dd    0x0  ;; 32K should be enough stack\r
                                        ;;   This allocation is coocked to insure\r
                                        ;;   that the the buffer for the FXSTORE instruction\r
                                        ;;   will be 16 byte aligned also.\r
                                        ;;\r
ExceptionNumber: dq 0                   ;; first entry will be the vector number pushed by the stub\r
\r
DebugStackBegin: db "<<<< DbgStkBegin"  ;; initial debug ESP == DebugStackBegin, set in stub\r
\r
DEFAULT REL\r
SECTION .text\r
\r
;------------------------------------------------------------------------------\r
; BOOLEAN\r
; FxStorSupport (\r
;   void\r
;   )\r
;\r
; Abstract: Returns TRUE if FxStor instructions are supported\r
;\r
global ASM_PFX(FxStorSupport)\r
ASM_PFX(FxStorSupport):\r
\r
;\r
; cpuid corrupts rbx which must be preserved per the C calling convention\r
;\r
                push    rbx\r
                mov     rax, dword 1\r
                cpuid\r
                mov     eax, edx\r
                and     rax, FXSTOR_FLAG\r
                shr     rax, 24\r
                pop     rbx\r
                ret\r
\r
;------------------------------------------------------------------------------\r
; void\r
; Vect2Desc (\r
;   IA32_IDT_GATE_DESCRIPTOR * DestDesc,  // rcx\r
;   void (*Vector) (void)   // rdx\r
;   )\r
;\r
; Abstract: Encodes an IDT descriptor with the given physical address\r
;\r
global ASM_PFX(Vect2Desc)\r
ASM_PFX(Vect2Desc):\r
\r
                mov     rax, rdx\r
                mov     word [rcx], ax                  ; write bits 15..0 of offset\r
                mov     dx, cs\r
                mov     word [rcx+2], dx                ; SYS_CODE_SEL from GDT\r
                mov     word [rcx+4], 0xe00 | 0x8000    ; type = 386 interrupt gate, present\r
                shr     rax, 16\r
                mov     word [rcx+6], ax                ; write bits 31..16 of offset\r
                shr     rax, 16\r
                mov     dword [rcx+8], eax              ; write bits 63..32 of offset\r
\r
                ret\r
\r
;------------------------------------------------------------------------------\r
; InterruptEntryStub\r
;\r
; Abstract: This code is not a function, but is a small piece of code that is\r
;               copied and fixed up once for each IDT entry that is hooked.\r
;\r
ASM_PFX(InterruptEntryStub):\r
                push    0                       ; push vector number - will be modified before installed\r
                db      0xe9                    ; jump rel32\r
                dd      0                       ; fixed up to relative address of CommonIdtEntry\r
InterruptEntryStubEnd:\r
\r
;------------------------------------------------------------------------------\r
; CommonIdtEntry\r
;\r
; Abstract: This code is not a function, but is the common part for all IDT\r
;               vectors.\r
;\r
ASM_PFX(CommonIdtEntry):\r
;;\r
;; At this point, the stub has saved the current application stack esp into AppRsp\r
;; and switched stacks to the debug stack, where it pushed the vector number\r
;;\r
;; The application stack looks like this:\r
;;\r
;;              ...\r
;;              (last application stack entry)\r
;;              [16 bytes alignment, do not care it]\r
;;              SS from interrupted task\r
;;              RSP from interrupted task\r
;;              rflags from interrupted task\r
;;              CS from interrupted task\r
;;              RIP from interrupted task\r
;;              Error code <-------------------- Only present for some exeption types\r
;;\r
;;              Vector Number <----------------- pushed in our IDT Entry\r
;;\r
\r
;; The stub switched us to the debug stack and pushed the interrupt number.\r
;;\r
;; Next, construct the context record.  It will be build on the debug stack by\r
;; pushing the registers in the correct order so as to create the context structure\r
;; on the debug stack.  The context record must be built from the end back to the\r
;; beginning because the stack grows down...\r
;\r
;; For reference, the context record looks like this:\r
;;\r
;; typedef\r
;; struct {\r
;;   UINT64            ExceptionData;\r
;;   FX_SAVE_STATE_X64 FxSaveState;\r
;;   UINT64            Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;\r
;;   UINT64            Cr0, Cr2, Cr3, Cr4, Cr8;\r
;;   UINT64            RFlags;\r
;;   UINT64            Ldtr, Tr;\r
;;   UINT64            Gdtr[2], Idtr[2];\r
;;   UINT64            Rip;\r
;;   UINT64            Gs, Fs, Es, Ds, Cs, Ss;\r
;;   UINT64            Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;\r
;;   UINT64            R8, R9, R10, R11, R12, R13, R14, R15;\r
;; } SYSTEM_CONTEXT_X64;  // 64 bit system context record\r
\r
;; NOTE: we save rsp here to prevent compiler put rip reference cause error AppRsp\r
                push    rax\r
                mov     rax, qword [rsp+8]               ; save vector number\r
                mov     [ExceptionNumber], rax           ; save vector number\r
                pop     rax\r
                add     rsp, 8                           ; pop vector number\r
                mov     [AppRsp], rsp                    ; save stack top\r
                mov     rsp, DebugStackBegin             ; switch to debugger stack\r
                sub     rsp, 8                           ; leave space for vector number\r
\r
;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;\r
;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;\r
                push    r15\r
                push    r14\r
                push    r13\r
                push    r12\r
                push    r11\r
                push    r10\r
                push    r9\r
                push    r8\r
                push    rax\r
                push    rcx\r
                push    rdx\r
                push    rbx\r
                push    rsp\r
                push    rbp\r
                push    rsi\r
                push    rdi\r
\r
;; Save interrupt state rflags register...\r
                pushfq\r
                pop     rax\r
                mov     [Rflags], rax\r
\r
;; We need to determine if any extra data was pushed by the exception, and if so, save it\r
;; To do this, we check the exception number pushed by the stub, and cache the\r
;; result in a variable since we'll need this again.\r
                cmp     qword [ExceptionNumber], EXCPT64_DOUBLE_FAULT\r
                jz      ExtraPushOne\r
                cmp     qword [ExceptionNumber], EXCPT64_INVALID_TSS\r
                jz      ExtraPushOne\r
                cmp     qword [ExceptionNumber], EXCPT64_SEG_NOT_PRESENT\r
                jz      ExtraPushOne\r
                cmp     qword [ExceptionNumber], EXCPT64_STACK_FAULT\r
                jz      ExtraPushOne\r
                cmp     qword [ExceptionNumber], EXCPT64_GP_FAULT\r
                jz      ExtraPushOne\r
                cmp     qword [ExceptionNumber], EXCPT64_PAGE_FAULT\r
                jz      ExtraPushOne\r
                cmp     qword [ExceptionNumber], EXCPT64_ALIGNMENT_CHECK\r
                jz      ExtraPushOne\r
                mov     qword [ExtraPush], 0\r
                mov     qword [ExceptData], 0\r
                jmp     ExtraPushDone\r
ExtraPushOne:\r
                mov     qword [ExtraPush], 1\r
\r
;; If there's some extra data, save it also, and modify the saved AppRsp to effectively\r
;; pop this value off the application's stack.\r
                mov     rax, [AppRsp]\r
                mov     rbx, [rax]\r
                mov     qword [ExceptData], rbx\r
                add     rax, 8\r
                mov     [AppRsp], rax\r
\r
ExtraPushDone:\r
\r
;; The "push" above pushed the debug stack rsp.  Since what we're actually doing\r
;; is building the context record on the debug stack, we need to save the pushed\r
;; debug RSP, and replace it with the application's last stack entry...\r
                mov     rax, [rsp + 24]\r
                mov     [DebugRsp], rax\r
                mov     rax, [AppRsp]\r
                mov     rax, QWORD [rax + 24]\r
                ; application stack has ss, rsp, rflags, cs, & rip, so\r
                ; last actual application stack entry is saved at offset\r
                ; 24 bytes from stack top.\r
                mov     [rsp + 24], rax\r
\r
;; continue building context record\r
;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;  insure high 16 bits of each is zero\r
                mov     rax, ss\r
                push    rax\r
\r
                ; CS from application is one entry back in application stack\r
                mov     rax, [AppRsp]\r
                movzx   rax, word [rax + 8]\r
                push    rax\r
\r
                mov     rax, ds\r
                push    rax\r
                mov     rax, es\r
                push    rax\r
                mov     rax, fs\r
                push    rax\r
                mov     rax, gs\r
                push    rax\r
\r
;; UINT64  Rip;\r
                ; Rip from application is on top of application stack\r
                mov     rax, [AppRsp]\r
                push    qword [rax]\r
\r
;; UINT64  Gdtr[2], Idtr[2];\r
                push    0\r
                push    0\r
                sidt    [rsp]\r
                push    0\r
                push    0\r
                sgdt    [rsp]\r
\r
;; UINT64  Ldtr, Tr;\r
                xor     rax, rax\r
                str     ax\r
                push    rax\r
                sldt    ax\r
                push    rax\r
\r
;; UINT64  RFlags;\r
;; Rflags from application is two entries back in application stack\r
                mov     rax, [AppRsp]\r
                push    qword [rax + 16]\r
\r
;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;\r
;; insure FXSAVE/FXRSTOR is enabled in CR4...\r
;; ... while we're at it, make sure DE is also enabled...\r
                mov     rax, cr8\r
                push    rax\r
                mov     rax, cr4\r
                or      rax, 0x208\r
                mov     cr4, rax\r
                push    rax\r
                mov     rax, cr3\r
                push    rax\r
                mov     rax, cr2\r
                push    rax\r
                push    0\r
                mov     rax, cr0\r
                push    rax\r
\r
;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;\r
                mov     rax, dr7\r
                push    rax\r
;; clear Dr7 while executing debugger itself\r
                xor     rax, rax\r
                mov     dr7, rax\r
\r
                mov     rax, dr6\r
                push    rax\r
;; insure all status bits in dr6 are clear...\r
                xor     rax, rax\r
                mov     dr6, rax\r
\r
                mov     rax, dr3\r
                push    rax\r
                mov     rax, dr2\r
                push    rax\r
                mov     rax, dr1\r
                push    rax\r
                mov     rax, dr0\r
                push    rax\r
\r
;; FX_SAVE_STATE_X64 FxSaveState;\r
                sub     rsp, 512\r
                mov     rdi, rsp\r
                ; IMPORTANT!! The debug stack has been carefully constructed to\r
                ; insure that rsp and rdi are 16 byte aligned when we get here.\r
                ; They MUST be.  If they are not, a GP fault will occur.\r
                FXSTOR_RDI\r
\r
;; UEFI calling convention for x64 requires that Direction flag in EFLAGs is clear\r
                cld\r
\r
;; UINT64  ExceptionData;\r
                mov     rax, [ExceptData]\r
                push    rax\r
\r
; call to C code which will in turn call registered handler\r
; pass in the vector number\r
                mov     rdx, rsp\r
                mov     rcx, [ExceptionNumber]\r
                sub     rsp, 40\r
                call    ASM_PFX(InterruptDistrubutionHub)\r
                add     rsp, 40\r
\r
; restore context...\r
;; UINT64  ExceptionData;\r
                add     rsp, 8\r
\r
;; FX_SAVE_STATE_X64 FxSaveState;\r
                mov     rsi, rsp\r
                FXRSTOR_RSI\r
                add     rsp, 512\r
\r
;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;\r
                pop     rax\r
                mov     dr0, rax\r
                pop     rax\r
                mov     dr1, rax\r
                pop     rax\r
                mov     dr2, rax\r
                pop     rax\r
                mov     dr3, rax\r
;; skip restore of dr6.  We cleared dr6 during the context save.\r
                add     rsp, 8\r
                pop     rax\r
                mov     dr7, rax\r
\r
;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;\r
                pop     rax\r
                mov     cr0, rax\r
                add     rsp, 8\r
                pop     rax\r
                mov     cr2, rax\r
                pop     rax\r
                mov     cr3, rax\r
                pop     rax\r
                mov     cr4, rax\r
                pop     rax\r
                mov     cr8, rax\r
\r
;; UINT64  RFlags;\r
                mov     rax, [AppRsp]\r
                pop     qword [rax + 16]\r
\r
;; UINT64  Ldtr, Tr;\r
;; UINT64  Gdtr[2], Idtr[2];\r
;; Best not let anyone mess with these particular registers...\r
                add     rsp, 48\r
\r
;; UINT64  Rip;\r
                pop     qword [rax]\r
\r
;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;\r
;; NOTE - modified segment registers could hang the debugger...  We\r
;;        could attempt to insulate ourselves against this possibility,\r
;;        but that poses risks as well.\r
;;\r
\r
                pop     rax\r
                ; mov     gs, rax\r
                pop     rax\r
                ; mov     fs, rax\r
                pop     rax\r
                mov     es, rax\r
                pop     rax\r
                mov     ds, rax\r
                mov     rax, [AppRsp]\r
                pop     qword [rax + 8]\r
                pop     rax\r
                mov     ss, rax\r
\r
;; The next stuff to restore is the general purpose registers that were pushed\r
;; using the "push" instruction.\r
;;\r
;; The value of RSP as stored in the context record is the application RSP\r
;; including the 5 entries on the application stack caused by the exception\r
;; itself. It may have been modified by the debug agent, so we need to\r
;; determine if we need to relocate the application stack.\r
\r
                mov     rbx, [rsp + 24]  ; move the potentially modified AppRsp into rbx\r
                mov     rax, [AppRsp]\r
                mov     rax, QWORD [rax + 24]\r
                cmp     rbx, rax\r
                je      NoAppStackMove\r
\r
                mov     rax, [AppRsp]\r
                mov     rcx, [rax]       ; RIP\r
                mov     [rbx], rcx\r
\r
                mov     rcx, [rax + 8]   ; CS\r
                mov     [rbx + 8], rcx\r
\r
                mov     rcx, [rax + 16]  ; RFLAGS\r
                mov     [rbx + 16], rcx\r
\r
                mov     rcx, [rax + 24]  ; RSP\r
                mov     [rbx + 24], rcx\r
\r
                mov     rcx, [rax + 32]  ; SS\r
                mov     [rbx + 32], rcx\r
\r
                mov     rax, rbx         ; modify the saved AppRsp to the new AppRsp\r
                mov     [AppRsp], rax\r
NoAppStackMove:\r
                mov     rax, [DebugRsp]  ; restore the DebugRsp on the debug stack\r
                                         ; so our "pop" will not cause a stack switch\r
                mov     [rsp + 24], rax\r
\r
                cmp     qword [ExceptionNumber], 0x68\r
                jne     NoChain\r
\r
Chain:\r
\r
;; Restore rflags so when we chain, the flags will be exactly as if we were never here.\r
;; We gin up the stack to do an iretq so we can get ALL the flags.\r
                mov     rax, [AppRsp]\r
                mov     rbx, [rax + 40]\r
                push    rbx\r
                mov     rax, ss\r
                push    rax\r
                mov     rax, rsp\r
                add     rax, 16\r
                push    rax\r
                mov     rax, [AppRsp]\r
                mov     rbx, [rax + 16]\r
                and     rbx, ~ 0x300 ; special handling for IF and TF\r
                push    rbx\r
                mov     rax, cs\r
                push    rax\r
                mov     rax, PhonyIretq\r
                push    rax\r
                iretq\r
PhonyIretq:\r
\r
;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;\r
;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;\r
                pop     rdi\r
                pop     rsi\r
                pop     rbp\r
                pop     rsp\r
                pop     rbx\r
                pop     rdx\r
                pop     rcx\r
                pop     rax\r
                pop     r8\r
                pop     r9\r
                pop     r10\r
                pop     r11\r
                pop     r12\r
                pop     r13\r
                pop     r14\r
                pop     r15\r
\r
;; Switch back to application stack\r
                mov     rsp, [AppRsp]\r
\r
;; Jump to original handler\r
                jmp     [ASM_PFX(OrigVector)]\r
\r
NoChain:\r
;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;\r
;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;\r
                pop     rdi\r
                pop     rsi\r
                pop     rbp\r
                pop     rsp\r
                pop     rbx\r
                pop     rdx\r
                pop     rcx\r
                pop     rax\r
                pop     r8\r
                pop     r9\r
                pop     r10\r
                pop     r11\r
                pop     r12\r
                pop     r13\r
                pop     r14\r
                pop     r15\r
\r
;; Switch back to application stack\r
                mov     rsp, [AppRsp]\r
\r
;; We're outa here...\r
                iretq\r
\r
Commit	Line	Data
63b865c6 JJ	1	;/** @file\r
	2	; Low level x64 routines used by the debug support driver.\r
	3	;\r
	4	; Copyright (c) 2007 - 2016, Intel Corporation. All rights reserved.<BR>\r
	5	; This program and the accompanying materials\r
	6	; are licensed and made available under the terms and conditions of the BSD License\r
	7	; which accompanies this distribution. The full text of the license may be found at\r
	8	; http://opensource.org/licenses/bsd-license.php\r
	9	;\r
	10	; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
	11	; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
	12	;\r
	13	;**/\r
	14	\r
	15	%define EXCPT64_DIVIDE_ERROR 0\r
	16	%define EXCPT64_DEBUG 1\r
	17	%define EXCPT64_NMI 2\r
	18	%define EXCPT64_BREAKPOINT 3\r
	19	%define EXCPT64_OVERFLOW 4\r
	20	%define EXCPT64_BOUND 5\r
	21	%define EXCPT64_INVALID_OPCODE 6\r
	22	%define EXCPT64_DOUBLE_FAULT 8\r
	23	%define EXCPT64_INVALID_TSS 10\r
	24	%define EXCPT64_SEG_NOT_PRESENT 11\r
	25	%define EXCPT64_STACK_FAULT 12\r
	26	%define EXCPT64_GP_FAULT 13\r
	27	%define EXCPT64_PAGE_FAULT 14\r
	28	%define EXCPT64_FP_ERROR 16\r
	29	%define EXCPT64_ALIGNMENT_CHECK 17\r
	30	%define EXCPT64_MACHINE_CHECK 18\r
	31	%define EXCPT64_SIMD 19\r
	32	\r
	33	%define FXSTOR_FLAG 0x1000000 ; bit cpuid 24 of feature flags\r
	34	\r
	35	;; The FXSTOR and FXRSTOR commands are used for saving and restoring the x87,\r
	36	;; MMX, SSE, SSE2, etc registers. The initialization of the debugsupport driver\r
	37	;; MUST check the CPUID feature flags to see that these instructions are available\r
	38	;; and fail to init if they are not.\r
	39	\r
	40	;; fxstor [rdi]\r
	41	%macro FXSTOR_RDI 0\r
	42	db 0xf, 0xae, 00000111y ; mod = 00, reg/op = 000, r/m = 111 = [rdi]\r
	43	%endmacro\r
	44	\r
	45	;; fxrstor [rsi]\r
	46	%macro FXRSTOR_RSI 0\r
	47	db 0xf, 0xae, 00001110y ; mod = 00, reg/op = 001, r/m = 110 = [rsi]\r
	48	%endmacro\r
	49	\r
	50	SECTION .data\r
	51	\r
	52	global ASM_PFX(OrigVector)\r
	53	global ASM_PFX(InterruptEntryStub)\r
	54	global ASM_PFX(StubSize)\r
	55	global ASM_PFX(CommonIdtEntry)\r
	56	global ASM_PFX(FxStorSupport)\r
	57	extern ASM_PFX(InterruptDistrubutionHub)\r
	58	\r
	59	ASM_PFX(StubSize): dd InterruptEntryStubEnd - ASM_PFX(InterruptEntryStub)\r
	60	AppRsp: dq 0x1111111111111111 ; ?\r
	61	DebugRsp: dq 0x2222222222222222 ; ?\r
	62	ExtraPush: dq 0x3333333333333333 ; ?\r
	63	ExceptData: dq 0x4444444444444444 ; ?\r
	64	Rflags: dq 0x5555555555555555 ; ?\r
65	ASM_PFX(OrigVector): dq 0x6666666666666666 ; ?\r
66	\r
67	;; The declarations below define the memory region that will be used for the debug stack.\r
68	;; The context record will be built by pushing register values onto this stack.\r
69	;; It is imparitive that alignment be carefully managed, since the FXSTOR and\r
70	;; FXRSTOR instructions will GP fault if their memory operand is not 16 byte aligned.\r
71	;;\r
72	;; The stub will switch stacks from the application stack to the debuger stack\r
73	;; and pushes the exception number.\r
74	;;\r
75	;; Then we building the context record on the stack. Since the stack grows down,\r
76	;; we push the fields of the context record from the back to the front. There\r
77	;; are 336 bytes of stack used prior allocating the 512 bytes of stack to be\r
78	;; used as the memory buffer for the fxstor instruction. Therefore address of\r
79	;; the buffer used for the FXSTOR instruction is &Eax - 336 - 512, which\r
80	;; must be 16 byte aligned.\r
81	;;\r
82	;; We carefully locate the stack to make this happen.\r
83	;;\r
84	;; For reference, the context structure looks like this:\r
85	;; struct {\r
86	;; UINT64 ExceptionData;\r
87	;; FX_SAVE_STATE_X64 FxSaveState; // 512 bytes, must be 16 byte aligned\r
88	;; UINT64 Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;\r
89	;; UINT64 Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;\r
90	;; UINT64 RFlags;\r
91	;; UINT64 Ldtr, Tr;\r
92	;; UINT64 Gdtr[2], Idtr[2];\r
93	;; UINT64 Rip;\r
94	;; UINT64 Gs, Fs, Es, Ds, Cs, Ss;\r
95	;; UINT64 Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;\r
96	;; UINT64 R8, R9, R10, R11, R12, R13, R14, R15;\r
97	;; } SYSTEM_CONTEXT_X64; // 64 bit system context record\r
98	\r
99	align 16\r
100	DebugStackEnd: db "DbgStkEnd >>>>>>" ;; 16 byte long string - must be 16 bytes to preserve alignment\r
101	times 0x1ffc dd 0x0 ;; 32K should be enough stack\r
102	;; This allocation is coocked to insure\r
103	;; that the the buffer for the FXSTORE instruction\r
104	;; will be 16 byte aligned also.\r
105	;;\r
106	ExceptionNumber: dq 0 ;; first entry will be the vector number pushed by the stub\r
107	\r
108	DebugStackBegin: db "<<<< DbgStkBegin" ;; initial debug ESP == DebugStackBegin, set in stub\r
109	\r
110	DEFAULT REL\r
111	SECTION .text\r
112	\r
113	;------------------------------------------------------------------------------\r
114	; BOOLEAN\r
115	; FxStorSupport (\r
116	; void\r
117	; )\r
118	;\r
119	; Abstract: Returns TRUE if FxStor instructions are supported\r
120	;\r
121	global ASM_PFX(FxStorSupport)\r
122	ASM_PFX(FxStorSupport):\r
123	\r
124	;\r
125	; cpuid corrupts rbx which must be preserved per the C calling convention\r
126	;\r
127	push rbx\r
128	mov rax, dword 1\r
129	cpuid\r
130	mov eax, edx\r
131	and rax, FXSTOR_FLAG\r
132	shr rax, 24\r
133	pop rbx\r
134	ret\r
135	\r
136	;------------------------------------------------------------------------------\r
137	; void\r
138	; Vect2Desc (\r
139	; IA32_IDT_GATE_DESCRIPTOR * DestDesc, // rcx\r
140	; void (*Vector) (void) // rdx\r
141	; )\r
142	;\r
143	; Abstract: Encodes an IDT descriptor with the given physical address\r
144	;\r
145	global ASM_PFX(Vect2Desc)\r
146	ASM_PFX(Vect2Desc):\r
147	\r
148	mov rax, rdx\r
149	mov word [rcx], ax ; write bits 15..0 of offset\r
150	mov dx, cs\r
151	mov word [rcx+2], dx ; SYS_CODE_SEL from GDT\r
152	mov word [rcx+4], 0xe00 \| 0x8000 ; type = 386 interrupt gate, present\r
153	shr rax, 16\r
154	mov word [rcx+6], ax ; write bits 31..16 of offset\r
155	shr rax, 16\r
156	mov dword [rcx+8], eax ; write bits 63..32 of offset\r
157	\r
158	ret\r
159	\r
160	;------------------------------------------------------------------------------\r
161	; InterruptEntryStub\r
162	;\r
163	; Abstract: This code is not a function, but is a small piece of code that is\r
164	; copied and fixed up once for each IDT entry that is hooked.\r
165	;\r
166	ASM_PFX(InterruptEntryStub):\r
167	push 0 ; push vector number - will be modified before installed\r
168	db 0xe9 ; jump rel32\r
169	dd 0 ; fixed up to relative address of CommonIdtEntry\r
170	InterruptEntryStubEnd:\r
171	\r
172	;------------------------------------------------------------------------------\r
173	; CommonIdtEntry\r
174	;\r
175	; Abstract: This code is not a function, but is the common part for all IDT\r
176	; vectors.\r
177	;\r
178	ASM_PFX(CommonIdtEntry):\r
179	;;\r
180	;; At this point, the stub has saved the current application stack esp into AppRsp\r
181	;; and switched stacks to the debug stack, where it pushed the vector number\r
182	;;\r
183	;; The application stack looks like this:\r
184	;;\r
185	;; ...\r
186	;; (last application stack entry)\r
187	;; [16 bytes alignment, do not care it]\r
188	;; SS from interrupted task\r
189	;; RSP from interrupted task\r
190	;; rflags from interrupted task\r
191	;; CS from interrupted task\r
192	;; RIP from interrupted task\r
193	;; Error code <-------------------- Only present for some exeption types\r
194	;;\r
195	;; Vector Number <----------------- pushed in our IDT Entry\r
196	;;\r
197	\r
198	;; The stub switched us to the debug stack and pushed the interrupt number.\r
199	;;\r
200	;; Next, construct the context record. It will be build on the debug stack by\r
201	;; pushing the registers in the correct order so as to create the context structure\r
202	;; on the debug stack. The context record must be built from the end back to the\r
203	;; beginning because the stack grows down...\r
204	;\r
205	;; For reference, the context record looks like this:\r
206	;;\r
207	;; typedef\r
208	;; struct {\r
209	;; UINT64 ExceptionData;\r
210	;; FX_SAVE_STATE_X64 FxSaveState;\r
211	;; UINT64 Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;\r
212	;; UINT64 Cr0, Cr2, Cr3, Cr4, Cr8;\r
213	;; UINT64 RFlags;\r
214	;; UINT64 Ldtr, Tr;\r
215	;; UINT64 Gdtr[2], Idtr[2];\r
216	;; UINT64 Rip;\r
217	;; UINT64 Gs, Fs, Es, Ds, Cs, Ss;\r
218	;; UINT64 Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;\r
219	;; UINT64 R8, R9, R10, R11, R12, R13, R14, R15;\r
220	;; } SYSTEM_CONTEXT_X64; // 64 bit system context record\r
221	\r
222	;; NOTE: we save rsp here to prevent compiler put rip reference cause error AppRsp\r
223	push rax\r
224	mov rax, qword [rsp+8] ; save vector number\r
225	mov [ExceptionNumber], rax ; save vector number\r
226	pop rax\r
227	add rsp, 8 ; pop vector number\r
228	mov [AppRsp], rsp ; save stack top\r
229	mov rsp, DebugStackBegin ; switch to debugger stack\r
230	sub rsp, 8 ; leave space for vector number\r
231	\r
232	;; UINT64 Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;\r
233	;; UINT64 R8, R9, R10, R11, R12, R13, R14, R15;\r
234	push r15\r
235	push r14\r
236	push r13\r
237	push r12\r
238	push r11\r
239	push r10\r
240	push r9\r
241	push r8\r
242	push rax\r
243	push rcx\r
244	push rdx\r
245	push rbx\r
246	push rsp\r
247	push rbp\r
248	push rsi\r
249	push rdi\r
250	\r
251	;; Save interrupt state rflags register...\r
252	pushfq\r
253	pop rax\r
254	mov [Rflags], rax\r
255	\r
256	;; We need to determine if any extra data was pushed by the exception, and if so, save it\r
257	;; To do this, we check the exception number pushed by the stub, and cache the\r
258	;; result in a variable since we'll need this again.\r
259	cmp qword [ExceptionNumber], EXCPT64_DOUBLE_FAULT\r
260	jz ExtraPushOne\r
261	cmp qword [ExceptionNumber], EXCPT64_INVALID_TSS\r
262	jz ExtraPushOne\r
263	cmp qword [ExceptionNumber], EXCPT64_SEG_NOT_PRESENT\r
264	jz ExtraPushOne\r
265	cmp qword [ExceptionNumber], EXCPT64_STACK_FAULT\r
266	jz ExtraPushOne\r
267	cmp qword [ExceptionNumber], EXCPT64_GP_FAULT\r
268	jz ExtraPushOne\r
269	cmp qword [ExceptionNumber], EXCPT64_PAGE_FAULT\r
270	jz ExtraPushOne\r
271	cmp qword [ExceptionNumber], EXCPT64_ALIGNMENT_CHECK\r
272	jz ExtraPushOne\r
273	mov qword [ExtraPush], 0\r
274	mov qword [ExceptData], 0\r
275	jmp ExtraPushDone\r
276	ExtraPushOne:\r
277	mov qword [ExtraPush], 1\r
278	\r
279	;; If there's some extra data, save it also, and modify the saved AppRsp to effectively\r
280	;; pop this value off the application's stack.\r
281	mov rax, [AppRsp]\r
282	mov rbx, [rax]\r
283	mov qword [ExceptData], rbx\r
284	add rax, 8\r
285	mov [AppRsp], rax\r
286	\r
287	ExtraPushDone:\r
288	\r
289	;; The "push" above pushed the debug stack rsp. Since what we're actually doing\r
290	;; is building the context record on the debug stack, we need to save the pushed\r
291	;; debug RSP, and replace it with the application's last stack entry...\r
292	mov rax, [rsp + 24]\r
293	mov [DebugRsp], rax\r
294	mov rax, [AppRsp]\r
295	mov rax, QWORD [rax + 24]\r
296	; application stack has ss, rsp, rflags, cs, & rip, so\r
297	; last actual application stack entry is saved at offset\r
298	; 24 bytes from stack top.\r
299	mov [rsp + 24], rax\r
300	\r
301	;; continue building context record\r
302	;; UINT64 Gs, Fs, Es, Ds, Cs, Ss; insure high 16 bits of each is zero\r
303	mov rax, ss\r
304	push rax\r
305	\r
306	; CS from application is one entry back in application stack\r
307	mov rax, [AppRsp]\r
308	movzx rax, word [rax + 8]\r
309	push rax\r
310	\r
311	mov rax, ds\r
312	push rax\r
313	mov rax, es\r
314	push rax\r
315	mov rax, fs\r
316	push rax\r
317	mov rax, gs\r
318	push rax\r
319	\r
320	;; UINT64 Rip;\r
321	; Rip from application is on top of application stack\r
322	mov rax, [AppRsp]\r
323	push qword [rax]\r
324	\r
325	;; UINT64 Gdtr[2], Idtr[2];\r
326	push 0\r
327	push 0\r
328	sidt [rsp]\r
329	push 0\r
330	push 0\r
331	sgdt [rsp]\r
332	\r
333	;; UINT64 Ldtr, Tr;\r
334	xor rax, rax\r
335	str ax\r
336	push rax\r
337	sldt ax\r
338	push rax\r
339	\r
340	;; UINT64 RFlags;\r
341	;; Rflags from application is two entries back in application stack\r
342	mov rax, [AppRsp]\r
343	push qword [rax + 16]\r
344	\r
345	;; UINT64 Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;\r
346	;; insure FXSAVE/FXRSTOR is enabled in CR4...\r
347	;; ... while we're at it, make sure DE is also enabled...\r
348	mov rax, cr8\r
349	push rax\r
350	mov rax, cr4\r
351	or rax, 0x208\r
352	mov cr4, rax\r
353	push rax\r
354	mov rax, cr3\r
355	push rax\r
356	mov rax, cr2\r
357	push rax\r
358	push 0\r
359	mov rax, cr0\r
360	push rax\r
361	\r
362	;; UINT64 Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;\r
363	mov rax, dr7\r
364	push rax\r
365	;; clear Dr7 while executing debugger itself\r
366	xor rax, rax\r
367	mov dr7, rax\r
368	\r
369	mov rax, dr6\r
370	push rax\r
371	;; insure all status bits in dr6 are clear...\r
372	xor rax, rax\r
373	mov dr6, rax\r
374	\r
375	mov rax, dr3\r
376	push rax\r
377	mov rax, dr2\r
378	push rax\r
379	mov rax, dr1\r
380	push rax\r
381	mov rax, dr0\r
382	push rax\r
383	\r
384	;; FX_SAVE_STATE_X64 FxSaveState;\r
385	sub rsp, 512\r
386	mov rdi, rsp\r
387	; IMPORTANT!! The debug stack has been carefully constructed to\r
388	; insure that rsp and rdi are 16 byte aligned when we get here.\r
389	; They MUST be. If they are not, a GP fault will occur.\r
390	FXSTOR_RDI\r
391	\r
392	;; UEFI calling convention for x64 requires that Direction flag in EFLAGs is clear\r
393	cld\r
394	\r
395	;; UINT64 ExceptionData;\r
396	mov rax, [ExceptData]\r
397	push rax\r
398	\r
399	; call to C code which will in turn call registered handler\r
400	; pass in the vector number\r
401	mov rdx, rsp\r
402	mov rcx, [ExceptionNumber]\r
403	sub rsp, 40\r
404	call ASM_PFX(InterruptDistrubutionHub)\r
405	add rsp, 40\r
406	\r
407	; restore context...\r
408	;; UINT64 ExceptionData;\r
409	add rsp, 8\r
410	\r
411	;; FX_SAVE_STATE_X64 FxSaveState;\r
412	mov rsi, rsp\r
413	FXRSTOR_RSI\r
414	add rsp, 512\r
415	\r
416	;; UINT64 Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;\r
417	pop rax\r
418	mov dr0, rax\r
419	pop rax\r
420	mov dr1, rax\r
421	pop rax\r
422	mov dr2, rax\r
423	pop rax\r
424	mov dr3, rax\r
425	;; skip restore of dr6. We cleared dr6 during the context save.\r
426	add rsp, 8\r
427	pop rax\r
428	mov dr7, rax\r
429	\r
430	;; UINT64 Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;\r
431	pop rax\r
432	mov cr0, rax\r
433	add rsp, 8\r
434	pop rax\r
435	mov cr2, rax\r
436	pop rax\r
437	mov cr3, rax\r
438	pop rax\r
439	mov cr4, rax\r
440	pop rax\r
441	mov cr8, rax\r
442	\r
443	;; UINT64 RFlags;\r
444	mov rax, [AppRsp]\r
445	pop qword [rax + 16]\r
446	\r
447	;; UINT64 Ldtr, Tr;\r
448	;; UINT64 Gdtr[2], Idtr[2];\r
449	;; Best not let anyone mess with these particular registers...\r
450	add rsp, 48\r
451	\r
452	;; UINT64 Rip;\r
453	pop qword [rax]\r
454	\r
455	;; UINT64 Gs, Fs, Es, Ds, Cs, Ss;\r
456	;; NOTE - modified segment registers could hang the debugger... We\r
457	;; could attempt to insulate ourselves against this possibility,\r
458	;; but that poses risks as well.\r
459	;;\r
460	\r
461	pop rax\r
462	; mov gs, rax\r
463	pop rax\r
464	; mov fs, rax\r
465	pop rax\r
466	mov es, rax\r
467	pop rax\r
468	mov ds, rax\r
469	mov rax, [AppRsp]\r
470	pop qword [rax + 8]\r
471	pop rax\r
472	mov ss, rax\r
473	\r
474	;; The next stuff to restore is the general purpose registers that were pushed\r
475	;; using the "push" instruction.\r
476	;;\r
477	;; The value of RSP as stored in the context record is the application RSP\r
478	;; including the 5 entries on the application stack caused by the exception\r
479	;; itself. It may have been modified by the debug agent, so we need to\r
480	;; determine if we need to relocate the application stack.\r
481	\r
482	mov rbx, [rsp + 24] ; move the potentially modified AppRsp into rbx\r
483	mov rax, [AppRsp]\r
484	mov rax, QWORD [rax + 24]\r
485	cmp rbx, rax\r
486	je NoAppStackMove\r
487	\r
488	mov rax, [AppRsp]\r
489	mov rcx, [rax] ; RIP\r
490	mov [rbx], rcx\r
491	\r
492	mov rcx, [rax + 8] ; CS\r
493	mov [rbx + 8], rcx\r
494	\r
495	mov rcx, [rax + 16] ; RFLAGS\r
496	mov [rbx + 16], rcx\r
497	\r
498	mov rcx, [rax + 24] ; RSP\r
499	mov [rbx + 24], rcx\r
500	\r
501	mov rcx, [rax + 32] ; SS\r
502	mov [rbx + 32], rcx\r
503	\r
504	mov rax, rbx ; modify the saved AppRsp to the new AppRsp\r
505	mov [AppRsp], rax\r
506	NoAppStackMove:\r
507	mov rax, [DebugRsp] ; restore the DebugRsp on the debug stack\r
508	; so our "pop" will not cause a stack switch\r
509	mov [rsp + 24], rax\r
510	\r
511	cmp qword [ExceptionNumber], 0x68\r
512	jne NoChain\r
513	\r
514	Chain:\r
515	\r
516	;; Restore rflags so when we chain, the flags will be exactly as if we were never here.\r
517	;; We gin up the stack to do an iretq so we can get ALL the flags.\r
518	mov rax, [AppRsp]\r
519	mov rbx, [rax + 40]\r
520	push rbx\r
521	mov rax, ss\r
522	push rax\r
523	mov rax, rsp\r
524	add rax, 16\r
525	push rax\r
526	mov rax, [AppRsp]\r
527	mov rbx, [rax + 16]\r
528	and rbx, ~ 0x300 ; special handling for IF and TF\r
529	push rbx\r
530	mov rax, cs\r
531	push rax\r
532	mov rax, PhonyIretq\r
533	push rax\r
534	iretq\r
535	PhonyIretq:\r
536	\r
537	;; UINT64 Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;\r
538	;; UINT64 R8, R9, R10, R11, R12, R13, R14, R15;\r
539	pop rdi\r
540	pop rsi\r
541	pop rbp\r
542	pop rsp\r
543	pop rbx\r
544	pop rdx\r
545	pop rcx\r
546	pop rax\r
547	pop r8\r
548	pop r9\r
549	pop r10\r
550	pop r11\r
551	pop r12\r
552	pop r13\r
553	pop r14\r
554	pop r15\r
555	\r
556	;; Switch back to application stack\r
557	mov rsp, [AppRsp]\r
558	\r
559	;; Jump to original handler\r
560	jmp [ASM_PFX(OrigVector)]\r
561	\r
562	NoChain:\r
563	;; UINT64 Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;\r
564	;; UINT64 R8, R9, R10, R11, R12, R13, R14, R15;\r
565	pop rdi\r
566	pop rsi\r
567	pop rbp\r
568	pop rsp\r
569	pop rbx\r
570	pop rdx\r
571	pop rcx\r
572	pop rax\r
573	pop r8\r
574	pop r9\r
575	pop r10\r
576	pop r11\r
577	pop r12\r
578	pop r13\r
579	pop r14\r
580	pop r15\r
581	\r
582	;; Switch back to application stack\r
583	mov rsp, [AppRsp]\r
584	\r
585	;; We're outa here...\r
586	iretq\r
587	\r