ArmPkg/BdsLib: Added support for FDT alignment through PcdArmLinuxFdtAlignment

[mirror_edk2.git] / ArmPkg / Library / BdsLib / BdsLinuxFdt.c

diff --git a/ArmPkg/Library/BdsLib/BdsLinuxFdt.c b/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
index b6d08944abd08b3b03802ad97a0fbaf4ca3b9f42..43daf5dd2a277d471558e290c663d0cc41702698 100644 (file)
--- a/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
+++ b/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
@@ -214,7 +214,9 @@ PrepareFdt (
 {\r
   EFI_STATUS            Status;\r
   EFI_PHYSICAL_ADDRESS  NewFdtBlobBase;\r
+  EFI_PHYSICAL_ADDRESS  NewFdtBlobAllocation;\r
   UINTN                 NewFdtBlobSize;\r
+  UINT32                FdtAlignment;\r
   VOID*                 fdt;\r
   INTN                  err;\r
   INTN                  node;\r
@@ -295,9 +297,15 @@ PrepareFdt (
   //\r
   NewFdtBlobSize = OriginalFdtSize + FDT_ADDITIONAL_ENTRIES_SIZE;\r
 \r
+  // If FDT load address needs to be aligned, allocate more space.\r
+  FdtAlignment = PcdGet32 (PcdArmLinuxFdtAlignment);\r
+  if (FdtAlignment != 0) {\r
+    NewFdtBlobSize += FdtAlignment;\r
+  }\r
+\r
   // Try below a watermark address\r
   Status = EFI_NOT_FOUND;\r
-  if (PcdGet32(PcdArmLinuxFdtMaxOffset) != 0) {\r
+  if (PcdGet32 (PcdArmLinuxFdtMaxOffset) != 0) {\r
     NewFdtBlobBase = LINUX_FDT_MAX_OFFSET;\r
     Status = gBS->AllocatePages (AllocateMaxAddress, EfiBootServicesData, EFI_SIZE_TO_PAGES(NewFdtBlobSize), &NewFdtBlobBase);\r
     if (EFI_ERROR(Status)) {\r
@@ -316,6 +324,11 @@ PrepareFdt (
     }\r
   }\r
 \r
+  NewFdtBlobAllocation = NewFdtBlobBase;\r
+  if (FdtAlignment != 0) {\r
+    NewFdtBlobBase = ALIGN (NewFdtBlobBase, FdtAlignment);\r
+  }\r
+\r
   // Load the Original FDT tree into the new region\r
   fdt = (VOID*)(UINTN)NewFdtBlobBase;\r
   err = fdt_open_into((VOID*)(UINTN)(*FdtBlobBase), fdt, NewFdtBlobSize);\r
@@ -530,7 +543,7 @@ PrepareFdt (
   return EFI_SUCCESS;\r
 \r
 FAIL_NEW_FDT:\r
-  gBS->FreePages (NewFdtBlobBase, EFI_SIZE_TO_PAGES (NewFdtBlobSize));\r
+  gBS->FreePages (NewFdtBlobAllocation, EFI_SIZE_TO_PAGES (NewFdtBlobSize));\r
 \r
 FAIL_ALLOCATE_NEW_FDT:\r
   *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(*FdtBlobBase));\r