clk: imx6sx: add AMP clock management support

i.MX6SX has A9 and M4 inside, they can run independently,
this patch adds shared clock management for AMP system.

Signed-off-by: Anson Huang <Anson.Huang@nxp.com>
[ Aisheng: update to CLK HW APIs ]
Signed-off-by: Dong Aisheng <aisheng.dong@nxp.com>

drivers/clk/imx/clk-gate2.c

@@ -7,11 +7,13 @@
  */
 
 #include <linux/clk-provider.h>
+#include <linux/imx_sema4.h>
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/string.h>
+#include <soc/imx/src.h>
 #include "clk.h"
 
 /**
@@ -35,11 +37,56 @@ struct clk_gate2 {
 };
 
 #define to_clk_gate2(_hw) container_of(_hw, struct clk_gate2, hw)
+#define CCM_CCGR_FULL_ENABLE	0x3
+
+static void clk_gate2_do_hardware(struct clk_gate2 *gate, bool enable)
+{
+	u32 reg;
+
+	reg = readl(gate->reg);
+	if (enable)
+		reg |= CCM_CCGR_FULL_ENABLE << gate->bit_idx;
+	else
+		reg &= ~(CCM_CCGR_FULL_ENABLE << gate->bit_idx);
+	writel(reg, gate->reg);
+}
+
+static void clk_gate2_do_shared_clks(struct clk_hw *hw, bool enable)
+{
+	struct clk_gate2 *gate = to_clk_gate2(hw);
+
+	if (imx_src_is_m4_enabled() && clk_on_imx6sx()) {
+#ifdef CONFIG_SOC_IMX6SX
+		if (!amp_power_mutex || !shared_mem) {
+			if (enable)
+				clk_gate2_do_hardware(gate, enable);
+			return;
+		}
+
+		imx_sema4_mutex_lock(amp_power_mutex);
+		if (shared_mem->ca9_valid != SHARED_MEM_MAGIC_NUMBER ||
+			shared_mem->cm4_valid != SHARED_MEM_MAGIC_NUMBER) {
+			imx_sema4_mutex_unlock(amp_power_mutex);
+			return;
+		}
+
+		if (!imx_update_shared_mem(hw, enable)) {
+			imx_sema4_mutex_unlock(amp_power_mutex);
+			return;
+		}
+
+		clk_gate2_do_hardware(gate, enable);
+
+		imx_sema4_mutex_unlock(amp_power_mutex);
+#endif
+	} else {
+		clk_gate2_do_hardware(gate, enable);
+	}
+}
 
 static int clk_gate2_enable(struct clk_hw *hw)
 {
 	struct clk_gate2 *gate = to_clk_gate2(hw);
-	u32 reg;
 	unsigned long flags = 0;
 
 	spin_lock_irqsave(gate->lock, flags);
@@ -47,11 +94,7 @@ static int clk_gate2_enable(struct clk_hw *hw)
 	if (gate->share_count && (*gate->share_count)++ > 0)
 		goto out;
 
-	reg = readl(gate->reg);
-	reg &= ~(3 << gate->bit_idx);
-	reg |= gate->cgr_val << gate->bit_idx;
-	writel(reg, gate->reg);
-
+	clk_gate2_do_shared_clks(hw, true);
 out:
 	spin_unlock_irqrestore(gate->lock, flags);
 
@@ -61,7 +104,6 @@ out:
 static void clk_gate2_disable(struct clk_hw *hw)
 {
 	struct clk_gate2 *gate = to_clk_gate2(hw);
-	u32 reg;
 	unsigned long flags = 0;
 
 	spin_lock_irqsave(gate->lock, flags);
@@ -73,10 +115,7 @@ static void clk_gate2_disable(struct clk_hw *hw)
 			goto out;
 	}
 
-	reg = readl(gate->reg);
-	reg &= ~(3 << gate->bit_idx);
-	writel(reg, gate->reg);
-
+	clk_gate2_do_shared_clks(hw, false);
 out:
 	spin_unlock_irqrestore(gate->lock, flags);
 }
@@ -102,15 +141,11 @@ static void clk_gate2_disable_unused(struct clk_hw *hw)
 {
 	struct clk_gate2 *gate = to_clk_gate2(hw);
 	unsigned long flags = 0;
-	u32 reg;
 
 	spin_lock_irqsave(gate->lock, flags);
 
-	if (!gate->share_count || *gate->share_count == 0) {
-		reg = readl(gate->reg);
-		reg &= ~(3 << gate->bit_idx);
-		writel(reg, gate->reg);
-	}
+	if (!gate->share_count || *gate->share_count == 0)
+		clk_gate2_do_shared_clks(hw, false);
 
 	spin_unlock_irqrestore(gate->lock, flags);
 }

drivers/clk/imx/clk-imx6sx.c

@@ -10,6 +10,7 @@
 #include <linux/err.h>
 #include <linux/init.h>
 #include <linux/io.h>
+#include <linux/imx_sema4.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
@@ -19,6 +20,8 @@
 
 #include "clk.h"
 
+#define CCM_CCGR_OFFSET(index)		(index * 2)
+
 static const char *step_sels[]		= { "osc", "pll2_pfd2_396m", };
 static const char *pll1_sw_sels[]	= { "pll1_sys", "step", };
 static const char *periph_pre_sels[]	= { "pll2_bus", "pll2_pfd2_396m", "pll2_pfd0_352m", "pll2_198m", };
@@ -85,6 +88,12 @@ static const char *pll7_bypass_sels[] = { "pll7", "pll7_bypass_src", };
 
 static struct clk_hw **hws;
 static struct clk_hw_onecell_data *clk_hw_data;
+struct imx_sema4_mutex *amp_power_mutex;
+
+static int clks_shared[MAX_SHARED_CLK_NUMBER];
+
+struct imx_shared_mem *shared_mem;
+static unsigned int shared_mem_paddr, shared_mem_size;
 
 static const struct clk_div_table clk_enet_ref_table[] = {
 	{ .val = 0, .div = 20, },
@@ -524,13 +533,59 @@ static void __init imx6sx_clocks_init(struct device_node *ccm_node)
 	hws[IMX6SX_CLK_CKO1]         = imx_clk_hw_gate("cko1",           "cko1_podf",         base + 0x60, 7);
 	hws[IMX6SX_CLK_CKO2]         = imx_clk_hw_gate("cko2",           "cko2_podf",         base + 0x60, 24);
 
+	/* get those shared clk nodes if M4 is active */
+	if (imx_src_is_m4_enabled()) {
+		u32 num;
+
+		of_property_read_u32(np, "fsl,shared-clks-number", &num);
+		if (num > MAX_SHARED_CLK_NUMBER)
+			pr_err("clk: shared clk nodes exceed the max number!\n");
+		of_property_read_u32_array(np, "fsl,shared-clks-index",
+			clks_shared, num);
+		if (of_property_read_u32(np, "fsl,shared-mem-addr",
+			&shared_mem_paddr))
+			pr_err("clk: fsl,shared-mem-addr NOT found!\n");
+		if (of_property_read_u32(np, "fsl,shared-mem-size",
+			&shared_mem_size))
+			pr_err("clk: fsl,shared-mem-size NOT found!\n");
+	}
+
 	/* mask handshake of mmdc */
 	imx_mmdc_mask_handshake(base, 0);
 
 	imx_check_clk_hws(hws, IMX6SX_CLK_CLK_END);
 
