1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (C) 2014 Intel Corporation
4  *
5  * Adjustable fractional divider clock implementation.
6  * Uses rational best approximation algorithm.
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
37  * at the same time a much better result of m and n than simple
38  * saturated values.
39  */
40 
41 #include <linux/clk-provider.h>
42 #include <linux/io.h>
43 #include <linux/module.h>
44 #include <linux/device.h>
45 #include <linux/slab.h>
46 #include <linux/rational.h>
47 
48 #include "clk-fractional-divider.h"
49 
clk_fd_readl(struct clk_fractional_divider * fd)50 static inline u32 clk_fd_readl(struct clk_fractional_divider *fd)
51 {
52 	if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
53 		return ioread32be(fd->reg);
54 
55 	return readl(fd->reg);
56 }
57 
clk_fd_writel(struct clk_fractional_divider * fd,u32 val)58 static inline void clk_fd_writel(struct clk_fractional_divider *fd, u32 val)
59 {
60 	if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
61 		iowrite32be(val, fd->reg);
62 	else
63 		writel(val, fd->reg);
64 }
65 
clk_fd_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)66 static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
67 					unsigned long parent_rate)
68 {
69 	struct clk_fractional_divider *fd = to_clk_fd(hw);
70 	unsigned long flags = 0;
71 	unsigned long m, n;
72 	u32 val;
73 	u64 ret;
74 
75 	if (fd->lock)
76 		spin_lock_irqsave(fd->lock, flags);
77 	else
78 		__acquire(fd->lock);
79 
80 	val = clk_fd_readl(fd);
81 
82 	if (fd->lock)
83 		spin_unlock_irqrestore(fd->lock, flags);
84 	else
85 		__release(fd->lock);
86 
87 	m = (val & fd->mmask) >> fd->mshift;
88 	n = (val & fd->nmask) >> fd->nshift;
89 
90 	if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
91 		m++;
92 		n++;
93 	}
94 
95 	if (!n || !m)
96 		return parent_rate;
97 
98 	ret = (u64)parent_rate * m;
99 	do_div(ret, n);
100 
101 	return ret;
102 }
103 
clk_fractional_divider_general_approximation(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate,unsigned long * m,unsigned long * n)104 void clk_fractional_divider_general_approximation(struct clk_hw *hw,
105 						  unsigned long rate,
106 						  unsigned long *parent_rate,
107 						  unsigned long *m, unsigned long *n)
108 {
109 	struct clk_fractional_divider *fd = to_clk_fd(hw);
110 
111 	/*
112 	 * Get rate closer to *parent_rate to guarantee there is no overflow
113 	 * for m and n. In the result it will be the nearest rate left shifted
114 	 * by (scale - fd->nwidth) bits.
115 	 *
116 	 * For the detailed explanation see the top comment in this file.
117 	 */
118 	if (fd->flags & CLK_FRAC_DIVIDER_POWER_OF_TWO_PS) {
119 		unsigned long scale = fls_long(*parent_rate / rate - 1);
120 
121 		if (scale > fd->nwidth)
122 			rate <<= scale - fd->nwidth;
123 	}
124 
125 	rational_best_approximation(rate, *parent_rate,
126 			GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
127 			m, n);
128 }
129 
clk_fd_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)130 static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
131 			      unsigned long *parent_rate)
132 {
133 	struct clk_fractional_divider *fd = to_clk_fd(hw);
134 	unsigned long m, n;
135 	u64 ret;
136 
137 	if (!rate || (!clk_hw_can_set_rate_parent(hw) && rate >= *parent_rate))
138 		return *parent_rate;
139 
140 	if (fd->approximation)
141 		fd->approximation(hw, rate, parent_rate, &m, &n);
142 	else
143 		clk_fractional_divider_general_approximation(hw, rate, parent_rate, &m, &n);
144 
145 	ret = (u64)*parent_rate * m;
146 	do_div(ret, n);
147 
148 	return ret;
149 }
150 
clk_fd_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)151 static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
152 			   unsigned long parent_rate)
153 {
154 	struct clk_fractional_divider *fd = to_clk_fd(hw);
155 	unsigned long flags = 0;
156 	unsigned long m, n;
157 	u32 val;
158 
159 	rational_best_approximation(rate, parent_rate,
160 			GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
161 			&m, &n);
162 
163 	if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
164 		m--;
165 		n--;
166 	}
167 
168 	if (fd->lock)
169 		spin_lock_irqsave(fd->lock, flags);
170 	else
171 		__acquire(fd->lock);
172 
173 	val = clk_fd_readl(fd);
174 	val &= ~(fd->mmask | fd->nmask);
175 	val |= (m << fd->mshift) | (n << fd->nshift);
176 	clk_fd_writel(fd, val);
177 
178 	if (fd->lock)
179 		spin_unlock_irqrestore(fd->lock, flags);
180 	else
181 		__release(fd->lock);
182 
183 	return 0;
184 }
185 
186 const struct clk_ops clk_fractional_divider_ops = {
187 	.recalc_rate = clk_fd_recalc_rate,
188 	.round_rate = clk_fd_round_rate,
189 	.set_rate = clk_fd_set_rate,
190 };
191 EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
192 
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)193 struct clk_hw *clk_hw_register_fractional_divider(struct device *dev,
194 		const char *name, const char *parent_name, unsigned long flags,
195 		void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
196 		u8 clk_divider_flags, spinlock_t *lock)
197 {
198 	struct clk_fractional_divider *fd;
199 	struct clk_init_data init;
200 	struct clk_hw *hw;
201 	int ret;
202 
203 	fd = kzalloc(sizeof(*fd), GFP_KERNEL);
204 	if (!fd)
205 		return ERR_PTR(-ENOMEM);
206 
207 	init.name = name;
208 	init.ops = &clk_fractional_divider_ops;
209 	init.flags = flags;
210 	init.parent_names = parent_name ? &parent_name : NULL;
211 	init.num_parents = parent_name ? 1 : 0;
212 
213 	fd->reg = reg;
214 	fd->mshift = mshift;
215 	fd->mwidth = mwidth;
216 	fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
217 	fd->nshift = nshift;
218 	fd->nwidth = nwidth;
219 	fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
220 	fd->flags = clk_divider_flags;
221 	fd->lock = lock;
222 	fd->hw.init = &init;
223 
224 	hw = &fd->hw;
225 	ret = clk_hw_register(dev, hw);
226 	if (ret) {
227 		kfree(fd);
228 		hw = ERR_PTR(ret);
229 	}
230 
231 	return hw;
232 }
233 EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
234 
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)235 struct clk *clk_register_fractional_divider(struct device *dev,
236 		const char *name, const char *parent_name, unsigned long flags,
237 		void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
238 		u8 clk_divider_flags, spinlock_t *lock)
239 {
240 	struct clk_hw *hw;
241 
242 	hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
243 			reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
244 			lock);
245 	if (IS_ERR(hw))
246 		return ERR_CAST(hw);
247 	return hw->clk;
248 }
249 EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
250 
clk_hw_unregister_fractional_divider(struct clk_hw * hw)251 void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
252 {
253 	struct clk_fractional_divider *fd;
254 
255 	fd = to_clk_fd(hw);
256 
257 	clk_hw_unregister(hw);
258 	kfree(fd);
259 }
260