ArmPkg/Library/BdsLib/BdsLinuxFdt.c

   1 /** @file
   2 *
   3 *  Copyright (c) 2011-2013, 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   UINTN                 OriginalFdtSize;
 247
 248   //
 249   // Ensure the Power State Coordination Interface (PSCI) SMCs are there if supported
 250   //
 251   PsciSmcSupported = FALSE;
 252   if (FeaturePcdGet (PcdArmPsciSupport) == TRUE) {
 253     // Check the SMC response to the Presence SMC
 254     Rx   = ARM_SMC_ID_PRESENCE;
 255     ArmCallSmc (&Rx);
 256     if (Rx == 1) {
 257       // Check the SMC UID
 258       Rx   = ARM_SMC_ID_UID;
 259       ArmCallSmc (&Rx);
 260       if (Rx == ARM_TRUSTZONE_UID_4LETTERID) {
 261         Rx   = ARM_SMC_ID_UID + 1;
 262         ArmCallSmc (&Rx);
 263         //TODO: Replace ARM magic number
 264         if (Rx == 0x40524d48) {
 265           PsciSmcSupported = TRUE;
 266         }
 267       }
 268       if (PsciSmcSupported == FALSE) {
 269         DEBUG((EFI_D_ERROR,"Warning: The Power State Coordination Interface (PSCI) is not supported"
 270                            "by your platform Trusted Firmware.\n"));
 271       }
 272     }
 273   }
 274
 275   //
 276   // Sanity checks on the original FDT blob.
 277   //
 278   err = fdt_check_header ((VOID*)(UINTN)(*FdtBlobBase));
 279   if (err != 0) {
 280     Print (L"ERROR: Device Tree header not valid (err:%d)\n", err);
 281     return EFI_INVALID_PARAMETER;
 282   }
 283
 284   // The original FDT blob might have been loaded partially.
 285   // Check that it is not the case.
 286   OriginalFdtSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(*FdtBlobBase));
 287   if (OriginalFdtSize > *FdtBlobSize) {
 288     Print (L"ERROR: Incomplete FDT. Only %d/%d bytes have been loaded.\n",
 289            *FdtBlobSize, OriginalFdtSize);
 290     return EFI_INVALID_PARAMETER;
 291   }
 292
 293   //
 294   // Allocate memory for the new FDT
 295   //
 296   NewFdtBlobSize = OriginalFdtSize + FDT_ADDITIONAL_ENTRIES_SIZE;
 297
 298   // Try below a watermark address
 299   Status = EFI_NOT_FOUND;
 300   if (PcdGet32(PcdArmLinuxFdtMaxOffset) != 0) {
 301     NewFdtBlobBase = LINUX_FDT_MAX_OFFSET;
 302     Status = gBS->AllocatePages (AllocateMaxAddress, EfiBootServicesData, EFI_SIZE_TO_PAGES(NewFdtBlobSize), &NewFdtBlobBase);
 303     if (EFI_ERROR(Status)) {
 304       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));
 305     }
 306   }
 307
 308   // Try anywhere there is available space
 309   if (EFI_ERROR(Status)) {
 310     Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData, EFI_SIZE_TO_PAGES(NewFdtBlobSize), &NewFdtBlobBase);
 311     if (EFI_ERROR(Status)) {
 312       ASSERT_EFI_ERROR(Status);
 313       goto FAIL_NEW_FDT;
 314     } else {
 315       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));
 316     }
 317   }
 318
 319   // Load the Original FDT tree into the new region
 320   fdt = (VOID*)(UINTN)NewFdtBlobBase;
 321   err = fdt_open_into((VOID*)(UINTN)(*FdtBlobBase), fdt, NewFdtBlobSize);
 322   if (err) {
 323     DEBUG((EFI_D_ERROR, "fdt_open_into(): %a\n", fdt_strerror(err)));
 324     Status = EFI_INVALID_PARAMETER;
 325     goto FAIL_NEW_FDT;
 326   }
 327
 328   DEBUG_CODE_BEGIN();
 329     //DebugDumpFdt (fdt);
 330   DEBUG_CODE_END();
 331
 332   node = fdt_subnode_offset(fdt, 0, "chosen");
 333   if (node < 0) {
 334     // The 'chosen' node does not exist, create it
 335     node = fdt_add_subnode(fdt, 0, "chosen");
 336     if (node < 0) {
 337       DEBUG((EFI_D_ERROR,"Error on finding 'chosen' node\n"));
 338       Status = EFI_INVALID_PARAMETER;
 339       goto FAIL_NEW_FDT;
 340     }
 341   }
 342
 343   DEBUG_CODE_BEGIN();
 344     BootArg = fdt_getprop(fdt, node, "bootargs", &lenp);
 345     if (BootArg != NULL) {
 346       DEBUG((EFI_D_ERROR,"BootArg: %a\n",BootArg));
 347     }
 348   DEBUG_CODE_END();
 349
 350   //
 351   // Set Linux CmdLine
 352   //
 353   if ((CommandLineArguments != NULL) && (AsciiStrLen (CommandLineArguments) > 0)) {
 354     err = fdt_setprop(fdt, node, "bootargs", CommandLineArguments, AsciiStrSize(CommandLineArguments));
 355     if (err) {
 356       DEBUG((EFI_D_ERROR,"Fail to set new 'bootarg' (err:%d)\n",err));
 357     }
 358   }
 359
 360   //
 361   // Set Linux Initrd
 362   //
 363   if (InitrdImageSize != 0) {
 364     InitrdImageStart = cpu_to_fdt64 (InitrdImage);
 365     err = fdt_setprop(fdt, node, "linux,initrd-start", &InitrdImageStart, sizeof(EFI_PHYSICAL_ADDRESS));
 366     if (err) {
 367       DEBUG((EFI_D_ERROR,"Fail to set new 'linux,initrd-start' (err:%d)\n",err));
 368     }
 369     InitrdImageEnd = cpu_to_fdt64 (InitrdImage + InitrdImageSize);
 370     err = fdt_setprop(fdt, node, "linux,initrd-end", &InitrdImageEnd, sizeof(EFI_PHYSICAL_ADDRESS));
 371     if (err) {
 372       DEBUG((EFI_D_ERROR,"Fail to set new 'linux,initrd-start' (err:%d)\n",err));
 373     }
 374   }
 375
 376   //
 377   // Set Physical memory setup if does not exist
 378   //
 379   node = fdt_subnode_offset(fdt, 0, "memory");
 380   if (node < 0) {
 381     // The 'memory' node does not exist, create it
 382     node = fdt_add_subnode(fdt, 0, "memory");
 383     if (node >= 0) {
 384       fdt_setprop_string(fdt, node, "name", "memory");
 385       fdt_setprop_string(fdt, node, "device_type", "memory");
 386
 387       GetSystemMemoryResources (&ResourceList);
 388       Resource = (BDS_SYSTEM_MEMORY_RESOURCE*)ResourceList.ForwardLink;
 389
 390       if (sizeof(UINTN) == sizeof(UINT32)) {
 391         Region.Base = cpu_to_fdt32((UINTN)Resource->PhysicalStart);
 392         Region.Size = cpu_to_fdt32((UINTN)Resource->ResourceLength);
 393       } else {
 394         Region.Base = cpu_to_fdt64((UINTN)Resource->PhysicalStart);
 395         Region.Size = cpu_to_fdt64((UINTN)Resource->ResourceLength);
 396       }
 397
 398       err = fdt_setprop(fdt, node, "reg", &Region, sizeof(Region));
 399       if (err) {
 400         DEBUG((EFI_D_ERROR,"Fail to set new 'memory region' (err:%d)\n",err));
 401       }
 402     }
 403   }
 404
 405   //
 406   // Add the memory regions reserved by the UEFI Firmware
 407   //
 408
 409   // Retrieve the UEFI Memory Map
 410   MemoryMap = NULL;
 411   MemoryMapSize = 0;
 412   Status = gBS->GetMemoryMap (&MemoryMapSize, MemoryMap, &MapKey, &DescriptorSize, &DescriptorVersion);
 413   if (Status == EFI_BUFFER_TOO_SMALL) {
 414     Pages = EFI_SIZE_TO_PAGES (MemoryMapSize) + 1;
 415     MemoryMap = AllocatePages (Pages);
 416     Status = gBS->GetMemoryMap (&MemoryMapSize, MemoryMap, &MapKey, &DescriptorSize, &DescriptorVersion);
 417   }
 418
 419   // Go through the list and add the reserved region to the Device Tree
 420   if (!EFI_ERROR(Status)) {
 421     for (Index = 0; Index < (MemoryMapSize / sizeof(EFI_MEMORY_DESCRIPTOR)); Index++) {
 422       if (IsLinuxReservedRegion ((EFI_MEMORY_TYPE)MemoryMap[Index].Type)) {
 423         DEBUG((DEBUG_VERBOSE, "Reserved region of type %d [0x%X, 0x%X]\n",
 424             MemoryMap[Index].Type,
 425             (UINTN)MemoryMap[Index].PhysicalStart,
 426             (UINTN)(MemoryMap[Index].PhysicalStart + MemoryMap[Index].NumberOfPages * EFI_PAGE_SIZE)));
 427         err = fdt_add_mem_rsv(fdt, MemoryMap[Index].PhysicalStart, MemoryMap[Index].NumberOfPages * EFI_PAGE_SIZE);
 428         if (err != 0) {
 429           Print(L"Warning: Fail to add 'memreserve' (err:%d)\n", err);
 430         }
 431       }
 432     }
 433   }
 434
 435   //
 436   // Setup Arm Mpcore Info if it is a multi-core or multi-cluster platforms
 437   //
 438   for (Index=0; Index < gST->NumberOfTableEntries; Index++) {
 439     // Check for correct GUID type
 440     if (CompareGuid (&gArmMpCoreInfoGuid, &(gST->ConfigurationTable[Index].VendorGuid))) {
 441       MpId = ArmReadMpidr ();
 442       ClusterId = GET_CLUSTER_ID(MpId);
 443       CoreId    = GET_CORE_ID(MpId);
 444
 445       node = fdt_subnode_offset(fdt, 0, "cpus");
 446       if (node < 0) {
 447         // Create the /cpus node
 448         node = fdt_add_subnode(fdt, 0, "cpus");
 449         fdt_setprop_string(fdt, node, "name", "cpus");
 450         fdt_setprop_cell(fdt, node, "#address-cells", 1);
 451         fdt_setprop_cell(fdt, node, "#size-cells", 0);
 452       }
 453
 454       // Get pointer to ARM processor table
 455       ArmProcessorTable = (ARM_PROCESSOR_TABLE *)gST->ConfigurationTable[Index].VendorTable;
 456       ArmCoreInfoTable = ArmProcessorTable->ArmCpus;
 457
 458       for (Index = 0; Index < ArmProcessorTable->NumberOfEntries; Index++) {
 459         AsciiSPrint (Name, 10, "cpu@%d", Index);
 460         cpu_node = fdt_subnode_offset(fdt, node, Name);
 461         if (cpu_node < 0) {
 462           cpu_node = fdt_add_subnode(fdt, node, Name);
 463           fdt_setprop_string(fdt, cpu_node, "device-type", "cpu");
 464           fdt_setprop(fdt, cpu_node, "reg", &Index, sizeof(Index));
 465         }
 466
 467         // If Power State Coordination Interface (PSCI) is not supported then it is expected the secondary
 468         // cores are spinning waiting for the Operating System to release them
 469         if (PsciSmcSupported == FALSE) {
 470           // We as the bootloader are responsible for either creating or updating
 471           // these entries. Do not trust the entries in the DT. We only know about
 472           // 'spin-table' type. Do not try to update other types if defined.
 473           Method = fdt_getprop(fdt, cpu_node, "enable-method", &lenp);
 474           if ( (Method == NULL) || (!AsciiStrCmp((CHAR8 *)Method, "spin-table")) ) {
 475             fdt_setprop_string(fdt, cpu_node, "enable-method", "spin-table");
 476             CpuReleaseAddr = cpu_to_fdt64(ArmCoreInfoTable[Index].MailboxSetAddress);
 477             fdt_setprop(fdt, cpu_node, "cpu-release-addr", &CpuReleaseAddr, sizeof(CpuReleaseAddr));
 478
 479             // If it is not the primary core than the cpu should be disabled
 480             if (((ArmCoreInfoTable[Index].ClusterId != ClusterId) || (ArmCoreInfoTable[Index].CoreId != CoreId))) {
 481               fdt_setprop_string(fdt, cpu_node, "status", "disabled");
 482             }
 483           } else {
 484             Print(L"Warning: Unsupported enable-method type for CPU[%d] : %a\n", Index, (CHAR8 *)Method);
 485           }
 486         }
 487       }
 488       break;
 489     }
 490   }
 491
 492   // If the Power State Coordination Interface is supported then we signal it in the Device Tree
 493   if (PsciSmcSupported == TRUE) {
 494     // Before to create it we check if the node is not already defined in the Device Tree
 495     node = fdt_subnode_offset(fdt, 0, "psci");
 496     if (node < 0) {
 497       // The 'psci' node does not exist, create it
 498       node = fdt_add_subnode(fdt, 0, "psci");
 499       if (node < 0) {
 500         DEBUG((EFI_D_ERROR,"Error on creating 'psci' node\n"));
 501         Status = EFI_INVALID_PARAMETER;
 502         goto FAIL_NEW_FDT;
 503       } else {
 504         fdt_setprop_string(fdt, node, "compatible", "arm,psci");
 505         fdt_setprop_string(fdt, node, "method", "smc");
 506         fdt_setprop_cell(fdt, node, "cpu_suspend", ARM_SMC_ARM_CPU_SUSPEND);
 507         fdt_setprop_cell(fdt, node, "cpu_off", ARM_SMC_ARM_CPU_OFF);
 508         fdt_setprop_cell(fdt, node, "cpu_on", ARM_SMC_ARM_CPU_ON);
 509         fdt_setprop_cell(fdt, node, "cpu_migrate", ARM_SMC_ARM_MIGRATE);
 510       }
 511     }
 512   }
 513
 514   DEBUG_CODE_BEGIN();
 515     //DebugDumpFdt (fdt);
 516   DEBUG_CODE_END();
 517
 518   *FdtBlobBase = NewFdtBlobBase;
 519   *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(NewFdtBlobBase));
 520   return EFI_SUCCESS;
 521
 522 FAIL_NEW_FDT:
 523   *FdtBlobSize = OriginalFdtSize;
 524   // Return success even if we failed to update the FDT blob. The original one is still valid.
 525   return EFI_SUCCESS;
 526 }
 527
 528