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