[linux-block.git] / drivers / clk / clk-multiplier.c

// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
 */

#include <linux/bitops.h>
#include <linux/clk-provider.h>
#include <linux/err.h>
#include <linux/export.h>
#include <linux/io.h>
#include <linux/kernel.h>
#include <linux/of.h>
#include <linux/slab.h>

static inline u32 clk_mult_readl(struct clk_multiplier *mult)
{
	if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
		return ioread32be(mult->reg);

	return readl(mult->reg);
}

static inline void clk_mult_writel(struct clk_multiplier *mult, u32 val)
{
	if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
		iowrite32be(val, mult->reg);
	else
		writel(val, mult->reg);
}

static unsigned long __get_mult(struct clk_multiplier *mult,
				unsigned long rate,
				unsigned long parent_rate)
{
	if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
		return DIV_ROUND_CLOSEST(rate, parent_rate);

	return rate / parent_rate;
}

static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
						unsigned long parent_rate)
{
	struct clk_multiplier *mult = to_clk_multiplier(hw);
	unsigned long val;

	val = clk_mult_readl(mult) >> mult->shift;
	val &= GENMASK(mult->width - 1, 0);

	if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
		val = 1;

	return parent_rate * val;
}

static bool __is_best_rate(unsigned long rate, unsigned long new,
			   unsigned long best, unsigned long flags)
{
	if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
		return abs(rate - new) < abs(rate - best);

	return new >= rate && new < best;
}

static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
				unsigned long *best_parent_rate,
				u8 width, unsigned long flags)
{
	struct clk_multiplier *mult = to_clk_multiplier(hw);
	unsigned long orig_parent_rate = *best_parent_rate;
	unsigned long parent_rate, current_rate, best_rate = ~0;
	unsigned int i, bestmult = 0;
	unsigned int maxmult = (1 << width) - 1;

	if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
		bestmult = rate / orig_parent_rate;

		/* Make sure we don't end up with a 0 multiplier */
		if ((bestmult == 0) &&
		    !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
			bestmult = 1;

		/* Make sure we don't overflow the multiplier */
		if (bestmult > maxmult)
			bestmult = maxmult;

		return bestmult;
	}

	for (i = 1; i < maxmult; i++) {
		if (rate == orig_parent_rate * i) {
			/*
			 * This is the best case for us if we have a
			 * perfect match without changing the parent
			 * rate.
			 */
			*best_parent_rate = orig_parent_rate;
			return i;
		}

		parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
						rate / i);
		current_rate = parent_rate * i;

		if (__is_best_rate(rate, current_rate, best_rate, flags)) {
			bestmult = i;
			best_rate = current_rate;
			*best_parent_rate = parent_rate;
		}
	}

	return bestmult;
}

static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
				  unsigned long *parent_rate)
{
	struct clk_multiplier *mult = to_clk_multiplier(hw);
	unsigned long factor = __bestmult(hw, rate, parent_rate,
					  mult->width, mult->flags);

	return *parent_rate * factor;
}

static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
			       unsigned long parent_rate)
{
	struct clk_multiplier *mult = to_clk_multiplier(hw);
	unsigned long factor = __get_mult(mult, rate, parent_rate);
	unsigned long flags = 0;
	unsigned long val;

	if (mult->lock)
		spin_lock_irqsave(mult->lock, flags);
	else
		__acquire(mult->lock);

	val = clk_mult_readl(mult);
	val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
	val |= factor << mult->shift;
	clk_mult_writel(mult, val);

	if (mult->lock)
		spin_unlock_irqrestore(mult->lock, flags);
	else
		__release(mult->lock);

	return 0;
}

const struct clk_ops clk_multiplier_ops = {
	.recalc_rate	= clk_multiplier_recalc_rate,
	.round_rate	= clk_multiplier_round_rate,
	.set_rate	= clk_multiplier_set_rate,
};
EXPORT_SYMBOL_GPL(clk_multiplier_ops);
Commit	Line	Data
e1bd55e5	1	// SPDX-License-Identifier: GPL-2.0
f2e0a532 MR	2	/*
f2e0a532 MR	3	* Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
f2e0a532 MR	4	*/
	5
	6	#include <linux/bitops.h>
	7	#include <linux/clk-provider.h>
	8	#include <linux/err.h>
	9	#include <linux/export.h>
62e59c4e	10	#include <linux/io.h>
f2e0a532 MR	11	#include <linux/kernel.h>
	12	#include <linux/of.h>
	13	#include <linux/slab.h>
	14
9427b71a JG	15	static inline u32 clk_mult_readl(struct clk_multiplier *mult)
	16	{
	17	if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
	18	return ioread32be(mult->reg);
	19
5834fd75	20	return readl(mult->reg);
9427b71a JG	21	}
	22
	23	static 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
5834fd75	28	writel(val, mult->reg);
9427b71a JG	29	}
9427b71a JG	30
f2e0a532 MR	31	static 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
	41	static 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
9427b71a	47	val = clk_mult_readl(mult) >> mult->shift;
f2e0a532 MR	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
	56	static 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
	65	static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
	66	unsigned long *best_parent_rate,
	67	u8 width, unsigned long flags)
	68	{
25f77a3a	69	struct clk_multiplier *mult = to_clk_multiplier(hw);
f2e0a532 MR	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;
25f77a3a MR	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;
f2e0a532	82
25f77a3a MR	83	/* Make sure we don't overflow the multiplier */
	84	if (bestmult > maxmult)
	85	bestmult = maxmult;
	86
	87	return bestmult;
	88	}
f2e0a532	89
25f77a3a	90	for (i = 1; i < maxmult; i++) {
f2e0a532 MR	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
	115	static 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
	125	static 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
9427b71a	138	val = clk_mult_readl(mult);
f2e0a532 MR	139	val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
f2e0a532 MR	140	val \|= factor << mult->shift;
9427b71a	141	clk_mult_writel(mult, val);
f2e0a532 MR	142
	143	if (mult->lock)
	144	spin_unlock_irqrestore(mult->lock, flags);
	145	else
	146	__release(mult->lock);
	147
	148	return 0;
	149	}
	150
	151	const 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	};
	156	EXPORT_SYMBOL_GPL(clk_multiplier_ops);