+ 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;