drivers/clk/at91/clk-master.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
   4  */
   5
   6 #include <linux/clk-provider.h>
   7 #include <linux/clkdev.h>
   8 #include <linux/clk.h>
   9 #include <linux/clk/at91_pmc.h>
  10 #include <linux/of.h>
  11 #include <linux/mfd/syscon.h>
  12 #include <linux/regmap.h>
  13
  14 #include "pmc.h"
  15
  16 #define MASTER_PRES_MASK        0x7
  17 #define MASTER_PRES_MAX         MASTER_PRES_MASK
  18 #define MASTER_DIV_SHIFT        8
  19 #define MASTER_DIV_MASK         0x7
  20
  21 #define PMC_MCR_CSS_SHIFT       (16)
  22
  23 #define MASTER_MAX_ID           4
  24
  25 #define to_clk_master(hw) container_of(hw, struct clk_master, hw)
  26
  27 struct clk_master {
  28         struct clk_hw hw;
  29         struct regmap *regmap;
  30         spinlock_t *lock;
  31         const struct clk_master_layout *layout;
  32         const struct clk_master_characteristics *characteristics;
  33         struct at91_clk_pms pms;
  34         u32 *mux_table;
  35         u32 mckr;
  36         int chg_pid;
  37         u8 id;
  38         u8 parent;
  39         u8 div;
  40         u32 safe_div;
  41 };
  42
  43 /* MCK div reference to be used by notifier. */
  44 static struct clk_master *master_div;
  45
  46 static inline bool clk_master_ready(struct clk_master *master)
  47 {
  48         unsigned int bit = master->id ? AT91_PMC_MCKXRDY : AT91_PMC_MCKRDY;
  49         unsigned int status;
  50
  51         regmap_read(master->regmap, AT91_PMC_SR, &status);
  52
  53         return !!(status & bit);
  54 }
  55
  56 static int clk_master_prepare(struct clk_hw *hw)
  57 {
  58         struct clk_master *master = to_clk_master(hw);
  59         unsigned long flags;
  60
  61         spin_lock_irqsave(master->lock, flags);
  62
  63         while (!clk_master_ready(master))
  64                 cpu_relax();
  65
  66         spin_unlock_irqrestore(master->lock, flags);
  67
  68         return 0;
  69 }
  70
  71 static int clk_master_is_prepared(struct clk_hw *hw)
  72 {
  73         struct clk_master *master = to_clk_master(hw);
  74         unsigned long flags;
  75         bool status;
  76
  77         spin_lock_irqsave(master->lock, flags);
  78         status = clk_master_ready(master);
  79         spin_unlock_irqrestore(master->lock, flags);
  80
  81         return status;
  82 }
  83
  84 static unsigned long clk_master_div_recalc_rate(struct clk_hw *hw,
  85                                                 unsigned long parent_rate)
  86 {
  87         u8 div;
  88         unsigned long flags, rate = parent_rate;
  89         struct clk_master *master = to_clk_master(hw);
  90         const struct clk_master_layout *layout = master->layout;
  91         const struct clk_master_characteristics *characteristics =
  92                                                 master->characteristics;
  93         unsigned int mckr;
  94
  95         spin_lock_irqsave(master->lock, flags);
  96         regmap_read(master->regmap, master->layout->offset, &mckr);
  97         spin_unlock_irqrestore(master->lock, flags);
  98
  99         mckr &= layout->mask;
 100
 101         div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
 102
 103         rate /= characteristics->divisors[div];
 104
 105         if (rate < characteristics->output.min)
 106                 pr_warn("master clk div is underclocked");
 107         else if (rate > characteristics->output.max)
 108                 pr_warn("master clk div is overclocked");
 109
 110         return rate;
 111 }
 112
 113 static int clk_master_div_save_context(struct clk_hw *hw)
 114 {
 115         struct clk_master *master = to_clk_master(hw);
 116         struct clk_hw *parent_hw = clk_hw_get_parent(hw);
 117         unsigned long flags;
 118         unsigned int mckr, div;
 119
 120         spin_lock_irqsave(master->lock, flags);
 121         regmap_read(master->regmap, master->layout->offset, &mckr);
 122         spin_unlock_irqrestore(master->lock, flags);
 123
 124         mckr &= master->layout->mask;
 125         div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
 126         div = master->characteristics->divisors[div];
 127
 128         master->pms.parent_rate = clk_hw_get_rate(parent_hw);
 129         master->pms.rate = DIV_ROUND_CLOSEST(master->pms.parent_rate, div);
 130
 131         return 0;
 132 }
 133
 134 static void clk_master_div_restore_context(struct clk_hw *hw)
 135 {
 136         struct clk_master *master = to_clk_master(hw);
 137         unsigned long flags;
 138         unsigned int mckr;
 139         u8 div;
 140
 141         spin_lock_irqsave(master->lock, flags);
 142         regmap_read(master->regmap, master->layout->offset, &mckr);
 143         spin_unlock_irqrestore(master->lock, flags);
 144
 145         mckr &= master->layout->mask;
 146         div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
 147         div = master->characteristics->divisors[div];
 148
 149         if (div != DIV_ROUND_CLOSEST(master->pms.parent_rate, master->pms.rate))
 150                 pr_warn("MCKR DIV not configured properly by firmware!\n");
 151 }
 152
 153 static const struct clk_ops master_div_ops = {
 154         .prepare = clk_master_prepare,
 155         .is_prepared = clk_master_is_prepared,
 156         .recalc_rate = clk_master_div_recalc_rate,
 157         .save_context = clk_master_div_save_context,
 158         .restore_context = clk_master_div_restore_context,
 159 };
 160
 161 /* This function must be called with lock acquired. */
 162 static int clk_master_div_set(struct clk_master *master,
 163                               unsigned long parent_rate, int div)
 164 {
 165         const struct clk_master_characteristics *characteristics =
 166                                                 master->characteristics;
 167         unsigned long rate = parent_rate;
 168         unsigned int max_div = 0, div_index = 0, max_div_index = 0;
 169         unsigned int i, mckr, tmp;
 170         int ret;
 171
 172         for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) {
 173                 if (!characteristics->divisors[i])
 174                         break;
 175
 176                 if (div == characteristics->divisors[i])
 177                         div_index = i;
 178
 179                 if (max_div < characteristics->divisors[i]) {
 180                         max_div = characteristics->divisors[i];
 181                         max_div_index = i;
 182                 }
 183         }
 184
 185         if (div > max_div)
 186                 div_index = max_div_index;
 187
 188         ret = regmap_read(master->regmap, master->layout->offset, &mckr);
 189         if (ret)
 190                 return ret;
 191
 192         mckr &= master->layout->mask;
 193         tmp = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
 194         if (tmp == div_index)
 195                 return 0;
 196
 197         rate /= characteristics->divisors[div_index];
 198         if (rate < characteristics->output.min)
 199                 pr_warn("master clk div is underclocked");
 200         else if (rate > characteristics->output.max)
 201                 pr_warn("master clk div is overclocked");
 202
 203         mckr &= ~(MASTER_DIV_MASK << MASTER_DIV_SHIFT);
 204         mckr |= (div_index << MASTER_DIV_SHIFT);
 205         ret = regmap_write(master->regmap, master->layout->offset, mckr);
 206         if (ret)
 207                 return ret;
 208
 209         while (!clk_master_ready(master))
 210                 cpu_relax();
 211
 212         master->div = characteristics->divisors[div_index];
 213
 214         return 0;
 215 }
 216
 217 static unsigned long clk_master_div_recalc_rate_chg(struct clk_hw *hw,
 218                                                     unsigned long parent_rate)
 219 {
 220         struct clk_master *master = to_clk_master(hw);
 221
 222         return DIV_ROUND_CLOSEST_ULL(parent_rate, master->div);
 223 }
 224
 225 static void clk_master_div_restore_context_chg(struct clk_hw *hw)
 226 {
 227         struct clk_master *master = to_clk_master(hw);
 228         unsigned long flags;
 229         int ret;
 230
 231         spin_lock_irqsave(master->lock, flags);
 232         ret = clk_master_div_set(master, master->pms.parent_rate,
 233                                  DIV_ROUND_CLOSEST(master->pms.parent_rate,
 234                                                    master->pms.rate));
 235         spin_unlock_irqrestore(master->lock, flags);
 236         if (ret)
 237                 pr_warn("Failed to restore MCK DIV clock\n");
 238 }
 239
 240 static const struct clk_ops master_div_ops_chg = {
 241         .prepare = clk_master_prepare,
 242         .is_prepared = clk_master_is_prepared,
 243         .recalc_rate = clk_master_div_recalc_rate_chg,
 244         .save_context = clk_master_div_save_context,
 245         .restore_context = clk_master_div_restore_context_chg,
 246 };
 247
 248 static int clk_master_div_notifier_fn(struct notifier_block *notifier,
 249                                       unsigned long code, void *data)
 250 {
 251         const struct clk_master_characteristics *characteristics =
 252                                                 master_div->characteristics;
 253         struct clk_notifier_data *cnd = data;
 254         unsigned long flags, new_parent_rate, new_rate;
 255         unsigned int mckr, div, new_div = 0;
 256         int ret, i;
 257         long tmp_diff;
 258         long best_diff = -1;
 259
 260         spin_lock_irqsave(master_div->lock, flags);
 261         switch (code) {
 262         case PRE_RATE_CHANGE:
 263                 /*
 264                  * We want to avoid any overclocking of MCK DIV domain. To do
 265                  * this we set a safe divider (the underclocking is not of
 266                  * interest as we can go as low as 32KHz). The relation
 267                  * b/w this clock and its parents are as follows:
 268                  *
 269                  * FRAC PLL -> DIV PLL -> MCK DIV
 270                  *
 271                  * With the proper safe divider we should be good even with FRAC
 272                  * PLL at its maximum value.
 273                  */
 274                 ret = regmap_read(master_div->regmap, master_div->layout->offset,
 275                                   &mckr);
 276                 if (ret) {
 277                         ret = NOTIFY_STOP_MASK;
 278                         goto unlock;
 279                 }
 280
 281                 mckr &= master_div->layout->mask;
 282                 div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
 283
 284                 /* Switch to safe divider. */
 285                 clk_master_div_set(master_div,
 286                                    cnd->old_rate * characteristics->divisors[div],
 287                                    master_div->safe_div);
 288                 break;
 289
 290         case POST_RATE_CHANGE:
 291                 /*
 292                  * At this point we want to restore MCK DIV domain to its maximum
 293                  * allowed rate.
 294                  */
 295                 ret = regmap_read(master_div->regmap, master_div->layout->offset,
 296                                   &mckr);
 297                 if (ret) {
 298                         ret = NOTIFY_STOP_MASK;
 299                         goto unlock;
 300                 }
 301
 302                 mckr &= master_div->layout->mask;
 303                 div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
 304                 new_parent_rate = cnd->new_rate * characteristics->divisors[div];
 305
 306                 for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) {
 307                         if (!characteristics->divisors[i])
 308                                 break;
 309
 310                         new_rate = DIV_ROUND_CLOSEST_ULL(new_parent_rate,
 311                                                          characteristics->divisors[i]);
 312
 313                         tmp_diff = characteristics->output.max - new_rate;
 314                         if (tmp_diff < 0)
 315                                 continue;
 316
 317                         if (best_diff < 0 || best_diff > tmp_diff) {
 318                                 new_div = characteristics->divisors[i];
 319                                 best_diff = tmp_diff;
 320                         }
 321
 322                         if (!tmp_diff)
 323                                 break;
 324                 }
 325
 326                 if (!new_div) {
 327                         ret = NOTIFY_STOP_MASK;
 328                         goto unlock;
 329                 }
 330
 331                 /* Update the div to preserve MCK DIV clock rate. */
 332                 clk_master_div_set(master_div, new_parent_rate,
 333                                    new_div);
 334
 335                 ret = NOTIFY_OK;
 336                 break;
 337
 338         default:
 339                 ret = NOTIFY_DONE;
 340                 break;
 341         }
 342
 343 unlock:
 344         spin_unlock_irqrestore(master_div->lock, flags);
 345
 346         return ret;
 347 }
 348
 349 static struct notifier_block clk_master_div_notifier = {
 350         .notifier_call = clk_master_div_notifier_fn,
 351 };
 352
 353 static void clk_sama7g5_master_best_diff(struct clk_rate_request *req,
 354                                          struct clk_hw *parent,
 355                                          unsigned long parent_rate,
 356                                          long *best_rate,
 357                                          long *best_diff,
 358                                          u32 div)
 359 {
 360         unsigned long tmp_rate, tmp_diff;
 361
 362         if (div == MASTER_PRES_MAX)
 363                 tmp_rate = parent_rate / 3;
 364         else
 365                 tmp_rate = parent_rate >> div;
 366
 367         tmp_diff = abs(req->rate - tmp_rate);
 368
 369         if (*best_diff < 0 || *best_diff >= tmp_diff) {
 370                 *best_rate = tmp_rate;
 371                 *best_diff = tmp_diff;
 372                 req->best_parent_rate = parent_rate;
 373                 req->best_parent_hw = parent;
 374         }
 375 }
 376
 377 static int clk_master_pres_determine_rate(struct clk_hw *hw,
 378                                           struct clk_rate_request *req)
 379 {
 380         struct clk_master *master = to_clk_master(hw);
 381         struct clk_rate_request req_parent = *req;
 382         const struct clk_master_characteristics *characteristics =
 383                                                         master->characteristics;
 384         struct clk_hw *parent;
 385         long best_rate = LONG_MIN, best_diff = LONG_MIN;
 386         u32 pres;
 387         int i;
 388
 389         if (master->chg_pid < 0)
 390                 return -EOPNOTSUPP;
 391
 392         parent = clk_hw_get_parent_by_index(hw, master->chg_pid);
 393         if (!parent)
 394                 return -EOPNOTSUPP;
 395
 396         for (i = 0; i <= MASTER_PRES_MAX; i++) {
 397                 if (characteristics->have_div3_pres && i == MASTER_PRES_MAX)
 398                         pres = 3;
 399                 else
 400                         pres = 1 << i;
 401
 402                 req_parent.rate = req->rate * pres;
 403                 if (__clk_determine_rate(parent, &req_parent))
 404                         continue;
 405
 406                 clk_sama7g5_master_best_diff(req, parent, req_parent.rate,
 407                                              &best_diff, &best_rate, pres);
 408                 if (!best_diff)
 409                         break;
 410         }
 411
 412         return 0;
 413 }
 414
 415 static int clk_master_pres_set_rate(struct clk_hw *hw, unsigned long rate,
 416                                     unsigned long parent_rate)
 417 {
 418         struct clk_master *master = to_clk_master(hw);
 419         unsigned long flags;
 420         unsigned int pres, mckr, tmp;
 421         int ret;
 422
 423         pres = DIV_ROUND_CLOSEST(parent_rate, rate);
 424         if (pres > MASTER_PRES_MAX)
 425                 return -EINVAL;
 426
 427         else if (pres == 3)
 428                 pres = MASTER_PRES_MAX;
 429         else if (pres)
 430                 pres = ffs(pres) - 1;
 431
 432         spin_lock_irqsave(master->lock, flags);
 433         ret = regmap_read(master->regmap, master->layout->offset, &mckr);
 434         if (ret)
 435                 goto unlock;
 436
 437         mckr &= master->layout->mask;
 438         tmp = (mckr >> master->layout->pres_shift) & MASTER_PRES_MASK;
 439         if (pres == tmp)
 440                 goto unlock;
 441
 442         mckr &= ~(MASTER_PRES_MASK << master->layout->pres_shift);
 443         mckr |= (pres << master->layout->pres_shift);
 444         ret = regmap_write(master->regmap, master->layout->offset, mckr);
 445         if (ret)
 446                 goto unlock;
 447
 448         while (!clk_master_ready(master))
 449                 cpu_relax();
 450 unlock:
 451         spin_unlock_irqrestore(master->lock, flags);
 452
 453         return ret;
 454 }
 455
 456 static unsigned long clk_master_pres_recalc_rate(struct clk_hw *hw,
 457                                                  unsigned long parent_rate)
 458 {
 459         struct clk_master *master = to_clk_master(hw);
 460         const struct clk_master_characteristics *characteristics =
 461                                                 master->characteristics;
 462         unsigned long flags;
 463         unsigned int val, pres;
 464
 465         spin_lock_irqsave(master->lock, flags);
 466         regmap_read(master->regmap, master->layout->offset, &val);
 467         spin_unlock_irqrestore(master->lock, flags);
 468
 469         val &= master->layout->mask;
 470         pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK;
 471         if (pres == MASTER_PRES_MAX && characteristics->have_div3_pres)
 472                 pres = 3;
 473         else
 474                 pres = (1 << pres);
 475
 476         return DIV_ROUND_CLOSEST_ULL(parent_rate, pres);
 477 }
 478
 479 static u8 clk_master_pres_get_parent(struct clk_hw *hw)
 480 {
 481         struct clk_master *master = to_clk_master(hw);
 482         unsigned long flags;
 483         unsigned int mckr;
 484
 485         spin_lock_irqsave(master->lock, flags);
 486         regmap_read(master->regmap, master->layout->offset, &mckr);
 487         spin_unlock_irqrestore(master->lock, flags);
 488
 489         mckr &= master->layout->mask;
 490
 491         return mckr & AT91_PMC_CSS;
 492 }
 493
 494 static int clk_master_pres_save_context(struct clk_hw *hw)
 495 {
 496         struct clk_master *master = to_clk_master(hw);
 497         struct clk_hw *parent_hw = clk_hw_get_parent(hw);
 498         unsigned long flags;
 499         unsigned int val, pres;
 500
 501         spin_lock_irqsave(master->lock, flags);
 502         regmap_read(master->regmap, master->layout->offset, &val);
 503         spin_unlock_irqrestore(master->lock, flags);
 504
 505         val &= master->layout->mask;
 506         pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK;
 507         if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres)
 508                 pres = 3;
 509         else
 510                 pres = (1 << pres);
 511
 512         master->pms.parent = val & AT91_PMC_CSS;
 513         master->pms.parent_rate = clk_hw_get_rate(parent_hw);
 514         master->pms.rate = DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres);
 515
 516         return 0;
 517 }
 518
 519 static void clk_master_pres_restore_context(struct clk_hw *hw)
 520 {
 521         struct clk_master *master = to_clk_master(hw);
 522         unsigned long flags;
 523         unsigned int val, pres;
 524
 525         spin_lock_irqsave(master->lock, flags);
 526         regmap_read(master->regmap, master->layout->offset, &val);
 527         spin_unlock_irqrestore(master->lock, flags);
 528
 529         val &= master->layout->mask;
 530         pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK;
 531         if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres)
 532                 pres = 3;
 533         else
 534                 pres = (1 << pres);
 535
 536         if (master->pms.rate !=
 537             DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres) ||
 538             (master->pms.parent != (val & AT91_PMC_CSS)))
 539                 pr_warn("MCKR PRES was not configured properly by firmware!\n");
 540 }
 541
 542 static void clk_master_pres_restore_context_chg(struct clk_hw *hw)
 543 {
 544         struct clk_master *master = to_clk_master(hw);
 545
 546         clk_master_pres_set_rate(hw, master->pms.rate, master->pms.parent_rate);
 547 }
 548
 549 static const struct clk_ops master_pres_ops = {
 550         .prepare = clk_master_prepare,
 551         .is_prepared = clk_master_is_prepared,
 552         .recalc_rate = clk_master_pres_recalc_rate,
 553         .get_parent = clk_master_pres_get_parent,
 554         .save_context = clk_master_pres_save_context,
 555         .restore_context = clk_master_pres_restore_context,
 556 };
 557
 558 static const struct clk_ops master_pres_ops_chg = {
 559         .prepare = clk_master_prepare,
 560         .is_prepared = clk_master_is_prepared,
 561         .determine_rate = clk_master_pres_determine_rate,
 562         .recalc_rate = clk_master_pres_recalc_rate,
 563         .get_parent = clk_master_pres_get_parent,
 564         .set_rate = clk_master_pres_set_rate,
 565         .save_context = clk_master_pres_save_context,
 566         .restore_context = clk_master_pres_restore_context_chg,
 567 };
 568
 569 static struct clk_hw * __init
 570 at91_clk_register_master_internal(struct regmap *regmap,
 571                 const char *name, int num_parents,
 572                 const char **parent_names,
 573                 const struct clk_master_layout *layout,
 574                 const struct clk_master_characteristics *characteristics,
 575                 const struct clk_ops *ops, spinlock_t *lock, u32 flags,
 576                 int chg_pid)
 577 {
 578         struct clk_master *master;
 579         struct clk_init_data init;
 580         struct clk_hw *hw;
 581         unsigned int mckr;
 582         unsigned long irqflags;
 583         int ret;
 584
 585         if (!name || !num_parents || !parent_names || !lock)
 586                 return ERR_PTR(-EINVAL);
 587
 588         master = kzalloc(sizeof(*master), GFP_KERNEL);
 589         if (!master)
 590                 return ERR_PTR(-ENOMEM);
 591
 592         init.name = name;
 593         init.ops = ops;
 594         init.parent_names = parent_names;
 595         init.num_parents = num_parents;
 596         init.flags = flags;
 597
 598         master->hw.init = &init;
 599         master->layout = layout;
 600         master->characteristics = characteristics;
 601         master->regmap = regmap;
 602         master->chg_pid = chg_pid;
 603         master->lock = lock;
 604
 605         if (ops == &master_div_ops_chg) {
 606                 spin_lock_irqsave(master->lock, irqflags);
 607                 regmap_read(master->regmap, master->layout->offset, &mckr);
 608                 spin_unlock_irqrestore(master->lock, irqflags);
 609
 610                 mckr &= layout->mask;
 611                 mckr = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
 612                 master->div = characteristics->divisors[mckr];
 613         }
 614
 615         hw = &master->hw;
 616         ret = clk_hw_register(NULL, &master->hw);
 617         if (ret) {
 618                 kfree(master);
 619                 hw = ERR_PTR(ret);
 620         }
 621
 622         return hw;
 623 }
 624
 625 struct clk_hw * __init
 626 at91_clk_register_master_pres(struct regmap *regmap,
 627                 const char *name, int num_parents,
 628                 const char **parent_names,
 629                 const struct clk_master_layout *layout,
 630                 const struct clk_master_characteristics *characteristics,
 631                 spinlock_t *lock, u32 flags, int chg_pid)
 632 {
 633         const struct clk_ops *ops;
 634
 635         if (flags & CLK_SET_RATE_GATE)
 636                 ops = &master_pres_ops;
 637         else
 638                 ops = &master_pres_ops_chg;
 639
 640         return at91_clk_register_master_internal(regmap, name, num_parents,
 641                                                  parent_names, layout,
 642                                                  characteristics, ops,
 643                                                  lock, flags, chg_pid);
 644 }
 645
 646 struct clk_hw * __init
 647 at91_clk_register_master_div(struct regmap *regmap,
 648                 const char *name, const char *parent_name,
 649                 const struct clk_master_layout *layout,
 650                 const struct clk_master_characteristics *characteristics,
 651                 spinlock_t *lock, u32 flags, u32 safe_div)
 652 {
 653         const struct clk_ops *ops;
 654         struct clk_hw *hw;
 655
 656         if (flags & CLK_SET_RATE_GATE)
 657                 ops = &master_div_ops;
 658         else
 659                 ops = &master_div_ops_chg;
 660
 661         hw = at91_clk_register_master_internal(regmap, name, 1,
 662                                                &parent_name, layout,
 663                                                characteristics, ops,
 664                                                lock, flags, -EINVAL);
 665
 666         if (!IS_ERR(hw) && safe_div) {
 667                 master_div = to_clk_master(hw);
 668                 master_div->safe_div = safe_div;
 669                 clk_notifier_register(hw->clk,
 670                                       &clk_master_div_notifier);
 671         }
 672
 673         return hw;
 674 }
 675
 676 static unsigned long
 677 clk_sama7g5_master_recalc_rate(struct clk_hw *hw,
 678                                unsigned long parent_rate)
 679 {
 680         struct clk_master *master = to_clk_master(hw);
 681
 682         return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div));
 683 }
 684
 685 static int clk_sama7g5_master_determine_rate(struct clk_hw *hw,
 686                                              struct clk_rate_request *req)
 687 {
 688         struct clk_master *master = to_clk_master(hw);
 689         struct clk_rate_request req_parent = *req;
 690         struct clk_hw *parent;
 691         long best_rate = LONG_MIN, best_diff = LONG_MIN;
 692         unsigned long parent_rate;
 693         unsigned int div, i;
 694
 695         /* First: check the dividers of MCR. */
 696         for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
 697                 parent = clk_hw_get_parent_by_index(hw, i);
 698                 if (!parent)
 699                         continue;
 700
 701                 parent_rate = clk_hw_get_rate(parent);
 702                 if (!parent_rate)
 703                         continue;
 704
 705                 for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
 706                         clk_sama7g5_master_best_diff(req, parent, parent_rate,
 707                                                      &best_rate, &best_diff,
 708                                                      div);
 709                         if (!best_diff)
 710                                 break;
 711                 }
 712
 713                 if (!best_diff)
 714                         break;
 715         }
 716
 717         /* Second: try to request rate form changeable parent. */
 718         if (master->chg_pid < 0)
 719                 goto end;
 720
 721         parent = clk_hw_get_parent_by_index(hw, master->chg_pid);
 722         if (!parent)
 723                 goto end;
 724
 725         for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
 726                 if (div == MASTER_PRES_MAX)
 727                         req_parent.rate = req->rate * 3;
 728                 else
 729                         req_parent.rate = req->rate << div;
 730
 731                 if (__clk_determine_rate(parent, &req_parent))
 732                         continue;
 733
 734                 clk_sama7g5_master_best_diff(req, parent, req_parent.rate,
 735                                              &best_rate, &best_diff, div);
 736
 737                 if (!best_diff)
 738                         break;
 739         }
 740
 741 end:
 742         pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
 743                  __func__, best_rate,
 744                  __clk_get_name((req->best_parent_hw)->clk),
 745                 req->best_parent_rate);
 746
 747         if (best_rate < 0)
 748                 return -EINVAL;
 749
 750         req->rate = best_rate;
 751
 752         return 0;
 753 }
 754
 755 static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw)
 756 {
 757         struct clk_master *master = to_clk_master(hw);
 758         unsigned long flags;
 759         u8 index;
 760
 761         spin_lock_irqsave(master->lock, flags);
 762         index = clk_mux_val_to_index(&master->hw, master->mux_table, 0,
 763                                      master->parent);
 764         spin_unlock_irqrestore(master->lock, flags);
 765
 766         return index;
 767 }
 768
 769 static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index)
 770 {
 771         struct clk_master *master = to_clk_master(hw);
 772         unsigned long flags;
 773
 774         if (index >= clk_hw_get_num_parents(hw))
 775                 return -EINVAL;
 776
 777         spin_lock_irqsave(master->lock, flags);
 778         master->parent = clk_mux_index_to_val(master->mux_table, 0, index);
 779         spin_unlock_irqrestore(master->lock, flags);
 780
 781         return 0;
 782 }
 783
 784 static void clk_sama7g5_master_set(struct clk_master *master,
 785                                    unsigned int status)
 786 {
 787         unsigned long flags;
 788         unsigned int val, cparent;
 789         unsigned int enable = status ? AT91_PMC_MCR_V2_EN : 0;
 790         unsigned int parent = master->parent << PMC_MCR_CSS_SHIFT;
 791         unsigned int div = master->div << MASTER_DIV_SHIFT;
 792
 793         spin_lock_irqsave(master->lock, flags);
 794
 795         regmap_write(master->regmap, AT91_PMC_MCR_V2,
 796                      AT91_PMC_MCR_V2_ID(master->id));
 797         regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
 798         regmap_update_bits(master->regmap, AT91_PMC_MCR_V2,
 799                            enable | AT91_PMC_MCR_V2_CSS | AT91_PMC_MCR_V2_DIV |
 800                            AT91_PMC_MCR_V2_CMD | AT91_PMC_MCR_V2_ID_MSK,
 801                            enable | parent | div | AT91_PMC_MCR_V2_CMD |
 802                            AT91_PMC_MCR_V2_ID(master->id));
 803
 804         cparent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT;
 805
 806         /* Wait here only if parent is being changed. */
 807         while ((cparent != master->parent) && !clk_master_ready(master))
 808                 cpu_relax();
 809
 810         spin_unlock_irqrestore(master->lock, flags);
 811 }
 812
 813 static int clk_sama7g5_master_enable(struct clk_hw *hw)
 814 {
 815         struct clk_master *master = to_clk_master(hw);
 816
 817         clk_sama7g5_master_set(master, 1);
 818
 819         return 0;
 820 }
 821
 822 static void clk_sama7g5_master_disable(struct clk_hw *hw)
 823 {
 824         struct clk_master *master = to_clk_master(hw);
 825         unsigned long flags;
 826
 827         spin_lock_irqsave(master->lock, flags);
 828
 829         regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);
 830         regmap_update_bits(master->regmap, AT91_PMC_MCR_V2,
 831                            AT91_PMC_MCR_V2_EN | AT91_PMC_MCR_V2_CMD |
 832                            AT91_PMC_MCR_V2_ID_MSK,
 833                            AT91_PMC_MCR_V2_CMD |
 834                            AT91_PMC_MCR_V2_ID(master->id));
 835
 836         spin_unlock_irqrestore(master->lock, flags);
 837 }
 838
 839 static int clk_sama7g5_master_is_enabled(struct clk_hw *hw)
 840 {
 841         struct clk_master *master = to_clk_master(hw);
 842         unsigned long flags;
 843         unsigned int val;
 844
 845         spin_lock_irqsave(master->lock, flags);
 846
 847         regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);
 848         regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
 849
 850         spin_unlock_irqrestore(master->lock, flags);
 851
 852         return !!(val & AT91_PMC_MCR_V2_EN);
 853 }
 854
 855 static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate,
 856                                        unsigned long parent_rate)
 857 {
 858         struct clk_master *master = to_clk_master(hw);
 859         unsigned long div, flags;
 860
 861         div = DIV_ROUND_CLOSEST(parent_rate, rate);
 862         if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1)))
 863                 return -EINVAL;
 864
 865         if (div == 3)
 866                 div = MASTER_PRES_MAX;
 867         else if (div)
 868                 div = ffs(div) - 1;
 869
 870         spin_lock_irqsave(master->lock, flags);
 871         master->div = div;
 872         spin_unlock_irqrestore(master->lock, flags);
 873
 874         return 0;
 875 }
 876
 877 static int clk_sama7g5_master_save_context(struct clk_hw *hw)
 878 {
 879         struct clk_master *master = to_clk_master(hw);
 880
 881         master->pms.status = clk_sama7g5_master_is_enabled(hw);
 882
 883         return 0;
 884 }
 885
 886 static void clk_sama7g5_master_restore_context(struct clk_hw *hw)
 887 {
 888         struct clk_master *master = to_clk_master(hw);
 889
 890         if (master->pms.status)
 891                 clk_sama7g5_master_set(master, master->pms.status);
 892 }
 893
 894 static const struct clk_ops sama7g5_master_ops = {
 895         .enable = clk_sama7g5_master_enable,
 896         .disable = clk_sama7g5_master_disable,
 897         .is_enabled = clk_sama7g5_master_is_enabled,
 898         .recalc_rate = clk_sama7g5_master_recalc_rate,
 899         .determine_rate = clk_sama7g5_master_determine_rate,
 900         .set_rate = clk_sama7g5_master_set_rate,
 901         .get_parent = clk_sama7g5_master_get_parent,
 902         .set_parent = clk_sama7g5_master_set_parent,
 903         .save_context = clk_sama7g5_master_save_context,
 904         .restore_context = clk_sama7g5_master_restore_context,
 905 };
 906
 907 struct clk_hw * __init
 908 at91_clk_sama7g5_register_master(struct regmap *regmap,
 909                                  const char *name, int num_parents,
 910                                  const char **parent_names,
 911                                  u32 *mux_table,
 912                                  spinlock_t *lock, u8 id,
 913                                  bool critical, int chg_pid)
 914 {
 915         struct clk_master *master;
 916         struct clk_hw *hw;
 917         struct clk_init_data init;
 918         unsigned long flags;
 919         unsigned int val;
 920         int ret;
 921
 922         if (!name || !num_parents || !parent_names || !mux_table ||
 923             !lock || id > MASTER_MAX_ID)
 924                 return ERR_PTR(-EINVAL);
 925
 926         master = kzalloc(sizeof(*master), GFP_KERNEL);
 927         if (!master)
 928                 return ERR_PTR(-ENOMEM);
 929
 930         init.name = name;
 931         init.ops = &sama7g5_master_ops;
 932         init.parent_names = parent_names;
 933         init.num_parents = num_parents;
 934         init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
 935         if (chg_pid >= 0)
 936                 init.flags |= CLK_SET_RATE_PARENT;
 937         if (critical)
 938                 init.flags |= CLK_IS_CRITICAL;
 939
 940         master->hw.init = &init;
 941         master->regmap = regmap;
 942         master->id = id;
 943         master->chg_pid = chg_pid;
 944         master->lock = lock;
 945         master->mux_table = mux_table;
 946
 947         spin_lock_irqsave(master->lock, flags);
 948         regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);
 949         regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
 950         master->parent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT;
 951         master->div = (val & AT91_PMC_MCR_V2_DIV) >> MASTER_DIV_SHIFT;
 952         spin_unlock_irqrestore(master->lock, flags);
 953
 954         hw = &master->hw;
 955         ret = clk_hw_register(NULL, &master->hw);
 956         if (ret) {
 957                 kfree(master);
 958                 hw = ERR_PTR(ret);
 959         }
 960
 961         return hw;
 962 }
 963
 964 const struct clk_master_layout at91rm9200_master_layout = {
 965         .mask = 0x31F,
 966         .pres_shift = 2,
 967         .offset = AT91_PMC_MCKR,
 968 };
 969
 970 const struct clk_master_layout at91sam9x5_master_layout = {
 971         .mask = 0x373,
 972         .pres_shift = 4,
 973         .offset = AT91_PMC_MCKR,
 974 };