drivers/media/platform/sunxi/sun6i-csi/sun6i_video.c

   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
  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 *
  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
  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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 421 static int vidioc_g_input(struct file *file, void *fh, unsigned int *i)
 422 {
 423         *i = 0;
 424
 425         return 0;
 426 }
 427
 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  */
 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
 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  */
 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
 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
 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
 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 }