ArmPlatformPkg: Moved ARMv7 specific files to a 'Arm' subdirectory

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

/** @file\r
*\r
*  Copyright (c) 2011-2012, 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/PcdLib.h>\r
#include <libfdt.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
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
  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
typedef struct {\r
  UINTN   Base;\r
  UINTN   Size;\r
} FdtRegion;\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
  UINTN                 NewFdtBlobSize;\r
  VOID*                 fdt;\r
  INTN                  err;\r
  INTN                  node;\r
  INTN                  cpu_node;\r
  INTN                  lenp;\r
  CONST VOID*           BootArg;\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
\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
  //\r
  // Allocate memory for the new FDT\r
  //\r
  NewFdtBlobSize = fdt_totalsize((VOID*)(UINTN)(*FdtBlobBase)) + FDT_ADDITIONAL_ENTRIES_SIZE;\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_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
  }\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
  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
    }\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
  // Set Linux CmdLine\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
  // Set Linux Initrd\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
  // Set Physical memory setup if does not exist\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
  // Setup Arm Mpcore Info if it is a multi-core or multi-cluster platforms\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
      }\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
        cpu_node = fdt_subnode_offset(fdt, node, Name);\r
        if (cpu_node < 0) {\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
        }\r
\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
      }\r
      break;\r
    }\r
  }\r
\r
  DEBUG_CODE_BEGIN();\r
    //DebugDumpFdt (fdt);\r
  DEBUG_CODE_END();\r
\r
  *FdtBlobBase = NewFdtBlobBase;\r
  *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(NewFdtBlobBase));\r
  return EFI_SUCCESS;\r
\r
FAIL_NEW_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 EFI_SUCCESS;\r
}\r
\r
\r