[U-Boot] [PATCH 16/41] imx8: add bootaux support

[Peng Fan]

There is a M4 Core for genernal purpose and a DSP core for audio,
add bootaux function in U-Boot to startup the core.
The DSP image is an ELF image.

Signed-off-by: Peng Fan <peng.fan at nxp.com>
---
 arch/arm/mach-imx/imx8/cpu.c | 179 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 179 insertions(+)

diff --git a/arch/arm/mach-imx/imx8/cpu.c b/arch/arm/mach-imx/imx8/cpu.c
index c2508c2600..5223726b59 100644
--- a/arch/arm/mach-imx/imx8/cpu.c
+++ b/arch/arm/mach-imx/imx8/cpu.c
@@ -13,6 +13,7 @@
 #include <asm/armv8/mmu.h>
 #include <asm/mach-imx/boot_mode.h>
 #include <asm/setup.h>
+#include <elf.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -125,6 +126,184 @@ void imx_get_mac_from_fuse(int dev_id, unsigned char *mac)
 	mac[5] = val2 >> 8;
 }
 
+#ifdef CONFIG_IMX_BOOTAUX
+#ifdef CONFIG_IMX8QXP
+static unsigned long load_elf_image_shdr(unsigned long addr)
+{
+	/* Elf header structure pointer */
+	Elf32_Ehdr *ehdr;
+	/* Section header structure pointer */
+	Elf32_Shdr *shdr;
+	/* String table pointer */
+	unsigned char *strtab = NULL;
+	/* Binary image pointer */
+	unsigned char *image;
+	/* Loop counter */
+	int i;
+
+	ehdr = (Elf32_Ehdr *)addr;
+
+	/* Find the section header string table for output info */
+	shdr = (Elf32_Shdr *)(addr + ehdr->e_shoff +
+			(ehdr->e_shstrndx * sizeof(Elf32_Shdr)));
+
+	if (shdr->sh_type == SHT_STRTAB)
+		strtab = (unsigned char *)(addr + shdr->sh_offset);
+
+	/* Load each appropriate section */
+	for (i = 0; i < ehdr->e_shnum; ++i) {
+		shdr = (Elf32_Shdr *)(addr + ehdr->e_shoff +
+				     (i * sizeof(Elf32_Shdr)));
+
+		if (!(shdr->sh_flags & SHF_ALLOC) ||
+		    shdr->sh_addr == 0 || shdr->sh_size == 0) {
+			continue;
+		}
+
+		if (strtab) {
+			debug("%sing %s @ 0x%08lx (%ld bytes)\n",
+			      (shdr->sh_type == SHT_NOBITS) ? "Clear" : "Load",
+			       &strtab[shdr->sh_name],
+			       (unsigned long)shdr->sh_addr,
+			       (long)shdr->sh_size);
+		}
+
+		if (shdr->sh_type == SHT_NOBITS) {
+			memset((void *)(uintptr_t)shdr->sh_addr, 0,
+			       shdr->sh_size);
+		} else {
+			image = (unsigned char *)addr + shdr->sh_offset;
+			memcpy((void *)(uintptr_t)shdr->sh_addr,
+			       (const void *)image, shdr->sh_size);
+		}
+		flush_cache(shdr->sh_addr, shdr->sh_size);
+	}
+
+	return ehdr->e_entry;
+}
+
+int arch_auxiliary_core_up(u32 core_id, ulong boot_private_data)
+{
+	sc_ipc_t ipchndl;
+	sc_rsrc_t core_rsrc, mu_rsrc = -1;
+	sc_faddr_t aux_core_ram;
+	u32 size;
+	ulong addr;
+
+	ipchndl = gd->arch.ipc_channel_handle;
+
+	switch (core_id) {
+	case 0:
+		core_rsrc = SC_R_M4_0_PID0;
+		aux_core_ram = 0x34FE0000;
+		mu_rsrc = SC_R_M4_0_MU_1A;
+		size = SZ_128K;
+		break;
+	case 1:
+		core_rsrc = SC_R_DSP;
+		aux_core_ram = 0x596f8000;
+		size = SZ_2K;
+		break;
+	default:
+		printf("Not support this core boot up, ID:%u\n", core_id);
+		return -EINVAL;
+	}
+
+	addr = (sc_faddr_t)boot_private_data;
+
+	if (addr >= aux_core_ram && addr <= aux_core_ram + size) {
+		printf("Wrong image address 0x%lx, should not in aux core ram\n", addr);
+		return -EINVAL;
+	}
+
+	printf("Power on aux core %d\n", core_id);
+
+	if (sc_pm_set_resource_power_mode(ipchndl, core_rsrc,
+					  SC_PM_PW_MODE_ON) != SC_ERR_NONE)
+		return -EIO;
+
+	if (mu_rsrc != -1) {
+		if (sc_pm_set_resource_power_mode(ipchndl, mu_rsrc,
+						  SC_PM_PW_MODE_ON) !=
+						  SC_ERR_NONE)
+			return -EIO;
+	}
+
+	if (core_id == 1) {
+		struct power_domain pd;
+
+		if (sc_pm_clock_enable(ipchndl, core_rsrc, SC_PM_CLK_PER,
+				       true, false) != SC_ERR_NONE) {
+			printf("Error enable clock\n");
+			return -EIO;
+		}
+
+		if (!power_domain_lookup_name("audio_sai0", &pd)) {
+			if (power_domain_on(&pd)) {
+				printf("Error power on SAI0\n");
+				return -EIO;
+			}
+		}
+
+		if (!power_domain_lookup_name("audio_ocram", &pd)) {
+			if (power_domain_on(&pd)) {
+				printf("Error power on HIFI RAM\n");
+				return -EIO;
+			}
+		}
+	}
+
+	printf("Copy image from 0x%lx to 0x%lx\n", addr, (ulong)aux_core_ram);
+	if (core_id == 0) {
+		/* M4 use bin file */
+		memcpy((void *)aux_core_ram, (void *)addr, size);
+	} else {
+		/* HIFI use elf file */
+		if (!valid_elf_image(addr))
+			return -1;
+		addr = load_elf_image_shdr(addr);
+	}
+
+	printf("Start %s\n", core_id == 0 ? "M4" : "HIFI");
+
+	if (sc_pm_cpu_start(ipcHndl, core_rsrc, true, aux_core_ram) !=
+	    SC_ERR_NONE)
+		return -EIO;
+
+	puts("bootaux complete\n");
+
+	return 0;
+}
+#endif
+
+int arch_auxiliary_core_check_up(u32 core_id)
+{
+	sc_rsrc_t core_rsrc;
+	sc_pm_power_mode_t power_mode;
+	sc_ipc_t ipchndl;
+
+	ipchndl = gd->arch.ipc_channel_handle;
+
+	switch (core_id) {
+	case 0:
+		core_rsrc = SC_R_M4_0_PID0;
+		break;
+	default:
+		printf("Not support this core, ID:%u\n", core_id);
+		return 0;
+	}
+
+	if (sc_pm_get_resource_power_mode(ipchndl, core_rsrc, &power_mode) !=
+	    SC_ERR_NONE)
+		return 0;
+
+	if (power_mode != SC_PM_PW_MODE_OFF)
+		return 1;
+
+	return 0;
+}
+#endif
+
 int print_bootinfo(void)
 {
 	enum boot_device bt_dev = get_boot_device();
-- 
2.14.1