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(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(clk_hw_get_parent(hw),
76 						((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(clk_hw_get_parent(hw), 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, prate,
102 				       divider->width,
103 				       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(divider->reg);
129 		val &= ~(div_mask(divider->width) << divider->shift);
130 	}
131 	val |= value << divider->shift;
132 	writel(val, divider->reg);
133 
134 	if (divider->lock)
135 		spin_unlock_irqrestore(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(-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(sizeof(*mux), GFP_KERNEL);
178 		if (!mux)
179 			return ERR_PTR(-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(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(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 ? &mux->hw : NULL, mux_ops,
217 				       div ? &div->hw : NULL, div_ops,
218 				       gate ? &gate->hw : NULL, gate_ops,
219 				       flags);
220 	if (IS_ERR(hw))
221 		goto err_div;
222 
223 	return hw->clk;
224 err_div:
225 	kfree(gate);
226 err_gate:
227 	kfree(mux);
228 	return ERR_CAST(hw);
229 }
230