1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Copyright (c) 2018 Fuzhou Rockchip Electronics Co., Ltd |
4 | */ |
5 | |
6 | #include <linux/clk-provider.h> |
7 | #include <linux/io.h> |
8 | #include <linux/slab.h> |
9 | #include "clk.h" |
10 | |
11 | #define div_mask(width) ((1 << (width)) - 1) |
12 | |
13 | static bool _is_best_half_div(unsigned long rate, unsigned long now, |
14 | unsigned long best, unsigned long flags) |
15 | { |
16 | if (flags & CLK_DIVIDER_ROUND_CLOSEST) |
17 | return abs(rate - now) < abs(rate - best); |
18 | |
19 | return now <= rate && now > best; |
20 | } |
21 | |
22 | static unsigned long clk_half_divider_recalc_rate(struct clk_hw *hw, |
23 | unsigned long parent_rate) |
24 | { |
25 | struct clk_divider *divider = to_clk_divider(hw); |
26 | unsigned int val; |
27 | |
28 | val = readl(addr: divider->reg) >> divider->shift; |
29 | val &= div_mask(divider->width); |
30 | val = val * 2 + 3; |
31 | |
32 | return DIV_ROUND_UP_ULL(((u64)parent_rate * 2), val); |
33 | } |
34 | |
35 | static int clk_half_divider_bestdiv(struct clk_hw *hw, unsigned long rate, |
36 | unsigned long *best_parent_rate, u8 width, |
37 | unsigned long flags) |
38 | { |
39 | unsigned int i, bestdiv = 0; |
40 | unsigned long parent_rate, best = 0, now, maxdiv; |
41 | unsigned long parent_rate_saved = *best_parent_rate; |
42 | |
43 | if (!rate) |
44 | rate = 1; |
45 | |
46 | maxdiv = div_mask(width); |
47 | |
48 | if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) { |
49 | parent_rate = *best_parent_rate; |
50 | bestdiv = DIV_ROUND_UP_ULL(((u64)parent_rate * 2), rate); |
51 | if (bestdiv < 3) |
52 | bestdiv = 0; |
53 | else |
54 | bestdiv = (bestdiv - 3) / 2; |
55 | bestdiv = bestdiv > maxdiv ? maxdiv : bestdiv; |
56 | return bestdiv; |
57 | } |
58 | |
59 | /* |
60 | * The maximum divider we can use without overflowing |
61 | * unsigned long in rate * i below |
62 | */ |
63 | maxdiv = min(ULONG_MAX / rate, maxdiv); |
64 | |
65 | for (i = 0; i <= maxdiv; i++) { |
66 | if (((u64)rate * (i * 2 + 3)) == ((u64)parent_rate_saved * 2)) { |
67 | /* |
68 | * It's the most ideal case if the requested rate can be |
69 | * divided from parent clock without needing to change |
70 | * parent rate, so return the divider immediately. |
71 | */ |
72 | *best_parent_rate = parent_rate_saved; |
73 | return i; |
74 | } |
75 | parent_rate = clk_hw_round_rate(hw: clk_hw_get_parent(hw), |
76 | rate: ((u64)rate * (i * 2 + 3)) / 2); |
77 | now = DIV_ROUND_UP_ULL(((u64)parent_rate * 2), |
78 | (i * 2 + 3)); |
79 | |
80 | if (_is_best_half_div(rate, now, best, flags)) { |
81 | bestdiv = i; |
82 | best = now; |
83 | *best_parent_rate = parent_rate; |
84 | } |
85 | } |
86 | |
87 | if (!bestdiv) { |
88 | bestdiv = div_mask(width); |
89 | *best_parent_rate = clk_hw_round_rate(hw: clk_hw_get_parent(hw), rate: 1); |
90 | } |
91 | |
92 | return bestdiv; |
93 | } |
94 | |
95 | static long clk_half_divider_round_rate(struct clk_hw *hw, unsigned long rate, |
96 | unsigned long *prate) |
97 | { |
98 | struct clk_divider *divider = to_clk_divider(hw); |
99 | int div; |
100 | |
101 | div = clk_half_divider_bestdiv(hw, rate, best_parent_rate: prate, |
102 | width: divider->width, |
103 | flags: divider->flags); |
104 | |
105 | return DIV_ROUND_UP_ULL(((u64)*prate * 2), div * 2 + 3); |
106 | } |
107 | |
108 | static int clk_half_divider_set_rate(struct clk_hw *hw, unsigned long rate, |
109 | unsigned long parent_rate) |
110 | { |
111 | struct clk_divider *divider = to_clk_divider(hw); |
112 | unsigned int value; |
113 | unsigned long flags = 0; |
114 | u32 val; |
115 | |
116 | value = DIV_ROUND_UP_ULL(((u64)parent_rate * 2), rate); |
117 | value = (value - 3) / 2; |
118 | value = min_t(unsigned int, value, div_mask(divider->width)); |
119 | |
120 | if (divider->lock) |
121 | spin_lock_irqsave(divider->lock, flags); |
122 | else |
123 | __acquire(divider->lock); |
124 | |
125 | if (divider->flags & CLK_DIVIDER_HIWORD_MASK) { |
126 | val = div_mask(divider->width) << (divider->shift + 16); |
127 | } else { |
128 | val = readl(addr: divider->reg); |
129 | val &= ~(div_mask(divider->width) << divider->shift); |
130 | } |
131 | val |= value << divider->shift; |
132 | writel(val, addr: divider->reg); |
133 | |
134 | if (divider->lock) |
135 | spin_unlock_irqrestore(lock: divider->lock, flags); |
136 | else |
137 | __release(divider->lock); |
138 | |
139 | return 0; |
140 | } |
141 | |
142 | static const struct clk_ops clk_half_divider_ops = { |
143 | .recalc_rate = clk_half_divider_recalc_rate, |
144 | .round_rate = clk_half_divider_round_rate, |
145 | .set_rate = clk_half_divider_set_rate, |
146 | }; |
147 | |
148 | /* |
149 | * Register a clock branch. |
150 | * Most clock branches have a form like |
151 | * |
152 | * src1 --|--\ |
153 | * |M |--[GATE]-[DIV]- |
154 | * src2 --|--/ |
155 | * |
156 | * sometimes without one of those components. |
157 | */ |
158 | struct clk *rockchip_clk_register_halfdiv(const char *name, |
159 | const char *const *parent_names, |
160 | u8 num_parents, void __iomem *base, |
161 | int muxdiv_offset, u8 mux_shift, |
162 | u8 mux_width, u8 mux_flags, |
163 | u8 div_shift, u8 div_width, |
164 | u8 div_flags, int gate_offset, |
165 | u8 gate_shift, u8 gate_flags, |
166 | unsigned long flags, |
167 | spinlock_t *lock) |
168 | { |
169 | struct clk_hw *hw = ERR_PTR(error: -ENOMEM); |
170 | struct clk_mux *mux = NULL; |
171 | struct clk_gate *gate = NULL; |
172 | struct clk_divider *div = NULL; |
173 | const struct clk_ops *mux_ops = NULL, *div_ops = NULL, |
174 | *gate_ops = NULL; |
175 | |
176 | if (num_parents > 1) { |
177 | mux = kzalloc(size: sizeof(*mux), GFP_KERNEL); |
178 | if (!mux) |
179 | return ERR_PTR(error: -ENOMEM); |
180 | |
181 | mux->reg = base + muxdiv_offset; |
182 | mux->shift = mux_shift; |
183 | mux->mask = BIT(mux_width) - 1; |
184 | mux->flags = mux_flags; |
185 | mux->lock = lock; |
186 | mux_ops = (mux_flags & CLK_MUX_READ_ONLY) ? &clk_mux_ro_ops |
187 | : &clk_mux_ops; |
188 | } |
189 | |
190 | if (gate_offset >= 0) { |
191 | gate = kzalloc(size: sizeof(*gate), GFP_KERNEL); |
192 | if (!gate) |
193 | goto err_gate; |
194 | |
195 | gate->flags = gate_flags; |
196 | gate->reg = base + gate_offset; |
197 | gate->bit_idx = gate_shift; |
198 | gate->lock = lock; |
199 | gate_ops = &clk_gate_ops; |
200 | } |
201 | |
202 | if (div_width > 0) { |
203 | div = kzalloc(size: sizeof(*div), GFP_KERNEL); |
204 | if (!div) |
205 | goto err_div; |
206 | |
207 | div->flags = div_flags; |
208 | div->reg = base + muxdiv_offset; |
209 | div->shift = div_shift; |
210 | div->width = div_width; |
211 | div->lock = lock; |
212 | div_ops = &clk_half_divider_ops; |
213 | } |
214 | |
215 | hw = clk_hw_register_composite(NULL, name, parent_names, num_parents, |
216 | mux_hw: mux ? &mux->hw : NULL, mux_ops, |
217 | rate_hw: div ? &div->hw : NULL, rate_ops: div_ops, |
218 | gate_hw: gate ? &gate->hw : NULL, gate_ops, |
219 | flags); |
220 | if (IS_ERR(ptr: hw)) |
221 | goto err_div; |
222 | |
223 | return hw->clk; |
224 | err_div: |
225 | kfree(objp: gate); |
226 | err_gate: |
227 | kfree(objp: mux); |
228 | return ERR_CAST(ptr: hw); |
229 | } |
230 | |