cmd_elf: add an option for loading ELFs according to PHDRs

The current ELF loading function does a lot of work above and beyond a
simple "loading".  It ignores the real load addresses and loads things
into their virtual (runtime) address.  This is undesirable when we just
want it to load an ELF and let the ELF do the actual C runtime init.

So add a command line option to let people choose to load via either the
program or section headers.  I'd prefer to have program header loading
be the default, but this would break historical behavior, so I'll leave
section header loading as the norm.

Signed-off-by: Mike Frysinger <vapier@gentoo.org>

common/cmd_elf.c

@@ -25,7 +25,8 @@ DECLARE_GLOBAL_DATA_PTR;
 #endif
 
 int valid_elf_image (unsigned long addr);
-unsigned long load_elf_image (unsigned long addr);
+static unsigned long load_elf_image_phdr(unsigned long addr);
+static unsigned long load_elf_image_shdr(unsigned long addr);
 
 /* Allow ports to override the default behavior */
 __attribute__((weak))
@@ -61,19 +62,34 @@ int do_bootelf (cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	unsigned long addr;		/* Address of the ELF image     */
 	unsigned long rc;		/* Return value from user code  */
+	char *sload, *saddr;
 
 	/* -------------------------------------------------- */
 	int rcode = 0;
 
-	if (argc < 2)
-		addr = load_addr;
+	sload = saddr = NULL;
+	if (argc == 3) {
+		sload = argv[1];
+		saddr = argv[2];
+	} else if (argc == 2) {
+		if (argv[1][0] == '-')
+			sload = argv[1];
+		else
+			saddr = argv[1];
+	}
+
+	if (saddr)
+		addr = simple_strtoul(saddr, NULL, 16);
 	else
-		addr = simple_strtoul (argv[1], NULL, 16);
+		addr = load_addr;
 
 	if (!valid_elf_image (addr))
 		return 1;
 
-	addr = load_elf_image (addr);
+	if (sload && sload[1] == 'p')
+		addr = load_elf_image_phdr(addr);
+	else
+		addr = load_elf_image_shdr(addr);
 
 	printf ("## Starting application at 0x%08lx ...\n", addr);
 
@@ -204,7 +220,7 @@ int do_bootvx (cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 	 */
 
 	if (valid_elf_image (addr)) {
-		addr = load_elf_image (addr);
+		addr = load_elf_image_shdr (addr);
 	} else {
 		puts ("## Not an ELF image, assuming binary\n");
 		/* leave addr as load_addr */
@@ -258,7 +274,33 @@ int valid_elf_image (unsigned long addr)
  * A very simple elf loader, assumes the image is valid, returns the
  * entry point address.
  * ====================================================================== */
-unsigned long load_elf_image (unsigned long addr)
+static unsigned long load_elf_image_phdr(unsigned long addr)
+{
+	Elf32_Ehdr *ehdr;		/* Elf header structure pointer     */
+	Elf32_Phdr *phdr;		/* Program header structure pointer */
+	int i;
+
+	ehdr = (Elf32_Ehdr *) addr;
+	phdr = (Elf32_Phdr *) (addr + ehdr->e_phoff);
+
+	/* Load each program header */
+	for (i = 0; i < ehdr->e_phnum; ++i) {
+		void *dst = (void *) phdr->p_paddr;
+		void *src = (void *) addr + phdr->p_offset;
+		debug("Loading phdr %i to 0x%p (%i bytes)\n",
+			i, dst, phdr->p_filesz);
+		if (phdr->p_filesz)
+			memcpy(dst, src, phdr->p_filesz);
+		if (phdr->p_filesz != phdr->p_memsz)
+			memset(dst + phdr->p_filesz, 0x00, phdr->p_memsz - phdr->p_filesz);
+		flush_cache((unsigned long)dst, phdr->p_filesz);
+		++phdr;
+	}
+
+	return ehdr->e_entry;
+}
+
+static unsigned long load_elf_image_shdr(unsigned long addr)
 {
 	Elf32_Ehdr *ehdr;		/* Elf header structure pointer     */
 	Elf32_Shdr *shdr;		/* Section header structure pointer */
@@ -312,9 +354,11 @@ unsigned long load_elf_image (unsigned long addr)
 
 /* ====================================================================== */
 U_BOOT_CMD(
-	bootelf,      2,      0,      do_bootelf,
+	bootelf,      3,      0,      do_bootelf,
 	"Boot from an ELF image in memory",
-	" [address] - load address of ELF image."
+	"[-p|-s] [address]\n"
+	"\t- load ELF image at [address] via program headers (-p)\n"
+	"\t  or via section headers (-s)"
 );
 
 U_BOOT_CMD(