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

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

/** @file\r
*\r
*  Copyright (c) 2011-2013, ARM Limited. All rights reserved.\r
*\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
**/\r
\r
#include <Library/ArmSmcLib.h>\r
#include <Library/PcdLib.h>\r
#include <libfdt.h>\r
\r
#include <IndustryStandard/ArmSmc.h>\r
\r
#include "BdsInternal.h"\r
#include "BdsLinuxLoader.h"\r
\r
#define ALIGN(x, a)     (((x) + ((a) - 1)) & ~((a) - 1))\r
#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
  IN CONST VOID* data,\r
  IN UINTN len\r
  )\r
{\r
  CONST CHAR8 *s = data;\r
  CONST CHAR8 *ss;\r
\r
  // Zero length is not\r
  if (len == 0) {\r
    return 0;\r
  }\r
\r
  // Must terminate with zero\r
  if (s[len - 1] != '\0') {\r
    return 0;\r
  }\r
\r
  ss = s;\r
  while (*s/* && isprint(*s)*/) {\r
    s++;\r
  }\r
\r
  // Not zero, or not done yet\r
  if (*s != '\0' || (s + 1 - ss) < len) {\r
    return 0;\r
  }\r
\r
  return 1;\r
}\r
\r
STATIC\r
VOID\r
PrintData (\r
  IN CONST CHAR8* data,\r
  IN UINTN len\r
  )\r
{\r
  UINTN i;\r
  CONST CHAR8 *p = data;\r
\r
  // No data, don't print\r
  if (len == 0)\r
    return;\r
\r
  if (IsPrintableString (data, len)) {\r
    Print(L" = \"%a\"", (const char *)data);\r
  } else if ((len % 4) == 0) {\r
    Print(L" = <");\r
    for (i = 0; i < len; i += 4) {\r
      Print(L"0x%08x%a", fdt32_to_cpu(GET_CELL(p)),i < (len - 4) ? " " : "");\r
    }\r
    Print(L">");\r
  } else {\r
    Print(L" = [");\r
    for (i = 0; i < len; i++)\r
      Print(L"%02x%a", *p++, i < len - 1 ? " " : "");\r
    Print(L"]");\r
  }\r
}\r
\r
VOID\r
DebugDumpFdt (\r
  IN VOID*                FdtBlob\r
  )\r
{\r
  struct fdt_header *bph;\r
  UINT32 off_dt;\r
  UINT32 off_str;\r
  CONST CHAR8* p_struct;\r
  CONST CHAR8* p_strings;\r
  CONST CHAR8* p;\r
  CONST CHAR8* s;\r
  CONST CHAR8* t;\r
  UINT32 tag;\r
  UINTN sz;\r
  UINTN depth;\r
  UINTN shift;\r
  UINT32 version;\r
\r
  {\r
    // Can 'memreserve' be printed by below code?\r
    INTN num = fdt_num_mem_rsv(FdtBlob);\r
    INTN i, err;\r
    UINT64 addr = 0,size = 0;\r
\r
    for (i = 0; i < num; i++) {\r
      err = fdt_get_mem_rsv(FdtBlob, i, &addr, &size);\r
      if (err) {\r
        DEBUG((EFI_D_ERROR, "Error (%d) : Cannot get memreserve section (%d)\n", err, i));\r
      }\r
      else {\r
        Print(L"/memreserve/ \t0x%lx \t0x%lx;\n",addr,size);\r
      }\r
    }\r
  }\r
\r
  depth = 0;\r
  shift = 4;\r
\r
  bph = FdtBlob;\r
  off_dt = fdt32_to_cpu(bph->off_dt_struct);\r
  off_str = fdt32_to_cpu(bph->off_dt_strings);\r
  p_struct = (CONST CHAR8*)FdtBlob + off_dt;\r
  p_strings = (CONST CHAR8*)FdtBlob + off_str;\r
  version = fdt32_to_cpu(bph->version);\r
\r
  p = p_struct;\r
  while ((tag = fdt32_to_cpu(GET_CELL(p))) != FDT_END) {\r
    if (tag == FDT_BEGIN_NODE) {\r
      s = p;\r
      p = PALIGN(p + AsciiStrLen (s) + 1, 4);\r
\r
      if (*s == '\0')\r
              s = "/";\r
\r
      Print(L"%*s%a {\n", depth * shift, L" ", s);\r
\r
      depth++;\r
      continue;\r
    }\r
\r
    if (tag == FDT_END_NODE) {\r
      depth--;\r
\r
      Print(L"%*s};\n", depth * shift, L" ");\r
      continue;\r
    }\r
\r
    if (tag == FDT_NOP) {\r
      Print(L"%*s// [NOP]\n", depth * shift, L" ");\r
      continue;\r
    }\r
\r
    if (tag != FDT_PROP) {\r
      Print(L"%*s ** Unknown tag 0x%08x\n", depth * shift, L" ", tag);\r
      break;\r
    }\r
    sz = fdt32_to_cpu(GET_CELL(p));\r
    s = p_strings + fdt32_to_cpu(GET_CELL(p));\r
    if (version < 16 && sz >= 8)\r
            p = PALIGN(p, 8);\r
    t = p;\r
\r
    p = PALIGN(p + sz, 4);\r
\r
    Print(L"%*s%a", depth * shift, L" ", s);\r
    PrintData(t, sz);\r
    Print(L";\n");\r
  }\r
}\r
\r
STATIC\r
BOOLEAN\r
IsLinuxReservedRegion (\r
  IN EFI_MEMORY_TYPE MemoryType\r
  )\r
{\r
  switch(MemoryType) {\r
  case EfiRuntimeServicesCode:\r
  case EfiRuntimeServicesData:\r
  case EfiUnusableMemory:\r
  case EfiACPIReclaimMemory:\r
  case EfiACPIMemoryNVS:\r
    return TRUE;\r
  default:\r
    return FALSE;\r
  }\r
}\r
\r
\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
  IN     CONST CHAR8*         CommandLineArguments,\r
  IN     EFI_PHYSICAL_ADDRESS InitrdImage,\r
  IN     UINTN                InitrdImageSize,\r
  IN OUT EFI_PHYSICAL_ADDRESS *FdtBlobBase,\r
  IN OUT UINTN                *FdtBlobSize\r
  )\r
{\r
  EFI_STATUS            Status;\r
  EFI_PHYSICAL_ADDRESS  NewFdtBlobBase;\r
  EFI_PHYSICAL_ADDRESS  NewFdtBlobAllocation;\r
  UINTN                 NewFdtBlobSize;\r
  VOID*                 fdt;\r
  INTN                  err;\r
  INTN                  node;\r
  INTN                  cpu_node;\r
  INTN                  lenp;\r
  CONST VOID*           BootArg;\r
  CONST VOID*           Method;\r
  EFI_PHYSICAL_ADDRESS  InitrdImageStart;\r
  EFI_PHYSICAL_ADDRESS  InitrdImageEnd;\r
  FdtRegion             Region;\r
  UINTN                 Index;\r
  CHAR8                 Name[10];\r
  LIST_ENTRY            ResourceList;\r
  BDS_SYSTEM_MEMORY_RESOURCE  *Resource;\r
  ARM_PROCESSOR_TABLE   *ArmProcessorTable;\r
  ARM_CORE_INFO         *ArmCoreInfoTable;\r
  UINT32                MpId;\r
  UINT32                ClusterId;\r
  UINT32                CoreId;\r
  UINT64                CpuReleaseAddr;\r
  UINTN                 MemoryMapSize;\r
  EFI_MEMORY_DESCRIPTOR *MemoryMap;\r
  UINTN                 MapKey;\r
  UINTN                 DescriptorSize;\r
  UINT32                DescriptorVersion;\r
  UINTN                 Pages;\r
  BOOLEAN               PsciSmcSupported;\r
  UINTN                 OriginalFdtSize;\r
  BOOLEAN               CpusNodeExist;\r
\r
  NewFdtBlobAllocation = 0;\r
\r
  //\r
  // Sanity checks on the original FDT blob.\r
  //\r
  err = fdt_check_header ((VOID*)(UINTN)(*FdtBlobBase));\r
  if (err != 0) {\r
    Print (L"ERROR: Device Tree header not valid (err:%d)\n", err);\r
    return EFI_INVALID_PARAMETER;\r
  }\r
\r
  // The original FDT blob might have been loaded partially.\r
  // Check that it is not the case.\r
  OriginalFdtSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(*FdtBlobBase));\r
  if (OriginalFdtSize > *FdtBlobSize) {\r
    Print (L"ERROR: Incomplete FDT. Only %d/%d bytes have been loaded.\n",\r
           *FdtBlobSize, OriginalFdtSize);\r
    return EFI_INVALID_PARAMETER;\r
  }\r
