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