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

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