ArmPkg/BdsLinuxFdt.c: Split PrepareFdt() into several functions.

PrepareFdt() function was getting pretty fat and big functions are more
complex to understand and find the information one is looking for.

This patch extracts some code from PrepareFdt() function and put it
in some new functions.

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Olivier Martin <olivier.martin@arm.com
git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@14349 6f19259b-4bc3-4df7-8a09-765794883524

diff --git a/ArmPkg/Library/BdsLib/BdsLinuxFdt.c b/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
index 43daf5dd2a277d471558e290c663d0cc41702698..380a666d0f2a0a321d9d49cd3597581c47efa415 100644 (file)
--- a/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
+++ b/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
@@ -25,6 +25,12 @@
 #define PALIGN(p, a)    ((void *)(ALIGN((unsigned long)(p), (a))))\r
 #define GET_CELL(p)     (p += 4, *((const UINT32 *)(p-4)))\r
 \r
+typedef struct {\r
+  UINTN   Base;\r
+  UINTN   Size;\r
+} FdtRegion;\r
+\r
+\r
 STATIC\r
 UINTN\r
 IsPrintableString (\r
@@ -198,10 +204,110 @@ IsLinuxReservedRegion (
 }\r
 \r
 \r
-typedef struct {\r
-  UINTN   Base;\r
-  UINTN   Size;\r
-} FdtRegion;\r
+STATIC\r
+BOOLEAN\r
+IsPsciSmcSupported (\r
+  VOID\r
+  )\r
+{\r
+  BOOLEAN               PsciSmcSupported;\r
+  UINTN                 Rx;\r
+\r
+  PsciSmcSupported = FALSE;\r
+\r
+  // Check the SMC response to the Presence SMC\r
+  Rx = ARM_SMC_ID_PRESENCE;\r
+  ArmCallSmc (&Rx);\r
+  if (Rx == 1) {\r
+    // Check the SMC UID\r
+    Rx = ARM_SMC_ID_UID;\r
+    ArmCallSmc (&Rx);\r
+    if (Rx == ARM_TRUSTZONE_UID_4LETTERID) {\r
+      Rx = ARM_SMC_ID_UID + 1;\r
+      ArmCallSmc (&Rx);\r
+      if (Rx == ARM_TRUSTZONE_ARM_UID) {\r
+        PsciSmcSupported = TRUE;\r
+      }\r
+    }\r
+  }\r
+\r
+  return PsciSmcSupported;\r
+}\r
+\r
+\r
+/**\r
+** Relocate the FDT blob to a more appropriate location for the Linux kernel.\r
+** This function will allocate memory for the relocated FDT blob.\r
+**\r
+** @retval EFI_SUCCESS on success.\r
+** @retval EFI_OUT_OF_RESOURCES or EFI_INVALID_PARAMETER on failure.\r
+*/\r
+STATIC\r
+EFI_STATUS\r
+RelocateFdt (\r
+  EFI_PHYSICAL_ADDRESS   OriginalFdt,\r
+  UINTN                  OriginalFdtSize,\r
+  EFI_PHYSICAL_ADDRESS   *RelocatedFdt,\r
+  UINTN                  *RelocatedFdtSize,\r
+  EFI_PHYSICAL_ADDRESS   *RelocatedFdtAlloc\r
+  )\r
+{\r
+  EFI_STATUS            Status;\r
+  INTN                  Error;\r
+  UINT32                FdtAlignment;\r
+\r
+  *RelocatedFdtSize = 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
+    *RelocatedFdtSize += FdtAlignment;\r
+  }\r
+\r
+  // Try below a watermark address.\r
+  Status = EFI_NOT_FOUND;\r
+  if (PcdGet32 (PcdArmLinuxFdtMaxOffset) != 0) {\r
+    *RelocatedFdt = LINUX_FDT_MAX_OFFSET;\r
+    Status = gBS->AllocatePages (AllocateMaxAddress, EfiBootServicesData,\r
+                    EFI_SIZE_TO_PAGES (*RelocatedFdtSize), RelocatedFdt);\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((EFI_D_WARN, "Warning: Failed to load FDT below address 0x%lX (%r). Will try again at a random address anywhere.\n", *RelocatedFdt, Status));\r
+    }\r
+  }\r
+\r
+  // Try anywhere there is available space.\r
+  if (EFI_ERROR (Status)) {\r
+    Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData,\r
+                    EFI_SIZE_TO_PAGES (*RelocatedFdtSize), RelocatedFdt);\r
+    if (EFI_ERROR (Status)) {\r
+      ASSERT_EFI_ERROR (Status);\r
+      return EFI_OUT_OF_RESOURCES;\r
+    } else {\r
+      DEBUG ((EFI_D_WARN, "WARNING: Loaded FDT at random address 0x%lX.\nWARNING: There is a risk of accidental overwriting by other code/data.\n", *RelocatedFdt));\r
+    }\r
+  }\r
+\r
+  *RelocatedFdtAlloc = *RelocatedFdt;\r
+  if (FdtAlignment != 0) {\r
+    *RelocatedFdt = ALIGN (*RelocatedFdt, FdtAlignment);\r
+  }\r
+\r
+  // Load the Original FDT tree into the new region\r
+  Error = fdt_open_into ((VOID*)(UINTN) OriginalFdt,\r
+            (VOID*)(UINTN)(*RelocatedFdt), *RelocatedFdtSize);\r
+  if (Error) {\r
+    DEBUG ((EFI_D_ERROR, "fdt_open_into(): %a\n", fdt_strerror (Error)));\r
+    gBS->FreePages (*RelocatedFdtAlloc, EFI_SIZE_TO_PAGES (*RelocatedFdtSize));\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+\r
+  DEBUG_CODE_BEGIN();\r
+    //DebugDumpFdt (fdt);\r
+  DEBUG_CODE_END();\r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
 \r
 EFI_STATUS\r
 PrepareFdt (\r
@@ -216,7 +322,6 @@ PrepareFdt (
   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
@@ -244,35 +349,10 @@ PrepareFdt (
   UINT32                DescriptorVersion;\r
   UINTN                 Pages;\r
   BOOLEAN               PsciSmcSupported;\r
-  UINTN                 Rx;\r
   UINTN                 OriginalFdtSize;\r
   BOOLEAN               CpusNodeExist;\r
 \r
-  //\r
-  // Ensure the Power State Coordination Interface (PSCI) SMCs are there if supported\r
-  //\r
-  PsciSmcSupported = FALSE;\r
-  if (FeaturePcdGet (PcdArmPsciSupport) == TRUE) {\r
-    // Check the SMC response to the Presence SMC\r
-    Rx   = ARM_SMC_ID_PRESENCE;\r
-    ArmCallSmc (&Rx);\r
-    if (Rx == 1) {\r
-      // Check the SMC UID\r
-      Rx   = ARM_SMC_ID_UID;\r
-      ArmCallSmc (&Rx);\r
-      if (Rx == ARM_TRUSTZONE_UID_4LETTERID) {\r
-        Rx   = ARM_SMC_ID_UID + 1;\r
-        ArmCallSmc (&Rx);\r
-        if (Rx == ARM_TRUSTZONE_ARM_UID) {\r
-          PsciSmcSupported = TRUE;\r
-        }\r
-      }\r
-      if (PsciSmcSupported == FALSE) {\r
-        DEBUG((EFI_D_ERROR,"Warning: The Power State Coordination Interface (PSCI) is not supported"\r
-                           "by your platform Trusted Firmware.\n"));\r
-      }\r
-    }\r
-  }\r
+  NewFdtBlobAllocation = 0;\r
 \r
   //\r
   // Sanity checks on the original FDT blob.\r
@@ -293,63 +373,35 @@ PrepareFdt (
   }\r
 \r
   //\r
-  // Allocate memory for the new FDT\r
+  // Relocate the FDT to its final location.\r
   //\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
+  Status = RelocateFdt (*FdtBlobBase, OriginalFdtSize,\r
+             &NewFdtBlobBase, &NewFdtBlobSize, &NewFdtBlobAllocation);\r
+  if (EFI_ERROR (Status)) {\r
+    goto FAIL_RELOCATE_FDT;\r
   }\r
 \r
-  // Try below a watermark address\r
-  Status = EFI_NOT_FOUND;\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
-      DEBUG ((EFI_D_WARN, "Warning: Failed to load FDT below address 0x%lX (%r). Will try again at a random address anywhere.\n", NewFdtBlobBase, Status));\r
-    }\r
-  }\r
-\r
-  // Try anywhere there is available space\r
-  if (EFI_ERROR(Status)) {\r
-    Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData, EFI_SIZE_TO_PAGES(NewFdtBlobSize), &NewFdtBlobBase);\r
-    if (EFI_ERROR(Status)) {\r
-      ASSERT_EFI_ERROR(Status);\r
-      goto FAIL_ALLOCATE_NEW_FDT;\r
-    } else {\r
-      DEBUG ((EFI_D_WARN, "WARNING: Loaded FDT at random address 0x%lX.\nWARNING: There is a risk of accidental overwriting by other code/data.\n", NewFdtBlobBase));\r
+  //\r
+  // Ensure the Power State Coordination Interface (PSCI) SMCs are there if supported\r
+  //\r
+  PsciSmcSupported = FALSE;\r
+  if (FeaturePcdGet (PcdArmPsciSupport) == TRUE) {\r
+    PsciSmcSupported = IsPsciSmcSupported();\r
+    if (PsciSmcSupported == FALSE) {\r
+      DEBUG ((EFI_D_ERROR, "Warning: The Power State Coordination Interface (PSCI) is not supported by your platform Trusted Firmware.\n"));\r
     }\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
-  if (err) {\r
-    DEBUG((EFI_D_ERROR, "fdt_open_into(): %a\n", fdt_strerror(err)));\r
-    Status = EFI_INVALID_PARAMETER;\r
-    goto FAIL_NEW_FDT;\r
-  }\r
 \r
-  DEBUG_CODE_BEGIN();\r
-    //DebugDumpFdt (fdt);\r
-  DEBUG_CODE_END();\r
-\r
-  node = fdt_subnode_offset(fdt, 0, "chosen");\r
+  node = fdt_subnode_offset (fdt, 0, "chosen");\r
   if (node < 0) {\r
     // The 'chosen' node does not exist, create it\r
     node = fdt_add_subnode(fdt, 0, "chosen");\r
     if (node < 0) {\r
       DEBUG((EFI_D_ERROR,"Error on finding 'chosen' node\n"));\r
       Status = EFI_INVALID_PARAMETER;\r
-      goto FAIL_NEW_FDT;\r
+      goto FAIL_COMPLETE_FDT;\r
     }\r
   }\r
 \r
@@ -519,7 +571,7 @@ PrepareFdt (
       if (node < 0) {\r
         DEBUG((EFI_D_ERROR,"Error on creating 'psci' node\n"));\r
         Status = EFI_INVALID_PARAMETER;\r
-        goto FAIL_NEW_FDT;\r
+        goto FAIL_COMPLETE_FDT;\r
       } else {\r
         fdt_setprop_string(fdt, node, "compatible", "arm,psci");\r
         fdt_setprop_string(fdt, node, "method", "smc");\r
@@ -542,13 +594,12 @@ PrepareFdt (
   *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(NewFdtBlobBase));\r
   return EFI_SUCCESS;\r
 \r
-FAIL_NEW_FDT:\r
+FAIL_COMPLETE_FDT:\r
   gBS->FreePages (NewFdtBlobAllocation, EFI_SIZE_TO_PAGES (NewFdtBlobSize));\r
 \r
-FAIL_ALLOCATE_NEW_FDT:\r
+FAIL_RELOCATE_FDT:\r
   *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(*FdtBlobBase));\r
-  // Return success even if we failed to update the FDT blob. The original one is still valid.\r
+  // Return success even if we failed to update the FDT blob.\r
+  // The original one is still valid.\r
   return EFI_SUCCESS;\r
 }\r
-\r
-\r