- //\r
- // Make sure that Pages plus EFI_SIZE_TO_PAGES (Alignment) does not overflow.\r
- //\r
- ASSERT (Pages <= (MAX_ADDRESS - EFI_SIZE_TO_PAGES (Alignment)));\r
- //\r
- // We would rather waste some memory to save PEI code size.\r
- //\r
- Memory = InternalAllocatePages (MemoryType, Pages + EFI_SIZE_TO_PAGES (Alignment));\r
- if (Alignment == 0) {\r
- AlignmentMask = Alignment;\r
+ if (Alignment > EFI_PAGE_SIZE) {\r
+ //\r
+ // Calculate the total number of pages since alignment is larger than page size.\r
+ //\r
+ AlignmentMask = Alignment - 1;\r
+ RealPages = Pages + EFI_SIZE_TO_PAGES (Alignment);\r
+ //\r
+ // Make sure that Pages plus EFI_SIZE_TO_PAGES (Alignment) does not overflow.\r
+ //\r
+ ASSERT (RealPages > Pages);\r
+\r
+ Status = PeiServicesAllocatePages (MemoryType, RealPages, &Memory);\r
+ if (EFI_ERROR (Status)) {\r
+ return NULL;\r
+ }\r
+ AlignedMemory = ((UINTN) Memory + AlignmentMask) & ~AlignmentMask;\r
+ UnalignedPages = EFI_SIZE_TO_PAGES (AlignedMemory - (UINTN) Memory);\r
+ if (UnalignedPages > 0) {\r
+ //\r
+ // Free first unaligned page(s).\r
+ //\r
+ Status = PeiServicesFreePages (Memory, UnalignedPages);\r
+ ASSERT_EFI_ERROR (Status);\r
+ }\r
+ Memory = AlignedMemory + EFI_PAGES_TO_SIZE (Pages);\r
+ UnalignedPages = RealPages - Pages - UnalignedPages;\r
+ if (UnalignedPages > 0) {\r
+ //\r
+ // Free last unaligned page(s).\r
+ //\r
+ Status = PeiServicesFreePages (Memory, UnalignedPages);\r
+ ASSERT_EFI_ERROR (Status);\r
+ }\r