imx: imx9: Add M33 release prepare function

To support on-demand booting M33 image from A core. SPL needs
to follow M33 kick up sequence to release M33 firstly,
then set M33 CPUWAIT signal. ATF will clear CPUWAIT to kick
M33 to run.

The prepare function also works around the M33 TCM ECC issue by
clean the TCM. Also enable sentinel handshake and WDOG1 clock
for M33 stop and reset.

Signed-off-by: Ye Li <ye.li@nxp.com>
Signed-off-by: Peng Fan <peng.fan@nxp.com>

2 files changed, 53 insertions, 0 deletions

diff --git a/arch/arm/include/asm/arch-imx9/sys_proto.h b/arch/arm/include/asm/arch-imx9/sys_proto.h
index 5ae7a04..ba97f92 100644
--- a/arch/arm/include/asm/arch-imx9/sys_proto.h
+++ b/arch/arm/include/asm/arch-imx9/sys_proto.h
@@ -9,4 +9,6 @@
 #include <asm/mach-imx/sys_proto.h>
 
 void soc_power_init(void);
+bool m33_is_rom_kicked(void);
+int m33_prepare(void);
 #endif
diff --git a/arch/arm/mach-imx/imx9/soc.c b/arch/arm/mach-imx/imx9/soc.c
index 68f3ddd..2a29454 100644
--- a/arch/arm/mach-imx/imx9/soc.c
+++ b/arch/arm/mach-imx/imx9/soc.c
@@ -13,6 +13,7 @@
 #include <asm/global_data.h>
 #include <asm/io.h>
 #include <asm/arch/clock.h>
+#include <asm/arch/ccm_regs.h>
 #include <asm/arch/sys_proto.h>
 #include <asm/arch/trdc.h>
 #include <asm/mach-imx/boot_mode.h>
@@ -378,3 +379,53 @@ void soc_power_init(void)
 
 	disable_isolation();
 }
+
+static bool m33_is_rom_kicked(void)
+{
+	struct blk_ctrl_s_aonmix_regs *s_regs =
+			(struct blk_ctrl_s_aonmix_regs *)BLK_CTRL_S_ANOMIX_BASE_ADDR;
+
+	if (!(readl(&s_regs->m33_cfg) & BIT(2)))
+		return true;
+
+	return false;
+}
+
+int m33_prepare(void)
+{
+	struct src_mix_slice_regs *mix_regs =
+		(struct src_mix_slice_regs *)(ulong)(SRC_IPS_BASE_ADDR + 0x400 * (SRC_MIX_CM33 + 1));
+	struct src_general_regs *global_regs =
+		(struct src_general_regs *)(ulong)SRC_GLOBAL_RBASE;
+	struct blk_ctrl_s_aonmix_regs *s_regs =
+			(struct blk_ctrl_s_aonmix_regs *)BLK_CTRL_S_ANOMIX_BASE_ADDR;
+	u32 val;
+
+	if (m33_is_rom_kicked())
+		return -EPERM;
+
+	/* Release reset of M33 */
+	setbits_le32(&global_regs->scr, BIT(0));
+
+	/* Check the reset released in M33 MIX func stat */
+	val = readl(&mix_regs->func_stat);
+	while (!(val & SRC_MIX_SLICE_FUNC_STAT_RST_STAT))
+		val = readl(&mix_regs->func_stat);
+
+	/* Release Sentinel TROUT */
+	ahab_release_m33_trout();
+
+	/* Mask WDOG1 IRQ from A55, we use it for M33 reset */
+	setbits_le32(&s_regs->ca55_irq_mask[1], BIT(6));
+
+	/* Turn on WDOG1 clock */
+	ccm_lpcg_on(CCGR_WDG1, 1);
+
+	/* Set sentinel LP handshake for M33 reset */
+	setbits_le32(&s_regs->lp_handshake[0], BIT(6));
+
+	/* Clear M33 TCM for ECC */
+	memset((void *)(ulong)0x201e0000, 0, 0x40000);
+
+	return 0;
+}