[linux-block.git] / drivers / clk / clk-fractional-divider.c

// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (C) 2014 Intel Corporation
 *
 * Adjustable fractional divider clock implementation.
 * Uses rational best approximation algorithm.
 *
 * Output is calculated as
 *
 *	rate = (m / n) * parent_rate				(1)
 *
 * This is useful when we have a prescaler block which asks for
 * m (numerator) and n (denominator) values to be provided to satisfy
 * the (1) as much as possible.
 *
 * Since m and n have the limitation by a range, e.g.
 *
 *	n >= 1, n < N_width, where N_width = 2^nwidth		(2)
 *
 * for some cases the output may be saturated. Hence, from (1) and (2),
 * assuming the worst case when m = 1, the inequality
 *
 *	floor(log2(parent_rate / rate)) <= nwidth		(3)
 *
 * may be derived. Thus, in cases when
 *
 *	(parent_rate / rate) >> N_width				(4)
 *
 * we might scale up the rate by 2^scale (see the description of
 * CLK_FRAC_DIVIDER_POWER_OF_TWO_PS for additional information), where
 *
 *	scale = floor(log2(parent_rate / rate)) - nwidth	(5)
 *
 * and assume that the IP, that needs m and n, has also its own
 * prescaler, which is capable to divide by 2^scale. In this way
 * we get the denominator to satisfy the desired range (2) and
 * at the same time a much better result of m and n than simple
 * saturated values.
 */

#include <linux/debugfs.h>
#include <linux/device.h>
#include <linux/io.h>
#include <linux/math.h>
#include <linux/module.h>
#include <linux/rational.h>
#include <linux/slab.h>

#include <linux/clk-provider.h>

#include "clk-fractional-divider.h"

static inline u32 clk_fd_readl(struct clk_fractional_divider *fd)
{
	if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
		return ioread32be(fd->reg);

	return readl(fd->reg);
}

static inline void clk_fd_writel(struct clk_fractional_divider *fd, u32 val)
{
	if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
		iowrite32be(val, fd->reg);
	else
		writel(val, fd->reg);
}

static void clk_fd_get_div(struct clk_hw *hw, struct u32_fract *fract)
{
	struct clk_fractional_divider *fd = to_clk_fd(hw);
	unsigned long flags = 0;
	unsigned long m, n;
	u32 mmask, nmask;
	u32 val;

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

	val = clk_fd_readl(fd);

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

	mmask = GENMASK(fd->mwidth - 1, 0) << fd->mshift;
	nmask = GENMASK(fd->nwidth - 1, 0) << fd->nshift;

	m = (val & mmask) >> fd->mshift;
	n = (val & nmask) >> fd->nshift;

	if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
		m++;
		n++;
	}

	fract->numerator = m;
	fract->denominator = n;
}

static unsigned long clk_fd_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
{
	struct u32_fract fract;
	u64 ret;

	clk_fd_get_div(hw, &fract);

	if (!fract.numerator || !fract.denominator)
		return parent_rate;

	ret = (u64)parent_rate * fract.numerator;
	do_div(ret, fract.denominator);

	return ret;
}

void clk_fractional_divider_general_approximation(struct clk_hw *hw,
						  unsigned long rate,
						  unsigned long *parent_rate,
						  unsigned long *m, unsigned long *n)
{
	struct clk_fractional_divider *fd = to_clk_fd(hw);

	/*
	 * Get rate closer to *parent_rate to guarantee there is no overflow
	 * for m and n. In the result it will be the nearest rate left shifted
	 * by (scale - fd->nwidth) bits.
	 *
	 * For the detailed explanation see the top comment in this file.
	 */
	if (fd->flags & CLK_FRAC_DIVIDER_POWER_OF_TWO_PS) {
		unsigned long scale = fls_long(*parent_rate / rate - 1);

		if (scale > fd->nwidth)
			rate <<= scale - fd->nwidth;
	}

	rational_best_approximation(rate, *parent_rate,
			GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
			m, n);
}