\r
  //\r
  // Relocate the FDT to its final location.\r
  //\r
  Status = RelocateFdt (*FdtBlobBase, OriginalFdtSize,\r
             &NewFdtBlobBase, &NewFdtBlobSize, &NewFdtBlobAllocation);\r
  if (EFI_ERROR (Status)) {\r
    goto FAIL_RELOCATE_FDT;\r
  }\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
    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
  fdt = (VOID*)(UINTN)NewFdtBlobBase;\r
\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_COMPLETE_FDT;\r
    }\r
  }\r
\r
  DEBUG_CODE_BEGIN();\r
    BootArg = fdt_getprop(fdt, node, "bootargs", &lenp);\r
    if (BootArg != NULL) {\r
      DEBUG((EFI_D_ERROR,"BootArg: %a\n",BootArg));\r
    }\r
  DEBUG_CODE_END();\r
\r
  //\r
  // Set Linux CmdLine\r
  //\r
  if ((CommandLineArguments != NULL) && (AsciiStrLen (CommandLineArguments) > 0)) {\r
    err = fdt_setprop(fdt, node, "bootargs", CommandLineArguments, AsciiStrSize(CommandLineArguments));\r
    if (err) {\r
      DEBUG((EFI_D_ERROR,"Fail to set new 'bootarg' (err:%d)\n",err));\r
    }\r
  }\r
