UefiCpuPkg/PiSmmCpuDxeSmm/Ia32/SmiException.nasm

   1 ;------------------------------------------------------------------------------ ;
   2 ; Copyright (c) 2009 - 2019, Intel Corporation. All rights reserved.<BR>
   3 ; This program and the accompanying materials
   4 ; are licensed and made available under the terms and conditions of the BSD License
   5 ; which accompanies this distribution.  The full text of the license may be found at
   6 ; http://opensource.org/licenses/bsd-license.php.
   7 ;
   8 ; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
   9 ; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
  10 ;
  11 ; Module Name:
  12 ;
  13 ;   SmiException.nasm
  14 ;
  15 ; Abstract:
  16 ;
  17 ;   Exception handlers used in SM mode
  18 ;
  19 ;-------------------------------------------------------------------------------
  20
  21 extern  ASM_PFX(FeaturePcdGet (PcdCpuSmmProfileEnable))
  22 extern  ASM_PFX(SmiPFHandler)
  23 extern  ASM_PFX(mSetupDebugTrap)
  24
  25 global  ASM_PFX(gcSmiIdtr)
  26 global  ASM_PFX(gcSmiGdtr)
  27 global  ASM_PFX(gTaskGateDescriptor)
  28 global  ASM_PFX(gcPsd)
  29
  30     SECTION .data
  31
  32 NullSeg: DQ 0                   ; reserved by architecture
  33 CodeSeg32:
  34             DW      -1                  ; LimitLow
  35             DW      0                   ; BaseLow
  36             DB      0                   ; BaseMid
  37             DB      0x9b
  38             DB      0xcf                ; LimitHigh
  39             DB      0                   ; BaseHigh
  40 ProtModeCodeSeg32:
  41             DW      -1                  ; LimitLow
  42             DW      0                   ; BaseLow
  43             DB      0                   ; BaseMid
  44             DB      0x9b
  45             DB      0xcf                ; LimitHigh
  46             DB      0                   ; BaseHigh
  47 ProtModeSsSeg32:
  48             DW      -1                  ; LimitLow
  49             DW      0                   ; BaseLow
  50             DB      0                   ; BaseMid
  51             DB      0x93
  52             DB      0xcf                ; LimitHigh
  53             DB      0                   ; BaseHigh
  54 DataSeg32:
  55             DW      -1                  ; LimitLow
  56             DW      0                   ; BaseLow
  57             DB      0                   ; BaseMid
  58             DB      0x93
  59             DB      0xcf                ; LimitHigh
  60             DB      0                   ; BaseHigh
  61 CodeSeg16:
  62             DW      -1
  63             DW      0
  64             DB      0
  65             DB      0x9b
  66             DB      0x8f
  67             DB      0
  68 DataSeg16:
  69             DW      -1
  70             DW      0
  71             DB      0
  72             DB      0x93
  73             DB      0x8f
  74             DB      0
  75 CodeSeg64:
  76             DW      -1                  ; LimitLow
  77             DW      0                   ; BaseLow
  78             DB      0                   ; BaseMid
  79             DB      0x9b
  80             DB      0xaf                ; LimitHigh
  81             DB      0                   ; BaseHigh
  82 GDT_SIZE equ $ - NullSeg
  83
  84 TssSeg:
  85             DW      TSS_DESC_SIZE       ; LimitLow
  86             DW      0                   ; BaseLow
  87             DB      0                   ; BaseMid
  88             DB      0x89
  89             DB      0x80                ; LimitHigh
  90             DB      0                   ; BaseHigh
  91 ExceptionTssSeg:
  92             DW      EXCEPTION_TSS_DESC_SIZE       ; LimitLow
  93             DW      0                   ; BaseLow
  94             DB      0                   ; BaseMid
  95             DB      0x89
  96             DB      0x80                ; LimitHigh
  97             DB      0                   ; BaseHigh
  98
  99 CODE_SEL          equ CodeSeg32 - NullSeg
 100 DATA_SEL          equ DataSeg32 - NullSeg
 101 TSS_SEL           equ TssSeg - NullSeg
 102 EXCEPTION_TSS_SEL equ ExceptionTssSeg - NullSeg
 103
 104 struc IA32_TSS
 105                     resw 1
 106                     resw 1
 107   .ESP0: resd 1
 108   .SS0:  resw 1
 109                     resw 1
 110   .ESP1: resd 1
 111   .SS1:  resw 1
 112                     resw 1
 113   .ESP2: resd 1
 114   .SS2:  resw 1
 115                     resw 1
 116   ._CR3: resd 1
 117   .EIP:  resd 1
 118   .EFLAGS: resd 1
 119   ._EAX: resd 1
 120   ._ECX: resd 1
 121   ._EDX: resd 1
 122   ._EBX: resd 1
 123   ._ESP: resd 1
 124   ._EBP: resd 1
 125   ._ESI: resd 1
 126   ._EDI: resd 1
 127   ._ES:  resw 1
 128                     resw 1
 129   ._CS: resw 1
 130                     resw 1
 131   ._SS: resw 1
 132                     resw 1
 133   ._DS: resw 1
 134                     resw 1
 135   ._FS: resw 1
 136                     resw 1
 137   ._GS: resw 1
 138                     resw 1
 139   .LDT: resw 1
 140                     resw 1
 141                     resw 1
 142                     resw 1
 143 endstruc
 144
 145 ; Create 2 TSS segments just after GDT
 146 TssDescriptor:
 147             DW      0                   ; PreviousTaskLink
 148             DW      0                   ; Reserved
 149             DD      0                   ; ESP0
 150             DW      0                   ; SS0
 151             DW      0                   ; Reserved
 152             DD      0                   ; ESP1
 153             DW      0                   ; SS1
 154             DW      0                   ; Reserved
 155             DD      0                   ; ESP2
 156             DW      0                   ; SS2
 157             DW      0                   ; Reserved
 158             DD      0                   ; CR3
 159             DD      0                   ; EIP
 160             DD      0                   ; EFLAGS
 161             DD      0                   ; EAX
 162             DD      0                   ; ECX
 163             DD      0                   ; EDX
 164             DD      0                   ; EBX
 165             DD      0                   ; ESP
 166             DD      0                   ; EBP
 167             DD      0                   ; ESI
 168             DD      0                   ; EDI
 169             DW      0                   ; ES
 170             DW      0                   ; Reserved
 171             DW      0                   ; CS
 172             DW      0                   ; Reserved
 173             DW      0                   ; SS
 174             DW      0                   ; Reserved
 175             DW      0                   ; DS
 176             DW      0                   ; Reserved
 177             DW      0                   ; FS
 178             DW      0                   ; Reserved
 179             DW      0                   ; GS
 180             DW      0                   ; Reserved
 181             DW      0                   ; LDT Selector
 182             DW      0                   ; Reserved
 183             DW      0                   ; T
 184             DW      0                   ; I/O Map Base
 185 TSS_DESC_SIZE equ $ - TssDescriptor
 186
 187 ExceptionTssDescriptor:
 188             DW      0                   ; PreviousTaskLink
 189             DW      0                   ; Reserved
 190             DD      0                   ; ESP0
 191             DW      0                   ; SS0
 192             DW      0                   ; Reserved
 193             DD      0                   ; ESP1
 194             DW      0                   ; SS1
 195             DW      0                   ; Reserved
 196             DD      0                   ; ESP2
 197             DW      0                   ; SS2
 198             DW      0                   ; Reserved
 199             DD      0                   ; CR3
 200             DD      PFHandlerEntry ; EIP
 201             DD      00000002            ; EFLAGS
 202             DD      0                   ; EAX
 203             DD      0                   ; ECX
 204             DD      0                   ; EDX
 205             DD      0                   ; EBX
 206             DD      0                   ; ESP
 207             DD      0                   ; EBP
 208             DD      0                   ; ESI
 209             DD      0                   ; EDI
 210             DW      DATA_SEL            ; ES
 211             DW      0                   ; Reserved
 212             DW      CODE_SEL            ; CS
 213             DW      0                   ; Reserved
 214             DW      DATA_SEL            ; SS
 215             DW      0                   ; Reserved
 216             DW      DATA_SEL            ; DS
 217             DW      0                   ; Reserved
 218             DW      DATA_SEL            ; FS
 219             DW      0                   ; Reserved
 220             DW      DATA_SEL            ; GS
 221             DW      0                   ; Reserved
 222             DW      0                   ; LDT Selector
 223             DW      0                   ; Reserved
 224             DW      0                   ; T
 225             DW      0                   ; I/O Map Base
 226             DD      0                   ; SSP
 227 EXCEPTION_TSS_DESC_SIZE equ $ - ExceptionTssDescriptor
 228
 229 ASM_PFX(gcPsd):
 230             DB      'PSDSIG  '
 231             DW      PSD_SIZE
 232             DW      2
 233             DW      1 << 2
 234             DW      CODE_SEL
 235             DW      DATA_SEL
 236             DW      DATA_SEL
 237             DW      DATA_SEL
 238             DW      0
 239             DQ      0
 240             DQ      0
 241             DQ      0
 242             DD      0
 243             DD      NullSeg
 244             DD      GDT_SIZE
 245             DD      0
 246             times   24 DB 0
 247             DD      0
 248             DD      0
 249 PSD_SIZE  equ $ - ASM_PFX(gcPsd)
 250
 251 ASM_PFX(gcSmiGdtr):
 252     DW      GDT_SIZE - 1
 253     DD      NullSeg
 254
 255 ASM_PFX(gcSmiIdtr):
 256     DW      0
 257     DD      0
 258
 259 ASM_PFX(gTaskGateDescriptor):
 260     DW      0                           ; Reserved
 261     DW      EXCEPTION_TSS_SEL           ; TSS Segment selector
 262     DB      0                           ; Reserved
 263     DB      0x85                         ; Task Gate, present, DPL = 0
 264     DW      0                           ; Reserved
 265
 266     SECTION .text
 267 ;------------------------------------------------------------------------------
 268 ; PageFaultIdtHandlerSmmProfile is the entry point page fault only
 269 ;
 270 ;
 271 ; Stack:
 272 ; +---------------------+
 273 ; +    EFlags           +
 274 ; +---------------------+
 275 ; +    CS               +
 276 ; +---------------------+
 277 ; +    EIP              +
 278 ; +---------------------+
 279 ; +    Error Code       +
 280 ; +---------------------+
 281 ; +    Vector Number    +
 282 ; +---------------------+
 283 ; +    EBP              +
 284 ; +---------------------+ <-- EBP
 285 ;
 286 ;
 287 ;------------------------------------------------------------------------------
 288 global ASM_PFX(PageFaultIdtHandlerSmmProfile)
 289 ASM_PFX(PageFaultIdtHandlerSmmProfile):
 290     push    0xe                         ; Page Fault
 291
 292     push    ebp
 293     mov     ebp, esp
 294
 295     ;
 296     ; Align stack to make sure that EFI_FX_SAVE_STATE_IA32 of EFI_SYSTEM_CONTEXT_IA32
 297     ; is 16-byte aligned
 298     ;
 299     and     esp, 0xfffffff0
 300     sub     esp, 12
 301
 302 ;; UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
 303     push    eax
 304     push    ecx
 305     push    edx
 306     push    ebx
 307     lea     ecx, [ebp + 6 * 4]
 308     push    ecx                          ; ESP
 309     push    dword [ebp]              ; EBP
 310     push    esi
 311     push    edi
 312
 313 ;; UINT32  Gs, Fs, Es, Ds, Cs, Ss;
 314     mov     eax, ss
 315     push    eax
 316     movzx   eax, word [ebp + 4 * 4]
 317     push    eax
 318     mov     eax, ds
 319     push    eax
 320     mov     eax, es
 321     push    eax
 322     mov     eax, fs
 323     push    eax
 324     mov     eax, gs
 325     push    eax
 326
 327 ;; UINT32  Eip;
 328     mov     eax, [ebp + 3 * 4]
 329     push    eax
 330
 331 ;; UINT32  Gdtr[2], Idtr[2];
 332     sub     esp, 8
 333     sidt    [esp]
 334     mov     eax, [esp + 2]
 335     xchg    eax, [esp]
 336     and     eax, 0xFFFF
 337     mov     [esp+4], eax
 338
 339     sub     esp, 8
 340     sgdt    [esp]
 341     mov     eax, [esp + 2]
 342     xchg    eax, [esp]
 343     and     eax, 0xFFFF
 344     mov     [esp+4], eax
 345
 346 ;; UINT32  Ldtr, Tr;
 347     xor     eax, eax
 348     str     ax
 349     push    eax
 350     sldt    ax
 351     push    eax
 352
 353 ;; UINT32  EFlags;
 354     mov     eax, [ebp + 5 * 4]
 355     push    eax
 356
 357 ;; UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
 358     mov     eax, cr4
 359     or      eax, 0x208
 360     mov     cr4, eax
 361     push    eax
 362     mov     eax, cr3
 363     push    eax
 364     mov     eax, cr2
 365     push    eax
 366     xor     eax, eax
 367     push    eax
 368     mov     eax, cr0
 369     push    eax
 370
 371 ;; UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
 372     mov     eax, dr7
 373     push    eax
 374     mov     eax, dr6
 375     push    eax
 376     mov     eax, dr3
 377     push    eax
 378     mov     eax, dr2
 379     push    eax
 380     mov     eax, dr1
 381     push    eax
 382     mov     eax, dr0
 383     push    eax
 384
 385 ;; FX_SAVE_STATE_IA32 FxSaveState;
 386     sub     esp, 512
 387     mov     edi, esp
 388     fxsave  [edi]
 389
 390 ; UEFI calling convention for IA32 requires that Direction flag in EFLAGs is clear
 391     cld
 392
 393 ;; UINT32  ExceptionData;
 394     push    dword [ebp + 2 * 4]
 395
 396 ;; call into exception handler
 397
 398 ;; Prepare parameter and call
 399     mov     edx, esp
 400     push    edx
 401     mov     edx, dword [ebp + 1 * 4]
 402     push    edx
 403
 404     ;
 405     ; Call External Exception Handler
 406     ;
 407     mov     eax, ASM_PFX(SmiPFHandler)
 408     call    eax
 409     add     esp, 8
 410
 411 ;; UINT32  ExceptionData;
 412     add     esp, 4
 413
 414 ;; FX_SAVE_STATE_IA32 FxSaveState;
 415     mov     esi, esp
 416     fxrstor [esi]
 417     add     esp, 512
 418
 419 ;; UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
 420 ;; Skip restoration of DRx registers to support debuggers
 421 ;; that set breakpoint in interrupt/exception context
 422     add     esp, 4 * 6
 423
 424 ;; UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
 425     pop     eax
 426     mov     cr0, eax
 427     add     esp, 4    ; not for Cr1
 428     pop     eax
 429     mov     cr2, eax
 430     pop     eax
 431     mov     cr3, eax
 432     pop     eax
 433     mov     cr4, eax
 434
 435 ;; UINT32  EFlags;
 436     pop     dword [ebp + 5 * 4]
 437
 438 ;; UINT32  Ldtr, Tr;
 439 ;; UINT32  Gdtr[2], Idtr[2];
 440 ;; Best not let anyone mess with these particular registers...
 441     add     esp, 24
 442
 443 ;; UINT32  Eip;
 444     pop     dword [ebp + 3 * 4]
 445
 446 ;; UINT32  Gs, Fs, Es, Ds, Cs, Ss;
 447 ;; NOTE - modified segment registers could hang the debugger...  We
 448 ;;        could attempt to insulate ourselves against this possibility,
 449 ;;        but that poses risks as well.
 450 ;;
 451     pop     gs
 452     pop     fs
 453     pop     es
 454     pop     ds
 455     pop     dword [ebp + 4 * 4]
 456     pop     ss
 457
 458 ;; UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
 459     pop     edi
 460     pop     esi
 461     add     esp, 4   ; not for ebp
 462     add     esp, 4   ; not for esp
 463     pop     ebx
 464     pop     edx
 465     pop     ecx
 466     pop     eax
 467
 468     mov     esp, ebp
 469     pop     ebp
 470
 471 ; Enable TF bit after page fault handler runs
 472     bts     dword [esp + 16], 8  ; EFLAGS
 473
 474     add     esp, 8                      ; skip INT# & ErrCode
 475 Return:
 476     iretd
 477 ;
 478 ; Page Fault Exception Handler entry when SMM Stack Guard is enabled
 479 ; Executiot starts here after a task switch
 480 ;
 481 PFHandlerEntry:
 482 ;
 483 ; Get this processor's TSS
 484 ;
 485     sub     esp, 8
 486     sgdt    [esp + 2]
 487     mov     eax, [esp + 4]              ; GDT base
 488     add     esp, 8
 489     mov     ecx, [eax + TSS_SEL + 2]
 490     shl     ecx, 8
 491     mov     cl, [eax + TSS_SEL + 7]
 492     ror     ecx, 8                      ; ecx = TSS base
 493
 494     mov     ebp, esp
 495
 496     ;
 497     ; Align stack to make sure that EFI_FX_SAVE_STATE_IA32 of EFI_SYSTEM_CONTEXT_IA32
 498     ; is 16-byte aligned
 499     ;
 500     and     esp, 0xfffffff0
 501     sub     esp, 12
 502
 503 ;; UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
 504     push    dword [ecx + IA32_TSS._EAX]
 505     push    dword [ecx + IA32_TSS._ECX]
 506     push    dword [ecx + IA32_TSS._EDX]
 507     push    dword [ecx + IA32_TSS._EBX]
 508     push    dword [ecx + IA32_TSS._ESP]
 509     push    dword [ecx + IA32_TSS._EBP]
 510     push    dword [ecx + IA32_TSS._ESI]
 511     push    dword [ecx + IA32_TSS._EDI]
 512
 513 ;; UINT32  Gs, Fs, Es, Ds, Cs, Ss;
 514     movzx   eax, word [ecx + IA32_TSS._SS]
 515     push    eax
 516     movzx   eax, word [ecx + IA32_TSS._CS]
 517     push    eax
 518     movzx   eax, word [ecx + IA32_TSS._DS]
 519     push    eax
 520     movzx   eax, word [ecx + IA32_TSS._ES]
 521     push    eax
 522     movzx   eax, word [ecx + IA32_TSS._FS]
 523     push    eax
 524     movzx   eax, word [ecx + IA32_TSS._GS]
 525     push    eax
 526
 527 ;; UINT32  Eip;
 528     push    dword [ecx + IA32_TSS.EIP]
 529
 530 ;; UINT32  Gdtr[2], Idtr[2];
 531     sub     esp, 8
 532     sidt    [esp]
 533     mov     eax, [esp + 2]
 534     xchg    eax, [esp]
 535     and     eax, 0xFFFF
 536     mov     [esp+4], eax
 537
 538     sub     esp, 8
 539     sgdt    [esp]
 540     mov     eax, [esp + 2]
 541     xchg    eax, [esp]
 542     and     eax, 0xFFFF
 543     mov     [esp+4], eax
 544
 545 ;; UINT32  Ldtr, Tr;
 546     mov     eax, TSS_SEL
 547     push    eax
 548     movzx   eax, word [ecx + IA32_TSS.LDT]
 549     push    eax
 550
 551 ;; UINT32  EFlags;
 552     push    dword [ecx + IA32_TSS.EFLAGS]
 553
 554 ;; UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
 555     mov     eax, cr4
 556     or      eax, 0x208
 557     mov     cr4, eax
 558     push    eax
 559     mov     eax, cr3
 560     push    eax
 561     mov     eax, cr2
 562     push    eax
 563     xor     eax, eax
 564     push    eax
 565     mov     eax, cr0
 566     push    eax
 567
 568 ;; UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
 569     mov     eax, dr7
 570     push    eax
 571     mov     eax, dr6
 572     push    eax
 573     mov     eax, dr3
 574     push    eax
 575     mov     eax, dr2
 576     push    eax
 577     mov     eax, dr1
 578     push    eax
 579     mov     eax, dr0
 580     push    eax
 581
 582 ;; FX_SAVE_STATE_IA32 FxSaveState;
 583 ;; Clear TS bit in CR0 to avoid Device Not Available Exception (#NM)
 584 ;; when executing fxsave/fxrstor instruction
 585     clts
 586     sub     esp, 512
 587     mov     edi, esp
 588     fxsave  [edi]
 589
 590 ; UEFI calling convention for IA32 requires that Direction flag in EFLAGs is clear
 591     cld
 592
 593 ;; UINT32  ExceptionData;
 594     push    dword [ebp]
 595
 596 ;; call into exception handler
 597     mov     ebx, ecx
 598     mov     eax, ASM_PFX(SmiPFHandler)
 599
 600 ;; Prepare parameter and call
 601     mov     edx, esp
 602     push    edx
 603     mov     edx, 14
 604     push    edx
 605
 606     ;
 607     ; Call External Exception Handler
 608     ;
 609     call    eax
 610     add     esp, 8
 611
 612     mov     ecx, ebx
 613 ;; UINT32  ExceptionData;
 614     add     esp, 4
 615
 616 ;; FX_SAVE_STATE_IA32 FxSaveState;
 617     mov     esi, esp
 618     fxrstor [esi]
 619     add     esp, 512
 620
 621 ;; UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
 622 ;; Skip restoration of DRx registers to support debuggers
 623 ;; that set breakpoints in interrupt/exception context
 624     add     esp, 4 * 6
 625
 626 ;; UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
 627     pop     eax
 628     mov     cr0, eax
 629     add     esp, 4    ; not for Cr1
 630     pop     eax
 631     mov     cr2, eax
 632     pop     eax
 633     mov     dword [ecx + IA32_TSS._CR3], eax
 634     pop     eax
 635     mov     cr4, eax
 636
 637 ;; UINT32  EFlags;
 638     pop     dword [ecx + IA32_TSS.EFLAGS]
 639
 640 ;; UINT32  Ldtr, Tr;
 641 ;; UINT32  Gdtr[2], Idtr[2];
 642 ;; Best not let anyone mess with these particular registers...
 643     add     esp, 24
 644
 645 ;; UINT32  Eip;
 646     pop     dword [ecx + IA32_TSS.EIP]
 647
 648 ;; UINT32  Gs, Fs, Es, Ds, Cs, Ss;
 649 ;; NOTE - modified segment registers could hang the debugger...  We
 650 ;;        could attempt to insulate ourselves against this possibility,
 651 ;;        but that poses risks as well.
 652 ;;
 653     pop     eax
 654 o16 mov     [ecx + IA32_TSS._GS], ax
 655     pop     eax
 656 o16 mov     [ecx + IA32_TSS._FS], ax
 657     pop     eax
 658 o16 mov     [ecx + IA32_TSS._ES], ax
 659     pop     eax
 660 o16 mov     [ecx + IA32_TSS._DS], ax
 661     pop     eax
 662 o16 mov     [ecx + IA32_TSS._CS], ax
 663     pop     eax
 664 o16 mov     [ecx + IA32_TSS._SS], ax
 665
 666 ;; UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
 667     pop     dword [ecx + IA32_TSS._EDI]
 668     pop     dword [ecx + IA32_TSS._ESI]
 669     add     esp, 4   ; not for ebp
 670     add     esp, 4   ; not for esp
 671     pop     dword [ecx + IA32_TSS._EBX]
 672     pop     dword [ecx + IA32_TSS._EDX]
 673     pop     dword [ecx + IA32_TSS._ECX]
 674     pop     dword [ecx + IA32_TSS._EAX]
 675
 676     mov     esp, ebp
 677
 678 ; Set single step DB# if SMM profile is enabled and page fault exception happens
 679     cmp     byte [dword ASM_PFX(mSetupDebugTrap)], 0
 680     jz      @Done2
 681
 682 ; Create return context for iretd in stub function
 683     mov    eax, dword [ecx + IA32_TSS._ESP]        ; Get old stack pointer
 684     mov    ebx, dword [ecx + IA32_TSS.EIP]
 685     mov    [eax - 0xc], ebx                      ; create EIP in old stack
 686     movzx  ebx, word [ecx + IA32_TSS._CS]
 687     mov    [eax - 0x8], ebx                      ; create CS in old stack
 688     mov    ebx, dword [ecx + IA32_TSS.EFLAGS]
 689     bts    ebx, 8
 690     mov    [eax - 0x4], ebx                      ; create eflags in old stack
 691     mov    eax, dword [ecx + IA32_TSS._ESP]        ; Get old stack pointer
 692     sub    eax, 0xc                              ; minus 12 byte
 693     mov    dword [ecx + IA32_TSS._ESP], eax        ; Set new stack pointer
 694 ; Replace the EIP of interrupted task with stub function
 695     mov    eax, ASM_PFX(PageFaultStubFunction)
 696     mov    dword [ecx + IA32_TSS.EIP], eax
 697 ; Jump to the iretd so next page fault handler as a task will start again after iretd.
 698 @Done2:
 699     add     esp, 4                      ; skip ErrCode
 700
 701     jmp     Return
 702
 703 global ASM_PFX(PageFaultStubFunction)
 704 ASM_PFX(PageFaultStubFunction):
 705 ;
 706 ; we need clean TS bit in CR0 to execute
 707 ; x87 FPU/MMX/SSE/SSE2/SSE3/SSSE3/SSE4 instructions.
 708 ;
 709     clts
 710     iretd
 711