drivers/clk/clk-multiplier.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
0004  */
0005
0006 #include <linux/bitops.h>
0007 #include <linux/clk-provider.h>
0008 #include <linux/err.h>
0009 #include <linux/export.h>
0010 #include <linux/io.h>
0011 #include <linux/kernel.h>
0012 #include <linux/of.h>
0013 #include <linux/slab.h>
0014
0015 static inline u32 clk_mult_readl(struct clk_multiplier *mult)
0016 {
0017     if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
0018         return ioread32be(mult->reg);
0019
0020     return readl(mult->reg);
0021 }
0022
0023 static inline void clk_mult_writel(struct clk_multiplier *mult, u32 val)
0024 {
0025     if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
0026         iowrite32be(val, mult->reg);
0027     else
0028         writel(val, mult->reg);
0029 }
0030
0031 static unsigned long __get_mult(struct clk_multiplier *mult,
0032                 unsigned long rate,
0033                 unsigned long parent_rate)
0034 {
0035     if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
0036         return DIV_ROUND_CLOSEST(rate, parent_rate);
0037
0038     return rate / parent_rate;
0039 }
0040
0041 static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
0042                         unsigned long parent_rate)
0043 {
0044     struct clk_multiplier *mult = to_clk_multiplier(hw);
0045     unsigned long val;
0046
0047     val = clk_mult_readl(mult) >> mult->shift;
0048     val &= GENMASK(mult->width - 1, 0);
0049
0050     if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
0051         val = 1;
0052
0053     return parent_rate * val;
0054 }
0055
0056 static bool __is_best_rate(unsigned long rate, unsigned long new,
0057                unsigned long best, unsigned long flags)
0058 {
0059     if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
0060         return abs(rate - new) < abs(rate - best);
0061
0062     return new >= rate && new < best;
0063 }
0064
0065 static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
0066                 unsigned long *best_parent_rate,
0067                 u8 width, unsigned long flags)
0068 {
0069     struct clk_multiplier *mult = to_clk_multiplier(hw);
0070     unsigned long orig_parent_rate = *best_parent_rate;
0071     unsigned long parent_rate, current_rate, best_rate = ~0;
0072     unsigned int i, bestmult = 0;
0073     unsigned int maxmult = (1 << width) - 1;
0074
0075     if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
0076         bestmult = rate / orig_parent_rate;
0077
0078         /* Make sure we don't end up with a 0 multiplier */
0079         if ((bestmult == 0) &&
0080             !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
0081             bestmult = 1;
0082
0083         /* Make sure we don't overflow the multiplier */
0084         if (bestmult > maxmult)
0085             bestmult = maxmult;
0086
0087         return bestmult;
0088     }
0089
0090     for (i = 1; i < maxmult; i++) {
0091         if (rate == orig_parent_rate * i) {
0092             /*
0093              * This is the best case for us if we have a
0094              * perfect match without changing the parent
0095              * rate.
0096              */
0097             *best_parent_rate = orig_parent_rate;
0098             return i;
0099         }
0100
0101         parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
0102                         rate / i);
0103         current_rate = parent_rate * i;
0104
0105         if (__is_best_rate(rate, current_rate, best_rate, flags)) {
0106             bestmult = i;
0107             best_rate = current_rate;
0108             *best_parent_rate = parent_rate;
0109         }
0110     }
0111
0112     return bestmult;
0113 }
0114
0115 static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
0116                   unsigned long *parent_rate)
0117 {
0118     struct clk_multiplier *mult = to_clk_multiplier(hw);
0119     unsigned long factor = __bestmult(hw, rate, parent_rate,
0120                       mult->width, mult->flags);
0121
0122     return *parent_rate * factor;
0123 }
0124
0125 static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
0126                    unsigned long parent_rate)
0127 {
0128     struct clk_multiplier *mult = to_clk_multiplier(hw);
0129     unsigned long factor = __get_mult(mult, rate, parent_rate);
0130     unsigned long flags = 0;
0131     unsigned long val;
0132
0133     if (mult->lock)
0134         spin_lock_irqsave(mult->lock, flags);
0135     else
0136         __acquire(mult->lock);
0137
0138     val = clk_mult_readl(mult);
0139     val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
0140     val |= factor << mult->shift;
0141     clk_mult_writel(mult, val);
0142
0143     if (mult->lock)
0144         spin_unlock_irqrestore(mult->lock, flags);
0145     else
0146         __release(mult->lock);
0147
0148     return 0;
0149 }
0150
0151 const struct clk_ops clk_multiplier_ops = {
0152     .recalc_rate    = clk_multiplier_recalc_rate,
0153     .round_rate = clk_multiplier_round_rate,
0154     .set_rate   = clk_multiplier_set_rate,
0155 };
0156 EXPORT_SYMBOL_GPL(clk_multiplier_ops);