Branch data Line data Source code
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 : :
17 : 0 : static inline u32 clk_fd_readl(struct clk_fractional_divider *fd)
18 : : {
19 : 0 : if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
20 : 0 : return ioread32be(fd->reg);
21 : :
22 : 0 : return readl(fd->reg);
23 : : }
24 : :
25 : 0 : static inline void clk_fd_writel(struct clk_fractional_divider *fd, u32 val)
26 : : {
27 : 0 : if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
28 : 0 : iowrite32be(val, fd->reg);
29 : : else
30 : 0 : writel(val, fd->reg);
31 : : }
32 : :
33 : 0 : static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
34 : : unsigned long parent_rate)
35 : : {
36 : 0 : struct clk_fractional_divider *fd = to_clk_fd(hw);
37 : 0 : unsigned long flags = 0;
38 : 0 : unsigned long m, n;
39 : 0 : u32 val;
40 : 0 : u64 ret;
41 : :
42 [ # # ]: 0 : if (fd->lock)
43 : 0 : spin_lock_irqsave(fd->lock, flags);
44 : : else
45 : 0 : __acquire(fd->lock);
46 : :
47 [ # # ]: 0 : val = clk_fd_readl(fd);
48 : :
49 [ # # ]: 0 : if (fd->lock)
50 : 0 : spin_unlock_irqrestore(fd->lock, flags);
51 : : else
52 : 0 : __release(fd->lock);
53 : :
54 : 0 : m = (val & fd->mmask) >> fd->mshift;
55 : 0 : n = (val & fd->nmask) >> fd->nshift;
56 : :
57 [ # # ]: 0 : if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
58 : 0 : m++;
59 : 0 : n++;
60 : : }
61 : :
62 [ # # ]: 0 : if (!n || !m)
63 : : return parent_rate;
64 : :
65 : 0 : ret = (u64)parent_rate * m;
66 : 0 : do_div(ret, n);
67 : :
68 : 0 : return ret;
69 : : }
70 : :
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 : :
92 : 0 : static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
93 : : unsigned long *parent_rate)
94 : : {
95 : 0 : struct clk_fractional_divider *fd = to_clk_fd(hw);
96 : 0 : unsigned long m, n;
97 : 0 : u64 ret;
98 : :
99 [ # # # # : 0 : if (!rate || (!clk_hw_can_set_rate_parent(hw) && rate >= *parent_rate))
# # ]
100 : 0 : return *parent_rate;
101 : :
102 [ # # ]: 0 : if (fd->approximation)
103 : 0 : fd->approximation(hw, rate, parent_rate, &m, &n);
104 : : else
105 : 0 : clk_fd_general_approximation(hw, rate, parent_rate, &m, &n);
106 : :
107 : 0 : ret = (u64)*parent_rate * m;
108 : 0 : do_div(ret, n);
109 : :
110 : 0 : return ret;
111 : : }
112 : :
113 : 0 : static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
114 : : unsigned long parent_rate)
115 : : {
116 : 0 : struct clk_fractional_divider *fd = to_clk_fd(hw);
117 : 0 : unsigned long flags = 0;
118 : 0 : unsigned long m, n;
119 : 0 : u32 val;
120 : :
121 : 0 : rational_best_approximation(rate, parent_rate,
122 : 0 : GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
123 : : &m, &n);
124 : :
125 [ # # ]: 0 : if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
126 : 0 : m--;
127 : 0 : n--;
128 : : }
129 : :
130 [ # # ]: 0 : if (fd->lock)
131 : 0 : spin_lock_irqsave(fd->lock, flags);
132 : : else
133 : 0 : __acquire(fd->lock);
134 : :
135 [ # # ]: 0 : val = clk_fd_readl(fd);
136 : 0 : val &= ~(fd->mmask | fd->nmask);
137 : 0 : val |= (m << fd->mshift) | (n << fd->nshift);
138 [ # # ]: 0 : clk_fd_writel(fd, val);
139 : :
140 [ # # ]: 0 : if (fd->lock)
141 : 0 : spin_unlock_irqrestore(fd->lock, flags);
142 : : else
143 : 0 : __release(fd->lock);
144 : :
145 : 0 : return 0;
146 : : }
147 : :
148 : : const 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 : : };
153 : : EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
154 : :
155 : 0 : struct 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 : 0 : struct clk_fractional_divider *fd;
161 : 0 : struct clk_init_data init;
162 : 0 : struct clk_hw *hw;
163 : 0 : int ret;
164 : :
165 : 0 : fd = kzalloc(sizeof(*fd), GFP_KERNEL);
166 [ # # ]: 0 : if (!fd)
167 : : return ERR_PTR(-ENOMEM);
168 : :
169 : 0 : init.name = name;
170 : 0 : init.ops = &clk_fractional_divider_ops;
171 : 0 : init.flags = flags;
172 [ # # ]: 0 : init.parent_names = parent_name ? &parent_name : NULL;
173 : 0 : init.num_parents = parent_name ? 1 : 0;
174 : :
175 : 0 : fd->reg = reg;
176 : 0 : fd->mshift = mshift;
177 : 0 : fd->mwidth = mwidth;
178 : 0 : fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
179 : 0 : fd->nshift = nshift;
180 : 0 : fd->nwidth = nwidth;
181 : 0 : fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
182 : 0 : fd->flags = clk_divider_flags;
183 : 0 : fd->lock = lock;
184 : 0 : fd->hw.init = &init;
185 : :
186 : 0 : hw = &fd->hw;
187 : 0 : ret = clk_hw_register(dev, hw);
188 [ # # ]: 0 : if (ret) {
189 : 0 : kfree(fd);
190 : 0 : hw = ERR_PTR(ret);
191 : : }
192 : :
193 : : return hw;
194 : : }
195 : : EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
196 : :
197 : 0 : struct 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 : 0 : struct clk_hw *hw;
203 : :
204 : 0 : hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
205 : : reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
206 : : lock);
207 [ # # ]: 0 : if (IS_ERR(hw))
208 : : return ERR_CAST(hw);
209 : 0 : return hw->clk;
210 : : }
211 : : EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
212 : :
213 : 0 : void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
214 : : {
215 : 0 : struct clk_fractional_divider *fd;
216 : :
217 : 0 : fd = to_clk_fd(hw);
218 : :
219 : 0 : clk_hw_unregister(hw);
220 : 0 : kfree(fd);
221 : 0 : }
|
