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 9 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 unsigned long clk_master_pres_recalc_rate(struct clk_hw *hw, 378 unsigned long parent_rate) 379 { 380 struct clk_master *master = to_clk_master(hw); 381 const struct clk_master_characteristics *characteristics = 382 master->characteristics; 383 unsigned long flags; 384 unsigned int val, pres; 385 386 spin_lock_irqsave(master->lock, flags); 387 regmap_read(master->regmap, master->layout->offset, &val); 388 spin_unlock_irqrestore(master->lock, flags); 389 390 val &= master->layout->mask; 391 pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; 392 if (pres == MASTER_PRES_MAX && characteristics->have_div3_pres) 393 pres = 3; 394 else 395 pres = (1 << pres); 396 397 return DIV_ROUND_CLOSEST_ULL(parent_rate, pres); 398 } 399 400 static u8 clk_master_pres_get_parent(struct clk_hw *hw) 401 { 402 struct clk_master *master = to_clk_master(hw); 403 unsigned long flags; 404 unsigned int mckr; 405 406 spin_lock_irqsave(master->lock, flags); 407 regmap_read(master->regmap, master->layout->offset, &mckr); 408 spin_unlock_irqrestore(master->lock, flags); 409 410 mckr &= master->layout->mask; 411 412 return mckr & AT91_PMC_CSS; 413 } 414 415 static int clk_master_pres_save_context(struct clk_hw *hw) 416 { 417 struct clk_master *master = to_clk_master(hw); 418 struct clk_hw *parent_hw = clk_hw_get_parent(hw); 419 unsigned long flags; 420 unsigned int val, pres; 421 422 spin_lock_irqsave(master->lock, flags); 423 regmap_read(master->regmap, master->layout->offset, &val); 424 spin_unlock_irqrestore(master->lock, flags); 425 426 val &= master->layout->mask; 427 pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; 428 if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres) 429 pres = 3; 430 else 431 pres = (1 << pres); 432 433 master->pms.parent = val & AT91_PMC_CSS; 434 master->pms.parent_rate = clk_hw_get_rate(parent_hw); 435 master->pms.rate = DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres); 436 437 return 0; 438 } 439 440 static void clk_master_pres_restore_context(struct clk_hw *hw) 441 { 442 struct clk_master *master = to_clk_master(hw); 443 unsigned long flags; 444 unsigned int val, pres; 445 446 spin_lock_irqsave(master->lock, flags); 447 regmap_read(master->regmap, master->layout->offset, &val); 448 spin_unlock_irqrestore(master->lock, flags); 449 450 val &= master->layout->mask; 451 pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; 452 if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres) 453 pres = 3; 454 else 455 pres = (1 << pres); 456 457 if (master->pms.rate != 458 DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres) || 459 (master->pms.parent != (val & AT91_PMC_CSS))) 460 pr_warn("MCKR PRES was not configured properly by firmware!\n"); 461 } 462 463 static const struct clk_ops master_pres_ops = { 464 .prepare = clk_master_prepare, 465 .is_prepared = clk_master_is_prepared, 466 .recalc_rate = clk_master_pres_recalc_rate, 467 .get_parent = clk_master_pres_get_parent, 468 .save_context = clk_master_pres_save_context, 469 .restore_context = clk_master_pres_restore_context, 470 }; 471 472 static struct clk_hw * __init 473 at91_clk_register_master_internal(struct regmap *regmap, 474 const char *name, int num_parents, 475 const char **parent_names, 476 struct clk_hw **parent_hws, 477 const struct clk_master_layout *layout, 478 const struct clk_master_characteristics *characteristics, 479 const struct clk_ops *ops, spinlock_t *lock, u32 flags) 480 { 481 struct clk_master *master; 482 struct clk_init_data init = {}; 483 struct clk_hw *hw; 484 unsigned int mckr; 485 unsigned long irqflags; 486 int ret; 487 488 if (!name || !num_parents || !(parent_names || parent_hws) || !lock) 489 return ERR_PTR(-EINVAL); 490 491 master = kzalloc(sizeof(*master), GFP_KERNEL); 492 if (!master) 493 return ERR_PTR(-ENOMEM); 494 495 init.name = name; 496 init.ops = ops; 497 if (parent_hws) 498 init.parent_hws = (const struct clk_hw **)parent_hws; 499 else 500 init.parent_names = parent_names; 501 init.num_parents = num_parents; 502 init.flags = flags; 503 504 master->hw.init = &init; 505 master->layout = layout; 506 master->characteristics = characteristics; 507 master->regmap = regmap; 508 master->lock = lock; 509 510 if (ops == &master_div_ops_chg) { 511 spin_lock_irqsave(master->lock, irqflags); 512 regmap_read(master->regmap, master->layout->offset, &mckr); 513 spin_unlock_irqrestore(master->lock, irqflags); 514 515 mckr &= layout->mask; 516 mckr = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; 517 master->div = characteristics->divisors[mckr]; 518 } 519 520 hw = &master->hw; 521 ret = clk_hw_register(NULL, &master->hw); 522 if (ret) { 523 kfree(master); 524 hw = ERR_PTR(ret); 525 } 526 527 return hw; 528 } 529 530 struct clk_hw * __init 531 at91_clk_register_master_pres(struct regmap *regmap, 532 const char *name, int num_parents, 533 const char **parent_names, 534 struct clk_hw **parent_hws, 535 const struct clk_master_layout *layout, 536 const struct clk_master_characteristics *characteristics, 537 spinlock_t *lock) 538 { 539 return at91_clk_register_master_internal(regmap, name, num_parents, 540 parent_names, parent_hws, layout, 541 characteristics, 542 &master_pres_ops, 543 lock, CLK_SET_RATE_GATE); 544 } 545 546 struct clk_hw * __init 547 at91_clk_register_master_div(struct regmap *regmap, 548 const char *name, const char *parent_name, 549 struct clk_hw *parent_hw, const struct clk_master_layout *layout, 550 const struct clk_master_characteristics *characteristics, 551 spinlock_t *lock, u32 flags, u32 safe_div) 552 { 553 const struct clk_ops *ops; 554 struct clk_hw *hw; 555 556 if (flags & CLK_SET_RATE_GATE) 557 ops = &master_div_ops; 558 else 559 ops = &master_div_ops_chg; 560 561 hw = at91_clk_register_master_internal(regmap, name, 1, 562 parent_name ? &parent_name : NULL, 563 parent_hw ? &parent_hw : NULL, layout, 564 characteristics, ops, 565 lock, flags); 566 567 if (!IS_ERR(hw) && safe_div) { 568 master_div = to_clk_master(hw); 569 master_div->safe_div = safe_div; 570 clk_notifier_register(hw->clk, 571 &clk_master_div_notifier); 572 } 573 574 return hw; 575 } 576 577 static unsigned long 578 clk_sama7g5_master_recalc_rate(struct clk_hw *hw, 579 unsigned long parent_rate) 580 { 581 struct clk_master *master = to_clk_master(hw); 582 583 if (master->div == MASTER_PRES_MAX) 584 return DIV_ROUND_CLOSEST_ULL(parent_rate, 3); 585 586 return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div)); 587 } 588 589 static int clk_sama7g5_master_determine_rate(struct clk_hw *hw, 590 struct clk_rate_request *req) 591 { 592 struct clk_master *master = to_clk_master(hw); 593 struct clk_hw *parent; 594 long best_rate = LONG_MIN, best_diff = LONG_MIN; 595 unsigned long parent_rate; 596 unsigned int div, i; 597 598 /* First: check the dividers of MCR. */ 599 for (i = 0; i < clk_hw_get_num_parents(hw); i++) { 600 parent = clk_hw_get_parent_by_index(hw, i); 601 if (!parent) 602 continue; 603 604 parent_rate = clk_hw_get_rate(parent); 605 if (!parent_rate) 606 continue; 607 608 for (div = 0; div < MASTER_PRES_MAX + 1; div++) { 609 clk_sama7g5_master_best_diff(req, parent, parent_rate, 610 &best_rate, &best_diff, 611 div); 612 if (!best_diff) 613 break; 614 } 615 616 if (!best_diff) 617 break; 618 } 619 620 /* Second: try to request rate form changeable parent. */ 621 if (master->chg_pid < 0) 622 goto end; 623 624 parent = clk_hw_get_parent_by_index(hw, master->chg_pid); 625 if (!parent) 626 goto end; 627 628 for (div = 0; div < MASTER_PRES_MAX + 1; div++) { 629 struct clk_rate_request req_parent; 630 unsigned long req_rate; 631 632 if (div == MASTER_PRES_MAX) 633 req_rate = req->rate * 3; 634 else 635 req_rate = req->rate << div; 636 637 clk_hw_forward_rate_request(hw, req, parent, &req_parent, req_rate); 638 if (__clk_determine_rate(parent, &req_parent)) 639 continue; 640 641 clk_sama7g5_master_best_diff(req, parent, req_parent.rate, 642 &best_rate, &best_diff, div); 643 644 if (!best_diff) 645 break; 646 } 647 648 end: 649 pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n", 650 __func__, best_rate, 651 __clk_get_name((req->best_parent_hw)->clk), 652 req->best_parent_rate); 653 654 if (best_rate < 0) 655 return -EINVAL; 656 657 req->rate = best_rate; 658 659 return 0; 660 } 661 662 static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw) 663 { 664 struct clk_master *master = to_clk_master(hw); 665 unsigned long flags; 666 u8 index; 667 668 spin_lock_irqsave(master->lock, flags); 669 index = clk_mux_val_to_index(&master->hw, master->mux_table, 0, 670 master->parent); 671 spin_unlock_irqrestore(master->lock, flags); 672 673 return index; 674 } 675 676 static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index) 677 { 678 struct clk_master *master = to_clk_master(hw); 679 unsigned long flags; 680 681 if (index >= clk_hw_get_num_parents(hw)) 682 return -EINVAL; 683 684 spin_lock_irqsave(master->lock, flags); 685 master->parent = clk_mux_index_to_val(master->mux_table, 0, index); 686 spin_unlock_irqrestore(master->lock, flags); 687 688 return 0; 689 } 690 691 static void clk_sama7g5_master_set(struct clk_master *master, 692 unsigned int status) 693 { 694 unsigned long flags; 695 unsigned int val, cparent; 696 unsigned int enable = status ? AT91_PMC_MCR_V2_EN : 0; 697 unsigned int parent = master->parent << PMC_MCR_CSS_SHIFT; 698 unsigned int div = master->div << MASTER_DIV_SHIFT; 699 700 spin_lock_irqsave(master->lock, flags); 701 702 regmap_write(master->regmap, AT91_PMC_MCR_V2, 703 AT91_PMC_MCR_V2_ID(master->id)); 704 regmap_read(master->regmap, AT91_PMC_MCR_V2, &val); 705 regmap_update_bits(master->regmap, AT91_PMC_MCR_V2, 706 enable | AT91_PMC_MCR_V2_CSS | AT91_PMC_MCR_V2_DIV | 707 AT91_PMC_MCR_V2_CMD | AT91_PMC_MCR_V2_ID_MSK, 708 enable | parent | div | AT91_PMC_MCR_V2_CMD | 709 AT91_PMC_MCR_V2_ID(master->id)); 710 711 cparent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT; 712 713 /* Wait here only if parent is being changed. */ 714 while ((cparent != master->parent) && !clk_master_ready(master)) 715 cpu_relax(); 716 717 spin_unlock_irqrestore(master->lock, flags); 718 } 719 720 static int clk_sama7g5_master_enable(struct clk_hw *hw) 721 { 722 struct clk_master *master = to_clk_master(hw); 723 724 clk_sama7g5_master_set(master, 1); 725 726 return 0; 727 } 728 729 static void clk_sama7g5_master_disable(struct clk_hw *hw) 730 { 731 struct clk_master *master = to_clk_master(hw); 732 unsigned long flags; 733 734 spin_lock_irqsave(master->lock, flags); 735 736 regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id); 737 regmap_update_bits(master->regmap, AT91_PMC_MCR_V2, 738 AT91_PMC_MCR_V2_EN | AT91_PMC_MCR_V2_CMD | 739 AT91_PMC_MCR_V2_ID_MSK, 740 AT91_PMC_MCR_V2_CMD | 741 AT91_PMC_MCR_V2_ID(master->id)); 742 743 spin_unlock_irqrestore(master->lock, flags); 744 } 745 746 static int clk_sama7g5_master_is_enabled(struct clk_hw *hw) 747 { 748 struct clk_master *master = to_clk_master(hw); 749 unsigned long flags; 750 unsigned int val; 751 752 spin_lock_irqsave(master->lock, flags); 753 754 regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id); 755 regmap_read(master->regmap, AT91_PMC_MCR_V2, &val); 756 757 spin_unlock_irqrestore(master->lock, flags); 758 759 return !!(val & AT91_PMC_MCR_V2_EN); 760 } 761 762 static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate, 763 unsigned long parent_rate) 764 { 765 struct clk_master *master = to_clk_master(hw); 766 unsigned long div, flags; 767 768 div = DIV_ROUND_CLOSEST(parent_rate, rate); 769 if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1))) 770 return -EINVAL; 771 772 if (div == 3) 773 div = MASTER_PRES_MAX; 774 else if (div) 775 div = ffs(div) - 1; 776 777 spin_lock_irqsave(master->lock, flags); 778 master->div = div; 779 spin_unlock_irqrestore(master->lock, flags); 780 781 return 0; 782 } 783 784 static int clk_sama7g5_master_save_context(struct clk_hw *hw) 785 { 786 struct clk_master *master = to_clk_master(hw); 787 788 master->pms.status = clk_sama7g5_master_is_enabled(hw); 789 790 return 0; 791 } 792 793 static void clk_sama7g5_master_restore_context(struct clk_hw *hw) 794 { 795 struct clk_master *master = to_clk_master(hw); 796 797 if (master->pms.status) 798 clk_sama7g5_master_set(master, master->pms.status); 799 } 800 801 static const struct clk_ops sama7g5_master_ops = { 802 .enable = clk_sama7g5_master_enable, 803 .disable = clk_sama7g5_master_disable, 804 .is_enabled = clk_sama7g5_master_is_enabled, 805 .recalc_rate = clk_sama7g5_master_recalc_rate, 806 .determine_rate = clk_sama7g5_master_determine_rate, 807 .set_rate = clk_sama7g5_master_set_rate, 808 .get_parent = clk_sama7g5_master_get_parent, 809 .set_parent = clk_sama7g5_master_set_parent, 810 .save_context = clk_sama7g5_master_save_context, 811 .restore_context = clk_sama7g5_master_restore_context, 812 }; 813 814 struct clk_hw * __init 815 at91_clk_sama7g5_register_master(struct regmap *regmap, 816 const char *name, int num_parents, 817 const char **parent_names, 818 struct clk_hw **parent_hws, 819 u32 *mux_table, 820 spinlock_t *lock, u8 id, 821 bool critical, int chg_pid) 822 { 823 struct clk_master *master; 824 struct clk_hw *hw; 825 struct clk_init_data init = {}; 826 unsigned long flags; 827 unsigned int val; 828 int ret; 829 830 if (!name || !num_parents || !(parent_names || parent_hws) || !mux_table || 831 !lock || id > MASTER_MAX_ID) 832 return ERR_PTR(-EINVAL); 833 834 master = kzalloc(sizeof(*master), GFP_KERNEL); 835 if (!master) 836 return ERR_PTR(-ENOMEM); 837 838 init.name = name; 839 init.ops = &sama7g5_master_ops; 840 if (parent_hws) 841 init.parent_hws = (const struct clk_hw **)parent_hws; 842 else 843 init.parent_names = parent_names; 844 init.num_parents = num_parents; 845 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; 846 if (chg_pid >= 0) 847 init.flags |= CLK_SET_RATE_PARENT; 848 if (critical) 849 init.flags |= CLK_IS_CRITICAL; 850 851 master->hw.init = &init; 852 master->regmap = regmap; 853 master->id = id; 854 master->chg_pid = chg_pid; 855 master->lock = lock; 856 master->mux_table = mux_table; 857 858 spin_lock_irqsave(master->lock, flags); 859 regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id); 860 regmap_read(master->regmap, AT91_PMC_MCR_V2, &val); 861 master->parent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT; 862 master->div = (val & AT91_PMC_MCR_V2_DIV) >> MASTER_DIV_SHIFT; 863 spin_unlock_irqrestore(master->lock, flags); 864 865 hw = &master->hw; 866 ret = clk_hw_register(NULL, &master->hw); 867 if (ret) { 868 kfree(master); 869 hw = ERR_PTR(ret); 870 } 871 872 return hw; 873 } 874 875 const struct clk_master_layout at91rm9200_master_layout = { 876 .mask = 0x31F, 877 .pres_shift = 2, 878 .offset = AT91_PMC_MCKR, 879 }; 880 881 const struct clk_master_layout at91sam9x5_master_layout = { 882 .mask = 0x373, 883 .pres_shift = 4, 884 .offset = AT91_PMC_MCKR, 885 }; 886