linux/drivers/media/platform/sunxi/sun6i-csi/sun6i_video.c
<<
>>
Prefs
   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
  26struct 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
  34static 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
  64static 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
  75static struct v4l2_subdev *
  76sun6i_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
  91static 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
 109static 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
 132static 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
 205stop_csi_stream:
 206        sun6i_csi_set_stream(video->csi, false);
 207stop_media_pipeline:
 208        media_pipeline_stop(&video->vdev.entity);
 209clear_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
 219static 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
 242static 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
 256void 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
 299unlock:
 300        video->sequence++;
 301        spin_unlock(&video->dma_queue_lock);
 302}
 303
 304static 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
 314static 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
 327static 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
 340static 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
 350static 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
 377static 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
 390static 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
 401static 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
 409static 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
 421static int vidioc_g_input(struct file *file, void *fh, unsigned int *i)
 422{
 423        *i = 0;
 424
 425        return 0;
 426}
 427
 428static 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
 436static 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 */
 465static 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_use(&video->vdev.entity, 1);
 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
 492fh_release:
 493        v4l2_fh_release(file);
 494unlock:
 495        mutex_unlock(&video->lock);
 496        return ret;
 497}
 498
 499static 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_use(&video->vdev.entity, 0);
 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
 520static 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 */
 532static 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
 547static 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
 591static const struct media_entity_operations sun6i_video_media_ops = {
 592        .link_validate = sun6i_video_link_validate
 593};
 594
 595int 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_GRABBER;
 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_GRABBER, -1);
 660        if (ret < 0) {
 661                v4l2_err(&csi->v4l2_dev,
 662                         "video_register_device failed: %d\n", ret);
 663                goto release_vb2;
 664        }
 665
 666        return 0;
 667
 668release_vb2:
 669        vb2_queue_release(&video->vb2_vidq);
 670clean_entity:
 671        media_entity_cleanup(&video->vdev.entity);
 672        mutex_destroy(&video->lock);
 673        return ret;
 674}
 675
 676void sun6i_video_cleanup(struct sun6i_video *video)
 677{
 678        video_unregister_device(&video->vdev);
 679        media_entity_cleanup(&video->vdev.entity);
 680        vb2_queue_release(&video->vb2_vidq);
 681        mutex_destroy(&video->lock);
 682}
 683