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         unsigned long orig_parent_rate = *best_parent_rate;
  58         unsigned long parent_rate, current_rate, best_rate = ~0;
  59         unsigned int i, bestmult = 0;
  60
  61         if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT))
  62                 return rate / *best_parent_rate;
  63
  64         for (i = 1; i < ((1 << width) - 1); i++) {
  65                 if (rate == orig_parent_rate * i) {
  66                         /*
  67                          * This is the best case for us if we have a
  68                          * perfect match without changing the parent
  69                          * rate.
  70                          */
  71                         *best_parent_rate = orig_parent_rate;
  72                         return i;
  73                 }
  74
  75                 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
  76                                                 rate / i);
  77                 current_rate = parent_rate * i;
  78
  79                 if (__is_best_rate(rate, current_rate, best_rate, flags)) {
  80                         bestmult = i;
  81                         best_rate = current_rate;
  82                         *best_parent_rate = parent_rate;
  83                 }
  84         }
  85
  86         return bestmult;
  87 }
  88
  89 static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
  90                                   unsigned long *parent_rate)
  91 {
  92         struct clk_multiplier *mult = to_clk_multiplier(hw);
  93         unsigned long factor = __bestmult(hw, rate, parent_rate,
  94                                           mult->width, mult->flags);
  95
  96         return *parent_rate * factor;
  97 }
  98
  99 static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
 100                                unsigned long parent_rate)
 101 {
 102         struct clk_multiplier *mult = to_clk_multiplier(hw);
 103         unsigned long factor = __get_mult(mult, rate, parent_rate);
 104         unsigned long flags = 0;
 105         unsigned long val;
 106
 107         if (mult->lock)
 108                 spin_lock_irqsave(mult->lock, flags);
 109         else
 110                 __acquire(mult->lock);
 111
 112         val = clk_readl(mult->reg);
 113         val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
 114         val |= factor << mult->shift;
 115         clk_writel(val, mult->reg);
 116
 117         if (mult->lock)
 118                 spin_unlock_irqrestore(mult->lock, flags);
 119         else
 120                 __release(mult->lock);
 121
 122         return 0;
 123 }
 124
 125 const struct clk_ops clk_multiplier_ops = {
 126         .recalc_rate    = clk_multiplier_recalc_rate,
 127         .round_rate     = clk_multiplier_round_rate,
 128         .set_rate       = clk_multiplier_set_rate,
 129 };
 130 EXPORT_SYMBOL_GPL(clk_multiplier_ops);
 131
 132 struct clk *clk_register_multiplier(struct device *dev, const char *name,
 133                                     const char *parent_name,
 134                                     unsigned long flags,
 135                                     void __iomem *reg, u8 shift, u8 width,
 136                                     u8 clk_mult_flags, spinlock_t *lock)
 137 {
 138         struct clk_init_data init;
 139         struct clk_multiplier *mult;
 140         struct clk *clk;
 141
 142         mult = kmalloc(sizeof(*mult), GFP_KERNEL);
 143         if (!mult)
 144                 return ERR_PTR(-ENOMEM);
 145
 146         init.name = name;
 147         init.ops = &clk_multiplier_ops;
 148         init.flags = flags | CLK_IS_BASIC;
 149         init.parent_names = &parent_name;
 150         init.num_parents = 1;
 151
 152         mult->reg = reg;
 153         mult->shift = shift;
 154         mult->width = width;
 155         mult->flags = clk_mult_flags;
 156         mult->lock = lock;
 157         mult->hw.init = &init;
 158
 159         clk = clk_register(dev, &mult->hw);
 160         if (IS_ERR(clk))
 161                 kfree(mult);
 162
 163         return clk;
 164 }
 165 EXPORT_SYMBOL_GPL(clk_register_multiplier);
 166
 167 void clk_unregister_multiplier(struct clk *clk)
 168 {
 169         struct clk_multiplier *mult;
 170         struct clk_hw *hw;
 171
 172         hw = __clk_get_hw(clk);
 173         if (!hw)
 174                 return;
 175
 176         mult = to_clk_multiplier(hw);
 177
 178         clk_unregister(clk);
 179         kfree(mult);
 180 }
 181 EXPORT_SYMBOL_GPL(clk_unregister_multiplier);