static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
			      unsigned long *parent_rate)
{
	struct clk_fractional_divider *fd = to_clk_fd(hw);
	unsigned long m, n;
	u64 ret;

	if (!rate || (!clk_hw_can_set_rate_parent(hw) && rate >= *parent_rate))
		return *parent_rate;

	if (fd->approximation)
		fd->approximation(hw, rate, parent_rate, &m, &n);
	else
		clk_fractional_divider_general_approximation(hw, rate, parent_rate, &m, &n);

	ret = (u64)*parent_rate * m;
	do_div(ret, n);

	return ret;
}

static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
			   unsigned long parent_rate)
{
	struct clk_fractional_divider *fd = to_clk_fd(hw);
	unsigned long flags = 0;
	unsigned long m, n;
	u32 mmask, nmask;
	u32 val;

	rational_best_approximation(rate, parent_rate,
			GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
			&m, &n);

	if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
		m--;
		n--;
	}

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

	mmask = GENMASK(fd->mwidth - 1, 0) << fd->mshift;
	nmask = GENMASK(fd->nwidth - 1, 0) << fd->nshift;

	val = clk_fd_readl(fd);
	val &= ~(mmask | nmask);
	val |= (m << fd->mshift) | (n << fd->nshift);
	clk_fd_writel(fd, val);

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

	return 0;
}

#ifdef CONFIG_DEBUG_FS
static int clk_fd_numerator_get(void *hw, u64 *val)
{
	struct u32_fract fract;

	clk_fd_get_div(hw, &fract);

	*val = fract.numerator;

	return 0;
}
DEFINE_DEBUGFS_ATTRIBUTE(clk_fd_numerator_fops, clk_fd_numerator_get, NULL, "%llu\n");

static int clk_fd_denominator_get(void *hw, u64 *val)
{
	struct u32_fract fract;

	clk_fd_get_div(hw, &fract);

	*val = fract.denominator;

	return 0;
}
DEFINE_DEBUGFS_ATTRIBUTE(clk_fd_denominator_fops, clk_fd_denominator_get, NULL, "%llu\n");

static void clk_fd_debug_init(struct clk_hw *hw, struct dentry *dentry)
{
	debugfs_create_file("numerator", 0444, dentry, hw, &clk_fd_numerator_fops);
	debugfs_create_file("denominator", 0444, dentry, hw, &clk_fd_denominator_fops);
}
#endif

const struct clk_ops clk_fractional_divider_ops = {
	.recalc_rate = clk_fd_recalc_rate,
	.round_rate = clk_fd_round_rate,
	.set_rate = clk_fd_set_rate,
#ifdef CONFIG_DEBUG_FS
	.debug_init = clk_fd_debug_init,
#endif
};
EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);

struct clk_hw *clk_hw_register_fractional_divider(struct device *dev,
		const char *name, const char *parent_name, unsigned long flags,
		void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
		u8 clk_divider_flags, spinlock_t *lock)
{
	struct clk_fractional_divider *fd;
	struct clk_init_data init;
	struct clk_hw *hw;
	int ret;

	fd = kzalloc(sizeof(*fd), GFP_KERNEL);
	if (!fd)
		return ERR_PTR(-ENOMEM);

	init.name = name;
	init.ops = &clk_fractional_divider_ops;
	init.flags = flags;
	init.parent_names = parent_name ? &parent_name : NULL;
	init.num_parents = parent_name ? 1 : 0;

	fd->reg = reg;
	fd->mshift = mshift;
	fd->mwidth = mwidth;
	fd->nshift = nshift;
	fd->nwidth = nwidth;
	fd->flags = clk_divider_flags;
	fd->lock = lock;
	fd->hw.init = &init;

	hw = &fd->hw;
	ret = clk_hw_register(dev, hw);
	if (ret) {
		kfree(fd);
		hw = ERR_PTR(ret);
	}

	return hw;
}
EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);

struct clk *clk_register_fractional_divider(struct device *dev,
		const char *name, const char *parent_name, unsigned long flags,
		void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
		u8 clk_divider_flags, spinlock_t *lock)
{
	struct clk_hw *hw;

	hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
			reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
			lock);
	if (IS_ERR(hw))
		return ERR_CAST(hw);
	return hw->clk;
}
EXPORT_SYMBOL_GPL(clk_register_fractional_divider);

