edk2: QcomModulePkg: Support new Header Version 2 of the bootimage

Bootimage header version v2 changes the way dtb's are appended. Boot
image with header version v2 will not have kernel Image appended dtb but
it is added to boot.img at an offset. It requires a new version to
maintain backward compatibility of images.

Change-Id: I8be2f4fe881180eaf7e60f998ed389a72f3a3b1c

6 files changed, 197 insertions, 28 deletions

diff --git a/QcomModulePkg/Include/Library/BootImage.h b/QcomModulePkg/Include/Library/BootImage.h
index b430c35410..632fafd1bf 100644
--- a/QcomModulePkg/Include/Library/BootImage.h
+++ b/QcomModulePkg/Include/Library/BootImage.h
@@ -151,6 +151,51 @@ struct boot_img_hdr_v1 {
  *    else: jump to kernel_addr
  */
 
+#define BOOT_IMAGE_HEADER_V2_OFFSET sizeof (struct boot_img_hdr_v1)
+#define BOOT_HEADER_VERSION_TWO 2
+
+struct boot_img_hdr_v2 {
+  UINT32 dtb_size; /* size in bytes for DTB image */
+  UINT64 dtb_addr; /* physical load address for DTB image */
+} __attribute__((packed));
+
+/* When the boot image header has a version of BOOT_HEADER_VERSION_TWO,
+ * the structure of the boot image is as follows:
+ *
+ * +-----------------+
+ * | boot header     | 1 page
+ * +-----------------+
+ * | kernel          | n pages
+ * +-----------------+
+ * | ramdisk         | m pages
+ * +-----------------+
+ * | second stage    | o pages
+ * +-----------------+
+ * | recovery dtbo   | p pages
+ * +-----------------+
+ * | dtb.img         | q pages
+ * +-----------------+
+ *
+ * n = (kernel_size + page_size - 1) / page_size
+ * m = (ramdisk_size + page_size - 1) / page_size
+ * o = (second_size + page_size - 1) / page_size
+ * p = (recovery_dtbo_size + page_size - 1) / page_size
+ * q = (dtb_size + page_size - 1) / page_size
+ *
+ * 0. all entities are page_size aligned in flash
+ * 1. kernel and ramdisk are required (size != 0)
+ * 2. recovery_dtbo is required for recovery.img (recovery_dtbo_size != 0)
+ * 3. second is optional (second_size == 0 -> no second)
+ * 4. dtb.img has all the dtbs catted one after the other
+ * 5. load each element (kernel, ramdisk, second, recovery_dtbo) at
+ *    the specified physical address (kernel_addr, etc)
+ * 6. prepare tags at tag_addr.  kernel_args[] is
+ *    appended to the kernel commandline in the tags.
+ * 7. r0 = 0, r1 = MACHINE_TYPE, r2 = tags_addr
+ * 8. if second_size != 0: jump to second_addr
+ *    else: jump to kernel_addr
+ */
+
 struct kernel64_hdr {
   UINT32 Code0;       /* Executable code */
   UINT32 Code1;       /* Executable code */
diff --git a/QcomModulePkg/Include/Library/LocateDeviceTree.h b/QcomModulePkg/Include/Library/LocateDeviceTree.h
index fcc0cbc3a9..b08120af13 100644
--- a/QcomModulePkg/Include/Library/LocateDeviceTree.h
+++ b/QcomModulePkg/Include/Library/LocateDeviceTree.h
@@ -157,6 +157,7 @@ struct dt_entry {
   UINT32 pmic_rev[4];
   UINT64 offset;
   UINT32 size;
+  UINT32 Idx;
 };
 
 /*Struct def for device tree entry*/
@@ -272,6 +273,7 @@ DeviceTreeValidate (UINT8 *DeviceTreeBuff,
                     UINT32 PageSize,
                     UINT32 *DeviceTreeSize);
 INT32 GetDtboIdx (VOID);
+INT32 GetDtbIdx (VOID);
 VOID DeleteDtList (struct fdt_entry_node** DtList);
 BOOLEAN AppendToDtList (struct fdt_entry_node **DtList,
                 UINT64 Address,
diff --git a/QcomModulePkg/Include/Library/UpdateCmdLine.h b/QcomModulePkg/Include/Library/UpdateCmdLine.h
index e535680512..89ca8467d3 100644
--- a/QcomModulePkg/Include/Library/UpdateCmdLine.h
+++ b/QcomModulePkg/Include/Library/UpdateCmdLine.h
@@ -3,7 +3,7 @@
  * Copyright (c) 2009, Google Inc.
  * All rights reserved.
  *
- * Copyright (c) 2009-2018, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2009-2019, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -77,6 +77,7 @@ typedef struct UpdateCmdLineParamList {
   CHAR8 *RootCmdLine;
   CHAR8 *InitCmdline;
   CHAR8 *DtboIdxStr;
+  CHAR8 *DtbIdxStr;
   CHAR8 *LEVerityCmdLine;
   CHAR8 *CvmSystemPtnCmdLine;
 } UpdateCmdLineParamList;
diff --git a/QcomModulePkg/Library/BootLib/BootLinux.c b/QcomModulePkg/Library/BootLib/BootLinux.c
index 95949bec96..e95f2e18fe 100644
--- a/QcomModulePkg/Library/BootLib/BootLinux.c
+++ b/QcomModulePkg/Library/BootLib/BootLinux.c
@@ -367,6 +367,12 @@ out:
   return EFI_SUCCESS;
 }
 
+STATIC UINT32
+GetNumberOfPages (UINT32 ImageSize, UINT32 PageSize)
+{
+   return (ImageSize + PageSize - 1) / PageSize;
+}
+
 STATIC EFI_STATUS
 DTBImgCheckAndAppendDT (BootInfo *Info, BootParamlist *BootParamlistPtr)
 {
@@ -380,6 +386,16 @@ DTBImgCheckAndAppendDT (BootInfo *Info, BootParamlist *BootParamlistPtr)
   BOOLEAN DtboImgInvalid = FALSE;
   struct fdt_entry_node *DtsList = NULL;
   EFI_STATUS Status;
+  UINT32 HeaderVersion = 0;
+  struct boot_img_hdr_v1 *BootImgHdrV1;
+  struct boot_img_hdr_v2 *BootImgHdrV2;
+  UINT32 NumHeaderPages;
+  UINT32 NumKernelPages;
+  UINT32 NumSecondPages;
+  UINT32 NumRamdiskPages;
+  UINT32 NumRecoveryDtboPages;
+  VOID* ImageBuffer = NULL;
+  UINT32 DtbSize = 0;
 
   if (Info == NULL ||
       BootParamlistPtr == NULL) {
@@ -387,18 +403,52 @@ DTBImgCheckAndAppendDT (BootInfo *Info, BootParamlist *BootParamlistPtr)
     return EFI_INVALID_PARAMETER;
   }
 
+  ImageBuffer = BootParamlistPtr->ImageBuffer +
+                        BootParamlistPtr->PageSize +
+                        BootParamlistPtr->PatchedKernelHdrSize;
+  DtbSize = BootParamlistPtr->KernelSize;
+  HeaderVersion = Info->HeaderVersion;
+
+  if (HeaderVersion > BOOT_HEADER_VERSION_ONE) {
+        BootImgHdrV1 = (struct boot_img_hdr_v1 *)
+                ((UINT64) BootParamlistPtr->ImageBuffer +
+                BOOT_IMAGE_HEADER_V1_RECOVERY_DTBO_SIZE_OFFSET);
+        BootImgHdrV2 = (struct boot_img_hdr_v2 *)
+            ((UINT64) BootParamlistPtr->ImageBuffer +
+            BOOT_IMAGE_HEADER_V1_RECOVERY_DTBO_SIZE_OFFSET +
+            BOOT_IMAGE_HEADER_V2_OFFSET);
+
+        NumHeaderPages = 1;
+        NumKernelPages =
+                GetNumberOfPages (BootParamlistPtr->KernelSize,
+                        BootParamlistPtr->PageSize);
+        NumRamdiskPages =
+                GetNumberOfPages (BootParamlistPtr->RamdiskSize,
+                        BootParamlistPtr->PageSize);
+        NumSecondPages =
+                GetNumberOfPages (BootParamlistPtr->SecondSize,
+                        BootParamlistPtr->PageSize);
+        NumRecoveryDtboPages =
+                GetNumberOfPages (BootImgHdrV1->recovery_dtbo_size,
+                        BootParamlistPtr->PageSize);
+        BootParamlistPtr->DtbOffset =
+                BootParamlistPtr->PageSize *
+                        (NumHeaderPages + NumKernelPages + NumRamdiskPages
+                                + NumSecondPages + NumRecoveryDtboPages);
+        DtbSize = BootImgHdrV2->dtb_size + BootParamlistPtr->DtbOffset;
+        ImageBuffer = BootParamlistPtr->ImageBuffer;
+  }
+
   DtboImgInvalid = LoadAndValidateDtboImg (Info, BootParamlistPtr);
   if (!DtboImgInvalid) {
     // appended device tree
-    Dtb = DeviceTreeAppended ((VOID *)(BootParamlistPtr->ImageBuffer +
-                             BootParamlistPtr->PageSize +
-                             BootParamlistPtr->PatchedKernelHdrSize),
-                             BootParamlistPtr->KernelSize,
+    Dtb = DeviceTreeAppended (ImageBuffer,
+                             DtbSize,
                              BootParamlistPtr->DtbOffset,
                              (VOID *)BootParamlistPtr->DeviceTreeLoadAddr);
     if (!Dtb) {
-      if (BootParamlistPtr->DtbOffset >= BootParamlistPtr->KernelSize) {
-        DEBUG ((EFI_D_ERROR, "Dtb offset goes beyond the kernel size\n"));
+      if (BootParamlistPtr->DtbOffset >= DtbSize) {
+        DEBUG ((EFI_D_ERROR, "Dtb offset goes beyond the image size\n"));
         return EFI_BAD_BUFFER_SIZE;
       }
       SingleDtHdr = (BootParamlistPtr->ImageBuffer +
@@ -406,9 +456,9 @@ DTBImgCheckAndAppendDT (BootInfo *Info, BootParamlist *BootParamlistPtr)
                      BootParamlistPtr->DtbOffset);
 
       if (!fdt_check_header (SingleDtHdr)) {
-        if ((BootParamlistPtr->KernelSize - BootParamlistPtr->DtbOffset) <
+        if ((DtbSize - BootParamlistPtr->DtbOffset) <
             fdt_totalsize (SingleDtHdr)) {
-          DEBUG ((EFI_D_ERROR, "Dtb offset goes beyond the kernel size\n"));
+          DEBUG ((EFI_D_ERROR, "Dtb offset goes beyond the image size\n"));
           return EFI_BAD_BUFFER_SIZE;
         }
 
@@ -430,19 +480,17 @@ DTBImgCheckAndAppendDT (BootInfo *Info, BootParamlist *BootParamlistPtr)
         gBS->CopyMem ((VOID *)BootParamlistPtr->DeviceTreeLoadAddr,
                       SingleDtHdr, fdt_totalsize (SingleDtHdr));
       } else {
-        DEBUG ((EFI_D_ERROR, "Error: Appended Device Tree blob not found\n"));
+        DEBUG ((EFI_D_ERROR, "Error: Device Tree blob not found\n"));
         return EFI_NOT_FOUND;
       }
     }
   } else {
     /*It is the case of DTB overlay Get the Soc specific dtb */
-    SocDtb =
-    GetSocDtb ((VOID *)(BootParamlistPtr->ImageBuffer +
-               BootParamlistPtr->PageSize +
-               BootParamlistPtr->PatchedKernelHdrSize),
-               BootParamlistPtr->KernelSize,
-               BootParamlistPtr->DtbOffset,
-               (VOID *)BootParamlistPtr->DeviceTreeLoadAddr);
+    SocDtb = GetSocDtb (ImageBuffer,
+         DtbSize,
+         BootParamlistPtr->DtbOffset,
+         (VOID *)BootParamlistPtr->DeviceTreeLoadAddr);
+
     if (!SocDtb) {
       DEBUG ((EFI_D_ERROR,
                   "Error: Appended Soc Device Tree blob not found\n"));
@@ -587,6 +635,8 @@ GZipPkgCheck (BootParamlist *BootParamlistPtr)
            ((VOID *)Kptr + DTB_OFFSET_LOCATION_IN_ARCH32_KERNEL_HDR),
            sizeof (BootParamlistPtr->DtbOffset));
     }
+    gBS->CopyMem ((VOID *)BootParamlistPtr->KernelLoadAddr, (VOID *)Kptr,
+                 BootParamlistPtr->KernelSize);
   }
 
   if (Kptr->magic_64 != KERNEL64_HDR_MAGIC) {
@@ -1325,19 +1375,51 @@ CheckImageHeader (VOID *ImageHdrBuffer,
   }
 
   if (BootIntoRecovery &&
-      HeaderVersion == BOOT_HEADER_VERSION_ONE) {
+      HeaderVersion > BOOT_HEADER_VERSION_ZERO) {
+
     struct boot_img_hdr_v1 *Hdr1 =
-        (struct boot_img_hdr_v1 *) (ImageHdrBuffer + sizeof (boot_img_hdr));
+      (struct boot_img_hdr_v1 *) (ImageHdrBuffer + sizeof (boot_img_hdr));
     UINT32 RecoveryDtboActual = 0;
 
-    RecoveryDtboActual = ROUND_TO_PAGE (Hdr1->recovery_dtbo_size,
+    if (HeaderVersion == BOOT_HEADER_VERSION_ONE) {
+        if ((Hdr1->header_size !=
+          sizeof (struct boot_img_hdr_v1) + sizeof (boot_img_hdr))) {
+           DEBUG ((EFI_D_ERROR,
+             "Invalid boot image header: %d\n", Hdr1->header_size));
+           return EFI_BAD_BUFFER_SIZE;
+        }
+    }
+    else {
+        UINT32 DtbActual = 0;
+        struct boot_img_hdr_v2 *Hdr2 = (struct boot_img_hdr_v2 *)
+            (ImageHdrBuffer +
+            BOOT_IMAGE_HEADER_V1_RECOVERY_DTBO_SIZE_OFFSET +
+            BOOT_IMAGE_HEADER_V2_OFFSET);
+        DtbActual = ROUND_TO_PAGE (Hdr2->dtb_size,
                                         *PageSize - 1);
-    if ((Hdr1->header_size !=
-         sizeof (struct boot_img_hdr_v1) + sizeof (boot_img_hdr))) {
-      DEBUG ((EFI_D_ERROR,
+        if ((Hdr1->header_size !=
+                        BOOT_IMAGE_HEADER_V1_RECOVERY_DTBO_SIZE_OFFSET +
+                        BOOT_IMAGE_HEADER_V2_OFFSET +
+                        sizeof (struct boot_img_hdr_v2))) {
+           DEBUG ((EFI_D_ERROR,
               "Invalid boot image header: %d\n", Hdr1->header_size));
-      return EFI_BAD_BUFFER_SIZE;
+           return EFI_BAD_BUFFER_SIZE;
+        }
+        if (Hdr2->dtb_size && !DtbActual) {
+           DEBUG ((EFI_D_ERROR,
+               "DTB Image not present: DTB Size = %u\n", Hdr2->dtb_size));
+           return EFI_BAD_BUFFER_SIZE;
+        }
+        tempImgSize = *ImageSizeActual;
+        *ImageSizeActual = ADD_OF (*ImageSizeActual, DtbActual);
+        if (!*ImageSizeActual) {
+           DEBUG ((EFI_D_ERROR, "Integer Overflow: ImgSizeActual=%u,"
+              " DtbActual=%u\n", tempImgSize, DtbActual));
+           return EFI_BAD_BUFFER_SIZE;
+        }
     }
+    RecoveryDtboActual = ROUND_TO_PAGE (Hdr1->recovery_dtbo_size,
+                                        *PageSize - 1);
 
     if (RecoveryDtboActual > DTBO_MAX_SIZE_ALLOWED) {
       DEBUG ((EFI_D_ERROR, "Recovery Dtbo Size too big %x, Allowed size %x\n",
diff --git a/QcomModulePkg/Library/BootLib/LocateDeviceTree.c b/QcomModulePkg/Library/BootLib/LocateDeviceTree.c
index cedc1b1ca8..43efc78d5d 100644
--- a/QcomModulePkg/Library/BootLib/LocateDeviceTree.c
+++ b/QcomModulePkg/Library/BootLib/LocateDeviceTree.c
@@ -48,6 +48,12 @@ INT32 GetDtboIdx (VOID)
    return DtboIdx;
 }
 
+STATIC INT32 DtbIdx = INVALID_PTN;
+INT32 GetDtbIdx (VOID)
+{
+   return DtbIdx;
+}
+
 BOOLEAN GetDtboNeeded (VOID)
 {
   return DtboNeed;
@@ -128,6 +134,7 @@ DeviceTreeCompatible (VOID *dtb,
   UINT32 board_data_count;
   UINT32 pmic_data_count;
   BOOLEAN Result = FALSE;
+  static UINT32 DtbCount;
 
   root_offset = fdt_path_offset (dtb, "/");
   if (root_offset < 0)
@@ -297,6 +304,7 @@ DeviceTreeCompatible (VOID *dtb,
      * <Z ,A >;<Z, B>;<Z, C>;
      */
     k = 0;
+    DtbCount++;
     for (i = 0; i < msm_data_count; i++) {
       for (j = 0; j < board_data_count; j++) {
         if (dtb_ver == DEV_TREE_VERSION_V3 && pmic_prop) {
@@ -311,6 +319,7 @@ DeviceTreeCompatible (VOID *dtb,
             dt_entry_array[k].pmic_rev[3] = pmic_data[n].pmic_version[3];
             dt_entry_array[k].offset = (UINT64)dtb;
             dt_entry_array[k].size = dtb_size;
+            dt_entry_array[k].Idx = DtbCount;
             k++;
           }
 
@@ -325,6 +334,7 @@ DeviceTreeCompatible (VOID *dtb,
           dt_entry_array[k].pmic_rev[3] = BoardPmicTarget (3);
           dt_entry_array[k].offset = (UINT64)dtb;
           dt_entry_array[k].size = dtb_size;
+          dt_entry_array[k].Idx = DtbCount;
           k++;
         }
       }
@@ -380,9 +390,13 @@ Exit:
  * Will relocate the DTB to the tags addr if the device tree is found and return
  * its address
  *
- * Arguments:    kernel - Start address of the kernel loaded in RAM
+ * For Header Version 2, the arguments Kernel and KernelSize will be
+ * the entire bootimage and the bootimage size.
+ *
+ * Arguments:    kernel - Start address of the kernel/bootimage
+ *                                loaded in RAM
  *               tags - Start address of the tags loaded in RAM
- *               kernel_size - Size of the kernel in bytes
+ *               kernel_size - Size of the kernel/bootimage in bytes
  *
  * Return Value: DTB address : If appended device tree is found
  *               'NULL'         : Otherwise
@@ -502,6 +516,7 @@ DeviceTreeAppended (VOID *kernel,
       goto out;
     }
     gBS->CopyMem (tags, bestmatch_tag, bestmatch_tag_size);
+    DtbIdx = best_match_dt_entry->Idx;
     /* clear out the old DTB magic so kernel doesn't find it */
     *((UINT32 *)(kernel + dtb_offset)) = 0;
     FreePool (dt_entry_queue);
@@ -896,7 +911,10 @@ cleanup:
 
   return FindBestMatch;
 }
-
+/*
+ * For Header Version 2, the arguments Kernel and KernelSize will be
+ * the entire bootimage and the bootimage size.
+ */
 VOID *
 GetSocDtb (VOID *Kernel, UINT32 KernelSize, UINT32 DtbOffset, VOID *DtbLoadAddr)
 {
@@ -904,6 +922,7 @@ GetSocDtb (VOID *Kernel, UINT32 KernelSize, UINT32 DtbOffset, VOID *DtbLoadAddr)
   VOID *Dtb = NULL;
   struct fdt_header DtbHdr;
   UINT32 DtbSize = 0;
+  INT32 DtbCount = 0;
   DtInfo CurDtbInfo = {0};
   DtInfo BestDtbInfo = {0};
   if (!DtbOffset) {
@@ -927,7 +946,10 @@ GetSocDtb (VOID *Kernel, UINT32 KernelSize, UINT32 DtbOffset, VOID *DtbLoadAddr)
       break;
 
     CurDtbInfo.Dtb = Dtb;
-    ReadDtbFindMatch (&CurDtbInfo, &BestDtbInfo, SOC_MATCH);
+    if (ReadDtbFindMatch (&CurDtbInfo, &BestDtbInfo, SOC_MATCH)) {
+        DtbIdx = DtbCount;
+    }
+
     if (CurDtbInfo.DtMatchVal) {
       if (CurDtbInfo.DtMatchVal & BIT (SOC_MATCH)) {
         if (CheckAllBitsSet (CurDtbInfo.DtMatchVal)) {
@@ -946,6 +968,7 @@ GetSocDtb (VOID *Kernel, UINT32 KernelSize, UINT32 DtbOffset, VOID *DtbLoadAddr)
 
     DEBUG ((EFI_D_VERBOSE, "Bestmatch = %x\n", BestDtbInfo.DtMatchVal));
     Dtb += DtbSize;
+    DtbCount++;
   }
 
   if (!BestDtbInfo.Dtb) {
diff --git a/QcomModulePkg/Library/BootLib/UpdateCmdLine.c b/QcomModulePkg/Library/BootLib/UpdateCmdLine.c
index 6ef1186570..467cb5c050 100644
--- a/QcomModulePkg/Library/BootLib/UpdateCmdLine.c
+++ b/QcomModulePkg/Library/BootLib/UpdateCmdLine.c
@@ -72,6 +72,7 @@ STATIC UINTN DisplayCmdLineLen = sizeof (DisplayCmdLine);
 
 #define MAX_DTBO_IDX_STR 64
 STATIC CHAR8 *AndroidBootDtboIdx = " androidboot.dtbo_idx=";
+STATIC CHAR8 *AndroidBootDtbIdx = " androidboot.dtb_idx=";
 
 STATIC EFI_STATUS
 TargetPauseForBatteryCharge (BOOLEAN *BatteryStatus)
@@ -499,6 +500,11 @@ UpdateCmdLineParams (UpdateCmdLineParamList *Param,
     AsciiStrCatS (Dst, MaxCmdLineLen, Src);
   }
 
+  if (Param->DtbIdxStr != NULL) {
+    Src = Param->DtbIdxStr;
+    AsciiStrCatS (Dst, MaxCmdLineLen, Src);
+  }
+
   if (Param->LEVerityCmdLine != NULL) {
     Src = Param->LEVerityCmdLine;
     AsciiStrCatS (Dst, MaxCmdLineLen, Src);
@@ -538,7 +544,9 @@ UpdateCmdLine (CONST CHAR8 *CmdLine,
   CHAR8 *CvmSystemPtnCmdLine = NULL;
   UpdateCmdLineParamList Param = {0};
   CHAR8 DtboIdxStr[MAX_DTBO_IDX_STR] = "\0";
+  CHAR8 DtbIdxStr[MAX_DTBO_IDX_STR] = "\0";
   INT32 DtboIdx = INVALID_PTN;
+  INT32 DtbIdx = INVALID_PTN;
   CHAR8 *LEVerityCmdLine = NULL;
   UINT32 LEVerityCmdLineLen = 0;
 
@@ -665,6 +673,13 @@ UpdateCmdLine (CONST CHAR8 *CmdLine,
                    "%a%d", AndroidBootDtboIdx, DtboIdx);
       CmdLineLen += AsciiStrLen (DtboIdxStr);
     }
+
+    DtbIdx = GetDtbIdx ();
+    if (DtbIdx != INVALID_PTN) {
+      AsciiSPrint (DtbIdxStr, sizeof (DtbIdxStr),
+                   "%a%d", AndroidBootDtbIdx, DtbIdx);
+      CmdLineLen += AsciiStrLen (DtbIdxStr);
+    }
   }
   /* 1 extra byte for NULL */
   CmdLineLen += 1;
@@ -704,6 +719,7 @@ UpdateCmdLine (CONST CHAR8 *CmdLine,
   Param.RootCmdLine = RootCmdLine;
   Param.InitCmdline = InitCmdline;
   Param.DtboIdxStr = DtboIdxStr;
+  Param.DtbIdxStr = DtbIdxStr;
   Param.LEVerityCmdLine = LEVerityCmdLine;
   Param.CvmSystemPtnCmdLine = CvmSystemPtnCmdLine;