drivers/media/platform/soc_camera/tegra_camera/vi2.c

   1 /*
   2  * Copyright (c) 2013-2015, NVIDIA CORPORATION.  All rights reserved.
   3  *
   4  * This program is free software; you can redistribute it and/or modify it
   5  * under the terms and conditions of the GNU General Public License,
   6  * version 2, as published by the Free Software Foundation.
   7  *
   8  * This program is distributed in the hope it will be useful, but WITHOUT
   9  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  10  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  11  * more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  15  */
  16
  17 #include <linux/delay.h>
  18 #include <linux/freezer.h>
  19 #include <linux/kernel.h>
  20 #include <linux/kthread.h>
  21 #include <linux/platform_device.h>
  22 #include <linux/module.h>
  23 #include <linux/of.h>
  24 #include <linux/of_device.h>
  25 #include <linux/of_platform.h>
  26 #include <linux/clk.h>
  27 #include <linux/reset.h>
  28 #include <linux/regulator/consumer.h>
  29 #include <linux/nvhost.h>
  30
  31 #include <media/soc_camera.h>
  32 #include <media/soc_mediabus.h>
  33 #include <media/tegra_v4l2_camera.h>
  34 #include <media/camera_common.h>
  35
  36 #include <mach/clk.h>
  37 #include <mach/io_dpd.h>
  38
  39 #include "nvhost_syncpt.h"
  40 #include "nvhost_acm.h"
  41 #include "bus_client.h"
  42 #include "t124/t124.h"
  43 #include "t210/t210.h"
  44 #include "common.h"
  45
  46 static int tpg_mode;
  47 module_param(tpg_mode, int, 0644);
  48
  49 #define VI2_CAM_DRV_NAME                                "vi"
  50 #define VI2_CAM_CARD_NAME                               "vi"
  51 #define VI2_CAM_VERSION                                 KERNEL_VERSION(0, 0, 5)
  52
  53 #define TEGRA_SYNCPT_RETRY_COUNT                        10
  54
  55 #define TEGRA_SYNCPT_CSI_WAIT_TIMEOUT                   200
  56
  57 /* VI registers */
  58 #define TEGRA_VI_CFG_VI_INCR_SYNCPT                     0x000
  59 #if (IS_ENABLED(CONFIG_ARCH_TEGRA_12x_SOC) || \
  60         IS_ENABLED(CONFIG_ARCH_TEGRA_13x_SOC))
  61 #define VI_MW_REQ_DONE                                  4
  62 #define VI_MW_ACK_DONE                                  6
  63 #define VI_FRAME_START                                  9
  64 #define VI_LINE_START                                   11
  65 #else
  66 #define VI_LINE_START                                   4
  67 #define VI_FRAME_START                                  5
  68 #define VI_MW_REQ_DONE                                  6
  69 #define VI_MW_ACK_DONE                                  7
  70 #endif
  71
  72 #define TEGRA_VI_CFG_VI_INCR_SYNCPT_CNTRL               0x004
  73 #define TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR               0x008
  74 #define TEGRA_VI_CFG_CTXSW                              0x020
  75 #define TEGRA_VI_CFG_INTSTATUS                          0x024
  76 #define TEGRA_VI_CFG_PWM_CONTROL                        0x038
  77 #define TEGRA_VI_CFG_PWM_HIGH_PULSE                     0x03c
  78 #define TEGRA_VI_CFG_PWM_LOW_PULSE                      0x040
  79 #define TEGRA_VI_CFG_PWM_SELECT_PULSE_A                 0x044
  80 #define TEGRA_VI_CFG_PWM_SELECT_PULSE_B                 0x048
  81 #define TEGRA_VI_CFG_PWM_SELECT_PULSE_C                 0x04c
  82 #define TEGRA_VI_CFG_PWM_SELECT_PULSE_D                 0x050
  83 #define TEGRA_VI_CFG_VGP1                               0x064
  84 #define TEGRA_VI_CFG_VGP2                               0x068
  85 #define TEGRA_VI_CFG_VGP3                               0x06c
  86 #define TEGRA_VI_CFG_VGP4                               0x070
  87 #define TEGRA_VI_CFG_VGP5                               0x074
  88 #define TEGRA_VI_CFG_VGP6                               0x078
  89 #define TEGRA_VI_CFG_INTERRUPT_MASK                     0x08c
  90 #define TEGRA_VI_CFG_INTERRUPT_TYPE_SELECT              0x090
  91 #define TEGRA_VI_CFG_INTERRUPT_POLARITY_SELECT          0x094
  92 #define TEGRA_VI_CFG_INTERRUPT_STATUS                   0x098
  93 #define TEGRA_VI_CFG_VGP_SYNCPT_CONFIG                  0x0ac
  94 #define TEGRA_VI_CFG_VI_SW_RESET                        0x0b4
  95 #define TEGRA_VI_CFG_CG_CTRL                            0x0b8
  96 #define TEGRA_VI_CFG_VI_MCCIF_FIFOCTRL                  0x0e4
  97 #define TEGRA_VI_CFG_TIMEOUT_WCOAL_VI                   0x0e8
  98 #define TEGRA_VI_CFG_DVFS                               0x0f0
  99 #define TEGRA_VI_CFG_RESERVE                            0x0f4
 100 #define TEGRA_VI_CFG_RESERVE_1                          0x0f8
 101
 102 /* CSI registers */
 103 #define TEGRA_VI_CSI_0_BASE                             0x100
 104 #define TEGRA_VI_CSI_1_BASE                             0x200
 105 #define TEGRA_VI_CSI_2_BASE                             0x300
 106 #define TEGRA_VI_CSI_3_BASE                             0x400
 107 #define TEGRA_VI_CSI_4_BASE                             0x500
 108 #define TEGRA_VI_CSI_5_BASE                             0x600
 109
 110 #define TEGRA_VI_CSI_SW_RESET                           0x000
 111 #define TEGRA_VI_CSI_SINGLE_SHOT                        0x004
 112 #define TEGRA_VI_CSI_SINGLE_SHOT_STATE_UPDATE           0x008
 113 #define TEGRA_VI_CSI_IMAGE_DEF                          0x00c
 114 #define TEGRA_VI_CSI_RGB2Y_CTRL                         0x010
 115 #define TEGRA_VI_CSI_MEM_TILING                         0x014
 116 #define TEGRA_VI_CSI_IMAGE_SIZE                         0x018
 117 #define TEGRA_VI_CSI_IMAGE_SIZE_WC                      0x01c
 118 #define TEGRA_VI_CSI_IMAGE_DT                           0x020
 119 #define TEGRA_VI_CSI_SURFACE0_OFFSET_MSB                0x024
 120 #define TEGRA_VI_CSI_SURFACE0_OFFSET_LSB                0x028
 121 #define TEGRA_VI_CSI_SURFACE1_OFFSET_MSB                0x02c
 122 #define TEGRA_VI_CSI_SURFACE1_OFFSET_LSB                0x030
 123 #define TEGRA_VI_CSI_SURFACE2_OFFSET_MSB                0x034
 124 #define TEGRA_VI_CSI_SURFACE2_OFFSET_LSB                0x038
 125 #define TEGRA_VI_CSI_SURFACE0_BF_OFFSET_MSB             0x03c
 126 #define TEGRA_VI_CSI_SURFACE0_BF_OFFSET_LSB             0x040
 127 #define TEGRA_VI_CSI_SURFACE1_BF_OFFSET_MSB             0x044
 128 #define TEGRA_VI_CSI_SURFACE1_BF_OFFSET_LSB             0x048
 129 #define TEGRA_VI_CSI_SURFACE2_BF_OFFSET_MSB             0x04c
 130 #define TEGRA_VI_CSI_SURFACE2_BF_OFFSET_LSB             0x050
 131 #define TEGRA_VI_CSI_SURFACE0_STRIDE                    0x054
 132 #define TEGRA_VI_CSI_SURFACE1_STRIDE                    0x058
 133 #define TEGRA_VI_CSI_SURFACE2_STRIDE                    0x05c
 134 #define TEGRA_VI_CSI_SURFACE_HEIGHT0                    0x060
 135 #define TEGRA_VI_CSI_ISPINTF_CONFIG                     0x064
 136 #define TEGRA_VI_CSI_ERROR_STATUS                       0x084
 137 #define TEGRA_VI_CSI_ERROR_INT_MASK                     0x088
 138 #define TEGRA_VI_CSI_WD_CTRL                            0x08c
 139 #define TEGRA_VI_CSI_WD_PERIOD                          0x090
 140
 141 #define TEGRA_CSI_CSI_CAP_CIL                           0x808
 142 #define TEGRA_CSI_CSI_CAP_CSI                           0x818
 143 #define TEGRA_CSI_CSI_CAP_PP                            0x828
 144
 145 /* CSI Pixel Parser registers */
 146 #define TEGRA_CSI_PIXEL_PARSER_0_BASE                   0x0838
 147 #define TEGRA_CSI_PIXEL_PARSER_1_BASE                   0x086c
 148 #define TEGRA_CSI_PIXEL_PARSER_2_BASE                   0x1038
 149 #define TEGRA_CSI_PIXEL_PARSER_3_BASE                   0x106c
 150 #define TEGRA_CSI_PIXEL_PARSER_4_BASE                   0x1838
 151 #define TEGRA_CSI_PIXEL_PARSER_5_BASE                   0x186c
 152
 153 #define TEGRA_CSI_INPUT_STREAM_CONTROL                  0x000
 154 #define TEGRA_CSI_PIXEL_STREAM_CONTROL0                 0x004
 155 #define TEGRA_CSI_PIXEL_STREAM_CONTROL1                 0x008
 156 #define TEGRA_CSI_PIXEL_STREAM_GAP                      0x00c
 157 #define TEGRA_CSI_PIXEL_STREAM_PP_COMMAND               0x010
 158 #define TEGRA_CSI_PIXEL_STREAM_EXPECTED_FRAME           0x014
 159 #define TEGRA_CSI_PIXEL_PARSER_INTERRUPT_MASK           0x018
 160 #define TEGRA_CSI_PIXEL_PARSER_STATUS                   0x01c
 161 #define TEGRA_CSI_CSI_SW_SENSOR_RESET                   0x020
 162
 163 /* CSI PHY registers */
 164 #define TEGRA_CSI_CIL_PHY_0_BASE                        0x0908
 165 #define TEGRA_CSI_CIL_PHY_1_BASE                        0x1108
 166 #define TEGRA_CSI_CIL_PHY_2_BASE                        0x1908
 167 #define TEGRA_CSI_PHY_CIL_COMMAND                       0x0908
 168
 169 /* CSI CIL registers */
 170 #define TEGRA_CSI_CIL_0_BASE                            0x092c
 171 #define TEGRA_CSI_CIL_1_BASE                            0x0960
 172 #define TEGRA_CSI_CIL_2_BASE                            0x112c
 173 #define TEGRA_CSI_CIL_3_BASE                            0x1160
 174 #define TEGRA_CSI_CIL_4_BASE                            0x192c
 175 #define TEGRA_CSI_CIL_5_BASE                            0x1960
 176
 177 #define TEGRA_CSI_CIL_PAD_CONFIG0                       0x000
 178 #define TEGRA_CSI_CIL_PAD_CONFIG1                       0x004
 179 #define TEGRA_CSI_CIL_PHY_CONTROL                       0x008
 180 #define TEGRA_CSI_CIL_INTERRUPT_MASK                    0x00c
 181 #define TEGRA_CSI_CIL_STATUS                            0x010
 182 #define TEGRA_CSI_CILX_STATUS                           0x014
 183 #define TEGRA_CSI_CIL_ESCAPE_MODE_COMMAND               0x018
 184 #define TEGRA_CSI_CIL_ESCAPE_MODE_DATA                  0x01c
 185 #define TEGRA_CSI_CIL_SW_SENSOR_RESET                   0x020
 186
 187 /* CSI Pattern Generator registers */
 188 #if (IS_ENABLED(CONFIG_ARCH_TEGRA_12x_SOC) || \
 189             IS_ENABLED(CONFIG_ARCH_TEGRA_13x_SOC))
 190 #define TEGRA_CSI_PATTERN_GENERATOR_0_BASE              0xa68
 191 #define TEGRA_CSI_PATTERN_GENERATOR_1_BASE              0xa9c
 192 #else
 193 #define TEGRA_CSI_PATTERN_GENERATOR_0_BASE              0x09c4
 194 #define TEGRA_CSI_PATTERN_GENERATOR_1_BASE              0x09f8
 195 #define TEGRA_CSI_PATTERN_GENERATOR_2_BASE              0x11c4
 196 #define TEGRA_CSI_PATTERN_GENERATOR_3_BASE              0x11f8
 197 #define TEGRA_CSI_PATTERN_GENERATOR_4_BASE              0x19c4
 198 #define TEGRA_CSI_PATTERN_GENERATOR_5_BASE              0x19f8
 199 #endif
 200
 201 #define TEGRA_CSI_PATTERN_GENERATOR_CTRL                0x000
 202 #define TEGRA_CSI_PG_BLANK                              0x004
 203 #define TEGRA_CSI_PG_PHASE                              0x008
 204 #define TEGRA_CSI_PG_RED_FREQ                           0x00c
 205 #define TEGRA_CSI_PG_RED_FREQ_RATE                      0x010
 206 #define TEGRA_CSI_PG_GREEN_FREQ                         0x014
 207 #define TEGRA_CSI_PG_GREEN_FREQ_RATE                    0x018
 208 #define TEGRA_CSI_PG_BLUE_FREQ                          0x01c
 209 #define TEGRA_CSI_PG_BLUE_FREQ_RATE                     0x020
 210 #define TEGRA_CSI_PG_AOHDR                              0x024
 211
 212 #define TEGRA_CSI_DPCM_CTRL_A                           0xad0
 213 #define TEGRA_CSI_DPCM_CTRL_B                           0xad4
 214 #define TEGRA_CSI_STALL_COUNTER                         0xae8
 215 #define TEGRA_CSI_CSI_READONLY_STATUS                   0xaec
 216 #define TEGRA_CSI_CSI_SW_STATUS_RESET                   0xaf0
 217 #define TEGRA_CSI_CLKEN_OVERRIDE                        0xaf4
 218 #define TEGRA_CSI_DEBUG_CONTROL                         0xaf8
 219 #define TEGRA_CSI_DEBUG_COUNTER_0                       0xafc
 220 #define TEGRA_CSI_DEBUG_COUNTER_1                       0xb00
 221 #define TEGRA_CSI_DEBUG_COUNTER_2                       0xb04
 222
 223 /* These go into the TEGRA_VI_CSI_n_IMAGE_DEF registers bits 23:16 */
 224 #define TEGRA_IMAGE_FORMAT_T_L8                         16
 225 #define TEGRA_IMAGE_FORMAT_T_R16_I                      32
 226 #define TEGRA_IMAGE_FORMAT_T_B5G6R5                     33
 227 #define TEGRA_IMAGE_FORMAT_T_R5G6B5                     34
 228 #define TEGRA_IMAGE_FORMAT_T_A1B5G5R5                   35
 229 #define TEGRA_IMAGE_FORMAT_T_A1R5G5B5                   36
 230 #define TEGRA_IMAGE_FORMAT_T_B5G5R5A1                   37
 231 #define TEGRA_IMAGE_FORMAT_T_R5G5B5A1                   38
 232 #define TEGRA_IMAGE_FORMAT_T_A4B4G4R4                   39
 233 #define TEGRA_IMAGE_FORMAT_T_A4R4G4B4                   40
 234 #define TEGRA_IMAGE_FORMAT_T_B4G4R4A4                   41
 235 #define TEGRA_IMAGE_FORMAT_T_R4G4B4A4                   42
 236 #define TEGRA_IMAGE_FORMAT_T_A8B8G8R8                   64
 237 #define TEGRA_IMAGE_FORMAT_T_A8R8G8B8                   65
 238 #define TEGRA_IMAGE_FORMAT_T_B8G8R8A8                   66
 239 #define TEGRA_IMAGE_FORMAT_T_R8G8B8A8                   67
 240 #define TEGRA_IMAGE_FORMAT_T_A2B10G10R10                68
 241 #define TEGRA_IMAGE_FORMAT_T_A2R10G10B10                69
 242 #define TEGRA_IMAGE_FORMAT_T_B10G10R10A2                70
 243 #define TEGRA_IMAGE_FORMAT_T_R10G10B10A2                71
 244 #define TEGRA_IMAGE_FORMAT_T_A8Y8U8V8                   193
 245 #define TEGRA_IMAGE_FORMAT_T_V8U8Y8A8                   194
 246 #define TEGRA_IMAGE_FORMAT_T_A2Y10U10V10                197
 247 #define TEGRA_IMAGE_FORMAT_T_V10U10Y10A2                198
 248 #define TEGRA_IMAGE_FORMAT_T_Y8_U8__Y8_V8               200
 249 #define TEGRA_IMAGE_FORMAT_T_Y8_V8__Y8_U8               201
 250 #define TEGRA_IMAGE_FORMAT_T_U8_Y8__V8_Y8               202
 251 #define TEGRA_IMAGE_FORMAT_T_V8_Y8__U8_Y8               203
 252 #define TEGRA_IMAGE_FORMAT_T_Y8__U8__V8_N444            224
 253 #define TEGRA_IMAGE_FORMAT_T_Y8__U8V8_N444              225
 254 #define TEGRA_IMAGE_FORMAT_T_Y8__V8U8_N444              226
 255 #define TEGRA_IMAGE_FORMAT_T_Y8__U8__V8_N422            227
 256 #define TEGRA_IMAGE_FORMAT_T_Y8__U8V8_N422              228
 257 #define TEGRA_IMAGE_FORMAT_T_Y8__V8U8_N422              229
 258 #define TEGRA_IMAGE_FORMAT_T_Y8__U8__V8_N420            230
 259 #define TEGRA_IMAGE_FORMAT_T_Y8__U8V8_N420              231
 260 #define TEGRA_IMAGE_FORMAT_T_Y8__V8U8_N420              232
 261 #define TEGRA_IMAGE_FORMAT_T_X2Lc10Lb10La10             233
 262 #define TEGRA_IMAGE_FORMAT_T_A2R6R6R6R6R6               234
 263
 264 /* These go into the TEGRA_VI_CSI_n_CSI_IMAGE_DT registers bits 7:0 */
 265 #define TEGRA_IMAGE_DT_YUV420_8                         24
 266 #define TEGRA_IMAGE_DT_YUV420_10                        25
 267 #define TEGRA_IMAGE_DT_YUV420CSPS_8                     28
 268 #define TEGRA_IMAGE_DT_YUV420CSPS_10                    29
 269 #define TEGRA_IMAGE_DT_YUV422_8                         30
 270 #define TEGRA_IMAGE_DT_YUV422_10                        31
 271 #define TEGRA_IMAGE_DT_RGB444                           32
 272 #define TEGRA_IMAGE_DT_RGB555                           33
 273 #define TEGRA_IMAGE_DT_RGB565                           34
 274 #define TEGRA_IMAGE_DT_RGB666                           35
 275 #define TEGRA_IMAGE_DT_RGB888                           36
 276 #define TEGRA_IMAGE_DT_RAW6                             40
 277 #define TEGRA_IMAGE_DT_RAW7                             41
 278 #define TEGRA_IMAGE_DT_RAW8                             42
 279 #define TEGRA_IMAGE_DT_RAW10                            43
 280 #define TEGRA_IMAGE_DT_RAW12                            44
 281 #define TEGRA_IMAGE_DT_RAW14                            45
 282
 283 struct chan_regs_config {
 284         u32 csi_base;
 285         u32 csi_pp_base;
 286         u32 cil_base;
 287         u32 cil_phy_base;
 288         u32 tpg_base;
 289 };
 290
 291 #define csi_regs_write(cam, chan, offset, val) \
 292                 TC_VI_REG_WT(cam, chan->regs.csi_base + offset, val)
 293 #define csi_regs_read(cam, chan, offset) \
 294                 TC_VI_REG_RD(cam, chan->regs.csi_base + offset)
 295 #define csi_pp_regs_write(cam, chan, offset, val) \
 296                 TC_VI_REG_WT(cam, chan->regs.csi_pp_base + offset, val)
 297 #define csi_pp_regs_read(cam, chan, offset) \
 298                 TC_VI_REG_RD(cam, chan->regs.csi_pp_base + offset)
 299 #define cil_regs_write(cam, chan, offset, val) \
 300                 TC_VI_REG_WT(cam, chan->regs.cil_base + offset, val)
 301 #define cil_regs_read(cam, chan, offset) \
 302                 TC_VI_REG_RD(cam, chan->regs.cil_base + offset)
 303 #define cil_phy_reg_write(cam, chan, val) \
 304                 TC_VI_REG_WT(cam, chan->regs.cil_phy_base, val)
 305 #define cil_phy_reg_read(cam, chan) TC_VI_REG_RD(cam, chan->regs.cil_phy_base)
 306 #define tpg_regs_write(cam, chan, offset, val) \
 307                 TC_VI_REG_WT(cam, chan->regs.tpg_base + offset, val)
 308 #define tpg_regs_read(cam, chan, offset) \
 309                 TC_VI_REG_RD(cam, chan->regs.tpg_base + offset)
 310
 311 static struct tegra_io_dpd vi2_io_dpd[] = {
 312         {
 313                 .name                   = "CSIA",
 314                 .io_dpd_reg_index       = 0,
 315                 .io_dpd_bit             = 0,
 316         },
 317         {
 318                 .name                   = "CSIB",
 319                 .io_dpd_reg_index       = 0,
 320                 .io_dpd_bit             = 1,
 321         },
 322         {
 323                 .name                   = "CSIC",
 324                 .io_dpd_reg_index       = 1,
 325                 .io_dpd_bit             = 10,
 326         },
 327         {
 328                 .name                   = "CSID",
 329                 .io_dpd_reg_index       = 1,
 330                 .io_dpd_bit             = 11,
 331         },
 332         {
 333                 .name                   = "CSIE",
 334                 .io_dpd_reg_index       = 1,
 335                 .io_dpd_bit             = 12,
 336         },
 337         {
 338                 .name                   = "CSIF",
 339                 .io_dpd_reg_index       = 1,
 340                 .io_dpd_bit             = 13,
 341         },
 342 };
 343
 344 struct vi2_camera_clk {
 345         const char                      *name;
 346         struct clk                      *clk;
 347         u32                             freq;
 348         int                             use_devname;
 349 };
 350
 351 #define MAX_CHAN_NUM    6
 352
 353 struct vi2_channel {
 354         struct vi2_camera               *vi2_cam;
 355
 356         /* syncpt ids */
 357         u32                             syncpt_id;
 358         u32                             syncpt_thresh;
 359
 360         struct tegra_io_dpd             *dpd;
 361         struct vi2_camera_clk           *clks;
 362         int                             num_clks;
 363
 364         struct chan_regs_config         regs;
 365
 366         struct list_head                capture;
 367         spinlock_t                      start_lock;
 368         struct list_head                done;
 369         spinlock_t                      done_lock;
 370
 371         struct task_struct              *kthread_capture_start;
 372         wait_queue_head_t               start_wait;
 373         struct task_struct              *kthread_capture_done;
 374         wait_queue_head_t               done_wait;
 375
 376         int                             port;
 377         int                             lanes;
 378         s32                             bytes_per_line;
 379         int                             fourcc;
 380         int                             code;
 381         int                             surface;
 382         int                             width;
 383         int                             height;
 384         u8                              bits_per_sample;
 385
 386         int                             sequence;
 387         int                             sof;
 388
 389         int                             buffer_offset;
 390         int                             gang_mode;
 391         int                             gang_refcnt;
 392         struct vi2_channel              *gang_channels[MAX_CHAN_NUM];
 393 };
 394
 395 struct vi2_camera {
 396         struct tegra_camera             cam;
 397
 398         void __iomem                    *reg_base;
 399
 400         struct nvhost_device_data       *ndata;
 401
 402         struct regulator                *reg;
 403         const char                      *regulator_name;
 404
 405         struct vi2_camera_clk           *clks;
 406         int                             num_clks;
 407
 408         struct vi2_channel              channels[MAX_CHAN_NUM];
 409
 410                 /* Test Pattern Generator mode */
 411         int                             tpg_mode;
 412         struct vi2_camera_clk           *tpg_clk;
 413
 414         int                             enable_refcnt;
 415 };
 416
 417 /* Clock settings for camera */
 418 static struct vi2_camera_clk vi2_common_clks[] = {
 419         {
 420                 .name = "vi",
 421                 .freq = 408000000,
 422                 .use_devname = 1,
 423         },
 424         {
 425                 .name = "csi",
 426                 .freq = 408000000,
 427                 .use_devname = 1,
 428         },
 429         {
 430                 .name = "isp",
 431                 .freq = 0,
 432         },
 433         {
 434                 .name = "csus",
 435                 .freq = 0,
 436                 .use_devname = 1,
 437         },
 438         {
 439                 .name = "sclk",
 440                 .freq = 80000000,
 441         },
 442         {
 443                 .name = "emc",
 444                 .freq = 300000000,
 445                 .use_devname = 1,
 446         },
 447 };
 448
 449 static struct vi2_camera_clk vi2_clks_tpg = {
 450         .name = "pll_d",
 451         .freq = 927000000,
 452 };
 453
 454 static struct vi2_camera_clk vi2_clks_ab[] = {
 455         {
 456                 .name = "vi_sensor",
 457                 .freq = 24000000,
 458         },
 459         {
 460                 .name = "cilab",
 461                 .freq = 102000000,
 462                 .use_devname = 1,
 463         },
 464 };
 465
 466 static struct vi2_camera_clk vi2_clks_cd[] = {
 467         {
 468                 .name = "vi_sensor2",
 469                 .freq = 24000000,
 470         },
 471         {
 472                 .name = "cilcd",
 473                 .freq = 102000000,
 474                 .use_devname = 1,
 475         },
 476 };
 477
 478 static struct vi2_camera_clk vi2_clks_ef[] = {
 479         {
 480                 .name = "cile",
 481                 .freq = 102000000,
 482                 .use_devname = 1,
 483         },
 484 };
 485
 486 static const struct soc_mbus_pixelfmt vi2_tpg_format = {
 487         .fourcc                 = V4L2_PIX_FMT_RGB32,
 488         .name                   = "RGBA 8-8-8-8",
 489         .bits_per_sample        = 32,
 490         .packing                = SOC_MBUS_PACKING_NONE,
 491         .order                  = SOC_MBUS_ORDER_LE,
 492 };
 493
 494 #define MAX_DEVID_LENGTH        16
 495
 496 static void vi2_init_syncpts(struct vi2_channel *chan)
 497 {
 498         chan->syncpt_id = nvhost_get_syncpt_client_managed("vi");
 499 }
 500
 501 static void vi2_free_syncpts(struct vi2_channel *chan)
 502 {
 503         nvhost_free_syncpt(chan->syncpt_id);
 504 }
 505
 506 static u32 vi2_syncpt_cond(u32 cond, int port)
 507 {
 508         if (IS_ENABLED(CONFIG_ARCH_TEGRA_12x_SOC) ||
 509             IS_ENABLED(CONFIG_ARCH_TEGRA_13x_SOC))
 510                 return (cond + port * 1) << 8;
 511         else
 512                 return (cond + port * 4) << 8;
 513 }
 514
 515 static int vi2_clock_start(struct vi2_camera *vi2_cam,
 516                            struct vi2_camera_clk *clks, int num_clks)
 517 {
 518         struct tegra_camera *cam = (struct tegra_camera *)vi2_cam;
 519         struct platform_device *pdev = cam->pdev;
 520         struct vi2_camera_clk *vi2_clk;
 521         int i;
 522
 523         for (i = 0; i < num_clks; i++) {
 524                 vi2_clk = &clks[i];
 525
 526                 if (vi2_clk->use_devname) {
 527                         char devname[MAX_DEVID_LENGTH];
 528                         snprintf(devname, MAX_DEVID_LENGTH,
 529                                  "tegra_%s", dev_name(&pdev->dev));
 530                         vi2_clk->clk = clk_get_sys(devname, vi2_clk->name);
 531                 } else
 532                         vi2_clk->clk = clk_get(&pdev->dev, vi2_clk->name);
 533
 534                 if (!IS_ERR_OR_NULL(vi2_clk->clk)) {
 535                         clk_prepare_enable(vi2_clk->clk);
 536                         if (vi2_clk->freq > 0)
 537                                 clk_set_rate(vi2_clk->clk, vi2_clk->freq);
 538                 } else {
 539                         dev_err(&pdev->dev, "Failed to get clock %s.\n",
 540                                 vi2_clk->name);
 541                         return PTR_ERR(vi2_clk->clk);
 542                 }
 543         }
 544
 545         return 0;
 546 }
 547
 548 static int vi2_common_clock_start(struct vi2_camera *vi2_cam)
 549 {
 550         int ret;
 551
 552         vi2_cam->num_clks = ARRAY_SIZE(vi2_common_clks);
 553         vi2_cam->clks = vi2_common_clks;
 554         ret = vi2_clock_start(vi2_cam, vi2_cam->clks, vi2_cam->num_clks);
 555         if (ret)
 556                 return ret;
 557
 558         if (vi2_cam->tpg_mode) {
 559                 vi2_cam->tpg_clk = &vi2_clks_tpg;
 560                 ret = vi2_clock_start(vi2_cam, vi2_cam->tpg_clk, 1);
 561                 if (!ret) {
 562                         tegra_clk_cfg_ex(vi2_cam->tpg_clk->clk,
 563                                         TEGRA_CLK_PLLD_CSI_OUT_ENB, 1);
 564                         tegra_clk_cfg_ex(vi2_cam->tpg_clk->clk,
 565                                         TEGRA_CLK_PLLD_DSI_OUT_ENB, 1);
 566                         tegra_clk_cfg_ex(vi2_cam->tpg_clk->clk,
 567                                         TEGRA_CLK_MIPI_CSI_OUT_ENB, 0);
 568                 }
 569         }
 570
 571         return ret;
 572 }
 573
 574 static void vi2_clock_stop(struct vi2_camera_clk *clks, int num_clks)
 575 {
 576         struct vi2_camera_clk *vi2_clk;
 577         int i;
 578
 579         for (i = 0; i < num_clks; i++) {
 580                 vi2_clk = &clks[i];
 581                 if (!IS_ERR_OR_NULL(vi2_clk->clk)) {
 582                         clk_disable_unprepare(vi2_clk->clk);
 583                         clk_put(vi2_clk->clk);
 584                 }
 585                 vi2_clk->clk = NULL;
 586         }
 587 }
 588
 589 static void vi2_common_clock_stop(struct vi2_camera *vi2_cam)
 590 {
 591         vi2_clock_stop(vi2_cam->clks, vi2_cam->num_clks);
 592
 593         if (vi2_cam->tpg_mode) {
 594                 if (!IS_ERR(vi2_cam->tpg_clk->clk)) {
 595                         tegra_clk_cfg_ex(vi2_cam->tpg_clk->clk,
 596                                          TEGRA_CLK_MIPI_CSI_OUT_ENB, 1);
 597                         tegra_clk_cfg_ex(vi2_cam->tpg_clk->clk,
 598                                          TEGRA_CLK_PLLD_CSI_OUT_ENB, 0);
 599                         tegra_clk_cfg_ex(vi2_cam->tpg_clk->clk,
 600                                          TEGRA_CLK_PLLD_DSI_OUT_ENB, 0);
 601                         clk_disable_unprepare(vi2_cam->tpg_clk->clk);
 602                         clk_put(vi2_cam->tpg_clk->clk);
 603                         vi2_cam->tpg_clk->clk = NULL;
 604                 }
 605         }
 606 }
 607
 608 static u32 vi2_cal_regs_base(u32 regs_base, int port)
 609 {
 610         return regs_base + (port / 2 * 0x800) + (port & 1) * 0x34;
 611 }
 612
 613 static int vi2_channel_capture_frame(struct vi2_channel *chan,
 614                                      struct tegra_camera_buffer *buf);
 615
 616 static int vi2_channel_kthread_capture_start(void *data)
 617 {
 618         struct vi2_channel *chan = data;
 619         struct tegra_camera_buffer *buf;
 620
 621         set_freezable();
 622
 623         while (1) {
 624                 try_to_freeze();
 625                 wait_event_interruptible(chan->start_wait,
 626                                 !list_empty(&chan->capture) ||
 627                                 kthread_should_stop());
 628                 if (kthread_should_stop())
 629                         break;
 630
 631                 spin_lock(&chan->start_lock);
 632                 if (list_empty(&chan->capture)) {
 633                         spin_unlock(&chan->start_lock);
 634                         continue;
 635                 }
 636
 637                 buf = list_entry(chan->capture.next, struct tegra_camera_buffer,
 638                                  queue);
 639                 list_del_init(&buf->queue);
 640                 spin_unlock(&chan->start_lock);
 641
 642                 vi2_channel_capture_frame(chan, buf);
 643         }
 644
 645         return 0;
 646 }
 647
 648 static int vi2_channel_capture_done(struct vi2_channel *chan,
 649                                     struct tegra_camera_buffer *buf);
 650
 651 static int vi2_channel_kthread_capture_done(void *data)
 652 {
 653         struct vi2_channel *chan = data;
 654         struct tegra_camera_buffer *buf;
 655
 656         set_freezable();
 657
 658         while (1) {
 659                 try_to_freeze();
 660                 wait_event_interruptible(chan->done_wait,
 661                                 !list_empty(&chan->done) ||
 662                                 kthread_should_stop());
 663                 if (kthread_should_stop() && list_empty(&chan->done))
 664                         break;
 665
 666                 spin_lock(&chan->done_lock);
 667                 if (list_empty(&chan->done)) {
 668                         spin_unlock(&chan->done_lock);
 669                         continue;
 670                 }
 671
 672                 buf = list_entry(chan->done.next, struct tegra_camera_buffer,
 673                                 queue);
 674                 list_del_init(&buf->queue);
 675                 spin_unlock(&chan->done_lock);
 676
 677                 vi2_channel_capture_done(chan, buf);
 678         }
 679
 680         return 0;
 681 }
 682
 683 static s32 vi2_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf);
 684
 685 static int vi2_get_gang_mode(struct vi2_camera *vi2_cam,
 686                              struct soc_camera_device *icd)
 687 {
 688         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 689         struct v4l2_control gang_mode_control;
 690         int gang_mode = CAMERA_GANG_DISABLED;
 691
 692         if (sd) {
 693                 gang_mode_control.id = V4L2_CID_GANG_MODE;
 694                 if (!v4l2_subdev_g_ctrl(sd, &gang_mode_control))
 695                         gang_mode = gang_mode_control.value;
 696         }
 697         return gang_mode;
 698 }
 699
 700 static int vi2_channel_init(struct vi2_camera *vi2_cam,
 701                             struct soc_camera_device *icd, int port)
 702 {
 703         struct vi2_channel *chan = &vi2_cam->channels[port];
 704         struct chan_regs_config *regs = &chan->regs;
 705
 706         INIT_LIST_HEAD(&chan->capture);
 707         INIT_LIST_HEAD(&chan->done);
 708         spin_lock_init(&chan->start_lock);
 709         spin_lock_init(&chan->done_lock);
 710         init_waitqueue_head(&chan->start_wait);
 711         init_waitqueue_head(&chan->done_wait);
 712
 713         chan->dpd = &vi2_io_dpd[port];
 714         chan->vi2_cam = vi2_cam;
 715         chan->sequence = 0;
 716         chan->surface = 0;
 717         chan->sof = 1;
 718         chan->port = port;
 719         chan->lanes = icd_to_lanes(icd);
 720         chan->buffer_offset = 0;
 721
 722         if (IS_ENABLED(CONFIG_ARCH_TEGRA_12x_SOC) ||
 723             IS_ENABLED(CONFIG_ARCH_TEGRA_13x_SOC))
 724                 /* Init and start channel related clocks */
 725                 if (port == TEGRA_CAMERA_PORT_CSI_A) {
 726                         chan->clks = vi2_clks_ab;
 727                         chan->num_clks = ARRAY_SIZE(vi2_clks_ab);
 728                 } else {
 729                         chan->clks = vi2_clks_cd;
 730                         chan->num_clks = ARRAY_SIZE(vi2_clks_cd);
 731                 }
 732         else {
 733                 /* Init and start channel related clocks */
 734                 switch (port) {
 735                 case TEGRA_CAMERA_PORT_CSI_A:
 736                 case TEGRA_CAMERA_PORT_CSI_B:
 737                         chan->clks = vi2_clks_ab;
 738                         chan->num_clks = ARRAY_SIZE(vi2_clks_ab);
 739                         break;
 740                 case TEGRA_CAMERA_PORT_CSI_C:
 741                 case TEGRA_CAMERA_PORT_CSI_D:
 742                         chan->clks = vi2_clks_cd;
 743                         chan->num_clks = ARRAY_SIZE(vi2_clks_cd);
 744                         break;
 745                 case TEGRA_CAMERA_PORT_CSI_E:
 746                 case TEGRA_CAMERA_PORT_CSI_F:
 747                         chan->clks = vi2_clks_ef;
 748                         chan->num_clks = ARRAY_SIZE(vi2_clks_ef);
 749                         break;
 750                 default:
 751                         return -EINVAL;
 752                 }
 753         }
 754
 755         vi2_clock_start(vi2_cam, chan->clks, chan->num_clks);
 756
 757         tegra_io_dpd_disable(chan->dpd);
 758
 759         /* Init syncpts */
 760         vi2_init_syncpts(chan);
 761
 762         /* Init channel register base */
 763         regs->csi_base = TEGRA_VI_CSI_0_BASE + port * 0x100;
 764         regs->csi_pp_base = vi2_cal_regs_base(TEGRA_CSI_PIXEL_PARSER_0_BASE,
 765                             port);
 766         regs->cil_base = vi2_cal_regs_base(TEGRA_CSI_CIL_0_BASE, port);
 767         regs->cil_phy_base = TEGRA_CSI_CIL_PHY_0_BASE + port / 2 * 0x800;
 768         regs->tpg_base = vi2_cal_regs_base(TEGRA_CSI_PATTERN_GENERATOR_0_BASE,
 769                          port);
 770
 771         /* Clean up status */
 772         cil_regs_write(vi2_cam, chan, TEGRA_CSI_CIL_STATUS, 0xFFFFFFFF);
 773         cil_regs_write(vi2_cam, chan, TEGRA_CSI_CILX_STATUS, 0xFFFFFFFF);
 774         csi_pp_regs_write(vi2_cam, chan, TEGRA_CSI_PIXEL_PARSER_STATUS,
 775                           0xFFFFFFFF);
 776         csi_regs_write(vi2_cam, chan, TEGRA_VI_CSI_ERROR_STATUS, 0xFFFFFFFF);
 777
 778         return 0;
 779 }
 780
 781 static void vi2_channel_deinit(struct vi2_camera *vi2_cam,
 782                                struct soc_camera_device *icd, int port)
 783 {
 784         struct vi2_channel *chan = &vi2_cam->channels[port];
 785
 786         /* free syncpts */
 787         vi2_free_syncpts(chan);
 788         chan->sequence = 0;
 789         chan->sof = 0;
 790         tegra_io_dpd_enable(chan->dpd);
 791         vi2_clock_stop(chan->clks, chan->num_clks);
 792 }
 793
 794 static int vi2_camera_activate(struct vi2_camera *vi2_cam)
 795 {
 796         int ret = 0;
 797
 798         /* Init and start common clocks */
 799         vi2_common_clock_start(vi2_cam);
 800
 801         ret = nvhost_module_busy_ext(vi2_cam->cam.pdev);
 802         if (ret) {
 803                 dev_err(&vi2_cam->cam.pdev->dev, "nvhost module is busy\n");
 804                 goto exit;
 805         }
 806
 807         /* Enable external power */
 808         if (vi2_cam->reg) {
 809                 ret = regulator_enable(vi2_cam->reg);
 810                 if (ret)
 811                         dev_err(&vi2_cam->cam.pdev->dev,
 812                                 "enabling regulator failed\n");
 813         }
 814
 815         /* Reset VI2/CSI2 when activating, no sepecial ops for deactiving  */
 816         /* T12_CG_2ND_LEVEL_EN */
 817         TC_VI_REG_WT(vi2_cam, TEGRA_VI_CFG_CG_CTRL, 1);
 818         TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CLKEN_OVERRIDE, 0x0);
 819         udelay(10);
 820
 821         /* Unpowergate VE */
 822         tegra_unpowergate_partition(TEGRA_POWERGATE_VENC);
 823
 824         return 0;
 825
 826 exit:
 827         vi2_common_clock_stop(vi2_cam);
 828         return ret;
 829 }
 830
 831 static int vi2_add_device(struct soc_camera_device *icd)
 832 {
 833         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 834         struct tegra_camera *cam = ici->priv;
 835         struct vi2_camera *vi2_cam = (struct vi2_camera *)cam;
 836         int port = icd_to_port(icd);
 837         struct vi2_channel *chan = &vi2_cam->channels[port];
 838         int gang_port = icd_to_gang_port(icd);
 839         int i, ret;
 840
 841         if (!vi2_cam->enable_refcnt) {
 842                 ret = vi2_camera_activate(vi2_cam);
 843                 if (ret)
 844                         return ret;
 845         }
 846
 847         vi2_channel_init(vi2_cam, icd, port);
 848
 849         /* Set base channel as gang_channel 0  */
 850         chan->gang_refcnt = 0;
 851         chan->gang_channels[chan->gang_refcnt++] = chan;
 852         chan->gang_mode = vi2_get_gang_mode(vi2_cam, icd);
 853
 854         /* Set required gang channel for gang mode */
 855         if (chan->gang_mode) {
 856                 for (i = 0; i < MAX_CHAN_NUM; i++) {
 857                         if (gang_port == i) {
 858                                 vi2_channel_init(vi2_cam, icd, gang_port);
 859                                 chan->gang_channels[chan->gang_refcnt++] =
 860                                         &vi2_cam->channels[gang_port];
 861                         }
 862                 }
 863         }
 864
 865         vi2_cam->enable_refcnt++;
 866
 867         return 0;
 868 }
 869
 870 static void vi2_camera_deactivate(struct vi2_camera *vi2_cam)
 871 {
 872         vi2_common_clock_stop(vi2_cam);
 873
 874         /* Powergate VE */
 875         tegra_powergate_partition(TEGRA_POWERGATE_VENC);
 876
 877         /* Disable external power */
 878         if (vi2_cam->reg)
 879                 regulator_disable(vi2_cam->reg);
 880
 881         nvhost_module_idle_ext(vi2_cam->cam.pdev);
 882 }
 883
 884 static void vi2_remove_device(struct soc_camera_device *icd)
 885 {
 886         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 887         struct tegra_camera *cam = ici->priv;
 888         struct vi2_camera *vi2_cam = (struct vi2_camera *)cam;
 889         int port = icd_to_port(icd);
 890         struct vi2_channel *chan = &vi2_cam->channels[port];
 891         struct vi2_channel *channel;
 892         int i;
 893
 894         vi2_cam->enable_refcnt--;
 895
 896         for (i = 0; i < chan->gang_refcnt; i++) {
 897                 channel = chan->gang_channels[i];
 898                 vi2_channel_deinit(vi2_cam, icd, channel->port);
 899         }
 900
 901         if (!vi2_cam->enable_refcnt)
 902                 vi2_camera_deactivate(vi2_cam);
 903 }
 904
 905 static bool vi2_port_is_valid(int port)
 906 {
 907         return (((port) >= TEGRA_CAMERA_PORT_CSI_A) &&
 908                 ((port) <= TEGRA_CAMERA_PORT_CSI_F));
 909 }
 910
 911 static s32 vi2_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf)
 912 {
 913         s32 bytes_per_line = soc_mbus_bytes_per_line(width, mf);
 914
 915         if (bytes_per_line % 64)
 916                 bytes_per_line = bytes_per_line + (64 - (bytes_per_line % 64));
 917
 918         return bytes_per_line;
 919 }
 920
 921 static bool vi2_ignore_subdev_fmt(struct tegra_camera *cam)
 922 {
 923         struct vi2_camera *vi2_cam = (struct vi2_camera *)cam;
 924
 925         /* If we are in TPG mode, we ignore the subdev's supported formats. */
 926         return vi2_cam->tpg_mode;
 927 }
 928
 929 static int vi2_get_formats(struct soc_camera_device *icd,
 930                            unsigned int idx,
 931                            struct soc_camera_format_xlate *xlate)
 932 {
 933         struct device *dev = icd->parent;
 934         struct soc_camera_host *ici = to_soc_camera_host(dev);
 935         struct vi2_camera *vi2_cam = ici->priv;
 936
 937         /*
 938          * If we are not in TPG mode, then let the regular get_formats handler
 939          * handle this.
 940          */
 941         if (!vi2_cam->tpg_mode)
 942                 return -EAGAIN;
 943
 944         if (xlate) {
 945                 xlate->host_fmt = &vi2_tpg_format;
 946                 xlate->code = V4L2_MBUS_FMT_RGBA8888_4X8_LE;
 947         }
 948
 949         return 1;
 950 }
 951
 952 /*
 953  * vi2_try_mbus_fmt() and vi2_s_mbus_fmt(): Tegra's VI2 camera host controller
 954  * can only support widths of multiples of 4.  In addition, though it doesn't
 955  * affect these functions but is just an interesting point, Tegra's VI2 camera
 956  * host controller will only output lines to memory in multiples of 64 bytes.
 957  * an image width that results in a line stride that is not a multiple of 64
 958  * bytes will result in the line being padded in order to satisfy the multiple
 959  * of 64 line stride (see vi2_bytes_per_line()).
 960  */
 961 static int vi2_try_mbus_fmt(struct v4l2_subdev *sd,
 962                             struct v4l2_mbus_framefmt *mf)
 963 {
 964         if (mf->width % 4) {
 965                 int width;
 966                 int ret;
 967
 968                 /* First, clamp our width to a multiple of 4. */
 969                 mf->width = ALIGN(mf->width, 4);
 970
 971                 /*
 972                  * Remember this new width, and ask the subdev if this width
 973                  * is okay.
 974                  */
 975                 width = mf->width;
 976                 ret = v4l2_subdev_call(sd, video, try_mbus_fmt, mf);
 977                 if (IS_ERR_VALUE(ret))
 978                         return ret;
 979
 980                 /* If this new width is not okay with the subdev, we fail. */
 981                 if (mf->width != width)
 982                         return -EINVAL;
 983         }
 984
 985         return 0;
 986 }
 987
 988 static int vi2_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 989 {
 990         if (mf->width % 4)
 991                 return -EINVAL;
 992
 993         return 0;
 994 }
 995
 996 static void vi2_channel_fmt_setup(struct tegra_camera *cam,
 997                                struct soc_camera_device *icd)
 998 {
 999         struct vi2_camera *vi2_cam = (struct vi2_camera *)cam;
1000         int port = icd_to_port(icd);
1001         struct vi2_channel *chan = &vi2_cam->channels[port];
1002         struct vi2_channel *channel;
1003         int i;
1004
1005         if (!icd->current_fmt)
1006                 return;
1007
1008         for (i = 0; i < chan->gang_refcnt; i++) {
1009                 channel = chan->gang_channels[i];
1010                 channel->fourcc = icd->current_fmt->host_fmt->fourcc;
1011                 channel->code = icd->current_fmt->code;
1012                 channel->bytes_per_line = vi2_bytes_per_line(icd->user_width,
1013                                 icd->current_fmt->host_fmt);
1014                 channel->bits_per_sample =
1015                         icd->current_fmt->host_fmt->bits_per_sample;
1016
1017                 channel->width = icd->user_width;
1018                 channel->height = icd->user_height;
1019                 switch (chan->gang_mode) {
1020                 case CAMERA_GANG_L_R:
1021                 case CAMERA_GANG_R_L:
1022                         channel->width = icd->user_width / 2;
1023                         channel->height = icd->user_height;
1024                         break;
1025                 case CAMERA_GANG_T_B:
1026                 case CAMERA_GANG_B_T:
1027                         channel->width = icd->user_width;
1028                         channel->height = icd->user_height / 2;
1029                         break;
1030                 default:
1031                         channel->width = icd->user_width;
1032                         channel->height = icd->user_height;
1033                         break;
1034                 };
1035                 if ((i == 0 && chan->gang_mode == CAMERA_GANG_R_L) ||
1036                     (i == 1 && chan->gang_mode == CAMERA_GANG_L_R))
1037                         channel->buffer_offset =
1038                                 (channel->width *
1039                                  channel->bits_per_sample) >> 3;
1040                 else if ((i == 0 && chan->gang_mode == CAMERA_GANG_B_T) ||
1041                          (i == 1 && chan->gang_mode == CAMERA_GANG_T_B))
1042                         channel->buffer_offset =
1043                                 (channel->width * channel->height *
1044                                  channel->bits_per_sample) >> 3;
1045         }
1046 }
1047
1048 static void vi2_videobuf_queue(struct tegra_camera *cam,
1049                                struct soc_camera_device *icd,
1050                                struct tegra_camera_buffer *buf)
1051 {
1052         struct vi2_camera *vi2_cam = (struct vi2_camera *)cam;
1053         int port = icd_to_port(icd);
1054         struct vi2_channel *chan = &vi2_cam->channels[port];
1055
1056         vi2_channel_fmt_setup(cam, icd);
1057
1058         spin_lock(&chan->start_lock);
1059         list_add_tail(&buf->queue, &chan->capture);
1060         spin_unlock(&chan->start_lock);
1061
1062         /* Wait up kthread for capture */
1063         wake_up_interruptible(&chan->start_wait);
1064 }
1065
1066 static int vi2_start_streaming(struct tegra_camera *cam,
1067                                struct soc_camera_device *icd,
1068                                unsigned int count)
1069 {
1070         struct vi2_camera *vi2_cam = (struct vi2_camera *)cam;
1071         int port = icd_to_port(icd);
1072         struct vi2_channel *chan = &vi2_cam->channels[port];
1073
1074         /* Start kthread to capture data to buffer */
1075         chan->kthread_capture_start = kthread_run(
1076                                         vi2_channel_kthread_capture_start, chan,
1077                                         "vi2_channel:%d_0", port);
1078         if (IS_ERR(chan->kthread_capture_start)) {
1079                 dev_err(&vi2_cam->cam.pdev->dev,
1080                         "failed to run kthread for capture start!\n");
1081                 return PTR_ERR(chan->kthread_capture_start);
1082         }
1083
1084         chan->kthread_capture_done = kthread_run(
1085                                         vi2_channel_kthread_capture_done, chan,
1086                                         "vi2_channel:%d_1", port);
1087         if (IS_ERR(chan->kthread_capture_done)) {
1088                 dev_err(&vi2_cam->cam.pdev->dev,
1089                         "failed to run kthread for capture done!\n");
1090                 return PTR_ERR(chan->kthread_capture_done);
1091         }
1092
1093         return 0;
1094 }
1095
1096 static int vi2_stop_streaming(struct tegra_camera *cam,
1097                               struct soc_camera_device *icd)
1098 {
1099         struct vi2_camera *vi2_cam = (struct vi2_camera *)cam;
1100         int port = icd_to_port(icd);
1101         struct vi2_channel *chan = &vi2_cam->channels[port];
1102
1103         /* Stop the kthread for capture */
1104         kthread_stop(chan->kthread_capture_start);
1105         chan->kthread_capture_start = NULL;
1106         kthread_stop(chan->kthread_capture_done);
1107         chan->kthread_capture_done = NULL;
1108
1109         return 0;
1110 }
1111
1112 #define TEGRA_CSI_CILA_PAD_CONFIG0      0x92c
1113 #define TEGRA_CSI_CILB_PAD_CONFIG0      0x960
1114 #define TEGRA_CSI_CILC_PAD_CONFIG0      0x994
1115 #define TEGRA_CSI_CILD_PAD_CONFIG0      0x9c8
1116 #define TEGRA_CSI_CILE_PAD_CONFIG0      0xa08
1117
1118 #define TEGRA_CSI_CSI_CIL_A_INTERRUPT_MASK      0x938
1119 #define TEGRA_CSI_CSI_CIL_B_INTERRUPT_MASK      0x96c
1120 #define TEGRA_CSI_CSI_CIL_C_INTERRUPT_MASK      0x9a0
1121 #define TEGRA_CSI_CSI_CIL_D_INTERRUPT_MASK      0x9d4
1122 #define TEGRA_CSI_CSI_CIL_E_INTERRUPT_MASK      0xa14
1123
1124 #define TEGRA_CSI_PHY_CILA_CONTROL0     0x934
1125 #define TEGRA_CSI_PHY_CILB_CONTROL0     0x968
1126 #define TEGRA_CSI_PHY_CILC_CONTROL0     0x99c
1127 #define TEGRA_CSI_PHY_CILD_CONTROL0     0x9d0
1128 #define TEGRA_CSI_PHY_CILE_CONTROL0     0xa10
1129
1130 static void vi2_capture_setup_cil_t124(struct vi2_camera *vi2_cam, int port)
1131 {
1132         if (port == TEGRA_CAMERA_PORT_CSI_A) {
1133                 /*
1134                  * PAD_CILA_PDVCLAMP 0, PAD_CILA_PDIO_CLK 0,
1135                  * PAD_CILA_PDIO 0, PAD_AB_BK_MODE 1
1136                  */
1137                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CILA_PAD_CONFIG0, 0x10000);
1138
1139                 /* PAD_CILB_PDVCLAMP 0, PAD_CILB_PDIO_CLK 0, PAD_CILB_PDIO 0 */
1140                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CILB_PAD_CONFIG0, 0x0);
1141
1142                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CSI_CIL_A_INTERRUPT_MASK, 0x0);
1143                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CSI_CIL_B_INTERRUPT_MASK, 0x0);
1144
1145 #ifdef DEBUG
1146                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_DEBUG_CONTROL,
1147                              0x3 | (0x1 << 5) | (0x40 << 8));
1148 #endif
1149                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CILA_CONTROL0, 0x9);
1150                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CILB_CONTROL0, 0x9);
1151         } else {
1152                 /*
1153                  * PAD_CILC_PDVCLAMP 0, PAD_CILC_PDIO_CLK 0,
1154                  * PAD_CILC_PDIO 0, PAD_CD_BK_MODE 1
1155                  */
1156                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CILC_PAD_CONFIG0, 0x10000);
1157
1158                 /* PAD_CILD_PDVCLAMP 0, PAD_CILD_PDIO_CLK 0, PAD_CILD_PDIO 0 */
1159                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CILD_PAD_CONFIG0, 0x0);
1160
1161                 /* PAD_CILE_PDVCLAMP 0, PAD_CILE_PDIO_CLK 0, PAD_CILE_PDIO 0 */
1162                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CILE_PAD_CONFIG0, 0x0);
1163
1164                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CSI_CIL_C_INTERRUPT_MASK, 0x0);
1165                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CSI_CIL_D_INTERRUPT_MASK, 0x0);
1166                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_CSI_CIL_E_INTERRUPT_MASK, 0x0);
1167 #ifdef DEBUG
1168                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_DEBUG_CONTROL,
1169                                 0x5 | (0x1 << 5) | (0x50 << 8));
1170 #endif
1171                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CILC_CONTROL0, 0x9);
1172                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CILD_CONTROL0, 0x9);
1173                 TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CILE_CONTROL0, 0x9);
1174         }
1175 }
1176
1177 static void vi2_capture_setup_cil_phy_t124(struct vi2_camera *vi2_cam,
1178                                            int lanes, int port)
1179 {
1180         u32 val;
1181
1182         /* Shared register */
1183         val = TC_VI_REG_RD(vi2_cam, TEGRA_CSI_PHY_CIL_COMMAND);
1184         if (port == TEGRA_CAMERA_PORT_CSI_A) {
1185                 if (lanes == 4)
1186                         TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CIL_COMMAND,
1187                                         (val & 0xFFFF0000) | 0x0101);
1188                 else
1189                         TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CIL_COMMAND,
1190                                         (val & 0xFFFF0000) | 0x0201);
1191         } else {
1192                 if (lanes == 4)
1193                         TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CIL_COMMAND,
1194                                         (val & 0x0000FFFF) | 0x21010000);
1195                 else if (lanes == 1)
1196                         TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CIL_COMMAND,
1197                                         (val & 0x0000FFFF) | 0x12020000);
1198                 else
1199                         TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CIL_COMMAND,
1200                                         (val & 0x0000FFFF) | 0x22010000);
1201         }
1202 }
1203
1204 static int vi2_channel_capture_setup(struct vi2_channel *chan)
1205 {
1206         struct vi2_camera *vi2_cam = chan->vi2_cam;
1207         int port = chan->port;
1208         int lanes = chan->lanes;
1209         int width = chan->width;
1210         int height = chan->height;
1211         struct chan_regs_config *regs = &chan->regs;
1212         int format = 0, data_type = 0, image_size = 0;
1213         int bypass_pixel_transform = 0;
1214
1215         /* Skip VI2/CSI2 setup for second and later frame capture */
1216         if (!chan->sof)
1217                 return 0;
1218
1219         /* CIL PHY register setup */
1220         if (IS_ENABLED(CONFIG_ARCH_TEGRA_12x_SOC) ||
1221                         IS_ENABLED(CONFIG_ARCH_TEGRA_13x_SOC))
1222                 vi2_capture_setup_cil_t124(vi2_cam, port);
1223         else {
1224                 if (port & 0x1) {
1225                         cil_regs_write(vi2_cam, chan,
1226                                        TEGRA_CSI_CIL_PAD_CONFIG0 - 0x34, 0x0);
1227                         cil_regs_write(vi2_cam, chan, TEGRA_CSI_CIL_PAD_CONFIG0,
1228                                        0x0);
1229                 } else {
1230                         cil_regs_write(vi2_cam, chan, TEGRA_CSI_CIL_PAD_CONFIG0,
1231                                        0x10000);
1232                         cil_regs_write(vi2_cam, chan,
1233                                        TEGRA_CSI_CIL_PAD_CONFIG0 + 0x34,
1234                                        0x0);
1235                 }
1236
1237                 cil_regs_write(vi2_cam, chan, TEGRA_CSI_CIL_INTERRUPT_MASK,
1238                                0x0);
1239                 cil_regs_write(vi2_cam, chan, TEGRA_CSI_CIL_PHY_CONTROL, 0xA);
1240                 if (lanes == 4) {
1241                         regs->cil_base = vi2_cal_regs_base(TEGRA_CSI_CIL_0_BASE,
1242                                                            port + 1);
1243                         cil_regs_write(vi2_cam, chan, TEGRA_CSI_CIL_PAD_CONFIG0,
1244                                        0x0);
1245                         cil_regs_write(vi2_cam, chan,
1246                                        TEGRA_CSI_CIL_INTERRUPT_MASK, 0x0);
1247                         cil_regs_write(vi2_cam, chan, TEGRA_CSI_CIL_PHY_CONTROL,
1248                                        0xA);
1249                         regs->cil_base = vi2_cal_regs_base(TEGRA_CSI_CIL_0_BASE,
1250                                                            port);
1251                 }
1252         }
1253
1254         /* CSI pixel parser registers setup */
1255         csi_pp_regs_write(vi2_cam, chan, TEGRA_CSI_PIXEL_STREAM_PP_COMMAND,
1256                           0xf007);
1257         csi_pp_regs_write(vi2_cam, chan, TEGRA_CSI_PIXEL_PARSER_INTERRUPT_MASK,
1258                           0x0);
1259         csi_pp_regs_write(vi2_cam, chan, TEGRA_CSI_PIXEL_STREAM_CONTROL0,
1260                           0x280301f0 | (port & 0x1));
1261         csi_pp_regs_write(vi2_cam, chan, TEGRA_CSI_PIXEL_STREAM_PP_COMMAND,
1262                           0xf007);
1263         csi_pp_regs_write(vi2_cam, chan, TEGRA_CSI_PIXEL_STREAM_CONTROL1, 0x11);
1264         csi_pp_regs_write(vi2_cam, chan, TEGRA_CSI_PIXEL_STREAM_GAP, 0x140000);
1265         csi_pp_regs_write(vi2_cam, chan, TEGRA_CSI_PIXEL_STREAM_EXPECTED_FRAME,
1266                           0x0);
1267         csi_pp_regs_write(vi2_cam, chan, TEGRA_CSI_INPUT_STREAM_CONTROL,
1268                           0x3f0000 | (lanes - 1));
1269
1270         /* CIL PHY register setup */
1271         if (IS_ENABLED(CONFIG_ARCH_TEGRA_12x_SOC) ||
1272             IS_ENABLED(CONFIG_ARCH_TEGRA_13x_SOC))
1273                 vi2_capture_setup_cil_phy_t124(vi2_cam, lanes, port);
1274         else {
1275                 if (lanes == 4)
1276                         cil_phy_reg_write(vi2_cam, chan, 0x0101);
1277                 else {
1278                         u32 val = cil_phy_reg_read(vi2_cam, chan);
1279                         if (port & 0x1)
1280                                 val = (val & ~(0x100)) | (0x100);
1281                         else
1282                                 val = (val & ~(0x1)) | (0x1);
1283                         cil_phy_reg_write(vi2_cam, chan, val);
1284                 }
1285         }
1286
1287         if (vi2_cam->tpg_mode) {
1288                 tpg_regs_write(vi2_cam, chan, TEGRA_CSI_PATTERN_GENERATOR_CTRL,
1289                                 ((vi2_cam->tpg_mode - 1) << 2) | 0x1);
1290                 tpg_regs_write(vi2_cam, chan, TEGRA_CSI_PG_PHASE, 0x0);
1291                 tpg_regs_write(vi2_cam, chan, TEGRA_CSI_PG_RED_FREQ, 0x100010);
1292                 tpg_regs_write(vi2_cam, chan, TEGRA_CSI_PG_RED_FREQ_RATE, 0x0);
1293                 tpg_regs_write(vi2_cam, chan, TEGRA_CSI_PG_GREEN_FREQ,
1294                                0x100010);
1295                 tpg_regs_write(vi2_cam, chan, TEGRA_CSI_PG_GREEN_FREQ_RATE,
1296                                0x0);
1297                 tpg_regs_write(vi2_cam, chan, TEGRA_CSI_PG_BLUE_FREQ, 0x100010);
1298                 tpg_regs_write(vi2_cam, chan, TEGRA_CSI_PG_BLUE_FREQ_RATE, 0x0);
1299                 if (IS_ENABLED(CONFIG_ARCH_TEGRA_12x_SOC) ||
1300                     IS_ENABLED(CONFIG_ARCH_TEGRA_13x_SOC))
1301                         TC_VI_REG_WT(vi2_cam, TEGRA_CSI_PHY_CIL_COMMAND,
1302                                      0x22020202);
1303                 else
1304                         cil_phy_reg_write(vi2_cam, chan, 0x0202);
1305
1306                 format = TEGRA_IMAGE_FORMAT_T_A8B8G8R8;
1307                 data_type = TEGRA_IMAGE_DT_RGB888;
1308                 image_size = width * 3;
1309                 bypass_pixel_transform = 0;
1310         } else if ((chan->code == V4L2_MBUS_FMT_UYVY8_2X8) ||
1311                    (chan->code == V4L2_MBUS_FMT_VYUY8_2X8) ||
1312                    (chan->code == V4L2_MBUS_FMT_YUYV8_2X8) ||
1313                    (chan->code == V4L2_MBUS_FMT_YVYU8_2X8)) {
1314                 switch (chan->code) {
1315                 case V4L2_MBUS_FMT_UYVY8_2X8:
1316                         format = TEGRA_IMAGE_FORMAT_T_U8_Y8__V8_Y8;
1317                         break;
1318                 case V4L2_MBUS_FMT_VYUY8_2X8:
1319                         format = TEGRA_IMAGE_FORMAT_T_V8_Y8__U8_Y8;
1320                         break;
1321                 case V4L2_MBUS_FMT_YUYV8_2X8:
1322                         format = TEGRA_IMAGE_FORMAT_T_Y8_U8__Y8_V8;
1323                         break;
1324                 case V4L2_MBUS_FMT_YVYU8_2X8:
1325                         format = TEGRA_IMAGE_FORMAT_T_Y8_V8__Y8_U8;
1326                         break;
1327                 }
1328                 data_type = TEGRA_IMAGE_DT_YUV422_8;
1329                 image_size = width * 2;
1330                 bypass_pixel_transform = 0;
1331         } else if ((chan->code == V4L2_MBUS_FMT_SBGGR8_1X8) ||
1332                    (chan->code == V4L2_MBUS_FMT_SGBRG8_1X8)) {
1333                 format = TEGRA_IMAGE_FORMAT_T_L8;
1334                 data_type = TEGRA_IMAGE_DT_RAW8;
1335                 image_size = width;
1336                 bypass_pixel_transform = 1;
1337         } else if ((chan->code == V4L2_MBUS_FMT_SBGGR10_1X10) ||
1338                    (chan->code == V4L2_MBUS_FMT_SRGGB10_1X10)) {
1339                 format = TEGRA_IMAGE_FORMAT_T_R16_I;
1340                 data_type = TEGRA_IMAGE_DT_RAW10;
1341                 image_size = width * 10 / 8;
1342                 bypass_pixel_transform = 1;
1343         } else if (chan->code == V4L2_MBUS_FMT_SRGGB12_1X12) {
1344                 format = TEGRA_IMAGE_FORMAT_T_R16_I;
1345                 data_type = TEGRA_IMAGE_DT_RAW12;
1346                 image_size = width * 12 / 8;
1347                 bypass_pixel_transform = 1;
1348         } else if (chan->code == V4L2_MBUS_FMT_RGBA8888_4X8_LE) {
1349                 format = TEGRA_IMAGE_FORMAT_T_A8B8G8R8;
1350                 data_type = TEGRA_IMAGE_DT_RGB888;
1351                 image_size = width * 3;
1352                 bypass_pixel_transform = 0;
1353         }
1354
1355         csi_regs_write(vi2_cam, chan, TEGRA_VI_CSI_IMAGE_DEF,
1356                         (bypass_pixel_transform << 24) | (format << 16) |
1357                         0x1);
1358         csi_regs_write(vi2_cam, chan, TEGRA_VI_CSI_IMAGE_DT, data_type);
1359         csi_regs_write(vi2_cam, chan, TEGRA_VI_CSI_IMAGE_SIZE_WC, image_size);
1360         csi_regs_write(vi2_cam, chan, TEGRA_VI_CSI_IMAGE_SIZE,
1361                         (height << 16) | width);
1362
1363         /* Start pixel parser in single shot mode at beginning */
1364         csi_pp_regs_write(vi2_cam, chan, TEGRA_CSI_PIXEL_STREAM_PP_COMMAND,
1365                           0xf005);
1366
1367         return 0;
1368 }
1369
1370 static int vi2_capture_buffer_setup(struct vi2_channel *chan,
1371                                     struct tegra_camera_buffer *buf)
1372 {
1373         struct vi2_camera *vi2_cam = chan->vi2_cam;
1374
1375         switch (chan->fourcc) {
1376         case V4L2_PIX_FMT_YUV420:
1377         case V4L2_PIX_FMT_YVU420:
1378                 /* FIXME: Setup YUV buffer */
1379
1380         case V4L2_PIX_FMT_UYVY:
1381         case V4L2_PIX_FMT_VYUY:
1382         case V4L2_PIX_FMT_YUYV:
1383         case V4L2_PIX_FMT_YVYU:
1384         case V4L2_PIX_FMT_SBGGR8:
1385         case V4L2_PIX_FMT_SGBRG8:
1386         case V4L2_PIX_FMT_SBGGR10:
1387         case V4L2_PIX_FMT_SRGGB10:
1388         case V4L2_PIX_FMT_SRGGB12:
1389         case V4L2_PIX_FMT_RGB32:
1390                 csi_regs_write(vi2_cam, chan,
1391                                TEGRA_VI_CSI_SURFACE0_OFFSET_MSB +
1392                                chan->surface * 8,
1393                                0x0);
1394                 csi_regs_write(vi2_cam, chan,
1395                                TEGRA_VI_CSI_SURFACE0_OFFSET_LSB +
1396                                chan->surface * 8,
1397                                buf->buffer_addr + chan->buffer_offset);
1398                 csi_regs_write(vi2_cam, chan,
1399                                TEGRA_VI_CSI_SURFACE0_STRIDE +
1400                                chan->surface * 4,
1401                                chan->bytes_per_line);
1402                 break;
1403         default:
1404                 dev_err(&vi2_cam->cam.pdev->dev, "Wrong host format %d\n",
1405                         chan->fourcc);
1406                 return -EINVAL;
1407         }
1408
1409         return 0;
1410 }
1411
1412 static void vi2_capture_error_status(struct vi2_channel *chan, int err)
1413 {
1414         struct vi2_camera *vi2_cam = chan->vi2_cam;
1415         u32 val;
1416
1417         dev_err(&vi2_cam->cam.pdev->dev,
1418                 "CSI %d syncpt timeout, syncpt = %d, err = %d\n",
1419                 chan->port, chan->syncpt_id, err);
1420
1421 #ifdef DEBUG
1422         val = TC_VI_REG_RD(vi2_cam, TEGRA_CSI_DEBUG_COUNTER_0);
1423         dev_err(&vi2_cam->cam.pdev->dev,
1424                 "TEGRA_CSI_DEBUG_COUNTER_0 0x%08x\n", val);
1425 #endif
1426         val = cil_regs_read(vi2_cam, chan, TEGRA_CSI_CIL_STATUS);
1427         dev_err(&vi2_cam->cam.pdev->dev,
1428                 "TEGRA_CSI_CSI_CIL_STATUS 0x%08x\n", val);
1429         val = cil_regs_read(vi2_cam, chan, TEGRA_CSI_CILX_STATUS);
1430         dev_err(&vi2_cam->cam.pdev->dev,
1431                 "TEGRA_CSI_CSI_CILX_STATUS 0x%08x\n", val);
1432         val = csi_pp_regs_read(vi2_cam, chan, TEGRA_CSI_PIXEL_PARSER_STATUS);
1433         dev_err(&vi2_cam->cam.pdev->dev,
1434                 "TEGRA_CSI_PIXEL_PARSER_STATUS 0x%08x\n", val);
1435         val = csi_regs_read(vi2_cam, chan, TEGRA_VI_CSI_ERROR_STATUS);
1436         dev_err(&vi2_cam->cam.pdev->dev,
1437                 "TEGRA_VI_CSI_ERROR_STATUS 0x%08x\n", val);
1438 }
1439
1440 static int vi2_channel_capture_frame(struct vi2_channel *chan,
1441                                      struct tegra_camera_buffer *buf)
1442 {
1443         struct vi2_camera *vi2_cam = chan->vi2_cam;
1444         struct tegra_camera *cam = &vi2_cam->cam;
1445         struct vi2_channel *channel;
1446         int err = 0, i;
1447
1448         /* Setup capture registers */
1449         for (i = 0; i < chan->gang_refcnt; i++) {
1450                 channel = chan->gang_channels[i];
1451                 vi2_channel_capture_setup(channel);
1452                 err = vi2_capture_buffer_setup(channel, buf);
1453                 if (err < 0)
1454                         return err;
1455
1456                 channel->syncpt_thresh = nvhost_syncpt_incr_max_ext(cam->pdev,
1457                         channel->syncpt_id, 1);
1458         }
1459
1460         /* Start capture */
1461         for (i = 0; i < chan->gang_refcnt; i++) {
1462                 channel = chan->gang_channels[i];
1463                 TC_VI_REG_WT(vi2_cam, TEGRA_VI_CFG_VI_INCR_SYNCPT,
1464                              vi2_syncpt_cond(VI_FRAME_START,
1465                              channel->port) | channel->syncpt_id);
1466
1467                 csi_regs_write(vi2_cam, channel, TEGRA_VI_CSI_SINGLE_SHOT, 0x1);
1468         }
1469
1470         for (i = 0; i < chan->gang_refcnt; i++) {
1471                 channel = chan->gang_channels[i];
1472                 err = nvhost_syncpt_wait_timeout_ext(cam->pdev,
1473                         channel->syncpt_id, channel->syncpt_thresh,
1474                         TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
1475                         NULL,
1476                         NULL);
1477         }
1478
1479         /* Move buffer to capture done queue */
1480         spin_lock(&chan->done_lock);
1481         list_add_tail(&buf->queue, &chan->done);
1482         spin_unlock(&chan->done_lock);
1483
1484         /* Wait up kthread for capture done */
1485         wake_up_interruptible(&chan->done_wait);
1486
1487         /* Mark SOF flag to Zero after we captured the FIRST frame */
1488         for (i = 0; i < chan->gang_refcnt; i++) {
1489                 channel = chan->gang_channels[i];
1490                 if (channel->sof)
1491                         channel->sof = 0;
1492         }
1493
1494         /* Capture syncpt timeout err, then dump error status */
1495         if (err)
1496                 vi2_capture_error_status(chan, err);
1497
1498         return err;
1499 }
1500
1501 static int vi2_channel_capture_done(struct vi2_channel *chan,
1502                                     struct tegra_camera_buffer *buf)
1503 {
1504         struct vi2_camera *vi2_cam = chan->vi2_cam;
1505         struct tegra_camera *cam = &vi2_cam->cam;
1506         struct vi2_channel *channel;
1507         struct vb2_buffer *vb = &buf->vb;
1508         int err = 0, i;
1509
1510         for (i = 0; i < chan->gang_refcnt; i++) {
1511                 channel = chan->gang_channels[i];
1512                 channel->syncpt_thresh = nvhost_syncpt_incr_max_ext(cam->pdev,
1513                         channel->syncpt_id, 1);
1514
1515                 /*
1516                  * Make sure recieve VI_MW_ACK_DONE of the last frame before
1517                  * stop and dequeue buffer, otherwise MC error will shows up
1518                  * for the last frame.
1519                  */
1520                 TC_VI_REG_WT(vi2_cam, TEGRA_VI_CFG_VI_INCR_SYNCPT,
1521                         vi2_syncpt_cond(VI_MW_ACK_DONE,  channel->port) |
1522                         channel->syncpt_id);
1523         }
1524
1525         for (i = 0; i < chan->gang_refcnt; i++) {
1526                 channel = chan->gang_channels[i];
1527                 /*
1528                  * Ignore error here and just stop pixel parser after waiting,
1529                  * even if it's timeout
1530                  */
1531                 err = nvhost_syncpt_wait_timeout_ext(cam->pdev,
1532                                 channel->syncpt_id, channel->syncpt_thresh,
1533                                 TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
1534                                 NULL,
1535                                 NULL);
1536                 if (err)
1537                         dev_err(&vi2_cam->cam.pdev->dev,
1538                                 "MW_ACK_DONE syncpoint time out!\n");
1539         }
1540
1541         /* Captured one frame */
1542         do_gettimeofday(&vb->v4l2_buf.timestamp);
1543         vb->v4l2_buf.sequence = chan->sequence++;
1544         vb->v4l2_buf.field = V4L2_FIELD_NONE;
1545         vb2_buffer_done(vb, err < 0 ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
1546
1547         return err;
1548 }
1549
1550 struct tegra_camera_ops vi2_ops = {
1551         .add_device             = vi2_add_device,
1552         .remove_device          = vi2_remove_device,
1553         .port_is_valid          = vi2_port_is_valid,
1554         .bytes_per_line         = vi2_bytes_per_line,
1555         .ignore_subdev_fmt      = vi2_ignore_subdev_fmt,
1556         .get_formats            = vi2_get_formats,
1557         .try_mbus_fmt           = vi2_try_mbus_fmt,
1558         .s_mbus_fmt             = vi2_s_mbus_fmt,
1559         .videobuf_queue         = vi2_videobuf_queue,
1560         .start_streaming        = vi2_start_streaming,
1561         .stop_streaming         = vi2_stop_streaming,
1562 };
1563
1564 static struct of_device_id vi2_of_match[] = {
1565         { .compatible = "nvidia,tegra124-vi",
1566                 .data = (struct nvhost_device_data *)&t124_vi_info },
1567         { .compatible = "nvidia,tegra210-vi",
1568                 .data = (struct nvhost_device_data *)&t21_vi_info },
1569         { },
1570 };
1571
1572 static int tegra_camera_slcg_handler(struct notifier_block *nb,
1573                 unsigned long action, void *data)
1574 {
1575         struct clk *clk;
1576         int ret = 0;
1577
1578         struct nvhost_device_data *pdata =
1579                 container_of(nb, struct nvhost_device_data,
1580                         toggle_slcg_notifier);
1581
1582         /* Skip this operation for TPG mode */
1583         if (tpg_mode)
1584                 return NOTIFY_OK;
1585
1586         clk = clk_get(&pdata->pdev->dev, "pll_d");
1587         if (IS_ERR(clk))
1588                 return -EINVAL;
1589
1590         /* Make CSI sourced from PLL_D */
1591         ret = tegra_clk_cfg_ex(clk, TEGRA_CLK_PLLD_CSI_OUT_ENB, 1);
1592         if (ret) {
1593                 dev_err(&pdata->pdev->dev,
1594                 "%s: failed to select CSI source pll_d: %d\n",
1595                 __func__, ret);
1596                 return ret;
1597         }
1598
1599         /* Enable PLL_D */
1600         ret = clk_prepare_enable(clk);
1601         if (ret) {
1602                 dev_err(&pdata->pdev->dev, "Can't enable pll_d: %d\n", ret);
1603                 return ret;
1604         }
1605
1606         udelay(1);
1607
1608         /* Disable PLL_D */
1609         clk_disable_unprepare(clk);
1610
1611         /* Restore CSI source */
1612         ret = tegra_clk_cfg_ex(clk, TEGRA_CLK_MIPI_CSI_OUT_ENB, 1);
1613         if (ret) {
1614                 dev_err(&pdata->pdev->dev,
1615                 "%s: failed to restore csi source: %d\n",
1616                 __func__, ret);
1617                 return ret;
1618         }
1619
1620         clk_put(clk);
1621
1622         return NOTIFY_OK;
1623 }
1624
1625 static int vi2_probe(struct platform_device *pdev)
1626 {
1627         struct vi2_camera *vi2_cam;
1628         struct nvhost_device_data *ndata = NULL;
1629         int err = 0;
1630
1631         if (pdev->dev.of_node) {
1632                 const struct of_device_id *match;
1633
1634                 match = of_match_device(vi2_of_match, &pdev->dev);
1635                 if (match) {
1636                         ndata = (struct nvhost_device_data *)match->data;
1637                         pdev->dev.platform_data = ndata;
1638                 }
1639
1640                 /*
1641                  * Device Tree will initialize this ID as -1
1642                  * Set it to the right value for future usage
1643                  */
1644                 pdev->id = pdev->dev.id;
1645         } else
1646                 ndata = pdev->dev.platform_data;
1647
1648         if (!ndata) {
1649                 dev_err(&pdev->dev, "No nvhost device data!\n");
1650                 return -EINVAL;
1651         }
1652
1653         /* vi.1 has to wait vi.0 initialized, so defer probing */
1654         if (pdev->id && ndata->master) {
1655                 struct nvhost_device_data *master_ndata =
1656                         ndata->master->dev.platform_data;
1657                 if (master_ndata == platform_get_drvdata(ndata->master))
1658                         return -EPROBE_DEFER;
1659         }
1660
1661         vi2_cam = devm_kzalloc(&pdev->dev, sizeof(struct vi2_camera),
1662                                GFP_KERNEL);
1663         if (!vi2_cam) {
1664                 dev_err(&pdev->dev, "couldn't allocate cam\n");
1665                 return -ENOMEM;
1666         }
1667
1668         vi2_cam->ndata = ndata;
1669         ndata->pdev = pdev;
1670         vi2_cam->cam.pdev = pdev;
1671
1672         /* Init Regulator */
1673         vi2_cam->regulator_name = "avdd_dsi_csi";
1674         vi2_cam->reg = devm_regulator_get(&pdev->dev, vi2_cam->regulator_name);
1675         if (IS_ERR_OR_NULL(vi2_cam->reg)) {
1676                 dev_err(&pdev->dev, "%s: couldn't get regulator %s, err %ld\n",
1677                         __func__, vi2_cam->regulator_name,
1678                         PTR_ERR(vi2_cam->reg));
1679                 return PTR_ERR(vi2_cam->reg);
1680         }
1681
1682         /* Initialize our nvhost client */
1683         mutex_init(&ndata->lock);
1684         platform_set_drvdata(pdev, ndata);
1685         err = nvhost_client_device_get_resources(pdev);
1686         if (err) {
1687                 dev_err(&pdev->dev, "%s: nvhost get resources failed %d\n",
1688                                 __func__, err);
1689                 return err;
1690         }
1691
1692         if (!ndata->aperture[0]) {
1693                 if (ndata->master) {
1694                         struct nvhost_device_data *master_ndata =
1695                                 ndata->master->dev.platform_data;
1696                         ndata->aperture[0] = master_ndata->aperture[0];
1697                 } else {
1698                         dev_err(&pdev->dev, "%s: failed to map register base\n",
1699                                 __func__);
1700                         return -ENXIO;
1701                 }
1702         }
1703
1704         /* Match the nvhost_module_init VENC powergating */
1705         tegra_unpowergate_partition(TEGRA_POWERGATE_VENC);
1706         nvhost_module_init(pdev);
1707
1708         err = nvhost_client_device_init(pdev);
1709         if (err) {
1710                 dev_err(&pdev->dev, "%s: nvhost init failed %d\n",
1711                         __func__, err);
1712                 return err;
1713         }
1714
1715         /* Get the VI register base */
1716         vi2_cam->reg_base = ndata->aperture[0];
1717
1718         /* Match the nvhost_module_init VENC powergating */
1719         if (ndata->slcg_notifier_enable &&
1720                         (ndata->powergate_id != -1)) {
1721                 ndata->toggle_slcg_notifier.notifier_call =
1722                 &tegra_camera_slcg_handler;
1723
1724                 slcg_register_notifier(ndata->powergate_id,
1725                         &ndata->toggle_slcg_notifier);
1726         }
1727
1728         platform_set_drvdata(pdev, vi2_cam);
1729
1730         vi2_cam->tpg_mode = tpg_mode;
1731
1732         /* Init VI2/CSI2 ops */
1733         strlcpy(vi2_cam->cam.card, VI2_CAM_CARD_NAME,
1734                 sizeof(vi2_cam->cam.card));
1735         vi2_cam->cam.version = VI2_CAM_VERSION;
1736         vi2_cam->cam.ops = &vi2_ops;
1737
1738         err = tegra_camera_init(pdev, &vi2_cam->cam);
1739         if (err) {
1740                 platform_set_drvdata(pdev, vi2_cam->ndata);
1741                 nvhost_client_device_release(pdev);
1742                 vi2_cam->ndata->aperture[0] = NULL;
1743                 return err;
1744         }
1745
1746         dev_notice(&pdev->dev, "Tegra camera driver loaded.\n");
1747
1748         return 0;
1749 }
1750
1751 static int vi2_remove(struct platform_device *pdev)
1752 {
1753         struct soc_camera_host *ici = to_soc_camera_host(&pdev->dev);
1754         struct tegra_camera *cam = container_of(ici,
1755                                         struct tegra_camera, ici);
1756         struct vi2_camera *vi2_cam = (struct vi2_camera *)cam;
1757         struct nvhost_device_data *ndata = vi2_cam->ndata;
1758
1759         tegra_camera_deinit(pdev, &vi2_cam->cam);
1760
1761         platform_set_drvdata(pdev, vi2_cam->ndata);
1762
1763         if (ndata->slcg_notifier_enable &&
1764             (ndata->powergate_id != -1))
1765                 slcg_unregister_notifier(ndata->powergate_id,
1766                                          &ndata->toggle_slcg_notifier);
1767
1768         nvhost_client_device_release(pdev);
1769         vi2_cam->ndata->aperture[0] = NULL;
1770
1771         dev_notice(&pdev->dev, "Tegra camera host driver unloaded\n");
1772
1773         return 0;
1774 }
1775
1776 static struct platform_driver vi2_driver = {
1777         .driver = {
1778                 .name   = VI2_CAM_DRV_NAME,
1779                 .of_match_table = of_match_ptr(vi2_of_match),
1780         },
1781         .probe          = vi2_probe,
1782         .remove         = vi2_remove,
1783 };
1784
1785 module_platform_driver(vi2_driver);
1786
1787 MODULE_DESCRIPTION("TEGRA SoC Camera Host driver");
1788 MODULE_AUTHOR("Bryan Wu <pengw@nvidia.com>");
1789 MODULE_LICENSE("GPL v2");
1790 MODULE_ALIAS("nvhost:" VI2_CAM_DRV_NAME);