drivers/staging/media/tegra-video/vi.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (C) 2020 NVIDIA CORPORATION.  All rights reserved.
   4  */
   5
   6 #include <linux/bitmap.h>
   7 #include <linux/clk.h>
   8 #include <linux/delay.h>
   9 #include <linux/host1x.h>
  10 #include <linux/lcm.h>
  11 #include <linux/list.h>
  12 #include <linux/module.h>
  13 #include <linux/of.h>
  14 #include <linux/of_device.h>
  15 #include <linux/of_graph.h>
  16 #include <linux/platform_device.h>
  17 #include <linux/regulator/consumer.h>
  18 #include <linux/pm_runtime.h>
  19 #include <linux/slab.h>
  20
  21 #include <media/v4l2-event.h>
  22 #include <media/v4l2-fh.h>
  23 #include <media/v4l2-fwnode.h>
  24 #include <media/v4l2-ioctl.h>
  25 #include <media/videobuf2-dma-contig.h>
  26
  27 #include <soc/tegra/pmc.h>
  28
  29 #include "vi.h"
  30 #include "video.h"
  31
  32 #define SURFACE_ALIGN_BYTES             64
  33 #define MAX_CID_CONTROLS                1
  34
  35 static const struct tegra_video_format tegra_default_format = {
  36         .img_dt = TEGRA_IMAGE_DT_RAW10,
  37         .bit_width = 10,
  38         .code = MEDIA_BUS_FMT_SRGGB10_1X10,
  39         .bpp = 2,
  40         .img_fmt = TEGRA_IMAGE_FORMAT_DEF,
  41         .fourcc = V4L2_PIX_FMT_SRGGB10,
  42 };
  43
  44 static inline struct tegra_vi *
  45 host1x_client_to_vi(struct host1x_client *client)
  46 {
  47         return container_of(client, struct tegra_vi, client);
  48 }
  49
  50 static inline struct tegra_channel_buffer *
  51 to_tegra_channel_buffer(struct vb2_v4l2_buffer *vb)
  52 {
  53         return container_of(vb, struct tegra_channel_buffer, buf);
  54 }
  55
  56 static inline struct tegra_vi_graph_entity *
  57 to_tegra_vi_graph_entity(struct v4l2_async_subdev *asd)
  58 {
  59         return container_of(asd, struct tegra_vi_graph_entity, asd);
  60 }
  61
  62 static int tegra_get_format_idx_by_code(struct tegra_vi *vi,
  63                                         unsigned int code,
  64                                         unsigned int offset)
  65 {
  66         unsigned int i;
  67
  68         for (i = offset; i < vi->soc->nformats; ++i) {
  69                 if (vi->soc->video_formats[i].code == code)
  70                         return i;
  71         }
  72
  73         return -1;
  74 }
  75
  76 static u32 tegra_get_format_fourcc_by_idx(struct tegra_vi *vi,
  77                                           unsigned int index)
  78 {
  79         if (index >= vi->soc->nformats)
  80                 return -EINVAL;
  81
  82         return vi->soc->video_formats[index].fourcc;
  83 }
  84
  85 static const struct tegra_video_format *
  86 tegra_get_format_by_fourcc(struct tegra_vi *vi, u32 fourcc)
  87 {
  88         unsigned int i;
  89
  90         for (i = 0; i < vi->soc->nformats; ++i) {
  91                 if (vi->soc->video_formats[i].fourcc == fourcc)
  92                         return &vi->soc->video_formats[i];
  93         }
  94
  95         return NULL;
  96 }
  97
  98 /*
  99  * videobuf2 queue operations
 100  */
 101 static int tegra_channel_queue_setup(struct vb2_queue *vq,
 102                                      unsigned int *nbuffers,
 103                                      unsigned int *nplanes,
 104                                      unsigned int sizes[],
 105                                      struct device *alloc_devs[])
 106 {
 107         struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
 108
 109         if (*nplanes)
 110                 return sizes[0] < chan->format.sizeimage ? -EINVAL : 0;
 111
 112         *nplanes = 1;
 113         sizes[0] = chan->format.sizeimage;
 114         alloc_devs[0] = chan->vi->dev;
 115
 116         return 0;
 117 }
 118
 119 static int tegra_channel_buffer_prepare(struct vb2_buffer *vb)
 120 {
 121         struct tegra_vi_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
 122         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 123         struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vbuf);
 124         unsigned long size = chan->format.sizeimage;
 125
 126         if (vb2_plane_size(vb, 0) < size) {
 127                 v4l2_err(chan->video.v4l2_dev,
 128                          "buffer too small (%lu < %lu)\n",
 129                          vb2_plane_size(vb, 0), size);
 130                 return -EINVAL;
 131         }
 132
 133         vb2_set_plane_payload(vb, 0, size);
 134         buf->chan = chan;
 135         buf->addr = vb2_dma_contig_plane_dma_addr(vb, 0);
 136
 137         return 0;
 138 }
 139
 140 static void tegra_channel_buffer_queue(struct vb2_buffer *vb)
 141 {
 142         struct tegra_vi_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
 143         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 144         struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vbuf);
 145
 146         /* put buffer into the capture queue */
 147         spin_lock(&chan->start_lock);
 148         list_add_tail(&buf->queue, &chan->capture);
 149         spin_unlock(&chan->start_lock);
 150
 151         /* wait up kthread for capture */
 152         wake_up_interruptible(&chan->start_wait);
 153 }
 154
 155 struct v4l2_subdev *
 156 tegra_channel_get_remote_csi_subdev(struct tegra_vi_channel *chan)
 157 {
 158         struct media_pad *pad;
 159
 160         pad = media_entity_remote_pad(&chan->pad);
 161         if (!pad)
 162                 return NULL;
 163
 164         return media_entity_to_v4l2_subdev(pad->entity);
 165 }
 166
 167 struct v4l2_subdev *
 168 tegra_channel_get_remote_source_subdev(struct tegra_vi_channel *chan)
 169 {
 170         struct media_pad *pad;
 171         struct v4l2_subdev *subdev;
 172         struct media_entity *entity;
 173
 174         subdev = tegra_channel_get_remote_csi_subdev(chan);
 175         if (!subdev)
 176                 return NULL;
 177
 178         pad = &subdev->entity.pads[0];
 179         while (!(pad->flags & MEDIA_PAD_FL_SOURCE)) {
 180                 pad = media_entity_remote_pad(pad);
 181                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 182                         break;
 183                 entity = pad->entity;
 184                 pad = &entity->pads[0];
 185                 subdev = media_entity_to_v4l2_subdev(entity);
 186         }
 187
 188         return subdev;
 189 }
 190
 191 static int tegra_channel_enable_stream(struct tegra_vi_channel *chan)
 192 {
 193         struct v4l2_subdev *csi_subdev, *src_subdev;
 194         struct tegra_csi_channel *csi_chan;
 195         int ret, err;
 196
 197         /*
 198          * Tegra CSI receiver can detect the first LP to HS transition.
 199          * So, start the CSI stream-on prior to sensor stream-on and
 200          * vice-versa for stream-off.
 201          */
 202         csi_subdev = tegra_channel_get_remote_csi_subdev(chan);
 203         ret = v4l2_subdev_call(csi_subdev, video, s_stream, true);
 204         if (ret < 0 && ret != -ENOIOCTLCMD)
 205                 return ret;
 206
 207         if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 208                 return 0;
 209
 210         csi_chan = v4l2_get_subdevdata(csi_subdev);
 211         /*
 212          * TRM has incorrectly documented to wait for done status from
 213          * calibration logic after CSI interface power on.
 214          * As per the design, calibration results are latched and applied
 215          * to the pads only when the link is in LP11 state which will happen
 216          * during the sensor stream-on.
 217          * CSI subdev stream-on triggers start of MIPI pads calibration.
 218          * Wait for calibration to finish here after sensor subdev stream-on.
 219          */
 220         src_subdev = tegra_channel_get_remote_source_subdev(chan);
 221         ret = v4l2_subdev_call(src_subdev, video, s_stream, true);
 222         err = tegra_mipi_finish_calibration(csi_chan->mipi);
 223
 224         if (ret < 0 && ret != -ENOIOCTLCMD)
 225                 goto err_disable_csi_stream;
 226
 227         if (err < 0)
 228                 dev_warn(csi_chan->csi->dev,
 229                          "MIPI calibration failed: %d\n", err);
 230
 231         return 0;
 232
 233 err_disable_csi_stream:
 234         v4l2_subdev_call(csi_subdev, video, s_stream, false);
 235         return ret;
 236 }
 237
 238 static int tegra_channel_disable_stream(struct tegra_vi_channel *chan)
 239 {
 240         struct v4l2_subdev *subdev;
 241         int ret;
 242
 243         /*
 244          * Stream-off subdevices in reverse order to stream-on.
 245          * Remote source subdev in TPG mode is same as CSI subdev.
 246          */
 247         subdev = tegra_channel_get_remote_source_subdev(chan);
 248         ret = v4l2_subdev_call(subdev, video, s_stream, false);
 249         if (ret < 0 && ret != -ENOIOCTLCMD)
 250                 return ret;
 251
 252         if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 253                 return 0;
 254
 255         subdev = tegra_channel_get_remote_csi_subdev(chan);
 256         ret = v4l2_subdev_call(subdev, video, s_stream, false);
 257         if (ret < 0 && ret != -ENOIOCTLCMD)
 258                 return ret;
 259
 260         return 0;
 261 }
 262
 263 int tegra_channel_set_stream(struct tegra_vi_channel *chan, bool on)
 264 {
 265         int ret;
 266
 267         if (on)
 268                 ret = tegra_channel_enable_stream(chan);
 269         else
 270                 ret = tegra_channel_disable_stream(chan);
 271
 272         return ret;
 273 }
 274
 275 void tegra_channel_release_buffers(struct tegra_vi_channel *chan,
 276                                    enum vb2_buffer_state state)
 277 {
 278         struct tegra_channel_buffer *buf, *nbuf;
 279
 280         spin_lock(&chan->start_lock);
 281         list_for_each_entry_safe(buf, nbuf, &chan->capture, queue) {
 282                 vb2_buffer_done(&buf->buf.vb2_buf, state);
 283                 list_del(&buf->queue);
 284         }
 285         spin_unlock(&chan->start_lock);
 286
 287         spin_lock(&chan->done_lock);
 288         list_for_each_entry_safe(buf, nbuf, &chan->done, queue) {
 289                 vb2_buffer_done(&buf->buf.vb2_buf, state);
 290                 list_del(&buf->queue);
 291         }
 292         spin_unlock(&chan->done_lock);
 293 }
 294
 295 static int tegra_channel_start_streaming(struct vb2_queue *vq, u32 count)
 296 {
 297         struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
 298         int ret;
 299
 300         ret = pm_runtime_get_sync(chan->vi->dev);
 301         if (ret < 0) {
 302                 dev_err(chan->vi->dev, "failed to get runtime PM: %d\n", ret);
 303                 pm_runtime_put_noidle(chan->vi->dev);
 304                 return ret;
 305         }
 306
 307         ret = chan->vi->ops->vi_start_streaming(vq, count);
 308         if (ret < 0)
 309                 pm_runtime_put(chan->vi->dev);
 310
 311         return ret;
 312 }
 313
 314 static void tegra_channel_stop_streaming(struct vb2_queue *vq)
 315 {
 316         struct tegra_vi_channel *chan = vb2_get_drv_priv(vq);
 317
 318         chan->vi->ops->vi_stop_streaming(vq);
 319         pm_runtime_put(chan->vi->dev);
 320 }
 321
 322 static const struct vb2_ops tegra_channel_queue_qops = {
 323         .queue_setup = tegra_channel_queue_setup,
 324         .buf_prepare = tegra_channel_buffer_prepare,
 325         .buf_queue = tegra_channel_buffer_queue,
 326         .wait_prepare = vb2_ops_wait_prepare,
 327         .wait_finish = vb2_ops_wait_finish,
 328         .start_streaming = tegra_channel_start_streaming,
 329         .stop_streaming = tegra_channel_stop_streaming,
 330 };
 331
 332 /*
 333  * V4L2 ioctl operations
 334  */
 335 static int tegra_channel_querycap(struct file *file, void *fh,
 336                                   struct v4l2_capability *cap)
 337 {
 338         struct tegra_vi_channel *chan = video_drvdata(file);
 339
 340         strscpy(cap->driver, "tegra-video", sizeof(cap->driver));
 341         strscpy(cap->card, chan->video.name, sizeof(cap->card));
 342         snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
 343                  dev_name(chan->vi->dev));
 344
 345         return 0;
 346 }
 347
 348 static int tegra_channel_g_parm(struct file *file, void *fh,
 349                                 struct v4l2_streamparm *a)
 350 {
 351         struct tegra_vi_channel *chan = video_drvdata(file);
 352         struct v4l2_subdev *subdev;
 353
 354         subdev = tegra_channel_get_remote_source_subdev(chan);
 355         return v4l2_g_parm_cap(&chan->video, subdev, a);
 356 }
 357
 358 static int tegra_channel_s_parm(struct file *file, void *fh,
 359                                 struct v4l2_streamparm *a)
 360 {
 361         struct tegra_vi_channel *chan = video_drvdata(file);
 362         struct v4l2_subdev *subdev;
 363
 364         subdev = tegra_channel_get_remote_source_subdev(chan);
 365         return v4l2_s_parm_cap(&chan->video, subdev, a);
 366 }
 367
 368 static int tegra_channel_enum_framesizes(struct file *file, void *fh,
 369                                          struct v4l2_frmsizeenum *sizes)
 370 {
 371         int ret;
 372         struct tegra_vi_channel *chan = video_drvdata(file);
 373         struct v4l2_subdev *subdev;
 374         const struct tegra_video_format *fmtinfo;
 375         struct v4l2_subdev_frame_size_enum fse = {
 376                 .index = sizes->index,
 377                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 378         };
 379
 380         fmtinfo = tegra_get_format_by_fourcc(chan->vi, sizes->pixel_format);
 381         if (!fmtinfo)
 382                 return -EINVAL;
 383
 384         fse.code = fmtinfo->code;
 385
 386         subdev = tegra_channel_get_remote_source_subdev(chan);
 387         ret = v4l2_subdev_call(subdev, pad, enum_frame_size, NULL, &fse);
 388         if (ret)
 389                 return ret;
 390
 391         sizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 392         sizes->discrete.width = fse.max_width;
 393         sizes->discrete.height = fse.max_height;
 394
 395         return 0;
 396 }
 397
 398 static int tegra_channel_enum_frameintervals(struct file *file, void *fh,
 399                                              struct v4l2_frmivalenum *ivals)
 400 {
 401         int ret;
 402         struct tegra_vi_channel *chan = video_drvdata(file);
 403         struct v4l2_subdev *subdev;
 404         const struct tegra_video_format *fmtinfo;
 405         struct v4l2_subdev_frame_interval_enum fie = {
 406                 .index = ivals->index,
 407                 .width = ivals->width,
 408                 .height = ivals->height,
 409                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 410         };
 411
 412         fmtinfo = tegra_get_format_by_fourcc(chan->vi, ivals->pixel_format);
 413         if (!fmtinfo)
 414                 return -EINVAL;
 415
 416         fie.code = fmtinfo->code;
 417
 418         subdev = tegra_channel_get_remote_source_subdev(chan);
 419         ret = v4l2_subdev_call(subdev, pad, enum_frame_interval, NULL, &fie);
 420         if (ret)
 421                 return ret;
 422
 423         ivals->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 424         ivals->discrete.numerator = fie.interval.numerator;
 425         ivals->discrete.denominator = fie.interval.denominator;
 426
 427         return 0;
 428 }
 429
 430 static int tegra_channel_enum_format(struct file *file, void *fh,
 431                                      struct v4l2_fmtdesc *f)
 432 {
 433         struct tegra_vi_channel *chan = video_drvdata(file);
 434         unsigned int index = 0, i;
 435         unsigned long *fmts_bitmap = chan->tpg_fmts_bitmap;
 436
 437         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
 438                 fmts_bitmap = chan->fmts_bitmap;
 439
 440         if (f->index >= bitmap_weight(fmts_bitmap, MAX_FORMAT_NUM))
 441                 return -EINVAL;
 442
 443         for (i = 0; i < f->index + 1; i++, index++)
 444                 index = find_next_bit(fmts_bitmap, MAX_FORMAT_NUM, index);
 445
 446         f->pixelformat = tegra_get_format_fourcc_by_idx(chan->vi, index - 1);
 447
 448         return 0;
 449 }
 450
 451 static int tegra_channel_get_format(struct file *file, void *fh,
 452                                     struct v4l2_format *format)
 453 {
 454         struct tegra_vi_channel *chan = video_drvdata(file);
 455
 456         format->fmt.pix = chan->format;
 457
 458         return 0;
 459 }
 460
 461 static void tegra_channel_fmt_align(struct tegra_vi_channel *chan,
 462                                     struct v4l2_pix_format *pix,
 463                                     unsigned int bpp)
 464 {
 465         unsigned int min_bpl;
 466         unsigned int max_bpl;
 467         unsigned int bpl;
 468
 469         /*
 470          * The transfer alignment requirements are expressed in bytes.
 471          * Clamp the requested width and height to the limits.
 472          */
 473         pix->width = clamp(pix->width, TEGRA_MIN_WIDTH, TEGRA_MAX_WIDTH);
 474         pix->height = clamp(pix->height, TEGRA_MIN_HEIGHT, TEGRA_MAX_HEIGHT);
 475
 476         /* Clamp the requested bytes per line value. If the maximum bytes per
 477          * line value is zero, the module doesn't support user configurable
 478          * line sizes. Override the requested value with the minimum in that
 479          * case.
 480          */
 481         min_bpl = pix->width * bpp;
 482         max_bpl = rounddown(TEGRA_MAX_WIDTH, SURFACE_ALIGN_BYTES);
 483         bpl = roundup(pix->bytesperline, SURFACE_ALIGN_BYTES);
 484
 485         pix->bytesperline = clamp(bpl, min_bpl, max_bpl);
 486         pix->sizeimage = pix->bytesperline * pix->height;
 487 }
 488
 489 static int __tegra_channel_try_format(struct tegra_vi_channel *chan,
 490                                       struct v4l2_pix_format *pix)
 491 {
 492         const struct tegra_video_format *fmtinfo;
 493         struct v4l2_subdev *subdev;
 494         struct v4l2_subdev_format fmt;
 495         struct v4l2_subdev_pad_config *pad_cfg;
 496         struct v4l2_subdev_frame_size_enum fse = {
 497                 .which = V4L2_SUBDEV_FORMAT_TRY,
 498         };
 499         struct v4l2_subdev_selection sdsel = {
 500                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 501                 .target = V4L2_SEL_TGT_CROP_BOUNDS,
 502         };
 503         int ret;
 504
 505         subdev = tegra_channel_get_remote_source_subdev(chan);
 506         if (!subdev)
 507                 return -ENODEV;
 508
 509         pad_cfg = v4l2_subdev_alloc_pad_config(subdev);
 510         if (!pad_cfg)
 511                 return -ENOMEM;
 512         /*
 513          * Retrieve the format information and if requested format isn't
 514          * supported, keep the current format.
 515          */
 516         fmtinfo = tegra_get_format_by_fourcc(chan->vi, pix->pixelformat);
 517         if (!fmtinfo) {
 518                 pix->pixelformat = chan->format.pixelformat;
 519                 pix->colorspace = chan->format.colorspace;
 520                 fmtinfo = tegra_get_format_by_fourcc(chan->vi,
 521                                                      pix->pixelformat);
 522         }
 523
 524         pix->field = V4L2_FIELD_NONE;
 525         fmt.which = V4L2_SUBDEV_FORMAT_TRY;
 526         fmt.pad = 0;
 527         v4l2_fill_mbus_format(&fmt.format, pix, fmtinfo->code);
 528
 529         /*
 530          * Attempt to obtain the format size from subdev.
 531          * If not available, try to get crop boundary from subdev.
 532          */
 533         fse.code = fmtinfo->code;
 534         ret = v4l2_subdev_call(subdev, pad, enum_frame_size, pad_cfg, &fse);
 535         if (ret) {
 536                 ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sdsel);
 537                 if (ret)
 538                         return -EINVAL;
 539                 pad_cfg->try_crop.width = sdsel.r.width;
 540                 pad_cfg->try_crop.height = sdsel.r.height;
 541         } else {
 542                 pad_cfg->try_crop.width = fse.max_width;
 543                 pad_cfg->try_crop.height = fse.max_height;
 544         }
 545
 546         ret = v4l2_subdev_call(subdev, pad, set_fmt, pad_cfg, &fmt);
 547         if (ret < 0)
 548                 return ret;
 549
 550         v4l2_fill_pix_format(pix, &fmt.format);
 551         tegra_channel_fmt_align(chan, pix, fmtinfo->bpp);
 552
 553         v4l2_subdev_free_pad_config(pad_cfg);
 554
 555         return 0;
 556 }
 557
 558 static int tegra_channel_try_format(struct file *file, void *fh,
 559                                     struct v4l2_format *format)
 560 {
 561         struct tegra_vi_channel *chan = video_drvdata(file);
 562
 563         return __tegra_channel_try_format(chan, &format->fmt.pix);
 564 }
 565
 566 static int tegra_channel_set_format(struct file *file, void *fh,
 567                                     struct v4l2_format *format)
 568 {
 569         struct tegra_vi_channel *chan = video_drvdata(file);
 570         const struct tegra_video_format *fmtinfo;
 571         struct v4l2_subdev_format fmt;
 572         struct v4l2_subdev *subdev;
 573         struct v4l2_pix_format *pix = &format->fmt.pix;
 574         int ret;
 575
 576         if (vb2_is_busy(&chan->queue))
 577                 return -EBUSY;
 578
 579         /* get supported format by try_fmt */
 580         ret = __tegra_channel_try_format(chan, pix);
 581         if (ret)
 582                 return ret;
 583
 584         fmtinfo = tegra_get_format_by_fourcc(chan->vi, pix->pixelformat);
 585
 586         fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 587         fmt.pad = 0;
 588         v4l2_fill_mbus_format(&fmt.format, pix, fmtinfo->code);
 589         subdev = tegra_channel_get_remote_source_subdev(chan);
 590         ret = v4l2_subdev_call(subdev, pad, set_fmt, NULL, &fmt);
 591         if (ret < 0)
 592                 return ret;
 593
 594         v4l2_fill_pix_format(pix, &fmt.format);
 595         tegra_channel_fmt_align(chan, pix, fmtinfo->bpp);
 596
 597         chan->format = *pix;
 598         chan->fmtinfo = fmtinfo;
 599
 600         return 0;
 601 }
 602
 603 static int tegra_channel_set_subdev_active_fmt(struct tegra_vi_channel *chan)
 604 {
 605         int ret, index;
 606         struct v4l2_subdev *subdev;
 607         struct v4l2_subdev_format fmt = {
 608                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 609         };
 610
 611         /*
 612          * Initialize channel format to the sub-device active format if there
 613          * is corresponding match in the Tegra supported video formats.
 614          */
 615         subdev = tegra_channel_get_remote_source_subdev(chan);
 616         ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
 617         if (ret)
 618                 return ret;
 619
 620         index = tegra_get_format_idx_by_code(chan->vi, fmt.format.code, 0);
 621         if (index < 0)
 622                 return -EINVAL;
 623
 624         chan->fmtinfo = &chan->vi->soc->video_formats[index];
 625         v4l2_fill_pix_format(&chan->format, &fmt.format);
 626         chan->format.pixelformat = chan->fmtinfo->fourcc;
 627         chan->format.bytesperline = chan->format.width * chan->fmtinfo->bpp;
 628         chan->format.sizeimage = chan->format.bytesperline *
 629                                  chan->format.height;
 630         tegra_channel_fmt_align(chan, &chan->format, chan->fmtinfo->bpp);
 631
 632         return 0;
 633 }
 634
 635 static int tegra_channel_g_selection(struct file *file, void *priv,
 636                                      struct v4l2_selection *sel)
 637 {
 638         struct tegra_vi_channel *chan = video_drvdata(file);
 639         struct v4l2_subdev *subdev;
 640         struct v4l2_subdev_format fmt = {
 641                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 642         };
 643         struct v4l2_subdev_selection sdsel = {
 644                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 645                 .target = sel->target,
 646         };
 647         int ret;
 648
 649         subdev = tegra_channel_get_remote_source_subdev(chan);
 650         if (!v4l2_subdev_has_op(subdev, pad, get_selection))
 651                 return -ENOTTY;
 652
 653         if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 654                 return -EINVAL;
 655         /*
 656          * Try the get selection operation and fallback to get format if not
 657          * implemented.
 658          */
 659         ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sdsel);
 660         if (!ret)
 661                 sel->r = sdsel.r;
 662         if (ret != -ENOIOCTLCMD)
 663                 return ret;
 664
 665         ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
 666         if (ret < 0)
 667                 return ret;
 668
 669         sel->r.left = 0;
 670         sel->r.top = 0;
 671         sel->r.width = fmt.format.width;
 672         sel->r.height = fmt.format.height;
 673
 674         return 0;
 675 }
 676
 677 static int tegra_channel_s_selection(struct file *file, void *fh,
 678                                      struct v4l2_selection *sel)
 679 {
 680         struct tegra_vi_channel *chan = video_drvdata(file);
 681         struct v4l2_subdev *subdev;
 682         int ret;
 683         struct v4l2_subdev_selection sdsel = {
 684                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 685                 .target = sel->target,
 686                 .flags = sel->flags,
 687                 .r = sel->r,
 688         };
 689
 690         subdev = tegra_channel_get_remote_source_subdev(chan);
 691         if (!v4l2_subdev_has_op(subdev, pad, set_selection))
 692                 return -ENOTTY;
 693
 694         if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 695                 return -EINVAL;
 696
 697         if (vb2_is_busy(&chan->queue))
 698                 return -EBUSY;
 699
 700         ret = v4l2_subdev_call(subdev, pad, set_selection, NULL, &sdsel);
 701         if (!ret) {
 702                 sel->r = sdsel.r;
 703                 /*
 704                  * Subdev active format resolution may have changed during
 705                  * set selection operation. So, update channel format to
 706                  * the sub-device active format.
 707                  */
 708                 return tegra_channel_set_subdev_active_fmt(chan);
 709         }
 710
 711         return ret;
 712 }
 713
 714 static int tegra_channel_enum_input(struct file *file, void *fh,
 715                                     struct v4l2_input *inp)
 716 {
 717         struct tegra_vi_channel *chan = video_drvdata(file);
 718         struct v4l2_subdev *subdev;
 719
 720         if (inp->index)
 721                 return -EINVAL;
 722
 723         inp->type = V4L2_INPUT_TYPE_CAMERA;
 724         subdev = tegra_channel_get_remote_source_subdev(chan);
 725         strscpy(inp->name, subdev->name, sizeof(inp->name));
 726
 727         return 0;
 728 }
 729
 730 static int tegra_channel_g_input(struct file *file, void *priv,
 731                                  unsigned int *i)
 732 {
 733         *i = 0;
 734
 735         return 0;
 736 }
 737
 738 static int tegra_channel_s_input(struct file *file, void *priv,
 739                                  unsigned int input)
 740 {
 741         if (input > 0)
 742                 return -EINVAL;
 743
 744         return 0;
 745 }
 746
 747 static const struct v4l2_ioctl_ops tegra_channel_ioctl_ops = {
 748         .vidioc_querycap                = tegra_channel_querycap,
 749         .vidioc_g_parm                  = tegra_channel_g_parm,
 750         .vidioc_s_parm                  = tegra_channel_s_parm,
 751         .vidioc_enum_framesizes         = tegra_channel_enum_framesizes,
 752         .vidioc_enum_frameintervals     = tegra_channel_enum_frameintervals,
 753         .vidioc_enum_fmt_vid_cap        = tegra_channel_enum_format,
 754         .vidioc_g_fmt_vid_cap           = tegra_channel_get_format,
 755         .vidioc_s_fmt_vid_cap           = tegra_channel_set_format,
 756         .vidioc_try_fmt_vid_cap         = tegra_channel_try_format,
 757         .vidioc_enum_input              = tegra_channel_enum_input,
 758         .vidioc_g_input                 = tegra_channel_g_input,
 759         .vidioc_s_input                 = tegra_channel_s_input,
 760         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
 761         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
 762         .vidioc_querybuf                = vb2_ioctl_querybuf,
 763         .vidioc_qbuf                    = vb2_ioctl_qbuf,
 764         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
 765         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
 766         .vidioc_expbuf                  = vb2_ioctl_expbuf,
 767         .vidioc_streamon                = vb2_ioctl_streamon,
 768         .vidioc_streamoff               = vb2_ioctl_streamoff,
 769         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
 770         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
 771         .vidioc_g_selection             = tegra_channel_g_selection,
 772         .vidioc_s_selection             = tegra_channel_s_selection,
 773 };
 774
 775 /*
 776  * V4L2 file operations
 777  */
 778 static const struct v4l2_file_operations tegra_channel_fops = {
 779         .owner          = THIS_MODULE,
 780         .unlocked_ioctl = video_ioctl2,
 781         .open           = v4l2_fh_open,
 782         .release        = vb2_fop_release,
 783         .read           = vb2_fop_read,
 784         .poll           = vb2_fop_poll,
 785         .mmap           = vb2_fop_mmap,
 786 };
 787
 788 /*
 789  * V4L2 control operations
 790  */
 791 #if IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)
 792 static int vi_s_ctrl(struct v4l2_ctrl *ctrl)
 793 {
 794         struct tegra_vi_channel *chan = container_of(ctrl->handler,
 795                                                      struct tegra_vi_channel,
 796                                                      ctrl_handler);
 797
 798         switch (ctrl->id) {
 799         case V4L2_CID_TEST_PATTERN:
 800                 /* pattern change takes effect on next stream */
 801                 chan->pg_mode = ctrl->val + 1;
 802                 break;
 803         default:
 804                 return -EINVAL;
 805         }
 806
 807         return 0;
 808 }
 809
 810 static const struct v4l2_ctrl_ops vi_ctrl_ops = {
 811         .s_ctrl = vi_s_ctrl,
 812 };
 813
 814 static const char *const vi_pattern_strings[] = {
 815         "Black/White Direct Mode",
 816         "Color Patch Mode",
 817 };
 818 #endif
 819
 820 static int tegra_channel_setup_ctrl_handler(struct tegra_vi_channel *chan)
 821 {
 822         int ret;
 823
 824 #if IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)
 825         /* add test pattern control handler to v4l2 device */
 826         v4l2_ctrl_new_std_menu_items(&chan->ctrl_handler, &vi_ctrl_ops,
 827                                      V4L2_CID_TEST_PATTERN,
 828                                      ARRAY_SIZE(vi_pattern_strings) - 1,
 829                                      0, 0, vi_pattern_strings);
 830         if (chan->ctrl_handler.error) {
 831                 dev_err(chan->vi->dev, "failed to add TPG ctrl handler: %d\n",
 832                         chan->ctrl_handler.error);
 833                 v4l2_ctrl_handler_free(&chan->ctrl_handler);
 834                 return chan->ctrl_handler.error;
 835         }
 836 #else
 837         struct v4l2_subdev *subdev;
 838
 839         subdev = tegra_channel_get_remote_source_subdev(chan);
 840         if (!subdev)
 841                 return -ENODEV;
 842
 843         ret = v4l2_ctrl_add_handler(&chan->ctrl_handler, subdev->ctrl_handler,
 844                                     NULL, true);
 845         if (ret < 0) {
 846                 dev_err(chan->vi->dev,
 847                         "failed to add subdev %s ctrl handler: %d\n",
 848                         subdev->name, ret);
 849                 v4l2_ctrl_handler_free(&chan->ctrl_handler);
 850                 return ret;
 851         }
 852 #endif
 853
 854         /* setup the controls */
 855         ret = v4l2_ctrl_handler_setup(&chan->ctrl_handler);
 856         if (ret < 0) {
 857                 dev_err(chan->vi->dev,
 858                         "failed to setup v4l2 ctrl handler: %d\n", ret);
 859                 return ret;
 860         }
 861
 862         return 0;
 863 }
 864
 865 /* VI only support 2 formats in TPG mode */
 866 static void vi_tpg_fmts_bitmap_init(struct tegra_vi_channel *chan)
 867 {
 868         int index;
 869
 870         bitmap_zero(chan->tpg_fmts_bitmap, MAX_FORMAT_NUM);
 871
 872         index = tegra_get_format_idx_by_code(chan->vi,
 873                                              MEDIA_BUS_FMT_SRGGB10_1X10, 0);
 874         bitmap_set(chan->tpg_fmts_bitmap, index, 1);
 875
 876         index = tegra_get_format_idx_by_code(chan->vi,
 877                                              MEDIA_BUS_FMT_RGB888_1X32_PADHI,
 878                                              0);
 879         bitmap_set(chan->tpg_fmts_bitmap, index, 1);
 880 }
 881
 882 static int vi_fmts_bitmap_init(struct tegra_vi_channel *chan)
 883 {
 884         int index, ret, match_code = 0;
 885         struct v4l2_subdev *subdev;
 886         struct v4l2_subdev_mbus_code_enum code = {
 887                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 888         };
 889
 890         bitmap_zero(chan->fmts_bitmap, MAX_FORMAT_NUM);
 891
 892         /*
 893          * Set the bitmap bits based on all the matched formats between the
 894          * available media bus formats of sub-device and the pre-defined Tegra
 895          * supported video formats.
 896          */
 897         subdev = tegra_channel_get_remote_source_subdev(chan);
 898         while (1) {
 899                 ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
 900                                        NULL, &code);
 901                 if (ret < 0)
 902                         break;
 903
 904                 index = tegra_get_format_idx_by_code(chan->vi, code.code, 0);
 905                 while (index >= 0) {
 906                         bitmap_set(chan->fmts_bitmap, index, 1);
 907                         if (!match_code)
 908                                 match_code = code.code;
 909                         /* look for other formats with same mbus code */
 910                         index = tegra_get_format_idx_by_code(chan->vi,
 911                                                              code.code,
 912                                                              index + 1);
 913                 }
 914
 915                 code.index++;
 916         }
 917
 918         /*
 919          * Set the bitmap bit corresponding to default tegra video format if
 920          * there are no matched formats.
 921          */
 922         if (!match_code) {
 923                 match_code = tegra_default_format.code;
 924                 index = tegra_get_format_idx_by_code(chan->vi, match_code, 0);
 925                 if (WARN_ON(index < 0))
 926                         return -EINVAL;
 927
 928                 bitmap_set(chan->fmts_bitmap, index, 1);
 929         }
 930
 931         /* initialize channel format to the sub-device active format */
 932         tegra_channel_set_subdev_active_fmt(chan);
 933
 934         return 0;
 935 }
 936
 937 static void tegra_channel_cleanup(struct tegra_vi_channel *chan)
 938 {
 939         v4l2_ctrl_handler_free(&chan->ctrl_handler);
 940         media_entity_cleanup(&chan->video.entity);
 941         host1x_syncpt_free(chan->mw_ack_sp);
 942         host1x_syncpt_free(chan->frame_start_sp);
 943         mutex_destroy(&chan->video_lock);
 944 }
 945
 946 void tegra_channels_cleanup(struct tegra_vi *vi)
 947 {
 948         struct tegra_vi_channel *chan, *tmp;
 949
 950         if (!vi)
 951                 return;
 952
 953         list_for_each_entry_safe(chan, tmp, &vi->vi_chans, list) {
 954                 tegra_channel_cleanup(chan);
 955                 list_del(&chan->list);
 956                 kfree(chan);
 957         }
 958 }
 959
 960 static int tegra_channel_init(struct tegra_vi_channel *chan)
 961 {
 962         struct tegra_vi *vi = chan->vi;
 963         struct tegra_video_device *vid = dev_get_drvdata(vi->client.host);
 964         unsigned long flags = HOST1X_SYNCPT_CLIENT_MANAGED;
 965         int ret;
 966
 967         mutex_init(&chan->video_lock);
 968         INIT_LIST_HEAD(&chan->capture);
 969         INIT_LIST_HEAD(&chan->done);
 970         spin_lock_init(&chan->start_lock);
 971         spin_lock_init(&chan->done_lock);
 972         spin_lock_init(&chan->sp_incr_lock);
 973         init_waitqueue_head(&chan->start_wait);
 974         init_waitqueue_head(&chan->done_wait);
 975
 976         /* initialize the video format */
 977         chan->fmtinfo = &tegra_default_format;
 978         chan->format.pixelformat = chan->fmtinfo->fourcc;
 979         chan->format.colorspace = V4L2_COLORSPACE_SRGB;
 980         chan->format.field = V4L2_FIELD_NONE;
 981         chan->format.width = TEGRA_DEF_WIDTH;
 982         chan->format.height = TEGRA_DEF_HEIGHT;
 983         chan->format.bytesperline = TEGRA_DEF_WIDTH * chan->fmtinfo->bpp;
 984         chan->format.sizeimage = chan->format.bytesperline * TEGRA_DEF_HEIGHT;
 985         tegra_channel_fmt_align(chan, &chan->format, chan->fmtinfo->bpp);
 986
 987         chan->frame_start_sp = host1x_syncpt_request(&vi->client, flags);
 988         if (!chan->frame_start_sp) {
 989                 dev_err(vi->dev, "failed to request frame start syncpoint\n");
 990                 return -ENOMEM;
 991         }
 992
 993         chan->mw_ack_sp = host1x_syncpt_request(&vi->client, flags);
 994         if (!chan->mw_ack_sp) {
 995                 dev_err(vi->dev, "failed to request memory ack syncpoint\n");
 996                 ret = -ENOMEM;
 997                 goto free_fs_syncpt;
 998         }
 999
1000         /* initialize the media entity */
1001         chan->pad.flags = MEDIA_PAD_FL_SINK;
1002         ret = media_entity_pads_init(&chan->video.entity, 1, &chan->pad);
1003         if (ret < 0) {
1004                 dev_err(vi->dev,
1005                         "failed to initialize media entity: %d\n", ret);
1006                 goto free_mw_ack_syncpt;
1007         }
1008
1009         ret = v4l2_ctrl_handler_init(&chan->ctrl_handler, MAX_CID_CONTROLS);
1010         if (chan->ctrl_handler.error) {
1011                 dev_err(vi->dev,
1012                         "failed to initialize v4l2 ctrl handler: %d\n", ret);
1013                 goto cleanup_media;
1014         }
1015
1016         /* initialize the video_device */
1017         chan->video.fops = &tegra_channel_fops;
1018         chan->video.v4l2_dev = &vid->v4l2_dev;
1019         chan->video.release = video_device_release_empty;
1020         chan->video.queue = &chan->queue;
1021         snprintf(chan->video.name, sizeof(chan->video.name), "%s-%s-%u",
1022                  dev_name(vi->dev), "output", chan->portno);
1023         chan->video.vfl_type = VFL_TYPE_VIDEO;
1024         chan->video.vfl_dir = VFL_DIR_RX;
1025         chan->video.ioctl_ops = &tegra_channel_ioctl_ops;
1026         chan->video.ctrl_handler = &chan->ctrl_handler;
1027         chan->video.lock = &chan->video_lock;
1028         chan->video.device_caps = V4L2_CAP_VIDEO_CAPTURE |
1029                                   V4L2_CAP_STREAMING |
1030                                   V4L2_CAP_READWRITE;
1031         video_set_drvdata(&chan->video, chan);
1032
1033         chan->queue.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1034         chan->queue.io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ;
1035         chan->queue.lock = &chan->video_lock;
1036         chan->queue.drv_priv = chan;
1037         chan->queue.buf_struct_size = sizeof(struct tegra_channel_buffer);
1038         chan->queue.ops = &tegra_channel_queue_qops;
1039         chan->queue.mem_ops = &vb2_dma_contig_memops;
1040         chan->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1041         chan->queue.min_buffers_needed = 2;
1042         chan->queue.dev = vi->dev;
1043         ret = vb2_queue_init(&chan->queue);
1044         if (ret < 0) {
1045                 dev_err(vi->dev, "failed to initialize vb2 queue: %d\n", ret);
1046                 goto free_v4l2_ctrl_hdl;
1047         }
1048
1049         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
1050                 v4l2_async_notifier_init(&chan->notifier);
1051
1052         return 0;
1053
1054 free_v4l2_ctrl_hdl:
1055         v4l2_ctrl_handler_free(&chan->ctrl_handler);
1056 cleanup_media:
1057         media_entity_cleanup(&chan->video.entity);
1058 free_mw_ack_syncpt:
1059         host1x_syncpt_free(chan->mw_ack_sp);
1060 free_fs_syncpt:
1061         host1x_syncpt_free(chan->frame_start_sp);
1062         return ret;
1063 }
1064
1065 static int tegra_vi_channel_alloc(struct tegra_vi *vi, unsigned int port_num,
1066                                   struct device_node *node)
1067 {
1068         struct tegra_vi_channel *chan;
1069
1070         /*
1071          * Do not use devm_kzalloc as memory is freed immediately
1072          * when device instance is unbound but application might still
1073          * be holding the device node open. Channel memory allocated
1074          * with kzalloc is freed during video device release callback.
1075          */
1076         chan = kzalloc(sizeof(*chan), GFP_KERNEL);
1077         if (!chan)
1078                 return -ENOMEM;
1079
1080         chan->vi = vi;
1081         chan->portno = port_num;
1082         chan->of_node = node;
1083         list_add_tail(&chan->list, &vi->vi_chans);
1084
1085         return 0;
1086 }
1087
1088 static int tegra_vi_tpg_channels_alloc(struct tegra_vi *vi)
1089 {
1090         unsigned int port_num;
1091         unsigned int nchannels = vi->soc->vi_max_channels;
1092         int ret;
1093
1094         for (port_num = 0; port_num < nchannels; port_num++) {
1095                 ret = tegra_vi_channel_alloc(vi, port_num, vi->dev->of_node);
1096                 if (ret < 0)
1097                         return ret;
1098         }
1099
1100         return 0;
1101 }
1102
1103 static int tegra_vi_channels_alloc(struct tegra_vi *vi)
1104 {
1105         struct device_node *node = vi->dev->of_node;
1106         struct device_node *ep = NULL;
1107         struct device_node *ports;
1108         struct device_node *port;
1109         unsigned int port_num;
1110         int ret = 0;
1111
1112         ports = of_get_child_by_name(node, "ports");
1113         if (!ports)
1114                 return -ENODEV;
1115
1116         for_each_child_of_node(ports, port) {
1117                 if (!of_node_name_eq(port, "port"))
1118                         continue;
1119
1120                 ret = of_property_read_u32(port, "reg", &port_num);
1121                 if (ret < 0)
1122                         continue;
1123
1124                 if (port_num > vi->soc->vi_max_channels) {
1125                         dev_err(vi->dev, "invalid port num %d for %pOF\n",
1126                                 port_num, port);
1127                         ret = -EINVAL;
1128                         of_node_put(port);
1129                         goto cleanup;
1130                 }
1131
1132                 ep = of_get_child_by_name(port, "endpoint");
1133                 if (!ep)
1134                         continue;
1135
1136                 of_node_put(ep);
1137                 ret = tegra_vi_channel_alloc(vi, port_num, port);
1138                 if (ret < 0) {
1139                         of_node_put(port);
1140                         goto cleanup;
1141                 }
1142         }
1143
1144 cleanup:
1145         of_node_put(ports);
1146         return ret;
1147 }
1148
1149 static int tegra_vi_channels_init(struct tegra_vi *vi)
1150 {
1151         struct tegra_vi_channel *chan;
1152         int ret;
1153
1154         list_for_each_entry(chan, &vi->vi_chans, list) {
1155                 ret = tegra_channel_init(chan);
1156                 if (ret < 0) {
1157                         dev_err(vi->dev,
1158                                 "failed to initialize channel-%d: %d\n",
1159                                 chan->portno, ret);
1160                         goto cleanup;
1161                 }
1162         }
1163
1164         return 0;
1165
1166 cleanup:
1167         list_for_each_entry_continue_reverse(chan, &vi->vi_chans, list)
1168                 tegra_channel_cleanup(chan);
1169
1170         return ret;
1171 }
1172
1173 void tegra_v4l2_nodes_cleanup_tpg(struct tegra_video_device *vid)
1174 {
1175         struct tegra_vi *vi = vid->vi;
1176         struct tegra_csi *csi = vid->csi;
1177         struct tegra_csi_channel *csi_chan;
1178         struct tegra_vi_channel *chan;
1179
1180         list_for_each_entry(chan, &vi->vi_chans, list)
1181                 vb2_video_unregister_device(&chan->video);
1182
1183         list_for_each_entry(csi_chan, &csi->csi_chans, list)
1184                 v4l2_device_unregister_subdev(&csi_chan->subdev);
1185 }
1186
1187 int tegra_v4l2_nodes_setup_tpg(struct tegra_video_device *vid)
1188 {
1189         struct tegra_vi *vi = vid->vi;
1190         struct tegra_csi *csi = vid->csi;
1191         struct tegra_vi_channel *vi_chan;
1192         struct tegra_csi_channel *csi_chan;
1193         u32 link_flags = MEDIA_LNK_FL_ENABLED;
1194         int ret;
1195
1196         if (!vi || !csi)
1197                 return -ENODEV;
1198
1199         csi_chan = list_first_entry(&csi->csi_chans,
1200                                     struct tegra_csi_channel, list);
1201
1202         list_for_each_entry(vi_chan, &vi->vi_chans, list) {
1203                 struct media_entity *source = &csi_chan->subdev.entity;
1204                 struct media_entity *sink = &vi_chan->video.entity;
1205                 struct media_pad *source_pad = csi_chan->pads;
1206                 struct media_pad *sink_pad = &vi_chan->pad;
1207
1208                 ret = v4l2_device_register_subdev(&vid->v4l2_dev,
1209                                                   &csi_chan->subdev);
1210                 if (ret) {
1211                         dev_err(vi->dev,
1212                                 "failed to register subdev: %d\n", ret);
1213                         goto cleanup;
1214                 }
1215
1216                 ret = video_register_device(&vi_chan->video,
1217                                             VFL_TYPE_VIDEO, -1);
1218                 if (ret < 0) {
1219                         dev_err(vi->dev,
1220                                 "failed to register video device: %d\n", ret);
1221                         goto cleanup;
1222                 }
1223
1224                 dev_dbg(vi->dev, "creating %s:%u -> %s:%u link\n",
1225                         source->name, source_pad->index,
1226                         sink->name, sink_pad->index);
1227
1228                 ret = media_create_pad_link(source, source_pad->index,
1229                                             sink, sink_pad->index,
1230                                             link_flags);
1231                 if (ret < 0) {
1232                         dev_err(vi->dev,
1233                                 "failed to create %s:%u -> %s:%u link: %d\n",
1234                                 source->name, source_pad->index,
1235                                 sink->name, sink_pad->index, ret);
1236                         goto cleanup;
1237                 }
1238
1239                 ret = tegra_channel_setup_ctrl_handler(vi_chan);
1240                 if (ret < 0)
1241                         goto cleanup;
1242
1243                 v4l2_set_subdev_hostdata(&csi_chan->subdev, vi_chan);
1244                 vi_tpg_fmts_bitmap_init(vi_chan);
1245                 csi_chan = list_next_entry(csi_chan, list);
1246         }
1247
1248         return 0;
1249
1250 cleanup:
1251         tegra_v4l2_nodes_cleanup_tpg(vid);
1252         return ret;
1253 }
1254
1255 static int __maybe_unused vi_runtime_resume(struct device *dev)
1256 {
1257         struct tegra_vi *vi = dev_get_drvdata(dev);
1258         int ret;
1259
1260         ret = regulator_enable(vi->vdd);
1261         if (ret) {
1262                 dev_err(dev, "failed to enable VDD supply: %d\n", ret);
1263                 return ret;
1264         }
1265
1266         ret = clk_set_rate(vi->clk, vi->soc->vi_max_clk_hz);
1267         if (ret) {
1268                 dev_err(dev, "failed to set vi clock rate: %d\n", ret);
1269                 goto disable_vdd;
1270         }
1271
1272         ret = clk_prepare_enable(vi->clk);
1273         if (ret) {
1274                 dev_err(dev, "failed to enable vi clock: %d\n", ret);
1275                 goto disable_vdd;
1276         }
1277
1278         return 0;
1279
1280 disable_vdd:
1281         regulator_disable(vi->vdd);
1282         return ret;
1283 }
1284
1285 static int __maybe_unused vi_runtime_suspend(struct device *dev)
1286 {
1287         struct tegra_vi *vi = dev_get_drvdata(dev);
1288
1289         clk_disable_unprepare(vi->clk);
1290
1291         regulator_disable(vi->vdd);
1292
1293         return 0;
1294 }
1295
1296 /*
1297  * Graph Management
1298  */
1299 static struct tegra_vi_graph_entity *
1300 tegra_vi_graph_find_entity(struct tegra_vi_channel *chan,
1301                            const struct fwnode_handle *fwnode)
1302 {
1303         struct tegra_vi_graph_entity *entity;
1304         struct v4l2_async_subdev *asd;
1305
1306         list_for_each_entry(asd, &chan->notifier.asd_list, asd_list) {
1307                 entity = to_tegra_vi_graph_entity(asd);
1308                 if (entity->asd.match.fwnode == fwnode)
1309                         return entity;
1310         }
1311
1312         return NULL;
1313 }
1314
1315 static int tegra_vi_graph_build(struct tegra_vi_channel *chan,
1316                                 struct tegra_vi_graph_entity *entity)
1317 {
1318         struct tegra_vi *vi = chan->vi;
1319         struct tegra_vi_graph_entity *ent;
1320         struct fwnode_handle *ep = NULL;
1321         struct v4l2_fwnode_link link;
1322         struct media_entity *local = entity->entity;
1323         struct media_entity *remote;
1324         struct media_pad *local_pad;
1325         struct media_pad *remote_pad;
1326         u32 link_flags = MEDIA_LNK_FL_ENABLED;
1327         int ret = 0;
1328
1329         dev_dbg(vi->dev, "creating links for entity %s\n", local->name);
1330
1331         while (1) {
1332                 ep = fwnode_graph_get_next_endpoint(entity->asd.match.fwnode,
1333                                                     ep);
1334                 if (!ep)
1335                         break;
1336
1337                 ret = v4l2_fwnode_parse_link(ep, &link);
1338                 if (ret < 0) {
1339                         dev_err(vi->dev, "failed to parse link for %pOF: %d\n",
1340                                 to_of_node(ep), ret);
1341                         continue;
1342                 }
1343
1344                 if (link.local_port >= local->num_pads) {
1345                         dev_err(vi->dev, "invalid port number %u on %pOF\n",
1346                                 link.local_port, to_of_node(link.local_node));
1347                         v4l2_fwnode_put_link(&link);
1348                         ret = -EINVAL;
1349                         break;
1350                 }
1351
1352                 local_pad = &local->pads[link.local_port];
1353                 /* Remote node is vi node. So use channel video entity and pad
1354                  * as remote/sink.
1355                  */
1356                 if (link.remote_node == of_fwnode_handle(vi->dev->of_node)) {
1357                         remote = &chan->video.entity;
1358                         remote_pad = &chan->pad;
1359                         goto create_link;
1360                 }
1361
1362                 /*
1363                  * Skip sink ports, they will be processed from the other end
1364                  * of the link.
1365                  */
1366                 if (local_pad->flags & MEDIA_PAD_FL_SINK) {
1367                         dev_dbg(vi->dev, "skipping sink port %pOF:%u\n",
1368                                 to_of_node(link.local_node), link.local_port);
1369                         v4l2_fwnode_put_link(&link);
1370                         continue;
1371                 }
1372
1373                 /* find the remote entity from notifier list */
1374                 ent = tegra_vi_graph_find_entity(chan, link.remote_node);
1375                 if (!ent) {
1376                         dev_err(vi->dev, "no entity found for %pOF\n",
1377                                 to_of_node(link.remote_node));
1378                         v4l2_fwnode_put_link(&link);
1379                         ret = -ENODEV;
1380                         break;
1381                 }
1382
1383                 remote = ent->entity;
1384                 if (link.remote_port >= remote->num_pads) {
1385                         dev_err(vi->dev, "invalid port number %u on %pOF\n",
1386                                 link.remote_port,
1387                                 to_of_node(link.remote_node));
1388                         v4l2_fwnode_put_link(&link);
1389                         ret = -EINVAL;
1390                         break;
1391                 }
1392
1393                 remote_pad = &remote->pads[link.remote_port];
1394
1395 create_link:
1396                 dev_dbg(vi->dev, "creating %s:%u -> %s:%u link\n",
1397                         local->name, local_pad->index,
1398                         remote->name, remote_pad->index);
1399
1400                 ret = media_create_pad_link(local, local_pad->index,
1401                                             remote, remote_pad->index,
1402                                             link_flags);
1403                 v4l2_fwnode_put_link(&link);
1404                 if (ret < 0) {
1405                         dev_err(vi->dev,
1406                                 "failed to create %s:%u -> %s:%u link: %d\n",
1407                                 local->name, local_pad->index,
1408                                 remote->name, remote_pad->index, ret);
1409                         break;
1410                 }
1411         }
1412
1413         fwnode_handle_put(ep);
1414         return ret;
1415 }
1416
1417 static int tegra_vi_graph_notify_complete(struct v4l2_async_notifier *notifier)
1418 {
1419         struct tegra_vi_graph_entity *entity;
1420         struct v4l2_async_subdev *asd;
1421         struct v4l2_subdev *subdev;
1422         struct tegra_vi_channel *chan;
1423         struct tegra_vi *vi;
1424         int ret;
1425
1426         chan = container_of(notifier, struct tegra_vi_channel, notifier);
1427         vi = chan->vi;
1428
1429         dev_dbg(vi->dev, "notify complete, all subdevs registered\n");
1430
1431         /*
1432          * Video device node should be created at the end of all the device
1433          * related initialization/setup.
1434          * Current video_register_device() does both initialize and register
1435          * video device in same API.
1436          *
1437          * TODO: Update v4l2-dev driver to split initialize and register into
1438          * separate APIs and then update Tegra video driver to do video device
1439          * initialize followed by all video device related setup and then
1440          * register the video device.
1441          */
1442         ret = video_register_device(&chan->video, VFL_TYPE_VIDEO, -1);
1443         if (ret < 0) {
1444                 dev_err(vi->dev,
1445                         "failed to register video device: %d\n", ret);
1446                 goto unregister_video;
1447         }
1448
1449         /* create links between the entities */
1450         list_for_each_entry(asd, &chan->notifier.asd_list, asd_list) {
1451                 entity = to_tegra_vi_graph_entity(asd);
1452                 ret = tegra_vi_graph_build(chan, entity);
1453                 if (ret < 0)
1454                         goto unregister_video;
1455         }
1456
1457         ret = tegra_channel_setup_ctrl_handler(chan);
1458         if (ret < 0) {
1459                 dev_err(vi->dev,
1460                         "failed to setup channel controls: %d\n", ret);
1461                 goto unregister_video;
1462         }
1463
1464         ret = vi_fmts_bitmap_init(chan);
1465         if (ret < 0) {
1466                 dev_err(vi->dev,
1467                         "failed to initialize formats bitmap: %d\n", ret);
1468                 goto unregister_video;
1469         }
1470
1471         subdev = tegra_channel_get_remote_csi_subdev(chan);
1472         if (!subdev) {
1473                 ret = -ENODEV;
1474                 dev_err(vi->dev,
1475                         "failed to get remote csi subdev: %d\n", ret);
1476                 goto unregister_video;
1477         }
1478
1479         v4l2_set_subdev_hostdata(subdev, chan);
1480
1481         return 0;
1482
1483 unregister_video:
1484         vb2_video_unregister_device(&chan->video);
1485         return ret;
1486 }
1487
1488 static int tegra_vi_graph_notify_bound(struct v4l2_async_notifier *notifier,
1489                                        struct v4l2_subdev *subdev,
1490                                        struct v4l2_async_subdev *asd)
1491 {
1492         struct tegra_vi_graph_entity *entity;
1493         struct tegra_vi *vi;
1494         struct tegra_vi_channel *chan;
1495
1496         chan = container_of(notifier, struct tegra_vi_channel, notifier);
1497         vi = chan->vi;
1498
1499         /*
1500          * Locate the entity corresponding to the bound subdev and store the
1501          * subdev pointer.
1502          */
1503         entity = tegra_vi_graph_find_entity(chan, subdev->fwnode);
1504         if (!entity) {
1505                 dev_err(vi->dev, "no entity for subdev %s\n", subdev->name);
1506                 return -EINVAL;
1507         }
1508
1509         if (entity->subdev) {
1510                 dev_err(vi->dev, "duplicate subdev for node %pOF\n",
1511                         to_of_node(entity->asd.match.fwnode));
1512                 return -EINVAL;
1513         }
1514
1515         dev_dbg(vi->dev, "subdev %s bound\n", subdev->name);
1516         entity->entity = &subdev->entity;
1517         entity->subdev = subdev;
1518
1519         return 0;
1520 }
1521
1522 static const struct v4l2_async_notifier_operations tegra_vi_async_ops = {
1523         .bound = tegra_vi_graph_notify_bound,
1524         .complete = tegra_vi_graph_notify_complete,
1525 };
1526
1527 static int tegra_vi_graph_parse_one(struct tegra_vi_channel *chan,
1528                                     struct fwnode_handle *fwnode)
1529 {
1530         struct tegra_vi *vi = chan->vi;
1531         struct fwnode_handle *ep = NULL;
1532         struct fwnode_handle *remote = NULL;
1533         struct v4l2_async_subdev *asd;
1534         struct device_node *node = NULL;
1535         int ret;
1536
1537         dev_dbg(vi->dev, "parsing node %pOF\n", to_of_node(fwnode));
1538
1539         /* parse all the remote entities and put them into the list */
1540         for_each_endpoint_of_node(to_of_node(fwnode), node) {
1541                 ep = of_fwnode_handle(node);
1542                 remote = fwnode_graph_get_remote_port_parent(ep);
1543                 if (!remote) {
1544                         dev_err(vi->dev,
1545                                 "remote device at %pOF not found\n", node);
1546                         ret = -EINVAL;
1547                         goto cleanup;
1548                 }
1549
1550                 /* skip entities that are already processed */
1551                 if (remote == dev_fwnode(vi->dev) ||
1552                     tegra_vi_graph_find_entity(chan, remote)) {
1553                         fwnode_handle_put(remote);
1554                         continue;
1555                 }
1556
1557                 asd = v4l2_async_notifier_add_fwnode_subdev(&chan->notifier,
1558                                 remote, sizeof(struct tegra_vi_graph_entity));
1559                 if (IS_ERR(asd)) {
1560                         ret = PTR_ERR(asd);
1561                         dev_err(vi->dev,
1562                                 "failed to add subdev to notifier: %d\n", ret);
1563                         fwnode_handle_put(remote);
1564                         goto cleanup;
1565                 }
1566
1567                 ret = tegra_vi_graph_parse_one(chan, remote);
1568                 if (ret < 0) {
1569                         fwnode_handle_put(remote);
1570                         goto cleanup;
1571                 }
1572
1573                 fwnode_handle_put(remote);
1574         }
1575
1576         return 0;
1577
1578 cleanup:
1579         dev_err(vi->dev, "failed parsing the graph: %d\n", ret);
1580         v4l2_async_notifier_cleanup(&chan->notifier);
1581         of_node_put(node);
1582         return ret;
1583 }
1584
1585 static int tegra_vi_graph_init(struct tegra_vi *vi)
1586 {
1587         struct tegra_video_device *vid = dev_get_drvdata(vi->client.host);
1588         struct tegra_vi_channel *chan;
1589         struct fwnode_handle *fwnode = dev_fwnode(vi->dev);
1590         int ret;
1591         struct fwnode_handle *remote = NULL;
1592
1593         /*
1594          * Walk the links to parse the full graph. Each channel will have
1595          * one endpoint of the composite node. Start by parsing the
1596          * composite node and parse the remote entities in turn.
1597          * Each channel will register v4l2 async notifier to make the graph
1598          * independent between the channels so we can the current channel
1599          * in case of something wrong during graph parsing and continue with
1600          * next channels.
1601          */
1602         list_for_each_entry(chan, &vi->vi_chans, list) {
1603                 remote = fwnode_graph_get_remote_node(fwnode, chan->portno, 0);
1604                 if (!remote)
1605                         continue;
1606
1607                 ret = tegra_vi_graph_parse_one(chan, remote);
1608                 fwnode_handle_put(remote);
1609                 if (ret < 0 || list_empty(&chan->notifier.asd_list))
1610                         continue;
1611
1612                 chan->notifier.ops = &tegra_vi_async_ops;
1613                 ret = v4l2_async_notifier_register(&vid->v4l2_dev,
1614                                                    &chan->notifier);
1615                 if (ret < 0) {
1616                         dev_err(vi->dev,
1617                                 "failed to register channel %d notifier: %d\n",
1618                                 chan->portno, ret);
1619                         v4l2_async_notifier_cleanup(&chan->notifier);
1620                 }
1621         }
1622
1623         return 0;
1624 }
1625
1626 static void tegra_vi_graph_cleanup(struct tegra_vi *vi)
1627 {
1628         struct tegra_vi_channel *chan;
1629
1630         list_for_each_entry(chan, &vi->vi_chans, list) {
1631                 vb2_video_unregister_device(&chan->video);
1632                 v4l2_async_notifier_unregister(&chan->notifier);
1633                 v4l2_async_notifier_cleanup(&chan->notifier);
1634         }
1635 }
1636
1637 static int tegra_vi_init(struct host1x_client *client)
1638 {
1639         struct tegra_video_device *vid = dev_get_drvdata(client->host);
1640         struct tegra_vi *vi = host1x_client_to_vi(client);
1641         struct tegra_vi_channel *chan, *tmp;
1642         int ret;
1643
1644         vid->media_dev.hw_revision = vi->soc->hw_revision;
1645         snprintf(vid->media_dev.bus_info, sizeof(vid->media_dev.bus_info),
1646                  "platform:%s", dev_name(vi->dev));
1647
1648         INIT_LIST_HEAD(&vi->vi_chans);
1649
1650         if (IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
1651                 ret = tegra_vi_tpg_channels_alloc(vi);
1652         else
1653                 ret = tegra_vi_channels_alloc(vi);
1654         if (ret < 0) {
1655                 dev_err(vi->dev,
1656                         "failed to allocate vi channels: %d\n", ret);
1657                 goto free_chans;
1658         }
1659
1660         ret = tegra_vi_channels_init(vi);
1661         if (ret < 0)
1662                 goto free_chans;
1663
1664         vid->vi = vi;
1665
1666         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG)) {
1667                 ret = tegra_vi_graph_init(vi);
1668                 if (ret < 0)
1669                         goto free_chans;
1670         }
1671
1672         return 0;
1673
1674 free_chans:
1675         list_for_each_entry_safe(chan, tmp, &vi->vi_chans, list) {
1676                 list_del(&chan->list);
1677                 kfree(chan);
1678         }
1679
1680         return ret;
1681 }
1682
1683 static int tegra_vi_exit(struct host1x_client *client)
1684 {
1685         struct tegra_vi *vi = host1x_client_to_vi(client);
1686
1687         /*
1688          * Do not cleanup the channels here as application might still be
1689          * holding video device nodes. Channels cleanup will happen during
1690          * v4l2_device release callback which gets called after all video
1691          * device nodes are released.
1692          */
1693
1694         if (!IS_ENABLED(CONFIG_VIDEO_TEGRA_TPG))
1695                 tegra_vi_graph_cleanup(vi);
1696
1697         return 0;
1698 }
1699
1700 static const struct host1x_client_ops vi_client_ops = {
1701         .init = tegra_vi_init,
1702         .exit = tegra_vi_exit,
1703 };
1704
1705 static int tegra_vi_probe(struct platform_device *pdev)
1706 {
1707         struct tegra_vi *vi;
1708         int ret;
1709
1710         vi = devm_kzalloc(&pdev->dev, sizeof(*vi), GFP_KERNEL);
1711         if (!vi)
1712                 return -ENOMEM;
1713
1714         vi->iomem = devm_platform_ioremap_resource(pdev, 0);
1715         if (IS_ERR(vi->iomem))
1716                 return PTR_ERR(vi->iomem);
1717
1718         vi->soc = of_device_get_match_data(&pdev->dev);
1719
1720         vi->clk = devm_clk_get(&pdev->dev, NULL);
1721         if (IS_ERR(vi->clk)) {
1722                 ret = PTR_ERR(vi->clk);
1723                 dev_err(&pdev->dev, "failed to get vi clock: %d\n", ret);
1724                 return ret;
1725         }
1726
1727         vi->vdd = devm_regulator_get(&pdev->dev, "avdd-dsi-csi");
1728         if (IS_ERR(vi->vdd)) {
1729                 ret = PTR_ERR(vi->vdd);
1730                 dev_err(&pdev->dev, "failed to get VDD supply: %d\n", ret);
1731                 return ret;
1732         }
1733
1734         if (!pdev->dev.pm_domain) {
1735                 ret = -ENOENT;
1736                 dev_warn(&pdev->dev, "PM domain is not attached: %d\n", ret);
1737                 return ret;
1738         }
1739
1740         ret = devm_of_platform_populate(&pdev->dev);
1741         if (ret < 0) {
1742                 dev_err(&pdev->dev,
1743                         "failed to populate vi child device: %d\n", ret);
1744                 return ret;
1745         }
1746
1747         vi->dev = &pdev->dev;
1748         vi->ops = vi->soc->ops;
1749         platform_set_drvdata(pdev, vi);
1750         pm_runtime_enable(&pdev->dev);
1751
1752         /* initialize host1x interface */
1753         INIT_LIST_HEAD(&vi->client.list);
1754         vi->client.ops = &vi_client_ops;
1755         vi->client.dev = &pdev->dev;
1756
1757         ret = host1x_client_register(&vi->client);
1758         if (ret < 0) {
1759                 dev_err(&pdev->dev,
1760                         "failed to register host1x client: %d\n", ret);
1761                 goto rpm_disable;
1762         }
1763
1764         return 0;
1765
1766 rpm_disable:
1767         pm_runtime_disable(&pdev->dev);
1768         return ret;
1769 }
1770
1771 static int tegra_vi_remove(struct platform_device *pdev)
1772 {
1773         struct tegra_vi *vi = platform_get_drvdata(pdev);
1774         int err;
1775
1776         err = host1x_client_unregister(&vi->client);
1777         if (err < 0) {
1778                 dev_err(&pdev->dev,
1779                         "failed to unregister host1x client: %d\n", err);
1780                 return err;
1781         }
1782
1783         pm_runtime_disable(&pdev->dev);
1784
1785         return 0;
1786 }
1787
1788 static const struct of_device_id tegra_vi_of_id_table[] = {
1789 #if defined(CONFIG_ARCH_TEGRA_210_SOC)
1790         { .compatible = "nvidia,tegra210-vi", .data = &tegra210_vi_soc },
1791 #endif
1792         { }
1793 };
1794 MODULE_DEVICE_TABLE(of, tegra_vi_of_id_table);
1795
1796 static const struct dev_pm_ops tegra_vi_pm_ops = {
1797         SET_RUNTIME_PM_OPS(vi_runtime_suspend, vi_runtime_resume, NULL)
1798 };
1799
1800 struct platform_driver tegra_vi_driver = {
1801         .driver = {
1802                 .name = "tegra-vi",
1803                 .of_match_table = tegra_vi_of_id_table,
1804                 .pm = &tegra_vi_pm_ops,
1805         },
1806         .probe = tegra_vi_probe,
1807         .remove = tegra_vi_remove,
1808 };