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         unsigned long orig_parent_rate = *best_parent_rate;
  56         unsigned long parent_rate, current_rate, best_rate = ~0;
  57         unsigned int i, bestmult = 0;
  58
  59         if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT))
  60                 return rate / *best_parent_rate;
  61
  62         for (i = 1; i < ((1 << width) - 1); i++) {
  63                 if (rate == orig_parent_rate * i) {
  64                         /*
  65                          * This is the best case for us if we have a
  66                          * perfect match without changing the parent
  67                          * rate.
  68                          */
  69                         *best_parent_rate = orig_parent_rate;
  70                         return i;
  71                 }
  72
  73                 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
  74                                                 rate / i);
  75                 current_rate = parent_rate * i;
  76
  77                 if (__is_best_rate(rate, current_rate, best_rate, flags)) {
  78                         bestmult = i;
  79                         best_rate = current_rate;
  80                         *best_parent_rate = parent_rate;
  81                 }
  82         }
  83
  84         return bestmult;
  85 }
  86
  87 static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
  88                                   unsigned long *parent_rate)
  89 {
  90         struct clk_multiplier *mult = to_clk_multiplier(hw);
  91         unsigned long factor = __bestmult(hw, rate, parent_rate,
  92                                           mult->width, mult->flags);
  93
  94         return *parent_rate * factor;
  95 }
  96
  97 static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
  98                                unsigned long parent_rate)
  99 {
 100         struct clk_multiplier *mult = to_clk_multiplier(hw);
 101         unsigned long factor = __get_mult(mult, rate, parent_rate);
 102         unsigned long flags = 0;
 103         unsigned long val;
 104
 105         if (mult->lock)
 106                 spin_lock_irqsave(mult->lock, flags);
 107         else
 108                 __acquire(mult->lock);
 109
 110         val = clk_readl(mult->reg);
 111         val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
 112         val |= factor << mult->shift;
 113         clk_writel(val, mult->reg);
 114
 115         if (mult->lock)
 116                 spin_unlock_irqrestore(mult->lock, flags);
 117         else
 118                 __release(mult->lock);
 119
 120         return 0;
 121 }
 122
 123 const struct clk_ops clk_multiplier_ops = {
 124         .recalc_rate    = clk_multiplier_recalc_rate,
 125         .round_rate     = clk_multiplier_round_rate,
 126         .set_rate       = clk_multiplier_set_rate,
 127 };
 128 EXPORT_SYMBOL_GPL(clk_multiplier_ops);