MLK-12068 imx: mx6ul/sx: fix mmdc_ch0 clk calculation

Fix mmdc_ch0 clk calculation. Also add PLL_AUDIO/VIDEO support
for decode_pll.

Reported-by: Bai Ping <ping.bai@nxp.com>
Signed-off-by: Peng Fan <peng.fan@nxp.com>

1 files changed, 57 insertions, 3 deletions

diff --git a/arch/arm/cpu/armv7/mx6/clock.c b/arch/arm/cpu/armv7/mx6/clock.c
index 1ef307a..5f0037a 100644
--- a/arch/arm/cpu/armv7/mx6/clock.c
+++ b/arch/arm/cpu/armv7/mx6/clock.c
@@ -18,6 +18,8 @@ enum pll_clocks {
 	PLL_BUS,	/* System Bus PLL*/
 	PLL_USBOTG,	/* OTG USB PLL */
 	PLL_ENET,	/* ENET PLL */
+	PLL_AUDIO,	/* AUDIO PLL */
+	PLL_VIDEO,	/* AUDIO PLL */
 };
 
 struct mxc_ccm_reg *imx_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
@@ -222,7 +224,7 @@ int enable_spi_clk(unsigned char enable, unsigned spi_num)
 }
 static u32 decode_pll(enum pll_clocks pll, u32 infreq)
 {
-	u32 div;
+	u32 div, test_div, pll_num, pll_denom;
 
 	switch (pll) {
 	case PLL_SYS:
@@ -245,6 +247,44 @@ static u32 decode_pll(enum pll_clocks pll, u32 infreq)
 		div &= BM_ANADIG_PLL_ENET_DIV_SELECT;
 
 		return 25000000 * (div + (div >> 1) + 1);
+	case PLL_AUDIO:
+		div = __raw_readl(&imx_ccm->analog_pll_audio);
+		if (!(div & BM_ANADIG_PLL_AUDIO_ENABLE))
+			return 0;
+		/* BM_ANADIG_PLL_AUDIO_BYPASS_CLK_SRC is ignored */
+		if (div & BM_ANADIG_PLL_AUDIO_BYPASS)
+			return MXC_HCLK;
+		pll_num = __raw_readl(&imx_ccm->analog_pll_audio_num);
+		pll_denom = __raw_readl(&imx_ccm->analog_pll_audio_denom);
+		test_div = (div & BM_ANADIG_PLL_AUDIO_TEST_DIV_SELECT) >>
+			BP_ANADIG_PLL_AUDIO_TEST_DIV_SELECT;
+		div &= BM_ANADIG_PLL_AUDIO_DIV_SELECT;
+		if (test_div == 3) {
+			debug("Error test_div\n");
+			return 0;
+		}
+		test_div = 1 << (2 - test_div);
+
+		return infreq * (div + pll_num / pll_denom) / test_div;
+	case PLL_VIDEO:
+		div = __raw_readl(&imx_ccm->analog_pll_video);
+		if (!(div & BM_ANADIG_PLL_VIDEO_ENABLE))
+			return 0;
+		/* BM_ANADIG_PLL_AUDIO_BYPASS_CLK_SRC is ignored */
+		if (div & BM_ANADIG_PLL_VIDEO_BYPASS)
+			return MXC_HCLK;
+		pll_num = __raw_readl(&imx_ccm->analog_pll_video_num);
+		pll_denom = __raw_readl(&imx_ccm->analog_pll_video_denom);
+		test_div = (div & BM_ANADIG_PLL_VIDEO_TEST_DIV_SELECT) >>
+			BP_ANADIG_PLL_VIDEO_TEST_DIV_SELECT;
+		div &= BM_ANADIG_PLL_VIDEO_DIV_SELECT;
+		if (test_div == 3) {
+			debug("Error test_div\n");
+			return 0;
+		}
+		test_div = 1 << (2 - test_div);
+
+		return infreq * (div + pll_num / pll_denom) / test_div;
 	default:
 		return 0;
 	}
@@ -454,7 +494,7 @@ static u32 get_mmdc_ch0_clk(void)
 {
 	u32 cbcmr = __raw_readl(&imx_ccm->cbcmr);
 	u32 cbcdr = __raw_readl(&imx_ccm->cbcdr);
-	u32 freq, mmdc_podf, per2_clk2_podf;
+	u32 freq, mmdc_podf, per2_clk2_podf, misc2_audio_podf;
 
 	mmdc_podf = (cbcdr & MXC_CCM_CBCDR_MMDC_CH1_PODF_MASK)
 		     >> MXC_CCM_CBCDR_MMDC_CH1_PODF_OFFSET;
@@ -480,7 +520,21 @@ static u32 get_mmdc_ch0_clk(void)
 			break;
 		case 3:
 			/* static / 2 divider */
-			freq =  mxc_get_pll_pfd(PLL_BUS, 2) / 2;
+			misc2_audio_podf = ((__raw_readl(&imx_ccm->ana_misc2) >> 22) & 2) |
+					    ((__raw_readl(&imx_ccm->ana_misc2) >> 15) & 1);
+			switch (misc2_audio_podf) {
+			case 0:
+			case 2:
+				misc2_audio_podf = 1;
+				break;
+			case 1:
+				misc2_audio_podf = 2;
+				break;
+			case 3:
+				misc2_audio_podf = 4;
+				break;
+			}
+			freq = decode_pll(PLL_AUDIO, MXC_HCLK) / misc2_audio_podf;
 			break;
 		}
 	}