drivers/clk/clk-multiplier.c

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