clk-cv18xx-pll.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Copyright (C) 2023 Inochi Amaoto <inochiama@outlook.com>
  4 */
  5
  6#include <linux/clk-provider.h>
  7#include <linux/io.h>
  8#include <linux/limits.h>
  9#include <linux/spinlock.h>
 10
 11#include "clk-cv18xx-pll.h"
 12
 13static inline struct cv1800_clk_pll *hw_to_cv1800_clk_pll(struct clk_hw *hw)
 14{
 15	struct cv1800_clk_common *common = hw_to_cv1800_clk_common(hw);
 16
 17	return container_of(common, struct cv1800_clk_pll, common);
 18}
 19
 20static unsigned long ipll_calc_rate(unsigned long parent_rate,
 21				    unsigned long pre_div_sel,
 22				    unsigned long div_sel,
 23				    unsigned long post_div_sel)
 24{
 25	uint64_t rate = parent_rate;
 26
 27	rate *= div_sel;
 28	do_div(rate, pre_div_sel * post_div_sel);
 29
 30	return rate;
 31}
 32
 33static unsigned long ipll_recalc_rate(struct clk_hw *hw,
 34				      unsigned long parent_rate)
 35{
 36	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
 37	u32 value;
 38
 39	value = readl(pll->common.base + pll->pll_reg);
 40
 41	return ipll_calc_rate(parent_rate,
 42			      PLL_GET_PRE_DIV_SEL(value),
 43			      PLL_GET_DIV_SEL(value),
 44			      PLL_GET_POST_DIV_SEL(value));
 45}
 46
 47static int ipll_find_rate(const struct cv1800_clk_pll_limit *limit,
 48			  unsigned long prate, unsigned long *rate,
 49			  u32 *value)
 50{
 51	unsigned long best_rate = 0;
 52	unsigned long trate = *rate;
 53	unsigned long pre_div_sel = 0, div_sel = 0, post_div_sel = 0;
 54	unsigned long pre, div, post;
 55	u32 detected = *value;
 56	unsigned long tmp;
 57
 58	for_each_pll_limit_range(pre, &limit->pre_div) {
 59		for_each_pll_limit_range(div, &limit->div) {
 60			for_each_pll_limit_range(post, &limit->post_div) {
 61				tmp = ipll_calc_rate(prate, pre, div, post);
 62
 63				if (tmp > trate)
 64					continue;
 65
 66				if ((trate - tmp) < (trate - best_rate)) {
 67					best_rate = tmp;
 68					pre_div_sel = pre;
 69					div_sel = div;
 70					post_div_sel = post;
 71				}
 72			}
 73		}
 74	}
 75
 76	if (best_rate) {
 77		detected = PLL_SET_PRE_DIV_SEL(detected, pre_div_sel);
 78		detected = PLL_SET_POST_DIV_SEL(detected, post_div_sel);
 79		detected = PLL_SET_DIV_SEL(detected, div_sel);
 80		*value = detected;
 81		*rate = best_rate;
 82		return 0;
 83	}
 84
 85	return -EINVAL;
 86}
 87
 88static int ipll_determine_rate(struct clk_hw *hw, struct clk_rate_request *req)
 89{
 90	u32 val;
 91	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
 92
 93	return ipll_find_rate(pll->pll_limit, req->best_parent_rate,
 94			      &req->rate, &val);
 95}
 96
 97static void pll_get_mode_ctrl(unsigned long div_sel,
 98			      bool (*mode_ctrl_check)(unsigned long,
 99						      unsigned long,
100						      unsigned long),
101			      const struct cv1800_clk_pll_limit *limit,
102			      u32 *value)
103{
104	unsigned long ictrl = 0, mode = 0;
105	u32 detected = *value;
106
107	for_each_pll_limit_range(mode, &limit->mode) {
108		for_each_pll_limit_range(ictrl, &limit->ictrl) {
109			if (mode_ctrl_check(div_sel, ictrl, mode)) {
110				detected = PLL_SET_SEL_MODE(detected, mode);
111				detected = PLL_SET_ICTRL(detected, ictrl);
112				*value = detected;
113				return;
114			}
115		}
116	}
117}
118
119static bool ipll_check_mode_ctrl_restrict(unsigned long div_sel,
120					  unsigned long ictrl,
121					  unsigned long mode)
122{
123	unsigned long left_rest = 20 * div_sel;
124	unsigned long right_rest = 35 * div_sel;
125	unsigned long test = 184 * (1 + mode) * (1 + ictrl) / 2;
126
127	return test > left_rest && test <= right_rest;
128}
129
130static int ipll_set_rate(struct clk_hw *hw, unsigned long rate,
131			 unsigned long parent_rate)
132{
133	u32 regval, detected = 0;
134	unsigned long flags;
135	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
136
137	ipll_find_rate(pll->pll_limit, parent_rate, &rate, &detected);
138	pll_get_mode_ctrl(PLL_GET_DIV_SEL(detected),
139			  ipll_check_mode_ctrl_restrict,
140			  pll->pll_limit, &detected);
141
142	spin_lock_irqsave(pll->common.lock, flags);
143
144	regval = readl(pll->common.base + pll->pll_reg);
145	regval = PLL_COPY_REG(regval, detected);
146
147	writel(regval, pll->common.base + pll->pll_reg);
148
149	spin_unlock_irqrestore(pll->common.lock, flags);
150
151	cv1800_clk_wait_for_lock(&pll->common, pll->pll_status.reg,
152			      BIT(pll->pll_status.shift));
153
154	return 0;
155}
156
157static int pll_enable(struct clk_hw *hw)
158{
159	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
160
161	return cv1800_clk_clearbit(&pll->common, &pll->pll_pwd);
162}
163
164static void pll_disable(struct clk_hw *hw)
165{
166	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
167
168	cv1800_clk_setbit(&pll->common, &pll->pll_pwd);
169}
170
171static int pll_is_enable(struct clk_hw *hw)
172{
173	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
174
175	return cv1800_clk_checkbit(&pll->common, &pll->pll_pwd) == 0;
176}
177
178const struct clk_ops cv1800_clk_ipll_ops = {
179	.disable = pll_disable,
180	.enable = pll_enable,
181	.is_enabled = pll_is_enable,
182
183	.recalc_rate = ipll_recalc_rate,
184	.determine_rate = ipll_determine_rate,
185	.set_rate = ipll_set_rate,
186};
187
188#define PLL_SYN_FACTOR_DOT_POS		26
189#define PLL_SYN_FACTOR_MINIMUM		((4 << PLL_SYN_FACTOR_DOT_POS) + 1)
190
191static bool fpll_is_factional_mode(struct cv1800_clk_pll *pll)
192{
193	return cv1800_clk_checkbit(&pll->common, &pll->pll_syn->en);
194}
195
196static unsigned long fpll_calc_rate(unsigned long parent_rate,
197				    unsigned long pre_div_sel,
198				    unsigned long div_sel,
199				    unsigned long post_div_sel,
200				    unsigned long ssc_syn_set,
201				    bool is_full_parent)
202{
203	u64 dividend = parent_rate * div_sel;
204	u64 factor = ssc_syn_set * pre_div_sel * post_div_sel;
205	unsigned long rate;
206
207	dividend <<= PLL_SYN_FACTOR_DOT_POS - 1;
208	rate = div64_u64_rem(dividend, factor, &dividend);
209
210	if (is_full_parent) {
211		dividend <<= 1;
212		rate <<= 1;
213	}
214
215	rate += DIV64_U64_ROUND_CLOSEST(dividend, factor);
216
217	return rate;
218}
219
220static unsigned long fpll_recalc_rate(struct clk_hw *hw,
221					  unsigned long parent_rate)
222{
223	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
224	u32 value;
225	bool clk_full;
226	u32 syn_set;
227
228	if (!fpll_is_factional_mode(pll))
229		return ipll_recalc_rate(hw, parent_rate);
230
231	syn_set = readl(pll->common.base + pll->pll_syn->set);
232
233	if (syn_set == 0)
234		return 0;
235
236	clk_full = cv1800_clk_checkbit(&pll->common,
237					  &pll->pll_syn->clk_half);
238
239	value = readl(pll->common.base + pll->pll_reg);
240
241	return fpll_calc_rate(parent_rate,
242			      PLL_GET_PRE_DIV_SEL(value),
243			      PLL_GET_DIV_SEL(value),
244			      PLL_GET_POST_DIV_SEL(value),
245			      syn_set, clk_full);
246}
247
248static unsigned long fpll_find_synthesizer(unsigned long parent,
249					   unsigned long rate,
250					   unsigned long pre_div,
251					   unsigned long div,
252					   unsigned long post_div,
253					   bool is_full_parent,
254					   u32 *ssc_syn_set)
255{
256	u32 test_max = U32_MAX, test_min = PLL_SYN_FACTOR_MINIMUM;
257	unsigned long trate;
258
259	while (test_min < test_max) {
260		u32 tssc = (test_max + test_min) / 2;
261
262		trate = fpll_calc_rate(parent, pre_div, div, post_div,
263				       tssc, is_full_parent);
264
265		if (trate == rate) {
266			test_min = tssc;
267			break;
268		}
269
270		if (trate > rate)
271			test_min = tssc + 1;
272		else
273			test_max = tssc - 1;
274	}
275
276	if (trate != 0)
277		*ssc_syn_set = test_min;
278
279	return trate;
280}
281
282static int fpll_find_rate(struct cv1800_clk_pll *pll,
283			  const struct cv1800_clk_pll_limit *limit,
284			  unsigned long prate,
285			  unsigned long *rate,
286			  u32 *value, u32 *ssc_syn_set)
287{
288	unsigned long best_rate = 0;
289	unsigned long pre_div_sel = 0, div_sel = 0, post_div_sel = 0;
290	unsigned long pre, div, post;
291	unsigned long trate = *rate;
292	u32 detected = *value;
293	unsigned long tmp;
294	bool clk_full = cv1800_clk_checkbit(&pll->common,
295					       &pll->pll_syn->clk_half);
296
297	for_each_pll_limit_range(pre, &limit->pre_div) {
298		for_each_pll_limit_range(post, &limit->post_div) {
299			for_each_pll_limit_range(div, &limit->div) {
300				tmp = fpll_find_synthesizer(prate, trate,
301							    pre, div, post,
302							    clk_full,
303							    ssc_syn_set);
304
305				if ((trate - tmp) < (trate - best_rate)) {
306					best_rate = tmp;
307					pre_div_sel = pre;
308					div_sel = div;
309					post_div_sel = post;
310				}
311			}
312		}
313	}
314
315	if (best_rate) {
316		detected = PLL_SET_PRE_DIV_SEL(detected, pre_div_sel);
317		detected = PLL_SET_POST_DIV_SEL(detected, post_div_sel);
318		detected = PLL_SET_DIV_SEL(detected, div_sel);
319		*value = detected;
320		*rate = best_rate;
321		return 0;
322	}
323
324	return -EINVAL;
325}
326
327static int fpll_determine_rate(struct clk_hw *hw, struct clk_rate_request *req)
328{
329	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
330	u32 val, ssc_syn_set;
331
332	if (!fpll_is_factional_mode(pll))
333		return ipll_determine_rate(hw, req);
334
335	fpll_find_rate(pll, &pll->pll_limit[2], req->best_parent_rate,
336		       &req->rate, &val, &ssc_syn_set);
337
338	return 0;
339}
340
341static bool fpll_check_mode_ctrl_restrict(unsigned long div_sel,
342					  unsigned long ictrl,
343					  unsigned long mode)
344{
345	unsigned long left_rest = 10 * div_sel;
346	unsigned long right_rest = 24 * div_sel;
347	unsigned long test = 184 * (1 + mode) * (1 + ictrl) / 2;
348
349	return test > left_rest && test <= right_rest;
350}
351
352static int fpll_set_rate(struct clk_hw *hw, unsigned long rate,
353			 unsigned long parent_rate)
354{
355	u32 regval;
356	u32 detected = 0, detected_ssc = 0;
357	unsigned long flags;
358	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
359
360	if (!fpll_is_factional_mode(pll))
361		return ipll_set_rate(hw, rate, parent_rate);
362
363	fpll_find_rate(pll, &pll->pll_limit[2], parent_rate,
364		       &rate, &detected, &detected_ssc);
365	pll_get_mode_ctrl(PLL_GET_DIV_SEL(detected),
366			  fpll_check_mode_ctrl_restrict,
367			  pll->pll_limit, &detected);
368
369	spin_lock_irqsave(pll->common.lock, flags);
370
371	writel(detected_ssc, pll->common.base + pll->pll_syn->set);
372
373	regval = readl(pll->common.base + pll->pll_reg);
374	regval = PLL_COPY_REG(regval, detected);
375
376	writel(regval, pll->common.base + pll->pll_reg);
377
378	spin_unlock_irqrestore(pll->common.lock, flags);
379
380	cv1800_clk_wait_for_lock(&pll->common, pll->pll_status.reg,
381			      BIT(pll->pll_status.shift));
382
383	return 0;
384}
385
386static u8 fpll_get_parent(struct clk_hw *hw)
387{
388	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
389
390	if (fpll_is_factional_mode(pll))
391		return 1;
392
393	return 0;
394}
395
396static int fpll_set_parent(struct clk_hw *hw, u8 index)
397{
398	struct cv1800_clk_pll *pll = hw_to_cv1800_clk_pll(hw);
399
400	if (index)
401		cv1800_clk_setbit(&pll->common, &pll->pll_syn->en);
402	else
403		cv1800_clk_clearbit(&pll->common, &pll->pll_syn->en);
404
405	return 0;
406}
407
408const struct clk_ops cv1800_clk_fpll_ops = {
409	.disable = pll_disable,
410	.enable = pll_enable,
411	.is_enabled = pll_is_enable,
412
413	.recalc_rate = fpll_recalc_rate,
414	.determine_rate = fpll_determine_rate,
415	.set_rate = fpll_set_rate,
416
417	.set_parent = fpll_set_parent,
418	.get_parent = fpll_get_parent,
419};