#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
}\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
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
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
}\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
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
*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