+	/*
+	 * QSPI2/GPMI_IO share the same clock source but with the
+	 * different gate, need explicitely gate the QSPI2 & GPMI_IO
+	 * during the clock init phase according to the SOC design.
+	 */
+	if (!imx_src_is_m4_enabled()) {
+		writel_relaxed(readl_relaxed(base + 0x78) &
+			~(3 << CCM_CCGR_OFFSET(5)), base + 0x78);
+		writel_relaxed(readl_relaxed(base + 0x78) &
+			~(3 << CCM_CCGR_OFFSET(14)), base + 0x78);
+	}
+
+
 	of_clk_add_hw_provider(np, of_clk_hw_onecell_get, clk_hw_data);
 
+	/*
+	 * As some of the modules need to access ocotp in MSL,
+	 * need to make sure ocotp clk(CCM_CCGR2_CG6) is enabled
+	 * during MSL, as on i.MX6SX, accessing OCOTP registers
+	 * needs its clk on, it will be disabled by clk late
+	 * init and managed by ocotp driver.
+	 */
+	writel_relaxed(readl_relaxed(base + 0x70) | 1 << 12, base + 0x70);
+
+	/* maintain M4 usecount */
+	if (imx_src_is_m4_enabled())
+		clk_prepare_enable(hws[IMX6SX_CLK_M4]->clk);
+
+	/* set perclk to from OSC */
+	clk_set_parent(hws[IMX6SX_CLK_PERCLK_SEL]->clk, hws[IMX6SX_CLK_OSC]->clk);
+
 	if (IS_ENABLED(CONFIG_USB_MXS_PHY)) {
 		clk_prepare_enable(hws[IMX6SX_CLK_USBPHY1_GATE]->clk);
 		clk_prepare_enable(hws[IMX6SX_CLK_USBPHY2_GATE]->clk);
@@ -592,6 +647,9 @@ static void __init imx6sx_clocks_init(struct device_node *ccm_node)
 	/* Update gpu clock from default 528M to 720M */
 	clk_set_parent(hws[IMX6SX_CLK_GPU_CORE_SEL]->clk, hws[IMX6SX_CLK_PLL3_PFD0]->clk);
 	clk_set_parent(hws[IMX6SX_CLK_GPU_AXI_SEL]->clk, hws[IMX6SX_CLK_PLL3_PFD0]->clk);
+	if (!imx_src_is_m4_enabled())
+		/* default parent of can_sel clock is invalid, manually set it here */
+		clk_set_parent(hws[IMX6SX_CLK_CAN_SEL]->clk, hws[IMX6SX_CLK_PLL3_60M]->clk);
 
 	clk_set_parent(hws[IMX6SX_CLK_QSPI1_SEL]->clk, hws[IMX6SX_CLK_PLL2_BUS]->clk);
 	clk_set_parent(hws[IMX6SX_CLK_QSPI2_SEL]->clk, hws[IMX6SX_CLK_PLL2_BUS]->clk);
@@ -605,3 +663,64 @@ static void __init imx6sx_clocks_init(struct device_node *ccm_node)
 	imx_register_uart_clocks(uart_clks);
 }
 CLK_OF_DECLARE(imx6sx, "fsl,imx6sx-ccm", imx6sx_clocks_init);
