LXR linux/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
  17static 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
  27static 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
  42static 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
  51static 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
 101static 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
 111static 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
 137const 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};
 142EXPORT_SYMBOL_GPL(clk_multiplier_ops);
 143