1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * vsp1_video.c -- R-Car VSP1 Video Node 4 * 5 * Copyright (C) 2013-2015 Renesas Electronics Corporation 6 * 7 * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com) 8 */ 9 10 #include <linux/list.h> 11 #include <linux/module.h> 12 #include <linux/mutex.h> 13 #include <linux/slab.h> 14 #include <linux/v4l2-mediabus.h> 15 #include <linux/videodev2.h> 16 #include <linux/wait.h> 17 18 #include <media/media-entity.h> 19 #include <media/v4l2-dev.h> 20 #include <media/v4l2-fh.h> 21 #include <media/v4l2-ioctl.h> 22 #include <media/v4l2-subdev.h> 23 #include <media/videobuf2-v4l2.h> 24 #include <media/videobuf2-dma-contig.h> 25 26 #include "vsp1.h" 27 #include "vsp1_brx.h" 28 #include "vsp1_dl.h" 29 #include "vsp1_entity.h" 30 #include "vsp1_hgo.h" 31 #include "vsp1_hgt.h" 32 #include "vsp1_pipe.h" 33 #include "vsp1_rwpf.h" 34 #include "vsp1_uds.h" 35 #include "vsp1_video.h" 36 37 #define VSP1_VIDEO_DEF_FORMAT V4L2_PIX_FMT_YUYV 38 #define VSP1_VIDEO_DEF_WIDTH 1024 39 #define VSP1_VIDEO_DEF_HEIGHT 768 40 41 #define VSP1_VIDEO_MAX_WIDTH 8190U 42 #define VSP1_VIDEO_MAX_HEIGHT 8190U 43 44 /* ----------------------------------------------------------------------------- 45 * Helper functions 46 */ 47 48 static struct v4l2_subdev * 49 vsp1_video_remote_subdev(struct media_pad *local, u32 *pad) 50 { 51 struct media_pad *remote; 52 53 remote = media_pad_remote_pad_first(local); 54 if (!remote || !is_media_entity_v4l2_subdev(remote->entity)) 55 return NULL; 56 57 if (pad) 58 *pad = remote->index; 59 60 return media_entity_to_v4l2_subdev(remote->entity); 61 } 62 63 static int vsp1_video_verify_format(struct vsp1_video *video) 64 { 65 struct v4l2_subdev_format fmt = { 66 .which = V4L2_SUBDEV_FORMAT_ACTIVE, 67 }; 68 struct v4l2_subdev *subdev; 69 int ret; 70 71 subdev = vsp1_video_remote_subdev(&video->pad, &fmt.pad); 72 if (subdev == NULL) 73 return -EINVAL; 74 75 ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt); 76 if (ret < 0) 77 return ret == -ENOIOCTLCMD ? -EINVAL : ret; 78 79 if (video->rwpf->fmtinfo->mbus != fmt.format.code || 80 video->rwpf->format.height != fmt.format.height || 81 video->rwpf->format.width != fmt.format.width) 82 return -EINVAL; 83 84 return 0; 85 } 86 87 static int __vsp1_video_try_format(struct vsp1_video *video, 88 struct v4l2_pix_format_mplane *pix, 89 const struct vsp1_format_info **fmtinfo) 90 { 91 static const u32 xrgb_formats[][2] = { 92 { V4L2_PIX_FMT_RGB444, V4L2_PIX_FMT_XRGB444 }, 93 { V4L2_PIX_FMT_RGB555, V4L2_PIX_FMT_XRGB555 }, 94 { V4L2_PIX_FMT_BGR32, V4L2_PIX_FMT_XBGR32 }, 95 { V4L2_PIX_FMT_RGB32, V4L2_PIX_FMT_XRGB32 }, 96 }; 97 98 const struct vsp1_format_info *info; 99 unsigned int width = pix->width; 100 unsigned int height = pix->height; 101 unsigned int i; 102 103 /* 104 * Backward compatibility: replace deprecated RGB formats by their XRGB 105 * equivalent. This selects the format older userspace applications want 106 * while still exposing the new format. 107 */ 108 for (i = 0; i < ARRAY_SIZE(xrgb_formats); ++i) { 109 if (xrgb_formats[i][0] == pix->pixelformat) { 110 pix->pixelformat = xrgb_formats[i][1]; 111 break; 112 } 113 } 114 115 /* 116 * Retrieve format information and select the default format if the 117 * requested format isn't supported. 118 */ 119 info = vsp1_get_format_info(video->vsp1, pix->pixelformat); 120 if (info == NULL) 121 info = vsp1_get_format_info(video->vsp1, VSP1_VIDEO_DEF_FORMAT); 122 123 pix->pixelformat = info->fourcc; 124 pix->colorspace = V4L2_COLORSPACE_SRGB; 125 pix->field = V4L2_FIELD_NONE; 126 127 if (info->fourcc == V4L2_PIX_FMT_HSV24 || 128 info->fourcc == V4L2_PIX_FMT_HSV32) 129 pix->hsv_enc = V4L2_HSV_ENC_256; 130 131 memset(pix->reserved, 0, sizeof(pix->reserved)); 132 133 /* Align the width and height for YUV 4:2:2 and 4:2:0 formats. */ 134 width = round_down(width, info->hsub); 135 height = round_down(height, info->vsub); 136 137 /* Clamp the width and height. */ 138 pix->width = clamp(width, info->hsub, VSP1_VIDEO_MAX_WIDTH); 139 pix->height = clamp(height, info->vsub, VSP1_VIDEO_MAX_HEIGHT); 140 141 /* 142 * Compute and clamp the stride and image size. While not documented in 143 * the datasheet, strides not aligned to a multiple of 128 bytes result 144 * in image corruption. 145 */ 146 for (i = 0; i < min(info->planes, 2U); ++i) { 147 unsigned int hsub = i > 0 ? info->hsub : 1; 148 unsigned int vsub = i > 0 ? info->vsub : 1; 149 unsigned int align = 128; 150 unsigned int bpl; 151 152 bpl = clamp_t(unsigned int, pix->plane_fmt[i].bytesperline, 153 pix->width / hsub * info->bpp[i] / 8, 154 round_down(65535U, align)); 155 156 pix->plane_fmt[i].bytesperline = round_up(bpl, align); 157 pix->plane_fmt[i].sizeimage = pix->plane_fmt[i].bytesperline 158 * pix->height / vsub; 159 } 160 161 if (info->planes == 3) { 162 /* The second and third planes must have the same stride. */ 163 pix->plane_fmt[2].bytesperline = pix->plane_fmt[1].bytesperline; 164 pix->plane_fmt[2].sizeimage = pix->plane_fmt[1].sizeimage; 165 } 166 167 pix->num_planes = info->planes; 168 169 if (fmtinfo) 170 *fmtinfo = info; 171 172 return 0; 173 } 174 175 /* ----------------------------------------------------------------------------- 176 * VSP1 Partition Algorithm support 177 */ 178 179 /** 180 * vsp1_video_calculate_partition - Calculate the active partition output window 181 * 182 * @pipe: the pipeline 183 * @partition: partition that will hold the calculated values 184 * @div_size: pre-determined maximum partition division size 185 * @index: partition index 186 */ 187 static void vsp1_video_calculate_partition(struct vsp1_pipeline *pipe, 188 struct vsp1_partition *partition, 189 unsigned int div_size, 190 unsigned int index) 191 { 192 const struct v4l2_mbus_framefmt *format; 193 struct vsp1_partition_window window; 194 unsigned int modulus; 195 196 /* 197 * Partitions are computed on the size before rotation, use the format 198 * at the WPF sink. 199 */ 200 format = vsp1_entity_get_pad_format(&pipe->output->entity, 201 pipe->output->entity.state, 202 RWPF_PAD_SINK); 203 204 /* A single partition simply processes the output size in full. */ 205 if (pipe->partitions <= 1) { 206 window.left = 0; 207 window.width = format->width; 208 209 vsp1_pipeline_propagate_partition(pipe, partition, index, 210 &window); 211 return; 212 } 213 214 /* Initialise the partition with sane starting conditions. */ 215 window.left = index * div_size; 216 window.width = div_size; 217 218 modulus = format->width % div_size; 219 220 /* 221 * We need to prevent the last partition from being smaller than the 222 * *minimum* width of the hardware capabilities. 223 * 224 * If the modulus is less than half of the partition size, 225 * the penultimate partition is reduced to half, which is added 226 * to the final partition: |1234|1234|1234|12|341| 227 * to prevent this: |1234|1234|1234|1234|1|. 228 */ 229 if (modulus) { 230 /* 231 * pipe->partitions is 1 based, whilst index is a 0 based index. 232 * Normalise this locally. 233 */ 234 unsigned int partitions = pipe->partitions - 1; 235 236 if (modulus < div_size / 2) { 237 if (index == partitions - 1) { 238 /* Halve the penultimate partition. */ 239 window.width = div_size / 2; 240 } else if (index == partitions) { 241 /* Increase the final partition. */ 242 window.width = (div_size / 2) + modulus; 243 window.left -= div_size / 2; 244 } 245 } else if (index == partitions) { 246 window.width = modulus; 247 } 248 } 249 250 vsp1_pipeline_propagate_partition(pipe, partition, index, &window); 251 } 252 253 static int vsp1_video_pipeline_setup_partitions(struct vsp1_pipeline *pipe) 254 { 255 struct vsp1_device *vsp1 = pipe->output->entity.vsp1; 256 const struct v4l2_mbus_framefmt *format; 257 struct vsp1_entity *entity; 258 unsigned int div_size; 259 unsigned int i; 260 261 /* 262 * Partitions are computed on the size before rotation, use the format 263 * at the WPF sink. 264 */ 265 format = vsp1_entity_get_pad_format(&pipe->output->entity, 266 pipe->output->entity.state, 267 RWPF_PAD_SINK); 268 div_size = format->width; 269 270 /* 271 * Only Gen3+ hardware requires image partitioning, Gen2 will operate 272 * with a single partition that covers the whole output. 273 */ 274 if (vsp1->info->gen >= 3) { 275 list_for_each_entry(entity, &pipe->entities, list_pipe) { 276 unsigned int entity_max; 277 278 if (!entity->ops->max_width) 279 continue; 280 281 entity_max = entity->ops->max_width(entity, pipe); 282 if (entity_max) 283 div_size = min(div_size, entity_max); 284 } 285 } 286 287 pipe->partitions = DIV_ROUND_UP(format->width, div_size); 288 pipe->part_table = kcalloc(pipe->partitions, sizeof(*pipe->part_table), 289 GFP_KERNEL); 290 if (!pipe->part_table) 291 return -ENOMEM; 292 293 for (i = 0; i < pipe->partitions; ++i) 294 vsp1_video_calculate_partition(pipe, &pipe->part_table[i], 295 div_size, i); 296 297 return 0; 298 } 299 300 /* ----------------------------------------------------------------------------- 301 * Pipeline Management 302 */ 303 304 /* 305 * vsp1_video_complete_buffer - Complete the current buffer 306 * @video: the video node 307 * 308 * This function completes the current buffer by filling its sequence number, 309 * time stamp and payload size, and hands it back to the vb2 core. 310 * 311 * Return the next queued buffer or NULL if the queue is empty. 312 */ 313 static struct vsp1_vb2_buffer * 314 vsp1_video_complete_buffer(struct vsp1_video *video) 315 { 316 struct vsp1_pipeline *pipe = video->rwpf->entity.pipe; 317 struct vsp1_vb2_buffer *next = NULL; 318 struct vsp1_vb2_buffer *done; 319 unsigned long flags; 320 unsigned int i; 321 322 spin_lock_irqsave(&video->irqlock, flags); 323 324 if (list_empty(&video->irqqueue)) { 325 spin_unlock_irqrestore(&video->irqlock, flags); 326 return NULL; 327 } 328 329 done = list_first_entry(&video->irqqueue, 330 struct vsp1_vb2_buffer, queue); 331 332 list_del(&done->queue); 333 334 if (!list_empty(&video->irqqueue)) 335 next = list_first_entry(&video->irqqueue, 336 struct vsp1_vb2_buffer, queue); 337 338 spin_unlock_irqrestore(&video->irqlock, flags); 339 340 done->buf.sequence = pipe->sequence; 341 done->buf.vb2_buf.timestamp = ktime_get_ns(); 342 for (i = 0; i < done->buf.vb2_buf.num_planes; ++i) 343 vb2_set_plane_payload(&done->buf.vb2_buf, i, 344 vb2_plane_size(&done->buf.vb2_buf, i)); 345 vb2_buffer_done(&done->buf.vb2_buf, VB2_BUF_STATE_DONE); 346 347 return next; 348 } 349 350 static void vsp1_video_frame_end(struct vsp1_pipeline *pipe, 351 struct vsp1_rwpf *rwpf) 352 { 353 struct vsp1_video *video = rwpf->video; 354 struct vsp1_vb2_buffer *buf; 355 356 buf = vsp1_video_complete_buffer(video); 357 if (buf == NULL) 358 return; 359 360 video->rwpf->mem = buf->mem; 361 pipe->buffers_ready |= 1 << video->pipe_index; 362 } 363 364 static void vsp1_video_pipeline_run_partition(struct vsp1_pipeline *pipe, 365 struct vsp1_dl_list *dl, 366 unsigned int partition) 367 { 368 struct vsp1_dl_body *dlb = vsp1_dl_list_get_body0(dl); 369 struct vsp1_entity *entity; 370 371 pipe->partition = &pipe->part_table[partition]; 372 373 list_for_each_entry(entity, &pipe->entities, list_pipe) 374 vsp1_entity_configure_partition(entity, pipe, dl, dlb); 375 } 376 377 static void vsp1_video_pipeline_run(struct vsp1_pipeline *pipe) 378 { 379 struct vsp1_device *vsp1 = pipe->output->entity.vsp1; 380 struct vsp1_entity *entity; 381 struct vsp1_dl_body *dlb; 382 struct vsp1_dl_list *dl; 383 unsigned int partition; 384 385 dl = vsp1_dl_list_get(pipe->output->dlm); 386 387 /* 388 * If the VSP hardware isn't configured yet (which occurs either when 389 * processing the first frame or after a system suspend/resume), add the 390 * cached stream configuration to the display list to perform a full 391 * initialisation. 392 */ 393 if (!pipe->configured) 394 vsp1_dl_list_add_body(dl, pipe->stream_config); 395 396 dlb = vsp1_dl_list_get_body0(dl); 397 398 list_for_each_entry(entity, &pipe->entities, list_pipe) 399 vsp1_entity_configure_frame(entity, pipe, dl, dlb); 400 401 /* Run the first partition. */ 402 vsp1_video_pipeline_run_partition(pipe, dl, 0); 403 404 /* Process consecutive partitions as necessary. */ 405 for (partition = 1; partition < pipe->partitions; ++partition) { 406 struct vsp1_dl_list *dl_next; 407 408 dl_next = vsp1_dl_list_get(pipe->output->dlm); 409 410 /* 411 * An incomplete chain will still function, but output only 412 * the partitions that had a dl available. The frame end 413 * interrupt will be marked on the last dl in the chain. 414 */ 415 if (!dl_next) { 416 dev_err(vsp1->dev, "Failed to obtain a dl list. Frame will be incomplete\n"); 417 break; 418 } 419 420 vsp1_video_pipeline_run_partition(pipe, dl_next, partition); 421 vsp1_dl_list_add_chain(dl, dl_next); 422 } 423 424 /* Complete, and commit the head display list. */ 425 vsp1_dl_list_commit(dl, 0); 426 pipe->configured = true; 427 428 vsp1_pipeline_run(pipe); 429 } 430 431 static void vsp1_video_pipeline_frame_end(struct vsp1_pipeline *pipe, 432 unsigned int completion) 433 { 434 struct vsp1_device *vsp1 = pipe->output->entity.vsp1; 435 enum vsp1_pipeline_state state; 436 unsigned long flags; 437 unsigned int i; 438 439 /* M2M Pipelines should never call here with an incomplete frame. */ 440 WARN_ON_ONCE(!(completion & VSP1_DL_FRAME_END_COMPLETED)); 441 442 spin_lock_irqsave(&pipe->irqlock, flags); 443 444 /* Complete buffers on all video nodes. */ 445 for (i = 0; i < vsp1->info->rpf_count; ++i) { 446 if (!pipe->inputs[i]) 447 continue; 448 449 vsp1_video_frame_end(pipe, pipe->inputs[i]); 450 } 451 452 vsp1_video_frame_end(pipe, pipe->output); 453 454 state = pipe->state; 455 pipe->state = VSP1_PIPELINE_STOPPED; 456 457 /* 458 * If a stop has been requested, mark the pipeline as stopped and 459 * return. Otherwise restart the pipeline if ready. 460 */ 461 if (state == VSP1_PIPELINE_STOPPING) 462 wake_up(&pipe->wq); 463 else if (vsp1_pipeline_ready(pipe)) 464 vsp1_video_pipeline_run(pipe); 465 466 spin_unlock_irqrestore(&pipe->irqlock, flags); 467 } 468 469 static int vsp1_video_pipeline_build_branch(struct vsp1_pipeline *pipe, 470 struct vsp1_rwpf *input, 471 struct vsp1_rwpf *output) 472 { 473 struct media_entity_enum ent_enum; 474 struct vsp1_entity *entity; 475 struct media_pad *pad; 476 struct vsp1_brx *brx = NULL; 477 int ret; 478 479 ret = media_entity_enum_init(&ent_enum, &input->entity.vsp1->media_dev); 480 if (ret < 0) 481 return ret; 482 483 /* 484 * The main data path doesn't include the HGO or HGT, use 485 * vsp1_entity_remote_pad() to traverse the graph. 486 */ 487 488 pad = vsp1_entity_remote_pad(&input->entity.pads[RWPF_PAD_SOURCE]); 489 490 while (1) { 491 if (pad == NULL) { 492 ret = -EPIPE; 493 goto out; 494 } 495 496 /* We've reached a video node, that shouldn't have happened. */ 497 if (!is_media_entity_v4l2_subdev(pad->entity)) { 498 ret = -EPIPE; 499 goto out; 500 } 501 502 entity = to_vsp1_entity( 503 media_entity_to_v4l2_subdev(pad->entity)); 504 505 /* 506 * A BRU or BRS is present in the pipeline, store its input pad 507 * number in the input RPF for use when configuring the RPF. 508 */ 509 if (entity->type == VSP1_ENTITY_BRU || 510 entity->type == VSP1_ENTITY_BRS) { 511 /* BRU and BRS can't be chained. */ 512 if (brx) { 513 ret = -EPIPE; 514 goto out; 515 } 516 517 brx = to_brx(&entity->subdev); 518 brx->inputs[pad->index].rpf = input; 519 input->brx_input = pad->index; 520 } 521 522 /* We've reached the WPF, we're done. */ 523 if (entity->type == VSP1_ENTITY_WPF) 524 break; 525 526 /* Ensure the branch has no loop. */ 527 if (media_entity_enum_test_and_set(&ent_enum, 528 &entity->subdev.entity)) { 529 ret = -EPIPE; 530 goto out; 531 } 532 533 /* UDS can't be chained. */ 534 if (entity->type == VSP1_ENTITY_UDS) { 535 if (pipe->uds) { 536 ret = -EPIPE; 537 goto out; 538 } 539 540 pipe->uds = entity; 541 pipe->uds_input = brx ? &brx->entity : &input->entity; 542 } 543 544 /* Follow the source link, ignoring any HGO or HGT. */ 545 pad = &entity->pads[entity->source_pad]; 546 pad = vsp1_entity_remote_pad(pad); 547 } 548 549 /* The last entity must be the output WPF. */ 550 if (entity != &output->entity) 551 ret = -EPIPE; 552 553 out: 554 media_entity_enum_cleanup(&ent_enum); 555 556 return ret; 557 } 558 559 static int vsp1_video_pipeline_build(struct vsp1_pipeline *pipe, 560 struct vsp1_video *video) 561 { 562 struct media_graph graph; 563 struct media_entity *entity = &video->video.entity; 564 struct media_device *mdev = entity->graph_obj.mdev; 565 unsigned int i; 566 int ret; 567 568 /* Walk the graph to locate the entities and video nodes. */ 569 ret = media_graph_walk_init(&graph, mdev); 570 if (ret) 571 return ret; 572 573 media_graph_walk_start(&graph, entity); 574 575 while ((entity = media_graph_walk_next(&graph))) { 576 struct v4l2_subdev *subdev; 577 struct vsp1_rwpf *rwpf; 578 struct vsp1_entity *e; 579 580 if (!is_media_entity_v4l2_subdev(entity)) 581 continue; 582 583 subdev = media_entity_to_v4l2_subdev(entity); 584 e = to_vsp1_entity(subdev); 585 list_add_tail(&e->list_pipe, &pipe->entities); 586 e->pipe = pipe; 587 588 switch (e->type) { 589 case VSP1_ENTITY_RPF: 590 rwpf = to_rwpf(subdev); 591 pipe->inputs[rwpf->entity.index] = rwpf; 592 rwpf->video->pipe_index = ++pipe->num_inputs; 593 break; 594 595 case VSP1_ENTITY_WPF: 596 rwpf = to_rwpf(subdev); 597 pipe->output = rwpf; 598 rwpf->video->pipe_index = 0; 599 break; 600 601 case VSP1_ENTITY_LIF: 602 pipe->lif = e; 603 break; 604 605 case VSP1_ENTITY_BRU: 606 case VSP1_ENTITY_BRS: 607 pipe->brx = e; 608 break; 609 610 case VSP1_ENTITY_HGO: 611 pipe->hgo = e; 612 break; 613 614 case VSP1_ENTITY_HGT: 615 pipe->hgt = e; 616 break; 617 618 default: 619 break; 620 } 621 } 622 623 media_graph_walk_cleanup(&graph); 624 625 /* We need one output and at least one input. */ 626 if (pipe->num_inputs == 0 || !pipe->output) 627 return -EPIPE; 628 629 /* 630 * Follow links downstream for each input and make sure the graph 631 * contains no loop and that all branches end at the output WPF. 632 */ 633 for (i = 0; i < video->vsp1->info->rpf_count; ++i) { 634 if (!pipe->inputs[i]) 635 continue; 636 637 ret = vsp1_video_pipeline_build_branch(pipe, pipe->inputs[i], 638 pipe->output); 639 if (ret < 0) 640 return ret; 641 } 642 643 return 0; 644 } 645 646 static int vsp1_video_pipeline_init(struct vsp1_pipeline *pipe, 647 struct vsp1_video *video) 648 { 649 vsp1_pipeline_init(pipe); 650 651 pipe->frame_end = vsp1_video_pipeline_frame_end; 652 653 return vsp1_video_pipeline_build(pipe, video); 654 } 655 656 static struct vsp1_pipeline *vsp1_video_pipeline_get(struct vsp1_video *video) 657 { 658 struct vsp1_pipeline *pipe; 659 int ret; 660 661 /* 662 * Get a pipeline object for the video node. If a pipeline has already 663 * been allocated just increment its reference count and return it. 664 * Otherwise allocate a new pipeline and initialize it, it will be freed 665 * when the last reference is released. 666 */ 667 if (!video->rwpf->entity.pipe) { 668 pipe = kzalloc(sizeof(*pipe), GFP_KERNEL); 669 if (!pipe) 670 return ERR_PTR(-ENOMEM); 671 672 ret = vsp1_video_pipeline_init(pipe, video); 673 if (ret < 0) { 674 vsp1_pipeline_reset(pipe); 675 kfree(pipe); 676 return ERR_PTR(ret); 677 } 678 } else { 679 pipe = video->rwpf->entity.pipe; 680 kref_get(&pipe->kref); 681 } 682 683 return pipe; 684 } 685 686 static void vsp1_video_pipeline_release(struct kref *kref) 687 { 688 struct vsp1_pipeline *pipe = container_of(kref, typeof(*pipe), kref); 689 690 vsp1_pipeline_reset(pipe); 691 kfree(pipe); 692 } 693 694 static void vsp1_video_pipeline_put(struct vsp1_pipeline *pipe) 695 { 696 struct media_device *mdev = &pipe->output->entity.vsp1->media_dev; 697 698 mutex_lock(&mdev->graph_mutex); 699 kref_put(&pipe->kref, vsp1_video_pipeline_release); 700 mutex_unlock(&mdev->graph_mutex); 701 } 702 703 /* ----------------------------------------------------------------------------- 704 * videobuf2 Queue Operations 705 */ 706 707 static int 708 vsp1_video_queue_setup(struct vb2_queue *vq, 709 unsigned int *nbuffers, unsigned int *nplanes, 710 unsigned int sizes[], struct device *alloc_devs[]) 711 { 712 struct vsp1_video *video = vb2_get_drv_priv(vq); 713 const struct v4l2_pix_format_mplane *format = &video->rwpf->format; 714 unsigned int i; 715 716 if (*nplanes) { 717 if (*nplanes != format->num_planes) 718 return -EINVAL; 719 720 for (i = 0; i < *nplanes; i++) 721 if (sizes[i] < format->plane_fmt[i].sizeimage) 722 return -EINVAL; 723 return 0; 724 } 725 726 *nplanes = format->num_planes; 727 728 for (i = 0; i < format->num_planes; ++i) 729 sizes[i] = format->plane_fmt[i].sizeimage; 730 731 return 0; 732 } 733 734 static int vsp1_video_buffer_prepare(struct vb2_buffer *vb) 735 { 736 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 737 struct vsp1_video *video = vb2_get_drv_priv(vb->vb2_queue); 738 struct vsp1_vb2_buffer *buf = to_vsp1_vb2_buffer(vbuf); 739 const struct v4l2_pix_format_mplane *format = &video->rwpf->format; 740 unsigned int i; 741 742 if (vb->num_planes < format->num_planes) 743 return -EINVAL; 744 745 for (i = 0; i < vb->num_planes; ++i) { 746 buf->mem.addr[i] = vb2_dma_contig_plane_dma_addr(vb, i); 747 748 if (vb2_plane_size(vb, i) < format->plane_fmt[i].sizeimage) 749 return -EINVAL; 750 } 751 752 for ( ; i < 3; ++i) 753 buf->mem.addr[i] = 0; 754 755 return 0; 756 } 757 758 static void vsp1_video_buffer_queue(struct vb2_buffer *vb) 759 { 760 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 761 struct vsp1_video *video = vb2_get_drv_priv(vb->vb2_queue); 762 struct vsp1_pipeline *pipe = video->rwpf->entity.pipe; 763 struct vsp1_vb2_buffer *buf = to_vsp1_vb2_buffer(vbuf); 764 unsigned long flags; 765 bool empty; 766 767 spin_lock_irqsave(&video->irqlock, flags); 768 empty = list_empty(&video->irqqueue); 769 list_add_tail(&buf->queue, &video->irqqueue); 770 spin_unlock_irqrestore(&video->irqlock, flags); 771 772 if (!empty) 773 return; 774 775 spin_lock_irqsave(&pipe->irqlock, flags); 776 777 video->rwpf->mem = buf->mem; 778 pipe->buffers_ready |= 1 << video->pipe_index; 779 780 if (vb2_start_streaming_called(&video->queue) && 781 vsp1_pipeline_ready(pipe)) 782 vsp1_video_pipeline_run(pipe); 783 784 spin_unlock_irqrestore(&pipe->irqlock, flags); 785 } 786 787 static int vsp1_video_setup_pipeline(struct vsp1_pipeline *pipe) 788 { 789 struct vsp1_entity *entity; 790 int ret; 791 792 /* Determine this pipelines sizes for image partitioning support. */ 793 ret = vsp1_video_pipeline_setup_partitions(pipe); 794 if (ret < 0) 795 return ret; 796 797 if (pipe->uds) { 798 struct vsp1_uds *uds = to_uds(&pipe->uds->subdev); 799 800 /* 801 * If a BRU or BRS is present in the pipeline before the UDS, 802 * the alpha component doesn't need to be scaled as the BRU and 803 * BRS output alpha value is fixed to 255. Otherwise we need to 804 * scale the alpha component only when available at the input 805 * RPF. 806 */ 807 if (pipe->uds_input->type == VSP1_ENTITY_BRU || 808 pipe->uds_input->type == VSP1_ENTITY_BRS) { 809 uds->scale_alpha = false; 810 } else { 811 struct vsp1_rwpf *rpf = 812 to_rwpf(&pipe->uds_input->subdev); 813 814 uds->scale_alpha = rpf->fmtinfo->alpha; 815 } 816 } 817 818 /* 819 * Compute and cache the stream configuration into a body. The cached 820 * body will be added to the display list by vsp1_video_pipeline_run() 821 * whenever the pipeline needs to be fully reconfigured. 822 */ 823 pipe->stream_config = vsp1_dlm_dl_body_get(pipe->output->dlm); 824 if (!pipe->stream_config) 825 return -ENOMEM; 826 827 list_for_each_entry(entity, &pipe->entities, list_pipe) { 828 vsp1_entity_route_setup(entity, pipe, pipe->stream_config); 829 vsp1_entity_configure_stream(entity, pipe, NULL, 830 pipe->stream_config); 831 } 832 833 return 0; 834 } 835 836 static void vsp1_video_release_buffers(struct vsp1_video *video) 837 { 838 struct vsp1_vb2_buffer *buffer; 839 unsigned long flags; 840 841 /* Remove all buffers from the IRQ queue. */ 842 spin_lock_irqsave(&video->irqlock, flags); 843 list_for_each_entry(buffer, &video->irqqueue, queue) 844 vb2_buffer_done(&buffer->buf.vb2_buf, VB2_BUF_STATE_ERROR); 845 INIT_LIST_HEAD(&video->irqqueue); 846 spin_unlock_irqrestore(&video->irqlock, flags); 847 } 848 849 static void vsp1_video_cleanup_pipeline(struct vsp1_pipeline *pipe) 850 { 851 lockdep_assert_held(&pipe->lock); 852 853 /* Release any cached configuration from our output video. */ 854 vsp1_dl_body_put(pipe->stream_config); 855 pipe->stream_config = NULL; 856 pipe->configured = false; 857 858 /* Release our partition table allocation. */ 859 kfree(pipe->part_table); 860 pipe->part_table = NULL; 861 } 862 863 static int vsp1_video_start_streaming(struct vb2_queue *vq, unsigned int count) 864 { 865 struct vsp1_video *video = vb2_get_drv_priv(vq); 866 struct vsp1_pipeline *pipe = video->rwpf->entity.pipe; 867 bool start_pipeline = false; 868 unsigned long flags; 869 int ret; 870 871 mutex_lock(&pipe->lock); 872 if (pipe->stream_count == pipe->num_inputs) { 873 ret = vsp1_video_setup_pipeline(pipe); 874 if (ret < 0) { 875 vsp1_video_release_buffers(video); 876 vsp1_video_cleanup_pipeline(pipe); 877 mutex_unlock(&pipe->lock); 878 return ret; 879 } 880 881 start_pipeline = true; 882 } 883 884 pipe->stream_count++; 885 mutex_unlock(&pipe->lock); 886 887 /* 888 * vsp1_pipeline_ready() is not sufficient to establish that all streams 889 * are prepared and the pipeline is configured, as multiple streams 890 * can race through streamon with buffers already queued; Therefore we 891 * don't even attempt to start the pipeline until the last stream has 892 * called through here. 893 */ 894 if (!start_pipeline) 895 return 0; 896 897 spin_lock_irqsave(&pipe->irqlock, flags); 898 if (vsp1_pipeline_ready(pipe)) 899 vsp1_video_pipeline_run(pipe); 900 spin_unlock_irqrestore(&pipe->irqlock, flags); 901 902 return 0; 903 } 904 905 static void vsp1_video_stop_streaming(struct vb2_queue *vq) 906 { 907 struct vsp1_video *video = vb2_get_drv_priv(vq); 908 struct vsp1_pipeline *pipe = video->rwpf->entity.pipe; 909 unsigned long flags; 910 int ret; 911 912 /* 913 * Clear the buffers ready flag to make sure the device won't be started 914 * by a QBUF on the video node on the other side of the pipeline. 915 */ 916 spin_lock_irqsave(&video->irqlock, flags); 917 pipe->buffers_ready &= ~(1 << video->pipe_index); 918 spin_unlock_irqrestore(&video->irqlock, flags); 919 920 mutex_lock(&pipe->lock); 921 if (--pipe->stream_count == pipe->num_inputs) { 922 /* Stop the pipeline. */ 923 ret = vsp1_pipeline_stop(pipe); 924 if (ret == -ETIMEDOUT) 925 dev_err(video->vsp1->dev, "pipeline stop timeout\n"); 926 927 vsp1_video_cleanup_pipeline(pipe); 928 } 929 mutex_unlock(&pipe->lock); 930 931 video_device_pipeline_stop(&video->video); 932 vsp1_video_release_buffers(video); 933 vsp1_video_pipeline_put(pipe); 934 } 935 936 static const struct vb2_ops vsp1_video_queue_qops = { 937 .queue_setup = vsp1_video_queue_setup, 938 .buf_prepare = vsp1_video_buffer_prepare, 939 .buf_queue = vsp1_video_buffer_queue, 940 .wait_prepare = vb2_ops_wait_prepare, 941 .wait_finish = vb2_ops_wait_finish, 942 .start_streaming = vsp1_video_start_streaming, 943 .stop_streaming = vsp1_video_stop_streaming, 944 }; 945 946 /* ----------------------------------------------------------------------------- 947 * V4L2 ioctls 948 */ 949 950 static int 951 vsp1_video_querycap(struct file *file, void *fh, struct v4l2_capability *cap) 952 { 953 struct v4l2_fh *vfh = file->private_data; 954 struct vsp1_video *video = to_vsp1_video(vfh->vdev); 955 956 cap->capabilities = V4L2_CAP_DEVICE_CAPS | V4L2_CAP_STREAMING 957 | V4L2_CAP_VIDEO_CAPTURE_MPLANE 958 | V4L2_CAP_VIDEO_OUTPUT_MPLANE; 959 960 961 strscpy(cap->driver, "vsp1", sizeof(cap->driver)); 962 strscpy(cap->card, video->video.name, sizeof(cap->card)); 963 964 return 0; 965 } 966 967 static int 968 vsp1_video_get_format(struct file *file, void *fh, struct v4l2_format *format) 969 { 970 struct v4l2_fh *vfh = file->private_data; 971 struct vsp1_video *video = to_vsp1_video(vfh->vdev); 972 973 if (format->type != video->queue.type) 974 return -EINVAL; 975 976 mutex_lock(&video->lock); 977 format->fmt.pix_mp = video->rwpf->format; 978 mutex_unlock(&video->lock); 979 980 return 0; 981 } 982 983 static int 984 vsp1_video_try_format(struct file *file, void *fh, struct v4l2_format *format) 985 { 986 struct v4l2_fh *vfh = file->private_data; 987 struct vsp1_video *video = to_vsp1_video(vfh->vdev); 988 989 if (format->type != video->queue.type) 990 return -EINVAL; 991 992 return __vsp1_video_try_format(video, &format->fmt.pix_mp, NULL); 993 } 994 995 static int 996 vsp1_video_set_format(struct file *file, void *fh, struct v4l2_format *format) 997 { 998 struct v4l2_fh *vfh = file->private_data; 999 struct vsp1_video *video = to_vsp1_video(vfh->vdev); 1000 const struct vsp1_format_info *info; 1001 int ret; 1002 1003 if (format->type != video->queue.type) 1004 return -EINVAL; 1005 1006 ret = __vsp1_video_try_format(video, &format->fmt.pix_mp, &info); 1007 if (ret < 0) 1008 return ret; 1009 1010 mutex_lock(&video->lock); 1011 1012 if (vb2_is_busy(&video->queue)) { 1013 ret = -EBUSY; 1014 goto done; 1015 } 1016 1017 video->rwpf->format = format->fmt.pix_mp; 1018 video->rwpf->fmtinfo = info; 1019 1020 done: 1021 mutex_unlock(&video->lock); 1022 return ret; 1023 } 1024 1025 static int 1026 vsp1_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type) 1027 { 1028 struct v4l2_fh *vfh = file->private_data; 1029 struct vsp1_video *video = to_vsp1_video(vfh->vdev); 1030 struct media_device *mdev = &video->vsp1->media_dev; 1031 struct vsp1_pipeline *pipe; 1032 int ret; 1033 1034 if (vb2_queue_is_busy(&video->queue, file)) 1035 return -EBUSY; 1036 1037 /* 1038 * Get a pipeline for the video node and start streaming on it. No link 1039 * touching an entity in the pipeline can be activated or deactivated 1040 * once streaming is started. 1041 */ 1042 mutex_lock(&mdev->graph_mutex); 1043 1044 pipe = vsp1_video_pipeline_get(video); 1045 if (IS_ERR(pipe)) { 1046 mutex_unlock(&mdev->graph_mutex); 1047 return PTR_ERR(pipe); 1048 } 1049 1050 ret = __video_device_pipeline_start(&video->video, &pipe->pipe); 1051 if (ret < 0) { 1052 mutex_unlock(&mdev->graph_mutex); 1053 goto err_pipe; 1054 } 1055 1056 mutex_unlock(&mdev->graph_mutex); 1057 1058 /* 1059 * Verify that the configured format matches the output of the connected 1060 * subdev. 1061 */ 1062 ret = vsp1_video_verify_format(video); 1063 if (ret < 0) 1064 goto err_stop; 1065 1066 /* Start the queue. */ 1067 ret = vb2_streamon(&video->queue, type); 1068 if (ret < 0) 1069 goto err_stop; 1070 1071 return 0; 1072 1073 err_stop: 1074 video_device_pipeline_stop(&video->video); 1075 err_pipe: 1076 vsp1_video_pipeline_put(pipe); 1077 return ret; 1078 } 1079 1080 static const struct v4l2_ioctl_ops vsp1_video_ioctl_ops = { 1081 .vidioc_querycap = vsp1_video_querycap, 1082 .vidioc_g_fmt_vid_cap_mplane = vsp1_video_get_format, 1083 .vidioc_s_fmt_vid_cap_mplane = vsp1_video_set_format, 1084 .vidioc_try_fmt_vid_cap_mplane = vsp1_video_try_format, 1085 .vidioc_g_fmt_vid_out_mplane = vsp1_video_get_format, 1086 .vidioc_s_fmt_vid_out_mplane = vsp1_video_set_format, 1087 .vidioc_try_fmt_vid_out_mplane = vsp1_video_try_format, 1088 .vidioc_reqbufs = vb2_ioctl_reqbufs, 1089 .vidioc_querybuf = vb2_ioctl_querybuf, 1090 .vidioc_qbuf = vb2_ioctl_qbuf, 1091 .vidioc_dqbuf = vb2_ioctl_dqbuf, 1092 .vidioc_expbuf = vb2_ioctl_expbuf, 1093 .vidioc_create_bufs = vb2_ioctl_create_bufs, 1094 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 1095 .vidioc_streamon = vsp1_video_streamon, 1096 .vidioc_streamoff = vb2_ioctl_streamoff, 1097 }; 1098 1099 /* ----------------------------------------------------------------------------- 1100 * V4L2 File Operations 1101 */ 1102 1103 static int vsp1_video_open(struct file *file) 1104 { 1105 struct vsp1_video *video = video_drvdata(file); 1106 struct v4l2_fh *vfh; 1107 int ret = 0; 1108 1109 vfh = kzalloc(sizeof(*vfh), GFP_KERNEL); 1110 if (vfh == NULL) 1111 return -ENOMEM; 1112 1113 v4l2_fh_init(vfh, &video->video); 1114 v4l2_fh_add(vfh); 1115 1116 file->private_data = vfh; 1117 1118 ret = vsp1_device_get(video->vsp1); 1119 if (ret < 0) { 1120 v4l2_fh_del(vfh); 1121 v4l2_fh_exit(vfh); 1122 kfree(vfh); 1123 } 1124 1125 return ret; 1126 } 1127 1128 static int vsp1_video_release(struct file *file) 1129 { 1130 struct vsp1_video *video = video_drvdata(file); 1131 1132 vb2_fop_release(file); 1133 1134 vsp1_device_put(video->vsp1); 1135 1136 return 0; 1137 } 1138 1139 static const struct v4l2_file_operations vsp1_video_fops = { 1140 .owner = THIS_MODULE, 1141 .unlocked_ioctl = video_ioctl2, 1142 .open = vsp1_video_open, 1143 .release = vsp1_video_release, 1144 .poll = vb2_fop_poll, 1145 .mmap = vb2_fop_mmap, 1146 }; 1147 1148 /* ----------------------------------------------------------------------------- 1149 * Suspend and Resume 1150 */ 1151 1152 void vsp1_video_suspend(struct vsp1_device *vsp1) 1153 { 1154 unsigned long flags; 1155 unsigned int i; 1156 int ret; 1157 1158 /* 1159 * To avoid increasing the system suspend time needlessly, loop over the 1160 * pipelines twice, first to set them all to the stopping state, and 1161 * then to wait for the stop to complete. 1162 */ 1163 for (i = 0; i < vsp1->info->wpf_count; ++i) { 1164 struct vsp1_rwpf *wpf = vsp1->wpf[i]; 1165 struct vsp1_pipeline *pipe; 1166 1167 if (wpf == NULL) 1168 continue; 1169 1170 pipe = wpf->entity.pipe; 1171 if (pipe == NULL) 1172 continue; 1173 1174 spin_lock_irqsave(&pipe->irqlock, flags); 1175 if (pipe->state == VSP1_PIPELINE_RUNNING) 1176 pipe->state = VSP1_PIPELINE_STOPPING; 1177 spin_unlock_irqrestore(&pipe->irqlock, flags); 1178 } 1179 1180 for (i = 0; i < vsp1->info->wpf_count; ++i) { 1181 struct vsp1_rwpf *wpf = vsp1->wpf[i]; 1182 struct vsp1_pipeline *pipe; 1183 1184 if (wpf == NULL) 1185 continue; 1186 1187 pipe = wpf->entity.pipe; 1188 if (pipe == NULL) 1189 continue; 1190 1191 ret = wait_event_timeout(pipe->wq, vsp1_pipeline_stopped(pipe), 1192 msecs_to_jiffies(500)); 1193 if (ret == 0) 1194 dev_warn(vsp1->dev, "pipeline %u stop timeout\n", 1195 wpf->entity.index); 1196 } 1197 } 1198 1199 void vsp1_video_resume(struct vsp1_device *vsp1) 1200 { 1201 unsigned long flags; 1202 unsigned int i; 1203 1204 /* Resume all running pipelines. */ 1205 for (i = 0; i < vsp1->info->wpf_count; ++i) { 1206 struct vsp1_rwpf *wpf = vsp1->wpf[i]; 1207 struct vsp1_pipeline *pipe; 1208 1209 if (wpf == NULL) 1210 continue; 1211 1212 pipe = wpf->entity.pipe; 1213 if (pipe == NULL) 1214 continue; 1215 1216 /* 1217 * The hardware may have been reset during a suspend and will 1218 * need a full reconfiguration. 1219 */ 1220 pipe->configured = false; 1221 1222 spin_lock_irqsave(&pipe->irqlock, flags); 1223 if (vsp1_pipeline_ready(pipe)) 1224 vsp1_video_pipeline_run(pipe); 1225 spin_unlock_irqrestore(&pipe->irqlock, flags); 1226 } 1227 } 1228 1229 /* ----------------------------------------------------------------------------- 1230 * Initialization and Cleanup 1231 */ 1232 1233 struct vsp1_video *vsp1_video_create(struct vsp1_device *vsp1, 1234 struct vsp1_rwpf *rwpf) 1235 { 1236 struct vsp1_video *video; 1237 const char *direction; 1238 int ret; 1239 1240 video = devm_kzalloc(vsp1->dev, sizeof(*video), GFP_KERNEL); 1241 if (!video) 1242 return ERR_PTR(-ENOMEM); 1243 1244 rwpf->video = video; 1245 1246 video->vsp1 = vsp1; 1247 video->rwpf = rwpf; 1248 1249 if (rwpf->entity.type == VSP1_ENTITY_RPF) { 1250 direction = "input"; 1251 video->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; 1252 video->pad.flags = MEDIA_PAD_FL_SOURCE; 1253 video->video.vfl_dir = VFL_DIR_TX; 1254 video->video.device_caps = V4L2_CAP_VIDEO_OUTPUT_MPLANE | 1255 V4L2_CAP_STREAMING; 1256 } else { 1257 direction = "output"; 1258 video->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; 1259 video->pad.flags = MEDIA_PAD_FL_SINK; 1260 video->video.vfl_dir = VFL_DIR_RX; 1261 video->video.device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | 1262 V4L2_CAP_STREAMING; 1263 } 1264 1265 mutex_init(&video->lock); 1266 spin_lock_init(&video->irqlock); 1267 INIT_LIST_HEAD(&video->irqqueue); 1268 1269 /* Initialize the media entity... */ 1270 ret = media_entity_pads_init(&video->video.entity, 1, &video->pad); 1271 if (ret < 0) 1272 return ERR_PTR(ret); 1273 1274 /* ... and the format ... */ 1275 rwpf->format.pixelformat = VSP1_VIDEO_DEF_FORMAT; 1276 rwpf->format.width = VSP1_VIDEO_DEF_WIDTH; 1277 rwpf->format.height = VSP1_VIDEO_DEF_HEIGHT; 1278 __vsp1_video_try_format(video, &rwpf->format, &rwpf->fmtinfo); 1279 1280 /* ... and the video node... */ 1281 video->video.v4l2_dev = &video->vsp1->v4l2_dev; 1282 video->video.fops = &vsp1_video_fops; 1283 snprintf(video->video.name, sizeof(video->video.name), "%s %s", 1284 rwpf->entity.subdev.name, direction); 1285 video->video.vfl_type = VFL_TYPE_VIDEO; 1286 video->video.release = video_device_release_empty; 1287 video->video.ioctl_ops = &vsp1_video_ioctl_ops; 1288 1289 video_set_drvdata(&video->video, video); 1290 1291 video->queue.type = video->type; 1292 video->queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; 1293 video->queue.lock = &video->lock; 1294 video->queue.drv_priv = video; 1295 video->queue.buf_struct_size = sizeof(struct vsp1_vb2_buffer); 1296 video->queue.ops = &vsp1_video_queue_qops; 1297 video->queue.mem_ops = &vb2_dma_contig_memops; 1298 video->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; 1299 video->queue.dev = video->vsp1->bus_master; 1300 ret = vb2_queue_init(&video->queue); 1301 if (ret < 0) { 1302 dev_err(video->vsp1->dev, "failed to initialize vb2 queue\n"); 1303 goto error; 1304 } 1305 1306 /* ... and register the video device. */ 1307 video->video.queue = &video->queue; 1308 ret = video_register_device(&video->video, VFL_TYPE_VIDEO, -1); 1309 if (ret < 0) { 1310 dev_err(video->vsp1->dev, "failed to register video device\n"); 1311 goto error; 1312 } 1313 1314 return video; 1315 1316 error: 1317 vsp1_video_cleanup(video); 1318 return ERR_PTR(ret); 1319 } 1320 1321 void vsp1_video_cleanup(struct vsp1_video *video) 1322 { 1323 if (video_is_registered(&video->video)) 1324 video_unregister_device(&video->video); 1325 1326 media_entity_cleanup(&video->video.entity); 1327 } 1328