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 *
vsp1_video_remote_subdev(struct media_pad * local,u32 * pad)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
vsp1_video_verify_format(struct vsp1_video * video)63 static int vsp1_video_verify_format(struct vsp1_video *video)
64 {
65 struct v4l2_subdev_format fmt = {
66 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
67 };
68 struct v4l2_subdev *subdev;
69 int ret;
70
71 subdev = vsp1_video_remote_subdev(&video->pad, &fmt.pad);
72 if (subdev == NULL)
73 return -EINVAL;
74
75 ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
76 if (ret < 0)
77 return ret == -ENOIOCTLCMD ? -EINVAL : ret;
78
79 if (video->rwpf->fmtinfo->mbus != fmt.format.code ||
80 video->rwpf->format.height != fmt.format.height ||
81 video->rwpf->format.width != fmt.format.width) {
82 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
__vsp1_video_try_format(struct vsp1_video * video,struct v4l2_pix_format_mplane * pix,const struct vsp1_format_info ** fmtinfo)93 static 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 XRGB
111 * equivalent. This selects the format older userspace applications want
112 * 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 the
123 * 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 in
149 * the datasheet, strides not aligned to a multiple of 128 bytes result
150 * 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].bytesperline
164 * 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 Management
183 */
184
185 /*
186 * vsp1_video_complete_buffer - Complete the current buffer
187 * @video: the video node
188 *
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 */
194 static struct vsp1_vb2_buffer *
vsp1_video_complete_buffer(struct vsp1_video * video)195 vsp1_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
vsp1_video_frame_end(struct vsp1_pipeline * pipe,struct vsp1_rwpf * rwpf)231 static 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
vsp1_video_pipeline_run_partition(struct vsp1_pipeline * pipe,struct vsp1_dl_list * dl,unsigned int partition)245 static 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
vsp1_video_pipeline_run(struct vsp1_pipeline * pipe)257 static 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 when
269 * processing the first frame or after a system suspend/resume), add the
270 * cached stream configuration to the display list to perform a full
271 * 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 only
292 * the partitions that had a dl available. The frame end
293 * 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
vsp1_video_pipeline_frame_end(struct vsp1_pipeline * pipe,unsigned int completion)311 static 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 and
339 * 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
vsp1_video_pipeline_build_branch(struct vsp1_pipeline * pipe,struct vsp1_rwpf * input,struct vsp1_rwpf * output)349 static 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, use
365 * 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 pad
387 * 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
433 out:
434 media_entity_enum_cleanup(&ent_enum);
435
436 return ret;
437 }
438
vsp1_video_pipeline_build(struct vsp1_pipeline * pipe,struct vsp1_video * video)439 static 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 graph
511 * 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
vsp1_video_pipeline_init(struct vsp1_pipeline * pipe,struct vsp1_video * video)526 static 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
vsp1_video_pipeline_get(struct vsp1_video * video)544 static 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 already
551 * been allocated just increment its reference count and return it.
552 * Otherwise allocate a new pipeline and initialize it, it will be freed
553 * 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
vsp1_video_pipeline_release(struct kref * kref)574 static 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
vsp1_video_pipeline_put(struct vsp1_pipeline * pipe)582 static 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 Operations
593 */
594
595 static int
vsp1_video_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])596 vsp1_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
vsp1_video_buffer_prepare(struct vb2_buffer * vb)622 static 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
vsp1_video_buffer_queue(struct vb2_buffer * vb)646 static 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
vsp1_video_pipeline_setup_partitions(struct vsp1_pipeline * pipe)675 static 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 format
685 * 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 operate
693 * 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
vsp1_video_setup_pipeline(struct vsp1_pipeline * pipe)723 static 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 and
739 * BRS output alpha value is fixed to 255. Otherwise we need to
740 * scale the alpha component only when available at the input
741 * 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 cached
756 * 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
vsp1_video_release_buffers(struct vsp1_video * video)772 static 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
vsp1_video_cleanup_pipeline(struct vsp1_pipeline * pipe)785 static 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
vsp1_video_start_streaming(struct vb2_queue * vq,unsigned int count)799 static 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 streams
825 * are prepared and the pipeline is configured, as multiple streams
826 * can race through streamon with buffers already queued; Therefore we
827 * don't even attempt to start the pipeline until the last stream has
828 * 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
vsp1_video_stop_streaming(struct vb2_queue * vq)841 static 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 started
850 * 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
872 static 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 .start_streaming = vsp1_video_start_streaming,
877 .stop_streaming = vsp1_video_stop_streaming,
878 };
879
880 /* -----------------------------------------------------------------------------
881 * V4L2 ioctls
882 */
883
884 static int
vsp1_video_querycap(struct file * file,void * fh,struct v4l2_capability * cap)885 vsp1_video_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
886 {
887 struct v4l2_fh *vfh = file->private_data;
888 struct vsp1_video *video = to_vsp1_video(vfh->vdev);
889
890 cap->capabilities = V4L2_CAP_DEVICE_CAPS | V4L2_CAP_STREAMING
891 | V4L2_CAP_VIDEO_CAPTURE_MPLANE
892 | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
893
894
895 strscpy(cap->driver, "vsp1", sizeof(cap->driver));
896 strscpy(cap->card, video->video.name, sizeof(cap->card));
897
898 return 0;
899 }
900
901 static int
vsp1_video_get_format(struct file * file,void * fh,struct v4l2_format * format)902 vsp1_video_get_format(struct file *file, void *fh, struct v4l2_format *format)
903 {
904 struct v4l2_fh *vfh = file->private_data;
905 struct vsp1_video *video = to_vsp1_video(vfh->vdev);
906
907 if (format->type != video->queue.type)
908 return -EINVAL;
909
910 mutex_lock(&video->lock);
911 format->fmt.pix_mp = video->rwpf->format;
912 mutex_unlock(&video->lock);
913
914 return 0;
915 }
916
917 static int
vsp1_video_try_format(struct file * file,void * fh,struct v4l2_format * format)918 vsp1_video_try_format(struct file *file, void *fh, struct v4l2_format *format)
919 {
920 struct v4l2_fh *vfh = file->private_data;
921 struct vsp1_video *video = to_vsp1_video(vfh->vdev);
922
923 if (format->type != video->queue.type)
924 return -EINVAL;
925
926 return __vsp1_video_try_format(video, &format->fmt.pix_mp, NULL);
927 }
928
929 static int
vsp1_video_set_format(struct file * file,void * fh,struct v4l2_format * format)930 vsp1_video_set_format(struct file *file, void *fh, struct v4l2_format *format)
931 {
932 struct v4l2_fh *vfh = file->private_data;
933 struct vsp1_video *video = to_vsp1_video(vfh->vdev);
934 const struct vsp1_format_info *info;
935 int ret;
936
937 if (format->type != video->queue.type)
938 return -EINVAL;
939
940 ret = __vsp1_video_try_format(video, &format->fmt.pix_mp, &info);
941 if (ret < 0)
942 return ret;
943
944 mutex_lock(&video->lock);
945
946 if (vb2_is_busy(&video->queue)) {
947 ret = -EBUSY;
948 goto done;
949 }
950
951 video->rwpf->format = format->fmt.pix_mp;
952 video->rwpf->fmtinfo = info;
953
954 done:
955 mutex_unlock(&video->lock);
956 return ret;
957 }
958
959 static int
vsp1_video_streamon(struct file * file,void * fh,enum v4l2_buf_type type)960 vsp1_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
961 {
962 struct v4l2_fh *vfh = file->private_data;
963 struct vsp1_video *video = to_vsp1_video(vfh->vdev);
964 struct media_device *mdev = &video->vsp1->media_dev;
965 struct vsp1_pipeline *pipe;
966 int ret;
967
968 if (vb2_queue_is_busy(&video->queue, file))
969 return -EBUSY;
970
971 /*
972 * Get a pipeline for the video node and start streaming on it. No link
973 * touching an entity in the pipeline can be activated or deactivated
974 * once streaming is started.
975 */
976 mutex_lock(&mdev->graph_mutex);
977
978 pipe = vsp1_video_pipeline_get(video);
979 if (IS_ERR(pipe)) {
980 mutex_unlock(&mdev->graph_mutex);
981 return PTR_ERR(pipe);
982 }
983
984 ret = __video_device_pipeline_start(&video->video, &pipe->pipe);
985 if (ret < 0) {
986 mutex_unlock(&mdev->graph_mutex);
987 goto err_pipe;
988 }
989
990 mutex_unlock(&mdev->graph_mutex);
991
992 /*
993 * Verify that the configured format matches the output of the connected
994 * subdev.
995 */
996 ret = vsp1_video_verify_format(video);
997 if (ret < 0)
998 goto err_stop;
999
1000 /* Start the queue. */
1001 ret = vb2_streamon(&video->queue, type);
1002 if (ret < 0)
1003 goto err_stop;
1004
1005 return 0;
1006
1007 err_stop:
1008 video_device_pipeline_stop(&video->video);
1009 err_pipe:
1010 vsp1_video_pipeline_put(pipe);
1011 return ret;
1012 }
1013
1014 static const struct v4l2_ioctl_ops vsp1_video_ioctl_ops = {
1015 .vidioc_querycap = vsp1_video_querycap,
1016 .vidioc_g_fmt_vid_cap_mplane = vsp1_video_get_format,
1017 .vidioc_s_fmt_vid_cap_mplane = vsp1_video_set_format,
1018 .vidioc_try_fmt_vid_cap_mplane = vsp1_video_try_format,
1019 .vidioc_g_fmt_vid_out_mplane = vsp1_video_get_format,
1020 .vidioc_s_fmt_vid_out_mplane = vsp1_video_set_format,
1021 .vidioc_try_fmt_vid_out_mplane = vsp1_video_try_format,
1022 .vidioc_reqbufs = vb2_ioctl_reqbufs,
1023 .vidioc_querybuf = vb2_ioctl_querybuf,
1024 .vidioc_qbuf = vb2_ioctl_qbuf,
1025 .vidioc_dqbuf = vb2_ioctl_dqbuf,
1026 .vidioc_expbuf = vb2_ioctl_expbuf,
1027 .vidioc_create_bufs = vb2_ioctl_create_bufs,
1028 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
1029 .vidioc_streamon = vsp1_video_streamon,
1030 .vidioc_streamoff = vb2_ioctl_streamoff,
1031 };
1032
1033 /* -----------------------------------------------------------------------------
1034 * V4L2 File Operations
1035 */
1036
vsp1_video_open(struct file * file)1037 static int vsp1_video_open(struct file *file)
1038 {
1039 struct vsp1_video *video = video_drvdata(file);
1040 struct v4l2_fh *vfh;
1041 int ret = 0;
1042
1043 vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
1044 if (vfh == NULL)
1045 return -ENOMEM;
1046
1047 v4l2_fh_init(vfh, &video->video);
1048 v4l2_fh_add(vfh);
1049
1050 file->private_data = vfh;
1051
1052 ret = vsp1_device_get(video->vsp1);
1053 if (ret < 0) {
1054 v4l2_fh_del(vfh);
1055 v4l2_fh_exit(vfh);
1056 kfree(vfh);
1057 }
1058
1059 return ret;
1060 }
1061
vsp1_video_release(struct file * file)1062 static int vsp1_video_release(struct file *file)
1063 {
1064 struct vsp1_video *video = video_drvdata(file);
1065
1066 vb2_fop_release(file);
1067
1068 vsp1_device_put(video->vsp1);
1069
1070 return 0;
1071 }
1072
1073 static const struct v4l2_file_operations vsp1_video_fops = {
1074 .owner = THIS_MODULE,
1075 .unlocked_ioctl = video_ioctl2,
1076 .open = vsp1_video_open,
1077 .release = vsp1_video_release,
1078 .poll = vb2_fop_poll,
1079 .mmap = vb2_fop_mmap,
1080 };
1081
1082 /* -----------------------------------------------------------------------------
1083 * Media entity operations
1084 */
1085
vsp1_video_link_validate(struct media_link * link)1086 static int vsp1_video_link_validate(struct media_link *link)
1087 {
1088 /*
1089 * Ideally, link validation should be implemented here instead of
1090 * calling vsp1_video_verify_format() in vsp1_video_streamon()
1091 * manually. That would however break userspace that start one video
1092 * device before configures formats on other video devices in the
1093 * pipeline. This operation is just a no-op to silence the warnings
1094 * from v4l2_subdev_link_validate().
1095 */
1096 return 0;
1097 }
1098
1099 static const struct media_entity_operations vsp1_video_media_ops = {
1100 .link_validate = vsp1_video_link_validate,
1101 };
1102
1103 /* -----------------------------------------------------------------------------
1104 * Suspend and Resume
1105 */
1106
vsp1_video_suspend(struct vsp1_device * vsp1)1107 void vsp1_video_suspend(struct vsp1_device *vsp1)
1108 {
1109 unsigned long flags;
1110 unsigned int i;
1111 int ret;
1112
1113 /*
1114 * To avoid increasing the system suspend time needlessly, loop over the
1115 * pipelines twice, first to set them all to the stopping state, and
1116 * then to wait for the stop to complete.
1117 */
1118 for (i = 0; i < vsp1->info->wpf_count; ++i) {
1119 struct vsp1_rwpf *wpf = vsp1->wpf[i];
1120 struct vsp1_pipeline *pipe;
1121
1122 if (wpf == NULL)
1123 continue;
1124
1125 pipe = wpf->entity.pipe;
1126 if (pipe == NULL)
1127 continue;
1128
1129 spin_lock_irqsave(&pipe->irqlock, flags);
1130 if (pipe->state == VSP1_PIPELINE_RUNNING)
1131 pipe->state = VSP1_PIPELINE_STOPPING;
1132 spin_unlock_irqrestore(&pipe->irqlock, flags);
1133 }
1134
1135 for (i = 0; i < vsp1->info->wpf_count; ++i) {
1136 struct vsp1_rwpf *wpf = vsp1->wpf[i];
1137 struct vsp1_pipeline *pipe;
1138
1139 if (wpf == NULL)
1140 continue;
1141
1142 pipe = wpf->entity.pipe;
1143 if (pipe == NULL)
1144 continue;
1145
1146 ret = wait_event_timeout(pipe->wq, vsp1_pipeline_stopped(pipe),
1147 msecs_to_jiffies(500));
1148 if (ret == 0)
1149 dev_warn(vsp1->dev, "pipeline %u stop timeout\n",
1150 wpf->entity.index);
1151 }
1152 }
1153
vsp1_video_resume(struct vsp1_device * vsp1)1154 void vsp1_video_resume(struct vsp1_device *vsp1)
1155 {
1156 unsigned long flags;
1157 unsigned int i;
1158
1159 /* Resume all running pipelines. */
1160 for (i = 0; i < vsp1->info->wpf_count; ++i) {
1161 struct vsp1_rwpf *wpf = vsp1->wpf[i];
1162 struct vsp1_pipeline *pipe;
1163
1164 if (wpf == NULL)
1165 continue;
1166
1167 pipe = wpf->entity.pipe;
1168 if (pipe == NULL)
1169 continue;
1170
1171 /*
1172 * The hardware may have been reset during a suspend and will
1173 * need a full reconfiguration.
1174 */
1175 pipe->configured = false;
1176
1177 spin_lock_irqsave(&pipe->irqlock, flags);
1178 if (vsp1_pipeline_ready(pipe))
1179 vsp1_video_pipeline_run(pipe);
1180 spin_unlock_irqrestore(&pipe->irqlock, flags);
1181 }
1182 }
1183
1184 /* -----------------------------------------------------------------------------
1185 * Initialization and Cleanup
1186 */
1187
vsp1_video_create(struct vsp1_device * vsp1,struct vsp1_rwpf * rwpf)1188 struct vsp1_video *vsp1_video_create(struct vsp1_device *vsp1,
1189 struct vsp1_rwpf *rwpf)
1190 {
1191 struct vsp1_video *video;
1192 const char *direction;
1193 int ret;
1194
1195 video = devm_kzalloc(vsp1->dev, sizeof(*video), GFP_KERNEL);
1196 if (!video)
1197 return ERR_PTR(-ENOMEM);
1198
1199 rwpf->video = video;
1200
1201 video->vsp1 = vsp1;
1202 video->rwpf = rwpf;
1203
1204 if (rwpf->entity.type == VSP1_ENTITY_RPF) {
1205 direction = "input";
1206 video->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
1207 video->pad.flags = MEDIA_PAD_FL_SOURCE;
1208 video->video.vfl_dir = VFL_DIR_TX;
1209 video->video.device_caps = V4L2_CAP_VIDEO_OUTPUT_MPLANE |
1210 V4L2_CAP_STREAMING;
1211 } else {
1212 direction = "output";
1213 video->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1214 video->pad.flags = MEDIA_PAD_FL_SINK;
1215 video->video.vfl_dir = VFL_DIR_RX;
1216 video->video.device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
1217 V4L2_CAP_STREAMING;
1218 }
1219
1220 mutex_init(&video->lock);
1221 spin_lock_init(&video->irqlock);
1222 INIT_LIST_HEAD(&video->irqqueue);
1223
1224 /* Initialize the media entity... */
1225 ret = media_entity_pads_init(&video->video.entity, 1, &video->pad);
1226 if (ret < 0)
1227 return ERR_PTR(ret);
1228
1229 /* ... and the format ... */
1230 rwpf->format.pixelformat = VSP1_VIDEO_DEF_FORMAT;
1231 rwpf->format.width = VSP1_VIDEO_DEF_WIDTH;
1232 rwpf->format.height = VSP1_VIDEO_DEF_HEIGHT;
1233 __vsp1_video_try_format(video, &rwpf->format, &rwpf->fmtinfo);
1234
1235 /* ... and the video node... */
1236 video->video.v4l2_dev = &video->vsp1->v4l2_dev;
1237 video->video.entity.ops = &vsp1_video_media_ops;
1238 video->video.fops = &vsp1_video_fops;
1239 snprintf(video->video.name, sizeof(video->video.name), "%s %s",
1240 rwpf->entity.subdev.name, direction);
1241 video->video.vfl_type = VFL_TYPE_VIDEO;
1242 video->video.release = video_device_release_empty;
1243 video->video.ioctl_ops = &vsp1_video_ioctl_ops;
1244
1245 video_set_drvdata(&video->video, video);
1246
1247 video->queue.type = video->type;
1248 video->queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1249 video->queue.lock = &video->lock;
1250 video->queue.drv_priv = video;
1251 video->queue.buf_struct_size = sizeof(struct vsp1_vb2_buffer);
1252 video->queue.ops = &vsp1_video_queue_qops;
1253 video->queue.mem_ops = &vb2_dma_contig_memops;
1254 video->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1255 video->queue.dev = video->vsp1->bus_master;
1256 ret = vb2_queue_init(&video->queue);
1257 if (ret < 0) {
1258 dev_err(video->vsp1->dev, "failed to initialize vb2 queue\n");
1259 goto error;
1260 }
1261
1262 /* ... and register the video device. */
1263 video->video.queue = &video->queue;
1264 ret = video_register_device(&video->video, VFL_TYPE_VIDEO, -1);
1265 if (ret < 0) {
1266 dev_err(video->vsp1->dev, "failed to register video device\n");
1267 goto error;
1268 }
1269
1270 return video;
1271
1272 error:
1273 vsp1_video_cleanup(video);
1274 return ERR_PTR(ret);
1275 }
1276
vsp1_video_cleanup(struct vsp1_video * video)1277 void vsp1_video_cleanup(struct vsp1_video *video)
1278 {
1279 if (video_is_registered(&video->video))
1280 video_unregister_device(&video->video);
1281
1282 media_entity_cleanup(&video->video.entity);
1283 }
1284