From: Lothar Waßmann <LW@KARO-electronics.de>
Date: Tue, 19 Apr 2016 14:07:37 +0000 (+0200)
Subject: arm: mx6: fix NFC clock get/set routines for i.MX6QP,i.MX6UL
X-Git-Tag: KARO-TX6-2016-04-26~6
X-Git-Url: https://git.kernelconcepts.de/?p=karo-tx-uboot.git;a=commitdiff_plain;h=da917c807a62894225badc2e29b9d57dff928afa

arm: mx6: fix NFC clock get/set routines for i.MX6QP,i.MX6UL
---

diff --git a/arch/arm/cpu/armv7/mx6/clock.c b/arch/arm/cpu/armv7/mx6/clock.c
index d3860fcf29..80f39690f6 100644
--- a/arch/arm/cpu/armv7/mx6/clock.c
+++ b/arch/arm/cpu/armv7/mx6/clock.c
@@ -595,36 +595,38 @@ static u32 get_emi_slow_clk(void)
 	return root_freq / (emi_slow_podf + 1);
 }
 
-static u32 get_nfc_clk(void)
+static inline unsigned long get_nfc_root_clk(int nfc_clk_sel)
 {
-	u32 cs2cdr = __raw_readl(&imx_ccm->cs2cdr);
-	u32 podf = (cs2cdr & MXC_CCM_CS2CDR_ENFC_CLK_PODF_MASK) >>
-		MXC_CCM_CS2CDR_ENFC_CLK_PODF_OFFSET;
-	u32 pred = (cs2cdr & MXC_CCM_CS2CDR_ENFC_CLK_PRED_MASK) >>
-		MXC_CCM_CS2CDR_ENFC_CLK_PRED_OFFSET;
-	int nfc_clk_sel = (cs2cdr & MXC_CCM_CS2CDR_ENFC_CLK_SEL_MASK) >>
-		MXC_CCM_CS2CDR_ENFC_CLK_SEL_OFFSET;
-	u32 root_freq;
-
 	switch (nfc_clk_sel) {
 	case 0:
-		root_freq = mxc_get_pll_pfd(PLL_528, 0);
+		return mxc_get_pll_pfd(PLL_528, 0);
 		break;
 	case 1:
-		root_freq = decode_pll(PLL_528, MXC_HCLK);
+		return decode_pll(PLL_528, MXC_HCLK);
 		break;
 	case 2:
-		root_freq = decode_pll(PLL_USBOTG, MXC_HCLK);
+		return decode_pll(PLL_USBOTG, MXC_HCLK);
 		break;
 	case 3:
-		root_freq = mxc_get_pll_pfd(PLL_528, 2);
+		return mxc_get_pll_pfd(PLL_528, 2);
 		break;
 	case 4:
-		root_freq = mxc_get_pll_pfd(PLL_USBOTG, 3);
-		break;
+		return mxc_get_pll_pfd(PLL_USBOTG, 3);
 	default:
 		return 0;
 	}
+}
+
+static u32 get_nfc_clk(void)
+{
+	u32 cs2cdr = __raw_readl(&imx_ccm->cs2cdr);
+	u32 podf = (cs2cdr & MXC_CCM_CS2CDR_ENFC_CLK_PODF_MASK) >>
+		MXC_CCM_CS2CDR_ENFC_CLK_PODF_OFFSET;
+	u32 pred = (cs2cdr & MXC_CCM_CS2CDR_ENFC_CLK_PRED_MASK) >>
+		MXC_CCM_CS2CDR_ENFC_CLK_PRED_OFFSET;
+	int nfc_clk_sel = (cs2cdr & MXC_CCM_CS2CDR_ENFC_CLK_SEL_MASK) >>
+		MXC_CCM_CS2CDR_ENFC_CLK_SEL_OFFSET;
+	u32 root_freq = get_nfc_root_clk(nfc_clk_sel);
 
 	return root_freq / (pred + 1) / (podf + 1);
 }
@@ -643,12 +645,13 @@ static int set_nfc_clk(u32 ref, u32 freq_khz)
 	u32 min_err = ~0;
 	u32 nfc_val = ~0;
 	u32 freq = freq_khz * 1000;
+	int num_sel = is_mx6dqp() || is_cpu_type(MXC_CPU_MX6UL) ? 5 : 4;
 
-	for (nfc_clk_sel = 0; nfc_clk_sel < 4; nfc_clk_sel++) {
+	for (nfc_clk_sel = 0; nfc_clk_sel < num_sel; nfc_clk_sel++) {
 		u32 act_freq;
 		u32 err;
 
-		if (ref < 4 && ref != nfc_clk_sel)
+		if (ref < num_sel && ref != nfc_clk_sel)
 			continue;
 
 		switch (nfc_clk_sel) {
@@ -664,6 +667,9 @@ static int set_nfc_clk(u32 ref, u32 freq_khz)
 		case 3:
 			root_freq = mxc_get_pll_pfd(PLL_528, 2);
 			break;
+		case 4:
+			root_freq = mxc_get_pll_pfd(PLL_USBOTG, 3);
+			break;
 		}
 		if (root_freq < freq)
 			continue;