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