clk/at91/clk-master.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
0004  */
0005
0006 #include <linux/clk-provider.h>
0007 #include <linux/clkdev.h>
0008 #include <linux/clk.h>
0009 #include <linux/clk/at91_pmc.h>
0010 #include <linux/of.h>
0011 #include <linux/mfd/syscon.h>
0012 #include <linux/regmap.h>
0013
0014 #include "pmc.h"
0015
0016 #define MASTER_PRES_MASK    0x7
0017 #define MASTER_PRES_MAX     MASTER_PRES_MASK
0018 #define MASTER_DIV_SHIFT    8
0019 #define MASTER_DIV_MASK     0x7
0020
0021 #define PMC_MCR_CSS_SHIFT   (16)
0022
0023 #define MASTER_MAX_ID       4
0024
0025 #define to_clk_master(hw) container_of(hw, struct clk_master, hw)
0026
0027 struct clk_master {
0028     struct clk_hw hw;
0029     struct regmap *regmap;
0030     spinlock_t *lock;
0031     const struct clk_master_layout *layout;
0032     const struct clk_master_characteristics *characteristics;
0033     struct at91_clk_pms pms;
0034     u32 *mux_table;
0035     u32 mckr;
0036     int chg_pid;
0037     u8 id;
0038     u8 parent;
0039     u8 div;
0040     u32 safe_div;
0041 };
0042
0043 /* MCK div reference to be used by notifier. */
0044 static struct clk_master *master_div;
0045
0046 static inline bool clk_master_ready(struct clk_master *master)
0047 {
0048     unsigned int bit = master->id ? AT91_PMC_MCKXRDY : AT91_PMC_MCKRDY;
0049     unsigned int status;
0050
0051     regmap_read(master->regmap, AT91_PMC_SR, &status);
0052
0053     return !!(status & bit);
0054 }
0055
0056 static int clk_master_prepare(struct clk_hw *hw)
0057 {
0058     struct clk_master *master = to_clk_master(hw);
0059     unsigned long flags;
0060
0061     spin_lock_irqsave(master->lock, flags);
0062
0063     while (!clk_master_ready(master))
0064         cpu_relax();
0065
0066     spin_unlock_irqrestore(master->lock, flags);
0067
0068     return 0;
0069 }
0070
0071 static int clk_master_is_prepared(struct clk_hw *hw)
0072 {
0073     struct clk_master *master = to_clk_master(hw);
0074     unsigned long flags;
0075     bool status;
0076
0077     spin_lock_irqsave(master->lock, flags);
0078     status = clk_master_ready(master);
0079     spin_unlock_irqrestore(master->lock, flags);
0080
0081     return status;
0082 }
0083
0084 static unsigned long clk_master_div_recalc_rate(struct clk_hw *hw,
0085                         unsigned long parent_rate)
0086 {
0087     u8 div;
0088     unsigned long flags, rate = parent_rate;
0089     struct clk_master *master = to_clk_master(hw);
0090     const struct clk_master_layout *layout = master->layout;
0091     const struct clk_master_characteristics *characteristics =
0092                         master->characteristics;
0093     unsigned int mckr;
0094
0095     spin_lock_irqsave(master->lock, flags);
0096     regmap_read(master->regmap, master->layout->offset, &mckr);
0097     spin_unlock_irqrestore(master->lock, flags);
0098
0099     mckr &= layout->mask;
0100
0101     div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
0102
0103     rate /= characteristics->divisors[div];
0104
0105     if (rate < characteristics->output.min)
0106         pr_warn("master clk div is underclocked");
0107     else if (rate > characteristics->output.max)
0108         pr_warn("master clk div is overclocked");
0109
0110     return rate;
0111 }
0112
0113 static int clk_master_div_save_context(struct clk_hw *hw)
0114 {
0115     struct clk_master *master = to_clk_master(hw);
0116     struct clk_hw *parent_hw = clk_hw_get_parent(hw);
0117     unsigned long flags;
0118     unsigned int mckr, div;
0119
0120     spin_lock_irqsave(master->lock, flags);
0121     regmap_read(master->regmap, master->layout->offset, &mckr);
0122     spin_unlock_irqrestore(master->lock, flags);
0123
0124     mckr &= master->layout->mask;
0125     div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
0126     div = master->characteristics->divisors[div];
0127
0128     master->pms.parent_rate = clk_hw_get_rate(parent_hw);
0129     master->pms.rate = DIV_ROUND_CLOSEST(master->pms.parent_rate, div);
0130
0131     return 0;
0132 }
0133
0134 static void clk_master_div_restore_context(struct clk_hw *hw)
0135 {
0136     struct clk_master *master = to_clk_master(hw);
0137     unsigned long flags;
0138     unsigned int mckr;
0139     u8 div;
0140
0141     spin_lock_irqsave(master->lock, flags);
0142     regmap_read(master->regmap, master->layout->offset, &mckr);
0143     spin_unlock_irqrestore(master->lock, flags);
0144
0145     mckr &= master->layout->mask;
0146     div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
0147     div = master->characteristics->divisors[div];
0148
0149     if (div != DIV_ROUND_CLOSEST(master->pms.parent_rate, master->pms.rate))
0150         pr_warn("MCKR DIV not configured properly by firmware!\n");
0151 }
0152
0153 static const struct clk_ops master_div_ops = {
0154     .prepare = clk_master_prepare,
0155     .is_prepared = clk_master_is_prepared,
0156     .recalc_rate = clk_master_div_recalc_rate,
0157     .save_context = clk_master_div_save_context,
0158     .restore_context = clk_master_div_restore_context,
0159 };
0160
0161 /* This function must be called with lock acquired. */
0162 static int clk_master_div_set(struct clk_master *master,
0163                   unsigned long parent_rate, int div)
0164 {
0165     const struct clk_master_characteristics *characteristics =
0166                         master->characteristics;
0167     unsigned long rate = parent_rate;
0168     unsigned int max_div = 0, div_index = 0, max_div_index = 0;
0169     unsigned int i, mckr, tmp;
0170     int ret;
0171
0172     for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) {
0173         if (!characteristics->divisors[i])
0174             break;
0175
0176         if (div == characteristics->divisors[i])
0177             div_index = i;
0178
0179         if (max_div < characteristics->divisors[i]) {
0180             max_div = characteristics->divisors[i];
0181             max_div_index = i;
0182         }
0183     }
0184
0185     if (div > max_div)
0186         div_index = max_div_index;
0187
0188     ret = regmap_read(master->regmap, master->layout->offset, &mckr);
0189     if (ret)
0190         return ret;
0191
0192     mckr &= master->layout->mask;
0193     tmp = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
0194     if (tmp == div_index)
0195         return 0;
0196
0197     rate /= characteristics->divisors[div_index];
0198     if (rate < characteristics->output.min)
0199         pr_warn("master clk div is underclocked");
0200     else if (rate > characteristics->output.max)
0201         pr_warn("master clk div is overclocked");
0202
0203     mckr &= ~(MASTER_DIV_MASK << MASTER_DIV_SHIFT);
0204     mckr |= (div_index << MASTER_DIV_SHIFT);
0205     ret = regmap_write(master->regmap, master->layout->offset, mckr);
0206     if (ret)
0207         return ret;
0208
0209     while (!clk_master_ready(master))
0210         cpu_relax();
0211
0212     master->div = characteristics->divisors[div_index];
0213
0214     return 0;
0215 }
0216
0217 static unsigned long clk_master_div_recalc_rate_chg(struct clk_hw *hw,
0218                             unsigned long parent_rate)
0219 {
0220     struct clk_master *master = to_clk_master(hw);
0221
0222     return DIV_ROUND_CLOSEST_ULL(parent_rate, master->div);
0223 }
0224
0225 static void clk_master_div_restore_context_chg(struct clk_hw *hw)
0226 {
0227     struct clk_master *master = to_clk_master(hw);
0228     unsigned long flags;
0229     int ret;
0230
0231     spin_lock_irqsave(master->lock, flags);
0232     ret = clk_master_div_set(master, master->pms.parent_rate,
0233                  DIV_ROUND_CLOSEST(master->pms.parent_rate,
0234                            master->pms.rate));
0235     spin_unlock_irqrestore(master->lock, flags);
0236     if (ret)
0237         pr_warn("Failed to restore MCK DIV clock\n");
0238 }
0239
0240 static const struct clk_ops master_div_ops_chg = {
0241     .prepare = clk_master_prepare,
0242     .is_prepared = clk_master_is_prepared,
0243     .recalc_rate = clk_master_div_recalc_rate_chg,
0244     .save_context = clk_master_div_save_context,
0245     .restore_context = clk_master_div_restore_context_chg,
0246 };
0247
0248 static int clk_master_div_notifier_fn(struct notifier_block *notifier,
0249                       unsigned long code, void *data)
0250 {
0251     const struct clk_master_characteristics *characteristics =
0252                         master_div->characteristics;
0253     struct clk_notifier_data *cnd = data;
0254     unsigned long flags, new_parent_rate, new_rate;
0255     unsigned int mckr, div, new_div = 0;
0256     int ret, i;
0257     long tmp_diff;
0258     long best_diff = -1;
0259
0260     spin_lock_irqsave(master_div->lock, flags);
0261     switch (code) {
0262     case PRE_RATE_CHANGE:
0263         /*
0264          * We want to avoid any overclocking of MCK DIV domain. To do
0265          * this we set a safe divider (the underclocking is not of
0266          * interest as we can go as low as 32KHz). The relation
0267          * b/w this clock and its parents are as follows:
0268          *
0269          * FRAC PLL -> DIV PLL -> MCK DIV
0270          *
0271          * With the proper safe divider we should be good even with FRAC
0272          * PLL at its maximum value.
0273          */
0274         ret = regmap_read(master_div->regmap, master_div->layout->offset,
0275                   &mckr);
0276         if (ret) {
0277             ret = NOTIFY_STOP_MASK;
0278             goto unlock;
0279         }
0280
0281         mckr &= master_div->layout->mask;
0282         div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
0283
0284         /* Switch to safe divider. */
0285         clk_master_div_set(master_div,
0286                    cnd->old_rate * characteristics->divisors[div],
0287                    master_div->safe_div);
0288         break;
0289
0290     case POST_RATE_CHANGE:
0291         /*
0292          * At this point we want to restore MCK DIV domain to its maximum
0293          * allowed rate.
0294          */
0295         ret = regmap_read(master_div->regmap, master_div->layout->offset,
0296                   &mckr);
0297         if (ret) {
0298             ret = NOTIFY_STOP_MASK;
0299             goto unlock;
0300         }
0301
0302         mckr &= master_div->layout->mask;
0303         div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
0304         new_parent_rate = cnd->new_rate * characteristics->divisors[div];
0305
0306         for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) {
0307             if (!characteristics->divisors[i])
0308                 break;
0309
0310             new_rate = DIV_ROUND_CLOSEST_ULL(new_parent_rate,
0311                              characteristics->divisors[i]);
0312
0313             tmp_diff = characteristics->output.max - new_rate;
0314             if (tmp_diff < 0)
0315                 continue;
0316
0317             if (best_diff < 0 || best_diff > tmp_diff) {
0318                 new_div = characteristics->divisors[i];
0319                 best_diff = tmp_diff;
0320             }
0321
0322             if (!tmp_diff)
0323                 break;
0324         }
0325
0326         if (!new_div) {
0327             ret = NOTIFY_STOP_MASK;
0328             goto unlock;
0329         }
0330
0331         /* Update the div to preserve MCK DIV clock rate. */
0332         clk_master_div_set(master_div, new_parent_rate,
0333                    new_div);
0334
0335         ret = NOTIFY_OK;
0336         break;
0337
0338     default:
0339         ret = NOTIFY_DONE;
0340         break;
0341     }
0342
0343 unlock:
0344     spin_unlock_irqrestore(master_div->lock, flags);
0345
0346     return ret;
0347 }
0348
0349 static struct notifier_block clk_master_div_notifier = {
0350     .notifier_call = clk_master_div_notifier_fn,
0351 };
0352
0353 static void clk_sama7g5_master_best_diff(struct clk_rate_request *req,
0354                      struct clk_hw *parent,
0355                      unsigned long parent_rate,
0356                      long *best_rate,
0357                      long *best_diff,
0358                      u32 div)
0359 {
0360     unsigned long tmp_rate, tmp_diff;
0361
0362     if (div == MASTER_PRES_MAX)
0363         tmp_rate = parent_rate / 3;
0364     else
0365         tmp_rate = parent_rate >> div;
0366
0367     tmp_diff = abs(req->rate - tmp_rate);
0368
0369     if (*best_diff < 0 || *best_diff >= tmp_diff) {
0370         *best_rate = tmp_rate;
0371         *best_diff = tmp_diff;
0372         req->best_parent_rate = parent_rate;
0373         req->best_parent_hw = parent;
0374     }
0375 }
0376
0377 static unsigned long clk_master_pres_recalc_rate(struct clk_hw *hw,
0378                          unsigned long parent_rate)
0379 {
0380     struct clk_master *master = to_clk_master(hw);
0381     const struct clk_master_characteristics *characteristics =
0382                         master->characteristics;
0383     unsigned long flags;
0384     unsigned int val, pres;
0385
0386     spin_lock_irqsave(master->lock, flags);
0387     regmap_read(master->regmap, master->layout->offset, &val);
0388     spin_unlock_irqrestore(master->lock, flags);
0389
0390     val &= master->layout->mask;
0391     pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK;
0392     if (pres == MASTER_PRES_MAX && characteristics->have_div3_pres)
0393         pres = 3;
0394     else
0395         pres = (1 << pres);
0396
0397     return DIV_ROUND_CLOSEST_ULL(parent_rate, pres);
0398 }
0399
0400 static u8 clk_master_pres_get_parent(struct clk_hw *hw)
0401 {
0402     struct clk_master *master = to_clk_master(hw);
0403     unsigned long flags;
0404     unsigned int mckr;
0405
0406     spin_lock_irqsave(master->lock, flags);
0407     regmap_read(master->regmap, master->layout->offset, &mckr);
0408     spin_unlock_irqrestore(master->lock, flags);
0409
0410     mckr &= master->layout->mask;
0411
0412     return mckr & AT91_PMC_CSS;
0413 }
0414
0415 static int clk_master_pres_save_context(struct clk_hw *hw)
0416 {
0417     struct clk_master *master = to_clk_master(hw);
0418     struct clk_hw *parent_hw = clk_hw_get_parent(hw);
0419     unsigned long flags;
0420     unsigned int val, pres;
0421
0422     spin_lock_irqsave(master->lock, flags);
0423     regmap_read(master->regmap, master->layout->offset, &val);
0424     spin_unlock_irqrestore(master->lock, flags);
0425
0426     val &= master->layout->mask;
0427     pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK;
0428     if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres)
0429         pres = 3;
0430     else
0431         pres = (1 << pres);
0432
0433     master->pms.parent = val & AT91_PMC_CSS;
0434     master->pms.parent_rate = clk_hw_get_rate(parent_hw);
0435     master->pms.rate = DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres);
0436
0437     return 0;
0438 }
0439
0440 static void clk_master_pres_restore_context(struct clk_hw *hw)
0441 {
0442     struct clk_master *master = to_clk_master(hw);
0443     unsigned long flags;
0444     unsigned int val, pres;
0445
0446     spin_lock_irqsave(master->lock, flags);
0447     regmap_read(master->regmap, master->layout->offset, &val);
0448     spin_unlock_irqrestore(master->lock, flags);
0449
0450     val &= master->layout->mask;
0451     pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK;
0452     if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres)
0453         pres = 3;
0454     else
0455         pres = (1 << pres);
0456
0457     if (master->pms.rate !=
0458         DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres) ||
0459         (master->pms.parent != (val & AT91_PMC_CSS)))
0460         pr_warn("MCKR PRES was not configured properly by firmware!\n");
0461 }
0462
0463 static const struct clk_ops master_pres_ops = {
0464     .prepare = clk_master_prepare,
0465     .is_prepared = clk_master_is_prepared,
0466     .recalc_rate = clk_master_pres_recalc_rate,
0467     .get_parent = clk_master_pres_get_parent,
0468     .save_context = clk_master_pres_save_context,
0469     .restore_context = clk_master_pres_restore_context,
0470 };
0471
0472 static struct clk_hw * __init
0473 at91_clk_register_master_internal(struct regmap *regmap,
0474         const char *name, int num_parents,
0475         const char **parent_names,
0476         const struct clk_master_layout *layout,
0477         const struct clk_master_characteristics *characteristics,
0478         const struct clk_ops *ops, spinlock_t *lock, u32 flags)
0479 {
0480     struct clk_master *master;
0481     struct clk_init_data init;
0482     struct clk_hw *hw;
0483     unsigned int mckr;
0484     unsigned long irqflags;
0485     int ret;
0486
0487     if (!name || !num_parents || !parent_names || !lock)
0488         return ERR_PTR(-EINVAL);
0489
0490     master = kzalloc(sizeof(*master), GFP_KERNEL);
0491     if (!master)
0492         return ERR_PTR(-ENOMEM);
0493
0494     init.name = name;
0495     init.ops = ops;
0496     init.parent_names = parent_names;
0497     init.num_parents = num_parents;
0498     init.flags = flags;
0499
0500     master->hw.init = &init;
0501     master->layout = layout;
0502     master->characteristics = characteristics;
0503     master->regmap = regmap;
0504     master->lock = lock;
0505
0506     if (ops == &master_div_ops_chg) {
0507         spin_lock_irqsave(master->lock, irqflags);
0508         regmap_read(master->regmap, master->layout->offset, &mckr);
0509         spin_unlock_irqrestore(master->lock, irqflags);
0510
0511         mckr &= layout->mask;
0512         mckr = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
0513         master->div = characteristics->divisors[mckr];
0514     }
0515
0516     hw = &master->hw;
0517     ret = clk_hw_register(NULL, &master->hw);
0518     if (ret) {
0519         kfree(master);
0520         hw = ERR_PTR(ret);
0521     }
0522
0523     return hw;
0524 }
0525
0526 struct clk_hw * __init
0527 at91_clk_register_master_pres(struct regmap *regmap,
0528         const char *name, int num_parents,
0529         const char **parent_names,
0530         const struct clk_master_layout *layout,
0531         const struct clk_master_characteristics *characteristics,
0532         spinlock_t *lock)
0533 {
0534     return at91_clk_register_master_internal(regmap, name, num_parents,
0535                          parent_names, layout,
0536                          characteristics,
0537                          &master_pres_ops,
0538                          lock, CLK_SET_RATE_GATE);
0539 }
0540
0541 struct clk_hw * __init
0542 at91_clk_register_master_div(struct regmap *regmap,
0543         const char *name, const char *parent_name,
0544         const struct clk_master_layout *layout,
0545         const struct clk_master_characteristics *characteristics,
0546         spinlock_t *lock, u32 flags, u32 safe_div)
0547 {
0548     const struct clk_ops *ops;
0549     struct clk_hw *hw;
0550
0551     if (flags & CLK_SET_RATE_GATE)
0552         ops = &master_div_ops;
0553     else
0554         ops = &master_div_ops_chg;
0555
0556     hw = at91_clk_register_master_internal(regmap, name, 1,
0557                            &parent_name, layout,
0558                            characteristics, ops,
0559                            lock, flags);
0560
0561     if (!IS_ERR(hw) && safe_div) {
0562         master_div = to_clk_master(hw);
0563         master_div->safe_div = safe_div;
0564         clk_notifier_register(hw->clk,
0565                       &clk_master_div_notifier);
0566     }
0567
0568     return hw;
0569 }
0570
0571 static unsigned long
0572 clk_sama7g5_master_recalc_rate(struct clk_hw *hw,
0573                    unsigned long parent_rate)
0574 {
0575     struct clk_master *master = to_clk_master(hw);
0576
0577     return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div));
0578 }
0579
0580 static int clk_sama7g5_master_determine_rate(struct clk_hw *hw,
0581                          struct clk_rate_request *req)
0582 {
0583     struct clk_master *master = to_clk_master(hw);
0584     struct clk_rate_request req_parent = *req;
0585     struct clk_hw *parent;
0586     long best_rate = LONG_MIN, best_diff = LONG_MIN;
0587     unsigned long parent_rate;
0588     unsigned int div, i;
0589
0590     /* First: check the dividers of MCR. */
0591     for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
0592         parent = clk_hw_get_parent_by_index(hw, i);
0593         if (!parent)
0594             continue;
0595
0596         parent_rate = clk_hw_get_rate(parent);
0597         if (!parent_rate)
0598             continue;
0599
0600         for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
0601             clk_sama7g5_master_best_diff(req, parent, parent_rate,
0602                              &best_rate, &best_diff,
0603                              div);
0604             if (!best_diff)
0605                 break;
0606         }
0607
0608         if (!best_diff)
0609             break;
0610     }
0611
0612     /* Second: try to request rate form changeable parent. */
0613     if (master->chg_pid < 0)
0614         goto end;
0615
0616     parent = clk_hw_get_parent_by_index(hw, master->chg_pid);
0617     if (!parent)
0618         goto end;
0619
0620     for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
0621         if (div == MASTER_PRES_MAX)
0622             req_parent.rate = req->rate * 3;
0623         else
0624             req_parent.rate = req->rate << div;
0625
0626         if (__clk_determine_rate(parent, &req_parent))
0627             continue;
0628
0629         clk_sama7g5_master_best_diff(req, parent, req_parent.rate,
0630                          &best_rate, &best_diff, div);
0631
0632         if (!best_diff)
0633             break;
0634     }
0635
0636 end:
0637     pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
0638          __func__, best_rate,
0639          __clk_get_name((req->best_parent_hw)->clk),
0640         req->best_parent_rate);
0641
0642     if (best_rate < 0)
0643         return -EINVAL;
0644
0645     req->rate = best_rate;
0646
0647     return 0;
0648 }
0649
0650 static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw)
0651 {
0652     struct clk_master *master = to_clk_master(hw);
0653     unsigned long flags;
0654     u8 index;
0655
0656     spin_lock_irqsave(master->lock, flags);
0657     index = clk_mux_val_to_index(&master->hw, master->mux_table, 0,
0658                      master->parent);
0659     spin_unlock_irqrestore(master->lock, flags);
0660
0661     return index;
0662 }
0663
0664 static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index)
0665 {
0666     struct clk_master *master = to_clk_master(hw);
0667     unsigned long flags;
0668
0669     if (index >= clk_hw_get_num_parents(hw))
0670         return -EINVAL;
0671
0672     spin_lock_irqsave(master->lock, flags);
0673     master->parent = clk_mux_index_to_val(master->mux_table, 0, index);
0674     spin_unlock_irqrestore(master->lock, flags);
0675
0676     return 0;
0677 }
0678
0679 static void clk_sama7g5_master_set(struct clk_master *master,
0680                    unsigned int status)
0681 {
0682     unsigned long flags;
0683     unsigned int val, cparent;
0684     unsigned int enable = status ? AT91_PMC_MCR_V2_EN : 0;
0685     unsigned int parent = master->parent << PMC_MCR_CSS_SHIFT;
0686     unsigned int div = master->div << MASTER_DIV_SHIFT;
0687
0688     spin_lock_irqsave(master->lock, flags);
0689
0690     regmap_write(master->regmap, AT91_PMC_MCR_V2,
0691              AT91_PMC_MCR_V2_ID(master->id));
0692     regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
0693     regmap_update_bits(master->regmap, AT91_PMC_MCR_V2,
0694                enable | AT91_PMC_MCR_V2_CSS | AT91_PMC_MCR_V2_DIV |
0695                AT91_PMC_MCR_V2_CMD | AT91_PMC_MCR_V2_ID_MSK,
0696                enable | parent | div | AT91_PMC_MCR_V2_CMD |
0697                AT91_PMC_MCR_V2_ID(master->id));
0698
0699     cparent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT;
0700
0701     /* Wait here only if parent is being changed. */
0702     while ((cparent != master->parent) && !clk_master_ready(master))
0703         cpu_relax();
0704
0705     spin_unlock_irqrestore(master->lock, flags);
0706 }
0707
0708 static int clk_sama7g5_master_enable(struct clk_hw *hw)
0709 {
0710     struct clk_master *master = to_clk_master(hw);
0711
0712     clk_sama7g5_master_set(master, 1);
0713
0714     return 0;
0715 }
0716
0717 static void clk_sama7g5_master_disable(struct clk_hw *hw)
0718 {
0719     struct clk_master *master = to_clk_master(hw);
0720     unsigned long flags;
0721
0722     spin_lock_irqsave(master->lock, flags);
0723
0724     regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);
0725     regmap_update_bits(master->regmap, AT91_PMC_MCR_V2,
0726                AT91_PMC_MCR_V2_EN | AT91_PMC_MCR_V2_CMD |
0727                AT91_PMC_MCR_V2_ID_MSK,
0728                AT91_PMC_MCR_V2_CMD |
0729                AT91_PMC_MCR_V2_ID(master->id));
0730
0731     spin_unlock_irqrestore(master->lock, flags);
0732 }
0733
0734 static int clk_sama7g5_master_is_enabled(struct clk_hw *hw)
0735 {
0736     struct clk_master *master = to_clk_master(hw);
0737     unsigned long flags;
0738     unsigned int val;
0739
0740     spin_lock_irqsave(master->lock, flags);
0741
0742     regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);
0743     regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
0744
0745     spin_unlock_irqrestore(master->lock, flags);
0746
0747     return !!(val & AT91_PMC_MCR_V2_EN);
0748 }
0749
0750 static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate,
0751                        unsigned long parent_rate)
0752 {
0753     struct clk_master *master = to_clk_master(hw);
0754     unsigned long div, flags;
0755
0756     div = DIV_ROUND_CLOSEST(parent_rate, rate);
0757     if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1)))
0758         return -EINVAL;
0759
0760     if (div == 3)
0761         div = MASTER_PRES_MAX;
0762     else if (div)
0763         div = ffs(div) - 1;
0764
0765     spin_lock_irqsave(master->lock, flags);
0766     master->div = div;
0767     spin_unlock_irqrestore(master->lock, flags);
0768
0769     return 0;
0770 }
0771
0772 static int clk_sama7g5_master_save_context(struct clk_hw *hw)
0773 {
0774     struct clk_master *master = to_clk_master(hw);
0775
0776     master->pms.status = clk_sama7g5_master_is_enabled(hw);
0777
0778     return 0;
0779 }
0780
0781 static void clk_sama7g5_master_restore_context(struct clk_hw *hw)
0782 {
0783     struct clk_master *master = to_clk_master(hw);
0784
0785     if (master->pms.status)
0786         clk_sama7g5_master_set(master, master->pms.status);
0787 }
0788
0789 static const struct clk_ops sama7g5_master_ops = {
0790     .enable = clk_sama7g5_master_enable,
0791     .disable = clk_sama7g5_master_disable,
0792     .is_enabled = clk_sama7g5_master_is_enabled,
0793     .recalc_rate = clk_sama7g5_master_recalc_rate,
0794     .determine_rate = clk_sama7g5_master_determine_rate,
0795     .set_rate = clk_sama7g5_master_set_rate,
0796     .get_parent = clk_sama7g5_master_get_parent,
0797     .set_parent = clk_sama7g5_master_set_parent,
0798     .save_context = clk_sama7g5_master_save_context,
0799     .restore_context = clk_sama7g5_master_restore_context,
0800 };
0801
0802 struct clk_hw * __init
0803 at91_clk_sama7g5_register_master(struct regmap *regmap,
0804                  const char *name, int num_parents,
0805                  const char **parent_names,
0806                  u32 *mux_table,
0807                  spinlock_t *lock, u8 id,
0808                  bool critical, int chg_pid)
0809 {
0810     struct clk_master *master;
0811     struct clk_hw *hw;
0812     struct clk_init_data init;
0813     unsigned long flags;
0814     unsigned int val;
0815     int ret;
0816
0817     if (!name || !num_parents || !parent_names || !mux_table ||
0818         !lock || id > MASTER_MAX_ID)
0819         return ERR_PTR(-EINVAL);
0820
0821     master = kzalloc(sizeof(*master), GFP_KERNEL);
0822     if (!master)
0823         return ERR_PTR(-ENOMEM);
0824
0825     init.name = name;
0826     init.ops = &sama7g5_master_ops;
0827     init.parent_names = parent_names;
0828     init.num_parents = num_parents;
0829     init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
0830     if (chg_pid >= 0)
0831         init.flags |= CLK_SET_RATE_PARENT;
0832     if (critical)
0833         init.flags |= CLK_IS_CRITICAL;
0834
0835     master->hw.init = &init;
0836     master->regmap = regmap;
0837     master->id = id;
0838     master->chg_pid = chg_pid;
0839     master->lock = lock;
0840     master->mux_table = mux_table;
0841
0842     spin_lock_irqsave(master->lock, flags);
0843     regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);
0844     regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
0845     master->parent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT;
0846     master->div = (val & AT91_PMC_MCR_V2_DIV) >> MASTER_DIV_SHIFT;
0847     spin_unlock_irqrestore(master->lock, flags);
0848
0849     hw = &master->hw;
0850     ret = clk_hw_register(NULL, &master->hw);
0851     if (ret) {
0852         kfree(master);
0853         hw = ERR_PTR(ret);
0854     }
0855
0856     return hw;
0857 }
0858
0859 const struct clk_master_layout at91rm9200_master_layout = {
0860     .mask = 0x31F,
0861     .pres_shift = 2,
0862     .offset = AT91_PMC_MCKR,
0863 };
0864
0865 const struct clk_master_layout at91sam9x5_master_layout = {
0866     .mask = 0x373,
0867     .pres_shift = 4,
0868     .offset = AT91_PMC_MCKR,
0869 };