1 // SPDX-License-Identifier: GPL-2.0 2 /* 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 13 static 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 20 static 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 33 static 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 47 static 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 88 static 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 97 static 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 119 static 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 130 static 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 157 static 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 164 static 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 171 static 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 178 const 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 26 189 #define PLL_SYN_FACTOR_MINIMUM ((4 << PLL_SYN_FACTOR_DOT_POS) + 1) 190 191 static bool fpll_is_factional_mode(struct cv1800_clk_pll *pll) 192 { 193 return cv1800_clk_checkbit(&pll->common, &pll->pll_syn->en); 194 } 195 196 static 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 220 static 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 248 static 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 else 273 test_max = tssc - 1; 274 } 275 276 if (trate != 0) 277 *ssc_syn_set = test_min; 278 279 return trate; 280 } 281 282 static 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 327 static 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 341 static 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 352 static 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 386 static 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 396 static 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 else 403 cv1800_clk_clearbit(&pll->common, &pll->pll_syn->en); 404 405 return 0; 406 } 407 408 const 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