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