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/ArmSmcLib.h>
  16 #include <Library/PcdLib.h>
  17 #include <libfdt.h>
  18
  19 #include <IndustryStandard/ArmSmc.h>
  20
  21 #include "BdsInternal.h"
  22 #include "BdsLinuxLoader.h"
  23
  24 #define ALIGN(x, a)     (((x) + ((a) - 1)) & ~((a) - 1))
  25 #define PALIGN(p, a)    ((void *)(ALIGN((unsigned long)(p), (a))))
  26 #define GET_CELL(p)     (p += 4, *((const UINT32 *)(p-4)))
  27
  28 STATIC
  29 UINTN
  30 IsPrintableString (
  31   IN CONST VOID* data,
  32   IN UINTN len
  33   )
  34 {
  35   CONST CHAR8 *s = data;
  36   CONST CHAR8 *ss;
  37
  38   // Zero length is not
  39   if (len == 0) {
  40     return 0;
  41   }
  42
  43   // Must terminate with zero
  44   if (s[len - 1] != '\0') {
  45     return 0;
  46   }
  47
  48   ss = s;
  49   while (*s/* && isprint(*s)*/) {
  50     s++;
  51   }
  52
  53   // Not zero, or not done yet
  54   if (*s != '\0' || (s + 1 - ss) < len) {
  55     return 0;
  56   }
  57
  58   return 1;
  59 }
  60
  61 STATIC
  62 VOID
  63 PrintData (
  64   IN CONST CHAR8* data,
  65   IN UINTN len
  66   )
  67 {
  68   UINTN i;
  69   CONST CHAR8 *p = data;
  70
  71   // No data, don't print
  72   if (len == 0)
  73     return;
  74
  75   if (IsPrintableString (data, len)) {
  76     Print(L" = \"%a\"", (const char *)data);
  77   } else if ((len % 4) == 0) {
  78     Print(L" = <");
  79     for (i = 0; i < len; i += 4) {
  80       Print(L"0x%08x%a", fdt32_to_cpu(GET_CELL(p)),i < (len - 4) ? " " : "");
  81     }
  82     Print(L">");
  83   } else {
  84     Print(L" = [");
  85     for (i = 0; i < len; i++)
  86       Print(L"%02x%a", *p++, i < len - 1 ? " " : "");
  87     Print(L"]");
  88   }
  89 }
  90
  91 VOID
  92 DebugDumpFdt (
  93   IN VOID*                FdtBlob
  94   )
  95 {
  96   struct fdt_header *bph;
  97   UINT32 off_dt;
  98   UINT32 off_str;
  99   CONST CHAR8* p_struct;
 100   CONST CHAR8* p_strings;
 101   CONST CHAR8* p;
 102   CONST CHAR8* s;
 103   CONST CHAR8* t;
 104   UINT32 tag;
 105   UINTN sz;
 106   UINTN depth;
 107   UINTN shift;
 108   UINT32 version;
 109
 110   {
 111     // Can 'memreserve' be printed by below code?
 112     INTN num = fdt_num_mem_rsv(FdtBlob);
 113     INTN i, err;
 114     UINT64 addr = 0,size = 0;
 115
 116     for (i = 0; i < num; i++) {
 117       err = fdt_get_mem_rsv(FdtBlob, i, &addr, &size);
 118       if (err) {
 119         DEBUG((EFI_D_ERROR, "Error (%d) : Cannot get memreserve section (%d)\n", err, i));
 120       }
 121       else {
 122         Print(L"/memreserve/ \t0x%lx \t0x%lx;\n",addr,size);
 123       }
 124     }
 125   }
 126
 127   depth = 0;
 128   shift = 4;
 129
 130   bph = FdtBlob;
 131   off_dt = fdt32_to_cpu(bph->off_dt_struct);
 132   off_str = fdt32_to_cpu(bph->off_dt_strings);
 133   p_struct = (CONST CHAR8*)FdtBlob + off_dt;
 134   p_strings = (CONST CHAR8*)FdtBlob + off_str;
 135   version = fdt32_to_cpu(bph->version);
 136
 137   p = p_struct;
 138   while ((tag = fdt32_to_cpu(GET_CELL(p))) != FDT_END) {
 139     if (tag == FDT_BEGIN_NODE) {
 140       s = p;
 141       p = PALIGN(p + AsciiStrLen (s) + 1, 4);
 142
 143       if (*s == '\0')
 144               s = "/";
 145
 146       Print(L"%*s%a {\n", depth * shift, L" ", s);
 147
 148       depth++;
 149       continue;
 150     }
 151
 152     if (tag == FDT_END_NODE) {
 153       depth--;
 154
 155       Print(L"%*s};\n", depth * shift, L" ");
 156       continue;
 157     }
 158
 159     if (tag == FDT_NOP) {
 160       Print(L"%*s// [NOP]\n", depth * shift, L" ");
 161       continue;
 162     }
 163
 164     if (tag != FDT_PROP) {
 165       Print(L"%*s ** Unknown tag 0x%08x\n", depth * shift, L" ", tag);
 166       break;
 167     }
 168     sz = fdt32_to_cpu(GET_CELL(p));
 169     s = p_strings + fdt32_to_cpu(GET_CELL(p));
 170     if (version < 16 && sz >= 8)
 171             p = PALIGN(p, 8);
 172     t = p;
 173
 174     p = PALIGN(p + sz, 4);
 175
 176     Print(L"%*s%a", depth * shift, L" ", s);
 177     PrintData(t, sz);
 178     Print(L";\n");
 179   }
 180 }
 181
 182 STATIC
 183 BOOLEAN
 184 IsLinuxReservedRegion (
 185   IN EFI_MEMORY_TYPE MemoryType
 186   )
 187 {
 188   switch(MemoryType) {
 189   case EfiRuntimeServicesCode:
 190   case EfiRuntimeServicesData:
 191   case EfiUnusableMemory:
 192   case EfiACPIReclaimMemory:
 193   case EfiACPIMemoryNVS:
 194     return TRUE;
 195   default:
 196     return FALSE;
 197   }
 198 }
 199
 200
 201 typedef struct {
 202   UINTN   Base;
 203   UINTN   Size;
 204 } FdtRegion;
 205
 206 EFI_STATUS
 207 PrepareFdt (
 208   IN     CONST CHAR8*         CommandLineArguments,
 209   IN     EFI_PHYSICAL_ADDRESS InitrdImage,
 210   IN     UINTN                InitrdImageSize,
 211   IN OUT EFI_PHYSICAL_ADDRESS *FdtBlobBase,
 212   IN OUT UINTN                *FdtBlobSize
 213   )
 214 {
 215   EFI_STATUS            Status;
 216   EFI_PHYSICAL_ADDRESS  NewFdtBlobBase;
 217   UINTN                 NewFdtBlobSize;
 218   VOID*                 fdt;
 219   INTN                  err;
 220   INTN                  node;
 221   INTN                  cpu_node;
 222   INTN                  lenp;
 223   CONST VOID*           BootArg;
 224   CONST VOID*           Method;
 225   EFI_PHYSICAL_ADDRESS  InitrdImageStart;
 226   EFI_PHYSICAL_ADDRESS  InitrdImageEnd;
 227   FdtRegion             Region;
 228   UINTN                 Index;
 229   CHAR8                 Name[10];
 230   LIST_ENTRY            ResourceList;
 231   BDS_SYSTEM_MEMORY_RESOURCE  *Resource;
 232   ARM_PROCESSOR_TABLE   *ArmProcessorTable;
 233   ARM_CORE_INFO         *ArmCoreInfoTable;
 234   UINT32                MpId;
 235   UINT32                ClusterId;
 236   UINT32                CoreId;
 237   UINT64                CpuReleaseAddr;
 238   UINTN                 MemoryMapSize;
 239   EFI_MEMORY_DESCRIPTOR *MemoryMap;
 240   UINTN                 MapKey;
 241   UINTN                 DescriptorSize;
 242   UINT32                DescriptorVersion;
 243   UINTN                 Pages;
 244   BOOLEAN               PsciSmcSupported;
 245   UINTN                 Rx;
 246
 247   //
 248   // Ensure the Power State Coordination Interface (PSCI) SMCs are there if supported
 249   //
 250   PsciSmcSupported = FALSE;
 251   if (FeaturePcdGet (PcdArmPsciSupport) == TRUE) {
 252     // Check the SMC response to the Presence SMC
 253     Rx   = ARM_SMC_ID_PRESENCE;
 254     ArmCallSmc (&Rx);
 255     if (Rx == 1) {
 256       // Check the SMC UID
 257       Rx   = ARM_SMC_ID_UID;
 258       ArmCallSmc (&Rx);
 259       if (Rx == ARM_TRUSTZONE_UID_4LETTERID) {
 260         Rx   = ARM_SMC_ID_UID + 1;
 261         ArmCallSmc (&Rx);
 262         //TODO: Replace ARM magic number
 263         if (Rx == 0x40524d48) {
 264           PsciSmcSupported = TRUE;
 265         }
 266       }
 267       if (PsciSmcSupported == FALSE) {
 268         DEBUG((EFI_D_ERROR,"Warning: The Power State Coordination Interface (PSCI) is not supported"
 269                            "by your platform Trusted Firmware.\n"));
 270       }
 271     }
 272   }
 273
 274   err = fdt_check_header ((VOID*)(UINTN)(*FdtBlobBase));
 275   if (err != 0) {
 276     Print (L"ERROR: Device Tree header not valid (err:%d)\n", err);
 277     return EFI_INVALID_PARAMETER;
 278   }
 279
 280   //
 281   // Allocate memory for the new FDT
 282   //
 283   NewFdtBlobSize = fdt_totalsize((VOID*)(UINTN)(*FdtBlobBase)) + FDT_ADDITIONAL_ENTRIES_SIZE;
 284
 285   // Try below a watermark address
 286   Status = EFI_NOT_FOUND;
 287   if (PcdGet32(PcdArmLinuxFdtMaxOffset) != 0) {
 288     NewFdtBlobBase = LINUX_FDT_MAX_OFFSET;
 289     Status = gBS->AllocatePages (AllocateMaxAddress, EfiBootServicesData, EFI_SIZE_TO_PAGES(NewFdtBlobSize), &NewFdtBlobBase);
 290     if (EFI_ERROR(Status)) {
 291       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));
 292     }
 293   }
 294
 295   // Try anywhere there is available space
 296   if (EFI_ERROR(Status)) {
 297     Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData, EFI_SIZE_TO_PAGES(NewFdtBlobSize), &NewFdtBlobBase);
 298     if (EFI_ERROR(Status)) {
 299       ASSERT_EFI_ERROR(Status);
 300       goto FAIL_NEW_FDT;
 301     } else {
 302       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));
 303     }
 304   }
 305
 306   // Load the Original FDT tree into the new region
 307   fdt = (VOID*)(UINTN)NewFdtBlobBase;
 308   err = fdt_open_into((VOID*)(UINTN)(*FdtBlobBase), fdt, NewFdtBlobSize);
 309   if (err) {
 310     DEBUG((EFI_D_ERROR, "fdt_open_into(): %a\n", fdt_strerror(err)));
 311     Status = EFI_INVALID_PARAMETER;
 312     goto FAIL_NEW_FDT;
 313   }
 314
 315   DEBUG_CODE_BEGIN();
 316     //DebugDumpFdt (fdt);
 317   DEBUG_CODE_END();
 318
 319   node = fdt_subnode_offset(fdt, 0, "chosen");
 320   if (node < 0) {
 321     // The 'chosen' node does not exist, create it
 322     node = fdt_add_subnode(fdt, 0, "chosen");
 323     if (node < 0) {
 324       DEBUG((EFI_D_ERROR,"Error on finding 'chosen' node\n"));
 325       Status = EFI_INVALID_PARAMETER;
 326       goto FAIL_NEW_FDT;
 327     }
 328   }
 329
 330   DEBUG_CODE_BEGIN();
 331     BootArg = fdt_getprop(fdt, node, "bootargs", &lenp);
 332     if (BootArg != NULL) {
 333       DEBUG((EFI_D_ERROR,"BootArg: %a\n",BootArg));
 334     }
 335   DEBUG_CODE_END();
 336
 337   //
 338   // Set Linux CmdLine
 339   //
 340   if ((CommandLineArguments != NULL) && (AsciiStrLen (CommandLineArguments) > 0)) {
 341     err = fdt_setprop(fdt, node, "bootargs", CommandLineArguments, AsciiStrSize(CommandLineArguments));
 342     if (err) {
 343       DEBUG((EFI_D_ERROR,"Fail to set new 'bootarg' (err:%d)\n",err));
 344     }
 345   }
 346
 347   //
 348   // Set Linux Initrd
 349   //
 350   if (InitrdImageSize != 0) {
 351     InitrdImageStart = cpu_to_fdt64 (InitrdImage);
 352     err = fdt_setprop(fdt, node, "linux,initrd-start", &InitrdImageStart, sizeof(EFI_PHYSICAL_ADDRESS));
 353     if (err) {
 354       DEBUG((EFI_D_ERROR,"Fail to set new 'linux,initrd-start' (err:%d)\n",err));
 355     }
 356     InitrdImageEnd = cpu_to_fdt64 (InitrdImage + InitrdImageSize);
 357     err = fdt_setprop(fdt, node, "linux,initrd-end", &InitrdImageEnd, sizeof(EFI_PHYSICAL_ADDRESS));
 358     if (err) {
 359       DEBUG((EFI_D_ERROR,"Fail to set new 'linux,initrd-start' (err:%d)\n",err));
 360     }
 361   }
 362
 363   //
 364   // Set Physical memory setup if does not exist
 365   //
 366   node = fdt_subnode_offset(fdt, 0, "memory");
 367   if (node < 0) {
 368     // The 'memory' node does not exist, create it
 369     node = fdt_add_subnode(fdt, 0, "memory");
 370     if (node >= 0) {
 371       fdt_setprop_string(fdt, node, "name", "memory");
 372       fdt_setprop_string(fdt, node, "device_type", "memory");
 373
 374       GetSystemMemoryResources (&ResourceList);
 375       Resource = (BDS_SYSTEM_MEMORY_RESOURCE*)ResourceList.ForwardLink;
 376
 377       if (sizeof(UINTN) == sizeof(UINT32)) {
 378         Region.Base = cpu_to_fdt32((UINTN)Resource->PhysicalStart);
 379         Region.Size = cpu_to_fdt32((UINTN)Resource->ResourceLength);
 380       } else {
 381         Region.Base = cpu_to_fdt64((UINTN)Resource->PhysicalStart);
 382         Region.Size = cpu_to_fdt64((UINTN)Resource->ResourceLength);
 383       }
 384
 385       err = fdt_setprop(fdt, node, "reg", &Region, sizeof(Region));
 386       if (err) {
 387         DEBUG((EFI_D_ERROR,"Fail to set new 'memory region' (err:%d)\n",err));
 388       }
 389     }
 390   }
 391
 392   //
 393   // Add the memory regions reserved by the UEFI Firmware
 394   //
 395
 396   // Retrieve the UEFI Memory Map
 397   MemoryMap = NULL;
 398   MemoryMapSize = 0;
 399   Status = gBS->GetMemoryMap (&MemoryMapSize, MemoryMap, &MapKey, &DescriptorSize, &DescriptorVersion);
 400   if (Status == EFI_BUFFER_TOO_SMALL) {
 401     Pages = EFI_SIZE_TO_PAGES (MemoryMapSize) + 1;
 402     MemoryMap = AllocatePages (Pages);
 403     Status = gBS->GetMemoryMap (&MemoryMapSize, MemoryMap, &MapKey, &DescriptorSize, &DescriptorVersion);
 404   }
 405
 406   // Go through the list and add the reserved region to the Device Tree
 407   if (!EFI_ERROR(Status)) {
 408     for (Index = 0; Index < (MemoryMapSize / sizeof(EFI_MEMORY_DESCRIPTOR)); Index++) {
 409       if (IsLinuxReservedRegion ((EFI_MEMORY_TYPE)MemoryMap[Index].Type)) {
 410         DEBUG((DEBUG_VERBOSE, "Reserved region of type %d [0x%X, 0x%X]\n",
 411             MemoryMap[Index].Type,
 412             (UINTN)MemoryMap[Index].PhysicalStart,
 413             (UINTN)(MemoryMap[Index].PhysicalStart + MemoryMap[Index].NumberOfPages * EFI_PAGE_SIZE)));
 414         err = fdt_add_mem_rsv(fdt, MemoryMap[Index].PhysicalStart, MemoryMap[Index].NumberOfPages * EFI_PAGE_SIZE);
 415         if (err != 0) {
 416           Print(L"Warning: Fail to add 'memreserve' (err:%d)\n", err);
 417         }
 418       }
 419     }
 420   }
 421
 422   //
 423   // Setup Arm Mpcore Info if it is a multi-core or multi-cluster platforms
 424   //
 425   for (Index=0; Index < gST->NumberOfTableEntries; Index++) {
 426     // Check for correct GUID type
 427     if (CompareGuid (&gArmMpCoreInfoGuid, &(gST->ConfigurationTable[Index].VendorGuid))) {
 428       MpId = ArmReadMpidr ();
 429       ClusterId = GET_CLUSTER_ID(MpId);
 430       CoreId    = GET_CORE_ID(MpId);
 431
 432       node = fdt_subnode_offset(fdt, 0, "cpus");
 433       if (node < 0) {
 434         // Create the /cpus node
 435         node = fdt_add_subnode(fdt, 0, "cpus");
 436         fdt_setprop_string(fdt, node, "name", "cpus");
 437         fdt_setprop_cell(fdt, node, "#address-cells", 1);
 438         fdt_setprop_cell(fdt, node, "#size-cells", 0);
 439       }
 440
 441       // Get pointer to ARM processor table
 442       ArmProcessorTable = (ARM_PROCESSOR_TABLE *)gST->ConfigurationTable[Index].VendorTable;
 443       ArmCoreInfoTable = ArmProcessorTable->ArmCpus;
 444
 445       for (Index = 0; Index < ArmProcessorTable->NumberOfEntries; Index++) {
 446         AsciiSPrint (Name, 10, "cpu@%d", Index);
 447         cpu_node = fdt_subnode_offset(fdt, node, Name);
 448         if (cpu_node < 0) {
 449           cpu_node = fdt_add_subnode(fdt, node, Name);
 450           fdt_setprop_string(fdt, cpu_node, "device-type", "cpu");
 451           fdt_setprop(fdt, cpu_node, "reg", &Index, sizeof(Index));
 452         }
 453
 454         // If Power State Coordination Interface (PSCI) is not supported then it is expected the secondary
 455         // cores are spinning waiting for the Operating System to release them
 456         if (PsciSmcSupported == FALSE) {
 457           // We as the bootloader are responsible for either creating or updating
 458           // these entries. Do not trust the entries in the DT. We only know about
 459           // 'spin-table' type. Do not try to update other types if defined.
 460           Method = fdt_getprop(fdt, cpu_node, "enable-method", &lenp);
 461           if ( (Method == NULL) || (!AsciiStrCmp((CHAR8 *)Method, "spin-table")) ) {
 462             fdt_setprop_string(fdt, cpu_node, "enable-method", "spin-table");
 463             CpuReleaseAddr = cpu_to_fdt64(ArmCoreInfoTable[Index].MailboxSetAddress);
 464             fdt_setprop(fdt, cpu_node, "cpu-release-addr", &CpuReleaseAddr, sizeof(CpuReleaseAddr));
 465
 466             // If it is not the primary core than the cpu should be disabled
 467             if (((ArmCoreInfoTable[Index].ClusterId != ClusterId) || (ArmCoreInfoTable[Index].CoreId != CoreId))) {
 468               fdt_setprop_string(fdt, cpu_node, "status", "disabled");
 469             }
 470           } else {
 471             Print(L"Warning: Unsupported enable-method type for CPU[%d] : %a\n", Index, (CHAR8 *)Method);
 472           }
 473         }
 474       }
 475       break;
 476     }
 477   }
 478
 479   // If the Power State Coordination Interface is supported then we signal it in the Device Tree
 480   if (PsciSmcSupported == TRUE) {
 481     // Before to create it we check if the node is not already defined in the Device Tree
 482     node = fdt_subnode_offset(fdt, 0, "psci");
 483     if (node < 0) {
 484       // The 'psci' node does not exist, create it
 485       node = fdt_add_subnode(fdt, 0, "psci");
 486       if (node < 0) {
 487         DEBUG((EFI_D_ERROR,"Error on creating 'psci' node\n"));
 488         Status = EFI_INVALID_PARAMETER;
 489         goto FAIL_NEW_FDT;
 490       } else {
 491         fdt_setprop_string(fdt, node, "compatible", "arm,psci");
 492         fdt_setprop_string(fdt, node, "method", "smc");
 493         fdt_setprop_cell(fdt, node, "cpu_suspend", ARM_SMC_ARM_CPU_SUSPEND);
 494         fdt_setprop_cell(fdt, node, "cpu_off", ARM_SMC_ARM_CPU_OFF);
 495         fdt_setprop_cell(fdt, node, "cpu_on", ARM_SMC_ARM_CPU_ON);
 496         fdt_setprop_cell(fdt, node, "cpu_migrate", ARM_SMC_ARM_MIGRATE);
 497       }
 498     }
 499   }
 500
 501   DEBUG_CODE_BEGIN();
 502     //DebugDumpFdt (fdt);
 503   DEBUG_CODE_END();
 504
 505   *FdtBlobBase = NewFdtBlobBase;
 506   *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(NewFdtBlobBase));
 507   return EFI_SUCCESS;
 508
 509 FAIL_NEW_FDT:
 510   *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(*FdtBlobBase));
 511   // Return success even if we failed to update the FDT blob. The original one is still valid.
 512   return EFI_SUCCESS;
 513 }
 514
 515