UefiCpuPkg: Put APs in 64 bit mode before handoff to OS.

[mirror_edk2.git] / UefiCpuPkg / Library / MpInitLib / X64 / MpFuncs.nasm

diff --git a/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
index 5d71995bf8194ce8e7a81a5ccb180d340dc7a97a..c0d7355c6b057d996dafc7db00c256038c3b7a21 100644 (file)
--- a/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
+++ b/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
@@ -1,5 +1,5 @@
 ;------------------------------------------------------------------------------ ;\r
-; Copyright (c) 2015 - 2022, Intel Corporation. All rights reserved.<BR>\r
+; Copyright (c) 2015 - 2023, Intel Corporation. All rights reserved.<BR>\r
 ; SPDX-License-Identifier: BSD-2-Clause-Patent\r
 ;\r
 ; Module Name:\r
@@ -447,6 +447,58 @@ DoHlt:
 BITS 64\r
 AsmRelocateApLoopEnd:\r
 \r
+;-------------------------------------------------------------------------------------\r
+;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, TopOfApStack, CountTofinish, Cr3);\r
+;  This function is called during the finalizaiton of Mp initialization before booting\r
+;  to OS, and aim to put Aps either in Mwait or HLT.\r
+;-------------------------------------------------------------------------------------\r
+; +----------------+\r
+; | Cr3            |  rsp+40\r
+; +----------------+\r
+; | CountTofinish  |  r9\r
+; +----------------+\r
+; | TopOfApStack   |  r8\r
+; +----------------+\r
+; | ApTargetCState |  rdx\r
+; +----------------+\r
+; | MwaitSupport   |  rcx\r
+; +----------------+\r
+; | the return     |\r
+; +----------------+ low address\r
+\r
+AsmRelocateApLoopGenericStart:\r
+    mov        rax, r9           ; CountTofinish\r
+    lock dec   dword [rax]       ; (*CountTofinish)--\r
+\r
+    mov        rax, [rsp + 40]    ; Cr3\r
+    ; Do not push on old stack, since old stack is not mapped\r
+    ; in the page table pointed by cr3\r
+    mov        cr3, rax\r
+    mov        rsp, r8            ; TopOfApStack\r
+\r
+MwaitCheckGeneric:\r
+    cmp        cl, 1              ; Check mwait-monitor support\r
+    jnz        HltLoopGeneric\r
+    mov        rbx, rdx           ; Save C-State to ebx\r
+\r
+MwaitLoopGeneric:\r
+    cli\r
+    mov        rax, rsp           ; Set Monitor Address\r
+    xor        ecx, ecx           ; ecx = 0\r
+    xor        edx, edx           ; edx = 0\r
+    monitor\r
+    mov        rax, rbx           ; Mwait Cx, Target C-State per eax[7:4]\r
+    shl        eax, 4\r
+    mwait\r
+    jmp        MwaitLoopGeneric\r
+\r
+HltLoopGeneric:\r
+    cli\r
+    hlt\r
+    jmp        HltLoopGeneric\r
+\r
+AsmRelocateApLoopGenericEnd:\r
+\r
 ;-------------------------------------------------------------------------------------\r
 ;  AsmGetAddressMap (&AddressMap);\r
 ;-------------------------------------------------------------------------------------\r
@@ -456,6 +508,9 @@ ASM_PFX(AsmGetAddressMap):
     mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RendezvousFunnelAddress], rax\r
     mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.ModeEntryOffset], LongModeStart - RendezvousFunnelProcStart\r
     mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RendezvousFunnelSize], RendezvousFunnelProcEnd - RendezvousFunnelProcStart\r
+lea        rax, [AsmRelocateApLoopGenericStart]\r
+    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddressGeneric], rax\r
+    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSizeGeneric], AsmRelocateApLoopGenericEnd - AsmRelocateApLoopGenericStart\r
     lea        rax, [AsmRelocateApLoopStart]\r
     mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddress], rax\r
     mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSize], AsmRelocateApLoopEnd - AsmRelocateApLoopStart\r