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 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 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 */ 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 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 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 280 static void vdic_start_direct(struct vdic_priv *priv) 281 { 282 } 283 284 static void vdic_stop_direct(struct vdic_priv *priv) 285 { 286 } 287 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 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 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 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 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 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 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 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 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 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 * 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_state_get_format(sd_state, pad); 540 else 541 return &priv->format_mbus[pad]; 542 } 543 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 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 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 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 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 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 783 static int vdic_get_frame_interval(struct v4l2_subdev *sd, 784 struct v4l2_subdev_state *sd_state, 785 struct v4l2_subdev_frame_interval *fi) 786 { 787 struct vdic_priv *priv = v4l2_get_subdevdata(sd); 788 789 /* 790 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2 791 * subdev active state API. 792 */ 793 if (fi->which != V4L2_SUBDEV_FORMAT_ACTIVE) 794 return -EINVAL; 795 796 if (fi->pad >= VDIC_NUM_PADS) 797 return -EINVAL; 798 799 mutex_lock(&priv->lock); 800 801 fi->interval = priv->frame_interval[fi->pad]; 802 803 mutex_unlock(&priv->lock); 804 805 return 0; 806 } 807 808 static int vdic_set_frame_interval(struct v4l2_subdev *sd, 809 struct v4l2_subdev_state *sd_state, 810 struct v4l2_subdev_frame_interval *fi) 811 { 812 struct vdic_priv *priv = v4l2_get_subdevdata(sd); 813 struct v4l2_fract *input_fi, *output_fi; 814 int ret = 0; 815 816 /* 817 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2 818 * subdev active state API. 819 */ 820 if (fi->which != V4L2_SUBDEV_FORMAT_ACTIVE) 821 return -EINVAL; 822 823 mutex_lock(&priv->lock); 824 825 input_fi = &priv->frame_interval[priv->active_input_pad]; 826 output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT]; 827 828 switch (fi->pad) { 829 case VDIC_SINK_PAD_DIRECT: 830 case VDIC_SINK_PAD_IDMAC: 831 /* No limits on valid input frame intervals */ 832 if (fi->interval.numerator == 0 || 833 fi->interval.denominator == 0) 834 fi->interval = priv->frame_interval[fi->pad]; 835 /* Reset output interval */ 836 *output_fi = fi->interval; 837 if (priv->csi_direct) 838 output_fi->denominator *= 2; 839 break; 840 case VDIC_SRC_PAD_DIRECT: 841 /* 842 * frame rate at output pad is double input 843 * rate when using direct CSI->VDIC pipeline. 844 * 845 * TODO: implement VDIC frame skipping 846 */ 847 fi->interval = *input_fi; 848 if (priv->csi_direct) 849 fi->interval.denominator *= 2; 850 break; 851 default: 852 ret = -EINVAL; 853 goto out; 854 } 855 856 priv->frame_interval[fi->pad] = fi->interval; 857 out: 858 mutex_unlock(&priv->lock); 859 return ret; 860 } 861 862 static int vdic_registered(struct v4l2_subdev *sd) 863 { 864 struct vdic_priv *priv = v4l2_get_subdevdata(sd); 865 int i, ret; 866 u32 code; 867 868 for (i = 0; i < VDIC_NUM_PADS; i++) { 869 code = 0; 870 if (i != VDIC_SINK_PAD_IDMAC) 871 imx_media_enum_ipu_formats(&code, 0, PIXFMT_SEL_YUV); 872 873 /* set a default mbus format */ 874 ret = imx_media_init_mbus_fmt(&priv->format_mbus[i], 875 IMX_MEDIA_DEF_PIX_WIDTH, 876 IMX_MEDIA_DEF_PIX_HEIGHT, code, 877 V4L2_FIELD_NONE, &priv->cc[i]); 878 if (ret) 879 return ret; 880 881 /* init default frame interval */ 882 priv->frame_interval[i].numerator = 1; 883 priv->frame_interval[i].denominator = 30; 884 if (i == VDIC_SRC_PAD_DIRECT) 885 priv->frame_interval[i].denominator *= 2; 886 } 887 888 priv->active_input_pad = VDIC_SINK_PAD_DIRECT; 889 890 return vdic_init_controls(priv); 891 } 892 893 static void vdic_unregistered(struct v4l2_subdev *sd) 894 { 895 struct vdic_priv *priv = v4l2_get_subdevdata(sd); 896 897 v4l2_ctrl_handler_free(&priv->ctrl_hdlr); 898 } 899 900 static const struct v4l2_subdev_pad_ops vdic_pad_ops = { 901 .enum_mbus_code = vdic_enum_mbus_code, 902 .get_fmt = vdic_get_fmt, 903 .set_fmt = vdic_set_fmt, 904 .get_frame_interval = vdic_get_frame_interval, 905 .set_frame_interval = vdic_set_frame_interval, 906 .link_validate = vdic_link_validate, 907 }; 908 909 static const struct v4l2_subdev_video_ops vdic_video_ops = { 910 .s_stream = vdic_s_stream, 911 }; 912 913 static const struct media_entity_operations vdic_entity_ops = { 914 .link_setup = vdic_link_setup, 915 .link_validate = v4l2_subdev_link_validate, 916 }; 917 918 static const struct v4l2_subdev_ops vdic_subdev_ops = { 919 .video = &vdic_video_ops, 920 .pad = &vdic_pad_ops, 921 }; 922 923 static const struct v4l2_subdev_internal_ops vdic_internal_ops = { 924 .init_state = imx_media_init_state, 925 .registered = vdic_registered, 926 .unregistered = vdic_unregistered, 927 }; 928 929 struct v4l2_subdev *imx_media_vdic_register(struct v4l2_device *v4l2_dev, 930 struct device *ipu_dev, 931 struct ipu_soc *ipu, 932 u32 grp_id) 933 { 934 struct vdic_priv *priv; 935 int i, ret; 936 937 priv = devm_kzalloc(ipu_dev, sizeof(*priv), GFP_KERNEL); 938 if (!priv) 939 return ERR_PTR(-ENOMEM); 940 941 priv->ipu_dev = ipu_dev; 942 priv->ipu = ipu; 943 944 v4l2_subdev_init(&priv->sd, &vdic_subdev_ops); 945 v4l2_set_subdevdata(&priv->sd, priv); 946 priv->sd.internal_ops = &vdic_internal_ops; 947 priv->sd.entity.ops = &vdic_entity_ops; 948 priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; 949 priv->sd.owner = ipu_dev->driver->owner; 950 priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE; 951 priv->sd.grp_id = grp_id; 952 imx_media_grp_id_to_sd_name(priv->sd.name, sizeof(priv->sd.name), 953 priv->sd.grp_id, ipu_get_num(ipu)); 954 955 mutex_init(&priv->lock); 956 957 for (i = 0; i < VDIC_NUM_PADS; i++) 958 priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ? 959 MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK; 960 961 ret = media_entity_pads_init(&priv->sd.entity, VDIC_NUM_PADS, 962 priv->pad); 963 if (ret) 964 goto free; 965 966 ret = v4l2_device_register_subdev(v4l2_dev, &priv->sd); 967 if (ret) 968 goto free; 969 970 return &priv->sd; 971 free: 972 mutex_destroy(&priv->lock); 973 return ERR_PTR(ret); 974 } 975 976 int imx_media_vdic_unregister(struct v4l2_subdev *sd) 977 { 978 struct vdic_priv *priv = v4l2_get_subdevdata(sd); 979 980 v4l2_info(sd, "Removing\n"); 981 982 v4l2_device_unregister_subdev(sd); 983 mutex_destroy(&priv->lock); 984 media_entity_cleanup(&sd->entity); 985 986 return 0; 987 } 988