1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * Copyright (c) 2011-2018 Magewell Electronics Co., Ltd. (Nanjing)
4 * All rights reserved.
5 * Author: Yong Deng <yong.deng@magewell.com>
6 */
7
8 #include <linux/of.h>
9
10 #include <media/v4l2-device.h>
11 #include <media/v4l2-event.h>
12 #include <media/v4l2-ioctl.h>
13 #include <media/v4l2-mc.h>
14 #include <media/videobuf2-dma-contig.h>
15 #include <media/videobuf2-v4l2.h>
16
17 #include "sun6i_csi.h"
18 #include "sun6i_video.h"
19
20 /* This is got from BSP sources. */
21 #define MIN_WIDTH (32)
22 #define MIN_HEIGHT (32)
23 #define MAX_WIDTH (4800)
24 #define MAX_HEIGHT (4800)
25
26 struct sun6i_csi_buffer {
27 struct vb2_v4l2_buffer vb;
28 struct list_head list;
29
30 dma_addr_t dma_addr;
31 bool queued_to_csi;
32 };
33
34 static const u32 supported_pixformats[] = {
35 V4L2_PIX_FMT_SBGGR8,
36 V4L2_PIX_FMT_SGBRG8,
37 V4L2_PIX_FMT_SGRBG8,
38 V4L2_PIX_FMT_SRGGB8,
39 V4L2_PIX_FMT_SBGGR10,
40 V4L2_PIX_FMT_SGBRG10,
41 V4L2_PIX_FMT_SGRBG10,
42 V4L2_PIX_FMT_SRGGB10,
43 V4L2_PIX_FMT_SBGGR12,
44 V4L2_PIX_FMT_SGBRG12,
45 V4L2_PIX_FMT_SGRBG12,
46 V4L2_PIX_FMT_SRGGB12,
47 V4L2_PIX_FMT_YUYV,
48 V4L2_PIX_FMT_YVYU,
49 V4L2_PIX_FMT_UYVY,
50 V4L2_PIX_FMT_VYUY,
51 V4L2_PIX_FMT_HM12,
52 V4L2_PIX_FMT_NV12,
53 V4L2_PIX_FMT_NV21,
54 V4L2_PIX_FMT_YUV420,
55 V4L2_PIX_FMT_YVU420,
56 V4L2_PIX_FMT_NV16,
57 V4L2_PIX_FMT_NV61,
58 V4L2_PIX_FMT_YUV422P,
59 V4L2_PIX_FMT_RGB565,
60 V4L2_PIX_FMT_RGB565X,
61 V4L2_PIX_FMT_JPEG,
62 };
63
is_pixformat_valid(unsigned int pixformat)64 static bool is_pixformat_valid(unsigned int pixformat)
65 {
66 unsigned int i;
67
68 for (i = 0; i < ARRAY_SIZE(supported_pixformats); i++)
69 if (supported_pixformats[i] == pixformat)
70 return true;
71
72 return false;
73 }
74
75 static struct v4l2_subdev *
sun6i_video_remote_subdev(struct sun6i_video * video,u32 * pad)76 sun6i_video_remote_subdev(struct sun6i_video *video, u32 *pad)
77 {
78 struct media_pad *remote;
79
80 remote = media_entity_remote_pad(&video->pad);
81
82 if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
83 return NULL;
84
85 if (pad)
86 *pad = remote->index;
87
88 return media_entity_to_v4l2_subdev(remote->entity);
89 }
90
sun6i_video_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])91 static int sun6i_video_queue_setup(struct vb2_queue *vq,
92 unsigned int *nbuffers,
93 unsigned int *nplanes,
94 unsigned int sizes[],
95 struct device *alloc_devs[])
96 {
97 struct sun6i_video *video = vb2_get_drv_priv(vq);
98 unsigned int size = video->fmt.fmt.pix.sizeimage;
99
100 if (*nplanes)
101 return sizes[0] < size ? -EINVAL : 0;
102
103 *nplanes = 1;
104 sizes[0] = size;
105
106 return 0;
107 }
108
sun6i_video_buffer_prepare(struct vb2_buffer * vb)109 static int sun6i_video_buffer_prepare(struct vb2_buffer *vb)
110 {
111 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
112 struct sun6i_csi_buffer *buf =
113 container_of(vbuf, struct sun6i_csi_buffer, vb);
114 struct sun6i_video *video = vb2_get_drv_priv(vb->vb2_queue);
115 unsigned long size = video->fmt.fmt.pix.sizeimage;
116
117 if (vb2_plane_size(vb, 0) < size) {
118 v4l2_err(video->vdev.v4l2_dev, "buffer too small (%lu < %lu)\n",
119 vb2_plane_size(vb, 0), size);
120 return -EINVAL;
121 }
122
123 vb2_set_plane_payload(vb, 0, size);
124
125 buf->dma_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
126
127 vbuf->field = video->fmt.fmt.pix.field;
128
129 return 0;
130 }
131
sun6i_video_start_streaming(struct vb2_queue * vq,unsigned int count)132 static int sun6i_video_start_streaming(struct vb2_queue *vq, unsigned int count)
133 {
134 struct sun6i_video *video = vb2_get_drv_priv(vq);
135 struct sun6i_csi_buffer *buf;
136 struct sun6i_csi_buffer *next_buf;
137 struct sun6i_csi_config config;
138 struct v4l2_subdev *subdev;
139 unsigned long flags;
140 int ret;
141
142 video->sequence = 0;
143
144 ret = media_pipeline_start(&video->vdev.entity, &video->vdev.pipe);
145 if (ret < 0)
146 goto clear_dma_queue;
147
148 if (video->mbus_code == 0) {
149 ret = -EINVAL;
150 goto stop_media_pipeline;
151 }
152
153 subdev = sun6i_video_remote_subdev(video, NULL);
154 if (!subdev)
155 goto stop_media_pipeline;
156
157 config.pixelformat = video->fmt.fmt.pix.pixelformat;
158 config.code = video->mbus_code;
159 config.field = video->fmt.fmt.pix.field;
160 config.width = video->fmt.fmt.pix.width;
161 config.height = video->fmt.fmt.pix.height;
162
163 ret = sun6i_csi_update_config(video->csi, &config);
164 if (ret < 0)
165 goto stop_media_pipeline;
166
167 spin_lock_irqsave(&video->dma_queue_lock, flags);
168
169 buf = list_first_entry(&video->dma_queue,
170 struct sun6i_csi_buffer, list);
171 buf->queued_to_csi = true;
172 sun6i_csi_update_buf_addr(video->csi, buf->dma_addr);
173
174 sun6i_csi_set_stream(video->csi, true);
175
176 /*
177 * CSI will lookup the next dma buffer for next frame before the
178 * the current frame done IRQ triggered. This is not documented
179 * but reported by Ondřej Jirman.
180 * The BSP code has workaround for this too. It skip to mark the
181 * first buffer as frame done for VB2 and pass the second buffer
182 * to CSI in the first frame done ISR call. Then in second frame
183 * done ISR call, it mark the first buffer as frame done for VB2
184 * and pass the third buffer to CSI. And so on. The bad thing is
185 * that the first buffer will be written twice and the first frame
186 * is dropped even the queued buffer is sufficient.
187 * So, I make some improvement here. Pass the next buffer to CSI
188 * just follow starting the CSI. In this case, the first frame
189 * will be stored in first buffer, second frame in second buffer.
190 * This method is used to avoid dropping the first frame, it
191 * would also drop frame when lacking of queued buffer.
192 */
193 next_buf = list_next_entry(buf, list);
194 next_buf->queued_to_csi = true;
195 sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
196
197 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
198
199 ret = v4l2_subdev_call(subdev, video, s_stream, 1);
200 if (ret && ret != -ENOIOCTLCMD)
201 goto stop_csi_stream;
202
203 return 0;
204
205 stop_csi_stream:
206 sun6i_csi_set_stream(video->csi, false);
207 stop_media_pipeline:
208 media_pipeline_stop(&video->vdev.entity);
209 clear_dma_queue:
210 spin_lock_irqsave(&video->dma_queue_lock, flags);
211 list_for_each_entry(buf, &video->dma_queue, list)
212 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
213 INIT_LIST_HEAD(&video->dma_queue);
214 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
215
216 return ret;
217 }
218
sun6i_video_stop_streaming(struct vb2_queue * vq)219 static void sun6i_video_stop_streaming(struct vb2_queue *vq)
220 {
221 struct sun6i_video *video = vb2_get_drv_priv(vq);
222 struct v4l2_subdev *subdev;
223 unsigned long flags;
224 struct sun6i_csi_buffer *buf;
225
226 subdev = sun6i_video_remote_subdev(video, NULL);
227 if (subdev)
228 v4l2_subdev_call(subdev, video, s_stream, 0);
229
230 sun6i_csi_set_stream(video->csi, false);
231
232 media_pipeline_stop(&video->vdev.entity);
233
234 /* Release all active buffers */
235 spin_lock_irqsave(&video->dma_queue_lock, flags);
236 list_for_each_entry(buf, &video->dma_queue, list)
237 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
238 INIT_LIST_HEAD(&video->dma_queue);
239 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
240 }
241
sun6i_video_buffer_queue(struct vb2_buffer * vb)242 static void sun6i_video_buffer_queue(struct vb2_buffer *vb)
243 {
244 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
245 struct sun6i_csi_buffer *buf =
246 container_of(vbuf, struct sun6i_csi_buffer, vb);
247 struct sun6i_video *video = vb2_get_drv_priv(vb->vb2_queue);
248 unsigned long flags;
249
250 spin_lock_irqsave(&video->dma_queue_lock, flags);
251 buf->queued_to_csi = false;
252 list_add_tail(&buf->list, &video->dma_queue);
253 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
254 }
255
sun6i_video_frame_done(struct sun6i_video * video)256 void sun6i_video_frame_done(struct sun6i_video *video)
257 {
258 struct sun6i_csi_buffer *buf;
259 struct sun6i_csi_buffer *next_buf;
260 struct vb2_v4l2_buffer *vbuf;
261
262 spin_lock(&video->dma_queue_lock);
263
264 buf = list_first_entry(&video->dma_queue,
265 struct sun6i_csi_buffer, list);
266 if (list_is_last(&buf->list, &video->dma_queue)) {
267 dev_dbg(video->csi->dev, "Frame dropped!\n");
268 goto unlock;
269 }
270
271 next_buf = list_next_entry(buf, list);
272 /* If a new buffer (#next_buf) had not been queued to CSI, the old
273 * buffer (#buf) is still holding by CSI for storing the next
274 * frame. So, we queue a new buffer (#next_buf) to CSI then wait
275 * for next ISR call.
276 */
277 if (!next_buf->queued_to_csi) {
278 next_buf->queued_to_csi = true;
279 sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
280 dev_dbg(video->csi->dev, "Frame dropped!\n");
281 goto unlock;
282 }
283
284 list_del(&buf->list);
285 vbuf = &buf->vb;
286 vbuf->vb2_buf.timestamp = ktime_get_ns();
287 vbuf->sequence = video->sequence;
288 vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
289
290 /* Prepare buffer for next frame but one. */
291 if (!list_is_last(&next_buf->list, &video->dma_queue)) {
292 next_buf = list_next_entry(next_buf, list);
293 next_buf->queued_to_csi = true;
294 sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
295 } else {
296 dev_dbg(video->csi->dev, "Next frame will be dropped!\n");
297 }
298
299 unlock:
300 video->sequence++;
301 spin_unlock(&video->dma_queue_lock);
302 }
303
304 static const struct vb2_ops sun6i_csi_vb2_ops = {
305 .queue_setup = sun6i_video_queue_setup,
306 .wait_prepare = vb2_ops_wait_prepare,
307 .wait_finish = vb2_ops_wait_finish,
308 .buf_prepare = sun6i_video_buffer_prepare,
309 .start_streaming = sun6i_video_start_streaming,
310 .stop_streaming = sun6i_video_stop_streaming,
311 .buf_queue = sun6i_video_buffer_queue,
312 };
313
vidioc_querycap(struct file * file,void * priv,struct v4l2_capability * cap)314 static int vidioc_querycap(struct file *file, void *priv,
315 struct v4l2_capability *cap)
316 {
317 struct sun6i_video *video = video_drvdata(file);
318
319 strscpy(cap->driver, "sun6i-video", sizeof(cap->driver));
320 strscpy(cap->card, video->vdev.name, sizeof(cap->card));
321 snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
322 video->csi->dev->of_node->name);
323
324 return 0;
325 }
326
vidioc_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)327 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
328 struct v4l2_fmtdesc *f)
329 {
330 u32 index = f->index;
331
332 if (index >= ARRAY_SIZE(supported_pixformats))
333 return -EINVAL;
334
335 f->pixelformat = supported_pixformats[index];
336
337 return 0;
338 }
339
vidioc_g_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * fmt)340 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
341 struct v4l2_format *fmt)
342 {
343 struct sun6i_video *video = video_drvdata(file);
344
345 *fmt = video->fmt;
346
347 return 0;
348 }
349
sun6i_video_try_fmt(struct sun6i_video * video,struct v4l2_format * f)350 static int sun6i_video_try_fmt(struct sun6i_video *video,
351 struct v4l2_format *f)
352 {
353 struct v4l2_pix_format *pixfmt = &f->fmt.pix;
354 int bpp;
355
356 if (!is_pixformat_valid(pixfmt->pixelformat))
357 pixfmt->pixelformat = supported_pixformats[0];
358
359 v4l_bound_align_image(&pixfmt->width, MIN_WIDTH, MAX_WIDTH, 1,
360 &pixfmt->height, MIN_HEIGHT, MAX_WIDTH, 1, 1);
361
362 bpp = sun6i_csi_get_bpp(pixfmt->pixelformat);
363 pixfmt->bytesperline = (pixfmt->width * bpp) >> 3;
364 pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
365
366 if (pixfmt->field == V4L2_FIELD_ANY)
367 pixfmt->field = V4L2_FIELD_NONE;
368
369 pixfmt->colorspace = V4L2_COLORSPACE_RAW;
370 pixfmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
371 pixfmt->quantization = V4L2_QUANTIZATION_DEFAULT;
372 pixfmt->xfer_func = V4L2_XFER_FUNC_DEFAULT;
373
374 return 0;
375 }
376
sun6i_video_set_fmt(struct sun6i_video * video,struct v4l2_format * f)377 static int sun6i_video_set_fmt(struct sun6i_video *video, struct v4l2_format *f)
378 {
379 int ret;
380
381 ret = sun6i_video_try_fmt(video, f);
382 if (ret)
383 return ret;
384
385 video->fmt = *f;
386
387 return 0;
388 }
389
vidioc_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)390 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
391 struct v4l2_format *f)
392 {
393 struct sun6i_video *video = video_drvdata(file);
394
395 if (vb2_is_busy(&video->vb2_vidq))
396 return -EBUSY;
397
398 return sun6i_video_set_fmt(video, f);
399 }
400
vidioc_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)401 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
402 struct v4l2_format *f)
403 {
404 struct sun6i_video *video = video_drvdata(file);
405
406 return sun6i_video_try_fmt(video, f);
407 }
408
vidioc_enum_input(struct file * file,void * fh,struct v4l2_input * inp)409 static int vidioc_enum_input(struct file *file, void *fh,
410 struct v4l2_input *inp)
411 {
412 if (inp->index != 0)
413 return -EINVAL;
414
415 strscpy(inp->name, "camera", sizeof(inp->name));
416 inp->type = V4L2_INPUT_TYPE_CAMERA;
417
418 return 0;
419 }
420
vidioc_g_input(struct file * file,void * fh,unsigned int * i)421 static int vidioc_g_input(struct file *file, void *fh, unsigned int *i)
422 {
423 *i = 0;
424
425 return 0;
426 }
427
vidioc_s_input(struct file * file,void * fh,unsigned int i)428 static int vidioc_s_input(struct file *file, void *fh, unsigned int i)
429 {
430 if (i != 0)
431 return -EINVAL;
432
433 return 0;
434 }
435
436 static const struct v4l2_ioctl_ops sun6i_video_ioctl_ops = {
437 .vidioc_querycap = vidioc_querycap,
438 .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
439 .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
440 .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
441 .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
442
443 .vidioc_enum_input = vidioc_enum_input,
444 .vidioc_s_input = vidioc_s_input,
445 .vidioc_g_input = vidioc_g_input,
446
447 .vidioc_reqbufs = vb2_ioctl_reqbufs,
448 .vidioc_querybuf = vb2_ioctl_querybuf,
449 .vidioc_qbuf = vb2_ioctl_qbuf,
450 .vidioc_expbuf = vb2_ioctl_expbuf,
451 .vidioc_dqbuf = vb2_ioctl_dqbuf,
452 .vidioc_create_bufs = vb2_ioctl_create_bufs,
453 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
454 .vidioc_streamon = vb2_ioctl_streamon,
455 .vidioc_streamoff = vb2_ioctl_streamoff,
456
457 .vidioc_log_status = v4l2_ctrl_log_status,
458 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
459 .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
460 };
461
462 /* -----------------------------------------------------------------------------
463 * V4L2 file operations
464 */
sun6i_video_open(struct file * file)465 static int sun6i_video_open(struct file *file)
466 {
467 struct sun6i_video *video = video_drvdata(file);
468 int ret;
469
470 if (mutex_lock_interruptible(&video->lock))
471 return -ERESTARTSYS;
472
473 ret = v4l2_fh_open(file);
474 if (ret < 0)
475 goto unlock;
476
477 ret = v4l2_pipeline_pm_get(&video->vdev.entity);
478 if (ret < 0)
479 goto fh_release;
480
481 /* check if already powered */
482 if (!v4l2_fh_is_singular_file(file))
483 goto unlock;
484
485 ret = sun6i_csi_set_power(video->csi, true);
486 if (ret < 0)
487 goto fh_release;
488
489 mutex_unlock(&video->lock);
490 return 0;
491
492 fh_release:
493 v4l2_fh_release(file);
494 unlock:
495 mutex_unlock(&video->lock);
496 return ret;
497 }
498
sun6i_video_close(struct file * file)499 static int sun6i_video_close(struct file *file)
500 {
501 struct sun6i_video *video = video_drvdata(file);
502 bool last_fh;
503
504 mutex_lock(&video->lock);
505
506 last_fh = v4l2_fh_is_singular_file(file);
507
508 _vb2_fop_release(file, NULL);
509
510 v4l2_pipeline_pm_put(&video->vdev.entity);
511
512 if (last_fh)
513 sun6i_csi_set_power(video->csi, false);
514
515 mutex_unlock(&video->lock);
516
517 return 0;
518 }
519
520 static const struct v4l2_file_operations sun6i_video_fops = {
521 .owner = THIS_MODULE,
522 .open = sun6i_video_open,
523 .release = sun6i_video_close,
524 .unlocked_ioctl = video_ioctl2,
525 .mmap = vb2_fop_mmap,
526 .poll = vb2_fop_poll
527 };
528
529 /* -----------------------------------------------------------------------------
530 * Media Operations
531 */
sun6i_video_link_validate_get_format(struct media_pad * pad,struct v4l2_subdev_format * fmt)532 static int sun6i_video_link_validate_get_format(struct media_pad *pad,
533 struct v4l2_subdev_format *fmt)
534 {
535 if (is_media_entity_v4l2_subdev(pad->entity)) {
536 struct v4l2_subdev *sd =
537 media_entity_to_v4l2_subdev(pad->entity);
538
539 fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
540 fmt->pad = pad->index;
541 return v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt);
542 }
543
544 return -EINVAL;
545 }
546
sun6i_video_link_validate(struct media_link * link)547 static int sun6i_video_link_validate(struct media_link *link)
548 {
549 struct video_device *vdev = container_of(link->sink->entity,
550 struct video_device, entity);
551 struct sun6i_video *video = video_get_drvdata(vdev);
552 struct v4l2_subdev_format source_fmt;
553 int ret;
554
555 video->mbus_code = 0;
556
557 if (!media_entity_remote_pad(link->sink->entity->pads)) {
558 dev_info(video->csi->dev,
559 "video node %s pad not connected\n", vdev->name);
560 return -ENOLINK;
561 }
562
563 ret = sun6i_video_link_validate_get_format(link->source, &source_fmt);
564 if (ret < 0)
565 return ret;
566
567 if (!sun6i_csi_is_format_supported(video->csi,
568 video->fmt.fmt.pix.pixelformat,
569 source_fmt.format.code)) {
570 dev_err(video->csi->dev,
571 "Unsupported pixformat: 0x%x with mbus code: 0x%x!\n",
572 video->fmt.fmt.pix.pixelformat,
573 source_fmt.format.code);
574 return -EPIPE;
575 }
576
577 if (source_fmt.format.width != video->fmt.fmt.pix.width ||
578 source_fmt.format.height != video->fmt.fmt.pix.height) {
579 dev_err(video->csi->dev,
580 "Wrong width or height %ux%u (%ux%u expected)\n",
581 video->fmt.fmt.pix.width, video->fmt.fmt.pix.height,
582 source_fmt.format.width, source_fmt.format.height);
583 return -EPIPE;
584 }
585
586 video->mbus_code = source_fmt.format.code;
587
588 return 0;
589 }
590
591 static const struct media_entity_operations sun6i_video_media_ops = {
592 .link_validate = sun6i_video_link_validate
593 };
594
sun6i_video_init(struct sun6i_video * video,struct sun6i_csi * csi,const char * name)595 int sun6i_video_init(struct sun6i_video *video, struct sun6i_csi *csi,
596 const char *name)
597 {
598 struct video_device *vdev = &video->vdev;
599 struct vb2_queue *vidq = &video->vb2_vidq;
600 struct v4l2_format fmt = { 0 };
601 int ret;
602
603 video->csi = csi;
604
605 /* Initialize the media entity... */
606 video->pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
607 vdev->entity.ops = &sun6i_video_media_ops;
608 ret = media_entity_pads_init(&vdev->entity, 1, &video->pad);
609 if (ret < 0)
610 return ret;
611
612 mutex_init(&video->lock);
613
614 INIT_LIST_HEAD(&video->dma_queue);
615 spin_lock_init(&video->dma_queue_lock);
616
617 video->sequence = 0;
618
619 /* Setup default format */
620 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
621 fmt.fmt.pix.pixelformat = supported_pixformats[0];
622 fmt.fmt.pix.width = 1280;
623 fmt.fmt.pix.height = 720;
624 fmt.fmt.pix.field = V4L2_FIELD_NONE;
625 sun6i_video_set_fmt(video, &fmt);
626
627 /* Initialize videobuf2 queue */
628 vidq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
629 vidq->io_modes = VB2_MMAP | VB2_DMABUF;
630 vidq->drv_priv = video;
631 vidq->buf_struct_size = sizeof(struct sun6i_csi_buffer);
632 vidq->ops = &sun6i_csi_vb2_ops;
633 vidq->mem_ops = &vb2_dma_contig_memops;
634 vidq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
635 vidq->lock = &video->lock;
636 /* Make sure non-dropped frame */
637 vidq->min_buffers_needed = 3;
638 vidq->dev = csi->dev;
639
640 ret = vb2_queue_init(vidq);
641 if (ret) {
642 v4l2_err(&csi->v4l2_dev, "vb2_queue_init failed: %d\n", ret);
643 goto clean_entity;
644 }
645
646 /* Register video device */
647 strscpy(vdev->name, name, sizeof(vdev->name));
648 vdev->release = video_device_release_empty;
649 vdev->fops = &sun6i_video_fops;
650 vdev->ioctl_ops = &sun6i_video_ioctl_ops;
651 vdev->vfl_type = VFL_TYPE_VIDEO;
652 vdev->vfl_dir = VFL_DIR_RX;
653 vdev->v4l2_dev = &csi->v4l2_dev;
654 vdev->queue = vidq;
655 vdev->lock = &video->lock;
656 vdev->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
657 video_set_drvdata(vdev, video);
658
659 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
660 if (ret < 0) {
661 v4l2_err(&csi->v4l2_dev,
662 "video_register_device failed: %d\n", ret);
663 goto clean_entity;
664 }
665
666 return 0;
667
668 clean_entity:
669 media_entity_cleanup(&video->vdev.entity);
670 mutex_destroy(&video->lock);
671 return ret;
672 }
673
sun6i_video_cleanup(struct sun6i_video * video)674 void sun6i_video_cleanup(struct sun6i_video *video)
675 {
676 vb2_video_unregister_device(&video->vdev);
677 media_entity_cleanup(&video->vdev.entity);
678 mutex_destroy(&video->lock);
679 }
680
