1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Rockchip Camera Interface (CIF) Driver 4 * 5 * Copyright (C) 2025 Michael Riesch <michael.riesch@wolfvision.net> 6 * Copyright (C) 2025 Collabora, Ltd. 7 */ 8 9 #include <linux/pm_runtime.h> 10 11 #include <media/v4l2-common.h> 12 #include <media/v4l2-fwnode.h> 13 #include <media/v4l2-ioctl.h> 14 #include <media/v4l2-mc.h> 15 #include <media/v4l2-subdev.h> 16 #include <media/videobuf2-dma-contig.h> 17 18 #include "rkcif-common.h" 19 #include "rkcif-stream.h" 20 21 #define CIF_REQ_BUFS_MIN 1 22 #define CIF_MIN_WIDTH 64 23 #define CIF_MIN_HEIGHT 64 24 #define CIF_MAX_WIDTH 8192 25 #define CIF_MAX_HEIGHT 8192 26 27 static inline struct rkcif_buffer *to_rkcif_buffer(struct vb2_v4l2_buffer *vb) 28 { 29 return container_of(vb, struct rkcif_buffer, vb); 30 } 31 32 static inline struct rkcif_stream *to_rkcif_stream(struct video_device *vdev) 33 { 34 return container_of(vdev, struct rkcif_stream, vdev); 35 } 36 37 static struct rkcif_buffer *rkcif_stream_pop_buffer(struct rkcif_stream *stream) 38 { 39 struct rkcif_buffer *buffer; 40 41 guard(spinlock_irqsave)(&stream->driver_queue_lock); 42 43 if (list_empty(&stream->driver_queue)) 44 return NULL; 45 46 buffer = list_first_entry(&stream->driver_queue, struct rkcif_buffer, 47 queue); 48 list_del(&buffer->queue); 49 50 return buffer; 51 } 52 53 static void rkcif_stream_push_buffer(struct rkcif_stream *stream, 54 struct rkcif_buffer *buffer) 55 { 56 guard(spinlock_irqsave)(&stream->driver_queue_lock); 57 58 list_add_tail(&buffer->queue, &stream->driver_queue); 59 } 60 61 static inline void rkcif_stream_return_buffer(struct rkcif_buffer *buffer, 62 enum vb2_buffer_state state) 63 { 64 struct vb2_v4l2_buffer *vb = &buffer->vb; 65 66 vb2_buffer_done(&vb->vb2_buf, state); 67 } 68 69 static void rkcif_stream_complete_buffer(struct rkcif_stream *stream, 70 struct rkcif_buffer *buffer) 71 { 72 struct vb2_v4l2_buffer *vb = &buffer->vb; 73 74 vb->vb2_buf.timestamp = ktime_get_ns(); 75 vb->sequence = stream->frame_idx; 76 vb2_buffer_done(&vb->vb2_buf, VB2_BUF_STATE_DONE); 77 stream->frame_idx++; 78 } 79 80 void rkcif_stream_pingpong(struct rkcif_stream *stream) 81 { 82 struct rkcif_buffer *buffer; 83 84 buffer = stream->buffers[stream->frame_phase]; 85 if (!buffer->is_dummy) 86 rkcif_stream_complete_buffer(stream, buffer); 87 88 buffer = rkcif_stream_pop_buffer(stream); 89 if (buffer) { 90 stream->buffers[stream->frame_phase] = buffer; 91 stream->buffers[stream->frame_phase]->is_dummy = false; 92 } else { 93 stream->buffers[stream->frame_phase] = &stream->dummy.buffer; 94 stream->buffers[stream->frame_phase]->is_dummy = true; 95 dev_dbg(stream->rkcif->dev, 96 "no buffer available, frame will be dropped\n"); 97 } 98 99 if (stream->queue_buffer) 100 stream->queue_buffer(stream, stream->frame_phase); 101 102 stream->frame_phase = 1 - stream->frame_phase; 103 } 104 105 static int rkcif_stream_init_buffers(struct rkcif_stream *stream) 106 { 107 struct v4l2_pix_format_mplane *pix = &stream->pix; 108 109 stream->buffers[0] = rkcif_stream_pop_buffer(stream); 110 if (!stream->buffers[0]) 111 goto err_buff_0; 112 113 stream->buffers[1] = rkcif_stream_pop_buffer(stream); 114 if (!stream->buffers[1]) 115 goto err_buff_1; 116 117 if (stream->queue_buffer) { 118 stream->queue_buffer(stream, 0); 119 stream->queue_buffer(stream, 1); 120 } 121 122 stream->dummy.size = pix->num_planes * pix->plane_fmt[0].sizeimage; 123 stream->dummy.vaddr = 124 dma_alloc_attrs(stream->rkcif->dev, stream->dummy.size, 125 &stream->dummy.buffer.buff_addr[0], GFP_KERNEL, 126 DMA_ATTR_NO_KERNEL_MAPPING); 127 if (!stream->dummy.vaddr) 128 goto err_dummy; 129 130 for (unsigned int i = 1; i < pix->num_planes; i++) 131 stream->dummy.buffer.buff_addr[i] = 132 stream->dummy.buffer.buff_addr[i - 1] + 133 pix->plane_fmt[i - 1].bytesperline * pix->height; 134 135 return 0; 136 137 err_dummy: 138 rkcif_stream_return_buffer(stream->buffers[1], VB2_BUF_STATE_QUEUED); 139 stream->buffers[1] = NULL; 140 141 err_buff_1: 142 rkcif_stream_return_buffer(stream->buffers[0], VB2_BUF_STATE_QUEUED); 143 stream->buffers[0] = NULL; 144 err_buff_0: 145 return -EINVAL; 146 } 147 148 static void rkcif_stream_return_all_buffers(struct rkcif_stream *stream, 149 enum vb2_buffer_state state) 150 { 151 struct rkcif_buffer *buffer; 152 153 if (stream->buffers[0] && !stream->buffers[0]->is_dummy) { 154 rkcif_stream_return_buffer(stream->buffers[0], state); 155 stream->buffers[0] = NULL; 156 } 157 158 if (stream->buffers[1] && !stream->buffers[1]->is_dummy) { 159 rkcif_stream_return_buffer(stream->buffers[1], state); 160 stream->buffers[1] = NULL; 161 } 162 163 while ((buffer = rkcif_stream_pop_buffer(stream))) 164 rkcif_stream_return_buffer(buffer, state); 165 166 if (stream->dummy.vaddr) { 167 dma_free_attrs(stream->rkcif->dev, stream->dummy.size, 168 stream->dummy.vaddr, 169 stream->dummy.buffer.buff_addr[0], 170 DMA_ATTR_NO_KERNEL_MAPPING); 171 stream->dummy.vaddr = NULL; 172 } 173 } 174 175 static int rkcif_stream_setup_queue(struct vb2_queue *queue, 176 unsigned int *num_buffers, 177 unsigned int *num_planes, 178 unsigned int sizes[], 179 struct device *alloc_devs[]) 180 { 181 struct rkcif_stream *stream = queue->drv_priv; 182 struct v4l2_pix_format_mplane *pix = &stream->pix; 183 184 if (*num_planes) { 185 if (*num_planes != pix->num_planes) 186 return -EINVAL; 187 188 for (unsigned int i = 0; i < pix->num_planes; i++) 189 if (sizes[i] < pix->plane_fmt[i].sizeimage) 190 return -EINVAL; 191 } else { 192 *num_planes = pix->num_planes; 193 for (unsigned int i = 0; i < pix->num_planes; i++) 194 sizes[i] = pix->plane_fmt[i].sizeimage; 195 } 196 197 return 0; 198 } 199 200 static int rkcif_stream_prepare_buffer(struct vb2_buffer *vb) 201 { 202 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 203 struct rkcif_buffer *buffer = to_rkcif_buffer(vbuf); 204 struct rkcif_stream *stream = vb->vb2_queue->drv_priv; 205 const struct rkcif_output_fmt *fmt; 206 struct v4l2_pix_format_mplane *pix = &stream->pix; 207 unsigned int i; 208 209 memset(buffer->buff_addr, 0, sizeof(buffer->buff_addr)); 210 for (i = 0; i < pix->num_planes; i++) 211 buffer->buff_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i); 212 213 /* apply fallback for non-mplane formats, if required */ 214 if (pix->num_planes == 1) { 215 fmt = rkcif_stream_find_output_fmt(stream, true, 216 pix->pixelformat); 217 for (i = 1; i < fmt->cplanes; i++) 218 buffer->buff_addr[i] = 219 buffer->buff_addr[i - 1] + 220 pix->plane_fmt[i - 1].bytesperline * 221 pix->height; 222 } 223 224 for (i = 0; i < pix->num_planes; i++) { 225 unsigned long size = pix->plane_fmt[i].sizeimage; 226 227 if (vb2_plane_size(vb, i) < size) { 228 dev_err(stream->rkcif->dev, 229 "user buffer too small (%ld < %ld)\n", 230 vb2_plane_size(vb, i), size); 231 return -EINVAL; 232 } 233 234 vb2_set_plane_payload(vb, i, size); 235 } 236 237 vbuf->field = V4L2_FIELD_NONE; 238 239 return 0; 240 } 241 242 static void rkcif_stream_queue_buffer(struct vb2_buffer *vb) 243 { 244 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 245 struct rkcif_buffer *buffer = to_rkcif_buffer(vbuf); 246 struct rkcif_stream *stream = vb->vb2_queue->drv_priv; 247 248 rkcif_stream_push_buffer(stream, buffer); 249 } 250 251 static int rkcif_stream_start_streaming(struct vb2_queue *queue, 252 unsigned int count) 253 { 254 struct rkcif_stream *stream = queue->drv_priv; 255 struct rkcif_device *rkcif = stream->rkcif; 256 u64 mask; 257 int ret; 258 259 stream->frame_idx = 0; 260 stream->frame_phase = 0; 261 262 ret = video_device_pipeline_start(&stream->vdev, &stream->pipeline); 263 if (ret) { 264 dev_err(rkcif->dev, "failed to start pipeline %d\n", ret); 265 goto err_out; 266 } 267 268 ret = pm_runtime_resume_and_get(rkcif->dev); 269 if (ret < 0) { 270 dev_err(rkcif->dev, "failed to get runtime pm, %d\n", ret); 271 goto err_pipeline_stop; 272 } 273 274 ret = rkcif_stream_init_buffers(stream); 275 if (ret) 276 goto err_runtime_put; 277 278 if (stream->start_streaming) { 279 ret = stream->start_streaming(stream); 280 if (ret < 0) 281 goto err_runtime_put; 282 } 283 284 mask = BIT_ULL(stream->id); 285 ret = v4l2_subdev_enable_streams(&stream->interface->sd, 286 RKCIF_IF_PAD_SRC, mask); 287 if (ret < 0) 288 goto err_stop_stream; 289 290 return 0; 291 292 err_stop_stream: 293 if (stream->stop_streaming) 294 stream->stop_streaming(stream); 295 err_runtime_put: 296 pm_runtime_put(rkcif->dev); 297 err_pipeline_stop: 298 video_device_pipeline_stop(&stream->vdev); 299 err_out: 300 rkcif_stream_return_all_buffers(stream, VB2_BUF_STATE_QUEUED); 301 return ret; 302 } 303 304 static void rkcif_stream_stop_streaming(struct vb2_queue *queue) 305 { 306 struct rkcif_stream *stream = queue->drv_priv; 307 struct rkcif_device *rkcif = stream->rkcif; 308 u64 mask; 309 int ret; 310 311 mask = BIT_ULL(stream->id); 312 v4l2_subdev_disable_streams(&stream->interface->sd, RKCIF_IF_PAD_SRC, 313 mask); 314 315 stream->stopping = true; 316 ret = wait_event_timeout(stream->wq_stopped, !stream->stopping, 317 msecs_to_jiffies(1000)); 318 319 if (!ret && stream->stop_streaming) 320 stream->stop_streaming(stream); 321 322 pm_runtime_put(rkcif->dev); 323 324 rkcif_stream_return_all_buffers(stream, VB2_BUF_STATE_ERROR); 325 326 video_device_pipeline_stop(&stream->vdev); 327 } 328 329 static const struct vb2_ops rkcif_stream_vb2_ops = { 330 .queue_setup = rkcif_stream_setup_queue, 331 .buf_prepare = rkcif_stream_prepare_buffer, 332 .buf_queue = rkcif_stream_queue_buffer, 333 .start_streaming = rkcif_stream_start_streaming, 334 .stop_streaming = rkcif_stream_stop_streaming, 335 }; 336 337 static int rkcif_stream_fill_format(struct rkcif_stream *stream, 338 struct v4l2_pix_format_mplane *pix) 339 { 340 const struct rkcif_output_fmt *fmt; 341 u32 height, width; 342 int ret; 343 344 fmt = rkcif_stream_find_output_fmt(stream, true, pix->pixelformat); 345 height = clamp_t(u32, pix->height, CIF_MIN_HEIGHT, CIF_MAX_HEIGHT); 346 width = clamp_t(u32, pix->width, CIF_MIN_WIDTH, CIF_MAX_WIDTH); 347 ret = v4l2_fill_pixfmt_mp(pix, fmt->fourcc, width, height); 348 if (ret) 349 return ret; 350 351 pix->field = V4L2_FIELD_NONE; 352 353 return 0; 354 } 355 356 static int rkcif_stream_try_format(struct file *file, void *fh, 357 struct v4l2_format *f) 358 { 359 struct rkcif_stream *stream = video_drvdata(file); 360 struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp; 361 362 return rkcif_stream_fill_format(stream, pix); 363 } 364 365 static int rkcif_stream_set_format(struct file *file, void *priv, 366 struct v4l2_format *f) 367 { 368 struct rkcif_stream *stream = video_drvdata(file); 369 struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp; 370 int ret; 371 372 if (vb2_is_busy(&stream->buf_queue)) 373 return -EBUSY; 374 375 ret = rkcif_stream_try_format(file, priv, f); 376 if (ret) 377 return ret; 378 379 stream->pix = *pix; 380 381 return 0; 382 } 383 384 static int rkcif_stream_get_format(struct file *file, void *fh, 385 struct v4l2_format *f) 386 { 387 struct rkcif_stream *stream = video_drvdata(file); 388 389 f->fmt.pix_mp = stream->pix; 390 391 return 0; 392 } 393 394 static int rkcif_stream_enum_formats(struct file *file, void *priv, 395 struct v4l2_fmtdesc *f) 396 { 397 struct rkcif_stream *stream = video_drvdata(file); 398 399 if (f->index >= stream->out_fmts_num) 400 return -EINVAL; 401 402 f->pixelformat = stream->out_fmts[f->index].fourcc; 403 404 return 0; 405 } 406 407 static int rkcif_stream_enum_framesizes(struct file *file, void *fh, 408 struct v4l2_frmsizeenum *fsize) 409 { 410 struct rkcif_stream *stream = video_drvdata(file); 411 412 if (fsize->index > 0) 413 return -EINVAL; 414 415 if (!rkcif_stream_find_output_fmt(stream, false, fsize->pixel_format)) 416 return -EINVAL; 417 418 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE; 419 fsize->stepwise.min_width = CIF_MIN_WIDTH; 420 fsize->stepwise.max_width = CIF_MAX_WIDTH; 421 fsize->stepwise.step_width = 8; 422 fsize->stepwise.min_height = CIF_MIN_HEIGHT; 423 fsize->stepwise.max_height = CIF_MAX_HEIGHT; 424 fsize->stepwise.step_height = 8; 425 426 return 0; 427 } 428 429 static int rkcif_stream_querycap(struct file *file, void *priv, 430 struct v4l2_capability *cap) 431 { 432 struct rkcif_stream *stream = video_drvdata(file); 433 struct device *dev = stream->rkcif->dev; 434 435 strscpy(cap->driver, dev->driver->name, sizeof(cap->driver)); 436 strscpy(cap->card, dev->driver->name, sizeof(cap->card)); 437 438 return 0; 439 } 440 441 static const struct v4l2_ioctl_ops rkcif_stream_ioctl_ops = { 442 .vidioc_reqbufs = vb2_ioctl_reqbufs, 443 .vidioc_querybuf = vb2_ioctl_querybuf, 444 .vidioc_create_bufs = vb2_ioctl_create_bufs, 445 .vidioc_qbuf = vb2_ioctl_qbuf, 446 .vidioc_expbuf = vb2_ioctl_expbuf, 447 .vidioc_dqbuf = vb2_ioctl_dqbuf, 448 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 449 .vidioc_streamon = vb2_ioctl_streamon, 450 .vidioc_streamoff = vb2_ioctl_streamoff, 451 .vidioc_try_fmt_vid_cap_mplane = rkcif_stream_try_format, 452 .vidioc_s_fmt_vid_cap_mplane = rkcif_stream_set_format, 453 .vidioc_g_fmt_vid_cap_mplane = rkcif_stream_get_format, 454 .vidioc_enum_fmt_vid_cap = rkcif_stream_enum_formats, 455 .vidioc_enum_framesizes = rkcif_stream_enum_framesizes, 456 .vidioc_querycap = rkcif_stream_querycap, 457 }; 458 459 static int rkcif_stream_link_validate(struct media_link *link) 460 { 461 struct video_device *vdev = 462 media_entity_to_video_device(link->sink->entity); 463 struct v4l2_mbus_framefmt *source_fmt; 464 struct v4l2_subdev *sd; 465 struct v4l2_subdev_state *state; 466 struct rkcif_stream *stream = to_rkcif_stream(vdev); 467 int ret = -EINVAL; 468 469 if (!media_entity_remote_source_pad_unique(link->sink->entity)) 470 return -ENOTCONN; 471 472 sd = media_entity_to_v4l2_subdev(link->source->entity); 473 474 state = v4l2_subdev_lock_and_get_active_state(sd); 475 476 source_fmt = v4l2_subdev_state_get_format(state, link->source->index, 477 stream->id); 478 if (!source_fmt) 479 goto out; 480 481 if (source_fmt->height != stream->pix.height || 482 source_fmt->width != stream->pix.width) { 483 dev_dbg(stream->rkcif->dev, 484 "link '%s':%u -> '%s':%u not valid: %ux%u != %ux%u\n", 485 link->source->entity->name, link->source->index, 486 link->sink->entity->name, link->sink->index, 487 source_fmt->width, source_fmt->height, 488 stream->pix.width, stream->pix.height); 489 goto out; 490 } 491 492 ret = 0; 493 494 out: 495 v4l2_subdev_unlock_state(state); 496 return ret; 497 } 498 499 static const struct media_entity_operations rkcif_stream_media_ops = { 500 .link_validate = rkcif_stream_link_validate, 501 }; 502 503 static const struct v4l2_file_operations rkcif_stream_file_ops = { 504 .open = v4l2_fh_open, 505 .release = vb2_fop_release, 506 .unlocked_ioctl = video_ioctl2, 507 .poll = vb2_fop_poll, 508 .mmap = vb2_fop_mmap, 509 }; 510 511 static int rkcif_stream_init_vb2_queue(struct vb2_queue *q, 512 struct rkcif_stream *stream) 513 { 514 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; 515 q->io_modes = VB2_MMAP | VB2_DMABUF; 516 q->drv_priv = stream; 517 q->ops = &rkcif_stream_vb2_ops; 518 q->mem_ops = &vb2_dma_contig_memops; 519 q->buf_struct_size = sizeof(struct rkcif_buffer); 520 q->min_queued_buffers = CIF_REQ_BUFS_MIN; 521 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 522 q->lock = &stream->vlock; 523 q->dev = stream->rkcif->dev; 524 525 return vb2_queue_init(q); 526 } 527 528 int rkcif_stream_register(struct rkcif_device *rkcif, 529 struct rkcif_stream *stream) 530 { 531 struct rkcif_interface *interface = stream->interface; 532 struct v4l2_device *v4l2_dev = &rkcif->v4l2_dev; 533 struct video_device *vdev = &stream->vdev; 534 u32 link_flags = 0; 535 int ret; 536 537 stream->rkcif = rkcif; 538 539 INIT_LIST_HEAD(&stream->driver_queue); 540 spin_lock_init(&stream->driver_queue_lock); 541 542 init_waitqueue_head(&stream->wq_stopped); 543 544 mutex_init(&stream->vlock); 545 546 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING | 547 V4L2_CAP_IO_MC; 548 vdev->entity.ops = &rkcif_stream_media_ops; 549 vdev->fops = &rkcif_stream_file_ops; 550 vdev->ioctl_ops = &rkcif_stream_ioctl_ops; 551 vdev->lock = &stream->vlock; 552 vdev->minor = -1; 553 vdev->release = video_device_release_empty; 554 vdev->v4l2_dev = v4l2_dev; 555 vdev->vfl_dir = VFL_DIR_RX; 556 video_set_drvdata(vdev, stream); 557 558 stream->pad.flags = MEDIA_PAD_FL_SINK; 559 560 stream->pix.height = CIF_MIN_HEIGHT; 561 stream->pix.width = CIF_MIN_WIDTH; 562 rkcif_stream_fill_format(stream, &stream->pix); 563 564 rkcif_stream_init_vb2_queue(&stream->buf_queue, stream); 565 566 vdev->queue = &stream->buf_queue; 567 if (interface->type == RKCIF_IF_DVP) 568 snprintf(vdev->name, sizeof(vdev->name), "rkcif-dvp0-id%d", 569 stream->id); 570 else if (interface->type == RKCIF_IF_MIPI) 571 snprintf(vdev->name, sizeof(vdev->name), "rkcif-mipi%d-id%d", 572 interface->index - RKCIF_MIPI_BASE, stream->id); 573 574 ret = media_entity_pads_init(&vdev->entity, 1, &stream->pad); 575 if (ret < 0) { 576 dev_err(rkcif->dev, 577 "failed to initialize stream media pad: %d\n", ret); 578 return ret; 579 } 580 581 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 582 if (ret < 0) { 583 dev_err(rkcif->dev, "failed to register video device: %d\n", 584 ret); 585 goto err_media_entity_cleanup; 586 } 587 588 /* enable only stream ID0 by default */ 589 if (stream->id == RKCIF_ID0) 590 link_flags |= MEDIA_LNK_FL_ENABLED; 591 592 ret = media_create_pad_link(&interface->sd.entity, RKCIF_IF_PAD_SRC, 593 &stream->vdev.entity, 0, link_flags); 594 if (ret) { 595 dev_err(rkcif->dev, "failed to link stream media pad: %d\n", 596 ret); 597 goto err_video_unregister; 598 } 599 600 v4l2_info(v4l2_dev, "registered %s as /dev/video%d\n", vdev->name, 601 vdev->num); 602 603 return 0; 604 605 err_video_unregister: 606 video_unregister_device(&stream->vdev); 607 err_media_entity_cleanup: 608 media_entity_cleanup(&stream->vdev.entity); 609 return ret; 610 } 611 612 void rkcif_stream_unregister(struct rkcif_stream *stream) 613 { 614 video_unregister_device(&stream->vdev); 615 media_entity_cleanup(&stream->vdev.entity); 616 } 617 618 const struct rkcif_output_fmt * 619 rkcif_stream_find_output_fmt(struct rkcif_stream *stream, bool ret_def, 620 u32 pixelfmt) 621 { 622 const struct rkcif_output_fmt *fmt; 623 624 WARN_ON(stream->out_fmts_num == 0); 625 626 for (unsigned int i = 0; i < stream->out_fmts_num; i++) { 627 fmt = &stream->out_fmts[i]; 628 if (fmt->fourcc == pixelfmt) 629 return fmt; 630 } 631 632 if (ret_def) 633 return &stream->out_fmts[0]; 634 else 635 return NULL; 636 } 637