[U-Boot] [PATCH 2/3] imx: imx-common: add elf firmware support

[Stefan Agner <stefan@agner.ch>]

From: Stefan Agner <stefan.agner@toradex.com>

Support elf firmware files for the auxiliary Cortex-M4 core. This
has the advantage that the user does not need to know to which
address the binary has been linked to. However, in order to load
the elf sections to the right address, we need to translate the
Cortex-M4 core memory addresses to primary/host CPU memory
addresses (U-Boot is typically running on the A7/A9 core). This
allows to boot firmwares from any location with just using
bootaux, e.g.:

  tftp ${loadaddr} low_power_demo.elf && bootaux ${loadaddr}

Signed-off-by: Stefan Agner <stefan.agner@toradex.com>
---

 arch/arm/cpu/armv7/mx6/soc.c                | 12 ++++
 arch/arm/cpu/armv7/mx7/soc.c                | 15 +++++
 arch/arm/imx-common/imx_bootaux.c           | 90 ++++++++++++++++++++++++++---
 arch/arm/include/asm/imx-common/sys_proto.h |  6 ++
 4 files changed, 116 insertions(+), 7 deletions(-)

diff --git a/arch/arm/cpu/armv7/mx6/soc.c b/arch/arm/cpu/armv7/mx6/soc.c
index 642195b97c..9b89c87e04 100644
--- a/arch/arm/cpu/armv7/mx6/soc.c
+++ b/arch/arm/cpu/armv7/mx6/soc.c
@@ -666,6 +666,18 @@ void imx_setup_hdmi(void)
 #endif
 
 #ifdef CONFIG_IMX_BOOTAUX
+#ifdef CONFIG_MX6SX
+const struct memorymap hostmap[] = {
+	{ .auxcore = 0x00000000, .host = 0x007f8000, .size = 0x8000 },
+	{ .auxcore = 0x1fff8000, .host = 0x007f8000, .size = 0x8000 },
+	{ .auxcore = 0x20000000, .host = 0x00800000, .size = 0x8000 },
+	{ .auxcore = 0x00900000, .host = 0x00900000, .size = 0x20000 },
+	{ .auxcore = 0x20900000, .host = 0x00900000, .size = 0x20000 },
+	{ .auxcore = 0x10000000, .host = 0x80000000, .size = 0x0fff0000 },
+	{ .auxcore = 0x80000000, .host = 0x80000000, .size = 0xe0000000 },
+	{ /* sentinel */ }
+#endif
+
 /*
  * Per the cortex-M reference manual, the reset vector of M4 needs
  * to exist at 0x0 (TCMUL). The PC and SP are the first two addresses
diff --git a/arch/arm/cpu/armv7/mx7/soc.c b/arch/arm/cpu/armv7/mx7/soc.c
index e949b8e557..6491778f5b 100644
--- a/arch/arm/cpu/armv7/mx7/soc.c
+++ b/arch/arm/cpu/armv7/mx7/soc.c
@@ -311,6 +311,21 @@ void imx_get_mac_from_fuse(int dev_id, unsigned char *mac)
 #endif
 
 #ifdef CONFIG_IMX_BOOTAUX
+const struct memorymap hostmap[] = {
+	{ .auxcore = 0x00000000, .host = 0x00180000, .size = 0x8000 },
+	{ .auxcore = 0x00180000, .host = 0x00180000, .size = 0x8000 },
+	{ .auxcore = 0x1fff8000, .host = 0x007f8000, .size = 0x8000 },
+	{ .auxcore = 0x20000000, .host = 0x00800000, .size = 0x8000 },
+	{ .auxcore = 0x00900000, .host = 0x00900000, .size = 0x20000 },
+	{ .auxcore = 0x20200000, .host = 0x00900000, .size = 0x20000 },
+	{ .auxcore = 0x00920000, .host = 0x00920000, .size = 0x20000 },
+	{ .auxcore = 0x20220000, .host = 0x00920000, .size = 0x20000 },
+	{ .auxcore = 0x00940000, .host = 0x00940000, .size = 0x20000 },
+	{ .auxcore = 0x20240000, .host = 0x00940000, .size = 0x20000 },
+	{ .auxcore = 0x10000000, .host = 0x80000000, .size = 0x0fff0000 },
+	{ .auxcore = 0x80000000, .host = 0x80000000, .size = 0xe0000000 },
+	{ /* sentinel */ }
+};
 
 /*
  * Per the cortex-M reference manual, the reset vector of M4 needs
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"
 );
diff --git a/arch/arm/include/asm/imx-common/sys_proto.h b/arch/arm/include/asm/imx-common/sys_proto.h
index a07061bc9b..dfd736ae50 100644
--- a/arch/arm/include/asm/imx-common/sys_proto.h
+++ b/arch/arm/include/asm/imx-common/sys_proto.h
@@ -87,6 +87,12 @@ static inline u8 imx6_is_bmode_from_gpr9(void)
 u32 imx6_src_get_boot_mode(void);
 #endif /* CONFIG_MX6 */
 
+struct memorymap {
+	unsigned long auxcore;
+	unsigned long host;
+	unsigned long size;
+};
+
 u32 get_nr_cpus(void);
 u32 get_cpu_rev(void);
 u32 get_cpu_speed_grade_hz(void);
-- 
2.12.1