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