\r
  //\r
  // Set Linux Initrd\r
  //\r
  if (InitrdImageSize != 0) {\r
    InitrdImageStart = cpu_to_fdt64 (InitrdImage);\r
    err = fdt_setprop(fdt, node, "linux,initrd-start", &InitrdImageStart, sizeof(EFI_PHYSICAL_ADDRESS));\r
    if (err) {\r
      DEBUG((EFI_D_ERROR,"Fail to set new 'linux,initrd-start' (err:%d)\n",err));\r
    }\r
    InitrdImageEnd = cpu_to_fdt64 (InitrdImage + InitrdImageSize);\r
    err = fdt_setprop(fdt, node, "linux,initrd-end", &InitrdImageEnd, sizeof(EFI_PHYSICAL_ADDRESS));\r
    if (err) {\r
      DEBUG((EFI_D_ERROR,"Fail to set new 'linux,initrd-start' (err:%d)\n",err));\r
    }\r
  }\r
\r
  //\r
  // Set Physical memory setup if does not exist\r
  //\r
  node = fdt_subnode_offset(fdt, 0, "memory");\r
  if (node < 0) {\r
    // The 'memory' node does not exist, create it\r
    node = fdt_add_subnode(fdt, 0, "memory");\r
    if (node >= 0) {\r
      fdt_setprop_string(fdt, node, "name", "memory");\r
      fdt_setprop_string(fdt, node, "device_type", "memory");\r
\r
      GetSystemMemoryResources (&ResourceList);\r
      Resource = (BDS_SYSTEM_MEMORY_RESOURCE*)ResourceList.ForwardLink;\r
\r
      if (sizeof(UINTN) == sizeof(UINT32)) {\r
        Region.Base = cpu_to_fdt32((UINTN)Resource->PhysicalStart);\r
        Region.Size = cpu_to_fdt32((UINTN)Resource->ResourceLength);\r
      } else {\r
        Region.Base = cpu_to_fdt64((UINTN)Resource->PhysicalStart);\r
        Region.Size = cpu_to_fdt64((UINTN)Resource->ResourceLength);\r
      }\r
\r
      err = fdt_setprop(fdt, node, "reg", &Region, sizeof(Region));\r
      if (err) {\r
        DEBUG((EFI_D_ERROR,"Fail to set new 'memory region' (err:%d)\n",err));\r
      }\r
    }\r
  }\r
