make mxc_set_clock() actually functional

diff --git a/arch/arm/cpu/armv7/mx6/clock.c b/arch/arm/cpu/armv7/mx6/clock.c
index 41d2b049efdf8ed3ccb4a7649ea47e354c20e467..7c95db18471ff310a8bc5a75b537e789fc4e3e3c 100644 (file)
--- a/arch/arm/cpu/armv7/mx6/clock.c
+++ b/arch/arm/cpu/armv7/mx6/clock.c
@@ -19,7 +19,6 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  * MA 02111-1307 USA
  */
-//#define DEBUG
 
 #include <common.h>
 #include <asm/io.h>
@@ -342,10 +341,83 @@ static u32 get_nfc_clk(void)
                root_freq = PLL2_PFD2_FREQ;
                break;
        }
+       debug("root=%d[%u] freq=%u pred=%u podf=%u\n", nfc_clk_sel,
+               root_freq, root_freq / (pred + 1) / (podf + 1), pred + 1, podf + 1);
 
        return root_freq / (pred + 1) / (podf + 1);
 }
 
+#define CS2CDR_ENFC_MASK       (MXC_CCM_CS2CDR_ENFC_CLK_PODF_MASK |    \
+                               MXC_CCM_CS2CDR_ENFC_CLK_PRED_MASK |     \
+                               MXC_CCM_CS2CDR_ENFC_CLK_SEL_MASK)
+
+static int set_nfc_clk(u32 ref, u32 freq_khz)
+{
+       u32 cs2cdr = __raw_readl(&imx_ccm->cs2cdr);
+       u32 podf;
+       u32 pred;
+       int nfc_clk_sel;
+       u32 root_freq;
+       u32 min_err = ~0;
+       u32 nfc_val = ~0;
+       u32 freq = freq_khz * 1000;
+
+       for (nfc_clk_sel = 0; nfc_clk_sel < 4; nfc_clk_sel++) {
+               u32 act_freq;
+               u32 err;
+
+               if (ref < 4 && ref != nfc_clk_sel)
+                       continue;
+
+               switch (nfc_clk_sel) {
+               case 0:
+                       root_freq = PLL2_PFD0_FREQ;
+                       break;
+               case 1:
+                       root_freq = decode_pll(PLL_BUS, MXC_HCLK);
+                       break;
+               case 2:
+                       root_freq = decode_pll(PLL_USBOTG, MXC_HCLK);
+                       break;
+               case 3:
+                       root_freq = PLL2_PFD2_FREQ;
+                       break;
+               }
+               if (root_freq < freq)
+                       continue;
+
+               podf = min(DIV_ROUND_UP(root_freq, freq), 1 << 6);
+               pred = min(DIV_ROUND_UP(root_freq / podf, freq), 8);
+               act_freq = root_freq / pred / podf;
+               err = (freq - act_freq) * 100 / freq;
+               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)
+                       continue;
+               if (err < min_err) {
+                       nfc_val = (podf - 1) << MXC_CCM_CS2CDR_ENFC_CLK_PODF_OFFSET;
+                       nfc_val |= (pred - 1) << MXC_CCM_CS2CDR_ENFC_CLK_PRED_OFFSET;
+                       nfc_val |= nfc_clk_sel << MXC_CCM_CS2CDR_ENFC_CLK_SEL_OFFSET;
+                       min_err = err;
+                       if (err == 0)
+                               break;
+               }
+       }
+
+       if (nfc_val == ~0 || min_err > 10)
+               return -EINVAL;
+
+       if ((cs2cdr & CS2CDR_ENFC_MASK) != nfc_val) {
+               debug("changing cs2cdr from %08x to %08x\n", cs2cdr,
+                       (cs2cdr & ~CS2CDR_ENFC_MASK) | nfc_val);
+               __raw_writel((cs2cdr & ~CS2CDR_ENFC_MASK) | nfc_val,
+                       &imx_ccm->cs2cdr);
+       } else {
+               debug("Leaving cs2cdr unchanged [%08x]\n", cs2cdr);
+       }
+       return 0;
+}
+
 static u32 get_mmdc_ch0_clk(void)
 {
        u32 cbcdr = __raw_readl(&imx_ccm->cbcdr);
@@ -521,122 +593,118 @@ static inline int gcd(int m, int n)
 }
 
 /* Config CPU clock */
