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

[karo-tx-uboot.git] / arch / arm / cpu / armv7 / mx6 / clock.c

diff --git a/arch/arm/cpu/armv7/mx6/clock.c b/arch/arm/cpu/armv7/mx6/clock.c
index d3860fcf2970a28a2d934ee742570a58a458ab66..80f39690f65191bab0599cbf0d6a7ac9e56b51ac 100644 (file)
--- 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;