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
_is_best_half_div(unsigned long rate,unsigned long now,unsigned long best,unsigned long flags)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
clk_half_divider_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)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(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
clk_half_divider_bestdiv(struct clk_hw * hw,unsigned long rate,unsigned long * best_parent_rate,u8 width,unsigned long flags)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 bool is_bestdiv = false;
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 = DIV_ROUND_UP(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 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
67 ((u64)rate * (i * 2 + 3)) / 2);
68 now = DIV_ROUND_UP_ULL(((u64)parent_rate * 2),
69 (i * 2 + 3));
70
71 if (_is_best_half_div(rate, now, best, flags)) {
72 is_bestdiv = true;
73 bestdiv = i;
74 best = now;
75 *best_parent_rate = parent_rate;
76 }
77 }
78
79 if (!is_bestdiv) {
80 bestdiv = div_mask(width);
81 *best_parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), 1);
82 }
83
84 return bestdiv;
85 }
86
clk_half_divider_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * prate)87 static long clk_half_divider_round_rate(struct clk_hw *hw, unsigned long rate,
88 unsigned long *prate)
89 {
90 struct clk_divider *divider = to_clk_divider(hw);
91 int div;
92
93 div = clk_half_divider_bestdiv(hw, rate, prate,
94 divider->width,
95 divider->flags);
96
97 return DIV_ROUND_UP_ULL(((u64)*prate * 2), div * 2 + 3);
98 }
99
clk_half_divider_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)100 static int clk_half_divider_set_rate(struct clk_hw *hw, unsigned long rate,
101 unsigned long parent_rate)
102 {
103 struct clk_divider *divider = to_clk_divider(hw);
104 unsigned int value;
105 unsigned long flags = 0;
106 u32 val;
107
108 value = DIV_ROUND_UP_ULL(((u64)parent_rate * 2), rate);
109 value = DIV_ROUND_UP(value - 3, 2);
110 value = min_t(unsigned int, value, div_mask(divider->width));
111
112 if (divider->lock)
113 spin_lock_irqsave(divider->lock, flags);
114 else
115 __acquire(divider->lock);
116
117 if (divider->flags & CLK_DIVIDER_HIWORD_MASK) {
118 val = div_mask(divider->width) << (divider->shift + 16);
119 } else {
120 val = readl(divider->reg);
121 val &= ~(div_mask(divider->width) << divider->shift);
122 }
123 val |= value << divider->shift;
124 writel(val, divider->reg);
125
126 if (divider->lock)
127 spin_unlock_irqrestore(divider->lock, flags);
128 else
129 __release(divider->lock);
130
131 return 0;
132 }
133
134 static const struct clk_ops clk_half_divider_ops = {
135 .recalc_rate = clk_half_divider_recalc_rate,
136 .round_rate = clk_half_divider_round_rate,
137 .set_rate = clk_half_divider_set_rate,
138 };
139
140 /**
141 * Register a clock branch.
142 * Most clock branches have a form like
143 *
144 * src1 --|--\
145 * |M |--[GATE]-[DIV]-
146 * src2 --|--/
147 *
148 * sometimes without one of those components.
149 */
rockchip_clk_register_halfdiv(const char * name,const char * const * parent_names,u8 num_parents,void __iomem * base,int muxdiv_offset,u8 mux_shift,u8 mux_width,u8 mux_flags,int div_offset,u8 div_shift,u8 div_width,u8 div_flags,int gate_offset,u8 gate_shift,u8 gate_flags,unsigned long flags,spinlock_t * lock)150 struct clk *rockchip_clk_register_halfdiv(const char *name,
151 const char *const *parent_names,
152 u8 num_parents, void __iomem *base,
153 int muxdiv_offset, u8 mux_shift,
154 u8 mux_width, u8 mux_flags,
155 int div_offset, u8 div_shift,
156 u8 div_width, u8 div_flags,
157 int gate_offset, u8 gate_shift,
158 u8 gate_flags, unsigned long flags,
159 spinlock_t *lock)
160 {
161 struct clk_hw *hw = ERR_PTR(-ENOMEM);
162 struct clk_mux *mux = NULL;
163 struct clk_gate *gate = NULL;
164 struct clk_divider *div = NULL;
165 const struct clk_ops *mux_ops = NULL, *div_ops = NULL,
166 *gate_ops = NULL;
167
168 if (num_parents > 1) {
169 mux = kzalloc(sizeof(*mux), GFP_KERNEL);
170 if (!mux)
171 return ERR_PTR(-ENOMEM);
172
173 mux->reg = base + muxdiv_offset;
174 mux->shift = mux_shift;
175 mux->mask = BIT(mux_width) - 1;
176 mux->flags = mux_flags;
177 mux->lock = lock;
178 mux_ops = (mux_flags & CLK_MUX_READ_ONLY) ? &clk_mux_ro_ops
179 : &clk_mux_ops;
180 }
181
182 if (gate_offset >= 0) {
183 gate = kzalloc(sizeof(*gate), GFP_KERNEL);
184 if (!gate)
185 goto err_gate;
186
187 gate->flags = gate_flags;
188 gate->reg = base + gate_offset;
189 gate->bit_idx = gate_shift;
190 gate->lock = lock;
191 gate_ops = &clk_gate_ops;
192 }
193
194 if (div_width > 0) {
195 div = kzalloc(sizeof(*div), GFP_KERNEL);
196 if (!div)
197 goto err_div;
198
199 div->flags = div_flags;
200 if (div_offset)
201 div->reg = base + div_offset;
202 else
203 div->reg = base + muxdiv_offset;
204 div->shift = div_shift;
205 div->width = div_width;
206 div->lock = lock;
207 div_ops = &clk_half_divider_ops;
208 }
209
210 hw = clk_hw_register_composite(NULL, name, parent_names, num_parents,
211 mux ? &mux->hw : NULL, mux_ops,
212 div ? &div->hw : NULL, div_ops,
213 gate ? &gate->hw : NULL, gate_ops,
214 flags);
215 if (IS_ERR(hw))
216 goto err_div;
217
218 return hw->clk;
219 err_div:
220 kfree(gate);
221 err_gate:
222 kfree(mux);
223 return ERR_CAST(hw);
224 }
225