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