1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * Copyright (C) 2025 Advanced Micro Devices, Inc. 4 */ 5 6 #include <media/v4l2-ioctl.h> 7 #include <media/v4l2-mc.h> 8 #include <media/videobuf2-vmalloc.h> 9 10 #include "isp4_interface.h" 11 #include "isp4_subdev.h" 12 #include "isp4_video.h" 13 14 #define ISP4VID_ISP_DRV_NAME "amd_isp_capture" 15 #define ISP4VID_MAX_PREVIEW_FPS 30 16 #define ISP4VID_DEFAULT_FMT V4L2_PIX_FMT_NV12 17 18 #define ISP4VID_PAD_VIDEO_OUTPUT 0 19 20 /* time perframe default */ 21 #define ISP4VID_ISP_TPF_DEFAULT isp4vid_tpfs[0] 22 23 static const char *const isp4vid_video_dev_name = "Preview"; 24 25 /* Sizes must be in increasing order */ 26 static const struct v4l2_frmsize_discrete isp4vid_frmsize[] = { 27 {640, 360}, 28 {640, 480}, 29 {1280, 720}, 30 {1280, 960}, 31 {1920, 1080}, 32 {1920, 1440}, 33 {2560, 1440}, 34 {2880, 1620}, 35 {2880, 1624}, 36 {2888, 1808}, 37 }; 38 39 static const u32 isp4vid_formats[] = { 40 V4L2_PIX_FMT_NV12, 41 V4L2_PIX_FMT_YUYV 42 }; 43 44 /* time perframe list */ 45 static const struct v4l2_fract isp4vid_tpfs[] = { 46 { 1, ISP4VID_MAX_PREVIEW_FPS } 47 }; 48 49 void isp4vid_handle_frame_done(struct isp4vid_dev *isp_vdev, 50 const struct isp4if_img_buf_info *img_buf) 51 { 52 struct isp4vid_capture_buffer *isp4vid_buf; 53 void *vbuf; 54 55 scoped_guard(mutex, &isp_vdev->buf_list_lock) { 56 isp4vid_buf = list_first_entry_or_null(&isp_vdev->buf_list, 57 typeof(*isp4vid_buf), 58 list); 59 if (!isp4vid_buf) 60 return; 61 62 vbuf = vb2_plane_vaddr(&isp4vid_buf->vb2.vb2_buf, 0); 63 64 if (vbuf != img_buf->planes[0].sys_addr) { 65 dev_err(isp_vdev->dev, "Invalid vbuf\n"); 66 return; 67 } 68 69 list_del(&isp4vid_buf->list); 70 } 71 72 /* Fill the buffer */ 73 isp4vid_buf->vb2.vb2_buf.timestamp = ktime_get_ns(); 74 isp4vid_buf->vb2.sequence = isp_vdev->sequence++; 75 isp4vid_buf->vb2.field = V4L2_FIELD_ANY; 76 77 vb2_set_plane_payload(&isp4vid_buf->vb2.vb2_buf, 78 0, isp_vdev->format.sizeimage); 79 80 vb2_buffer_done(&isp4vid_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE); 81 82 dev_dbg(isp_vdev->dev, "call vb2_buffer_done(size=%u)\n", 83 isp_vdev->format.sizeimage); 84 } 85 86 static const struct v4l2_pix_format isp4vid_fmt_default = { 87 .width = 1920, 88 .height = 1080, 89 .pixelformat = ISP4VID_DEFAULT_FMT, 90 .field = V4L2_FIELD_NONE, 91 .colorspace = V4L2_COLORSPACE_SRGB, 92 }; 93 94 static void isp4vid_capture_return_all_buffers(struct isp4vid_dev *isp_vdev, 95 enum vb2_buffer_state state) 96 { 97 struct isp4vid_capture_buffer *vbuf, *node; 98 99 scoped_guard(mutex, &isp_vdev->buf_list_lock) { 100 list_for_each_entry_safe(vbuf, node, &isp_vdev->buf_list, list) 101 vb2_buffer_done(&vbuf->vb2.vb2_buf, state); 102 INIT_LIST_HEAD(&isp_vdev->buf_list); 103 } 104 105 dev_dbg(isp_vdev->dev, "call vb2_buffer_done(%d)\n", state); 106 } 107 108 static int isp4vid_vdev_link_validate(struct media_link *link) 109 { 110 return 0; 111 } 112 113 static const struct media_entity_operations isp4vid_vdev_ent_ops = { 114 .link_validate = isp4vid_vdev_link_validate, 115 }; 116 117 static const struct v4l2_file_operations isp4vid_vdev_fops = { 118 .owner = THIS_MODULE, 119 .open = v4l2_fh_open, 120 .release = vb2_fop_release, 121 .read = vb2_fop_read, 122 .poll = vb2_fop_poll, 123 .unlocked_ioctl = video_ioctl2, 124 .mmap = vb2_fop_mmap, 125 }; 126 127 static int isp4vid_ioctl_querycap(struct file *file, void *fh, 128 struct v4l2_capability *cap) 129 { 130 struct isp4vid_dev *isp_vdev = video_drvdata(file); 131 132 strscpy(cap->driver, ISP4VID_ISP_DRV_NAME, sizeof(cap->driver)); 133 snprintf(cap->card, sizeof(cap->card), "%s", ISP4VID_ISP_DRV_NAME); 134 cap->capabilities |= V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE; 135 136 dev_dbg(isp_vdev->dev, "%s|capabilities=0x%X\n", isp_vdev->vdev.name, 137 cap->capabilities); 138 139 return 0; 140 } 141 142 static int isp4vid_g_fmt_vid_cap(struct file *file, void *priv, 143 struct v4l2_format *f) 144 { 145 struct isp4vid_dev *isp_vdev = video_drvdata(file); 146 147 f->fmt.pix = isp_vdev->format; 148 149 return 0; 150 } 151 152 static int isp4vid_fill_buffer_size(struct v4l2_pix_format *fmt) 153 { 154 int ret = 0; 155 156 switch (fmt->pixelformat) { 157 case V4L2_PIX_FMT_NV12: 158 fmt->bytesperline = fmt->width; 159 fmt->sizeimage = fmt->bytesperline * fmt->height * 3 / 2; 160 break; 161 case V4L2_PIX_FMT_YUYV: 162 fmt->bytesperline = fmt->width * 2; 163 fmt->sizeimage = fmt->bytesperline * fmt->height; 164 break; 165 default: 166 ret = -EINVAL; 167 break; 168 } 169 170 return ret; 171 } 172 173 static int isp4vid_try_fmt_vid_cap(struct file *file, void *priv, 174 struct v4l2_format *f) 175 { 176 struct isp4vid_dev *isp_vdev = video_drvdata(file); 177 struct v4l2_pix_format *format = &f->fmt.pix; 178 const struct v4l2_frmsize_discrete *fsz; 179 size_t i; 180 181 /* 182 * Check if the hardware supports the requested format, use the default 183 * format otherwise. 184 */ 185 for (i = 0; i < ARRAY_SIZE(isp4vid_formats); i++) 186 if (isp4vid_formats[i] == format->pixelformat) 187 break; 188 189 if (i == ARRAY_SIZE(isp4vid_formats)) 190 format->pixelformat = ISP4VID_DEFAULT_FMT; 191 192 switch (format->pixelformat) { 193 case V4L2_PIX_FMT_NV12: 194 case V4L2_PIX_FMT_YUYV: 195 fsz = v4l2_find_nearest_size(isp4vid_frmsize, 196 ARRAY_SIZE(isp4vid_frmsize), 197 width, height, format->width, 198 format->height); 199 format->width = fsz->width; 200 format->height = fsz->height; 201 break; 202 default: 203 dev_err(isp_vdev->dev, "%s|unsupported fmt=%u\n", 204 isp_vdev->vdev.name, 205 format->pixelformat); 206 return -EINVAL; 207 } 208 209 /* 210 * There is no need to check the return value, as failure will never 211 * happen here 212 */ 213 isp4vid_fill_buffer_size(format); 214 215 if (format->field == V4L2_FIELD_ANY) 216 format->field = isp4vid_fmt_default.field; 217 218 if (format->colorspace == V4L2_COLORSPACE_DEFAULT) 219 format->colorspace = isp4vid_fmt_default.colorspace; 220 221 return 0; 222 } 223 224 static int isp4vid_set_fmt_2_isp(struct v4l2_subdev *sdev, 225 struct v4l2_pix_format *pix_fmt) 226 { 227 struct v4l2_subdev_format fmt = {}; 228 229 switch (pix_fmt->pixelformat) { 230 case V4L2_PIX_FMT_NV12: 231 fmt.format.code = MEDIA_BUS_FMT_YUYV8_1_5X8; 232 break; 233 case V4L2_PIX_FMT_YUYV: 234 fmt.format.code = MEDIA_BUS_FMT_YUYV8_1X16; 235 break; 236 default: 237 return -EINVAL; 238 } 239 fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; 240 fmt.pad = ISP4VID_PAD_VIDEO_OUTPUT; 241 fmt.format.width = pix_fmt->width; 242 fmt.format.height = pix_fmt->height; 243 return v4l2_subdev_call(sdev, pad, set_fmt, NULL, &fmt); 244 } 245 246 static int isp4vid_s_fmt_vid_cap(struct file *file, void *priv, 247 struct v4l2_format *f) 248 { 249 struct isp4vid_dev *isp_vdev = video_drvdata(file); 250 int ret; 251 252 /* Do not change the format while stream is on */ 253 if (vb2_is_busy(&isp_vdev->vbq)) 254 return -EBUSY; 255 256 ret = isp4vid_try_fmt_vid_cap(file, priv, f); 257 if (ret) 258 return ret; 259 260 dev_dbg(isp_vdev->dev, "%s|width height:%ux%u->%ux%u\n", 261 isp_vdev->vdev.name, 262 isp_vdev->format.width, isp_vdev->format.height, 263 f->fmt.pix.width, f->fmt.pix.height); 264 dev_dbg(isp_vdev->dev, "%s|pixelformat:0x%x-0x%x\n", 265 isp_vdev->vdev.name, isp_vdev->format.pixelformat, 266 f->fmt.pix.pixelformat); 267 dev_dbg(isp_vdev->dev, "%s|bytesperline:%u->%u\n", 268 isp_vdev->vdev.name, isp_vdev->format.bytesperline, 269 f->fmt.pix.bytesperline); 270 dev_dbg(isp_vdev->dev, "%s|sizeimage:%u->%u\n", 271 isp_vdev->vdev.name, isp_vdev->format.sizeimage, 272 f->fmt.pix.sizeimage); 273 274 isp_vdev->format = f->fmt.pix; 275 ret = isp4vid_set_fmt_2_isp(isp_vdev->isp_sdev, &isp_vdev->format); 276 277 return ret; 278 } 279 280 static int isp4vid_enum_fmt_vid_cap(struct file *file, void *priv, 281 struct v4l2_fmtdesc *f) 282 { 283 struct isp4vid_dev *isp_vdev = video_drvdata(file); 284 285 switch (f->index) { 286 case 0: 287 f->pixelformat = V4L2_PIX_FMT_NV12; 288 break; 289 case 1: 290 f->pixelformat = V4L2_PIX_FMT_YUYV; 291 break; 292 default: 293 return -EINVAL; 294 } 295 296 dev_dbg(isp_vdev->dev, "%s|index=%d, pixelformat=0x%X\n", 297 isp_vdev->vdev.name, f->index, f->pixelformat); 298 299 return 0; 300 } 301 302 static int isp4vid_enum_framesizes(struct file *file, void *fh, 303 struct v4l2_frmsizeenum *fsize) 304 { 305 struct isp4vid_dev *isp_vdev = video_drvdata(file); 306 unsigned int i; 307 308 for (i = 0; i < ARRAY_SIZE(isp4vid_formats); i++) { 309 if (isp4vid_formats[i] == fsize->pixel_format) 310 break; 311 } 312 313 if (i == ARRAY_SIZE(isp4vid_formats)) 314 return -EINVAL; 315 316 if (fsize->index < ARRAY_SIZE(isp4vid_frmsize)) { 317 fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE; 318 fsize->discrete = isp4vid_frmsize[fsize->index]; 319 dev_dbg(isp_vdev->dev, "%s|size[%d]=%dx%d\n", 320 isp_vdev->vdev.name, fsize->index, 321 fsize->discrete.width, fsize->discrete.height); 322 } else { 323 return -EINVAL; 324 } 325 326 return 0; 327 } 328 329 static int isp4vid_ioctl_enum_frameintervals(struct file *file, void *priv, 330 struct v4l2_frmivalenum *fival) 331 { 332 struct isp4vid_dev *isp_vdev = video_drvdata(file); 333 size_t i; 334 335 if (fival->index >= ARRAY_SIZE(isp4vid_tpfs)) 336 return -EINVAL; 337 338 for (i = 0; i < ARRAY_SIZE(isp4vid_formats); i++) 339 if (isp4vid_formats[i] == fival->pixel_format) 340 break; 341 342 if (i == ARRAY_SIZE(isp4vid_formats)) 343 return -EINVAL; 344 345 for (i = 0; i < ARRAY_SIZE(isp4vid_frmsize); i++) 346 if (isp4vid_frmsize[i].width == fival->width && 347 isp4vid_frmsize[i].height == fival->height) 348 break; 349 350 if (i == ARRAY_SIZE(isp4vid_frmsize)) 351 return -EINVAL; 352 353 fival->type = V4L2_FRMIVAL_TYPE_DISCRETE; 354 fival->discrete = isp4vid_tpfs[fival->index]; 355 v4l2_simplify_fraction(&fival->discrete.numerator, 356 &fival->discrete.denominator, 8, 333); 357 358 dev_dbg(isp_vdev->dev, "%s|interval[%d]=%d/%d\n", 359 isp_vdev->vdev.name, fival->index, 360 fival->discrete.numerator, 361 fival->discrete.denominator); 362 363 return 0; 364 } 365 366 static int isp4vid_ioctl_g_param(struct file *file, void *priv, 367 struct v4l2_streamparm *param) 368 { 369 struct v4l2_captureparm *capture = ¶m->parm.capture; 370 struct isp4vid_dev *isp_vdev = video_drvdata(file); 371 372 if (param->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) 373 return -EINVAL; 374 375 capture->capability = V4L2_CAP_TIMEPERFRAME; 376 capture->timeperframe = isp_vdev->timeperframe; 377 capture->readbuffers = 0; 378 379 dev_dbg(isp_vdev->dev, "%s|timeperframe=%d/%d\n", isp_vdev->vdev.name, 380 capture->timeperframe.numerator, 381 capture->timeperframe.denominator); 382 383 return 0; 384 } 385 386 static const struct v4l2_ioctl_ops isp4vid_vdev_ioctl_ops = { 387 .vidioc_querycap = isp4vid_ioctl_querycap, 388 .vidioc_enum_fmt_vid_cap = isp4vid_enum_fmt_vid_cap, 389 .vidioc_g_fmt_vid_cap = isp4vid_g_fmt_vid_cap, 390 .vidioc_s_fmt_vid_cap = isp4vid_s_fmt_vid_cap, 391 .vidioc_try_fmt_vid_cap = isp4vid_try_fmt_vid_cap, 392 .vidioc_reqbufs = vb2_ioctl_reqbufs, 393 .vidioc_querybuf = vb2_ioctl_querybuf, 394 .vidioc_qbuf = vb2_ioctl_qbuf, 395 .vidioc_expbuf = vb2_ioctl_expbuf, 396 .vidioc_dqbuf = vb2_ioctl_dqbuf, 397 .vidioc_create_bufs = vb2_ioctl_create_bufs, 398 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 399 .vidioc_streamon = vb2_ioctl_streamon, 400 .vidioc_streamoff = vb2_ioctl_streamoff, 401 .vidioc_g_parm = isp4vid_ioctl_g_param, 402 .vidioc_s_parm = isp4vid_ioctl_g_param, 403 .vidioc_enum_framesizes = isp4vid_enum_framesizes, 404 .vidioc_enum_frameintervals = isp4vid_ioctl_enum_frameintervals, 405 }; 406 407 static unsigned int isp4vid_get_image_size(struct v4l2_pix_format *fmt) 408 { 409 switch (fmt->pixelformat) { 410 case V4L2_PIX_FMT_NV12: 411 return fmt->width * fmt->height * 3 / 2; 412 case V4L2_PIX_FMT_YUYV: 413 return fmt->width * fmt->height * 2; 414 default: 415 return 0; 416 } 417 } 418 419 static int isp4vid_qops_queue_setup(struct vb2_queue *vq, 420 unsigned int *nbuffers, 421 unsigned int *nplanes, unsigned int sizes[], 422 struct device *alloc_devs[]) 423 { 424 struct isp4vid_dev *isp_vdev = vb2_get_drv_priv(vq); 425 unsigned int q_num_bufs = vb2_get_num_buffers(vq); 426 427 if (*nplanes > 1) { 428 dev_err(isp_vdev->dev, 429 "fail to setup queue, no mplane supported %u\n", 430 *nplanes); 431 return -EINVAL; 432 } 433 434 if (*nplanes == 1) { 435 unsigned int size; 436 437 size = isp4vid_get_image_size(&isp_vdev->format); 438 if (sizes[0] < size) { 439 dev_err(isp_vdev->dev, 440 "fail for small plane size %u, %u expected\n", 441 sizes[0], size); 442 return -EINVAL; 443 } 444 } 445 446 if (q_num_bufs + *nbuffers < ISP4IF_MAX_STREAM_BUF_COUNT) 447 *nbuffers = ISP4IF_MAX_STREAM_BUF_COUNT - q_num_bufs; 448 449 switch (isp_vdev->format.pixelformat) { 450 case V4L2_PIX_FMT_NV12: 451 case V4L2_PIX_FMT_YUYV: { 452 *nplanes = 1; 453 sizes[0] = max(sizes[0], isp_vdev->format.sizeimage); 454 isp_vdev->format.sizeimage = sizes[0]; 455 } 456 break; 457 default: 458 dev_err(isp_vdev->dev, "%s|unsupported fmt=%u\n", 459 isp_vdev->vdev.name, isp_vdev->format.pixelformat); 460 return -EINVAL; 461 } 462 463 dev_dbg(isp_vdev->dev, "%s|*nbuffers=%u *nplanes=%u sizes[0]=%u\n", 464 isp_vdev->vdev.name, 465 *nbuffers, *nplanes, sizes[0]); 466 467 return 0; 468 } 469 470 static void isp4vid_qops_buffer_queue(struct vb2_buffer *vb) 471 { 472 struct isp4vid_capture_buffer *buf = 473 container_of(vb, struct isp4vid_capture_buffer, vb2.vb2_buf); 474 struct isp4vid_dev *isp_vdev = vb2_get_drv_priv(vb->vb2_queue); 475 struct isp4if_img_buf_info *img_buf = &buf->img_buf; 476 void *vaddr = vb2_plane_vaddr(vb, 0); 477 478 dev_dbg(isp_vdev->dev, "queue buf, vaddr %p, gpuva 0x%llx, size %u\n", 479 vaddr, buf->gpu_addr, vb->planes[0].length); 480 481 switch (isp_vdev->format.pixelformat) { 482 case V4L2_PIX_FMT_NV12: { 483 u32 y_size = isp_vdev->format.sizeimage / 3 * 2; 484 u32 uv_size = isp_vdev->format.sizeimage / 3; 485 486 img_buf->planes[0].len = y_size; 487 img_buf->planes[0].sys_addr = vaddr; 488 img_buf->planes[0].mc_addr = buf->gpu_addr; 489 490 dev_dbg(isp_vdev->dev, "img_buf[0]: mc=0x%llx size=%u\n", 491 img_buf->planes[0].mc_addr, 492 img_buf->planes[0].len); 493 494 img_buf->planes[1].len = uv_size; 495 img_buf->planes[1].sys_addr = vaddr + y_size; 496 img_buf->planes[1].mc_addr = buf->gpu_addr + y_size; 497 498 dev_dbg(isp_vdev->dev, "img_buf[1]: mc=0x%llx size=%u\n", 499 img_buf->planes[1].mc_addr, 500 img_buf->planes[1].len); 501 502 img_buf->planes[2].len = 0; 503 } 504 break; 505 case V4L2_PIX_FMT_YUYV: { 506 img_buf->planes[0].len = isp_vdev->format.sizeimage; 507 img_buf->planes[0].sys_addr = vaddr; 508 img_buf->planes[0].mc_addr = buf->gpu_addr; 509 510 dev_dbg(isp_vdev->dev, "img_buf[0]: mc=0x%llx size=%u\n", 511 img_buf->planes[0].mc_addr, 512 img_buf->planes[0].len); 513 514 img_buf->planes[1].len = 0; 515 img_buf->planes[2].len = 0; 516 } 517 break; 518 default: 519 dev_err(isp_vdev->dev, "%s|unsupported fmt=%u\n", 520 isp_vdev->vdev.name, isp_vdev->format.pixelformat); 521 return; 522 } 523 524 if (isp_vdev->stream_started) 525 isp4sd_ioc_send_img_buf(isp_vdev->isp_sdev, img_buf); 526 527 scoped_guard(mutex, &isp_vdev->buf_list_lock) 528 list_add_tail(&buf->list, &isp_vdev->buf_list); 529 } 530 531 static int isp4vid_qops_start_streaming(struct vb2_queue *vq, 532 unsigned int count) 533 { 534 struct isp4vid_dev *isp_vdev = vb2_get_drv_priv(vq); 535 struct isp4vid_capture_buffer *isp4vid_buf; 536 struct media_entity *entity; 537 struct v4l2_subdev *subdev; 538 struct media_pad *pad; 539 int ret = 0; 540 541 isp_vdev->sequence = 0; 542 543 ret = isp4sd_pwron_and_init(isp_vdev->isp_sdev); 544 if (ret) { 545 dev_err(isp_vdev->dev, "power up isp fail %d\n", ret); 546 goto release_buffers; 547 } 548 549 entity = &isp_vdev->vdev.entity; 550 while (1) { 551 pad = &entity->pads[0]; 552 if (!(pad->flags & MEDIA_PAD_FL_SINK)) 553 break; 554 555 pad = media_pad_remote_pad_first(pad); 556 if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) 557 break; 558 559 entity = pad->entity; 560 subdev = media_entity_to_v4l2_subdev(entity); 561 562 ret = v4l2_subdev_call(subdev, video, s_stream, 1); 563 if (ret < 0 && ret != -ENOIOCTLCMD) { 564 dev_dbg(isp_vdev->dev, "fail start streaming: %s %d\n", 565 subdev->name, ret); 566 goto release_buffers; 567 } 568 } 569 570 list_for_each_entry(isp4vid_buf, &isp_vdev->buf_list, list) 571 isp4sd_ioc_send_img_buf(isp_vdev->isp_sdev, 572 &isp4vid_buf->img_buf); 573 574 isp_vdev->stream_started = true; 575 576 return 0; 577 578 release_buffers: 579 isp4vid_capture_return_all_buffers(isp_vdev, VB2_BUF_STATE_QUEUED); 580 return ret; 581 } 582 583 static void isp4vid_qops_stop_streaming(struct vb2_queue *vq) 584 { 585 struct isp4vid_dev *isp_vdev = vb2_get_drv_priv(vq); 586 struct media_entity *entity; 587 struct v4l2_subdev *subdev; 588 struct media_pad *pad; 589 int ret; 590 591 entity = &isp_vdev->vdev.entity; 592 while (1) { 593 pad = &entity->pads[0]; 594 if (!(pad->flags & MEDIA_PAD_FL_SINK)) 595 break; 596 597 pad = media_pad_remote_pad_first(pad); 598 if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) 599 break; 600 601 entity = pad->entity; 602 subdev = media_entity_to_v4l2_subdev(entity); 603 604 ret = v4l2_subdev_call(subdev, video, s_stream, 0); 605 606 if (ret < 0 && ret != -ENOIOCTLCMD) 607 dev_dbg(isp_vdev->dev, "fail stop streaming: %s %d\n", 608 subdev->name, ret); 609 } 610 611 isp_vdev->stream_started = false; 612 isp4sd_pwroff_and_deinit(isp_vdev->isp_sdev); 613 614 /* Release all active buffers */ 615 isp4vid_capture_return_all_buffers(isp_vdev, VB2_BUF_STATE_ERROR); 616 } 617 618 static int isp4vid_qops_buf_init(struct vb2_buffer *vb) 619 { 620 struct isp4vid_capture_buffer *buf = 621 container_of(vb, struct isp4vid_capture_buffer, vb2.vb2_buf); 622 struct isp4vid_dev *isp_vdev = vb2_get_drv_priv(vb->vb2_queue); 623 void *mem_priv = vb->planes[0].mem_priv; 624 struct device *dev = isp_vdev->dev; 625 u64 gpu_addr; 626 void *bo; 627 int ret; 628 629 if (vb->planes[0].dbuf) { 630 buf->dbuf = vb->planes[0].dbuf; 631 } else { 632 /* 633 * HAS_DMA is a Kconfig dependency so CONFIG_HAS_DMA is always 634 * defined when this driver is compiled. The #else branch is 635 * kept as a safeguard in case the dependency is ever removed. 636 */ 637 #ifdef CONFIG_HAS_DMA 638 buf->dbuf = vb2_vmalloc_memops.get_dmabuf(vb, mem_priv, 0); 639 if (IS_ERR_OR_NULL(buf->dbuf)) { 640 dev_err(dev, "fail to get dma buf\n"); 641 return -EINVAL; 642 } 643 #else 644 dev_err(dev, "get dmabuf fail -- CONFIG_HAS_DMA not defined\n"); 645 buf->dbuf = NULL; 646 return -EINVAL; 647 #endif 648 } 649 650 /* create isp user BO and obtain gpu_addr */ 651 ret = isp_user_buffer_alloc(dev, buf->dbuf, &bo, &gpu_addr); 652 if (ret) { 653 dev_err(dev, "fail to create isp user BO\n"); 654 if (!vb->planes[0].dbuf) { 655 dma_buf_put(buf->dbuf); 656 buf->dbuf = NULL; 657 } 658 659 return ret; 660 } 661 662 buf->bo = bo; 663 buf->gpu_addr = gpu_addr; 664 return 0; 665 } 666 667 static void isp4vid_qops_buf_cleanup(struct vb2_buffer *vb) 668 { 669 struct isp4vid_capture_buffer *buf = 670 container_of(vb, struct isp4vid_capture_buffer, vb2.vb2_buf); 671 672 if (buf->bo) { 673 isp_user_buffer_free(buf->bo); 674 buf->bo = NULL; 675 } 676 677 /* 678 * Only put dmabufs we obtained ourselves via get_dmabuf, not ones 679 * provided by the framework for DMABUF import 680 */ 681 if (buf->dbuf && buf->dbuf != vb->planes[0].dbuf) 682 dma_buf_put(buf->dbuf); 683 684 buf->dbuf = NULL; 685 } 686 687 static const struct vb2_ops isp4vid_qops = { 688 .queue_setup = isp4vid_qops_queue_setup, 689 .buf_init = isp4vid_qops_buf_init, 690 .buf_cleanup = isp4vid_qops_buf_cleanup, 691 .start_streaming = isp4vid_qops_start_streaming, 692 .stop_streaming = isp4vid_qops_stop_streaming, 693 .buf_queue = isp4vid_qops_buffer_queue, 694 }; 695 696 int isp4vid_dev_init(struct isp4vid_dev *isp_vdev, struct v4l2_subdev *isp_sd) 697 { 698 const char *vdev_name = isp4vid_video_dev_name; 699 struct v4l2_device *v4l2_dev; 700 struct video_device *vdev; 701 struct vb2_queue *q; 702 int ret; 703 704 if (!isp_vdev || !isp_sd || !isp_sd->v4l2_dev) 705 return -EINVAL; 706 707 v4l2_dev = isp_sd->v4l2_dev; 708 vdev = &isp_vdev->vdev; 709 710 isp_vdev->isp_sdev = isp_sd; 711 isp_vdev->dev = v4l2_dev->dev; 712 713 /* Initialize the vb2_queue struct */ 714 mutex_init(&isp_vdev->vbq_lock); 715 q = &isp_vdev->vbq; 716 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 717 q->io_modes = VB2_MMAP | VB2_DMABUF; 718 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 719 q->buf_struct_size = sizeof(struct isp4vid_capture_buffer); 720 q->min_queued_buffers = 2; 721 q->ops = &isp4vid_qops; 722 q->drv_priv = isp_vdev; 723 q->mem_ops = &vb2_vmalloc_memops; 724 q->lock = &isp_vdev->vbq_lock; 725 q->dev = v4l2_dev->dev; 726 ret = vb2_queue_init(q); 727 if (ret) { 728 dev_err(v4l2_dev->dev, "vb2_queue_init error:%d\n", ret); 729 return ret; 730 } 731 732 /* Initialize buffer list and its lock */ 733 mutex_init(&isp_vdev->buf_list_lock); 734 INIT_LIST_HEAD(&isp_vdev->buf_list); 735 736 /* Set default frame format */ 737 isp_vdev->format = isp4vid_fmt_default; 738 isp_vdev->timeperframe = ISP4VID_ISP_TPF_DEFAULT; 739 v4l2_simplify_fraction(&isp_vdev->timeperframe.numerator, 740 &isp_vdev->timeperframe.denominator, 8, 333); 741 742 ret = isp4vid_fill_buffer_size(&isp_vdev->format); 743 if (ret) { 744 dev_err(v4l2_dev->dev, "fail to fill buffer size: %d\n", ret); 745 goto err_release_vb2_queue; 746 } 747 748 ret = isp4vid_set_fmt_2_isp(isp_sd, &isp_vdev->format); 749 if (ret) { 750 dev_err(v4l2_dev->dev, "fail init format :%d\n", ret); 751 goto err_release_vb2_queue; 752 } 753 754 /* Initialize the video_device struct */ 755 isp_vdev->vdev.entity.name = vdev_name; 756 isp_vdev->vdev.entity.function = MEDIA_ENT_F_IO_V4L; 757 isp_vdev->vdev_pad.flags = MEDIA_PAD_FL_SINK; 758 ret = media_entity_pads_init(&isp_vdev->vdev.entity, 1, 759 &isp_vdev->vdev_pad); 760 761 if (ret) { 762 dev_err(v4l2_dev->dev, "init media entity pad fail:%d\n", ret); 763 goto err_release_vb2_queue; 764 } 765 766 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | 767 V4L2_CAP_STREAMING | V4L2_CAP_IO_MC; 768 vdev->entity.ops = &isp4vid_vdev_ent_ops; 769 vdev->release = video_device_release_empty; 770 vdev->fops = &isp4vid_vdev_fops; 771 vdev->ioctl_ops = &isp4vid_vdev_ioctl_ops; 772 vdev->lock = NULL; 773 vdev->queue = q; 774 vdev->v4l2_dev = v4l2_dev; 775 vdev->vfl_dir = VFL_DIR_RX; 776 strscpy(vdev->name, vdev_name, sizeof(vdev->name)); 777 video_set_drvdata(vdev, isp_vdev); 778 779 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 780 if (ret) { 781 dev_err(v4l2_dev->dev, "register video device fail:%d\n", ret); 782 goto err_entity_cleanup; 783 } 784 785 return 0; 786 787 err_entity_cleanup: 788 media_entity_cleanup(&isp_vdev->vdev.entity); 789 err_release_vb2_queue: 790 vb2_queue_release(q); 791 return ret; 792 } 793 794 void isp4vid_dev_deinit(struct isp4vid_dev *isp_vdev) 795 { 796 vb2_video_unregister_device(&isp_vdev->vdev); 797 } 798