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

/*
 * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */

#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>

#define to_clk_multiplier(_hw) container_of(_hw, struct clk_multiplier, hw)

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)
{
	unsigned long orig_parent_rate = *best_parent_rate;
	unsigned long parent_rate, current_rate, best_rate = ~0;
	unsigned int i, bestmult = 0;

	if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT))
		return rate / *best_parent_rate;

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