]> git.proxmox.com Git - mirror_edk2.git/blobdiff - UefiCpuPkg/PiSmmCpuDxeSmm/MpService.c
UefiCpuPkg/PiSmmCpuDxeSmm: improve the coding style
[mirror_edk2.git] / UefiCpuPkg / PiSmmCpuDxeSmm / MpService.c
index c7aa48b04e77ddc6b0fbd38b2b96c4337d4f43a2..35a6996ba345792f1de96481367aa33b55d523d6 100644 (file)
@@ -1,14 +1,10 @@
 /** @file\r
 SMM MP service implementation\r
 \r
-Copyright (c) 2009 - 2016, Intel Corporation. All rights reserved.<BR>\r
-This program and the accompanying materials\r
-are licensed and made available under the terms and conditions of the BSD License\r
-which accompanies this distribution.  The full text of the license may be found at\r
-http://opensource.org/licenses/bsd-license.php\r
+Copyright (c) 2009 - 2020, Intel Corporation. All rights reserved.<BR>\r
+Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>\r
 \r
-THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
-WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
+SPDX-License-Identifier: BSD-2-Clause-Patent\r
 \r
 **/\r
 \r
@@ -25,6 +21,7 @@ SMM_CPU_SEMAPHORES                          mSmmCpuSemaphores;
 UINTN                                       mSemaphoreSize;\r
 SPIN_LOCK                                   *mPFLock = NULL;\r
 SMM_CPU_SYNC_MODE                           mCpuSmmSyncMode;\r