+
+int imx_update_shared_mem(struct clk_hw *hw, bool enable)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(clks_shared); i++) {
+		if (shared_mem->imx_clk[i].self == hw->clk)
+			break;
+	}
+
+	if (i >= ARRAY_SIZE(clks_shared))
+		return 1;
+
+	/* update ca9 clk status in shared memory */
+	if (enable)
+		shared_mem->imx_clk[i].ca9_enabled = 1;
+	else
+		shared_mem->imx_clk[i].ca9_enabled = 0;
+
+	if (shared_mem->imx_clk[i].cm4_enabled == 0)
+		return 1;
+
+	return 0;
+}
+
+static int __init imx_amp_power_init(void)
+{
+	int i;
+	void __iomem *shared_mem_base;
+
+	if (!(imx_src_is_m4_enabled() && clk_on_imx6sx()))
+		return 0;
+
+	amp_power_mutex = imx_sema4_mutex_create(0, MCC_POWER_SHMEM_NUMBER);
+
+	shared_mem_base = ioremap_nocache(shared_mem_paddr, shared_mem_size);
+
+	if (!amp_power_mutex) {
+		pr_err("Failed to create sema4 mutex!\n");
+		return 0;
+	}
+
+	shared_mem = (struct imx_shared_mem *)shared_mem_base;
+
+	for (i = 0; i < ARRAY_SIZE(clks_shared); i++) {
+		shared_mem->imx_clk[i].self = hws[clks_shared[i]]->clk;
+		shared_mem->imx_clk[i].ca9_enabled = 1;
+		pr_debug("%d: name %s, addr 0x%x\n", i,
+			__clk_get_name(shared_mem->imx_clk[i].self),
+			(u32)&(shared_mem->imx_clk[i]));
+	}
+	/* enable amp power management */
+	shared_mem->ca9_valid = SHARED_MEM_MAGIC_NUMBER;
+
+	pr_info("A9-M4 sema4 num %d, A9-M4 magic number 0x%x - 0x%x.\n",
+		amp_power_mutex->gate_num, shared_mem->ca9_valid,
+		shared_mem->cm4_valid);
+
+	return 0;
+}
+late_initcall(imx_amp_power_init);

