1
2
3
4
5
6
7
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
80
81
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}
222
