drivers/clk/sophgo/clk-cv18xx-pll.c

   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, &dividend);
 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 };