>From e923dec23238c3a24c5febdeef080444c7a34798 Mon Sep 17 00:00:00 2001
From: Fu Wei <fu.wei@linaro.org>
Date: Wed, 12 Mar 2014 15:39:26 +0800
Subject: [PATCH] Add multiboot/module command support in linux module for
 aarch64.

---
 grub-core/Makefile.core.def        |   1 +
 grub-core/loader/arm64/linux.c     |  26 +-
 grub-core/loader/arm64/multiboot.c | 590 +++++++++++++++++++++++++++++++++++++
 3 files changed, 610 insertions(+), 7 deletions(-)
 create mode 100644 grub-core/loader/arm64/multiboot.c

diff --git a/grub-core/Makefile.core.def b/grub-core/Makefile.core.def
index 42443bc..b963a62 100644
--- a/grub-core/Makefile.core.def
+++ b/grub-core/Makefile.core.def
@@ -1673,6 +1673,7 @@ module = {
   ia64_efi = loader/ia64/efi/linux.c;
   arm = loader/arm/linux.c;
   arm64 = loader/arm64/linux.c;
+  arm64 = loader/arm64/multiboot.c;
   fdt = lib/fdt.c;
   common = loader/linux.c;
   common = lib/cmdline.c;
diff --git a/grub-core/loader/arm64/linux.c b/grub-core/loader/arm64/linux.c
index 65129c2..dccd104 100644
--- a/grub-core/loader/arm64/linux.c
+++ b/grub-core/loader/arm64/linux.c
@@ -37,10 +37,11 @@ GRUB_MOD_LICENSE ("GPLv3+");
 #define BYTES_TO_PAGES(bytes)   (((bytes) + 0xfff) >> GRUB_EFI_PAGE_SHIFT)
 #define GRUB_EFI_PE_MAGIC	0x5A4D
 
-static grub_efi_guid_t fdt_guid = GRUB_EFI_DEVICE_TREE_GUID;
-
-static grub_dl_t my_mod;
-static int loaded;
+grub_efi_guid_t fdt_guid = GRUB_EFI_DEVICE_TREE_GUID;
+grub_dl_t my_mod;
+int loaded;
+void *fdt;
+void get_fdt (void);
 
 static void *kernel_addr;
 static grub_uint64_t kernel_size;
@@ -52,7 +53,6 @@ static grub_addr_t initrd_start;
 static grub_addr_t initrd_end;
 
 static void *loaded_fdt;
-static void *fdt;
 
 static void *
 get_firmware_fdt (void)
@@ -75,7 +75,7 @@ get_firmware_fdt (void)
   return firmware_fdt;
 }
 
-static void
+void
 get_fdt (void)
 {
   void *raw_fdt;
@@ -464,8 +464,12 @@ fail:
   return grub_errno;
 }
 
-
 static grub_command_t cmd_linux, cmd_initrd, cmd_devicetree;
+static grub_command_t cmd_multiboot, cmd_module;
+extern grub_err_t grub_cmd_multiboot (grub_command_t cmd __attribute__ ((unused)),
+		int argc, char *argv[]);
+extern grub_err_t grub_cmd_module(grub_command_t cmd __attribute__ ((unused)),
+		 int argc, char *argv[]);
 
 GRUB_MOD_INIT (linux)
 {
@@ -476,6 +480,12 @@ GRUB_MOD_INIT (linux)
   cmd_devicetree =
     grub_register_command ("devicetree", grub_cmd_devicetree, 0,
 			   N_("Load DTB file."));
+  cmd_multiboot =
+    grub_register_command ("multiboot", grub_cmd_multiboot, 0,
+			   N_("Load Multiboot Image file."));
+  cmd_module =
+    grub_register_command ("module", grub_cmd_module, 0,
+			   N_("Load module file for Multiboot."));
   my_mod = mod;
 }
 
@@ -484,4 +494,6 @@ GRUB_MOD_FINI (linux)
   grub_unregister_command (cmd_linux);
   grub_unregister_command (cmd_initrd);
   grub_unregister_command (cmd_devicetree);
