1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * Copyright 2021-2022 Bootlin 4 * Author: Paul Kocialkowski <paul.kocialkowski@bootlin.com> 5 */ 6 7 #include <media/v4l2-device.h> 8 #include <media/v4l2-event.h> 9 #include <media/v4l2-ioctl.h> 10 #include <media/v4l2-mc.h> 11 #include <media/videobuf2-dma-contig.h> 12 #include <media/videobuf2-v4l2.h> 13 14 #include "sun6i_isp.h" 15 #include "sun6i_isp_capture.h" 16 #include "sun6i_isp_proc.h" 17 #include "sun6i_isp_reg.h" 18 19 /* Helpers */ 20 21 void sun6i_isp_capture_dimensions(struct sun6i_isp_device *isp_dev, 22 unsigned int *width, unsigned int *height) 23 { 24 if (width) 25 *width = isp_dev->capture.format.fmt.pix.width; 26 if (height) 27 *height = isp_dev->capture.format.fmt.pix.height; 28 } 29 30 void sun6i_isp_capture_format(struct sun6i_isp_device *isp_dev, 31 u32 *pixelformat) 32 { 33 if (pixelformat) 34 *pixelformat = isp_dev->capture.format.fmt.pix.pixelformat; 35 } 36 37 /* Format */ 38 39 static const struct sun6i_isp_capture_format sun6i_isp_capture_formats[] = { 40 { 41 .pixelformat = V4L2_PIX_FMT_NV12, 42 .output_format = SUN6I_ISP_OUTPUT_FMT_YUV420SP, 43 }, 44 { 45 .pixelformat = V4L2_PIX_FMT_NV21, 46 .output_format = SUN6I_ISP_OUTPUT_FMT_YVU420SP, 47 }, 48 }; 49 50 const struct sun6i_isp_capture_format * 51 sun6i_isp_capture_format_find(u32 pixelformat) 52 { 53 unsigned int i; 54 55 for (i = 0; i < ARRAY_SIZE(sun6i_isp_capture_formats); i++) 56 if (sun6i_isp_capture_formats[i].pixelformat == pixelformat) 57 return &sun6i_isp_capture_formats[i]; 58 59 return NULL; 60 } 61 62 /* Capture */ 63 64 static void 65 sun6i_isp_capture_buffer_configure(struct sun6i_isp_device *isp_dev, 66 struct sun6i_isp_buffer *isp_buffer) 67 { 68 const struct v4l2_format_info *info; 69 struct vb2_buffer *vb2_buffer; 70 unsigned int width, height; 71 unsigned int width_aligned; 72 dma_addr_t address; 73 u32 pixelformat; 74 75 vb2_buffer = &isp_buffer->v4l2_buffer.vb2_buf; 76 address = vb2_dma_contig_plane_dma_addr(vb2_buffer, 0); 77 78 sun6i_isp_load_write(isp_dev, SUN6I_ISP_MCH_Y_ADDR0_REG, 79 SUN6I_ISP_ADDR_VALUE(address)); 80 81 sun6i_isp_capture_dimensions(isp_dev, &width, &height); 82 sun6i_isp_capture_format(isp_dev, &pixelformat); 83 84 info = v4l2_format_info(pixelformat); 85 if (WARN_ON(!info)) 86 return; 87 88 /* Stride needs to be aligned to 4. */ 89 width_aligned = ALIGN(width, 2); 90 91 if (info->comp_planes > 1) { 92 address += info->bpp[0] * width_aligned * height; 93 94 sun6i_isp_load_write(isp_dev, SUN6I_ISP_MCH_U_ADDR0_REG, 95 SUN6I_ISP_ADDR_VALUE(address)); 96 } 97 98 if (info->comp_planes > 2) { 99 address += info->bpp[1] * 100 DIV_ROUND_UP(width_aligned, info->hdiv) * 101 DIV_ROUND_UP(height, info->vdiv); 102 103 sun6i_isp_load_write(isp_dev, SUN6I_ISP_MCH_V_ADDR0_REG, 104 SUN6I_ISP_ADDR_VALUE(address)); 105 } 106 } 107 108 void sun6i_isp_capture_configure(struct sun6i_isp_device *isp_dev) 109 { 110 unsigned int width, height; 111 unsigned int stride_luma, stride_chroma = 0; 112 unsigned int stride_luma_div4, stride_chroma_div4; 113 const struct sun6i_isp_capture_format *format; 114 const struct v4l2_format_info *info; 115 u32 pixelformat; 116 117 sun6i_isp_capture_dimensions(isp_dev, &width, &height); 118 sun6i_isp_capture_format(isp_dev, &pixelformat); 119 120 format = sun6i_isp_capture_format_find(pixelformat); 121 if (WARN_ON(!format)) 122 return; 123 124 sun6i_isp_load_write(isp_dev, SUN6I_ISP_MCH_SIZE_CFG_REG, 125 SUN6I_ISP_MCH_SIZE_CFG_WIDTH(width) | 126 SUN6I_ISP_MCH_SIZE_CFG_HEIGHT(height)); 127 128 info = v4l2_format_info(pixelformat); 129 if (WARN_ON(!info)) 130 return; 131 132 stride_luma = width * info->bpp[0]; 133 stride_luma_div4 = DIV_ROUND_UP(stride_luma, 4); 134 135 if (info->comp_planes > 1) { 136 stride_chroma = width * info->bpp[1] / info->hdiv; 137 stride_chroma_div4 = DIV_ROUND_UP(stride_chroma, 4); 138 } 139 140 sun6i_isp_load_write(isp_dev, SUN6I_ISP_MCH_CFG_REG, 141 SUN6I_ISP_MCH_CFG_EN | 142 SUN6I_ISP_MCH_CFG_OUTPUT_FMT(format->output_format) | 143 SUN6I_ISP_MCH_CFG_STRIDE_Y_DIV4(stride_luma_div4) | 144 SUN6I_ISP_MCH_CFG_STRIDE_UV_DIV4(stride_chroma_div4)); 145 } 146 147 /* State */ 148 149 static void sun6i_isp_capture_state_cleanup(struct sun6i_isp_device *isp_dev, 150 bool error) 151 { 152 struct sun6i_isp_capture_state *state = &isp_dev->capture.state; 153 struct sun6i_isp_buffer **isp_buffer_states[] = { 154 &state->pending, &state->current, &state->complete, 155 }; 156 struct sun6i_isp_buffer *isp_buffer; 157 struct vb2_buffer *vb2_buffer; 158 unsigned long flags; 159 unsigned int i; 160 161 spin_lock_irqsave(&state->lock, flags); 162 163 for (i = 0; i < ARRAY_SIZE(isp_buffer_states); i++) { 164 isp_buffer = *isp_buffer_states[i]; 165 if (!isp_buffer) 166 continue; 167 168 vb2_buffer = &isp_buffer->v4l2_buffer.vb2_buf; 169 vb2_buffer_done(vb2_buffer, error ? VB2_BUF_STATE_ERROR : 170 VB2_BUF_STATE_QUEUED); 171 172 *isp_buffer_states[i] = NULL; 173 } 174 175 list_for_each_entry(isp_buffer, &state->queue, list) { 176 vb2_buffer = &isp_buffer->v4l2_buffer.vb2_buf; 177 vb2_buffer_done(vb2_buffer, error ? VB2_BUF_STATE_ERROR : 178 VB2_BUF_STATE_QUEUED); 179 } 180 181 INIT_LIST_HEAD(&state->queue); 182 183 spin_unlock_irqrestore(&state->lock, flags); 184 } 185 186 void sun6i_isp_capture_state_update(struct sun6i_isp_device *isp_dev, 187 bool *update) 188 { 189 struct sun6i_isp_capture_state *state = &isp_dev->capture.state; 190 struct sun6i_isp_buffer *isp_buffer; 191 unsigned long flags; 192 193 spin_lock_irqsave(&state->lock, flags); 194 195 if (list_empty(&state->queue)) 196 goto complete; 197 198 if (state->pending) 199 goto complete; 200 201 isp_buffer = list_first_entry(&state->queue, struct sun6i_isp_buffer, 202 list); 203 204 sun6i_isp_capture_buffer_configure(isp_dev, isp_buffer); 205 206 list_del(&isp_buffer->list); 207 208 state->pending = isp_buffer; 209 210 if (update) 211 *update = true; 212 213 complete: 214 spin_unlock_irqrestore(&state->lock, flags); 215 } 216 217 void sun6i_isp_capture_state_complete(struct sun6i_isp_device *isp_dev) 218 { 219 struct sun6i_isp_capture_state *state = &isp_dev->capture.state; 220 unsigned long flags; 221 222 spin_lock_irqsave(&state->lock, flags); 223 224 if (!state->pending) 225 goto complete; 226 227 state->complete = state->current; 228 state->current = state->pending; 229 state->pending = NULL; 230 231 if (state->complete) { 232 struct sun6i_isp_buffer *isp_buffer = state->complete; 233 struct vb2_buffer *vb2_buffer = 234 &isp_buffer->v4l2_buffer.vb2_buf; 235 236 vb2_buffer->timestamp = ktime_get_ns(); 237 isp_buffer->v4l2_buffer.sequence = state->sequence; 238 239 vb2_buffer_done(vb2_buffer, VB2_BUF_STATE_DONE); 240 241 state->complete = NULL; 242 } 243 244 complete: 245 spin_unlock_irqrestore(&state->lock, flags); 246 } 247 248 void sun6i_isp_capture_finish(struct sun6i_isp_device *isp_dev) 249 { 250 struct sun6i_isp_capture_state *state = &isp_dev->capture.state; 251 unsigned long flags; 252 253 spin_lock_irqsave(&state->lock, flags); 254 state->sequence++; 255 spin_unlock_irqrestore(&state->lock, flags); 256 } 257 258 /* Queue */ 259 260 static int sun6i_isp_capture_queue_setup(struct vb2_queue *queue, 261 unsigned int *buffers_count, 262 unsigned int *planes_count, 263 unsigned int sizes[], 264 struct device *alloc_devs[]) 265 { 266 struct sun6i_isp_device *isp_dev = vb2_get_drv_priv(queue); 267 unsigned int size = isp_dev->capture.format.fmt.pix.sizeimage; 268 269 if (*planes_count) 270 return sizes[0] < size ? -EINVAL : 0; 271 272 *planes_count = 1; 273 sizes[0] = size; 274 275 return 0; 276 } 277 278 static int sun6i_isp_capture_buffer_prepare(struct vb2_buffer *vb2_buffer) 279 { 280 struct sun6i_isp_device *isp_dev = 281 vb2_get_drv_priv(vb2_buffer->vb2_queue); 282 struct v4l2_device *v4l2_dev = &isp_dev->v4l2.v4l2_dev; 283 unsigned int size = isp_dev->capture.format.fmt.pix.sizeimage; 284 285 if (vb2_plane_size(vb2_buffer, 0) < size) { 286 v4l2_err(v4l2_dev, "buffer too small (%lu < %u)\n", 287 vb2_plane_size(vb2_buffer, 0), size); 288 return -EINVAL; 289 } 290 291 vb2_set_plane_payload(vb2_buffer, 0, size); 292 293 return 0; 294 } 295 296 static void sun6i_isp_capture_buffer_queue(struct vb2_buffer *vb2_buffer) 297 { 298 struct sun6i_isp_device *isp_dev = 299 vb2_get_drv_priv(vb2_buffer->vb2_queue); 300 struct sun6i_isp_capture_state *state = &isp_dev->capture.state; 301 struct vb2_v4l2_buffer *v4l2_buffer = to_vb2_v4l2_buffer(vb2_buffer); 302 struct sun6i_isp_buffer *isp_buffer = 303 container_of(v4l2_buffer, struct sun6i_isp_buffer, v4l2_buffer); 304 unsigned long flags; 305 306 spin_lock_irqsave(&state->lock, flags); 307 list_add_tail(&isp_buffer->list, &state->queue); 308 spin_unlock_irqrestore(&state->lock, flags); 309 310 /* Update the state to schedule our buffer as soon as possible. */ 311 if (state->streaming) 312 sun6i_isp_state_update(isp_dev, false); 313 } 314 315 static int sun6i_isp_capture_start_streaming(struct vb2_queue *queue, 316 unsigned int count) 317 { 318 struct sun6i_isp_device *isp_dev = vb2_get_drv_priv(queue); 319 struct sun6i_isp_capture_state *state = &isp_dev->capture.state; 320 struct video_device *video_dev = &isp_dev->capture.video_dev; 321 struct v4l2_subdev *subdev = &isp_dev->proc.subdev; 322 int ret; 323 324 state->sequence = 0; 325 326 ret = video_device_pipeline_alloc_start(video_dev); 327 if (ret < 0) 328 goto error_state; 329 330 state->streaming = true; 331 332 ret = v4l2_subdev_call(subdev, video, s_stream, 1); 333 if (ret && ret != -ENOIOCTLCMD) 334 goto error_streaming; 335 336 return 0; 337 338 error_streaming: 339 state->streaming = false; 340 341 video_device_pipeline_stop(video_dev); 342 343 error_state: 344 sun6i_isp_capture_state_cleanup(isp_dev, false); 345 346 return ret; 347 } 348 349 static void sun6i_isp_capture_stop_streaming(struct vb2_queue *queue) 350 { 351 struct sun6i_isp_device *isp_dev = vb2_get_drv_priv(queue); 352 struct sun6i_isp_capture_state *state = &isp_dev->capture.state; 353 struct video_device *video_dev = &isp_dev->capture.video_dev; 354 struct v4l2_subdev *subdev = &isp_dev->proc.subdev; 355 356 v4l2_subdev_call(subdev, video, s_stream, 0); 357 358 state->streaming = false; 359 360 video_device_pipeline_stop(video_dev); 361 362 sun6i_isp_capture_state_cleanup(isp_dev, true); 363 } 364 365 static const struct vb2_ops sun6i_isp_capture_queue_ops = { 366 .queue_setup = sun6i_isp_capture_queue_setup, 367 .buf_prepare = sun6i_isp_capture_buffer_prepare, 368 .buf_queue = sun6i_isp_capture_buffer_queue, 369 .start_streaming = sun6i_isp_capture_start_streaming, 370 .stop_streaming = sun6i_isp_capture_stop_streaming, 371 .wait_prepare = vb2_ops_wait_prepare, 372 .wait_finish = vb2_ops_wait_finish, 373 }; 374 375 /* Video Device */ 376 377 static void sun6i_isp_capture_format_prepare(struct v4l2_format *format) 378 { 379 struct v4l2_pix_format *pix_format = &format->fmt.pix; 380 const struct v4l2_format_info *info; 381 unsigned int width, height; 382 unsigned int width_aligned; 383 unsigned int i; 384 385 v4l_bound_align_image(&pix_format->width, SUN6I_ISP_CAPTURE_WIDTH_MIN, 386 SUN6I_ISP_CAPTURE_WIDTH_MAX, 1, 387 &pix_format->height, SUN6I_ISP_CAPTURE_HEIGHT_MIN, 388 SUN6I_ISP_CAPTURE_HEIGHT_MAX, 1, 0); 389 390 if (!sun6i_isp_capture_format_find(pix_format->pixelformat)) 391 pix_format->pixelformat = 392 sun6i_isp_capture_formats[0].pixelformat; 393 394 info = v4l2_format_info(pix_format->pixelformat); 395 if (WARN_ON(!info)) 396 return; 397 398 width = pix_format->width; 399 height = pix_format->height; 400 401 /* Stride needs to be aligned to 4. */ 402 width_aligned = ALIGN(width, 2); 403 404 pix_format->bytesperline = width_aligned * info->bpp[0]; 405 pix_format->sizeimage = 0; 406 407 for (i = 0; i < info->comp_planes; i++) { 408 unsigned int hdiv = (i == 0) ? 1 : info->hdiv; 409 unsigned int vdiv = (i == 0) ? 1 : info->vdiv; 410 411 pix_format->sizeimage += info->bpp[i] * 412 DIV_ROUND_UP(width_aligned, hdiv) * 413 DIV_ROUND_UP(height, vdiv); 414 } 415 416 pix_format->field = V4L2_FIELD_NONE; 417 418 pix_format->colorspace = V4L2_COLORSPACE_RAW; 419 pix_format->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT; 420 pix_format->quantization = V4L2_QUANTIZATION_DEFAULT; 421 pix_format->xfer_func = V4L2_XFER_FUNC_DEFAULT; 422 } 423 424 static int sun6i_isp_capture_querycap(struct file *file, void *private, 425 struct v4l2_capability *capability) 426 { 427 struct sun6i_isp_device *isp_dev = video_drvdata(file); 428 struct video_device *video_dev = &isp_dev->capture.video_dev; 429 430 strscpy(capability->driver, SUN6I_ISP_NAME, sizeof(capability->driver)); 431 strscpy(capability->card, video_dev->name, sizeof(capability->card)); 432 snprintf(capability->bus_info, sizeof(capability->bus_info), 433 "platform:%s", dev_name(isp_dev->dev)); 434 435 return 0; 436 } 437 438 static int sun6i_isp_capture_enum_fmt(struct file *file, void *private, 439 struct v4l2_fmtdesc *fmtdesc) 440 { 441 u32 index = fmtdesc->index; 442 443 if (index >= ARRAY_SIZE(sun6i_isp_capture_formats)) 444 return -EINVAL; 445 446 fmtdesc->pixelformat = sun6i_isp_capture_formats[index].pixelformat; 447 448 return 0; 449 } 450 451 static int sun6i_isp_capture_g_fmt(struct file *file, void *private, 452 struct v4l2_format *format) 453 { 454 struct sun6i_isp_device *isp_dev = video_drvdata(file); 455 456 *format = isp_dev->capture.format; 457 458 return 0; 459 } 460 461 static int sun6i_isp_capture_s_fmt(struct file *file, void *private, 462 struct v4l2_format *format) 463 { 464 struct sun6i_isp_device *isp_dev = video_drvdata(file); 465 466 if (vb2_is_busy(&isp_dev->capture.queue)) 467 return -EBUSY; 468 469 sun6i_isp_capture_format_prepare(format); 470 471 isp_dev->capture.format = *format; 472 473 return 0; 474 } 475 476 static int sun6i_isp_capture_try_fmt(struct file *file, void *private, 477 struct v4l2_format *format) 478 { 479 sun6i_isp_capture_format_prepare(format); 480 481 return 0; 482 } 483 484 static int sun6i_isp_capture_enum_input(struct file *file, void *private, 485 struct v4l2_input *input) 486 { 487 if (input->index != 0) 488 return -EINVAL; 489 490 input->type = V4L2_INPUT_TYPE_CAMERA; 491 strscpy(input->name, "Camera", sizeof(input->name)); 492 493 return 0; 494 } 495 496 static int sun6i_isp_capture_g_input(struct file *file, void *private, 497 unsigned int *index) 498 { 499 *index = 0; 500 501 return 0; 502 } 503 504 static int sun6i_isp_capture_s_input(struct file *file, void *private, 505 unsigned int index) 506 { 507 if (index != 0) 508 return -EINVAL; 509 510 return 0; 511 } 512 513 static const struct v4l2_ioctl_ops sun6i_isp_capture_ioctl_ops = { 514 .vidioc_querycap = sun6i_isp_capture_querycap, 515 516 .vidioc_enum_fmt_vid_cap = sun6i_isp_capture_enum_fmt, 517 .vidioc_g_fmt_vid_cap = sun6i_isp_capture_g_fmt, 518 .vidioc_s_fmt_vid_cap = sun6i_isp_capture_s_fmt, 519 .vidioc_try_fmt_vid_cap = sun6i_isp_capture_try_fmt, 520 521 .vidioc_enum_input = sun6i_isp_capture_enum_input, 522 .vidioc_g_input = sun6i_isp_capture_g_input, 523 .vidioc_s_input = sun6i_isp_capture_s_input, 524 525 .vidioc_create_bufs = vb2_ioctl_create_bufs, 526 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 527 .vidioc_reqbufs = vb2_ioctl_reqbufs, 528 .vidioc_querybuf = vb2_ioctl_querybuf, 529 .vidioc_expbuf = vb2_ioctl_expbuf, 530 .vidioc_qbuf = vb2_ioctl_qbuf, 531 .vidioc_dqbuf = vb2_ioctl_dqbuf, 532 .vidioc_streamon = vb2_ioctl_streamon, 533 .vidioc_streamoff = vb2_ioctl_streamoff, 534 }; 535 536 static int sun6i_isp_capture_open(struct file *file) 537 { 538 struct sun6i_isp_device *isp_dev = video_drvdata(file); 539 struct video_device *video_dev = &isp_dev->capture.video_dev; 540 struct mutex *lock = &isp_dev->capture.lock; 541 int ret; 542 543 if (mutex_lock_interruptible(lock)) 544 return -ERESTARTSYS; 545 546 ret = v4l2_pipeline_pm_get(&video_dev->entity); 547 if (ret) 548 goto error_mutex; 549 550 ret = v4l2_fh_open(file); 551 if (ret) 552 goto error_pipeline; 553 554 mutex_unlock(lock); 555 556 return 0; 557 558 error_pipeline: 559 v4l2_pipeline_pm_put(&video_dev->entity); 560 561 error_mutex: 562 mutex_unlock(lock); 563 564 return ret; 565 } 566 567 static int sun6i_isp_capture_release(struct file *file) 568 { 569 struct sun6i_isp_device *isp_dev = video_drvdata(file); 570 struct video_device *video_dev = &isp_dev->capture.video_dev; 571 struct mutex *lock = &isp_dev->capture.lock; 572 573 mutex_lock(lock); 574 575 _vb2_fop_release(file, NULL); 576 v4l2_pipeline_pm_put(&video_dev->entity); 577 578 mutex_unlock(lock); 579 580 return 0; 581 } 582 583 static const struct v4l2_file_operations sun6i_isp_capture_fops = { 584 .owner = THIS_MODULE, 585 .open = sun6i_isp_capture_open, 586 .release = sun6i_isp_capture_release, 587 .unlocked_ioctl = video_ioctl2, 588 .poll = vb2_fop_poll, 589 .mmap = vb2_fop_mmap, 590 }; 591 592 /* Media Entity */ 593 594 static int sun6i_isp_capture_link_validate(struct media_link *link) 595 { 596 struct video_device *video_dev = 597 media_entity_to_video_device(link->sink->entity); 598 struct sun6i_isp_device *isp_dev = video_get_drvdata(video_dev); 599 struct v4l2_device *v4l2_dev = &isp_dev->v4l2.v4l2_dev; 600 unsigned int capture_width, capture_height; 601 unsigned int proc_width, proc_height; 602 603 sun6i_isp_capture_dimensions(isp_dev, &capture_width, &capture_height); 604 sun6i_isp_proc_dimensions(isp_dev, &proc_width, &proc_height); 605 606 /* No cropping/scaling is supported (yet). */ 607 if (capture_width != proc_width || capture_height != proc_height) { 608 v4l2_err(v4l2_dev, 609 "invalid input/output dimensions: %ux%u/%ux%u\n", 610 proc_width, proc_height, capture_width, 611 capture_height); 612 return -EINVAL; 613 } 614 615 return 0; 616 } 617 618 static const struct media_entity_operations sun6i_isp_capture_entity_ops = { 619 .link_validate = sun6i_isp_capture_link_validate, 620 }; 621 622 /* Capture */ 623 624 int sun6i_isp_capture_setup(struct sun6i_isp_device *isp_dev) 625 { 626 struct sun6i_isp_capture *capture = &isp_dev->capture; 627 struct sun6i_isp_capture_state *state = &capture->state; 628 struct v4l2_device *v4l2_dev = &isp_dev->v4l2.v4l2_dev; 629 struct v4l2_subdev *proc_subdev = &isp_dev->proc.subdev; 630 struct video_device *video_dev = &capture->video_dev; 631 struct vb2_queue *queue = &capture->queue; 632 struct media_pad *pad = &capture->pad; 633 struct v4l2_format *format = &capture->format; 634 struct v4l2_pix_format *pix_format = &format->fmt.pix; 635 int ret; 636 637 /* State */ 638 639 INIT_LIST_HEAD(&state->queue); 640 spin_lock_init(&state->lock); 641 642 /* Media Entity */ 643 644 video_dev->entity.ops = &sun6i_isp_capture_entity_ops; 645 646 /* Media Pads */ 647 648 pad->flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT; 649 650 ret = media_entity_pads_init(&video_dev->entity, 1, pad); 651 if (ret) 652 goto error_mutex; 653 654 /* Queue */ 655 656 mutex_init(&capture->lock); 657 658 queue->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 659 queue->io_modes = VB2_MMAP | VB2_DMABUF; 660 queue->buf_struct_size = sizeof(struct sun6i_isp_buffer); 661 queue->ops = &sun6i_isp_capture_queue_ops; 662 queue->mem_ops = &vb2_dma_contig_memops; 663 queue->min_buffers_needed = 2; 664 queue->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 665 queue->lock = &capture->lock; 666 queue->dev = isp_dev->dev; 667 queue->drv_priv = isp_dev; 668 669 ret = vb2_queue_init(queue); 670 if (ret) { 671 v4l2_err(v4l2_dev, "failed to initialize vb2 queue: %d\n", ret); 672 goto error_media_entity; 673 } 674 675 /* V4L2 Format */ 676 677 format->type = queue->type; 678 pix_format->pixelformat = sun6i_isp_capture_formats[0].pixelformat; 679 pix_format->width = 1280; 680 pix_format->height = 720; 681 682 sun6i_isp_capture_format_prepare(format); 683 684 /* Video Device */ 685 686 strscpy(video_dev->name, SUN6I_ISP_CAPTURE_NAME, 687 sizeof(video_dev->name)); 688 video_dev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING; 689 video_dev->vfl_dir = VFL_DIR_RX; 690 video_dev->release = video_device_release_empty; 691 video_dev->fops = &sun6i_isp_capture_fops; 692 video_dev->ioctl_ops = &sun6i_isp_capture_ioctl_ops; 693 video_dev->v4l2_dev = v4l2_dev; 694 video_dev->queue = queue; 695 video_dev->lock = &capture->lock; 696 697 video_set_drvdata(video_dev, isp_dev); 698 699 ret = video_register_device(video_dev, VFL_TYPE_VIDEO, -1); 700 if (ret) { 701 v4l2_err(v4l2_dev, "failed to register video device: %d\n", 702 ret); 703 goto error_media_entity; 704 } 705 706 /* Media Pad Link */ 707 708 ret = media_create_pad_link(&proc_subdev->entity, 709 SUN6I_ISP_PROC_PAD_SOURCE, 710 &video_dev->entity, 0, 711 MEDIA_LNK_FL_ENABLED | 712 MEDIA_LNK_FL_IMMUTABLE); 713 if (ret < 0) { 714 v4l2_err(v4l2_dev, "failed to create %s:%u -> %s:%u link\n", 715 proc_subdev->entity.name, SUN6I_ISP_PROC_PAD_SOURCE, 716 video_dev->entity.name, 0); 717 goto error_video_device; 718 } 719 720 return 0; 721 722 error_video_device: 723 vb2_video_unregister_device(video_dev); 724 725 error_media_entity: 726 media_entity_cleanup(&video_dev->entity); 727 728 error_mutex: 729 mutex_destroy(&capture->lock); 730 731 return ret; 732 } 733 734 void sun6i_isp_capture_cleanup(struct sun6i_isp_device *isp_dev) 735 { 736 struct sun6i_isp_capture *capture = &isp_dev->capture; 737 struct video_device *video_dev = &capture->video_dev; 738 739 vb2_video_unregister_device(video_dev); 740 media_entity_cleanup(&video_dev->entity); 741 mutex_destroy(&capture->lock); 742 } 743