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
clk_fd_readl(struct clk_fractional_divider * fd)17 static 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
clk_fd_writel(struct clk_fractional_divider * fd,u32 val)25 static 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
clk_fd_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)33 static 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
clk_fd_general_approximation(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate,unsigned long * m,unsigned long * n)71 static 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
clk_fd_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)92 static 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 && 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
clk_fd_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)113 static 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 * When compensation the fractional divider,
131 * the [1:0] bits of the numerator register are omitted,
132 * which will lead to a large deviation in the result.
133 * Therefore, it is required that the numerator must
134 * be greater than 4.
135 *
136 * Note that there are some exceptions here:
137 * If there is an even frac div, we need to keep the original
138 * numerator(<4) and denominator. Otherwise, it may cause the
139 * issue that the duty ratio is not 50%.
140 */
141 if (m < 4 && m != 0) {
142 if (n % 2 == 0)
143 val = 1;
144 else
145 val = DIV_ROUND_UP(4, m);
146
147 n *= val;
148 m *= val;
149 if (n > fd->nmask) {
150 pr_debug("%s n(%ld) is overflow, use mask value\n",
151 __func__, n);
152 n = fd->nmask;
153 }
154 }
155
156 if (fd->lock)
157 spin_lock_irqsave(fd->lock, flags);
158 else
159 __acquire(fd->lock);
160
161 val = clk_fd_readl(fd);
162 val &= ~(fd->mmask | fd->nmask);
163 val |= (m << fd->mshift) | (n << fd->nshift);
164 clk_fd_writel(fd, val);
165
166 if (fd->lock)
167 spin_unlock_irqrestore(fd->lock, flags);
168 else
169 __release(fd->lock);
170
171 return 0;
172 }
173
174 const struct clk_ops clk_fractional_divider_ops = {
175 .recalc_rate = clk_fd_recalc_rate,
176 .round_rate = clk_fd_round_rate,
177 .set_rate = clk_fd_set_rate,
178 };
179 EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
180
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)181 struct clk_hw *clk_hw_register_fractional_divider(struct device *dev,
182 const char *name, const char *parent_name, unsigned long flags,
183 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
184 u8 clk_divider_flags, spinlock_t *lock)
185 {
186 struct clk_fractional_divider *fd;
187 struct clk_init_data init;
188 struct clk_hw *hw;
189 int ret;
190
191 fd = kzalloc(sizeof(*fd), GFP_KERNEL);
192 if (!fd)
193 return ERR_PTR(-ENOMEM);
194
195 init.name = name;
196 init.ops = &clk_fractional_divider_ops;
197 init.flags = flags;
198 init.parent_names = parent_name ? &parent_name : NULL;
199 init.num_parents = parent_name ? 1 : 0;
200
201 fd->reg = reg;
202 fd->mshift = mshift;
203 fd->mwidth = mwidth;
204 fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
205 fd->nshift = nshift;
206 fd->nwidth = nwidth;
207 fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
208 fd->flags = clk_divider_flags;
209 fd->lock = lock;
210 fd->hw.init = &init;
211
212 hw = &fd->hw;
213 ret = clk_hw_register(dev, hw);
214 if (ret) {
215 kfree(fd);
216 hw = ERR_PTR(ret);
217 }
218
219 return hw;
220 }
221 EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
222
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)223 struct clk *clk_register_fractional_divider(struct device *dev,
224 const char *name, const char *parent_name, unsigned long flags,
225 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
226 u8 clk_divider_flags, spinlock_t *lock)
227 {
228 struct clk_hw *hw;
229
230 hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
231 reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
232 lock);
233 if (IS_ERR(hw))
234 return ERR_CAST(hw);
235 return hw->clk;
236 }
237 EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
238
clk_hw_unregister_fractional_divider(struct clk_hw * hw)239 void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
240 {
241 struct clk_fractional_divider *fd;
242
243 fd = to_clk_fd(hw);
244
245 clk_hw_unregister(hw);
246 kfree(fd);
247 }
248