drivers/clk/mmp/clk-mix.c

   1 /*
   2  * mmp mix(div and mux) clock operation source file
   3  *
   4  * Copyright (C) 2014 Marvell
   5  * Chao Xie <chao.xie@marvell.com>
   6  *
   7  * This file is licensed under the terms of the GNU General Public
   8  * License version 2. This program is licensed "as is" without any
   9  * warranty of any kind, whether express or implied.
  10  */
  11
  12 #include <linux/clk-provider.h>
  13 #include <linux/slab.h>
  14 #include <linux/io.h>
  15 #include <linux/err.h>
  16
  17 #include "clk.h"
  18
  19 /*
  20  * The mix clock is a clock combined mux and div type clock.
  21  * Because the div field and mux field need to be set at same
  22  * time, we can not divide it into 2 types of clock
  23  */
  24
  25 #define to_clk_mix(hw)  container_of(hw, struct mmp_clk_mix, hw)
  26
  27 static unsigned int _get_maxdiv(struct mmp_clk_mix *mix)
  28 {
  29         unsigned int div_mask = (1 << mix->reg_info.width_div) - 1;
  30         unsigned int maxdiv = 0;
  31         struct clk_div_table *clkt;
  32
  33         if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
  34                 return div_mask;
  35         if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
  36                 return 1 << div_mask;
  37         if (mix->div_table) {
  38                 for (clkt = mix->div_table; clkt->div; clkt++)
  39                         if (clkt->div > maxdiv)
  40                                 maxdiv = clkt->div;
  41                 return maxdiv;
  42         }
  43         return div_mask + 1;
  44 }
  45
  46 static unsigned int _get_div(struct mmp_clk_mix *mix, unsigned int val)
  47 {
  48         struct clk_div_table *clkt;
  49
  50         if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
  51                 return val;
  52         if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
  53                 return 1 << val;
  54         if (mix->div_table) {
  55                 for (clkt = mix->div_table; clkt->div; clkt++)
  56                         if (clkt->val == val)
  57                                 return clkt->div;
  58                 if (clkt->div == 0)
  59                         return 0;
  60         }
  61         return val + 1;
  62 }
  63
  64 static unsigned int _get_mux(struct mmp_clk_mix *mix, unsigned int val)
  65 {
  66         int num_parents = __clk_get_num_parents(mix->hw.clk);
  67         int i;
  68
  69         if (mix->mux_flags & CLK_MUX_INDEX_BIT)
  70                 return ffs(val) - 1;
  71         if (mix->mux_flags & CLK_MUX_INDEX_ONE)
  72                 return val - 1;
  73         if (mix->mux_table) {
  74                 for (i = 0; i < num_parents; i++)
  75                         if (mix->mux_table[i] == val)
  76                                 return i;
  77                 if (i == num_parents)
  78                         return 0;
  79         }
  80
  81         return val;
  82 }
  83 static unsigned int _get_div_val(struct mmp_clk_mix *mix, unsigned int div)
  84 {
  85         struct clk_div_table *clkt;
  86
  87         if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
  88                 return div;
  89         if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
  90                 return __ffs(div);
  91         if (mix->div_table) {
  92                 for (clkt = mix->div_table; clkt->div; clkt++)
  93                         if (clkt->div == div)
  94                                 return clkt->val;
  95                 if (clkt->div == 0)
  96                         return 0;
  97         }
  98
  99         return div - 1;
 100 }
 101
 102 static unsigned int _get_mux_val(struct mmp_clk_mix *mix, unsigned int mux)
 103 {
 104         if (mix->mux_table)
 105                 return mix->mux_table[mux];
 106
 107         return mux;
 108 }
 109
 110 static void _filter_clk_table(struct mmp_clk_mix *mix,
 111                                 struct mmp_clk_mix_clk_table *table,
 112                                 unsigned int table_size)
 113 {
 114         int i;
 115         struct mmp_clk_mix_clk_table *item;
 116         struct clk *parent, *clk;
 117         unsigned long parent_rate;
 118
 119         clk = mix->hw.clk;
 120
 121         for (i = 0; i < table_size; i++) {
 122                 item = &table[i];
 123                 parent = clk_get_parent_by_index(clk, item->parent_index);
 124                 parent_rate = __clk_get_rate(parent);
 125                 if (parent_rate % item->rate) {
 126                         item->valid = 0;
 127                 } else {
 128                         item->divisor = parent_rate / item->rate;
 129                         item->valid = 1;
 130                 }
 131         }
 132 }
 133
 134 static int _set_rate(struct mmp_clk_mix *mix, u32 mux_val, u32 div_val,
 135                         unsigned int change_mux, unsigned int change_div)
 136 {
 137         struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
 138         u8 width, shift;
 139         u32 mux_div, fc_req;
 140         int ret, timeout = 50;
 141         unsigned long flags = 0;
 142
 143         if (!change_mux && !change_div)
 144                 return -EINVAL;
 145
 146         if (mix->lock)
 147                 spin_lock_irqsave(mix->lock, flags);
 148
 149         if (mix->type == MMP_CLK_MIX_TYPE_V1
 150                 || mix->type == MMP_CLK_MIX_TYPE_V2)
 151                 mux_div = readl(ri->reg_clk_ctrl);
 152         else
 153                 mux_div = readl(ri->reg_clk_sel);
 154
 155         if (change_div) {
 156                 width = ri->width_div;
 157                 shift = ri->shift_div;
 158                 mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
 159                 mux_div |= MMP_CLK_BITS_SET_VAL(div_val, width, shift);
 160         }
 161
 162         if (change_mux) {
 163                 width = ri->width_mux;
 164                 shift = ri->shift_mux;
 165                 mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
 166                 mux_div |= MMP_CLK_BITS_SET_VAL(mux_val, width, shift);
 167         }
 168
 169         if (mix->type == MMP_CLK_MIX_TYPE_V1) {
 170                 writel(mux_div, ri->reg_clk_ctrl);
 171         } else if (mix->type == MMP_CLK_MIX_TYPE_V2) {
 172                 mux_div |= (1 << ri->bit_fc);
 173                 writel(mux_div, ri->reg_clk_ctrl);
 174
 175                 do {
 176                         fc_req = readl(ri->reg_clk_ctrl);
 177                         timeout--;
 178                         if (!(fc_req & (1 << ri->bit_fc)))
 179                                 break;
 180                 } while (timeout);
 181
 182                 if (timeout == 0) {
 183                         pr_err("%s:%s cannot do frequency change\n",
 184                                 __func__, __clk_get_name(mix->hw.clk));
 185                         ret = -EBUSY;
 186                         goto error;
 187                 }
 188         } else {
 189                 fc_req = readl(ri->reg_clk_ctrl);
 190                 fc_req |= 1 << ri->bit_fc;
 191                 writel(fc_req, ri->reg_clk_ctrl);
 192                 writel(mux_div, ri->reg_clk_sel);
 193                 fc_req &= ~(1 << ri->bit_fc);
 194         }
 195
 196         ret = 0;
 197 error:
 198         if (mix->lock)
 199                 spin_unlock_irqrestore(mix->lock, flags);
 200
 201         return ret;
 202 }
 203
 204 static long mmp_clk_mix_determine_rate(struct clk_hw *hw, unsigned long rate,
 205                                         unsigned long min_rate,
 206                                         unsigned long max_rate,
 207                                         unsigned long *best_parent_rate,
 208                                         struct clk_hw **best_parent_clk)
 209 {
 210         struct mmp_clk_mix *mix = to_clk_mix(hw);
 211         struct mmp_clk_mix_clk_table *item;
 212         struct clk *parent, *parent_best, *mix_clk;
 213         unsigned long parent_rate, mix_rate, mix_rate_best, parent_rate_best;
 214         unsigned long gap, gap_best;
 215         u32 div_val_max;
 216         unsigned int div;
 217         int i, j;
 218
 219         mix_clk = hw->clk;
 220
 221         parent = NULL;
 222         mix_rate_best = 0;
 223         parent_rate_best = 0;
 224         gap_best = rate;
 225         parent_best = NULL;
 226
 227         if (mix->table) {
 228                 for (i = 0; i < mix->table_size; i++) {
 229                         item = &mix->table[i];
 230                         if (item->valid == 0)
 231                                 continue;
 232                         parent = clk_get_parent_by_index(mix_clk,
 233                                                         item->parent_index);
 234                         parent_rate = __clk_get_rate(parent);
 235                         mix_rate = parent_rate / item->divisor;
 236                         gap = abs(mix_rate - rate);
 237                         if (parent_best == NULL || gap < gap_best) {
 238                                 parent_best = parent;
 239                                 parent_rate_best = parent_rate;
 240                                 mix_rate_best = mix_rate;
 241                                 gap_best = gap;
 242                                 if (gap_best == 0)
 243                                         goto found;
 244                         }
 245                 }
 246         } else {
 247                 for (i = 0; i < __clk_get_num_parents(mix_clk); i++) {
 248                         parent = clk_get_parent_by_index(mix_clk, i);
 249                         parent_rate = __clk_get_rate(parent);
 250                         div_val_max = _get_maxdiv(mix);
 251                         for (j = 0; j < div_val_max; j++) {
 252                                 div = _get_div(mix, j);
 253                                 mix_rate = parent_rate / div;
 254                                 gap = abs(mix_rate - rate);
 255                                 if (parent_best == NULL || gap < gap_best) {
 256                                         parent_best = parent;
 257                                         parent_rate_best = parent_rate;
 258                                         mix_rate_best = mix_rate;
 259                                         gap_best = gap;
 260                                         if (gap_best == 0)
 261                                                 goto found;
 262                                 }
 263                         }
 264                 }
 265         }
 266
 267 found:
 268         *best_parent_rate = parent_rate_best;
 269         *best_parent_clk = __clk_get_hw(parent_best);
 270
 271         return mix_rate_best;
 272 }
 273
 274 static int mmp_clk_mix_set_rate_and_parent(struct clk_hw *hw,
 275                                                 unsigned long rate,
 276                                                 unsigned long parent_rate,
 277                                                 u8 index)
 278 {
 279         struct mmp_clk_mix *mix = to_clk_mix(hw);
 280         unsigned int div;
 281         u32 div_val, mux_val;
 282
 283         div = parent_rate / rate;
 284         div_val = _get_div_val(mix, div);
 285         mux_val = _get_mux_val(mix, index);
 286
 287         return _set_rate(mix, mux_val, div_val, 1, 1);
 288 }
 289
 290 static u8 mmp_clk_mix_get_parent(struct clk_hw *hw)
 291 {
 292         struct mmp_clk_mix *mix = to_clk_mix(hw);
 293         struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
 294         unsigned long flags = 0;
 295         u32 mux_div = 0;
 296         u8 width, shift;
 297         u32 mux_val;
 298
 299         if (mix->lock)
 300                 spin_lock_irqsave(mix->lock, flags);
 301
 302         if (mix->type == MMP_CLK_MIX_TYPE_V1
 303                 || mix->type == MMP_CLK_MIX_TYPE_V2)
 304                 mux_div = readl(ri->reg_clk_ctrl);
 305         else
 306                 mux_div = readl(ri->reg_clk_sel);
 307
 308         if (mix->lock)
 309                 spin_unlock_irqrestore(mix->lock, flags);
 310
 311         width = mix->reg_info.width_mux;
 312         shift = mix->reg_info.shift_mux;
 313
 314         mux_val = MMP_CLK_BITS_GET_VAL(mux_div, width, shift);
 315
 316         return _get_mux(mix, mux_val);
 317 }
 318
 319 static unsigned long mmp_clk_mix_recalc_rate(struct clk_hw *hw,
 320                                         unsigned long parent_rate)
 321 {
 322         struct mmp_clk_mix *mix = to_clk_mix(hw);
 323         struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
 324         unsigned long flags = 0;
 325         u32 mux_div = 0;
 326         u8 width, shift;
 327         unsigned int div;
 328
 329         if (mix->lock)
 330                 spin_lock_irqsave(mix->lock, flags);
 331
 332         if (mix->type == MMP_CLK_MIX_TYPE_V1
 333                 || mix->type == MMP_CLK_MIX_TYPE_V2)
 334                 mux_div = readl(ri->reg_clk_ctrl);
 335         else
 336                 mux_div = readl(ri->reg_clk_sel);
 337
 338         if (mix->lock)
 339                 spin_unlock_irqrestore(mix->lock, flags);
 340
 341         width = mix->reg_info.width_div;
 342         shift = mix->reg_info.shift_div;
 343
 344         div = _get_div(mix, MMP_CLK_BITS_GET_VAL(mux_div, width, shift));
 345
 346         return parent_rate / div;
 347 }
 348
 349 static int mmp_clk_set_parent(struct clk_hw *hw, u8 index)
 350 {
 351         struct mmp_clk_mix *mix = to_clk_mix(hw);
 352         struct mmp_clk_mix_clk_table *item;
 353         int i;
 354         u32 div_val, mux_val;
 355
 356         if (mix->table) {
 357                 for (i = 0; i < mix->table_size; i++) {
 358                         item = &mix->table[i];
 359                         if (item->valid == 0)
 360                                 continue;
 361                         if (item->parent_index == index)
 362                                 break;
 363                 }
 364                 if (i < mix->table_size) {
 365                         div_val = _get_div_val(mix, item->divisor);
 366                         mux_val = _get_mux_val(mix, item->parent_index);
 367                 } else
 368                         return -EINVAL;
 369         } else {
 370                 mux_val = _get_mux_val(mix, index);
 371                 div_val = 0;
 372         }
 373
 374         return _set_rate(mix, mux_val, div_val, 1, div_val ? 1 : 0);
 375 }
 376
 377 static int mmp_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 378                                 unsigned long best_parent_rate)
 379 {
 380         struct mmp_clk_mix *mix = to_clk_mix(hw);
 381         struct mmp_clk_mix_clk_table *item;
 382         unsigned long parent_rate;
 383         unsigned int best_divisor;
 384         struct clk *mix_clk, *parent;
 385         int i;
 386
 387         best_divisor = best_parent_rate / rate;
 388
 389         mix_clk = hw->clk;
 390         if (mix->table) {
 391                 for (i = 0; i < mix->table_size; i++) {
 392                         item = &mix->table[i];
 393                         if (item->valid == 0)
 394                                 continue;
 395                         parent = clk_get_parent_by_index(mix_clk,
 396                                                         item->parent_index);
 397                         parent_rate = __clk_get_rate(parent);
 398                         if (parent_rate == best_parent_rate
 399                                 && item->divisor == best_divisor)
 400                                 break;
 401                 }
 402                 if (i < mix->table_size)
 403                         return _set_rate(mix,
 404                                         _get_mux_val(mix, item->parent_index),
 405                                         _get_div_val(mix, item->divisor),
 406                                         1, 1);
 407                 else
 408                         return -EINVAL;
 409         } else {
 410                 for (i = 0; i < __clk_get_num_parents(mix_clk); i++) {
 411                         parent = clk_get_parent_by_index(mix_clk, i);
 412                         parent_rate = __clk_get_rate(parent);
 413                         if (parent_rate == best_parent_rate)
 414                                 break;
 415                 }
 416                 if (i < __clk_get_num_parents(mix_clk))
 417                         return _set_rate(mix, _get_mux_val(mix, i),
 418                                         _get_div_val(mix, best_divisor), 1, 1);
 419                 else
 420                         return -EINVAL;
 421         }
 422 }
 423
 424 static void mmp_clk_mix_init(struct clk_hw *hw)
 425 {
 426         struct mmp_clk_mix *mix = to_clk_mix(hw);
 427
 428         if (mix->table)
 429                 _filter_clk_table(mix, mix->table, mix->table_size);
 430 }
 431
 432 const struct clk_ops mmp_clk_mix_ops = {
 433         .determine_rate = mmp_clk_mix_determine_rate,
 434         .set_rate_and_parent = mmp_clk_mix_set_rate_and_parent,
 435         .set_rate = mmp_clk_set_rate,
 436         .set_parent = mmp_clk_set_parent,
 437         .get_parent = mmp_clk_mix_get_parent,
 438         .recalc_rate = mmp_clk_mix_recalc_rate,
 439         .init = mmp_clk_mix_init,
 440 };
 441
 442 struct clk *mmp_clk_register_mix(struct device *dev,
 443                                         const char *name,
 444                                         const char **parent_names,
 445                                         u8 num_parents,
 446                                         unsigned long flags,
 447                                         struct mmp_clk_mix_config *config,
 448                                         spinlock_t *lock)
 449 {
 450         struct mmp_clk_mix *mix;
 451         struct clk *clk;
 452         struct clk_init_data init;
 453         size_t table_bytes;
 454
 455         mix = kzalloc(sizeof(*mix), GFP_KERNEL);
 456         if (!mix) {
 457                 pr_err("%s:%s: could not allocate mmp mix clk\n",
 458                         __func__, name);
 459                 return ERR_PTR(-ENOMEM);
 460         }
 461
 462         init.name = name;
 463         init.flags = flags | CLK_GET_RATE_NOCACHE;
 464         init.parent_names = parent_names;
 465         init.num_parents = num_parents;
 466         init.ops = &mmp_clk_mix_ops;
 467
 468         memcpy(&mix->reg_info, &config->reg_info, sizeof(config->reg_info));
 469         if (config->table) {
 470                 table_bytes = sizeof(*config->table) * config->table_size;
 471                 mix->table = kzalloc(table_bytes, GFP_KERNEL);
 472                 if (!mix->table) {
 473                         pr_err("%s:%s: could not allocate mmp mix table\n",
 474                                 __func__, name);
 475                         kfree(mix);
 476                         return ERR_PTR(-ENOMEM);
 477                 }
 478                 memcpy(mix->table, config->table, table_bytes);
 479                 mix->table_size = config->table_size;
 480         }
 481
 482         if (config->mux_table) {
 483                 table_bytes = sizeof(u32) * num_parents;
 484                 mix->mux_table = kzalloc(table_bytes, GFP_KERNEL);
 485                 if (!mix->mux_table) {
 486                         pr_err("%s:%s: could not allocate mmp mix mux-table\n",
 487                                 __func__, name);
 488                         kfree(mix->table);
 489                         kfree(mix);
 490                         return ERR_PTR(-ENOMEM);
 491                 }
 492                 memcpy(mix->mux_table, config->mux_table, table_bytes);
 493         }
 494
 495         mix->div_flags = config->div_flags;
 496         mix->mux_flags = config->mux_flags;
 497         mix->lock = lock;
 498         mix->hw.init = &init;
 499
 500         if (config->reg_info.bit_fc >= 32)
 501                 mix->type = MMP_CLK_MIX_TYPE_V1;
 502         else if (config->reg_info.reg_clk_sel)
 503                 mix->type = MMP_CLK_MIX_TYPE_V3;
 504         else
 505                 mix->type = MMP_CLK_MIX_TYPE_V2;
 506         clk = clk_register(dev, &mix->hw);
 507
 508         if (IS_ERR(clk)) {
 509                 kfree(mix->mux_table);
 510                 kfree(mix->table);
 511                 kfree(mix);
 512         }
 513
 514         return clk;
 515 }