void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
{
	struct clk_fractional_divider *fd;

	fd = to_clk_fd(hw);

	clk_hw_unregister(hw);
	kfree(fd);
}
Commit	Line	Data
e1bd55e5	1	// SPDX-License-Identifier: GPL-2.0
e2d0e90f HK	2	/*
	3	* Copyright (C) 2014 Intel Corporation
	4	*
e2d0e90f	5	* Adjustable fractional divider clock implementation.
0777591e	6	* Uses rational best approximation algorithm.
e81b917a AS	7	*
	8	* Output is calculated as
	9	*
	10	* rate = (m / n) * parent_rate (1)
	11	*
	12	* This is useful when we have a prescaler block which asks for
	13	* m (numerator) and n (denominator) values to be provided to satisfy
	14	* the (1) as much as possible.
	15	*
	16	* Since m and n have the limitation by a range, e.g.
	17	*
	18	* n >= 1, n < N_width, where N_width = 2^nwidth (2)
	19	*
	20	* for some cases the output may be saturated. Hence, from (1) and (2),
	21	* assuming the worst case when m = 1, the inequality
	22	*
	23	* floor(log2(parent_rate / rate)) <= nwidth (3)
	24	*
	25	* may be derived. Thus, in cases when
	26	*
	27	* (parent_rate / rate) >> N_width (4)
	28	*
	29	* we might scale up the rate by 2^scale (see the description of
	30	* CLK_FRAC_DIVIDER_POWER_OF_TWO_PS for additional information), where
	31	*
	32	* scale = floor(log2(parent_rate / rate)) - nwidth (5)
	33	*
	34	* and assume that the IP, that needs m and n, has also its own
	35	* prescaler, which is capable to divide by 2^scale. In this way
	36	* we get the denominator to satisfy the desired range (2) and
7c55e8ef	37	* at the same time a much better result of m and n than simple
e81b917a	38	* saturated values.
e2d0e90f HK	39	*/
e2d0e90f HK	40
ac49a192	41	#include <linux/debugfs.h>
76c340e9	42	#include <linux/device.h>
62e59c4e	43	#include <linux/io.h>
7fffdb77	44	#include <linux/math.h>
e2d0e90f	45	#include <linux/module.h>
0777591e	46	#include <linux/rational.h>
76c340e9 AS	47	#include <linux/slab.h>
	48
	49	#include <linux/clk-provider.h>
e2d0e90f	50
4e7cf74f AS	51	#include "clk-fractional-divider.h"
4e7cf74f AS	52
58a2b4c9 JG	53	static inline u32 clk_fd_readl(struct clk_fractional_divider *fd)
	54	{
	55	if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
	56	return ioread32be(fd->reg);
	57
5834fd75	58	return readl(fd->reg);
58a2b4c9 JG	59	}
	60
	61	static inline void clk_fd_writel(struct clk_fractional_divider *fd, u32 val)
	62	{
	63	if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
	64	iowrite32be(val, fd->reg);
	65	else
5834fd75	66	writel(val, fd->reg);
58a2b4c9 JG	67	}
58a2b4c9 JG	68
7fffdb77	69	static void clk_fd_get_div(struct clk_hw hw, struct u32_fract fract)
e2d0e90f HK	70	{
	71	struct clk_fractional_divider *fd = to_clk_fd(hw);
	72	unsigned long flags = 0;
0777591e	73	unsigned long m, n;
141b3251	74	u32 mmask, nmask;
0777591e	75	u32 val;
e2d0e90f HK	76
	77	if (fd->lock)
	78	spin_lock_irqsave(fd->lock, flags);
661e2180 SB	79	else
661e2180 SB	80	__acquire(fd->lock);
e2d0e90f	81
58a2b4c9	82	val = clk_fd_readl(fd);
e2d0e90f HK	83
	84	if (fd->lock)
	85	spin_unlock_irqrestore(fd->lock, flags);
661e2180 SB	86	else
661e2180 SB	87	__release(fd->lock);
e2d0e90f	88
141b3251 CJ	89	mmask = GENMASK(fd->mwidth - 1, 0) << fd->mshift;
	90	nmask = GENMASK(fd->nwidth - 1, 0) << fd->nshift;
	91
	92	m = (val & mmask) >> fd->mshift;
	93	n = (val & nmask) >> fd->nshift;
e2d0e90f	94
e983da27 D	95	if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
	96	m++;
	97	n++;
	98	}
	99
