drivers/clk/st/clkgen-fsyn.c

/* [<][>][^][v][top][bottom][index][help] */
This source file includes following definitions.
quadfs_pll_enable
quadfs_pll_disable
quadfs_pll_is_enabled
clk_fs660c32_vco_get_rate
quadfs_pll_fs660c32_recalc_rate
clk_fs660c32_vco_get_params
quadfs_pll_fs660c32_round_rate
quadfs_pll_fs660c32_set_rate
st_clk_register_quadfs_pll
quadfs_fsynth_program_enable
quadfs_fsynth_program_rate
quadfs_fsynth_enable
quadfs_fsynth_disable
quadfs_fsynth_is_enabled
clk_fs660c32_dig_get_rate
clk_fs660c32_get_pe
clk_fs660c32_dig_get_params
quadfs_fsynt_get_hw_value_for_recalc
quadfs_find_best_rate
quadfs_recalc_rate
quadfs_round_rate
quadfs_program_and_enable
quadfs_set_rate
st_clk_register_quadfs_fsynth
st_of_create_quadfs_fsynths
st_of_quadfs_setup
st_of_quadfs660C_setup
st_of_quadfs660D_setup
   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (C) 2014 STMicroelectronics R&D Ltd
   4  */
   5 
   6 /*
   7  * Authors:
   8  * Stephen Gallimore <stephen.gallimore@st.com>,
   9  * Pankaj Dev <pankaj.dev@st.com>.
  10  */
  11 
  12 #include <linux/slab.h>
  13 #include <linux/of_address.h>
  14 #include <linux/clk.h>
  15 #include <linux/clk-provider.h>
  16 
  17 #include "clkgen.h"
  18 
  19 /*
  20  * Maximum input clock to the PLL before we divide it down by 2
  21  * although in reality in actual systems this has never been seen to
  22  * be used.
  23  */
  24 #define QUADFS_NDIV_THRESHOLD 30000000
  25 
  26 #define PLL_BW_GOODREF   (0L)
  27 #define PLL_BW_VBADREF   (1L)
  28 #define PLL_BW_BADREF    (2L)
  29 #define PLL_BW_VGOODREF  (3L)
  30 
  31 #define QUADFS_MAX_CHAN 4
  32 
  33 struct stm_fs {
  34         unsigned long ndiv;
  35         unsigned long mdiv;
  36         unsigned long pe;
  37         unsigned long sdiv;
  38         unsigned long nsdiv;
  39 };
  40 
  41 struct clkgen_quadfs_data {
  42         bool reset_present;
  43         bool bwfilter_present;
  44         bool lockstatus_present;
  45         bool powerup_polarity;
  46         bool standby_polarity;
  47         bool nsdiv_present;
  48         bool nrst_present;
  49         struct clkgen_field ndiv;
  50         struct clkgen_field ref_bw;
  51         struct clkgen_field nreset;
  52         struct clkgen_field npda;
  53         struct clkgen_field lock_status;
  54 
  55         struct clkgen_field nrst[QUADFS_MAX_CHAN];
  56         struct clkgen_field nsb[QUADFS_MAX_CHAN];
  57         struct clkgen_field en[QUADFS_MAX_CHAN];
  58         struct clkgen_field mdiv[QUADFS_MAX_CHAN];
  59         struct clkgen_field pe[QUADFS_MAX_CHAN];
  60         struct clkgen_field sdiv[QUADFS_MAX_CHAN];
  61         struct clkgen_field nsdiv[QUADFS_MAX_CHAN];
  62 
  63         const struct clk_ops *pll_ops;
  64         int  (*get_params)(unsigned long, unsigned long, struct stm_fs *);
  65         int  (*get_rate)(unsigned long , const struct stm_fs *,
  66                         unsigned long *);
  67 };
  68 
  69 static const struct clk_ops st_quadfs_pll_c32_ops;
  70 
  71 static int clk_fs660c32_dig_get_params(unsigned long input,
  72                 unsigned long output, struct stm_fs *fs);
  73 static int clk_fs660c32_dig_get_rate(unsigned long, const struct stm_fs *,
  74                 unsigned long *);
  75 
  76 static const struct clkgen_quadfs_data st_fs660c32_C = {
  77         .nrst_present = true,
  78         .nrst   = { CLKGEN_FIELD(0x2f0, 0x1, 0),
  79                     CLKGEN_FIELD(0x2f0, 0x1, 1),
  80                     CLKGEN_FIELD(0x2f0, 0x1, 2),
  81                     CLKGEN_FIELD(0x2f0, 0x1, 3) },
  82         .npda   = CLKGEN_FIELD(0x2f0, 0x1, 12),
  83         .nsb    = { CLKGEN_FIELD(0x2f0, 0x1, 8),
  84                     CLKGEN_FIELD(0x2f0, 0x1, 9),
  85                     CLKGEN_FIELD(0x2f0, 0x1, 10),
  86                     CLKGEN_FIELD(0x2f0, 0x1, 11) },
  87         .nsdiv_present = true,
  88         .nsdiv  = { CLKGEN_FIELD(0x304, 0x1, 24),
  89                     CLKGEN_FIELD(0x308, 0x1, 24),
  90                     CLKGEN_FIELD(0x30c, 0x1, 24),
  91                     CLKGEN_FIELD(0x310, 0x1, 24) },
  92         .mdiv   = { CLKGEN_FIELD(0x304, 0x1f, 15),
  93                     CLKGEN_FIELD(0x308, 0x1f, 15),
  94                     CLKGEN_FIELD(0x30c, 0x1f, 15),
  95                     CLKGEN_FIELD(0x310, 0x1f, 15) },
  96         .en     = { CLKGEN_FIELD(0x2fc, 0x1, 0),
  97                     CLKGEN_FIELD(0x2fc, 0x1, 1),
  98                     CLKGEN_FIELD(0x2fc, 0x1, 2),
  99                     CLKGEN_FIELD(0x2fc, 0x1, 3) },
 100         .ndiv   = CLKGEN_FIELD(0x2f4, 0x7, 16),
 101         .pe     = { CLKGEN_FIELD(0x304, 0x7fff, 0),
 102                     CLKGEN_FIELD(0x308, 0x7fff, 0),
 103                     CLKGEN_FIELD(0x30c, 0x7fff, 0),
 104                     CLKGEN_FIELD(0x310, 0x7fff, 0) },
 105         .sdiv   = { CLKGEN_FIELD(0x304, 0xf, 20),
 106                     CLKGEN_FIELD(0x308, 0xf, 20),
 107                     CLKGEN_FIELD(0x30c, 0xf, 20),
 108                     CLKGEN_FIELD(0x310, 0xf, 20) },
 109         .lockstatus_present = true,
 110         .lock_status = CLKGEN_FIELD(0x2f0, 0x1, 24),
 111         .powerup_polarity = 1,
 112         .standby_polarity = 1,
 113         .pll_ops        = &st_quadfs_pll_c32_ops,
 114         .get_params     = clk_fs660c32_dig_get_params,
 115         .get_rate       = clk_fs660c32_dig_get_rate,
 116 };
 117 
 118 static const struct clkgen_quadfs_data st_fs660c32_D = {
 119         .nrst_present = true,
 120         .nrst   = { CLKGEN_FIELD(0x2a0, 0x1, 0),
 121                     CLKGEN_FIELD(0x2a0, 0x1, 1),
 122                     CLKGEN_FIELD(0x2a0, 0x1, 2),
 123                     CLKGEN_FIELD(0x2a0, 0x1, 3) },
 124         .ndiv   = CLKGEN_FIELD(0x2a4, 0x7, 16),
 125         .pe     = { CLKGEN_FIELD(0x2b4, 0x7fff, 0),
 126                     CLKGEN_FIELD(0x2b8, 0x7fff, 0),
 127                     CLKGEN_FIELD(0x2bc, 0x7fff, 0),
 128                     CLKGEN_FIELD(0x2c0, 0x7fff, 0) },
 129         .sdiv   = { CLKGEN_FIELD(0x2b4, 0xf, 20),
 130                     CLKGEN_FIELD(0x2b8, 0xf, 20),
 131                     CLKGEN_FIELD(0x2bc, 0xf, 20),
 132                     CLKGEN_FIELD(0x2c0, 0xf, 20) },
 133         .npda   = CLKGEN_FIELD(0x2a0, 0x1, 12),
 134         .nsb    = { CLKGEN_FIELD(0x2a0, 0x1, 8),
 135                     CLKGEN_FIELD(0x2a0, 0x1, 9),
 136                     CLKGEN_FIELD(0x2a0, 0x1, 10),
 137                     CLKGEN_FIELD(0x2a0, 0x1, 11) },
 138         .nsdiv_present = true,
 139         .nsdiv  = { CLKGEN_FIELD(0x2b4, 0x1, 24),
 140                     CLKGEN_FIELD(0x2b8, 0x1, 24),
 141                     CLKGEN_FIELD(0x2bc, 0x1, 24),
 142                     CLKGEN_FIELD(0x2c0, 0x1, 24) },
 143         .mdiv   = { CLKGEN_FIELD(0x2b4, 0x1f, 15),
 144                     CLKGEN_FIELD(0x2b8, 0x1f, 15),
 145                     CLKGEN_FIELD(0x2bc, 0x1f, 15),
 146                     CLKGEN_FIELD(0x2c0, 0x1f, 15) },
 147         .en     = { CLKGEN_FIELD(0x2ac, 0x1, 0),
 148                     CLKGEN_FIELD(0x2ac, 0x1, 1),
 149                     CLKGEN_FIELD(0x2ac, 0x1, 2),
 150                     CLKGEN_FIELD(0x2ac, 0x1, 3) },
 151         .lockstatus_present = true,
 152         .lock_status = CLKGEN_FIELD(0x2A0, 0x1, 24),
 153         .powerup_polarity = 1,
 154         .standby_polarity = 1,
 155         .pll_ops        = &st_quadfs_pll_c32_ops,
 156         .get_params     = clk_fs660c32_dig_get_params,
 157         .get_rate       = clk_fs660c32_dig_get_rate,};
 158 
 159 /**
 160  * DOC: A Frequency Synthesizer that multiples its input clock by a fixed factor
 161  *
 162  * Traits of this clock:
 163  * prepare - clk_(un)prepare only ensures parent is (un)prepared
 164  * enable - clk_enable and clk_disable are functional & control the Fsyn
 165  * rate - inherits rate from parent. set_rate/round_rate/recalc_rate
 166  * parent - fixed parent.  No clk_set_parent support
 167  */
 168 
 169 /**
 170  * struct st_clk_quadfs_pll - A pll which outputs a fixed multiplier of
 171  *                                  its parent clock, found inside a type of
 172  *                                  ST quad channel frequency synthesizer block
 173  *
 174  * @hw: handle between common and hardware-specific interfaces.
 175  * @ndiv: regmap field for the ndiv control.
 176  * @regs_base: base address of the configuration registers.
 177  * @lock: spinlock.
 178  *
 179  */
 180 struct st_clk_quadfs_pll {
 181         struct clk_hw   hw;
 182         void __iomem    *regs_base;
 183         spinlock_t      *lock;
 184         struct clkgen_quadfs_data *data;
 185         u32 ndiv;
 186 };
 187 
 188 #define to_quadfs_pll(_hw) container_of(_hw, struct st_clk_quadfs_pll, hw)
 189 
 190 static int quadfs_pll_enable(struct clk_hw *hw)
 191 {
 192         struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
 193         unsigned long flags = 0, timeout = jiffies + msecs_to_jiffies(10);
 194 
 195         if (pll->lock)
 196                 spin_lock_irqsave(pll->lock, flags);
 197 
 198         /*
 199          * Bring block out of reset if we have reset control.
 200          */
 201         if (pll->data->reset_present)
 202                 CLKGEN_WRITE(pll, nreset, 1);
 203 
 204         /*
 205          * Use a fixed input clock noise bandwidth filter for the moment
 206          */
 207         if (pll->data->bwfilter_present)
 208                 CLKGEN_WRITE(pll, ref_bw, PLL_BW_GOODREF);
 209 
 210 
 211         CLKGEN_WRITE(pll, ndiv, pll->ndiv);
 212 
 213         /*
 214          * Power up the PLL
 215          */
 216         CLKGEN_WRITE(pll, npda, !pll->data->powerup_polarity);
 217 
 218         if (pll->lock)
 219                 spin_unlock_irqrestore(pll->lock, flags);
 220 
 221         if (pll->data->lockstatus_present)
 222                 while (!CLKGEN_READ(pll, lock_status)) {
 223                         if (time_after(jiffies, timeout))
 224                                 return -ETIMEDOUT;
 225                         cpu_relax();
 226                 }
 227 
 228         return 0;
 229 }
 230 
 231 static void quadfs_pll_disable(struct clk_hw *hw)
 232 {
 233         struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
 234         unsigned long flags = 0;
 235 
 236         if (pll->lock)
 237                 spin_lock_irqsave(pll->lock, flags);
 238 
 239         /*
 240          * Powerdown the PLL and then put block into soft reset if we have
 241          * reset control.
 242          */
 243         CLKGEN_WRITE(pll, npda, pll->data->powerup_polarity);
 244 
 245         if (pll->data->reset_present)
 246                 CLKGEN_WRITE(pll, nreset, 0);
 247 
 248         if (pll->lock)
 249                 spin_unlock_irqrestore(pll->lock, flags);
 250 }
 251 
 252 static int quadfs_pll_is_enabled(struct clk_hw *hw)
 253 {
 254         struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
 255         u32 npda = CLKGEN_READ(pll, npda);
 256 
 257         return pll->data->powerup_polarity ? !npda : !!npda;
 258 }
 259 
 260 static int clk_fs660c32_vco_get_rate(unsigned long input, struct stm_fs *fs,
 261                            unsigned long *rate)
 262 {
 263         unsigned long nd = fs->ndiv + 16; /* ndiv value */
 264 
 265         *rate = input * nd;
 266 
 267         return 0;
 268 }
 269 
 270 static unsigned long quadfs_pll_fs660c32_recalc_rate(struct clk_hw *hw,
 271                                         unsigned long parent_rate)
 272 {
 273         struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
 274         unsigned long rate = 0;
 275         struct stm_fs params;
 276 
 277         params.ndiv = CLKGEN_READ(pll, ndiv);
 278         if (clk_fs660c32_vco_get_rate(parent_rate, &params, &rate))
 279                 pr_err("%s:%s error calculating rate\n",
 280                        clk_hw_get_name(hw), __func__);
 281 
 282         pll->ndiv = params.ndiv;
 283 
 284         return rate;
 285 }
 286 
 287 static int clk_fs660c32_vco_get_params(unsigned long input,
 288                                 unsigned long output, struct stm_fs *fs)
 289 {
 290 /* Formula
 291    VCO frequency = (fin x ndiv) / pdiv
 292    ndiv = VCOfreq * pdiv / fin
 293    */
 294         unsigned long pdiv = 1, n;
 295 
 296         /* Output clock range: 384Mhz to 660Mhz */
 297         if (output < 384000000 || output > 660000000)
 298                 return -EINVAL;
 299 
 300         if (input > 40000000)
 301                 /* This means that PDIV would be 2 instead of 1.
 302                    Not supported today. */
 303                 return -EINVAL;
 304 
 305         input /= 1000;
 306         output /= 1000;
 307 
 308         n = output * pdiv / input;
 309         if (n < 16)
 310                 n = 16;
 311         fs->ndiv = n - 16; /* Converting formula value to reg value */
 312 
 313         return 0;
 314 }
 315 
 316 static long quadfs_pll_fs660c32_round_rate(struct clk_hw *hw,
 317                                            unsigned long rate,
 318                                            unsigned long *prate)
 319 {
 320         struct stm_fs params;
 321 
 322         if (clk_fs660c32_vco_get_params(*prate, rate, &params))
 323                 return rate;
 324 
 325         clk_fs660c32_vco_get_rate(*prate, &params, &rate);
 326 
 327         pr_debug("%s: %s new rate %ld [ndiv=%u]\n",
 328                  __func__, clk_hw_get_name(hw),
 329                  rate, (unsigned int)params.ndiv);
 330 
 331         return rate;
 332 }
 333 
 334 static int quadfs_pll_fs660c32_set_rate(struct clk_hw *hw, unsigned long rate,
 335                                 unsigned long parent_rate)
 336 {
 337         struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
 338         struct stm_fs params;
 339         long hwrate = 0;
 340         unsigned long flags = 0;
 341         int ret;
 342 
 343         if (!rate || !parent_rate)
 344                 return -EINVAL;
 345 
 346         ret = clk_fs660c32_vco_get_params(parent_rate, rate, &params);
 347         if (ret)
 348                 return ret;
 349 
 350         clk_fs660c32_vco_get_rate(parent_rate, &params, &hwrate);
 351 
 352         pr_debug("%s: %s new rate %ld [ndiv=0x%x]\n",
 353                  __func__, clk_hw_get_name(hw),
 354                  hwrate, (unsigned int)params.ndiv);
 355 
 356         if (!hwrate)
 357                 return -EINVAL;
 358 
 359         pll->ndiv = params.ndiv;
 360 
 361         if (pll->lock)
 362                 spin_lock_irqsave(pll->lock, flags);
 363 
 364         CLKGEN_WRITE(pll, ndiv, pll->ndiv);
 365 
 366         if (pll->lock)
 367                 spin_unlock_irqrestore(pll->lock, flags);
 368 
 369         return 0;
 370 }
 371 
 372 static const struct clk_ops st_quadfs_pll_c32_ops = {
 373         .enable         = quadfs_pll_enable,
 374         .disable        = quadfs_pll_disable,
 375         .is_enabled     = quadfs_pll_is_enabled,
 376         .recalc_rate    = quadfs_pll_fs660c32_recalc_rate,
 377         .round_rate     = quadfs_pll_fs660c32_round_rate,
 378         .set_rate       = quadfs_pll_fs660c32_set_rate,
 379 };
 380 
 381 static struct clk * __init st_clk_register_quadfs_pll(
 382                 const char *name, const char *parent_name,
 383                 struct clkgen_quadfs_data *quadfs, void __iomem *reg,
 384                 spinlock_t *lock)
 385 {
 386         struct st_clk_quadfs_pll *pll;
 387         struct clk *clk;
 388         struct clk_init_data init;
 389 
 390         /*
 391          * Sanity check required pointers.
 392          */
 393         if (WARN_ON(!name || !parent_name))
 394                 return ERR_PTR(-EINVAL);
 395 
 396         pll = kzalloc(sizeof(*pll), GFP_KERNEL);
 397         if (!pll)
 398                 return ERR_PTR(-ENOMEM);
 399 
 400         init.name = name;
 401         init.ops = quadfs->pll_ops;
 402         init.flags = CLK_GET_RATE_NOCACHE;
 403         init.parent_names = &parent_name;
 404         init.num_parents = 1;
 405 
 406         pll->data = quadfs;
 407         pll->regs_base = reg;
 408         pll->lock = lock;
 409         pll->hw.init = &init;
 410 
 411         clk = clk_register(NULL, &pll->hw);
 412 
 413         if (IS_ERR(clk))
 414                 kfree(pll);
 415 
 416         return clk;
 417 }
 418 
 419 /**
 420  * DOC: A digital frequency synthesizer
 421  *
 422  * Traits of this clock:
 423  * prepare - clk_(un)prepare only ensures parent is (un)prepared
 424  * enable - clk_enable and clk_disable are functional
 425  * rate - set rate is functional
 426  * parent - fixed parent.  No clk_set_parent support
 427  */
 428 
 429 /**
 430  * struct st_clk_quadfs_fsynth - One clock output from a four channel digital
 431  *                                  frequency synthesizer (fsynth) block.
 432  *
 433  * @hw: handle between common and hardware-specific interfaces
 434  *
 435  * @nsb: regmap field in the output control register for the digital
 436  *       standby of this fsynth channel. This control is active low so
 437  *       the channel is in standby when the control bit is cleared.
 438  *
 439  * @nsdiv: regmap field in the output control register for
 440  *          for the optional divide by 3 of this fsynth channel. This control
 441  *          is active low so the divide by 3 is active when the control bit is
 442  *          cleared and the divide is bypassed when the bit is set.
 443  */
 444 struct st_clk_quadfs_fsynth {
 445         struct clk_hw   hw;
 446         void __iomem    *regs_base;
 447         spinlock_t      *lock;
 448         struct clkgen_quadfs_data *data;
 449 
 450         u32 chan;
 451         /*
 452          * Cached hardware values from set_rate so we can program the
 453          * hardware in enable. There are two reasons for this:
 454          *
 455          *  1. The registers may not be writable until the parent has been
 456          *     enabled.
 457          *
 458          *  2. It restores the clock rate when a driver does an enable
 459          *     on PM restore, after a suspend to RAM has lost the hardware
 460          *     setup.
 461          */
 462         u32 md;
 463         u32 pe;
 464         u32 sdiv;
 465         u32 nsdiv;
 466 };
 467 
 468 #define to_quadfs_fsynth(_hw) \
 469         container_of(_hw, struct st_clk_quadfs_fsynth, hw)
 470 
 471 static void quadfs_fsynth_program_enable(struct st_clk_quadfs_fsynth *fs)
 472 {
 473         /*
 474          * Pulse the program enable register lsb to make the hardware take
 475          * notice of the new md/pe values with a glitchless transition.
 476          */
 477         CLKGEN_WRITE(fs, en[fs->chan], 1);
 478         CLKGEN_WRITE(fs, en[fs->chan], 0);
 479 }
 480 
 481 static void quadfs_fsynth_program_rate(struct st_clk_quadfs_fsynth *fs)
 482 {
 483         unsigned long flags = 0;
 484 
 485         /*
 486          * Ensure the md/pe parameters are ignored while we are
 487          * reprogramming them so we can get a glitchless change
 488          * when fine tuning the speed of a running clock.
 489          */
 490         CLKGEN_WRITE(fs, en[fs->chan], 0);
 491 
 492         CLKGEN_WRITE(fs, mdiv[fs->chan], fs->md);
 493         CLKGEN_WRITE(fs, pe[fs->chan], fs->pe);
 494         CLKGEN_WRITE(fs, sdiv[fs->chan], fs->sdiv);
 495 
 496         if (fs->lock)
 497                 spin_lock_irqsave(fs->lock, flags);
 498 
 499         if (fs->data->nsdiv_present)
 500                 CLKGEN_WRITE(fs, nsdiv[fs->chan], fs->nsdiv);
 501 
 502         if (fs->lock)
 503                 spin_unlock_irqrestore(fs->lock, flags);
 504 }
 505 
 506 static int quadfs_fsynth_enable(struct clk_hw *hw)
 507 {
 508         struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
 509         unsigned long flags = 0;
 510 
 511         pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw));
 512 
 513         quadfs_fsynth_program_rate(fs);
 514 
 515         if (fs->lock)
 516                 spin_lock_irqsave(fs->lock, flags);
 517 
 518         CLKGEN_WRITE(fs, nsb[fs->chan], !fs->data->standby_polarity);
 519 
 520         if (fs->data->nrst_present)
 521                 CLKGEN_WRITE(fs, nrst[fs->chan], 0);
 522 
 523         if (fs->lock)
 524                 spin_unlock_irqrestore(fs->lock, flags);
 525 
 526         quadfs_fsynth_program_enable(fs);
 527 
 528         return 0;
 529 }
 530 
 531 static void quadfs_fsynth_disable(struct clk_hw *hw)
 532 {
 533         struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
 534         unsigned long flags = 0;
 535 
 536         pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw));
 537 
 538         if (fs->lock)
 539                 spin_lock_irqsave(fs->lock, flags);
 540 
 541         CLKGEN_WRITE(fs, nsb[fs->chan], fs->data->standby_polarity);
 542 
 543         if (fs->lock)
 544                 spin_unlock_irqrestore(fs->lock, flags);
 545 }
 546 
 547 static int quadfs_fsynth_is_enabled(struct clk_hw *hw)
 548 {
 549         struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
 550         u32 nsb = CLKGEN_READ(fs, nsb[fs->chan]);
 551 
 552         pr_debug("%s: %s enable bit = 0x%x\n",
 553                  __func__, clk_hw_get_name(hw), nsb);
 554 
 555         return fs->data->standby_polarity ? !nsb : !!nsb;
 556 }
 557 
 558 #define P20             (uint64_t)(1 << 20)
 559 
 560 static int clk_fs660c32_dig_get_rate(unsigned long input,
 561                                 const struct stm_fs *fs, unsigned long *rate)
 562 {
 563         unsigned long s = (1 << fs->sdiv);
 564         unsigned long ns;
 565         uint64_t res;
 566 
 567         /*
 568          * 'nsdiv' is a register value ('BIN') which is translated
 569          * to a decimal value according to following rules.
 570          *
 571          *     nsdiv      ns.dec
 572          *       0        3
 573          *       1        1
 574          */
 575         ns = (fs->nsdiv == 1) ? 1 : 3;
 576 
 577         res = (P20 * (32 + fs->mdiv) + 32 * fs->pe) * s * ns;
 578         *rate = (unsigned long)div64_u64(input * P20 * 32, res);
 579 
 580         return 0;
 581 }
 582 
 583 
 584 static int clk_fs660c32_get_pe(int m, int si, unsigned long *deviation,
 585                 signed long input, unsigned long output, uint64_t *p,
 586                 struct stm_fs *fs)
 587 {
 588         unsigned long new_freq, new_deviation;
 589         struct stm_fs fs_tmp;
 590         uint64_t val;
 591 
 592         val = (uint64_t)output << si;
 593 
 594         *p = (uint64_t)input * P20 - (32LL  + (uint64_t)m) * val * (P20 / 32LL);
 595 
 596         *p = div64_u64(*p, val);
 597 
 598         if (*p > 32767LL)
 599                 return 1;
 600 
 601         fs_tmp.mdiv = (unsigned long) m;
 602         fs_tmp.pe = (unsigned long)*p;
 603         fs_tmp.sdiv = si;
 604         fs_tmp.nsdiv = 1;
 605 
 606         clk_fs660c32_dig_get_rate(input, &fs_tmp, &new_freq);
 607 
 608         new_deviation = abs(output - new_freq);
 609 
 610         if (new_deviation < *deviation) {
 611                 fs->mdiv = m;
 612                 fs->pe = (unsigned long)*p;
 613                 fs->sdiv = si;
 614                 fs->nsdiv = 1;
 615                 *deviation = new_deviation;
 616         }
 617         return 0;
 618 }
 619 
 620 static int clk_fs660c32_dig_get_params(unsigned long input,
 621                 unsigned long output, struct stm_fs *fs)
 622 {
 623         int si; /* sdiv_reg (8 downto 0) */
 624         int m; /* md value */
 625         unsigned long new_freq, new_deviation;
 626         /* initial condition to say: "infinite deviation" */
 627         unsigned long deviation = ~0;
 628         uint64_t p, p1, p2;     /* pe value */
 629         int r1, r2;
 630 
 631         struct stm_fs fs_tmp;
 632 
 633         for (si = 0; (si <= 8) && deviation; si++) {
 634 
 635                 /* Boundary test to avoid useless iteration */
 636                 r1 = clk_fs660c32_get_pe(0, si, &deviation,
 637                                 input, output, &p1, fs);
 638                 r2 = clk_fs660c32_get_pe(31, si, &deviation,
 639                                 input, output, &p2, fs);
 640 
 641                 /* No solution */
 642                 if (r1 && r2 && (p1 > p2))
 643                         continue;
 644 
 645                 /* Try to find best deviation */
 646                 for (m = 1; (m < 31) && deviation; m++)
 647                         clk_fs660c32_get_pe(m, si, &deviation,
 648                                         input, output, &p, fs);
 649 
 650         }
 651 
 652         if (deviation == ~0) /* No solution found */
 653                 return -1;
 654 
 655         /* pe fine tuning if deviation not 0: +/- 2 around computed pe value */
 656         if (deviation) {
 657                 fs_tmp.mdiv = fs->mdiv;
 658                 fs_tmp.sdiv = fs->sdiv;
 659                 fs_tmp.nsdiv = fs->nsdiv;
 660 
 661                 if (fs->pe > 2)
 662                         p2 = fs->pe - 2;
 663                 else
 664                         p2 = 0;
 665 
 666                 for (; p2 < 32768ll && (p2 <= (fs->pe + 2)); p2++) {
 667                         fs_tmp.pe = (unsigned long)p2;
 668 
 669                         clk_fs660c32_dig_get_rate(input, &fs_tmp, &new_freq);
 670 
 671                         new_deviation = abs(output - new_freq);
 672 
 673                         /* Check if this is a better solution */
 674                         if (new_deviation < deviation) {
 675                                 fs->pe = (unsigned long)p2;
 676                                 deviation = new_deviation;
 677 
 678                         }
 679                 }
 680         }
 681         return 0;
 682 }
 683 
 684 static int quadfs_fsynt_get_hw_value_for_recalc(struct st_clk_quadfs_fsynth *fs,
 685                 struct stm_fs *params)
 686 {
 687         /*
 688          * Get the initial hardware values for recalc_rate
 689          */
 690         params->mdiv    = CLKGEN_READ(fs, mdiv[fs->chan]);
 691         params->pe      = CLKGEN_READ(fs, pe[fs->chan]);
 692         params->sdiv    = CLKGEN_READ(fs, sdiv[fs->chan]);
 693 
 694         if (fs->data->nsdiv_present)
 695                 params->nsdiv = CLKGEN_READ(fs, nsdiv[fs->chan]);
 696         else
 697                 params->nsdiv = 1;
 698 
 699         /*
 700          * If All are NULL then assume no clock rate is programmed.
 701          */
 702         if (!params->mdiv && !params->pe && !params->sdiv)
 703                 return 1;
 704 
 705         fs->md = params->mdiv;
 706         fs->pe = params->pe;
 707         fs->sdiv = params->sdiv;
 708         fs->nsdiv = params->nsdiv;
 709 
 710         return 0;
 711 }
 712 
 713 static long quadfs_find_best_rate(struct clk_hw *hw, unsigned long drate,
 714                                 unsigned long prate, struct stm_fs *params)
 715 {
 716         struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
 717         int (*clk_fs_get_rate)(unsigned long ,
 718                                 const struct stm_fs *, unsigned long *);
 719         int (*clk_fs_get_params)(unsigned long, unsigned long, struct stm_fs *);
 720         unsigned long rate = 0;
 721 
 722         clk_fs_get_rate = fs->data->get_rate;
 723         clk_fs_get_params = fs->data->get_params;
 724 
 725         if (!clk_fs_get_params(prate, drate, params))
 726                 clk_fs_get_rate(prate, params, &rate);
 727 
 728         return rate;
 729 }
 730 
 731 static unsigned long quadfs_recalc_rate(struct clk_hw *hw,
 732                 unsigned long parent_rate)
 733 {
 734         struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
 735         unsigned long rate = 0;
 736         struct stm_fs params;
 737         int (*clk_fs_get_rate)(unsigned long ,
 738                                 const struct stm_fs *, unsigned long *);
 739 
 740         clk_fs_get_rate = fs->data->get_rate;
 741 
 742         if (quadfs_fsynt_get_hw_value_for_recalc(fs, &params))
 743                 return 0;
 744 
 745         if (clk_fs_get_rate(parent_rate, &params, &rate)) {
 746                 pr_err("%s:%s error calculating rate\n",
 747                        clk_hw_get_name(hw), __func__);
 748         }
 749 
 750         pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
 751 
 752         return rate;
 753 }
 754 
 755 static long quadfs_round_rate(struct clk_hw *hw, unsigned long rate,
 756                                      unsigned long *prate)
 757 {
 758         struct stm_fs params;
 759 
 760         rate = quadfs_find_best_rate(hw, rate, *prate, &params);
 761 
 762         pr_debug("%s: %s new rate %ld [sdiv=0x%x,md=0x%x,pe=0x%x,nsdiv3=%u]\n",
 763                  __func__, clk_hw_get_name(hw),
 764                  rate, (unsigned int)params.sdiv, (unsigned int)params.mdiv,
 765                          (unsigned int)params.pe, (unsigned int)params.nsdiv);
 766 
 767         return rate;
 768 }
 769 
 770 
 771 static void quadfs_program_and_enable(struct st_clk_quadfs_fsynth *fs,
 772                 struct stm_fs *params)
 773 {
 774         fs->md = params->mdiv;
 775         fs->pe = params->pe;
 776         fs->sdiv = params->sdiv;
 777         fs->nsdiv = params->nsdiv;
 778 
 779         /*
 780          * In some integrations you can only change the fsynth programming when
 781          * the parent entity containing it is enabled.
 782          */
 783         quadfs_fsynth_program_rate(fs);
 784         quadfs_fsynth_program_enable(fs);
 785 }
 786 
 787 static int quadfs_set_rate(struct clk_hw *hw, unsigned long rate,
 788                                   unsigned long parent_rate)
 789 {
 790         struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
 791         struct stm_fs params;
 792         long hwrate;
 793         int uninitialized_var(i);
 794 
 795         if (!rate || !parent_rate)
 796                 return -EINVAL;
 797 
 798         memset(&params, 0, sizeof(struct stm_fs));
 799 
 800         hwrate = quadfs_find_best_rate(hw, rate, parent_rate, &params);
 801         if (!hwrate)
 802                 return -EINVAL;
 803 
 804         quadfs_program_and_enable(fs, &params);
 805 
 806         return 0;
 807 }
 808 
 809 
 810 
 811 static const struct clk_ops st_quadfs_ops = {
 812         .enable         = quadfs_fsynth_enable,
 813         .disable        = quadfs_fsynth_disable,
 814         .is_enabled     = quadfs_fsynth_is_enabled,
 815         .round_rate     = quadfs_round_rate,
 816         .set_rate       = quadfs_set_rate,
 817         .recalc_rate    = quadfs_recalc_rate,
 818 };
 819 
 820 static struct clk * __init st_clk_register_quadfs_fsynth(
 821                 const char *name, const char *parent_name,
 822                 struct clkgen_quadfs_data *quadfs, void __iomem *reg, u32 chan,
 823                 unsigned long flags, spinlock_t *lock)
 824 {
 825         struct st_clk_quadfs_fsynth *fs;
 826         struct clk *clk;
 827         struct clk_init_data init;
 828 
 829         /*
 830          * Sanity check required pointers, note that nsdiv3 is optional.
 831          */
 832         if (WARN_ON(!name || !parent_name))
 833                 return ERR_PTR(-EINVAL);
 834 
 835         fs = kzalloc(sizeof(*fs), GFP_KERNEL);
 836         if (!fs)
 837                 return ERR_PTR(-ENOMEM);
 838 
 839         init.name = name;
 840         init.ops = &st_quadfs_ops;
 841         init.flags = flags | CLK_GET_RATE_NOCACHE;
 842         init.parent_names = &parent_name;
 843         init.num_parents = 1;
 844 
 845         fs->data = quadfs;
 846         fs->regs_base = reg;
 847         fs->chan = chan;
 848         fs->lock = lock;
 849         fs->hw.init = &init;
 850 
 851         clk = clk_register(NULL, &fs->hw);
 852 
 853         if (IS_ERR(clk))
 854                 kfree(fs);
 855 
 856         return clk;
 857 }
 858 
 859 static void __init st_of_create_quadfs_fsynths(
 860                 struct device_node *np, const char *pll_name,
 861                 struct clkgen_quadfs_data *quadfs, void __iomem *reg,
 862                 spinlock_t *lock)
 863 {
 864         struct clk_onecell_data *clk_data;
 865         int fschan;
 866 
 867         clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
 868         if (!clk_data)
 869                 return;
 870 
 871         clk_data->clk_num = QUADFS_MAX_CHAN;
 872         clk_data->clks = kcalloc(QUADFS_MAX_CHAN, sizeof(struct clk *),
 873                                  GFP_KERNEL);
 874 
 875         if (!clk_data->clks) {
 876                 kfree(clk_data);
 877                 return;
 878         }
 879 
 880         for (fschan = 0; fschan < QUADFS_MAX_CHAN; fschan++) {
 881                 struct clk *clk;
 882                 const char *clk_name;
 883                 unsigned long flags = 0;
 884 
 885                 if (of_property_read_string_index(np, "clock-output-names",
 886                                                   fschan, &clk_name)) {
 887                         break;
 888                 }
 889 
 890                 /*
 891                  * If we read an empty clock name then the channel is unused
 892                  */
 893                 if (*clk_name == '\0')
 894                         continue;
 895 
 896                 of_clk_detect_critical(np, fschan, &flags);
 897 
 898                 clk = st_clk_register_quadfs_fsynth(clk_name, pll_name,
 899                                                     quadfs, reg, fschan,
 900                                                     flags, lock);
 901 
 902                 /*
 903                  * If there was an error registering this clock output, clean
 904                  * up and move on to the next one.
 905                  */
 906                 if (!IS_ERR(clk)) {
 907                         clk_data->clks[fschan] = clk;
 908                         pr_debug("%s: parent %s rate %u\n",
 909                                 __clk_get_name(clk),
 910                                 __clk_get_name(clk_get_parent(clk)),
 911                                 (unsigned int)clk_get_rate(clk));
 912                 }
 913         }
 914 
 915         of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
 916 }
 917 
 918 static void __init st_of_quadfs_setup(struct device_node *np,
 919                 struct clkgen_quadfs_data *data)
 920 {
 921         struct clk *clk;
 922         const char *pll_name, *clk_parent_name;
 923         void __iomem *reg;
 924         spinlock_t *lock;
 925 
 926         reg = of_iomap(np, 0);
 927         if (!reg)
 928                 return;
 929 
 930         clk_parent_name = of_clk_get_parent_name(np, 0);
 931         if (!clk_parent_name)
 932                 return;
 933 
 934         pll_name = kasprintf(GFP_KERNEL, "%pOFn.pll", np);
 935         if (!pll_name)
 936                 return;
 937 
 938         lock = kzalloc(sizeof(*lock), GFP_KERNEL);
 939         if (!lock)
 940                 goto err_exit;
 941 
 942         spin_lock_init(lock);
 943 
 944         clk = st_clk_register_quadfs_pll(pll_name, clk_parent_name, data,
 945                         reg, lock);
 946         if (IS_ERR(clk))
 947                 goto err_exit;
 948         else
 949                 pr_debug("%s: parent %s rate %u\n",
 950                         __clk_get_name(clk),
 951                         __clk_get_name(clk_get_parent(clk)),
 952                         (unsigned int)clk_get_rate(clk));
 953 
 954         st_of_create_quadfs_fsynths(np, pll_name, data, reg, lock);
 955 
 956 err_exit:
 957         kfree(pll_name); /* No longer need local copy of the PLL name */
 958 }
 959 
 960 static void __init st_of_quadfs660C_setup(struct device_node *np)
 961 {
 962         st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_C);
 963 }
 964 CLK_OF_DECLARE(quadfs660C, "st,quadfs-pll", st_of_quadfs660C_setup);
 965 
 966 static void __init st_of_quadfs660D_setup(struct device_node *np)
 967 {
 968         st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_D);
 969 }
 970 CLK_OF_DECLARE(quadfs660D, "st,quadfs", st_of_quadfs660D_setup);
/* [<][>][^][v][top][bottom][index][help] */
root/drivers/clk/st/clkgen-fsyn.c

DEFINITIONS
