LXR linux/drivers/clk/clk-multiplier.c

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