drivers/clk/st/clkgen-fsyn.c

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