diff --git a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
index 8f6f0bfd5b..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
 
-Releaselock:
-    mov        eax, VacantFlag
-    mov        edi, esi
-    add        edi, LockLocation
-    xchg       [edi], eax
+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]
+
+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
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 2fde765744..fd32482d55 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -432,7 +432,8 @@ VOID
 InitializeApData (
   IN OUT CPU_MP_DATA      *CpuMpData,
   IN     UINTN            ProcessorNumber,
-  IN     UINT32           BistData
+  IN     UINT32           BistData,
+  IN     UINTN            ApTopOfStack
   )
 {
   CPU_INFO_IN_HOB          *CpuInfoInHob;
@@ -441,6 +442,7 @@ InitializeApData (
   CpuInfoInHob[ProcessorNumber].InitialApicId = GetInitialApicId ();
   CpuInfoInHob[ProcessorNumber].ApicId        = GetApicId ();
   CpuInfoInHob[ProcessorNumber].Health        = BistData;
+  CpuInfoInHob[ProcessorNumber].ApTopOfStack  = (UINT32) ApTopOfStack;
 
   CpuMpData->CpuData[ProcessorNumber].Waiting    = FALSE;
   CpuMpData->CpuData[ProcessorNumber].CpuHealthy = (BistData == 0) ? TRUE : FALSE;
@@ -478,6 +480,7 @@ ApWakeupFunction (
   UINT32                     BistData;
   volatile UINT32            *ApStartupSignalBuffer;
   CPU_INFO_IN_HOB            *CpuInfoInHob;
+  UINTN                      ApTopOfStack;
 
   //
   // AP finished assembly code and begin to execute C code
@@ -496,7 +499,8 @@ ApWakeupFunction (
       //
       // This is first time AP wakeup, get BIST information from AP stack
       //
-      BistData = *(UINT32 *) (CpuMpData->Buffer + ProcessorNumber * CpuMpData->CpuApStackSize - sizeof (UINTN));
+      ApTopOfStack  = CpuMpData->Buffer + (ProcessorNumber + 1) * CpuMpData->CpuApStackSize;
+      BistData = *(UINT32 *) (ApTopOfStack - sizeof (UINTN));
       //
       // Do some AP initialize sync
       //
@@ -505,7 +509,7 @@ ApWakeupFunction (
       // Sync BSP's Control registers to APs
       //
       RestoreVolatileRegisters (&CpuMpData->CpuData[0].VolatileRegisters, FALSE);
-      InitializeApData (CpuMpData, ProcessorNumber, BistData);
+      InitializeApData (CpuMpData, ProcessorNumber, BistData, ApTopOfStack);
       ApStartupSignalBuffer = CpuMpData->CpuData[ProcessorNumber].StartupApSignal;
     } else {
       //
@@ -1194,7 +1198,7 @@ MpInitLibInitialize (
   //
   // Set BSP basic information
   //
-  InitializeApData (CpuMpData, 0, 0);
+  InitializeApData (CpuMpData, 0, 0, CpuMpData->Buffer);
   //
   // Save assembly code information
   //
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index a58c855a6c..f81c819c34 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -133,6 +133,7 @@ typedef struct {
   UINT32                         InitialApicId;
   UINT32                         ApicId;
   UINT32                         Health;
+  UINT32                         ApTopOfStack;
 } CPU_INFO_IN_HOB;
 
 //
diff --git a/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
index 090e9fae07..bfc3ff1f5c 100644
--- a/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
+++ b/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
@@ -119,6 +119,12 @@ LongModeStart:
     mov        es,  ax
     mov        ss,  ax
 
+    mov        esi, ebx
+    lea        edi, [esi + InitFlagLocation]
+    cmp        qword [edi], 1       ; ApInitConfig
+    jnz        GetApicId
+
+    ; AP init
     mov        esi, ebx
     mov        edi, esi
     add        edi, LockLocation
@@ -129,26 +135,64 @@ TestLock:
     cmp        rax, NotVacantFlag
     jz         TestLock
 
-    mov        edi, esi
-    add        edi, NumApsExecutingLocation
-    inc        dword [edi]
-    mov        ebx, [edi]
+    lea        ecx, [esi + InitFlagLocation]
+    inc        dword [ecx]
+    mov        ebx, [ecx]
 
-ProgramStack:
+Releaselock:
+    mov        rax, VacantFlag
+    xchg       qword [edi], rax
+    ; program stack
     mov        edi, esi
     add        edi, StackSizeLocation
-    mov        rax, qword [edi]
+    mov        eax, dword [edi]
+    mov        ecx, ebx
+    inc        ecx
+    mul        ecx                               ; EAX = StackSize * (CpuNumber + 1)
     mov        edi, esi
     add        edi, StackStartAddressLocation
     add        rax, qword [edi]
     mov        rsp, rax
-    mov        qword [edi], rax
+    jmp        CProcedureInvoke
 
-Releaselock:
-    mov        rax, VacantFlag
-    mov        edi, esi
-    add        edi, LockLocation
-    xchg       qword [edi], rax
+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]
+
+GetNextProcNumber:
+    cmp         dword [edi], edx                      ; APIC ID match?
+    jz          ProgramStack
+    add         edi, 16
+    inc         ebx
+    jmp         GetNextProcNumber    
+
+ProgramStack:
+    xor         rsp, rsp
+    mov         esp, dword [edi + 12]
 
 CProcedureInvoke:
     push       rbp               ; Push BIST data at top of AP stack