MLK-9928-2 arm: imx: support scaling M4 freq by A9

M4 can NOT switch its clk parent due to glitch MUX,
to handle this case, A9 will help switch M4's clk
parent, the flow is as below:

M4:
1. enter low power idle, send bus use count-- to A9;
2. enter wfi and only wait for MU interrupt;
3. receive A9's clk switch ready message, go into low
   power idle;
4. receive interrupt to exit low power idle, send request
   to A9 for increase busfreq and M4 freq, enter wfi
   and only wait for MU interrupt;
5. receive A9 ready message, go out of low power idle.

A9:
1. when receive M4's message of entering low power idle,
   wait M4 into wfi, hold M4 in wfi by hardware, gate
   M4 clk, then switch M4's clk to OSC, ungate M4 clk,
   send ready command to wake up M4 into low power idle;
2. when receive M4's message of exiting low power idle,
   wait M4 into wfi, hold M4 in wfi by hardware, gate
   M4 clk, then switch M4's clk to origin high clk,
   ungate M4 clk, send ready command to wake up M4
   to exit low power idle;

Signed-off-by: Anson Huang <b20788@freescale.com>

4 files changed, 51 insertions, 2 deletions

diff --git a/arch/arm/mach-imx/clk-imx6sx.c b/arch/arm/mach-imx/clk-imx6sx.c
index 96ae86f7cf24..7224c111d808 100644
--- a/arch/arm/mach-imx/clk-imx6sx.c
+++ b/arch/arm/mach-imx/clk-imx6sx.c
@@ -134,6 +134,39 @@ static u32 share_count_asrc;
 static u32 share_count_audio;
 static u32 share_count_esai;
 
+
+/*
+ * As IMX6SX_CLK_M4_PRE_SEL is NOT a glitchless MUX, so when
+ * M4 is trying to change its clk parent, need to ask A9 to
+ * help do it, and M4 must be hold in wfi. To avoid glitch
+ * occur, need to gate M4 clk first before switching its parent.
+ */
+void imx6sx_set_m4_highfreq(bool high_freq)
+{
+	static struct clk *m4_high_freq_sel;
+
+	imx_gpc_hold_m4_in_sleep();
+
+	clk_disable_unprepare(clks[IMX6SX_CLK_M4]);
+	imx_clk_set_parent(clks[IMX6SX_CLK_M4_SEL],
+		clks[IMX6SX_CLK_LDB_DI0]);
+
+	if (high_freq) {
+		imx_clk_set_parent(clks[IMX6SX_CLK_M4_PRE_SEL],
+			m4_high_freq_sel);
+	} else {
+		m4_high_freq_sel = clk_get_parent(clks[IMX6SX_CLK_M4_PRE_SEL]);
+		imx_clk_set_parent(clks[IMX6SX_CLK_M4_PRE_SEL],
+			clks[IMX6SX_CLK_OSC]);
+	}
+
+	imx_clk_set_parent(clks[IMX6SX_CLK_M4_SEL],
+		clks[IMX6SX_CLK_M4_PRE_SEL]);
+	clk_prepare_enable(clks[IMX6SX_CLK_M4]);
+
+	imx_gpc_release_m4_in_sleep();
+}
+
 /*
  * The ldb_di_sel is buggy and could generate a glitch during clock switch.
  * Find out the parent set up by bootloader and register it statically to
@@ -524,6 +557,10 @@ static void __init imx6sx_clocks_init(struct device_node *ccm_node)
 	clk_register_clkdev(clks[IMX6SX_CLK_LVDS2_IN], "lvds2_in", NULL);
 	clk_register_clkdev(clks[IMX6SX_CLK_ESAI_EXTAL], "esai_extal", NULL);
 
+	/* maintain M4 usecount */
+	if (imx_src_is_m4_enabled())
+		imx_clk_prepare_enable(clks[IMX6SX_CLK_M4]);
+
 	/* set perclk to from OSC */
 	imx_clk_set_parent(clks[IMX6SX_CLK_PERCLK_SEL], clks[IMX6SX_CLK_OSC]);
 
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index 7abd21c0f711..2366ffe17ffd 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -168,7 +168,7 @@ extern void imx_gpc_hold_m4_in_sleep(void);
 extern void imx_gpc_release_m4_in_sleep(void);
 extern void imx_gpc_add_m4_wake_up_irq(u32 irq, bool enable);
 extern void mcc_enable_m4_irqs_in_gic(bool enable);
-
+extern void imx6sx_set_m4_highfreq(bool high_freq);
 extern void imx_cpu_die(unsigned int cpu);
 extern int imx_cpu_kill(unsigned int cpu);
 
diff --git a/arch/arm/mach-imx/gpc.c b/arch/arm/mach-imx/gpc.c
index 7aa0fa5803c7..fc74dabdc0f1 100644
--- a/arch/arm/mach-imx/gpc.c
+++ b/arch/arm/mach-imx/gpc.c
@@ -234,16 +234,24 @@ void imx_gpc_pre_suspend(bool arm_power_off)
 void imx_gpc_hold_m4_in_sleep()
 {
 	int val;
+	unsigned long timeout = jiffies + msecs_to_jiffies(500);
+
+	/* wait M4 in wfi before asserting hold request */
+	while (!imx_gpc_is_m4_sleeping())
+		if (time_after(jiffies, timeout))
+			pr_err("M4 is NOT in expected sleep!\n");
 
 	val = readl_relaxed(gpc_base + GPC_M4_LPSR);
 	val &= ~(GPC_M4_LPSR_M4_SLEEP_HOLD_REQ_MASK <<
 		GPC_M4_LPSR_M4_SLEEP_HOLD_REQ_SHIFT);
 	writel_relaxed(val, gpc_base + GPC_M4_LPSR);
 
+	timeout = jiffies + msecs_to_jiffies(500);
 	while (readl_relaxed(gpc_base + GPC_M4_LPSR)
 		& (GPC_M4_LPSR_M4_SLEEP_HOLD_ACK_MASK <<
 		GPC_M4_LPSR_M4_SLEEP_HOLD_ACK_SHIFT))
-		;
+		if (time_after(jiffies, timeout))
+			pr_err("Wait M4 hold ack timeout!\n");
 }
 
 void imx_gpc_release_m4_in_sleep()
diff --git a/arch/arm/mach-imx/mcc_linux.c b/arch/arm/mach-imx/mcc_linux.c
index 5de2bef1241d..68c7bd024bb8 100644
--- a/arch/arm/mach-imx/mcc_linux.c
+++ b/arch/arm/mach-imx/mcc_linux.c
@@ -237,12 +237,16 @@ static void mu_work_handler(struct work_struct *work)
 	switch (m4_message) {
 	case MU_LPM_M4_REQUEST_HIGH_BUS:
 		request_bus_freq(BUS_FREQ_HIGH);
+		imx6sx_set_m4_highfreq(true);
 		mcc_send_via_mu_buffer(MU_LPM_HANDSHAKE_INDEX,
 			MU_LPM_BUS_HIGH_READY_FOR_M4);
 		m4_freq_low = false;
 		break;
 	case MU_LPM_M4_RELEASE_HIGH_BUS:
 		release_bus_freq(BUS_FREQ_HIGH);
+		imx6sx_set_m4_highfreq(false);
+		mcc_send_via_mu_buffer(MU_LPM_HANDSHAKE_INDEX,
+			MU_LPM_M4_FREQ_CHANGE_READY);
 		m4_freq_low = true;
 		break;
 	default: