[U-Boot] [PATCH 07/11] imx: mx6: implement functions to boot auxiliary core

[Peng Fan <van.freenix@gmail.com>]

From: Peng Fan <peng.fan@nxp.com>

Implement arch_auxiliary_core_up and arch_auxiliary_core_check_up.

arch_auxiliary_core_check_up is used to check whether M4 is running
or not. arch_auxiliary_core_up is to boot M4 core, the m4 core will
use the pc and stack which is set in arch_auxiliary_core_up to set R15
and R13 register and boot.

Signed-off-by: Ye.Li <ye.li@nxp.com>
Signed-off-by: Peng Fan <peng.fan@nxp.com>
---
 arch/arm/cpu/armv7/mx6/soc.c             | 38 ++++++++++++++++++++++++++++++++
 arch/arm/include/asm/arch-mx6/imx-regs.h |  5 +++++
 2 files changed, 43 insertions(+)

diff --git a/arch/arm/cpu/armv7/mx6/soc.c b/arch/arm/cpu/armv7/mx6/soc.c
index bf5ae8c..ab0ccb0 100644
--- a/arch/arm/cpu/armv7/mx6/soc.c
+++ b/arch/arm/cpu/armv7/mx6/soc.c
@@ -491,3 +491,41 @@ void imx_setup_hdmi(void)
 	writel(reg, &mxc_ccm->chsccdr);
 }
 #endif
+
+#ifdef CONFIG_IMX_BOOTAUX
+int arch_auxiliary_core_up(u32 core_id, u32 boot_private_data)
+{
+	struct src *src_reg;
+	u32 stack, pc;
+
+	if (!boot_private_data)
+		return -EINVAL;
+
+	stack = *(u32 *)boot_private_data;
+	pc = *(u32 *)(boot_private_data + 4);
+
+	/* Set the stack and pc to M4 bootROM */
+	writel(stack, M4_BOOTROM_BASE_ADDR);
+	writel(pc, M4_BOOTROM_BASE_ADDR + 4);
+
+	/* Enable M4 */
+	src_reg = (struct src *)SRC_BASE_ADDR;
+	clrsetbits_le32(&src_reg->scr, SRC_SCR_M4C_NON_SCLR_RST_MASK,
+			SRC_SCR_M4_ENABLE_MASK);
+
+	return 0;
+}
+
+int arch_auxiliary_core_check_up(u32 core_id)
+{
+	struct src *src_reg = (struct src *)SRC_BASE_ADDR;
+	unsigned val;
+
+	val = readl(&src_reg->scr);
+
+	if (val & SRC_SCR_M4C_NON_SCLR_RST_MASK)
+		return 0;  /* assert in reset */
+
+	return 1;
+}
+#endif
diff --git a/arch/arm/include/asm/arch-mx6/imx-regs.h b/arch/arm/include/asm/arch-mx6/imx-regs.h
index b1de751..f5ce31c 100644
--- a/arch/arm/include/asm/arch-mx6/imx-regs.h
+++ b/arch/arm/include/asm/arch-mx6/imx-regs.h
@@ -438,6 +438,11 @@ struct src {
 	u32     gpr10;
 };
 
+#define SRC_SCR_M4_ENABLE_OFFSET                22
+#define SRC_SCR_M4_ENABLE_MASK                  (1 << 22)
+#define SRC_SCR_M4C_NON_SCLR_RST_OFFSET         4
+#define SRC_SCR_M4C_NON_SCLR_RST_MASK           (1 << 4)
+
 /* GPR1 bitfields */
 #define IOMUXC_GPR1_APP_CLK_REQ_N		BIT(30)
 #define IOMUXC_GPR1_PCIE_EXIT_L1		BIT(28)
-- 
2.6.2