OsLoader: Handle module align flag for multiboot images in ELF format

There is a flag in both multiboot and multiboot2 image indicating
modules must be loaded to page boundaries. Currently this flag is not
handled when loading multiboot images in ELF format. Check this flag
and move the loaded modules if needed before SetupMultibootInfo().

Signed-off-by: Jiaqing Zhao <jiaqing.zhao@intel.com>

BootloaderCommonPkg/Include/Library/MultibootLib.h

@@ -296,6 +296,20 @@ SetupMultibootInfo (
   IN OUT MULTIBOOT_IMAGE     *MultiBoot
   );
 
+/**
+  Align multiboot modules if requested by header.
+
+  @param[in,out] MultiBoot    Point to loaded Multiboot image structure
+
+  @retval  RETURN_SUCCESS     Align modules successfully
+  @retval  Others             There is error when align image
+**/
+EFI_STATUS
+EFIAPI
+CheckAndAlignMultibootModules (
+  IN OUT MULTIBOOT_IMAGE     *MultiBoot
+  );
+
 /**
   ASM inline function that goes from payload to a Multiboot enabled OS.
   @param[in]  State  Boot state structure
@@ -346,6 +360,20 @@ SetupMultiboot2Info (
   IN OUT MULTIBOOT_IMAGE     *MultiBoot
   );
 
+/**
+  Align multiboot modules if requested by module alignment tag.
+
+  @param[in,out] MultiBoot    Point to loaded Multiboot image structure
+
+  @retval  RETURN_SUCCESS     Align modules successfully
+  @retval  Others             There is error when align image
+**/
+EFI_STATUS
+EFIAPI
+CheckAndAlignMultiboot2Modules (
+  IN OUT MULTIBOOT_IMAGE     *MultiBoot
+  );
+
 /**
   Update the memory info inside the Multiboot-2 info.
 

BootloaderCommonPkg/Library/MultibootLib/Multiboot.c

@@ -346,6 +346,41 @@ AlignMultibootModules (
   return RETURN_SUCCESS;
 }
 
+/**
+  Align multiboot modules if requested by header.
+
+  @param[in,out] MultiBoot    Point to loaded Multiboot image structure
+
+  @retval  RETURN_SUCCESS     Align modules successfully
+  @retval  Others             There is error when align image
+**/
+EFI_STATUS
+EFIAPI
+CheckAndAlignMultibootModules (
+  IN OUT MULTIBOOT_IMAGE     *MultiBoot
+  )
+{
+  EFI_STATUS                 Status;
+  CONST MULTIBOOT_HEADER     *MbHeader;
+
+  if (MultiBoot == NULL) {
+    return RETURN_INVALID_PARAMETER;
+  }
+
+  MbHeader = GetMultibootHeader (MultiBoot->BootFile.Addr);
+  if (MbHeader == NULL) {
+    return RETURN_LOAD_ERROR;
+  }
+
+  if ((MbHeader->Flags & MULTIBOOT_HEADER_MODS_ALIGNED) != 0) {
+    // Modules should be page (4KB) aligned
+    Status = AlignMultibootModules (MultiBoot);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+  return EFI_SUCCESS;
+}
 
 /**
   Setup Multiboot image and its boot info.

BootloaderCommonPkg/Library/MultibootLib/Multiboot2.c

@@ -423,6 +423,54 @@ ParseMultiboot2Header (
   return (tags_needed ? RETURN_UNSUPPORTED : RETURN_SUCCESS);
 }
 
+/**
+  Align multiboot modules if requested by module alignment tag.
+
+  @param[in,out] MultiBoot    Point to loaded Multiboot image structure
+
+  @retval  RETURN_SUCCESS     Align modules successfully
+  @retval  Others             There is error when align image
+**/
+EFI_STATUS
+EFIAPI
+CheckAndAlignMultiboot2Modules (
+  IN OUT MULTIBOOT_IMAGE     *MultiBoot
+  )
+{
+  EFI_STATUS                 Status;
+  BOOLEAN                    AlignModules;
+  UINT8                      *HeaderAddr;
+  UINT8                      *LoadAddr;
+  UINT8                      *LoadEnd;
+  UINT8                      *BssEnd;
+  UINT32                     EntryPoint;
+  CONST struct multiboot2_header     *MbHeader;
+
+  if (MultiBoot == NULL) {
+    return RETURN_INVALID_PARAMETER;
+  }
+
+  MbHeader = GetMultiboot2Header (MultiBoot->BootFile.Addr);
+  if (MbHeader == NULL) {
+    return RETURN_LOAD_ERROR;
+  }
+
+  Status = ParseMultiboot2Header (MbHeader, &AlignModules, &HeaderAddr, &LoadAddr, &LoadEnd, &BssEnd, &EntryPoint);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (AlignModules) {
+    // Modules should be page (4KB) aligned
+    Status = AlignMultibootModules (MultiBoot);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
 /**
   Setup Multiboot image and its boot info.
 

PayloadPkg/OsLoader/OsLoader.c

@@ -497,10 +497,16 @@ SetupBootImage (
       EntryPoint = PayloadInfo.EntryPoint;
       if (IsMultiboot (BootFile->Addr)) {
         DEBUG ((DEBUG_INFO, "and Image is Multiboot format\n"));
-        SetupMultibootInfo (MultiBoot);
+        Status = CheckAndAlignMultibootModules (MultiBoot);
+        if (!EFI_ERROR (Status)) {
+          SetupMultibootInfo (MultiBoot);
+        }
       } else if (IsMultiboot2 (BootFile->Addr)) {
         DEBUG ((DEBUG_INFO, "and Image is Multiboot-2 format\n"));
-        SetupMultiboot2Info (MultiBoot);
+        Status = CheckAndAlignMultiboot2Modules (MultiBoot);
+        if (!EFI_ERROR (Status)) {
+          SetupMultiboot2Info (MultiBoot);
+        }
       }
       MultiBoot->BootState.EntryPoint = (UINT32)(UINTN)EntryPoint;
     }