rkcif-stream.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Rockchip Camera Interface (CIF) Driver
  4 *
  5 * Copyright (C) 2025 Michael Riesch <michael.riesch@wolfvision.net>
  6 * Copyright (C) 2025 Collabora, Ltd.
  7 */
  8
  9#include <linux/pm_runtime.h>
 10
 11#include <media/v4l2-common.h>
 12#include <media/v4l2-fwnode.h>
 13#include <media/v4l2-ioctl.h>
 14#include <media/v4l2-mc.h>
 15#include <media/v4l2-subdev.h>
 16#include <media/videobuf2-dma-contig.h>
 17
 18#include "rkcif-common.h"
 19#include "rkcif-stream.h"
 20
 21#define CIF_REQ_BUFS_MIN 1
 22#define CIF_MIN_WIDTH	 64
 23#define CIF_MIN_HEIGHT	 64
 24#define CIF_MAX_WIDTH	 8192
 25#define CIF_MAX_HEIGHT	 8192
 26
 27static inline struct rkcif_buffer *to_rkcif_buffer(struct vb2_v4l2_buffer *vb)
 28{
 29	return container_of(vb, struct rkcif_buffer, vb);
 30}
 31
 32static inline struct rkcif_stream *to_rkcif_stream(struct video_device *vdev)
 33{
 34	return container_of(vdev, struct rkcif_stream, vdev);
 35}
 36
 37static struct rkcif_buffer *rkcif_stream_pop_buffer(struct rkcif_stream *stream)
 38{
 39	struct rkcif_buffer *buffer;
 40
 41	guard(spinlock_irqsave)(&stream->driver_queue_lock);
 42
 43	if (list_empty(&stream->driver_queue))
 44		return NULL;
 45
 46	buffer = list_first_entry(&stream->driver_queue, struct rkcif_buffer,
 47				  queue);
 48	list_del(&buffer->queue);
 49
 50	return buffer;
 51}
 52
 53static void rkcif_stream_push_buffer(struct rkcif_stream *stream,
 54				     struct rkcif_buffer *buffer)
 55{
 56	guard(spinlock_irqsave)(&stream->driver_queue_lock);
 57
 58	list_add_tail(&buffer->queue, &stream->driver_queue);
 59}
 60
 61static inline void rkcif_stream_return_buffer(struct rkcif_buffer *buffer,
 62					      enum vb2_buffer_state state)
 63{
 64	struct vb2_v4l2_buffer *vb = &buffer->vb;
 65
 66	vb2_buffer_done(&vb->vb2_buf, state);
 67}
 68
 69static void rkcif_stream_complete_buffer(struct rkcif_stream *stream,
 70					 struct rkcif_buffer *buffer)
 71{
 72	struct vb2_v4l2_buffer *vb = &buffer->vb;
 73
 74	vb->vb2_buf.timestamp = ktime_get_ns();
 75	vb->sequence = stream->frame_idx;
 76	vb2_buffer_done(&vb->vb2_buf, VB2_BUF_STATE_DONE);
 77	stream->frame_idx++;
 78}
 79
 80void rkcif_stream_pingpong(struct rkcif_stream *stream)
 81{
 82	struct rkcif_buffer *buffer;
 83
 84	buffer = stream->buffers[stream->frame_phase];
 85	if (!buffer->is_dummy)
 86		rkcif_stream_complete_buffer(stream, buffer);
 87
 88	buffer = rkcif_stream_pop_buffer(stream);
 89	if (buffer) {
 90		stream->buffers[stream->frame_phase] = buffer;
 91		stream->buffers[stream->frame_phase]->is_dummy = false;
 92	} else {
 93		stream->buffers[stream->frame_phase] = &stream->dummy.buffer;
 94		stream->buffers[stream->frame_phase]->is_dummy = true;
 95		dev_dbg(stream->rkcif->dev,
 96			"no buffer available, frame will be dropped\n");
 97	}
 98
 99	if (stream->queue_buffer)
100		stream->queue_buffer(stream, stream->frame_phase);
101
102	stream->frame_phase = 1 - stream->frame_phase;
103}
104
105static int rkcif_stream_init_buffers(struct rkcif_stream *stream)
106{
107	struct v4l2_pix_format_mplane *pix = &stream->pix;
108
109	stream->buffers[0] = rkcif_stream_pop_buffer(stream);
110	if (!stream->buffers[0])
111		goto err_buff_0;
112
113	stream->buffers[1] = rkcif_stream_pop_buffer(stream);
114	if (!stream->buffers[1])
115		goto err_buff_1;
116
117	if (stream->queue_buffer) {
118		stream->queue_buffer(stream, 0);
119		stream->queue_buffer(stream, 1);
120	}
121
122	stream->dummy.size = pix->num_planes * pix->plane_fmt[0].sizeimage;
123	stream->dummy.vaddr =
124		dma_alloc_attrs(stream->rkcif->dev, stream->dummy.size,
125				&stream->dummy.buffer.buff_addr[0], GFP_KERNEL,
126				DMA_ATTR_NO_KERNEL_MAPPING);
127	if (!stream->dummy.vaddr)
128		goto err_dummy;
129
130	for (unsigned int i = 1; i < pix->num_planes; i++)
131		stream->dummy.buffer.buff_addr[i] =
132			stream->dummy.buffer.buff_addr[i - 1] +
133			pix->plane_fmt[i - 1].bytesperline * pix->height;
134
135	return 0;
136
137err_dummy:
138	rkcif_stream_return_buffer(stream->buffers[1], VB2_BUF_STATE_QUEUED);
139	stream->buffers[1] = NULL;
140
141err_buff_1:
142	rkcif_stream_return_buffer(stream->buffers[0], VB2_BUF_STATE_QUEUED);
143	stream->buffers[0] = NULL;
144err_buff_0:
145	return -EINVAL;
146}
147
148static void rkcif_stream_return_all_buffers(struct rkcif_stream *stream,
149					    enum vb2_buffer_state state)
150{
151	struct rkcif_buffer *buffer;
152
153	if (stream->buffers[0] && !stream->buffers[0]->is_dummy) {
154		rkcif_stream_return_buffer(stream->buffers[0], state);
155		stream->buffers[0] = NULL;
156	}
157
158	if (stream->buffers[1] && !stream->buffers[1]->is_dummy) {
159		rkcif_stream_return_buffer(stream->buffers[1], state);
160		stream->buffers[1] = NULL;
161	}
162
163	while ((buffer = rkcif_stream_pop_buffer(stream)))
164		rkcif_stream_return_buffer(buffer, state);
165
166	if (stream->dummy.vaddr) {
167		dma_free_attrs(stream->rkcif->dev, stream->dummy.size,
168			       stream->dummy.vaddr,
169			       stream->dummy.buffer.buff_addr[0],
170			       DMA_ATTR_NO_KERNEL_MAPPING);
171		stream->dummy.vaddr = NULL;
172	}
173}
174
175static int rkcif_stream_setup_queue(struct vb2_queue *queue,
176				    unsigned int *num_buffers,
177				    unsigned int *num_planes,
178				    unsigned int sizes[],
179				    struct device *alloc_devs[])
180{
181	struct rkcif_stream *stream = queue->drv_priv;
182	struct v4l2_pix_format_mplane *pix = &stream->pix;
183
184	if (*num_planes) {
185		if (*num_planes != pix->num_planes)
186			return -EINVAL;
187
188		for (unsigned int i = 0; i < pix->num_planes; i++)
189			if (sizes[i] < pix->plane_fmt[i].sizeimage)
190				return -EINVAL;
191	} else {
192		*num_planes = pix->num_planes;
193		for (unsigned int i = 0; i < pix->num_planes; i++)
194			sizes[i] = pix->plane_fmt[i].sizeimage;
195	}
196
197	return 0;
198}
199
200static int rkcif_stream_prepare_buffer(struct vb2_buffer *vb)
201{
202	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
203	struct rkcif_buffer *buffer = to_rkcif_buffer(vbuf);
204	struct rkcif_stream *stream = vb->vb2_queue->drv_priv;
205	const struct rkcif_output_fmt *fmt;
206	struct v4l2_pix_format_mplane *pix = &stream->pix;
207	unsigned int i;
208
209	memset(buffer->buff_addr, 0, sizeof(buffer->buff_addr));
210	for (i = 0; i < pix->num_planes; i++)
211		buffer->buff_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
212
213	/* apply fallback for non-mplane formats, if required */
214	if (pix->num_planes == 1) {
215		fmt = rkcif_stream_find_output_fmt(stream, true,
216						   pix->pixelformat);
217		for (i = 1; i < fmt->cplanes; i++)
218			buffer->buff_addr[i] =
219				buffer->buff_addr[i - 1] +
220				pix->plane_fmt[i - 1].bytesperline *
221					pix->height;
222	}
223
224	for (i = 0; i < pix->num_planes; i++) {
225		unsigned long size = pix->plane_fmt[i].sizeimage;
226
227		if (vb2_plane_size(vb, i) < size) {
228			dev_err(stream->rkcif->dev,
229				"user buffer too small (%ld < %ld)\n",
230				vb2_plane_size(vb, i), size);
231			return -EINVAL;
232		}
233
234		vb2_set_plane_payload(vb, i, size);
235	}
236
237	vbuf->field = V4L2_FIELD_NONE;
238
239	return 0;
240}
241
242static void rkcif_stream_queue_buffer(struct vb2_buffer *vb)
243{
244	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
245	struct rkcif_buffer *buffer = to_rkcif_buffer(vbuf);
246	struct rkcif_stream *stream = vb->vb2_queue->drv_priv;
247
248	rkcif_stream_push_buffer(stream, buffer);
249}
250
251static int rkcif_stream_start_streaming(struct vb2_queue *queue,
252					unsigned int count)
253{
254	struct rkcif_stream *stream = queue->drv_priv;
255	struct rkcif_device *rkcif = stream->rkcif;
256	u64 mask;
257	int ret;
258
259	stream->frame_idx = 0;
260	stream->frame_phase = 0;
261
262	ret = video_device_pipeline_start(&stream->vdev, &stream->pipeline);
263	if (ret) {
264		dev_err(rkcif->dev, "failed to start pipeline %d\n", ret);
265		goto err_out;
266	}
267
268	ret = pm_runtime_resume_and_get(rkcif->dev);
269	if (ret < 0) {
270		dev_err(rkcif->dev, "failed to get runtime pm, %d\n", ret);
271		goto err_pipeline_stop;
272	}
273
274	ret = rkcif_stream_init_buffers(stream);
275	if (ret)
276		goto err_runtime_put;
277
278	if (stream->start_streaming) {
279		ret = stream->start_streaming(stream);
280		if (ret < 0)
281			goto err_runtime_put;
282	}
283
284	mask = BIT_ULL(stream->id);
285	ret = v4l2_subdev_enable_streams(&stream->interface->sd,
286					 RKCIF_IF_PAD_SRC, mask);
287	if (ret < 0)
288		goto err_stop_stream;
289
290	return 0;
291
292err_stop_stream:
293	if (stream->stop_streaming)
294		stream->stop_streaming(stream);
295err_runtime_put:
296	pm_runtime_put(rkcif->dev);
297err_pipeline_stop:
298	video_device_pipeline_stop(&stream->vdev);
299err_out:
300	rkcif_stream_return_all_buffers(stream, VB2_BUF_STATE_QUEUED);
301	return ret;
302}
303
304static void rkcif_stream_stop_streaming(struct vb2_queue *queue)
305{
306	struct rkcif_stream *stream = queue->drv_priv;
307	struct rkcif_device *rkcif = stream->rkcif;
308	u64 mask;
309	int ret;
310
311	mask = BIT_ULL(stream->id);
312	v4l2_subdev_disable_streams(&stream->interface->sd, RKCIF_IF_PAD_SRC,
313				    mask);
314
315	stream->stopping = true;
316	ret = wait_event_timeout(stream->wq_stopped, !stream->stopping,
317				 msecs_to_jiffies(1000));
318
319	if (!ret && stream->stop_streaming)
320		stream->stop_streaming(stream);
321
322	pm_runtime_put(rkcif->dev);
323
324	rkcif_stream_return_all_buffers(stream, VB2_BUF_STATE_ERROR);
325
326	video_device_pipeline_stop(&stream->vdev);
327}
328
329static const struct vb2_ops rkcif_stream_vb2_ops = {
330	.queue_setup = rkcif_stream_setup_queue,
331	.buf_prepare = rkcif_stream_prepare_buffer,
332	.buf_queue = rkcif_stream_queue_buffer,
333	.start_streaming = rkcif_stream_start_streaming,
334	.stop_streaming = rkcif_stream_stop_streaming,
335};
336
337static int rkcif_stream_fill_format(struct rkcif_stream *stream,
338				    struct v4l2_pix_format_mplane *pix)
339{
340	const struct rkcif_output_fmt *fmt;
341	u32 height, width;
342	int ret;
343
344	fmt = rkcif_stream_find_output_fmt(stream, true, pix->pixelformat);
345	height = clamp_t(u32, pix->height, CIF_MIN_HEIGHT, CIF_MAX_HEIGHT);
346	width = clamp_t(u32, pix->width, CIF_MIN_WIDTH, CIF_MAX_WIDTH);
347	ret = v4l2_fill_pixfmt_mp(pix, fmt->fourcc, width, height);
348	if (ret)
349		return ret;
350
351	pix->field = V4L2_FIELD_NONE;
352
353	return 0;
354}
355
356static int rkcif_stream_try_format(struct file *file, void *fh,
357				   struct v4l2_format *f)
358{
359	struct rkcif_stream *stream = video_drvdata(file);
360	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
361
362	return rkcif_stream_fill_format(stream, pix);
363}
364
365static int rkcif_stream_set_format(struct file *file, void *priv,
366				   struct v4l2_format *f)
367{
368	struct rkcif_stream *stream = video_drvdata(file);
369	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
370	int ret;
371
372	if (vb2_is_busy(&stream->buf_queue))
373		return -EBUSY;
374
375	ret = rkcif_stream_try_format(file, priv, f);
376	if (ret)
377		return ret;
378
379	stream->pix = *pix;
380
381	return 0;
382}
383
384static int rkcif_stream_get_format(struct file *file, void *fh,
385				   struct v4l2_format *f)
386{
387	struct rkcif_stream *stream = video_drvdata(file);
388
389	f->fmt.pix_mp = stream->pix;
390
391	return 0;
392}
393
394static int rkcif_stream_enum_formats(struct file *file, void *priv,
395				     struct v4l2_fmtdesc *f)
396{
397	struct rkcif_stream *stream = video_drvdata(file);
398
399	if (f->index >= stream->out_fmts_num)
400		return -EINVAL;
401
402	f->pixelformat = stream->out_fmts[f->index].fourcc;
403
404	return 0;
405}
406
407static int rkcif_stream_enum_framesizes(struct file *file, void *fh,
408					struct v4l2_frmsizeenum *fsize)
409{
410	struct rkcif_stream *stream = video_drvdata(file);
411
412	if (fsize->index > 0)
413		return -EINVAL;
414
415	if (!rkcif_stream_find_output_fmt(stream, false, fsize->pixel_format))
416		return -EINVAL;
417
418	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
419	fsize->stepwise.min_width = CIF_MIN_WIDTH;
420	fsize->stepwise.max_width = CIF_MAX_WIDTH;
421	fsize->stepwise.step_width = 8;
422	fsize->stepwise.min_height = CIF_MIN_HEIGHT;
423	fsize->stepwise.max_height = CIF_MAX_HEIGHT;
424	fsize->stepwise.step_height = 8;
425
426	return 0;
427}
428
429static int rkcif_stream_querycap(struct file *file, void *priv,
430				 struct v4l2_capability *cap)
431{
432	struct rkcif_stream *stream = video_drvdata(file);
433	struct device *dev = stream->rkcif->dev;
434
435	strscpy(cap->driver, dev->driver->name, sizeof(cap->driver));
436	strscpy(cap->card, dev->driver->name, sizeof(cap->card));
437
438	return 0;
439}
440
441static const struct v4l2_ioctl_ops rkcif_stream_ioctl_ops = {
442	.vidioc_reqbufs = vb2_ioctl_reqbufs,
443	.vidioc_querybuf = vb2_ioctl_querybuf,
444	.vidioc_create_bufs = vb2_ioctl_create_bufs,
445	.vidioc_qbuf = vb2_ioctl_qbuf,
446	.vidioc_expbuf = vb2_ioctl_expbuf,
447	.vidioc_dqbuf = vb2_ioctl_dqbuf,
448	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
449	.vidioc_streamon = vb2_ioctl_streamon,
450	.vidioc_streamoff = vb2_ioctl_streamoff,
451	.vidioc_try_fmt_vid_cap_mplane = rkcif_stream_try_format,
452	.vidioc_s_fmt_vid_cap_mplane = rkcif_stream_set_format,
453	.vidioc_g_fmt_vid_cap_mplane = rkcif_stream_get_format,
454	.vidioc_enum_fmt_vid_cap = rkcif_stream_enum_formats,
455	.vidioc_enum_framesizes = rkcif_stream_enum_framesizes,
456	.vidioc_querycap = rkcif_stream_querycap,
457};
458
459static int rkcif_stream_link_validate(struct media_link *link)
460{
461	struct video_device *vdev =
462		media_entity_to_video_device(link->sink->entity);
463	struct v4l2_mbus_framefmt *source_fmt;
464	struct v4l2_subdev *sd;
465	struct v4l2_subdev_state *state;
466	struct rkcif_stream *stream = to_rkcif_stream(vdev);
467	int ret = -EINVAL;
468
469	if (!media_entity_remote_source_pad_unique(link->sink->entity))
470		return -ENOTCONN;
471
472	sd = media_entity_to_v4l2_subdev(link->source->entity);
473
474	state = v4l2_subdev_lock_and_get_active_state(sd);
475
476	source_fmt = v4l2_subdev_state_get_format(state, link->source->index,
477						  stream->id);
478	if (!source_fmt)
479		goto out;
480
481	if (source_fmt->height != stream->pix.height ||
482	    source_fmt->width != stream->pix.width) {
483		dev_dbg(stream->rkcif->dev,
484			"link '%s':%u -> '%s':%u not valid: %ux%u != %ux%u\n",
485			link->source->entity->name, link->source->index,
486			link->sink->entity->name, link->sink->index,
487			source_fmt->width, source_fmt->height,
488			stream->pix.width, stream->pix.height);
489		goto out;
490	}
491
492	ret = 0;
493
494out:
495	v4l2_subdev_unlock_state(state);
496	return ret;
497}
498
499static const struct media_entity_operations rkcif_stream_media_ops = {
500	.link_validate = rkcif_stream_link_validate,
501};
502
503static const struct v4l2_file_operations rkcif_stream_file_ops = {
504	.open = v4l2_fh_open,
505	.release = vb2_fop_release,
506	.unlocked_ioctl = video_ioctl2,
507	.poll = vb2_fop_poll,
508	.mmap = vb2_fop_mmap,
509};
510
511static int rkcif_stream_init_vb2_queue(struct vb2_queue *q,
512				       struct rkcif_stream *stream)
513{
514	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
515	q->io_modes = VB2_MMAP | VB2_DMABUF;
516	q->drv_priv = stream;
517	q->ops = &rkcif_stream_vb2_ops;
518	q->mem_ops = &vb2_dma_contig_memops;
519	q->buf_struct_size = sizeof(struct rkcif_buffer);
520	q->min_queued_buffers = CIF_REQ_BUFS_MIN;
521	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
522	q->lock = &stream->vlock;
523	q->dev = stream->rkcif->dev;
524
525	return vb2_queue_init(q);
526}
527
528int rkcif_stream_register(struct rkcif_device *rkcif,
529			  struct rkcif_stream *stream)
530{
531	struct rkcif_interface *interface = stream->interface;
532	struct v4l2_device *v4l2_dev = &rkcif->v4l2_dev;
533	struct video_device *vdev = &stream->vdev;
534	u32 link_flags = 0;
535	int ret;
536
537	stream->rkcif = rkcif;
538
539	INIT_LIST_HEAD(&stream->driver_queue);
540	spin_lock_init(&stream->driver_queue_lock);
541
542	init_waitqueue_head(&stream->wq_stopped);
543
544	mutex_init(&stream->vlock);
545
546	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING |
547			    V4L2_CAP_IO_MC;
548	vdev->entity.ops = &rkcif_stream_media_ops;
549	vdev->fops = &rkcif_stream_file_ops;
550	vdev->ioctl_ops = &rkcif_stream_ioctl_ops;
551	vdev->lock = &stream->vlock;
552	vdev->minor = -1;
553	vdev->release = video_device_release_empty;
554	vdev->v4l2_dev = v4l2_dev;
555	vdev->vfl_dir = VFL_DIR_RX;
556	video_set_drvdata(vdev, stream);
557
558	stream->pad.flags = MEDIA_PAD_FL_SINK;
559
560	stream->pix.height = CIF_MIN_HEIGHT;
561	stream->pix.width = CIF_MIN_WIDTH;
562	rkcif_stream_fill_format(stream, &stream->pix);
563
564	rkcif_stream_init_vb2_queue(&stream->buf_queue, stream);
565
566	vdev->queue = &stream->buf_queue;
567	if (interface->type == RKCIF_IF_DVP)
568		snprintf(vdev->name, sizeof(vdev->name), "rkcif-dvp0-id%d",
569			 stream->id);
570	else if (interface->type == RKCIF_IF_MIPI)
571		snprintf(vdev->name, sizeof(vdev->name), "rkcif-mipi%d-id%d",
572			 interface->index - RKCIF_MIPI_BASE, stream->id);
573
574	ret = media_entity_pads_init(&vdev->entity, 1, &stream->pad);
575	if (ret < 0) {
576		dev_err(rkcif->dev,
577			"failed to initialize stream media pad: %d\n", ret);
578		return ret;
579	}
580
581	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
582	if (ret < 0) {
583		dev_err(rkcif->dev, "failed to register video device: %d\n",
584			ret);
585		goto err_media_entity_cleanup;
586	}
587
588	/* enable only stream ID0 by default */
589	if (stream->id == RKCIF_ID0)
590		link_flags |= MEDIA_LNK_FL_ENABLED;
591
592	ret = media_create_pad_link(&interface->sd.entity, RKCIF_IF_PAD_SRC,
593				    &stream->vdev.entity, 0, link_flags);
594	if (ret) {
595		dev_err(rkcif->dev, "failed to link stream media pad: %d\n",
596			ret);
597		goto err_video_unregister;
598	}
599
600	v4l2_info(v4l2_dev, "registered %s as /dev/video%d\n", vdev->name,
601		  vdev->num);
602
603	return 0;
604
605err_video_unregister:
606	video_unregister_device(&stream->vdev);
607err_media_entity_cleanup:
608	media_entity_cleanup(&stream->vdev.entity);
609	return ret;
610}
611
612void rkcif_stream_unregister(struct rkcif_stream *stream)
613{
614	video_unregister_device(&stream->vdev);
615	media_entity_cleanup(&stream->vdev.entity);
616}
617
618const struct rkcif_output_fmt *
619rkcif_stream_find_output_fmt(struct rkcif_stream *stream, bool ret_def,
620			     u32 pixelfmt)
621{
622	const struct rkcif_output_fmt *fmt;
623
624	WARN_ON(stream->out_fmts_num == 0);
625
626	for (unsigned int i = 0; i < stream->out_fmts_num; i++) {
627		fmt = &stream->out_fmts[i];
628		if (fmt->fourcc == pixelfmt)
629			return fmt;
630	}
631
632	if (ret_def)
633		return &stream->out_fmts[0];
634	else
635		return NULL;
636}