1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3  * V4L2 Deinterlacer Subdev for Freescale i.MX5/6 SOC
4  *
5  * Copyright (c) 2017 Mentor Graphics Inc.
6  */
7 #include <media/v4l2-ctrls.h>
8 #include <media/v4l2-device.h>
9 #include <media/v4l2-ioctl.h>
10 #include <media/v4l2-mc.h>
11 #include <media/v4l2-subdev.h>
12 #include <media/imx.h>
13 #include "imx-media.h"
14 
15 /*
16  * This subdev implements two different video pipelines:
17  *
18  * CSI -> VDIC
19  *
20  * In this pipeline, the CSI sends a single interlaced field F(n-1)
21  * directly to the VDIC (and optionally the following field F(n)
22  * can be sent to memory via IDMAC channel 13). This pipeline only works
23  * in VDIC's high motion mode, which only requires a single field for
24  * processing. The other motion modes (low and medium) require three
25  * fields, so this pipeline does not work in those modes. Also, it is
26  * not clear how this pipeline can deal with the various field orders
27  * (sequential BT/TB, interlaced BT/TB).
28  *
29  * MEM -> CH8,9,10 -> VDIC
30  *
31  * In this pipeline, previous field F(n-1), current field F(n), and next
32  * field F(n+1) are transferred to the VDIC via IDMAC channels 8,9,10.
33  * These memory buffers can come from a video output or mem2mem device.
34  * All motion modes are supported by this pipeline.
35  *
36  * The "direct" CSI->VDIC pipeline requires no DMA, but it can only be
37  * used in high motion mode.
38  */
39 
40 struct vdic_priv;
41 
42 struct vdic_pipeline_ops {
43 	int (*setup)(struct vdic_priv *priv);
44 	void (*start)(struct vdic_priv *priv);
45 	void (*stop)(struct vdic_priv *priv);
46 	void (*disable)(struct vdic_priv *priv);
47 };
48 
49 /*
50  * Min/Max supported width and heights.
51  */
52 #define MIN_W        32
53 #define MIN_H        32
54 #define MAX_W_VDIC  968
55 #define MAX_H_VDIC 2048
56 #define W_ALIGN    4 /* multiple of 16 pixels */
57 #define H_ALIGN    1 /* multiple of 2 lines */
58 #define S_ALIGN    1 /* multiple of 2 */
59 
60 struct vdic_priv {
61 	struct device *ipu_dev;
62 	struct ipu_soc *ipu;
63 
64 	struct v4l2_subdev   sd;
65 	struct media_pad pad[VDIC_NUM_PADS];
66 
67 	/* lock to protect all members below */
68 	struct mutex lock;
69 
70 	/* IPU units we require */
71 	struct ipu_vdi *vdi;
72 
73 	int active_input_pad;
74 
75 	struct ipuv3_channel *vdi_in_ch_p; /* F(n-1) transfer channel */
76 	struct ipuv3_channel *vdi_in_ch;   /* F(n) transfer channel */
77 	struct ipuv3_channel *vdi_in_ch_n; /* F(n+1) transfer channel */
78 
79 	/* pipeline operations */
80 	struct vdic_pipeline_ops *ops;
81 
82 	/* current and previous input buffers indirect path */
83 	struct imx_media_buffer *curr_in_buf;
84 	struct imx_media_buffer *prev_in_buf;
85 
86 	/*
87 	 * translated field type, input line stride, and field size
88 	 * for indirect path
89 	 */
90 	u32 fieldtype;
91 	u32 in_stride;
92 	u32 field_size;
93 
94 	/* the source (a video device or subdev) */
95 	struct media_entity *src;
96 	/* the sink that will receive the progressive out buffers */
97 	struct v4l2_subdev *sink_sd;
98 
99 	struct v4l2_mbus_framefmt format_mbus[VDIC_NUM_PADS];
100 	const struct imx_media_pixfmt *cc[VDIC_NUM_PADS];
101 	struct v4l2_fract frame_interval[VDIC_NUM_PADS];
102 
103 	/* the video device at IDMAC input pad */
104 	struct imx_media_video_dev *vdev;
105 
106 	bool csi_direct;  /* using direct CSI->VDIC->IC pipeline */
107 
108 	/* motion select control */
109 	struct v4l2_ctrl_handler ctrl_hdlr;
110 	enum ipu_motion_sel motion;
111 
112 	int stream_count;
113 };
114 
vdic_put_ipu_resources(struct vdic_priv * priv)115 static void vdic_put_ipu_resources(struct vdic_priv *priv)
116 {
117 	if (priv->vdi_in_ch_p)
118 		ipu_idmac_put(priv->vdi_in_ch_p);
119 	priv->vdi_in_ch_p = NULL;
120 
121 	if (priv->vdi_in_ch)
122 		ipu_idmac_put(priv->vdi_in_ch);
123 	priv->vdi_in_ch = NULL;
124 
125 	if (priv->vdi_in_ch_n)
126 		ipu_idmac_put(priv->vdi_in_ch_n);
127 	priv->vdi_in_ch_n = NULL;
128 
129 	if (!IS_ERR_OR_NULL(priv->vdi))
130 		ipu_vdi_put(priv->vdi);
131 	priv->vdi = NULL;
132 }
133 
vdic_get_ipu_resources(struct vdic_priv * priv)134 static int vdic_get_ipu_resources(struct vdic_priv *priv)
135 {
136 	int ret, err_chan;
137 	struct ipuv3_channel *ch;
138 	struct ipu_vdi *vdi;
139 
140 	vdi = ipu_vdi_get(priv->ipu);
141 	if (IS_ERR(vdi)) {
142 		v4l2_err(&priv->sd, "failed to get VDIC\n");
143 		ret = PTR_ERR(vdi);
144 		goto out;
145 	}
146 	priv->vdi = vdi;
147 
148 	if (!priv->csi_direct) {
149 		ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_PREV);
150 		if (IS_ERR(ch)) {
151 			err_chan = IPUV3_CHANNEL_MEM_VDI_PREV;
152 			ret = PTR_ERR(ch);
153 			goto out_err_chan;
154 		}
155 		priv->vdi_in_ch_p = ch;
156 
157 		ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_CUR);
158 		if (IS_ERR(ch)) {
159 			err_chan = IPUV3_CHANNEL_MEM_VDI_CUR;
160 			ret = PTR_ERR(ch);
161 			goto out_err_chan;
162 		}
163 		priv->vdi_in_ch = ch;
164 
165 		ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_NEXT);
166 		if (IS_ERR(ch)) {
167 			err_chan = IPUV3_CHANNEL_MEM_VDI_NEXT;
168 			ret = PTR_ERR(ch);
169 			goto out_err_chan;
170 		}
171 		priv->vdi_in_ch_n = ch;
172 	}
173 
174 	return 0;
175 
176 out_err_chan:
177 	v4l2_err(&priv->sd, "could not get IDMAC channel %u\n", err_chan);
178 out:
179 	vdic_put_ipu_resources(priv);
180 	return ret;
181 }
182 
183 /*
184  * This function is currently unused, but will be called when the
185  * output/mem2mem device at the IDMAC input pad sends us a new
186  * buffer. It kicks off the IDMAC read channels to bring in the
187  * buffer fields from memory and begin the conversions.
188  */
prepare_vdi_in_buffers(struct vdic_priv * priv,struct imx_media_buffer * curr)189 static void __maybe_unused prepare_vdi_in_buffers(struct vdic_priv *priv,
190 						  struct imx_media_buffer *curr)
191 {
192 	dma_addr_t prev_phys, curr_phys, next_phys;
193 	struct imx_media_buffer *prev;
194 	struct vb2_buffer *curr_vb, *prev_vb;
195 	u32 fs = priv->field_size;
196 	u32 is = priv->in_stride;
197 
198 	/* current input buffer is now previous */
199 	priv->prev_in_buf = priv->curr_in_buf;
200 	priv->curr_in_buf = curr;
201 	prev = priv->prev_in_buf ? priv->prev_in_buf : curr;
202 
203 	prev_vb = &prev->vbuf.vb2_buf;
204 	curr_vb = &curr->vbuf.vb2_buf;
205 
206 	switch (priv->fieldtype) {
207 	case V4L2_FIELD_SEQ_TB:
208 	case V4L2_FIELD_SEQ_BT:
209 		prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + fs;
210 		curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
211 		next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + fs;
212 		break;
213 	case V4L2_FIELD_INTERLACED_TB:
214 	case V4L2_FIELD_INTERLACED_BT:
215 	case V4L2_FIELD_INTERLACED:
216 		prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + is;
217 		curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
218 		next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + is;
219 		break;
220 	default:
221 		/*
222 		 * can't get here, priv->fieldtype can only be one of
223 		 * the above. This is to quiet smatch errors.
224 		 */
225 		return;
226 	}
227 
228 	ipu_cpmem_set_buffer(priv->vdi_in_ch_p, 0, prev_phys);
229 	ipu_cpmem_set_buffer(priv->vdi_in_ch,   0, curr_phys);
230 	ipu_cpmem_set_buffer(priv->vdi_in_ch_n, 0, next_phys);
231 
232 	ipu_idmac_select_buffer(priv->vdi_in_ch_p, 0);
233 	ipu_idmac_select_buffer(priv->vdi_in_ch, 0);
234 	ipu_idmac_select_buffer(priv->vdi_in_ch_n, 0);
235 }
236 
setup_vdi_channel(struct vdic_priv * priv,struct ipuv3_channel * channel,dma_addr_t phys0,dma_addr_t phys1)237 static int setup_vdi_channel(struct vdic_priv *priv,
238 			     struct ipuv3_channel *channel,
239 			     dma_addr_t phys0, dma_addr_t phys1)
240 {
241 	struct imx_media_video_dev *vdev = priv->vdev;
242 	unsigned int burst_size;
243 	struct ipu_image image;
244 	int ret;
245 
246 	ipu_cpmem_zero(channel);
247 
248 	memset(&image, 0, sizeof(image));
249 	image.pix = vdev->fmt;
250 	image.rect = vdev->compose;
251 	/* one field to VDIC channels */
252 	image.pix.height /= 2;
253 	image.rect.height /= 2;
254 	image.phys0 = phys0;
255 	image.phys1 = phys1;
256 
257 	ret = ipu_cpmem_set_image(channel, &image);
258 	if (ret)
259 		return ret;
260 
261 	burst_size = (image.pix.width & 0xf) ? 8 : 16;
262 	ipu_cpmem_set_burstsize(channel, burst_size);
263 
264 	ipu_cpmem_set_axi_id(channel, 1);
265 
266 	ipu_idmac_set_double_buffer(channel, false);
267 
268 	return 0;
269 }
270 
vdic_setup_direct(struct vdic_priv * priv)271 static int vdic_setup_direct(struct vdic_priv *priv)
272 {
273 	/* set VDIC to receive from CSI for direct path */
274 	ipu_fsu_link(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
275 		     IPUV3_CHANNEL_CSI_VDI_PREV);
276 
277 	return 0;
278 }
279 
vdic_start_direct(struct vdic_priv * priv)280 static void vdic_start_direct(struct vdic_priv *priv)
281 {
282 }
283 
vdic_stop_direct(struct vdic_priv * priv)284 static void vdic_stop_direct(struct vdic_priv *priv)
285 {
286 }
287 
vdic_disable_direct(struct vdic_priv * priv)288 static void vdic_disable_direct(struct vdic_priv *priv)
289 {
290 	ipu_fsu_unlink(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
291 		       IPUV3_CHANNEL_CSI_VDI_PREV);
292 }
293 
vdic_setup_indirect(struct vdic_priv * priv)294 static int vdic_setup_indirect(struct vdic_priv *priv)
295 {
296 	struct v4l2_mbus_framefmt *infmt;
297 	const struct imx_media_pixfmt *incc;
298 	int in_size, ret;
299 
300 	infmt = &priv->format_mbus[VDIC_SINK_PAD_IDMAC];
301 	incc = priv->cc[VDIC_SINK_PAD_IDMAC];
302 
303 	in_size = (infmt->width * incc->bpp * infmt->height) >> 3;
304 
305 	/* 1/2 full image size */
306 	priv->field_size = in_size / 2;
307 	priv->in_stride = incc->planar ?
308 		infmt->width : (infmt->width * incc->bpp) >> 3;
309 
310 	priv->prev_in_buf = NULL;
311 	priv->curr_in_buf = NULL;
312 
313 	priv->fieldtype = infmt->field;
314 
315 	/* init the vdi-in channels */
316 	ret = setup_vdi_channel(priv, priv->vdi_in_ch_p, 0, 0);
317 	if (ret)
318 		return ret;
319 	ret = setup_vdi_channel(priv, priv->vdi_in_ch, 0, 0);
320 	if (ret)
321 		return ret;
322 	return setup_vdi_channel(priv, priv->vdi_in_ch_n, 0, 0);
323 }
324 
vdic_start_indirect(struct vdic_priv * priv)325 static void vdic_start_indirect(struct vdic_priv *priv)
326 {
327 	/* enable the channels */
328 	ipu_idmac_enable_channel(priv->vdi_in_ch_p);
329 	ipu_idmac_enable_channel(priv->vdi_in_ch);
330 	ipu_idmac_enable_channel(priv->vdi_in_ch_n);
331 }
332 
vdic_stop_indirect(struct vdic_priv * priv)333 static void vdic_stop_indirect(struct vdic_priv *priv)
334 {
335 	/* disable channels */
336 	ipu_idmac_disable_channel(priv->vdi_in_ch_p);
337 	ipu_idmac_disable_channel(priv->vdi_in_ch);
338 	ipu_idmac_disable_channel(priv->vdi_in_ch_n);
339 }
340 
vdic_disable_indirect(struct vdic_priv * priv)341 static void vdic_disable_indirect(struct vdic_priv *priv)
342 {
343 }
344 
345 static struct vdic_pipeline_ops direct_ops = {
346 	.setup = vdic_setup_direct,
347 	.start = vdic_start_direct,
348 	.stop = vdic_stop_direct,
349 	.disable = vdic_disable_direct,
350 };
351 
352 static struct vdic_pipeline_ops indirect_ops = {
353 	.setup = vdic_setup_indirect,
354 	.start = vdic_start_indirect,
355 	.stop = vdic_stop_indirect,
356 	.disable = vdic_disable_indirect,
357 };
358 
vdic_start(struct vdic_priv * priv)359 static int vdic_start(struct vdic_priv *priv)
360 {
361 	struct v4l2_mbus_framefmt *infmt;
362 	int ret;
363 
364 	infmt = &priv->format_mbus[priv->active_input_pad];
365 
366 	priv->ops = priv->csi_direct ? &direct_ops : &indirect_ops;
367 
368 	ret = vdic_get_ipu_resources(priv);
369 	if (ret)
370 		return ret;
371 
372 	/*
373 	 * init the VDIC.
374 	 *
375 	 * note we don't give infmt->code to ipu_vdi_setup(). The VDIC
376 	 * only supports 4:2:2 or 4:2:0, and this subdev will only
377 	 * negotiate 4:2:2 at its sink pads.
378 	 */
379 	ipu_vdi_setup(priv->vdi, MEDIA_BUS_FMT_UYVY8_2X8,
380 		      infmt->width, infmt->height);
381 	ipu_vdi_set_field_order(priv->vdi, V4L2_STD_UNKNOWN, infmt->field);
382 	ipu_vdi_set_motion(priv->vdi, priv->motion);
383 
384 	ret = priv->ops->setup(priv);
385 	if (ret)
386 		goto out_put_ipu;
387 
388 	ipu_vdi_enable(priv->vdi);
389 
390 	priv->ops->start(priv);
391 
392 	return 0;
393 
394 out_put_ipu:
395 	vdic_put_ipu_resources(priv);
396 	return ret;
397 }
398 
vdic_stop(struct vdic_priv * priv)399 static void vdic_stop(struct vdic_priv *priv)
400 {
401 	priv->ops->stop(priv);
402 	ipu_vdi_disable(priv->vdi);
403 	priv->ops->disable(priv);
404 
405 	vdic_put_ipu_resources(priv);
406 }
407 
408 /*
409  * V4L2 subdev operations.
410  */
411 
vdic_s_ctrl(struct v4l2_ctrl * ctrl)412 static int vdic_s_ctrl(struct v4l2_ctrl *ctrl)
413 {
414 	struct vdic_priv *priv = container_of(ctrl->handler,
415 					      struct vdic_priv, ctrl_hdlr);
416 	enum ipu_motion_sel motion;
417 	int ret = 0;
418 
419 	mutex_lock(&priv->lock);
420 
421 	switch (ctrl->id) {
422 	case V4L2_CID_DEINTERLACING_MODE:
423 		motion = ctrl->val;
424 		if (motion != priv->motion) {
425 			/* can't change motion control mid-streaming */
426 			if (priv->stream_count > 0) {
427 				ret = -EBUSY;
428 				goto out;
429 			}
430 			priv->motion = motion;
431 		}
432 		break;
433 	default:
434 		v4l2_err(&priv->sd, "Invalid control\n");
435 		ret = -EINVAL;
436 	}
437 
438 out:
439 	mutex_unlock(&priv->lock);
440 	return ret;
441 }
442 
443 static const struct v4l2_ctrl_ops vdic_ctrl_ops = {
444 	.s_ctrl = vdic_s_ctrl,
445 };
446 
447 static const char * const vdic_ctrl_motion_menu[] = {
448 	"No Motion Compensation",
449 	"Low Motion",
450 	"Medium Motion",
451 	"High Motion",
452 };
453 
vdic_init_controls(struct vdic_priv * priv)454 static int vdic_init_controls(struct vdic_priv *priv)
455 {
456 	struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr;
457 	int ret;
458 
459 	v4l2_ctrl_handler_init(hdlr, 1);
460 
461 	v4l2_ctrl_new_std_menu_items(hdlr, &vdic_ctrl_ops,
462 				     V4L2_CID_DEINTERLACING_MODE,
463 				     HIGH_MOTION, 0, HIGH_MOTION,
464 				     vdic_ctrl_motion_menu);
465 
466 	priv->sd.ctrl_handler = hdlr;
467 
468 	if (hdlr->error) {
469 		ret = hdlr->error;
470 		goto out_free;
471 	}
472 
473 	v4l2_ctrl_handler_setup(hdlr);
474 	return 0;
475 
476 out_free:
477 	v4l2_ctrl_handler_free(hdlr);
478 	return ret;
479 }
480 
vdic_s_stream(struct v4l2_subdev * sd,int enable)481 static int vdic_s_stream(struct v4l2_subdev *sd, int enable)
482 {
483 	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
484 	struct v4l2_subdev *src_sd = NULL;
485 	int ret = 0;
486 
487 	mutex_lock(&priv->lock);
488 
489 	if (!priv->src || !priv->sink_sd) {
490 		ret = -EPIPE;
491 		goto out;
492 	}
493 
494 	if (priv->csi_direct)
495 		src_sd = media_entity_to_v4l2_subdev(priv->src);
496 
497 	/*
498 	 * enable/disable streaming only if stream_count is
499 	 * going from 0 to 1 / 1 to 0.
500 	 */
501 	if (priv->stream_count != !enable)
502 		goto update_count;
503 
504 	dev_dbg(priv->ipu_dev, "%s: stream %s\n", sd->name,
505 		enable ? "ON" : "OFF");
506 
507 	if (enable)
508 		ret = vdic_start(priv);
509 	else
510 		vdic_stop(priv);
511 	if (ret)
512 		goto out;
513 
514 	if (src_sd) {
515 		/* start/stop upstream */
516 		ret = v4l2_subdev_call(src_sd, video, s_stream, enable);
517 		ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
518 		if (ret) {
519 			if (enable)
520 				vdic_stop(priv);
521 			goto out;
522 		}
523 	}
524 
525 update_count:
526 	priv->stream_count += enable ? 1 : -1;
527 	if (priv->stream_count < 0)
528 		priv->stream_count = 0;
529 out:
530 	mutex_unlock(&priv->lock);
531 	return ret;
532 }
533 
534 static struct v4l2_mbus_framefmt *
__vdic_get_fmt(struct vdic_priv * priv,struct v4l2_subdev_state * sd_state,unsigned int pad,enum v4l2_subdev_format_whence which)535 __vdic_get_fmt(struct vdic_priv *priv, struct v4l2_subdev_state *sd_state,
536 	       unsigned int pad, enum v4l2_subdev_format_whence which)
537 {
538 	if (which == V4L2_SUBDEV_FORMAT_TRY)
539 		return v4l2_subdev_get_try_format(&priv->sd, sd_state, pad);
540 	else
541 		return &priv->format_mbus[pad];
542 }
543 
vdic_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)544 static int vdic_enum_mbus_code(struct v4l2_subdev *sd,
545 			       struct v4l2_subdev_state *sd_state,
546 			       struct v4l2_subdev_mbus_code_enum *code)
547 {
548 	if (code->pad >= VDIC_NUM_PADS)
549 		return -EINVAL;
550 
551 	return imx_media_enum_ipu_formats(&code->code, code->index,
552 					  PIXFMT_SEL_YUV);
553 }
554 
vdic_get_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sdformat)555 static int vdic_get_fmt(struct v4l2_subdev *sd,
556 			struct v4l2_subdev_state *sd_state,
557 			struct v4l2_subdev_format *sdformat)
558 {
559 	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
560 	struct v4l2_mbus_framefmt *fmt;
561 	int ret = 0;
562 
563 	if (sdformat->pad >= VDIC_NUM_PADS)
564 		return -EINVAL;
565 
566 	mutex_lock(&priv->lock);
567 
568 	fmt = __vdic_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
569 	if (!fmt) {
570 		ret = -EINVAL;
571 		goto out;
572 	}
573 
574 	sdformat->format = *fmt;
575 out:
576 	mutex_unlock(&priv->lock);
577 	return ret;
578 }
579 
vdic_try_fmt(struct vdic_priv * priv,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sdformat,const struct imx_media_pixfmt ** cc)580 static void vdic_try_fmt(struct vdic_priv *priv,
581 			 struct v4l2_subdev_state *sd_state,
582 			 struct v4l2_subdev_format *sdformat,
583 			 const struct imx_media_pixfmt **cc)
584 {
585 	struct v4l2_mbus_framefmt *infmt;
586 
587 	*cc = imx_media_find_ipu_format(sdformat->format.code,
588 					PIXFMT_SEL_YUV);
589 	if (!*cc) {
590 		u32 code;
591 
592 		imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV);
593 		*cc = imx_media_find_ipu_format(code, PIXFMT_SEL_YUV);
594 		sdformat->format.code = (*cc)->codes[0];
595 	}
596 
597 	infmt = __vdic_get_fmt(priv, sd_state, priv->active_input_pad,
598 			       sdformat->which);
599 
600 	switch (sdformat->pad) {
601 	case VDIC_SRC_PAD_DIRECT:
602 		sdformat->format = *infmt;
603 		/* output is always progressive! */
604 		sdformat->format.field = V4L2_FIELD_NONE;
605 		break;
606 	case VDIC_SINK_PAD_DIRECT:
607 	case VDIC_SINK_PAD_IDMAC:
608 		v4l_bound_align_image(&sdformat->format.width,
609 				      MIN_W, MAX_W_VDIC, W_ALIGN,
610 				      &sdformat->format.height,
611 				      MIN_H, MAX_H_VDIC, H_ALIGN, S_ALIGN);
612 
613 		/* input must be interlaced! Choose SEQ_TB if not */
614 		if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field))
615 			sdformat->format.field = V4L2_FIELD_SEQ_TB;
616 		break;
617 	}
618 
619 	imx_media_try_colorimetry(&sdformat->format, true);
620 }
621 
vdic_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sdformat)622 static int vdic_set_fmt(struct v4l2_subdev *sd,
623 			struct v4l2_subdev_state *sd_state,
624 			struct v4l2_subdev_format *sdformat)
625 {
626 	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
627 	const struct imx_media_pixfmt *cc;
628 	struct v4l2_mbus_framefmt *fmt;
629 	int ret = 0;
630 
631 	if (sdformat->pad >= VDIC_NUM_PADS)
632 		return -EINVAL;
633 
634 	mutex_lock(&priv->lock);
635 
636 	if (priv->stream_count > 0) {
637 		ret = -EBUSY;
638 		goto out;
639 	}
640 
641 	vdic_try_fmt(priv, sd_state, sdformat, &cc);
642 
643 	fmt = __vdic_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
644 	*fmt = sdformat->format;
645 
646 	/* propagate format to source pad */
647 	if (sdformat->pad == VDIC_SINK_PAD_DIRECT ||
648 	    sdformat->pad == VDIC_SINK_PAD_IDMAC) {
649 		const struct imx_media_pixfmt *outcc;
650 		struct v4l2_mbus_framefmt *outfmt;
651 		struct v4l2_subdev_format format;
652 
653 		format.pad = VDIC_SRC_PAD_DIRECT;
654 		format.which = sdformat->which;
655 		format.format = sdformat->format;
656 		vdic_try_fmt(priv, sd_state, &format, &outcc);
657 
658 		outfmt = __vdic_get_fmt(priv, sd_state, VDIC_SRC_PAD_DIRECT,
659 					sdformat->which);
660 		*outfmt = format.format;
661 		if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
662 			priv->cc[VDIC_SRC_PAD_DIRECT] = outcc;
663 	}
664 
665 	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
666 		priv->cc[sdformat->pad] = cc;
667 out:
668 	mutex_unlock(&priv->lock);
669 	return ret;
670 }
671 
vdic_link_setup(struct media_entity * entity,const struct media_pad * local,const struct media_pad * remote,u32 flags)672 static int vdic_link_setup(struct media_entity *entity,
673 			    const struct media_pad *local,
674 			    const struct media_pad *remote, u32 flags)
675 {
676 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
677 	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
678 	struct v4l2_subdev *remote_sd;
679 	int ret = 0;
680 
681 	dev_dbg(priv->ipu_dev, "%s: link setup %s -> %s",
682 		sd->name, remote->entity->name, local->entity->name);
683 
684 	mutex_lock(&priv->lock);
685 
686 	if (local->flags & MEDIA_PAD_FL_SOURCE) {
687 		if (!is_media_entity_v4l2_subdev(remote->entity)) {
688 			ret = -EINVAL;
689 			goto out;
690 		}
691 
692 		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
693 
694 		if (flags & MEDIA_LNK_FL_ENABLED) {
695 			if (priv->sink_sd) {
696 				ret = -EBUSY;
697 				goto out;
698 			}
699 			priv->sink_sd = remote_sd;
700 		} else {
701 			priv->sink_sd = NULL;
702 		}
703 
704 		goto out;
705 	}
706 
707 	/* this is a sink pad */
708 
709 	if (flags & MEDIA_LNK_FL_ENABLED) {
710 		if (priv->src) {
711 			ret = -EBUSY;
712 			goto out;
713 		}
714 	} else {
715 		priv->src = NULL;
716 		goto out;
717 	}
718 
719 	if (local->index == VDIC_SINK_PAD_IDMAC) {
720 		struct imx_media_video_dev *vdev = priv->vdev;
721 
722 		if (!is_media_entity_v4l2_video_device(remote->entity)) {
723 			ret = -EINVAL;
724 			goto out;
725 		}
726 		if (!vdev) {
727 			ret = -ENODEV;
728 			goto out;
729 		}
730 
731 		priv->csi_direct = false;
732 	} else {
733 		if (!is_media_entity_v4l2_subdev(remote->entity)) {
734 			ret = -EINVAL;
735 			goto out;
736 		}
737 
738 		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
739 
740 		/* direct pad must connect to a CSI */
741 		if (!(remote_sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) ||
742 		    remote->index != CSI_SRC_PAD_DIRECT) {
743 			ret = -EINVAL;
744 			goto out;
745 		}
746 
747 		priv->csi_direct = true;
748 	}
749 
750 	priv->src = remote->entity;
751 	/* record which input pad is now active */
752 	priv->active_input_pad = local->index;
753 out:
754 	mutex_unlock(&priv->lock);
755 	return ret;
756 }
757 
vdic_link_validate(struct v4l2_subdev * sd,struct media_link * link,struct v4l2_subdev_format * source_fmt,struct v4l2_subdev_format * sink_fmt)758 static int vdic_link_validate(struct v4l2_subdev *sd,
759 			      struct media_link *link,
760 			      struct v4l2_subdev_format *source_fmt,
761 			      struct v4l2_subdev_format *sink_fmt)
762 {
763 	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
764 	int ret;
765 
766 	ret = v4l2_subdev_link_validate_default(sd, link,
767 						source_fmt, sink_fmt);
768 	if (ret)
769 		return ret;
770 
771 	mutex_lock(&priv->lock);
772 
773 	if (priv->csi_direct && priv->motion != HIGH_MOTION) {
774 		v4l2_err(&priv->sd,
775 			 "direct CSI pipeline requires high motion\n");
776 		ret = -EINVAL;
777 	}
778 
779 	mutex_unlock(&priv->lock);
780 	return ret;
781 }
782 
vdic_g_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_frame_interval * fi)783 static int vdic_g_frame_interval(struct v4l2_subdev *sd,
784 				struct v4l2_subdev_frame_interval *fi)
785 {
786 	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
787 
788 	if (fi->pad >= VDIC_NUM_PADS)
789 		return -EINVAL;
790 
791 	mutex_lock(&priv->lock);
792 
793 	fi->interval = priv->frame_interval[fi->pad];
794 
795 	mutex_unlock(&priv->lock);
796 
797 	return 0;
798 }
799 
vdic_s_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_frame_interval * fi)800 static int vdic_s_frame_interval(struct v4l2_subdev *sd,
801 				struct v4l2_subdev_frame_interval *fi)
802 {
803 	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
804 	struct v4l2_fract *input_fi, *output_fi;
805 	int ret = 0;
806 
807 	mutex_lock(&priv->lock);
808 
809 	input_fi = &priv->frame_interval[priv->active_input_pad];
810 	output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT];
811 
812 	switch (fi->pad) {
813 	case VDIC_SINK_PAD_DIRECT:
814 	case VDIC_SINK_PAD_IDMAC:
815 		/* No limits on valid input frame intervals */
816 		if (fi->interval.numerator == 0 ||
817 		    fi->interval.denominator == 0)
818 			fi->interval = priv->frame_interval[fi->pad];
819 		/* Reset output interval */
820 		*output_fi = fi->interval;
821 		if (priv->csi_direct)
822 			output_fi->denominator *= 2;
823 		break;
824 	case VDIC_SRC_PAD_DIRECT:
825 		/*
826 		 * frame rate at output pad is double input
827 		 * rate when using direct CSI->VDIC pipeline.
828 		 *
829 		 * TODO: implement VDIC frame skipping
830 		 */
831 		fi->interval = *input_fi;
832 		if (priv->csi_direct)
833 			fi->interval.denominator *= 2;
834 		break;
835 	default:
836 		ret = -EINVAL;
837 		goto out;
838 	}
839 
840 	priv->frame_interval[fi->pad] = fi->interval;
841 out:
842 	mutex_unlock(&priv->lock);
843 	return ret;
844 }
845 
vdic_registered(struct v4l2_subdev * sd)846 static int vdic_registered(struct v4l2_subdev *sd)
847 {
848 	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
849 	int i, ret;
850 	u32 code;
851 
852 	for (i = 0; i < VDIC_NUM_PADS; i++) {
853 		code = 0;
854 		if (i != VDIC_SINK_PAD_IDMAC)
855 			imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV);
856 
857 		/* set a default mbus format  */
858 		ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
859 					      IMX_MEDIA_DEF_PIX_WIDTH,
860 					      IMX_MEDIA_DEF_PIX_HEIGHT, code,
861 					      V4L2_FIELD_NONE, &priv->cc[i]);
862 		if (ret)
863 			return ret;
864 
865 		/* init default frame interval */
866 		priv->frame_interval[i].numerator = 1;
867 		priv->frame_interval[i].denominator = 30;
868 		if (i == VDIC_SRC_PAD_DIRECT)
869 			priv->frame_interval[i].denominator *= 2;
870 	}
871 
872 	priv->active_input_pad = VDIC_SINK_PAD_DIRECT;
873 
874 	return vdic_init_controls(priv);
875 }
876 
vdic_unregistered(struct v4l2_subdev * sd)877 static void vdic_unregistered(struct v4l2_subdev *sd)
878 {
879 	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
880 
881 	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
882 }
883 
884 static const struct v4l2_subdev_pad_ops vdic_pad_ops = {
885 	.init_cfg = imx_media_init_cfg,
886 	.enum_mbus_code = vdic_enum_mbus_code,
887 	.get_fmt = vdic_get_fmt,
888 	.set_fmt = vdic_set_fmt,
889 	.link_validate = vdic_link_validate,
890 };
891 
892 static const struct v4l2_subdev_video_ops vdic_video_ops = {
893 	.g_frame_interval = vdic_g_frame_interval,
894 	.s_frame_interval = vdic_s_frame_interval,
895 	.s_stream = vdic_s_stream,
896 };
897 
898 static const struct media_entity_operations vdic_entity_ops = {
899 	.link_setup = vdic_link_setup,
900 	.link_validate = v4l2_subdev_link_validate,
901 };
902 
903 static const struct v4l2_subdev_ops vdic_subdev_ops = {
904 	.video = &vdic_video_ops,
905 	.pad = &vdic_pad_ops,
906 };
907 
908 static const struct v4l2_subdev_internal_ops vdic_internal_ops = {
909 	.registered = vdic_registered,
910 	.unregistered = vdic_unregistered,
911 };
912 
imx_media_vdic_register(struct v4l2_device * v4l2_dev,struct device * ipu_dev,struct ipu_soc * ipu,u32 grp_id)913 struct v4l2_subdev *imx_media_vdic_register(struct v4l2_device *v4l2_dev,
914 					    struct device *ipu_dev,
915 					    struct ipu_soc *ipu,
916 					    u32 grp_id)
917 {
918 	struct vdic_priv *priv;
919 	int i, ret;
920 
921 	priv = devm_kzalloc(ipu_dev, sizeof(*priv), GFP_KERNEL);
922 	if (!priv)
923 		return ERR_PTR(-ENOMEM);
924 
925 	priv->ipu_dev = ipu_dev;
926 	priv->ipu = ipu;
927 
928 	v4l2_subdev_init(&priv->sd, &vdic_subdev_ops);
929 	v4l2_set_subdevdata(&priv->sd, priv);
930 	priv->sd.internal_ops = &vdic_internal_ops;
931 	priv->sd.entity.ops = &vdic_entity_ops;
932 	priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
933 	priv->sd.owner = ipu_dev->driver->owner;
934 	priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
935 	priv->sd.grp_id = grp_id;
936 	imx_media_grp_id_to_sd_name(priv->sd.name, sizeof(priv->sd.name),
937 				    priv->sd.grp_id, ipu_get_num(ipu));
938 
939 	mutex_init(&priv->lock);
940 
941 	for (i = 0; i < VDIC_NUM_PADS; i++)
942 		priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ?
943 			MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
944 
945 	ret = media_entity_pads_init(&priv->sd.entity, VDIC_NUM_PADS,
946 				     priv->pad);
947 	if (ret)
948 		goto free;
949 
950 	ret = v4l2_device_register_subdev(v4l2_dev, &priv->sd);
951 	if (ret)
952 		goto free;
953 
954 	return &priv->sd;
955 free:
956 	mutex_destroy(&priv->lock);
957 	return ERR_PTR(ret);
958 }
959 
imx_media_vdic_unregister(struct v4l2_subdev * sd)960 int imx_media_vdic_unregister(struct v4l2_subdev *sd)
961 {
962 	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
963 
964 	v4l2_info(sd, "Removing\n");
965 
966 	v4l2_device_unregister_subdev(sd);
967 	mutex_destroy(&priv->lock);
968 	media_entity_cleanup(&sd->entity);
969 
970 	return 0;
971 }
972