1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Copyright (C) 2014 Intel Corporation
  4 *
  5 * Adjustable fractional divider clock implementation.
  6 * Output rate = (m / n) * parent_rate.
  7 * Uses rational best approximation algorithm.
  8 */
  9
 10#include <linux/clk-provider.h>
 11#include <linux/io.h>
 12#include <linux/module.h>
 13#include <linux/device.h>
 14#include <linux/slab.h>
 15#include <linux/rational.h>
 16
 17static inline u32 clk_fd_readl(struct clk_fractional_divider *fd)
 18{
 19	if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
 20		return ioread32be(fd->reg);
 21
 22	return readl(fd->reg);
 23}
 24
 25static inline void clk_fd_writel(struct clk_fractional_divider *fd, u32 val)
 26{
 27	if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
 28		iowrite32be(val, fd->reg);
 29	else
 30		writel(val, fd->reg);
 31}
 32
 33static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
 34					unsigned long parent_rate)
 35{
 36	struct clk_fractional_divider *fd = to_clk_fd(hw);
 37	unsigned long flags = 0;
 38	unsigned long m, n;
 39	u32 val;
 40	u64 ret;
 41
 42	if (fd->lock)
 43		spin_lock_irqsave(fd->lock, flags);
 44	else
 45		__acquire(fd->lock);
 46
 47	val = clk_fd_readl(fd);
 48
 49	if (fd->lock)
 50		spin_unlock_irqrestore(fd->lock, flags);
 51	else
 52		__release(fd->lock);
 53
 54	m = (val & fd->mmask) >> fd->mshift;
 55	n = (val & fd->nmask) >> fd->nshift;
 56
 57	if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
 58		m++;
 59		n++;
 60	}
 61
 62	if (!n || !m)
 63		return parent_rate;
 64
 65	ret = (u64)parent_rate * m;
 66	do_div(ret, n);
 67
 68	return ret;
 69}
 70
 71static void clk_fd_general_approximation(struct clk_hw *hw, unsigned long rate,
 72					 unsigned long *parent_rate,
 73					 unsigned long *m, unsigned long *n)
 74{
 75	struct clk_fractional_divider *fd = to_clk_fd(hw);
 76	unsigned long scale;
 77
 78	/*
 79	 * Get rate closer to *parent_rate to guarantee there is no overflow
 80	 * for m and n. In the result it will be the nearest rate left shifted
 81	 * by (scale - fd->nwidth) bits.
 82	 */
 83	scale = fls_long(*parent_rate / rate - 1);
 84	if (scale > fd->nwidth)
 85		rate <<= scale - fd->nwidth;
 86
 87	rational_best_approximation(rate, *parent_rate,
 88			GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
 89			m, n);
 90}
 91
 92static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
 93			      unsigned long *parent_rate)
 94{
 95	struct clk_fractional_divider *fd = to_clk_fd(hw);
 96	unsigned long m, n;
 97	u64 ret;
 98
 99	if (!rate || (!clk_hw_can_set_rate_parent(hw) && rate >= *parent_rate))
100		return *parent_rate;
101
102	if (fd->approximation)
103		fd->approximation(hw, rate, parent_rate, &m, &n);
104	else
105		clk_fd_general_approximation(hw, rate, parent_rate, &m, &n);
106
107	ret = (u64)*parent_rate * m;
108	do_div(ret, n);
109
110	return ret;
111}
112
113static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
114			   unsigned long parent_rate)
115{
116	struct clk_fractional_divider *fd = to_clk_fd(hw);
117	unsigned long flags = 0;
118	unsigned long m, n;
119	u32 val;
120
121	rational_best_approximation(rate, parent_rate,
122			GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
123			&m, &n);
124
125	if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
126		m--;
127		n--;
128	}
129
130	if (fd->lock)
131		spin_lock_irqsave(fd->lock, flags);
132	else
133		__acquire(fd->lock);
134
135	val = clk_fd_readl(fd);
136	val &= ~(fd->mmask | fd->nmask);
137	val |= (m << fd->mshift) | (n << fd->nshift);
138	clk_fd_writel(fd, val);
139
140	if (fd->lock)
141		spin_unlock_irqrestore(fd->lock, flags);
142	else
143		__release(fd->lock);
144
145	return 0;
146}
147
148const struct clk_ops clk_fractional_divider_ops = {
149	.recalc_rate = clk_fd_recalc_rate,
150	.round_rate = clk_fd_round_rate,
151	.set_rate = clk_fd_set_rate,
152};
153EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
154
155struct clk_hw *clk_hw_register_fractional_divider(struct device *dev,
156		const char *name, const char *parent_name, unsigned long flags,
157		void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
158		u8 clk_divider_flags, spinlock_t *lock)
159{
160	struct clk_fractional_divider *fd;
161	struct clk_init_data init;
162	struct clk_hw *hw;
163	int ret;
164
165	fd = kzalloc(sizeof(*fd), GFP_KERNEL);
166	if (!fd)
167		return ERR_PTR(-ENOMEM);
168
169	init.name = name;
170	init.ops = &clk_fractional_divider_ops;
171	init.flags = flags;
172	init.parent_names = parent_name ? &parent_name : NULL;
173	init.num_parents = parent_name ? 1 : 0;
174
175	fd->reg = reg;
176	fd->mshift = mshift;
177	fd->mwidth = mwidth;
178	fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
179	fd->nshift = nshift;
180	fd->nwidth = nwidth;
181	fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
182	fd->flags = clk_divider_flags;
183	fd->lock = lock;
184	fd->hw.init = &init;
185
186	hw = &fd->hw;
187	ret = clk_hw_register(dev, hw);
188	if (ret) {
189		kfree(fd);
190		hw = ERR_PTR(ret);
191	}
192
193	return hw;
194}
195EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
196
197struct clk *clk_register_fractional_divider(struct device *dev,
198		const char *name, const char *parent_name, unsigned long flags,
199		void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
200		u8 clk_divider_flags, spinlock_t *lock)
201{
202	struct clk_hw *hw;
203
204	hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
205			reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
206			lock);
207	if (IS_ERR(hw))
208		return ERR_CAST(hw);
209	return hw->clk;
210}
211EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
212
213void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
214{
215	struct clk_fractional_divider *fd;
216
217	fd = to_clk_fd(hw);
218
219	clk_hw_unregister(hw);
220	kfree(fd);
221}