]> git.proxmox.com Git - mirror_edk2.git/blobdiff - MdeModulePkg/Core/Pei/Memory/MemoryServices.c
MdeModulePkg: Replace BSD License with BSD+Patent License
[mirror_edk2.git] / MdeModulePkg / Core / Pei / Memory / MemoryServices.c
index 1213702c680580e9968dedaee49821bb4db9bca2..42f79ab07633a591f5ea0a2f3d64e134e2640a27 100644 (file)
@@ -1,14 +1,8 @@
 /** @file\r
   EFI PEI Core memory services\r
-  \r
-Copyright (c) 2006 - 2014, 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
-                                                                                          \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
+\r
+Copyright (c) 2006 - 2018, Intel Corporation. All rights reserved.<BR>\r
+SPDX-License-Identifier: BSD-2-Clause-Patent\r
 \r
 **/\r
 \r
@@ -33,7 +27,7 @@ InitializeMemoryServices (
   IN PEI_CORE_INSTANCE           *OldCoreData\r
   )\r
 {\r
-  \r
+\r
   PrivateData->SwitchStackSignal    = FALSE;\r
 \r
   //\r
@@ -44,7 +38,7 @@ InitializeMemoryServices (
 \r
     PrivateData->PeiMemoryInstalled = FALSE;\r
     PrivateData->HobList.Raw        = SecCoreData->PeiTemporaryRamBase;\r
-    \r
+\r
     PeiCoreBuildHobHandoffInfoTable (\r
       BOOT_WITH_FULL_CONFIGURATION,\r
       (EFI_PHYSICAL_ADDRESS) (UINTN) SecCoreData->PeiTemporaryRamBase,\r
@@ -56,7 +50,7 @@ InitializeMemoryServices (
     //\r
     PrivateData->Ps = &(PrivateData->ServiceTableShadow);\r
   }\r
-  \r
+\r
   return;\r
 }\r
 \r
@@ -68,7 +62,7 @@ InitializeMemoryServices (
   This routine will hold discoveried memory information into PeiCore's private data,\r
   and set SwitchStackSignal flag. After PEIM who discovery memory is dispatched,\r
   PeiDispatcher will migrate temporary memory to permenement memory.\r
-  \r
+\r
   @param PeiServices        An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.\r
   @param MemoryBegin        Start of memory address.\r
   @param MemoryLength       Length of memory.\r
@@ -93,37 +87,406 @@ PeiInstallPeiMemory (
   // PEI_SERVICE.InstallPeiMemory should only be called one time during whole PEI phase.\r
   // If it is invoked more than one time, ASSERT information is given for developer debugging in debug tip and\r
   // simply return EFI_SUCESS in release tip to ignore it.\r
-  // \r
+  //\r
   if (PrivateData->PeiMemoryInstalled) {\r
     DEBUG ((EFI_D_ERROR, "ERROR: PeiInstallPeiMemory is called more than once!\n"));\r
-    ASSERT (PrivateData->PeiMemoryInstalled);\r
+    ASSERT (FALSE);\r
     return EFI_SUCCESS;\r
   }\r
-  \r
+\r
   PrivateData->PhysicalMemoryBegin   = MemoryBegin;\r
   PrivateData->PhysicalMemoryLength  = MemoryLength;\r
   PrivateData->FreePhysicalMemoryTop = MemoryBegin + MemoryLength;\r
-   \r
+\r
   PrivateData->SwitchStackSignal      = TRUE;\r
 \r
-  return EFI_SUCCESS;   \r
+  return EFI_SUCCESS;\r
+}\r
+\r
+/**\r
+  Migrate memory pages allocated in pre-memory phase.\r
+  Copy memory pages at temporary heap top to permanent heap top.\r
+\r
+  @param[in] Private                Pointer to the private data passed in from caller.\r
+  @param[in] TemporaryRamMigrated   Temporary memory has been migrated to permanent memory.\r
+\r
+**/\r
+VOID\r
+MigrateMemoryPages (\r
+  IN PEI_CORE_INSTANCE      *Private,\r
+  IN BOOLEAN                TemporaryRamMigrated\r
+  )\r
+{\r
+  EFI_PHYSICAL_ADDRESS      NewMemPagesBase;\r
+  EFI_PHYSICAL_ADDRESS      MemPagesBase;\r
+\r
+  Private->MemoryPages.Size = (UINTN) (Private->HobList.HandoffInformationTable->EfiMemoryTop -\r
+                                       Private->HobList.HandoffInformationTable->EfiFreeMemoryTop);\r
+  if (Private->MemoryPages.Size == 0) {\r
+    //\r
+    // No any memory page allocated in pre-memory phase.\r
+    //\r
+    return;\r
+  }\r
+  Private->MemoryPages.Base = Private->HobList.HandoffInformationTable->EfiFreeMemoryTop;\r
+\r
+  ASSERT (Private->MemoryPages.Size <= Private->FreePhysicalMemoryTop);\r
+  NewMemPagesBase = Private->FreePhysicalMemoryTop - Private->MemoryPages.Size;\r
+  NewMemPagesBase &= ~(UINT64)EFI_PAGE_MASK;\r
+  ASSERT (NewMemPagesBase >= Private->PhysicalMemoryBegin);\r
+  //\r
+  // Copy memory pages at temporary heap top to permanent heap top.\r
+  //\r
+  if (TemporaryRamMigrated) {\r
+    //\r
+    // Memory pages at temporary heap top has been migrated to permanent heap,\r
+    // Here still needs to copy them from permanent heap to permanent heap top.\r
+    //\r
+    MemPagesBase = Private->MemoryPages.Base;\r
+    if (Private->HeapOffsetPositive) {\r
+      MemPagesBase += Private->HeapOffset;\r
+    } else {\r
+      MemPagesBase -= Private->HeapOffset;\r
+    }\r
+    CopyMem ((VOID *)(UINTN)NewMemPagesBase, (VOID *)(UINTN)MemPagesBase, Private->MemoryPages.Size);\r
+  } else {\r
+    CopyMem ((VOID *)(UINTN)NewMemPagesBase, (VOID *)(UINTN)Private->MemoryPages.Base, Private->MemoryPages.Size);\r
+  }\r
+\r
+  if (NewMemPagesBase >= Private->MemoryPages.Base) {\r
+    Private->MemoryPages.OffsetPositive = TRUE;\r
+    Private->MemoryPages.Offset = (UINTN)(NewMemPagesBase - Private->MemoryPages.Base);\r
+  } else {\r
+    Private->MemoryPages.OffsetPositive = FALSE;\r
+    Private->MemoryPages.Offset = (UINTN)(Private->MemoryPages.Base - NewMemPagesBase);\r
+  }\r
+\r
+  DEBUG ((DEBUG_INFO, "Pages Offset = 0x%lX\n", (UINT64) Private->MemoryPages.Offset));\r
+\r
+  Private->FreePhysicalMemoryTop = NewMemPagesBase;\r
+}\r
+\r
+/**\r
+  Migrate MemoryBaseAddress in memory allocation HOBs\r
+  from the temporary memory to PEI installed memory.\r
+\r
+  @param[in] PrivateData        Pointer to PeiCore's private data structure.\r
+\r
+**/\r
+VOID\r
+ConvertMemoryAllocationHobs (\r
+  IN PEI_CORE_INSTANCE          *PrivateData\r
+  )\r
+{\r
+  EFI_PEI_HOB_POINTERS          Hob;\r
+  EFI_HOB_MEMORY_ALLOCATION     *MemoryAllocationHob;\r
+  EFI_PHYSICAL_ADDRESS          OldMemPagesBase;\r
+  UINTN                         OldMemPagesSize;\r
+\r
+  if (PrivateData->MemoryPages.Size == 0) {\r
+    //\r
+    // No any memory page allocated in pre-memory phase.\r
+    //\r
+    return;\r
+  }\r
+\r
+  OldMemPagesBase = PrivateData->MemoryPages.Base;\r
+  OldMemPagesSize = PrivateData->MemoryPages.Size;\r
+\r
+  MemoryAllocationHob = NULL;\r
+  Hob.Raw = GetFirstHob (EFI_HOB_TYPE_MEMORY_ALLOCATION);\r
+  while (Hob.Raw != NULL) {\r
+    MemoryAllocationHob = (EFI_HOB_MEMORY_ALLOCATION *) Hob.Raw;\r
+    if ((MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress >= OldMemPagesBase) &&\r
+        (MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress < (OldMemPagesBase + OldMemPagesSize))\r
+        ) {\r
+      if (PrivateData->MemoryPages.OffsetPositive) {\r
+        MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress += PrivateData->MemoryPages.Offset;\r
+      } else {\r
+        MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress -= PrivateData->MemoryPages.Offset;\r
+      }\r
+    }\r
+\r
+    Hob.Raw = GET_NEXT_HOB (Hob);\r
+    Hob.Raw = GetNextHob (EFI_HOB_TYPE_MEMORY_ALLOCATION, Hob.Raw);\r
+  }\r
+}\r
+\r
+/**\r
+  Internal function to build a HOB for the memory allocation.\r
+  It will search and reuse the unused(freed) memory allocation HOB,\r
+  or build memory allocation HOB normally if no unused(freed) memory allocation HOB found.\r
+\r
+  @param[in] BaseAddress        The 64 bit physical address of the memory.\r
+  @param[in] Length             The length of the memory allocation in bytes.\r
+  @param[in] MemoryType         The type of memory allocated by this HOB.\r
+\r
+**/\r
+VOID\r
+InternalBuildMemoryAllocationHob (\r
+  IN EFI_PHYSICAL_ADDRESS       BaseAddress,\r
+  IN UINT64                     Length,\r
+  IN EFI_MEMORY_TYPE            MemoryType\r
+  )\r
+{\r
+  EFI_PEI_HOB_POINTERS          Hob;\r
+  EFI_HOB_MEMORY_ALLOCATION     *MemoryAllocationHob;\r
+\r
+  //\r
+  // Search unused(freed) memory allocation HOB.\r
+  //\r
+  MemoryAllocationHob = NULL;\r
+  Hob.Raw = GetFirstHob (EFI_HOB_TYPE_UNUSED);\r
+  while (Hob.Raw != NULL) {\r
+    if (Hob.Header->HobLength == sizeof (EFI_HOB_MEMORY_ALLOCATION)) {\r
+      MemoryAllocationHob = (EFI_HOB_MEMORY_ALLOCATION *) Hob.Raw;\r
+      break;\r
+    }\r
+\r
+    Hob.Raw = GET_NEXT_HOB (Hob);\r
+    Hob.Raw = GetNextHob (EFI_HOB_TYPE_UNUSED, Hob.Raw);\r
+  }\r
+\r
+  if (MemoryAllocationHob != NULL) {\r
+    //\r
+    // Reuse the unused(freed) memory allocation HOB.\r
+    //\r
+    MemoryAllocationHob->Header.HobType = EFI_HOB_TYPE_MEMORY_ALLOCATION;\r
+    ZeroMem (&(MemoryAllocationHob->AllocDescriptor.Name), sizeof (EFI_GUID));\r
+    MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress = BaseAddress;\r
+    MemoryAllocationHob->AllocDescriptor.MemoryLength      = Length;\r
+    MemoryAllocationHob->AllocDescriptor.MemoryType        = MemoryType;\r
+    //\r
+    // Zero the reserved space to match HOB spec\r
+    //\r
+    ZeroMem (MemoryAllocationHob->AllocDescriptor.Reserved, sizeof (MemoryAllocationHob->AllocDescriptor.Reserved));\r
+  } else {\r
+    //\r
+    // No unused(freed) memory allocation HOB found.\r
+    // Build memory allocation HOB normally.\r
+    //\r
+    BuildMemoryAllocationHob (\r
+      BaseAddress,\r
+      Length,\r
+      MemoryType\r
+      );\r
+  }\r
+}\r
+\r
+/**\r
+  Update or split memory allocation HOB for memory pages allocate and free.\r
+\r
+  @param[in, out] MemoryAllocationHob   Pointer to the memory allocation HOB\r
+                                        that needs to be updated or split.\r
+                                        On output, it will be filled with\r
+                                        the input Memory, Bytes and MemoryType.\r
+  @param[in]      Memory                Memory to allocate or free.\r
+  @param[in]      Bytes                 Bytes to allocate or free.\r
+  @param[in]      MemoryType            EfiConventionalMemory for pages free,\r
+                                        others for pages allocate.\r
+\r
+**/\r
+VOID\r
+UpdateOrSplitMemoryAllocationHob (\r
+  IN OUT EFI_HOB_MEMORY_ALLOCATION      *MemoryAllocationHob,\r
+  IN EFI_PHYSICAL_ADDRESS               Memory,\r
+  IN UINT64                             Bytes,\r
+  IN EFI_MEMORY_TYPE                    MemoryType\r
+  )\r
+{\r
+  if ((Memory + Bytes) <\r
+      (MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress + MemoryAllocationHob->AllocDescriptor.MemoryLength)) {\r
+    //\r
+    // Last pages need to be split out.\r
+    //\r
+    InternalBuildMemoryAllocationHob (\r
+      Memory + Bytes,\r
+      (MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress + MemoryAllocationHob->AllocDescriptor.MemoryLength) - (Memory + Bytes),\r
+      MemoryAllocationHob->AllocDescriptor.MemoryType\r
+      );\r
+  }\r
+\r
+  if (Memory > MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress) {\r
+    //\r
+    // First pages need to be split out.\r
+    //\r
+    InternalBuildMemoryAllocationHob (\r
+      MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress,\r
+      Memory - MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress,\r
+      MemoryAllocationHob->AllocDescriptor.MemoryType\r
+      );\r
+  }\r
+\r
+  //\r
+  // Update the memory allocation HOB.\r
+  //\r
+  MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress = Memory;\r
+  MemoryAllocationHob->AllocDescriptor.MemoryLength = Bytes;\r
+  MemoryAllocationHob->AllocDescriptor.MemoryType = MemoryType;\r
 }\r
 \r
 /**\r
-  The purpose of the service is to publish an interface that allows \r
+  Merge adjacent free memory ranges in memory allocation HOBs.\r
+\r
+  @retval TRUE          There are free memory ranges merged.\r
+  @retval FALSE         No free memory ranges merged.\r
+\r
+**/\r
+BOOLEAN\r
+MergeFreeMemoryInMemoryAllocationHob (\r
+  VOID\r
+  )\r
+{\r
+  EFI_PEI_HOB_POINTERS          Hob;\r
+  EFI_PEI_HOB_POINTERS          Hob2;\r
+  EFI_HOB_MEMORY_ALLOCATION     *MemoryHob;\r
+  EFI_HOB_MEMORY_ALLOCATION     *MemoryHob2;\r
+  UINT64                        Start;\r
+  UINT64                        End;\r
+  BOOLEAN                       Merged;\r
+\r
+  Merged = FALSE;\r
+\r
+  Hob.Raw = GetFirstHob (EFI_HOB_TYPE_MEMORY_ALLOCATION);\r
+  while (Hob.Raw != NULL) {\r
+    if (Hob.MemoryAllocation->AllocDescriptor.MemoryType == EfiConventionalMemory) {\r
+      MemoryHob = (EFI_HOB_MEMORY_ALLOCATION *) Hob.Raw;\r
+      Start = MemoryHob->AllocDescriptor.MemoryBaseAddress;\r
+      End = MemoryHob->AllocDescriptor.MemoryBaseAddress + MemoryHob->AllocDescriptor.MemoryLength;\r
+\r
+      Hob2.Raw = GET_NEXT_HOB (Hob);\r
+      Hob2.Raw = GetNextHob (EFI_HOB_TYPE_MEMORY_ALLOCATION, Hob.Raw);\r
+      while (Hob2.Raw != NULL) {\r
+        if (Hob2.MemoryAllocation->AllocDescriptor.MemoryType == EfiConventionalMemory) {\r
+          MemoryHob2 = (EFI_HOB_MEMORY_ALLOCATION *) Hob2.Raw;\r
+          if (Start == (MemoryHob2->AllocDescriptor.MemoryBaseAddress + MemoryHob2->AllocDescriptor.MemoryLength)) {\r
+            //\r
+            // Merge adjacent two free memory ranges.\r
+            //\r
+            MemoryHob2->AllocDescriptor.MemoryLength += MemoryHob->AllocDescriptor.MemoryLength;\r
+            Merged = TRUE;\r
+            //\r
+            // Mark MemoryHob to be unused(freed).\r
+            //\r
+            MemoryHob->Header.HobType = EFI_HOB_TYPE_UNUSED;\r
+            break;\r
+          } else if (End == MemoryHob2->AllocDescriptor.MemoryBaseAddress) {\r
+            //\r
+            // Merge adjacent two free memory ranges.\r
+            //\r
+            MemoryHob2->AllocDescriptor.MemoryBaseAddress = MemoryHob->AllocDescriptor.MemoryBaseAddress;\r
+            MemoryHob2->AllocDescriptor.MemoryLength += MemoryHob->AllocDescriptor.MemoryLength;\r
+            Merged = TRUE;\r
+            //\r
+            // Mark MemoryHob to be unused(freed).\r
+            //\r
+            MemoryHob->Header.HobType = EFI_HOB_TYPE_UNUSED;\r
+            break;\r
+          }\r
+        }\r
+        Hob2.Raw = GET_NEXT_HOB (Hob2);\r
+        Hob2.Raw = GetNextHob (EFI_HOB_TYPE_MEMORY_ALLOCATION, Hob2.Raw);\r
+      }\r
+    }\r
+    Hob.Raw = GET_NEXT_HOB (Hob);\r
+    Hob.Raw = GetNextHob (EFI_HOB_TYPE_MEMORY_ALLOCATION, Hob.Raw);\r
+  }\r
+\r
+  return Merged;\r
+}\r
+\r
+/**\r
+  Find free memory by searching memory allocation HOBs.\r
+\r
+  @param[in]  MemoryType        The type of memory to allocate.\r
+  @param[in]  Pages             The number of contiguous 4 KB pages to allocate.\r
+  @param[in]  Granularity       Page allocation granularity.\r
+  @param[out] Memory            Pointer to a physical address. On output, the address is set to the base\r
+                                of the page range that was allocated.\r
+\r
+  @retval EFI_SUCCESS           The memory range was successfully allocated.\r
+  @retval EFI_NOT_FOUND         No memory allocation HOB with big enough free memory found.\r
+\r
+**/\r
+EFI_STATUS\r
+FindFreeMemoryFromMemoryAllocationHob (\r
+  IN  EFI_MEMORY_TYPE           MemoryType,\r
+  IN  UINTN                     Pages,\r
+  IN  UINTN                     Granularity,\r
+  OUT EFI_PHYSICAL_ADDRESS      *Memory\r
+  )\r
+{\r
+  EFI_PEI_HOB_POINTERS          Hob;\r
+  EFI_HOB_MEMORY_ALLOCATION     *MemoryAllocationHob;\r
+  UINT64                        Bytes;\r
+  EFI_PHYSICAL_ADDRESS          BaseAddress;\r
+\r
+  Bytes = LShiftU64 (Pages, EFI_PAGE_SHIFT);\r
+\r
+  BaseAddress = 0;\r
+  MemoryAllocationHob = NULL;\r
+  Hob.Raw = GetFirstHob (EFI_HOB_TYPE_MEMORY_ALLOCATION);\r
+  while (Hob.Raw != NULL) {\r
+    if ((Hob.MemoryAllocation->AllocDescriptor.MemoryType == EfiConventionalMemory) &&\r
+        (Hob.MemoryAllocation->AllocDescriptor.MemoryLength >= Bytes)) {\r
+      //\r
+      // Found one memory allocation HOB with big enough free memory.\r
+      //\r
+      MemoryAllocationHob = (EFI_HOB_MEMORY_ALLOCATION *) Hob.Raw;\r
+      BaseAddress = MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress +\r
+                    MemoryAllocationHob->AllocDescriptor.MemoryLength - Bytes;\r
+      //\r
+      // Make sure the granularity could be satisfied.\r
+      //\r
+      BaseAddress &= ~((EFI_PHYSICAL_ADDRESS) Granularity - 1);\r
+      if (BaseAddress >= MemoryAllocationHob->AllocDescriptor.MemoryBaseAddress) {\r
+        break;\r
+      }\r
+      BaseAddress = 0;\r
+      MemoryAllocationHob = NULL;\r
+    }\r
+    //\r
+    // Continue to find.\r
+    //\r
+    Hob.Raw = GET_NEXT_HOB (Hob);\r
+    Hob.Raw = GetNextHob (EFI_HOB_TYPE_MEMORY_ALLOCATION, Hob.Raw);\r
+  }\r
+\r
+  if (MemoryAllocationHob != NULL) {\r
+    UpdateOrSplitMemoryAllocationHob (MemoryAllocationHob, BaseAddress, Bytes, MemoryType);\r
+    *Memory = BaseAddress;\r
+    return EFI_SUCCESS;\r
+  } else {\r
+    if (MergeFreeMemoryInMemoryAllocationHob ()) {\r
+      //\r
+      // Retry if there are free memory ranges merged.\r
+      //\r
+      return FindFreeMemoryFromMemoryAllocationHob (MemoryType, Pages, Granularity, Memory);\r
+    }\r
+    return EFI_NOT_FOUND;\r
+  }\r
+}\r
+\r
+/**\r
+  The purpose of the service is to publish an interface that allows\r
   PEIMs to allocate memory ranges that are managed by the PEI Foundation.\r
 \r
+  Prior to InstallPeiMemory() being called, PEI will allocate pages from the heap.\r
+  After InstallPeiMemory() is called, PEI will allocate pages within the region\r
+  of memory provided by InstallPeiMemory() service in a best-effort fashion.\r
+  Location-specific allocations are not managed by the PEI foundation code.\r
+\r
   @param  PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.\r
   @param  MemoryType       The type of memory to allocate.\r
   @param  Pages            The number of contiguous 4 KB pages to allocate.\r
-  @param  Memory           Pointer to a physical address. On output, the address is set to the base \r
+  @param  Memory           Pointer to a physical address. On output, the address is set to the base\r
                            of the page range that was allocated.\r
 \r
   @retval EFI_SUCCESS           The memory range was successfully allocated.\r
   @retval EFI_OUT_OF_RESOURCES  The pages could not be allocated.\r
-  @retval EFI_INVALID_PARAMETER Type is not equal to EfiLoaderCode, EfiLoaderData, EfiRuntimeServicesCode, \r
+  @retval EFI_INVALID_PARAMETER Type is not equal to EfiLoaderCode, EfiLoaderData, EfiRuntimeServicesCode,\r
                                 EfiRuntimeServicesData, EfiBootServicesCode, EfiBootServicesData,\r
-                                EfiACPIReclaimMemory, or EfiACPIMemoryNVS.\r
+                                EfiACPIReclaimMemory, EfiReservedMemoryType, or EfiACPIMemoryNVS.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -135,11 +498,14 @@ PeiAllocatePages (
   OUT      EFI_PHYSICAL_ADDRESS *Memory\r
   )\r
 {\r
+  EFI_STATUS                              Status;\r
   PEI_CORE_INSTANCE                       *PrivateData;\r
   EFI_PEI_HOB_POINTERS                    Hob;\r
   EFI_PHYSICAL_ADDRESS                    *FreeMemoryTop;\r
   EFI_PHYSICAL_ADDRESS                    *FreeMemoryBottom;\r
   UINTN                                   RemainingPages;\r
+  UINTN                                   Granularity;\r
+  UINTN                                   Padding;\r
 \r
   if ((MemoryType != EfiLoaderCode) &&\r
       (MemoryType != EfiLoaderData) &&\r
@@ -148,50 +514,86 @@ PeiAllocatePages (
       (MemoryType != EfiBootServicesCode) &&\r
       (MemoryType != EfiBootServicesData) &&\r
       (MemoryType != EfiACPIReclaimMemory) &&\r
+      (MemoryType != EfiReservedMemoryType) &&\r
       (MemoryType != EfiACPIMemoryNVS)) {\r
     return EFI_INVALID_PARAMETER;\r
   }\r
 \r
+  Granularity = DEFAULT_PAGE_ALLOCATION_GRANULARITY;\r
+\r
   PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS (PeiServices);\r
   Hob.Raw     = PrivateData->HobList.Raw;\r
-  \r
-  //\r
-  // Check if Hob already available\r
-  //\r
-  if (!PrivateData->PeiMemoryInstalled) {\r
+\r
+  if (Hob.Raw == NULL) {\r
+    //\r
+    // HOB is not initialized yet.\r
+    //\r
+    return EFI_NOT_AVAILABLE_YET;\r
+  }\r
+\r
+  if  (RUNTIME_PAGE_ALLOCATION_GRANULARITY > DEFAULT_PAGE_ALLOCATION_GRANULARITY &&\r
+       (MemoryType == EfiACPIReclaimMemory   ||\r
+        MemoryType == EfiACPIMemoryNVS       ||\r
+        MemoryType == EfiRuntimeServicesCode ||\r
+        MemoryType == EfiRuntimeServicesData)) {\r
+\r
+    Granularity = RUNTIME_PAGE_ALLOCATION_GRANULARITY;\r
+\r
+    DEBUG ((DEBUG_INFO, "AllocatePages: aligning allocation to %d KB\n",\r
+      Granularity / SIZE_1KB));\r
+  }\r
+\r
+  if (!PrivateData->PeiMemoryInstalled && PrivateData->SwitchStackSignal) {\r
     //\r
-    // When PeiInstallMemory is called but temporary memory has *not* been moved to temporary memory,\r
+    // When PeiInstallMemory is called but temporary memory has *not* been moved to permanent memory,\r
     // the AllocatePage will depend on the field of PEI_CORE_INSTANCE structure.\r
     //\r
-    if (!PrivateData->SwitchStackSignal) {\r
-      return EFI_NOT_AVAILABLE_YET;\r
-    } else {\r
-      FreeMemoryTop     = &(PrivateData->FreePhysicalMemoryTop);\r
-      FreeMemoryBottom  = &(PrivateData->PhysicalMemoryBegin);\r
-    }\r
+    FreeMemoryTop     = &(PrivateData->FreePhysicalMemoryTop);\r
+    FreeMemoryBottom  = &(PrivateData->PhysicalMemoryBegin);\r
   } else {\r
     FreeMemoryTop     = &(Hob.HandoffInformationTable->EfiFreeMemoryTop);\r
     FreeMemoryBottom  = &(Hob.HandoffInformationTable->EfiFreeMemoryBottom);\r
   }\r
 \r
   //\r
-  // Check to see if on 4k boundary, If not aligned, make the allocation aligned.\r
+  // Check to see if on correct boundary for the memory type.\r
+  // If not aligned, make the allocation aligned.\r
   //\r
-  *(FreeMemoryTop) -= *(FreeMemoryTop) & 0xFFF;\r
+  Padding = *(FreeMemoryTop) & (Granularity - 1);\r
+  if ((UINTN) (*FreeMemoryTop - *FreeMemoryBottom) < Padding) {\r
+    DEBUG ((DEBUG_ERROR, "AllocatePages failed: Out of space after padding.\n"));\r
+    return EFI_OUT_OF_RESOURCES;\r
+  }\r
+\r
+  *(FreeMemoryTop) -= Padding;\r
+  if (Padding >= EFI_PAGE_SIZE) {\r
+    //\r
+    // Create a memory allocation HOB to cover\r
+    // the pages that we will lose to rounding\r
+    //\r
+    InternalBuildMemoryAllocationHob (\r
+      *(FreeMemoryTop),\r
+      Padding & ~(UINTN)EFI_PAGE_MASK,\r
+      EfiConventionalMemory\r
+      );\r
+  }\r
 \r
   //\r
   // Verify that there is sufficient memory to satisfy the allocation.\r
-  // For page allocation, the overhead sizeof (EFI_HOB_MEMORY_ALLOCATION) needs to be considered.\r
   //\r
-  if ((UINTN) (*FreeMemoryTop - *FreeMemoryBottom) < (UINTN) ALIGN_VALUE (sizeof (EFI_HOB_MEMORY_ALLOCATION), 8)) {\r
-    DEBUG ((EFI_D_ERROR, "AllocatePages failed: No space to build memory allocation hob.\n"));\r
-    return  EFI_OUT_OF_RESOURCES;\r
-  }\r
-  RemainingPages = (UINTN)(*FreeMemoryTop - *FreeMemoryBottom - ALIGN_VALUE (sizeof (EFI_HOB_MEMORY_ALLOCATION), 8)) >> EFI_PAGE_SHIFT;\r
+  RemainingPages = (UINTN)(*FreeMemoryTop - *FreeMemoryBottom) >> EFI_PAGE_SHIFT;\r
   //\r
   // The number of remaining pages needs to be greater than or equal to that of the request pages.\r
   //\r
+  Pages = ALIGN_VALUE (Pages, EFI_SIZE_TO_PAGES (Granularity));\r
   if (RemainingPages < Pages) {\r
+    //\r
+    // Try to find free memory by searching memory allocation HOBs.\r
+    //\r
+    Status = FindFreeMemoryFromMemoryAllocationHob (MemoryType, Pages, Granularity, Memory);\r
+    if (!EFI_ERROR (Status)) {\r
+      return Status;\r
+    }\r
     DEBUG ((EFI_D_ERROR, "AllocatePages failed: No 0x%lx Pages is available.\n", (UINT64) Pages));\r
     DEBUG ((EFI_D_ERROR, "There is only left 0x%lx pages memory resource to be allocated.\n", (UINT64) RemainingPages));\r
     return  EFI_OUT_OF_RESOURCES;\r
@@ -209,7 +611,7 @@ PeiAllocatePages (
     //\r
     // Create a memory allocation HOB.\r
     //\r
-    BuildMemoryAllocationHob (\r
+    InternalBuildMemoryAllocationHob (\r
       *(FreeMemoryTop),\r
       Pages * EFI_PAGE_SIZE,\r
       MemoryType\r
@@ -220,10 +622,145 @@ PeiAllocatePages (
 }\r
 \r
 /**\r
+  Mark the memory allocation HOB to be unused(freed) and update *FreeMemoryTop\r
+  if MemoryBaseAddress == *FreeMemoryTop.\r
+\r
+  @param[in]      PrivateData                   Pointer to PeiCore's private data structure.\r
+  @param[in, out] MemoryAllocationHobToFree     Pointer to memory allocation HOB to be freed.\r
 \r
-  Pool allocation service. Before permenent memory is discoveried, the pool will \r
-  be allocated the heap in the temporary memory. Genenrally, the size of heap in temporary \r
-  memory does not exceed to 64K, so the biggest pool size could be allocated is \r
+**/\r
+VOID\r
+FreeMemoryAllocationHob (\r
+  IN PEI_CORE_INSTANCE                  *PrivateData,\r
+  IN OUT EFI_HOB_MEMORY_ALLOCATION      *MemoryAllocationHobToFree\r
+  )\r
+{\r
+  EFI_PEI_HOB_POINTERS                  Hob;\r
+  EFI_PHYSICAL_ADDRESS                  *FreeMemoryTop;\r
+  EFI_HOB_MEMORY_ALLOCATION             *MemoryAllocationHob;\r
+\r
+  Hob.Raw = PrivateData->HobList.Raw;\r
+\r
+  if (!PrivateData->PeiMemoryInstalled && PrivateData->SwitchStackSignal) {\r
+    //\r
+    // When PeiInstallMemory is called but temporary memory has *not* been moved to permanent memory,\r
+    // use the FreePhysicalMemoryTop field of PEI_CORE_INSTANCE structure.\r
+    //\r
+    FreeMemoryTop = &(PrivateData->FreePhysicalMemoryTop);\r
+  } else {\r
+    FreeMemoryTop = &(Hob.HandoffInformationTable->EfiFreeMemoryTop);\r
+  }\r
+\r
+  if (MemoryAllocationHobToFree->AllocDescriptor.MemoryBaseAddress == *FreeMemoryTop) {\r
+    //\r
+    // Update *FreeMemoryTop.\r
+    //\r
+    *FreeMemoryTop += MemoryAllocationHobToFree->AllocDescriptor.MemoryLength;\r
+    //\r
+    // Mark the memory allocation HOB to be unused(freed).\r
+    //\r
+    MemoryAllocationHobToFree->Header.HobType = EFI_HOB_TYPE_UNUSED;\r
+\r
+    MemoryAllocationHob = NULL;\r
+    Hob.Raw = GetFirstHob (EFI_HOB_TYPE_MEMORY_ALLOCATION);\r
+    while (Hob.Raw != NULL) {\r
+      if ((Hob.MemoryAllocation->AllocDescriptor.MemoryType == EfiConventionalMemory) &&\r
+          (Hob.MemoryAllocation->AllocDescriptor.MemoryBaseAddress == *FreeMemoryTop)) {\r
+        //\r
+        // Found memory allocation HOB that has EfiConventionalMemory MemoryType and\r
+        // MemoryBaseAddress == new *FreeMemoryTop.\r
+        //\r
+        MemoryAllocationHob = (EFI_HOB_MEMORY_ALLOCATION *) Hob.Raw;\r
+        break;\r
+      }\r
+      Hob.Raw = GET_NEXT_HOB (Hob);\r
+      Hob.Raw = GetNextHob (EFI_HOB_TYPE_MEMORY_ALLOCATION, Hob.Raw);\r
+    }\r
+    //\r
+    // Free memory allocation HOB iteratively.\r
+    //\r
+    if (MemoryAllocationHob != NULL) {\r
+      FreeMemoryAllocationHob (PrivateData, MemoryAllocationHob);\r
+    }\r
+  }\r
+}\r
+\r
+/**\r
+  Frees memory pages.\r
+\r
+  @param[in] PeiServices        An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.\r
+  @param[in] Memory             The base physical address of the pages to be freed.\r
+  @param[in] Pages              The number of contiguous 4 KB pages to free.\r
+\r
+  @retval EFI_SUCCESS           The requested pages were freed.\r
+  @retval EFI_INVALID_PARAMETER Memory is not a page-aligned address or Pages is invalid.\r
+  @retval EFI_NOT_FOUND         The requested memory pages were not allocated with\r
+                                AllocatePages().\r
+\r
+**/\r
+EFI_STATUS\r
+EFIAPI\r
+PeiFreePages (\r
+  IN CONST EFI_PEI_SERVICES     **PeiServices,\r
+  IN EFI_PHYSICAL_ADDRESS       Memory,\r
+  IN UINTN                      Pages\r
+  )\r
+{\r
+  PEI_CORE_INSTANCE             *PrivateData;\r
+  UINT64                        Bytes;\r
+  UINT64                        Start;\r
+  UINT64                        End;\r
+  EFI_PEI_HOB_POINTERS          Hob;\r
+  EFI_HOB_MEMORY_ALLOCATION     *MemoryAllocationHob;\r
+\r
+  Bytes = LShiftU64 (Pages, EFI_PAGE_SHIFT);\r
+  Start = Memory;\r
+  End = Start + Bytes - 1;\r
+\r
+  if (Pages == 0 || ((Start & EFI_PAGE_MASK) != 0) || (Start >= End)) {\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+\r
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS (PeiServices);\r
+  Hob.Raw     = PrivateData->HobList.Raw;\r
+\r
+  if (Hob.Raw == NULL) {\r
+    //\r
+    // HOB is not initialized yet.\r
+    //\r
+    return EFI_NOT_AVAILABLE_YET;\r
+  }\r
+\r
+  MemoryAllocationHob = NULL;\r
+  Hob.Raw = GetFirstHob (EFI_HOB_TYPE_MEMORY_ALLOCATION);\r
+  while (Hob.Raw != NULL) {\r
+    if ((Hob.MemoryAllocation->AllocDescriptor.MemoryType != EfiConventionalMemory) &&\r
+        (Memory >= Hob.MemoryAllocation->AllocDescriptor.MemoryBaseAddress) &&\r
+        ((Memory + Bytes) <= (Hob.MemoryAllocation->AllocDescriptor.MemoryBaseAddress + Hob.MemoryAllocation->AllocDescriptor.MemoryLength))) {\r
+      //\r
+      // Found the memory allocation HOB that includes the memory pages to be freed.\r
+      //\r
+      MemoryAllocationHob = (EFI_HOB_MEMORY_ALLOCATION *) Hob.Raw;\r
+      break;\r
+    }\r
+    Hob.Raw = GET_NEXT_HOB (Hob);\r
+    Hob.Raw = GetNextHob (EFI_HOB_TYPE_MEMORY_ALLOCATION, Hob.Raw);\r
+  }\r
+\r
+  if (MemoryAllocationHob != NULL) {\r
+    UpdateOrSplitMemoryAllocationHob (MemoryAllocationHob, Memory, Bytes, EfiConventionalMemory);\r
+    FreeMemoryAllocationHob (PrivateData, MemoryAllocationHob);\r
+    return EFI_SUCCESS;\r
+  } else {\r
+    return EFI_NOT_FOUND;\r
+  }\r
+}\r
+\r
+/**\r
+\r
+  Pool allocation service. Before permanent memory is discoveried, the pool will\r
+  be allocated the heap in the temporary memory. Genenrally, the size of heap in temporary\r
+  memory does not exceed to 64K, so the biggest pool size could be allocated is\r
   64K.\r
 \r
   @param PeiServices               An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.\r
@@ -250,7 +787,7 @@ PeiAllocatePool (
   // If some "post-memory" PEIM wishes to allocate larger pool,\r
   // it should use AllocatePages service instead.\r
   //\r
-  \r
+\r
   //\r
   // Generally, the size of heap in temporary memory does not exceed to 64K,\r
   // HobLength is multiples of 8 bytes, so the maxmium size of pool is 0xFFF8 - sizeof (EFI_HOB_MEMORY_POOL)\r
@@ -258,14 +795,14 @@ PeiAllocatePool (
   if (Size > (0xFFF8 - sizeof (EFI_HOB_MEMORY_POOL))) {\r
     return EFI_OUT_OF_RESOURCES;\r
   }\r
-  \r
+\r
   Status = PeiServicesCreateHob (\r
              EFI_HOB_TYPE_MEMORY_POOL,\r
              (UINT16)(sizeof (EFI_HOB_MEMORY_POOL) + Size),\r
              (VOID **)&Hob\r
              );\r
   ASSERT_EFI_ERROR (Status);\r
-  *Buffer = Hob+1;  \r
+  *Buffer = Hob+1;\r
 \r
   return Status;\r
 }\r