883 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*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 0x717#define MASTER_PRES_MAX MASTER_PRES_MASK18#define MASTER_DIV_SHIFT 819#define MASTER_DIV_MASK 0x720 21#define PMC_MCR_CSS_SHIFT (16)22 23#define MASTER_MAX_ID 424 25#define to_clk_master(hw) container_of(hw, struct clk_master, hw)26 27struct 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. */44static struct clk_master *master_div;45 46static 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 56static 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 71static 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 84static 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 113static 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 134static 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 153static 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. */162static 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 217static 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 225static 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 240static 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 248static 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 do265 * this we set a safe divider (the underclocking is not of266 * interest as we can go as low as 32KHz). The relation267 * b/w this clock and its parents are as follows:268 *269 * FRAC PLL -> DIV PLL -> MCK DIV270 *271 * With the proper safe divider we should be good even with FRAC272 * 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 maximum293 * 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 343unlock:344 spin_unlock_irqrestore(master_div->lock, flags);345 346 return ret;347}348 349static struct notifier_block clk_master_div_notifier = {350 .notifier_call = clk_master_div_notifier_fn,351};352 353static 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 else365 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 377static 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 else395 pres = (1 << pres);396 397 return DIV_ROUND_CLOSEST_ULL(parent_rate, pres);398}399 400static 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 415static 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 else431 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 440static 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 else455 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 463static 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 472static struct clk_hw * __init473at91_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 else500 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 530struct clk_hw * __init531at91_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 546struct clk_hw * __init547at91_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 else559 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 577static unsigned long578clk_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 return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div));584}585 586static int clk_sama7g5_master_determine_rate(struct clk_hw *hw,587 struct clk_rate_request *req)588{589 struct clk_master *master = to_clk_master(hw);590 struct clk_hw *parent;591 long best_rate = LONG_MIN, best_diff = LONG_MIN;592 unsigned long parent_rate;593 unsigned int div, i;594 595 /* First: check the dividers of MCR. */596 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {597 parent = clk_hw_get_parent_by_index(hw, i);598 if (!parent)599 continue;600 601 parent_rate = clk_hw_get_rate(parent);602 if (!parent_rate)603 continue;604 605 for (div = 0; div < MASTER_PRES_MAX + 1; div++) {606 clk_sama7g5_master_best_diff(req, parent, parent_rate,607 &best_rate, &best_diff,608 div);609 if (!best_diff)610 break;611 }612 613 if (!best_diff)614 break;615 }616 617 /* Second: try to request rate form changeable parent. */618 if (master->chg_pid < 0)619 goto end;620 621 parent = clk_hw_get_parent_by_index(hw, master->chg_pid);622 if (!parent)623 goto end;624 625 for (div = 0; div < MASTER_PRES_MAX + 1; div++) {626 struct clk_rate_request req_parent;627 unsigned long req_rate;628 629 if (div == MASTER_PRES_MAX)630 req_rate = req->rate * 3;631 else632 req_rate = req->rate << div;633 634 clk_hw_forward_rate_request(hw, req, parent, &req_parent, req_rate);635 if (__clk_determine_rate(parent, &req_parent))636 continue;637 638 clk_sama7g5_master_best_diff(req, parent, req_parent.rate,639 &best_rate, &best_diff, div);640 641 if (!best_diff)642 break;643 }644 645end:646 pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",647 __func__, best_rate,648 __clk_get_name((req->best_parent_hw)->clk),649 req->best_parent_rate);650 651 if (best_rate < 0)652 return -EINVAL;653 654 req->rate = best_rate;655 656 return 0;657}658 659static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw)660{661 struct clk_master *master = to_clk_master(hw);662 unsigned long flags;663 u8 index;664 665 spin_lock_irqsave(master->lock, flags);666 index = clk_mux_val_to_index(&master->hw, master->mux_table, 0,667 master->parent);668 spin_unlock_irqrestore(master->lock, flags);669 670 return index;671}672 673static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index)674{675 struct clk_master *master = to_clk_master(hw);676 unsigned long flags;677 678 if (index >= clk_hw_get_num_parents(hw))679 return -EINVAL;680 681 spin_lock_irqsave(master->lock, flags);682 master->parent = clk_mux_index_to_val(master->mux_table, 0, index);683 spin_unlock_irqrestore(master->lock, flags);684 685 return 0;686}687 688static void clk_sama7g5_master_set(struct clk_master *master,689 unsigned int status)690{691 unsigned long flags;692 unsigned int val, cparent;693 unsigned int enable = status ? AT91_PMC_MCR_V2_EN : 0;694 unsigned int parent = master->parent << PMC_MCR_CSS_SHIFT;695 unsigned int div = master->div << MASTER_DIV_SHIFT;696 697 spin_lock_irqsave(master->lock, flags);698 699 regmap_write(master->regmap, AT91_PMC_MCR_V2,700 AT91_PMC_MCR_V2_ID(master->id));701 regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);702 regmap_update_bits(master->regmap, AT91_PMC_MCR_V2,703 enable | AT91_PMC_MCR_V2_CSS | AT91_PMC_MCR_V2_DIV |704 AT91_PMC_MCR_V2_CMD | AT91_PMC_MCR_V2_ID_MSK,705 enable | parent | div | AT91_PMC_MCR_V2_CMD |706 AT91_PMC_MCR_V2_ID(master->id));707 708 cparent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT;709 710 /* Wait here only if parent is being changed. */711 while ((cparent != master->parent) && !clk_master_ready(master))712 cpu_relax();713 714 spin_unlock_irqrestore(master->lock, flags);715}716 717static int clk_sama7g5_master_enable(struct clk_hw *hw)718{719 struct clk_master *master = to_clk_master(hw);720 721 clk_sama7g5_master_set(master, 1);722 723 return 0;724}725 726static void clk_sama7g5_master_disable(struct clk_hw *hw)727{728 struct clk_master *master = to_clk_master(hw);729 unsigned long flags;730 731 spin_lock_irqsave(master->lock, flags);732 733 regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);734 regmap_update_bits(master->regmap, AT91_PMC_MCR_V2,735 AT91_PMC_MCR_V2_EN | AT91_PMC_MCR_V2_CMD |736 AT91_PMC_MCR_V2_ID_MSK,737 AT91_PMC_MCR_V2_CMD |738 AT91_PMC_MCR_V2_ID(master->id));739 740 spin_unlock_irqrestore(master->lock, flags);741}742 743static int clk_sama7g5_master_is_enabled(struct clk_hw *hw)744{745 struct clk_master *master = to_clk_master(hw);746 unsigned long flags;747 unsigned int val;748 749 spin_lock_irqsave(master->lock, flags);750 751 regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);752 regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);753 754 spin_unlock_irqrestore(master->lock, flags);755 756 return !!(val & AT91_PMC_MCR_V2_EN);757}758 759static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate,760 unsigned long parent_rate)761{762 struct clk_master *master = to_clk_master(hw);763 unsigned long div, flags;764 765 div = DIV_ROUND_CLOSEST(parent_rate, rate);766 if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1)))767 return -EINVAL;768 769 if (div == 3)770 div = MASTER_PRES_MAX;771 else if (div)772 div = ffs(div) - 1;773 774 spin_lock_irqsave(master->lock, flags);775 master->div = div;776 spin_unlock_irqrestore(master->lock, flags);777 778 return 0;779}780 781static int clk_sama7g5_master_save_context(struct clk_hw *hw)782{783 struct clk_master *master = to_clk_master(hw);784 785 master->pms.status = clk_sama7g5_master_is_enabled(hw);786 787 return 0;788}789 790static void clk_sama7g5_master_restore_context(struct clk_hw *hw)791{792 struct clk_master *master = to_clk_master(hw);793 794 if (master->pms.status)795 clk_sama7g5_master_set(master, master->pms.status);796}797 798static const struct clk_ops sama7g5_master_ops = {799 .enable = clk_sama7g5_master_enable,800 .disable = clk_sama7g5_master_disable,801 .is_enabled = clk_sama7g5_master_is_enabled,802 .recalc_rate = clk_sama7g5_master_recalc_rate,803 .determine_rate = clk_sama7g5_master_determine_rate,804 .set_rate = clk_sama7g5_master_set_rate,805 .get_parent = clk_sama7g5_master_get_parent,806 .set_parent = clk_sama7g5_master_set_parent,807 .save_context = clk_sama7g5_master_save_context,808 .restore_context = clk_sama7g5_master_restore_context,809};810 811struct clk_hw * __init812at91_clk_sama7g5_register_master(struct regmap *regmap,813 const char *name, int num_parents,814 const char **parent_names,815 struct clk_hw **parent_hws,816 u32 *mux_table,817 spinlock_t *lock, u8 id,818 bool critical, int chg_pid)819{820 struct clk_master *master;821 struct clk_hw *hw;822 struct clk_init_data init = {};823 unsigned long flags;824 unsigned int val;825 int ret;826 827 if (!name || !num_parents || !(parent_names || parent_hws) || !mux_table ||828 !lock || id > MASTER_MAX_ID)829 return ERR_PTR(-EINVAL);830 831 master = kzalloc(sizeof(*master), GFP_KERNEL);832 if (!master)833 return ERR_PTR(-ENOMEM);834 835 init.name = name;836 init.ops = &sama7g5_master_ops;837 if (parent_hws)838 init.parent_hws = (const struct clk_hw **)parent_hws;839 else840 init.parent_names = parent_names;841 init.num_parents = num_parents;842 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;843 if (chg_pid >= 0)844 init.flags |= CLK_SET_RATE_PARENT;845 if (critical)846 init.flags |= CLK_IS_CRITICAL;847 848 master->hw.init = &init;849 master->regmap = regmap;850 master->id = id;851 master->chg_pid = chg_pid;852 master->lock = lock;853 master->mux_table = mux_table;854 855 spin_lock_irqsave(master->lock, flags);856 regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);857 regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);858 master->parent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT;859 master->div = (val & AT91_PMC_MCR_V2_DIV) >> MASTER_DIV_SHIFT;860 spin_unlock_irqrestore(master->lock, flags);861 862 hw = &master->hw;863 ret = clk_hw_register(NULL, &master->hw);864 if (ret) {865 kfree(master);866 hw = ERR_PTR(ret);867 }868 869 return hw;870}871 872const struct clk_master_layout at91rm9200_master_layout = {873 .mask = 0x31F,874 .pres_shift = 2,875 .offset = AT91_PMC_MCKR,876};877 878const struct clk_master_layout at91sam9x5_master_layout = {879 .mask = 0x373,880 .pres_shift = 4,881 .offset = AT91_PMC_MCKR,882};883