UefiCpuPkg/CpuDxe: Enable protection for newly added page table

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

diff --git a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
index 49f55035309643d38a548896332126ec3a03c59c..2b6c27d4ec675f6389436ce3862b01b22284a5b1 100644 (file)
--- a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
+++ b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
@@ -86,6 +86,12 @@ Flat32Start:                                   ; protected mode entry point
 \r
     mov        esi, ebx\r
 \r
+    ; Increment the number of APs executing here as early as possible\r
+    ; This is decremented in C code when AP is finished executing\r
+    mov        edi, esi\r
+    add        edi, NumApsExecutingLocation\r
+    lock inc   dword [edi]\r
+\r
     mov         edi, esi\r
     add         edi, EnableExecuteDisableLocation\r
     cmp         byte [edi], 0\r
@@ -114,7 +120,12 @@ Flat32Start:                                   ; protected mode entry point
     mov         cr0, eax\r
 \r
 SkipEnableExecuteDisable:\r
+    mov        edi, esi\r
+    add        edi, InitFlagLocation\r
+    cmp        dword [edi], 1       ; 1 == ApInitConfig\r
+    jnz        GetApicId\r
 \r
+    ; AP init\r
     mov        edi, esi\r
     add        edi, LockLocation\r
     mov        eax, NotVacantFlag\r
@@ -124,27 +135,68 @@ TestLock:
     cmp        eax, NotVacantFlag\r
     jz         TestLock\r
 \r
-    mov        edi, esi\r
-    add        edi, NumApsExecutingLocation\r
-    inc        dword [edi]\r
-    mov        ebx, [edi]\r
+    mov        ecx, esi\r
+    add        ecx, ApIndexLocation\r
+    inc        dword [ecx]\r
+    mov        ebx, [ecx]\r
+\r
+Releaselock:\r
+    mov        eax, VacantFlag\r
+    xchg       [edi], eax\r
 \r
-ProgramStack:\r
     mov        edi, esi\r
     add        edi, StackSizeLocation\r
     mov        eax, [edi]\r
+    mov        ecx, ebx\r
+    inc        ecx\r
+    mul        ecx                               ; EAX = StackSize * (CpuNumber + 1)\r
     mov        edi, esi\r
     add        edi, StackStartAddressLocation\r
     add        eax, [edi]\r
     mov        esp, eax\r
-    mov        [edi], eax\r
+    jmp        CProcedureInvoke\r
 \r
-Releaselock:\r
-    mov        eax, VacantFlag\r
-    mov        edi, esi\r
-    add        edi, LockLocation\r
-    xchg       [edi], eax\r
+GetApicId:\r
+    mov        eax, 0\r
+    cpuid\r
+    cmp        eax, 0bh\r
+    jb         NoX2Apic             ; CPUID level below CPUID_EXTENDED_TOPOLOGY\r
+\r
+    mov        eax, 0bh\r
+    xor        ecx, ecx\r
+    cpuid\r
+    test       ebx, 0ffffh\r
+    jz         NoX2Apic             ; CPUID.0BH:EBX[15:0] is zero\r
+\r
+    ; Processor is x2APIC capable; 32-bit x2APIC ID is already in EDX\r
+    jmp        GetProcessorNumber\r
+\r
+NoX2Apic:\r
+    ; Processor is not x2APIC capable, so get 8-bit APIC ID\r
+    mov        eax, 1\r
+    cpuid\r
+    shr        ebx, 24\r
+    mov        edx, ebx\r
+\r
+GetProcessorNumber:\r
+    ;\r
+    ; Get processor number for this AP\r
+    ; Note that BSP may become an AP due to SwitchBsp()\r
+    ;\r
+    xor         ebx, ebx\r
+    lea         eax, [esi + CpuInfoLocation]\r
+    mov         edi, [eax]\r
+\r
+GetNextProcNumber:\r
+    cmp         [edi], edx                       ; APIC ID match?\r
+    jz          ProgramStack\r
+    add         edi, 20\r
+    inc         ebx\r
+    jmp         GetNextProcNumber    \r
 \r
+ProgramStack:\r
+    mov         esp, [edi + 12]\r
+   \r
 CProcedureInvoke:\r
     push       ebp               ; push BIST data at top of AP stack\r
     xor        ebp, ebp          ; clear ebp for call stack trace\r
@@ -154,7 +206,7 @@ CProcedureInvoke:
     mov        eax, ASM_PFX(InitializeFloatingPointUnits)\r
     call       eax               ; Call assembly function to initialize FPU per UEFI spec\r
 \r
-    push       ebx               ; Push NumApsExecuting\r
+    push       ebx               ; Push ApIndex\r
     mov        eax, esi\r
     add        eax, LockLocation\r
     push       eax               ; push address of exchange info data buffer\r
@@ -169,19 +221,28 @@ CProcedureInvoke:
 RendezvousFunnelProcEnd:\r
 \r
 ;-------------------------------------------------------------------------------------\r
-;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, PmCodeSegment);\r
+;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, PmCodeSegment, TopOfApStack, CountTofinish);\r
 ;-------------------------------------------------------------------------------------\r
 global ASM_PFX(AsmRelocateApLoop)\r
 ASM_PFX(AsmRelocateApLoop):\r
 AsmRelocateApLoopStart:\r
-    cmp        byte [esp + 4], 1\r
+    mov        eax, esp\r
+    mov        esp, [eax + 16]     ; TopOfApStack\r
+    push       dword [eax]         ; push return address for stack trace\r
+    push       ebp\r
+    mov        ebp, esp\r
+    mov        ebx, [eax + 8]      ; ApTargetCState\r
+    mov        ecx, [eax + 4]      ; MwaitSupport\r
+    mov        eax, [eax + 20]     ; CountTofinish\r
+    lock dec   dword [eax]         ; (*CountTofinish)--\r
+    cmp        cl,  1              ; Check mwait-monitor support\r
     jnz        HltLoop\r
 MwaitLoop:\r
     mov        eax, esp\r
     xor        ecx, ecx\r
     xor        edx, edx\r
     monitor\r
-    mov        eax, [esp + 8]    ; Mwait Cx, Target C-State per eax[7:4]\r
+    mov        eax, ebx            ; Mwait Cx, Target C-State per eax[7:4]\r
     shl        eax, 4\r
     mwait\r
     jmp        MwaitLoop\r
@@ -189,7 +250,6 @@ HltLoop:
     cli\r
     hlt\r
     jmp        HltLoop\r
-    ret\r
 AsmRelocateApLoopEnd:\r
 \r
 ;-------------------------------------------------------------------------------------\r
@@ -204,6 +264,8 @@ ASM_PFX(AsmGetAddressMap):
     mov        dword [ebx], RendezvousFunnelProcStart\r
     mov        dword [ebx +  4h], Flat32Start - RendezvousFunnelProcStart\r
     mov        dword [ebx +  8h], RendezvousFunnelProcEnd - RendezvousFunnelProcStart\r
+    mov        dword [ebx + 0Ch], AsmRelocateApLoopStart\r
+    mov        dword [ebx + 10h], AsmRelocateApLoopEnd - AsmRelocateApLoopStart\r
 \r
     popad\r
     ret\r