\r
  //\r
  // Add the memory regions reserved by the UEFI Firmware\r
  //\r
\r
  // Retrieve the UEFI Memory Map\r
  MemoryMap = NULL;\r
  MemoryMapSize = 0;\r
  Status = gBS->GetMemoryMap (&MemoryMapSize, MemoryMap, &MapKey, &DescriptorSize, &DescriptorVersion);\r
  if (Status == EFI_BUFFER_TOO_SMALL) {\r
    Pages = EFI_SIZE_TO_PAGES (MemoryMapSize) + 1;\r
    MemoryMap = AllocatePages (Pages);\r
    Status = gBS->GetMemoryMap (&MemoryMapSize, MemoryMap, &MapKey, &DescriptorSize, &DescriptorVersion);\r
  }\r
\r
  // Go through the list and add the reserved region to the Device Tree\r
  if (!EFI_ERROR(Status)) {\r
    for (Index = 0; Index < (MemoryMapSize / sizeof(EFI_MEMORY_DESCRIPTOR)); Index++) {\r
      if (IsLinuxReservedRegion ((EFI_MEMORY_TYPE)MemoryMap[Index].Type)) {\r
        DEBUG((DEBUG_VERBOSE, "Reserved region of type %d [0x%X, 0x%X]\n",\r
            MemoryMap[Index].Type,\r
            (UINTN)MemoryMap[Index].PhysicalStart,\r
            (UINTN)(MemoryMap[Index].PhysicalStart + MemoryMap[Index].NumberOfPages * EFI_PAGE_SIZE)));\r
        err = fdt_add_mem_rsv(fdt, MemoryMap[Index].PhysicalStart, MemoryMap[Index].NumberOfPages * EFI_PAGE_SIZE);\r
        if (err != 0) {\r
          Print(L"Warning: Fail to add 'memreserve' (err:%d)\n", err);\r
        }\r
      }\r
    }\r
  }\r
