clk/at91/clk-peripheral.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/bitops.h>
0007 #include <linux/clk-provider.h>
0008 #include <linux/clkdev.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_SPINLOCK(pmc_pcr_lock);
0017
0018 #define PERIPHERAL_ID_MIN   2
0019 #define PERIPHERAL_ID_MAX   31
0020 #define PERIPHERAL_MASK(id) (1 << ((id) & PERIPHERAL_ID_MAX))
0021
0022 #define PERIPHERAL_MAX_SHIFT    3
0023
0024 struct clk_peripheral {
0025     struct clk_hw hw;
0026     struct regmap *regmap;
0027     u32 id;
0028 };
0029
0030 #define to_clk_peripheral(hw) container_of(hw, struct clk_peripheral, hw)
0031
0032 struct clk_sam9x5_peripheral {
0033     struct clk_hw hw;
0034     struct regmap *regmap;
0035     struct clk_range range;
0036     spinlock_t *lock;
0037     u32 id;
0038     u32 div;
0039     const struct clk_pcr_layout *layout;
0040     struct at91_clk_pms pms;
0041     bool auto_div;
0042     int chg_pid;
0043 };
0044
0045 #define to_clk_sam9x5_peripheral(hw) \
0046     container_of(hw, struct clk_sam9x5_peripheral, hw)
0047
0048 static int clk_peripheral_enable(struct clk_hw *hw)
0049 {
0050     struct clk_peripheral *periph = to_clk_peripheral(hw);
0051     int offset = AT91_PMC_PCER;
0052     u32 id = periph->id;
0053
0054     if (id < PERIPHERAL_ID_MIN)
0055         return 0;
0056     if (id > PERIPHERAL_ID_MAX)
0057         offset = AT91_PMC_PCER1;
0058     regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id));
0059
0060     return 0;
0061 }
0062
0063 static void clk_peripheral_disable(struct clk_hw *hw)
0064 {
0065     struct clk_peripheral *periph = to_clk_peripheral(hw);
0066     int offset = AT91_PMC_PCDR;
0067     u32 id = periph->id;
0068
0069     if (id < PERIPHERAL_ID_MIN)
0070         return;
0071     if (id > PERIPHERAL_ID_MAX)
0072         offset = AT91_PMC_PCDR1;
0073     regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id));
0074 }
0075
0076 static int clk_peripheral_is_enabled(struct clk_hw *hw)
0077 {
0078     struct clk_peripheral *periph = to_clk_peripheral(hw);
0079     int offset = AT91_PMC_PCSR;
0080     unsigned int status;
0081     u32 id = periph->id;
0082
0083     if (id < PERIPHERAL_ID_MIN)
0084         return 1;
0085     if (id > PERIPHERAL_ID_MAX)
0086         offset = AT91_PMC_PCSR1;
0087     regmap_read(periph->regmap, offset, &status);
0088
0089     return status & PERIPHERAL_MASK(id) ? 1 : 0;
0090 }
0091
0092 static const struct clk_ops peripheral_ops = {
0093     .enable = clk_peripheral_enable,
0094     .disable = clk_peripheral_disable,
0095     .is_enabled = clk_peripheral_is_enabled,
0096 };
0097
0098 struct clk_hw * __init
0099 at91_clk_register_peripheral(struct regmap *regmap, const char *name,
0100                  const char *parent_name, u32 id)
0101 {
0102     struct clk_peripheral *periph;
0103     struct clk_init_data init;
0104     struct clk_hw *hw;
0105     int ret;
0106
0107     if (!name || !parent_name || id > PERIPHERAL_ID_MAX)
0108         return ERR_PTR(-EINVAL);
0109
0110     periph = kzalloc(sizeof(*periph), GFP_KERNEL);
0111     if (!periph)
0112         return ERR_PTR(-ENOMEM);
0113
0114     init.name = name;
0115     init.ops = &peripheral_ops;
0116     init.parent_names = &parent_name;
0117     init.num_parents = 1;
0118     init.flags = 0;
0119
0120     periph->id = id;
0121     periph->hw.init = &init;
0122     periph->regmap = regmap;
0123
0124     hw = &periph->hw;
0125     ret = clk_hw_register(NULL, &periph->hw);
0126     if (ret) {
0127         kfree(periph);
0128         hw = ERR_PTR(ret);
0129     }
0130
0131     return hw;
0132 }
0133
0134 static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph)
0135 {
0136     struct clk_hw *parent;
0137     unsigned long parent_rate;
0138     int shift = 0;
0139
0140     if (!periph->auto_div)
0141         return;
0142
0143     if (periph->range.max) {
0144         parent = clk_hw_get_parent_by_index(&periph->hw, 0);
0145         parent_rate = clk_hw_get_rate(parent);
0146         if (!parent_rate)
0147             return;
0148
0149         for (; shift < PERIPHERAL_MAX_SHIFT; shift++) {
0150             if (parent_rate >> shift <= periph->range.max)
0151                 break;
0152         }
0153     }
0154
0155     periph->auto_div = false;
0156     periph->div = shift;
0157 }
0158
0159 static int clk_sam9x5_peripheral_set(struct clk_sam9x5_peripheral *periph,
0160                      unsigned int status)
0161 {
0162     unsigned long flags;
0163     unsigned int enable = status ? AT91_PMC_PCR_EN : 0;
0164
0165     if (periph->id < PERIPHERAL_ID_MIN)
0166         return 0;
0167
0168     spin_lock_irqsave(periph->lock, flags);
0169     regmap_write(periph->regmap, periph->layout->offset,
0170              (periph->id & periph->layout->pid_mask));
0171     regmap_update_bits(periph->regmap, periph->layout->offset,
0172                periph->layout->div_mask | periph->layout->cmd |
0173                enable,
0174                field_prep(periph->layout->div_mask, periph->div) |
0175                periph->layout->cmd | enable);
0176     spin_unlock_irqrestore(periph->lock, flags);
0177
0178     return 0;
0179 }
0180
0181 static int clk_sam9x5_peripheral_enable(struct clk_hw *hw)
0182 {
0183     struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
0184
0185     return clk_sam9x5_peripheral_set(periph, 1);
0186 }
0187
0188 static void clk_sam9x5_peripheral_disable(struct clk_hw *hw)
0189 {
0190     struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
0191     unsigned long flags;
0192
0193     if (periph->id < PERIPHERAL_ID_MIN)
0194         return;
0195
0196     spin_lock_irqsave(periph->lock, flags);
0197     regmap_write(periph->regmap, periph->layout->offset,
0198              (periph->id & periph->layout->pid_mask));
0199     regmap_update_bits(periph->regmap, periph->layout->offset,
0200                AT91_PMC_PCR_EN | periph->layout->cmd,
0201                periph->layout->cmd);
0202     spin_unlock_irqrestore(periph->lock, flags);
0203 }
0204
0205 static int clk_sam9x5_peripheral_is_enabled(struct clk_hw *hw)
0206 {
0207     struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
0208     unsigned long flags;
0209     unsigned int status;
0210
0211     if (periph->id < PERIPHERAL_ID_MIN)
0212         return 1;
0213
0214     spin_lock_irqsave(periph->lock, flags);
0215     regmap_write(periph->regmap, periph->layout->offset,
0216              (periph->id & periph->layout->pid_mask));
0217     regmap_read(periph->regmap, periph->layout->offset, &status);
0218     spin_unlock_irqrestore(periph->lock, flags);
0219
0220     return !!(status & AT91_PMC_PCR_EN);
0221 }
0222
0223 static unsigned long
0224 clk_sam9x5_peripheral_recalc_rate(struct clk_hw *hw,
0225                   unsigned long parent_rate)
0226 {
0227     struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
0228     unsigned long flags;
0229     unsigned int status;
0230
0231     if (periph->id < PERIPHERAL_ID_MIN)
0232         return parent_rate;
0233
0234     spin_lock_irqsave(periph->lock, flags);
0235     regmap_write(periph->regmap, periph->layout->offset,
0236              (periph->id & periph->layout->pid_mask));
0237     regmap_read(periph->regmap, periph->layout->offset, &status);
0238     spin_unlock_irqrestore(periph->lock, flags);
0239
0240     if (status & AT91_PMC_PCR_EN) {
0241         periph->div = field_get(periph->layout->div_mask, status);
0242         periph->auto_div = false;
0243     } else {
0244         clk_sam9x5_peripheral_autodiv(periph);
0245     }
0246
0247     return parent_rate >> periph->div;
0248 }
0249
0250 static void clk_sam9x5_peripheral_best_diff(struct clk_rate_request *req,
0251                         struct clk_hw *parent,
0252                         unsigned long parent_rate,
0253                         u32 shift, long *best_diff,
0254                         long *best_rate)
0255 {
0256     unsigned long tmp_rate = parent_rate >> shift;
0257     unsigned long tmp_diff = abs(req->rate - tmp_rate);
0258
0259     if (*best_diff < 0 || *best_diff >= tmp_diff) {
0260         *best_rate = tmp_rate;
0261         *best_diff = tmp_diff;
0262         req->best_parent_rate = parent_rate;
0263         req->best_parent_hw = parent;
0264     }
0265 }
0266
0267 static int clk_sam9x5_peripheral_determine_rate(struct clk_hw *hw,
0268                         struct clk_rate_request *req)
0269 {
0270     struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
0271     struct clk_hw *parent = clk_hw_get_parent(hw);
0272     struct clk_rate_request req_parent = *req;
0273     unsigned long parent_rate = clk_hw_get_rate(parent);
0274     unsigned long tmp_rate;
0275     long best_rate = LONG_MIN;
0276     long best_diff = LONG_MIN;
0277     u32 shift;
0278
0279     if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max)
0280         return parent_rate;
0281
0282     /* Fist step: check the available dividers. */
0283     for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
0284         tmp_rate = parent_rate >> shift;
0285
0286         if (periph->range.max && tmp_rate > periph->range.max)
0287             continue;
0288
0289         clk_sam9x5_peripheral_best_diff(req, parent, parent_rate,
0290                         shift, &best_diff, &best_rate);
0291
0292         if (!best_diff || best_rate <= req->rate)
0293             break;
0294     }
0295
0296     if (periph->chg_pid < 0)
0297         goto end;
0298
0299     /* Step two: try to request rate from parent. */
0300     parent = clk_hw_get_parent_by_index(hw, periph->chg_pid);
0301     if (!parent)
0302         goto end;
0303
0304     for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
0305         req_parent.rate = req->rate << shift;
0306
0307         if (__clk_determine_rate(parent, &req_parent))
0308             continue;
0309
0310         clk_sam9x5_peripheral_best_diff(req, parent, req_parent.rate,
0311                         shift, &best_diff, &best_rate);
0312
0313         if (!best_diff)
0314             break;
0315     }
0316 end:
0317     if (best_rate < 0 ||
0318         (periph->range.max && best_rate > periph->range.max))
0319         return -EINVAL;
0320
0321     pr_debug("PCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
0322          __func__, best_rate,
0323          __clk_get_name((req->best_parent_hw)->clk),
0324          req->best_parent_rate);
0325
0326     req->rate = best_rate;
0327
0328     return 0;
0329 }
0330
0331 static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw,
0332                          unsigned long rate,
0333                          unsigned long *parent_rate)
0334 {
0335     int shift = 0;
0336     unsigned long best_rate;
0337     unsigned long best_diff;
0338     unsigned long cur_rate = *parent_rate;
0339     unsigned long cur_diff;
0340     struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
0341
0342     if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max)
0343         return *parent_rate;
0344
0345     if (periph->range.max) {
0346         for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
0347             cur_rate = *parent_rate >> shift;
0348             if (cur_rate <= periph->range.max)
0349                 break;
0350         }
0351     }
0352
0353     if (rate >= cur_rate)
0354         return cur_rate;
0355
0356     best_diff = cur_rate - rate;
0357     best_rate = cur_rate;
0358     for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
0359         cur_rate = *parent_rate >> shift;
0360         if (cur_rate < rate)
0361             cur_diff = rate - cur_rate;
0362         else
0363             cur_diff = cur_rate - rate;
0364
0365         if (cur_diff < best_diff) {
0366             best_diff = cur_diff;
0367             best_rate = cur_rate;
0368         }
0369
0370         if (!best_diff || cur_rate < rate)
0371             break;
0372     }
0373
0374     return best_rate;
0375 }
0376
0377 static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw,
0378                       unsigned long rate,
0379                       unsigned long parent_rate)
0380 {
0381     int shift;
0382     struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
0383     if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) {
0384         if (parent_rate == rate)
0385             return 0;
0386         else
0387             return -EINVAL;
0388     }
0389
0390     if (periph->range.max && rate > periph->range.max)
0391         return -EINVAL;
0392
0393     for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
0394         if (parent_rate >> shift == rate) {
0395             periph->auto_div = false;
0396             periph->div = shift;
0397             return 0;
0398         }
0399     }
0400
0401     return -EINVAL;
0402 }
0403
0404 static int clk_sam9x5_peripheral_save_context(struct clk_hw *hw)
0405 {
0406     struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
0407
0408     periph->pms.status = clk_sam9x5_peripheral_is_enabled(hw);
0409
0410     return 0;
0411 }
0412
0413 static void clk_sam9x5_peripheral_restore_context(struct clk_hw *hw)
0414 {
0415     struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
0416
0417     if (periph->pms.status)
0418         clk_sam9x5_peripheral_set(periph, periph->pms.status);
0419 }
0420
0421 static const struct clk_ops sam9x5_peripheral_ops = {
0422     .enable = clk_sam9x5_peripheral_enable,
0423     .disable = clk_sam9x5_peripheral_disable,
0424     .is_enabled = clk_sam9x5_peripheral_is_enabled,
0425     .recalc_rate = clk_sam9x5_peripheral_recalc_rate,
0426     .round_rate = clk_sam9x5_peripheral_round_rate,
0427     .set_rate = clk_sam9x5_peripheral_set_rate,
0428     .save_context = clk_sam9x5_peripheral_save_context,
0429     .restore_context = clk_sam9x5_peripheral_restore_context,
0430 };
0431
0432 static const struct clk_ops sam9x5_peripheral_chg_ops = {
0433     .enable = clk_sam9x5_peripheral_enable,
0434     .disable = clk_sam9x5_peripheral_disable,
0435     .is_enabled = clk_sam9x5_peripheral_is_enabled,
0436     .recalc_rate = clk_sam9x5_peripheral_recalc_rate,
0437     .determine_rate = clk_sam9x5_peripheral_determine_rate,
0438     .set_rate = clk_sam9x5_peripheral_set_rate,
0439     .save_context = clk_sam9x5_peripheral_save_context,
0440     .restore_context = clk_sam9x5_peripheral_restore_context,
0441 };
0442
0443 struct clk_hw * __init
0444 at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
0445                     const struct clk_pcr_layout *layout,
0446                     const char *name, const char *parent_name,
0447                     u32 id, const struct clk_range *range,
0448                     int chg_pid)
0449 {
0450     struct clk_sam9x5_peripheral *periph;
0451     struct clk_init_data init;
0452     struct clk_hw *hw;
0453     int ret;
0454
0455     if (!name || !parent_name)
0456         return ERR_PTR(-EINVAL);
0457
0458     periph = kzalloc(sizeof(*periph), GFP_KERNEL);
0459     if (!periph)
0460         return ERR_PTR(-ENOMEM);
0461
0462     init.name = name;
0463     init.parent_names = &parent_name;
0464     init.num_parents = 1;
0465     if (chg_pid < 0) {
0466         init.flags = 0;
0467         init.ops = &sam9x5_peripheral_ops;
0468     } else {
0469         init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE |
0470                  CLK_SET_RATE_PARENT;
0471         init.ops = &sam9x5_peripheral_chg_ops;
0472     }
0473
0474     periph->id = id;
0475     periph->hw.init = &init;
0476     periph->div = 0;
0477     periph->regmap = regmap;
0478     periph->lock = lock;
0479     if (layout->div_mask)
0480         periph->auto_div = true;
0481     periph->layout = layout;
0482     periph->range = *range;
0483     periph->chg_pid = chg_pid;
0484
0485     hw = &periph->hw;
0486     ret = clk_hw_register(NULL, &periph->hw);
0487     if (ret) {
0488         kfree(periph);
0489         hw = ERR_PTR(ret);
0490     } else {
0491         clk_sam9x5_peripheral_autodiv(periph);
0492     }
0493
0494     return hw;
0495 }