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