camss-video.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * camss-video.c
  4 *
  5 * Qualcomm MSM Camera Subsystem - V4L2 device node
  6 *
  7 * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
  8 * Copyright (C) 2015-2018 Linaro Ltd.
  9 */
 10#include <linux/slab.h>
 11#include <media/media-entity.h>
 12#include <media/v4l2-dev.h>
 13#include <media/v4l2-device.h>
 14#include <media/v4l2-ioctl.h>
 15#include <media/v4l2-mc.h>
 16#include <media/videobuf2-dma-sg.h>
 17
 18#include "camss-video.h"
 19#include "camss.h"
 20
 21#define CAMSS_FRAME_MIN_WIDTH		1
 22#define CAMSS_FRAME_MAX_WIDTH		8191
 23#define CAMSS_FRAME_MIN_HEIGHT		1
 24#define CAMSS_FRAME_MAX_HEIGHT_RDI	8191
 25#define CAMSS_FRAME_MAX_HEIGHT_PIX	4096
 26
 27/* -----------------------------------------------------------------------------
 28 * Helper functions
 29 */
 30
 31/*
 32 * video_mbus_to_pix_mp - Convert v4l2_mbus_framefmt to v4l2_pix_format_mplane
 33 * @mbus: v4l2_mbus_framefmt format (input)
 34 * @pix: v4l2_pix_format_mplane format (output)
 35 * @f: a pointer to formats array element to be used for the conversion
 36 * @alignment: bytesperline alignment value
 37 *
 38 * Fill the output pix structure with information from the input mbus format.
 39 *
 40 * Return 0 on success or a negative error code otherwise
 41 */
 42static int video_mbus_to_pix_mp(const struct v4l2_mbus_framefmt *mbus,
 43				struct v4l2_pix_format_mplane *pix,
 44				const struct camss_format_info *f,
 45				unsigned int alignment)
 46{
 47	unsigned int i;
 48	u32 bytesperline;
 49
 50	memset(pix, 0, sizeof(*pix));
 51	v4l2_fill_pix_format_mplane(pix, mbus);
 52	pix->pixelformat = f->pixelformat;
 53	pix->num_planes = f->planes;
 54	for (i = 0; i < pix->num_planes; i++) {
 55		bytesperline = pix->width / f->hsub[i].numerator *
 56			f->hsub[i].denominator * f->bpp[i] / 8;
 57		bytesperline = ALIGN(bytesperline, alignment);
 58		pix->plane_fmt[i].bytesperline = bytesperline;
 59		pix->plane_fmt[i].sizeimage = pix->height /
 60				f->vsub[i].numerator * f->vsub[i].denominator *
 61				bytesperline;
 62	}
 63
 64	return 0;
 65}
 66
 67static struct v4l2_subdev *video_remote_subdev(struct camss_video *video,
 68					       u32 *pad)
 69{
 70	struct media_pad *remote;
 71
 72	remote = media_pad_remote_pad_first(&video->pad);
 73
 74	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
 75		return NULL;
 76
 77	if (pad)
 78		*pad = remote->index;
 79
 80	return media_entity_to_v4l2_subdev(remote->entity);
 81}
 82
 83static int video_get_subdev_format(struct camss_video *video,
 84				   struct v4l2_format *format)
 85{
 86	struct v4l2_subdev_format fmt = {
 87		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 88	};
 89	struct v4l2_subdev *subdev;
 90	u32 pad;
 91	int ret;
 92
 93	subdev = video_remote_subdev(video, &pad);
 94	if (subdev == NULL)
 95		return -EPIPE;
 96
 97	fmt.pad = pad;
 98
 99	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
100	if (ret)
101		return ret;
102
103	ret = camss_format_find_format(fmt.format.code, format->fmt.pix_mp.pixelformat,
104				       video->formats, video->nformats);
105	if (ret < 0)
106		return ret;
107
108	format->type = video->type;
109
110	return video_mbus_to_pix_mp(&fmt.format, &format->fmt.pix_mp,
111				    &video->formats[ret], video->bpl_alignment);
112}
113
114/* -----------------------------------------------------------------------------
115 * Video queue operations
116 */
117
118static int video_queue_setup(struct vb2_queue *q,
119	unsigned int *num_buffers, unsigned int *num_planes,
120	unsigned int sizes[], struct device *alloc_devs[])
121{
122	struct camss_video *video = vb2_get_drv_priv(q);
123	const struct v4l2_pix_format_mplane *format =
124						&video->active_fmt.fmt.pix_mp;
125	unsigned int i;
126
127	if (*num_planes) {
128		if (*num_planes != format->num_planes)
129			return -EINVAL;
130
131		for (i = 0; i < *num_planes; i++)
132			if (sizes[i] < format->plane_fmt[i].sizeimage)
133				return -EINVAL;
134
135		return 0;
136	}
137
138	*num_planes = format->num_planes;
139
140	for (i = 0; i < *num_planes; i++)
141		sizes[i] = format->plane_fmt[i].sizeimage;
142
143	return 0;
144}
145
146static int video_buf_init(struct vb2_buffer *vb)
147{
148	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
149	struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
150	struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer,
151						   vb);
152	const struct v4l2_pix_format_mplane *format =
153						&video->active_fmt.fmt.pix_mp;
154	struct sg_table *sgt;
155	unsigned int i;
156
157	for (i = 0; i < format->num_planes; i++) {
158		sgt = vb2_dma_sg_plane_desc(vb, i);
159		if (!sgt)
160			return -EFAULT;
161
162		buffer->addr[i] = sg_dma_address(sgt->sgl);
163	}
164
165	if (format->pixelformat == V4L2_PIX_FMT_NV12 ||
166			format->pixelformat == V4L2_PIX_FMT_NV21 ||
167			format->pixelformat == V4L2_PIX_FMT_NV16 ||
168			format->pixelformat == V4L2_PIX_FMT_NV61)
169		buffer->addr[1] = buffer->addr[0] +
170				format->plane_fmt[0].bytesperline *
171				format->height;
172
173	return 0;
174}
175
176static int video_buf_prepare(struct vb2_buffer *vb)
177{
178	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
179	struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
180	const struct v4l2_pix_format_mplane *format =
181						&video->active_fmt.fmt.pix_mp;
182	unsigned int i;
183
184	for (i = 0; i < format->num_planes; i++) {
185		if (format->plane_fmt[i].sizeimage > vb2_plane_size(vb, i))
186			return -EINVAL;
187
188		vb2_set_plane_payload(vb, i, format->plane_fmt[i].sizeimage);
189	}
190
191	vbuf->field = V4L2_FIELD_NONE;
192
193	return 0;
194}
195
196static void video_buf_queue(struct vb2_buffer *vb)
197{
198	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
199	struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
200	struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer,
201						   vb);
202
203	video->ops->queue_buffer(video, buffer);
204}
205
206static int video_check_format(struct camss_video *video)
207{
208	struct v4l2_pix_format_mplane *pix = &video->active_fmt.fmt.pix_mp;
209	struct v4l2_format format;
210	struct v4l2_pix_format_mplane *sd_pix = &format.fmt.pix_mp;
211	int ret;
212
213	sd_pix->pixelformat = pix->pixelformat;
214	ret = video_get_subdev_format(video, &format);
215	if (ret < 0)
216		return ret;
217
218	if (pix->pixelformat != sd_pix->pixelformat ||
219	    pix->height != sd_pix->height ||
220	    pix->width != sd_pix->width ||
221	    pix->num_planes != sd_pix->num_planes ||
222	    pix->field != format.fmt.pix_mp.field)
223		return -EPIPE;
224
225	return 0;
226}
227
228static int video_prepare_streaming(struct vb2_queue *q)
229{
230	struct camss_video *video = vb2_get_drv_priv(q);
231	struct video_device *vdev = &video->vdev;
232	int ret;
233
234	ret = v4l2_pipeline_pm_get(&vdev->entity);
235	if (ret < 0) {
236		dev_err(video->camss->dev, "Failed to power up pipeline: %d\n",
237			ret);
238	}
239
240	return ret;
241}
242
243static int video_start_streaming(struct vb2_queue *q, unsigned int count)
244{
245	struct camss_video *video = vb2_get_drv_priv(q);
246	struct video_device *vdev = &video->vdev;
247	struct media_entity *entity;
248	struct media_pad *pad;
249	struct v4l2_subdev *subdev;
250	int ret;
251
252	ret = video_device_pipeline_alloc_start(vdev);
253	if (ret < 0) {
254		dev_err(video->camss->dev, "Failed to start media pipeline: %d\n", ret);
255		goto flush_buffers;
256	}
257
258	ret = video_check_format(video);
259	if (ret < 0)
260		goto error;
261
262	entity = &vdev->entity;
263	while (1) {
264		pad = &entity->pads[0];
265		if (!(pad->flags & MEDIA_PAD_FL_SINK))
266			break;
267
268		pad = media_pad_remote_pad_first(pad);
269		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
270			break;
271
272		entity = pad->entity;
273		subdev = media_entity_to_v4l2_subdev(entity);
274
275		ret = v4l2_subdev_call(subdev, video, s_stream, 1);
276		if (ret < 0 && ret != -ENOIOCTLCMD)
277			goto error;
278	}
279
280	return 0;
281
282error:
283	video_device_pipeline_stop(vdev);
284
285flush_buffers:
286	video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED);
287
288	return ret;
289}
290
291static void video_stop_streaming(struct vb2_queue *q)
292{
293	struct camss_video *video = vb2_get_drv_priv(q);
294	struct video_device *vdev = &video->vdev;
295	struct media_entity *entity;
296	struct media_pad *pad;
297	struct v4l2_subdev *subdev;
298	int ret;
299
300	entity = &vdev->entity;
301	while (1) {
302		pad = &entity->pads[0];
303		if (!(pad->flags & MEDIA_PAD_FL_SINK))
304			break;
305
306		pad = media_pad_remote_pad_first(pad);
307		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
308			break;
309
310		entity = pad->entity;
311		subdev = media_entity_to_v4l2_subdev(entity);
312
313		ret = v4l2_subdev_call(subdev, video, s_stream, 0);
314
315		if (ret) {
316			dev_err(video->camss->dev, "Video pipeline stop failed: %d\n", ret);
317			return;
318		}
319	}
320
321	video_device_pipeline_stop(vdev);
322
323	video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
324}
325
326static void video_unprepare_streaming(struct vb2_queue *q)
327{
328	struct camss_video *video = vb2_get_drv_priv(q);
329	struct video_device *vdev = &video->vdev;
330
331	v4l2_pipeline_pm_put(&vdev->entity);
332}
333
334static const struct vb2_ops msm_video_vb2_q_ops = {
335	.queue_setup     = video_queue_setup,
336	.buf_init        = video_buf_init,
337	.buf_prepare     = video_buf_prepare,
338	.buf_queue       = video_buf_queue,
339	.prepare_streaming = video_prepare_streaming,
340	.start_streaming = video_start_streaming,
341	.stop_streaming  = video_stop_streaming,
342	.unprepare_streaming = video_unprepare_streaming,
343};
344
345/* -----------------------------------------------------------------------------
346 * V4L2 ioctls
347 */
348
349static int video_querycap(struct file *file, void *fh,
350			  struct v4l2_capability *cap)
351{
352	strscpy(cap->driver, "qcom-camss", sizeof(cap->driver));
353	strscpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card));
354
355	return 0;
356}
357
358static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
359{
360	struct camss_video *video = video_drvdata(file);
361	int i, j, k;
362	u32 mcode = f->mbus_code;
363
364	if (f->type != video->type)
365		return -EINVAL;
366
367	if (f->index >= video->nformats)
368		return -EINVAL;
369
370	/*
371	 * Find index "i" of "k"th unique pixelformat in formats array.
372	 *
373	 * If f->mbus_code passed to video_enum_fmt() is not zero, a device
374	 * with V4L2_CAP_IO_MC capability restricts enumeration to only the
375	 * pixel formats that can be produced from that media bus code.
376	 * This is implemented by skipping video->formats[] entries with
377	 * code != f->mbus_code (if f->mbus_code is not zero).
378	 * If the f->mbus_code passed to video_enum_fmt() is not supported,
379	 * -EINVAL is returned.
380	 * If f->mbus_code is zero, all the pixel formats are enumerated.
381	 */
382	k = -1;
383	for (i = 0; i < video->nformats; i++) {
384		if (mcode != 0 && video->formats[i].code != mcode)
385			continue;
386
387		for (j = 0; j < i; j++) {
388			if (mcode != 0 && video->formats[j].code != mcode)
389				continue;
390			if (video->formats[i].pixelformat ==
391					video->formats[j].pixelformat)
392				break;
393		}
394
395		if (j == i)
396			k++;
397
398		if (k == f->index)
399			break;
400	}
401
402	if (k == -1 || k < f->index)
403		/*
404		 * All the unique pixel formats matching the arguments
405		 * have been enumerated (k >= 0 and f->index > 0), or
406		 * no pixel formats match the non-zero f->mbus_code (k == -1).
407		 */
408		return -EINVAL;
409
410	f->pixelformat = video->formats[i].pixelformat;
411
412	return 0;
413}
414
415static int video_enum_framesizes(struct file *file, void *fh,
416				 struct v4l2_frmsizeenum *fsize)
417{
418	struct camss_video *video = video_drvdata(file);
419	int i;
420
421	if (fsize->index)
422		return -EINVAL;
423
424	/* Only accept pixel format present in the formats[] table */
425	for (i = 0; i < video->nformats; i++) {
426		if (video->formats[i].pixelformat == fsize->pixel_format)
427			break;
428	}
429
430	if (i == video->nformats)
431		return -EINVAL;
432
433	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
434	fsize->stepwise.min_width = CAMSS_FRAME_MIN_WIDTH;
435	fsize->stepwise.max_width = CAMSS_FRAME_MAX_WIDTH;
436	fsize->stepwise.min_height = CAMSS_FRAME_MIN_HEIGHT;
437	fsize->stepwise.max_height = (video->line_based) ?
438		CAMSS_FRAME_MAX_HEIGHT_PIX : CAMSS_FRAME_MAX_HEIGHT_RDI;
439	fsize->stepwise.step_width = 1;
440	fsize->stepwise.step_height = 1;
441
442	return 0;
443}
444
445static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
446{
447	struct camss_video *video = video_drvdata(file);
448
449	*f = video->active_fmt;
450
451	return 0;
452}
453
454static int __video_try_fmt(struct camss_video *video, struct v4l2_format *f)
455{
456	struct v4l2_pix_format_mplane *pix_mp;
457	const struct camss_format_info *fi;
458	struct v4l2_plane_pix_format *p;
459	u32 bytesperline[3] = { 0 };
460	u32 sizeimage[3] = { 0 };
461	u32 width, height;
462	u32 bpl, lines;
463	int i, j;
464
465	pix_mp = &f->fmt.pix_mp;
466
467	if (video->line_based)
468		for (i = 0; i < pix_mp->num_planes && i < 3; i++) {
469			p = &pix_mp->plane_fmt[i];
470			bytesperline[i] = clamp_t(u32, p->bytesperline,
471						  1, 65528);
472			sizeimage[i] = clamp_t(u32, p->sizeimage,
473					       bytesperline[i],
474					       bytesperline[i] * CAMSS_FRAME_MAX_HEIGHT_PIX);
475		}
476
477	for (j = 0; j < video->nformats; j++)
478		if (pix_mp->pixelformat == video->formats[j].pixelformat)
479			break;
480
481	if (j == video->nformats)
482		j = 0; /* default format */
483
484	fi = &video->formats[j];
485	width = pix_mp->width;
486	height = pix_mp->height;
487
488	memset(pix_mp, 0, sizeof(*pix_mp));
489
490	pix_mp->pixelformat = fi->pixelformat;
491	pix_mp->width = clamp_t(u32, width, 1, CAMSS_FRAME_MAX_WIDTH);
492	pix_mp->height = clamp_t(u32, height, 1, CAMSS_FRAME_MAX_HEIGHT_RDI);
493	pix_mp->num_planes = fi->planes;
494	for (i = 0; i < pix_mp->num_planes; i++) {
495		bpl = pix_mp->width / fi->hsub[i].numerator *
496			fi->hsub[i].denominator * fi->bpp[i] / 8;
497		bpl = ALIGN(bpl, video->bpl_alignment);
498		pix_mp->plane_fmt[i].bytesperline = bpl;
499		pix_mp->plane_fmt[i].sizeimage = pix_mp->height /
500			fi->vsub[i].numerator * fi->vsub[i].denominator * bpl;
501	}
502
503	pix_mp->field = V4L2_FIELD_NONE;
504	pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
505	pix_mp->flags = 0;
506	pix_mp->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix_mp->colorspace);
507	pix_mp->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
508					pix_mp->colorspace, pix_mp->ycbcr_enc);
509	pix_mp->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix_mp->colorspace);
510
511	if (video->line_based)
512		for (i = 0; i < pix_mp->num_planes; i++) {
513			p = &pix_mp->plane_fmt[i];
514			p->bytesperline = clamp_t(u32, p->bytesperline,
515						  1, 65528);
516			p->sizeimage = clamp_t(u32, p->sizeimage,
517					       p->bytesperline,
518					       p->bytesperline * CAMSS_FRAME_MAX_HEIGHT_PIX);
519			lines = p->sizeimage / p->bytesperline;
520
521			if (p->bytesperline < bytesperline[i])
522				p->bytesperline = ALIGN(bytesperline[i], 8);
523
524			if (p->sizeimage < p->bytesperline * lines)
525				p->sizeimage = p->bytesperline * lines;
526
527			if (p->sizeimage < sizeimage[i])
528				p->sizeimage = sizeimage[i];
529		}
530
531	return 0;
532}
533
534static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
535{
536	struct camss_video *video = video_drvdata(file);
537
538	return __video_try_fmt(video, f);
539}
540
541static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
542{
543	struct camss_video *video = video_drvdata(file);
544	int ret;
545
546	if (vb2_is_busy(&video->vb2_q))
547		return -EBUSY;
548
549	ret = __video_try_fmt(video, f);
550	if (ret < 0)
551		return ret;
552
553	video->active_fmt = *f;
554
555	return 0;
556}
557
558static int video_enum_input(struct file *file, void *fh,
559			    struct v4l2_input *input)
560{
561	if (input->index > 0)
562		return -EINVAL;
563
564	strscpy(input->name, "camera", sizeof(input->name));
565	input->type = V4L2_INPUT_TYPE_CAMERA;
566
567	return 0;
568}
569
570static int video_g_input(struct file *file, void *fh, unsigned int *input)
571{
572	*input = 0;
573
574	return 0;
575}
576
577static int video_s_input(struct file *file, void *fh, unsigned int input)
578{
579	return input == 0 ? 0 : -EINVAL;
580}
581
582static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
583	.vidioc_querycap		= video_querycap,
584	.vidioc_enum_fmt_vid_cap	= video_enum_fmt,
585	.vidioc_enum_framesizes		= video_enum_framesizes,
586	.vidioc_g_fmt_vid_cap_mplane	= video_g_fmt,
587	.vidioc_s_fmt_vid_cap_mplane	= video_s_fmt,
588	.vidioc_try_fmt_vid_cap_mplane	= video_try_fmt,
589	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
590	.vidioc_querybuf		= vb2_ioctl_querybuf,
591	.vidioc_qbuf			= vb2_ioctl_qbuf,
592	.vidioc_expbuf			= vb2_ioctl_expbuf,
593	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
594	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
595	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
596	.vidioc_streamon		= vb2_ioctl_streamon,
597	.vidioc_streamoff		= vb2_ioctl_streamoff,
598	.vidioc_enum_input		= video_enum_input,
599	.vidioc_g_input			= video_g_input,
600	.vidioc_s_input			= video_s_input,
601};
602
603/* -----------------------------------------------------------------------------
604 * V4L2 file operations
605 */
606
607static const struct v4l2_file_operations msm_vid_fops = {
608	.owner          = THIS_MODULE,
609	.unlocked_ioctl = video_ioctl2,
610	.open           = v4l2_fh_open,
611	.release        = vb2_fop_release,
612	.poll           = vb2_fop_poll,
613	.mmap		= vb2_fop_mmap,
614	.read		= vb2_fop_read,
615};
616
617/* -----------------------------------------------------------------------------
618 * CAMSS video core
619 */
620
621static void msm_video_release(struct video_device *vdev)
622{
623	struct camss_video *video = video_get_drvdata(vdev);
624
625	media_entity_cleanup(&vdev->entity);
626
627	mutex_destroy(&video->q_lock);
628	mutex_destroy(&video->lock);
629
630	if (atomic_dec_and_test(&video->camss->ref_count))
631		camss_delete(video->camss);
632}
633
634/*
635 * msm_video_init_format - Helper function to initialize format
636 * @video: struct camss_video
637 *
638 * Initialize pad format with default value.
639 *
640 * Return 0 on success or a negative error code otherwise
641 */
642static int msm_video_init_format(struct camss_video *video)
643{
644	int ret;
645	struct v4l2_format format = {
646		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
647		.fmt.pix_mp = {
648			.width = 1920,
649			.height = 1080,
650			.pixelformat = video->formats[0].pixelformat,
651		},
652	};
653
654	ret = __video_try_fmt(video, &format);
655	if (ret < 0)
656		return ret;
657
658	video->active_fmt = format;
659
660	return 0;
661}
662
663/*
664 * msm_video_register - Register a video device node
665 * @video: struct camss_video
666 * @v4l2_dev: V4L2 device
667 * @name: name to be used for the video device node
668 *
669 * Initialize and register a video device node to a V4L2 device. Also
670 * initialize the vb2 queue.
671 *
672 * Return 0 on success or a negative error code otherwise
673 */
674
675int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
676		       const char *name)
677{
678	struct media_pad *pad = &video->pad;
679	struct video_device *vdev;
680	struct vb2_queue *q;
681	int ret;
682
683	vdev = &video->vdev;
684
685	mutex_init(&video->q_lock);
686
687	q = &video->vb2_q;
688	q->drv_priv = video;
689	q->mem_ops = &vb2_dma_sg_memops;
690	q->ops = &msm_video_vb2_q_ops;
691	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
692	q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ;
693	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
694	q->buf_struct_size = sizeof(struct camss_buffer);
695	q->dev = video->camss->dev;
696	q->lock = &video->q_lock;
697	ret = vb2_queue_init(q);
698	if (ret < 0) {
699		dev_err(v4l2_dev->dev, "Failed to init vb2 queue: %d\n", ret);
700		goto error_vb2_init;
701	}
702
703	pad->flags = MEDIA_PAD_FL_SINK;
704	ret = media_entity_pads_init(&vdev->entity, 1, pad);
705	if (ret < 0) {
706		dev_err(v4l2_dev->dev, "Failed to init video entity: %d\n",
707			ret);
708		goto error_vb2_init;
709	}
710
711	mutex_init(&video->lock);
712
713	ret = msm_video_init_format(video);
714	if (ret < 0) {
715		dev_err(v4l2_dev->dev, "Failed to init format: %d\n", ret);
716		goto error_video_register;
717	}
718
719	vdev->fops = &msm_vid_fops;
720	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING
721			  | V4L2_CAP_READWRITE | V4L2_CAP_IO_MC;
722	vdev->ioctl_ops = &msm_vid_ioctl_ops;
723	vdev->release = msm_video_release;
724	vdev->v4l2_dev = v4l2_dev;
725	vdev->vfl_dir = VFL_DIR_RX;
726	vdev->queue = &video->vb2_q;
727	vdev->lock = &video->lock;
728	strscpy(vdev->name, name, sizeof(vdev->name));
729
730	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
731	if (ret < 0) {
732		dev_err(v4l2_dev->dev, "Failed to register video device: %d\n",
733			ret);
734		goto error_video_register;
735	}
736
737	video_set_drvdata(vdev, video);
738	atomic_inc(&video->camss->ref_count);
739
740	return 0;
741
742error_video_register:
743	media_entity_cleanup(&vdev->entity);
744	mutex_destroy(&video->lock);
745error_vb2_init:
746	mutex_destroy(&video->q_lock);
747
748	return ret;
749}
750
751void msm_video_unregister(struct camss_video *video)
752{
753	atomic_inc(&video->camss->ref_count);
754	vb2_video_unregister_device(&video->vdev);
755	atomic_dec(&video->camss->ref_count);
756}