1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Copyright (C) 2020 NVIDIA CORPORATION.  All rights reserved.
4  */
5 
6 #include <linux/bitmap.h>
7 #include <linux/clk.h>
8 #include <linux/delay.h>
9 #include <linux/host1x.h>
10 #include <linux/lcm.h>
11 #include <linux/list.h>
12 #include <linux/module.h>
13 #include <linux/of.h>
14 #include <linux/of_device.h>
15 #include <linux/of_graph.h>
16 #include <linux/platform_device.h>
17 #include <linux/regulator/consumer.h>
18 #include <linux/pm_runtime.h>
19 #include <linux/slab.h>
20 
21 #include <media/v4l2-dv-timings.h>
22 #include <media/v4l2-event.h>
23 #include <media/v4l2-fh.h>
24 #include <media/v4l2-fwnode.h>
25 #include <media/v4l2-ioctl.h>
26 #include <media/videobuf2-dma-contig.h>
27 
28 #include <soc/tegra/pmc.h>
29 
30 #include "vi.h"
31 #include "video.h"
32 
33 #define MAX_CID_CONTROLS		1
34 
35 static const struct tegra_video_format tegra_default_format = {
36 	.img_dt = TEGRA_IMAGE_DT_RAW10,
37 	.bit_width = 10,
38 	.code = MEDIA_BUS_FMT_SRGGB10_1X10,
39 	.bpp = 2,
40 	.img_fmt = TEGRA_IMAGE_FORMAT_DEF,
41 	.fourcc = V4L2_PIX_FMT_SRGGB10,
42 };
43 
44 static inline struct tegra_vi *
host1x_client_to_vi(struct host1x_client * client)45 host1x_client_to_vi(struct host1x_client *client)
46 {
47 	return container_of(client, struct tegra_vi, client);
48 }
49 
50 static inline struct tegra_channel_buffer *
to_tegra_channel_buffer(struct vb2_v4l2_buffer * vb)51 to_tegra_channel_buffer(struct vb2_v4l2_buffer *vb)
52 {
53 	return container_of(vb, struct tegra_channel_buffer, buf);
54 }
55 
56 static inline struct tegra_vi_graph_entity *
to_tegra_vi_graph_entity(struct v4l2_async_subdev * asd)57 to_tegra_vi_graph_entity(struct v4l2_async_subdev *asd)
58 {
59 	return container_of(asd, struct tegra_vi_graph_entity, asd);
60 }
61 
tegra_get_format_idx_by_code(struct tegra_vi * vi,unsigned int code,unsigned int offset)62 static int tegra_get_format_idx_by_code(struct tegra_vi *vi,
63 					unsigned int code,
64 					unsigned int offset)
65 {
66 	unsigned int i;
67 
68 	for (i = offset; i < vi->soc->nformats; ++i) {
69 		if (vi->soc->video_formats[i].code == code)
70 			return i;
71 	}
72 
73 	return -1;
74 }
75 
tegra_get_format_fourcc_by_idx(struct tegra_vi * vi,unsigned int index)76 static u32 tegra_get_format_fourcc_by_idx(struct tegra_vi *vi,
77 					  unsigned int index)
78 {
79 	if (index >= vi->soc->nformats)
80 		return -EINVAL;
81 
82 	return vi->soc->video_formats[index].fourcc;
83 }
84 
85 static const struct tegra_video_format *
tegra_get_format_by_fourcc(struct tegra_vi * vi,u32 fourcc)86 tegra_get_format_by_fourcc(struct tegra_vi *vi, u32 fourcc)
87 {
88 	unsigned int i;
89 
90 	for (i = 0; i < vi->soc->nformats; ++i) {
91 		if (vi->soc->video_formats[i].fourcc == fourcc)
92 			return &vi->soc->video_formats[i];
93 	}
94 
95 	return NULL;
96 }
97 
98 /*
99  * videobuf2 queue operations
100  */
tegra_channel_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])101 static int tegra_channel_queue_setup(struct vb2_queue *vq,
102 				     unsigned int *nbuffers,
103 				     unsigned int *nplanes,
104 				     unsigned int sizes[],
105 				     struct device *alloc_devs[])
106 {
107 	struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
108 
109 	if (*nplanes)
110 		return sizes[0] < chan->format.sizeimage ? -EINVAL : 0;
111 
112 	*nplanes = 1;
113 	sizes[0] = chan->format.sizeimage;
114 	alloc_devs[0] = chan->vi->dev;
115 
116 	return 0;
117 }
118 
tegra_channel_buffer_prepare(struct vb2_buffer * vb)119 static int tegra_channel_buffer_prepare(struct vb2_buffer *vb)
120 {
121 	struct tegra_vi_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
122 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
123 	struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vbuf);
124 	unsigned long size = chan->format.sizeimage;
125 
126 	if (vb2_plane_size(vb, 0) < size) {
127 		v4l2_err(chan->video.v4l2_dev,
128 			 "buffer too small (%lu < %lu)\n",
129 			 vb2_plane_size(vb, 0), size);
130 		return -EINVAL;
131 	}
132 
133 	vb2_set_plane_payload(vb, 0, size);
134 	buf->chan = chan;
135 	buf->addr = vb2_dma_contig_plane_dma_addr(vb, 0);
136 
137 	return 0;
138 }
139 
tegra_channel_buffer_queue(struct vb2_buffer * vb)140 static void tegra_channel_buffer_queue(struct vb2_buffer *vb)
141 {
142 	struct tegra_vi_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
143 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
144 	struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vbuf);
145 
146 	/* put buffer into the capture queue */
147 	spin_lock(&chan->start_lock);
148 	list_add_tail(&buf->queue, &chan->capture);
149 	spin_unlock(&chan->start_lock);
150 
151 	/* wait up kthread for capture */
152 	wake_up_interruptible(&chan->start_wait);
153 }
154 
155 struct v4l2_subdev *
tegra_channel_get_remote_csi_subdev(struct tegra_vi_channel * chan)156 tegra_channel_get_remote_csi_subdev(struct tegra_vi_channel *chan)
157 {
158 	struct media_pad *pad;
159 
160 	pad = media_pad_remote_pad_first(&chan->pad);
161 	if (!pad)
162 		return NULL;
163 
164 	return media_entity_to_v4l2_subdev(pad->entity);
165 }
166 
167 struct v4l2_subdev *
tegra_channel_get_remote_source_subdev(struct tegra_vi_channel * chan)168 tegra_channel_get_remote_source_subdev(struct tegra_vi_channel *chan)
169 {
170 	struct media_pad *pad;
171 	struct v4l2_subdev *subdev;
172 	struct media_entity *entity;
173 
174 	subdev = tegra_channel_get_remote_csi_subdev(chan);
175 	if (!subdev)
176 		return NULL;
177 
178 	pad = &subdev->entity.pads[0];
179 	while (!(pad->flags & MEDIA_PAD_FL_SOURCE)) {
180 		pad = media_pad_remote_pad_first(pad);
181 		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
182 			break;
183 		entity = pad->entity;
184 		pad = &entity->pads[0];
185 		subdev = media_entity_to_v4l2_subdev(entity);
186 	}
187 
188 	return subdev;
189 }
190 
tegra_channel_enable_stream(struct tegra_vi_channel * chan)191 static int tegra_channel_enable_stream(struct tegra_vi_channel *chan)
192 {
193 	struct v4l2_subdev *csi_subdev, *src_subdev;
194 	struct tegra_csi_channel *csi_chan;
195 	int ret, err;
196 
197 	/*
198 	 * Tegra CSI receiver can detect the first LP to HS transition.
199 	 * So, start the CSI stream-on prior to sensor stream-on and
200 	 * vice-versa for stream-off.
201 	 */
202 	csi_subdev = tegra_channel_get_remote_csi_subdev(chan);
203 	ret = v4l2_subdev_call(csi_subdev, video, s_stream, true);
204 	if (ret < 0 && ret != -ENOIOCTLCMD)
205 		return ret;
206 
207 	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
208 		return 0;
209 
210 	csi_chan = v4l2_get_subdevdata(csi_subdev);
211 	/*
212 	 * TRM has incorrectly documented to wait for done status from
213 	 * calibration logic after CSI interface power on.
214 	 * As per the design, calibration results are latched and applied
215 	 * to the pads only when the link is in LP11 state which will happen
216 	 * during the sensor stream-on.
217 	 * CSI subdev stream-on triggers start of MIPI pads calibration.
218 	 * Wait for calibration to finish here after sensor subdev stream-on.
219 	 */
220 	src_subdev = tegra_channel_get_remote_source_subdev(chan);
221 	ret = v4l2_subdev_call(src_subdev, video, s_stream, true);
222 	err = tegra_mipi_finish_calibration(csi_chan->mipi);
223 
224 	if (ret < 0 && ret != -ENOIOCTLCMD)
225 		goto err_disable_csi_stream;
226 
227 	if (err < 0)
228 		dev_warn(csi_chan->csi->dev,
229 			 "MIPI calibration failed: %d\n", err);
230 
231 	return 0;
232 
233 err_disable_csi_stream:
234 	v4l2_subdev_call(csi_subdev, video, s_stream, false);
235 	return ret;
236 }
237 
tegra_channel_disable_stream(struct tegra_vi_channel * chan)238 static int tegra_channel_disable_stream(struct tegra_vi_channel *chan)
239 {
240 	struct v4l2_subdev *subdev;
241 	int ret;
242 
243 	/*
244 	 * Stream-off subdevices in reverse order to stream-on.
245 	 * Remote source subdev in TPG mode is same as CSI subdev.
246 	 */
247 	subdev = tegra_channel_get_remote_source_subdev(chan);
248 	ret = v4l2_subdev_call(subdev, video, s_stream, false);
249 	if (ret < 0 && ret != -ENOIOCTLCMD)
250 		return ret;
251 
252 	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
253 		return 0;
254 
255 	subdev = tegra_channel_get_remote_csi_subdev(chan);
256 	ret = v4l2_subdev_call(subdev, video, s_stream, false);
257 	if (ret < 0 && ret != -ENOIOCTLCMD)
258 		return ret;
259 
260 	return 0;
261 }
262 
tegra_channel_set_stream(struct tegra_vi_channel * chan,bool on)263 int tegra_channel_set_stream(struct tegra_vi_channel *chan, bool on)
264 {
265 	int ret;
266 
267 	if (on)
268 		ret = tegra_channel_enable_stream(chan);
269 	else
270 		ret = tegra_channel_disable_stream(chan);
271 
272 	return ret;
273 }
274 
tegra_channel_release_buffers(struct tegra_vi_channel * chan,enum vb2_buffer_state state)275 void tegra_channel_release_buffers(struct tegra_vi_channel *chan,
276 				   enum vb2_buffer_state state)
277 {
278 	struct tegra_channel_buffer *buf, *nbuf;
279 
280 	spin_lock(&chan->start_lock);
281 	list_for_each_entry_safe(buf, nbuf, &chan->capture, queue) {
282 		vb2_buffer_done(&buf->buf.vb2_buf, state);
283 		list_del(&buf->queue);
284 	}
285 	spin_unlock(&chan->start_lock);
286 
287 	spin_lock(&chan->done_lock);
288 	list_for_each_entry_safe(buf, nbuf, &chan->done, queue) {
289 		vb2_buffer_done(&buf->buf.vb2_buf, state);
290 		list_del(&buf->queue);
291 	}
292 	spin_unlock(&chan->done_lock);
293 }
294 
tegra_channel_start_streaming(struct vb2_queue * vq,u32 count)295 static int tegra_channel_start_streaming(struct vb2_queue *vq, u32 count)
296 {
297 	struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
298 	int ret;
299 
300 	ret = pm_runtime_resume_and_get(chan->vi->dev);
301 	if (ret < 0) {
302 		dev_err(chan->vi->dev, "failed to get runtime PM: %d\n", ret);
303 		return ret;
304 	}
305 
306 	ret = chan->vi->ops->vi_start_streaming(vq, count);
307 	if (ret < 0)
308 		pm_runtime_put(chan->vi->dev);
309 
310 	return ret;
311 }
312 
tegra_channel_stop_streaming(struct vb2_queue * vq)313 static void tegra_channel_stop_streaming(struct vb2_queue *vq)
314 {
315 	struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
316 
317 	chan->vi->ops->vi_stop_streaming(vq);
318 	pm_runtime_put(chan->vi->dev);
319 }
320 
321 static const struct vb2_ops tegra_channel_queue_qops = {
322 	.queue_setup = tegra_channel_queue_setup,
323 	.buf_prepare = tegra_channel_buffer_prepare,
324 	.buf_queue = tegra_channel_buffer_queue,
325 	.wait_prepare = vb2_ops_wait_prepare,
326 	.wait_finish = vb2_ops_wait_finish,
327 	.start_streaming = tegra_channel_start_streaming,
328 	.stop_streaming = tegra_channel_stop_streaming,
329 };
330 
331 /*
332  * V4L2 ioctl operations
333  */
tegra_channel_querycap(struct file * file,void * fh,struct v4l2_capability * cap)334 static int tegra_channel_querycap(struct file *file, void *fh,
335 				  struct v4l2_capability *cap)
336 {
337 	struct tegra_vi_channel *chan = video_drvdata(file);
338 
339 	strscpy(cap->driver, "tegra-video", sizeof(cap->driver));
340 	strscpy(cap->card, chan->video.name, sizeof(cap->card));
341 	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
342 		 dev_name(chan->vi->dev));
343 
344 	return 0;
345 }
346 
tegra_channel_g_parm(struct file * file,void * fh,struct v4l2_streamparm * a)347 static int tegra_channel_g_parm(struct file *file, void *fh,
348 				struct v4l2_streamparm *a)
349 {
350 	struct tegra_vi_channel *chan = video_drvdata(file);
351 	struct v4l2_subdev *subdev;
352 
353 	subdev = tegra_channel_get_remote_source_subdev(chan);
354 	return v4l2_g_parm_cap(&chan->video, subdev, a);
355 }
356 
tegra_channel_s_parm(struct file * file,void * fh,struct v4l2_streamparm * a)357 static int tegra_channel_s_parm(struct file *file, void *fh,
358 				struct v4l2_streamparm *a)
359 {
360 	struct tegra_vi_channel *chan = video_drvdata(file);
361 	struct v4l2_subdev *subdev;
362 
363 	subdev = tegra_channel_get_remote_source_subdev(chan);
364 	return v4l2_s_parm_cap(&chan->video, subdev, a);
365 }
366 
tegra_channel_enum_framesizes(struct file * file,void * fh,struct v4l2_frmsizeenum * sizes)367 static int tegra_channel_enum_framesizes(struct file *file, void *fh,
368 					 struct v4l2_frmsizeenum *sizes)
369 {
370 	int ret;
371 	struct tegra_vi_channel *chan = video_drvdata(file);
372 	struct v4l2_subdev *subdev;
373 	const struct tegra_video_format *fmtinfo;
374 	struct v4l2_subdev_frame_size_enum fse = {
375 		.index = sizes->index,
376 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
377 	};
378 
379 	fmtinfo = tegra_get_format_by_fourcc(chan->vi, sizes->pixel_format);
380 	if (!fmtinfo)
381 		return -EINVAL;
382 
383 	fse.code = fmtinfo->code;
384 
385 	subdev = tegra_channel_get_remote_source_subdev(chan);
386 	ret = v4l2_subdev_call(subdev, pad, enum_frame_size, NULL, &fse);
387 	if (ret)
388 		return ret;
389 
390 	sizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
391 	sizes->discrete.width = fse.max_width;
392 	sizes->discrete.height = fse.max_height;
393 
394 	return 0;
395 }
396 
tegra_channel_enum_frameintervals(struct file * file,void * fh,struct v4l2_frmivalenum * ivals)397 static int tegra_channel_enum_frameintervals(struct file *file, void *fh,
398 					     struct v4l2_frmivalenum *ivals)
399 {
400 	int ret;
401 	struct tegra_vi_channel *chan = video_drvdata(file);
402 	struct v4l2_subdev *subdev;
403 	const struct tegra_video_format *fmtinfo;
404 	struct v4l2_subdev_frame_interval_enum fie = {
405 		.index = ivals->index,
406 		.width = ivals->width,
407 		.height = ivals->height,
408 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
409 	};
410 
411 	fmtinfo = tegra_get_format_by_fourcc(chan->vi, ivals->pixel_format);
412 	if (!fmtinfo)
413 		return -EINVAL;
414 
415 	fie.code = fmtinfo->code;
416 
417 	subdev = tegra_channel_get_remote_source_subdev(chan);
418 	ret = v4l2_subdev_call(subdev, pad, enum_frame_interval, NULL, &fie);
419 	if (ret)
420 		return ret;
421 
422 	ivals->type = V4L2_FRMIVAL_TYPE_DISCRETE;
423 	ivals->discrete.numerator = fie.interval.numerator;
424 	ivals->discrete.denominator = fie.interval.denominator;
425 
426 	return 0;
427 }
428 
tegra_channel_enum_format(struct file * file,void * fh,struct v4l2_fmtdesc * f)429 static int tegra_channel_enum_format(struct file *file, void *fh,
430 				     struct v4l2_fmtdesc *f)
431 {
432 	struct tegra_vi_channel *chan = video_drvdata(file);
433 	unsigned int index = 0, i;
434 	unsigned long *fmts_bitmap = chan->tpg_fmts_bitmap;
435 
436 	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
437 		fmts_bitmap = chan->fmts_bitmap;
438 
439 	if (f->index >= bitmap_weight(fmts_bitmap, MAX_FORMAT_NUM))
440 		return -EINVAL;
441 
442 	for (i = 0; i < f->index + 1; i++, index++)
443 		index = find_next_bit(fmts_bitmap, MAX_FORMAT_NUM, index);
444 
445 	f->pixelformat = tegra_get_format_fourcc_by_idx(chan->vi, index - 1);
446 
447 	return 0;
448 }
449 
tegra_channel_get_format(struct file * file,void * fh,struct v4l2_format * format)450 static int tegra_channel_get_format(struct file *file, void *fh,
451 				    struct v4l2_format *format)
452 {
453 	struct tegra_vi_channel *chan = video_drvdata(file);
454 
455 	format->fmt.pix = chan->format;
456 
457 	return 0;
458 }
459 
tegra_channel_fmt_align(struct tegra_vi_channel * chan,struct v4l2_pix_format * pix,unsigned int bpp)460 static void tegra_channel_fmt_align(struct tegra_vi_channel *chan,
461 				    struct v4l2_pix_format *pix,
462 				    unsigned int bpp)
463 {
464 	unsigned int min_bpl;
465 	unsigned int max_bpl;
466 	unsigned int bpl;
467 
468 	/*
469 	 * The transfer alignment requirements are expressed in bytes.
470 	 * Clamp the requested width and height to the limits.
471 	 */
472 	pix->width = clamp(pix->width, TEGRA_MIN_WIDTH, TEGRA_MAX_WIDTH);
473 	pix->height = clamp(pix->height, TEGRA_MIN_HEIGHT, TEGRA_MAX_HEIGHT);
474 
475 	/* Clamp the requested bytes per line value. If the maximum bytes per
476 	 * line value is zero, the module doesn't support user configurable
477 	 * line sizes. Override the requested value with the minimum in that
478 	 * case.
479 	 */
480 	min_bpl = pix->width * bpp;
481 	max_bpl = rounddown(TEGRA_MAX_WIDTH, SURFACE_ALIGN_BYTES);
482 	bpl = roundup(pix->bytesperline, SURFACE_ALIGN_BYTES);
483 
484 	pix->bytesperline = clamp(bpl, min_bpl, max_bpl);
485 	pix->sizeimage = pix->bytesperline * pix->height;
486 	if (pix->pixelformat == V4L2_PIX_FMT_NV16)
487 		pix->sizeimage *= 2;
488 }
489 
__tegra_channel_try_format(struct tegra_vi_channel * chan,struct v4l2_pix_format * pix)490 static int __tegra_channel_try_format(struct tegra_vi_channel *chan,
491 				      struct v4l2_pix_format *pix)
492 {
493 	const struct tegra_video_format *fmtinfo;
494 	static struct lock_class_key key;
495 	struct v4l2_subdev *subdev;
496 	struct v4l2_subdev_format fmt;
497 	struct v4l2_subdev_state *sd_state;
498 	struct v4l2_subdev_frame_size_enum fse = {
499 		.which = V4L2_SUBDEV_FORMAT_TRY,
500 	};
501 	struct v4l2_subdev_selection sdsel = {
502 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
503 		.target = V4L2_SEL_TGT_CROP_BOUNDS,
504 	};
505 	int ret;
506 
507 	subdev = tegra_channel_get_remote_source_subdev(chan);
508 	if (!subdev)
509 		return -ENODEV;
510 
511 	/*
512 	 * FIXME: Drop this call, drivers are not supposed to use
513 	 * __v4l2_subdev_state_alloc().
514 	 */
515 	sd_state = __v4l2_subdev_state_alloc(subdev, "tegra:state->lock",
516 					     &key);
517 	if (IS_ERR(sd_state))
518 		return PTR_ERR(sd_state);
519 	/*
520 	 * Retrieve the format information and if requested format isn't
521 	 * supported, keep the current format.
522 	 */
523 	fmtinfo = tegra_get_format_by_fourcc(chan->vi, pix->pixelformat);
524 	if (!fmtinfo) {
525 		pix->pixelformat = chan->format.pixelformat;
526 		pix->colorspace = chan->format.colorspace;
527 		fmtinfo = tegra_get_format_by_fourcc(chan->vi,
528 						     pix->pixelformat);
529 	}
530 
531 	pix->field = V4L2_FIELD_NONE;
532 	fmt.which = V4L2_SUBDEV_FORMAT_TRY;
533 	fmt.pad = 0;
534 	v4l2_fill_mbus_format(&fmt.format, pix, fmtinfo->code);
535 
536 	/*
537 	 * Attempt to obtain the format size from subdev.
538 	 * If not available, try to get crop boundary from subdev.
539 	 */
540 	fse.code = fmtinfo->code;
541 	ret = v4l2_subdev_call(subdev, pad, enum_frame_size, sd_state, &fse);
542 	if (ret) {
543 		if (!v4l2_subdev_has_op(subdev, pad, get_selection)) {
544 			sd_state->pads->try_crop.width = 0;
545 			sd_state->pads->try_crop.height = 0;
546 		} else {
547 			ret = v4l2_subdev_call(subdev, pad, get_selection,
548 					       NULL, &sdsel);
549 			if (ret)
550 				return -EINVAL;
551 
552 			sd_state->pads->try_crop.width = sdsel.r.width;
553 			sd_state->pads->try_crop.height = sdsel.r.height;
554 		}
555 	} else {
556 		sd_state->pads->try_crop.width = fse.max_width;
557 		sd_state->pads->try_crop.height = fse.max_height;
558 	}
559 
560 	ret = v4l2_subdev_call(subdev, pad, set_fmt, sd_state, &fmt);
561 	if (ret < 0)
562 		return ret;
563 
564 	v4l2_fill_pix_format(pix, &fmt.format);
565 	tegra_channel_fmt_align(chan, pix, fmtinfo->bpp);
566 
567 	__v4l2_subdev_state_free(sd_state);
568 
569 	return 0;
570 }
571 
tegra_channel_try_format(struct file * file,void * fh,struct v4l2_format * format)572 static int tegra_channel_try_format(struct file *file, void *fh,
573 				    struct v4l2_format *format)
574 {
575 	struct tegra_vi_channel *chan = video_drvdata(file);
576 
577 	return __tegra_channel_try_format(chan, &format->fmt.pix);
578 }
579 
tegra_channel_update_gangports(struct tegra_vi_channel * chan)580 static void tegra_channel_update_gangports(struct tegra_vi_channel *chan)
581 {
582 	if (chan->format.width <= 1920)
583 		chan->numgangports = 1;
584 	else
585 		chan->numgangports = chan->totalports;
586 }
587 
tegra_channel_set_format(struct file * file,void * fh,struct v4l2_format * format)588 static int tegra_channel_set_format(struct file *file, void *fh,
589 				    struct v4l2_format *format)
590 {
591 	struct tegra_vi_channel *chan = video_drvdata(file);
592 	const struct tegra_video_format *fmtinfo;
593 	struct v4l2_subdev_format fmt;
594 	struct v4l2_subdev *subdev;
595 	struct v4l2_pix_format *pix = &format->fmt.pix;
596 	int ret;
597 
598 	if (vb2_is_busy(&chan->queue))
599 		return -EBUSY;
600 
601 	/* get supported format by try_fmt */
602 	ret = __tegra_channel_try_format(chan, pix);
603 	if (ret)
604 		return ret;
605 
606 	fmtinfo = tegra_get_format_by_fourcc(chan->vi, pix->pixelformat);
607 
608 	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
609 	fmt.pad = 0;
610 	v4l2_fill_mbus_format(&fmt.format, pix, fmtinfo->code);
611 	subdev = tegra_channel_get_remote_source_subdev(chan);
612 	ret = v4l2_subdev_call(subdev, pad, set_fmt, NULL, &fmt);
613 	if (ret < 0)
614 		return ret;
615 
616 	v4l2_fill_pix_format(pix, &fmt.format);
617 	tegra_channel_fmt_align(chan, pix, fmtinfo->bpp);
618 
619 	chan->format = *pix;
620 	chan->fmtinfo = fmtinfo;
621 	tegra_channel_update_gangports(chan);
622 
623 	return 0;
624 }
625 
tegra_channel_set_subdev_active_fmt(struct tegra_vi_channel * chan)626 static int tegra_channel_set_subdev_active_fmt(struct tegra_vi_channel *chan)
627 {
628 	int ret, index;
629 	struct v4l2_subdev *subdev;
630 	struct v4l2_subdev_format fmt = {
631 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
632 	};
633 
634 	/*
635 	 * Initialize channel format to the sub-device active format if there
636 	 * is corresponding match in the Tegra supported video formats.
637 	 */
638 	subdev = tegra_channel_get_remote_source_subdev(chan);
639 	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
640 	if (ret)
641 		return ret;
642 
643 	index = tegra_get_format_idx_by_code(chan->vi, fmt.format.code, 0);
644 	if (index < 0)
645 		return -EINVAL;
646 
647 	chan->fmtinfo = &chan->vi->soc->video_formats[index];
648 	v4l2_fill_pix_format(&chan->format, &fmt.format);
649 	chan->format.pixelformat = chan->fmtinfo->fourcc;
650 	chan->format.bytesperline = chan->format.width * chan->fmtinfo->bpp;
651 	chan->format.sizeimage = chan->format.bytesperline *
652 				 chan->format.height;
653 	tegra_channel_fmt_align(chan, &chan->format, chan->fmtinfo->bpp);
654 	tegra_channel_update_gangports(chan);
655 
656 	return 0;
657 }
658 
659 static int
tegra_channel_subscribe_event(struct v4l2_fh * fh,const struct v4l2_event_subscription * sub)660 tegra_channel_subscribe_event(struct v4l2_fh *fh,
661 			      const struct v4l2_event_subscription *sub)
662 {
663 	switch (sub->type) {
664 	case V4L2_EVENT_SOURCE_CHANGE:
665 		return v4l2_event_subscribe(fh, sub, 4, NULL);
666 	}
667 
668 	return v4l2_ctrl_subscribe_event(fh, sub);
669 }
670 
tegra_channel_g_selection(struct file * file,void * priv,struct v4l2_selection * sel)671 static int tegra_channel_g_selection(struct file *file, void *priv,
672 				     struct v4l2_selection *sel)
673 {
674 	struct tegra_vi_channel *chan = video_drvdata(file);
675 	struct v4l2_subdev *subdev;
676 	struct v4l2_subdev_format fmt = {
677 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
678 	};
679 	struct v4l2_subdev_selection sdsel = {
680 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
681 		.target = sel->target,
682 	};
683 	int ret;
684 
685 	subdev = tegra_channel_get_remote_source_subdev(chan);
686 	if (!v4l2_subdev_has_op(subdev, pad, get_selection))
687 		return -ENOTTY;
688 
689 	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
690 		return -EINVAL;
691 	/*
692 	 * Try the get selection operation and fallback to get format if not
693 	 * implemented.
694 	 */
695 	ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sdsel);
696 	if (!ret)
697 		sel->r = sdsel.r;
698 	if (ret != -ENOIOCTLCMD)
699 		return ret;
700 
701 	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
702 	if (ret < 0)
703 		return ret;
704 
705 	sel->r.left = 0;
706 	sel->r.top = 0;
707 	sel->r.width = fmt.format.width;
708 	sel->r.height = fmt.format.height;
709 
710 	return 0;
711 }
712 
tegra_channel_s_selection(struct file * file,void * fh,struct v4l2_selection * sel)713 static int tegra_channel_s_selection(struct file *file, void *fh,
714 				     struct v4l2_selection *sel)
715 {
716 	struct tegra_vi_channel *chan = video_drvdata(file);
717 	struct v4l2_subdev *subdev;
718 	int ret;
719 	struct v4l2_subdev_selection sdsel = {
720 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
721 		.target = sel->target,
722 		.flags = sel->flags,
723 		.r = sel->r,
724 	};
725 
726 	subdev = tegra_channel_get_remote_source_subdev(chan);
727 	if (!v4l2_subdev_has_op(subdev, pad, set_selection))
728 		return -ENOTTY;
729 
730 	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
731 		return -EINVAL;
732 
733 	if (vb2_is_busy(&chan->queue))
734 		return -EBUSY;
735 
736 	ret = v4l2_subdev_call(subdev, pad, set_selection, NULL, &sdsel);
737 	if (!ret) {
738 		sel->r = sdsel.r;
739 		/*
740 		 * Subdev active format resolution may have changed during
741 		 * set selection operation. So, update channel format to
742 		 * the sub-device active format.
743 		 */
744 		return tegra_channel_set_subdev_active_fmt(chan);
745 	}
746 
747 	return ret;
748 }
749 
tegra_channel_g_edid(struct file * file,void * fh,struct v4l2_edid * edid)750 static int tegra_channel_g_edid(struct file *file, void *fh,
751 				struct v4l2_edid *edid)
752 {
753 	struct tegra_vi_channel *chan = video_drvdata(file);
754 	struct v4l2_subdev *subdev;
755 
756 	subdev = tegra_channel_get_remote_source_subdev(chan);
757 	if (!v4l2_subdev_has_op(subdev, pad, get_edid))
758 		return -ENOTTY;
759 
760 	return v4l2_subdev_call(subdev, pad, get_edid, edid);
761 }
762 
tegra_channel_s_edid(struct file * file,void * fh,struct v4l2_edid * edid)763 static int tegra_channel_s_edid(struct file *file, void *fh,
764 				struct v4l2_edid *edid)
765 {
766 	struct tegra_vi_channel *chan = video_drvdata(file);
767 	struct v4l2_subdev *subdev;
768 
769 	subdev = tegra_channel_get_remote_source_subdev(chan);
770 	if (!v4l2_subdev_has_op(subdev, pad, set_edid))
771 		return -ENOTTY;
772 
773 	return v4l2_subdev_call(subdev, pad, set_edid, edid);
774 }
775 
tegra_channel_g_dv_timings(struct file * file,void * fh,struct v4l2_dv_timings * timings)776 static int tegra_channel_g_dv_timings(struct file *file, void *fh,
777 				      struct v4l2_dv_timings *timings)
778 {
779 	struct tegra_vi_channel *chan = video_drvdata(file);
780 	struct v4l2_subdev *subdev;
781 
782 	subdev = tegra_channel_get_remote_source_subdev(chan);
783 	if (!v4l2_subdev_has_op(subdev, video, g_dv_timings))
784 		return -ENOTTY;
785 
786 	return v4l2_device_call_until_err(chan->video.v4l2_dev, 0,
787 					  video, g_dv_timings, timings);
788 }
789 
tegra_channel_s_dv_timings(struct file * file,void * fh,struct v4l2_dv_timings * timings)790 static int tegra_channel_s_dv_timings(struct file *file, void *fh,
791 				      struct v4l2_dv_timings *timings)
792 {
793 	struct tegra_vi_channel *chan = video_drvdata(file);
794 	struct v4l2_subdev *subdev;
795 	struct v4l2_bt_timings *bt = &timings->bt;
796 	struct v4l2_dv_timings curr_timings;
797 	int ret;
798 
799 	subdev = tegra_channel_get_remote_source_subdev(chan);
800 	if (!v4l2_subdev_has_op(subdev, video, s_dv_timings))
801 		return -ENOTTY;
802 
803 	ret = tegra_channel_g_dv_timings(file, fh, &curr_timings);
804 	if (ret)
805 		return ret;
806 
807 	if (v4l2_match_dv_timings(timings, &curr_timings, 0, false))
808 		return 0;
809 
810 	if (vb2_is_busy(&chan->queue))
811 		return -EBUSY;
812 
813 	ret = v4l2_device_call_until_err(chan->video.v4l2_dev, 0,
814 					 video, s_dv_timings, timings);
815 	if (ret)
816 		return ret;
817 
818 	chan->format.width = bt->width;
819 	chan->format.height = bt->height;
820 	chan->format.bytesperline = bt->width * chan->fmtinfo->bpp;
821 	chan->format.sizeimage = chan->format.bytesperline * bt->height;
822 	tegra_channel_fmt_align(chan, &chan->format, chan->fmtinfo->bpp);
823 	tegra_channel_update_gangports(chan);
824 
825 	return 0;
826 }
827 
tegra_channel_query_dv_timings(struct file * file,void * fh,struct v4l2_dv_timings * timings)828 static int tegra_channel_query_dv_timings(struct file *file, void *fh,
829 					  struct v4l2_dv_timings *timings)
830 {
831 	struct tegra_vi_channel *chan = video_drvdata(file);
832 	struct v4l2_subdev *subdev;
833 
834 	subdev = tegra_channel_get_remote_source_subdev(chan);
835 	if (!v4l2_subdev_has_op(subdev, video, query_dv_timings))
836 		return -ENOTTY;
837 
838 	return v4l2_device_call_until_err(chan->video.v4l2_dev, 0,
839 					  video, query_dv_timings, timings);
840 }
841 
tegra_channel_enum_dv_timings(struct file * file,void * fh,struct v4l2_enum_dv_timings * timings)842 static int tegra_channel_enum_dv_timings(struct file *file, void *fh,
843 					 struct v4l2_enum_dv_timings *timings)
844 {
845 	struct tegra_vi_channel *chan = video_drvdata(file);
846 	struct v4l2_subdev *subdev;
847 
848 	subdev = tegra_channel_get_remote_source_subdev(chan);
849 	if (!v4l2_subdev_has_op(subdev, pad, enum_dv_timings))
850 		return -ENOTTY;
851 
852 	return v4l2_subdev_call(subdev, pad, enum_dv_timings, timings);
853 }
854 
tegra_channel_dv_timings_cap(struct file * file,void * fh,struct v4l2_dv_timings_cap * cap)855 static int tegra_channel_dv_timings_cap(struct file *file, void *fh,
856 					struct v4l2_dv_timings_cap *cap)
857 {
858 	struct tegra_vi_channel *chan = video_drvdata(file);
859 	struct v4l2_subdev *subdev;
860 
861 	subdev = tegra_channel_get_remote_source_subdev(chan);
862 	if (!v4l2_subdev_has_op(subdev, pad, dv_timings_cap))
863 		return -ENOTTY;
864 
865 	return v4l2_subdev_call(subdev, pad, dv_timings_cap, cap);
866 }
867 
tegra_channel_log_status(struct file * file,void * fh)868 static int tegra_channel_log_status(struct file *file, void *fh)
869 {
870 	struct tegra_vi_channel *chan = video_drvdata(file);
871 
872 	v4l2_device_call_all(chan->video.v4l2_dev, 0, core, log_status);
873 
874 	return 0;
875 }
876 
tegra_channel_enum_input(struct file * file,void * fh,struct v4l2_input * inp)877 static int tegra_channel_enum_input(struct file *file, void *fh,
878 				    struct v4l2_input *inp)
879 {
880 	struct tegra_vi_channel *chan = video_drvdata(file);
881 	struct v4l2_subdev *subdev;
882 
883 	if (inp->index)
884 		return -EINVAL;
885 
886 	inp->type = V4L2_INPUT_TYPE_CAMERA;
887 	subdev = tegra_channel_get_remote_source_subdev(chan);
888 	strscpy(inp->name, subdev->name, sizeof(inp->name));
889 	if (v4l2_subdev_has_op(subdev, pad, dv_timings_cap))
890 		inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
891 
892 	return 0;
893 }
894 
tegra_channel_g_input(struct file * file,void * priv,unsigned int * i)895 static int tegra_channel_g_input(struct file *file, void *priv,
896 				 unsigned int *i)
897 {
898 	*i = 0;
899 
900 	return 0;
901 }
902 
tegra_channel_s_input(struct file * file,void * priv,unsigned int input)903 static int tegra_channel_s_input(struct file *file, void *priv,
904 				 unsigned int input)
905 {
906 	if (input > 0)
907 		return -EINVAL;
908 
909 	return 0;
910 }
911 
912 static const struct v4l2_ioctl_ops tegra_channel_ioctl_ops = {
913 	.vidioc_querycap		= tegra_channel_querycap,
914 	.vidioc_g_parm			= tegra_channel_g_parm,
915 	.vidioc_s_parm			= tegra_channel_s_parm,
916 	.vidioc_enum_framesizes		= tegra_channel_enum_framesizes,
917 	.vidioc_enum_frameintervals	= tegra_channel_enum_frameintervals,
918 	.vidioc_enum_fmt_vid_cap	= tegra_channel_enum_format,
919 	.vidioc_g_fmt_vid_cap		= tegra_channel_get_format,
920 	.vidioc_s_fmt_vid_cap		= tegra_channel_set_format,
921 	.vidioc_try_fmt_vid_cap		= tegra_channel_try_format,
922 	.vidioc_enum_input		= tegra_channel_enum_input,
923 	.vidioc_g_input			= tegra_channel_g_input,
924 	.vidioc_s_input			= tegra_channel_s_input,
925 	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
926 	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
927 	.vidioc_querybuf		= vb2_ioctl_querybuf,
928 	.vidioc_qbuf			= vb2_ioctl_qbuf,
929 	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
930 	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
931 	.vidioc_expbuf			= vb2_ioctl_expbuf,
932 	.vidioc_streamon		= vb2_ioctl_streamon,
933 	.vidioc_streamoff		= vb2_ioctl_streamoff,
934 	.vidioc_subscribe_event		= tegra_channel_subscribe_event,
935 	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
936 	.vidioc_g_selection		= tegra_channel_g_selection,
937 	.vidioc_s_selection		= tegra_channel_s_selection,
938 	.vidioc_g_edid			= tegra_channel_g_edid,
939 	.vidioc_s_edid			= tegra_channel_s_edid,
940 	.vidioc_g_dv_timings		= tegra_channel_g_dv_timings,
941 	.vidioc_s_dv_timings		= tegra_channel_s_dv_timings,
942 	.vidioc_query_dv_timings	= tegra_channel_query_dv_timings,
943 	.vidioc_enum_dv_timings		= tegra_channel_enum_dv_timings,
944 	.vidioc_dv_timings_cap		= tegra_channel_dv_timings_cap,
945 	.vidioc_log_status		= tegra_channel_log_status,
946 };
947 
948 /*
949  * V4L2 file operations
950  */
951 static const struct v4l2_file_operations tegra_channel_fops = {
952 	.owner		= THIS_MODULE,
953 	.unlocked_ioctl	= video_ioctl2,
954 	.open		= v4l2_fh_open,
955 	.release	= vb2_fop_release,
956 	.read		= vb2_fop_read,
957 	.poll		= vb2_fop_poll,
958 	.mmap		= vb2_fop_mmap,
959 };
960 
961 /*
962  * V4L2 control operations
963  */
vi_s_ctrl(struct v4l2_ctrl * ctrl)964 static int vi_s_ctrl(struct v4l2_ctrl *ctrl)
965 {
966 	struct tegra_vi_channel *chan = container_of(ctrl->handler,
967 						     struct tegra_vi_channel,
968 						     ctrl_handler);
969 
970 	switch (ctrl->id) {
971 	case V4L2_CID_TEST_PATTERN:
972 		/* pattern change takes effect on next stream */
973 		chan->pg_mode = ctrl->val + 1;
974 		break;
975 	case V4L2_CID_TEGRA_SYNCPT_TIMEOUT_RETRY:
976 		chan->syncpt_timeout_retry = ctrl->val;
977 		break;
978 	default:
979 		return -EINVAL;
980 	}
981 
982 	return 0;
983 }
984 
985 static const struct v4l2_ctrl_ops vi_ctrl_ops = {
986 	.s_ctrl	= vi_s_ctrl,
987 };
988 
989 #if IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)
990 static const char *const vi_pattern_strings[] = {
991 	"Black/White Direct Mode",
992 	"Color Patch Mode",
993 };
994 #else
995 static const struct v4l2_ctrl_config syncpt_timeout_ctrl = {
996 	.ops = &vi_ctrl_ops,
997 	.id = V4L2_CID_TEGRA_SYNCPT_TIMEOUT_RETRY,
998 	.name = "Syncpt timeout retry",
999 	.type = V4L2_CTRL_TYPE_INTEGER,
1000 	.min = 1,
1001 	.max = 10000,
1002 	.step = 1,
1003 	.def = 5,
1004 };
1005 #endif
1006 
tegra_channel_setup_ctrl_handler(struct tegra_vi_channel * chan)1007 static int tegra_channel_setup_ctrl_handler(struct tegra_vi_channel *chan)
1008 {
1009 	int ret;
1010 
1011 #if IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)
1012 	/* add test pattern control handler to v4l2 device */
1013 	v4l2_ctrl_new_std_menu_items(&chan->ctrl_handler, &vi_ctrl_ops,
1014 				     V4L2_CID_TEST_PATTERN,
1015 				     ARRAY_SIZE(vi_pattern_strings) - 1,
1016 				     0, 0, vi_pattern_strings);
1017 	if (chan->ctrl_handler.error) {
1018 		dev_err(chan->vi->dev, "failed to add TPG ctrl handler: %d\n",
1019 			chan->ctrl_handler.error);
1020 		v4l2_ctrl_handler_free(&chan->ctrl_handler);
1021 		return chan->ctrl_handler.error;
1022 	}
1023 #else
1024 	struct v4l2_subdev *subdev;
1025 
1026 	/* custom control */
1027 	v4l2_ctrl_new_custom(&chan->ctrl_handler, &syncpt_timeout_ctrl, NULL);
1028 	if (chan->ctrl_handler.error) {
1029 		dev_err(chan->vi->dev, "failed to add %s ctrl handler: %d\n",
1030 			syncpt_timeout_ctrl.name,
1031 			chan->ctrl_handler.error);
1032 		v4l2_ctrl_handler_free(&chan->ctrl_handler);
1033 		return chan->ctrl_handler.error;
1034 	}
1035 
1036 	subdev = tegra_channel_get_remote_source_subdev(chan);
1037 	if (!subdev)
1038 		return -ENODEV;
1039 
1040 	ret = v4l2_ctrl_add_handler(&chan->ctrl_handler, subdev->ctrl_handler,
1041 				    NULL, true);
1042 	if (ret < 0) {
1043 		dev_err(chan->vi->dev,
1044 			"failed to add subdev %s ctrl handler: %d\n",
1045 			subdev->name, ret);
1046 		v4l2_ctrl_handler_free(&chan->ctrl_handler);
1047 		return ret;
1048 	}
1049 #endif
1050 
1051 	/* setup the controls */
1052 	ret = v4l2_ctrl_handler_setup(&chan->ctrl_handler);
1053 	if (ret < 0) {
1054 		dev_err(chan->vi->dev,
1055 			"failed to setup v4l2 ctrl handler: %d\n", ret);
1056 		return ret;
1057 	}
1058 
1059 	return 0;
1060 }
1061 
1062 /* VI only support 2 formats in TPG mode */
vi_tpg_fmts_bitmap_init(struct tegra_vi_channel * chan)1063 static void vi_tpg_fmts_bitmap_init(struct tegra_vi_channel *chan)
1064 {
1065 	int index;
1066 
1067 	bitmap_zero(chan->tpg_fmts_bitmap, MAX_FORMAT_NUM);
1068 
1069 	index = tegra_get_format_idx_by_code(chan->vi,
1070 					     MEDIA_BUS_FMT_SRGGB10_1X10, 0);
1071 	bitmap_set(chan->tpg_fmts_bitmap, index, 1);
1072 
1073 	index = tegra_get_format_idx_by_code(chan->vi,
1074 					     MEDIA_BUS_FMT_RGB888_1X32_PADHI,
1075 					     0);
1076 	bitmap_set(chan->tpg_fmts_bitmap, index, 1);
1077 }
1078 
vi_fmts_bitmap_init(struct tegra_vi_channel * chan)1079 static int vi_fmts_bitmap_init(struct tegra_vi_channel *chan)
1080 {
1081 	int index, ret, match_code = 0;
1082 	struct v4l2_subdev *subdev;
1083 	struct v4l2_subdev_mbus_code_enum code = {
1084 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1085 	};
1086 
1087 	bitmap_zero(chan->fmts_bitmap, MAX_FORMAT_NUM);
1088 
1089 	/*
1090 	 * Set the bitmap bits based on all the matched formats between the
1091 	 * available media bus formats of sub-device and the pre-defined Tegra
1092 	 * supported video formats.
1093 	 */
1094 	subdev = tegra_channel_get_remote_source_subdev(chan);
1095 	while (1) {
1096 		ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
1097 				       NULL, &code);
1098 		if (ret < 0)
1099 			break;
1100 
1101 		index = tegra_get_format_idx_by_code(chan->vi, code.code, 0);
1102 		while (index >= 0) {
1103 			bitmap_set(chan->fmts_bitmap, index, 1);
1104 			if (!match_code)
1105 				match_code = code.code;
1106 			/* look for other formats with same mbus code */
1107 			index = tegra_get_format_idx_by_code(chan->vi,
1108 							     code.code,
1109 							     index + 1);
1110 		}
1111 
1112 		code.index++;
1113 	}
1114 
1115 	/*
1116 	 * Set the bitmap bit corresponding to default tegra video format if
1117 	 * there are no matched formats.
1118 	 */
1119 	if (!match_code) {
1120 		match_code = tegra_default_format.code;
1121 		index = tegra_get_format_idx_by_code(chan->vi, match_code, 0);
1122 		if (WARN_ON(index < 0))
1123 			return -EINVAL;
1124 
1125 		bitmap_set(chan->fmts_bitmap, index, 1);
1126 	}
1127 
1128 	/* initialize channel format to the sub-device active format */
1129 	tegra_channel_set_subdev_active_fmt(chan);
1130 
1131 	return 0;
1132 }
1133 
tegra_channel_host1x_syncpts_free(struct tegra_vi_channel * chan)1134 static void tegra_channel_host1x_syncpts_free(struct tegra_vi_channel *chan)
1135 {
1136 	int i;
1137 
1138 	for (i = 0; i < chan->numgangports; i++) {
1139 		host1x_syncpt_put(chan->mw_ack_sp[i]);
1140 		host1x_syncpt_put(chan->frame_start_sp[i]);
1141 	}
1142 }
1143 
tegra_channel_cleanup(struct tegra_vi_channel * chan)1144 static void tegra_channel_cleanup(struct tegra_vi_channel *chan)
1145 {
1146 	v4l2_ctrl_handler_free(&chan->ctrl_handler);
1147 	media_entity_cleanup(&chan->video.entity);
1148 	tegra_channel_host1x_syncpts_free(chan);
1149 	mutex_destroy(&chan->video_lock);
1150 }
1151 
tegra_channels_cleanup(struct tegra_vi * vi)1152 void tegra_channels_cleanup(struct tegra_vi *vi)
1153 {
1154 	struct tegra_vi_channel *chan, *tmp;
1155 
1156 	if (!vi)
1157 		return;
1158 
1159 	list_for_each_entry_safe(chan, tmp, &vi->vi_chans, list) {
1160 		tegra_channel_cleanup(chan);
1161 		list_del(&chan->list);
1162 		kfree(chan);
1163 	}
1164 }
1165 
tegra_channel_host1x_syncpt_init(struct tegra_vi_channel * chan)1166 static int tegra_channel_host1x_syncpt_init(struct tegra_vi_channel *chan)
1167 {
1168 	struct tegra_vi *vi = chan->vi;
1169 	unsigned long flags = HOST1X_SYNCPT_CLIENT_MANAGED;
1170 	struct host1x_syncpt *fs_sp;
1171 	struct host1x_syncpt *mw_sp;
1172 	int ret, i;
1173 
1174 	for (i = 0; i < chan->numgangports; i++) {
1175 		fs_sp = host1x_syncpt_request(&vi->client, flags);
1176 		if (!fs_sp) {
1177 			dev_err(vi->dev, "failed to request frame start syncpoint\n");
1178 			ret = -ENOMEM;
1179 			goto free_syncpts;
1180 		}
1181 
1182 		mw_sp = host1x_syncpt_request(&vi->client, flags);
1183 		if (!mw_sp) {
1184 			dev_err(vi->dev, "failed to request memory ack syncpoint\n");
1185 			host1x_syncpt_put(fs_sp);
1186 			ret = -ENOMEM;
1187 			goto free_syncpts;
1188 		}
1189 
1190 		chan->frame_start_sp[i] = fs_sp;
1191 		chan->mw_ack_sp[i] = mw_sp;
1192 		spin_lock_init(&chan->sp_incr_lock[i]);
1193 	}
1194 
1195 	return 0;
1196 
1197 free_syncpts:
1198 	tegra_channel_host1x_syncpts_free(chan);
1199 	return ret;
1200 }
1201 
tegra_channel_init(struct tegra_vi_channel * chan)1202 static int tegra_channel_init(struct tegra_vi_channel *chan)
1203 {
1204 	struct tegra_vi *vi = chan->vi;
1205 	struct tegra_video_device *vid = dev_get_drvdata(vi->client.host);
1206 	int ret;
1207 
1208 	mutex_init(&chan->video_lock);
1209 	INIT_LIST_HEAD(&chan->capture);
1210 	INIT_LIST_HEAD(&chan->done);
1211 	spin_lock_init(&chan->start_lock);
1212 	spin_lock_init(&chan->done_lock);
1213 	init_waitqueue_head(&chan->start_wait);
1214 	init_waitqueue_head(&chan->done_wait);
1215 
1216 	/* initialize the video format */
1217 	chan->fmtinfo = &tegra_default_format;
1218 	chan->format.pixelformat = chan->fmtinfo->fourcc;
1219 	chan->format.colorspace = V4L2_COLORSPACE_SRGB;
1220 	chan->format.field = V4L2_FIELD_NONE;
1221 	chan->format.width = TEGRA_DEF_WIDTH;
1222 	chan->format.height = TEGRA_DEF_HEIGHT;
1223 	chan->format.bytesperline = TEGRA_DEF_WIDTH * chan->fmtinfo->bpp;
1224 	chan->format.sizeimage = chan->format.bytesperline * TEGRA_DEF_HEIGHT;
1225 	tegra_channel_fmt_align(chan, &chan->format, chan->fmtinfo->bpp);
1226 
1227 	ret = tegra_channel_host1x_syncpt_init(chan);
1228 	if (ret)
1229 		return ret;
1230 
1231 	/* initialize the media entity */
1232 	chan->pad.flags = MEDIA_PAD_FL_SINK;
1233 	ret = media_entity_pads_init(&chan->video.entity, 1, &chan->pad);
1234 	if (ret < 0) {
1235 		dev_err(vi->dev,
1236 			"failed to initialize media entity: %d\n", ret);
1237 		goto free_syncpts;
1238 	}
1239 
1240 	ret = v4l2_ctrl_handler_init(&chan->ctrl_handler, MAX_CID_CONTROLS);
1241 	if (chan->ctrl_handler.error) {
1242 		dev_err(vi->dev,
1243 			"failed to initialize v4l2 ctrl handler: %d\n", ret);
1244 		goto cleanup_media;
1245 	}
1246 
1247 	/* initialize the video_device */
1248 	chan->video.fops = &tegra_channel_fops;
1249 	chan->video.v4l2_dev = &vid->v4l2_dev;
1250 	chan->video.release = video_device_release_empty;
1251 	chan->video.queue = &chan->queue;
1252 	snprintf(chan->video.name, sizeof(chan->video.name), "%s-%s-%u",
1253 		 dev_name(vi->dev), "output", chan->portnos[0]);
1254 	chan->video.vfl_type = VFL_TYPE_VIDEO;
1255 	chan->video.vfl_dir = VFL_DIR_RX;
1256 	chan->video.ioctl_ops = &tegra_channel_ioctl_ops;
1257 	chan->video.ctrl_handler = &chan->ctrl_handler;
1258 	chan->video.lock = &chan->video_lock;
1259 	chan->video.device_caps = V4L2_CAP_VIDEO_CAPTURE |
1260 				  V4L2_CAP_STREAMING |
1261 				  V4L2_CAP_READWRITE;
1262 	video_set_drvdata(&chan->video, chan);
1263 
1264 	chan->queue.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1265 	chan->queue.io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ;
1266 	chan->queue.lock = &chan->video_lock;
1267 	chan->queue.drv_priv = chan;
1268 	chan->queue.buf_struct_size = sizeof(struct tegra_channel_buffer);
1269 	chan->queue.ops = &tegra_channel_queue_qops;
1270 	chan->queue.mem_ops = &vb2_dma_contig_memops;
1271 	chan->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1272 	chan->queue.min_buffers_needed = 2;
1273 	chan->queue.dev = vi->dev;
1274 	ret = vb2_queue_init(&chan->queue);
1275 	if (ret < 0) {
1276 		dev_err(vi->dev, "failed to initialize vb2 queue: %d\n", ret);
1277 		goto free_v4l2_ctrl_hdl;
1278 	}
1279 
1280 	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
1281 		v4l2_async_nf_init(&chan->notifier);
1282 
1283 	return 0;
1284 
1285 free_v4l2_ctrl_hdl:
1286 	v4l2_ctrl_handler_free(&chan->ctrl_handler);
1287 cleanup_media:
1288 	media_entity_cleanup(&chan->video.entity);
1289 free_syncpts:
1290 	tegra_channel_host1x_syncpts_free(chan);
1291 	return ret;
1292 }
1293 
tegra_vi_channel_alloc(struct tegra_vi * vi,unsigned int port_num,struct device_node * node,unsigned int lanes)1294 static int tegra_vi_channel_alloc(struct tegra_vi *vi, unsigned int port_num,
1295 				  struct device_node *node, unsigned int lanes)
1296 {
1297 	struct tegra_vi_channel *chan;
1298 	unsigned int i;
1299 
1300 	/*
1301 	 * Do not use devm_kzalloc as memory is freed immediately
1302 	 * when device instance is unbound but application might still
1303 	 * be holding the device node open. Channel memory allocated
1304 	 * with kzalloc is freed during video device release callback.
1305 	 */
1306 	chan = kzalloc(sizeof(*chan), GFP_KERNEL);
1307 	if (!chan)
1308 		return -ENOMEM;
1309 
1310 	chan->vi = vi;
1311 	chan->portnos[0] = port_num;
1312 	/*
1313 	 * For data lanes more than maximum csi lanes per brick, multiple of
1314 	 * x4 ports are used simultaneously for capture.
1315 	 */
1316 	if (lanes <= CSI_LANES_PER_BRICK)
1317 		chan->totalports = 1;
1318 	else
1319 		chan->totalports = lanes / CSI_LANES_PER_BRICK;
1320 	chan->numgangports = chan->totalports;
1321 
1322 	for (i = 1; i < chan->totalports; i++)
1323 		chan->portnos[i] = chan->portnos[0] + i * CSI_PORTS_PER_BRICK;
1324 
1325 	chan->of_node = node;
1326 	list_add_tail(&chan->list, &vi->vi_chans);
1327 
1328 	return 0;
1329 }
1330 
tegra_vi_tpg_channels_alloc(struct tegra_vi * vi)1331 static int tegra_vi_tpg_channels_alloc(struct tegra_vi *vi)
1332 {
1333 	unsigned int port_num;
1334 	unsigned int nchannels = vi->soc->vi_max_channels;
1335 	int ret;
1336 
1337 	for (port_num = 0; port_num < nchannels; port_num++) {
1338 		ret = tegra_vi_channel_alloc(vi, port_num,
1339 					     vi->dev->of_node, 2);
1340 		if (ret < 0)
1341 			return ret;
1342 	}
1343 
1344 	return 0;
1345 }
1346 
tegra_vi_channels_alloc(struct tegra_vi * vi)1347 static int tegra_vi_channels_alloc(struct tegra_vi *vi)
1348 {
1349 	struct device_node *node = vi->dev->of_node;
1350 	struct device_node *ep = NULL;
1351 	struct device_node *ports;
1352 	struct device_node *port;
1353 	unsigned int port_num;
1354 	struct device_node *parent;
1355 	struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = 0 };
1356 	unsigned int lanes;
1357 	int ret = 0;
1358 
1359 	ports = of_get_child_by_name(node, "ports");
1360 	if (!ports)
1361 		return -ENODEV;
1362 
1363 	for_each_child_of_node(ports, port) {
1364 		if (!of_node_name_eq(port, "port"))
1365 			continue;
1366 
1367 		ret = of_property_read_u32(port, "reg", &port_num);
1368 		if (ret < 0)
1369 			continue;
1370 
1371 		if (port_num > vi->soc->vi_max_channels) {
1372 			dev_err(vi->dev, "invalid port num %d for %pOF\n",
1373 				port_num, port);
1374 			ret = -EINVAL;
1375 			of_node_put(port);
1376 			goto cleanup;
1377 		}
1378 
1379 		ep = of_get_child_by_name(port, "endpoint");
1380 		if (!ep)
1381 			continue;
1382 
1383 		parent = of_graph_get_remote_port_parent(ep);
1384 		of_node_put(ep);
1385 		if (!parent)
1386 			continue;
1387 
1388 		ep = of_graph_get_endpoint_by_regs(parent, 0, 0);
1389 		of_node_put(parent);
1390 		ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep),
1391 						 &v4l2_ep);
1392 		of_node_put(ep);
1393 		if (ret)
1394 			continue;
1395 
1396 		lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
1397 		ret = tegra_vi_channel_alloc(vi, port_num, port, lanes);
1398 		if (ret < 0) {
1399 			of_node_put(port);
1400 			goto cleanup;
1401 		}
1402 	}
1403 
1404 cleanup:
1405 	of_node_put(ports);
1406 	return ret;
1407 }
1408 
tegra_vi_channels_init(struct tegra_vi * vi)1409 static int tegra_vi_channels_init(struct tegra_vi *vi)
1410 {
1411 	struct tegra_vi_channel *chan;
1412 	int ret;
1413 
1414 	list_for_each_entry(chan, &vi->vi_chans, list) {
1415 		ret = tegra_channel_init(chan);
1416 		if (ret < 0) {
1417 			dev_err(vi->dev,
1418 				"failed to initialize channel-%d: %d\n",
1419 				chan->portnos[0], ret);
1420 			goto cleanup;
1421 		}
1422 	}
1423 
1424 	return 0;
1425 
1426 cleanup:
1427 	list_for_each_entry_continue_reverse(chan, &vi->vi_chans, list)
1428 		tegra_channel_cleanup(chan);
1429 
1430 	return ret;
1431 }
1432 
tegra_v4l2_nodes_cleanup_tpg(struct tegra_video_device * vid)1433 void tegra_v4l2_nodes_cleanup_tpg(struct tegra_video_device *vid)
1434 {
1435 	struct tegra_vi *vi = vid->vi;
1436 	struct tegra_csi *csi = vid->csi;
1437 	struct tegra_csi_channel *csi_chan;
1438 	struct tegra_vi_channel *chan;
1439 
1440 	list_for_each_entry(chan, &vi->vi_chans, list)
1441 		vb2_video_unregister_device(&chan->video);
1442 
1443 	list_for_each_entry(csi_chan, &csi->csi_chans, list)
1444 		v4l2_device_unregister_subdev(&csi_chan->subdev);
1445 }
1446 
tegra_v4l2_nodes_setup_tpg(struct tegra_video_device * vid)1447 int tegra_v4l2_nodes_setup_tpg(struct tegra_video_device *vid)
1448 {
1449 	struct tegra_vi *vi = vid->vi;
1450 	struct tegra_csi *csi = vid->csi;
1451 	struct tegra_vi_channel *vi_chan;
1452 	struct tegra_csi_channel *csi_chan;
1453 	u32 link_flags = MEDIA_LNK_FL_ENABLED;
1454 	int ret;
1455 
1456 	if (!vi || !csi)
1457 		return -ENODEV;
1458 
1459 	csi_chan = list_first_entry(&csi->csi_chans,
1460 				    struct tegra_csi_channel, list);
1461 
1462 	list_for_each_entry(vi_chan, &vi->vi_chans, list) {
1463 		struct media_entity *source = &csi_chan->subdev.entity;
1464 		struct media_entity *sink = &vi_chan->video.entity;
1465 		struct media_pad *source_pad = csi_chan->pads;
1466 		struct media_pad *sink_pad = &vi_chan->pad;
1467 
1468 		ret = v4l2_device_register_subdev(&vid->v4l2_dev,
1469 						  &csi_chan->subdev);
1470 		if (ret) {
1471 			dev_err(vi->dev,
1472 				"failed to register subdev: %d\n", ret);
1473 			goto cleanup;
1474 		}
1475 
1476 		ret = video_register_device(&vi_chan->video,
1477 					    VFL_TYPE_VIDEO, -1);
1478 		if (ret < 0) {
1479 			dev_err(vi->dev,
1480 				"failed to register video device: %d\n", ret);
1481 			goto cleanup;
1482 		}
1483 
1484 		dev_dbg(vi->dev, "creating %s:%u -> %s:%u link\n",
1485 			source->name, source_pad->index,
1486 			sink->name, sink_pad->index);
1487 
1488 		ret = media_create_pad_link(source, source_pad->index,
1489 					    sink, sink_pad->index,
1490 					    link_flags);
1491 		if (ret < 0) {
1492 			dev_err(vi->dev,
1493 				"failed to create %s:%u -> %s:%u link: %d\n",
1494 				source->name, source_pad->index,
1495 				sink->name, sink_pad->index, ret);
1496 			goto cleanup;
1497 		}
1498 
1499 		ret = tegra_channel_setup_ctrl_handler(vi_chan);
1500 		if (ret < 0)
1501 			goto cleanup;
1502 
1503 		v4l2_set_subdev_hostdata(&csi_chan->subdev, vi_chan);
1504 		vi_tpg_fmts_bitmap_init(vi_chan);
1505 		csi_chan = list_next_entry(csi_chan, list);
1506 	}
1507 
1508 	return 0;
1509 
1510 cleanup:
1511 	tegra_v4l2_nodes_cleanup_tpg(vid);
1512 	return ret;
1513 }
1514 
vi_runtime_resume(struct device * dev)1515 static int __maybe_unused vi_runtime_resume(struct device *dev)
1516 {
1517 	struct tegra_vi *vi = dev_get_drvdata(dev);
1518 	int ret;
1519 
1520 	ret = regulator_enable(vi->vdd);
1521 	if (ret) {
1522 		dev_err(dev, "failed to enable VDD supply: %d\n", ret);
1523 		return ret;
1524 	}
1525 
1526 	ret = clk_set_rate(vi->clk, vi->soc->vi_max_clk_hz);
1527 	if (ret) {
1528 		dev_err(dev, "failed to set vi clock rate: %d\n", ret);
1529 		goto disable_vdd;
1530 	}
1531 
1532 	ret = clk_prepare_enable(vi->clk);
1533 	if (ret) {
1534 		dev_err(dev, "failed to enable vi clock: %d\n", ret);
1535 		goto disable_vdd;
1536 	}
1537 
1538 	return 0;
1539 
1540 disable_vdd:
1541 	regulator_disable(vi->vdd);
1542 	return ret;
1543 }
1544 
vi_runtime_suspend(struct device * dev)1545 static int __maybe_unused vi_runtime_suspend(struct device *dev)
1546 {
1547 	struct tegra_vi *vi = dev_get_drvdata(dev);
1548 
1549 	clk_disable_unprepare(vi->clk);
1550 
1551 	regulator_disable(vi->vdd);
1552 
1553 	return 0;
1554 }
1555 
1556 /*
1557  * Graph Management
1558  */
1559 static struct tegra_vi_graph_entity *
tegra_vi_graph_find_entity(struct tegra_vi_channel * chan,const struct fwnode_handle * fwnode)1560 tegra_vi_graph_find_entity(struct tegra_vi_channel *chan,
1561 			   const struct fwnode_handle *fwnode)
1562 {
1563 	struct tegra_vi_graph_entity *entity;
1564 	struct v4l2_async_subdev *asd;
1565 
1566 	list_for_each_entry(asd, &chan->notifier.asd_list, asd_list) {
1567 		entity = to_tegra_vi_graph_entity(asd);
1568 		if (entity->asd.match.fwnode == fwnode)
1569 			return entity;
1570 	}
1571 
1572 	return NULL;
1573 }
1574 
tegra_vi_graph_build(struct tegra_vi_channel * chan,struct tegra_vi_graph_entity * entity)1575 static int tegra_vi_graph_build(struct tegra_vi_channel *chan,
1576 				struct tegra_vi_graph_entity *entity)
1577 {
1578 	struct tegra_vi *vi = chan->vi;
1579 	struct tegra_vi_graph_entity *ent;
1580 	struct fwnode_handle *ep = NULL;
1581 	struct v4l2_fwnode_link link;
1582 	struct media_entity *local = entity->entity;
1583 	struct media_entity *remote;
1584 	struct media_pad *local_pad;
1585 	struct media_pad *remote_pad;
1586 	u32 link_flags = MEDIA_LNK_FL_ENABLED;
1587 	int ret = 0;
1588 
1589 	dev_dbg(vi->dev, "creating links for entity %s\n", local->name);
1590 
1591 	while (1) {
1592 		ep = fwnode_graph_get_next_endpoint(entity->asd.match.fwnode,
1593 						    ep);
1594 		if (!ep)
1595 			break;
1596 
1597 		ret = v4l2_fwnode_parse_link(ep, &link);
1598 		if (ret < 0) {
1599 			dev_err(vi->dev, "failed to parse link for %pOF: %d\n",
1600 				to_of_node(ep), ret);
1601 			continue;
1602 		}
1603 
1604 		if (link.local_port >= local->num_pads) {
1605 			dev_err(vi->dev, "invalid port number %u on %pOF\n",
1606 				link.local_port, to_of_node(link.local_node));
1607 			v4l2_fwnode_put_link(&link);
1608 			ret = -EINVAL;
1609 			break;
1610 		}
1611 
1612 		local_pad = &local->pads[link.local_port];
1613 		/* Remote node is vi node. So use channel video entity and pad
1614 		 * as remote/sink.
1615 		 */
1616 		if (link.remote_node == of_fwnode_handle(vi->dev->of_node)) {
1617 			remote = &chan->video.entity;
1618 			remote_pad = &chan->pad;
1619 			goto create_link;
1620 		}
1621 
1622 		/*
1623 		 * Skip sink ports, they will be processed from the other end
1624 		 * of the link.
1625 		 */
1626 		if (local_pad->flags & MEDIA_PAD_FL_SINK) {
1627 			dev_dbg(vi->dev, "skipping sink port %pOF:%u\n",
1628 				to_of_node(link.local_node), link.local_port);
1629 			v4l2_fwnode_put_link(&link);
1630 			continue;
1631 		}
1632 
1633 		/* find the remote entity from notifier list */
1634 		ent = tegra_vi_graph_find_entity(chan, link.remote_node);
1635 		if (!ent) {
1636 			dev_err(vi->dev, "no entity found for %pOF\n",
1637 				to_of_node(link.remote_node));
1638 			v4l2_fwnode_put_link(&link);
1639 			ret = -ENODEV;
1640 			break;
1641 		}
1642 
1643 		remote = ent->entity;
1644 		if (link.remote_port >= remote->num_pads) {
1645 			dev_err(vi->dev, "invalid port number %u on %pOF\n",
1646 				link.remote_port,
1647 				to_of_node(link.remote_node));
1648 			v4l2_fwnode_put_link(&link);
1649 			ret = -EINVAL;
1650 			break;
1651 		}
1652 
1653 		remote_pad = &remote->pads[link.remote_port];
1654 
1655 create_link:
1656 		dev_dbg(vi->dev, "creating %s:%u -> %s:%u link\n",
1657 			local->name, local_pad->index,
1658 			remote->name, remote_pad->index);
1659 
1660 		ret = media_create_pad_link(local, local_pad->index,
1661 					    remote, remote_pad->index,
1662 					    link_flags);
1663 		v4l2_fwnode_put_link(&link);
1664 		if (ret < 0) {
1665 			dev_err(vi->dev,
1666 				"failed to create %s:%u -> %s:%u link: %d\n",
1667 				local->name, local_pad->index,
1668 				remote->name, remote_pad->index, ret);
1669 			break;
1670 		}
1671 	}
1672 
1673 	fwnode_handle_put(ep);
1674 	return ret;
1675 }
1676 
tegra_vi_graph_notify_complete(struct v4l2_async_notifier * notifier)1677 static int tegra_vi_graph_notify_complete(struct v4l2_async_notifier *notifier)
1678 {
1679 	struct tegra_vi_graph_entity *entity;
1680 	struct v4l2_async_subdev *asd;
1681 	struct v4l2_subdev *subdev;
1682 	struct tegra_vi_channel *chan;
1683 	struct tegra_vi *vi;
1684 	int ret;
1685 
1686 	chan = container_of(notifier, struct tegra_vi_channel, notifier);
1687 	vi = chan->vi;
1688 
1689 	dev_dbg(vi->dev, "notify complete, all subdevs registered\n");
1690 
1691 	/*
1692 	 * Video device node should be created at the end of all the device
1693 	 * related initialization/setup.
1694 	 * Current video_register_device() does both initialize and register
1695 	 * video device in same API.
1696 	 *
1697 	 * TODO: Update v4l2-dev driver to split initialize and register into
1698 	 * separate APIs and then update Tegra video driver to do video device
1699 	 * initialize followed by all video device related setup and then
1700 	 * register the video device.
1701 	 */
1702 	ret = video_register_device(&chan->video, VFL_TYPE_VIDEO, -1);
1703 	if (ret < 0) {
1704 		dev_err(vi->dev,
1705 			"failed to register video device: %d\n", ret);
1706 		goto unregister_video;
1707 	}
1708 
1709 	/* create links between the entities */
1710 	list_for_each_entry(asd, &chan->notifier.asd_list, asd_list) {
1711 		entity = to_tegra_vi_graph_entity(asd);
1712 		ret = tegra_vi_graph_build(chan, entity);
1713 		if (ret < 0)
1714 			goto unregister_video;
1715 	}
1716 
1717 	ret = tegra_channel_setup_ctrl_handler(chan);
1718 	if (ret < 0) {
1719 		dev_err(vi->dev,
1720 			"failed to setup channel controls: %d\n", ret);
1721 		goto unregister_video;
1722 	}
1723 
1724 	ret = vi_fmts_bitmap_init(chan);
1725 	if (ret < 0) {
1726 		dev_err(vi->dev,
1727 			"failed to initialize formats bitmap: %d\n", ret);
1728 		goto unregister_video;
1729 	}
1730 
1731 	subdev = tegra_channel_get_remote_csi_subdev(chan);
1732 	if (!subdev) {
1733 		ret = -ENODEV;
1734 		dev_err(vi->dev,
1735 			"failed to get remote csi subdev: %d\n", ret);
1736 		goto unregister_video;
1737 	}
1738 
1739 	v4l2_set_subdev_hostdata(subdev, chan);
1740 
1741 	subdev = tegra_channel_get_remote_source_subdev(chan);
1742 	v4l2_set_subdev_hostdata(subdev, chan);
1743 
1744 	return 0;
1745 
1746 unregister_video:
1747 	vb2_video_unregister_device(&chan->video);
1748 	return ret;
1749 }
1750 
tegra_vi_graph_notify_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * subdev,struct v4l2_async_subdev * asd)1751 static int tegra_vi_graph_notify_bound(struct v4l2_async_notifier *notifier,
1752 				       struct v4l2_subdev *subdev,
1753 				       struct v4l2_async_subdev *asd)
1754 {
1755 	struct tegra_vi_graph_entity *entity;
1756 	struct tegra_vi *vi;
1757 	struct tegra_vi_channel *chan;
1758 
1759 	chan = container_of(notifier, struct tegra_vi_channel, notifier);
1760 	vi = chan->vi;
1761 
1762 	/*
1763 	 * Locate the entity corresponding to the bound subdev and store the
1764 	 * subdev pointer.
1765 	 */
1766 	entity = tegra_vi_graph_find_entity(chan, subdev->fwnode);
1767 	if (!entity) {
1768 		dev_err(vi->dev, "no entity for subdev %s\n", subdev->name);
1769 		return -EINVAL;
1770 	}
1771 
1772 	if (entity->subdev) {
1773 		dev_err(vi->dev, "duplicate subdev for node %pOF\n",
1774 			to_of_node(entity->asd.match.fwnode));
1775 		return -EINVAL;
1776 	}
1777 
1778 	dev_dbg(vi->dev, "subdev %s bound\n", subdev->name);
1779 	entity->entity = &subdev->entity;
1780 	entity->subdev = subdev;
1781 
1782 	return 0;
1783 }
1784 
1785 static const struct v4l2_async_notifier_operations tegra_vi_async_ops = {
1786 	.bound = tegra_vi_graph_notify_bound,
1787 	.complete = tegra_vi_graph_notify_complete,
1788 };
1789 
tegra_vi_graph_parse_one(struct tegra_vi_channel * chan,struct fwnode_handle * fwnode)1790 static int tegra_vi_graph_parse_one(struct tegra_vi_channel *chan,
1791 				    struct fwnode_handle *fwnode)
1792 {
1793 	struct tegra_vi *vi = chan->vi;
1794 	struct fwnode_handle *ep = NULL;
1795 	struct fwnode_handle *remote = NULL;
1796 	struct tegra_vi_graph_entity *tvge;
1797 	struct device_node *node = NULL;
1798 	int ret;
1799 
1800 	dev_dbg(vi->dev, "parsing node %pOF\n", to_of_node(fwnode));
1801 
1802 	/* parse all the remote entities and put them into the list */
1803 	for_each_endpoint_of_node(to_of_node(fwnode), node) {
1804 		ep = of_fwnode_handle(node);
1805 		remote = fwnode_graph_get_remote_port_parent(ep);
1806 		if (!remote) {
1807 			dev_err(vi->dev,
1808 				"remote device at %pOF not found\n", node);
1809 			ret = -EINVAL;
1810 			goto cleanup;
1811 		}
1812 
1813 		/* skip entities that are already processed */
1814 		if (remote == dev_fwnode(vi->dev) ||
1815 		    tegra_vi_graph_find_entity(chan, remote)) {
1816 			fwnode_handle_put(remote);
1817 			continue;
1818 		}
1819 
1820 		tvge = v4l2_async_nf_add_fwnode(&chan->notifier, remote,
1821 						struct tegra_vi_graph_entity);
1822 		if (IS_ERR(tvge)) {
1823 			ret = PTR_ERR(tvge);
1824 			dev_err(vi->dev,
1825 				"failed to add subdev to notifier: %d\n", ret);
1826 			fwnode_handle_put(remote);
1827 			goto cleanup;
1828 		}
1829 
1830 		ret = tegra_vi_graph_parse_one(chan, remote);
1831 		if (ret < 0) {
1832 			fwnode_handle_put(remote);
1833 			goto cleanup;
1834 		}
1835 
1836 		fwnode_handle_put(remote);
1837 	}
1838 
1839 	return 0;
1840 
1841 cleanup:
1842 	dev_err(vi->dev, "failed parsing the graph: %d\n", ret);
1843 	v4l2_async_nf_cleanup(&chan->notifier);
1844 	of_node_put(node);
1845 	return ret;
1846 }
1847 
tegra_vi_graph_init(struct tegra_vi * vi)1848 static int tegra_vi_graph_init(struct tegra_vi *vi)
1849 {
1850 	struct tegra_video_device *vid = dev_get_drvdata(vi->client.host);
1851 	struct tegra_vi_channel *chan;
1852 	struct fwnode_handle *fwnode = dev_fwnode(vi->dev);
1853 	int ret;
1854 
1855 	/*
1856 	 * Walk the links to parse the full graph. Each channel will have
1857 	 * one endpoint of the composite node. Start by parsing the
1858 	 * composite node and parse the remote entities in turn.
1859 	 * Each channel will register v4l2 async notifier to make the graph
1860 	 * independent between the channels so we can the current channel
1861 	 * in case of something wrong during graph parsing and continue with
1862 	 * next channels.
1863 	 */
1864 	list_for_each_entry(chan, &vi->vi_chans, list) {
1865 		struct fwnode_handle *ep, *remote;
1866 
1867 		ep = fwnode_graph_get_endpoint_by_id(fwnode,
1868 						     chan->portnos[0], 0, 0);
1869 		if (!ep)
1870 			continue;
1871 
1872 		remote = fwnode_graph_get_remote_port_parent(ep);
1873 		fwnode_handle_put(ep);
1874 
1875 		ret = tegra_vi_graph_parse_one(chan, remote);
1876 		fwnode_handle_put(remote);
1877 		if (ret < 0 || list_empty(&chan->notifier.asd_list))
1878 			continue;
1879 
1880 		chan->notifier.ops = &tegra_vi_async_ops;
1881 		ret = v4l2_async_nf_register(&vid->v4l2_dev, &chan->notifier);
1882 		if (ret < 0) {
1883 			dev_err(vi->dev,
1884 				"failed to register channel %d notifier: %d\n",
1885 				chan->portnos[0], ret);
1886 			v4l2_async_nf_cleanup(&chan->notifier);
1887 		}
1888 	}
1889 
1890 	return 0;
1891 }
1892 
tegra_vi_graph_cleanup(struct tegra_vi * vi)1893 static void tegra_vi_graph_cleanup(struct tegra_vi *vi)
1894 {
1895 	struct tegra_vi_channel *chan;
1896 
1897 	list_for_each_entry(chan, &vi->vi_chans, list) {
1898 		vb2_video_unregister_device(&chan->video);
1899 		v4l2_async_nf_unregister(&chan->notifier);
1900 		v4l2_async_nf_cleanup(&chan->notifier);
1901 	}
1902 }
1903 
tegra_vi_init(struct host1x_client * client)1904 static int tegra_vi_init(struct host1x_client *client)
1905 {
1906 	struct tegra_video_device *vid = dev_get_drvdata(client->host);
1907 	struct tegra_vi *vi = host1x_client_to_vi(client);
1908 	struct tegra_vi_channel *chan, *tmp;
1909 	int ret;
1910 
1911 	vid->media_dev.hw_revision = vi->soc->hw_revision;
1912 	snprintf(vid->media_dev.bus_info, sizeof(vid->media_dev.bus_info),
1913 		 "platform:%s", dev_name(vi->dev));
1914 
1915 	INIT_LIST_HEAD(&vi->vi_chans);
1916 
1917 	if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
1918 		ret = tegra_vi_tpg_channels_alloc(vi);
1919 	else
1920 		ret = tegra_vi_channels_alloc(vi);
1921 	if (ret < 0) {
1922 		dev_err(vi->dev,
1923 			"failed to allocate vi channels: %d\n", ret);
1924 		goto free_chans;
1925 	}
1926 
1927 	ret = tegra_vi_channels_init(vi);
1928 	if (ret < 0)
1929 		goto free_chans;
1930 
1931 	vid->vi = vi;
1932 
1933 	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)) {
1934 		ret = tegra_vi_graph_init(vi);
1935 		if (ret < 0)
1936 			goto cleanup_chans;
1937 	}
1938 
1939 	return 0;
1940 
1941 cleanup_chans:
1942 	list_for_each_entry(chan, &vi->vi_chans, list)
1943 		tegra_channel_cleanup(chan);
1944 free_chans:
1945 	list_for_each_entry_safe(chan, tmp, &vi->vi_chans, list) {
1946 		list_del(&chan->list);
1947 		kfree(chan);
1948 	}
1949 
1950 	return ret;
1951 }
1952 
tegra_vi_exit(struct host1x_client * client)1953 static int tegra_vi_exit(struct host1x_client *client)
1954 {
1955 	struct tegra_vi *vi = host1x_client_to_vi(client);
1956 
1957 	/*
1958 	 * Do not cleanup the channels here as application might still be
1959 	 * holding video device nodes. Channels cleanup will happen during
1960 	 * v4l2_device release callback which gets called after all video
1961 	 * device nodes are released.
1962 	 */
1963 
1964 	if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
1965 		tegra_vi_graph_cleanup(vi);
1966 
1967 	return 0;
1968 }
1969 
1970 static const struct host1x_client_ops vi_client_ops = {
1971 	.init = tegra_vi_init,
1972 	.exit = tegra_vi_exit,
1973 };
1974 
tegra_vi_probe(struct platform_device * pdev)1975 static int tegra_vi_probe(struct platform_device *pdev)
1976 {
1977 	struct tegra_vi *vi;
1978 	int ret;
1979 
1980 	vi = devm_kzalloc(&pdev->dev, sizeof(*vi), GFP_KERNEL);
1981 	if (!vi)
1982 		return -ENOMEM;
1983 
1984 	vi->iomem = devm_platform_ioremap_resource(pdev, 0);
1985 	if (IS_ERR(vi->iomem))
1986 		return PTR_ERR(vi->iomem);
1987 
1988 	vi->soc = of_device_get_match_data(&pdev->dev);
1989 
1990 	vi->clk = devm_clk_get(&pdev->dev, NULL);
1991 	if (IS_ERR(vi->clk)) {
1992 		ret = PTR_ERR(vi->clk);
1993 		dev_err(&pdev->dev, "failed to get vi clock: %d\n", ret);
1994 		return ret;
1995 	}
1996 
1997 	vi->vdd = devm_regulator_get(&pdev->dev, "avdd-dsi-csi");
1998 	if (IS_ERR(vi->vdd)) {
1999 		ret = PTR_ERR(vi->vdd);
2000 		dev_err(&pdev->dev, "failed to get VDD supply: %d\n", ret);
2001 		return ret;
2002 	}
2003 
2004 	if (!pdev->dev.pm_domain) {
2005 		ret = -ENOENT;
2006 		dev_warn(&pdev->dev, "PM domain is not attached: %d\n", ret);
2007 		return ret;
2008 	}
2009 
2010 	ret = devm_of_platform_populate(&pdev->dev);
2011 	if (ret < 0) {
2012 		dev_err(&pdev->dev,
2013 			"failed to populate vi child device: %d\n", ret);
2014 		return ret;
2015 	}
2016 
2017 	vi->dev = &pdev->dev;
2018 	vi->ops = vi->soc->ops;
2019 	platform_set_drvdata(pdev, vi);
2020 	pm_runtime_enable(&pdev->dev);
2021 
2022 	/* initialize host1x interface */
2023 	INIT_LIST_HEAD(&vi->client.list);
2024 	vi->client.ops = &vi_client_ops;
2025 	vi->client.dev = &pdev->dev;
2026 
2027 	ret = host1x_client_register(&vi->client);
2028 	if (ret < 0) {
2029 		dev_err(&pdev->dev,
2030 			"failed to register host1x client: %d\n", ret);
2031 		goto rpm_disable;
2032 	}
2033 
2034 	return 0;
2035 
2036 rpm_disable:
2037 	pm_runtime_disable(&pdev->dev);
2038 	return ret;
2039 }
2040 
tegra_vi_remove(struct platform_device * pdev)2041 static int tegra_vi_remove(struct platform_device *pdev)
2042 {
2043 	struct tegra_vi *vi = platform_get_drvdata(pdev);
2044 	int err;
2045 
2046 	err = host1x_client_unregister(&vi->client);
2047 	if (err < 0) {
2048 		dev_err(&pdev->dev,
2049 			"failed to unregister host1x client: %d\n", err);
2050 		return err;
2051 	}
2052 
2053 	pm_runtime_disable(&pdev->dev);
2054 
2055 	return 0;
2056 }
2057 
2058 static const struct of_device_id tegra_vi_of_id_table[] = {
2059 #if defined(CONFIG_ARCH_TEGRA_210_SOC)
2060 	{ .compatible = "nvidia,tegra210-vi", .data = &tegra210_vi_soc },
2061 #endif
2062 	{ }
2063 };
2064 MODULE_DEVICE_TABLE(of, tegra_vi_of_id_table);
2065 
2066 static const struct dev_pm_ops tegra_vi_pm_ops = {
2067 	SET_RUNTIME_PM_OPS(vi_runtime_suspend, vi_runtime_resume, NULL)
2068 };
2069 
2070 struct platform_driver tegra_vi_driver = {
2071 	.driver = {
2072 		.name = "tegra-vi",
2073 		.of_match_table = tegra_vi_of_id_table,
2074 		.pm = &tegra_vi_pm_ops,
2075 	},
2076 	.probe = tegra_vi_probe,
2077 	.remove = tegra_vi_remove,
2078 };
2079