drivers/media/platform/tegra/camera/channel.c

   1 /*
   2  * NVIDIA Tegra Video Input Device
   3  *
   4  * Copyright (c) 2015-2016, NVIDIA CORPORATION.  All rights reserved.
   5  *
   6  * Author: Bryan Wu <pengw@nvidia.com>
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License version 2 as
  10  * published by the Free Software Foundation.
  11  */
  12
  13 #include <linux/atomic.h>
  14 #include <linux/bitmap.h>
  15 #include <linux/clk.h>
  16 #include <linux/delay.h>
  17 #include <linux/nvhost.h>
  18 #include <linux/kthread.h>
  19 #include <linux/freezer.h>
  20 #include <linux/lcm.h>
  21 #include <linux/list.h>
  22 #include <linux/module.h>
  23 #include <linux/of.h>
  24 #include <linux/sched.h>
  25 #include <linux/slab.h>
  26
  27 #include <media/v4l2-ctrls.h>
  28 #include <media/v4l2-event.h>
  29 #include <media/v4l2-dev.h>
  30 #include <media/v4l2-fh.h>
  31 #include <media/v4l2-ioctl.h>
  32 #include <media/videobuf2-core.h>
  33 #include <media/videobuf2-dma-contig.h>
  34 #include <media/camera_common.h>
  35 #include <media/tegra_camera_platform.h>
  36
  37 #include <mach/clk.h>
  38 #include <mach/io_dpd.h>
  39
  40 #include "camera/mc_common.h"
  41 #include "vi/vi.h"
  42 #include "nvhost_acm.h"
  43 #include "mipi_cal.h"
  44
  45 #define FRAMERATE       30
  46 #define BPP_MEM         2
  47
  48 extern int _vb2_fop_release(struct file *file, struct mutex *lock);
  49 static void tegra_channel_queued_buf_done(struct tegra_channel *chan,
  50                                           enum vb2_buffer_state state);
  51 static void tegra_channel_stop_kthreads(struct tegra_channel *chan);
  52 static int tegra_channel_set_stream(struct tegra_channel *chan, bool on);
  53 static int tegra_channel_mipi_cal(struct tegra_channel *chan, char is_bypass);
  54
  55 static void gang_buffer_offsets(struct tegra_channel *chan)
  56 {
  57         bool reverse = false;
  58         int i, ports;
  59         u32 offset = 0;
  60
  61         for (i = 0; i < chan->total_ports; i++) {
  62                 switch (chan->gang_mode) {
  63                 case CAMERA_GANG_R_L:
  64                         reverse = true;
  65                 case CAMERA_NO_GANG_MODE:
  66                 case CAMERA_GANG_L_R:
  67                         offset = chan->gang_bytesperline;
  68                         break;
  69                 case CAMERA_GANG_B_T:
  70                         reverse = true;
  71                 case CAMERA_GANG_T_B:
  72                         offset = chan->gang_sizeimage;
  73                         break;
  74                 default:
  75                         offset = 0;
  76                 }
  77                 offset = ((offset + TEGRA_SURFACE_ALIGNMENT - 1) &
  78                                         ~(TEGRA_SURFACE_ALIGNMENT - 1));
  79                 ports = reverse ? (chan->total_ports - 1 - i) : i;
  80                 chan->buffer_offset[i] = ports * offset;
  81         }
  82 }
  83
  84 static u32 gang_mode_width(enum camera_gang_mode gang_mode,
  85                                         unsigned int width)
  86 {
  87         if ((gang_mode == CAMERA_GANG_L_R) ||
  88                 (gang_mode == CAMERA_GANG_R_L))
  89                 return width >> 1;
  90         else
  91                 return width;
  92 }
  93
  94 static u32 gang_mode_height(enum camera_gang_mode gang_mode,
  95                                         unsigned int height)
  96 {
  97         if ((gang_mode == CAMERA_GANG_T_B) ||
  98                 (gang_mode == CAMERA_GANG_B_T))
  99                 return height >> 1;
 100         else
 101                 return height;
 102 }
 103
 104 static void update_gang_mode_params(struct tegra_channel *chan)
 105 {
 106         chan->gang_width = gang_mode_width(chan->gang_mode,
 107                                                 chan->format.width);
 108         chan->gang_height = gang_mode_height(chan->gang_mode,
 109                                                 chan->format.height);
 110         chan->gang_bytesperline = chan->gang_width *
 111                                         chan->fmtinfo->bpp;
 112         chan->gang_sizeimage = chan->gang_bytesperline *
 113                                         chan->format.height;
 114         gang_buffer_offsets(chan);
 115 }
 116
 117 static void update_gang_mode(struct tegra_channel *chan)
 118 {
 119         int width = chan->format.width;
 120         int height = chan->format.height;
 121
 122         /*
 123          * At present only 720p, 1080p and 4k resolutions
 124          * are supported and only 4K requires gang mode
 125          * Update this code with CID for future extensions
 126          * Also, validate width and height of images based
 127          * on gang mode and surface stride alignment
 128          */
 129         if ((width > 1920) && (height > 1080)) {
 130                 chan->gang_mode = chan->gang_mode_default;
 131                 chan->valid_ports = chan->total_ports;
 132         } else {
 133                 chan->gang_mode = CAMERA_NO_GANG_MODE;
 134                 chan->valid_ports = 1;
 135         }
 136
 137         update_gang_mode_params(chan);
 138 }
 139
 140 static void tegra_channel_fmt_align(struct tegra_channel *chan,
 141                                      u32 *width, u32 *height, u32 *bytesperline)
 142 {
 143         unsigned int min_width;
 144         unsigned int max_width;
 145         unsigned int min_bpl;
 146         unsigned int max_bpl;
 147         unsigned int temp_width;
 148         unsigned int align;
 149         unsigned int temp_bpl;
 150         unsigned int bpp = chan->fmtinfo->bpp;
 151
 152         /* Init, if un-init */
 153         if (!*width)
 154                 *width = chan->format.width;
 155         if (!*height)
 156                 *height = chan->format.height;
 157         if (!*bytesperline)
 158                 *bytesperline = *width * chan->fmtinfo->bpp;
 159
 160         /* The transfer alignment requirements are expressed in bytes. Compute
 161          * the minimum and maximum values, clamp the requested width and convert
 162          * it back to pixels.
 163          */
 164         align = lcm(chan->width_align, bpp);
 165         min_width = roundup(TEGRA_MIN_WIDTH, align);
 166         max_width = rounddown(TEGRA_MAX_WIDTH, align);
 167         temp_width = roundup(*width * bpp, align);
 168
 169         *width = clamp(temp_width, min_width, max_width) / bpp;
 170         *height = clamp(*height, TEGRA_MIN_HEIGHT, TEGRA_MAX_HEIGHT);
 171
 172         /* Clamp the requested bytes per line value. If the maximum bytes per
 173          * line value is zero, the module doesn't support user configurable line
 174          * sizes. Override the requested value with the minimum in that case.
 175          */
 176         min_bpl = *width * bpp;
 177         max_bpl = rounddown(TEGRA_MAX_WIDTH, chan->stride_align);
 178         temp_bpl = roundup(*bytesperline, chan->stride_align);
 179
 180         *bytesperline = clamp(temp_bpl, min_bpl, max_bpl);
 181 }
 182
 183 static void tegra_channel_fmts_bitmap_init(struct tegra_channel *chan)
 184 {
 185         int ret, pixel_format_index = 0, init_code = 0;
 186         struct v4l2_subdev *subdev = chan->subdev_on_csi;
 187         struct v4l2_mbus_framefmt mbus_fmt;
 188         struct v4l2_subdev_mbus_code_enum code = {
 189                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 190         };
 191
 192         bitmap_zero(chan->fmts_bitmap, MAX_FORMAT_NUM);
 193
 194         /*
 195          * Initialize all the formats available from
 196          * the sub-device and extract the corresponding
 197          * index from the pre-defined video formats and initialize
 198          * the channel default format with the active code
 199          * Index zero as the only sub-device is sensor
 200          */
 201         while (1) {
 202                 ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
 203                                        NULL, &code);
 204                 if (ret < 0)
 205                         /* no more formats */
 206                         break;
 207
 208                 pixel_format_index = tegra_core_get_idx_by_code(code.code);
 209                 if (pixel_format_index >= 0) {
 210                         bitmap_set(chan->fmts_bitmap, pixel_format_index, 1);
 211                         if (!init_code)
 212                                 init_code = code.code;
 213                 }
 214
 215                 code.index++;
 216         }
 217
 218         if (!init_code) {
 219                 pixel_format_index = tegra_core_get_idx_by_code(TEGRA_VF_DEF);
 220                 if (pixel_format_index >= 0) {
 221                         bitmap_set(chan->fmts_bitmap, pixel_format_index, 1);
 222                         init_code = TEGRA_VF_DEF;
 223                 }
 224         }
 225
 226         /* Get the format based on active code of the sub-device */
 227         ret = v4l2_subdev_call(subdev, video, g_mbus_fmt, &mbus_fmt);
 228         if (ret)
 229                 return;
 230
 231         chan->fmtinfo = tegra_core_get_format_by_code(mbus_fmt.code);
 232         chan->format.pixelformat = chan->fmtinfo->fourcc;
 233         chan->format.colorspace = mbus_fmt.colorspace;
 234         chan->format.field = mbus_fmt.field;
 235         chan->format.width = mbus_fmt.width;
 236         chan->format.height = mbus_fmt.height;
 237         chan->format.maxframerate = mbus_fmt.maxframerate;
 238         chan->format.bytesperline = chan->format.width *
 239                 chan->fmtinfo->bpp;
 240         tegra_channel_fmt_align(chan,
 241                                 &chan->format.width, &chan->format.height,
 242                                 &chan->format.bytesperline);
 243         chan->format.sizeimage = chan->format.bytesperline *
 244                 chan->format.height;
 245         if (chan->total_ports > 1)
 246                 update_gang_mode(chan);
 247 }
 248
 249 /*
 250  * -----------------------------------------------------------------------------
 251  * Tegra channel frame setup and capture operations
 252  * -----------------------------------------------------------------------------
 253  */
 254
 255 static int tegra_channel_capture_setup(struct tegra_channel *chan)
 256 {
 257         return chan->fops->soc_channel_capture_setup(chan);
 258 }
 259
 260 static int tegra_channel_enable_stream(struct tegra_channel *chan)
 261 {
 262         int ret = 0, i;
 263
 264         /*
 265          * enable pad power and perform calibration before arming
 266          * single shot for first frame after the HW setup is complete
 267          */
 268         /* enable pad power */
 269         tegra_csi_pad_control(chan->vi->csi, chan->port, ENABLE);
 270         /* start streaming */
 271         if (chan->vi->pg_mode) {
 272                 for (i = 0; i < chan->valid_ports; i++)
 273                         tegra_csi_tpg_start_streaming(chan->vi->csi,
 274                                                       chan->port[i]);
 275                 atomic_set(&chan->is_streaming, ENABLE);
 276         } else {
 277                 ret = tegra_channel_set_stream(chan, true);
 278                 if (ret < 0)
 279                         return ret;
 280         }
 281         /* perform calibration as sensor started streaming */
 282         tegra_mipi_bias_pad_enable();
 283         if (!chan->vi->pg_mode) {
 284                 mutex_lock(&chan->vi->mipical_lock);
 285                 tegra_channel_mipi_cal(chan, 0);
 286                 mutex_unlock(&chan->vi->mipical_lock);
 287         }
 288
 289         return ret;
 290 }
 291
 292 static int tegra_channel_error_status(struct tegra_channel *chan)
 293 {
 294         return chan->fops->soc_channel_error_status(chan);
 295 }
 296
 297 static void tegra_channel_init_ring_buffer(struct tegra_channel *chan)
 298 {
 299         chan->released_bufs = 0;
 300         chan->num_buffers = 0;
 301         chan->save_index = 0;
 302         chan->free_index = 0;
 303         chan->bfirst_fstart = false;
 304 }
 305
 306 static void free_ring_buffers(struct tegra_channel *chan, int frames)
 307 {
 308         struct vb2_buffer *vb;
 309
 310         while (frames) {
 311                 vb = chan->buffers[chan->free_index];
 312
 313                 /* release one frame */
 314                 vb->v4l2_buf.sequence = chan->sequence++;
 315                 vb->v4l2_buf.field = V4L2_FIELD_NONE;
 316                 vb2_set_plane_payload(vb, 0, chan->format.sizeimage);
 317
 318                 /*
 319                  * WAR to force buffer state if capture state is not good
 320                  * WAR - After sync point timeout or error frame capture
 321                  * the second buffer is intermittently frame of zeros
 322                  * with no error status or padding.
 323                  */
 324                 if (chan->capture_state != CAPTURE_GOOD ||
 325                         chan->released_bufs < 2)
 326                         chan->buffer_state[chan->free_index] =
 327                                                 VB2_BUF_STATE_ERROR;
 328
 329                 vb2_buffer_done(vb, chan->buffer_state[chan->free_index++]);
 330
 331                 if (chan->free_index >= QUEUED_BUFFERS)
 332                         chan->free_index = 0;
 333                 chan->num_buffers--;
 334                 chan->released_bufs++;
 335                 frames--;
 336         }
 337 }
 338
 339 static void add_buffer_to_ring(struct tegra_channel *chan,
 340                                 struct vb2_buffer *vb)
 341 {
 342         /* save the buffer to the ring first */
 343         /* Mark buffer state as error before start */
 344         chan->buffer_state[chan->save_index] = VB2_BUF_STATE_ERROR;
 345         chan->buffers[chan->save_index++] = vb;
 346         if (chan->save_index >= QUEUED_BUFFERS)
 347                 chan->save_index = 0;
 348         chan->num_buffers++;
 349 }
 350
 351 static void update_state_to_buffer(struct tegra_channel *chan, int state)
 352 {
 353         int save_index = (chan->save_index - PREVIOUS_BUFFER_DEC_INDEX);
 354
 355         /* save index decrements by 2 as 3 bufs are added in ring buffer */
 356         if (save_index < 0)
 357                 save_index += QUEUED_BUFFERS;
 358         /* update state for the previous buffer */
 359         chan->buffer_state[save_index] = state;
 360
 361         /* for timeout/error case update the current buffer state as well */
 362         if (chan->capture_state != CAPTURE_GOOD)
 363                 chan->buffer_state[chan->save_index] = state;
 364 }
 365
 366 static void tegra_channel_ring_buffer(struct tegra_channel *chan,
 367                                         struct vb2_buffer *vb,
 368                                         struct timespec *ts, int state)
 369
 370 {
 371         if (!chan->bfirst_fstart)
 372                 chan->bfirst_fstart = true;
 373         else
 374                 update_state_to_buffer(chan, state);
 375
 376         /* update time stamp of the buffer */
 377         vb->v4l2_buf.timestamp.tv_sec = ts->tv_sec;
 378         vb->v4l2_buf.timestamp.tv_usec = ts->tv_nsec / NSEC_PER_USEC;
 379
 380         /* Capture state is not GOOD, release all buffers and re-init state */
 381         if (chan->capture_state != CAPTURE_GOOD) {
 382                 free_ring_buffers(chan, chan->num_buffers);
 383                 tegra_channel_init_ring_buffer(chan);
 384                 return;
 385         }
 386
 387         /* release buffer N at N+2 frame start event */
 388         if (chan->num_buffers >= (QUEUED_BUFFERS - 1))
 389                 free_ring_buffers(chan, 1);
 390 }
 391
 392 void tegra_channel_ec_close(struct tegra_mc_vi *vi)
 393 {
 394         int i;
 395         struct tegra_channel *chan = &vi->chans[0];
 396
 397         /* clear all channles sync point fifo context */
 398         for (i = 0; i < vi->num_channels; i++) {
 399                 chan = &vi->chans[i];
 400                 memset(&chan->syncpoint_fifo[0], 0, TEGRA_CSI_BLOCKS);
 401         }
 402 }
 403
 404 static void tegra_channel_ec_init(struct tegra_channel *chan)
 405 {
 406         /*
 407          * error recover initialization sequence
 408          * set timeout as 200 ms, use default if fps not available
 409          * Time limit allow CSI to capture good frames and drop error frames
 410          * Timeout units is jiffies, 1 jiffy = 10ms
 411          * TODO: Get frame rate from sub-device and adopt timeout
 412          */
 413         chan->timeout = 20;
 414
 415         chan->fops->soc_channel_ec_init(chan);
 416 }
 417
 418 static void tegra_channel_ec_recover(struct tegra_channel *chan)
 419 {
 420         int index, valid_ports = chan->valid_ports;
 421
 422         /* Disable pad power to start recovery */
 423         tegra_csi_pad_control(chan->vi->csi, chan->port, DISABLE);
 424
 425         chan->fops->soc_channel_ec_recover(chan);
 426
 427         /* Re-init VI and CSI */
 428         tegra_channel_capture_setup(chan);
 429         for (index = 0; index < valid_ports; index++) {
 430                 tegra_csi_stop_streaming(chan->vi->csi,
 431                                                 chan->port[index]);
 432                 tegra_csi_start_streaming(chan->vi->csi,
 433                                                 chan->port[index]);
 434                 nvhost_syncpt_set_min_eq_max_ext(chan->vi->ndev,
 435                                                 chan->syncpt[index]);
 436         }
 437 }
 438
 439 static int tegra_channel_capture_frame(struct tegra_channel *chan,
 440                                        struct tegra_channel_buffer *buf)
 441 {
 442         struct vb2_buffer *vb = &buf->buf;
 443         struct timespec ts;
 444         u32 thresh[TEGRA_CSI_BLOCKS] = { 0 };
 445         int err = 0;
 446         int state = VB2_BUF_STATE_DONE;
 447
 448         /* Init registers related to each frames */
 449         chan->fops->soc_channel_capture_frame_init(chan, buf, thresh);
 450
 451         if (!chan->bfirst_fstart) {
 452                 err = tegra_channel_enable_stream(chan);
 453                 if (err) {
 454                         state = VB2_BUF_STATE_ERROR;
 455                         chan->capture_state = CAPTURE_ERROR;
 456                         tegra_channel_ring_buffer(chan, vb, &ts, state);
 457                         return err;
 458                 }
 459                 /* Enable input stream once the VI registers are configured */
 460                 chan->fops->soc_channel_capture_frame_enable(chan);
 461         }
 462
 463         /* Arm capture and wait for notifier or syncpoint */
 464         err = chan->fops->soc_channel_capture_frame(chan, &ts, thresh);
 465         if (err) {
 466                 state = VB2_BUF_STATE_ERROR;
 467                 /* perform error recovery for timeout */
 468                 tegra_channel_ec_recover(chan);
 469         } else if (!chan->vi->pg_mode) {
 470                 /* Marking error frames and resume capture */
 471                 /* TODO: TPG has frame height short error always set */
 472                 err = tegra_channel_error_status(chan);
 473                 if (err) {
 474                         state = VB2_BUF_STATE_ERROR;
 475                         chan->capture_state = CAPTURE_ERROR;
 476                         /* do we have to run recover here ?? */
 477                         /* tegra_channel_ec_recover(chan); */
 478                 }
 479         }
 480
 481         tegra_channel_ring_buffer(chan, vb, &ts, state);
 482
 483         return 0;
 484 }
 485
 486 static
 487 struct tegra_channel_buffer *dequeue_buffer(struct tegra_channel *chan)
 488 {
 489         struct tegra_channel_buffer *buf = NULL;
 490
 491         spin_lock(&chan->start_lock);
 492         if (list_empty(&chan->capture))
 493                 goto done;
 494
 495         buf = list_entry(chan->capture.next,
 496                          struct tegra_channel_buffer, queue);
 497         list_del_init(&buf->queue);
 498
 499         /* add dequeued buffer to the ring buffer */
 500         add_buffer_to_ring(chan, &buf->buf);
 501 done:
 502         spin_unlock(&chan->start_lock);
 503         return buf;
 504 }
 505
 506 static void tegra_channel_capture_done(struct tegra_channel *chan)
 507 {
 508         struct timespec ts;
 509         int err;
 510         struct tegra_channel_buffer *buf;
 511         int state = VB2_BUF_STATE_DONE;
 512
 513         /* dequeue buffer and return if no buffer exists */
 514         buf = dequeue_buffer(chan);
 515         if (!buf)
 516                 return;
 517
 518         err = chan->fops->soc_channel_capture_done(chan, buf, &ts);
 519         if (err) {
 520                 state = VB2_BUF_STATE_ERROR;
 521                 /* perform error recovery for timeout */
 522                 tegra_channel_ec_recover(chan);
 523                 chan->capture_state = CAPTURE_TIMEOUT;
 524         }
 525
 526         /* Mark capture state to IDLE as capture is finished */
 527         chan->capture_state = CAPTURE_IDLE;
 528
 529         tegra_channel_ring_buffer(chan, &buf->buf, &ts, state);
 530 }
 531
 532 static int tegra_channel_kthread_capture_start(void *data)
 533 {
 534         struct tegra_channel *chan = data;
 535         struct tegra_channel_buffer *buf;
 536         int err = 0;
 537
 538         set_freezable();
 539
 540         while (1) {
 541
 542                 try_to_freeze();
 543
 544                 wait_event_interruptible(chan->start_wait,
 545                                          !list_empty(&chan->capture) ||
 546                                          kthread_should_stop());
 547
 548                 if (kthread_should_stop()) {
 549                         complete(&chan->capture_comp);
 550                         break;
 551                 }
 552
 553                 /* source is not streaming if error is non-zero */
 554                 /* wait till kthread stop and dont DeQ buffers */
 555                 if (err)
 556                         continue;
 557
 558                 buf = dequeue_buffer(chan);
 559                 if (!buf)
 560                         continue;
 561
 562                 err = tegra_channel_capture_frame(chan, buf);
 563         }
 564
 565         return 0;
 566 }
 567
 568 static void tegra_channel_stop_kthreads(struct tegra_channel *chan)
 569 {
 570         mutex_lock(&chan->stop_kthread_lock);
 571         /* Stop the kthread for capture */
 572         if (chan->kthread_capture_start) {
 573                 kthread_stop(chan->kthread_capture_start);
 574                 wait_for_completion(&chan->capture_comp);
 575                 chan->kthread_capture_start = NULL;
 576         }
 577         mutex_unlock(&chan->stop_kthread_lock);
 578 }
 579
 580 /*
 581  * -----------------------------------------------------------------------------
 582  * videobuf2 queue operations
 583  * -----------------------------------------------------------------------------
 584  */
 585 static int
 586 tegra_channel_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 587                      unsigned int *nbuffers, unsigned int *nplanes,
 588                      unsigned int sizes[], void *alloc_ctxs[])
 589 {
 590         struct tegra_channel *chan = vb2_get_drv_priv(vq);
 591
 592         /* Make sure the image size is large enough. */
 593         if (fmt && fmt->fmt.pix.sizeimage < chan->format.sizeimage)
 594                 return -EINVAL;
 595
 596         *nplanes = 1;
 597
 598         sizes[0] = fmt ? fmt->fmt.pix.sizeimage : chan->format.sizeimage;
 599         alloc_ctxs[0] = chan->alloc_ctx;
 600
 601         /* Make sure minimum number of buffers are passed */
 602         if (*nbuffers < (QUEUED_BUFFERS - 1))
 603                 *nbuffers = QUEUED_BUFFERS - 1;
 604
 605         return 0;
 606 }
 607
 608 static int tegra_channel_buffer_prepare(struct vb2_buffer *vb)
 609 {
 610         struct tegra_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
 611         struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vb);
 612
 613         buf->chan = chan;
 614         vb2_set_plane_payload(vb, 0, chan->format.sizeimage);
 615         buf->addr = vb2_dma_contig_plane_dma_addr(vb, 0);
 616
 617         return 0;
 618 }
 619
 620 static void tegra_channel_buffer_queue(struct vb2_buffer *vb)
 621 {
 622         struct tegra_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
 623         struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vb);
 624
 625         /* for bypass mode - do nothing */
 626         if (chan->bypass)
 627                 return;
 628
 629         /* Put buffer into the capture queue */
 630         spin_lock(&chan->start_lock);
 631         list_add_tail(&buf->queue, &chan->capture);
 632         spin_unlock(&chan->start_lock);
 633
 634         /* Wait up kthread for capture */
 635         wake_up_interruptible(&chan->start_wait);
 636 }
 637
 638 /* Return all queued buffers back to videobuf2 */
 639 static void tegra_channel_queued_buf_done(struct tegra_channel *chan,
 640                                           enum vb2_buffer_state state)
 641 {
 642         struct tegra_channel_buffer *buf, *nbuf;
 643         spinlock_t *lock = &chan->start_lock;
 644         struct list_head *q = &chan->capture;
 645
 646         spin_lock(lock);
 647         list_for_each_entry_safe(buf, nbuf, q, queue) {
 648                 vb2_buffer_done(&buf->buf, state);
 649                 list_del(&buf->queue);
 650         }
 651         spin_unlock(lock);
 652 }
 653
 654 #if defined(CONFIG_ARCH_TEGRA_21x_SOC)
 655 static int tegra_channel_mipi_cal(struct tegra_channel *chan, char is_bypass)
 656 {
 657         unsigned int lanes, cur_lanes, val;
 658         unsigned int csi_phya, csi_phyb, csi_phya_mask, csi_phyb_mask;
 659         struct tegra_mc_vi *vi = chan->vi;
 660         int j;
 661
 662         lanes = 0;
 663         csi_phya = 0x1 << CSI_A_PHY_CIL_ENABLE_SHIFT;
 664         csi_phya_mask = 0x3 << CSI_A_PHY_CIL_ENABLE_SHIFT;
 665         csi_phyb = 0x1 << CSI_B_PHY_CIL_ENABLE_SHIFT;
 666         csi_phyb_mask = 0x3 << CSI_B_PHY_CIL_ENABLE_SHIFT;
 667         if (chan->numlanes == 2 && chan->total_ports == 1) {
 668                 switch (chan->port[0]) {
 669                 case PORT_A:
 670                         lanes = CSIA;
 671                         val = (host1x_readl(vi->ndev, CSI_PHY_CIL_COMMAND_0) &
 672                                 (~csi_phya_mask)) | csi_phya;
 673                         if (is_bypass)
 674                                 host1x_writel(vi->ndev, CSI_PHY_CIL_COMMAND_0,
 675                                                 val);
 676                         break;
 677                 case PORT_B:
 678                         lanes = CSIB;
 679                         val = (host1x_readl(vi->ndev, CSI_PHY_CIL_COMMAND_0) &
 680                                 (~csi_phyb_mask)) | csi_phyb;
 681                         if (is_bypass)
 682                                 host1x_writel(vi->ndev, CSI_PHY_CIL_COMMAND_0,
 683                                                 val);
 684                         break;
 685                 case PORT_C:
 686                         lanes = CSIC;
 687                         val = (host1x_readl(vi->ndev, CSI1_PHY_CIL_COMMAND_0) &
 688                                 (~csi_phya_mask)) | csi_phya;
 689                         if (is_bypass)
 690                                 host1x_writel(vi->ndev, CSI1_PHY_CIL_COMMAND_0,
 691                                                 val);
 692                         break;
 693                 case PORT_D:
 694                         lanes = CSID;
 695                         val = (host1x_readl(vi->ndev, CSI1_PHY_CIL_COMMAND_0) &
 696                               (~csi_phyb_mask)) | csi_phyb;
 697                         if (is_bypass)
 698                                 host1x_writel(vi->ndev, CSI1_PHY_CIL_COMMAND_0,
 699                                                 val);
 700                         break;
 701                 case PORT_E:
 702                         lanes = CSIE;
 703                         val = (host1x_readl(vi->ndev, CSI2_PHY_CIL_COMMAND_0) &
 704                                 (~csi_phya_mask)) | csi_phya;
 705                         if (is_bypass)
 706                                 host1x_writel(vi->ndev, CSI2_PHY_CIL_COMMAND_0,
 707                                                 val);
 708                         break;
 709                 case PORT_F:
 710                         lanes = CSIF;
 711                         val = (host1x_readl(vi->ndev, CSI2_PHY_CIL_COMMAND_0) &
 712                                 (~csi_phyb_mask)) | csi_phyb;
 713                         if (is_bypass)
 714                                 host1x_writel(vi->ndev, CSI2_PHY_CIL_COMMAND_0,
 715                                                 val);
 716                         break;
 717                 default:
 718                         dev_err(vi->dev, "csi_port number: %d", chan->port[0]);
 719                         break;
 720                 }
 721         } else if (chan->numlanes == 4 && chan->total_ports == 1) {
 722                 switch (chan->port[0]) {
 723                 case PORT_A:
 724                 case PORT_B:
 725                         lanes = CSIA|CSIB;
 726                         if (is_bypass)
 727                                 host1x_writel(vi->ndev, CSI_PHY_CIL_COMMAND_0,
 728                                         csi_phya | csi_phyb);
 729                         break;
 730                 case PORT_C:
 731                 case PORT_D:
 732                         lanes = CSIC|CSID;
 733                         if (is_bypass)
 734                                 host1x_writel(vi->ndev, CSI1_PHY_CIL_COMMAND_0,
 735                                         csi_phya | csi_phyb);
 736                         break;
 737                 case PORT_E:
 738                 case PORT_F:
 739                         lanes = CSIE|CSIF;
 740                         if (is_bypass)
 741                                 host1x_writel(vi->ndev, CSI2_PHY_CIL_COMMAND_0,
 742                                         csi_phya | csi_phyb);
 743                         break;
 744                 default:
 745                         dev_err(vi->dev, "csi_port number: %d", chan->port[0]);
 746                         break;
 747                 }
 748         } else if (chan->numlanes == 8) {
 749                 cur_lanes = 0;
 750                 for (j = 0; j < chan->valid_ports; ++j) {
 751                         switch (chan->port[j]) {
 752                         case PORT_A:
 753                         case PORT_B:
 754                                 cur_lanes = CSIA|CSIB;
 755                                 if (is_bypass)
 756                                         host1x_writel(vi->ndev,
 757                                                         CSI_PHY_CIL_COMMAND_0,
 758                                                         csi_phya | csi_phyb);
 759                                 break;
 760                         case PORT_C:
 761                         case PORT_D:
 762                                 cur_lanes = CSIC|CSID;
 763                                 if (is_bypass)
 764                                         host1x_writel(vi->ndev,
 765                                                         CSI1_PHY_CIL_COMMAND_0,
 766                                                         csi_phya | csi_phyb);
 767                                 break;
 768                         case PORT_E:
 769                         case PORT_F:
 770                                 cur_lanes = CSIE|CSIF;
 771                                 if (is_bypass)
 772                                         host1x_writel(vi->ndev,
 773                                                         CSI2_PHY_CIL_COMMAND_0,
 774                                                         csi_phya | csi_phyb);
 775                                 break;
 776                         default:
 777                                 dev_err(vi->dev, "csi_port number: %d",
 778                                                 chan->port[0]);
 779                                 break;
 780                         }
 781                         lanes |= cur_lanes;
 782                 }
 783         }
 784         if (!lanes) {
 785                 dev_err(vi->dev, "Selected no CSI lane, cannot do calibration");
 786                 return -EINVAL;
 787         }
 788         return tegra_mipi_calibration(lanes);
 789
 790 }
 791 #else
 792 static int tegra_channel_mipi_cal(struct tegra_channel *chan, char is_bypass)
 793 {
 794         return 0;
 795 }
 796 #endif
 797
 798 /*
 799  * -----------------------------------------------------------------------------
 800  * subdevice set/unset operations
 801  * -----------------------------------------------------------------------------
 802  */
 803 static int tegra_channel_set_stream(struct tegra_channel *chan, bool on)
 804 {
 805         int ret = 0;
 806
 807         if (atomic_read(&chan->is_streaming) == on)
 808                 return 0;
 809
 810         ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
 811                         chan->grp_id, video, s_stream, on);
 812         if (ret)
 813                 return ret;
 814
 815         atomic_set(&chan->is_streaming, on);
 816
 817         return 0;
 818 }
 819
 820 static int tegra_channel_set_power(struct tegra_channel *chan, bool on)
 821 {
 822         return v4l2_device_call_until_err(chan->video.v4l2_dev,
 823                         chan->grp_id, core, s_power, on);
 824 }
 825
 826 static int update_clk(struct tegra_mc_vi *vi)
 827 {
 828         unsigned int i;
 829         unsigned long max_clk = 0;
 830
 831         for (i = 0; i < vi->num_channels; i++) {
 832                 max_clk = max_clk > vi->chans[i].requested_hz ?
 833                         max_clk : vi->chans[i].requested_hz;
 834         }
 835         return clk_set_rate(vi->clk, max_clk);
 836 }
 837
 838 static void tegra_channel_update_clknbw(struct tegra_channel *chan, u8 on)
 839 {
 840         /* width * height * fps * KBytes write to memory
 841          * WAR: Using fix fps until we have a way to set it
 842          */
 843         chan->requested_kbyteps = (on > 0 ? 1 : -1) * ((chan->format.width
 844                                 * chan->format.height
 845                                 * FRAMERATE * BPP_MEM) / 1000);
 846         chan->requested_hz = on > 0 ? chan->format.width * chan->format.height
 847                                 * FRAMERATE : 0;
 848         mutex_lock(&chan->vi->bw_update_lock);
 849         chan->vi->aggregated_kbyteps += chan->requested_kbyteps;
 850         vi_v4l2_update_isobw(chan->vi->aggregated_kbyteps, 0);
 851         vi_v4l2_set_la(tegra_vi_get(), 0, 0);
 852         update_clk(chan->vi);
 853         mutex_unlock(&chan->vi->bw_update_lock);
 854 }
 855
 856 static int tegra_channel_start_streaming(struct vb2_queue *vq, u32 count)
 857 {
 858         struct tegra_channel *chan = vb2_get_drv_priv(vq);
 859         struct media_pipeline *pipe = chan->video.entity.pipe;
 860         int ret = 0, i;
 861
 862         tegra_channel_ec_init(chan);
 863
 864         if (!chan->vi->pg_mode) {
 865                 /* Start the pipeline. */
 866                 ret = media_entity_pipeline_start(&chan->video.entity, pipe);
 867                 if (ret < 0)
 868                         goto error_pipeline_start;
 869         }
 870
 871         if (chan->bypass) {
 872                 ret = tegra_channel_set_stream(chan, true);
 873                 if (ret < 0)
 874                         goto error_set_stream;
 875                 nvhost_module_enable_clk(chan->vi->dev);
 876                 tegra_mipi_bias_pad_enable();
 877                 mutex_lock(&chan->vi->mipical_lock);
 878                 tegra_channel_mipi_cal(chan, 1);
 879                 mutex_unlock(&chan->vi->mipical_lock);
 880                 nvhost_module_disable_clk(chan->vi->dev);
 881                 return ret;
 882         }
 883
 884         chan->capture_state = CAPTURE_IDLE;
 885         for (i = 0; i < chan->valid_ports; i++) {
 886                 tegra_csi_start_streaming(chan->vi->csi, chan->port[i]);
 887                 /* ensure sync point state is clean */
 888                 nvhost_syncpt_set_min_eq_max_ext(chan->vi->ndev,
 889                                                         chan->syncpt[i]);
 890         }
 891
 892         /* Note: Program VI registers after TPG, sensors and CSI streaming */
 893         ret = tegra_channel_capture_setup(chan);
 894         if (ret < 0)
 895                 goto error_capture_setup;
 896
 897         chan->sequence = 0;
 898         tegra_channel_init_ring_buffer(chan);
 899
 900         /* Update clock and bandwidth based on the format */
 901         tegra_channel_update_clknbw(chan, 1);
 902
 903         /* Start kthread to capture data to buffer */
 904         chan->kthread_capture_start = kthread_run(
 905                                         tegra_channel_kthread_capture_start,
 906                                         chan, chan->video.name);
 907         if (IS_ERR(chan->kthread_capture_start)) {
 908                 dev_err(&chan->video.dev,
 909                         "failed to run kthread for capture start\n");
 910                 ret = PTR_ERR(chan->kthread_capture_start);
 911                 goto error_capture_setup;
 912         }
 913
 914         return 0;
 915
 916 error_capture_setup:
 917         if (!chan->vi->pg_mode)
 918                 tegra_channel_set_stream(chan, false);
 919 error_set_stream:
 920         if (!chan->vi->pg_mode)
 921                 media_entity_pipeline_stop(&chan->video.entity);
 922 error_pipeline_start:
 923         vq->start_streaming_called = 0;
 924         tegra_channel_queued_buf_done(chan, VB2_BUF_STATE_QUEUED);
 925
 926         return ret;
 927 }
 928
 929 static int tegra_channel_stop_streaming(struct vb2_queue *vq)
 930 {
 931         struct tegra_channel *chan = vb2_get_drv_priv(vq);
 932         bool is_streaming = atomic_read(&chan->is_streaming);
 933
 934         if (!chan->bypass) {
 935                 tegra_channel_stop_kthreads(chan);
 936                 /* wait for last frame memory write ack */
 937                 if (is_streaming && chan->capture_state == CAPTURE_GOOD)
 938                         tegra_channel_capture_done(chan);
 939                 /* free all the ring buffers */
 940                 free_ring_buffers(chan, chan->num_buffers);
 941                 /* dequeue buffers back to app which are in capture queue */
 942                 tegra_channel_queued_buf_done(chan, VB2_BUF_STATE_ERROR);
 943
 944                 chan->fops->soc_channel_stop_streaming(chan);
 945
 946                 tegra_csi_pad_control(chan->vi->csi, chan->port, DISABLE);
 947         }
 948
 949         if (!chan->vi->pg_mode) {
 950                 tegra_channel_set_stream(chan, false);
 951                 media_entity_pipeline_stop(&chan->video.entity);
 952         }
 953
 954         if (!chan->bypass)
 955                 tegra_channel_update_clknbw(chan, 0);
 956
 957         tegra_mipi_bias_pad_disable();
 958
 959         return 0;
 960 }
 961
 962 static const struct vb2_ops tegra_channel_queue_qops = {
 963         .queue_setup = tegra_channel_queue_setup,
 964         .buf_prepare = tegra_channel_buffer_prepare,
 965         .buf_queue = tegra_channel_buffer_queue,
 966         .wait_prepare = vb2_ops_wait_prepare,
 967         .wait_finish = vb2_ops_wait_finish,
 968         .start_streaming = tegra_channel_start_streaming,
 969         .stop_streaming = tegra_channel_stop_streaming,
 970 };
 971
 972 /* -----------------------------------------------------------------------------
 973  * V4L2 ioctls
 974  */
 975
 976 static int
 977 tegra_channel_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
 978 {
 979         struct v4l2_fh *vfh = file->private_data;
 980         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 981
 982         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 983         cap->device_caps |= V4L2_CAP_EXT_PIX_FORMAT;
 984         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 985
 986         strlcpy(cap->driver, "tegra-video", sizeof(cap->driver));
 987         strlcpy(cap->card, chan->video.name, sizeof(cap->card));
 988         snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s:%u",
 989                  dev_name(chan->vi->dev), chan->port[0]);
 990
 991         return 0;
 992 }
 993
 994 static int
 995 tegra_channel_enum_framesizes(struct file *file, void *fh,
 996                               struct v4l2_frmsizeenum *sizes)
 997 {
 998         struct v4l2_fh *vfh = file->private_data;
 999         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1000
1001         return v4l2_device_call_until_err(chan->video.v4l2_dev,
1002                         chan->grp_id, video, enum_framesizes, sizes);
1003 }
1004
1005 static int
1006 tegra_channel_enum_frameintervals(struct file *file, void *fh,
1007                               struct v4l2_frmivalenum *intervals)
1008 {
1009         struct v4l2_fh *vfh = file->private_data;
1010         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1011
1012         return v4l2_device_call_until_err(chan->video.v4l2_dev,
1013                         chan->grp_id, video, enum_frameintervals,
1014                         intervals);
1015 }
1016
1017
1018 static int
1019 tegra_channel_enum_format(struct file *file, void *fh, struct v4l2_fmtdesc *f)
1020 {
1021         struct v4l2_fh *vfh = file->private_data;
1022         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1023         unsigned int index = 0, i;
1024         unsigned long *fmts_bitmap = NULL;
1025
1026         if (chan->vi->pg_mode)
1027                 fmts_bitmap = chan->vi->tpg_fmts_bitmap;
1028         else
1029                 fmts_bitmap = chan->fmts_bitmap;
1030
1031         if (f->index >= bitmap_weight(fmts_bitmap, MAX_FORMAT_NUM))
1032                 return -EINVAL;
1033
1034         for (i = 0; i < f->index + 1; i++, index++)
1035                 index = find_next_bit(fmts_bitmap, MAX_FORMAT_NUM, index);
1036
1037         index -= 1;
1038         f->pixelformat = tegra_core_get_fourcc_by_idx(index);
1039         tegra_core_get_description_by_idx(index, f->description);
1040
1041         return 0;
1042 }
1043
1044 static int
1045 tegra_channel_g_edid(struct file *file, void *fh, struct v4l2_edid *edid)
1046 {
1047         struct v4l2_fh *vfh = file->private_data;
1048         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1049         struct v4l2_subdev *sd = chan->subdev_on_csi;
1050
1051         if (!v4l2_subdev_has_op(sd, pad, get_edid))
1052                 return -ENOTTY;
1053
1054         return v4l2_subdev_call(sd, pad, get_edid, edid);
1055 }
1056
1057 static int
1058 tegra_channel_s_edid(struct file *file, void *fh, struct v4l2_edid *edid)
1059 {
1060         struct v4l2_fh *vfh = file->private_data;
1061         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1062         struct v4l2_subdev *sd = chan->subdev_on_csi;
1063
1064         if (!v4l2_subdev_has_op(sd, pad, set_edid))
1065                 return -ENOTTY;
1066
1067         return v4l2_subdev_call(sd, pad, set_edid, edid);
1068 }
1069
1070 static int
1071 tegra_channel_s_dv_timings(struct file *file, void *fh,
1072                 struct v4l2_dv_timings *timings)
1073 {
1074         struct v4l2_fh *vfh = file->private_data;
1075         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1076         struct v4l2_bt_timings *bt = &timings->bt;
1077         int ret;
1078
1079         if (!v4l2_subdev_has_op(chan->subdev_on_csi, video, s_dv_timings))
1080                 return -ENOTTY;
1081
1082         ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
1083                         chan->grp_id, video, s_dv_timings, timings);
1084
1085         if (!ret) {
1086                 chan->format.width = bt->width;
1087                 chan->format.height = bt->height;
1088                 chan->format.bytesperline = bt->width *
1089                         chan->fmtinfo->bpp;
1090                 tegra_channel_fmt_align(chan,
1091                                         &chan->format.width,
1092                                         &chan->format.height,
1093                                         &chan->format.bytesperline);
1094                 chan->format.sizeimage = chan->format.bytesperline *
1095                         chan->format.height;
1096         }
1097
1098         if (chan->total_ports > 1)
1099                 update_gang_mode(chan);
1100
1101         return ret;
1102 }
1103
1104 static int
1105 tegra_channel_g_dv_timings(struct file *file, void *fh,
1106                 struct v4l2_dv_timings *timings)
1107 {
1108         struct v4l2_fh *vfh = file->private_data;
1109         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1110
1111         if (!v4l2_subdev_has_op(chan->subdev_on_csi, video, g_dv_timings))
1112                 return -ENOTTY;
1113
1114         return v4l2_device_call_until_err(chan->video.v4l2_dev,
1115                         chan->grp_id, video, g_dv_timings, timings);
1116 }
1117
1118 static int
1119 tegra_channel_query_dv_timings(struct file *file, void *fh,
1120                 struct v4l2_dv_timings *timings)
1121 {
1122         struct v4l2_fh *vfh = file->private_data;
1123         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1124
1125         if (!v4l2_subdev_has_op(chan->subdev_on_csi, video, query_dv_timings))
1126                 return -ENOTTY;
1127
1128         return v4l2_device_call_until_err(chan->video.v4l2_dev,
1129                         chan->grp_id, video, query_dv_timings, timings);
1130 }
1131
1132 static int
1133 tegra_channel_enum_dv_timings(struct file *file, void *fh,
1134                 struct v4l2_enum_dv_timings *timings)
1135 {
1136         struct v4l2_fh *vfh = file->private_data;
1137         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1138         struct v4l2_subdev *sd = chan->subdev_on_csi;
1139
1140         if (!v4l2_subdev_has_op(sd, pad, enum_dv_timings))
1141                 return -ENOTTY;
1142
1143         return v4l2_subdev_call(sd, pad, enum_dv_timings, timings);
1144 }
1145
1146 static int
1147 tegra_channel_dv_timings_cap(struct file *file, void *fh,
1148                 struct v4l2_dv_timings_cap *cap)
1149 {
1150         struct v4l2_fh *vfh = file->private_data;
1151         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1152         struct v4l2_subdev *sd = chan->subdev_on_csi;
1153
1154         if (!v4l2_subdev_has_op(sd, pad, dv_timings_cap))
1155                 return -ENOTTY;
1156
1157         return v4l2_subdev_call(sd, pad, dv_timings_cap, cap);
1158 }
1159
1160 static int tegra_channel_s_ctrl(struct v4l2_ctrl *ctrl)
1161 {
1162         struct tegra_channel *chan = container_of(ctrl->handler,
1163                                 struct tegra_channel, ctrl_handler);
1164
1165         switch (ctrl->id) {
1166         case V4L2_CID_VI_BYPASS_MODE:
1167                 if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON)
1168                         chan->bypass = true;
1169                 else if (chan->vi->vi->bypass) {
1170                         dev_dbg(&chan->video.dev,
1171                                 "can't disable bypass mode\n");
1172                         dev_dbg(&chan->video.dev,
1173                                 "because the VI/CSI is in bypass mode\n");
1174                         chan->bypass = true;
1175                 } else
1176                         chan->bypass = false;
1177                 break;
1178         default:
1179                 dev_err(&chan->video.dev, "%s:Not valid ctrl\n", __func__);
1180                 return -EINVAL;
1181         }
1182
1183         return 0;
1184 }
1185
1186 static const struct v4l2_ctrl_ops channel_ctrl_ops = {
1187         .s_ctrl = tegra_channel_s_ctrl,
1188 };
1189
1190 /**
1191  * By default channel will be in VI mode
1192  * User space can set it to 0 for working in bypass mode
1193  */
1194 static const struct v4l2_ctrl_config bypass_mode_ctrl = {
1195         .ops = &channel_ctrl_ops,
1196         .id = V4L2_CID_VI_BYPASS_MODE,
1197         .name = "Bypass Mode",
1198         .type = V4L2_CTRL_TYPE_INTEGER_MENU,
1199         .def = 0,
1200         .min = 0,
1201         .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
1202         .menu_skip_mask = 0,
1203         .qmenu_int = switch_ctrl_qmenu,
1204 };
1205
1206 static int tegra_channel_setup_controls(struct tegra_channel *chan)
1207 {
1208         int num_sd = 0;
1209         struct v4l2_subdev *sd = NULL;
1210
1211         /* Initialize the subdev and controls here at first open */
1212         sd = chan->subdev[num_sd];
1213         while ((sd = chan->subdev[num_sd++]) &&
1214                 (num_sd <= chan->num_subdevs)) {
1215                 /* Add control handler for the subdevice */
1216                 v4l2_ctrl_add_handler(&chan->ctrl_handler,
1217                                         sd->ctrl_handler, NULL);
1218                 if (chan->ctrl_handler.error)
1219                         dev_err(chan->vi->dev,
1220                                 "Failed to add sub-device controls\n");
1221         }
1222
1223         /* Add the bypass mode ctrl */
1224         v4l2_ctrl_new_custom(&chan->ctrl_handler, &bypass_mode_ctrl, NULL);
1225         if (chan->ctrl_handler.error) {
1226                 dev_err(chan->vi->dev,
1227                         "Failed to add bypass control\n");
1228                 return chan->ctrl_handler.error;
1229         }
1230
1231         /* setup the controls */
1232         return v4l2_ctrl_handler_setup(&chan->ctrl_handler);
1233 }
1234
1235 int tegra_channel_init_subdevices(struct tegra_channel *chan)
1236 {
1237         struct media_entity *entity;
1238         struct media_pad *pad;
1239         int index = 0;
1240         int num_sd = 0;
1241         int grp_id = chan->port[0] + 1;
1242         struct v4l2_subdev *sd;
1243
1244         /* set_stream of CSI */
1245         entity = &chan->video.entity;
1246         pad = media_entity_remote_source(&chan->pad);
1247         if (!pad)
1248                 return -ENODEV;
1249
1250         /* the remote source entity */
1251         entity = pad->entity;
1252         sd = media_entity_to_v4l2_subdev(entity);
1253         sd->grp_id = grp_id;
1254         chan->grp_id = grp_id;
1255         chan->subdev[num_sd++] = sd;
1256         /* Each CSI channel has only one pad, thus there
1257          * is only one subdev directly attached to this
1258          * CSI channel. Set this subdev to subdev_on_csi */
1259         chan->subdev_on_csi = sd;
1260
1261         /* Append subdev name to this video dev name*/
1262         snprintf(chan->video.name, sizeof(chan->video.name), "%s, %s",
1263         chan->video.name, sd->name);
1264
1265         index = pad->index - 1;
1266         while (index >= 0) {
1267                 pad = &entity->pads[index];
1268                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1269                         break;
1270
1271                 pad = media_entity_remote_source(pad);
1272                 if (pad == NULL ||
1273                     media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
1274                         break;
1275
1276                 if (num_sd >= MAX_SUBDEVICES)
1277                         break;
1278
1279                 entity = pad->entity;
1280                 sd = media_entity_to_v4l2_subdev(entity);
1281                 sd->grp_id = grp_id;
1282                 chan->subdev[num_sd++] = sd;
1283
1284                 index = pad->index - 1;
1285         }
1286         chan->num_subdevs = num_sd;
1287
1288         /* initialize the available formats */
1289         if (chan->num_subdevs)
1290                 tegra_channel_fmts_bitmap_init(chan);
1291
1292         return tegra_channel_setup_controls(chan);
1293 }
1294
1295 static int
1296 __tegra_channel_get_format(struct tegra_channel *chan,
1297                         struct v4l2_pix_format *pix)
1298 {
1299         struct tegra_video_format const *vfmt;
1300         struct v4l2_subdev_format fmt;
1301         int ret = 0;
1302         struct v4l2_subdev *sd = chan->subdev_on_csi;
1303
1304         memset(&fmt, 0x0, sizeof(fmt));
1305         fmt.pad = 0;
1306         ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
1307         if (ret == -ENOIOCTLCMD)
1308                 return -ENOTTY;
1309
1310         v4l2_fill_pix_format(pix, &fmt.format);
1311         vfmt = tegra_core_get_format_by_code(fmt.format.code);
1312         if (vfmt != NULL) {
1313                 pix->pixelformat = vfmt->fourcc;
1314                 tegra_channel_fmt_align(chan,
1315                         &pix->width, &pix->height, &pix->bytesperline);
1316                 pix->sizeimage = pix->height * pix->bytesperline;
1317         }
1318
1319         return ret;
1320 }
1321
1322 static int
1323 tegra_channel_get_format(struct file *file, void *fh,
1324                         struct v4l2_format *format)
1325 {
1326         struct v4l2_fh *vfh = file->private_data;
1327         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1328         struct v4l2_pix_format *pix = &format->fmt.pix;
1329
1330         return  __tegra_channel_get_format(chan, pix);
1331 }
1332
1333 static int
1334 __tegra_channel_try_format(struct tegra_channel *chan,
1335                         struct v4l2_pix_format *pix)
1336 {
1337         const struct tegra_video_format *vfmt;
1338         struct v4l2_subdev_format fmt;
1339         struct v4l2_subdev *sd = chan->subdev_on_csi;
1340         int ret = 0;
1341
1342         /* Use the channel format if pixformat is not supported */
1343         vfmt = tegra_core_get_format_by_fourcc(pix->pixelformat);
1344         if (!vfmt) {
1345                 pix->pixelformat = chan->format.pixelformat;
1346                 vfmt = tegra_core_get_format_by_fourcc(pix->pixelformat);
1347         }
1348
1349         tegra_channel_fmt_align(chan,
1350                                 &pix->width, &pix->height, &pix->bytesperline);
1351         pix->sizeimage = pix->bytesperline * pix->height;
1352
1353         fmt.which = V4L2_SUBDEV_FORMAT_TRY;
1354         fmt.pad = 0;
1355         v4l2_fill_mbus_format(&fmt.format, pix, vfmt->code);
1356
1357         ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
1358         if (ret == -ENOIOCTLCMD)
1359                 return -ENOTTY;
1360
1361         v4l2_fill_pix_format(pix, &fmt.format);
1362
1363         pix->sizeimage = pix->height * pix->bytesperline;
1364
1365         return ret;
1366 }
1367
1368 static int
1369 tegra_channel_try_format(struct file *file, void *fh,
1370                         struct v4l2_format *format)
1371 {
1372         struct v4l2_fh *vfh = file->private_data;
1373         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1374
1375         return  __tegra_channel_try_format(chan, &format->fmt.pix);
1376 }
1377
1378 static int
1379 __tegra_channel_set_format(struct tegra_channel *chan,
1380                         struct v4l2_pix_format *pix)
1381 {
1382         const struct tegra_video_format *vfmt;
1383         struct v4l2_subdev_format fmt;
1384         struct v4l2_subdev *sd = chan->subdev_on_csi;
1385         int ret = 0;
1386
1387         vfmt = tegra_core_get_format_by_fourcc(pix->pixelformat);
1388
1389         fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1390         fmt.pad = 0;
1391         v4l2_fill_mbus_format(&fmt.format, pix, vfmt->code);
1392
1393         ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
1394         if (ret == -ENOIOCTLCMD)
1395                 return -ENOTTY;
1396
1397         v4l2_fill_pix_format(pix, &fmt.format);
1398
1399         if (!ret) {
1400                 chan->format = *pix;
1401                 chan->fmtinfo = vfmt;
1402                 if (chan->total_ports > 1)
1403                         update_gang_mode(chan);
1404         }
1405
1406         return ret;
1407 }
1408
1409 static int
1410 tegra_channel_set_format(struct file *file, void *fh,
1411                         struct v4l2_format *format)
1412 {
1413         struct v4l2_fh *vfh = file->private_data;
1414         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1415         int ret = 0;
1416
1417         /* get the supported format by try_fmt */
1418         ret = __tegra_channel_try_format(chan, &format->fmt.pix);
1419         if (ret)
1420                 return ret;
1421
1422         if (vb2_is_busy(&chan->queue))
1423                 return -EBUSY;
1424
1425         return __tegra_channel_set_format(chan, &format->fmt.pix);
1426 }
1427
1428 static int tegra_channel_s_crop(struct file *file, void *fh,
1429                                 const struct v4l2_crop *crop)
1430 {
1431         struct v4l2_fh *vfh = file->private_data;
1432         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1433         struct v4l2_subdev *subdev = chan->subdev_on_csi;
1434         int ret = 0;
1435
1436         if (!v4l2_subdev_has_op(subdev, video, s_crop))
1437                 return -ENOTTY;
1438
1439         ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
1440                         chan->grp_id, video, s_crop, crop);
1441
1442         return ret;
1443 }
1444
1445 static int tegra_channel_g_crop(struct file *file, void *fh,
1446                                 struct v4l2_crop *crop)
1447 {
1448         struct v4l2_fh *vfh = file->private_data;
1449         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1450         struct v4l2_subdev *subdev = chan->subdev_on_csi;
1451         int ret = 0;
1452
1453         if (!v4l2_subdev_has_op(subdev, video, g_crop))
1454                 return -ENOTTY;
1455
1456         ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
1457                         chan->grp_id, video, g_crop, crop);
1458
1459         return ret;
1460 }
1461
1462 static int tegra_channel_subscribe_event(struct v4l2_fh *fh,
1463                                   const struct v4l2_event_subscription *sub)
1464 {
1465         switch (sub->type) {
1466         case V4L2_EVENT_SOURCE_CHANGE:
1467                 return v4l2_event_subscribe(fh, sub, 4, NULL);
1468         }
1469         return v4l2_ctrl_subscribe_event(fh, sub);
1470 }
1471
1472 static int
1473 tegra_channel_enum_input(struct file *file, void *fh, struct v4l2_input *inp)
1474 {
1475         struct v4l2_fh *vfh = file->private_data;
1476         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1477         struct v4l2_subdev *sd_on_csi = chan->subdev_on_csi;
1478         int ret;
1479
1480         if (inp->index)
1481                 return -EINVAL;
1482
1483         ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
1484                         chan->grp_id, video, g_input_status, &inp->status);
1485
1486         if (ret != -ENODEV) {
1487                 if (v4l2_subdev_has_op(sd_on_csi, video, s_dv_timings))
1488                         inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
1489
1490                 inp->type = V4L2_INPUT_TYPE_CAMERA;
1491                 if (inp->capabilities == V4L2_IN_CAP_DV_TIMINGS)
1492                         snprintf(inp->name,
1493                                 sizeof(inp->name), "HDMI %u",
1494                                 chan->port[0]);
1495                 else
1496                         snprintf(inp->name,
1497                                 sizeof(inp->name), "Camera %u",
1498                                 chan->port[0]);
1499
1500                 return ret;
1501         }
1502
1503         return -ENOTTY;
1504 }
1505
1506 static int tegra_channel_g_input(struct file *file, void *priv, unsigned int *i)
1507 {
1508         *i = 0;
1509         return 0;
1510 }
1511
1512 static int tegra_channel_s_input(struct file *file, void *priv, unsigned int i)
1513 {
1514         if (i > 0)
1515                 return -EINVAL;
1516         return 0;
1517 }
1518
1519 static int tegra_channel_g_parm(struct file *file, void *fh,
1520                                 struct v4l2_streamparm *parm)
1521 {
1522         struct v4l2_fh *vfh = file->private_data;
1523         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1524         struct v4l2_subdev *subdev = chan->subdev_on_csi;
1525         int ret = 0;
1526
1527         ret = v4l2_subdev_call(subdev, video, g_parm, parm);
1528         if (ret == -ENOIOCTLCMD)
1529                 return -ENOTTY;
1530
1531         return ret;
1532 }
1533
1534 static int tegra_channel_s_parm(struct file *file, void *fh,
1535                              struct v4l2_streamparm *parm)
1536 {
1537         struct v4l2_fh *vfh = file->private_data;
1538         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1539         struct v4l2_subdev *subdev = chan->subdev_on_csi;
1540         int ret = 0;
1541
1542         ret = v4l2_subdev_call(subdev, video, s_parm, parm);
1543         if (ret == -ENOIOCTLCMD)
1544                 return -ENOTTY;
1545
1546         return ret;
1547 }
1548
1549 static const struct v4l2_ioctl_ops tegra_channel_ioctl_ops = {
1550         .vidioc_querycap                = tegra_channel_querycap,
1551         .vidioc_enum_framesizes         = tegra_channel_enum_framesizes,
1552         .vidioc_enum_frameintervals     = tegra_channel_enum_frameintervals,
1553         .vidioc_enum_fmt_vid_cap        = tegra_channel_enum_format,
1554         .vidioc_g_fmt_vid_cap           = tegra_channel_get_format,
1555         .vidioc_s_fmt_vid_cap           = tegra_channel_set_format,
1556         .vidioc_try_fmt_vid_cap         = tegra_channel_try_format,
1557         .vidioc_s_crop                  = tegra_channel_s_crop,
1558         .vidioc_g_crop                  = tegra_channel_g_crop,
1559         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1560         .vidioc_querybuf                = vb2_ioctl_querybuf,
1561         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1562         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1563         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1564         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1565         .vidioc_streamon                = vb2_ioctl_streamon,
1566         .vidioc_streamoff               = vb2_ioctl_streamoff,
1567         .vidioc_g_edid                  = tegra_channel_g_edid,
1568         .vidioc_s_edid                  = tegra_channel_s_edid,
1569         .vidioc_s_dv_timings            = tegra_channel_s_dv_timings,
1570         .vidioc_g_dv_timings            = tegra_channel_g_dv_timings,
1571         .vidioc_query_dv_timings        = tegra_channel_query_dv_timings,
1572         .vidioc_enum_dv_timings         = tegra_channel_enum_dv_timings,
1573         .vidioc_dv_timings_cap          = tegra_channel_dv_timings_cap,
1574         .vidioc_subscribe_event         = tegra_channel_subscribe_event,
1575         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1576         .vidioc_enum_input              = tegra_channel_enum_input,
1577         .vidioc_g_input                 = tegra_channel_g_input,
1578         .vidioc_s_input                 = tegra_channel_s_input,
1579         .vidioc_g_parm                  = tegra_channel_g_parm,
1580         .vidioc_s_parm                  = tegra_channel_s_parm,
1581 };
1582
1583 static int tegra_channel_open(struct file *fp)
1584 {
1585         int ret;
1586         struct video_device *vdev = video_devdata(fp);
1587         struct tegra_channel *chan = video_get_drvdata(vdev);
1588         struct vi *tegra_vi;
1589         struct tegra_mc_vi *vi;
1590         struct tegra_csi_device *csi;
1591
1592         mutex_lock(&chan->video_lock);
1593         ret = v4l2_fh_open(fp);
1594         if (ret || !v4l2_fh_is_singular_file(fp))
1595                 goto unlock;
1596
1597         if (chan->subdev_on_csi == NULL) {
1598                 ret = -ENODEV;
1599                 goto unlock;
1600         }
1601
1602         vi = chan->vi;
1603         tegra_vi = vi->vi;
1604         csi = vi->csi;
1605
1606         /* TPG mode and a real sensor is open, return busy */
1607         if (vi->pg_mode && tegra_vi->sensor_opened)
1608                 return -EBUSY;
1609
1610         /* None TPG mode and a TPG channel is opened, return busy */
1611         if (!vi->pg_mode && tegra_vi->tpg_opened)
1612                 return -EBUSY;
1613
1614         /* The first open then turn on power */
1615         tegra_vi_power_on(vi);
1616         if (atomic_add_return(1, &vi->csi_power_on_refcnt) == 1) {
1617                 tegra_csi_power_on(csi);
1618                 if (vi->pg_mode)
1619                         tegra_vi->tpg_opened = true;
1620                 else
1621                         tegra_vi->sensor_opened = true;
1622         }
1623
1624         if (!vi->pg_mode &&
1625                 (atomic_add_return(1, &chan->power_on_refcnt) == 1)) {
1626                 /* power on sensors connected in channel */
1627                 tegra_csi_channel_power_on(csi, chan->port);
1628                 ret = tegra_channel_set_power(chan, 1);
1629                 if (ret < 0)
1630                         goto unlock;
1631         }
1632
1633         chan->fh = (struct v4l2_fh *)fp->private_data;
1634
1635 unlock:
1636         mutex_unlock(&chan->video_lock);
1637         return ret;
1638 }
1639
1640 static int tegra_channel_close(struct file *fp)
1641 {
1642         int ret = 0;
1643         struct video_device *vdev = video_devdata(fp);
1644         struct tegra_channel *chan = video_get_drvdata(vdev);
1645         struct tegra_mc_vi *vi = chan->vi;
1646         struct vi *tegra_vi = vi->vi;
1647         struct tegra_csi_device *csi = vi->csi;
1648         bool is_singular;
1649
1650         mutex_lock(&chan->video_lock);
1651         is_singular = v4l2_fh_is_singular_file(fp);
1652         ret = _vb2_fop_release(fp, NULL);
1653
1654         if (!is_singular) {
1655                 mutex_unlock(&chan->video_lock);
1656                 return ret;
1657         }
1658
1659         if (!vi->pg_mode &&
1660                 atomic_dec_and_test(&chan->power_on_refcnt)) {
1661                 /* power off sensors connected in channel */
1662                 tegra_csi_channel_power_off(csi, chan->port);
1663                 ret = tegra_channel_set_power(chan, 0);
1664                 if (ret < 0)
1665                         dev_err(vi->dev, "Failed to power off subdevices\n");
1666         }
1667
1668         /* The last release then turn off power */
1669         if (atomic_dec_and_test(&vi->csi_power_on_refcnt)) {
1670                 tegra_csi_power_off(csi);
1671                 if (vi->pg_mode)
1672                         tegra_vi->tpg_opened = false;
1673                 else
1674                         tegra_vi->sensor_opened = false;
1675         }
1676         tegra_vi_power_off(vi);
1677
1678         mutex_unlock(&chan->video_lock);
1679         return ret;
1680 }
1681
1682 /* -----------------------------------------------------------------------------
1683  * V4L2 file operations
1684  */
1685 static const struct v4l2_file_operations tegra_channel_fops = {
1686         .owner          = THIS_MODULE,
1687         .unlocked_ioctl = video_ioctl2,
1688         .open           = tegra_channel_open,
1689         .release        = tegra_channel_close,
1690         .read           = vb2_fop_read,
1691         .poll           = vb2_fop_poll,
1692         .mmap           = vb2_fop_mmap,
1693 };
1694
1695 static void vi_channel_syncpt_init(struct tegra_channel *chan)
1696 {
1697         int i;
1698
1699         for (i = 0; i < chan->total_ports; i++)
1700                 chan->syncpt[i] =
1701                         nvhost_get_syncpt_client_managed(chan->vi->ndev, "vi");
1702 }
1703
1704 static void vi_channel_syncpt_free(struct tegra_channel *chan)
1705 {
1706         int i;
1707
1708         for (i = 0; i < chan->total_ports; i++)
1709                 nvhost_syncpt_put_ref_ext(chan->vi->ndev, chan->syncpt[i]);
1710 }
1711
1712 static void tegra_channel_csi_init(struct tegra_mc_vi *vi, unsigned int index)
1713 {
1714         int numlanes = 0;
1715         int idx = 0;
1716         struct tegra_channel *chan  = &vi->chans[index];
1717
1718         chan->gang_mode = CAMERA_NO_GANG_MODE;
1719         chan->total_ports = 0;
1720         memset(&chan->port[0], INVALID_CSI_PORT, TEGRA_CSI_BLOCKS);
1721         memset(&chan->syncpoint_fifo[0], 0, TEGRA_CSI_BLOCKS);
1722         if (vi->pg_mode) {
1723                 chan->port[0] = index;
1724                 chan->numlanes = 2;
1725         } else
1726                 tegra_vi_get_port_info(chan, vi->dev->of_node, index);
1727
1728         for (idx = 0; csi_port_is_valid(chan->port[idx]); idx++) {
1729                 chan->total_ports++;
1730                 numlanes = chan->numlanes - (idx * 4);
1731                 numlanes = numlanes > 4 ? 4 : numlanes;
1732                 /* maximum of 4 lanes are present per CSI block */
1733                 chan->csibase[idx] = vi->iomem +
1734                                         TEGRA_VI_CSI_BASE(chan->port[idx]);
1735                 set_csi_portinfo(vi->csi, chan->port[idx], numlanes);
1736         }
1737         /* based on gang mode valid ports will be updated - set default to 1 */
1738         chan->valid_ports = chan->total_ports ? 1 : 0;
1739 }
1740
1741 static int tegra_channel_init(struct tegra_mc_vi *vi, unsigned int index)
1742 {
1743         int ret;
1744         struct tegra_channel *chan = &vi->chans[index];
1745
1746         /* VI/CSI is in bypass mode, then channel has to be in bypass */
1747         if (vi->vi->bypass)
1748                 chan->bypass = true;
1749
1750         chan->vi = vi;
1751         chan->fops = vi->vi->data->channel_fops;
1752         tegra_channel_csi_init(vi, index);
1753
1754         chan->width_align = TEGRA_WIDTH_ALIGNMENT;
1755         chan->stride_align = TEGRA_STRIDE_ALIGNMENT;
1756         chan->num_subdevs = 0;
1757         mutex_init(&chan->video_lock);
1758         INIT_LIST_HEAD(&chan->capture);
1759         init_waitqueue_head(&chan->start_wait);
1760         spin_lock_init(&chan->start_lock);
1761         mutex_init(&chan->stop_kthread_lock);
1762         init_completion(&chan->capture_comp);
1763         atomic_set(&chan->is_streaming, DISABLE);
1764
1765         /* Init video format */
1766         chan->fmtinfo = tegra_core_get_format_by_code(TEGRA_VF_DEF);
1767         chan->format.pixelformat = chan->fmtinfo->fourcc;
1768         chan->format.colorspace = V4L2_COLORSPACE_SRGB;
1769         chan->format.field = V4L2_FIELD_NONE;
1770         chan->format.width = TEGRA_DEF_WIDTH;
1771         chan->format.height = TEGRA_DEF_HEIGHT;
1772         chan->format.bytesperline = chan->format.width * chan->fmtinfo->bpp;
1773         chan->format.sizeimage = chan->format.bytesperline *
1774                                     chan->format.height;
1775         chan->buffer_offset[0] = 0;
1776
1777         /* Initialize the media entity... */
1778         chan->pad.flags = MEDIA_PAD_FL_SINK;
1779
1780         ret = media_entity_init(&chan->video.entity, 1, &chan->pad, 0);
1781         if (ret < 0)
1782                 return ret;
1783
1784         /* init control handler */
1785         ret = v4l2_ctrl_handler_init(&chan->ctrl_handler, MAX_CID_CONTROLS);
1786         if (chan->ctrl_handler.error) {
1787                 dev_err(&chan->video.dev, "failed to init control handler\n");
1788                 goto video_register_error;
1789         }
1790
1791         /* init video node... */
1792         chan->video.fops = &tegra_channel_fops;
1793         chan->video.v4l2_dev = &vi->v4l2_dev;
1794         chan->video.queue = &chan->queue;
1795         snprintf(chan->video.name, sizeof(chan->video.name), "%s-%s-%u",
1796                 dev_name(vi->dev), vi->pg_mode ? "tpg" : "output",
1797                 chan->port[0]);
1798         chan->video.vfl_type = VFL_TYPE_GRABBER;
1799         chan->video.vfl_dir = VFL_DIR_RX;
1800         chan->video.release = video_device_release_empty;
1801         chan->video.ioctl_ops = &tegra_channel_ioctl_ops;
1802         chan->video.ctrl_handler = &chan->ctrl_handler;
1803         chan->video.lock = &chan->video_lock;
1804
1805         set_bit(V4L2_FL_USE_FH_PRIO, &chan->video.flags);
1806
1807         video_set_drvdata(&chan->video, chan);
1808
1809         vi_channel_syncpt_init(chan);
1810
1811         /* get the buffers queue... */
1812         chan->alloc_ctx = vb2_dma_contig_init_ctx(chan->vi->dev);
1813         if (IS_ERR(chan->alloc_ctx)) {
1814                 dev_err(chan->vi->dev, "failed to init vb2 buffer\n");
1815                 ret = -ENOMEM;
1816                 goto vb2_init_error;
1817         }
1818
1819         chan->queue.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1820         chan->queue.io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ | VB2_USERPTR;
1821         chan->queue.lock = &chan->video_lock;
1822         chan->queue.drv_priv = chan;
1823         chan->queue.buf_struct_size = sizeof(struct tegra_channel_buffer);
1824         chan->queue.ops = &tegra_channel_queue_qops;
1825         chan->queue.mem_ops = &vb2_dma_contig_memops;
1826         chan->queue.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
1827                                    | V4L2_BUF_FLAG_TSTAMP_SRC_EOF;
1828         ret = vb2_queue_init(&chan->queue);
1829         if (ret < 0) {
1830                 dev_err(chan->vi->dev, "failed to initialize VB2 queue\n");
1831                 goto vb2_queue_error;
1832         }
1833
1834         ret = video_register_device(&chan->video, VFL_TYPE_GRABBER, -1);
1835         if (ret < 0) {
1836                 dev_err(&chan->video.dev, "failed to register video device\n");
1837                 goto video_register_error;
1838         }
1839
1840         return 0;
1841
1842 video_register_error:
1843         vb2_queue_release(&chan->queue);
1844 vb2_queue_error:
1845         vb2_dma_contig_cleanup_ctx(chan->alloc_ctx);
1846 vb2_init_error:
1847         media_entity_cleanup(&chan->video.entity);
1848         return ret;
1849 }
1850
1851 static int tegra_channel_cleanup(struct tegra_channel *chan)
1852 {
1853         video_unregister_device(&chan->video);
1854
1855         v4l2_ctrl_handler_free(&chan->ctrl_handler);
1856         vb2_queue_release(&chan->queue);
1857         vb2_dma_contig_cleanup_ctx(chan->alloc_ctx);
1858
1859         vi_channel_syncpt_free(chan);
1860         media_entity_cleanup(&chan->video.entity);
1861
1862         return 0;
1863 }
1864
1865 int tegra_vi_channels_init(struct tegra_mc_vi *vi)
1866 {
1867         unsigned int i;
1868         int ret;
1869
1870         for (i = 0; i < vi->num_channels; i++) {
1871                 ret = tegra_channel_init(vi, i);
1872                 if (ret < 0) {
1873                         dev_err(vi->dev, "channel %d init failed\n", i);
1874                         return ret;
1875                 }
1876         }
1877         return 0;
1878 }
1879 EXPORT_SYMBOL(tegra_vi_channels_init);
1880
1881 int tegra_vi_channels_cleanup(struct tegra_mc_vi *vi)
1882 {
1883         unsigned int i;
1884         int ret;
1885
1886         for (i = 0; i < vi->num_channels; i++) {
1887                 ret = tegra_channel_cleanup(&vi->chans[i]);
1888                 if (ret < 0) {
1889                         dev_err(vi->dev, "channel %d cleanup failed\n", i);
1890                         return ret;
1891                 }
1892         }
1893         return 0;
1894 }
1895 EXPORT_SYMBOL(tegra_vi_channels_cleanup);
1896
1897 int tegra_clean_unlinked_channels(struct tegra_mc_vi *vi)
1898 {
1899         unsigned int i;
1900         int ret;
1901
1902         for (i = 0; i < vi->num_channels; i++) {
1903                 struct tegra_channel *chan = &vi->chans[i];
1904
1905                 if (chan->num_subdevs)
1906                         continue;
1907
1908                 ret = tegra_channel_cleanup(chan);
1909                 if (ret < 0) {
1910                         dev_err(vi->dev, "channel %d cleanup failed\n", i);
1911                         return ret;
1912                 }
1913         }
1914
1915         return 0;
1916 }
1917 EXPORT_SYMBOL(tegra_clean_unlinked_channels);