+BOOLEAN                                     mMachineCheckSupported = FALSE;\r
 \r
 /**\r
   Performs an atomic compare exchange operation to get semaphore.\r
@@ -139,11 +136,9 @@ ReleaseAllAPs (
   )\r
 {\r
   UINTN                             Index;\r
-  UINTN                             BspIndex;\r
 \r
-  BspIndex = mSmmMpSyncData->BspIndex;\r
-  for (Index = mMaxNumberOfCpus; Index-- > 0;) {\r
-    if (Index != BspIndex && *(mSmmMpSyncData->CpuData[Index].Present)) {\r
+  for (Index = 0; Index < mMaxNumberOfCpus; Index++) {\r
+    if (IsPresentAp (Index)) {\r
       ReleaseSemaphore (mSmmMpSyncData->CpuData[Index].Run);\r
     }\r
   }\r
@@ -175,7 +170,7 @@ AllCpusInSmmWithExceptions (
 \r
   CpuData = mSmmMpSyncData->CpuData;\r
   ProcessorInfo = gSmmCpuPrivate->ProcessorInfo;\r
-  for (Index = mMaxNumberOfCpus; Index-- > 0;) {\r
+  for (Index = 0; Index < mMaxNumberOfCpus; Index++) {\r
     if (!(*(CpuData[Index].Present)) && ProcessorInfo[Index].ProcessorId != INVALID_APIC_ID) {\r
       if (((Exceptions & ARRIVAL_EXCEPTION_DELAYED) != 0) && SmmCpuFeaturesGetSmmRegister (Index, SmmRegSmmDelayed) != 0) {\r
         continue;\r
@@ -194,6 +189,56 @@ AllCpusInSmmWithExceptions (
   return TRUE;\r
 }\r
 \r
+/**\r
+  Has OS enabled Lmce in the MSR_IA32_MCG_EXT_CTL\r
+\r
+  @retval TRUE     Os enable lmce.\r
+  @retval FALSE    Os not enable lmce.\r
+\r
+**/\r
+BOOLEAN\r
+IsLmceOsEnabled (\r
+  VOID\r
+  )\r
+{\r
+  MSR_IA32_MCG_CAP_REGISTER          McgCap;\r
+  MSR_IA32_FEATURE_CONTROL_REGISTER  FeatureCtrl;\r
+  MSR_IA32_MCG_EXT_CTL_REGISTER      McgExtCtrl;\r
+\r
+  McgCap.Uint64 = AsmReadMsr64 (MSR_IA32_MCG_CAP);\r
+  if (McgCap.Bits.MCG_LMCE_P == 0) {\r
+    return FALSE;\r
+  }\r
+\r
+  FeatureCtrl.Uint64 = AsmReadMsr64 (MSR_IA32_FEATURE_CONTROL);\r
+  if (FeatureCtrl.Bits.LmceOn == 0) {\r
+    return FALSE;\r
+  }\r
+\r
+  McgExtCtrl.Uint64 = AsmReadMsr64 (MSR_IA32_MCG_EXT_CTL);\r
+  return (BOOLEAN) (McgExtCtrl.Bits.LMCE_EN == 1);\r
+}\r
+\r
+/**\r
+  Return if Local machine check exception signaled.\r
+\r
+  Indicates (when set) that a local machine check exception was generated. This indicates that the current machine-check event was\r
+  delivered to only the logical processor.\r
+\r
+  @retval TRUE    LMCE was signaled.\r
+  @retval FALSE   LMCE was not signaled.\r
+\r
+**/\r
+BOOLEAN\r
+IsLmceSignaled (\r
+  VOID\r
+  )\r
+{\r
+  MSR_IA32_MCG_STATUS_REGISTER McgStatus;\r
+\r
+  McgStatus.Uint64 = AsmReadMsr64 (MSR_IA32_MCG_STATUS);\r
+  return (BOOLEAN) (McgStatus.Bits.LMCE_S == 1);\r
+}\r
 \r
 /**\r
   Given timeout constraint, wait for all APs to arrive, and insure when this function returns, no AP will execute normal mode code before\r
@@ -207,9 +252,18 @@ SmmWaitForApArrival (
 {\r
   UINT64                            Timer;\r
   UINTN                             Index;\r
+  BOOLEAN                           LmceEn;\r
+  BOOLEAN                           LmceSignal;\r
 \r
   ASSERT (*mSmmMpSyncData->Counter <= mNumberOfCpus);\r
 \r
+  LmceEn     = FALSE;\r
+  LmceSignal = FALSE;\r
+  if (mMachineCheckSupported) {\r
+    LmceEn     = IsLmceOsEnabled ();\r
+    LmceSignal = IsLmceSignaled();\r
+  }\r
+\r
   //\r
   // Platform implementor should choose a timeout value appropriately:\r
   // - The timeout value should balance the SMM time constrains and the likelihood that delayed CPUs are excluded in the SMM run. Note\r
@@ -225,7 +279,7 @@ SmmWaitForApArrival (
   // Sync with APs 1st timeout\r
   //\r
   for (Timer = StartSyncTimer ();\r
-       !IsSyncTimerTimeout (Timer) &&\r
+       !IsSyncTimerTimeout (Timer) && !(LmceEn && LmceSignal) &&\r
        !AllCpusInSmmWithExceptions (ARRIVAL_EXCEPTION_BLOCKED | ARRIVAL_EXCEPTION_SMI_DISABLED );\r
        ) {\r
     CpuPause ();\r
@@ -251,7 +305,7 @@ SmmWaitForApArrival (
     //\r
     // Send SMI IPIs to bring outside processors in\r
     //\r
-    for (Index = mMaxNumberOfCpus; Index-- > 0;) {\r
+    for (Index = 0; Index < mMaxNumberOfCpus; Index++) {\r
       if (!(*(mSmmMpSyncData->CpuData[Index].Present)) && gSmmCpuPrivate->ProcessorInfo[Index].ProcessorId != INVALID_APIC_ID) {\r
         SendSmiIpi ((UINT32)gSmmCpuPrivate->ProcessorInfo[Index].ProcessorId);\r
       }\r
@@ -291,6 +345,116 @@ ReplaceOSMtrrs (
   MtrrSetAllMtrrs (&gSmiMtrrs);\r
 }\r
 \r
+/**\r
+  Wheck whether task has been finished by all APs.\r
+\r
+  @param       BlockMode   Whether did it in block mode or non-block mode.\r
+\r
+  @retval      TRUE        Task has been finished by all APs.\r
+  @retval      FALSE       Task not has been finished by all APs.\r
+\r
+**/\r
+BOOLEAN\r
+WaitForAllAPsNotBusy (\r
+  IN BOOLEAN                        BlockMode\r
+  )\r
+{\r
+  UINTN                             Index;\r
+\r
+  for (Index = 0; Index < mMaxNumberOfCpus; Index++) {\r
+    //\r
+    // Ignore BSP and APs which not call in SMM.\r
+    //\r
+    if (!IsPresentAp(Index)) {\r
+      continue;\r
+    }\r
+\r
+    if (BlockMode) {\r
+      AcquireSpinLock(mSmmMpSyncData->CpuData[Index].Busy);\r
+      ReleaseSpinLock(mSmmMpSyncData->CpuData[Index].Busy);\r
+    } else {\r
+      if (AcquireSpinLockOrFail (mSmmMpSyncData->CpuData[Index].Busy)) {\r
+        ReleaseSpinLock(mSmmMpSyncData->CpuData[Index].Busy);\r
+      } else {\r
+        return FALSE;\r
+      }\r
+    }\r
+  }\r
+\r
+  return TRUE;\r
+}\r
+\r
+/**\r
+  Check whether it is an present AP.\r
+\r
+  @param   CpuIndex      The AP index which calls this function.\r
+\r
+  @retval  TRUE           It's a present AP.\r
+  @retval  TRUE           This is not an AP or it is not present.\r
+\r
+**/\r
+BOOLEAN\r
+IsPresentAp (\r
+  IN UINTN        CpuIndex\r
+  )\r
+{\r
+  return ((CpuIndex != gSmmCpuPrivate->SmmCoreEntryContext.CurrentlyExecutingCpu) &&\r
+    *(mSmmMpSyncData->CpuData[CpuIndex].Present));\r
+}\r
+\r
+/**\r
+  Clean up the status flags used during executing the procedure.\r
+\r
+  @param   CpuIndex      The AP index which calls this function.\r
+\r
+**/\r
+VOID\r
+ReleaseToken (\r
+  IN UINTN                  CpuIndex\r
+  )\r
+{\r
+  PROCEDURE_TOKEN                         *Token;\r
+\r
+  Token = mSmmMpSyncData->CpuData[CpuIndex].Token;\r
+\r
+  if (InterlockedDecrement (&Token->RunningApCount) == 0) {\r
+    ReleaseSpinLock (Token->SpinLock);\r
+  }\r
+\r
+  mSmmMpSyncData->CpuData[CpuIndex].Token = NULL;\r
+}\r
+\r
+/**\r
+  Free the tokens in the maintained list.\r
+\r
+**/\r
+VOID\r
+ResetTokens (\r
+  VOID\r
+  )\r
+{\r
+  LIST_ENTRY            *Link;\r
+  PROCEDURE_TOKEN       *ProcToken;\r
+\r
+  Link = GetFirstNode (&gSmmCpuPrivate->TokenList);\r
+  while (!IsNull (&gSmmCpuPrivate->TokenList, Link)) {\r
+    ProcToken = PROCEDURE_TOKEN_FROM_LINK (Link);\r
+\r
+    ProcToken->RunningApCount = 0;\r
+    ProcToken->Used = FALSE;\r
+\r
+    //\r
+    // Check the spinlock status and release it if not released yet.\r
+    //\r
+    if (!AcquireSpinLockOrFail(ProcToken->SpinLock)) {\r
+      DEBUG((DEBUG_ERROR, "Risk::SpinLock still not released!"));\r
+    }\r
+    ReleaseSpinLock (ProcToken->SpinLock);\r
+\r
+    Link = GetNextNode (&gSmmCpuPrivate->TokenList, Link);\r
+  }\r
+}\r
+\r
 /**\r
   SMI handler for BSP.\r
 \r
@@ -405,7 +569,7 @@ BSPHandler (
   //\r
   // The BUSY lock is initialized to Acquired state\r
   //\r
-  AcquireSpinLockOrFail (mSmmMpSyncData->CpuData[CpuIndex].Busy);\r
+  AcquireSpinLock (mSmmMpSyncData->CpuData[CpuIndex].Busy);\r
 \r
   //\r
   // Perform the pre tasks\r
@@ -420,12 +584,7 @@ BSPHandler (
   //\r
   // Make sure all APs have completed their pending none-block tasks\r
   //\r
-  for (Index = mMaxNumberOfCpus; Index-- > 0;) {\r
-    if (Index != CpuIndex && *(mSmmMpSyncData->CpuData[Index].Present)) {\r
-      AcquireSpinLock (mSmmMpSyncData->CpuData[Index].Busy);\r
-      ReleaseSpinLock (mSmmMpSyncData->CpuData[Index].Busy);\r
-    }\r
-  }\r
+  WaitForAllAPsNotBusy (TRUE);\r
 \r
   //\r
   // Perform the remaining tasks\r
@@ -449,7 +608,7 @@ BSPHandler (
     //\r
     while (TRUE) {\r
       PresentCount = 0;\r
-      for (Index = mMaxNumberOfCpus; Index-- > 0;) {\r
+      for (Index = 0; Index < mMaxNumberOfCpus; Index++) {\r
         if (*(mSmmMpSyncData->CpuData[Index].Present)) {\r
           PresentCount ++;\r
         }\r
@@ -516,6 +675,11 @@ BSPHandler (
   //\r
   WaitForAllAPs (ApCount);\r
 \r
+  //\r
+  // Reset the tokens buffer.\r
+  //\r
+  ResetTokens ();\r
+\r
   //\r
   // Reset BspIndex to -1, meaning BSP has not been elected.\r
   //\r
@@ -548,6 +712,7 @@ APHandler (
   UINT64                            Timer;\r
   UINTN                             BspIndex;\r
   MTRR_SETTINGS                     Mtrrs;\r
+  EFI_STATUS                        ProcedureStatus;\r
 \r
   //\r
   // Timeout BSP\r
@@ -674,9 +839,16 @@ APHandler (
     //\r
     // Invoke the scheduled procedure\r
     //\r
-    (*mSmmMpSyncData->CpuData[CpuIndex].Procedure) (\r
-      (VOID*)mSmmMpSyncData->CpuData[CpuIndex].Parameter\r
-      );\r
+    ProcedureStatus = (*mSmmMpSyncData->CpuData[CpuIndex].Procedure) (\r
+                          (VOID*)mSmmMpSyncData->CpuData[CpuIndex].Parameter\r
+                          );\r
+    if (mSmmMpSyncData->CpuData[CpuIndex].Status != NULL) {\r
+      *mSmmMpSyncData->CpuData[CpuIndex].Status = ProcedureStatus;\r
+    }\r
+\r
+    if (mSmmMpSyncData->CpuData[CpuIndex].Token != NULL) {\r
+      ReleaseToken (CpuIndex);\r
+    }\r
 \r
     //\r
     // Release BUSY\r
@@ -781,7 +953,8 @@ Gen4GPageTable (
   // Set Page Directory Pointers\r
   //\r
   for (Index = 0; Index < 4; Index++) {\r
-    Pte[Index] = (UINTN)PageTable + EFI_PAGE_SIZE * (Index + 1) + (Is32BitPageTable ? IA32_PAE_PDPTE_ATTRIBUTE_BITS : PAGE_ATTRIBUTE_BITS);\r
+    Pte[Index] = ((UINTN)PageTable + EFI_PAGE_SIZE * (Index + 1)) | mAddressEncMask |\r
+                   (Is32BitPageTable ? IA32_PAE_PDPTE_ATTRIBUTE_BITS : PAGE_ATTRIBUTE_BITS);\r
   }\r
   Pte += EFI_PAGE_SIZE / sizeof (*Pte);\r
 \r
@@ -789,16 +962,16 @@ Gen4GPageTable (
   // Fill in Page Directory Entries\r
   //\r
   for (Index = 0; Index < EFI_PAGE_SIZE * 4 / sizeof (*Pte); Index++) {\r
-    Pte[Index] = (Index << 21) | IA32_PG_PS | PAGE_ATTRIBUTE_BITS;\r
+    Pte[Index] = (Index << 21) | mAddressEncMask | IA32_PG_PS | PAGE_ATTRIBUTE_BITS;\r
   }\r
 \r
+  Pdpte = (UINT64*)PageTable;\r
   if (FeaturePcdGet (PcdCpuSmmStackGuard)) {\r
     Pages = (UINTN)PageTable + EFI_PAGES_TO_SIZE (5);\r
     GuardPage = mSmmStackArrayBase + EFI_PAGE_SIZE;\r
-    Pdpte = (UINT64*)PageTable;\r
     for (PageIndex = Low2MBoundary; PageIndex <= High2MBoundary; PageIndex += SIZE_2MB) {\r
-      Pte = (UINT64*)(UINTN)(Pdpte[BitFieldRead32 ((UINT32)PageIndex, 30, 31)] & ~(EFI_PAGE_SIZE - 1));\r
-      Pte[BitFieldRead32 ((UINT32)PageIndex, 21, 29)] = (UINT64)Pages | PAGE_ATTRIBUTE_BITS;\r
+      Pte = (UINT64*)(UINTN)(Pdpte[BitFieldRead32 ((UINT32)PageIndex, 30, 31)] & ~mAddressEncMask & ~(EFI_PAGE_SIZE - 1));\r
+      Pte[BitFieldRead32 ((UINT32)PageIndex, 21, 29)] = (UINT64)Pages | mAddressEncMask | PAGE_ATTRIBUTE_BITS;\r
       //\r
       // Fill in Page Table Entries\r
       //\r
@@ -809,13 +982,13 @@ Gen4GPageTable (
           //\r
           // Mark the guard page as non-present\r
           //\r
-          Pte[Index] = PageAddress;\r
+          Pte[Index] = PageAddress | mAddressEncMask;\r
           GuardPage += mSmmStackSize;\r
           if (GuardPage > mSmmStackArrayEnd) {\r
             GuardPage = 0;\r
           }\r
         } else {\r
-          Pte[Index] = PageAddress | PAGE_ATTRIBUTE_BITS;\r
+          Pte[Index] = PageAddress | mAddressEncMask | PAGE_ATTRIBUTE_BITS;\r
         }\r
         PageAddress+= EFI_PAGE_SIZE;\r
       }\r
@@ -823,75 +996,199 @@ Gen4GPageTable (
     }\r
   }\r
 \r
+  if ((PcdGet8 (PcdNullPointerDetectionPropertyMask) & BIT1) != 0) {\r
+    Pte = (UINT64*)(UINTN)(Pdpte[0] & ~mAddressEncMask & ~(EFI_PAGE_SIZE - 1));\r
+    if ((Pte[0] & IA32_PG_PS) == 0) {\r
+      // 4K-page entries are already mapped. Just hide the first one anyway.\r
+      Pte = (UINT64*)(UINTN)(Pte[0] & ~mAddressEncMask & ~(EFI_PAGE_SIZE - 1));\r
+      Pte[0] &= ~(UINT64)IA32_PG_P; // Hide page 0\r
+    } else {\r
+      // Create 4K-page entries\r
+      Pages = (UINTN)AllocatePageTableMemory (1);\r
+      ASSERT (Pages != 0);\r
+\r
+      Pte[0] = (UINT64)(Pages | mAddressEncMask | PAGE_ATTRIBUTE_BITS);\r
+\r
+      Pte = (UINT64*)Pages;\r
+      PageAddress = 0;\r
+      Pte[0] = PageAddress | mAddressEncMask; // Hide page 0 but present left\r
+      for (Index = 1; Index < EFI_PAGE_SIZE / sizeof (*Pte); Index++) {\r
+        PageAddress += EFI_PAGE_SIZE;\r
+        Pte[Index] = PageAddress | mAddressEncMask | PAGE_ATTRIBUTE_BITS;\r
+      }\r
+    }\r
+  }\r
+\r
   return (UINT32)(UINTN)PageTable;\r
 }\r
 \r
 /**\r
-  Set memory cache ability.\r
+  Checks whether the input token is the current used token.\r
 \r
-  @param    PageTable              PageTable Address\r
-  @param    Address                Memory Address to change cache ability\r
-  @param    Cacheability           Cache ability to set\r
+  @param[in]  Token      This parameter describes the token that was passed into DispatchProcedure or\r
+                         BroadcastProcedure.\r
 \r
+  @retval TRUE           The input token is the current used token.\r
+  @retval FALSE          The input token is not the current used token.\r
 **/\r
