420 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (C) 2023 Inochi Amaoto <inochiama@outlook.com>4 */5 6#include <linux/clk-provider.h>7#include <linux/io.h>8#include <linux/limits.h>9#include <linux/spinlock.h>10 11#include "clk-cv18xx-pll.h"12 13static inline struct cv1800_clk_pll *hw_to_cv1800_clk_pll(struct clk_hw *hw)14{15 struct cv1800_clk_common *common = hw_to_cv1800_clk_common(hw);16 17 return container_of(common, struct cv1800_clk_pll, common);18}19 20static unsigned long ipll_calc_rate(unsigned long parent_rate,21 unsigned long pre_div_sel,22 unsigned long div_sel,23 unsigned long post_div_sel)24{25 uint64_t rate = parent_rate;26 27 rate *= div_sel;28 do_div(rate, pre_div_sel * post_div_sel);29 30 return rate;31}32 33static unsigned long ipll_recalc_rate(struct clk_hw *hw,34 unsigned long parent_rate)35{36 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);37 u32 value;38 39 value = readl(pll->common.base + pll->pll_reg);40 41 return ipll_calc_rate(parent_rate,42 PLL_GET_PRE_DIV_SEL(value),43 PLL_GET_DIV_SEL(value),44 PLL_GET_POST_DIV_SEL(value));45}46 47static int ipll_find_rate(const struct cv1800_clk_pll_limit *limit,48 unsigned long prate, unsigned long *rate,49 u32 *value)50{51 unsigned long best_rate = 0;52 unsigned long trate = *rate;53 unsigned long pre_div_sel = 0, div_sel = 0, post_div_sel = 0;54 unsigned long pre, div, post;55 u32 detected = *value;56 unsigned long tmp;57 58 for_each_pll_limit_range(pre, &limit->pre_div) {59 for_each_pll_limit_range(div, &limit->div) {60 for_each_pll_limit_range(post, &limit->post_div) {61 tmp = ipll_calc_rate(prate, pre, div, post);62 63 if (tmp > trate)64 continue;65 66 if ((trate - tmp) < (trate - best_rate)) {67 best_rate = tmp;68 pre_div_sel = pre;69 div_sel = div;70 post_div_sel = post;71 }72 }73 }74 }75 76 if (best_rate) {77 detected = PLL_SET_PRE_DIV_SEL(detected, pre_div_sel);78 detected = PLL_SET_POST_DIV_SEL(detected, post_div_sel);79 detected = PLL_SET_DIV_SEL(detected, div_sel);80 *value = detected;81 *rate = best_rate;82 return 0;83 }84 85 return -EINVAL;86}87 88static int ipll_determine_rate(struct clk_hw *hw, struct clk_rate_request *req)89{90 u32 val;91 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);92 93 return ipll_find_rate(pll->pll_limit, req->best_parent_rate,94 &req->rate, &val);95}96 97static void pll_get_mode_ctrl(unsigned long div_sel,98 bool (*mode_ctrl_check)(unsigned long,99 unsigned long,100 unsigned long),101 const struct cv1800_clk_pll_limit *limit,102 u32 *value)103{104 unsigned long ictrl = 0, mode = 0;105 u32 detected = *value;106 107 for_each_pll_limit_range(mode, &limit->mode) {108 for_each_pll_limit_range(ictrl, &limit->ictrl) {109 if (mode_ctrl_check(div_sel, ictrl, mode)) {110 detected = PLL_SET_SEL_MODE(detected, mode);111 detected = PLL_SET_ICTRL(detected, ictrl);112 *value = detected;113 return;114 }115 }116 }117}118 119static bool ipll_check_mode_ctrl_restrict(unsigned long div_sel,120 unsigned long ictrl,121 unsigned long mode)122{123 unsigned long left_rest = 20 * div_sel;124 unsigned long right_rest = 35 * div_sel;125 unsigned long test = 184 * (1 + mode) * (1 + ictrl) / 2;126 127 return test > left_rest && test <= right_rest;128}129 130static int ipll_set_rate(struct clk_hw *hw, unsigned long rate,131 unsigned long parent_rate)132{133 u32 regval, detected = 0;134 unsigned long flags;135 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);136 137 ipll_find_rate(pll->pll_limit, parent_rate, &rate, &detected);138 pll_get_mode_ctrl(PLL_GET_DIV_SEL(detected),139 ipll_check_mode_ctrl_restrict,140 pll->pll_limit, &detected);141 142 spin_lock_irqsave(pll->common.lock, flags);143 144 regval = readl(pll->common.base + pll->pll_reg);145 regval = PLL_COPY_REG(regval, detected);146 147 writel(regval, pll->common.base + pll->pll_reg);148 149 spin_unlock_irqrestore(pll->common.lock, flags);150 151 cv1800_clk_wait_for_lock(&pll->common, pll->pll_status.reg,152 BIT(pll->pll_status.shift));153 154 return 0;155}156 157static int pll_enable(struct clk_hw *hw)158{159 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);160 161 return cv1800_clk_clearbit(&pll->common, &pll->pll_pwd);162}163 164static void pll_disable(struct clk_hw *hw)165{166 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);167 168 cv1800_clk_setbit(&pll->common, &pll->pll_pwd);169}170 171static int pll_is_enable(struct clk_hw *hw)172{173 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);174 175 return cv1800_clk_checkbit(&pll->common, &pll->pll_pwd) == 0;176}177 178const struct clk_ops cv1800_clk_ipll_ops = {179 .disable = pll_disable,180 .enable = pll_enable,181 .is_enabled = pll_is_enable,182 183 .recalc_rate = ipll_recalc_rate,184 .determine_rate = ipll_determine_rate,185 .set_rate = ipll_set_rate,186};187 188#define PLL_SYN_FACTOR_DOT_POS 26189#define PLL_SYN_FACTOR_MINIMUM ((4 << PLL_SYN_FACTOR_DOT_POS) + 1)190 191static bool fpll_is_factional_mode(struct cv1800_clk_pll *pll)192{193 return cv1800_clk_checkbit(&pll->common, &pll->pll_syn->en);194}195 196static unsigned long fpll_calc_rate(unsigned long parent_rate,197 unsigned long pre_div_sel,198 unsigned long div_sel,199 unsigned long post_div_sel,200 unsigned long ssc_syn_set,201 bool is_full_parent)202{203 u64 dividend = parent_rate * div_sel;204 u64 factor = ssc_syn_set * pre_div_sel * post_div_sel;205 unsigned long rate;206 207 dividend <<= PLL_SYN_FACTOR_DOT_POS - 1;208 rate = div64_u64_rem(dividend, factor, ÷nd);209 210 if (is_full_parent) {211 dividend <<= 1;212 rate <<= 1;213 }214 215 rate += DIV64_U64_ROUND_CLOSEST(dividend, factor);216 217 return rate;218}219 220static unsigned long fpll_recalc_rate(struct clk_hw *hw,221 unsigned long parent_rate)222{223 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);224 u32 value;225 bool clk_full;226 u32 syn_set;227 228 if (!fpll_is_factional_mode(pll))229 return ipll_recalc_rate(hw, parent_rate);230 231 syn_set = readl(pll->common.base + pll->pll_syn->set);232 233 if (syn_set == 0)234 return 0;235 236 clk_full = cv1800_clk_checkbit(&pll->common,237 &pll->pll_syn->clk_half);238 239 value = readl(pll->common.base + pll->pll_reg);240 241 return fpll_calc_rate(parent_rate,242 PLL_GET_PRE_DIV_SEL(value),243 PLL_GET_DIV_SEL(value),244 PLL_GET_POST_DIV_SEL(value),245 syn_set, clk_full);246}247 248static unsigned long fpll_find_synthesizer(unsigned long parent,249 unsigned long rate,250 unsigned long pre_div,251 unsigned long div,252 unsigned long post_div,253 bool is_full_parent,254 u32 *ssc_syn_set)255{256 u32 test_max = U32_MAX, test_min = PLL_SYN_FACTOR_MINIMUM;257 unsigned long trate;258 259 while (test_min < test_max) {260 u32 tssc = (test_max + test_min) / 2;261 262 trate = fpll_calc_rate(parent, pre_div, div, post_div,263 tssc, is_full_parent);264 265 if (trate == rate) {266 test_min = tssc;267 break;268 }269 270 if (trate > rate)271 test_min = tssc + 1;272 else273 test_max = tssc - 1;274 }275 276 if (trate != 0)277 *ssc_syn_set = test_min;278 279 return trate;280}281 282static int fpll_find_rate(struct cv1800_clk_pll *pll,283 const struct cv1800_clk_pll_limit *limit,284 unsigned long prate,285 unsigned long *rate,286 u32 *value, u32 *ssc_syn_set)287{288 unsigned long best_rate = 0;289 unsigned long pre_div_sel = 0, div_sel = 0, post_div_sel = 0;290 unsigned long pre, div, post;291 unsigned long trate = *rate;292 u32 detected = *value;293 unsigned long tmp;294 bool clk_full = cv1800_clk_checkbit(&pll->common,295 &pll->pll_syn->clk_half);296 297 for_each_pll_limit_range(pre, &limit->pre_div) {298 for_each_pll_limit_range(post, &limit->post_div) {299 for_each_pll_limit_range(div, &limit->div) {300 tmp = fpll_find_synthesizer(prate, trate,301 pre, div, post,302 clk_full,303 ssc_syn_set);304 305 if ((trate - tmp) < (trate - best_rate)) {306 best_rate = tmp;307 pre_div_sel = pre;308 div_sel = div;309 post_div_sel = post;310 }311 }312 }313 }314 315 if (best_rate) {316 detected = PLL_SET_PRE_DIV_SEL(detected, pre_div_sel);317 detected = PLL_SET_POST_DIV_SEL(detected, post_div_sel);318 detected = PLL_SET_DIV_SEL(detected, div_sel);319 *value = detected;320 *rate = best_rate;321 return 0;322 }323 324 return -EINVAL;325}326 327static int fpll_determine_rate(struct clk_hw *hw, struct clk_rate_request *req)328{329 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);330 u32 val, ssc_syn_set;331 332 if (!fpll_is_factional_mode(pll))333 return ipll_determine_rate(hw, req);334 335 fpll_find_rate(pll, &pll->pll_limit[2], req->best_parent_rate,336 &req->rate, &val, &ssc_syn_set);337 338 return 0;339}340 341static bool fpll_check_mode_ctrl_restrict(unsigned long div_sel,342 unsigned long ictrl,343 unsigned long mode)344{345 unsigned long left_rest = 10 * div_sel;346 unsigned long right_rest = 24 * div_sel;347 unsigned long test = 184 * (1 + mode) * (1 + ictrl) / 2;348 349 return test > left_rest && test <= right_rest;350}351 352static int fpll_set_rate(struct clk_hw *hw, unsigned long rate,353 unsigned long parent_rate)354{355 u32 regval;356 u32 detected = 0, detected_ssc = 0;357 unsigned long flags;358 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);359 360 if (!fpll_is_factional_mode(pll))361 return ipll_set_rate(hw, rate, parent_rate);362 363 fpll_find_rate(pll, &pll->pll_limit[2], parent_rate,364 &rate, &detected, &detected_ssc);365 pll_get_mode_ctrl(PLL_GET_DIV_SEL(detected),366 fpll_check_mode_ctrl_restrict,367 pll->pll_limit, &detected);368 369 spin_lock_irqsave(pll->common.lock, flags);370 371 writel(detected_ssc, pll->common.base + pll->pll_syn->set);372 373 regval = readl(pll->common.base + pll->pll_reg);374 regval = PLL_COPY_REG(regval, detected);375 376 writel(regval, pll->common.base + pll->pll_reg);377 378 spin_unlock_irqrestore(pll->common.lock, flags);379 380 cv1800_clk_wait_for_lock(&pll->common, pll->pll_status.reg,381 BIT(pll->pll_status.shift));382 383 return 0;384}385 386static u8 fpll_get_parent(struct clk_hw *hw)387{388 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);389 390 if (fpll_is_factional_mode(pll))391 return 1;392 393 return 0;394}395 396static int fpll_set_parent(struct clk_hw *hw, u8 index)397{398 struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);399 400 if (index)401 cv1800_clk_setbit(&pll->common, &pll->pll_syn->en);402 else403 cv1800_clk_clearbit(&pll->common, &pll->pll_syn->en);404 405 return 0;406}407 408const struct clk_ops cv1800_clk_fpll_ops = {409 .disable = pll_disable,410 .enable = pll_enable,411 .is_enabled = pll_is_enable,412 413 .recalc_rate = fpll_recalc_rate,414 .determine_rate = fpll_determine_rate,415 .set_rate = fpll_set_rate,416 417 .set_parent = fpll_set_parent,418 .get_parent = fpll_get_parent,419};420