[PATCH 2/4] EmbeddedPkg/AndroidFastboot: fix to boot android kernel

[Haojian Zhuang <haojian.zhuang@linaro.org>]

Fix the issue of booting android kernel since LinuxLoader is broken.

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Haojian Zhuang <haojian.zhuang@linaro.org>
---
 .../AndroidFastboot/AndroidFastbootApp.c           |   2 +-
 .../AndroidFastboot/AndroidFastbootApp.h           |  21 +-
 .../AndroidFastboot/AndroidFastbootApp.inf         |   4 +
 .../AndroidFastboot/Arm/BootAndroidBootImg.c       | 230 ++++++++++++++++-----
 4 files changed, 192 insertions(+), 65 deletions(-)

diff --git a/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.c b/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.c
index c5e8a7e..204754f 100644
--- a/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.c
+++ b/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.c
@@ -216,7 +216,7 @@ HandleBoot (
   // boot we lose control of the system.
   SEND_LITERAL ("OKAY");
 
-  Status = BootAndroidBootImg (mNumDataBytes, mDataBuffer);
+  Status = BootAndroidBootImg (mPlatform, mNumDataBytes, mDataBuffer);
   if (EFI_ERROR (Status)) {
     DEBUG ((EFI_D_ERROR, "Failed to boot downloaded image: %r\n", Status));
   }
diff --git a/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.h b/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.h
index f62660f..24d31de 100644
--- a/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.h
+++ b/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.h
@@ -19,24 +19,27 @@
 #include <Library/DebugLib.h>
 #include <Library/MemoryAllocationLib.h>
 
-#define BOOTIMG_KERNEL_ARGS_SIZE 512
+#include <Protocol/AndroidFastbootPlatform.h>
+
+#define BOOTIMG_KERNEL_ARGS_SIZE 1024
 
 #define ANDROID_FASTBOOT_VERSION "0.4"
 
 EFI_STATUS
 BootAndroidBootImg (
-  IN  UINTN    BufferSize,
-  IN  VOID    *Buffer
+  IN  FASTBOOT_PLATFORM_PROTOCOL      *Platform,
+  IN  UINTN                            BufferSize,
+  IN  VOID                            *Buffer
   );
 
 EFI_STATUS
 ParseAndroidBootImg (
-  IN  VOID    *BootImg,
-  OUT VOID   **Kernel,
-  OUT UINTN   *KernelSize,
-  OUT VOID   **Ramdisk,
-  OUT UINTN   *RamdiskSize,
-  OUT CHAR8   *KernelArgs
+  IN  VOID                            *BootImg,
+  OUT VOID                           **Kernel,
+  OUT UINTN                           *KernelSize,
+  OUT VOID                           **Ramdisk,
+  OUT UINTN                           *RamdiskSize,
+  OUT CHAR8                           *KernelArgs
   );
 
 #endif //ifdef __ANDROID_FASTBOOT_APP_H__
diff --git a/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.inf b/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.inf
index 3e11517..daf4008 100644
--- a/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.inf
+++ b/EmbeddedPkg/Application/AndroidFastboot/AndroidFastbootApp.inf
@@ -34,6 +34,7 @@
   DebugLib
   DevicePathLib
   DxeServicesTableLib
+  FdtLib
   MemoryAllocationLib
   PcdLib
   PrintLib
@@ -56,3 +57,6 @@
 [Packages.ARM, Packages.AARCH64]
   ArmPkg/ArmPkg.dec
   ArmPlatformPkg/ArmPlatformPkg.dec
+
+[Guids]
+  gFdtTableGuid
diff --git a/EmbeddedPkg/Application/AndroidFastboot/Arm/BootAndroidBootImg.c b/EmbeddedPkg/Application/AndroidFastboot/Arm/BootAndroidBootImg.c
index acedd3e..04d175b 100644
--- a/EmbeddedPkg/Application/AndroidFastboot/Arm/BootAndroidBootImg.c
+++ b/EmbeddedPkg/Application/AndroidFastboot/Arm/BootAndroidBootImg.c
@@ -15,16 +15,21 @@
 #include "AndroidFastbootApp.h"
 
 #include <Protocol/DevicePath.h>
+#include <Protocol/LoadedImage.h>
 
 #include <Library/BdsLib.h>
 #include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PrintLib.h>
 #include <Library/UefiBootServicesTableLib.h>
 #include <Library/UefiLib.h>
 
-#define LINUX_LOADER_COMMAND_LINE       L"%s -f %s -c %s"
+#include <libfdt.h>
 
-// This GUID is defined in the INGF file of ArmPkg/Application/LinuxLoader
-CONST EFI_GUID mLinuxLoaderAppGuid = { 0x701f54f2, 0x0d70, 0x4b89, { 0xbc, 0x0a, 0xd9, 0xca, 0x25, 0x37, 0x90, 0x59 }};
+#define ALIGN(x, a)     (((x) + ((a) - 1)) & ~((a) - 1))
+
+// Additional size that could be used for FDT entries added by the UEFI OS Loader
+#define FDT_ADDITIONAL_ENTRIES_SIZE       0x400
 
 // Device Path representing an image in memory
 #pragma pack(1)
@@ -34,6 +39,12 @@ typedef struct {
 } MEMORY_DEVICE_PATH;
 #pragma pack()
 
+/* It's the value of arm64 efi stub kernel */
+#define KERNEL_IMAGE_STEXT_OFFSET         0x12C
+#define KERNEL_IMAGE_RAW_SIZE_OFFSET      0x130
+
+#define FDT_SIZE_OFFSET                   0x4
+
 STATIC CONST MEMORY_DEVICE_PATH MemoryDevicePathTemplate =
 {
   {
@@ -56,9 +67,114 @@ STATIC CONST MEMORY_DEVICE_PATH MemoryDevicePathTemplate =
 };
 
 EFI_STATUS
+PrepareFdt (
+  IN EFI_PHYSICAL_ADDRESS             FdtBlobBase,
+  IN UINTN                           *FdtBlobSize,
+  IN OUT CHAR16                      *KernelArgs
+  )
+{
+  VOID                               *fdt;
+  INTN                                err;
+  INTN                                node;
+  INT32                               lenp;
+  CONST VOID                         *BootArg;
+  UINTN                               OriginalFdtSize;
+  EFI_STATUS                          Status;
+  EFI_PHYSICAL_ADDRESS                NewFdtBlobBase;
+  UINTN                               NewFdtBlobSize;
+  CHAR16                              Arg[BOOTIMG_KERNEL_ARGS_SIZE];
+  UINTN                               Size;
+
+  //
+  // Sanity checks on the original FDT blob.
+  //
+  err = fdt_check_header ((VOID*)(UINTN)FdtBlobBase);
+  if (err != 0) {
+    Print (L"ERROR: Device Tree header not valid (err:%d)\n", err);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  // The original FDT blob might have been loaded partially.
+  // Check that it is not the case.
+  OriginalFdtSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)FdtBlobBase);
+  if (OriginalFdtSize > *FdtBlobSize) {
+    Print (L"ERROR: Incomplete FDT. Only %d/%d bytes have been loaded.\n",
+           FdtBlobSize, OriginalFdtSize);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Relocate the FDT to its final location since some platform may update FDT.
+  //
+  Size = OriginalFdtSize + FDT_ADDITIONAL_ENTRIES_SIZE;
+  NewFdtBlobSize = ALIGN (Size, EFI_PAGE_SIZE);
+
+  // Try anywhere there is available space.
+  Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData,
+                  EFI_SIZE_TO_PAGES (NewFdtBlobSize), &NewFdtBlobBase);
+  if (EFI_ERROR (Status)) {
+    ASSERT_EFI_ERROR (Status);
+    return EFI_OUT_OF_RESOURCES;
+  } else {
+    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));
+  }
+
+  // Load the Original FDT tree into the new region
+  err = fdt_open_into ((VOID*)(UINTN) FdtBlobBase,
+            (VOID*)(UINTN)(NewFdtBlobBase), NewFdtBlobSize);
+  if (err) {
+    DEBUG ((EFI_D_ERROR, "fdt_open_into(): %a\n", fdt_strerror (err)));
+    gBS->FreePages (NewFdtBlobBase, EFI_SIZE_TO_PAGES (NewFdtBlobSize));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  // If we succeeded to generate the new Device Tree then free the old Device Tree
+  gBS->FreePages (FdtBlobBase, EFI_SIZE_TO_PAGES (OriginalFdtSize));
+
+  fdt = (VOID*)(UINTN)NewFdtBlobBase;
+
+  node = fdt_subnode_offset (fdt, 0, "chosen");
+  if (node < 0) {
+    // The 'chosen' node does not exist, create it
+    node = fdt_add_subnode (fdt, 0, "chosen");
+    if (node < 0) {
+      DEBUG ((EFI_D_ERROR, "Error on finding 'chosen' node\n"));
+      Status = EFI_INVALID_PARAMETER;
+      goto FAIL_COMPLETE_FDT;
+    }
+  }
+
+  // Merge bootargs into command line arguments
+  BootArg = fdt_getprop (fdt, node, "bootargs", &lenp);
+  if (BootArg != NULL) {
+    AsciiStrToUnicodeStrS (BootArg, Arg, BOOTIMG_KERNEL_ARGS_SIZE);
+    // StrCatS() is using the size of CHAR16
+    StrCatS (KernelArgs, BOOTIMG_KERNEL_ARGS_SIZE >> 1, L" ");
+    StrCatS (KernelArgs, BOOTIMG_KERNEL_ARGS_SIZE >> 1, Arg);
+  }
+
+  // Update the real size of the Device Tree
+  fdt_pack ((VOID*)(UINTN)(NewFdtBlobBase));
+
+  *FdtBlobSize = (UINTN)fdt_totalsize ((VOID*)(UINTN)(NewFdtBlobBase));
+
+  Status = gBS->InstallConfigurationTable (
+                  &gFdtTableGuid,
+                  (VOID *)(UINTN)NewFdtBlobBase
+                  );
+  return Status;
+
+FAIL_COMPLETE_FDT:
+  gBS->FreePages (NewFdtBlobBase, EFI_SIZE_TO_PAGES (NewFdtBlobSize));
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
 BootAndroidBootImg (
-  IN UINTN    BufferSize,
-  IN VOID    *Buffer
+  IN FASTBOOT_PLATFORM_PROTOCOL      *Platform,
+  IN UINTN                            BufferSize,
+  IN VOID                            *Buffer
   )
 {
   EFI_STATUS                          Status;
@@ -68,11 +184,13 @@ BootAndroidBootImg (
   VOID                               *Ramdisk;
   UINTN                               RamdiskSize;
   MEMORY_DEVICE_PATH                  KernelDevicePath;
-  MEMORY_DEVICE_PATH*                 RamdiskDevicePath;
-  CHAR16*                             KernelDevicePathTxt;
-  CHAR16*                             RamdiskDevicePathTxt;
-  EFI_DEVICE_PATH*                    LinuxLoaderDevicePath;
-  CHAR16*                             LoadOptions;
+  EFI_HANDLE                          ImageHandle;
+  EFI_PHYSICAL_ADDRESS                FdtBase;
+  UINTN                               FdtSize, Index;
+  UINT8                              *FdtPtr;
+  VOID                               *NewKernelArg;
+  EFI_LOADED_IMAGE_PROTOCOL          *ImageInfo;
+  CHAR16                             *PlatformKernelArgs;
 
   Status = ParseAndroidBootImg (
             Buffer,
@@ -86,62 +204,64 @@ BootAndroidBootImg (
     return Status;
   }
 
-  KernelDevicePath = MemoryDevicePathTemplate;
-
-  // Have to cast to UINTN before casting to EFI_PHYSICAL_ADDRESS in order to
-  // appease GCC.
-  KernelDevicePath.Node1.StartingAddress = (EFI_PHYSICAL_ADDRESS)(UINTN) Kernel;
-  KernelDevicePath.Node1.EndingAddress   = (EFI_PHYSICAL_ADDRESS)(UINTN) Kernel + KernelSize;
-
-  RamdiskDevicePath = NULL;
-  if (RamdiskSize != 0) {
-    RamdiskDevicePath = (MEMORY_DEVICE_PATH*)DuplicateDevicePath ((EFI_DEVICE_PATH_PROTOCOL*) &MemoryDevicePathTemplate);
+  /* Install Fdt that is attached at the end of kernel */
+  KernelSize = *(UINT32 *)((EFI_PHYSICAL_ADDRESS)(UINTN)Kernel + KERNEL_IMAGE_STEXT_OFFSET) +
+               *(UINT32 *)((EFI_PHYSICAL_ADDRESS)(UINTN)Kernel + KERNEL_IMAGE_RAW_SIZE_OFFSET);
 
-    RamdiskDevicePath->Node1.StartingAddress = (EFI_PHYSICAL_ADDRESS)(UINTN) Ramdisk;
-    RamdiskDevicePath->Node1.EndingAddress   = ((EFI_PHYSICAL_ADDRESS)(UINTN) Ramdisk) + RamdiskSize;
+  /* FDT is at the end of kernel image */
+  FdtBase = (EFI_PHYSICAL_ADDRESS)(UINTN)Kernel + KernelSize;
+  FdtPtr = (UINT8 *)(FdtBase + FDT_SIZE_OFFSET);
+  for (Index = 0, FdtSize = 0; Index < sizeof (UINT32); Index++) {
+    FdtSize |= *FdtPtr << ((sizeof (UINT32) - 1 - Index) * 8);
+    FdtPtr++;
   }
 
-  //
-  // Boot Linux using the Legacy Linux Loader
-  //
-
-  Status = LocateEfiApplicationInFvByGuid (&mLinuxLoaderAppGuid, &LinuxLoaderDevicePath);
-  if (EFI_ERROR (Status)) {
-    Print (L"Couldn't Boot Linux: %d\n", Status);
-    return EFI_DEVICE_ERROR;
-  }
-
-  KernelDevicePathTxt = ConvertDevicePathToText ((EFI_DEVICE_PATH_PROTOCOL *) &KernelDevicePath, FALSE, FALSE);
-  if (KernelDevicePathTxt == NULL) {
-    return EFI_OUT_OF_RESOURCES;
-  }
-
-  RamdiskDevicePathTxt = ConvertDevicePathToText ((EFI_DEVICE_PATH_PROTOCOL *) RamdiskDevicePath, FALSE, FALSE);
-  if (RamdiskDevicePathTxt == NULL) {
+  NewKernelArg = AllocateZeroPool (BOOTIMG_KERNEL_ARGS_SIZE);
+  if (NewKernelArg == NULL) {
+    DEBUG ((DEBUG_ERROR, "Fail to allocate memory\n"));
     return EFI_OUT_OF_RESOURCES;
   }
 
-  // Initialize Legacy Linux loader command line
-  LoadOptions = CatSPrint (NULL, LINUX_LOADER_COMMAND_LINE, KernelDevicePathTxt, RamdiskDevicePathTxt, KernelArgs);
-  if (LoadOptions == NULL) {
-    return EFI_OUT_OF_RESOURCES;
+  // Set the ramdisk in command line arguments
+  UnicodeSPrint (
+    (CHAR16 *)NewKernelArg, BOOTIMG_KERNEL_ARGS_SIZE,
+    L"initrd=0x%x,0x%x ",
+    (UINTN)Ramdisk, (UINTN)RamdiskSize
+    );
+  // Merge kernel arguments from Android boot image into command line arguments
+  AsciiStrToUnicodeStrS (KernelArgs, NewKernelArg + StrLen (NewKernelArg) * sizeof (CHAR16), BOOTIMG_KERNEL_ARGS_SIZE >> 1);
+  // StrCatS() is using the size of CHAR16
+  StrCatS ((CHAR16 *)NewKernelArg, BOOTIMG_KERNEL_ARGS_SIZE >> 1, L" ");
+  // Merge platform arguemnts into command line arguments
+  PlatformKernelArgs = Platform->GetKernelArgs ();
+  if (PlatformKernelArgs) {
+    StrCatS ((CHAR16 *)NewKernelArg, BOOTIMG_KERNEL_ARGS_SIZE >> 1, PlatformKernelArgs);
   }
-
-  Status = BdsStartEfiApplication (gImageHandle, LinuxLoaderDevicePath, StrSize (LoadOptions), LoadOptions);
+  Status = PrepareFdt (FdtBase, &FdtSize, NewKernelArg);
   if (EFI_ERROR (Status)) {
-    DEBUG ((EFI_D_ERROR, "Couldn't Boot Linux: %d\n", Status));
-    return EFI_DEVICE_ERROR;
+    FreePool (NewKernelArg);
+    return EFI_INVALID_PARAMETER;
   }
 
-  if (RamdiskDevicePath) {
-    FreePool (RamdiskDevicePathTxt);
-    FreePool (RamdiskDevicePath);
-  }
+  KernelDevicePath = MemoryDevicePathTemplate;
+
+  // Have to cast to UINTN before casting to EFI_PHYSICAL_ADDRESS in order to
+  // appease GCC.
+  KernelDevicePath.Node1.StartingAddress = (EFI_PHYSICAL_ADDRESS)(UINTN) Kernel;
+  KernelDevicePath.Node1.EndingAddress   = (EFI_PHYSICAL_ADDRESS)(UINTN) Kernel + KernelSize;
+
+  Status = gBS->LoadImage (TRUE, gImageHandle, (EFI_DEVICE_PATH *)&KernelDevicePath, (VOID*)(UINTN)Kernel, KernelSize, &ImageHandle);
 
-  FreePool (KernelDevicePathTxt);
+  // Set kernel arguments
+  Status = gBS->HandleProtocol (ImageHandle, &gEfiLoadedImageProtocolGuid, (VOID **) &ImageInfo);
+  ImageInfo->LoadOptions = NewKernelArg;
+  ImageInfo->LoadOptionsSize = StrLen (NewKernelArg) * sizeof (CHAR16);
 
-  // If we got here we do a confused face because BootLinuxFdt returned,
-  // reporting success.
-  DEBUG ((EFI_D_ERROR, "WARNING: BdsBootLinuxFdt returned EFI_SUCCESS.\n"));
+  // Before calling the image, enable the Watchdog Timer for  the 5 Minute period
+  gBS->SetWatchdogTimer (5 * 60, 0x0000, 0x00, NULL);
+  // Start the image
+  Status = gBS->StartImage (ImageHandle, NULL, NULL);
+  // Clear the Watchdog Timer after the image returns
+  gBS->SetWatchdogTimer (0x0000, 0x0000, 0x0000, NULL);
   return EFI_SUCCESS;
 }
-- 
2.7.4