drivers/clk/clk-multiplier.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
   4  */
   5
   6 #include <linux/bitops.h>
   7 #include <linux/clk-provider.h>
   8 #include <linux/err.h>
   9 #include <linux/export.h>
  10 #include <linux/io.h>
  11 #include <linux/kernel.h>
  12 #include <linux/of.h>
  13 #include <linux/slab.h>
  14
  15 static inline u32 clk_mult_readl(struct clk_multiplier *mult)
  16 {
  17         if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
  18                 return ioread32be(mult->reg);
  19
  20         return readl(mult->reg);
  21 }
  22
  23 static inline void clk_mult_writel(struct clk_multiplier *mult, u32 val)
  24 {
  25         if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
  26                 iowrite32be(val, mult->reg);
  27         else
  28                 writel(val, mult->reg);
  29 }
  30
  31 static unsigned long __get_mult(struct clk_multiplier *mult,
  32                                 unsigned long rate,
  33                                 unsigned long parent_rate)
  34 {
  35         if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
  36                 return DIV_ROUND_CLOSEST(rate, parent_rate);
  37
  38         return rate / parent_rate;
  39 }
  40
  41 static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
  42                                                 unsigned long parent_rate)
  43 {
  44         struct clk_multiplier *mult = to_clk_multiplier(hw);
  45         unsigned long val;
  46
  47         val = clk_mult_readl(mult) >> mult->shift;
  48         val &= GENMASK(mult->width - 1, 0);
  49
  50         if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
  51                 val = 1;
  52
  53         return parent_rate * val;
  54 }
  55
  56 static bool __is_best_rate(unsigned long rate, unsigned long new,
  57                            unsigned long best, unsigned long flags)
  58 {
  59         if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
  60                 return abs(rate - new) < abs(rate - best);
  61
  62         return new >= rate && new < best;
  63 }
  64
  65 static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
  66                                 unsigned long *best_parent_rate,
  67                                 u8 width, unsigned long flags)
  68 {
  69         struct clk_multiplier *mult = to_clk_multiplier(hw);
  70         unsigned long orig_parent_rate = *best_parent_rate;
  71         unsigned long parent_rate, current_rate, best_rate = ~0;
  72         unsigned int i, bestmult = 0;
  73         unsigned int maxmult = (1 << width) - 1;
  74
  75         if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
  76                 bestmult = rate / orig_parent_rate;
  77
  78                 /* Make sure we don't end up with a 0 multiplier */
  79                 if ((bestmult == 0) &&
  80                     !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
  81                         bestmult = 1;
  82
  83                 /* Make sure we don't overflow the multiplier */
  84                 if (bestmult > maxmult)
  85                         bestmult = maxmult;
  86
  87                 return bestmult;
  88         }
  89
  90         for (i = 1; i < maxmult; i++) {
  91                 if (rate == orig_parent_rate * i) {
  92                         /*
  93                          * This is the best case for us if we have a
  94                          * perfect match without changing the parent
  95                          * rate.
  96                          */
  97                         *best_parent_rate = orig_parent_rate;
  98                         return i;
  99                 }
 100
 101                 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
 102                                                 rate / i);
 103                 current_rate = parent_rate * i;
 104
 105                 if (__is_best_rate(rate, current_rate, best_rate, flags)) {
 106                         bestmult = i;
 107                         best_rate = current_rate;
 108                         *best_parent_rate = parent_rate;
 109                 }
 110         }
 111
 112         return bestmult;
 113 }
 114
 115 static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
 116                                   unsigned long *parent_rate)
 117 {
 118         struct clk_multiplier *mult = to_clk_multiplier(hw);
 119         unsigned long factor = __bestmult(hw, rate, parent_rate,
 120                                           mult->width, mult->flags);
 121
 122         return *parent_rate * factor;
 123 }
 124
 125 static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
 126                                unsigned long parent_rate)
 127 {
 128         struct clk_multiplier *mult = to_clk_multiplier(hw);
 129         unsigned long factor = __get_mult(mult, rate, parent_rate);
 130         unsigned long flags = 0;
 131         unsigned long val;
 132
 133         if (mult->lock)
 134                 spin_lock_irqsave(mult->lock, flags);
 135         else
 136                 __acquire(mult->lock);
 137
 138         val = clk_mult_readl(mult);
 139         val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
 140         val |= factor << mult->shift;
 141         clk_mult_writel(mult, val);
 142
 143         if (mult->lock)
 144                 spin_unlock_irqrestore(mult->lock, flags);
 145         else
 146                 __release(mult->lock);
 147
 148         return 0;
 149 }
 150
 151 const struct clk_ops clk_multiplier_ops = {
 152         .recalc_rate    = clk_multiplier_recalc_rate,
 153         .round_rate     = clk_multiplier_round_rate,
 154         .set_rate       = clk_multiplier_set_rate,
 155 };
 156 EXPORT_SYMBOL_GPL(clk_multiplier_ops);