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