From 3c9221ad27b10d649ff46e5ee89d0d54f86745eb Mon Sep 17 00:00:00 2001
From: Peng Fan <peng.fan@nxp.com>
Date: Thu, 9 Jul 2020 13:14:20 +0800
Subject: [PATCH] imx8m: add eqos clk

Add imx_eqos_txclk_set_rate/imx_get_eqos_csr_clk to override the
weak function in driver

Add set_clk_eqos to configure eQoS clk

Signed-off-by: Peng Fan <peng.fan@nxp.com>
---
 arch/arm/mach-imx/imx8m/clock_imx8mm.c | 90 ++++++++++++++++++++++++++
 1 file changed, 90 insertions(+)

diff --git a/arch/arm/mach-imx/imx8m/clock_imx8mm.c b/arch/arm/mach-imx/imx8m/clock_imx8mm.c
index 68effbd54f..3610f5b2fc 100644
--- a/arch/arm/mach-imx/imx8m/clock_imx8mm.c
+++ b/arch/arm/mach-imx/imx8m/clock_imx8mm.c
@@ -788,6 +788,96 @@ u32 mxc_get_clock(enum mxc_clock clk)
 	return 0;
 }
 
+#ifdef CONFIG_DWC_ETH_QOS
+int set_clk_eqos(enum enet_freq type)
+{
+	u32 target;
+	u32 enet1_ref;
+
+	switch (type) {
+	case ENET_125MHZ:
+		enet1_ref = ENET1_REF_CLK_ROOT_FROM_PLL_ENET_MAIN_125M_CLK;
+		break;
+	case ENET_50MHZ:
+		enet1_ref = ENET1_REF_CLK_ROOT_FROM_PLL_ENET_MAIN_50M_CLK;
+		break;
+	case ENET_25MHZ:
+		enet1_ref = ENET1_REF_CLK_ROOT_FROM_PLL_ENET_MAIN_25M_CLK;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* disable the clock first */
+	clock_enable(CCGR_QOS_ETHENET, 0);
+	clock_enable(CCGR_SDMA2, 0);
+
+	/* set enet axi clock 266Mhz */
+	target = CLK_ROOT_ON | ENET_AXI_CLK_ROOT_FROM_SYS1_PLL_266M |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(ENET_AXI_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON | enet1_ref |
+		 CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		 CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV1);
+	clock_set_target_val(ENET_QOS_CLK_ROOT, target);
+
+	target = CLK_ROOT_ON |
+		ENET1_TIME_CLK_ROOT_FROM_PLL_ENET_MAIN_100M_CLK |
+		CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+		CLK_ROOT_POST_DIV(CLK_ROOT_POST_DIV4);
+	clock_set_target_val(ENET_QOS_TIMER_CLK_ROOT, target);
+
+	/* enable clock */
+	clock_enable(CCGR_QOS_ETHENET, 1);
+	clock_enable(CCGR_SDMA2, 1);
+
+	return 0;
+}
+
+int imx_eqos_txclk_set_rate(u32 rate)
+{
+	u32 val;
+	u32 eqos_post_div;
+
+	/* disable the clock first */
+	clock_enable(CCGR_QOS_ETHENET, 0);
+	clock_enable(CCGR_SDMA2, 0);
+
+	switch (rate) {
+	case 125000000:
+		eqos_post_div = 1;
+		break;
+	case 25000000:
+		eqos_post_div = 125000000 / 25000000;
+		break;
+	case 2500000:
+		eqos_post_div = 125000000 / 2500000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	clock_get_target_val(ENET_QOS_CLK_ROOT, &val);
+	val &= ~(CLK_ROOT_PRE_DIV_MASK | CLK_ROOT_POST_DIV_MASK);
+	val |= CLK_ROOT_PRE_DIV(CLK_ROOT_PRE_DIV1) |
+	       CLK_ROOT_POST_DIV(eqos_post_div - 1);
+	clock_set_target_val(ENET_QOS_CLK_ROOT, val);
+
+	/* enable clock */
+	clock_enable(CCGR_QOS_ETHENET, 1);
+	clock_enable(CCGR_SDMA2, 1);
+
+	return 0;
+}
+
+u32 imx_get_eqos_csr_clk(void)
+{
+	return get_root_clk(ENET_AXI_CLK_ROOT);
+}
+#endif
+
 #ifdef CONFIG_FEC_MXC
 int set_clk_enet(enum enet_freq type)
 {
-- 
2.39.5