+#ifdef CONFIG_VIDEO_MXS
+void lcdif_clk_enable(void)
+{
+ setbits_le32(&imx_ccm->CCGR3, MXC_CCM_CCGR3_LCDIF_MASK);
+ setbits_le32(&imx_ccm->CCGR2, MXC_CCM_CCGR2_LCD_MASK);
+}
+
+void lcdif_clk_disable(void)
+{
+ clrbits_le32(&imx_ccm->CCGR2, MXC_CCM_CCGR2_LCD_MASK);
+ clrbits_le32(&imx_ccm->CCGR3, MXC_CCM_CCGR3_LCDIF_MASK);
+}
+
+#define CBCMR_LCDIF_MASK MXC_CCM_CBCMR_LCDIF_PODF_MASK
+#define CSCDR2_LCDIF_MASK (MXC_CCM_CSCDR2_LCDIF_PRED_MASK | \
+ MXC_CCM_CSCDR2_LCDIF_CLK_SEL_MASK)
+
+static u32 get_lcdif_root_clk(u32 cscdr2)
+{
+ int lcdif_pre_clk_sel = (cscdr2 & MXC_CCM_CSCDR2_LCDIF_PRE_CLK_SEL_MASK) >>
+ MXC_CCM_CSCDR2_LCDIF_PRE_CLK_SEL_OFFSET;
+ int lcdif_clk_sel = (cscdr2 & MXC_CCM_CSCDR2_LCDIF_CLK_SEL_MASK) >>
+ MXC_CCM_CSCDR2_LCDIF_CLK_SEL_OFFSET;
+ u32 root_freq;
+
+ switch (lcdif_clk_sel) {
+ case 0:
+ switch (lcdif_pre_clk_sel) {
+ case 0:
+ root_freq = decode_pll(PLL_528, MXC_HCLK);
+ break;
+ case 1:
+ root_freq = mxc_get_pll_pfd(PLL_USBOTG, 3);
+ break;
+ case 2:
+ root_freq = decode_pll(PLL_VIDEO, MXC_HCLK);
+ break;
+ case 3:
+ root_freq = mxc_get_pll_pfd(PLL_528, 0);
+ break;
+ case 4:
+ root_freq = mxc_get_pll_pfd(PLL_528, 1);
+ break;
+ case 5:
+ root_freq = mxc_get_pll_pfd(PLL_USBOTG, 1);
+ break;
+ default:
+ return 0;
+ }
+ break;
+ case 1:
+ root_freq = mxc_get_pll_pfd(PLL_VIDEO, 0);
+ break;
+ case 2:
+ root_freq = decode_pll(PLL_USBOTG, MXC_HCLK);
+ break;
+ case 3:
+ root_freq = mxc_get_pll_pfd(PLL_528, 2);
+ break;
+ default:
+ return 0;
+ }
+
+ return root_freq;
+}
+
+static int set_lcdif_pll(u32 ref, u32 freq_khz,
+ unsigned post_div)
+{
+ int ret;
+ u64 freq = freq_khz * 1000;
+ u32 post_div_mask = 1 << (2 - post_div);
+ int mul = 1;
+ u32 min_err = ~0;
+ u32 reg;
+ int num = 0;
+ int denom = 1;
+ const int min_div = 27;
+ const int max_div = 54;
+ const int div_mask = 0x7f;
+ const u32 max_freq = ref * max_div / post_div;
+ const u32 min_freq = ref * min_div / post_div;
+
+ if (freq > max_freq || freq < min_freq) {
+ printf("Frequency %u.%03uMHz is out of range: %u.%03u..%u.%03uMHz\n",
+ freq_khz / 1000, freq_khz % 1000,
+ min_freq / 1000000, min_freq / 1000 % 1000,
+ max_freq / 1000000, max_freq / 1000 % 1000);
+ return -EINVAL;
+ }
+ {
+ int d = post_div;
+ int m = lldiv(freq * d + ref - 1, ref);
+ u32 err;
+ u32 f;
+
+ debug("%s@%d: d=%d m=%d max_div=%u min_div=%u\n", __func__, __LINE__,
+ d, m, max_div, min_div);
+ if (m > max_div || m < min_div)
+ return -EINVAL;
+
+ f = ref * m / d;
+ if (f > freq) {
+ debug("%s@%d: d=%d m=%d f=%u freq=%llu\n", __func__, __LINE__,
+ d, m, f, freq);
+ return -EINVAL;
+ }
+ err = freq - f;
+ debug("%s@%d: d=%d m=%d f=%u freq=%llu err=%d\n", __func__, __LINE__,
+ d, m, f, freq, err);
+ if (err < min_err) {
+ mul = m;
+ min_err = err;
+ }
+ }
+ if (min_err == ~0) {
+ printf("Cannot set VIDEO PLL to %u.%03uMHz\n",
+ freq_khz / 1000, freq_khz % 1000);
+ return -EINVAL;
+ }
+
+ debug("Setting M=%3u D=%u N=%d DE=%u for %u.%03uMHz (actual: %u.%03uMHz)\n",
+ mul, post_div, num, denom,
+ freq_khz / post_div / 1000, freq_khz / post_div % 1000,
+ ref * mul / post_div / 1000000,
+ ref * mul / post_div / 1000 % 1000);
+
+ reg = readl(&anatop->pll_video);
+ setbits_le32(&anatop->pll_video, BM_ANADIG_PLL_VIDEO_BYPASS);
+
+ reg = (reg & ~(div_mask |
+ BM_ANADIG_PLL_VIDEO_POST_DIV_SELECT)) |
+ mul | (post_div_mask << BP_ANADIG_PLL_VIDEO_POST_DIV_SELECT);
+ writel(reg, &anatop->pll_video);
+
+ ret = wait_pll_lock(&anatop->pll_video);
+ if (ret) {
+ printf("Video PLL failed to lock\n");
+ return ret;
+ }
+
+ clrbits_le32(&anatop->pll_video, BM_ANADIG_PLL_VIDEO_BYPASS);
+ return 0;
+}
+
+static int set_lcdif_clk(u32 ref, u32 freq_khz)
+{
+ u32 cbcmr = __raw_readl(&imx_ccm->cbcmr);
+ u32 cscdr2 = __raw_readl(&imx_ccm->cscdr2);
+ u32 cbcmr_val;
+ u32 cscdr2_val;
+ u32 freq = freq_khz * 1000;
+ u32 act_freq;
+ u32 err;
+ u32 min_div = 27;
+ u32 max_div = 54;
+ u32 min_pll_khz = ref * min_div / 4 / 1000;
+ u32 max_pll_khz = ref * max_div / 1000;
+ u32 pll_khz;
+ u32 post_div = 0;
+ u32 m;
+ u32 min_err = ~0;
+ u32 best_m = 0;
+ u32 best_pred = 1;
+ u32 best_podf = 1;
+ u32 div;
+ unsigned pd;
+
+ if (freq_khz > max_pll_khz)
+ return -EINVAL;
+
+ for (pd = 1; min_err && pd <= 4; pd <<= 1) {
+ for (m = max(min_div, DIV_ROUND_UP(648000 / pd, freq_khz * 64));
+ m <= max_div; m++) {
+ u32 err;
+ int pred = 0;
+ int podf = 0;
+ u32 root_freq = ref * m / pd;
+
+ div = DIV_ROUND_UP(root_freq, freq);
+
+ while (pred * podf == 0 && div <= 64) {
+ int p1, p2;
+
+ for (p1 = 1; p1 <= 8; p1++) {
+ for (p2 = 1; p2 <= 8; p2++) {
+ if (p1 * p2 == div) {
+ podf = p1;
+ pred = p2;
+ break;
+ }
+ }
+ }
+ if (pred * podf == 0) {
+ div++;
+ }
+ }
+ if (pred * podf == 0)
+ continue;
+
+ /* relative error in per mille */
+ act_freq = root_freq / div;
+ err = abs(act_freq - freq) / freq_khz;
+
+ if (err < min_err) {
+ best_m = m;
+ best_pred = pred;
+ best_podf = podf;
+ post_div = pd;
+ min_err = err;
+ if (err <= 10)
+ break;
+ }
+ }
+ }
+ if (min_err > 50)
+ return -EINVAL;
+
+ pll_khz = ref / 1000 * best_m;
+ if (pll_khz > max_pll_khz)
+ return -EINVAL;
+
+ if (pll_khz < min_pll_khz)
+ return -EINVAL;
+
+ err = set_lcdif_pll(ref, pll_khz / post_div, post_div);
+ if (err)
+ return err;
+
+ cbcmr_val = (best_podf - 1) << MXC_CCM_CBCMR_LCDIF_PODF_OFFSET;
+ cscdr2_val = (best_pred - 1) << MXC_CCM_CSCDR2_LCDIF_PRED_OFFSET;
+
+ if ((cbcmr & CBCMR_LCDIF_MASK) != cbcmr_val) {
+ debug("changing cbcmr from %08x to %08x\n", cbcmr,
+ (cbcmr & ~CBCMR_LCDIF_MASK) | cbcmr_val);
+ clrsetbits_le32(&imx_ccm->cbcmr,
+ CBCMR_LCDIF_MASK,
+ cbcmr_val);
+ } else {
+ debug("Leaving cbcmr unchanged [%08x]\n", cbcmr);
+ }
+ if ((cscdr2 & CSCDR2_LCDIF_MASK) != cscdr2_val) {
+ debug("changing cscdr2 from %08x to %08x\n", cscdr2,
+ (cscdr2 & ~CSCDR2_LCDIF_MASK) | cscdr2_val);
+ clrsetbits_le32(&imx_ccm->cscdr2,
+ CSCDR2_LCDIF_MASK,
+ cscdr2_val);
+ } else {
+ debug("Leaving cscdr2 unchanged [%08x]\n", cscdr2);
+ }
+ return 0;
+}
+
+void mxs_set_lcdclk(u32 khz)
+{
+ set_lcdif_clk(CONFIG_SYS_MX6_HCLK, khz);
+}
+
+static u32 get_lcdif_clk(void)
+{
+ u32 cbcmr = __raw_readl(&imx_ccm->cbcmr);
+ u32 podf = ((cbcmr & MXC_CCM_CBCMR_LCDIF_PODF_MASK) >>
+ MXC_CCM_CBCMR_LCDIF_PODF_OFFSET) + 1;
+ u32 cscdr2 = __raw_readl(&imx_ccm->cscdr2);
+ u32 pred = ((cscdr2 & MXC_CCM_CSCDR2_LCDIF_PRED_MASK) >>
+ MXC_CCM_CSCDR2_LCDIF_PRED_OFFSET) + 1;
+ u32 root_freq = get_lcdif_root_clk(cscdr2);
+
+ return root_freq / pred / podf;
+}
+#endif
+