drivers/clk/mmp/clk-mix.c

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