1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * camss-video.c 4 * 5 * Qualcomm MSM Camera Subsystem - V4L2 device node 6 * 7 * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved. 8 * Copyright (C) 2015-2018 Linaro Ltd. 9 */ 10 #include <linux/slab.h> 11 #include <media/media-entity.h> 12 #include <media/v4l2-dev.h> 13 #include <media/v4l2-device.h> 14 #include <media/v4l2-ioctl.h> 15 #include <media/v4l2-mc.h> 16 #include <media/videobuf2-dma-sg.h> 17 18 #include "camss-video.h" 19 #include "camss.h" 20 21 #define CAMSS_FRAME_MIN_WIDTH 1 22 #define CAMSS_FRAME_MAX_WIDTH 8191 23 #define CAMSS_FRAME_MIN_HEIGHT 1 24 #define CAMSS_FRAME_MAX_HEIGHT_RDI 8191 25 #define CAMSS_FRAME_MAX_HEIGHT_PIX 4096 26 27 /* ----------------------------------------------------------------------------- 28 * Helper functions 29 */ 30 31 /* 32 * video_mbus_to_pix_mp - Convert v4l2_mbus_framefmt to v4l2_pix_format_mplane 33 * @mbus: v4l2_mbus_framefmt format (input) 34 * @pix: v4l2_pix_format_mplane format (output) 35 * @f: a pointer to formats array element to be used for the conversion 36 * @alignment: bytesperline alignment value 37 * 38 * Fill the output pix structure with information from the input mbus format. 39 * 40 * Return 0 on success or a negative error code otherwise 41 */ 42 static int video_mbus_to_pix_mp(const struct v4l2_mbus_framefmt *mbus, 43 struct v4l2_pix_format_mplane *pix, 44 const struct camss_format_info *f, 45 unsigned int alignment) 46 { 47 unsigned int i; 48 u32 bytesperline; 49 50 memset(pix, 0, sizeof(*pix)); 51 v4l2_fill_pix_format_mplane(pix, mbus); 52 pix->pixelformat = f->pixelformat; 53 pix->num_planes = f->planes; 54 for (i = 0; i < pix->num_planes; i++) { 55 bytesperline = pix->width / f->hsub[i].numerator * 56 f->hsub[i].denominator * f->bpp[i] / 8; 57 bytesperline = ALIGN(bytesperline, alignment); 58 pix->plane_fmt[i].bytesperline = bytesperline; 59 pix->plane_fmt[i].sizeimage = pix->height / 60 f->vsub[i].numerator * f->vsub[i].denominator * 61 bytesperline; 62 } 63 64 return 0; 65 } 66 67 static struct v4l2_subdev *video_remote_subdev(struct camss_video *video, 68 u32 *pad) 69 { 70 struct media_pad *remote; 71 72 remote = media_pad_remote_pad_first(&video->pad); 73 74 if (!remote || !is_media_entity_v4l2_subdev(remote->entity)) 75 return NULL; 76 77 if (pad) 78 *pad = remote->index; 79 80 return media_entity_to_v4l2_subdev(remote->entity); 81 } 82 83 static int video_get_subdev_format(struct camss_video *video, 84 struct v4l2_format *format) 85 { 86 struct v4l2_subdev_format fmt = { 87 .which = V4L2_SUBDEV_FORMAT_ACTIVE, 88 }; 89 struct v4l2_subdev *subdev; 90 u32 pad; 91 int ret; 92 93 subdev = video_remote_subdev(video, &pad); 94 if (subdev == NULL) 95 return -EPIPE; 96 97 fmt.pad = pad; 98 99 ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt); 100 if (ret) 101 return ret; 102 103 ret = camss_format_find_format(fmt.format.code, format->fmt.pix_mp.pixelformat, 104 video->formats, video->nformats); 105 if (ret < 0) 106 return ret; 107 108 format->type = video->type; 109 110 return video_mbus_to_pix_mp(&fmt.format, &format->fmt.pix_mp, 111 &video->formats[ret], video->bpl_alignment); 112 } 113 114 /* ----------------------------------------------------------------------------- 115 * Video queue operations 116 */ 117 118 static int video_queue_setup(struct vb2_queue *q, 119 unsigned int *num_buffers, unsigned int *num_planes, 120 unsigned int sizes[], struct device *alloc_devs[]) 121 { 122 struct camss_video *video = vb2_get_drv_priv(q); 123 const struct v4l2_pix_format_mplane *format = 124 &video->active_fmt.fmt.pix_mp; 125 unsigned int i; 126 127 if (*num_planes) { 128 if (*num_planes != format->num_planes) 129 return -EINVAL; 130 131 for (i = 0; i < *num_planes; i++) 132 if (sizes[i] < format->plane_fmt[i].sizeimage) 133 return -EINVAL; 134 135 return 0; 136 } 137 138 *num_planes = format->num_planes; 139 140 for (i = 0; i < *num_planes; i++) 141 sizes[i] = format->plane_fmt[i].sizeimage; 142 143 return 0; 144 } 145 146 static int video_buf_init(struct vb2_buffer *vb) 147 { 148 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 149 struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue); 150 struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer, 151 vb); 152 const struct v4l2_pix_format_mplane *format = 153 &video->active_fmt.fmt.pix_mp; 154 struct sg_table *sgt; 155 unsigned int i; 156 157 for (i = 0; i < format->num_planes; i++) { 158 sgt = vb2_dma_sg_plane_desc(vb, i); 159 if (!sgt) 160 return -EFAULT; 161 162 buffer->addr[i] = sg_dma_address(sgt->sgl); 163 } 164 165 if (format->pixelformat == V4L2_PIX_FMT_NV12 || 166 format->pixelformat == V4L2_PIX_FMT_NV21 || 167 format->pixelformat == V4L2_PIX_FMT_NV16 || 168 format->pixelformat == V4L2_PIX_FMT_NV61) 169 buffer->addr[1] = buffer->addr[0] + 170 format->plane_fmt[0].bytesperline * 171 format->height; 172 173 return 0; 174 } 175 176 static int video_buf_prepare(struct vb2_buffer *vb) 177 { 178 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 179 struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue); 180 const struct v4l2_pix_format_mplane *format = 181 &video->active_fmt.fmt.pix_mp; 182 unsigned int i; 183 184 for (i = 0; i < format->num_planes; i++) { 185 if (format->plane_fmt[i].sizeimage > vb2_plane_size(vb, i)) 186 return -EINVAL; 187 188 vb2_set_plane_payload(vb, i, format->plane_fmt[i].sizeimage); 189 } 190 191 vbuf->field = V4L2_FIELD_NONE; 192 193 return 0; 194 } 195 196 static void video_buf_queue(struct vb2_buffer *vb) 197 { 198 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 199 struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue); 200 struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer, 201 vb); 202 203 video->ops->queue_buffer(video, buffer); 204 } 205 206 static int video_check_format(struct camss_video *video) 207 { 208 struct v4l2_pix_format_mplane *pix = &video->active_fmt.fmt.pix_mp; 209 struct v4l2_format format; 210 struct v4l2_pix_format_mplane *sd_pix = &format.fmt.pix_mp; 211 int ret; 212 213 sd_pix->pixelformat = pix->pixelformat; 214 ret = video_get_subdev_format(video, &format); 215 if (ret < 0) 216 return ret; 217 218 if (pix->pixelformat != sd_pix->pixelformat || 219 pix->height != sd_pix->height || 220 pix->width != sd_pix->width || 221 pix->num_planes != sd_pix->num_planes || 222 pix->field != format.fmt.pix_mp.field) 223 return -EPIPE; 224 225 return 0; 226 } 227 228 static int video_start_streaming(struct vb2_queue *q, unsigned int count) 229 { 230 struct camss_video *video = vb2_get_drv_priv(q); 231 struct video_device *vdev = &video->vdev; 232 struct media_entity *entity; 233 struct media_pad *pad; 234 struct v4l2_subdev *subdev; 235 int ret; 236 237 ret = video_device_pipeline_alloc_start(vdev); 238 if (ret < 0) { 239 dev_err(video->camss->dev, "Failed to start media pipeline: %d\n", ret); 240 goto flush_buffers; 241 } 242 243 ret = video_check_format(video); 244 if (ret < 0) 245 goto error; 246 247 entity = &vdev->entity; 248 while (1) { 249 pad = &entity->pads[0]; 250 if (!(pad->flags & MEDIA_PAD_FL_SINK)) 251 break; 252 253 pad = media_pad_remote_pad_first(pad); 254 if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) 255 break; 256 257 entity = pad->entity; 258 subdev = media_entity_to_v4l2_subdev(entity); 259 260 ret = v4l2_subdev_call(subdev, video, s_stream, 1); 261 if (ret < 0 && ret != -ENOIOCTLCMD) 262 goto error; 263 } 264 265 return 0; 266 267 error: 268 video_device_pipeline_stop(vdev); 269 270 flush_buffers: 271 video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED); 272 273 return ret; 274 } 275 276 static void video_stop_streaming(struct vb2_queue *q) 277 { 278 struct camss_video *video = vb2_get_drv_priv(q); 279 struct video_device *vdev = &video->vdev; 280 struct media_entity *entity; 281 struct media_pad *pad; 282 struct v4l2_subdev *subdev; 283 int ret; 284 285 entity = &vdev->entity; 286 while (1) { 287 pad = &entity->pads[0]; 288 if (!(pad->flags & MEDIA_PAD_FL_SINK)) 289 break; 290 291 pad = media_pad_remote_pad_first(pad); 292 if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) 293 break; 294 295 entity = pad->entity; 296 subdev = media_entity_to_v4l2_subdev(entity); 297 298 ret = v4l2_subdev_call(subdev, video, s_stream, 0); 299 300 if (ret) { 301 dev_err(video->camss->dev, "Video pipeline stop failed: %d\n", ret); 302 return; 303 } 304 } 305 306 video_device_pipeline_stop(vdev); 307 308 video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR); 309 } 310 311 static const struct vb2_ops msm_video_vb2_q_ops = { 312 .queue_setup = video_queue_setup, 313 .wait_prepare = vb2_ops_wait_prepare, 314 .wait_finish = vb2_ops_wait_finish, 315 .buf_init = video_buf_init, 316 .buf_prepare = video_buf_prepare, 317 .buf_queue = video_buf_queue, 318 .start_streaming = video_start_streaming, 319 .stop_streaming = video_stop_streaming, 320 }; 321 322 /* ----------------------------------------------------------------------------- 323 * V4L2 ioctls 324 */ 325 326 static int video_querycap(struct file *file, void *fh, 327 struct v4l2_capability *cap) 328 { 329 strscpy(cap->driver, "qcom-camss", sizeof(cap->driver)); 330 strscpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card)); 331 332 return 0; 333 } 334 335 static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f) 336 { 337 struct camss_video *video = video_drvdata(file); 338 int i, j, k; 339 u32 mcode = f->mbus_code; 340 341 if (f->type != video->type) 342 return -EINVAL; 343 344 if (f->index >= video->nformats) 345 return -EINVAL; 346 347 /* 348 * Find index "i" of "k"th unique pixelformat in formats array. 349 * 350 * If f->mbus_code passed to video_enum_fmt() is not zero, a device 351 * with V4L2_CAP_IO_MC capability restricts enumeration to only the 352 * pixel formats that can be produced from that media bus code. 353 * This is implemented by skipping video->formats[] entries with 354 * code != f->mbus_code (if f->mbus_code is not zero). 355 * If the f->mbus_code passed to video_enum_fmt() is not supported, 356 * -EINVAL is returned. 357 * If f->mbus_code is zero, all the pixel formats are enumerated. 358 */ 359 k = -1; 360 for (i = 0; i < video->nformats; i++) { 361 if (mcode != 0 && video->formats[i].code != mcode) 362 continue; 363 364 for (j = 0; j < i; j++) { 365 if (mcode != 0 && video->formats[j].code != mcode) 366 continue; 367 if (video->formats[i].pixelformat == 368 video->formats[j].pixelformat) 369 break; 370 } 371 372 if (j == i) 373 k++; 374 375 if (k == f->index) 376 break; 377 } 378 379 if (k == -1 || k < f->index) 380 /* 381 * All the unique pixel formats matching the arguments 382 * have been enumerated (k >= 0 and f->index > 0), or 383 * no pixel formats match the non-zero f->mbus_code (k == -1). 384 */ 385 return -EINVAL; 386 387 f->pixelformat = video->formats[i].pixelformat; 388 389 return 0; 390 } 391 392 static int video_enum_framesizes(struct file *file, void *fh, 393 struct v4l2_frmsizeenum *fsize) 394 { 395 struct camss_video *video = video_drvdata(file); 396 int i; 397 398 if (fsize->index) 399 return -EINVAL; 400 401 /* Only accept pixel format present in the formats[] table */ 402 for (i = 0; i < video->nformats; i++) { 403 if (video->formats[i].pixelformat == fsize->pixel_format) 404 break; 405 } 406 407 if (i == video->nformats) 408 return -EINVAL; 409 410 fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS; 411 fsize->stepwise.min_width = CAMSS_FRAME_MIN_WIDTH; 412 fsize->stepwise.max_width = CAMSS_FRAME_MAX_WIDTH; 413 fsize->stepwise.min_height = CAMSS_FRAME_MIN_HEIGHT; 414 fsize->stepwise.max_height = (video->line_based) ? 415 CAMSS_FRAME_MAX_HEIGHT_PIX : CAMSS_FRAME_MAX_HEIGHT_RDI; 416 fsize->stepwise.step_width = 1; 417 fsize->stepwise.step_height = 1; 418 419 return 0; 420 } 421 422 static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f) 423 { 424 struct camss_video *video = video_drvdata(file); 425 426 *f = video->active_fmt; 427 428 return 0; 429 } 430 431 static int __video_try_fmt(struct camss_video *video, struct v4l2_format *f) 432 { 433 struct v4l2_pix_format_mplane *pix_mp; 434 const struct camss_format_info *fi; 435 struct v4l2_plane_pix_format *p; 436 u32 bytesperline[3] = { 0 }; 437 u32 sizeimage[3] = { 0 }; 438 u32 width, height; 439 u32 bpl, lines; 440 int i, j; 441 442 pix_mp = &f->fmt.pix_mp; 443 444 if (video->line_based) 445 for (i = 0; i < pix_mp->num_planes && i < 3; i++) { 446 p = &pix_mp->plane_fmt[i]; 447 bytesperline[i] = clamp_t(u32, p->bytesperline, 448 1, 65528); 449 sizeimage[i] = clamp_t(u32, p->sizeimage, 450 bytesperline[i], 451 bytesperline[i] * CAMSS_FRAME_MAX_HEIGHT_PIX); 452 } 453 454 for (j = 0; j < video->nformats; j++) 455 if (pix_mp->pixelformat == video->formats[j].pixelformat) 456 break; 457 458 if (j == video->nformats) 459 j = 0; /* default format */ 460 461 fi = &video->formats[j]; 462 width = pix_mp->width; 463 height = pix_mp->height; 464 465 memset(pix_mp, 0, sizeof(*pix_mp)); 466 467 pix_mp->pixelformat = fi->pixelformat; 468 pix_mp->width = clamp_t(u32, width, 1, CAMSS_FRAME_MAX_WIDTH); 469 pix_mp->height = clamp_t(u32, height, 1, CAMSS_FRAME_MAX_HEIGHT_RDI); 470 pix_mp->num_planes = fi->planes; 471 for (i = 0; i < pix_mp->num_planes; i++) { 472 bpl = pix_mp->width / fi->hsub[i].numerator * 473 fi->hsub[i].denominator * fi->bpp[i] / 8; 474 bpl = ALIGN(bpl, video->bpl_alignment); 475 pix_mp->plane_fmt[i].bytesperline = bpl; 476 pix_mp->plane_fmt[i].sizeimage = pix_mp->height / 477 fi->vsub[i].numerator * fi->vsub[i].denominator * bpl; 478 } 479 480 pix_mp->field = V4L2_FIELD_NONE; 481 pix_mp->colorspace = V4L2_COLORSPACE_SRGB; 482 pix_mp->flags = 0; 483 pix_mp->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix_mp->colorspace); 484 pix_mp->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true, 485 pix_mp->colorspace, pix_mp->ycbcr_enc); 486 pix_mp->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix_mp->colorspace); 487 488 if (video->line_based) 489 for (i = 0; i < pix_mp->num_planes; i++) { 490 p = &pix_mp->plane_fmt[i]; 491 p->bytesperline = clamp_t(u32, p->bytesperline, 492 1, 65528); 493 p->sizeimage = clamp_t(u32, p->sizeimage, 494 p->bytesperline, 495 p->bytesperline * CAMSS_FRAME_MAX_HEIGHT_PIX); 496 lines = p->sizeimage / p->bytesperline; 497 498 if (p->bytesperline < bytesperline[i]) 499 p->bytesperline = ALIGN(bytesperline[i], 8); 500 501 if (p->sizeimage < p->bytesperline * lines) 502 p->sizeimage = p->bytesperline * lines; 503 504 if (p->sizeimage < sizeimage[i]) 505 p->sizeimage = sizeimage[i]; 506 } 507 508 return 0; 509 } 510 511 static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f) 512 { 513 struct camss_video *video = video_drvdata(file); 514 515 return __video_try_fmt(video, f); 516 } 517 518 static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f) 519 { 520 struct camss_video *video = video_drvdata(file); 521 int ret; 522 523 if (vb2_is_busy(&video->vb2_q)) 524 return -EBUSY; 525 526 ret = __video_try_fmt(video, f); 527 if (ret < 0) 528 return ret; 529 530 video->active_fmt = *f; 531 532 return 0; 533 } 534 535 static int video_enum_input(struct file *file, void *fh, 536 struct v4l2_input *input) 537 { 538 if (input->index > 0) 539 return -EINVAL; 540 541 strscpy(input->name, "camera", sizeof(input->name)); 542 input->type = V4L2_INPUT_TYPE_CAMERA; 543 544 return 0; 545 } 546 547 static int video_g_input(struct file *file, void *fh, unsigned int *input) 548 { 549 *input = 0; 550 551 return 0; 552 } 553 554 static int video_s_input(struct file *file, void *fh, unsigned int input) 555 { 556 return input == 0 ? 0 : -EINVAL; 557 } 558 559 static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = { 560 .vidioc_querycap = video_querycap, 561 .vidioc_enum_fmt_vid_cap = video_enum_fmt, 562 .vidioc_enum_framesizes = video_enum_framesizes, 563 .vidioc_g_fmt_vid_cap_mplane = video_g_fmt, 564 .vidioc_s_fmt_vid_cap_mplane = video_s_fmt, 565 .vidioc_try_fmt_vid_cap_mplane = video_try_fmt, 566 .vidioc_reqbufs = vb2_ioctl_reqbufs, 567 .vidioc_querybuf = vb2_ioctl_querybuf, 568 .vidioc_qbuf = vb2_ioctl_qbuf, 569 .vidioc_expbuf = vb2_ioctl_expbuf, 570 .vidioc_dqbuf = vb2_ioctl_dqbuf, 571 .vidioc_create_bufs = vb2_ioctl_create_bufs, 572 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 573 .vidioc_streamon = vb2_ioctl_streamon, 574 .vidioc_streamoff = vb2_ioctl_streamoff, 575 .vidioc_enum_input = video_enum_input, 576 .vidioc_g_input = video_g_input, 577 .vidioc_s_input = video_s_input, 578 }; 579 580 /* ----------------------------------------------------------------------------- 581 * V4L2 file operations 582 */ 583 584 static int video_open(struct file *file) 585 { 586 struct video_device *vdev = video_devdata(file); 587 struct camss_video *video = video_drvdata(file); 588 struct v4l2_fh *vfh; 589 int ret; 590 591 mutex_lock(&video->lock); 592 593 vfh = kzalloc(sizeof(*vfh), GFP_KERNEL); 594 if (vfh == NULL) { 595 ret = -ENOMEM; 596 goto error_alloc; 597 } 598 599 v4l2_fh_init(vfh, vdev); 600 v4l2_fh_add(vfh); 601 602 file->private_data = vfh; 603 604 ret = v4l2_pipeline_pm_get(&vdev->entity); 605 if (ret < 0) { 606 dev_err(video->camss->dev, "Failed to power up pipeline: %d\n", 607 ret); 608 goto error_pm_use; 609 } 610 611 mutex_unlock(&video->lock); 612 613 return 0; 614 615 error_pm_use: 616 v4l2_fh_release(file); 617 618 error_alloc: 619 mutex_unlock(&video->lock); 620 621 return ret; 622 } 623 624 static int video_release(struct file *file) 625 { 626 struct video_device *vdev = video_devdata(file); 627 628 vb2_fop_release(file); 629 630 v4l2_pipeline_pm_put(&vdev->entity); 631 632 file->private_data = NULL; 633 634 return 0; 635 } 636 637 static const struct v4l2_file_operations msm_vid_fops = { 638 .owner = THIS_MODULE, 639 .unlocked_ioctl = video_ioctl2, 640 .open = video_open, 641 .release = video_release, 642 .poll = vb2_fop_poll, 643 .mmap = vb2_fop_mmap, 644 .read = vb2_fop_read, 645 }; 646 647 /* ----------------------------------------------------------------------------- 648 * CAMSS video core 649 */ 650 651 static void msm_video_release(struct video_device *vdev) 652 { 653 struct camss_video *video = video_get_drvdata(vdev); 654 655 media_entity_cleanup(&vdev->entity); 656 657 mutex_destroy(&video->q_lock); 658 mutex_destroy(&video->lock); 659 660 if (atomic_dec_and_test(&video->camss->ref_count)) 661 camss_delete(video->camss); 662 } 663 664 /* 665 * msm_video_init_format - Helper function to initialize format 666 * @video: struct camss_video 667 * 668 * Initialize pad format with default value. 669 * 670 * Return 0 on success or a negative error code otherwise 671 */ 672 static int msm_video_init_format(struct camss_video *video) 673 { 674 int ret; 675 struct v4l2_format format = { 676 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 677 .fmt.pix_mp = { 678 .width = 1920, 679 .height = 1080, 680 .pixelformat = video->formats[0].pixelformat, 681 }, 682 }; 683 684 ret = __video_try_fmt(video, &format); 685 if (ret < 0) 686 return ret; 687 688 video->active_fmt = format; 689 690 return 0; 691 } 692 693 /* 694 * msm_video_register - Register a video device node 695 * @video: struct camss_video 696 * @v4l2_dev: V4L2 device 697 * @name: name to be used for the video device node 698 * 699 * Initialize and register a video device node to a V4L2 device. Also 700 * initialize the vb2 queue. 701 * 702 * Return 0 on success or a negative error code otherwise 703 */ 704 705 int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev, 706 const char *name) 707 { 708 struct media_pad *pad = &video->pad; 709 struct video_device *vdev; 710 struct vb2_queue *q; 711 int ret; 712 713 vdev = &video->vdev; 714 715 mutex_init(&video->q_lock); 716 717 q = &video->vb2_q; 718 q->drv_priv = video; 719 q->mem_ops = &vb2_dma_sg_memops; 720 q->ops = &msm_video_vb2_q_ops; 721 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; 722 q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ; 723 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 724 q->buf_struct_size = sizeof(struct camss_buffer); 725 q->dev = video->camss->dev; 726 q->lock = &video->q_lock; 727 ret = vb2_queue_init(q); 728 if (ret < 0) { 729 dev_err(v4l2_dev->dev, "Failed to init vb2 queue: %d\n", ret); 730 goto error_vb2_init; 731 } 732 733 pad->flags = MEDIA_PAD_FL_SINK; 734 ret = media_entity_pads_init(&vdev->entity, 1, pad); 735 if (ret < 0) { 736 dev_err(v4l2_dev->dev, "Failed to init video entity: %d\n", 737 ret); 738 goto error_vb2_init; 739 } 740 741 mutex_init(&video->lock); 742 743 ret = msm_video_init_format(video); 744 if (ret < 0) { 745 dev_err(v4l2_dev->dev, "Failed to init format: %d\n", ret); 746 goto error_video_register; 747 } 748 749 vdev->fops = &msm_vid_fops; 750 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING 751 | V4L2_CAP_READWRITE | V4L2_CAP_IO_MC; 752 vdev->ioctl_ops = &msm_vid_ioctl_ops; 753 vdev->release = msm_video_release; 754 vdev->v4l2_dev = v4l2_dev; 755 vdev->vfl_dir = VFL_DIR_RX; 756 vdev->queue = &video->vb2_q; 757 vdev->lock = &video->lock; 758 strscpy(vdev->name, name, sizeof(vdev->name)); 759 760 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 761 if (ret < 0) { 762 dev_err(v4l2_dev->dev, "Failed to register video device: %d\n", 763 ret); 764 goto error_video_register; 765 } 766 767 video_set_drvdata(vdev, video); 768 atomic_inc(&video->camss->ref_count); 769 770 return 0; 771 772 error_video_register: 773 media_entity_cleanup(&vdev->entity); 774 mutex_destroy(&video->lock); 775 error_vb2_init: 776 mutex_destroy(&video->q_lock); 777 778 return ret; 779 } 780 781 void msm_video_unregister(struct camss_video *video) 782 { 783 atomic_inc(&video->camss->ref_count); 784 vb2_video_unregister_device(&video->vdev); 785 atomic_dec(&video->camss->ref_count); 786 } 787