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