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

   1 /*
   2  * NVIDIA Tegra Video Input Device
   3  *
   4  * Copyright (c) 2015-2018, 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/lcm.h>
  19 #include <linux/list.h>
  20 #include <linux/module.h>
  21 #include <linux/of.h>
  22 #include <linux/of_graph.h>
  23 #include <linux/sched.h>
  24 #include <linux/slab.h>
  25
  26 #include <media/v4l2-ctrls.h>
  27 #include <media/v4l2-event.h>
  28 #include <media/v4l2-dev.h>
  29 #include <media/v4l2-fh.h>
  30 #include <media/v4l2-ioctl.h>
  31 #include <media/videobuf2-core.h>
  32 #include <media/videobuf2-dma-contig.h>
  33 #include <media/tegra-v4l2-camera.h>
  34 #include <media/camera_common.h>
  35 #include <media/tegra_camera_platform.h>
  36 #include <media/v4l2-dv-timings.h>
  37
  38 #include <linux/clk/tegra.h>
  39
  40 #include "mc_common.h"
  41 #include "vi/vi.h"
  42 #include "mipical/mipi_cal.h"
  43 #include "nvcsi/nvcsi.h"
  44
  45 #define TPG_CSI_GROUP_ID        10
  46
  47 static s64 queue_init_ts;
  48
  49 static void gang_buffer_offsets(struct tegra_channel *chan)
  50 {
  51         int i;
  52         u32 offset = 0;
  53
  54         for (i = 0; i < chan->total_ports; i++) {
  55                 switch (chan->gang_mode) {
  56                 case CAMERA_NO_GANG_MODE:
  57                 case CAMERA_GANG_L_R:
  58                 case CAMERA_GANG_R_L:
  59                         offset = chan->gang_bytesperline;
  60                         break;
  61                 case CAMERA_GANG_T_B:
  62                 case CAMERA_GANG_B_T:
  63                         offset = chan->gang_sizeimage;
  64                         break;
  65                 default:
  66                         offset = 0;
  67                 }
  68                 offset = ((offset + TEGRA_SURFACE_ALIGNMENT - 1) &
  69                                         ~(TEGRA_SURFACE_ALIGNMENT - 1));
  70                 chan->buffer_offset[i] = i * offset;
  71         }
  72 }
  73
  74 static u32 gang_mode_width(enum camera_gang_mode gang_mode,
  75                                         unsigned int width)
  76 {
  77         if ((gang_mode == CAMERA_GANG_L_R) ||
  78                 (gang_mode == CAMERA_GANG_R_L))
  79                 return width >> 1;
  80         else
  81                 return width;
  82 }
  83
  84 static u32 gang_mode_height(enum camera_gang_mode gang_mode,
  85                                         unsigned int height)
  86 {
  87         if ((gang_mode == CAMERA_GANG_T_B) ||
  88                 (gang_mode == CAMERA_GANG_B_T))
  89                 return height >> 1;
  90         else
  91                 return height;
  92 }
  93
  94 static void update_gang_mode_params(struct tegra_channel *chan)
  95 {
  96         chan->gang_width = gang_mode_width(chan->gang_mode,
  97                                                 chan->format.width);
  98         chan->gang_height = gang_mode_height(chan->gang_mode,
  99                                                 chan->format.height);
 100         chan->gang_bytesperline = ((chan->gang_width *
 101                                         chan->fmtinfo->bpp.numerator) /
 102                                         chan->fmtinfo->bpp.denominator);
 103         chan->gang_sizeimage = chan->gang_bytesperline *
 104                                         chan->format.height;
 105         gang_buffer_offsets(chan);
 106 }
 107
 108 static void update_gang_mode(struct tegra_channel *chan)
 109 {
 110         int width = chan->format.width;
 111         int height = chan->format.height;
 112
 113         /*
 114          * At present only 720p, 1080p and 4k resolutions
 115          * are supported and only 4K requires gang mode
 116          * Update this code with CID for future extensions
 117          * Also, validate width and height of images based
 118          * on gang mode and surface stride alignment
 119          */
 120         if ((width > 1920) && (height > 1080)) {
 121                 chan->gang_mode = CAMERA_GANG_L_R;
 122                 chan->valid_ports = chan->total_ports;
 123         } else {
 124                 chan->gang_mode = CAMERA_NO_GANG_MODE;
 125                 chan->valid_ports = 1;
 126         }
 127
 128         update_gang_mode_params(chan);
 129 }
 130
 131 static u32 get_aligned_buffer_size(struct tegra_channel *chan,
 132                 u32 bytesperline, u32 height)
 133 {
 134         u32 height_aligned;
 135         u32 temp_size, size;
 136
 137         height_aligned = roundup(height, chan->height_align);
 138         temp_size = bytesperline * height_aligned;
 139         size = roundup(temp_size, chan->size_align);
 140
 141         return size;
 142 }
 143
 144 static void tegra_channel_fmt_align(struct tegra_channel *chan,
 145                                 const struct tegra_video_format *vfmt,
 146                                 u32 *width, u32 *height, u32 *bytesperline)
 147 {
 148         unsigned int min_width;
 149         unsigned int max_width;
 150         unsigned int min_bpl;
 151         unsigned int max_bpl;
 152         unsigned int temp_width;
 153         unsigned int align, fmt_align;
 154         unsigned int temp_bpl;
 155         unsigned int bpl;
 156         unsigned int numerator, denominator;
 157         const struct tegra_frac *bpp = &vfmt->bpp;
 158
 159         /* Init, if un-init */
 160         if (!*width || !*height) {
 161                 *width = chan->format.width;
 162                 *height = chan->format.height;
 163         }
 164
 165         denominator = (!bpp->denominator) ? 1 : bpp->denominator;
 166         numerator = (!bpp->numerator) ? 1 : bpp->numerator;
 167
 168         bpl = (*width * numerator) / denominator;
 169         if (!*bytesperline)
 170                 *bytesperline = bpl;
 171
 172         /* The transfer alignment requirements are expressed in bytes. Compute
 173          * the minimum and maximum values, clamp the requested width and convert
 174          * it back to pixels.
 175          * use denominator for base width alignment when >1.
 176          * use bytesperline to adjust width for applicaton related requriements.
 177          */
 178         fmt_align = (denominator == 1) ? numerator : 1;
 179         align = lcm(chan->width_align, fmt_align);
 180         min_width = roundup(TEGRA_MIN_WIDTH, align);
 181         max_width = rounddown(TEGRA_MAX_WIDTH, align);
 182         temp_width = roundup(bpl, align);
 183
 184         *width = (clamp(temp_width, min_width, max_width) * denominator) /
 185                         numerator;
 186         *height = clamp(*height, TEGRA_MIN_HEIGHT, TEGRA_MAX_HEIGHT);
 187
 188         /* Clamp the requested bytes per line value. If the maximum bytes per
 189          * line value is zero, the module doesn't support user configurable line
 190          * sizes. Override the requested value with the minimum in that case.
 191          */
 192         min_bpl = bpl;
 193         max_bpl = rounddown(TEGRA_MAX_WIDTH, chan->stride_align);
 194         temp_bpl = roundup(*bytesperline, chan->stride_align);
 195
 196         *bytesperline = clamp(temp_bpl, min_bpl, max_bpl);
 197 }
 198
 199 static void tegra_channel_update_format(struct tegra_channel *chan,
 200                 u32 width, u32 height, u32 fourcc,
 201                 const struct tegra_frac *bpp,
 202                 u32 preferred_stride)
 203 {
 204         u32 denominator = (!bpp->denominator) ? 1 : bpp->denominator;
 205         u32 numerator = (!bpp->numerator) ? 1 : bpp->numerator;
 206         u32 bytesperline = (width * numerator / denominator);
 207
 208         chan->format.width = width;
 209         chan->format.height = height;
 210         chan->format.pixelformat = fourcc;
 211         chan->format.bytesperline = preferred_stride ?: bytesperline;
 212
 213         tegra_channel_fmt_align(chan, chan->fmtinfo,
 214                                 &chan->format.width,
 215                                 &chan->format.height,
 216                                 &chan->format.bytesperline);
 217
 218         /* Calculate the sizeimage per plane */
 219         chan->format.sizeimage = get_aligned_buffer_size(chan,
 220                         chan->format.bytesperline, chan->format.height);
 221
 222         if (fourcc == V4L2_PIX_FMT_NV16)
 223                 chan->format.sizeimage *= 2;
 224 }
 225
 226 static void tegra_channel_fmts_bitmap_init(struct tegra_channel *chan)
 227 {
 228         int ret, pixel_format_index = 0, init_code = 0;
 229         struct v4l2_subdev *subdev = chan->subdev_on_csi;
 230         struct v4l2_subdev_format fmt = {};
 231         struct v4l2_subdev_mbus_code_enum code = {
 232                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 233         };
 234
 235         bitmap_zero(chan->fmts_bitmap, MAX_FORMAT_NUM);
 236
 237         /*
 238          * Initialize all the formats available from
 239          * the sub-device and extract the corresponding
 240          * index from the pre-defined video formats and initialize
 241          * the channel default format with the active code
 242          * Index zero as the only sub-device is sensor
 243          */
 244         while (1) {
 245                 ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
 246                                        NULL, &code);
 247                 if (ret < 0)
 248                         /* no more formats */
 249                         break;
 250
 251                 pixel_format_index =
 252                         tegra_core_get_idx_by_code(chan, code.code, 0);
 253                 while (pixel_format_index >= 0) {
 254                         bitmap_set(chan->fmts_bitmap, pixel_format_index, 1);
 255                         /* Set init_code to the first matched format */
 256                         if (!init_code)
 257                                 init_code = code.code;
 258                         /* Look for other formats with the same mbus code */
 259                         pixel_format_index = tegra_core_get_idx_by_code(chan,
 260                                 code.code, pixel_format_index + 1);
 261                 }
 262
 263                 code.index++;
 264         }
 265
 266         if (!init_code) {
 267                 pixel_format_index =
 268                         tegra_core_get_idx_by_code(chan, TEGRA_VF_DEF, 0);
 269                 if (pixel_format_index >= 0) {
 270                         bitmap_set(chan->fmts_bitmap, pixel_format_index, 1);
 271                         init_code = TEGRA_VF_DEF;
 272                 }
 273         }
 274                 /* Get the format based on active code of the sub-device */
 275         ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
 276         if (ret)
 277                 return;
 278
 279         /* Initiate the channel format to the first matched format */
 280         chan->fmtinfo =
 281                 tegra_core_get_format_by_code(chan, fmt.format.code, 0);
 282         v4l2_fill_pix_format(&chan->format, &fmt.format);
 283         tegra_channel_update_format(chan, chan->format.width,
 284                                 chan->format.height,
 285                                 chan->fmtinfo->fourcc,
 286                                 &chan->fmtinfo->bpp, 0);
 287
 288         if (chan->total_ports > 1)
 289                 update_gang_mode(chan);
 290 }
 291
 292 /*
 293  * -----------------------------------------------------------------------------
 294  * Tegra channel frame setup and capture operations
 295  * -----------------------------------------------------------------------------
 296  */
 297
 298 void release_buffer(struct tegra_channel *chan, struct tegra_channel_buffer* buf)
 299 {
 300         struct vb2_v4l2_buffer* vbuf = &buf->buf;
 301         s64 frame_arrived_ts = 0;
 302         /* release one frame */
 303         vbuf->sequence = chan->sequence++;
 304         vbuf->field = V4L2_FIELD_NONE;
 305         vb2_set_plane_payload(&vbuf->vb2_buf,
 306                 0, chan->format.sizeimage);
 307
 308         /*
 309          * WAR to force buffer state if capture state is not good
 310          * WAR - After sync point timeout or error frame capture
 311          * the second buffer is intermittently frame of zeros
 312          * with no error status or padding.
 313          */
 314         if (chan->capture_state != CAPTURE_GOOD || vbuf->sequence < 2) {
 315                 buf->state = VB2_BUF_STATE_ERROR;
 316         }
 317
 318         if (chan->sequence == 1) {
 319                 /*
 320                  * Evaluate the initial capture latency between videobuf2 queue
 321                  * and first captured frame release to user-space.
 322                  */
 323                 frame_arrived_ts = ktime_to_ms(ktime_get());
 324                 dev_dbg(&chan->video.dev, "%s: capture init latency is %lld ms\n",
 325                         __func__, (frame_arrived_ts - queue_init_ts));
 326         }
 327
 328         dev_dbg(&chan->video.dev,
 329                 "%s: release buf[%p] frame[%d] to user-space\n",
 330                 __func__, buf, chan->sequence);
 331         vb2_buffer_done(&vbuf->vb2_buf, buf->state);
 332 }
 333
 334 /*
 335  * `buf` has been successfully setup to receive a frame and is
 336  * "in flight" through the VI hardware. We are currently waiting
 337  * on it to be filled. Moves the pointer into the `release` list
 338  * for the release thread to wait on.
 339  */
 340 void enqueue_inflight(struct tegra_channel *chan,
 341                 struct tegra_channel_buffer *buf)
 342 {
 343         /* Put buffer into the release queue */
 344         spin_lock(&chan->release_lock);
 345         list_add_tail(&buf->queue, &chan->release);
 346         spin_unlock(&chan->release_lock);
 347
 348         /* Wake up kthread for release */
 349         wake_up_interruptible(&chan->release_wait);
 350 }
 351
 352 void tegra_channel_ec_close(struct tegra_mc_vi *vi)
 353 {
 354         struct tegra_channel *chan;
 355
 356         /* clear all channles sync point fifo context */
 357         list_for_each_entry(chan, &vi->vi_chans, list) {
 358                 memset(&chan->syncpoint_fifo[0], 0, TEGRA_CSI_BLOCKS);
 359         }
 360 }
 361
 362 struct tegra_channel_buffer* dequeue_inflight(struct tegra_channel* chan)
 363 {
 364
 365         struct tegra_channel_buffer *buf = NULL;
 366
 367         spin_lock(&chan->release_lock);
 368         if (list_empty(&chan->release)) {
 369                 spin_unlock(&chan->release_lock);
 370                 return NULL;
 371         }
 372
 373         buf = list_entry(chan->release.next,
 374                          struct tegra_channel_buffer, queue);
 375
 376         if(buf) {
 377                 list_del_init(&buf->queue);
 378         }
 379         spin_unlock(&chan->release_lock);
 380         return buf;
 381 }
 382
 383 struct tegra_channel_buffer *dequeue_buffer(struct tegra_channel *chan)
 384 {
 385         struct tegra_channel_buffer *buf = NULL;
 386
 387         spin_lock(&chan->start_lock);
 388         if (list_empty(&chan->capture))
 389                 goto done;
 390
 391         buf = list_entry(chan->capture.next,
 392                          struct tegra_channel_buffer, queue);
 393         list_del_init(&buf->queue);
 394
 395 done:
 396         spin_unlock(&chan->start_lock);
 397         return buf;
 398 }
 399
 400 /*
 401  * -----------------------------------------------------------------------------
 402  * videobuf2 queue operations
 403  * -----------------------------------------------------------------------------
 404  */
 405 static int
 406 tegra_channel_queue_setup(struct vb2_queue *vq, const void *parg,
 407                      unsigned int *nbuffers, unsigned int *nplanes,
 408                      unsigned int sizes[], void *alloc_ctxs[])
 409 {
 410         const struct v4l2_format *fmt = parg;
 411         struct tegra_channel *chan = vb2_get_drv_priv(vq);
 412         /* Make sure the image size is large enough. */
 413         if (fmt && fmt->fmt.pix.sizeimage < chan->format.sizeimage)
 414                 return -EINVAL;
 415
 416         *nplanes = 1;
 417
 418         sizes[0] = fmt ? fmt->fmt.pix.sizeimage : chan->format.sizeimage;
 419         alloc_ctxs[0] = chan->alloc_ctx;
 420
 421         /* Make sure minimum number of buffers are passed */
 422         if (*nbuffers < (QUEUED_BUFFERS - 1))
 423                 *nbuffers = QUEUED_BUFFERS - 1;
 424
 425         return 0;
 426 }
 427
 428 static int tegra_channel_buffer_prepare(struct vb2_buffer *vb)
 429 {
 430         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 431         struct tegra_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
 432         struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vbuf);
 433
 434         buf->chan = chan;
 435         vb2_set_plane_payload(&vbuf->vb2_buf, 0, chan->format.sizeimage);
 436 #if defined(CONFIG_VIDEOBUF2_DMA_CONTIG)
 437         buf->addr = vb2_dma_contig_plane_dma_addr(vb, 0);
 438 #endif
 439
 440         return 0;
 441 }
 442
 443 static void tegra_channel_buffer_queue(struct vb2_buffer *vb)
 444 {
 445         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 446         struct tegra_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
 447         struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vbuf);
 448
 449         /* for bypass mode - do nothing */
 450         if (chan->bypass)
 451                 return;
 452
 453         if (!queue_init_ts) {
 454                 /*
 455                  * Record videobuf2 queue initial timestamp.
 456                  * Note: latency is accurate when streaming is already turned ON
 457                  */
 458                 queue_init_ts = ktime_to_ms(ktime_get());
 459         }
 460
 461         /* Put buffer into the capture queue */
 462         spin_lock(&chan->start_lock);
 463         list_add_tail(&buf->queue, &chan->capture);
 464         spin_unlock(&chan->start_lock);
 465
 466         /* Wait up kthread for capture */
 467         wake_up_interruptible(&chan->start_wait);
 468 }
 469
 470 /* Return all queued buffers back to videobuf2 */
 471 void tegra_channel_queued_buf_done(struct tegra_channel *chan,
 472                                           enum vb2_buffer_state state)
 473 {
 474         struct tegra_channel_buffer *buf, *nbuf;
 475         spinlock_t *lock = &chan->start_lock;
 476         struct list_head *q = &chan->capture;
 477         spinlock_t *release_lock = &chan->release_lock;
 478         struct list_head *rel_q = &chan->release;
 479
 480         spin_lock(lock);
 481         if(!list_empty(q)) {
 482                 list_for_each_entry_safe(buf, nbuf, q, queue) {
 483                         vb2_buffer_done(&buf->buf.vb2_buf, state);
 484                         list_del(&buf->queue);
 485                 }
 486         }
 487         spin_unlock(lock);
 488
 489         /* delete release list */
 490         spin_lock(release_lock);
 491         if(!list_empty(rel_q)) {
 492                 list_for_each_entry_safe(buf, nbuf, rel_q, queue) {
 493                         vb2_buffer_done(&buf->buf.vb2_buf, state);
 494                         list_del(&buf->queue);
 495                 }
 496         }
 497         spin_unlock(release_lock);
 498 }
 499
 500 #define __tegra_channel_device_call_subdevs_all_p(v4l2_dev, sd, cond, o,\
 501                 f, args...)                                             \
 502 ({                                                                      \
 503         long __err = 0;                                                 \
 504         long e = 0;                                                     \
 505                                                                         \
 506         list_for_each_entry((sd), &(v4l2_dev)->subdevs, list) {         \
 507                 if ((cond) && (sd)->ops->o && (sd)->ops->o->f)          \
 508                         e = (sd)->ops->o->f((sd), ##args);              \
 509                 if (!__err && e && e != -ENOIOCTLCMD)                   \
 510                         __err = e;                                      \
 511                 e = 0;                                                  \
 512         }                                                               \
 513         __err;                                                          \
 514 })
 515
 516 /*
 517  * Call the specified callback for all subdevs matching grp_id (if 0, then
 518  * match them all), errors are ignored until the end, and the first error
 519  * encountered is returned. If the callback returns an error other than 0 or
 520  * -ENOIOCTLCMD, then return with that error code. Note that you cannot
 521  * add or delete a subdev while walking the subdevs list.
 522  */
 523 #define tegra_channel_device_call_all(v4l2_dev, grpid, o, f, args...)   \
 524 ({                                                                      \
 525         struct v4l2_subdev *__sd;                                       \
 526         __tegra_channel_device_call_subdevs_all_p(v4l2_dev, __sd,       \
 527                         !(grpid) || __sd->grp_id == (grpid), o, f,      \
 528                         ##args);                                        \
 529 })
 530
 531 /*
 532  * -----------------------------------------------------------------------------
 533  * subdevice set/unset operations
 534  * -----------------------------------------------------------------------------
 535  */
 536 int tegra_channel_set_stream(struct tegra_channel *chan, bool on)
 537 {
 538         int num_sd;
 539         int ret = 0;
 540         int err = 0;
 541         struct v4l2_subdev *sd;
 542
 543         if (atomic_read(&chan->is_streaming) == on)
 544                 return 0;
 545
 546         if (on) {
 547                 /* Enable CSI before sensor. Reason is as follows:
 548                  * CSI is able to catch the very first clk transition.
 549                  * Ensure mipi calibration is done before transmission/first frame data.
 550                  * TODO:Ensure deskew is setup properly before first deskew sync signal.
 551                  */
 552                 for (num_sd = 0; num_sd < chan->num_subdevs; num_sd++) {
 553                         sd = chan->subdev[num_sd];
 554
 555                         err = v4l2_subdev_call(sd, video, s_stream, on);
 556                         if (!ret && err < 0 && err != -ENOIOCTLCMD)
 557                                 ret = err;
 558                 }
 559         } else {
 560                 for (num_sd = chan->num_subdevs - 1; num_sd >= 0; num_sd--) {
 561                         sd = chan->subdev[num_sd];
 562
 563                         err = v4l2_subdev_call(sd, video, s_stream, on);
 564                         if (!ret && err < 0 && err != -ENOIOCTLCMD)
 565                                 ret = err;
 566                 }
 567         }
 568
 569         atomic_set(&chan->is_streaming, on);
 570         return ret;
 571 }
 572
 573 int tegra_channel_set_power(struct tegra_channel *chan, bool on)
 574 {
 575         int num_sd;
 576         int ret = 0;
 577         int err = 0;
 578         struct v4l2_subdev *sd;
 579
 580         /* Power on CSI at the last to complete calibration of mipi lanes */
 581         for (num_sd = chan->num_subdevs - 1; num_sd >= 0; num_sd--) {
 582                 sd = chan->subdev[num_sd];
 583
 584                 err = v4l2_subdev_call(sd, core, s_power, on);
 585                 if (!ret && err < 0 && err != -ENOIOCTLCMD)
 586                         ret = err;
 587         }
 588
 589         return ret;
 590 }
 591
 592 static int tegra_channel_start_streaming(struct vb2_queue *vq, u32 count)
 593 {
 594         struct tegra_channel *chan = vb2_get_drv_priv(vq);
 595         struct tegra_mc_vi *vi = chan->vi;
 596
 597         if (vi->fops)
 598                 return vi->fops->vi_start_streaming(vq, count);
 599         return 0;
 600 }
 601
 602 static void tegra_channel_stop_streaming(struct vb2_queue *vq)
 603 {
 604         struct tegra_channel *chan = vb2_get_drv_priv(vq);
 605         struct tegra_mc_vi *vi = chan->vi;
 606
 607         if (vi->fops)
 608                 vi->fops->vi_stop_streaming(vq);
 609
 610         /* Clean-up recorded videobuf2 queue initial timestamp */
 611         queue_init_ts = 0;
 612 }
 613
 614 static const struct vb2_ops tegra_channel_queue_qops = {
 615         .queue_setup = tegra_channel_queue_setup,
 616         .buf_prepare = tegra_channel_buffer_prepare,
 617         .buf_queue = tegra_channel_buffer_queue,
 618         .wait_prepare = vb2_ops_wait_prepare,
 619         .wait_finish = vb2_ops_wait_finish,
 620         .start_streaming = tegra_channel_start_streaming,
 621         .stop_streaming = tegra_channel_stop_streaming,
 622 };
 623
 624 /* -----------------------------------------------------------------------------
 625  * V4L2 ioctls
 626  */
 627
 628 static int
 629 tegra_channel_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
 630 {
 631         struct v4l2_fh *vfh = file->private_data;
 632         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 633
 634         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 635         cap->device_caps |= V4L2_CAP_EXT_PIX_FORMAT;
 636         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 637
 638         strlcpy(cap->driver, "tegra-video", sizeof(cap->driver));
 639         strlcpy(cap->card, chan->video.name, sizeof(cap->card));
 640         snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s:%u",
 641                  dev_name(chan->vi->dev), chan->port[0]);
 642
 643         return 0;
 644 }
 645
 646 static int
 647 tegra_channel_enum_framesizes(struct file *file, void *fh,
 648                               struct v4l2_frmsizeenum *sizes)
 649 {
 650         struct v4l2_fh *vfh = file->private_data;
 651         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 652         struct v4l2_subdev *sd = chan->subdev_on_csi;
 653         struct v4l2_subdev_frame_size_enum fse = {
 654                 .index = sizes->index,
 655                 .code = sizes->pixel_format,
 656         };
 657         int ret = 0;
 658
 659         ret = v4l2_subdev_call(sd, pad, enum_frame_size, NULL, &fse);
 660
 661         if (!ret) {
 662                 sizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 663                 sizes->discrete.width = fse.max_width;
 664                 sizes->discrete.height = fse.max_height;
 665         }
 666
 667         return ret;
 668 }
 669
 670 static int
 671 tegra_channel_enum_frameintervals(struct file *file, void *fh,
 672                               struct v4l2_frmivalenum *intervals)
 673 {
 674         struct v4l2_fh *vfh = file->private_data;
 675         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 676         struct v4l2_subdev *sd = chan->subdev_on_csi;
 677         struct v4l2_subdev_frame_interval_enum fie = {
 678                 .index = intervals->index,
 679                 .code = intervals->pixel_format,
 680                 .width = intervals->width,
 681                 .height = intervals->height,
 682         };
 683         int ret = 0;
 684
 685         ret = v4l2_subdev_call(sd, pad, enum_frame_interval, NULL, &fie);
 686
 687         if (!ret) {
 688                 intervals->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 689                 intervals->discrete.numerator = fie.interval.numerator;
 690                 intervals->discrete.denominator = fie.interval.denominator;
 691         }
 692
 693         return ret;
 694 }
 695
 696 static int
 697 tegra_channel_enum_format(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 698 {
 699         struct v4l2_fh *vfh = file->private_data;
 700         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 701         unsigned int index = 0, i;
 702         unsigned long *fmts_bitmap = chan->fmts_bitmap;
 703
 704         if (f->index >= bitmap_weight(fmts_bitmap, MAX_FORMAT_NUM))
 705                 return -EINVAL;
 706
 707         for (i = 0; i < f->index + 1; i++, index++)
 708                 index = find_next_bit(fmts_bitmap, MAX_FORMAT_NUM, index);
 709
 710         index -= 1;
 711         f->pixelformat = tegra_core_get_fourcc_by_idx(chan, index);
 712         tegra_core_get_description_by_idx(chan, index, f->description);
 713
 714         return 0;
 715 }
 716
 717 static int
 718 tegra_channel_g_edid(struct file *file, void *fh, struct v4l2_edid *edid)
 719 {
 720         struct v4l2_fh *vfh = file->private_data;
 721         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 722         struct v4l2_subdev *sd = chan->subdev_on_csi;
 723
 724         if (!v4l2_subdev_has_op(sd, pad, get_edid))
 725                 return -ENOTTY;
 726
 727         return v4l2_subdev_call(sd, pad, get_edid, edid);
 728 }
 729
 730 static int
 731 tegra_channel_s_edid(struct file *file, void *fh, struct v4l2_edid *edid)
 732 {
 733         struct v4l2_fh *vfh = file->private_data;
 734         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 735         struct v4l2_subdev *sd = chan->subdev_on_csi;
 736
 737         if (!v4l2_subdev_has_op(sd, pad, set_edid))
 738                 return -ENOTTY;
 739
 740         return v4l2_subdev_call(sd, pad, set_edid, edid);
 741 }
 742
 743 static int
 744 tegra_channel_g_dv_timings(struct file *file, void *fh,
 745                 struct v4l2_dv_timings *timings)
 746 {
 747         struct v4l2_fh *vfh = file->private_data;
 748         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 749
 750         if (!v4l2_subdev_has_op(chan->subdev_on_csi, video, g_dv_timings))
 751                 return -ENOTTY;
 752
 753         return v4l2_device_call_until_err(chan->video.v4l2_dev,
 754                         chan->grp_id, video, g_dv_timings, timings);
 755 }
 756
 757 static int
 758 tegra_channel_s_dv_timings(struct file *file, void *fh,
 759                 struct v4l2_dv_timings *timings)
 760 {
 761         struct v4l2_fh *vfh = file->private_data;
 762         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 763         struct v4l2_bt_timings *bt = &timings->bt;
 764         struct v4l2_dv_timings curr_timings;
 765         int ret;
 766
 767         if (!v4l2_subdev_has_op(chan->subdev_on_csi, video, s_dv_timings))
 768                 return -ENOTTY;
 769
 770         ret = tegra_channel_g_dv_timings(file, fh, &curr_timings);
 771         if (ret)
 772                 return ret;
 773
 774         if (v4l2_match_dv_timings(timings, &curr_timings, 0))
 775                 return 0;
 776
 777         if (vb2_is_busy(&chan->queue))
 778                 return -EBUSY;
 779
 780         ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
 781                         chan->grp_id, video, s_dv_timings, timings);
 782
 783         if (!ret)
 784                 tegra_channel_update_format(chan, bt->width, bt->height,
 785                         chan->fmtinfo->fourcc, &chan->fmtinfo->bpp, 0);
 786
 787         if (chan->total_ports > 1)
 788                 update_gang_mode(chan);
 789
 790         return ret;
 791 }
 792
 793 static int
 794 tegra_channel_query_dv_timings(struct file *file, void *fh,
 795                 struct v4l2_dv_timings *timings)
 796 {
 797         struct v4l2_fh *vfh = file->private_data;
 798         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 799
 800         if (!v4l2_subdev_has_op(chan->subdev_on_csi, video, query_dv_timings))
 801                 return -ENOTTY;
 802
 803         return v4l2_device_call_until_err(chan->video.v4l2_dev,
 804                         chan->grp_id, video, query_dv_timings, timings);
 805 }
 806
 807 static int
 808 tegra_channel_enum_dv_timings(struct file *file, void *fh,
 809                 struct v4l2_enum_dv_timings *timings)
 810 {
 811         struct v4l2_fh *vfh = file->private_data;
 812         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 813         struct v4l2_subdev *sd = chan->subdev_on_csi;
 814
 815         if (!v4l2_subdev_has_op(sd, pad, enum_dv_timings))
 816                 return -ENOTTY;
 817
 818         return v4l2_subdev_call(sd, pad, enum_dv_timings, timings);
 819 }
 820
 821 static int
 822 tegra_channel_dv_timings_cap(struct file *file, void *fh,
 823                 struct v4l2_dv_timings_cap *cap)
 824 {
 825         struct v4l2_fh *vfh = file->private_data;
 826         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
 827         struct v4l2_subdev *sd = chan->subdev_on_csi;
 828
 829         if (!v4l2_subdev_has_op(sd, pad, dv_timings_cap))
 830                 return -ENOTTY;
 831
 832         return v4l2_subdev_call(sd, pad, dv_timings_cap, cap);
 833 }
 834
 835 int tegra_channel_s_ctrl(struct v4l2_ctrl *ctrl)
 836 {
 837         struct tegra_channel *chan = container_of(ctrl->handler,
 838                                 struct tegra_channel, ctrl_handler);
 839
 840         switch (ctrl->id) {
 841         case TEGRA_CAMERA_CID_VI_BYPASS_MODE:
 842                 if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON)
 843                         chan->bypass = true;
 844                 else if (chan->vi->bypass) {
 845                         dev_dbg(&chan->video.dev,
 846                                 "can't disable bypass mode\n");
 847                         dev_dbg(&chan->video.dev,
 848                                 "because the VI/CSI is in bypass mode\n");
 849                         chan->bypass = true;
 850                 } else
 851                         chan->bypass = false;
 852                 break;
 853         case TEGRA_CAMERA_CID_OVERRIDE_ENABLE:
 854                 {
 855                         struct v4l2_subdev *sd = chan->subdev_on_csi;
 856                         struct camera_common_data *s_data =
 857                                 to_camera_common_data(sd->dev);
 858
 859                         if (!s_data)
 860                                 break;
 861                         if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON) {
 862                                 s_data->override_enable = true;
 863                                 dev_dbg(&chan->video.dev,
 864                                         "enable override control\n");
 865                         } else {
 866                                 s_data->override_enable = false;
 867                                 dev_dbg(&chan->video.dev,
 868                                         "disable override control\n");
 869                         }
 870                 }
 871                 break;
 872         case TEGRA_CAMERA_CID_VI_HEIGHT_ALIGN:
 873                 chan->height_align = ctrl->val;
 874                 tegra_channel_update_format(chan, chan->format.width,
 875                                 chan->format.height,
 876                                 chan->format.pixelformat,
 877                                 &chan->fmtinfo->bpp, 0);
 878                 break;
 879         case TEGRA_CAMERA_CID_VI_SIZE_ALIGN:
 880                 chan->size_align = size_align_ctrl_qmenu[ctrl->val];
 881                 tegra_channel_update_format(chan, chan->format.width,
 882                                 chan->format.height,
 883                                 chan->format.pixelformat,
 884                                 &chan->fmtinfo->bpp, 0);
 885                 break;
 886         case TEGRA_CAMERA_CID_WRITE_ISPFORMAT:
 887                 chan->write_ispformat = ctrl->val;
 888                 break;
 889         default:
 890                 dev_err(&chan->video.dev, "%s: Invalid ctrl %u\n",
 891                         __func__, ctrl->id);
 892                 return -EINVAL;
 893         }
 894
 895         return 0;
 896 }
 897
 898 static const struct v4l2_ctrl_ops channel_ctrl_ops = {
 899         .s_ctrl = tegra_channel_s_ctrl,
 900 };
 901
 902 static const struct v4l2_ctrl_config common_custom_ctrls[] = {
 903         {
 904                 .ops = &channel_ctrl_ops,
 905                 .id = TEGRA_CAMERA_CID_VI_BYPASS_MODE,
 906                 .name = "Bypass Mode",
 907                 .type = V4L2_CTRL_TYPE_INTEGER_MENU,
 908                 .def = 0,
 909                 .min = 0,
 910                 .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
 911                 .menu_skip_mask = 0,
 912                 .qmenu_int = switch_ctrl_qmenu,
 913         },
 914         {
 915                 .ops = &channel_ctrl_ops,
 916                 .id = TEGRA_CAMERA_CID_OVERRIDE_ENABLE,
 917                 .name = "Override Enable",
 918                 .type = V4L2_CTRL_TYPE_INTEGER_MENU,
 919                 .def = 0,
 920                 .min = 0,
 921                 .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
 922                 .menu_skip_mask = 0,
 923                 .qmenu_int = switch_ctrl_qmenu,
 924         },
 925         {
 926                 .ops = &channel_ctrl_ops,
 927                 .id = TEGRA_CAMERA_CID_VI_HEIGHT_ALIGN,
 928                 .name = "Height Align",
 929                 .type = V4L2_CTRL_TYPE_INTEGER,
 930                 .min = 1,
 931                 .max = 16,
 932                 .step = 1,
 933                 .def = 1,
 934         },
 935         {
 936                 .ops = &channel_ctrl_ops,
 937                 .id = TEGRA_CAMERA_CID_VI_SIZE_ALIGN,
 938                 .name = "Size Align",
 939                 .type = V4L2_CTRL_TYPE_INTEGER_MENU,
 940                 .def = 0,
 941                 .min = 0,
 942                 .max = ARRAY_SIZE(size_align_ctrl_qmenu) - 1,
 943                 .menu_skip_mask = 0,
 944                 .qmenu_int = size_align_ctrl_qmenu,
 945         },
 946         {
 947                 .ops = &channel_ctrl_ops,
 948                 .id = TEGRA_CAMERA_CID_SENSOR_MODES,
 949                 .name = "Sensor Modes",
 950                 .type = V4L2_CTRL_TYPE_INTEGER,
 951                 .flags = V4L2_CTRL_FLAG_READ_ONLY,
 952                 .min = 0,
 953                 .max = MAX_NUM_SENSOR_MODES,
 954                 .def = MAX_NUM_SENSOR_MODES,
 955                 .step = 1,
 956         },
 957         {
 958                 .ops = &channel_ctrl_ops,
 959                 .id = TEGRA_CAMERA_CID_SENSOR_SIGNAL_PROPERTIES,
 960                 .name = "Sensor Signal Properties",
 961                 .type = V4L2_CTRL_TYPE_U32,
 962                 .flags = V4L2_CTRL_FLAG_HAS_PAYLOAD |
 963                          V4L2_CTRL_FLAG_READ_ONLY,
 964                 .min = 0,
 965                 .max = 0xFFFFFFFF,
 966                 .step = 1,
 967                 .def = 0,
 968                 .dims = { MAX_NUM_SENSOR_MODES,
 969                           SENSOR_SIGNAL_PROPERTIES_CID_SIZE },
 970         },
 971         {
 972                 .ops = &channel_ctrl_ops,
 973                 .id = TEGRA_CAMERA_CID_SENSOR_IMAGE_PROPERTIES,
 974                 .name = "Sensor Image Properties",
 975                 .type = V4L2_CTRL_TYPE_U32,
 976                 .flags = V4L2_CTRL_FLAG_HAS_PAYLOAD |
 977                          V4L2_CTRL_FLAG_READ_ONLY,
 978                 .min = 0,
 979                 .max = 0xFFFFFFFF,
 980                 .step = 1,
 981                 .def = 0,
 982                 .dims = { MAX_NUM_SENSOR_MODES,
 983                           SENSOR_IMAGE_PROPERTIES_CID_SIZE },
 984         },
 985         {
 986                 .ops = &channel_ctrl_ops,
 987                 .id = TEGRA_CAMERA_CID_SENSOR_CONTROL_PROPERTIES,
 988                 .name = "Sensor Control Properties",
 989                 .type = V4L2_CTRL_TYPE_U32,
 990                 .flags = V4L2_CTRL_FLAG_HAS_PAYLOAD |
 991                          V4L2_CTRL_FLAG_READ_ONLY,
 992                 .min = 0,
 993                 .max = 0xFFFFFFFF,
 994                 .step = 1,
 995                 .def = 0,
 996                 .dims = { MAX_NUM_SENSOR_MODES,
 997                           SENSOR_CONTROL_PROPERTIES_CID_SIZE },
 998         },
 999         {
1000                 .ops = &channel_ctrl_ops,
1001                 .id = TEGRA_CAMERA_CID_SENSOR_DV_TIMINGS,
1002                 .name = "Sensor DV Timings",
1003                 .type = V4L2_CTRL_TYPE_U32,
1004                 .flags = V4L2_CTRL_FLAG_HAS_PAYLOAD |
1005                          V4L2_CTRL_FLAG_READ_ONLY,
1006                 .min = 0,
1007                 .max = 0xFFFFFFFF,
1008                 .step = 1,
1009                 .def = 0,
1010                 .dims = { MAX_NUM_SENSOR_MODES,
1011                           SENSOR_DV_TIMINGS_CID_SIZE },
1012         },
1013 };
1014
1015 #define GET_TEGRA_CAMERA_CTRL(id, c)                                    \
1016 do {                                                                    \
1017         c = v4l2_ctrl_find(&chan->ctrl_handler, TEGRA_CAMERA_CID_##id); \
1018         if (!c) {                                                       \
1019                 dev_err(chan->vi->dev, "%s: could not find ctrl %s\n",  \
1020                         __func__, "##id");                              \
1021                 return -EINVAL;                                         \
1022         }                                                               \
1023 } while (0)
1024
1025 static int tegra_channel_sensorprops_setup(struct tegra_channel *chan)
1026 {
1027         const struct v4l2_subdev *sd = chan->subdev_on_csi;
1028         const struct camera_common_data *s_data =
1029                         to_camera_common_data(sd->dev);
1030         const struct sensor_mode_properties *modes;
1031         struct v4l2_ctrl *ctrl_modes;
1032         struct v4l2_ctrl *ctrl_signalprops;
1033         struct v4l2_ctrl *ctrl_imageprops;
1034         struct v4l2_ctrl *ctrl_controlprops;
1035         struct v4l2_ctrl *ctrl_dvtimings;
1036         u32 i;
1037
1038         GET_TEGRA_CAMERA_CTRL(SENSOR_MODES, ctrl_modes);
1039         GET_TEGRA_CAMERA_CTRL(SENSOR_SIGNAL_PROPERTIES, ctrl_signalprops);
1040         GET_TEGRA_CAMERA_CTRL(SENSOR_IMAGE_PROPERTIES, ctrl_imageprops);
1041         GET_TEGRA_CAMERA_CTRL(SENSOR_CONTROL_PROPERTIES, ctrl_controlprops);
1042         GET_TEGRA_CAMERA_CTRL(SENSOR_DV_TIMINGS, ctrl_dvtimings);
1043
1044         ctrl_modes->val = s_data->sensor_props.num_modes;
1045         ctrl_modes->cur.val = s_data->sensor_props.num_modes;
1046
1047         modes = s_data->sensor_props.sensor_modes;
1048         for (i = 0; i < s_data->sensor_props.num_modes; i++) {
1049                 void *ptr = NULL;
1050                 u32 size;
1051
1052                 size = sizeof(struct sensor_signal_properties);
1053                 ptr = ctrl_signalprops->p_new.p + (i * size);
1054                 memcpy(ptr, &modes[i].signal_properties, size);
1055
1056                 size = sizeof(struct sensor_image_properties);
1057                 ptr = ctrl_imageprops->p_new.p + (i * size);
1058                 memcpy(ptr, &modes[i].image_properties, size);
1059
1060                 size = sizeof(struct sensor_control_properties);
1061                 ptr = ctrl_controlprops->p_new.p + (i * size);
1062                 memcpy(ptr, &modes[i].control_properties, size);
1063
1064                 size = sizeof(struct sensor_dv_timings);
1065                 ptr = ctrl_dvtimings->p_new.p + (i * size);
1066                 memcpy(ptr, &modes[i].dv_timings, size);
1067         }
1068         ctrl_signalprops->p_cur.p = ctrl_signalprops->p_new.p;
1069         ctrl_imageprops->p_cur.p = ctrl_imageprops->p_new.p;
1070         ctrl_controlprops->p_cur.p = ctrl_controlprops->p_new.p;
1071         ctrl_dvtimings->p_cur.p = ctrl_dvtimings->p_new.p;
1072
1073         return 0;
1074 }
1075
1076 static int tegra_channel_setup_controls(struct tegra_channel *chan)
1077 {
1078         int num_sd = 0;
1079         struct v4l2_subdev *sd = NULL;
1080         struct tegra_mc_vi *vi = chan->vi;
1081         int i;
1082         int ret = 0;
1083
1084         /* Initialize the subdev and controls here at first open */
1085         sd = chan->subdev[num_sd];
1086         while ((sd = chan->subdev[num_sd++]) &&
1087                 (num_sd <= chan->num_subdevs)) {
1088                 /* Add control handler for the subdevice */
1089                 ret = v4l2_ctrl_add_handler(&chan->ctrl_handler,
1090                                         sd->ctrl_handler, NULL);
1091                 if (ret || chan->ctrl_handler.error)
1092                         dev_err(chan->vi->dev,
1093                                 "Failed to add sub-device controls\n");
1094         }
1095
1096         /* Add new custom controls */
1097         for (i = 0; i < ARRAY_SIZE(common_custom_ctrls); i++) {
1098                 /* don't create override control for pg mode and hdmiin */
1099                 if (common_custom_ctrls[i].id ==
1100                         TEGRA_CAMERA_CID_OVERRIDE_ENABLE &&
1101                         (chan->pg_mode || chan->hdmiin))
1102                         continue;
1103                 v4l2_ctrl_new_custom(&chan->ctrl_handler,
1104                         &common_custom_ctrls[i], NULL);
1105                 if (chan->ctrl_handler.error) {
1106                         dev_err(chan->vi->dev,
1107                                 "Failed to add %s ctrl\n",
1108                                 common_custom_ctrls[i].name);
1109                         return chan->ctrl_handler.error;
1110                 }
1111         }
1112
1113         vi->fops->vi_add_ctrls(chan);
1114
1115         if (chan->pg_mode) {
1116                 ret = v4l2_ctrl_add_handler(&chan->ctrl_handler,
1117                                         &chan->vi->ctrl_handler, NULL);
1118                 if (ret || chan->ctrl_handler.error)
1119                         dev_err(chan->vi->dev,
1120                                 "Failed to add VI controls\n");
1121         }
1122
1123         /* setup the controls */
1124         ret = v4l2_ctrl_handler_setup(&chan->ctrl_handler);
1125         if (ret < 0)
1126                 goto error;
1127
1128         return 0;
1129
1130 error:
1131         v4l2_ctrl_handler_free(&chan->ctrl_handler);
1132         return ret;
1133 }
1134
1135 static void tegra_channel_free_sensor_properties(
1136                 const struct v4l2_subdev *sensor_sd)
1137 {
1138         struct device *sensor_dev = sensor_sd->dev;
1139         struct camera_common_data *s_data = to_camera_common_data(sensor_dev);
1140
1141         if (sensor_dev == NULL || s_data == NULL)
1142                 return;
1143
1144         if (s_data->sensor_props.sensor_modes)
1145                 devm_kfree(sensor_dev, s_data->sensor_props.sensor_modes);
1146
1147         s_data->sensor_props.sensor_modes = NULL;
1148 }
1149
1150 static int tegra_channel_connect_sensor(
1151         struct tegra_channel *chan, struct v4l2_subdev *sensor_sd)
1152 {
1153         struct device *sensor_dev;
1154         struct device_node *sensor_of_node;
1155         struct tegra_csi_device *csi_device;
1156         struct device_node *ep_node;
1157
1158         if (!chan)
1159                 return -EINVAL;
1160
1161         if (!sensor_sd)
1162                 return -EINVAL;
1163
1164         sensor_dev = sensor_sd->dev;
1165         if (!sensor_dev)
1166                 return -EINVAL;
1167
1168         sensor_of_node = sensor_dev->of_node;
1169         if (!sensor_of_node)
1170                 return -EINVAL;
1171
1172         csi_device = tegra_get_mc_csi();
1173         WARN_ON(!csi_device);
1174         if (!csi_device)
1175                 return -ENODEV;
1176
1177         for_each_endpoint_of_node(sensor_of_node, ep_node) {
1178                 struct device_node *csi_chan_of_node;
1179                 struct tegra_csi_channel *csi_chan;
1180
1181                 csi_chan_of_node =
1182                         of_graph_get_remote_port_parent(ep_node);
1183
1184                 list_for_each_entry(csi_chan, &csi_device->csi_chans, list)
1185                         if (csi_chan->of_node == csi_chan_of_node)
1186                                 break;
1187
1188                 of_node_put(csi_chan_of_node);
1189
1190                 if (!csi_chan)
1191                         continue;
1192
1193                 csi_chan->s_data =
1194                         to_camera_common_data(chan->subdev_on_csi->dev);
1195                 csi_chan->sensor_sd = chan->subdev_on_csi;
1196         }
1197
1198         return 0;
1199 }
1200
1201 int tegra_channel_init_subdevices(struct tegra_channel *chan)
1202 {
1203         int ret = 0;
1204         struct media_entity *entity;
1205         struct media_pad *pad;
1206         struct v4l2_subdev *sd;
1207         int index = 0;
1208         int num_sd = 0;
1209         int grp_id = chan->pg_mode ? (TPG_CSI_GROUP_ID + chan->port[0] + 1)
1210                 : chan->port[0] + 1;
1211
1212         /* set_stream of CSI */
1213         pad = media_entity_remote_pad(&chan->pad);
1214         if (!pad)
1215                 return -ENODEV;
1216
1217         entity = pad->entity;
1218         sd = media_entity_to_v4l2_subdev(entity);
1219         v4l2_set_subdev_hostdata(sd, chan);
1220         chan->subdev[num_sd++] = sd;
1221         /* Add subdev name to this video dev name with vi-output tag*/
1222         snprintf(chan->video.name, sizeof(chan->video.name), "%s, %s",
1223                 "vi-output", sd->name);
1224         sd->grp_id = grp_id;
1225         chan->grp_id = grp_id;
1226         index = pad->index - 1;
1227         while (index >= 0) {
1228                 pad = &entity->pads[index];
1229                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1230                         break;
1231
1232                 pad = media_entity_remote_pad(pad);
1233                 if (pad == NULL ||
1234                     media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
1235                         break;
1236
1237                 if (num_sd >= MAX_SUBDEVICES)
1238                         break;
1239
1240                 entity = pad->entity;
1241                 sd = media_entity_to_v4l2_subdev(entity);
1242                 v4l2_set_subdev_hostdata(sd, chan);
1243                 sd->grp_id = grp_id;
1244                 chan->subdev[num_sd++] = sd;
1245                 /* Add subdev name to this video dev name with vi-output tag*/
1246                 snprintf(chan->video.name, sizeof(chan->video.name), "%s, %s",
1247                         "vi-output", sd->name);
1248
1249                 index = pad->index - 1;
1250         }
1251         chan->num_subdevs = num_sd;
1252         /*
1253          * Each CSI channel has only one final remote source,
1254          * Mark that subdev as subdev_on_csi
1255          */
1256         chan->subdev_on_csi = sd;
1257
1258         /* initialize the available formats */
1259         if (chan->num_subdevs)
1260                 tegra_channel_fmts_bitmap_init(chan);
1261
1262         chan->hdmiin = v4l2_subdev_has_op(chan->subdev_on_csi,
1263                                 video, s_dv_timings);
1264
1265         ret = tegra_channel_setup_controls(chan);
1266         if (ret < 0) {
1267                 dev_err(chan->vi->dev, "%s: failed to setup controls\n",
1268                         __func__);
1269                 goto fail;
1270         }
1271
1272         /*
1273          * If subdev on csi is csi or channel is in pg mode
1274          * then don't look for sensor props
1275          */
1276         if (strstr(chan->subdev_on_csi->name, "nvcsi") != NULL ||
1277                         chan->pg_mode)
1278                 return 0;
1279
1280         ret = tegra_channel_sensorprops_setup(chan);
1281         if (ret < 0) {
1282                 dev_err(chan->vi->dev, "%s: failed to setup sensor props\n",
1283                         __func__);
1284                 goto fail;
1285         }
1286
1287         /* Add a link for the camera_common_data in the tegra_csi_channel. */
1288         ret = tegra_channel_connect_sensor(chan, chan->subdev_on_csi);
1289         if (ret < 0) {
1290                 dev_err(chan->vi->dev,
1291                         "%s: failed to connect sensor to channel\n", __func__);
1292                 goto fail;
1293         }
1294
1295         return 0;
1296 fail:
1297         tegra_channel_free_sensor_properties(chan->subdev_on_csi);
1298         return ret;
1299 }
1300
1301 static int
1302 tegra_channel_get_format(struct file *file, void *fh,
1303                         struct v4l2_format *format)
1304 {
1305         struct v4l2_fh *vfh = file->private_data;
1306         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1307         struct v4l2_pix_format *pix = &format->fmt.pix;
1308
1309         *pix = chan->format;
1310
1311         return 0;
1312 }
1313
1314 static int
1315 __tegra_channel_try_format(struct tegra_channel *chan,
1316                         struct v4l2_pix_format *pix)
1317 {
1318         const struct tegra_video_format *vfmt;
1319         struct v4l2_subdev_format fmt;
1320         struct v4l2_subdev *sd = chan->subdev_on_csi;
1321         int ret = 0;
1322
1323         /* Use the channel format if pixformat is not supported */
1324         vfmt = tegra_core_get_format_by_fourcc(chan, pix->pixelformat);
1325         if (!vfmt) {
1326                 pix->pixelformat = chan->format.pixelformat;
1327                 vfmt = tegra_core_get_format_by_fourcc(chan, pix->pixelformat);
1328         }
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         tegra_channel_fmt_align(chan, vfmt,
1341                                 &pix->width, &pix->height, &pix->bytesperline);
1342         pix->sizeimage = get_aligned_buffer_size(chan,
1343                         pix->bytesperline, pix->height);
1344         if (chan->fmtinfo->fourcc == V4L2_PIX_FMT_NV16)
1345                 pix->sizeimage *= 2;
1346
1347         return ret;
1348 }
1349
1350 static int
1351 tegra_channel_try_format(struct file *file, void *fh,
1352                         struct v4l2_format *format)
1353 {
1354         struct v4l2_fh *vfh = file->private_data;
1355         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1356
1357         return  __tegra_channel_try_format(chan, &format->fmt.pix);
1358 }
1359
1360 static int
1361 __tegra_channel_set_format(struct tegra_channel *chan,
1362                         struct v4l2_pix_format *pix)
1363 {
1364         const struct tegra_video_format *vfmt;
1365         struct v4l2_subdev_format fmt;
1366         struct v4l2_subdev *sd = chan->subdev_on_csi;
1367         int ret = 0;
1368
1369         vfmt = tegra_core_get_format_by_fourcc(chan, pix->pixelformat);
1370
1371         fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1372         fmt.pad = 0;
1373         v4l2_fill_mbus_format(&fmt.format, pix, vfmt->code);
1374
1375         ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
1376         if (ret == -ENOIOCTLCMD)
1377                 return -ENOTTY;
1378
1379         v4l2_fill_pix_format(pix, &fmt.format);
1380
1381         if (!ret) {
1382                 chan->format = *pix;
1383                 chan->fmtinfo = vfmt;
1384                 tegra_channel_update_format(chan, pix->width,
1385                         pix->height, vfmt->fourcc, &vfmt->bpp,
1386                         pix->bytesperline);
1387
1388                 *pix = chan->format;
1389
1390                 if (chan->total_ports > 1)
1391                         update_gang_mode(chan);
1392         }
1393
1394         return ret;
1395 }
1396
1397 static int
1398 tegra_channel_set_format(struct file *file, void *fh,
1399                         struct v4l2_format *format)
1400 {
1401         struct v4l2_fh *vfh = file->private_data;
1402         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1403         int ret = 0;
1404
1405         /* get the suppod format by try_fmt */
1406         ret = __tegra_channel_try_format(chan, &format->fmt.pix);
1407         if (ret)
1408                 return ret;
1409
1410         if (vb2_is_busy(&chan->queue))
1411                 return -EBUSY;
1412
1413         return __tegra_channel_set_format(chan, &format->fmt.pix);
1414 }
1415
1416 static int tegra_channel_subscribe_event(struct v4l2_fh *fh,
1417                                   const struct v4l2_event_subscription *sub)
1418 {
1419         switch (sub->type) {
1420         case V4L2_EVENT_SOURCE_CHANGE:
1421                 return v4l2_event_subscribe(fh, sub, 4, NULL);
1422         }
1423         return v4l2_ctrl_subscribe_event(fh, sub);
1424 }
1425
1426 static int
1427 tegra_channel_enum_input(struct file *file, void *fh, struct v4l2_input *inp)
1428 {
1429         struct v4l2_fh *vfh = file->private_data;
1430         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1431         struct v4l2_subdev *sd_on_csi = chan->subdev_on_csi;
1432         int ret;
1433
1434         if (inp->index)
1435                 return -EINVAL;
1436
1437         ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
1438                         chan->grp_id, video, g_input_status, &inp->status);
1439
1440         if (ret == -ENODEV || sd_on_csi == NULL)
1441                 return -ENODEV;
1442
1443         inp->type = V4L2_INPUT_TYPE_CAMERA;
1444         if (v4l2_subdev_has_op(sd_on_csi, video, s_dv_timings)) {
1445                 inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
1446                 snprintf(inp->name,
1447                         sizeof(inp->name), "HDMI %u",
1448                         chan->port[0]);
1449         } else
1450                 snprintf(inp->name,
1451                         sizeof(inp->name), "Camera %u",
1452                         chan->port[0]);
1453
1454         return ret;
1455 }
1456
1457 static int tegra_channel_g_input(struct file *file, void *priv, unsigned int *i)
1458 {
1459         *i = 0;
1460         return 0;
1461 }
1462
1463 static int tegra_channel_s_input(struct file *file, void *priv, unsigned int i)
1464 {
1465         if (i > 0)
1466                 return -EINVAL;
1467         return 0;
1468 }
1469
1470 static int tegra_channel_log_status(struct file *file, void *priv)
1471 {
1472         struct v4l2_fh *vfh = file->private_data;
1473         struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
1474
1475         v4l2_device_call_all(chan->video.v4l2_dev,
1476                 chan->grp_id, core, log_status);
1477         return 0;
1478 }
1479 static const struct v4l2_ioctl_ops tegra_channel_ioctl_ops = {
1480         .vidioc_querycap                = tegra_channel_querycap,
1481         .vidioc_enum_framesizes         = tegra_channel_enum_framesizes,
1482         .vidioc_enum_frameintervals     = tegra_channel_enum_frameintervals,
1483         .vidioc_enum_fmt_vid_cap        = tegra_channel_enum_format,
1484         .vidioc_g_fmt_vid_cap           = tegra_channel_get_format,
1485         .vidioc_s_fmt_vid_cap           = tegra_channel_set_format,
1486         .vidioc_try_fmt_vid_cap         = tegra_channel_try_format,
1487         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1488         .vidioc_querybuf                = vb2_ioctl_querybuf,
1489         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1490         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1491         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1492         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1493         .vidioc_streamon                = vb2_ioctl_streamon,
1494         .vidioc_streamoff               = vb2_ioctl_streamoff,
1495         .vidioc_g_edid                  = tegra_channel_g_edid,
1496         .vidioc_s_edid                  = tegra_channel_s_edid,
1497         .vidioc_s_dv_timings            = tegra_channel_s_dv_timings,
1498         .vidioc_g_dv_timings            = tegra_channel_g_dv_timings,
1499         .vidioc_query_dv_timings        = tegra_channel_query_dv_timings,
1500         .vidioc_enum_dv_timings         = tegra_channel_enum_dv_timings,
1501         .vidioc_dv_timings_cap          = tegra_channel_dv_timings_cap,
1502         .vidioc_subscribe_event         = tegra_channel_subscribe_event,
1503         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1504         .vidioc_enum_input              = tegra_channel_enum_input,
1505         .vidioc_g_input                 = tegra_channel_g_input,
1506         .vidioc_s_input                 = tegra_channel_s_input,
1507         .vidioc_log_status              = tegra_channel_log_status,
1508 };
1509
1510 static int tegra_channel_close(struct file *fp);
1511 static int tegra_channel_open(struct file *fp)
1512 {
1513         int ret;
1514         struct video_device *vdev = video_devdata(fp);
1515         struct tegra_channel *chan = video_get_drvdata(vdev);
1516         struct tegra_mc_vi *vi;
1517         struct tegra_csi_device *csi;
1518
1519         mutex_lock(&chan->video_lock);
1520         ret = v4l2_fh_open(fp);
1521         if (ret || !v4l2_fh_is_singular_file(fp)) {
1522                 mutex_unlock(&chan->video_lock);
1523                 return ret;
1524         }
1525
1526         if (chan->subdev[0] == NULL) {
1527                 ret = -ENODEV;
1528                 goto fail;
1529         }
1530
1531         vi = chan->vi;
1532         csi = vi->csi;
1533
1534         /* The first open then turn on power */
1535         if (vi->fops) {
1536                 ret = vi->fops->vi_power_on(chan);
1537                 if (ret < 0)
1538                         goto fail;
1539         }
1540
1541         chan->fh = (struct v4l2_fh *)fp->private_data;
1542
1543         mutex_unlock(&chan->video_lock);
1544         return 0;
1545
1546 fail:
1547         _vb2_fop_release(fp, NULL);
1548         mutex_unlock(&chan->video_lock);
1549         return ret;
1550 }
1551
1552 static int tegra_channel_close(struct file *fp)
1553 {
1554         int ret = 0;
1555         struct video_device *vdev = video_devdata(fp);
1556         struct tegra_channel *chan = video_get_drvdata(vdev);
1557         struct tegra_mc_vi *vi = chan->vi;
1558         bool is_singular;
1559
1560         mutex_lock(&chan->video_lock);
1561         is_singular = v4l2_fh_is_singular_file(fp);
1562         ret = _vb2_fop_release(fp, NULL);
1563
1564         if (!is_singular) {
1565                 mutex_unlock(&chan->video_lock);
1566                 return ret;
1567         }
1568         vi->fops->vi_power_off(chan);
1569
1570         mutex_unlock(&chan->video_lock);
1571         return ret;
1572 }
1573
1574 /* -----------------------------------------------------------------------------
1575  * V4L2 file operations
1576  */
1577 static const struct v4l2_file_operations tegra_channel_fops = {
1578         .owner          = THIS_MODULE,
1579         .unlocked_ioctl = video_ioctl2,
1580         .open           = tegra_channel_open,
1581         .release        = tegra_channel_close,
1582         .read           = vb2_fop_read,
1583         .poll           = vb2_fop_poll,
1584         .mmap           = vb2_fop_mmap,
1585 };
1586
1587 static int tegra_channel_csi_init(struct tegra_channel *chan)
1588 {
1589         int idx = 0;
1590         struct tegra_mc_vi *vi = chan->vi;
1591         int ret = 0;
1592
1593         chan->gang_mode = CAMERA_NO_GANG_MODE;
1594         chan->total_ports = 0;
1595         memset(&chan->port[0], INVALID_CSI_PORT, TEGRA_CSI_BLOCKS);
1596         memset(&chan->syncpoint_fifo[0], 0, TEGRA_CSI_BLOCKS);
1597         if (chan->pg_mode) {
1598                 /* If VI has 4 existing channels, chan->id will start
1599                  * from 4 for the first TPG channel, which uses PORT_A(0).
1600                  * To get the correct PORT number, subtract existing number of
1601                  * channels from chan->id.
1602                  */
1603                 chan->port[0] = chan->id - vi->num_channels;
1604                 WARN_ON(chan->port[0] > TPG_CHANNELS);
1605                 chan->numlanes = 2;
1606         } else {
1607                 ret = tegra_vi_get_port_info(chan, vi->dev->of_node, chan->id);
1608                 if (ret) {
1609                         dev_err(vi->dev, "%s:Fail to parse port info\n",
1610                                         __func__);
1611                         return ret;
1612                 }
1613         }
1614
1615         for (idx = 0; csi_port_is_valid(chan->port[idx]); idx++) {
1616                 chan->total_ports++;
1617                 /* maximum of 4 lanes are present per CSI block */
1618                 chan->csibase[idx] = vi->iomem +
1619                                         TEGRA_VI_CSI_BASE(chan->port[idx]);
1620         }
1621         /* based on gang mode valid ports will be updated - set default to 1 */
1622         chan->valid_ports = chan->total_ports ? 1 : 0;
1623         return ret;
1624 }
1625
1626 int tegra_channel_init(struct tegra_channel *chan)
1627 {
1628         int ret;
1629         struct tegra_mc_vi *vi = chan->vi;
1630
1631         ret = tegra_channel_csi_init(chan);
1632         if (ret)
1633                 return ret;
1634
1635         atomic_set(&chan->restart_version, 1);
1636         chan->capture_version = 0;
1637         chan->width_align = TEGRA_WIDTH_ALIGNMENT;
1638         chan->stride_align = TEGRA_STRIDE_ALIGNMENT;
1639         chan->num_subdevs = 0;
1640         mutex_init(&chan->video_lock);
1641         INIT_LIST_HEAD(&chan->capture);
1642         INIT_LIST_HEAD(&chan->entities);
1643         init_waitqueue_head(&chan->start_wait);
1644         spin_lock_init(&chan->start_lock);
1645         INIT_LIST_HEAD(&chan->release);
1646         init_waitqueue_head(&chan->release_wait);
1647         spin_lock_init(&chan->release_lock);
1648         mutex_init(&chan->stop_kthread_lock);
1649         atomic_set(&chan->is_streaming, DISABLE);
1650         spin_lock_init(&chan->capture_state_lock);
1651
1652         /* Init video format */
1653         vi->fops->vi_init_video_formats(chan);
1654         chan->fmtinfo = tegra_core_get_default_format();
1655         tegra_channel_update_format(chan, TEGRA_DEF_WIDTH,
1656                                 TEGRA_DEF_HEIGHT,
1657                                 chan->fmtinfo->fourcc,
1658                                 &chan->fmtinfo->bpp, 0);
1659
1660         chan->buffer_offset[0] = 0;
1661
1662         /* Initialize the media entity... */
1663         chan->pad.flags = MEDIA_PAD_FL_SINK;
1664
1665         ret = media_entity_init(&chan->video.entity, 1, &chan->pad, 0);
1666         if (ret < 0) {
1667                 dev_err(&chan->video.dev, "failed to init video entity\n");
1668                 return ret;
1669         }
1670
1671         /* init control handler */
1672         ret = v4l2_ctrl_handler_init(&chan->ctrl_handler, MAX_CID_CONTROLS);
1673         if (chan->ctrl_handler.error) {
1674                 dev_err(&chan->video.dev, "failed to init control handler\n");
1675                 goto ctrl_init_error;
1676         }
1677
1678         /* init video node... */
1679         chan->video.fops = &tegra_channel_fops;
1680         chan->video.v4l2_dev = &vi->v4l2_dev;
1681         chan->video.queue = &chan->queue;
1682         snprintf(chan->video.name, sizeof(chan->video.name), "%s-%s-%u",
1683                 dev_name(vi->dev), chan->pg_mode ? "tpg" : "output",
1684                 chan->port[0]);
1685         chan->video.vfl_type = VFL_TYPE_GRABBER;
1686         chan->video.vfl_dir = VFL_DIR_RX;
1687         chan->video.release = video_device_release_empty;
1688         chan->video.ioctl_ops = &tegra_channel_ioctl_ops;
1689         chan->video.ctrl_handler = &chan->ctrl_handler;
1690         chan->video.lock = &chan->video_lock;
1691
1692         set_bit(_IOC_NR(VIDIOC_G_PRIORITY), chan->video.valid_ioctls);
1693         set_bit(_IOC_NR(VIDIOC_S_PRIORITY), chan->video.valid_ioctls);
1694
1695         video_set_drvdata(&chan->video, chan);
1696
1697 #if defined(CONFIG_VIDEOBUF2_DMA_CONTIG)
1698         /* get the buffers queue... */
1699         chan->alloc_ctx = vb2_dma_contig_init_ctx(chan->vi->dev);
1700         if (IS_ERR(chan->alloc_ctx)) {
1701                 dev_err(chan->vi->dev, "failed to init vb2 buffer\n");
1702                 ret = -ENOMEM;
1703                 goto vb2_init_error;
1704         }
1705 #endif
1706
1707         chan->queue.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1708         chan->queue.io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ | VB2_USERPTR;
1709         chan->queue.lock = &chan->video_lock;
1710         chan->queue.drv_priv = chan;
1711         chan->queue.buf_struct_size = sizeof(struct tegra_channel_buffer);
1712         chan->queue.ops = &tegra_channel_queue_qops;
1713 #if defined(CONFIG_VIDEOBUF2_DMA_CONTIG)
1714         chan->queue.mem_ops = &vb2_dma_contig_memops;
1715 #endif
1716         chan->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
1717                                    | V4L2_BUF_FLAG_TSTAMP_SRC_EOF;
1718         ret = vb2_queue_init(&chan->queue);
1719         if (ret < 0) {
1720                 dev_err(chan->vi->dev, "failed to initialize VB2 queue\n");
1721                 goto vb2_queue_error;
1722         }
1723
1724         if (vi->fops->vi_syncpt_init)
1725                 vi->fops->vi_syncpt_init(chan);
1726
1727         chan->init_done = true;
1728         return 0;
1729
1730 vb2_queue_error:
1731 #if defined(CONFIG_VIDEOBUF2_DMA_CONTIG)
1732         vb2_dma_contig_cleanup_ctx(chan->alloc_ctx);
1733 vb2_init_error:
1734 #endif
1735         v4l2_ctrl_handler_free(&chan->ctrl_handler);
1736 ctrl_init_error:
1737         media_entity_cleanup(&chan->video.entity);
1738         return ret;
1739 }
1740
1741 int tegra_channel_cleanup(struct tegra_channel *chan)
1742 {
1743         if (chan->vi->fops->vi_syncpt_free)
1744                 chan->vi->fops->vi_syncpt_free(chan);
1745
1746         /* release embedded data buffer */
1747         if (chan->vi->emb_buf_size > 0) {
1748                 dma_free_coherent(chan->vi->dev,
1749                         chan->vi->emb_buf_size,
1750                         chan->vi->emb_buf_addr, chan->vi->emb_buf);
1751                 chan->vi->emb_buf_size = 0;
1752         }
1753
1754         v4l2_ctrl_handler_free(&chan->ctrl_handler);
1755         vb2_queue_release(&chan->queue);
1756 #if defined(CONFIG_VIDEOBUF2_DMA_CONTIG)
1757         vb2_dma_contig_cleanup_ctx(chan->alloc_ctx);
1758 #endif
1759
1760         media_entity_cleanup(&chan->video.entity);
1761
1762         return 0;
1763 }
1764
1765 int tegra_vi_channels_register(struct tegra_mc_vi *vi)
1766 {
1767         int ret = 0;
1768         struct tegra_channel *it;
1769         int count = 0;
1770
1771         list_for_each_entry(it, &vi->vi_chans, list) {
1772                 struct v4l2_subdev *sd = it->subdev_on_csi;
1773                 bool is_csi = false;
1774
1775                 if (sd) {
1776                         /*
1777                          * If subdevice on csi is csi itself,
1778                          * then sensor subdevice is not connected
1779                          */
1780                         is_csi = strstr(sd->name, "nvcsi") != NULL;
1781
1782                         if (is_csi)
1783                                 continue;
1784                 } else
1785                         continue;
1786
1787                 if (!it->init_done)
1788                         continue;
1789                 ret = video_register_device(&it->video, VFL_TYPE_GRABBER, -1);
1790                 if (ret < 0) {
1791                         dev_err(&it->video.dev, "failed to register %s\n",
1792                                 it->video.name);
1793                         continue;
1794                 }
1795                 count++;
1796         }
1797
1798         if (count == 0) {
1799                 dev_err(vi->dev, "all channel register failed\n");
1800                 return ret;
1801         }
1802
1803         return 0;
1804 }
1805
1806 void tegra_vi_channels_unregister(struct tegra_mc_vi *vi)
1807 {
1808         struct tegra_channel *it;
1809
1810         list_for_each_entry(it, &vi->vi_chans, list) {
1811                 if (it->video.cdev != NULL)
1812                         video_unregister_device(&it->video);
1813         }
1814 }
1815
1816 int tegra_vi_mfi_work(struct tegra_mc_vi *vi, int channel)
1817 {
1818         if (vi->fops)
1819                 return vi->fops->vi_mfi_work(vi, channel);
1820
1821         return 0;
1822 }
1823 EXPORT_SYMBOL(tegra_vi_mfi_work);
1824
1825 int tegra_vi_channels_init(struct tegra_mc_vi *vi)
1826 {
1827         int ret = 0;
1828         struct tegra_channel *it;
1829         int count = 0;
1830
1831         list_for_each_entry(it, &vi->vi_chans, list) {
1832                 it->vi = vi;
1833                 ret = tegra_channel_init(it);
1834                 if (ret < 0) {
1835                         dev_err(vi->dev, "channel init failed\n");
1836                         continue;
1837                 }
1838                 count++;
1839         }
1840
1841         if (count == 0) {
1842                 dev_err(vi->dev, "all channel init failed\n");
1843                 return ret;
1844         }
1845
1846         return 0;
1847 }
1848 EXPORT_SYMBOL(tegra_vi_channels_init);
1849 int tegra_vi_channels_cleanup(struct tegra_mc_vi *vi)
1850 {
1851         int ret = 0, err = 0;
1852         struct tegra_channel *it;
1853
1854         list_for_each_entry(it, &vi->vi_chans, list) {
1855                 if (!it->init_done)
1856                         continue;
1857                 err = tegra_channel_cleanup(it);
1858                 if (err < 0) {
1859                         ret = err;
1860                         dev_err(vi->dev, "channel cleanup failed, err %d\n",
1861                                         err);
1862                 }
1863         }
1864         return ret;
1865 }
1866 EXPORT_SYMBOL(tegra_vi_channels_cleanup);