\r
  //\r
  // Setup Arm Mpcore Info if it is a multi-core or multi-cluster platforms\r
  //\r
  for (Index=0; Index < gST->NumberOfTableEntries; Index++) {\r
    // Check for correct GUID type\r
    if (CompareGuid (&gArmMpCoreInfoGuid, &(gST->ConfigurationTable[Index].VendorGuid))) {\r
      MpId = ArmReadMpidr ();\r
      ClusterId = GET_CLUSTER_ID(MpId);\r
      CoreId    = GET_CORE_ID(MpId);\r
\r
      node = fdt_subnode_offset(fdt, 0, "cpus");\r
      if (node < 0) {\r
        // Create the /cpus node\r
        node = fdt_add_subnode(fdt, 0, "cpus");\r
        fdt_setprop_string(fdt, node, "name", "cpus");\r
        fdt_setprop_cell(fdt, node, "#address-cells", 1);\r
        fdt_setprop_cell(fdt, node, "#size-cells", 0);\r
        CpusNodeExist = FALSE;\r
      } else {\r
        CpusNodeExist = TRUE;\r
      }\r
\r
      // Get pointer to ARM processor table\r
      ArmProcessorTable = (ARM_PROCESSOR_TABLE *)gST->ConfigurationTable[Index].VendorTable;\r
      ArmCoreInfoTable = ArmProcessorTable->ArmCpus;\r
\r
      for (Index = 0; Index < ArmProcessorTable->NumberOfEntries; Index++) {\r
        AsciiSPrint (Name, 10, "cpu@%d", Index);\r
\r
        // If the 'cpus' node did not exist then creates the 'cpu' nodes. In case 'cpus' node\r
        // is provided in the original FDT then we do not add any 'cpu' node.\r
        if (!CpusNodeExist) {\r
          cpu_node = fdt_add_subnode(fdt, node, Name);\r
          fdt_setprop_string(fdt, cpu_node, "device-type", "cpu");\r
          fdt_setprop(fdt, cpu_node, "reg", &Index, sizeof(Index));\r
        } else {\r
          cpu_node = fdt_subnode_offset(fdt, node, Name);\r
        }\r
\r
        // If Power State Coordination Interface (PSCI) is not supported then it is expected the secondary\r
        // cores are spinning waiting for the Operating System to release them\r
        if ((PsciSmcSupported == FALSE) && (cpu_node >= 0)) {\r
          // We as the bootloader are responsible for either creating or updating\r
          // these entries. Do not trust the entries in the DT. We only know about\r
          // 'spin-table' type. Do not try to update other types if defined.\r
          Method = fdt_getprop(fdt, cpu_node, "enable-method", &lenp);\r
          if ( (Method == NULL) || (!AsciiStrCmp((CHAR8 *)Method, "spin-table")) ) {\r
            fdt_setprop_string(fdt, cpu_node, "enable-method", "spin-table");\r
            CpuReleaseAddr = cpu_to_fdt64(ArmCoreInfoTable[Index].MailboxSetAddress);\r
            fdt_setprop(fdt, cpu_node, "cpu-release-addr", &CpuReleaseAddr, sizeof(CpuReleaseAddr));\r
\r
            // If it is not the primary core than the cpu should be disabled\r
            if (((ArmCoreInfoTable[Index].ClusterId != ClusterId) || (ArmCoreInfoTable[Index].CoreId != CoreId))) {\r
              fdt_setprop_string(fdt, cpu_node, "status", "disabled");\r
            }\r
          } else {\r
            Print(L"Warning: Unsupported enable-method type for CPU[%d] : %a\n", Index, (CHAR8 *)Method);\r
          }\r
        }\r
      }\r
      break;\r
    }\r
  }\r
\r
  // If the Power State Coordination Interface is supported then we signal it in the Device Tree\r
  if (PsciSmcSupported == TRUE) {\r
    // Before to create it we check if the node is not already defined in the Device Tree\r
    node = fdt_subnode_offset(fdt, 0, "psci");\r
    if (node < 0) {\r
      // The 'psci' node does not exist, create it\r
      node = fdt_add_subnode(fdt, 0, "psci");\r
      if (node < 0) {\r
        DEBUG((EFI_D_ERROR,"Error on creating 'psci' node\n"));\r
        Status = EFI_INVALID_PARAMETER;\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
        fdt_setprop_cell(fdt, node, "cpu_suspend", ARM_SMC_ARM_CPU_SUSPEND);\r
        fdt_setprop_cell(fdt, node, "cpu_off", ARM_SMC_ARM_CPU_OFF);\r
        fdt_setprop_cell(fdt, node, "cpu_on", ARM_SMC_ARM_CPU_ON);\r
        fdt_setprop_cell(fdt, node, "cpu_migrate", ARM_SMC_ARM_MIGRATE);\r
      }\r
    }\r
  }\r
\r
  DEBUG_CODE_BEGIN();\r
    //DebugDumpFdt (fdt);\r
  DEBUG_CODE_END();\r
\r
  // If we succeeded to generate the new Device Tree then free the old Device Tree\r
  gBS->FreePages (*FdtBlobBase, EFI_SIZE_TO_PAGES (*FdtBlobSize));\r
\r
  *FdtBlobBase = NewFdtBlobBase;\r
  *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(NewFdtBlobBase));\r
  return EFI_SUCCESS;\r
\r
FAIL_COMPLETE_FDT:\r
  gBS->FreePages (NewFdtBlobAllocation, EFI_SIZE_TO_PAGES (NewFdtBlobSize));\r
\r
FAIL_RELOCATE_FDT:\r
  *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(*FdtBlobBase));\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