EmbeddedPkg/Library/FdtLib/0002-ArmPkg-BdsLib-Added-support-for-modifying-the-passed.patch

   1 From 9c16a23fba659cdf0ce798aa085a4fb8c3bd47d1 Mon Sep 17 00:00:00 2001
   2 From: Olivier Martin <olivier.martin@arm.com>
   3 Date: Thu, 16 Feb 2012 15:50:59 +0000
   4 Subject: [PATCH 2/3] ArmPkg/BdsLib: Added support for modifying the passed FDT blob
   5
   6 - Add Linux CmdLine if not defined
   7 - Add initrd if not defined
   8 - Add CPU parking address if not defined
   9 - Add System Memory info if not defined
  10 ---
  11  ArmPkg/ArmPkg.dsc                      |    1 +
  12  ArmPkg/Library/BdsLib/BdsInternal.h    |    9 +
  13  ArmPkg/Library/BdsLib/BdsLib.inf       |    3 +
  14  ArmPkg/Library/BdsLib/BdsLinuxFdt.c    |  353 ++++++++++++++++++++++++++++++++
  15  ArmPkg/Library/BdsLib/BdsLinuxLoader.c |    8 +
  16  ArmPkg/Library/BdsLib/BdsLinuxLoader.h |   10 +-
  17  6 files changed, 383 insertions(+), 1 deletions(-)
  18  mode change 100644 => 100755 ArmPkg/ArmPkg.dsc
  19  mode change 100644 => 100755 ArmPkg/Library/BdsLib/BdsInternal.h
  20  mode change 100644 => 100755 ArmPkg/Library/BdsLib/BdsLib.inf
  21  create mode 100755 ArmPkg/Library/BdsLib/BdsLinuxFdt.c
  22
  23 diff --git a/ArmPkg/ArmPkg.dsc b/ArmPkg/ArmPkg.dsc
  24 old mode 100644
  25 new mode 100755
  26 index f4989a6..07c825d
  27 --- a/ArmPkg/ArmPkg.dsc
  28 +++ b/ArmPkg/ArmPkg.dsc
  29 @@ -72,6 +72,7 @@
  30    SerialPortLib|MdePkg/Library/BaseSerialPortLibNull/BaseSerialPortLibNull.inf
  31
  32    BdsLib|ArmPkg/Library/BdsLib/BdsLib.inf
  33 +  FdtLib|EmbeddedPkg/Library/FdtLib/FdtLib.inf
  34
  35    IoLib|MdePkg/Library/BaseIoLibIntrinsic/BaseIoLibIntrinsic.inf
  36
  37 diff --git a/ArmPkg/Library/BdsLib/BdsInternal.h b/ArmPkg/Library/BdsLib/BdsInternal.h
  38 old mode 100644
  39 new mode 100755
  40 index 880d780..80d21b2
  41 --- a/ArmPkg/Library/BdsLib/BdsInternal.h
  42 +++ b/ArmPkg/Library/BdsLib/BdsInternal.h
  43 @@ -103,4 +103,13 @@ PrepareAtagList (
  44    OUT UINT32                *AtagSize
  45    );
  46
  47 +EFI_STATUS
  48 +PrepareFdt (
  49 +  IN     CONST CHAR8*         CommandLineString,
  50 +  IN     EFI_PHYSICAL_ADDRESS InitrdImage,
  51 +  IN     UINTN                InitrdImageSize,
  52 +  IN OUT EFI_PHYSICAL_ADDRESS *FdtBlobBase,
  53 +  IN OUT UINT32               *FdtBlobSize
  54 +  );
  55 +
  56  #endif
  57 diff --git a/ArmPkg/Library/BdsLib/BdsLib.inf b/ArmPkg/Library/BdsLib/BdsLib.inf
  58 old mode 100644
  59 new mode 100755
  60 index 20644f1..b3cab21
  61 --- a/ArmPkg/Library/BdsLib/BdsLib.inf
  62 +++ b/ArmPkg/Library/BdsLib/BdsLib.inf
  63 @@ -27,6 +27,7 @@
  64
  65    BdsLinuxLoader.c
  66    BdsLinuxAtag.c
  67 +  BdsLinuxFdt.c
  68
  69  [Packages]
  70    MdePkg/MdePkg.dec
  71 @@ -41,9 +42,11 @@
  72    HobLib
  73    PerformanceLib
  74    SerialPortLib
  75 +  FdtLib
  76
  77  [Guids]
  78    gEfiFileInfoGuid
  79 +  gArmMpCoreInfoGuid
  80
  81  [Protocols]
  82    gEfiBdsArchProtocolGuid
  83 diff --git a/ArmPkg/Library/BdsLib/BdsLinuxFdt.c b/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
  84 new file mode 100755
  85 index 0000000..5c14b65
  86 --- /dev/null
  87 +++ b/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
  88 @@ -0,0 +1,353 @@
  89 +/** @file
  90 +*
  91 +*  Copyright (c) 2011-2012, ARM Limited. All rights reserved.
  92 +*
  93 +*  This program and the accompanying materials
  94 +*  are licensed and made available under the terms and conditions of the BSD License
  95 +*  which accompanies this distribution.  The full text of the license may be found at
  96 +*  http://opensource.org/licenses/bsd-license.php
  97 +*
  98 +*  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
  99 +*  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
 100 +*
 101 +**/
 102 +
 103 +#include <Library/PcdLib.h>
 104 +#include <libfdt.h>
 105 +
 106 +#include "BdsInternal.h"
 107 +
 108 +#define LINUX_FDT_MAX_OFFSET     (PcdGet32(PcdSystemMemoryBase) + PcdGet32(PcdArmLinuxAtagMaxOffset))
 109 +
 110 +#define ALIGN(x, a)     (((x) + ((a) - 1)) & ~((a) - 1))
 111 +#define PALIGN(p, a)    ((void *)(ALIGN((unsigned long)(p), (a))))
 112 +#define GET_CELL(p)     (p += 4, *((const UINT32 *)(p-4)))
 113 +
 114 +STATIC
 115 +UINTN
 116 +IsPrintableString (
 117 +  IN CONST VOID* data,
 118 +  IN UINTN len
 119 +  )
 120 +{
 121 +  CONST CHAR8 *s = data;
 122 +  CONST CHAR8 *ss;
 123 +
 124 +  // zero length is not
 125 +  if (len == 0) {
 126 +    return 0;
 127 +  }
 128 +
 129 +  // must terminate with zero
 130 +  if (s[len - 1] != '\0') {
 131 +    return 0;
 132 +  }
 133 +
 134 +  ss = s;
 135 +  while (*s/* && isprint(*s)*/) {
 136 +    s++;
 137 +  }
 138 +
 139 +  // not zero, or not done yet
 140 +  if (*s != '\0' || (s + 1 - ss) < len) {
 141 +    return 0;
 142 +  }
 143 +
 144 +  return 1;
 145 +}
 146 +
 147 +STATIC
 148 +VOID
 149 +PrintData (
 150 +  IN CONST CHAR8* data,
 151 +  IN UINTN len
 152 +  )
 153 +{
 154 +  UINTN i;
 155 +  CONST CHAR8 *p = data;
 156 +
 157 +  // no data, don't print
 158 +  if (len == 0)
 159 +    return;
 160 +
 161 +  if (IsPrintableString (data, len)) {
 162 +    Print(L" = \"%a\"", (const char *)data);
 163 +  } else if ((len % 4) == 0) {
 164 +    Print(L" = <");
 165 +    for (i = 0; i < len; i += 4) {
 166 +      Print(L"0x%08x%a", fdt32_to_cpu(GET_CELL(p)),i < (len - 4) ? " " : "");
 167 +    }
 168 +    Print(L">");
 169 +  } else {
 170 +    Print(L" = [");
 171 +    for (i = 0; i < len; i++)
 172 +      Print(L"%02x%a", *p++, i < len - 1 ? " " : "");
 173 +    Print(L"]");
 174 +  }
 175 +}
 176 +
 177 +VOID
 178 +DebugDumpFdt (
 179 +  IN VOID*                FdtBlob
 180 +  )
 181 +{
 182 +  struct fdt_header *bph;
 183 +  UINT32 off_dt;
 184 +  UINT32 off_str;
 185 +  CONST CHAR8* p_struct;
 186 +  CONST CHAR8* p_strings;
 187 +  CONST CHAR8* p;
 188 +  CONST CHAR8* s;
 189 +  CONST CHAR8* t;
 190 +  UINT32 tag;
 191 +  UINTN sz;
 192 +  UINTN depth;
 193 +  UINTN shift;
 194 +  UINT32 version;
 195 +
 196 +  depth = 0;
 197 +  shift = 4;
 198 +
 199 +  bph = FdtBlob;
 200 +  off_dt = fdt32_to_cpu(bph->off_dt_struct);
 201 +  off_str = fdt32_to_cpu(bph->off_dt_strings);
 202 +  p_struct = (CONST CHAR8*)FdtBlob + off_dt;
 203 +  p_strings = (CONST CHAR8*)FdtBlob + off_str;
 204 +  version = fdt32_to_cpu(bph->version);
 205 +
 206 +  p = p_struct;
 207 +  while ((tag = fdt32_to_cpu(GET_CELL(p))) != FDT_END) {
 208 +
 209 +    //printf("tag: 0x%08x (%d)\n", tag, p - p_struct);
 210 +
 211 +    if (tag == FDT_BEGIN_NODE) {
 212 +      s = p;
 213 +      p = PALIGN(p + AsciiStrLen (s) + 1, 4);
 214 +
 215 +      if (*s == '\0')
 216 +              s = "/";
 217 +
 218 +      Print(L"%*s%a {\n", depth * shift, L" ", s);
 219 +
 220 +      depth++;
 221 +      continue;
 222 +    }
 223 +
 224 +    if (tag == FDT_END_NODE) {
 225 +      depth--;
 226 +
 227 +      Print(L"%*s};\n", depth * shift, L" ");
 228 +      continue;
 229 +    }
 230 +
 231 +    if (tag == FDT_NOP) {
 232 +      Print(L"%*s// [NOP]\n", depth * shift, L" ");
 233 +      continue;
 234 +    }
 235 +
 236 +    if (tag != FDT_PROP) {
 237 +      Print(L"%*s ** Unknown tag 0x%08x\n", depth * shift, L" ", tag);
 238 +      break;
 239 +    }
 240 +    sz = fdt32_to_cpu(GET_CELL(p));
 241 +    s = p_strings + fdt32_to_cpu(GET_CELL(p));
 242 +    if (version < 16 && sz >= 8)
 243 +            p = PALIGN(p, 8);
 244 +    t = p;
 245 +
 246 +    p = PALIGN(p + sz, 4);
 247 +
 248 +    Print(L"%*s%a", depth * shift, L" ", s);
 249 +    PrintData(t, sz);
 250 +    Print(L";\n");
 251 +  }
 252 +}
 253 +
 254 +typedef struct {
 255 +  UINTN   Base;
 256 +  UINTN   Size;
 257 +} FdtRegion;
 258 +
 259 +EFI_STATUS
 260 +PrepareFdt (
 261 +  IN     CONST CHAR8*         CommandLineString,
 262 +  IN     EFI_PHYSICAL_ADDRESS InitrdImage,
 263 +  IN     UINTN                InitrdImageSize,
 264 +  IN OUT EFI_PHYSICAL_ADDRESS *FdtBlobBase,
 265 +  IN OUT UINT32               *FdtBlobSize
 266 +  )
 267 +{
 268 +  EFI_STATUS            Status;
 269 +  EFI_PHYSICAL_ADDRESS  NewFdtBlobBase;
 270 +  UINTN                 NewFdtBlobSize;
 271 +  VOID*                 fdt;
 272 +  INTN                  err;
 273 +  INTN                  node;
 274 +  INTN                  cpu_node;
 275 +  INTN                  lenp;
 276 +  CONST VOID*           BootArg;
 277 +  EFI_PHYSICAL_ADDRESS  InitrdImageStart;
 278 +  EFI_PHYSICAL_ADDRESS  InitrdImageEnd;
 279 +  FdtRegion             Region;
 280 +  UINTN                 Index;
 281 +  CHAR8                 Name[10];
 282 +  LIST_ENTRY            ResourceList;
 283 +  BDS_SYSTEM_MEMORY_RESOURCE  *Resource;
 284 +  ARM_PROCESSOR_TABLE   *ArmProcessorTable;
 285 +  ARM_CORE_INFO         *ArmCoreInfoTable;
 286 +  UINT32                MpId;
 287 +  UINT32                ClusterId;
 288 +  UINT32                CoreId;
 289 +  UINT64                CpuReleaseAddr;
 290 +
 291 +  err = fdt_check_header ((VOID*)(UINTN)(*FdtBlobBase));
 292 +  if (err != 0) {
 293 +    Print (L"ERROR: Device Tree header not valid (err:%d)\n", err);
 294 +    return EFI_INVALID_PARAMETER;
 295 +  }
 296 +
 297 +  // Allocate memory for the new FDT
 298 +  NewFdtBlobBase = LINUX_FDT_MAX_OFFSET;
 299 +  NewFdtBlobSize = *FdtBlobSize + FDT_ADDITIONAL_ENTRIES_SIZE;
 300 +  Status = gBS->AllocatePages (AllocateMaxAddress, EfiBootServicesData, EFI_SIZE_TO_PAGES(NewFdtBlobSize), &NewFdtBlobBase);
 301 +  if (EFI_ERROR(Status)) {
 302 +    DEBUG ((EFI_D_WARN, "Warning: Failed to allocate Fdt below 0x%lX (%r). The Fdt will be allocated somewhere else in System Memory.\n",NewFdtBlobBase,Status));
 303 +    Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData, EFI_SIZE_TO_PAGES(NewFdtBlobSize), &NewFdtBlobBase);
 304 +    ASSERT_EFI_ERROR(Status);
 305 +    goto FAIL_NEW_FDT;
 306 +  }
 307 +
 308 +  // Load the Original FDT tree into the new region
 309 +  fdt = (VOID*)(UINTN)NewFdtBlobBase;
 310 +  err = fdt_open_into((VOID*)(UINTN)(*FdtBlobBase), fdt, NewFdtBlobSize);
 311 +  if (err) {
 312 +    DEBUG((EFI_D_ERROR, "fdt_open_into(): %a\n", fdt_strerror(err)));
 313 +    Status = EFI_INVALID_PARAMETER;
 314 +    goto FAIL_NEW_FDT;
 315 +  }
 316 +
 317 +  DEBUG_CODE_BEGIN();
 318 +    DebugDumpFdt (fdt);
 319 +  DEBUG_CODE_END();
 320 +
 321 +  node = fdt_subnode_offset(fdt, 0, "chosen");
 322 +  if (node < 0) {
 323 +    // The 'chosen' node does not exist, create it
 324 +    node = fdt_add_subnode(fdt, 0, "chosen");
 325 +    if (node < 0) {
 326 +      DEBUG((EFI_D_ERROR,"Error on finding 'chosen' node\n"));
 327 +      Status = EFI_INVALID_PARAMETER;
 328 +      goto FAIL_NEW_FDT;
 329 +    }
 330 +  }
 331 +
 332 +  DEBUG_CODE_BEGIN();
 333 +    BootArg = fdt_getprop(fdt, node, "bootargs", &lenp);
 334 +    if (BootArg != NULL) {
 335 +      DEBUG((EFI_D_ERROR,"BootArg: %a\n",BootArg));
 336 +    }
 337 +  DEBUG_CODE_END();
 338 +
 339 +  // Set Linux CmdLine
 340 +  if ((CommandLineString != NULL) && (AsciiStrLen (CommandLineString) > 0)) {
 341 +    err = fdt_setprop(fdt, node, "bootargs", CommandLineString, AsciiStrSize(CommandLineString));
 342 +    if (err) {
 343 +      DEBUG((EFI_D_ERROR,"Fail to set new 'bootarg' (err:%d)\n",err));
 344 +    }
 345 +  }
 346 +
 347 +  // Set Linux Initrd
 348 +  if (InitrdImageSize != 0) {
 349 +    InitrdImageStart = cpu_to_fdt64 (InitrdImage);
 350 +    err = fdt_setprop(fdt, node, "linux,initrd-start", &InitrdImageStart, sizeof(EFI_PHYSICAL_ADDRESS));
 351 +    if (err) {
 352 +      DEBUG((EFI_D_ERROR,"Fail to set new 'linux,initrd-start' (err:%d)\n",err));
 353 +    }
 354 +    InitrdImageEnd = cpu_to_fdt64 (InitrdImage + InitrdImageSize);
 355 +    err = fdt_setprop(fdt, node, "linux,initrd-end", &InitrdImageEnd, sizeof(EFI_PHYSICAL_ADDRESS));
 356 +    if (err) {
 357 +      DEBUG((EFI_D_ERROR,"Fail to set new 'linux,initrd-start' (err:%d)\n",err));
 358 +    }
 359 +  }
 360 +
 361 +  // Set Physical memory setup if does not exist
 362 +  node = fdt_subnode_offset(fdt, 0, "memory");
 363 +  if (node < 0) {
 364 +    // The 'chosen' node does not exist, create it
 365 +    node = fdt_add_subnode(fdt, 0, "memory");
 366 +    if (node >= 0) {
 367 +      fdt_setprop_string(fdt, node, "name", "memory");
 368 +      fdt_setprop_string(fdt, node, "device_type", "memory");
 369 +
 370 +      GetSystemMemoryResources (&ResourceList);
 371 +      Resource = (BDS_SYSTEM_MEMORY_RESOURCE*)ResourceList.ForwardLink;
 372 +
 373 +      if (sizeof(UINTN) == sizeof(UINT32)) {
 374 +        Region.Base = cpu_to_fdt32((UINTN)Resource->PhysicalStart);
 375 +        Region.Size = cpu_to_fdt32((UINTN)Resource->ResourceLength);
 376 +      } else {
 377 +        Region.Base = cpu_to_fdt64((UINTN)Resource->PhysicalStart);
 378 +        Region.Size = cpu_to_fdt64((UINTN)Resource->ResourceLength);
 379 +      }
 380 +
 381 +      err = fdt_setprop(fdt, node, "reg", &Region, sizeof(Region));
 382 +      if (err) {
 383 +        DEBUG((EFI_D_ERROR,"Fail to set new 'memory region' (err:%d)\n",err));
 384 +      }
 385 +    }
 386 +  }
 387 +
 388 +  // Setup Arm Mpcore Info if it is a multi-core or multi-cluster platforms
 389 +  for (Index=0; Index < gST->NumberOfTableEntries; Index++) {
 390 +    // Check for correct GUID type
 391 +    if (CompareGuid (&gArmMpCoreInfoGuid, &(gST->ConfigurationTable[Index].VendorGuid))) {
 392 +      MpId = ArmReadMpidr ();
 393 +      ClusterId = GET_CLUSTER_ID(MpId);
 394 +      CoreId    = GET_CORE_ID(MpId);
 395 +
 396 +      node = fdt_subnode_offset(fdt, 0, "cpus");
 397 +      if (node < 0) {
 398 +        // Create the /cpus node
 399 +        node = fdt_add_subnode(fdt, 0, "cpus");
 400 +        fdt_setprop_string(fdt, node, "name", "cpus");
 401 +        fdt_setprop_cell(fdt, node, "#address-cells", 1);
 402 +        fdt_setprop_cell(fdt, node, "#size-cells", 0);
 403 +      }
 404 +
 405 +      // Get pointer to ARM processor table
 406 +      ArmProcessorTable = (ARM_PROCESSOR_TABLE *)gST->ConfigurationTable[Index].VendorTable;
 407 +      ArmCoreInfoTable = ArmProcessorTable->ArmCpus;
 408 +
 409 +      for (Index = 0; Index < ArmProcessorTable->NumberOfEntries; Index++) {
 410 +        if (((ArmCoreInfoTable[Index].ClusterId != ClusterId) || (ArmCoreInfoTable[Index].CoreId != CoreId))) {
 411 +          AsciiSPrint (Name, 10, "cpu@%d", Index);
 412 +          cpu_node = fdt_subnode_offset(fdt, node, Name);
 413 +          if (cpu_node < 0) {
 414 +            cpu_node = fdt_add_subnode(fdt, node, Name);
 415 +          }
 416 +          fdt_setprop_string(fdt, cpu_node, "device-type", "cpu");
 417 +          fdt_setprop_string(fdt, cpu_node, "enable-method", "spin-table");
 418 +          fdt_setprop_string(fdt, cpu_node, "status", "disabled");
 419 +          CpuReleaseAddr = cpu_to_fdt64(ArmCoreInfoTable[Index].MailboxSetAddress);
 420 +          fdt_setprop(fdt, cpu_node, "cpu-release-addr", &CpuReleaseAddr, sizeof(CpuReleaseAddr));
 421 +        }
 422 +      }
 423 +      break;
 424 +    }
 425 +  }
 426 +
 427 +  DEBUG_CODE_BEGIN();
 428 +    DebugDumpFdt (fdt);
 429 +  DEBUG_CODE_END();
 430 +
 431 +  *FdtBlobBase = NewFdtBlobBase;
 432 +  *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(NewFdtBlobBase));;
 433 +  return EFI_SUCCESS;
 434 +
 435 +FAIL_NEW_FDT:
 436 +  *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(*FdtBlobBase));
 437 +  // Return success even if we failed to update the FDT blob. The original one is still valid.
 438 +  return EFI_SUCCESS;
 439 +}
 440 +
 441 +
 442 diff --git a/ArmPkg/Library/BdsLib/BdsLinuxLoader.c b/ArmPkg/Library/BdsLib/BdsLinuxLoader.c
 443 index 12a8862..82fa811 100755
 444 --- a/ArmPkg/Library/BdsLib/BdsLinuxLoader.c
 445 +++ b/ArmPkg/Library/BdsLib/BdsLinuxLoader.c
 446 @@ -241,6 +241,14 @@ BdsBootLinuxFdt (
 447      Print (L"ERROR: Did not find Device Tree blob.\n");
 448      return Status;
 449    }
 450 +
 451 +  // By setting address=0 we leave the memory allocation to the function
 452 +  Status = PrepareFdt (Arguments, InitrdImage, InitrdImageSize, &KernelParamsAddress, &KernelParamsSize);
 453 +  if (EFI_ERROR(Status)) {
 454 +    Print(L"ERROR: Can not load Linux kernel with Device Tree. Status=0x%X\n", Status);
 455 +    return Status;
 456 +  }
 457 +
 458    return StartLinux (LinuxImage, LinuxImageSize, KernelParamsAddress, KernelParamsSize, FdtMachineType);
 459  }
 460
 461 diff --git a/ArmPkg/Library/BdsLib/BdsLinuxLoader.h b/ArmPkg/Library/BdsLib/BdsLinuxLoader.h
 462 index 8d58ce1..9e45e03 100755
 463 --- a/ArmPkg/Library/BdsLib/BdsLinuxLoader.h
 464 +++ b/ArmPkg/Library/BdsLib/BdsLinuxLoader.h
 465 @@ -15,12 +15,20 @@
 466  #ifndef __BDSLINUXLOADER_H
 467  #define __BDSLINUXLOADER_H
 468
 469 +#include <Guid/ArmMpCoreInfo.h>
 470 +
 471  #define LINUX_UIMAGE_SIGNATURE    0x56190527
 472
 473  #define LINUX_ATAG_MAX_OFFSET     (PcdGet32(PcdSystemMemoryBase) + PcdGet32(PcdArmLinuxAtagMaxOffset))
 474  #define LINUX_KERNEL_MAX_OFFSET   (PcdGet32(PcdSystemMemoryBase) + PcdGet32(PcdArmLinuxKernelMaxOffset))
 475
 476 -#define ATAG_MAX_SIZE       0x3000
 477 +// Size allocated for the Atag list
 478 +#define ATAG_MAX_SIZE                   0x3000
 479 +
 480 +// Additional size that could be used for FDT entries added by the UEFI OS Loader
 481 +// Estimation based on: EDID (300bytes) + bootargs (200bytes) + initrd region (20bytes)
 482 +//                      + system memory region (20bytes) + mp_core entries (200 bytes)
 483 +#define FDT_ADDITIONAL_ENTRIES_SIZE     0x300
 484
 485  /* ATAG : list of possible tags */
 486  #define ATAG_NONE            0x00000000
 487 --
 488 1.7.0.4
 489