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 48static struct v4l2_subdev * 49vsp1_video_remote_subdev(struct media_pad *local, u32 *pad) 50{ 51 struct media_pad *remote; 52 53 remote = media_entity_remote_pad(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 63static 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 86static 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 */ 186static 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 252static 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 videobuf core. 309 * 310 * Return the next queued buffer or NULL if the queue is empty. 311 */ 312static struct vsp1_vb2_buffer * 313vsp1_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 349static 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 363static 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 376static 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 430static 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 468static 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 552out: 553 media_entity_enum_cleanup(&ent_enum); 554 555 return ret; 556} 557 558static 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 645static 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 655static 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 685static 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 693static 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 706static int 707vsp1_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 733static 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 757static 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 786static 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 835static 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 848static 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 862static 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 904static 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 935static 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 949static int 950vsp1_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 snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s", 963 dev_name(video->vsp1->dev)); 964 965 return 0; 966} 967 968static int 969vsp1_video_get_format(struct file *file, void *fh, struct v4l2_format *format) 970{ 971 struct v4l2_fh *vfh = file->private_data; 972 struct vsp1_video *video = to_vsp1_video(vfh->vdev); 973 974 if (format->type != video->queue.type) 975 return -EINVAL; 976 977 mutex_lock(&video->lock); 978 format->fmt.pix_mp = video->rwpf->format; 979 mutex_unlock(&video->lock); 980 981 return 0; 982} 983 984static int 985vsp1_video_try_format(struct file *file, void *fh, struct v4l2_format *format) 986{ 987 struct v4l2_fh *vfh = file->private_data; 988 struct vsp1_video *video = to_vsp1_video(vfh->vdev); 989 990 if (format->type != video->queue.type) 991 return -EINVAL; 992 993 return __vsp1_video_try_format(video, &format->fmt.pix_mp, NULL); 994} 995 996static int 997vsp1_video_set_format(struct file *file, void *fh, struct v4l2_format *format) 998{ 999 struct v4l2_fh *vfh = file->private_data; 1000 struct vsp1_video *video = to_vsp1_video(vfh->vdev); 1001 const struct vsp1_format_info *info; 1002 int ret; 1003 1004 if (format->type != video->queue.type) 1005 return -EINVAL; 1006 1007 ret = __vsp1_video_try_format(video, &format->fmt.pix_mp, &info); 1008 if (ret < 0) 1009 return ret; 1010 1011 mutex_lock(&video->lock); 1012 1013 if (vb2_is_busy(&video->queue)) { 1014 ret = -EBUSY; 1015 goto done; 1016 } 1017 1018 video->rwpf->format = format->fmt.pix_mp; 1019 video->rwpf->fmtinfo = info; 1020 1021done: 1022 mutex_unlock(&video->lock); 1023 return ret; 1024} 1025 1026static int 1027vsp1_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type) 1028{ 1029 struct v4l2_fh *vfh = file->private_data; 1030 struct vsp1_video *video = to_vsp1_video(vfh->vdev); 1031 struct media_device *mdev = &video->vsp1->media_dev; 1032 struct vsp1_pipeline *pipe; 1033 int ret; 1034 1035 if (video->queue.owner && video->queue.owner != file->private_data) 1036 return -EBUSY; 1037 1038 /* 1039 * Get a pipeline for the video node and start streaming on it. No link 1040 * touching an entity in the pipeline can be activated or deactivated 1041 * once streaming is started. 1042 */ 1043 mutex_lock(&mdev->graph_mutex); 1044 1045 pipe = vsp1_video_pipeline_get(video); 1046 if (IS_ERR(pipe)) { 1047 mutex_unlock(&mdev->graph_mutex); 1048 return PTR_ERR(pipe); 1049 } 1050 1051 ret = __media_pipeline_start(&video->video.entity, &pipe->pipe); 1052 if (ret < 0) { 1053 mutex_unlock(&mdev->graph_mutex); 1054 goto err_pipe; 1055 } 1056 1057 mutex_unlock(&mdev->graph_mutex); 1058 1059 /* 1060 * Verify that the configured format matches the output of the connected 1061 * subdev. 1062 */ 1063 ret = vsp1_video_verify_format(video); 1064 if (ret < 0) 1065 goto err_stop; 1066 1067 /* Start the queue. */ 1068 ret = vb2_streamon(&video->queue, type); 1069 if (ret < 0) 1070 goto err_stop; 1071 1072 return 0; 1073 1074err_stop: 1075 media_pipeline_stop(&video->video.entity); 1076err_pipe: 1077 vsp1_video_pipeline_put(pipe); 1078 return ret; 1079} 1080 1081static const struct v4l2_ioctl_ops vsp1_video_ioctl_ops = { 1082 .vidioc_querycap = vsp1_video_querycap, 1083 .vidioc_g_fmt_vid_cap_mplane = vsp1_video_get_format, 1084 .vidioc_s_fmt_vid_cap_mplane = vsp1_video_set_format, 1085 .vidioc_try_fmt_vid_cap_mplane = vsp1_video_try_format, 1086 .vidioc_g_fmt_vid_out_mplane = vsp1_video_get_format, 1087 .vidioc_s_fmt_vid_out_mplane = vsp1_video_set_format, 1088 .vidioc_try_fmt_vid_out_mplane = vsp1_video_try_format, 1089 .vidioc_reqbufs = vb2_ioctl_reqbufs, 1090 .vidioc_querybuf = vb2_ioctl_querybuf, 1091 .vidioc_qbuf = vb2_ioctl_qbuf, 1092 .vidioc_dqbuf = vb2_ioctl_dqbuf, 1093 .vidioc_expbuf = vb2_ioctl_expbuf, 1094 .vidioc_create_bufs = vb2_ioctl_create_bufs, 1095 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 1096 .vidioc_streamon = vsp1_video_streamon, 1097 .vidioc_streamoff = vb2_ioctl_streamoff, 1098}; 1099 1100/* ----------------------------------------------------------------------------- 1101 * V4L2 File Operations 1102 */ 1103 1104static int vsp1_video_open(struct file *file) 1105{ 1106 struct vsp1_video *video = video_drvdata(file); 1107 struct v4l2_fh *vfh; 1108 int ret = 0; 1109 1110 vfh = kzalloc(sizeof(*vfh), GFP_KERNEL); 1111 if (vfh == NULL) 1112 return -ENOMEM; 1113 1114 v4l2_fh_init(vfh, &video->video); 1115 v4l2_fh_add(vfh); 1116 1117 file->private_data = vfh; 1118 1119 ret = vsp1_device_get(video->vsp1); 1120 if (ret < 0) { 1121 v4l2_fh_del(vfh); 1122 v4l2_fh_exit(vfh); 1123 kfree(vfh); 1124 } 1125 1126 return ret; 1127} 1128 1129static int vsp1_video_release(struct file *file) 1130{ 1131 struct vsp1_video *video = video_drvdata(file); 1132 struct v4l2_fh *vfh = file->private_data; 1133 1134 mutex_lock(&video->lock); 1135 if (video->queue.owner == vfh) { 1136 vb2_queue_release(&video->queue); 1137 video->queue.owner = NULL; 1138 } 1139 mutex_unlock(&video->lock); 1140 1141 vsp1_device_put(video->vsp1); 1142 1143 v4l2_fh_release(file); 1144 1145 file->private_data = NULL; 1146 1147 return 0; 1148} 1149 1150static const struct v4l2_file_operations vsp1_video_fops = { 1151 .owner = THIS_MODULE, 1152 .unlocked_ioctl = video_ioctl2, 1153 .open = vsp1_video_open, 1154 .release = vsp1_video_release, 1155 .poll = vb2_fop_poll, 1156 .mmap = vb2_fop_mmap, 1157}; 1158 1159/* ----------------------------------------------------------------------------- 1160 * Suspend and Resume 1161 */ 1162 1163void vsp1_video_suspend(struct vsp1_device *vsp1) 1164{ 1165 unsigned long flags; 1166 unsigned int i; 1167 int ret; 1168 1169 /* 1170 * To avoid increasing the system suspend time needlessly, loop over the 1171 * pipelines twice, first to set them all to the stopping state, and 1172 * then to wait for the stop to complete. 1173 */ 1174 for (i = 0; i < vsp1->info->wpf_count; ++i) { 1175 struct vsp1_rwpf *wpf = vsp1->wpf[i]; 1176 struct vsp1_pipeline *pipe; 1177 1178 if (wpf == NULL) 1179 continue; 1180 1181 pipe = wpf->entity.pipe; 1182 if (pipe == NULL) 1183 continue; 1184 1185 spin_lock_irqsave(&pipe->irqlock, flags); 1186 if (pipe->state == VSP1_PIPELINE_RUNNING) 1187 pipe->state = VSP1_PIPELINE_STOPPING; 1188 spin_unlock_irqrestore(&pipe->irqlock, flags); 1189 } 1190 1191 for (i = 0; i < vsp1->info->wpf_count; ++i) { 1192 struct vsp1_rwpf *wpf = vsp1->wpf[i]; 1193 struct vsp1_pipeline *pipe; 1194 1195 if (wpf == NULL) 1196 continue; 1197 1198 pipe = wpf->entity.pipe; 1199 if (pipe == NULL) 1200 continue; 1201 1202 ret = wait_event_timeout(pipe->wq, vsp1_pipeline_stopped(pipe), 1203 msecs_to_jiffies(500)); 1204 if (ret == 0) 1205 dev_warn(vsp1->dev, "pipeline %u stop timeout\n", 1206 wpf->entity.index); 1207 } 1208} 1209 1210void vsp1_video_resume(struct vsp1_device *vsp1) 1211{ 1212 unsigned long flags; 1213 unsigned int i; 1214 1215 /* Resume all running pipelines. */ 1216 for (i = 0; i < vsp1->info->wpf_count; ++i) { 1217 struct vsp1_rwpf *wpf = vsp1->wpf[i]; 1218 struct vsp1_pipeline *pipe; 1219 1220 if (wpf == NULL) 1221 continue; 1222 1223 pipe = wpf->entity.pipe; 1224 if (pipe == NULL) 1225 continue; 1226 1227 /* 1228 * The hardware may have been reset during a suspend and will 1229 * need a full reconfiguration. 1230 */ 1231 pipe->configured = false; 1232 1233 spin_lock_irqsave(&pipe->irqlock, flags); 1234 if (vsp1_pipeline_ready(pipe)) 1235 vsp1_video_pipeline_run(pipe); 1236 spin_unlock_irqrestore(&pipe->irqlock, flags); 1237 } 1238} 1239 1240/* ----------------------------------------------------------------------------- 1241 * Initialization and Cleanup 1242 */ 1243 1244struct vsp1_video *vsp1_video_create(struct vsp1_device *vsp1, 1245 struct vsp1_rwpf *rwpf) 1246{ 1247 struct vsp1_video *video; 1248 const char *direction; 1249 int ret; 1250 1251 video = devm_kzalloc(vsp1->dev, sizeof(*video), GFP_KERNEL); 1252 if (!video) 1253 return ERR_PTR(-ENOMEM); 1254 1255 rwpf->video = video; 1256 1257 video->vsp1 = vsp1; 1258 video->rwpf = rwpf; 1259 1260 if (rwpf->entity.type == VSP1_ENTITY_RPF) { 1261 direction = "input"; 1262 video->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; 1263 video->pad.flags = MEDIA_PAD_FL_SOURCE; 1264 video->video.vfl_dir = VFL_DIR_TX; 1265 video->video.device_caps = V4L2_CAP_VIDEO_OUTPUT_MPLANE | 1266 V4L2_CAP_STREAMING; 1267 } else { 1268 direction = "output"; 1269 video->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; 1270 video->pad.flags = MEDIA_PAD_FL_SINK; 1271 video->video.vfl_dir = VFL_DIR_RX; 1272 video->video.device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | 1273 V4L2_CAP_STREAMING; 1274 } 1275 1276 mutex_init(&video->lock); 1277 spin_lock_init(&video->irqlock); 1278 INIT_LIST_HEAD(&video->irqqueue); 1279 1280 /* Initialize the media entity... */ 1281 ret = media_entity_pads_init(&video->video.entity, 1, &video->pad); 1282 if (ret < 0) 1283 return ERR_PTR(ret); 1284 1285 /* ... and the format ... */ 1286 rwpf->format.pixelformat = VSP1_VIDEO_DEF_FORMAT; 1287 rwpf->format.width = VSP1_VIDEO_DEF_WIDTH; 1288 rwpf->format.height = VSP1_VIDEO_DEF_HEIGHT; 1289 __vsp1_video_try_format(video, &rwpf->format, &rwpf->fmtinfo); 1290 1291 /* ... and the video node... */ 1292 video->video.v4l2_dev = &video->vsp1->v4l2_dev; 1293 video->video.fops = &vsp1_video_fops; 1294 snprintf(video->video.name, sizeof(video->video.name), "%s %s", 1295 rwpf->entity.subdev.name, direction); 1296 video->video.vfl_type = VFL_TYPE_VIDEO; 1297 video->video.release = video_device_release_empty; 1298 video->video.ioctl_ops = &vsp1_video_ioctl_ops; 1299 1300 video_set_drvdata(&video->video, video); 1301 1302 video->queue.type = video->type; 1303 video->queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; 1304 video->queue.lock = &video->lock; 1305 video->queue.drv_priv = video; 1306 video->queue.buf_struct_size = sizeof(struct vsp1_vb2_buffer); 1307 video->queue.ops = &vsp1_video_queue_qops; 1308 video->queue.mem_ops = &vb2_dma_contig_memops; 1309 video->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; 1310 video->queue.dev = video->vsp1->bus_master; 1311 ret = vb2_queue_init(&video->queue); 1312 if (ret < 0) { 1313 dev_err(video->vsp1->dev, "failed to initialize vb2 queue\n"); 1314 goto error; 1315 } 1316 1317 /* ... and register the video device. */ 1318 video->video.queue = &video->queue; 1319 ret = video_register_device(&video->video, VFL_TYPE_VIDEO, -1); 1320 if (ret < 0) { 1321 dev_err(video->vsp1->dev, "failed to register video device\n"); 1322 goto error; 1323 } 1324 1325 return video; 1326 1327error: 1328 vsp1_video_cleanup(video); 1329 return ERR_PTR(ret); 1330} 1331 1332void vsp1_video_cleanup(struct vsp1_video *video) 1333{ 1334 if (video_is_registered(&video->video)) 1335 video_unregister_device(&video->video); 1336 1337 media_entity_cleanup(&video->video.entity); 1338} 1339