X-Git-Url: https://git.proxmox.com/?p=mirror_edk2.git;a=blobdiff_plain;f=UefiCpuPkg%2FLibrary%2FMpInitLib%2FIa32%2FMpFuncs.nasm;h=4bfa084c85a91457a1e60bc5cb5be217ca6fb3a7;hp=49f55035309643d38a548896332126ec3a03c59c;hb=845c5be1fd9bf7edfac4a103dfab70829686978f;hpb=7615702169b838f9341c312739d29f0795ebd998

diff --git a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
index 49f5503530..4bfa084c85 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,65 @@ 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
+
+GetApicId:
+    mov        eax, 0
+    cpuid
+    cmp        eax, 0bh
+    jnb        X2Apic
+    ; Processor is not x2APIC capable, so get 8-bit APIC ID
+    mov        eax, 1
+    cpuid
+    shr        ebx, 24
+    mov        edx, ebx
+    jmp        GetProcessorNumber
+
+X2Apic:
+    ; Processor is x2APIC capable, so get 32-bit x2APIC ID
+    mov        eax, 0bh
+    xor        ecx, ecx
+    cpuid                   
+    ; edx save x2APIC ID
+    
+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]
 
-Releaselock:
-    mov        eax, VacantFlag
-    mov        edi, esi
-    add        edi, LockLocation
-    xchg       [edi], eax
+GetNextProcNumber:
+    cmp         [edi], edx                       ; APIC ID match?
+    jz          ProgramStack
+    add         edi, 16
+    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
@@ -204,6 +247,8 @@ ASM_PFX(AsmGetAddressMap):
     mov        dword [ebx], RendezvousFunnelProcStart
     mov        dword [ebx +  4h], Flat32Start - RendezvousFunnelProcStart
     mov        dword [ebx +  8h], RendezvousFunnelProcEnd - RendezvousFunnelProcStart
+    mov        dword [ebx + 0Ch], AsmRelocateApLoopStart
+    mov        dword [ebx + 10h], AsmRelocateApLoopEnd - AsmRelocateApLoopStart
 
     popad
     ret