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