1 files changed, 27 insertions, 12 deletions
diff --git a/MdePkg/Library/BaseLib/Ia32/EnablePaging64.asm b/MdePkg/Library/BaseLib/Ia32/EnablePaging64.asm
index 6c9b0ea76b..97d859e966 100644
--- a/MdePkg/Library/BaseLib/Ia32/EnablePaging64.asm
+++ b/MdePkg/Library/BaseLib/Ia32/EnablePaging64.asm
@@ -26,9 +26,13 @@
     .code
 
 InternalX86EnablePaging64 PROC
-    cli
-    mov     [esp], @F                   ; offset for far retf
-    mov     eax, cr4
+    cli
+
+    mov     ebx, [esp + 4]              ; save CS
+    mov     eax, OFFSET cs_address
+    mov     word ptr [eax], bx          ; Update CS selector for far jump
+
+    mov     eax, cr4 
     or      al, (1 SHL 5)
     mov     cr4, eax                    ; enable PAE
     mov     ecx, 0c0000080h
@@ -37,19 +41,30 @@ InternalX86EnablePaging64 PROC
     wrmsr
     mov     eax, cr0
     bts     eax, 31
-    mov     cr0, eax                    ; enable paging
-    retf
-@@:                                     ; long mode starts here
-    DB      67h, 48h
-    mov     ebx, [esp]                  ; mov rbx, [esp]
+    mov     cr0, eax                    ; enable paging
+    retf
+    jmp     go_to_long_mode
+go_to_long_mode:
+
+    ;
+    ; This is the next instruction after enabling paging.  Jump to long mode
+    ;
+    db      067h
+    db      0eah                        ;   Far Jump Offset:Selector to reload CS
+    dd      OFFSET in_long_mode         ;   Offset is ensuing instruction boundary
+cs_address:
+    dw      0h                          ;   CS selector will be updated at runtime 
+
+in_long_mode:                           ; now in long mode
     DB      67h, 48h
-    mov     ecx, [esp + 8]              ; mov rcx, [esp + 8]
+    mov     ebx, [esp + 8]              ; mov rbx, [esp]
+    DB      67h, 48h
+                   
+    mov     ecx, [esp + 10h]            ; mov rcx, [esp + 8]
     DB      67h, 48h
-    mov     edx, [esp + 10h]            ; mov rdx, [esp + 10h]
+    mov     edx, [esp + 18h]            ; mov rdx, [esp + 10h]
     DB      67h, 48h
-    mov     esp, [esp + 18h]            ; mov rsp, [esp + 18h]
+    mov     esp, [esp + 20h]            ; mov rsp, [esp + 18h]
     DB      48h
-    add     esp, -20h                   ; add rsp, -20h
     call    ebx                         ; call rbx
     jmp     $
 InternalX86EnablePaging64 ENDP