X-Git-Url: https://git.proxmox.com/?a=blobdiff_plain;f=UefiCpuPkg%2FLibrary%2FMpInitLib%2FIa32%2FMpFuncs.nasm;h=52363e6e08df686b76c3296e8c0f7f19449c6d52;hb=f17e28c7911f6d2f985e7bb77e1c68cfd79bf056;hp=8f6f0bfd5b935b297fc34a0b73c86c9ffcd39f2e;hpb=f7f85d83609842e7a3243f8cb15e3d4cb8345177;p=mirror_edk2.git

diff --git a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
index 8f6f0bfd5b..52363e6e08 100644
--- a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
+++ b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
@@ -114,7 +114,12 @@ Flat32Start:                                   ; protected mode entry point
     mov         cr0, eax
 
 SkipEnableExecuteDisable:
+    mov        edi, esi
+    add        edi, InitFlagLocation
+    cmp        dword [edi], 1       ; 1 == ApInitConfig
+    jnz        GetApicId
 
+    ; AP init
     mov        edi, esi
     add        edi, LockLocation
     mov        eax, NotVacantFlag
@@ -124,27 +129,68 @@ TestLock:
     cmp        eax, NotVacantFlag
     jz         TestLock
 
-    mov        edi, esi
-    add        edi, NumApsExecutingLocation
-    inc        dword [edi]
-    mov        ebx, [edi]
+    mov        ecx, esi
+    add        ecx, NumApsExecutingLocation
+    inc        dword [ecx]
+    mov        ebx, [ecx]
+
+Releaselock:
+    mov        eax, VacantFlag
+    xchg       [edi], eax
 
-ProgramStack:
     mov        edi, esi
     add        edi, StackSizeLocation
     mov        eax, [edi]
+    mov        ecx, ebx
+    inc        ecx
+    mul        ecx                               ; EAX = StackSize * (CpuNumber + 1)
     mov        edi, esi
     add        edi, StackStartAddressLocation
     add        eax, [edi]
     mov        esp, eax
-    mov        [edi], eax
+    jmp        CProcedureInvoke
 
-Releaselock:
-    mov        eax, VacantFlag
-    mov        edi, esi
-    add        edi, LockLocation
-    xchg       [edi], eax
+GetApicId:
+    mov        eax, 0
+    cpuid
+    cmp        eax, 0bh
+    jb         NoX2Apic             ; CPUID level below CPUID_EXTENDED_TOPOLOGY
 
+    mov        eax, 0bh
+    xor        ecx, ecx
+    cpuid
+    test       ebx, 0ffffh
+    jz         NoX2Apic             ; CPUID.0BH:EBX[15:0] is zero
+
+    ; Processor is x2APIC capable; 32-bit x2APIC ID is already in EDX
+    jmp        GetProcessorNumber
+
+NoX2Apic:
+    ; Processor is not x2APIC capable, so get 8-bit APIC ID
+    mov        eax, 1
+    cpuid
+    shr        ebx, 24
+    mov        edx, ebx
+
+GetProcessorNumber:
+    ;
+    ; Get processor number for this AP
+    ; Note that BSP may become an AP due to SwitchBsp()
+    ;
+    xor         ebx, ebx
+    lea         eax, [esi + CpuInfoLocation]
+    mov         edi, [eax]
+
+GetNextProcNumber:
+    cmp         [edi], edx                       ; APIC ID match?
+    jz          ProgramStack
+    add         edi, 20
+    inc         ebx
+    jmp         GetNextProcNumber    
+
+ProgramStack:
+    mov         esp, [edi + 12]
+   
 CProcedureInvoke:
     push       ebp               ; push BIST data at top of AP stack
     xor        ebp, ebp          ; clear ebp for call stack trace
@@ -169,19 +215,28 @@ CProcedureInvoke:
 RendezvousFunnelProcEnd:
 
 ;-------------------------------------------------------------------------------------
-;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, PmCodeSegment);
+;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, PmCodeSegment, TopOfApStack, CountTofinish);
 ;-------------------------------------------------------------------------------------
 global ASM_PFX(AsmRelocateApLoop)
 ASM_PFX(AsmRelocateApLoop):
 AsmRelocateApLoopStart:
-    cmp        byte [esp + 4], 1
+    mov        eax, esp
+    mov        esp, [eax + 16]     ; TopOfApStack
+    push       dword [eax]         ; push return address for stack trace
+    push       ebp
+    mov        ebp, esp
+    mov        ebx, [eax + 8]      ; ApTargetCState
+    mov        ecx, [eax + 4]      ; MwaitSupport
+    mov        eax, [eax + 20]     ; CountTofinish
+    lock dec   dword [eax]         ; (*CountTofinish)--
+    cmp        cl,  1              ; Check mwait-monitor support
     jnz        HltLoop
 MwaitLoop:
     mov        eax, esp
     xor        ecx, ecx
     xor        edx, edx
     monitor
-    mov        eax, [esp + 8]    ; Mwait Cx, Target C-State per eax[7:4]
+    mov        eax, ebx            ; Mwait Cx, Target C-State per eax[7:4]
     shl        eax, 4
     mwait
     jmp        MwaitLoop
@@ -189,7 +244,6 @@ HltLoop:
     cli
     hlt
     jmp        HltLoop
-    ret
 AsmRelocateApLoopEnd:
 
 ;-------------------------------------------------------------------------------------