+  grub_unregister_command (cmd_multiboot);
+  grub_unregister_command (cmd_module);
 }
diff --git a/grub-core/loader/arm64/multiboot.c b/grub-core/loader/arm64/multiboot.c
new file mode 100644
index 0000000..39b29e7
--- /dev/null
+++ b/grub-core/loader/arm64/multiboot.c
@@ -0,0 +1,590 @@
+/*
+ *  GRUB  --  GRand Unified Bootloader
+ *  Copyright (C) 2013  Free Software Foundation, Inc.
+ *
+ *  GRUB is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  GRUB is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with GRUB.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <grub/charset.h>
+#include <grub/command.h>
+#include <grub/err.h>
+#include <grub/file.h>
+#include <grub/fdt.h>
+#include <grub/linux.h>
+#include <grub/loader.h>
+#include <grub/mm.h>
+#include <grub/types.h>
+#include <grub/cpu/linux.h>
+#include <grub/efi/efi.h>
+#include <grub/efi/pe32.h>
+#include <grub/i18n.h>
+#include <grub/lib/cmdline.h>
+#include <grub/misc.h>
+#include <grub/cache.h>
+
+GRUB_MOD_LICENSE ("GPLv3+");
+
+#define GRUB_EFI_PAGE_SHIFT	12
+#define BYTES_TO_PAGES(bytes)   (((bytes) + 0xfff) >> GRUB_EFI_PAGE_SHIFT)
+#define ADDRESS_ALIGN(addr,size) ((void *)((((grub_int64_t)addr)+(size)-1)&\
+		(~((size)-1))))
+
+#ifdef MULTIBOOT_LOAD_ADDR_DEFAULT
+#define MULTIBOOT_BASE_ADDR  (0x80A00000)
+#define MODULE0_BASE_ADDR  (0x80080000)
+#define MODULE1_BASE_ADDR  (0)
+#else
+#define MULTIBOOT_BASE_ADDR  (0)
+#define MODULE0_BASE_ADDR  (0)
+#define MODULE1_BASE_ADDR  (0)
+#endif
+
+/**variable and function in linux.c**/
+extern grub_dl_t my_mod;
+extern int loaded;
+extern grub_efi_guid_t fdt_guid;
+extern void *fdt;
+
+extern void get_fdt (void);
+/************************************/
+
+grub_err_t grub_cmd_multiboot (grub_command_t cmd __attribute__ ((unused)),
+		int argc, char *argv[]);
+grub_err_t grub_cmd_module(grub_command_t cmd __attribute__ ((unused)),
+		 int argc, char *argv[]);
+
+static int module0_loaded;
+
+static void *multiboot_addr_unaligned;
+static void *multiboot_addr;
+static grub_uint64_t multiboot_size;
+
+static char *multiboot_args;
+static grub_uint32_t multiboot_cmdline_size;
+
+static void *module0_addr;
+static grub_uint64_t module0_size;
+
+static char *module0_args;
+static grub_uint32_t module0_cmdline_size;
+
+static grub_addr_t module1_addr;
+static grub_uint64_t module1_size;
+
+typedef void (*kernel_entry_t) (void *, int, int, int);
+
+enum grub_multiboot_module_type
+  {
+    GRUB_MULTIBOOT_MODULE_0,
+    GRUB_MULTIBOOT_MODULE_1,
+    GRUB_MULTIBOOT_MODULE_END
+  };
+typedef enum grub_multiboot_module_type grub_multiboot_module_type_t;
+static grub_multiboot_module_type_t type = GRUB_MULTIBOOT_MODULE_0;
+
+enum grub_multiboot_unload_type
+  {
+    GRUB_MULTIBOOT_UNLOAD_NONE,
+    GRUB_MULTIBOOT_UNLOAD_MULTIBOOT,
+    GRUB_MULTIBOOT_UNLOAD_MODULE_0,
+    GRUB_MULTIBOOT_UNLOAD_MODULE_1,
+    GRUB_MULTIBOOT_UNLOAD_ALL
+  };
+typedef enum grub_multiboot_unload_type grub_multiboot_unload_type_t;
+
+static grub_err_t
+finalize_params_multiboot (void)
+{
+  grub_efi_boot_services_t *b;
+  grub_efi_status_t status;
+  int chosen_node, module_node_0, module_node_1, retval;
+
+  get_fdt ();
+  if (!fdt)
+      goto failure;
+
+  chosen_node = grub_fdt_find_subnode (fdt, 0, "chosen");
+  if (chosen_node < 0)
+      chosen_node = grub_fdt_add_subnode (fdt, 0, "chosen");
+
+  if (chosen_node < 1)
+      goto failure;
+
+  retval = grub_fdt_set_prop32 (fdt, chosen_node, "#address-cells", 0x1);
+  retval = grub_fdt_set_prop32 (fdt, chosen_node, "#size-cells", 0x1);
+
+  if (retval)
+      goto failure;
+
+  /* Set Multiboot Image bootargs info */
+  if (multiboot_args && multiboot_cmdline_size > 0)
+    {
+      grub_dprintf ("linux", "Multiboot Image cmdline : %s @ %p size:%d\n",
+            multiboot_args, multiboot_args, multiboot_cmdline_size);
+
+      retval = grub_fdt_set_prop (fdt, chosen_node, "xen,xen-bootargs",
+                    multiboot_args, multiboot_cmdline_size);
+      if (retval)
+          goto failure;
+
+    } else {
+      grub_dprintf ("linux", "Please set Multiboot Image bootargs!\n");
+      goto failure;
+    }
+
+  /* Set module0 binary info */
+  if (module0_addr && module0_size > 0)
+    {
+      grub_dprintf ("linux", "Module0 @ %p size:0x%lx\n",
+              module0_addr, module0_size);
+
+      module_node_0 = grub_fdt_find_subnode (fdt, chosen_node, "module@0");
+      if (module_node_0 < 0)
+          module_node_0 = grub_fdt_add_subnode (fdt, chosen_node, "module@0");
+
+      retval = grub_fdt_set_prop (fdt, module_node_0, "compatible",
+             "xen,linux-zimage\0xen,multiboot-module",
+             sizeof("xen,linux-zimage\0xen,multiboot-module"));
+      if (retval)
+          goto failure;
+
+      grub_dprintf ("linux", "Module0 compatible = %s size = %ld\n",
+              "xen,linux-zimage xen,multiboot-module",
+              sizeof("xen,linux-zimage\0xen,multiboot-module"));
+
+      grub_uint32_t module0_reg[2];
+      grub_uint64_t temp_addr = (grub_uint64_t)module0_addr;
+      module0_reg[0] = grub_cpu_to_be32((grub_uint32_t)temp_addr);
+      module0_reg[1] = grub_cpu_to_be32((grub_uint32_t)module0_size);
+
+      grub_fdt_set_prop (fdt, module_node_0, "reg", module0_reg, 8);
+      if (retval)
+          goto failure;
+      grub_dprintf ("linux", "Module0 reg = <0x%012lx 0x%012lx>\n",
+              (long unsigned int) module0_addr, module0_size);
+
+      if (module0_args && module0_cmdline_size > 0)
+        {
+          grub_dprintf ("linux", "Module0 cmdline : %s @ %p size:%d\n",
+                  module0_args, module0_args, module0_cmdline_size);
+          retval = grub_fdt_set_prop (fdt, module_node_0, "bootargs",
+                  module0_args, grub_strlen (module0_args) + 1);
+          if (retval)
+              goto failure;
+        }
+    } else {
+      grub_dprintf ("linux", "Please load Module0 with cmdline by module command!\n");
+      goto failure;
+    }
+
+  /* Set module1 info (optional)*/
+  if (module1_addr && module1_size > 0)
+    {
+      grub_dprintf ("linux", "Module1 @ 0x%012lx-0x%012lx\n",
+              module1_addr, module1_addr + module1_size);
+
+      module_node_1 = grub_fdt_find_subnode (fdt, chosen_node, "module@1");
+      if (module_node_1 < 0)
+          module_node_1 = grub_fdt_add_subnode (fdt, chosen_node, "module@1");
+
+      retval = grub_fdt_set_prop (fdt, module_node_1, "compatible",
+              "xen,linux-initrd\0xen,multiboot-module",
+              sizeof("xen,linux-initrd\0xen,multiboot-module"));
+      if (retval)
+          goto failure;
+
+      grub_uint32_t module1_reg[2];
+      grub_uint64_t temp_addr = (grub_uint64_t)module1_addr;
+      module1_reg[0] = grub_cpu_to_be32((grub_uint32_t)temp_addr);
+      module1_reg[1] = grub_cpu_to_be32((grub_uint32_t)module1_size);
+
+      grub_fdt_set_prop (fdt, module_node_1, "reg", module1_reg, 8);
+      if (retval)
+          goto failure;
+
+      grub_dprintf ("linux", "Module1 reg = <0x%012lx 0x%012lx>\n",
+              (long unsigned int)module1_addr, module1_size);
+    }
+
+  b = grub_efi_system_table->boot_services;
+  status = b->install_configuration_table (&fdt_guid, fdt);
+  if (status != GRUB_EFI_SUCCESS)
+      goto failure;
+
+  grub_dprintf ("linux", "Installed/updated FDT configuration table @ %p\n",
+        fdt);
+
+  return (GRUB_ERR_NONE);
+
+failure:
+  grub_efi_free_pages ((grub_efi_physical_address_t) fdt,
+		       BYTES_TO_PAGES (grub_fdt_get_totalsize (fdt)));
+  fdt = NULL;
+  return (grub_error(GRUB_ERR_BAD_OS, "failed to install/update FDT"));
+}
+
+static grub_err_t
+grub_multiboot_boot (void)
+{
+  grub_err_t retval;
+  kernel_entry_t linuxmain;
+
+  retval = finalize_params_multiboot();
+  if (retval != GRUB_ERR_NONE)
+      return (retval);
+
+#if 0
+  /* FIXME: I am not sure if I really need this, grub works fine without it*/
+  grub_arch_sync_caches ((void *) multiboot_addr, multiboot_size);
+  if (multiboot_args && multiboot_cmdline_size > 0) {
+      grub_arch_sync_caches ((void *) multiboot_args, multiboot_cmdline_size);
+  }
+  if (module0_addr && module0_size > 0) {
+      grub_arch_sync_caches ((void *) module0_addr, module0_size);
+  }
+  if (module0_args && module0_cmdline_size > 0) {
+      grub_arch_sync_caches ((void *) module0_args, module0_cmdline_size);
+  }
+
+  if (module1_addr && module1_size > 0) {
+      grub_arch_sync_caches ((void *) module1_addr, module1_size);
+  }
+#endif
+
+  grub_dprintf ("linux", "Jumping to Multiboot Image @ %p\n", multiboot_addr);
+
+   /* Boot the Multiboot Image.
+    *   Arguments to Multiboot Image:
+    *     x0 - address of DTB
+    *     x1 - 0 (reserved for future use)
+    *     x2 - 0 (reserved for future use)
+    *     x3 - 0 (reserved for future use)
+    */
+   linuxmain = (kernel_entry_t) multiboot_addr;
+
+  /* FIXME: I am not sure if I really need this, grub works fine without it*/
+//  grub_arm64_disable_caches_mmu ();
+
+   linuxmain (fdt, 0, 0, 0);
+
+   return (grub_error (GRUB_ERR_BAD_OS, "Multiboot call returned"));
+}
+
+static grub_err_t
+grub_module_unload (grub_multiboot_unload_type_t unload_type)
+{
+  if (module1_addr &&
+         (unload_type == GRUB_MULTIBOOT_UNLOAD_MODULE_1 ||
+          unload_type == GRUB_MULTIBOOT_UNLOAD_ALL)) {
+      grub_efi_free_pages ((grub_efi_physical_address_t) module1_addr,
+              BYTES_TO_PAGES (module1_size));
+      grub_dprintf ("linux", "Multiboot Image memory free @ %p size: %lld\n",
+              (void *)module1_addr, (long long) module1_size);
+      module1_size = 0;
+      module1_addr = (grub_addr_t)0;
+      type = GRUB_MULTIBOOT_MODULE_1;
+  }
+
+  if (module0_addr &&
+         (unload_type == GRUB_MULTIBOOT_UNLOAD_MODULE_0 ||
+          unload_type == GRUB_MULTIBOOT_UNLOAD_ALL)) {
+      grub_efi_free_pages ((grub_efi_physical_address_t) module0_addr,
+            BYTES_TO_PAGES (module0_size));
+      grub_dprintf ("linux", "Module0 memory free @ %p size: %lld\n",
+              module0_addr, (long long) module0_size);
+      module0_size = 0;
+      module0_addr = (void *)0;
+      type = GRUB_MULTIBOOT_MODULE_0;
+      if (module0_args) {
+          grub_free (module0_args);
+              grub_dprintf ("linux", "Module0 args memory free @ %p size: %lld\n",
+                      module0_args, (long long) module0_cmdline_size);
+          module0_cmdline_size = 0;
+          module0_args = (char *)0;
+      }
+  }
+
+  if (multiboot_addr_unaligned &&
+         (unload_type == GRUB_MULTIBOOT_UNLOAD_MULTIBOOT ||
+          unload_type == GRUB_MULTIBOOT_UNLOAD_ALL)) {
+      grub_efi_free_pages ((grub_efi_physical_address_t) multiboot_addr_unaligned,
+            (BYTES_TO_PAGES (multiboot_size) + BYTES_TO_PAGES (0x200000)));
+      grub_dprintf ("linux", "Multiboot image memory free pages @ %p size: %ld\n",
+              multiboot_addr_unaligned,
+              (BYTES_TO_PAGES (multiboot_size) + BYTES_TO_PAGES (0x200000)));
+      multiboot_size = 0;
+      multiboot_addr = (void *)0;
+      type = GRUB_MULTIBOOT_MODULE_0;
+      if (multiboot_args) {
+          grub_free (multiboot_args);
+          grub_dprintf ("linux", "Multiboot args memory free @ %p size: %lld\n",
+                  multiboot_args, (long long) multiboot_cmdline_size);
+          multiboot_cmdline_size = 0;
+          multiboot_args = (char *)0;
+      }
+  }
+
+  return (GRUB_ERR_NONE);
+}
+
+static grub_err_t
+grub_multiboot_unload (void)
+{
+  grub_dl_unref (my_mod);
+  loaded = 0;
+  module0_loaded = 0;
+  grub_module_unload (GRUB_MULTIBOOT_UNLOAD_ALL);
+  type = GRUB_MULTIBOOT_MODULE_0;
+  if (fdt)
+    grub_efi_free_pages ((grub_efi_physical_address_t) fdt,
+            BYTES_TO_PAGES (grub_fdt_get_totalsize (fdt)));
+
+  return (GRUB_ERR_NONE);
+}
+
+grub_err_t
+grub_cmd_module (grub_command_t cmd __attribute__ ((unused)),
+		 int argc, char *argv[])
+{
+  struct grub_linux_initrd_context initrd_ctx;
+  int initrd_size, initrd_pages;
+  void *initrd_mem = NULL;
+  grub_multiboot_unload_type_t unload_type = GRUB_MULTIBOOT_UNLOAD_NONE;
+  grub_file_t file = 0;
+
+  if (argc == 0)
+    {
+      grub_error (GRUB_ERR_BAD_ARGUMENT, N_("filename(and option) expected"));
+      goto fail;
+    }
+
+  if (!loaded)
+    {
+      grub_error (GRUB_ERR_BAD_ARGUMENT,
+          N_("you need to multiboot command first"));
+      goto fail;
+    }
+
+  /* get module type */
+  if (argc >= 3 && grub_strcmp (argv[0], "--type") == 0)
+    {
+      argc--;
+      argv++;
+      if (grub_strcmp (argv[0], "kernel") == 0) {
+          type = GRUB_MULTIBOOT_MODULE_0;
+      } else if (grub_strcmp (argv[0], "initrd") == 0) {
+              type = GRUB_MULTIBOOT_MODULE_1;
+          } else {
+              grub_error (GRUB_ERR_BAD_ARGUMENT,
+                  N_("please check your module type [kernel, initrd]\n"));
+              goto fail;
+          }
+      argc--;
+      argv++;
+    }
+
+  if (type != GRUB_MULTIBOOT_MODULE_0 &&
+          type != GRUB_MULTIBOOT_MODULE_1) {
+      grub_error (GRUB_ERR_BAD_ARGUMENT,
+          N_("Do not support this module type!\n"
+             "Or you have loaded module0 and module1 binary.\n"
+             "If you want to reload any of them, "
+             "please use --type <kernel/initrd>"));
+      goto fail;
+    }
+
+  if (type == GRUB_MULTIBOOT_MODULE_0) {
+      if (argc == 0)
+        {
+          grub_error (GRUB_ERR_BAD_ARGUMENT, N_("filename expected"));
+          goto fail;
+        }
+
+      file = grub_file_open (argv[0]);
+      if (!file)
+          goto fail;
+      unload_type = GRUB_MULTIBOOT_UNLOAD_MODULE_0;
+      grub_module_unload (unload_type);
+
+      module0_size = grub_file_size (file);
+
+      grub_dprintf ("linux", "Module0 file size: %lld\n",
+              (long long) module0_size);
+      module0_addr = (void *) ((unsigned long)grub_efi_allocate_pages (
+              MODULE0_BASE_ADDR ,
+              BYTES_TO_PAGES(module0_size)));
+      grub_dprintf ("linux", "Module0 numpages: %lld\n",
+            (long long) BYTES_TO_PAGES (module0_size));
+      if (!module0_addr)
+        {
+          grub_error (GRUB_ERR_OUT_OF_MEMORY, N_("out of memory"));
+          goto fail_all;
+        }
+
+      if (grub_file_read (file, module0_addr, module0_size)
+          < (grub_int64_t) module0_size)
+        {
+            if (!grub_errno)
+            grub_error (GRUB_ERR_BAD_OS,
+                    N_("premature end of file %s"), argv[0]);
+            goto fail_all;
+        }
+
+      grub_dprintf ("linux", "Module0 @ %p\n", module0_addr);
+
+      /* FIXME: Do we need to have "BOOT_IMAGE="?? */
+      module0_cmdline_size = grub_loader_cmdline_size (argc, argv) +
+              sizeof (LINUX_IMAGE);
+      module0_args = grub_malloc (module0_cmdline_size);
+      if (!module0_args)
+        {
+            grub_error (GRUB_ERR_OUT_OF_MEMORY, N_("out of memory"));
+            goto fail_all;
+        }
+      grub_memcpy (module0_args, LINUX_IMAGE, sizeof (LINUX_IMAGE));
+      grub_create_loader_cmdline (argc, argv,
+              module0_args + sizeof (LINUX_IMAGE) - 1,
+              module0_cmdline_size);
+      grub_dprintf ("linux", "Module0 cmdline @ %p %s, size: %d\n",
+              module0_args, module0_args, module0_cmdline_size);
+
+      module0_loaded = 1;
+      type++;
+  } else {
+	  if (argc >= 2) {
+          grub_error (GRUB_ERR_BAD_ARGUMENT,
+          N_("some useless arguments after Module1 file name!\n"
+             "If you want to reload Module0, "
+             "please use \"--type kernel\" argument!"));
+          goto fail;
+	  }
+      if (grub_initrd_init (argc, argv, &initrd_ctx))
+        goto fail;
+
+      unload_type = GRUB_MULTIBOOT_UNLOAD_MODULE_1;
+      grub_module_unload (unload_type);
+
+      initrd_size = grub_get_initrd_size (&initrd_ctx);
+      grub_dprintf ("linux", "Loading Module1\n");
+
+      initrd_pages = (BYTES_TO_PAGES (initrd_size));
+      initrd_mem = grub_efi_allocate_pages (0, initrd_pages);
+      if (!initrd_mem)
+        {
+            grub_error (GRUB_ERR_OUT_OF_MEMORY, N_("out of memory"));
+            goto fail_module1;
+        }
+
+      if (grub_initrd_load (&initrd_ctx, argv, initrd_mem))
+        goto fail_module1;
+
+      module1_addr = (grub_addr_t) initrd_mem;
+      module1_size = (grub_uint64_t)initrd_size;
+      grub_dprintf ("linux", "Module1 [addr=%p, size=0x%lx]\n",
+            (void *) module1_addr, module1_size);
+
+      type++;
+      grub_initrd_close (&initrd_ctx);
+  }
+
+ fail_all:
+    if (file)
+        grub_file_close (file);
+// fail_module0:
+    if (!module0_loaded && module0_addr)
+        grub_module_unload (GRUB_MULTIBOOT_UNLOAD_MODULE_0);
+ fail_module1:
+    if (!module1_addr && initrd_mem)
+        grub_module_unload (GRUB_MULTIBOOT_UNLOAD_MODULE_1);
+ fail:
+  return (grub_errno);
+}
+
+grub_err_t
+grub_cmd_multiboot (grub_command_t cmd __attribute__ ((unused)),
+		int argc, char *argv[])
+{
+  grub_file_t file = 0;
+
+  grub_dl_ref (my_mod);
+
+  if (argc == 0)
+    {
+      grub_error (GRUB_ERR_BAD_ARGUMENT, N_("filename expected"));
+      goto fail;
+    }
+
+  file = grub_file_open (argv[0]);
+  if (!file)
+    goto fail;
+
+  grub_module_unload (GRUB_MULTIBOOT_UNLOAD_MULTIBOOT);
+
+  multiboot_size = grub_file_size (file);
+
+  grub_loader_unset();
+
+  grub_dprintf ("linux", "Multiboot Image file size: %lld\n", (long long) multiboot_size);
+  multiboot_addr_unaligned = (void *) ((unsigned long)grub_efi_allocate_pages (
+          MULTIBOOT_BASE_ADDR,
+          (BYTES_TO_PAGES (multiboot_size) + BYTES_TO_PAGES (0x200000))));
+  multiboot_addr = ADDRESS_ALIGN(multiboot_addr_unaligned, 0x200000);
+  grub_dprintf ("linux", "Multiboot Image numpages: %lld\n",
+        (long long) BYTES_TO_PAGES (multiboot_size));
+  if (!multiboot_addr)
+    {
+      grub_error (GRUB_ERR_OUT_OF_MEMORY, N_("out of memory"));
+      goto fail;
+    }
+
+  if (grub_file_read (file, multiboot_addr, multiboot_size)
+      < (grub_int64_t) multiboot_size)
+    {
+      if (!grub_errno)
+	grub_error (GRUB_ERR_BAD_OS, N_("premature end of file %s"), argv[0]);
+      goto fail;
+    }
+
+  grub_dprintf ("linux", "Multiboot Image @ %p\n", multiboot_addr);
+
+  /* FIXME: Do we really need to have "BOOT_IMAGE="?? */
+  multiboot_cmdline_size = grub_loader_cmdline_size (argc, argv);
+  multiboot_args = grub_malloc (multiboot_cmdline_size);
+  if (!multiboot_args)
+    {
+      grub_error (GRUB_ERR_OUT_OF_MEMORY, N_("out of memory"));
+      goto fail;
+    }
+  grub_memcpy (multiboot_args, "BOOT_IMAGE=", sizeof ("BOOT_IMAGE="));
+  grub_create_loader_cmdline (argc-1, argv+1,
+          multiboot_args,
+          multiboot_cmdline_size);
+  grub_dprintf ("linux", "Multiboot Image cmdline @ %p %s, size: %d\n",
+          multiboot_args, multiboot_args, multiboot_cmdline_size);
+  if (grub_errno == GRUB_ERR_NONE)
+    {
+      grub_loader_set (grub_multiboot_boot, grub_multiboot_unload, 0);
+      loaded = 1;
+    }
+
+fail:
+  if (file)
+    grub_file_close (file);
+  if (grub_errno != GRUB_ERR_NONE)
+    {
+      grub_module_unload (GRUB_MULTIBOOT_UNLOAD_ALL);
+    }
+
+  return (grub_errno);
+}
-- 
1.8.3.1

