1 files changed, 83 insertions, 7 deletions

diff --git a/arch/arm/imx-common/imx_bootaux.c b/arch/arm/imx-common/imx_bootaux.c
index 4d697b0660..365c32f3dd 100644
--- a/arch/arm/imx-common/imx_bootaux.c
+++ b/arch/arm/imx-common/imx_bootaux.c
@@ -4,8 +4,10 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 
+#include <asm/arch/sys_proto.h>
 #include <common.h>
 #include <command.h>
+#include <elf.h>
 
 /* Allow for arch specific config before we boot */
 static int __arch_auxiliary_core_up(u32 core_id, u32 stack, u32 pc)
@@ -27,11 +29,68 @@ static int __arch_auxiliary_core_check_up(u32 core_id)
 int arch_auxiliary_core_check_up(u32 core_id)
 	__attribute__((weak, alias("__arch_auxiliary_core_check_up")));
 
+const __weak struct memorymap hostmap[] = { };
+
 /*
- * To i.MX6SX and i.MX7D, the image supported by bootaux needs
- * the reset vector at the head for the image, with SP and PC
- * as the first two words.
+ * Get memory map by auxiliary core memory address
  */
+static const struct memorymap *get_host_mapping(unsigned long auxcore)
+{
+	const struct memorymap *mmap = hostmap;
+
+	while (mmap) {
+		if (mmap->auxcore <= auxcore &&
+		    mmap->auxcore + mmap->size > auxcore)
+			return mmap;
+		mmap++;
+	}
+
+	return NULL;
+}
+
+/*
+ * A very simple elf loader, assumes the image is valid, returns the
+ * entry point address.
+ */
+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, ++phdr) {
+		const struct memorymap *mmap = get_host_mapping(phdr->p_paddr);
+		void *dst, *src;
+
+		if (phdr->p_type != PT_LOAD)
+			continue;
+
+		if (!mmap) {
+			error("Invalid aux core address: %08x", phdr->p_paddr);
+			return 0;
+		}
+
+		dst = (void *)(phdr->p_paddr - mmap->auxcore) + mmap->host;
+		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 & ~(CONFIG_SYS_CACHELINE_SIZE-1),
+			    ALIGN(phdr->p_filesz, CONFIG_SYS_CACHELINE_SIZE));
+	}
+
+	return ehdr->e_entry;
+}
+
+/* Supports firmware files in binary and elf format (using autodetection) */
 int do_bootaux(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	u32 stack, pc;
@@ -48,10 +107,24 @@ int do_bootaux(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 	}
 
 	addr = simple_strtoul(argv[1], NULL, 16);
+	if (!addr)
+		return CMD_RET_FAILURE;
 
-	/* Assume binary file with vector table at the beginning */
-	stack = *(u32 *)addr;
-	pc = *(u32 *)(addr + 4);
+	if (valid_elf_image(addr)) {
+		stack = 0x0;
+		pc = load_elf_image_phdr(addr);
+		if (!pc)
+			return CMD_RET_FAILURE;
+
+	} else {
+		/*
+		 * Assume binary file with vector table at the beginning.
+		 * Cortex-M4 vector tables start with the stack pointer (SP)
+		 * and reset vector (initial PC).
+		 */
+		stack = *(u32 *)addr;
+		pc = *(u32 *)(addr + 4);
+	}
 
 	printf("## Starting auxiliary core at 0x%08X ...\n", pc);
 
@@ -65,5 +138,8 @@ int do_bootaux(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 U_BOOT_CMD(
 	bootaux, CONFIG_SYS_MAXARGS, 1,	do_bootaux,
 	"Start auxiliary core",
-	""
+	"<addr>\n"
+	"Boot firmware at 'addr' on auxiliary core. Firmware formats:\n"
+	"  - bin: 'addr' must be the address the fw has been linked to\n"
+	"  - elf: 'addr' can be anywhere, relocating according to elf headers\n"
 );