[mirror_ubuntu-disco-kernel.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/kernel.h>
#include <linux/of.h>
#include <linux/slab.h>

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_readl(mult->reg) >> 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_readl(mult->reg);
	val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
	val |= factor << mult->shift;
	clk_writel(val, mult->reg);

	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>
	10	#include <linux/kernel.h>
	11	#include <linux/of.h>
	12	#include <linux/slab.h>
	13
f2e0a532 MR	14	static unsigned long __get_mult(struct clk_multiplier *mult,
	15	unsigned long rate,
	16	unsigned long parent_rate)
	17	{
	18	if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
	19	return DIV_ROUND_CLOSEST(rate, parent_rate);
	20
	21	return rate / parent_rate;
	22	}
	23
	24	static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
	25	unsigned long parent_rate)
	26	{
	27	struct clk_multiplier *mult = to_clk_multiplier(hw);
	28	unsigned long val;
	29
	30	val = clk_readl(mult->reg) >> mult->shift;
	31	val &= GENMASK(mult->width - 1, 0);
	32
	33	if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
	34	val = 1;
	35
	36	return parent_rate * val;
	37	}
	38
	39	static bool __is_best_rate(unsigned long rate, unsigned long new,
	40	unsigned long best, unsigned long flags)
	41	{
	42	if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
	43	return abs(rate - new) < abs(rate - best);
	44
	45	return new >= rate && new < best;
	46	}
	47
	48	static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
	49	unsigned long *best_parent_rate,
	50	u8 width, unsigned long flags)
	51	{
25f77a3a	52	struct clk_multiplier *mult = to_clk_multiplier(hw);
f2e0a532 MR	53	unsigned long orig_parent_rate = *best_parent_rate;
	54	unsigned long parent_rate, current_rate, best_rate = ~0;
	55	unsigned int i, bestmult = 0;
25f77a3a MR	56	unsigned int maxmult = (1 << width) - 1;
	57
	58	if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
	59	bestmult = rate / orig_parent_rate;
	60
	61	/* Make sure we don't end up with a 0 multiplier */
	62	if ((bestmult == 0) &&
	63	!(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
	64	bestmult = 1;
f2e0a532	65
25f77a3a MR	66	/* Make sure we don't overflow the multiplier */
	67	if (bestmult > maxmult)
	68	bestmult = maxmult;
	69
	70	return bestmult;
	71	}
f2e0a532	72
25f77a3a	73	for (i = 1; i < maxmult; i++) {
f2e0a532 MR	74	if (rate == orig_parent_rate * i) {
	75	/*
	76	* This is the best case for us if we have a
	77	* perfect match without changing the parent
	78	* rate.
	79	*/
	80	*best_parent_rate = orig_parent_rate;
	81	return i;
	82	}
	83
	84	parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
	85	rate / i);
	86	current_rate = parent_rate * i;
	87
	88	if (__is_best_rate(rate, current_rate, best_rate, flags)) {
	89	bestmult = i;
	90	best_rate = current_rate;
	91	*best_parent_rate = parent_rate;
	92	}
	93	}
	94
	95	return bestmult;
	96	}
	97
	98	static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
	99	unsigned long *parent_rate)
	100	{
	101	struct clk_multiplier *mult = to_clk_multiplier(hw);
	102	unsigned long factor = __bestmult(hw, rate, parent_rate,
	103	mult->width, mult->flags);
	104
	105	return parent_rate factor;
	106	}
	107
	108	static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
	109	unsigned long parent_rate)
	110	{
	111	struct clk_multiplier *mult = to_clk_multiplier(hw);
	112	unsigned long factor = __get_mult(mult, rate, parent_rate);
	113	unsigned long flags = 0;
	114	unsigned long val;
	115
	116	if (mult->lock)
	117	spin_lock_irqsave(mult->lock, flags);
	118	else
	119	__acquire(mult->lock);
	120
	121	val = clk_readl(mult->reg);
	122	val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
	123	val \|= factor << mult->shift;
	124	clk_writel(val, mult->reg);
	125
	126	if (mult->lock)
	127	spin_unlock_irqrestore(mult->lock, flags);
	128	else
	129	__release(mult->lock);
	130
	131	return 0;
	132	}
	133
	134	const struct clk_ops clk_multiplier_ops = {
	135	.recalc_rate = clk_multiplier_recalc_rate,
	136	.round_rate = clk_multiplier_round_rate,
	137	.set_rate = clk_multiplier_set_rate,
138	};
139	EXPORT_SYMBOL_GPL(clk_multiplier_ops);