7fffdb77 AS	100	fract->numerator = m;
	101	fract->denominator = n;
	102	}
	103
	104	static unsigned long clk_fd_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
	105	{
	106	struct u32_fract fract;
	107	u64 ret;
	108
	109	clk_fd_get_div(hw, &fract);
	110
	111	if (!fract.numerator \|\| !fract.denominator)
6b547836 HK	112	return parent_rate;
6b547836 HK	113
7fffdb77 AS	114	ret = (u64)parent_rate * fract.numerator;
7fffdb77 AS	115	do_div(ret, fract.denominator);
e2d0e90f HK	116
	117	return ret;
	118	}
	119
4e7cf74f AS	120	void clk_fractional_divider_general_approximation(struct clk_hw *hw,
	121	unsigned long rate,
	122	unsigned long *parent_rate,
	123	unsigned long m, unsigned long n)
e2d0e90f HK	124	{
e2d0e90f HK	125	struct clk_fractional_divider *fd = to_clk_fd(hw);
e2d0e90f	126
0777591e AS	127	/*
	128	* Get rate closer to *parent_rate to guarantee there is no overflow
	129	* for m and n. In the result it will be the nearest rate left shifted
	130	* by (scale - fd->nwidth) bits.
e81b917a AS	131	*
e81b917a AS	132	* For the detailed explanation see the top comment in this file.
0777591e	133	*/
82f53f9e AS	134	if (fd->flags & CLK_FRAC_DIVIDER_POWER_OF_TWO_PS) {
	135	unsigned long scale = fls_long(*parent_rate / rate - 1);
	136
	137	if (scale > fd->nwidth)
	138	rate <<= scale - fd->nwidth;
	139	}
e2d0e90f	140
0777591e AS	141	rational_best_approximation(rate, *parent_rate,
0777591e AS	142	GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
ec52e462 EZ	143	m, n);
	144	}
	145
	146	static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
	147	unsigned long *parent_rate)
	148	{
	149	struct clk_fractional_divider *fd = to_clk_fd(hw);
	150	unsigned long m, n;
	151	u64 ret;
	152
d13501a2	153	if (!rate \|\| (!clk_hw_can_set_rate_parent(hw) && rate >= *parent_rate))
ec52e462 EZ	154	return *parent_rate;
	155
	156	if (fd->approximation)
	157	fd->approximation(hw, rate, parent_rate, &m, &n);
	158	else
4e7cf74f	159	clk_fractional_divider_general_approximation(hw, rate, parent_rate, &m, &n);
e2d0e90f	160
0777591e AS	161	ret = (u64)parent_rate m;
	162	do_div(ret, n);
	163
	164	return ret;
e2d0e90f HK	165	}
	166
	167	static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
	168	unsigned long parent_rate)
	169	{
	170	struct clk_fractional_divider *fd = to_clk_fd(hw);
	171	unsigned long flags = 0;
0777591e	172	unsigned long m, n;
141b3251	173	u32 mmask, nmask;
e2d0e90f HK	174	u32 val;
e2d0e90f HK	175
0777591e AS	176	rational_best_approximation(rate, parent_rate,
	177	GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
	178	&m, &n);
e2d0e90f	179
e983da27 D	180	if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
	181	m--;
	182	n--;
	183	}
	184
e2d0e90f HK	185	if (fd->lock)
e2d0e90f HK	186	spin_lock_irqsave(fd->lock, flags);
661e2180 SB	187	else
661e2180 SB	188	__acquire(fd->lock);
e2d0e90f	189
141b3251 CJ	190	mmask = GENMASK(fd->mwidth - 1, 0) << fd->mshift;
	191	nmask = GENMASK(fd->nwidth - 1, 0) << fd->nshift;
	192
58a2b4c9	193	val = clk_fd_readl(fd);
141b3251	194	val &= ~(mmask \| nmask);
e2d0e90f	195	val \|= (m << fd->mshift) \| (n << fd->nshift);
58a2b4c9	196	clk_fd_writel(fd, val);
e2d0e90f HK	197
	198	if (fd->lock)
	199	spin_unlock_irqrestore(fd->lock, flags);
661e2180 SB	200	else
661e2180 SB	201	__release(fd->lock);
e2d0e90f HK	202
	203	return 0;
	204	}
	205
