ArmPkg/Library/BdsLib/BdsLinuxFdt.c

   1 /** @file
   2 *
   3 *  Copyright (c) 2011-2012, ARM Limited. All rights reserved.
   4 *
   5 *  This program and the accompanying materials
   6 *  are licensed and made available under the terms and conditions of the BSD License
   7 *  which accompanies this distribution.  The full text of the license may be found at
   8 *  http://opensource.org/licenses/bsd-license.php
   9 *
  10 *  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
  11 *  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
  12 *
  13 **/
  14
  15 #include <Library/PcdLib.h>
  16 #include <libfdt.h>
  17
  18 #include "BdsInternal.h"
  19 #include "BdsLinuxLoader.h"
  20
  21 #define ALIGN(x, a)     (((x) + ((a) - 1)) & ~((a) - 1))
  22 #define PALIGN(p, a)    ((void *)(ALIGN((unsigned long)(p), (a))))
  23 #define GET_CELL(p)     (p += 4, *((const UINT32 *)(p-4)))
  24
  25 STATIC
  26 UINTN
  27 IsPrintableString (
  28   IN CONST VOID* data,
  29   IN UINTN len
  30   )
  31 {
  32   CONST CHAR8 *s = data;
  33   CONST CHAR8 *ss;
  34
  35   // Zero length is not
  36   if (len == 0) {
  37     return 0;
  38   }
  39
  40   // Must terminate with zero
  41   if (s[len - 1] != '\0') {
  42     return 0;
  43   }
  44
  45   ss = s;
  46   while (*s/* && isprint(*s)*/) {
  47     s++;
  48   }
  49
  50   // Not zero, or not done yet
  51   if (*s != '\0' || (s + 1 - ss) < len) {
  52     return 0;
  53   }
  54
  55   return 1;
  56 }
  57
  58 STATIC
  59 VOID
  60 PrintData (
  61   IN CONST CHAR8* data,
  62   IN UINTN len
  63   )
  64 {
  65   UINTN i;
  66   CONST CHAR8 *p = data;
  67
  68   // No data, don't print
  69   if (len == 0)
  70     return;
  71
  72   if (IsPrintableString (data, len)) {
  73     Print(L" = \"%a\"", (const char *)data);
  74   } else if ((len % 4) == 0) {
  75     Print(L" = <");
  76     for (i = 0; i < len; i += 4) {
  77       Print(L"0x%08x%a", fdt32_to_cpu(GET_CELL(p)),i < (len - 4) ? " " : "");
  78     }
  79     Print(L">");
  80   } else {
  81     Print(L" = [");
  82     for (i = 0; i < len; i++)
  83       Print(L"%02x%a", *p++, i < len - 1 ? " " : "");
  84     Print(L"]");
  85   }
  86 }
  87
  88 VOID
  89 DebugDumpFdt (
  90   IN VOID*                FdtBlob
  91   )
  92 {
  93   struct fdt_header *bph;
  94   UINT32 off_dt;
  95   UINT32 off_str;
  96   CONST CHAR8* p_struct;
  97   CONST CHAR8* p_strings;
  98   CONST CHAR8* p;
  99   CONST CHAR8* s;
 100   CONST CHAR8* t;
 101   UINT32 tag;
 102   UINTN sz;
 103   UINTN depth;
 104   UINTN shift;
 105   UINT32 version;
 106
 107   depth = 0;
 108   shift = 4;
 109
 110   bph = FdtBlob;
 111   off_dt = fdt32_to_cpu(bph->off_dt_struct);
 112   off_str = fdt32_to_cpu(bph->off_dt_strings);
 113   p_struct = (CONST CHAR8*)FdtBlob + off_dt;
 114   p_strings = (CONST CHAR8*)FdtBlob + off_str;
 115   version = fdt32_to_cpu(bph->version);
 116
 117   p = p_struct;
 118   while ((tag = fdt32_to_cpu(GET_CELL(p))) != FDT_END) {
 119     if (tag == FDT_BEGIN_NODE) {
 120       s = p;
 121       p = PALIGN(p + AsciiStrLen (s) + 1, 4);
 122
 123       if (*s == '\0')
 124               s = "/";
 125
 126       Print(L"%*s%a {\n", depth * shift, L" ", s);
 127
 128       depth++;
 129       continue;
 130     }
 131
 132     if (tag == FDT_END_NODE) {
 133       depth--;
 134
 135       Print(L"%*s};\n", depth * shift, L" ");
 136       continue;
 137     }
 138
 139     if (tag == FDT_NOP) {
 140       Print(L"%*s// [NOP]\n", depth * shift, L" ");
 141       continue;
 142     }
 143
 144     if (tag != FDT_PROP) {
 145       Print(L"%*s ** Unknown tag 0x%08x\n", depth * shift, L" ", tag);
 146       break;
 147     }
 148     sz = fdt32_to_cpu(GET_CELL(p));
 149     s = p_strings + fdt32_to_cpu(GET_CELL(p));
 150     if (version < 16 && sz >= 8)
 151             p = PALIGN(p, 8);
 152     t = p;
 153
 154     p = PALIGN(p + sz, 4);
 155
 156     Print(L"%*s%a", depth * shift, L" ", s);
 157     PrintData(t, sz);
 158     Print(L";\n");
 159   }
 160 }
 161
 162 typedef struct {
 163   UINTN   Base;
 164   UINTN   Size;
 165 } FdtRegion;
 166
 167 EFI_STATUS
 168 PrepareFdt (
 169   IN     CONST CHAR8*         CommandLineArguments,
 170   IN     EFI_PHYSICAL_ADDRESS InitrdImage,
 171   IN     UINTN                InitrdImageSize,
 172   IN OUT EFI_PHYSICAL_ADDRESS *FdtBlobBase,
 173   IN OUT UINT32               *FdtBlobSize
 174   )
 175 {
 176   EFI_STATUS            Status;
 177   EFI_PHYSICAL_ADDRESS  NewFdtBlobBase;
 178   UINTN                 NewFdtBlobSize;
 179   VOID*                 fdt;
 180   INTN                  err;
 181   INTN                  node;
 182   INTN                  cpu_node;
 183   INTN                  lenp;
 184   CONST VOID*           BootArg;
 185   EFI_PHYSICAL_ADDRESS  InitrdImageStart;
 186   EFI_PHYSICAL_ADDRESS  InitrdImageEnd;
 187   FdtRegion             Region;
 188   UINTN                 Index;
 189   CHAR8                 Name[10];
 190   LIST_ENTRY            ResourceList;
 191   BDS_SYSTEM_MEMORY_RESOURCE  *Resource;
 192   ARM_PROCESSOR_TABLE   *ArmProcessorTable;
 193   ARM_CORE_INFO         *ArmCoreInfoTable;
 194   UINT32                MpId;
 195   UINT32                ClusterId;
 196   UINT32                CoreId;
 197   UINT64                CpuReleaseAddr;
 198
 199   err = fdt_check_header ((VOID*)(UINTN)(*FdtBlobBase));
 200   if (err != 0) {
 201     Print (L"ERROR: Device Tree header not valid (err:%d)\n", err);
 202     return EFI_INVALID_PARAMETER;
 203   }
 204
 205   //
 206   // Allocate memory for the new FDT
 207   //
 208   NewFdtBlobSize = fdt_totalsize((VOID*)(UINTN)(*FdtBlobBase)) + FDT_ADDITIONAL_ENTRIES_SIZE;
 209
 210   // Try below a watermark address
 211   Status = EFI_NOT_FOUND;
 212   if (PcdGet32(PcdArmLinuxFdtMaxOffset) != 0) {
 213     NewFdtBlobBase = LINUX_FDT_MAX_OFFSET;
 214     Status = gBS->AllocatePages (AllocateMaxAddress, EfiBootServicesData, EFI_SIZE_TO_PAGES(NewFdtBlobSize), &NewFdtBlobBase);
 215     if (EFI_ERROR(Status)) {
 216       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));
 217     }
 218   }
 219
 220   // Try anywhere there is available space
 221   if (EFI_ERROR(Status)) {
 222     Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData, EFI_SIZE_TO_PAGES(NewFdtBlobSize), &NewFdtBlobBase);
 223     if (EFI_ERROR(Status)) {
 224       ASSERT_EFI_ERROR(Status);
 225       goto FAIL_NEW_FDT;
 226     } else {
 227       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));
 228     }
 229   }
 230
 231   // Load the Original FDT tree into the new region
 232   fdt = (VOID*)(UINTN)NewFdtBlobBase;
 233   err = fdt_open_into((VOID*)(UINTN)(*FdtBlobBase), fdt, NewFdtBlobSize);
 234   if (err) {
 235     DEBUG((EFI_D_ERROR, "fdt_open_into(): %a\n", fdt_strerror(err)));
 236     Status = EFI_INVALID_PARAMETER;
 237     goto FAIL_NEW_FDT;
 238   }
 239
 240   DEBUG_CODE_BEGIN();
 241     //DebugDumpFdt (fdt);
 242   DEBUG_CODE_END();
 243
 244   node = fdt_subnode_offset(fdt, 0, "chosen");
 245   if (node < 0) {
 246     // The 'chosen' node does not exist, create it
 247     node = fdt_add_subnode(fdt, 0, "chosen");
 248     if (node < 0) {
 249       DEBUG((EFI_D_ERROR,"Error on finding 'chosen' node\n"));
 250       Status = EFI_INVALID_PARAMETER;
 251       goto FAIL_NEW_FDT;
 252     }
 253   }
 254
 255   DEBUG_CODE_BEGIN();
 256     BootArg = fdt_getprop(fdt, node, "bootargs", &lenp);
 257     if (BootArg != NULL) {
 258       DEBUG((EFI_D_ERROR,"BootArg: %a\n",BootArg));
 259     }
 260   DEBUG_CODE_END();
 261
 262   // Set Linux CmdLine
 263   if ((CommandLineArguments != NULL) && (AsciiStrLen (CommandLineArguments) > 0)) {
 264     err = fdt_setprop(fdt, node, "bootargs", CommandLineArguments, AsciiStrSize(CommandLineArguments));
 265     if (err) {
 266       DEBUG((EFI_D_ERROR,"Fail to set new 'bootarg' (err:%d)\n",err));
 267     }
 268   }
 269
 270   // Set Linux Initrd
 271   if (InitrdImageSize != 0) {
 272     InitrdImageStart = cpu_to_fdt64 (InitrdImage);
 273     err = fdt_setprop(fdt, node, "linux,initrd-start", &InitrdImageStart, sizeof(EFI_PHYSICAL_ADDRESS));
 274     if (err) {
 275       DEBUG((EFI_D_ERROR,"Fail to set new 'linux,initrd-start' (err:%d)\n",err));
 276     }
 277     InitrdImageEnd = cpu_to_fdt64 (InitrdImage + InitrdImageSize);
 278     err = fdt_setprop(fdt, node, "linux,initrd-end", &InitrdImageEnd, sizeof(EFI_PHYSICAL_ADDRESS));
 279     if (err) {
 280       DEBUG((EFI_D_ERROR,"Fail to set new 'linux,initrd-start' (err:%d)\n",err));
 281     }
 282   }
 283
 284   // Set Physical memory setup if does not exist
 285   node = fdt_subnode_offset(fdt, 0, "memory");
 286   if (node < 0) {
 287     // The 'memory' node does not exist, create it
 288     node = fdt_add_subnode(fdt, 0, "memory");
 289     if (node >= 0) {
 290       fdt_setprop_string(fdt, node, "name", "memory");
 291       fdt_setprop_string(fdt, node, "device_type", "memory");
 292
 293       GetSystemMemoryResources (&ResourceList);
 294       Resource = (BDS_SYSTEM_MEMORY_RESOURCE*)ResourceList.ForwardLink;
 295
 296       if (sizeof(UINTN) == sizeof(UINT32)) {
 297         Region.Base = cpu_to_fdt32((UINTN)Resource->PhysicalStart);
 298         Region.Size = cpu_to_fdt32((UINTN)Resource->ResourceLength);
 299       } else {
 300         Region.Base = cpu_to_fdt64((UINTN)Resource->PhysicalStart);
 301         Region.Size = cpu_to_fdt64((UINTN)Resource->ResourceLength);
 302       }
 303
 304       err = fdt_setprop(fdt, node, "reg", &Region, sizeof(Region));
 305       if (err) {
 306         DEBUG((EFI_D_ERROR,"Fail to set new 'memory region' (err:%d)\n",err));
 307       }
 308     }
 309   }
 310
 311   // Setup Arm Mpcore Info if it is a multi-core or multi-cluster platforms
 312   for (Index=0; Index < gST->NumberOfTableEntries; Index++) {
 313     // Check for correct GUID type
 314     if (CompareGuid (&gArmMpCoreInfoGuid, &(gST->ConfigurationTable[Index].VendorGuid))) {
 315       MpId = ArmReadMpidr ();
 316       ClusterId = GET_CLUSTER_ID(MpId);
 317       CoreId    = GET_CORE_ID(MpId);
 318
 319       node = fdt_subnode_offset(fdt, 0, "cpus");
 320       if (node < 0) {
 321         // Create the /cpus node
 322         node = fdt_add_subnode(fdt, 0, "cpus");
 323         fdt_setprop_string(fdt, node, "name", "cpus");
 324         fdt_setprop_cell(fdt, node, "#address-cells", 1);
 325         fdt_setprop_cell(fdt, node, "#size-cells", 0);
 326       }
 327
 328       // Get pointer to ARM processor table
 329       ArmProcessorTable = (ARM_PROCESSOR_TABLE *)gST->ConfigurationTable[Index].VendorTable;
 330       ArmCoreInfoTable = ArmProcessorTable->ArmCpus;
 331
 332       for (Index = 0; Index < ArmProcessorTable->NumberOfEntries; Index++) {
 333         AsciiSPrint (Name, 10, "cpu@%d", Index);
 334         cpu_node = fdt_subnode_offset(fdt, node, Name);
 335         if (cpu_node < 0) {
 336           cpu_node = fdt_add_subnode(fdt, node, Name);
 337           fdt_setprop_string(fdt, cpu_node, "device-type", "cpu");
 338           fdt_setprop(fdt, cpu_node, "reg", &Index, sizeof(Index));
 339         }
 340
 341         fdt_setprop_string(fdt, cpu_node, "enable-method", "spin-table");
 342         CpuReleaseAddr = cpu_to_fdt64(ArmCoreInfoTable[Index].MailboxSetAddress);
 343         fdt_setprop(fdt, cpu_node, "cpu-release-addr", &CpuReleaseAddr, sizeof(CpuReleaseAddr));
 344
 345         // If it is not the primary core than the cpu should be disabled
 346         if (((ArmCoreInfoTable[Index].ClusterId != ClusterId) || (ArmCoreInfoTable[Index].CoreId != CoreId))) {
 347           fdt_setprop_string(fdt, cpu_node, "status", "disabled");
 348         }
 349       }
 350       break;
 351     }
 352   }
 353
 354   DEBUG_CODE_BEGIN();
 355     //DebugDumpFdt (fdt);
 356   DEBUG_CODE_END();
 357
 358   *FdtBlobBase = NewFdtBlobBase;
 359   *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(NewFdtBlobBase));
 360   return EFI_SUCCESS;
 361
 362 FAIL_NEW_FDT:
 363   *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(*FdtBlobBase));
 364   // Return success even if we failed to update the FDT blob. The original one is still valid.
 365   return EFI_SUCCESS;
 366 }
 367
 368