-VOID\r
-SetCacheability (\r
-  IN      UINT64                    *PageTable,\r
-  IN      UINTN                     Address,\r
-  IN      UINT8                     Cacheability\r
+BOOLEAN\r
+IsTokenInUse (\r
+  IN SPIN_LOCK           *Token\r
   )\r
 {\r
-  UINTN   PTIndex;\r
-  VOID    *NewPageTableAddress;\r
-  UINT64  *NewPageTable;\r
-  UINTN   Index;\r
+  LIST_ENTRY        *Link;\r
+  PROCEDURE_TOKEN   *ProcToken;\r
+\r
+  if (Token == NULL) {\r
+    return FALSE;\r
+  }\r
+\r
+  Link = GetFirstNode (&gSmmCpuPrivate->TokenList);\r
+  while (!IsNull (&gSmmCpuPrivate->TokenList, Link)) {\r
+    ProcToken = PROCEDURE_TOKEN_FROM_LINK (Link);\r
 \r
-  ASSERT ((Address & EFI_PAGE_MASK) == 0);\r
+    if (ProcToken->Used && ProcToken->SpinLock == Token) {\r
+      return TRUE;\r
+    }\r
 \r
-  if (sizeof (UINTN) == sizeof (UINT64)) {\r
-    PTIndex = (UINTN)RShiftU64 (Address, 39) & 0x1ff;\r
-    ASSERT (PageTable[PTIndex] & IA32_PG_P);\r
-    PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & gPhyMask);\r
+    Link = GetNextNode (&gSmmCpuPrivate->TokenList, Link);\r
   }\r
 \r
-  PTIndex = (UINTN)RShiftU64 (Address, 30) & 0x1ff;\r
-  ASSERT (PageTable[PTIndex] & IA32_PG_P);\r
-  PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & gPhyMask);\r
+  return FALSE;\r
+}\r
+\r
+/**\r
+  Allocate buffer for the SPIN_LOCK and PROCEDURE_TOKEN.\r
+\r
+**/\r
+VOID\r
+AllocateTokenBuffer (\r
+  VOID\r
+  )\r
+{\r
+  UINTN               SpinLockSize;\r
+  UINT32              TokenCountPerChunk;\r
+  UINTN               ProcTokenSize;\r
+  UINTN               Index;\r
+  PROCEDURE_TOKEN     *ProcToken;\r
+  SPIN_LOCK           *SpinLock;\r
+  UINT8               *SpinLockBuffer;\r
+  UINT8               *ProcTokenBuffer;\r
+\r
+  SpinLockSize = GetSpinLockProperties ();\r
+  ProcTokenSize = sizeof (PROCEDURE_TOKEN);\r
+\r
+  TokenCountPerChunk = FixedPcdGet32 (PcdCpuSmmMpTokenCountPerChunk);\r
+  ASSERT (TokenCountPerChunk != 0);\r
+  if (TokenCountPerChunk == 0) {\r
+    DEBUG ((DEBUG_ERROR, "PcdCpuSmmMpTokenCountPerChunk should not be Zero!\n"));\r
+    CpuDeadLoop ();\r
+  }\r
+  DEBUG ((DEBUG_INFO, "CpuSmm: SpinLock Size = 0x%x, PcdCpuSmmMpTokenCountPerChunk = 0x%x\n", SpinLockSize, TokenCountPerChunk));\r
 \r
   //\r
-  // A perfect implementation should check the original cacheability with the\r
-  // one being set, and break a 2M page entry into pieces only when they\r
-  // disagreed.\r
+  // Separate the Spin_lock and Proc_token because the alignment requires by Spin_Lock.\r
   //\r
-  PTIndex = (UINTN)RShiftU64 (Address, 21) & 0x1ff;\r
-  if ((PageTable[PTIndex] & IA32_PG_PS) != 0) {\r
-    //\r
-    // Allocate a page from SMRAM\r
-    //\r
-    NewPageTableAddress = AllocatePageTableMemory (1);\r
-    ASSERT (NewPageTableAddress != NULL);\r
+  SpinLockBuffer = AllocatePool (SpinLockSize * TokenCountPerChunk);\r
+  ASSERT (SpinLockBuffer != NULL);\r
 \r
-    NewPageTable = (UINT64 *)NewPageTableAddress;\r
+  ProcTokenBuffer = AllocatePool (ProcTokenSize * TokenCountPerChunk);\r
+  ASSERT (ProcTokenBuffer != NULL);\r
 \r
-    for (Index = 0; Index < 0x200; Index++) {\r
-      NewPageTable[Index] = PageTable[PTIndex];\r
-      if ((NewPageTable[Index] & IA32_PG_PAT_2M) != 0) {\r
-        NewPageTable[Index] &= ~((UINT64)IA32_PG_PAT_2M);\r
-        NewPageTable[Index] |= (UINT64)IA32_PG_PAT_4K;\r
-      }\r
-      NewPageTable[Index] |= (UINT64)(Index << EFI_PAGE_SHIFT);\r
+  for (Index = 0; Index < TokenCountPerChunk; Index++) {\r
+    SpinLock = (SPIN_LOCK *)(SpinLockBuffer + SpinLockSize * Index);\r
+    InitializeSpinLock (SpinLock);\r
+\r
+    ProcToken = (PROCEDURE_TOKEN *)(ProcTokenBuffer + ProcTokenSize * Index);\r
+    ProcToken->Signature = PROCEDURE_TOKEN_SIGNATURE;\r
+    ProcToken->SpinLock = SpinLock;\r
+    ProcToken->Used = FALSE;\r
+    ProcToken->RunningApCount = 0;\r
+\r
+    InsertTailList (&gSmmCpuPrivate->TokenList, &ProcToken->Link);\r
+  }\r
+}\r
+\r
+/**\r
+  Find first free token in the allocated token list.\r
+\r
+  @retval    return the first free PROCEDURE_TOKEN.\r
+\r
+**/\r
+PROCEDURE_TOKEN *\r
+FindFirstFreeToken (\r
+  VOID\r
+  )\r
+{\r
+  LIST_ENTRY        *Link;\r
+  PROCEDURE_TOKEN   *ProcToken;\r
+\r
+  Link = GetFirstNode (&gSmmCpuPrivate->TokenList);\r
+  while (!IsNull (&gSmmCpuPrivate->TokenList, Link)) {\r
+    ProcToken = PROCEDURE_TOKEN_FROM_LINK (Link);\r
+\r
+    if (!ProcToken->Used) {\r
+      return ProcToken;\r
     }\r
 \r
-    PageTable[PTIndex] = ((UINTN)NewPageTableAddress & gPhyMask) | PAGE_ATTRIBUTE_BITS;\r
+    Link = GetNextNode (&gSmmCpuPrivate->TokenList, Link);\r
+  }\r
+\r
+  return NULL;\r
+}\r
+\r
+/**\r
+  Get the free token.\r
+\r
+  If no free token, allocate new tokens then return the free one.\r
+\r
+  @retval    return the first free PROCEDURE_TOKEN.\r
+\r
+**/\r
+PROCEDURE_TOKEN *\r
+GetFreeToken (\r
+  IN UINT32       RunningApsCount\r
+  )\r
+{\r
+  PROCEDURE_TOKEN  *NewToken;\r
+\r
+  NewToken = FindFirstFreeToken ();\r
+  if (NewToken == NULL) {\r
+    AllocateTokenBuffer ();\r
+    NewToken = FindFirstFreeToken ();\r
   }\r
+  ASSERT (NewToken != NULL);\r
+\r
+  NewToken->Used = TRUE;\r
+  NewToken->RunningApCount = RunningApsCount;\r
+  AcquireSpinLock (NewToken->SpinLock);\r
+\r
+  return NewToken;\r
+}\r
+\r
+/**\r
+  Checks status of specified AP.\r
+\r
+  This function checks whether the specified AP has finished the task assigned\r
+  by StartupThisAP(), and whether timeout expires.\r
+\r
+  @param[in]  Token             This parameter describes the token that was passed into DispatchProcedure or\r
+                                BroadcastProcedure.\r
 \r
-  ASSERT (PageTable[PTIndex] & IA32_PG_P);\r
-  PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & gPhyMask);\r
+  @retval EFI_SUCCESS           Specified AP has finished task assigned by StartupThisAPs().\r
+  @retval EFI_NOT_READY         Specified AP has not finished task and timeout has not expired.\r
+**/\r
+EFI_STATUS\r
+IsApReady (\r
+  IN SPIN_LOCK          *Token\r
+  )\r
+{\r
+  if (AcquireSpinLockOrFail (Token)) {\r
+    ReleaseSpinLock (Token);\r
+    return EFI_SUCCESS;\r
+  }\r
 \r
-  PTIndex = (UINTN)RShiftU64 (Address, 12) & 0x1ff;\r
-  ASSERT (PageTable[PTIndex] & IA32_PG_P);\r
-  PageTable[PTIndex] &= ~((UINT64)((IA32_PG_PAT_4K | IA32_PG_CD | IA32_PG_WT)));\r
-  PageTable[PTIndex] |= (UINT64)Cacheability;\r
+  return EFI_NOT_READY;\r
 }\r
 \r
 /**\r
@@ -899,8 +1196,28 @@ SetCacheability (
 \r
   @param[in]       Procedure                The address of the procedure to run\r
   @param[in]       CpuIndex                 Target CPU Index\r
-  @param[in, out]  ProcArguments            The parameter to pass to the procedure\r
-  @param[in]       BlockingMode             Startup AP in blocking mode or not\r
+  @param[in,out]   ProcArguments            The parameter to pass to the procedure\r
+  @param[in]       Token                    This is an optional parameter that allows the caller to execute the\r
+                                            procedure in a blocking or non-blocking fashion. If it is NULL the\r
+                                            call is blocking, and the call will not return until the AP has\r
+                                            completed the procedure. If the token is not NULL, the call will\r
+                                            return immediately. The caller can check whether the procedure has\r
+                                            completed with CheckOnProcedure or WaitForProcedure.\r
+  @param[in]       TimeoutInMicroseconds    Indicates the time limit in microseconds for the APs to finish\r
+                                            execution of Procedure, either for blocking or non-blocking mode.\r
+                                            Zero means infinity. If the timeout expires before all APs return\r
+                                            from Procedure, then Procedure on the failed APs is terminated. If\r
+                                            the timeout expires in blocking mode, the call returns EFI_TIMEOUT.\r
+                                            If the timeout expires in non-blocking mode, the timeout determined\r
+                                            can be through CheckOnProcedure or WaitForProcedure.\r
+                                            Note that timeout support is optional. Whether an implementation\r
+                                            supports this feature can be determined via the Attributes data\r
+                                            member.\r
+  @param[in,out]   CpuStatus                This optional pointer may be used to get the status code returned\r
+                                            by Procedure when it completes execution on the target AP, or with\r
+                                            EFI_TIMEOUT if the Procedure fails to complete within the optional\r
+                                            timeout. The implementation will update this variable with\r
+                                            EFI_NOT_READY prior to starting Procedure on the target AP.\r
 \r
   @retval EFI_INVALID_PARAMETER    CpuNumber not valid\r
   @retval EFI_INVALID_PARAMETER    CpuNumber specifying BSP\r
@@ -911,12 +1228,16 @@ SetCacheability (
 **/\r
 EFI_STATUS\r
 InternalSmmStartupThisAp (\r
-  IN      EFI_AP_PROCEDURE          Procedure,\r
-  IN      UINTN                     CpuIndex,\r
-  IN OUT  VOID                      *ProcArguments OPTIONAL,\r
-  IN      BOOLEAN                   BlockingMode\r
+  IN      EFI_AP_PROCEDURE2              Procedure,\r
+  IN      UINTN                          CpuIndex,\r
+  IN OUT  VOID                           *ProcArguments OPTIONAL,\r
+  IN      MM_COMPLETION                  *Token,\r
+  IN      UINTN                          TimeoutInMicroseconds,\r
+  IN OUT  EFI_STATUS                     *CpuStatus\r
   )\r
 {\r
+  PROCEDURE_TOKEN    *ProcToken;\r
+\r
   if (CpuIndex >= gSmmCpuPrivate->SmmCoreEntryContext.NumberOfCpus) {\r
     DEBUG((DEBUG_ERROR, "CpuIndex(%d) >= gSmmCpuPrivate->SmmCoreEntryContext.NumberOfCpus(%d)\n", CpuIndex, gSmmCpuPrivate->SmmCoreEntryContext.NumberOfCpus));\r
     return EFI_INVALID_PARAMETER;\r
@@ -925,6 +1246,9 @@ InternalSmmStartupThisAp (
     DEBUG((DEBUG_ERROR, "CpuIndex(%d) == gSmmCpuPrivate->SmmCoreEntryContext.CurrentlyExecutingCpu\n", CpuIndex));\r
     return EFI_INVALID_PARAMETER;\r
   }\r
+  if (gSmmCpuPrivate->ProcessorInfo[CpuIndex].ProcessorId == INVALID_APIC_ID) {\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
   if (!(*(mSmmMpSyncData->CpuData[CpuIndex].Present))) {\r
     if (mSmmMpSyncData->EffectiveSyncMode == SmmCpuSyncModeTradition) {\r
       DEBUG((DEBUG_ERROR, "!mSmmMpSyncData->CpuData[%d].Present\n", CpuIndex));\r
@@ -937,24 +1261,194 @@ InternalSmmStartupThisAp (
     }\r
     return EFI_INVALID_PARAMETER;\r
   }\r
-\r
-  if (BlockingMode) {\r
-    AcquireSpinLock (mSmmMpSyncData->CpuData[CpuIndex].Busy);\r
-  } else {\r
-    if (!AcquireSpinLockOrFail (mSmmMpSyncData->CpuData[CpuIndex].Busy)) {\r
-      DEBUG((DEBUG_ERROR, "mSmmMpSyncData->CpuData[%d].Busy\n", CpuIndex));\r
-      return EFI_INVALID_PARAMETER;\r
-    }\r
+  if ((TimeoutInMicroseconds != 0) && ((mSmmMp.Attributes & EFI_MM_MP_TIMEOUT_SUPPORTED) == 0)) {\r
+    return EFI_INVALID_PARAMETER;\r
   }\r
+  if (Procedure == NULL) {\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+\r
+  AcquireSpinLock (mSmmMpSyncData->CpuData[CpuIndex].Busy);\r
 \r
   mSmmMpSyncData->CpuData[CpuIndex].Procedure = Procedure;\r
   mSmmMpSyncData->CpuData[CpuIndex].Parameter = ProcArguments;\r
+  if (Token != NULL) {\r
+    ProcToken= GetFreeToken (1);\r
+    mSmmMpSyncData->CpuData[CpuIndex].Token = ProcToken;\r
+    *Token = (MM_COMPLETION)ProcToken->SpinLock;\r
+  }\r
+  mSmmMpSyncData->CpuData[CpuIndex].Status    = CpuStatus;\r
+  if (mSmmMpSyncData->CpuData[CpuIndex].Status != NULL) {\r
+    *mSmmMpSyncData->CpuData[CpuIndex].Status = EFI_NOT_READY;\r
+  }\r
+\r
   ReleaseSemaphore (mSmmMpSyncData->CpuData[CpuIndex].Run);\r
 \r
-  if (BlockingMode) {\r
+  if (Token == NULL) {\r
     AcquireSpinLock (mSmmMpSyncData->CpuData[CpuIndex].Busy);\r
     ReleaseSpinLock (mSmmMpSyncData->CpuData[CpuIndex].Busy);\r
   }\r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+/**\r
+  Worker function to execute a caller provided function on all enabled APs.\r
+\r
+  @param[in]     Procedure               A pointer to the function to be run on\r
+                                         enabled APs of the system.\r
+  @param[in]     TimeoutInMicroseconds   Indicates the time limit in microseconds for\r
+                                         APs to return from Procedure, either for\r
+                                         blocking or non-blocking mode.\r
+  @param[in,out] ProcedureArguments      The parameter passed into Procedure for\r
+                                         all APs.\r
+  @param[in,out] Token                   This is an optional parameter that allows the caller to execute the\r
+                                         procedure in a blocking or non-blocking fashion. If it is NULL the\r
+                                         call is blocking, and the call will not return until the AP has\r
+                                         completed the procedure. If the token is not NULL, the call will\r
+                                         return immediately. The caller can check whether the procedure has\r
+                                         completed with CheckOnProcedure or WaitForProcedure.\r
+  @param[in,out] CPUStatus               This optional pointer may be used to get the status code returned\r
+                                         by Procedure when it completes execution on the target AP, or with\r
+                                         EFI_TIMEOUT if the Procedure fails to complete within the optional\r
+                                         timeout. The implementation will update this variable with\r
+                                         EFI_NOT_READY prior to starting Procedure on the target AP.\r
+\r
+\r
+  @retval EFI_SUCCESS             In blocking mode, all APs have finished before\r
+                                  the timeout expired.\r
+  @retval EFI_SUCCESS             In non-blocking mode, function has been dispatched\r
+                                  to all enabled APs.\r
+  @retval others                  Failed to Startup all APs.\r
+\r
+**/\r
+EFI_STATUS\r
+InternalSmmStartupAllAPs (\r
+  IN       EFI_AP_PROCEDURE2             Procedure,\r
+  IN       UINTN                         TimeoutInMicroseconds,\r
+  IN OUT   VOID                          *ProcedureArguments OPTIONAL,\r
+  IN OUT   MM_COMPLETION                 *Token,\r
+  IN OUT   EFI_STATUS                    *CPUStatus\r
+  )\r
+{\r
+  UINTN               Index;\r
+  UINTN               CpuCount;\r
+  PROCEDURE_TOKEN     *ProcToken;\r
+\r
+  if ((TimeoutInMicroseconds != 0) && ((mSmmMp.Attributes & EFI_MM_MP_TIMEOUT_SUPPORTED) == 0)) {\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+  if (Procedure == NULL) {\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+\r
+  CpuCount = 0;\r
+  for (Index = 0; Index < mMaxNumberOfCpus; Index++) {\r
+    if (IsPresentAp (Index)) {\r
+      CpuCount ++;\r
+\r
+      if (gSmmCpuPrivate->Operation[Index] == SmmCpuRemove) {\r
+        return EFI_INVALID_PARAMETER;\r
+      }\r
+\r
+      if (!AcquireSpinLockOrFail(mSmmMpSyncData->CpuData[Index].Busy)) {\r
+        return EFI_NOT_READY;\r
+      }\r
+      ReleaseSpinLock (mSmmMpSyncData->CpuData[Index].Busy);\r
+    }\r
+  }\r
+  if (CpuCount == 0) {\r
+    return EFI_NOT_STARTED;\r
+  }\r
+\r
+  if (Token != NULL) {\r
+    ProcToken = GetFreeToken ((UINT32)mMaxNumberOfCpus);\r
+    *Token = (MM_COMPLETION)ProcToken->SpinLock;\r
+  } else {\r
+    ProcToken = NULL;\r
+  }\r
+\r
+  //\r
+  // Make sure all BUSY should be acquired.\r
+  //\r
+  // Because former code already check mSmmMpSyncData->CpuData[***].Busy for each AP.\r
+  // Here code always use AcquireSpinLock instead of AcquireSpinLockOrFail for not\r
+  // block mode.\r
+  //\r
+  for (Index = 0; Index < mMaxNumberOfCpus; Index++) {\r
+    if (IsPresentAp (Index)) {\r
+      AcquireSpinLock (mSmmMpSyncData->CpuData[Index].Busy);\r
+    }\r
+  }\r
+\r
+  for (Index = 0; Index < mMaxNumberOfCpus; Index++) {\r
+    if (IsPresentAp (Index)) {\r
+      mSmmMpSyncData->CpuData[Index].Procedure = (EFI_AP_PROCEDURE2) Procedure;\r
+      mSmmMpSyncData->CpuData[Index].Parameter = ProcedureArguments;\r
+      if (ProcToken != NULL) {\r
+        mSmmMpSyncData->CpuData[Index].Token   = ProcToken;\r
+      }\r
+      if (CPUStatus != NULL) {\r
+        mSmmMpSyncData->CpuData[Index].Status    = &CPUStatus[Index];\r
+        if (mSmmMpSyncData->CpuData[Index].Status != NULL) {\r
+          *mSmmMpSyncData->CpuData[Index].Status = EFI_NOT_READY;\r
+        }\r
+      }\r
+    } else {\r
+      //\r
+      // PI spec requirement:\r
+      // For every excluded processor, the array entry must contain a value of EFI_NOT_STARTED.\r
+      //\r
+      if (CPUStatus != NULL) {\r
+        CPUStatus[Index] = EFI_NOT_STARTED;\r
+      }\r
+\r
+      //\r
+      // Decrease the count to mark this processor(AP or BSP) as finished.\r
+      //\r
+      if (ProcToken != NULL) {\r
+        WaitForSemaphore (&ProcToken->RunningApCount);\r
+      }\r
+    }\r
+  }\r
+\r
+  ReleaseAllAPs ();\r
+\r
+  if (Token == NULL) {\r
+    //\r
+    // Make sure all APs have completed their tasks.\r
+    //\r
+    WaitForAllAPsNotBusy (TRUE);\r
+  }\r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+/**\r
+  ISO C99 6.5.2.2 "Function calls", paragraph 9:\r
+  If the function is defined with a type that is not compatible with\r
+  the type (of the expression) pointed to by the expression that\r
+  denotes the called function, the behavior is undefined.\r
+\r
+  So add below wrapper function to convert between EFI_AP_PROCEDURE\r
+  and EFI_AP_PROCEDURE2.\r
+\r
+  Wrapper for Procedures.\r
+\r
+  @param[in]  Buffer              Pointer to PROCEDURE_WRAPPER buffer.\r
+\r
+**/\r
+EFI_STATUS\r
+EFIAPI\r
+ProcedureWrapper (\r
+  IN     VOID *Buffer\r
+  )\r
+{\r
+  PROCEDURE_WRAPPER *Wrapper;\r
+\r
+  Wrapper = Buffer;\r
+  Wrapper->Procedure (Wrapper->ProcedureArgument);\r
+\r
   return EFI_SUCCESS;\r
 }\r
 \r
@@ -980,7 +1474,15 @@ SmmBlockingStartupThisAp (
   IN OUT  VOID                      *ProcArguments OPTIONAL\r
   )\r
 {\r
-  return InternalSmmStartupThisAp(Procedure, CpuIndex, ProcArguments, TRUE);\r
+  PROCEDURE_WRAPPER  Wrapper;\r
+\r
+  Wrapper.Procedure = Procedure;\r
+  Wrapper.ProcedureArgument = ProcArguments;\r
+\r
+  //\r
+  // Use wrapper function to convert EFI_AP_PROCEDURE to EFI_AP_PROCEDURE2.\r
+  //\r
+  return InternalSmmStartupThisAp (ProcedureWrapper, CpuIndex, &Wrapper, NULL, 0, NULL);\r
 }\r
 \r
 /**\r
@@ -1005,7 +1507,22 @@ SmmStartupThisAp (
   IN OUT  VOID                      *ProcArguments OPTIONAL\r
   )\r
 {\r
-  return InternalSmmStartupThisAp(Procedure, CpuIndex, ProcArguments, FeaturePcdGet (PcdCpuSmmBlockStartupThisAp));\r
+  MM_COMPLETION               Token;\r
+\r
+  gSmmCpuPrivate->ApWrapperFunc[CpuIndex].Procedure = Procedure;\r
+  gSmmCpuPrivate->ApWrapperFunc[CpuIndex].ProcedureArgument = ProcArguments;\r
+\r
+  //\r
+  // Use wrapper function to convert EFI_AP_PROCEDURE to EFI_AP_PROCEDURE2.\r
+  //\r
+  return InternalSmmStartupThisAp (\r
+    ProcedureWrapper,\r
+    CpuIndex,\r
+    &gSmmCpuPrivate->ApWrapperFunc[CpuIndex],\r
+    FeaturePcdGet (PcdCpuSmmBlockStartupThisAp) ? NULL : &Token,\r
+    0,\r
+    NULL\r
+    );\r
 }\r
 \r
 /**\r
@@ -1025,7 +1542,7 @@ CpuSmmDebugEntry (
   )\r
 {\r
   SMRAM_SAVE_STATE_MAP *CpuSaveState;\r
-  \r
+\r
   if (FeaturePcdGet (PcdCpuSmmDebug)) {\r
     ASSERT(CpuIndex < mMaxNumberOfCpus);\r
     CpuSaveState = (SMRAM_SAVE_STATE_MAP *)gSmmCpuPrivate->CpuSaveState[CpuIndex];\r
@@ -1091,9 +1608,18 @@ SmiRendezvous (
   ASSERT(CpuIndex < mMaxNumberOfCpus);\r
 \r
   //\r
-  // Save Cr2 because Page Fault exception in SMM may override its value\r
+  // Save Cr2 because Page Fault exception in SMM may override its value,\r
+  // when using on-demand paging for above 4G memory.\r
   //\r
-  Cr2 = AsmReadCr2 ();\r
+  Cr2 = 0;\r
+  SaveCr2 (&Cr2);\r
+\r
+  //\r
+  // Call the user register Startup function first.\r
+  //\r
+  if (mSmmMpSyncData->StartupProcedure != NULL) {\r
+    mSmmMpSyncData->StartupProcedure (mSmmMpSyncData->StartupProcArgs);\r
+  }\r
 \r
   //\r
   // Perform CPU specific entry hooks\r
@@ -1232,10 +1758,28 @@ SmiRendezvous (
 \r
 Exit:\r
   SmmCpuFeaturesRendezvousExit (CpuIndex);\r
+\r
   //\r
   // Restore Cr2\r
   //\r
-  AsmWriteCr2 (Cr2);\r
+  RestoreCr2 (Cr2);\r
+}\r
+\r
+/**\r
+  Allocate buffer for SpinLock and Wrapper function buffer.\r
+\r
+**/\r
+VOID\r
+InitializeDataForMmMp (\r
+  VOID\r
+  )\r
+{\r
+  gSmmCpuPrivate->ApWrapperFunc = AllocatePool (sizeof (PROCEDURE_WRAPPER) * gSmmCpuPrivate->SmmCoreEntryContext.NumberOfCpus);\r
+  ASSERT (gSmmCpuPrivate->ApWrapperFunc != NULL);\r
+\r
+  InitializeListHead (&gSmmCpuPrivate->TokenList);\r
+\r
+  AllocateTokenBuffer ();\r
 }\r
 \r
 /**\r
@@ -1251,7 +1795,6 @@ InitializeSmmCpuSemaphores (
   UINTN                      TotalSize;\r
   UINTN                      GlobalSemaphoresSize;\r
   UINTN                      CpuSemaphoresSize;\r
-  UINTN                      MsrSemahporeSize;\r
   UINTN                      SemaphoreSize;\r
   UINTN                      Pages;\r
   UINTN                      *SemaphoreBlock;\r
@@ -1261,8 +1804,7 @@ InitializeSmmCpuSemaphores (
   ProcessorCount = gSmmCpuPrivate->SmmCoreEntryContext.NumberOfCpus;\r
   GlobalSemaphoresSize = (sizeof (SMM_CPU_SEMAPHORE_GLOBAL) / sizeof (VOID *)) * SemaphoreSize;\r
   CpuSemaphoresSize    = (sizeof (SMM_CPU_SEMAPHORE_CPU) / sizeof (VOID *)) * ProcessorCount * SemaphoreSize;\r
-  MsrSemahporeSize     = MSR_SPIN_LOCK_INIT_NUM * SemaphoreSize;\r
-  TotalSize = GlobalSemaphoresSize + CpuSemaphoresSize + MsrSemahporeSize;\r
+  TotalSize = GlobalSemaphoresSize + CpuSemaphoresSize;\r
   DEBUG((EFI_D_INFO, "One Semaphore Size    = 0x%x\n", SemaphoreSize));\r
   DEBUG((EFI_D_INFO, "Total Semaphores Size = 0x%x\n", TotalSize));\r
   Pages = EFI_SIZE_TO_PAGES (TotalSize);\r
@@ -1282,8 +1824,6 @@ InitializeSmmCpuSemaphores (
   mSmmCpuSemaphores.SemaphoreGlobal.CodeAccessCheckLock\r
                                                   = (SPIN_LOCK *)SemaphoreAddr;\r
   SemaphoreAddr += SemaphoreSize;\r
-  mSmmCpuSemaphores.SemaphoreGlobal.MemoryMappedLock\r
-                                                  = (SPIN_LOCK *)SemaphoreAddr;\r
 \r
   SemaphoreAddr = (UINTN)SemaphoreBlock + GlobalSemaphoresSize;\r
   mSmmCpuSemaphores.SemaphoreCpu.Busy    = (SPIN_LOCK *)SemaphoreAddr;\r
@@ -1292,15 +1832,8 @@ InitializeSmmCpuSemaphores (
   SemaphoreAddr += ProcessorCount * SemaphoreSize;\r
   mSmmCpuSemaphores.SemaphoreCpu.Present = (BOOLEAN *)SemaphoreAddr;\r
 \r
-  SemaphoreAddr = (UINTN)SemaphoreBlock + GlobalSemaphoresSize + CpuSemaphoresSize;\r
-  mSmmCpuSemaphores.SemaphoreMsr.Msr              = (SPIN_LOCK *)SemaphoreAddr;\r
-  mSmmCpuSemaphores.SemaphoreMsr.AvailableCounter =\r
-        ((UINTN)SemaphoreBlock + Pages * SIZE_4KB - SemaphoreAddr) / SemaphoreSize;\r
-  ASSERT (mSmmCpuSemaphores.SemaphoreMsr.AvailableCounter >= MSR_SPIN_LOCK_INIT_NUM);\r
-\r
   mPFLock                       = mSmmCpuSemaphores.SemaphoreGlobal.PFLock;\r
   mConfigSmmCodeAccessCheckLock = mSmmCpuSemaphores.SemaphoreGlobal.CodeAccessCheckLock;\r
-  mMemoryMappedLock             = mSmmCpuSemaphores.SemaphoreGlobal.MemoryMappedLock;\r
 \r
   mSemaphoreSize = SemaphoreSize;\r
 }\r
@@ -1359,20 +1892,29 @@ InitializeMpSyncData (
 /**\r
   Initialize global data for MP synchronization.\r
 \r
-  @param Stacks       Base address of SMI stack buffer for all processors.\r
-  @param StackSize    Stack size for each processor in SMM.\r
+  @param Stacks             Base address of SMI stack buffer for all processors.\r
+  @param StackSize          Stack size for each processor in SMM.\r
+  @param ShadowStackSize    Shadow Stack size for each processor in SMM.\r
 \r
 **/\r
 UINT32\r
 InitializeMpServiceData (\r
   IN VOID        *Stacks,\r
-  IN UINTN       StackSize\r
+  IN UINTN       StackSize,\r
+  IN UINTN       ShadowStackSize\r
   )\r
 {\r
   UINT32                    Cr3;\r
   UINTN                     Index;\r
   UINT8                     *GdtTssTables;\r
   UINTN                     GdtTableStepSize;\r
+  CPUID_VERSION_INFO_EDX    RegEdx;\r
+\r
+  //\r
+  // Determine if this CPU supports machine check\r
+  //\r
+  AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &RegEdx.Uint32);\r
+  mMachineCheckSupported = (BOOLEAN)(RegEdx.Bits.MCA == 1);\r
 \r
   //\r
   // Allocate memory for all locks and semaphores\r
@@ -1411,7 +1953,7 @@ InitializeMpServiceData (
     InstallSmiHandler (\r
       Index,\r
       (UINT32)mCpuHotPlugData.SmBase[Index],\r
-      (VOID*)((UINTN)Stacks + (StackSize * Index)),\r
+      (VOID*)((UINTN)Stacks + (StackSize + ShadowStackSize) * Index),\r
       StackSize,\r
       (UINTN)(GdtTssTables + GdtTableStepSize * Index),\r
       gcSmiGdtr.Limit + 1,\r
@@ -1453,3 +1995,40 @@ RegisterSmmEntry (
   gSmmCpuPrivate->SmmCoreEntry = SmmEntryPoint;\r
   return EFI_SUCCESS;\r
 }\r
+\r
+/**\r
+\r
+  Register the SMM Foundation entry point.\r
+\r
+  @param[in]      Procedure            A pointer to the code stream to be run on the designated target AP\r
+                                       of the system. Type EFI_AP_PROCEDURE is defined below in Volume 2\r
+                                       with the related definitions of\r
+                                       EFI_MP_SERVICES_PROTOCOL.StartupAllAPs.\r
+                                       If caller may pass a value of NULL to deregister any existing\r
+                                       startup procedure.\r
+  @param[in,out]  ProcedureArguments   Allows the caller to pass a list of parameters to the code that is\r
+                                       run by the AP. It is an optional common mailbox between APs and\r
+                                       the caller to share information\r
+\r
+  @retval EFI_SUCCESS                  The Procedure has been set successfully.\r
+  @retval EFI_INVALID_PARAMETER        The Procedure is NULL but ProcedureArguments not NULL.\r
+\r
+**/\r
+EFI_STATUS\r
+RegisterStartupProcedure (\r
+  IN     EFI_AP_PROCEDURE    Procedure,\r
+  IN OUT VOID                *ProcedureArguments OPTIONAL\r
+  )\r
+{\r
+  if (Procedure == NULL && ProcedureArguments != NULL) {\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+  if (mSmmMpSyncData == NULL) {\r
+    return EFI_NOT_READY;\r
+  }\r
+\r
+  mSmmMpSyncData->StartupProcedure = Procedure;\r
+  mSmmMpSyncData->StartupProcArgs  = ProcedureArguments;\r
+\r
+  return EFI_SUCCESS;\r
+}\r