1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * camss-csid.c
4  *
5  * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
6  *
7  * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
8  * Copyright (C) 2015-2018 Linaro Ltd.
9  */
10 #include <linux/clk.h>
11 #include <linux/completion.h>
12 #include <linux/interrupt.h>
13 #include <linux/io.h>
14 #include <linux/kernel.h>
15 #include <linux/of.h>
16 #include <linux/platform_device.h>
17 #include <linux/pm_runtime.h>
18 #include <linux/regulator/consumer.h>
19 #include <media/media-entity.h>
20 #include <media/v4l2-device.h>
21 #include <media/v4l2-event.h>
22 #include <media/v4l2-subdev.h>
23 
24 #include "camss-csid.h"
25 #include "camss-csid-gen1.h"
26 #include "camss.h"
27 
28 /* offset of CSID registers in VFE region for VFE 480 */
29 #define VFE_480_CSID_OFFSET 0x1200
30 #define VFE_480_LITE_CSID_OFFSET 0x200
31 
32 #define MSM_CSID_NAME "msm_csid"
33 
34 const char * const csid_testgen_modes[] = {
35 	"Disabled",
36 	"Incrementing",
37 	"Alternating 0x55/0xAA",
38 	"All Zeros 0x00",
39 	"All Ones 0xFF",
40 	"Pseudo-random Data",
41 	"User Specified",
42 	"Complex pattern",
43 	"Color box",
44 	"Color bars",
45 	NULL
46 };
47 
48 u32 csid_find_code(u32 *codes, unsigned int ncodes,
49 		   unsigned int match_format_idx, u32 match_code)
50 {
51 	int i;
52 
53 	if (!match_code && (match_format_idx >= ncodes))
54 		return 0;
55 
56 	for (i = 0; i < ncodes; i++)
57 		if (match_code) {
58 			if (codes[i] == match_code)
59 				return match_code;
60 		} else {
61 			if (i == match_format_idx)
62 				return codes[i];
63 		}
64 
65 	return codes[0];
66 }
67 
68 const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,
69 					     unsigned int nformats,
70 					     u32 code)
71 {
72 	unsigned int i;
73 
74 	for (i = 0; i < nformats; i++)
75 		if (code == formats[i].code)
76 			return &formats[i];
77 
78 	WARN(1, "Unknown format\n");
79 
80 	return &formats[0];
81 }
82 
83 /*
84  * csid_set_clock_rates - Calculate and set clock rates on CSID module
85  * @csiphy: CSID device
86  */
87 static int csid_set_clock_rates(struct csid_device *csid)
88 {
89 	struct device *dev = csid->camss->dev;
90 	const struct csid_format *fmt;
91 	s64 link_freq;
92 	int i, j;
93 	int ret;
94 
95 	fmt = csid_get_fmt_entry(csid->formats, csid->nformats,
96 				 csid->fmt[MSM_CSIPHY_PAD_SINK].code);
97 	link_freq = camss_get_link_freq(&csid->subdev.entity, fmt->bpp,
98 					csid->phy.lane_cnt);
99 	if (link_freq < 0)
100 		link_freq = 0;
101 
102 	for (i = 0; i < csid->nclocks; i++) {
103 		struct camss_clock *clock = &csid->clock[i];
104 
105 		if (!strcmp(clock->name, "csi0") ||
106 		    !strcmp(clock->name, "csi1") ||
107 		    !strcmp(clock->name, "csi2") ||
108 		    !strcmp(clock->name, "csi3")) {
109 			u64 min_rate = link_freq / 4;
110 			long rate;
111 
112 			camss_add_clock_margin(&min_rate);
113 
114 			for (j = 0; j < clock->nfreqs; j++)
115 				if (min_rate < clock->freq[j])
116 					break;
117 
118 			if (j == clock->nfreqs) {
119 				dev_err(dev,
120 					"Pixel clock is too high for CSID\n");
121 				return -EINVAL;
122 			}
123 
124 			/* if sensor pixel clock is not available */
125 			/* set highest possible CSID clock rate */
126 			if (min_rate == 0)
127 				j = clock->nfreqs - 1;
128 
129 			rate = clk_round_rate(clock->clk, clock->freq[j]);
130 			if (rate < 0) {
131 				dev_err(dev, "clk round rate failed: %ld\n",
132 					rate);
133 				return -EINVAL;
134 			}
135 
136 			ret = clk_set_rate(clock->clk, rate);
137 			if (ret < 0) {
138 				dev_err(dev, "clk set rate failed: %d\n", ret);
139 				return ret;
140 			}
141 		} else if (clock->nfreqs) {
142 			clk_set_rate(clock->clk, clock->freq[0]);
143 		}
144 	}
145 
146 	return 0;
147 }
148 
149 /*
150  * csid_set_power - Power on/off CSID module
151  * @sd: CSID V4L2 subdevice
152  * @on: Requested power state
153  *
154  * Return 0 on success or a negative error code otherwise
155  */
156 static int csid_set_power(struct v4l2_subdev *sd, int on)
157 {
158 	struct csid_device *csid = v4l2_get_subdevdata(sd);
159 	struct camss *camss = csid->camss;
160 	struct device *dev = camss->dev;
161 	struct vfe_device *vfe = &camss->vfe[csid->id];
162 	u32 version = camss->version;
163 	int ret = 0;
164 
165 	if (on) {
166 		if (version == CAMSS_8250 || version == CAMSS_845) {
167 			ret = vfe_get(vfe);
168 			if (ret < 0)
169 				return ret;
170 		}
171 
172 		ret = pm_runtime_resume_and_get(dev);
173 		if (ret < 0)
174 			return ret;
175 
176 		ret = regulator_bulk_enable(csid->num_supplies,
177 					    csid->supplies);
178 		if (ret < 0) {
179 			pm_runtime_put_sync(dev);
180 			return ret;
181 		}
182 
183 		ret = csid_set_clock_rates(csid);
184 		if (ret < 0) {
185 			regulator_bulk_disable(csid->num_supplies,
186 					       csid->supplies);
187 			pm_runtime_put_sync(dev);
188 			return ret;
189 		}
190 
191 		ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
192 		if (ret < 0) {
193 			regulator_bulk_disable(csid->num_supplies,
194 					       csid->supplies);
195 			pm_runtime_put_sync(dev);
196 			return ret;
197 		}
198 
199 		csid->phy.need_vc_update = true;
200 
201 		enable_irq(csid->irq);
202 
203 		ret = csid->ops->reset(csid);
204 		if (ret < 0) {
205 			disable_irq(csid->irq);
206 			camss_disable_clocks(csid->nclocks, csid->clock);
207 			regulator_bulk_disable(csid->num_supplies,
208 					       csid->supplies);
209 			pm_runtime_put_sync(dev);
210 			return ret;
211 		}
212 
213 		csid->ops->hw_version(csid);
214 	} else {
215 		disable_irq(csid->irq);
216 		camss_disable_clocks(csid->nclocks, csid->clock);
217 		regulator_bulk_disable(csid->num_supplies,
218 				       csid->supplies);
219 		pm_runtime_put_sync(dev);
220 		if (version == CAMSS_8250 || version == CAMSS_845)
221 			vfe_put(vfe);
222 	}
223 
224 	return ret;
225 }
226 
227 /*
228  * csid_set_stream - Enable/disable streaming on CSID module
229  * @sd: CSID V4L2 subdevice
230  * @enable: Requested streaming state
231  *
232  * Main configuration of CSID module is also done here.
233  *
234  * Return 0 on success or a negative error code otherwise
235  */
236 static int csid_set_stream(struct v4l2_subdev *sd, int enable)
237 {
238 	struct csid_device *csid = v4l2_get_subdevdata(sd);
239 	int ret;
240 
241 	if (enable) {
242 		ret = v4l2_ctrl_handler_setup(&csid->ctrls);
243 		if (ret < 0) {
244 			dev_err(csid->camss->dev,
245 				"could not sync v4l2 controls: %d\n", ret);
246 			return ret;
247 		}
248 
249 		if (!csid->testgen.enabled &&
250 		    !media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK]))
251 			return -ENOLINK;
252 	}
253 
254 	if (csid->phy.need_vc_update) {
255 		csid->ops->configure_stream(csid, enable);
256 		csid->phy.need_vc_update = false;
257 	}
258 
259 	return 0;
260 }
261 
262 /*
263  * __csid_get_format - Get pointer to format structure
264  * @csid: CSID device
265  * @cfg: V4L2 subdev pad configuration
266  * @pad: pad from which format is requested
267  * @which: TRY or ACTIVE format
268  *
269  * Return pointer to TRY or ACTIVE format structure
270  */
271 static struct v4l2_mbus_framefmt *
272 __csid_get_format(struct csid_device *csid,
273 		  struct v4l2_subdev_state *sd_state,
274 		  unsigned int pad,
275 		  enum v4l2_subdev_format_whence which)
276 {
277 	if (which == V4L2_SUBDEV_FORMAT_TRY)
278 		return v4l2_subdev_get_try_format(&csid->subdev, sd_state,
279 						  pad);
280 
281 	return &csid->fmt[pad];
282 }
283 
284 /*
285  * csid_try_format - Handle try format by pad subdev method
286  * @csid: CSID device
287  * @cfg: V4L2 subdev pad configuration
288  * @pad: pad on which format is requested
289  * @fmt: pointer to v4l2 format structure
290  * @which: wanted subdev format
291  */
292 static void csid_try_format(struct csid_device *csid,
293 			    struct v4l2_subdev_state *sd_state,
294 			    unsigned int pad,
295 			    struct v4l2_mbus_framefmt *fmt,
296 			    enum v4l2_subdev_format_whence which)
297 {
298 	unsigned int i;
299 
300 	switch (pad) {
301 	case MSM_CSID_PAD_SINK:
302 		/* Set format on sink pad */
303 
304 		for (i = 0; i < csid->nformats; i++)
305 			if (fmt->code == csid->formats[i].code)
306 				break;
307 
308 		/* If not found, use UYVY as default */
309 		if (i >= csid->nformats)
310 			fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
311 
312 		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
313 		fmt->height = clamp_t(u32, fmt->height, 1, 8191);
314 
315 		fmt->field = V4L2_FIELD_NONE;
316 		fmt->colorspace = V4L2_COLORSPACE_SRGB;
317 
318 		break;
319 
320 	case MSM_CSID_PAD_SRC:
321 		if (csid->testgen_mode->cur.val == 0) {
322 			/* Test generator is disabled, */
323 			/* keep pad formats in sync */
324 			u32 code = fmt->code;
325 
326 			*fmt = *__csid_get_format(csid, sd_state,
327 						      MSM_CSID_PAD_SINK, which);
328 			fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);
329 		} else {
330 			/* Test generator is enabled, set format on source */
331 			/* pad to allow test generator usage */
332 
333 			for (i = 0; i < csid->nformats; i++)
334 				if (csid->formats[i].code == fmt->code)
335 					break;
336 
337 			/* If not found, use UYVY as default */
338 			if (i >= csid->nformats)
339 				fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
340 
341 			fmt->width = clamp_t(u32, fmt->width, 1, 8191);
342 			fmt->height = clamp_t(u32, fmt->height, 1, 8191);
343 
344 			fmt->field = V4L2_FIELD_NONE;
345 		}
346 		break;
347 	}
348 
349 	fmt->colorspace = V4L2_COLORSPACE_SRGB;
350 }
351 
352 /*
353  * csid_enum_mbus_code - Handle pixel format enumeration
354  * @sd: CSID V4L2 subdevice
355  * @cfg: V4L2 subdev pad configuration
356  * @code: pointer to v4l2_subdev_mbus_code_enum structure
357  * return -EINVAL or zero on success
358  */
359 static int csid_enum_mbus_code(struct v4l2_subdev *sd,
360 			       struct v4l2_subdev_state *sd_state,
361 			       struct v4l2_subdev_mbus_code_enum *code)
362 {
363 	struct csid_device *csid = v4l2_get_subdevdata(sd);
364 
365 	if (code->pad == MSM_CSID_PAD_SINK) {
366 		if (code->index >= csid->nformats)
367 			return -EINVAL;
368 
369 		code->code = csid->formats[code->index].code;
370 	} else {
371 		if (csid->testgen_mode->cur.val == 0) {
372 			struct v4l2_mbus_framefmt *sink_fmt;
373 
374 			sink_fmt = __csid_get_format(csid, sd_state,
375 						     MSM_CSID_PAD_SINK,
376 						     code->which);
377 
378 			code->code = csid->ops->src_pad_code(csid, sink_fmt->code,
379 						       code->index, 0);
380 			if (!code->code)
381 				return -EINVAL;
382 		} else {
383 			if (code->index >= csid->nformats)
384 				return -EINVAL;
385 
386 			code->code = csid->formats[code->index].code;
387 		}
388 	}
389 
390 	return 0;
391 }
392 
393 /*
394  * csid_enum_frame_size - Handle frame size enumeration
395  * @sd: CSID V4L2 subdevice
396  * @cfg: V4L2 subdev pad configuration
397  * @fse: pointer to v4l2_subdev_frame_size_enum structure
398  * return -EINVAL or zero on success
399  */
400 static int csid_enum_frame_size(struct v4l2_subdev *sd,
401 				struct v4l2_subdev_state *sd_state,
402 				struct v4l2_subdev_frame_size_enum *fse)
403 {
404 	struct csid_device *csid = v4l2_get_subdevdata(sd);
405 	struct v4l2_mbus_framefmt format;
406 
407 	if (fse->index != 0)
408 		return -EINVAL;
409 
410 	format.code = fse->code;
411 	format.width = 1;
412 	format.height = 1;
413 	csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
414 	fse->min_width = format.width;
415 	fse->min_height = format.height;
416 
417 	if (format.code != fse->code)
418 		return -EINVAL;
419 
420 	format.code = fse->code;
421 	format.width = -1;
422 	format.height = -1;
423 	csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
424 	fse->max_width = format.width;
425 	fse->max_height = format.height;
426 
427 	return 0;
428 }
429 
430 /*
431  * csid_get_format - Handle get format by pads subdev method
432  * @sd: CSID V4L2 subdevice
433  * @cfg: V4L2 subdev pad configuration
434  * @fmt: pointer to v4l2 subdev format structure
435  *
436  * Return -EINVAL or zero on success
437  */
438 static int csid_get_format(struct v4l2_subdev *sd,
439 			   struct v4l2_subdev_state *sd_state,
440 			   struct v4l2_subdev_format *fmt)
441 {
442 	struct csid_device *csid = v4l2_get_subdevdata(sd);
443 	struct v4l2_mbus_framefmt *format;
444 
445 	format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
446 	if (format == NULL)
447 		return -EINVAL;
448 
449 	fmt->format = *format;
450 
451 	return 0;
452 }
453 
454 /*
455  * csid_set_format - Handle set format by pads subdev method
456  * @sd: CSID V4L2 subdevice
457  * @cfg: V4L2 subdev pad configuration
458  * @fmt: pointer to v4l2 subdev format structure
459  *
460  * Return -EINVAL or zero on success
461  */
462 static int csid_set_format(struct v4l2_subdev *sd,
463 			   struct v4l2_subdev_state *sd_state,
464 			   struct v4l2_subdev_format *fmt)
465 {
466 	struct csid_device *csid = v4l2_get_subdevdata(sd);
467 	struct v4l2_mbus_framefmt *format;
468 	int i;
469 
470 	format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
471 	if (format == NULL)
472 		return -EINVAL;
473 
474 	csid_try_format(csid, sd_state, fmt->pad, &fmt->format, fmt->which);
475 	*format = fmt->format;
476 
477 	/* Propagate the format from sink to source pads */
478 	if (fmt->pad == MSM_CSID_PAD_SINK) {
479 		for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i) {
480 			format = __csid_get_format(csid, sd_state, i, fmt->which);
481 
482 			*format = fmt->format;
483 			csid_try_format(csid, sd_state, i, format, fmt->which);
484 		}
485 	}
486 
487 	return 0;
488 }
489 
490 /*
491  * csid_init_formats - Initialize formats on all pads
492  * @sd: CSID V4L2 subdevice
493  * @fh: V4L2 subdev file handle
494  *
495  * Initialize all pad formats with default values.
496  *
497  * Return 0 on success or a negative error code otherwise
498  */
499 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
500 {
501 	struct v4l2_subdev_format format = {
502 		.pad = MSM_CSID_PAD_SINK,
503 		.which = fh ? V4L2_SUBDEV_FORMAT_TRY :
504 			      V4L2_SUBDEV_FORMAT_ACTIVE,
505 		.format = {
506 			.code = MEDIA_BUS_FMT_UYVY8_2X8,
507 			.width = 1920,
508 			.height = 1080
509 		}
510 	};
511 
512 	return csid_set_format(sd, fh ? fh->state : NULL, &format);
513 }
514 
515 /*
516  * csid_set_test_pattern - Set test generator's pattern mode
517  * @csid: CSID device
518  * @value: desired test pattern mode
519  *
520  * Return 0 on success or a negative error code otherwise
521  */
522 static int csid_set_test_pattern(struct csid_device *csid, s32 value)
523 {
524 	struct csid_testgen_config *tg = &csid->testgen;
525 
526 	/* If CSID is linked to CSIPHY, do not allow to enable test generator */
527 	if (value && media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK]))
528 		return -EBUSY;
529 
530 	tg->enabled = !!value;
531 
532 	return csid->ops->configure_testgen_pattern(csid, value);
533 }
534 
535 /*
536  * csid_s_ctrl - Handle set control subdev method
537  * @ctrl: pointer to v4l2 control structure
538  *
539  * Return 0 on success or a negative error code otherwise
540  */
541 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
542 {
543 	struct csid_device *csid = container_of(ctrl->handler,
544 						struct csid_device, ctrls);
545 	int ret = -EINVAL;
546 
547 	switch (ctrl->id) {
548 	case V4L2_CID_TEST_PATTERN:
549 		ret = csid_set_test_pattern(csid, ctrl->val);
550 		break;
551 	}
552 
553 	return ret;
554 }
555 
556 static const struct v4l2_ctrl_ops csid_ctrl_ops = {
557 	.s_ctrl = csid_s_ctrl,
558 };
559 
560 /*
561  * msm_csid_subdev_init - Initialize CSID device structure and resources
562  * @csid: CSID device
563  * @res: CSID module resources table
564  * @id: CSID module id
565  *
566  * Return 0 on success or a negative error code otherwise
567  */
568 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
569 			 const struct resources *res, u8 id)
570 {
571 	struct device *dev = camss->dev;
572 	struct platform_device *pdev = to_platform_device(dev);
573 	int i, j;
574 	int ret;
575 
576 	csid->camss = camss;
577 	csid->id = id;
578 
579 	if (camss->version == CAMSS_8x16) {
580 		csid->ops = &csid_ops_4_1;
581 	} else if (camss->version == CAMSS_8x96 ||
582 		   camss->version == CAMSS_660) {
583 		csid->ops = &csid_ops_4_7;
584 	} else if (camss->version == CAMSS_845 ||
585 		   camss->version == CAMSS_8250) {
586 		csid->ops = &csid_ops_gen2;
587 	} else {
588 		return -EINVAL;
589 	}
590 	csid->ops->subdev_init(csid);
591 
592 	/* Memory */
593 
594 	if (camss->version == CAMSS_8250) {
595 		/* for titan 480, CSID registers are inside the VFE region,
596 		 * between the VFE "top" and "bus" registers. this requires
597 		 * VFE to be initialized before CSID
598 		 */
599 		if (id >= 2) /* VFE/CSID lite */
600 			csid->base = camss->vfe[id].base + VFE_480_LITE_CSID_OFFSET;
601 		else
602 			csid->base = camss->vfe[id].base + VFE_480_CSID_OFFSET;
603 	} else {
604 		csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
605 		if (IS_ERR(csid->base))
606 			return PTR_ERR(csid->base);
607 	}
608 
609 	/* Interrupt */
610 
611 	ret = platform_get_irq_byname(pdev, res->interrupt[0]);
612 	if (ret < 0)
613 		return ret;
614 
615 	csid->irq = ret;
616 	snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
617 		 dev_name(dev), MSM_CSID_NAME, csid->id);
618 	ret = devm_request_irq(dev, csid->irq, csid->ops->isr,
619 			       IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,
620 			       csid->irq_name, csid);
621 	if (ret < 0) {
622 		dev_err(dev, "request_irq failed: %d\n", ret);
623 		return ret;
624 	}
625 
626 	/* Clocks */
627 
628 	csid->nclocks = 0;
629 	while (res->clock[csid->nclocks])
630 		csid->nclocks++;
631 
632 	csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock),
633 				    GFP_KERNEL);
634 	if (!csid->clock)
635 		return -ENOMEM;
636 
637 	for (i = 0; i < csid->nclocks; i++) {
638 		struct camss_clock *clock = &csid->clock[i];
639 
640 		clock->clk = devm_clk_get(dev, res->clock[i]);
641 		if (IS_ERR(clock->clk))
642 			return PTR_ERR(clock->clk);
643 
644 		clock->name = res->clock[i];
645 
646 		clock->nfreqs = 0;
647 		while (res->clock_rate[i][clock->nfreqs])
648 			clock->nfreqs++;
649 
650 		if (!clock->nfreqs) {
651 			clock->freq = NULL;
652 			continue;
653 		}
654 
655 		clock->freq = devm_kcalloc(dev,
656 					   clock->nfreqs,
657 					   sizeof(*clock->freq),
658 					   GFP_KERNEL);
659 		if (!clock->freq)
660 			return -ENOMEM;
661 
662 		for (j = 0; j < clock->nfreqs; j++)
663 			clock->freq[j] = res->clock_rate[i][j];
664 	}
665 
666 	/* Regulator */
667 	for (i = 0; i < ARRAY_SIZE(res->regulators); i++) {
668 		if (res->regulators[i])
669 			csid->num_supplies++;
670 	}
671 
672 	if (csid->num_supplies) {
673 		csid->supplies = devm_kmalloc_array(camss->dev,
674 						    csid->num_supplies,
675 						    sizeof(*csid->supplies),
676 						    GFP_KERNEL);
677 		if (!csid->supplies)
678 			return -ENOMEM;
679 	}
680 
681 	for (i = 0; i < csid->num_supplies; i++)
682 		csid->supplies[i].supply = res->regulators[i];
683 
684 	ret = devm_regulator_bulk_get(camss->dev, csid->num_supplies,
685 				      csid->supplies);
686 	if (ret)
687 		return ret;
688 
689 	init_completion(&csid->reset_complete);
690 
691 	return 0;
692 }
693 
694 /*
695  * msm_csid_get_csid_id - Get CSID HW module id
696  * @entity: Pointer to CSID media entity structure
697  * @id: Return CSID HW module id here
698  */
699 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
700 {
701 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
702 	struct csid_device *csid = v4l2_get_subdevdata(sd);
703 
704 	*id = csid->id;
705 }
706 
707 /*
708  * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
709  * @lane_cfg - CSI2 lane configuration
710  *
711  * Return lane assign
712  */
713 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
714 {
715 	u32 lane_assign = 0;
716 	int i;
717 
718 	for (i = 0; i < lane_cfg->num_data; i++)
719 		lane_assign |= lane_cfg->data[i].pos << (i * 4);
720 
721 	return lane_assign;
722 }
723 
724 /*
725  * csid_link_setup - Setup CSID connections
726  * @entity: Pointer to media entity structure
727  * @local: Pointer to local pad
728  * @remote: Pointer to remote pad
729  * @flags: Link flags
730  *
731  * Return 0 on success
732  */
733 static int csid_link_setup(struct media_entity *entity,
734 			   const struct media_pad *local,
735 			   const struct media_pad *remote, u32 flags)
736 {
737 	if (flags & MEDIA_LNK_FL_ENABLED)
738 		if (media_pad_remote_pad_first(local))
739 			return -EBUSY;
740 
741 	if ((local->flags & MEDIA_PAD_FL_SINK) &&
742 	    (flags & MEDIA_LNK_FL_ENABLED)) {
743 		struct v4l2_subdev *sd;
744 		struct csid_device *csid;
745 		struct csiphy_device *csiphy;
746 		struct csiphy_lanes_cfg *lane_cfg;
747 
748 		sd = media_entity_to_v4l2_subdev(entity);
749 		csid = v4l2_get_subdevdata(sd);
750 
751 		/* If test generator is enabled */
752 		/* do not allow a link from CSIPHY to CSID */
753 		if (csid->testgen_mode->cur.val != 0)
754 			return -EBUSY;
755 
756 		sd = media_entity_to_v4l2_subdev(remote->entity);
757 		csiphy = v4l2_get_subdevdata(sd);
758 
759 		/* If a sensor is not linked to CSIPHY */
760 		/* do no allow a link from CSIPHY to CSID */
761 		if (!csiphy->cfg.csi2)
762 			return -EPERM;
763 
764 		csid->phy.csiphy_id = csiphy->id;
765 
766 		lane_cfg = &csiphy->cfg.csi2->lane_cfg;
767 		csid->phy.lane_cnt = lane_cfg->num_data;
768 		csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
769 	}
770 	/* Decide which virtual channels to enable based on which source pads are enabled */
771 	if (local->flags & MEDIA_PAD_FL_SOURCE) {
772 		struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
773 		struct csid_device *csid = v4l2_get_subdevdata(sd);
774 		struct device *dev = csid->camss->dev;
775 
776 		if (flags & MEDIA_LNK_FL_ENABLED)
777 			csid->phy.en_vc |= BIT(local->index - 1);
778 		else
779 			csid->phy.en_vc &= ~BIT(local->index - 1);
780 
781 		csid->phy.need_vc_update = true;
782 
783 		dev_dbg(dev, "%s: Enabled CSID virtual channels mask 0x%x\n",
784 			__func__, csid->phy.en_vc);
785 	}
786 
787 	return 0;
788 }
789 
790 static const struct v4l2_subdev_core_ops csid_core_ops = {
791 	.s_power = csid_set_power,
792 	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
793 	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
794 };
795 
796 static const struct v4l2_subdev_video_ops csid_video_ops = {
797 	.s_stream = csid_set_stream,
798 };
799 
800 static const struct v4l2_subdev_pad_ops csid_pad_ops = {
801 	.enum_mbus_code = csid_enum_mbus_code,
802 	.enum_frame_size = csid_enum_frame_size,
803 	.get_fmt = csid_get_format,
804 	.set_fmt = csid_set_format,
805 };
806 
807 static const struct v4l2_subdev_ops csid_v4l2_ops = {
808 	.core = &csid_core_ops,
809 	.video = &csid_video_ops,
810 	.pad = &csid_pad_ops,
811 };
812 
813 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
814 	.open = csid_init_formats,
815 };
816 
817 static const struct media_entity_operations csid_media_ops = {
818 	.link_setup = csid_link_setup,
819 	.link_validate = v4l2_subdev_link_validate,
820 };
821 
822 /*
823  * msm_csid_register_entity - Register subdev node for CSID module
824  * @csid: CSID device
825  * @v4l2_dev: V4L2 device
826  *
827  * Return 0 on success or a negative error code otherwise
828  */
829 int msm_csid_register_entity(struct csid_device *csid,
830 			     struct v4l2_device *v4l2_dev)
831 {
832 	struct v4l2_subdev *sd = &csid->subdev;
833 	struct media_pad *pads = csid->pads;
834 	struct device *dev = csid->camss->dev;
835 	int i;
836 	int ret;
837 
838 	v4l2_subdev_init(sd, &csid_v4l2_ops);
839 	sd->internal_ops = &csid_v4l2_internal_ops;
840 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
841 		     V4L2_SUBDEV_FL_HAS_EVENTS;
842 	snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
843 		 MSM_CSID_NAME, csid->id);
844 	v4l2_set_subdevdata(sd, csid);
845 
846 	ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
847 	if (ret < 0) {
848 		dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
849 		return ret;
850 	}
851 
852 	csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
853 				&csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
854 				csid->testgen.nmodes, 0, 0,
855 				csid->testgen.modes);
856 
857 	if (csid->ctrls.error) {
858 		dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
859 		ret = csid->ctrls.error;
860 		goto free_ctrl;
861 	}
862 
863 	csid->subdev.ctrl_handler = &csid->ctrls;
864 
865 	ret = csid_init_formats(sd, NULL);
866 	if (ret < 0) {
867 		dev_err(dev, "Failed to init format: %d\n", ret);
868 		goto free_ctrl;
869 	}
870 
871 	pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
872 	for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i)
873 		pads[i].flags = MEDIA_PAD_FL_SOURCE;
874 
875 	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
876 	sd->entity.ops = &csid_media_ops;
877 	ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
878 	if (ret < 0) {
879 		dev_err(dev, "Failed to init media entity: %d\n", ret);
880 		goto free_ctrl;
881 	}
882 
883 	ret = v4l2_device_register_subdev(v4l2_dev, sd);
884 	if (ret < 0) {
885 		dev_err(dev, "Failed to register subdev: %d\n", ret);
886 		goto media_cleanup;
887 	}
888 
889 	return 0;
890 
891 media_cleanup:
892 	media_entity_cleanup(&sd->entity);
893 free_ctrl:
894 	v4l2_ctrl_handler_free(&csid->ctrls);
895 
896 	return ret;
897 }
898 
899 /*
900  * msm_csid_unregister_entity - Unregister CSID module subdev node
901  * @csid: CSID device
902  */
903 void msm_csid_unregister_entity(struct csid_device *csid)
904 {
905 	v4l2_device_unregister_subdev(&csid->subdev);
906 	media_entity_cleanup(&csid->subdev.entity);
907 	v4l2_ctrl_handler_free(&csid->ctrls);
908 }
909