+static inline int gcd(int m, int n)
+{
+ int t;
+ while (m > 0) {
+ if (n > m) {
+ t = m;
+ m = n;
+ n = t;
+ } /* swap */
+ m -= n;
+ }
+ return n;
+}
+
+/* Config CPU clock */
+static int set_arm_clk(u32 ref, u32 freq_khz)
+{
+ int d;
+ int div = 0;
+ int mul = 0;
+ u32 min_err = ~0;
+ u32 reg;
+
+ if (freq_khz > ref / 1000 * 108 / 2 || freq_khz < ref / 1000 * 54 / 8 / 2) {
+ printf("Frequency %u.%03uMHz is out of range: %u.%03u..%u.%03u\n",
+ freq_khz / 1000, freq_khz % 1000,
+ 54 * ref / 1000000 / 8 / 2, 54 * ref / 1000 / 8 / 2 % 1000,
+ 108 * ref / 1000000 / 2, 108 * ref / 1000 / 2 % 1000);
+ return -EINVAL;
+ }
+
+ 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) {
+ debug("%s@%d: d=%d m=%d\n", __func__, __LINE__,
+ d, m);
+ break;
+ }
+
+ 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 / 2;
+ }
+ 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;
+ }
+ }
+ 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);
+ debug("anadig_pll_arm=%08x -> %08x\n",
+ reg, (reg & ~0x7f) | mul);
+
+ 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)
+{
+ int ret;
+
+ freq *= 1000;
+
+ switch (clk) {
+ case MXC_ARM_CLK:
+ ret = set_arm_clk(ref, freq);
+ break;
+
+ case MXC_NFC_CLK:
+ ret = set_nfc_clk(ref, freq);
+ break;
+
+ default:
+ printf("Warning: Unsupported or invalid clock type: %d\n",
+ clk);
+ return -EINVAL;
+ }
+
+ return ret;
+}
+