-static int config_core_clk(u32 ref, u32 freq)
+static int set_arm_clk(u32 ref, u32 freq_khz)
 {
        int d;
        int div = 0;
        int mul = 0;
-       int min_err = ~0 >> 1;
+       u32 min_err = ~0;
        u32 reg;
 
-       if (freq / ref > 108 || freq / ref * 8 < 54) {
+       if (freq_khz > ref * 108 / 2000 || freq_khz < ref * 54 / 16000) {
+               printf("Frequency %u.%03uMHz is out of range: %u.%03u..%u.%03u\n",
+                       freq_khz / 1000, freq_khz % 1000,
+                       54 * ref / 16000000, 54 * ref / 16000 % 1000,
+                       108 * ref / 2000000, 108 * ref / 2000 % 1000);
                return -EINVAL;
        }
 
-       for (d = 1; d < 8; d++) {
-               int m = (freq + (ref - 1)) / ref;
-               unsigned long f;
-               int err;
+       for (d = DIV_ROUND_UP(648000, freq_khz); d <= 8; d++) {
+               int m = freq_khz * 2 * d / (ref / 1000);
+               u32 f;
+               u32 err;
 
-               if (m > 108 || m < 54)
-                       return -EINVAL;
+               if (m > 108 || m < 54) {
+                       debug("%s@%d: d=%d m=%d\n", __func__, __LINE__,
+                               d, m);
+                       continue;
+               }
 
-               f = ref * m / d;
-               while (f > freq) {
+               f = ref * m / d / 2;
+               if (f > freq_khz * 1000) {
+                       debug("%s@%d: d=%d m=%d f=%u freq=%u\n", __func__, __LINE__,
+                               d, m, f, freq_khz);
                        if (--m < 54)
                                return -EINVAL;
-                       f = ref * m / d;
+                       f = ref * m / d / 2;
                }
-               err = freq - f;
-               if (err == 0)
-                       break;
-               if (err < 0)
-                       return -EINVAL;
+               err = freq_khz * 1000 - f;
+               debug("%s@%d: d=%d m=%d f=%u freq=%u err=%d\n", __func__, __LINE__,
+                       d, m, f, freq_khz, err);
                if (err < min_err) {
                        mul = m;
                        div = d;
+                       min_err = err;
+                       if (err == 0)
+                               break;
                }
        }
-       printf("Setting M=%3u D=%2u for %u.%03uMHz (actual: %u.%03uMHz)\n",
-               mul, div, freq / 1000000, freq / 1000 % 1000,
-               ref * mul / div / 1000000, ref * mul / div / 1000 % 1000);
+       if (min_err == ~0)
+               return -EINVAL;
+       debug("Setting M=%3u D=%2u for %u.%03uMHz (actual: %u.%03uMHz)\n",
+               mul, div, freq_khz / 1000, freq_khz % 1000,
+               ref * mul / 2 / div / 1000000, ref * mul / 2 / div / 1000 % 1000);
 
        reg = readl(&anatop->pll_arm);
-       printf("anadig_pll_arm=%08x -> %08x\n",
+       debug("anadig_pll_arm=%08x -> %08x\n",
                reg, (reg & ~0x7f) | mul);
-#if 0
-       writel(div - 1, &imx_ccm->caccr);
-       reg &= 0x7f;
-       writel(reg | mul, &anatop->pll_arm);
-#endif
+
+       reg |= 1 << 16;
+       writel(reg, &anatop->pll_arm); /* bypass PLL */
+
+       reg = (reg & ~0x7f) | mul;
+       writel(reg, &anatop->pll_arm);
+
+       writel(div - 1, &imx_ccm->cacrr);
+
+       reg &= ~(1 << 16);
+       writel(reg, &anatop->pll_arm); /* disable PLL bypass */
+
        return 0;
 }
 
-/*
- * This function assumes the expected core clock has to be changed by
- * modifying the PLL. This is NOT true always but for most of the times,
- * it is. So it assumes the PLL output freq is the same as the expected
- * core clock (presc=1) unless the core clock is less than PLL_FREQ_MIN.
- * In the latter case, it will try to increase the presc value until
- * (presc*core_clk) is greater than PLL_FREQ_MIN. It then makes call to
- * calc_pll_params() and obtains the values of PD, MFI,MFN, MFD based
- * on the targeted PLL and reference input clock to the PLL. Lastly,
- * it sets the register based on these values along with the dividers.
- * Note 1) There is no value checking for the passed-in divider values
- *         so the caller has to make sure those values are sensible.
- *      2) Also adjust the NFC divider such that the NFC clock doesn't
- *         exceed NFC_CLK_MAX.
- *      3) IPU HSP clock is independent of AHB clock. Even it can go up to
- *         177MHz for higher voltage, this function fixes the max to 133MHz.
- *      4) This function should not have allowed diag_printf() calls since
- *         the serial driver has been stoped. But leave then here to allow
- *         easy debugging by NOT calling the cyg_hal_plf_serial_stop().
- */
 int mxc_set_clock(u32 ref, u32 freq, enum mxc_clock clk)
 {
-       freq *= 1000000;
+       int ret;
+
+       freq *= 1000;
 
        switch (clk) {
        case MXC_ARM_CLK:
-               if (config_core_clk(ref, freq))
-                       return -EINVAL;
-               break;
-#if 0
-       case MXC_PER_CLK:
-               if (config_periph_clk(ref, freq))
-                       return -EINVAL;
-               break;
-       case MXC_DDR_CLK:
-               if (config_ddr_clk(freq))
-                       return -EINVAL;
+               ret = set_arm_clk(ref, freq);
                break;
+
        case MXC_NFC_CLK:
-               if (config_nfc_clk(freq))
-                       return -EINVAL;
+               ret = set_nfc_clk(ref, freq);
                break;
-#endif
+
        default:
                printf("Warning: Unsupported or invalid clock type: %d\n",
                        clk);
                return -EINVAL;
        }
 
-       return 0;
+       return ret;
 }
 
 /*
  * Dump some core clocks.
  */
-#define print_pll(pll) printf("%-12s %4d.%03d MHz\n", #pll,            \
-                               decode_pll(pll, MXC_HCLK) / 1000000,    \
-                               decode_pll(pll, MXC_HCLK) / 1000 % 1000)
+#define print_pll(pll) {                               \
+       u32 __pll = decode_pll(pll, MXC_HCLK);          \
+       printf("%-12s %4d.%03d MHz\n", #pll,            \
+               __pll / 1000000, __pll / 1000 % 1000);  \
+       }
 
 #define MXC_IPG_PER_CLK        MXC_IPG_PERCLK
-#define print_clk(clk) printf("%-12s %4d.%03d MHz\n", #clk,            \
-                               mxc_get_clock(MXC_##clk##_CLK) / 1000000, \
-                               mxc_get_clock(MXC_##clk##_CLK) / 1000 % 1000)
 
-int do_mx6_showclocks(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+#define print_clk(clk) {                               \
+       u32 __clk = mxc_get_clock(MXC_##clk##_CLK);     \
+       printf("%-12s %4d.%03d MHz\n", #clk,            \
+               __clk / 1000000, __clk / 1000 % 1000);  \
+       }
+
+static void do_mx6_showclocks(void)
 {
        print_pll(PLL_ARM);
        print_pll(PLL_BUS);
@@ -661,14 +729,81 @@ int do_mx6_showclocks(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
        print_clk(NFC);
        print_clk(IPG_PER);
        print_clk(ARM);
+}
 
-       return 0;
+static struct clk_lookup {
+       const char *name;
+       unsigned int index;
+} mx6_clk_lookup[] = {
+       { "arm", MXC_ARM_CLK, },
+       { "nfc", MXC_NFC_CLK, },
+};
+
+int do_clocks(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
+{
+       int i;
+       unsigned long freq;
+       unsigned long ref = ~0UL;
+
+       if (argc < 2) {
+               do_mx6_showclocks();
+               return CMD_RET_SUCCESS;
+       } else if (argc == 2 || argc > 4) {
+               return CMD_RET_USAGE;
+       }
+
+       freq = simple_strtoul(argv[2], NULL, 0);
+       if (freq == 0) {
+               printf("Invalid clock frequency %lu\n", freq);
+               return CMD_RET_FAILURE;
+       }
+       if (argc > 3) {
+               ref = simple_strtoul(argv[3], NULL, 0);
+       }
+       for (i = 0; i < ARRAY_SIZE(mx6_clk_lookup); i++) {
+               if (strcasecmp(argv[1], mx6_clk_lookup[i].name) == 0) {
+                       switch (mx6_clk_lookup[i].index) {
+                       case MXC_ARM_CLK:
+                               if (argc > 3)
+                                       return CMD_RET_USAGE;
+                               ref = CONFIG_SYS_MX6_HCLK;
+                               break;
+
+                       case MXC_NFC_CLK:
+                               if (argc > 3 && ref > 3) {
+                                       printf("Invalid clock selector value: %lu\n", ref);
+                                       return CMD_RET_FAILURE;
+                               }
+                               break;
+                       }
+                       printf("Setting %s clock to %lu MHz\n",
+                               mx6_clk_lookup[i].name, freq);
+                       if (mxc_set_clock(ref, freq, mx6_clk_lookup[i].index))
+                               break;
+                       freq = mxc_get_clock(mx6_clk_lookup[i].index);
+                       printf("%s clock set to %lu.%03lu MHz\n",
+                               mx6_clk_lookup[i].name,
+                               freq / 1000000, freq / 1000 % 1000);
+                       return CMD_RET_SUCCESS;
+               }
+       }
+       if (i == ARRAY_SIZE(mx6_clk_lookup)) {
+               printf("clock %s not found; supported clocks are:\n", argv[1]);
+               for (i = 0; i < ARRAY_SIZE(mx6_clk_lookup); i++) {
+                       printf("\t%s\n", mx6_clk_lookup[i].name);
+               }
+       } else {
+               printf("Failed to set clock %s to %s MHz\n",
+                       argv[1], argv[2]);
+       }
+       return CMD_RET_FAILURE;
 }
 
 /***************************************************/
 
 U_BOOT_CMD(
-       clocks, CONFIG_SYS_MAXARGS, 1, do_mx6_showclocks,
-       "display clocks",
-       ""
+       clocks, 4, 0, do_clocks,
+       "display/set clocks",
+       "                    - display clock settings\n"
+       "clocks <clkname> <freq>    - set clock <clkname> to <freq> MHz"
 );