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