mx6: clock: use setup_gpmi_io_clk() to change nfc clk divider for CONFIG_NAND_MXS

[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..c3c9c5e6512ba13ebacd96181056e915db9e14da 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;
@@ -671,7 +677,7 @@ static int set_nfc_clk(u32 ref, u32 freq_khz)
                podf = min(DIV_ROUND_UP(root_freq, freq), 1U << 6);
                pred = min(DIV_ROUND_UP(root_freq / podf, freq), 8U);
                act_freq = root_freq / pred / podf;
-               err = (freq - act_freq) * 100 / freq;
+               err = (freq - act_freq) / (freq / 1000);
                debug("root=%d[%u] freq=%u pred=%u podf=%u act=%u err=%d\n",
                        nfc_clk_sel, root_freq, freq, pred, podf, act_freq, err);
                if (act_freq > freq)
@@ -686,14 +692,18 @@ static int set_nfc_clk(u32 ref, u32 freq_khz)
                }
        }
 
-       if (nfc_val == ~0 || min_err > 10)
+       if (nfc_val == ~0 || min_err > 100)
                return -EINVAL;
 
        if ((cs2cdr & CS2CDR_ENFC_MASK) != nfc_val) {
                debug("changing cs2cdr from %08x to %08x\n", cs2cdr,
                        (cs2cdr & ~CS2CDR_ENFC_MASK) | nfc_val);
+#ifdef CONFIG_NAND_MXS
+               setup_gpmi_io_clk(nfc_val);
+#else
                __raw_writel((cs2cdr & ~CS2CDR_ENFC_MASK) | nfc_val,
                        &imx_ccm->cs2cdr);
+#endif
        } else {
                debug("Leaving cs2cdr unchanged [%08x]\n", cs2cdr);
        }