drivers/clk/st/clkgen-mux.c

   1 /*
   2  * clkgen-mux.c: ST GEN-MUX Clock driver
   3  *
   4  * Copyright (C) 2014 STMicroelectronics (R&D) Limited
   5  *
   6  * Authors: Stephen Gallimore <stephen.gallimore@st.com>
   7  *          Pankaj Dev <pankaj.dev@st.com>
   8  *
   9  * This program is free software; you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License as published by
  11  * the Free Software Foundation; either version 2 of the License, or
  12  * (at your option) any later version.
  13  *
  14  */
  15
  16 #include <linux/slab.h>
  17 #include <linux/of_address.h>
  18 #include <linux/clk-provider.h>
  19
  20 static DEFINE_SPINLOCK(clkgena_divmux_lock);
  21 static DEFINE_SPINLOCK(clkgenf_lock);
  22
  23 static const char ** __init clkgen_mux_get_parents(struct device_node *np,
  24                                                        int *num_parents)
  25 {
  26         const char **parents;
  27         int nparents, i;
  28
  29         nparents = of_clk_get_parent_count(np);
  30         if (WARN_ON(nparents <= 0))
  31                 return ERR_PTR(-EINVAL);
  32
  33         parents = kzalloc(nparents * sizeof(const char *), GFP_KERNEL);
  34         if (!parents)
  35                 return ERR_PTR(-ENOMEM);
  36
  37         for (i = 0; i < nparents; i++)
  38                 parents[i] = of_clk_get_parent_name(np, i);
  39
  40         *num_parents = nparents;
  41         return parents;
  42 }
  43
  44 /**
  45  * DOC: Clock mux with a programmable divider on each of its three inputs.
  46  *      The mux has an input setting which effectively gates its output.
  47  *
  48  * Traits of this clock:
  49  * prepare - clk_(un)prepare only ensures parent is (un)prepared
  50  * enable - clk_enable and clk_disable are functional & control gating
  51  * rate - set rate is supported
  52  * parent - set/get parent
  53  */
  54
  55 #define NUM_INPUTS 3
  56
  57 struct clkgena_divmux {
  58         struct clk_hw hw;
  59         /* Subclassed mux and divider structures */
  60         struct clk_mux mux;
  61         struct clk_divider div[NUM_INPUTS];
  62         /* Enable/running feedback register bits for each input */
  63         void __iomem *feedback_reg[NUM_INPUTS];
  64         int feedback_bit_idx;
  65
  66         u8              muxsel;
  67 };
  68
  69 #define to_clkgena_divmux(_hw) container_of(_hw, struct clkgena_divmux, hw)
  70
  71 struct clkgena_divmux_data {
  72         int num_outputs;
  73         int mux_offset;
  74         int mux_offset2;
  75         int mux_start_bit;
  76         int div_offsets[NUM_INPUTS];
  77         int fb_offsets[NUM_INPUTS];
  78         int fb_start_bit_idx;
  79 };
  80
  81 #define CKGAX_CLKOPSRC_SWITCH_OFF 0x3
  82
  83 static int clkgena_divmux_is_running(struct clkgena_divmux *mux)
  84 {
  85         u32 regval = readl(mux->feedback_reg[mux->muxsel]);
  86         u32 running = regval & BIT(mux->feedback_bit_idx);
  87         return !!running;
  88 }
  89
  90 static int clkgena_divmux_enable(struct clk_hw *hw)
  91 {
  92         struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
  93         struct clk_hw *mux_hw = &genamux->mux.hw;
  94         unsigned long timeout;
  95         int ret = 0;
  96
  97         __clk_hw_set_clk(mux_hw, hw);
  98
  99         ret = clk_mux_ops.set_parent(mux_hw, genamux->muxsel);
 100         if (ret)
 101                 return ret;
 102
 103         timeout = jiffies + msecs_to_jiffies(10);
 104
 105         while (!clkgena_divmux_is_running(genamux)) {
 106                 if (time_after(jiffies, timeout))
 107                         return -ETIMEDOUT;
 108                 cpu_relax();
 109         }
 110
 111         return 0;
 112 }
 113
 114 static void clkgena_divmux_disable(struct clk_hw *hw)
 115 {
 116         struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
 117         struct clk_hw *mux_hw = &genamux->mux.hw;
 118
 119         __clk_hw_set_clk(mux_hw, hw);
 120
 121         clk_mux_ops.set_parent(mux_hw, CKGAX_CLKOPSRC_SWITCH_OFF);
 122 }
 123
 124 static int clkgena_divmux_is_enabled(struct clk_hw *hw)
 125 {
 126         struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
 127         struct clk_hw *mux_hw = &genamux->mux.hw;
 128
 129         __clk_hw_set_clk(mux_hw, hw);
 130
 131         return (s8)clk_mux_ops.get_parent(mux_hw) > 0;
 132 }
 133
 134 static u8 clkgena_divmux_get_parent(struct clk_hw *hw)
 135 {
 136         struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
 137         struct clk_hw *mux_hw = &genamux->mux.hw;
 138
 139         __clk_hw_set_clk(mux_hw, hw);
 140
 141         genamux->muxsel = clk_mux_ops.get_parent(mux_hw);
 142         if ((s8)genamux->muxsel < 0) {
 143                 pr_debug("%s: %s: Invalid parent, setting to default.\n",
 144                       __func__, __clk_get_name(hw->clk));
 145                 genamux->muxsel = 0;
 146         }
 147
 148         return genamux->muxsel;
 149 }
 150
 151 static int clkgena_divmux_set_parent(struct clk_hw *hw, u8 index)
 152 {
 153         struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
 154
 155         if (index >= CKGAX_CLKOPSRC_SWITCH_OFF)
 156                 return -EINVAL;
 157
 158         genamux->muxsel = index;
 159
 160         /*
 161          * If the mux is already enabled, call enable directly to set the
 162          * new mux position and wait for it to start running again. Otherwise
 163          * do nothing.
 164          */
 165         if (clkgena_divmux_is_enabled(hw))
 166                 clkgena_divmux_enable(hw);
 167
 168         return 0;
 169 }
 170
 171 static unsigned long clkgena_divmux_recalc_rate(struct clk_hw *hw,
 172                 unsigned long parent_rate)
 173 {
 174         struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
 175         struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;
 176
 177         __clk_hw_set_clk(div_hw, hw);
 178
 179         return clk_divider_ops.recalc_rate(div_hw, parent_rate);
 180 }
 181
 182 static int clkgena_divmux_set_rate(struct clk_hw *hw, unsigned long rate,
 183                                 unsigned long parent_rate)
 184 {
 185         struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
 186         struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;
 187
 188         __clk_hw_set_clk(div_hw, hw);
 189
 190         return clk_divider_ops.set_rate(div_hw, rate, parent_rate);
 191 }
 192
 193 static long clkgena_divmux_round_rate(struct clk_hw *hw, unsigned long rate,
 194                                    unsigned long *prate)
 195 {
 196         struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
 197         struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;
 198
 199         __clk_hw_set_clk(div_hw, hw);
 200
 201         return clk_divider_ops.round_rate(div_hw, rate, prate);
 202 }
 203
 204 static const struct clk_ops clkgena_divmux_ops = {
 205         .enable = clkgena_divmux_enable,
 206         .disable = clkgena_divmux_disable,
 207         .is_enabled = clkgena_divmux_is_enabled,
 208         .get_parent = clkgena_divmux_get_parent,
 209         .set_parent = clkgena_divmux_set_parent,
 210         .round_rate = clkgena_divmux_round_rate,
 211         .recalc_rate = clkgena_divmux_recalc_rate,
 212         .set_rate = clkgena_divmux_set_rate,
 213 };
 214
 215 /**
 216  * clk_register_genamux - register a genamux clock with the clock framework
 217  */
 218 static struct clk *clk_register_genamux(const char *name,
 219                                 const char **parent_names, u8 num_parents,
 220                                 void __iomem *reg,
 221                                 const struct clkgena_divmux_data *muxdata,
 222                                 u32 idx)
 223 {
 224         /*
 225          * Fixed constants across all ClockgenA variants
 226          */
 227         const int mux_width = 2;
 228         const int divider_width = 5;
 229         struct clkgena_divmux *genamux;
 230         struct clk *clk;
 231         struct clk_init_data init;
 232         int i;
 233
 234         genamux = kzalloc(sizeof(*genamux), GFP_KERNEL);
 235         if (!genamux)
 236                 return ERR_PTR(-ENOMEM);
 237
 238         init.name = name;
 239         init.ops = &clkgena_divmux_ops;
 240         init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
 241         init.parent_names = parent_names;
 242         init.num_parents = num_parents;
 243
 244         genamux->mux.lock  = &clkgena_divmux_lock;
 245         genamux->mux.mask = BIT(mux_width) - 1;
 246         genamux->mux.shift = muxdata->mux_start_bit + (idx * mux_width);
 247         if (genamux->mux.shift > 31) {
 248                 /*
 249                  * We have spilled into the second mux register so
 250                  * adjust the register address and the bit shift accordingly
 251                  */
 252                 genamux->mux.reg = reg + muxdata->mux_offset2;
 253                 genamux->mux.shift -= 32;
 254         } else {
 255                 genamux->mux.reg   = reg + muxdata->mux_offset;
 256         }
 257
 258         for (i = 0; i < NUM_INPUTS; i++) {
 259                 /*
 260                  * Divider config for each input
 261                  */
 262                 void __iomem *divbase = reg + muxdata->div_offsets[i];
 263                 genamux->div[i].width = divider_width;
 264                 genamux->div[i].reg = divbase + (idx * sizeof(u32));
 265
 266                 /*
 267                  * Mux enabled/running feedback register for each input.
 268                  */
 269                 genamux->feedback_reg[i] = reg + muxdata->fb_offsets[i];
 270         }
 271
 272         genamux->feedback_bit_idx = muxdata->fb_start_bit_idx + idx;
 273         genamux->hw.init = &init;
 274
 275         clk = clk_register(NULL, &genamux->hw);
 276         if (IS_ERR(clk)) {
 277                 kfree(genamux);
 278                 goto err;
 279         }
 280
 281         pr_debug("%s: parent %s rate %lu\n",
 282                         __clk_get_name(clk),
 283                         __clk_get_name(clk_get_parent(clk)),
 284                         clk_get_rate(clk));
 285 err:
 286         return clk;
 287 }
 288
 289 static struct clkgena_divmux_data st_divmux_c65hs = {
 290         .num_outputs = 4,
 291         .mux_offset = 0x14,
 292         .mux_start_bit = 0,
 293         .div_offsets = { 0x800, 0x900, 0xb00 },
 294         .fb_offsets = { 0x18, 0x1c, 0x20 },
 295         .fb_start_bit_idx = 0,
 296 };
 297
 298 static struct clkgena_divmux_data st_divmux_c65ls = {
 299         .num_outputs = 14,
 300         .mux_offset = 0x14,
 301         .mux_offset2 = 0x24,
 302         .mux_start_bit = 8,
 303         .div_offsets = { 0x810, 0xa10, 0xb10 },
 304         .fb_offsets = { 0x18, 0x1c, 0x20 },
 305         .fb_start_bit_idx = 4,
 306 };
 307
 308 static struct clkgena_divmux_data st_divmux_c32odf0 = {
 309         .num_outputs = 8,
 310         .mux_offset = 0x1c,
 311         .mux_start_bit = 0,
 312         .div_offsets = { 0x800, 0x900, 0xa60 },
 313         .fb_offsets = { 0x2c, 0x24, 0x28 },
 314         .fb_start_bit_idx = 0,
 315 };
 316
 317 static struct clkgena_divmux_data st_divmux_c32odf1 = {
 318         .num_outputs = 8,
 319         .mux_offset = 0x1c,
 320         .mux_start_bit = 16,
 321         .div_offsets = { 0x820, 0x980, 0xa80 },
 322         .fb_offsets = { 0x2c, 0x24, 0x28 },
 323         .fb_start_bit_idx = 8,
 324 };
 325
 326 static struct clkgena_divmux_data st_divmux_c32odf2 = {
 327         .num_outputs = 8,
 328         .mux_offset = 0x20,
 329         .mux_start_bit = 0,
 330         .div_offsets = { 0x840, 0xa20, 0xb10 },
 331         .fb_offsets = { 0x2c, 0x24, 0x28 },
 332         .fb_start_bit_idx = 16,
 333 };
 334
 335 static struct clkgena_divmux_data st_divmux_c32odf3 = {
 336         .num_outputs = 8,
 337         .mux_offset = 0x20,
 338         .mux_start_bit = 16,
 339         .div_offsets = { 0x860, 0xa40, 0xb30 },
 340         .fb_offsets = { 0x2c, 0x24, 0x28 },
 341         .fb_start_bit_idx = 24,
 342 };
 343
 344 static const struct of_device_id clkgena_divmux_of_match[] = {
 345         {
 346                 .compatible = "st,clkgena-divmux-c65-hs",
 347                 .data = &st_divmux_c65hs,
 348         },
 349         {
 350                 .compatible = "st,clkgena-divmux-c65-ls",
 351                 .data = &st_divmux_c65ls,
 352         },
 353         {
 354                 .compatible = "st,clkgena-divmux-c32-odf0",
 355                 .data = &st_divmux_c32odf0,
 356         },
 357         {
 358                 .compatible = "st,clkgena-divmux-c32-odf1",
 359                 .data = &st_divmux_c32odf1,
 360         },
 361         {
 362                 .compatible = "st,clkgena-divmux-c32-odf2",
 363                 .data = &st_divmux_c32odf2,
 364         },
 365         {
 366                 .compatible = "st,clkgena-divmux-c32-odf3",
 367                 .data = &st_divmux_c32odf3,
 368         },
 369         {}
 370 };
 371
 372 static void __iomem * __init clkgen_get_register_base(
 373                                 struct device_node *np)
 374 {
 375         struct device_node *pnode;
 376         void __iomem *reg = NULL;
 377
 378         pnode = of_get_parent(np);
 379         if (!pnode)
 380                 return NULL;
 381
 382         reg = of_iomap(pnode, 0);
 383
 384         of_node_put(pnode);
 385         return reg;
 386 }
 387
 388 static void __init st_of_clkgena_divmux_setup(struct device_node *np)
 389 {
 390         const struct of_device_id *match;
 391         const struct clkgena_divmux_data *data;
 392         struct clk_onecell_data *clk_data;
 393         void __iomem *reg;
 394         const char **parents;
 395         int num_parents = 0, i;
 396
 397         match = of_match_node(clkgena_divmux_of_match, np);
 398         if (WARN_ON(!match))
 399                 return;
 400
 401         data = (struct clkgena_divmux_data *)match->data;
 402
 403         reg = clkgen_get_register_base(np);
 404         if (!reg)
 405                 return;
 406
 407         parents = clkgen_mux_get_parents(np, &num_parents);
 408         if (IS_ERR(parents))
 409                 return;
 410
 411         clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
 412         if (!clk_data)
 413                 goto err;
 414
 415         clk_data->clk_num = data->num_outputs;
 416         clk_data->clks = kzalloc(clk_data->clk_num * sizeof(struct clk *),
 417                                  GFP_KERNEL);
 418
 419         if (!clk_data->clks)
 420                 goto err;
 421
 422         for (i = 0; i < clk_data->clk_num; i++) {
 423                 struct clk *clk;
 424                 const char *clk_name;
 425
 426                 if (of_property_read_string_index(np, "clock-output-names",
 427                                                   i, &clk_name))
 428                         break;
 429
 430                 /*
 431                  * If we read an empty clock name then the output is unused
 432                  */
 433                 if (*clk_name == '\0')
 434                         continue;
 435
 436                 clk = clk_register_genamux(clk_name, parents, num_parents,
 437                                            reg, data, i);
 438
 439                 if (IS_ERR(clk))
 440                         goto err;
 441
 442                 clk_data->clks[i] = clk;
 443         }
 444
 445         kfree(parents);
 446
 447         of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
 448         return;
 449 err:
 450         if (clk_data)
 451                 kfree(clk_data->clks);
 452
 453         kfree(clk_data);
 454         kfree(parents);
 455 }
 456 CLK_OF_DECLARE(clkgenadivmux, "st,clkgena-divmux", st_of_clkgena_divmux_setup);
 457
 458 struct clkgena_prediv_data {
 459         u32 offset;
 460         u8 shift;
 461         struct clk_div_table *table;
 462 };
 463
 464 static struct clk_div_table prediv_table16[] = {
 465         { .val = 0, .div = 1 },
 466         { .val = 1, .div = 16 },
 467         { .div = 0 },
 468 };
 469
 470 static struct clkgena_prediv_data prediv_c65_data = {
 471         .offset = 0x4c,
 472         .shift = 31,
 473         .table = prediv_table16,
 474 };
 475
 476 static struct clkgena_prediv_data prediv_c32_data = {
 477         .offset = 0x50,
 478         .shift = 1,
 479         .table = prediv_table16,
 480 };
 481
 482 static const struct of_device_id clkgena_prediv_of_match[] = {
 483         { .compatible = "st,clkgena-prediv-c65", .data = &prediv_c65_data },
 484         { .compatible = "st,clkgena-prediv-c32", .data = &prediv_c32_data },
 485         {}
 486 };
 487
 488 static void __init st_of_clkgena_prediv_setup(struct device_node *np)
 489 {
 490         const struct of_device_id *match;
 491         void __iomem *reg;
 492         const char *parent_name, *clk_name;
 493         struct clk *clk;
 494         struct clkgena_prediv_data *data;
 495
 496         match = of_match_node(clkgena_prediv_of_match, np);
 497         if (!match) {
 498                 pr_err("%s: No matching data\n", __func__);
 499                 return;
 500         }
 501
 502         data = (struct clkgena_prediv_data *)match->data;
 503
 504         reg = clkgen_get_register_base(np);
 505         if (!reg)
 506                 return;
 507
 508         parent_name = of_clk_get_parent_name(np, 0);
 509         if (!parent_name)
 510                 return;
 511
 512         if (of_property_read_string_index(np, "clock-output-names",
 513                                           0, &clk_name))
 514                 return;
 515
 516         clk = clk_register_divider_table(NULL, clk_name, parent_name,
 517                                          CLK_GET_RATE_NOCACHE,
 518                                          reg + data->offset, data->shift, 1,
 519                                          0, data->table, NULL);
 520         if (IS_ERR(clk))
 521                 return;
 522
 523         of_clk_add_provider(np, of_clk_src_simple_get, clk);
 524         pr_debug("%s: parent %s rate %u\n",
 525                 __clk_get_name(clk),
 526                 __clk_get_name(clk_get_parent(clk)),
 527                 (unsigned int)clk_get_rate(clk));
 528
 529         return;
 530 }
 531 CLK_OF_DECLARE(clkgenaprediv, "st,clkgena-prediv", st_of_clkgena_prediv_setup);
 532
 533 struct clkgen_mux_data {
 534         u32 offset;
 535         u8 shift;
 536         u8 width;
 537         spinlock_t *lock;
 538         unsigned long clk_flags;
 539         u8 mux_flags;
 540 };
 541
 542 static struct clkgen_mux_data clkgen_mux_c_vcc_hd_416 = {
 543         .offset = 0,
 544         .shift = 0,
 545         .width = 1,
 546 };
 547
 548 static struct clkgen_mux_data clkgen_mux_f_vcc_fvdp_416 = {
 549         .offset = 0,
 550         .shift = 0,
 551         .width = 1,
 552 };
 553
 554 static struct clkgen_mux_data clkgen_mux_f_vcc_hva_416 = {
 555         .offset = 0,
 556         .shift = 0,
 557         .width = 1,
 558 };
 559
 560 static struct clkgen_mux_data clkgen_mux_f_vcc_hd_416 = {
 561         .offset = 0,
 562         .shift = 16,
 563         .width = 1,
 564         .lock = &clkgenf_lock,
 565 };
 566
 567 static struct clkgen_mux_data clkgen_mux_c_vcc_sd_416 = {
 568         .offset = 0,
 569         .shift = 17,
 570         .width = 1,
 571         .lock = &clkgenf_lock,
 572 };
 573
 574 static struct clkgen_mux_data stih415_a9_mux_data = {
 575         .offset = 0,
 576         .shift = 1,
 577         .width = 2,
 578 };
 579 static struct clkgen_mux_data stih416_a9_mux_data = {
 580         .offset = 0,
 581         .shift = 0,
 582         .width = 2,
 583 };
 584 static struct clkgen_mux_data stih407_a9_mux_data = {
 585         .offset = 0x1a4,
 586         .shift = 0,
 587         .width = 2,
 588 };
 589
 590 static const struct of_device_id mux_of_match[] = {
 591         {
 592                 .compatible = "st,stih416-clkgenc-vcc-hd",
 593                 .data = &clkgen_mux_c_vcc_hd_416,
 594         },
 595         {
 596                 .compatible = "st,stih416-clkgenf-vcc-fvdp",
 597                 .data = &clkgen_mux_f_vcc_fvdp_416,
 598         },
 599         {
 600                 .compatible = "st,stih416-clkgenf-vcc-hva",
 601                 .data = &clkgen_mux_f_vcc_hva_416,
 602         },
 603         {
 604                 .compatible = "st,stih416-clkgenf-vcc-hd",
 605                 .data = &clkgen_mux_f_vcc_hd_416,
 606         },
 607         {
 608                 .compatible = "st,stih416-clkgenf-vcc-sd",
 609                 .data = &clkgen_mux_c_vcc_sd_416,
 610         },
 611         {
 612                 .compatible = "st,stih415-clkgen-a9-mux",
 613                 .data = &stih415_a9_mux_data,
 614         },
 615         {
 616                 .compatible = "st,stih416-clkgen-a9-mux",
 617                 .data = &stih416_a9_mux_data,
 618         },
 619         {
 620                 .compatible = "st,stih407-clkgen-a9-mux",
 621                 .data = &stih407_a9_mux_data,
 622         },
 623         {}
 624 };
 625
 626 static void __init st_of_clkgen_mux_setup(struct device_node *np)
 627 {
 628         const struct of_device_id *match;
 629         struct clk *clk;
 630         void __iomem *reg;
 631         const char **parents;
 632         int num_parents;
 633         struct clkgen_mux_data *data;
 634
 635         match = of_match_node(mux_of_match, np);
 636         if (!match) {
 637                 pr_err("%s: No matching data\n", __func__);
 638                 return;
 639         }
 640
 641         data = (struct clkgen_mux_data *)match->data;
 642
 643         reg = of_iomap(np, 0);
 644         if (!reg) {
 645                 pr_err("%s: Failed to get base address\n", __func__);
 646                 return;
 647         }
 648
 649         parents = clkgen_mux_get_parents(np, &num_parents);
 650         if (IS_ERR(parents)) {
 651                 pr_err("%s: Failed to get parents (%ld)\n",
 652                                 __func__, PTR_ERR(parents));
 653                 return;
 654         }
 655
 656         clk = clk_register_mux(NULL, np->name, parents, num_parents,
 657                                 data->clk_flags | CLK_SET_RATE_PARENT,
 658                                 reg + data->offset,
 659                                 data->shift, data->width, data->mux_flags,
 660                                 data->lock);
 661         if (IS_ERR(clk))
 662                 goto err;
 663
 664         pr_debug("%s: parent %s rate %u\n",
 665                         __clk_get_name(clk),
 666                         __clk_get_name(clk_get_parent(clk)),
 667                         (unsigned int)clk_get_rate(clk));
 668
 669         of_clk_add_provider(np, of_clk_src_simple_get, clk);
 670
 671 err:
 672         kfree(parents);
 673
 674         return;
 675 }
 676 CLK_OF_DECLARE(clkgen_mux, "st,clkgen-mux", st_of_clkgen_mux_setup);
 677
 678 #define VCC_MAX_CHANNELS 16
 679
 680 #define VCC_GATE_OFFSET 0x0
 681 #define VCC_MUX_OFFSET 0x4
 682 #define VCC_DIV_OFFSET 0x8
 683
 684 struct clkgen_vcc_data {
 685         spinlock_t *lock;
 686         unsigned long clk_flags;
 687 };
 688
 689 static struct clkgen_vcc_data st_clkgenc_vcc_416 = {
 690         .clk_flags = CLK_SET_RATE_PARENT,
 691 };
 692
 693 static struct clkgen_vcc_data st_clkgenf_vcc_416 = {
 694         .lock = &clkgenf_lock,
 695 };
 696
 697 static const struct of_device_id vcc_of_match[] = {
 698         { .compatible = "st,stih416-clkgenc", .data = &st_clkgenc_vcc_416 },
 699         { .compatible = "st,stih416-clkgenf", .data = &st_clkgenf_vcc_416 },
 700         {}
 701 };
 702
 703 static void __init st_of_clkgen_vcc_setup(struct device_node *np)
 704 {
 705         const struct of_device_id *match;
 706         void __iomem *reg;
 707         const char **parents;
 708         int num_parents, i;
 709         struct clk_onecell_data *clk_data;
 710         struct clkgen_vcc_data *data;
 711
 712         match = of_match_node(vcc_of_match, np);
 713         if (WARN_ON(!match))
 714                 return;
 715         data = (struct clkgen_vcc_data *)match->data;
 716
 717         reg = of_iomap(np, 0);
 718         if (!reg)
 719                 return;
 720
 721         parents = clkgen_mux_get_parents(np, &num_parents);
 722         if (IS_ERR(parents))
 723                 return;
 724
 725         clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
 726         if (!clk_data)
 727                 goto err;
 728
 729         clk_data->clk_num = VCC_MAX_CHANNELS;
 730         clk_data->clks = kzalloc(clk_data->clk_num * sizeof(struct clk *),
 731                                  GFP_KERNEL);
 732
 733         if (!clk_data->clks)
 734                 goto err;
 735
 736         for (i = 0; i < clk_data->clk_num; i++) {
 737                 struct clk *clk;
 738                 const char *clk_name;
 739                 struct clk_gate *gate;
 740                 struct clk_divider *div;
 741                 struct clk_mux *mux;
 742
 743                 if (of_property_read_string_index(np, "clock-output-names",
 744                                                   i, &clk_name))
 745                         break;
 746
 747                 /*
 748                  * If we read an empty clock name then the output is unused
 749                  */
 750                 if (*clk_name == '\0')
 751                         continue;
 752
 753                 gate = kzalloc(sizeof(struct clk_gate), GFP_KERNEL);
 754                 if (!gate)
 755                         break;
 756
 757                 div = kzalloc(sizeof(struct clk_divider), GFP_KERNEL);
 758                 if (!div) {
 759                         kfree(gate);
 760                         break;
 761                 }
 762
 763                 mux = kzalloc(sizeof(struct clk_mux), GFP_KERNEL);
 764                 if (!mux) {
 765                         kfree(gate);
 766                         kfree(div);
 767                         break;
 768                 }
 769
 770                 gate->reg = reg + VCC_GATE_OFFSET;
 771                 gate->bit_idx = i;
 772                 gate->flags = CLK_GATE_SET_TO_DISABLE;
 773                 gate->lock = data->lock;
 774
 775                 div->reg = reg + VCC_DIV_OFFSET;
 776                 div->shift = 2 * i;
 777                 div->width = 2;
 778                 div->flags = CLK_DIVIDER_POWER_OF_TWO |
 779                         CLK_DIVIDER_ROUND_CLOSEST;
 780
 781                 mux->reg = reg + VCC_MUX_OFFSET;
 782                 mux->shift = 2 * i;
 783                 mux->mask = 0x3;
 784
 785                 clk = clk_register_composite(NULL, clk_name, parents,
 786                                              num_parents,
 787                                              &mux->hw, &clk_mux_ops,
 788                                              &div->hw, &clk_divider_ops,
 789                                              &gate->hw, &clk_gate_ops,
 790                                              data->clk_flags |
 791                                              CLK_GET_RATE_NOCACHE);
 792                 if (IS_ERR(clk)) {
 793                         kfree(gate);
 794                         kfree(div);
 795                         kfree(mux);
 796                         goto err;
 797                 }
 798
 799                 pr_debug("%s: parent %s rate %u\n",
 800                         __clk_get_name(clk),
 801                         __clk_get_name(clk_get_parent(clk)),
 802                         (unsigned int)clk_get_rate(clk));
 803
 804                 clk_data->clks[i] = clk;
 805         }
 806
 807         kfree(parents);
 808
 809         of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
 810         return;
 811
 812 err:
 813         for (i = 0; i < clk_data->clk_num; i++) {
 814                 struct clk_composite *composite;
 815
 816                 if (!clk_data->clks[i])
 817                         continue;
 818
 819                 composite = container_of(__clk_get_hw(clk_data->clks[i]),
 820                                          struct clk_composite, hw);
 821                 kfree(container_of(composite->gate_hw, struct clk_gate, hw));
 822                 kfree(container_of(composite->rate_hw, struct clk_divider, hw));
 823                 kfree(container_of(composite->mux_hw, struct clk_mux, hw));
 824         }
 825
 826         if (clk_data)
 827                 kfree(clk_data->clks);
 828
 829         kfree(clk_data);
 830         kfree(parents);
 831 }
 832 CLK_OF_DECLARE(clkgen_vcc, "st,clkgen-vcc", st_of_clkgen_vcc_setup);