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/kernel.h>
  11 #include <linux/of.h>
  12 #include <linux/slab.h>
  13
  14 static unsigned long __get_mult(struct clk_multiplier *mult,
  15                                 unsigned long rate,
  16                                 unsigned long parent_rate)
  17 {
  18         if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
  19                 return DIV_ROUND_CLOSEST(rate, parent_rate);
  20
  21         return rate / parent_rate;
  22 }
  23
  24 static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
  25                                                 unsigned long parent_rate)
  26 {
  27         struct clk_multiplier *mult = to_clk_multiplier(hw);
  28         unsigned long val;
  29
  30         val = clk_readl(mult->reg) >> mult->shift;
  31         val &= GENMASK(mult->width - 1, 0);
  32
  33         if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
  34                 val = 1;
  35
  36         return parent_rate * val;
  37 }
  38
  39 static bool __is_best_rate(unsigned long rate, unsigned long new,
  40                            unsigned long best, unsigned long flags)
  41 {
  42         if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
  43                 return abs(rate - new) < abs(rate - best);
  44
  45         return new >= rate && new < best;
  46 }
  47
  48 static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
  49                                 unsigned long *best_parent_rate,
  50                                 u8 width, unsigned long flags)
  51 {
  52         struct clk_multiplier *mult = to_clk_multiplier(hw);
  53         unsigned long orig_parent_rate = *best_parent_rate;
  54         unsigned long parent_rate, current_rate, best_rate = ~0;
  55         unsigned int i, bestmult = 0;
  56         unsigned int maxmult = (1 << width) - 1;
  57
  58         if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
  59                 bestmult = rate / orig_parent_rate;
  60
  61                 /* Make sure we don't end up with a 0 multiplier */
  62                 if ((bestmult == 0) &&
  63                     !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
  64                         bestmult = 1;
  65
  66                 /* Make sure we don't overflow the multiplier */
  67                 if (bestmult > maxmult)
  68                         bestmult = maxmult;
  69
  70                 return bestmult;
  71         }
  72
  73         for (i = 1; i < maxmult; i++) {
  74                 if (rate == orig_parent_rate * i) {
  75                         /*
  76                          * This is the best case for us if we have a
  77                          * perfect match without changing the parent
  78                          * rate.
  79                          */
  80                         *best_parent_rate = orig_parent_rate;
  81                         return i;
  82                 }
  83
  84                 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
  85                                                 rate / i);
  86                 current_rate = parent_rate * i;
  87
  88                 if (__is_best_rate(rate, current_rate, best_rate, flags)) {
  89                         bestmult = i;
  90                         best_rate = current_rate;
  91                         *best_parent_rate = parent_rate;
  92                 }
  93         }
  94
  95         return bestmult;
  96 }
  97
  98 static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
  99                                   unsigned long *parent_rate)
 100 {
 101         struct clk_multiplier *mult = to_clk_multiplier(hw);
 102         unsigned long factor = __bestmult(hw, rate, parent_rate,
 103                                           mult->width, mult->flags);
 104
 105         return *parent_rate * factor;
 106 }
 107
 108 static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
 109                                unsigned long parent_rate)
 110 {
 111         struct clk_multiplier *mult = to_clk_multiplier(hw);
 112         unsigned long factor = __get_mult(mult, rate, parent_rate);
 113         unsigned long flags = 0;
 114         unsigned long val;
 115
 116         if (mult->lock)
 117                 spin_lock_irqsave(mult->lock, flags);
 118         else
 119                 __acquire(mult->lock);
 120
 121         val = clk_readl(mult->reg);
 122         val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
 123         val |= factor << mult->shift;
 124         clk_writel(val, mult->reg);
 125
 126         if (mult->lock)
 127                 spin_unlock_irqrestore(mult->lock, flags);
 128         else
 129                 __release(mult->lock);
 130
 131         return 0;
 132 }
 133
 134 const struct clk_ops clk_multiplier_ops = {
 135         .recalc_rate    = clk_multiplier_recalc_rate,
 136         .round_rate     = clk_multiplier_round_rate,
 137         .set_rate       = clk_multiplier_set_rate,
 138 };
 139 EXPORT_SYMBOL_GPL(clk_multiplier_ops);