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
setup_vdi_channel(struct vdic_priv * priv,struct ipuv3_channel * channel,dma_addr_t phys0,dma_addr_t phys1)183 static 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
vdic_setup_direct(struct vdic_priv * priv)217 static 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
vdic_start_direct(struct vdic_priv * priv)226 static void vdic_start_direct(struct vdic_priv *priv)
227 {
228 }
229
vdic_stop_direct(struct vdic_priv * priv)230 static void vdic_stop_direct(struct vdic_priv *priv)
231 {
232 }
233
vdic_disable_direct(struct vdic_priv * priv)234 static 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
vdic_setup_indirect(struct vdic_priv * priv)240 static 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
vdic_start_indirect(struct vdic_priv * priv)271 static 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
vdic_stop_indirect(struct vdic_priv * priv)279 static 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
vdic_disable_indirect(struct vdic_priv * priv)287 static void vdic_disable_indirect(struct vdic_priv *priv)
288 {
289 }
290
291 static 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
298 static 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
vdic_start(struct vdic_priv * priv)305 static 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
340 out_put_ipu:
341 vdic_put_ipu_resources(priv);
342 return ret;
343 }
344
vdic_stop(struct vdic_priv * priv)345 static 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
vdic_s_ctrl(struct v4l2_ctrl * ctrl)358 static 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
384 out:
385 mutex_unlock(&priv->lock);
386 return ret;
387 }
388
389 static const struct v4l2_ctrl_ops vdic_ctrl_ops = {
390 .s_ctrl = vdic_s_ctrl,
391 };
392
393 static const char * const vdic_ctrl_motion_menu[] = {
394 "No Motion Compensation",
395 "Low Motion",
396 "Medium Motion",
397 "High Motion",
398 };
399
vdic_init_controls(struct vdic_priv * priv)400 static 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
422 out_free:
423 v4l2_ctrl_handler_free(hdlr);
424 return ret;
425 }
426
vdic_s_stream(struct v4l2_subdev * sd,int enable)427 static 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
471 update_count:
472 priv->stream_count += enable ? 1 : -1;
473 if (priv->stream_count < 0)
474 priv->stream_count = 0;
475 out:
476 mutex_unlock(&priv->lock);
477 return ret;
478 }
479
480 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)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
vdic_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)490 static 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
vdic_get_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sdformat)501 static 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;
521 out:
522 mutex_unlock(&priv->lock);
523 return ret;
524 }
525
vdic_try_fmt(struct vdic_priv * priv,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sdformat,const struct imx_media_pixfmt ** cc)526 static 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
vdic_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sdformat)568 static 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;
613 out:
614 mutex_unlock(&priv->lock);
615 return ret;
616 }
617
vdic_link_setup(struct media_entity * entity,const struct media_pad * local,const struct media_pad * remote,u32 flags)618 static 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;
699 out:
700 mutex_unlock(&priv->lock);
701 return ret;
702 }
703
vdic_link_validate(struct v4l2_subdev * sd,struct media_link * link,struct v4l2_subdev_format * source_fmt,struct v4l2_subdev_format * sink_fmt)704 static 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
vdic_get_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_interval * fi)729 static 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
vdic_set_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_interval * fi)754 static 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;
803 out:
804 mutex_unlock(&priv->lock);
805 return ret;
806 }
807
vdic_registered(struct v4l2_subdev * sd)808 static 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
vdic_unregistered(struct v4l2_subdev * sd)839 static 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
846 static 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
855 static const struct v4l2_subdev_video_ops vdic_video_ops = {
856 .s_stream = vdic_s_stream,
857 };
858
859 static const struct media_entity_operations vdic_entity_ops = {
860 .link_setup = vdic_link_setup,
861 .link_validate = v4l2_subdev_link_validate,
862 };
863
864 static const struct v4l2_subdev_ops vdic_subdev_ops = {
865 .video = &vdic_video_ops,
866 .pad = &vdic_pad_ops,
867 };
868
869 static 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
imx_media_vdic_register(struct v4l2_device * v4l2_dev,struct device * ipu_dev,struct ipu_soc * ipu,u32 grp_id)875 struct 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;
917 free:
918 mutex_destroy(&priv->lock);
919 return ERR_PTR(ret);
920 }
921
imx_media_vdic_unregister(struct v4l2_subdev * sd)922 int 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 }
934