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_prepare_streaming(struct vb2_queue *q) 229 { 230 struct camss_video *video = vb2_get_drv_priv(q); 231 struct video_device *vdev = &video->vdev; 232 int ret; 233 234 ret = v4l2_pipeline_pm_get(&vdev->entity); 235 if (ret < 0) { 236 dev_err(video->camss->dev, "Failed to power up pipeline: %d\n", 237 ret); 238 } 239 240 return ret; 241 } 242 243 static int video_start_streaming(struct vb2_queue *q, unsigned int count) 244 { 245 struct camss_video *video = vb2_get_drv_priv(q); 246 struct video_device *vdev = &video->vdev; 247 struct media_entity *entity; 248 struct media_pad *pad; 249 struct v4l2_subdev *subdev; 250 int ret; 251 252 ret = video_device_pipeline_alloc_start(vdev); 253 if (ret < 0) { 254 dev_err(video->camss->dev, "Failed to start media pipeline: %d\n", ret); 255 goto flush_buffers; 256 } 257 258 ret = video_check_format(video); 259 if (ret < 0) 260 goto error; 261 262 entity = &vdev->entity; 263 while (1) { 264 pad = &entity->pads[0]; 265 if (!(pad->flags & MEDIA_PAD_FL_SINK)) 266 break; 267 268 pad = media_pad_remote_pad_first(pad); 269 if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) 270 break; 271 272 entity = pad->entity; 273 subdev = media_entity_to_v4l2_subdev(entity); 274 275 ret = v4l2_subdev_call(subdev, video, s_stream, 1); 276 if (ret < 0 && ret != -ENOIOCTLCMD) 277 goto error; 278 } 279 280 return 0; 281 282 error: 283 video_device_pipeline_stop(vdev); 284 285 flush_buffers: 286 video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED); 287 288 return ret; 289 } 290 291 static void video_stop_streaming(struct vb2_queue *q) 292 { 293 struct camss_video *video = vb2_get_drv_priv(q); 294 struct video_device *vdev = &video->vdev; 295 struct media_entity *entity; 296 struct media_pad *pad; 297 struct v4l2_subdev *subdev; 298 int ret; 299 300 entity = &vdev->entity; 301 while (1) { 302 pad = &entity->pads[0]; 303 if (!(pad->flags & MEDIA_PAD_FL_SINK)) 304 break; 305 306 pad = media_pad_remote_pad_first(pad); 307 if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) 308 break; 309 310 entity = pad->entity; 311 subdev = media_entity_to_v4l2_subdev(entity); 312 313 ret = v4l2_subdev_call(subdev, video, s_stream, 0); 314 315 if (ret) { 316 dev_err(video->camss->dev, "Video pipeline stop failed: %d\n", ret); 317 return; 318 } 319 } 320 321 video_device_pipeline_stop(vdev); 322 323 video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR); 324 } 325 326 static void video_unprepare_streaming(struct vb2_queue *q) 327 { 328 struct camss_video *video = vb2_get_drv_priv(q); 329 struct video_device *vdev = &video->vdev; 330 331 v4l2_pipeline_pm_put(&vdev->entity); 332 } 333 334 static const struct vb2_ops msm_video_vb2_q_ops = { 335 .queue_setup = video_queue_setup, 336 .buf_init = video_buf_init, 337 .buf_prepare = video_buf_prepare, 338 .buf_queue = video_buf_queue, 339 .prepare_streaming = video_prepare_streaming, 340 .start_streaming = video_start_streaming, 341 .stop_streaming = video_stop_streaming, 342 .unprepare_streaming = video_unprepare_streaming, 343 }; 344 345 /* ----------------------------------------------------------------------------- 346 * V4L2 ioctls 347 */ 348 349 static int video_querycap(struct file *file, void *fh, 350 struct v4l2_capability *cap) 351 { 352 strscpy(cap->driver, "qcom-camss", sizeof(cap->driver)); 353 strscpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card)); 354 355 return 0; 356 } 357 358 static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f) 359 { 360 struct camss_video *video = video_drvdata(file); 361 int i, j, k; 362 u32 mcode = f->mbus_code; 363 364 if (f->type != video->type) 365 return -EINVAL; 366 367 if (f->index >= video->nformats) 368 return -EINVAL; 369 370 /* 371 * Find index "i" of "k"th unique pixelformat in formats array. 372 * 373 * If f->mbus_code passed to video_enum_fmt() is not zero, a device 374 * with V4L2_CAP_IO_MC capability restricts enumeration to only the 375 * pixel formats that can be produced from that media bus code. 376 * This is implemented by skipping video->formats[] entries with 377 * code != f->mbus_code (if f->mbus_code is not zero). 378 * If the f->mbus_code passed to video_enum_fmt() is not supported, 379 * -EINVAL is returned. 380 * If f->mbus_code is zero, all the pixel formats are enumerated. 381 */ 382 k = -1; 383 for (i = 0; i < video->nformats; i++) { 384 if (mcode != 0 && video->formats[i].code != mcode) 385 continue; 386 387 for (j = 0; j < i; j++) { 388 if (mcode != 0 && video->formats[j].code != mcode) 389 continue; 390 if (video->formats[i].pixelformat == 391 video->formats[j].pixelformat) 392 break; 393 } 394 395 if (j == i) 396 k++; 397 398 if (k == f->index) 399 break; 400 } 401 402 if (k == -1 || k < f->index) 403 /* 404 * All the unique pixel formats matching the arguments 405 * have been enumerated (k >= 0 and f->index > 0), or 406 * no pixel formats match the non-zero f->mbus_code (k == -1). 407 */ 408 return -EINVAL; 409 410 f->pixelformat = video->formats[i].pixelformat; 411 412 return 0; 413 } 414 415 static int video_enum_framesizes(struct file *file, void *fh, 416 struct v4l2_frmsizeenum *fsize) 417 { 418 struct camss_video *video = video_drvdata(file); 419 int i; 420 421 if (fsize->index) 422 return -EINVAL; 423 424 /* Only accept pixel format present in the formats[] table */ 425 for (i = 0; i < video->nformats; i++) { 426 if (video->formats[i].pixelformat == fsize->pixel_format) 427 break; 428 } 429 430 if (i == video->nformats) 431 return -EINVAL; 432 433 fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS; 434 fsize->stepwise.min_width = CAMSS_FRAME_MIN_WIDTH; 435 fsize->stepwise.max_width = CAMSS_FRAME_MAX_WIDTH; 436 fsize->stepwise.min_height = CAMSS_FRAME_MIN_HEIGHT; 437 fsize->stepwise.max_height = (video->line_based) ? 438 CAMSS_FRAME_MAX_HEIGHT_PIX : CAMSS_FRAME_MAX_HEIGHT_RDI; 439 fsize->stepwise.step_width = 1; 440 fsize->stepwise.step_height = 1; 441 442 return 0; 443 } 444 445 static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f) 446 { 447 struct camss_video *video = video_drvdata(file); 448 449 *f = video->active_fmt; 450 451 return 0; 452 } 453 454 static int __video_try_fmt(struct camss_video *video, struct v4l2_format *f) 455 { 456 struct v4l2_pix_format_mplane *pix_mp; 457 const struct camss_format_info *fi; 458 struct v4l2_plane_pix_format *p; 459 u32 bytesperline[3] = { 0 }; 460 u32 sizeimage[3] = { 0 }; 461 u32 width, height; 462 u32 bpl, lines; 463 int i, j; 464 465 pix_mp = &f->fmt.pix_mp; 466 467 if (video->line_based) 468 for (i = 0; i < pix_mp->num_planes && i < 3; i++) { 469 p = &pix_mp->plane_fmt[i]; 470 bytesperline[i] = clamp_t(u32, p->bytesperline, 471 1, 65528); 472 sizeimage[i] = clamp_t(u32, p->sizeimage, 473 bytesperline[i], 474 bytesperline[i] * CAMSS_FRAME_MAX_HEIGHT_PIX); 475 } 476 477 for (j = 0; j < video->nformats; j++) 478 if (pix_mp->pixelformat == video->formats[j].pixelformat) 479 break; 480 481 if (j == video->nformats) 482 j = 0; /* default format */ 483 484 fi = &video->formats[j]; 485 width = pix_mp->width; 486 height = pix_mp->height; 487 488 memset(pix_mp, 0, sizeof(*pix_mp)); 489 490 pix_mp->pixelformat = fi->pixelformat; 491 pix_mp->width = clamp_t(u32, width, 1, CAMSS_FRAME_MAX_WIDTH); 492 pix_mp->height = clamp_t(u32, height, 1, CAMSS_FRAME_MAX_HEIGHT_RDI); 493 pix_mp->num_planes = fi->planes; 494 for (i = 0; i < pix_mp->num_planes; i++) { 495 bpl = pix_mp->width / fi->hsub[i].numerator * 496 fi->hsub[i].denominator * fi->bpp[i] / 8; 497 bpl = ALIGN(bpl, video->bpl_alignment); 498 pix_mp->plane_fmt[i].bytesperline = bpl; 499 pix_mp->plane_fmt[i].sizeimage = pix_mp->height / 500 fi->vsub[i].numerator * fi->vsub[i].denominator * bpl; 501 } 502 503 pix_mp->field = V4L2_FIELD_NONE; 504 pix_mp->colorspace = V4L2_COLORSPACE_SRGB; 505 pix_mp->flags = 0; 506 pix_mp->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix_mp->colorspace); 507 pix_mp->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true, 508 pix_mp->colorspace, pix_mp->ycbcr_enc); 509 pix_mp->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix_mp->colorspace); 510 511 if (video->line_based) 512 for (i = 0; i < pix_mp->num_planes; i++) { 513 p = &pix_mp->plane_fmt[i]; 514 p->bytesperline = clamp_t(u32, p->bytesperline, 515 1, 65528); 516 p->sizeimage = clamp_t(u32, p->sizeimage, 517 p->bytesperline, 518 p->bytesperline * CAMSS_FRAME_MAX_HEIGHT_PIX); 519 lines = p->sizeimage / p->bytesperline; 520 521 if (p->bytesperline < bytesperline[i]) 522 p->bytesperline = ALIGN(bytesperline[i], 8); 523 524 if (p->sizeimage < p->bytesperline * lines) 525 p->sizeimage = p->bytesperline * lines; 526 527 if (p->sizeimage < sizeimage[i]) 528 p->sizeimage = sizeimage[i]; 529 } 530 531 return 0; 532 } 533 534 static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f) 535 { 536 struct camss_video *video = video_drvdata(file); 537 538 return __video_try_fmt(video, f); 539 } 540 541 static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f) 542 { 543 struct camss_video *video = video_drvdata(file); 544 int ret; 545 546 if (vb2_is_busy(&video->vb2_q)) 547 return -EBUSY; 548 549 ret = __video_try_fmt(video, f); 550 if (ret < 0) 551 return ret; 552 553 video->active_fmt = *f; 554 555 return 0; 556 } 557 558 static int video_enum_input(struct file *file, void *fh, 559 struct v4l2_input *input) 560 { 561 if (input->index > 0) 562 return -EINVAL; 563 564 strscpy(input->name, "camera", sizeof(input->name)); 565 input->type = V4L2_INPUT_TYPE_CAMERA; 566 567 return 0; 568 } 569 570 static int video_g_input(struct file *file, void *fh, unsigned int *input) 571 { 572 *input = 0; 573 574 return 0; 575 } 576 577 static int video_s_input(struct file *file, void *fh, unsigned int input) 578 { 579 return input == 0 ? 0 : -EINVAL; 580 } 581 582 static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = { 583 .vidioc_querycap = video_querycap, 584 .vidioc_enum_fmt_vid_cap = video_enum_fmt, 585 .vidioc_enum_framesizes = video_enum_framesizes, 586 .vidioc_g_fmt_vid_cap_mplane = video_g_fmt, 587 .vidioc_s_fmt_vid_cap_mplane = video_s_fmt, 588 .vidioc_try_fmt_vid_cap_mplane = video_try_fmt, 589 .vidioc_reqbufs = vb2_ioctl_reqbufs, 590 .vidioc_querybuf = vb2_ioctl_querybuf, 591 .vidioc_qbuf = vb2_ioctl_qbuf, 592 .vidioc_expbuf = vb2_ioctl_expbuf, 593 .vidioc_dqbuf = vb2_ioctl_dqbuf, 594 .vidioc_create_bufs = vb2_ioctl_create_bufs, 595 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 596 .vidioc_streamon = vb2_ioctl_streamon, 597 .vidioc_streamoff = vb2_ioctl_streamoff, 598 .vidioc_enum_input = video_enum_input, 599 .vidioc_g_input = video_g_input, 600 .vidioc_s_input = video_s_input, 601 }; 602 603 /* ----------------------------------------------------------------------------- 604 * V4L2 file operations 605 */ 606 607 static const struct v4l2_file_operations msm_vid_fops = { 608 .owner = THIS_MODULE, 609 .unlocked_ioctl = video_ioctl2, 610 .open = v4l2_fh_open, 611 .release = vb2_fop_release, 612 .poll = vb2_fop_poll, 613 .mmap = vb2_fop_mmap, 614 .read = vb2_fop_read, 615 }; 616 617 /* ----------------------------------------------------------------------------- 618 * CAMSS video core 619 */ 620 621 static void msm_video_release(struct video_device *vdev) 622 { 623 struct camss_video *video = video_get_drvdata(vdev); 624 625 media_entity_cleanup(&vdev->entity); 626 627 mutex_destroy(&video->q_lock); 628 mutex_destroy(&video->lock); 629 630 if (atomic_dec_and_test(&video->camss->ref_count)) 631 camss_delete(video->camss); 632 } 633 634 /* 635 * msm_video_init_format - Helper function to initialize format 636 * @video: struct camss_video 637 * 638 * Initialize pad format with default value. 639 * 640 * Return 0 on success or a negative error code otherwise 641 */ 642 static int msm_video_init_format(struct camss_video *video) 643 { 644 int ret; 645 struct v4l2_format format = { 646 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 647 .fmt.pix_mp = { 648 .width = 1920, 649 .height = 1080, 650 .pixelformat = video->formats[0].pixelformat, 651 }, 652 }; 653 654 ret = __video_try_fmt(video, &format); 655 if (ret < 0) 656 return ret; 657 658 video->active_fmt = format; 659 660 return 0; 661 } 662 663 /* 664 * msm_video_register - Register a video device node 665 * @video: struct camss_video 666 * @v4l2_dev: V4L2 device 667 * @name: name to be used for the video device node 668 * 669 * Initialize and register a video device node to a V4L2 device. Also 670 * initialize the vb2 queue. 671 * 672 * Return 0 on success or a negative error code otherwise 673 */ 674 675 int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev, 676 const char *name) 677 { 678 struct media_pad *pad = &video->pad; 679 struct video_device *vdev; 680 struct vb2_queue *q; 681 int ret; 682 683 vdev = &video->vdev; 684 685 mutex_init(&video->q_lock); 686 687 q = &video->vb2_q; 688 q->drv_priv = video; 689 q->mem_ops = &vb2_dma_sg_memops; 690 q->ops = &msm_video_vb2_q_ops; 691 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; 692 q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ; 693 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 694 q->buf_struct_size = sizeof(struct camss_buffer); 695 q->dev = video->camss->dev; 696 q->lock = &video->q_lock; 697 ret = vb2_queue_init(q); 698 if (ret < 0) { 699 dev_err(v4l2_dev->dev, "Failed to init vb2 queue: %d\n", ret); 700 goto error_vb2_init; 701 } 702 703 pad->flags = MEDIA_PAD_FL_SINK; 704 ret = media_entity_pads_init(&vdev->entity, 1, pad); 705 if (ret < 0) { 706 dev_err(v4l2_dev->dev, "Failed to init video entity: %d\n", 707 ret); 708 goto error_vb2_init; 709 } 710 711 mutex_init(&video->lock); 712 713 ret = msm_video_init_format(video); 714 if (ret < 0) { 715 dev_err(v4l2_dev->dev, "Failed to init format: %d\n", ret); 716 goto error_video_register; 717 } 718 719 vdev->fops = &msm_vid_fops; 720 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING 721 | V4L2_CAP_READWRITE | V4L2_CAP_IO_MC; 722 vdev->ioctl_ops = &msm_vid_ioctl_ops; 723 vdev->release = msm_video_release; 724 vdev->v4l2_dev = v4l2_dev; 725 vdev->vfl_dir = VFL_DIR_RX; 726 vdev->queue = &video->vb2_q; 727 vdev->lock = &video->lock; 728 strscpy(vdev->name, name, sizeof(vdev->name)); 729 730 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 731 if (ret < 0) { 732 dev_err(v4l2_dev->dev, "Failed to register video device: %d\n", 733 ret); 734 goto error_video_register; 735 } 736 737 video_set_drvdata(vdev, video); 738 atomic_inc(&video->camss->ref_count); 739 740 return 0; 741 742 error_video_register: 743 media_entity_cleanup(&vdev->entity); 744 mutex_destroy(&video->lock); 745 error_vb2_init: 746 mutex_destroy(&video->q_lock); 747 748 return ret; 749 } 750 751 void msm_video_unregister(struct camss_video *video) 752 { 753 atomic_inc(&video->camss->ref_count); 754 vb2_video_unregister_device(&video->vdev); 755 atomic_dec(&video->camss->ref_count); 756 } 757