ac49a192 AS	206	#ifdef CONFIG_DEBUG_FS
	207	static int clk_fd_numerator_get(void hw, u64 val)
	208	{
	209	struct u32_fract fract;
	210
	211	clk_fd_get_div(hw, &fract);
	212
	213	*val = fract.numerator;
	214
	215	return 0;
	216	}
	217	DEFINE_DEBUGFS_ATTRIBUTE(clk_fd_numerator_fops, clk_fd_numerator_get, NULL, "%llu\n");
	218
	219	static int clk_fd_denominator_get(void hw, u64 val)
	220	{
	221	struct u32_fract fract;
	222
	223	clk_fd_get_div(hw, &fract);
	224
	225	*val = fract.denominator;
	226
	227	return 0;
	228	}
	229	DEFINE_DEBUGFS_ATTRIBUTE(clk_fd_denominator_fops, clk_fd_denominator_get, NULL, "%llu\n");
	230
	231	static void clk_fd_debug_init(struct clk_hw hw, struct dentry dentry)
	232	{
	233	debugfs_create_file("numerator", 0444, dentry, hw, &clk_fd_numerator_fops);
	234	debugfs_create_file("denominator", 0444, dentry, hw, &clk_fd_denominator_fops);
	235	}
	236	#endif
	237
e2d0e90f HK	238	const struct clk_ops clk_fractional_divider_ops = {
	239	.recalc_rate = clk_fd_recalc_rate,
	240	.round_rate = clk_fd_round_rate,
	241	.set_rate = clk_fd_set_rate,
ac49a192 AS	242	#ifdef CONFIG_DEBUG_FS
	243	.debug_init = clk_fd_debug_init,
	244	#endif
e2d0e90f HK	245	};
	246	EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
	247
39b44cff	248	struct clk_hw clk_hw_register_fractional_divider(struct device dev,
e2d0e90f HK	249	const char name, const char parent_name, unsigned long flags,
	250	void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
	251	u8 clk_divider_flags, spinlock_t *lock)
	252	{
	253	struct clk_fractional_divider *fd;
	254	struct clk_init_data init;
39b44cff SB	255	struct clk_hw *hw;
39b44cff SB	256	int ret;
e2d0e90f HK	257
e2d0e90f HK	258	fd = kzalloc(sizeof(*fd), GFP_KERNEL);
d122db7e	259	if (!fd)
e2d0e90f	260	return ERR_PTR(-ENOMEM);
e2d0e90f HK	261
	262	init.name = name;
	263	init.ops = &clk_fractional_divider_ops;
90b6c5c7	264	init.flags = flags;
e2d0e90f HK	265	init.parent_names = parent_name ? &parent_name : NULL;
	266	init.num_parents = parent_name ? 1 : 0;
	267
	268	fd->reg = reg;
	269	fd->mshift = mshift;
934e2536	270	fd->mwidth = mwidth;
e2d0e90f	271	fd->nshift = nshift;
934e2536	272	fd->nwidth = nwidth;
e2d0e90f HK	273	fd->flags = clk_divider_flags;
	274	fd->lock = lock;
	275	fd->hw.init = &init;
	276
39b44cff SB	277	hw = &fd->hw;
	278	ret = clk_hw_register(dev, hw);
	279	if (ret) {
e2d0e90f	280	kfree(fd);
39b44cff SB	281	hw = ERR_PTR(ret);
	282	}
	283
	284	return hw;
	285	}
	286	EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
e2d0e90f	287
39b44cff SB	288	struct clk clk_register_fractional_divider(struct device dev,
	289	const char name, const char parent_name, unsigned long flags,
	290	void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
	291	u8 clk_divider_flags, spinlock_t *lock)
	292	{
	293	struct clk_hw *hw;
	294
	295	hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
	296	reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
	297	lock);
	298	if (IS_ERR(hw))
	299	return ERR_CAST(hw);
	300	return hw->clk;
e2d0e90f HK	301	}
e2d0e90f HK	302	EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
39b44cff SB	303
	304	void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
	305	{
	306	struct clk_fractional_divider *fd;
	307
	308	fd = to_clk_fd(hw);
	309
	310	clk_hw_unregister(hw);
	311	kfree(fd);
	312	}