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

   1 /*
   2  * Tegra Video Input 4 device common APIs
   3  *
   4  * Copyright (c) 2016-2018, NVIDIA CORPORATION.  All rights reserved.
   5  *
   6  * Author: Frank Chen <frank@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 #include <linux/nvhost.h>
  13 #include <linux/tegra-powergate.h>
  14 #include <linux/kthread.h>
  15 #include <linux/freezer.h>
  16 #include <media/tegra_camera_platform.h>
  17 #include "nvhost_acm.h"
  18 #include "linux/nvhost_ioctl.h"
  19 #include "vi/vi4.h"
  20 #include "mc_common.h"
  21 #include "vi4_registers.h"
  22 #include "vi4_formats.h"
  23 #include "vi/vi_notify.h"
  24 #include <media/sensor_common.h>
  25
  26 #define DEFAULT_FRAMERATE       30
  27 #define BPP_MEM         2
  28 #define MAX_VI_CHANNEL 12
  29 #define NUM_PPC         8
  30 #define VI_CSI_CLK_SCALE        110
  31 #define SOF_SYNCPT_IDX  0
  32 #define FE_SYNCPT_IDX   1
  33 #define PG_BITRATE      32
  34
  35 extern void tegra_channel_queued_buf_done(struct tegra_channel *chan,
  36                                           enum vb2_buffer_state state);
  37 extern int tegra_channel_set_stream(struct tegra_channel *chan, bool on);
  38 extern void enqueue_inflight(struct tegra_channel *chan,
  39                         struct tegra_channel_buffer *buf);
  40 extern struct tegra_channel_buffer *dequeue_inflight(
  41                         struct tegra_channel *chan);
  42
  43 extern struct tegra_channel_buffer *dequeue_buffer(struct tegra_channel *chan);
  44 extern int tegra_channel_set_power(struct tegra_channel *chan, bool on);
  45 static void tegra_channel_stop_kthreads(struct tegra_channel *chan);
  46 static int tegra_channel_stop_increments(struct tegra_channel *chan);
  47 static void tegra_channel_notify_status_callback(
  48                                 struct vi_notify_channel *,
  49                                 const struct vi_capture_status *,
  50                                 void *);
  51 static void tegra_channel_error_worker(struct work_struct *status_work);
  52 static void tegra_channel_notify_error_callback(void *);
  53 extern void release_buffer(struct tegra_channel *chan,
  54                                 struct tegra_channel_buffer *buf);
  55
  56 u32 csimux_config_stream[] = {
  57         CSIMUX_CONFIG_STREAM_0,
  58         CSIMUX_CONFIG_STREAM_1,
  59         CSIMUX_CONFIG_STREAM_2,
  60         CSIMUX_CONFIG_STREAM_3,
  61         CSIMUX_CONFIG_STREAM_4,
  62         CSIMUX_CONFIG_STREAM_5
  63 };
  64
  65 static void vi4_write(struct tegra_channel *chan, unsigned int addr, u32 val)
  66 {
  67         writel(val, chan->vi->iomem + addr);
  68 }
  69
  70 static u32 vi4_read(struct tegra_channel *chan, unsigned int addr)
  71 {
  72         return readl(chan->vi->iomem + addr);
  73 }
  74
  75 static void vi4_channel_write(struct tegra_channel *chan,
  76                 unsigned int index, unsigned int addr, u32 val)
  77 {
  78         writel(val,
  79                 chan->vi->iomem + VI4_CHANNEL_OFFSET * (index + 1) + addr);
  80 }
  81
  82 void vi4_init_video_formats(struct tegra_channel *chan)
  83 {
  84         int i;
  85
  86         chan->num_video_formats = ARRAY_SIZE(vi4_video_formats);
  87         for (i = 0; i < chan->num_video_formats; i++)
  88                 chan->video_formats[i] = &vi4_video_formats[i];
  89 }
  90
  91 int tegra_vi4_s_ctrl(struct v4l2_ctrl *ctrl)
  92 {
  93         struct tegra_channel *chan = container_of(ctrl->handler,
  94                                 struct tegra_channel, ctrl_handler);
  95         int err = 0;
  96
  97         switch (ctrl->id) {
  98         case TEGRA_CAMERA_CID_WRITE_ISPFORMAT:
  99                 chan->write_ispformat = ctrl->val;
 100                 break;
 101         default:
 102                 dev_err(&chan->video.dev, "%s:Not valid ctrl\n", __func__);
 103                 return -EINVAL;
 104         }
 105
 106         return err;
 107 }
 108
 109 static const struct v4l2_ctrl_ops vi4_ctrl_ops = {
 110         .s_ctrl = tegra_channel_s_ctrl,
 111 };
 112
 113 static const struct v4l2_ctrl_config vi4_custom_ctrls[] = {
 114         {
 115                 .ops = &vi4_ctrl_ops,
 116                 .id = TEGRA_CAMERA_CID_WRITE_ISPFORMAT,
 117                 .name = "Write ISP format",
 118                 .type = V4L2_CTRL_TYPE_INTEGER,
 119                 .def = 1,
 120                 .min = 1,
 121                 .max = 1,
 122                 .step = 1,
 123         },
 124 };
 125
 126 int vi4_add_ctrls(struct tegra_channel *chan)
 127 {
 128         int i;
 129
 130         /* Add vi4 custom controls */
 131         for (i = 0; i < ARRAY_SIZE(vi4_custom_ctrls); i++) {
 132                 v4l2_ctrl_new_custom(&chan->ctrl_handler,
 133                         &vi4_custom_ctrls[i], NULL);
 134                 if (chan->ctrl_handler.error) {
 135                         dev_err(chan->vi->dev,
 136                                 "Failed to add %s ctrl\n",
 137                                 vi4_custom_ctrls[i].name);
 138                         return chan->ctrl_handler.error;
 139                 }
 140         }
 141
 142         return 0;
 143 }
 144
 145 static bool vi4_init(struct tegra_channel *chan)
 146 {
 147         vi4_write(chan, NOTIFY_ERROR, 0x1);
 148         vi4_write(chan, NOTIFY_TAG_CLASSIFY_0, 0xe39c08e3);
 149         return true;
 150 }
 151
 152 static bool vi4_check_status(struct tegra_channel *chan)
 153 {
 154         int status;
 155
 156         /* check interrupt status error */
 157         status = vi4_read(chan, CFG_INTERRUPT_STATUS);
 158         if (status & 0x1)
 159                 dev_err(chan->vi->dev,
 160                         "VI_CFG_INTERRUPT_STATUS_0: MASTER_ERR_STATUS error!\n");
 161
 162         /* Check VI NOTIFY input FIFO error */
 163         status = vi4_read(chan, NOTIFY_ERROR);
 164         if (status & 0x1)
 165                 dev_err(chan->vi->dev,
 166                         "VI_NOTIFY_ERROR_0: NOTIFY_FIFO_OVERFLOW error!\n");
 167
 168         return true;
 169 }
 170
 171 static bool vi_notify_wait(struct tegra_channel *chan,
 172                 struct timespec *ts)
 173 {
 174         int i, err;
 175         u32 thresh[TEGRA_CSI_BLOCKS], temp;
 176
 177         /*
 178          * Increment syncpt for ATOMP_FE
 179          *
 180          * This is needed in order to keep the syncpt max up to date,
 181          * even if we are not waiting for ATOMP_FE here
 182          */
 183         for (i = 0; i < chan->valid_ports; i++)
 184                 temp = nvhost_syncpt_incr_max_ext(chan->vi->ndev,
 185                                         chan->syncpt[i][FE_SYNCPT_IDX], 1);
 186
 187         /*
 188          * Increment syncpt for PXL_SOF
 189          *
 190          * Increment and retrieve PXL_SOF syncpt max value.
 191          * This value will be used to wait for next syncpt
 192          */
 193         for (i = 0; i < chan->valid_ports; i++)
 194                 thresh[i] = nvhost_syncpt_incr_max_ext(chan->vi->ndev,
 195                                         chan->syncpt[i][SOF_SYNCPT_IDX], 1);
 196
 197         /*
 198          * Wait for PXL_SOF syncpt
 199          *
 200          * Use the syncpt max value we just set as threshold
 201          */
 202         for (i = 0; i < chan->valid_ports; i++) {
 203                 err = nvhost_syncpt_wait_timeout_ext(chan->vi->ndev,
 204                                 chan->syncpt[i][SOF_SYNCPT_IDX], thresh[i],
 205                                 250, NULL, NULL);
 206                 if (unlikely(err))
 207                         dev_err(chan->vi->dev,
 208                                 "PXL_SOF syncpt timeout! err = %d\n", err);
 209                 else {
 210                         struct vi_capture_status status;
 211
 212                         err = vi_notify_get_capture_status(chan->vnc[i],
 213                                         chan->vnc_id[i],
 214                                         thresh[i], &status);
 215                         if (unlikely(err))
 216                                 dev_err(chan->vi->dev,
 217                                         "no capture status! err = %d\n", err);
 218                         else
 219                                 *ts = ns_to_timespec((s64)status.sof_ts);
 220                 }
 221         }
 222         return true;
 223 }
 224
 225 static void tegra_channel_surface_setup(
 226         struct tegra_channel *chan, struct tegra_channel_buffer *buf, int index)
 227 {
 228         int vnc_id = chan->vnc_id[index];
 229         unsigned int offset = chan->buffer_offset[index];
 230
 231         if (chan->embedded_data_height > 0)
 232                 vi4_channel_write(chan, vnc_id, ATOMP_EMB_SURFACE_OFFSET0,
 233                                                   chan->vi->emb_buf);
 234         else
 235                 vi4_channel_write(chan, vnc_id, ATOMP_EMB_SURFACE_OFFSET0, 0);
 236         vi4_channel_write(chan, vnc_id, ATOMP_EMB_SURFACE_OFFSET0_H, 0x0);
 237         vi4_channel_write(chan, vnc_id, ATOMP_EMB_SURFACE_STRIDE0,
 238                                           chan->embedded_data_width * BPP_MEM);
 239         vi4_channel_write(chan, vnc_id,
 240                 ATOMP_SURFACE_OFFSET0, buf->addr + offset);
 241         vi4_channel_write(chan, vnc_id,
 242                 ATOMP_SURFACE_STRIDE0, chan->format.bytesperline);
 243         vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_OFFSET0_H, 0x0);
 244
 245         if (chan->fmtinfo->fourcc == V4L2_PIX_FMT_NV16) {
 246                 vi4_channel_write(chan, vnc_id,
 247                         ATOMP_SURFACE_OFFSET1, buf->addr + offset +
 248                         chan->format.sizeimage / 2);
 249                 vi4_channel_write(chan, vnc_id,
 250                         ATOMP_SURFACE_OFFSET1_H, 0x0);
 251                 vi4_channel_write(chan, vnc_id,
 252                         ATOMP_SURFACE_STRIDE1, chan->format.bytesperline);
 253
 254         } else {
 255                 vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_OFFSET1, 0x0);
 256                 vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_OFFSET1_H, 0x0);
 257                 vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_STRIDE1, 0x0);
 258         }
 259
 260         vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_OFFSET2, 0x0);
 261         vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_OFFSET2_H, 0x0);
 262         vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_STRIDE2, 0x0);
 263 }
 264
 265 static void tegra_channel_handle_error(struct tegra_channel *chan)
 266 {
 267         struct v4l2_subdev *sd_on_csi = chan->subdev_on_csi;
 268         static const struct v4l2_event source_ev_fmt = {
 269                 .type = V4L2_EVENT_SOURCE_CHANGE,
 270                 .u.src_change.changes = V4L2_EVENT_SRC_ERROR,
 271         };
 272
 273         tegra_channel_stop_increments(chan);
 274         vb2_queue_error(&chan->queue);
 275
 276         /* Application gets notified after CSI Tx's are reset */
 277         if (sd_on_csi->devnode)
 278                 v4l2_subdev_notify_event(sd_on_csi, &source_ev_fmt);
 279 }
 280
 281 static void tegra_channel_status_worker(struct work_struct *status_work)
 282 {
 283         struct tegra_channel *chan;
 284
 285         chan = container_of(status_work, struct tegra_channel, status_work);
 286
 287         tegra_channel_handle_error(chan);
 288 }
 289
 290 static void tegra_channel_notify_status_callback(
 291                                 struct vi_notify_channel *vnc,
 292                                 const struct vi_capture_status *status,
 293                                 void *client_data)
 294 {
 295         struct tegra_channel *chan = (struct tegra_channel *)client_data;
 296         int i;
 297
 298         spin_lock(&chan->capture_state_lock);
 299         if (chan->capture_state == CAPTURE_GOOD)
 300                 chan->capture_state = CAPTURE_ERROR;
 301         else {
 302                 spin_unlock(&chan->capture_state_lock);
 303                 return;
 304         }
 305         spin_unlock(&chan->capture_state_lock);
 306
 307         for (i = 0; i < chan->valid_ports; i++)
 308                 dev_err(chan->vi->dev, "Status: %2u channel:%02X frame:%04X\n",
 309                         status->status, chan->vnc_id[i], status->frame);
 310         dev_err(chan->vi->dev, "         timestamp sof %llu eof %llu data 0x%08x\n",
 311                 status->sof_ts, status->eof_ts, status->data);
 312         dev_err(chan->vi->dev, "         capture_id %u stream %2u vchan %2u\n",
 313                 status->capture_id, status->st, status->vc);
 314
 315         schedule_work(&chan->status_work);
 316 }
 317
 318 static int tegra_channel_notify_enable(
 319         struct tegra_channel *chan, unsigned int index)
 320 {
 321         struct tegra_vi4_syncpts_req req;
 322         int i, err;
 323
 324         chan->vnc_id[index] = -1;
 325         for (i = 0; i < MAX_VI_CHANNEL; i++) {
 326                 chan->vnc[index] = vi_notify_channel_open(i);
 327                 if (!IS_ERR(chan->vnc[index])) {
 328                         chan->vnc_id[index] = i;
 329                         break;
 330                 }
 331         }
 332         if (chan->vnc_id[index] < 0) {
 333                 dev_err(chan->vi->dev, "No VI channel available!\n");
 334                 return -EFAULT;
 335         }
 336
 337         vi_notify_channel_set_notify_funcs(chan->vnc[index],
 338                         &tegra_channel_notify_status_callback,
 339                         &tegra_channel_notify_error_callback,
 340                         (void *)chan);
 341
 342         /* get PXL_SOF syncpt id */
 343         chan->syncpt[index][SOF_SYNCPT_IDX] =
 344                 nvhost_get_syncpt_client_managed(chan->vi->ndev, "tegra-vi4");
 345         if (chan->syncpt[index][SOF_SYNCPT_IDX] == 0) {
 346                 dev_err(chan->vi->dev, "Failed to get PXL_SOF syncpt!\n");
 347                 return -EFAULT;
 348         }
 349
 350         /* get ATOMP_FE syncpt id */
 351         chan->syncpt[index][FE_SYNCPT_IDX] =
 352                 nvhost_get_syncpt_client_managed(chan->vi->ndev, "tegra-vi4");
 353         if (chan->syncpt[index][FE_SYNCPT_IDX] == 0) {
 354                 dev_err(chan->vi->dev, "Failed to get ATOMP_FE syncpt!\n");
 355                 nvhost_syncpt_put_ref_ext(
 356                         chan->vi->ndev, chan->syncpt[index][SOF_SYNCPT_IDX]);
 357                 return -EFAULT;
 358         }
 359
 360         nvhost_syncpt_set_min_eq_max_ext(
 361                 chan->vi->ndev, chan->syncpt[index][SOF_SYNCPT_IDX]);
 362         nvhost_syncpt_set_min_eq_max_ext(
 363                 chan->vi->ndev, chan->syncpt[index][FE_SYNCPT_IDX]);
 364
 365         /* enable VI Notify report */
 366         req.syncpt_ids[0] = chan->syncpt[index][SOF_SYNCPT_IDX]; /* PXL_SOF */
 367         req.syncpt_ids[1] = chan->syncpt[index][FE_SYNCPT_IDX]; /* ATOMP_FE */
 368         req.syncpt_ids[2] = 0xffffffff;
 369         req.stream = chan->port[index];
 370         req.vc = 0;
 371         req.pad = 0;
 372
 373         err = vi_notify_channel_enable_reports(
 374                 chan->vnc_id[index], chan->vnc[index], &req);
 375         if (err < 0)
 376                 dev_err(chan->vi->dev,
 377                         "Failed to enable report for VI Notify, err = %d\n",
 378                         err);
 379
 380         return err;
 381 }
 382
 383 static int tegra_channel_notify_disable(
 384         struct tegra_channel *chan, unsigned int index)
 385 {
 386         int err;
 387         int ret = 0;
 388         struct tegra_vi4_syncpts_req req;
 389
 390         /* free syncpts */
 391         nvhost_syncpt_put_ref_ext(
 392                 chan->vi->ndev, chan->syncpt[index][SOF_SYNCPT_IDX]);
 393         nvhost_syncpt_put_ref_ext(
 394                 chan->vi->ndev, chan->syncpt[index][FE_SYNCPT_IDX]);
 395
 396         /* close vi-notifier */
 397         req.syncpt_ids[0] = 0xffffffff;
 398         req.syncpt_ids[1] = 0xffffffff;
 399         req.syncpt_ids[2] = 0xffffffff;
 400         req.stream = chan->port[index];
 401         req.vc = 0;
 402         req.pad = 0;
 403
 404         err = vi_notify_channel_reset(
 405                 chan->vnc_id[index], chan->vnc[index], &req);
 406         if (err < 0) {
 407                 dev_err(chan->vi->dev,
 408                         "VI Notify channel reset failed, err = %d\n", err);
 409                 if (!ret)
 410                         ret = err;
 411         }
 412
 413         err = vi_notify_channel_close(chan->vnc_id[index], chan->vnc[index]);
 414         if (err < 0) {
 415                 dev_err(chan->vi->dev,
 416                         "VI Notify channel close failed, err = %d\n", err);
 417                 if (!ret)
 418                         ret = err;
 419         }
 420
 421         return ret;
 422 }
 423
 424 static int tegra_channel_capture_setup(struct tegra_channel *chan,
 425                 unsigned int index)
 426 {
 427         u32 height = chan->format.height;
 428         u32 width = chan->format.width;
 429         u32 format = chan->fmtinfo->img_fmt;
 430         u32 data_type = chan->fmtinfo->img_dt;
 431         u32 csi_port = chan->port[index];
 432         u32 stream = 1U << csi_port;
 433         u32 virtual_ch = 1U << 0;
 434         u32 vnc_id;
 435         int err;
 436
 437         if (chan->valid_ports > 1) {
 438                 height = chan->gang_height;
 439                 width = chan->gang_width;
 440         }
 441
 442         err = tegra_channel_notify_enable(chan, index);
 443         if (err < 0) {
 444                 dev_err(chan->vi->dev,
 445                         "Failed to setup VI Notifier, err = %d\n", err);
 446                 return err;
 447         }
 448
 449         vnc_id = chan->vnc_id[index];
 450
 451         vi4_write(chan, csimux_config_stream[csi_port], 0x1);
 452
 453         vi4_channel_write(chan, vnc_id, MATCH,
 454                         ((stream << STREAM_SHIFT) & STREAM) |
 455                         STREAM_MASK |
 456                         ((virtual_ch << VIRTUAL_CHANNEL_SHIFT) &
 457                         VIRTUAL_CHANNEL)  |
 458                         VIRTUAL_CHANNEL_MASK);
 459
 460         vi4_channel_write(chan, vnc_id, MATCH_DATATYPE,
 461                         ((data_type << DATATYPE_SHIFT) & DATATYPE) |
 462                         DATATYPE_MASK);
 463
 464         vi4_channel_write(chan, vnc_id, DT_OVERRIDE, 0x0);
 465
 466         vi4_channel_write(chan, vnc_id, MATCH_FRAMEID,
 467                         ((0 << FRAMEID_SHIFT) & FRAMEID) | 0);
 468
 469         vi4_channel_write(chan, vnc_id, FRAME_X, width);
 470         vi4_channel_write(chan, vnc_id, FRAME_Y, height);
 471         vi4_channel_write(chan, vnc_id, SKIP_X, 0x0);
 472         vi4_channel_write(chan, vnc_id, CROP_X, width);
 473         vi4_channel_write(chan, vnc_id, OUT_X, width);
 474         vi4_channel_write(chan, vnc_id, SKIP_Y, 0x0);
 475         vi4_channel_write(chan, vnc_id, CROP_Y, height);
 476         vi4_channel_write(chan, vnc_id, OUT_Y, height);
 477         vi4_channel_write(chan, vnc_id, PIXFMT_ENABLE, PIXFMT_EN);
 478         vi4_channel_write(chan, vnc_id, PIXFMT_WIDE, 0x0);
 479         vi4_channel_write(chan, vnc_id, PIXFMT_FORMAT, format);
 480         vi4_channel_write(chan, vnc_id, DPCM_STRIP, 0x0);
 481         vi4_channel_write(chan, vnc_id, ATOMP_DPCM_CHUNK, 0x0);
 482         vi4_channel_write(chan, vnc_id, ISPBUFA, 0x0);
 483         vi4_channel_write(chan, vnc_id, LINE_TIMER, 0x1000000);
 484         if (chan->embedded_data_height > 0) {
 485                 vi4_channel_write(chan, vnc_id, EMBED_X,
 486                         chan->embedded_data_width * BPP_MEM);
 487                 vi4_channel_write(chan, vnc_id, EMBED_Y,
 488                         chan->embedded_data_height | EXPECT);
 489         } else {
 490                 vi4_channel_write(chan, vnc_id, EMBED_X, 0);
 491                 vi4_channel_write(chan, vnc_id, EMBED_Y, 0);
 492         }
 493         /*
 494          * Set ATOMP_RESERVE to 0 so rctpu won't increment syncpt
 495          * for captureInfo. This is copied from nvvi driver.
 496          *
 497          * If we don't set this register to 0, ATOMP_FE syncpt
 498          * will be increment by 2 for each frame
 499          */
 500         vi4_channel_write(chan, vnc_id, ATOMP_RESERVE, 0x0);
 501         dev_dbg(chan->vi->dev,
 502                 "Create Surface with imgW=%d, imgH=%d, memFmt=%d\n",
 503                 width, height, format);
 504
 505         return 0;
 506 }
 507
 508 static int tegra_channel_capture_frame(struct tegra_channel *chan,
 509                                         struct tegra_channel_buffer *buf)
 510 {
 511         struct timespec ts;
 512         unsigned long flags;
 513         bool is_streaming = atomic_read(&chan->is_streaming);
 514         int restart_version = 0;
 515         int err = false;
 516         int i;
 517
 518         for (i = 0; i < chan->valid_ports; i++)
 519                 tegra_channel_surface_setup(chan, buf, i);
 520
 521         restart_version = atomic_read(&chan->restart_version);
 522         if (!is_streaming ||
 523                 restart_version != chan->capture_version) {
 524
 525                 chan->capture_version = restart_version;
 526                 err = tegra_channel_set_stream(chan, true);
 527                 if (err < 0)
 528                         return err;
 529         }
 530
 531         for (i = 0; i < chan->valid_ports; i++) {
 532                 vi4_channel_write(chan, chan->vnc_id[i], CHANNEL_COMMAND, LOAD);
 533                 vi4_channel_write(chan, chan->vnc_id[i],
 534                         CONTROL, SINGLESHOT | MATCH_STATE_EN);
 535         }
 536
 537         /* wait for vi notifier events */
 538         vi_notify_wait(chan, &ts);
 539         dev_dbg(&chan->video.dev,
 540                 "%s: vi4 got SOF syncpt buf[%p]\n", __func__, buf);
 541
 542         vi4_check_status(chan);
 543
 544         spin_lock_irqsave(&chan->capture_state_lock, flags);
 545         if (chan->capture_state != CAPTURE_ERROR)
 546                 chan->capture_state = CAPTURE_GOOD;
 547         spin_unlock_irqrestore(&chan->capture_state_lock, flags);
 548
 549         if (chan->capture_state == CAPTURE_GOOD) {
 550                 /*
 551                  * Set the buffer version to match
 552                  * current capture version
 553                  */
 554                 buf->version = chan->capture_version;
 555                 enqueue_inflight(chan, buf);
 556         } else {
 557                 release_buffer(chan, buf);
 558                 atomic_inc(&chan->restart_version);
 559         }
 560
 561         return 0;
 562 }
 563
 564 static void tegra_channel_release_frame(struct tegra_channel *chan,
 565                                         struct tegra_channel_buffer *buf)
 566 {
 567         struct timespec ts = {0, 0};
 568         int index;
 569         int err = 0;
 570         int restart_version = 0;
 571
 572         buf->state = VB2_BUF_STATE_DONE;
 573
 574         /*
 575          * If the frame capture was started on a different reset version
 576          * than our current version than either a reset is imminent or
 577          * it has already happened so don't bother waiting for the frame
 578          * to complete.
 579          */
 580         restart_version = atomic_read(&chan->restart_version);
 581         if (buf->version != restart_version) {
 582                 buf->state = VB2_BUF_STATE_ERROR;
 583                 release_buffer(chan, buf);
 584                 return;
 585         }
 586
 587         for (index = 0; index < chan->valid_ports; index++) {
 588                 err = nvhost_syncpt_wait_timeout_ext(chan->vi->ndev,
 589                         chan->syncpt[index][FE_SYNCPT_IDX], buf->thresh[index],
 590                         chan->timeout, NULL, &ts);
 591                 if (err)
 592                         dev_err(&chan->video.dev,
 593                                 "MW_ACK_DONE syncpoint time out!%d\n", index);
 594         }
 595         dev_dbg(&chan->video.dev,
 596                 "%s: vi4 got EOF syncpt buf[%p]\n", __func__, buf);
 597
 598         if (err) {
 599                 buf->state = VB2_BUF_STATE_ERROR;
 600
 601                 /* NOTE:
 602                  * Disabling the following, it will happen in the
 603                  * capture thread on the next frame start due to
 604                  * the reset request we make by incrementing the
 605                  * reset counter.
 606
 607                 tegra_channel_ec_recover(chan);
 608                 chan->capture_state = CAPTURE_TIMEOUT;
 609                  */
 610                 atomic_inc(&chan->restart_version);
 611         }
 612         release_buffer(chan, buf);
 613 }
 614
 615 static int tegra_channel_stop_increments(struct tegra_channel *chan)
 616 {
 617         int i;
 618         struct tegra_vi4_syncpts_req req = {
 619                 .syncpt_ids = {
 620                         0xffffffff,
 621                         0xffffffff,
 622                         0xffffffff,
 623                 },
 624                 .stream = chan->port[0],
 625                 .vc = 0,
 626         };
 627
 628         /* No need to check errors. There's nothing we could do. */
 629         for (i = 0; i < chan->valid_ports; i++)
 630                 vi_notify_channel_reset(chan->vnc_id[i], chan->vnc[i], &req);
 631
 632         return 0;
 633 }
 634
 635 static void tegra_channel_capture_done(struct tegra_channel *chan)
 636 {
 637         struct timespec ts;
 638         struct tegra_channel_buffer *buf;
 639         u32 thresh[TEGRA_CSI_BLOCKS];
 640         int i, err;
 641
 642         /* dequeue buffer and return if no buffer exists */
 643         buf = dequeue_buffer(chan);
 644         if (!buf)
 645                 return;
 646
 647         /* make sure to read the last frame out before exit */
 648         for (i = 0; i < chan->valid_ports; i++) {
 649                 tegra_channel_surface_setup(chan, buf, i);
 650                 vi4_channel_write(chan, chan->vnc_id[i], CHANNEL_COMMAND, LOAD);
 651                 vi4_channel_write(chan, chan->vnc_id[i],
 652                         CONTROL, SINGLESHOT | MATCH_STATE_EN);
 653         }
 654
 655         for (i = 0; i < chan->valid_ports; i++) {
 656                 err = nvhost_syncpt_read_ext_check(chan->vi->ndev,
 657                                 chan->syncpt[i][FE_SYNCPT_IDX], &thresh[i]);
 658                 /* Get current ATOMP_FE syncpt min value */
 659                 if (!err) {
 660                         struct vi_capture_status status;
 661                         u32 index = thresh[i] + 1;
 662                         /* Wait for ATOMP_FE syncpt
 663                          *
 664                          * This is to make sure we don't exit the capture thread
 665                          * before the last frame is done writing to memory
 666                          */
 667                         err = nvhost_syncpt_wait_timeout_ext(chan->vi->ndev,
 668                                         chan->syncpt[i][FE_SYNCPT_IDX],
 669                                         index,
 670                                         250, NULL, NULL);
 671                         if (unlikely(err))
 672                                 dev_err(chan->vi->dev, "ATOMP_FE syncpt timeout!\n");
 673                         else {
 674                                 err = vi_notify_get_capture_status(chan->vnc[i],
 675                                                 chan->vnc_id[i],
 676                                                 index, &status);
 677                                 if (unlikely(err))
 678                                         dev_err(chan->vi->dev,
 679                                                 "no capture status! err = %d\n",
 680                                                 err);
 681                                 else
 682                                         ts = ns_to_timespec((s64)status.eof_ts);
 683                         }
 684                 }
 685         }
 686
 687         /* Mark capture state to IDLE as capture is finished */
 688         chan->capture_state = CAPTURE_IDLE;
 689         release_buffer(chan, buf);
 690 }
 691
 692 static int tegra_channel_kthread_capture_start(void *data)
 693 {
 694         struct tegra_channel *chan = data;
 695         struct tegra_channel_buffer *buf;
 696         int err = 0;
 697
 698         set_freezable();
 699
 700         while (1) {
 701
 702                 try_to_freeze();
 703
 704                 wait_event_interruptible(chan->start_wait,
 705                                          !list_empty(&chan->capture) ||
 706                                          kthread_should_stop());
 707
 708                 if (kthread_should_stop())
 709                         break;
 710
 711                 /* source is not streaming if error is non-zero */
 712                 /* wait till kthread stop and dont DeQ buffers */
 713                 if (err)
 714                         continue;
 715
 716                 buf = dequeue_buffer(chan);
 717                 if (!buf)
 718                         continue;
 719
 720                 err = tegra_channel_capture_frame(chan, buf);
 721         }
 722
 723         return 0;
 724 }
 725
 726 static int tegra_channel_kthread_release(void *data)
 727 {
 728         struct tegra_channel *chan = data;
 729         struct tegra_channel_buffer *buf;
 730
 731         set_freezable();
 732
 733         while (1) {
 734
 735                 try_to_freeze();
 736
 737                 wait_event_interruptible(chan->release_wait,
 738                                          !list_empty(&chan->release) ||
 739                                          kthread_should_stop());
 740
 741                 if (kthread_should_stop())
 742                         break;
 743
 744                 buf = dequeue_inflight(chan);
 745                 if (!buf)
 746                         continue;
 747
 748                 tegra_channel_release_frame(chan, buf);
 749         }
 750
 751         return 0;
 752 }
 753
 754 static void tegra_channel_stop_kthreads(struct tegra_channel *chan)
 755 {
 756         mutex_lock(&chan->stop_kthread_lock);
 757         /* Stop the kthread for capture */
 758         if (chan->kthread_capture_start) {
 759                 kthread_stop(chan->kthread_capture_start);
 760                 chan->kthread_capture_start = NULL;
 761         }
 762
 763         if (chan->kthread_release) {
 764                 kthread_stop(chan->kthread_release);
 765                 chan->kthread_release = NULL;
 766         }
 767         mutex_unlock(&chan->stop_kthread_lock);
 768 }
 769
 770 static int tegra_channel_update_clknbw(struct tegra_channel *chan, u8 on)
 771 {
 772         int ret = 0;
 773         unsigned long request_pixelrate;
 774         struct v4l2_subdev_frame_interval fie;
 775         unsigned long csi_freq = 0;
 776         unsigned int ppc_multiplier = 1;
 777
 778         /* if bytes per pixel is greater than 2, then num_ppc is 4 */
 779         /* since num_ppc in nvhost framework is always 8, use multiplier */
 780         if (chan->fmtinfo->bpp.numerator > 2)
 781                 ppc_multiplier = 2;
 782
 783         fie.interval.denominator = DEFAULT_FRAMERATE;
 784         fie.interval.numerator = 1;
 785
 786         if (v4l2_subdev_has_op(chan->subdev_on_csi,
 787                                 video, g_frame_interval))
 788                 v4l2_subdev_call(chan->subdev_on_csi, video,
 789                                 g_frame_interval, &fie);
 790         else {
 791                 if (v4l2_subdev_has_op(chan->subdev_on_csi,
 792                                 video, g_dv_timings)) {
 793                         u32 total_width;
 794                         u32 total_height;
 795                         struct v4l2_dv_timings dvtimings;
 796                         struct v4l2_bt_timings *timings = &dvtimings.bt;
 797
 798                         v4l2_subdev_call(chan->subdev_on_csi,
 799                                 video, g_dv_timings, &dvtimings);
 800                         total_width = timings->width + timings->hfrontporch +
 801                                 timings->hsync + timings->hbackporch;
 802                         total_height = timings->height + timings->vfrontporch +
 803                                 timings->vsync + timings->vbackporch;
 804                         fie.interval.denominator = timings->pixelclock /
 805                                 (total_width * total_height);
 806                 }
 807         }
 808
 809         if (on) {
 810                 /* for PG, using default frequence */
 811                 if (chan->pg_mode) {
 812                         ret = nvhost_module_get_rate(chan->vi->csi->pdev,
 813                                 &csi_freq, 0);
 814                         if (ret)
 815                                 return ret;
 816                         request_pixelrate = csi_freq * PG_BITRATE /
 817                                 chan->fmtinfo->width;
 818                         request_pixelrate *= ppc_multiplier;
 819                 } else {
 820                         /**
 821                          * TODO: use real sensor pixelrate
 822                          * See PowerService code
 823                          */
 824                         request_pixelrate = (long long)(chan->format.width
 825                                 * chan->format.height
 826                                 * fie.interval.denominator / 100)
 827                                 * VI_CSI_CLK_SCALE * ppc_multiplier;
 828                 }
 829
 830                 /* VI clk should be slightly faster than CSI clk*/
 831                 ret = nvhost_module_set_rate(chan->vi->ndev, &chan->video,
 832                                 request_pixelrate, 0, NVHOST_PIXELRATE);
 833                 if (ret) {
 834                         dev_err(chan->vi->dev, "Fail to update vi clk\n");
 835                         return ret;
 836                 }
 837         } else {
 838                 ret = nvhost_module_set_rate(chan->vi->ndev, &chan->video, 0, 0,
 839                                 NVHOST_PIXELRATE);
 840                 if (ret) {
 841                         dev_err(chan->vi->dev, "Fail to update vi clk\n");
 842                         return ret;
 843                 }
 844         }
 845         if (chan->pg_mode)
 846                 chan->requested_kbyteps = on ?
 847                         (((long long)csi_freq * PG_BITRATE * BPP_MEM /
 848                          chan->fmtinfo->width) / 1000) :
 849                         (-chan->requested_kbyteps);
 850         else
 851                 chan->requested_kbyteps = on ?
 852                 ((((long long) chan->format.width * chan->format.height
 853                 * fie.interval.denominator * BPP_MEM) * 115 / 100) / 1000) :
 854                 (-chan->requested_kbyteps);
 855
 856         mutex_lock(&chan->vi->bw_update_lock);
 857         chan->vi->aggregated_kbyteps += chan->requested_kbyteps;
 858         ret = vi_v4l2_update_isobw(chan->vi->aggregated_kbyteps, 0);
 859         mutex_unlock(&chan->vi->bw_update_lock);
 860         if (ret)
 861                 dev_info(chan->vi->dev,
 862                 "WAR:Calculation not precise.Ignore BW request failure\n");
 863         ret = vi4_v4l2_set_la(chan->vi->ndev, 0, 0);
 864         if (ret)
 865                 dev_info(chan->vi->dev,
 866                 "WAR:Calculation not precise.Ignore LA failure\n");
 867         return 0;
 868 }
 869
 870 int vi4_channel_start_streaming(struct vb2_queue *vq, u32 count)
 871 {
 872         struct tegra_channel *chan = vb2_get_drv_priv(vq);
 873         struct media_pipeline *pipe = chan->video.entity.pipe;
 874         int ret = 0, i;
 875         unsigned long flags;
 876         struct v4l2_ctrl *override_ctrl;
 877         struct v4l2_subdev *sd;
 878         struct device_node *node;
 879         struct sensor_mode_properties *sensor_mode;
 880         struct camera_common_data *s_data;
 881         unsigned int emb_buf_size = 0;
 882
 883         ret = media_entity_pipeline_start(&chan->video.entity, pipe);
 884         if (ret < 0)
 885                 goto error_pipeline_start;
 886
 887         if (chan->bypass) {
 888                 ret = tegra_channel_set_stream(chan, true);
 889                 if (ret < 0)
 890                         goto error_set_stream;
 891                 return ret;
 892         }
 893
 894         vi4_init(chan);
 895
 896         spin_lock_irqsave(&chan->capture_state_lock, flags);
 897         chan->capture_state = CAPTURE_IDLE;
 898         spin_unlock_irqrestore(&chan->capture_state_lock, flags);
 899
 900         if (!chan->pg_mode) {
 901                 sd = chan->subdev_on_csi;
 902                 node = sd->dev->of_node;
 903                 s_data = to_camera_common_data(sd->dev);
 904
 905                 if (s_data == NULL) {
 906                         dev_err(&chan->video.dev,
 907                                 "Camera common data missing!\n");
 908                         return -EINVAL;
 909                 }
 910
 911                 /* get sensor properties from DT */
 912                 if (node != NULL) {
 913                         int idx = s_data->mode_prop_idx;
 914
 915                         emb_buf_size = 0;
 916                         if (idx < s_data->sensor_props.num_modes) {
 917                                 sensor_mode =
 918                                         &s_data->sensor_props.sensor_modes[idx];
 919
 920                                 chan->embedded_data_width =
 921                                         sensor_mode->image_properties.width;
 922                                 chan->embedded_data_height =
 923                                         sensor_mode->image_properties.\
 924                                         embedded_metadata_height;
 925                                 /* rounding up to page size */
 926                                 emb_buf_size =
 927                                         round_up(chan->embedded_data_width *
 928                                                 chan->embedded_data_height *
 929                                                 BPP_MEM,
 930                                                 PAGE_SIZE);
 931                         }
 932                 }
 933
 934
 935                 /* Allocate buffer for Embedded Data if need to*/
 936                 if (emb_buf_size > chan->vi->emb_buf_size) {
 937                         /*
 938                          * if old buffer is smaller than what we need,
 939                          * release the old buffer and re-allocate a bigger
 940                          * one below
 941                          */
 942                         if (chan->vi->emb_buf_size > 0) {
 943                                 dma_free_coherent(chan->vi->dev,
 944                                         chan->vi->emb_buf_size,
 945                                         chan->vi->emb_buf_addr,
 946                                         chan->vi->emb_buf);
 947                                 chan->vi->emb_buf_size = 0;
 948                         }
 949
 950                         chan->vi->emb_buf_addr =
 951                                 dma_alloc_coherent(chan->vi->dev,
 952                                         emb_buf_size,
 953                                         &chan->vi->emb_buf, GFP_KERNEL);
 954                         if (!chan->vi->emb_buf_addr) {
 955                                 dev_err(&chan->video.dev,
 956                                                 "Can't allocate memory for embedded data\n");
 957                                 goto error_capture_setup;
 958                         }
 959                         chan->vi->emb_buf_size = emb_buf_size;
 960                 }
 961         }
 962
 963         for (i = 0; i < chan->valid_ports; i++) {
 964                 ret = tegra_channel_capture_setup(chan, i);
 965                 if (ret < 0)
 966                         goto error_capture_setup;
 967         }
 968
 969         chan->sequence = 0;
 970
 971         /* disable override for vi mode */
 972         override_ctrl = v4l2_ctrl_find(
 973                 &chan->ctrl_handler, TEGRA_CAMERA_CID_OVERRIDE_ENABLE);
 974         if (!chan->pg_mode) {
 975                 if (override_ctrl) {
 976                         ret = v4l2_ctrl_s_ctrl(override_ctrl, false);
 977                         if (ret < 0)
 978                                 dev_err(&chan->video.dev,
 979                                         "failed to disable override control\n");
 980                 } else
 981                         dev_err(&chan->video.dev,
 982                                 "No override control\n");
 983         }
 984
 985         /* Update clock and bandwidth based on the format */
 986         ret = tegra_channel_update_clknbw(chan, 1);
 987         if (ret)
 988                 goto error_capture_setup;
 989
 990         INIT_WORK(&chan->error_work, tegra_channel_error_worker);
 991         INIT_WORK(&chan->status_work, tegra_channel_status_worker);
 992
 993         /* Start kthread to capture data to buffer */
 994         chan->kthread_capture_start = kthread_run(
 995                                         tegra_channel_kthread_capture_start,
 996                                         chan, chan->video.name);
 997         if (IS_ERR(chan->kthread_capture_start)) {
 998                 dev_err(&chan->video.dev,
 999                         "failed to run kthread for capture start\n");
1000                 ret = PTR_ERR(chan->kthread_capture_start);
1001                 goto error_capture_setup;
1002         }
1003
1004         /* Start thread to release buffers */
1005         chan->kthread_release = kthread_run(
1006                                         tegra_channel_kthread_release,
1007                                         chan, chan->video.name);
1008         if (IS_ERR(chan->kthread_release)) {
1009                 dev_err(&chan->video.dev,
1010                         "failed to run kthread for release\n");
1011                 ret = PTR_ERR(chan->kthread_release);
1012                 goto error_capture_setup;
1013         }
1014
1015         return 0;
1016
1017 error_capture_setup:
1018         if (!chan->pg_mode)
1019                 tegra_channel_set_stream(chan, false);
1020 error_set_stream:
1021         media_entity_pipeline_stop(&chan->video.entity);
1022 error_pipeline_start:
1023         vq->start_streaming_called = 0;
1024         tegra_channel_queued_buf_done(chan, VB2_BUF_STATE_QUEUED);
1025
1026         return ret;
1027 }
1028
1029 int vi4_channel_stop_streaming(struct vb2_queue *vq)
1030 {
1031         struct tegra_channel *chan = vb2_get_drv_priv(vq);
1032         bool is_streaming = atomic_read(&chan->is_streaming);
1033         int i;
1034
1035         for (i = 0; i < chan->valid_ports; i++) {
1036                 if (chan->vnc_id[i] == -1)
1037                         return 0;
1038         }
1039
1040         cancel_work_sync(&chan->status_work);
1041         cancel_work_sync(&chan->error_work);
1042
1043         if (!chan->bypass) {
1044                 tegra_channel_stop_kthreads(chan);
1045                 /* wait for last frame memory write ack */
1046                 if (is_streaming)
1047                         tegra_channel_capture_done(chan);
1048                 for (i = 0; i < chan->valid_ports; i++)
1049                         tegra_channel_notify_disable(chan, i);
1050
1051                 /* dequeue buffers back to app which are in capture queue */
1052                 tegra_channel_queued_buf_done(chan, VB2_BUF_STATE_ERROR);
1053         }
1054
1055         tegra_channel_set_stream(chan, false);
1056         media_entity_pipeline_stop(&chan->video.entity);
1057
1058         if (!chan->bypass)
1059                 tegra_channel_update_clknbw(chan, 0);
1060
1061         return 0;
1062 }
1063
1064 int vi4_mfi_work(struct tegra_mc_vi *vi, int channel)
1065 {
1066         struct tegra_channel *it = NULL;
1067         int ret = 0;
1068
1069         /* for vi4, the input argument is the hw channel id*/
1070         /* search the list and match the hw id */
1071         list_for_each_entry(it, &vi->vi_chans, list) {
1072                 if (channel == it->vnc_id[0]) {
1073                         ret = v4l2_subdev_call(it->subdev_on_csi, core,
1074                                         sync, V4L2_SYNC_EVENT_FOCUS_POS);
1075                         if (ret < 0 && ret != -ENOIOCTLCMD) {
1076                                 dev_err(vi->dev,
1077                                         "%s:channel failed\n", __func__);
1078                                 return ret;
1079                         }
1080                 }
1081         }
1082
1083         return ret;
1084 }
1085
1086 int tegra_vi4_power_on(struct tegra_mc_vi *vi)
1087 {
1088         int ret;
1089
1090         ret = nvhost_module_busy(vi->ndev);
1091         if (ret) {
1092                 dev_err(vi->dev, "%s:nvhost module is busy\n", __func__);
1093                 return ret;
1094         }
1095
1096         ret = tegra_camera_emc_clk_enable();
1097         if (ret)
1098                 goto err_emc_enable;
1099
1100         return 0;
1101
1102 err_emc_enable:
1103         nvhost_module_idle(vi->ndev);
1104
1105         return ret;
1106 }
1107
1108 void tegra_vi4_power_off(struct tegra_mc_vi *vi)
1109 {
1110         tegra_channel_ec_close(vi);
1111         tegra_camera_emc_clk_disable();
1112         nvhost_module_idle(vi->ndev);
1113 }
1114
1115 int vi4_power_on(struct tegra_channel *chan)
1116 {
1117         int ret = 0;
1118         struct tegra_mc_vi *vi;
1119         struct tegra_csi_device *csi;
1120
1121         vi = chan->vi;
1122         csi = vi->csi;
1123
1124         /* Use chan->video as identifier of vi4 nvhost_module client
1125          * since they are unique per channel
1126          */
1127         ret = nvhost_module_add_client(vi->ndev, &chan->video);
1128         if (ret)
1129                 return ret;
1130         tegra_vi4_power_on(vi);
1131
1132         if (atomic_add_return(1, &chan->power_on_refcnt) == 1) {
1133                 ret = tegra_channel_set_power(chan, 1);
1134                 if (ret < 0)
1135                         dev_err(vi->dev, "Failed to power on subdevices\n");
1136         }
1137
1138         return ret;
1139 }
1140
1141 void vi4_power_off(struct tegra_channel *chan)
1142 {
1143         int ret = 0;
1144         struct tegra_mc_vi *vi;
1145         struct tegra_csi_device *csi;
1146
1147         vi = chan->vi;
1148         csi = vi->csi;
1149
1150         if (atomic_dec_and_test(&chan->power_on_refcnt)) {
1151                 ret = tegra_channel_set_power(chan, 0);
1152                 if (ret < 0)
1153                         dev_err(vi->dev, "Failed to power off subdevices\n");
1154         }
1155
1156         tegra_vi4_power_off(vi);
1157         nvhost_module_remove_client(vi->ndev, &chan->video);
1158 }
1159
1160 static void tegra_channel_error_worker(struct work_struct *error_work)
1161 {
1162         struct tegra_channel *chan;
1163
1164         chan = container_of(error_work, struct tegra_channel, error_work);
1165
1166         vi4_power_off(chan);
1167         tegra_channel_handle_error(chan);
1168 }
1169
1170 static void tegra_channel_notify_error_callback(void *client_data)
1171 {
1172         struct tegra_channel *chan = (struct tegra_channel *)client_data;
1173
1174         spin_lock(&chan->capture_state_lock);
1175         if (chan->capture_state == CAPTURE_GOOD)
1176                 chan->capture_state = CAPTURE_ERROR;
1177         else {
1178                 spin_unlock(&chan->capture_state_lock);
1179                 return;
1180         }
1181         spin_unlock(&chan->capture_state_lock);
1182
1183         schedule_work(&chan->error_work);
1184 }