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);