1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * drivers/media/i2c/smiapp-pll.c
4  *
5  * Generic driver for SMIA/SMIA++ compliant camera modules
6  *
7  * Copyright (C) 2011--2012 Nokia Corporation
8  * Contact: Sakari Ailus <sakari.ailus@iki.fi>
9  */
10 
11 #include <linux/device.h>
12 #include <linux/gcd.h>
13 #include <linux/lcm.h>
14 #include <linux/module.h>
15 
16 #include "smiapp-pll.h"
17 
18 /* Return an even number or one. */
clk_div_even(uint32_t a)19 static inline uint32_t clk_div_even(uint32_t a)
20 {
21 	return max_t(uint32_t, 1, a & ~1);
22 }
23 
24 /* Return an even number or one. */
clk_div_even_up(uint32_t a)25 static inline uint32_t clk_div_even_up(uint32_t a)
26 {
27 	if (a == 1)
28 		return 1;
29 	return (a + 1) & ~1;
30 }
31 
is_one_or_even(uint32_t a)32 static inline uint32_t is_one_or_even(uint32_t a)
33 {
34 	if (a == 1)
35 		return 1;
36 	if (a & 1)
37 		return 0;
38 
39 	return 1;
40 }
41 
bounds_check(struct device * dev,uint32_t val,uint32_t min,uint32_t max,char * str)42 static int bounds_check(struct device *dev, uint32_t val,
43 			uint32_t min, uint32_t max, char *str)
44 {
45 	if (val >= min && val <= max)
46 		return 0;
47 
48 	dev_dbg(dev, "%s out of bounds: %d (%d--%d)\n", str, val, min, max);
49 
50 	return -EINVAL;
51 }
52 
print_pll(struct device * dev,struct smiapp_pll * pll)53 static void print_pll(struct device *dev, struct smiapp_pll *pll)
54 {
55 	dev_dbg(dev, "pre_pll_clk_div\t%u\n",  pll->pre_pll_clk_div);
56 	dev_dbg(dev, "pll_multiplier \t%u\n",  pll->pll_multiplier);
57 	if (!(pll->flags & SMIAPP_PLL_FLAG_NO_OP_CLOCKS)) {
58 		dev_dbg(dev, "op_sys_clk_div \t%u\n", pll->op.sys_clk_div);
59 		dev_dbg(dev, "op_pix_clk_div \t%u\n", pll->op.pix_clk_div);
60 	}
61 	dev_dbg(dev, "vt_sys_clk_div \t%u\n",  pll->vt.sys_clk_div);
62 	dev_dbg(dev, "vt_pix_clk_div \t%u\n",  pll->vt.pix_clk_div);
63 
64 	dev_dbg(dev, "ext_clk_freq_hz \t%u\n", pll->ext_clk_freq_hz);
65 	dev_dbg(dev, "pll_ip_clk_freq_hz \t%u\n", pll->pll_ip_clk_freq_hz);
66 	dev_dbg(dev, "pll_op_clk_freq_hz \t%u\n", pll->pll_op_clk_freq_hz);
67 	if (!(pll->flags & SMIAPP_PLL_FLAG_NO_OP_CLOCKS)) {
68 		dev_dbg(dev, "op_sys_clk_freq_hz \t%u\n",
69 			pll->op.sys_clk_freq_hz);
70 		dev_dbg(dev, "op_pix_clk_freq_hz \t%u\n",
71 			pll->op.pix_clk_freq_hz);
72 	}
73 	dev_dbg(dev, "vt_sys_clk_freq_hz \t%u\n", pll->vt.sys_clk_freq_hz);
74 	dev_dbg(dev, "vt_pix_clk_freq_hz \t%u\n", pll->vt.pix_clk_freq_hz);
75 }
76 
check_all_bounds(struct device * dev,const struct smiapp_pll_limits * limits,const struct smiapp_pll_branch_limits * op_limits,struct smiapp_pll * pll,struct smiapp_pll_branch * op_pll)77 static int check_all_bounds(struct device *dev,
78 			    const struct smiapp_pll_limits *limits,
79 			    const struct smiapp_pll_branch_limits *op_limits,
80 			    struct smiapp_pll *pll,
81 			    struct smiapp_pll_branch *op_pll)
82 {
83 	int rval;
84 
85 	rval = bounds_check(dev, pll->pll_ip_clk_freq_hz,
86 			    limits->min_pll_ip_freq_hz,
87 			    limits->max_pll_ip_freq_hz,
88 			    "pll_ip_clk_freq_hz");
89 	if (!rval)
90 		rval = bounds_check(
91 			dev, pll->pll_multiplier,
92 			limits->min_pll_multiplier, limits->max_pll_multiplier,
93 			"pll_multiplier");
94 	if (!rval)
95 		rval = bounds_check(
96 			dev, pll->pll_op_clk_freq_hz,
97 			limits->min_pll_op_freq_hz, limits->max_pll_op_freq_hz,
98 			"pll_op_clk_freq_hz");
99 	if (!rval)
100 		rval = bounds_check(
101 			dev, op_pll->sys_clk_div,
102 			op_limits->min_sys_clk_div, op_limits->max_sys_clk_div,
103 			"op_sys_clk_div");
104 	if (!rval)
105 		rval = bounds_check(
106 			dev, op_pll->sys_clk_freq_hz,
107 			op_limits->min_sys_clk_freq_hz,
108 			op_limits->max_sys_clk_freq_hz,
109 			"op_sys_clk_freq_hz");
110 	if (!rval)
111 		rval = bounds_check(
112 			dev, op_pll->pix_clk_freq_hz,
113 			op_limits->min_pix_clk_freq_hz,
114 			op_limits->max_pix_clk_freq_hz,
115 			"op_pix_clk_freq_hz");
116 
117 	/*
118 	 * If there are no OP clocks, the VT clocks are contained in
119 	 * the OP clock struct.
120 	 */
121 	if (pll->flags & SMIAPP_PLL_FLAG_NO_OP_CLOCKS)
122 		return rval;
123 
124 	if (!rval)
125 		rval = bounds_check(
126 			dev, pll->vt.sys_clk_freq_hz,
127 			limits->vt.min_sys_clk_freq_hz,
128 			limits->vt.max_sys_clk_freq_hz,
129 			"vt_sys_clk_freq_hz");
130 	if (!rval)
131 		rval = bounds_check(
132 			dev, pll->vt.pix_clk_freq_hz,
133 			limits->vt.min_pix_clk_freq_hz,
134 			limits->vt.max_pix_clk_freq_hz,
135 			"vt_pix_clk_freq_hz");
136 
137 	return rval;
138 }
139 
140 /*
141  * Heuristically guess the PLL tree for a given common multiplier and
142  * divisor. Begin with the operational timing and continue to video
143  * timing once operational timing has been verified.
144  *
145  * @mul is the PLL multiplier and @div is the common divisor
146  * (pre_pll_clk_div and op_sys_clk_div combined). The final PLL
147  * multiplier will be a multiple of @mul.
148  *
149  * @return Zero on success, error code on error.
150  */
__smiapp_pll_calculate(struct device * dev,const struct smiapp_pll_limits * limits,const struct smiapp_pll_branch_limits * op_limits,struct smiapp_pll * pll,struct smiapp_pll_branch * op_pll,uint32_t mul,uint32_t div,uint32_t lane_op_clock_ratio)151 static int __smiapp_pll_calculate(
152 	struct device *dev, const struct smiapp_pll_limits *limits,
153 	const struct smiapp_pll_branch_limits *op_limits,
154 	struct smiapp_pll *pll, struct smiapp_pll_branch *op_pll, uint32_t mul,
155 	uint32_t div, uint32_t lane_op_clock_ratio)
156 {
157 	uint32_t sys_div;
158 	uint32_t best_pix_div = INT_MAX >> 1;
159 	uint32_t vt_op_binning_div;
160 	/*
161 	 * Higher multipliers (and divisors) are often required than
162 	 * necessitated by the external clock and the output clocks.
163 	 * There are limits for all values in the clock tree. These
164 	 * are the minimum and maximum multiplier for mul.
165 	 */
166 	uint32_t more_mul_min, more_mul_max;
167 	uint32_t more_mul_factor;
168 	uint32_t min_vt_div, max_vt_div, vt_div;
169 	uint32_t min_sys_div, max_sys_div;
170 	unsigned int i;
171 
172 	/*
173 	 * Get pre_pll_clk_div so that our pll_op_clk_freq_hz won't be
174 	 * too high.
175 	 */
176 	dev_dbg(dev, "pre_pll_clk_div %u\n", pll->pre_pll_clk_div);
177 
178 	/* Don't go above max pll multiplier. */
179 	more_mul_max = limits->max_pll_multiplier / mul;
180 	dev_dbg(dev, "more_mul_max: max_pll_multiplier check: %u\n",
181 		more_mul_max);
182 	/* Don't go above max pll op frequency. */
183 	more_mul_max =
184 		min_t(uint32_t,
185 		      more_mul_max,
186 		      limits->max_pll_op_freq_hz
187 		      / (pll->ext_clk_freq_hz / pll->pre_pll_clk_div * mul));
188 	dev_dbg(dev, "more_mul_max: max_pll_op_freq_hz check: %u\n",
189 		more_mul_max);
190 	/* Don't go above the division capability of op sys clock divider. */
191 	more_mul_max = min(more_mul_max,
192 			   op_limits->max_sys_clk_div * pll->pre_pll_clk_div
193 			   / div);
194 	dev_dbg(dev, "more_mul_max: max_op_sys_clk_div check: %u\n",
195 		more_mul_max);
196 	/* Ensure we won't go above min_pll_multiplier. */
197 	more_mul_max = min(more_mul_max,
198 			   DIV_ROUND_UP(limits->max_pll_multiplier, mul));
199 	dev_dbg(dev, "more_mul_max: min_pll_multiplier check: %u\n",
200 		more_mul_max);
201 
202 	/* Ensure we won't go below min_pll_op_freq_hz. */
203 	more_mul_min = DIV_ROUND_UP(limits->min_pll_op_freq_hz,
204 				    pll->ext_clk_freq_hz / pll->pre_pll_clk_div
205 				    * mul);
206 	dev_dbg(dev, "more_mul_min: min_pll_op_freq_hz check: %u\n",
207 		more_mul_min);
208 	/* Ensure we won't go below min_pll_multiplier. */
209 	more_mul_min = max(more_mul_min,
210 			   DIV_ROUND_UP(limits->min_pll_multiplier, mul));
211 	dev_dbg(dev, "more_mul_min: min_pll_multiplier check: %u\n",
212 		more_mul_min);
213 
214 	if (more_mul_min > more_mul_max) {
215 		dev_dbg(dev,
216 			"unable to compute more_mul_min and more_mul_max\n");
217 		return -EINVAL;
218 	}
219 
220 	more_mul_factor = lcm(div, pll->pre_pll_clk_div) / div;
221 	dev_dbg(dev, "more_mul_factor: %u\n", more_mul_factor);
222 	more_mul_factor = lcm(more_mul_factor, op_limits->min_sys_clk_div);
223 	dev_dbg(dev, "more_mul_factor: min_op_sys_clk_div: %d\n",
224 		more_mul_factor);
225 	i = roundup(more_mul_min, more_mul_factor);
226 	if (!is_one_or_even(i))
227 		i <<= 1;
228 
229 	dev_dbg(dev, "final more_mul: %u\n", i);
230 	if (i > more_mul_max) {
231 		dev_dbg(dev, "final more_mul is bad, max %u\n", more_mul_max);
232 		return -EINVAL;
233 	}
234 
235 	pll->pll_multiplier = mul * i;
236 	op_pll->sys_clk_div = div * i / pll->pre_pll_clk_div;
237 	dev_dbg(dev, "op_sys_clk_div: %u\n", op_pll->sys_clk_div);
238 
239 	pll->pll_ip_clk_freq_hz = pll->ext_clk_freq_hz
240 		/ pll->pre_pll_clk_div;
241 
242 	pll->pll_op_clk_freq_hz = pll->pll_ip_clk_freq_hz
243 		* pll->pll_multiplier;
244 
245 	/* Derive pll_op_clk_freq_hz. */
246 	op_pll->sys_clk_freq_hz =
247 		pll->pll_op_clk_freq_hz / op_pll->sys_clk_div;
248 
249 	op_pll->pix_clk_div = pll->bits_per_pixel;
250 	dev_dbg(dev, "op_pix_clk_div: %u\n", op_pll->pix_clk_div);
251 
252 	op_pll->pix_clk_freq_hz =
253 		op_pll->sys_clk_freq_hz / op_pll->pix_clk_div;
254 
255 	if (pll->flags & SMIAPP_PLL_FLAG_NO_OP_CLOCKS) {
256 		/* No OP clocks --- VT clocks are used instead. */
257 		goto out_skip_vt_calc;
258 	}
259 
260 	/*
261 	 * Some sensors perform analogue binning and some do this
262 	 * digitally. The ones doing this digitally can be roughly be
263 	 * found out using this formula. The ones doing this digitally
264 	 * should run at higher clock rate, so smaller divisor is used
265 	 * on video timing side.
266 	 */
267 	if (limits->min_line_length_pck_bin > limits->min_line_length_pck
268 	    / pll->binning_horizontal)
269 		vt_op_binning_div = pll->binning_horizontal;
270 	else
271 		vt_op_binning_div = 1;
272 	dev_dbg(dev, "vt_op_binning_div: %u\n", vt_op_binning_div);
273 
274 	/*
275 	 * Profile 2 supports vt_pix_clk_div E [4, 10]
276 	 *
277 	 * Horizontal binning can be used as a base for difference in
278 	 * divisors. One must make sure that horizontal blanking is
279 	 * enough to accommodate the CSI-2 sync codes.
280 	 *
281 	 * Take scaling factor into account as well.
282 	 *
283 	 * Find absolute limits for the factor of vt divider.
284 	 */
285 	dev_dbg(dev, "scale_m: %u\n", pll->scale_m);
286 	min_vt_div = DIV_ROUND_UP(op_pll->pix_clk_div * op_pll->sys_clk_div
287 				  * pll->scale_n,
288 				  lane_op_clock_ratio * vt_op_binning_div
289 				  * pll->scale_m);
290 
291 	/* Find smallest and biggest allowed vt divisor. */
292 	dev_dbg(dev, "min_vt_div: %u\n", min_vt_div);
293 	min_vt_div = max(min_vt_div,
294 			 DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
295 				      limits->vt.max_pix_clk_freq_hz));
296 	dev_dbg(dev, "min_vt_div: max_vt_pix_clk_freq_hz: %u\n",
297 		min_vt_div);
298 	min_vt_div = max_t(uint32_t, min_vt_div,
299 			   limits->vt.min_pix_clk_div
300 			   * limits->vt.min_sys_clk_div);
301 	dev_dbg(dev, "min_vt_div: min_vt_clk_div: %u\n", min_vt_div);
302 
303 	max_vt_div = limits->vt.max_sys_clk_div * limits->vt.max_pix_clk_div;
304 	dev_dbg(dev, "max_vt_div: %u\n", max_vt_div);
305 	max_vt_div = min(max_vt_div,
306 			 DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
307 				      limits->vt.min_pix_clk_freq_hz));
308 	dev_dbg(dev, "max_vt_div: min_vt_pix_clk_freq_hz: %u\n",
309 		max_vt_div);
310 
311 	/*
312 	 * Find limitsits for sys_clk_div. Not all values are possible
313 	 * with all values of pix_clk_div.
314 	 */
315 	min_sys_div = limits->vt.min_sys_clk_div;
316 	dev_dbg(dev, "min_sys_div: %u\n", min_sys_div);
317 	min_sys_div = max(min_sys_div,
318 			  DIV_ROUND_UP(min_vt_div,
319 				       limits->vt.max_pix_clk_div));
320 	dev_dbg(dev, "min_sys_div: max_vt_pix_clk_div: %u\n", min_sys_div);
321 	min_sys_div = max(min_sys_div,
322 			  pll->pll_op_clk_freq_hz
323 			  / limits->vt.max_sys_clk_freq_hz);
324 	dev_dbg(dev, "min_sys_div: max_pll_op_clk_freq_hz: %u\n", min_sys_div);
325 	min_sys_div = clk_div_even_up(min_sys_div);
326 	dev_dbg(dev, "min_sys_div: one or even: %u\n", min_sys_div);
327 
328 	max_sys_div = limits->vt.max_sys_clk_div;
329 	dev_dbg(dev, "max_sys_div: %u\n", max_sys_div);
330 	max_sys_div = min(max_sys_div,
331 			  DIV_ROUND_UP(max_vt_div,
332 				       limits->vt.min_pix_clk_div));
333 	dev_dbg(dev, "max_sys_div: min_vt_pix_clk_div: %u\n", max_sys_div);
334 	max_sys_div = min(max_sys_div,
335 			  DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
336 				       limits->vt.min_pix_clk_freq_hz));
337 	dev_dbg(dev, "max_sys_div: min_vt_pix_clk_freq_hz: %u\n", max_sys_div);
338 
339 	/*
340 	 * Find pix_div such that a legal pix_div * sys_div results
341 	 * into a value which is not smaller than div, the desired
342 	 * divisor.
343 	 */
344 	for (vt_div = min_vt_div; vt_div <= max_vt_div;
345 	     vt_div += 2 - (vt_div & 1)) {
346 		for (sys_div = min_sys_div;
347 		     sys_div <= max_sys_div;
348 		     sys_div += 2 - (sys_div & 1)) {
349 			uint16_t pix_div = DIV_ROUND_UP(vt_div, sys_div);
350 
351 			if (pix_div < limits->vt.min_pix_clk_div
352 			    || pix_div > limits->vt.max_pix_clk_div) {
353 				dev_dbg(dev,
354 					"pix_div %u too small or too big (%u--%u)\n",
355 					pix_div,
356 					limits->vt.min_pix_clk_div,
357 					limits->vt.max_pix_clk_div);
358 				continue;
359 			}
360 
361 			/* Check if this one is better. */
362 			if (pix_div * sys_div
363 			    <= roundup(min_vt_div, best_pix_div))
364 				best_pix_div = pix_div;
365 		}
366 		if (best_pix_div < INT_MAX >> 1)
367 			break;
368 	}
369 
370 	pll->vt.sys_clk_div = DIV_ROUND_UP(min_vt_div, best_pix_div);
371 	pll->vt.pix_clk_div = best_pix_div;
372 
373 	pll->vt.sys_clk_freq_hz =
374 		pll->pll_op_clk_freq_hz / pll->vt.sys_clk_div;
375 	pll->vt.pix_clk_freq_hz =
376 		pll->vt.sys_clk_freq_hz / pll->vt.pix_clk_div;
377 
378 out_skip_vt_calc:
379 	pll->pixel_rate_csi =
380 		op_pll->pix_clk_freq_hz * lane_op_clock_ratio;
381 	pll->pixel_rate_pixel_array = pll->vt.pix_clk_freq_hz;
382 
383 	return check_all_bounds(dev, limits, op_limits, pll, op_pll);
384 }
385 
smiapp_pll_calculate(struct device * dev,const struct smiapp_pll_limits * limits,struct smiapp_pll * pll)386 int smiapp_pll_calculate(struct device *dev,
387 			 const struct smiapp_pll_limits *limits,
388 			 struct smiapp_pll *pll)
389 {
390 	const struct smiapp_pll_branch_limits *op_limits = &limits->op;
391 	struct smiapp_pll_branch *op_pll = &pll->op;
392 	uint16_t min_pre_pll_clk_div;
393 	uint16_t max_pre_pll_clk_div;
394 	uint32_t lane_op_clock_ratio;
395 	uint32_t mul, div;
396 	unsigned int i;
397 	int rval = -EINVAL;
398 
399 	if (pll->flags & SMIAPP_PLL_FLAG_NO_OP_CLOCKS) {
400 		/*
401 		 * If there's no OP PLL at all, use the VT values
402 		 * instead. The OP values are ignored for the rest of
403 		 * the PLL calculation.
404 		 */
405 		op_limits = &limits->vt;
406 		op_pll = &pll->vt;
407 	}
408 
409 	if (pll->flags & SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE)
410 		lane_op_clock_ratio = pll->csi2.lanes;
411 	else
412 		lane_op_clock_ratio = 1;
413 	dev_dbg(dev, "lane_op_clock_ratio: %u\n", lane_op_clock_ratio);
414 
415 	dev_dbg(dev, "binning: %ux%u\n", pll->binning_horizontal,
416 		pll->binning_vertical);
417 
418 	switch (pll->bus_type) {
419 	case SMIAPP_PLL_BUS_TYPE_CSI2:
420 		/* CSI transfers 2 bits per clock per lane; thus times 2 */
421 		pll->pll_op_clk_freq_hz = pll->link_freq * 2
422 			* (pll->csi2.lanes / lane_op_clock_ratio);
423 		break;
424 	case SMIAPP_PLL_BUS_TYPE_PARALLEL:
425 		pll->pll_op_clk_freq_hz = pll->link_freq * pll->bits_per_pixel
426 			/ DIV_ROUND_UP(pll->bits_per_pixel,
427 				       pll->parallel.bus_width);
428 		break;
429 	default:
430 		return -EINVAL;
431 	}
432 
433 	/* Figure out limits for pre-pll divider based on extclk */
434 	dev_dbg(dev, "min / max pre_pll_clk_div: %u / %u\n",
435 		limits->min_pre_pll_clk_div, limits->max_pre_pll_clk_div);
436 	max_pre_pll_clk_div =
437 		min_t(uint16_t, limits->max_pre_pll_clk_div,
438 		      clk_div_even(pll->ext_clk_freq_hz /
439 				   limits->min_pll_ip_freq_hz));
440 	min_pre_pll_clk_div =
441 		max_t(uint16_t, limits->min_pre_pll_clk_div,
442 		      clk_div_even_up(
443 			      DIV_ROUND_UP(pll->ext_clk_freq_hz,
444 					   limits->max_pll_ip_freq_hz)));
445 	dev_dbg(dev, "pre-pll check: min / max pre_pll_clk_div: %u / %u\n",
446 		min_pre_pll_clk_div, max_pre_pll_clk_div);
447 
448 	i = gcd(pll->pll_op_clk_freq_hz, pll->ext_clk_freq_hz);
449 	mul = div_u64(pll->pll_op_clk_freq_hz, i);
450 	div = pll->ext_clk_freq_hz / i;
451 	dev_dbg(dev, "mul %u / div %u\n", mul, div);
452 
453 	min_pre_pll_clk_div =
454 		max_t(uint16_t, min_pre_pll_clk_div,
455 		      clk_div_even_up(
456 			      DIV_ROUND_UP(mul * pll->ext_clk_freq_hz,
457 					   limits->max_pll_op_freq_hz)));
458 	dev_dbg(dev, "pll_op check: min / max pre_pll_clk_div: %u / %u\n",
459 		min_pre_pll_clk_div, max_pre_pll_clk_div);
460 
461 	for (pll->pre_pll_clk_div = min_pre_pll_clk_div;
462 	     pll->pre_pll_clk_div <= max_pre_pll_clk_div;
463 	     pll->pre_pll_clk_div += 2 - (pll->pre_pll_clk_div & 1)) {
464 		rval = __smiapp_pll_calculate(dev, limits, op_limits, pll,
465 					      op_pll, mul, div,
466 					      lane_op_clock_ratio);
467 		if (rval)
468 			continue;
469 
470 		print_pll(dev, pll);
471 		return 0;
472 	}
473 
474 	dev_dbg(dev, "unable to compute pre_pll divisor\n");
475 
476 	return rval;
477 }
478 EXPORT_SYMBOL_GPL(smiapp_pll_calculate);
479 
480 MODULE_AUTHOR("Sakari Ailus <sakari.ailus@iki.fi>");
481 MODULE_DESCRIPTION("Generic SMIA/SMIA++ PLL calculator");
482 MODULE_LICENSE("GPL");
483