LFU-330-37 arm: 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>

3 files changed, 59 insertions, 0 deletions

diff --git a/arch/arm/include/asm/arch-imx9/imx-regs.h b/arch/arm/include/asm/arch-imx9/imx-regs.h
index 1d8ac32a22..22908944c1 100644
--- a/arch/arm/include/asm/arch-imx9/imx-regs.h
+++ b/arch/arm/include/asm/arch-imx9/imx-regs.h
@@ -53,6 +53,7 @@
 #define BCTRL_GPR_ENET_QOS_INTF_SEL_RGMII        (0x1 << 1)
 #define BCTRL_GPR_ENET_QOS_CLK_GEN_EN            (0x1 << 0)
 
+#define BCTRL_S_ANOMIX_M33_CPU_WAIT_MASK 		BIT(2)
 
 enum mix_power_domain {
 	MIX_PD_MEDIAMIX,
diff --git a/arch/arm/include/asm/arch-imx9/sys_proto.h b/arch/arm/include/asm/arch-imx9/sys_proto.h
index 1529a3762d..292635982f 100644
--- a/arch/arm/include/asm/arch-imx9/sys_proto.h
+++ b/arch/arm/include/asm/arch-imx9/sys_proto.h
@@ -15,4 +15,6 @@ enum boot_device get_boot_device(void);
 bool is_usb_boot(void);
 int mix_power_init(enum mix_power_domain pd);
 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 0c3ceed9af..96d71eacec 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>
@@ -468,3 +469,58 @@ void soc_power_init(void)
 
 	disable_isolation();
 }
+
+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) & BCTRL_S_ANOMIX_M33_CPU_WAIT_MASK))
+		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;
+
+	/* Allow NS to set it */
+	setbits_le32(&mix_regs->authen_ctrl, BIT(9));
+
+	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);
+	}
+
+	/* Because CPUWAIT is default set, so M33 won't run, Clear it when kick M33 */
+	/* 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;
+}