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 .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 ioctls
884 */
885
886 static int
vsp1_video_querycap(struct file * file,void * fh,struct v4l2_capability * cap)887 vsp1_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_STREAMING
893 | V4L2_CAP_VIDEO_CAPTURE_MPLANE
894 | 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
903 static int
vsp1_video_get_format(struct file * file,void * fh,struct v4l2_format * format)904 vsp1_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
919 static int
vsp1_video_try_format(struct file * file,void * fh,struct v4l2_format * format)920 vsp1_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
931 static int
vsp1_video_set_format(struct file * file,void * fh,struct v4l2_format * format)932 vsp1_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
956 done:
957 mutex_unlock(&video->lock);
958 return ret;
959 }
960
961 static int
vsp1_video_streamon(struct file * file,void * fh,enum v4l2_buf_type type)962 vsp1_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 link
975 * touching an entity in the pipeline can be activated or deactivated
976 * 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 connected
996 * 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
1009 err_stop:
1010 video_device_pipeline_stop(&video->video);
1011 err_pipe:
1012 vsp1_video_pipeline_put(pipe);
1013 return ret;
1014 }
1015
1016 static 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 Operations
1037 */
1038
vsp1_video_open(struct file * file)1039 static 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
vsp1_video_release(struct file * file)1064 static 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
1075 static 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 operations
1086 */
1087
vsp1_video_link_validate(struct media_link * link)1088 static int vsp1_video_link_validate(struct media_link *link)
1089 {
1090 /*
1091 * Ideally, link validation should be implemented here instead of
1092 * calling vsp1_video_verify_format() in vsp1_video_streamon()
1093 * manually. That would however break userspace that start one video
1094 * device before configures formats on other video devices in the
1095 * pipeline. This operation is just a no-op to silence the warnings
1096 * from v4l2_subdev_link_validate().
1097 */
1098 return 0;
1099 }
1100
1101 static const struct media_entity_operations vsp1_video_media_ops = {
1102 .link_validate = vsp1_video_link_validate,
1103 };
1104
1105 /* -----------------------------------------------------------------------------
1106 * Suspend and Resume
1107 */
1108
vsp1_video_suspend(struct vsp1_device * vsp1)1109 void 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 the
1117 * pipelines twice, first to set them all to the stopping state, and
1118 * 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
vsp1_video_resume(struct vsp1_device * vsp1)1156 void 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 will
1175 * 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 Cleanup
1188 */
1189
vsp1_video_create(struct vsp1_device * vsp1,struct vsp1_rwpf * rwpf)1190 struct 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
1274 error:
1275 vsp1_video_cleanup(video);
1276 return ERR_PTR(ret);
1277 }
1278
vsp1_video_cleanup(struct vsp1_video * video)1279 void 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