drivers/clk/imx/clk-pfd.c

@@ -5,9 +5,11 @@
  */
 
 #include <linux/clk-provider.h>
+#include <linux/imx_sema4.h>
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/err.h>
+#include <soc/imx/src.h>
 #include "clk.h"
 
 /**
@@ -32,20 +34,57 @@ struct clk_pfd {
 #define CLR	0x8
 #define OTG	0xc
 
-static int clk_pfd_enable(struct clk_hw *hw)
+static void clk_pfd_do_hardware(struct clk_pfd *pfd, bool enable)
+{
+	if (enable)
+		writel_relaxed(1 << ((pfd->idx + 1) * 8 - 1), pfd->reg + CLR);
+	else
+		writel_relaxed(1 << ((pfd->idx + 1) * 8 - 1), pfd->reg + SET);
+}
+
+static void clk_pfd_do_shared_clks(struct clk_hw *hw, bool enable)
 {
 	struct clk_pfd *pfd = to_clk_pfd(hw);
 
-	writel_relaxed(1 << ((pfd->idx + 1) * 8 - 1), pfd->reg + CLR);
+	if (imx_src_is_m4_enabled() && clk_on_imx6sx()) {
+#ifdef CONFIG_SOC_IMX6SX
+		if (!amp_power_mutex || !shared_mem) {
+			if (enable)
+				clk_pfd_do_hardware(pfd, enable);
+			return;
+		}
+
+		imx_sema4_mutex_lock(amp_power_mutex);
+		if (shared_mem->ca9_valid != SHARED_MEM_MAGIC_NUMBER ||
+			shared_mem->cm4_valid != SHARED_MEM_MAGIC_NUMBER) {
+			imx_sema4_mutex_unlock(amp_power_mutex);
+			return;
+		}
+
+		if (!imx_update_shared_mem(hw, enable)) {
+			imx_sema4_mutex_unlock(amp_power_mutex);
+			return;
+		}
+
+		clk_pfd_do_hardware(pfd, enable);
+
+		imx_sema4_mutex_unlock(amp_power_mutex);
+#endif
+	} else {
+		clk_pfd_do_hardware(pfd, enable);
+	}
+}
+
+static int clk_pfd_enable(struct clk_hw *hw)
+{
+	clk_pfd_do_shared_clks(hw, true);
 
 	return 0;
 }
 
 static void clk_pfd_disable(struct clk_hw *hw)
 {
-	struct clk_pfd *pfd = to_clk_pfd(hw);
-
-	writel_relaxed(1 << ((pfd->idx + 1) * 8 - 1), pfd->reg + SET);
+	clk_pfd_do_shared_clks(hw, false);
 }
 
 static unsigned long clk_pfd_recalc_rate(struct clk_hw *hw,

drivers/clk/imx/clk-pllv3.c

@@ -7,9 +7,11 @@
 #include <linux/clk-provider.h>
 #include <linux/delay.h>
 #include <linux/io.h>
+#include <linux/imx_sema4.h>
 #include <linux/slab.h>
 #include <linux/jiffies.h>
 #include <linux/err.h>
+#include <soc/imx/src.h>
 #include "clk.h"
 
 #define PLL_NUM_OFFSET		0x10
@@ -72,32 +74,74 @@ static int clk_pllv3_wait_lock(struct clk_pllv3 *pll)
 	return readl_relaxed(pll->base) & BM_PLL_LOCK ? 0 : -ETIMEDOUT;
 }
 
-static int clk_pllv3_prepare(struct clk_hw *hw)
+static int clk_pllv3_do_hardware(struct clk_hw *hw, bool enable)
 {
 	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	int ret;
 	u32 val;
 
 	val = readl_relaxed(pll->base);
-	if (pll->powerup_set)
-		val |= pll->power_bit;
-	else
-		val &= ~pll->power_bit;
-	writel_relaxed(val, pll->base);
+	if (enable) {
+		if (pll->powerup_set)
+			val |= pll->power_bit;
+		else
+			val &= ~pll->power_bit;
+		writel_relaxed(val, pll->base);
 
-	return clk_pllv3_wait_lock(pll);
+		ret = clk_pllv3_wait_lock(pll);
+		if (ret)
+			return ret;
+	} else {
+		if (pll->powerup_set)
+			val &= ~pll->power_bit;
+		else
+			val |= pll->power_bit;
+		writel_relaxed(val, pll->base);
+	}
+
+	return 0;
+}
+
+static void clk_pllv3_do_shared_clks(struct clk_hw *hw, bool enable)
+{
+	if (imx_src_is_m4_enabled() && clk_on_imx6sx()) {
+#ifdef CONFIG_SOC_IMX6SX
+		if (!amp_power_mutex || !shared_mem) {
+			if (enable)
+				clk_pllv3_do_hardware(hw, enable);
+			return;
+		}
+
+		imx_sema4_mutex_lock(amp_power_mutex);
+		if (shared_mem->ca9_valid != SHARED_MEM_MAGIC_NUMBER ||
+			shared_mem->cm4_valid != SHARED_MEM_MAGIC_NUMBER) {
+			imx_sema4_mutex_unlock(amp_power_mutex);
+			return;
+		}
+
+		if (!imx_update_shared_mem(hw, enable)) {
+			imx_sema4_mutex_unlock(amp_power_mutex);
+			return;
+		}
+		clk_pllv3_do_hardware(hw, enable);
+
+		imx_sema4_mutex_unlock(amp_power_mutex);
+#endif
+	} else {
+		clk_pllv3_do_hardware(hw, enable);
+	}
+}
+
+static int clk_pllv3_prepare(struct clk_hw *hw)
+{
+	clk_pllv3_do_shared_clks(hw, true);
+
+	return 0;
 }
 
 static void clk_pllv3_unprepare(struct clk_hw *hw)
 {
-	struct clk_pllv3 *pll = to_clk_pllv3(hw);
-	u32 val;
-
-	val = readl_relaxed(pll->base);
-	if (pll->powerup_set)
-		val &= ~pll->power_bit;
-	else
-		val |= pll->power_bit;
-	writel_relaxed(val, pll->base);
+	clk_pllv3_do_shared_clks(hw, false);
 }
 
 static int clk_pllv3_is_prepared(struct clk_hw *hw)

drivers/clk/imx/clk.h

@@ -14,6 +14,8 @@ void imx_mmdc_mask_handshake(void __iomem *ccm_base, unsigned int chn);
 void imx_unregister_clocks(struct clk *clks[], unsigned int count);
 
 extern void imx_cscmr1_fixup(u32 *val);
+extern struct imx_sema4_mutex *amp_power_mutex;
+extern struct imx_shared_mem *shared_mem;
 extern bool uart_from_osc;
 
 enum imx_pllv1_type {
@@ -127,6 +129,25 @@ enum imx_pllv3_type {
 	IMX_PLLV3_AV_IMX7,
 };
 
+#define MAX_SHARED_CLK_NUMBER		100
+#define SHARED_MEM_MAGIC_NUMBER		0x12345678
+#define MCC_POWER_SHMEM_NUMBER		(6)
+
+struct imx_shared_clk {
+	struct clk *self;
+	struct clk *parent;
+	void *m4_clk;
+	void *m4_clk_parent;
+	u8 ca9_enabled;
+	u8 cm4_enabled;
+};
+
+struct imx_shared_mem {
+	u32 ca9_valid;
+	u32 cm4_valid;
+	struct imx_shared_clk imx_clk[MAX_SHARED_CLK_NUMBER];
+};
+
 struct clk_hw *imx_clk_hw_pllv3(enum imx_pllv3_type type, const char *name,
 		const char *parent_name, void __iomem *base, u32 div_mask);
 
@@ -182,6 +203,13 @@ struct clk_hw *imx_clk_hw_busy_mux(const char *name, void __iomem *reg, u8 shift
 			     u8 width, void __iomem *busy_reg, u8 busy_shift,
 			     const char * const *parent_names, int num_parents);
 
+int imx_update_shared_mem(struct clk_hw *hw, bool enable);
+
+static inline int clk_on_imx6sx(void)
+{
+	return of_machine_is_compatible("fsl,imx6sx");
+}
+
 struct clk_hw *imx7ulp_clk_composite(const char *name,
 				     const char * const *parent_names,
 				     int num_parents, bool mux_present,