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 */
video_mbus_to_pix_mp(const struct v4l2_mbus_framefmt * mbus,struct v4l2_pix_format_mplane * pix,const struct camss_format_info * f,unsigned int alignment)42 static 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
video_remote_subdev(struct camss_video * video,u32 * pad)67 static 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
video_get_subdev_format(struct camss_video * video,struct v4l2_format * format)83 static 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
video_queue_setup(struct vb2_queue * q,unsigned int * num_buffers,unsigned int * num_planes,unsigned int sizes[],struct device * alloc_devs[])118 static 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
video_buf_init(struct vb2_buffer * vb)146 static 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
video_buf_prepare(struct vb2_buffer * vb)176 static 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
video_buf_queue(struct vb2_buffer * vb)196 static 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
video_check_format(struct camss_video * video)206 static 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
video_start_streaming(struct vb2_queue * q,unsigned int count)228 static int video_start_streaming(struct vb2_queue *q, unsigned int count)
229 {
230 struct camss_video *video = vb2_get_drv_priv(q);
231 struct video_device *vdev = &video->vdev;
232 struct media_entity *entity;
233 struct media_pad *pad;
234 struct v4l2_subdev *subdev;
235 int ret;
236
237 ret = video_device_pipeline_alloc_start(vdev);
238 if (ret < 0) {
239 dev_err(video->camss->dev, "Failed to start media pipeline: %d\n", ret);
240 goto flush_buffers;
241 }
242
243 ret = video_check_format(video);
244 if (ret < 0)
245 goto error;
246
247 entity = &vdev->entity;
248 while (1) {
249 pad = &entity->pads[0];
250 if (!(pad->flags & MEDIA_PAD_FL_SINK))
251 break;
252
253 pad = media_pad_remote_pad_first(pad);
254 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
255 break;
256
257 entity = pad->entity;
258 subdev = media_entity_to_v4l2_subdev(entity);
259
260 ret = v4l2_subdev_call(subdev, video, s_stream, 1);
261 if (ret < 0 && ret != -ENOIOCTLCMD)
262 goto error;
263 }
264
265 return 0;
266
267 error:
268 video_device_pipeline_stop(vdev);
269
270 flush_buffers:
271 video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED);
272
273 return ret;
274 }
275
video_stop_streaming(struct vb2_queue * q)276 static void video_stop_streaming(struct vb2_queue *q)
277 {
278 struct camss_video *video = vb2_get_drv_priv(q);
279 struct video_device *vdev = &video->vdev;
280 struct media_entity *entity;
281 struct media_pad *pad;
282 struct v4l2_subdev *subdev;
283 int ret;
284
285 entity = &vdev->entity;
286 while (1) {
287 pad = &entity->pads[0];
288 if (!(pad->flags & MEDIA_PAD_FL_SINK))
289 break;
290
291 pad = media_pad_remote_pad_first(pad);
292 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
293 break;
294
295 entity = pad->entity;
296 subdev = media_entity_to_v4l2_subdev(entity);
297
298 ret = v4l2_subdev_call(subdev, video, s_stream, 0);
299
300 if (ret) {
301 dev_err(video->camss->dev, "Video pipeline stop failed: %d\n", ret);
302 return;
303 }
304 }
305
306 video_device_pipeline_stop(vdev);
307
308 video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
309 }
310
311 static const struct vb2_ops msm_video_vb2_q_ops = {
312 .queue_setup = video_queue_setup,
313 .wait_prepare = vb2_ops_wait_prepare,
314 .wait_finish = vb2_ops_wait_finish,
315 .buf_init = video_buf_init,
316 .buf_prepare = video_buf_prepare,
317 .buf_queue = video_buf_queue,
318 .start_streaming = video_start_streaming,
319 .stop_streaming = video_stop_streaming,
320 };
321
322 /* -----------------------------------------------------------------------------
323 * V4L2 ioctls
324 */
325
video_querycap(struct file * file,void * fh,struct v4l2_capability * cap)326 static int video_querycap(struct file *file, void *fh,
327 struct v4l2_capability *cap)
328 {
329 strscpy(cap->driver, "qcom-camss", sizeof(cap->driver));
330 strscpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card));
331
332 return 0;
333 }
334
video_enum_fmt(struct file * file,void * fh,struct v4l2_fmtdesc * f)335 static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
336 {
337 struct camss_video *video = video_drvdata(file);
338 int i, j, k;
339 u32 mcode = f->mbus_code;
340
341 if (f->type != video->type)
342 return -EINVAL;
343
344 if (f->index >= video->nformats)
345 return -EINVAL;
346
347 /*
348 * Find index "i" of "k"th unique pixelformat in formats array.
349 *
350 * If f->mbus_code passed to video_enum_fmt() is not zero, a device
351 * with V4L2_CAP_IO_MC capability restricts enumeration to only the
352 * pixel formats that can be produced from that media bus code.
353 * This is implemented by skipping video->formats[] entries with
354 * code != f->mbus_code (if f->mbus_code is not zero).
355 * If the f->mbus_code passed to video_enum_fmt() is not supported,
356 * -EINVAL is returned.
357 * If f->mbus_code is zero, all the pixel formats are enumerated.
358 */
359 k = -1;
360 for (i = 0; i < video->nformats; i++) {
361 if (mcode != 0 && video->formats[i].code != mcode)
362 continue;
363
364 for (j = 0; j < i; j++) {
365 if (mcode != 0 && video->formats[j].code != mcode)
366 continue;
367 if (video->formats[i].pixelformat ==
368 video->formats[j].pixelformat)
369 break;
370 }
371
372 if (j == i)
373 k++;
374
375 if (k == f->index)
376 break;
377 }
378
379 if (k == -1 || k < f->index)
380 /*
381 * All the unique pixel formats matching the arguments
382 * have been enumerated (k >= 0 and f->index > 0), or
383 * no pixel formats match the non-zero f->mbus_code (k == -1).
384 */
385 return -EINVAL;
386
387 f->pixelformat = video->formats[i].pixelformat;
388
389 return 0;
390 }
391
video_enum_framesizes(struct file * file,void * fh,struct v4l2_frmsizeenum * fsize)392 static int video_enum_framesizes(struct file *file, void *fh,
393 struct v4l2_frmsizeenum *fsize)
394 {
395 struct camss_video *video = video_drvdata(file);
396 int i;
397
398 if (fsize->index)
399 return -EINVAL;
400
401 /* Only accept pixel format present in the formats[] table */
402 for (i = 0; i < video->nformats; i++) {
403 if (video->formats[i].pixelformat == fsize->pixel_format)
404 break;
405 }
406
407 if (i == video->nformats)
408 return -EINVAL;
409
410 fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
411 fsize->stepwise.min_width = CAMSS_FRAME_MIN_WIDTH;
412 fsize->stepwise.max_width = CAMSS_FRAME_MAX_WIDTH;
413 fsize->stepwise.min_height = CAMSS_FRAME_MIN_HEIGHT;
414 fsize->stepwise.max_height = (video->line_based) ?
415 CAMSS_FRAME_MAX_HEIGHT_PIX : CAMSS_FRAME_MAX_HEIGHT_RDI;
416 fsize->stepwise.step_width = 1;
417 fsize->stepwise.step_height = 1;
418
419 return 0;
420 }
421
video_g_fmt(struct file * file,void * fh,struct v4l2_format * f)422 static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
423 {
424 struct camss_video *video = video_drvdata(file);
425
426 *f = video->active_fmt;
427
428 return 0;
429 }
430
__video_try_fmt(struct camss_video * video,struct v4l2_format * f)431 static int __video_try_fmt(struct camss_video *video, struct v4l2_format *f)
432 {
433 struct v4l2_pix_format_mplane *pix_mp;
434 const struct camss_format_info *fi;
435 struct v4l2_plane_pix_format *p;
436 u32 bytesperline[3] = { 0 };
437 u32 sizeimage[3] = { 0 };
438 u32 width, height;
439 u32 bpl, lines;
440 int i, j;
441
442 pix_mp = &f->fmt.pix_mp;
443
444 if (video->line_based)
445 for (i = 0; i < pix_mp->num_planes && i < 3; i++) {
446 p = &pix_mp->plane_fmt[i];
447 bytesperline[i] = clamp_t(u32, p->bytesperline,
448 1, 65528);
449 sizeimage[i] = clamp_t(u32, p->sizeimage,
450 bytesperline[i],
451 bytesperline[i] * CAMSS_FRAME_MAX_HEIGHT_PIX);
452 }
453
454 for (j = 0; j < video->nformats; j++)
455 if (pix_mp->pixelformat == video->formats[j].pixelformat)
456 break;
457
458 if (j == video->nformats)
459 j = 0; /* default format */
460
461 fi = &video->formats[j];
462 width = pix_mp->width;
463 height = pix_mp->height;
464
465 memset(pix_mp, 0, sizeof(*pix_mp));
466
467 pix_mp->pixelformat = fi->pixelformat;
468 pix_mp->width = clamp_t(u32, width, 1, CAMSS_FRAME_MAX_WIDTH);
469 pix_mp->height = clamp_t(u32, height, 1, CAMSS_FRAME_MAX_HEIGHT_RDI);
470 pix_mp->num_planes = fi->planes;
471 for (i = 0; i < pix_mp->num_planes; i++) {
472 bpl = pix_mp->width / fi->hsub[i].numerator *
473 fi->hsub[i].denominator * fi->bpp[i] / 8;
474 bpl = ALIGN(bpl, video->bpl_alignment);
475 pix_mp->plane_fmt[i].bytesperline = bpl;
476 pix_mp->plane_fmt[i].sizeimage = pix_mp->height /
477 fi->vsub[i].numerator * fi->vsub[i].denominator * bpl;
478 }
479
480 pix_mp->field = V4L2_FIELD_NONE;
481 pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
482 pix_mp->flags = 0;
483 pix_mp->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix_mp->colorspace);
484 pix_mp->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
485 pix_mp->colorspace, pix_mp->ycbcr_enc);
486 pix_mp->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix_mp->colorspace);
487
488 if (video->line_based)
489 for (i = 0; i < pix_mp->num_planes; i++) {
490 p = &pix_mp->plane_fmt[i];
491 p->bytesperline = clamp_t(u32, p->bytesperline,
492 1, 65528);
493 p->sizeimage = clamp_t(u32, p->sizeimage,
494 p->bytesperline,
495 p->bytesperline * CAMSS_FRAME_MAX_HEIGHT_PIX);
496 lines = p->sizeimage / p->bytesperline;
497
498 if (p->bytesperline < bytesperline[i])
499 p->bytesperline = ALIGN(bytesperline[i], 8);
500
501 if (p->sizeimage < p->bytesperline * lines)
502 p->sizeimage = p->bytesperline * lines;
503
504 if (p->sizeimage < sizeimage[i])
505 p->sizeimage = sizeimage[i];
506 }
507
508 return 0;
509 }
510
video_try_fmt(struct file * file,void * fh,struct v4l2_format * f)511 static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
512 {
513 struct camss_video *video = video_drvdata(file);
514
515 return __video_try_fmt(video, f);
516 }
517
video_s_fmt(struct file * file,void * fh,struct v4l2_format * f)518 static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
519 {
520 struct camss_video *video = video_drvdata(file);
521 int ret;
522
523 if (vb2_is_busy(&video->vb2_q))
524 return -EBUSY;
525
526 ret = __video_try_fmt(video, f);
527 if (ret < 0)
528 return ret;
529
530 video->active_fmt = *f;
531
532 return 0;
533 }
534
video_enum_input(struct file * file,void * fh,struct v4l2_input * input)535 static int video_enum_input(struct file *file, void *fh,
536 struct v4l2_input *input)
537 {
538 if (input->index > 0)
539 return -EINVAL;
540
541 strscpy(input->name, "camera", sizeof(input->name));
542 input->type = V4L2_INPUT_TYPE_CAMERA;
543
544 return 0;
545 }
546
video_g_input(struct file * file,void * fh,unsigned int * input)547 static int video_g_input(struct file *file, void *fh, unsigned int *input)
548 {
549 *input = 0;
550
551 return 0;
552 }
553
video_s_input(struct file * file,void * fh,unsigned int input)554 static int video_s_input(struct file *file, void *fh, unsigned int input)
555 {
556 return input == 0 ? 0 : -EINVAL;
557 }
558
559 static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
560 .vidioc_querycap = video_querycap,
561 .vidioc_enum_fmt_vid_cap = video_enum_fmt,
562 .vidioc_enum_framesizes = video_enum_framesizes,
563 .vidioc_g_fmt_vid_cap_mplane = video_g_fmt,
564 .vidioc_s_fmt_vid_cap_mplane = video_s_fmt,
565 .vidioc_try_fmt_vid_cap_mplane = video_try_fmt,
566 .vidioc_reqbufs = vb2_ioctl_reqbufs,
567 .vidioc_querybuf = vb2_ioctl_querybuf,
568 .vidioc_qbuf = vb2_ioctl_qbuf,
569 .vidioc_expbuf = vb2_ioctl_expbuf,
570 .vidioc_dqbuf = vb2_ioctl_dqbuf,
571 .vidioc_create_bufs = vb2_ioctl_create_bufs,
572 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
573 .vidioc_streamon = vb2_ioctl_streamon,
574 .vidioc_streamoff = vb2_ioctl_streamoff,
575 .vidioc_enum_input = video_enum_input,
576 .vidioc_g_input = video_g_input,
577 .vidioc_s_input = video_s_input,
578 };
579
580 /* -----------------------------------------------------------------------------
581 * V4L2 file operations
582 */
583
video_open(struct file * file)584 static int video_open(struct file *file)
585 {
586 struct video_device *vdev = video_devdata(file);
587 struct camss_video *video = video_drvdata(file);
588 struct v4l2_fh *vfh;
589 int ret;
590
591 mutex_lock(&video->lock);
592
593 vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
594 if (vfh == NULL) {
595 ret = -ENOMEM;
596 goto error_alloc;
597 }
598
599 v4l2_fh_init(vfh, vdev);
600 v4l2_fh_add(vfh);
601
602 file->private_data = vfh;
603
604 ret = v4l2_pipeline_pm_get(&vdev->entity);
605 if (ret < 0) {
606 dev_err(video->camss->dev, "Failed to power up pipeline: %d\n",
607 ret);
608 goto error_pm_use;
609 }
610
611 mutex_unlock(&video->lock);
612
613 return 0;
614
615 error_pm_use:
616 v4l2_fh_release(file);
617
618 error_alloc:
619 mutex_unlock(&video->lock);
620
621 return ret;
622 }
623
video_release(struct file * file)624 static int video_release(struct file *file)
625 {
626 struct video_device *vdev = video_devdata(file);
627
628 vb2_fop_release(file);
629
630 v4l2_pipeline_pm_put(&vdev->entity);
631
632 file->private_data = NULL;
633
634 return 0;
635 }
636
637 static const struct v4l2_file_operations msm_vid_fops = {
638 .owner = THIS_MODULE,
639 .unlocked_ioctl = video_ioctl2,
640 .open = video_open,
641 .release = video_release,
642 .poll = vb2_fop_poll,
643 .mmap = vb2_fop_mmap,
644 .read = vb2_fop_read,
645 };
646
647 /* -----------------------------------------------------------------------------
648 * CAMSS video core
649 */
650
msm_video_release(struct video_device * vdev)651 static void msm_video_release(struct video_device *vdev)
652 {
653 struct camss_video *video = video_get_drvdata(vdev);
654
655 media_entity_cleanup(&vdev->entity);
656
657 mutex_destroy(&video->q_lock);
658 mutex_destroy(&video->lock);
659
660 if (atomic_dec_and_test(&video->camss->ref_count))
661 camss_delete(video->camss);
662 }
663
664 /*
665 * msm_video_init_format - Helper function to initialize format
666 * @video: struct camss_video
667 *
668 * Initialize pad format with default value.
669 *
670 * Return 0 on success or a negative error code otherwise
671 */
msm_video_init_format(struct camss_video * video)672 static int msm_video_init_format(struct camss_video *video)
673 {
674 int ret;
675 struct v4l2_format format = {
676 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
677 .fmt.pix_mp = {
678 .width = 1920,
679 .height = 1080,
680 .pixelformat = video->formats[0].pixelformat,
681 },
682 };
683
684 ret = __video_try_fmt(video, &format);
685 if (ret < 0)
686 return ret;
687
688 video->active_fmt = format;
689
690 return 0;
691 }
692
693 /*
694 * msm_video_register - Register a video device node
695 * @video: struct camss_video
696 * @v4l2_dev: V4L2 device
697 * @name: name to be used for the video device node
698 *
699 * Initialize and register a video device node to a V4L2 device. Also
700 * initialize the vb2 queue.
701 *
702 * Return 0 on success or a negative error code otherwise
703 */
704
msm_video_register(struct camss_video * video,struct v4l2_device * v4l2_dev,const char * name)705 int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
706 const char *name)
707 {
708 struct media_pad *pad = &video->pad;
709 struct video_device *vdev;
710 struct vb2_queue *q;
711 int ret;
712
713 vdev = &video->vdev;
714
715 mutex_init(&video->q_lock);
716
717 q = &video->vb2_q;
718 q->drv_priv = video;
719 q->mem_ops = &vb2_dma_sg_memops;
720 q->ops = &msm_video_vb2_q_ops;
721 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
722 q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ;
723 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
724 q->buf_struct_size = sizeof(struct camss_buffer);
725 q->dev = video->camss->dev;
726 q->lock = &video->q_lock;
727 ret = vb2_queue_init(q);
728 if (ret < 0) {
729 dev_err(v4l2_dev->dev, "Failed to init vb2 queue: %d\n", ret);
730 goto error_vb2_init;
731 }
732
733 pad->flags = MEDIA_PAD_FL_SINK;
734 ret = media_entity_pads_init(&vdev->entity, 1, pad);
735 if (ret < 0) {
736 dev_err(v4l2_dev->dev, "Failed to init video entity: %d\n",
737 ret);
738 goto error_vb2_init;
739 }
740
741 mutex_init(&video->lock);
742
743 ret = msm_video_init_format(video);
744 if (ret < 0) {
745 dev_err(v4l2_dev->dev, "Failed to init format: %d\n", ret);
746 goto error_video_register;
747 }
748
749 vdev->fops = &msm_vid_fops;
750 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING
751 | V4L2_CAP_READWRITE | V4L2_CAP_IO_MC;
752 vdev->ioctl_ops = &msm_vid_ioctl_ops;
753 vdev->release = msm_video_release;
754 vdev->v4l2_dev = v4l2_dev;
755 vdev->vfl_dir = VFL_DIR_RX;
756 vdev->queue = &video->vb2_q;
757 vdev->lock = &video->lock;
758 strscpy(vdev->name, name, sizeof(vdev->name));
759
760 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
761 if (ret < 0) {
762 dev_err(v4l2_dev->dev, "Failed to register video device: %d\n",
763 ret);
764 goto error_video_register;
765 }
766
767 video_set_drvdata(vdev, video);
768 atomic_inc(&video->camss->ref_count);
769
770 return 0;
771
772 error_video_register:
773 media_entity_cleanup(&vdev->entity);
774 mutex_destroy(&video->lock);
775 error_vb2_init:
776 mutex_destroy(&video->q_lock);
777
778 return ret;
779 }
780
msm_video_unregister(struct camss_video * video)781 void msm_video_unregister(struct camss_video *video)
782 {
783 atomic_inc(&video->camss->ref_count);
784 vb2_video_unregister_device(&video->vdev);
785 atomic_dec(&video->camss->ref_count);
786 }
787