1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Copyright (c) 2013 NVIDIA CORPORATION.  All rights reserved.
  4 */
  5
  6#include <linux/clk-provider.h>
  7#include <linux/err.h>
  8#include <linux/slab.h>
  9
 10static u8 clk_composite_get_parent(struct clk_hw *hw)
 11{
 12	struct clk_composite *composite = to_clk_composite(hw);
 13	const struct clk_ops *mux_ops = composite->mux_ops;
 14	struct clk_hw *mux_hw = composite->mux_hw;
 15
 16	__clk_hw_set_clk(mux_hw, hw);
 17
 18	return mux_ops->get_parent(mux_hw);
 19}
 20
 21static int clk_composite_set_parent(struct clk_hw *hw, u8 index)
 22{
 23	struct clk_composite *composite = to_clk_composite(hw);
 24	const struct clk_ops *mux_ops = composite->mux_ops;
 25	struct clk_hw *mux_hw = composite->mux_hw;
 26
 27	__clk_hw_set_clk(mux_hw, hw);
 28
 29	return mux_ops->set_parent(mux_hw, index);
 30}
 31
 32static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
 33					    unsigned long parent_rate)
 34{
 35	struct clk_composite *composite = to_clk_composite(hw);
 36	const struct clk_ops *rate_ops = composite->rate_ops;
 37	struct clk_hw *rate_hw = composite->rate_hw;
 38
 39	__clk_hw_set_clk(rate_hw, hw);
 40
 41	return rate_ops->recalc_rate(rate_hw, parent_rate);
 42}
 43
 44static int clk_composite_determine_rate(struct clk_hw *hw,
 45					struct clk_rate_request *req)
 46{
 47	struct clk_composite *composite = to_clk_composite(hw);
 48	const struct clk_ops *rate_ops = composite->rate_ops;
 49	const struct clk_ops *mux_ops = composite->mux_ops;
 50	struct clk_hw *rate_hw = composite->rate_hw;
 51	struct clk_hw *mux_hw = composite->mux_hw;
 52	struct clk_hw *parent;
 53	unsigned long parent_rate;
 54	long tmp_rate, best_rate = 0;
 55	unsigned long rate_diff;
 56	unsigned long best_rate_diff = ULONG_MAX;
 57	long rate;
 58	int i;
 59
 60	if (rate_hw && rate_ops && rate_ops->determine_rate) {
 61		__clk_hw_set_clk(rate_hw, hw);
 62		return rate_ops->determine_rate(rate_hw, req);
 63	} else if (rate_hw && rate_ops && rate_ops->round_rate &&
 64		   mux_hw && mux_ops && mux_ops->set_parent) {
 65		req->best_parent_hw = NULL;
 66
 67		if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) {
 68			parent = clk_hw_get_parent(mux_hw);
 69			req->best_parent_hw = parent;
 70			req->best_parent_rate = clk_hw_get_rate(parent);
 71
 72			rate = rate_ops->round_rate(rate_hw, req->rate,
 73						    &req->best_parent_rate);
 74			if (rate < 0)
 75				return rate;
 76
 77			req->rate = rate;
 78			return 0;
 79		}
 80
 81		for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) {
 82			parent = clk_hw_get_parent_by_index(mux_hw, i);
 83			if (!parent)
 84				continue;
 85
 86			parent_rate = clk_hw_get_rate(parent);
 87
 88			tmp_rate = rate_ops->round_rate(rate_hw, req->rate,
 89							&parent_rate);
 90			if (tmp_rate < 0)
 91				continue;
 92
 93			rate_diff = abs(req->rate - tmp_rate);
 94
 95			if (!rate_diff || !req->best_parent_hw
 96				       || best_rate_diff > rate_diff) {
 97				req->best_parent_hw = parent;
 98				req->best_parent_rate = parent_rate;
 99				best_rate_diff = rate_diff;
100				best_rate = tmp_rate;
101			}
102
103			if (!rate_diff)
104				return 0;
105		}
106
107		req->rate = best_rate;
108		return 0;
109	} else if (mux_hw && mux_ops && mux_ops->determine_rate) {
110		__clk_hw_set_clk(mux_hw, hw);
111		return mux_ops->determine_rate(mux_hw, req);
112	} else {
113		pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n");
114		return -EINVAL;
115	}
116}
117
118static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,
119				  unsigned long *prate)
120{
121	struct clk_composite *composite = to_clk_composite(hw);
122	const struct clk_ops *rate_ops = composite->rate_ops;
123	struct clk_hw *rate_hw = composite->rate_hw;
124
125	__clk_hw_set_clk(rate_hw, hw);
126
127	return rate_ops->round_rate(rate_hw, rate, prate);
128}
129
130static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,
131			       unsigned long parent_rate)
132{
133	struct clk_composite *composite = to_clk_composite(hw);
134	const struct clk_ops *rate_ops = composite->rate_ops;
135	struct clk_hw *rate_hw = composite->rate_hw;
136
137	__clk_hw_set_clk(rate_hw, hw);
138
139	return rate_ops->set_rate(rate_hw, rate, parent_rate);
140}
141
142static int clk_composite_set_rate_and_parent(struct clk_hw *hw,
143					     unsigned long rate,
144					     unsigned long parent_rate,
145					     u8 index)
146{
147	struct clk_composite *composite = to_clk_composite(hw);
148	const struct clk_ops *rate_ops = composite->rate_ops;
149	const struct clk_ops *mux_ops = composite->mux_ops;
150	struct clk_hw *rate_hw = composite->rate_hw;
151	struct clk_hw *mux_hw = composite->mux_hw;
152	unsigned long temp_rate;
153
154	__clk_hw_set_clk(rate_hw, hw);
155	__clk_hw_set_clk(mux_hw, hw);
156
157	temp_rate = rate_ops->recalc_rate(rate_hw, parent_rate);
158	if (temp_rate > rate) {
159		rate_ops->set_rate(rate_hw, rate, parent_rate);
160		mux_ops->set_parent(mux_hw, index);
161	} else {
162		mux_ops->set_parent(mux_hw, index);
163		rate_ops->set_rate(rate_hw, rate, parent_rate);
164	}
165
166	return 0;
167}
168
169static int clk_composite_is_enabled(struct clk_hw *hw)
170{
171	struct clk_composite *composite = to_clk_composite(hw);
172	const struct clk_ops *gate_ops = composite->gate_ops;
173	struct clk_hw *gate_hw = composite->gate_hw;
174
175	__clk_hw_set_clk(gate_hw, hw);
176
177	return gate_ops->is_enabled(gate_hw);
178}
179
180static int clk_composite_enable(struct clk_hw *hw)
181{
182	struct clk_composite *composite = to_clk_composite(hw);
183	const struct clk_ops *gate_ops = composite->gate_ops;
184	struct clk_hw *gate_hw = composite->gate_hw;
185
186	__clk_hw_set_clk(gate_hw, hw);
187
188	return gate_ops->enable(gate_hw);
189}
190
191static void clk_composite_disable(struct clk_hw *hw)
192{
193	struct clk_composite *composite = to_clk_composite(hw);
194	const struct clk_ops *gate_ops = composite->gate_ops;
195	struct clk_hw *gate_hw = composite->gate_hw;
196
197	__clk_hw_set_clk(gate_hw, hw);
198
199	gate_ops->disable(gate_hw);
200}
201
202struct clk_hw *clk_hw_register_composite(struct device *dev, const char *name,
203			const char * const *parent_names, int num_parents,
204			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
205			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
206			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
207			unsigned long flags)
208{
209	struct clk_hw *hw;
210	struct clk_init_data init;
211	struct clk_composite *composite;
212	struct clk_ops *clk_composite_ops;
213	int ret;
214
215	composite = kzalloc(sizeof(*composite), GFP_KERNEL);
216	if (!composite)
217		return ERR_PTR(-ENOMEM);
218
219	init.name = name;
220	init.flags = flags;
221	init.parent_names = parent_names;
222	init.num_parents = num_parents;
223	hw = &composite->hw;
224
225	clk_composite_ops = &composite->ops;
226
227	if (mux_hw && mux_ops) {
228		if (!mux_ops->get_parent) {
229			hw = ERR_PTR(-EINVAL);
230			goto err;
231		}
232
233		composite->mux_hw = mux_hw;
234		composite->mux_ops = mux_ops;
235		clk_composite_ops->get_parent = clk_composite_get_parent;
236		if (mux_ops->set_parent)
237			clk_composite_ops->set_parent = clk_composite_set_parent;
238		if (mux_ops->determine_rate)
239			clk_composite_ops->determine_rate = clk_composite_determine_rate;
240	}
241
242	if (rate_hw && rate_ops) {
243		if (!rate_ops->recalc_rate) {
244			hw = ERR_PTR(-EINVAL);
245			goto err;
246		}
247		clk_composite_ops->recalc_rate = clk_composite_recalc_rate;
248
249		if (rate_ops->determine_rate)
250			clk_composite_ops->determine_rate =
251				clk_composite_determine_rate;
252		else if (rate_ops->round_rate)
253			clk_composite_ops->round_rate =
254				clk_composite_round_rate;
255
256		/* .set_rate requires either .round_rate or .determine_rate */
257		if (rate_ops->set_rate) {
258			if (rate_ops->determine_rate || rate_ops->round_rate)
259				clk_composite_ops->set_rate =
260						clk_composite_set_rate;
261			else
262				WARN(1, "%s: missing round_rate op is required\n",
263						__func__);
264		}
265
266		composite->rate_hw = rate_hw;
267		composite->rate_ops = rate_ops;
268	}
269
270	if (mux_hw && mux_ops && rate_hw && rate_ops) {
271		if (mux_ops->set_parent && rate_ops->set_rate)
272			clk_composite_ops->set_rate_and_parent =
273			clk_composite_set_rate_and_parent;
274	}
275
276	if (gate_hw && gate_ops) {
277		if (!gate_ops->is_enabled || !gate_ops->enable ||
278		    !gate_ops->disable) {
279			hw = ERR_PTR(-EINVAL);
280			goto err;
281		}
282
283		composite->gate_hw = gate_hw;
284		composite->gate_ops = gate_ops;
285		clk_composite_ops->is_enabled = clk_composite_is_enabled;
286		clk_composite_ops->enable = clk_composite_enable;
287		clk_composite_ops->disable = clk_composite_disable;
288	}
289
290	init.ops = clk_composite_ops;
291	composite->hw.init = &init;
292
293	ret = clk_hw_register(dev, hw);
294	if (ret) {
295		hw = ERR_PTR(ret);
296		goto err;
297	}
298
299	if (composite->mux_hw)
300		composite->mux_hw->clk = hw->clk;
301
302	if (composite->rate_hw)
303		composite->rate_hw->clk = hw->clk;
304
305	if (composite->gate_hw)
306		composite->gate_hw->clk = hw->clk;
307
308	return hw;
309
310err:
311	kfree(composite);
312	return hw;
313}
314
315struct clk *clk_register_composite(struct device *dev, const char *name,
316			const char * const *parent_names, int num_parents,
317			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
318			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
319			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
320			unsigned long flags)
321{
322	struct clk_hw *hw;
323
324	hw = clk_hw_register_composite(dev, name, parent_names, num_parents,
325			mux_hw, mux_ops, rate_hw, rate_ops, gate_hw, gate_ops,
326			flags);
327	if (IS_ERR(hw))
328		return ERR_CAST(hw);
329	return hw->clk;
330}
331
332void clk_unregister_composite(struct clk *clk)
333{
334	struct clk_composite *composite;
335	struct clk_hw *hw;
336
337	hw = __clk_get_hw(clk);
338	if (!hw)
339		return;
340
341	composite = to_clk_composite(hw);
342
343	clk_unregister(clk);
344	kfree(composite);
345}