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