imx-media-vdic.c

  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
 40struct vdic_priv;
 41
 42struct 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
 60struct 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
115static 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
134static 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
176out_err_chan:
177	v4l2_err(&priv->sd, "could not get IDMAC channel %u\n", err_chan);
178out:
179	vdic_put_ipu_resources(priv);
180	return ret;
181}
182
183static int setup_vdi_channel(struct vdic_priv *priv,
184			     struct ipuv3_channel *channel,
185			     dma_addr_t phys0, dma_addr_t phys1)
186{
187	struct imx_media_video_dev *vdev = priv->vdev;
188	unsigned int burst_size;
189	struct ipu_image image;
190	int ret;
191
192	ipu_cpmem_zero(channel);
193
194	memset(&image, 0, sizeof(image));
195	image.pix = vdev->fmt;
196	image.rect = vdev->compose;
197	/* one field to VDIC channels */
198	image.pix.height /= 2;
199	image.rect.height /= 2;
200	image.phys0 = phys0;
201	image.phys1 = phys1;
202
203	ret = ipu_cpmem_set_image(channel, &image);
204	if (ret)
205		return ret;
206
207	burst_size = (image.pix.width & 0xf) ? 8 : 16;
208	ipu_cpmem_set_burstsize(channel, burst_size);
209
210	ipu_cpmem_set_axi_id(channel, 1);
211
212	ipu_idmac_set_double_buffer(channel, false);
213
214	return 0;
215}
216
217static int vdic_setup_direct(struct vdic_priv *priv)
218{
219	/* set VDIC to receive from CSI for direct path */
220	ipu_fsu_link(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
221		     IPUV3_CHANNEL_CSI_VDI_PREV);
222
223	return 0;
224}
225
226static void vdic_start_direct(struct vdic_priv *priv)
227{
228}
229
230static void vdic_stop_direct(struct vdic_priv *priv)
231{
232}
233
234static void vdic_disable_direct(struct vdic_priv *priv)
235{
236	ipu_fsu_unlink(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
237		       IPUV3_CHANNEL_CSI_VDI_PREV);
238}
239
240static int vdic_setup_indirect(struct vdic_priv *priv)
241{
242	struct v4l2_mbus_framefmt *infmt;
243	const struct imx_media_pixfmt *incc;
244	int in_size, ret;
245
246	infmt = &priv->format_mbus[VDIC_SINK_PAD_IDMAC];
247	incc = priv->cc[VDIC_SINK_PAD_IDMAC];
248
249	in_size = (infmt->width * incc->bpp * infmt->height) >> 3;
250
251	/* 1/2 full image size */
252	priv->field_size = in_size / 2;
253	priv->in_stride = incc->planar ?
254		infmt->width : (infmt->width * incc->bpp) >> 3;
255
256	priv->prev_in_buf = NULL;
257	priv->curr_in_buf = NULL;
258
259	priv->fieldtype = infmt->field;
260
261	/* init the vdi-in channels */
262	ret = setup_vdi_channel(priv, priv->vdi_in_ch_p, 0, 0);
263	if (ret)
264		return ret;
265	ret = setup_vdi_channel(priv, priv->vdi_in_ch, 0, 0);
266	if (ret)
267		return ret;
268	return setup_vdi_channel(priv, priv->vdi_in_ch_n, 0, 0);
269}
270
271static void vdic_start_indirect(struct vdic_priv *priv)
272{
273	/* enable the channels */
274	ipu_idmac_enable_channel(priv->vdi_in_ch_p);
275	ipu_idmac_enable_channel(priv->vdi_in_ch);
276	ipu_idmac_enable_channel(priv->vdi_in_ch_n);
277}
278
279static void vdic_stop_indirect(struct vdic_priv *priv)
280{
281	/* disable channels */
282	ipu_idmac_disable_channel(priv->vdi_in_ch_p);
283	ipu_idmac_disable_channel(priv->vdi_in_ch);
284	ipu_idmac_disable_channel(priv->vdi_in_ch_n);
285}
286
287static void vdic_disable_indirect(struct vdic_priv *priv)
288{
289}
290
291static struct vdic_pipeline_ops direct_ops = {
292	.setup = vdic_setup_direct,
293	.start = vdic_start_direct,
294	.stop = vdic_stop_direct,
295	.disable = vdic_disable_direct,
296};
297
298static struct vdic_pipeline_ops indirect_ops = {
299	.setup = vdic_setup_indirect,
300	.start = vdic_start_indirect,
301	.stop = vdic_stop_indirect,
302	.disable = vdic_disable_indirect,
303};
304
305static int vdic_start(struct vdic_priv *priv)
306{
307	struct v4l2_mbus_framefmt *infmt;
308	int ret;
309
310	infmt = &priv->format_mbus[priv->active_input_pad];
311
312	priv->ops = priv->csi_direct ? &direct_ops : &indirect_ops;
313
314	ret = vdic_get_ipu_resources(priv);
315	if (ret)
316		return ret;
317
318	/*
319	 * init the VDIC.
320	 *
321	 * note we don't give infmt->code to ipu_vdi_setup(). The VDIC
322	 * only supports 4:2:2 or 4:2:0, and this subdev will only
323	 * negotiate 4:2:2 at its sink pads.
324	 */
325	ipu_vdi_setup(priv->vdi, MEDIA_BUS_FMT_UYVY8_2X8,
326		      infmt->width, infmt->height);
327	ipu_vdi_set_field_order(priv->vdi, V4L2_STD_UNKNOWN, infmt->field);
328	ipu_vdi_set_motion(priv->vdi, priv->motion);
329
330	ret = priv->ops->setup(priv);
331	if (ret)
332		goto out_put_ipu;
333
334	ipu_vdi_enable(priv->vdi);
335
336	priv->ops->start(priv);
337
338	return 0;
339
340out_put_ipu:
341	vdic_put_ipu_resources(priv);
342	return ret;
343}
344
345static void vdic_stop(struct vdic_priv *priv)
346{
347	priv->ops->stop(priv);
348	ipu_vdi_disable(priv->vdi);
349	priv->ops->disable(priv);
350
351	vdic_put_ipu_resources(priv);
352}
353
354/*
355 * V4L2 subdev operations.
356 */
357
358static int vdic_s_ctrl(struct v4l2_ctrl *ctrl)
359{
360	struct vdic_priv *priv = container_of(ctrl->handler,
361					      struct vdic_priv, ctrl_hdlr);
362	enum ipu_motion_sel motion;
363	int ret = 0;
364
365	mutex_lock(&priv->lock);
366
367	switch (ctrl->id) {
368	case V4L2_CID_DEINTERLACING_MODE:
369		motion = ctrl->val;
370		if (motion != priv->motion) {
371			/* can't change motion control mid-streaming */
372			if (priv->stream_count > 0) {
373				ret = -EBUSY;
374				goto out;
375			}
376			priv->motion = motion;
377		}
378		break;
379	default:
380		v4l2_err(&priv->sd, "Invalid control\n");
381		ret = -EINVAL;
382	}
383
384out:
385	mutex_unlock(&priv->lock);
386	return ret;
387}
388
389static const struct v4l2_ctrl_ops vdic_ctrl_ops = {
390	.s_ctrl = vdic_s_ctrl,
391};
392
393static const char * const vdic_ctrl_motion_menu[] = {
394	"No Motion Compensation",
395	"Low Motion",
396	"Medium Motion",
397	"High Motion",
398};
399
400static int vdic_init_controls(struct vdic_priv *priv)
401{
402	struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr;
403	int ret;
404
405	v4l2_ctrl_handler_init(hdlr, 1);
406
407	v4l2_ctrl_new_std_menu_items(hdlr, &vdic_ctrl_ops,
408				     V4L2_CID_DEINTERLACING_MODE,
409				     HIGH_MOTION, 0, HIGH_MOTION,
410				     vdic_ctrl_motion_menu);
411
412	priv->sd.ctrl_handler = hdlr;
413
414	if (hdlr->error) {
415		ret = hdlr->error;
416		goto out_free;
417	}
418
419	v4l2_ctrl_handler_setup(hdlr);
420	return 0;
421
422out_free:
423	v4l2_ctrl_handler_free(hdlr);
424	return ret;
425}
426
427static int vdic_s_stream(struct v4l2_subdev *sd, int enable)
428{
429	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
430	struct v4l2_subdev *src_sd = NULL;
431	int ret = 0;
432
433	mutex_lock(&priv->lock);
434
435	if (!priv->src || !priv->sink_sd) {
436		ret = -EPIPE;
437		goto out;
438	}
439
440	if (priv->csi_direct)
441		src_sd = media_entity_to_v4l2_subdev(priv->src);
442
443	/*
444	 * enable/disable streaming only if stream_count is
445	 * going from 0 to 1 / 1 to 0.
446	 */
447	if (priv->stream_count != !enable)
448		goto update_count;
449
450	dev_dbg(priv->ipu_dev, "%s: stream %s\n", sd->name,
451		enable ? "ON" : "OFF");
452
453	if (enable)
454		ret = vdic_start(priv);
455	else
456		vdic_stop(priv);
457	if (ret)
458		goto out;
459
460	if (src_sd) {
461		/* start/stop upstream */
462		ret = v4l2_subdev_call(src_sd, video, s_stream, enable);
463		ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
464		if (ret) {
465			if (enable)
466				vdic_stop(priv);
467			goto out;
468		}
469	}
470
471update_count:
472	priv->stream_count += enable ? 1 : -1;
473	if (priv->stream_count < 0)
474		priv->stream_count = 0;
475out:
476	mutex_unlock(&priv->lock);
477	return ret;
478}
479
480static struct v4l2_mbus_framefmt *
481__vdic_get_fmt(struct vdic_priv *priv, struct v4l2_subdev_state *sd_state,
482	       unsigned int pad, enum v4l2_subdev_format_whence which)
483{
484	if (which == V4L2_SUBDEV_FORMAT_TRY)
485		return v4l2_subdev_state_get_format(sd_state, pad);
486	else
487		return &priv->format_mbus[pad];
488}
489
490static int vdic_enum_mbus_code(struct v4l2_subdev *sd,
491			       struct v4l2_subdev_state *sd_state,
492			       struct v4l2_subdev_mbus_code_enum *code)
493{
494	if (code->pad >= VDIC_NUM_PADS)
495		return -EINVAL;
496
497	return imx_media_enum_ipu_formats(&code->code, code->index,
498					  PIXFMT_SEL_YUV);
499}
500
501static int vdic_get_fmt(struct v4l2_subdev *sd,
502			struct v4l2_subdev_state *sd_state,
503			struct v4l2_subdev_format *sdformat)
504{
505	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
506	struct v4l2_mbus_framefmt *fmt;
507	int ret = 0;
508
509	if (sdformat->pad >= VDIC_NUM_PADS)
510		return -EINVAL;
511
512	mutex_lock(&priv->lock);
513
514	fmt = __vdic_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
515	if (!fmt) {
516		ret = -EINVAL;
517		goto out;
518	}
519
520	sdformat->format = *fmt;
521out:
522	mutex_unlock(&priv->lock);
523	return ret;
524}
525
526static void vdic_try_fmt(struct vdic_priv *priv,
527			 struct v4l2_subdev_state *sd_state,
528			 struct v4l2_subdev_format *sdformat,
529			 const struct imx_media_pixfmt **cc)
530{
531	struct v4l2_mbus_framefmt *infmt;
532
533	*cc = imx_media_find_ipu_format(sdformat->format.code,
534					PIXFMT_SEL_YUV);
535	if (!*cc) {
536		u32 code;
537
538		imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV);
539		*cc = imx_media_find_ipu_format(code, PIXFMT_SEL_YUV);
540		sdformat->format.code = (*cc)->codes[0];
541	}
542
543	infmt = __vdic_get_fmt(priv, sd_state, priv->active_input_pad,
544			       sdformat->which);
545
546	switch (sdformat->pad) {
547	case VDIC_SRC_PAD_DIRECT:
548		sdformat->format = *infmt;
549		/* output is always progressive! */
550		sdformat->format.field = V4L2_FIELD_NONE;
551		break;
552	case VDIC_SINK_PAD_DIRECT:
553	case VDIC_SINK_PAD_IDMAC:
554		v4l_bound_align_image(&sdformat->format.width,
555				      MIN_W, MAX_W_VDIC, W_ALIGN,
556				      &sdformat->format.height,
557				      MIN_H, MAX_H_VDIC, H_ALIGN, S_ALIGN);
558
559		/* input must be interlaced! Choose SEQ_TB if not */
560		if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field))
561			sdformat->format.field = V4L2_FIELD_SEQ_TB;
562		break;
563	}
564
565	imx_media_try_colorimetry(&sdformat->format, true);
566}
567
568static int vdic_set_fmt(struct v4l2_subdev *sd,
569			struct v4l2_subdev_state *sd_state,
570			struct v4l2_subdev_format *sdformat)
571{
572	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
573	const struct imx_media_pixfmt *cc;
574	struct v4l2_mbus_framefmt *fmt;
575	int ret = 0;
576
577	if (sdformat->pad >= VDIC_NUM_PADS)
578		return -EINVAL;
579
580	mutex_lock(&priv->lock);
581
582	if (priv->stream_count > 0) {
583		ret = -EBUSY;
584		goto out;
585	}
586
587	vdic_try_fmt(priv, sd_state, sdformat, &cc);
588
589	fmt = __vdic_get_fmt(priv, sd_state, sdformat->pad, sdformat->which);
590	*fmt = sdformat->format;
591
592	/* propagate format to source pad */
593	if (sdformat->pad == VDIC_SINK_PAD_DIRECT ||
594	    sdformat->pad == VDIC_SINK_PAD_IDMAC) {
595		const struct imx_media_pixfmt *outcc;
596		struct v4l2_mbus_framefmt *outfmt;
597		struct v4l2_subdev_format format;
598
599		format.pad = VDIC_SRC_PAD_DIRECT;
600		format.which = sdformat->which;
601		format.format = sdformat->format;
602		vdic_try_fmt(priv, sd_state, &format, &outcc);
603
604		outfmt = __vdic_get_fmt(priv, sd_state, VDIC_SRC_PAD_DIRECT,
605					sdformat->which);
606		*outfmt = format.format;
607		if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
608			priv->cc[VDIC_SRC_PAD_DIRECT] = outcc;
609	}
610
611	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
612		priv->cc[sdformat->pad] = cc;
613out:
614	mutex_unlock(&priv->lock);
615	return ret;
616}
617
618static int vdic_link_setup(struct media_entity *entity,
619			    const struct media_pad *local,
620			    const struct media_pad *remote, u32 flags)
621{
622	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
623	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
624	struct v4l2_subdev *remote_sd;
625	int ret = 0;
626
627	dev_dbg(priv->ipu_dev, "%s: link setup %s -> %s",
628		sd->name, remote->entity->name, local->entity->name);
629
630	mutex_lock(&priv->lock);
631
632	if (local->flags & MEDIA_PAD_FL_SOURCE) {
633		if (!is_media_entity_v4l2_subdev(remote->entity)) {
634			ret = -EINVAL;
635			goto out;
636		}
637
638		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
639
640		if (flags & MEDIA_LNK_FL_ENABLED) {
641			if (priv->sink_sd) {
642				ret = -EBUSY;
643				goto out;
644			}
645			priv->sink_sd = remote_sd;
646		} else {
647			priv->sink_sd = NULL;
648		}
649
650		goto out;
651	}
652
653	/* this is a sink pad */
654
655	if (flags & MEDIA_LNK_FL_ENABLED) {
656		if (priv->src) {
657			ret = -EBUSY;
658			goto out;
659		}
660	} else {
661		priv->src = NULL;
662		goto out;
663	}
664
665	if (local->index == VDIC_SINK_PAD_IDMAC) {
666		struct imx_media_video_dev *vdev = priv->vdev;
667
668		if (!is_media_entity_v4l2_video_device(remote->entity)) {
669			ret = -EINVAL;
670			goto out;
671		}
672		if (!vdev) {
673			ret = -ENODEV;
674			goto out;
675		}
676
677		priv->csi_direct = false;
678	} else {
679		if (!is_media_entity_v4l2_subdev(remote->entity)) {
680			ret = -EINVAL;
681			goto out;
682		}
683
684		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
685
686		/* direct pad must connect to a CSI */
687		if (!(remote_sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) ||
688		    remote->index != CSI_SRC_PAD_DIRECT) {
689			ret = -EINVAL;
690			goto out;
691		}
692
693		priv->csi_direct = true;
694	}
695
696	priv->src = remote->entity;
697	/* record which input pad is now active */
698	priv->active_input_pad = local->index;
699out:
700	mutex_unlock(&priv->lock);
701	return ret;
702}
703
704static int vdic_link_validate(struct v4l2_subdev *sd,
705			      struct media_link *link,
706			      struct v4l2_subdev_format *source_fmt,
707			      struct v4l2_subdev_format *sink_fmt)
708{
709	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
710	int ret;
711
712	ret = v4l2_subdev_link_validate_default(sd, link,
713						source_fmt, sink_fmt);
714	if (ret)
715		return ret;
716
717	mutex_lock(&priv->lock);
718
719	if (priv->csi_direct && priv->motion != HIGH_MOTION) {
720		v4l2_err(&priv->sd,
721			 "direct CSI pipeline requires high motion\n");
722		ret = -EINVAL;
723	}
724
725	mutex_unlock(&priv->lock);
726	return ret;
727}
728
729static int vdic_get_frame_interval(struct v4l2_subdev *sd,
730				   struct v4l2_subdev_state *sd_state,
731				   struct v4l2_subdev_frame_interval *fi)
732{
733	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
734
735	/*
736	 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2
737	 * subdev active state API.
738	 */
739	if (fi->which != V4L2_SUBDEV_FORMAT_ACTIVE)
740		return -EINVAL;
741
742	if (fi->pad >= VDIC_NUM_PADS)
743		return -EINVAL;
744
745	mutex_lock(&priv->lock);
746
747	fi->interval = priv->frame_interval[fi->pad];
748
749	mutex_unlock(&priv->lock);
750
751	return 0;
752}
753
754static int vdic_set_frame_interval(struct v4l2_subdev *sd,
755				   struct v4l2_subdev_state *sd_state,
756				   struct v4l2_subdev_frame_interval *fi)
757{
758	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
759	struct v4l2_fract *input_fi, *output_fi;
760	int ret = 0;
761
762	/*
763	 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2
764	 * subdev active state API.
765	 */
766	if (fi->which != V4L2_SUBDEV_FORMAT_ACTIVE)
767		return -EINVAL;
768
769	mutex_lock(&priv->lock);
770
771	input_fi = &priv->frame_interval[priv->active_input_pad];
772	output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT];
773
774	switch (fi->pad) {
775	case VDIC_SINK_PAD_DIRECT:
776	case VDIC_SINK_PAD_IDMAC:
777		/* No limits on valid input frame intervals */
778		if (fi->interval.numerator == 0 ||
779		    fi->interval.denominator == 0)
780			fi->interval = priv->frame_interval[fi->pad];
781		/* Reset output interval */
782		*output_fi = fi->interval;
783		if (priv->csi_direct)
784			output_fi->denominator *= 2;
785		break;
786	case VDIC_SRC_PAD_DIRECT:
787		/*
788		 * frame rate at output pad is double input
789		 * rate when using direct CSI->VDIC pipeline.
790		 *
791		 * TODO: implement VDIC frame skipping
792		 */
793		fi->interval = *input_fi;
794		if (priv->csi_direct)
795			fi->interval.denominator *= 2;
796		break;
797	default:
798		ret = -EINVAL;
799		goto out;
800	}
801
802	priv->frame_interval[fi->pad] = fi->interval;
803out:
804	mutex_unlock(&priv->lock);
805	return ret;
806}
807
808static int vdic_registered(struct v4l2_subdev *sd)
809{
810	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
811	int i, ret;
812	u32 code;
813
814	for (i = 0; i < VDIC_NUM_PADS; i++) {
815		code = 0;
816		if (i != VDIC_SINK_PAD_IDMAC)
817			imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV);
818
819		/* set a default mbus format  */
820		ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
821					      IMX_MEDIA_DEF_PIX_WIDTH,
822					      IMX_MEDIA_DEF_PIX_HEIGHT, code,
823					      V4L2_FIELD_NONE, &priv->cc[i]);
824		if (ret)
825			return ret;
826
827		/* init default frame interval */
828		priv->frame_interval[i].numerator = 1;
829		priv->frame_interval[i].denominator = 30;
830		if (i == VDIC_SRC_PAD_DIRECT)
831			priv->frame_interval[i].denominator *= 2;
832	}
833
834	priv->active_input_pad = VDIC_SINK_PAD_DIRECT;
835
836	return vdic_init_controls(priv);
837}
838
839static void vdic_unregistered(struct v4l2_subdev *sd)
840{
841	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
842
843	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
844}
845
846static const struct v4l2_subdev_pad_ops vdic_pad_ops = {
847	.enum_mbus_code = vdic_enum_mbus_code,
848	.get_fmt = vdic_get_fmt,
849	.set_fmt = vdic_set_fmt,
850	.get_frame_interval = vdic_get_frame_interval,
851	.set_frame_interval = vdic_set_frame_interval,
852	.link_validate = vdic_link_validate,
853};
854
855static const struct v4l2_subdev_video_ops vdic_video_ops = {
856	.s_stream = vdic_s_stream,
857};
858
859static const struct media_entity_operations vdic_entity_ops = {
860	.link_setup = vdic_link_setup,
861	.link_validate = v4l2_subdev_link_validate,
862};
863
864static const struct v4l2_subdev_ops vdic_subdev_ops = {
865	.video = &vdic_video_ops,
866	.pad = &vdic_pad_ops,
867};
868
869static const struct v4l2_subdev_internal_ops vdic_internal_ops = {
870	.init_state = imx_media_init_state,
871	.registered = vdic_registered,
872	.unregistered = vdic_unregistered,
873};
874
875struct v4l2_subdev *imx_media_vdic_register(struct v4l2_device *v4l2_dev,
876					    struct device *ipu_dev,
877					    struct ipu_soc *ipu,
878					    u32 grp_id)
879{
880	struct vdic_priv *priv;
881	int i, ret;
882
883	priv = devm_kzalloc(ipu_dev, sizeof(*priv), GFP_KERNEL);
884	if (!priv)
885		return ERR_PTR(-ENOMEM);
886
887	priv->ipu_dev = ipu_dev;
888	priv->ipu = ipu;
889
890	v4l2_subdev_init(&priv->sd, &vdic_subdev_ops);
891	v4l2_set_subdevdata(&priv->sd, priv);
892	priv->sd.internal_ops = &vdic_internal_ops;
893	priv->sd.entity.ops = &vdic_entity_ops;
894	priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
895	priv->sd.owner = ipu_dev->driver->owner;
896	priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
897	priv->sd.grp_id = grp_id;
898	imx_media_grp_id_to_sd_name(priv->sd.name, sizeof(priv->sd.name),
899				    priv->sd.grp_id, ipu_get_num(ipu));
900
901	mutex_init(&priv->lock);
902
903	for (i = 0; i < VDIC_NUM_PADS; i++)
904		priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ?
905			MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
906
907	ret = media_entity_pads_init(&priv->sd.entity, VDIC_NUM_PADS,
908				     priv->pad);
909	if (ret)
910		goto free;
911
912	ret = v4l2_device_register_subdev(v4l2_dev, &priv->sd);
913	if (ret)
914		goto free;
915
916	return &priv->sd;
917free:
918	mutex_destroy(&priv->lock);
919	return ERR_PTR(ret);
920}
921
922int imx_media_vdic_unregister(struct v4l2_subdev *sd)
923{
924	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
925
926	v4l2_info(sd, "Removing\n");
927
928	v4l2_device_unregister_subdev(sd);
929	mutex_destroy(&priv->lock);
930	media_entity_cleanup(&sd->entity);
931
932	return 0;
933}