73 files changed, 41536 insertions, 0 deletions

diff --git a/drivers/i2c/busses/i2c-tegra-vi.c b/drivers/i2c/busses/i2c-tegra-vi.c
new file mode 100644
index 000000000..4c2645504
--- /dev/null
+++ b/drivers/i2c/busses/i2c-tegra-vi.c
@@ -0,0 +1,2238 @@
+/*
+ * drivers/i2c/busses/i2c-tegra-vi.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Colin Cross <ccross@android.com>
+ *
+ * Copyright (C) 2015 Google, Inc.
+ * Author: Tomasz Figa <tfiga@chromium.org>
+ *
+ * Copyright (C) 2010-2016 NVIDIA Corporation. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/of_device.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/reset.h>
+#include <linux/iopoll.h>
+#include <linux/of_gpio.h>
+#include <linux/i2c-algo-bit.h>
+#include <linux/i2c-gpio.h>
+#include <linux/tegra_prod.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/pm_runtime.h>
+#include <soc/tegra/tegra_powergate.h>
+#include <asm/unaligned.h>
+
+#define TEGRA_I2C_TIMEOUT (msecs_to_jiffies(1000))
+#define BYTES_PER_FIFO_WORD 4
+
+#define VI_I2C_REG_SHIFT                        2
+#define VI_I2C_REG_OFFSET                       0xc00
+
+#define I2C_CNFG                                0x000
+#define I2C_CNFG_DEBOUNCE_CNT_SHIFT             12
+#define I2C_CNFG_PACKET_MODE_EN                 (1<<10)
+#define I2C_CNFG_NEW_MASTER_FSM                 (1<<11)
+#define I2C_CNFG_MULTI_MASTER_MODE              (1<<17)
+#define I2C_STATUS                              0x01C
+#define I2C_SL_CNFG                             0x020
+#define I2C_SL_CNFG_NACK                        (1<<1)
+#define I2C_SL_CNFG_NEWSL                       (1<<2)
+#define I2C_SL_ADDR1                            0x02c
+#define I2C_SL_ADDR2                            0x030
+#define I2C_TLOW_SEXT                           0x034
+#define I2C_TX_FIFO                             0x050
+#define I2C_RX_FIFO                             0x054
+#define I2C_PACKET_TRANSFER_STATUS              0x058
+#define I2C_FIFO_CONTROL                        0x05c
+#define I2C_FIFO_CONTROL_TX_FLUSH               (1<<1)
+#define I2C_FIFO_CONTROL_RX_FLUSH               (1<<0)
+#define I2C_FIFO_CONTROL_RX_TRIG_1              (0<<2)
+#define I2C_FIFO_CONTROL_RX_TRIG_4              (3<<2)
+#define I2C_FIFO_CONTROL_RX_TRIG_8              (7<<2)
+#define I2C_FIFO_CONTROL_TX_TRIG_1              (0<<5)
+#define I2C_FIFO_CONTROL_TX_TRIG_4              (3<<5)
+#define I2C_FIFO_CONTROL_TX_TRIG_8              (7<<5)
+#define I2C_FIFO_CONTROL_TX_TRIG_SHIFT          5
+#define I2C_FIFO_CONTROL_RX_TRIG_SHIFT          2
+#define I2C_FIFO_STATUS                         0x060
+#define I2C_FIFO_STATUS_TX_MASK                 0xF0
+#define I2C_FIFO_STATUS_TX_SHIFT                4
+#define I2C_FIFO_STATUS_RX_MASK                 0x0F
+#define I2C_FIFO_STATUS_RX_SHIFT                0
+#define I2C_INT_MASK                            0x064
+#define I2C_INT_STATUS                          0x068
+#define I2C_INT_BUS_CLEAR_DONE                  (1<<11)
+#define I2C_INT_PACKET_XFER_COMPLETE            (1<<7)
+#define I2C_INT_ALL_PACKETS_XFER_COMPLETE       (1<<6)
+#define I2C_INT_TX_FIFO_OVERFLOW                (1<<5)
+#define I2C_INT_RX_FIFO_UNDERFLOW               (1<<4)
+#define I2C_INT_NO_ACK                          (1<<3)
+#define I2C_INT_ARBITRATION_LOST                (1<<2)
+#define I2C_INT_TX_FIFO_DATA_REQ                (1<<1)
+#define I2C_INT_RX_FIFO_DATA_REQ                (1<<0)
+#define I2C_CLK_DIVISOR                         0x06c
+#define I2C_CLK_DIVISOR_STD_FAST_MODE_SHIFT     16
+#define I2C_CLK_MULTIPLIER_STD_FAST_MODE        8
+#define I2C_CLK_DIVISOR_HS_MODE_MASK            0xFFFF
+
+#define I2C_BUS_CLEAR_CNFG                      0x084
+#define I2C_TLOW                                2
+#define I2C_TLOW_SHIFT                          0
+#define I2C_THIGH                               4
+#define I2C_THIGH_SHIFT                         8
+#define I2C_INTERFACE_TIMING_1                  0x098
+#define I2C_HS_INTERFACE_TIMING_0               0x09C
+#define I2C_HS_INTERFACE_TIMING_1               0x0A0
+
+#define DVC_CTRL_REG1                           0x000
+#define DVC_CTRL_REG1_INTR_EN                   (1<<10)
+#define DVC_CTRL_REG2                           0x004
+#define DVC_CTRL_REG3                           0x008
+#define DVC_CTRL_REG3_SW_PROG                   (1<<26)
+#define DVC_CTRL_REG3_I2C_DONE_INTR_EN          (1<<30)
+#define DVC_STATUS                              0x00c
+#define DVC_STATUS_I2C_DONE_INTR                (1<<30)
+
+#define I2C_ERR_NONE                            0x00
+#define I2C_ERR_NO_ACK                          0x01
+#define I2C_ERR_ARBITRATION_LOST                0x02
+#define I2C_ERR_UNKNOWN_INTERRUPT               0x04
+
+#define PACKET_HEADER0_HEADER_SIZE_SHIFT        28
+#define PACKET_HEADER0_PACKET_ID_SHIFT          16
+#define PACKET_HEADER0_CONT_ID_SHIFT            12
+#define PACKET_HEADER0_CONT_ID_MASK             0xF
+#define PACKET_HEADER0_PROTOCOL_I2C             (1<<4)
+
+#define I2C_HEADER_HIGHSPEED_MODE               (1<<22)
+#define I2C_HEADER_CONT_ON_NAK                  (1<<21)
+#define I2C_HEADER_SEND_START_BYTE              (1<<20)
+#define I2C_HEADER_READ                         (1<<19)
+#define I2C_HEADER_10BIT_ADDR                   (1<<18)
+#define I2C_HEADER_IE_ENABLE                    (1<<17)
+#define I2C_HEADER_REPEAT_START                 (1<<16)
+#define I2C_HEADER_CONTINUE_XFER                (1<<15)
+#define I2C_HEADER_MASTER_ADDR_SHIFT            12
+#define I2C_HEADER_SLAVE_ADDR_SHIFT             1
+
+#define I2C_BUS_CLEAR_CNFG                      0x084
+#define I2C_BC_SCLK_THRESHOLD                   (9<<16)
+#define I2C_BC_STOP_COND                        (1<<2)
+#define I2C_BC_TERMINATE                        (1<<1)
+#define I2C_BC_ENABLE                           (1<<0)
+
+#define I2C_BUS_CLEAR_STATUS                    0x088
+#define I2C_BC_STATUS                           (1<<0)
+
+#define I2C_CONFIG_LOAD                         0x08C
+#define I2C_MSTR_CONFIG_LOAD                    (1 << 0)
+#define I2C_SLV_CONFIG_LOAD                     (1 << 1)
+#define I2C_TIMEOUT_CONFIG_LOAD                 (1 << 2)
+
+#define I2C_CLKEN_OVERRIDE                      0x090
+#define I2C_MST_CORE_CLKEN_OVR                  (1 << 0)
+
+#define I2C_INTERFACE_TIMING_0                  0x94
+#define I2C_TLOW_MASK                           0x3F
+#define I2C_THIGH_SHIFT                         8
+#define I2C_THIGH_MASK                          (0x3F << I2C_THIGH_SHIFT)
+
+#define I2C_MASTER_RESET_CONTROL                0x0A8
+#define I2C_CMD_ADDR0                           0x004
+#define I2C_CMD_ADDR1                           0x008
+#define I2C_CMD_DATA1                           0x00C
+#define I2C_CMD_DATA2                           0x010
+#define I2C_DEBUG_CONTROL                       0x0A4
+#define I2C_TLOW_SEXT                           0x034
+#define I2C_INTERRUPT_SET_REGISTER              0x074
+
+#define I2C_MAX_TRANSFER_LEN                    4096
+#define I2C_CONFIG_LOAD_TIMEOUT                 1000000
+
+/* Define speed modes */
+#define I2C_STANDARD_MODE                       100000
+#define I2C_FAST_MODE                           400000
+#define I2C_FAST_MODE_PLUS                      1000000
+#define I2C_HS_MODE                             3500000
+
+/* Max DMA buffer size = max packet transfer length + size of pkt header */
+#define I2C_DMA_MAX_BUF_LEN                     (I2C_MAX_TRANSFER_LEN + 12)
+#define DATA_DMA_DIR_TX                         (1 << 0)
+#define DATA_DMA_DIR_RX                         (1 << 1)
+
+/* Upto I2C_PIO_MODE_MAX_LEN bytes, controller will use PIO mode,
+ * above this, controller will use DMA to fill FIFO.
+ * Here MAX PIO len is 20 bytes excluding packet header
+ */
+#define I2C_PIO_MODE_MAX_LEN                    (20)
+
+/* Packet header size in words */
+#define I2C_PACKET_HEADER_SIZE                  (3)
+
+/*
+ * msg_end_type: The bus control which need to be send at end of transfer.
+ * @MSG_END_STOP: Send stop pulse at end of transfer.
+ * @MSG_END_REPEAT_START: Send repeat start at end of transfer.
+ * @MSG_END_CONTINUE: The following on message is coming and so do not send
+ *              stop or repeat start.
+ */
+enum msg_end_type {
+        MSG_END_STOP,
+        MSG_END_REPEAT_START,
+        MSG_END_CONTINUE,
+};
+
+/**
+ * struct tegra_i2c_hw_feature : Different HW support on Tegra
+ * @has_continue_xfer_support: Continue transfer supports.
+ * @has_per_pkt_xfer_complete_irq: Has enable/disable capability for transfer
+ *              complete interrupt per packet basis.
+ * @has_single_clk_source: The i2c controller has single clock source. Tegra30
+ *              and earlier Socs has two clock sources i.e. div-clk and
+ *              fast-clk.
+ * @has_config_load_reg: Has the config load register to load the new
+ *              configuration.
+ * @has_regulator: Controller requires extrnal regulator to be powered on
+ *              during transfers.
+ * @has_powergate: Controller is located inside a powergate partition.
+ * @is_vi: Identifies the VI i2c controller, has a different register layout,
+ *              and needs more clocks.
+ * @powergate: Powergate partition ID, if applicable.
+ * @clk_divisor_hs_mode: Clock divisor in HS mode.
+ * @clk_divisor_std_fast_mode: Clock divisor in standard/fast mode. It is
+ *              applicable if there is no fast clock source i.e. single clock
+ *              source.
+ */
+
+struct tegra_i2c_hw_feature {
+        bool has_continue_xfer_support;
+        bool has_per_pkt_xfer_complete_irq;
+        bool has_single_clk_source;
+        bool has_config_load_reg;
+        int clk_divisor_hs_mode;
+        int clk_multiplier_hs_mode;
+        int clk_divisor_std_fast_mode;
+        u16 clk_divisor_fast_plus_mode;
+        bool has_multi_master_mode;
+        bool has_slcg_override_reg;
+        bool has_sw_reset_reg;
+        bool has_hw_arb_support;
+        bool has_reg_write_buffering;
+        bool has_slcg_support;
+        bool has_regulator;
+        bool has_powergate;
+        bool is_vi;
+        int powergate_id;
+};
+
+/**
+ * struct tegra_i2c_dev - per device i2c context
+ * @dev: device reference for power management
+ * @hw: Tegra i2c hw feature.
+ * @adapter: core i2c layer adapter information
+ * @div_clk: clock reference for div clock of i2c controller.
+ * @fast_clk: clock reference for fast clock of i2c controller.
+ * @base: ioremapped registers cookie
+ * @cont_id: i2c controller id, used for for packet header
+ * @irq: irq number of transfer complete interrupt
+ * @is_dvc: identifies the DVC i2c controller, has a different register layout
+ * @msg_complete: transfer completion notifier
+ * @msg_err: error code for completed message
+ * @msg_buf: pointer to current message data
+ * @msg_buf_remaining: size of unsent data in the message buffer
+ * @msg_read: identifies read transfers
+ * @bus_clk_rate: current i2c bus clock rate
+ * @is_suspended: prevents i2c controller accesses after suspend is called
+ */
+struct tegra_i2c_dev {
+        struct device *dev;
+        const struct tegra_i2c_hw_feature *hw;
+        struct i2c_adapter adapter;
+        struct clk *div_clk;
+        struct clk *fast_clk;
+        struct reset_control *rst;
+        void __iomem *base;
+        phys_addr_t phys_addr;
+        int cont_id;
+        int irq;
+        bool irq_disabled;
+        int is_dvc;
+        struct completion msg_complete;
+        int msg_add;
+        int msg_err;
+        u8 *msg_buf;
+        size_t msg_buf_remaining;
+        int msg_read;
+        u32 bus_clk_rate;
+        u16 clk_divisor_non_hs_mode;
+        bool is_suspended;
+        bool is_multimaster_mode;
+        bool is_periph_reset_done;
+        spinlock_t xfer_lock;
+        struct i2c_bus_recovery_info bri;
+        int scl_gpio;
+        int sda_gpio;
+        struct i2c_algo_bit_data bit_data;
+        const struct i2c_algorithm *bit_algo;
+        bool bit_banging_xfer_after_shutdown;
+        bool is_shutdown;
+        u32 low_clock_count;
+        u32 high_clock_count;
+        struct tegra_prod *prod_list;
+        int clk_divisor_hs_mode;
+        u16 hs_master_code;
+        struct dma_async_tx_descriptor *rx_dma_desc;
+        struct dma_chan *rx_dma_chan;
+        u32 *rx_dma_buf;
+        dma_addr_t rx_dma_phys;
+        struct dma_async_tx_descriptor *tx_dma_desc;
+        struct dma_chan *tx_dma_chan;
+        u32 *tx_dma_buf;
+        dma_addr_t tx_dma_phys;
+        unsigned dma_buf_size;
+        bool is_curr_dma_xfer;
+        struct completion rx_dma_complete;
+        struct completion tx_dma_complete;
+        int curr_direction;
+        int rx_dma_len;
+        dma_cookie_t rx_cookie;
+        bool disable_dma_mode;
+        bool is_clkon_always;
+        struct clk *slow_clk;
+        struct clk *host1x_clk;
+        struct regulator *reg;
+};
+
+static void dvc_writel(struct tegra_i2c_dev *i2c_dev,
+        u32 val, unsigned long reg)
+{
+        writel(val, i2c_dev->base + reg);
+}
+
+static u32 dvc_readl(struct tegra_i2c_dev *i2c_dev, unsigned long reg)
+{
+        return readl(i2c_dev->base + reg);
+}
+
+/*
+ * i2c_writel and i2c_readl will offset the register if necessary to talk
+ * to the I2C block inside the DVC block
+ */
+static unsigned long tegra_i2c_reg_addr(struct tegra_i2c_dev *i2c_dev,
+        unsigned long reg)
+{
+        if (i2c_dev->is_dvc)
+                reg += (reg >= I2C_TX_FIFO) ? 0x10 : 0x40;
+        else if (i2c_dev->hw->is_vi)
+                reg = VI_I2C_REG_OFFSET + (reg << VI_I2C_REG_SHIFT);
+        return reg;
+}
+
+static void i2c_writel(struct tegra_i2c_dev *i2c_dev, u32 val,
+        unsigned long reg)
+{
+        writel(val, i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg));
+
+        /* Read back register to make sure that register writes completed */
+        if (i2c_dev->hw->has_reg_write_buffering) {
+                if (reg != I2C_TX_FIFO)
+                        readl(i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg));
+        }
+}
+
+static u32 i2c_readl(struct tegra_i2c_dev *i2c_dev, unsigned long reg)
+{
+        return readl(i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg));
+}
+
+static void i2c_writesl(struct tegra_i2c_dev *i2c_dev, void *data,
+        unsigned long reg, int len)
+{
+        writesl(i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg), data, len);
+}
+
+static void i2c_readsl(struct tegra_i2c_dev *i2c_dev, void *data,
+        unsigned long reg, int len)
+{
+        readsl(i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg), data, len);
+}
+
+static inline void tegra_i2c_gpio_setscl(void *data, int state)
+{
+        struct tegra_i2c_dev *i2c_dev = data;
+
+        gpio_set_value(i2c_dev->scl_gpio, state);
+}
+
+static inline int tegra_i2c_gpio_getscl(void *data)
+{
+        struct tegra_i2c_dev *i2c_dev = data;
+
+        return gpio_get_value(i2c_dev->scl_gpio);
+}
+
+static inline void tegra_i2c_gpio_setsda(void *data, int state)
+{
+        struct tegra_i2c_dev *i2c_dev = data;
+
+        gpio_set_value(i2c_dev->sda_gpio, state);
+}
+
+static inline int tegra_i2c_gpio_getsda(void *data)
+{
+        struct tegra_i2c_dev *i2c_dev = data;
+
+        return gpio_get_value(i2c_dev->sda_gpio);
+}
+
+static int tegra_i2c_gpio_request(struct tegra_i2c_dev *i2c_dev)
+{
+        int ret;
+
+        ret = gpio_request_one(i2c_dev->scl_gpio,
+                                GPIOF_OUT_INIT_HIGH | GPIOF_OPEN_DRAIN,
+                                "i2c-gpio-scl");
+        if (ret < 0) {
+                dev_err(i2c_dev->dev, "GPIO request for gpio %d failed %d\n",
+                                i2c_dev->scl_gpio, ret);
+                return ret;
+        }
+
+        ret = gpio_request_one(i2c_dev->sda_gpio,
+                                GPIOF_OUT_INIT_HIGH | GPIOF_OPEN_DRAIN,
+                                "i2c-gpio-sda");
+        if (ret < 0) {
+                dev_err(i2c_dev->dev, "GPIO request for gpio %d failed %d\n",
+                                i2c_dev->sda_gpio, ret);
+                gpio_free(i2c_dev->scl_gpio);
+                return ret;
+        }
+        return ret;
+}
+
+static void tegra_i2c_gpio_free(struct tegra_i2c_dev *i2c_dev)
+{
+        gpio_free(i2c_dev->scl_gpio);
+        gpio_free(i2c_dev->sda_gpio);
+}
+
+static int tegra_i2c_gpio_xfer(struct i2c_adapter *adap,
+        struct i2c_msg msgs[], int num)
+{
+        struct tegra_i2c_dev *i2c_dev = i2c_get_adapdata(adap);
+        int ret;
+
+        ret = tegra_i2c_gpio_request(i2c_dev);
+        if (ret < 0)
+                return ret;
+
+        ret = i2c_dev->bit_algo->master_xfer(adap, msgs, num);
+        if (ret < 0)
+                dev_err(i2c_dev->dev, "i2c-bit-algo xfer failed %d\n", ret);
+
+        tegra_i2c_gpio_free(i2c_dev);
+        return ret;
+}
+
+static int tegra_i2c_gpio_init(struct tegra_i2c_dev *i2c_dev)
+{
+        struct i2c_algo_bit_data *bit_data = &i2c_dev->bit_data;
+
+        bit_data->setsda = tegra_i2c_gpio_setsda;
+        bit_data->getsda = tegra_i2c_gpio_getsda;
+        bit_data->setscl = tegra_i2c_gpio_setscl;
+        bit_data->getscl = tegra_i2c_gpio_getscl;
+        bit_data->data = i2c_dev;
+        bit_data->udelay = 5; /* 100KHz */
+        bit_data->timeout = HZ; /* 10 ms*/
+        i2c_dev->bit_algo = &i2c_bit_algo;
+        i2c_dev->adapter.algo_data = bit_data;
+        return 0;
+}
+
+static void tegra_i2c_rx_dma_complete(void *args)
+{
+        struct tegra_i2c_dev *i2c_dev = args;
+        struct dma_tx_state state;
+
+        dma_sync_single_for_cpu(i2c_dev->dev, i2c_dev->rx_dma_phys,
+                        i2c_dev->dma_buf_size, DMA_FROM_DEVICE);
+        dmaengine_tx_status(i2c_dev->rx_dma_chan, i2c_dev->rx_cookie, &state);
+        memcpy(i2c_dev->msg_buf, i2c_dev->rx_dma_buf, i2c_dev->rx_dma_len);
+        dma_sync_single_for_device(i2c_dev->dev, i2c_dev->rx_dma_phys,
+                        i2c_dev->dma_buf_size, DMA_FROM_DEVICE);
+        complete(&i2c_dev->rx_dma_complete);
+}
+
+static void tegra_i2c_tx_dma_complete(void *args)
+{
+        struct tegra_i2c_dev *i2c_dev = args;
+
+        complete(&i2c_dev->tx_dma_complete);
+}
+
+static int tegra_i2c_start_tx_dma(struct tegra_i2c_dev *i2c_dev, int len)
+{
+        reinit_completion(&i2c_dev->tx_dma_complete);
+        dev_dbg(i2c_dev->dev, "Starting tx dma for len:%d\n", len);
+        i2c_dev->tx_dma_desc = dmaengine_prep_slave_single(i2c_dev->tx_dma_chan,
+                                i2c_dev->tx_dma_phys, len, DMA_MEM_TO_DEV,
+                                DMA_PREP_INTERRUPT |  DMA_CTRL_ACK);
+        if (!i2c_dev->tx_dma_desc) {
+                dev_err(i2c_dev->dev, "Not able to get desc for Tx\n");
+                return -EIO;
+        }
+
+        i2c_dev->tx_dma_desc->callback = tegra_i2c_tx_dma_complete;
+        i2c_dev->tx_dma_desc->callback_param = i2c_dev;
+
+        dmaengine_submit(i2c_dev->tx_dma_desc);
+        dma_async_issue_pending(i2c_dev->tx_dma_chan);
+        return 0;
+}
+
+static int tegra_i2c_start_rx_dma(struct tegra_i2c_dev *i2c_dev, int len)
+{
+        reinit_completion(&i2c_dev->rx_dma_complete);
+        i2c_dev->rx_dma_len = len;
+
+        dev_dbg(i2c_dev->dev, "Starting rx dma for len:%d\n", len);
+        i2c_dev->rx_dma_desc = dmaengine_prep_slave_single(i2c_dev->rx_dma_chan,
+                                i2c_dev->rx_dma_phys, len, DMA_DEV_TO_MEM,
+                                DMA_PREP_INTERRUPT |  DMA_CTRL_ACK);
+        if (!i2c_dev->rx_dma_desc) {
+                dev_err(i2c_dev->dev, "Not able to get desc for Rx\n");
+                return -EIO;
+        }
+
+        i2c_dev->rx_dma_desc->callback = tegra_i2c_rx_dma_complete;
+        i2c_dev->rx_dma_desc->callback_param = i2c_dev;
+
+        dmaengine_submit(i2c_dev->rx_dma_desc);
+        dma_async_issue_pending(i2c_dev->rx_dma_chan);
+        return 0;
+}
+
+
+static int tegra_i2c_init_dma_param(struct tegra_i2c_dev *i2c_dev,
+                                                        bool dma_to_memory)
+{
+        struct dma_chan *dma_chan;
+        u32 *dma_buf;
+        dma_addr_t dma_phys;
+        int ret;
+        struct dma_slave_config dma_sconfig;
+
+        dma_chan = dma_request_slave_channel_reason(i2c_dev->dev,
+                                                dma_to_memory ? "rx" : "tx");
+        if (IS_ERR(dma_chan)) {
+                ret = PTR_ERR(dma_chan);
+                if (ret != -EPROBE_DEFER)
+                        dev_err(i2c_dev->dev,
+                                "Dma channel is not available: %d\n", ret);
+                return ret;
+        }
+
+        dma_buf = dma_alloc_coherent(i2c_dev->dev, i2c_dev->dma_buf_size,
+                                                        &dma_phys, GFP_KERNEL);
+        if (!dma_buf) {
+                dev_err(i2c_dev->dev, "Not able to allocate the dma buffer\n");
+                dma_release_channel(dma_chan);
+                return -ENOMEM;
+        }
+
+        if (dma_to_memory) {
+                dma_sconfig.src_addr = i2c_dev->phys_addr + I2C_RX_FIFO;
+                dma_sconfig.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+                dma_sconfig.src_maxburst = 0;
+        } else {
+                dma_sconfig.dst_addr = i2c_dev->phys_addr + I2C_TX_FIFO;
+                dma_sconfig.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+                dma_sconfig.dst_maxburst = 0;
+        }
+
+        ret = dmaengine_slave_config(dma_chan, &dma_sconfig);
+        if (ret)
+                goto scrub;
+        if (dma_to_memory) {
+                i2c_dev->rx_dma_chan = dma_chan;
+                i2c_dev->rx_dma_buf = dma_buf;
+                i2c_dev->rx_dma_phys = dma_phys;
+        } else {
+                i2c_dev->tx_dma_chan = dma_chan;
+                i2c_dev->tx_dma_buf = dma_buf;
+                i2c_dev->tx_dma_phys = dma_phys;
+        }
+        return 0;
+
+scrub:
+        dma_free_coherent(i2c_dev->dev, i2c_dev->dma_buf_size,
+                                                        dma_buf, dma_phys);
+        dma_release_channel(dma_chan);
+        return ret;
+}
+
+static void tegra_i2c_deinit_dma_param(struct tegra_i2c_dev *i2c_dev,
+        bool dma_to_memory)
+{
+        u32 *dma_buf;
+        dma_addr_t dma_phys;
+        struct dma_chan *dma_chan;
+
+        if (dma_to_memory) {
+                dma_buf = i2c_dev->rx_dma_buf;
+                dma_chan = i2c_dev->rx_dma_chan;
+                dma_phys = i2c_dev->rx_dma_phys;
+                i2c_dev->rx_dma_chan = NULL;
+                i2c_dev->rx_dma_buf = NULL;
+        } else {
+                dma_buf = i2c_dev->tx_dma_buf;
+                dma_chan = i2c_dev->tx_dma_chan;
+                dma_phys = i2c_dev->tx_dma_phys;
+                i2c_dev->tx_dma_buf = NULL;
+                i2c_dev->tx_dma_chan = NULL;
+        }
+        if (!dma_chan)
+                return;
+
+        dma_free_coherent(i2c_dev->dev, i2c_dev->dma_buf_size,
+                          dma_buf, dma_phys);
+        dma_release_channel(dma_chan);
+}
+
+static void tegra_i2c_mask_irq(struct tegra_i2c_dev *i2c_dev, u32 mask)
+{
+        u32 int_mask = i2c_readl(i2c_dev, I2C_INT_MASK);
+
+        int_mask &= ~mask;
+        i2c_writel(i2c_dev, int_mask, I2C_INT_MASK);
+}
+
+static void tegra_i2c_unmask_irq(struct tegra_i2c_dev *i2c_dev, u32 mask)
+{
+        u32 int_mask = i2c_readl(i2c_dev, I2C_INT_MASK);
+
+        int_mask |= mask;
+        i2c_writel(i2c_dev, int_mask, I2C_INT_MASK);
+}
+
+static int tegra_i2c_flush_fifos(struct tegra_i2c_dev *i2c_dev)
+{
+        unsigned long timeout = jiffies + HZ;
+        u32 val = i2c_readl(i2c_dev, I2C_FIFO_CONTROL);
+
+        val |= I2C_FIFO_CONTROL_TX_FLUSH | I2C_FIFO_CONTROL_RX_FLUSH;
+        i2c_writel(i2c_dev, val, I2C_FIFO_CONTROL);
+
+        while (i2c_readl(i2c_dev, I2C_FIFO_CONTROL) &
+                (I2C_FIFO_CONTROL_TX_FLUSH | I2C_FIFO_CONTROL_RX_FLUSH)) {
+                if (time_after(jiffies, timeout)) {
+                        dev_warn(i2c_dev->dev, "timeout waiting for fifo flush\n");
+                        return -ETIMEDOUT;
+                }
+                msleep(1);
+        }
+        return 0;
+}
+
+static int tegra_i2c_empty_rx_fifo(struct tegra_i2c_dev *i2c_dev)
+{
+        u32 val;
+        int rx_fifo_avail;
+        u8 *buf = i2c_dev->msg_buf;
+        size_t buf_remaining = i2c_dev->msg_buf_remaining;
+        int words_to_transfer;
+
+        val = i2c_readl(i2c_dev, I2C_FIFO_STATUS);
+        rx_fifo_avail = (val & I2C_FIFO_STATUS_RX_MASK) >>
+                I2C_FIFO_STATUS_RX_SHIFT;
+
+        /* Rounds down to not include partial word at the end of buf */
+        words_to_transfer = buf_remaining / BYTES_PER_FIFO_WORD;
+        if (words_to_transfer > rx_fifo_avail)
+                words_to_transfer = rx_fifo_avail;
+
+        i2c_readsl(i2c_dev, buf, I2C_RX_FIFO, words_to_transfer);
+
+        buf += words_to_transfer * BYTES_PER_FIFO_WORD;
+        buf_remaining -= words_to_transfer * BYTES_PER_FIFO_WORD;
+        rx_fifo_avail -= words_to_transfer;
+
+        /*
+         * If there is a partial word at the end of buf, handle it manually to
+         * prevent overwriting past the end of buf
+         */
+        if (rx_fifo_avail > 0 && buf_remaining > 0) {
+                BUG_ON(buf_remaining > 3);
+                val = i2c_readl(i2c_dev, I2C_RX_FIFO);
+                val = cpu_to_le32(val);
+                memcpy(buf, &val, buf_remaining);
+                buf_remaining = 0;
+                rx_fifo_avail--;
+        }
+
+        BUG_ON(rx_fifo_avail > 0 && buf_remaining > 0);
+        i2c_dev->msg_buf_remaining = buf_remaining;
+        i2c_dev->msg_buf = buf;
+        return 0;
+}
+
+static int tegra_i2c_fill_tx_fifo(struct tegra_i2c_dev *i2c_dev)
+{
+        u32 val;
+        int tx_fifo_avail;
+        u8 *buf = i2c_dev->msg_buf;
+        size_t buf_remaining = i2c_dev->msg_buf_remaining;
+        int words_to_transfer;
+
+        val = i2c_readl(i2c_dev, I2C_FIFO_STATUS);
+        tx_fifo_avail = (val & I2C_FIFO_STATUS_TX_MASK) >>
+                I2C_FIFO_STATUS_TX_SHIFT;
+
+        /* Rounds down to not include partial word at the end of buf */
+        words_to_transfer = buf_remaining / BYTES_PER_FIFO_WORD;
+
+        /* It's very common to have < 4 bytes, so optimize that case. */
+        if (words_to_transfer) {
+                if (words_to_transfer > tx_fifo_avail)
+                        words_to_transfer = tx_fifo_avail;
+
+                /*
+                 * Update state before writing to FIFO.  If this casues us
+                 * to finish writing all bytes (AKA buf_remaining goes to 0) we
+                 * have a potential for an interrupt (PACKET_XFER_COMPLETE is
+                 * not maskable).  We need to make sure that the isr sees
+                 * buf_remaining as 0 and doesn't call us back re-entrantly.
+                 */
+                buf_remaining -= words_to_transfer * BYTES_PER_FIFO_WORD;
+                tx_fifo_avail -= words_to_transfer;
+                i2c_dev->msg_buf_remaining = buf_remaining;
+                i2c_dev->msg_buf = buf +
+                        words_to_transfer * BYTES_PER_FIFO_WORD;
+                barrier();
+
+                i2c_writesl(i2c_dev, buf, I2C_TX_FIFO, words_to_transfer);
+
+                buf += words_to_transfer * BYTES_PER_FIFO_WORD;
+        }
+
+        /*
+         * If there is a partial word at the end of buf, handle it manually to
+         * prevent reading past the end of buf, which could cross a page
+         * boundary and fault.
+         */
+        if (tx_fifo_avail > 0 && buf_remaining > 0) {
+                BUG_ON(buf_remaining > 3);
+                memcpy(&val, buf, buf_remaining);
+                val = le32_to_cpu(val);
+
+                /* Again update before writing to FIFO to make sure isr sees. */
+                i2c_dev->msg_buf_remaining = 0;
+                i2c_dev->msg_buf = NULL;
+                barrier();
+
+                i2c_writel(i2c_dev, val, I2C_TX_FIFO);
+        }
+
+        return 0;
+}
+
+/*
+ * One of the Tegra I2C blocks is inside the DVC (Digital Voltage Controller)
+ * block.  This block is identical to the rest of the I2C blocks, except that
+ * it only supports master mode, it has registers moved around, and it needs
+ * some extra init to get it into I2C mode.  The register moves are handled
+ * by i2c_readl and i2c_writel
+ */
+static void tegra_dvc_init(struct tegra_i2c_dev *i2c_dev)
+{
+        u32 val = 0;
+
+        val = dvc_readl(i2c_dev, DVC_CTRL_REG3);
+        val |= DVC_CTRL_REG3_SW_PROG;
+        val |= DVC_CTRL_REG3_I2C_DONE_INTR_EN;
+        dvc_writel(i2c_dev, val, DVC_CTRL_REG3);
+
+        val = dvc_readl(i2c_dev, DVC_CTRL_REG1);
+        val |= DVC_CTRL_REG1_INTR_EN;
+        dvc_writel(i2c_dev, val, DVC_CTRL_REG1);
+}
+
+/*
+ * One of the Tegra I2C blocks is inside the VI (Video Interface?)
+ * block.  This block is identical to the rest of the I2C blocks, except that
+ * it only supports master mode, it has registers moved around, and it needs
+ * some extra init to get it into I2C mode.  The register moves are handled
+ * by i2c_readl and i2c_writel.
+ */
+static void tegra_vi_init(struct tegra_i2c_dev *i2c_dev)
+{
+        i2c_writel(i2c_dev, (I2C_TLOW << I2C_TLOW_SHIFT) |
+                (I2C_THIGH << I2C_THIGH_SHIFT), I2C_INTERFACE_TIMING_0);
+        i2c_writel(i2c_dev, 0x04070404, I2C_INTERFACE_TIMING_1);
+        i2c_writel(i2c_dev, 0x308, I2C_HS_INTERFACE_TIMING_0);
+        i2c_writel(i2c_dev, 0x0B0B0B, I2C_HS_INTERFACE_TIMING_1);
+        i2c_writel(i2c_dev, 0x90004, I2C_BUS_CLEAR_CNFG);
+        i2c_writel(i2c_dev, 0x0, I2C_TLOW_SEXT);
+}
+
+static inline int tegra_i2c_clock_enable(struct tegra_i2c_dev *i2c_dev)
+{
+        int ret;
+
+        if (!i2c_dev->hw->has_single_clk_source) {
+                ret = clk_enable(i2c_dev->fast_clk);
+                if (ret < 0) {
+                        dev_err(i2c_dev->dev,
+                                "Enabling fast clk failed, err %d\n", ret);
+                        return ret;
+                }
+        }
+        ret = clk_enable(i2c_dev->div_clk);
+        if (ret < 0) {
+                dev_err(i2c_dev->dev,
+                        "Enabling div clk failed, err %d\n", ret);
+                goto err_fast_disable;
+        }
+        if (i2c_dev->hw->is_vi) {
+                ret = clk_prepare_enable(i2c_dev->slow_clk);
+                if (ret < 0) {
+                        dev_err(i2c_dev->dev,
+                                "Enabling slow clk failed, err %d\n", ret);
+                        goto err_div_disable;
+                }
+
+                ret = clk_prepare_enable(i2c_dev->host1x_clk);
+                if (ret < 0) {
+                        dev_err(i2c_dev->dev,
+                                "Enabling host1x clk failed, err %d\n", ret);
+                        goto err_slow_disable;
+                }
+        }
+
+        return 0;
+
+err_slow_disable:
+        if (i2c_dev->hw->is_vi)
+                clk_disable_unprepare(i2c_dev->slow_clk);
+err_div_disable:
+        clk_disable(i2c_dev->div_clk);
+err_fast_disable:
+        if (!i2c_dev->hw->has_single_clk_source)
+                clk_disable(i2c_dev->fast_clk);
+
+        return ret;
+}
+
+static inline void tegra_i2c_clock_disable(struct tegra_i2c_dev *i2c_dev)
+{
+        if (i2c_dev->hw->is_vi) {
+                clk_disable_unprepare(i2c_dev->host1x_clk);
+                clk_disable_unprepare(i2c_dev->slow_clk);
+        }
+        clk_disable(i2c_dev->div_clk);
+        if (!i2c_dev->hw->has_single_clk_source)
+                clk_disable(i2c_dev->fast_clk);
+}
+
+static int tegra_i2c_wait_for_config_load(struct tegra_i2c_dev *i2c_dev)
+{
+        if (i2c_dev->hw->has_config_load_reg) {
+                u32 val;
+                int err;
+
+                i2c_writel(i2c_dev, I2C_MSTR_CONFIG_LOAD, I2C_CONFIG_LOAD);
+                if (!in_atomic())
+                        err = readx_poll_timeout(readl, i2c_dev->base +
+                                                tegra_i2c_reg_addr(i2c_dev,
+                                                I2C_CONFIG_LOAD), val, val == 0,
+                                                1000, I2C_CONFIG_LOAD_TIMEOUT);
+                else
+                        err = readx_poll_timeout_atomic(readl, i2c_dev->base +
+                                                tegra_i2c_reg_addr(i2c_dev,
+                                                I2C_CONFIG_LOAD), val, val == 0,
+                                                1000, I2C_CONFIG_LOAD_TIMEOUT);
+
+                if (err) {
+                        dev_warn(i2c_dev->dev,
+                                 "timeout waiting for config load\n");
+                        return err;
+                }
+        }
+
+        return 0;
+}
+
+static int tegra_i2c_set_clk_rate(struct tegra_i2c_dev *i2c_dev)
+{
+        u32 clk_multiplier = I2C_CLK_MULTIPLIER_STD_FAST_MODE;
+        int ret = 0;
+
+
+        switch (i2c_dev->bus_clk_rate) {
+        case I2C_HS_MODE:
+                clk_multiplier = i2c_dev->hw->clk_multiplier_hs_mode;
+                clk_multiplier *= (i2c_dev->clk_divisor_hs_mode + 1);
+                break;
+        case I2C_FAST_MODE_PLUS:
+        case I2C_STANDARD_MODE:
+        case I2C_FAST_MODE:
+        default:
+                clk_multiplier = (i2c_dev->low_clock_count +
+                                  i2c_dev->high_clock_count + 2);
+                clk_multiplier *= (i2c_dev->clk_divisor_non_hs_mode + 1);
+                break;
+        }
+
+        ret = clk_set_rate(i2c_dev->div_clk,
+                           i2c_dev->bus_clk_rate * clk_multiplier);
+        if (ret) {
+                dev_err(i2c_dev->dev, "Clock rate change failed %d\n", ret);
+                return ret;
+        }
+
+        return ret;
+}
+
+static void tegra_i2c_config_prod_settings(struct tegra_i2c_dev *i2c_dev)
+{
+        char *prod_name;
+        int ret;
+
+        switch (i2c_dev->bus_clk_rate) {
+        case I2C_FAST_MODE:
+                prod_name = "prod_c_fm";
+                break;
+        case I2C_FAST_MODE_PLUS:
+                prod_name = "prod_c_fmplus";
+                break;
+        case I2C_HS_MODE:
+                prod_name = "prod_c_hs";
+                break;
+        case I2C_STANDARD_MODE:
+        default:
+                prod_name = "prod_c_sm";
+                break;
+        }
+
+        ret = tegra_prod_set_by_name(&i2c_dev->base, prod_name,
+                                     i2c_dev->prod_list);
+        if (ret == 0)
+                dev_dbg(i2c_dev->dev, "setting prod: %s\n", prod_name);
+}
+
+static void tegra_i2c_get_clk_parameters(struct tegra_i2c_dev *i2c_dev)
+{
+        u32 val;
+
+        val = i2c_readl(i2c_dev, I2C_INTERFACE_TIMING_0);
+        i2c_dev->low_clock_count = val & I2C_TLOW_MASK;
+        i2c_dev->high_clock_count = (val & I2C_THIGH_MASK) >> I2C_THIGH_SHIFT;
+
+        val = i2c_readl(i2c_dev, I2C_CLK_DIVISOR);
+        i2c_dev->clk_divisor_hs_mode = val & I2C_CLK_DIVISOR_HS_MODE_MASK;
+        i2c_dev->clk_divisor_non_hs_mode = (val >>
+                        I2C_CLK_DIVISOR_STD_FAST_MODE_SHIFT);
+}
+
+static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
+{
+        u32 val;
+        int err = 0;
+        u32 clk_divisor;
+
+        err = tegra_i2c_clock_enable(i2c_dev);
+        if (err < 0) {
+                dev_err(i2c_dev->dev, "Clock enable failed %d\n", err);
+                return err;
+        }
+
+        if (i2c_dev->hw->has_sw_reset_reg) {
+                if (i2c_dev->is_periph_reset_done) {
+                        /* If already controller reset is done through */
+                        /* clock reset control register, then use SW reset */
+                        i2c_writel(i2c_dev, 1, I2C_MASTER_RESET_CONTROL);
+                        udelay(2);
+                        i2c_writel(i2c_dev, 0, I2C_MASTER_RESET_CONTROL);
+                        goto skip_periph_reset;
+                }
+        }
+        reset_control_assert(i2c_dev->rst);
+        udelay(2);
+        reset_control_deassert(i2c_dev->rst);
+
+skip_periph_reset:
+        if (i2c_dev->is_dvc)
+                tegra_dvc_init(i2c_dev);
+
+        /* configuring below register to default as per TRM*/
+        i2c_writel(i2c_dev, 0, I2C_TLOW_SEXT);
+        i2c_writel(i2c_dev, 0, I2C_CMD_ADDR0);
+        i2c_writel(i2c_dev, 0, I2C_CMD_ADDR1);
+        i2c_writel(i2c_dev, 0, I2C_CMD_DATA1);
+        i2c_writel(i2c_dev, 0, I2C_CMD_DATA2);
+        i2c_writel(i2c_dev, 0, I2C_DEBUG_CONTROL);
+        i2c_writel(i2c_dev, 0, I2C_INTERRUPT_SET_REGISTER);
+        i2c_writel(i2c_dev, 0, I2C_FIFO_CONTROL);
+
+        val = I2C_CNFG_NEW_MASTER_FSM | I2C_CNFG_PACKET_MODE_EN;
+        if (i2c_dev->bus_clk_rate != I2C_HS_MODE)
+                val |= (0x2 << I2C_CNFG_DEBOUNCE_CNT_SHIFT);
+
+        if (i2c_dev->hw->has_multi_master_mode)
+                val |= I2C_CNFG_MULTI_MASTER_MODE;
+
+        i2c_writel(i2c_dev, val, I2C_CNFG);
+        i2c_writel(i2c_dev, 0, I2C_INT_MASK);
+
+        if (i2c_dev->hw->is_vi)
+                tegra_vi_init(i2c_dev);
+
+        /* Make sure clock divisor programmed correctly */
+        if (i2c_dev->bus_clk_rate == I2C_HS_MODE) {
+                i2c_dev->clk_divisor_hs_mode = i2c_dev->hw->clk_divisor_hs_mode;
+        } else {
+                val = i2c_readl(i2c_dev, I2C_CLK_DIVISOR);
+                i2c_dev->clk_divisor_hs_mode =
+                        val & I2C_CLK_DIVISOR_HS_MODE_MASK;
+        }
+
+        clk_divisor = i2c_dev->clk_divisor_hs_mode;
+        clk_divisor |= i2c_dev->clk_divisor_non_hs_mode <<
+                                        I2C_CLK_DIVISOR_STD_FAST_MODE_SHIFT;
+        i2c_writel(i2c_dev, clk_divisor, I2C_CLK_DIVISOR);
+
+        if (i2c_dev->prod_list)
+                tegra_i2c_config_prod_settings(i2c_dev);
+
+        tegra_i2c_get_clk_parameters(i2c_dev);
+
+        err = tegra_i2c_set_clk_rate(i2c_dev);
+        if (err < 0)
+                return err;
+
+        if (!i2c_dev->is_dvc && !i2c_dev->hw->is_vi) {
+                u32 sl_cfg = i2c_readl(i2c_dev, I2C_SL_CNFG);
+
+                sl_cfg |= I2C_SL_CNFG_NACK | I2C_SL_CNFG_NEWSL;
+                i2c_writel(i2c_dev, sl_cfg, I2C_SL_CNFG);
+                i2c_writel(i2c_dev, 0xfc, I2C_SL_ADDR1);
+                i2c_writel(i2c_dev, 0x00, I2C_SL_ADDR2);
+
+        }
+
+        val = 7 << I2C_FIFO_CONTROL_TX_TRIG_SHIFT |
+                0 << I2C_FIFO_CONTROL_RX_TRIG_SHIFT;
+        i2c_writel(i2c_dev, val, I2C_FIFO_CONTROL);
+
+        err = tegra_i2c_flush_fifos(i2c_dev);
+        if (err)
+                goto err;
+
+        if (i2c_dev->is_multimaster_mode && i2c_dev->hw->has_slcg_override_reg)
+                i2c_writel(i2c_dev, I2C_MST_CORE_CLKEN_OVR, I2C_CLKEN_OVERRIDE);
+
+        err = tegra_i2c_wait_for_config_load(i2c_dev);
+        if (err)
+                goto err;
+
+        if (i2c_dev->irq_disabled) {
+                i2c_dev->irq_disabled = 0;
+                enable_irq(i2c_dev->irq);
+        }
+
+err:
+        tegra_i2c_clock_disable(i2c_dev);
+        return err;
+}
+
+static int tegra_i2c_power_enable(struct tegra_i2c_dev *i2c_dev)
+{
+        int ret;
+
+        if (i2c_dev->hw->has_regulator) {
+                ret = regulator_enable(i2c_dev->reg);
+                if (ret)
+                        return ret;
+        }
+
+        if (i2c_dev->hw->has_powergate) {
+                ret = tegra_unpowergate_partition(i2c_dev->hw->powergate_id);
+                if (ret)
+                        goto err_regulator;
+
+                ret = tegra_i2c_init(i2c_dev);
+                if (ret)
+                        goto err_powergate;
+        }
+
+        return 0;
+
+err_regulator:
+        if (i2c_dev->hw->has_regulator)
+                regulator_disable(i2c_dev->reg);
+err_powergate:
+        if (i2c_dev->hw->has_powergate)
+                tegra_powergate_partition(i2c_dev->hw->powergate_id);
+
+        return ret;
+}
+
+static void tegra_i2c_power_disable(struct tegra_i2c_dev *i2c_dev)
+{
+        if (i2c_dev->hw->has_regulator)
+                regulator_disable(i2c_dev->reg);
+
+        if (i2c_dev->hw->has_powergate)
+                tegra_powergate_partition(i2c_dev->hw->powergate_id);
+}
+
+static int tegra_i2c_disable_packet_mode(struct tegra_i2c_dev *i2c_dev)
+{
+        u32 cnfg;
+
+        cnfg = i2c_readl(i2c_dev, I2C_CNFG);
+        if (cnfg & I2C_CNFG_PACKET_MODE_EN)
+                i2c_writel(i2c_dev, cnfg & ~I2C_CNFG_PACKET_MODE_EN, I2C_CNFG);
+
+        return tegra_i2c_wait_for_config_load(i2c_dev);
+}
+
+static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
+{
+        u32 status;
+        const u32 status_err = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST;
+        struct tegra_i2c_dev *i2c_dev = dev_id;
+        unsigned long flags;
+        u32 mask;
+
+        spin_lock_irqsave(&i2c_dev->xfer_lock, flags);
+
+        status = i2c_readl(i2c_dev, I2C_INT_STATUS);
+
+        if (status == 0) {
+                dev_warn(i2c_dev->dev, "irq status 0 %08x %08x %08x\n",
+                         i2c_readl(i2c_dev, I2C_PACKET_TRANSFER_STATUS),
+                         i2c_readl(i2c_dev, I2C_STATUS),
+                         i2c_readl(i2c_dev, I2C_CNFG));
+                i2c_dev->msg_err |= I2C_ERR_UNKNOWN_INTERRUPT;
+
+                if (!i2c_dev->irq_disabled) {
+                        disable_irq_nosync(i2c_dev->irq);
+                        i2c_dev->irq_disabled = 1;
+                }
+                goto err;
+        }
+
+        if (unlikely(status & status_err)) {
+                tegra_i2c_disable_packet_mode(i2c_dev);
+                if (status & I2C_INT_NO_ACK)
+                        i2c_dev->msg_err |= I2C_ERR_NO_ACK;
+                if (status & I2C_INT_ARBITRATION_LOST)
+                        i2c_dev->msg_err |= I2C_ERR_ARBITRATION_LOST;
+                goto err;
+        }
+
+        if (i2c_dev->hw->has_hw_arb_support &&
+                (status & I2C_INT_BUS_CLEAR_DONE))
+                goto err;
+
+        if (i2c_dev->msg_read && (status & I2C_INT_RX_FIFO_DATA_REQ)) {
+                if (i2c_dev->msg_buf_remaining)
+                        tegra_i2c_empty_rx_fifo(i2c_dev);
+                else
+                        BUG();
+        }
+
+        if (!i2c_dev->msg_read && (status & I2C_INT_TX_FIFO_DATA_REQ)) {
+                if (i2c_dev->msg_buf_remaining)
+                        tegra_i2c_fill_tx_fifo(i2c_dev);
+                else
+                        tegra_i2c_mask_irq(i2c_dev, I2C_INT_TX_FIFO_DATA_REQ);
+        }
+
+        i2c_writel(i2c_dev, status, I2C_INT_STATUS);
+        if (i2c_dev->is_dvc)
+                dvc_writel(i2c_dev, DVC_STATUS_I2C_DONE_INTR, DVC_STATUS);
+
+        if (status & I2C_INT_PACKET_XFER_COMPLETE) {
+                BUG_ON(i2c_dev->msg_buf_remaining);
+                complete(&i2c_dev->msg_complete);
+        }
+        goto done;
+err:
+        /* An error occurred, mask all interrupts */
+        mask = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST |
+                I2C_INT_PACKET_XFER_COMPLETE | I2C_INT_TX_FIFO_DATA_REQ |
+                I2C_INT_RX_FIFO_DATA_REQ;
+
+        if (i2c_dev->hw->has_hw_arb_support)
+                mask |= I2C_INT_BUS_CLEAR_DONE;
+
+        /* An error occurred, mask all interrupts */
+        tegra_i2c_mask_irq(i2c_dev, mask);
+
+        i2c_writel(i2c_dev, status, I2C_INT_STATUS);
+        if (i2c_dev->is_dvc)
+                dvc_writel(i2c_dev, DVC_STATUS_I2C_DONE_INTR, DVC_STATUS);
+
+        if (i2c_dev->is_curr_dma_xfer) {
+                if (i2c_dev->curr_direction == DATA_DMA_DIR_TX) {
+                        dmaengine_terminate_all(i2c_dev->tx_dma_chan);
+                        complete(&i2c_dev->tx_dma_complete);
+                } else {
+                        dmaengine_terminate_all(i2c_dev->rx_dma_chan);
+                        complete(&i2c_dev->rx_dma_complete);
+                }
+        }
+
+        complete(&i2c_dev->msg_complete);
+done:
+        spin_unlock_irqrestore(&i2c_dev->xfer_lock, flags);
+        return IRQ_HANDLED;
+}
+
+static int tegra_i2c_issue_bus_clear(struct tegra_i2c_dev *i2c_dev)
+{
+        int ret;
+
+        if (i2c_dev->hw->has_hw_arb_support) {
+                reinit_completion(&i2c_dev->msg_complete);
+                i2c_writel(i2c_dev, I2C_BC_ENABLE
+                                | I2C_BC_SCLK_THRESHOLD
+                                | I2C_BC_STOP_COND
+                                | I2C_BC_TERMINATE
+                                , I2C_BUS_CLEAR_CNFG);
+                if (i2c_dev->hw->has_config_load_reg) {
+                        ret = tegra_i2c_wait_for_config_load(i2c_dev);
+                        if (ret)
+                                return ret;
+                }
+                tegra_i2c_unmask_irq(i2c_dev, I2C_INT_BUS_CLEAR_DONE);
+
+                ret = wait_for_completion_timeout(&i2c_dev->msg_complete,
+                                                  TEGRA_I2C_TIMEOUT);
+                if (ret == 0)
+                        dev_err(i2c_dev->dev, "timed out for bus clear\n");
+
+                if (!(i2c_readl(i2c_dev, I2C_BUS_CLEAR_STATUS) & I2C_BC_STATUS))
+                        dev_warn(i2c_dev->dev, "Un-recovered Arb lost\n");
+        } else {
+                ret = i2c_recover_bus(&i2c_dev->adapter);
+                if (ret) {
+                        dev_warn(i2c_dev->dev, "Un-recovered Arb lost\n");
+                        return ret;
+                }
+        }
+
+        return 0;
+}
+
+static void tegra_i2c_fill_packet_header(struct tegra_i2c_dev *i2c_dev,
+        struct i2c_msg *msg, enum msg_end_type end_state,
+        u32 *packet_header)
+{
+        u32 io_header = 0;
+
+        /* Generic header packet */
+        packet_header[0] = (0 << PACKET_HEADER0_HEADER_SIZE_SHIFT)
+                | PACKET_HEADER0_PROTOCOL_I2C |
+                (i2c_dev->cont_id << PACKET_HEADER0_CONT_ID_SHIFT) |
+                (1 << PACKET_HEADER0_PACKET_ID_SHIFT);
+
+        /* Payload size */
+        packet_header[1] = msg->len - 1;
+
+        /* IO header */
+        io_header = I2C_HEADER_IE_ENABLE;
+
+        if (end_state == MSG_END_CONTINUE)
+                io_header |= I2C_HEADER_CONTINUE_XFER;
+        else if (end_state == MSG_END_REPEAT_START)
+                io_header |= I2C_HEADER_REPEAT_START;
+
+        if (msg->flags & I2C_M_TEN) {
+                io_header |= msg->addr;
+                io_header |= I2C_HEADER_10BIT_ADDR;
+        } else {
+                io_header |=
+                        msg->addr << I2C_HEADER_SLAVE_ADDR_SHIFT;
+        }
+
+        if (msg->flags & I2C_M_IGNORE_NAK)
+                io_header |= I2C_HEADER_CONT_ON_NAK;
+        if (msg->flags & I2C_M_RD)
+                io_header |= I2C_HEADER_READ;
+
+        if (i2c_dev->bus_clk_rate == I2C_HS_MODE) {
+                io_header |= I2C_HEADER_HIGHSPEED_MODE;
+                io_header |= (i2c_dev->hs_master_code & 0x7)
+                        << I2C_HEADER_MASTER_ADDR_SHIFT;
+        }
+        packet_header[2] = io_header;
+}
+
+static void tegra_i2c_config_fifo_trig(struct tegra_i2c_dev *i2c_dev,
+                int len)
+{
+        u32 val;
+        u8 maxburst = 0;
+        struct dma_slave_config dma_sconfig;
+
+        val = i2c_readl(i2c_dev, I2C_FIFO_CONTROL);
+        if (len & 0xF) {
+                val |= I2C_FIFO_CONTROL_TX_TRIG_1
+                        | I2C_FIFO_CONTROL_RX_TRIG_1;
+                maxburst = 1;
+        } else if (((len) >> 4) & 0x1) {
+                val |= I2C_FIFO_CONTROL_TX_TRIG_4
+                        | I2C_FIFO_CONTROL_RX_TRIG_4;
+                maxburst = 4;
+        } else {
+                val |= I2C_FIFO_CONTROL_TX_TRIG_8
+                        | I2C_FIFO_CONTROL_RX_TRIG_8;
+                maxburst = 8;
+        }
+        i2c_writel(i2c_dev, val, I2C_FIFO_CONTROL);
+
+        if (i2c_dev->curr_direction == DATA_DMA_DIR_TX) {
+                dma_sconfig.dst_addr = i2c_dev->phys_addr + I2C_TX_FIFO;
+                dma_sconfig.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+                dma_sconfig.dst_maxburst = maxburst;
+                dmaengine_slave_config(i2c_dev->tx_dma_chan, &dma_sconfig);
+        } else {
+                dma_sconfig.src_addr = i2c_dev->phys_addr + I2C_RX_FIFO;
+                dma_sconfig.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+                dma_sconfig.src_maxburst = maxburst;
+                dmaengine_slave_config(i2c_dev->rx_dma_chan, &dma_sconfig);
+        }
+}
+
+static int tegra_i2c_start_dma_based_xfer(struct tegra_i2c_dev *i2c_dev,
+        struct i2c_msg *msg, enum msg_end_type end_state)
+{
+        int ret = 0;
+        u32 packet_header[3];
+        u32 dma_xfer_len;
+        u32 int_mask;
+        unsigned long flags = 0;
+
+        i2c_dev->is_curr_dma_xfer = true;
+
+        /* set msg_buf_remaining to zero as complete xfer is done thru DMA*/
+        i2c_dev->msg_buf_remaining = 0;
+
+        /* Enable error interrupts */
+        int_mask = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST
+                                        | I2C_INT_TX_FIFO_OVERFLOW;
+        tegra_i2c_unmask_irq(i2c_dev, int_mask);
+
+        if (!(msg->flags & I2C_M_RD)) {
+                i2c_dev->curr_direction = DATA_DMA_DIR_TX;
+                dma_sync_single_for_cpu(i2c_dev->dev, i2c_dev->tx_dma_phys,
+                                i2c_dev->dma_buf_size, DMA_TO_DEVICE);
+                /* Fill packet header */
+                tegra_i2c_fill_packet_header(i2c_dev, msg, end_state,
+                                             packet_header);
+
+                dma_xfer_len = I2C_PACKET_HEADER_SIZE;
+                memcpy(i2c_dev->tx_dma_buf, packet_header, (dma_xfer_len * 4));
+
+                /* Copy data payload to dma buffer*/
+                memcpy(i2c_dev->tx_dma_buf + dma_xfer_len, i2c_dev->msg_buf,
+                                msg->len);
+                /* make the dma buffer to read by dma */
+                dma_sync_single_for_device(i2c_dev->dev, i2c_dev->tx_dma_phys,
+                                i2c_dev->dma_buf_size, DMA_TO_DEVICE);
+                dma_xfer_len += DIV_ROUND_UP(msg->len, 4);
+
+                /* Round up final length for DMA xfer in bytes*/
+                dma_xfer_len = DIV_ROUND_UP(dma_xfer_len * 4, 4) * 4;
+
+                tegra_i2c_config_fifo_trig(i2c_dev, dma_xfer_len);
+
+                /* Acquire the lock before posting the data to FIFO */
+                spin_lock_irqsave(&i2c_dev->xfer_lock, flags);
+
+                ret = tegra_i2c_start_tx_dma(i2c_dev, dma_xfer_len);
+                if (ret < 0) {
+                        dev_err(i2c_dev->dev,
+                                "Starting tx dma failed, err %d\n", ret);
+                        goto exit;
+                }
+        } else {
+                i2c_dev->curr_direction = DATA_DMA_DIR_RX;
+                /* Round up final length for DMA xfer */
+                dma_xfer_len = DIV_ROUND_UP(msg->len, 4) * 4;
+                tegra_i2c_config_fifo_trig(i2c_dev, dma_xfer_len);
+
+                ret = tegra_i2c_start_rx_dma(i2c_dev, dma_xfer_len);
+                if (ret < 0) {
+                        dev_err(i2c_dev->dev,
+                                "Starting rx dma failed, err %d\n", ret);
+                        return ret;
+                }
+                /* Fill packet header */
+                tegra_i2c_fill_packet_header(i2c_dev, msg, end_state,
+                                             packet_header);
+
+                /* Acquire the lock before posting the data to FIFO */
+                spin_lock_irqsave(&i2c_dev->xfer_lock, flags);
+
+                /* Transfer packet header through PIO */
+                i2c_writel(i2c_dev, packet_header[0], I2C_TX_FIFO);
+                i2c_writel(i2c_dev, packet_header[1], I2C_TX_FIFO);
+                i2c_writel(i2c_dev, packet_header[2], I2C_TX_FIFO);
+        }
+
+        if (i2c_dev->hw->has_per_pkt_xfer_complete_irq)
+                int_mask |= I2C_INT_PACKET_XFER_COMPLETE;
+
+        tegra_i2c_unmask_irq(i2c_dev, int_mask);
+
+exit:
+        spin_unlock_irqrestore(&i2c_dev->xfer_lock, flags);
+        return ret;
+}
+
+static int tegra_i2c_start_pio_based_xfer(struct tegra_i2c_dev *i2c_dev,
+        struct i2c_msg *msg, enum msg_end_type end_state)
+{
+
+        u32 val;
+        u32 int_mask;
+        u32 packet_header[3];
+        unsigned long flags = 0;
+
+        i2c_dev->is_curr_dma_xfer = false;
+        val = 7 << I2C_FIFO_CONTROL_TX_TRIG_SHIFT |
+                0 << I2C_FIFO_CONTROL_RX_TRIG_SHIFT;
+        i2c_writel(i2c_dev, val, I2C_FIFO_CONTROL);
+
+        i2c_dev->msg_add = msg->addr;
+
+        /* Enable error interrupts */
+        int_mask = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST
+                                        | I2C_INT_TX_FIFO_OVERFLOW;
+        tegra_i2c_unmask_irq(i2c_dev, int_mask);
+
+        /* Fill packet header */
+        tegra_i2c_fill_packet_header(i2c_dev, msg, end_state, packet_header);
+
+        /* Acquire the lock before posting the data to FIFO */
+        spin_lock_irqsave(&i2c_dev->xfer_lock, flags);
+
+        i2c_writel(i2c_dev, packet_header[0], I2C_TX_FIFO);
+        i2c_writel(i2c_dev, packet_header[1], I2C_TX_FIFO);
+        i2c_writel(i2c_dev, packet_header[2], I2C_TX_FIFO);
+
+        if (!(msg->flags & I2C_M_RD))
+                tegra_i2c_fill_tx_fifo(i2c_dev);
+
+        if (i2c_dev->hw->has_per_pkt_xfer_complete_irq)
+                int_mask |= I2C_INT_PACKET_XFER_COMPLETE;
+
+        if (msg->flags & I2C_M_RD)
+                int_mask |= I2C_INT_RX_FIFO_DATA_REQ;
+        else if (i2c_dev->msg_buf_remaining)
+                int_mask |= I2C_INT_TX_FIFO_DATA_REQ;
+
+        tegra_i2c_unmask_irq(i2c_dev, int_mask);
+        spin_unlock_irqrestore(&i2c_dev->xfer_lock, flags);
+
+        return 0;
+}
+
+static void tegra_i2c_pre_xfer_config(struct tegra_i2c_dev *i2c_dev,
+                struct i2c_msg *msg)
+{
+        tegra_i2c_flush_fifos(i2c_dev);
+
+        i2c_dev->is_curr_dma_xfer = false;
+        i2c_dev->msg_buf = msg->buf;
+        i2c_dev->msg_buf_remaining = msg->len;
+        i2c_dev->msg_err = I2C_ERR_NONE;
+        i2c_dev->msg_read = (msg->flags & I2C_M_RD);
+        reinit_completion(&i2c_dev->msg_complete);
+
+        i2c_writel(i2c_dev, 0, I2C_INT_MASK);
+}
+
+static int tegra_i2c_handle_xfer_error(struct tegra_i2c_dev *i2c_dev)
+{
+        int ret;
+
+        /* Prints errors */
+        if (i2c_dev->msg_err & I2C_ERR_UNKNOWN_INTERRUPT)
+                dev_warn(i2c_dev->dev, "unknown interrupt Add 0x%02x\n",
+                                i2c_dev->msg_add);
+        if (i2c_dev->msg_err & I2C_ERR_NO_ACK)
+                dev_warn(i2c_dev->dev, "no acknowledge from address 0x%x\n",
+                                i2c_dev->msg_add);
+        if (i2c_dev->msg_err & I2C_ERR_ARBITRATION_LOST)
+                dev_warn(i2c_dev->dev, "arb lost in communicate to add 0x%x\n",
+                                i2c_dev->msg_add);
+        if (i2c_dev->msg_err & I2C_INT_TX_FIFO_OVERFLOW)
+                dev_warn(i2c_dev->dev, "Tx fifo overflow to add 0x%x\n",
+                                i2c_dev->msg_add);
+        /*
+         * NACK interrupt is generated before the I2C controller generates the
+         * STOP condition on the bus.
+         * So wait for 2 clock periods before resetting
+         * the controller so that STOP condition has been delivered properly.
+         */
+        if (i2c_dev->msg_err == I2C_ERR_NO_ACK)
+                udelay(DIV_ROUND_UP(2 * 1000000, i2c_dev->bus_clk_rate));
+
+        ret = tegra_i2c_init(i2c_dev);
+        if (ret) {
+                WARN_ON(1);
+                return ret;
+        }
+
+        /* Arbitration Lost occurs, Start recovery */
+        if (i2c_dev->msg_err == I2C_ERR_ARBITRATION_LOST) {
+                if (!i2c_dev->is_multimaster_mode) {
+                        ret = tegra_i2c_issue_bus_clear(i2c_dev);
+                        if (ret)
+                                return ret;
+                }
+                return -EAGAIN;
+        }
+
+        if (i2c_dev->msg_err == I2C_ERR_NO_ACK)
+                return -EREMOTEIO;
+
+        return -EIO;
+}
+
+static void tegra_i2c_reg_dump(struct tegra_i2c_dev *i2c_dev,
+                struct i2c_msg *msg)
+{
+        dev_err(i2c_dev->dev, "--- register dump for debugging ----\n");
+        dev_err(i2c_dev->dev, "I2C_CNFG - 0x%x\n",
+                        i2c_readl(i2c_dev, I2C_CNFG));
+        dev_err(i2c_dev->dev, "I2C_PACKET_TRANSFER_STATUS - 0x%x\n",
+                        i2c_readl(i2c_dev, I2C_PACKET_TRANSFER_STATUS));
+        dev_err(i2c_dev->dev, "I2C_FIFO_CONTROL - 0x%x\n",
+                        i2c_readl(i2c_dev, I2C_FIFO_CONTROL));
+        dev_err(i2c_dev->dev, "I2C_FIFO_STATUS - 0x%x\n",
+                        i2c_readl(i2c_dev, I2C_FIFO_STATUS));
+        dev_err(i2c_dev->dev, "I2C_INT_MASK - 0x%x\n",
+                        i2c_readl(i2c_dev, I2C_INT_MASK));
+        dev_err(i2c_dev->dev, "I2C_INT_STATUS - 0x%x\n",
+                        i2c_readl(i2c_dev, I2C_INT_STATUS));
+        dev_err(i2c_dev->dev, "msg->len - %d\n", msg->len);
+        dev_err(i2c_dev->dev, "is_msg_write - %d\n",
+                        !(msg->flags & I2C_M_RD));
+        dev_err(i2c_dev->dev, "buf_remaining - %zd\n",
+                        i2c_dev->msg_buf_remaining);
+}
+
+static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
+        struct i2c_msg *msg, enum msg_end_type end_state)
+{
+        u32 int_mask = 0;
+        unsigned long time_left;
+        int ret;
+
+        if (msg->len == 0)
+                return -EINVAL;
+
+        if (!i2c_dev->disable_dma_mode && (msg->len > I2C_PIO_MODE_MAX_LEN)
+                        && !(i2c_dev->tx_dma_chan && i2c_dev->rx_dma_chan)) {
+                ret = tegra_i2c_init_dma_param(i2c_dev, true);
+                if (ret && (ret != -EPROBE_DEFER) && (ret != -ENODEV))
+                        return ret;
+                ret = tegra_i2c_init_dma_param(i2c_dev, false);
+                if (ret && (ret != -EPROBE_DEFER) && (ret != -ENODEV))
+                        return ret;
+        }
+
+        tegra_i2c_pre_xfer_config(i2c_dev, msg);
+
+        if ((msg->len > I2C_PIO_MODE_MAX_LEN) && i2c_dev->tx_dma_chan
+                                && i2c_dev->rx_dma_chan)
+                ret = tegra_i2c_start_dma_based_xfer(i2c_dev, msg, end_state);
+        else
+                ret = tegra_i2c_start_pio_based_xfer(i2c_dev, msg, end_state);
+
+        if (ret)
+                return ret;
+
+        dev_dbg(i2c_dev->dev, "unmasked irq: %02x\n",
+                i2c_readl(i2c_dev, I2C_INT_MASK));
+
+        if (i2c_dev->is_curr_dma_xfer) {
+                if (i2c_dev->curr_direction == DATA_DMA_DIR_TX) {
+                        time_left = wait_for_completion_timeout(
+                                        &i2c_dev->tx_dma_complete,
+                                        TEGRA_I2C_TIMEOUT);
+                        if (time_left == 0) {
+                                dev_err(i2c_dev->dev, "tx dma timeout\n");
+                                dmaengine_terminate_all(i2c_dev->tx_dma_chan);
+                                goto end_xfer;
+                        }
+                } else if (i2c_dev->curr_direction == DATA_DMA_DIR_RX) {
+                        time_left = wait_for_completion_timeout(
+                                        &i2c_dev->rx_dma_complete,
+                                        TEGRA_I2C_TIMEOUT);
+                        if (time_left == 0) {
+                                dev_err(i2c_dev->dev, "rx dma timeout\n");
+                                dmaengine_terminate_all(i2c_dev->rx_dma_chan);
+                                goto end_xfer;
+                        }
+                }
+        }
+
+        time_left = wait_for_completion_timeout(&i2c_dev->msg_complete,
+                                                TEGRA_I2C_TIMEOUT);
+        if (time_left == 0) {
+                tegra_i2c_reg_dump(i2c_dev, msg);
+                if (i2c_dev->is_curr_dma_xfer) {
+                        if (i2c_dev->curr_direction == DATA_DMA_DIR_TX)
+                                dmaengine_terminate_all(i2c_dev->tx_dma_chan);
+                        else if (i2c_dev->curr_direction == DATA_DMA_DIR_RX)
+                                dmaengine_terminate_all(i2c_dev->rx_dma_chan);
+                }
+        }
+
+end_xfer:
+        int_mask = i2c_readl(i2c_dev, I2C_INT_MASK);
+        tegra_i2c_mask_irq(i2c_dev, int_mask);
+
+        if (time_left == 0) {
+                dev_err(i2c_dev->dev,
+                        "i2c transfer timed out, addr 0x%04x, data 0x%02x\n",
+                        msg->addr, msg->buf[0]);
+
+                ret = tegra_i2c_init(i2c_dev);
+                if (!ret)
+                        ret = -ETIMEDOUT;
+                else
+                        WARN_ON(1);
+                return ret;
+        }
+
+        dev_dbg(i2c_dev->dev, "transfer complete: %d %d %d\n",
+                ret, completion_done(&i2c_dev->msg_complete), i2c_dev->msg_err);
+
+        if (likely(i2c_dev->msg_err == I2C_ERR_NONE))
+                return 0;
+
+        return tegra_i2c_handle_xfer_error(i2c_dev);
+}
+
+static int tegra_i2c_split_i2c_msg_xfer(struct tegra_i2c_dev *i2c_dev,
+        struct i2c_msg *msg, enum msg_end_type end_type)
+{
+        int size, len, ret;
+        struct i2c_msg temp_msg;
+        u8 *buf = msg->buf;
+        enum msg_end_type temp_end_type;
+
+        size = msg->len;
+        temp_msg.flags = msg->flags;
+        temp_msg.addr = msg->addr;
+        temp_end_type = end_type;
+        do {
+                temp_msg.buf = buf;
+                len = min(size, I2C_MAX_TRANSFER_LEN);
+                temp_msg.len = len;
+                size -= len;
+                if ((len == I2C_MAX_TRANSFER_LEN) && size)
+                        end_type = MSG_END_CONTINUE;
+                else
+                        end_type = temp_end_type;
+                ret = tegra_i2c_xfer_msg(i2c_dev, &temp_msg, end_type);
+                if (ret)
+                        return ret;
+                buf += len;
+        } while (size != 0);
+
+        return ret;
+}
+
+static int tegra_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
+        int num)
+{
+        struct tegra_i2c_dev *i2c_dev = i2c_get_adapdata(adap);
+        int i;
+        int ret = 0;
+
+        if (i2c_dev->is_suspended)
+                return -EBUSY;
+
+        ret = tegra_i2c_power_enable(i2c_dev);
+        if (ret < 0) {
+                dev_err(i2c_dev->dev, "Regulator enable failed %d\n", ret);
+                return ret;
+        }
+
+        if (i2c_dev->is_shutdown && i2c_dev->bit_banging_xfer_after_shutdown)
+                return tegra_i2c_gpio_xfer(adap, msgs, num);
+
+        pm_runtime_get_sync(&adap->dev);
+        ret = tegra_i2c_clock_enable(i2c_dev);
+        if (ret < 0) {
+                tegra_i2c_power_disable(i2c_dev);
+                dev_err(i2c_dev->dev, "Clock enable failed %d\n", ret);
+                pm_runtime_put(&adap->dev);
+                return ret;
+        }
+
+        if (adap->bus_clk_rate != i2c_dev->bus_clk_rate) {
+                i2c_dev->bus_clk_rate = adap->bus_clk_rate;
+                ret = tegra_i2c_set_clk_rate(i2c_dev);
+                if (ret < 0)
+                        return ret;
+        }
+
+        for (i = 0; i < num; i++) {
+                enum msg_end_type end_type = MSG_END_STOP;
+
+                if (i < (num - 1)) {
+                        if (msgs[i + 1].flags & I2C_M_NOSTART)
+                                end_type = MSG_END_CONTINUE;
+                        else
+                                end_type = MSG_END_REPEAT_START;
+                }
+                if (msgs[i].len > I2C_MAX_TRANSFER_LEN)
+                        ret = tegra_i2c_split_i2c_msg_xfer(i2c_dev, &msgs[i],
+                                                           end_type);
+                else
+                        ret = tegra_i2c_xfer_msg(i2c_dev, &msgs[i], end_type);
+                if (ret)
+                        break;
+        }
+        tegra_i2c_clock_disable(i2c_dev);
+        tegra_i2c_power_disable(i2c_dev);
+        pm_runtime_put(&adap->dev);
+        return ret ?: i;
+}
+
+static u32 tegra_i2c_func(struct i2c_adapter *adap)
+{
+        struct tegra_i2c_dev *i2c_dev = i2c_get_adapdata(adap);
+        u32 ret = I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK) |
+                  I2C_FUNC_10BIT_ADDR | I2C_FUNC_PROTOCOL_MANGLING;
+
+        if (i2c_dev->hw->has_continue_xfer_support)
+                ret |= I2C_FUNC_NOSTART;
+        return ret;
+}
+
+static void tegra_i2c_parse_dt(struct tegra_i2c_dev *i2c_dev)
+{
+        struct device_node *np = i2c_dev->dev->of_node;
+        int ret;
+        u32 prop;
+
+        ret = of_property_read_u32(np,
+                "clock-frequency", &i2c_dev->bus_clk_rate);
+        if (ret)
+                i2c_dev->bus_clk_rate = 100000; /* default clock rate */
+
+        i2c_dev->is_multimaster_mode =
+                of_property_read_bool(np, "multi-master");
+
+        i2c_dev->scl_gpio = of_get_named_gpio(np, "scl-gpio", 0);
+        i2c_dev->sda_gpio = of_get_named_gpio(np, "sda-gpio", 0);
+
+        i2c_dev->bit_banging_xfer_after_shutdown = of_property_read_bool(np,
+                        "nvidia,bit-banging-xfer-after-shutdown");
+
+        ret = of_property_read_u32(np, "nvidia,hs-master-code", &prop);
+        if (!ret)
+                i2c_dev->hs_master_code = prop;
+
+        i2c_dev->disable_dma_mode = of_property_read_bool(np,
+                        "nvidia,disable-dma-mode");
+
+        i2c_dev->is_clkon_always = of_property_read_bool(np,
+                        "nvidia,clock-always-on");
+}
+
+static const struct i2c_algorithm tegra_i2c_algo = {
+        .master_xfer    = tegra_i2c_xfer,
+        .functionality  = tegra_i2c_func,
+};
+
+/* payload size is only 12 bit */
+static struct i2c_adapter_quirks tegra_i2c_quirks = {
+        .max_read_len = 4096,
+        .max_write_len = 4096,
+};
+
+static const struct tegra_i2c_hw_feature tegra20_i2c_hw = {
+        .has_continue_xfer_support = false,
+        .has_per_pkt_xfer_complete_irq = false,
+        .has_single_clk_source = false,
+        .clk_divisor_hs_mode = 3,
+        .clk_multiplier_hs_mode = 12,
+        .clk_divisor_std_fast_mode = 0,
+        .clk_divisor_fast_plus_mode = 0,
+        .has_config_load_reg = false,
+        .has_multi_master_mode = false,
+        .has_slcg_override_reg = false,
+        .has_sw_reset_reg = false,
+        .has_hw_arb_support = false,
+        .has_reg_write_buffering = true,
+        .has_slcg_support = false,
+};
+
+static const struct tegra_i2c_hw_feature tegra30_i2c_hw = {
+        .has_continue_xfer_support = true,
+        .has_per_pkt_xfer_complete_irq = false,
+        .has_single_clk_source = false,
+        .clk_divisor_hs_mode = 3,
+        .clk_multiplier_hs_mode = 12,
+        .clk_divisor_std_fast_mode = 0,
+        .clk_divisor_fast_plus_mode = 0,
+        .has_config_load_reg = false,
+        .has_multi_master_mode = false,
+        .has_slcg_override_reg = false,
+        .has_sw_reset_reg = false,
+        .has_hw_arb_support = false,
+        .has_reg_write_buffering = true,
+        .has_slcg_support = false,
+};
+
+static const struct tegra_i2c_hw_feature tegra114_i2c_hw = {
+        .has_continue_xfer_support = true,
+        .has_per_pkt_xfer_complete_irq = true,
+        .has_single_clk_source = true,
+        .clk_divisor_hs_mode = 1,
+        .clk_multiplier_hs_mode = 3,
+        .clk_divisor_std_fast_mode = 0x19,
+        .clk_divisor_fast_plus_mode = 0x10,
+        .has_config_load_reg = false,
+        .has_multi_master_mode = false,
+        .has_slcg_override_reg = false,
+        .has_sw_reset_reg = false,
+        .has_hw_arb_support = true,
+        .has_reg_write_buffering = true,
+        .has_slcg_support = false,
+};
+
+static const struct tegra_i2c_hw_feature tegra124_i2c_hw = {
+        .has_continue_xfer_support = true,
+        .has_per_pkt_xfer_complete_irq = true,
+        .has_single_clk_source = true,
+        .clk_divisor_hs_mode = 2,
+        .clk_multiplier_hs_mode = 13,
+        .clk_divisor_std_fast_mode = 0x19,
+        .clk_divisor_fast_plus_mode = 0x10,
+        .has_config_load_reg = true,
+        .has_multi_master_mode = false,
+        .has_slcg_override_reg = true,
+        .has_sw_reset_reg = false,
+        .has_hw_arb_support = true,
+        .has_reg_write_buffering = true,
+        .has_slcg_support = false,
+};
+
+static const struct tegra_i2c_hw_feature tegra210_i2c_hw = {
+        .has_continue_xfer_support = true,
+        .has_per_pkt_xfer_complete_irq = true,
+        .has_single_clk_source = true,
+        .clk_divisor_hs_mode = 2,
+        .clk_multiplier_hs_mode = 13,
+        .clk_divisor_std_fast_mode = 0x19,
+        .clk_divisor_fast_plus_mode = 0x10,
+        .has_config_load_reg = true,
+        .has_multi_master_mode = true,
+        .has_slcg_override_reg = true,
+        .has_sw_reset_reg = false,
+        .has_hw_arb_support = true,
+        .has_reg_write_buffering = true,
+        .has_slcg_support = false,
+};
+
+static const struct tegra_i2c_hw_feature tegra210_vii2c_hw = {
+        .has_continue_xfer_support = true,
+        .has_per_pkt_xfer_complete_irq = true,
+        .has_single_clk_source = true,
+        .clk_divisor_hs_mode = 2,
+        .clk_multiplier_hs_mode = 13,
+        .clk_divisor_std_fast_mode = 0x19,
+        .clk_divisor_fast_plus_mode = 0x10,
+        .has_config_load_reg = true,
+        .has_multi_master_mode = true,
+        .has_slcg_override_reg = true,
+        .has_sw_reset_reg = false,
+        .has_hw_arb_support = true,
+        .has_reg_write_buffering = true,
+        .has_slcg_support = false,
+        .has_regulator = true,
+        .has_powergate = true,
+        .powergate_id = TEGRA210_POWER_DOMAIN_VENC,
+        .is_vi = true,
+};
+
+static const struct tegra_i2c_hw_feature tegra186_i2c_hw = {
+        .has_continue_xfer_support = true,
+        .has_per_pkt_xfer_complete_irq = true,
+        .has_single_clk_source = true,
+        .clk_divisor_hs_mode = 2,
+        .clk_multiplier_hs_mode = 13,
+        .clk_divisor_std_fast_mode = 0x19,
+        .clk_divisor_fast_plus_mode = 0x10,
+        .has_config_load_reg = true,
+        .has_multi_master_mode = true,
+        .has_slcg_override_reg = true,
+        .has_sw_reset_reg = true,
+        .has_hw_arb_support = true,
+        .has_reg_write_buffering = false,
+        .has_slcg_support = true,
+};
+
+/* Match table for of_platform binding */
+static const struct of_device_id tegra_i2c_of_match[] = {
+        { .compatible = "nvidia,tegra210-vii2c",
+                .data = &tegra210_vii2c_hw, },
+        {},
+};
+MODULE_DEVICE_TABLE(of, tegra_i2c_of_match);
+
+static int tegra_i2c_probe(struct platform_device *pdev)
+{
+        struct tegra_i2c_dev *i2c_dev;
+        struct resource *res;
+        struct clk *div_clk;
+        struct clk *fast_clk;
+        void __iomem *base;
+        phys_addr_t phys_addr;
+        int irq;
+        int ret = 0;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        phys_addr = res->start;
+        base = devm_ioremap_resource(&pdev->dev, res);
+        if (IS_ERR(base))
+                return PTR_ERR(base);
+
+        res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+        if (!res) {
+                dev_err(&pdev->dev, "no irq resource\n");
+                return -EINVAL;
+        }
+        irq = res->start;
+
+        div_clk = devm_clk_get(&pdev->dev, "vii2c");
+        if (IS_ERR(div_clk)) {
+                dev_err(&pdev->dev, "missing controller clock");
+                return PTR_ERR(div_clk);
+        }
+
+        i2c_dev = devm_kzalloc(&pdev->dev, sizeof(*i2c_dev), GFP_KERNEL);
+        if (!i2c_dev)
+                return -ENOMEM;
+
+        i2c_dev->base = base;
+        i2c_dev->phys_addr = phys_addr;
+        i2c_dev->div_clk = div_clk;
+        i2c_dev->adapter.algo = &tegra_i2c_algo;
+        i2c_dev->adapter.quirks = &tegra_i2c_quirks;
+        i2c_dev->irq = irq;
+        i2c_dev->dev = &pdev->dev;
+        i2c_dev->dma_buf_size = I2C_DMA_MAX_BUF_LEN;
+
+        i2c_dev->rst = devm_reset_control_get(&pdev->dev, "vii2c");
+        if (IS_ERR(i2c_dev->rst)) {
+                dev_err(&pdev->dev, "missing controller reset");
+                return PTR_ERR(i2c_dev->rst);
+        }
+
+        tegra_i2c_parse_dt(i2c_dev);
+
+        i2c_dev->hw = &tegra20_i2c_hw;
+
+        if (pdev->dev.of_node) {
+                const struct of_device_id *match;
+
+                match = of_match_device(tegra_i2c_of_match, &pdev->dev);
+                i2c_dev->hw = match->data;
+                i2c_dev->is_dvc = of_device_is_compatible(pdev->dev.of_node,
+                                                "nvidia,tegra20-i2c-dvc");
+        } else if (pdev->id == 3) {
+                i2c_dev->is_dvc = 1;
+        }
+        init_completion(&i2c_dev->msg_complete);
+        init_completion(&i2c_dev->tx_dma_complete);
+        init_completion(&i2c_dev->rx_dma_complete);
+
+        if (!i2c_dev->hw->has_single_clk_source) {
+                fast_clk = devm_clk_get(&pdev->dev, "fast-clk");
+                if (IS_ERR(fast_clk)) {
+                        dev_err(&pdev->dev, "missing fast clock");
+                        return PTR_ERR(fast_clk);
+                }
+                i2c_dev->fast_clk = fast_clk;
+        }
+
+        if (i2c_dev->hw->is_vi) {
+                i2c_dev->slow_clk = devm_clk_get(&pdev->dev, "i2cslow");
+                if (IS_ERR(i2c_dev->slow_clk)) {
+                        dev_err(&pdev->dev, "missing slow clock");
+                        return PTR_ERR(i2c_dev->slow_clk);
+                }
+                i2c_dev->host1x_clk = devm_clk_get(&pdev->dev, "host1x");
+                if (IS_ERR(i2c_dev->host1x_clk)) {
+                        dev_err(&pdev->dev, "missing host1x clock");
+                        return PTR_ERR(i2c_dev->host1x_clk);
+                }
+        }
+
+        if (i2c_dev->hw->has_regulator) {
+                i2c_dev->reg = devm_regulator_get(&pdev->dev, "avdd_dsi_csi");
+                if (IS_ERR(i2c_dev->reg)) {
+                        dev_err(&pdev->dev, "could not get regulator");
+                        return PTR_ERR(i2c_dev->reg);
+                }
+        }
+
+        spin_lock_init(&i2c_dev->xfer_lock);
+
+        i2c_dev->prod_list = devm_tegra_prod_get(&pdev->dev);
+        if (IS_ERR_OR_NULL(i2c_dev->prod_list)) {
+                dev_dbg(&pdev->dev, "Prod-setting not available\n");
+                i2c_dev->prod_list = NULL;
+        }
+
+        platform_set_drvdata(pdev, i2c_dev);
+
+        if (!i2c_dev->hw->has_single_clk_source) {
+                ret = clk_prepare(i2c_dev->fast_clk);
+                if (ret < 0) {
+                        dev_err(i2c_dev->dev, "Clock prepare failed %d\n", ret);
+                        return ret;
+                }
+        }
+
+        i2c_dev->clk_divisor_non_hs_mode =
+                        i2c_dev->hw->clk_divisor_std_fast_mode;
+        if (i2c_dev->hw->clk_divisor_fast_plus_mode &&
+                (i2c_dev->bus_clk_rate == I2C_FAST_MODE_PLUS))
+                i2c_dev->clk_divisor_non_hs_mode =
+                        i2c_dev->hw->clk_divisor_fast_plus_mode;
+
+        ret = tegra_i2c_set_clk_rate(i2c_dev);
+        if (ret < 0)
+                return ret;
+
+        ret = clk_prepare(i2c_dev->div_clk);
+        if (ret < 0) {
+                dev_err(i2c_dev->dev, "Clock prepare failed %d\n", ret);
+                goto unprepare_fast_clk;
+        }
+
+        if (!i2c_dev->hw->has_powergate) {
+                ret = tegra_i2c_init(i2c_dev);
+                if (ret) {
+                        dev_err(&pdev->dev, "Failed to initialize i2c controller");
+                        goto unprepare_div_clk;
+                }
+        }
+
+        if (i2c_dev->is_multimaster_mode || i2c_dev->hw->has_slcg_support)
+                i2c_dev->is_clkon_always = true;
+
+        if (i2c_dev->is_clkon_always) {
+                ret = tegra_i2c_clock_enable(i2c_dev);
+                if (ret < 0) {
+                        dev_err(i2c_dev->dev, "div_clk enable failed %d\n",
+                                ret);
+                        goto unprepare_div_clk;
+                }
+        }
+
+        if (!i2c_dev->disable_dma_mode) {
+                ret = tegra_i2c_init_dma_param(i2c_dev, true);
+                if (ret && (ret != -EPROBE_DEFER) && (ret != -ENODEV))
+                        goto disable_clk;
+                ret = tegra_i2c_init_dma_param(i2c_dev, false);
+                if (ret && (ret != -EPROBE_DEFER) && (ret != -ENODEV))
+                        goto disable_clk;
+        }
+
+        ret = devm_request_irq(&pdev->dev, i2c_dev->irq,
+                        tegra_i2c_isr, 0, dev_name(&pdev->dev), i2c_dev);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to request irq %i\n", i2c_dev->irq);
+                goto disable_clk;
+        }
+
+        pm_runtime_enable(&pdev->dev);
+        i2c_set_adapdata(&i2c_dev->adapter, i2c_dev);
+        i2c_dev->adapter.owner = THIS_MODULE;
+        i2c_dev->adapter.class = I2C_CLASS_DEPRECATED;
+        strlcpy(i2c_dev->adapter.name, "Tegra I2C adapter",
+                sizeof(i2c_dev->adapter.name));
+        i2c_dev->adapter.bus_clk_rate = i2c_dev->bus_clk_rate;
+        i2c_dev->adapter.dev.parent = &pdev->dev;
+        i2c_dev->adapter.nr = pdev->id;
+        i2c_dev->adapter.dev.of_node = pdev->dev.of_node;
+        i2c_dev->bri.scl_gpio = i2c_dev->scl_gpio;
+        i2c_dev->bri.sda_gpio = i2c_dev->sda_gpio;
+        i2c_dev->bri.recover_bus = i2c_generic_gpio_recovery;
+        i2c_dev->adapter.bus_recovery_info = &i2c_dev->bri;
+
+        ret = i2c_add_numbered_adapter(&i2c_dev->adapter);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to add I2C adapter\n");
+                goto disable_clk;
+        }
+        i2c_dev->cont_id = i2c_dev->adapter.nr & PACKET_HEADER0_CONT_ID_MASK;
+        pm_runtime_enable(&i2c_dev->adapter.dev);
+        tegra_i2c_gpio_init(i2c_dev);
+
+        return 0;
+
+disable_clk:
+        tegra_i2c_clock_disable(i2c_dev);
+
+unprepare_div_clk:
+        clk_unprepare(i2c_dev->div_clk);
+
+unprepare_fast_clk:
+        if (!i2c_dev->hw->has_single_clk_source)
+                clk_unprepare(i2c_dev->fast_clk);
+
+        return ret;
+}
+
+static int tegra_i2c_remove(struct platform_device *pdev)
+{
+        struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
+
+        i2c_del_adapter(&i2c_dev->adapter);
+        pm_runtime_disable(&i2c_dev->adapter.dev);
+
+        if (i2c_dev->tx_dma_chan)
+                tegra_i2c_deinit_dma_param(i2c_dev, false);
+        if (i2c_dev->rx_dma_chan)
+                tegra_i2c_deinit_dma_param(i2c_dev, true);
+
+        if (i2c_dev->is_clkon_always)
+                tegra_i2c_clock_disable(i2c_dev);
+        pm_runtime_disable(&pdev->dev);
+
+        clk_unprepare(i2c_dev->div_clk);
+        if (!i2c_dev->hw->has_single_clk_source)
+                clk_unprepare(i2c_dev->fast_clk);
+
+        return 0;
+}
+
+static void tegra_i2c_shutdown(struct platform_device *pdev)
+{
+        struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
+
+        dev_info(i2c_dev->dev, "Bus is shutdown down..\n");
+        i2c_shutdown_adapter(&i2c_dev->adapter);
+        i2c_dev->is_shutdown = true;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int tegra_i2c_suspend(struct device *dev)
+{
+        struct tegra_i2c_dev *i2c_dev = dev_get_drvdata(dev);
+
+        i2c_lock_adapter(&i2c_dev->adapter);
+        i2c_dev->is_suspended = true;
+
+        if (i2c_dev->is_clkon_always)
+                tegra_i2c_clock_disable(i2c_dev);
+
+        i2c_unlock_adapter(&i2c_dev->adapter);
+
+        return 0;
+}
+
+static int tegra_i2c_resume(struct device *dev)
+{
+        struct tegra_i2c_dev *i2c_dev = dev_get_drvdata(dev);
+        int ret;
+
+        i2c_lock_adapter(&i2c_dev->adapter);
+
+        if (!i2c_dev->hw->has_powergate) {
+                ret = tegra_i2c_init(i2c_dev);
+                if (ret) {
+                        i2c_unlock_adapter(&i2c_dev->adapter);
+                        return ret;
+                }
+        }
+
+        if (i2c_dev->is_clkon_always) {
+                ret = tegra_i2c_clock_enable(i2c_dev);
+                if (ret < 0) {
+                        dev_err(i2c_dev->dev, "clock enable failed %d\n",
+                                ret);
+                        return ret;
+                }
+        }
+        i2c_dev->is_suspended = false;
+        i2c_unlock_adapter(&i2c_dev->adapter);
+
+        return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(tegra_i2c_pm, tegra_i2c_suspend, tegra_i2c_resume);
+#define TEGRA_I2C_PM    (&tegra_i2c_pm)
+#else
+#define TEGRA_I2C_PM    NULL
+#endif
+
+static struct platform_driver tegra_i2c_driver = {
+        .probe   = tegra_i2c_probe,
+        .remove  = tegra_i2c_remove,
+        .late_shutdown = tegra_i2c_shutdown,
+        .driver  = {
+                .name  = "tegra-vii2c",
+                .of_match_table = tegra_i2c_of_match,
+                .pm    = TEGRA_I2C_PM,
+        },
+};
+
+static int __init tegra_i2c_init_driver(void)
+{
+        return platform_driver_register(&tegra_i2c_driver);
+}
+
+static void __exit tegra_i2c_exit_driver(void)
+{
+        platform_driver_unregister(&tegra_i2c_driver);
+}
+
+subsys_initcall(tegra_i2c_init_driver);
+module_exit(tegra_i2c_exit_driver);
+
+MODULE_DESCRIPTION("nVidia Tegra2 I2C Bus Controller driver");
+MODULE_AUTHOR("Colin Cross");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx185.c b/drivers/media/i2c/imx185.c
new file mode 100644
index 000000000..2dbd79d41
--- /dev/null
+++ b/drivers/media/i2c/imx185.c
@@ -0,0 +1,1125 @@
+/*
+ * imx185.c - imx185 sensor driver
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+
+#include <linux/seq_file.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <media/tegra_v4l2_camera.h>
+#include <media/camera_common.h>
+#include "imx185_mode_tbls.h"
+
+#define IMX185_DEFAULT_MODE     IMX185_MODE_1920X1080_CROP_30FPS
+#define IMX185_DEFAULT_DATAFMT  MEDIA_BUS_FMT_SRGGB12_1X12
+
+#define IMX185_MIN_FRAME_LENGTH (1125)
+#define IMX185_MAX_FRAME_LENGTH (0x1FFFF)
+#define IMX185_MIN_SHS1_1080P_HDR       (5)
+#define IMX185_MIN_SHS2_1080P_HDR       (82)
+#define IMX185_MAX_SHS2_1080P_HDR       (IMX185_MAX_FRAME_LENGTH - 5)
+#define IMX185_MAX_SHS1_1080P_HDR       (IMX185_MAX_SHS2_1080P_HDR / 16)
+
+#define IMX185_FRAME_LENGTH_ADDR_MSB            0x301A
+#define IMX185_FRAME_LENGTH_ADDR_MID            0x3019
+#define IMX185_FRAME_LENGTH_ADDR_LSB            0x3018
+#define IMX185_COARSE_TIME_SHS1_ADDR_MSB        0x3022
+#define IMX185_COARSE_TIME_SHS1_ADDR_MID        0x3021
+#define IMX185_COARSE_TIME_SHS1_ADDR_LSB        0x3020
+#define IMX185_COARSE_TIME_SHS2_ADDR_MSB        0x3025
+#define IMX185_COARSE_TIME_SHS2_ADDR_MID        0x3024
+#define IMX185_COARSE_TIME_SHS2_ADDR_LSB        0x3023
+#define IMX185_GAIN_ADDR                                        0x3014
+#define IMX185_GROUP_HOLD_ADDR                          0x3001
+#define IMX185_SW_RESET_ADDR                    0x3003
+
+#define IMX185_FUSE_ID_ADDR     0x3382
+#define IMX185_FUSE_ID_SIZE     6
+#define IMX185_FUSE_ID_STR_SIZE (IMX185_FUSE_ID_SIZE * 2)
+#define IMX185_DEFAULT_WIDTH    1920
+#define IMX185_DEFAULT_HEIGHT   1080
+#define IMX185_DEFAULT_CLK_FREQ 37125000
+
+struct imx185 {
+        struct camera_common_power_rail power;
+        int     numctrls;
+        struct v4l2_ctrl_handler        ctrl_handler;
+        struct i2c_client       *i2c_client;
+        struct v4l2_subdev      *subdev;
+        struct media_pad        pad;
+        u32                             frame_length;
+        s32     group_hold_prev;
+        bool    group_hold_en;
+        s64 last_wdr_et_val;
+        struct regmap   *regmap;
+        struct camera_common_data       *s_data;
+        struct camera_common_pdata      *pdata;
+        struct v4l2_ctrl                *ctrls[];
+};
+
+static const struct regmap_config sensor_regmap_config = {
+        .reg_bits = 16,
+        .val_bits = 8,
+        .cache_type = REGCACHE_RBTREE,
+        .use_single_rw = true,
+};
+
+static int imx185_g_volatile_ctrl(struct v4l2_ctrl *ctrl);
+static int imx185_s_ctrl(struct v4l2_ctrl *ctrl);
+
+static const struct v4l2_ctrl_ops imx185_ctrl_ops = {
+        .g_volatile_ctrl = imx185_g_volatile_ctrl,
+        .s_ctrl = imx185_s_ctrl,
+};
+
+static struct v4l2_ctrl_config ctrl_config_list[] = {
+/* Do not change the name field for the controls! */
+        {
+                .ops = &imx185_ctrl_ops,
+                .id = V4L2_CID_GAIN,
+                .name = "Gain",
+                .type = V4L2_CTRL_TYPE_INTEGER64,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = 0 * FIXED_POINT_SCALING_FACTOR,
+                .max = 48 * FIXED_POINT_SCALING_FACTOR,
+                .def = 0 * FIXED_POINT_SCALING_FACTOR,
+                .step = 1,
+        },
+        {
+                .ops = &imx185_ctrl_ops,
+                .id = V4L2_CID_EXPOSURE,
+                .name = "Exposure",
+                .type = V4L2_CTRL_TYPE_INTEGER64,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = 30 * FIXED_POINT_SCALING_FACTOR / 1000000,
+                .max = 1000000LL * FIXED_POINT_SCALING_FACTOR / 1000000,
+                .def = 30 * FIXED_POINT_SCALING_FACTOR / 1000000,
+                .step = 1,
+        },
+        {
+                .ops = &imx185_ctrl_ops,
+                .id = V4L2_CID_FRAME_RATE,
+                .name = "Frame Rate",
+                .type = V4L2_CTRL_TYPE_INTEGER64,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = 1 * FIXED_POINT_SCALING_FACTOR,
+                .max = 60 * FIXED_POINT_SCALING_FACTOR,
+                .def = 30 * FIXED_POINT_SCALING_FACTOR,
+                .step = 1,
+        },
+        {
+                .ops = &imx185_ctrl_ops,
+                .id = V4L2_CID_GROUP_HOLD,
+                .name = "Group Hold",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &imx185_ctrl_ops,
+                .id = V4L2_CID_HDR_EN,
+                .name = "HDR enable",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &imx185_ctrl_ops,
+                .id = V4L2_CID_FUSE_ID,
+                .name = "Fuse ID",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = IMX185_FUSE_ID_STR_SIZE,
+                .step = 2,
+        },
+        {
+                .ops = &imx185_ctrl_ops,
+                .id = V4L2_CID_SENSOR_MODE_ID,
+                .name = "Sensor Mode",
+                .type = V4L2_CTRL_TYPE_INTEGER64,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = 0,
+                .max = 0xFF,
+                .def = 0xFE,
+                .step = 1,
+        },
+};
+
+static inline void imx185_get_frame_length_regs(imx185_reg *regs,
+                                u32 frame_length)
+{
+        regs->addr = IMX185_FRAME_LENGTH_ADDR_MSB;
+        regs->val = (frame_length >> 16) & 0x01;
+
+        (regs + 1)->addr = IMX185_FRAME_LENGTH_ADDR_MID;
+        (regs + 1)->val = (frame_length >> 8) & 0xff;
+
+        (regs + 2)->addr = IMX185_FRAME_LENGTH_ADDR_LSB;
+        (regs + 2)->val = (frame_length) & 0xff;
+}
+
+static inline void imx185_get_coarse_time_regs_shs1(imx185_reg *regs,
+                                u32 coarse_time)
+{
+        regs->addr = IMX185_COARSE_TIME_SHS1_ADDR_MSB;
+        regs->val = (coarse_time >> 16) & 0x01;
+
+        (regs + 1)->addr = IMX185_COARSE_TIME_SHS1_ADDR_MID;
+        (regs + 1)->val = (coarse_time >> 8) & 0xff;
+
+        (regs + 2)->addr = IMX185_COARSE_TIME_SHS1_ADDR_LSB;
+        (regs + 2)->val = (coarse_time) & 0xff;
+
+}
+
+static inline void imx185_get_coarse_time_regs_shs2(imx185_reg *regs,
+                                u32 coarse_time)
+{
+        regs->addr = IMX185_COARSE_TIME_SHS2_ADDR_MSB;
+        regs->val = (coarse_time >> 16) & 0x01;
+
+        (regs + 1)->addr = IMX185_COARSE_TIME_SHS2_ADDR_MID;
+        (regs + 1)->val = (coarse_time >> 8) & 0xff;
+
+        (regs + 2)->addr = IMX185_COARSE_TIME_SHS2_ADDR_LSB;
+        (regs + 2)->val = (coarse_time) & 0xff;
+
+}
+
+static inline void imx185_get_gain_reg(imx185_reg *regs,
+                                u8 gain)
+{
+        regs->addr = IMX185_GAIN_ADDR;
+        regs->val = (gain) & 0xff;
+}
+
+static int test_mode;
+module_param(test_mode, int, 0644);
+
+static inline int imx185_read_reg(struct camera_common_data *s_data,
+                                u16 addr, u8 *val)
+{
+        struct imx185 *priv = (struct imx185 *)s_data->priv;
+
+        return regmap_read(priv->regmap, addr, (unsigned int *) val);
+}
+
+static int imx185_write_reg(struct camera_common_data *s_data,
+                                u16 addr, u8 val)
+{
+        int err;
+        struct imx185 *priv = (struct imx185 *)s_data->priv;
+
+        err = regmap_write(priv->regmap, addr, val);
+        if (err)
+                pr_err("%s:i2c write failed, 0x%x = %x\n",
+                        __func__, addr, val);
+
+        return err;
+}
+
+static int imx185_write_table(struct imx185 *priv,
+                                const imx185_reg table[])
+{
+        return regmap_util_write_table_8(priv->regmap,
+                                         table,
+                                         NULL, 0,
+                                         IMX185_TABLE_WAIT_MS,
+                                         IMX185_TABLE_END);
+}
+
+static int imx185_power_on(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct imx185 *priv = (struct imx185 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power on\n", __func__);
+        if (priv->pdata && priv->pdata->power_on) {
+                err = priv->pdata->power_on(pw);
+                if (err)
+                        pr_err("%s failed.\n", __func__);
+                else
+                        pw->state = SWITCH_ON;
+                return err;
+        }
+
+        /*exit reset mode: XCLR */
+        if (pw->reset_gpio) {
+                gpio_set_value(pw->reset_gpio, 0);
+                usleep_range(30, 50);
+                gpio_set_value(pw->reset_gpio, 1);
+                usleep_range(30, 50);
+        }
+
+        pw->state = SWITCH_ON;
+        return 0;
+
+}
+
+static int imx185_power_off(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct imx185 *priv = (struct imx185 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power off\n", __func__);
+
+        if (priv->pdata && priv->pdata->power_off) {
+                err = priv->pdata->power_off(pw);
+                if (!err)
+                        goto power_off_done;
+                else
+                        pr_err("%s failed.\n", __func__);
+                return err;
+        }
+        /* enter reset mode: XCLR */
+        usleep_range(1, 2);
+        if (pw->reset_gpio)
+                gpio_set_value(pw->reset_gpio, 0);
+
+power_off_done:
+        pw->state = SWITCH_OFF;
+
+        return 0;
+}
+
+static int imx185_power_get(struct imx185 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        struct camera_common_pdata *pdata = priv->pdata;
+        const char *mclk_name;
+        struct clk *parent;
+        int err = 0;
+
+        mclk_name = priv->pdata->mclk_name ?
+                    priv->pdata->mclk_name : "extperiph1";
+        pw->mclk = devm_clk_get(&priv->i2c_client->dev, mclk_name);
+        if (IS_ERR(pw->mclk)) {
+                dev_err(&priv->i2c_client->dev,
+                        "unable to get clock %s\n", mclk_name);
+                return PTR_ERR(pw->mclk);
+        }
+
+        parent = devm_clk_get(&priv->i2c_client->dev, "pllp_grtba");
+        if (IS_ERR(parent))
+                dev_err(&priv->i2c_client->dev, "devm_clk_get failed for pllp_grtba");
+        else
+                clk_set_parent(pw->mclk, parent);
+
+        pw->reset_gpio = pdata->reset_gpio;
+
+        pw->state = SWITCH_OFF;
+        return err;
+}
+
+static int imx185_set_coarse_time(struct imx185 *priv, s64 val);
+static int imx185_set_coarse_time_hdr(struct imx185 *priv, s64 val);
+static int imx185_set_gain(struct imx185 *priv, s64 val);
+static int imx185_set_frame_rate(struct imx185 *priv, s64 val);
+static int imx185_set_exposure(struct imx185 *priv, s64 val);
+
+static int imx185_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx185 *priv = (struct imx185 *)s_data->priv;
+        struct v4l2_ext_controls ctrls;
+        struct v4l2_ext_control control[3];
+        int err;
+
+        dev_dbg(&client->dev, "%s++ enable %d\n", __func__, enable);
+
+        if (!enable) {
+                err =  imx185_write_table(priv,
+                        mode_table[IMX185_MODE_STOP_STREAM]);
+
+                if (err)
+                        return err;
+
+                /* SW_RESET will have no ACK */
+                regmap_write(priv->regmap, IMX185_SW_RESET_ADDR, 0x01);
+
+                /* Wait for one frame to make sure sensor is set to
+                 * software standby in V-blank
+                 *
+                 * delay = frame length rows * Tline (10 us)
+                 */
+                usleep_range(priv->frame_length * 10,
+                        priv->frame_length * 10 + 1000);
+                return 0;
+        }
+        err = imx185_write_table(priv, mode_table[s_data->mode]);
+        if (err)
+                goto exit;
+
+        if (s_data->override_enable) {
+                /* write list of override regs for the asking gain, */
+                /* frame rate and exposure time    */
+                memset(&ctrls, 0, sizeof(ctrls));
+                ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(V4L2_CID_GAIN);
+                ctrls.count = 3;
+                ctrls.controls = control;
+
+                control[0].id = V4L2_CID_GAIN;
+                control[1].id = V4L2_CID_FRAME_RATE;
+                control[2].id = V4L2_CID_EXPOSURE;
+
+                err = v4l2_g_ext_ctrls(&priv->ctrl_handler, &ctrls);
+                if (err == 0) {
+                        err |= imx185_set_gain(priv, control[0].value64);
+                        if (err)
+                                dev_err(&client->dev,
+                                        "%s: error gain override\n", __func__);
+
+                        err |= imx185_set_frame_rate(priv, control[1].value64);
+                        if (err)
+                                dev_err(&client->dev,
+                                        "%s: error frame length override\n",
+                                        __func__);
+
+                        err |= imx185_set_exposure(priv, control[2].value64);
+                        if (err)
+                                dev_err(&client->dev,
+                                        "%s: error exposure override\n",
+                                        __func__);
+
+                } else {
+                        dev_err(&client->dev, "%s: faile to get overrides\n",
+                                __func__);
+                }
+        }
+
+        if (test_mode) {
+                err = imx185_write_table(priv,
+                        mode_table[IMX185_MODE_TEST_PATTERN]);
+                if (err)
+                        goto exit;
+        }
+
+        err = imx185_write_table(priv, mode_table[IMX185_MODE_START_STREAM]);
+        if (err)
+                goto exit;
+
+        return 0;
+exit:
+        dev_err(&client->dev, "%s: error setting stream\n", __func__);
+        return err;
+}
+
+static int imx185_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx185 *priv = (struct imx185 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        *status = pw->state == SWITCH_ON;
+        return 0;
+}
+
+static struct v4l2_subdev_video_ops imx185_subdev_video_ops = {
+        .s_stream       = imx185_s_stream,
+        .g_mbus_config  = camera_common_g_mbus_config,
+        .g_input_status = imx185_g_input_status,
+};
+
+static struct v4l2_subdev_core_ops imx185_subdev_core_ops = {
+        .s_power        = camera_common_s_power,
+};
+
+static int imx185_get_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        return camera_common_g_fmt(sd, &format->format);
+}
+
+static int imx185_set_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+        struct v4l2_subdev_format *format)
+{
+        int ret;
+
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+                ret = camera_common_try_fmt(sd, &format->format);
+        else
+                ret = camera_common_s_fmt(sd, &format->format);
+
+        return ret;
+}
+
+static struct v4l2_subdev_pad_ops imx185_subdev_pad_ops = {
+        .set_fmt = imx185_set_fmt,
+        .get_fmt = imx185_get_fmt,
+        .enum_mbus_code = camera_common_enum_mbus_code,
+        .enum_frame_size        = camera_common_enum_framesizes,
+        .enum_frame_interval    = camera_common_enum_frameintervals,
+};
+
+static struct v4l2_subdev_ops imx185_subdev_ops = {
+        .core   = &imx185_subdev_core_ops,
+        .video  = &imx185_subdev_video_ops,
+        .pad = &imx185_subdev_pad_ops,
+};
+
+const struct of_device_id imx185_of_match[] = {
+        { .compatible = "nvidia,imx185",},
+        { },
+};
+
+static struct camera_common_sensor_ops imx185_common_ops = {
+        .power_on = imx185_power_on,
+        .power_off = imx185_power_off,
+        .write_reg = imx185_write_reg,
+        .read_reg = imx185_read_reg,
+};
+
+static int imx185_set_group_hold(struct imx185 *priv, s32 val)
+{
+        int err;
+        int gh_en = switch_ctrl_qmenu[val];
+
+        priv->group_hold_prev = val;
+        if (gh_en == SWITCH_ON) {
+
+                err = imx185_write_reg(priv->s_data,
+                                       IMX185_GROUP_HOLD_ADDR, 0x1);
+                if (err)
+                        goto fail;
+        } else if (gh_en == SWITCH_OFF) {
+                err = imx185_write_reg(priv->s_data,
+                                       IMX185_GROUP_HOLD_ADDR, 0x0);
+                if (err)
+                        goto fail;
+        }
+        return 0;
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: Group hold control error\n", __func__);
+        return err;
+}
+
+static int imx185_set_gain(struct imx185 *priv, s64 val)
+{
+        imx185_reg reg_list[1];
+        int err;
+        s64 gain64;
+        u8 gain;
+
+        /* translate value */
+        gain64 = (s64)(val / FIXED_POINT_SCALING_FACTOR);
+        gain = (u8)(gain64 * 160 / 48);
+        dev_dbg(&priv->i2c_client->dev,
+                "%s:  gain reg: %d, db: %lld\n",  __func__, gain, gain64);
+
+        imx185_get_gain_reg(reg_list, gain);
+
+        err = imx185_write_reg(priv->s_data, reg_list[0].addr,
+                 reg_list[0].val);
+        if (err)
+                goto fail;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: GAIN control error\n", __func__);
+        return err;
+}
+
+static int imx185_set_frame_rate(struct imx185 *priv, s64 val)
+{
+        imx185_reg reg_list[3];
+        int err;
+        s64 frame_length;
+        struct camera_common_mode_info *mode = priv->pdata->mode_info;
+        struct camera_common_data *s_data = priv->s_data;
+        struct v4l2_control control;
+        int hdr_en;
+        int i = 0;
+
+        frame_length = mode[s_data->mode].pixel_clock *
+                FIXED_POINT_SCALING_FACTOR /
+                mode[s_data->mode].line_length / val;
+
+        priv->frame_length = (u32) frame_length;
+        if (priv->frame_length > IMX185_MAX_FRAME_LENGTH)
+                priv->frame_length = IMX185_MAX_FRAME_LENGTH;
+
+        dev_dbg(&priv->i2c_client->dev,
+                "%s: val: %lld, , frame_length: %d\n", __func__,
+                val, priv->frame_length);
+
+        imx185_get_frame_length_regs(reg_list, priv->frame_length);
+
+        for (i = 0; i < 3; i++) {
+                err = imx185_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        /* check hdr enable ctrl */
+        control.id = V4L2_CID_HDR_EN;
+        err = camera_common_g_ctrl(priv->s_data, &control);
+        if (err < 0) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return err;
+        }
+
+        hdr_en = switch_ctrl_qmenu[control.value];
+        if ((hdr_en == SWITCH_ON) && (priv->last_wdr_et_val != 0)) {
+                err = imx185_set_coarse_time_hdr(priv, priv->last_wdr_et_val);
+                if (err)
+                        dev_dbg(&priv->i2c_client->dev,
+                        "%s: error coarse time SHS1 SHS2 override\n", __func__);
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: FRAME_LENGTH control error\n", __func__);
+        return err;
+}
+
+static int imx185_set_exposure(struct imx185 *priv, s64 val)
+{
+        int err;
+        struct v4l2_control control;
+        int hdr_en;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %lld\n", __func__, val);
+
+        /* check hdr enable ctrl */
+        control.id = V4L2_CID_HDR_EN;
+        err = camera_common_g_ctrl(priv->s_data, &control);
+        if (err < 0) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return err;
+        }
+
+        hdr_en = switch_ctrl_qmenu[control.value];
+        if (hdr_en == SWITCH_ON) {
+                err = imx185_set_coarse_time_hdr(priv, val);
+                if (err)
+                        dev_dbg(&priv->i2c_client->dev,
+                        "%s: error coarse time SHS1 SHS2 override\n", __func__);
+        } else {
+                err = imx185_set_coarse_time(priv, val);
+                if (err)
+                        dev_dbg(&priv->i2c_client->dev,
+                        "%s: error coarse time SHS1 override\n", __func__);
+        }
+        return err;
+}
+
+static int imx185_set_coarse_time(struct imx185 *priv, s64 val)
+{
+        struct camera_common_mode_info *mode = priv->pdata->mode_info;
+        struct camera_common_data *s_data = priv->s_data;
+        imx185_reg reg_list[3];
+        int err;
+        u32 coarse_time_shs1;
+        u32 reg_shs1;
+        int i = 0;
+
+        coarse_time_shs1 = mode[s_data->mode].pixel_clock * val /
+                mode[s_data->mode].line_length / FIXED_POINT_SCALING_FACTOR;
+
+        if (priv->frame_length == 0)
+                priv->frame_length = IMX185_MIN_FRAME_LENGTH;
+
+        reg_shs1 = priv->frame_length - coarse_time_shs1 - 1;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: coarse1:%d, shs1:%d, FL:%d\n", __func__,
+                 coarse_time_shs1, reg_shs1, priv->frame_length);
+
+        imx185_get_coarse_time_regs_shs1(reg_list, reg_shs1);
+
+        for (i = 0; i < 3; i++) {
+                err = imx185_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: set coarse time error\n", __func__);
+        return err;
+}
+
+static int imx185_set_coarse_time_hdr(struct imx185 *priv, s64 val)
+{
+        struct camera_common_mode_info *mode = priv->pdata->mode_info;
+        struct camera_common_data *s_data = priv->s_data;
+        imx185_reg reg_list_shs1[3];
+        imx185_reg reg_list_shs2[3];
+        u32 coarse_time_shs1;
+        u32 coarse_time_shs2;
+        u32 reg_shs1;
+        u32 reg_shs2;
+        int err;
+        int i = 0;
+
+        if (priv->frame_length == 0)
+                priv->frame_length = IMX185_MIN_FRAME_LENGTH;
+
+        priv->last_wdr_et_val = val;
+
+        /*WDR, update SHS1 as short ET, and SHS2 is 16x of short*/
+        coarse_time_shs1 = mode[s_data->mode].pixel_clock * val /
+                mode[s_data->mode].line_length /
+                FIXED_POINT_SCALING_FACTOR / 16;
+        if (coarse_time_shs1 < IMX185_MIN_SHS1_1080P_HDR)
+                coarse_time_shs1 = IMX185_MIN_SHS1_1080P_HDR;
+        if (coarse_time_shs1 > IMX185_MAX_SHS1_1080P_HDR)
+                coarse_time_shs1 = IMX185_MAX_SHS1_1080P_HDR;
+
+        coarse_time_shs2 = (coarse_time_shs1 - IMX185_MIN_SHS1_1080P_HDR) * 16 +
+                                IMX185_MIN_SHS2_1080P_HDR;
+
+        reg_shs1 = priv->frame_length - coarse_time_shs1 - 1;
+        reg_shs2 = priv->frame_length - coarse_time_shs2 - 1;
+
+        imx185_get_coarse_time_regs_shs1(reg_list_shs1, reg_shs1);
+        imx185_get_coarse_time_regs_shs2(reg_list_shs2, reg_shs2);
+
+        dev_dbg(&priv->i2c_client->dev,
+                "%s: coarse1:%d, shs1:%d, coarse2:%d, shs2: %d, FL:%d\n",
+                __func__,
+                 coarse_time_shs1, reg_shs1,
+                 coarse_time_shs2, reg_shs2,
+                 priv->frame_length);
+
+        for (i = 0; i < 3; i++) {
+                err = imx185_write_reg(priv->s_data, reg_list_shs1[i].addr,
+                         reg_list_shs1[i].val);
+                if (err)
+                        goto fail;
+
+                err = imx185_write_reg(priv->s_data, reg_list_shs2[i].addr,
+                         reg_list_shs2[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: set WDR coarse time error\n", __func__);
+        return err;
+}
+
+static int imx185_fuse_id_setup(struct imx185 *priv)
+{
+        int err;
+        int i;
+        struct i2c_client *client = v4l2_get_subdevdata(priv->subdev);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+
+        struct v4l2_ctrl *ctrl;
+        u8 fuse_id[IMX185_FUSE_ID_SIZE];
+        u8 bak = 0;
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        for (i = 0; i < IMX185_FUSE_ID_SIZE; i++) {
+                err |= imx185_read_reg(s_data,
+                        IMX185_FUSE_ID_ADDR + i, &bak);
+                if (!err)
+                        fuse_id[i] = bak;
+                else {
+                        pr_err("%s: can not read fuse id\n", __func__);
+                        return -EINVAL;
+                }
+        }
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_FUSE_ID);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return -EINVAL;
+        }
+
+        for (i = 0; i < IMX185_FUSE_ID_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x",
+                        fuse_id[i]);
+        ctrl->p_cur.p_char = ctrl->p_new.p_char;
+        dev_info(&client->dev, "%s,  fuse id: %s\n", __func__,
+                ctrl->p_cur.p_char);
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int imx185_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct imx185 *priv =
+                container_of(ctrl->handler, struct imx185, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+
+        default:
+                        pr_err("%s: unknown ctrl id.\n", __func__);
+                        return -EINVAL;
+        }
+
+        return err;
+}
+
+static int imx185_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct imx185 *priv =
+                container_of(ctrl->handler, struct imx185, ctrl_handler);
+        struct camera_common_data       *s_data = priv->s_data;
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_GAIN:
+                err = imx185_set_gain(priv, *ctrl->p_new.p_s64);
+                break;
+        case V4L2_CID_EXPOSURE:
+                err = imx185_set_exposure(priv, *ctrl->p_new.p_s64);
+                break;
+        case V4L2_CID_FRAME_RATE:
+                err = imx185_set_frame_rate(priv, *ctrl->p_new.p_s64);
+                break;
+        case V4L2_CID_GROUP_HOLD:
+                err = imx185_set_group_hold(priv, ctrl->val);
+                break;
+        case V4L2_CID_HDR_EN:
+                break;
+        case V4L2_CID_SENSOR_MODE_ID:
+                s_data->sensor_mode_id = (int) (*ctrl->p_new.p_s64);
+                break;
+        default:
+                pr_err("%s: unknown ctrl id.\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static int imx185_ctrls_init(struct imx185 *priv)
+{
+        struct i2c_client *client = priv->i2c_client;
+        struct v4l2_ctrl *ctrl;
+        int num_ctrls;
+        int err;
+        int i;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        num_ctrls = ARRAY_SIZE(ctrl_config_list);
+        v4l2_ctrl_handler_init(&priv->ctrl_handler, num_ctrls);
+
+        for (i = 0; i < num_ctrls; i++) {
+                ctrl = v4l2_ctrl_new_custom(&priv->ctrl_handler,
+                        &ctrl_config_list[i], NULL);
+                if (ctrl == NULL) {
+                        dev_err(&client->dev, "Failed to init %s ctrl\n",
+                                ctrl_config_list[i].name);
+                        continue;
+                }
+
+                if (ctrl_config_list[i].type == V4L2_CTRL_TYPE_STRING &&
+                        ctrl_config_list[i].flags & V4L2_CTRL_FLAG_READ_ONLY) {
+                        ctrl->p_new.p_char = devm_kzalloc(&client->dev,
+                                ctrl_config_list[i].max + 1, GFP_KERNEL);
+                }
+                priv->ctrls[i] = ctrl;
+        }
+
+        priv->numctrls = num_ctrls;
+        priv->subdev->ctrl_handler = &priv->ctrl_handler;
+        if (priv->ctrl_handler.error) {
+                dev_err(&client->dev, "Error %d adding controls\n",
+                        priv->ctrl_handler.error);
+                err = priv->ctrl_handler.error;
+                goto error;
+        }
+
+        err = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d setting default controls\n", err);
+                goto error;
+        }
+
+        err = imx185_fuse_id_setup(priv);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d reading fuse id data\n", err);
+                goto error;
+        }
+
+        return 0;
+
+error:
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        return err;
+}
+
+MODULE_DEVICE_TABLE(of, imx185_of_match);
+
+static struct camera_common_pdata *imx185_parse_dt(struct imx185 *priv,
+                                struct i2c_client *client,
+                                struct camera_common_data *s_data)
+{
+        struct device_node *np = client->dev.of_node;
+        struct camera_common_pdata *board_priv_pdata;
+        const struct of_device_id *match;
+        int err;
+        const char *str;
+
+        if (!np)
+                return NULL;
+
+        match = of_match_device(imx185_of_match, &client->dev);
+        if (!match) {
+                dev_err(&client->dev, "Failed to find matching dt id\n");
+                return NULL;
+        }
+
+        err = of_property_read_string(np, "use_sensor_mode_id", &str);
+        if (!err) {
+                if (!strcmp(str, "true"))
+                        s_data->use_sensor_mode_id = true;
+                else
+                        s_data->use_sensor_mode_id = false;
+        }
+        board_priv_pdata = devm_kzalloc(&client->dev,
+                           sizeof(*board_priv_pdata), GFP_KERNEL);
+
+        err = of_property_read_string(np, "mclk",
+                                      &board_priv_pdata->mclk_name);
+        if (err)
+                dev_err(&client->dev, "mclk not in DT\n");
+
+        board_priv_pdata->reset_gpio = of_get_named_gpio(np, "reset-gpios", 0);
+        if (err) {
+                dev_err(&client->dev, "reset-gpios not found %d\n", err);
+                board_priv_pdata->reset_gpio = 0;
+        }
+
+        err = camera_common_parse_sensor_mode(client, board_priv_pdata);
+        if (err)
+                dev_err(&client->dev, "Failed to load mode info %d\n", err);
+
+        return board_priv_pdata;
+}
+
+static int imx185_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        dev_dbg(&client->dev, "%s:\n", __func__);
+
+        return 0;
+}
+
+static const struct v4l2_subdev_internal_ops imx185_subdev_internal_ops = {
+        .open = imx185_open,
+};
+
+static const struct media_entity_operations imx185_media_ops = {
+        .link_validate = v4l2_subdev_link_validate,
+};
+
+static int imx185_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct camera_common_data *common_data;
+        struct imx185 *priv;
+        char debugfs_name[10];
+        int err;
+
+        dev_info(&client->dev, "[IMX185]: probing v4l2 sensor at addr 0x%0x.\n",
+                client->addr);
+
+        if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
+                return -EINVAL;
+
+        common_data = devm_kzalloc(&client->dev,
+                            sizeof(struct camera_common_data), GFP_KERNEL);
+
+        priv = devm_kzalloc(&client->dev,
+                            sizeof(struct imx185) + sizeof(struct v4l2_ctrl *) *
+                            ARRAY_SIZE(ctrl_config_list),
+                            GFP_KERNEL);
+        if (!priv) {
+                dev_err(&client->dev, "unable to allocate memory!\n");
+                return -ENOMEM;
+        }
+
+        priv->regmap = devm_regmap_init_i2c(client, &sensor_regmap_config);
+        if (IS_ERR(priv->regmap)) {
+                dev_err(&client->dev,
+                        "regmap init failed: %ld\n", PTR_ERR(priv->regmap));
+                return -ENODEV;
+        }
+
+        if (client->dev.of_node)
+                priv->pdata = imx185_parse_dt(priv, client, common_data);
+        if (!priv->pdata) {
+                dev_err(&client->dev, "unable to get platform data\n");
+                return -EFAULT;
+        }
+
+        common_data->ops = &imx185_common_ops;
+        common_data->ctrl_handler = &priv->ctrl_handler;
+        common_data->i2c_client = client;
+        common_data->frmfmt = &imx185_frmfmt[0];
+        common_data->colorfmt = camera_common_find_datafmt(
+                                          IMX185_DEFAULT_DATAFMT);
+        common_data->power = &priv->power;
+        common_data->ctrls = priv->ctrls;
+        common_data->priv = (void *)priv;
+        common_data->numctrls = ARRAY_SIZE(ctrl_config_list);
+        common_data->numfmts = ARRAY_SIZE(imx185_frmfmt);
+        common_data->def_mode = IMX185_DEFAULT_MODE;
+        common_data->def_width = IMX185_DEFAULT_WIDTH;
+        common_data->def_height = IMX185_DEFAULT_HEIGHT;
+        common_data->fmt_width = common_data->def_width;
+        common_data->fmt_height = common_data->def_height;
+        common_data->def_clk_freq = IMX185_DEFAULT_CLK_FREQ;
+
+        priv->i2c_client = client;
+        priv->s_data = common_data;
+        priv->subdev = &common_data->subdev;
+        priv->subdev->dev = &client->dev;
+        priv->s_data->dev = &client->dev;
+        priv->last_wdr_et_val = 0;
+
+        err = imx185_power_get(priv);
+        if (err)
+                return err;
+
+        err = camera_common_parse_ports(client, common_data);
+        if (err) {
+                dev_err(&client->dev, "Failed to find port info\n");
+                return err;
+        }
+        sprintf(debugfs_name, "imx185_%c", common_data->csi_port + 'a');
+        dev_dbg(&client->dev, "%s: name %s\n", __func__, debugfs_name);
+
+        camera_common_create_debugfs(common_data, debugfs_name);
+
+        v4l2_i2c_subdev_init(priv->subdev, client, &imx185_subdev_ops);
+
+        err = imx185_ctrls_init(priv);
+        if (err)
+                return err;
+
+        priv->subdev->internal_ops = &imx185_subdev_internal_ops;
+        priv->subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+                     V4L2_SUBDEV_FL_HAS_EVENTS;
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+        priv->subdev->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+        priv->subdev->entity.ops = &imx185_media_ops;
+        err = media_entity_init(&priv->subdev->entity, 1, &priv->pad, 0);
+        if (err < 0) {
+                dev_err(&client->dev, "unable to init media entity\n");
+                return err;
+        }
+#endif
+
+        err = v4l2_async_register_subdev(priv->subdev);
+        if (err)
+                return err;
+
+        dev_info(&client->dev, "Detected IMX185 sensor\n");
+
+        return 0;
+}
+
+static int
+imx185_remove(struct i2c_client *client)
+{
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx185 *priv = (struct imx185 *)s_data->priv;
+
+        v4l2_async_unregister_subdev(priv->subdev);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev->entity);
+#endif
+
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        camera_common_remove_debugfs(s_data);
+        return 0;
+}
+
+static const struct i2c_device_id imx185_id[] = {
+        { "imx185", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, imx185_id);
+
+static struct i2c_driver imx185_i2c_driver = {
+        .driver = {
+                .name = "imx185",
+                .owner = THIS_MODULE,
+                .of_match_table = of_match_ptr(imx185_of_match),
+        },
+        .probe = imx185_probe,
+        .remove = imx185_remove,
+        .id_table = imx185_id,
+};
+
+module_i2c_driver(imx185_i2c_driver);
+
+MODULE_DESCRIPTION("Media Controller driver for Sony IMX185");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx185_mode_tbls.h b/drivers/media/i2c/imx185_mode_tbls.h
new file mode 100644
index 000000000..bf7042ee5
--- /dev/null
+++ b/drivers/media/i2c/imx185_mode_tbls.h
@@ -0,0 +1,1009 @@
+/*
+ * imx185_mode_tbls.h - imx185 sensor mode tables
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __IMX185_I2C_TABLES__
+#define __IMX185_I2C_TABLES__
+
+#include <media/camera_common.h>
+#include <linux/miscdevice.h>
+
+#define IMX185_TABLE_WAIT_MS    0
+#define IMX185_TABLE_END        1
+#define IMX185_MAX_RETRIES      3
+#define IMX185_WAIT_MS_STOP     1
+#define IMX185_WAIT_MS_START    30
+#define IMX185_WAIT_MS_STREAM   210
+#define IMX185_GAIN_TABLE_SIZE 255
+
+/* #define INIT_ET_INSETTING 1 */
+
+#define imx185_reg struct reg_8
+
+static imx185_reg imx185_start[] = {
+        {0x3000, 0x00 },
+        {IMX185_TABLE_WAIT_MS, IMX185_WAIT_MS_START},
+        {0x3002, 0x00},
+        {0x3049, 0x0a},
+        {IMX185_TABLE_WAIT_MS, IMX185_WAIT_MS_STREAM},
+        { IMX185_TABLE_END, 0x00 }
+};
+
+static imx185_reg imx185_stop[] = {
+        {0x3000, 0x01 },
+        {IMX185_TABLE_WAIT_MS, IMX185_WAIT_MS_STOP},
+        {IMX185_TABLE_END, 0x00 }
+};
+
+static imx185_reg tp_colorbars[] = {
+        {0x300A, 0x00},/*BLC for PG*/
+        {0x300E, 0x00},
+        {0x3089, 0x00},
+        {0x308C, 0x13},
+        /*
+        * bit 0: PG mode enable
+        * bit 1: Back Ground Transient:
+        * bit [4-7]: PG mode setting, Set at 0h to Fh, suggest 1 or 5
+        * raw12 max output FFEh
+        */
+        {IMX185_TABLE_WAIT_MS, IMX185_WAIT_MS_STOP},
+        {IMX185_TABLE_END, 0x00}
+};
+
+static  imx185_reg imx185_1920x1080_crop_60fps[] = {
+        {0x3002, 0x01},
+        {0x3005, 0x01},
+        {0x3006, 0x00},
+        {0x3007, 0x50},
+        {0x3009, 0x01},
+        {0x300a, 0xf0},
+        {0x300f, 0x01},
+        {0x3018, 0x65},
+        {0x3019, 0x04},
+        {0x301b, 0x4c},
+        {0x301c, 0x04},
+        {0x301d, 0x08},
+        {0x301e, 0x02},
+
+        {0x3036, 0x06},
+        {0x3038, 0x08},
+        {0x3039, 0x00},
+        {0x303a, 0x40},
+        {0x303b, 0x04},
+        {0x303c, 0x0c},
+        {0x303d, 0x00},
+        {0x303e, 0x7c},
+        {0x303f, 0x07},
+
+        {0x3044, 0xe1},
+        {0x3048, 0x33},
+
+        {0x305C, 0x20},
+        {0x305D, 0x00},
+        {0x305E, 0x18},
+        {0x305F, 0x00},
+        {0x3063, 0x74},
+
+        {0x3084, 0x0f},
+        {0x3086, 0x10},
+        {0x30A1, 0x44},
+        {0x30cf, 0xe1},
+        {0x30d0, 0x29},
+        {0x30d2, 0x9b},
+        {0x30d3, 0x01},
+
+        {0x311d, 0x0a},
+        {0x3123, 0x0f},
+        {0x3126, 0xdf},
+        {0x3147, 0x87},
+        {0x31e0, 0x01},
+        {0x31e1, 0x9e},
+        {0x31e2, 0x01},
+        {0x31e5, 0x05},
+        {0x31e6, 0x05},
+        {0x31e7, 0x3a},
+        {0x31e8, 0x3a},
+
+        {0x3203, 0xc8},
+        {0x3207, 0x54},
+        {0x3213, 0x16},
+        {0x3215, 0xf6},
+        {0x321a, 0x14},
+        {0x321b, 0x51},
+        {0x3229, 0xe7},
+        {0x322a, 0xf0},
+        {0x322b, 0x10},
+        {0x3231, 0xe7},
+        {0x3232, 0xf0},
+        {0x3233, 0x10},
+        {0x323c, 0xe8},
+        {0x323d, 0x70},
+        {0x3243, 0x08},
+        {0x3244, 0xe1},
+        {0x3245, 0x10},
+        {0x3247, 0xe7},
+        {0x3248, 0x60},
+        {0x3249, 0x1e},
+        {0x324b, 0x00},
+        {0x324c, 0x41},
+        {0x3250, 0x30},
+        {0x3251, 0x0a},
+        {0x3252, 0xff},
+        {0x3253, 0xff},
+        {0x3254, 0xff},
+        {0x3255, 0x02},
+        {0x3257, 0xf0},
+        {0x325a, 0xa6},
+        {0x325d, 0x14},
+        {0x325e, 0x51},
+        {0x3260, 0x00},
+        {0x3261, 0x61},
+        {0x3266, 0x30},
+        {0x3267, 0x05},
+        {0x3275, 0xe7},
+        {0x3281, 0xea},
+        {0x3282, 0x70},
+        {0x3285, 0xff},
+        {0x328a, 0xf0},
+        {0x328d, 0xb6},
+        {0x328e, 0x40},
+        {0x3290, 0x42},
+        {0x3291, 0x51},
+        {0x3292, 0x1e},
+        {0x3294, 0xc4},
+        {0x3295, 0x20},
+        {0x3297, 0x50},
+        {0x3298, 0x31},
+        {0x3299, 0x1f},
+        {0x329b, 0xc0},
+        {0x329c, 0x60},
+        {0x329e, 0x4c},
+        {0x329f, 0x71},
+        {0x32a0, 0x1f},
+        {0x32a2, 0xb6},
+        {0x32a3, 0xc0},
+        {0x32a4, 0x0b},
+        {0x32a9, 0x24},
+        {0x32aa, 0x41},
+        {0x32b0, 0x25},
+        {0x32b1, 0x51},
+        {0x32b7, 0x1c},
+        {0x32b8, 0xc1},
+        {0x32b9, 0x12},
+        {0x32be, 0x1d},
+        {0x32bf, 0xd1},
+        {0x32c0, 0x12},
+        {0x32c2, 0xa8},
+        {0x32c3, 0xc0},
+        {0x32c4, 0x0a},
+        {0x32c5, 0x1e},
+        {0x32c6, 0x21},
+        {0x32c9, 0xb0},
+        {0x32ca, 0x40},
+        {0x32cc, 0x26},
+        {0x32cd, 0xa1},
+        {0x32d0, 0xb6},
+        {0x32d1, 0xc0},
+        {0x32d2, 0x0b},
+        {0x32d4, 0xe2},
+        {0x32d5, 0x40},
+        {0x32d8, 0x4e},
+        {0x32d9, 0xa1},
+        {0x32ec, 0xf0},
+
+        {0x3303, 0x00},
+        {0x3305, 0x03},
+        {0x3314, 0x04},
+        {0x3315, 0x01},
+        {0x3316, 0x04},
+        {0x3317, 0x04},
+        {0x3318, 0x38},
+        {0x3319, 0x04},
+        {0x332c, 0x40},
+        {0x332d, 0x20},
+        {0x332e, 0x03},
+        {0x333e, 0x0c},
+        {0x333f, 0x0c},
+        {0x3340, 0x03},
+        {0x3341, 0x20},
+        {0x3342, 0x25},
+        {0x3343, 0x68},
+        {0x3344, 0x20},
+        {0x3345, 0x40},
+        {0x3346, 0x28},
+        {0x3347, 0x20},
+        {0x3348, 0x18},
+        {0x3349, 0x78},
+        {0x334a, 0x28},
+        {0x334e, 0xb4},
+        {0x334f, 0x01},
+#ifdef INIT_ET_INSETTING
+        {0x3020, 0xe1},
+        {0x3021, 0x04},
+#endif
+        {IMX185_TABLE_END, 0x00}
+};
+
+static  imx185_reg imx185_1920x1080_crop_30fps[] = {
+        {0x3002, 0x01},
+        {0x3005, 0x01},
+        {0x3006, 0x00},
+        {0x3007, 0x50},
+        {0x3009, 0x02},
+        {0x300a, 0xf0},
+        {0x300f, 0x01},
+        {0x3018, 0x65},
+        {0x3019, 0x04},
+        {0x301b, 0x89},
+        {0x301c, 0x08},
+        {0x301d, 0x08},
+        {0x301e, 0x02},
+
+        {0x3036, 0x06},
+        {0x3038, 0x08},
+        {0x3039, 0x00},
+        {0x303a, 0x40},
+        {0x303b, 0x04},
+        {0x303c, 0x0c},
+        {0x303d, 0x00},
+        {0x303e, 0x7c},
+        {0x303f, 0x07},
+
+        {0x3048, 0x33},
+
+        {0x305C, 0x20},
+        {0x305D, 0x00},
+        {0x305E, 0x18},
+        {0x305F, 0x00},
+        {0x3063, 0x74},
+
+        {0x3084, 0x0f},
+        {0x3086, 0x10},
+        {0x30cf, 0xe1},
+        {0x30d0, 0x29},
+        {0x30d2, 0x9b},
+        {0x30d3, 0x01},
+
+        {0x311d, 0x0a},
+        {0x3123, 0x0f},
+        {0x3126, 0xdf},
+        {0x3147, 0x87},
+        {0x31e0, 0x01},
+        {0x31e1, 0x9e},
+        {0x31e2, 0x01},
+        {0x31e5, 0x05},
+        {0x31e6, 0x05},
+        {0x31e7, 0x3a},
+        {0x31e8, 0x3a},
+
+        {0x3203, 0xc8},
+        {0x3207, 0x54},
+        {0x3213, 0x16},
+        {0x3215, 0xf6},
+        {0x321a, 0x14},
+        {0x321b, 0x51},
+        {0x3229, 0xe7},
+        {0x322a, 0xf0},
+        {0x322b, 0x10},
+        {0x3231, 0xe7},
+        {0x3232, 0xf0},
+        {0x3233, 0x10},
+        {0x323c, 0xe8},
+        {0x323d, 0x70},
+        {0x3243, 0x08},
+        {0x3244, 0xe1},
+        {0x3245, 0x10},
+        {0x3247, 0xe7},
+        {0x3248, 0x60},
+        {0x3249, 0x1e},
+        {0x324b, 0x00},
+        {0x324c, 0x41},
+        {0x3250, 0x30},
+        {0x3251, 0x0a},
+        {0x3252, 0xff},
+        {0x3253, 0xff},
+        {0x3254, 0xff},
+        {0x3255, 0x02},
+        {0x3257, 0xf0},
+        {0x325a, 0xa6},
+        {0x325d, 0x14},
+        {0x325e, 0x51},
+        {0x3260, 0x00},
+        {0x3261, 0x61},
+        {0x3266, 0x30},
+        {0x3267, 0x05},
+        {0x3275, 0xe7},
+        {0x3281, 0xea},
+        {0x3282, 0x70},
+        {0x3285, 0xff},
+        {0x328a, 0xf0},
+        {0x328d, 0xb6},
+        {0x328e, 0x40},
+        {0x3290, 0x42},
+        {0x3291, 0x51},
+        {0x3292, 0x1e},
+        {0x3294, 0xc4},
+        {0x3295, 0x20},
+        {0x3297, 0x50},
+        {0x3298, 0x31},
+        {0x3299, 0x1f},
+        {0x329b, 0xc0},
+        {0x329c, 0x60},
+        {0x329e, 0x4c},
+        {0x329f, 0x71},
+        {0x32a0, 0x1f},
+        {0x32a2, 0xb6},
+        {0x32a3, 0xc0},
+        {0x32a4, 0x0b},
+        {0x32a9, 0x24},
+        {0x32aa, 0x41},
+        {0x32b0, 0x25},
+        {0x32b1, 0x51},
+        {0x32b7, 0x1c},
+        {0x32b8, 0xc1},
+        {0x32b9, 0x12},
+        {0x32be, 0x1d},
+        {0x32bf, 0xd1},
+        {0x32c0, 0x12},
+        {0x32c2, 0xa8},
+        {0x32c3, 0xc0},
+        {0x32c4, 0x0a},
+        {0x32c5, 0x1e},
+        {0x32c6, 0x21},
+        {0x32c9, 0xb0},
+        {0x32ca, 0x40},
+        {0x32cc, 0x26},
+        {0x32cd, 0xa1},
+        {0x32d0, 0xb6},
+        {0x32d1, 0xc0},
+        {0x32d2, 0x0b},
+        {0x32d4, 0xe2},
+        {0x32d5, 0x40},
+        {0x32d8, 0x4e},
+        {0x32d9, 0xa1},
+        {0x32ec, 0xf0},
+
+        {0x3303, 0x10},
+        {0x3305, 0x03},
+        {0x3314, 0x04},
+        {0x3315, 0x01},
+        {0x3316, 0x04},
+        {0x3317, 0x04},
+        {0x3318, 0x38},
+        {0x3319, 0x04},
+        {0x332c, 0x30},
+        {0x332d, 0x20},
+        {0x332e, 0x03},
+        {0x333e, 0x0c},
+        {0x333f, 0x0c},
+        {0x3340, 0x03},
+        {0x3341, 0x20},
+        {0x3342, 0x25},
+        {0x3343, 0x58},
+        {0x3344, 0x10},
+        {0x3345, 0x30},
+        {0x3346, 0x18},
+        {0x3347, 0x10},
+        {0x3348, 0x10},
+        {0x3349, 0x48},
+        {0x334a, 0x28},
+        {0x334e, 0xb4},
+        {0x334f, 0x01},
+#ifdef INIT_ET_INSETTING
+        {0x3020, 0xe1},
+        {0x3021, 0x04},
+#endif
+        {IMX185_TABLE_END, 0x00}
+};
+
+static imx185_reg imx185_1920x1080_hdr_crop_30fps[] = {
+        {0x3002, 0x01},
+        {0x3005, 0x01},
+        {0x3006, 0x00},
+        {0x3007, 0x50},
+        {0x3009, 0x02},
+        {0x300a, 0xf0},
+
+        {0x300c, 0x02},
+        {0x300f, 0x01},
+        {0x3010, 0x38},
+        {0x3011, 0x00},
+        {0x3012, 0x0f},
+        {0x3013, 0x00},
+
+        {0x3018, 0x65},
+        {0x3019, 0x04},
+        {0x301b, 0x98},
+        {0x301c, 0x08},
+        {0x301d, 0x08},
+        {0x301e, 0x02},
+
+        {0x3036, 0x06},
+        {0x3038, 0x08},
+        {0x3039, 0x00},
+        {0x303a, 0x40},
+        {0x303b, 0x04},
+        {0x303c, 0x0c},
+        {0x303d, 0x00},
+        {0x303e, 0x7c},
+        {0x303f, 0x07},
+
+        {0x3044, 0xe1},
+        {0x3048, 0x33},
+#ifdef INIT_ET_INSETTING
+        {0x3020, 0x1F},/*SHS1 1055, coarse 69*/
+        {0x3021, 0x04},
+        {0x3022, 0x00},
+        {0x3023, 0x12},/*SHS2 18, coarse 1106*/
+        {0x3024, 0x00},
+        {0x3025, 0x00},
+#endif
+        {0x3056, 0xc9},
+        {0x3057, 0x64},
+
+        {0x305C, 0x20},
+        {0x305D, 0x00},
+        {0x305E, 0x18},
+        {0x305F, 0x00},
+        {0x3063, 0x74},
+
+        {0x3065, 0x00},
+
+        {0x3084, 0x0f},
+        {0x3086, 0x10},
+        {0x30cf, 0xe1},
+        {0x30d0, 0x29},
+        {0x30d2, 0x9b},
+        {0x30d3, 0x01},
+
+        {0x311d, 0x0a},
+        {0x3123, 0x0f},
+        {0x3126, 0xdf},
+        {0x3147, 0x87},
+        {0x31e0, 0x01},
+        {0x31e1, 0x9e},
+        {0x31e2, 0x01},
+        {0x31e5, 0x05},
+        {0x31e6, 0x05},
+        {0x31e7, 0x3a},
+        {0x31e8, 0x3a},
+
+        {0x3203, 0xc8},
+        {0x3207, 0x54},
+        {0x3213, 0x16},
+        {0x3215, 0xf6},
+        {0x321a, 0x14},
+        {0x321b, 0x51},
+        {0x3229, 0xe7},
+        {0x322a, 0xf0},
+        {0x322b, 0x10},
+        {0x3231, 0xe7},
+        {0x3232, 0xf0},
+        {0x3233, 0x10},
+        {0x323c, 0xe8},
+        {0x323d, 0x70},
+        {0x3243, 0x08},
+        {0x3244, 0xe1},
+        {0x3245, 0x10},
+        {0x3247, 0xe7},
+        {0x3248, 0x60},
+        {0x3249, 0x1e},
+        {0x324b, 0x00},
+        {0x324c, 0x41},
+        {0x3250, 0x30},
+        {0x3251, 0x0a},
+        {0x3252, 0xff},
+        {0x3253, 0xff},
+        {0x3254, 0xff},
+        {0x3255, 0x02},
+        {0x3257, 0xf0},
+        {0x325a, 0xa6},
+        {0x325d, 0x14},
+        {0x325e, 0x51},
+        {0x3260, 0x00},
+        {0x3261, 0x61},
+        {0x3266, 0x30},
+        {0x3267, 0x05},
+        {0x3275, 0xe7},
+        {0x3281, 0xea},
+        {0x3282, 0x70},
+        {0x3285, 0xff},
+        {0x328a, 0xf0},
+        {0x328d, 0xb6},
+        {0x328e, 0x40},
+        {0x3290, 0x42},
+        {0x3291, 0x51},
+        {0x3292, 0x1e},
+        {0x3294, 0xc4},
+        {0x3295, 0x20},
+        {0x3297, 0x50},
+        {0x3298, 0x31},
+        {0x3299, 0x1f},
+        {0x329b, 0xc0},
+        {0x329c, 0x60},
+        {0x329e, 0x4c},
+        {0x329f, 0x71},
+        {0x32a0, 0x1f},
+        {0x32a2, 0xb6},
+        {0x32a3, 0xc0},
+        {0x32a4, 0x0b},
+        {0x32a9, 0x24},
+        {0x32aa, 0x41},
+        {0x32b0, 0x25},
+        {0x32b1, 0x51},
+        {0x32b7, 0x1c},
+        {0x32b8, 0xc1},
+        {0x32b9, 0x12},
+        {0x32be, 0x1d},
+        {0x32bf, 0xd1},
+        {0x32c0, 0x12},
+        {0x32c2, 0xa8},
+        {0x32c3, 0xc0},
+        {0x32c4, 0x0a},
+        {0x32c5, 0x1e},
+        {0x32c6, 0x21},
+        {0x32c9, 0xb0},
+        {0x32ca, 0x40},
+        {0x32cc, 0x26},
+        {0x32cd, 0xa1},
+        {0x32d0, 0xb6},
+        {0x32d1, 0xc0},
+        {0x32d2, 0x0b},
+        {0x32d4, 0xe2},
+        {0x32d5, 0x40},
+        {0x32d8, 0x4e},
+        {0x32d9, 0xa1},
+        {0x32ec, 0xf0},
+
+        {0x3303, 0x10},
+        {0x3305, 0x03},
+        {0x3314, 0x04},
+        {0x3315, 0x01},
+        {0x3316, 0x04},
+        {0x3317, 0x04},
+        {0x3318, 0x38},
+        {0x3319, 0x04},
+        {0x332c, 0x30},
+        {0x332d, 0x20},
+        {0x332e, 0x03},
+        {0x333e, 0x0c},
+        {0x333f, 0x0c},
+        {0x3340, 0x03},
+
+        {0x3341, 0x20},
+        {0x3342, 0x25},
+        {0x3343, 0x58},
+        {0x3344, 0x10},
+        {0x3345, 0x30},
+        {0x3346, 0x18},
+        {0x3347, 0x10},
+        {0x3348, 0x10},
+        {0x3349, 0x48},
+        {0x334a, 0x28},
+        {0x334e, 0xb4},
+        {0x334f, 0x01},
+
+#ifdef INIT_ET_INSETTING
+        {0x3020, 0x1F},
+        {0x3021, 0x04},
+        {0x3022, 0x00},
+        {0x3023, 0x12},
+        {0x3024, 0x00},
+        {0x3025, 0x00},
+#endif
+        {0x300C, 0x02},
+        {0x300F, 0x05},
+        {0x3010, 0x38},
+        {0x3012, 0x0F},
+        {0x3084, 0x0F},
+        {0x3065, 0x00},
+        {IMX185_TABLE_END, 0x00}
+};
+
+static imx185_reg imx185_1920x1080_crop_10bit_60fps[] = {
+        {0x3002, 0x01},
+        {0x3005, 0x00},/*10BIT*/
+        {0x3006, 0x00},
+        {0x3007, 0x50},
+        {0x3009, 0x01},
+        {0x300a, 0x3c},/*10BIT*/
+        {0x300f, 0x01},
+        {0x3018, 0x65},
+        {0x3019, 0x04},
+        {0x301b, 0x4c},
+        {0x301c, 0x04},
+        {0x301d, 0x08},
+        {0x301e, 0x02},
+
+        {0x3036, 0x06},
+        {0x3038, 0x08},
+        {0x3039, 0x00},
+        {0x303a, 0x40},
+        {0x303b, 0x04},
+        {0x303c, 0x0c},
+        {0x303d, 0x00},
+        {0x303e, 0x7c},
+        {0x303f, 0x07},
+
+        {0x3044, 0xe1},
+        {0x3048, 0x33},
+
+        {0x305C, 0x20},
+        {0x305D, 0x00},
+        {0x305E, 0x18},
+        {0x305F, 0x00},
+        {0x3063, 0x74},
+
+        {0x3084, 0x0f},
+        {0x3086, 0x10},
+        {0x30A1, 0x44},
+        {0x30cf, 0xe1},
+        {0x30d0, 0x29},
+        {0x30d2, 0x9b},
+        {0x30d3, 0x01},
+
+        {0x311d, 0x0a},
+        {0x3123, 0x0f},
+        {0x3126, 0xdf},
+        {0x3147, 0x87},
+        {0x31e0, 0x01},
+        {0x31e1, 0x9e},
+        {0x31e2, 0x01},
+        {0x31e5, 0x05},
+        {0x31e6, 0x05},
+        {0x31e7, 0x3a},
+        {0x31e8, 0x3a},
+
+        {0x3203, 0xc8},
+        {0x3207, 0x54},
+        {0x3213, 0x16},
+        {0x3215, 0xf6},
+        {0x321a, 0x14},
+        {0x321b, 0x51},
+        {0x3229, 0xe7},
+        {0x322a, 0xf0},
+        {0x322b, 0x10},
+        {0x3231, 0xe7},
+        {0x3232, 0xf0},
+        {0x3233, 0x10},
+        {0x323c, 0xe8},
+        {0x323d, 0x70},
+        {0x3243, 0x08},
+        {0x3244, 0xe1},
+        {0x3245, 0x10},
+        {0x3247, 0xe7},
+        {0x3248, 0x60},
+        {0x3249, 0x1e},
+        {0x324b, 0x00},
+        {0x324c, 0x41},
+        {0x3250, 0x30},
+        {0x3251, 0x0a},
+        {0x3252, 0xff},
+        {0x3253, 0xff},
+        {0x3254, 0xff},
+        {0x3255, 0x02},
+        {0x3257, 0xf0},
+        {0x325a, 0xa6},
+        {0x325d, 0x14},
+        {0x325e, 0x51},
+        {0x3260, 0x00},
+        {0x3261, 0x61},
+        {0x3266, 0x30},
+        {0x3267, 0x05},
+        {0x3275, 0xe7},
+        {0x3281, 0xea},
+        {0x3282, 0x70},
+        {0x3285, 0xff},
+        {0x328a, 0xf0},
+        {0x328d, 0xb6},
+        {0x328e, 0x40},
+        {0x3290, 0x42},
+        {0x3291, 0x51},
+        {0x3292, 0x1e},
+        {0x3294, 0xc4},
+        {0x3295, 0x20},
+        {0x3297, 0x50},
+        {0x3298, 0x31},
+        {0x3299, 0x1f},
+        {0x329b, 0xc0},
+        {0x329c, 0x60},
+        {0x329e, 0x4c},
+        {0x329f, 0x71},
+        {0x32a0, 0x1f},
+        {0x32a2, 0xb6},
+        {0x32a3, 0xc0},
+        {0x32a4, 0x0b},
+        {0x32a9, 0x24},
+        {0x32aa, 0x41},
+        {0x32b0, 0x25},
+        {0x32b1, 0x51},
+        {0x32b7, 0x1c},
+        {0x32b8, 0xc1},
+        {0x32b9, 0x12},
+        {0x32be, 0x1d},
+        {0x32bf, 0xd1},
+        {0x32c0, 0x12},
+        {0x32c2, 0xa8},
+        {0x32c3, 0xc0},
+        {0x32c4, 0x0a},
+        {0x32c5, 0x1e},
+        {0x32c6, 0x21},
+        {0x32c9, 0xb0},
+        {0x32ca, 0x40},
+        {0x32cc, 0x26},
+        {0x32cd, 0xa1},
+        {0x32d0, 0xb6},
+        {0x32d1, 0xc0},
+        {0x32d2, 0x0b},
+        {0x32d4, 0xe2},
+        {0x32d5, 0x40},
+        {0x32d8, 0x4e},
+        {0x32d9, 0xa1},
+        {0x32ec, 0xf0},
+
+        {0x3303, 0x00},
+        {0x3305, 0x03},
+        {0x3314, 0x04},
+        {0x3315, 0x01},
+        {0x3316, 0x04},
+        {0x3317, 0x04},
+        {0x3318, 0x38},
+        {0x3319, 0x04},
+        {0x332c, 0x40},
+        {0x332d, 0x20},
+        {0x332e, 0x03},
+        {0x333e, 0x0a},/*10BIT*/
+        {0x333f, 0x0a},/*10BIT*/
+        {0x3340, 0x03},
+        {0x3341, 0x20},
+        {0x3342, 0x25},
+        {0x3343, 0x68},
+        {0x3344, 0x20},
+        {0x3345, 0x40},
+        {0x3346, 0x28},
+        {0x3347, 0x20},
+        {0x3348, 0x18},
+        {0x3349, 0x78},
+        {0x334a, 0x28},
+        {0x334e, 0xb4},
+        {0x334f, 0x01},
+#ifdef INIT_ET_INSETTING
+        {0x3020, 0xe1},
+        {0x3021, 0x04},
+#endif
+        {IMX185_TABLE_END, 0x00}
+};
+
+static imx185_reg imx185_1920x1080_crop_10bit_30fps[] = {
+        {0x3002, 0x01},
+        {0x3005, 0x00},
+        {0x3006, 0x00},
+        {0x3007, 0x50},
+        {0x3009, 0x02},
+        {0x300a, 0x3c},
+        {0x300f, 0x01},
+        {0x3018, 0x65},
+        {0x3019, 0x04},
+        {0x301b, 0x98},
+        {0x301c, 0x08},
+        {0x301d, 0x08},
+        {0x301e, 0x02},
+
+        {0x3036, 0x06},
+        {0x3038, 0x08},
+        {0x3039, 0x00},
+        {0x303a, 0x40},
+        {0x303b, 0x04},
+        {0x303c, 0x0c},
+        {0x303d, 0x00},
+        {0x303e, 0x7c},
+        {0x303f, 0x07},
+
+        {0x3044, 0xe1},
+        {0x3048, 0x33},
+
+        {0x305C, 0x20},
+        {0x305D, 0x00},
+        {0x305E, 0x18},
+        {0x305F, 0x00},
+        {0x3063, 0x74},
+
+        {0x3084, 0x0f},
+        {0x3086, 0x10},
+        {0x30cf, 0xe1},
+        {0x30d0, 0x29},
+        {0x30d2, 0x9b},
+        {0x30d3, 0x01},
+
+        {0x311d, 0x0a},
+        {0x3123, 0x0f},
+        {0x3126, 0xdf},
+        {0x3147, 0x87},
+        {0x31e0, 0x01},
+        {0x31e1, 0x9e},
+        {0x31e2, 0x01},
+        {0x31e5, 0x05},
+        {0x31e6, 0x05},
+        {0x31e7, 0x3a},
+        {0x31e8, 0x3a},
+
+        {0x3203, 0xc8},
+        {0x3207, 0x54},
+        {0x3213, 0x16},
+        {0x3215, 0xf6},
+        {0x321a, 0x14},
+        {0x321b, 0x51},
+        {0x3229, 0xe7},
+        {0x322a, 0xf0},
+        {0x322b, 0x10},
+        {0x3231, 0xe7},
+        {0x3232, 0xf0},
+        {0x3233, 0x10},
+        {0x323c, 0xe8},
+        {0x323d, 0x70},
+        {0x3243, 0x08},
+        {0x3244, 0xe1},
+        {0x3245, 0x10},
+        {0x3247, 0xe7},
+        {0x3248, 0x60},
+        {0x3249, 0x1e},
+        {0x324b, 0x00},
+        {0x324c, 0x41},
+        {0x3250, 0x30},
+        {0x3251, 0x0a},
+        {0x3252, 0xff},
+        {0x3253, 0xff},
+        {0x3254, 0xff},
+        {0x3255, 0x02},
+        {0x3257, 0xf0},
+        {0x325a, 0xa6},
+        {0x325d, 0x14},
+        {0x325e, 0x51},
+        {0x3260, 0x00},
+        {0x3261, 0x61},
+        {0x3266, 0x30},
+        {0x3267, 0x05},
+        {0x3275, 0xe7},
+        {0x3281, 0xea},
+        {0x3282, 0x70},
+        {0x3285, 0xff},
+        {0x328a, 0xf0},
+        {0x328d, 0xb6},
+        {0x328e, 0x40},
+        {0x3290, 0x42},
+        {0x3291, 0x51},
+        {0x3292, 0x1e},
+        {0x3294, 0xc4},
+        {0x3295, 0x20},
+        {0x3297, 0x50},
+        {0x3298, 0x31},
+        {0x3299, 0x1f},
+        {0x329b, 0xc0},
+        {0x329c, 0x60},
+        {0x329e, 0x4c},
+        {0x329f, 0x71},
+        {0x32a0, 0x1f},
+        {0x32a2, 0xb6},
+        {0x32a3, 0xc0},
+        {0x32a4, 0x0b},
+        {0x32a9, 0x24},
+        {0x32aa, 0x41},
+        {0x32b0, 0x25},
+        {0x32b1, 0x51},
+        {0x32b7, 0x1c},
+        {0x32b8, 0xc1},
+        {0x32b9, 0x12},
+        {0x32be, 0x1d},
+        {0x32bf, 0xd1},
+        {0x32c0, 0x12},
+        {0x32c2, 0xa8},
+        {0x32c3, 0xc0},
+        {0x32c4, 0x0a},
+        {0x32c5, 0x1e},
+        {0x32c6, 0x21},
+        {0x32c9, 0xb0},
+        {0x32ca, 0x40},
+        {0x32cc, 0x26},
+        {0x32cd, 0xa1},
+        {0x32d0, 0xb6},
+        {0x32d1, 0xc0},
+        {0x32d2, 0x0b},
+        {0x32d4, 0xe2},
+        {0x32d5, 0x40},
+        {0x32d8, 0x4e},
+        {0x32d9, 0xa1},
+        {0x32ec, 0xf0},
+
+        {0x3303, 0x10},
+        {0x3305, 0x03},
+        {0x3314, 0x04},
+        {0x3315, 0x01},
+        {0x3316, 0x04},
+        {0x3317, 0x04},
+        {0x3318, 0x38},
+        {0x3319, 0x04},
+        {0x332c, 0x30},
+        {0x332d, 0x20},
+        {0x332e, 0x03},
+        {0x333e, 0x0a},
+        {0x333f, 0x0a},
+        {0x3340, 0x03},
+
+        {0x3341, 0x20},
+        {0x3342, 0x25},
+        {0x3343, 0x58},
+        {0x3344, 0x10},
+        {0x3345, 0x30},
+        {0x3346, 0x18},
+        {0x3347, 0x10},
+        {0x3348, 0x10},
+        {0x3349, 0x48},
+        {0x334a, 0x28},
+        {0x334e, 0xb4},
+        {0x334f, 0x01},
+#ifdef INIT_ET_INSETTING
+        {0x3020, 0xe1},
+        {0x3021, 0x04},
+#endif
+        {IMX185_TABLE_END, 0x00}
+};
+
+enum {
+        IMX185_MODE_1920X1080_CROP_30FPS,
+        IMX185_MODE_1920X1080_CROP_10BIT_30FPS,
+        IMX185_MODE_1920X1080_CROP_60FPS,
+        IMX185_MODE_1920X1080_CROP_10BIT_60FPS,
+        IMX185_MODE_1920X1080_CROP_HDR_30FPS,
+        IMX185_MODE_START_STREAM,
+        IMX185_MODE_STOP_STREAM,
+        IMX185_MODE_TEST_PATTERN
+};
+
+static imx185_reg *mode_table[] = {
+        [IMX185_MODE_1920X1080_CROP_30FPS] = imx185_1920x1080_crop_30fps,
+        [IMX185_MODE_1920X1080_CROP_10BIT_30FPS] =
+                imx185_1920x1080_crop_10bit_30fps,
+        [IMX185_MODE_1920X1080_CROP_60FPS] = imx185_1920x1080_crop_60fps,
+        [IMX185_MODE_1920X1080_CROP_10BIT_60FPS] =
+                imx185_1920x1080_crop_10bit_60fps,
+        [IMX185_MODE_1920X1080_CROP_HDR_30FPS] =
+                imx185_1920x1080_hdr_crop_30fps,
+        [IMX185_MODE_START_STREAM] = imx185_start,
+        [IMX185_MODE_STOP_STREAM] = imx185_stop,
+        [IMX185_MODE_TEST_PATTERN] = tp_colorbars,
+};
+
+static const int imx185_30fps[] = {
+        30,
+};
+
+static const int imx185_60fps[] = {
+        60,
+};
+
+static const struct camera_common_frmfmt imx185_frmfmt[] = {
+        {{1920, 1080}, imx185_30fps, 1, 0,
+                        IMX185_MODE_1920X1080_CROP_30FPS},
+        {{1920, 1080}, imx185_30fps, 1, 0,
+                        IMX185_MODE_1920X1080_CROP_10BIT_30FPS},
+        {{1920, 1080}, imx185_60fps, 1, 0,
+                        IMX185_MODE_1920X1080_CROP_60FPS},
+        {{1920, 1080}, imx185_60fps, 1, 0,
+                        IMX185_MODE_1920X1080_CROP_10BIT_60FPS},
+        {{1920, 1080}, imx185_30fps, 1, 1,
+                        IMX185_MODE_1920X1080_CROP_HDR_30FPS},
+};
+#endif /* __IMX185_I2C_TABLES__ */
diff --git a/drivers/media/i2c/imx214.c b/drivers/media/i2c/imx214.c
new file mode 100644
index 000000000..adb0d16d4
--- /dev/null
+++ b/drivers/media/i2c/imx214.c
@@ -0,0 +1,1294 @@
+/*
+ * imx214.c - imx214 sensor driver
+ *
+ * Copyright (c) 2013-2016, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+
+#include <linux/seq_file.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <media/camera_common.h>
+#include <media/imx214.h>
+
+#include "imx214_mode_tbls.h"
+
+#define IMX214_MAX_COARSE_DIFF          10
+
+#define IMX214_GAIN_SHIFT               8
+#define IMX214_MIN_GAIN         (1 << IMX214_GAIN_SHIFT)
+#define IMX214_MAX_GAIN         (16 << IMX214_GAIN_SHIFT)
+#define IMX214_MIN_FRAME_LENGTH (0x0)
+#define IMX214_MAX_FRAME_LENGTH (0xffff)
+#define IMX214_MIN_EXPOSURE_COARSE      (0x0001)
+#define IMX214_MAX_EXPOSURE_COARSE      \
+        (IMX214_MAX_FRAME_LENGTH-IMX214_MAX_COARSE_DIFF)
+
+#define IMX214_DEFAULT_GAIN             IMX214_MIN_GAIN
+#define IMX214_DEFAULT_FRAME_LENGTH     (0x0C7A)
+#define IMX214_DEFAULT_EXPOSURE_COARSE  \
+        (IMX214_DEFAULT_FRAME_LENGTH-IMX214_MAX_COARSE_DIFF)
+
+#define IMX214_DEFAULT_MODE     IMX214_MODE_4096X3072
+#define IMX214_DEFAULT_HDR_MODE IMX214_MODE_4096X3072_HDR
+#define IMX214_DEFAULT_WIDTH    4096
+#define IMX214_DEFAULT_HEIGHT   3072
+#define IMX214_DEFAULT_DATAFMT  V4L2_MBUS_FMT_SRGGB10_1X10
+#define IMX214_DEFAULT_CLK_FREQ 24000000
+
+struct imx214 {
+        struct camera_common_power_rail power;
+        int                             numctrls;
+        struct v4l2_ctrl_handler        ctrl_handler;
+        struct camera_common_eeprom_data eeprom[IMX214_EEPROM_NUM_BLOCKS];
+        u8                              eeprom_buf[IMX214_EEPROM_SIZE];
+        struct i2c_client               *i2c_client;
+        struct v4l2_subdev              *subdev;
+        struct media_pad                pad;
+
+        s32                             group_hold_prev;
+        bool                            group_hold_en;
+        struct regmap                   *regmap;
+        struct camera_common_data       *s_data;
+        struct camera_common_pdata      *pdata;
+        struct v4l2_ctrl                *ctrls[];
+};
+
+static const struct regmap_config sensor_regmap_config = {
+        .reg_bits = 16,
+        .val_bits = 8,
+        .cache_type = REGCACHE_RBTREE,
+};
+
+static int imx214_g_volatile_ctrl(struct v4l2_ctrl *ctrl);
+static int imx214_s_ctrl(struct v4l2_ctrl *ctrl);
+
+static const struct v4l2_ctrl_ops imx214_ctrl_ops = {
+        .g_volatile_ctrl = imx214_g_volatile_ctrl,
+        .s_ctrl         = imx214_s_ctrl,
+};
+
+static struct v4l2_ctrl_config ctrl_config_list[] = {
+/* Do not change the name field for the controls! */
+        {
+                .ops = &imx214_ctrl_ops,
+                .id = V4L2_CID_GAIN,
+                .name = "Gain",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX214_MIN_GAIN,
+                .max = IMX214_MAX_GAIN,
+                .def = IMX214_DEFAULT_GAIN,
+                .step = 1,
+        },
+        {
+                .ops = &imx214_ctrl_ops,
+                .id = V4L2_CID_FRAME_LENGTH,
+                .name = "Frame Length",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX214_MIN_FRAME_LENGTH,
+                .max = IMX214_MAX_FRAME_LENGTH,
+                .def = IMX214_DEFAULT_FRAME_LENGTH,
+                .step = 1,
+        },
+        {
+                .ops = &imx214_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME,
+                .name = "Coarse Time",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX214_MIN_EXPOSURE_COARSE,
+                .max = IMX214_MAX_EXPOSURE_COARSE,
+                .def = IMX214_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &imx214_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME_SHORT,
+                .name = "Coarse Time Short",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX214_MIN_EXPOSURE_COARSE,
+                .max = IMX214_MAX_EXPOSURE_COARSE,
+                .def = IMX214_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &imx214_ctrl_ops,
+                .id = V4L2_CID_GROUP_HOLD,
+                .name = "Group Hold",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &imx214_ctrl_ops,
+                .id = V4L2_CID_HDR_EN,
+                .name = "HDR enable",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &imx214_ctrl_ops,
+                .id = V4L2_CID_EEPROM_DATA,
+                .name = "EEPROM Data",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_VOLATILE,
+                .min = 0,
+                .max = IMX214_EEPROM_STR_SIZE,
+                .step = 2,
+        },
+        {
+                .ops = &imx214_ctrl_ops,
+                .id = V4L2_CID_OTP_DATA,
+                .name = "OTP Data",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = IMX214_OTP_STR_SIZE,
+                .step = 2,
+        },
+        {
+                .ops = &imx214_ctrl_ops,
+                .id = V4L2_CID_FUSE_ID,
+                .name = "Fuse ID",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = IMX214_FUSE_ID_STR_SIZE,
+                .step = 2,
+        },
+};
+
+static inline void imx214_get_frame_length_regs(imx214_reg *regs,
+                                u16 frame_length)
+{
+        regs->addr = IMX214_FRAME_LENGTH_ADDR_MSB;
+        regs->val = (frame_length >> 8) & 0xff;
+        (regs + 1)->addr = IMX214_FRAME_LENGTH_ADDR_LSB;
+        (regs + 1)->val = (frame_length) & 0xff;
+}
+
+static inline void imx214_get_coarse_time_regs(imx214_reg *regs,
+                                u16 coarse_time)
+{
+        regs->addr = IMX214_COARSE_TIME_ADDR_MSB;
+        regs->val = (coarse_time >> 8) & 0xff;
+        (regs + 1)->addr = IMX214_COARSE_TIME_ADDR_LSB;
+        (regs + 1)->val = (coarse_time) & 0xff;
+}
+
+static inline void imx214_get_coarse_time_short_regs(imx214_reg *regs,
+                                u16 coarse_time)
+{
+        regs->addr = IMX214_COARSE_TIME_SHORT_ADDR_MSB;
+        regs->val = (coarse_time >> 8) & 0xff;
+        (regs + 1)->addr = IMX214_COARSE_TIME_SHORT_ADDR_LSB;
+        (regs + 1)->val = (coarse_time) & 0xff;
+}
+
+static inline void imx214_get_gain_regs(imx214_reg *regs,
+                                u16 gain)
+{
+        regs->addr = IMX214_GAIN_ADDR_MSB;
+        regs->val = (gain >> 8) & 0xff;
+        (regs + 1)->addr = IMX214_GAIN_ADDR_LSB;
+        (regs + 1)->val = (gain) & 0xff;
+}
+
+static inline void imx214_get_gain_short_reg(imx214_reg *regs,
+                                u16 gain)
+{
+        regs->addr = IMX214_GAIN_SHORT_ADDR_MSB;
+        regs->val = (gain >> 8) & 0xff;
+        (regs + 1)->addr = IMX214_GAIN_SHORT_ADDR_LSB;
+        (regs + 1)->val = (gain) & 0xff;
+}
+
+static int test_mode;
+module_param(test_mode, int, 0644);
+
+static inline int imx214_read_reg(struct camera_common_data *s_data,
+                                u16 addr, u8 *val)
+{
+        struct imx214 *priv = (struct imx214 *)s_data->priv;
+
+        return regmap_read(priv->regmap, addr, (unsigned int *) val);
+}
+
+static int imx214_write_reg(struct camera_common_data *s_data, u16 addr, u8 val)
+{
+        int err;
+        struct imx214 *priv = (struct imx214 *)s_data->priv;
+
+        err = regmap_write(priv->regmap, addr, val);
+        if (err)
+                pr_err("%s:i2c write failed, %x = %x\n",
+                        __func__, addr, val);
+
+        return err;
+}
+
+static int imx214_write_table(struct imx214 *priv,
+                                const imx214_reg table[])
+{
+        return regmap_util_write_table_8(priv->regmap,
+                                         table,
+                                         NULL, 0,
+                                         IMX214_TABLE_WAIT_MS,
+                                         IMX214_TABLE_END);
+}
+
+static int imx214_power_on(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct imx214 *priv = (struct imx214 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power on\n", __func__);
+
+        if (priv->pdata && priv->pdata->power_on) {
+                err = priv->pdata->power_on(pw);
+                if (err)
+                        pr_err("%s failed.\n", __func__);
+                else
+                        pw->state = SWITCH_ON;
+                return err;
+        }
+
+        /* sleep calls in the sequence below are for internal device
+         * signal propagation as specified by sensor vendor */
+
+        if (pw->reset_gpio)
+                gpio_set_value(pw->reset_gpio, 0);
+        if (pw->af_gpio)
+                gpio_set_value(pw->af_gpio, 1);
+        if (pw->pwdn_gpio)
+                gpio_set_value(pw->pwdn_gpio, 0);
+        usleep_range(10, 20);
+
+        if (pw->avdd)
+                err = regulator_enable(pw->avdd);
+        if (err)
+                goto imx214_avdd_fail;
+
+        if (pw->iovdd)
+                err = regulator_enable(pw->iovdd);
+        if (err)
+                goto imx214_iovdd_fail;
+
+        udelay(1);
+        if (pw->reset_gpio)
+                gpio_set_value(pw->reset_gpio, 1);
+        if (pw->pwdn_gpio)
+                gpio_set_value(pw->pwdn_gpio, 1);
+
+        usleep_range(300, 310);
+
+        pw->state = SWITCH_ON;
+        return 0;
+
+imx214_iovdd_fail:
+        regulator_disable(pw->avdd);
+
+imx214_avdd_fail:
+        if (pw->af_gpio)
+                gpio_set_value(pw->af_gpio, 0);
+
+        pr_err("%s failed.\n", __func__);
+        return -ENODEV;
+}
+
+static int imx214_power_off(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct imx214 *priv = (struct imx214 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power off\n", __func__);
+
+        if (priv->pdata && priv->pdata->power_on) {
+                err = priv->pdata->power_off(pw);
+                if (err) {
+                        pr_err("%s failed.\n", __func__);
+                        return err;
+                } else {
+                        goto power_off_done;
+                }
+        }
+
+        /* sleeps calls in the sequence below are for internal device
+         * signal propagation as specified by sensor vendor */
+
+        usleep_range(1, 2);
+        if (pw->reset_gpio)
+                gpio_set_value(pw->reset_gpio, 0);
+        if (pw->af_gpio)
+                gpio_set_value(pw->af_gpio, 0);
+        if (pw->pwdn_gpio)
+                gpio_set_value(pw->pwdn_gpio, 0);
+        usleep_range(1, 2);
+
+        if (pw->iovdd)
+                regulator_disable(pw->iovdd);
+        if (pw->avdd)
+                regulator_disable(pw->avdd);
+
+power_off_done:
+        pw->state = SWITCH_OFF;
+        return 0;
+}
+
+static int imx214_power_put(struct imx214 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        if (unlikely(!pw))
+                return -EFAULT;
+
+        if (likely(pw->avdd))
+                regulator_put(pw->avdd);
+
+        if (likely(pw->iovdd))
+                regulator_put(pw->iovdd);
+
+        if (likely(pw->dvdd))
+                regulator_put(pw->dvdd);
+
+        pw->avdd = NULL;
+        pw->iovdd = NULL;
+        pw->dvdd = NULL;
+
+        return 0;
+}
+
+static int imx214_power_get(struct imx214 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        struct camera_common_pdata *pdata = priv->pdata;
+        const char *mclk_name;
+        int err = 0;
+
+        mclk_name = priv->pdata->mclk_name ?
+                    priv->pdata->mclk_name : "cam_mclk1";
+        pw->mclk = devm_clk_get(&priv->i2c_client->dev, mclk_name);
+        if (IS_ERR(pw->mclk)) {
+                dev_err(&priv->i2c_client->dev,
+                        "unable to get clock %s\n", mclk_name);
+                return PTR_ERR(pw->mclk);
+        }
+
+        /* analog 2.7v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->avdd, pdata->regulators.avdd);
+        /* digital 1.2v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->dvdd, pdata->regulators.dvdd);
+        /* IO 1.8v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->iovdd, pdata->regulators.iovdd);
+
+        if (!err) {
+                pw->reset_gpio = pdata->reset_gpio;
+                pw->af_gpio = pdata->af_gpio;
+                pw->pwdn_gpio = pdata->pwdn_gpio;
+        }
+
+        pw->state = SWITCH_OFF;
+        return err;
+}
+
+static int imx214_set_gain(struct imx214 *priv, s32 val);
+static int imx214_set_frame_length(struct imx214 *priv, s32 val);
+static int imx214_set_coarse_time(struct imx214 *priv, s32 val);
+static int imx214_set_coarse_time_short(struct imx214 *priv, s32 val);
+
+static int imx214_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx214 *priv = (struct imx214 *)s_data->priv;
+        struct v4l2_control control;
+        int err;
+
+        dev_dbg(&client->dev, "%s++ enable %d\n", __func__, enable);
+        if (!enable)
+                return imx214_write_table(priv,
+                        mode_table[IMX214_MODE_STOP_STREAM]);
+
+        err = imx214_write_table(priv, mode_table[IMX214_MODE_COMMON]);
+        if (err)
+                goto exit;
+        err = imx214_write_table(priv, mode_table[s_data->mode]);
+        if (err)
+                goto exit;
+
+        if (s_data->override_enable) {
+                /* write list of override regs for the asking frame length, */
+                /*
+                 * coarse integration time, and gain. Failures to write
+                 * overrides are non-fatal
+                 */
+                control.id = V4L2_CID_GAIN;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= imx214_set_gain(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev, "%s: warning gain override failed\n",
+                                __func__);
+
+                control.id = V4L2_CID_FRAME_LENGTH;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= imx214_set_frame_length(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: frame length override failed\n", __func__);
+
+                control.id = V4L2_CID_COARSE_TIME;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= imx214_set_coarse_time(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: coarse time override failed\n", __func__);
+
+                control.id = V4L2_CID_COARSE_TIME_SHORT;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= imx214_set_coarse_time_short(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: warning coarse time short override failed\n",
+                                __func__);
+        }
+
+        err = imx214_write_table(priv, mode_table[IMX214_MODE_START_STREAM]);
+        if (err)
+                goto exit;
+
+        if (test_mode)
+                err = imx214_write_table(priv,
+                        mode_table[IMX214_MODE_TEST_PATTERN]);
+
+        return 0;
+exit:
+        dev_dbg(&client->dev, "%s: error setting stream\n", __func__);
+        return err;
+}
+
+static struct v4l2_subdev_video_ops imx214_subdev_video_ops = {
+        .s_stream       = imx214_s_stream,
+        .s_mbus_fmt     = camera_common_s_fmt,
+        .g_mbus_fmt     = camera_common_g_fmt,
+        .try_mbus_fmt   = camera_common_try_fmt,
+        .enum_mbus_fmt  = camera_common_enum_fmt,
+        .g_mbus_config  = camera_common_g_mbus_config,
+};
+
+static struct v4l2_subdev_core_ops imx214_subdev_core_ops = {
+        .s_power        = camera_common_s_power,
+};
+
+static struct v4l2_subdev_pad_ops imx214_subdev_pad_ops = {
+        .enum_mbus_code = camera_common_enum_mbus_code,
+};
+
+static struct v4l2_subdev_ops imx214_subdev_ops = {
+        .core   = &imx214_subdev_core_ops,
+        .video  = &imx214_subdev_video_ops,
+        .pad    = &imx214_subdev_pad_ops,
+};
+
+static struct of_device_id imx214_of_match[] = {
+        { .compatible = "nvidia,imx214", },
+        { },
+};
+
+static struct camera_common_sensor_ops imx214_common_ops = {
+        .power_on = imx214_power_on,
+        .power_off = imx214_power_off,
+        .write_reg = imx214_write_reg,
+        .read_reg = imx214_read_reg,
+};
+
+static int imx214_set_group_hold(struct imx214 *priv)
+{
+        int err;
+        int gh_prev = switch_ctrl_qmenu[priv->group_hold_prev];
+
+        if (priv->group_hold_en == true && gh_prev == SWITCH_OFF) {
+                err = imx214_write_reg(priv->s_data,
+                                       IMX214_GROUP_HOLD_ADDR, 0x1);
+                if (err)
+                        goto fail;
+                priv->group_hold_prev = 1;
+        } else if (priv->group_hold_en == false && gh_prev == SWITCH_ON) {
+                err = imx214_write_reg(priv->s_data,
+                                       IMX214_GROUP_HOLD_ADDR, 0x0);
+                if (err)
+                        goto fail;
+                priv->group_hold_prev = 0;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: Group hold control error\n", __func__);
+        return err;
+}
+
+static int imx214_calculate_gain(u32 rep, int shift)
+{
+        int gain;
+        int gain_int;
+        int gain_dec;
+        int min_int = (1 << shift);
+        int denom;
+
+        /* shift indicates number of least significant bits
+         * used for decimal representation of gain */
+        gain_int = (int)(rep >> shift);
+        gain_dec = (int)(rep & ~(0xffff << shift));
+
+        denom = gain_int * min_int + gain_dec;
+        gain = 512 - ((512 * min_int + (denom - 1)) / denom);
+
+        return gain;
+}
+
+static int imx214_set_gain(struct imx214 *priv, s32 val)
+{
+        imx214_reg reg_list[2];
+        imx214_reg reg_list_short[2];
+        int err;
+        u16 gain;
+        int i = 0;
+
+        /* translate value */
+        gain = (u16)imx214_calculate_gain(val, IMX214_GAIN_SHIFT);
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, gain);
+
+        imx214_get_gain_regs(reg_list, gain);
+        imx214_get_gain_short_reg(reg_list_short, gain);
+        imx214_set_group_hold(priv);
+
+        /* writing long gain */
+        for (i = 0; i < 2; i++) {
+                err = imx214_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+        /* writing short gain */
+        for (i = 0; i < 2; i++) {
+                err = imx214_write_reg(priv->s_data, reg_list_short[i].addr,
+                         reg_list_short[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: GAIN control error\n", __func__);
+        return err;
+}
+
+static int imx214_set_frame_length(struct imx214 *priv, s32 val)
+{
+        imx214_reg reg_list[2];
+        int err;
+        u16 frame_length;
+        int i = 0;
+
+        frame_length = (u16)val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, frame_length);
+
+        imx214_get_frame_length_regs(reg_list, frame_length);
+        imx214_set_group_hold(priv);
+
+        for (i = 0; i < 2; i++) {
+                err = imx214_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: FRAME_LENGTH control error\n", __func__);
+        return err;
+}
+
+static int imx214_set_coarse_time(struct imx214 *priv, s32 val)
+{
+        imx214_reg reg_list[2];
+        int err;
+        u16 coarse_time;
+        int i = 0;
+
+        coarse_time = (u16)val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, coarse_time);
+
+        imx214_get_coarse_time_regs(reg_list, coarse_time);
+        imx214_set_group_hold(priv);
+
+        for (i = 0; i < 2; i++) {
+                err = imx214_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: COARSE_TIME control error\n", __func__);
+        return err;
+}
+
+static int imx214_set_coarse_time_short(struct imx214 *priv, s32 val)
+{
+        imx214_reg reg_list[2];
+        int err;
+        struct v4l2_control hdr_control;
+        int hdr_en;
+        u16 coarse_time_short;
+        int i = 0;
+
+        /* check hdr enable ctrl */
+        hdr_control.id = V4L2_CID_HDR_EN;
+
+        err = camera_common_g_ctrl(priv->s_data, &hdr_control);
+        if (err < 0) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return err;
+        }
+
+        hdr_en = switch_ctrl_qmenu[hdr_control.value];
+        if (hdr_en == SWITCH_OFF)
+                return 0;
+
+        coarse_time_short = (u16)val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, coarse_time_short);
+
+        imx214_get_coarse_time_short_regs(reg_list, coarse_time_short);
+        imx214_set_group_hold(priv);
+
+        for (i = 0; i < 2; i++) {
+                err  = imx214_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: COARSE_TIME_SHORT control error\n", __func__);
+        return err;
+}
+
+static int imx214_eeprom_device_release(struct imx214 *priv)
+{
+        int i;
+
+        for (i = 0; i < IMX214_EEPROM_NUM_BLOCKS; i++) {
+                if (priv->eeprom[i].i2c_client != NULL) {
+                        i2c_unregister_device(priv->eeprom[i].i2c_client);
+                        priv->eeprom[i].i2c_client = NULL;
+                }
+        }
+
+        return 0;
+}
+
+static int imx214_eeprom_device_init(struct imx214 *priv)
+{
+        char *dev_name = "eeprom_imx214";
+        static struct regmap_config eeprom_regmap_config = {
+                .reg_bits = 8,
+                .val_bits = 8,
+        };
+        int i;
+        int err;
+        struct v4l2_ctrl *ctrl;
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_EEPROM_DATA);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return -EINVAL;
+        }
+
+        for (i = 0; i < IMX214_EEPROM_NUM_BLOCKS; i++) {
+                priv->eeprom[i].adap = i2c_get_adapter(
+                                priv->i2c_client->adapter->nr);
+                memset(&priv->eeprom[i].brd, 0, sizeof(priv->eeprom[i].brd));
+                strncpy(priv->eeprom[i].brd.type, dev_name,
+                                sizeof(priv->eeprom[i].brd.type));
+                priv->eeprom[i].brd.addr = IMX214_EEPROM_ADDRESS + i;
+                priv->eeprom[i].i2c_client = i2c_new_device(
+                                priv->eeprom[i].adap, &priv->eeprom[i].brd);
+
+                priv->eeprom[i].regmap = devm_regmap_init_i2c(
+                        priv->eeprom[i].i2c_client, &eeprom_regmap_config);
+                if (IS_ERR(priv->eeprom[i].regmap)) {
+                        err = PTR_ERR(priv->eeprom[i].regmap);
+                        imx214_eeprom_device_release(priv);
+                        ctrl->flags = V4L2_CTRL_FLAG_DISABLED;
+                        return err;
+                }
+        }
+
+        return 0;
+}
+
+static int imx214_read_eeprom(struct imx214 *priv,
+                                struct v4l2_ctrl *ctrl)
+{
+        int err, i;
+
+        for (i = 0; i < IMX214_EEPROM_NUM_BLOCKS; i++) {
+                err = regmap_bulk_read(priv->eeprom[i].regmap, 0,
+                        &priv->eeprom_buf[i * IMX214_EEPROM_BLOCK_SIZE],
+                        IMX214_EEPROM_BLOCK_SIZE);
+                if (err)
+                        return err;
+        }
+
+        for (i = 0; i < IMX214_EEPROM_SIZE; i++)
+                sprintf(&ctrl->string[i*2], "%02x",
+                        priv->eeprom_buf[i]);
+        return 0;
+}
+
+static int imx214_write_eeprom(struct imx214 *priv,
+                                char *string)
+{
+        int err;
+        int i;
+        u8 curr[3];
+        unsigned long data;
+
+        for (i = 0; i < IMX214_EEPROM_SIZE; i++) {
+                curr[0] = string[i*2];
+                curr[1] = string[i*2+1];
+                curr[2] = '\0';
+
+                err = kstrtol(curr, 16, &data);
+                if (err) {
+                        dev_err(&priv->i2c_client->dev,
+                                "invalid eeprom string\n");
+                        return -EINVAL;
+                }
+
+                priv->eeprom_buf[i] = (u8)data;
+                err = regmap_write(priv->eeprom[i >> 8].regmap,
+                                   i & 0xFF, (u8)data);
+                if (err)
+                        return err;
+                msleep(20);
+        }
+        return 0;
+}
+
+static int imx214_read_otp_page(struct imx214 *priv,
+                                u8 *buf, int page, u16 addr, int size)
+{
+        u8 status;
+        int err;
+
+        err = imx214_write_reg(priv->s_data, IMX214_OTP_PAGE_NUM_ADDR, page);
+        if (err)
+                return err;
+        err = imx214_write_reg(priv->s_data, IMX214_OTP_CTRL_ADDR, 0x01);
+        if (err)
+                return err;
+        err = imx214_read_reg(priv->s_data, IMX214_OTP_STATUS_ADDR, &status);
+        if (err)
+                return err;
+        if (status == IMX214_OTP_STATUS_IN_PROGRESS) {
+                dev_err(&priv->i2c_client->dev,
+                        "another OTP read in progress\n");
+                return err;
+        }
+
+        err = regmap_bulk_read(priv->regmap, addr, buf, size);
+        if (err)
+                return err;
+
+        err = imx214_read_reg(priv->s_data, IMX214_OTP_STATUS_ADDR, &status);
+        if (err)
+                return err;
+        if (status == IMX214_OTP_STATUS_READ_FAIL) {
+                dev_err(&priv->i2c_client->dev, "fuse id read error\n");
+                return err;
+        }
+
+        return 0;
+}
+
+static int imx214_otp_setup(struct imx214 *priv)
+{
+        int err;
+        int i;
+        struct v4l2_ctrl *ctrl;
+        u8 otp_buf[IMX214_OTP_SIZE];
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        for (i = 0; i < IMX214_OTP_NUM_PAGES; i++) {
+                imx214_read_otp_page(priv,
+                                   &otp_buf[i * IMX214_OTP_PAGE_SIZE],
+                                   i,
+                                   IMX214_OTP_PAGE_START_ADDR,
+                                   IMX214_OTP_PAGE_SIZE);
+        }
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_OTP_DATA);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return -EINVAL;
+        }
+
+        for (i = 0; i < IMX214_OTP_SIZE; i++)
+                sprintf(&ctrl->string[i*2], "%02x",
+                        otp_buf[i]);
+        ctrl->cur.string = ctrl->string;
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int imx214_fuse_id_setup(struct imx214 *priv)
+{
+        int err;
+        int i;
+        struct v4l2_ctrl *ctrl;
+        u8 fuse_id[IMX214_FUSE_ID_SIZE];
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        imx214_read_otp_page(priv,
+                           &fuse_id[0],
+                           IMX214_FUSE_ID_OTP_PAGE,
+                           IMX214_FUSE_ID_OTP_ROW_ADDR,
+                           IMX214_FUSE_ID_SIZE);
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_FUSE_ID);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return -EINVAL;
+        }
+
+        for (i = 0; i < IMX214_FUSE_ID_SIZE; i++)
+                sprintf(&ctrl->string[i*2], "%02x",
+                        fuse_id[i]);
+        ctrl->cur.string = ctrl->string;
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int imx214_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct imx214 *priv =
+                container_of(ctrl->handler, struct imx214, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_EEPROM_DATA:
+                err = imx214_read_eeprom(priv, ctrl);
+                if (err)
+                        return err;
+                break;
+        default:
+                        pr_err("%s: unknown ctrl id.\n", __func__);
+                        return -EINVAL;
+        }
+
+        return err;
+}
+
+static int imx214_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct imx214 *priv =
+                container_of(ctrl->handler, struct imx214, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_GAIN:
+                err = imx214_set_gain(priv, ctrl->val);
+                break;
+        case V4L2_CID_FRAME_LENGTH:
+                err = imx214_set_frame_length(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME:
+                err = imx214_set_coarse_time(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME_SHORT:
+                err = imx214_set_coarse_time_short(priv, ctrl->val);
+                break;
+        case V4L2_CID_GROUP_HOLD:
+                if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON) {
+                        priv->group_hold_en = true;
+                } else {
+                        priv->group_hold_en = false;
+                        err = imx214_set_group_hold(priv);
+                }
+                break;
+        case V4L2_CID_EEPROM_DATA:
+                if (!ctrl->string[0])
+                        break;
+                err = imx214_write_eeprom(priv, ctrl->string);
+                if (err)
+                        return err;
+                break;
+        case V4L2_CID_HDR_EN:
+                break;
+        default:
+                pr_err("%s: unknown ctrl id.\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static int imx214_ctrls_init(struct imx214 *priv)
+{
+        struct i2c_client *client = priv->i2c_client;
+        struct v4l2_ctrl *ctrl;
+        int numctrls;
+        int err;
+        int i;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        numctrls = ARRAY_SIZE(ctrl_config_list);
+        v4l2_ctrl_handler_init(&priv->ctrl_handler, numctrls);
+
+        for (i = 0; i < numctrls; i++) {
+                ctrl = v4l2_ctrl_new_custom(&priv->ctrl_handler,
+                        &ctrl_config_list[i], NULL);
+                if (ctrl == NULL) {
+                        dev_err(&client->dev, "Failed to init %s ctrl\n",
+                                ctrl_config_list[i].name);
+                        continue;
+                }
+
+                if (ctrl_config_list[i].type == V4L2_CTRL_TYPE_STRING &&
+                        ctrl_config_list[i].flags & V4L2_CTRL_FLAG_READ_ONLY) {
+                        ctrl->string = devm_kzalloc(&client->dev,
+                                ctrl_config_list[i].max + 1, GFP_KERNEL);
+                        if (!ctrl->string) {
+                                dev_err(&client->dev,
+                                        "Failed to allocate otp data\n");
+                                return -ENOMEM;
+                        }
+                }
+                priv->ctrls[i] = ctrl;
+        }
+
+        priv->numctrls = numctrls;
+        priv->subdev->ctrl_handler = &priv->ctrl_handler;
+        if (priv->ctrl_handler.error) {
+                dev_err(&client->dev, "Error %d adding controls\n",
+                        priv->ctrl_handler.error);
+                err = priv->ctrl_handler.error;
+                goto error;
+        }
+
+        err = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d setting default controls\n", err);
+                goto error;
+        }
+
+        err = imx214_otp_setup(priv);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d reading otp data\n", err);
+                goto error;
+        }
+
+        err = imx214_fuse_id_setup(priv);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d reading fuse id data\n", err);
+                goto error;
+        }
+
+        return 0;
+
+error:
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        return err;
+}
+
+MODULE_DEVICE_TABLE(of, imx214_of_match);
+
+static struct camera_common_pdata *imx214_parse_dt(struct i2c_client *client)
+{
+        struct device_node *np = client->dev.of_node;
+        struct camera_common_pdata *board_priv_pdata;
+        const struct of_device_id *match;
+
+        match = of_match_device(imx214_of_match, &client->dev);
+        if (!match) {
+                dev_err(&client->dev, "Failed to find matching dt id\n");
+                return NULL;
+        }
+
+        board_priv_pdata = devm_kzalloc(&client->dev,
+                           sizeof(*board_priv_pdata), GFP_KERNEL);
+        if (!board_priv_pdata) {
+                dev_err(&client->dev, "Failed to allocate pdata\n");
+                return NULL;
+        }
+
+        of_property_read_string(np, "mclk", &board_priv_pdata->mclk_name);
+        board_priv_pdata->pwdn_gpio = of_get_named_gpio(np, "pwdn-gpios", 0);
+        board_priv_pdata->reset_gpio = of_get_named_gpio(np, "reset-gpios", 0);
+        board_priv_pdata->af_gpio = of_get_named_gpio(np, "af-gpios", 0);
+
+        of_property_read_string(np, "avdd-reg",
+                        &board_priv_pdata->regulators.avdd);
+        of_property_read_string(np, "dvdd-reg",
+                        &board_priv_pdata->regulators.dvdd);
+        of_property_read_string(np, "iovdd-reg",
+                        &board_priv_pdata->regulators.iovdd);
+
+        return board_priv_pdata;
+}
+
+static int imx214_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        dev_dbg(&client->dev, "%s:\n", __func__);
+
+
+        return 0;
+}
+
+static const struct v4l2_subdev_internal_ops imx214_subdev_internal_ops = {
+        .open = imx214_open,
+};
+
+static const struct media_entity_operations imx214_media_ops = {
+        .link_validate = v4l2_subdev_link_validate,
+};
+
+static int imx214_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct camera_common_data *common_data;
+        struct device_node *node = client->dev.of_node;
+        struct imx214 *priv;
+        char debugfs_name[10];
+        int err;
+
+        pr_info("[IMX214]: probing v4l2 sensor.\n");
+
+        if (!IS_ENABLED(CONFIG_OF) || !node)
+                return -EINVAL;
+
+        common_data = devm_kzalloc(&client->dev,
+                            sizeof(struct camera_common_data), GFP_KERNEL);
+        if (!common_data) {
+                dev_err(&client->dev, "unable to allocate memory!\n");
+                return -ENOMEM;
+        }
+
+        priv = devm_kzalloc(&client->dev,
+                            sizeof(struct imx214) + sizeof(struct v4l2_ctrl *) *
+                            ARRAY_SIZE(ctrl_config_list),
+                            GFP_KERNEL);
+        if (!priv) {
+                dev_err(&client->dev, "unable to allocate memory!\n");
+                return -ENOMEM;
+        }
+
+        priv->regmap = devm_regmap_init_i2c(client, &sensor_regmap_config);
+        if (IS_ERR(priv->regmap)) {
+                dev_err(&client->dev,
+                        "regmap init failed: %ld\n", PTR_ERR(priv->regmap));
+                return -ENODEV;
+        }
+
+        priv->pdata = imx214_parse_dt(client);
+        if (!priv->pdata) {
+                dev_err(&client->dev, "unable to get platform data\n");
+                return -EFAULT;
+        }
+
+        common_data->ops                = &imx214_common_ops;
+        common_data->ctrl_handler       = &priv->ctrl_handler;
+        common_data->i2c_client         = client;
+        common_data->frmfmt             = &imx214_frmfmt[0];
+        common_data->colorfmt           = camera_common_find_datafmt(
+                                          IMX214_DEFAULT_DATAFMT);
+        common_data->ctrls              = priv->ctrls;
+        common_data->power              = &priv->power;
+        common_data->priv               = (void *)priv;
+        common_data->numctrls           = ARRAY_SIZE(ctrl_config_list);
+        common_data->numfmts            = ARRAY_SIZE(imx214_frmfmt);
+        common_data->def_mode           = IMX214_DEFAULT_MODE;
+        common_data->def_width          = IMX214_DEFAULT_WIDTH;
+        common_data->def_height         = IMX214_DEFAULT_HEIGHT;
+        common_data->fmt_width          = common_data->def_width;
+        common_data->fmt_height         = common_data->def_height;
+        common_data->def_clk_freq       = IMX214_DEFAULT_CLK_FREQ;
+
+        priv->i2c_client                = client;
+        priv->s_data                    = common_data;
+        priv->subdev                    = &common_data->subdev;
+        priv->subdev->dev               = &client->dev;
+        priv->s_data->dev               = &client->dev;
+
+        err = imx214_power_get(priv);
+        if (err)
+                return err;
+
+        err = camera_common_parse_ports(client, common_data);
+        if (err) {
+                dev_err(&client->dev, "Failed to find port info\n");
+                return err;
+        }
+        sprintf(debugfs_name, "imx214_%c", common_data->csi_port + 'a');
+        dev_dbg(&client->dev, "%s: name %s\n", __func__, debugfs_name);
+        camera_common_create_debugfs(common_data, debugfs_name);
+
+        v4l2_i2c_subdev_init(priv->subdev, client, &imx214_subdev_ops);
+
+        err = imx214_ctrls_init(priv);
+        if (err)
+                return err;
+
+        /* eeprom interface */
+        err = imx214_eeprom_device_init(priv);
+        if (err)
+                dev_err(&client->dev,
+                        "Failed to allocate eeprom register map: %d\n", err);
+
+        priv->subdev->internal_ops = &imx214_subdev_internal_ops;
+        priv->subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+                     V4L2_SUBDEV_FL_HAS_EVENTS;
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+        priv->subdev->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+        priv->subdev->entity.ops = &imx214_media_ops;
+        err = media_entity_init(&priv->subdev->entity, 1, &priv->pad, 0);
+        if (err < 0) {
+                dev_err(&client->dev, "unable to init media entity\n");
+                return err;
+        }
+#endif
+
+        err = v4l2_async_register_subdev(priv->subdev);
+        if (err)
+                return err;
+
+        dev_dbg(&client->dev, "Detected IMX214 sensor\n");
+
+        return 0;
+}
+
+static int
+imx214_remove(struct i2c_client *client)
+{
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx214 *priv = (struct imx214 *)s_data->priv;
+
+        v4l2_async_unregister_subdev(priv->subdev);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev->entity);
+#endif
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        imx214_power_put(priv);
+        camera_common_remove_debugfs(s_data);
+
+        return 0;
+}
+
+static const struct i2c_device_id imx214_id[] = {
+        { "imx214", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, imx214_id);
+
+static struct i2c_driver imx214_i2c_driver = {
+        .driver = {
+                .name = "imx214",
+                .owner = THIS_MODULE,
+                .of_match_table = of_match_ptr(imx214_of_match),
+        },
+        .probe = imx214_probe,
+        .remove = imx214_remove,
+        .id_table = imx214_id,
+};
+
+module_i2c_driver(imx214_i2c_driver);
+
+MODULE_DESCRIPTION("SoC Camera driver for Sony IMX214");
+MODULE_AUTHOR("David Wang <davidw@nvidia.com>");
+MODULE_LICENSE("GPL v2");
+
diff --git a/drivers/media/i2c/imx219.c b/drivers/media/i2c/imx219.c
new file mode 100644
index 000000000..ba36dc721
--- /dev/null
+++ b/drivers/media/i2c/imx219.c
@@ -0,0 +1,873 @@
+/*
+ * imx219.c - imx219 sensor driver
+ *
+ * Copyright (c) 2015-2016, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <dt-bindings/gpio/tegra-gpio.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+
+#include <linux/seq_file.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <media/camera_common.h>
+#include <media/soc_camera.h>
+#include <media/imx219.h>
+
+#include "imx219_mode_tbls.h"
+
+#define IMX219_MAX_COARSE_DIFF          4
+
+#define IMX219_GAIN_SHIFT               8
+#define IMX219_MIN_GAIN         (1 << IMX219_GAIN_SHIFT)
+#define IMX219_MAX_GAIN         (16 << IMX219_GAIN_SHIFT)
+#define IMX219_MIN_FRAME_LENGTH (0x9C3)
+#define IMX219_MAX_FRAME_LENGTH (0xFFFF)
+#define IMX219_MIN_EXPOSURE_COARSE      (0x0001)
+#define IMX219_MAX_EXPOSURE_COARSE      \
+        (IMX219_MAX_FRAME_LENGTH-IMX219_MAX_COARSE_DIFF)
+
+#define IMX219_DEFAULT_GAIN             IMX219_MIN_GAIN
+#define IMX219_DEFAULT_FRAME_LENGTH     (0x09C3)
+#define IMX219_DEFAULT_EXPOSURE_COARSE  \
+        (IMX219_DEFAULT_FRAME_LENGTH-IMX219_MAX_COARSE_DIFF)
+
+#define IMX219_DEFAULT_MODE     IMX219_MODE_3280x2464
+#define IMX219_DEFAULT_WIDTH    3280
+#define IMX219_DEFAULT_HEIGHT   2464
+#define IMX219_DEFAULT_DATAFMT  MEDIA_BUS_FMT_SRGGB10_1X10
+#define IMX219_DEFAULT_CLK_FREQ 12000000
+
+struct imx219 {
+        struct camera_common_power_rail power;
+        int                             num_ctrls;
+        struct v4l2_ctrl_handler        ctrl_handler;
+        struct i2c_client               *i2c_client;
+        struct v4l2_subdev              *subdev;
+        struct media_pad                pad;
+        struct regmap                   *regmap;
+        struct camera_common_data       *s_data;
+        struct camera_common_pdata      *pdata;
+        struct v4l2_ctrl                *ctrls[];
+};
+
+static const struct regmap_config sensor_regmap_config = {
+        .reg_bits = 16,
+        .val_bits = 8,
+        .cache_type = REGCACHE_RBTREE,
+};
+
+static int imx219_g_volatile_ctrl(struct v4l2_ctrl *ctrl);
+static int imx219_s_ctrl(struct v4l2_ctrl *ctrl);
+
+static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
+        .g_volatile_ctrl = imx219_g_volatile_ctrl,
+        .s_ctrl = imx219_s_ctrl,
+};
+
+static struct v4l2_ctrl_config ctrl_config_list[] = {
+/* Do not change the name field for the controls! */
+        {
+                .ops = &imx219_ctrl_ops,
+                .id = V4L2_CID_GAIN,
+                .name = "Gain",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX219_MIN_GAIN,
+                .max = IMX219_MAX_GAIN,
+                .def = IMX219_DEFAULT_GAIN,
+                .step = 1,
+        },
+        {
+                .ops = &imx219_ctrl_ops,
+                .id = V4L2_CID_FRAME_LENGTH,
+                .name = "Frame Length",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX219_MIN_FRAME_LENGTH,
+                .max = IMX219_MAX_FRAME_LENGTH,
+                .def = IMX219_DEFAULT_FRAME_LENGTH,
+                .step = 1,
+        },
+        {
+                .ops = &imx219_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME,
+                .name = "Coarse Time",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX219_MIN_EXPOSURE_COARSE,
+                .max = IMX219_MAX_EXPOSURE_COARSE,
+                .def = IMX219_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &imx219_ctrl_ops,
+                .id = V4L2_CID_GROUP_HOLD,
+                .name = "Group Hold",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &imx219_ctrl_ops,
+                .id = V4L2_CID_HDR_EN,
+                .name = "HDR enable",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &imx219_ctrl_ops,
+                .id = V4L2_CID_FUSE_ID,
+                .name = "Fuse ID",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = IMX219_FUSE_ID_STR_SIZE,
+                .step = 2,
+        },
+};
+
+static inline void imx219_get_gain_reg(struct reg_8 *regs,
+                                u8 gain)
+{
+        regs->addr = IMX219_GAIN_ADDR;
+        regs->val = gain & 0xff;
+}
+
+static int test_mode;
+module_param(test_mode, int, 0644);
+
+static inline int imx219_read_reg(struct camera_common_data *s_data,
+                                u16 addr, u8 *val)
+{
+        struct imx219 *priv = (struct imx219 *)s_data->priv;
+        return regmap_read(priv->regmap, addr, (unsigned int *) val);
+}
+
+static int imx219_write_reg(struct camera_common_data *s_data, u16 addr, u8 val)
+{
+        int err;
+        struct imx219 *priv = (struct imx219 *)s_data->priv;
+
+        err = regmap_write(priv->regmap, addr, val);
+        if (err)
+                pr_err("%s:i2c write failed, %x = %x\n",
+                        __func__, addr, val);
+
+        return err;
+}
+
+static int imx219_write_table(struct imx219 *priv,
+                              const struct reg_8 table[])
+{
+        return regmap_util_write_table_8(priv->regmap,
+                                         table,
+                                         NULL, 0,
+                                         IMX219_TABLE_WAIT_MS,
+                                         IMX219_TABLE_END);
+}
+
+static void imx219_mclk_disable(struct camera_common_power_rail *pw)
+{
+        clk_disable_unprepare(pw->mclk);
+}
+
+static void imx219_mclk_enable(struct camera_common_power_rail *pw)
+{
+        clk_set_rate(pw->mclk, IMX219_DEFAULT_CLK_FREQ);
+        clk_prepare_enable(pw->mclk);
+}
+
+static int imx219_power_on(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct imx219 *priv = (struct imx219 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power on\n", __func__);
+
+        if (gpio_cansleep(pw->reset_gpio))
+                gpio_set_value_cansleep(pw->reset_gpio, 0);
+        else
+                gpio_set_value(pw->reset_gpio, 0);
+        usleep_range(10, 20);
+
+        if (pw->avdd)
+                err = regulator_enable(pw->avdd);
+        if (err)
+                goto imx219_avdd_fail;
+
+        if (pw->iovdd)
+                err = regulator_enable(pw->iovdd);
+        if (err)
+                goto imx219_iovdd_fail;
+
+        if (pw->dvdd)
+                err = regulator_enable(pw->dvdd);
+        if (err)
+                goto imx219_dvdd_fail;
+
+        usleep_range(1, 2);
+        if (gpio_cansleep(pw->reset_gpio))
+                gpio_set_value_cansleep(pw->reset_gpio, 1);
+        else
+                gpio_set_value(pw->reset_gpio, 1);
+
+        usleep_range(1, 2);
+        imx219_mclk_enable(pw);
+
+        /* Need to wait for t4 + t5 + t9 time as per the data sheet */
+        /* t4 - 200us, t5 - 6ms, t9 - 1.2ms */
+        usleep_range(7400, 7410);
+
+        pw->state = SWITCH_ON;
+        return 0;
+
+imx219_dvdd_fail:
+        regulator_disable(pw->iovdd);
+
+imx219_iovdd_fail:
+        regulator_disable(pw->avdd);
+
+imx219_avdd_fail:
+        return -ENODEV;
+}
+
+static int imx219_power_off(struct camera_common_data *s_data)
+{
+        struct imx219 *priv = (struct imx219 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power off\n", __func__);
+
+        usleep_range(1, 2);
+        if (gpio_cansleep(pw->reset_gpio))
+                gpio_set_value_cansleep(pw->reset_gpio, 0);
+        else
+                gpio_set_value(pw->reset_gpio, 0);
+        usleep_range(1, 2);
+
+        if (pw->dvdd)
+                regulator_disable(pw->dvdd);
+        if (pw->iovdd)
+                regulator_disable(pw->iovdd);
+        if (pw->avdd)
+                regulator_disable(pw->avdd);
+
+        imx219_mclk_disable(pw);
+        pw->state = SWITCH_OFF;
+        return 0;
+}
+
+static int imx219_power_put(struct imx219 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        if (unlikely(!pw))
+                return -EFAULT;
+
+        if (likely(pw->avdd))
+                regulator_put(pw->avdd);
+
+        if (likely(pw->iovdd))
+                regulator_put(pw->iovdd);
+
+        if (likely(pw->dvdd))
+                regulator_put(pw->dvdd);
+
+        pw->avdd = NULL;
+        pw->iovdd = NULL;
+        pw->dvdd = NULL;
+
+        return 0;
+}
+
+static int imx219_power_get(struct imx219 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        struct camera_common_pdata *pdata = priv->pdata;
+        const char *mclk_name;
+        int err = 0;
+
+        mclk_name = priv->pdata->mclk_name ?
+                    priv->pdata->mclk_name : "cam_mclk1";
+        pw->mclk = devm_clk_get(&priv->i2c_client->dev, mclk_name);
+        if (IS_ERR(pw->mclk)) {
+                dev_err(&priv->i2c_client->dev,
+                        "unable to get clock %s\n", mclk_name);
+                return PTR_ERR(pw->mclk);
+        }
+
+        /* ananlog 2.7v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->avdd, pdata->regulators.avdd);
+        /* digital 1.2v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->dvdd, pdata->regulators.dvdd);
+        /* IO 1.8v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->iovdd, pdata->regulators.iovdd);
+
+        if (!err)
+                pw->reset_gpio = pdata->reset_gpio;
+
+        pw->state = SWITCH_OFF;
+        return err;
+}
+
+static int imx219_set_gain(struct imx219 *priv, s32 val);
+static int imx219_set_frame_length(struct imx219 *priv, s32 val);
+static int imx219_set_coarse_time(struct imx219 *priv, s32 val);
+
+static int imx219_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx219 *priv = (struct imx219 *)s_data->priv;
+        int err;
+
+        dev_dbg(&client->dev, "%s\n", __func__);
+
+        if (!enable)
+                return 0;
+
+        err = imx219_write_table(priv, mode_table[s_data->mode]);
+        if (err)
+                goto exit;
+
+        return 0;
+exit:
+        dev_dbg(&client->dev, "%s: error setting stream\n", __func__);
+        return err;
+}
+
+static int imx219_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx219 *priv = (struct imx219 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        *status = pw->state == SWITCH_ON;
+        return 0;
+}
+
+static int imx219_get_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        return camera_common_g_fmt(sd, &format->format);
+}
+
+static int imx219_set_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        int ret;
+
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+                ret = camera_common_try_fmt(sd, &format->format);
+        else
+                ret = camera_common_s_fmt(sd, &format->format);
+
+        return ret;
+}
+
+static struct v4l2_subdev_video_ops imx219_subdev_video_ops = {
+        .s_stream       = imx219_s_stream,
+        .g_mbus_config  = camera_common_g_mbus_config,
+        .g_input_status = imx219_g_input_status,
+};
+
+static struct v4l2_subdev_core_ops imx219_subdev_core_ops = {
+        .s_power        = camera_common_s_power,
+};
+
+static struct v4l2_subdev_pad_ops imx219_subdev_pad_ops = {
+        .enum_mbus_code = camera_common_enum_mbus_code,
+        .set_fmt = imx219_set_fmt,
+        .get_fmt = imx219_get_fmt,
+        .enum_frame_size        = camera_common_enum_framesizes,
+        .enum_frame_interval    = camera_common_enum_frameintervals,
+};
+
+static struct v4l2_subdev_ops imx219_subdev_ops = {
+        .core   = &imx219_subdev_core_ops,
+        .video  = &imx219_subdev_video_ops,
+        .pad    = &imx219_subdev_pad_ops,
+};
+
+static struct of_device_id imx219_of_match[] = {
+        { .compatible = "nvidia,imx219", },
+        { },
+};
+
+static struct camera_common_sensor_ops imx219_common_ops = {
+        .power_on = imx219_power_on,
+        .power_off = imx219_power_off,
+        .write_reg = imx219_write_reg,
+        .read_reg = imx219_read_reg,
+};
+
+static int imx219_set_group_hold(struct imx219 *priv, s32 val)
+{
+        /* IMX219 does not support group hold */
+        return 0;
+}
+
+static int imx219_set_gain(struct imx219 *priv, s32 val)
+{
+        struct reg_8 reg;
+        int err;
+        u8 gain;
+
+        /* translate value */
+        gain = 256 - (256 * (1 << IMX219_GAIN_SHIFT) / val);
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, gain);
+
+        imx219_get_gain_reg(&reg, gain);
+
+        err = imx219_write_reg(priv->s_data, reg.addr, reg.val);
+        if (err)
+                goto fail;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: GAIN control error\n", __func__);
+        return err;
+}
+
+static int imx219_set_frame_length(struct imx219 *priv, s32 val)
+{
+        u8 data[2];
+        int err;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, val);
+
+        data[0] = (val >> 8) & 0xff;
+        data[1] = val & 0xff;
+        err = regmap_raw_write(priv->regmap, IMX219_FRAME_LENGTH_ADDR_MSB,
+                data, 2);
+        if (err)
+                goto fail;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: FRAME_LENGTH control error\n", __func__);
+        return err;
+}
+
+static int imx219_set_coarse_time(struct imx219 *priv, s32 val)
+{
+        u8 data[2];
+        int err;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, val);
+
+        data[0] = (val >> 8) & 0xff;
+        data[1] = val & 0xff;
+        err = regmap_raw_write(priv->regmap, IMX219_COARSE_TIME_ADDR_MSB,
+                data, 2);
+        if (err)
+                goto fail;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: COARSE_TIME control error\n", __func__);
+        return err;
+}
+
+static int imx219_verify_streaming(struct imx219 *priv)
+{
+        int err = 0;
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return err;
+
+        err = imx219_s_stream(priv->subdev, true);
+        if (err)
+                goto error;
+
+error:
+        imx219_s_stream(priv->subdev, false);
+        camera_common_s_power(priv->subdev, false);
+
+        return err;
+}
+
+static int imx219_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct imx219 *priv =
+                container_of(ctrl->handler, struct imx219, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        default:
+                pr_err("%s: unknown ctrl id.\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static int imx219_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct imx219 *priv =
+                container_of(ctrl->handler, struct imx219, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_GAIN:
+                err = imx219_set_gain(priv, ctrl->val);
+                break;
+        case V4L2_CID_FRAME_LENGTH:
+                err = imx219_set_frame_length(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME:
+                err = imx219_set_coarse_time(priv, ctrl->val);
+                break;
+        case V4L2_CID_GROUP_HOLD:
+                err = imx219_set_group_hold(priv, ctrl->val);
+                break;
+        case V4L2_CID_HDR_EN:
+                break;
+        case V4L2_CID_FUSE_ID:
+                break;
+        default:
+                pr_err("%s: unknown ctrl id.\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static int imx219_ctrls_init(struct imx219 *priv)
+{
+        struct i2c_client *client = priv->i2c_client;
+        struct v4l2_ctrl *ctrl;
+        int num_ctrls;
+        int err;
+        int i;
+
+        dev_info(&client->dev, "%s++\n", __func__);
+
+        num_ctrls = ARRAY_SIZE(ctrl_config_list);
+        v4l2_ctrl_handler_init(&priv->ctrl_handler, num_ctrls);
+
+        for (i = 0; i < num_ctrls; i++) {
+                ctrl = v4l2_ctrl_new_custom(&priv->ctrl_handler,
+                        &ctrl_config_list[i], NULL);
+                if (ctrl == NULL) {
+                        dev_err(&client->dev, "Failed to init %s ctrl\n",
+                                ctrl_config_list[i].name);
+                        continue;
+                }
+
+                if (ctrl_config_list[i].type == V4L2_CTRL_TYPE_STRING &&
+                        ctrl_config_list[i].flags & V4L2_CTRL_FLAG_READ_ONLY) {
+                        ctrl->p_new.p_char = devm_kzalloc(&client->dev,
+                                ctrl_config_list[i].max + 1, GFP_KERNEL);
+                        if (!ctrl->p_new.p_char) {
+                                dev_err(&client->dev,
+                                        "Failed to allocate otp data\n");
+                                return -ENOMEM;
+                        }
+                }
+                priv->ctrls[i] = ctrl;
+        }
+
+        priv->num_ctrls = num_ctrls;
+        priv->subdev->ctrl_handler = &priv->ctrl_handler;
+        if (priv->ctrl_handler.error) {
+                dev_err(&client->dev, "Error %d adding controls\n",
+                        priv->ctrl_handler.error);
+                err = priv->ctrl_handler.error;
+                goto error;
+        }
+
+        err = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d setting default controls\n", err);
+                goto error;
+        }
+
+        return 0;
+
+error:
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        return err;
+}
+
+MODULE_DEVICE_TABLE(of, imx219_of_match);
+
+static struct camera_common_pdata *imx219_parse_dt(struct i2c_client *client)
+{
+        struct device_node *np = client->dev.of_node;
+        struct camera_common_pdata *board_priv_pdata;
+        const struct of_device_id *match;
+        int gpio, err;
+        struct camera_common_pdata *ret = NULL;
+
+        match = of_match_device(imx219_of_match, &client->dev);
+        if (!match) {
+                dev_err(&client->dev, "Failed to find matching dt id\n");
+                return NULL;
+        }
+
+        board_priv_pdata = devm_kzalloc(&client->dev,
+                           sizeof(*board_priv_pdata), GFP_KERNEL);
+        if (!board_priv_pdata) {
+                dev_err(&client->dev, "Failed to allocate pdata\n");
+                return NULL;
+        }
+
+        err = of_property_read_string(np, "mclk", &board_priv_pdata->mclk_name);
+        if (err) {
+                dev_err(&client->dev, "mclk not in DT\n");
+                goto error;
+        }
+
+        gpio = of_get_named_gpio(np, "reset-gpios", 0);
+        if (gpio < 0) {
+                if (gpio == -EPROBE_DEFER) {
+                        ret = ERR_PTR(-EPROBE_DEFER);
+                        goto error;
+                }
+                dev_err(&client->dev, "reset gpios not in DT\n");
+                goto error;
+        }
+        board_priv_pdata->reset_gpio = (unsigned int)gpio;
+
+        err = of_property_read_string(np, "avdd-reg",
+                        &board_priv_pdata->regulators.avdd);
+        if (err) {
+                dev_err(&client->dev, "avdd-reg not in DT\n");
+                goto error;
+        }
+        err = of_property_read_string(np, "dvdd-reg",
+                        &board_priv_pdata->regulators.dvdd);
+        if (err) {
+                dev_err(&client->dev, "dvdd-reg not in DT\n");
+                goto error;
+        }
+        err = of_property_read_string(np, "iovdd-reg",
+                        &board_priv_pdata->regulators.iovdd);
+        if (err) {
+                dev_err(&client->dev, "iovdd-reg not in DT\n");
+                goto error;
+        }
+
+        return board_priv_pdata;
+
+error:
+        devm_kfree(&client->dev, board_priv_pdata);
+        return ret;
+}
+
+static int imx219_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        dev_dbg(&client->dev, "%s:\n", __func__);
+
+        return 0;
+}
+
+static const struct v4l2_subdev_internal_ops imx219_subdev_internal_ops = {
+        .open = imx219_open,
+};
+
+static const struct media_entity_operations imx219_media_ops = {
+        .link_validate = v4l2_subdev_link_validate,
+};
+
+static int imx219_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct camera_common_data *common_data;
+        struct device_node *node = client->dev.of_node;
+        struct imx219 *priv;
+        int err;
+        char debugfs_name[32];
+
+        if (!IS_ENABLED(CONFIG_OF) || !node)
+                return -EINVAL;
+
+        common_data = devm_kzalloc(&client->dev,
+                            sizeof(struct camera_common_data), GFP_KERNEL);
+        if (!common_data) {
+                dev_err(&client->dev, "unable to allocate memory!\n");
+                return -ENOMEM;
+        }
+
+        priv = devm_kzalloc(&client->dev,
+                            sizeof(struct imx219) + sizeof(struct v4l2_ctrl *) *
+                            ARRAY_SIZE(ctrl_config_list),
+                            GFP_KERNEL);
+        if (!priv) {
+                dev_err(&client->dev, "unable to allocate memory!\n");
+                return -ENOMEM;
+        }
+
+        priv->regmap = devm_regmap_init_i2c(client, &sensor_regmap_config);
+        if (IS_ERR(priv->regmap)) {
+                dev_err(&client->dev,
+                        "regmap init failed: %ld\n", PTR_ERR(priv->regmap));
+                return -ENODEV;
+        }
+
+        priv->pdata = imx219_parse_dt(client);
+        if (PTR_ERR(priv->pdata) == -EPROBE_DEFER) {
+                devm_kfree(&client->dev, priv);
+                return -EPROBE_DEFER;
+        }
+        if (!priv->pdata) {
+                dev_err(&client->dev, "unable to get platform data\n");
+                return -EFAULT;
+        }
+
+        common_data->ops                = &imx219_common_ops;
+        common_data->ctrl_handler       = &priv->ctrl_handler;
+        common_data->i2c_client         = client;
+        common_data->frmfmt             = &imx219_frmfmt[0];
+        common_data->colorfmt           = camera_common_find_datafmt(
+                                          IMX219_DEFAULT_DATAFMT);
+        common_data->power              = &priv->power;
+        common_data->ctrls              = priv->ctrls;
+        common_data->priv               = (void *)priv;
+        common_data->numctrls           = ARRAY_SIZE(ctrl_config_list);
+        common_data->numfmts            = ARRAY_SIZE(imx219_frmfmt);
+        common_data->def_mode           = IMX219_DEFAULT_MODE;
+        common_data->def_width          = IMX219_DEFAULT_WIDTH;
+        common_data->def_height         = IMX219_DEFAULT_HEIGHT;
+        common_data->fmt_width          = common_data->def_width;
+        common_data->fmt_height         = common_data->def_height;
+        common_data->def_clk_freq       = IMX219_DEFAULT_CLK_FREQ;
+
+        priv->i2c_client                = client;
+        priv->s_data                    = common_data;
+        priv->subdev                    = &common_data->subdev;
+        priv->subdev->dev               = &client->dev;
+
+        err = imx219_power_get(priv);
+        if (err)
+                return err;
+
+        snprintf(debugfs_name, sizeof(debugfs_name), "%s.%s",
+                dev_driver_string(&client->dev), dev_name(&client->dev));
+        camera_common_create_debugfs(common_data, debugfs_name);
+
+        v4l2_i2c_subdev_init(&common_data->subdev, client, &imx219_subdev_ops);
+
+        err = imx219_ctrls_init(priv);
+        if (err)
+                return err;
+
+        err = imx219_verify_streaming(priv);
+        if (err)
+                return err;
+
+        priv->subdev->internal_ops = &imx219_subdev_internal_ops;
+        priv->subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+                     V4L2_SUBDEV_FL_HAS_EVENTS;
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+        priv->subdev->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+        priv->subdev->entity.ops = &imx219_media_ops;
+        err = media_entity_init(&priv->subdev->entity, 1, &priv->pad, 0);
+        if (err < 0) {
+                dev_err(&client->dev, "unable to init media entity\n");
+                return err;
+        }
+#endif
+
+        err = v4l2_async_register_subdev(priv->subdev);
+        if (err)
+                return err;
+
+        dev_dbg(&client->dev, "Detected IMX219 sensor\n");
+
+        return 0;
+}
+
+static int
+imx219_remove(struct i2c_client *client)
+{
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx219 *priv = (struct imx219 *)s_data->priv;
+
+        v4l2_async_unregister_subdev(priv->subdev);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev->entity);
+#endif
+
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        imx219_power_put(priv);
+        camera_common_remove_debugfs(s_data);
+
+        return 0;
+}
+
+static const struct i2c_device_id imx219_id[] = {
+        { "imx219", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, imx219_id);
+
+static struct i2c_driver imx219_i2c_driver = {
+        .driver = {
+                .name = "imx219",
+                .owner = THIS_MODULE,
+                .of_match_table = of_match_ptr(imx219_of_match),
+        },
+        .probe = imx219_probe,
+        .remove = imx219_remove,
+        .id_table = imx219_id,
+};
+
+module_i2c_driver(imx219_i2c_driver);
+
+MODULE_DESCRIPTION("SoC Camera driver for Sony IMX219");
+MODULE_AUTHOR("Bryan Wu <pengw@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx219_mode_tbls.h b/drivers/media/i2c/imx219_mode_tbls.h
new file mode 100644
index 000000000..8bf129ffe
--- /dev/null
+++ b/drivers/media/i2c/imx219_mode_tbls.h
@@ -0,0 +1,388 @@
+/*
+ * imx219_tables.h - sensor mode tables for imx219 HDR sensor.
+ *
+ * Copyright (c) 2015-2016, NVIDIA CORPORATION, All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef IMX219_I2C_TABLES
+#define IMX219_I2C_TABLES
+
+#define IMX219_TABLE_WAIT_MS 0
+#define IMX219_TABLE_END 1
+#define IMX219_MAX_RETRIES 3
+#define IMX219_WAIT_MS 3
+
+static struct reg_8 mode_3280x2464[] = {
+        {IMX219_TABLE_WAIT_MS, 10},
+        /* software reset */
+        {0x0103, 0x01},
+        /* global settings */
+        {0x30EB, 0x05},
+        {0x30EB, 0x0C},
+        {0x300A, 0xFF},
+        {0x300B, 0xFF},
+        {0x30EB, 0x05},
+        {0x30EB, 0x09},
+        {0x0114, 0x01},
+        {0x0128, 0x00},
+        {0x012A, 0x0C},
+        {0x012B, 0x00},
+        {0x0160, 0x09},
+        {0x0161, 0xC3},
+        {0x0162, 0x0D},
+        {0x0163, 0x78},
+        {0x0164, 0x00},
+        {0x0165, 0x00},
+        {0x0166, 0x0C},
+        {0x0167, 0xCF},
+        {0x0168, 0x00},
+        {0x0169, 0x00},
+        {0x016A, 0x09},
+        {0x016B, 0x9F},
+        {0x016C, 0x0C},
+        {0x016D, 0xD0},
+        {0x016E, 0x09},
+        {0x016F, 0xA0},
+        {0x0170, 0x01},
+        {0x0171, 0x01},
+        {0x0174, 0x00},
+        {0x0175, 0x00},
+        {0x018C, 0x0A},
+        {0x018D, 0x0A},
+        {0x0301, 0x05},
+        {0x0303, 0x01},
+        {0x0304, 0x02},
+        {0x0305, 0x02},
+        {0x0306, 0x00},
+        {0x0307, 0x4C},
+        {0x0309, 0x0A},
+        {0x030B, 0x01},
+        {0x030C, 0x00},
+        {0x030D, 0x98},
+        {0x4767, 0x0F},
+        {0x4750, 0x14},
+        {0x47B4, 0x14},
+        /* stream on */
+        {0x0100, 0x01},
+        {IMX219_TABLE_WAIT_MS, IMX219_WAIT_MS},
+        {IMX219_TABLE_END, 0x00}
+};
+
+static struct reg_8 mode_3280x2460[] = {
+        {IMX219_TABLE_WAIT_MS, 10},
+        /* software reset */
+        {0x0103, 0x01},
+        /* global settings */
+        {0x30EB, 0x05},
+        {0x30EB, 0x0C},
+        {0x300A, 0xFF},
+        {0x300B, 0xFF},
+        {0x30EB, 0x05},
+        {0x30EB, 0x09},
+        {0x0114, 0x03},
+        {0x0128, 0x00},
+        {0x012A, 0x18},
+        {0x012B, 0x00},
+        /* Bank A Settings */
+        {0x0157, 0x00},
+        {0x015A, 0x08},
+        {0x015B, 0x8F},
+        {0x0160, 0x0A},
+        {0x0161, 0x83},
+        {0x0162, 0x0D},
+        {0x0163, 0x78},
+        {0x0164, 0x00},
+        {0x0165, 0x00},
+        {0x0166, 0x0C},
+        {0x0167, 0xCF},
+        {0x0168, 0x00},
+        {0x0169, 0x00},
+        {0x016A, 0x09},
+        {0x016B, 0x9F},
+        {0x016C, 0x0C},
+        {0x016D, 0xD0},
+        {0x016E, 0x09},
+        {0x016F, 0x9C},
+        {0x0170, 0x01},
+        {0x0171, 0x01},
+        {0x0174, 0x00},
+        {0x0175, 0x00},
+        {0x018C, 0x0A},
+        {0x018D, 0x0A},
+        /* Bank B Settings */
+        {0x0257, 0x00},
+        {0x025A, 0x08},
+        {0x025B, 0x8F},
+        {0x0260, 0x0A},
+        {0x0261, 0x83},
+        {0x0262, 0x0D},
+        {0x0263, 0x78},
+        {0x0264, 0x00},
+        {0x0265, 0x00},
+        {0x0266, 0x0C},
+        {0x0267, 0xCF},
+        {0x0268, 0x00},
+        {0x0269, 0x00},
+        {0x026A, 0x09},
+        {0x026B, 0x9F},
+        {0x026C, 0x0C},
+        {0x026D, 0xD0},
+        {0x026E, 0x09},
+        {0x026F, 0x9C},
+        {0x0270, 0x01},
+        {0x0271, 0x01},
+        {0x0274, 0x00},
+        {0x0275, 0x00},
+        {0x028C, 0x0A},
+        {0x028D, 0x0A},
+        /* clock setting */
+        {0x0301, 0x05},
+        {0x0303, 0x01},
+        {0x0304, 0x03},
+        {0x0305, 0x03},
+        {0x0306, 0x00},
+        {0x0307, 0x57},
+        {0x0309, 0x0A},
+        {0x030B, 0x01},
+        {0x030C, 0x00},
+        {0x030D, 0x5A},
+        {0x455E, 0x00},
+        {0x471E, 0x4B},
+        {0x4767, 0x0F},
+        {0x4750, 0x14},
+        {0x4540, 0x00},
+        {0x47B4, 0x14},
+        {0x4713, 0x30},
+        {0x478B, 0x10},
+        {0x478F, 0x10},
+        {0x4793, 0x10},
+        {0x4797, 0x0E},
+        {0x479B, 0x0E},
+        /* stream on */
+        {0x0100, 0x01},
+        {IMX219_TABLE_WAIT_MS, IMX219_WAIT_MS},
+        {IMX219_TABLE_END, 0x00}
+};
+static struct reg_8 mode_3280x1846[] = {
+        {IMX219_TABLE_WAIT_MS, 10},
+        /* software reset */
+        {0x0103, 0x01},
+        /* global settings */
+        {0x30EB, 0x05},
+        {0x30EB, 0x0C},
+        {0x300A, 0xFF},
+        {0x300B, 0xFF},
+        {0x30EB, 0x05},
+        {0x30EB, 0x09},
+        {0x0114, 0x03},
+        {0x0128, 0x00},
+        {0x012A, 0x18},
+        {0x012B, 0x00},
+        /* Bank A Settings */
+        {0x0157, 0x00},
+        {0x015A, 0x08},
+        {0x015B, 0x8F},
+        {0x0160, 0x07},
+        {0x0161, 0x5E},
+        {0x0162, 0x0D},
+        {0x0163, 0x78},
+        {0x0164, 0x00},
+        {0x0165, 0x00},
+        {0x0166, 0x0C},
+        {0x0167, 0xCF},
+        {0x0168, 0x01},
+        {0x0169, 0x36},
+        {0x016A, 0x08},
+        {0x016B, 0x6B},
+        {0x016C, 0x0C},
+        {0x016D, 0xD0},
+        {0x016E, 0x07},
+        {0x016F, 0x36},
+        {0x0170, 0x01},
+        {0x0171, 0x01},
+        {0x0174, 0x00},
+        {0x0175, 0x00},
+        {0x018C, 0x0A},
+        {0x018D, 0x0A},
+        /* Bank B Settings */
+        {0x0257, 0x00},
+        {0x025A, 0x08},
+        {0x025B, 0x8F},
+        {0x0260, 0x07},
+        {0x0261, 0x5E},
+        {0x0262, 0x0D},
+        {0x0263, 0x78},
+        {0x0264, 0x00},
+        {0x0265, 0x00},
+        {0x0266, 0x0C},
+        {0x0267, 0xCF},
+        {0x0268, 0x01},
+        {0x0269, 0x36},
+        {0x026A, 0x08},
+        {0x026B, 0x6B},
+        {0x026C, 0x0C},
+        {0x026D, 0xD0},
+        {0x026E, 0x07},
+        {0x026F, 0x36},
+        {0x0270, 0x01},
+        {0x0271, 0x01},
+        {0x0274, 0x00},
+        {0x0275, 0x00},
+        {0x028C, 0x0A},
+        {0x028D, 0x0A},
+        /* clock setting */
+        {0x0301, 0x05},
+        {0x0303, 0x01},
+        {0x0304, 0x03},
+        {0x0305, 0x03},
+        {0x0306, 0x00},
+        {0x0307, 0x57},
+        {0x0309, 0x0A},
+        {0x030B, 0x01},
+        {0x030C, 0x00},
+        {0x030D, 0x5A},
+        {0x455E, 0x00},
+        {0x471E, 0x4B},
+        {0x4767, 0x0F},
+        {0x4750, 0x14},
+        {0x4540, 0x00},
+        {0x47B4, 0x14},
+        {0x4713, 0x30},
+        {0x478B, 0x10},
+        {0x478F, 0x10},
+        {0x4793, 0x10},
+        {0x4797, 0x0E},
+        {0x479B, 0x0E},
+        /* stream on */
+        {0x0100, 0x01},
+        {IMX219_TABLE_WAIT_MS, IMX219_WAIT_MS},
+        {IMX219_TABLE_END, 0x00}
+};
+
+static struct reg_8 mode_1280x720[] = {
+        {IMX219_TABLE_WAIT_MS, 10},
+        /* software reset */
+        {0x0103, 0x01},
+        /* global settings */
+        {0x30EB, 0x05},
+        {0x30EB, 0x0C},
+        {0x300A, 0xFF},
+        {0x300B, 0xFF},
+        {0x30EB, 0x05},
+        {0x30EB, 0x09},
+        {0x0114, 0x03},
+        {0x0128, 0x00},
+        {0x012A, 0x18},
+        {0x012B, 0x00},
+        /* Bank A Settings */
+        {0x0160, 0x02},
+        {0x0161, 0x8C},
+        {0x0162, 0x0D},
+        {0x0163, 0xE8},
+        {0x0164, 0x01},
+        {0x0165, 0x68},
+        {0x0166, 0x0B},
+        {0x0167, 0x67},
+        {0x0168, 0x02},
+        {0x0169, 0x00},
+        {0x016A, 0x07},
+        {0x016B, 0x9F},
+        {0x016C, 0x05},
+        {0x016D, 0x00},
+        {0x016E, 0x02},
+        {0x016F, 0xD0},
+        {0x0170, 0x01},
+        {0x0171, 0x01},
+        {0x0174, 0x03},
+        {0x0175, 0x03},
+        {0x018C, 0x0A},
+        {0x018D, 0x0A},
+        /* Bank B Settings */
+        {0x0260, 0x02},
+        {0x0261, 0x8C},
+        {0x0262, 0x0D},
+        {0x0263, 0xE8},
+        {0x0264, 0x01},
+        {0x0265, 0x68},
+        {0x0266, 0x0B},
+        {0x0267, 0x67},
+        {0x0268, 0x02},
+        {0x0269, 0x00},
+        {0x026A, 0x07},
+        {0x026B, 0x9F},
+        {0x026C, 0x05},
+        {0x026D, 0x00},
+        {0x026E, 0x02},
+        {0x026F, 0xD0},
+        {0x0270, 0x01},
+        {0x0271, 0x01},
+        {0x0274, 0x03},
+        {0x0275, 0x03},
+        {0x028C, 0x0A},
+        {0x028D, 0x0A},
+        /* clock setting */
+        {0x0301, 0x05},
+        {0x0303, 0x01},
+        {0x0304, 0x03},
+        {0x0305, 0x03},
+        {0x0306, 0x00},
+        {0x0307, 0x57},
+        {0x0309, 0x0A},
+        {0x030B, 0x01},
+        {0x030C, 0x00},
+        {0x030D, 0x5A},
+        {0x455E, 0x00},
+        {0x471E, 0x4B},
+        {0x4767, 0x0F},
+        {0x4750, 0x14},
+        {0x4540, 0x00},
+        {0x47B4, 0x14},
+        {0x4713, 0x30},
+        {0x478B, 0x10},
+        {0x478F, 0x10},
+        {0x4793, 0x10},
+        {0x4797, 0x0E},
+        {0x479B, 0x0E},
+        /* stream on */
+        {0x0100, 0x01},
+        {IMX219_TABLE_WAIT_MS, IMX219_WAIT_MS},
+        {IMX219_TABLE_END, 0x00}
+};
+enum {
+        IMX219_MODE_3280x2464,
+        IMX219_MODE_3280x2460,
+        IMX219_MODE_3280x1846,
+        IMX219_MODE_1280x720,
+};
+
+static struct reg_8 *mode_table[] = {
+        [IMX219_MODE_3280x2464] = mode_3280x2464,
+        [IMX219_MODE_3280x2460] = mode_3280x2460,
+        [IMX219_MODE_3280x1846] = mode_3280x1846,
+        [IMX219_MODE_1280x720]  = mode_1280x720,
+};
+
+static const int imx219_21fps[] = {
+        20,
+};
+
+static const struct camera_common_frmfmt imx219_frmfmt[] = {
+        {{3280, 2464},  imx219_21fps, 1, 0, IMX219_MODE_3280x2464},
+        {{3280, 2460},  imx219_21fps, 1, 0, IMX219_MODE_3280x2460},
+        {{3280, 1846},  imx219_21fps, 1, 0, IMX219_MODE_3280x1846},
+        {{1280, 720},   NULL, 0, 0, IMX219_MODE_1280x720},
+};
+
+#endif
diff --git a/drivers/media/i2c/imx274.c b/drivers/media/i2c/imx274.c
new file mode 100644
index 000000000..5a2382ed8
--- /dev/null
+++ b/drivers/media/i2c/imx274.c
@@ -0,0 +1,1041 @@
+/*
+ * imx274.c - imx274 sensor driver
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+
+#include <linux/seq_file.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <media/camera_common.h>
+#include <media/imx274.h>
+
+#include "imx274_mode_tbls.h"
+
+#define IMX274_MAX_COARSE_DIFF          10
+
+#define IMX274_GAIN_SHIFT               8
+#define IMX274_MIN_GAIN         (1 << IMX274_GAIN_SHIFT)
+#define IMX274_MAX_GAIN         (23 << IMX274_GAIN_SHIFT)
+#define IMX274_MIN_FRAME_LENGTH (0x8ED)
+#define IMX274_MAX_FRAME_LENGTH (0xB292)
+#define IMX274_MIN_EXPOSURE_COARSE      (0x0001)
+#define IMX274_MAX_EXPOSURE_COARSE      \
+        (IMX274_MAX_FRAME_LENGTH-IMX274_MAX_COARSE_DIFF)
+
+#define IMX274_DEFAULT_GAIN             IMX274_MIN_GAIN
+#define IMX274_DEFAULT_FRAME_LENGTH     (0x111B)
+#define IMX274_DEFAULT_EXPOSURE_COARSE  \
+        (IMX274_DEFAULT_FRAME_LENGTH-IMX274_MAX_COARSE_DIFF)
+
+#define IMX274_DEFAULT_MODE     IMX274_MODE_3840X2160
+
+#define IMX274_DEFAULT_WIDTH    3864
+#define IMX274_DEFAULT_HEIGHT   2174
+#define IMX274_DEFAULT_DATAFMT  MEDIA_BUS_FMT_SRGGB10_1X10
+#define IMX274_DEFAULT_CLK_FREQ 24000000
+
+struct imx274 {
+        struct camera_common_power_rail power;
+        int                             num_ctrls;
+        struct v4l2_ctrl_handler        ctrl_handler;
+        struct i2c_client               *i2c_client;
+        struct v4l2_subdev              *subdev;
+        struct media_pad                pad;
+
+        s32                             group_hold_prev;
+        bool                            group_hold_en;
+        struct regmap                   *regmap;
+        struct camera_common_data       *s_data;
+        struct camera_common_pdata      *pdata;
+        struct v4l2_ctrl                *ctrls[];
+};
+
+static const struct regmap_config sensor_regmap_config = {
+        .reg_bits = 16,
+        .val_bits = 8,
+        .cache_type = REGCACHE_RBTREE,
+};
+
+static int imx274_s_ctrl(struct v4l2_ctrl *ctrl);
+
+static const struct v4l2_ctrl_ops imx274_ctrl_ops = {
+        .s_ctrl         = imx274_s_ctrl,
+};
+
+static struct v4l2_ctrl_config ctrl_config_list[] = {
+/* Do not change the name field for the controls! */
+        {
+                .ops = &imx274_ctrl_ops,
+                .id = V4L2_CID_GAIN,
+                .name = "Gain",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX274_MIN_GAIN,
+                .max = IMX274_MAX_GAIN,
+                .def = IMX274_DEFAULT_GAIN,
+                .step = 1,
+        },
+        {
+                .ops = &imx274_ctrl_ops,
+                .id = V4L2_CID_FRAME_LENGTH,
+                .name = "Frame Length",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX274_MIN_FRAME_LENGTH,
+                .max = IMX274_MAX_FRAME_LENGTH,
+                .def = IMX274_DEFAULT_FRAME_LENGTH,
+                .step = 1,
+        },
+        {
+                .ops = &imx274_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME,
+                .name = "Coarse Time",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX274_MIN_EXPOSURE_COARSE,
+                .max = IMX274_MAX_EXPOSURE_COARSE,
+                .def = IMX274_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &imx274_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME_SHORT,
+                .name = "Coarse Time Short",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = IMX274_MIN_EXPOSURE_COARSE,
+                .max = IMX274_MAX_EXPOSURE_COARSE,
+                .def = IMX274_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &imx274_ctrl_ops,
+                .id = V4L2_CID_GROUP_HOLD,
+                .name = "Group Hold",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &imx274_ctrl_ops,
+                .id = V4L2_CID_HDR_EN,
+                .name = "HDR enable",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+};
+
+static inline void imx274_get_vmax_regs(imx274_reg *regs,
+                                u32 vmax)
+{
+        regs->addr = IMX274_VMAX_ADDR_MSB;
+        regs->val = (vmax >> 8) & 0xff;
+        (regs + 1)->addr = IMX274_VMAX_ADDR_LSB;
+        (regs + 1)->val = (vmax) & 0xff;
+}
+
+static inline void imx274_get_shr_regs(imx274_reg *regs,
+                                u32 shr)
+{
+        regs->addr = IMX274_SHR_ADDR_MSB;
+        regs->val = (shr >> 8) & 0xff;
+        (regs + 1)->addr = IMX274_SHR_ADDR_LSB;
+        (regs + 1)->val = (shr) & 0xff;
+}
+
+static inline void imx274_get_gain_reg(imx274_reg *regs,
+                                u16 gain)
+{
+        regs->addr = IMX274_GAIN_ADDR_MSB;
+        regs->val = (gain >> 8) & 0xff;
+        (regs + 1)->addr = IMX274_GAIN_ADDR_LSB;
+        (regs + 1)->val = (gain) & 0xff;
+}
+
+static int test_mode;
+module_param(test_mode, int, 0644);
+
+static inline int imx274_read_reg(struct camera_common_data *s_data,
+                                u16 addr, u8 *val)
+{
+        struct imx274 *priv = (struct imx274 *)s_data->priv;
+        return regmap_read(priv->regmap, addr, (unsigned int *) val);
+}
+
+static int imx274_write_reg(struct camera_common_data *s_data, u16 addr, u8 val)
+{
+        int err;
+        struct imx274 *priv = (struct imx274 *)s_data->priv;
+
+        err = regmap_write(priv->regmap, addr, val);
+        if (err)
+                pr_err("%s:i2c write failed, %x = %x\n",
+                        __func__, addr, val);
+
+        return err;
+}
+
+static int imx274_write_table(struct imx274 *priv,
+                                const imx274_reg table[])
+{
+        return regmap_util_write_table_8(priv->regmap,
+                                         table,
+                                         NULL, 0,
+                                         IMX274_TABLE_WAIT_MS,
+                                         IMX274_TABLE_END);
+}
+
+static int imx274_power_on(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct imx274 *priv = (struct imx274 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power on\n", __func__);
+
+        if (priv->pdata && priv->pdata->power_on) {
+                err = priv->pdata->power_on(pw);
+                if (err)
+                        pr_err("%s failed.\n", __func__);
+                else
+                        pw->state = SWITCH_ON;
+                return err;
+        }
+
+        if (pw->reset_gpio)
+                gpio_set_value(pw->reset_gpio, 0);
+        if (pw->af_gpio)
+                gpio_set_value(pw->af_gpio, 1);
+        if (pw->pwdn_gpio)
+                gpio_set_value(pw->pwdn_gpio, 0);
+        usleep_range(10, 20);
+
+
+        if (pw->dvdd)
+                err = regulator_enable(pw->dvdd);
+        if (err)
+                goto imx274_dvdd_fail;
+
+        if (pw->iovdd)
+                err = regulator_enable(pw->iovdd);
+        if (err)
+                goto imx274_iovdd_fail;
+
+        if (pw->avdd)
+                err = regulator_enable(pw->avdd);
+        if (err)
+                goto imx274_avdd_fail;
+
+        usleep_range(1, 2);
+        if (pw->reset_gpio)
+                gpio_set_value(pw->reset_gpio, 1);
+        if (pw->pwdn_gpio)
+                gpio_set_value(pw->pwdn_gpio, 1);
+
+        usleep_range(300, 310);
+
+        pw->state = SWITCH_ON;
+        return 0;
+
+imx274_dvdd_fail:
+        if (pw->af_gpio)
+                gpio_set_value(pw->af_gpio, 0);
+
+imx274_iovdd_fail:
+        regulator_disable(pw->dvdd);
+
+imx274_avdd_fail:
+        regulator_disable(pw->iovdd);
+
+        pr_err("%s failed.\n", __func__);
+        return -ENODEV;
+}
+
+static int imx274_power_off(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct imx274 *priv = (struct imx274 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power off\n", __func__);
+
+        if (priv->pdata->power_off) {
+                err = priv->pdata->power_off(pw);
+                if (err) {
+                        pr_err("%s failed.\n", __func__);
+                        return err;
+                } else {
+                        goto power_off_done;
+                }
+        }
+
+        usleep_range(1, 2);
+        if (pw->reset_gpio)
+                gpio_set_value(pw->reset_gpio, 0);
+        if (pw->af_gpio)
+                gpio_set_value(pw->af_gpio, 0);
+        if (pw->pwdn_gpio)
+                gpio_set_value(pw->pwdn_gpio, 0);
+        usleep_range(1, 2);
+
+        if (pw->avdd)
+                regulator_disable(pw->avdd);
+        if (pw->iovdd)
+                regulator_disable(pw->iovdd);
+        if (pw->dvdd)
+                regulator_disable(pw->dvdd);
+
+power_off_done:
+        pw->state = SWITCH_OFF;
+        return 0;
+}
+
+static int imx274_power_put(struct imx274 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        if (unlikely(!pw))
+                return -EFAULT;
+
+        if (likely(pw->avdd))
+                regulator_put(pw->avdd);
+
+        if (likely(pw->iovdd))
+                regulator_put(pw->iovdd);
+
+        if (likely(pw->dvdd))
+                regulator_put(pw->dvdd);
+
+        pw->avdd = NULL;
+        pw->iovdd = NULL;
+        pw->dvdd = NULL;
+
+        return 0;
+}
+
+static int imx274_power_get(struct imx274 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        struct camera_common_pdata *pdata = priv->pdata;
+        const char *mclk_name;
+        const char *parentclk_name;
+        struct clk *parent;
+        int err = 0;
+
+        mclk_name = priv->pdata->mclk_name ?
+                    priv->pdata->mclk_name : "cam_mclk1";
+        pw->mclk = devm_clk_get(&priv->i2c_client->dev, mclk_name);
+        if (IS_ERR(pw->mclk)) {
+                dev_err(&priv->i2c_client->dev,
+                        "unable to get clock %s\n", mclk_name);
+                return PTR_ERR(pw->mclk);
+        }
+
+        parentclk_name = priv->pdata->parentclk_name;
+        if (parentclk_name) {
+                parent = devm_clk_get(&priv->i2c_client->dev, parentclk_name);
+                if (IS_ERR(parent)) {
+                        dev_err(&priv->i2c_client->dev,
+                                "unable to get parent clcok %s",
+                                parentclk_name);
+                } else
+                        clk_set_parent(pw->mclk, parent);
+        }
+
+        /* ananlog 2.7v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->avdd, pdata->regulators.avdd);
+        /* digital 1.2v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->dvdd, pdata->regulators.dvdd);
+        /* IO 1.8v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->iovdd, pdata->regulators.iovdd);
+
+        if (!err) {
+                pw->reset_gpio = pdata->reset_gpio;
+                pw->af_gpio = pdata->af_gpio;
+                pw->pwdn_gpio = pdata->pwdn_gpio;
+        }
+
+        pw->state = SWITCH_OFF;
+        return err;
+}
+
+static int imx274_set_gain(struct imx274 *priv, s32 val);
+static int imx274_set_frame_length(struct imx274 *priv, s32 val);
+static int imx274_set_coarse_time(struct imx274 *priv, s32 val);
+
+static int imx274_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx274 *priv = (struct imx274 *)s_data->priv;
+        struct v4l2_control control;
+        int err;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        imx274_write_table(priv, mode_table[IMX274_MODE_STOP_STREAM]);
+
+        if (!enable)
+                return 0;
+
+        dev_dbg(&client->dev, "%s mode[%d]\n", __func__, s_data->mode);
+
+        err = imx274_write_table(priv, mode_table[s_data->mode]);
+        if (err)
+                goto exit;
+
+
+        if (s_data->override_enable) {
+                /* write list of override regs for the asking frame length, */
+                /* coarse integration time, and gain.                       */
+                control.id = V4L2_CID_GAIN;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= imx274_set_gain(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: error gain override\n", __func__);
+
+                control.id = V4L2_CID_FRAME_LENGTH;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= imx274_set_frame_length(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: error frame length override\n", __func__);
+
+                control.id = V4L2_CID_COARSE_TIME;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= imx274_set_coarse_time(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: error coarse time override\n", __func__);
+        }
+
+        if (test_mode) {
+                err = imx274_write_table(priv,
+                        mode_table[IMX274_MODE_TEST_PATTERN]);
+                if (err)
+                        goto exit;
+                }
+
+        err = imx274_write_table(priv, mode_table[IMX274_MODE_START_STREAM]);
+        if (err)
+                goto exit;
+
+
+        return 0;
+exit:
+        dev_dbg(&client->dev, "%s: error setting stream\n", __func__);
+        return err;
+}
+
+static int imx274_get_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        return camera_common_g_fmt(sd, &format->format);
+}
+
+static int imx274_set_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+        struct v4l2_subdev_format *format)
+{
+        int ret;
+
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+                ret = camera_common_try_fmt(sd, &format->format);
+        else
+                ret = camera_common_s_fmt(sd, &format->format);
+
+        return ret;
+}
+
+static int imx274_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx274 *priv = (struct imx274 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        *status = pw->state == SWITCH_ON;
+        return 0;
+}
+
+static struct v4l2_subdev_video_ops imx274_subdev_video_ops = {
+        .s_stream       = imx274_s_stream,
+        .g_mbus_config  = camera_common_g_mbus_config,
+        .g_input_status = imx274_g_input_status,
+};
+
+static struct v4l2_subdev_core_ops imx274_subdev_core_ops = {
+        .s_power        = camera_common_s_power,
+};
+
+static struct v4l2_subdev_pad_ops imx274_subdev_pad_ops = {
+        .set_fmt        = imx274_set_fmt,
+        .get_fmt        = imx274_get_fmt,
+        .enum_mbus_code = camera_common_enum_mbus_code,
+        .enum_frame_size        = camera_common_enum_framesizes,
+        .enum_frame_interval    = camera_common_enum_frameintervals,
+};
+
+static struct v4l2_subdev_ops imx274_subdev_ops = {
+        .core   = &imx274_subdev_core_ops,
+        .video  = &imx274_subdev_video_ops,
+        .pad    = &imx274_subdev_pad_ops,
+};
+
+static struct of_device_id imx274_of_match[] = {
+        { .compatible = "nvidia,imx274", },
+        { },
+};
+
+static struct camera_common_sensor_ops imx274_common_ops = {
+        .power_on = imx274_power_on,
+        .power_off = imx274_power_off,
+        .write_reg = imx274_write_reg,
+        .read_reg = imx274_read_reg,
+};
+
+static int imx274_set_group_hold(struct imx274 *priv)
+{
+        int err;
+        int gh_prev = switch_ctrl_qmenu[priv->group_hold_prev];
+
+        if (priv->group_hold_en == true && gh_prev == SWITCH_OFF) {
+                err = imx274_write_reg(priv->s_data,
+                                       IMX274_GROUP_HOLD_ADDR, 0x1);
+                if (err)
+                        goto fail;
+                priv->group_hold_prev = 1;
+        } else if (priv->group_hold_en == false && gh_prev == SWITCH_ON) {
+                err = imx274_write_reg(priv->s_data,
+                                       IMX274_GROUP_HOLD_ADDR, 0x0);
+                if (err)
+                        goto fail;
+                priv->group_hold_prev = 0;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: Group hold control error\n", __func__);
+        return err;
+}
+
+static int imx274_set_gain(struct imx274 *priv, s32 val)
+{
+        imx274_reg reg_list[2];
+        int err;
+        int i = 0;
+        u16 gain;
+
+        dev_dbg(&priv->i2c_client->dev,
+                "%s: val: %d\n", __func__, val);
+
+        if (val < IMX274_MIN_GAIN)
+                val = IMX274_MIN_GAIN;
+        else if (val > IMX274_MAX_GAIN)
+                val = IMX274_MAX_GAIN;
+
+        gain = 2048 - (2048 * IMX274_MIN_GAIN / val);
+
+        imx274_get_gain_reg(reg_list, gain);
+        imx274_set_group_hold(priv);
+
+        /* writing analog gain */
+        for (i = 0; i < 2; i++) {
+                err = imx274_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: GAIN control error\n", __func__);
+        return err;
+}
+
+static int imx274_set_frame_length(struct imx274 *priv, s32 val)
+{
+        imx274_reg reg_list[2];
+        int err;
+        u32 frame_length;
+        u32 frame_rate;
+        int i = 0;
+        u8 svr;
+        u32 vmax;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %u\n", __func__, val);
+
+        frame_length = (u32)val;
+
+        frame_rate = (u32)(IMX274_PIXEL_CLK_HZ /
+                                (u32)(frame_length * IMX274_LINE_LENGTH));
+
+        imx274_read_reg(priv->s_data, IMX274_SVR_ADDR, &svr);
+
+        vmax = (u32)(72000000 /
+                        (u32)(frame_rate * IMX274_HMAX * (svr + 1))) - 12;
+
+        imx274_get_vmax_regs(reg_list, vmax);
+
+        imx274_set_group_hold(priv);
+
+        for (i = 0; i < 2; i++) {
+                err = imx274_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        dev_dbg(&priv->i2c_client->dev,
+                "%s: frame_rate: %d vmax: %u\n", __func__, frame_rate, vmax);
+        return 0;
+
+fail:
+        dev_info(&priv->i2c_client->dev,
+                 "%s: FRAME_LENGTH control error\n", __func__);
+        return err;
+}
+
+static int imx274_calculate_shr(struct imx274 *priv, u32 rep)
+{
+        u8 svr;
+        int shr;
+        int min;
+        int max;
+        u8 vmax_l;
+        u8 vmax_m;
+        u32 vmax;
+
+        imx274_read_reg(priv->s_data, IMX274_SVR_ADDR, &svr);
+
+        imx274_read_reg(priv->s_data, IMX274_VMAX_ADDR_LSB, &vmax_l);
+        imx274_read_reg(priv->s_data, IMX274_VMAX_ADDR_MSB, &vmax_m);
+
+        vmax = ((vmax_m << 8) + vmax_l);
+
+        min = IMX274_MODE1_SHR_MIN;
+        max = ((svr + 1) * IMX274_VMAX) - 4;
+
+        shr = vmax * (svr + 1) -
+                        (rep * IMX274_ET_FACTOR - IMX274_MODE1_OFFSET) /
+                        IMX274_HMAX;
+
+        if (shr < min)
+                shr = min;
+
+        if (shr > max)
+                shr = max;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: shr: %u vmax: %d\n", __func__, shr, vmax);
+        return shr;
+}
+
+static int imx274_set_coarse_time(struct imx274 *priv, s32 val)
+{
+        imx274_reg reg_list[2];
+        int err;
+        u32 coarse_time;
+        u32 shr;
+        int i = 0;
+
+        coarse_time = val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, coarse_time);
+
+        shr = imx274_calculate_shr(priv, coarse_time);
+
+        imx274_get_shr_regs(reg_list, shr);
+        imx274_set_group_hold(priv);
+
+        for (i = 0; i < 2; i++) {
+                err = imx274_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: COARSE_TIME control error\n", __func__);
+        return err;
+}
+
+static int imx274_verify_streaming(struct imx274 *priv)
+{
+        int err = 0;
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return err;
+
+        err = imx274_s_stream(priv->subdev, true);
+        if (err)
+                goto error;
+
+error:
+        imx274_s_stream(priv->subdev, false);
+        camera_common_s_power(priv->subdev, false);
+
+        return err;
+}
+
+static int imx274_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct imx274 *priv =
+                container_of(ctrl->handler, struct imx274, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_GAIN:
+                err = imx274_set_gain(priv, ctrl->val);
+                break;
+        case V4L2_CID_FRAME_LENGTH:
+                err = imx274_set_frame_length(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME:
+                err = imx274_set_coarse_time(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME_SHORT:
+                err = imx274_set_coarse_time(priv, ctrl->val);
+                break;
+        case V4L2_CID_GROUP_HOLD:
+                if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON) {
+                        priv->group_hold_en = true;
+                } else {
+                        priv->group_hold_en = false;
+                        err = imx274_set_group_hold(priv);
+                }
+                break;
+        case V4L2_CID_HDR_EN:
+                break;
+        default:
+                pr_err("%s: unknown ctrl id.\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static int imx274_ctrls_init(struct imx274 *priv)
+{
+        struct i2c_client *client = priv->i2c_client;
+        struct v4l2_ctrl *ctrl;
+        int num_ctrls;
+        int err;
+        int i;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        num_ctrls = ARRAY_SIZE(ctrl_config_list);
+        v4l2_ctrl_handler_init(&priv->ctrl_handler, num_ctrls);
+
+        for (i = 0; i < num_ctrls; i++) {
+                ctrl = v4l2_ctrl_new_custom(&priv->ctrl_handler,
+                        &ctrl_config_list[i], NULL);
+                if (ctrl == NULL) {
+                        dev_err(&client->dev, "Failed to init %s ctrl\n",
+                                ctrl_config_list[i].name);
+                        continue;
+                }
+
+                if (ctrl_config_list[i].type == V4L2_CTRL_TYPE_STRING &&
+                        ctrl_config_list[i].flags & V4L2_CTRL_FLAG_READ_ONLY) {
+                        ctrl->p_new.p_char = devm_kzalloc(&client->dev,
+                                ctrl_config_list[i].max + 1, GFP_KERNEL);
+                }
+                priv->ctrls[i] = ctrl;
+        }
+
+        priv->num_ctrls = num_ctrls;
+        priv->subdev->ctrl_handler = &priv->ctrl_handler;
+        if (priv->ctrl_handler.error) {
+                dev_err(&client->dev, "Error %d adding controls\n",
+                        priv->ctrl_handler.error);
+                err = priv->ctrl_handler.error;
+                goto error;
+        }
+
+        err = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d setting default controls\n", err);
+                goto error;
+        }
+
+        return 0;
+
+error:
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        return err;
+}
+
+MODULE_DEVICE_TABLE(of, imx274_of_match);
+
+static struct camera_common_pdata *imx274_parse_dt(struct i2c_client *client)
+{
+        struct device_node *node = client->dev.of_node;
+        struct camera_common_pdata *board_priv_pdata;
+        const struct of_device_id *match;
+        int err;
+
+        if (!node)
+                return NULL;
+
+        match = of_match_device(imx274_of_match, &client->dev);
+        if (!match) {
+                dev_err(&client->dev, " Failed to find matching dt id\n");
+                return NULL;
+        }
+
+        board_priv_pdata = devm_kzalloc(&client->dev,
+                           sizeof(*board_priv_pdata), GFP_KERNEL);
+        if (!board_priv_pdata) {
+                dev_err(&client->dev, "Failed to allocate pdata\n");
+                return NULL;
+        }
+
+
+        err = camera_common_parse_clocks(client, board_priv_pdata);
+        if (err) {
+                dev_err(&client->dev, "Failed to find clocks\n");
+                goto error;
+        }
+
+        err = of_property_read_string(node, "mclk",
+                                      &board_priv_pdata->mclk_name);
+        if (err)
+                dev_err(&client->dev, "mclk not in DT\n");
+
+        board_priv_pdata->reset_gpio = of_get_named_gpio(node,
+                        "reset-gpios", 0);
+
+        of_property_read_string(node, "avdd-reg",
+                        &board_priv_pdata->regulators.avdd);
+        of_property_read_string(node, "dvdd-reg",
+                        &board_priv_pdata->regulators.dvdd);
+        of_property_read_string(node, "iovdd-reg",
+                        &board_priv_pdata->regulators.iovdd);
+
+        return board_priv_pdata;
+
+error:
+        devm_kfree(&client->dev, board_priv_pdata);
+        return NULL;
+}
+
+static int imx274_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        dev_dbg(&client->dev, "%s:\n", __func__);
+
+
+        return 0;
+}
+
+static const struct v4l2_subdev_internal_ops imx274_subdev_internal_ops = {
+        .open = imx274_open,
+};
+
+static const struct media_entity_operations imx274_media_ops = {
+#ifdef CONFIG_MEDIA_CONTROLLER
+        .link_validate = v4l2_subdev_link_validate,
+#endif
+};
+
+static int imx274_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct camera_common_data *common_data;
+        struct device_node *node = client->dev.of_node;
+        struct imx274 *priv;
+        char debugfs_name[10];
+        int err;
+
+        pr_info("[IMX274]: probing v4l2 sensor.\n");
+
+        if (!IS_ENABLED(CONFIG_OF) || !node)
+                return -EINVAL;
+
+        common_data = devm_kzalloc(&client->dev,
+                            sizeof(struct camera_common_data), GFP_KERNEL);
+        if (!common_data) {
+                dev_err(&client->dev, "unable to allocate memory!\n");
+                return -ENOMEM;
+        }
+
+        priv = devm_kzalloc(&client->dev,
+                            sizeof(struct imx274) + sizeof(struct v4l2_ctrl *) *
+                            ARRAY_SIZE(ctrl_config_list),
+                            GFP_KERNEL);
+        if (!priv) {
+                dev_err(&client->dev, "unable to allocate memory!\n");
+                return -ENOMEM;
+        }
+
+        priv->regmap = devm_regmap_init_i2c(client, &sensor_regmap_config);
+        if (IS_ERR(priv->regmap)) {
+                dev_err(&client->dev,
+                        "regmap init failed: %ld\n", PTR_ERR(priv->regmap));
+                return -ENODEV;
+        }
+
+        priv->pdata = imx274_parse_dt(client);
+        if (!priv->pdata) {
+                dev_err(&client->dev, " unable to get platform data\n");
+                return -EFAULT;
+        }
+
+        common_data->ops                = &imx274_common_ops;
+        common_data->ctrl_handler       = &priv->ctrl_handler;
+        common_data->i2c_client         = client;
+        common_data->frmfmt             = &imx274_frmfmt[0];
+        common_data->colorfmt           = camera_common_find_datafmt(
+                                          IMX274_DEFAULT_DATAFMT);
+        common_data->power              = &priv->power;
+        common_data->ctrls              = priv->ctrls;
+        common_data->priv               = (void *)priv;
+        common_data->numctrls           = ARRAY_SIZE(ctrl_config_list);
+        common_data->numfmts            = ARRAY_SIZE(imx274_frmfmt);
+        common_data->def_mode           = IMX274_DEFAULT_MODE;
+        common_data->def_width          = IMX274_DEFAULT_WIDTH;
+        common_data->def_height         = IMX274_DEFAULT_HEIGHT;
+        common_data->fmt_width          = common_data->def_width;
+        common_data->fmt_height         = common_data->def_height;
+        common_data->def_clk_freq       = IMX274_DEFAULT_CLK_FREQ;
+
+        priv->i2c_client                = client;
+        priv->s_data                    = common_data;
+        priv->subdev                    = &common_data->subdev;
+        priv->subdev->dev               = &client->dev;
+
+        err = imx274_power_get(priv);
+        if (err)
+                return err;
+
+        err = camera_common_parse_ports(client, common_data);
+        if (err) {
+                dev_err(&client->dev, "Failed to find port info\n");
+                return err;
+        }
+        sprintf(debugfs_name, "imx274_%c", common_data->csi_port + 'a');
+        dev_dbg(&client->dev, "%s: name %s\n", __func__, debugfs_name);
+
+        camera_common_create_debugfs(common_data, "imx274");
+
+        v4l2_i2c_subdev_init(priv->subdev, client, &imx274_subdev_ops);
+
+        err = imx274_ctrls_init(priv);
+        if (err)
+                return err;
+
+        err = imx274_verify_streaming(priv);
+        if (err)
+                return err;
+
+        priv->subdev->internal_ops = &imx274_subdev_internal_ops;
+        priv->subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+                               V4L2_SUBDEV_FL_HAS_EVENTS;
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+        priv->subdev->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+        priv->subdev->entity.ops = &imx274_media_ops;
+        err = media_entity_init(&priv->subdev->entity, 1, &priv->pad, 0);
+        if (err < 0) {
+                dev_err(&client->dev, "unable to init media entity\n");
+                return err;
+        }
+#endif
+
+        err = v4l2_async_register_subdev(priv->subdev);
+        if (err)
+                return err;
+
+        dev_dbg(&client->dev, "Detected IMX274 sensor\n");
+
+        return 0;
+}
+
+static int
+imx274_remove(struct i2c_client *client)
+{
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct imx274 *priv = (struct imx274 *)s_data->priv;
+
+        v4l2_async_unregister_subdev(priv->subdev);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev->entity);
+#endif
+
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        imx274_power_put(priv);
+        camera_common_remove_debugfs(s_data);
+
+        return 0;
+}
+
+static const struct i2c_device_id imx274_id[] = {
+        { "imx274", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, imx274_id);
+
+static struct i2c_driver imx274_i2c_driver = {
+        .driver = {
+                .name = "imx274",
+                .owner = THIS_MODULE,
+        },
+        .probe = imx274_probe,
+        .remove = imx274_remove,
+        .id_table = imx274_id,
+};
+
+module_i2c_driver(imx274_i2c_driver);
+
+MODULE_DESCRIPTION("Media Controller driver for Sony IMX274");
+MODULE_AUTHOR("Josh Kuo <joshk@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/imx274_mode_tbls.h b/drivers/media/i2c/imx274_mode_tbls.h
new file mode 100644
index 000000000..81ca2a289
--- /dev/null
+++ b/drivers/media/i2c/imx274_mode_tbls.h
@@ -0,0 +1,430 @@
+/*
+ * imx274.c - imx274 sensor driver
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION, All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __IMX274_I2C_TABLES__
+#define __IMX274_I2C_TABLES__
+
+#include <media/camera_common.h>
+
+
+#define IMX274_TABLE_WAIT_MS 0
+#define IMX274_TABLE_END 1
+#define IMX274_WAIT_MS 1
+
+#define imx274_reg struct reg_8
+
+static const imx274_reg imx274_start[] = {
+        {0x3000, 0x00}, /* mode select streaming on */
+        {0x303E, 0x02},
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {0x30F4, 0x00},
+        {0x3018, 0xA2},
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {IMX274_TABLE_END, 0x00}
+};
+
+static const imx274_reg imx274_stop[] = {
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {0x3000, 0x01}, /* mode select streaming off */
+        {IMX274_TABLE_END, 0x00}
+};
+
+static const imx274_reg tp_colorbars[] = {
+        /* test pattern */
+        {0x303C, 0x11},
+        {0x303D, 0x0B},
+        {0x370B, 0x11},
+        {0x370E, 0x00},
+        {0x377F, 0x01},
+        {0x3781, 0x01},
+        {IMX274_TABLE_END, 0x00}
+};
+
+
+/* Mode 1 : 3840X2160 10 bits 30fps*/
+static const imx274_reg mode_3840X2160[] = {
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {0x3000, 0x12}, /* mode select streaming on */
+        /* input freq. 24M */
+        {0x3120, 0xF0},
+        {0x3122, 0x02},
+        {0x3129, 0x9c},
+        {0x312A, 0x02},
+        {0x312D, 0x02},
+
+        {0x310B, 0x00},
+        {0x304C, 0x00},
+        {0x304D, 0x03},
+        {0x331C, 0x1A},
+        {0x3502, 0x02},
+        {0x3529, 0x0E},
+        {0x352A, 0x0E},
+        {0x352B, 0x0E},
+        {0x3538, 0x0E},
+        {0x3539, 0x0E},
+        {0x3553, 0x00},
+        {0x357D, 0x05},
+        {0x357F, 0x05},
+        {0x3581, 0x04},
+        {0x3583, 0x76},
+        {0x3587, 0x01},
+        {0x35BB, 0x0E},
+        {0x35BC, 0x0E},
+        {0x35BD, 0x0E},
+        {0x35BE, 0x0E},
+        {0x35BF, 0x0E},
+        {0x366E, 0x00},
+        {0x366F, 0x00},
+        {0x3670, 0x00},
+        {0x3671, 0x00},
+        {0x30EE, 0x01},
+        {0x3304, 0x32},
+        {0x3306, 0x32},
+        {0x3590, 0x32},
+        {0x3686, 0x32},
+        /* resolution */
+        {0x30E2, 0x01},
+        {0x30F6, 0x07},
+        {0x30F7, 0x01},
+        {0x30F8, 0xC6},
+        {0x30F9, 0x11},
+        {0x3130, 0x86},
+        {0x3131, 0x08},
+        {0x3132, 0x7E},
+        {0x3133, 0x08},
+        /* mode setting */
+        {0x3004, 0x01},
+        {0x3005, 0x01},
+        {0x3006, 0x00},
+        {0x3007, 0x02},
+        {0x3A41, 0x08},
+        {0x3342, 0x0A},
+        {0x3343, 0x00},
+        {0x3344, 0x16},
+        {0x3345, 0x00},
+        {0x3528, 0x0E},
+        {0x3554, 0x1F},
+        {0x3555, 0x01},
+        {0x3556, 0x01},
+        {0x3557, 0x01},
+        {0x3558, 0x01},
+        {0x3559, 0x00},
+        {0x355A, 0x00},
+        {0x35BA, 0x0E},
+        {0x366A, 0x1B},
+        {0x366B, 0x1A},
+        {0x366C, 0x19},
+        {0x366D, 0x17},
+        {0x33A6, 0x01},
+        {0x306B, 0x05},
+
+        /* d gain setting */
+        {0x3012, 0x01},
+        {0x300E, 0x01},
+
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {IMX274_TABLE_END, 0x0000}
+};
+
+/* Mode 1 : 3840X2160 10 bits 60fps*/
+static const imx274_reg mode_3840X2160_60fps[] = {
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {0x3000, 0x12}, /* mode select streaming on */
+        /* input freq. 24M */
+        {0x3120, 0xF0},
+        {0x3122, 0x02},
+        {0x3129, 0x9c},
+        {0x312A, 0x02},
+        {0x312D, 0x02},
+
+        {0x310B, 0x00},
+        {0x304C, 0x00},
+        {0x304D, 0x03},
+        {0x331C, 0x1A},
+        {0x3502, 0x02},
+        {0x3529, 0x0E},
+        {0x352A, 0x0E},
+        {0x352B, 0x0E},
+        {0x3538, 0x0E},
+        {0x3539, 0x0E},
+        {0x3553, 0x00},
+        {0x357D, 0x05},
+        {0x357F, 0x05},
+        {0x3581, 0x04},
+        {0x3583, 0x76},
+        {0x3587, 0x01},
+        {0x35BB, 0x0E},
+        {0x35BC, 0x0E},
+        {0x35BD, 0x0E},
+        {0x35BE, 0x0E},
+        {0x35BF, 0x0E},
+        {0x366E, 0x00},
+        {0x366F, 0x00},
+        {0x3670, 0x00},
+        {0x3671, 0x00},
+        {0x30EE, 0x01},
+        {0x3304, 0x32},
+        {0x3306, 0x32},
+        {0x3590, 0x32},
+        {0x3686, 0x32},
+        /* resolution */
+        {0x30E2, 0x01},
+        {0x30F6, 0x07},
+        {0x30F7, 0x01},
+        {0x30F8, 0xC6},
+        {0x30F9, 0x11},
+        {0x3130, 0x86},
+        {0x3131, 0x08},
+        {0x3132, 0x7E},
+        {0x3133, 0x08},
+        /* mode setting */
+        {0x3004, 0x01},
+        {0x3005, 0x01},
+        {0x3006, 0x00},
+        {0x3007, 0x02},
+        {0x3A41, 0x08},
+        {0x3342, 0x0A},
+        {0x3343, 0x00},
+        {0x3344, 0x16},
+        {0x3345, 0x00},
+        {0x3528, 0x0E},
+        {0x3554, 0x1F},
+        {0x3555, 0x01},
+        {0x3556, 0x01},
+        {0x3557, 0x01},
+        {0x3558, 0x01},
+        {0x3559, 0x00},
+        {0x355A, 0x00},
+        {0x35BA, 0x0E},
+        {0x366A, 0x1B},
+        {0x366B, 0x1A},
+        {0x366C, 0x19},
+        {0x366D, 0x17},
+        {0x33A6, 0x01},
+        {0x306B, 0x05},
+
+        /* d gain setting */
+        {0x3012, 0x01},
+        {0x300E, 0x00},
+
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {IMX274_TABLE_END, 0x0000}
+};
+
+/* Mode 3 : 1920X1080 10 bits 60fps*/
+static imx274_reg mode_1920X1080[] = {
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {0x3000, 0x12}, /* mode select streaming on */
+        /* input freq. 24M */
+        {0x3120, 0xF0},
+        {0x3122, 0x02},
+        {0x3129, 0x9c},
+        {0x312A, 0x02},
+        {0x312D, 0x02},
+
+        {0x310B, 0x00},
+        {0x304C, 0x00},
+        {0x304D, 0x03},
+        {0x331C, 0x1A},
+        {0x3502, 0x02},
+        {0x3529, 0x0E},
+        {0x352A, 0x0E},
+        {0x352B, 0x0E},
+        {0x3538, 0x0E},
+        {0x3539, 0x0E},
+        {0x3553, 0x00},
+        {0x357D, 0x05},
+        {0x357F, 0x05},
+        {0x3581, 0x04},
+        {0x3583, 0x76},
+        {0x3587, 0x01},
+        {0x35BB, 0x0E},
+        {0x35BC, 0x0E},
+        {0x35BD, 0x0E},
+        {0x35BE, 0x0E},
+        {0x35BF, 0x0E},
+        {0x366E, 0x00},
+        {0x366F, 0x00},
+        {0x3670, 0x00},
+        {0x3671, 0x00},
+        {0x30EE, 0x01},
+        {0x3304, 0x32},
+        {0x3306, 0x32},
+        {0x3590, 0x32},
+        {0x3686, 0x32},
+        /* resolution */
+        {0x30E2, 0x02},
+        {0x30F6, 0x04},
+        {0x30F7, 0x01},
+        {0x30F8, 0x06},
+        {0x30F9, 0x09},
+        {0x3130, 0x4E},
+        {0x3131, 0x04},
+        {0x3132, 0x46},
+        {0x3133, 0x04},
+        /* mode setting */
+        {0x3004, 0x02},
+        {0x3005, 0x21},
+        {0x3006, 0x00},
+        {0x3007, 0x11},
+        {0x3A41, 0x08},
+        {0x3342, 0x0A},
+        {0x3343, 0x00},
+        {0x3344, 0x1A},
+        {0x3345, 0x00},
+        {0x3528, 0x0E},
+        {0x3554, 0x00},
+        {0x3555, 0x01},
+        {0x3556, 0x01},
+        {0x3557, 0x01},
+        {0x3558, 0x01},
+        {0x3559, 0x00},
+        {0x355A, 0x00},
+        {0x35BA, 0x0E},
+        {0x366A, 0x1B},
+        {0x366B, 0x1A},
+        {0x366C, 0x19},
+        {0x366D, 0x17},
+        {0x33A6, 0x01},
+        {0x306B, 0x05},
+
+        /* d gain setting */
+        {0x3012, 0x01},
+        {0x300E, 0x01},
+
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {IMX274_TABLE_END, 0x0000}
+};
+
+/* Mode 5 : 1280X720 10 bits */
+static imx274_reg mode_1280X720[] = {
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {0x3000, 0x12}, /* mode select streaming on */
+        /* input freq. 24M */
+        {0x3120, 0xF0},
+        {0x3122, 0x02},
+        {0x3129, 0x9c},
+        {0x312A, 0x02},
+        {0x312D, 0x02},
+
+        {0x310B, 0x00},
+        {0x304C, 0x00},
+        {0x304D, 0x03},
+        {0x331C, 0x1A},
+        {0x3502, 0x02},
+        {0x3529, 0x0E},
+        {0x352A, 0x0E},
+        {0x352B, 0x0E},
+        {0x3538, 0x0E},
+        {0x3539, 0x0E},
+        {0x3553, 0x00},
+        {0x357D, 0x05},
+        {0x357F, 0x05},
+        {0x3581, 0x04},
+        {0x3583, 0x76},
+        {0x3587, 0x01},
+        {0x35BB, 0x0E},
+        {0x35BC, 0x0E},
+        {0x35BD, 0x0E},
+        {0x35BE, 0x0E},
+        {0x35BF, 0x0E},
+        {0x366E, 0x00},
+        {0x366F, 0x00},
+        {0x3670, 0x00},
+        {0x3671, 0x00},
+        {0x30EE, 0x01},
+        {0x3304, 0x32},
+        {0x3306, 0x32},
+        {0x3590, 0x32},
+        {0x3686, 0x32},
+        /* resolution */
+        {0x30E2, 0x03},
+        {0x30F6, 0x04},
+        {0x30F7, 0x01},
+        {0x30F8, 0x06},
+        {0x30F9, 0x09},
+        {0x3130, 0xE2},
+        {0x3131, 0x02},
+        {0x3132, 0xDE},
+        {0x3133, 0x02},
+        /* mode setting */
+        {0x3004, 0x03},
+        {0x3005, 0x31},
+        {0x3006, 0x00},
+        {0x3007, 0x09},
+        {0x3A41, 0x04},
+        {0x3342, 0x0A},
+        {0x3343, 0x00},
+        {0x3344, 0x1B},
+        {0x3345, 0x00},
+        {0x3528, 0x0E},
+        {0x3554, 0x00},
+        {0x3555, 0x01},
+        {0x3556, 0x01},
+        {0x3557, 0x01},
+        {0x3558, 0x01},
+        {0x3559, 0x00},
+        {0x355A, 0x00},
+        {0x35BA, 0x0E},
+        {0x366A, 0x1B},
+        {0x366B, 0x19},
+        {0x366C, 0x17},
+        {0x366D, 0x17},
+        {0x33A6, 0x01},
+        {0x306B, 0x05},
+
+        /* d gain setting */
+        {0x3012, 0x01},
+
+        {IMX274_TABLE_WAIT_MS, IMX274_WAIT_MS},
+        {IMX274_TABLE_END, 0x0000}
+};
+
+enum {
+        IMX274_MODE_3840X2160,
+        IMX274_MODE_1920X1080,
+        IMX274_MODE_1280X720,
+        IMX274_MODE_START_STREAM,
+        IMX274_MODE_STOP_STREAM,
+        IMX274_MODE_TEST_PATTERN,
+};
+
+static const imx274_reg *mode_table[] = {
+        [IMX274_MODE_3840X2160] = mode_3840X2160_60fps,
+        [IMX274_MODE_1920X1080] = mode_1920X1080,
+        [IMX274_MODE_1280X720] = mode_1280X720,
+
+        [IMX274_MODE_START_STREAM]              = imx274_start,
+        [IMX274_MODE_STOP_STREAM]               = imx274_stop,
+        [IMX274_MODE_TEST_PATTERN]              = tp_colorbars,
+};
+
+static const int imx274_30_fr[] = {
+        30,
+};
+
+static const int imx274_60_fr[] = {
+        60,
+};
+
+static const struct camera_common_frmfmt imx274_frmfmt[] = {
+        {{3864, 2174}, imx274_60_fr, 1, 0, IMX274_MODE_3840X2160},
+        {{1920, 1080}, imx274_60_fr, 1, 0, IMX274_MODE_1920X1080},
+        {{1280, 720},  imx274_60_fr, 1, 0, IMX274_MODE_1280X720},
+};
+#endif  /* __IMX274_I2C_TABLES__ */
diff --git a/drivers/media/i2c/lc898212.c b/drivers/media/i2c/lc898212.c
new file mode 100644
index 000000000..44cdb876e
--- /dev/null
+++ b/drivers/media/i2c/lc898212.c
@@ -0,0 +1,645 @@
+/*
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/gpio.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <media/camera_common.h>
+
+#include "tegra_camera_dev_mfi.h"
+
+#define LC898212_ACTUATOR_RANGE 1023
+#define LC898212_POS_LOW_DEFAULT        (0)
+#define LC898212_POS_HIGH_DEFAULT       (1023)
+#define LC898212_FOCUS_MACRO    (896)
+#define LC898212_FOCUS_INFINITY (128)
+#define LC898212_PWR_DEV_OFF          (0)
+#define LC898212_PWR_DEV_ON           (1)
+
+#define SETTLETIME_MS   (15)
+#define FOCAL_LENGTH    (47300)
+#define MAX_APERTURE    (22000)
+#define FNUMBER         (22000)
+#define LC898212_MOVE_TIME_VALUE        (0x43)
+
+#define LC898212_MAX_RETRIES (3)
+
+#define LC898212_WAIT_REPEAT 0xFE
+#define LC898212_TABLE_END   0xFF
+
+#define LC898212_REG_ACCESS     0x1
+#define LC898212_RAM_ACCESS     0x2
+
+#define AF_MIN 0
+#define AF_MAX 1023
+#define AF_RANGE (AF_MAX - AF_MIN + 1)
+
+#define LC898212_RZ                     0x04
+#define LC898212_ADOFFSET       0x3C
+#define LC898212_EQENBL         0x87
+
+/**
+* standard + custom controls
+* custom controls not added for this focuser yet
+*/
+#define NUM_FOCUS_STD_CTRLS     1
+#define NUM_FOCUS_CUSTOM_CTRLS  0
+#define NUM_FOCUS_CTRLS (NUM_FOCUS_STD_CTRLS + NUM_FOCUS_CUSTOM_CTRLS)
+
+static int lc898212_s_ctrl(struct v4l2_ctrl *ctrl);
+
+struct lc898212_reg {
+        u8 type;
+        u8 addr;
+        u16 val;
+};
+
+struct lc898212 {
+        struct  i2c_client                      *i2c_client;
+        struct  v4l2_subdev                     *subdev;
+        struct  media_pad                       pad;
+
+        struct  v4l2_ctrl_handler               ctrl_handler;
+        struct  regulator                       *regulator;
+        struct  camera_common_focuser_data      *s_data;
+        struct  regmap                          *regmap8;
+        struct  regmap                          *regmap16;
+        int                                     numctrls;
+        struct camera_mfi_dev                   *cmfi_dev16;
+        bool                                    support_mfi;
+        struct  v4l2_ctrl                       *ctrls[];
+};
+
+static struct lc898212_reg lc898212_init_setting[] = {
+        {LC898212_REG_ACCESS, 0x80, 0x34},
+        {LC898212_REG_ACCESS, 0x81, 0xA0},
+        {LC898212_REG_ACCESS, 0x84, 0xE0},
+        {LC898212_REG_ACCESS, 0x87, 0x05},
+        {LC898212_REG_ACCESS, 0xA4, 0x24},
+        {LC898212_RAM_ACCESS, 0x3A, 0x0000},
+        {LC898212_RAM_ACCESS, 0x04, 0x0000},
+        {LC898212_RAM_ACCESS, 0x02, 0x0000},
+        {LC898212_RAM_ACCESS, 0x18, 0x0000},
+
+        /* Filter Setting */
+        {LC898212_RAM_ACCESS, 0x40, 0x4030},
+        {LC898212_RAM_ACCESS, 0x42, 0x7150},
+        {LC898212_RAM_ACCESS, 0x44, 0x8F90},
+        {LC898212_RAM_ACCESS, 0x46, 0x61B0},
+        {LC898212_RAM_ACCESS, 0x48, 0x7FF0},
+        {LC898212_RAM_ACCESS, 0x4A, 0x3930},
+        {LC898212_RAM_ACCESS, 0x4C, 0x4030},
+        {LC898212_RAM_ACCESS, 0x4E, 0x8010},
+        {LC898212_RAM_ACCESS, 0x50, 0x04F0},
+        {LC898212_RAM_ACCESS, 0x52, 0x7610},
+        {LC898212_RAM_ACCESS, 0x54, 0x2030},
+        {LC898212_RAM_ACCESS, 0x56, 0x0000},
+        {LC898212_RAM_ACCESS, 0x58, 0x7FF0},
+        {LC898212_RAM_ACCESS, 0x5A, 0x0680},
+        {LC898212_RAM_ACCESS, 0x5C, 0x72F0},
+        {LC898212_RAM_ACCESS, 0x5E, 0x7F70},
+        {LC898212_RAM_ACCESS, 0x60, 0x7ED0},
+        {LC898212_RAM_ACCESS, 0x62, 0x7FF0},
+        {LC898212_RAM_ACCESS, 0x64, 0x0000},
+        {LC898212_RAM_ACCESS, 0x66, 0x0000},
+        {LC898212_RAM_ACCESS, 0x68, 0x5130},
+        {LC898212_RAM_ACCESS, 0x6A, 0x72F0},
+        {LC898212_RAM_ACCESS, 0x6C, 0x8010},
+        {LC898212_RAM_ACCESS, 0x6E, 0x0000},
+        {LC898212_RAM_ACCESS, 0x70, 0x0000},
+        {LC898212_RAM_ACCESS, 0x72, 0x18E0},
+        {LC898212_RAM_ACCESS, 0x74, 0x4E30},
+        {LC898212_RAM_ACCESS, 0x30, 0x0000},
+        {LC898212_RAM_ACCESS, 0x76, 0x0C50},
+        {LC898212_RAM_ACCESS, 0x78, 0x4000},
+
+        {LC898212_REG_ACCESS, 0x86, 0x60},
+        {LC898212_REG_ACCESS, 0x88, 0x70},
+        {LC898212_RAM_ACCESS, 0x28, 0x8020},
+        {LC898212_RAM_ACCESS, 0x4C, 0x4000},
+        {LC898212_REG_ACCESS, 0x83, 0x2C},
+        {LC898212_REG_ACCESS, 0x85, 0xC0},
+
+        /* Repeat to read the register until the value turns 0x00 */
+        {LC898212_REG_ACCESS, LC898212_WAIT_REPEAT, 0x85},
+
+        {LC898212_REG_ACCESS, 0x84, 0xE3},
+        {LC898212_REG_ACCESS, 0x97, 0x00},
+        {LC898212_REG_ACCESS, 0x98, 0x42},
+        {LC898212_REG_ACCESS, 0x99, 0x00},
+        {LC898212_REG_ACCESS, 0x9A, 0x00},
+
+        {LC898212_REG_ACCESS, LC898212_TABLE_END, 0x00}
+};
+
+const struct of_device_id lc898212_of_match[] = {
+        { .compatible = "nvidia,lc898212", },
+        { },
+};
+MODULE_DEVICE_TABLE(of, lc898212_of_match);
+
+static int lc898212_set_position(struct lc898212 *priv, u32 position)
+{
+        int ret = 0;
+        s16 new_pos = 0;
+        struct camera_common_focuser_data *s_data = priv->s_data;
+        struct nv_focuser_config *cfg = &s_data->config;
+
+        dev_dbg(&s_data->i2c_client->dev, "%s++\n", __func__);
+        if (position < cfg->pos_actual_low ||
+                position > cfg->pos_actual_high) {
+                dev_dbg(&priv->i2c_client->dev,
+                        "%s: position(%d) out of bound([%d, %d])\n",
+                        __func__, position, cfg->pos_actual_low,
+                        cfg->pos_actual_high);
+                if (position < cfg->pos_actual_low)
+                        position = cfg->pos_actual_low;
+                if (position > cfg->pos_actual_high)
+                        position = cfg->pos_actual_high;
+        }
+
+        /* unsigned 10 bit to signed 16 bit */
+        new_pos = ((s16) position - AF_RANGE / 2) * 64;
+
+        if (priv->support_mfi) {
+                ret = tegra_camera_dev_mfi_clear(priv->cmfi_dev16);
+                ret |= tegra_camera_dev_mfi_wr_add(priv->cmfi_dev16,
+                                LC898212_RZ,
+                                (u16) new_pos);
+        } else {
+                ret = regmap_write(priv->regmap16, LC898212_RZ, (u16) new_pos);
+        }
+
+        dev_dbg(&s_data->i2c_client->dev, "%s--\n", __func__);
+        return ret;
+}
+
+/*
+ * V4l2 controls
+ */
+
+static const struct v4l2_ctrl_ops lc898212_ctrl_ops = {
+        .s_ctrl         = lc898212_s_ctrl,
+};
+
+static int lc898212_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct lc898212 *priv =
+                container_of(ctrl->handler, struct lc898212, ctrl_handler);
+        int err;
+
+        dev_dbg(&priv->s_data->i2c_client->dev, "%s++\n", __func__);
+        /* check for power state */
+        if (priv->s_data->pwr_dev == LC898212_PWR_DEV_OFF)
+                return -ENODEV;
+
+        switch (ctrl->id) {
+        case V4L2_CID_FOCUS_ABSOLUTE:
+                err = lc898212_set_position(priv, ctrl->val);
+                break;
+        default:
+                pr_err("%s: unknown v4l2 ctlr id\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static int lc898212_ctrls_init(struct camera_common_focuser_data *s_data)
+{
+        struct lc898212 *priv = (struct lc898212 *)s_data->priv;
+        struct i2c_client *client = priv->i2c_client;
+        struct v4l2_ctrl *ctrl;
+        struct nv_focuser_config *cfg = &s_data->config;
+        int min = cfg->pos_actual_low;
+        int max = cfg->pos_actual_high;
+        int def = priv->s_data->def_position;
+        int err = 0;
+
+        v4l2_ctrl_handler_init(&priv->ctrl_handler, priv->numctrls);
+        priv->subdev->ctrl_handler = &priv->ctrl_handler;
+        err = priv->ctrl_handler.error;
+        if (err) {
+                dev_err(&client->dev, "Error %d adding controls\n", err);
+                goto error;
+        }
+
+        /* add std controls */
+        ctrl = v4l2_ctrl_new_std(&priv->ctrl_handler, &lc898212_ctrl_ops,
+                        V4L2_CID_FOCUS_ABSOLUTE, min, max, 1, def);
+        if (ctrl == NULL) {
+                dev_err(&client->dev, "Error initializing controls\n");
+                err = -EINVAL;
+                goto error;
+        }
+        priv->ctrls[0] = ctrl;
+
+        err = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
+        if (err) {
+                dev_err(&client->dev, "Error setting default controls\n");
+                goto error;
+        }
+
+        return 0;
+error:
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        return err;
+}
+
+static int lc898212_write_table(struct lc898212 *priv,
+                                 const struct lc898212_reg table[])
+{
+        int err;
+        const struct lc898212_reg *next;
+        u16 val;
+
+        for (next = table; next->addr != LC898212_TABLE_END; next++) {
+                val = next->val;
+
+                if (next->addr == LC898212_WAIT_REPEAT) {
+                        u8 data = 0;
+                        u8 count = 0;
+
+                        err = regmap_read(priv->regmap8, val,
+                                (unsigned int *) &data);
+                        if (err) {
+                                pr_err("%s: regmap_read: %d\n", __func__, err);
+                                return err;
+                        }
+                        while (data != 0) {
+                                if (count >= 10)
+                                        return -EFAULT; /* focuser not ready */
+
+                                usleep_range(10, 20);
+                                err = regmap_read(priv->regmap8, val,
+                                        (unsigned int *) &data);
+                                if (err) {
+                                        pr_err("%s: regmap_read: %d\n",
+                                                __func__, err);
+                                        return err;
+                                }
+                                count++;
+                        }
+                        continue;
+                }
+
+                if (next->type == LC898212_RAM_ACCESS)
+                        err = regmap_write(priv->regmap16, next->addr, val);
+                else
+                        err = regmap_write(priv->regmap8, next->addr, val);
+                if (err) {
+                        pr_err("%s:lc898212_write_table:%d", __func__, err);
+                        return err;
+                }
+        }
+        return 0;
+}
+
+static unsigned int convert_signed16b_to_unsigned10b(s16 data)
+{
+        return (u16)(data / 64 + (AF_RANGE>>1));
+}
+
+static int lc898212_init(struct lc898212 *priv)
+{
+        int err;
+        int data;
+
+        err = lc898212_write_table(priv, lc898212_init_setting);
+
+        err |= regmap_read(priv->regmap16, LC898212_ADOFFSET, &data);
+        priv->s_data->def_position =
+                        convert_signed16b_to_unsigned10b((s16)(data & 0xffff));
+        err |= regmap_write(priv->regmap16, LC898212_RZ, data);
+
+        /* Servo On */
+        err |= regmap_write(priv->regmap8, LC898212_EQENBL, 0x85);
+
+        return err;
+}
+
+static int lc898212_load_config(struct camera_common_focuser_data *s_data)
+{
+        struct nv_focuser_config *cfg = &s_data->config;
+
+        /* load default configuration */
+        /* TODO: parse these values from DT */
+
+        cfg->focal_length = FOCAL_LENGTH;
+        cfg->fnumber = FNUMBER;
+        cfg->max_aperture = MAX_APERTURE;
+        cfg->range_ends_reversed = 0;
+
+        cfg->pos_working_low = LC898212_FOCUS_INFINITY;
+        cfg->pos_working_high = LC898212_FOCUS_MACRO;
+        cfg->pos_actual_low = LC898212_POS_LOW_DEFAULT;
+        cfg->pos_actual_high = LC898212_POS_HIGH_DEFAULT;
+
+        cfg->num_focuser_sets = 1;
+        cfg->focuser_set[0].macro = LC898212_FOCUS_MACRO;
+        cfg->focuser_set[0].hyper = LC898212_FOCUS_INFINITY;
+        cfg->focuser_set[0].inf = LC898212_FOCUS_INFINITY;
+        cfg->focuser_set[0].settle_time = SETTLETIME_MS;
+
+        return 0;
+}
+
+static int lc898212_power_off(struct camera_common_focuser_data *s_data)
+{
+        struct lc898212 *priv = (struct lc898212 *)s_data->priv;
+
+        dev_dbg(&s_data->i2c_client->dev, "%s++\n", __func__);
+        if (priv->regulator)
+                regulator_disable(priv->regulator);
+        s_data->pwr_dev = LC898212_PWR_DEV_OFF;
+
+        return 0;
+}
+
+static int lc898212_power_on(struct camera_common_focuser_data *s_data)
+{
+        int err = 0;
+        struct lc898212 *priv = (struct lc898212 *)s_data->priv;
+
+        dev_dbg(&s_data->i2c_client->dev, "%s++\n", __func__);
+        if (priv->regulator) {
+                err = regulator_enable(priv->regulator);
+                if (err) {
+                        dev_err(&s_data->i2c_client->dev,
+                                "%s:regulator enabled failed\n", __func__);
+                        return err;
+                }
+        }
+
+        err = lc898212_init(priv);
+        if (err)
+                return err;
+
+        s_data->pwr_dev = LC898212_PWR_DEV_ON;
+
+        return 0;
+}
+
+static struct camera_common_focuser_ops lc898212_ops = {
+        .power_on = lc898212_power_on,
+        .power_off = lc898212_power_off,
+        .load_config = lc898212_load_config,
+        .ctrls_init = lc898212_ctrls_init,
+};
+
+#if 0
+static int lc898212_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_focuser_data *s_data =
+                to_camera_common_focuser_data(client);
+        struct lc898212 *priv = (struct lc898212 *)s_data->priv;
+        struct v4l2_control control;
+        int err = 0;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+        err = lc898212_init(priv);
+        if (err)
+                return err;
+
+        /* write override registers for focus position */
+        control.id = V4L2_CID_FOCUS_ABSOLUTE;
+        err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+        err |= lc898212_set_position(priv, control.value);
+        if (err)
+                dev_dbg(&client->dev, "%s:warning focus pos %d set failed\n",
+                        __func__, control.value);
+
+        dev_dbg(&client->dev, "%s--\n", __func__);
+        return 0;
+}
+
+static struct v4l2_subdev_video_ops lc898212_subdev_video_ops = {
+        .s_stream       = lc898212_s_stream,
+};
+
+#endif
+
+static int lc898212_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        dev_dbg(&client->dev, "%s:\n", __func__);
+        return 0;
+}
+
+static struct v4l2_subdev_core_ops lc898212_subdev_core_ops = {
+        .s_power        = camera_common_focuser_s_power,
+};
+
+static struct v4l2_subdev_ops lc898212_subdev_ops = {
+        .core   = &lc898212_subdev_core_ops,
+};
+
+static const struct v4l2_subdev_internal_ops lc898212_subdev_internal_ops = {
+        .open = lc898212_open,
+};
+
+static const struct media_entity_operations lc898212_media_ops = {
+#ifdef CONFIG_MEDIA_CONTROLLER
+        .link_validate = v4l2_subdev_link_validate,
+#endif
+};
+
+static int lc898212_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        int err;
+        struct lc898212 *priv;
+        struct camera_common_focuser_data *common_data;
+        char dev_id[20];
+        const char *p_mfi_str;
+
+        static struct regmap_config lc898212_regmap_config8 = {
+                .reg_bits = 8,
+                .val_bits = 8,
+                .name = "8bit",
+        };
+        static struct regmap_config lc898212_regmap_config16 = {
+                .reg_bits = 8,
+                .val_bits = 16,
+                .name = "16bit",
+        };
+
+        pr_info("[lc898212]: probing sensor\n");
+
+        common_data = devm_kzalloc(&client->dev,
+                sizeof(struct camera_common_focuser_data), GFP_KERNEL);
+        if (!common_data)
+                return -ENOMEM;
+
+        priv = devm_kzalloc(&client->dev, (sizeof(struct lc898212) +
+                sizeof(struct v4l2_ctrl *) * NUM_FOCUS_CTRLS), GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
+
+        priv->regulator = devm_regulator_get(&client->dev, "vvcm");
+        if (IS_ERR(priv->regulator)) {
+                dev_err(&client->dev, "unable to get regulator %s\n",
+                        dev_name(&client->dev));
+                priv->regulator = NULL;
+        }
+
+        priv->regmap8 = devm_regmap_init_i2c(client,
+                &lc898212_regmap_config8);
+        if (IS_ERR(priv->regmap8)) {
+                err = PTR_ERR(priv->regmap8);
+                dev_err(&client->dev,
+                        "Failed to allocate register map 8: %d\n", err);
+                goto ERROR_RET;
+        }
+        priv->regmap16 = devm_regmap_init_i2c(client,
+                &lc898212_regmap_config16);
+        if (IS_ERR(priv->regmap16)) {
+                err = PTR_ERR(priv->regmap16);
+                dev_err(&client->dev,
+                        "Failed to allocate register map 16: %d\n", err);
+                goto ERROR_RET;
+        }
+
+        common_data->ops = &lc898212_ops;
+        common_data->ctrl_handler = &priv->ctrl_handler;
+        common_data->i2c_client = client;
+        common_data->ctrls = priv->ctrls;
+        common_data->priv = (void *)priv;
+        priv->numctrls = NUM_FOCUS_CTRLS;
+        priv->i2c_client                = client;
+        priv->s_data                    = common_data;
+        priv->subdev                    = &common_data->subdev;
+        priv->subdev->dev               = &client->dev;
+
+        if (client->dev.of_node) {
+                err = of_property_read_string(client->dev.of_node,
+                                        "support_mfi",
+                                        &p_mfi_str);
+                if (err < 0) {
+                        dev_err(&client->dev,
+                                        "%s unable to read MFI property\n",
+                                        __func__);
+                        goto ERROR_RET;
+                }
+
+                priv->support_mfi = !strcmp(p_mfi_str, "true") ? true : false;
+
+                if (priv->support_mfi) {
+                        int remain;
+
+                        memset(dev_id, 0, sizeof(dev_id));
+                        strncpy(dev_id, "lc898212", (sizeof(dev_id) - 1));
+                        /* calculate how many bytes remaining in dev_id[] */
+                        remain = sizeof(dev_id) - strlen(dev_id) - 1;
+                        /* strncat most of 'remain' bytes from dev_name[] */
+                        strncat(dev_id, dev_name(&client->dev), remain);
+                        err = tegra_camera_dev_mfi_add_regmap(&priv->cmfi_dev16,
+                                dev_id, priv->regmap16);
+                        if (err < 0) {
+                                dev_err(&client->dev,
+                                        "%s unable to add to mfi regmap\n",
+                                        __func__);
+                                goto ERROR_RET;
+                        }
+                }
+        }
+
+        err = camera_common_focuser_init(common_data);
+        if (err) {
+                dev_err(&client->dev, "unable to initialize focuser\n");
+                goto ERROR_RET;
+        }
+
+        v4l2_i2c_subdev_init(priv->subdev, client, &lc898212_subdev_ops);
+
+        priv->subdev->internal_ops = &lc898212_subdev_internal_ops;
+        priv->subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+                     V4L2_SUBDEV_FL_HAS_EVENTS;
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+        priv->subdev->entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
+        priv->subdev->entity.ops = &lc898212_media_ops;
+        err = media_entity_init(&priv->subdev->entity, 1, &priv->pad, 0);
+        if (err < 0) {
+                dev_err(&client->dev, "unable to init media entity\n");
+                goto ERROR_RET;
+        }
+#endif
+
+        err = v4l2_async_register_subdev(priv->subdev);
+        if (err)
+                goto ERROR_RET;
+
+        dev_dbg(&client->dev, "Detected lc898212 sensor\n");
+        return 0;
+
+ERROR_RET:
+        dev_err(&client->dev, "lc898212: probing sensor failed!!\n");
+        return err;
+}
+
+static int lc898212_remove(struct i2c_client *client)
+{
+        struct camera_common_focuser_data *s_data =
+                        to_camera_common_focuser_data(client);
+        struct lc898212 *priv = (struct lc898212 *)s_data->priv;
+
+        v4l2_async_unregister_subdev(priv->subdev);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev->entity);
+#endif
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+
+        return 0;
+}
+
+static const struct i2c_device_id lc898212_id[] = {
+        { "lc898212", 0 },
+        { },
+};
+
+MODULE_DEVICE_TABLE(i2c, lc898212_id);
+
+static struct i2c_driver lc898212_i2c_driver = {
+        .driver = {
+                .name = "lc898212",
+                .owner = THIS_MODULE,
+                .of_match_table = of_match_ptr(lc898212_of_match),
+        },
+        .probe = lc898212_probe,
+        .remove = lc898212_remove,
+        .id_table = lc898212_id,
+};
+
+module_i2c_driver(lc898212_i2c_driver);
+
+MODULE_DESCRIPTION("I2C driver for LC898212");
+MODULE_AUTHOR("Bhanu Murthy V <bmurthyv@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov10823.c b/drivers/media/i2c/ov10823.c
new file mode 100644
index 000000000..660578777
--- /dev/null
+++ b/drivers/media/i2c/ov10823.c
@@ -0,0 +1,1224 @@
+/*
+ * ov10823.c - ov10823 sensor driver
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+
+#include <linux/seq_file.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <media/tegra_v4l2_camera.h>
+#include <media/camera_common.h>
+#include <media/ov10823.h>
+
+#include "ov10823_mode_tbls.h"
+
+#define OV10823_SC_CHIP_ID_HIGH_ADDR    0x300A
+#define OV10823_SC_CHIP_ID_LOW_ADDR     0x300B
+#define OV10823_SC_SCCB_ID_ADDR         0x300C
+
+#define OV10823_MAX_COARSE_DIFF 8
+
+#define OV10823_GAIN_SHIFT              8
+#define OV10823_MIN_GAIN                (1 << OV10823_GAIN_SHIFT)
+#define OV10823_MAX_GAIN \
+        ((15 << OV10823_GAIN_SHIFT) | (1 << (OV10823_GAIN_SHIFT - 1)))
+#define OV10823_MIN_FRAME_LENGTH                (0x04F0)
+#define OV10823_MAX_FRAME_LENGTH                (0x7FFF)
+#define OV10823_MIN_EXPOSURE_COARSE             (0x8)
+#define OV10823_MAX_EXPOSURE_COARSE     \
+        (OV10823_MAX_FRAME_LENGTH-OV10823_MAX_COARSE_DIFF)
+
+#define OV10823_DEFAULT_GAIN            OV10823_MIN_GAIN
+#define OV10823_DEFAULT_FRAME_LENGTH            OV10823_MIN_FRAME_LENGTH
+#define OV10823_DEFAULT_EXPOSURE_COARSE \
+        (OV10823_DEFAULT_FRAME_LENGTH-OV10823_MAX_COARSE_DIFF)
+
+#define OV10823_DEFAULT_MODE            OV10823_MODE_2168X1220_60FPS
+#define OV10823_DEFAULT_WIDTH           2168
+#define OV10823_DEFAULT_HEIGHT          1220
+#define OV10823_DEFAULT_DATAFMT         MEDIA_BUS_FMT_SRGGB10_1X10
+#define OV10823_DEFAULT_CLK_FREQ        26000000
+
+#define OV10823_DEFAULT_I2C_ADDRESS_20  (0x20 >> 1)
+#define OV10823_DEFAULT_I2C_ADDRESS_6C  (0x6C >> 1)
+
+struct ov10823 {
+        struct camera_common_power_rail power;
+        int                             num_ctrls;
+        int                             fsync;
+        int                             cam_sid_gpio;
+        int                             mcu_boot_gpio;
+        int                             mcu_reset_gpio;
+        struct v4l2_ctrl_handler        ctrl_handler;
+        struct i2c_client               *i2c_client;
+        struct v4l2_subdev              *subdev;
+        struct media_pad                pad;
+
+        s32                             group_hold_prev;
+        bool                            group_hold_en;
+        struct regmap                   *regmap;
+        struct camera_common_data       *s_data;
+        struct camera_common_pdata      *pdata;
+        struct v4l2_ctrl                *ctrls[];
+};
+
+static const struct regmap_config sensor_regmap_config = {
+        .reg_bits = 16,
+        .val_bits = 8,
+        .cache_type = REGCACHE_RBTREE,
+};
+
+u16 ov10823_to_gain(u32 rep, int shift)
+{
+        u16 gain;
+        int gain_int;
+        int gain_dec;
+        int min_int = (1 << shift);
+
+        if (rep < OV10823_MIN_GAIN)
+                rep = OV10823_MIN_GAIN;
+        else if (rep > OV10823_MAX_GAIN)
+                rep = OV10823_MAX_GAIN;
+
+        /* shift indicates number of least significant bits */
+        /* used for decimal representation of gain */
+        gain_int = (int)(rep >> shift);
+        gain_dec = (int)(rep & ~(0xffff << shift));
+
+        /* derived from formulat gain = (x * 16 + 0.5) */
+        gain = ((gain_int * min_int + gain_dec) * 32 + min_int) / (2 * min_int);
+
+        return gain;
+}
+
+static int ov10823_g_volatile_ctrl(struct v4l2_ctrl *ctrl);
+static int ov10823_s_ctrl(struct v4l2_ctrl *ctrl);
+
+static const struct v4l2_ctrl_ops ov10823_ctrl_ops = {
+        .g_volatile_ctrl = ov10823_g_volatile_ctrl,
+        .s_ctrl         = ov10823_s_ctrl,
+};
+
+static struct v4l2_ctrl_config ctrl_config_list[] = {
+/* Do not change the name field for the controls! */
+        {
+                .ops = &ov10823_ctrl_ops,
+                .id = V4L2_CID_GAIN,
+                .name = "Gain",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV10823_MIN_GAIN,
+                .max = OV10823_MAX_GAIN,
+                .def = OV10823_DEFAULT_GAIN,
+                .step = 1,
+        },
+        {
+                .ops = &ov10823_ctrl_ops,
+                .id = V4L2_CID_FRAME_LENGTH,
+                .name = "Frame Length",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV10823_MIN_FRAME_LENGTH,
+                .max = OV10823_MAX_FRAME_LENGTH,
+                .def = OV10823_DEFAULT_FRAME_LENGTH,
+                .step = 1,
+        },
+        {
+                .ops = &ov10823_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME,
+                .name = "Coarse Time",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV10823_MIN_EXPOSURE_COARSE,
+                .max = OV10823_MAX_EXPOSURE_COARSE,
+                .def = OV10823_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &ov10823_ctrl_ops,
+                .id = V4L2_CID_GROUP_HOLD,
+                .name = "Group Hold",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &ov10823_ctrl_ops,
+                .id = V4L2_CID_HDR_EN,
+                .name = "HDR enable",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &ov10823_ctrl_ops,
+                .id = V4L2_CID_OTP_DATA,
+                .name = "OTP Data",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = OV10823_OTP_STR_SIZE,
+                .step = 2,
+        },
+        {
+                .ops = &ov10823_ctrl_ops,
+                .id = V4L2_CID_FUSE_ID,
+                .name = "Fuse ID",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = OV10823_FUSE_ID_STR_SIZE,
+                .step = 2,
+        },
+};
+
+static inline void ov10823_get_frame_length_regs(ov10823_reg *regs,
+                                u16 frame_length, int fsync)
+{
+        /* 2 registers for FL, i.e., 2-byte FL */
+        regs->addr = 0x380e;
+        regs->val = (frame_length >> 8) & 0xff;
+        (regs + 1)->addr = 0x380f;
+        (regs + 1)->val = (frame_length) & 0xff;
+        if (fsync == OV10823_FSYNC_SLAVE) {
+                (regs + 2)->addr = 0x3826;
+                (regs + 2)->val = ((frame_length - 4) >> 8) & 0xff;
+                (regs + 3)->addr = 0x3827;
+                (regs + 3)->val = (frame_length - 4) & 0xff;
+        } else {
+                (regs + 2)->addr = 0x3830;
+                (regs + 2)->val = ((frame_length - 4) >> 8) & 0xff;
+                (regs + 3)->addr = 0x3831;
+                (regs + 3)->val = (frame_length - 4) & 0xff;
+        }
+        (regs + 4)->addr = OV10823_TABLE_END;
+        (regs + 4)->val = 0;
+}
+
+static inline void ov10823_get_coarse_time_regs(ov10823_reg *regs,
+                                u16 coarse_time)
+{
+        /* 3 registers for CT, i.e., 3-byte CT */
+        regs->addr = 0x3500;
+        regs->val = (coarse_time >> 12) & 0xff;
+        (regs + 1)->addr = 0x3501;
+        (regs + 1)->val = (coarse_time >> 4) & 0xff;
+        (regs + 2)->addr = 0x3502;
+        (regs + 2)->val = (coarse_time & 0xf) << 4;
+        (regs + 3)->addr = OV10823_TABLE_END;
+        (regs + 3)->val = 0;
+}
+
+static inline void ov10823_get_gain_reg(ov10823_reg *regs,
+                                u16 gain)
+{
+        /* 2 register for gain, i.e., 2-byte gain */
+        regs->addr = 0x350a;
+        regs->val = (gain >> 8) & 0xff;
+        (regs + 1)->addr = 0x350b;
+        (regs + 1)->val = (gain) & 0xff;
+        (regs + 2)->addr = OV10823_TABLE_END;
+        (regs + 2)->val = 0;
+}
+
+static inline int ov10823_read_reg(struct camera_common_data *s_data,
+                                u16 addr, u8 *val)
+{
+        struct ov10823 *priv = (struct ov10823 *)s_data->priv;
+        unsigned int temp_val;
+        int err;
+
+        err = regmap_read(priv->regmap, addr, &temp_val);
+        if (!err)
+                *val = temp_val;
+
+        return err;
+}
+
+static int ov10823_write_reg(struct camera_common_data *s_data,
+                u16 addr, u8 val)
+{
+        int err;
+        struct ov10823 *priv = (struct ov10823 *)s_data->priv;
+
+        err = regmap_write(priv->regmap, addr, val);
+        if (err)
+                pr_err("%s:i2c write failed, %x = %x\n",
+                        __func__, addr, val);
+
+        return err;
+}
+
+static int ov10823_write_table(struct ov10823 *priv,
+                                const ov10823_reg table[])
+{
+        return regmap_util_write_table_8(priv->regmap,
+                                         table,
+                                         NULL, 0,
+                                         OV10823_TABLE_WAIT_MS,
+                                         OV10823_TABLE_END);
+}
+
+static int ov10823_power_on(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct ov10823 *priv = (struct ov10823 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power on\n", __func__);
+
+        if (priv->pdata->power_on) {
+                err = priv->pdata->power_on(pw);
+                if (err)
+                        pr_err("%s failed.\n", __func__);
+                else
+                        pw->state = SWITCH_ON;
+                return err;
+        }
+
+        usleep_range(5350, 5360);
+        pw->state = SWITCH_ON;
+        return 0;
+}
+
+static int ov10823_power_off(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct ov10823 *priv = (struct ov10823 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power off\n", __func__);
+        ov10823_write_table(priv, mode_table[OV10823_MODE_STOP_STREAM]);
+
+        if (priv->pdata->power_off) {
+                err = priv->pdata->power_off(pw);
+                if (err)
+                        pr_err("%s failed.\n", __func__);
+                else
+                        goto power_off_done;
+        }
+
+        return err;
+
+power_off_done:
+        pw->state = SWITCH_OFF;
+        return 0;
+}
+
+static int ov10823_power_put(struct ov10823 *priv)
+{
+        return 0;
+}
+
+static int ov10823_power_get(struct ov10823 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        const char *mclk_name;
+        int err = 0;
+
+        mclk_name = priv->pdata->mclk_name ?
+                    priv->pdata->mclk_name : "cam_mclk1";
+        pw->mclk = devm_clk_get(&priv->i2c_client->dev, mclk_name);
+        if (IS_ERR(pw->mclk)) {
+                dev_err(&priv->i2c_client->dev,
+                        "unable to get clock %s\n", mclk_name);
+                return PTR_ERR(pw->mclk);
+        }
+
+        pw->state = SWITCH_OFF;
+        return err;
+}
+
+static int ov10823_verify_chip_id(struct ov10823 *priv)
+{
+        struct i2c_client *client = priv->i2c_client;
+        struct camera_common_data *s_data = priv->s_data;
+        u8 chip_id_hi, chip_id_lo;
+        u16 chip_id;
+        int err;
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        err = ov10823_read_reg(s_data, OV10823_SC_CHIP_ID_HIGH_ADDR,
+                               &chip_id_hi);
+        if (err) {
+                dev_err(&client->dev, "Failed to read chip ID\n");
+                return err;
+        }
+        err = ov10823_read_reg(s_data, OV10823_SC_CHIP_ID_LOW_ADDR,
+                               &chip_id_lo);
+        if (err) {
+                dev_err(&client->dev, "Failed to read chip ID\n");
+                return err;
+        }
+
+        chip_id = (chip_id_hi << 8) | chip_id_lo;
+        if (chip_id != 0xA820) {
+                dev_err(&client->dev, "Read unknown chip ID 0x%04x\n", chip_id);
+                return -EINVAL;
+        }
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int ov10823_set_gain(struct ov10823 *priv, s32 val);
+static int ov10823_set_frame_length(struct ov10823 *priv, s32 val);
+static int ov10823_set_coarse_time(struct ov10823 *priv, s32 val);
+
+static int ov10823_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov10823 *priv = (struct ov10823 *)s_data->priv;
+        struct v4l2_control control;
+        int err;
+
+        if (!enable) {
+                dev_dbg(&client->dev, "%s: stream off\n", __func__);
+                return ov10823_write_table(priv,
+                        mode_table[OV10823_MODE_STOP_STREAM]);
+                }
+
+        dev_dbg(&client->dev, "%s: write mode table %d\n",
+                __func__, s_data->mode);
+        err = ov10823_write_table(priv, mode_table[s_data->mode]);
+
+        if (fsync_table[priv->fsync]) {
+                dev_dbg(&client->dev, "%s: write fsync table %d\n", __func__,
+                        priv->fsync);
+                err = ov10823_write_table(priv, fsync_table[priv->fsync]);
+                if (err)
+                        goto exit;
+        }
+
+        if (s_data->override_enable) {
+                /* write list of override regs for the asking frame length, */
+                /* coarse integration time, and gain. Failures to write */
+                /* overrides are non-fatal. */
+                control.id = V4L2_CID_GAIN;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov10823_set_gain(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: error gain override\n", __func__);
+
+                control.id = V4L2_CID_FRAME_LENGTH;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov10823_set_frame_length(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: error frame length override\n", __func__);
+
+                control.id = V4L2_CID_COARSE_TIME;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov10823_set_coarse_time(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: error coarse time override\n", __func__);
+        }
+
+        dev_dbg(&client->dev, "%s: stream on\n", __func__);
+        err = ov10823_write_table(priv, mode_table[OV10823_MODE_START_STREAM]);
+        if (err)
+                goto exit;
+
+        /*
+         * If the sensor is in fsync slave mode, and is in the middle of
+         * sending a frame when it gets a strobe on the fsin pin, it may
+         * prematurely end the frame, resulting in a short frame on our
+         * camera host.  So, after starting streaming, we assume fsync
+         * master has already been told to start streaming, and we wait some
+         * amount of time in order to skip the possible short frame.  The
+         * length of time to wait should be at least our sample period.
+         * Assume worse case of 30fps (33.3ms), and add a bit more.
+         */
+        if (priv->fsync == OV10823_FSYNC_SLAVE)
+                msleep(40);
+
+        return 0;
+exit:
+        dev_dbg(&client->dev, "%s: error setting stream\n", __func__);
+        return err;
+}
+
+static int ov10823_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov10823 *priv = (struct ov10823 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        *status = pw->state == SWITCH_ON;
+        return 0;
+}
+
+static struct v4l2_subdev_video_ops ov10823_subdev_video_ops = {
+        .s_stream       = ov10823_s_stream,
+        .g_mbus_config  = camera_common_g_mbus_config,
+        .g_input_status = ov10823_g_input_status,
+};
+
+static struct v4l2_subdev_core_ops ov10823_subdev_core_ops = {
+        .s_power        = camera_common_s_power,
+};
+
+static int ov10823_get_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        return camera_common_g_fmt(sd, &format->format);
+}
+
+static int ov10823_set_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        int ret;
+
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+                ret = camera_common_try_fmt(sd, &format->format);
+        else
+                ret = camera_common_s_fmt(sd, &format->format);
+
+        return ret;
+}
+
+static struct v4l2_subdev_pad_ops ov10823_subdev_pad_ops = {
+        .set_fmt = ov10823_set_fmt,
+        .get_fmt = ov10823_get_fmt,
+        .enum_mbus_code = camera_common_enum_mbus_code,
+        .enum_frame_size        = camera_common_enum_framesizes,
+        .enum_frame_interval    = camera_common_enum_frameintervals,
+};
+
+static struct v4l2_subdev_ops ov10823_subdev_ops = {
+        .core   = &ov10823_subdev_core_ops,
+        .video  = &ov10823_subdev_video_ops,
+        .pad    = &ov10823_subdev_pad_ops,
+};
+
+static struct of_device_id ov10823_of_match[] = {
+        { .compatible = "nvidia,ov10823", },
+        { },
+};
+
+static struct camera_common_sensor_ops ov10823_common_ops = {
+        .power_on = ov10823_power_on,
+        .power_off = ov10823_power_off,
+        .write_reg = ov10823_write_reg,
+        .read_reg = ov10823_read_reg,
+};
+
+static int ov10823_set_group_hold(struct ov10823 *priv)
+{
+        int err;
+        int gh_prev = switch_ctrl_qmenu[priv->group_hold_prev];
+
+        if (priv->group_hold_en == true && gh_prev == SWITCH_OFF) {
+                /* group hold start */
+                err = ov10823_write_reg(priv->s_data,
+                                       OV10823_GROUP_HOLD_ADDR, 0x00);
+                if (err)
+                        goto fail;
+                priv->group_hold_prev = 1;
+        } else if (priv->group_hold_en == false && gh_prev == SWITCH_ON) {
+                /* group hold end */
+                err = ov10823_write_reg(priv->s_data,
+                                       OV10823_GROUP_HOLD_ADDR, 0x10);
+                /* quick launch */
+                err |= ov10823_write_reg(priv->s_data,
+                                       OV10823_GROUP_HOLD_ADDR, 0xA0);
+                if (err)
+                        goto fail;
+                priv->group_hold_prev = 0;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: Group hold control error\n", __func__);
+        return err;
+}
+
+static int ov10823_set_gain(struct ov10823 *priv, s32 val)
+{
+        ov10823_reg reg_list[3];
+        int err;
+        u16 gain;
+
+        /* max_gain 15.5x ---> 0x350A=0x00, 0x350B=0xF8 */
+        /* min_gain 1.0x  ---> 0x350A=0x00, 0x350B=0x10 */
+        /* translate value */
+        gain = ov10823_to_gain((u32)val, OV10823_GAIN_SHIFT);
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: gain: %d\n", __func__, gain);
+
+        ov10823_get_gain_reg(reg_list, gain);
+        ov10823_set_group_hold(priv);
+        err = ov10823_write_table(priv, reg_list);
+        if (err)
+                goto fail;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: GAIN control error\n", __func__);
+        return err;
+}
+
+static int ov10823_set_frame_length(struct ov10823 *priv, s32 val)
+{
+        ov10823_reg reg_list[5];
+        int err;
+        u16 frame_length;
+
+        /*
+         * This is a workaround for nvbug 1865041, where setting the VTS
+         * timing registers when the sensor is set up for fsync master or
+         * slave leads to streaming instability.
+         */
+        if (priv->fsync != OV10823_FSYNC_NONE)
+                return 0;
+
+        frame_length = (u16)val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: frame_length: %d\n", __func__, frame_length);
+
+        ov10823_get_frame_length_regs(reg_list, frame_length, priv->fsync);
+        ov10823_set_group_hold(priv);
+        err = ov10823_write_table(priv, reg_list);
+        if (err)
+                goto fail;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: FRAME_LENGTH control error\n", __func__);
+        return err;
+}
+
+static int ov10823_set_coarse_time(struct ov10823 *priv, s32 val)
+{
+        ov10823_reg reg_list[4];
+        int err;
+        u16 coarse_time;
+
+        coarse_time = (u16)val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: coarse_time: %d\n", __func__, coarse_time);
+
+        ov10823_get_coarse_time_regs(reg_list, coarse_time);
+        ov10823_set_group_hold(priv);
+        err = ov10823_write_table(priv, reg_list);
+        if (err)
+                goto fail;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: COARSE_TIME control error\n", __func__);
+        return err;
+}
+
+static int ov10823_read_otp(struct ov10823 *priv, u8 *buf,
+                u16 addr, int size)
+{
+        int err;
+
+        err = ov10823_write_reg(priv->s_data, OV10823_ISP_CTRL_ADDR, 0x00);
+        if (err)
+                return err;
+        /* Start streaming before write or read */
+        err = ov10823_write_reg(priv->s_data, 0x0100, 0x01);
+        if (err)
+                return err;
+        msleep(20);
+
+        /* By default otp loading works in auto mode, but we can switch to */
+        /* manual mode through OV10823_OTP_MODE_CTRL_ADDR[6] and the start */
+        /* addr and end addr of manual mode can be configured by registers */
+        /* accordingly */
+
+        /* Loading enable */
+        /* 1: manual mode */
+        /* 0: auto mode   */
+        err = ov10823_write_reg(priv->s_data, OV10823_OTP_LOAD_CTRL_ADDR, 0x01);
+        if (err)
+                return err;
+
+        msleep(20);
+        err = regmap_bulk_read(priv->regmap, addr, buf, size);
+        if (err)
+                return err;
+
+        return 0;
+}
+
+static int ov10823_otp_setup(struct ov10823 *priv)
+{
+        int err;
+        int i;
+        struct v4l2_ctrl *ctrl;
+        u8 otp_buf[OV10823_OTP_SIZE];
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        ov10823_read_otp(priv, &otp_buf[0],
+                                   OV10823_OTP_SRAM_START_ADDR,
+                                   OV10823_OTP_SIZE);
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_OTP_DATA);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return -EINVAL;
+        }
+
+        for (i = 0; i < OV10823_OTP_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x",
+                        otp_buf[i]);
+        ctrl->p_cur.p_char = ctrl->p_new.p_char;
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int ov10823_fuse_id_setup(struct ov10823 *priv)
+{
+        int err;
+        int i;
+        struct v4l2_ctrl *ctrl;
+        u8 fuse_id[OV10823_FUSE_ID_SIZE];
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        ov10823_read_otp(priv, &fuse_id[0],
+                                   OV10823_FUSE_ID_OTP_BASE_ADDR,
+                                   OV10823_FUSE_ID_SIZE);
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_FUSE_ID);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return -EINVAL;
+        }
+
+        for (i = 0; i < OV10823_FUSE_ID_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x",
+                        fuse_id[i]);
+        ctrl->p_cur.p_char = ctrl->p_new.p_char;
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int ov10823_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct ov10823 *priv =
+                container_of(ctrl->handler, struct ov10823, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        default:
+                        pr_err("%s: unknown ctrl id.\n", __func__);
+                        return -EINVAL;
+        }
+
+        return err;
+}
+
+static int ov10823_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct ov10823 *priv =
+                container_of(ctrl->handler, struct ov10823, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_GAIN:
+                err = ov10823_set_gain(priv, ctrl->val);
+                break;
+        case V4L2_CID_FRAME_LENGTH:
+                err = ov10823_set_frame_length(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME:
+                err = ov10823_set_coarse_time(priv, ctrl->val);
+                break;
+        case V4L2_CID_GROUP_HOLD:
+                if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON) {
+                        priv->group_hold_en = true;
+                } else {
+                        priv->group_hold_en = false;
+                        err = ov10823_set_group_hold(priv);
+                }
+                break;
+        case V4L2_CID_HDR_EN:
+                break;
+        default:
+                pr_err("%s: unknown ctrl id.\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static int ov10823_ctrls_init(struct ov10823 *priv)
+{
+        struct i2c_client *client = priv->i2c_client;
+        struct v4l2_ctrl *ctrl;
+        int num_ctrls;
+        int err;
+        int i;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        num_ctrls = ARRAY_SIZE(ctrl_config_list);
+        v4l2_ctrl_handler_init(&priv->ctrl_handler, num_ctrls);
+
+        for (i = 0; i < num_ctrls; i++) {
+                ctrl = v4l2_ctrl_new_custom(&priv->ctrl_handler,
+                        &ctrl_config_list[i], NULL);
+                if (ctrl == NULL) {
+                        dev_err(&client->dev, "Failed to init %s ctrl\n",
+                                ctrl_config_list[i].name);
+                        continue;
+                }
+
+                if (ctrl_config_list[i].type == V4L2_CTRL_TYPE_STRING &&
+                        ctrl_config_list[i].flags & V4L2_CTRL_FLAG_READ_ONLY) {
+                        ctrl->p_new.p_char = devm_kzalloc(&client->dev,
+                                ctrl_config_list[i].max + 1, GFP_KERNEL);
+                }
+                priv->ctrls[i] = ctrl;
+        }
+
+        priv->num_ctrls = num_ctrls;
+        priv->subdev->ctrl_handler = &priv->ctrl_handler;
+        if (priv->ctrl_handler.error) {
+                dev_err(&client->dev, "Error %d adding controls\n",
+                        priv->ctrl_handler.error);
+                err = priv->ctrl_handler.error;
+                goto error;
+        }
+
+        err = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d setting default controls\n", err);
+                goto error;
+        }
+
+        return 0;
+
+error:
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        return err;
+}
+
+MODULE_DEVICE_TABLE(of, ov10823_of_match);
+
+static int ov10823_parse_dt(struct i2c_client *client, struct ov10823 *priv)
+{
+        struct device_node *np = client->dev.of_node;
+        const char *fsync_str;
+        int gpio;
+        int err;
+
+        err = of_property_read_string(np, "mclk", &priv->pdata->mclk_name);
+        if (err) {
+                dev_err(&client->dev, "mclk not in DT\n");
+                return -EINVAL;
+        }
+
+        err = of_property_read_string(np, "fsync", &fsync_str);
+        if (!err && fsync_str && (strcmp(fsync_str, "master") == 0))
+                priv->fsync = OV10823_FSYNC_MASTER;
+        else if (!err && fsync_str && (strcmp(fsync_str, "slave") == 0))
+                priv->fsync = OV10823_FSYNC_SLAVE;
+        else
+                priv->fsync = OV10823_FSYNC_NONE;
+
+        gpio = of_get_named_gpio(np, "pwdn-gpios", 0);
+        if (gpio < 0) {
+                dev_dbg(&client->dev, "pwdn gpios not in DT\n");
+                gpio = 0;
+        }
+        priv->pdata->pwdn_gpio = (unsigned int)gpio;
+
+        gpio = of_get_named_gpio(np, "reset-gpios", 0);
+        if (gpio < 0) {
+                dev_dbg(&client->dev, "reset gpios not in DT\n");
+                gpio = 0;
+        }
+        priv->pdata->reset_gpio = (unsigned int)gpio;
+
+        priv->mcu_boot_gpio =
+                of_get_named_gpio(np, "mcu-boot-gpios", 0);
+        priv->mcu_reset_gpio =
+                of_get_named_gpio(np, "mcu-reset-gpios", 0);
+
+        priv->cam_sid_gpio = of_get_named_gpio(np, "cam-sid-gpios", 0);
+
+        return 0;
+}
+
+static int ov10823_i2c_addr_assign(struct ov10823 *priv, u8 i2c_addr)
+{
+        struct i2c_msg msg;
+        unsigned char data[3];
+        int err;
+
+        /*
+         * I wish i2c_check_addr_validity() was available.  Oh well.
+         * 7-bit address, reject the general call address
+         */
+        if ((i2c_addr == 0x00) || (i2c_addr > 0x7f))
+                return -EINVAL;
+
+        /*
+         * It seems that the way SID works for the OV10823 I2C slave address is
+         * that:
+         *
+         * SID 0 = 0x20
+         * SID 1 = 0x6c
+         *
+         * Address 0x20 is programmable via register 0x300c, and
+         * address 0x6c is programmable via register 0x3661.
+         *
+         * So, the scheme to assign addresses to an (almost) arbitrary
+         * number of sensors is to consider 0x20 to be the "off" address.
+         * Start each sensor with SID as 0 so that they appear to be off.
+         *
+         * Then, to assign an address to one sensor:
+         *
+         * 0. Set corresponding SID to 1 (now only that sensor responds
+         *    to 0x6c).
+         * 1. Use 0x6C to program address 0x20 to the new address.
+         * 2. Set corresponding SID back to 0 (so it no longer responds
+         *    to 0x6c, but instead responds to the new address).
+         */
+
+        if (i2c_addr == OV10823_DEFAULT_I2C_ADDRESS_20) {
+                dev_info(&priv->i2c_client->dev,
+                         "Using default I2C address 0x%02x\n", i2c_addr);
+                if (gpio_is_valid(priv->cam_sid_gpio)) {
+                        gpio_set_value(priv->cam_sid_gpio, 0);
+                        msleep_range(1);
+                }
+                return 0;
+        } else if (i2c_addr == OV10823_DEFAULT_I2C_ADDRESS_6C) {
+                dev_info(&priv->i2c_client->dev,
+                         "Using default I2C address 0x%02x\n", i2c_addr);
+                if (gpio_is_valid(priv->cam_sid_gpio)) {
+                        gpio_set_value(priv->cam_sid_gpio, 1);
+                        msleep_range(1);
+                }
+                return 0;
+        }
+
+        /*
+         * From this point on, we are trying to program the programmable
+         * slave address.  We necessarily need to have a cam-sid-gpio for this.
+         */
+        if (!gpio_is_valid(priv->cam_sid_gpio)) {
+                dev_err(&priv->i2c_client->dev,
+                        "Missing cam-sid-gpio, cannot program I2C address\n");
+                return -EINVAL;
+        }
+
+        gpio_set_value(priv->cam_sid_gpio, 1);
+        msleep_range(1);
+
+        dev_info(&priv->i2c_client->dev, "Changing I2C address to 0x%02x\n",
+                 i2c_addr);
+
+        /*
+         * Have to make the I2C message manually because we are using a
+         * different I2C slave address for this transaction, rather than
+         * the one in the device tree for this device.
+         */
+        data[0] = (OV10823_SC_SCCB_ID_ADDR >> 8) & 0xff;
+        data[1] = OV10823_SC_SCCB_ID_ADDR & 0xff;
+        data[2] = ((i2c_addr) << 1) & 0xff;
+        /*
+         * Use the programmable default I2C slave address so that if we have
+         * multiple sensors of this same kind, when we change one sensor's
+         * address, the next sensor address change message won't go to that
+         * same sensor.
+         */
+        msg.addr = OV10823_DEFAULT_I2C_ADDRESS_6C;
+        msg.flags = 0;
+        msg.len = 3;
+        msg.buf = data;
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                goto done;
+
+        if (i2c_transfer(priv->i2c_client->adapter, &msg, 1) != 1)
+                err = -EIO;
+
+        camera_common_s_power(priv->subdev, false);
+
+done:
+        gpio_set_value(priv->cam_sid_gpio, 0);
+        msleep_range(1);
+
+        return err;
+}
+
+static int ov10823_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        dev_dbg(&client->dev, "%s:\n", __func__);
+
+        return 0;
+}
+
+static const struct v4l2_subdev_internal_ops ov10823_subdev_internal_ops = {
+        .open = ov10823_open,
+};
+
+static const struct media_entity_operations ov10823_media_ops = {
+        .link_validate = v4l2_subdev_link_validate,
+};
+
+static int ov10823_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct camera_common_data *common_data;
+        struct ov10823 *priv;
+        char dev_name[10];
+        int err;
+
+        pr_info("[OV10823]: probing v4l2 sensor.\n");
+
+        common_data = devm_kzalloc(&client->dev,
+                            sizeof(struct camera_common_data), GFP_KERNEL);
+
+        priv = devm_kzalloc(&client->dev,
+                        sizeof(struct ov10823) + sizeof(struct v4l2_ctrl *) *
+                        ARRAY_SIZE(ctrl_config_list),
+                        GFP_KERNEL);
+        if (!priv) {
+                dev_err(&client->dev, "unable to allocate memory!\n");
+                return -ENOMEM;
+        }
+
+        priv->regmap = devm_regmap_init_i2c(client, &sensor_regmap_config);
+        if (IS_ERR(priv->regmap)) {
+                dev_err(&client->dev,
+                        "regmap init failed: %ld\n", PTR_ERR(priv->regmap));
+                return -ENODEV;
+        }
+
+        priv->pdata = devm_kzalloc(&client->dev,
+                                   sizeof(struct camera_common_pdata),
+                                   GFP_KERNEL);
+        if (!priv->pdata) {
+                dev_err(&client->dev,
+                        "unable to allocate camera_common_pdata\n");
+                return -ENOMEM;
+        }
+
+        err = ov10823_parse_dt(client, priv);
+        if (err)
+                return err;
+
+        common_data->ops                = &ov10823_common_ops;
+        common_data->ctrl_handler       = &priv->ctrl_handler;
+        common_data->i2c_client         = client;
+        common_data->frmfmt             = ov10823_frmfmt;
+        common_data->colorfmt           = camera_common_find_datafmt(
+                                          OV10823_DEFAULT_DATAFMT);
+        common_data->power              = &priv->power;
+        common_data->ctrls              = priv->ctrls;
+        common_data->priv               = (void *)priv;
+        common_data->numctrls           = ARRAY_SIZE(ctrl_config_list);
+        common_data->numfmts            = ARRAY_SIZE(ov10823_frmfmt);
+        common_data->def_mode           = OV10823_DEFAULT_MODE;
+        common_data->def_width          = OV10823_DEFAULT_WIDTH;
+        common_data->def_height         = OV10823_DEFAULT_HEIGHT;
+        common_data->def_clk_freq       = OV10823_DEFAULT_CLK_FREQ;
+        common_data->fmt_width          = common_data->def_width;
+        common_data->fmt_height         = common_data->def_height;
+
+        priv->i2c_client                = client;
+        priv->s_data                    = common_data;
+        priv->subdev                    = &common_data->subdev;
+        priv->subdev->dev               = &client->dev;
+        priv->group_hold_prev           = 0;
+
+        err = ov10823_power_get(priv);
+        if (err)
+                return err;
+
+        /*
+         * If our device tree node is given MCU GPIOs, then we are expected to
+         * reset the MCU.
+         */
+        if (gpio_is_valid(priv->mcu_boot_gpio) &&
+            gpio_is_valid(priv->mcu_reset_gpio)) {
+                dev_info(&client->dev, "Resetting MCU\n");
+                gpio_set_value(priv->mcu_boot_gpio, 0);
+                gpio_set_value(priv->mcu_reset_gpio, 0);
+                msleep_range(1);
+                gpio_set_value(priv->mcu_reset_gpio, 1);
+        }
+
+        err = camera_common_parse_ports(client, common_data);
+        if (err) {
+                dev_err(&client->dev, "Failed to find port info\n");
+                return err;
+        }
+        sprintf(dev_name, "ov10823_%c", common_data->csi_port + 'a');
+        dev_dbg(&client->dev, "%s: name %s\n", __func__, dev_name);
+        camera_common_create_debugfs(common_data, dev_name);
+
+        v4l2_i2c_subdev_init(&common_data->subdev, client,
+                             &ov10823_subdev_ops);
+
+        err = ov10823_ctrls_init(priv);
+        if (err)
+                return err;
+
+        err = ov10823_i2c_addr_assign(priv, client->addr);
+        if (err)
+                return err;
+
+        err = ov10823_verify_chip_id(priv);
+        if (err)
+                return err;
+
+        err = ov10823_otp_setup(priv);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d reading otp data\n", err);
+                return err;
+        }
+
+        err = ov10823_fuse_id_setup(priv);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d reading fuse id data\n", err);
+                return err;
+        }
+
+        priv->subdev->internal_ops = &ov10823_subdev_internal_ops;
+        priv->subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+                     V4L2_SUBDEV_FL_HAS_EVENTS;
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+        priv->subdev->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+        priv->subdev->entity.ops = &ov10823_media_ops;
+        err = media_entity_init(&priv->subdev->entity, 1, &priv->pad, 0);
+        if (err < 0) {
+                dev_err(&client->dev, "unable to init media entity\n");
+                return err;
+        }
+#endif
+
+        err = v4l2_async_register_subdev(priv->subdev);
+        if (err)
+                return err;
+
+        dev_info(&client->dev, "Detected OV10823 sensor\n");
+
+        return 0;
+}
+
+static int
+ov10823_remove(struct i2c_client *client)
+{
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov10823 *priv = (struct ov10823 *)s_data->priv;
+
+        v4l2_async_unregister_subdev(priv->subdev);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev->entity);
+#endif
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        ov10823_power_put(priv);
+        camera_common_remove_debugfs(s_data);
+
+        return 0;
+}
+
+static const struct i2c_device_id ov10823_id[] = {
+        { "ov10823", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, ov10823_id);
+
+static struct i2c_driver ov10823_i2c_driver = {
+        .driver = {
+                .name = "ov10823",
+                .owner = THIS_MODULE,
+                .of_match_table = of_match_ptr(ov10823_of_match),
+        },
+        .probe = ov10823_probe,
+        .remove = ov10823_remove,
+        .id_table = ov10823_id,
+};
+
+module_i2c_driver(ov10823_i2c_driver);
+
+MODULE_DESCRIPTION("SoC Camera driver for Omnivison OV10823");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov10823_mode_tbls.h b/drivers/media/i2c/ov10823_mode_tbls.h
new file mode 100644
index 000000000..543097d31
--- /dev/null
+++ b/drivers/media/i2c/ov10823_mode_tbls.h
@@ -0,0 +1,1637 @@
+/*
+ * ov10823.c - ov10823 sensor driver
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION, All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <media/camera_common.h>
+
+#ifndef __OV10823_I2C_TABLES__
+#define __OV10823_I2C_TABLES__
+
+#define OV10823_TABLE_WAIT_MS   0
+#define OV10823_TABLE_END       1
+#define OV10823_MAX_RETRIES     3
+#define OV10823_WAIT_MS 3
+#define OV10823_ADJUST_HS_PREPARE 1
+
+#define ov10823_reg struct reg_8
+
+static ov10823_reg ov10823_start[] = {
+        { 0x0100, 0x01 },
+        {OV10823_TABLE_WAIT_MS, 66},
+        { OV10823_TABLE_END, 0x00 }
+};
+
+static ov10823_reg ov10823_stop[] = {
+        { 0x0100, 0x00 },
+        { OV10823_TABLE_END, 0x00 }
+};
+
+static ov10823_reg fsync_master[] = {
+        {0x3002, 0x88},
+        {0x3009, 0x06},
+        {0x3823, 0x00},
+
+        {OV10823_TABLE_END, 0x00}
+};
+
+static ov10823_reg fsync_slave[] = {
+        {0x3002, 0x80},
+        {0x3009, 0x02},
+        {0x3823, 0x30},
+
+        {OV10823_TABLE_END, 0x00}
+};
+
+#ifdef OV10823_UNUSED_MODES
+static ov10823_reg mode_4336x2440_24MhzMCLK[] = {
+
+        /* PLL */
+        {0x3080, 0x04},
+        {0x3083, 0x00},
+        {0x3084, 0x03},
+        {0x308b, 0x05},
+        {0x308d, 0xae},
+        {0x308e, 0x00},
+        {0x308f, 0x09},
+
+        {0x3011, 0xd0},
+        {0x3012, 0x41},
+        {0x3031, 0x0c},
+        {0x3032, 0xc0},
+        {0x3503, 0x07},
+        {0x3603, 0x04},
+        {0x3641, 0x83},
+        {0x3643, 0x02},
+        {0x3645, 0x41},
+        {0x3646, 0x83},
+        {0x3648, 0x0d},
+        {0x3649, 0x86},
+        {0x3653, 0x37},
+        {0x3658, 0x8b},
+        {0x3659, 0x87},
+        {0x365c, 0x5a},
+        {0x3660, 0x82},
+        {0x3705, 0x22},
+        {0x3719, 0x0c},
+        {0x371b, 0x00},
+        {0x371c, 0x80},
+        {0x371e, 0x00},
+        {0x371f, 0x20},
+        {0x3720, 0xa0},
+        {0x3721, 0xf0},
+        {0x3723, 0x78},
+        {0x372a, 0x08},
+        {0x372c, 0x11},
+        {0x373c, 0x02},
+        {0x3749, 0xd7},
+        {0x374a, 0x8c},
+        {0x3752, 0x80},
+        {0x3821, 0x00},
+        {0x382e, 0x04},
+        {0x3901, 0x80},
+        {0x3902, 0xc6},
+        {0x4004, 0x00},
+        {0x4005, 0x20},
+        {0x400d, 0x10},
+        {0x401b, 0x00},
+        {0x401d, 0x00},
+        {0x401f, 0x00},
+        {0x4020, 0x01},
+        {0x4022, 0x03},
+        {0x4024, 0x0e},
+        {0x4025, 0x10},
+        {0x4026, 0x0f},
+        {0x4027, 0x9f},
+        {0x4028, 0x00},
+        {0x4029, 0x04},
+        {0x402d, 0x04},
+        {0x402f, 0x08},
+        {0x4602, 0x02},
+
+#if OV10823_ADJUST_HS_PREPARE
+        /* Bit[7] is hs_preparei_sel */
+        {0x4800, 0x20},
+        {0x4831, 0x6a},
+#endif
+
+        {0x4810, 0x00},
+        {0x4811, 0x1f},
+        {0x4819, 0xa0},
+        {0x481b, 0x35},
+        {0x481f, 0x30},
+        {0x4823, 0x35},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4d0b, 0x01},
+        {0x5002, 0x08},
+        {0x5a01, 0x08},
+        {0x5a03, 0x06},
+        {0x5a04, 0x10},
+        {0x5a05, 0xf0},
+        {0x5a06, 0x09},
+        {0x5a07, 0x88},
+        {0x5a08, 0x01},
+        {0x5a04, 0x00},
+        {0x5a06, 0x00},
+        {0x5b01, 0x10},
+        {0x5b05, 0xec},
+        {0x5b09, 0x02},
+        {0x5e10, 0x0c},
+
+        {0x3082, 0x6c},
+        {0x3092, 0x04},
+
+        {0x300d, 0x22},
+        {0x301b, 0xb4},
+        {0x3033, 0x5c},
+        {0x3034, 0x91},
+        {0x3035, 0x6e},
+        {0x3036, 0xa4},
+        {0x3037, 0x6e},
+        {0x3038, 0xa4},
+
+        {0x3642, 0x35},
+        {0x3663, 0x01},
+
+        {0x3700, 0x44},
+        {0x3701, 0x1c},
+        {0x3702, 0x90},
+        {0x3703, 0x60},
+        {0x3704, 0x20},
+        {0x3706, 0x18},
+        {0x3707, 0x5d},
+        {0x3708, 0x74},
+        {0x3709, 0x40},
+        {0x370a, 0x31},
+        {0x370b, 0x33},
+        {0x370c, 0x8a},
+        {0x370d, 0x03},
+        {0x370e, 0x28},
+        {0x370f, 0x3b},
+        {0x3710, 0x21},
+        {0x3711, 0x15},
+        {0x3712, 0x19},
+        {0x3714, 0x42},
+        {0x3730, 0x01},
+        {0x3731, 0x6e},
+        {0x3732, 0x01},
+        {0x3733, 0x03},
+        {0x3738, 0x02},
+        {0x3739, 0x19},
+        {0x373a, 0x01},
+        {0x373b, 0xe6},
+        {0x373d, 0xa3},
+        {0x373e, 0x02},
+        {0x373f, 0x70},
+        {0x3740, 0x03},
+        {0x3741, 0x33},
+        {0x3742, 0x02},
+        {0x3743, 0xf8},
+        {0x3748, 0xa5},
+        {0x374c, 0x88},
+
+        {0x3802, 0x00},
+        {0x3803, 0x08},
+        {0x3806, 0x09},
+        {0x3807, 0x98},
+        {0x3808, 0x10},
+        {0x3809, 0xf0},
+        {0x380a, 0x09},
+        {0x380b, 0x88},
+        {0x380c, 0x15},
+        {0x380d, 0x86},
+        {0x380e, 0x09},
+        {0x380f, 0xe0},
+        {0x3810, 0x00},
+        {0x3811, 0x08},
+        {0x3813, 0x04},
+        {0x3815, 0x11},
+        {0x3820, 0x04},
+        {0x3834, 0x00},
+        {0x4001, 0x00},
+        {0x4003, 0x30},
+        {0x402a, 0x0c},
+        {0x402b, 0x08},
+        {0x402e, 0x1c},
+        {0x4837, 0x11},
+        {0x5b04, 0x02},
+
+        /* Exposure/Gain */
+        {0x3500, 0x00},
+        {0x3501, 0x9c},
+        {0x3502, 0x00},
+        {0x350b, 0x40},
+
+        /* fsync */
+        {0x3002, 0x80},
+        {0x3009, 0x06},
+        {0x3823, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x3830, 0x00},
+        {0x3831, 0x00},
+
+        {OV10823_TABLE_END, 0x00}
+};
+
+static ov10823_reg mode_3000x2440_24MhzMCLK[] = {
+
+        /* PLL */
+        {0x3080, 0x04},
+        {0x3083, 0x00},
+        {0x3084, 0x03},
+        {0x308b, 0x05},
+        {0x308d, 0xae},
+        {0x308e, 0x00},
+        {0x308f, 0x09},
+
+        {0x3011, 0xd0},
+        {0x3012, 0x41},
+        {0x3031, 0x0c},
+        {0x3032, 0xc0},
+        {0x3503, 0x07},
+        {0x3603, 0x04},
+        {0x3641, 0x83},
+        {0x3643, 0x02},
+        {0x3645, 0x41},
+        {0x3646, 0x83},
+        {0x3648, 0x0d},
+        {0x3649, 0x86},
+        {0x3653, 0x37},
+        {0x3658, 0x8b},
+        {0x3659, 0x87},
+        {0x365c, 0x5a},
+        {0x3660, 0x82},
+        {0x3705, 0x22},
+        {0x3719, 0x0c},
+        {0x371b, 0x00},
+        {0x371c, 0x80},
+        {0x371e, 0x00},
+        {0x371f, 0x20},
+        {0x3720, 0xa0},
+        {0x3721, 0xf0},
+        {0x3723, 0x78},
+        {0x372a, 0x08},
+        {0x372c, 0x11},
+        {0x373c, 0x02},
+        {0x3749, 0xd7},
+        {0x374a, 0x8c},
+        {0x3752, 0x80},
+        {0x3821, 0x04},
+        {0x382e, 0x04},
+        {0x3901, 0x80},
+        {0x3902, 0xc6},
+        {0x4004, 0x00},
+        {0x4005, 0x20},
+        {0x400d, 0x10},
+        {0x401b, 0x00},
+        {0x401d, 0x00},
+        {0x401f, 0x00},
+        {0x4020, 0x01},
+        {0x4022, 0x03},
+        {0x4024, 0x0e},
+        {0x4025, 0x10},
+        {0x4026, 0x0f},
+        {0x4027, 0x9f},
+        {0x4028, 0x00},
+        {0x4029, 0x04},
+        {0x402d, 0x04},
+        {0x402f, 0x08},
+        {0x4602, 0x02},
+
+#if OV10823_ADJUST_HS_PREPARE
+        /* Bit[7] is hs_preparei_sel */
+        {0x4802, 0x80},
+        {0x4826, 0x10},
+#endif
+
+        {0x4810, 0x00},
+        {0x4811, 0x1f},
+        {0x4819, 0xa0},
+        {0x481b, 0x35},
+        {0x481f, 0x30},
+        {0x4823, 0x35},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4d0b, 0x01},
+        {0x5002, 0x08},
+        {0x5a00, 0x02},
+        {0x5a01, 0xf6},
+        {0x5a03, 0x06},
+        {0x5a04, 0x0b},
+        {0x5a05, 0xb8},
+        {0x5a06, 0x09},
+        {0x5a07, 0x88},
+        {0x5a08, 0x01},
+        {0x5b01, 0x10},
+        {0x5b05, 0xec},
+        {0x5b09, 0x02},
+        {0x5e10, 0x0c},
+
+        {0x3082, 0x6c},
+        {0x3092, 0x04},
+
+        {0x300d, 0x22},
+        {0x301b, 0xb4},
+        {0x3033, 0x5c},
+        {0x3034, 0x91},
+        {0x3035, 0x6e},
+        {0x3036, 0xa4},
+        {0x3037, 0x6e},
+        {0x3038, 0xa4},
+
+        {0x3642, 0x35},
+        {0x3663, 0x01},
+
+        {0x3700, 0x44},
+        {0x3701, 0x1c},
+        {0x3702, 0x90},
+        {0x3703, 0x60},
+        {0x3704, 0x20},
+        {0x3706, 0x18},
+        {0x3707, 0x5d},
+        {0x3708, 0x74},
+        {0x3709, 0x40},
+        {0x370a, 0x31},
+        {0x370b, 0x33},
+        {0x370c, 0x8a},
+        {0x370d, 0x03},
+        {0x370e, 0x28},
+        {0x370f, 0x3b},
+        {0x3710, 0x21},
+        {0x3711, 0x15},
+        {0x3712, 0x19},
+        {0x3714, 0x42},
+        {0x3730, 0x01},
+        {0x3731, 0x6e},
+        {0x3732, 0x01},
+        {0x3733, 0x03},
+        {0x3738, 0x02},
+        {0x3739, 0x19},
+        {0x373a, 0x01},
+        {0x373b, 0xe6},
+        {0x373d, 0xa3},
+        {0x373e, 0x02},
+        {0x373f, 0x70},
+        {0x3740, 0x03},
+        {0x3741, 0x33},
+        {0x3742, 0x02},
+        {0x3743, 0xf8},
+        {0x3748, 0xa5},
+        {0x374c, 0x88},
+
+        {0x3802, 0x00},
+        {0x3803, 0x08},
+        {0x3806, 0x09},
+        {0x3807, 0x98},
+        {0x3808, 0x0b},
+        {0x3809, 0xb8},
+        {0x380a, 0x09},
+        {0x380b, 0x88},
+        {0x380c, 0x15},
+        {0x380d, 0x80},
+        {0x380e, 0x09},
+        {0x380f, 0xe0},
+        {0x3810, 0x02},
+        {0x3811, 0xf6},
+        {0x3813, 0x04},
+        {0x3815, 0x11},
+        {0x3820, 0x04},
+        {0x3821, 0x00},
+        {0x3834, 0x00},
+        {0x4001, 0x00},
+        {0x4003, 0x30},
+        {0x402a, 0x0c},
+        {0x402b, 0x08},
+        {0x402e, 0x1c},
+        {0x4837, 0x12},
+        {0x5b04, 0x02},
+
+        /* Exposure/Gain */
+        {0x3500, 0x00},
+        {0x3501, 0x9c},
+        {0x3502, 0x00},
+        {0x350b, 0x40},
+
+        /* fsync */
+        {0x3002, 0x80},
+        {0x3009, 0x06},
+        {0x3823, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x3830, 0x00},
+        {0x3831, 0x00},
+
+        {OV10823_TABLE_END, 0x00}
+};
+
+static ov10823_reg mode_2168x1220_24MhzMCLK[] = {
+
+        /* PLL */
+        {0x3080, 0x04},
+        {0x3083, 0x00},
+        {0x3084, 0x03},
+        {0x308b, 0x05},
+        {0x308d, 0xae},
+        {0x308e, 0x00},
+        {0x308f, 0x09},
+
+        {0x3011, 0xd0},
+        {0x3012, 0x41},
+        {0x3031, 0x0c},
+        {0x3032, 0xc0},
+        {0x3503, 0x07},
+        {0x3603, 0x04},
+        {0x3641, 0x83},
+        {0x3643, 0x02},
+        {0x3645, 0x41},
+        {0x3646, 0x83},
+        {0x3648, 0x0d},
+        {0x3649, 0x86},
+        {0x3653, 0x37},
+        {0x3658, 0x8b},
+        {0x3659, 0x87},
+        {0x365c, 0x5a},
+        {0x3660, 0x82},
+        {0x3705, 0x22},
+        {0x3719, 0x0c},
+        {0x371b, 0x00},
+        {0x371c, 0x80},
+        {0x371e, 0x00},
+        {0x371f, 0x20},
+        {0x3720, 0xa0},
+        {0x3721, 0xf0},
+        {0x3723, 0x78},
+        {0x372a, 0x08},
+        {0x372c, 0x11},
+        {0x373c, 0x02},
+        {0x3749, 0xd7},
+        {0x374a, 0x8c},
+        {0x3752, 0x80},
+        {0x3821, 0x04},
+        {0x382e, 0x04},
+        {0x3901, 0x80},
+        {0x3902, 0xc6},
+        {0x4004, 0x00},
+        {0x4005, 0x20},
+        {0x400d, 0x10},
+        {0x401b, 0x00},
+        {0x401d, 0x00},
+        {0x401f, 0x00},
+        {0x4020, 0x01},
+        {0x4022, 0x03},
+        {0x4024, 0x0e},
+        {0x4025, 0x10},
+        {0x4026, 0x0f},
+        {0x4027, 0x9f},
+        {0x4028, 0x00},
+        {0x4029, 0x04},
+        {0x402d, 0x04},
+        {0x402f, 0x08},
+        {0x4602, 0x02},
+
+#if OV10823_ADJUST_HS_PREPARE
+        /* Bit[7] is hs_preparei_sel */
+        {0x4802, 0x80},
+        {0x4826, 0x10},
+#endif
+
+        {0x4810, 0x00},
+        {0x4811, 0x1f},
+        {0x4819, 0xa0},
+        {0x481b, 0x35},
+        {0x481f, 0x30},
+        {0x4823, 0x35},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4d0b, 0x01},
+        {0x5002, 0x08},
+        {0x5a04, 0x00},
+        {0x5a06, 0x00},
+        {0x5b01, 0x10},
+        {0x5b05, 0xec},
+        {0x5b09, 0x02},
+        {0x5e10, 0x0c},
+
+        {0x3082, 0x6c},
+        {0x3092, 0x04},
+
+        {0x300d, 0x22},
+        {0x301b, 0xb4},
+        {0x3033, 0x5c},
+        {0x3034, 0x91},
+        {0x3035, 0x6e},
+        {0x3036, 0xa4},
+        {0x3037, 0x6e},
+        {0x3038, 0xa4},
+
+        {0x3642, 0x35},
+        {0x3663, 0x11},
+
+        {0x3700, 0x44},
+        {0x3701, 0x1c},
+        {0x3702, 0x90},
+        {0x3703, 0x60},
+        {0x3704, 0x20},
+        {0x3706, 0x18},
+        {0x3707, 0x5d},
+        {0x3708, 0x74},
+        {0x3709, 0x40},
+        {0x370a, 0x33},
+        {0x370b, 0x33},
+        {0x370c, 0x8a},
+        {0x370d, 0x03},
+        {0x370e, 0x28},
+        {0x370f, 0x3b},
+        {0x3710, 0x21},
+        {0x3711, 0x15},
+        {0x3712, 0x19},
+        {0x3714, 0x42},
+        {0x3730, 0x01},
+        {0x3731, 0x6e},
+        {0x3732, 0x01},
+        {0x3733, 0x03},
+        {0x3738, 0x02},
+        {0x3739, 0x19},
+        {0x373a, 0x01},
+        {0x373b, 0xe6},
+        {0x373d, 0xa3},
+        {0x373e, 0x02},
+        {0x373f, 0x00},
+        {0x3740, 0x03},
+        {0x3741, 0x33},
+        {0x3742, 0x02},
+        {0x3743, 0xf8},
+        {0x3748, 0xa5},
+        {0x374c, 0x88},
+
+        {0x3802, 0x00},
+        {0x3803, 0x08},
+        {0x3806, 0x09},
+        {0x3807, 0x97},
+        {0x3808, 0x08},
+        {0x3809, 0x78},
+        {0x380a, 0x04},
+        {0x380b, 0xc4},
+        {0x380c, 0x15},
+        {0x380d, 0x80},
+        {0x380e, 0x09},
+        {0x380f, 0xe0},
+        {0x3810, 0x00},
+        {0x3811, 0x04},
+        {0x3813, 0x02},
+        {0x3815, 0x31},
+        {0x3820, 0x06},
+        {0x3821, 0x00},
+        {0x3834, 0x01},
+        {0x4001, 0x00},
+        {0x4003, 0x1c},
+        {0x402a, 0x0a},
+        {0x402b, 0x06},
+        {0x402e, 0x14},
+        {0x4837, 0x12},
+        {0x5b04, 0x02},
+
+        /* Exposure/Gain */
+        {0x3500, 0x00},
+        {0x3501, 0x9c},
+        {0x3502, 0x00},
+        {0x350b, 0x40},
+
+        /* fsync */
+        {0x3002, 0x80},
+        {0x3009, 0x06},
+        {0x3823, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x3830, 0x00},
+        {0x3831, 0x00},
+
+        {OV10823_TABLE_END, 0x00}
+};
+
+static ov10823_reg mode_4336x2440_26MhzMCLK[] = {
+
+        /* PLL */
+        {0x3080, 0x04},
+        {0x3083, 0x00},
+        {0x3084, 0x03},
+        {0x308b, 0x06},
+        {0x308d, 0xf1},
+        {0x308e, 0x00},
+        {0x308f, 0x09},
+
+        {0x3011, 0xd0},
+        {0x3012, 0x41},
+        {0x3031, 0x0c},
+        {0x3032, 0xc0},
+        {0x3503, 0x07},
+        {0x3603, 0x04},
+        {0x3641, 0x83},
+        {0x3643, 0x02},
+        {0x3645, 0x41},
+        {0x3646, 0x83},
+        {0x3648, 0x0d},
+        {0x3649, 0x86},
+        {0x3653, 0x37},
+        {0x3658, 0x8b},
+        {0x3659, 0x87},
+        {0x365c, 0x5a},
+        {0x3660, 0x82},
+        {0x3705, 0x22},
+        {0x3719, 0x0c},
+        {0x371b, 0x00},
+        {0x371c, 0x80},
+        {0x371e, 0x00},
+        {0x371f, 0x20},
+        {0x3720, 0xa0},
+        {0x3721, 0xf0},
+        {0x3723, 0x78},
+        {0x372a, 0x08},
+        {0x372c, 0x11},
+        {0x373c, 0x02},
+        {0x3749, 0xd7},
+        {0x374a, 0x8c},
+        {0x3752, 0x80},
+        {0x3821, 0x00},
+        {0x382e, 0x04},
+        {0x3901, 0x80},
+        {0x3902, 0xc6},
+        {0x4004, 0x00},
+        {0x4005, 0x20},
+        {0x400d, 0x10},
+        {0x401b, 0x00},
+        {0x401d, 0x00},
+        {0x401f, 0x00},
+        {0x4020, 0x01},
+        {0x4022, 0x03},
+        {0x4024, 0x0e},
+        {0x4025, 0x10},
+        {0x4026, 0x0f},
+        {0x4027, 0x9f},
+        {0x4028, 0x00},
+        {0x4029, 0x04},
+        {0x402d, 0x04},
+        {0x402f, 0x08},
+        {0x4602, 0x02},
+
+#if OV10823_ADJUST_HS_PREPARE
+        {0x4800, 0x20},
+        {0x4831, 0x6a},
+#endif
+
+        {0x4810, 0x00},
+        {0x4811, 0x1f},
+        {0x4819, 0xa0},
+        {0x481b, 0x35},
+        {0x481f, 0x30},
+        {0x4823, 0x35},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4d0b, 0x01},
+        {0x5002, 0x08},
+        {0x5a01, 0x08},
+        {0x5a03, 0x06},
+        {0x5a04, 0x10},
+        {0x5a05, 0xf0},
+        {0x5a06, 0x09},
+        {0x5a07, 0x88},
+        {0x5a08, 0x01},
+        {0x5b01, 0x10},
+        {0x5b05, 0xec},
+        {0x5b09, 0x02},
+        {0x5e10, 0x0c},
+
+        {0x3082, 0x69},
+        {0x3092, 0x04},
+
+        {0x300d, 0x22},
+        {0x301b, 0xb4},
+        {0x3033, 0x5c},
+        {0x3034, 0x91},
+        {0x3035, 0x6e},
+        {0x3036, 0xa4},
+        {0x3037, 0x6e},
+        {0x3038, 0xa4},
+
+        {0x3642, 0x35},
+        {0x3663, 0x01},
+
+        {0x3700, 0x44},
+        {0x3701, 0x1c},
+        {0x3702, 0x90},
+        {0x3703, 0x60},
+        {0x3704, 0x20},
+        {0x3706, 0x18},
+        {0x3707, 0x5d},
+        {0x3708, 0x74},
+        {0x3709, 0x40},
+        {0x370a, 0x31},
+        {0x370b, 0x33},
+        {0x370c, 0x8a},
+        {0x370d, 0x03},
+        {0x370e, 0x28},
+        {0x370f, 0x3b},
+        {0x3710, 0x21},
+        {0x3711, 0x15},
+        {0x3712, 0x19},
+        {0x3714, 0x42},
+        {0x3730, 0x01},
+        {0x3731, 0x6e},
+        {0x3732, 0x01},
+        {0x3733, 0x03},
+        {0x3738, 0x02},
+        {0x3739, 0x19},
+        {0x373a, 0x01},
+        {0x373b, 0xe6},
+        {0x373d, 0xa3},
+        {0x373e, 0x02},
+        {0x373f, 0x70},
+        {0x3740, 0x03},
+        {0x3741, 0x33},
+        {0x3742, 0x02},
+        {0x3743, 0xf8},
+        {0x3748, 0xa5},
+        {0x374c, 0x88},
+
+        {0x3802, 0x00},
+        {0x3803, 0x08},
+        {0x3806, 0x09},
+        {0x3807, 0x98},
+        {0x3808, 0x10},
+        {0x3809, 0xf0},
+        {0x380a, 0x09},
+        {0x380b, 0x88},
+        {0x380c, 0x15},
+        {0x380d, 0x86},
+        {0x380e, 0x09},
+        {0x380f, 0xe0},
+        {0x3810, 0x00},
+        {0x3811, 0x08},
+        {0x3813, 0x04},
+        {0x3815, 0x11},
+        {0x3820, 0x04},
+        {0x3834, 0x00},
+        {0x4001, 0x00},
+        {0x4003, 0x30},
+        {0x402a, 0x0c},
+        {0x402b, 0x08},
+        {0x402e, 0x1c},
+        {0x4837, 0x11},
+        {0x5b04, 0x02},
+
+        /* Exposure/Gain */
+        {0x3500, 0x00},
+        {0x3501, 0x9c},
+        {0x3502, 0x00},
+        {0x350b, 0x40},
+
+        /* fsync */
+        {0x3002, 0x80},
+        {0x3009, 0x06},
+        {0x3823, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x3830, 0x00},
+        {0x3831, 0x00},
+
+        {OV10823_TABLE_END, 0x00}
+};
+
+static ov10823_reg mode_3000x2440_26MhzMCLK[] = {
+
+        /* PLL */
+        {0x3080, 0x04},
+        {0x3083, 0x00},
+        {0x3084, 0x03},
+        {0x308b, 0x06},
+        {0x308d, 0xf1},
+        {0x308e, 0x00},
+        {0x308f, 0x09},
+
+        {0x3011, 0xd0},
+        {0x3012, 0x41},
+        {0x3031, 0x0c},
+        {0x3032, 0xc0},
+        {0x3503, 0x07},
+        {0x3603, 0x04},
+        {0x3641, 0x83},
+        {0x3643, 0x02},
+        {0x3645, 0x41},
+        {0x3646, 0x83},
+        {0x3648, 0x0d},
+        {0x3649, 0x86},
+        {0x3653, 0x37},
+        {0x3658, 0x8b},
+        {0x3659, 0x87},
+        {0x365c, 0x5a},
+        {0x3660, 0x82},
+        {0x3705, 0x22},
+        {0x3719, 0x0c},
+        {0x371b, 0x00},
+        {0x371c, 0x80},
+        {0x371e, 0x00},
+        {0x371f, 0x20},
+        {0x3720, 0xa0},
+        {0x3721, 0xf0},
+        {0x3723, 0x78},
+        {0x372a, 0x08},
+        {0x372c, 0x11},
+        {0x373c, 0x02},
+        {0x3749, 0xd7},
+        {0x374a, 0x8c},
+        {0x3752, 0x80},
+        {0x3821, 0x04},
+        {0x382e, 0x04},
+        {0x3901, 0x80},
+        {0x3902, 0xc6},
+        {0x4004, 0x00},
+        {0x4005, 0x20},
+        {0x400d, 0x10},
+        {0x401b, 0x00},
+        {0x401d, 0x00},
+        {0x401f, 0x00},
+        {0x4020, 0x01},
+        {0x4022, 0x03},
+        {0x4024, 0x0e},
+        {0x4025, 0x10},
+        {0x4026, 0x0f},
+        {0x4027, 0x9f},
+        {0x4028, 0x00},
+        {0x4029, 0x04},
+        {0x402d, 0x04},
+        {0x402f, 0x08},
+        {0x4602, 0x02},
+
+#if OV10823_ADJUST_HS_PREPARE
+        {0x4800, 0x20},
+        {0x4831, 0x6a},
+#endif
+
+        {0x4810, 0x00},
+        {0x4811, 0x1f},
+        {0x4819, 0xa0},
+        {0x481b, 0x35},
+        {0x481f, 0x30},
+        {0x4823, 0x35},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4d0b, 0x01},
+        {0x5002, 0x08},
+        {0x5a00, 0x02},
+        {0x5a01, 0xf6},
+        {0x5a03, 0x06},
+        {0x5a04, 0x0b},
+        {0x5a05, 0xb8},
+        {0x5a06, 0x09},
+        {0x5a07, 0x88},
+        {0x5a08, 0x01},
+        {0x5b01, 0x10},
+        {0x5b05, 0xec},
+        {0x5b09, 0x02},
+        {0x5e10, 0x0c},
+
+        {0x3082, 0x64},
+        {0x3092, 0x04},
+
+        {0x300d, 0x22},
+        {0x301b, 0xb4},
+        {0x3033, 0x5c},
+        {0x3034, 0x91},
+        {0x3035, 0x6e},
+        {0x3036, 0xa4},
+        {0x3037, 0x6e},
+        {0x3038, 0xa4},
+
+        {0x3642, 0x35},
+        {0x3663, 0x01},
+
+        {0x3700, 0x44},
+        {0x3701, 0x1c},
+        {0x3702, 0x90},
+        {0x3703, 0x60},
+        {0x3704, 0x20},
+        {0x3706, 0x18},
+        {0x3707, 0x5d},
+        {0x3708, 0x74},
+        {0x3709, 0x40},
+        {0x370a, 0x31},
+        {0x370b, 0x33},
+        {0x370c, 0x8a},
+        {0x370d, 0x03},
+        {0x370e, 0x28},
+        {0x370f, 0x3b},
+        {0x3710, 0x21},
+        {0x3711, 0x15},
+        {0x3712, 0x19},
+        {0x3714, 0x42},
+        {0x3730, 0x01},
+        {0x3731, 0x6e},
+        {0x3732, 0x01},
+        {0x3733, 0x03},
+        {0x3738, 0x02},
+        {0x3739, 0x19},
+        {0x373a, 0x01},
+        {0x373b, 0xe6},
+        {0x373d, 0xa3},
+        {0x373e, 0x02},
+        {0x373f, 0x70},
+        {0x3740, 0x03},
+        {0x3741, 0x33},
+        {0x3742, 0x02},
+        {0x3743, 0xf8},
+        {0x3748, 0xa5},
+        {0x374c, 0x88},
+
+        {0x3802, 0x00},
+        {0x3803, 0x08},
+        {0x3806, 0x09},
+        {0x3807, 0x98},
+        {0x3808, 0x0b},
+        {0x3809, 0xb8},
+        {0x380a, 0x09},
+        {0x380b, 0x88},
+        {0x380c, 0x15},
+        {0x380d, 0x80},
+        {0x380e, 0x09},
+        {0x380f, 0xe0},
+        {0x3810, 0x02},
+        {0x3811, 0xf6},
+        {0x3813, 0x04},
+        {0x3815, 0x11},
+        {0x3820, 0x04},
+        {0x3821, 0x00},
+        {0x3834, 0x00},
+        {0x4001, 0x00},
+        {0x4003, 0x30},
+        {0x402a, 0x0c},
+        {0x402b, 0x08},
+        {0x402e, 0x1c},
+        {0x4837, 0x12},
+        {0x5b04, 0x02},
+
+        /* Exposure/Gain */
+        {0x3500, 0x00},
+        {0x3501, 0x9c},
+        {0x3502, 0x00},
+        {0x350b, 0x40},
+
+        /* fsync */
+        {0x3002, 0x80},
+        {0x3009, 0x06},
+        {0x3823, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x3830, 0x00},
+        {0x3831, 0x00},
+
+        {OV10823_TABLE_END, 0x00}
+};
+
+static ov10823_reg mode_2168x1220_26MhzMCLK[] = {
+
+        /* PLL */
+        {0x3080, 0x04},
+        {0x3083, 0x00},
+        {0x3084, 0x03},
+        {0x308b, 0x06},
+        {0x308d, 0xf1},
+        {0x308e, 0x00},
+        {0x308f, 0x09},
+
+        {0x3011, 0xd0},
+        {0x3012, 0x41},
+        {0x3031, 0x0c},
+        {0x3032, 0xc0},
+        {0x3503, 0x07},
+        {0x3603, 0x04},
+        {0x3641, 0x83},
+        {0x3643, 0x02},
+        {0x3645, 0x41},
+        {0x3646, 0x83},
+        {0x3648, 0x0d},
+        {0x3649, 0x86},
+        {0x3653, 0x37},
+        {0x3658, 0x8b},
+        {0x3659, 0x87},
+        {0x365c, 0x5a},
+        {0x3660, 0x82},
+        {0x3705, 0x22},
+        {0x3719, 0x0c},
+        {0x371b, 0x00},
+        {0x371c, 0x80},
+        {0x371e, 0x00},
+        {0x371f, 0x20},
+        {0x3720, 0xa0},
+        {0x3721, 0xf0},
+        {0x3723, 0x78},
+        {0x372a, 0x08},
+        {0x372c, 0x11},
+        {0x373c, 0x02},
+        {0x3749, 0xd7},
+        {0x374a, 0x8c},
+        {0x3752, 0x80},
+        {0x3821, 0x04},
+        {0x382e, 0x04},
+        {0x3901, 0x80},
+        {0x3902, 0xc6},
+        {0x4004, 0x00},
+        {0x4005, 0x20},
+        {0x400d, 0x10},
+        {0x401b, 0x00},
+        {0x401d, 0x00},
+        {0x401f, 0x00},
+        {0x4020, 0x01},
+        {0x4022, 0x03},
+        {0x4024, 0x0e},
+        {0x4025, 0x10},
+        {0x4026, 0x0f},
+        {0x4027, 0x9f},
+        {0x4028, 0x00},
+        {0x4029, 0x04},
+        {0x402d, 0x04},
+        {0x402f, 0x08},
+        {0x4602, 0x02},
+
+#if OV10823_ADJUST_HS_PREPARE
+        /* Bit[7] is hs_preparei_sel */
+        {0x4802, 0x80},
+        {0x4826, 0x10},
+#endif
+
+        {0x4810, 0x00},
+        {0x4811, 0x1f},
+        {0x4819, 0xa0},
+        {0x481b, 0x35},
+        {0x481f, 0x30},
+        {0x4823, 0x35},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4d0b, 0x01},
+        {0x5002, 0x08},
+        {0x5a04, 0x00},
+        {0x5a06, 0x00},
+        {0x5b01, 0x10},
+        {0x5b05, 0xec},
+        {0x5b09, 0x02},
+        {0x5e10, 0x0c},
+
+        {0x3082, 0x64},
+        {0x3092, 0x04},
+
+        {0x300d, 0x22},
+        {0x301b, 0xb4},
+        {0x3033, 0x5c},
+        {0x3034, 0x91},
+        {0x3035, 0x6e},
+        {0x3036, 0xa4},
+        {0x3037, 0x6e},
+        {0x3038, 0xa4},
+
+        {0x3642, 0x35},
+        {0x3663, 0x11},
+
+        {0x3700, 0x44},
+        {0x3701, 0x1c},
+        {0x3702, 0x90},
+        {0x3703, 0x60},
+        {0x3704, 0x20},
+        {0x3706, 0x18},
+        {0x3707, 0x5d},
+        {0x3708, 0x74},
+        {0x3709, 0x40},
+        {0x370a, 0x33},
+        {0x370b, 0x33},
+        {0x370c, 0x8a},
+        {0x370d, 0x03},
+        {0x370e, 0x28},
+        {0x370f, 0x3b},
+        {0x3710, 0x21},
+        {0x3711, 0x15},
+        {0x3712, 0x19},
+        {0x3714, 0x42},
+        {0x3730, 0x01},
+        {0x3731, 0x6e},
+        {0x3732, 0x01},
+        {0x3733, 0x03},
+        {0x3738, 0x02},
+        {0x3739, 0x19},
+        {0x373a, 0x01},
+        {0x373b, 0xe6},
+        {0x373d, 0xa3},
+        {0x373e, 0x02},
+        {0x373f, 0x00},
+        {0x3740, 0x03},
+        {0x3741, 0x33},
+        {0x3742, 0x02},
+        {0x3743, 0xf8},
+        {0x3748, 0xa5},
+        {0x374c, 0x88},
+
+        {0x3802, 0x00},
+        {0x3803, 0x08},
+        {0x3806, 0x09},
+        {0x3807, 0x97},
+        {0x3808, 0x08},
+        {0x3809, 0x78},
+        {0x380a, 0x04},
+        {0x380b, 0xc4},
+        {0x380c, 0x15},
+        {0x380d, 0x80},
+        {0x380e, 0x09},
+        {0x380f, 0xe0},
+        {0x3810, 0x00},
+        {0x3811, 0x04},
+        {0x3813, 0x02},
+        {0x3815, 0x31},
+        {0x3820, 0x06},
+        {0x3821, 0x00},
+        {0x3834, 0x01},
+        {0x4001, 0x00},
+        {0x4003, 0x1c},
+        {0x402a, 0x0a},
+        {0x402b, 0x06},
+        {0x402e, 0x14},
+        {0x4837, 0x12},
+        {0x5b04, 0x02},
+
+        /* Exposure/Gain */
+        {0x3500, 0x00},
+        {0x3501, 0x9c},
+        {0x3502, 0x00},
+        {0x350b, 0x40},
+
+        /* fsync */
+        {0x3002, 0x80},
+        {0x3009, 0x06},
+        {0x3823, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x3830, 0x00},
+        {0x3831, 0x00},
+
+        {OV10823_TABLE_END, 0x00}
+};
+
+static ov10823_reg mode_4336x1220_60fps_26MhzMCLK[] = {
+
+        /* PLL */
+        {0x3080, 0x04},
+        {0x3083, 0x00},
+        {0x3084, 0x03},
+        {0x308b, 0x06},
+        {0x308d, 0xf1},
+        {0x308e, 0x00},
+        {0x308f, 0x09},
+
+        {0x3011, 0xd0},
+        {0x3012, 0x41},
+        {0x3031, 0x0c},
+        {0x3032, 0xc0},
+        {0x3503, 0x07},
+        {0x3603, 0x04},
+        {0x3641, 0x83},
+        {0x3643, 0x02},
+        {0x3645, 0x41},
+        {0x3646, 0x83},
+        {0x3648, 0x0d},
+        {0x3649, 0x86},
+        {0x3653, 0x37},
+        {0x3658, 0x8b},
+        {0x3659, 0x87},
+        {0x365c, 0x5a},
+        {0x3660, 0x82},
+        {0x3705, 0x22},
+        {0x3719, 0x0c},
+        {0x371b, 0x00},
+        {0x371c, 0x80},
+        {0x371e, 0x00},
+        {0x371f, 0x20},
+        {0x3720, 0xf0},
+        {0x3721, 0xf0},
+        {0x3723, 0x78},
+        {0x372a, 0x08},
+        {0x372c, 0x11},
+        {0x373c, 0x02},
+        {0x3749, 0xd7},
+        {0x374a, 0x8c},
+        {0x3752, 0x80},
+        {0x3821, 0x00},
+        {0x382e, 0x04},
+        {0x3901, 0x00},
+        {0x3902, 0xc6},
+        {0x4004, 0x00},
+        {0x4005, 0x20},
+        {0x400d, 0x10},
+        {0x401b, 0x00},
+        {0x401d, 0x00},
+        {0x401f, 0x00},
+        {0x4020, 0x01},
+        {0x4022, 0x03},
+        {0x4024, 0x0e},
+        {0x4025, 0x10},
+        {0x4026, 0x0f},
+        {0x4027, 0x9f},
+        {0x4028, 0x00},
+        {0x4029, 0x04},
+        {0x402d, 0x04},
+        {0x402f, 0x08},
+        {0x4602, 0x02},
+
+#if OV10823_ADJUST_HS_PREPARE
+        {0x4802, 0x80},
+        {0x4826, 0x10},
+#endif
+
+        {0x4810, 0x00},
+        {0x4811, 0x1f},
+        {0x4819, 0xa0},
+        {0x481b, 0x35},
+        {0x481f, 0x30},
+        {0x4823, 0x35},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4d0b, 0x01},
+        {0x5001, 0x03},
+        {0x5002, 0x08},
+        {0x5a04, 0x00},
+        {0x5a06, 0x00},
+        {0x5b01, 0x10},
+        {0x5b05, 0xec},
+        {0x5b09, 0x02},
+        {0x5e10, 0x0c},
+
+        {0x3082, 0x64},
+        {0x3092, 0x04},
+
+        {0x300d, 0x22},
+        {0x301b, 0xb4},
+        {0x3033, 0x5c},
+        {0x3034, 0x91},
+        {0x3035, 0x6e},
+        {0x3036, 0xa4},
+        {0x3037, 0x6e},
+        {0x3038, 0xa4},
+
+        {0x3642, 0x35},
+        {0x3663, 0x01},
+
+        {0x3700, 0x44},
+        {0x3701, 0x1c},
+        {0x3702, 0x90},
+        {0x3703, 0x60},
+        {0x3704, 0x20},
+        {0x3706, 0x18},
+        {0x3707, 0x5d},
+        {0x3708, 0x74},
+        {0x3709, 0x40},
+        {0x370a, 0x33},
+        {0x370b, 0x33},
+        {0x370c, 0x8a},
+        {0x370d, 0x03},
+        {0x370e, 0x28},
+        {0x370f, 0x3b},
+        {0x3710, 0x21},
+        {0x3711, 0x15},
+        {0x3712, 0x19},
+        {0x3714, 0x42},
+        {0x3730, 0x01},
+        {0x3731, 0x6e},
+        {0x3732, 0x01},
+        {0x3733, 0x03},
+        {0x3738, 0x02},
+        {0x3739, 0x19},
+        {0x373a, 0x01},
+        {0x373b, 0xe6},
+        {0x373d, 0xa3},
+        {0x373e, 0x02},
+        {0x373f, 0x70},
+        {0x3740, 0x03},
+        {0x3741, 0x33},
+        {0x3742, 0x02},
+        {0x3743, 0xf8},
+        {0x3748, 0xa5},
+        {0x374c, 0x88},
+
+        {0x3802, 0x00},
+        {0x3803, 0x08},
+        {0x3806, 0x09},
+        {0x3807, 0x97},
+        {0x3808, 0x10},
+        {0x3809, 0xf0},
+        {0x380a, 0x04},
+        {0x380b, 0xc4},
+        {0x380c, 0x15},
+        {0x380d, 0x80},
+        {0x380e, 0x04},
+        {0x380f, 0xf0},
+        {0x3810, 0x00},
+        {0x3811, 0x08},
+        {0x3813, 0x02},
+        {0x3815, 0x31},
+        {0x3820, 0x06},
+        {0x3821, 0x00},
+        {0x3834, 0x00},
+        {0x4001, 0x00},
+        {0x4003, 0x1c},
+        {0x402a, 0x0a},
+        {0x402b, 0x06},
+        {0x402e, 0x14},
+        {0x4837, 0x12},
+        {0x5b04, 0x02},
+
+        /* Exposure/Gain */
+        {0x3500, 0x00},
+        {0x3501, 0x4d},
+        {0x3502, 0x00},
+        {0x350b, 0x40},
+
+        /* fsync */
+        {0x3002, 0x80},
+        {0x3009, 0x06},
+        {0x3823, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x3830, 0x00},
+        {0x3831, 0x00},
+
+        {OV10823_TABLE_END, 0x00}
+};
+#endif /* OV10823_UNUSED_MODES */
+
+static ov10823_reg mode_2168x1220_60fps_26MhzMCLK[] = {
+
+        /* PLL */
+        {0x3080, 0x04},
+        {0x3083, 0x00},
+        {0x3084, 0x03},
+        {0x308b, 0x06},
+        {0x308d, 0xf1},
+        {0x308e, 0x00},
+        {0x308f, 0x09},
+
+        {0x3011, 0xd0},
+        {0x3012, 0x41},
+        {0x3031, 0x0c},
+        {0x3032, 0xc0},
+        {0x3503, 0x07},
+        {0x3603, 0x04},
+        {0x3641, 0x83},
+        {0x3643, 0x02},
+        {0x3645, 0x41},
+        {0x3646, 0x83},
+        {0x3648, 0x0d},
+        {0x3649, 0x86},
+        {0x3653, 0x37},
+        {0x3658, 0x8b},
+        {0x3659, 0x87},
+        {0x365c, 0x5a},
+        {0x3660, 0x82},
+        {0x3705, 0x22},
+        {0x3719, 0x0c},
+        {0x371b, 0x00},
+        {0x371c, 0x80},
+        {0x371e, 0x00},
+        {0x371f, 0x20},
+        {0x3720, 0xf0},
+        {0x3721, 0xf0},
+        {0x3723, 0x78},
+        {0x372a, 0x08},
+        {0x372c, 0x11},
+        {0x373c, 0x02},
+        {0x3749, 0xd7},
+        {0x374a, 0x8c},
+        {0x3752, 0x80},
+        {0x3821, 0x04},
+        {0x382e, 0x04},
+        {0x3901, 0x00},
+        {0x3902, 0xc6},
+        {0x4004, 0x00},
+        {0x4005, 0x20},
+        {0x400d, 0x10},
+        {0x401b, 0x00},
+        {0x401d, 0x00},
+        {0x401f, 0x00},
+        {0x4020, 0x01},
+        {0x4022, 0x03},
+        {0x4024, 0x0e},
+        {0x4025, 0x10},
+        {0x4026, 0x0f},
+        {0x4027, 0x9f},
+        {0x4028, 0x00},
+        {0x4029, 0x04},
+        {0x402d, 0x04},
+        {0x402f, 0x08},
+        {0x4602, 0x02},
+
+#if OV10823_ADJUST_HS_PREPARE
+        /* Bit[7] is hs_preparei_sel */
+        {0x4802, 0x80},
+        {0x4826, 0x10},
+#endif
+
+        {0x4810, 0x00},
+        {0x4811, 0x1f},
+        {0x4819, 0xa0},
+        {0x481b, 0x35},
+        {0x481f, 0x30},
+        {0x4823, 0x35},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4d0b, 0x01},
+        {0x5001, 0x03},
+        {0x5002, 0x08},
+        {0x5a04, 0x00},
+        {0x5a06, 0x00},
+        {0x5b01, 0x10},
+        {0x5b05, 0xec},
+        {0x5b09, 0x02},
+        {0x5e10, 0x0c},
+
+        {0x3082, 0x64},
+        {0x3092, 0x04},
+
+        {0x300d, 0x22},
+        {0x301b, 0xb4},
+        {0x3033, 0x5c},
+        {0x3034, 0x91},
+        {0x3035, 0x6e},
+        {0x3036, 0xa4},
+        {0x3037, 0x6e},
+        {0x3038, 0xa4},
+
+        {0x3642, 0x35},
+        {0x3663, 0x11},
+
+        {0x3700, 0x44},
+        {0x3701, 0x1c},
+        {0x3702, 0x90},
+        {0x3703, 0x60},
+        {0x3704, 0x20},
+        {0x3706, 0x18},
+        {0x3707, 0x5d},
+        {0x3708, 0x74},
+        {0x3709, 0x40},
+        {0x370a, 0x33},
+        {0x370b, 0x33},
+        {0x370c, 0x8a},
+        {0x370d, 0x03},
+        {0x370e, 0x28},
+        {0x370f, 0x3b},
+        {0x3710, 0x21},
+        {0x3711, 0x15},
+        {0x3712, 0x19},
+        {0x3714, 0x42},
+        {0x3730, 0x01},
+        {0x3731, 0x6e},
+        {0x3732, 0x01},
+        {0x3733, 0x03},
+        {0x3738, 0x02},
+        {0x3739, 0x19},
+        {0x373a, 0x01},
+        {0x373b, 0xe6},
+        {0x373d, 0xa3},
+        {0x373e, 0x02},
+        {0x373f, 0x70},
+        {0x3740, 0x03},
+        {0x3741, 0x33},
+        {0x3742, 0x02},
+        {0x3743, 0xf8},
+        {0x3748, 0xa5},
+        {0x374c, 0x88},
+
+        {0x3802, 0x00},
+        {0x3803, 0x08},
+        {0x3806, 0x09},
+        {0x3807, 0x97},
+        {0x3808, 0x08},
+        {0x3809, 0x78},
+        {0x380a, 0x04},
+        {0x380b, 0xc4},
+        {0x380c, 0x15},
+        {0x380d, 0x80},
+        {0x380e, 0x04},
+        {0x380f, 0xf0},
+        {0x3810, 0x00},
+        {0x3811, 0x04},
+        {0x3813, 0x02},
+        {0x3815, 0x31},
+        {0x3820, 0x06},
+        {0x3821, 0x00},
+        {0x3834, 0x01},
+        {0x4001, 0x00},
+        {0x4003, 0x1c},
+        {0x402a, 0x0a},
+        {0x402b, 0x06},
+        {0x402e, 0x14},
+        {0x4837, 0x12},
+        {0x5b04, 0x02},
+
+        /* Exposure/Gain */
+        {0x3500, 0x00},
+        {0x3501, 0x4d},
+        {0x3502, 0x00},
+        {0x350b, 0x40},
+
+        /* fsync */
+        {0x3002, 0x80},
+        {0x3009, 0x06},
+        {0x3823, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x3830, 0x00},
+        {0x3831, 0x00},
+
+        {OV10823_TABLE_END, 0x00}
+};
+
+enum {
+#ifdef OV10823_UNUSED_MODES
+        OV10823_MODE_4336X2440,
+        OV10823_MODE_3000X2440,
+        OV10823_MODE_2168X1220,
+#endif
+        OV10823_MODE_2168X1220_60FPS,
+        OV10823_MODE_START_STREAM,
+        OV10823_MODE_STOP_STREAM,
+};
+
+static ov10823_reg *mode_table[] = {
+#ifdef OV10823_UNUSED_MODES
+        [OV10823_MODE_4336X2440] = mode_4336x2440_26MhzMCLK,
+        [OV10823_MODE_3000X2440] = mode_3000x2440_26MhzMCLK,
+        [OV10823_MODE_2168X1220] = mode_2168x1220_26MhzMCLK,
+#endif
+        [OV10823_MODE_2168X1220_60FPS] = mode_2168x1220_60fps_26MhzMCLK,
+
+        [OV10823_MODE_START_STREAM] = ov10823_start,
+        [OV10823_MODE_STOP_STREAM] = ov10823_stop,
+};
+
+enum {
+        OV10823_FSYNC_NONE,
+        OV10823_FSYNC_MASTER,
+        OV10823_FSYNC_SLAVE,
+};
+
+static ov10823_reg *fsync_table[] = {
+        [OV10823_FSYNC_NONE] = NULL,
+        [OV10823_FSYNC_MASTER] = fsync_master,
+        [OV10823_FSYNC_SLAVE] = fsync_slave,
+};
+
+static const int ov10823_30fps[] = {
+        30,
+};
+
+static const int ov10823_60fps[] = {
+        60,
+};
+
+static const struct camera_common_frmfmt ov10823_frmfmt[] = {
+#ifdef OV10823_UNUSED_MODES
+        {{4336, 2440}, ov10823_30fps, 1, 0, OV10823_MODE_4336X2440},
+        {{3000, 2440}, ov10823_30fps, 1, 0, OV10823_MODE_3000X2440},
+        {{2168, 1220}, ov10823_30fps, 1, 0, OV10823_MODE_2168X1220},
+#endif
+        {{2168, 1220}, ov10823_60fps, 1, 0, OV10823_MODE_2168X1220_60FPS},
+};
+#endif  /* __OV10823_I2C_TABLES__ */
diff --git a/drivers/media/i2c/ov23850.c b/drivers/media/i2c/ov23850.c
new file mode 100644
index 000000000..710314fba
--- /dev/null
+++ b/drivers/media/i2c/ov23850.c
@@ -0,0 +1,1443 @@
+/*
+ * ov23850.c - ov23850 sensor driver
+ *
+ * Copyright (c) 2013-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+
+#include <linux/seq_file.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <media/tegra_v4l2_camera.h>
+#include <media/camera_common.h>
+#include <media/ov23850.h>
+
+#include "ov23850_mode_tbls.h"
+
+#define OV23850_MAX_COARSE_DIFF         0x20
+
+#define OV23850_GAIN_SHIFT              8
+#define OV23850_MIN_GAIN                (1 << OV23850_GAIN_SHIFT)
+#define OV23850_MAX_GAIN                (16 << OV23850_GAIN_SHIFT)
+#define OV23850_MIN_FRAME_LENGTH        (0x0)
+#define OV23850_MAX_FRAME_LENGTH        (0x7fff)
+#define OV23850_MIN_EXPOSURE_COARSE     (0x0002)
+#define OV23850_MAX_EXPOSURE_COARSE     \
+        (OV23850_MAX_FRAME_LENGTH-OV23850_MAX_COARSE_DIFF)
+
+#define OV23850_DEFAULT_GAIN            OV23850_MIN_GAIN
+#define OV23850_DEFAULT_FRAME_LENGTH    (0x12C6)
+#define OV23850_DEFAULT_EXPOSURE_COARSE \
+        (OV23850_DEFAULT_FRAME_LENGTH-OV23850_MAX_COARSE_DIFF)
+
+#define OV23850_DEFAULT_MODE    OV23850_MODE_5632X4224
+#define OV23850_DEFAULT_WIDTH   5632
+#define OV23850_DEFAULT_HEIGHT  4224
+#define OV23850_DEFAULT_DATAFMT MEDIA_BUS_FMT_SRGGB10_1X10
+#define OV23850_DEFAULT_CLK_FREQ        24000000
+
+struct ov23850 {
+        struct mutex                    ov23850_camera_lock;
+        struct camera_common_power_rail power;
+        int                             numctrls;
+        struct v4l2_ctrl_handler        ctrl_handler;
+#if 0
+        struct camera_common_eeprom_data eeprom[OV23850_EEPROM_NUM_BLOCKS];
+        u8                              eeprom_buf[OV23850_EEPROM_SIZE];
+#endif
+        struct i2c_client               *i2c_client;
+        struct v4l2_subdev              *subdev;
+        struct media_pad                pad;
+
+        u32                             frame_length;
+        s32                             group_hold_prev;
+        bool                            group_hold_en;
+        struct regmap                   *regmap;
+        struct camera_common_data       *s_data;
+        struct camera_common_pdata      *pdata;
+        struct v4l2_ctrl                *ctrls[];
+};
+
+static const struct regmap_config sensor_regmap_config = {
+        .reg_bits = 16,
+        .val_bits = 8,
+        .cache_type = REGCACHE_RBTREE,
+};
+
+u16 ov23850_to_gain(u32 rep, int shift)
+{
+        u16 gain;
+        int gain_int;
+        int gain_dec;
+        int min_int = (1 << shift);
+        int step = 1;
+        int num_step;
+        int i;
+
+        if (rep < 0x0100)
+                rep = 0x0100;
+        else if (rep > 0x0F80)
+                rep = 0x0F80;
+
+        /* last 4 bit of rep is
+         * decimal representation of gain */
+        gain_int = (int)(rep >> shift);
+        gain_dec = (int)(rep & ~(0xffff << shift));
+
+        for (i = 1; gain_int >> i != 0; i++)
+                ;
+        step = step << (5 - i);
+
+        num_step = gain_dec * step / min_int;
+        gain = 16 * gain_int + 16 * num_step / step;
+
+        return gain;
+}
+
+static int ov23850_g_volatile_ctrl(struct v4l2_ctrl *ctrl);
+static int ov23850_s_ctrl(struct v4l2_ctrl *ctrl);
+
+static const struct v4l2_ctrl_ops ov23850_ctrl_ops = {
+        .g_volatile_ctrl = ov23850_g_volatile_ctrl,
+        .s_ctrl         = ov23850_s_ctrl,
+};
+
+static struct v4l2_ctrl_config ctrl_config_list[] = {
+/* Do not change the name field for the controls! */
+        {
+                .ops = &ov23850_ctrl_ops,
+                .id = V4L2_CID_GAIN,
+                .name = "Gain",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV23850_MIN_GAIN,
+                .max = OV23850_MAX_GAIN,
+                .def = OV23850_DEFAULT_GAIN,
+                .step = 1,
+        },
+        {
+                .ops = &ov23850_ctrl_ops,
+                .id = V4L2_CID_FRAME_LENGTH,
+                .name = "Frame Length",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV23850_MIN_FRAME_LENGTH,
+                .max = OV23850_MAX_FRAME_LENGTH,
+                .def = OV23850_DEFAULT_FRAME_LENGTH,
+                .step = 1,
+        },
+        {
+                .ops = &ov23850_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME,
+                .name = "Coarse Time",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV23850_MIN_EXPOSURE_COARSE,
+                .max = OV23850_MAX_EXPOSURE_COARSE,
+                .def = OV23850_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &ov23850_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME_SHORT,
+                .name = "Coarse Time Short",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV23850_MIN_EXPOSURE_COARSE,
+                .max = OV23850_MAX_EXPOSURE_COARSE,
+                .def = OV23850_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &ov23850_ctrl_ops,
+                .id = V4L2_CID_GROUP_HOLD,
+                .name = "Group Hold",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &ov23850_ctrl_ops,
+                .id = V4L2_CID_HDR_EN,
+                .name = "HDR enable",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = 0,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &ov23850_ctrl_ops,
+                .id = V4L2_CID_EEPROM_DATA,
+                .name = "EEPROM Data",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_VOLATILE,
+                .min = 0,
+                .max = OV23850_EEPROM_STR_SIZE,
+                .step = 2,
+        },
+        {
+                .ops = &ov23850_ctrl_ops,
+                .id = V4L2_CID_OTP_DATA,
+                .name = "OTP Data",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = OV23850_OTP_STR_SIZE,
+                .step = 2,
+        },
+        {
+                .ops = &ov23850_ctrl_ops,
+                .id = V4L2_CID_FUSE_ID,
+                .name = "Fuse ID",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = OV23850_FUSE_ID_STR_SIZE,
+                .step = 2,
+        },
+};
+
+static inline void ov23850_get_frame_length_regs(ov23850_reg *regs,
+                                u32 frame_length)
+{
+        regs->addr = OV23850_FRAME_LENGTH_ADDR_MSB;
+        regs->val = (frame_length >> 8) & 0x7f;
+        (regs + 1)->addr = OV23850_FRAME_LENGTH_ADDR_LSB;
+        (regs + 1)->val = (frame_length) & 0xff;
+}
+
+static inline void ov23850_get_coarse_time_regs(ov23850_reg *regs,
+                                u32 coarse_time)
+{
+        regs->addr = OV23850_COARSE_TIME_ADDR_MSB;
+        regs->val = (coarse_time >> 8) & 0x7f;
+        (regs + 1)->addr = OV23850_COARSE_TIME_ADDR_LSB;
+        (regs + 1)->val = (coarse_time) & 0xff;
+}
+
+static inline void ov23850_get_coarse_time_short_regs(ov23850_reg *regs,
+                                u32 coarse_time)
+{
+        regs->addr = OV23850_COARSE_TIME_SHORT_ADDR_MSB;
+        regs->val = (coarse_time >> 8) & 0xff;
+        (regs + 1)->addr = OV23850_COARSE_TIME_SHORT_ADDR_LSB;
+        (regs + 1)->val = (coarse_time) & 0xff;
+}
+
+static inline void ov23850_get_gain_reg(ov23850_reg *regs,
+                                u16 gain)
+{
+        regs->addr = OV23850_GAIN_ADDR_MSB;
+        regs->val = (gain >> 8) & 0x07;
+        (regs + 1)->addr = OV23850_GAIN_ADDR_LSB;
+        (regs + 1)->val = (gain) & 0xff;
+}
+
+static inline void ov23850_get_gain_short_reg(ov23850_reg *regs,
+                                u16 gain)
+{
+        regs->addr = OV23850_GAIN_SHORT_ADDR_MSB;
+        regs->val = (gain >> 8) & 0x07;
+        (regs + 1)->addr = OV23850_GAIN_SHORT_ADDR_LSB;
+        (regs + 1)->val = (gain) & 0xff;
+}
+
+static int test_mode;
+module_param(test_mode, int, 0644);
+
+static inline int ov23850_read_reg(struct camera_common_data *s_data,
+                                u16 addr, u8 *val)
+{
+        struct ov23850 *priv = (struct ov23850 *)s_data->priv;
+
+        return regmap_read(priv->regmap, addr, (unsigned int *) val);
+}
+
+static int ov23850_write_reg(struct camera_common_data *s_data,
+                             u16 addr, u8 val)
+{
+        int err;
+        struct ov23850 *priv = (struct ov23850 *)s_data->priv;
+
+        err = regmap_write(priv->regmap, addr, val);
+        if (err)
+                pr_err("%s:i2c write failed, %x = %x\n",
+                        __func__, addr, val);
+
+        return err;
+}
+
+static int ov23850_write_table(struct ov23850 *priv,
+                                const ov23850_reg table[])
+{
+        return regmap_util_write_table_8(priv->regmap,
+                                         table,
+                                         NULL, 0,
+                                         OV23850_TABLE_WAIT_MS,
+                                         OV23850_TABLE_END);
+}
+
+static int ov23850_power_on(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct ov23850 *priv = (struct ov23850 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power on\n", __func__);
+
+        if (priv->pdata->power_on) {
+                err = priv->pdata->power_on(pw);
+                if (err)
+                        pr_err("%s failed.\n", __func__);
+                else
+                        pw->state = SWITCH_ON;
+                return err;
+        }
+
+        if (pw->reset_gpio)
+                gpio_set_value(pw->reset_gpio, 0);
+        if (pw->pwdn_gpio)
+                gpio_set_value(pw->pwdn_gpio, 0);
+        usleep_range(10, 20);
+
+        if (pw->avdd)
+                err = regulator_enable(pw->avdd);
+        if (err)
+                goto ov23850_avdd_fail;
+
+        if (pw->dvdd)
+                err = regulator_enable(pw->dvdd);
+        if (err)
+                goto ov23850_dvdd_fail;
+
+        if (pw->iovdd)
+                err = regulator_enable(pw->iovdd);
+        if (err)
+                goto ov23850_iovdd_fail;
+
+        if (pw->vcmvdd)
+                err = regulator_enable(pw->vcmvdd);
+        if (err)
+                goto ov23850_vcmvdd_fail;
+
+        if (pw->pwdn_gpio)
+                gpio_set_value(pw->pwdn_gpio, 1);
+        if (pw->reset_gpio)
+                gpio_set_value(pw->reset_gpio, 1);
+
+        usleep_range(5350, 5360);       /* 5ms + 8192 EXTCLK cycles */
+
+        pw->state = SWITCH_ON;
+        return 0;
+
+ov23850_vcmvdd_fail:
+        regulator_disable(pw->iovdd);
+
+ov23850_iovdd_fail:
+        regulator_disable(pw->dvdd);
+
+ov23850_dvdd_fail:
+        regulator_disable(pw->avdd);
+
+ov23850_avdd_fail:
+        pr_err("%s failed.\n", __func__);
+        return -ENODEV;
+}
+
+static int ov23850_power_off(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct ov23850 *priv = (struct ov23850 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power off\n", __func__);
+
+        if (priv->pdata->power_off) {
+                err = priv->pdata->power_off(pw);
+                if (!err)
+                        goto power_off_done;
+                else
+                        pr_err("%s failed.\n", __func__);
+                return err;
+        }
+
+        usleep_range(1, 2);
+        if (pw->reset_gpio)
+                gpio_set_value(pw->reset_gpio, 0);
+        if (pw->pwdn_gpio)
+                gpio_set_value(pw->pwdn_gpio, 0);
+        usleep_range(1, 2);
+
+        if (pw->vcmvdd)
+                regulator_disable(pw->vcmvdd);
+        if (pw->iovdd)
+                regulator_disable(pw->iovdd);
+        if (pw->dvdd)
+                regulator_disable(pw->dvdd);
+        if (pw->avdd)
+                regulator_disable(pw->avdd);
+
+power_off_done:
+        pw->state = SWITCH_OFF;
+        return 0;
+}
+
+static int ov23850_power_put(struct ov23850 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+
+        if (unlikely(!pw))
+                return -EFAULT;
+
+        if (likely(pw->avdd))
+                regulator_put(pw->avdd);
+
+        if (likely(pw->iovdd))
+                regulator_put(pw->iovdd);
+
+        if (likely(pw->dvdd))
+                regulator_put(pw->dvdd);
+
+        pw->avdd = NULL;
+        pw->iovdd = NULL;
+        pw->dvdd = NULL;
+
+        return 0;
+}
+
+static int ov23850_power_get(struct ov23850 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        struct camera_common_pdata *pdata = priv->pdata;
+        const char *mclk_name;
+        struct clk *parent;
+        int err = 0;
+
+        mclk_name = priv->pdata->mclk_name ?
+                    priv->pdata->mclk_name : "cam_mclk1";
+        pw->mclk = devm_clk_get(&priv->i2c_client->dev, mclk_name);
+        if (IS_ERR(pw->mclk)) {
+                dev_err(&priv->i2c_client->dev,
+                        "unable to get clock %s\n", mclk_name);
+                return PTR_ERR(pw->mclk);
+        }
+
+        parent = devm_clk_get(&priv->i2c_client->dev, "pllp_grtba");
+        if (IS_ERR(parent))
+                dev_err(&priv->i2c_client->dev, "devm_clk_get failed for pllp_grtba");
+        else
+                clk_set_parent(pw->mclk, parent);
+
+        /* ananlog 2.7v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->avdd, pdata->regulators.avdd);
+        /* digital 1.2v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->dvdd, pdata->regulators.dvdd);
+        /* IO 1.8v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->iovdd, pdata->regulators.iovdd);
+
+        if (!err) {
+                pw->reset_gpio = pdata->reset_gpio;
+                pw->pwdn_gpio = pdata->pwdn_gpio;
+        }
+
+        pw->state = SWITCH_OFF;
+        return err;
+}
+
+static int ov23850_set_gain(struct ov23850 *priv, s32 val);
+static int ov23850_set_frame_length(struct ov23850 *priv, s32 val);
+static int ov23850_set_coarse_time(struct ov23850 *priv, s32 val);
+static int ov23850_set_coarse_time_short(struct ov23850 *priv, s32 val);
+
+static int ov23850_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov23850 *priv = (struct ov23850 *)s_data->priv;
+        struct v4l2_control control;
+        int err;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        if (!enable) {
+                err = ov23850_write_table(priv,
+                        mode_table[OV23850_MODE_STOP_STREAM]);
+                if (err)
+                        return err;
+
+                /* Wait for one frame to make sure sensor is set to
+                 * software standby in V-blank
+                 *
+                 * delay = frame length rows * Tline (10 us)
+                 */
+                usleep_range(priv->frame_length * 10,
+                        priv->frame_length * 10 + 1000);
+                return 0;
+        }
+
+        err = ov23850_write_table(priv, mode_table[OV23850_MODE_COMMON]);
+        if (err)
+                goto exit;
+        err = ov23850_write_table(priv, mode_table[s_data->mode]);
+        if (err)
+                goto exit;
+
+        if (s_data->override_enable) {
+                /* write list of override regs for the asking frame length, */
+                /* coarse integration time, and gain.                       */
+                control.id = V4L2_CID_GAIN;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov23850_set_gain(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev, "%s: error gain override\n",
+                                __func__);
+
+                control.id = V4L2_CID_FRAME_LENGTH;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov23850_set_frame_length(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: error frame length override\n", __func__);
+
+                control.id = V4L2_CID_COARSE_TIME;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov23850_set_coarse_time(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: error coarse time override\n", __func__);
+
+                control.id = V4L2_CID_COARSE_TIME_SHORT;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov23850_set_coarse_time_short(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: error coarse time short override\n",
+                                __func__);
+        }
+
+        err = ov23850_write_table(priv, mode_table[OV23850_MODE_START_STREAM]);
+        if (err)
+                goto exit;
+
+        if (test_mode)
+                err = ov23850_write_table(priv,
+                        mode_table[OV23850_MODE_TEST_PATTERN]);
+
+        return 0;
+exit:
+        dev_dbg(&client->dev, "%s: error setting stream\n", __func__);
+        return err;
+}
+
+static int ov23850_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov23850 *priv = (struct ov23850 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        *status = pw->state == SWITCH_ON;
+        return 0;
+}
+
+static struct v4l2_subdev_video_ops ov23850_subdev_video_ops = {
+        .s_stream       = ov23850_s_stream,
+        .g_mbus_config  = camera_common_g_mbus_config,
+        .g_input_status = ov23850_g_input_status,
+};
+
+static struct v4l2_subdev_core_ops ov23850_subdev_core_ops = {
+        .s_power        = camera_common_s_power,
+};
+
+static int ov23850_get_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        return camera_common_g_fmt(sd, &format->format);
+}
+
+static int ov23850_set_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+        struct v4l2_subdev_format *format)
+{
+        int ret;
+
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+                ret = camera_common_try_fmt(sd, &format->format);
+        else
+                ret = camera_common_s_fmt(sd, &format->format);
+
+        return ret;
+}
+
+static struct v4l2_subdev_pad_ops ov23850_subdev_pad_ops = {
+        .set_fmt = ov23850_set_fmt,
+        .get_fmt = ov23850_get_fmt,
+        .enum_mbus_code = camera_common_enum_mbus_code,
+        .enum_frame_size        = camera_common_enum_framesizes,
+        .enum_frame_interval    = camera_common_enum_frameintervals,
+};
+
+static struct v4l2_subdev_ops ov23850_subdev_ops = {
+        .core   = &ov23850_subdev_core_ops,
+        .video  = &ov23850_subdev_video_ops,
+        .pad    = &ov23850_subdev_pad_ops,
+};
+
+const struct of_device_id ov23850_of_match[] = {
+        { .compatible = "nvidia,ov23850", },
+        { },
+};
+
+static struct camera_common_sensor_ops ov23850_common_ops = {
+        .power_on = ov23850_power_on,
+        .power_off = ov23850_power_off,
+        .write_reg = ov23850_write_reg,
+        .read_reg = ov23850_read_reg,
+};
+
+static int ov23850_set_group_hold(struct ov23850 *priv)
+{
+        int err;
+        int gh_prev = switch_ctrl_qmenu[priv->group_hold_prev];
+
+        if (priv->group_hold_en == true && gh_prev == SWITCH_OFF) {
+                err = ov23850_write_reg(priv->s_data,
+                                       OV23850_GROUP_HOLD_ADDR, 0x00);
+                if (err)
+                        goto fail;
+                priv->group_hold_prev = 1;
+        } else if (priv->group_hold_en == false && gh_prev == SWITCH_ON) {
+                err = ov23850_write_reg(priv->s_data,
+                                       OV23850_GROUP_HOLD_ADDR, 0x10);
+                err |= ov23850_write_reg(priv->s_data,
+                                       OV23850_GROUP_HOLD_ADDR, 0xE0);
+                if (err)
+                        goto fail;
+                priv->group_hold_prev = 0;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: Group hold control error\n", __func__);
+        return err;
+}
+
+static int ov23850_set_gain(struct ov23850 *priv, s32 val)
+{
+        ov23850_reg reg_list[2];
+        ov23850_reg reg_list_short[2];
+        int err;
+        u16 gain;
+        int i = 0;
+
+        /* translate value */
+        gain = ov23850_to_gain((u32)val, OV23850_GAIN_SHIFT);
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, gain);
+
+        ov23850_get_gain_reg(reg_list, gain);
+        ov23850_get_gain_short_reg(reg_list_short, gain);
+        ov23850_set_group_hold(priv);
+
+        /* writing long gain */
+        for (i = 0; i < 2; i++) {
+                err = ov23850_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+        /* writing short gain */
+        for (i = 0; i < 2; i++) {
+                err = ov23850_write_reg(priv->s_data, reg_list_short[i].addr,
+                         reg_list_short[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: GAIN control error\n", __func__);
+        return err;
+}
+
+static int ov23850_set_frame_length(struct ov23850 *priv, s32 val)
+{
+        ov23850_reg reg_list[2];
+        int err;
+        u32 frame_length;
+        int i = 0;
+
+        frame_length = val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, frame_length);
+
+        ov23850_get_frame_length_regs(reg_list, frame_length);
+        ov23850_set_group_hold(priv);
+
+        for (i = 0; i < 2; i++) {
+                err = ov23850_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        priv->frame_length = frame_length;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: FRAME_LENGTH control error\n", __func__);
+        return err;
+}
+
+static int ov23850_set_coarse_time(struct ov23850 *priv, s32 val)
+{
+        ov23850_reg reg_list[2];
+        int err;
+        u32 coarse_time;
+        int i = 0;
+
+        coarse_time = val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, coarse_time);
+
+        ov23850_get_coarse_time_regs(reg_list, coarse_time);
+        ov23850_set_group_hold(priv);
+
+        for (i = 0; i < 2; i++) {
+                err = ov23850_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: COARSE_TIME control error\n", __func__);
+        return err;
+}
+
+static int ov23850_set_coarse_time_short(struct ov23850 *priv, s32 val)
+{
+        ov23850_reg reg_list[2];
+        int err;
+        struct v4l2_control hdr_control;
+        int hdr_en;
+        u32 coarse_time_short;
+        int i = 0;
+
+        /* check hdr enable ctrl */
+        hdr_control.id = V4L2_CID_HDR_EN;
+
+        err = camera_common_g_ctrl(priv->s_data, &hdr_control);
+        if (err < 0) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return err;
+        }
+
+        hdr_en = switch_ctrl_qmenu[hdr_control.value];
+        if (hdr_en == SWITCH_OFF)
+                return 0;
+
+        coarse_time_short = val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, coarse_time_short);
+
+        ov23850_get_coarse_time_short_regs(reg_list, coarse_time_short);
+        ov23850_set_group_hold(priv);
+
+        for (i = 0; i < 2; i++) {
+                err  = ov23850_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: COARSE_TIME_SHORT control error\n", __func__);
+        return err;
+}
+
+#if 0
+static int ov23850_eeprom_device_release(struct ov23850 *priv)
+{
+        int i;
+
+        for (i = 0; i < OV23850_EEPROM_NUM_BLOCKS; i++) {
+                if (priv->eeprom[i].i2c_client != NULL) {
+                        i2c_unregister_device(priv->eeprom[i].i2c_client);
+                        priv->eeprom[i].i2c_client = NULL;
+                }
+        }
+
+        return 0;
+}
+
+static int ov23850_eeprom_device_init(struct ov23850 *priv)
+{
+        char *dev_name = "eeprom_ov23850";
+        static struct regmap_config eeprom_regmap_config = {
+                .reg_bits = 8,
+                .val_bits = 8,
+        };
+        int i;
+        int err;
+
+        for (i = 0; i < OV23850_EEPROM_NUM_BLOCKS; i++) {
+                priv->eeprom[i].adap = i2c_get_adapter(
+                                priv->i2c_client->adapter->nr);
+                memset(&priv->eeprom[i].brd, 0, sizeof(priv->eeprom[i].brd));
+                strncpy(priv->eeprom[i].brd.type, dev_name,
+                                sizeof(priv->eeprom[i].brd.type));
+                priv->eeprom[i].brd.addr = OV23850_EEPROM_ADDRESS + i;
+                priv->eeprom[i].i2c_client = i2c_new_device(
+                                priv->eeprom[i].adap, &priv->eeprom[i].brd);
+
+                priv->eeprom[i].regmap = devm_regmap_init_i2c(
+                        priv->eeprom[i].i2c_client, &eeprom_regmap_config);
+                if (IS_ERR(priv->eeprom[i].regmap)) {
+                        err = PTR_ERR(priv->eeprom[i].regmap);
+                        ov23850_eeprom_device_release(priv);
+                        return err;
+                }
+        }
+
+        return 0;
+}
+
+static int ov23850_read_eeprom(struct ov23850 *priv,
+                                struct v4l2_ctrl *ctrl)
+{
+        int err, i;
+
+        for (i = 0; i < OV23850_EEPROM_NUM_BLOCKS; i++) {
+                err = regmap_bulk_read(priv->eeprom[i].regmap, 0,
+                        &priv->eeprom_buf[i * OV23850_EEPROM_BLOCK_SIZE],
+                        OV23850_EEPROM_BLOCK_SIZE);
+                if (err)
+                        return err;
+        }
+
+        for (i = 0; i < OV23850_EEPROM_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x",
+                        priv->eeprom_buf[i]);
+        return 0;
+}
+
+static int ov23850_write_eeprom(struct ov23850 *priv,
+                                char *string)
+{
+        int err;
+        int i;
+        u8 curr[3];
+        unsigned long data;
+
+        for (i = 0; i < OV23850_EEPROM_SIZE; i++) {
+                curr[0] = string[i*2];
+                curr[1] = string[i*2+1];
+                curr[2] = '\0';
+
+                err = kstrtol(curr, 16, &data);
+                if (err) {
+                        dev_err(&priv->i2c_client->dev,
+                                "invalid eeprom string\n");
+                        return -EINVAL;
+                }
+
+                priv->eeprom_buf[i] = (u8)data;
+                err = regmap_write(priv->eeprom[i >> 8].regmap,
+                                   i & 0xFF, (u8)data);
+                if (err)
+                        return err;
+                msleep(20);
+        }
+        return 0;
+}
+#endif
+
+static int ov23850_read_otp_manual(struct ov23850 *priv,
+                                u8 *buf, u16 addr_start, u16 addr_end)
+{
+        u8 status;
+        int i;
+        int err;
+        int size = addr_end - addr_start + 1;
+        u8 isp;
+        u16 addr_start_capped = addr_start;
+
+        if (addr_start > 0x6A00)
+                addr_start_capped = 0x69FF;
+
+        usleep_range(10000, 11000);
+        err = ov23850_write_table(priv, mode_table[OV23850_MODE_START_STREAM]);
+        if (err)
+                return err;
+
+        err = ov23850_read_reg(priv->s_data, OV23850_OTP_ISP_CTRL_ADDR, &isp);
+        if (err)
+                return err;
+        err = ov23850_write_reg(priv->s_data, OV23850_OTP_ISP_CTRL_ADDR,
+                                isp & 0xfe);
+        if (err)
+                return err;
+
+        err = ov23850_write_reg(priv->s_data, OV23850_OTP_MODE_CTRL_ADDR, 0x40);
+        if (err)
+                return err;
+
+
+        err = ov23850_write_reg(priv->s_data, OV23850_OTP_START_REG_ADDR_MSB,
+                                (addr_start_capped >> 8) & 0xff);
+        if (err)
+                return err;
+        err = ov23850_write_reg(priv->s_data, OV23850_OTP_START_REG_ADDR_LSB,
+                                addr_start_capped & 0xff);
+        if (err)
+                return err;
+
+        err = ov23850_write_reg(priv->s_data, OV23850_OTP_END_REG_ADDR_MSB,
+                                (addr_end >> 8) & 0xff);
+        if (err)
+                return err;
+        err = ov23850_write_reg(priv->s_data, OV23850_OTP_END_REG_ADDR_LSB,
+                                addr_end & 0xff);
+        if (err)
+                return err;
+
+        err = ov23850_write_reg(priv->s_data, OV23850_OTP_LOAD_CTRL_ADDR, 0x01);
+        if (err)
+                return err;
+
+        usleep_range(10000, 11000);
+        for (i = 0; i < size; i++) {
+                err = ov23850_read_reg(priv->s_data, addr_start + i, &buf[i]);
+                if (err)
+                        return err;
+
+                err = ov23850_read_reg(priv->s_data,
+                                       OV23850_OTP_LOAD_CTRL_ADDR, &status);
+                if (err)
+                        return err;
+
+                if (status & OV23850_OTP_RD_BUSY_MASK) {
+                        dev_err(&priv->i2c_client->dev,
+                                "another OTP read in progress\n");
+                        return err;
+                } else if (status & OV23850_OTP_BIST_ERROR_MASK) {
+                        dev_err(&priv->i2c_client->dev, "fuse id read error\n");
+                        return err;
+                }
+        }
+
+        err = ov23850_write_table(priv,
+                        mode_table[OV23850_MODE_STOP_STREAM]);
+        if (err)
+                return err;
+
+        err = ov23850_read_reg(priv->s_data, OV23850_OTP_ISP_CTRL_ADDR, &isp);
+        if (err)
+                return err;
+        err = ov23850_write_reg(priv->s_data, OV23850_OTP_ISP_CTRL_ADDR,
+                                isp | 0x01);
+        if (err)
+                return err;
+
+        return 0;
+}
+
+static int ov23850_otp_setup(struct ov23850 *priv)
+{
+        int err;
+        int i;
+        struct v4l2_ctrl *ctrl;
+        u8 otp_buf[OV23850_OTP_SIZE];
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        err = ov23850_read_otp_manual(priv,
+                                otp_buf,
+                                OV23850_OTP_START_ADDR,
+                                OV23850_OTP_END_ADDR);
+        if (err)
+                return -ENODEV;
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_OTP_DATA);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return -EINVAL;
+        }
+
+        for (i = 0; i < OV23850_OTP_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x",
+                        otp_buf[i]);
+        ctrl->p_cur.p_char = ctrl->p_new.p_char;
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int ov23850_fuse_id_setup(struct ov23850 *priv)
+{
+        int err;
+        int i;
+        struct v4l2_ctrl *ctrl;
+        u8 fuse_id[OV23850_FUSE_ID_SIZE];
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        err = ov23850_read_otp_manual(priv,
+                                fuse_id,
+                                OV23850_FUSE_ID_OTP_START_ADDR,
+                                OV23850_FUSE_ID_OTP_END_ADDR);
+        if (err)
+                return -ENODEV;
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_FUSE_ID);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return -EINVAL;
+        }
+
+        for (i = 0; i < OV23850_FUSE_ID_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x",
+                        fuse_id[i]);
+        ctrl->p_cur.p_char = ctrl->p_new.p_char;
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int ov23850_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct ov23850 *priv =
+                container_of(ctrl->handler, struct ov23850, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_EEPROM_DATA:
+#if 0
+                err = ov23850_read_eeprom(priv, ctrl);
+                if (err)
+                        return err;
+#endif
+                break;
+        default:
+                        pr_err("%s: unknown ctrl id.\n", __func__);
+                        return -EINVAL;
+        }
+
+        return err;
+}
+
+static int ov23850_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct ov23850 *priv =
+                container_of(ctrl->handler, struct ov23850, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_GAIN:
+                err = ov23850_set_gain(priv, ctrl->val);
+                break;
+        case V4L2_CID_FRAME_LENGTH:
+                err = ov23850_set_frame_length(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME:
+                err = ov23850_set_coarse_time(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME_SHORT:
+                err = ov23850_set_coarse_time_short(priv, ctrl->val);
+                break;
+        case V4L2_CID_GROUP_HOLD:
+                if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON) {
+                        priv->group_hold_en = true;
+                } else {
+                        priv->group_hold_en = false;
+                        err = ov23850_set_group_hold(priv);
+                }
+                break;
+        case V4L2_CID_EEPROM_DATA:
+#if 0
+                if (!ctrl->p_new.p_char[0])
+                        break;
+                err = ov23850_write_eeprom(priv, ctrl->p_new.p_char);
+                if (err)
+                        return err;
+#endif
+                break;
+        case V4L2_CID_HDR_EN:
+                break;
+        default:
+                pr_err("%s: unknown ctrl id.\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static int ov23850_ctrls_init(struct ov23850 *priv)
+{
+        struct i2c_client *client = priv->i2c_client;
+        struct v4l2_ctrl *ctrl;
+        int numctrls;
+        int err;
+        int i;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        numctrls = ARRAY_SIZE(ctrl_config_list);
+        v4l2_ctrl_handler_init(&priv->ctrl_handler, numctrls);
+
+        for (i = 0; i < numctrls; i++) {
+                ctrl = v4l2_ctrl_new_custom(&priv->ctrl_handler,
+                        &ctrl_config_list[i], NULL);
+                if (ctrl == NULL) {
+                        dev_err(&client->dev, "Failed to init %s ctrl\n",
+                                ctrl_config_list[i].name);
+                        continue;
+                }
+
+                if (ctrl_config_list[i].type == V4L2_CTRL_TYPE_STRING &&
+                        ctrl_config_list[i].flags & V4L2_CTRL_FLAG_READ_ONLY) {
+                        ctrl->p_new.p_char = devm_kzalloc(&client->dev,
+                                ctrl_config_list[i].max + 1, GFP_KERNEL);
+                }
+                priv->ctrls[i] = ctrl;
+        }
+
+        priv->numctrls = numctrls;
+        priv->subdev->ctrl_handler = &priv->ctrl_handler;
+        if (priv->ctrl_handler.error) {
+                dev_err(&client->dev, "Error %d adding controls\n",
+                        priv->ctrl_handler.error);
+                err = priv->ctrl_handler.error;
+                goto error;
+        }
+
+        err = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d setting default controls\n", err);
+                goto error;
+        }
+
+        err = ov23850_otp_setup(priv);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d reading otp data\n", err);
+                goto error;
+        }
+
+        err = ov23850_fuse_id_setup(priv);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d reading fuse id data\n", err);
+                goto error;
+        }
+
+        return 0;
+
+error:
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        return err;
+}
+
+MODULE_DEVICE_TABLE(of, ov23850_of_match);
+
+static struct camera_common_pdata *ov23850_parse_dt(struct i2c_client *client)
+{
+        struct device_node *np = client->dev.of_node;
+        struct camera_common_pdata *board_priv_pdata;
+        const struct of_device_id *match;
+        int gpio;
+        int err;
+
+        match = of_match_device(ov23850_of_match, &client->dev);
+        if (!match) {
+                dev_err(&client->dev, "Failed to find matching dt id\n");
+                return NULL;
+        }
+
+        board_priv_pdata = devm_kzalloc(&client->dev,
+                           sizeof(*board_priv_pdata), GFP_KERNEL);
+
+        err = of_property_read_string(np, "mclk",
+                                      &board_priv_pdata->mclk_name);
+        if (err) {
+                dev_err(&client->dev, "mclk not in DT\n");
+                goto error;
+        }
+
+        gpio = of_get_named_gpio(np, "pwdn-gpios", 0);
+        if (gpio < 0) {
+                dev_err(&client->dev, "pwdn gpios not in DT\n");
+                goto error;
+        }
+        board_priv_pdata->pwdn_gpio = (unsigned int)gpio;
+
+        gpio = of_get_named_gpio(np, "reset-gpios", 0);
+        if (gpio < 0) {
+                dev_err(&client->dev, "reset gpios not in DT\n");
+                goto error;
+        }
+        board_priv_pdata->reset_gpio = (unsigned int)gpio;
+
+        err = of_property_read_string(np, "avdd-reg",
+                        &board_priv_pdata->regulators.avdd);
+        if (err) {
+                dev_err(&client->dev, "avdd-reg not in DT\n");
+                goto error;
+        }
+        err = of_property_read_string(np, "dvdd-reg",
+                        &board_priv_pdata->regulators.dvdd);
+        if (err) {
+                dev_err(&client->dev, "dvdd-reg not in DT\n");
+                goto error;
+        }
+        err = of_property_read_string(np, "iovdd-reg",
+                        &board_priv_pdata->regulators.iovdd);
+        if (err) {
+                dev_err(&client->dev, "iovdd-reg not in DT\n");
+                goto error;
+        }
+        err = of_property_read_string(np, "vcmvdd-reg",
+                        &board_priv_pdata->regulators.vcmvdd);
+        if (err) {
+                dev_err(&client->dev, "vcmdd-reg not in DT\n");
+                goto error;
+        }
+
+        return board_priv_pdata;
+
+error:
+        devm_kfree(&client->dev, board_priv_pdata);
+        return NULL;
+}
+
+static int ov23850_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        dev_dbg(&client->dev, "%s:\n", __func__);
+        return 0;
+}
+
+static const struct v4l2_subdev_internal_ops ov23850_subdev_internal_ops = {
+        .open = ov23850_open,
+};
+
+static const struct media_entity_operations ov23850_media_ops = {
+        .link_validate = v4l2_subdev_link_validate,
+};
+
+static int ov23850_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct camera_common_data *common_data;
+        struct ov23850 *priv;
+        char debugfs_name[10];
+        int err;
+
+        pr_info("[OV23850]: probing v4l2 sensor at addr 0x%0x.\n",
+                client->addr);
+
+        common_data = devm_kzalloc(&client->dev,
+                            sizeof(struct camera_common_data), GFP_KERNEL);
+
+        priv = devm_kzalloc(&client->dev,
+                            sizeof(struct ov23850) + sizeof(struct v4l2_ctrl *)
+                            * ARRAY_SIZE(ctrl_config_list), GFP_KERNEL);
+
+        priv->regmap = devm_regmap_init_i2c(client, &sensor_regmap_config);
+        if (IS_ERR(priv->regmap)) {
+                dev_err(&client->dev,
+                        "regmap init failed: %ld\n", PTR_ERR(priv->regmap));
+                return -ENODEV;
+        }
+
+
+        priv->pdata = ov23850_parse_dt(client);
+        if (!priv->pdata) {
+                dev_err(&client->dev, "unable to get platform data\n");
+                return -EFAULT;
+        }
+
+        common_data->ops                = &ov23850_common_ops;
+        common_data->ctrl_handler       = &priv->ctrl_handler;
+        common_data->i2c_client         = client;
+        common_data->frmfmt             = &ov23850_frmfmt[0];
+        common_data->colorfmt           = camera_common_find_datafmt(
+                                          OV23850_DEFAULT_DATAFMT);
+        common_data->power              = &priv->power;
+        common_data->ctrls              = priv->ctrls;
+        common_data->priv               = (void *)priv;
+        common_data->numctrls           = ARRAY_SIZE(ctrl_config_list);
+        common_data->numfmts            = ARRAY_SIZE(ov23850_frmfmt);
+        common_data->def_mode           = OV23850_DEFAULT_MODE;
+        common_data->def_width          = OV23850_DEFAULT_WIDTH;
+        common_data->def_height         = OV23850_DEFAULT_HEIGHT;
+        common_data->fmt_width          = common_data->def_width;
+        common_data->fmt_height         = common_data->def_height;
+        common_data->def_clk_freq       = OV23850_DEFAULT_CLK_FREQ;
+
+        priv->i2c_client                = client;
+        priv->s_data                    = common_data;
+        priv->subdev                    = &common_data->subdev;
+        priv->subdev->dev               = &client->dev;
+        priv->s_data->dev               = &client->dev;
+        priv->group_hold_prev           = 0;
+
+        err = ov23850_power_get(priv);
+        if (err)
+                return err;
+
+        err = camera_common_parse_ports(client, common_data);
+        if (err) {
+                dev_err(&client->dev, "Failed to find port info\n");
+                return err;
+        }
+        sprintf(debugfs_name, "ov23850_%c", common_data->csi_port + 'a');
+        dev_dbg(&client->dev, "%s: dt node name %s\n", __func__, debugfs_name);
+        camera_common_create_debugfs(common_data, debugfs_name);
+
+        v4l2_i2c_subdev_init(&common_data->subdev, client,
+                             &ov23850_subdev_ops);
+
+        err = ov23850_ctrls_init(priv);
+        if (err)
+                return err;
+
+#if 0
+        /* eeprom interface */
+        err = ov23850_eeprom_device_init(priv);
+        if (err)
+                dev_err(&client->dev,
+                        "Failed to allocate eeprom register map: %d\n", err);
+#endif
+
+        priv->subdev->internal_ops = &ov23850_subdev_internal_ops;
+        priv->subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+                     V4L2_SUBDEV_FL_HAS_EVENTS;
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+        priv->subdev->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+        priv->subdev->entity.ops = &ov23850_media_ops;
+        err = media_entity_init(&priv->subdev->entity, 1, &priv->pad, 0);
+        if (err < 0) {
+                dev_err(&client->dev, "unable to init media entity\n");
+                return err;
+        }
+#endif
+
+        err = v4l2_async_register_subdev(priv->subdev);
+        if (err)
+                return err;
+
+        dev_info(&client->dev, "Detected OV23850 sensor\n");
+
+        return 0;
+}
+
+static int
+ov23850_remove(struct i2c_client *client)
+{
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov23850 *priv = (struct ov23850 *)s_data->priv;
+
+        v4l2_async_unregister_subdev(priv->subdev);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev->entity);
+#endif
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        ov23850_power_put(priv);
+        camera_common_remove_debugfs(s_data);
+
+        return 0;
+}
+
+static const struct i2c_device_id ov23850_id[] = {
+        { "ov23850", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, ov23850_id);
+
+static struct i2c_driver ov23850_i2c_driver = {
+        .driver = {
+                .name = "ov23850",
+                .owner = THIS_MODULE,
+                .of_match_table = of_match_ptr(ov23850_of_match),
+        },
+        .probe = ov23850_probe,
+        .remove = ov23850_remove,
+        .id_table = ov23850_id,
+};
+
+module_i2c_driver(ov23850_i2c_driver);
+
+MODULE_DESCRIPTION("I2C driver for OmniVision OV23850");
+MODULE_AUTHOR("David Wang <davidw@nvidia.com>");
+MODULE_LICENSE("GPL v2");
+
diff --git a/drivers/media/i2c/ov23850_mode_tbls.h b/drivers/media/i2c/ov23850_mode_tbls.h
new file mode 100644
index 000000000..350aae19d
--- /dev/null
+++ b/drivers/media/i2c/ov23850_mode_tbls.h
@@ -0,0 +1,4094 @@
+/*
+ * ov23850.c - ov23850 sensor driver
+ *
+ * Copyright (c) 2015-2016, NVIDIA CORPORATION, All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http:
+ */
+
+#include <media/camera_common.h>
+
+#ifndef __OV23850_I2C_TABLES__
+#define __OV23850_I2C_TABLES__
+
+#define OV23850_TABLE_WAIT_MS   0
+#define OV23850_TABLE_END       1
+#define OV23850_MAX_RETRIES     3
+#define OV23850_WAIT_MS         3
+
+#define ov23850_reg struct reg_8
+
+static ov23850_reg ov23850_start[] = {
+        { 0x0100, 0x01 }, /* mode select streaming on */
+        { OV23850_TABLE_END, 0x00 }
+};
+
+static ov23850_reg ov23850_stop[] = {
+        { 0x0100, 0x00 }, /* mode select streaming off */
+        { OV23850_TABLE_END, 0x00 }
+};
+
+static ov23850_reg tp_colorbars[] = {
+        {0x0600, 0x00},
+        {0x0601, 0x02},
+
+        {OV23850_TABLE_WAIT_MS, OV23850_WAIT_MS},
+        {OV23850_TABLE_END, 0x00}
+};
+
+static ov23850_reg mode_5632x4224_20fps[] = {
+        {0x0316, 0x1e},
+        {0x0317, 0x00},
+        {0x0318, 0x03},
+        {0x031c, 0x01},
+        {0x031d, 0x02},
+        {0x031e, 0x09},
+        {0x2b05, 0x02},
+        {0x2b06, 0x87},
+        {0x2b07, 0x01},
+        {0x2b08, 0xa8},
+        {0x0320, 0x02},
+        {0x3002, 0x00},
+        {0x300f, 0x11},
+        {0x3010, 0x01},
+        {0x3012, 0x41},
+        {0x3016, 0xd2},
+        {0x3018, 0x70},
+        {0x3019, 0xc3},
+        {0x301b, 0x96},
+        {0x3022, 0x0f},
+        {0x3023, 0xb4},
+        {0x3031, 0x91},
+        {0x3034, 0x41},
+        {0x340c, 0xff},
+        {0x3501, 0x12},
+        {0x3502, 0xa6},
+        {0x3503, 0x00},
+        {0x3507, 0x00},
+        {0x3508, 0x00},
+        {0x3509, 0x12},
+        {0x350a, 0x00},
+        {0x350b, 0x80},
+        {0x350f, 0x10},
+        {0x3541, 0x02},
+        {0x3542, 0x00},
+        {0x3543, 0x00},
+        {0x3547, 0x00},
+        {0x3548, 0x00},
+        {0x3549, 0x12},
+        {0x354b, 0x10},
+        {0x354f, 0x10},
+        {0x3601, 0xa4},
+        {0x3603, 0x97},
+        {0x3604, 0x02},
+        {0x3605, 0xf2},
+        {0x3606, 0xe8},
+        {0x3607, 0x11},
+        {0x360a, 0x34},
+        {0x360c, 0x13},
+        {0x3618, 0xcc},
+        {0x3620, 0x50},
+        {0x3621, 0x99},
+        {0x3622, 0x7d},
+        {0x3624, 0x05},
+        {0x362a, 0x25},
+        {0x3650, 0x04},
+        {0x3660, 0xc0},
+        {0x3661, 0x00},
+        {0x3662, 0x00},
+        {0x3664, 0x88},
+        {0x3667, 0x00},
+        {0x366a, 0x5c},
+        {0x366c, 0x80},
+        {0x3700, 0x62},
+        {0x3701, 0x08},
+        {0x3702, 0x10},
+        {0x3703, 0x3e},
+        {0x3704, 0x26},
+        {0x3705, 0x01},
+        {0x3706, 0x3a},
+        {0x3707, 0xc4},
+        {0x3708, 0x3c},
+        {0x3709, 0x1c},
+        {0x370a, 0x23},
+        {0x370b, 0x2c},
+        {0x370c, 0x42},
+        {0x370d, 0xa4},
+        {0x370e, 0x14},
+        {0x370f, 0x0a},
+        {0x3710, 0x15},
+        {0x3711, 0x0a},
+        {0x3712, 0xa2},
+        {0x3713, 0x00},
+        {0x371e, 0x2a},
+        {0x371f, 0x13},
+        {0x3714, 0x00},
+        {0x3717, 0x00},
+        {0x3719, 0x00},
+        {0x371c, 0x04},
+        {0x3720, 0xaa},
+        {0x3721, 0x10},
+        {0x3722, 0x50},
+        {0x3725, 0xf0},
+        {0x3726, 0x22},
+        {0x3727, 0x44},
+        {0x3728, 0x40},
+        {0x3729, 0x00},
+        {0x372b, 0x00},
+        {0x372c, 0x92},
+        {0x372d, 0x0c},
+        {0x372e, 0x22},
+        {0x372f, 0x91},
+        {0x3732, 0x01},
+        {0x3733, 0xd0},
+        {0x3730, 0x01},
+        {0x3731, 0xc8},
+        {0x3744, 0x01},
+        {0x3745, 0x24},
+        {0x3746, 0x00},
+        {0x3747, 0xd0},
+        {0x3748, 0x27},
+        {0x374a, 0x4b},
+        {0x374b, 0x44},
+        {0x3760, 0xd1},
+        {0x3761, 0x52},
+        {0x3762, 0xa4},
+        {0x3763, 0x14},
+        {0x3766, 0x0c},
+        {0x3767, 0x25},
+        {0x3768, 0x0c},
+        {0x3769, 0x24},
+        {0x376a, 0x09},
+        {0x376b, 0x02},
+        {0x376d, 0x01},
+        {0x376e, 0x53},
+        {0x376f, 0x01},
+        {0x378c, 0x08},
+        {0x378d, 0x46},
+        {0x378e, 0x14},
+        {0x378f, 0x02},
+        {0x3790, 0xc4},
+        {0x3792, 0x64},
+        {0x3793, 0x5d},
+        {0x3794, 0x29},
+        {0x3795, 0x4f},
+        {0x3796, 0x43},
+        {0x3797, 0x09},
+        {0x3798, 0x02},
+        {0x3799, 0x33},
+        {0x379a, 0x09},
+        {0x379b, 0x1e},
+        {0x379f, 0x3e},
+        {0x37a0, 0x44},
+        {0x37a1, 0x00},
+        {0x37a2, 0x44},
+        {0x37a3, 0x41},
+        {0x37a4, 0x88},
+        {0x37a5, 0x69},
+        {0x37b0, 0x48},
+        {0x37b1, 0x20},
+        {0x37b2, 0x03},
+        {0x37b3, 0x48},
+        {0x37b4, 0x02},
+        {0x37b5, 0x33},
+        {0x37b6, 0x22},
+        {0x37b8, 0x02},
+        {0x37bc, 0x02},
+        {0x37c0, 0x3b},
+        {0x37c1, 0xc2},
+        {0x37c2, 0x06},
+        {0x37c3, 0x06},
+        {0x37c5, 0x33},
+        {0x37c6, 0x35},
+        {0x37c7, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x14},
+        {0x3802, 0x00},
+        {0x3803, 0x0c},
+        {0x3804, 0x10},
+        {0x3805, 0x8b},
+        {0x3806, 0x0c},
+        {0x3807, 0x43},
+        {0x3808, 0x16},
+        {0x3809, 0x00},
+        {0x380a, 0x10},
+        {0x380b, 0x80},
+        {0x380c, 0x1C},
+        {0x380d, 0x20},
+        {0x380e, 0x12},
+        {0x380f, 0xc6},
+        {0x3810, 0x00},
+        {0x3811, 0x03},
+        {0x3813, 0x06},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x04},
+        {0x3834, 0x00},
+        {0x3835, 0x04},
+        {0x3836, 0x18},
+        {0x3837, 0x02},
+        {0x382f, 0x84},
+        {0x383c, 0xc8},
+        {0x383d, 0xff},
+        {0x3842, 0x00},
+        {0x384b, 0x00},
+        {0x3d85, 0x16},
+        {0x3d8c, 0x77},
+        {0x3d8d, 0x10},
+        {0x3f00, 0x52},
+        {0x4000, 0x17},
+        {0x4001, 0x60},
+        {0x4001, 0x60},
+        {0x4008, 0x00},
+        {0x4009, 0x13},
+        {0x400f, 0x00},
+        {0x4011, 0xfb},
+        {0x4017, 0x08},
+        {0x4018, 0x00},
+        {0x401a, 0xce},
+        {0x4019, 0x18},
+        {0x4020, 0x08},
+        {0x4022, 0x08},
+        {0x4024, 0x08},
+        {0x4026, 0x08},
+        {0x4028, 0x08},
+        {0x402a, 0x08},
+        {0x402c, 0x08},
+        {0x402e, 0x08},
+        {0x4030, 0x08},
+        {0x4032, 0x08},
+        {0x4034, 0x08},
+        {0x4036, 0x08},
+        {0x4038, 0x08},
+        {0x403a, 0x08},
+        {0x403c, 0x08},
+        {0x403e, 0x08},
+        {0x405c, 0x3f},
+        {0x4066, 0x04},
+        {0x4051, 0x03},
+        {0x4052, 0x00},
+        {0x4053, 0x80},
+        {0x4054, 0x00},
+        {0x4055, 0x80},
+        {0x4056, 0x00},
+        {0x4057, 0x80},
+        {0x4058, 0x00},
+        {0x4059, 0x80},
+        {0x4202, 0x00},
+        {0x4203, 0x01},
+        {0x430b, 0xff},
+        {0x430d, 0x00},
+        {0x4500, 0x72},
+        {0x4605, 0x00},
+        {0x4640, 0x01},
+        {0x4641, 0x04},
+        {0x4645, 0x00},
+        {0x4800, 0x04},
+        {0x4809, 0x2b},
+        {0x4813, 0x90},
+        {0x4817, 0x04},
+        {0x4833, 0x18},
+        {0x4837, 0x0A},
+        {0x484b, 0x01},
+        {0x4850, 0x5c},
+        {0x4852, 0x27},
+        {0x4856, 0x5c},
+        {0x4857, 0x55},
+        {0x486a, 0xaa},
+        {0x486e, 0x03},
+        {0x486f, 0x55},
+        {0x4875, 0xf0},
+        {0x4b04, 0x80},
+        {0x4b05, 0xb3},
+        {0x4b06, 0x00},
+        {0x4c01, 0xdf},
+        {0x4d00, 0x04},
+        {0x4d01, 0xf0},
+        {0x4d02, 0xb8},
+        {0x4d03, 0xf2},
+        {0x4d04, 0x88},
+        {0x4d05, 0x9d},
+        {0x4e00, 0x04},
+        {0x4e17, 0x04},
+        {0x4e33, 0x18},
+        {0x4e37, 0x11},
+        {0x4e4b, 0x01},
+        {0x4e50, 0x5c},
+        {0x4e52, 0x27},
+        {0x4e56, 0x5c},
+        {0x4e57, 0x55},
+        {0x4e6a, 0xaa},
+        {0x4e6e, 0x03},
+        {0x4e6f, 0x55},
+        {0x4e75, 0xf0},
+        {0x5000, 0x9b},
+        {0x5001, 0x42},
+        {0x5002, 0x10},
+        {0x5003, 0x01},
+        {0x5004, 0x00},
+        {0x5005, 0x00},
+        {0x501d, 0x00},
+        {0x501f, 0x06},
+        {0x5020, 0x03},
+        {0x5021, 0x00},
+        {0x5022, 0x13},
+        {0x5061, 0xff},
+        {0x5062, 0xff},
+        {0x5063, 0xff},
+        {0x5064, 0xff},
+        {0x506f, 0x00},
+        {0x5280, 0x00},
+        {0x5282, 0x00},
+        {0x5283, 0x01},
+        {0x5200, 0x00},
+        {0x5201, 0x71},
+        {0x5203, 0x04},
+        {0x5204, 0x00},
+        {0x5205, 0x88},
+        {0x5209, 0x00},
+        {0x520a, 0x80},
+        {0x520b, 0x04},
+        {0x520c, 0x01},
+        {0x5210, 0x10},
+        {0x5211, 0xa0},
+        {0x5292, 0x04},
+        {0x5500, 0x00},
+        {0x5501, 0x00},
+        {0x5502, 0x00},
+        {0x5503, 0x00},
+        {0x5504, 0x00},
+        {0x5505, 0x00},
+        {0x5506, 0x00},
+        {0x5507, 0x00},
+        {0x5508, 0x10},
+        {0x5509, 0x00},
+        {0x550a, 0x10},
+        {0x550b, 0x00},
+        {0x550c, 0x20},
+        {0x550d, 0x00},
+        {0x550e, 0x20},
+        {0x550f, 0x00},
+        {0x5510, 0x00},
+        {0x5511, 0x00},
+        {0x5512, 0x00},
+        {0x5513, 0x00},
+        {0x5514, 0x00},
+        {0x5515, 0x00},
+        {0x5516, 0x00},
+        {0x5517, 0x00},
+        {0x5518, 0x00},
+        {0x5519, 0x00},
+        {0x551a, 0x00},
+        {0x551b, 0x00},
+        {0x551c, 0x00},
+        {0x551d, 0x00},
+        {0x551e, 0x00},
+        {0x551f, 0x00},
+        {0x5520, 0x00},
+        {0x5521, 0x00},
+        {0x5522, 0x00},
+        {0x5523, 0x00},
+        {0x5524, 0x00},
+        {0x5525, 0x00},
+        {0x5526, 0x00},
+        {0x5527, 0x00},
+        {0x5528, 0x00},
+        {0x5529, 0x10},
+        {0x552a, 0x00},
+        {0x552b, 0x10},
+        {0x552c, 0x00},
+        {0x552d, 0x20},
+        {0x552e, 0x00},
+        {0x552f, 0x20},
+        {0x5530, 0x00},
+        {0x5531, 0x00},
+        {0x5532, 0x00},
+        {0x5533, 0x00},
+        {0x5534, 0x00},
+        {0x5535, 0x00},
+        {0x5536, 0x00},
+        {0x5537, 0x00},
+        {0x5538, 0x00},
+        {0x5539, 0x00},
+        {0x553a, 0x00},
+        {0x553b, 0x00},
+        {0x553c, 0x00},
+        {0x553d, 0x00},
+        {0x553e, 0x00},
+        {0x553f, 0x00},
+        {0x5540, 0x00},
+        {0x5541, 0x00},
+        {0x5542, 0x00},
+        {0x5543, 0x00},
+        {0x5544, 0x00},
+        {0x5545, 0x00},
+        {0x5546, 0x00},
+        {0x5547, 0x00},
+        {0x5548, 0x01},
+        {0x5549, 0x00},
+        {0x554a, 0x01},
+        {0x554b, 0x00},
+        {0x554c, 0x02},
+        {0x554d, 0x00},
+        {0x554e, 0x02},
+        {0x554f, 0x00},
+        {0x5550, 0x00},
+        {0x5551, 0x00},
+        {0x5552, 0x00},
+        {0x5553, 0x00},
+        {0x5554, 0x00},
+        {0x5555, 0x00},
+        {0x5556, 0x00},
+        {0x5557, 0x00},
+        {0x5558, 0x00},
+        {0x5559, 0x00},
+        {0x555a, 0x00},
+        {0x555b, 0x00},
+        {0x555c, 0x00},
+        {0x555d, 0x00},
+        {0x555e, 0x00},
+        {0x555f, 0x00},
+        {0x5560, 0x00},
+        {0x5561, 0x00},
+        {0x5562, 0x00},
+        {0x5563, 0x00},
+        {0x5564, 0x00},
+        {0x5565, 0x00},
+        {0x5566, 0x00},
+        {0x5567, 0x00},
+        {0x5568, 0x00},
+        {0x5569, 0x10},
+        {0x556a, 0x00},
+        {0x556b, 0x10},
+        {0x556c, 0x00},
+        {0x556d, 0x20},
+        {0x556e, 0x00},
+        {0x556f, 0x20},
+        {0x5570, 0x00},
+        {0x5571, 0x00},
+        {0x5572, 0x00},
+        {0x5573, 0x00},
+        {0x5574, 0x00},
+        {0x5575, 0x00},
+        {0x5576, 0x00},
+        {0x5577, 0x00},
+        {0x5578, 0x00},
+        {0x5579, 0x00},
+        {0x557a, 0x00},
+        {0x557b, 0x00},
+        {0x557c, 0x00},
+        {0x557d, 0x00},
+        {0x557e, 0x00},
+        {0x557f, 0x00},
+        {0x5580, 0x00},
+        {0x5581, 0x00},
+        {0x5582, 0x00},
+        {0x5583, 0x00},
+        {0x5584, 0x10},
+        {0x5585, 0x00},
+        {0x5586, 0x10},
+        {0x5587, 0x00},
+        {0x5588, 0x38},
+        {0x5589, 0x00},
+        {0x558a, 0x38},
+        {0x558b, 0x00},
+        {0x558c, 0x70},
+        {0x558d, 0x00},
+        {0x558e, 0x70},
+        {0x558f, 0x00},
+        {0x5590, 0x20},
+        {0x5591, 0x00},
+        {0x5592, 0x20},
+        {0x5593, 0x00},
+        {0x5594, 0x00},
+        {0x5595, 0x00},
+        {0x5596, 0x00},
+        {0x5597, 0x00},
+        {0x5598, 0x00},
+        {0x5599, 0x00},
+        {0x559a, 0x00},
+        {0x559b, 0x00},
+        {0x559c, 0x00},
+        {0x559d, 0x00},
+        {0x559e, 0x00},
+        {0x559f, 0x00},
+        {0x55a0, 0x00},
+        {0x55a1, 0x00},
+        {0x55a2, 0x00},
+        {0x55a3, 0x00},
+        {0x55a4, 0x00},
+        {0x55a5, 0x10},
+        {0x55a6, 0x00},
+        {0x55a7, 0x10},
+        {0x55a8, 0x00},
+        {0x55a9, 0x38},
+        {0x55aa, 0x00},
+        {0x55ab, 0x38},
+        {0x55ac, 0x00},
+        {0x55ad, 0x70},
+        {0x55ae, 0x00},
+        {0x55af, 0x70},
+        {0x55b0, 0x00},
+        {0x55b1, 0x20},
+        {0x55b2, 0x00},
+        {0x55b3, 0x20},
+        {0x55b4, 0x00},
+        {0x55b5, 0x00},
+        {0x55b6, 0x00},
+        {0x55b7, 0x00},
+        {0x55b8, 0x00},
+        {0x55b9, 0x00},
+        {0x55ba, 0x00},
+        {0x55bb, 0x00},
+        {0x55bc, 0x00},
+        {0x55bd, 0x00},
+        {0x55be, 0x00},
+        {0x55bf, 0x00},
+        {0x55c0, 0x00},
+        {0x55c1, 0x00},
+        {0x55c2, 0x00},
+        {0x55c3, 0x00},
+        {0x55c4, 0x01},
+        {0x55c5, 0x00},
+        {0x55c6, 0x01},
+        {0x55c7, 0x00},
+        {0x55c8, 0x03},
+        {0x55c9, 0x80},
+        {0x55ca, 0x03},
+        {0x55cb, 0x80},
+        {0x55cc, 0x07},
+        {0x55cd, 0x00},
+        {0x55ce, 0x07},
+        {0x55cf, 0x00},
+        {0x55d0, 0x02},
+        {0x55d1, 0x00},
+        {0x55d2, 0x02},
+        {0x55d3, 0x00},
+        {0x55d4, 0x00},
+        {0x55d5, 0x00},
+        {0x55d6, 0x00},
+        {0x55d7, 0x00},
+        {0x55d8, 0x00},
+        {0x55d9, 0x00},
+        {0x55da, 0x00},
+        {0x55db, 0x00},
+        {0x55dc, 0x00},
+        {0x55dd, 0x00},
+        {0x55de, 0x00},
+        {0x55df, 0x00},
+        {0x55e0, 0x00},
+        {0x55e1, 0x00},
+        {0x55e2, 0x00},
+        {0x55e3, 0x00},
+        {0x55e4, 0x00},
+        {0x55e5, 0x10},
+        {0x55e6, 0x00},
+        {0x55e7, 0x10},
+        {0x55e8, 0x00},
+        {0x55e9, 0x38},
+        {0x55ea, 0x00},
+        {0x55eb, 0x38},
+        {0x55ec, 0x00},
+        {0x55ed, 0x70},
+        {0x55ee, 0x00},
+        {0x55ef, 0x70},
+        {0x55f0, 0x00},
+        {0x55f1, 0x20},
+        {0x55f2, 0x00},
+        {0x55f3, 0x20},
+        {0x55f4, 0x00},
+        {0x55f5, 0x00},
+        {0x55f6, 0x00},
+        {0x55f7, 0x00},
+        {0x55f8, 0x00},
+        {0x55f9, 0x00},
+        {0x55fa, 0x00},
+        {0x55fb, 0x00},
+        {0x55fc, 0x00},
+        {0x55fd, 0x00},
+        {0x55fe, 0x00},
+        {0x55ff, 0x00},
+        {0x5600, 0x30},
+        {0x5601, 0x00},
+        {0x5602, 0x00},
+        {0x5603, 0x00},
+        {0x5604, 0x00},
+        {0x5605, 0x00},
+        {0x5606, 0x00},
+        {0x5607, 0x01},
+        {0x5608, 0x01},
+        {0x5609, 0x01},
+        {0x560f, 0xfc},
+        {0x5610, 0xf0},
+        {0x5611, 0x10},
+        {0x562f, 0xfc},
+        {0x5630, 0xf0},
+        {0x5631, 0x10},
+        {0x564f, 0xfc},
+        {0x5650, 0xf0},
+        {0x5651, 0x10},
+        {0x566f, 0xfc},
+        {0x5670, 0xf0},
+        {0x5671, 0x10},
+        {0x567b, 0x40},
+        {0x5690, 0x00},
+        {0x5691, 0x00},
+        {0x5692, 0x00},
+        {0x5693, 0x0a},
+        {0x5694, 0x80},
+        {0x5696, 0x06},
+        {0x5697, 0x0a},
+        {0x5698, 0x00},
+        {0x5699, 0x90},
+        {0x569a, 0x15},
+        {0x569b, 0x90},
+        {0x569c, 0x00},
+        {0x569d, 0x50},
+        {0x569e, 0x10},
+        {0x569f, 0x50},
+        {0x56a0, 0x36},
+        {0x56a1, 0x50},
+        {0x56a2, 0x16},
+        {0x56a3, 0x20},
+        {0x56a4, 0x10},
+        {0x56a5, 0xa0},
+        {0x5c80, 0x06},
+        {0x5c81, 0x80},
+        {0x5c82, 0x09},
+        {0x5c83, 0x5f},
+        {0x5d04, 0x01},
+        {0x5d05, 0x1a},
+        {0x5d06, 0x01},
+        {0x5d07, 0x1a},
+        {0x5d12, 0xf7},
+        {0x5d13, 0xdd},
+        {0x5d14, 0x97},
+        {0x5d15, 0x69},
+        {0x5d16, 0x61},
+        {0x5d17, 0x5b},
+        {0x5d18, 0x62},
+        {0x5d19, 0x8d},
+        {0x5d1a, 0xdd},
+        {0x5d1b, 0xff},
+        {0x5d24, 0x00},
+        {0x5d25, 0x00},
+        {0x5d26, 0x00},
+        {0x5d27, 0x0a},
+        {0x5d28, 0x80},
+        {0x5d2a, 0x00},
+        {0x5d2b, 0x90},
+        {0x5d2c, 0x15},
+        {0x5d2d, 0x90},
+        {0x5d2e, 0x00},
+        {0x5d2f, 0x50},
+        {0x5d30, 0x10},
+        {0x5d31, 0x50},
+        {0x5d32, 0x10},
+        {0x5d34, 0x36},
+        {0x5d35, 0x20},
+        {0x5d36, 0x90},
+        {0x5d37, 0xa0},
+        {0x5d38, 0x5c},
+        {0x5d39, 0x7b},
+        {0x5d1c, 0x00},
+        {0x5d1d, 0x90},
+        {0x5d1e, 0x00},
+        {0x5d1f, 0x50},
+        {0x5d20, 0x15},
+        {0x5d21, 0x00},
+        {0x5d22, 0x10},
+        {0x5d23, 0x00},
+        {0x5d29, 0xc0},
+        {0x3008, 0x01},
+        {0x3663, 0x60},
+        {0x3002, 0x01},
+        {0x3c00, 0x3c},
+        {0x3025, 0x03},
+        {0x3668, 0xf0},
+        {0x3400, 0x04},
+        {OV23850_TABLE_END, 0x00},
+};
+
+static ov23850_reg mode_5632x4224_8fps_dpcm[] = {
+        {0x0303, 0x20},
+        {0x0316, 0x1e},
+        {0x0318, 0x03},
+        {0x031c, 0x01},
+        {0x031d, 0x02},
+        {0x031e, 0x00},
+        {0x2b05, 0x02},
+        {0x2b06, 0x87},
+        {0x2b07, 0x01},
+        {0x2b08, 0xa8},
+        {0x0320, 0x02},
+        {0x3002, 0x00},
+        {0x300f, 0x11},
+        {0x3010, 0x01},
+        {0x3012, 0x41},
+        {0x3031, 0x91},
+        {0x3034, 0x41},
+        {0x340c, 0xff},
+        {0x3501, 0x12},
+        {0x3502, 0xa6},
+        {0x3503, 0x00},
+        {0x3507, 0x00},
+        {0x3508, 0x00},
+        {0x3509, 0x12},
+        {0x350a, 0x00},
+        {0x350b, 0x80},
+        {0x350f, 0x10},
+        {0x3541, 0x02},
+        {0x3542, 0x00},
+        {0x3543, 0x00},
+        {0x3547, 0x00},
+        {0x3548, 0x00},
+        {0x3549, 0x12},
+        {0x354b, 0x10},
+        {0x354f, 0x10},
+        {0x3603, 0x97},
+        {0x3606, 0xe8},
+        {0x3607, 0x11},
+        {0x360a, 0x44},
+        {0x360c, 0x11},
+        {0x3618, 0xcc},
+        {0x3620, 0x60},
+        {0x3621, 0x99},
+        {0x3622, 0x7c},
+        {0x3624, 0x05},
+        {0x362a, 0x25},
+        {0x3650, 0x04},
+        {0x3660, 0xc4},
+        {0x3661, 0x00},
+        {0x3662, 0x00},
+        {0x3664, 0x88},
+        {0x3667, 0x00},
+        {0x366a, 0x1c},
+        {0x366c, 0x80},
+        {0x3700, 0x62},
+        {0x3701, 0x08},
+        {0x3702, 0x10},
+        {0x3703, 0x38},
+        {0x3704, 0x26},
+        {0x3705, 0x01},
+        {0x3706, 0x3a},
+        {0x3707, 0xc4},
+        {0x3708, 0x3c},
+        {0x3709, 0x30},
+        {0x370a, 0x23},
+        {0x370b, 0x2c},
+        {0x370c, 0x42},
+        {0x370d, 0xa4},
+        {0x370e, 0x14},
+        {0x370f, 0x0a},
+        {0x3710, 0x15},
+        {0x3711, 0x0a},
+        {0x3712, 0xa2},
+        {0x3713, 0x00},
+        {0x371e, 0x2a},
+        {0x371f, 0x13},
+        {0x3714, 0x00},
+        {0x3717, 0x00},
+        {0x3719, 0x00},
+        {0x371c, 0x04},
+        {0x3720, 0xaa},
+        {0x3721, 0x10},
+        {0x3722, 0x50},
+        {0x3726, 0x22},
+        {0x3727, 0x44},
+        {0x3728, 0x40},
+        {0x3729, 0x00},
+        {0x372b, 0x00},
+        {0x372c, 0x92},
+        {0x372d, 0x0c},
+        {0x372e, 0x22},
+        {0x372f, 0x91},
+        {0x3732, 0x01},
+        {0x3733, 0xd0},
+        {0x3730, 0x01},
+        {0x3731, 0xc8},
+        {0x3744, 0x01},
+        {0x3745, 0x24},
+        {0x3746, 0x00},
+        {0x3747, 0xd0},
+        {0x3748, 0x27},
+        {0x374a, 0x4b},
+        {0x374b, 0x44},
+        {0x3760, 0xd1},
+        {0x3761, 0x52},
+        {0x3762, 0xa4},
+        {0x3763, 0x14},
+        {0x3766, 0x0c},
+        {0x3767, 0x25},
+        {0x3768, 0x0c},
+        {0x3769, 0x24},
+        {0x376a, 0x09},
+        {0x376b, 0x02},
+        {0x376d, 0x01},
+        {0x376e, 0x53},
+        {0x376f, 0x01},
+        {0x378c, 0x08},
+        {0x378d, 0x46},
+        {0x378e, 0x14},
+        {0x378f, 0x02},
+        {0x3790, 0xc4},
+        {0x3792, 0x64},
+        {0x3793, 0x5d},
+        {0x3794, 0x29},
+        {0x3795, 0x4f},
+        {0x3796, 0x43},
+        {0x3797, 0x09},
+        {0x3798, 0x02},
+        {0x3799, 0x33},
+        {0x379a, 0x09},
+        {0x379b, 0x1e},
+        {0x379f, 0x3e},
+        {0x37a0, 0x44},
+        {0x37a1, 0x00},
+        {0x37a2, 0x44},
+        {0x37a3, 0x41},
+        {0x37a4, 0x88},
+        {0x37a5, 0x69},
+        {0x37b0, 0x48},
+        {0x37b1, 0x20},
+        {0x37b2, 0x03},
+        {0x37b3, 0x48},
+        {0x37b4, 0x02},
+        {0x37b5, 0x33},
+        {0x37b6, 0x22},
+        {0x37b8, 0x02},
+        {0x37bc, 0x02},
+        {0x37c0, 0x3b},
+        {0x37c1, 0xc2},
+        {0x37c2, 0x06},
+        {0x37c3, 0x06},
+        {0x37c5, 0x33},
+        {0x37c6, 0x35},
+        {0x37c7, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x14},
+        {0x3802, 0x00},
+        {0x3803, 0x0c},
+        {0x3804, 0x10},
+        {0x3805, 0x8b},
+        {0x3806, 0x0c},
+        {0x3807, 0x43},
+        {0x3808, 0x16},
+        {0x3809, 0x00},
+        {0x380a, 0x10},
+        {0x380b, 0x80},
+        {0x380c, 0x18},
+        {0x380d, 0x60},
+        {0x380e, 0x12},
+        {0x380f, 0xc6},
+        {0x3810, 0x00},
+        {0x3811, 0x03},
+        {0x3813, 0x02},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x04},
+        {0x3834, 0x00},
+        {0x3835, 0x04},
+        {0x3836, 0x18},
+        {0x3837, 0x02},
+        {0x382f, 0x84},
+        {0x383c, 0xc8},
+        {0x383d, 0xff},
+        {0x3842, 0x00},
+        {0x384b, 0x00},
+        {0x3d85, 0x16},
+        {0x3f00, 0x12},
+        {0x4000, 0x17},
+        {0x4001, 0x60},
+        {0x4001, 0x60},
+        {0x4008, 0x00},
+        {0x4009, 0x13},
+        {0x400f, 0x00},
+        {0x4011, 0xfb},
+        {0x4017, 0x08},
+        {0x4018, 0x00},
+        {0x401a, 0x4e},
+        {0x4019, 0x18},
+        {0x4020, 0x08},
+        {0x4022, 0x08},
+        {0x4024, 0x08},
+        {0x4026, 0x08},
+        {0x4028, 0x08},
+        {0x402a, 0x08},
+        {0x402c, 0x08},
+        {0x402e, 0x08},
+        {0x4030, 0x08},
+        {0x4032, 0x08},
+        {0x4034, 0x08},
+        {0x4036, 0x08},
+        {0x4038, 0x08},
+        {0x403a, 0x08},
+        {0x403c, 0x08},
+        {0x403e, 0x08},
+        {0x405c, 0x3f},
+        {0x4066, 0x04},
+        {0x4051, 0x03},
+        {0x4052, 0x00},
+        {0x4053, 0x80},
+        {0x4054, 0x00},
+        {0x4055, 0x80},
+        {0x4056, 0x00},
+        {0x4057, 0x80},
+        {0x4058, 0x00},
+        {0x4059, 0x80},
+        {0x4202, 0x00},
+        {0x4203, 0x01},
+        {0x430b, 0xff},
+        {0x430d, 0x00},
+        {0x4500, 0x72},
+        {0x4605, 0x00},
+        {0x4640, 0x01},
+        {0x4641, 0x04},
+        {0x4645, 0x00},
+        {0x4800, 0x04},
+        {0x4809, 0x2b},
+        {0x4813, 0x90},
+        {0x4817, 0x04},
+        {0x4833, 0x18},
+        {0x4837, 0x20},
+        {0x484b, 0x01},
+        {0x4850, 0x5c},
+        {0x4852, 0x27},
+        {0x4856, 0x5c},
+        {0x4857, 0x55},
+        {0x486a, 0xaa},
+        {0x486e, 0x03},
+        {0x486f, 0x55},
+        {0x4875, 0xf0},
+        {0x4b04, 0x80},
+        {0x4b06, 0x00},
+        {0x4c01, 0xdf},
+        {0x4e00, 0x04},
+        {0x4e17, 0x04},
+        {0x4e33, 0x18},
+        {0x4e37, 0x11},
+        {0x4e4b, 0x01},
+        {0x4e50, 0x5c},
+        {0x4e52, 0x27},
+        {0x4e56, 0x5c},
+        {0x4e57, 0x55},
+        {0x4e6a, 0xaa},
+        {0x4e6e, 0x03},
+        {0x4e6f, 0x55},
+        {0x4e75, 0xf0},
+        {0x5000, 0x99},
+        {0x5001, 0x42},
+        {0x5002, 0x04},
+        {0x5003, 0x01},
+        {0x5004, 0x00},
+        {0x5005, 0x00},
+        {0x501d, 0x00},
+        {0x501f, 0x06},
+        {0x5021, 0x00},
+        {0x5022, 0x13},
+        {0x5061, 0xff},
+        {0x5062, 0xff},
+        {0x5063, 0xff},
+        {0x5064, 0xff},
+        {0x5280, 0x00},
+        {0x5282, 0x00},
+        {0x5283, 0x01},
+        {0x5200, 0x00},
+        {0x5201, 0x71},
+        {0x5203, 0x04},
+        {0x5204, 0x00},
+        {0x5205, 0x88},
+        {0x5209, 0x00},
+        {0x520a, 0x80},
+        {0x520b, 0x04},
+        {0x520c, 0x01},
+        {0x5210, 0x10},
+        {0x5211, 0xa0},
+        {0x5292, 0x00},
+        {0x5500, 0x00},
+        {0x5501, 0x00},
+        {0x5502, 0x00},
+        {0x5503, 0x00},
+        {0x5504, 0x00},
+        {0x5505, 0x00},
+        {0x5506, 0x00},
+        {0x5507, 0x00},
+        {0x5508, 0x00},
+        {0x5509, 0x00},
+        {0x550a, 0x00},
+        {0x550b, 0x00},
+        {0x550c, 0x00},
+        {0x550d, 0x00},
+        {0x550e, 0x00},
+        {0x550f, 0x00},
+        {0x5510, 0x04},
+        {0x5511, 0x04},
+        {0x5512, 0x04},
+        {0x5513, 0x04},
+        {0x5514, 0x08},
+        {0x5515, 0x08},
+        {0x5516, 0x08},
+        {0x5517, 0x08},
+        {0x5518, 0x00},
+        {0x5519, 0x00},
+        {0x551a, 0x00},
+        {0x551b, 0x00},
+        {0x551c, 0x00},
+        {0x551d, 0x00},
+        {0x551e, 0x00},
+        {0x551f, 0x00},
+        {0x5520, 0x00},
+        {0x5521, 0x00},
+        {0x5522, 0x00},
+        {0x5523, 0x00},
+        {0x5524, 0x00},
+        {0x5525, 0x00},
+        {0x5526, 0x00},
+        {0x5527, 0x00},
+        {0x5528, 0x00},
+        {0x5529, 0x00},
+        {0x552a, 0x00},
+        {0x552b, 0x00},
+        {0x552c, 0x00},
+        {0x552d, 0x00},
+        {0x552e, 0x00},
+        {0x552f, 0x00},
+        {0x5530, 0x04},
+        {0x5531, 0x04},
+        {0x5532, 0x04},
+        {0x5533, 0x04},
+        {0x5534, 0x08},
+        {0x5535, 0x08},
+        {0x5536, 0x08},
+        {0x5537, 0x08},
+        {0x5538, 0x00},
+        {0x5539, 0x00},
+        {0x553a, 0x00},
+        {0x553b, 0x00},
+        {0x553c, 0x00},
+        {0x553d, 0x00},
+        {0x553e, 0x00},
+        {0x553f, 0x00},
+        {0x5540, 0x00},
+        {0x5541, 0x00},
+        {0x5542, 0x00},
+        {0x5543, 0x00},
+        {0x5544, 0x00},
+        {0x5545, 0x00},
+        {0x5546, 0x00},
+        {0x5547, 0x00},
+        {0x5548, 0x00},
+        {0x5549, 0x00},
+        {0x554a, 0x00},
+        {0x554b, 0x00},
+        {0x554c, 0x00},
+        {0x554d, 0x00},
+        {0x554e, 0x00},
+        {0x554f, 0x00},
+        {0x5550, 0x04},
+        {0x5551, 0x04},
+        {0x5552, 0x04},
+        {0x5553, 0x04},
+        {0x5554, 0x08},
+        {0x5555, 0x08},
+        {0x5556, 0x08},
+        {0x5557, 0x08},
+        {0x5558, 0x00},
+        {0x5559, 0x00},
+        {0x555a, 0x00},
+        {0x555b, 0x00},
+        {0x555c, 0x00},
+        {0x555d, 0x00},
+        {0x555e, 0x00},
+        {0x555f, 0x00},
+        {0x5560, 0x00},
+        {0x5561, 0x00},
+        {0x5562, 0x00},
+        {0x5563, 0x00},
+        {0x5564, 0x00},
+        {0x5565, 0x00},
+        {0x5566, 0x00},
+        {0x5567, 0x00},
+        {0x5568, 0x00},
+        {0x5569, 0x00},
+        {0x556a, 0x00},
+        {0x556b, 0x00},
+        {0x556c, 0x00},
+        {0x556d, 0x00},
+        {0x556e, 0x00},
+        {0x556f, 0x00},
+        {0x5570, 0x04},
+        {0x5571, 0x04},
+        {0x5572, 0x04},
+        {0x5573, 0x04},
+        {0x5574, 0x08},
+        {0x5575, 0x08},
+        {0x5576, 0x08},
+        {0x5577, 0x08},
+        {0x5578, 0x00},
+        {0x5579, 0x00},
+        {0x557a, 0x00},
+        {0x557b, 0x00},
+        {0x557c, 0x00},
+        {0x557d, 0x00},
+        {0x557e, 0x00},
+        {0x557f, 0x00},
+        {0x5580, 0x00},
+        {0x5581, 0x00},
+        {0x5582, 0x00},
+        {0x5583, 0x00},
+        {0x5584, 0x00},
+        {0x5585, 0x00},
+        {0x5586, 0x00},
+        {0x5587, 0x00},
+        {0x5588, 0x00},
+        {0x5589, 0x00},
+        {0x558a, 0x00},
+        {0x558b, 0x00},
+        {0x558c, 0x04},
+        {0x558d, 0x04},
+        {0x558e, 0x04},
+        {0x558f, 0x04},
+        {0x5590, 0x0e},
+        {0x5591, 0x0e},
+        {0x5592, 0x0e},
+        {0x5593, 0x0e},
+        {0x5594, 0x1c},
+        {0x5595, 0x1c},
+        {0x5596, 0x1c},
+        {0x5597, 0x1c},
+        {0x5598, 0x08},
+        {0x5599, 0x08},
+        {0x559a, 0x08},
+        {0x559b, 0x08},
+        {0x559c, 0x00},
+        {0x559d, 0x00},
+        {0x559e, 0x00},
+        {0x559f, 0x00},
+        {0x55a0, 0x00},
+        {0x55a1, 0x00},
+        {0x55a2, 0x00},
+        {0x55a3, 0x00},
+        {0x55a4, 0x00},
+        {0x55a5, 0x00},
+        {0x55a6, 0x00},
+        {0x55a7, 0x00},
+        {0x55a8, 0x00},
+        {0x55a9, 0x00},
+        {0x55aa, 0x00},
+        {0x55ab, 0x00},
+        {0x55ac, 0x04},
+        {0x55ad, 0x04},
+        {0x55ae, 0x04},
+        {0x55af, 0x04},
+        {0x55b0, 0x0e},
+        {0x55b1, 0x0e},
+        {0x55b2, 0x0e},
+        {0x55b3, 0x0e},
+        {0x55b4, 0x1c},
+        {0x55b5, 0x1c},
+        {0x55b6, 0x1c},
+        {0x55b7, 0x1c},
+        {0x55b8, 0x08},
+        {0x55b9, 0x08},
+        {0x55ba, 0x08},
+        {0x55bb, 0x08},
+        {0x55bc, 0x00},
+        {0x55bd, 0x00},
+        {0x55be, 0x00},
+        {0x55bf, 0x00},
+        {0x55c0, 0x00},
+        {0x55c1, 0x00},
+        {0x55c2, 0x00},
+        {0x55c3, 0x00},
+        {0x55c4, 0x00},
+        {0x55c5, 0x00},
+        {0x55c6, 0x00},
+        {0x55c7, 0x00},
+        {0x55c8, 0x00},
+        {0x55c9, 0x00},
+        {0x55ca, 0x00},
+        {0x55cb, 0x00},
+        {0x55cc, 0x04},
+        {0x55cd, 0x04},
+        {0x55ce, 0x04},
+        {0x55cf, 0x04},
+        {0x55d0, 0x0e},
+        {0x55d1, 0x0e},
+        {0x55d2, 0x0e},
+        {0x55d3, 0x0e},
+        {0x55d4, 0x1c},
+        {0x55d5, 0x1c},
+        {0x55d6, 0x1c},
+        {0x55d7, 0x1c},
+        {0x55d8, 0x08},
+        {0x55d9, 0x08},
+        {0x55da, 0x08},
+        {0x55db, 0x08},
+        {0x55dc, 0x00},
+        {0x55dd, 0x00},
+        {0x55de, 0x00},
+        {0x55df, 0x00},
+        {0x55e0, 0x00},
+        {0x55e1, 0x00},
+        {0x55e2, 0x00},
+        {0x55e3, 0x00},
+        {0x55e4, 0x00},
+        {0x55e5, 0x00},
+        {0x55e6, 0x00},
+        {0x55e7, 0x00},
+        {0x55e8, 0x00},
+        {0x55e9, 0x00},
+        {0x55ea, 0x00},
+        {0x55eb, 0x00},
+        {0x55ec, 0x04},
+        {0x55ed, 0x04},
+        {0x55ee, 0x04},
+        {0x55ef, 0x04},
+        {0x55f0, 0x0e},
+        {0x55f1, 0x0e},
+        {0x55f2, 0x0e},
+        {0x55f3, 0x0e},
+        {0x55f4, 0x1c},
+        {0x55f5, 0x1c},
+        {0x55f6, 0x1c},
+        {0x55f7, 0x1c},
+        {0x55f8, 0x08},
+        {0x55f9, 0x08},
+        {0x55fa, 0x08},
+        {0x55fb, 0x08},
+        {0x55fc, 0x00},
+        {0x55fd, 0x00},
+        {0x55fe, 0x00},
+        {0x55ff, 0x00},
+        {0x567b, 0x00},
+        {0x5690, 0x00},
+        {0x5691, 0x00},
+        {0x5692, 0x00},
+        {0x5693, 0x0a},
+        {0x5694, 0xc0},
+        {0x5696, 0x06},
+        {0x5697, 0x0a},
+        {0x5698, 0x00},
+        {0x5699, 0x1c},
+        {0x569a, 0x16},
+        {0x569b, 0x04},
+        {0x569c, 0x00},
+        {0x569d, 0x1c},
+        {0x569e, 0x10},
+        {0x569f, 0x84},
+        {0x56a0, 0x36},
+        {0x56a1, 0x50},
+        {0x56a2, 0x16},
+        {0x56a3, 0x20},
+        {0x56a4, 0x10},
+        {0x56a5, 0xa0},
+        {0x5c80, 0x06},
+        {0x5c81, 0x80},
+        {0x5c82, 0x09},
+        {0x5c83, 0x5f},
+        {0x5d00, 0x01},
+        {0x5d01, 0x00},
+        {0x5d02, 0x01},
+        {0x5d03, 0x00},
+        {0x5d04, 0x02},
+        {0x5d05, 0x00},
+        {0x5d06, 0x02},
+        {0x5d07, 0x00},
+        {0x5d08, 0x70},
+        {0x5d09, 0x74},
+        {0x5d0a, 0x78},
+        {0x5d0b, 0x7c},
+        {0x5d0c, 0x80},
+        {0x5d0d, 0x80},
+        {0x5d0e, 0x7c},
+        {0x5d0f, 0x78},
+        {0x5d10, 0x74},
+        {0x5d11, 0x70},
+        {0x5d12, 0x98},
+        {0x5d13, 0x96},
+        {0x5d14, 0x63},
+        {0x5d15, 0x3a},
+        {0x5d16, 0x38},
+        {0x5d17, 0x39},
+        {0x5d18, 0x37},
+        {0x5d19, 0x53},
+        {0x5d1a, 0x8b},
+        {0x5d1b, 0x9d},
+        {0x5d24, 0x00},
+        {0x5d25, 0x00},
+        {0x5d26, 0x00},
+        {0x5d27, 0x0a},
+        {0x5d28, 0xc0},
+        {0x5d2a, 0x00},
+        {0x5d2b, 0x1c},
+        {0x5d2c, 0x16},
+        {0x5d2d, 0x04},
+        {0x5d2e, 0x00},
+        {0x5d2f, 0x1c},
+        {0x5d30, 0x10},
+        {0x5d31, 0x84},
+        {0x5d32, 0x10},
+        {0x5d34, 0x16},
+        {0x5d35, 0x20},
+        {0x5d36, 0x10},
+        {0x5d37, 0xa0},
+        {0x5d38, 0x5c},
+        {0x5d39, 0x7b},
+        {0x5d1c, 0x00},
+        {0x5d1d, 0x1c},
+        {0x5d1e, 0x00},
+        {0x5d1f, 0x1c},
+        {0x5d20, 0x15},
+        {0x5d21, 0xe0},
+        {0x5d22, 0x10},
+        {0x5d23, 0x40},
+        {0x5d29, 0x40},
+        {0x3008, 0x01},
+        {0x3663, 0x60},
+        {0x3002, 0x01},
+        {0x3c00, 0x3c},
+        {0x3025, 0x03},
+        {0x3668, 0xf0},
+        {0x3400, 0x04},
+        {OV23850_TABLE_END, 0x00},
+};
+
+static ov23850_reg mode_5632x4224_8fps[] = {
+        {0x0303, 0x20},
+        {0x0316, 0x1e},
+        {0x0317, 0x04},
+        {0x0318, 0x03},
+        {0x031c, 0x01},
+        {0x031d, 0x02},
+        {0x031e, 0x01},
+        {0x2b05, 0x02},
+        {0x2b06, 0x87},
+        {0x2b07, 0x01},
+        {0x2b08, 0xa8},
+        {0x0320, 0x02},
+        {0x3002, 0x00},
+        {0x300f, 0x11},
+        {0x3010, 0x01},
+        {0x3012, 0x41},
+        {0x3016, 0xd2},
+        {0x3018, 0x70},
+        {0x3019, 0xe1},
+        {0x301b, 0x96},
+        {0x3022, 0x0f},
+        {0x3023, 0xb4},
+        {0x3031, 0x91},
+        {0x3034, 0x41},
+        {0x340c, 0xff},
+        {0x3501, 0x12},
+        {0x3502, 0xa6},
+        {0x3503, 0x00},
+        {0x3507, 0x00},
+        {0x3508, 0x00},
+        {0x3509, 0x12},
+        {0x350a, 0x00},
+        {0x350b, 0x80},
+        {0x350f, 0x10},
+        {0x3541, 0x02},
+        {0x3542, 0x00},
+        {0x3543, 0x00},
+        {0x3547, 0x00},
+        {0x3548, 0x00},
+        {0x3549, 0x12},
+        {0x354b, 0x10},
+        {0x354f, 0x10},
+        {0x3601, 0xa4},
+        {0x3603, 0x97},
+        {0x3604, 0x02},
+        {0x3605, 0xf2},
+        {0x3606, 0xe8},
+        {0x3607, 0x11},
+        {0x360a, 0x34},
+        {0x360c, 0x13},
+        {0x3618, 0xcc},
+        {0x3620, 0x50},
+        {0x3621, 0x99},
+        {0x3622, 0x7d},
+        {0x3624, 0x05},
+        {0x362a, 0x25},
+        {0x3650, 0x04},
+        {0x3660, 0xc0},
+        {0x3661, 0x00},
+        {0x3662, 0x00},
+        {0x3664, 0x88},
+        {0x3667, 0x00},
+        {0x366a, 0x5c},
+        {0x366c, 0x80},
+        {0x3700, 0x62},
+        {0x3701, 0x08},
+        {0x3702, 0x10},
+        {0x3703, 0x3e},
+        {0x3704, 0x26},
+        {0x3705, 0x01},
+        {0x3706, 0x3a},
+        {0x3707, 0xc4},
+        {0x3708, 0x3c},
+        {0x3709, 0x1c},
+        {0x370a, 0x23},
+        {0x370b, 0x2c},
+        {0x370c, 0x42},
+        {0x370d, 0xa4},
+        {0x370e, 0x14},
+        {0x370f, 0x0a},
+        {0x3710, 0x15},
+        {0x3711, 0x0a},
+        {0x3712, 0xa2},
+        {0x3713, 0x00},
+        {0x371e, 0x2a},
+        {0x371f, 0x13},
+        {0x3714, 0x00},
+        {0x3717, 0x00},
+        {0x3719, 0x00},
+        {0x371c, 0x04},
+        {0x3720, 0xaa},
+        {0x3721, 0x10},
+        {0x3722, 0x50},
+        {0x3725, 0xf0},
+        {0x3726, 0x22},
+        {0x3727, 0x44},
+        {0x3728, 0x40},
+        {0x3729, 0x00},
+        {0x372b, 0x00},
+        {0x372c, 0x92},
+        {0x372d, 0x0c},
+        {0x372e, 0x22},
+        {0x372f, 0x91},
+        {0x3732, 0x01},
+        {0x3733, 0xd0},
+        {0x3730, 0x01},
+        {0x3731, 0xc8},
+        {0x3744, 0x01},
+        {0x3745, 0x24},
+        {0x3746, 0x00},
+        {0x3747, 0xd0},
+        {0x3748, 0x27},
+        {0x374a, 0x4b},
+        {0x374b, 0x44},
+        {0x3760, 0xd1},
+        {0x3761, 0x52},
+        {0x3762, 0xa4},
+        {0x3763, 0x14},
+        {0x3766, 0x0c},
+        {0x3767, 0x25},
+        {0x3768, 0x0c},
+        {0x3769, 0x24},
+        {0x376a, 0x09},
+        {0x376b, 0x02},
+        {0x376d, 0x01},
+        {0x376e, 0x53},
+        {0x376f, 0x01},
+        {0x378c, 0x08},
+        {0x378d, 0x46},
+        {0x378e, 0x14},
+        {0x378f, 0x02},
+        {0x3790, 0xc4},
+        {0x3792, 0x64},
+        {0x3793, 0x5d},
+        {0x3794, 0x29},
+        {0x3795, 0x4f},
+        {0x3796, 0x43},
+        {0x3797, 0x09},
+        {0x3798, 0x02},
+        {0x3799, 0x33},
+        {0x379a, 0x09},
+        {0x379b, 0x1e},
+        {0x379f, 0x3e},
+        {0x37a0, 0x44},
+        {0x37a1, 0x00},
+        {0x37a2, 0x44},
+        {0x37a3, 0x41},
+        {0x37a4, 0x88},
+        {0x37a5, 0x69},
+        {0x37b0, 0x48},
+        {0x37b1, 0x20},
+        {0x37b2, 0x03},
+        {0x37b3, 0x48},
+        {0x37b4, 0x02},
+        {0x37b5, 0x33},
+        {0x37b6, 0x22},
+        {0x37b8, 0x02},
+        {0x37bc, 0x02},
+        {0x37c0, 0x3b},
+        {0x37c1, 0xc2},
+        {0x37c2, 0x06},
+        {0x37c3, 0x06},
+        {0x37c5, 0x33},
+        {0x37c6, 0x35},
+        {0x37c7, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x14},
+        {0x3802, 0x00},
+        {0x3803, 0x0c},
+        {0x3804, 0x10},
+        {0x3805, 0x8b},
+        {0x3806, 0x0c},
+        {0x3807, 0x43},
+        {0x3808, 0x16},
+        {0x3809, 0x00},
+        {0x380a, 0x10},
+        {0x380b, 0x80},
+        {0x380c, 0x1c},
+        {0x380d, 0x20},
+        {0x380e, 0x12},
+        {0x380f, 0xc6},
+        {0x3810, 0x00},
+        {0x3811, 0x03},
+        {0x3813, 0x06},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x04},
+        {0x3834, 0x00},
+        {0x3835, 0x04},
+        {0x3836, 0x18},
+        {0x3837, 0x02},
+        {0x382f, 0x84},
+        {0x383c, 0xc8},
+        {0x383d, 0xff},
+        {0x3842, 0x00},
+        {0x384b, 0x00},
+        {0x3d85, 0x16},
+        {0x3d8c, 0x77},
+        {0x3d8d, 0x10},
+        {0x3f00, 0x52},
+        {0x4000, 0x17},
+        {0x4001, 0x60},
+        {0x4001, 0x60},
+        {0x4008, 0x00},
+        {0x4009, 0x13},
+        {0x400f, 0x00},
+        {0x4011, 0xfb},
+        {0x4017, 0x08},
+        {0x4018, 0x00},
+        {0x401a, 0xce},
+        {0x4019, 0x18},
+        {0x4020, 0x08},
+        {0x4022, 0x08},
+        {0x4024, 0x08},
+        {0x4026, 0x08},
+        {0x4028, 0x08},
+        {0x402a, 0x08},
+        {0x402c, 0x08},
+        {0x402e, 0x08},
+        {0x4030, 0x08},
+        {0x4032, 0x08},
+        {0x4034, 0x08},
+        {0x4036, 0x08},
+        {0x4038, 0x08},
+        {0x403a, 0x08},
+        {0x403c, 0x08},
+        {0x403e, 0x08},
+        {0x405c, 0x3f},
+        {0x4066, 0x04},
+        {0x4051, 0x03},
+        {0x4052, 0x00},
+        {0x4053, 0x80},
+        {0x4054, 0x00},
+        {0x4055, 0x80},
+        {0x4056, 0x00},
+        {0x4057, 0x80},
+        {0x4058, 0x00},
+        {0x4059, 0x80},
+        {0x4202, 0x00},
+        {0x4203, 0x01},
+        {0x430b, 0xff},
+        {0x430d, 0x00},
+        {0x4500, 0x72},
+        {0x4605, 0x00},
+        {0x4640, 0x01},
+        {0x4641, 0x04},
+        {0x4645, 0x00},
+        {0x4800, 0x04},
+        {0x4809, 0x2b},
+        {0x4813, 0x90},
+        {0x4817, 0x04},
+        {0x4833, 0x18},
+        {0x4837, 0x20},
+        {0x484b, 0x01},
+        {0x4850, 0x5c},
+        {0x4852, 0x27},
+        {0x4856, 0x5c},
+        {0x4857, 0x55},
+        {0x486a, 0xaa},
+        {0x486e, 0x03},
+        {0x486f, 0x55},
+        {0x4875, 0xf0},
+        {0x4b04, 0x80},
+        {0x4b05, 0xb3},
+        {0x4b06, 0x00},
+        {0x4c01, 0xdf},
+        {0x4d00, 0x04},
+        {0x4d01, 0xf0},
+        {0x4d02, 0xb8},
+        {0x4d03, 0xf2},
+        {0x4d04, 0x88},
+        {0x4d05, 0x9d},
+        {0x4e00, 0x04},
+        {0x4e17, 0x04},
+        {0x4e33, 0x18},
+        {0x4e37, 0x11},
+        {0x4e4b, 0x01},
+        {0x4e50, 0x5c},
+        {0x4e52, 0x27},
+        {0x4e56, 0x5c},
+        {0x4e57, 0x55},
+        {0x4e6a, 0xaa},
+        {0x4e6e, 0x03},
+        {0x4e6f, 0x55},
+        {0x4e75, 0xf0},
+        {0x5000, 0x9b},
+        {0x5001, 0x42},
+        {0x5002, 0x10},
+        {0x5003, 0x01},
+        {0x5004, 0x00},
+        {0x5005, 0x00},
+        {0x501d, 0x00},
+        {0x501f, 0x06},
+        {0x5020, 0x03},
+        {0x5021, 0x00},
+        {0x5022, 0x13},
+        {0x5061, 0xff},
+        {0x5062, 0xff},
+        {0x5063, 0xff},
+        {0x5064, 0xff},
+        {0x506f, 0x00},
+        {0x5280, 0x00},
+        {0x5282, 0x00},
+        {0x5283, 0x01},
+        {0x5200, 0x00},
+        {0x5201, 0x71},
+        {0x5203, 0x04},
+        {0x5204, 0x00},
+        {0x5205, 0x88},
+        {0x5209, 0x00},
+        {0x520a, 0x80},
+        {0x520b, 0x04},
+        {0x520c, 0x01},
+        {0x5210, 0x10},
+        {0x5211, 0xa0},
+        {0x5292, 0x04},
+        {0x5500, 0x00},
+        {0x5501, 0x00},
+        {0x5502, 0x00},
+        {0x5503, 0x00},
+        {0x5504, 0x00},
+        {0x5505, 0x00},
+        {0x5506, 0x00},
+        {0x5507, 0x00},
+        {0x5508, 0x10},
+        {0x5509, 0x00},
+        {0x550a, 0x10},
+        {0x550b, 0x00},
+        {0x550c, 0x20},
+        {0x550d, 0x00},
+        {0x550e, 0x20},
+        {0x550f, 0x00},
+        {0x5510, 0x00},
+        {0x5511, 0x00},
+        {0x5512, 0x00},
+        {0x5513, 0x00},
+        {0x5514, 0x00},
+        {0x5515, 0x00},
+        {0x5516, 0x00},
+        {0x5517, 0x00},
+        {0x5518, 0x00},
+        {0x5519, 0x00},
+        {0x551a, 0x00},
+        {0x551b, 0x00},
+        {0x551c, 0x00},
+        {0x551d, 0x00},
+        {0x551e, 0x00},
+        {0x551f, 0x00},
+        {0x5520, 0x00},
+        {0x5521, 0x00},
+        {0x5522, 0x00},
+        {0x5523, 0x00},
+        {0x5524, 0x00},
+        {0x5525, 0x00},
+        {0x5526, 0x00},
+        {0x5527, 0x00},
+        {0x5528, 0x00},
+        {0x5529, 0x10},
+        {0x552a, 0x00},
+        {0x552b, 0x10},
+        {0x552c, 0x00},
+        {0x552d, 0x20},
+        {0x552e, 0x00},
+        {0x552f, 0x20},
+        {0x5530, 0x00},
+        {0x5531, 0x00},
+        {0x5532, 0x00},
+        {0x5533, 0x00},
+        {0x5534, 0x00},
+        {0x5535, 0x00},
+        {0x5536, 0x00},
+        {0x5537, 0x00},
+        {0x5538, 0x00},
+        {0x5539, 0x00},
+        {0x553a, 0x00},
+        {0x553b, 0x00},
+        {0x553c, 0x00},
+        {0x553d, 0x00},
+        {0x553e, 0x00},
+        {0x553f, 0x00},
+        {0x5540, 0x00},
+        {0x5541, 0x00},
+        {0x5542, 0x00},
+        {0x5543, 0x00},
+        {0x5544, 0x00},
+        {0x5545, 0x00},
+        {0x5546, 0x00},
+        {0x5547, 0x00},
+        {0x5548, 0x01},
+        {0x5549, 0x00},
+        {0x554a, 0x01},
+        {0x554b, 0x00},
+        {0x554c, 0x02},
+        {0x554d, 0x00},
+        {0x554e, 0x02},
+        {0x554f, 0x00},
+        {0x5550, 0x00},
+        {0x5551, 0x00},
+        {0x5552, 0x00},
+        {0x5553, 0x00},
+        {0x5554, 0x00},
+        {0x5555, 0x00},
+        {0x5556, 0x00},
+        {0x5557, 0x00},
+        {0x5558, 0x00},
+        {0x5559, 0x00},
+        {0x555a, 0x00},
+        {0x555b, 0x00},
+        {0x555c, 0x00},
+        {0x555d, 0x00},
+        {0x555e, 0x00},
+        {0x555f, 0x00},
+        {0x5560, 0x00},
+        {0x5561, 0x00},
+        {0x5562, 0x00},
+        {0x5563, 0x00},
+        {0x5564, 0x00},
+        {0x5565, 0x00},
+        {0x5566, 0x00},
+        {0x5567, 0x00},
+        {0x5568, 0x00},
+        {0x5569, 0x10},
+        {0x556a, 0x00},
+        {0x556b, 0x10},
+        {0x556c, 0x00},
+        {0x556d, 0x20},
+        {0x556e, 0x00},
+        {0x556f, 0x20},
+        {0x5570, 0x00},
+        {0x5571, 0x00},
+        {0x5572, 0x00},
+        {0x5573, 0x00},
+        {0x5574, 0x00},
+        {0x5575, 0x00},
+        {0x5576, 0x00},
+        {0x5577, 0x00},
+        {0x5578, 0x00},
+        {0x5579, 0x00},
+        {0x557a, 0x00},
+        {0x557b, 0x00},
+        {0x557c, 0x00},
+        {0x557d, 0x00},
+        {0x557e, 0x00},
+        {0x557f, 0x00},
+        {0x5580, 0x00},
+        {0x5581, 0x00},
+        {0x5582, 0x00},
+        {0x5583, 0x00},
+        {0x5584, 0x10},
+        {0x5585, 0x00},
+        {0x5586, 0x10},
+        {0x5587, 0x00},
+        {0x5588, 0x38},
+        {0x5589, 0x00},
+        {0x558a, 0x38},
+        {0x558b, 0x00},
+        {0x558c, 0x70},
+        {0x558d, 0x00},
+        {0x558e, 0x70},
+        {0x558f, 0x00},
+        {0x5590, 0x20},
+        {0x5591, 0x00},
+        {0x5592, 0x20},
+        {0x5593, 0x00},
+        {0x5594, 0x00},
+        {0x5595, 0x00},
+        {0x5596, 0x00},
+        {0x5597, 0x00},
+        {0x5598, 0x00},
+        {0x5599, 0x00},
+        {0x559a, 0x00},
+        {0x559b, 0x00},
+        {0x559c, 0x00},
+        {0x559d, 0x00},
+        {0x559e, 0x00},
+        {0x559f, 0x00},
+        {0x55a0, 0x00},
+        {0x55a1, 0x00},
+        {0x55a2, 0x00},
+        {0x55a3, 0x00},
+        {0x55a4, 0x00},
+        {0x55a5, 0x10},
+        {0x55a6, 0x00},
+        {0x55a7, 0x10},
+        {0x55a8, 0x00},
+        {0x55a9, 0x38},
+        {0x55aa, 0x00},
+        {0x55ab, 0x38},
+        {0x55ac, 0x00},
+        {0x55ad, 0x70},
+        {0x55ae, 0x00},
+        {0x55af, 0x70},
+        {0x55b0, 0x00},
+        {0x55b1, 0x20},
+        {0x55b2, 0x00},
+        {0x55b3, 0x20},
+        {0x55b4, 0x00},
+        {0x55b5, 0x00},
+        {0x55b6, 0x00},
+        {0x55b7, 0x00},
+        {0x55b8, 0x00},
+        {0x55b9, 0x00},
+        {0x55ba, 0x00},
+        {0x55bb, 0x00},
+        {0x55bc, 0x00},
+        {0x55bd, 0x00},
+        {0x55be, 0x00},
+        {0x55bf, 0x00},
+        {0x55c0, 0x00},
+        {0x55c1, 0x00},
+        {0x55c2, 0x00},
+        {0x55c3, 0x00},
+        {0x55c4, 0x01},
+        {0x55c5, 0x00},
+        {0x55c6, 0x01},
+        {0x55c7, 0x00},
+        {0x55c8, 0x03},
+        {0x55c9, 0x80},
+        {0x55ca, 0x03},
+        {0x55cb, 0x80},
+        {0x55cc, 0x07},
+        {0x55cd, 0x00},
+        {0x55ce, 0x07},
+        {0x55cf, 0x00},
+        {0x55d0, 0x02},
+        {0x55d1, 0x00},
+        {0x55d2, 0x02},
+        {0x55d3, 0x00},
+        {0x55d4, 0x00},
+        {0x55d5, 0x00},
+        {0x55d6, 0x00},
+        {0x55d7, 0x00},
+        {0x55d8, 0x00},
+        {0x55d9, 0x00},
+        {0x55da, 0x00},
+        {0x55db, 0x00},
+        {0x55dc, 0x00},
+        {0x55dd, 0x00},
+        {0x55de, 0x00},
+        {0x55df, 0x00},
+        {0x55e0, 0x00},
+        {0x55e1, 0x00},
+        {0x55e2, 0x00},
+        {0x55e3, 0x00},
+        {0x55e4, 0x00},
+        {0x55e5, 0x10},
+        {0x55e6, 0x00},
+        {0x55e7, 0x10},
+        {0x55e8, 0x00},
+        {0x55e9, 0x38},
+        {0x55ea, 0x00},
+        {0x55eb, 0x38},
+        {0x55ec, 0x00},
+        {0x55ed, 0x70},
+        {0x55ee, 0x00},
+        {0x55ef, 0x70},
+        {0x55f0, 0x00},
+        {0x55f1, 0x20},
+        {0x55f2, 0x00},
+        {0x55f3, 0x20},
+        {0x55f4, 0x00},
+        {0x55f5, 0x00},
+        {0x55f6, 0x00},
+        {0x55f7, 0x00},
+        {0x55f8, 0x00},
+        {0x55f9, 0x00},
+        {0x55fa, 0x00},
+        {0x55fb, 0x00},
+        {0x55fc, 0x00},
+        {0x55fd, 0x00},
+        {0x55fe, 0x00},
+        {0x55ff, 0x00},
+        {0x5600, 0x30},
+        {0x5601, 0x00},
+        {0x5602, 0x00},
+        {0x5603, 0x00},
+        {0x5604, 0x00},
+        {0x5605, 0x00},
+        {0x5606, 0x00},
+        {0x5607, 0x01},
+        {0x5608, 0x01},
+        {0x5609, 0x01},
+        {0x560f, 0xfc},
+        {0x5610, 0xf0},
+        {0x5611, 0x10},
+        {0x562f, 0xfc},
+        {0x5630, 0xf0},
+        {0x5631, 0x10},
+        {0x564f, 0xfc},
+        {0x5650, 0xf0},
+        {0x5651, 0x10},
+        {0x566f, 0xfc},
+        {0x5670, 0xf0},
+        {0x5671, 0x10},
+        {0x567b, 0x40},
+        {0x5690, 0x00},
+        {0x5691, 0x00},
+        {0x5692, 0x00},
+        {0x5693, 0x0a},
+        {0x5694, 0x80},
+        {0x5696, 0x06},
+        {0x5697, 0x0a},
+        {0x5698, 0x00},
+        {0x5699, 0x90},
+        {0x569a, 0x15},
+        {0x569b, 0x90},
+        {0x569c, 0x00},
+        {0x569d, 0x50},
+        {0x569e, 0x10},
+        {0x569f, 0x50},
+        {0x56a0, 0x36},
+        {0x56a1, 0x50},
+        {0x56a2, 0x16},
+        {0x56a3, 0x20},
+        {0x56a4, 0x10},
+        {0x56a5, 0xa0},
+        {0x5c80, 0x06},
+        {0x5c81, 0x80},
+        {0x5c82, 0x09},
+        {0x5c83, 0x5f},
+        {0x5d04, 0x01},
+        {0x5d05, 0x1a},
+        {0x5d06, 0x01},
+        {0x5d07, 0x1a},
+        {0x5d12, 0xf7},
+        {0x5d13, 0xdd},
+        {0x5d14, 0x97},
+        {0x5d15, 0x69},
+        {0x5d16, 0x61},
+        {0x5d17, 0x5b},
+        {0x5d18, 0x62},
+        {0x5d19, 0x8d},
+        {0x5d1a, 0xdd},
+        {0x5d1b, 0xff},
+        {0x5d24, 0x00},
+        {0x5d25, 0x00},
+        {0x5d26, 0x00},
+        {0x5d27, 0x0a},
+        {0x5d28, 0x80},
+        {0x5d2a, 0x00},
+        {0x5d2b, 0x90},
+        {0x5d2c, 0x15},
+        {0x5d2d, 0x90},
+        {0x5d2e, 0x00},
+        {0x5d2f, 0x50},
+        {0x5d30, 0x10},
+        {0x5d31, 0x50},
+        {0x5d32, 0x10},
+        {0x5d34, 0x36},
+        {0x5d35, 0x20},
+        {0x5d36, 0x90},
+        {0x5d37, 0xa0},
+        {0x5d38, 0x5c},
+        {0x5d39, 0x7b},
+        {0x5d1c, 0x00},
+        {0x5d1d, 0x90},
+        {0x5d1e, 0x00},
+        {0x5d1f, 0x50},
+        {0x5d20, 0x15},
+        {0x5d21, 0x00},
+        {0x5d22, 0x10},
+        {0x5d23, 0x00},
+        {0x5d29, 0xc0},
+        {0x3008, 0x01},
+        {0x3663, 0x60},
+        {0x3002, 0x01},
+        {0x3c00, 0x3c},
+        {0x3025, 0x03},
+        {0x3668, 0xf0},
+        {0x3400, 0x04},
+        {OV23850_TABLE_END, 0x00}
+};
+
+static ov23850_reg mode_5632x4224_24fps_dpcm[] = {
+        {0x0317, 0x00},
+        {0x0318, 0x03},
+        {0x0316, 0x1e},
+        {0x0318, 0x03},
+        {0x031c, 0x01},
+        {0x031d, 0x02},
+        {0x031e, 0x00},
+        {0x2b05, 0x02},
+        {0x2b06, 0x87},
+        {0x2b07, 0x01},
+        {0x2b08, 0xa8},
+        {0x0320, 0x02},
+        {0x3002, 0x00},
+        {0x300f, 0x11},
+        {0x3010, 0x01},
+        {0x3012, 0x41},
+        {0x3031, 0x91},
+        {0x3034, 0x41},
+        {0x340c, 0xff},
+        {0x3501, 0x12},
+        {0x3502, 0xa6},
+        {0x3503, 0x00},
+        {0x3507, 0x00},
+        {0x3508, 0x00},
+        {0x3509, 0x12},
+        {0x350a, 0x00},
+        {0x350b, 0x80},
+        {0x350f, 0x10},
+        {0x3541, 0x02},
+        {0x3542, 0x00},
+        {0x3543, 0x00},
+        {0x3547, 0x00},
+        {0x3548, 0x00},
+        {0x3549, 0x12},
+        {0x354b, 0x10},
+        {0x354f, 0x10},
+        {0x3603, 0x97},
+        {0x3606, 0xe8},
+        {0x3607, 0x11},
+        {0x360a, 0x44},
+        {0x360c, 0x11},
+        {0x3618, 0xcc},
+        {0x3620, 0x60},
+        {0x3621, 0x99},
+        {0x3622, 0x7c},
+        {0x3624, 0x05},
+        {0x362a, 0x25},
+        {0x3650, 0x04},
+        {0x3660, 0xc4},
+        {0x3661, 0x00},
+        {0x3662, 0x00},
+        {0x3664, 0x88},
+        {0x3667, 0x00},
+        {0x366a, 0x1c},
+        {0x366c, 0x80},
+        {0x3700, 0x62},
+        {0x3701, 0x08},
+        {0x3702, 0x10},
+        {0x3703, 0x38},
+        {0x3704, 0x26},
+        {0x3705, 0x01},
+        {0x3706, 0x3a},
+        {0x3707, 0xc4},
+        {0x3708, 0x3c},
+        {0x3709, 0x30},
+        {0x370a, 0x23},
+        {0x370b, 0x2c},
+        {0x370c, 0x42},
+        {0x370d, 0xa4},
+        {0x370e, 0x14},
+        {0x370f, 0x0a},
+        {0x3710, 0x15},
+        {0x3711, 0x0a},
+        {0x3712, 0xa2},
+        {0x3713, 0x00},
+        {0x371e, 0x2a},
+        {0x371f, 0x13},
+        {0x3714, 0x00},
+        {0x3717, 0x00},
+        {0x3719, 0x00},
+        {0x371c, 0x04},
+        {0x3720, 0xaa},
+        {0x3721, 0x10},
+        {0x3722, 0x50},
+        {0x3726, 0x22},
+        {0x3727, 0x44},
+        {0x3728, 0x40},
+        {0x3729, 0x00},
+        {0x372b, 0x00},
+        {0x372c, 0x92},
+        {0x372d, 0x0c},
+        {0x372e, 0x22},
+        {0x372f, 0x91},
+        {0x3732, 0x01},
+        {0x3733, 0xd0},
+        {0x3730, 0x01},
+        {0x3731, 0xc8},
+        {0x3744, 0x01},
+        {0x3745, 0x24},
+        {0x3746, 0x00},
+        {0x3747, 0xd0},
+        {0x3748, 0x27},
+        {0x374a, 0x4b},
+        {0x374b, 0x44},
+        {0x3760, 0xd1},
+        {0x3761, 0x52},
+        {0x3762, 0xa4},
+        {0x3763, 0x14},
+        {0x3766, 0x0c},
+        {0x3767, 0x25},
+        {0x3768, 0x0c},
+        {0x3769, 0x24},
+        {0x376a, 0x09},
+        {0x376b, 0x02},
+        {0x376d, 0x01},
+        {0x376e, 0x53},
+        {0x376f, 0x01},
+        {0x378c, 0x08},
+        {0x378d, 0x46},
+        {0x378e, 0x14},
+        {0x378f, 0x02},
+        {0x3790, 0xc4},
+        {0x3792, 0x64},
+        {0x3793, 0x5d},
+        {0x3794, 0x29},
+        {0x3795, 0x4f},
+        {0x3796, 0x43},
+        {0x3797, 0x09},
+        {0x3798, 0x02},
+        {0x3799, 0x33},
+        {0x379a, 0x09},
+        {0x379b, 0x1e},
+        {0x379f, 0x3e},
+        {0x37a0, 0x44},
+        {0x37a1, 0x00},
+        {0x37a2, 0x44},
+        {0x37a3, 0x41},
+        {0x37a4, 0x88},
+        {0x37a5, 0x69},
+        {0x37b0, 0x48},
+        {0x37b1, 0x20},
+        {0x37b2, 0x03},
+        {0x37b3, 0x48},
+        {0x37b4, 0x02},
+        {0x37b5, 0x33},
+        {0x37b6, 0x22},
+        {0x37b8, 0x02},
+        {0x37bc, 0x02},
+        {0x37c0, 0x3b},
+        {0x37c1, 0xc2},
+        {0x37c2, 0x06},
+        {0x37c3, 0x06},
+        {0x37c5, 0x33},
+        {0x37c6, 0x35},
+        {0x37c7, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x14},
+        {0x3802, 0x00},
+        {0x3803, 0x0c},
+        {0x3804, 0x10},
+        {0x3805, 0x8b},
+        {0x3806, 0x0c},
+        {0x3807, 0x43},
+        {0x3808, 0x16},
+        {0x3809, 0x00},
+        {0x380a, 0x10},
+        {0x380b, 0x80},
+        {0x380c, 0x18},
+        {0x380d, 0x60},
+        {0x380e, 0x12},
+        {0x380f, 0xc6},
+        {0x3810, 0x00},
+        {0x3811, 0x03},
+        {0x3813, 0x02},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x04},
+        {0x3834, 0x00},
+        {0x3835, 0x04},
+        {0x3836, 0x18},
+        {0x3837, 0x02},
+        {0x382f, 0x84},
+        {0x383c, 0xc8},
+        {0x383d, 0xff},
+        {0x3842, 0x00},
+        {0x384b, 0x00},
+        {0x3d85, 0x16},
+        {0x3f00, 0x12},
+        {0x4000, 0x17},
+        {0x4001, 0x60},
+        {0x4001, 0x60},
+        {0x4008, 0x00},
+        {0x4009, 0x13},
+        {0x400f, 0x00},
+        {0x4011, 0xfb},
+        {0x4017, 0x08},
+        {0x4018, 0x00},
+        {0x401a, 0x4e},
+        {0x4019, 0x18},
+        {0x4020, 0x08},
+        {0x4022, 0x08},
+        {0x4024, 0x08},
+        {0x4026, 0x08},
+        {0x4028, 0x08},
+        {0x402a, 0x08},
+        {0x402c, 0x08},
+        {0x402e, 0x08},
+        {0x4030, 0x08},
+        {0x4032, 0x08},
+        {0x4034, 0x08},
+        {0x4036, 0x08},
+        {0x4038, 0x08},
+        {0x403a, 0x08},
+        {0x403c, 0x08},
+        {0x403e, 0x08},
+        {0x405c, 0x3f},
+        {0x4066, 0x04},
+        {0x4051, 0x03},
+        {0x4052, 0x00},
+        {0x4053, 0x80},
+        {0x4054, 0x00},
+        {0x4055, 0x80},
+        {0x4056, 0x00},
+        {0x4057, 0x80},
+        {0x4058, 0x00},
+        {0x4059, 0x80},
+        {0x4202, 0x00},
+        {0x4203, 0x01},
+        {0x430b, 0xff},
+        {0x430d, 0x00},
+        {0x4500, 0x72},
+        {0x4605, 0x00},
+        {0x4640, 0x01},
+        {0x4641, 0x04},
+        {0x4645, 0x00},
+        {0x4800, 0x04},
+        {0x4809, 0x2b},
+        {0x4813, 0x90},
+        {0x4817, 0x04},
+        {0x4833, 0x18},
+        {0x4837, 0x0A},
+        {0x484b, 0x01},
+        {0x4850, 0x5c},
+        {0x4852, 0x27},
+        {0x4856, 0x5c},
+        {0x4857, 0x55},
+        {0x486a, 0xaa},
+        {0x486e, 0x03},
+        {0x486f, 0x55},
+        {0x4875, 0xf0},
+        {0x4b04, 0x80},
+        {0x4b06, 0x00},
+        {0x4c01, 0xdf},
+        {0x4e00, 0x04},
+        {0x4e17, 0x04},
+        {0x4e33, 0x18},
+        {0x4e37, 0x11},
+        {0x4e4b, 0x01},
+        {0x4e50, 0x5c},
+        {0x4e52, 0x27},
+        {0x4e56, 0x5c},
+        {0x4e57, 0x55},
+        {0x4e6a, 0xaa},
+        {0x4e6e, 0x03},
+        {0x4e6f, 0x55},
+        {0x4e75, 0xf0},
+        {0x5000, 0x99},
+        {0x5001, 0x42},
+        {0x5002, 0x04},
+        {0x5003, 0x01},
+        {0x5004, 0x00},
+        {0x5005, 0x00},
+        {0x501d, 0x00},
+        {0x501f, 0x06},
+        {0x5021, 0x00},
+        {0x5022, 0x13},
+        {0x5061, 0xff},
+        {0x5062, 0xff},
+        {0x5063, 0xff},
+        {0x5064, 0xff},
+        {0x5280, 0x00},
+        {0x5282, 0x00},
+        {0x5283, 0x01},
+        {0x5200, 0x00},
+        {0x5201, 0x71},
+        {0x5203, 0x04},
+        {0x5204, 0x00},
+        {0x5205, 0x88},
+        {0x5209, 0x00},
+        {0x520a, 0x80},
+        {0x520b, 0x04},
+        {0x520c, 0x01},
+        {0x5210, 0x10},
+        {0x5211, 0xa0},
+        {0x5292, 0x00},
+        {0x5500, 0x00},
+        {0x5501, 0x00},
+        {0x5502, 0x00},
+        {0x5503, 0x00},
+        {0x5504, 0x00},
+        {0x5505, 0x00},
+        {0x5506, 0x00},
+        {0x5507, 0x00},
+        {0x5508, 0x00},
+        {0x5509, 0x00},
+        {0x550a, 0x00},
+        {0x550b, 0x00},
+        {0x550c, 0x00},
+        {0x550d, 0x00},
+        {0x550e, 0x00},
+        {0x550f, 0x00},
+        {0x5510, 0x04},
+        {0x5511, 0x04},
+        {0x5512, 0x04},
+        {0x5513, 0x04},
+        {0x5514, 0x08},
+        {0x5515, 0x08},
+        {0x5516, 0x08},
+        {0x5517, 0x08},
+        {0x5518, 0x00},
+        {0x5519, 0x00},
+        {0x551a, 0x00},
+        {0x551b, 0x00},
+        {0x551c, 0x00},
+        {0x551d, 0x00},
+        {0x551e, 0x00},
+        {0x551f, 0x00},
+        {0x5520, 0x00},
+        {0x5521, 0x00},
+        {0x5522, 0x00},
+        {0x5523, 0x00},
+        {0x5524, 0x00},
+        {0x5525, 0x00},
+        {0x5526, 0x00},
+        {0x5527, 0x00},
+        {0x5528, 0x00},
+        {0x5529, 0x00},
+        {0x552a, 0x00},
+        {0x552b, 0x00},
+        {0x552c, 0x00},
+        {0x552d, 0x00},
+        {0x552e, 0x00},
+        {0x552f, 0x00},
+        {0x5530, 0x04},
+        {0x5531, 0x04},
+        {0x5532, 0x04},
+        {0x5533, 0x04},
+        {0x5534, 0x08},
+        {0x5535, 0x08},
+        {0x5536, 0x08},
+        {0x5537, 0x08},
+        {0x5538, 0x00},
+        {0x5539, 0x00},
+        {0x553a, 0x00},
+        {0x553b, 0x00},
+        {0x553c, 0x00},
+        {0x553d, 0x00},
+        {0x553e, 0x00},
+        {0x553f, 0x00},
+        {0x5540, 0x00},
+        {0x5541, 0x00},
+        {0x5542, 0x00},
+        {0x5543, 0x00},
+        {0x5544, 0x00},
+        {0x5545, 0x00},
+        {0x5546, 0x00},
+        {0x5547, 0x00},
+        {0x5548, 0x00},
+        {0x5549, 0x00},
+        {0x554a, 0x00},
+        {0x554b, 0x00},
+        {0x554c, 0x00},
+        {0x554d, 0x00},
+        {0x554e, 0x00},
+        {0x554f, 0x00},
+        {0x5550, 0x04},
+        {0x5551, 0x04},
+        {0x5552, 0x04},
+        {0x5553, 0x04},
+        {0x5554, 0x08},
+        {0x5555, 0x08},
+        {0x5556, 0x08},
+        {0x5557, 0x08},
+        {0x5558, 0x00},
+        {0x5559, 0x00},
+        {0x555a, 0x00},
+        {0x555b, 0x00},
+        {0x555c, 0x00},
+        {0x555d, 0x00},
+        {0x555e, 0x00},
+        {0x555f, 0x00},
+        {0x5560, 0x00},
+        {0x5561, 0x00},
+        {0x5562, 0x00},
+        {0x5563, 0x00},
+        {0x5564, 0x00},
+        {0x5565, 0x00},
+        {0x5566, 0x00},
+        {0x5567, 0x00},
+        {0x5568, 0x00},
+        {0x5569, 0x00},
+        {0x556a, 0x00},
+        {0x556b, 0x00},
+        {0x556c, 0x00},
+        {0x556d, 0x00},
+        {0x556e, 0x00},
+        {0x556f, 0x00},
+        {0x5570, 0x04},
+        {0x5571, 0x04},
+        {0x5572, 0x04},
+        {0x5573, 0x04},
+        {0x5574, 0x08},
+        {0x5575, 0x08},
+        {0x5576, 0x08},
+        {0x5577, 0x08},
+        {0x5578, 0x00},
+        {0x5579, 0x00},
+        {0x557a, 0x00},
+        {0x557b, 0x00},
+        {0x557c, 0x00},
+        {0x557d, 0x00},
+        {0x557e, 0x00},
+        {0x557f, 0x00},
+        {0x5580, 0x00},
+        {0x5581, 0x00},
+        {0x5582, 0x00},
+        {0x5583, 0x00},
+        {0x5584, 0x00},
+        {0x5585, 0x00},
+        {0x5586, 0x00},
+        {0x5587, 0x00},
+        {0x5588, 0x00},
+        {0x5589, 0x00},
+        {0x558a, 0x00},
+        {0x558b, 0x00},
+        {0x558c, 0x04},
+        {0x558d, 0x04},
+        {0x558e, 0x04},
+        {0x558f, 0x04},
+        {0x5590, 0x0e},
+        {0x5591, 0x0e},
+        {0x5592, 0x0e},
+        {0x5593, 0x0e},
+        {0x5594, 0x1c},
+        {0x5595, 0x1c},
+        {0x5596, 0x1c},
+        {0x5597, 0x1c},
+        {0x5598, 0x08},
+        {0x5599, 0x08},
+        {0x559a, 0x08},
+        {0x559b, 0x08},
+        {0x559c, 0x00},
+        {0x559d, 0x00},
+        {0x559e, 0x00},
+        {0x559f, 0x00},
+        {0x55a0, 0x00},
+        {0x55a1, 0x00},
+        {0x55a2, 0x00},
+        {0x55a3, 0x00},
+        {0x55a4, 0x00},
+        {0x55a5, 0x00},
+        {0x55a6, 0x00},
+        {0x55a7, 0x00},
+        {0x55a8, 0x00},
+        {0x55a9, 0x00},
+        {0x55aa, 0x00},
+        {0x55ab, 0x00},
+        {0x55ac, 0x04},
+        {0x55ad, 0x04},
+        {0x55ae, 0x04},
+        {0x55af, 0x04},
+        {0x55b0, 0x0e},
+        {0x55b1, 0x0e},
+        {0x55b2, 0x0e},
+        {0x55b3, 0x0e},
+        {0x55b4, 0x1c},
+        {0x55b5, 0x1c},
+        {0x55b6, 0x1c},
+        {0x55b7, 0x1c},
+        {0x55b8, 0x08},
+        {0x55b9, 0x08},
+        {0x55ba, 0x08},
+        {0x55bb, 0x08},
+        {0x55bc, 0x00},
+        {0x55bd, 0x00},
+        {0x55be, 0x00},
+        {0x55bf, 0x00},
+        {0x55c0, 0x00},
+        {0x55c1, 0x00},
+        {0x55c2, 0x00},
+        {0x55c3, 0x00},
+        {0x55c4, 0x00},
+        {0x55c5, 0x00},
+        {0x55c6, 0x00},
+        {0x55c7, 0x00},
+        {0x55c8, 0x00},
+        {0x55c9, 0x00},
+        {0x55ca, 0x00},
+        {0x55cb, 0x00},
+        {0x55cc, 0x04},
+        {0x55cd, 0x04},
+        {0x55ce, 0x04},
+        {0x55cf, 0x04},
+        {0x55d0, 0x0e},
+        {0x55d1, 0x0e},
+        {0x55d2, 0x0e},
+        {0x55d3, 0x0e},
+        {0x55d4, 0x1c},
+        {0x55d5, 0x1c},
+        {0x55d6, 0x1c},
+        {0x55d7, 0x1c},
+        {0x55d8, 0x08},
+        {0x55d9, 0x08},
+        {0x55da, 0x08},
+        {0x55db, 0x08},
+        {0x55dc, 0x00},
+        {0x55dd, 0x00},
+        {0x55de, 0x00},
+        {0x55df, 0x00},
+        {0x55e0, 0x00},
+        {0x55e1, 0x00},
+        {0x55e2, 0x00},
+        {0x55e3, 0x00},
+        {0x55e4, 0x00},
+        {0x55e5, 0x00},
+        {0x55e6, 0x00},
+        {0x55e7, 0x00},
+        {0x55e8, 0x00},
+        {0x55e9, 0x00},
+        {0x55ea, 0x00},
+        {0x55eb, 0x00},
+        {0x55ec, 0x04},
+        {0x55ed, 0x04},
+        {0x55ee, 0x04},
+        {0x55ef, 0x04},
+        {0x55f0, 0x0e},
+        {0x55f1, 0x0e},
+        {0x55f2, 0x0e},
+        {0x55f3, 0x0e},
+        {0x55f4, 0x1c},
+        {0x55f5, 0x1c},
+        {0x55f6, 0x1c},
+        {0x55f7, 0x1c},
+        {0x55f8, 0x08},
+        {0x55f9, 0x08},
+        {0x55fa, 0x08},
+        {0x55fb, 0x08},
+        {0x55fc, 0x00},
+        {0x55fd, 0x00},
+        {0x55fe, 0x00},
+        {0x55ff, 0x00},
+        {0x567b, 0x00},
+        {0x5690, 0x00},
+        {0x5691, 0x00},
+        {0x5692, 0x00},
+        {0x5693, 0x0a},
+        {0x5694, 0xc0},
+        {0x5696, 0x06},
+        {0x5697, 0x0a},
+        {0x5698, 0x00},
+        {0x5699, 0x1c},
+        {0x569a, 0x16},
+        {0x569b, 0x04},
+        {0x569c, 0x00},
+        {0x569d, 0x1c},
+        {0x569e, 0x10},
+        {0x569f, 0x84},
+        {0x56a0, 0x36},
+        {0x56a1, 0x50},
+        {0x56a2, 0x16},
+        {0x56a3, 0x20},
+        {0x56a4, 0x10},
+        {0x56a5, 0xa0},
+        {0x5c80, 0x06},
+        {0x5c81, 0x80},
+        {0x5c82, 0x09},
+        {0x5c83, 0x5f},
+        {0x5d00, 0x01},
+        {0x5d01, 0x00},
+        {0x5d02, 0x01},
+        {0x5d03, 0x00},
+        {0x5d04, 0x02},
+        {0x5d05, 0x00},
+        {0x5d06, 0x02},
+        {0x5d07, 0x00},
+        {0x5d08, 0x70},
+        {0x5d09, 0x74},
+        {0x5d0a, 0x78},
+        {0x5d0b, 0x7c},
+        {0x5d0c, 0x80},
+        {0x5d0d, 0x80},
+        {0x5d0e, 0x7c},
+        {0x5d0f, 0x78},
+        {0x5d10, 0x74},
+        {0x5d11, 0x70},
+        {0x5d12, 0x98},
+        {0x5d13, 0x96},
+        {0x5d14, 0x63},
+        {0x5d15, 0x3a},
+        {0x5d16, 0x38},
+        {0x5d17, 0x39},
+        {0x5d18, 0x37},
+        {0x5d19, 0x53},
+        {0x5d1a, 0x8b},
+        {0x5d1b, 0x9d},
+        {0x5d24, 0x00},
+        {0x5d25, 0x00},
+        {0x5d26, 0x00},
+        {0x5d27, 0x0a},
+        {0x5d28, 0xc0},
+        {0x5d2a, 0x00},
+        {0x5d2b, 0x1c},
+        {0x5d2c, 0x16},
+        {0x5d2d, 0x04},
+        {0x5d2e, 0x00},
+        {0x5d2f, 0x1c},
+        {0x5d30, 0x10},
+        {0x5d31, 0x84},
+        {0x5d32, 0x10},
+        {0x5d34, 0x16},
+        {0x5d35, 0x20},
+        {0x5d36, 0x10},
+        {0x5d37, 0xa0},
+        {0x5d38, 0x5c},
+        {0x5d39, 0x7b},
+        {0x5d1c, 0x00},
+        {0x5d1d, 0x1c},
+        {0x5d1e, 0x00},
+        {0x5d1f, 0x1c},
+        {0x5d20, 0x15},
+        {0x5d21, 0xe0},
+        {0x5d22, 0x10},
+        {0x5d23, 0x40},
+        {0x5d29, 0x40},
+        {0x3008, 0x01},
+        {0x3663, 0x60},
+        {0x3002, 0x01},
+        {0x3c00, 0x3c},
+        {0x3025, 0x03},
+        {0x3668, 0xf0},
+        {0x3400, 0x04},
+        {OV23850_TABLE_END, 0x00}
+};
+
+static ov23850_reg mode_5632x3168_30fps[] = {
+        {0x0316, 0x1e},
+        {0x0317, 0x00},
+        {0x0318, 0x03},
+        {0x031c, 0x01},
+        {0x031d, 0x02},
+        {0x031e, 0x09},
+        {0x2b05, 0x02},
+        {0x2b06, 0x87},
+        {0x2b07, 0x01},
+        {0x2b08, 0xa8},
+        {0x0320, 0x02},
+        {0x3002, 0x00},
+        {0x300f, 0x11},
+        {0x3010, 0x01},
+        {0x3012, 0x41},
+        {0x3016, 0xd2},
+        {0x3018, 0x70},
+        {0x3019, 0xc3},
+        {0x301b, 0x96},
+        {0x3022, 0x0f},
+        {0x3023, 0xb4},
+        {0x3031, 0x91},
+        {0x3034, 0x41},
+        {0x340c, 0xff},
+        {0x3501, 0x0D},
+        {0x3502, 0x28},
+        {0x3503, 0x00},
+        {0x3507, 0x00},
+        {0x3508, 0x00},
+        {0x3509, 0x12},
+        {0x350a, 0x00},
+        {0x350b, 0x80},
+        {0x350f, 0x10},
+        {0x3541, 0x02},
+        {0x3542, 0x00},
+        {0x3543, 0x00},
+        {0x3547, 0x00},
+        {0x3548, 0x00},
+        {0x3549, 0x12},
+        {0x354b, 0x10},
+        {0x354f, 0x10},
+        {0x3601, 0xa4},
+        {0x3603, 0x97},
+        {0x3604, 0x02},
+        {0x3605, 0xf2},
+        {0x3606, 0xe8},
+        {0x3607, 0x11},
+        {0x360a, 0x34},
+        {0x360c, 0x13},
+        {0x3618, 0xcc},
+        {0x3620, 0x50},
+        {0x3621, 0x99},
+        {0x3622, 0x7d},
+        {0x3624, 0x05},
+        {0x362a, 0x25},
+        {0x3650, 0x04},
+        {0x3660, 0xc0},
+        {0x3661, 0x00},
+        {0x3662, 0x00},
+        {0x3664, 0x88},
+        {0x3667, 0x00},
+        {0x366a, 0x5c},
+        {0x366c, 0x80},
+        {0x3700, 0x62},
+        {0x3701, 0x08},
+        {0x3702, 0x10},
+        {0x3703, 0x3e},
+        {0x3704, 0x26},
+        {0x3705, 0x01},
+        {0x3706, 0x3a},
+        {0x3707, 0xc4},
+        {0x3708, 0x3c},
+        {0x3709, 0x1c},
+        {0x370a, 0x23},
+        {0x370b, 0x2c},
+        {0x370c, 0x42},
+        {0x370d, 0xa4},
+        {0x370e, 0x14},
+        {0x370f, 0x0a},
+        {0x3710, 0x15},
+        {0x3711, 0x0a},
+        {0x3712, 0xa2},
+        {0x3713, 0x00},
+        {0x371e, 0x2a},
+        {0x371f, 0x13},
+        {0x3714, 0x00},
+        {0x3717, 0x00},
+        {0x3719, 0x00},
+        {0x371c, 0x04},
+        {0x3720, 0xaa},
+        {0x3721, 0x10},
+        {0x3722, 0x50},
+        {0x3725, 0xf0},
+        {0x3726, 0x22},
+        {0x3727, 0x44},
+        {0x3728, 0x40},
+        {0x3729, 0x00},
+        {0x372b, 0x00},
+        {0x372c, 0x92},
+        {0x372d, 0x0c},
+        {0x372e, 0x22},
+        {0x372f, 0x91},
+        {0x3732, 0x01},
+        {0x3733, 0xd0},
+        {0x3730, 0x01},
+        {0x3731, 0xc8},
+        {0x3744, 0x01},
+        {0x3745, 0x24},
+        {0x3746, 0x00},
+        {0x3747, 0xd0},
+        {0x3748, 0x27},
+        {0x374a, 0x4b},
+        {0x374b, 0x44},
+        {0x3760, 0xd1},
+        {0x3761, 0x52},
+        {0x3762, 0xa4},
+        {0x3763, 0x14},
+        {0x3766, 0x0c},
+        {0x3767, 0x25},
+        {0x3768, 0x0c},
+        {0x3769, 0x24},
+        {0x376a, 0x09},
+        {0x376b, 0x02},
+        {0x376d, 0x01},
+        {0x376e, 0x53},
+        {0x376f, 0x01},
+        {0x378c, 0x08},
+        {0x378d, 0x46},
+        {0x378e, 0x14},
+        {0x378f, 0x02},
+        {0x3790, 0xc4},
+        {0x3792, 0x64},
+        {0x3793, 0x5d},
+        {0x3794, 0x29},
+        {0x3795, 0x4f},
+        {0x3796, 0x43},
+        {0x3797, 0x09},
+        {0x3798, 0x02},
+        {0x3799, 0x33},
+        {0x379a, 0x09},
+        {0x379b, 0x1e},
+        {0x379f, 0x3e},
+        {0x37a0, 0x44},
+        {0x37a1, 0x00},
+        {0x37a2, 0x44},
+        {0x37a3, 0x41},
+        {0x37a4, 0x88},
+        {0x37a5, 0x69},
+        {0x37b0, 0x48},
+        {0x37b1, 0x20},
+        {0x37b2, 0x03},
+        {0x37b3, 0x48},
+        {0x37b4, 0x02},
+        {0x37b5, 0x33},
+        {0x37b6, 0x22},
+        {0x37b8, 0x02},
+        {0x37bc, 0x02},
+        {0x37c0, 0x3b},
+        {0x37c1, 0xc2},
+        {0x37c2, 0x06},
+        {0x37c3, 0x06},
+        {0x37c5, 0x33},
+        {0x37c6, 0x35},
+        {0x37c7, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x14},
+        {0x3802, 0x00},
+        {0x3803, 0x0c},
+        {0x3804, 0x10},
+        {0x3805, 0x8b},
+        {0x3806, 0x0c},
+        {0x3807, 0x43},
+        {0x3808, 0x16},
+        {0x3809, 0x00},
+        {0x380a, 0x0C},
+        {0x380b, 0x60},
+        {0x380c, 0x1B},
+        {0x380d, 0xC4},
+        {0x380e, 0x0D},
+        {0x380f, 0x30},
+        {0x3810, 0x00},
+        {0x3811, 0x03},
+        {0x3813, 0x06},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x04},
+        {0x3834, 0x00},
+        {0x3835, 0x04},
+        {0x3836, 0x18},
+        {0x3837, 0x02},
+        {0x382f, 0x84},
+        {0x383c, 0xc8},
+        {0x383d, 0xff},
+        {0x3842, 0x00},
+        {0x384b, 0x00},
+        {0x3d85, 0x16},
+        {0x3d8c, 0x77},
+        {0x3d8d, 0x10},
+        {0x3f00, 0x52},
+        {0x4000, 0x17},
+        {0x4001, 0x60},
+        {0x4001, 0x60},
+        {0x4008, 0x00},
+        {0x4009, 0x13},
+        {0x400f, 0x00},
+        {0x4011, 0xfb},
+        {0x4017, 0x08},
+        {0x4018, 0x00},
+        {0x401a, 0xce},
+        {0x4019, 0x18},
+        {0x4020, 0x08},
+        {0x4022, 0x08},
+        {0x4024, 0x08},
+        {0x4026, 0x08},
+        {0x4028, 0x08},
+        {0x402a, 0x08},
+        {0x402c, 0x08},
+        {0x402e, 0x08},
+        {0x4030, 0x08},
+        {0x4032, 0x08},
+        {0x4034, 0x08},
+        {0x4036, 0x08},
+        {0x4038, 0x08},
+        {0x403a, 0x08},
+        {0x403c, 0x08},
+        {0x403e, 0x08},
+        {0x405c, 0x3f},
+        {0x4066, 0x04},
+        {0x4051, 0x03},
+        {0x4052, 0x00},
+        {0x4053, 0x80},
+        {0x4054, 0x00},
+        {0x4055, 0x80},
+        {0x4056, 0x00},
+        {0x4057, 0x80},
+        {0x4058, 0x00},
+        {0x4059, 0x80},
+        {0x4202, 0x00},
+        {0x4203, 0x01},
+        {0x430b, 0xff},
+        {0x430d, 0x00},
+        {0x4500, 0x72},
+        {0x4605, 0x00},
+        {0x4640, 0x01},
+        {0x4641, 0x04},
+        {0x4645, 0x00},
+        {0x4800, 0x04},
+        {0x4809, 0x2b},
+        {0x4813, 0x90},
+        {0x4817, 0x04},
+        {0x4833, 0x18},
+        {0x4837, 0x0A},
+        {0x484b, 0x01},
+        {0x4850, 0x5c},
+        {0x4852, 0x27},
+        {0x4856, 0x5c},
+        {0x4857, 0x55},
+        {0x486a, 0xaa},
+        {0x486e, 0x03},
+        {0x486f, 0x55},
+        {0x4875, 0xf0},
+        {0x4b04, 0x80},
+        {0x4b05, 0xb3},
+        {0x4b06, 0x00},
+        {0x4c01, 0xdf},
+        {0x4d00, 0x04},
+        {0x4d01, 0xf0},
+        {0x4d02, 0xb8},
+        {0x4d03, 0xf2},
+        {0x4d04, 0x88},
+        {0x4d05, 0x9d},
+        {0x4e00, 0x04},
+        {0x4e17, 0x04},
+        {0x4e33, 0x18},
+        {0x4e37, 0x11},
+        {0x4e4b, 0x01},
+        {0x4e50, 0x5c},
+        {0x4e52, 0x27},
+        {0x4e56, 0x5c},
+        {0x4e57, 0x55},
+        {0x4e6a, 0xaa},
+        {0x4e6e, 0x03},
+        {0x4e6f, 0x55},
+        {0x4e75, 0xf0},
+        {0x5000, 0x9b},
+        {0x5001, 0x42},
+        {0x5002, 0x10},
+        {0x5003, 0x01},
+        {0x5004, 0x00},
+        {0x5005, 0x00},
+        {0x501d, 0x00},
+        {0x501f, 0x06},
+        {0x5020, 0x03},
+        {0x5021, 0x00},
+        {0x5022, 0x13},
+        {0x5061, 0xff},
+        {0x5062, 0xff},
+        {0x5063, 0xff},
+        {0x5064, 0xff},
+        {0x506f, 0x00},
+        {0x5280, 0x00},
+        {0x5282, 0x00},
+        {0x5283, 0x01},
+        {0x5200, 0x00},
+        {0x5201, 0x71},
+        {0x5203, 0x04},
+        {0x5204, 0x00},
+        {0x5205, 0x88},
+        {0x5209, 0x00},
+        {0x520a, 0x80},
+        {0x520b, 0x04},
+        {0x520c, 0x01},
+        {0x5210, 0x10},
+        {0x5211, 0xa0},
+        {0x5292, 0x04},
+        {0x5500, 0x00},
+        {0x5501, 0x00},
+        {0x5502, 0x00},
+        {0x5503, 0x00},
+        {0x5504, 0x00},
+        {0x5505, 0x00},
+        {0x5506, 0x00},
+        {0x5507, 0x00},
+        {0x5508, 0x10},
+        {0x5509, 0x00},
+        {0x550a, 0x10},
+        {0x550b, 0x00},
+        {0x550c, 0x20},
+        {0x550d, 0x00},
+        {0x550e, 0x20},
+        {0x550f, 0x00},
+        {0x5510, 0x00},
+        {0x5511, 0x00},
+        {0x5512, 0x00},
+        {0x5513, 0x00},
+        {0x5514, 0x00},
+        {0x5515, 0x00},
+        {0x5516, 0x00},
+        {0x5517, 0x00},
+        {0x5518, 0x00},
+        {0x5519, 0x00},
+        {0x551a, 0x00},
+        {0x551b, 0x00},
+        {0x551c, 0x00},
+        {0x551d, 0x00},
+        {0x551e, 0x00},
+        {0x551f, 0x00},
+        {0x5520, 0x00},
+        {0x5521, 0x00},
+        {0x5522, 0x00},
+        {0x5523, 0x00},
+        {0x5524, 0x00},
+        {0x5525, 0x00},
+        {0x5526, 0x00},
+        {0x5527, 0x00},
+        {0x5528, 0x00},
+        {0x5529, 0x10},
+        {0x552a, 0x00},
+        {0x552b, 0x10},
+        {0x552c, 0x00},
+        {0x552d, 0x20},
+        {0x552e, 0x00},
+        {0x552f, 0x20},
+        {0x5530, 0x00},
+        {0x5531, 0x00},
+        {0x5532, 0x00},
+        {0x5533, 0x00},
+        {0x5534, 0x00},
+        {0x5535, 0x00},
+        {0x5536, 0x00},
+        {0x5537, 0x00},
+        {0x5538, 0x00},
+        {0x5539, 0x00},
+        {0x553a, 0x00},
+        {0x553b, 0x00},
+        {0x553c, 0x00},
+        {0x553d, 0x00},
+        {0x553e, 0x00},
+        {0x553f, 0x00},
+        {0x5540, 0x00},
+        {0x5541, 0x00},
+        {0x5542, 0x00},
+        {0x5543, 0x00},
+        {0x5544, 0x00},
+        {0x5545, 0x00},
+        {0x5546, 0x00},
+        {0x5547, 0x00},
+        {0x5548, 0x01},
+        {0x5549, 0x00},
+        {0x554a, 0x01},
+        {0x554b, 0x00},
+        {0x554c, 0x02},
+        {0x554d, 0x00},
+        {0x554e, 0x02},
+        {0x554f, 0x00},
+        {0x5550, 0x00},
+        {0x5551, 0x00},
+        {0x5552, 0x00},
+        {0x5553, 0x00},
+        {0x5554, 0x00},
+        {0x5555, 0x00},
+        {0x5556, 0x00},
+        {0x5557, 0x00},
+        {0x5558, 0x00},
+        {0x5559, 0x00},
+        {0x555a, 0x00},
+        {0x555b, 0x00},
+        {0x555c, 0x00},
+        {0x555d, 0x00},
+        {0x555e, 0x00},
+        {0x555f, 0x00},
+        {0x5560, 0x00},
+        {0x5561, 0x00},
+        {0x5562, 0x00},
+        {0x5563, 0x00},
+        {0x5564, 0x00},
+        {0x5565, 0x00},
+        {0x5566, 0x00},
+        {0x5567, 0x00},
+        {0x5568, 0x00},
+        {0x5569, 0x10},
+        {0x556a, 0x00},
+        {0x556b, 0x10},
+        {0x556c, 0x00},
+        {0x556d, 0x20},
+        {0x556e, 0x00},
+        {0x556f, 0x20},
+        {0x5570, 0x00},
+        {0x5571, 0x00},
+        {0x5572, 0x00},
+        {0x5573, 0x00},
+        {0x5574, 0x00},
+        {0x5575, 0x00},
+        {0x5576, 0x00},
+        {0x5577, 0x00},
+        {0x5578, 0x00},
+        {0x5579, 0x00},
+        {0x557a, 0x00},
+        {0x557b, 0x00},
+        {0x557c, 0x00},
+        {0x557d, 0x00},
+        {0x557e, 0x00},
+        {0x557f, 0x00},
+        {0x5580, 0x00},
+        {0x5581, 0x00},
+        {0x5582, 0x00},
+        {0x5583, 0x00},
+        {0x5584, 0x10},
+        {0x5585, 0x00},
+        {0x5586, 0x10},
+        {0x5587, 0x00},
+        {0x5588, 0x38},
+        {0x5589, 0x00},
+        {0x558a, 0x38},
+        {0x558b, 0x00},
+        {0x558c, 0x70},
+        {0x558d, 0x00},
+        {0x558e, 0x70},
+        {0x558f, 0x00},
+        {0x5590, 0x20},
+        {0x5591, 0x00},
+        {0x5592, 0x20},
+        {0x5593, 0x00},
+        {0x5594, 0x00},
+        {0x5595, 0x00},
+        {0x5596, 0x00},
+        {0x5597, 0x00},
+        {0x5598, 0x00},
+        {0x5599, 0x00},
+        {0x559a, 0x00},
+        {0x559b, 0x00},
+        {0x559c, 0x00},
+        {0x559d, 0x00},
+        {0x559e, 0x00},
+        {0x559f, 0x00},
+        {0x55a0, 0x00},
+        {0x55a1, 0x00},
+        {0x55a2, 0x00},
+        {0x55a3, 0x00},
+        {0x55a4, 0x00},
+        {0x55a5, 0x10},
+        {0x55a6, 0x00},
+        {0x55a7, 0x10},
+        {0x55a8, 0x00},
+        {0x55a9, 0x38},
+        {0x55aa, 0x00},
+        {0x55ab, 0x38},
+        {0x55ac, 0x00},
+        {0x55ad, 0x70},
+        {0x55ae, 0x00},
+        {0x55af, 0x70},
+        {0x55b0, 0x00},
+        {0x55b1, 0x20},
+        {0x55b2, 0x00},
+        {0x55b3, 0x20},
+        {0x55b4, 0x00},
+        {0x55b5, 0x00},
+        {0x55b6, 0x00},
+        {0x55b7, 0x00},
+        {0x55b8, 0x00},
+        {0x55b9, 0x00},
+        {0x55ba, 0x00},
+        {0x55bb, 0x00},
+        {0x55bc, 0x00},
+        {0x55bd, 0x00},
+        {0x55be, 0x00},
+        {0x55bf, 0x00},
+        {0x55c0, 0x00},
+        {0x55c1, 0x00},
+        {0x55c2, 0x00},
+        {0x55c3, 0x00},
+        {0x55c4, 0x01},
+        {0x55c5, 0x00},
+        {0x55c6, 0x01},
+        {0x55c7, 0x00},
+        {0x55c8, 0x03},
+        {0x55c9, 0x80},
+        {0x55ca, 0x03},
+        {0x55cb, 0x80},
+        {0x55cc, 0x07},
+        {0x55cd, 0x00},
+        {0x55ce, 0x07},
+        {0x55cf, 0x00},
+        {0x55d0, 0x02},
+        {0x55d1, 0x00},
+        {0x55d2, 0x02},
+        {0x55d3, 0x00},
+        {0x55d4, 0x00},
+        {0x55d5, 0x00},
+        {0x55d6, 0x00},
+        {0x55d7, 0x00},
+        {0x55d8, 0x00},
+        {0x55d9, 0x00},
+        {0x55da, 0x00},
+        {0x55db, 0x00},
+        {0x55dc, 0x00},
+        {0x55dd, 0x00},
+        {0x55de, 0x00},
+        {0x55df, 0x00},
+        {0x55e0, 0x00},
+        {0x55e1, 0x00},
+        {0x55e2, 0x00},
+        {0x55e3, 0x00},
+        {0x55e4, 0x00},
+        {0x55e5, 0x10},
+        {0x55e6, 0x00},
+        {0x55e7, 0x10},
+        {0x55e8, 0x00},
+        {0x55e9, 0x38},
+        {0x55ea, 0x00},
+        {0x55eb, 0x38},
+        {0x55ec, 0x00},
+        {0x55ed, 0x70},
+        {0x55ee, 0x00},
+        {0x55ef, 0x70},
+        {0x55f0, 0x00},
+        {0x55f1, 0x20},
+        {0x55f2, 0x00},
+        {0x55f3, 0x20},
+        {0x55f4, 0x00},
+        {0x55f5, 0x00},
+        {0x55f6, 0x00},
+        {0x55f7, 0x00},
+        {0x55f8, 0x00},
+        {0x55f9, 0x00},
+        {0x55fa, 0x00},
+        {0x55fb, 0x00},
+        {0x55fc, 0x00},
+        {0x55fd, 0x00},
+        {0x55fe, 0x00},
+        {0x55ff, 0x00},
+        {0x5600, 0x30},
+        {0x5601, 0x00},
+        {0x5602, 0x00},
+        {0x5603, 0x00},
+        {0x5604, 0x00},
+        {0x5605, 0x00},
+        {0x5606, 0x00},
+        {0x5607, 0x01},
+        {0x5608, 0x01},
+        {0x5609, 0x01},
+        {0x560f, 0xfc},
+        {0x5610, 0xf0},
+        {0x5611, 0x10},
+        {0x562f, 0xfc},
+        {0x5630, 0xf0},
+        {0x5631, 0x10},
+        {0x564f, 0xfc},
+        {0x5650, 0xf0},
+        {0x5651, 0x10},
+        {0x566f, 0xfc},
+        {0x5670, 0xf0},
+        {0x5671, 0x10},
+        {0x567b, 0x40},
+        {0x5690, 0x00},
+        {0x5691, 0x00},
+        {0x5692, 0x00},
+        {0x5693, 0x0a},
+        {0x5694, 0x80},
+        {0x5696, 0x06},
+        {0x5697, 0x0a},
+        {0x5698, 0x00},
+        {0x5699, 0x90},
+        {0x569a, 0x15},
+        {0x569b, 0x90},
+        {0x569c, 0x00},
+        {0x569d, 0x50},
+        {0x569e, 0x10},
+        {0x569f, 0x50},
+        {0x56a0, 0x36},
+        {0x56a1, 0x50},
+        {0x56a2, 0x16},
+        {0x56a3, 0x20},
+        {0x56a4, 0x10},
+        {0x56a5, 0xa0},
+        {0x5c80, 0x06},
+        {0x5c81, 0x80},
+        {0x5c82, 0x09},
+        {0x5c83, 0x5f},
+        {0x5d04, 0x01},
+        {0x5d05, 0x1a},
+        {0x5d06, 0x01},
+        {0x5d07, 0x1a},
+        {0x5d12, 0xf7},
+        {0x5d13, 0xdd},
+        {0x5d14, 0x97},
+        {0x5d15, 0x69},
+        {0x5d16, 0x61},
+        {0x5d17, 0x5b},
+        {0x5d18, 0x62},
+        {0x5d19, 0x8d},
+        {0x5d1a, 0xdd},
+        {0x5d1b, 0xff},
+        {0x5d24, 0x00},
+        {0x5d25, 0x00},
+        {0x5d26, 0x00},
+        {0x5d27, 0x0a},
+        {0x5d28, 0x80},
+        {0x5d2a, 0x00},
+        {0x5d2b, 0x90},
+        {0x5d2c, 0x15},
+        {0x5d2d, 0x90},
+        {0x5d2e, 0x00},
+        {0x5d2f, 0x50},
+        {0x5d30, 0x10},
+        {0x5d31, 0x50},
+        {0x5d32, 0x10},
+        {0x5d34, 0x36},
+        {0x5d35, 0x20},
+        {0x5d36, 0x90},
+        {0x5d37, 0xa0},
+        {0x5d38, 0x5c},
+        {0x5d39, 0x7b},
+        {0x5d1c, 0x00},
+        {0x5d1d, 0x90},
+        {0x5d1e, 0x00},
+        {0x5d1f, 0x50},
+        {0x5d20, 0x15},
+        {0x5d21, 0x00},
+        {0x5d22, 0x10},
+        {0x5d23, 0x00},
+        {0x5d29, 0xc0},
+        {0x3008, 0x01},
+        {0x3663, 0x60},
+        {0x3002, 0x01},
+        {0x3c00, 0x3c},
+        {0x3025, 0x03},
+        {0x3668, 0xf0},
+        {0x3400, 0x04},
+        {OV23850_TABLE_END, 0x00}
+};
+
+static ov23850_reg mode_5632x3168_30fps_dpcm[] = {
+        {0x0316, 0x1e},
+        {0x0317, 0x00},
+        {0x0318, 0x03},
+        {0x031c, 0x01},
+        {0x031d, 0x02},
+        {0x031e, 0x08},
+        {0x2b05, 0x02},
+        {0x2b06, 0x87},
+        {0x2b07, 0x01},
+        {0x2b08, 0xa8},
+        {0x0320, 0x02},
+        {0x3002, 0x00},
+        {0x300f, 0x11},
+        {0x3010, 0x01},
+        {0x3012, 0x41},
+        {0x3016, 0xd2},
+        {0x3018, 0x70},
+        {0x3019, 0xe1},
+        {0x301b, 0x96},
+        {0x3022, 0x0f},
+        {0x3023, 0xb4},
+        {0x3031, 0x91},
+        {0x3034, 0x41},
+        {0x340c, 0xff},
+        {0x3501, 0x0D},
+        {0x3502, 0x28},
+        {0x3503, 0x00},
+        {0x3507, 0x00},
+        {0x3508, 0x00},
+        {0x3509, 0x12},
+        {0x350a, 0x00},
+        {0x350b, 0x80},
+        {0x350f, 0x10},
+        {0x3541, 0x02},
+        {0x3542, 0x00},
+        {0x3543, 0x00},
+        {0x3547, 0x00},
+        {0x3548, 0x00},
+        {0x3549, 0x12},
+        {0x354b, 0x10},
+        {0x354f, 0x10},
+        {0x3601, 0xa4},
+        {0x3603, 0x97},
+        {0x3604, 0x02},
+        {0x3605, 0xf2},
+        {0x3606, 0xe8},
+        {0x3607, 0x11},
+        {0x360a, 0x34},
+        {0x360c, 0x13},
+        {0x3618, 0xcc},
+        {0x3620, 0x50},
+        {0x3621, 0x99},
+        {0x3622, 0x7d},
+        {0x3624, 0x05},
+        {0x362a, 0x25},
+        {0x3650, 0x04},
+        {0x3660, 0xc4},
+        {0x3661, 0x00},
+        {0x3662, 0x00},
+        {0x3664, 0x88},
+        {0x3667, 0x00},
+        {0x366a, 0x5c},
+        {0x366c, 0x80},
+        {0x3700, 0x62},
+        {0x3701, 0x08},
+        {0x3702, 0x10},
+        {0x3703, 0x3e},
+        {0x3704, 0x26},
+        {0x3705, 0x01},
+        {0x3706, 0x3a},
+        {0x3707, 0xc4},
+        {0x3708, 0x3c},
+        {0x3709, 0x1c},
+        {0x370a, 0x23},
+        {0x370b, 0x2c},
+        {0x370c, 0x42},
+        {0x370d, 0xa4},
+        {0x370e, 0x14},
+        {0x370f, 0x0a},
+        {0x3710, 0x15},
+        {0x3711, 0x0a},
+        {0x3712, 0xa2},
+        {0x3713, 0x00},
+        {0x371e, 0x2a},
+        {0x371f, 0x13},
+        {0x3714, 0x00},
+        {0x3717, 0x00},
+        {0x3719, 0x00},
+        {0x371c, 0x04},
+        {0x3720, 0xaa},
+        {0x3721, 0x10},
+        {0x3722, 0x50},
+        {0x3725, 0xf0},
+        {0x3726, 0x22},
+        {0x3727, 0x44},
+        {0x3728, 0x40},
+        {0x3729, 0x00},
+        {0x372b, 0x00},
+        {0x372c, 0x92},
+        {0x372d, 0x0c},
+        {0x372e, 0x22},
+        {0x372f, 0x91},
+        {0x3732, 0x01},
+        {0x3733, 0xd0},
+        {0x3730, 0x01},
+        {0x3731, 0xc8},
+        {0x3744, 0x01},
+        {0x3745, 0x24},
+        {0x3746, 0x00},
+        {0x3747, 0xd0},
+        {0x3748, 0x27},
+        {0x374a, 0x4b},
+        {0x374b, 0x44},
+        {0x3760, 0xd1},
+        {0x3761, 0x52},
+        {0x3762, 0xa4},
+        {0x3763, 0x14},
+        {0x3766, 0x0c},
+        {0x3767, 0x25},
+        {0x3768, 0x0c},
+        {0x3769, 0x24},
+        {0x376a, 0x09},
+        {0x376b, 0x02},
+        {0x376d, 0x01},
+        {0x376e, 0x53},
+        {0x376f, 0x01},
+        {0x378c, 0x08},
+        {0x378d, 0x46},
+        {0x378e, 0x14},
+        {0x378f, 0x02},
+        {0x3790, 0xc4},
+        {0x3792, 0x64},
+        {0x3793, 0x5d},
+        {0x3794, 0x29},
+        {0x3795, 0x4f},
+        {0x3796, 0x43},
+        {0x3797, 0x09},
+        {0x3798, 0x02},
+        {0x3799, 0x33},
+        {0x379a, 0x09},
+        {0x379b, 0x1e},
+        {0x379f, 0x3e},
+        {0x37a0, 0x44},
+        {0x37a1, 0x00},
+        {0x37a2, 0x44},
+        {0x37a3, 0x41},
+        {0x37a4, 0x88},
+        {0x37a5, 0x69},
+        {0x37b0, 0x48},
+        {0x37b1, 0x20},
+        {0x37b2, 0x03},
+        {0x37b3, 0x48},
+        {0x37b4, 0x02},
+        {0x37b5, 0x33},
+        {0x37b6, 0x22},
+        {0x37b8, 0x02},
+        {0x37bc, 0x02},
+        {0x37c0, 0x3b},
+        {0x37c1, 0xc2},
+        {0x37c2, 0x06},
+        {0x37c3, 0x06},
+        {0x37c5, 0x33},
+        {0x37c6, 0x35},
+        {0x37c7, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x14},
+        {0x3802, 0x00},
+        {0x3803, 0x0c},
+        {0x3804, 0x10},
+        {0x3805, 0x8b},
+        {0x3806, 0x0c},
+        {0x3807, 0x43},
+        {0x3808, 0x16},
+        {0x3809, 0x00},
+        {0x380a, 0x0C},
+        {0x380b, 0x60},
+        {0x380c, 0x1B},
+        {0x380d, 0xC4},
+        {0x380e, 0x0D},
+        {0x380f, 0x30},
+        {0x3810, 0x00},
+        {0x3811, 0x03},
+        {0x3813, 0x06},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x04},
+        {0x3834, 0x00},
+        {0x3835, 0x04},
+        {0x3836, 0x18},
+        {0x3837, 0x02},
+        {0x382f, 0x84},
+        {0x383c, 0xc8},
+        {0x383d, 0xff},
+        {0x3842, 0x00},
+        {0x384b, 0x00},
+        {0x3d85, 0x16},
+        {0x3d8c, 0x77},
+        {0x3d8d, 0x10},
+        {0x3f00, 0x52},
+        {0x4000, 0x17},
+        {0x4001, 0x60},
+        {0x4001, 0x60},
+        {0x4008, 0x00},
+        {0x4009, 0x13},
+        {0x400f, 0x00},
+        {0x4011, 0xfb},
+        {0x4017, 0x08},
+        {0x4018, 0x00},
+        {0x401a, 0xce},
+        {0x4019, 0x18},
+        {0x4020, 0x08},
+        {0x4022, 0x08},
+        {0x4024, 0x08},
+        {0x4026, 0x08},
+        {0x4028, 0x08},
+        {0x402a, 0x08},
+        {0x402c, 0x08},
+        {0x402e, 0x08},
+        {0x4030, 0x08},
+        {0x4032, 0x08},
+        {0x4034, 0x08},
+        {0x4036, 0x08},
+        {0x4038, 0x08},
+        {0x403a, 0x08},
+        {0x403c, 0x08},
+        {0x403e, 0x08},
+        {0x405c, 0x3f},
+        {0x4066, 0x04},
+        {0x4051, 0x03},
+        {0x4052, 0x00},
+        {0x4053, 0x80},
+        {0x4054, 0x00},
+        {0x4055, 0x80},
+        {0x4056, 0x00},
+        {0x4057, 0x80},
+        {0x4058, 0x00},
+        {0x4059, 0x80},
+        {0x4202, 0x00},
+        {0x4203, 0x01},
+        {0x430b, 0xff},
+        {0x430d, 0x00},
+        {0x4500, 0x72},
+        {0x4605, 0x00},
+        {0x4640, 0x01},
+        {0x4641, 0x04},
+        {0x4645, 0x00},
+        {0x4800, 0x04},
+        {0x4809, 0x2b},
+        {0x4813, 0x90},
+        {0x4817, 0x04},
+        {0x4833, 0x18},
+        {0x4837, 0x0A},
+        {0x484b, 0x01},
+        {0x4850, 0x5c},
+        {0x4852, 0x27},
+        {0x4856, 0x5c},
+        {0x4857, 0x55},
+        {0x486a, 0xaa},
+        {0x486e, 0x03},
+        {0x486f, 0x55},
+        {0x4875, 0xf0},
+        {0x4b04, 0x80},
+        {0x4b05, 0xb3},
+        {0x4b06, 0x00},
+        {0x4c01, 0xdf},
+        {0x4d00, 0x04},
+        {0x4d01, 0xf0},
+        {0x4d02, 0xb8},
+        {0x4d03, 0xf2},
+        {0x4d04, 0x88},
+        {0x4d05, 0x9d},
+        {0x4e00, 0x04},
+        {0x4e17, 0x04},
+        {0x4e33, 0x18},
+        {0x4e37, 0x11},
+        {0x4e4b, 0x01},
+        {0x4e50, 0x5c},
+        {0x4e52, 0x27},
+        {0x4e56, 0x5c},
+        {0x4e57, 0x55},
+        {0x4e6a, 0xaa},
+        {0x4e6e, 0x03},
+        {0x4e6f, 0x55},
+        {0x4e75, 0xf0},
+        {0x5000, 0x9b},
+        {0x5001, 0x42},
+        {0x5002, 0x10},
+        {0x5003, 0x01},
+        {0x5004, 0x00},
+        {0x5005, 0x00},
+        {0x501d, 0x00},
+        {0x501f, 0x06},
+        {0x5020, 0x03},
+        {0x5021, 0x00},
+        {0x5022, 0x13},
+        {0x5061, 0xff},
+        {0x5062, 0xff},
+        {0x5063, 0xff},
+        {0x5064, 0xff},
+        {0x506f, 0x00},
+        {0x5280, 0x00},
+        {0x5282, 0x00},
+        {0x5283, 0x01},
+        {0x5200, 0x00},
+        {0x5201, 0x71},
+        {0x5203, 0x04},
+        {0x5204, 0x00},
+        {0x5205, 0x88},
+        {0x5209, 0x00},
+        {0x520a, 0x80},
+        {0x520b, 0x04},
+        {0x520c, 0x01},
+        {0x5210, 0x10},
+        {0x5211, 0xa0},
+        {0x5292, 0x04},
+        {0x5500, 0x00},
+        {0x5501, 0x00},
+        {0x5502, 0x00},
+        {0x5503, 0x00},
+        {0x5504, 0x00},
+        {0x5505, 0x00},
+        {0x5506, 0x00},
+        {0x5507, 0x00},
+        {0x5508, 0x10},
+        {0x5509, 0x00},
+        {0x550a, 0x10},
+        {0x550b, 0x00},
+        {0x550c, 0x20},
+        {0x550d, 0x00},
+        {0x550e, 0x20},
+        {0x550f, 0x00},
+        {0x5510, 0x00},
+        {0x5511, 0x00},
+        {0x5512, 0x00},
+        {0x5513, 0x00},
+        {0x5514, 0x00},
+        {0x5515, 0x00},
+        {0x5516, 0x00},
+        {0x5517, 0x00},
+        {0x5518, 0x00},
+        {0x5519, 0x00},
+        {0x551a, 0x00},
+        {0x551b, 0x00},
+        {0x551c, 0x00},
+        {0x551d, 0x00},
+        {0x551e, 0x00},
+        {0x551f, 0x00},
+        {0x5520, 0x00},
+        {0x5521, 0x00},
+        {0x5522, 0x00},
+        {0x5523, 0x00},
+        {0x5524, 0x00},
+        {0x5525, 0x00},
+        {0x5526, 0x00},
+        {0x5527, 0x00},
+        {0x5528, 0x00},
+        {0x5529, 0x10},
+        {0x552a, 0x00},
+        {0x552b, 0x10},
+        {0x552c, 0x00},
+        {0x552d, 0x20},
+        {0x552e, 0x00},
+        {0x552f, 0x20},
+        {0x5530, 0x00},
+        {0x5531, 0x00},
+        {0x5532, 0x00},
+        {0x5533, 0x00},
+        {0x5534, 0x00},
+        {0x5535, 0x00},
+        {0x5536, 0x00},
+        {0x5537, 0x00},
+        {0x5538, 0x00},
+        {0x5539, 0x00},
+        {0x553a, 0x00},
+        {0x553b, 0x00},
+        {0x553c, 0x00},
+        {0x553d, 0x00},
+        {0x553e, 0x00},
+        {0x553f, 0x00},
+        {0x5540, 0x00},
+        {0x5541, 0x00},
+        {0x5542, 0x00},
+        {0x5543, 0x00},
+        {0x5544, 0x00},
+        {0x5545, 0x00},
+        {0x5546, 0x00},
+        {0x5547, 0x00},
+        {0x5548, 0x01},
+        {0x5549, 0x00},
+        {0x554a, 0x01},
+        {0x554b, 0x00},
+        {0x554c, 0x02},
+        {0x554d, 0x00},
+        {0x554e, 0x02},
+        {0x554f, 0x00},
+        {0x5550, 0x00},
+        {0x5551, 0x00},
+        {0x5552, 0x00},
+        {0x5553, 0x00},
+        {0x5554, 0x00},
+        {0x5555, 0x00},
+        {0x5556, 0x00},
+        {0x5557, 0x00},
+        {0x5558, 0x00},
+        {0x5559, 0x00},
+        {0x555a, 0x00},
+        {0x555b, 0x00},
+        {0x555c, 0x00},
+        {0x555d, 0x00},
+        {0x555e, 0x00},
+        {0x555f, 0x00},
+        {0x5560, 0x00},
+        {0x5561, 0x00},
+        {0x5562, 0x00},
+        {0x5563, 0x00},
+        {0x5564, 0x00},
+        {0x5565, 0x00},
+        {0x5566, 0x00},
+        {0x5567, 0x00},
+        {0x5568, 0x00},
+        {0x5569, 0x10},
+        {0x556a, 0x00},
+        {0x556b, 0x10},
+        {0x556c, 0x00},
+        {0x556d, 0x20},
+        {0x556e, 0x00},
+        {0x556f, 0x20},
+        {0x5570, 0x00},
+        {0x5571, 0x00},
+        {0x5572, 0x00},
+        {0x5573, 0x00},
+        {0x5574, 0x00},
+        {0x5575, 0x00},
+        {0x5576, 0x00},
+        {0x5577, 0x00},
+        {0x5578, 0x00},
+        {0x5579, 0x00},
+        {0x557a, 0x00},
+        {0x557b, 0x00},
+        {0x557c, 0x00},
+        {0x557d, 0x00},
+        {0x557e, 0x00},
+        {0x557f, 0x00},
+        {0x5580, 0x00},
+        {0x5581, 0x00},
+        {0x5582, 0x00},
+        {0x5583, 0x00},
+        {0x5584, 0x10},
+        {0x5585, 0x00},
+        {0x5586, 0x10},
+        {0x5587, 0x00},
+        {0x5588, 0x38},
+        {0x5589, 0x00},
+        {0x558a, 0x38},
+        {0x558b, 0x00},
+        {0x558c, 0x70},
+        {0x558d, 0x00},
+        {0x558e, 0x70},
+        {0x558f, 0x00},
+        {0x5590, 0x20},
+        {0x5591, 0x00},
+        {0x5592, 0x20},
+        {0x5593, 0x00},
+        {0x5594, 0x00},
+        {0x5595, 0x00},
+        {0x5596, 0x00},
+        {0x5597, 0x00},
+        {0x5598, 0x00},
+        {0x5599, 0x00},
+        {0x559a, 0x00},
+        {0x559b, 0x00},
+        {0x559c, 0x00},
+        {0x559d, 0x00},
+        {0x559e, 0x00},
+        {0x559f, 0x00},
+        {0x55a0, 0x00},
+        {0x55a1, 0x00},
+        {0x55a2, 0x00},
+        {0x55a3, 0x00},
+        {0x55a4, 0x00},
+        {0x55a5, 0x10},
+        {0x55a6, 0x00},
+        {0x55a7, 0x10},
+        {0x55a8, 0x00},
+        {0x55a9, 0x38},
+        {0x55aa, 0x00},
+        {0x55ab, 0x38},
+        {0x55ac, 0x00},
+        {0x55ad, 0x70},
+        {0x55ae, 0x00},
+        {0x55af, 0x70},
+        {0x55b0, 0x00},
+        {0x55b1, 0x20},
+        {0x55b2, 0x00},
+        {0x55b3, 0x20},
+        {0x55b4, 0x00},
+        {0x55b5, 0x00},
+        {0x55b6, 0x00},
+        {0x55b7, 0x00},
+        {0x55b8, 0x00},
+        {0x55b9, 0x00},
+        {0x55ba, 0x00},
+        {0x55bb, 0x00},
+        {0x55bc, 0x00},
+        {0x55bd, 0x00},
+        {0x55be, 0x00},
+        {0x55bf, 0x00},
+        {0x55c0, 0x00},
+        {0x55c1, 0x00},
+        {0x55c2, 0x00},
+        {0x55c3, 0x00},
+        {0x55c4, 0x01},
+        {0x55c5, 0x00},
+        {0x55c6, 0x01},
+        {0x55c7, 0x00},
+        {0x55c8, 0x03},
+        {0x55c9, 0x80},
+        {0x55ca, 0x03},
+        {0x55cb, 0x80},
+        {0x55cc, 0x07},
+        {0x55cd, 0x00},
+        {0x55ce, 0x07},
+        {0x55cf, 0x00},
+        {0x55d0, 0x02},
+        {0x55d1, 0x00},
+        {0x55d2, 0x02},
+        {0x55d3, 0x00},
+        {0x55d4, 0x00},
+        {0x55d5, 0x00},
+        {0x55d6, 0x00},
+        {0x55d7, 0x00},
+        {0x55d8, 0x00},
+        {0x55d9, 0x00},
+        {0x55da, 0x00},
+        {0x55db, 0x00},
+        {0x55dc, 0x00},
+        {0x55dd, 0x00},
+        {0x55de, 0x00},
+        {0x55df, 0x00},
+        {0x55e0, 0x00},
+        {0x55e1, 0x00},
+        {0x55e2, 0x00},
+        {0x55e3, 0x00},
+        {0x55e4, 0x00},
+        {0x55e5, 0x10},
+        {0x55e6, 0x00},
+        {0x55e7, 0x10},
+        {0x55e8, 0x00},
+        {0x55e9, 0x38},
+        {0x55ea, 0x00},
+        {0x55eb, 0x38},
+        {0x55ec, 0x00},
+        {0x55ed, 0x70},
+        {0x55ee, 0x00},
+        {0x55ef, 0x70},
+        {0x55f0, 0x00},
+        {0x55f1, 0x20},
+        {0x55f2, 0x00},
+        {0x55f3, 0x20},
+        {0x55f4, 0x00},
+        {0x55f5, 0x00},
+        {0x55f6, 0x00},
+        {0x55f7, 0x00},
+        {0x55f8, 0x00},
+        {0x55f9, 0x00},
+        {0x55fa, 0x00},
+        {0x55fb, 0x00},
+        {0x55fc, 0x00},
+        {0x55fd, 0x00},
+        {0x55fe, 0x00},
+        {0x55ff, 0x00},
+        {0x5600, 0x30},
+        {0x5601, 0x00},
+        {0x5602, 0x00},
+        {0x5603, 0x00},
+        {0x5604, 0x00},
+        {0x5605, 0x00},
+        {0x5606, 0x00},
+        {0x5607, 0x01},
+        {0x5608, 0x01},
+        {0x5609, 0x01},
+        {0x560f, 0xfc},
+        {0x5610, 0xf0},
+        {0x5611, 0x10},
+        {0x562f, 0xfc},
+        {0x5630, 0xf0},
+        {0x5631, 0x10},
+        {0x564f, 0xfc},
+        {0x5650, 0xf0},
+        {0x5651, 0x10},
+        {0x566f, 0xfc},
+        {0x5670, 0xf0},
+        {0x5671, 0x10},
+        {0x567b, 0x40},
+        {0x5690, 0x00},
+        {0x5691, 0x00},
+        {0x5692, 0x00},
+        {0x5693, 0x0a},
+        {0x5694, 0x80},
+        {0x5696, 0x06},
+        {0x5697, 0x0a},
+        {0x5698, 0x00},
+        {0x5699, 0x90},
+        {0x569a, 0x15},
+        {0x569b, 0x90},
+        {0x569c, 0x00},
+        {0x569d, 0x50},
+        {0x569e, 0x10},
+        {0x569f, 0x50},
+        {0x56a0, 0x36},
+        {0x56a1, 0x50},
+        {0x56a2, 0x16},
+        {0x56a3, 0x20},
+        {0x56a4, 0x10},
+        {0x56a5, 0xa0},
+        {0x5c80, 0x06},
+        {0x5c81, 0x80},
+        {0x5c82, 0x09},
+        {0x5c83, 0x5f},
+        {0x5d04, 0x01},
+        {0x5d05, 0x1a},
+        {0x5d06, 0x01},
+        {0x5d07, 0x1a},
+        {0x5d12, 0xf7},
+        {0x5d13, 0xdd},
+        {0x5d14, 0x97},
+        {0x5d15, 0x69},
+        {0x5d16, 0x61},
+        {0x5d17, 0x5b},
+        {0x5d18, 0x62},
+        {0x5d19, 0x8d},
+        {0x5d1a, 0xdd},
+        {0x5d1b, 0xff},
+        {0x5d24, 0x00},
+        {0x5d25, 0x00},
+        {0x5d26, 0x00},
+        {0x5d27, 0x0a},
+        {0x5d28, 0x80},
+        {0x5d2a, 0x00},
+        {0x5d2b, 0x90},
+        {0x5d2c, 0x15},
+        {0x5d2d, 0x90},
+        {0x5d2e, 0x00},
+        {0x5d2f, 0x50},
+        {0x5d30, 0x10},
+        {0x5d31, 0x50},
+        {0x5d32, 0x10},
+        {0x5d34, 0x36},
+        {0x5d35, 0x20},
+        {0x5d36, 0x90},
+        {0x5d37, 0xa0},
+        {0x5d38, 0x5c},
+        {0x5d39, 0x7b},
+        {0x5d1c, 0x00},
+        {0x5d1d, 0x90},
+        {0x5d1e, 0x00},
+        {0x5d1f, 0x50},
+        {0x5d20, 0x15},
+        {0x5d21, 0x00},
+        {0x5d22, 0x10},
+        {0x5d23, 0x00},
+        {0x5d29, 0xc0},
+        {0x3008, 0x01},
+        {0x3663, 0x60},
+        {0x3002, 0x01},
+        {0x3c00, 0x3c},
+        {0x3025, 0x03},
+        {0x3668, 0xf0},
+        {0x3400, 0x04},
+        {OV23850_TABLE_END, 0x00}
+};
+
+static ov23850_reg mode_table_common[] = {
+        {0x301e, 0x00},
+        {OV23850_TABLE_WAIT_MS, OV23850_WAIT_MS},
+        {0x0103, 0x01},
+        {0x0300, 0x00},
+        {0x0301, 0x6C},
+        {0x0302, 0x10},
+        {0x0303, 0x00},
+        {0x0304, 0x3A},
+        {0x030d, 0x2d},
+        {0x030e, 0x00},
+        {0x030f, 0x05},
+        {0x0312, 0x02},
+        {0x0313, 0x11},
+        {OV23850_TABLE_END, 0x00},
+};
+
+enum {
+        OV23850_MODE_5632X4224,
+        OV23850_MODE_5632X4224_8FPS,
+        OV23850_MODE_5632X4224_8FPS_DPCM,
+        OV23850_MODE_5632X4224_24FPS_DPCM,
+        OV23850_MODE_5632X3168,
+        OV23850_MODE_5632X3168_30FPS_DPCM,
+
+        OV23850_MODE_COMMON,
+        OV23850_MODE_START_STREAM,
+        OV23850_MODE_STOP_STREAM,
+        OV23850_MODE_TEST_PATTERN,
+};
+
+static ov23850_reg *mode_table[] = {
+        [OV23850_MODE_5632X4224] = mode_5632x4224_20fps,
+        [OV23850_MODE_5632X4224_8FPS] = mode_5632x4224_8fps,
+        [OV23850_MODE_5632X4224_8FPS_DPCM] = mode_5632x4224_8fps_dpcm,
+        [OV23850_MODE_5632X4224_24FPS_DPCM] = mode_5632x4224_24fps_dpcm,
+        [OV23850_MODE_5632X3168] = mode_5632x3168_30fps,
+        [OV23850_MODE_5632X3168_30FPS_DPCM] = mode_5632x3168_30fps_dpcm,
+
+        [OV23850_MODE_COMMON] = mode_table_common,
+        [OV23850_MODE_START_STREAM] = ov23850_start,
+        [OV23850_MODE_STOP_STREAM] = ov23850_stop,
+        [OV23850_MODE_TEST_PATTERN] = tp_colorbars,
+};
+
+static const int ov23850_20fps[] = {
+        20,
+};
+
+static const int ov23850_30fps[] = {
+        30,
+};
+
+static const struct camera_common_frmfmt ov23850_frmfmt[] = {
+        {{5632, 4224}, ov23850_20fps, 1, 0, OV23850_MODE_5632X4224},
+        {{5632, 3168}, ov23850_30fps, 1, 0, OV23850_MODE_5632X3168},
+};
+#endif  /* __OV23850_I2C_TABLES__ */
diff --git a/drivers/media/i2c/ov5693.c b/drivers/media/i2c/ov5693.c
new file mode 100644
index 000000000..048fcfa6a
--- /dev/null
+++ b/drivers/media/i2c/ov5693.c
@@ -0,0 +1,1507 @@
+/*
+ * ov5693_v4l2.c - ov5693 sensor driver
+ *
+ * Copyright (c) 2013-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+
+#include <linux/seq_file.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <media/camera_common.h>
+#include <media/ov5693.h>
+
+#include "../platform/tegra/camera/camera_gpio.h"
+
+#include "ov5693_mode_tbls.h"
+
+#define OV5693_MAX_COARSE_DIFF          6
+
+#define OV5693_GAIN_SHIFT               8
+#define OV5693_REAL_GAIN_SHIFT          4
+#define OV5693_MIN_GAIN         (1 << OV5693_GAIN_SHIFT)
+#define OV5693_MAX_GAIN         (16 << OV5693_GAIN_SHIFT)
+#define OV5693_MAX_UNREAL_GAIN  (0x0F80)
+#define OV5693_MIN_FRAME_LENGTH (0x0)
+#define OV5693_MAX_FRAME_LENGTH (0x7fff)
+#define OV5693_MIN_EXPOSURE_COARSE      (0x0002)
+#define OV5693_MAX_EXPOSURE_COARSE      \
+        (OV5693_MAX_FRAME_LENGTH-OV5693_MAX_COARSE_DIFF)
+#define OV5693_DEFAULT_LINE_LENGTH      (0xA80)
+#define OV5693_DEFAULT_PIXEL_CLOCK      (160)
+
+#define OV5693_DEFAULT_GAIN             OV5693_MIN_GAIN
+#define OV5693_DEFAULT_FRAME_LENGTH     (0x07C0)
+#define OV5693_DEFAULT_EXPOSURE_COARSE  \
+        (OV5693_DEFAULT_FRAME_LENGTH-OV5693_MAX_COARSE_DIFF)
+
+#define OV5693_DEFAULT_MODE     OV5693_MODE_2592X1944
+#define OV5693_DEFAULT_HDR_MODE OV5693_MODE_2592X1944_HDR
+#define OV5693_DEFAULT_WIDTH    2592
+#define OV5693_DEFAULT_HEIGHT   1944
+#define OV5693_DEFAULT_DATAFMT  MEDIA_BUS_FMT_SRGGB10_1X10
+#define OV5693_DEFAULT_CLK_FREQ 24000000
+
+struct ov5693 {
+        struct camera_common_power_rail power;
+        int                             numctrls;
+        struct v4l2_ctrl_handler        ctrl_handler;
+        struct camera_common_eeprom_data eeprom[OV5693_EEPROM_NUM_BLOCKS];
+        u8                              eeprom_buf[OV5693_EEPROM_SIZE];
+        struct i2c_client               *i2c_client;
+        struct v4l2_subdev              *subdev;
+        struct media_pad                pad;
+
+        int                             reg_offset;
+
+        s32                             group_hold_prev;
+        u32                             frame_length;
+        bool                            group_hold_en;
+        struct regmap                   *regmap;
+        struct camera_common_data       *s_data;
+        struct camera_common_pdata      *pdata;
+        struct v4l2_ctrl                *ctrls[];
+};
+
+static struct regmap_config ov5693_regmap_config = {
+        .reg_bits = 16,
+        .val_bits = 8,
+};
+
+static int ov5693_g_volatile_ctrl(struct v4l2_ctrl *ctrl);
+static int ov5693_s_ctrl(struct v4l2_ctrl *ctrl);
+static void ov5693_update_ctrl_range(struct ov5693 *priv, s32 frame_length);
+
+static const struct v4l2_ctrl_ops ov5693_ctrl_ops = {
+        .g_volatile_ctrl = ov5693_g_volatile_ctrl,
+        .s_ctrl         = ov5693_s_ctrl,
+};
+
+static struct v4l2_ctrl_config ctrl_config_list[] = {
+/* Do not change the name field for the controls! */
+        {
+                .ops = &ov5693_ctrl_ops,
+                .id = V4L2_CID_GAIN,
+                .name = "Gain",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV5693_MIN_GAIN,
+                .max = OV5693_MAX_GAIN,
+                .def = OV5693_DEFAULT_GAIN,
+                .step = 1,
+        },
+        {
+                .ops = &ov5693_ctrl_ops,
+                .id = V4L2_CID_FRAME_LENGTH,
+                .name = "Frame Length",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV5693_MIN_FRAME_LENGTH,
+                .max = OV5693_MAX_FRAME_LENGTH,
+                .def = OV5693_DEFAULT_FRAME_LENGTH,
+                .step = 1,
+        },
+        {
+                .ops = &ov5693_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME,
+                .name = "Coarse Time",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV5693_MIN_EXPOSURE_COARSE,
+                .max = OV5693_MAX_EXPOSURE_COARSE,
+                .def = OV5693_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &ov5693_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME_SHORT,
+                .name = "Coarse Time Short",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV5693_MIN_EXPOSURE_COARSE,
+                .max = OV5693_MAX_EXPOSURE_COARSE,
+                .def = OV5693_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &ov5693_ctrl_ops,
+                .id = V4L2_CID_GROUP_HOLD,
+                .name = "Group Hold",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &ov5693_ctrl_ops,
+                .id = V4L2_CID_HDR_EN,
+                .name = "HDR enable",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &ov5693_ctrl_ops,
+                .id = V4L2_CID_EEPROM_DATA,
+                .name = "EEPROM Data",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_VOLATILE,
+                .min = 0,
+                .max = OV5693_EEPROM_STR_SIZE,
+                .step = 2,
+        },
+        {
+                .ops = &ov5693_ctrl_ops,
+                .id = V4L2_CID_OTP_DATA,
+                .name = "OTP Data",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = OV5693_OTP_STR_SIZE,
+                .step = 2,
+        },
+        {
+                .ops = &ov5693_ctrl_ops,
+                .id = V4L2_CID_FUSE_ID,
+                .name = "Fuse ID",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = OV5693_FUSE_ID_STR_SIZE,
+                .step = 2,
+        },
+};
+
+static inline void ov5693_get_frame_length_regs(ov5693_reg *regs,
+                                u32 frame_length)
+{
+        regs->addr = OV5693_FRAME_LENGTH_ADDR_MSB;
+        regs->val = (frame_length >> 8) & 0xff;
+        (regs + 1)->addr = OV5693_FRAME_LENGTH_ADDR_LSB;
+        (regs + 1)->val = (frame_length) & 0xff;
+}
+
+
+static inline void ov5693_get_coarse_time_regs(ov5693_reg *regs,
+                                u32 coarse_time)
+{
+        regs->addr = OV5693_COARSE_TIME_ADDR_1;
+        regs->val = (coarse_time >> 12) & 0xff;
+        (regs + 1)->addr = OV5693_COARSE_TIME_ADDR_2;
+        (regs + 1)->val = (coarse_time >> 4) & 0xff;
+        (regs + 2)->addr = OV5693_COARSE_TIME_ADDR_3;
+        (regs + 2)->val = (coarse_time & 0xf) << 4;
+}
+
+static inline void ov5693_get_coarse_time_short_regs(ov5693_reg *regs,
+                                u32 coarse_time)
+{
+        regs->addr = OV5693_COARSE_TIME_SHORT_ADDR_1;
+        regs->val = (coarse_time >> 12) & 0xff;
+        (regs + 1)->addr = OV5693_COARSE_TIME_SHORT_ADDR_2;
+        (regs + 1)->val = (coarse_time >> 4) & 0xff;
+        (regs + 2)->addr = OV5693_COARSE_TIME_SHORT_ADDR_3;
+        (regs + 2)->val = (coarse_time & 0xf) << 4;
+}
+
+static inline void ov5693_get_gain_regs(ov5693_reg *regs,
+                                u16 gain)
+{
+        regs->addr = OV5693_GAIN_ADDR_MSB;
+        regs->val = (gain >> 8) & 0xff;
+
+        (regs + 1)->addr = OV5693_GAIN_ADDR_LSB;
+        (regs + 1)->val = (gain) & 0xff;
+}
+
+static int test_mode;
+module_param(test_mode, int, 0644);
+
+static inline int ov5693_read_reg(struct camera_common_data *s_data,
+                                u16 addr, u8 *val)
+{
+        struct ov5693 *priv = (struct ov5693 *)s_data->priv;
+
+        return regmap_read(priv->regmap, addr, (unsigned int *) val);
+}
+
+static int ov5693_write_reg(struct camera_common_data *s_data, u16 addr, u8 val)
+{
+        int err;
+        struct ov5693 *priv = (struct ov5693 *)s_data->priv;
+
+        err = regmap_write(priv->regmap, addr, val);
+        if (err)
+                pr_err("%s:i2c write failed, %x = %x\n",
+                        __func__, addr, val);
+
+        return err;
+}
+
+static int ov5693_write_table(struct ov5693 *priv,
+                              const ov5693_reg table[])
+{
+        return regmap_util_write_table_8(priv->regmap,
+                                         table,
+                                         NULL, 0,
+                                         OV5693_TABLE_WAIT_MS,
+                                         OV5693_TABLE_END);
+}
+
+static void ov5693_gpio_set(struct ov5693 *priv,
+                            unsigned int gpio, int val)
+{
+        if (priv->pdata && priv->pdata->use_cam_gpio)
+                cam_gpio_ctrl(priv->i2c_client, gpio, val, 1);
+        else {
+                if (gpio_cansleep(gpio))
+                        gpio_set_value_cansleep(gpio, val);
+                else
+                        gpio_set_value(gpio, val);
+        }
+}
+
+static int ov5693_power_on(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct ov5693 *priv = (struct ov5693 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power on\n", __func__);
+
+        if (priv->pdata && priv->pdata->power_on) {
+                err = priv->pdata->power_on(pw);
+                if (err)
+                        pr_err("%s failed.\n", __func__);
+                else
+                        pw->state = SWITCH_ON;
+                return err;
+        }
+
+        /* sleeps calls in the sequence below are for internal device
+         * signal propagation as specified by sensor vendor */
+
+        if (pw->avdd)
+                err = regulator_enable(pw->avdd);
+        if (err)
+                goto ov5693_avdd_fail;
+
+        if (pw->iovdd)
+                err = regulator_enable(pw->iovdd);
+        if (err)
+                goto ov5693_iovdd_fail;
+
+        usleep_range(1, 2);
+        if (pw->pwdn_gpio)
+                ov5693_gpio_set(priv, pw->pwdn_gpio, 1);
+
+        /*
+         * datasheet 2.9: reset requires ~2ms settling time
+         * a power on reset is generated after core power becomes stable
+         */
+        usleep_range(2000, 2010);
+
+        if (pw->reset_gpio)
+                ov5693_gpio_set(priv, pw->reset_gpio, 1);
+
+        /* datasheet fig 2-9: t3 */
+        usleep_range(2000, 2010);
+
+        pw->state = SWITCH_ON;
+        return 0;
+
+ov5693_iovdd_fail:
+        regulator_disable(pw->avdd);
+
+ov5693_avdd_fail:
+        pr_err("%s failed.\n", __func__);
+        return -ENODEV;
+}
+
+static int ov5693_power_off(struct camera_common_data *s_data)
+{
+        int err = 0;
+        struct ov5693 *priv = (struct ov5693 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power off\n", __func__);
+
+        if (priv->pdata && priv->pdata->power_off) {
+                err = priv->pdata->power_off(pw);
+                if (!err) {
+                        goto power_off_done;
+                } else {
+                        pr_err("%s failed.\n", __func__);
+                        return err;
+                }
+        }
+
+        /* sleeps calls in the sequence below are for internal device
+         * signal propagation as specified by sensor vendor */
+
+        usleep_range(21, 25);
+        if (pw->pwdn_gpio)
+                ov5693_gpio_set(priv, pw->pwdn_gpio, 0);
+        usleep_range(1, 2);
+        if (pw->reset_gpio)
+                ov5693_gpio_set(priv, pw->reset_gpio, 0);
+
+        /* datasheet 2.9: reset requires ~2ms settling time*/
+        usleep_range(2000, 2010);
+
+        if (pw->iovdd)
+                regulator_disable(pw->iovdd);
+        if (pw->avdd)
+                regulator_disable(pw->avdd);
+
+power_off_done:
+        pw->state = SWITCH_OFF;
+        return 0;
+}
+
+static int ov5693_power_put(struct ov5693 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+
+        if (unlikely(!pw))
+                return -EFAULT;
+
+        if (likely(pw->avdd))
+                regulator_put(pw->avdd);
+
+        if (likely(pw->iovdd))
+                regulator_put(pw->iovdd);
+
+        pw->avdd = NULL;
+        pw->iovdd = NULL;
+
+        if (priv->pdata && priv->pdata->use_cam_gpio)
+                cam_gpio_deregister(priv->i2c_client, pw->pwdn_gpio);
+        else {
+                gpio_free(pw->pwdn_gpio);
+                gpio_free(pw->reset_gpio);
+        }
+
+        return 0;
+}
+
+static int ov5693_power_get(struct ov5693 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        struct camera_common_pdata *pdata = priv->pdata;
+        const char *mclk_name;
+        const char *parentclk_name;
+        struct clk *parent;
+        int err = 0, ret = 0;
+
+        if (!pdata) {
+                dev_err(&priv->i2c_client->dev, "pdata missing\n");
+                return -EFAULT;
+        }
+
+        mclk_name = pdata->mclk_name ?
+                    pdata->mclk_name : "cam_mclk1";
+        pw->mclk = devm_clk_get(&priv->i2c_client->dev, mclk_name);
+        if (IS_ERR(pw->mclk)) {
+                dev_err(&priv->i2c_client->dev,
+                        "unable to get clock %s\n", mclk_name);
+                return PTR_ERR(pw->mclk);
+        }
+
+        parentclk_name = pdata->parentclk_name;
+        if (parentclk_name) {
+                parent = devm_clk_get(&priv->i2c_client->dev, parentclk_name);
+                if (IS_ERR(parent)) {
+                        dev_err(&priv->i2c_client->dev,
+                                "unable to get parent clcok %s",
+                                parentclk_name);
+                } else
+                        clk_set_parent(pw->mclk, parent);
+        }
+
+
+        /* analog 2.8v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->avdd, pdata->regulators.avdd);
+        /* IO 1.8v */
+        err |= camera_common_regulator_get(priv->i2c_client,
+                        &pw->iovdd, pdata->regulators.iovdd);
+
+        if (!err) {
+                pw->reset_gpio = pdata->reset_gpio;
+                pw->pwdn_gpio = pdata->pwdn_gpio;
+        }
+
+        if (pdata->use_cam_gpio) {
+                err = cam_gpio_register(priv->i2c_client, pw->pwdn_gpio);
+                if (err)
+                        dev_err(&priv->i2c_client->dev,
+                                "%s ERR can't register cam gpio %u!\n",
+                                 __func__, pw->pwdn_gpio);
+        } else {
+                ret = gpio_request(pw->pwdn_gpio, "cam_pwdn_gpio");
+                if (ret < 0)
+                        dev_dbg(&priv->i2c_client->dev,
+                                "%s can't request pwdn_gpio %d\n",
+                                __func__, ret);
+                ret = gpio_request(pw->reset_gpio, "cam_reset_gpio");
+                if (ret < 0)
+                        dev_dbg(&priv->i2c_client->dev,
+                                "%s can't request reset_gpio %d\n",
+                                __func__, ret);
+        }
+
+
+        pw->state = SWITCH_OFF;
+        return err;
+}
+
+static int ov5693_set_gain(struct ov5693 *priv, s32 val);
+static int ov5693_set_frame_length(struct ov5693 *priv, s32 val);
+static int ov5693_set_coarse_time(struct ov5693 *priv, s32 val);
+static int ov5693_set_coarse_time_short(struct ov5693 *priv, s32 val);
+
+static int ov5693_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov5693 *priv = (struct ov5693 *)s_data->priv;
+        struct v4l2_control control;
+        int err;
+        u32 frame_time;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        if (!enable) {
+                ov5693_update_ctrl_range(priv, OV5693_MAX_FRAME_LENGTH);
+
+                err = ov5693_write_table(priv,
+                        mode_table[OV5693_MODE_STOP_STREAM]);
+                if (err)
+                        return err;
+
+                /*
+                 * Wait for one frame to make sure sensor is set to
+                 * software standby in V-blank
+                 *
+                 * frame_time = frame length rows * Tline
+                 * Tline = line length / pixel clock (in MHz)
+                 */
+                frame_time = priv->frame_length *
+                        OV5693_DEFAULT_LINE_LENGTH / OV5693_DEFAULT_PIXEL_CLOCK;
+
+                usleep_range(frame_time, frame_time + 1000);
+                return 0;
+        }
+
+        err = ov5693_write_table(priv, mode_table[s_data->mode]);
+        if (err)
+                goto exit;
+
+
+        if (s_data->override_enable) {
+                /*
+                 * write list of override regs for the asking frame length,
+                 * coarse integration time, and gain. Failures to write
+                 * overrides are non-fatal
+                 */
+                control.id = V4L2_CID_GAIN;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov5693_set_gain(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev, "%s: warning gain override failed\n",
+                                __func__);
+
+                control.id = V4L2_CID_FRAME_LENGTH;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov5693_set_frame_length(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: warning frame length override failed\n",
+                                __func__);
+
+                control.id = V4L2_CID_COARSE_TIME;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov5693_set_coarse_time(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: warning coarse time override failed\n",
+                                __func__);
+
+                control.id = V4L2_CID_COARSE_TIME_SHORT;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov5693_set_coarse_time_short(priv, control.value);
+                if (err)
+                        dev_dbg(&client->dev,
+                                "%s: warning coarse time short override failed\n",
+                                __func__);
+        }
+
+        err = ov5693_write_table(priv, mode_table[OV5693_MODE_START_STREAM]);
+        if (err)
+                goto exit;
+
+        if (test_mode)
+                err = ov5693_write_table(priv,
+                        mode_table[OV5693_MODE_TEST_PATTERN]);
+
+        dev_dbg(&client->dev, "%s--\n", __func__);
+        return 0;
+exit:
+        dev_dbg(&client->dev, "%s: error setting stream\n", __func__);
+        return err;
+}
+
+static int ov5693_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov5693 *priv = (struct ov5693 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        *status = pw->state == SWITCH_ON;
+        return 0;
+}
+
+static struct v4l2_subdev_video_ops ov5693_subdev_video_ops = {
+        .s_stream       = ov5693_s_stream,
+        .g_mbus_config  = camera_common_g_mbus_config,
+        .g_input_status = ov5693_g_input_status,
+};
+
+static struct v4l2_subdev_core_ops ov5693_subdev_core_ops = {
+        .s_power        = camera_common_s_power,
+};
+
+static int ov5693_get_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        return camera_common_g_fmt(sd, &format->format);
+}
+
+static int ov5693_set_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        int ret;
+
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+                ret = camera_common_try_fmt(sd, &format->format);
+        else
+                ret = camera_common_s_fmt(sd, &format->format);
+
+        return ret;
+}
+
+static struct v4l2_subdev_pad_ops ov5693_subdev_pad_ops = {
+        .set_fmt = ov5693_set_fmt,
+        .get_fmt = ov5693_get_fmt,
+        .enum_mbus_code = camera_common_enum_mbus_code,
+        .enum_frame_size        = camera_common_enum_framesizes,
+        .enum_frame_interval    = camera_common_enum_frameintervals,
+};
+
+static struct v4l2_subdev_ops ov5693_subdev_ops = {
+        .core   = &ov5693_subdev_core_ops,
+        .video  = &ov5693_subdev_video_ops,
+        .pad    = &ov5693_subdev_pad_ops,
+};
+
+static struct of_device_id ov5693_of_match[] = {
+        { .compatible = "nvidia,ov5693", },
+        { },
+};
+
+static struct camera_common_sensor_ops ov5693_common_ops = {
+        .power_on = ov5693_power_on,
+        .power_off = ov5693_power_off,
+        .write_reg = ov5693_write_reg,
+        .read_reg = ov5693_read_reg,
+};
+
+static int ov5693_set_group_hold(struct ov5693 *priv)
+{
+        int err;
+        int gh_prev = switch_ctrl_qmenu[priv->group_hold_prev];
+
+        if (priv->group_hold_en == true && gh_prev == SWITCH_OFF) {
+                /* enter group hold */
+                err = ov5693_write_reg(priv->s_data,
+                                       OV5693_GROUP_HOLD_ADDR, 0x01);
+                if (err)
+                        goto fail;
+
+                priv->group_hold_prev = 1;
+
+                dev_dbg(&priv->i2c_client->dev,
+                         "%s: enter group hold\n", __func__);
+        } else if (priv->group_hold_en == false && gh_prev == SWITCH_ON) {
+                /* leave group hold */
+                err = ov5693_write_reg(priv->s_data,
+                                       OV5693_GROUP_HOLD_ADDR, 0x11);
+                if (err)
+                        goto fail;
+
+                err = ov5693_write_reg(priv->s_data,
+                                       OV5693_GROUP_HOLD_ADDR, 0x61);
+                if (err)
+                        goto fail;
+
+                priv->group_hold_prev = 0;
+
+                dev_dbg(&priv->i2c_client->dev,
+                         "%s: leave group hold\n", __func__);
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: Group hold control error\n", __func__);
+        return err;
+}
+
+static u16 ov5693_to_real_gain(u32 rep, int shift)
+{
+        u16 gain;
+        int gain_int;
+        int gain_dec;
+        int min_int = (1 << shift);
+
+        if (rep < OV5693_MIN_GAIN)
+                rep = OV5693_MIN_GAIN;
+        else if (rep > OV5693_MAX_GAIN)
+                rep = OV5693_MAX_GAIN;
+
+        gain_int = (int)(rep >> shift);
+        gain_dec = (int)(rep & ~(0xffff << shift));
+
+        /* derived from formulat gain = (x * 16 + 0.5) */
+        gain = ((gain_int * min_int + gain_dec) * 32 + min_int) / (2 * min_int);
+
+        return gain;
+}
+
+static int ov5693_set_gain(struct ov5693 *priv, s32 val)
+{
+        ov5693_reg reg_list[2];
+        int err;
+        u16 gain;
+        int i;
+
+        if (!priv->group_hold_prev)
+                ov5693_set_group_hold(priv);
+
+        /* translate value */
+        gain = ov5693_to_real_gain((u32)val, OV5693_GAIN_SHIFT);
+
+        ov5693_get_gain_regs(reg_list, gain);
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: gain %04x val: %04x\n", __func__, val, gain);
+
+        for (i = 0; i < 2; i++) {
+                err = ov5693_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: GAIN control error\n", __func__);
+        return err;
+}
+
+static void ov5693_update_ctrl_range(struct ov5693 *priv, s32 frame_length)
+{
+        struct v4l2_ctrl *ctrl = NULL;
+        int ctrl_ids[2] = {V4L2_CID_COARSE_TIME,
+                        V4L2_CID_COARSE_TIME_SHORT};
+        s32 max, min, def;
+        int i, j;
+
+        for (i = 0; i < ARRAY_SIZE(ctrl_ids); i++) {
+                for (j = 0; j < priv->numctrls; j++) {
+                        if (priv->ctrls[j]->id == ctrl_ids[i]) {
+                                ctrl = priv->ctrls[j];
+                                break;
+                        }
+                }
+
+                if (j == priv->numctrls) {
+                        dev_err(&priv->i2c_client->dev,
+                                "could not find ctrl %x\n",
+                                ctrl_ids[i]);
+                        continue;
+                }
+
+                max = frame_length - OV5693_MAX_COARSE_DIFF;
+                /* clamp the value in case above is negative */
+                max = clamp_val(max, OV5693_MIN_EXPOSURE_COARSE,
+                        OV5693_MAX_EXPOSURE_COARSE);
+                min = OV5693_MIN_EXPOSURE_COARSE;
+                def = clamp_val(OV5693_DEFAULT_EXPOSURE_COARSE, min, max);
+                if (__v4l2_ctrl_modify_range(ctrl, min, max, 1, def))
+                        dev_err(&priv->i2c_client->dev,
+                                "ctrl %x: range update failed\n",
+                                ctrl_ids[i]);
+        }
+
+}
+
+static int ov5693_set_frame_length(struct ov5693 *priv, s32 val)
+{
+        ov5693_reg reg_list[2];
+        int err;
+        u32 frame_length;
+        int i;
+
+        if (!priv->group_hold_prev)
+                ov5693_set_group_hold(priv);
+
+        frame_length = (u32)val;
+
+        ov5693_get_frame_length_regs(reg_list, frame_length);
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, frame_length);
+
+        for (i = 0; i < 2; i++) {
+                err = ov5693_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        priv->frame_length = frame_length;
+
+        ov5693_update_ctrl_range(priv, val);
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: FRAME_LENGTH control error\n", __func__);
+        return err;
+}
+
+static int ov5693_set_coarse_time(struct ov5693 *priv, s32 val)
+{
+        ov5693_reg reg_list[3];
+        int err;
+        u32 coarse_time;
+        int i;
+
+        if (!priv->group_hold_prev)
+                ov5693_set_group_hold(priv);
+
+        coarse_time = (u32)val;
+
+        ov5693_get_coarse_time_regs(reg_list, coarse_time);
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, coarse_time);
+
+        for (i = 0; i < 3; i++) {
+                err = ov5693_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: COARSE_TIME control error\n", __func__);
+        return err;
+}
+
+static int ov5693_set_coarse_time_short(struct ov5693 *priv, s32 val)
+{
+        ov5693_reg reg_list[3];
+        int err;
+        struct v4l2_control hdr_control;
+        int hdr_en;
+        u32 coarse_time_short;
+        int i;
+
+        if (!priv->group_hold_prev)
+                ov5693_set_group_hold(priv);
+
+        /* check hdr enable ctrl */
+        hdr_control.id = V4L2_CID_HDR_EN;
+
+        err = camera_common_g_ctrl(priv->s_data, &hdr_control);
+        if (err < 0) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return err;
+        }
+
+        hdr_en = switch_ctrl_qmenu[hdr_control.value];
+        if (hdr_en == SWITCH_OFF)
+                return 0;
+
+        coarse_time_short = (u32)val;
+
+        ov5693_get_coarse_time_short_regs(reg_list, coarse_time_short);
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: val: %d\n", __func__, coarse_time_short);
+
+        for (i = 0; i < 3; i++) {
+                err = ov5693_write_reg(priv->s_data, reg_list[i].addr,
+                         reg_list[i].val);
+                if (err)
+                        goto fail;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: COARSE_TIME_SHORT control error\n", __func__);
+        return err;
+}
+
+static int ov5693_eeprom_device_release(struct ov5693 *priv)
+{
+        int i;
+
+        for (i = 0; i < OV5693_EEPROM_NUM_BLOCKS; i++) {
+                if (priv->eeprom[i].i2c_client != NULL) {
+                        i2c_unregister_device(priv->eeprom[i].i2c_client);
+                        priv->eeprom[i].i2c_client = NULL;
+                }
+        }
+
+        return 0;
+}
+
+static int ov5693_eeprom_device_init(struct ov5693 *priv)
+{
+        char *dev_name = "eeprom_ov5693";
+        static struct regmap_config eeprom_regmap_config = {
+                .reg_bits = 8,
+                .val_bits = 8,
+        };
+        int i;
+        int err;
+
+        if (!priv->pdata->has_eeprom)
+                return -EINVAL;
+
+        for (i = 0; i < OV5693_EEPROM_NUM_BLOCKS; i++) {
+                priv->eeprom[i].adap = i2c_get_adapter(
+                                priv->i2c_client->adapter->nr);
+                memset(&priv->eeprom[i].brd, 0, sizeof(priv->eeprom[i].brd));
+                strncpy(priv->eeprom[i].brd.type, dev_name,
+                                sizeof(priv->eeprom[i].brd.type));
+                priv->eeprom[i].brd.addr = OV5693_EEPROM_ADDRESS + i;
+                priv->eeprom[i].i2c_client = i2c_new_device(
+                                priv->eeprom[i].adap, &priv->eeprom[i].brd);
+
+                priv->eeprom[i].regmap = devm_regmap_init_i2c(
+                        priv->eeprom[i].i2c_client, &eeprom_regmap_config);
+                if (IS_ERR(priv->eeprom[i].regmap)) {
+                        err = PTR_ERR(priv->eeprom[i].regmap);
+                        ov5693_eeprom_device_release(priv);
+                        return err;
+                }
+        }
+
+        return 0;
+}
+
+static int ov5693_read_eeprom(struct ov5693 *priv,
+                                struct v4l2_ctrl *ctrl)
+{
+        int err, i;
+
+        for (i = 0; i < OV5693_EEPROM_NUM_BLOCKS; i++) {
+                err = regmap_bulk_read(priv->eeprom[i].regmap, 0,
+                        &priv->eeprom_buf[i * OV5693_EEPROM_BLOCK_SIZE],
+                        OV5693_EEPROM_BLOCK_SIZE);
+                if (err)
+                        return err;
+        }
+
+        for (i = 0; i < OV5693_EEPROM_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x",
+                        priv->eeprom_buf[i]);
+        return 0;
+}
+
+static int ov5693_write_eeprom(struct ov5693 *priv,
+                                char *string)
+{
+        int err;
+        int i;
+        u8 curr[3];
+        unsigned long data;
+
+        for (i = 0; i < OV5693_EEPROM_SIZE; i++) {
+                curr[0] = string[i*2];
+                curr[1] = string[i*2+1];
+                curr[2] = '\0';
+
+                err = kstrtol(curr, 16, &data);
+                if (err) {
+                        dev_err(&priv->i2c_client->dev,
+                                "invalid eeprom string\n");
+                        return -EINVAL;
+                }
+
+                priv->eeprom_buf[i] = (u8)data;
+                err = regmap_write(priv->eeprom[i >> 8].regmap,
+                                   i & 0xFF, (u8)data);
+                if (err)
+                        return err;
+                msleep(20);
+        }
+        return 0;
+}
+
+static int ov5693_read_otp_bank(struct ov5693 *priv,
+                                u8 *buf, int bank, u16 addr, int size)
+{
+        int err;
+
+        /* sleeps calls in the sequence below are for internal device
+         * signal propagation as specified by sensor vendor */
+
+        usleep_range(10000, 11000);
+        err = ov5693_write_table(priv, mode_table[OV5693_MODE_START_STREAM]);
+        if (err)
+                return err;
+
+        err = ov5693_write_reg(priv->s_data, OV5693_OTP_BANK_SELECT_ADDR,
+                               0xC0 | bank);
+        if (err)
+                return err;
+
+        err = ov5693_write_reg(priv->s_data, OV5693_OTP_LOAD_CTRL_ADDR, 0x01);
+        if (err)
+                return err;
+
+        usleep_range(10000, 11000);
+        err = regmap_bulk_read(priv->regmap, addr, buf, size);
+        if (err)
+                return err;
+
+        err = ov5693_write_table(priv,
+                        mode_table[OV5693_MODE_STOP_STREAM]);
+        if (err)
+                return err;
+
+        return 0;
+}
+
+static int ov5693_otp_setup(struct ov5693 *priv)
+{
+        int err;
+        int i;
+        struct v4l2_ctrl *ctrl;
+        u8 otp_buf[OV5693_OTP_SIZE];
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        for (i = 0; i < OV5693_OTP_NUM_BANKS; i++) {
+                err = ov5693_read_otp_bank(priv,
+                                        &otp_buf[i * OV5693_OTP_BANK_SIZE],
+                                        i,
+                                        OV5693_OTP_BANK_START_ADDR,
+                                        OV5693_OTP_BANK_SIZE);
+                if (err) {
+                        dev_err(&priv->i2c_client->dev,
+                                "could not read otp bank\n");
+                        goto ret;
+                }
+        }
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_OTP_DATA);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                err = -EINVAL;
+                goto ret;
+        }
+
+        for (i = 0; i < OV5693_OTP_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x",
+                        otp_buf[i]);
+        ctrl->p_cur.p_char = ctrl->p_new.p_char;
+
+ret:
+        camera_common_s_power(priv->subdev, false);
+
+        return err;
+}
+
+static int ov5693_fuse_id_setup(struct ov5693 *priv)
+{
+        int err;
+        int i;
+        struct v4l2_ctrl *ctrl;
+        u8 fuse_id[OV5693_FUSE_ID_SIZE];
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        err = ov5693_read_otp_bank(priv,
+                                &fuse_id[0],
+                                OV5693_FUSE_ID_OTP_BANK,
+                                OV5693_FUSE_ID_OTP_START_ADDR,
+                                OV5693_FUSE_ID_SIZE);
+        if (err) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not read otp bank\n");
+                goto ret;
+        }
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_FUSE_ID);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                err = -EINVAL;
+                goto ret;
+        }
+
+        for (i = 0; i < OV5693_FUSE_ID_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x",
+                        fuse_id[i]);
+        ctrl->p_cur.p_char = ctrl->p_new.p_char;
+
+ret:
+        camera_common_s_power(priv->subdev, false);
+
+        return err;
+}
+
+static int ov5693_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct ov5693 *priv =
+                container_of(ctrl->handler, struct ov5693, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_EEPROM_DATA:
+                err = ov5693_read_eeprom(priv, ctrl);
+                if (err)
+                        return err;
+                break;
+        default:
+                        pr_err("%s: unknown ctrl id.\n", __func__);
+                        return -EINVAL;
+        }
+
+        return err;
+}
+
+static int ov5693_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct ov5693 *priv =
+                container_of(ctrl->handler, struct ov5693, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_GAIN:
+                err = ov5693_set_gain(priv, ctrl->val);
+                break;
+        case V4L2_CID_FRAME_LENGTH:
+                err = ov5693_set_frame_length(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME:
+                err = ov5693_set_coarse_time(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME_SHORT:
+                err = ov5693_set_coarse_time_short(priv, ctrl->val);
+                break;
+        case V4L2_CID_GROUP_HOLD:
+                if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON) {
+                        priv->group_hold_en = true;
+                } else {
+                        priv->group_hold_en = false;
+                        err = ov5693_set_group_hold(priv);
+                }
+                break;
+        case V4L2_CID_EEPROM_DATA:
+                if (!ctrl->p_new.p_char[0])
+                        break;
+                err = ov5693_write_eeprom(priv, ctrl->p_new.p_char);
+                if (err)
+                        return err;
+                break;
+        case V4L2_CID_HDR_EN:
+                break;
+        default:
+                pr_err("%s: unknown ctrl id.\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static int ov5693_ctrls_init(struct ov5693 *priv, bool eeprom_ctrl)
+{
+        struct i2c_client *client = priv->i2c_client;
+        struct camera_common_data *common_data = priv->s_data;
+        struct v4l2_ctrl *ctrl;
+        int numctrls;
+        int err;
+        int i;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        numctrls = ARRAY_SIZE(ctrl_config_list);
+        v4l2_ctrl_handler_init(&priv->ctrl_handler, numctrls);
+
+        for (i = 0; i < numctrls; i++) {
+                /* Skip control 'V4L2_CID_EEPROM_DATA' if eeprom inint err */
+                if (ctrl_config_list[i].id == V4L2_CID_EEPROM_DATA) {
+                        if (!eeprom_ctrl) {
+                                common_data->numctrls -= 1;
+                                continue;
+                        }
+                }
+
+                ctrl = v4l2_ctrl_new_custom(&priv->ctrl_handler,
+                        &ctrl_config_list[i], NULL);
+                if (ctrl == NULL) {
+                        dev_err(&client->dev, "Failed to init %s ctrl\n",
+                                ctrl_config_list[i].name);
+                        continue;
+                }
+
+                if (ctrl_config_list[i].type == V4L2_CTRL_TYPE_STRING &&
+                        ctrl_config_list[i].flags & V4L2_CTRL_FLAG_READ_ONLY) {
+                        ctrl->p_new.p_char = devm_kzalloc(&client->dev,
+                                ctrl_config_list[i].max + 1, GFP_KERNEL);
+                        if (!ctrl->p_new.p_char)
+                                return -ENOMEM;
+                }
+                priv->ctrls[i] = ctrl;
+        }
+
+        priv->numctrls = numctrls;
+        priv->subdev->ctrl_handler = &priv->ctrl_handler;
+        if (priv->ctrl_handler.error) {
+                dev_err(&client->dev, "Error %d adding controls\n",
+                        priv->ctrl_handler.error);
+                err = priv->ctrl_handler.error;
+                goto error;
+        }
+
+        err = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d setting default controls\n", err);
+                goto error;
+        }
+
+        err = ov5693_otp_setup(priv);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d reading otp data\n", err);
+                goto error;
+        }
+
+        err = ov5693_fuse_id_setup(priv);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d reading fuse id data\n", err);
+                goto error;
+        }
+
+        return 0;
+
+error:
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        return err;
+}
+
+MODULE_DEVICE_TABLE(of, ov5693_of_match);
+
+static struct camera_common_pdata *ov5693_parse_dt(struct i2c_client *client)
+{
+        struct device_node *node = client->dev.of_node;
+        struct camera_common_pdata *board_priv_pdata;
+        const struct of_device_id *match;
+        int gpio;
+        int err;
+        struct camera_common_pdata *ret = NULL;
+
+        if (!node)
+                return NULL;
+
+        match = of_match_device(ov5693_of_match, &client->dev);
+        if (!match) {
+                dev_err(&client->dev, "Failed to find matching dt id\n");
+                return NULL;
+        }
+
+        board_priv_pdata = devm_kzalloc(&client->dev,
+                           sizeof(*board_priv_pdata), GFP_KERNEL);
+        if (!board_priv_pdata)
+                return NULL;
+
+        err = camera_common_parse_clocks(client, board_priv_pdata);
+        if (err) {
+                dev_err(&client->dev, "Failed to find clocks\n");
+                goto error;
+        }
+
+        gpio = of_get_named_gpio(node, "pwdn-gpios", 0);
+        if (gpio < 0) {
+                if (gpio == -EPROBE_DEFER) {
+                        ret = ERR_PTR(-EPROBE_DEFER);
+                        goto error;
+                }
+                dev_err(&client->dev, "pwdn gpios not in DT\n");
+                goto error;
+        }
+        board_priv_pdata->pwdn_gpio = (unsigned int)gpio;
+
+        gpio = of_get_named_gpio(node, "reset-gpios", 0);
+        if (gpio < 0) {
+                /* reset-gpio is not absolutely needed */
+                if (gpio == -EPROBE_DEFER) {
+                        ret = ERR_PTR(-EPROBE_DEFER);
+                        goto error;
+                }
+                dev_dbg(&client->dev, "reset gpios not in DT\n");
+                gpio = 0;
+        }
+        board_priv_pdata->reset_gpio = (unsigned int)gpio;
+
+        board_priv_pdata->use_cam_gpio =
+                of_property_read_bool(node, "cam,use-cam-gpio");
+
+        err = of_property_read_string(node, "avdd-reg",
+                        &board_priv_pdata->regulators.avdd);
+        if (err) {
+                dev_err(&client->dev, "avdd-reg not in DT\n");
+                goto error;
+        }
+        err = of_property_read_string(node, "iovdd-reg",
+                        &board_priv_pdata->regulators.iovdd);
+        if (err) {
+                dev_err(&client->dev, "iovdd-reg not in DT\n");
+                goto error;
+        }
+
+        board_priv_pdata->has_eeprom =
+                of_property_read_bool(node, "has-eeprom");
+
+        return board_priv_pdata;
+
+error:
+        devm_kfree(&client->dev, board_priv_pdata);
+        return ret;
+}
+
+static int ov5693_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        dev_dbg(&client->dev, "%s:\n", __func__);
+        return 0;
+}
+
+static const struct v4l2_subdev_internal_ops ov5693_subdev_internal_ops = {
+        .open = ov5693_open,
+};
+
+static const struct media_entity_operations ov5693_media_ops = {
+        .link_validate = v4l2_subdev_link_validate,
+};
+
+static int ov5693_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct camera_common_data *common_data;
+        struct device_node *node = client->dev.of_node;
+        struct ov5693 *priv;
+        char debugfs_name[10];
+        int err;
+
+        pr_info("[OV5693]: probing v4l2 sensor.\n");
+
+        if (!IS_ENABLED(CONFIG_OF) || !node)
+                return -EINVAL;
+
+        common_data = devm_kzalloc(&client->dev,
+                            sizeof(struct camera_common_data), GFP_KERNEL);
+        if (!common_data)
+                return -ENOMEM;
+
+        priv = devm_kzalloc(&client->dev,
+                            sizeof(struct ov5693) + sizeof(struct v4l2_ctrl *) *
+                            ARRAY_SIZE(ctrl_config_list),
+                            GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
+
+        priv->regmap = devm_regmap_init_i2c(client, &ov5693_regmap_config);
+        if (IS_ERR(priv->regmap)) {
+                dev_err(&client->dev,
+                        "regmap init failed: %ld\n", PTR_ERR(priv->regmap));
+                return -ENODEV;
+        }
+
+        priv->pdata = ov5693_parse_dt(client);
+        if (PTR_ERR(priv->pdata) == -EPROBE_DEFER)
+                return -EPROBE_DEFER;
+        if (!priv->pdata) {
+                dev_err(&client->dev, "unable to get platform data\n");
+                return -EFAULT;
+        }
+
+        common_data->ops                = &ov5693_common_ops;
+        common_data->ctrl_handler       = &priv->ctrl_handler;
+        common_data->i2c_client         = client;
+        common_data->frmfmt             = ov5693_frmfmt;
+        common_data->colorfmt           = camera_common_find_datafmt(
+                                          OV5693_DEFAULT_DATAFMT);
+        common_data->power              = &priv->power;
+        common_data->ctrls              = priv->ctrls;
+        common_data->priv               = (void *)priv;
+        common_data->numctrls           = ARRAY_SIZE(ctrl_config_list);
+        common_data->numfmts            = ARRAY_SIZE(ov5693_frmfmt);
+        common_data->def_mode           = OV5693_DEFAULT_MODE;
+        common_data->def_width          = OV5693_DEFAULT_WIDTH;
+        common_data->def_height         = OV5693_DEFAULT_HEIGHT;
+        common_data->fmt_width          = common_data->def_width;
+        common_data->fmt_height         = common_data->def_height;
+        common_data->def_clk_freq       = OV5693_DEFAULT_CLK_FREQ;
+
+        priv->i2c_client = client;
+        priv->s_data                    = common_data;
+        priv->subdev                    = &common_data->subdev;
+        priv->subdev->dev               = &client->dev;
+        priv->s_data->dev               = &client->dev;
+
+        err = ov5693_power_get(priv);
+        if (err)
+                return err;
+
+        err = camera_common_parse_ports(client, common_data);
+        if (err) {
+                dev_err(&client->dev, "Failed to find port info\n");
+                return err;
+        }
+        sprintf(debugfs_name, "ov5693_%c", common_data->csi_port + 'a');
+        dev_dbg(&client->dev, "%s: name %s\n", __func__, debugfs_name);
+        camera_common_create_debugfs(common_data, debugfs_name);
+
+        v4l2_i2c_subdev_init(priv->subdev, client, &ov5693_subdev_ops);
+
+        /* eeprom interface */
+        err = ov5693_eeprom_device_init(priv);
+        if (err && priv->pdata->has_eeprom)
+                dev_err(&client->dev,
+                        "Failed to allocate eeprom reg map: %d\n", err);
+
+        err = ov5693_ctrls_init(priv, !err);
+        if (err)
+                return err;
+
+        priv->subdev->internal_ops = &ov5693_subdev_internal_ops;
+        priv->subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+                               V4L2_SUBDEV_FL_HAS_EVENTS;
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+        priv->subdev->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+        priv->subdev->entity.ops = &ov5693_media_ops;
+        err = media_entity_init(&priv->subdev->entity, 1, &priv->pad, 0);
+        if (err < 0) {
+                dev_err(&client->dev, "unable to init media entity\n");
+                return err;
+        }
+#endif
+
+        err = v4l2_async_register_subdev(priv->subdev);
+        if (err)
+                return err;
+
+        dev_dbg(&client->dev, "Detected OV5693 sensor\n");
+
+
+        return 0;
+}
+
+static int
+ov5693_remove(struct i2c_client *client)
+{
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov5693 *priv = (struct ov5693 *)s_data->priv;
+
+        v4l2_async_unregister_subdev(priv->subdev);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev->entity);
+#endif
+
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        ov5693_power_put(priv);
+        camera_common_remove_debugfs(s_data);
+
+        return 0;
+}
+
+static const struct i2c_device_id ov5693_id[] = {
+        { "ov5693", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, ov5693_id);
+
+static struct i2c_driver ov5693_i2c_driver = {
+        .driver = {
+                .name = "ov5693",
+                .owner = THIS_MODULE,
+                .of_match_table = of_match_ptr(ov5693_of_match),
+        },
+        .probe = ov5693_probe,
+        .remove = ov5693_remove,
+        .id_table = ov5693_id,
+};
+
+module_i2c_driver(ov5693_i2c_driver);
+
+MODULE_DESCRIPTION("SoC Camera driver for Sony OV5693");
+MODULE_AUTHOR("David Wang <davidw@nvidia.com>");
+MODULE_LICENSE("GPL v2");
+
diff --git a/drivers/media/i2c/ov5693_mode_tbls.h b/drivers/media/i2c/ov5693_mode_tbls.h
new file mode 100644
index 000000000..1639a366e
--- /dev/null
+++ b/drivers/media/i2c/ov5693_mode_tbls.h
@@ -0,0 +1,2135 @@
+/*
+ * ov5693_mode_tbls.h - ov5693 sensor mode tables
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION, All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __OV5693_TABLES__
+#define __OV5693_TABLES__
+
+#include <media/camera_common.h>
+
+#define OV5693_TABLE_WAIT_MS    0
+#define OV5693_TABLE_END        1
+#define OV5693_MAX_RETRIES      3
+#define OV5693_WAIT_MS          10
+
+#define ov5693_reg struct reg_8
+
+static const ov5693_reg ov5693_start[] = {
+        {0x0100, 0x01}, /* mode select streaming on */
+        {OV5693_TABLE_END, 0x00}
+};
+
+static const ov5693_reg ov5693_stop[] = {
+        {0x0100, 0x00}, /* mode select streaming on */
+        {OV5693_TABLE_END, 0x00}
+};
+
+static const ov5693_reg tp_colorbars[] = {
+        {0x0600, 0x00},
+        {0x0601, 0x02},
+
+        {OV5693_TABLE_WAIT_MS, OV5693_WAIT_MS},
+        {OV5693_TABLE_END, 0x00}
+};
+
+static const ov5693_reg mode_2592x1944[] = {
+        {OV5693_TABLE_WAIT_MS, OV5693_WAIT_MS},
+        {0x0100, 0x00},/* Including sw reset */
+        {0x3001, 0x0a},
+        {0x3002, 0x80},
+        {0x3006, 0x00},
+        {0x3011, 0x21},
+        {0x3012, 0x09},
+        {0x3013, 0x10},
+        {0x3014, 0x00},
+        {0x3015, 0x08},
+        {0x3016, 0xf0},
+        {0x3017, 0xf0},
+        {0x3018, 0xf0},
+        {0x301b, 0xb4},
+        {0x301d, 0x02},
+        {0x3021, 0x00},
+        {0x3022, 0x01},
+        {0x3028, 0x44},
+        {0x3090, 0x02},
+        {0x3091, 0x0e},
+        {0x3092, 0x00},
+        {0x3093, 0x00},
+        {0x3098, 0x03},
+        {0x3099, 0x1e},
+        {0x309a, 0x02},
+        {0x309b, 0x01},
+        {0x309c, 0x00},
+        {0x30a0, 0xd2},
+        {0x30a2, 0x01},
+        {0x30b2, 0x00},
+        {0x30b3, 0x68},
+        {0x30b4, 0x03},
+        {0x30b5, 0x04},
+        {0x30b6, 0x01},
+        {0x3104, 0x21},
+        {0x3106, 0x00},
+        {0x3406, 0x01},
+        {0x3500, 0x00},
+        {0x3501, 0x7b},
+        {0x3502, 0x00},
+        {0x3503, 0x07},
+        {0x3504, 0x00},
+        {0x3505, 0x00},
+        {0x3506, 0x00},
+        {0x3507, 0x02},
+        {0x3508, 0x00},
+        {0x3509, 0x10},
+        {0x350a, 0x00},
+        {0x350b, 0x40},
+        {0x3601, 0x0a},
+        {0x3602, 0x18},
+        {0x3612, 0x80},
+        {0x3620, 0x54},
+        {0x3621, 0xc7},
+        {0x3622, 0x0f},
+        {0x3625, 0x10},
+        {0x3630, 0x55},
+        {0x3631, 0xf4},
+        {0x3632, 0x00},
+        {0x3633, 0x34},
+        {0x3634, 0x02},
+        {0x364d, 0x0d},
+        {0x364f, 0xdd},
+        {0x3660, 0x04},
+        {0x3662, 0x10},
+        {0x3663, 0xf1},
+        {0x3665, 0x00},
+        {0x3666, 0x20},
+        {0x3667, 0x00},
+        {0x366a, 0x80},
+        {0x3680, 0xe0},
+        {0x3681, 0x00},
+        {0x3700, 0x42},
+        {0x3701, 0x14},
+        {0x3702, 0xa0},
+        {0x3703, 0xd8},
+        {0x3704, 0x78},
+        {0x3705, 0x02},
+        {0x3708, 0xe2},
+        {0x3709, 0xc3},
+        {0x370a, 0x00},
+        {0x370b, 0x20},
+        {0x370c, 0x0c},
+        {0x370d, 0x11},
+        {0x370e, 0x00},
+        {0x370f, 0x40},
+        {0x3710, 0x00},
+        {0x371a, 0x1c},
+        {0x371b, 0x05},
+        {0x371c, 0x01},
+        {0x371e, 0xa1},
+        {0x371f, 0x0c},
+        {0x3721, 0x00},
+        {0x3724, 0x10},
+        {0x3726, 0x00},
+        {0x372a, 0x01},
+        {0x3730, 0x10},
+        {0x3738, 0x22},
+        {0x3739, 0xe5},
+        {0x373a, 0x50},
+        {0x373b, 0x02},
+        {0x373c, 0x41},
+        {0x373f, 0x02},
+        {0x3740, 0x42},
+        {0x3741, 0x02},
+        {0x3742, 0x18},
+        {0x3743, 0x01},
+        {0x3744, 0x02},
+        {0x3747, 0x10},
+        {0x374c, 0x04},
+        {0x3751, 0xf0},
+        {0x3752, 0x00},
+        {0x3753, 0x00},
+        {0x3754, 0xc0},
+        {0x3755, 0x00},
+        {0x3756, 0x1a},
+        {0x3758, 0x00},
+        {0x3759, 0x0f},
+        {0x376b, 0x44},
+        {0x375c, 0x04},
+        {0x3776, 0x00},
+        {0x377f, 0x08},
+        {0x3780, 0x22},
+        {0x3781, 0x0c},
+        {0x3784, 0x2c},
+        {0x3785, 0x1e},
+        {0x378f, 0xf5},
+        {0x3791, 0xb0},
+        {0x3795, 0x00},
+        {0x3796, 0x64},
+        {0x3797, 0x11},
+        {0x3798, 0x30},
+        {0x3799, 0x41},
+        {0x379a, 0x07},
+        {0x379b, 0xb0},
+        {0x379c, 0x0c},
+        {0x37c5, 0x00},
+        {0x37c6, 0x00},
+        {0x37c7, 0x00},
+        {0x37c9, 0x00},
+        {0x37ca, 0x00},
+        {0x37cb, 0x00},
+        {0x37de, 0x00},
+        {0x37df, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x02},
+        {0x3802, 0x00},
+        {0x3803, 0x02},
+        {0x3804, 0x0a},
+        {0x3805, 0x41},
+        {0x3806, 0x07},
+        {0x3807, 0xa5},
+        {0x3808, 0x0a},
+        {0x3809, 0x20},
+        {0x380a, 0x07},
+        {0x380b, 0x98},
+        {0x380c, 0x0a},
+        {0x380d, 0x80},
+        {0x380e, 0x07},
+        {0x380f, 0xc0},
+        {0x3810, 0x00},
+        {0x3811, 0x02},
+        {0x3812, 0x00},
+        {0x3813, 0x02},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x1e},
+        {0x3823, 0x00},
+        {0x3824, 0x00},
+        {0x3825, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x382a, 0x04},
+        {0x3a04, 0x06},
+        {0x3a05, 0x14},
+        {0x3a06, 0x00},
+        {0x3a07, 0xfe},
+        {0x3b00, 0x00},
+        {0x3b02, 0x00},
+        {0x3b03, 0x00},
+        {0x3b04, 0x00},
+        {0x3b05, 0x00},
+        {0x3d00, 0x00},
+        {0x3d01, 0x00},
+        {0x3d02, 0x00},
+        {0x3d03, 0x00},
+        {0x3d04, 0x00},
+        {0x3d05, 0x00},
+        {0x3d06, 0x00},
+        {0x3d07, 0x00},
+        {0x3d08, 0x00},
+        {0x3d09, 0x00},
+        {0x3d0a, 0x00},
+        {0x3d0b, 0x00},
+        {0x3d0c, 0x00},
+        {0x3d0d, 0x00},
+        {0x3d0e, 0x00},
+        {0x3d0f, 0x00},
+        {0x3d80, 0x00},
+        {0x3d81, 0x00},
+        {0x3d84, 0x00},
+        {0x3e07, 0x20},
+        {0x4000, 0x08},
+        {0x4001, 0x04},
+        {0x4002, 0x45},
+        {0x4004, 0x08},
+        {0x4005, 0x18},
+        {0x4006, 0x20},
+        {0x4008, 0x24},
+        {0x4009, 0x10},
+        {0x400c, 0x00},
+        {0x400d, 0x00},
+        {0x4058, 0x00},
+        {0x4101, 0xb2},
+        {0x4303, 0x00},
+        {0x4304, 0x08},
+        {0x4307, 0x30},
+        {0x4311, 0x04},
+        {0x4315, 0x01},
+        {0x4511, 0x05},
+        {0x4512, 0x01},
+        {0x4800, 0x20}, /* dis-continuous */
+        {0x4806, 0x00},
+        {0x4816, 0x52},
+        {0x481f, 0x30},
+        {0x4826, 0x32},
+        {0x4831, 0x6a},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d02, 0xfd},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4837, 0x0a},
+        {0x5000, 0x06},
+        {0x5001, 0x01},
+        {0x5002, 0x00},
+        {0x5003, 0x20},
+        {0x5046, 0x0a},
+        {0x5013, 0x00},
+        {0x5046, 0x0a},
+        {0x5780, 0x1c},
+        {0x5786, 0x20},
+        {0x5787, 0x10},
+        {0x5788, 0x18},
+        {0x578a, 0x04},
+        {0x578b, 0x02},
+        {0x578c, 0x02},
+        {0x578e, 0x06},
+        {0x578f, 0x02},
+        {0x5790, 0x02},
+        {0x5791, 0xff},
+        {0x5842, 0x01},
+        {0x5843, 0x2b},
+        {0x5844, 0x01},
+        {0x5845, 0x92},
+        {0x5846, 0x01},
+        {0x5847, 0x8f},
+        {0x5848, 0x01},
+        {0x5849, 0x0c},
+        {0x5e00, 0x00},
+        {0x5e10, 0x0c},
+        {OV5693_TABLE_END, 0x0000}
+};
+
+static const ov5693_reg mode_2592x1458[] = {
+        {OV5693_TABLE_WAIT_MS, OV5693_WAIT_MS},
+        {0x0100, 0x00},/* Including sw reset */
+        {0x3001, 0x0a},
+        {0x3002, 0x80},
+        {0x3006, 0x00},
+        {0x3011, 0x21},
+        {0x3012, 0x09},
+        {0x3013, 0x10},
+        {0x3014, 0x00},
+        {0x3015, 0x08},
+        {0x3016, 0xf0},
+        {0x3017, 0xf0},
+        {0x3018, 0xf0},
+        {0x301b, 0xb4},
+        {0x301d, 0x02},
+        {0x3021, 0x00},
+        {0x3022, 0x01},
+        {0x3028, 0x44},
+        {0x3098, 0x03},
+        {0x3099, 0x1e},
+        {0x309a, 0x02},
+        {0x309b, 0x01},
+        {0x309c, 0x00},
+        {0x30a0, 0xd2},
+        {0x30a2, 0x01},
+        {0x30b2, 0x00},
+        {0x30b3, 0x68},
+        {0x30b4, 0x03},
+        {0x30b5, 0x04},
+        {0x30b6, 0x01},
+        {0x3104, 0x21},
+        {0x3106, 0x00},
+        {0x3400, 0x04},
+        {0x3401, 0x00},
+        {0x3402, 0x04},
+        {0x3403, 0x00},
+        {0x3404, 0x04},
+        {0x3405, 0x00},
+        {0x3406, 0x01},
+        {0x3500, 0x00},
+        {0x3501, 0x7b},
+        {0x3502, 0x00},
+        {0x3503, 0x07},
+        {0x3504, 0x00},
+        {0x3505, 0x00},
+        {0x3506, 0x00},
+        {0x3507, 0x02},
+        {0x3508, 0x00},
+        {0x3509, 0x10},
+        {0x350a, 0x00},
+        {0x350b, 0x40},
+        {0x3600, 0xbc},
+        {0x3601, 0x0a},
+        {0x3602, 0x38},
+        {0x3612, 0x80},
+        {0x3620, 0x44},
+        {0x3621, 0xb5},
+        {0x3622, 0x0c},
+        {0x3625, 0x10},
+        {0x3630, 0x55},
+        {0x3631, 0xf4},
+        {0x3632, 0x00},
+        {0x3633, 0x34},
+        {0x3634, 0x02},
+        {0x364d, 0x0d},
+        {0x364f, 0xdd},
+        {0x3660, 0x04},
+        {0x3662, 0x10},
+        {0x3663, 0xf1},
+        {0x3665, 0x00},
+        {0x3666, 0x20},
+        {0x3667, 0x00},
+        {0x366a, 0x80},
+        {0x3680, 0xe0},
+        {0x3681, 0x00},
+        {0x3700, 0x42},
+        {0x3701, 0x14},
+        {0x3702, 0xa0},
+        {0x3703, 0xd8},
+        {0x3704, 0x78},
+        {0x3705, 0x02},
+        {0x3708, 0xe2},
+        {0x3709, 0xc3},
+        {0x370a, 0x00},
+        {0x370b, 0x20},
+        {0x370c, 0x0c},
+        {0x370d, 0x11},
+        {0x370e, 0x00},
+        {0x370f, 0x40},
+        {0x3710, 0x00},
+        {0x371a, 0x1c},
+        {0x371b, 0x05},
+        {0x371c, 0x01},
+        {0x371e, 0xa1},
+        {0x371f, 0x0c},
+        {0x3721, 0x00},
+        {0x3724, 0x10},
+        {0x3726, 0x00},
+        {0x372a, 0x01},
+        {0x3730, 0x10},
+        {0x3738, 0x22},
+        {0x3739, 0xe5},
+        {0x373a, 0x50},
+        {0x373b, 0x02},
+        {0x373c, 0x41},
+        {0x373f, 0x02},
+        {0x3740, 0x42},
+        {0x3741, 0x02},
+        {0x3742, 0x18},
+        {0x3743, 0x01},
+        {0x3744, 0x02},
+        {0x3747, 0x10},
+        {0x374c, 0x04},
+        {0x3751, 0xf0},
+        {0x3752, 0x00},
+        {0x3753, 0x00},
+        {0x3754, 0xc0},
+        {0x3755, 0x00},
+        {0x3756, 0x1a},
+        {0x3758, 0x00},
+        {0x3759, 0x0f},
+        {0x376b, 0x44},
+        {0x375c, 0x04},
+        {0x3774, 0x10},
+        {0x3776, 0x00},
+        {0x377f, 0x08},
+        {0x3780, 0x22},
+        {0x3781, 0x0c},
+        {0x3784, 0x2c},
+        {0x3785, 0x1e},
+        {0x378f, 0xf5},
+        {0x3791, 0xb0},
+        {0x3795, 0x00},
+        {0x3796, 0x64},
+        {0x3797, 0x11},
+        {0x3798, 0x30},
+        {0x3799, 0x41},
+        {0x379a, 0x07},
+        {0x379b, 0xb0},
+        {0x379c, 0x0c},
+        {0x37c5, 0x00},
+        {0x37c6, 0x00},
+        {0x37c7, 0x00},
+        {0x37c9, 0x00},
+        {0x37ca, 0x00},
+        {0x37cb, 0x00},
+        {0x37de, 0x00},
+        {0x37df, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x00},
+        {0x3802, 0x00},
+        {0x3803, 0xf4},
+        {0x3804, 0x0a},
+        {0x3805, 0x3f},
+        {0x3806, 0x06},
+        {0x3807, 0xb1},
+        {0x3808, 0x0a},
+        {0x3809, 0x20},
+        {0x380a, 0x05},
+        {0x380b, 0xb2},
+        {0x380c, 0x0a},
+        {0x380d, 0x80},
+        {0x380e, 0x07},
+        {0x380f, 0xc0},
+        {0x3810, 0x00},
+        {0x3811, 0x10},
+        {0x3812, 0x00},
+        {0x3813, 0x06},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x1e},
+        {0x3823, 0x00},
+        {0x3824, 0x00},
+        {0x3825, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x382a, 0x04},
+        {0x3a04, 0x06},
+        {0x3a05, 0x14},
+        {0x3a06, 0x00},
+        {0x3a07, 0xfe},
+        {0x3b00, 0x00},
+        {0x3b02, 0x00},
+        {0x3b03, 0x00},
+        {0x3b04, 0x00},
+        {0x3b05, 0x00},
+        {0x3e07, 0x20},
+        {0x4000, 0x08},
+        {0x4001, 0x04},
+        {0x4002, 0x45},
+        {0x4004, 0x08},
+        {0x4005, 0x18},
+        {0x4006, 0x20},
+        {0x4008, 0x24},
+        {0x4009, 0x10},
+        {0x400c, 0x00},
+        {0x400d, 0x00},
+        {0x4058, 0x00},
+        {0x404e, 0x37},
+        {0x404f, 0x8f},
+        {0x4058, 0x00},
+        {0x4101, 0xb2},
+        {0x4303, 0x00},
+        {0x4304, 0x08},
+        {0x4307, 0x30},
+        {0x4311, 0x04},
+        {0x4315, 0x01},
+        {0x4511, 0x05},
+        {0x4512, 0x01},
+        {0x4800, 0x20}, /* dis-continuous */
+        {0x4806, 0x00},
+        {0x4816, 0x52},
+        {0x481f, 0x30},
+        {0x4826, 0x32},
+        {0x4831, 0x6a},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d02, 0xfd},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4837, 0x0a},
+        {0x5000, 0x06},
+        {0x5001, 0x01},
+        {0x5002, 0x00},
+        {0x5003, 0x20},
+        {0x5046, 0x0a},
+        {0x5013, 0x00},
+        {0x5046, 0x0a},
+        {0x5780, 0xfc},
+        {0x5781, 0x13},
+        {0x5782, 0x03},
+        {0x5786, 0x20},
+        {0x5787, 0x40},
+        {0x5788, 0x08},
+        {0x5789, 0x08},
+        {0x578a, 0x02},
+        {0x578b, 0x01},
+        {0x578c, 0x01},
+        {0x578d, 0x0c},
+        {0x578e, 0x02},
+        {0x578f, 0x01},
+        {0x5790, 0x01},
+        {0x5791, 0xff},
+        {0x5842, 0x01},
+        {0x5843, 0x2b},
+        {0x5844, 0x01},
+        {0x5845, 0x92},
+        {0x5846, 0x01},
+        {0x5847, 0x8f},
+        {0x5848, 0x01},
+        {0x5849, 0x0c},
+        {0x5e00, 0x00},
+        {0x5e10, 0x0c},
+        {OV5693_TABLE_END, 0x0000}
+};
+
+static const ov5693_reg mode_1920x1080[] = {
+        {OV5693_TABLE_WAIT_MS, OV5693_WAIT_MS},
+        {0x0100, 0x00},/*, 0xIncluding, 0xsw, 0xreset, 0x*/
+        {0x3001, 0x0a},
+        {0x3002, 0x80},
+        {0x3006, 0x00},
+        {0x3011, 0x21},
+        {0x3012, 0x09},
+        {0x3013, 0x10},
+        {0x3014, 0x00},
+        {0x3015, 0x08},
+        {0x3016, 0xf0},
+        {0x3017, 0xf0},
+        {0x3018, 0xf0},
+        {0x301b, 0xb4},
+        {0x301d, 0x02},
+        {0x3021, 0x00},
+        {0x3022, 0x01},
+        {0x3028, 0x44},
+        {0x3098, 0x03},
+        {0x3099, 0x1e},
+        {0x309a, 0x02},
+        {0x309b, 0x01},
+        {0x309c, 0x00},
+        {0x30a0, 0xd2},
+        {0x30a2, 0x01},
+        {0x30b2, 0x00},
+        {0x30b3, 0x68},
+        {0x30b4, 0x03},
+        {0x30b5, 0x04},
+        {0x30b6, 0x01},
+        {0x3104, 0x21},
+        {0x3106, 0x00},
+        {0x3406, 0x01},
+        {0x3500, 0x00},
+        {0x3501, 0x7b},
+        {0x3502, 0x00},
+        {0x3503, 0x07},
+        {0x3504, 0x00},
+        {0x3505, 0x00},
+        {0x3506, 0x00},
+        {0x3507, 0x02},
+        {0x3508, 0x00},
+        {0x3509, 0x10},
+        {0x350a, 0x00},
+        {0x350b, 0x40},
+        {0x3601, 0x0a},
+        {0x3602, 0x38},
+        {0x3612, 0x80},
+        {0x3620, 0x54},
+        {0x3621, 0xc7},
+        {0x3622, 0x0f},
+        {0x3625, 0x10},
+        {0x3630, 0x55},
+        {0x3631, 0xf4},
+        {0x3632, 0x00},
+        {0x3633, 0x34},
+        {0x3634, 0x02},
+        {0x364d, 0x0d},
+        {0x364f, 0xdd},
+        {0x3660, 0x04},
+        {0x3662, 0x10},
+        {0x3663, 0xf1},
+        {0x3665, 0x00},
+        {0x3666, 0x20},
+        {0x3667, 0x00},
+        {0x366a, 0x80},
+        {0x3680, 0xe0},
+        {0x3681, 0x00},
+        {0x3700, 0x42},
+        {0x3701, 0x14},
+        {0x3702, 0xa0},
+        {0x3703, 0xd8},
+        {0x3704, 0x78},
+        {0x3705, 0x02},
+        {0x3708, 0xe2},
+        {0x3709, 0xc3},
+        {0x370a, 0x00},
+        {0x370b, 0x20},
+        {0x370c, 0x0c},
+        {0x370d, 0x11},
+        {0x370e, 0x00},
+        {0x370f, 0x40},
+        {0x3710, 0x00},
+        {0x371a, 0x1c},
+        {0x371b, 0x05},
+        {0x371c, 0x01},
+        {0x371e, 0xa1},
+        {0x371f, 0x0c},
+        {0x3721, 0x00},
+        {0x3724, 0x10},
+        {0x3726, 0x00},
+        {0x372a, 0x01},
+        {0x3730, 0x10},
+        {0x3738, 0x22},
+        {0x3739, 0xe5},
+        {0x373a, 0x50},
+        {0x373b, 0x02},
+        {0x373c, 0x41},
+        {0x373f, 0x02},
+        {0x3740, 0x42},
+        {0x3741, 0x02},
+        {0x3742, 0x18},
+        {0x3743, 0x01},
+        {0x3744, 0x02},
+        {0x3747, 0x10},
+        {0x374c, 0x04},
+        {0x3751, 0xf0},
+        {0x3752, 0x00},
+        {0x3753, 0x00},
+        {0x3754, 0xc0},
+        {0x3755, 0x00},
+        {0x3756, 0x1a},
+        {0x3758, 0x00},
+        {0x3759, 0x0f},
+        {0x376b, 0x44},
+        {0x375c, 0x04},
+        {0x3774, 0x10},
+        {0x3776, 0x00},
+        {0x377f, 0x08},
+        {0x3780, 0x22},
+        {0x3781, 0x0c},
+        {0x3784, 0x2c},
+        {0x3785, 0x1e},
+        {0x378f, 0xf5},
+        {0x3791, 0xb0},
+        {0x3795, 0x00},
+        {0x3796, 0x64},
+        {0x3797, 0x11},
+        {0x3798, 0x30},
+        {0x3799, 0x41},
+        {0x379a, 0x07},
+        {0x379b, 0xb0},
+        {0x379c, 0x0c},
+        {0x37c5, 0x00},
+        {0x37c6, 0x00},
+        {0x37c7, 0x00},
+        {0x37c9, 0x00},
+        {0x37ca, 0x00},
+        {0x37cb, 0x00},
+        {0x37de, 0x00},
+        {0x37df, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x00},
+        {0x3802, 0x00},
+        {0x3803, 0xf8},
+        {0x3804, 0x0a},
+        {0x3805, 0x3f},
+        {0x3806, 0x06},
+        {0x3807, 0xab},
+        {0x3808, 0x07},
+        {0x3809, 0x80},
+        {0x380a, 0x04},
+        {0x380b, 0x38},
+        {0x380c, 0x0a},
+        {0x380d, 0x80},
+        {0x380e, 0x07},
+        {0x380f, 0xc0},
+        {0x3810, 0x00},
+        {0x3811, 0x02},
+        {0x3812, 0x00},
+        {0x3813, 0x02},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x1e},
+        {0x3823, 0x00},
+        {0x3824, 0x00},
+        {0x3825, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x382a, 0x04},
+        {0x3a04, 0x06},
+        {0x3a05, 0x14},
+        {0x3a06, 0x00},
+        {0x3a07, 0xfe},
+        {0x3b00, 0x00},
+        {0x3b02, 0x00},
+        {0x3b03, 0x00},
+        {0x3b04, 0x00},
+        {0x3b05, 0x00},
+        {0x3e07, 0x20},
+        {0x4000, 0x08},
+        {0x4001, 0x04},
+        {0x4002, 0x45},
+        {0x4004, 0x08},
+        {0x4005, 0x18},
+        {0x4006, 0x20},
+        {0x4008, 0x24},
+        {0x4009, 0x10},
+        {0x400c, 0x00},
+        {0x400d, 0x00},
+        {0x4058, 0x00},
+        {0x404e, 0x37},
+        {0x404f, 0x8f},
+        {0x4058, 0x00},
+        {0x4101, 0xb2},
+        {0x4303, 0x00},
+        {0x4304, 0x08},
+        {0x4307, 0x30},
+        {0x4311, 0x04},
+        {0x4315, 0x01},
+        {0x4511, 0x05},
+        {0x4512, 0x01},
+        {0x4800, 0x20}, /* dis-continuous */
+        {0x4806, 0x00},
+        {0x4816, 0x52},
+        {0x481f, 0x30},
+        {0x4826, 0x32},
+        {0x4831, 0x6a},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d02, 0xfd},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4837, 0x0a},
+        {0x5000, 0x06},
+        {0x5001, 0x01},
+        {0x5002, 0x80},
+        {0x5003, 0x20},
+        {0x5046, 0x0a},
+        {0x5013, 0x00},
+        {0x5046, 0x0a},
+        {0x5780, 0x1c},
+        {0x5786, 0x20},
+        {0x5787, 0x10},
+        {0x5788, 0x18},
+        {0x578a, 0x04},
+        {0x578b, 0x02},
+        {0x578c, 0x02},
+        {0x578e, 0x06},
+        {0x578f, 0x02},
+        {0x5790, 0x02},
+        {0x5791, 0xff},
+        {0x5842, 0x01},
+        {0x5843, 0x2b},
+        {0x5844, 0x01},
+        {0x5845, 0x92},
+        {0x5846, 0x01},
+        {0x5847, 0x8f},
+        {0x5848, 0x01},
+        {0x5849, 0x0c},
+        {0x5e00, 0x00},
+        {0x5e10, 0x0c},
+        {OV5693_TABLE_END, 0x0000}
+};
+
+static const ov5693_reg mode_1280x720_120fps[] = {
+        {OV5693_TABLE_WAIT_MS, OV5693_WAIT_MS},
+        {0x0100, 0x00},/* Including sw reset */
+        {0x3001, 0x0a},
+        {0x3002, 0x80},
+        {0x3006, 0x00},
+        {0x3011, 0x21},
+        {0x3012, 0x09},
+        {0x3013, 0x10},
+        {0x3014, 0x00},
+        {0x3015, 0x08},
+        {0x3016, 0xf0},
+        {0x3017, 0xf0},
+        {0x3018, 0xf0},
+        {0x301b, 0xb4},
+        {0x301d, 0x02},
+        {0x3021, 0x00},
+        {0x3022, 0x01},
+        {0x3028, 0x44},
+        {0x3098, 0x03},
+        {0x3099, 0x1e},
+        {0x309a, 0x02},
+        {0x309b, 0x01},
+        {0x309c, 0x00},
+        {0x30a0, 0xd2},
+        {0x30a2, 0x01},
+        {0x30b2, 0x00},
+        {0x30b3, 0x68},
+        {0x30b4, 0x03},
+        {0x30b5, 0x04},
+        {0x30b6, 0x01},
+        {0x3104, 0x21},
+        {0x3106, 0x00},
+        {0x3406, 0x01},
+        {0x3500, 0x00},
+        {0x3501, 0x2e},
+        {0x3502, 0x80},
+        {0x3503, 0x07},
+        {0x3504, 0x00},
+        {0x3505, 0x00},
+        {0x3506, 0x00},
+        {0x3507, 0x02},
+        {0x3508, 0x00},
+        {0x3509, 0x10},
+        {0x350a, 0x00},
+        {0x350b, 0x40},
+        {0x3601, 0x0a},
+        {0x3602, 0x38},
+        {0x3612, 0x80},
+        {0x3620, 0x54},
+        {0x3621, 0xc7},
+        {0x3622, 0x0f},
+        {0x3625, 0x10},
+        {0x3630, 0x55},
+        {0x3631, 0xf4},
+        {0x3632, 0x00},
+        {0x3633, 0x34},
+        {0x3634, 0x02},
+        {0x364d, 0x0d},
+        {0x364f, 0xdd},
+        {0x3660, 0x04},
+        {0x3662, 0x10},
+        {0x3663, 0xf1},
+        {0x3665, 0x00},
+        {0x3666, 0x20},
+        {0x3667, 0x00},
+        {0x366a, 0x80},
+        {0x3680, 0xe0},
+        {0x3681, 0x00},
+        {0x3700, 0x42},
+        {0x3701, 0x14},
+        {0x3702, 0xa0},
+        {0x3703, 0xd8},
+        {0x3704, 0x78},
+        {0x3705, 0x02},
+        {0x3708, 0xe6},
+        {0x3709, 0xc7},
+        {0x370a, 0x00},
+        {0x370b, 0x20},
+        {0x370c, 0x0c},
+        {0x370d, 0x11},
+        {0x370e, 0x00},
+        {0x370f, 0x40},
+        {0x3710, 0x00},
+        {0x371a, 0x1c},
+        {0x371b, 0x05},
+        {0x371c, 0x01},
+        {0x371e, 0xa1},
+        {0x371f, 0x0c},
+        {0x3721, 0x00},
+        {0x3724, 0x10},
+        {0x3726, 0x00},
+        {0x372a, 0x01},
+        {0x3730, 0x10},
+        {0x3738, 0x22},
+        {0x3739, 0xe5},
+        {0x373a, 0x50},
+        {0x373b, 0x02},
+        {0x373c, 0x41},
+        {0x373f, 0x02},
+        {0x3740, 0x42},
+        {0x3741, 0x02},
+        {0x3742, 0x18},
+        {0x3743, 0x01},
+        {0x3744, 0x02},
+        {0x3747, 0x10},
+        {0x374c, 0x04},
+        {0x3751, 0xf0},
+        {0x3752, 0x00},
+        {0x3753, 0x00},
+        {0x3754, 0xc0},
+        {0x3755, 0x00},
+        {0x3756, 0x1a},
+        {0x3758, 0x00},
+        {0x3759, 0x0f},
+        {0x376b, 0x44},
+        {0x375c, 0x04},
+        {0x3774, 0x10},
+        {0x3776, 0x00},
+        {0x377f, 0x08},
+        {0x3780, 0x22},
+        {0x3781, 0x0c},
+        {0x3784, 0x2c},
+        {0x3785, 0x1e},
+        {0x378f, 0xf5},
+        {0x3791, 0xb0},
+        {0x3795, 0x00},
+        {0x3796, 0x64},
+        {0x3797, 0x11},
+        {0x3798, 0x30},
+        {0x3799, 0x41},
+        {0x379a, 0x07},
+        {0x379b, 0xb0},
+        {0x379c, 0x0c},
+        {0x37c5, 0x00},
+        {0x37c6, 0x00},
+        {0x37c7, 0x00},
+        {0x37c9, 0x00},
+        {0x37ca, 0x00},
+        {0x37cb, 0x00},
+        {0x37de, 0x00},
+        {0x37df, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x00},
+        {0x3802, 0x00},
+        {0x3803, 0xf4},
+        {0x3804, 0x0a},
+        {0x3805, 0x3f},
+        {0x3806, 0x06},
+        {0x3807, 0xab},
+        {0x3808, 0x05},
+        {0x3809, 0x00},
+        {0x380a, 0x02},
+        {0x380b, 0xd0},
+        {0x380c, 0x06},
+        {0x380d, 0xd8},
+        {0x380e, 0x02},
+        {0x380f, 0xf8},
+        {0x3810, 0x00},
+        {0x3811, 0x02},
+        {0x3812, 0x00},
+        {0x3813, 0x02},
+        {0x3814, 0x31},
+        {0x3815, 0x31},
+        {0x3820, 0x04},
+        {0x3821, 0x1f},
+        {0x3823, 0x00},
+        {0x3824, 0x00},
+        {0x3825, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x382a, 0x04},
+        {0x3a04, 0x06},
+        {0x3a05, 0x14},
+        {0x3a06, 0x00},
+        {0x3a07, 0xfe},
+        {0x3b00, 0x00},
+        {0x3b02, 0x00},
+        {0x3b03, 0x00},
+        {0x3b04, 0x00},
+        {0x3b05, 0x00},
+        {0x3e07, 0x20},
+        {0x4000, 0x08},
+        {0x4001, 0x04},
+        {0x4002, 0x45},
+        {0x4004, 0x08},
+        {0x4005, 0x18},
+        {0x4006, 0x20},
+        {0x4008, 0x24},
+        {0x4009, 0x10},
+        {0x400c, 0x00},
+        {0x400d, 0x00},
+        {0x4058, 0x00},
+        {0x404e, 0x37},
+        {0x404f, 0x8f},
+        {0x4058, 0x00},
+        {0x4101, 0xb2},
+        {0x4303, 0x00},
+        {0x4304, 0x08},
+        {0x4307, 0x30},
+        {0x4311, 0x04},
+        {0x4315, 0x01},
+        {0x4511, 0x05},
+        {0x4512, 0x00},
+        {0x4800, 0x20}, /* dis-continuous */
+        {0x4806, 0x00},
+        {0x4816, 0x52},
+        {0x481f, 0x30},
+        {0x4826, 0x32},
+        {0x4831, 0x6a},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d02, 0xfd},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4837, 0x0a},
+        {0x5000, 0x06},
+        {0x5001, 0x01},
+        {0x5002, 0x00},
+        {0x5003, 0x20},
+        {0x5046, 0x0a},
+        {0x5013, 0x00},
+        {0x5046, 0x0a},
+        {0x5780, 0x1c},
+        {0x5786, 0x20},
+        {0x5787, 0x10},
+        {0x5788, 0x18},
+        {0x578a, 0x04},
+        {0x578b, 0x02},
+        {0x578c, 0x02},
+        {0x578e, 0x06},
+        {0x578f, 0x02},
+        {0x5790, 0x02},
+        {0x5791, 0xff},
+        {0x5842, 0x01},
+        {0x5843, 0x2b},
+        {0x5844, 0x01},
+        {0x5845, 0x92},
+        {0x5846, 0x01},
+        {0x5847, 0x8f},
+        {0x5848, 0x01},
+        {0x5849, 0x0c},
+        {0x5e00, 0x00},
+        {0x5e10, 0x0c},
+        {OV5693_TABLE_END, 0x0000}
+};
+
+static const ov5693_reg mode_2592x1944_HDR_24fps[] = {
+        {OV5693_TABLE_WAIT_MS, OV5693_WAIT_MS},
+        {0x0100, 0x00},/* Including sw reset */
+        {0x0103, 0x01},
+        {0x3001, 0x0a},
+        {0x3002, 0x80},
+        {0x3006, 0x00},
+        {0x3011, 0x21},
+        {0x3012, 0x09},
+        {0x3013, 0x10},
+        {0x3014, 0x00},
+        {0x3015, 0x08},
+        {0x3016, 0xf0},
+        {0x3017, 0xf0},
+        {0x3018, 0xf0},
+        {0x301b, 0xb4},
+        {0x301d, 0x02},
+        {0x3021, 0x00},
+        {0x3022, 0x01},
+        {0x3028, 0x44},
+        {0x3098, 0x02},
+        {0x3099, 0x16},
+        {0x309a, 0x02},
+        {0x309b, 0x01},
+        {0x309c, 0x00},
+        {0x30b2, 0x00},
+        {0x30b3, 0x6e},
+        {0x30b4, 0x03},
+        {0x30a0, 0xd2},
+        {0x30a2, 0x01},
+        {0x30b5, 0x04},
+        {0x30b6, 0x01},
+        {0x3104, 0x21},
+        {0x3106, 0x00},
+        {0x3406, 0x01},
+        {0x3500, 0x00},
+        {0x3501, 0x7b},
+        {0x3502, 0x80},
+        {0x3503, 0x07},
+        {0x3504, 0x00},
+        {0x3505, 0x00},
+        {0x3506, 0x00},
+        {0x3507, 0x01},
+        {0x3508, 0x80},
+        {0x3509, 0x10},
+        {0x350a, 0x00},
+        {0x350b, 0x40},
+        {0x3601, 0x0a},
+        {0x3602, 0x38},
+        {0x3612, 0x80},
+        {0x3620, 0x54},
+        {0x3621, 0xc7},
+        {0x3622, 0x05},
+        {0x3625, 0x10},
+        {0x3630, 0x55},
+        {0x3631, 0xf4},
+        {0x3632, 0x00},
+        {0x3633, 0x34},
+        {0x3634, 0x02},
+        {0x364d, 0x0d},
+        {0x364f, 0xdd},
+        {0x3660, 0x04},
+        {0x3662, 0x10},
+        {0x3663, 0xf1},
+        {0x3665, 0x00},
+        {0x3666, 0x20},
+        {0x3667, 0x00},
+        {0x366a, 0x80},
+        {0x3680, 0xe0},
+        {0x3681, 0x00},
+        {0x3700, 0x42},
+        {0x3701, 0x14},
+        {0x3702, 0xa0},
+        {0x3703, 0xa8},
+        {0x3704, 0x78},
+        {0x3705, 0x02},
+        {0x3708, 0xe2},
+        {0x3709, 0xc3},
+        {0x370a, 0x00},
+        {0x370b, 0x20},
+        {0x370c, 0x0c},
+        {0x370d, 0x11},
+        {0x370e, 0x00},
+        {0x370f, 0x40},
+        {0x3710, 0x00},
+        {0x371a, 0x0c},
+        {0x371b, 0x05},
+        {0x371c, 0x01},
+        {0x371e, 0xa1},
+        {0x371f, 0x0c},
+        {0x3721, 0x00},
+        {0x3724, 0x10},
+        {0x3726, 0x00},
+        {0x372a, 0x01},
+        {0x3730, 0x10},
+        {0x3738, 0x22},
+        {0x3739, 0xe5},
+        {0x373a, 0x50},
+        {0x373b, 0x02},
+        {0x373c, 0x41},
+        {0x373f, 0x02},
+        {0x3740, 0x42},
+        {0x3741, 0x02},
+        {0x3742, 0x18},
+        {0x3743, 0x01},
+        {0x3744, 0x02},
+        {0x3747, 0x10},
+        {0x374c, 0x04},
+        {0x3751, 0xf0},
+        {0x3752, 0x00},
+        {0x3753, 0x00},
+        {0x3754, 0xc0},
+        {0x3755, 0x00},
+        {0x3756, 0x1a},
+        {0x3758, 0x00},
+        {0x3759, 0x0f},
+        {0x376b, 0x44},
+        {0x375c, 0x04},
+        {0x3774, 0x10},
+        {0x3776, 0x00},
+        {0x377f, 0x08},
+        {0x3780, 0x22},
+        {0x3781, 0x0c},
+        {0x3784, 0x2c},
+        {0x3785, 0x1e},
+        {0x378f, 0xf5},
+        {0x3791, 0xb0},
+        {0x3795, 0x00},
+        {0x3796, 0x64},
+        {0x3797, 0x11},
+        {0x3798, 0x30},
+        {0x3799, 0x41},
+        {0x379a, 0x07},
+        {0x379b, 0xb0},
+        {0x379c, 0x0c},
+        {0x37c5, 0x00},
+        {0x37c6, 0x00},
+        {0x37c7, 0x00},
+        {0x37c9, 0x00},
+        {0x37ca, 0x00},
+        {0x37cb, 0x00},
+        {0x37de, 0x00},
+        {0x37df, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x02},
+        {0x3802, 0x00},
+        {0x3803, 0x06},
+        {0x3804, 0x0a},
+        {0x3805, 0x41},
+        {0x3806, 0x07},
+        {0x3807, 0xa1},
+        {0x3808, 0x0a},
+        {0x3809, 0x20},
+        {0x380a, 0x07},
+        {0x380b, 0x98},
+        {0x380c, 0x0e},
+        {0x380d, 0x70},
+        {0x380e, 0x07},
+        {0x380f, 0xc0},
+        {0x3810, 0x00},
+        {0x3811, 0x10},
+        {0x3812, 0x00},
+        {0x3813, 0x02},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x9e},
+        {0x3823, 0x00},
+        {0x3824, 0x00},
+        {0x3825, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x382a, 0x04},
+        {0x3a04, 0x09},
+        {0x3a05, 0xa9},
+        {0x3a06, 0x00},
+        {0x3a07, 0xfe},
+        {0x3b00, 0x00},
+        {0x3b02, 0x00},
+        {0x3b03, 0x00},
+        {0x3b04, 0x00},
+        {0x3b05, 0x00},
+        {0x3e07, 0x20},
+        {0x4000, 0x08},
+        {0x4001, 0x04},
+        {0x4002, 0x45},
+        {0x4004, 0x08},
+        {0x4005, 0x18},
+        {0x4006, 0x20},
+        {0x4008, 0x24},
+        {0x4009, 0x10},
+        {0x400c, 0x00},
+        {0x400d, 0x00},
+        {0x4058, 0x00},
+        {0x404e, 0x37},
+        {0x404f, 0x8f},
+        {0x4058, 0x00},
+        {0x4101, 0xb2},
+        {0x4303, 0x00},
+        {0x4304, 0x08},
+        {0x4307, 0x30},
+        {0x4311, 0x04},
+        {0x4315, 0x01},
+        {0x4511, 0x05},
+        {0x4512, 0x01},
+        {0x4800, 0x20}, /* dis-continuous */
+        {0x4806, 0x00},
+        {0x4816, 0x52},
+        {0x481f, 0x30},
+        {0x4826, 0x32},
+        {0x4831, 0x6a},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d02, 0xfd},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4837, 0x0a},
+        {0x5000, 0x06},
+        {0x5001, 0x01},
+        {0x5002, 0x00},
+        {0x5003, 0x20},
+        {0x5046, 0x0a},
+        {0x5013, 0x00},
+        {0x5046, 0x0a},
+        {0x5780, 0x1c},
+        {0x5786, 0x20},
+        {0x5787, 0x10},
+        {0x5788, 0x18},
+        {0x578a, 0x04},
+        {0x578b, 0x02},
+        {0x578c, 0x02},
+        {0x578e, 0x06},
+        {0x578f, 0x02},
+        {0x5790, 0x02},
+        {0x5791, 0xff},
+        {0x5842, 0x01},
+        {0x5843, 0x2b},
+        {0x5844, 0x01},
+        {0x5845, 0x92},
+        {0x5846, 0x01},
+        {0x5847, 0x8f},
+        {0x5848, 0x01},
+        {0x5849, 0x0c},
+        {0x5e00, 0x00},
+        {0x5e10, 0x0c},
+        {OV5693_TABLE_END, 0x0000}
+};
+
+static const ov5693_reg mode_1920x1080_HDR_30fps[] = {
+        {OV5693_TABLE_WAIT_MS, OV5693_WAIT_MS},
+        {0x0100, 0x00},/* Including sw reset */
+        {0x0103, 0x01},
+        {0x3001, 0x0a},
+        {0x3002, 0x80},
+        {0x3006, 0x00},
+        {0x3011, 0x21},
+        {0x3012, 0x09},
+        {0x3013, 0x10},
+        {0x3014, 0x00},
+        {0x3015, 0x08},
+        {0x3016, 0xf0},
+        {0x3017, 0xf0},
+        {0x3018, 0xf0},
+        {0x301b, 0xb4},
+        {0x301d, 0x02},
+        {0x3021, 0x00},
+        {0x3022, 0x01},
+        {0x3028, 0x44},
+        {0x3098, 0x03},
+        {0x3099, 0x1e},
+        {0x309a, 0x02},
+        {0x309b, 0x01},
+        {0x309c, 0x00},
+        {0x30a0, 0xd2},
+        {0x30a2, 0x01},
+        {0x30b2, 0x00},
+        {0x30b3, 0x68},
+        {0x30b4, 0x03},
+        {0x30b5, 0x04},
+        {0x30b6, 0x01},
+        {0x3104, 0x21},
+        {0x3106, 0x00},
+        {0x3406, 0x01},
+        {0x3500, 0x00},
+        {0x3501, 0x72},
+        {0x3502, 0x00},
+        {0x3503, 0x07},
+        {0x3504, 0x00},
+        {0x3505, 0x00},
+        {0x3506, 0x00},
+        {0x3507, 0x01},
+        {0x3508, 0x80},
+        {0x3509, 0x10},
+        {0x350a, 0x00},
+        {0x350b, 0x40},
+        {0x3601, 0x0a},
+        {0x3602, 0x38},
+        {0x3612, 0x80},
+        {0x3620, 0x54},
+        {0x3621, 0xc7},
+        {0x3622, 0x0f},
+        {0x3625, 0x10},
+        {0x3630, 0x55},
+        {0x3631, 0xf4},
+        {0x3632, 0x00},
+        {0x3633, 0x34},
+        {0x3634, 0x02},
+        {0x364d, 0x0d},
+        {0x364f, 0xdd},
+        {0x3660, 0x04},
+        {0x3662, 0x10},
+        {0x3663, 0xf1},
+        {0x3665, 0x00},
+        {0x3666, 0x20},
+        {0x3667, 0x00},
+        {0x366a, 0x80},
+        {0x3680, 0xe0},
+        {0x3681, 0x00},
+        {0x3700, 0x42},
+        {0x3701, 0x14},
+        {0x3702, 0xa0},
+        {0x3703, 0xd8},
+        {0x3704, 0x78},
+        {0x3705, 0x02},
+        {0x3708, 0xe2},
+        {0x3709, 0xc3},
+        {0x370a, 0x00},
+        {0x370b, 0x20},
+        {0x370c, 0x0c},
+        {0x370d, 0x11},
+        {0x370e, 0x00},
+        {0x370f, 0x40},
+        {0x3710, 0x00},
+        {0x371a, 0x1c},
+        {0x371b, 0x05},
+        {0x371c, 0x01},
+        {0x371e, 0xa1},
+        {0x371f, 0x0c},
+        {0x3721, 0x00},
+        {0x3724, 0x10},
+        {0x3726, 0x00},
+        {0x372a, 0x01},
+        {0x3730, 0x10},
+        {0x3738, 0x22},
+        {0x3739, 0xe5},
+        {0x373a, 0x50},
+        {0x373b, 0x02},
+        {0x373c, 0x41},
+        {0x373f, 0x02},
+        {0x3740, 0x42},
+        {0x3741, 0x02},
+        {0x3742, 0x18},
+        {0x3743, 0x01},
+        {0x3744, 0x02},
+        {0x3747, 0x10},
+        {0x374c, 0x04},
+        {0x3751, 0xf0},
+        {0x3752, 0x00},
+        {0x3753, 0x00},
+        {0x3754, 0xc0},
+        {0x3755, 0x00},
+        {0x3756, 0x1a},
+        {0x3758, 0x00},
+        {0x3759, 0x0f},
+        {0x376b, 0x44},
+        {0x375c, 0x04},
+        {0x3774, 0x10},
+        {0x3776, 0x00},
+        {0x377f, 0x08},
+        {0x3780, 0x22},
+        {0x3781, 0x0c},
+        {0x3784, 0x2c},
+        {0x3785, 0x1e},
+        {0x378f, 0xf5},
+        {0x3791, 0xb0},
+        {0x3795, 0x00},
+        {0x3796, 0x64},
+        {0x3797, 0x11},
+        {0x3798, 0x30},
+        {0x3799, 0x41},
+        {0x379a, 0x07},
+        {0x379b, 0xb0},
+        {0x379c, 0x0c},
+        {0x37c5, 0x00},
+        {0x37c6, 0x00},
+        {0x37c7, 0x00},
+        {0x37c9, 0x00},
+        {0x37ca, 0x00},
+        {0x37cb, 0x00},
+        {0x37de, 0x00},
+        {0x37df, 0x00},
+        {0x3800, 0x01},
+        {0x3801, 0x70},
+        {0x3802, 0x01},
+        {0x3803, 0xbc},
+        {0x3804, 0x09},
+        {0x3805, 0x0f},
+        {0x3806, 0x05},
+        {0x3807, 0xff},
+        {0x3808, 0x07},
+        {0x3809, 0x80},
+        {0x380a, 0x04},
+        {0x380b, 0x38},
+        {0x380c, 0x0b},
+        {0x380d, 0x40},
+        {0x380e, 0x07},
+        {0x380f, 0x3a},
+        {0x3810, 0x00},
+        {0x3811, 0x02},
+        {0x3812, 0x00},
+        {0x3813, 0x02},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x9e},
+        {0x3823, 0x00},
+        {0x3824, 0x00},
+        {0x3825, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x382a, 0x04},
+        {0x3a04, 0x09},
+        {0x3a05, 0xa9},
+        {0x3a06, 0x00},
+        {0x3a07, 0xfe},
+        {0x3b00, 0x00},
+        {0x3b02, 0x00},
+        {0x3b03, 0x00},
+        {0x3b04, 0x00},
+        {0x3b05, 0x00},
+        {0x3e07, 0x20},
+        {0x4000, 0x08},
+        {0x4001, 0x04},
+        {0x4002, 0x45},
+        {0x4004, 0x08},
+        {0x4005, 0x18},
+        {0x4006, 0x20},
+        {0x4008, 0x24},
+        {0x4009, 0x10},
+        {0x400c, 0x00},
+        {0x400d, 0x00},
+        {0x4058, 0x00},
+        {0x404e, 0x37},
+        {0x404f, 0x8f},
+        {0x4058, 0x00},
+        {0x4101, 0xb2},
+        {0x4303, 0x00},
+        {0x4304, 0x08},
+        {0x4307, 0x30},
+        {0x4311, 0x04},
+        {0x4315, 0x01},
+        {0x4511, 0x05},
+        {0x4512, 0x01},
+        {0x4800, 0x20}, /* dis-continuous */
+        {0x4806, 0x00},
+        {0x4816, 0x52},
+        {0x481f, 0x30},
+        {0x4826, 0x32},
+        {0x4831, 0x6a},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d02, 0xfd},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4837, 0x0a},
+        {0x5000, 0x06},
+        {0x5001, 0x01},
+        {0x5002, 0x00},
+        {0x5003, 0x20},
+        {0x5046, 0x0a},
+        {0x5013, 0x00},
+        {0x5046, 0x0a},
+        {0x5780, 0x1c},
+        {0x5786, 0x20},
+        {0x5787, 0x10},
+        {0x5788, 0x18},
+        {0x578a, 0x04},
+        {0x578b, 0x02},
+        {0x578c, 0x02},
+        {0x578e, 0x06},
+        {0x578f, 0x02},
+        {0x5790, 0x02},
+        {0x5791, 0xff},
+        {0x5842, 0x01},
+        {0x5843, 0x2b},
+        {0x5844, 0x01},
+        {0x5845, 0x92},
+        {0x5846, 0x01},
+        {0x5847, 0x8f},
+        {0x5848, 0x01},
+        {0x5849, 0x0c},
+        {0x5e00, 0x00},
+        {0x5e10, 0x0c},
+        {OV5693_TABLE_END, 0x0000}
+};
+
+static const ov5693_reg mode_1280x720_HDR_60fps[] = {
+        {OV5693_TABLE_WAIT_MS, OV5693_WAIT_MS},
+        {0x0100, 0x00},/* Including sw reset */
+        {0x0103, 0x01},
+        {0x3001, 0x0a},
+        {0x3002, 0x80},
+        {0x3006, 0x00},
+        {0x3011, 0x21},
+        {0x3012, 0x09},
+        {0x3013, 0x10},
+        {0x3014, 0x00},
+        {0x3015, 0x08},
+        {0x3016, 0xf0},
+        {0x3017, 0xf0},
+        {0x3018, 0xf0},
+        {0x301b, 0xb4},
+        {0x301d, 0x02},
+        {0x3021, 0x00},
+        {0x3022, 0x01},
+        {0x3028, 0x44},
+        {0x3098, 0x03},
+        {0x3099, 0x1e},
+        {0x309a, 0x02},
+        {0x309b, 0x01},
+        {0x309c, 0x00},
+        {0x30a0, 0xd2},
+        {0x30a2, 0x01},
+        {0x30b2, 0x00},
+        {0x30b3, 0x68},
+        {0x30b4, 0x03},
+        {0x30b5, 0x04},
+        {0x30b6, 0x01},
+        {0x3104, 0x21},
+        {0x3106, 0x00},
+        {0x3406, 0x01},
+        {0x3500, 0x00},
+        {0x3501, 0x39},
+        {0x3502, 0x00},
+        {0x3503, 0x07},
+        {0x3504, 0x00},
+        {0x3505, 0x00},
+        {0x3506, 0x00},
+        {0x3507, 0x01},
+        {0x3508, 0x80},
+        {0x3509, 0x10},
+        {0x350a, 0x00},
+        {0x350b, 0x40},
+        {0x3601, 0x0a},
+        {0x3602, 0x38},
+        {0x3612, 0x80},
+        {0x3620, 0x54},
+        {0x3621, 0xc7},
+        {0x3622, 0x0f},
+        {0x3625, 0x10},
+        {0x3630, 0x55},
+        {0x3631, 0xf4},
+        {0x3632, 0x00},
+        {0x3633, 0x34},
+        {0x3634, 0x02},
+        {0x364d, 0x0d},
+        {0x364f, 0xdd},
+        {0x3660, 0x04},
+        {0x3662, 0x10},
+        {0x3663, 0xf1},
+        {0x3665, 0x00},
+        {0x3666, 0x20},
+        {0x3667, 0x00},
+        {0x366a, 0x80},
+        {0x3680, 0xe0},
+        {0x3681, 0x00},
+        {0x3700, 0x42},
+        {0x3701, 0x14},
+        {0x3702, 0xa0},
+        {0x3703, 0xd8},
+        {0x3704, 0x78},
+        {0x3705, 0x02},
+        {0x3708, 0xe2},
+        {0x3709, 0xc3},
+        {0x370a, 0x00},
+        {0x370b, 0x20},
+        {0x370c, 0x0c},
+        {0x370d, 0x11},
+        {0x370e, 0x00},
+        {0x370f, 0x40},
+        {0x3710, 0x00},
+        {0x371a, 0x1c},
+        {0x371b, 0x05},
+        {0x371c, 0x01},
+        {0x371e, 0xa1},
+        {0x371f, 0x0c},
+        {0x3721, 0x00},
+        {0x3724, 0x10},
+        {0x3726, 0x00},
+        {0x372a, 0x01},
+        {0x3730, 0x10},
+        {0x3738, 0x22},
+        {0x3739, 0xe5},
+        {0x373a, 0x50},
+        {0x373b, 0x02},
+        {0x373c, 0x41},
+        {0x373f, 0x02},
+        {0x3740, 0x42},
+        {0x3741, 0x02},
+        {0x3742, 0x18},
+        {0x3743, 0x01},
+        {0x3744, 0x02},
+        {0x3747, 0x10},
+        {0x374c, 0x04},
+        {0x3751, 0xf0},
+        {0x3752, 0x00},
+        {0x3753, 0x00},
+        {0x3754, 0xc0},
+        {0x3755, 0x00},
+        {0x3756, 0x1a},
+        {0x3758, 0x00},
+        {0x3759, 0x0f},
+        {0x376b, 0x44},
+        {0x375c, 0x04},
+        {0x3774, 0x10},
+        {0x3776, 0x00},
+        {0x377f, 0x08},
+        {0x3780, 0x22},
+        {0x3781, 0x0c},
+        {0x3784, 0x2c},
+        {0x3785, 0x1e},
+        {0x378f, 0xf5},
+        {0x3791, 0xb0},
+        {0x3795, 0x00},
+        {0x3796, 0x64},
+        {0x3797, 0x11},
+        {0x3798, 0x30},
+        {0x3799, 0x41},
+        {0x379a, 0x07},
+        {0x379b, 0xb0},
+        {0x379c, 0x0c},
+        {0x37c5, 0x00},
+        {0x37c6, 0x00},
+        {0x37c7, 0x00},
+        {0x37c9, 0x00},
+        {0x37ca, 0x00},
+        {0x37cb, 0x00},
+        {0x37de, 0x00},
+        {0x37df, 0x00},
+        {0x3800, 0x02},
+        {0x3801, 0xa8},
+        {0x3802, 0x02},
+        {0x3803, 0x68},
+        {0x3804, 0x07},
+        {0x3805, 0xb7},
+        {0x3806, 0x05},
+        {0x3807, 0x3b},
+        {0x3808, 0x05},
+        {0x3809, 0x00},
+        {0x380a, 0x02},
+        {0x380b, 0xd0},
+        {0x380c, 0x0b},
+        {0x380d, 0x40},
+        {0x380e, 0x03},
+        {0x380f, 0x9e},
+        {0x3810, 0x00},
+        {0x3811, 0x02},
+        {0x3812, 0x00},
+        {0x3813, 0x02},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x9e},
+        {0x3823, 0x00},
+        {0x3824, 0x00},
+        {0x3825, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x382a, 0x04},
+        {0x3a04, 0x09},
+        {0x3a05, 0xa9},
+        {0x3a06, 0x00},
+        {0x3a07, 0xfe},
+        {0x3b00, 0x00},
+        {0x3b02, 0x00},
+        {0x3b03, 0x00},
+        {0x3b04, 0x00},
+        {0x3b05, 0x00},
+        {0x3e07, 0x20},
+        {0x4000, 0x08},
+        {0x4001, 0x04},
+        {0x4002, 0x45},
+        {0x4004, 0x08},
+        {0x4005, 0x18},
+        {0x4006, 0x20},
+        {0x4008, 0x24},
+        {0x4009, 0x10},
+        {0x400c, 0x00},
+        {0x400d, 0x00},
+        {0x4058, 0x00},
+        {0x404e, 0x37},
+        {0x404f, 0x8f},
+        {0x4058, 0x00},
+        {0x4101, 0xb2},
+        {0x4303, 0x00},
+        {0x4304, 0x08},
+        {0x4307, 0x30},
+        {0x4311, 0x04},
+        {0x4315, 0x01},
+        {0x4511, 0x05},
+        {0x4512, 0x01},
+        {0x4800, 0x20}, /* dis-continuous */
+        {0x4806, 0x00},
+        {0x4816, 0x52},
+        {0x481f, 0x30},
+        {0x4826, 0x32},
+        {0x4831, 0x6a},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d02, 0xfd},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4837, 0x0a},
+        {0x5000, 0x06},
+        {0x5001, 0x01},
+        {0x5002, 0x00},
+        {0x5003, 0x20},
+        {0x5046, 0x0a},
+        {0x5013, 0x00},
+        {0x5046, 0x0a},
+        {0x5780, 0x1c},
+        {0x5786, 0x20},
+        {0x5787, 0x10},
+        {0x5788, 0x18},
+        {0x578a, 0x04},
+        {0x578b, 0x02},
+        {0x578c, 0x02},
+        {0x578e, 0x06},
+        {0x578f, 0x02},
+        {0x5790, 0x02},
+        {0x5791, 0xff},
+        {0x5842, 0x01},
+        {0x5843, 0x2b},
+        {0x5844, 0x01},
+        {0x5845, 0x92},
+        {0x5846, 0x01},
+        {0x5847, 0x8f},
+        {0x5848, 0x01},
+        {0x5849, 0x0c},
+        {0x5e00, 0x00},
+        {0x5e10, 0x0c},
+        {OV5693_TABLE_END, 0x0000}
+};
+
+static const ov5693_reg mode_2592x1944_one_lane_15fps[] = {
+        {0x0100, 0x00},/* Including sw reset */
+        {0x0103, 0x01},
+        {0x3001, 0x0a},
+        {0x3002, 0x80},
+        {0x3006, 0x00},
+        {0x3011, 0x11},
+        {0x3012, 0x09},
+        {0x3013, 0x10},
+        {0x3014, 0x00},
+        {0x3015, 0x28},
+        {0x3016, 0xf0},
+        {0x3017, 0xf0},
+        {0x3018, 0xf0},
+        {0x301b, 0xb4},
+        {0x301d, 0x02},
+        {0x3021, 0x00},
+        {0x3022, 0x01},
+        {0x3028, 0x44},
+        {0x3098, 0x03},
+        {0x3099, 0x1e},
+        {0x309a, 0x05},
+        {0x309b, 0x01},
+        {0x309c, 0x00},
+        {0x30a0, 0xd2},
+        {0x30a2, 0x01},
+        {0x30b2, 0x00},
+        {0x30b3, 0x64},
+        {0x30b4, 0x03},
+        {0x30b5, 0x04},
+        {0x30b6, 0x01},
+        {0x3104, 0x21},
+        {0x3106, 0x00},
+        {0x3400, 0x04},
+        {0x3401, 0x00},
+        {0x3402, 0x04},
+        {0x3403, 0x00},
+        {0x3404, 0x04},
+        {0x3405, 0x00},
+        {0x3406, 0x01},
+        {0x3500, 0x00},
+        {0x3501, 0x7b},
+        {0x3502, 0x00},
+        {0x3503, 0x07},
+        {0x3504, 0x00},
+        {0x3505, 0x00},
+        {0x3506, 0x00},
+        {0x3507, 0x02},
+        {0x3508, 0x00},
+        {0x3509, 0x10},
+        {0x350a, 0x00},
+        {0x350b, 0x20},
+        {0x3600, 0xbc},
+        {0x3601, 0x0a},
+        {0x3602, 0x38},
+        {0x3612, 0x80},
+        {0x3620, 0x44},
+        {0x3621, 0xb5},
+        {0x3622, 0x0c},
+        {0x3625, 0x10},
+        {0x3630, 0x55},
+        {0x3631, 0xf4},
+        {0x3632, 0x00},
+        {0x3633, 0x34},
+        {0x3634, 0x02},
+        {0x364d, 0x0d},
+        {0x364f, 0xdd},
+        {0x3660, 0x04},
+        {0x3662, 0x10},
+        {0x3663, 0xf1},
+        {0x3665, 0x00},
+        {0x3666, 0x20},
+        {0x3667, 0x00},
+        {0x366a, 0x80},
+        {0x3680, 0xe0},
+        {0x3681, 0x00},
+        {0x3700, 0x42},
+        {0x3701, 0x14},
+        {0x3702, 0xa0},
+        {0x3703, 0xd8},
+        {0x3704, 0x78},
+        {0x3705, 0x02},
+        {0x3708, 0xe2},
+        {0x3709, 0xc3},
+        {0x370a, 0x00},
+        {0x370b, 0x20},
+        {0x370c, 0x0c},
+        {0x370d, 0x11},
+        {0x370e, 0x00},
+        {0x370f, 0x40},
+        {0x3710, 0x00},
+        {0x371a, 0x1c},
+        {0x371b, 0x05},
+        {0x371c, 0x01},
+        {0x371e, 0xa1},
+        {0x371f, 0x0c},
+        {0x3721, 0x00},
+        {0x3724, 0x10},
+        {0x3726, 0x00},
+        {0x372a, 0x01},
+        {0x3730, 0x10},
+        {0x3738, 0x22},
+        {0x3739, 0xe5},
+        {0x373a, 0x50},
+        {0x373b, 0x02},
+        {0x373c, 0x41},
+        {0x373f, 0x02},
+        {0x3740, 0x42},
+        {0x3741, 0x02},
+        {0x3742, 0x18},
+        {0x3743, 0x01},
+        {0x3744, 0x02},
+        {0x3747, 0x10},
+        {0x374c, 0x04},
+        {0x3751, 0xf0},
+        {0x3752, 0x00},
+        {0x3753, 0x00},
+        {0x3754, 0xc0},
+        {0x3755, 0x00},
+        {0x3756, 0x1a},
+        {0x3758, 0x00},
+        {0x3759, 0x0f},
+        {0x376b, 0x44},
+        {0x375c, 0x04},
+        {0x3774, 0x10},
+        {0x3776, 0x00},
+        {0x377f, 0x08},
+        {0x3780, 0x22},
+        {0x3781, 0x0c},
+        {0x3784, 0x2c},
+        {0x3785, 0x1e},
+        {0x378f, 0xf5},
+        {0x3791, 0xb0},
+        {0x3795, 0x00},
+        {0x3796, 0x64},
+        {0x3797, 0x11},
+        {0x3798, 0x30},
+        {0x3799, 0x41},
+        {0x379a, 0x07},
+        {0x379b, 0xb0},
+        {0x379c, 0x0c},
+        {0x37c5, 0x00},
+        {0x37c6, 0x00},
+        {0x37c7, 0x00},
+        {0x37c9, 0x00},
+        {0x37ca, 0x00},
+        {0x37cb, 0x00},
+        {0x37de, 0x00},
+        {0x37df, 0x00},
+        {0x3800, 0x00},
+        {0x3801, 0x00},
+        {0x3802, 0x00},
+        {0x3803, 0x00},
+        {0x3804, 0x0a},
+        {0x3805, 0x3f},
+        {0x3806, 0x07},
+        {0x3807, 0xa3},
+        {0x3808, 0x0a},
+        {0x3809, 0x20},
+        {0x380a, 0x07},
+        {0x380b, 0x98},
+        {0x380c, 0x0a},
+        {0x380d, 0x80},
+        {0x380e, 0x07},
+        {0x380f, 0xc0},
+        {0x3810, 0x00},
+        {0x3811, 0x02},
+        {0x3812, 0x00},
+        {0x3813, 0x02},
+        {0x3814, 0x11},
+        {0x3815, 0x11},
+        {0x3820, 0x00},
+        {0x3821, 0x1e},
+        {0x3823, 0x00},
+        {0x3824, 0x00},
+        {0x3825, 0x00},
+        {0x3826, 0x00},
+        {0x3827, 0x00},
+        {0x382a, 0x04},
+        {0x3a04, 0x06},
+        {0x3a05, 0x14},
+        {0x3a06, 0x00},
+        {0x3a07, 0xfe},
+        {0x3b00, 0x00},
+        {0x3b02, 0x00},
+        {0x3b03, 0x00},
+        {0x3b04, 0x00},
+        {0x3b05, 0x00},
+        {0x3e07, 0x20},
+        {0x4000, 0x08},
+        {0x4001, 0x04},
+        {0x4002, 0x45},
+        {0x4004, 0x08},
+        {0x4005, 0x18},
+        {0x4006, 0x20},
+        {0x4008, 0x24},
+        {0x4009, 0x10},
+        {0x400c, 0x00},
+        {0x400d, 0x00},
+        {0x4058, 0x00},
+        {0x404e, 0x37},
+        {0x404f, 0x8f},
+        {0x4058, 0x00},
+        {0x4101, 0xb2},
+        {0x4303, 0x00},
+        {0x4304, 0x08},
+        {0x4307, 0x30},
+        {0x4311, 0x04},
+        {0x4315, 0x01},
+        {0x4511, 0x05},
+        {0x4512, 0x01},
+        {0x4806, 0x00},
+        {0x4816, 0x52},
+        {0x481f, 0x30},
+        {0x4826, 0x2c},
+        {0x4831, 0x64},
+        {0x4d00, 0x04},
+        {0x4d01, 0x71},
+        {0x4d02, 0xfd},
+        {0x4d03, 0xf5},
+        {0x4d04, 0x0c},
+        {0x4d05, 0xcc},
+        {0x4837, 0x0a},
+        {0x5000, 0x06},
+        {0x5001, 0x01},
+        {0x5002, 0x00},
+        {0x5003, 0x20},
+        {0x5046, 0x0a},
+        {0x5013, 0x00},
+        {0x5046, 0x0a},
+        {0x5780, 0xfc},
+        {0x5781, 0x13},
+        {0x5782, 0x03},
+        {0x5786, 0x20},
+        {0x5787, 0x40},
+        {0x5788, 0x08},
+        {0x5789, 0x08},
+        {0x578a, 0x02},
+        {0x578b, 0x01},
+        {0x578c, 0x01},
+        {0x578d, 0x0c},
+        {0x578e, 0x02},
+        {0x578f, 0x01},
+        {0x5790, 0x01},
+        {0x5791, 0xff},
+        {0x5842, 0x01},
+        {0x5843, 0x2b},
+        {0x5844, 0x01},
+        {0x5845, 0x92},
+        {0x5846, 0x01},
+        {0x5847, 0x8f},
+        {0x5848, 0x01},
+        {0x5849, 0x0c},
+        {0x5e00, 0x00},
+        {0x5e10, 0x0c},
+        {0x0100, 0x01},
+        {0x3810, 0x00},
+        {0x3811, 0x10},
+        {0x3812, 0x00},
+        {0x3813, 0x06},
+        {OV5693_TABLE_END, 0x0000}
+};
+
+enum {
+        OV5693_MODE_2592X1944,
+        OV5693_MODE_2592X1458,
+        OV5693_MODE_1920X1080,
+        OV5693_MODE_1280X720_120FPS,
+        OV5693_MODE_2592X1944_HDR,
+        OV5693_MODE_1920X1080_HDR,
+        OV5693_MODE_2592x1944_15FPS,
+
+        OV5693_MODE_START_STREAM,
+        OV5693_MODE_STOP_STREAM,
+        OV5693_MODE_TEST_PATTERN
+};
+
+static const ov5693_reg *mode_table[] = {
+        [OV5693_MODE_2592X1944]                 = mode_2592x1944,
+        [OV5693_MODE_2592X1458]                 = mode_2592x1458,
+        [OV5693_MODE_1920X1080]                 = mode_1920x1080,
+        [OV5693_MODE_1280X720_120FPS]           = mode_1280x720_120fps,
+        [OV5693_MODE_2592X1944_HDR]             = mode_2592x1944_HDR_24fps,
+        [OV5693_MODE_1920X1080_HDR]             = mode_1920x1080_HDR_30fps,
+        [OV5693_MODE_2592x1944_15FPS]           = mode_2592x1944_one_lane_15fps,
+
+        [OV5693_MODE_START_STREAM]              = ov5693_start,
+        [OV5693_MODE_STOP_STREAM]               = ov5693_stop,
+        [OV5693_MODE_TEST_PATTERN]              = tp_colorbars,
+};
+
+static const int ov5693_15fps[] = {
+        15,
+};
+
+static const int ov5693_24fps[] = {
+        24,
+};
+
+static const int ov5693_30fps[] = {
+        30,
+};
+
+static const int ov5693_120fps[] = {
+        120,
+};
+
+static const struct camera_common_frmfmt ov5693_frmfmt[] = {
+        {{2592, 1944},  ov5693_30fps,   1, 0,   OV5693_MODE_2592X1944},
+        {{2592, 1458},  ov5693_30fps,   1, 0,   OV5693_MODE_2592X1458},
+        {{1920, 1080},  ov5693_30fps,   1, 0,   OV5693_MODE_1920X1080},
+        {{1280, 720},   ov5693_120fps,  1, 0,   OV5693_MODE_1280X720_120FPS},
+        {{2592, 1944},  ov5693_24fps,   1, 1,   OV5693_MODE_2592X1944_HDR},
+        {{1920, 1080},  ov5693_30fps,   1, 1,   OV5693_MODE_1920X1080_HDR},
+        {{2592, 1944},  ov5693_15fps,   1, 1,   OV5693_MODE_2592x1944_15FPS},
+};
+#endif  /* __OV5693_TABLES__ */
+
diff --git a/drivers/media/i2c/ov9281.c b/drivers/media/i2c/ov9281.c
new file mode 100644
index 000000000..acbec072f
--- /dev/null
+++ b/drivers/media/i2c/ov9281.c
@@ -0,0 +1,1216 @@
+/*
+ * ov9281.c - ov9281 sensor driver
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+
+#include <linux/seq_file.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <media/tegra_v4l2_camera.h>
+#include <media/camera_common.h>
+
+#include "ov9281_mode_tbls.h"
+
+/* OV9281 Registers */
+#define OV9281_SC_MODE_SELECT_ADDR      0x0100
+#define OV9281_SC_MODE_SELECT_STREAMING 0x01
+#define OV9281_SC_CHIP_ID_HIGH_ADDR     0x300A
+#define OV9281_SC_CHIP_ID_LOW_ADDR      0x300B
+#define OV9281_SC_CTRL_SCCB_ID_ADDR     0x302B
+#define OV9281_SC_CTRL_3B_ADDR          0x303B
+#define OV9281_SC_CTRL_3B_SCCB_ID2_NACK_EN      (1 << 0)
+#define OV9281_SC_CTRL_3B_SCCB_PGM_ID_EN        (1 << 1)
+
+#define OV9281_GROUP_HOLD_ADDR          0x3208
+#define OV9281_GROUP_HOLD_START         0x00
+#define OV9281_GROUP_HOLD_END           0x10
+#define OV9281_GROUP_HOLD_LAUNCH_LBLANK 0x60
+#define OV9281_GROUP_HOLD_LAUNCH_VBLANK 0xA0
+#define OV9281_GROUP_HOLD_LAUNCH_IMMED  0xE0
+#define OV9281_GROUP_HOLD_BANK_0        0x00
+#define OV9281_GROUP_HOLD_BANK_1        0x01
+
+#define OV9281_EXPO_HIGH_ADDR           0x3500
+#define OV9281_EXPO_MID_ADDR            0x3501
+#define OV9281_EXPO_LOW_ADDR            0x3502
+
+#define OV9281_GAIN_SHIFT_ADDR          0x3507
+#define OV9281_GAIN_HIGH_ADDR           0x3508
+#define OV9281_GAIN_LOW_ADDR            0x3509
+
+#define OV9281_TIMING_VTS_HIGH_ADDR     0x380E
+#define OV9281_TIMING_VTS_LOW_ADDR      0x380F
+#define OV9281_TIMING_RST_FSIN_HIGH_ADDR        0x3826
+#define OV9281_TIMING_RST_FSIN_LOW_ADDR 0x3827
+
+#define OV9281_OTP_BUFFER_ADDR          0x3D00
+#define OV9281_OTP_BUFFER_SIZE          32
+#define OV9281_OTP_STR_SIZE             (OV9281_OTP_BUFFER_SIZE * 2)
+#define OV9281_FUSE_ID_OTP_BUFFER_ADDR  0x3D00
+#define OV9281_FUSE_ID_OTP_BUFFER_SIZE  16
+#define OV9281_FUSE_ID_STR_SIZE         (OV9281_FUSE_ID_OTP_BUFFER_SIZE * 2)
+#define OV9281_OTP_PROGRAM_CTRL_ADDR    0x3D80
+#define OV9281_OTP_LOAD_CTRL_ADDR       0x3D81
+#define OV9281_OTP_LOAD_CTRL_OTP_RD     0x01
+
+#define OV9281_PRE_CTRL00_ADDR          0x5E00
+#define OV9281_PRE_CTRL00_TEST_PATTERN_EN       (1 << 7)
+
+/* OV9281 Other Stuffs */
+#define OV9281_DEFAULT_GAIN             0x0010 /* 1.0x real gain */
+#define OV9281_MIN_GAIN                 0x0001
+#define OV9281_MAX_GAIN                 0x1FFF
+
+#define OV9281_DEFAULT_FRAME_LENGTH     0x038E
+#define OV9281_MIN_FRAME_LENGTH         0x0001
+#define OV9281_MAX_FRAME_LENGTH         0xFFFF
+
+#define OV9281_MIN_EXPOSURE_COARSE      0x00000001
+#define OV9281_MAX_EXPOSURE_COARSE      0x000FFFFF
+#define OV9281_DEFAULT_EXPOSURE_COARSE  0x00002A90
+
+#define OV9281_DEFAULT_MODE             OV9281_MODE_1280X800
+#define OV9281_DEFAULT_WIDTH            1280
+#define OV9281_DEFAULT_HEIGHT           800
+#define OV9281_DEFAULT_DATAFMT          MEDIA_BUS_FMT_SRGGB10_1X10
+#define OV9281_DEFAULT_CLK_FREQ         26000000
+
+#define OV9281_DEFAULT_I2C_ADDRESS_C0           (0xc0 >> 1)
+#define OV9281_DEFAULT_I2C_ADDRESS_20           (0x20 >> 1)
+#define OV9281_DEFAULT_I2C_ADDRESS_PROGRAMMABLE (0xe0 >> 1)
+
+struct ov9281 {
+        struct camera_common_power_rail power;
+        int                             num_ctrls;
+        int                             fsync;
+        int                             cam_sid_gpio;
+        int                             mcu_boot_gpio;
+        int                             mcu_reset_gpio;
+        struct v4l2_ctrl_handler        ctrl_handler;
+        struct i2c_client               *i2c_client;
+        struct v4l2_subdev              *subdev;
+        struct media_pad                pad;
+
+        s32                             group_hold_prev;
+        bool                            group_hold_en;
+        struct regmap                   *regmap;
+        struct camera_common_data       *s_data;
+        struct camera_common_pdata      *pdata;
+        struct v4l2_ctrl                *ctrls[];
+};
+
+/* Register/regmap stuff */
+static int ov9281_read_reg(struct camera_common_data *s_data, u16 addr, u8 *val)
+{
+        struct ov9281 *priv = (struct ov9281 *)s_data->priv;
+        unsigned int temp_val;
+        int err;
+
+        err = regmap_read(priv->regmap, addr, &temp_val);
+        if (!err)
+                *val = temp_val;
+
+        return err;
+}
+
+static int ov9281_write_reg(struct camera_common_data *s_data, u16 addr, u8 val)
+{
+        struct ov9281 *priv = (struct ov9281 *)s_data->priv;
+        int err;
+
+        err = regmap_write(priv->regmap, addr, val);
+        if (err)
+                dev_err(&priv->i2c_client->dev,
+                        "%s:i2c write failed, %x = %x\n", __func__, addr, val);
+
+        return err;
+}
+
+static int ov9281_write_table(struct ov9281 *priv, const ov9281_reg table[])
+{
+        return regmap_util_write_table_8(priv->regmap, table, NULL, 0,
+                                         OV9281_TABLE_WAIT_MS,
+                                         OV9281_TABLE_END);
+}
+
+static const struct regmap_config ov9281_regmap_config = {
+        .reg_bits = 16,
+        .val_bits = 8,
+        .cache_type = REGCACHE_RBTREE,
+        .use_single_rw = true,
+};
+
+/* NVIDIA camera_common stuff */
+static int ov9281_power_on(struct camera_common_data *s_data)
+{
+        struct ov9281 *priv = (struct ov9281 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+        int err;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power on\n", __func__);
+
+        if (priv->pdata->power_on) {
+                err = priv->pdata->power_on(pw);
+                if (err)
+                        dev_err(&priv->i2c_client->dev, "%s failed.\n",
+                                __func__);
+                else
+                        pw->state = SWITCH_ON;
+                return err;
+        }
+
+        usleep_range(5350, 5360);
+        pw->state = SWITCH_ON;
+        return 0;
+}
+
+static int ov9281_power_off(struct camera_common_data *s_data)
+{
+        struct ov9281 *priv = (struct ov9281 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+        int err = 0;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: power off\n", __func__);
+        ov9281_write_table(priv, ov9281_mode_table[OV9281_MODE_STOP_STREAM]);
+
+        if (priv->pdata->power_off) {
+                err = priv->pdata->power_off(pw);
+                if (err)
+                        dev_err(&priv->i2c_client->dev, "%s failed.\n",
+                                __func__);
+                else
+                        goto power_off_done;
+        }
+
+        return err;
+
+power_off_done:
+        pw->state = SWITCH_OFF;
+        return 0;
+}
+
+static int ov9281_power_put(struct ov9281 *priv)
+{
+        return 0;
+}
+
+static int ov9281_power_get(struct ov9281 *priv)
+{
+        struct camera_common_power_rail *pw = &priv->power;
+        const char *mclk_name;
+        int err = 0;
+
+        mclk_name = priv->pdata->mclk_name ?
+                    priv->pdata->mclk_name : "cam_mclk1";
+        pw->mclk = devm_clk_get(&priv->i2c_client->dev, mclk_name);
+        if (IS_ERR(pw->mclk)) {
+                dev_err(&priv->i2c_client->dev,
+                        "unable to get clock %s\n", mclk_name);
+                return PTR_ERR(pw->mclk);
+        }
+
+        pw->state = SWITCH_OFF;
+        return err;
+}
+
+static struct camera_common_sensor_ops ov9281_common_ops = {
+        .power_on = ov9281_power_on,
+        .power_off = ov9281_power_off,
+        .write_reg = ov9281_write_reg,
+        .read_reg = ov9281_read_reg,
+};
+
+/* Miscellaneous OV9281-specific stuff */
+static int ov9281_i2c_addr_assign(struct ov9281 *priv, u8 i2c_addr)
+{
+        struct i2c_msg msg;
+        unsigned char data[3];
+        int err;
+
+        /*
+         * I wish i2c_check_addr_validity() was available.  Oh well.
+         * 7-bit address, reject the general call address
+         */
+        if ((i2c_addr == 0x00) || (i2c_addr > 0x7f))
+                return -EINVAL;
+
+        /*
+         * It seems that the way SID works for the OV9281 I2C slave address is
+         * that:
+         *
+         * SID 0 = 0xc0, 0xe0
+         * SID 1 = 0x20, 0xe0
+         *
+         * Address 0xe0 is programmable via register 0x302B
+         * (OV9281_SC_CTRL_SCCB_ID_ADDR).
+         *
+         * So, the scheme to assign addresses to an (almost) arbitrary
+         * number of sensors is to consider 0x20 to be the "off" address.
+         * Start each sensor with SID as 1 so that they appear to be off.
+         *
+         * Then, to assign an address to one sensor:
+         *
+         * 0. Set corresponding SID to 0 (now only that sensor responds
+         *    to 0xc0).
+         * 1. Use 0xc0 to program the address from the default programmable
+         *    address of 0xe0 to the new address.
+         * 2. Set corresponding SID back to 1 (so it no longer responds
+         *    to 0xc0).
+         */
+
+        if (i2c_addr == OV9281_DEFAULT_I2C_ADDRESS_C0) {
+                dev_info(&priv->i2c_client->dev,
+                         "Using default I2C address 0x%02x\n", i2c_addr);
+                if (gpio_is_valid(priv->cam_sid_gpio)) {
+                        gpio_set_value(priv->cam_sid_gpio, 0);
+                        msleep_range(1);
+                }
+                return 0;
+        } else if (i2c_addr == OV9281_DEFAULT_I2C_ADDRESS_20) {
+                dev_info(&priv->i2c_client->dev,
+                         "Using default I2C address 0x%02x\n", i2c_addr);
+                if (gpio_is_valid(priv->cam_sid_gpio)) {
+                        gpio_set_value(priv->cam_sid_gpio, 1);
+                        msleep_range(1);
+                }
+                return 0;
+        } else if (i2c_addr == OV9281_DEFAULT_I2C_ADDRESS_PROGRAMMABLE) {
+                dev_info(&priv->i2c_client->dev,
+                         "Using default I2C address 0x%02x\n", i2c_addr);
+                return 0;
+        }
+
+        /*
+         * From this point on, we are trying to program the programmable
+         * slave address.  We necessarily need to have a cam-sid-gpio for this.
+         */
+        if (!gpio_is_valid(priv->cam_sid_gpio)) {
+                dev_err(&priv->i2c_client->dev,
+                        "Missing cam-sid-gpio, cannot program I2C address\n");
+                return -EINVAL;
+        }
+
+        gpio_set_value(priv->cam_sid_gpio, 0);
+        msleep_range(1);
+
+        dev_info(&priv->i2c_client->dev, "Changing I2C address to 0x%02x\n",
+                 i2c_addr);
+
+        /*
+         * Have to make the I2C message manually because we are using a
+         * different I2C slave address for this transaction, rather than
+         * the one in the device tree for this device.
+         */
+        data[0] = (OV9281_SC_CTRL_SCCB_ID_ADDR >> 8) & 0xff;
+        data[1] = OV9281_SC_CTRL_SCCB_ID_ADDR & 0xff;
+        data[2] = ((i2c_addr) << 1) & 0xff;
+
+        /*
+         * Use the programmable default I2C slave address so that if we have
+         * multiple sensors of this same kind, when we change one sensor's
+         * address, the next sensor address change message won't go to that
+         * same sensor.
+         */
+        msg.addr = OV9281_DEFAULT_I2C_ADDRESS_C0;
+        msg.flags = 0;
+        msg.len = 3;
+        msg.buf = data;
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                goto done;
+
+        if (i2c_transfer(priv->i2c_client->adapter, &msg, 1) != 1)
+                err = -EIO;
+
+        camera_common_s_power(priv->subdev, false);
+
+done:
+        gpio_set_value(priv->cam_sid_gpio, 1);
+        msleep_range(1);
+
+        return err;
+}
+
+static int ov9281_set_group_hold(struct ov9281 *priv)
+{
+        int gh_prev = switch_ctrl_qmenu[priv->group_hold_prev];
+        int err;
+
+        if (priv->group_hold_en == true && gh_prev == SWITCH_OFF) {
+                /* group hold start */
+                err = ov9281_write_reg(priv->s_data, OV9281_GROUP_HOLD_ADDR,
+                                       (OV9281_GROUP_HOLD_START |
+                                        OV9281_GROUP_HOLD_BANK_0));
+                if (err)
+                        goto fail;
+                priv->group_hold_prev = 1;
+        } else if (priv->group_hold_en == false && gh_prev == SWITCH_ON) {
+                /* group hold end */
+                err = ov9281_write_reg(priv->s_data, OV9281_GROUP_HOLD_ADDR,
+                                       (OV9281_GROUP_HOLD_END |
+                                        OV9281_GROUP_HOLD_BANK_0));
+                /* quick launch */
+                err |= ov9281_write_reg(priv->s_data,
+                                       OV9281_GROUP_HOLD_ADDR,
+                                       (OV9281_GROUP_HOLD_LAUNCH_VBLANK |
+                                        OV9281_GROUP_HOLD_BANK_0));
+                if (err)
+                        goto fail;
+                priv->group_hold_prev = 0;
+        }
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: Group hold control error\n", __func__);
+        return err;
+}
+
+static int ov9281_set_gain(struct ov9281 *priv, s32 val)
+{
+        ov9281_reg regs[4];
+        u16 gain;
+        int err;
+
+        if (val < OV9281_MIN_GAIN)
+                gain = OV9281_MIN_GAIN;
+        else if (val > OV9281_MAX_GAIN)
+                gain = OV9281_MAX_GAIN;
+        else
+                gain = val;
+
+        dev_dbg(&priv->i2c_client->dev, "%s: gain: %d\n", __func__, gain);
+
+        regs[0].addr = OV9281_GAIN_SHIFT_ADDR;
+        regs[0].val = 0x03;
+        regs[1].addr = OV9281_GAIN_HIGH_ADDR;
+        regs[1].val = gain >> 8;
+        regs[2].addr = OV9281_GAIN_LOW_ADDR;
+        regs[2].val = gain & 0xff;
+        regs[3].addr = OV9281_TABLE_END;
+        regs[3].val = 0;
+
+        ov9281_set_group_hold(priv);
+        err = ov9281_write_table(priv, regs);
+        if (err)
+                goto fail;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev, "%s: GAIN control error\n", __func__);
+        return err;
+}
+
+static int ov9281_set_frame_length(struct ov9281 *priv, s32 val)
+{
+        ov9281_reg regs[5];
+        u16 frame_length;
+        int err;
+
+        /*
+         * This is a workaround for nvbug 1865041, where setting the VTS
+         * timing registers when the sensor is set up for fsync master or
+         * slave leads to streaming instability.
+         */
+        if (priv->fsync != OV9281_FSYNC_NONE)
+                return 0;
+
+        frame_length = (u16)val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                "%s: frame_length: %d\n", __func__, frame_length);
+
+        regs[0].addr = OV9281_TIMING_VTS_HIGH_ADDR;
+        regs[0].val = (frame_length >> 8) & 0xff;
+        regs[1].addr = OV9281_TIMING_VTS_LOW_ADDR;
+        regs[1].val = (frame_length) & 0xff;
+        regs[2].addr = OV9281_TABLE_END;
+        regs[2].val = 0;
+
+        if (priv->fsync == OV9281_FSYNC_SLAVE) {
+                regs[2].addr = OV9281_TIMING_RST_FSIN_HIGH_ADDR;
+                regs[2].val = ((frame_length - 4) >> 8) & 0xff;
+                regs[3].addr = OV9281_TIMING_RST_FSIN_LOW_ADDR;
+                regs[3].val = (frame_length - 4) & 0xff;
+                regs[4].addr = OV9281_TABLE_END;
+                regs[4].val = 0;
+        }
+
+        ov9281_set_group_hold(priv);
+        err = ov9281_write_table(priv, regs);
+        if (err)
+                goto fail;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                "%s: FRAME_LENGTH control error\n", __func__);
+        return err;
+}
+
+static int ov9281_set_coarse_time(struct ov9281 *priv, s32 val)
+{
+        ov9281_reg regs[4];
+        u32 coarse_time;
+        int err;
+
+        coarse_time = (u32)val;
+
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: coarse_time: %d\n", __func__, coarse_time);
+
+        regs[0].addr = OV9281_EXPO_HIGH_ADDR;
+        regs[0].val = (coarse_time >> 16) & 0xff;
+        regs[1].addr = OV9281_EXPO_MID_ADDR;
+        regs[1].val = (coarse_time >> 8) & 0xff;
+        regs[2].addr = OV9281_EXPO_LOW_ADDR;
+        regs[2].val = (coarse_time & 0xff);
+        regs[3].addr = OV9281_TABLE_END;
+        regs[3].val = 0;
+
+        ov9281_set_group_hold(priv);
+        err = ov9281_write_table(priv, regs);
+        if (err)
+                goto fail;
+
+        return 0;
+
+fail:
+        dev_dbg(&priv->i2c_client->dev,
+                 "%s: COARSE_TIME control error\n", __func__);
+        return err;
+}
+
+/* OTP stuff */
+static int ov9281_read_otp(struct ov9281 *priv, u8 *buf, u16 addr, int size)
+{
+        int i;
+        int err;
+
+        err = ov9281_write_reg(priv->s_data, OV9281_SC_MODE_SELECT_ADDR,
+                               OV9281_SC_MODE_SELECT_STREAMING);
+        if (err)
+                return err;
+
+        for (i = 0; i < size; i++) {
+                err = ov9281_write_reg(priv->s_data, addr + i, 0x00);
+                if (err)
+                        return err;
+        }
+
+        err = ov9281_write_reg(priv->s_data, OV9281_OTP_LOAD_CTRL_ADDR,
+                               OV9281_OTP_LOAD_CTRL_OTP_RD);
+
+        msleep(20);
+
+        return regmap_bulk_read(priv->regmap, addr, buf, size);
+}
+
+static int ov9281_otp_setup(struct ov9281 *priv)
+{
+        struct v4l2_ctrl *ctrl;
+        u8 otp_buf[OV9281_OTP_BUFFER_SIZE];
+        int i;
+        int err;
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        err = ov9281_read_otp(priv, otp_buf, OV9281_OTP_BUFFER_ADDR,
+                              OV9281_OTP_BUFFER_SIZE);
+        if (err)
+                return err;
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_OTP_DATA);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return -EINVAL;
+        }
+
+        for (i = 0; i < OV9281_OTP_BUFFER_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x", otp_buf[i]);
+        ctrl->p_cur.p_char = ctrl->p_new.p_char;
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+static int ov9281_fuse_id_setup(struct ov9281 *priv)
+{
+        struct v4l2_ctrl *ctrl;
+        u8 fuse_id[OV9281_FUSE_ID_OTP_BUFFER_SIZE];
+        int i;
+        int err;
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        err = ov9281_read_otp(priv, fuse_id, OV9281_FUSE_ID_OTP_BUFFER_ADDR,
+                              OV9281_FUSE_ID_OTP_BUFFER_SIZE);
+        if (err)
+                return err;
+
+        ctrl = v4l2_ctrl_find(&priv->ctrl_handler, V4L2_CID_FUSE_ID);
+        if (!ctrl) {
+                dev_err(&priv->i2c_client->dev,
+                        "could not find device ctrl.\n");
+                return -EINVAL;
+        }
+
+        for (i = 0; i < OV9281_FUSE_ID_OTP_BUFFER_SIZE; i++)
+                sprintf(&ctrl->p_new.p_char[i*2], "%02x", fuse_id[i]);
+        ctrl->p_cur.p_char = ctrl->p_new.p_char;
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+/* V4L2 subdev stuff */
+static int ov9281_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov9281 *priv = (struct ov9281 *)s_data->priv;
+        struct v4l2_control control;
+        int err;
+
+        if (!enable) {
+                dev_dbg(&client->dev, "%s: stream off\n", __func__);
+                return ov9281_write_table(priv,
+                        ov9281_mode_table[OV9281_MODE_STOP_STREAM]);
+        }
+
+        dev_dbg(&client->dev, "%s: write mode table %d\n", __func__,
+                s_data->mode);
+        err = ov9281_write_table(priv, ov9281_mode_table[s_data->mode]);
+        if (err)
+                goto exit;
+
+        if (ov9281_fsync_table[priv->fsync]) {
+                dev_dbg(&client->dev, "%s: write fsync table %d\n", __func__,
+                        priv->fsync);
+                err = ov9281_write_table(priv, ov9281_fsync_table[priv->fsync]);
+                if (err)
+                        goto exit;
+        }
+
+        if (s_data->override_enable) {
+                /* write list of override regs for the asking frame length, */
+                /* coarse integration time, and gain. Failures to write */
+                /* overrides are non-fatal. */
+                control.id = V4L2_CID_GAIN;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov9281_set_gain(priv, control.value);
+                if (err)
+                        dev_warn(&client->dev,
+                                 "%s: error gain override\n", __func__);
+
+                control.id = V4L2_CID_FRAME_LENGTH;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov9281_set_frame_length(priv, control.value);
+                if (err)
+                        dev_warn(&client->dev,
+                                 "%s: error frame length override\n", __func__);
+
+                control.id = V4L2_CID_COARSE_TIME;
+                err = v4l2_g_ctrl(&priv->ctrl_handler, &control);
+                err |= ov9281_set_coarse_time(priv, control.value);
+                if (err)
+                        dev_warn(&client->dev,
+                                 "%s: error coarse time override\n", __func__);
+        }
+
+#ifdef TPG
+        err = ov9281_write_reg(priv->s_data, OV9281_PRE_CTRL00_ADDR,
+                               OV9281_PRE_CTRL00_TEST_PATTERN_EN);
+        if (err)
+                dev_warn(&client->dev, "%s: error enabling TPG\n", __func__);
+#endif
+
+        dev_dbg(&client->dev, "%s: stream on\n", __func__);
+        err = ov9281_write_table(priv,
+                ov9281_mode_table[OV9281_MODE_START_STREAM]);
+        if (err)
+                goto exit;
+
+        /*
+         * If the sensor is in fsync slave mode, and is in the middle of
+         * sending a frame when it gets a strobe on the fsin pin, it may
+         * prematurely end the frame, resulting in a short frame on our
+         * camera host.  So, after starting streaming, we assume fsync
+         * master has already been told to start streaming, and we wait some
+         * amount of time in order to skip the possible short frame.  The
+         * length of time to wait should be at least our sample period.
+         * Assume worse case of 120fps (8.3ms), and add a bit more.
+         */
+        if (priv->fsync == OV9281_FSYNC_SLAVE)
+                msleep_range(10);
+
+        return 0;
+
+exit:
+        dev_err(&client->dev, "%s: error setting stream\n", __func__);
+        return err;
+}
+
+static int ov9281_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov9281 *priv = (struct ov9281 *)s_data->priv;
+        struct camera_common_power_rail *pw = &priv->power;
+
+        *status = pw->state == SWITCH_ON;
+        return 0;
+}
+
+static int ov9281_set_fmt(struct v4l2_subdev *sd,
+                          struct v4l2_subdev_pad_config *cfg,
+                          struct v4l2_subdev_format *format)
+{
+        int err;
+
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+                err = camera_common_try_fmt(sd, &format->format);
+        else
+                err = camera_common_s_fmt(sd, &format->format);
+
+        return err;
+}
+
+static int ov9281_get_fmt(struct v4l2_subdev *sd,
+                          struct v4l2_subdev_pad_config *cfg,
+                          struct v4l2_subdev_format *format)
+{
+        return camera_common_g_fmt(sd, &format->format);
+}
+
+static struct v4l2_subdev_core_ops ov9281_subdev_core_ops = {
+        .s_power        = camera_common_s_power,
+};
+
+static struct v4l2_subdev_video_ops ov9281_subdev_video_ops = {
+        .s_stream       = ov9281_s_stream,
+        .g_mbus_config  = camera_common_g_mbus_config,
+        .g_input_status = ov9281_g_input_status,
+};
+
+static struct v4l2_subdev_pad_ops ov9281_subdev_pad_ops = {
+        .set_fmt        = ov9281_set_fmt,
+        .get_fmt        = ov9281_get_fmt,
+        .enum_mbus_code = camera_common_enum_mbus_code,
+        .enum_frame_size        = camera_common_enum_framesizes,
+        .enum_frame_interval    = camera_common_enum_frameintervals,
+};
+
+static struct v4l2_subdev_ops ov9281_subdev_ops = {
+        .core           = &ov9281_subdev_core_ops,
+        .video          = &ov9281_subdev_video_ops,
+        .pad            = &ov9281_subdev_pad_ops,
+};
+
+/* V4L2 controls stuff */
+static int ov9281_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct ov9281 *priv =
+                container_of(ctrl->handler, struct ov9281, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        default:
+                dev_err(&priv->i2c_client->dev,
+                        "%s: unknown ctrl id.\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static int ov9281_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct ov9281 *priv =
+                container_of(ctrl->handler, struct ov9281, ctrl_handler);
+        int err = 0;
+
+        if (priv->power.state == SWITCH_OFF)
+                return 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_GAIN:
+                err = ov9281_set_gain(priv, ctrl->val);
+                break;
+        case V4L2_CID_FRAME_LENGTH:
+                err = ov9281_set_frame_length(priv, ctrl->val);
+                break;
+        case V4L2_CID_COARSE_TIME:
+                err = ov9281_set_coarse_time(priv, ctrl->val);
+                break;
+        case V4L2_CID_GROUP_HOLD:
+                if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON) {
+                        priv->group_hold_en = true;
+                } else {
+                        priv->group_hold_en = false;
+                        err = ov9281_set_group_hold(priv);
+                }
+                break;
+        case V4L2_CID_HDR_EN:
+                break;
+        default:
+                dev_err(&priv->i2c_client->dev, "%s: unknown ctrl id.\n",
+                        __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static const struct v4l2_ctrl_ops ov9281_ctrl_ops = {
+        .g_volatile_ctrl = ov9281_g_volatile_ctrl,
+        .s_ctrl         = ov9281_s_ctrl,
+};
+
+static struct v4l2_ctrl_config ctrl_config_list[] = {
+/* Do not change the name field for the controls! */
+        {
+                .ops = &ov9281_ctrl_ops,
+                .id = V4L2_CID_GAIN,
+                .name = "Gain",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV9281_MIN_GAIN,
+                .max = OV9281_MAX_GAIN,
+                .def = OV9281_DEFAULT_GAIN,
+                .step = 1,
+        },
+        {
+                .ops = &ov9281_ctrl_ops,
+                .id = V4L2_CID_FRAME_LENGTH,
+                .name = "Frame Length",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV9281_MIN_FRAME_LENGTH,
+                .max = OV9281_MAX_FRAME_LENGTH,
+                .def = OV9281_DEFAULT_FRAME_LENGTH,
+                .step = 1,
+        },
+        {
+                .ops = &ov9281_ctrl_ops,
+                .id = V4L2_CID_COARSE_TIME,
+                .name = "Coarse Time",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .flags = V4L2_CTRL_FLAG_SLIDER,
+                .min = OV9281_MIN_EXPOSURE_COARSE,
+                .max = OV9281_MAX_EXPOSURE_COARSE,
+                .def = OV9281_DEFAULT_EXPOSURE_COARSE,
+                .step = 1,
+        },
+        {
+                .ops = &ov9281_ctrl_ops,
+                .id = V4L2_CID_GROUP_HOLD,
+                .name = "Group Hold",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &ov9281_ctrl_ops,
+                .id = V4L2_CID_HDR_EN,
+                .name = "HDR enable",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .def = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &ov9281_ctrl_ops,
+                .id = V4L2_CID_OTP_DATA,
+                .name = "OTP Data",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = OV9281_OTP_STR_SIZE,
+                .step = 2,
+        },
+        {
+                .ops = &ov9281_ctrl_ops,
+                .id = V4L2_CID_FUSE_ID,
+                .name = "Fuse ID",
+                .type = V4L2_CTRL_TYPE_STRING,
+                .flags = V4L2_CTRL_FLAG_READ_ONLY,
+                .min = 0,
+                .max = OV9281_FUSE_ID_STR_SIZE,
+                .step = 2,
+        },
+};
+
+static int ov9281_ctrls_init(struct ov9281 *priv)
+{
+        struct i2c_client *client = priv->i2c_client;
+        struct v4l2_ctrl *ctrl;
+        int num_ctrls;
+        int i;
+        int err;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        num_ctrls = ARRAY_SIZE(ctrl_config_list);
+        v4l2_ctrl_handler_init(&priv->ctrl_handler, num_ctrls);
+
+        for (i = 0; i < num_ctrls; i++) {
+                ctrl = v4l2_ctrl_new_custom(&priv->ctrl_handler,
+                        &ctrl_config_list[i], NULL);
+                if (ctrl == NULL) {
+                        dev_err(&client->dev, "Failed to init %s ctrl\n",
+                                ctrl_config_list[i].name);
+                        continue;
+                }
+
+                if (ctrl_config_list[i].type == V4L2_CTRL_TYPE_STRING &&
+                        ctrl_config_list[i].flags & V4L2_CTRL_FLAG_READ_ONLY) {
+                        ctrl->p_new.p_char = devm_kzalloc(&client->dev,
+                                ctrl_config_list[i].max + 1, GFP_KERNEL);
+                }
+                priv->ctrls[i] = ctrl;
+        }
+
+        priv->num_ctrls = num_ctrls;
+        priv->subdev->ctrl_handler = &priv->ctrl_handler;
+        if (priv->ctrl_handler.error) {
+                dev_err(&client->dev, "Error %d adding controls\n",
+                        priv->ctrl_handler.error);
+                err = priv->ctrl_handler.error;
+                goto error;
+        }
+
+        err = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
+        if (err) {
+                dev_err(&client->dev,
+                        "Error %d setting default controls\n", err);
+                goto error;
+        }
+
+        return 0;
+
+error:
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        return err;
+}
+
+static const struct media_entity_operations ov9281_media_ops = {
+        .link_validate  = v4l2_subdev_link_validate,
+};
+
+/* Driver probe helper stuff */
+static int ov9281_parse_dt(struct i2c_client *client, struct ov9281 *priv)
+{
+        struct device_node *np = client->dev.of_node;
+        const char *fsync_str;
+        int gpio;
+        int err;
+
+        err = of_property_read_string(np, "mclk", &priv->pdata->mclk_name);
+        if (err) {
+                dev_err(&client->dev, "mclk not in DT\n");
+                return -EINVAL;
+        }
+
+        err = of_property_read_string(np, "fsync", &fsync_str);
+        if (!err && fsync_str && (strcmp(fsync_str, "master") == 0))
+                priv->fsync = OV9281_FSYNC_MASTER;
+        else if (!err && fsync_str && (strcmp(fsync_str, "slave") == 0))
+                priv->fsync = OV9281_FSYNC_SLAVE;
+        else
+                priv->fsync = OV9281_FSYNC_NONE;
+
+        gpio = of_get_named_gpio(np, "pwdn-gpios", 0);
+        if (!gpio_is_valid(gpio)) {
+                dev_dbg(&client->dev, "pwdn gpios not in DT\n");
+                gpio = 0;
+        }
+        priv->pdata->pwdn_gpio = (unsigned int)gpio;
+
+        gpio = of_get_named_gpio(np, "reset-gpios", 0);
+        if (!gpio_is_valid(gpio)) {
+                dev_dbg(&client->dev, "reset gpios not in DT\n");
+                gpio = 0;
+        }
+        priv->pdata->reset_gpio = (unsigned int)gpio;
+
+        priv->mcu_boot_gpio =
+                of_get_named_gpio(np, "mcu-boot-gpios", 0);
+        priv->mcu_reset_gpio =
+                of_get_named_gpio(np, "mcu-reset-gpios", 0);
+
+        priv->cam_sid_gpio = of_get_named_gpio(np, "cam-sid-gpios", 0);
+
+        return 0;
+}
+
+static int ov9281_verify_chip_id(struct ov9281 *priv)
+{
+        struct i2c_client *client = priv->i2c_client;
+        struct camera_common_data *s_data = priv->s_data;
+        u8 chip_id_hi, chip_id_lo;
+        u16 chip_id;
+        int err;
+
+        err = camera_common_s_power(priv->subdev, true);
+        if (err)
+                return -ENODEV;
+
+        err = ov9281_read_reg(s_data, OV9281_SC_CHIP_ID_HIGH_ADDR, &chip_id_hi);
+        if (err) {
+                dev_err(&client->dev, "Failed to read chip ID\n");
+                return err;
+        }
+        err = ov9281_read_reg(s_data, OV9281_SC_CHIP_ID_LOW_ADDR, &chip_id_lo);
+        if (err) {
+                dev_err(&client->dev, "Failed to read chip ID\n");
+                return err;
+        }
+
+        chip_id = (chip_id_hi << 8) | chip_id_lo;
+        if (chip_id != 0x9281) {
+                dev_err(&client->dev, "Read unknown chip ID 0x%04x\n", chip_id);
+                return -EINVAL;
+        }
+
+        err = camera_common_s_power(priv->subdev, false);
+        if (err)
+                return -ENODEV;
+
+        return 0;
+}
+
+static const struct of_device_id ov9281_of_match[] = {
+        { .compatible = "nvidia,ov9281", },
+        { },
+};
+
+MODULE_DEVICE_TABLE(of, ov9281_of_match);
+
+static int ov9281_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct camera_common_data *common_data;
+        struct ov9281 *priv;
+        char dev_name[10];
+        int err;
+
+        dev_info(&client->dev, "Probing v4l2 sensor.\n");
+
+        common_data = devm_kzalloc(&client->dev,
+                                   sizeof(struct camera_common_data),
+                                   GFP_KERNEL);
+
+        priv = devm_kzalloc(&client->dev,
+                            sizeof(struct ov9281) +
+                            (sizeof(struct v4l2_ctrl *) *
+                             ARRAY_SIZE(ctrl_config_list)),
+                            GFP_KERNEL);
+        if (!priv) {
+                dev_err(&client->dev,
+                        "unable to allocate camera_common_data\n");
+                return -ENOMEM;
+        }
+
+        priv->regmap = devm_regmap_init_i2c(client, &ov9281_regmap_config);
+        if (IS_ERR(priv->regmap)) {
+                dev_err(&client->dev,
+                        "regmap init failed: %ld\n", PTR_ERR(priv->regmap));
+                return -ENODEV;
+        }
+
+        priv->pdata = devm_kzalloc(&client->dev,
+                                   sizeof(struct camera_common_pdata),
+                                   GFP_KERNEL);
+        if (!priv->pdata) {
+                dev_err(&client->dev,
+                        "unable to allocate camera_common_pdata\n");
+                return -ENOMEM;
+        }
+
+        err = ov9281_parse_dt(client, priv);
+        if (err)
+                return err;
+
+        common_data->ops                = &ov9281_common_ops;
+        common_data->ctrl_handler       = &priv->ctrl_handler;
+        common_data->i2c_client         = client;
+        common_data->frmfmt             = ov9281_frmfmt;
+        common_data->colorfmt           =
+                camera_common_find_datafmt(OV9281_DEFAULT_DATAFMT);
+        common_data->power              = &priv->power;
+        common_data->ctrls              = priv->ctrls;
+        common_data->priv               = (void *)priv;
+        common_data->numctrls           = ARRAY_SIZE(ctrl_config_list);
+        common_data->numfmts            = ARRAY_SIZE(ov9281_frmfmt);
+        common_data->def_mode           = OV9281_DEFAULT_MODE;
+        common_data->def_width          = OV9281_DEFAULT_WIDTH;
+        common_data->def_height         = OV9281_DEFAULT_HEIGHT;
+        common_data->def_clk_freq       = OV9281_DEFAULT_CLK_FREQ;
+        common_data->fmt_width          = common_data->def_width;
+        common_data->fmt_height         = common_data->def_height;
+
+        priv->i2c_client                = client;
+        priv->s_data                    = common_data;
+        priv->subdev                    = &common_data->subdev;
+        priv->subdev->dev               = &client->dev;
+        priv->group_hold_prev           = 0;
+
+        err = ov9281_power_get(priv);
+        if (err)
+                return err;
+
+        /*
+         * If our device tree node is given MCU GPIOs, then we are expected to
+         * reset the MCU.
+         */
+        if (gpio_is_valid(priv->mcu_boot_gpio) &&
+            gpio_is_valid(priv->mcu_reset_gpio)) {
+                dev_info(&client->dev, "Resetting MCU\n");
+                gpio_set_value(priv->mcu_boot_gpio, 0);
+                gpio_set_value(priv->mcu_reset_gpio, 0);
+                msleep_range(1);
+                gpio_set_value(priv->mcu_reset_gpio, 1);
+        }
+
+        err = camera_common_parse_ports(client, common_data);
+        if (err) {
+                dev_err(&client->dev, "Failed to find port info\n");
+                return err;
+        }
+        sprintf(dev_name, "ov9281_%c", common_data->csi_port + 'a');
+        dev_dbg(&client->dev, "%s: name %s\n", __func__, dev_name);
+        camera_common_create_debugfs(common_data, dev_name);
+
+        v4l2_i2c_subdev_init(&common_data->subdev, client,
+                             &ov9281_subdev_ops);
+
+        err = ov9281_ctrls_init(priv);
+        if (err)
+                return err;
+
+        err = ov9281_i2c_addr_assign(priv, client->addr);
+        if (err)
+                return err;
+
+        err = ov9281_verify_chip_id(priv);
+        if (err)
+                return err;
+
+        err = ov9281_otp_setup(priv);
+        if (err) {
+                dev_err(&client->dev, "Error %d reading otp data\n", err);
+                return err;
+        }
+
+        err = ov9281_fuse_id_setup(priv);
+        if (err) {
+                dev_err(&client->dev, "Error %d reading fuse id data\n", err);
+                return err;
+        }
+
+        priv->subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+                     V4L2_SUBDEV_FL_HAS_EVENTS;
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+        priv->subdev->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+        priv->subdev->entity.ops = &ov9281_media_ops;
+        err = media_entity_init(&priv->subdev->entity, 1, &priv->pad, 0);
+        if (err < 0) {
+                dev_err(&client->dev, "unable to init media entity\n");
+                return err;
+        }
+#endif
+
+        err = v4l2_async_register_subdev(priv->subdev);
+        if (err)
+                return err;
+
+        dev_info(&client->dev, "Probed v4l2 sensor.\n");
+
+        return 0;
+}
+
+static int
+ov9281_remove(struct i2c_client *client)
+{
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct ov9281 *priv = (struct ov9281 *)s_data->priv;
+
+        v4l2_async_unregister_subdev(priv->subdev);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&priv->subdev->entity);
+#endif
+        v4l2_ctrl_handler_free(&priv->ctrl_handler);
+        ov9281_power_put(priv);
+        camera_common_remove_debugfs(s_data);
+
+        return 0;
+}
+
+static const struct i2c_device_id ov9281_id[] = {
+        { "ov9281", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, ov9281_id);
+
+static struct i2c_driver ov9281_i2c_driver = {
+        .driver = {
+                .name = "ov9281",
+                .owner = THIS_MODULE,
+                .of_match_table = of_match_ptr(ov9281_of_match),
+        },
+        .probe = ov9281_probe,
+        .remove = ov9281_remove,
+        .id_table = ov9281_id,
+};
+
+module_i2c_driver(ov9281_i2c_driver);
+
+MODULE_DESCRIPTION("SoC Camera driver for Omnivison OV9281");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov9281_mode_tbls.h b/drivers/media/i2c/ov9281_mode_tbls.h
new file mode 100644
index 000000000..65a86794e
--- /dev/null
+++ b/drivers/media/i2c/ov9281_mode_tbls.h
@@ -0,0 +1,425 @@
+/*
+ * ov9281.c - ov9281 sensor driver
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION, All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <media/camera_common.h>
+
+#ifndef __OV9281_I2C_TABLES__
+#define __OV9281_I2C_TABLES__
+
+#define OV9281_TABLE_WAIT_MS    0
+#define OV9281_TABLE_END        1
+
+#define ov9281_reg struct reg_8
+
+enum {
+        OV9281_MODE_1280X800,
+        OV9281_MODE_1280X720,
+        OV9281_MODE_640X400,
+        OV9281_MODE_START_STREAM,
+        OV9281_MODE_STOP_STREAM,
+};
+
+enum {
+        OV9281_FSYNC_NONE,
+        OV9281_FSYNC_MASTER,
+        OV9281_FSYNC_SLAVE,
+};
+
+static const ov9281_reg ov9281_start[] = {
+        { 0x0100, 0x01 },
+        { OV9281_TABLE_END, 0x00 }
+};
+
+static const ov9281_reg ov9281_stop[] = {
+        { 0x0100, 0x00 },
+        { OV9281_TABLE_END, 0x00 }
+};
+
+static const ov9281_reg ov9281_fsync_master[] = {
+        {0x3006, 0x02}, /* fsin pin out */
+        {0x3823, 0x00},
+        {OV9281_TABLE_WAIT_MS, 66},
+        {OV9281_TABLE_END, 0x00}
+};
+
+static const ov9281_reg ov9281_fsync_slave[] = {
+        {0x3006, 0x00}, /* fsin pin in */
+        {0x3823, 0x30}, /* ext_vs_en, r_init_man */
+        {OV9281_TABLE_WAIT_MS, 66},
+        {OV9281_TABLE_END, 0x00}
+};
+
+static const ov9281_reg ov9281_mode_1280x800_26MhzMCLK[] = {
+        /* PLL control */
+        { 0x0302, 0x32 },
+        { 0x030d, 0x50 },
+        { 0x030e, 0x02 },
+
+        /* system control */
+        { 0x3001, 0x00 },
+        { 0x3004, 0x00 },
+        { 0x3005, 0x00 },
+        { 0x3006, 0x04 },
+        { 0x3011, 0x0a },
+        { 0x3013, 0x18 },
+        { 0x3022, 0x01 },
+        { 0x3030, 0x10 },
+        { 0x3039, 0x32 },
+        { 0x303a, 0x00 },
+
+        /* manual AEC/AGC */
+        { 0x3500, 0x00 },
+        { 0x3501, 0x2a },
+        { 0x3502, 0x90 },
+        { 0x3503, 0x08 },
+        { 0x3505, 0x8c },
+        { 0x3507, 0x03 },
+        { 0x3508, 0x00 },
+        { 0x3509, 0x10 },
+
+        /* analog control */
+        { 0x3610, 0x80 },
+        { 0x3611, 0xa0 },
+        { 0x3620, 0x6e },
+        { 0x3632, 0x56 },
+        { 0x3633, 0x78 },
+        { 0x3662, 0x05 },
+        { 0x3666, 0x00 },
+        { 0x366f, 0x5a },
+        { 0x3680, 0x84 },
+
+        /* sensor control */
+        { 0x3712, 0x80 },
+        { 0x372d, 0x22 },
+        { 0x3731, 0x80 },
+        { 0x3732, 0x30 },
+        { 0x3778, 0x00 },
+        { 0x377d, 0x22 },
+        { 0x3788, 0x02 },
+        { 0x3789, 0xa4 },
+        { 0x378a, 0x00 },
+        { 0x378b, 0x4a },
+        { 0x3799, 0x20 },
+
+        /* timing control */
+        { 0x3800, 0x00 },
+        { 0x3801, 0x00 },
+        { 0x3802, 0x00 },
+        { 0x3803, 0x00 },
+        { 0x3804, 0x05 },
+        { 0x3805, 0x0f },
+        { 0x3806, 0x03 },
+        { 0x3807, 0x2f },
+        { 0x3808, 0x05 },
+        { 0x3809, 0x00 },
+        { 0x380a, 0x03 },
+        { 0x380b, 0x20 },
+        { 0x380c, 0x02 },
+        { 0x380d, 0xd8 },
+        { 0x380e, 0x03 },
+        { 0x380f, 0x8e },
+        { 0x3810, 0x00 },
+        { 0x3811, 0x08 },
+        { 0x3812, 0x00 },
+        { 0x3813, 0x08 },
+        { 0x3814, 0x11 },
+        { 0x3815, 0x11 },
+        { 0x3820, 0x40 },
+        { 0x3821, 0x00 },
+        { 0x3881, 0x42 },
+        { 0x38a8, 0x02 },
+        { 0x38a9, 0x80 },
+        { 0x38b1, 0x00 },
+        { 0x38c4, 0x00 },
+        { 0x38c5, 0xc0 },
+        { 0x38c6, 0x04 },
+        { 0x38c7, 0x80 },
+
+        /* PWM and strobe control */
+        { 0x3920, 0xff },
+
+        /* BLC control */
+        { 0x4003, 0x40 },
+        { 0x4008, 0x04 },
+        { 0x4009, 0x0b },
+        { 0x400c, 0x00 },
+        { 0x400d, 0x07 },
+        { 0x4010, 0x40 },
+        { 0x4043, 0x40 },
+
+        /* format control */
+        { 0x4307, 0x30 },
+        { 0x4317, 0x00 },
+
+        /* ???? */
+        { 0x4501, 0x00 },
+        { 0x4507, 0x00 },
+        { 0x4509, 0x00 },
+        { 0x450a, 0x08 },
+
+        /* VFIFO control */
+        { 0x4601, 0x04 },
+
+        /* DVP control */
+        { 0x470f, 0x00 },
+
+        /* low power mode control */
+        { 0x4f07, 0x00 },
+
+        /* MIPI top control */
+        { 0x4800, 0x00 }, /* bit 5: discontinuous clk */
+
+        /* ISP top control */
+        { 0x5000, 0x9f },
+        { 0x5001, 0x00 },
+        { 0x5e00, 0x00 },
+
+        /* ???? */
+        { 0x5d00, 0x07 },
+        { 0x5d01, 0x00 },
+        { OV9281_TABLE_END, 0x00}
+};
+
+static const ov9281_reg ov9281_mode_1280x720_26MhzMCLK[] = {
+        { 0x0302, 0x32 },
+        { 0x030d, 0x50 },
+        { 0x030e, 0x02 },
+        { 0x3001, 0x00 },
+        { 0x3004, 0x00 },
+        { 0x3005, 0x00 },
+        { 0x3006, 0x04 },
+        { 0x3011, 0x0a },
+        { 0x3013, 0x18 },
+        { 0x3022, 0x01 },
+        { 0x3030, 0x10 },
+        { 0x3039, 0x32 },
+        { 0x303a, 0x00 },
+        { 0x3500, 0x00 },
+        { 0x3501, 0x2a },
+        { 0x3502, 0x90 },
+        { 0x3503, 0x08 },
+        { 0x3505, 0x8c },
+        { 0x3507, 0x03 },
+        { 0x3508, 0x00 },
+        { 0x3509, 0x10 },
+        { 0x3610, 0x80 },
+        { 0x3611, 0xa0 },
+        { 0x3620, 0x6e },
+        { 0x3632, 0x56 },
+        { 0x3633, 0x78 },
+        { 0x3662, 0x05 },
+        { 0x3666, 0x00 },
+        { 0x366f, 0x5a },
+        { 0x3680, 0x84 },
+        { 0x3712, 0x80 },
+        { 0x372d, 0x22 },
+        { 0x3731, 0x80 },
+        { 0x3732, 0x30 },
+        { 0x3778, 0x00 },
+        { 0x377d, 0x22 },
+        { 0x3788, 0x02 },
+        { 0x3789, 0xa4 },
+        { 0x378a, 0x00 },
+        { 0x378b, 0x4a },
+        { 0x3799, 0x20 },
+        { 0x3800, 0x00 },
+        { 0x3801, 0x00 },
+        { 0x3802, 0x00 },
+        { 0x3803, 0x28 },
+        { 0x3804, 0x05 },
+        { 0x3805, 0x0f },
+        { 0x3806, 0x03 },
+        { 0x3807, 0x07 },
+        { 0x3808, 0x05 },
+        { 0x3809, 0x00 },
+        { 0x380a, 0x02 },
+        { 0x380b, 0xd0 },
+        { 0x380c, 0x02 },
+        { 0x380d, 0xd8 },
+        { 0x380e, 0x03 },
+        { 0x380f, 0x8e },
+        { 0x3810, 0x00 },
+        { 0x3811, 0x08 },
+        { 0x3812, 0x00 },
+        { 0x3813, 0x08 },
+        { 0x3814, 0x11 },
+        { 0x3815, 0x11 },
+        { 0x3820, 0x40 },
+        { 0x3821, 0x00 },
+        { 0x3881, 0x42 },
+        { 0x38a8, 0x02 },
+        { 0x38a9, 0x80 },
+        { 0x38b1, 0x00 },
+        { 0x38c4, 0x00 },
+        { 0x38c5, 0xc0 },
+        { 0x38c6, 0x04 },
+        { 0x38c7, 0x80 },
+        { 0x3920, 0xff },
+        { 0x4003, 0x40 },
+        { 0x4008, 0x04 },
+        { 0x4009, 0x0b },
+        { 0x400c, 0x00 },
+        { 0x400d, 0x07 },
+        { 0x4010, 0x40 },
+        { 0x4043, 0x40 },
+        { 0x4307, 0x30 },
+        { 0x4317, 0x00 },
+        { 0x4501, 0x00 },
+        { 0x4507, 0x00 },
+        { 0x4509, 0x00 },
+        { 0x450a, 0x08 },
+        { 0x4601, 0x04 },
+        { 0x470f, 0x00 },
+        { 0x4f07, 0x00 },
+        { 0x4800, 0x00 },
+        { 0x5000, 0x9f },
+        { 0x5001, 0x00 },
+        { 0x5e00, 0x00 },
+        { 0x5d00, 0x07 },
+        { 0x5d01, 0x00 },
+        { OV9281_TABLE_END, 0x00}
+};
+
+static const ov9281_reg ov9281_mode_640x400_26MhzMCLK[] = {
+        { 0x0302, 0x32 },
+        { 0x030d, 0x50 },
+        { 0x030e, 0x02 },
+        { 0x3001, 0x00 },
+        { 0x3004, 0x00 },
+        { 0x3005, 0x00 },
+        { 0x3006, 0x04 },
+        { 0x3011, 0x0a },
+        { 0x3013, 0x18 },
+        { 0x3022, 0x01 },
+        { 0x3030, 0x10 },
+        { 0x3039, 0x32 },
+        { 0x303a, 0x00 },
+        { 0x3500, 0x00 },
+        { 0x3501, 0x01 },
+        { 0x3502, 0xf4 },
+        { 0x3503, 0x08 },
+        { 0x3505, 0x8c },
+        { 0x3507, 0x03 },
+        { 0x3508, 0x00 },
+        { 0x3509, 0x10 },
+        { 0x3610, 0x80 },
+        { 0x3611, 0xa0 },
+        { 0x3620, 0x6e },
+        { 0x3632, 0x56 },
+        { 0x3633, 0x78 },
+        { 0x3662, 0x05 },
+        { 0x3666, 0x00 },
+        { 0x366f, 0x5a },
+        { 0x3680, 0x84 },
+        { 0x3712, 0x80 },
+        { 0x372d, 0x22 },
+        { 0x3731, 0x80 },
+        { 0x3732, 0x30 },
+        { 0x3778, 0x10 },
+        { 0x377d, 0x22 },
+        { 0x3788, 0x02 },
+        { 0x3789, 0xa4 },
+        { 0x378a, 0x00 },
+        { 0x378b, 0x4a },
+        { 0x3799, 0x20 },
+        { 0x3800, 0x00 },
+        { 0x3801, 0x00 },
+        { 0x3802, 0x00 },
+        { 0x3803, 0x00 },
+        { 0x3804, 0x05 },
+        { 0x3805, 0x0f },
+        { 0x3806, 0x03 },
+        { 0x3807, 0x2f },
+        { 0x3808, 0x02 },
+        { 0x3809, 0x80 },
+        { 0x380a, 0x01 },
+        { 0x380b, 0x90 },
+        { 0x380c, 0x02 },
+        { 0x380d, 0xd8 },
+        { 0x380e, 0x02 },
+        { 0x380f, 0x08 },
+        { 0x3810, 0x00 },
+        { 0x3811, 0x04 },
+        { 0x3812, 0x00 },
+        { 0x3813, 0x04 },
+        { 0x3814, 0x31 },
+        { 0x3815, 0x22 },
+        { 0x3820, 0x60 },
+        { 0x3821, 0x01 },
+        { 0x3881, 0x42 },
+        { 0x38a8, 0x02 },
+        { 0x38a9, 0x80 },
+        { 0x38b1, 0x00 },
+        { 0x38c4, 0x00 },
+        { 0x38c5, 0xc0 },
+        { 0x38c6, 0x04 },
+        { 0x38c7, 0x80 },
+        { 0x3920, 0xff },
+        { 0x4003, 0x40 },
+        { 0x4008, 0x02 },
+        { 0x4009, 0x05 },
+        { 0x400c, 0x00 },
+        { 0x400d, 0x03 },
+        { 0x4010, 0x40 },
+        { 0x4043, 0x40 },
+        { 0x4307, 0x30 },
+        { 0x4317, 0x00 },
+        { 0x4501, 0x00 },
+        { 0x4507, 0x03 },
+        { 0x4509, 0x80 },
+        { 0x450a, 0x08 },
+        { 0x4601, 0x04 },
+        { 0x470f, 0x00 },
+        { 0x4f07, 0x00 },
+        { 0x4800, 0x00 },
+        { 0x5000, 0x9f },
+        { 0x5001, 0x00 },
+        { 0x5e00, 0x00 },
+        { 0x5d00, 0x07 },
+        { 0x5d01, 0x00 },
+        { OV9281_TABLE_END, 0x00 }
+};
+
+static const ov9281_reg *ov9281_mode_table[] = {
+        [OV9281_MODE_1280X800] = ov9281_mode_1280x800_26MhzMCLK,
+        [OV9281_MODE_1280X720] = ov9281_mode_1280x720_26MhzMCLK,
+        [OV9281_MODE_640X400] = ov9281_mode_640x400_26MhzMCLK,
+        [OV9281_MODE_START_STREAM] = ov9281_start,
+        [OV9281_MODE_STOP_STREAM] = ov9281_stop,
+};
+
+static const ov9281_reg *ov9281_fsync_table[] = {
+        [OV9281_FSYNC_NONE] = NULL,
+        [OV9281_FSYNC_MASTER] = ov9281_fsync_master,
+        [OV9281_FSYNC_SLAVE] = ov9281_fsync_slave,
+};
+
+static const int ov9281_120fps[] = {
+        120,
+};
+
+static const struct camera_common_frmfmt ov9281_frmfmt[] = {
+        { { 1280, 800 }, ov9281_120fps, ARRAY_SIZE(ov9281_120fps), 0,
+          OV9281_MODE_1280X800 },
+        { { 1280, 720 }, ov9281_120fps, ARRAY_SIZE(ov9281_120fps), 0,
+          OV9281_MODE_1280X720 },
+        { { 640, 400 }, ov9281_120fps, ARRAY_SIZE(ov9281_120fps), 0,
+          OV9281_MODE_640X400 },
+};
+
+#endif  /* __OV9281_I2C_TABLES__ */
diff --git a/drivers/media/i2c/pca9570.c b/drivers/media/i2c/pca9570.c
new file mode 100644
index 000000000..e937f191c
--- /dev/null
+++ b/drivers/media/i2c/pca9570.c
@@ -0,0 +1,367 @@
+/*
+ * pca9570.c - pca9570 IO Expander driver
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/seq_file.h>
+#include <linux/debugfs.h>
+#include <media/camera_common.h>
+#include <linux/module.h>
+
+
+#define IMX185_PCA9570_I2C_ADDR (0x24)
+#define PCA9570_MODE_STEPS      5
+#define PCA9570_FORWARD 0
+#define PCA9570_REVERSE 1
+
+#define BA6208_FORWARD  0x02
+#define BA6208_BACKWARD 0x01
+#define BA6208_BREAK    0x03
+#define DRV8838_FORWARD 0x03
+#define DRV8838_BACKWARD        0x02
+#define DRV8838_BREAK   0x00
+
+enum drive_ic {
+        BA6208 = 0,
+        DRV8838,
+};
+
+struct pca9570 {
+        struct i2c_client *i2c_client;
+        struct regmap *regmap;
+        const char *channel;
+        const char *drive_ic_name;
+        int drive_ic;
+};
+
+static int pca9570_write_reg(struct pca9570 *priv,
+                                u8 addr, u8 val)
+{
+        struct i2c_client *i2c_client = priv->i2c_client;
+        int err;
+
+        err = regmap_write(priv->regmap, addr, val);
+        if (err)
+                dev_err(&i2c_client->dev, "%s:i2c write failed, 0x%x = %x\n",
+                        __func__, addr, val);
+
+        return err;
+}
+
+static int pca9570_icr_move(struct pca9570 *priv, u32 direction, u32 val)
+{
+        struct i2c_client *i2c_client = priv->i2c_client;
+        int i;
+        int steps = 0;
+        int err = 0;
+        u8 reg = 0;
+        u8 reg_forward, reg_revert, reg_brake;
+
+        switch (priv->drive_ic) {
+        case DRV8838:
+                reg_forward = DRV8838_FORWARD;
+                reg_revert = DRV8838_BACKWARD;
+                reg_brake = DRV8838_BREAK;
+                break;
+        case BA6208:
+        default:
+                reg_forward = BA6208_FORWARD;
+                reg_revert = BA6208_BACKWARD;
+                reg_brake = BA6208_BREAK;
+                break;
+        }
+
+        steps = val;
+        if (steps < 1)
+                steps = PCA9570_MODE_STEPS;
+
+        if (direction == PCA9570_FORWARD) {
+                dev_info(&i2c_client->dev, "%s, forward val=%d\n",
+                         __func__, steps);
+                reg = reg_forward;
+        } else if (direction == PCA9570_REVERSE) {
+                dev_info(&i2c_client->dev, "%s, reverse val=%d\n",
+                         __func__, steps);
+                reg = reg_revert;
+        } else
+                return -ENOMEM;
+
+        for (i = 0; i < steps; i++) {
+                usleep_range(1000*100, 1000*110);
+                err |= pca9570_write_reg(priv, IMX185_PCA9570_I2C_ADDR, 0x48);
+                err |= pca9570_write_reg(priv, IMX185_PCA9570_I2C_ADDR, reg);
+                usleep_range(1000*100, 1000*110);
+                err |= pca9570_write_reg(priv, IMX185_PCA9570_I2C_ADDR, 0x48);
+                err |= pca9570_write_reg(priv, IMX185_PCA9570_I2C_ADDR,
+                                reg_brake);
+        }
+        if (err)
+                dev_err(&i2c_client->dev, "%s:i2c write failed\n", __func__);
+
+        return err;
+}
+
+
+static int pca9570_icr_daymode(struct pca9570 *priv)
+{
+        int err = 0;
+
+        err = pca9570_icr_move(priv, PCA9570_FORWARD, PCA9570_MODE_STEPS);
+        return err;
+}
+
+
+static int pca9570_icr_nightmode(struct pca9570 *priv)
+{
+        int err = 0;
+
+        err = pca9570_icr_move(priv, PCA9570_REVERSE, PCA9570_MODE_STEPS);
+        return err;
+}
+
+static int pca9570_stats_show(struct seq_file *s, void *data)
+{
+        return 0;
+}
+
+static int pca9570_debugfs_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, pca9570_stats_show, inode->i_private);
+}
+
+static ssize_t pca9570_debugfs_write(struct file *s,
+                                const char __user *user_buf,
+                                size_t count, loff_t *ppos)
+{
+        struct pca9570 *priv =
+                ((struct seq_file *)s->private_data)->private;
+        struct i2c_client *i2c_client = priv->i2c_client;
+
+        char buf[255];
+        int buf_size;
+        u32 val = 0;
+        int err = 0;
+
+        if (!user_buf || count <= 1)
+                return -EFAULT;
+
+        memset(buf, 0, sizeof(buf));
+        buf_size = min(count, sizeof(buf) - 1);
+        if (copy_from_user(buf, user_buf, buf_size))
+                return -EFAULT;
+
+        if (buf[0] == 'd') {
+                dev_info(&i2c_client->dev, "%s, set daymode\n", __func__);
+                err = pca9570_icr_daymode(priv);
+                if (err)
+                        dev_info(&i2c_client->dev, "%s, set daymode fail\n",
+                                 __func__);
+                return count;
+        }
+
+        if (buf[0] == 'n') {
+                dev_info(&i2c_client->dev, "%s, set nightmode\n", __func__);
+                err = pca9570_icr_nightmode(priv);
+                if (err)
+                        dev_info(&i2c_client->dev, "%s, set nightmode fail\n",
+                                 __func__);
+                return count;
+        }
+
+        if (sscanf(buf + 1, "0x%x", &val) == 1)
+                goto set_attr;
+        if (sscanf(buf + 1, "0X%x", &val) == 1)
+                goto set_attr;
+        if (sscanf(buf + 1, "%d", &val) == 1)
+                goto set_attr;
+
+        dev_err(&i2c_client->dev, "SYNTAX ERROR: %s\n", buf);
+        return -EFAULT;
+
+set_attr:
+        dev_info(&i2c_client->dev, "%s, val=%c%d\n", __func__, buf[0], val);
+        switch (buf[0]) {
+        case 'f':
+                err = pca9570_icr_move(priv, PCA9570_FORWARD, val);
+                if (err)
+                        dev_info(&i2c_client->dev, "%s, move forward fail\n",
+                                 __func__);
+                break;
+        case 'r':
+                err = pca9570_icr_move(priv, PCA9570_REVERSE, val);
+                if (err)
+                        dev_info(&i2c_client->dev, "%s, move reverse fail\n",
+                                 __func__);
+                break;
+        }
+
+        return count;
+}
+
+
+static const struct file_operations pca9570_debugfs_fops = {
+        .open = pca9570_debugfs_open,
+        .read = seq_read,
+        .write = pca9570_debugfs_write,
+        .llseek = seq_lseek,
+        .release = single_release,
+};
+
+static int pca9570_debugfs_init(const char *dir_name,
+                                struct dentry **d_entry,
+                                struct dentry **f_entry,
+                                struct pca9570 *priv)
+{
+        struct dentry  *dp, *fp;
+        char dev_name[20];
+        struct i2c_client *i2c_client = priv->i2c_client;
+        struct device_node *np = i2c_client->dev.of_node;
+        int err = 0;
+
+        if (np) {
+                err = of_property_read_string(np, "channel", &priv->channel);
+                if (err)
+                        dev_err(&i2c_client->dev, "channel not found\n");
+                snprintf(dev_name, sizeof(dev_name), "pca9570_%s",
+                        priv->channel);
+        }
+
+        dp = debugfs_create_dir(dev_name, NULL);
+        if (dp == NULL) {
+                dev_err(&i2c_client->dev, "%s: debugfs create dir failed\n",
+                        __func__);
+                return -ENOMEM;
+        }
+
+        fp = debugfs_create_file("pca9570", S_IRUGO|S_IWUSR,
+                dp, priv, &pca9570_debugfs_fops);
+        if (!fp) {
+                dev_err(&i2c_client->dev, "%s: debugfs create file failed\n",
+                        __func__);
+                debugfs_remove_recursive(dp);
+                return -ENOMEM;
+        }
+
+        if (d_entry)
+                *d_entry = dp;
+        if (f_entry)
+                *f_entry = fp;
+        return 0;
+}
+
+static  struct regmap_config pca9570_regmap_config = {
+        .reg_bits = 8,
+        .val_bits = 8,
+        .cache_type = REGCACHE_RBTREE,
+};
+
+static int pca9570_probe(struct i2c_client *client,
+                                const struct i2c_device_id *id)
+{
+        struct pca9570 *priv;
+        struct device_node *np = client->dev.of_node;
+        int err = 0;
+
+        priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
+        priv->i2c_client = client;
+        priv->regmap = devm_regmap_init_i2c(priv->i2c_client,
+                                &pca9570_regmap_config);
+        if (IS_ERR(priv->regmap)) {
+                dev_err(&client->dev,
+                        "regmap init failed: %ld\n", PTR_ERR(priv->regmap));
+                return -ENODEV;
+        }
+
+        err = of_property_read_string(np, "drive_ic", &priv->drive_ic_name);
+        if (!err && !IS_ERR(priv->drive_ic_name)) {
+                if (!strcmp(priv->drive_ic_name, "BA6208"))
+                        priv->drive_ic = BA6208;
+                else if (!strcmp(priv->drive_ic_name, "DRV8838"))
+                        priv->drive_ic = DRV8838;
+                else {
+                        priv->drive_ic = BA6208;
+                        dev_info(&client->dev,
+                                "%s, unsupport driver ic found\n", __func__);
+                }
+        } else {
+                priv->drive_ic = BA6208;
+                dev_info(&client->dev,
+                                "%s, drive_ic not found\n", __func__);
+        }
+
+        err = pca9570_debugfs_init(NULL, NULL, NULL, priv);
+        if (err)
+                return err;
+        /*set daymode by fault*/
+        err = pca9570_icr_daymode(priv);
+        if (err)
+                return err;
+        dev_info(&client->dev, "%s:  success\n", __func__);
+
+        return err;
+}
+
+
+static int
+pca9570_remove(struct i2c_client *client)
+{
+
+        if (client != NULL) {
+                i2c_unregister_device(client);
+                client = NULL;
+        }
+
+        return 0;
+}
+
+static const struct i2c_device_id pca9570_id[] = {
+        { "pca9570", 0 },
+        { },
+};
+
+const struct of_device_id pca9570_of_match[] = {
+        { .compatible = "nvidia,pca9570", },
+        { },
+};
+MODULE_DEVICE_TABLE(of, imx185_of_match);
+MODULE_DEVICE_TABLE(i2c, pca9570_id);
+
+static struct i2c_driver pca9570_i2c_driver = {
+        .driver = {
+                .name = "pca9570",
+                .owner = THIS_MODULE,
+        },
+        .probe = pca9570_probe,
+        .remove = pca9570_remove,
+        .id_table = pca9570_id,
+};
+
+static int __init pca9570_init(void)
+{
+        return i2c_add_driver(&pca9570_i2c_driver);
+}
+
+static void __exit pca9570_exit(void)
+{
+        i2c_del_driver(&pca9570_i2c_driver);
+}
+
+module_init(pca9570_init);
+module_exit(pca9570_exit);
+
+MODULE_DESCRIPTION("IO Expander driver pca9570");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/tc358840.c b/drivers/media/i2c/tc358840.c
new file mode 100644
index 000000000..a545d4571
--- /dev/null
+++ b/drivers/media/i2c/tc358840.c
@@ -0,0 +1,2465 @@
+/*
+ * tc358840.c - Toshiba UH2C/D HDMI-CSI bridge driver
+ *
+ * Copyright (c) 2015, Armin Weiss <weii@zhaw.ch>
+ * Copyright (c) 2016 - 2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is based on the tc358840 - Toshiba HDMI to CSI-2 bridge driver
+ * from Cisco Systems, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <linux/module.h>
+
+#include <linux/i2c.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_gpio.h>
+#include <linux/of_irq.h>
+#include <linux/interrupt.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+#include <linux/hdmi.h>
+#include <linux/v4l2-dv-timings.h>
+
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-of.h>
+#include <media/camera_common.h>
+
+#include <media/i2c/tc358840.h>
+#include "tc358840_regs.h"
+
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-3)");
+
+/* TODO: Implement Colorbar TPG */
+
+#define EDID_NUM_BLOCKS_MAX 8
+#define EDID_BLOCK_SIZE 128
+#define DELAY_ENABLE_INTERRUPT_MS 10000
+
+static u8 edid[] = {
+        0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00,
+        0x52, 0x62, 0x88, 0x88, 0x00, 0x88, 0x88, 0x88,
+        0x1C, 0x15, 0x01, 0x03, 0x80, 0x00, 0x00, 0x78,
+        0x0A, 0x0D, 0xC9, 0xA0, 0x57, 0x47, 0x98, 0x27,
+        0x12, 0x48, 0x4C, 0x00, 0x00, 0x00, 0x01, 0x01,
+        0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+        0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0xEC, 0x68,
+        0x00, 0xA0, 0xF0, 0x70, 0x37, 0x80, 0x30, 0x20,
+        0x3A, 0x00, 0x00, 0x70, 0xF8, 0x00, 0x00, 0x1C,
+        0x02, 0x3A, 0x80, 0x18, 0x71, 0x38, 0x2D, 0x40,
+        0x58, 0x2C, 0x45, 0x00, 0xC4, 0x8E, 0x21, 0x00,
+        0x00, 0x1E, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x54,
+        0x6F, 0x73, 0x68, 0x69, 0x62, 0x61, 0x2D, 0x55,
+        0x48, 0x32, 0x44, 0x0A, 0x00, 0x00, 0x00, 0xFD,
+        0x00, 0x17, 0x3D, 0x0F, 0x8C, 0x17, 0x00, 0x0A,
+        0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x01, 0xAF,
+        0x02, 0x03, 0x1A, 0x74, 0x47, 0x32, 0x10, 0x04,
+        0x32, 0x32, 0x32, 0x32, 0x23, 0x09, 0x07, 0x01,
+        0x83, 0x01, 0x00, 0x00, 0x65, 0x03, 0x0C, 0x00,
+        0x10, 0x00, 0x01, 0x1D, 0x00, 0x72, 0x51, 0xD0,
+        0x1E, 0x20, 0x6E, 0x28, 0x55, 0x00, 0xC4, 0x8E,
+        0x21, 0x00, 0x00, 0x1E, 0xEC, 0x68, 0x00, 0xA0,
+        0xF0, 0x70, 0x37, 0x80, 0x30, 0x20, 0x3A, 0x00,
+        0x00, 0x70, 0xF8, 0x00, 0x00, 0x1C, 0xEC, 0x68,
+        0x00, 0xA0, 0xF0, 0x70, 0x37, 0x80, 0x30, 0x20,
+        0x3A, 0x00, 0x00, 0x70, 0xF8, 0x00, 0x00, 0x1C,
+        0xEC, 0x68, 0x00, 0xA0, 0xF0, 0x70, 0x37, 0x80,
+        0x30, 0x20, 0x3A, 0x00, 0x00, 0x70, 0xF8, 0x00,
+        0x00, 0x1C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x26,
+};
+
+static const struct v4l2_dv_timings_cap tc358840_timings_cap = {
+        .type = V4L2_DV_BT_656_1120,
+        /* keep this initialization for compatibility with GCC < 4.4.6 */
+        .reserved = { 0 },
+        /* Pixel clock from REF_01 p. 20. Min/max height/width are unknown */
+        V4L2_INIT_BT_TIMINGS(
+                1, 10000, 1, 10000, 0, 297000000,
+                V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+                V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
+                V4L2_DV_BT_CAP_PROGRESSIVE |
+                V4L2_DV_BT_CAP_REDUCED_BLANKING |
+                V4L2_DV_BT_CAP_CUSTOM)
+};
+
+struct tc358840_state {
+        struct tc358840_platform_data pdata;
+        struct v4l2_subdev sd;
+        struct media_pad pad[2];
+        struct v4l2_ctrl_handler hdl;
+        struct i2c_client *i2c_client;
+        bool enabled;
+        bool format_changed;
+
+        /* controls */
+        struct v4l2_ctrl *detect_tx_5v_ctrl;
+        struct v4l2_ctrl *audio_sampling_rate_ctrl;
+        struct v4l2_ctrl *audio_present_ctrl;
+        struct v4l2_ctrl *splitter_width_ctrl;
+
+        /* work queues */
+        struct workqueue_struct *work_queues;
+        struct delayed_work delayed_work_enable_hotplug;
+        struct delayed_work delayed_work_enable_interrupt;
+        struct work_struct process_isr;
+        struct mutex isr_lock;
+
+        /* edid  */
+        u8 edid_blocks_written;
+
+        /* timing / mbus */
+        struct v4l2_dv_timings timings;
+        u32 mbus_fmt_code;
+};
+
+static inline struct tc358840_state *to_state(struct v4l2_subdev *sd)
+{
+        return container_of(sd, struct tc358840_state, sd);
+}
+
+
+static void tc358840_enable_interrupts(
+        struct v4l2_subdev *sd, bool cable_connected);
+static int tc358840_s_ctrl_detect_tx_5v(struct v4l2_subdev *sd);
+static void tc358840_init_interrupts(struct v4l2_subdev *sd);
+static int tc358840_s_dv_timings(
+        struct v4l2_subdev *sd, struct v4l2_dv_timings *timings);
+static void tc358840_set_csi(struct v4l2_subdev *sd);
+static void tc358840_set_splitter(struct v4l2_subdev *sd);
+static int tc358840_s_edid(struct v4l2_subdev *sd,
+        struct v4l2_subdev_edid *edid);
+
+/* --------------- I2C --------------- */
+
+static void i2c_rd(struct v4l2_subdev *sd, u16 reg, u8 *values, u32 n)
+{
+        struct tc358840_state *state = to_state(sd);
+        struct i2c_client *client = state->i2c_client;
+        int err;
+        u8 buf[2] = { reg >> 8, reg & 0xff };
+        struct i2c_msg msgs[] = {
+                {
+                        .addr = client->addr,
+                        .flags = 0,
+                        .len = 2,
+                        .buf = buf,
+                },
+                {
+                        .addr = client->addr,
+                        .flags = I2C_M_RD,
+                        .len = n,
+                        .buf = values,
+                },
+        };
+
+        err = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+        if (err != ARRAY_SIZE(msgs)) {
+                v4l2_err(sd, "%s: reading register 0x%x from 0x%x failed\n",
+                        __func__, reg, client->addr);
+        }
+
+        if (debug < 3)
+                return;
+
+        switch (n) {
+        case 1:
+                v4l2_info(sd, "I2C read 0x%04X = 0x%02X\n", reg, values[0]);
+                break;
+        case 2:
+                v4l2_info(sd, "I2C read 0x%04X = 0x%02X%02X\n",
+                        reg, values[1], values[0]);
+                break;
+        case 4:
+                v4l2_info(sd, "I2C read 0x%04X = 0x%02X%02X%02X%02X\n",
+                        reg, values[3], values[2], values[1], values[0]);
+                break;
+        default:
+                v4l2_info(sd, "I2C read %d bytes from address 0x%04X\n",
+                        n, reg);
+        }
+
+}
+
+static void i2c_wr(struct v4l2_subdev *sd, u16 reg, u8 *values, u32 n)
+{
+        struct tc358840_state *state = to_state(sd);
+        struct i2c_client *client = state->i2c_client;
+        int err, i;
+        struct i2c_msg msg;
+        u8 data[2 + n];
+
+        msg.addr = client->addr;
+        msg.buf = data;
+        msg.len = 2 + n;
+        msg.flags = 0;
+
+        data[0] = reg >> 8;
+        data[1] = reg & 0xff;
+
+        for (i = 0; i < n; i++)
+                data[2 + i] = values[i];
+
+        err = i2c_transfer(client->adapter, &msg, 1);
+        if (err != 1) {
+                v4l2_err(sd, "%s: writing register 0x%x from 0x%x failed\n",
+                        __func__, reg, client->addr);
+                return;
+        }
+
+        if (debug < 3)
+                return;
+
+        switch (n) {
+        case 1:
+                v4l2_info(sd, "I2C write 0x%04X = 0x%02X\n", reg, data[2]);
+                break;
+        case 2:
+                v4l2_info(sd, "I2C write 0x%04X = 0x%02X%02X\n",
+                        reg, data[3], data[2]);
+                break;
+        case 4:
+                v4l2_info(sd, "I2C write 0x%04X = 0x%02X%02X%02X%02X\n",
+                        reg, data[5], data[4], data[3], data[2]);
+                break;
+        default:
+                v4l2_info(sd, "I2C write %d bytes from address 0x%04X\n",
+                        n, reg);
+        }
+}
+
+static u8 i2c_rd8(struct v4l2_subdev *sd, u16 reg)
+{
+        u8 val;
+
+        i2c_rd(sd, reg, &val, 1);
+
+        return val;
+}
+
+static void i2c_wr8(struct v4l2_subdev *sd, u16 reg, u8 val)
+{
+        i2c_wr(sd, reg, &val, 1);
+}
+
+static void i2c_wr8_and_or(struct v4l2_subdev *sd, u16 reg,
+                u8 mask, u8 val)
+{
+        i2c_wr8(sd, reg, (i2c_rd8(sd, reg) & mask) | val);
+}
+
+static u16 i2c_rd16(struct v4l2_subdev *sd, u16 reg)
+{
+        u16 val;
+
+        i2c_rd(sd, reg, (u8 *)&val, 2);
+
+        return val;
+}
+
+static void i2c_wr16(struct v4l2_subdev *sd, u16 reg, u16 val)
+{
+        i2c_wr(sd, reg, (u8 *)&val, 2);
+}
+
+static void i2c_wr16_and_or(struct v4l2_subdev *sd, u16 reg, u16 mask, u16 val)
+{
+        i2c_wr16(sd, reg, (i2c_rd16(sd, reg) & mask) | val);
+}
+
+static u32 i2c_rd32(struct v4l2_subdev *sd, u16 reg)
+{
+        u32 val;
+
+        i2c_rd(sd, reg, (u8 *)&val, 4);
+
+        return val;
+}
+
+static void i2c_wr32(struct v4l2_subdev *sd, u16 reg, u32 val)
+{
+        i2c_wr(sd, reg, (u8 *)&val, 4);
+}
+
+static void i2c_wr32_and_or(struct v4l2_subdev *sd, u32 reg, u32 mask, u32 val)
+{
+        i2c_wr32(sd, reg, (i2c_rd32(sd, reg) & mask) | val);
+}
+
+/* --------------- STATUS --------------- */
+
+static inline bool is_hdmi(struct v4l2_subdev *sd)
+{
+        return i2c_rd8(sd, SYS_STATUS) & MASK_S_HDMI;
+}
+
+static inline bool tx_5v_power_present(struct v4l2_subdev *sd)
+{
+        return i2c_rd8(sd, SYS_STATUS) & MASK_S_DDC5V;
+}
+
+static inline bool no_signal(struct v4l2_subdev *sd)
+{
+        return !(i2c_rd8(sd, SYS_STATUS) & MASK_S_TMDS);
+}
+
+static inline bool no_sync(struct v4l2_subdev *sd)
+{
+        return !(i2c_rd8(sd, SYS_STATUS) & MASK_S_SYNC);
+}
+
+static inline bool audio_present(struct v4l2_subdev *sd)
+{
+        return i2c_rd8(sd, AU_STATUS0) & MASK_S_A_SAMPLE;
+}
+
+static int get_audio_sampling_rate(struct v4l2_subdev *sd)
+{
+        static const int code_to_rate[] = {
+                44100, 0, 48000, 32000, 22050, 384000, 24000, 352800,
+                88200, 768000, 96000, 705600, 176400, 0, 192000, 0
+        };
+
+        /* Register FS_SET is not cleared when the cable is disconnected */
+        if (no_signal(sd))
+                return 0;
+
+        return code_to_rate[i2c_rd8(sd, FS_SET) & MASK_FS];
+}
+
+static unsigned tc358840_num_csi_lanes_in_use(struct v4l2_subdev *sd)
+{
+        /* FIXME: Read # of lanes from both, TX0 and TX1 */
+        return i2c_rd32(sd, CSITX0_BASE_ADDR+LANEEN) & MASK_LANES;
+}
+
+/* --------------- TIMINGS --------------- */
+
+static inline unsigned fps(const struct v4l2_bt_timings *t)
+{
+        if (!V4L2_DV_BT_FRAME_HEIGHT(t) || !V4L2_DV_BT_FRAME_WIDTH(t))
+                return 0;
+
+        return DIV_ROUND_CLOSEST((unsigned)t->pixelclock,
+                        V4L2_DV_BT_FRAME_HEIGHT(t) * V4L2_DV_BT_FRAME_WIDTH(t));
+}
+
+static int tc358840_get_detected_timings(struct v4l2_subdev *sd,
+                struct v4l2_dv_timings *timings)
+{
+        struct v4l2_bt_timings *bt = &timings->bt;
+        unsigned width, height, frame_width, frame_height, frame_interval, fps;
+
+        memset(timings, 0, sizeof(struct v4l2_dv_timings));
+
+        if (no_signal(sd)) {
+                v4l2_dbg(1, debug, sd, "%s: no valid signal\n", __func__);
+                return -ENOLINK;
+        }
+        if (no_sync(sd)) {
+                v4l2_dbg(1, debug, sd, "%s: no sync on signal\n", __func__);
+                return -ENOLCK;
+        }
+
+        timings->type = V4L2_DV_BT_656_1120;
+
+        bt->interlaced = i2c_rd8(sd, VI_STATUS1) &
+                MASK_S_V_INTERLACE ? V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
+
+        width = ((i2c_rd8(sd, DE_HSIZE_HI) & 0x1f) << 8) +
+                i2c_rd8(sd, DE_HSIZE_LO);
+        height = ((i2c_rd8(sd, DE_VSIZE_HI) & 0x1f) << 8) +
+                i2c_rd8(sd, DE_VSIZE_LO);
+        frame_width = ((i2c_rd8(sd, IN_HSIZE_HI) & 0x1f) << 8) +
+                i2c_rd8(sd, IN_HSIZE_LO);
+        frame_height = (((i2c_rd8(sd, IN_VSIZE_HI) & 0x3f) << 8) +
+                i2c_rd8(sd, IN_VSIZE_LO)) / 2;
+
+        /* TODO: Check if frame_interval is correct
+         * since the register is not in the datasheet rev. 1.5 */
+
+        /* frame interval in milliseconds * 10
+         * Require SYS_FREQ0 and SYS_FREQ1 are precisely set */
+        frame_interval = ((i2c_rd8(sd, FV_CNT_HI) & 0x3) << 8) +
+                i2c_rd8(sd, FV_CNT_LO);
+        fps = (frame_interval > 0) ?
+                DIV_ROUND_CLOSEST(10000, frame_interval) : 0;
+
+        bt->width = width;
+        bt->height = height;
+        bt->vsync = frame_height - height;
+        bt->hsync = frame_width - width;
+        bt->pixelclock = frame_width * frame_height * fps;
+        if (bt->interlaced == V4L2_DV_INTERLACED) {
+                bt->height *= 2;
+                bt->il_vsync = bt->vsync + 1;
+                bt->pixelclock /= 2;
+        }
+
+        return 0;
+}
+
+/* --------------- HOTPLUG / HDCP / EDID --------------- */
+
+static void tc358840_delayed_work_enable_hotplug(struct work_struct *work)
+{
+        struct delayed_work *dwork = to_delayed_work(work);
+        struct tc358840_state *state = container_of(dwork,
+                struct tc358840_state, delayed_work_enable_hotplug);
+        struct v4l2_subdev *sd = &state->sd;
+
+        v4l2_dbg(2, debug, sd, "%s:\n", __func__);
+
+        i2c_wr8_and_or(sd, HPD_CTL, ~MASK_HPD_OUT0, MASK_HPD_OUT0);
+}
+
+static void tc358840_set_hdmi_hdcp(struct v4l2_subdev *sd, bool enable)
+{
+        v4l2_dbg(2, debug, sd, "%s: %s\n", __func__, enable ?
+                                "enable" : "disable");
+
+        i2c_wr8_and_or(sd, HDCP_REG1,
+                        ~(MASK_AUTH_UNAUTH_SEL | MASK_AUTH_UNAUTH),
+                        MASK_AUTH_UNAUTH_SEL_16_FRAMES | MASK_AUTH_UNAUTH_AUTO);
+
+        i2c_wr8_and_or(sd, HDCP_REG2, ~MASK_AUTO_P3_RESET,
+                        SET_AUTO_P3_RESET_FRAMES(0x0f));
+
+        /* HDCP is disabled by configuring the receiver as HDCP repeater. The
+         * repeater mode require software support to work, so HDCP
+         * authentication will fail.
+         */
+        i2c_wr8_and_or(sd, HDCP_REG3, ~KEY_RD_CMD, enable ? KEY_RD_CMD : 0);
+        i2c_wr8_and_or(sd, HDCP_MODE, ~(MASK_AUTO_CLR | MASK_MODE_RST_TN),
+                        enable ?  (MASK_AUTO_CLR | MASK_MODE_RST_TN) : 0);
+
+        /* Apple MacBook Pro gen.8 has a bug that makes it freeze every fifth
+         * second when HDCP is disabled, but the MAX_EXCED bit is handled
+         * correctly and HDCP is disabled on the HDMI output.
+         */
+        i2c_wr8_and_or(sd, BSTATUS1, ~MASK_MAX_EXCED,
+                        enable ? 0 : MASK_MAX_EXCED);
+        i2c_wr8_and_or(sd, BCAPS, ~(MASK_REPEATER | MASK_READY),
+                        enable ? 0 : MASK_REPEATER | MASK_READY);
+}
+
+static void tc358840_disable_edid(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+
+        v4l2_dbg(2, debug, sd, "%s:\n", __func__);
+
+        cancel_delayed_work_sync(&state->delayed_work_enable_hotplug);
+
+        /* DDC access to EDID is also disabled when hotplug is disabled. See
+         * register DDC_CTL */
+        i2c_wr8_and_or(sd, HPD_CTL, ~MASK_HPD_OUT0, 0x0);
+}
+
+static void tc358840_enable_edid(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+
+        if (state->edid_blocks_written == 0) {
+                v4l2_dbg(2, debug, sd, "%s: no EDID -> no hotplug\n", __func__);
+                return;
+        }
+
+        v4l2_dbg(2, debug, sd, "%s:\n", __func__);
+
+        /* Enable hotplug after 100 ms. DDC access to EDID is also enabled when
+         * hotplug is enabled. See register DDC_CTL */
+        queue_delayed_work(state->work_queues,
+                &state->delayed_work_enable_hotplug, HZ / 10);
+
+        tc358840_enable_interrupts(sd, true);
+        tc358840_s_ctrl_detect_tx_5v(sd);
+}
+
+static void tc358840_delayed_work_enable_interrupt(struct work_struct *work)
+{
+        struct delayed_work *dwork = to_delayed_work(work);
+        struct tc358840_state *state = container_of(dwork,
+                struct tc358840_state, delayed_work_enable_interrupt);
+        struct v4l2_subdev *sd = &state->sd;
+
+        struct v4l2_subdev_edid sd_edid = {
+                .blocks = 2,
+                .edid = edid,
+        };
+
+        v4l2_dbg(2, debug, sd, "%s:\n", __func__);
+
+        tc358840_enable_interrupts(sd, tx_5v_power_present(sd));
+
+        /* Temporary EDID. Should be set by userspace */
+        tc358840_s_edid(sd, &sd_edid);
+}
+
+static void tc358840_erase_bksv(struct v4l2_subdev *sd)
+{
+        int i;
+
+        for (i = 0; i < 5; i++)
+                i2c_wr8(sd, BKSV + i, 0);
+}
+
+/* --------------- AVI infoframe --------------- */
+
+static void print_avi_infoframe(struct v4l2_subdev *sd)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct device *dev = &client->dev;
+        union hdmi_infoframe frame;
+        u8 buffer[HDMI_INFOFRAME_SIZE(MAX)];
+
+        if (!is_hdmi(sd)) {
+                v4l2_info(sd, "DVI-D signal - AVI infoframe not supported\n");
+                return;
+        }
+
+        i2c_rd(sd, PK_AVI_0HEAD, buffer, HDMI_INFOFRAME_SIZE(AVI));
+
+        if (hdmi_infoframe_unpack(&frame, buffer) < 0) {
+                v4l2_err(sd, "%s: unpack of AVI infoframe failed\n", __func__);
+                return;
+        }
+
+        hdmi_infoframe_log(KERN_INFO, dev, &frame);
+}
+
+/* --------------- CTRLS --------------- */
+
+static int tc358840_s_ctrl_detect_tx_5v(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+
+        return v4l2_ctrl_s_ctrl(state->detect_tx_5v_ctrl,
+                tx_5v_power_present(sd));
+}
+
+static int tc358840_s_ctrl_audio_sampling_rate(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+
+        return v4l2_ctrl_s_ctrl(state->audio_sampling_rate_ctrl,
+                        get_audio_sampling_rate(sd));
+}
+
+static int tc358840_s_ctrl_audio_present(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+
+        return v4l2_ctrl_s_ctrl(state->audio_present_ctrl,
+                        audio_present(sd));
+}
+
+static int tc358840_update_controls(struct v4l2_subdev *sd)
+{
+        int ret = 0;
+
+        ret |= tc358840_s_ctrl_detect_tx_5v(sd);
+        ret |= tc358840_s_ctrl_audio_sampling_rate(sd);
+        ret |= tc358840_s_ctrl_audio_present(sd);
+
+        return ret;
+}
+
+/* --------------- INIT --------------- */
+
+static void tc358840_reset_phy(struct v4l2_subdev *sd)
+{
+        v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
+        i2c_wr8_and_or(sd, PHY_RST, ~MASK_RESET_CTRL, 0);
+        i2c_wr8_and_or(sd, PHY_RST, ~MASK_RESET_CTRL, MASK_RESET_CTRL);
+}
+
+static void tc358840_reset(struct v4l2_subdev *sd, u16 mask)
+{
+        u16 sysctl = i2c_rd16(sd, SYSCTL);
+
+        i2c_wr16(sd, SYSCTL, sysctl | mask);
+        i2c_wr16(sd, SYSCTL, sysctl & ~mask);
+}
+
+static inline void tc358840_sleep_mode(struct v4l2_subdev *sd, bool enable)
+{
+        v4l2_dbg(1, debug, sd, "%s(): %s\n", __func__,
+                enable ? "enable" : "disable");
+
+        i2c_wr16_and_or(sd, SYSCTL, ~MASK_SLEEP, enable ? MASK_SLEEP : 0);
+}
+
+static int enable_stream(struct v4l2_subdev *sd, bool enable)
+{
+        struct tc358840_state *state = to_state(sd);
+        struct tc358840_platform_data *pdata = &state->pdata;
+
+        u32 sync_timeout_ctr;
+
+        v4l2_dbg(2, debug, sd, "%s: %sable\n", __func__, enable ? "en" : "dis");
+
+        if (enable == state->enabled)
+                return 0;
+
+        if (enable) {
+#if 0           /* Wait until we can use the clock-noncontinuous property */
+                if (pdata->endpoint.bus.mipi_csi2.flags &
+                    V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK) {
+                        i2c_wr32_and_or(sd, FUNCMODE, ~(MASK_CONTCLKMODE),
+                                        MASK_FORCESTOP);
+                } else {
+                        /* It is critical for CSI receiver to see lane transition
+                         * LP11->HS. Set to non-continuous mode to enable clock lane
+                         * LP11 state. */
+                        i2c_wr32_and_or(sd, FUNCMODE, ~(MASK_CONTCLKMODE), 0);
+                        /* Set to continuous mode to trigger LP11->HS transition */
+                        i2c_wr32_and_or(sd, FUNCMODE, 0, MASK_CONTCLKMODE);
+                }
+#else
+                i2c_wr32_and_or(sd, FUNCMODE, ~(MASK_CONTCLKMODE),
+                                MASK_FORCESTOP);
+#endif
+                /* Unmute video */
+                i2c_wr8(sd, VI_MUTE, MASK_AUTO_MUTE);
+                /* Signal end of initialization */
+                i2c_wr8(sd, INIT_END, MASK_INIT_END);
+        } else {
+                /* Enable Registers to be initialized */
+                i2c_wr8_and_or(sd, INIT_END, ~(MASK_INIT_END), 0x00);
+
+                /* Mute video so that all data lanes go to LSP11 state.
+                 * No data is output to CSI Tx block. */
+
+                i2c_wr8(sd, VI_MUTE, MASK_AUTO_MUTE | MASK_VI_MUTE);
+                tc358840_set_csi(sd);
+                tc358840_set_splitter(sd);
+        }
+
+        /* Wait for HDMI input to become stable */
+        if (enable) {
+                sync_timeout_ctr = 100;
+                while (no_sync(sd) && sync_timeout_ctr)
+                        sync_timeout_ctr--;
+
+                if (sync_timeout_ctr == 0) {
+                        /* Disable stream again. Probably no cable inserted.. */
+                        v4l2_err(sd, "%s: Timeout: HDMI input sync failed.\n",
+                                        __func__);
+                        enable_stream(sd, false);
+                        return -EIO;
+                }
+
+                v4l2_dbg(2, debug, sd,
+                        "%s: Stream enabled! Remaining timeout attempts: %d\n",
+                        __func__, sync_timeout_ctr);
+        }
+
+        i2c_wr16_and_or(sd, CONFCTL0,
+                ~(MASK_VTX0EN | MASK_VTX1EN | MASK_ABUFEN),
+                enable ? ((pdata->csi_port & (MASK_VTX0EN | MASK_VTX1EN)) |
+                MASK_ABUFEN | MASK_TX_MSEL | MASK_AUTOINDEX) :
+                (MASK_TX_MSEL | MASK_AUTOINDEX));
+        state->enabled = enable;
+
+        return 0;
+}
+
+static void tc358840_set_splitter(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        if (state->timings.bt.width <= 1920) {
+                i2c_wr16_and_or(sd, SPLITTX0_CTRL, ~(MASK_IFEN | MASK_LCD_CSEL),
+                                MASK_SPBP);
+                i2c_wr16_and_or(sd, SPLITTX1_CTRL, ~(MASK_IFEN | MASK_LCD_CSEL),
+                                MASK_SPBP);
+
+                i2c_wr16_and_or(sd, SPLITTX0_SPLIT, (u16)~(MASK_TX1SEL | MASK_EHW), 0);
+        } else {
+                i2c_wr16_and_or(sd, SPLITTX0_CTRL, ~(MASK_IFEN | MASK_LCD_CSEL | MASK_SPBP), 0);
+                i2c_wr16_and_or(sd, SPLITTX1_CTRL, ~(MASK_IFEN | MASK_LCD_CSEL | MASK_SPBP), 0);
+
+                i2c_wr16_and_or(sd, SPLITTX0_SPLIT, ~(MASK_TX1SEL), MASK_EHW);
+        }
+}
+
+static void tc358840_set_pll(struct v4l2_subdev *sd,
+                                enum tc358840_csi_port port)
+{
+        struct tc358840_state *state = to_state(sd);
+        struct tc358840_platform_data *pdata = &state->pdata;
+        u16 base_addr;
+        u16 pll_frs;
+        u32 pllconf;
+        u32 pllconf_new;
+        u32 hsck;
+
+        v4l2_dbg(2, debug, sd, "%s:\n", __func__);
+
+        BUG_ON((pdata->csi_port <= CSI_TX_NONE) ||
+                (pdata->csi_port > CSI_TX_BOTH));
+
+        if (pdata->csi_port == CSI_TX_NONE) {
+                v4l2_err(sd, "%s: No CSI port defined!\n", __func__);
+                return;
+        }
+
+        base_addr = (port == CSI_TX_0) ? CSITX0_BASE_ADDR :
+                                        CSITX1_BASE_ADDR;
+        pllconf = i2c_rd32(sd, base_addr+PLLCONF);
+        pllconf_new = SET_PLL_PRD(pdata->pll_prd) |
+        SET_PLL_FBD(pdata->pll_fbd);
+
+        hsck = (pdata->refclk_hz / pdata->pll_prd) *
+                        pdata->pll_fbd;
+
+        if (hsck > 500000000)
+                pll_frs = 0x0;
+        else if (hsck > 250000000)
+                pll_frs = 0x1;
+        else if (hsck > 125000000)
+                pll_frs = 0x2;
+        else
+                pll_frs = 0x3;
+
+        v4l2_dbg(1, debug, sd, "%s: Updating PLL clock of CSI TX%d\n",
+                        __func__, port-1);
+
+        i2c_wr32(sd, base_addr+PLLCONF,
+                        pllconf_new | SET_PLL_FRS(pll_frs));
+}
+
+static void tc358840_set_ref_clk(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+        struct tc358840_platform_data *pdata = &state->pdata;
+
+        u32 sys_freq;
+        u32 lock_ref_freq;
+        u32 nco;
+        u16 csc;
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        BUG_ON((pdata->refclk_hz < 40000000) || (pdata->refclk_hz > 50000000));
+
+        /* System Frequency */
+        sys_freq = pdata->refclk_hz / 10000;
+        i2c_wr8(sd, SYS_FREQ0, sys_freq & 0x00FF);
+        i2c_wr8(sd, SYS_FREQ1, (sys_freq & 0xFF00) >> 8);
+
+        /* Audio System Frequency */
+        lock_ref_freq = pdata->refclk_hz / 100;
+        i2c_wr8(sd, LOCK_REF_FREQA, lock_ref_freq & 0xFF);
+        i2c_wr8(sd, LOCK_REF_FREQB, (lock_ref_freq >> 8) & 0xFF);
+        i2c_wr8(sd, LOCK_REF_FREQC, (lock_ref_freq >> 16) & 0x0F);
+
+        /* Audio PLL */
+        i2c_wr8(sd, NCO_F0_MOD, MASK_NCO_F0_MOD_REG);
+        /* 6.144 * 2^28 = 1649267442 */
+        nco = (1649267442 / (pdata->refclk_hz / 1000000));
+        i2c_wr8(sd, NCO_48F0A, nco & 0xFF);
+        i2c_wr8(sd, NCO_48F0B, (nco >> 8) & 0xFF);
+        i2c_wr8(sd, NCO_48F0C, (nco >> 16) & 0xFF);
+        i2c_wr8(sd, NCO_48F0D, (nco >> 24) & 0xFF);
+
+        /* Color Space Conversion */
+        csc = pdata->refclk_hz / 10000;
+        i2c_wr8(sd, SCLK_CSC0, csc & 0xFF);
+        i2c_wr8(sd, SCLK_CSC1, (csc >> 8) & 0xFF);
+}
+
+static void tc358840_set_csi_mbus_config(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        switch (state->mbus_fmt_code) {
+        case MEDIA_BUS_FMT_UYVY8_1X16:
+                v4l2_dbg(2, debug, sd, "%s: YCbCr 422 16-bit\n", __func__);
+
+                i2c_wr8(sd, VOUT_FMT, MASK_OUTFMT_422 | MASK_422FMT_NORMAL);
+                i2c_wr8(sd, VOUT_FIL, MASK_422FIL_3_TAP_444 |
+                        MASK_444FIL_2_TAP);
+                i2c_wr8(sd, VOUT_SYNC0, MASK_MODE_2);
+                i2c_wr8(sd, VOUT_CSC, MASK_CSC_MODE_BUILTIN |
+                        MASK_COLOR_601_YCBCR_LIMITED);
+                i2c_wr16_and_or(sd, CONFCTL0, ~(MASK_YCBCRFMT),
+                        MASK_YCBCRFMT_YCBCR422_8);
+                i2c_wr16(sd, CONFCTL1, 0x0);
+                break;
+
+        case MEDIA_BUS_FMT_RGB888_1X24:
+                v4l2_dbg(2, debug, sd, "%s: RGB 888 24-bit\n", __func__);
+
+                i2c_wr8(sd, VOUT_FMT, MASK_OUTFMT_444_RGB);
+                i2c_wr8(sd, VOUT_FIL, MASK_422FIL_3_TAP_444 |
+                        MASK_444FIL_2_TAP);
+                i2c_wr8(sd, VOUT_SYNC0, MASK_MODE_2);
+                i2c_wr8(sd, VOUT_CSC, MASK_CSC_MODE_BUILTIN |
+                        MASK_COLOR_RGB_LIMITED);
+                i2c_wr16_and_or(sd, CONFCTL0, ~(MASK_YCBCRFMT), 0x0);
+                i2c_wr16_and_or(sd, CONFCTL1, 0x0, MASK_TX_OUT_FMT_RGB888);
+                break;
+
+        default:
+                v4l2_dbg(2, debug, sd, "%s: Unsupported format code 0x%x\n",
+                                __func__, state->mbus_fmt_code);
+                break;
+        }
+}
+
+static unsigned tc358840_num_csi_lanes_needed(struct v4l2_subdev *sd)
+{
+
+        /* TODO: Check if this can be useful */
+#if 0
+        struct tc358840_state *state = to_state(sd);
+        struct v4l2_bt_timings *bt = &state->timings.bt;
+        struct tc358840_platform_data *pdata = &state->pdata;
+        u32 bits_pr_pixel =
+                (state->mbus_fmt_code == MEDIA_BUS_FMT_UYVY8_1X16) ?  16 : 24;
+        u32 bps = bt->width * bt->height * fps(bt) * bits_pr_pixel;
+        u32 bps_pr_lane = (pdata->refclk_hz / pdata->pll_prd) * pdata->pll_fbd;
+
+        return DIV_ROUND_UP(bps, bps_pr_lane);
+#endif
+
+        /* Always use 4 lanes for one CSI */
+        return 4;
+}
+
+static void tc358840_set_csi(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+        struct tc358840_platform_data *pdata = &state->pdata;
+#if 0
+        struct v4l2_bt_timings *bt = &state->timings.bt;
+#endif
+        unsigned lanes = tc358840_num_csi_lanes_needed(sd);
+
+        enum tc358840_csi_port port;
+        u16 base_addr;
+
+        v4l2_dbg(3, debug, sd, "%s:\n", __func__);
+
+        tc358840_reset(sd, MASK_CTXRST);
+
+        for (port = CSI_TX_0; port <= CSI_TX_1; port++) {
+                base_addr = (port == CSI_TX_0) ? CSITX0_BASE_ADDR :
+                                                 CSITX1_BASE_ADDR;
+
+                if (pdata->csi_port != CSI_TX_BOTH &&
+                    pdata->csi_port != port) {
+                        v4l2_dbg(1, debug, sd,
+                                "%s: Disabling CSI TX%d\n", __func__, port-1);
+
+                        /* Disable CSI lanes (High Z)*/
+                        i2c_wr32_and_or(sd, base_addr+LANEEN,
+                                ~(MASK_CLANEEN), 0);
+                        continue;
+                }
+
+                v4l2_dbg(1, debug, sd,
+                        "%s: Enabling CSI TX%d\n", __func__, port-1);
+
+                /* (0x0108) */
+                i2c_wr32(sd, base_addr+CSITX_CLKEN, MASK_CSITX_EN);
+                /*
+                 * PLL has to be enabled between CSITX_CLKEN and
+                 * LANEEN (0x02AC)
+                 */
+                tc358840_set_pll(sd, port);
+                /* (0x02A0) */
+                i2c_wr32_and_or(sd, base_addr+MIPICLKEN,
+                        ~(MASK_MP_CKEN), MASK_MP_ENABLE);
+                usleep_range(10000, 11000);
+                /* (0x02A0) */
+                i2c_wr32(sd, base_addr+MIPICLKEN,
+                        MASK_MP_CKEN | MASK_MP_ENABLE);
+                /* (0x010C) */
+                i2c_wr32(sd, base_addr+PPICLKEN, MASK_HSTXCLKEN);
+                /* (0x0118) */
+                i2c_wr32(sd, base_addr+LANEEN,
+                        (lanes & MASK_LANES) | MASK_CLANEEN);
+
+                /* (0x0120) */
+                i2c_wr32(sd, base_addr+LINEINITCNT, pdata->lineinitcnt);
+
+                /*
+                 * TODO: Check if this is the correct register
+                 * (0x0150)
+                 */
+                i2c_rd32(sd, base_addr+FUNCMODE);
+                /* (0x0254) */
+                i2c_wr32(sd, base_addr+LPTXTIMECNT, pdata->lptxtimecnt);
+                /* (0x0258) */
+                i2c_wr32(sd, base_addr+TCLK_HEADERCNT,
+                        pdata->tclk_headercnt);
+                /* (0x025C) */
+                i2c_wr32(sd, base_addr+TCLK_TRAILCNT,
+                        pdata->tclk_trailcnt);
+                /* (0x0260) */
+                i2c_wr32(sd, base_addr+THS_HEADERCNT,
+                        pdata->ths_headercnt);
+                /* (0x0264) */
+                i2c_wr32(sd, base_addr+TWAKEUP, pdata->twakeup);
+                /* (0x0268) */
+                i2c_wr32(sd, base_addr+TCLK_POSTCNT,
+                        pdata->tclk_postcnt);
+                /* (0x026C) */
+                i2c_wr32(sd, base_addr+THS_TRAILCNT,
+                        pdata->ths_trailcnt);
+
+                /* (0x0270) */
+                i2c_wr32(sd, base_addr+HSTXVREGCNT, pdata->hstxvregcnt);
+
+                /* (0x0274) */
+                i2c_wr32(sd, base_addr+HSTXVREGEN,
+                        ((lanes > 0) ? MASK_CLM_HSTXVREGEN : 0x0) |
+                        ((lanes > 0) ? MASK_D0M_HSTXVREGEN : 0x0) |
+                        ((lanes > 1) ? MASK_D1M_HSTXVREGEN : 0x0) |
+                        ((lanes > 2) ? MASK_D2M_HSTXVREGEN : 0x0) |
+                        ((lanes > 3) ? MASK_D3M_HSTXVREGEN : 0x0));
+
+                /*
+                 * Finishing configuration by setting CSITX to start
+                 * (0X011C)
+                 */
+                i2c_wr32(sd, base_addr+CSITX_START, 0x00000001);
+
+                i2c_rd32(sd, base_addr+CSITX_INTERNAL_STAT);
+        }
+}
+
+static void tc358840_set_hdmi_phy(struct v4l2_subdev *sd)
+{
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        /* Reset PHY */
+        tc358840_reset_phy(sd);
+
+        /* Set PHY to manual */
+        i2c_wr8(sd, PHY_CTL, MASK_48_MHZ);
+
+        /* Enable PHY */
+        i2c_wr8_and_or(sd, PHY_ENB, ~MASK_ENABLE_PHY, 0x0);
+        i2c_wr8_and_or(sd, PHY_ENB, ~MASK_ENABLE_PHY, MASK_ENABLE_PHY);
+
+        /* Enable Audio PLL */
+        i2c_wr8(sd, APPL_CTL, MASK_APLL_CPCTL_NORMAL | MASK_APLL_ON);
+
+        /* Enable DDC IO */
+        i2c_wr8(sd, DDCIO_CTL, MASK_DDC_PWR_ON);
+}
+
+static void tc358840_set_hdmi_audio(struct v4l2_subdev *sd)
+{
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        i2c_wr8(sd, FORCE_MUTE, 0x00);
+        i2c_wr8(sd, AUTO_CMD0, MASK_AUTO_MUTE7 | MASK_AUTO_MUTE6 |
+                        MASK_AUTO_MUTE5 | MASK_AUTO_MUTE4 |
+                        MASK_AUTO_MUTE1 | MASK_AUTO_MUTE0);
+        i2c_wr8(sd, AUTO_CMD1, MASK_AUTO_MUTE9);
+        i2c_wr8(sd, AUTO_CMD2, MASK_AUTO_PLAY3 | MASK_AUTO_PLAY2);
+        i2c_wr8(sd, BUFINIT_START, SET_BUFINIT_START_MS(500));
+        i2c_wr8(sd, FS_MUTE, 0x00);
+        i2c_wr8(sd, FS_IMODE, MASK_NLPCM_SMODE | MASK_FS_SMODE);
+        i2c_wr8(sd, ACR_MODE, MASK_CTS_MODE);
+        i2c_wr8(sd, ACR_MDF0, MASK_ACR_L2MDF_1976_PPM | MASK_ACR_L1MDF_976_PPM);
+        i2c_wr8(sd, ACR_MDF1, MASK_ACR_L3MDF_3906_PPM);
+        /*
+         * TODO: Set output data bit length (currently 16 bit, 8 bit discarded)
+         */
+        i2c_wr8(sd, SDO_MODE1, MASK_SDO_FMT_I2S);
+        i2c_wr8(sd, DIV_MODE, SET_DIV_DLY_MS(100));
+        i2c_wr16_and_or(sd, CONFCTL0, 0xFFFF, MASK_AUDCHNUM_2 |
+                        MASK_AUDOUTSEL_I2S | MASK_AUTOINDEX);
+}
+
+static void tc358840_set_hdmi_info_frame_mode(struct v4l2_subdev *sd)
+{
+        v4l2_dbg(3, debug, sd, "%s(): DUMMY\n", __func__);
+
+        /* TODO: Check which registers are needed/available */
+#if 0
+        i2c_wr8(sd, PK_INT_MODE, MASK_ISRC2_INT_MODE | MASK_ISRC_INT_MODE |
+                        MASK_ACP_INT_MODE | MASK_VS_INT_MODE |
+                        MASK_SPD_INT_MODE | MASK_MS_INT_MODE |
+                        MASK_AUD_INT_MODE | MASK_AVI_INT_MODE);
+        i2c_wr8(sd, NO_PKT_LIMIT, 0x2c);
+        i2c_wr8(sd, NO_PKT_CLR, 0x53);
+        i2c_wr8(sd, ERR_PK_LIMIT, 0x01);
+        i2c_wr8(sd, NO_PKT_LIMIT2, 0x30);
+        i2c_wr8(sd, NO_GDB_LIMIT, 0x10);
+#endif
+}
+
+static void tc358840_initial_setup(struct v4l2_subdev *sd)
+{
+        static struct v4l2_dv_timings default_timing = V4L2_DV_BT_CEA_1920X1080P60;
+        struct tc358840_state *state = to_state(sd);
+        struct tc358840_platform_data *pdata = &state->pdata;
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        /* *** Reset *** */
+        enable_stream(sd, false);
+
+        tc358840_sleep_mode(sd, false);
+        tc358840_reset(sd, MASK_RESET_ALL);
+
+        tc358840_init_interrupts(sd);
+
+        /* *** Init CSI *** */
+        tc358840_s_dv_timings(sd, &default_timing);
+
+        tc358840_set_ref_clk(sd);
+
+        i2c_wr8_and_or(sd, DDC_CTL, ~MASK_DDC5V_MODE,
+                       pdata->ddc5v_delay & MASK_DDC5V_MODE);
+
+        i2c_wr8_and_or(sd, EDID_MODE, ~MASK_EDID_MODE_ALL, MASK_RAM_EDDC);
+
+        i2c_wr8_and_or(sd, HPD_CTL, ~MASK_HPD_CTL0, 0);
+
+        tc358840_set_hdmi_phy(sd);
+
+        tc358840_set_hdmi_hdcp(sd, pdata->enable_hdcp);
+        tc358840_set_hdmi_audio(sd);
+        tc358840_set_hdmi_info_frame_mode(sd);
+
+        /* All CE and IT formats are detected as RGB full range in DVI mode */
+        i2c_wr8_and_or(sd, VI_MODE, ~MASK_RGB_DVI, 0);
+}
+
+/* --------------- IRQ --------------- */
+
+static void tc358840_format_change(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+        struct v4l2_dv_timings timings;
+        static const struct v4l2_event tc358840_ev_fmt = {
+                .type = V4L2_EVENT_SOURCE_CHANGE,
+                .u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+        };
+
+        if (tc358840_get_detected_timings(sd, &timings)) {
+                enable_stream(sd, false);
+
+                v4l2_info(sd, "%s: No Signal\n", __func__);
+        } else {
+                if (!v4l2_match_dv_timings(&state->timings, &timings, 0))
+                        enable_stream(sd, false);
+
+                v4l2_print_dv_timings(sd->name,
+                                "tc358840_format_change: New format: ",
+                                &timings, false);
+        }
+
+        /* Application gets notified after CSI Tx's are reset */
+        if (sd->devnode)
+                v4l2_subdev_notify_event(sd, &tc358840_ev_fmt);
+}
+
+static void tc358840_init_interrupts(struct v4l2_subdev *sd)
+{
+        u16 i;
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        /* clear interrupt status registers */
+        for (i = SYS_INT; i <= MISC_INT; i++) {
+                /* No interrupt register at Address 0x850A */
+                if (i != 0x850A)
+                        i2c_wr8(sd, i, 0xFF);
+        }
+
+        /* Clear and disable all interrupts */
+        i2c_wr16(sd, INTSTATUS, MASK_INT_STATUS_MASK_ALL);
+        i2c_wr16(sd, INTSTATUS, 0x0);
+
+        i2c_wr16(sd, INTMASK, MASK_INT_STATUS_MASK_ALL);
+}
+
+static void tc358840_enable_interrupts(struct v4l2_subdev *sd,
+                bool cable_connected)
+{
+        v4l2_dbg(2, debug, sd, "%s: cable connected = %d\n", __func__,
+                        cable_connected);
+
+        if (cable_connected) {
+                i2c_wr8(sd, SYS_INTM, ~(MASK_DDC | MASK_DVI |
+                                        MASK_HDMI) & 0xFF);
+                i2c_wr8(sd, SYS_INTM, ~(MASK_DDC) & 0xFF);
+                i2c_wr8(sd, CLK_INTM, ~MASK_IN_DE_CHG);
+                i2c_wr8(sd, CBIT_INTM, ~(MASK_CBIT_FS | MASK_AF_LOCK |
+                                        MASK_AF_UNLOCK) & 0xFF);
+                i2c_wr8(sd, AUDIO_INTM, ~MASK_BUFINIT_END);
+                i2c_wr8(sd, MISC_INTM, ~MASK_SYNC_CHG);
+        } else {
+                i2c_wr8(sd, SYS_INTM, ~MASK_DDC & 0xFF);
+                i2c_wr8(sd, CLK_INTM, 0xFF);
+                i2c_wr8(sd, CBIT_INTM, 0xFF);
+                i2c_wr8(sd, AUDIO_INTM, 0xFF);
+                i2c_wr8(sd, MISC_INTM, 0xFF);
+        }
+}
+
+static void tc358840_hdmi_audio_int_handler(struct v4l2_subdev *sd,
+                bool *handled)
+{
+        u8 audio_int_mask = i2c_rd8(sd, AUDIO_INTM);
+        u8 audio_int = i2c_rd8(sd, AUDIO_INT) & ~audio_int_mask;
+
+        i2c_wr8(sd, AUDIO_INT, audio_int);
+
+        v4l2_dbg(3, debug, sd, "%s: AUDIO_INT = 0x%02x\n", __func__, audio_int);
+
+        tc358840_s_ctrl_audio_sampling_rate(sd);
+        tc358840_s_ctrl_audio_present(sd);
+}
+
+static void tc358840_hdmi_misc_int_handler(struct v4l2_subdev *sd,
+                bool *handled)
+{
+        struct tc358840_state *state = to_state(sd);
+        u8 misc_int_mask = i2c_rd8(sd, MISC_INTM);
+        u8 misc_int = i2c_rd8(sd, MISC_INT) & ~misc_int_mask;
+
+        i2c_wr8(sd, MISC_INT, misc_int);
+
+        v4l2_dbg(3, debug, sd, "%s: MISC_INT = 0x%02x\n", __func__, misc_int);
+
+        if (misc_int & MASK_SYNC_CHG) {
+                /* Reset the HDMI PHY to try to trigger proper lock on the
+                 * incoming video format. Erase BKSV to prevent that old keys
+                 * are used when a new source is connected. */
+
+                state->format_changed = true;
+
+                misc_int &= ~MASK_SYNC_CHG;
+                if (handled)
+                        *handled = true;
+        }
+
+        if (misc_int) {
+                v4l2_err(sd, "%s: Unhandled MISC_INT interrupts: 0x%02x\n",
+                                __func__, misc_int);
+        }
+}
+
+static void tc358840_hdmi_cbit_int_handler(struct v4l2_subdev *sd,
+                bool *handled)
+{
+        u8 cbit_int_mask = i2c_rd8(sd, CBIT_INTM);
+        u8 cbit_int = i2c_rd8(sd, CBIT_INT) & ~cbit_int_mask;
+
+        i2c_wr8(sd, CBIT_INT, cbit_int);
+
+        v4l2_dbg(3, debug, sd, "%s: CBIT_INT = 0x%02x\n", __func__, cbit_int);
+
+        if (cbit_int & MASK_CBIT_FS) {
+
+                v4l2_dbg(1, debug, sd, "%s: Audio sample rate changed\n",
+                                __func__);
+                tc358840_s_ctrl_audio_sampling_rate(sd);
+
+                cbit_int &= ~MASK_CBIT_FS;
+                if (handled)
+                        *handled = true;
+        }
+
+        if (cbit_int & (MASK_AF_LOCK | MASK_AF_UNLOCK)) {
+
+                v4l2_dbg(1, debug, sd, "%s: Audio present changed\n",
+                                __func__);
+                tc358840_s_ctrl_audio_present(sd);
+
+                cbit_int &= ~(MASK_AF_LOCK | MASK_AF_UNLOCK);
+                if (handled)
+                        *handled = true;
+        }
+
+        if (cbit_int) {
+                v4l2_err(sd, "%s: Unhandled CBIT_INT interrupts: 0x%02x\n",
+                                __func__, cbit_int);
+        }
+}
+
+static void tc358840_hdmi_clk_int_handler(struct v4l2_subdev *sd, bool *handled)
+{
+        struct tc358840_state *state = to_state(sd);
+        u8 clk_int_mask = i2c_rd8(sd, CLK_INTM);
+        u8 clk_int = i2c_rd8(sd, CLK_INT) & ~clk_int_mask;
+
+        /* Bit 7 and bit 6 are set even when they are masked */
+        i2c_wr8(sd, CLK_INT, clk_int | 0x80 | MASK_OUT_H_CHG);
+
+        v4l2_dbg(3, debug, sd, "%s: CLK_INT = 0x%02x\n", __func__, clk_int);
+
+        if (clk_int & (MASK_IN_DE_CHG)) {
+
+                v4l2_dbg(1, debug, sd, "%s: DE size or position has changed\n",
+                                __func__);
+
+                /* TODO: Check if also true for tc358840 */
+                /* If the source switch to a new resolution with the same pixel
+                 * frequency as the existing (e.g. 1080p25 -> 720p50), the
+                 * I_SYNC_CHG interrupt is not always triggered, while the
+                 * I_IN_DE_CHG interrupt seems to work fine. FMT_CHANGE
+                 * notifications are only sent when the signal is stable to
+                 * reduce the number of notifications. */
+                if (!no_signal(sd) && !no_sync(sd))
+                        state->format_changed = true;
+
+                clk_int &= ~MASK_IN_DE_CHG;
+                if (handled)
+                        *handled = true;
+        }
+
+        if (clk_int) {
+                v4l2_err(sd, "%s: Unhandled CLK_INT interrupts: 0x%02x\n",
+                                __func__, clk_int);
+        }
+}
+
+static void tc358840_hdmi_sys_int_handler(struct v4l2_subdev *sd, bool *handled)
+{
+        struct tc358840_state *state = to_state(sd);
+        u8 sys_int_mask = i2c_rd8(sd, SYS_INTM);
+        u8 sys_int = i2c_rd8(sd, SYS_INT) & ~sys_int_mask;
+
+        i2c_wr8(sd, SYS_INT, sys_int);
+
+        v4l2_dbg(3, debug, sd, "%s: SYS_INT = 0x%02x\n", __func__, sys_int);
+
+        if (sys_int & MASK_DDC) {
+                bool tx_5v = tx_5v_power_present(sd);
+
+                v4l2_dbg(1, debug, sd, "%s: Tx 5V power present: %s\n",
+                                __func__, tx_5v ?  "yes" : "no");
+
+                if (tx_5v) {
+                        tc358840_enable_edid(sd);
+                } else {
+                        tc358840_enable_interrupts(sd, false);
+                        tc358840_disable_edid(sd);
+                        memset(&state->timings, 0, sizeof(state->timings));
+                        tc358840_erase_bksv(sd);
+                        tc358840_update_controls(sd);
+                }
+
+                sys_int &= ~MASK_DDC;
+                if (handled)
+                        *handled = true;
+        }
+
+        if (sys_int & MASK_DVI) {
+                v4l2_dbg(1, debug, sd, "%s: HDMI->DVI change detected\n",
+                                __func__);
+
+                /* Reset the HDMI PHY to try to trigger proper lock on the
+                 * incoming video format. Erase BKSV to prevent that old keys
+                 * are used when a new source is connected. */
+                if (no_sync(sd) || no_signal(sd))
+                        state->format_changed = true;
+
+                sys_int &= ~MASK_DVI;
+                if (handled)
+                        *handled = true;
+        }
+
+        if (sys_int & MASK_HDMI) {
+                v4l2_dbg(1, debug, sd, "%s: DVI->HDMI change detected\n",
+                                __func__);
+
+                /* TODO: Check if reg is required. Reg not found in Rev. 1.5 */
+#if 0
+                i2c_wr8(sd, ANA_CTL, MASK_APPL_PCSX_NORMAL | MASK_ANALOG_ON);
+#endif
+                sys_int &= ~MASK_HDMI;
+                if (handled)
+                        *handled = true;
+        }
+
+        if (sys_int) {
+                v4l2_err(sd, "%s: Unhandled SYS_INT interrupts: 0x%02x\n",
+                                __func__, sys_int);
+        }
+}
+
+/* --------------- CORE OPS --------------- */
+
+static int tc358840_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
+{
+        struct tc358840_state *state = to_state(sd);
+        u16 intstatus;
+        unsigned retry = 10;
+
+        //disable_irq_nosync(state->pdata.interrupt);
+        intstatus = i2c_rd16(sd, INTSTATUS);
+
+        v4l2_dbg(1, debug, sd, "%s: IntStatus = 0x%04X\n", __func__, intstatus);
+
+        /*
+         * Need to figure out why these msleeps are needed, and which of these
+         * are needed. Without msleeps the interrupts just stop.
+         */
+        usleep_range(500, 1000);
+        state->format_changed = false;
+        if (intstatus & MASK_HDMI_INT) {
+                u8 hdmi_int0;
+                u8 hdmi_int1;
+retry:
+                retry--;
+                hdmi_int0 = i2c_rd8(sd, HDMI_INT0);
+                usleep_range(500, 1000);
+                hdmi_int1 = i2c_rd8(sd, HDMI_INT1);
+                usleep_range(500, 1000);
+
+                if (hdmi_int0 & MASK_MISC)
+                        tc358840_hdmi_misc_int_handler(sd, handled);
+                if (hdmi_int1 & MASK_ACBIT)
+                        tc358840_hdmi_cbit_int_handler(sd, handled);
+                if (hdmi_int1 & MASK_CLK)
+                        tc358840_hdmi_clk_int_handler(sd, handled);
+                if (hdmi_int1 & MASK_SYS)
+                        tc358840_hdmi_sys_int_handler(sd, handled);
+                if (hdmi_int1 & MASK_AUD)
+                        tc358840_hdmi_audio_int_handler(sd, handled);
+
+                usleep_range(500, 1000);
+                i2c_wr16(sd, INTSTATUS, MASK_HDMI_INT);
+                intstatus &= ~MASK_HDMI_INT;
+                usleep_range(500, 1000);
+
+                /* Display unhandled HDMI interrupts */
+                hdmi_int0 = i2c_rd8(sd, HDMI_INT0);
+                if (hdmi_int0) {
+                        v4l2_dbg(1, debug, sd,
+                                "%s: Unhandled HDMI_INT0 interrupts: 0x%02X\n",
+                                __func__, hdmi_int0);
+                        if (retry)
+                                goto retry;
+                }
+                usleep_range(500, 1000);
+                hdmi_int1 = i2c_rd8(sd, HDMI_INT1);
+                if (hdmi_int1) {
+                        v4l2_dbg(1, debug, sd,
+                                "%s: Unhandled HDMI_INT1 interrupts: 0x%02X\n",
+                                __func__, hdmi_int1);
+                        if (retry)
+                                goto retry;
+                }
+        }
+
+        if (handled && state->format_changed) {
+                tc358840_format_change(sd);
+                if (no_sync(sd) || no_signal(sd)) {
+                        tc358840_reset_phy(sd);
+                        tc358840_erase_bksv(sd);
+                }
+        }
+
+        if (intstatus & MASK_CSITX0_INT) {
+                v4l2_dbg(3, debug, sd, "%s: MASK_CSITX0_INT\n", __func__);
+
+                i2c_wr16(sd, INTSTATUS, MASK_CSITX0_INT);
+                intstatus &= ~MASK_CSITX0_INT;
+        }
+
+        if (intstatus & MASK_CSITX1_INT) {
+                v4l2_dbg(3, debug, sd, "%s: MASK_CSITX1_INT\n", __func__);
+
+                i2c_wr16(sd, INTSTATUS, MASK_CSITX1_INT);
+                intstatus &= ~MASK_CSITX1_INT;
+        }
+
+        if (intstatus) {
+                v4l2_dbg(1, debug, sd,
+                        "%s: Unhandled IntStatus interrupts: 0x%04x\n",
+                        __func__, intstatus);
+        }
+        //enable_irq(state->pdata.interrupt);
+        return 0;
+}
+
+static void tc358840_process_isr(struct work_struct *work)
+{
+        struct tc358840_state *state = container_of(work,
+                struct tc358840_state, process_isr);
+        struct v4l2_subdev *sd = &state->sd;
+        bool handled;
+
+        v4l2_dbg(2, debug, sd, "%s:\n", __func__);
+
+        mutex_lock(&state->isr_lock);
+        tc358840_isr(sd, 0, &handled);
+        mutex_unlock(&state->isr_lock);
+}
+
+static irqreturn_t tc358840_irq_handler(int irq, void *dev_id)
+{
+        struct v4l2_subdev *sd = dev_id;
+        struct tc358840_state *state = to_state(sd);
+
+        queue_work(state->work_queues, &state->process_isr);
+
+        return IRQ_HANDLED;
+}
+
+/* --------------- PAD OPS --------------- */
+
+static int tc358840_get_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        struct tc358840_state *state = to_state(sd);
+        u8 vout_csc = i2c_rd8(sd, VOUT_CSC);
+        struct v4l2_mbus_framefmt *fmt;
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        if (format->pad != 0)
+                return -EINVAL;
+
+        format->format.code = state->mbus_fmt_code;
+        format->format.width = state->timings.bt.width;
+        format->format.height = state->timings.bt.height;
+        format->format.field = V4L2_FIELD_NONE;
+
+        switch (vout_csc & MASK_COLOR) {
+        case MASK_COLOR_RGB_FULL:
+        case MASK_COLOR_RGB_LIMITED:
+                format->format.colorspace = V4L2_COLORSPACE_SRGB;
+                break;
+        case MASK_COLOR_601_YCBCR_FULL:
+        case MASK_COLOR_601_YCBCR_LIMITED:
+                format->format.colorspace = V4L2_COLORSPACE_SMPTE170M;
+                break;
+        case MASK_COLOR_709_YCBCR_FULL:
+        case MASK_COLOR_709_YCBCR_LIMITED:
+                format->format.colorspace = V4L2_COLORSPACE_REC709;
+                break;
+        default:
+                format->format.colorspace = 0;
+                break;
+        }
+
+        fmt = &format->format;
+        v4l2_dbg(3, debug, sd,
+                "%s(): width=%d, height=%d, code=0x%08X, field=%d\n",
+                __func__, fmt->width, fmt->height, fmt->code, fmt->field);
+
+        return 0;
+}
+
+static int tc358840_set_fmt(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_format *format)
+{
+        struct tc358840_state *state = to_state(sd);
+        u32 code = format->format.code; /* is overwritten by get_fmt */
+        int ret = tc358840_get_fmt(sd, cfg, format);
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        format->format.code = code;
+
+        if (ret)
+                return ret;
+
+        switch (code) {
+        case MEDIA_BUS_FMT_RGB888_1X24:
+        case MEDIA_BUS_FMT_UYVY8_1X16:
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+                return 0;
+
+        v4l2_dbg(3, debug, sd, "%s(): format->which=%d\n",
+                __func__, format->which);
+
+        state->mbus_fmt_code = format->format.code;
+        enable_stream(sd, false);
+        tc358840_set_csi(sd);
+        tc358840_set_csi_mbus_config(sd);
+
+        return 0;
+}
+
+static int tc358840_g_edid(struct v4l2_subdev *sd,
+                struct v4l2_subdev_edid *edid)
+{
+        struct tc358840_state *state = to_state(sd);
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        memset(edid->reserved, 0, sizeof(edid->reserved));
+
+        if (edid->pad != 0)
+                return -EINVAL;
+
+        if (edid->start_block == 0 && edid->blocks == 0) {
+                edid->blocks = state->edid_blocks_written;
+                return 0;
+        }
+
+        if (state->edid_blocks_written == 0)
+                return -ENODATA;
+
+        if (edid->start_block >= state->edid_blocks_written ||
+                        edid->blocks == 0)
+                return -EINVAL;
+
+        if (edid->start_block + edid->blocks > state->edid_blocks_written)
+                edid->blocks = state->edid_blocks_written - edid->start_block;
+
+        i2c_rd(sd, EDID_RAM + (edid->start_block * EDID_BLOCK_SIZE), edid->edid,
+                   edid->blocks * EDID_BLOCK_SIZE);
+
+        return 0;
+}
+
+static int tc358840_s_edid(struct v4l2_subdev *sd,
+                                struct v4l2_subdev_edid *edid)
+{
+        struct tc358840_state *state = to_state(sd);
+        u16 edid_len = edid->blocks * EDID_BLOCK_SIZE;
+        int i;
+
+        v4l2_dbg(2, debug, sd, "%s, pad %d, start block %d, blocks %d\n",
+                 __func__, edid->pad, edid->start_block, edid->blocks);
+
+        memset(edid->reserved, 0, sizeof(edid->reserved));
+
+        if (edid->pad != 0)
+                return -EINVAL;
+
+        if (edid->start_block != 0)
+                return -EINVAL;
+
+        if (edid->blocks > EDID_NUM_BLOCKS_MAX) {
+                edid->blocks = EDID_NUM_BLOCKS_MAX;
+                return -E2BIG;
+        }
+
+        tc358840_disable_edid(sd);
+
+        i2c_wr8(sd, EDID_LEN1, edid_len & 0xFF);
+        i2c_wr8(sd, EDID_LEN2, edid_len >> 8);
+
+        if (edid->blocks == 0) {
+                state->edid_blocks_written = 0;
+                return 0;
+        }
+
+        for (i = 0; i < 256; i++) {
+                unsigned int val;
+                val = edid->edid[i];
+                i2c_wr8(sd, EDID_RAM + i, val);
+        }
+
+        state->edid_blocks_written = edid->blocks;
+
+        if (tx_5v_power_present(sd))
+                tc358840_enable_edid(sd);
+
+        return 0;
+}
+
+static int tc358840_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+                                    struct v4l2_event_subscription *sub)
+{
+        switch (sub->type) {
+        case V4L2_EVENT_SOURCE_CHANGE:
+                return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
+        case V4L2_EVENT_CTRL:
+                return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
+        default:
+                return -EINVAL;
+        }
+}
+
+/* --------------- VIDEO OPS --------------- */
+
+static int tc358840_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        *status = 0;
+        *status |= no_signal(sd) ? V4L2_IN_ST_NO_SIGNAL : 0;
+        *status |= no_sync(sd) ? V4L2_IN_ST_NO_SYNC : 0;
+
+        v4l2_dbg(1, debug, sd, "%s: status = 0x%x\n", __func__, *status);
+
+        return 0;
+}
+
+static int tc358840_s_dv_timings(struct v4l2_subdev *sd,
+                                 struct v4l2_dv_timings *timings)
+{
+        struct tc358840_state *state = to_state(sd);
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        if (!timings)
+                return -EINVAL;
+
+        if (debug)
+                v4l2_print_dv_timings(sd->name, "tc358840_s_dv_timings: ",
+                                timings, false);
+
+        if (v4l2_match_dv_timings(&state->timings, timings, 0)) {
+                v4l2_dbg(1, debug, sd, "%s: no change\n", __func__);
+                return 0;
+        }
+
+        if (!v4l2_valid_dv_timings(timings, &tc358840_timings_cap, NULL, NULL)) {
+                v4l2_dbg(1, debug, sd, "%s: timings out of range\n", __func__);
+                return -ERANGE;
+        }
+
+        state->timings = *timings;
+
+        enable_stream(sd, false);
+        tc358840_set_csi(sd);
+        tc358840_set_splitter(sd);
+
+        return 0;
+}
+
+static int tc358840_g_dv_timings(struct v4l2_subdev *sd,
+                                 struct v4l2_dv_timings *timings)
+{
+        struct tc358840_state *state = to_state(sd);
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        *timings = state->timings;
+
+        return 0;
+}
+
+static int tc358840_enum_dv_timings(struct v4l2_subdev *sd,
+                                    struct v4l2_enum_dv_timings *timings)
+{
+        v4l2_dbg(3, debug, sd, "%s(): DUMMY\n", __func__);
+
+        if (timings->pad != 0)
+                return -EINVAL;
+
+        return v4l2_enum_dv_timings_cap(timings,
+                        &tc358840_timings_cap, NULL, NULL);
+}
+
+static int tc358840_query_dv_timings(struct v4l2_subdev *sd,
+                                     struct v4l2_dv_timings *timings)
+{
+        int ret;
+
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        ret = tc358840_get_detected_timings(sd, timings);
+        if (ret)
+                return ret;
+
+        if (debug)
+                v4l2_print_dv_timings(sd->name, "tc358840_query_dv_timings: ",
+                        timings, false);
+        if (!v4l2_valid_dv_timings(timings, &tc358840_timings_cap, NULL, NULL)) {
+                v4l2_dbg(1, debug, sd, "%s: timings out of range\n", __func__);
+                return -ERANGE;
+        }
+
+        return 0;
+}
+
+static int tc358840_dv_timings_cap(struct v4l2_subdev *sd,
+                                   struct v4l2_dv_timings_cap *cap)
+{
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        if (cap->pad != 0)
+                return -EINVAL;
+
+        *cap = tc358840_timings_cap;
+
+        return 0;
+}
+
+static int tc358840_g_mbus_config(struct v4l2_subdev *sd,
+                                  struct v4l2_mbus_config *cfg)
+{
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        cfg->type = V4L2_MBUS_CSI2;
+
+        /* Support for non-continuous CSI-2 clock is missing in the driver */
+        cfg->flags = V4L2_MBUS_CSI2_CONTINUOUS_CLOCK | V4L2_MBUS_CSI2_CHANNEL_0;
+
+        switch (tc358840_num_csi_lanes_in_use(sd)) {
+        case 1:
+                cfg->flags |= V4L2_MBUS_CSI2_1_LANE;
+                break;
+        case 2:
+                cfg->flags |= V4L2_MBUS_CSI2_2_LANE;
+                break;
+        case 3:
+                cfg->flags |= V4L2_MBUS_CSI2_3_LANE;
+                break;
+        case 4:
+                cfg->flags |= V4L2_MBUS_CSI2_4_LANE;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        v4l2_dbg(2, debug, sd, "%s: Lanes: 0x%02X\n",
+                __func__, cfg->flags & 0x0F);
+
+        return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int tc358840_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+        switch (reg->size) {
+        case 1:
+                reg->val = i2c_rd8(sd, reg->reg);
+                break;
+        case 2:
+                reg->val = i2c_rd16(sd, reg->reg);
+                break;
+        case 4:
+        default:
+                reg->val = i2c_rd32(sd, reg->reg);
+                break;
+        }
+        return 0;
+}
+
+static int tc358840_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+        switch (reg->size) {
+        case 1:
+                i2c_wr8(sd, reg->reg, reg->val);
+                break;
+        case 2:
+                i2c_wr16(sd, reg->reg, reg->val);
+                break;
+        case 4:
+        default:
+                i2c_wr32(sd, reg->reg, reg->val);
+                break;
+        }
+        return 0;
+}
+#endif
+
+static int tc358840_log_status(struct v4l2_subdev *sd)
+{
+        struct tc358840_state *state = to_state(sd);
+        struct v4l2_dv_timings timings;
+        u8 hdmi_sys_status =  i2c_rd8(sd, SYS_STATUS);
+        u16 sysctl = i2c_rd16(sd, SYSCTL);
+        u8 vi_status3 = i2c_rd8(sd, VI_STATUS3);
+        const int deep_color_mode[4] = { 8, 10, 12, 16 };
+        static const char * const input_color_space[] = {
+                "RGB", "YCbCr 601", "Adobe RGB", "YCbCr 709", "NA (4)",
+                "xvYCC 601", "NA(6)", "xvYCC 709", "NA(8)", "sYCC601",
+                "NA(10)", "NA(11)", "NA(12)", "Adobe YCC 601"};
+
+        v4l2_ctrl_subdev_log_status(sd);
+        v4l2_info(sd, "-----Chip status-----\n");
+        v4l2_info(sd, "Chip ID: 0x%02x\n",
+                        (i2c_rd16(sd, CHIPID_ADDR) & MASK_CHIPID) >> 8);
+        v4l2_info(sd, "Chip revision: 0x%02x\n",
+                        i2c_rd16(sd, CHIPID_ADDR) & MASK_REVID);
+        v4l2_info(sd, "Reset: IR: %d, CEC: %d, CSI TX: %d, HDMI: %d\n",
+                        !!(sysctl & MASK_IRRST),
+                        !!(sysctl & MASK_CECRST),
+                        !!(sysctl & MASK_CTXRST),
+                        !!(sysctl & MASK_HDMIRST));
+        v4l2_info(sd, "Sleep mode: %s\n", sysctl & MASK_SLEEP ? "on" : "off");
+        v4l2_info(sd, "Cable detected (+5V power): %s\n",
+                        hdmi_sys_status & MASK_S_DDC5V ? "yes" : "no");
+        v4l2_info(sd, "DDC lines enabled: %s\n",
+                        (i2c_rd8(sd, EDID_MODE) & MASK_EDID_MODE_ALL) ?
+                        "yes" : "no");
+        v4l2_info(sd, "Hotplug enabled: %s\n",
+                        (i2c_rd8(sd, HPD_CTL) & MASK_HPD_OUT0) ?
+                        "yes" : "no");
+        v4l2_info(sd, "CEC enabled: %s\n",
+                        (i2c_rd16(sd, CECEN) & MASK_CECEN) ?  "yes" : "no");
+        v4l2_info(sd, "-----Signal status-----\n");
+        v4l2_info(sd, "TMDS signal detected: %s\n",
+                        hdmi_sys_status & MASK_S_TMDS ? "yes" : "no");
+        v4l2_info(sd, "Stable sync signal: %s\n",
+                        hdmi_sys_status & MASK_S_SYNC ? "yes" : "no");
+        v4l2_info(sd, "PHY PLL locked: %s\n",
+                        hdmi_sys_status & MASK_S_PHY_PLL ? "yes" : "no");
+        v4l2_info(sd, "PHY DE detected: %s\n",
+                        hdmi_sys_status & MASK_S_PHY_SCDT ? "yes" : "no");
+
+        if (tc358840_get_detected_timings(sd, &timings)) {
+                v4l2_info(sd, "No video detected\n");
+        } else {
+                v4l2_print_dv_timings(sd->name, "Detected format: ", &timings,
+                                true);
+        }
+        v4l2_print_dv_timings(sd->name, "Configured format: ", &state->timings,
+                        true);
+
+        v4l2_info(sd, "-----CSI-TX status-----\n");
+        v4l2_info(sd, "Lanes needed: %d\n",
+                        tc358840_num_csi_lanes_needed(sd));
+        v4l2_info(sd, "Lanes in use: %d\n",
+                        tc358840_num_csi_lanes_in_use(sd));
+        v4l2_info(sd, "Splitter %sabled\n",
+                  (i2c_rd16(sd, SPLITTX0_CTRL) & MASK_SPBP) ? "dis" : "en");
+        /*
+        v4l2_info(sd, "Waiting for particular sync signal: %s\n",
+                        (i2c_rd16(sd, CSI_STATUS) & MASK_S_WSYNC) ?
+                        "yes" : "no");
+        v4l2_info(sd, "Transmit mode: %s\n",
+                        (i2c_rd16(sd, CSI_STATUS) & MASK_S_TXACT) ?
+                        "yes" : "no");
+        v4l2_info(sd, "Receive mode: %s\n",
+                        (i2c_rd16(sd, CSI_STATUS) & MASK_S_RXACT) ?
+                        "yes" : "no");
+        v4l2_info(sd, "Stopped: %s\n",
+                        (i2c_rd16(sd, CSI_STATUS) & MASK_S_HLT) ?
+                        "yes" : "no");*/
+        v4l2_info(sd, "Color space: %s\n",
+                        state->mbus_fmt_code == MEDIA_BUS_FMT_UYVY8_1X16 ?
+                        "YCbCr 422 16-bit" :
+                        state->mbus_fmt_code == MEDIA_BUS_FMT_RGB888_1X24 ?
+                        "RGB 888 24-bit" : "Unsupported");
+
+        v4l2_info(sd, "-----%s status-----\n", is_hdmi(sd) ? "HDMI" : "DVI-D");
+        v4l2_info(sd, "HDCP encrypted content: %s\n",
+                        hdmi_sys_status & MASK_S_HDCP ? "yes" : "no");
+        v4l2_info(sd, "Input color space: %s %s range\n",
+                        input_color_space[(vi_status3 & MASK_S_V_COLOR) >> 1],
+                        (vi_status3 & MASK_LIMITED) ? "limited" : "full");
+        if (!is_hdmi(sd))
+                return 0;
+        v4l2_info(sd, "AV Mute: %s\n", hdmi_sys_status & MASK_S_AVMUTE ? "on" :
+                        "off");
+        v4l2_info(sd, "Deep color mode: %d-bits per channel\n",
+                        deep_color_mode[(i2c_rd8(sd, VI_STATUS1) &
+                                MASK_S_DEEPCOLOR) >> 2]);
+        print_avi_infoframe(sd);
+
+        return 0;
+}
+
+static int tc358840_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        v4l2_dbg(3, debug, sd, "%s():\n", __func__);
+
+        return enable_stream(sd, enable);
+}
+
+static int tc358840_enum_mbus_code(struct v4l2_subdev *sd,
+                                struct v4l2_subdev_pad_config *cfg,
+                                struct v4l2_subdev_mbus_code_enum *code)
+{
+        v4l2_dbg(2, debug, sd, "%s()\n", __func__);
+
+        if (code->index >= 2)
+                return -EINVAL;
+
+        switch (code->index) {
+        case 0:
+                code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+                break;
+        case 1:
+                code->code = MEDIA_BUS_FMT_RGB888_1X24;
+                break;
+        }
+        return 0;
+}
+
+static int tc358840_enum_framesizes(struct v4l2_subdev *sd,
+                                struct v4l2_subdev_pad_config *cfg,                             
+                                struct v4l2_subdev_frame_size_enum *fse)
+{
+        const struct camera_common_frmfmt *frmfmt = tc358840_frmfmt;
+        int num_frmfmt = ARRAY_SIZE(tc358840_frmfmt);
+
+        v4l2_dbg(2, debug, sd, "%s()\n", __func__);
+
+        if (fse->code != V4L2_PIX_FMT_UYVY &&
+            fse->code != V4L2_PIX_FMT_ABGR32)
+                return -EINVAL;
+
+        if (fse->index >= num_frmfmt)
+                return -EINVAL;
+
+        fse->min_width = fse->max_width = frmfmt[fse->index].size.width;
+        fse->min_height = fse->max_height = frmfmt[fse->index].size.height;
+
+        return 0;
+}
+
+static int tc358840_enum_frameintervals(struct v4l2_subdev *sd,
+                                struct v4l2_subdev_pad_config *cfg,
+                                struct v4l2_subdev_frame_interval_enum *fie)
+{
+        const struct camera_common_frmfmt *frmfmt = tc358840_frmfmt;
+        int num_frmfmt = ARRAY_SIZE(tc358840_frmfmt);
+        int i;
+
+        v4l2_dbg(2, debug, sd, "%s()\n", __func__);
+
+        if (fie->code != V4L2_PIX_FMT_UYVY &&
+            fie->code != V4L2_PIX_FMT_ABGR32)
+                return -EINVAL;
+
+        for (i = 0; i < num_frmfmt; i++) {
+                if (frmfmt[i].size.width == fie->width &&
+                    frmfmt[i].size.height == fie->height)
+                        break;
+        }
+        if (i >= num_frmfmt)
+                return -EINVAL;
+
+        if (fie->index >= frmfmt[i].num_framerates)
+                return -EINVAL;
+
+        fie->interval.numerator = 1;
+        fie->interval.denominator = frmfmt[i].framerates[fie->index];
+
+        return 0;
+}
+
+static int tc358840_s_power(struct v4l2_subdev *sd, int on)
+{
+        return 0;
+}
+
+static struct v4l2_subdev_video_ops tc358840_subdev_video_ops = {
+        .g_input_status = tc358840_g_input_status,
+        .s_dv_timings = tc358840_s_dv_timings,
+        .g_dv_timings = tc358840_g_dv_timings,
+        .query_dv_timings = tc358840_query_dv_timings,
+        .g_mbus_config = tc358840_g_mbus_config,
+        .s_stream = tc358840_s_stream,
+};
+
+static struct v4l2_subdev_core_ops tc358840_subdev_core_ops = {
+        .s_power = tc358840_s_power,
+        .log_status = tc358840_log_status,
+        .interrupt_service_routine = tc358840_isr,
+        .subscribe_event = tc358840_subscribe_event,
+        .unsubscribe_event = v4l2_event_subdev_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+        .g_register = tc358840_g_register,
+        .s_register = tc358840_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_pad_ops tc358840_pad_ops = {
+        .set_fmt = tc358840_set_fmt,
+        .get_fmt = tc358840_get_fmt,
+        .enum_mbus_code = tc358840_enum_mbus_code,
+        .get_edid = tc358840_g_edid,
+        .set_edid = tc358840_s_edid,
+        .dv_timings_cap = tc358840_dv_timings_cap,
+        .enum_dv_timings = tc358840_enum_dv_timings,
+        .enum_frame_size = tc358840_enum_framesizes,
+        .enum_frame_interval = tc358840_enum_frameintervals,
+};
+
+static struct v4l2_subdev_ops tc358840_ops = {
+        .core = &tc358840_subdev_core_ops,
+        .video = &tc358840_subdev_video_ops,
+        .pad = &tc358840_pad_ops,
+};
+
+
+/* --------------- CUSTOM CTRLS --------------- */
+
+static const struct v4l2_ctrl_config tc358840_ctrl_audio_sampling_rate = {
+        .id = TC358840_CID_AUDIO_SAMPLING_RATE,
+        .name = "Audio sampling rate",
+        .type = V4L2_CTRL_TYPE_INTEGER,
+        .min = 0,
+        .max = 768000,
+        .step = 1,
+        .def = 0,
+        .flags = V4L2_CTRL_FLAG_READ_ONLY,
+};
+
+static const struct v4l2_ctrl_config tc358840_ctrl_audio_present = {
+        .id = TC358840_CID_AUDIO_PRESENT,
+        .name = "Audio present",
+        .type = V4L2_CTRL_TYPE_BOOLEAN,
+        .min = 0,
+        .max = 1,
+        .step = 1,
+        .def = 0,
+        .flags = V4L2_CTRL_FLAG_READ_ONLY,
+};
+
+static const struct v4l2_ctrl_config tc358840_ctrl_splitter_width = {
+        .id = TC358840_CID_SPLITTER_WIDTH,
+        .name = "Splitter Width",
+        .type = V4L2_CTRL_TYPE_INTEGER,
+        .min = 320,
+        .max = 1920,
+        .step = 16,
+        .def = 1920,
+        .flags = V4L2_CTRL_FLAG_READ_ONLY,
+};
+
+/* --------------- PROBE / REMOVE --------------- */
+
+#ifdef CONFIG_OF
+
+static bool tc358840_parse_dt(struct tc358840_platform_data *pdata,
+                struct i2c_client *client)
+{
+        struct device_node *node = client->dev.of_node;
+        const u32 *property;
+
+        v4l_dbg(1, debug, client, "Device Tree Parameters:\n");
+
+        pdata->reset_gpio = of_get_named_gpio(node, "reset-gpios", 0);
+        if (pdata->reset_gpio == 0)
+                return false;
+        v4l_dbg(1, debug, client, "reset_gpio = %d\n", pdata->reset_gpio);
+
+//      if (v4l2_of_parse_endpoint(node, &pdata->endpoint))
+//              return false;
+
+        property = of_get_property(node, "refclk_hz", NULL);
+        if (property == NULL)
+                return false;
+        pdata->refclk_hz = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "refclk_hz = %d\n", be32_to_cpup(property));
+
+        property = of_get_property(node, "ddc5v_delay", NULL);
+        if (property == NULL)
+                return false;
+        pdata->ddc5v_delay = be32_to_cpup(property);
+        if (pdata->ddc5v_delay > DDC5V_DELAY_MAX)
+                pdata->ddc5v_delay = DDC5V_DELAY_MAX;
+        v4l_dbg(1, debug, client, "ddc5v_delay = %d ms\n",
+                50 * pdata->ddc5v_delay);
+
+        property = of_get_property(node, "enable_hdcp", NULL);
+        if (property == NULL)
+                return false;
+        pdata->enable_hdcp = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "enable_hdcp = %d\n", be32_to_cpup(property));
+
+        property = of_get_property(node, "csi_port", NULL);
+        if (property == NULL)
+                return false;
+        pdata->csi_port = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "csi_port = %d\n", be32_to_cpup(property));
+
+        property = of_get_property(node, "lineinitcnt", NULL);
+        if (property == NULL)
+                return false;
+        pdata->lineinitcnt = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "lineinitcnt = %d\n", be32_to_cpup(property));
+
+        property = of_get_property(node, "lptxtimecnt", NULL);
+        if (property == NULL)
+                return false;
+        pdata->lptxtimecnt = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "lptxtimecnt = %d\n", be32_to_cpup(property));
+
+        property = of_get_property(node, "tclk_headercnt", NULL);
+        if (property == NULL)
+                return false;
+        pdata->tclk_headercnt = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "tclk_headercnt = %d\n",
+                be32_to_cpup(property));
+
+        property = of_get_property(node, "tclk_trailcnt", NULL);
+        if (property == NULL)
+                return false;
+        pdata->tclk_trailcnt = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "tclk_trailcnt = %d\n",
+                be32_to_cpup(property));
+
+        property = of_get_property(node, "ths_headercnt", NULL);
+        if (property == NULL)
+                return false;
+        pdata->ths_headercnt = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "ths_headercnt = %d\n",
+                be32_to_cpup(property));
+
+        property = of_get_property(node, "twakeup", NULL);
+        if (property == NULL)
+                return false;
+        pdata->twakeup = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "twakeup = %d\n", be32_to_cpup(property));
+
+        property = of_get_property(node, "tclk_postcnt", NULL);
+        if (property == NULL)
+                return false;
+        pdata->tclk_postcnt = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "tclk_postcnt = %d\n",
+                be32_to_cpup(property));
+
+        property = of_get_property(node, "ths_trailcnt", NULL);
+        if (property == NULL)
+                return false;
+        pdata->ths_trailcnt = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "ths_trailcnt = %d\n",
+                be32_to_cpup(property));
+
+        property = of_get_property(node, "hstxvregcnt", NULL);
+        if (property == NULL)
+                return false;
+        pdata->hstxvregcnt = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "hstxvregcnt = %d\n", be32_to_cpup(property));
+
+        property = of_get_property(node, "pll_prd", NULL);
+        if (property == NULL)
+                return false;
+        pdata->pll_prd = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "pll_prd = %d\n", be32_to_cpup(property));
+
+        property = of_get_property(node, "pll_fbd", NULL);
+        if (property == NULL)
+                return false;
+        pdata->pll_fbd = be32_to_cpup(property);
+        v4l_dbg(1, debug, client, "pll_fbd = %d\n", be32_to_cpup(property));
+
+        return true;
+}
+#endif
+
+static int tc358840_pwr_init(struct tc358840_platform_data *pdata,
+                struct i2c_client *client)
+{
+        struct device_node *node = client->dev.of_node;
+        int cam2_rst;
+        int err;
+        struct regulator *dvdd;
+        struct regulator *iovdd;
+
+        err = camera_common_regulator_get(client, &iovdd, "vif");
+        if (err < 0) {
+                dev_err(&client->dev, "cannot get regulator vif %d\n", err);
+                return -EINVAL;
+        }
+
+        err = camera_common_regulator_get(client, &dvdd, "vdig");
+        if (err < 0) {
+                dev_err(&client->dev, "cannot get regulator vdig %d\n", err);
+                return -EINVAL;
+        }
+
+        /*  cam2 rst */
+        cam2_rst = of_get_named_gpio(node, "cam2_rst", 0);
+        if (cam2_rst == 0)
+                return false;
+        err = gpio_request(cam2_rst, "cam2-rst");
+        if (err < 0)
+                dev_err(&client->dev,
+                                "cam2 rst gpio request failed %d\n", err);
+
+        gpio_direction_output(cam2_rst, 1);
+
+        if (dvdd) {
+                err = regulator_enable(dvdd);
+                if (err < 0) {
+                        dev_err(&client->dev, "cannot enable regulator vif %d\n", err);
+                        return -EINVAL;
+                }
+        }
+        if (iovdd) {
+                err = regulator_enable(iovdd);
+                if (err < 0) {
+                        dev_err(&client->dev, "cannot enable regulator vdig %d\n", err);
+                        return -EINVAL;
+                }
+        }
+        gpio_direction_output(cam2_rst, 1);
+
+        return true;
+}
+
+static int tc358840_verify_chipid(struct v4l2_subdev *sd)
+{
+        u16 cid = 0;
+
+        cid = i2c_rd16(sd, CHIPID_ADDR);
+        if (cid != TC358840_CHIPID) {
+                v4l2_err(sd, "Invalid chip ID 0x%04X\n", cid);
+                return -ENODEV;
+        }
+
+        v4l2_dbg(1, debug, sd, "TC358840 ChipID 0x%02x, Revision 0x%02x\n",
+                (cid & MASK_CHIPID) >> 8, cid & MASK_REVID);
+
+        return 0;
+}
+
+static int tc358840_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        dev_dbg(&client->dev, "%s:\n", __func__);
+        return 0;
+}
+
+static const struct v4l2_subdev_internal_ops tc358840_subdev_internal_ops = {
+        .open = tc358840_open,
+};
+
+static const struct media_entity_operations tc358840_media_ops = {
+#ifdef CONFIG_MEDIA_CONTROLLER
+        .link_validate = v4l2_subdev_link_validate,
+#endif
+};
+
+static int tc358840_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+        struct tc358840_state *state;
+        struct v4l2_subdev *sd;
+        int err;
+
+        state = devm_kzalloc(&client->dev, sizeof(struct tc358840_state), GFP_KERNEL);
+        if (!state)
+                return -ENOMEM;
+
+        if (client->dev.of_node) {
+                if (!tc358840_parse_dt(&state->pdata, client)) {
+                        v4l_err(client, "Couldn't parse device tree\n");
+                        return -ENODEV;
+                }
+                if (!tc358840_pwr_init(&state->pdata, client)) {
+                        v4l_err(client, "Couldn't power init\n");
+                        return -ENODEV;
+                }
+        } else {
+                if (!client->dev.platform_data) {
+                        v4l_err(client, "No platform data!\n");
+                        return -ENODEV;
+                }
+                state->pdata = *(struct tc358840_platform_data *)client->dev.platform_data;
+        }
+
+        state->i2c_client = client;
+        sd = &state->sd;
+
+        i2c_set_clientdata(client, state);
+
+        v4l2_i2c_subdev_init(sd, client, &tc358840_ops);
+
+        /* Release System Reset (pin K8) */
+        v4l2_info(sd, "Releasing System Reset (gpio 0x%04X)\n",
+                state->pdata.reset_gpio);
+        if (!gpio_is_valid(state->pdata.reset_gpio)) {
+                v4l_err(client, "Reset GPIO is invalid!\n");
+                return state->pdata.reset_gpio;
+        }
+        err = devm_gpio_request_one(&client->dev, state->pdata.reset_gpio,
+                                        GPIOF_OUT_INIT_HIGH, "tc358840-reset");
+        if (err) {
+                dev_err(&client->dev,
+                        "Failed to request Reset GPIO 0x%04X: %d\n",
+                        state->pdata.reset_gpio, err);
+                return err;
+        }
+
+        /*  */
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+                return -EIO;
+        v4l_info(client, "Chip found @ 7h%02X (%s)\n",
+                client->addr, client->adapter->name);
+
+
+        /* Verify chip ID */
+        err = tc358840_verify_chipid(sd);
+        if (err)
+                return err;
+
+        /* Control Handlers */
+        v4l2_ctrl_handler_init(&state->hdl, 4);
+
+        /* Custom controls */
+        state->detect_tx_5v_ctrl = v4l2_ctrl_new_std(&state->hdl, NULL,
+                        V4L2_CID_DV_RX_POWER_PRESENT, 0, 1, 0, 0);
+
+        state->audio_sampling_rate_ctrl = v4l2_ctrl_new_custom(&state->hdl,
+                        &tc358840_ctrl_audio_sampling_rate, NULL);
+
+        state->audio_present_ctrl = v4l2_ctrl_new_custom(&state->hdl,
+                        &tc358840_ctrl_audio_present, NULL);
+
+        state->splitter_width_ctrl = v4l2_ctrl_new_custom(&state->hdl,
+                        &tc358840_ctrl_splitter_width, NULL);
+
+        if (state->hdl.error) {
+                err = state->hdl.error;
+                goto err_hdl;
+        }
+
+        sd->ctrl_handler = &state->hdl;
+
+        if (tc358840_update_controls(sd)) {
+                err = -ENODEV;
+                goto err_hdl;
+        }
+
+        /* Work Queues */
+        state->work_queues = create_singlethread_workqueue(client->name);
+        if (!state->work_queues) {
+                v4l2_err(sd, "Could not create work queue!\n");
+                err = -ENOMEM;
+                goto err_hdl;
+        }
+        INIT_DELAYED_WORK(&state->delayed_work_enable_hotplug,
+                        tc358840_delayed_work_enable_hotplug);
+        INIT_DELAYED_WORK(&state->delayed_work_enable_interrupt,
+                        tc358840_delayed_work_enable_interrupt);
+        INIT_WORK(&state->process_isr, tc358840_process_isr);
+        mutex_init(&state->isr_lock);
+
+        /* Initial Setup */
+        state->mbus_fmt_code = MEDIA_BUS_FMT_UYVY8_1X16;
+        tc358840_initial_setup(sd);
+
+        tc358840_set_csi_mbus_config(sd);
+
+        /* Get interrupt */
+        if (client->irq) {
+                err = devm_request_threaded_irq(&state->i2c_client->dev,
+                                                client->irq, NULL, tc358840_irq_handler,
+                                                IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+                                                sd->name, (void *)sd);
+                if (err) {
+                        v4l2_err(sd, "Could not request interrupt %d!\n",
+                                 client->irq);
+                        goto err_hdl;
+                }
+        }
+
+        queue_delayed_work(state->work_queues,
+                &state->delayed_work_enable_interrupt,
+                msecs_to_jiffies(DELAY_ENABLE_INTERRUPT_MS));
+
+        /*
+         * FIXME: Don't know what MASK_CSITX0_INT and MASK_CSITX1_INT do.
+         * Thus, disable them for now...
+         */
+#if 0
+        i2c_wr16(sd, INTMASK, ~(MASK_HDMI_INT | MASK_CSITX0_INT |
+                MASK_CSITX1_INT) & 0xFFFF);
+#endif
+        i2c_wr16(sd, INTMASK, ~(MASK_HDMI_INT) & 0xFFFF);
+
+        v4l2_ctrl_handler_setup(sd->ctrl_handler);
+
+        v4l2_info(sd, "%s found @ 7h%02X (%s)\n", client->name,
+                  client->addr, client->adapter->name);
+
+        sd->dev = &client->dev;
+        sd->internal_ops = &tc358840_subdev_internal_ops;
+        sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        state->pad[0].flags = MEDIA_PAD_FL_SOURCE;
+        state->pad[1].flags = MEDIA_PAD_FL_SOURCE;
+        sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+        sd->entity.ops = &tc358840_media_ops;
+        err = media_entity_init(&sd->entity, 2, state->pad, 0);
+        if (err < 0) {
+                dev_err(&client->dev, "unable to init media entity\n");
+                return err;
+        }
+#endif
+
+        err = v4l2_async_register_subdev(sd);
+        if (err == 0)
+                return 0;
+
+err_hdl:
+        v4l2_ctrl_handler_free(&state->hdl);
+        return err;
+}
+
+static int tc358840_remove(struct i2c_client *client)
+{
+        struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+        v4l_dbg(1, debug, client, "%s()\n", __func__);
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+        media_entity_cleanup(&sd->entity);
+#endif
+        return 0;
+}
+
+static const struct i2c_device_id tc358840_id[] = {
+        { "tc358840", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, tc358840_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id tc358840_of_table[] = {
+        { .compatible = "toshiba,tc358840" },
+        { }
+};
+MODULE_DEVICE_TABLE(of, tc358840_of_table);
+#endif
+
+static struct i2c_driver tc358840_driver = {
+        .driver = {
+                .of_match_table = of_match_ptr(tc358840_of_table),
+                .name = "tc358840",
+                .owner = THIS_MODULE,
+        },
+        .probe = tc358840_probe,
+        .remove = tc358840_remove,
+        .id_table = tc358840_id,
+};
+module_i2c_driver(tc358840_driver);
+
+MODULE_DESCRIPTION("Driver for Toshiba TC358840 HDMI to CSI-2 Bridge");
+MODULE_AUTHOR("Armin Weiss (weii@zhaw.ch)");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/tc358840_regs.h b/drivers/media/i2c/tc358840_regs.h
new file mode 100644
index 000000000..1cc41628b
--- /dev/null
+++ b/drivers/media/i2c/tc358840_regs.h
@@ -0,0 +1,730 @@
+/*
+ * tc358840_regs.h - Toshiba UH2C/D HDMI-CSI bridge registers
+ *
+ * Copyright (c) 2015, Armin Weiss <weii@zhaw.ch>
+ * Copyright (c) 2016, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __TC358840_TABLES__
+#define __TC358840_TABLES__
+
+#include <media/camera_common.h>
+
+/**************************************************
+ * Register Addresses
+ *************************************************/
+
+/* *** General (16 bit) *** */
+#define CHIPID_ADDR                             0x0000
+#define MASK_CHIPID                             0xFF00
+#define MASK_REVID                              0x00FF
+#define TC358840_CHIPID                         0x4700
+
+#define SYSCTL                                  0x0002
+#define MASK_SLEEP                              (1 << 0)
+#define MASK_I2SDIS                             (1 << 7)
+#define MASK_HDMIRST                            (1 << 8)
+#define MASK_CTXRST                             (1 << 9)
+#define MASK_CECRST                             (1 << 10)
+#define MASK_IRRST                              (1 << 11)
+#define MASK_SPLRST                             (1 << 12)
+#define MASK_ABRST                              (1 << 14)
+#define MASK_RESET_ALL                          0x5F80
+
+#define CONFCTL0                                0x0004
+#define MASK_VTX0EN                             (1 << 0)
+#define MASK_VTX1EN                             (1 << 1)
+#define MASK_AUTOINDEX                          (1 << 2)
+#define MASK_AUDOUTSEL_CSITX0                   (0 << 3)
+#define MASK_AUDOUTSEL_CSITX1                   (1 << 3)
+#define MASK_AUDOUTSEL_I2S                      (2 << 3)
+#define MASK_AUDOUTSEL_TDM                      (3 << 3)
+#define MASK_ABUFEN                             (1 << 5)
+#define MASK_YCBCRFMT                           (3 << 6)
+#define MASK_YCBCRFMT_YCBCR444                  (0 << 6)
+#define MASK_YCBCRFMT_YCBCR422_12               (1 << 6)
+#define MASK_YCBCRFMT_VPID2                     (2 << 6)
+#define MASK_YCBCRFMT_YCBCR422_8                (3 << 6)
+#define MASK_I2SDLYOPT                          (1 << 8)
+#define MASK_AUDCHSEL                           (1 << 9)
+#define MASK_AUDCHNUM_8                         (0 << 10)
+#define MASK_AUDCHNUM_6                         (1 << 10)
+#define MASK_AUDCHNUM_4                         (2 << 10)
+#define MASK_AUDCHNUM_2                         (3 << 10)
+#define MASK_ACLKOPT                            (1 << 12)
+#define MASK_IECEN                              (1 << 13)
+#define MASK_SLMBEN                             (1 << 14)
+#define MASK_TX_MSEL                            (1 << 15)
+
+#define CONFCTL1                                0x0006
+#define MASK_TX_OUT_FMT                         0x0003
+#define MASK_TX_OUT_FMT_RGB888                  (0 << 0)
+#define MASK_TX_OUT_FMT_RGB666                  (2 << 0)
+#define MASK_TX_MS_EN                           (1 << 2)
+
+/* *** Interrupt (16 bit) *** */
+#define INTSTATUS                               0x0014
+#define INTMASK                                 0x0016
+#define MASK_IR_DINT                            (1 << 0)
+#define MASK_IR_EINT                            (1 << 1)
+#define MASK_CEC_RINT                           (1 << 2)
+#define MASK_CEC_TINT                           (1 << 3)
+#define MASK_CEC_EINT                           (1 << 4)
+#define MASK_SYS_INT                            (1 << 5)
+#define MASK_CSITX0_INT                         (1 << 8)
+#define MASK_HDMI_INT                           (1 << 9)
+#define MASK_AMUTE_INT                          (1 << 10)
+#define MASK_CSITX1_INT                         (1 << 11)
+#define MASK_INT_STATUS_MASK_ALL                0x0F3F
+
+/* *** Interrupt and MASKs (8 bit) *** */
+#define HDMI_INT0                               0x8500
+#define MASK_KEY                                0x01
+#define MASK_MISC                               0x0002
+
+#define HDMI_INT1                               0x8501
+#define MASK_SYS                                0x01
+#define MASK_CLK                                0x02
+#define MASK_PACKET                             0x04
+#define MASK_ACBIT                              0x08
+#define MASK_AUD                                0x10
+#define MASK_ERR                                0x20
+#define MASK_HDCP                               0x40
+#define MASK_GBD                                0x80
+
+#define SYS_INT                                 0x8502
+#define SYS_INTM                                0x8512
+#define MASK_DDC                                0x01
+#define MASK_TMDS                               0x02
+#define MASK_DPMBDET                            0x04
+#define MASK_NOPMBDET                           0x08
+#define MASK_HDMI                               0x10
+#define MASK_DVI                                0x20
+#define MASK_ACRN                               0x40
+#define MASK_ACR_CTS                            0x80
+
+#define CLK_INT                                 0x8503
+#define CLK_INTM                                0x8513
+#define MASK_TMDSCLK_CHG                        0x01
+#define MASK_PHYCLK_CHG                         0x02
+#define MASK_PXCLK_CHG                          0x04
+#define MASK_DC_CHG                             0x08
+#define MASK_IN_HV_CHG                          0x10
+#define MASK_IN_DE_CHG                          0x20
+#define MASK_OUT_H_CHG                          0x40
+#define MASK_OUT_DE_CHG                         0x80
+
+#define PACKET_INT                              0x8504
+#define PACKET_INTM                             0x8514
+#define MASK_PK_AVI                             0x01
+#define MASK_PK_AUD                             0x02
+#define MASK_PK_MS                              0x04
+#define MASK_PK_SPD                             0x08
+#define MASK_PK_VS                              0x10
+#define MASK_PK_ACP                             0x20
+#define MASK_PK_ISRC                            0x40
+#define MASK_PK_ISRC2                           0x80
+
+#define CBIT_INT                                0x8505
+#define CBIT_INTM                               0x8515
+#define MASK_CBIT                               0x01
+#define MASK_CBIT_FS                            0x02
+#define MASK_CBIT_NLPCM                         0x04
+#define MASK_AU_HBR                             0x08
+#define MASK_AU_DSD                             0x10
+#define MASK_AF_UNLOCK                          0x40
+#define MASK_AF_LOCK                            0x80
+
+#define AUDIO_INT                               0x8506
+#define AUDIO_INTM                              0x8516
+#define MASK_BUFINIT_END                        0x01
+#define MASK_BUF_UNDER                          0x02
+#define MASK_BUF_NU2                            0x04
+#define MASK_BUF_NU1                            0x08
+#define MASK_BUF_CENTER                         0x10
+#define MASK_BUF_NO1                            0x20
+#define MASK_BUF_NO2                            0x40
+#define MASK_BUF_OVER                           0x80
+
+#define ERR_INT                                 0x8507
+#define ERR_INTM                                0x8517
+#define MASK_DC_PPERR                           0x01
+#define MASK_DC_BUFERR                          0x02
+#define MASK_DC_DEERR                           0x04
+#define MASK_DC_NOCD                            0x08
+#define MASK_NO_AVI                             0x10
+#define MASK_NO_ACP                             0x20
+#define MASK_AU_FRAME                           0x40
+#define MASK_EESS_ERR                           0x80
+
+#define HDCP_INT                                0x8508
+#define HDCP_INTM                               0x8518
+#define MASK_AN_END                             0x01
+#define MASK_AKSV_END                           0x02
+#define MASK_KM_END                             0x04
+#define MASK_R0_END                             0x08
+#define MASK_SHA_END                            0x10
+#define MASK_LINKERR                            0x20
+#define MASK_AVM_CLR                            0x40
+#define MASK_AVM_SET                            0x80
+
+#define GBD_INT                                 0x8509
+#define GBD_INTM                                0x8519
+#define MASK_GBD_ON                             0x01
+#define MASK_GBD_OFF                            0x02
+#define MASK_P1GBD_DET                          0x04
+#define MASK_P0GBD_CHG                          0x10
+#define MASK_P1GBD_CHG                          0x20
+#define MASK_GBD_ACLR                           0x40
+#define MASK_GBD_PKERR                          0x80
+
+#define MISC_INT                                0x850B
+#define MISC_INTM                               0x851B
+#define MASK_AUDIO_MUTE                         0x01
+#define MASK_SYNC_CHG                           0x02
+#define MASK_NO_VS                              0x04
+#define MASK_NO_SPD                             0x08
+#define MASK_AS_LAYOUT                          0x10
+#define MASK_VIDEO_COLOR                        0x20
+#define MASK_AU_HBR_OFF                         0x40
+#define MASK_AU_DSD_OFF                         0x80
+
+/* *** STATUS *** */
+#define SYS_STATUS                              0x8520
+#define MASK_S_SYNC                             0x80
+#define MASK_S_AVMUTE                           0x40
+#define MASK_S_HDCP                             0x20
+#define MASK_S_HDMI                             0x10
+#define MASK_S_PHY_SCDT                         0x08
+#define MASK_S_PHY_PLL                          0x04
+#define MASK_S_TMDS                             0x02
+#define MASK_S_DDC5V                            0x01
+
+#define VI_STATUS1                              0x8522
+#define MASK_S_V_GBD                            0x08
+#define MASK_S_DEEPCOLOR                        0x0c
+#define MASK_S_V_422                            0x02
+#define MASK_S_V_INTERLACE                      0x01
+
+#define VI_STATUS3                              0x8528
+#define MASK_S_V_COLOR                          0x1F
+#define MASK_RGB_FULL                           0x00
+#define MASK_RGB_LIMITED                        0x01
+#define MASK_YCBCR601_FULL                      0x02
+#define MASK_YCBCR601_LIMITED                   0x03
+#define MASK_ADOBE_RGB_FULL                     0x04
+#define MASK_ADOBE_RGB_LIMITED                  0x05
+#define MASK_YCBCR709_FULL                      0x06
+#define MASK_YCBCR709_LIMITED                   0x07
+#define MASK_XVYCC601_FULL                      0x0A
+#define MASK_XVYCC601_LIMITED                   0x0B
+#define MASK_XVYCC709_FULL                      0x0E
+#define MASK_XVYCC709_LIMITED                   0x0F
+#define MASK_SYCC601_FULL                       0x12
+#define MASK_SYCC601_LIMITED                    0x13
+#define MASK_ADOBE_YCC601_FULL                  0x1A
+#define MASK_ADOBE_YCC601_LIMITED               0x1B
+#define MASK_LIMITED                            0x01
+
+
+/* *** CSI TX (32 bit) *** */
+#define CSITX0_BASE_ADDR                        0x0000
+#define CSITX1_BASE_ADDR                        0x0200
+
+#define CSITX_CLKEN                             0x0108
+#define MASK_CSITX_EN                           (1 << 0)
+
+#define PPICLKEN                                0x010C
+#define MASK_HSTXCLKEN                          0x00000001
+
+#define MODECONF                                0x0110  /* Not in Ref. v1.5 */
+#define MASK_CSI2MODE                           (1 << 0)
+#define MASK_VSYNC_POL_SW                       (1 << 1)
+#define MASK_HSYNC_POL_SW                       (1 << 2)
+#define MASK_DTVALID_POL_SW                     (1 << 3)
+#define MASK_INDMODE                            (1 << 4)
+
+#define LANEEN                                  0x0118
+#define MASK_LANES                              0x00000007
+#define MASK_LANE_0_EN                          (1 << 0)
+#define MASK_LANE_0_1_EN                        (2 << 0)
+#define MASK_LANE_0_1_2_EN                      (3 << 0)
+#define MASK_LANE_0_1_2_3_EN                    (4 << 0)
+#define MASK_LANES                              0x00000007
+#define MASK_CLANEEN                            (1 << 4)
+
+#define CSITX_START                             0x011C
+#define LINEINITCNT                             0x0120
+#define HSTOCNT                                 0x0124
+
+#define INTEN                                   0x0128  /* Not in Ref. v1.5 */
+#define MASK_VH_DLY_EN                          (1 << 0)
+#define MASK_VFHSYNCMASK_EN                     (1 << 7)
+#define MASK_IND_MODE_SEL_PORT                  (0 << 8)
+#define MASK_IND_MODE_SEL_REG                   (1 << 8)
+#define MASK_IND_TO_EN                          (1 << 9)
+#define MASK_HSTX_TO_EN                         (1 << 10)
+#define MASK_LRX_H_TO_EN                        (1 << 11)
+#define MASK_TA_TO_EN                           (1 << 12)
+#define MASK_PR_TO_EN                           (1 << 13)
+#define MASK_PRESP_TO_EN                        (1 << 14)
+#define MASK_DSI_RX_STATE_INT_EN                (1 << 16)
+#define MASK_DSI_RX_TRIG_INT_EN                 (1 << 17)
+#define MASK_DSI_LP_TX_INT_EN                   (1 << 18)
+#define MASK_DSI_RX_ERR_INT_EN                  (1 << 19)
+#define MASK_DSI_RP_TO_INT_EN                   (1 << 20)
+#define MASK_APP_SIDE_ERR_INT_EN                (1 << 21)
+#define MASK_INIT_INT_EN                        (1 << 22)
+#define MASK_DEBUG_MODE_EN                      (1 << 31)
+
+#define FUNCMODE                                0x0150
+#define MASK_CONTCLKMODE                        (1 << 5)
+#define MASK_FORCESTOP                          (1 << 10)
+
+#define CSITX_INTERNAL_STAT                     0x01B0
+
+#define LPTXTIMECNT                             0x0254
+#define TCLK_HEADERCNT                          0x0258
+#define TCLK_TRAILCNT                           0x025C
+#define THS_HEADERCNT                           0x0260
+#define TWAKEUP                                 0x0264
+#define TCLK_POSTCNT                            0x0268
+#define THS_TRAILCNT                            0x026C
+#define HSTXVREGCNT                             0x0270
+
+#define HSTXVREGEN                              0x0274
+#define MASK_D3M_HSTXVREGEN                     0x0010
+#define MASK_D2M_HSTXVREGEN                     0x0008
+#define MASK_D1M_HSTXVREGEN                     0x0004
+#define MASK_D0M_HSTXVREGEN                     0x0002
+#define MASK_CLM_HSTXVREGEN                     0x0001
+
+#define MIPICLKEN                               0x02A0
+#define MASK_MP_ENABLE                          0x00000001
+#define MASK_MP_CKEN                            0x00000002
+
+#define PLLCONF                                 0x02AC
+#define MASK_LFBREN                             (1 << 9)
+#define MASK_MPLBW                              0x00030000
+#define MASK_MPLBW_25                           (0 << 16)
+#define MASK_MPLBW_33                           (1 << 16)
+#define MASK_MPLBW_50                           (2 << 16)
+#define MASK_MPLBW_MAX                          (3 << 16)
+#define MASK_PLL_FBD                            0x000000FF
+#define SET_PLL_FBD(fbd)                        ((fbd) & MASK_PLL_FBD)
+#define MASK_PLL_FRS                            0x00000C00
+#define SET_PLL_FRS(frs)                        (((frs) << 10) & MASK_PLL_FRS)
+#define MASK_PLL_PRD                            0x0000F000
+#define SET_PLL_PRD(prd)                        (((prd) << 12) & \
+                                                  MASK_PLL_PRD)
+#define MASK_PLL_LBW                            0x00030000
+#define SET_PLL_LBW(lbw)                        ((((lbw) - 1) << 16) & \
+                                                  MASK_PLL_LBW)
+
+#define CECEN                                 0x0600
+#define MASK_CECEN                            0x0001
+
+/* *** Split Control (16 bit) *** */
+#define SPLITTX0_CTRL                           0x5000
+#define SPLITTX1_CTRL                           0x5080
+#define MASK_LCD_CSEL                           0x0001
+#define MASK_IFEN                               0x0002
+#define MASK_SPBP                               0x0100
+
+#define SPLITTX0_WC                             0x5008  /*Removed in rev. 1.1*/
+#define SPLITTX1_WC                             0x5088  /*Removed in rev. 1.1*/
+
+#define SPLITTX0_SPLIT                          0x500C
+#define SPLITTX1_SPLIT                          0x508C
+#define MASK_FPXV                               0x0FFF
+/* NOTE: Only available for TX0 */
+#define MASK_TX1SEL                             0x4000
+/* NOTE: Only available for TX0 */
+#define MASK_EHW                                0x8000
+
+/* *** HDMI PHY (8 bit) *** */
+#define PHY_CTL                                 0x8410
+/* TODO: Check name of mask */
+#define MASK_POWERCTL                           (1 << 0)
+/* TODO: Check name of mask */
+#define MASK_48_MHZ                             (1 << 1)
+
+#define PHY_CTL2                                0x8412
+#define MASK_PHY_FREE_RUN                       (1 << 5)
+
+#define PHY_ENB                                 0x8413
+#define MASK_ENABLE_PHY                         0x01
+
+#define PHY_RST                                 0x8414
+#define MASK_RESET_CTRL                         0x01    /* Reset active low */
+
+#define APPL_CTL                                0x84F0
+#define MASK_APLL_ON                            0x01
+#define MASK_APLL_CPCTL                         0x30
+#define MASK_APLL_CPCTL_HIZ                     0x00
+#define MASK_APLL_CPCTL_LFIX                    0x10
+#define MASK_APLL_CPCTL_HFIX                    0x20
+#define MASK_APLL_CPCTL_NORMAL                  0x30
+
+#define DDCIO_CTL                               0x84F4
+#define MASK_DDC_PWR_ON                         (1 << 0)
+
+/** *** HDMI Clock (8 bit) *** */
+#define AU_STATUS0                              0x8523
+#define MASK_S_A_SAMPLE                         0x01
+
+#define SYS_FREQ0                               0x8540
+#define SYS_FREQ1                               0x8541
+#define LOCK_REF_FREQA                          0x8630
+#define LOCK_REF_FREQB                          0x8631
+#define LOCK_REF_FREQC                          0x8632
+
+#define FS_SET                                  0x8621
+#define MASK_FS                                 0x0F
+
+#define NCO_F0_MOD                              0x8670
+#define MASK_NCO_F0_MOD_42MHZ                   0x00
+#define MASK_NCO_F0_MOD_REG                     0x02
+
+#define NCO_48F0A                               0x8671
+#define NCO_48F0B                               0x8672
+#define NCO_48F0C                               0x8673
+#define NCO_48F0D                               0x8674
+
+#define NCO_44F0A                               0x8675
+#define NCO_44F0B                               0x8676
+#define NCO_44F0C                               0x8677
+#define NCO_44F0D                               0x8678
+
+#define SCLK_CSC0                               0x8A0C
+#define SCLK_CSC1                               0x8A0D
+
+#define HDCP_MODE                             0x8560
+#define MASK_MODE_RST_TN                      0x20
+#define MASK_LINE_REKEY                       0x10
+#define MASK_AUTO_CLR                         0x04
+
+#define HDCP_REG1                             0x8563 /* Not in REF_01 */
+#define MASK_AUTH_UNAUTH_SEL                  0x70
+#define MASK_AUTH_UNAUTH_SEL_12_FRAMES        0x70
+#define MASK_AUTH_UNAUTH_SEL_8_FRAMES         0x60
+#define MASK_AUTH_UNAUTH_SEL_4_FRAMES         0x50
+#define MASK_AUTH_UNAUTH_SEL_2_FRAMES         0x40
+#define MASK_AUTH_UNAUTH_SEL_64_FRAMES        0x30
+#define MASK_AUTH_UNAUTH_SEL_32_FRAMES        0x20
+#define MASK_AUTH_UNAUTH_SEL_16_FRAMES        0x10
+#define MASK_AUTH_UNAUTH_SEL_ONCE             0x00
+#define MASK_AUTH_UNAUTH                      0x01
+#define MASK_AUTH_UNAUTH_AUTO                 0x01
+
+#define HDCP_REG2                             0x8564 /* Not in REF_01 */
+#define MASK_AUTO_P3_RESET                    0x0F
+#define SET_AUTO_P3_RESET_FRAMES(n)          (n & MASK_AUTO_P3_RESET)
+#define MASK_AUTO_P3_RESET_OFF                0x00
+
+
+/* *** VI *** */
+#define VI_MODE                                 0x8570
+/* TODO: Probably wrong bit (see p. 292 rev. 0.93) */
+#define MASK_RGB_DVI                            0x08
+
+#define DE_HSIZE_LO                             0x8582
+#define DE_HSIZE_HI                             0x8583
+#define DE_VSIZE_LO                             0x858C
+#define DE_VSIZE_HI                             0x858D
+
+#define IN_HSIZE_LO                             0x858E
+#define IN_HSIZE_HI                             0x858F
+
+#define IN_VSIZE_LO                             0x8590
+#define IN_VSIZE_HI                             0x8591
+
+#define FV_CNT_LO                               0x85C1  /* Not in Ref. v1.5 */
+#define FV_CNT_HI                               0x85C2  /* Not in Ref. v1.5 */
+
+#define HDCP_REG3                             0x85D1 /* Not in REF_01 */
+#define KEY_RD_CMD                            0x01
+
+/* *** EDID (8 bit) *** */
+#define EDID_MODE                               0x85E0
+#define MASK_DIRECT                             0x00
+#define MASK_RAM_DDC2B                          (1 << 0)
+#define MASK_RAM_EDDC                           (1 << 1)
+#define MASK_EDID_MODE_ALL                      0x03
+
+#define EDID_LEN1                               0x85E3
+#define EDID_LEN2                               0x85E4
+
+#define EDID_RAM                                0x8C00
+
+/* *** HDCP *** */
+#define BKSV                                    0x8800
+
+#define BCAPS                                 0x8840
+#define MASK_HDMI_RSVD                        0x80
+#define MASK_REPEATER                         0x40
+#define MASK_READY                            0x20
+#define MASK_FASTI2C                          0x10
+#define MASK_1_1_FEA                          0x02
+#define MASK_FAST_REAU                        0x01
+
+#define BSTATUS0                              0x8841
+#define BSTATUS1                              0x8842
+#define MASK_HDMI_MODE                        0x10
+#define MASK_MAX_EXCED                        0x08
+
+/* *** Video Output Format (8 bit) *** */
+#define VOUT_FMT                                0x8A00
+#define MASK_OUTFMT_444_RGB                     (0 << 0)
+#define MASK_OUTFMT_422                         (1 << 0)
+#define MASK_OUTFMT_THROUGH                     (2 << 0)
+#define MASK_422FMT_NORMAL                      (0 << 4)
+#define MASK_422FMT_HDMITHROUGH                 (1 << 4)
+
+#define VOUT_FIL                                0x8A01
+#define MASK_422FIL                             0x07
+#define MASK_422FIL_2_TAP                       (0 << 0)
+#define MASK_422FIL_3_TAP                       (1 << 0)
+#define MASK_422FIL_NO_FILTER                   (2 << 0)
+#define MASK_422FIL_2_TAP_444                   (3 << 0)
+#define MASK_422FIL_3_TAP_444                   (4 << 0)
+#define MASK_422FIL_2_TAP_444_CSC               (5 << 0)
+#define MASK_422FIL_3_TAP_444_CSC               (6 << 0)
+#define MASK_444FIL                             0x10
+#define MASK_444FIL_REPEAT                      (0 << 4)
+#define MASK_444FIL_2_TAP                       (1 << 4)
+
+#define VOUT_SYNC0                              0x8A02
+#define MASK_MODE_2                             (2 << 0)
+#define MASK_MODE_3                             (3 << 0)
+#define MASK_M3_HSIZE                           0x30
+#define MASK_M3_VSIZE                           0xC0
+
+#define VOUT_CSC                                0x8A08
+#define MASK_CSC_MODE                           0x03
+#define MASK_CSC_MODE_OFF                       (0 << 0)
+#define MASK_CSC_MODE_BUILTIN                   (1 << 0)
+#define MASK_CSC_MODE_AUTO                      (2 << 0)
+#define MASK_CSC_MODE_HOST                      (3 << 0)
+#define MASK_COLOR                              0x70
+#define MASK_COLOR_RGB_FULL                     (0 << 4)
+#define MASK_COLOR_RGB_LIMITED                  (1 << 4)
+#define MASK_COLOR_601_YCBCR_FULL               (2 << 4)
+#define MASK_COLOR_601_YCBCR_LIMITED            (3 << 4)
+#define MASK_COLOR_709_YCBCR_FULL               (4 << 4)
+#define MASK_COLOR_709_YCBCR_LIMITED            (5 << 4)
+#define MASK_COLOR_FULL_TO_LIMITED              (6 << 4)
+#define MASK_COLOR_LIMITED_TO_FULL              (7 << 4)
+
+
+/* *** HDMI Audio RefClk (8 bit) *** */
+#define FORCE_MUTE                              0x8600
+#define MASK_FORCE_DMUTE                        (1 << 0)
+#define MASK_FORCE_AMUTE                        (1 << 4)
+
+#define AUTO_CMD0                               0x8602
+#define MASK_AUTO_MUTE7                         0x80
+#define MASK_AUTO_MUTE6                         0x40
+#define MASK_AUTO_MUTE5                         0x20
+#define MASK_AUTO_MUTE4                         0x10
+#define MASK_AUTO_MUTE3                         0x08
+#define MASK_AUTO_MUTE2                         0x04
+#define MASK_AUTO_MUTE1                         0x02
+#define MASK_AUTO_MUTE0                         0x01
+
+#define AUTO_CMD1                               0x8603
+#define MASK_AUTO_MUTE10                        0x04
+#define MASK_AUTO_MUTE9                         0x02
+#define MASK_AUTO_MUTE8                         0x01
+
+#define AUTO_CMD2                               0x8604
+#define MASK_AUTO_PLAY3                         0x08
+#define MASK_AUTO_PLAY2                         0x04
+
+#define BUFINIT_START                           0x8606
+#define SET_BUFINIT_START_MS(milliseconds)      ((milliseconds) / 100)
+
+#define FS_MUTE                                 0x8607
+#define MASK_FS_ELSE_MUTE                       0x80
+#define MASK_FS22_MUTE                          0x40
+#define MASK_FS24_MUTE                          0x20
+#define MASK_FS88_MUTE                          0x10
+#define MASK_FS96_MUTE                          0x08
+#define MASK_FS176_MUTE                         0x04
+#define MASK_FS192_MUTE                         0x02
+#define MASK_FS_NO_MUTE                         0x01
+
+#define FS_IMODE                                0x8620
+#define MASK_NLPCM_HMODE                        0x40
+#define MASK_NLPCM_SMODE                        0x20
+#define MASK_NLPCM_IMODE                        0x10
+#define MASK_FS_HMODE                           0x08
+#define MASK_FS_AMODE                           0x04
+#define MASK_FS_SMODE                           0x02
+#define MASK_FS_IMODE                           0x01
+
+#define ACR_MODE                                0x8640
+#define MASK_ACR_LOAD                           0x10
+#define MASK_N_MODE                             0x04
+#define MASK_CTS_MODE                           0x01
+
+#define ACR_MDF0                                0x8641
+#define MASK_ACR_L2MDF                          0x70
+#define MASK_ACR_L2MDF_0_PPM                    0x00
+#define MASK_ACR_L2MDF_61_PPM                   0x10
+#define MASK_ACR_L2MDF_122_PPM                  0x20
+#define MASK_ACR_L2MDF_244_PPM                  0x30
+#define MASK_ACR_L2MDF_488_PPM                  0x40
+#define MASK_ACR_L2MDF_976_PPM                  0x50
+#define MASK_ACR_L2MDF_1976_PPM                 0x60
+#define MASK_ACR_L2MDF_3906_PPM                 0x70
+#define MASK_ACR_L1MDF                          0x07
+#define MASK_ACR_L1MDF_0_PPM                    0x00
+#define MASK_ACR_L1MDF_61_PPM                   0x01
+#define MASK_ACR_L1MDF_122_PPM                  0x02
+#define MASK_ACR_L1MDF_244_PPM                  0x03
+#define MASK_ACR_L1MDF_488_PPM                  0x04
+#define MASK_ACR_L1MDF_976_PPM                  0x05
+#define MASK_ACR_L1MDF_1976_PPM                 0x06
+#define MASK_ACR_L1MDF_3906_PPM                 0x07
+
+#define ACR_MDF1                                0x8642
+#define MASK_ACR_L3MDF                          0x07
+#define MASK_ACR_L3MDF_0_PPM                    0x00
+#define MASK_ACR_L3MDF_61_PPM                   0x01
+#define MASK_ACR_L3MDF_122_PPM                  0x02
+#define MASK_ACR_L3MDF_244_PPM                  0x03
+#define MASK_ACR_L3MDF_488_PPM                  0x04
+#define MASK_ACR_L3MDF_976_PPM                  0x05
+#define MASK_ACR_L3MDF_1976_PPM                 0x06
+#define MASK_ACR_L3MDF_3906_PPM                 0x07
+
+#define SDO_MODE1                               0x8652
+#define MASK_SDO_BIT_LENG                       0x70
+#define MASK_SDO_FMT                            0x03
+#define MASK_SDO_FMT_RIGHT                      0x00
+#define MASK_SDO_FMT_LEFT                       0x01
+#define MASK_SDO_FMT_I2S                        0x02
+
+#define DIV_MODE                                0x8665 /* Not in REF_01 */
+#define MASK_DIV_DLY                            0xf0
+#define SET_DIV_DLY_MS(milliseconds)            ((((milliseconds)/100) << 4) & \
+                                                MASK_DIV_DLY)
+#define MASK_DIV_MODE                           0x01
+
+#define HDMIAUDIO_MODE                          0x8680
+
+/* *** HDMI General (16 bit) *** */
+#define DDC_CTL                                 0x8543
+#define MASK_DDC_ACTION                         0x04
+#define MASK_DDC5V_MODE                         0x03
+#define MASK_DDC5V_MODE_0MS                     0x00
+#define MASK_DDC5V_MODE_50MS                    0x01
+#define MASK_DDC5V_MODE_100MS                   0x02
+#define MASK_DDC5V_MODE_200MS                   0x03
+
+#define HPD_CTL                                 0x8544
+#define MASK_HPD_OUT0                           (1 << 0)
+#define MASK_HPD_CTL0                           (1 << 4)
+
+#define INIT_END                                0x854A
+#define MASK_INIT_END                           0x01
+
+/* *** Video Mute *** */
+#define VI_MUTE                                 0x857F
+#define MASK_AUTO_MUTE                          0xC0
+#define MASK_VI_MUTE                            0x10
+#define MASK_VI_BLACK                           0x01
+
+/* *** Info Frame *** */
+#define PK_INT_MODE                             0x8709
+#define MASK_ISRC2_INT_MODE                     0x80
+#define MASK_ISRC_INT_MODE                      0x40
+#define MASK_ACP_INT_MODE                       0x20
+#define MASK_VS_INT_MODE                        0x10
+#define MASK_SPD_INT_MODE                       0x08
+#define MASK_MS_INT_MODE                        0x04
+#define MASK_AUD_INT_MODE                       0x02
+#define MASK_AVI_INT_MODE                       0x01
+
+#define NO_PKT_LIMIT                            0x870B
+#define MASK_NO_ACP_LIMIT                       0xF0
+#define SET_NO_ACP_LIMIT_MS(milliseconds)       ((((milliseconds)/80) << 4) & \
+                                                  MASK_NO_ACP_LIMIT)
+
+#define NO_PKT_CLR                              0x870C
+#define MASK_NO_VS_CLR                          0x40
+#define MASK_NO_SPD_CLR                         0x20
+#define MASK_NO_ACP_CLR                         0x10
+#define MASK_NO_AVI_CLR1                        0x02
+#define MASK_NO_AVI_CLR0                        0x01
+
+#define ERR_PK_LIMIT                            0x870D
+#define NO_PKT_LIMIT2                           0x870E
+#define PK_AVI_0HEAD                            0x8710
+#define PK_AVI_1HEAD                            0x8711
+#define PK_AVI_2HEAD                            0x8712
+#define PK_AVI_0BYTE                            0x8713
+#define PK_AVI_1BYTE                            0x8714
+#define PK_AVI_2BYTE                            0x8715
+#define PK_AVI_3BYTE                            0x8716
+#define PK_AVI_4BYTE                            0x8717
+#define PK_AVI_5BYTE                            0x8718
+#define PK_AVI_6BYTE                            0x8719
+#define PK_AVI_7BYTE                            0x871A
+#define PK_AVI_8BYTE                            0x871B
+#define PK_AVI_9BYTE                            0x871C
+#define PK_AVI_10BYTE                           0x871D
+#define PK_AVI_11BYTE                           0x871E
+#define PK_AVI_12BYTE                           0x871F
+#define PK_AVI_13BYTE                           0x8720
+#define PK_AVI_14BYTE                           0x8721
+#define PK_AVI_15BYTE                           0x8722
+#define PK_AVI_16BYTE                           0x8723
+
+#define NO_GDB_LIMIT                            0x9007
+
+/* *** Color Bar (16 bit) *** */
+#define CB_CTL                                  0x7000
+#define CB_HSW                                  0x7008
+#define CB_VSW                                  0x700A
+#define CB_HTOTOAL                              0x700C
+#define CB_VTOTOAL                              0x700E
+#define CB_HACT                                 0x7010
+#define CB_VACT                                 0x7012
+#define CB_HSTART                               0x7014
+#define CB_VSTART                               0x7016
+
+enum {
+        TC358840_MODE_3840X2160,
+        TC358840_MODE_1920X1080,
+        TC358840_MODE_1280X720,
+};
+
+static const int tc358840_30fps[] = {
+        30,
+};
+
+static const int tc358840_30_60fps[] = {
+        30,
+        60,
+};
+
+static const struct camera_common_frmfmt tc358840_frmfmt[] = {
+        {{3840, 2160},  tc358840_30fps, 1, 1, TC358840_MODE_3840X2160},
+        {{1920, 1080},  tc358840_30_60fps, 2, 1, TC358840_MODE_1920X1080},
+        {{1280, 720},   tc358840_30_60fps, 2, 1, TC358840_MODE_1280X720},
+};
+#endif  /* __TC358840_TABLES__ */
diff --git a/drivers/media/platform/tegra/Makefile b/drivers/media/platform/tegra/Makefile
new file mode 100644
index 000000000..414f22f69
--- /dev/null
+++ b/drivers/media/platform/tegra/Makefile
@@ -0,0 +1,12 @@
+GCOV_PROFILE := y
+
+#subdir-ccflags-y := -Werror
+#
+# Makefile for the video capture/playback device drivers.
+#
+obj-y   += camera/
+obj-y   += vi/
+obj-$(CONFIG_VIDEO_CAMERA)      += regmap_util.o
+obj-$(CONFIG_VIDEO_TEGRA_VI_TPG) += tpg/
+
+obj-$(CONFIG_TEGRA_MIPI_CAL)    += mipical/
diff --git a/drivers/media/platform/tegra/camera/Makefile b/drivers/media/platform/tegra/camera/Makefile
new file mode 100644
index 000000000..8474f451a
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/Makefile
@@ -0,0 +1,10 @@
+GCOV_PROFILE := y
+obj-y                           += vi/
+obj-y                           += csi/
+ccflags-y += -I../nvhost/drivers/video/tegra/host
+ccflags-y += -Idrivers/video/tegra/host
+ccflags-y += -Idrivers/video/tegra/camera
+ccflags-y += -Idrivers/media/platform/tegra
+ccflags-y += -Werror
+
+obj-y += camera_common.o camera_gpio.o sensor_common.o
diff --git a/drivers/media/platform/tegra/camera/camera_common.c b/drivers/media/platform/tegra/camera/camera_common.c
new file mode 100644
index 000000000..17a253640
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/camera_common.c
@@ -0,0 +1,858 @@
+/*
+ * camera_common.c - utilities for tegra camera driver
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <media/camera_common.h>
+#include <linux/of_graph.h>
+#include <linux/string.h>
+#include <soc/tegra/pmc.h>
+#include "camera/vi/mc_common.h"
+
+#define has_s_op(master, op) \
+        (master->ops && master->ops->op)
+#define call_s_op(master, op) \
+        (has_s_op(master, op) ? \
+         master->ops->op(master) : 0)
+#define call_s_ops(master, op, ...) \
+        (has_s_op(master, op) ? \
+         master->ops->op(master, __VA_ARGS__) : 0)
+
+static const struct camera_common_colorfmt camera_common_color_fmts[] = {
+        {
+                MEDIA_BUS_FMT_SRGGB12_1X12,
+                V4L2_COLORSPACE_SRGB,
+                V4L2_PIX_FMT_SRGGB12,
+        },
+        {
+                MEDIA_BUS_FMT_SRGGB10_1X10,
+                V4L2_COLORSPACE_SRGB,
+                V4L2_PIX_FMT_SRGGB10,
+        },
+        {
+                MEDIA_BUS_FMT_SBGGR10_1X10,
+                V4L2_COLORSPACE_SRGB,
+                V4L2_PIX_FMT_SBGGR10,
+        },
+        {
+                MEDIA_BUS_FMT_SRGGB8_1X8,
+                V4L2_COLORSPACE_SRGB,
+                V4L2_PIX_FMT_SRGGB8,
+        },
+        /*
+         * The below two formats are not supported by VI4,
+         * keep them at the last to ensure they get discarded
+         */
+        {
+                MEDIA_BUS_FMT_XRGGB10P_3X10,
+                V4L2_COLORSPACE_SRGB,
+                V4L2_PIX_FMT_XRGGB10P,
+        },
+        {
+                MEDIA_BUS_FMT_XBGGR10P_3X10,
+                V4L2_COLORSPACE_SRGB,
+                V4L2_PIX_FMT_XRGGB10P,
+        },
+};
+
+static const char *camera_common_csi_io_pads[] = {
+        "csia",
+        "csib",
+        "csic",
+        "csid",
+        "csie",
+        "csif",
+};
+
+bool camera_common_verify_code(struct tegra_channel *chan, unsigned int code)
+{
+        int i;
+
+        for (i = 0; i < chan->num_video_formats; i++) {
+                if (chan->video_formats[i]->code == code)
+                        return true;
+        }
+
+        return false;
+}
+
+int camera_common_g_ctrl(struct camera_common_data *s_data,
+                         struct v4l2_control *control)
+{
+        int i;
+
+        for (i = 0; i < s_data->numctrls; i++) {
+                if (s_data->ctrls[i]->id == control->id) {
+                        control->value = s_data->ctrls[i]->val;
+                        dev_dbg(&s_data->i2c_client->dev,
+                                 "%s: found control %s\n", __func__,
+                                 s_data->ctrls[i]->name);
+                        return 0;
+                }
+        }
+
+        return -EFAULT;
+}
+
+int camera_common_regulator_get(struct i2c_client *client,
+                       struct regulator **vreg, const char *vreg_name)
+{
+        struct regulator *reg = NULL;
+        int err = 0;
+
+        reg = devm_regulator_get(&client->dev, vreg_name);
+        if (unlikely(IS_ERR(reg))) {
+                dev_err(&client->dev, "%s %s ERR: %p\n",
+                        __func__, vreg_name, reg);
+                err = PTR_ERR(reg);
+                reg = NULL;
+        } else
+                dev_dbg(&client->dev, "%s: %s\n",
+                        __func__, vreg_name);
+
+        *vreg = reg;
+        return err;
+}
+
+int camera_common_parse_clocks(struct i2c_client *client,
+                        struct camera_common_pdata *pdata)
+{
+        struct device_node *np = client->dev.of_node;
+        const char *prop;
+        int proplen = 0;
+        int i = 0;
+        int numclocks = 0;
+        int mclk_index = 0;
+        int parentclk_index = -1;
+        int err = 0;
+
+
+        pdata->mclk_name = NULL;
+        pdata->parentclk_name = NULL;
+        err = of_property_read_string(np, "mclk", &pdata->mclk_name);
+        if (!err) {
+                dev_dbg(&client->dev, "mclk in DT %s\n", pdata->mclk_name);
+                of_property_read_string(np, "parent-clk",
+                                              &pdata->parentclk_name);
+                return 0;
+        }
+
+        prop = (const char *)of_get_property(np, "clock-names", &proplen);
+        if (!prop)
+                return -ENODATA;
+
+        /* find length of clock-names string array */
+        for (i = 0; i < proplen; i++) {
+                if (prop[i] == '\0')
+                        numclocks++;
+        }
+
+        if (numclocks > 1) {
+                err = of_property_read_u32(np, "mclk-index", &mclk_index);
+                if (err) {
+                        dev_err(&client->dev, "Failed to find mclk index\n");
+                        return err;
+                }
+                err = of_property_read_u32(np, "parent-clk-index",
+                                           &parentclk_index);
+        }
+
+        for (i = 0; i < numclocks; i++) {
+                if (i == mclk_index) {
+                        pdata->mclk_name = prop;
+                        dev_dbg(&client->dev, "%s: mclk_name is %s\n",
+                                 __func__, pdata->mclk_name);
+                } else if (i == parentclk_index) {
+                        pdata->parentclk_name = prop;
+                        dev_dbg(&client->dev, "%s: parentclk_name is %s\n",
+                                 __func__, pdata->parentclk_name);
+                } else
+                        dev_dbg(&client->dev, "%s: %s\n", __func__, prop);
+                prop += strlen(prop) + 1;
+        }
+
+        return 0;
+}
+
+int camera_common_parse_ports(struct i2c_client *client,
+                              struct camera_common_data *s_data)
+{
+        struct device_node *node = client->dev.of_node;
+        struct device_node *ep = NULL;
+        struct device_node *next;
+        int bus_width = 0;
+        int err = 0;
+        int port = 0;
+
+        /* Parse all the remote entities and put them into the list */
+        next = of_graph_get_next_endpoint(node, ep);
+        if (!next)
+                return -ENODATA;
+
+        of_node_put(ep);
+        ep = next;
+
+        err = of_property_read_u32(ep, "bus-width", &bus_width);
+        if (err) {
+                dev_err(&client->dev,
+                        "Failed to find num of lanes\n");
+                return err;
+        }
+        s_data->numlanes = bus_width;
+
+        err = of_property_read_u32(ep, "csi-port", &port);
+        if (err) {
+                dev_err(&client->dev,
+                        "Failed to find CSI port\n");
+                return err;
+        }
+        s_data->csi_port = port;
+
+        dev_dbg(&client->dev, "%s: csi port %d num of lanes %d\n",
+                __func__, s_data->csi_port, s_data->numlanes);
+
+        return 0;
+}
+
+int camera_common_debugfs_show(struct seq_file *s, void *unused)
+{
+        struct camera_common_data *s_data = s->private;
+
+        dev_dbg(&s_data->i2c_client->dev, "%s: ++\n", __func__);
+
+        return 0;
+}
+
+ssize_t camera_common_debugfs_write(
+        struct file *file,
+        char const __user *buf,
+        size_t count,
+        loff_t *offset)
+{
+        struct camera_common_data *s_data =
+                ((struct seq_file *)file->private_data)->private;
+        struct i2c_client *client = s_data->i2c_client;
+        int err = 0;
+        char buffer[MAX_BUFFER_SIZE];
+        u32 address;
+        u32 data;
+        u8 readback = 0;
+
+        dev_dbg(&client->dev, "%s: ++\n", __func__);
+
+        if (copy_from_user(&buffer, buf, sizeof(buffer)))
+                goto debugfs_write_fail;
+
+        if (sscanf(buf, "0x%x 0x%x", &address, &data) == 2)
+                goto set_attr;
+        if (sscanf(buf, "0X%x 0X%x", &address, &data) == 2)
+                goto set_attr;
+        if (sscanf(buf, "%d %d", &address, &data) == 2)
+                goto set_attr;
+
+        if (sscanf(buf, "0x%x 0x%x", &address, &data) == 1)
+                goto read;
+        if (sscanf(buf, "0X%x 0X%x", &address, &data) == 1)
+                goto read;
+        if (sscanf(buf, "%d %d", &address, &data) == 1)
+                goto read;
+
+        dev_err(&client->dev, "SYNTAX ERROR: %s\n", buf);
+        return -EFAULT;
+
+set_attr:
+        dev_dbg(&client->dev,
+                        "new address = %x, data = %x\n", address, data);
+        err |= call_s_ops(s_data, write_reg, address, data);
+read:
+        err |= call_s_ops(s_data, read_reg, address, &readback);
+        dev_dbg(&client->dev,
+                        "wrote to address 0x%x with value 0x%x\n",
+                        address, readback);
+
+        if (err)
+                goto debugfs_write_fail;
+
+        return count;
+
+debugfs_write_fail:
+        dev_err(&client->dev,
+                        "%s: test pattern write failed\n", __func__);
+        return -EFAULT;
+}
+
+int camera_common_debugfs_open(struct inode *inode, struct file *file)
+{
+        struct camera_common_data *s_data = inode->i_private;
+        struct i2c_client *client = s_data->i2c_client;
+
+        dev_dbg(&client->dev, "%s: ++\n", __func__);
+
+        return single_open(file, camera_common_debugfs_show, inode->i_private);
+}
+
+static const struct file_operations camera_common_debugfs_fops = {
+        .open           = camera_common_debugfs_open,
+        .read           = seq_read,
+        .write          = camera_common_debugfs_write,
+        .llseek         = seq_lseek,
+        .release        = single_release,
+};
+
+void camera_common_remove_debugfs(
+                struct camera_common_data *s_data)
+{
+        struct i2c_client *client = s_data->i2c_client;
+
+        dev_dbg(&client->dev, "%s: ++\n", __func__);
+
+        debugfs_remove_recursive(s_data->debugdir);
+        s_data->debugdir = NULL;
+}
+
+void camera_common_create_debugfs(
+                struct camera_common_data *s_data,
+                const char *name)
+{
+        struct dentry *err;
+        struct i2c_client *client = s_data->i2c_client;
+
+        dev_dbg(&client->dev, "%s %s\n", __func__, name);
+
+        s_data->debugdir =
+                debugfs_create_dir(name, NULL);
+        if (!s_data->debugdir)
+                goto remove_debugfs;
+
+        err = debugfs_create_file("d",
+                                S_IWUSR | S_IRUGO,
+                                s_data->debugdir, s_data,
+                                &camera_common_debugfs_fops);
+        if (!err)
+                goto remove_debugfs;
+
+        return;
+remove_debugfs:
+        dev_err(&client->dev, "couldn't create debugfs\n");
+        camera_common_remove_debugfs(s_data);
+}
+
+/* Find a data format by a pixel code in an array */
+const struct camera_common_colorfmt *camera_common_find_datafmt(
+                unsigned int code)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(camera_common_color_fmts); i++)
+                if (camera_common_color_fmts[i].code == code)
+                        return camera_common_color_fmts + i;
+
+        return NULL;
+}
+EXPORT_SYMBOL(camera_common_find_datafmt);
+
+int camera_common_enum_mbus_code(struct v4l2_subdev *sd,
+                                struct v4l2_subdev_pad_config *cfg,
+                                struct v4l2_subdev_mbus_code_enum *code)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct tegra_channel *chan = v4l2_get_subdev_hostdata(sd);
+        unsigned int mbus_code;
+
+        if (s_data->num_color_fmts < 1 || !s_data->color_fmts) {
+                s_data->color_fmts = camera_common_color_fmts;
+                s_data->num_color_fmts = ARRAY_SIZE(camera_common_color_fmts);
+        }
+
+        if ((unsigned int)code->index >= s_data->num_color_fmts)
+                return -EINVAL;
+
+        mbus_code = s_data->color_fmts[code->index].code;
+        if (!camera_common_verify_code(chan, mbus_code))
+                return -EINVAL;
+
+        code->code = mbus_code;
+        return 0;
+}
+EXPORT_SYMBOL(camera_common_enum_mbus_code);
+
+int camera_common_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+                        unsigned int *code)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+
+        if (s_data->num_color_fmts < 1 || !s_data->color_fmts) {
+                s_data->color_fmts = camera_common_color_fmts;
+                s_data->num_color_fmts = ARRAY_SIZE(camera_common_color_fmts);
+        }
+
+        if ((unsigned int)index >= s_data->num_color_fmts)
+                return -EINVAL;
+        *code = s_data->color_fmts[index].code;
+        return 0;
+}
+
+int camera_common_try_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        struct tegra_channel *chan = v4l2_get_subdev_hostdata(sd);
+        struct v4l2_control hdr_control;
+        const struct camera_common_frmfmt *frmfmt = s_data->frmfmt;
+        int hdr_en;
+        int err = 0;
+        int i;
+
+        dev_dbg(&client->dev, "%s: size %i x %i\n", __func__,
+                 mf->width, mf->height);
+
+        /* check hdr enable ctrl */
+        hdr_control.id = V4L2_CID_HDR_EN;
+
+        err = v4l2_g_ctrl(s_data->ctrl_handler, &hdr_control);
+        if (err < 0) {
+                dev_err(&client->dev, "could not find device ctrl.\n");
+                return err;
+        }
+
+        hdr_en = switch_ctrl_qmenu[hdr_control.value];
+
+        s_data->mode = s_data->def_mode;
+        s_data->fmt_width = s_data->def_width;
+        s_data->fmt_height = s_data->def_height;
+
+        if (s_data->use_sensor_mode_id &&
+                s_data->sensor_mode_id >= 0 &&
+                s_data->sensor_mode_id < s_data->numfmts) {
+                dev_dbg(&client->dev, "%s: use_sensor_mode_id %d\n",
+                                __func__, s_data->sensor_mode_id);
+                s_data->mode = frmfmt[s_data->sensor_mode_id].mode;
+                s_data->fmt_width = mf->width;
+                s_data->fmt_height = mf->height;
+        } else {
+                for (i = 0; i < s_data->numfmts; i++) {
+                        if (mf->width == frmfmt[i].size.width &&
+                                mf->height == frmfmt[i].size.height &&
+                                hdr_en == frmfmt[i].hdr_en) {
+                                s_data->mode = frmfmt[i].mode;
+                                s_data->fmt_width = mf->width;
+                                s_data->fmt_height = mf->height;
+                                break;
+                        }
+                }
+
+                if (i == s_data->numfmts) {
+                        mf->width = s_data->fmt_width;
+                        mf->height = s_data->fmt_height;
+                        dev_dbg(&client->dev,
+                                "%s: invalid resolution supplied to set mode %d %d\n",
+                                __func__, mf->width, mf->height);
+                }
+        }
+
+        if (!camera_common_verify_code(chan, mf->code))
+                err = -EINVAL;
+
+        mf->field = V4L2_FIELD_NONE;
+        mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+        return err;
+}
+
+int camera_common_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        int ret;
+
+        dev_dbg(&client->dev, "%s(%u) size %i x %i\n", __func__,
+                        mf->code, mf->width, mf->height);
+
+        /* MIPI CSI could have changed the format, double-check */
+        if (!camera_common_find_datafmt(mf->code))
+                return -EINVAL;
+
+        ret = camera_common_try_fmt(sd, mf);
+
+        s_data->colorfmt = camera_common_find_datafmt(mf->code);
+
+        return ret;
+}
+
+int camera_common_g_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        const struct camera_common_colorfmt *fmt = s_data->colorfmt;
+
+        dev_dbg(&client->dev, "%s++\n", __func__);
+
+        mf->code        = fmt->code;
+        mf->colorspace  = fmt->colorspace;
+        mf->width       = s_data->fmt_width;
+        mf->height      = s_data->fmt_height;
+        mf->field       = V4L2_FIELD_NONE;
+
+        return 0;
+}
+
+static int camera_common_evaluate_color_format(struct v4l2_subdev *sd,
+                                               int pixelformat)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        int i;
+
+        if (!s_data)
+                return -EINVAL;
+
+        if (s_data->num_color_fmts < 1 || !s_data->color_fmts) {
+                s_data->color_fmts = camera_common_color_fmts;
+                s_data->num_color_fmts = ARRAY_SIZE(camera_common_color_fmts);
+        }
+
+        for (i = 0; i < s_data->num_color_fmts; i++) {
+                if (s_data->color_fmts[i].pix_fmt == pixelformat)
+                        break;
+        }
+
+        if (i >= s_data->num_color_fmts)
+                return -EINVAL;
+
+        return 0;
+}
+
+int camera_common_enum_framesizes(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_frame_size_enum *fse)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        int ret;
+
+        if (!s_data || !s_data->frmfmt)
+                return -EINVAL;
+
+        if (fse->index >= s_data->numfmts)
+                return -EINVAL;
+
+        ret = camera_common_evaluate_color_format(sd, fse->code);
+        if (ret)
+                return ret;
+
+        fse->min_width = fse->max_width =
+                s_data->frmfmt[fse->index].size.width;
+        fse->min_height = fse->max_height =
+                s_data->frmfmt[fse->index].size.height;
+        return 0;
+}
+EXPORT_SYMBOL(camera_common_enum_framesizes);
+
+int camera_common_enum_frameintervals(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_frame_interval_enum *fie)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+        int i, ret;
+
+        if (!s_data || !s_data->frmfmt)
+                return -EINVAL;
+
+        /* Check color format */
+        ret = camera_common_evaluate_color_format(sd, fie->code);
+        if (ret)
+                return ret;
+
+        /* Check resolution sizes */
+        for (i = 0; i < s_data->numfmts; i++) {
+                if (s_data->frmfmt[i].size.width == fie->width &&
+                    s_data->frmfmt[i].size.height == fie->height)
+                        break;
+        }
+        if (i >= s_data->numfmts)
+                return -EINVAL;
+
+        /* Check index is in the rage of framerates array index */
+        if (fie->index >= s_data->frmfmt[i].num_framerates)
+                return -EINVAL;
+
+        fie->interval.numerator = 1;
+        fie->interval.denominator =
+                s_data->frmfmt[i].framerates[fie->index];
+
+        return 0;
+}
+EXPORT_SYMBOL(camera_common_enum_frameintervals);
+
+static void camera_common_mclk_disable(struct camera_common_data *s_data)
+{
+        struct camera_common_power_rail *pw = s_data->power;
+
+        if (!pw) {
+                dev_err(&s_data->i2c_client->dev, "%s: no device power rail\n",
+                        __func__);
+                return;
+        }
+
+        dev_dbg(&s_data->i2c_client->dev, "%s: disable MCLK\n", __func__);
+        clk_disable_unprepare(pw->mclk);
+}
+
+static int camera_common_mclk_enable(struct camera_common_data *s_data)
+{
+        int err;
+        struct camera_common_power_rail *pw = s_data->power;
+        unsigned long mclk_init_rate = s_data->def_clk_freq;
+
+        if (!pw) {
+                dev_err(s_data->dev, "%s: no device power rail\n",
+                        __func__);
+                return -ENODEV;
+        }
+
+        dev_dbg(s_data->dev, "%s: enable MCLK with %lu Hz\n",
+                __func__, mclk_init_rate);
+
+        err = clk_set_rate(pw->mclk, mclk_init_rate);
+        if (!err)
+                err = clk_prepare_enable(pw->mclk);
+
+        return err;
+}
+
+void camera_common_dpd_disable(struct camera_common_data *s_data)
+{
+        int i;
+        int io_idx;
+        /* 2 lanes per port, divide by two to get numports */
+        int numports = (s_data->numlanes + 1) >> 1;
+
+        /* disable CSI IOs DPD mode to turn on camera */
+        for (i = 0; i < numports; i++) {
+                io_idx = s_data->csi_port + i;
+                tegra_pmc_io_pad_low_power_disable(
+                                camera_common_csi_io_pads[io_idx]);
+                dev_dbg(s_data->dev,
+                         "%s: csi %d\n", __func__, io_idx);
+        }
+}
+
+void camera_common_dpd_enable(struct camera_common_data *s_data)
+{
+        int i;
+        int io_idx;
+        /* 2 lanes per port, divide by two to get numports */
+        int numports = (s_data->numlanes + 1) >> 1;
+
+        /* disable CSI IOs DPD mode to turn on camera */
+        for (i = 0; i < numports; i++) {
+                io_idx = s_data->csi_port + i;
+                tegra_pmc_io_pad_low_power_enable(
+                                camera_common_csi_io_pads[io_idx]);
+                dev_dbg(s_data->dev,
+                         "%s: csi %d\n", __func__, io_idx);
+        }
+}
+
+int camera_common_s_power(struct v4l2_subdev *sd, int on)
+{
+        int err = 0;
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_data *s_data = to_camera_common_data(client);
+
+        if (on) {
+                err = camera_common_mclk_enable(s_data);
+                if (err)
+                        return err;
+
+                camera_common_dpd_disable(s_data);
+
+                err = call_s_op(s_data, power_on);
+                if (err) {
+                        dev_err(s_data->dev,
+                                "%s: error power on\n", __func__);
+                        camera_common_dpd_enable(s_data);
+                        camera_common_mclk_disable(s_data);
+                }
+        } else {
+                call_s_op(s_data, power_off);
+                camera_common_dpd_enable(s_data);
+                camera_common_mclk_disable(s_data);
+        }
+
+        return err;
+}
+
+int camera_common_g_mbus_config(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_config *cfg)
+{
+        cfg->type = V4L2_MBUS_CSI2;
+        cfg->flags = V4L2_MBUS_CSI2_4_LANE |
+                V4L2_MBUS_CSI2_CHANNEL_0 |
+                V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
+
+        return 0;
+}
+
+int camera_common_focuser_s_power(struct v4l2_subdev *sd, int on)
+{
+        int err = 0;
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_common_focuser_data *s_data =
+                        to_camera_common_focuser_data(client);
+
+        if (on) {
+                err = call_s_op(s_data, power_on);
+                if (err)
+                        dev_err(&s_data->i2c_client->dev,
+                                "%s: error power on\n", __func__);
+        } else
+                err = call_s_op(s_data, power_off);
+
+        return err;
+}
+
+int camera_common_focuser_init(struct camera_common_focuser_data *s_data)
+{
+        int err = 0;
+
+        /* power on */
+        err = call_s_op(s_data, power_on);
+        if (err) {
+                dev_err(&s_data->i2c_client->dev,
+                        "%s: error power on\n", __func__);
+                return err;
+        }
+
+        /* load default configuration */
+        err = call_s_op(s_data, load_config);
+        if (err) {
+                dev_err(&s_data->i2c_client->dev,
+                        "%s: error loading config\n", __func__);
+                goto fail;
+        }
+
+        /* set controls */
+        err = call_s_op(s_data, ctrls_init);
+        if (err)
+                dev_err(&s_data->i2c_client->dev,
+                        "%s: error initializing controls\n", __func__);
+
+fail:
+        /* power off */
+        err |= call_s_op(s_data, power_off);
+
+        return err;
+}
+
+int camera_common_parse_sensor_mode(struct i2c_client *client,
+                        struct camera_common_pdata *pdata)
+{
+        struct device_node *np = client->dev.of_node;
+        char temp_str[OF_MAX_STR_LEN];
+        const char *str;
+        struct device_node *node;
+        int num_modes = 0;
+        int err, i;
+
+        /* get number of modes */
+        for (i = 0; num_modes < MAX_NUM_SENSOR_MODES; i++) {
+                snprintf(temp_str, sizeof(temp_str), "%s%d",
+                        OF_SENSORMODE_PREFIX, i);
+                of_node_get(np);
+                node = of_find_node_by_name(np, temp_str);
+                if (node == NULL)
+                        break;
+                num_modes++;
+        }
+
+        pdata->mode_info = devm_kzalloc(&client->dev,
+                num_modes * sizeof(struct camera_common_mode_info),
+                GFP_KERNEL);
+        if (!pdata->mode_info) {
+                dev_err(&client->dev, "Failed to allocate memory for mode info\n");
+                return -ENOMEM;
+        }
+        memset(pdata->mode_info, 0, num_modes *
+               sizeof(struct camera_common_mode_info));
+
+        /* parse mode info */
+        for (i = 0; i < num_modes; i++) {
+                snprintf(temp_str, sizeof(temp_str), "%s%d",
+                        OF_SENSORMODE_PREFIX, i);
+                of_node_get(np);
+                node = of_find_node_by_name(np, temp_str);
+                if (node == NULL) {
+                        dev_err(&client->dev, "Failed to find mode\n");
+                        return -ENODATA;
+                };
+
+                /* read mode width */
+                of_property_read_string(node, "active_w", &str);
+                if (str == NULL) {
+                        dev_err(&client->dev, "Failed to read mode width\n");
+                        return -ENODATA;
+                };
+                err = kstrtoint(str, 10, &pdata->mode_info[i].width);
+                if (err) {
+                        dev_err(&client->dev, "Failed to convert mode width\n");
+                        return -EFAULT;
+                }
+                /* read mode height */
+                of_property_read_string(node, "active_h", &str);
+                if (str == NULL) {
+                        dev_err(&client->dev, "Failed to read mode height\n");
+                        return -ENODATA;
+                };
+                err = kstrtoint(str, 10, &pdata->mode_info[i].height);
+                if (err) {
+                        dev_err(&client->dev, "Failed to convert mode height\n");
+                        return -EFAULT;
+                }
+                dev_info(&client->dev, "%s: mode %d x %d:\n", __func__,
+                        pdata->mode_info[i].width, pdata->mode_info[i].height);
+
+                of_property_read_string(node, "line_length", &str);
+                if (str == NULL) {
+                        dev_err(&client->dev, "Failed to read mode line_length\n");
+                        return -ENODATA;
+                };
+                err = kstrtoint(str, 10, &pdata->mode_info[i].line_length);
+                if (err) {
+                        dev_err(&client->dev, "Failed to convert mode line_length\n");
+                        return -EFAULT;
+                }
+
+                of_property_read_string(node, "pix_clk_hz", &str);
+                if (str == NULL) {
+                        dev_err(&client->dev, "Failed to read mode pix_clk_hz\n");
+                        return -ENODATA;
+                };
+                err = kstrtoll(str, 10, &pdata->mode_info[i].pixel_clock);
+                if (err) {
+                        dev_err(&client->dev, "Failed to convert mode pix_clk_hz\n");
+                        return -EFAULT;
+                }
+                dev_info(&client->dev, "%s: line_length = %d, pixel_clock = %llu\n",
+                        __func__, pdata->mode_info[i].line_length,
+                        pdata->mode_info[i].pixel_clock);
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL(camera_common_parse_sensor_mode);
diff --git a/drivers/media/platform/tegra/camera/camera_gpio.c b/drivers/media/platform/tegra/camera/camera_gpio.c
new file mode 100644
index 000000000..fcd53e3be
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/camera_gpio.c
@@ -0,0 +1,160 @@
+/*
+ * virtual.c - Camera GPIO driver
+ *
+ * Copyright (c) 2014-2015, NVIDIA CORPORATION.  All rights reserved.
+
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/list.h>
+#include <linux/debugfs.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+
+#include "camera_gpio.h"
+
+struct camera_gpio {
+        struct list_head list;
+        unsigned gpio_num;
+        struct mutex mutex;
+        atomic_t state_cnt;
+        atomic_t use_cnt;
+};
+
+static DEFINE_MUTEX(g_mutex);
+static LIST_HEAD(cam_gpio_list);
+
+int cam_gpio_register(struct i2c_client *client,
+                        unsigned pin_num) {
+        struct camera_gpio *new_gpio;
+        struct camera_gpio *next_gpio;
+
+        mutex_lock(&g_mutex);
+
+
+        list_for_each_entry(next_gpio, &cam_gpio_list, list) {
+                if (next_gpio->gpio_num == pin_num) {
+                        dev_dbg(&client->dev,
+                                 "%s: gpio pin %u already registered.\n",
+                                 __func__, pin_num);
+
+                        atomic_inc(&next_gpio->use_cnt);
+
+                        mutex_unlock(&g_mutex);
+                        return 0;
+                }
+        }
+
+        /* gpio is not present in the cam_gpio_list, add it */
+        new_gpio = kzalloc(sizeof(*new_gpio), GFP_KERNEL);
+        if (!new_gpio) {
+                dev_err(&client->dev, "%s: memory low!\n", __func__);
+                mutex_unlock(&g_mutex);
+                return -EFAULT;
+        }
+
+        dev_dbg(&client->dev, "%s: adding cam gpio %u\n",
+                 __func__, pin_num);
+
+        new_gpio->gpio_num = pin_num;
+        mutex_init(&new_gpio->mutex);
+        atomic_inc(&new_gpio->use_cnt);
+
+        list_add(&new_gpio->list, &cam_gpio_list);
+
+        mutex_unlock(&g_mutex);
+        return 0;
+}
+EXPORT_SYMBOL(cam_gpio_register);
+
+void cam_gpio_deregister(struct i2c_client *client,
+                        unsigned pin_num) {
+        struct camera_gpio *next_gpio;
+
+        mutex_lock(&g_mutex);
+
+
+        list_for_each_entry(next_gpio, &cam_gpio_list, list) {
+                if (next_gpio->gpio_num == pin_num) {
+                        atomic_dec(&next_gpio->use_cnt);
+
+                        if (atomic_read(&next_gpio->use_cnt) == 0) {
+                                list_del(&next_gpio->list);
+                                kfree(next_gpio);
+
+                                dev_dbg(&client->dev,
+                                         "%s: removing cam gpio %u\n",
+                                         __func__, pin_num);
+                        }
+
+                        break;
+                }
+        }
+
+        mutex_unlock(&g_mutex);
+        return;
+}
+EXPORT_SYMBOL(cam_gpio_deregister);
+
+int cam_gpio_ctrl(struct i2c_client *client,
+                        unsigned pin_num, int val,
+                        bool active_high) /* val: 0=deassert, 1=assert */
+{
+        struct camera_gpio *next_gpio;
+        int err = -EINVAL;
+        int pin_val;
+        bool found = false;
+
+        list_for_each_entry(next_gpio, &cam_gpio_list, list) {
+                mutex_lock(&next_gpio->mutex);
+                if (next_gpio->gpio_num == pin_num) {
+                        found = true;
+
+                        if (!atomic_read(&next_gpio->state_cnt) &&
+                                        !val) {
+                                dev_err(&client->dev,
+                                         "%s: state cnt can't be < 0\n",
+                                         __func__);
+                                mutex_unlock(&next_gpio->mutex);
+                                return err;
+                        }
+
+                        if (val)
+                                atomic_inc(&next_gpio->state_cnt);
+                        else
+                                atomic_dec(&next_gpio->state_cnt);
+
+                        pin_val = active_high ? val : !val;
+                        pin_val &= 1;
+                        err = pin_val;
+
+                        /* subtract val allows a 0 check to be
+                         * used to indicate that gpio can be written to*/
+                        if (atomic_read(&next_gpio->state_cnt) - val == 0) {
+                                gpio_set_value_cansleep(pin_num, pin_val);
+                                dev_dbg(&client->dev, "%s %u %d\n",
+                                         __func__, pin_num, pin_val);
+                        }
+                }
+                mutex_unlock(&next_gpio->mutex);
+        }
+
+        if (!found)
+                dev_dbg(&client->dev,
+                         "WARNING %s: gpio %u not in list\n",
+                         __func__, pin_num);
+
+        return err; /* return value written or error */
+}
+EXPORT_SYMBOL(cam_gpio_ctrl);
diff --git a/drivers/media/platform/tegra/camera/camera_gpio.h b/drivers/media/platform/tegra/camera/camera_gpio.h
new file mode 100644
index 000000000..b388e178d
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/camera_gpio.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __CAMERA_GPIO_H__
+#define __CAMERA_GPIO_H__
+
+int cam_gpio_register(struct i2c_client *client,
+                        unsigned pin_num);
+
+void cam_gpio_deregister(struct i2c_client *client,
+                        unsigned pin_num);
+
+int cam_gpio_ctrl(struct i2c_client *client,
+                        unsigned pin_num, int ref_inc, bool active_high);
+
+#endif
+/* __CAMERA_GPIO_H__ */
diff --git a/drivers/media/platform/tegra/camera/csi/Makefile b/drivers/media/platform/tegra/camera/csi/Makefile
new file mode 100644
index 000000000..3d307cb83
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/csi/Makefile
@@ -0,0 +1,8 @@
+GCOV_PROFILE := y
+ccflags-y += -I../nvhost/drivers/video/tegra/host
+ccflags-y += -I../nvhost/include
+ccflags-y += -Idrivers/video/tegra/camera
+ccflags-y += -Idrivers/media/platform/tegra
+ccflags-y += -Werror
+
+obj-y += csi.o csi2_fops.o csi4_fops.o
diff --git a/drivers/media/platform/tegra/camera/csi/csi.c b/drivers/media/platform/tegra/camera/csi/csi.c
new file mode 100644
index 000000000..f6c838ff2
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/csi/csi.c
@@ -0,0 +1,784 @@
+/*
+ * NVIDIA Tegra CSI Device
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/of_platform.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/camera_common.h>
+
+#include "dev.h"
+#include "vi/vi.h"
+#include "camera/csi/csi.h"
+#include "camera/vi/mc_common.h"
+#include "mipical/mipi_cal.h"
+#include "linux/nvhost.h"
+
+static int set_csi_properties(struct tegra_csi_device *csi,
+                        struct platform_device *pdev)
+{
+        struct camera_common_data *s_data = &csi->s_data[0];
+
+        /*
+        * These values are only used for tpg mode
+        * With sensor, CSI power and clock info are provided
+        * by the sensor sub device
+        */
+        s_data->csi_port = 0;
+        s_data->numlanes = 12;
+        csi->clk_freq = TEGRA_CLOCK_CSI_PORT_MAX;
+
+        return 0;
+}
+
+static void update_blank_intervals(struct tegra_csi_channel *chan,
+                int portnum, int fmtindex)
+{
+        struct tegra_csi_port *port = &chan->ports[portnum];
+        const struct tpg_frmfmt *tegra_csi_tpg_frmfmt =
+                                                chan->csi->tpg_frmfmt_table;
+
+        port->framerate = tegra_csi_tpg_frmfmt[fmtindex].framerate;
+        port->h_blank = tegra_csi_tpg_frmfmt[fmtindex].h_blank;
+        port->v_blank = tegra_csi_tpg_frmfmt[fmtindex].v_blank;
+}
+
+void set_csi_portinfo(struct tegra_csi_device *csi,
+        unsigned int port, unsigned int numlanes)
+{
+        struct camera_common_data *s_data = &csi->s_data[port];
+
+        s_data->csi_port = port;
+        s_data->numlanes = numlanes;
+        s_data->def_clk_freq = TEGRA_CLOCK_CSI_PORT_MAX;
+}
+EXPORT_SYMBOL(set_csi_portinfo);
+
+int tegra_csi_power(struct tegra_csi_device *csi, int enable)
+{
+        int err = 0;
+
+        if (enable) {
+                err = csi->fops->csi_power_on(csi);
+                if (!err)
+                        atomic_inc(&csi->power_ref);
+        } else {
+                err = csi->fops->csi_power_off(csi);
+                if (!err)
+                        atomic_dec(&csi->power_ref);
+        }
+        return err;
+}
+EXPORT_SYMBOL(tegra_csi_power);
+
+int tegra_csi_s_power(struct v4l2_subdev *subdev, int enable)
+{
+        int err = 0;
+        struct tegra_csi_device *csi = to_csi(subdev);
+
+        err = tegra_csi_power(csi, enable);
+
+        return err;
+}
+
+/*
+ * -----------------------------------------------------------------------------
+ * CSI Subdevice Video Operations
+ * -----------------------------------------------------------------------------
+ */
+
+int tegra_csi_start_streaming(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_device *csi = chan->csi;
+
+        return csi->fops->csi_start_streaming(chan, port_num);
+}
+EXPORT_SYMBOL(tegra_csi_start_streaming);
+
+void tegra_csi_stop_streaming(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_device *csi = chan->csi;
+
+        csi->fops->csi_stop_streaming(chan, port_num);
+}
+EXPORT_SYMBOL(tegra_csi_stop_streaming);
+
+static int tegra_csi_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+        struct tegra_csi_device *csi;
+        struct tegra_csi_channel *chan = to_csi_chan(subdev);
+        struct tegra_channel *tegra_chan = v4l2_get_subdev_hostdata(subdev);
+        int i, ret = 0;
+
+        csi = to_csi(subdev);
+        if (!csi)
+                return -EINVAL;
+        if (!chan->pg_mode) {
+                if (enable) {
+                        tegra_mipi_bias_pad_enable();
+                        csi->fops->mipical(chan);
+                } else
+                        tegra_mipi_bias_pad_disable();
+        }
+        if (tegra_chan->bypass)
+                return 0;
+
+        for (i = 0; i < tegra_chan->valid_ports; i++) {
+                if (enable) {
+                        ret = tegra_csi_start_streaming(chan, i);
+                        if (ret)
+                                tegra_csi_stop_streaming(chan, i);
+                } else
+                        tegra_csi_stop_streaming(chan, i);
+        }
+        return ret;
+}
+
+/*
+ * Only use this subdevice media bus ops for test pattern generator,
+ * because CSI device is an separated subdevice which has 6 source
+ * pads to generate test pattern.
+ */
+static struct v4l2_mbus_framefmt tegra_csi_tpg_fmts[] = {
+        {
+                TEGRA_DEF_WIDTH,
+                TEGRA_DEF_HEIGHT,
+                MEDIA_BUS_FMT_SRGGB10_1X10,
+                V4L2_FIELD_NONE,
+                V4L2_COLORSPACE_SRGB
+        },
+        {
+                TEGRA_DEF_WIDTH,
+                TEGRA_DEF_HEIGHT,
+                MEDIA_BUS_FMT_RGB888_1X32_PADHI,
+                V4L2_FIELD_NONE,
+                V4L2_COLORSPACE_SRGB
+        }
+
+};
+
+static struct v4l2_frmsize_discrete tegra_csi_tpg_sizes[] = {
+        {1280, 720},
+        {1920, 1080},
+        {3840, 2160}
+};
+
+static int tegra_csi_enum_framesizes(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_frame_size_enum *fse)
+{
+        int i;
+        struct tegra_csi_channel *chan = to_csi_chan(sd);
+        struct tegra_channel *vi_chan = v4l2_get_subdev_hostdata(sd);
+
+        if (!chan->pg_mode)
+                return -ENOIOCTLCMD;
+
+        if (fse->index >= ARRAY_SIZE(tegra_csi_tpg_sizes))
+                return -EINVAL;
+
+        for (i = 0; i < ARRAY_SIZE(tegra_csi_tpg_fmts); i++) {
+                const struct tegra_video_format *format =
+                      tegra_core_get_format_by_code(vi_chan,
+                                tegra_csi_tpg_fmts[i].code, 0);
+                if (format && format->fourcc == fse->code)
+                        break;
+        }
+        if (i == ARRAY_SIZE(tegra_csi_tpg_fmts))
+                return -EINVAL;
+
+        fse->min_width = fse->max_width =
+                        tegra_csi_tpg_sizes[fse->index].width;
+        fse->min_height = fse->max_height =
+                        tegra_csi_tpg_sizes[fse->index].height;
+        return 0;
+}
+
+static int tegra_csi_get_fmtindex(struct tegra_csi_channel *chan,
+                                int width, int height, int pixel_format)
+{
+        int i;
+        const struct tpg_frmfmt *tegra_csi_tpg_frmfmt =
+                                                chan->csi->tpg_frmfmt_table;
+
+        for (i = 0; i < chan->csi->tpg_frmfmt_table_size; i++) {
+                if (tegra_csi_tpg_frmfmt[i].frmsize.width == width &&
+                    tegra_csi_tpg_frmfmt[i].frmsize.height == height &&
+                    tegra_csi_tpg_frmfmt[i].pixel_format == pixel_format)
+                        break;
+        }
+
+        if (i == chan->csi->tpg_frmfmt_table_size)
+                return -EINVAL;
+
+        return i;
+}
+
+static int tegra_csi_enum_frameintervals(struct v4l2_subdev *sd,
+                struct v4l2_subdev_pad_config *cfg,
+                struct v4l2_subdev_frame_interval_enum *fie)
+{
+        int index;
+        struct tegra_csi_channel *chan = to_csi_chan(sd);
+        const struct tegra_video_format *format;
+        const struct tpg_frmfmt *tegra_csi_tpg_frmfmt =
+                                                chan->csi->tpg_frmfmt_table;
+        struct tegra_channel *vi_chan = v4l2_get_subdev_hostdata(sd);
+
+        if (!chan->pg_mode)
+                return -ENOIOCTLCMD;
+
+        /* One resolution just one framerate */
+        if (fie->index > 0)
+                return -EINVAL;
+        format = tegra_core_get_format_by_fourcc(vi_chan, fie->code);
+        if (!format)
+                return -EINVAL;
+        index = tegra_csi_get_fmtindex(chan, fie->width, fie->height,
+                                        format->fourcc);
+        if (index < 0)
+                return -EINVAL;
+
+        fie->interval.numerator = 1;
+        fie->interval.denominator = tegra_csi_tpg_frmfmt[index].framerate;
+
+        return 0;
+}
+
+static int tegra_csi_try_mbus_fmt(struct v4l2_subdev *sd,
+                                  struct v4l2_mbus_framefmt *mf)
+{
+        int i, j;
+        struct tegra_csi_channel *chan = to_csi_chan(sd);
+        static struct v4l2_frmsize_discrete *sizes;
+
+        if (!chan->pg_mode)
+                return -ENOIOCTLCMD;
+
+        for (i = 0; i < ARRAY_SIZE(tegra_csi_tpg_fmts); i++) {
+                struct v4l2_mbus_framefmt *fmt = &tegra_csi_tpg_fmts[i];
+
+                if (mf->code == fmt->code && mf->field == fmt->field &&
+                    mf->colorspace == fmt->colorspace) {
+                        for (j = 0; j < ARRAY_SIZE(tegra_csi_tpg_sizes); j++) {
+                                sizes = &tegra_csi_tpg_sizes[j];
+                                if (mf->width == sizes->width &&
+                                    mf->height == sizes->height)
+                                        return 0;
+                        }
+                }
+        }
+
+        memcpy(mf, tegra_csi_tpg_fmts, sizeof(struct v4l2_mbus_framefmt));
+
+        return 0;
+}
+
+static int tegra_csi_g_mbus_fmt(struct v4l2_subdev *sd,
+                                struct v4l2_mbus_framefmt *fmt)
+{
+        struct tegra_csi_channel *chan = to_csi_chan(sd);
+        struct v4l2_mbus_framefmt *format = &chan->ports[0].format;
+
+        if (!chan->pg_mode) {
+                dev_err(chan->csi->dev, "CSI is not in TPG mode\n");
+                return -EINVAL;
+        }
+
+        mutex_lock(&chan->format_lock);
+        memcpy(fmt, format, sizeof(struct v4l2_mbus_framefmt));
+        mutex_unlock(&chan->format_lock);
+
+        return 0;
+}
+
+static int csi_is_power_on(struct tegra_csi_device *csi)
+{
+        return atomic_read(&csi->power_ref);
+}
+static int tegra_csi_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+        struct tegra_csi_device *csi = to_csi(sd);
+
+        /* Set status to 0 if power is on
+         * Set status to 1 if power is off
+         */
+        *status = !csi_is_power_on(csi);
+
+        return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+static int tegra_csi_get_format(struct v4l2_subdev *subdev,
+                           struct v4l2_subdev_pad_config *cfg,
+                           struct v4l2_subdev_format *fmt)
+{
+        struct tegra_csi_channel *chan = to_csi_chan(subdev);
+        struct v4l2_mbus_framefmt *mbus_fmt = &fmt->format;
+        int ret;
+
+        if (!chan->pg_mode)
+                return -ENOIOCTLCMD;
+        ret = tegra_csi_g_mbus_fmt(subdev, mbus_fmt);
+        if (ret)
+                return ret;
+
+        return 0;
+}
+
+static int tegra_csi_set_format(struct v4l2_subdev *subdev,
+                           struct v4l2_subdev_pad_config *cfg,
+                           struct v4l2_subdev_format *fmt)
+{
+        int ret;
+        struct tegra_csi_channel *chan = to_csi_chan(subdev);
+        struct v4l2_mbus_framefmt *format = &fmt->format;
+        const struct tegra_video_format *vf;
+        struct tegra_channel *vi_chan = v4l2_get_subdev_hostdata(subdev);
+        int index, i;
+
+        if (!chan->pg_mode)
+                return -ENOIOCTLCMD;
+
+        ret = tegra_csi_try_mbus_fmt(subdev, format);
+        if (ret)
+                return ret;
+
+        if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+                return 0;
+
+        vf = tegra_core_get_format_by_code(vi_chan, format->code, 0);
+        if (!vf) {
+                dev_err(chan->csi->dev, "Fail to find tegra video fmt");
+                mutex_unlock(&chan->format_lock);
+                return -EINVAL;
+        }
+        index = tegra_csi_get_fmtindex(chan, format->width,
+                                format->height, vf->fourcc);
+        if (index < 0) {
+                dev_err(chan->csi->dev, "Fail to find matching fmt");
+                return -EINVAL;
+        }
+
+        mutex_lock(&chan->format_lock);
+        for (i = 0; i < vi_chan->valid_ports; i++) {
+                memcpy(&chan->ports[i].format,
+                       &fmt->format, sizeof(struct v4l2_mbus_framefmt));
+                chan->ports[i].core_format = vf;
+                update_blank_intervals(chan, i, index);
+        }
+        mutex_unlock(&chan->format_lock);
+
+        return 0;
+}
+
+static int tegra_csi_g_frame_interval(struct v4l2_subdev *sd,
+                        struct v4l2_subdev_frame_interval *vfi)
+{
+        struct tegra_csi_channel *chan = to_csi_chan(sd);
+        struct tegra_csi_port *port = &chan->ports[0];
+
+        if (!port->framerate)
+                return -EINVAL;
+
+        vfi->interval.numerator = 1;
+        vfi->interval.denominator = port->framerate;
+
+        return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+static struct v4l2_subdev_video_ops tegra_csi_video_ops = {
+        .s_stream       = tegra_csi_s_stream,
+        .g_input_status = tegra_csi_g_input_status,
+        .g_frame_interval = tegra_csi_g_frame_interval,
+};
+
+static struct v4l2_subdev_pad_ops tegra_csi_pad_ops = {
+        .get_fmt        = tegra_csi_get_format,
+        .set_fmt        = tegra_csi_set_format,
+        .enum_frame_size = tegra_csi_enum_framesizes,
+        .enum_frame_interval = tegra_csi_enum_frameintervals,
+};
+
+static struct v4l2_subdev_core_ops tegra_csi_core_ops = {
+        .s_power        = tegra_csi_s_power,
+};
+
+static struct v4l2_subdev_ops tegra_csi_ops = {
+        .core   = &tegra_csi_core_ops,
+        .video  = &tegra_csi_video_ops,
+        .pad    = &tegra_csi_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media Operations
+ */
+
+static const struct media_entity_operations tegra_csi_media_ops = {
+        .link_validate = v4l2_subdev_link_validate,
+};
+
+/* -----------------------------------------------------------------------------
+ * Platform Device Driver
+ */
+
+static int tegra_csi_get_port_info(struct tegra_csi_channel *chan,
+                                struct device_node *node, unsigned int index)
+{
+        struct device_node *ep = NULL;
+        struct device_node *ports;
+        struct device_node *port;
+        struct device_node *chan_dt;
+
+        int value = 0xFFFF;
+        int ret = 0, i;
+
+        memset(&chan->port[0], INVALID_CSI_PORT, TEGRA_CSI_BLOCKS);
+        for_each_child_of_node(node, chan_dt) {
+                if (!chan_dt->name || of_node_cmp(chan_dt->name, "channel"))
+                        continue;
+                ret = of_property_read_u32(chan_dt, "reg", &value);
+                if (ret < 0)
+                        return -EINVAL;
+                if (value == index)
+                        break;
+        }
+
+        chan->subdev.of_node = chan_dt;
+        ports = of_get_child_by_name(chan_dt, "ports");
+        if (ports == NULL)
+                return -EINVAL;
+
+        for_each_child_of_node(ports, port) {
+                if (!port->name || of_node_cmp(port->name, "port"))
+                        continue;
+                ret = of_property_read_u32(port, "reg", &value);
+                if (ret < 0)
+                        continue;
+                if (value != 0)
+                        continue;
+                for_each_child_of_node(port, ep) {
+                        if (!ep->name || of_node_cmp(ep->name, "endpoint"))
+                                continue;
+                        ret = of_property_read_u32(ep, "csi-port", &value);
+                        if (ret < 0)
+                                dev_err(chan->csi->dev, "No csi port info\n");
+                        chan->port[0] = value;
+
+                        ret = of_property_read_u32(ep, "bus-width", &value);
+                        if (ret < 0)
+                                dev_err(chan->csi->dev, "No bus width info\n");
+                        chan->numlanes = value;
+                        if (value > 12) {
+                                dev_err(chan->csi->dev, "Invalid num lanes\n");
+                                return -EINVAL;
+                        }
+                        /*
+                         * for numlanes greater than 4 multiple CSI bricks
+                         * are needed to capture the image, the logic below
+                         * checks for numlanes > 4 and add a new CSI brick
+                         * as a valid port. Loops around the three CSI
+                         * bricks to add as many ports necessary.
+                         */
+                        value -= 4;
+                        for (i = 1; value > 0; i++, value -= 4) {
+                                int next_port = chan->port[i-1] + 2;
+
+                                next_port = (next_port % (PORT_F + 1));
+                                chan->port[i] = next_port;
+                        }
+                }
+        }
+
+        for (i = 0; csi_port_is_valid(chan->port[i]); i++)
+                chan->numports++;
+
+        return 0;
+}
+
+int tegra_csi_init(struct tegra_csi_device *csi,
+                struct platform_device *pdev)
+{
+        int err = 0;
+        struct nvhost_device_data *pdata = platform_get_drvdata(pdev);
+
+        csi->dev = &pdev->dev;
+        err = set_csi_properties(csi, pdev);
+        if (err)
+                return err;
+
+        csi->iomem_base = pdata->aperture[0];
+        csi->fops->hw_init(csi);
+        return err;
+}
+
+static int tegra_csi_channel_init_one(struct tegra_csi_channel *chan)
+{
+        struct v4l2_subdev *sd;
+        int numlanes = 0;
+        struct tegra_csi_device *csi = chan->csi;
+        int i, ret;
+        const struct tegra_video_format *vf;
+
+        mutex_init(&chan->format_lock);
+
+        vf = tegra_core_get_default_format();
+        if (vf == NULL) {
+                dev_err(csi->dev, "Fail to find tegra video fmt");
+                return -EINVAL;
+        }
+
+        sd = &chan->subdev;
+        /* Initialize V4L2 subdevice and media entity */
+        v4l2_subdev_init(sd, &tegra_csi_ops);
+        sd->dev = chan->csi->dev;
+        v4l2_set_subdevdata(sd, csi);
+        sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+        sd->entity.ops = &tegra_csi_media_ops;
+        chan->ports = devm_kzalloc(csi->dev,
+                        chan->numports * sizeof(struct tegra_csi_port),
+                        GFP_KERNEL);
+        if (!chan->ports)
+                return -ENOMEM;
+
+        /* Initialize the default format */
+        for (i = 0; i < chan->numports; i++) {
+                chan->ports[i].format.code = vf->vf_code;
+                chan->ports[i].format.field = V4L2_FIELD_NONE;
+                chan->ports[i].format.colorspace = V4L2_COLORSPACE_SRGB;
+                chan->ports[i].format.width = TEGRA_DEF_WIDTH;
+                chan->ports[i].format.height = TEGRA_DEF_HEIGHT;
+                chan->ports[i].core_format = vf;
+        }
+        if (chan->pg_mode) {
+                /* If CSI has 2 existing channels, chan->id will start
+                 * from 2 for the first TPG channel, which uses PORT_A(0).
+                 * To get the correct PORT number, subtract existing number of
+                 * channels from chan->id.
+                 */
+                chan->port[0] = chan->id - csi->num_channels;
+                WARN_ON(chan->port[0] > TPG_CHANNELS);
+                chan->ports[0].num = chan->id - csi->num_channels;
+                chan->ports->lanes = 2;
+                chan->pads = devm_kzalloc(csi->dev, sizeof(*chan->pads),
+                                GFP_KERNEL);
+                if (!chan->pads)
+                        return -ENOMEM;
+                chan->pads[0].flags = MEDIA_PAD_FL_SOURCE;
+        } else {
+                chan->pads = devm_kzalloc(csi->dev, 2 * sizeof(*chan->pads),
+                        GFP_KERNEL);
+                if (!chan->pads)
+                        return -ENOMEM;
+                chan->pads[0].flags = MEDIA_PAD_FL_SINK;
+                chan->pads[1].flags = MEDIA_PAD_FL_SOURCE;
+        }
+        snprintf(sd->name, sizeof(sd->name), "%s-%d",
+                         chan->pg_mode ? "tpg" :
+                         (strlen(csi->devname) == 0 ?
+                          dev_name(csi->dev) : csi->devname),
+                          chan->port[0]);
+        /* Initialize media entity */
+        ret = media_entity_init(&sd->entity,
+                        chan->pg_mode ? 1 : 2,
+                        chan->pads, 0);
+        if (ret < 0)
+                return ret;
+
+        for (i = 0; i < chan->numports; i++) {
+                numlanes = chan->numlanes - (i * MAX_CSI_BLOCK_LANES);
+                WARN_ON(numlanes < 0);
+                numlanes = numlanes > MAX_CSI_BLOCK_LANES ?
+                        MAX_CSI_BLOCK_LANES : numlanes;
+                chan->ports[i].lanes = numlanes;
+                chan->ports[i].num = chan->port[i];
+        }
+
+        if (!chan->pg_mode) {
+                ret = v4l2_async_register_subdev(sd);
+                if (ret < 0) {
+                        dev_err(csi->dev, "failed to register subdev\n");
+                        media_entity_cleanup(&sd->entity);
+                }
+        }
+        return ret;
+}
+
+static int tegra_csi_channels_init(struct tegra_csi_device *csi)
+{
+        int ret;
+        struct tegra_csi_channel *it;
+
+        list_for_each_entry(it, &csi->csi_chans, list) {
+                ret = tegra_csi_channel_init_one(it);
+                if (ret)
+                        return ret;
+        }
+
+        return 0;
+}
+
+static int csi_parse_dt(struct tegra_csi_device *csi,
+                        struct platform_device *pdev)
+{
+        int err = 0, i;
+        int num_channels = 0;
+        struct device_node *node = pdev->dev.of_node;
+        struct tegra_csi_channel *item;
+
+        if (strncmp(node->name, "nvcsi", 5)) {
+                node = of_find_node_by_name(node, "nvcsi");
+                strncpy(csi->devname, "nvcsi", 6);
+        }
+        if (!node)
+                return -EINVAL;
+        err = of_property_read_u32(node, "num-channels", &num_channels);
+        if (err) {
+                dev_dbg(csi->dev, " Failed to find num of channels, set to 0\n");
+                num_channels = 0;
+        }
+
+        csi->num_channels = num_channels;
+        for (i = 0; i < num_channels; i++) {
+                item = devm_kzalloc(csi->dev, sizeof(*item), GFP_KERNEL);
+                if (!item)
+                        return -ENOMEM;
+                list_add_tail(&item->list, &csi->csi_chans);
+                item->csi = csi;
+                item->id = i;
+                err = tegra_csi_get_port_info(item, node, item->id);
+                if (err)
+                        return err;
+        }
+
+        return 0;
+}
+
+int tpg_csi_media_controller_init(struct tegra_csi_device *csi, int pg_mode)
+{
+        int i, err;
+        struct tegra_csi_channel *item;
+
+        if (!csi)
+                return -EINVAL;
+        for (i = 0; i < TPG_CHANNELS; i++) {
+                item = devm_kzalloc(csi->dev, sizeof(*item), GFP_KERNEL);
+                if (!item) {
+                        err = -ENOMEM;
+                        goto channel_init_error;
+                }
+                if (i == 0)
+                        csi->tpg_start = item;
+                list_add_tail(&item->list, &csi->csi_chans);
+                item->numlanes = 2;
+                item->numports = 1;
+                item->csi = csi;
+                item->pg_mode = pg_mode;
+                item->id = csi->num_channels + i;
+                err = tegra_csi_channel_init_one(item);
+                if (err)
+                        goto channel_init_error;
+        }
+        csi->fops->hw_init(csi);
+        csi->num_channels += TPG_CHANNELS;
+
+        return err;
+
+channel_init_error:
+        if (csi->tpg_start)
+                tpg_csi_media_controller_cleanup(csi);
+        dev_err(csi->dev, "%s: Error\n", __func__);
+        return err;
+}
+EXPORT_SYMBOL(tpg_csi_media_controller_init);
+
+void tpg_csi_media_controller_cleanup(struct tegra_csi_device *csi)
+{
+        struct tegra_csi_channel *item;
+        struct tegra_csi_channel *itemn;
+        struct v4l2_subdev *sd;
+
+        list_for_each_entry_safe(item, itemn, &csi->csi_chans, list) {
+                if (!item->pg_mode)
+                        continue;
+                sd = &item->subdev;
+                /* decrement media device entity count */
+                if (sd->entity.parent)
+                        sd->entity.parent->entity_id--;
+                v4l2_device_unregister_subdev(sd);
+                media_entity_cleanup(&sd->entity);
+                list_del(&item->list);
+                devm_kfree(csi->dev, item);
+        }
+        csi->num_channels -= TPG_CHANNELS;
+        csi->tpg_start = NULL;
+}
+EXPORT_SYMBOL(tpg_csi_media_controller_cleanup);
+int tegra_csi_media_controller_init(struct tegra_csi_device *csi,
+                                    struct platform_device *pdev)
+{
+        int ret;
+
+        csi->dev = &pdev->dev;
+        csi->pdev = pdev;
+        atomic_set(&csi->power_ref, 0);
+        atomic_set(&csi->tpg_enabled, 0);
+        INIT_LIST_HEAD(&csi->csi_chans);
+        ret = csi_parse_dt(csi, pdev);
+        if (ret < 0)
+                return ret;
+
+        /*
+         * if there is no csi channels listed in DT,
+         * no need to init the channel and graph
+         */
+        if (csi->num_channels == 0)
+                return 0;
+
+        ret = tegra_csi_channels_init(csi);
+        ret = tegra_csi_init(csi, pdev);
+        if (ret < 0)
+                dev_err(&pdev->dev, "Failed to init csi property,clks\n");
+
+        return 0;
+}
+EXPORT_SYMBOL(tegra_csi_media_controller_init);
+
+int tegra_csi_media_controller_remove(struct tegra_csi_device *csi)
+{
+        struct tegra_csi_channel *chan;
+        struct v4l2_subdev *sd;
+
+        list_for_each_entry(chan, &csi->csi_chans, list) {
+                sd = &chan->subdev;
+                v4l2_async_unregister_subdev(sd);
+                media_entity_cleanup(&sd->entity);
+        }
+        return 0;
+}
+EXPORT_SYMBOL(tegra_csi_media_controller_remove);
diff --git a/drivers/media/platform/tegra/camera/csi/csi.h b/drivers/media/platform/tegra/camera/csi/csi.h
new file mode 100644
index 000000000..208e15e42
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/csi/csi.h
@@ -0,0 +1,158 @@
+/*
+ * NVIDIA Tegra CSI Device Header
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __CSI_H_
+#define __CSI_H_
+
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+#include <linux/platform_device.h>
+
+#include <media/camera_common.h>
+#include <linux/platform_device.h>
+
+#include "camera/vi/registers.h"
+#include "camera/csi/csi4_registers.h"
+
+#define MAX_CSI_BLOCK_LANES 4
+
+enum tegra_csi_port_num {
+        PORT_A = 0,
+        PORT_B = 1,
+        PORT_C = 2,
+        PORT_D = 3,
+        PORT_E = 4,
+        PORT_F = 5,
+};
+
+#define csi_port_is_valid(port) (port > PORT_F ? 0 : 1)
+
+enum camera_gang_mode {
+        CAMERA_NO_GANG_MODE = 0,
+        CAMERA_GANG_L_R = 1,
+        CAMERA_GANG_T_B,
+        CAMERA_GANG_R_L,
+        CAMERA_GANG_B_T
+};
+
+struct tegra_channel;
+
+struct tpg_frmfmt {
+        struct v4l2_frmsize_discrete frmsize;
+        int pixel_format;
+        int framerate;
+        int h_blank;
+        int v_blank;
+};
+
+struct tegra_csi_port {
+        void __iomem *pixel_parser;
+        void __iomem *cil;
+        void __iomem *tpg;
+
+        /* One pair of sink/source pad has one format */
+        struct v4l2_mbus_framefmt format;
+        const struct tegra_video_format *core_format;
+        unsigned int lanes;
+        unsigned int framerate;
+        unsigned int h_blank;
+        unsigned int v_blank;
+
+        enum tegra_csi_port_num num;
+};
+
+struct tegra_csi_device {
+        struct device *dev;
+        struct platform_device *pdev;
+        char devname[32];
+        void __iomem *iomem_base;
+        void __iomem *iomem[3];
+        struct clk *plld_dsi;
+        struct clk *plld;
+
+        struct camera_common_data s_data[6];
+        struct tegra_csi_port *ports;
+        struct media_pad *pads;
+
+        unsigned int clk_freq;
+        int num_ports;
+        int num_channels;
+        struct list_head csi_chans;
+        struct tegra_csi_channel *tpg_start;
+        struct tegra_csi_fops *fops;
+        const struct tpg_frmfmt *tpg_frmfmt_table;
+        unsigned int tpg_frmfmt_table_size;
+        atomic_t power_ref;
+
+        struct dentry *debugdir;
+        atomic_t tpg_enabled;
+};
+
+/*
+ * subdev: channel subdev
+ * numports: Number of CSI ports in use for this channel
+ * numlanes: Number of CIL lanes in use
+ */
+struct tegra_csi_channel {
+        struct list_head list;
+        struct v4l2_subdev subdev;
+        struct media_pad *pads;
+        struct media_pipeline pipe;
+
+        struct tegra_csi_device *csi;
+        struct tegra_csi_port *ports;
+        unsigned char port[TEGRA_CSI_BLOCKS];
+        struct mutex format_lock;
+        unsigned int numports;
+        unsigned int numlanes;
+        unsigned int pg_mode;
+        struct camera_common_data *s_data;
+        unsigned int id;
+};
+
+static inline struct tegra_csi_channel *to_csi_chan(struct v4l2_subdev *subdev)
+{
+        return container_of(subdev, struct tegra_csi_channel, subdev);
+}
+
+static inline struct tegra_csi_device *to_csi(struct v4l2_subdev *subdev)
+{
+        struct tegra_csi_channel *chan = to_csi_chan(subdev);
+
+        return chan->csi;
+}
+
+void set_csi_portinfo(struct tegra_csi_device *csi,
+        unsigned int port, unsigned int numlanes);
+void tegra_csi_status(struct tegra_csi_channel *chan,
+                        enum tegra_csi_port_num port_num);
+int tegra_csi_error(struct tegra_csi_channel *chan,
+                        enum tegra_csi_port_num port_num);
+int tegra_csi_start_streaming(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num);
+void tegra_csi_stop_streaming(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num);
+void tegra_csi_error_recover(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num);
+int tegra_csi_power(struct tegra_csi_device *csi, int enable);
+#define tegra_csi_power_on(csi) tegra_csi_power(csi, 1)
+#define tegra_csi_power_off(csi) tegra_csi_power(csi, 0)
+int tegra_csi_init(struct tegra_csi_device *csi,
+                struct platform_device *pdev);
+int tegra_csi_media_controller_init(struct tegra_csi_device *csi,
+                                struct platform_device *pdev);
+int tegra_csi_media_controller_remove(struct tegra_csi_device *csi);
+int tpg_csi_media_controller_init(struct tegra_csi_device *csi, int pg_mode);
+void tpg_csi_media_controller_cleanup(struct tegra_csi_device *csi);
+#endif
diff --git a/drivers/media/platform/tegra/camera/csi/csi2_fops.c b/drivers/media/platform/tegra/camera/csi/csi2_fops.c
new file mode 100644
index 000000000..d633a6205
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/csi/csi2_fops.c
@@ -0,0 +1,455 @@
+/*
+ * Tegra CSI2 device common APIs
+ *
+ * Copyright (c) 2016, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/clk/tegra.h>
+#include "camera/csi/csi.h"
+#include "camera/vi/mc_common.h"
+#include "mipical/mipi_cal.h"
+#include "nvhost_acm.h"
+#include <linux/clk/tegra.h>
+
+static void csi_write(struct tegra_csi_channel *chan, unsigned int addr,
+                        u32 val, u8 port)
+{
+        struct tegra_csi_device *csi = chan->csi;
+
+        writel(val, (csi->iomem[port] + addr));
+}
+
+static u32 csi_read(struct tegra_csi_channel *chan, unsigned int addr,
+                        u8 port)
+{
+        struct tegra_csi_device *csi = chan->csi;
+
+        return readl((csi->iomem[port] + addr));
+}
+
+/* Pixel parser registers accessors */
+static void pp_write(struct tegra_csi_port *port, u32 addr, u32 val)
+{
+        writel(val, port->pixel_parser + addr);
+}
+
+static u32 pp_read(struct tegra_csi_port *port, u32 addr)
+{
+        return readl(port->pixel_parser + addr);
+}
+
+/* CSI CIL registers accessors */
+static void cil_write(struct tegra_csi_port *port, u32 addr, u32 val)
+{
+        writel(val, port->cil + addr);
+}
+
+static u32 cil_read(struct tegra_csi_port *port, u32 addr)
+{
+        return readl(port->cil + addr);
+}
+
+/* Test pattern generator registers accessor */
+static void tpg_write(struct tegra_csi_port *port,
+                        unsigned int addr, u32 val)
+{
+        writel(val, port->tpg + addr);
+}
+
+int tegra_csi_error(struct tegra_csi_channel *chan,
+                        enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_port *port;
+        u32 val;
+        int err = 0;
+
+        port = &chan->ports[port_num];
+        /*
+         * only uncorrectable header error and multi-bit
+         * transmission errors are checked as they cannot be
+         * corrected automatically
+         */
+        val = pp_read(port, TEGRA_CSI_PIXEL_PARSER_STATUS);
+        err |= val & 0x4000;
+        pp_write(port, TEGRA_CSI_PIXEL_PARSER_STATUS, val);
+
+        val = cil_read(port, TEGRA_CSI_CIL_STATUS);
+        err |= val & 0x02;
+        cil_write(port, TEGRA_CSI_CIL_STATUS, val);
+
+        val = cil_read(port, TEGRA_CSI_CILX_STATUS);
+        err |= val & 0x00020020;
+        cil_write(port, TEGRA_CSI_CILX_STATUS, val);
+
+        return err;
+}
+
+void tegra_csi_status(struct tegra_csi_channel *chan,
+                        enum tegra_csi_port_num port_num)
+{
+        int i;
+        u32 val;
+        struct tegra_csi_port *port;
+
+        for (i = 0; i < chan->numports; i++) {
+                port = &chan->ports[i];
+                val = pp_read(port, TEGRA_CSI_PIXEL_PARSER_STATUS);
+
+                dev_dbg(chan->csi->dev,
+                        "TEGRA_CSI_PIXEL_PARSER_STATUS 0x%08x\n",
+                        val);
+
+                val = cil_read(port, TEGRA_CSI_CIL_STATUS);
+                dev_dbg(chan->csi->dev,
+                        "TEGRA_CSI_CIL_STATUS 0x%08x\n", val);
+
+                val = cil_read(port, TEGRA_CSI_CILX_STATUS);
+                dev_dbg(chan->csi->dev,
+                        "TEGRA_CSI_CILX_STATUS 0x%08x\n", val);
+        }
+}
+EXPORT_SYMBOL(tegra_csi_status);
+
+void tegra_csi_error_recover(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_port *port;
+        struct tegra_csi_device *csi;
+        int i;
+
+        csi = chan->csi;
+
+        for (i = 0; i < chan->numports; i++) {
+                port = &chan->ports[i];
+
+                if (port->lanes == 4) {
+                        int port_val = ((port_num >> 1) << 1);
+                        struct tegra_csi_port *port_a =
+                                &chan->ports[port_val];
+                        struct tegra_csi_port *port_b =
+                                &chan->ports[port_val+1];
+
+                        tpg_write(port_a,
+                                TEGRA_CSI_PATTERN_GENERATOR_CTRL, PG_ENABLE);
+                        tpg_write(port_b,
+                                TEGRA_CSI_PATTERN_GENERATOR_CTRL, PG_ENABLE);
+                        cil_write(port_a,
+                                TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x1);
+                        cil_write(port_b,
+                                TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x1);
+                        csi_write(chan, TEGRA_CSI_CSI_SW_STATUS_RESET, 0x1,
+                                        port_num >> 1);
+                        /* sleep for clock cycles to drain the Rx FIFO */
+                        usleep_range(10, 20);
+                        cil_write(port_a,
+                                TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x0);
+                        cil_write(port_b,
+                                TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x0);
+                        csi_write(chan,
+                                TEGRA_CSI_CSI_SW_STATUS_RESET,
+                                0x0, port_num >> 1);
+                        tpg_write(port_a,
+                                TEGRA_CSI_PATTERN_GENERATOR_CTRL, PG_DISABLE);
+                        tpg_write(port_b,
+                                TEGRA_CSI_PATTERN_GENERATOR_CTRL, PG_DISABLE);
+                } else {
+                        tpg_write(port,
+                                TEGRA_CSI_PATTERN_GENERATOR_CTRL, PG_ENABLE);
+                        cil_write(port,
+                                TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x1);
+                        csi_write(chan,
+                                TEGRA_CSI_CSI_SW_STATUS_RESET,
+                                0x1, port_num >> 1);
+                        /* sleep for clock cycles to drain the Rx FIFO */
+                        usleep_range(10, 20);
+                        cil_write(port,
+                                TEGRA_CSI_CIL_SW_SENSOR_RESET, 0x0);
+                        csi_write(chan,
+                                TEGRA_CSI_CSI_SW_STATUS_RESET,
+                                0x0, port_num >> 1);
+                        tpg_write(port,
+                                TEGRA_CSI_PATTERN_GENERATOR_CTRL, PG_DISABLE);
+                }
+        }
+}
+
+
+static int tpg_clk_enable(struct tegra_csi_device *csi)
+{
+        int err = 0;
+
+        if (atomic_inc_return(&csi->tpg_enabled) != 1)
+                return 0;
+
+        clk_set_rate(csi->plld, TEGRA_CLOCK_TPG);
+        err = clk_prepare_enable(csi->plld);
+        if (err) {
+                dev_err(csi->dev, "pll_d enable failed");
+                return err;
+        }
+
+        err = clk_prepare_enable(csi->plld_dsi);
+        if (err) {
+                dev_err(csi->dev, "pll_d enable failed");
+                goto plld_dsi_err;
+        }
+        tegra210_csi_source_from_plld();
+        return err;
+plld_dsi_err:
+        clk_disable_unprepare(csi->plld);
+        return err;
+}
+
+static int tpg_clk_disable(struct tegra_csi_device *csi)
+{
+        int err = 0;
+
+        if (!atomic_dec_and_test(&csi->tpg_enabled))
+                return 0;
+        tegra210_csi_source_from_brick();
+        clk_disable_unprepare(csi->plld_dsi);
+        clk_disable_unprepare(csi->plld);
+
+        return err;
+}
+
+static int csi2_tpg_start_streaming(struct tegra_csi_channel *chan,
+                              enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_port *port = &chan->ports[port_num];
+
+        tpg_write(port, TEGRA_CSI_PATTERN_GENERATOR_CTRL,
+                       ((chan->pg_mode - 1) << PG_MODE_OFFSET) |
+                       PG_ENABLE);
+        tpg_write(port, TEGRA_CSI_PG_BLANK,
+                        port->v_blank << PG_VBLANK_OFFSET |
+                        port->h_blank);
+        tpg_write(port, TEGRA_CSI_PG_PHASE, 0x0);
+        tpg_write(port, TEGRA_CSI_PG_RED_FREQ,
+                       (0x10 << PG_RED_VERT_INIT_FREQ_OFFSET) |
+                       (0x10 << PG_RED_HOR_INIT_FREQ_OFFSET));
+        tpg_write(port, TEGRA_CSI_PG_RED_FREQ_RATE, 0x0);
+        tpg_write(port, TEGRA_CSI_PG_GREEN_FREQ,
+                       (0x10 << PG_GREEN_VERT_INIT_FREQ_OFFSET) |
+                       (0x10 << PG_GREEN_HOR_INIT_FREQ_OFFSET));
+        tpg_write(port, TEGRA_CSI_PG_GREEN_FREQ_RATE, 0x0);
+        tpg_write(port, TEGRA_CSI_PG_BLUE_FREQ,
+                       (0x10 << PG_BLUE_VERT_INIT_FREQ_OFFSET) |
+                       (0x10 << PG_BLUE_HOR_INIT_FREQ_OFFSET));
+        tpg_write(port, TEGRA_CSI_PG_BLUE_FREQ_RATE, 0x0);
+        return 0;
+}
+
+int csi2_start_streaming(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_port *port = &chan->ports[port_num];
+        int csi_port, csi_lanes;
+
+        csi_port = chan->ports[port_num].num;
+        csi_lanes = chan->ports[port_num].lanes;
+
+        csi_write(chan, TEGRA_CSI_CLKEN_OVERRIDE, 0, csi_port >> 1);
+
+        /* Clean up status */
+        pp_write(port, TEGRA_CSI_PIXEL_PARSER_STATUS, 0xFFFFFFFF);
+        cil_write(port, TEGRA_CSI_CIL_STATUS, 0xFFFFFFFF);
+        cil_write(port, TEGRA_CSI_CILX_STATUS, 0xFFFFFFFF);
+
+        cil_write(port, TEGRA_CSI_CIL_INTERRUPT_MASK, 0x0);
+
+        /* CIL PHY registers setup */
+        cil_write(port, TEGRA_CSI_CIL_PAD_CONFIG0, 0x0);
+        cil_write(port, TEGRA_CSI_CIL_PHY_CONTROL,
+                        BYPASS_LP_SEQ | 0xA);
+
+        /*
+         * The CSI unit provides for connection of up to six cameras in
+         * the system and is organized as three identical instances of
+         * two MIPI support blocks, each with a separate 4-lane
+         * interface that can be configured as a single camera with 4
+         * lanes or as a dual camera with 2 lanes available for each
+         * camera.
+         */
+        if (csi_lanes == 4) {
+                unsigned int cilb_offset;
+
+                cilb_offset = TEGRA_CSI_CIL_OFFSET + TEGRA_CSI_PORT_OFFSET;
+
+                cil_write(port, TEGRA_CSI_CIL_PAD_CONFIG0,
+                                BRICK_CLOCK_A_4X);
+                csi_write(chan, cilb_offset + TEGRA_CSI_CIL_PAD_CONFIG0, 0x0,
+                                csi_port >> 1);
+                csi_write(chan, cilb_offset + TEGRA_CSI_CIL_INTERRUPT_MASK, 0x0,
+                                csi_port >> 1);
+                cil_write(port, TEGRA_CSI_CIL_PHY_CONTROL,
+                                BYPASS_LP_SEQ | 0xA);
+                csi_write(chan, cilb_offset + TEGRA_CSI_CIL_PHY_CONTROL,
+                                BYPASS_LP_SEQ | 0xA, csi_port >> 1);
+                csi_write(chan, TEGRA_CSI_PHY_CIL_COMMAND,
+                                CSI_A_PHY_CIL_ENABLE | CSI_B_PHY_CIL_ENABLE,
+                                csi_port >> 1);
+        } else {
+                u32 val = csi_read(chan, TEGRA_CSI_PHY_CIL_COMMAND,
+                                        csi_port >> 1);
+
+                csi_write(chan,
+                        TEGRA_CSI_CIL_OFFSET + TEGRA_CSI_CIL_PAD_CONFIG0, 0x0,
+                        csi_port >> 1);
+                val |= ((csi_port & 0x1) == PORT_A) ? CSI_A_PHY_CIL_ENABLE :
+                        CSI_B_PHY_CIL_ENABLE;
+                csi_write(chan, TEGRA_CSI_PHY_CIL_COMMAND, val,
+                                csi_port >> 1);
+        }
+        /* CSI pixel parser registers setup */
+        pp_write(port, TEGRA_CSI_PIXEL_STREAM_PP_COMMAND,
+                        (0xF << CSI_PP_START_MARKER_FRAME_MAX_OFFSET) |
+                        CSI_PP_SINGLE_SHOT_ENABLE | CSI_PP_RST);
+        pp_write(port, TEGRA_CSI_PIXEL_PARSER_INTERRUPT_MASK, 0x0);
+        pp_write(port, TEGRA_CSI_PIXEL_STREAM_CONTROL0,
+                        CSI_PP_PACKET_HEADER_SENT |
+                        CSI_PP_DATA_IDENTIFIER_ENABLE |
+                        CSI_PP_WORD_COUNT_SELECT_HEADER |
+                        CSI_PP_CRC_CHECK_ENABLE |  CSI_PP_WC_CHECK |
+                        CSI_PP_OUTPUT_FORMAT_STORE | CSI_PPA_PAD_LINE_NOPAD |
+                        CSI_PP_HEADER_EC_DISABLE | CSI_PPA_PAD_FRAME_NOPAD |
+                        (csi_port & 1));
+        pp_write(port, TEGRA_CSI_PIXEL_STREAM_CONTROL1,
+                        (0x1 << CSI_PP_TOP_FIELD_FRAME_OFFSET) |
+                        (0x1 << CSI_PP_TOP_FIELD_FRAME_MASK_OFFSET));
+        pp_write(port, TEGRA_CSI_PIXEL_STREAM_GAP,
+                        0x14 << PP_FRAME_MIN_GAP_OFFSET);
+        pp_write(port, TEGRA_CSI_PIXEL_STREAM_EXPECTED_FRAME, 0x0);
+        pp_write(port, TEGRA_CSI_INPUT_STREAM_CONTROL,
+                        (0x3f << CSI_SKIP_PACKET_THRESHOLD_OFFSET) |
+                        (csi_lanes - 1));
+
+        if (chan->pg_mode) {
+                tpg_clk_enable(chan->csi);
+                csi2_tpg_start_streaming(chan, port_num);
+        }
+
+        pp_write(port, TEGRA_CSI_PIXEL_STREAM_PP_COMMAND,
+                        (0xF << CSI_PP_START_MARKER_FRAME_MAX_OFFSET) |
+                        CSI_PP_SINGLE_SHOT_ENABLE | CSI_PP_ENABLE);
+        return 0;
+}
+
+void csi2_stop_streaming(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_port *port = &chan->ports[port_num];
+
+
+        if (chan->pg_mode) {
+                tpg_write(port, TEGRA_CSI_PATTERN_GENERATOR_CTRL, PG_DISABLE);
+                tpg_clk_disable(chan->csi);
+        }
+        if (!port) {
+                pr_err("%s:no port\n", __func__);
+                return;
+        }
+        pp_write(port, TEGRA_CSI_PIXEL_STREAM_PP_COMMAND,
+                        (0xF << CSI_PP_START_MARKER_FRAME_MAX_OFFSET) |
+                        CSI_PP_DISABLE);
+}
+
+int csi2_hw_init(struct tegra_csi_device *csi)
+{
+        int i, csi_port;
+        struct tegra_csi_channel *it;
+        struct tegra_csi_port *port;
+
+        csi->iomem[0] = (csi->iomem_base + TEGRA_CSI_PIXEL_PARSER_0_BASE);
+        csi->iomem[1] = (csi->iomem_base + TEGRA_CSI_PIXEL_PARSER_2_BASE);
+        csi->iomem[2] = (csi->iomem_base + TEGRA_CSI_PIXEL_PARSER_4_BASE);
+        list_for_each_entry(it, &csi->csi_chans, list) {
+                for (i = 0; i < it->numports; i++) {
+                        port = &it->ports[i];
+                        csi_port = it->ports[i].num;
+                        port->pixel_parser = csi->iomem[csi_port >> 1] +
+                                (csi_port % 2) * TEGRA_CSI_PORT_OFFSET;
+                        port->cil = port->pixel_parser + TEGRA_CSI_CIL_OFFSET;
+                        port->tpg = port->pixel_parser + TEGRA_CSI_TPG_OFFSET;
+                }
+        }
+        csi->plld = devm_clk_get(csi->dev, "pll_d");
+        if (IS_ERR(csi->plld)) {
+                dev_err(csi->dev, "Fail to get pll_d\n");
+                return PTR_ERR(csi->plld);
+        }
+        csi->plld_dsi = devm_clk_get(csi->dev, "pll_d_dsi_out");
+        if (IS_ERR(csi->plld_dsi)) {
+                dev_err(csi->dev, "Fail to get pll_d_dsi_out\n");
+                return PTR_ERR(csi->plld_dsi);
+        }
+        return 0;
+}
+
+int csi2_mipi_cal(struct tegra_csi_channel *chan)
+{
+        unsigned int lanes, num_ports, val, csi_port;
+        struct tegra_csi_port *port;
+        struct tegra_csi_device *csi = chan->csi;
+
+        lanes = 0;
+        num_ports = 0;
+
+        nvhost_module_enable_clk(csi->dev);
+        while (num_ports < chan->numports) {
+                port = &chan->ports[num_ports];
+                csi_port = port->num;
+                dev_dbg(csi->dev, "Calibrate csi port %d\n", port->num);
+
+                if (chan->numlanes == 2) {
+                        lanes |= CSIA << csi_port;
+                        val = csi_read(chan, TEGRA_CSI_PHY_CIL_COMMAND,
+                                        csi_port >> 1);
+                        csi_write(chan,
+                                TEGRA_CSI_CIL_OFFSET +
+                                TEGRA_CSI_CIL_PAD_CONFIG0, 0x0, csi_port >> 1);
+                        val |= ((csi_port & 0x1) == PORT_A) ?
+                                CSI_A_PHY_CIL_ENABLE : CSI_B_PHY_CIL_ENABLE;
+                        csi_write(chan, TEGRA_CSI_PHY_CIL_COMMAND, val,
+                                csi_port >> 1);
+                } else {
+                        lanes |= (CSIA | CSIB) << port->num;
+                        csi_write(chan, TEGRA_CSI_PHY_CIL_COMMAND,
+                                CSI_A_PHY_CIL_ENABLE | CSI_B_PHY_CIL_ENABLE,
+                                csi_port >> 1);
+                }
+                num_ports++;
+        }
+        if (!lanes) {
+                dev_err(csi->dev,
+                        "Selected no CSI lane, cannot do calibration");
+                return -EINVAL;
+        }
+        nvhost_module_disable_clk(csi->dev);
+        return tegra_mipi_calibration(lanes);
+}
+
+int csi2_power_on(struct tegra_csi_device *csi)
+{
+        return 0;
+}
+int csi2_power_off(struct tegra_csi_device *csi)
+{
+        return 0;
+}
+const struct tegra_csi_fops csi2_fops = {
+        .csi_power_on = csi2_power_on,
+        .csi_power_off = csi2_power_off,
+        .csi_start_streaming = csi2_start_streaming,
+        .csi_stop_streaming = csi2_stop_streaming,
+        .mipical = csi2_mipi_cal,
+        .hw_init = csi2_hw_init,
+};
+EXPORT_SYMBOL(csi2_fops);
diff --git a/drivers/media/platform/tegra/camera/csi/csi2_fops.h b/drivers/media/platform/tegra/camera/csi/csi2_fops.h
new file mode 100644
index 000000000..7b801e777
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/csi/csi2_fops.h
@@ -0,0 +1,27 @@
+/*
+ * Tegra CSI2 device common APIs
+ *
+ * Tegra Graphics Host VI
+ *
+ * Copyright (c) 2016, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __CSI2_H__
+#define __CSI2_H__
+
+#include "csi.h"
+
+int csi2_start_streaming(struct tegra_csi_channel *chan,
+                enum tegra_csi_port_num port_num);
+void csi2_stop_streaming(struct tegra_csi_channel *chan,
+                enum tegra_csi_port_num port_num);
+
+extern struct tegra_csi_fops csi2_fops;
+
+#endif
diff --git a/drivers/media/platform/tegra/camera/csi/csi4_fops.c b/drivers/media/platform/tegra/camera/csi/csi4_fops.c
new file mode 100644
index 000000000..6fd28156e
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/csi/csi4_fops.c
@@ -0,0 +1,506 @@
+/*
+ * Tegra CSI4 device common APIs
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Frank Chen <frankc@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/clk/tegra.h>
+#include "nvhost_acm.h"
+#include "camera/csi/csi.h"
+#include "camera/csi/csi4_registers.h"
+#include "camera/vi/core.h"
+#include "mipical/mipi_cal.h"
+#include "nvcsi/nvcsi.h"
+
+static void csi4_stream_write(struct tegra_csi_channel *chan,
+                unsigned int index, unsigned int addr, u32 val)
+{
+        struct tegra_csi_device *csi = chan->csi;
+        u32 cilb_offset = (index & 0x1) ? CSI4_STREAM_OFFSET : 0x0;
+
+        writel(val, csi->iomem[index >> 1] + cilb_offset + addr);
+}
+
+static u32 csi4_stream_read(struct tegra_csi_channel *chan,
+                unsigned int index, unsigned int addr)
+{
+        struct tegra_csi_device *csi = chan->csi;
+        u32 cilb_offset = (index & 0x1) ? CSI4_STREAM_OFFSET : 0x0;
+
+        return readl(csi->iomem[index >> 1] + cilb_offset + addr);
+}
+
+static void csi4_phy_write(struct tegra_csi_channel *chan,
+                unsigned int index, unsigned int addr, u32 val)
+{
+        struct tegra_csi_device *csi = chan->csi;
+
+        writel(val, csi->iomem_base +
+                CSI4_BASE_ADDRESS + (CSI4_PHY_OFFSET * index) + addr);
+}
+
+static u32 csi4_phy_read(struct tegra_csi_channel *chan,
+                unsigned int index, unsigned int addr)
+{
+        struct tegra_csi_device *csi = chan->csi;
+
+        return readl(csi->iomem_base +
+                CSI4_BASE_ADDRESS + (CSI4_PHY_OFFSET * index) + addr);
+}
+
+static void csi4_stream_init(struct tegra_csi_channel *chan, int port_num)
+{
+        struct tegra_csi_device *csi = chan->csi;
+
+        dev_dbg(csi->dev, "%s\n", __func__);
+
+        csi4_stream_write(chan, port_num, CIL_INTR_STATUS, 0xffffffff);
+        csi4_stream_write(chan, port_num, CIL_ERR_INTR_STATUS, 0xffffffff);
+        csi4_stream_write(chan, port_num, CIL_INTR_MASK, 0xffffffff);
+        csi4_stream_write(chan, port_num, CIL_ERR_INTR_MASK, 0xffffffff);
+        csi4_stream_write(chan, port_num, INTR_STATUS, 0x3ffff);
+        csi4_stream_write(chan, port_num, ERR_INTR_STATUS, 0x7ffff);
+        csi4_stream_write(chan, port_num, ERROR_STATUS2VI_MASK, 0x0);
+        csi4_stream_write(chan, port_num, INTR_MASK, 0x0);
+        csi4_stream_write(chan, port_num, ERR_INTR_MASK, 0x0);
+}
+
+static void csi4_stream_config(struct tegra_csi_channel *chan, int port_num)
+{
+        struct tegra_csi_device *csi = chan->csi;
+        int val;
+
+        dev_dbg(csi->dev, "%s\n", __func__);
+
+        csi4_stream_write(chan, port_num, PPFSM_TIMEOUT_CTRL, 0);
+        csi4_stream_write(chan, port_num, PH_CHK_CTRL,
+                        CFG_PH_CRC_CHK_EN | CFG_PH_ECC_CHK_EN);
+        csi4_stream_write(chan, port_num, VC0_DPCM_CTRL, 0);
+        csi4_stream_write(chan, port_num, VC0_DT_OVERRIDE, 0);
+
+        val = csi4_stream_read(chan, port_num, VC0_DPCM_CTRL);
+        dev_dbg(csi->dev, "%s (%d) read VC0_DPCM_CTRL = %08x\n",
+                        __func__, port_num, val);
+}
+
+
+static void csi4_phy_config(
+        struct tegra_csi_channel *chan, int csi_port,
+        int csi_lanes, bool enable)
+{
+        struct tegra_csi_device *csi = chan->csi;
+        int phy_num = (csi_port & 0x6) >> 1;
+        bool cil_a = (csi_port & 0x1) ? false : true;
+        int cil_config;
+
+        dev_dbg(csi->dev, "%s\n", __func__);
+
+        /* set to DPHY */
+        csi4_phy_write(chan, phy_num, NVCSI_CIL_PHY_CTRL, DPHY);
+
+        /* read current NVCSI_CIL_CONFIG setting */
+        cil_config = csi4_phy_read(chan, phy_num, NVCSI_CIL_CONFIG);
+        dev_dbg(csi->dev, "NVCSI_CIL_CONFIG = %08x\n", cil_config);
+
+        if (cil_a) {
+                /* soft reset for data lane */
+                csi4_phy_write(chan, phy_num, NVCSI_CIL_A_SW_RESET,
+                        SW_RESET1_EN | SW_RESET0_EN);
+                /* reset CSI lane number */
+                csi4_phy_write(chan, phy_num, NVCSI_CIL_CONFIG,
+                        cil_config & ~DATA_LANE_A);
+                /* disable clock lane*/
+                csi4_phy_write(chan, phy_num,
+                        NVCSI_CIL_A_PAD_CONFIG,
+                        PD_CLK | PD_IO0 | PD_IO1 | SPARE_IO0 | SPARE_IO1);
+
+                /* setting up CIL B for 4 lane */
+                if (csi_lanes > 2) {
+                        /* soft reset for data lane */
+                        csi4_phy_write(chan, phy_num, NVCSI_CIL_B_SW_RESET,
+                                SW_RESET1_EN | SW_RESET0_EN);
+                        /* reset CSI lane number */
+                        csi4_phy_write(chan, phy_num, NVCSI_CIL_CONFIG,
+                                cil_config & ~DATA_LANE_B);
+                        /* disable clock lane*/
+                        csi4_phy_write(chan, phy_num,
+                                NVCSI_CIL_B_PAD_CONFIG,
+                                PD_CLK | PD_IO0 | PD_IO1 |
+                                SPARE_IO0 | SPARE_IO1);
+                }
+
+                /* power down de-serializer is CIL B is not in use*/
+                if ((cil_config & DATA_LANE_B) == 0)
+                        csi4_phy_write(chan, phy_num,
+                                NVCSI_CIL_PAD_CONFIG, PDVCLAMP);
+        } else {
+                /* soft reset for data lane */
+                csi4_phy_write(chan, phy_num, NVCSI_CIL_B_SW_RESET,
+                        SW_RESET1_EN | SW_RESET0_EN);
+                /* reset CSI lane number */
+                csi4_phy_write(chan, phy_num, NVCSI_CIL_CONFIG,
+                        cil_config & ~DATA_LANE_B);
+                /* disable clock lane*/
+                csi4_phy_write(chan, phy_num,
+                        NVCSI_CIL_B_PAD_CONFIG,
+                        PD_CLK | PD_IO0 | PD_IO1 | SPARE_IO0 | SPARE_IO1);
+
+                /* power down de-serializer if CIL A is not in use*/
+                if ((cil_config & DATA_LANE_A) == 0)
+                        csi4_phy_write(chan, phy_num,
+                                NVCSI_CIL_PAD_CONFIG, PDVCLAMP);
+        }
+
+        if (!enable)
+                return;
+
+        /* power on de-serializer */
+        csi4_phy_write(chan, phy_num, NVCSI_CIL_PAD_CONFIG, 0);
+
+        if (cil_a) {
+                /* set CSI lane number */
+                csi4_phy_write(chan, phy_num, NVCSI_CIL_CONFIG,
+                        (cil_config & ~DATA_LANE_A) |
+                        (csi_lanes << DATA_LANE_A_OFFSET));
+                /* enable clock lane*/
+                csi4_phy_write(chan, phy_num,
+                        NVCSI_CIL_A_PAD_CONFIG,
+                        E_INPUT_LP_CLK | E_INPUT_LP_IO0 | E_INPUT_LP_IO1);
+                /* setup settle time */
+                csi4_phy_write(chan, phy_num,
+                        NVCSI_CIL_A_CONTROL,
+                        DEFAULT_DESKEW_COMPARE | DEFAULT_DESKEW_SETTLE |
+                        DEFAULT_CLK_SETTLE |
+                        T18X_BYPASS_LP_SEQ | DEFAULT_THS_SETTLE);
+                /* release soft reset */
+                csi4_phy_write(chan, phy_num, NVCSI_CIL_A_SW_RESET, 0x0);
+
+                /* setting up CIL B for 4 lane */
+                if (csi_lanes > 2) {
+                        /* set CSI lane number */
+                        csi4_phy_write(chan, phy_num, NVCSI_CIL_CONFIG,
+                                csi_lanes << DATA_LANE_A_OFFSET);
+                        /* enable clock lane*/
+                        csi4_phy_write(chan, phy_num,
+                                NVCSI_CIL_B_PAD_CONFIG,
+                                E_INPUT_LP_IO0 | E_INPUT_LP_IO1 | PD_CLK);
+                        /* setup settle time */
+                        csi4_phy_write(chan, phy_num,
+                                NVCSI_CIL_B_CONTROL,
+                                DEFAULT_DESKEW_COMPARE | DEFAULT_DESKEW_SETTLE
+                                | DEFAULT_CLK_SETTLE |
+                                T18X_BYPASS_LP_SEQ | DEFAULT_THS_SETTLE);
+                        /* release soft reset */
+                        csi4_phy_write(chan, phy_num,
+                                NVCSI_CIL_B_SW_RESET, 0x0);
+                }
+        } else {
+                /* set CSI lane number */
+                csi4_phy_write(chan, phy_num, NVCSI_CIL_CONFIG,
+                        (cil_config & ~DATA_LANE_B) |
+                        (csi_lanes << DATA_LANE_B_OFFSET));
+                /* enable clock lane*/
+                csi4_phy_write(chan, phy_num,
+                        NVCSI_CIL_B_PAD_CONFIG,
+                        E_INPUT_LP_CLK | E_INPUT_LP_IO0 | E_INPUT_LP_IO1);
+                /* setup settle time */
+                csi4_phy_write(chan, phy_num,
+                        NVCSI_CIL_B_CONTROL,
+                        DEFAULT_DESKEW_COMPARE | DEFAULT_DESKEW_SETTLE |
+                        DEFAULT_CLK_SETTLE |
+                        T18X_BYPASS_LP_SEQ | DEFAULT_THS_SETTLE);
+                /* release soft reset */
+                csi4_phy_write(chan, phy_num, NVCSI_CIL_B_SW_RESET, 0x0);
+        }
+}
+
+static void csi4_stream_check_status(
+        struct tegra_csi_channel *chan, int port_num)
+{
+        struct tegra_csi_device *csi = chan->csi;
+        int status = 0;
+
+        dev_dbg(csi->dev, "%s\n", __func__);
+        if (!chan->pg_mode) {
+                status = csi4_stream_read(chan, port_num, ERROR_STATUS2VI_VC0);
+                if (status)
+                        dev_err(csi->dev,
+                                "%s (%d) ERROR_STATUS2VI_VC0 = 0x%08x\n",
+                                __func__, port_num, status);
+
+                status = csi4_stream_read(chan, port_num, ERROR_STATUS2VI_VC1);
+                if (status)
+                        dev_err(csi->dev,
+                                "%s (%d) ERROR_STATUS2VI_VC1 = 0x%08x\n",
+                                __func__, port_num, status);
+
+                status = csi4_stream_read(chan, port_num, ERROR_STATUS2VI_VC2);
+                if (status)
+                        dev_err(csi->dev,
+                                "%s (%d) ERROR_STATUS2VI_VC2 = 0x%08x\n",
+                                __func__, port_num, status);
+
+                status = csi4_stream_read(chan, port_num, ERROR_STATUS2VI_VC3);
+                if (status)
+                        dev_err(csi->dev,
+                                "%s (%d) ERROR_STATUS2VI_VC2 = 0x%08x\n",
+                                __func__, port_num, status);
+        }
+        status = csi4_stream_read(chan, port_num, INTR_STATUS);
+        if (status)
+                dev_err(csi->dev,
+                                "%s (%d) INTR_STATUS 0x%08x\n",
+                                __func__, port_num, status);
+
+        status = csi4_stream_read(chan, port_num, ERR_INTR_STATUS);
+        if (status)
+                dev_err(csi->dev,
+                                "%s (%d) ERR_INTR_STATUS 0x%08x\n",
+                                __func__, port_num, status);
+}
+
+static void csi4_cil_check_status(struct tegra_csi_channel *chan, int port_num)
+{
+        struct tegra_csi_device *csi = chan->csi;
+        int status = 0;
+
+        dev_dbg(csi->dev, "%s %d\n", __func__, __LINE__);
+
+        status = csi4_stream_read(chan, port_num, CIL_INTR_STATUS);
+        if (status)
+                dev_err(csi->dev,
+                        "%s (%d) CIL_INTR_STATUS 0x%08x\n",
+                        __func__, port_num, status);
+
+        status = csi4_stream_read(chan, port_num, CIL_ERR_INTR_STATUS);
+        if (status)
+                dev_err(csi->dev,
+                        "%s (%d) CIL_ERR_INTR_STATUS 0x%08x\n",
+                        __func__, port_num, status);
+}
+
+
+int csi4_power_on(struct tegra_csi_device *csi)
+{
+        int err = 0;
+
+        err = nvhost_module_busy(csi->pdev);
+        if (err)
+                dev_err(csi->dev, "%s:cannot enable csi\n", __func__);
+
+        return err;
+}
+
+int csi4_power_off(struct tegra_csi_device *csi)
+{
+        nvhost_module_idle(csi->pdev);
+
+        return 0;
+}
+
+static void csi4_tpg_stop_streaming(struct tegra_csi_channel *chan,
+                                int ports_index)
+{
+        unsigned int csi_port = chan->ports[ports_index].num;
+        struct tegra_csi_device *csi = chan->csi;
+
+        dev_dbg(csi->dev, "%s\n", __func__);
+        csi4_stream_check_status(chan, csi_port);
+        csi4_cil_check_status(chan, csi_port);
+        csi4_stream_write(chan, csi_port, PP_EN_CTRL, 0);
+        csi4_stream_write(chan, csi_port, TPG_EN_0, 0);
+        csi4_stream_write(chan, csi_port, PG_CTRL, PG_DISABLE);
+}
+static int csi4_tpg_start_streaming(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_port *port = &chan->ports[port_num];
+        struct tegra_csi_device *csi = chan->csi;
+        unsigned int val, csi_port, csi_lanes;
+
+        if (!port->core_format) {
+                dev_err(csi->dev, "Fail to find tegra video fmt");
+                return -EINVAL;
+        }
+
+        csi_port = port->num;
+        csi_lanes = port->lanes;
+        dev_dbg(csi->dev, "%s CSI port=%d, # lanes=%d\n",
+                        __func__, csi_port, csi_lanes);
+
+        csi4_stream_write(chan, csi_port, PH_CHK_CTRL, 0);
+        csi4_stream_write(chan, csi_port, INTR_MASK, PH_ECC_MULTI_BIT_ERR |
+                        PD_CRC_ERR_VC0 | PH_ECC_SINGLE_BIT_ERR_VC0);
+        csi4_stream_write(chan, csi_port, ERR_INTR_MASK, PH_ECC_MULTI_BIT_ERR |
+                        PD_CRC_ERR_VC0 | PH_ECC_SINGLE_BIT_ERR_VC0);
+        csi4_stream_write(chan, csi_port, ERROR_STATUS2VI_MASK,
+                        CFG_ERR_STATUS2VI_MASK_VC0 |
+                        CFG_ERR_STATUS2VI_MASK_VC1 |
+                        CFG_ERR_STATUS2VI_MASK_VC2 |
+                        CFG_ERR_STATUS2VI_MASK_VC3);
+        /* calculate PG blank */
+        csi4_stream_write(chan, csi_port, PG_BLANK,
+                        ((port->v_blank & PG_VBLANK_MASK) << PG_VBLANK_OFFSET) |
+                        ((port->h_blank & PG_HBLANK_MASK) << PG_HBLANK_OFFSET));
+        csi4_stream_write(chan, csi_port, PG_PHASE, 0x0);
+        csi4_stream_write(chan, csi_port, PG_RED_FREQ,
+                        (0x10 << PG_VERT_INIT_FREQ_OFFSET)|
+                        (0x10 << PG_HOR_INIT_FREQ_OFFSET));
+        csi4_stream_write(chan, csi_port, PG_RED_FREQ_RATE, 0x0);
+        csi4_stream_write(chan, csi_port, PG_GREEN_FREQ,
+                        (0x10 << PG_VERT_INIT_FREQ_OFFSET)|
+                        (0x10 << PG_HOR_INIT_FREQ_OFFSET));
+        csi4_stream_write(chan, csi_port, PG_GREEN_FREQ_RATE, 0x0);
+        csi4_stream_write(chan, csi_port, PG_BLUE_FREQ,
+                        (0x10 << PG_VERT_INIT_FREQ_OFFSET)|
+                        (0x10 << PG_HOR_INIT_FREQ_OFFSET));
+        csi4_stream_write(chan, csi_port, PG_BLUE_FREQ_RATE, 0x0);
+        /* calculate PG IMAGE SIZE and DT */
+        mutex_lock(&chan->format_lock);
+        val = port->format.height << HEIGHT_OFFSET |
+                (port->format.width *
+                (port->core_format->vf_code == TEGRA_VF_RAW10 ? 10 : 24) / 8);
+        mutex_unlock(&chan->format_lock);
+        csi4_stream_write(chan, csi_port, PG_IMAGE_SIZE, val);
+        csi4_stream_write(chan, csi_port, PG_IMAGE_DT,
+                        port->core_format->img_dt);
+        csi4_stream_write(chan, csi_port, PP_EN_CTRL, CFG_PP_EN);
+        csi4_stream_write(chan, csi_port, TPG_EN_0, cfg_tpg_en);
+
+        csi4_stream_write(chan, csi_port, PG_CTRL,
+                        ((chan->pg_mode - 1) << PG_MODE_OFFSET) | PG_ENABLE);
+        return 0;
+}
+int csi4_hw_init(struct tegra_csi_device *csi)
+{
+        csi->iomem[0] = csi->iomem_base + TEGRA_CSI_STREAM_0_BASE;
+        csi->iomem[1] = csi->iomem_base + TEGRA_CSI_STREAM_2_BASE;
+        csi->iomem[2] = csi->iomem_base + TEGRA_CSI_STREAM_4_BASE;
+
+        return 0;
+}
+int csi4_start_streaming(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_device *csi = chan->csi;
+        int csi_port, csi_lanes, ret = 0;
+
+        csi_port = chan->ports[port_num].num;
+        csi_lanes = chan->ports[port_num].lanes;
+        dev_dbg(csi->dev, "%s ports index=%d, lanes=%d\n",
+                        __func__, csi_port, csi_lanes);
+
+        if (chan->pg_mode)
+                ret = csi4_tpg_start_streaming(chan, port_num);
+        else {
+                csi4_stream_init(chan, csi_port);
+                csi4_stream_config(chan, csi_port);
+                /* enable DPHY */
+                csi4_phy_config(chan, csi_port, csi_lanes, true);
+                csi4_stream_write(chan, csi_port, PP_EN_CTRL, CFG_PP_EN);
+        }
+        return ret;
+}
+
+void csi4_stop_streaming(struct tegra_csi_channel *chan,
+                                enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_device *csi = chan->csi;
+        int csi_port, csi_lanes;
+
+        dev_dbg(csi->dev, "%s ports index=%d, lanes=%d\n",
+                        __func__, port_num, chan->numlanes);
+
+        csi_port = chan->ports[port_num].num;
+        csi_lanes = chan->ports[port_num].lanes;
+
+        if (chan->pg_mode)
+                csi4_tpg_stop_streaming(chan, port_num);
+        else {
+                /* disable DPHY */
+                csi4_phy_config(chan, csi_port, csi_lanes, false);
+                csi4_stream_check_status(chan, csi_port);
+                csi4_cil_check_status(chan, csi_port);
+        }
+}
+
+void csi4_override_format(struct tegra_csi_channel *chan,
+                enum tegra_csi_port_num port_num)
+{
+        struct tegra_csi_port *port = &chan->ports[port_num];
+        unsigned int val;
+        int csi_port;
+
+        if (!chan->pg_mode) {
+                dev_err(chan->csi->dev, "%s non PG format update failed\n",
+                                __func__);
+                return;
+        }
+
+        /* calculate PG IMAGE SIZE and DT */
+        mutex_lock(&chan->format_lock);
+        val = port->format.height << HEIGHT_OFFSET |
+                (port->format.width *
+                (port->core_format->vf_code == TEGRA_VF_RAW10 ? 10 : 24) / 8);
+        mutex_unlock(&chan->format_lock);
+
+        csi_port = chan->ports[port_num].num;
+        csi4_stream_write(chan, csi_port, PG_IMAGE_SIZE, val);
+}
+
+int csi4_mipi_cal(struct tegra_csi_channel *chan)
+{
+        unsigned int lanes, num_ports, port, addr;
+        unsigned int cila, cilb;
+        struct tegra_csi_device *csi = chan->csi;
+
+        lanes = 0;
+        num_ports = 0;
+        port = 0;
+        while (num_ports < chan->numports) {
+                port = chan->ports[num_ports].num;
+                dev_dbg(csi->dev, "csi port:%d\n", port);
+
+                if (chan->numlanes == 2) {
+                        lanes |= CSIA << port;
+                        cila =  (0x01 << E_INPUT_LP_IO0_SHIFT) |
+                                (0x01 << E_INPUT_LP_IO1_SHIFT) |
+                                (0x01 << E_INPUT_LP_CLK_SHIFT) |
+                                (0x00 << PD_CLK_SHIFT) |
+                                (0x00 << PD_IO0_SHIFT) |
+                                (0x00 << PD_IO1_SHIFT);
+                        addr = (port % 2 == 0 ?
+                                NVCSI_CIL_A_BASE : NVCSI_CIL_B_BASE)
+                                + PAD_CONFIG_0;
+                        csi4_phy_write(chan, port >> 1, addr, cila);
+                } else {
+                        lanes |= (CSIA | CSIB) << port;
+                        cila =  (0x01 << E_INPUT_LP_IO0_SHIFT) |
+                                (0x01 << E_INPUT_LP_IO1_SHIFT) |
+                                (0x01 << E_INPUT_LP_CLK_SHIFT) |
+                                (0x00 << PD_CLK_SHIFT) |
+                                (0x00 << PD_IO0_SHIFT) |
+                                (0x00 << PD_IO1_SHIFT);
+                        cilb =  (0x01 << E_INPUT_LP_IO0_SHIFT) |
+                                (0x01 << E_INPUT_LP_IO1_SHIFT) |
+                                (0x01 << PD_CLK_SHIFT) |
+                                (0x00 << PD_IO0_SHIFT) |
+                                (0x00 << PD_IO1_SHIFT);
+                        csi4_phy_write(chan, port >> 1,
+                                NVCSI_CIL_A_BASE + PAD_CONFIG_0, cila);
+                        csi4_phy_write(chan, port >> 1,
+                                NVCSI_CIL_B_BASE + PAD_CONFIG_0, cilb);
+                }
+                num_ports++;
+        }
+        if (!lanes) {
+                dev_err(csi->dev, "Selected no CSI lane, cannot do calibration");
+                return -EINVAL;
+        }
+        return tegra_mipi_calibration(lanes);
+}
diff --git a/drivers/media/platform/tegra/camera/csi/csi4_fops.h b/drivers/media/platform/tegra/camera/csi/csi4_fops.h
new file mode 100644
index 000000000..49f0aa8ad
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/csi/csi4_fops.h
@@ -0,0 +1,41 @@
+/*
+ * Tegra CSI4 device common APIs
+ *
+ * Tegra Graphics Host VI
+ *
+ * Copyright (c) 2016, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Frank Chen <frankc@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __CSI4_H__
+#define __CSI4_H__
+
+#include "csi.h"
+
+int csi4_power_on(struct tegra_csi_device *csi);
+int csi4_power_off(struct tegra_csi_device *csi);
+int csi4_start_streaming(struct tegra_csi_channel *chacsin,
+                enum tegra_csi_port_num port_num);
+void csi4_stop_streaming(struct tegra_csi_channel *chan,
+                enum tegra_csi_port_num port_num);
+void csi4_override_format(struct tegra_csi_channel *chan,
+                enum tegra_csi_port_num port_num);
+int csi4_mipi_cal(struct tegra_csi_channel *chan);
+int csi4_hw_init(struct tegra_csi_device *csi);
+
+struct tegra_csi_fops csi4_fops = {
+        .csi_power_on = csi4_power_on,
+        .csi_power_off = csi4_power_off,
+        .csi_start_streaming = csi4_start_streaming,
+        .csi_stop_streaming = csi4_stop_streaming,
+        .csi_override_format = csi4_override_format,
+        .mipical = csi4_mipi_cal,
+        .hw_init = csi4_hw_init,
+};
+
+#endif
diff --git a/drivers/media/platform/tegra/camera/csi/csi4_registers.h b/drivers/media/platform/tegra/camera/csi/csi4_registers.h
new file mode 100644
index 000000000..ed4ef936c
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/csi/csi4_registers.h
@@ -0,0 +1,196 @@
+/*
+ * drivers/media/platform/tegra/camera/csi/csi4_registers.h
+ *
+ * Tegra 18x CSI register offsets
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __CSI4_REGISTERS_H__
+#define __CSI4_REGISTERS_H__
+
+#define CSI4_BASE_ADDRESS               0x18000
+#define CSI4_PHY_OFFSET                 0x10000
+#define CSI4_STREAM_OFFSET              0x800
+
+#define CSI_PORTS       (6)
+#define PHY_BRICKS      (3)
+
+/* NVCSI registers. Starts from 0x0 */
+#define CFG_NVCSI_INCR_SYNCPT_CNTRL     0x04
+
+/* NVCSI_STREAM registers */
+#define TEGRA_CSI_STREAM_0_BASE                 0x010000
+#define TEGRA_CSI_STREAM_1_BASE                 0x010800
+#define TEGRA_CSI_STREAM_2_BASE                 0x020000
+#define TEGRA_CSI_STREAM_3_BASE                 0x020800
+#define TEGRA_CSI_STREAM_4_BASE                 0x030000
+#define TEGRA_CSI_STREAM_5_BASE                 0x030800
+
+#define PP_EN_CTRL                                      0x08
+#define CFG_PP_EN                                       (0x1 << 0)
+
+#define PPFSM_TIMEOUT_CTRL                      0x6c
+#define CFG_TIMEOUT_EN                          (0x1 << 31)
+#define CFG_TIMEOUT_PERIOD                      (0x7fffffff << 0)
+
+#define VC0_DT_OVERRIDE                         0x20
+#define CFG_VC0_DT_OVERRIDE_EN          (0x1 << 31)
+#define CFG_VC0_DT_OVERRIDE                     (0x3f << 0)
+
+#define PH_CHK_CTRL                                     0x70
+#define CFG_PH_CRC_CHK_EN                       (0x1 << 1)
+#define CFG_PH_ECC_CHK_EN                       (0x1 << 0)
+
+#define VC0_DPCM_CTRL                                   0x74
+#define CFG_VC0_DPCM_COMPRESSION_RATIO (0xf << 0)
+
+#define ERROR_STATUS2VI_MASK            0x90
+
+/* T186 TPG */
+#define TPG_EN_0                        0x0b8
+#define cfg_tpg_en                      0x1
+/* NVCSI_STREAM Legacy T210 PG*/
+#define PG_CTRL                         0x194
+#define         PG_MODE_OFFSET          2
+#define         PG_ENABLE               0x1
+#define         PG_DISABLE              0x0
+#define PG_BLANK                        0x198
+#define         PG_VBLANK_MASK          0xffff
+#define         PG_HBLANK_MASK          0xffff
+#define         PG_VBLANK_OFFSET        16
+#define         PG_HBLANK_OFFSET        0
+#define PG_PHASE                        0x19c
+#define PG_RED_FREQ                     0x1a0
+#define         PG_VERT_INIT_FREQ_OFFSET 16
+#define         PG_HOR_INIT_FREQ_OFFSET 0
+#define PG_RED_FREQ_RATE                0x1a4
+#define PG_GREEN_FREQ                   0x1a8
+#define PG_GREEN_FREQ_RATE              0x1ac
+#define PG_BLUE_FREQ                    0x1b0
+#define PG_BLUE_FREQ_RATE               0X1b4
+#define PG_AOHDR                        0x1b8
+#define PG_IMAGE_SIZE                   0x1bc
+#define         HEIGHT_OFFSET           16
+#define PG_IMAGE_DT                     0x1c0
+
+/* TODO - double check if rr_status2vi_vc0:[0] means bit or value */
+#define ERROR_STATUS2VI_VC0                     0x94
+#define ERROR_STATUS2VI_VC1                     0x98
+#define ERROR_STATUS2VI_VC2                     0x9c
+#define ERROR_STATUS2VI_VC3                     0xa0
+#define ERR_STATUS2VI_VC                        (0xf << 0)
+#define ERR_PP_FSM_TIMEOUT                      (0)
+#define ERR_PH_ECC_SINGLE_BIT           (1)
+#define ERR_PACKET_PAYLOAD_CRC          (2)
+#define ERR_PACKET_PAYLOAD_LESS         (3)
+
+#define INTR_STATUS                                     0xa4
+#define INTR_MASK                                       0xa8
+#define PD_CRC_ERR_VC0                          (0x1 << 2)
+#define PH_ECC_SINGLE_BIT_ERR_VC0               (0x1 << 1)
+#define PH_ECC_MULTI_BIT_ERR                    (0x1 << 16)
+#define ERR_INTR_STATUS                         0xac
+#define ERR_INTR_MASK                           0xb0
+#define MASK_PH_CRC_ERR                                 (0x1 << 17)
+#define MASK_PH_ECC_MULTI_BIT_ERR               (0x1 << 16)
+#define MASK_PD_WC_SHORT_ERR_VC3                (0x1 << 15)
+#define MASK_PD_CRC_ERR_VC3                             (0x1 << 14)
+#define MASK_PH_ECC_SINGLE_BIT_ERR_VC3  (0x1 << 13)
+#define MASK_PPFSM_TIMEOUT_VC3                  (0x1 << 12)
+#define MASK_PD_WC_SHORT_ERR_VC2                (0x1 << 11)
+#define MASK_PD_CRC_ERR_VC2                             (0x1 << 10)
+#define MASK_PH_ECC_SINGLE_BIT_ERR_VC2  (0x1 << 9)
+#define MASK_PPFSM_TIMEOUT_VC2                  (0x1 << 8)
+#define MASK_PD_WC_SHORT_ERR_VC1                (0x1 << 7)
+#define MASK_PD_CRC_ERR_VC1                             (0x1 << 6)
+#define MASK_PH_ECC_SINGLE_BIT_ERR_VC1  (0x1 << 5)
+#define MASK_PPFSM_TIMEOUT_VC1                  (0x1 << 4)
+#define MASK_PD_WC_SHORT_ERR_VC0                (0x1 << 3)
+#define MASK_PD_CRC_ERR_VC0                             (0x1 << 2)
+#define MASK_PH_ECC_SINGLE_BIT_ERR_VC0  (0x1 << 1)
+#define MASK_PPFSM_TIMEOUT_VC0                  (0x1 << 0)
+/* For ERR_INTR_MASK and ERR_INTR_MASK */
+#define MASK_HSM_INTR_SW_TRIGGER                (0x1 << 18)
+
+/* NVCSI_PHY CIL registers */
+#define NVCSI_PHY_0_CILA_BASE                   0x010400
+#define NVCSI_PHY_0_CILB_BASE                   0x010C00
+#define NVCSI_PHY_1_CILA_BASE                   0x020400
+#define NVCSI_PHY_1_CILB_BASE                   0x020C00
+#define NVCSI_PHY_2_CILA_BASE                   0x030400
+#define NVCSI_PHY_2_CILB_BASE                   0x030C00
+
+#define CIL_INTR_STATUS                                 0x400
+#define CIL_INTR_MASK                                   0x404
+#define CIL_ERR_INTR_STATUS                             0x408
+#define CIL_ERR_INTR_MASK                               0x40c
+
+/* NVCSI_PHY registers */
+#define NVCSI_CIL_PHY_CTRL                      0x00
+#define CFG_PHY_MODE                            (0x1 << 0)
+#define DPHY                                            (0)
+#define CPHY                                            (1)
+
+#define NVCSI_CIL_CONFIG                        0x04
+#define DATA_LANE_B_OFFSET                      0x8
+#define DATA_LANE_A_OFFSET                      0x0
+#define DATA_LANE_B                     (0x7 << DATA_LANE_B_OFFSET)
+#define DATA_LANE_A                     (0x7 << DATA_LANE_A_OFFSET)
+
+#define NVCSI_CIL_PAD_CONFIG            0x0c
+#define LOADADJ                                         (0xf << 12)
+#define PDVCLAMP                                        (0x1 << 9)
+#define E_VCLAMP                                        (0x1 << 8)
+#define SPARE_TOP                                       (0xff << 0)
+
+#define NVCSI_CIL_A_SW_RESET            0x18
+#define NVCSI_CIL_B_SW_RESET            0x7c
+#define SW_RESET1_EN                            (0x1 << 1)
+#define SW_RESET0_EN                            (0x1 << 0)
+
+#define NVCSI_CIL_A_PAD_CONFIG          0x20
+#define NVCSI_CIL_B_PAD_CONFIG          0x84
+#define E_INPUT_LP_IO1_SHIFT            22
+#define E_INPUT_LP_IO0_SHIFT            21
+#define E_INPUT_LP_CLK_SHIFT            20
+#define E_INPUT_LP_IO1                          (0x1 << 22)
+#define E_INPUT_LP_IO0                          (0x1 << 21)
+#define E_INPUT_LP_CLK                          (0x1 << 20)
+#define BANDWD_IN                                       (0x1 << 19)
+#define PD_CLK                                          (0x1 << 18)
+#define PD_IO1                                          (0x1 << 17)
+#define PD_IO0                                          (0x1 << 16)
+#define PD_CLK_SHIFT                            18
+#define PD_IO1_SHIFT                            17
+#define PD_IO0_SHIFT                            16
+#define SPARE_CLK                                       (0x1 << 8)
+#define SPARE_IO1                                       (0x1 << 4)
+#define SPARE_IO0                                       (0x1 << 0)
+
+#define NVCSI_CIL_A_CONTROL                     0x5c
+#define NVCSI_CIL_B_CONTROL                     0xc0
+#define DEFAULT_DESKEW_COMPARE                          (0x4 << 20)
+#define DEFAULT_DESKEW_SETTLE                           (0x6 << 16)
+#define DEFAULT_CLK_SETTLE                                      (0x21 << 8)
+#define T18X_BYPASS_LP_SEQ                      (0x1 << 7)
+#define DEFAULT_THS_SETTLE                                      (0x14 << 0)
+
+/* MIPICAL */
+#define NVCSI_CIL_A_BASE            0x18
+#define NVCSI_CIL_B_BASE                        0x7c
+#define PAD_CONFIG_0                            0x8
+
+#endif /* __CSI4_REGISTERS_H__ */
diff --git a/drivers/media/platform/tegra/camera/sensor_common.c b/drivers/media/platform/tegra/camera/sensor_common.c
new file mode 100644
index 000000000..b1504e61d
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/sensor_common.c
@@ -0,0 +1,329 @@
+/*
+ * sensor_common.c - utilities for tegra sensor drivers
+ *
+ * Copyright (c) 2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <media/sensor_common.h>
+#include <linux/of_graph.h>
+#include <linux/string.h>
+
+static int read_property_u32(
+        struct device_node *node, const char *name, u32 *value)
+{
+        const char *str;
+        int err = 0;
+
+        err = of_property_read_string(node, name, &str);
+        if (err)
+                return -ENODATA;
+
+        err = kstrtou32(str, 10, value);
+        if (err)
+                return -EFAULT;
+
+        return 0;
+}
+
+static int read_property_u64(
+        struct device_node *node, const char *name, u64 *value)
+{
+        const char *str;
+        int err = 0;
+
+        err = of_property_read_string(node, name, &str);
+        if (err)
+                return -ENODATA;
+
+        err = kstrtou64(str, 10, value);
+        if (err)
+                return -EFAULT;
+
+        return 0;
+}
+
+static int sensor_common_parse_signal_props(
+        struct device *dev, struct device_node *node,
+        struct sensor_signal_properties *signal)
+{
+        const char *temp_str;
+        int err = 0;
+
+        /* Do not report error for these properties yet */
+        read_property_u32(node, "readout_orientation",
+                &signal->readout_orientation);
+        read_property_u32(node, "num_lanes",
+                &signal->num_lanes);
+        read_property_u32(node, "mclk_khz",
+                &signal->mclk_freq);
+        read_property_u64(node, "pix_clk_hz",
+                &signal->pixel_clock.val);
+        read_property_u32(node, "cil_settletime",
+                &signal->cil_settletime);
+        /* initialize default if this prop not available */
+        err = of_property_read_string(node, "discontinuous_clk",
+                &temp_str);
+        if (!err)
+                signal->discontinuous_clk =
+                        !strncmp(temp_str, "yes", sizeof("yes"));
+        else
+                signal->discontinuous_clk = 1;
+        /* initialize default if this prop not available */
+        err = of_property_read_string(node, "dpcm_enable",
+                &temp_str);
+        if (!err)
+                signal->dpcm_enable =
+                        !strncmp(temp_str, "true", sizeof("true"));
+        else
+                signal->dpcm_enable = 0;
+
+        return 0;
+}
+
+static int extract_pixel_format(
+        const char *pixel_t, u32 *format)
+{
+        size_t size = strnlen(pixel_t, OF_MAX_STR_LEN);
+
+        if (strncmp(pixel_t, "bayer_bggr10", size) == 0)
+                *format = V4L2_PIX_FMT_SBGGR10;
+        else if (strncmp(pixel_t, "bayer_rggb10", size) == 0)
+                *format = V4L2_PIX_FMT_SRGGB10;
+        else if (strncmp(pixel_t, "bayer_bggr12", size) == 0)
+                *format = V4L2_PIX_FMT_SBGGR12;
+        else if (strncmp(pixel_t, "bayer_rggb12", size) == 0)
+                *format = V4L2_PIX_FMT_SRGGB12;
+        else if (strncmp(pixel_t, "bayer_xbggr10p", size) == 0)
+                *format = V4L2_PIX_FMT_XBGGR10P;
+        else if (strncmp(pixel_t, "bayer_xrggb10p", size) == 0)
+                *format = V4L2_PIX_FMT_XRGGB10P;
+        else {
+                pr_err("%s: Need to extend format%s\n", __func__, pixel_t);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static int sensor_common_parse_image_props(
+        struct device *dev, struct device_node *node,
+        struct sensor_image_properties *image)
+{
+        const char *temp_str;
+        int err = 0;
+
+        err = read_property_u32(node, "active_w",
+                &image->width);
+        if (err) {
+                dev_err(dev, "%s:active_w property missing\n", __func__);
+                goto fail;
+        }
+
+        err = read_property_u32(node, "active_h",
+                &image->height);
+        if (err) {
+                dev_err(dev, "%s:active_h property missing\n", __func__);
+                goto fail;
+        }
+
+        err = read_property_u32(node, "line_length",
+                &image->line_length);
+        if (err) {
+                dev_err(dev, "%s:Line length property missing\n", __func__);
+                goto fail;
+        }
+
+        err = of_property_read_string(node, "pixel_t",
+                &temp_str);
+        if (err) {
+                dev_err(dev, "%s:pixel_t property missing\n", __func__);
+                goto fail;
+        }
+
+        err = extract_pixel_format(temp_str, &image->pixel_format);
+        if (err) {
+                dev_err(dev, "Unsupported pixel format\n");
+                goto fail;
+        }
+
+        /* ignore err for this prop */
+        read_property_u32(node, "embedded_metadata_height",
+                &image->embedded_metadata_height);
+
+fail:
+        return err;
+}
+
+static int sensor_common_parse_dv_timings(
+        struct device *dev, struct device_node *node,
+        struct sensor_dv_timings *timings)
+{
+        /* Do not report error for these properties yet */
+        read_property_u32(node, "horz_front_porch",
+                &timings->hfrontporch);
+        read_property_u32(node, "horz_sync",
+                &timings->hsync);
+        read_property_u32(node, "horz_back_porch",
+                &timings->hbackporch);
+        read_property_u32(node, "vert_front_porch",
+                &timings->vfrontporch);
+        read_property_u32(node, "vert_sync",
+                &timings->vsync);
+        read_property_u32(node, "vert_back_porch",
+                &timings->vbackporch);
+
+        return 0;
+}
+
+static int sensor_common_parse_control_props(
+        struct device *dev, struct device_node *node,
+        struct sensor_control_properties *control)
+{
+        int err = 0;
+
+        err = read_property_u32(node, "gain_factor",
+                &control->gain_factor);
+        if (err)
+                dev_err(dev, "%s:%s:property missing\n",
+                        __func__, "gain_factor");
+
+        err = read_property_u32(node, "framerate_factor",
+                &control->framerate_factor);
+        if (err)
+                dev_err(dev, "%s:%s:property missing\n",
+                        __func__, "framerate_factor");
+
+        /* ignore err for this prop */
+        err = read_property_u32(node, "inherent_gain",
+                &control->inherent_gain);
+
+        err = read_property_u32(node, "min_gain_val",
+                &control->min_gain_val);
+        if (err)
+                dev_err(dev, "%s:%s:property missing\n",
+                        __func__, "min_gain_val");
+
+        err = read_property_u32(node, "max_gain_val",
+                &control->max_gain_val);
+        if (err)
+                dev_err(dev, "%s:%s:property missing\n",
+                        __func__, "max_gain_val");
+
+        /* ignore err for this prop */
+        err = read_property_u32(node, "min_hdr_ratio",
+                &control->min_hdr_ratio);
+        err = read_property_u32(node, "max_hdr_ratio",
+                &control->max_hdr_ratio);
+
+        err = read_property_u32(node, "min_framerate",
+                &control->min_framerate);
+        if (err)
+                dev_err(dev, "%s:%s:property missing\n",
+                        __func__, "min_framerate");
+
+        err = read_property_u32(node, "max_framerate",
+                &control->max_framerate);
+        if (err)
+                dev_err(dev, "%s:%s:property missing\n",
+                        __func__, "max_framerate");
+
+        err = read_property_u64(node, "min_exp_time",
+                &control->min_exp_time.val);
+        if (err)
+                dev_err(dev, "%s:%s:property missing\n",
+                        __func__, "min_exp_time");
+
+        err = read_property_u64(node, "max_exp_time",
+                &control->max_exp_time.val);
+        if (err)
+                dev_err(dev, "%s:%s:property missing\n",
+                        __func__, "max_exp_time");
+
+
+        return err;
+}
+
+int sensor_common_init_sensor_properties(
+        struct device *dev, struct device_node *node,
+        struct sensor_properties *sensor)
+{
+        char temp_str[OF_MAX_STR_LEN];
+        struct device_node *temp_node;
+        int num_modes = 0;
+        int err, i;
+
+        /* get number of modes */
+        for (i = 0; num_modes < MAX_NUM_SENSOR_MODES; i++) {
+                snprintf(temp_str, sizeof(temp_str), "%s%d",
+                        OF_SENSORMODE_PREFIX, i);
+                temp_node = of_find_node_by_name(node, temp_str);
+                if (temp_node == NULL)
+                        break;
+                num_modes++;
+        }
+        sensor->num_modes = num_modes;
+
+        sensor->sensor_modes = devm_kzalloc(dev,
+                num_modes * sizeof(struct sensor_mode_properties),
+                GFP_KERNEL);
+        if (!sensor->sensor_modes) {
+                dev_err(dev, "Failed to allocate memory for sensor modes\n");
+                return -ENOMEM;
+        }
+        memset(sensor->sensor_modes, 0, num_modes *
+               sizeof(struct sensor_mode_properties));
+
+        for (i = 0; i < num_modes; i++) {
+                snprintf(temp_str, sizeof(temp_str), "%s%d",
+                        OF_SENSORMODE_PREFIX, i);
+
+                temp_node = of_find_node_by_name(node, temp_str);
+                if (temp_node == NULL) {
+                        dev_err(dev, "Failed to find mode\n");
+                        return -ENODATA;
+                };
+
+                err = sensor_common_parse_signal_props(dev, temp_node,
+                        &sensor->sensor_modes[i].signal_properties);
+                if (err) {
+                        dev_err(dev, "Failed to read signal properties\n");
+                        return err;
+                }
+
+                err = sensor_common_parse_image_props(dev, temp_node,
+                        &sensor->sensor_modes[i].image_properties);
+                if (err) {
+                        dev_err(dev, "Failed to read image properties\n");
+                        return err;
+                }
+
+                err = sensor_common_parse_dv_timings(dev, temp_node,
+                        &sensor->sensor_modes[i].dv_timings);
+                if (err) {
+                        dev_err(dev, "Failed to read DV timings\n");
+                        return err;
+                }
+
+                err = sensor_common_parse_control_props(dev, temp_node,
+                        &sensor->sensor_modes[i].control_properties);
+                if (err) {
+                        dev_err(dev, "Failed to read control properties\n");
+                        return err;
+                }
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL(sensor_common_init_sensor_properties);
diff --git a/drivers/media/platform/tegra/camera/vi/Makefile b/drivers/media/platform/tegra/camera/vi/Makefile
new file mode 100644
index 000000000..11bce8f3b
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/Makefile
@@ -0,0 +1,21 @@
+GCOV_PROFILE := y
+ccflags-y += -I../nvhost/drivers/video/tegra/host
+ccflags-y += -Idrivers/video/tegra/host
+ccflags-y += -Idrivers/video/tegra/camera
+ccflags-y += -Idrivers/media/platform/tegra
+ccflags-y += -Werror
+
+ifeq ($(CONFIG_ARCH_TEGRA_18x_SOC),y)
+ccflags-y += -I../t18x/drivers/video/tegra/host/
+ccflags-y += -I../t18x/include
+ccflags-y += -I../nvhost/include
+obj-y += mc_common.o core.o channel.o graph.o vi2_fops.o vi4_fops.o
+endif
+
+obj-$(CONFIG_TEGRA_CAMERA_RTCPU) += capture.o
+
+ifeq ($(CONFIG_ARCH_TEGRA_210_SOC),y)
+ccflags-y += -I../nvhost/include
+ccflags-y += -I../t18x/drivers/video/tegra/host/
+obj-y += mc_common.o core.o channel.o graph.o vi2_fops.o
+endif
diff --git a/drivers/media/platform/tegra/camera/vi/capture.c b/drivers/media/platform/tegra/camera/vi/capture.c
new file mode 100644
index 000000000..7a8083098
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/capture.c
@@ -0,0 +1,880 @@
+/*
+ * Tegra Video Input capture operations
+ *
+ * Tegra Graphics Host VI
+ *
+ * Copyright (c) 2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: David Wang <davidw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/completion.h>
+#include <linux/dma-buf.h>
+#include <linux/nvhost.h>
+#include <linux/of_platform.h>
+#include <linux/printk.h>
+#include <linux/scatterlist.h>
+#include <linux/tegra-capture-ivc.h>
+#include <media/capture.h>
+
+#include "soc/tegra/camrtc-capture.h"
+#include "soc/tegra/camrtc-capture-messages.h"
+#include "mc_common.h"
+
+
+#define CAPTURE_CHANNEL_UNKNOWN_RESP 0xFFFFFFFF
+#define CAPTURE_CHANNEL_INVALID_ID 0xFFFF
+#define CAPTURE_CHANNEL_INVALID_MASK 0llu
+#define PROGRESS_SP_IDX 0
+#define EMBDATA_SP_IDX 1
+#define LINETIMER_SP_IDX 2
+#define CAPTURE_CHANNEL_MAX_NUM_SPS 3
+
+struct vi_capture_buf {
+        struct dma_buf *buf;
+        struct dma_buf_attachment *attach;
+        struct sg_table *sgt;
+        dma_addr_t iova;
+};
+
+struct vi_capture_unpins {
+        uint32_t num_unpins;
+        struct vi_capture_buf data[];
+};
+
+struct vi_capture {
+        uint16_t channel_id;
+        struct device *rtcpu_dev;
+        struct tegra_channel *vi_channel;
+        struct vi_capture_buf requests;
+        uint32_t request_size;
+
+        uint32_t syncpts[CAPTURE_CHANNEL_MAX_NUM_SPS];
+
+        struct completion control_resp;
+        struct completion capture_resp;
+        struct mutex control_msg_lock;
+        struct CAPTURE_CONTROL_MSG control_resp_msg;
+
+        struct mutex unpins_list_lock;
+        struct vi_capture_unpins *unpins_list;
+};
+
+static void vi_capture_ivc_control_callback(const void *ivc_resp,
+                const void *pcontext)
+{
+        const struct CAPTURE_CONTROL_MSG *control_msg = ivc_resp;
+        struct vi_capture *capture = (struct vi_capture *)pcontext;
+        struct tegra_channel *chan = capture->vi_channel;
+
+        if (unlikely(capture == NULL)) {
+                dev_err(chan->vi->dev, "%s: invalid context", __func__);
+                return;
+        }
+
+        if (unlikely(control_msg == NULL)) {
+                dev_err(chan->vi->dev, "%s: invalid response", __func__);
+                return;
+        }
+
+        switch (control_msg->header.msg_id) {
+        case CAPTURE_CHANNEL_SETUP_RESP:
+        case CAPTURE_CHANNEL_RESET_RESP:
+        case CAPTURE_CHANNEL_RELEASE_RESP:
+        case CAPTURE_COMPAND_CONFIG_RESP:
+        case CAPTURE_PDAF_CONFIG_RESP:
+        case CAPTURE_SYNCGEN_ENABLE_RESP:
+        case CAPTURE_SYNCGEN_DISABLE_RESP:
+                memcpy(&capture->control_resp_msg, control_msg,
+                                sizeof(*control_msg));
+                complete(&capture->control_resp);
+                break;
+        default:
+                dev_err(chan->vi->dev,
+                        "%s: unknown capture control resp", __func__);
+                break;
+        }
+}
+
+static void vi_capture_request_unpin(struct tegra_channel *chan,
+                uint32_t buffer_index);
+static void vi_capture_ivc_status_callback(const void *ivc_resp,
+                const void *pcontext)
+{
+        struct CAPTURE_MSG *status_msg = (struct CAPTURE_MSG *)ivc_resp;
+        struct vi_capture *capture = (struct vi_capture *)pcontext;
+        struct tegra_channel *chan = capture->vi_channel;
+
+        if (unlikely(capture == NULL)) {
+                dev_err(chan->vi->dev, "%s: invalid context", __func__);
+                return;
+        }
+
+        if (unlikely(status_msg == NULL)) {
+                dev_err(chan->vi->dev, "%s: invalid response", __func__);
+                return;
+        }
+
+        switch (status_msg->header.msg_id) {
+        case CAPTURE_STATUS_IND:
+                vi_capture_request_unpin(chan,
+                                status_msg->capture_status_ind.buffer_index);
+                complete(&capture->capture_resp);
+                dev_dbg(chan->vi->dev, "%s: status chan_id %u msg_id %u\n",
+                                __func__, status_msg->header.channel_id,
+                                status_msg->header.msg_id);
+                break;
+        default:
+                dev_err(chan->vi->dev,
+                        "%s: unknown capture resp", __func__);
+                break;
+        }
+}
+
+int vi_capture_init(struct tegra_channel *chan)
+{
+        struct vi_capture *capture;
+        struct device_node *dn;
+        struct platform_device *rtc_pdev;
+
+        dev_dbg(chan->vi->dev, "%s++\n", __func__);
+        dn = of_find_node_by_path("tegra-camera-rtcpu");
+        if (of_device_is_available(dn) == 0) {
+                dev_err(chan->vi->dev, "failed to find rtcpu device node\n");
+                return -ENODEV;
+        }
+        rtc_pdev = of_find_device_by_node(dn);
+        if (rtc_pdev == NULL) {
+                dev_err(chan->vi->dev, "failed to find rtcpu platform\n");
+                return -ENODEV;
+        }
+
+        capture = devm_kzalloc(chan->vi->dev,
+                        sizeof(*capture), GFP_KERNEL);
+        if (unlikely(capture == NULL)) {
+                dev_err(chan->vi->dev, "failed to allocate capture channel\n");
+                return -ENOMEM;
+        }
+
+        capture->rtcpu_dev = &rtc_pdev->dev;
+
+        init_completion(&capture->control_resp);
+        init_completion(&capture->capture_resp);
+
+        mutex_init(&capture->control_msg_lock);
+        mutex_init(&capture->unpins_list_lock);
+
+        capture->vi_channel = chan;
+        chan->capture_data = capture;
+
+        capture->channel_id = CAPTURE_CHANNEL_INVALID_ID;
+
+        return 0;
+}
+
+void vi_capture_shutdown(struct tegra_channel *chan)
+{
+        struct vi_capture *capture = chan->capture_data;
+
+        dev_dbg(chan->vi->dev, "%s--\n", __func__);
+        if (capture == NULL)
+                return;
+
+        if (capture->channel_id != CAPTURE_CHANNEL_INVALID_ID)
+                vi_capture_release(chan, 0);
+
+        devm_kfree(chan->vi->dev, capture);
+        chan->capture_data = NULL;
+}
+
+static int vi_capture_ivc_send_control(struct tegra_channel *chan,
+                const struct CAPTURE_CONTROL_MSG *msg, size_t size,
+                uint32_t resp_id)
+{
+        struct vi_capture *capture = chan->capture_data;
+        struct CAPTURE_MSG_HEADER resp_header = msg->header;
+        uint32_t timeout = HZ;
+        int err = 0;
+
+        dev_dbg(chan->vi->dev, "%s: sending chan_id %u msg_id %u\n",
+                        __func__, resp_header.channel_id, resp_header.msg_id);
+        resp_header.msg_id = resp_id;
+        /* Send capture control IVC message */
+        mutex_lock(&capture->control_msg_lock);
+        err = tegra_capture_ivc_control_submit(msg, size);
+        if (err < 0) {
+                dev_err(chan->vi->dev, "IVC control submit failed\n");
+                goto fail;
+        }
+
+        timeout = wait_for_completion_killable_timeout(
+                        &capture->control_resp, timeout);
+        if (timeout <= 0) {
+                dev_err(chan->vi->dev,
+                        "no reply from camera processor\n");
+                err = -ETIMEDOUT;
+                goto fail;
+        }
+
+        if (memcmp(&resp_header, &capture->control_resp_msg.header,
+                        sizeof(resp_header)) != 0) {
+                dev_err(chan->vi->dev,
+                        "unexpected response from camera processor\n");
+                err = -EINVAL;
+                goto fail;
+        }
+
+        mutex_unlock(&capture->control_msg_lock);
+        dev_dbg(chan->vi->dev, "%s: response chan_id %u msg_id %u\n",
+                        __func__, capture->control_resp_msg.header.channel_id,
+                        capture->control_resp_msg.header.msg_id);
+        return 0;
+
+fail:
+        mutex_unlock(&capture->control_msg_lock);
+        return err;
+}
+
+static int pin_memory(struct device *dev,
+                uint32_t mem, struct vi_capture_buf *unpin_data);
+static void unpin_memory(struct vi_capture_buf *unpin_data);
+
+static int pin_memory(struct device *dev,
+                uint32_t mem, struct vi_capture_buf *unpin_data)
+{
+        struct dma_buf *buf;
+        struct dma_buf_attachment *attach;
+        struct sg_table *sgt;
+        int err = 0;
+
+        buf = dma_buf_get(mem);
+        if (IS_ERR(buf)) {
+                err = PTR_ERR(buf);
+                goto fail;
+        }
+
+        attach = dma_buf_attach(buf, dev);
+        if (IS_ERR(attach)) {
+                err = PTR_ERR(attach);
+                goto fail;
+        }
+
+        sgt = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+        if (IS_ERR(sgt)) {
+                err = PTR_ERR(sgt);
+                goto fail;
+        }
+
+        if (sg_dma_address(sgt->sgl) == 0)
+                sg_dma_address(sgt->sgl) = sg_phys(sgt->sgl);
+
+        unpin_data->iova = sg_dma_address(sgt->sgl);
+        unpin_data->buf = buf;
+        unpin_data->attach = attach;
+        unpin_data->sgt = sgt;
+
+        return 0;
+
+fail:
+        unpin_memory(unpin_data);
+        return err;
+}
+
+static void unpin_memory(struct vi_capture_buf *unpin_data)
+{
+        if (unpin_data->sgt != NULL)
+                dma_buf_unmap_attachment(unpin_data->attach, unpin_data->sgt,
+                                DMA_BIDIRECTIONAL);
+        if (unpin_data->attach != NULL)
+                dma_buf_detach(unpin_data->buf, unpin_data->attach);
+        if (unpin_data->buf != NULL)
+                dma_buf_put(unpin_data->buf);
+
+        unpin_data->sgt = NULL;
+        unpin_data->attach = NULL;
+        unpin_data->buf = NULL;
+        unpin_data->iova = 0;
+}
+
+static int vi_capture_setup_syncpts(struct tegra_channel *chan);
+static void vi_capture_release_syncpts(struct tegra_channel *chan);
+
+static int vi_capture_setup_syncpts(struct tegra_channel *chan)
+{
+        struct vi_capture *capture = chan->capture_data;
+        int i;
+        int err = 0;
+
+        for (i = 0; i < CAPTURE_CHANNEL_MAX_NUM_SPS; i++) {
+                        capture->syncpts[i] = nvhost_get_syncpt_client_managed(
+                        chan->vi->ndev, "vi-capture");
+                if (capture->syncpts[i] == 0) {
+                        dev_err(chan->vi->dev, "failed to get syncpt %i!\n", i);
+                        err = -ENODEV;
+                        goto fail;
+                }
+        }
+
+        return 0;
+
+fail:
+        vi_capture_release_syncpts(chan);
+        return err;
+}
+
+static void vi_capture_release_syncpts(struct tegra_channel *chan)
+{
+        struct vi_capture *capture = chan->capture_data;
+        int i;
+
+        for (i = 0; i < CAPTURE_CHANNEL_MAX_NUM_SPS; i++) {
+                if (capture->syncpts[i] != 0)
+                        nvhost_syncpt_put_ref_ext(chan->vi->ndev,
+                                        capture->syncpts[i]);
+                capture->syncpts[i] = 0;
+        }
+}
+
+int vi_capture_setup(struct tegra_channel *chan,
+                struct vi_capture_setup *setup)
+{
+        struct vi_capture *capture = chan->capture_data;
+        uint32_t transaction;
+        struct CAPTURE_CONTROL_MSG control_desc;
+        struct CAPTURE_CONTROL_MSG *resp_msg = &capture->control_resp_msg;
+        struct capture_channel_config *config =
+                &control_desc.channel_setup_req.channel_config;
+        int err = 0;
+
+        if (capture == NULL) {
+                dev_err(chan->vi->dev,
+                         "%s: vi capture uninitialized\n", __func__);
+                return -ENODEV;
+        }
+
+        if (capture->channel_id != CAPTURE_CHANNEL_INVALID_ID) {
+                dev_err(chan->vi->dev,
+                        "%s: already setup, release first\n", __func__);
+                return -EEXIST;
+        }
+
+        dev_dbg(chan->vi->dev, "chan flags %u\n", setup->channel_flags);
+        dev_dbg(chan->vi->dev, "chan mask %llx\n", setup->vi_channel_mask);
+        dev_dbg(chan->vi->dev, "queue depth %u\n", setup->queue_depth);
+        dev_dbg(chan->vi->dev, "request size %u\n", setup->request_size);
+
+        if (setup->vi_channel_mask == CAPTURE_CHANNEL_INVALID_MASK ||
+                        setup->channel_flags == 0 ||
+                        setup->queue_depth == 0 ||
+                        setup->request_size == 0)
+                return -EINVAL;
+
+        /* pin the capture descriptor ring buffer */
+        dev_dbg(chan->vi->dev, "%s: descr buffer handle %u\n",
+                        __func__, setup->mem);
+        err = pin_memory(capture->rtcpu_dev, setup->mem, &capture->requests);
+        if (err < 0) {
+                dev_err(chan->vi->dev, "%s: memory setup failed\n", __func__);
+                return -EFAULT;
+        }
+        capture->request_size = setup->request_size;
+
+        /* allocate for unpin list based on queue depth */
+        capture->unpins_list = devm_kzalloc(chan->vi->dev,
+                        sizeof(struct vi_capture_unpins) * setup->queue_depth,
+                        GFP_KERNEL);
+        if (unlikely(capture->unpins_list == NULL)) {
+                dev_err(chan->vi->dev, "failed to allocate unpins array\n");
+                goto unpins_list_fail;
+        }
+
+        err = vi_capture_setup_syncpts(chan);
+        if (err < 0) {
+                dev_err(chan->vi->dev, "%s: syncpt setup failed\n", __func__);
+                goto syncpt_fail;
+        }
+
+        err = tegra_capture_ivc_register_control_cb(
+                        &vi_capture_ivc_control_callback,
+                        &transaction, capture);
+        if (err < 0) {
+                dev_err(chan->vi->dev, "failed to register control callback\n");
+                goto control_cb_fail;
+        }
+
+        memset(&control_desc, 0, sizeof(control_desc));
+        control_desc.header.msg_id = CAPTURE_CHANNEL_SETUP_REQ;
+        control_desc.header.transaction = transaction;
+
+        config->channel_flags = setup->channel_flags;
+        config->vi_channel_mask = setup->vi_channel_mask;
+
+        config->queue_depth = setup->queue_depth;
+        config->request_size = setup->request_size;
+        config->requests = capture->requests.iova;
+
+        config->progress_sp.id = capture->syncpts[PROGRESS_SP_IDX];
+        config->embdata_sp.id = capture->syncpts[EMBDATA_SP_IDX];
+        config->linetimer_sp.id = capture->syncpts[LINETIMER_SP_IDX];
+
+        err = vi_capture_ivc_send_control(chan, &control_desc,
+                        sizeof(control_desc), CAPTURE_CHANNEL_SETUP_RESP);
+        if (err < 0)
+                goto submit_fail;
+
+        if (resp_msg->channel_setup_resp.result != CAPTURE_OK) {
+                dev_err(chan->vi->dev, "%s: control failed, errno %d", __func__,
+                        resp_msg->channel_setup_resp.result);
+                err = -EINVAL;
+                goto resp_fail;
+        }
+
+        capture->channel_id = resp_msg->channel_setup_resp.channel_id;
+
+        err = tegra_capture_ivc_notify_chan_id(capture->channel_id,
+                        transaction);
+        if (err < 0) {
+                dev_err(chan->vi->dev, "failed to update control callback\n");
+                goto cb_fail;
+        }
+
+        err = tegra_capture_ivc_register_capture_cb(
+                        &vi_capture_ivc_status_callback,
+                        capture->channel_id, capture);
+        if (err < 0) {
+                dev_err(chan->vi->dev, "failed to register capture callback\n");
+                goto cb_fail;
+        }
+
+        return 0;
+
+cb_fail:
+        vi_capture_release(chan, CAPTURE_CHANNEL_RESET_FLAG_IMMEDIATE);
+resp_fail:
+submit_fail:
+        tegra_capture_ivc_unregister_control_cb(transaction);
+control_cb_fail:
+        vi_capture_release_syncpts(chan);
+syncpt_fail:
+        devm_kfree(chan->vi->dev, capture->unpins_list);
+unpins_list_fail:
+        unpin_memory(&capture->requests);
+        return err;
+}
+
+int vi_capture_reset(struct tegra_channel *chan,
+                uint32_t reset_flags)
+{
+        struct vi_capture *capture = chan->capture_data;
+        struct CAPTURE_CONTROL_MSG control_desc;
+        struct CAPTURE_CONTROL_MSG *resp_msg = &capture->control_resp_msg;
+        int err = 0;
+
+        if (capture == NULL) {
+                dev_err(chan->vi->dev,
+                         "%s: vi capture uninitialized\n", __func__);
+                return -ENODEV;
+        }
+
+        if (capture->channel_id == CAPTURE_CHANNEL_INVALID_ID) {
+                dev_err(chan->vi->dev,
+                        "%s: setup channel first\n", __func__);
+                return -ENODEV;
+        }
+
+        memset(&control_desc, 0, sizeof(control_desc));
+        control_desc.header.msg_id = CAPTURE_CHANNEL_RESET_REQ;
+        control_desc.header.channel_id = capture->channel_id;
+        control_desc.channel_reset_req.reset_flags = reset_flags;
+
+        err = vi_capture_ivc_send_control(chan, &control_desc,
+                        sizeof(control_desc), CAPTURE_CHANNEL_RESET_RESP);
+        if (err < 0)
+                goto submit_fail;
+
+        if (resp_msg->channel_reset_resp.result != CAPTURE_OK) {
+                dev_err(chan->vi->dev, "%s: control failed, errno %d", __func__,
+                        resp_msg->channel_reset_resp.result);
+                err = -EINVAL;
+        }
+
+        return 0;
+
+submit_fail:
+        return err;
+}
+
+int vi_capture_release(struct tegra_channel *chan,
+                uint32_t reset_flags)
+{
+        struct vi_capture *capture = chan->capture_data;
+        struct CAPTURE_CONTROL_MSG control_desc;
+        struct CAPTURE_CONTROL_MSG *resp_msg = &capture->control_resp_msg;
+        int err = 0;
+        int ret = 0;
+
+        if (capture == NULL) {
+                dev_err(chan->vi->dev,
+                         "%s: vi capture uninitialized\n", __func__);
+                return -ENODEV;
+        }
+
+        if (capture->channel_id == CAPTURE_CHANNEL_INVALID_ID) {
+                dev_err(chan->vi->dev,
+                        "%s: setup channel first\n", __func__);
+                return -ENODEV;
+        }
+
+        memset(&control_desc, 0, sizeof(control_desc));
+        control_desc.header.msg_id = CAPTURE_CHANNEL_RELEASE_REQ;
+        control_desc.header.channel_id = capture->channel_id;
+        control_desc.channel_release_req.reset_flags = reset_flags;
+
+        err = vi_capture_ivc_send_control(chan, &control_desc,
+                        sizeof(control_desc), CAPTURE_CHANNEL_RELEASE_RESP);
+        if (err < 0)
+                goto submit_fail;
+
+        if (resp_msg->channel_release_resp.result != CAPTURE_OK) {
+                dev_err(chan->vi->dev, "%s: control failed, errno %d", __func__,
+                        resp_msg->channel_release_resp.result);
+                err = -EINVAL;
+        }
+
+        vi_capture_release_syncpts(chan);
+        unpin_memory(&capture->requests);
+
+        ret = tegra_capture_ivc_unregister_capture_cb(capture->channel_id);
+        if (ret < 0 && err == 0) {
+                dev_err(chan->vi->dev,
+                        "failed to unregister capture callback\n");
+                err = ret;
+        }
+
+        ret = tegra_capture_ivc_unregister_control_cb(capture->channel_id);
+        if (ret < 0 && err == 0) {
+                dev_err(chan->vi->dev,
+                        "failed to unregister control callback\n");
+                err = ret;
+        }
+
+        capture->channel_id = CAPTURE_CHANNEL_INVALID_ID;
+
+        return 0;
+
+submit_fail:
+        return err;
+}
+
+int vi_capture_get_info(struct tegra_channel *chan,
+                struct vi_capture_info *info)
+{
+        struct vi_capture *capture = chan->capture_data;
+
+        if (capture == NULL) {
+                dev_err(chan->vi->dev,
+                         "%s: vi capture uninitialized\n", __func__);
+                return -ENODEV;
+        }
+
+        if (capture->channel_id == CAPTURE_CHANNEL_INVALID_ID) {
+                dev_err(chan->vi->dev,
+                        "%s: setup channel first\n", __func__);
+                return -ENODEV;
+        }
+
+        if (info == NULL)
+                return -EINVAL;
+
+        info->syncpts.progress_syncpt = capture->syncpts[PROGRESS_SP_IDX];
+        info->syncpts.emb_data_syncpt = capture->syncpts[EMBDATA_SP_IDX];
+        info->syncpts.line_timer_syncpt = capture->syncpts[LINETIMER_SP_IDX];
+
+        return 0;
+}
+
+int vi_capture_control_message(struct tegra_channel *chan,
+                struct vi_capture_control_msg *msg)
+{
+        struct vi_capture *capture = chan->capture_data;
+        const void __user *msg_ptr =
+                        (const void __user *)(uintptr_t)msg->ptr;
+        void __user *response =
+                        (void __user *)(uintptr_t)msg->response;
+        void *msg_cpy;
+        struct CAPTURE_MSG_HEADER *header;
+        uint32_t resp_id;
+        struct CAPTURE_CONTROL_MSG *resp_msg = &capture->control_resp_msg;
+        int err = 0;
+
+        if (capture == NULL) {
+                dev_err(chan->vi->dev,
+                         "%s: vi capture uninitialized\n", __func__);
+                return -ENODEV;
+        }
+
+        if (msg->ptr == 0ull || msg->response == 0ull || msg->size == 0)
+                return -EINVAL;
+
+        msg_cpy = devm_kzalloc(chan->vi->dev, msg->size, GFP_KERNEL);
+        if (unlikely(msg_cpy == NULL))
+                return -ENOMEM;
+
+        err = copy_from_user(msg_cpy, msg_ptr, msg->size) ? -EFAULT : 0;
+        if (err < 0)
+                goto fail;
+        header = (struct CAPTURE_MSG_HEADER *)msg_cpy;
+        header->channel_id = capture->channel_id;
+
+        switch (header->msg_id) {
+        case CAPTURE_COMPAND_CONFIG_REQ:
+                resp_id = CAPTURE_COMPAND_CONFIG_RESP;
+                break;
+        case CAPTURE_PDAF_CONFIG_REQ:
+                resp_id = CAPTURE_PDAF_CONFIG_RESP;
+                break;
+        case CAPTURE_SYNCGEN_ENABLE_REQ:
+                resp_id = CAPTURE_SYNCGEN_ENABLE_RESP;
+                break;
+        case CAPTURE_SYNCGEN_DISABLE_REQ:
+                resp_id = CAPTURE_SYNCGEN_DISABLE_RESP;
+                break;
+        default:
+                dev_err(chan->vi->dev,
+                                "%s: unknown capture control resp", __func__);
+                err = -EINVAL;
+                goto fail;
+        }
+
+        err = vi_capture_ivc_send_control(chan, msg_cpy, msg->size, resp_id);
+        if (err < 0)
+                goto fail;
+
+        err = copy_to_user(response, resp_msg,
+                        sizeof(*resp_msg)) ? -EFAULT : 0;
+
+fail:
+        devm_kfree(chan->vi->dev, msg_cpy);
+        return err;
+}
+
+struct surface_t {
+        uint32_t offset;
+        uint32_t offset_hi;
+};
+
+static int vi_capture_request_pin_and_reloc(struct tegra_channel *chan,
+                struct vi_capture_req *req)
+{
+        struct vi_capture *capture = chan->capture_data;
+        uint32_t num_relocs = req->num_relocs;
+        uint32_t __user *reloc_relatives =
+                        (uint32_t __user *)(uintptr_t)req->reloc_relatives;
+        uint32_t local_reloc_relatives[num_relocs];
+        struct vi_capture_unpins *unpins;
+        uint32_t request_offset = req->buffer_index * capture->request_size;
+        void *reloc_page_addr = NULL;
+        uint32_t prev_mem = 0;
+        int last_page = -1;
+        dma_addr_t surface_phys_addr = 0;
+        dma_addr_t surface_prev_addr = 0;
+        int i, pin_count = 0;
+        int err = 0;
+
+        err = copy_from_user(local_reloc_relatives, reloc_relatives,
+                num_relocs * sizeof(uint32_t)) ? -EFAULT : 0;
+        if (err < 0)
+                return err;
+
+        unpins = devm_kzalloc(chan->vi->dev,
+                        sizeof(struct vi_capture_unpins) +
+                        sizeof(struct vi_capture_buf) * num_relocs,
+                        GFP_KERNEL);
+        if (unpins == NULL)
+                return -ENOMEM;
+
+        dev_dbg(chan->vi->dev, "%s: relocating %u surfaces\n",
+                        __func__, num_relocs);
+        for (i = 0; i < num_relocs; i++) {
+                uint32_t reloc_offset =
+                                request_offset + local_reloc_relatives[i];
+                uint64_t surface_raw;
+                struct surface_t *surface;
+                uint32_t mem;
+                uint32_t target_offset;
+                dma_addr_t target_phys_addr;
+
+                dev_dbg(chan->vi->dev,
+                        "%s: idx:%i reloc:%u reloc_offset:%u", __func__,
+                        i, local_reloc_relatives[i], reloc_offset);
+
+                /* locate page of the request descr buffer relocation is on */
+                if (last_page != reloc_offset >> PAGE_SHIFT) {
+                        if (reloc_page_addr != NULL)
+                                dma_buf_kunmap(capture->requests.buf, last_page,
+                                                reloc_page_addr);
+
+                        reloc_page_addr = dma_buf_kmap(capture->requests.buf,
+                                        reloc_offset >> PAGE_SHIFT);
+                        last_page = reloc_offset >> PAGE_SHIFT;
+
+                        if (unlikely(reloc_page_addr == NULL)) {
+                                dev_err(chan->vi->dev,
+                                        "%s: couldn't map request\n", __func__);
+                                goto fail;
+                        }
+                }
+
+                /* read surface offset and memory handle from request descr */
+                surface_raw = __raw_readq(
+                                (void __iomem *)(reloc_page_addr +
+                                (reloc_offset & ~PAGE_MASK)));
+                surface = (struct surface_t *)&surface_raw;
+                target_offset = surface->offset;
+                mem = surface->offset_hi;
+                dev_dbg(chan->vi->dev, "%s: hmem:%u offset:%u\n", __func__,
+                                target_offset, mem);
+
+                if (mem != prev_mem) {
+                        err = pin_memory(capture->rtcpu_dev,
+                                         mem, &unpins->data[pin_count]);
+                        if (err < 0) {
+                                unpins->num_unpins = pin_count;
+                                goto fail;
+                        }
+                        surface_prev_addr = unpins->data[i].iova;
+                        surface_phys_addr = unpins->data[i].iova;
+
+                        mutex_lock(&capture->unpins_list_lock);
+                        memcpy(&capture->unpins_list[req->buffer_index],
+                                        unpins, sizeof(*unpins));
+                        mutex_unlock(&capture->unpins_list_lock);
+
+                        pin_count++;
+                } else
+                        surface_phys_addr = surface_prev_addr;
+
+                target_phys_addr = surface_phys_addr + target_offset;
+                /* write relocated physical address to request descr */
+                __raw_writeq(
+                        target_phys_addr,
+                        (void __iomem *)(reloc_page_addr +
+                                (reloc_offset & ~PAGE_MASK)));
+        }
+
+        unpins->num_unpins = pin_count;
+
+        return 0;
+
+fail:
+        if (reloc_page_addr != NULL)
+                dma_buf_kunmap(capture->requests.buf, last_page,
+                        reloc_page_addr);
+
+        for (i = 0; i < unpins->num_unpins; i++)
+                unpin_memory(&unpins->data[i]);
+        devm_kfree(chan->vi->dev, unpins);
+
+        return err;
+}
+
+static void vi_capture_request_unpin(struct tegra_channel *chan,
+                uint32_t buffer_index)
+{
+        struct vi_capture *capture = chan->capture_data;
+        struct vi_capture_unpins *unpins;
+        int i = 0;
+
+        mutex_lock(&capture->unpins_list_lock);
+        unpins = &capture->unpins_list[buffer_index];
+        for (i = 0; i < unpins->num_unpins; i++)
+                unpin_memory(&unpins->data[i]);
+        unpins->num_unpins = 0u;
+        mutex_unlock(&capture->unpins_list_lock);
+}
+
+int vi_capture_request(struct tegra_channel *chan,
+                struct vi_capture_req *req)
+{
+        struct vi_capture *capture = chan->capture_data;
+        struct CAPTURE_MSG capture_desc;
+        int err = 0;
+
+        if (capture == NULL) {
+                dev_err(chan->vi->dev,
+                        "%s: vi capture uninitialized\n", __func__);
+                return -ENODEV;
+        }
+
+        if (capture->channel_id == CAPTURE_CHANNEL_INVALID_ID) {
+                dev_err(chan->vi->dev,
+                        "%s: setup channel first\n", __func__);
+                return -ENODEV;
+        }
+
+        memset(&capture_desc, 0, sizeof(capture_desc));
+        capture_desc.header.msg_id = CAPTURE_REQUEST_REQ;
+        capture_desc.header.channel_id = capture->channel_id;
+        capture_desc.capture_request_req.buffer_index = req->buffer_index;
+
+        /* perform surface pinning and relocation */
+        err = vi_capture_request_pin_and_reloc(chan, req);
+        if (err < 0) {
+                dev_err(chan->vi->dev, "relocation failed\n");
+                return err;
+        }
+
+        dev_dbg(chan->vi->dev, "%s: sending chan_id %u msg_id %u buf:%u\n",
+                        __func__, capture_desc.header.channel_id,
+                        capture_desc.header.msg_id, req->buffer_index);
+        err = tegra_capture_ivc_capture_submit(&capture_desc,
+                        sizeof(capture_desc));
+        if (err < 0) {
+                dev_err(chan->vi->dev, "IVC capture submit failed\n");
+                goto fail;
+        }
+
+        return 0;
+
+fail:
+        vi_capture_request_unpin(chan, req->buffer_index);
+        return err;
+}
+
+int vi_capture_status(struct tegra_channel *chan,
+                int32_t timeout_ms)
+{
+        struct vi_capture *capture = chan->capture_data;
+        int ret = 0;
+        int err = 0;
+
+        if (capture == NULL) {
+                dev_err(chan->vi->dev,
+                         "%s: vi capture uninitialized\n", __func__);
+                return -ENODEV;
+        }
+
+        if (capture->channel_id == CAPTURE_CHANNEL_INVALID_ID) {
+                dev_err(chan->vi->dev,
+                        "%s: setup channel first\n", __func__);
+                return -ENODEV;
+        }
+
+        dev_dbg(chan->vi->dev, "%s: waiting for status, timeout:%d ms\n",
+                __func__, timeout_ms);
+
+        ret = wait_for_completion_killable_timeout(
+                        &capture->capture_resp,
+                        (unsigned long)(timeout_ms/1000*HZ));
+        if (ret <= 0) {
+                dev_err(chan->vi->dev,
+                        "no reply from camera processor\n");
+                return -ETIMEDOUT;
+        }
+
+        return err;
+}
diff --git a/drivers/media/platform/tegra/camera/vi/channel.c b/drivers/media/platform/tegra/camera/vi/channel.c
new file mode 100644
index 000000000..5c8e066be
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/channel.c
@@ -0,0 +1,1650 @@
+/*
+ * NVIDIA Tegra Video Input Device
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/atomic.h>
+#include <linux/bitmap.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/nvhost.h>
+#include <linux/lcm.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/camera_common.h>
+#include <media/tegra_camera_platform.h>
+#include <media/v4l2-dv-timings.h>
+
+#include <linux/clk/tegra.h>
+
+#include "mc_common.h"
+#include "vi/vi.h"
+#include "mipical/mipi_cal.h"
+
+#define TPG_CSI_GROUP_ID        10
+
+static void gang_buffer_offsets(struct tegra_channel *chan)
+{
+        int i;
+        u32 offset = 0;
+
+        for (i = 0; i < chan->total_ports; i++) {
+                switch (chan->gang_mode) {
+                case CAMERA_NO_GANG_MODE:
+                case CAMERA_GANG_L_R:
+                case CAMERA_GANG_R_L:
+                        offset = chan->gang_bytesperline;
+                        break;
+                case CAMERA_GANG_T_B:
+                case CAMERA_GANG_B_T:
+                        offset = chan->gang_sizeimage;
+                        break;
+                default:
+                        offset = 0;
+                }
+                offset = ((offset + TEGRA_SURFACE_ALIGNMENT - 1) &
+                                        ~(TEGRA_SURFACE_ALIGNMENT - 1));
+                chan->buffer_offset[i] = i * offset;
+        }
+}
+
+static u32 gang_mode_width(enum camera_gang_mode gang_mode,
+                                        unsigned int width)
+{
+        if ((gang_mode == CAMERA_GANG_L_R) ||
+                (gang_mode == CAMERA_GANG_R_L))
+                return width >> 1;
+        else
+                return width;
+}
+
+static u32 gang_mode_height(enum camera_gang_mode gang_mode,
+                                        unsigned int height)
+{
+        if ((gang_mode == CAMERA_GANG_T_B) ||
+                (gang_mode == CAMERA_GANG_B_T))
+                return height >> 1;
+        else
+                return height;
+}
+
+static void update_gang_mode_params(struct tegra_channel *chan)
+{
+        chan->gang_width = gang_mode_width(chan->gang_mode,
+                                                chan->format.width);
+        chan->gang_height = gang_mode_height(chan->gang_mode,
+                                                chan->format.height);
+        chan->gang_bytesperline = ((chan->gang_width *
+                                        chan->fmtinfo->bpp.numerator) /
+                                        chan->fmtinfo->bpp.denominator);
+        chan->gang_sizeimage = chan->gang_bytesperline *
+                                        chan->format.height;
+        gang_buffer_offsets(chan);
+}
+
+static void update_gang_mode(struct tegra_channel *chan)
+{
+        int width = chan->format.width;
+        int height = chan->format.height;
+
+        /*
+         * At present only 720p, 1080p and 4k resolutions
+         * are supported and only 4K requires gang mode
+         * Update this code with CID for future extensions
+         * Also, validate width and height of images based
+         * on gang mode and surface stride alignment
+         */
+        if ((width > 1920) && (height > 1080)) {
+                chan->gang_mode = CAMERA_GANG_L_R;
+                chan->valid_ports = chan->total_ports;
+        } else {
+                chan->gang_mode = CAMERA_NO_GANG_MODE;
+                chan->valid_ports = 1;
+        }
+
+        update_gang_mode_params(chan);
+}
+
+static u32 get_aligned_buffer_size(struct tegra_channel *chan,
+                u32 bytesperline, u32 height)
+{
+        u32 height_aligned;
+        u32 temp_size, size;
+
+        height_aligned = roundup(height, chan->height_align);
+        temp_size = bytesperline * height_aligned;
+        size = roundup(temp_size, chan->size_align);
+
+        return size;
+}
+
+static void tegra_channel_fmt_align(struct tegra_channel *chan,
+                                const struct tegra_video_format *vfmt,
+                                u32 *width, u32 *height, u32 *bytesperline)
+{
+        unsigned int min_width;
+        unsigned int max_width;
+        unsigned int min_bpl;
+        unsigned int max_bpl;
+        unsigned int temp_width;
+        unsigned int align, fmt_align;
+        unsigned int temp_bpl;
+        unsigned int bpl;
+        unsigned int numerator, denominator;
+        const struct tegra_frac *bpp = &vfmt->bpp;
+
+        /* Init, if un-init */
+        if (!*width || !*height) {
+                *width = chan->format.width;
+                *height = chan->format.height;
+        }
+
+        denominator = (!bpp->denominator) ? 1 : bpp->denominator;
+        numerator = (!bpp->numerator) ? 1 : bpp->numerator;
+
+        bpl = (*width * numerator) / denominator;
+        if (!*bytesperline)
+                *bytesperline = bpl;
+
+        /* The transfer alignment requirements are expressed in bytes. Compute
+         * the minimum and maximum values, clamp the requested width and convert
+         * it back to pixels.
+         * use denominator for base width alignment when >1.
+         * use bytesperline to adjust width for applicaton related requriements.
+         */
+        fmt_align = (denominator == 1) ? numerator : 1;
+        align = lcm(chan->width_align, fmt_align);
+        min_width = roundup(TEGRA_MIN_WIDTH, align);
+        max_width = rounddown(TEGRA_MAX_WIDTH, align);
+        temp_width = roundup(bpl, align);
+
+        *width = (clamp(temp_width, min_width, max_width) * denominator) /
+                        numerator;
+        *height = clamp(*height, TEGRA_MIN_HEIGHT, TEGRA_MAX_HEIGHT);
+
+        /* Clamp the requested bytes per line value. If the maximum bytes per
+         * line value is zero, the module doesn't support user configurable line
+         * sizes. Override the requested value with the minimum in that case.
+         */
+        min_bpl = bpl;
+        max_bpl = rounddown(TEGRA_MAX_WIDTH, chan->stride_align);
+        temp_bpl = roundup(*bytesperline, chan->stride_align);
+
+        *bytesperline = clamp(temp_bpl, min_bpl, max_bpl);
+}
+
+static void tegra_channel_update_format(struct tegra_channel *chan,
+                u32 width, u32 height, u32 fourcc,
+                const struct tegra_frac *bpp,
+                u32 preferred_stride)
+{
+        u32 denominator = (!bpp->denominator) ? 1 : bpp->denominator;
+        u32 numerator = (!bpp->numerator) ? 1 : bpp->numerator;
+        u32 bytesperline = (width * numerator / denominator);
+
+        chan->format.width = width;
+        chan->format.height = height;
+        chan->format.pixelformat = fourcc;
+        chan->format.bytesperline = preferred_stride ?: bytesperline;
+
+        tegra_channel_fmt_align(chan, chan->fmtinfo,
+                                &chan->format.width,
+                                &chan->format.height,
+                                &chan->format.bytesperline);
+
+        /* Calculate the sizeimage per plane */
+        chan->format.sizeimage = get_aligned_buffer_size(chan,
+                        chan->format.bytesperline, chan->format.height);
+
+        if (fourcc == V4L2_PIX_FMT_NV16)
+                chan->format.sizeimage *= 2;
+}
+
+static void tegra_channel_fmts_bitmap_init(struct tegra_channel *chan)
+{
+        int ret, pixel_format_index = 0, init_code = 0;
+        struct v4l2_subdev *subdev = chan->subdev_on_csi;
+        struct v4l2_subdev_format fmt = {};
+        struct v4l2_subdev_mbus_code_enum code = {
+                .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+        };
+
+        bitmap_zero(chan->fmts_bitmap, MAX_FORMAT_NUM);
+
+        /*
+         * Initialize all the formats available from
+         * the sub-device and extract the corresponding
+         * index from the pre-defined video formats and initialize
+         * the channel default format with the active code
+         * Index zero as the only sub-device is sensor
+         */
+        while (1) {
+                ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
+                                       NULL, &code);
+                if (ret < 0)
+                        /* no more formats */
+                        break;
+
+                pixel_format_index =
+                        tegra_core_get_idx_by_code(chan, code.code, 0);
+                while (pixel_format_index >= 0) {
+                        bitmap_set(chan->fmts_bitmap, pixel_format_index, 1);
+                        /* Set init_code to the first matched format */
+                        if (!init_code)
+                                init_code = code.code;
+                        /* Look for other formats with the same mbus code */
+                        pixel_format_index = tegra_core_get_idx_by_code(chan,
+                                code.code, pixel_format_index + 1);
+                }
+
+                code.index++;
+        }
+
+        if (!init_code) {
+                pixel_format_index =
+                        tegra_core_get_idx_by_code(chan, TEGRA_VF_DEF, 0);
+                if (pixel_format_index >= 0) {
+                        bitmap_set(chan->fmts_bitmap, pixel_format_index, 1);
+                        init_code = TEGRA_VF_DEF;
+                }
+        }
+                /* Get the format based on active code of the sub-device */
+        ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+        if (ret)
+                return;
+
+        /* Initiate the channel format to the first matched format */
+        chan->fmtinfo =
+                tegra_core_get_format_by_code(chan, fmt.format.code, 0);
+        v4l2_fill_pix_format(&chan->format, &fmt.format);
+        tegra_channel_update_format(chan, chan->format.width,
+                                chan->format.height,
+                                chan->fmtinfo->fourcc,
+                                &chan->fmtinfo->bpp, 0);
+
+        if (chan->total_ports > 1)
+                update_gang_mode(chan);
+}
+
+/*
+ * -----------------------------------------------------------------------------
+ * Tegra channel frame setup and capture operations
+ * -----------------------------------------------------------------------------
+ */
+
+void tegra_channel_init_ring_buffer(struct tegra_channel *chan)
+{
+        chan->released_bufs = 0;
+        chan->num_buffers = 0;
+        chan->save_index = 0;
+        chan->free_index = 0;
+        chan->bfirst_fstart = false;
+}
+
+void free_ring_buffers(struct tegra_channel *chan, int frames)
+{
+        struct vb2_v4l2_buffer *vbuf;
+
+        while (frames) {
+                vbuf = chan->buffers[chan->free_index];
+
+                /* release one frame */
+                vbuf->sequence = chan->sequence++;
+                vbuf->field = V4L2_FIELD_NONE;
+                vb2_set_plane_payload(&vbuf->vb2_buf,
+                        0, chan->format.sizeimage);
+
+                /*
+                 * WAR to force buffer state if capture state is not good
+                 * WAR - After sync point timeout or error frame capture
+                 * the second buffer is intermittently frame of zeros
+                 * with no error status or padding.
+                 */
+#if 0
+                /* This will drop the first two frames. Disable for now. */
+                if (chan->capture_state != CAPTURE_GOOD ||
+                        chan->released_bufs < 2)
+                        chan->buffer_state[chan->free_index] =
+                                                VB2_BUF_STATE_ERROR;
+#endif
+                vb2_buffer_done(&vbuf->vb2_buf,
+                        chan->buffer_state[chan->free_index++]);
+
+                if (chan->free_index >= QUEUED_BUFFERS)
+                        chan->free_index = 0;
+                chan->num_buffers--;
+                chan->released_bufs++;
+                frames--;
+        }
+}
+
+static void add_buffer_to_ring(struct tegra_channel *chan,
+                                struct vb2_v4l2_buffer *vb)
+{
+        /* save the buffer to the ring first */
+        /* Mark buffer state as error before start */
+        chan->buffer_state[chan->save_index] = VB2_BUF_STATE_ERROR;
+        chan->buffers[chan->save_index++] = vb;
+        if (chan->save_index >= QUEUED_BUFFERS)
+                chan->save_index = 0;
+        chan->num_buffers++;
+}
+
+static void update_state_to_buffer(struct tegra_channel *chan, int state)
+{
+        int save_index = (chan->save_index - PREVIOUS_BUFFER_DEC_INDEX);
+
+        /* save index decrements by 2 as 3 bufs are added in ring buffer */
+        if (save_index < 0)
+                save_index += QUEUED_BUFFERS;
+        /* update state for the previous buffer */
+        chan->buffer_state[save_index] = state;
+
+        /* for timeout/error case update the current buffer state as well */
+        if (chan->capture_state != CAPTURE_GOOD)
+                chan->buffer_state[chan->save_index] = state;
+}
+
+void tegra_channel_ring_buffer(struct tegra_channel *chan,
+                                        struct vb2_v4l2_buffer *vb,
+                                        struct timespec *ts, int state)
+
+{
+        if (!chan->bfirst_fstart)
+                chan->bfirst_fstart = true;
+        else
+                update_state_to_buffer(chan, state);
+
+        /* Capture state is not GOOD, release all buffers and re-init state */
+        if (chan->capture_state != CAPTURE_GOOD) {
+                free_ring_buffers(chan, chan->num_buffers);
+                tegra_channel_init_ring_buffer(chan);
+                return;
+        } else {
+                /* update time stamp of the buffer */
+                vb->timestamp.tv_sec = ts->tv_sec;
+                vb->timestamp.tv_usec = ts->tv_nsec / NSEC_PER_USEC;
+        }
+
+        /* release buffer N at N+2 frame start event */
+        if (chan->num_buffers >= (QUEUED_BUFFERS - 1))
+                free_ring_buffers(chan, 1);
+}
+
+void tegra_channel_ec_close(struct tegra_mc_vi *vi)
+{
+        struct tegra_channel *chan;
+
+        /* clear all channles sync point fifo context */
+        list_for_each_entry(chan, &vi->vi_chans, list) {
+                memset(&chan->syncpoint_fifo[0], 0, TEGRA_CSI_BLOCKS);
+        }
+}
+
+struct tegra_channel_buffer *dequeue_buffer(struct tegra_channel *chan)
+{
+        struct tegra_channel_buffer *buf = NULL;
+
+        spin_lock(&chan->start_lock);
+        if (list_empty(&chan->capture))
+                goto done;
+
+        buf = list_entry(chan->capture.next,
+                         struct tegra_channel_buffer, queue);
+        list_del_init(&buf->queue);
+
+        /* add dequeued buffer to the ring buffer */
+        add_buffer_to_ring(chan, &buf->buf);
+done:
+        spin_unlock(&chan->start_lock);
+        return buf;
+}
+
+/*
+ * -----------------------------------------------------------------------------
+ * videobuf2 queue operations
+ * -----------------------------------------------------------------------------
+ */
+static int
+tegra_channel_queue_setup(struct vb2_queue *vq, const void *parg,
+                     unsigned int *nbuffers, unsigned int *nplanes,
+                     unsigned int sizes[], void *alloc_ctxs[])
+{
+        const struct v4l2_format *fmt = parg;
+        struct tegra_channel *chan = vb2_get_drv_priv(vq);
+        /* Make sure the image size is large enough. */
+        if (fmt && fmt->fmt.pix.sizeimage < chan->format.sizeimage)
+                return -EINVAL;
+
+        *nplanes = 1;
+
+        sizes[0] = fmt ? fmt->fmt.pix.sizeimage : chan->format.sizeimage;
+        alloc_ctxs[0] = chan->alloc_ctx;
+
+        /* Make sure minimum number of buffers are passed */
+        if (*nbuffers < (QUEUED_BUFFERS - 1))
+                *nbuffers = QUEUED_BUFFERS - 1;
+
+        return 0;
+}
+
+static int tegra_channel_buffer_prepare(struct vb2_buffer *vb)
+{
+        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+        struct tegra_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
+        struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vbuf);
+
+        buf->chan = chan;
+        vb2_set_plane_payload(&vbuf->vb2_buf, 0, chan->format.sizeimage);
+#if defined(CONFIG_VIDEOBUF2_DMA_CONTIG)
+        buf->addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+#endif
+
+        return 0;
+}
+
+static void tegra_channel_buffer_queue(struct vb2_buffer *vb)
+{
+        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+        struct tegra_channel *chan = vb2_get_drv_priv(vb->vb2_queue);
+        struct tegra_channel_buffer *buf = to_tegra_channel_buffer(vbuf);
+
+        /* for bypass mode - do nothing */
+        if (chan->bypass)
+                return;
+
+        /* Put buffer into the capture queue */
+        spin_lock(&chan->start_lock);
+        list_add_tail(&buf->queue, &chan->capture);
+        spin_unlock(&chan->start_lock);
+
+        /* Wait up kthread for capture */
+        wake_up_interruptible(&chan->start_wait);
+}
+
+/* Return all queued buffers back to videobuf2 */
+void tegra_channel_queued_buf_done(struct tegra_channel *chan,
+                                          enum vb2_buffer_state state)
+{
+        struct tegra_channel_buffer *buf, *nbuf;
+        spinlock_t *lock = &chan->start_lock;
+        struct list_head *q = &chan->capture;
+
+        spin_lock(lock);
+        list_for_each_entry_safe(buf, nbuf, q, queue) {
+                vb2_buffer_done(&buf->buf.vb2_buf, state);
+                list_del(&buf->queue);
+        }
+        spin_unlock(lock);
+}
+
+#define __tegra_channel_device_call_subdevs_all_p(v4l2_dev, sd, cond, o,\
+                f, args...)                                             \
+({                                                                      \
+        long __err = 0;                                                 \
+        long e = 0;                                                     \
+                                                                        \
+        list_for_each_entry((sd), &(v4l2_dev)->subdevs, list) {         \
+                if ((cond) && (sd)->ops->o && (sd)->ops->o->f)          \
+                        e = (sd)->ops->o->f((sd), ##args);              \
+                if (!__err && e && e != -ENOIOCTLCMD)                   \
+                        __err = e;                                      \
+                e = 0;                                                  \
+        }                                                               \
+        __err;                                                          \
+})
+
+/*
+ * Call the specified callback for all subdevs matching grp_id (if 0, then
+ * match them all), errors are ignored until the end, and the first error
+ * encountered is returned. If the callback returns an error other than 0 or
+ * -ENOIOCTLCMD, then return with that error code. Note that you cannot
+ * add or delete a subdev while walking the subdevs list.
+ */
+#define tegra_channel_device_call_all(v4l2_dev, grpid, o, f, args...)   \
+({                                                                      \
+        struct v4l2_subdev *__sd;                                       \
+        __tegra_channel_device_call_subdevs_all_p(v4l2_dev, __sd,       \
+                        !(grpid) || __sd->grp_id == (grpid), o, f,      \
+                        ##args);                                        \
+})
+
+/*
+ * -----------------------------------------------------------------------------
+ * subdevice set/unset operations
+ * -----------------------------------------------------------------------------
+ */
+int tegra_channel_set_stream(struct tegra_channel *chan, bool on)
+{
+        int num_sd;
+        int ret = 0;
+
+        if (atomic_read(&chan->is_streaming) == on)
+                return 0;
+
+        for (num_sd = chan->num_subdevs - 1; num_sd >= 0; num_sd--) {
+                struct v4l2_subdev *sd = chan->subdev[num_sd];
+                int err = 0;
+
+                err = v4l2_subdev_call(sd, video, s_stream, on);
+                if (!ret && err < 0 && err != -ENOIOCTLCMD)
+                        ret = err;
+        }
+
+        atomic_set(&chan->is_streaming, on);
+
+        return ret;
+}
+
+int tegra_channel_set_power(struct tegra_channel *chan, bool on)
+{
+        int num_sd;
+        int ret = 0;
+
+        for (num_sd = 0; num_sd < chan->num_subdevs; num_sd++) {
+                struct v4l2_subdev *sd = chan->subdev[num_sd];
+                int err = 0;
+
+                err = v4l2_subdev_call(sd, core, s_power, on);
+                if (!ret && err < 0 && err != -ENOIOCTLCMD)
+                        ret = err;
+        }
+
+        return ret;
+}
+
+static int tegra_channel_start_streaming(struct vb2_queue *vq, u32 count)
+{
+        struct tegra_channel *chan = vb2_get_drv_priv(vq);
+        struct tegra_mc_vi *vi = chan->vi;
+
+        if (vi->fops)
+                return vi->fops->vi_start_streaming(vq, count);
+        return 0;
+}
+
+static void tegra_channel_stop_streaming(struct vb2_queue *vq)
+{
+        struct tegra_channel *chan = vb2_get_drv_priv(vq);
+        struct tegra_mc_vi *vi = chan->vi;
+
+        if (vi->fops)
+                vi->fops->vi_stop_streaming(vq);
+}
+
+static const struct vb2_ops tegra_channel_queue_qops = {
+        .queue_setup = tegra_channel_queue_setup,
+        .buf_prepare = tegra_channel_buffer_prepare,
+        .buf_queue = tegra_channel_buffer_queue,
+        .wait_prepare = vb2_ops_wait_prepare,
+        .wait_finish = vb2_ops_wait_finish,
+        .start_streaming = tegra_channel_start_streaming,
+        .stop_streaming = tegra_channel_stop_streaming,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int
+tegra_channel_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+
+        cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+        cap->device_caps |= V4L2_CAP_EXT_PIX_FORMAT;
+        cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+        strlcpy(cap->driver, "tegra-video", sizeof(cap->driver));
+        strlcpy(cap->card, chan->video.name, sizeof(cap->card));
+        snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s:%u",
+                 dev_name(chan->vi->dev), chan->port[0]);
+
+        return 0;
+}
+
+static int
+tegra_channel_enum_framesizes(struct file *file, void *fh,
+                              struct v4l2_frmsizeenum *sizes)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        struct v4l2_subdev_frame_size_enum fse = {
+                .index = sizes->index,
+                .code = sizes->pixel_format,
+        };
+        int ret = 0;
+
+        ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
+                        chan->grp_id, pad, enum_frame_size, NULL, &fse);
+        if (!ret) {
+                sizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+                sizes->discrete.width = fse.max_width;
+                sizes->discrete.height = fse.max_height;
+        }
+
+        return ret;
+}
+
+static int
+tegra_channel_enum_frameintervals(struct file *file, void *fh,
+                              struct v4l2_frmivalenum *intervals)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        struct v4l2_subdev_frame_interval_enum fie = {
+                .index = intervals->index,
+                .code = intervals->pixel_format,
+                .width = intervals->width,
+                .height = intervals->height,
+        };
+        int ret = 0;
+
+        ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
+                        chan->grp_id, pad, enum_frame_interval, NULL, &fie);
+
+        if (!ret) {
+                intervals->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+                intervals->discrete.numerator = fie.interval.numerator;
+                intervals->discrete.denominator = fie.interval.denominator;
+        }
+
+        return ret;
+}
+
+static int
+tegra_channel_enum_format(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        unsigned int index = 0, i;
+        unsigned long *fmts_bitmap = chan->fmts_bitmap;
+
+        if (f->index >= bitmap_weight(fmts_bitmap, MAX_FORMAT_NUM))
+                return -EINVAL;
+
+        for (i = 0; i < f->index + 1; i++, index++)
+                index = find_next_bit(fmts_bitmap, MAX_FORMAT_NUM, index);
+
+        index -= 1;
+        f->pixelformat = tegra_core_get_fourcc_by_idx(chan, index);
+        tegra_core_get_description_by_idx(chan, index, f->description);
+
+        return 0;
+}
+
+static int
+tegra_channel_g_edid(struct file *file, void *fh, struct v4l2_edid *edid)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        struct v4l2_subdev *sd = chan->subdev_on_csi;
+
+        if (!v4l2_subdev_has_op(sd, pad, get_edid))
+                return -ENOTTY;
+
+        return v4l2_subdev_call(sd, pad, get_edid, edid);
+}
+
+static int
+tegra_channel_s_edid(struct file *file, void *fh, struct v4l2_edid *edid)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        struct v4l2_subdev *sd = chan->subdev_on_csi;
+
+        if (!v4l2_subdev_has_op(sd, pad, set_edid))
+                return -ENOTTY;
+
+        return v4l2_subdev_call(sd, pad, set_edid, edid);
+}
+
+static int
+tegra_channel_g_dv_timings(struct file *file, void *fh,
+                struct v4l2_dv_timings *timings)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+
+        if (!v4l2_subdev_has_op(chan->subdev_on_csi, video, g_dv_timings))
+                return -ENOTTY;
+
+        return v4l2_device_call_until_err(chan->video.v4l2_dev,
+                        chan->grp_id, video, g_dv_timings, timings);
+}
+
+static int
+tegra_channel_s_dv_timings(struct file *file, void *fh,
+                struct v4l2_dv_timings *timings)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        struct v4l2_bt_timings *bt = &timings->bt;
+        struct v4l2_dv_timings curr_timings;
+        int ret;
+
+        if (!v4l2_subdev_has_op(chan->subdev_on_csi, video, s_dv_timings))
+                return -ENOTTY;
+
+        ret = tegra_channel_g_dv_timings(file, fh, &curr_timings);
+        if (ret)
+                return ret;
+
+        if (v4l2_match_dv_timings(timings, &curr_timings, 0))
+                return 0;
+
+        if (vb2_is_busy(&chan->queue))
+                return -EBUSY;
+
+        ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
+                        chan->grp_id, video, s_dv_timings, timings);
+
+        if (!ret)
+                tegra_channel_update_format(chan, bt->width, bt->height,
+                        chan->fmtinfo->fourcc, &chan->fmtinfo->bpp, 0);
+
+        if (chan->total_ports > 1)
+                update_gang_mode(chan);
+
+        return ret;
+}
+
+static int
+tegra_channel_query_dv_timings(struct file *file, void *fh,
+                struct v4l2_dv_timings *timings)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+
+        if (!v4l2_subdev_has_op(chan->subdev_on_csi, video, query_dv_timings))
+                return -ENOTTY;
+
+        return v4l2_device_call_until_err(chan->video.v4l2_dev,
+                        chan->grp_id, video, query_dv_timings, timings);
+}
+
+static int
+tegra_channel_enum_dv_timings(struct file *file, void *fh,
+                struct v4l2_enum_dv_timings *timings)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        struct v4l2_subdev *sd = chan->subdev_on_csi;
+
+        if (!v4l2_subdev_has_op(sd, pad, enum_dv_timings))
+                return -ENOTTY;
+
+        return v4l2_subdev_call(sd, pad, enum_dv_timings, timings);
+}
+
+static int
+tegra_channel_dv_timings_cap(struct file *file, void *fh,
+                struct v4l2_dv_timings_cap *cap)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        struct v4l2_subdev *sd = chan->subdev_on_csi;
+
+        if (!v4l2_subdev_has_op(sd, pad, dv_timings_cap))
+                return -ENOTTY;
+
+        return v4l2_subdev_call(sd, pad, dv_timings_cap, cap);
+}
+
+int tegra_channel_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct tegra_channel *chan = container_of(ctrl->handler,
+                                struct tegra_channel, ctrl_handler);
+
+        switch (ctrl->id) {
+        case V4L2_CID_VI_BYPASS_MODE:
+                if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON)
+                        chan->bypass = true;
+                else if (chan->vi->bypass) {
+                        dev_dbg(&chan->video.dev,
+                                "can't disable bypass mode\n");
+                        dev_dbg(&chan->video.dev,
+                                "because the VI/CSI is in bypass mode\n");
+                        chan->bypass = true;
+                } else
+                        chan->bypass = false;
+                break;
+        case V4L2_CID_OVERRIDE_ENABLE:
+                {
+                        struct v4l2_subdev *sd = chan->subdev_on_csi;
+                        struct i2c_client *client = v4l2_get_subdevdata(sd);
+                        struct camera_common_data *s_data =
+                                to_camera_common_data(client);
+
+                        if (!i2c_get_clientdata(client)) {
+                                dev_info(&chan->video.dev,
+                                         "i2c_client drvdata is NULL\n");
+                                break;
+                        }
+                        if (!s_data)
+                                break;
+                        if (switch_ctrl_qmenu[ctrl->val] == SWITCH_ON) {
+                                s_data->override_enable = true;
+                                dev_dbg(&chan->video.dev,
+                                        "enable override control\n");
+                        } else {
+                                s_data->override_enable = false;
+                                dev_dbg(&chan->video.dev,
+                                        "disable override control\n");
+                        }
+                }
+                break;
+        case V4L2_CID_VI_HEIGHT_ALIGN:
+                chan->height_align = ctrl->val;
+                tegra_channel_update_format(chan, chan->format.width,
+                                chan->format.height,
+                                chan->format.pixelformat,
+                                &chan->fmtinfo->bpp, 0);
+                break;
+        case V4L2_CID_VI_SIZE_ALIGN:
+                chan->size_align = size_align_ctrl_qmenu[ctrl->val];
+                tegra_channel_update_format(chan, chan->format.width,
+                                chan->format.height,
+                                chan->format.pixelformat,
+                                &chan->fmtinfo->bpp, 0);
+                break;
+        default:
+                dev_err(&chan->video.dev, "%s:Not valid ctrl\n", __func__);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static const struct v4l2_ctrl_ops channel_ctrl_ops = {
+        .s_ctrl = tegra_channel_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config common_custom_ctrls[] = {
+        {
+                .ops = &channel_ctrl_ops,
+                .id = V4L2_CID_VI_BYPASS_MODE,
+                .name = "Bypass Mode",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .def = 0,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &channel_ctrl_ops,
+                .id = V4L2_CID_OVERRIDE_ENABLE,
+                .name = "Override Enable",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .def = 0,
+                .min = 0,
+                .max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .qmenu_int = switch_ctrl_qmenu,
+        },
+        {
+                .ops = &channel_ctrl_ops,
+                .id = V4L2_CID_VI_HEIGHT_ALIGN,
+                .name = "Height Align",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .min = 1,
+                .max = 16,
+                .step = 1,
+                .def = 1,
+        },
+        {
+                .ops = &channel_ctrl_ops,
+                .id = V4L2_CID_VI_SIZE_ALIGN,
+                .name = "Size Align",
+                .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+                .def = 0,
+                .min = 0,
+                .max = ARRAY_SIZE(size_align_ctrl_qmenu) - 1,
+                .menu_skip_mask = 0,
+                .qmenu_int = size_align_ctrl_qmenu,
+        },
+};
+
+static int tegra_channel_setup_controls(struct tegra_channel *chan)
+{
+        int num_sd = 0;
+        struct v4l2_subdev *sd = NULL;
+        struct tegra_mc_vi *vi = chan->vi;
+        int i;
+
+        /* Initialize the subdev and controls here at first open */
+        sd = chan->subdev[num_sd];
+        while ((sd = chan->subdev[num_sd++]) &&
+                (num_sd <= chan->num_subdevs)) {
+                /* Add control handler for the subdevice */
+                v4l2_ctrl_add_handler(&chan->ctrl_handler,
+                                        sd->ctrl_handler, NULL);
+                if (chan->ctrl_handler.error)
+                        dev_err(chan->vi->dev,
+                                "Failed to add sub-device controls\n");
+        }
+
+        /* Add new custom controls */
+        for (i = 0; i < ARRAY_SIZE(common_custom_ctrls); i++) {
+                /* don't create override control for pg mode */
+                if (common_custom_ctrls[i].id == V4L2_CID_OVERRIDE_ENABLE &&
+                        chan->pg_mode)
+                        continue;
+                v4l2_ctrl_new_custom(&chan->ctrl_handler,
+                        &common_custom_ctrls[i], NULL);
+                if (chan->ctrl_handler.error) {
+                        dev_err(chan->vi->dev,
+                                "Failed to add %s ctrl\n",
+                                common_custom_ctrls[i].name);
+                        return chan->ctrl_handler.error;
+                }
+        }
+
+        vi->fops->vi_add_ctrls(chan);
+
+        if (chan->pg_mode) {
+                v4l2_ctrl_add_handler(&chan->ctrl_handler,
+                                        &chan->vi->ctrl_handler, NULL);
+                if (chan->ctrl_handler.error)
+                        dev_err(chan->vi->dev,
+                                "Failed to add VI controls\n");
+        }
+
+        /* setup the controls */
+        return v4l2_ctrl_handler_setup(&chan->ctrl_handler);
+}
+
+int tegra_channel_init_subdevices(struct tegra_channel *chan)
+{
+        struct media_entity *entity;
+        struct media_pad *pad;
+        struct v4l2_subdev *sd;
+        int index = 0;
+        int num_sd = 0;
+        int grp_id = chan->pg_mode ? (TPG_CSI_GROUP_ID + chan->port[0] + 1)
+                : chan->port[0] + 1;
+
+        /* set_stream of CSI */
+        pad = media_entity_remote_pad(&chan->pad);
+        if (!pad)
+                return -ENODEV;
+
+        entity = pad->entity;
+        sd = media_entity_to_v4l2_subdev(entity);
+        v4l2_set_subdev_hostdata(sd, chan);
+        chan->subdev[num_sd++] = sd;
+        /* Add subdev name to this video dev name with vi-output tag*/
+        snprintf(chan->video.name, sizeof(chan->video.name), "%s, %s",
+                "vi-output", sd->name);
+        sd->grp_id = grp_id;
+        chan->grp_id = grp_id;
+        index = pad->index - 1;
+        while (index >= 0) {
+                pad = &entity->pads[index];
+                if (!(pad->flags & MEDIA_PAD_FL_SINK))
+                        break;
+
+                pad = media_entity_remote_pad(pad);
+                if (pad == NULL ||
+                    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+                        break;
+
+                if (num_sd >= MAX_SUBDEVICES)
+                        break;
+
+                entity = pad->entity;
+                sd = media_entity_to_v4l2_subdev(entity);
+                v4l2_set_subdev_hostdata(sd, chan);
+                sd->grp_id = grp_id;
+                chan->subdev[num_sd++] = sd;
+                /* Add subdev name to this video dev name with vi-output tag*/
+                snprintf(chan->video.name, sizeof(chan->video.name), "%s, %s",
+                        "vi-output", sd->name);
+
+                index = pad->index - 1;
+        }
+        chan->num_subdevs = num_sd;
+        /*
+         * Each CSI channel has only one final remote source,
+         * Mark that subdev as subdev_on_csi
+         */
+        chan->subdev_on_csi = sd;
+
+        /* initialize the available formats */
+        if (chan->num_subdevs)
+                tegra_channel_fmts_bitmap_init(chan);
+
+        return tegra_channel_setup_controls(chan);
+}
+
+static int
+tegra_channel_get_format(struct file *file, void *fh,
+                        struct v4l2_format *format)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        struct v4l2_pix_format *pix = &format->fmt.pix;
+
+        *pix = chan->format;
+
+        return 0;
+}
+
+static int
+__tegra_channel_try_format(struct tegra_channel *chan,
+                        struct v4l2_pix_format *pix)
+{
+        const struct tegra_video_format *vfmt;
+        struct v4l2_subdev_format fmt;
+        struct v4l2_subdev *sd = chan->subdev_on_csi;
+        int ret = 0;
+
+        /* Use the channel format if pixformat is not supported */
+        vfmt = tegra_core_get_format_by_fourcc(chan, pix->pixelformat);
+        if (!vfmt) {
+                pix->pixelformat = chan->format.pixelformat;
+                vfmt = tegra_core_get_format_by_fourcc(chan, pix->pixelformat);
+        }
+
+        fmt.which = V4L2_SUBDEV_FORMAT_TRY;
+        fmt.pad = 0;
+        v4l2_fill_mbus_format(&fmt.format, pix, vfmt->code);
+
+        ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
+        if (ret == -ENOIOCTLCMD)
+                return -ENOTTY;
+
+        v4l2_fill_pix_format(pix, &fmt.format);
+
+        tegra_channel_fmt_align(chan, vfmt,
+                                &pix->width, &pix->height, &pix->bytesperline);
+        pix->sizeimage = get_aligned_buffer_size(chan,
+                        pix->bytesperline, pix->height);
+        if (chan->fmtinfo->fourcc == V4L2_PIX_FMT_NV16)
+                pix->sizeimage *= 2;
+
+        return ret;
+}
+
+static int
+tegra_channel_try_format(struct file *file, void *fh,
+                        struct v4l2_format *format)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+
+        return  __tegra_channel_try_format(chan, &format->fmt.pix);
+}
+
+static int
+__tegra_channel_set_format(struct tegra_channel *chan,
+                        struct v4l2_pix_format *pix)
+{
+        const struct tegra_video_format *vfmt;
+        struct v4l2_subdev_format fmt;
+        struct v4l2_subdev *sd = chan->subdev_on_csi;
+        int ret = 0;
+
+        vfmt = tegra_core_get_format_by_fourcc(chan, pix->pixelformat);
+
+        fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+        fmt.pad = 0;
+        v4l2_fill_mbus_format(&fmt.format, pix, vfmt->code);
+
+        ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
+        if (ret == -ENOIOCTLCMD)
+                return -ENOTTY;
+
+        v4l2_fill_pix_format(pix, &fmt.format);
+
+        if (!ret) {
+                chan->format = *pix;
+                chan->fmtinfo = vfmt;
+                tegra_channel_update_format(chan, pix->width,
+                        pix->height, vfmt->fourcc, &vfmt->bpp,
+                        pix->bytesperline);
+
+                *pix = chan->format;
+
+                if (chan->total_ports > 1)
+                        update_gang_mode(chan);
+        }
+
+        return ret;
+}
+
+static int
+tegra_channel_set_format(struct file *file, void *fh,
+                        struct v4l2_format *format)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        int ret = 0;
+
+        /* get the suppod format by try_fmt */
+        ret = __tegra_channel_try_format(chan, &format->fmt.pix);
+        if (ret)
+                return ret;
+
+        if (vb2_is_busy(&chan->queue))
+                return -EBUSY;
+
+        return __tegra_channel_set_format(chan, &format->fmt.pix);
+}
+
+static int tegra_channel_subscribe_event(struct v4l2_fh *fh,
+                                  const struct v4l2_event_subscription *sub)
+{
+        switch (sub->type) {
+        case V4L2_EVENT_SOURCE_CHANGE:
+                return v4l2_event_subscribe(fh, sub, 4, NULL);
+        }
+        return v4l2_ctrl_subscribe_event(fh, sub);
+}
+
+static int
+tegra_channel_enum_input(struct file *file, void *fh, struct v4l2_input *inp)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        struct v4l2_subdev *sd_on_csi = chan->subdev_on_csi;
+        int ret;
+
+        if (inp->index)
+                return -EINVAL;
+
+        ret = v4l2_device_call_until_err(chan->video.v4l2_dev,
+                        chan->grp_id, video, g_input_status, &inp->status);
+
+        if (ret == -ENODEV || sd_on_csi == NULL)
+                return -ENODEV;
+
+        inp->type = V4L2_INPUT_TYPE_CAMERA;
+        if (v4l2_subdev_has_op(sd_on_csi, video, s_dv_timings)) {
+                inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
+                snprintf(inp->name,
+                        sizeof(inp->name), "HDMI %u",
+                        chan->port[0]);
+        } else
+                snprintf(inp->name,
+                        sizeof(inp->name), "Camera %u",
+                        chan->port[0]);
+
+        return ret;
+}
+
+static int tegra_channel_g_input(struct file *file, void *priv, unsigned int *i)
+{
+        *i = 0;
+        return 0;
+}
+
+static int tegra_channel_s_input(struct file *file, void *priv, unsigned int i)
+{
+        if (i > 0)
+                return -EINVAL;
+        return 0;
+}
+
+static int tegra_channel_log_status(struct file *file, void *priv)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+
+        v4l2_device_call_all(chan->video.v4l2_dev,
+                chan->grp_id, core, log_status);
+        return 0;
+}
+
+static long tegra_channel_default_ioctl(struct file *file, void *fh,
+                        bool use_prio, unsigned int cmd, void *arg)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        struct tegra_mc_vi *vi = chan->vi;
+        long ret = 0;
+
+        if (vi->fops && vi->fops->vi_default_ioctl)
+                ret = vi->fops->vi_default_ioctl(file, fh, use_prio, cmd, arg);
+
+        return ret;
+}
+
+#ifdef CONFIG_COMPAT
+static long tegra_channel_compat_ioctl(struct file *filp,
+               unsigned int cmd, unsigned long arg)
+{
+        struct video_device *vdev = video_devdata(filp);
+        int ret = -ENODEV;
+
+        if (vdev->fops->unlocked_ioctl) {
+                struct mutex *lock = v4l2_ioctl_get_lock(vdev, cmd);
+
+                if (lock && mutex_lock_interruptible(lock))
+                        return -ERESTARTSYS;
+                if (video_is_registered(vdev))
+                        ret = vdev->fops->unlocked_ioctl(filp, cmd, arg);
+                if (lock)
+                        mutex_unlock(lock);
+        } else
+                ret = -ENOTTY;
+
+        return ret;
+}
+#endif
+
+static const struct v4l2_ioctl_ops tegra_channel_ioctl_ops = {
+        .vidioc_querycap                = tegra_channel_querycap,
+        .vidioc_enum_framesizes         = tegra_channel_enum_framesizes,
+        .vidioc_enum_frameintervals     = tegra_channel_enum_frameintervals,
+        .vidioc_enum_fmt_vid_cap        = tegra_channel_enum_format,
+        .vidioc_g_fmt_vid_cap           = tegra_channel_get_format,
+        .vidioc_s_fmt_vid_cap           = tegra_channel_set_format,
+        .vidioc_try_fmt_vid_cap         = tegra_channel_try_format,
+        .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
+        .vidioc_querybuf                = vb2_ioctl_querybuf,
+        .vidioc_qbuf                    = vb2_ioctl_qbuf,
+        .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
+        .vidioc_create_bufs             = vb2_ioctl_create_bufs,
+        .vidioc_expbuf                  = vb2_ioctl_expbuf,
+        .vidioc_streamon                = vb2_ioctl_streamon,
+        .vidioc_streamoff               = vb2_ioctl_streamoff,
+        .vidioc_g_edid                  = tegra_channel_g_edid,
+        .vidioc_s_edid                  = tegra_channel_s_edid,
+        .vidioc_s_dv_timings            = tegra_channel_s_dv_timings,
+        .vidioc_g_dv_timings            = tegra_channel_g_dv_timings,
+        .vidioc_query_dv_timings        = tegra_channel_query_dv_timings,
+        .vidioc_enum_dv_timings         = tegra_channel_enum_dv_timings,
+        .vidioc_dv_timings_cap          = tegra_channel_dv_timings_cap,
+        .vidioc_subscribe_event         = tegra_channel_subscribe_event,
+        .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
+        .vidioc_enum_input              = tegra_channel_enum_input,
+        .vidioc_g_input                 = tegra_channel_g_input,
+        .vidioc_s_input                 = tegra_channel_s_input,
+        .vidioc_log_status              = tegra_channel_log_status,
+        .vidioc_default                 = tegra_channel_default_ioctl,
+};
+
+static int tegra_channel_close(struct file *fp);
+static int tegra_channel_open(struct file *fp)
+{
+        int ret;
+        struct video_device *vdev = video_devdata(fp);
+        struct tegra_channel *chan = video_get_drvdata(vdev);
+#ifdef T210
+        struct vi *tegra_vi;
+#endif
+        struct tegra_mc_vi *vi;
+        struct tegra_csi_device *csi;
+
+        mutex_lock(&chan->video_lock);
+        ret = v4l2_fh_open(fp);
+        if (ret || !v4l2_fh_is_singular_file(fp)) {
+                mutex_unlock(&chan->video_lock);
+                return ret;
+        }
+
+        if (chan->subdev[0] == NULL) {
+                ret = -ENODEV;
+                goto fail;
+        }
+
+        vi = chan->vi;
+#ifdef T210
+        tegra_vi = vi->vi;
+#endif
+        csi = vi->csi;
+
+        /* The first open then turn on power */
+        if (vi->fops)
+                ret = vi->fops->vi_power_on(chan);
+        if (ret < 0)
+                goto fail;
+
+        chan->fh = (struct v4l2_fh *)fp->private_data;
+
+        mutex_unlock(&chan->video_lock);
+        return 0;
+
+fail:
+        tegra_channel_close(fp);
+        mutex_unlock(&chan->video_lock);
+        return ret;
+}
+
+static int tegra_channel_close(struct file *fp)
+{
+        int ret = 0;
+        struct video_device *vdev = video_devdata(fp);
+        struct tegra_channel *chan = video_get_drvdata(vdev);
+        struct tegra_mc_vi *vi = chan->vi;
+        bool is_singular;
+
+        mutex_lock(&chan->video_lock);
+        is_singular = v4l2_fh_is_singular_file(fp);
+        ret = _vb2_fop_release(fp, NULL);
+
+        if (!is_singular) {
+                mutex_unlock(&chan->video_lock);
+                return ret;
+        }
+        vi->fops->vi_power_off(chan);
+
+        mutex_unlock(&chan->video_lock);
+        return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 file operations
+ */
+static const struct v4l2_file_operations tegra_channel_fops = {
+        .owner          = THIS_MODULE,
+        .unlocked_ioctl = video_ioctl2,
+#ifdef CONFIG_COMPAT
+        .compat_ioctl32 = tegra_channel_compat_ioctl,
+#endif
+        .open           = tegra_channel_open,
+        .release        = tegra_channel_close,
+        .read           = vb2_fop_read,
+        .poll           = vb2_fop_poll,
+        .mmap           = vb2_fop_mmap,
+};
+
+static int tegra_channel_csi_init(struct tegra_channel *chan)
+{
+        int numlanes = 0;
+        int idx = 0;
+        struct tegra_mc_vi *vi = chan->vi;
+        int ret = 0;
+
+        chan->gang_mode = CAMERA_NO_GANG_MODE;
+        chan->total_ports = 0;
+        memset(&chan->port[0], INVALID_CSI_PORT, TEGRA_CSI_BLOCKS);
+        memset(&chan->syncpoint_fifo[0], 0, TEGRA_CSI_BLOCKS);
+        if (chan->pg_mode) {
+                /* If VI has 4 existing channels, chan->id will start
+                 * from 4 for the first TPG channel, which uses PORT_A(0).
+                 * To get the correct PORT number, subtract existing number of
+                 * channels from chan->id.
+                 */
+                chan->port[0] = chan->id - vi->num_channels;
+                WARN_ON(chan->port[0] > TPG_CHANNELS);
+                chan->numlanes = 2;
+        } else {
+                ret = tegra_vi_get_port_info(chan, vi->dev->of_node, chan->id);
+                if (ret) {
+                        dev_err(vi->dev, "%s:Fail to parse port info\n",
+                                        __func__);
+                        return ret;
+                }
+        }
+
+        for (idx = 0; csi_port_is_valid(chan->port[idx]); idx++) {
+                chan->total_ports++;
+                numlanes = chan->numlanes - (idx * MAX_CSI_BLOCK_LANES);
+                numlanes = numlanes > MAX_CSI_BLOCK_LANES ?
+                        MAX_CSI_BLOCK_LANES : numlanes;
+                /* maximum of 4 lanes are present per CSI block */
+                chan->csibase[idx] = vi->iomem +
+                                        TEGRA_VI_CSI_BASE(chan->port[idx]);
+#ifdef T210
+                set_csi_portinfo(vi->csi, chan->port[idx], numlanes);
+#endif
+        }
+        /* based on gang mode valid ports will be updated - set default to 1 */
+        chan->valid_ports = chan->total_ports ? 1 : 0;
+        return ret;
+}
+
+int tegra_channel_init(struct tegra_channel *chan)
+{
+        int ret;
+        struct tegra_mc_vi *vi = chan->vi;
+
+#ifdef T210
+        chan->fops = vi->vi->data->channel_fops;
+#endif
+        ret = tegra_channel_csi_init(chan);
+        if (ret)
+                return ret;
+
+        chan->width_align = TEGRA_WIDTH_ALIGNMENT;
+        chan->stride_align = TEGRA_STRIDE_ALIGNMENT;
+        chan->num_subdevs = 0;
+        mutex_init(&chan->video_lock);
+        INIT_LIST_HEAD(&chan->capture);
+        init_waitqueue_head(&chan->start_wait);
+        spin_lock_init(&chan->start_lock);
+        mutex_init(&chan->stop_kthread_lock);
+        atomic_set(&chan->is_streaming, DISABLE);
+        spin_lock_init(&chan->capture_state_lock);
+
+        /* Init video format */
+        vi->fops->vi_init_video_formats(chan);
+        chan->fmtinfo = tegra_core_get_default_format();
+        tegra_channel_update_format(chan, TEGRA_DEF_WIDTH,
+                                TEGRA_DEF_HEIGHT,
+                                chan->fmtinfo->fourcc,
+                                &chan->fmtinfo->bpp, 0);
+
+        chan->buffer_offset[0] = 0;
+
+        /* Initialize the media entity... */
+        chan->pad.flags = MEDIA_PAD_FL_SINK;
+
+        ret = media_entity_init(&chan->video.entity, 1, &chan->pad, 0);
+        if (ret < 0) {
+                dev_err(&chan->video.dev, "failed to init video entity\n");
+                return ret;
+        }
+
+        /* init control handler */
+        ret = v4l2_ctrl_handler_init(&chan->ctrl_handler, MAX_CID_CONTROLS);
+        if (chan->ctrl_handler.error) {
+                dev_err(&chan->video.dev, "failed to init control handler\n");
+                goto ctrl_init_error;
+        }
+
+        /* init video node... */
+        chan->video.fops = &tegra_channel_fops;
+        chan->video.v4l2_dev = &vi->v4l2_dev;
+        chan->video.queue = &chan->queue;
+        snprintf(chan->video.name, sizeof(chan->video.name), "%s-%s-%u",
+                dev_name(vi->dev), chan->pg_mode ? "tpg" : "output",
+                chan->port[0]);
+        chan->video.vfl_type = VFL_TYPE_GRABBER;
+        chan->video.vfl_dir = VFL_DIR_RX;
+        chan->video.release = video_device_release_empty;
+        chan->video.ioctl_ops = &tegra_channel_ioctl_ops;
+        chan->video.ctrl_handler = &chan->ctrl_handler;
+        chan->video.lock = &chan->video_lock;
+
+        set_bit(_IOC_NR(VIDIOC_G_PRIORITY), chan->video.valid_ioctls);
+        set_bit(_IOC_NR(VIDIOC_S_PRIORITY), chan->video.valid_ioctls);
+
+        video_set_drvdata(&chan->video, chan);
+
+#if defined(CONFIG_VIDEOBUF2_DMA_CONTIG)
+        /* get the buffers queue... */
+        chan->alloc_ctx = vb2_dma_contig_init_ctx(chan->vi->dev);
+        if (IS_ERR(chan->alloc_ctx)) {
+                dev_err(chan->vi->dev, "failed to init vb2 buffer\n");
+                ret = -ENOMEM;
+                goto vb2_init_error;
+        }
+#endif
+
+        chan->queue.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        chan->queue.io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ | VB2_USERPTR;
+        chan->queue.lock = &chan->video_lock;
+        chan->queue.drv_priv = chan;
+        chan->queue.buf_struct_size = sizeof(struct tegra_channel_buffer);
+        chan->queue.ops = &tegra_channel_queue_qops;
+#if defined(CONFIG_VIDEOBUF2_DMA_CONTIG)
+        chan->queue.mem_ops = &vb2_dma_contig_memops;
+#endif
+        chan->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
+                                   | V4L2_BUF_FLAG_TSTAMP_SRC_EOF;
+        ret = vb2_queue_init(&chan->queue);
+        if (ret < 0) {
+                dev_err(chan->vi->dev, "failed to initialize VB2 queue\n");
+                goto vb2_queue_error;
+        }
+
+        chan->init_done = true;
+        return 0;
+
+vb2_queue_error:
+#if defined(CONFIG_VIDEOBUF2_DMA_CONTIG)
+        vb2_dma_contig_cleanup_ctx(chan->alloc_ctx);
+vb2_init_error:
+#endif
+        v4l2_ctrl_handler_free(&chan->ctrl_handler);
+ctrl_init_error:
+        media_entity_cleanup(&chan->video.entity);
+        return ret;
+}
+
+int tegra_channel_cleanup(struct tegra_channel *chan)
+{
+        v4l2_ctrl_handler_free(&chan->ctrl_handler);
+        vb2_queue_release(&chan->queue);
+#if defined(CONFIG_VIDEOBUF2_DMA_CONTIG)
+        vb2_dma_contig_cleanup_ctx(chan->alloc_ctx);
+#endif
+
+        media_entity_cleanup(&chan->video.entity);
+
+        return 0;
+}
+
+int tegra_vi_channels_register(struct tegra_mc_vi *vi)
+{
+        int ret = 0;
+        struct tegra_channel *it;
+        int count = 0;
+
+        list_for_each_entry(it, &vi->vi_chans, list) {
+                if (!it->init_done)
+                        continue;
+                ret = video_register_device(&it->video, VFL_TYPE_GRABBER, -1);
+                if (ret < 0) {
+                        dev_err(&it->video.dev, "failed to register %s\n",
+                                it->video.name);
+                        continue;
+                }
+                count++;
+        }
+
+        if (count == 0) {
+                dev_err(vi->dev, "all channel register failed\n");
+                return ret;
+        }
+
+        return 0;
+}
+
+void tegra_vi_channels_unregister(struct tegra_mc_vi *vi)
+{
+        struct tegra_channel *it;
+
+        list_for_each_entry(it, &vi->vi_chans, list) {
+                if (it->video.cdev != NULL)
+                      video_unregister_device(&it->video);
+        }
+}
+
+int tegra_vi_channels_init(struct tegra_mc_vi *vi)
+{
+        int ret = 0;
+        struct tegra_channel *it;
+        int count = 0;
+
+        list_for_each_entry(it, &vi->vi_chans, list) {
+                it->vi = vi;
+                ret = tegra_channel_init(it);
+                if (ret < 0) {
+                        dev_err(vi->dev, "channel init failed\n");
+                        continue;
+                }
+                count++;
+        }
+
+        if (count == 0) {
+                dev_err(vi->dev, "all channel init failed\n");
+                return ret;
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL(tegra_vi_channels_init);
+
+int tegra_vi_channels_cleanup(struct tegra_mc_vi *vi)
+{
+        int ret = 0, err = 0;
+        struct tegra_channel *it;
+
+        list_for_each_entry(it, &vi->vi_chans, list) {
+                if (!it->init_done)
+                        continue;
+                err = tegra_channel_cleanup(it);
+                if (err < 0) {
+                        ret = err;
+                        dev_err(vi->dev, "channel cleanup failed, err %d\n",
+                                        err);
+                }
+        }
+        return ret;
+}
+EXPORT_SYMBOL(tegra_vi_channels_cleanup);
+
+int tegra_clean_unlinked_channels(struct tegra_mc_vi *vi)
+{
+        int ret, err = 0;
+        struct tegra_channel *chan;
+
+        list_for_each_entry(chan, &vi->vi_chans, list) {
+                struct v4l2_subdev *sd = chan->subdev_on_csi;
+                bool is_csi = false;
+
+                /*
+                 * If subdevice on csi is csi itself,
+                 * then sensor subdevice is not connected
+                 */
+                if (sd)
+                        is_csi = strstr(sd->name, "nvcsi") != NULL;
+
+                if (chan->num_subdevs && !is_csi)
+                        continue;
+
+                ret = tegra_channel_cleanup(chan);
+                if (ret < 0) {
+                        err = ret;
+                        dev_err(vi->dev, "channel cleanup failed, err %d\n",
+                                        err);
+                }
+        }
+
+        return err;
+}
+EXPORT_SYMBOL(tegra_clean_unlinked_channels);
diff --git a/drivers/media/platform/tegra/camera/vi/core.c b/drivers/media/platform/tegra/camera/vi/core.c
new file mode 100644
index 000000000..97de67085
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/core.c
@@ -0,0 +1,173 @@
+/*
+ * NVIDIA Tegra Video Input Device Driver Core Helpers
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/export.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+#include "camera/vi/mc_common.h"
+
+static const struct tegra_video_format tegra_default_format[] = {
+        {
+                TEGRA_VF_DEF,
+                10,
+                MEDIA_BUS_FMT_SRGGB10_1X10,
+                {2, 1},
+                TEGRA_IMAGE_FORMAT_DEF,
+                TEGRA_IMAGE_DT_RAW10,
+                V4L2_PIX_FMT_SRGGB10,
+                "RGRG.. GBGB..",
+        },
+};
+
+/* -----------------------------------------------------------------------------
+ * Helper functions
+ */
+
+/**
+ * tegra_core_get_fourcc_by_idx - get fourcc of a tegra_video format
+ * @index: array index of the tegra_video_formats
+ *
+ * Return: fourcc code
+ */
+u32 tegra_core_get_fourcc_by_idx(struct tegra_channel *chan,
+                                unsigned int index)
+{
+        /* return default fourcc format if the index out of bounds */
+        if (index > (chan->num_video_formats - 1))
+                return V4L2_PIX_FMT_SGRBG10;
+
+        return chan->video_formats[index]->fourcc;
+}
+
+/**
+ * tegra_core_get_description_by_idx - get description of a tegra_video format
+ * @index: array index of the tegra_video_formats
+ */
+void tegra_core_get_description_by_idx(struct tegra_channel *chan,
+                        unsigned int index, __u8 *description)
+{
+        if (index > (chan->num_video_formats - 1))
+                return;
+
+        if (description)
+                strlcpy(description,
+                        chan->video_formats[index]->description,
+                        sizeof(chan->video_formats[index]->description));
+}
+
+/**
+ * tegra_core_get_word_count - Calculate word count
+ * @frame_width: number of pixels per line
+ * @fmt: Tegra Video format struct which has BPP information
+ *
+ * Return: word count number
+ */
+u32 tegra_core_get_word_count(unsigned int frame_width,
+                              const struct tegra_video_format *fmt)
+{
+        return frame_width * fmt->width / 8;
+}
+
+/**
+ * tegra_core_get_idx_by_code - Retrieve index for a media bus code
+ * @code: the format media bus code
+ *
+ * Return: a index to the format information structure corresponding to the
+ * given V4L2 media bus format @code, or -1 if no corresponding format can
+ * be found.
+ */
+int tegra_core_get_idx_by_code(struct tegra_channel *chan,
+                unsigned int code, unsigned offset)
+{
+        unsigned int i;
+
+        for (i = offset; i < chan->num_video_formats; ++i) {
+                if (chan->video_formats[i]->code == code)
+                        return i;
+        }
+
+        return -1;
+}
+
+/**
+ * tegra_core_get_default_format - Get default format
+ *
+ * Return: pointer to the format where the default format needs
+ * to be filled in.
+ */
+const struct tegra_video_format *tegra_core_get_default_format(void)
+{
+        return &tegra_default_format[0];
+}
+
+/**
+ * tegra_core_get_format_by_code - Retrieve format information for a media
+ * bus code
+ * @code: the format media bus code
+ *
+ * Return: a pointer to the format information structure corresponding to the
+ * given V4L2 media bus format @code, or NULL if no corresponding format can
+ * be found.
+ */
+const struct tegra_video_format *
+tegra_core_get_format_by_code(struct tegra_channel *chan,
+                unsigned int code, unsigned offset)
+{
+        unsigned int i;
+
+        for (i = offset; i < chan->num_video_formats; ++i) {
+                if (chan->video_formats[i]->code == code)
+                        return chan->video_formats[i];
+        }
+
+        return NULL;
+}
+
+/**
+ * tegra_core_get_format_by_fourcc - Retrieve format information for a 4CC
+ * @fourcc: the format 4CC
+ *
+ * Return: a pointer to the format information structure corresponding to the
+ * given V4L2 format @fourcc, or NULL if no corresponding format can be
+ * found.
+ */
+const struct tegra_video_format *
+tegra_core_get_format_by_fourcc(struct tegra_channel *chan, u32 fourcc)
+{
+        unsigned int i;
+
+        for (i = 0; i < chan->num_video_formats; ++i) {
+                if (chan->video_formats[i]->fourcc == fourcc)
+                        return chan->video_formats[i];
+        }
+
+        return NULL;
+}
+
+/**
+ * tegra_core_bytes_per_line - Calculate bytes per line in one frame
+ * @width: frame width
+ * @align: number of alignment bytes
+ * @fmt: Tegra Video format
+ *
+ * Simply calcualte the bytes_per_line and if it's not aligned it
+ * will be padded to alignment boundary.
+ */
+u32 tegra_core_bytes_per_line(unsigned int width, unsigned int align,
+                              const struct tegra_video_format *fmt)
+{
+        u32 value = ((width * fmt->bpp.numerator) / fmt->bpp.denominator);
+
+        return roundup(value, align);
+}
diff --git a/drivers/media/platform/tegra/camera/vi/core.h b/drivers/media/platform/tegra/camera/vi/core.h
new file mode 100644
index 000000000..56b24fd59
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/core.h
@@ -0,0 +1,127 @@
+/*
+ * NVIDIA Tegra Video Input Device Driver Core Helpers
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __TEGRA_CORE_H__
+#define __TEGRA_CORE_H__
+
+#include <media/v4l2-subdev.h>
+
+/* Minimum and maximum width and height common to Tegra video input device. */
+#define TEGRA_MIN_WIDTH         32U
+#define TEGRA_MAX_WIDTH         32768U
+#define TEGRA_MIN_HEIGHT        32U
+#define TEGRA_MAX_HEIGHT        32768U
+/* Width alignment */
+#define TEGRA_WIDTH_ALIGNMENT   1
+/* Stride alignment is 256, for VIC worse case */
+#define TEGRA_STRIDE_ALIGNMENT  256
+
+/* 1080p resolution as default resolution for test pattern generator */
+#define TEGRA_DEF_WIDTH         1920
+#define TEGRA_DEF_HEIGHT        1080
+
+#define TEGRA_VF_DEF            MEDIA_BUS_FMT_SRGGB10_1X10
+#define TEGRA_IMAGE_FORMAT_DEF  32
+
+enum tegra_image_dt {
+        TEGRA_IMAGE_DT_YUV420_8 = 24,
+        TEGRA_IMAGE_DT_YUV420_10,
+
+        TEGRA_IMAGE_DT_YUV420CSPS_8 = 28,
+        TEGRA_IMAGE_DT_YUV420CSPS_10,
+        TEGRA_IMAGE_DT_YUV422_8,
+        TEGRA_IMAGE_DT_YUV422_10,
+        TEGRA_IMAGE_DT_RGB444,
+        TEGRA_IMAGE_DT_RGB555,
+        TEGRA_IMAGE_DT_RGB565,
+        TEGRA_IMAGE_DT_RGB666,
+        TEGRA_IMAGE_DT_RGB888,
+
+        TEGRA_IMAGE_DT_RAW6 = 40,
+        TEGRA_IMAGE_DT_RAW7,
+        TEGRA_IMAGE_DT_RAW8,
+        TEGRA_IMAGE_DT_RAW10,
+        TEGRA_IMAGE_DT_RAW12,
+        TEGRA_IMAGE_DT_RAW14,
+};
+
+/* Supported CSI to VI Data Formats */
+enum tegra_vf_code {
+        TEGRA_VF_RAW6 = 0,
+        TEGRA_VF_RAW7,
+        TEGRA_VF_RAW8,
+        TEGRA_VF_RAW10,
+        TEGRA_VF_RAW12,
+        TEGRA_VF_RAW14,
+        TEGRA_VF_EMBEDDED8,
+        TEGRA_VF_RGB565,
+        TEGRA_VF_RGB555,
+        TEGRA_VF_RGB888,
+        TEGRA_VF_RGB444,
+        TEGRA_VF_RGB666,
+        TEGRA_VF_YUV422,
+        TEGRA_VF_YUV420,
+        TEGRA_VF_YUV420_CSPS,
+};
+
+/**
+ * struct tegra_frac
+ * @numerator: numerator of the fraction
+ * @denominator: denominator of the fraction
+ */
+struct tegra_frac {
+        unsigned int numerator;
+        unsigned int denominator;
+};
+
+/**
+ * struct tegra_video_format - Tegra video format description
+ * @vf_code: video format code
+ * @width: format width in bits per component
+ * @code: media bus format code
+ * @bpp: bytes per pixel fraction (when stored in memory)
+ * @img_fmt: image format
+ * @img_dt: image data type
+ * @fourcc: V4L2 pixel format FCC identifier
+ * @description: format description, suitable for userspace
+ */
+struct tegra_video_format {
+        enum tegra_vf_code vf_code;
+        unsigned int width;
+        unsigned int code;
+        struct tegra_frac bpp;
+        u32 img_fmt;
+        enum tegra_image_dt img_dt;
+        u32 fourcc;
+        __u8 description[32];
+};
+
+#define TEGRA_VIDEO_FORMAT(VF_CODE, BPP, MBUS_CODE, FRAC_BPP_NUM,       \
+        FRAC_BPP_DEN, FORMAT, DATA_TYPE, FOURCC, DESCRIPTION)           \
+{                                                                       \
+        TEGRA_VF_##VF_CODE,                                             \
+        BPP,                                                            \
+        MEDIA_BUS_FMT_##MBUS_CODE,                                      \
+        {FRAC_BPP_NUM, FRAC_BPP_DEN},                                   \
+        TEGRA_IMAGE_FORMAT_##FORMAT,                                    \
+        TEGRA_IMAGE_DT_##DATA_TYPE,                                     \
+        V4L2_PIX_FMT_##FOURCC,                                          \
+        DESCRIPTION,                                                    \
+}
+
+u32 tegra_core_get_word_count(unsigned int frame_width,
+                const struct tegra_video_format *fmt);
+u32 tegra_core_bytes_per_line(unsigned int width, unsigned int align,
+                const struct tegra_video_format *fmt);
+const struct tegra_video_format *tegra_core_get_default_format(void);
+
+#endif
diff --git a/drivers/media/platform/tegra/camera/vi/graph.c b/drivers/media/platform/tegra/camera/vi/graph.c
new file mode 100644
index 000000000..c2dd058a1
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/graph.c
@@ -0,0 +1,602 @@
+/*
+ * NVIDIA Media controller graph management
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/clk.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/reset.h>
+#include <linux/slab.h>
+#include <linux/tegra-soc.h>
+#include <linux/tegra_pm_domains.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-of.h>
+#include <media/tegra_v4l2_camera.h>
+
+#include "camera/vi/mc_common.h"
+#include "camera/csi/csi.h"
+#include "vi/vi4.h"
+#include "nvcsi/nvcsi.h"
+
+/* -----------------------------------------------------------------------------
+ * Graph Management
+ */
+
+static struct tegra_vi_graph_entity *
+tegra_vi_graph_find_entity(struct tegra_mc_vi *vi,
+                       const struct device_node *node)
+{
+        struct tegra_vi_graph_entity *entity;
+
+        list_for_each_entry(entity, &vi->entities, list) {
+                if (entity->node == node)
+                        return entity;
+        }
+
+        return NULL;
+}
+
+static int tegra_vi_graph_build_one(struct tegra_mc_vi *vi,
+                                    struct tegra_vi_graph_entity *entity)
+{
+        u32 link_flags = MEDIA_LNK_FL_ENABLED;
+        struct media_entity *local;
+        struct media_entity *remote;
+        struct media_pad *local_pad;
+        struct media_pad *remote_pad;
+        struct tegra_vi_graph_entity *ent;
+        struct v4l2_of_link link;
+        struct device_node *ep = NULL;
+        struct device_node *next;
+        int ret = 0;
+
+        if (!entity->subdev) {
+                dev_err(vi->dev, "%s:No subdev under entity, skip linking\n",
+                                __func__);
+                return 0;
+        }
+
+        local = entity->entity;
+        dev_dbg(vi->dev, "creating links for entity %s\n", local->name);
+
+        do {
+                /* Get the next endpoint and parse its link. */
+                next = of_graph_get_next_endpoint(entity->node, ep);
+                if (next == NULL)
+                        break;
+
+                ep = next;
+
+                dev_dbg(vi->dev, "processing endpoint %s\n", ep->full_name);
+
+                ret = v4l2_of_parse_link(ep, &link);
+                if (ret < 0) {
+                        dev_err(vi->dev, "failed to parse link for %s\n",
+                                ep->full_name);
+                        continue;
+                }
+
+                /* Skip sink ports, they will be processed from the other end of
+                 * the link.
+                 */
+                if (link.local_port >= local->num_pads) {
+                        dev_err(vi->dev, "invalid port number %u on %s\n",
+                                link.local_port, link.local_node->full_name);
+                        v4l2_of_put_link(&link);
+                        ret = -EINVAL;
+                        break;
+                }
+
+                local_pad = &local->pads[link.local_port];
+
+                if (local_pad->flags & MEDIA_PAD_FL_SINK) {
+                        dev_dbg(vi->dev, "skipping sink port %s:%u\n",
+                                link.local_node->full_name, link.local_port);
+                        v4l2_of_put_link(&link);
+                        continue;
+                }
+
+                /* Skip channel entity , they will be processed separately. */
+                if (link.remote_node == vi->dev->of_node) {
+                        dev_dbg(vi->dev, "skipping channel port %s:%u\n",
+                                link.local_node->full_name, link.local_port);
+                        v4l2_of_put_link(&link);
+                        continue;
+                }
+
+                /* Find the remote entity. */
+                ent = tegra_vi_graph_find_entity(vi, link.remote_node);
+                if (ent == NULL) {
+                        dev_err(vi->dev, "no entity found for %s\n",
+                                link.remote_node->full_name);
+                        v4l2_of_put_link(&link);
+                        ret = -EINVAL;
+                        break;
+                }
+
+                remote = ent->entity;
+
+                if (link.remote_port >= remote->num_pads) {
+                        dev_err(vi->dev, "invalid port number %u on %s\n",
+                                link.remote_port, link.remote_node->full_name);
+                        v4l2_of_put_link(&link);
+                        ret = -EINVAL;
+                        break;
+                }
+
+                remote_pad = &remote->pads[link.remote_port];
+
+                v4l2_of_put_link(&link);
+
+                /* Create the media link. */
+                dev_dbg(vi->dev, "creating %s:%u -> %s:%u link\n",
+                        local->name, local_pad->index,
+                        remote->name, remote_pad->index);
+
+                ret = media_entity_create_link(local, local_pad->index,
+                                               remote, remote_pad->index,
+                                               link_flags);
+                if (ret < 0) {
+                        dev_err(vi->dev,
+                                "failed to create %s:%u -> %s:%u link\n",
+                                local->name, local_pad->index,
+                                remote->name, remote_pad->index);
+                        break;
+                }
+        } while (next);
+
+        return ret;
+}
+
+static int tegra_vi_graph_build_links(struct tegra_mc_vi *vi)
+{
+        u32 link_flags = MEDIA_LNK_FL_ENABLED;
+        struct device_node *node = vi->dev->of_node;
+        struct media_entity *source;
+        struct media_entity *sink;
+        struct media_pad *source_pad;
+        struct media_pad *sink_pad;
+        struct tegra_vi_graph_entity *ent;
+        struct v4l2_of_link link;
+        struct device_node *ep = NULL;
+        struct device_node *next;
+        struct tegra_channel *chan;
+        int ret = 0;
+
+        dev_dbg(vi->dev, "creating links for channels\n");
+
+        chan = list_first_entry(&vi->vi_chans, struct tegra_channel, list);
+        do {
+                /* Get the next endpoint and parse its link. */
+                next = of_graph_get_next_endpoint(node, ep);
+                if (next == NULL || !of_device_is_available(next))
+                        break;
+
+                /* Device not registered */
+                if (!chan->init_done) {
+                        chan = list_next_entry(chan, list);
+                        continue;
+                }
+
+                ep = next;
+
+                dev_dbg(vi->dev, "processing endpoint %s\n", ep->full_name);
+
+                ret = v4l2_of_parse_link(ep, &link);
+                if (ret < 0) {
+                        dev_err(vi->dev, "failed to parse link for %s\n",
+                                ep->full_name);
+                        continue;
+                }
+
+                if (link.local_port >= vi->num_channels) {
+                        dev_err(vi->dev, "wrong channel number for port %u\n",
+                                link.local_port);
+                        v4l2_of_put_link(&link);
+                        ret = -EINVAL;
+                        break;
+                }
+
+                dev_dbg(vi->dev, "creating link for channel %s\n",
+                        chan->video.name);
+
+                /* Find the remote entity. */
+                ent = tegra_vi_graph_find_entity(vi, link.remote_node);
+                if (ent == NULL) {
+                        dev_err(vi->dev, "no entity found for %s\n",
+                                link.remote_node->full_name);
+                        v4l2_of_put_link(&link);
+                        ret = -EINVAL;
+                        break;
+                }
+
+                if (ent->entity == NULL) {
+                        dev_err(vi->dev, "entity not bounded %s\n",
+                                link.remote_node->full_name);
+                        continue;
+                }
+
+                source = ent->entity;
+                source_pad = &source->pads[link.remote_port];
+                sink = &chan->video.entity;
+                sink_pad = &chan->pad;
+
+                v4l2_of_put_link(&link);
+
+                /* Create the media link. */
+                dev_dbg(vi->dev, "creating %s:%u -> %s:%u link\n",
+                        source->name, source_pad->index,
+                        sink->name, sink_pad->index);
+
+                ret = media_entity_create_link(source, source_pad->index,
+                                               sink, sink_pad->index,
+                                               link_flags);
+                if (ret < 0) {
+                        dev_err(vi->dev,
+                                "failed to create %s:%u -> %s:%u link\n",
+                                source->name, source_pad->index,
+                                sink->name, sink_pad->index);
+                        break;
+                }
+
+                tegra_channel_init_subdevices(chan);
+                chan = list_next_entry(chan, list);
+        } while (next != NULL);
+
+        return ret;
+}
+
+static int tegra_vi_graph_notify_complete(struct v4l2_async_notifier *notifier)
+{
+        struct tegra_mc_vi *vi =
+                container_of(notifier, struct tegra_mc_vi, notifier);
+        struct tegra_vi_graph_entity *entity;
+        int ret;
+
+        dev_dbg(vi->dev, "notify complete, all subdevs registered\n");
+
+        /* Create links for every entity. */
+        list_for_each_entry(entity, &vi->entities, list) {
+                if (entity->entity != NULL) {
+                        ret = tegra_vi_graph_build_one(vi, entity);
+                        if (ret < 0)
+                                return ret;
+                }
+        }
+
+        /* Create links for channels */
+        ret = tegra_vi_graph_build_links(vi);
+        if (ret < 0)
+                return ret;
+
+        ret = v4l2_device_register_subdev_nodes(&vi->v4l2_dev);
+        if (ret < 0)
+                dev_err(vi->dev, "failed to register subdev nodes\n");
+
+        vi->link_status++;
+
+        return ret;
+}
+
+static int tegra_vi_graph_notify_bound(struct v4l2_async_notifier *notifier,
+                                   struct v4l2_subdev *subdev,
+                                   struct v4l2_async_subdev *asd)
+{
+        struct tegra_mc_vi *vi =
+                container_of(notifier, struct tegra_mc_vi, notifier);
+        struct tegra_vi_graph_entity *entity;
+
+        /* Locate the entity corresponding to the bound subdev and store the
+         * subdev pointer.
+         */
+        list_for_each_entry(entity, &vi->entities, list) {
+                if (entity->node != subdev->dev->of_node &&
+                        entity->node != subdev->of_node)
+                        continue;
+
+                if (entity->subdev) {
+                        dev_err(vi->dev, "duplicate subdev for node %s\n",
+                                entity->node->full_name);
+                        return -EINVAL;
+                }
+
+                dev_dbg(vi->dev, "subdev %s bound\n", subdev->name);
+                entity->entity = &subdev->entity;
+                entity->subdev = subdev;
+                vi->subdevs_bound++;
+                return 0;
+        }
+
+        dev_err(vi->dev, "no entity for subdev %s\n", subdev->name);
+        return -EINVAL;
+}
+
+void tegra_vi_graph_cleanup(struct tegra_mc_vi *vi)
+{
+        struct tegra_vi_graph_entity *entityp;
+        struct tegra_vi_graph_entity *entity;
+
+        v4l2_async_notifier_unregister(&vi->notifier);
+
+        list_for_each_entry_safe(entity, entityp, &vi->entities, list) {
+                of_node_put(entity->node);
+                list_del(&entity->list);
+        }
+}
+
+int tegra_vi_get_port_info(struct tegra_channel *chan,
+                        struct device_node *node, unsigned int index)
+{
+        struct device_node *ep = NULL;
+        struct device_node *ports;
+        struct device_node *port;
+        int value = 0xFFFF;
+        int ret = 0, i;
+
+        ports = of_get_child_by_name(node, "ports");
+        if (ports == NULL)
+                ports = node;
+
+        for_each_child_of_node(ports, port) {
+                if (!port->name || of_node_cmp(port->name, "port"))
+                        continue;
+
+                ret = of_property_read_u32(port, "reg", &value);
+                if (ret < 0)
+                        continue;
+
+                if (value != index)
+                        continue;
+
+                for_each_child_of_node(port, ep) {
+                        if (!ep->name || of_node_cmp(ep->name, "endpoint"))
+                                continue;
+
+                        /* Get CSI port */
+                        ret = of_property_read_u32(ep, "csi-port", &value);
+                        if (ret < 0)
+                                dev_err(&chan->video.dev, "csi port error\n");
+                        chan->port[0] = value;
+
+                        /* Get number of data lanes for the endpoint */
+                        ret = of_property_read_u32(ep, "bus-width", &value);
+                        if (ret < 0)
+                                dev_err(&chan->video.dev, "num lanes error\n");
+                        chan->numlanes = value;
+
+                        if (value > 12) {
+                                dev_err(&chan->video.dev, "num lanes >12!\n");
+                                return -EINVAL;
+                        }
+                        /*
+                         * for numlanes greater than 4 multiple CSI bricks
+                         * are needed to capture the image, the logic below
+                         * checks for numlanes > 4 and add a new CSI brick
+                         * as a valid port. Loops around the three CSI
+                         * bricks to add as many ports necessary.
+                         */
+                        value -= 4;
+                        for (i = 1; value > 0; i++, value -= 4) {
+                                int next_port = chan->port[i-1] + 2;
+
+                                next_port = (next_port % (PORT_F + 1));
+                                chan->port[i] = next_port;
+                        }
+                }
+        }
+
+        return ret;
+}
+
+static int tegra_vi_graph_parse_one(struct tegra_mc_vi *vi,
+                                struct device_node *node)
+{
+        struct device_node *ep = NULL;
+        struct device_node *next;
+        struct device_node *remote = NULL;
+        struct tegra_vi_graph_entity *entity;
+        int ret = 0;
+
+        dev_dbg(vi->dev, "parsing node %s\n", node->full_name);
+
+        do {
+                /* Parse all the remote entities and put them into the list */
+                next = of_graph_get_next_endpoint(node, ep);
+                if (next == NULL || !of_device_is_available(next))
+                        break;
+                ep = next;
+
+                dev_dbg(vi->dev, "handling endpoint %s\n", ep->full_name);
+
+                remote = of_graph_get_remote_port_parent(ep);
+                if (!remote) {
+                        ret = -EINVAL;
+                        break;
+                }
+
+                /* Skip entities that we have already processed. */
+                if (remote == vi->dev->of_node ||
+                    tegra_vi_graph_find_entity(vi, remote) ||
+                    !of_device_is_available(remote))
+                        continue;
+
+                entity = devm_kzalloc(vi->dev, sizeof(*entity),
+                                GFP_KERNEL);
+                if (entity == NULL) {
+                        ret = -ENOMEM;
+                        break;
+                }
+
+                entity->node = remote;
+                entity->asd.match_type = V4L2_ASYNC_MATCH_OF;
+                entity->asd.match.of.node = remote;
+                list_add_tail(&entity->list, &vi->entities);
+                vi->num_subdevs++;
+        } while (next);
+
+        return ret;
+}
+
+int tegra_vi_tpg_graph_init(struct tegra_mc_vi *mc_vi)
+{
+        int err = 0;
+        u32 link_flags = MEDIA_LNK_FL_ENABLED;
+        struct tegra_csi_device *csi = mc_vi->csi;
+        struct tegra_channel *vi_it;
+        struct tegra_csi_channel *csi_it;
+
+        if (!csi) {
+                dev_err(mc_vi->dev, "CSI is NULL\n");
+                return -EINVAL;
+        }
+        mc_vi->num_subdevs = mc_vi->num_channels;
+        vi_it = mc_vi->tpg_start;
+        csi_it = csi->tpg_start;
+
+        list_for_each_entry_from(vi_it, &mc_vi->vi_chans, list) {
+                /* Device not registered */
+                if (!vi_it->init_done)
+                        continue;
+
+                list_for_each_entry_from(csi_it, &csi->csi_chans, list) {
+                        struct media_entity *source = &csi_it->subdev.entity;
+                        struct media_entity *sink = &vi_it->video.entity;
+                        struct media_pad *source_pad = csi_it->pads;
+                        struct media_pad *sink_pad = &vi_it->pad;
+
+                        vi_it->bypass = 0;
+                        err = v4l2_device_register_subdev(&mc_vi->v4l2_dev,
+                                        &csi_it->subdev);
+                        if (err) {
+                                dev_err(mc_vi->dev,
+                                        "%s:Fail to register subdev\n",
+                                        __func__);
+                                goto register_fail;
+                        }
+                        dev_dbg(mc_vi->dev, "creating %s:%u -> %s:%u link\n",
+                                source->name, source_pad->index,
+                                sink->name, sink_pad->index);
+
+                        err = media_entity_create_link(source,
+                                                        source_pad->index,
+                                                        sink, sink_pad->index,
+                                                        link_flags);
+                        if (err < 0) {
+                                dev_err(mc_vi->dev,
+                                        "failed to create %s:%u -> %s:%u link\n",
+                                        source->name, source_pad->index,
+                                        sink->name, sink_pad->index);
+                                goto register_fail;
+                        }
+                        err = tegra_channel_init_subdevices(vi_it);
+                        if (err) {
+                                dev_err(mc_vi->dev,
+                                        "%s:Init subdevice error\n", __func__);
+                                goto register_fail;
+                        }
+                        csi_it = list_next_entry(csi_it, list);
+                        break;
+                }
+        }
+
+        return 0;
+register_fail:
+        csi_it = csi->tpg_start;
+        list_for_each_entry_from(csi_it, &csi->csi_chans, list)
+                v4l2_device_unregister_subdev(&csi_it->subdev);
+        return err;
+}
+
+int tegra_vi_graph_init(struct tegra_mc_vi *vi)
+{
+        struct tegra_vi_graph_entity *entity;
+        struct v4l2_async_subdev **subdevs = NULL;
+        unsigned int num_subdevs = 0;
+        int ret = 0, i;
+
+        /*
+         * Walk the links to parse the full graph. Start by parsing the
+         * composite node and then parse entities in turn. The list_for_each
+         * loop will handle entities added at the end of the list while walking
+         * the links.
+         */
+        ret = tegra_vi_graph_parse_one(vi, vi->dev->of_node);
+        if (ret < 0)
+                return 0;
+
+        list_for_each_entry(entity, &vi->entities, list) {
+                ret = tegra_vi_graph_parse_one(vi, entity->node);
+                if (ret < 0)
+                        break;
+        }
+
+        if (!vi->num_subdevs) {
+                dev_dbg(vi->dev, "warning: no subdev found in graph\n");
+                goto done;
+        }
+
+        /* Register the subdevices notifier. */
+        num_subdevs = vi->num_subdevs;
+        subdevs = devm_kzalloc(vi->dev, sizeof(*subdevs) * num_subdevs,
+                               GFP_KERNEL);
+        if (subdevs == NULL) {
+                ret = -ENOMEM;
+                goto done;
+        }
+
+        /*
+         * Add code to check for sensors and
+         * set TPG mode for VI if no sensors found
+         * logic varies for different platforms
+         */
+        i = 0;
+        list_for_each_entry(entity, &vi->entities, list)
+                subdevs[i++] = &entity->asd;
+
+        vi->notifier.subdevs = subdevs;
+        vi->notifier.num_subdevs = num_subdevs;
+        vi->notifier.bound = tegra_vi_graph_notify_bound;
+        vi->notifier.complete = tegra_vi_graph_notify_complete;
+        vi->link_status = 0;
+        vi->subdevs_bound = 0;
+
+        ret = v4l2_async_notifier_register(&vi->v4l2_dev, &vi->notifier);
+        if (ret < 0) {
+                dev_err(vi->dev, "notifier registration failed\n");
+                goto done;
+        }
+
+        if (!vi->link_status) {
+                if (vi->subdevs_bound) {
+                        ret = tegra_vi_graph_notify_complete(&vi->notifier);
+                        if (ret < 0)
+                                goto done;
+                }
+                tegra_clean_unlinked_channels(vi);
+        }
+
+        return 0;
+
+done:
+        tegra_clean_unlinked_channels(vi);
+        if (ret < 0)
+                tegra_vi_graph_cleanup(vi);
+
+        return ret;
+}
diff --git a/drivers/media/platform/tegra/camera/vi/mc_common.c b/drivers/media/platform/tegra/camera/vi/mc_common.c
new file mode 100644
index 000000000..11acb7cfe
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/mc_common.c
@@ -0,0 +1,346 @@
+/*
+ * Tegra Video Input device common APIs
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+
+#include <media/tegra_v4l2_camera.h>
+#include <media/camera_common.h>
+#include <media/v4l2-event.h>
+#include <media/tegra_camera_platform.h>
+
+#include "dev.h"
+#include "camera/vi/mc_common.h"
+#include "vi/vi.h"
+#include "vi/vi4.h"
+#include "host1x/host1x.h"
+#include "camera/vi/registers.h"
+
+/* In TPG mode, VI only support 2 formats */
+static void vi_tpg_fmts_bitmap_init(struct tegra_channel *chan)
+{
+        int index;
+
+        bitmap_zero(chan->fmts_bitmap, MAX_FORMAT_NUM);
+
+        index = tegra_core_get_idx_by_code(chan,
+                        MEDIA_BUS_FMT_SRGGB10_1X10, 0);
+        bitmap_set(chan->fmts_bitmap, index, 1);
+
+        index = tegra_core_get_idx_by_code(chan,
+                        MEDIA_BUS_FMT_RGB888_1X32_PADHI, 0);
+        bitmap_set(chan->fmts_bitmap, index, 1);
+}
+
+/* -----------------------------------------------------------------------------
+ * Media Controller and V4L2
+ */
+
+static const char *const vi_pattern_strings[] = {
+        "Disabled",
+        "Black/White Direct Mode",
+        "Color Patch Mode",
+};
+
+static int vi_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct tegra_mc_vi *vi = container_of(ctrl->handler, struct tegra_mc_vi,
+                                           ctrl_handler);
+
+        switch (ctrl->id) {
+        case V4L2_CID_TEST_PATTERN:
+                /*
+                 * TPG control is only avaiable to TPG driver,
+                 * it can't be changed to 0 to disable TPG mode.
+                 */
+                if (ctrl->val) {
+                        dev_info(&vi->ndev->dev, "Set TPG mode to %d\n",
+                                 ctrl->val);
+                        vi->pg_mode = ctrl->val;
+                }
+                break;
+        default:
+                dev_err(vi->dev, "%s:Not valid ctrl\n", __func__);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static const struct v4l2_ctrl_ops vi_ctrl_ops = {
+        .s_ctrl = vi_s_ctrl,
+};
+
+void tegra_vi_v4l2_cleanup(struct tegra_mc_vi *vi)
+{
+        v4l2_ctrl_handler_free(&vi->ctrl_handler);
+        v4l2_device_unregister(&vi->v4l2_dev);
+        if (!vi->pg_mode)
+                media_device_unregister(&vi->media_dev);
+}
+EXPORT_SYMBOL(tegra_vi_v4l2_cleanup);
+
+static void tegra_vi_notify(struct v4l2_subdev *sd,
+                                          unsigned int notification, void *arg)
+{
+        struct tegra_mc_vi *vi = container_of(sd->v4l2_dev,
+                        struct tegra_mc_vi, v4l2_dev);
+        unsigned i;
+        struct tegra_channel *chan;
+
+        if (notification != V4L2_DEVICE_NOTIFY_EVENT)
+                return;
+
+        list_for_each_entry(chan, &vi->vi_chans, list) {
+                for (i = 0; i < chan->num_subdevs; i++)
+                        if (sd == chan->subdev[i])
+                                v4l2_event_queue(&chan->video, arg);
+        }
+}
+
+int tegra_vi_v4l2_init(struct tegra_mc_vi *vi)
+{
+        int ret;
+
+        vi->media_dev.dev = vi->dev;
+        strlcpy(vi->media_dev.model, "NVIDIA Tegra Video Input Device",
+                sizeof(vi->media_dev.model));
+        vi->media_dev.hw_revision = 3;
+
+        ret = media_device_register(&vi->media_dev);
+        if (ret < 0) {
+                dev_err(vi->dev,
+                        "media device registration failed (%d)\n",
+                        ret);
+                return ret;
+        }
+
+        mutex_init(&vi->bw_update_lock);
+        vi->v4l2_dev.mdev = &vi->media_dev;
+        vi->v4l2_dev.notify = tegra_vi_notify;
+        ret = v4l2_device_register(vi->dev, &vi->v4l2_dev);
+        if (ret < 0) {
+                dev_err(vi->dev, "V4L2 device registration failed (%d)\n",
+                        ret);
+                goto register_error;
+        }
+
+        return 0;
+
+register_error:
+        media_device_unregister(&vi->media_dev);
+        return ret;
+}
+
+static int vi_parse_dt(struct tegra_mc_vi *vi, struct platform_device *dev)
+{
+        int err = 0;
+        int num_channels = 0;
+        int i;
+        struct tegra_channel *item;
+        struct device_node *node = dev->dev.of_node;
+
+        err = of_property_read_u32(node, "num-channels", &num_channels);
+        if (err) {
+                dev_dbg(&dev->dev,
+                        "Failed to find num of channels, set to 0\n");
+                num_channels = 0;
+        }
+        vi->num_channels = num_channels;
+        for (i = 0; i < num_channels; i++) {
+                item = devm_kzalloc(vi->dev, sizeof(*item), GFP_KERNEL);
+                if (!item)
+                        return -ENOMEM;
+                item->id = i;
+                list_add_tail(&item->list, &vi->vi_chans);
+        }
+
+        return 0;
+}
+
+static void set_vi_register_base(struct tegra_mc_vi *mc_vi,
+                        void __iomem *regbase)
+{
+        mc_vi->iomem = regbase;
+}
+int tpg_vi_media_controller_init(struct tegra_mc_vi *mc_vi, int pg_mode)
+{
+        int err = 0, i;
+        struct tegra_channel *item;
+        int count = 0;
+
+        /* Allocate TPG channel */
+        v4l2_ctrl_handler_init(&mc_vi->ctrl_handler, 1);
+        mc_vi->pattern = v4l2_ctrl_new_std_menu_items(&mc_vi->ctrl_handler,
+                        &vi_ctrl_ops, V4L2_CID_TEST_PATTERN,
+                        ARRAY_SIZE(vi_pattern_strings) - 1,
+                        0, mc_vi->pg_mode, vi_pattern_strings);
+
+        if (mc_vi->ctrl_handler.error) {
+                dev_err(mc_vi->dev, "failed to add controls\n");
+                err = mc_vi->ctrl_handler.error;
+                goto ctrl_error;
+        }
+
+        mc_vi->tpg_start = NULL;
+        for (i = 0; i < TPG_CHANNELS; i++) {
+                item = devm_kzalloc(mc_vi->dev, sizeof(*item), GFP_KERNEL);
+                if (!item)
+                        continue;
+                item->id = mc_vi->num_channels + i;
+                item->pg_mode = pg_mode;
+                item->vi = mc_vi;
+
+                err = tegra_channel_init(item);
+                if (err) {
+                        devm_kfree(mc_vi->dev, item);
+                        continue;
+                }
+                vi_tpg_fmts_bitmap_init(item);
+                /* only inited tpg channels are added */
+                list_add_tail(&item->list, &mc_vi->vi_chans);
+                mc_vi->num_channels++;
+                if (mc_vi->tpg_start == NULL)
+                        mc_vi->tpg_start = item;
+        }
+
+        err = tegra_vi_tpg_graph_init(mc_vi);
+        if (err)
+                goto channel_init_error;
+
+        list_for_each_entry(item, &mc_vi->vi_chans, list) {
+                if (!item->pg_mode)
+                        continue;
+                err = video_register_device(&item->video, VFL_TYPE_GRABBER, -1);
+                if (err < 0) {
+                        dev_err(&item->video.dev, "failed to register %s\n",
+                                item->video.name);
+                        continue;
+                }
+                count++;
+        }
+
+        if (count == 0) {
+                dev_err(mc_vi->dev, "all tpg register failed\n");
+                goto channel_init_error;
+        }
+
+        return err;
+
+channel_init_error:
+        dev_err(mc_vi->dev, "%s: channel init failed\n", __func__);
+        if (!mc_vi->tpg_start)
+                tpg_vi_media_controller_cleanup(mc_vi);
+        return err;
+ctrl_error:
+        v4l2_ctrl_handler_free(&mc_vi->ctrl_handler);
+        dev_err(mc_vi->dev, "%s: v2l4_ctl error\n", __func__);
+        return err;
+}
+EXPORT_SYMBOL(tpg_vi_media_controller_init);
+
+void tpg_vi_media_controller_cleanup(struct tegra_mc_vi *mc_vi)
+{
+        struct tegra_channel *item;
+        struct tegra_channel *itemn;
+
+        list_for_each_entry_safe(item, itemn, &mc_vi->vi_chans, list) {
+                if (!item->pg_mode)
+                        continue;
+                if (item->video.cdev != NULL)
+                      video_unregister_device(&item->video);
+                tegra_channel_cleanup(item);
+                list_del(&item->list);
+                devm_kfree(mc_vi->dev, item);
+                /* decrement media device entity count */
+                mc_vi->media_dev.entity_id--;
+                mc_vi->num_channels--;
+        }
+        mc_vi->tpg_start = NULL;
+        v4l2_ctrl_handler_free(&mc_vi->ctrl_handler);
+}
+EXPORT_SYMBOL(tpg_vi_media_controller_cleanup);
+
+int tegra_vi_media_controller_init(struct tegra_mc_vi *mc_vi,
+                                   struct platform_device *pdev)
+{
+        int err = 0;
+        struct nvhost_device_data *pdata = (struct nvhost_device_data *)
+                platform_get_drvdata(pdev);
+
+        if (!pdata)
+                return -EINVAL;
+        set_vi_register_base(mc_vi, pdata->aperture[0]);
+
+        mc_vi->ndev = pdev;
+        mc_vi->dev = &pdev->dev;
+        INIT_LIST_HEAD(&mc_vi->vi_chans);
+        INIT_LIST_HEAD(&mc_vi->entities);
+        mutex_init(&mc_vi->mipical_lock);
+
+        err = vi_parse_dt(mc_vi, pdev);
+        if (err)
+                goto mc_init_fail;
+
+        /*
+         * if there is no vi channels listed in DT,
+         * no need to init the channel and graph
+         */
+        if (mc_vi->num_channels == 0)
+                return 0;
+
+        err = tegra_vi_v4l2_init(mc_vi);
+        if (err < 0)
+                goto mc_init_fail;
+
+        /* Init Tegra VI channels */
+        err = tegra_vi_channels_init(mc_vi);
+        if (err < 0) {
+                dev_err(&pdev->dev, "Init channel failed\n");
+                goto channels_error;
+        }
+
+        /* Setup media links between VI and external sensor subdev. */
+        err = tegra_vi_graph_init(mc_vi);
+        if (err < 0)
+                goto graph_error;
+
+        err = tegra_vi_channels_register(mc_vi);
+        if (err < 0)
+                goto register_error;
+
+        return 0;
+
+register_error:
+        tegra_vi_graph_cleanup(mc_vi);
+graph_error:
+        tegra_vi_channels_cleanup(mc_vi);
+channels_error:
+        tegra_vi_v4l2_cleanup(mc_vi);
+mc_init_fail:
+        dev_err(&pdev->dev, "%s: failed\n", __func__);
+        return err;
+}
+EXPORT_SYMBOL(tegra_vi_media_controller_init);
+
+void tegra_vi_media_controller_cleanup(struct tegra_mc_vi *mc_vi)
+{
+        tegra_vi_channels_unregister(mc_vi);
+        tegra_vi_graph_cleanup(mc_vi);
+        tegra_vi_channels_cleanup(mc_vi);
+        tegra_vi_v4l2_cleanup(mc_vi);
+}
+EXPORT_SYMBOL(tegra_vi_media_controller_cleanup);
diff --git a/drivers/media/platform/tegra/camera/vi/mc_common.h b/drivers/media/platform/tegra/camera/vi/mc_common.h
new file mode 100644
index 000000000..42982bef8
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/mc_common.h
@@ -0,0 +1,352 @@
+/*
+ * drivers/media/platform/tegra/camera/mc_common.h
+ *
+ * Tegra Media controller common APIs
+ *
+ * Copyright (c) 2012-2017, NVIDIA CORPORATION. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __CAMERA_MC_COMMON_H__
+#define __CAMERA_MC_COMMON_H__
+
+#include <media/media-device.h>
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dev.h>
+#include <media/videobuf2-core.h>
+#include <linux/workqueue.h>
+
+#include "core.h"
+#include "../csi/csi.h"
+
+#define MAX_FORMAT_NUM  64
+#define MAX_SUBDEVICES  4
+#define QUEUED_BUFFERS  4
+#define ENABLE          1
+#define DISABLE         0
+#define MAX_SYNCPT_PER_CHANNEL 3
+#define TPG_CHANNELS 6
+
+#define TEGRA_MEM_FORMAT 0
+#define TEGRA_ISP_FORMAT 1
+
+enum channel_capture_state {
+        CAPTURE_IDLE = 0,
+        CAPTURE_GOOD,
+        CAPTURE_TIMEOUT,
+        CAPTURE_ERROR,
+};
+
+enum tegra_vi_pg_mode {
+        TEGRA_VI_PG_DISABLED = 0,
+        TEGRA_VI_PG_DIRECT,
+        TEGRA_VI_PG_PATCH,
+};
+
+/**
+ * struct tegra_channel_buffer - video channel buffer
+ * @buf: vb2 buffer base object
+ * @queue: buffer list entry in the channel queued buffers list
+ * @chan: channel that uses the buffer
+ * @addr: Tegra IOVA buffer address for VI output
+ */
+struct tegra_channel_buffer {
+        struct vb2_v4l2_buffer buf;
+        struct list_head queue;
+        struct tegra_channel *chan;
+
+        dma_addr_t addr;
+};
+
+#define to_tegra_channel_buffer(vb) \
+        container_of(vb, struct tegra_channel_buffer, buf)
+
+/**
+ * struct tegra_vi_graph_entity - Entity in the video graph
+ * @list: list entry in a graph entities list
+ * @node: the entity's DT node
+ * @entity: media entity, from the corresponding V4L2 subdev
+ * @asd: subdev asynchronous registration information
+ * @subdev: V4L2 subdev
+ */
+struct tegra_vi_graph_entity {
+        struct list_head list;
+        struct device_node *node;
+        struct media_entity *entity;
+
+        struct v4l2_async_subdev asd;
+        struct v4l2_subdev *subdev;
+};
+
+/**
+ * struct tegra_channel - Tegra video channel
+ * @list: list entry in a composite device dmas list
+ * @video: V4L2 video device associated with the video channel
+ * @video_lock:
+ * @pad: media pad for the video device entity
+ * @pipe: pipeline belonging to the channel
+ *
+ * @vi: composite device DT node port number for the channel
+ *
+ * @kthread_capture: kernel thread task structure of this video channel
+ * @wait: wait queue structure for kernel thread
+ *
+ * @format: active V4L2 pixel format
+ * @fmtinfo: format information corresponding to the active @format
+ *
+ * @queue: vb2 buffers queue
+ * @alloc_ctx: allocation context for the vb2 @queue
+ * @sequence: V4L2 buffers sequence number
+ *
+ * @capture: list of queued buffers for capture
+ * @queued_lock: protects the buf_queued list
+ *
+ * @csi: CSI register bases
+ * @stride_align: channel buffer stride alignment, default is 64
+ * @width_align: image width alignment, default is 4
+ * @height_align: channel buffer height alignment, default is 1
+ * @size_align: channel buffer size alignment, default is 128K bytes
+ * @port: CSI port of this video channel
+ * @io_id: Tegra IO rail ID of this video channel
+ *
+ * @fmts_bitmap: a bitmap for formats supported
+ * @bypass: bypass flag for VI bypass mode
+ */
+struct tegra_channel {
+        int id;
+        struct list_head list;
+        struct video_device video;
+        struct media_pad pad;
+        struct media_pipeline pipe;
+        struct mutex video_lock;
+
+        struct tegra_mc_vi *vi;
+        struct v4l2_subdev *subdev[MAX_SUBDEVICES];
+        struct v4l2_subdev *subdev_on_csi;
+
+        struct v4l2_ctrl_handler ctrl_handler;
+        struct v4l2_pix_format format;
+        const struct tegra_video_format *fmtinfo;
+        const struct tegra_video_format *video_formats[MAX_FORMAT_NUM];
+        unsigned int num_video_formats;
+        struct mutex stop_kthread_lock;
+
+        unsigned char port[TEGRA_CSI_BLOCKS];
+        unsigned int syncpt[TEGRA_CSI_BLOCKS][MAX_SYNCPT_PER_CHANNEL];
+        unsigned int syncpoint_fifo[TEGRA_CSI_BLOCKS];
+        unsigned int buffer_offset[TEGRA_CSI_BLOCKS];
+        unsigned int buffer_state[QUEUED_BUFFERS];
+        struct vb2_v4l2_buffer *buffers[QUEUED_BUFFERS];
+        unsigned int timeout;
+        unsigned int save_index;
+        unsigned int free_index;
+        unsigned int num_buffers;
+        unsigned int released_bufs;
+
+        struct task_struct *kthread_capture_start;
+        wait_queue_head_t start_wait;
+        struct vb2_queue queue;
+        void *alloc_ctx;
+        bool init_done;
+        struct list_head capture;
+        spinlock_t start_lock;
+        struct work_struct status_work;
+        struct work_struct error_work;
+
+        void __iomem *csibase[TEGRA_CSI_BLOCKS];
+        unsigned int stride_align;
+        unsigned int width_align;
+        unsigned int height_align;
+        unsigned int size_align;
+        unsigned int valid_ports;
+        unsigned int total_ports;
+        unsigned int numlanes;
+        unsigned int io_id;
+        unsigned int num_subdevs;
+        unsigned int sequence;
+        unsigned int saved_ctx_bypass;
+        unsigned int saved_ctx_pgmode;
+        unsigned int gang_mode;
+        unsigned int gang_width;
+        unsigned int gang_height;
+        unsigned int gang_bytesperline;
+        unsigned int gang_sizeimage;
+
+        DECLARE_BITMAP(fmts_bitmap, MAX_FORMAT_NUM);
+        atomic_t power_on_refcnt;
+        struct v4l2_fh *fh;
+        bool bypass;
+        bool write_ispformat;
+        enum tegra_vi_pg_mode pg_mode;
+        bool bfirst_fstart;
+        enum channel_capture_state capture_state;
+        spinlock_t capture_state_lock;
+        atomic_t is_streaming;
+        int requested_kbyteps;
+        unsigned long requested_hz;
+
+        struct vi_notify_channel *vnc[TEGRA_CSI_BLOCKS];
+        int vnc_id[TEGRA_CSI_BLOCKS];
+        int grp_id;
+
+        struct vi_capture *capture_data;
+};
+
+#define to_tegra_channel(vdev) \
+        container_of(vdev, struct tegra_channel, video)
+
+/**
+ * struct tegra_mc_vi - NVIDIA Tegra Media controller structure
+ * @v4l2_dev: V4L2 device
+ * @media_dev: media device
+ * @dev: device struct
+ * @tegra_camera: tegra camera structure
+ * @nvhost_device_data: NvHost VI device information
+ *
+ * @notifier: V4L2 asynchronous subdevs notifier
+ * @entities: entities in the graph as a list of tegra_vi_graph_entity
+ * @num_subdevs: number of subdevs in the pipeline
+ *
+ * @channels: list of channels at the pipeline output and input
+ *
+ * @ctrl_handler: V4L2 control handler
+ * @pattern: test pattern generator V4L2 control
+ * @pg_mode: test pattern generator mode (disabled/direct/patch)
+ *
+ * @has_sensors: a flag to indicate whether is a real sensor connecting
+ */
+struct tegra_mc_vi {
+        struct vi *vi;
+        struct platform_device *ndev;
+        struct v4l2_device v4l2_dev;
+        struct media_device media_dev;
+        struct device *dev;
+        struct nvhost_device_data *ndata;
+
+        struct regulator *reg;
+        struct clk *clk;
+        struct clk *parent_clk;
+
+        struct v4l2_async_notifier notifier;
+        struct list_head entities;
+        unsigned int num_channels;
+        unsigned int num_subdevs;
+
+        struct tegra_csi_device *csi;
+        struct list_head vi_chans;
+        struct tegra_channel *tpg_start;
+        void __iomem *iomem;
+
+        struct v4l2_ctrl_handler ctrl_handler;
+        struct v4l2_ctrl *pattern;
+        enum tegra_vi_pg_mode pg_mode;
+
+        bool has_sensors;
+        atomic_t power_on_refcnt;
+        unsigned int link_status;
+        unsigned int subdevs_bound;
+        struct mutex bw_update_lock;
+        unsigned long aggregated_kbyteps;
+        unsigned long max_requested_hz;
+        struct mutex mipical_lock;
+
+        bool bypass;
+
+        struct tegra_vi_fops *fops;
+};
+
+int tegra_vi_get_port_info(struct tegra_channel *chan,
+                        struct device_node *node, unsigned int index);
+void tegra_vi_v4l2_cleanup(struct tegra_mc_vi *vi);
+int tegra_vi_v4l2_init(struct tegra_mc_vi *vi);
+int tegra_vi_tpg_graph_init(struct tegra_mc_vi *vi);
+int tegra_vi_graph_init(struct tegra_mc_vi *vi);
+void tegra_vi_graph_cleanup(struct tegra_mc_vi *vi);
+int tegra_channel_init(struct tegra_channel *chan);
+int tegra_vi_channels_register(struct tegra_mc_vi *vi);
+void tegra_vi_channels_unregister(struct tegra_mc_vi *vi);
+int tegra_vi_channels_init(struct tegra_mc_vi *vi);
+int tegra_channel_cleanup(struct tegra_channel *chan);
+int tegra_vi_channels_cleanup(struct tegra_mc_vi *vi);
+int tegra_channel_init_subdevices(struct tegra_channel *chan);
+int tegra_vi2_power_on(struct tegra_mc_vi *vi);
+void tegra_vi2_power_off(struct tegra_mc_vi *vi);
+int tegra_vi4_power_on(struct tegra_mc_vi *vi);
+void tegra_vi4_power_off(struct tegra_mc_vi *vi);
+int tegra_clean_unlinked_channels(struct tegra_mc_vi *vi);
+int tegra_channel_s_ctrl(struct v4l2_ctrl *ctrl);
+int tegra_vi_media_controller_init(struct tegra_mc_vi *mc_vi,
+                        struct platform_device *pdev);
+void tegra_vi_media_controller_cleanup(struct tegra_mc_vi *mc_vi);
+void tegra_channel_ec_close(struct tegra_mc_vi *mc_vi);
+void tegra_channel_query_hdmiin_unplug(struct tegra_channel *chan,
+                struct v4l2_event *event);
+int tpg_vi_media_controller_init(struct tegra_mc_vi *mc_vi, int pg_mode);
+void tpg_vi_media_controller_cleanup(struct tegra_mc_vi *mc_vi);
+
+u32 tegra_core_get_fourcc_by_idx(struct tegra_channel *chan,
+                unsigned int index);
+int tegra_core_get_idx_by_code(struct tegra_channel *chan,
+                unsigned int code, unsigned offset);
+const struct tegra_video_format *tegra_core_get_format_by_code(
+                struct tegra_channel *chan,
+                unsigned int code, unsigned offset);
+const struct tegra_video_format *tegra_core_get_format_by_fourcc(
+                struct tegra_channel *chan, u32 fourcc);
+void tegra_core_get_description_by_idx(struct tegra_channel *chan,
+                unsigned int index, __u8 *description);
+
+struct tegra_vi_fops {
+        int (*vi_power_on)(struct tegra_channel *chan);
+        void (*vi_power_off)(struct tegra_channel *chan);
+        int (*vi_start_streaming)(struct vb2_queue *vq, u32 count);
+        int (*vi_stop_streaming)(struct vb2_queue *vq);
+        int (*vi_add_ctrls)(struct tegra_channel *chan);
+        void (*vi_init_video_formats)(struct tegra_channel *chan);
+        long (*vi_default_ioctl)(struct file *file, void *fh,
+                        bool use_prio, unsigned int cmd, void *arg);
+};
+
+struct tegra_csi_fops {
+        int (*csi_power_on)(struct tegra_csi_device *csi);
+        int (*csi_power_off)(struct tegra_csi_device *csi);
+        int (*csi_start_streaming)(struct tegra_csi_channel *chan,
+                enum tegra_csi_port_num port_num);
+        void (*csi_stop_streaming)(struct tegra_csi_channel *chan,
+                enum tegra_csi_port_num port_num);
+        void (*csi_override_format)(struct tegra_csi_channel *chan,
+                enum tegra_csi_port_num port_num);
+        int (*mipical)(struct tegra_csi_channel *chan);
+        int (*hw_init)(struct tegra_csi_device *csi);
+};
+
+struct tegra_t210_vi_data {
+        struct nvhost_device_data *info;
+        struct tegra_vi_fops *vi_fops;
+        struct tegra_csi_fops *csi_fops;
+};
+
+struct tegra_vi_data {
+        struct nvhost_device_data *info;
+        struct tegra_vi_fops *vi_fops;
+};
+
+struct tegra_csi_data {
+        struct nvhost_device_data *info;
+        struct tegra_csi_fops *csi_fops;
+};
+#endif
diff --git a/drivers/media/platform/tegra/camera/vi/registers.h b/drivers/media/platform/tegra/camera/vi/registers.h
new file mode 100644
index 000000000..176575dff
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/registers.h
@@ -0,0 +1,223 @@
+/*
+ * drivers/media/platform/tegra/camera/registers.h
+ *
+ * Tegra VI/CSI register offsets
+ *
+ * Copyright (c) 2016, NVIDIA CORPORATION. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __REGISTERS_H__
+#define __REGISTERS_H__
+
+/* VI registers */
+#define TEGRA_VI_SYNCPT_WAIT_TIMEOUT                    200
+#define TEGRA_VI_CFG_VI_INCR_SYNCPT                     0x000
+#define VI_CFG_VI_INCR_SYNCPT_COND(x)                   (x << 8)
+#define VI_CSI_PP_LINE_START(port)                      (4 + (port) * 4)
+#define VI_CSI_PP_FRAME_START(port)                     (5 + (port) * 4)
+#define VI_CSI_MW_REQ_DONE(port)                        (6 + (port) * 4)
+#define VI_CSI_MW_ACK_DONE(port)                        (7 + (port) * 4)
+
+#define TEGRA_VI_CFG_VI_INCR_SYNCPT_CNTRL               0x004
+#define TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR               0x008
+#define TEGRA_VI_CFG_CTXSW                              0x020
+#define TEGRA_VI_CFG_INTSTATUS                          0x024
+#define TEGRA_VI_CFG_PWM_CONTROL                        0x038
+#define TEGRA_VI_CFG_PWM_HIGH_PULSE                     0x03c
+#define TEGRA_VI_CFG_PWM_LOW_PULSE                      0x040
+#define TEGRA_VI_CFG_PWM_SELECT_PULSE_A                 0x044
+#define TEGRA_VI_CFG_PWM_SELECT_PULSE_B                 0x048
+#define TEGRA_VI_CFG_PWM_SELECT_PULSE_C                 0x04c
+#define TEGRA_VI_CFG_PWM_SELECT_PULSE_D                 0x050
+#define TEGRA_VI_CFG_VGP1                               0x064
+#define TEGRA_VI_CFG_VGP2                               0x068
+#define TEGRA_VI_CFG_VGP3                               0x06c
+#define TEGRA_VI_CFG_VGP4                               0x070
+#define TEGRA_VI_CFG_VGP5                               0x074
+#define TEGRA_VI_CFG_VGP6                               0x078
+#define TEGRA_VI_CFG_INTERRUPT_MASK                     0x08c
+#define TEGRA_VI_CFG_INTERRUPT_TYPE_SELECT              0x090
+#define TEGRA_VI_CFG_INTERRUPT_POLARITY_SELECT          0x094
+#define TEGRA_VI_CFG_INTERRUPT_STATUS                   0x098
+#define TEGRA_VI_CFG_VGP_SYNCPT_CONFIG                  0x0ac
+#define TEGRA_VI_CFG_VI_SW_RESET                        0x0b4
+#define TEGRA_VI_CFG_CG_CTRL                            0x0b8
+#define VI_CG_2ND_LEVEL_EN                              0x1
+#define TEGRA_VI_CFG_VI_MCCIF_FIFOCTRL                  0x0e4
+#define TEGRA_VI_CFG_TIMEOUT_WCOAL_VI                   0x0e8
+#define TEGRA_VI_CFG_DVFS                               0x0f0
+#define TEGRA_VI_CFG_RESERVE                            0x0f4
+#define TEGRA_VI_CFG_RESERVE_1                          0x0f8
+
+/* CSI registers */
+#define TEGRA_VI_CSI_BASE(x)                            (0x100 + (x) * 0x100)
+
+#define TEGRA_VI_CSI_SW_RESET                           0x000
+#define TEGRA_VI_CSI_SINGLE_SHOT                        0x004
+#define SINGLE_SHOT_CAPTURE                             0x1
+#define CAPTURE_GOOD_FRAME                              0x1
+#define TEGRA_VI_CSI_SINGLE_SHOT_STATE_UPDATE           0x008
+#define TEGRA_VI_CSI_IMAGE_DEF                          0x00c
+#define BYPASS_PXL_TRANSFORM_OFFSET                     24
+#define IMAGE_DEF_FORMAT_OFFSET                         16
+#define IMAGE_DEF_DEST_MEM                              0x1
+#define TEGRA_VI_CSI_RGB2Y_CTRL                         0x010
+#define TEGRA_VI_CSI_MEM_TILING                         0x014
+#define TEGRA_VI_CSI_IMAGE_SIZE                         0x018
+#define IMAGE_SIZE_HEIGHT_OFFSET                        16
+#define TEGRA_VI_CSI_IMAGE_SIZE_WC                      0x01c
+#define TEGRA_VI_CSI_IMAGE_DT                           0x020
+#define TEGRA_VI_CSI_SURFACE0_OFFSET_MSB                0x024
+#define TEGRA_VI_CSI_SURFACE0_OFFSET_LSB                0x028
+#define TEGRA_VI_CSI_SURFACE1_OFFSET_MSB                0x02c
+#define TEGRA_VI_CSI_SURFACE1_OFFSET_LSB                0x030
+#define TEGRA_VI_CSI_SURFACE2_OFFSET_MSB                0x034
+#define TEGRA_VI_CSI_SURFACE2_OFFSET_LSB                0x038
+#define TEGRA_VI_CSI_SURFACE0_BF_OFFSET_MSB             0x03c
+#define TEGRA_VI_CSI_SURFACE0_BF_OFFSET_LSB             0x040
+#define TEGRA_VI_CSI_SURFACE1_BF_OFFSET_MSB             0x044
+#define TEGRA_VI_CSI_SURFACE1_BF_OFFSET_LSB             0x048
+#define TEGRA_VI_CSI_SURFACE2_BF_OFFSET_MSB             0x04c
+#define TEGRA_VI_CSI_SURFACE2_BF_OFFSET_LSB             0x050
+#define TEGRA_VI_CSI_SURFACE0_STRIDE                    0x054
+#define TEGRA_VI_CSI_SURFACE1_STRIDE                    0x058
+#define TEGRA_VI_CSI_SURFACE2_STRIDE                    0x05c
+#define TEGRA_VI_CSI_SURFACE_HEIGHT0                    0x060
+#define TEGRA_VI_CSI_ISPINTF_CONFIG                     0x064
+#define TEGRA_VI_CSI_ERROR_STATUS                       0x084
+#define TEGRA_VI_CSI_ERROR_INT_MASK                     0x088
+#define TEGRA_VI_CSI_WD_CTRL                            0x08c
+#define TEGRA_VI_CSI_WD_PERIOD                          0x090
+
+/* CSI Pixel Parser registers: Starts from 0x838, offset 0x0 */
+#define TEGRA_CSI_INPUT_STREAM_CONTROL                  0x000
+#define CSI_SKIP_PACKET_THRESHOLD_OFFSET                16
+
+#define TEGRA_CSI_PIXEL_STREAM_CONTROL0                 0x004
+#define CSI_PP_PACKET_HEADER_SENT                       (0x1 << 4)
+#define CSI_PP_DATA_IDENTIFIER_ENABLE                   (0x1 << 5)
+#define CSI_PP_WORD_COUNT_SELECT_HEADER                 (0x1 << 6)
+#define CSI_PP_CRC_CHECK_ENABLE                         (0x1 << 7)
+#define CSI_PP_WC_CHECK                                 (0x1 << 8)
+#define CSI_PP_OUTPUT_FORMAT_STORE                      (0x3 << 16)
+#define CSI_PPA_PAD_LINE_NOPAD                          (0x2 << 24)
+#define CSI_PP_HEADER_EC_DISABLE                        (0x1 << 27)
+#define CSI_PPA_PAD_FRAME_NOPAD                         (0x2 << 28)
+
+#define TEGRA_CSI_PIXEL_STREAM_CONTROL1                 0x008
+#define CSI_PP_TOP_FIELD_FRAME_OFFSET                   0
+#define CSI_PP_TOP_FIELD_FRAME_MASK_OFFSET              4
+
+#define TEGRA_CSI_PIXEL_STREAM_GAP                      0x00c
+#define PP_FRAME_MIN_GAP_OFFSET                         16
+
+#define TEGRA_CSI_PIXEL_STREAM_PP_COMMAND               0x010
+#define CSI_PP_ENABLE                                   0x1
+#define CSI_PP_DISABLE                                  0x2
+#define CSI_PP_RST                                      0x3
+#define CSI_PP_SINGLE_SHOT_ENABLE                       (0x1 << 2)
+#define CSI_PP_START_MARKER_FRAME_MAX_OFFSET            12
+
+#define TEGRA_CSI_PIXEL_STREAM_EXPECTED_FRAME           0x014
+#define TEGRA_CSI_PIXEL_PARSER_INTERRUPT_MASK           0x018
+#define TEGRA_CSI_PIXEL_PARSER_STATUS                   0x01c
+#define TEGRA_CSI_CSI_SW_SENSOR_RESET                   0x020
+
+/* CSI PHY registers */
+/* CSI_PHY_CIL_COMMAND_0 offset 0x0d0 from TEGRA_CSI_PIXEL_PARSER_0_BASE */
+#define TEGRA_CSI_PHY_CIL_COMMAND                       0x0d0
+#define CSI_A_PHY_CIL_ENABLE                            0x1
+#define CSI_B_PHY_CIL_ENABLE                            (0x1 << 8)
+
+/* CSI CIL registers: Starts from 0x92c, offset 0xF4 */
+#define TEGRA_CSI_CIL_OFFSET                            0x0f4
+
+#define TEGRA_CSI_CIL_PAD_CONFIG0                       0x000
+#define BRICK_CLOCK_A_4X                                (0x1 << 16)
+#define BRICK_CLOCK_B_4X                                (0x2 << 16)
+#define TEGRA_CSI_CIL_PAD_CONFIG1                       0x004
+#define TEGRA_CSI_CIL_PHY_CONTROL                       0x008
+#define BYPASS_LP_SEQ                                   (0x1 << 6)
+#define TEGRA_CSI_CIL_INTERRUPT_MASK                    0x00c
+#define TEGRA_CSI_CIL_STATUS                            0x010
+#define TEGRA_CSI_CILX_STATUS                           0x014
+#define TEGRA_CSI_CIL_ESCAPE_MODE_COMMAND               0x018
+#define TEGRA_CSI_CIL_ESCAPE_MODE_DATA                  0x01c
+#define TEGRA_CSI_CIL_SW_SENSOR_RESET                   0x020
+
+/* CSI Pattern Generator registers: Starts from 0x9c4, offset 0x18c */
+#define TEGRA_CSI_TPG_OFFSET                            0x18c
+
+#define TEGRA_CSI_PATTERN_GENERATOR_CTRL                0x000
+#define PG_MODE_OFFSET                                  2
+#define PG_ENABLE                                       0x1
+#define PG_DISABLE                                      0x0
+
+#define PG_VBLANK_OFFSET                                16
+#define TEGRA_CSI_PG_BLANK                              0x004
+#define TEGRA_CSI_PG_PHASE                              0x008
+#define TEGRA_CSI_PG_RED_FREQ                           0x00c
+#define PG_RED_VERT_INIT_FREQ_OFFSET                    16
+#define PG_RED_HOR_INIT_FREQ_OFFSET                     0
+
+#define TEGRA_CSI_PG_RED_FREQ_RATE                      0x010
+#define TEGRA_CSI_PG_GREEN_FREQ                         0x014
+#define PG_GREEN_VERT_INIT_FREQ_OFFSET                  16
+#define PG_GREEN_HOR_INIT_FREQ_OFFSET                   0
+
+#define TEGRA_CSI_PG_GREEN_FREQ_RATE                    0x018
+#define TEGRA_CSI_PG_BLUE_FREQ                          0x01c
+#define PG_BLUE_VERT_INIT_FREQ_OFFSET                   16
+#define PG_BLUE_HOR_INIT_FREQ_OFFSET                    0
+
+#define TEGRA_CSI_PG_BLUE_FREQ_RATE                     0x020
+#define TEGRA_CSI_PG_AOHDR                              0x024
+
+#define TEGRA_CSI_DPCM_CTRL_A                           0xa2c
+#define TEGRA_CSI_DPCM_CTRL_B                           0xa30
+
+/* Other CSI registers: Starts from 0xa44, offset 0x20c */
+#define TEGRA_CSI_STALL_COUNTER                         0x20c
+#define TEGRA_CSI_CSI_READONLY_STATUS                   0x210
+#define TEGRA_CSI_CSI_SW_STATUS_RESET                   0x214
+#define TEGRA_CSI_CLKEN_OVERRIDE                        0x218
+#define TEGRA_CSI_DEBUG_CONTROL                         0x21c
+#define TEGRA_CSI_DEBUG_COUNTER_0                       0x220
+#define TEGRA_CSI_DEBUG_COUNTER_1                       0x224
+#define TEGRA_CSI_DEBUG_COUNTER_2                       0x228
+
+
+/* CSI Pixel Parser registers */
+#define TEGRA_CSI_PIXEL_PARSER_0_BASE                   0x0838
+#define TEGRA_CSI_PIXEL_PARSER_1_BASE                   0x086c
+#define TEGRA_CSI_PIXEL_PARSER_2_BASE                   0x1038
+#define TEGRA_CSI_PIXEL_PARSER_3_BASE                   0x106c
+#define TEGRA_CSI_PIXEL_PARSER_4_BASE                   0x1838
+#define TEGRA_CSI_PIXEL_PARSER_5_BASE                   0x186c
+
+/* CSIA to CSIB register offset */
+#define TEGRA_CSI_PORT_OFFSET                           0x34
+
+#define INVALID_CSI_PORT                                0xFF
+#define TEGRA_CSI_BLOCKS                                3
+#define SYNCPT_FIFO_DEPTH                               2
+#define PREVIOUS_BUFFER_DEC_INDEX                       2
+
+#define TEGRA_CLOCK_VI_MAX                              793600000
+#define TEGRA_CLOCK_TPG                                 927000000
+#define TEGRA_CLOCK_CSI_PORT_MAX                        102000000
+
+#define TEGRA_SURFACE_ALIGNMENT                         64
+#endif
diff --git a/drivers/media/platform/tegra/camera/vi/vi2_fops.c b/drivers/media/platform/tegra/camera/vi/vi2_fops.c
new file mode 100644
index 000000000..fa081b9e0
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/vi2_fops.c
@@ -0,0 +1,825 @@
+/*
+ * Tegra Video Input 2 device common APIs
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/device.h>
+#include <linux/nvhost.h>
+#include <linux/tegra-powergate.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <media/tegra_camera_platform.h>
+#include "nvhost_acm.h"
+#include "camera/vi/mc_common.h"
+#include "camera/csi/csi2_fops.h"
+#include "vi2_formats.h"
+#include "vi/vi.h"
+
+#define DEFAULT_FRAMERATE       30
+#define DEFAULT_CSI_FREQ        204000000
+#define BPP_MEM         2
+#define NUM_PPC         2
+#define VI_CSI_CLK_SCALE        110
+
+extern void tegra_channel_queued_buf_done(struct tegra_channel *chan,
+                                          enum vb2_buffer_state state);
+static void tegra_channel_stop_kthreads(struct tegra_channel *chan);
+extern int tegra_channel_set_stream(struct tegra_channel *chan, bool on);
+extern void tegra_channel_ring_buffer(struct tegra_channel *chan,
+                                struct vb2_v4l2_buffer *vb,
+                                struct timespec *ts, int state);
+extern struct tegra_channel_buffer *dequeue_buffer(struct tegra_channel *chan);
+extern void tegra_channel_init_ring_buffer(struct tegra_channel *chan);
+extern void free_ring_buffers(struct tegra_channel *chan, int frames);
+extern int tegra_channel_set_power(struct tegra_channel *chan, bool on);
+
+static void vi_write(struct tegra_mc_vi *vi, unsigned int addr, u32 val)
+{
+        writel(val, vi->iomem + addr);
+}
+
+static u32 tegra_channel_read(struct tegra_channel *chan,
+                        unsigned int addr)
+{
+        return readl(chan->vi->iomem + addr);
+}
+
+static void tegra_channel_write(struct tegra_channel *chan,
+                        unsigned int addr, u32 val)
+{
+        writel(val, chan->vi->iomem + addr);
+}
+
+/* CSI registers */
+static void csi_write(struct tegra_channel *chan, unsigned int index,
+                        unsigned int addr, u32 val)
+{
+        writel(val, chan->csibase[index] + addr);
+}
+
+static u32 csi_read(struct tegra_channel *chan, unsigned int index,
+                                        unsigned int addr)
+{
+        return readl(chan->csibase[index] + addr);
+}
+
+static void vi_channel_syncpt_init(struct tegra_channel *chan)
+{
+        int i;
+
+        for (i = 0; i < chan->total_ports; i++)
+                chan->syncpt[i][0] =
+                        nvhost_get_syncpt_client_managed(chan->vi->ndev, "vi");
+}
+
+static void vi_channel_syncpt_free(struct tegra_channel *chan)
+{
+        int i;
+
+        for (i = 0; i < chan->total_ports; i++)
+                nvhost_syncpt_put_ref_ext(chan->vi->ndev, chan->syncpt[i][0]);
+}
+
+void vi2_init_video_formats(struct tegra_channel *chan)
+{
+        int i;
+
+        chan->num_video_formats = ARRAY_SIZE(vi2_video_formats);
+        for (i = 0; i < chan->num_video_formats; i++)
+                chan->video_formats[i] = &vi2_video_formats[i];
+}
+
+int tegra_vi2_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct tegra_channel *chan = container_of(ctrl->handler,
+                                struct tegra_channel, ctrl_handler);
+        int err = 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_WRITE_ISPFORMAT:
+                chan->write_ispformat = ctrl->val;
+                break;
+        default:
+                dev_err(&chan->video.dev, "%s:Not valid ctrl\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static const struct v4l2_ctrl_ops vi2_ctrl_ops = {
+        .s_ctrl = tegra_vi2_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vi2_custom_ctrls[] = {
+        {
+                .ops = &vi2_ctrl_ops,
+                .id = V4L2_CID_WRITE_ISPFORMAT,
+                .name = "Write ISP format",
+                .type = V4L2_CTRL_TYPE_BOOLEAN,
+                .def = 0,
+                .min = 0,
+                .step = 1,
+                .max = 1,
+        },
+};
+
+int vi2_add_ctrls(struct tegra_channel *chan)
+{
+        int i;
+
+        /* Add vi2 custom controls */
+        for (i = 0; i < ARRAY_SIZE(vi2_custom_ctrls); i++) {
+                v4l2_ctrl_new_custom(&chan->ctrl_handler,
+                        &vi2_custom_ctrls[i], NULL);
+                if (chan->ctrl_handler.error) {
+                        dev_err(chan->vi->dev,
+                                "Failed to add %s ctrl\n",
+                                vi2_custom_ctrls[i].name);
+                        return chan->ctrl_handler.error;
+                }
+        }
+
+        return 0;
+}
+
+static struct tegra_csi_channel *find_linked_csi_channel(
+        struct tegra_channel *chan, struct tegra_csi_device *csi)
+{
+        struct tegra_csi_channel *csi_it;
+        struct tegra_csi_channel *csi_chan = NULL;
+        int i;
+        /* Find connected csi_channel */
+        list_for_each_entry(csi_it, &csi->csi_chans, list) {
+                for (i = 0; i < chan->num_subdevs; i++) {
+                        if (chan->subdev[i] == &csi_it->subdev) {
+                                csi_chan = csi_it;
+                                break;
+                        }
+                }
+        }
+        return csi_chan;
+}
+static int tegra_channel_capture_setup(struct tegra_channel *chan)
+{
+        u32 height = chan->format.height;
+        u32 width = chan->format.width;
+        u32 format = chan->fmtinfo->img_fmt;
+        u32 data_type = chan->fmtinfo->img_dt;
+        u32 word_count = tegra_core_get_word_count(width, chan->fmtinfo);
+        u32 bypass_pixel_transform = 1;
+        int index;
+
+        if (chan->valid_ports > 1) {
+                height = chan->gang_height;
+                width = chan->gang_width;
+                word_count = tegra_core_get_word_count(width, chan->fmtinfo);
+        }
+
+        if (chan->pg_mode ||
+           (chan->write_ispformat == TEGRA_ISP_FORMAT) ||
+           (chan->fmtinfo->vf_code == TEGRA_VF_YUV422) ||
+           (chan->fmtinfo->vf_code == TEGRA_VF_RGB888))
+                bypass_pixel_transform = 0;
+
+        for (index = 0; index < chan->valid_ports; index++) {
+                csi_write(chan, index, TEGRA_VI_CSI_ERROR_STATUS, 0xFFFFFFFF);
+                csi_write(chan, index, TEGRA_VI_CSI_IMAGE_DEF,
+                  (bypass_pixel_transform << BYPASS_PXL_TRANSFORM_OFFSET) |
+                  (format << IMAGE_DEF_FORMAT_OFFSET));
+                csi_write(chan, index, TEGRA_VI_CSI_IMAGE_DT, data_type);
+                csi_write(chan, index, TEGRA_VI_CSI_IMAGE_SIZE_WC, word_count);
+                csi_write(chan, index, TEGRA_VI_CSI_IMAGE_SIZE,
+                        (height << IMAGE_SIZE_HEIGHT_OFFSET) | width);
+        }
+
+        return 0;
+}
+
+static int tegra_channel_enable_stream(struct tegra_channel *chan)
+{
+        int ret = 0;
+        struct tegra_csi_channel *csi_chan = NULL;
+        struct tegra_csi_device *csi = chan->vi->csi;
+        /*
+         * enable pad power and perform calibration before arming
+         * single shot for first frame after the HW setup is complete
+         */
+        /* Find connected csi_channel */
+        csi_chan = find_linked_csi_channel(chan, csi);
+
+        /* start streaming */
+        ret = tegra_channel_set_stream(chan, true);
+        return ret;
+}
+
+static void tegra_channel_ec_init(struct tegra_channel *chan)
+{
+        /*
+         * error recover initialization sequence
+         * set timeout as 200 ms, use default if fps not available
+         * Time limit allow CSI to capture good frames and drop error frames
+         * Timeout units is jiffies, 1 jiffy = 10ms
+         * TODO: Get frame rate from sub-device and adopt timeout
+         */
+        chan->timeout = 20;
+
+        /*
+         * Sync point FIFO full blocks host interface
+         * Below setting enables SW to process error recovery
+         */
+        tegra_channel_write(chan, TEGRA_VI_CFG_VI_INCR_SYNCPT_CNTRL, 0x100);
+}
+
+
+static void tegra_channel_clear_singleshot(struct tegra_channel *chan,
+                                                int index)
+{
+        /* clear single shot */
+        csi_write(chan, index, TEGRA_VI_CSI_SW_RESET, 0xF);
+        csi_write(chan, index, TEGRA_VI_CSI_SW_RESET, 0x0);
+}
+
+static void tegra_channel_vi_csi_recover(struct tegra_channel *chan)
+{
+        u32 error_val = tegra_channel_read(chan,
+                                        TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR);
+        u32 frame_start;
+        int index, valid_ports = chan->valid_ports;
+        struct tegra_csi_channel *csi_chan;
+        struct tegra_csi_device *csi = chan->vi->csi;
+
+        /* Disable clock gating to enable continuous clock */
+        tegra_channel_write(chan, TEGRA_VI_CFG_CG_CTRL, DISABLE);
+        /* Find connected csi_channel */
+        csi_chan = find_linked_csi_channel(chan, csi);
+
+        /* clear CSI state */
+        for (index = 0; index < valid_ports; index++) {
+                tegra_csi_error_recover(csi_chan, index);
+                csi_write(chan, index,
+                                TEGRA_VI_CSI_IMAGE_DEF, 0);
+                tegra_channel_clear_singleshot(chan, index);
+        }
+
+        /* clear VI errors */
+        for (index = 0; index < valid_ports; index++) {
+                frame_start = VI_CSI_PP_FRAME_START(chan->port[index]);
+                if (error_val & frame_start)
+                        chan->syncpoint_fifo[index] = SYNCPT_FIFO_DEPTH;
+        }
+        /* clear FIFO error status */
+        tegra_channel_write(chan,
+                TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR, error_val);
+
+        /* Enable clock gating so VI can be clock gated if necessary */
+        tegra_channel_write(chan, TEGRA_VI_CFG_CG_CTRL, ENABLE);
+
+        /* re-init VI and CSI */
+        tegra_channel_capture_setup(chan);
+        for (index = 0; index < valid_ports; index++) {
+                csi2_stop_streaming(csi_chan, index);
+                csi2_start_streaming(csi_chan, index);
+                nvhost_syncpt_set_min_eq_max_ext(chan->vi->ndev,
+                                                chan->syncpt[index][0]);
+        }
+}
+
+static void tegra_channel_capture_error(struct tegra_channel *chan)
+{
+        u32 val;
+        int index = 0;
+        struct tegra_csi_channel *csi_chan;
+        struct tegra_csi_device *csi = chan->vi->csi;
+
+        /* Find connected csi_channel */
+        csi_chan = find_linked_csi_channel(chan, csi);
+
+        for (index = 0; index < chan->valid_ports; index++) {
+                val = csi_read(chan, index, TEGRA_VI_CSI_ERROR_STATUS);
+                dev_dbg(&chan->video.dev,
+                        "TEGRA_VI_CSI_ERROR_STATUS 0x%08x\n", val);
+                tegra_csi_status(csi_chan, index);
+        }
+}
+
+static void tegra_channel_ec_recover(struct tegra_channel *chan)
+{
+        tegra_channel_capture_error(chan);
+        tegra_channel_vi_csi_recover(chan);
+}
+
+static int tegra_channel_error_status(struct tegra_channel *chan)
+{
+        u32 val;
+        int err = 0;
+        int index = 0;
+        struct tegra_csi_channel *csi_chan;
+        struct tegra_csi_device *csi = chan->vi->csi;
+
+        /* Find connected csi_channel */
+        csi_chan = find_linked_csi_channel(chan, csi);
+
+        for (index = 0; index < chan->valid_ports; index++) {
+                /* Ignore error based on resolution but reset status */
+                val = csi_read(chan, index, TEGRA_VI_CSI_ERROR_STATUS);
+                csi_write(chan, index, TEGRA_VI_CSI_ERROR_STATUS, val);
+                err = tegra_csi_error(csi_chan, index);
+        }
+
+        if (err)
+                dev_err(chan->vi->dev, "%s:error %x frame %d\n",
+                                __func__, err, chan->sequence);
+        return err;
+}
+
+static int tegra_channel_capture_frame(struct tegra_channel *chan,
+                                        struct tegra_channel_buffer *buf)
+{
+        struct vb2_v4l2_buffer *vb = &buf->buf;
+        struct timespec ts;
+        int err = 0;
+        u32 val, frame_start;
+        int bytes_per_line = chan->format.bytesperline;
+        int index = 0;
+        u32 thresh[TEGRA_CSI_BLOCKS] = { 0 };
+        int valid_ports = chan->valid_ports;
+        int state = VB2_BUF_STATE_DONE;
+
+        /* Init registers related to each frames */
+        for (index = 0; index < valid_ports; index++) {
+                /* Program buffer address by using surface 0 */
+                csi_write(chan, index, TEGRA_VI_CSI_SURFACE0_OFFSET_MSB, 0x0);
+                csi_write(chan, index, TEGRA_VI_CSI_SURFACE0_OFFSET_LSB,
+                        (buf->addr + chan->buffer_offset[index]));
+                csi_write(chan, index,
+                        TEGRA_VI_CSI_SURFACE0_STRIDE, bytes_per_line);
+
+                /* Program syncpoints */
+                thresh[index] = nvhost_syncpt_incr_max_ext(chan->vi->ndev,
+                                        chan->syncpt[index][0], 1);
+                /* Do not arm sync points if FIFO had entries before */
+                if (!chan->syncpoint_fifo[index]) {
+                        frame_start = VI_CSI_PP_FRAME_START(chan->port[index]);
+                        val = VI_CFG_VI_INCR_SYNCPT_COND(frame_start) |
+                                chan->syncpt[index][0];
+                        tegra_channel_write(chan,
+                                TEGRA_VI_CFG_VI_INCR_SYNCPT, val);
+                } else
+                        chan->syncpoint_fifo[index]--;
+        }
+
+        /* enable input stream once the VI registers are configured */
+        if (!chan->bfirst_fstart) {
+                err = tegra_channel_enable_stream(chan);
+                if (err) {
+                        state = VB2_BUF_STATE_ERROR;
+                        chan->capture_state = CAPTURE_ERROR;
+                        tegra_channel_ring_buffer(chan, vb, &ts, state);
+                        return err;
+                }
+                /* Bit controls VI memory write, enable after all regs */
+                for (index = 0; index < valid_ports; index++) {
+                        val = csi_read(chan, index,
+                                        TEGRA_VI_CSI_IMAGE_DEF);
+                        csi_write(chan, index, TEGRA_VI_CSI_IMAGE_DEF,
+                                        val | IMAGE_DEF_DEST_MEM);
+                }
+        }
+
+        /* Ensure all CSI ports are ready with setup to avoid timing issue */
+        for (index = 0; index < valid_ports; index++)
+                csi_write(chan, index,
+                        TEGRA_VI_CSI_SINGLE_SHOT, SINGLE_SHOT_CAPTURE);
+
+        chan->capture_state = CAPTURE_GOOD;
+        for (index = 0; index < valid_ports; index++) {
+                err = nvhost_syncpt_wait_timeout_ext(chan->vi->ndev,
+                        chan->syncpt[index][0], thresh[index],
+                        chan->timeout, NULL, &ts);
+                if (err) {
+                        dev_err(&chan->video.dev,
+                                "frame start syncpt timeout!%d\n", index);
+                        state = VB2_BUF_STATE_ERROR;
+                        /* perform error recovery for timeout */
+                        tegra_channel_ec_recover(chan);
+                        chan->capture_state = CAPTURE_TIMEOUT;
+                        break;
+                }
+        }
+
+        if (!err && !chan->pg_mode) {
+                /* Marking error frames and resume capture */
+                /* TODO: TPG has frame height short error always set */
+                err = tegra_channel_error_status(chan);
+                if (err) {
+                        state = VB2_BUF_STATE_ERROR;
+                        chan->capture_state = CAPTURE_ERROR;
+                        /* do we have to run recover here ?? */
+                        /* tegra_channel_ec_recover(chan); */
+                }
+        }
+
+        tegra_channel_ring_buffer(chan, vb, &ts, state);
+
+        return 0;
+}
+
+static void tegra_channel_capture_done(struct tegra_channel *chan)
+{
+        struct timespec ts;
+        int index, err;
+        int bytes_per_line = chan->format.bytesperline;
+        u32 val, mw_ack_done;
+        u32 thresh[TEGRA_CSI_BLOCKS] = { 0 };
+        struct tegra_channel_buffer *buf;
+        int state = VB2_BUF_STATE_DONE;
+
+        /* dequeue buffer and return if no buffer exists */
+        buf = dequeue_buffer(chan);
+        if (!buf)
+                return;
+
+        for (index = 0; index < chan->valid_ports; index++) {
+                /* Program buffer address by using surface 0 */
+                csi_write(chan, index, TEGRA_VI_CSI_SURFACE0_OFFSET_MSB, 0x0);
+                csi_write(chan, index, TEGRA_VI_CSI_SURFACE0_OFFSET_LSB,
+                        (buf->addr + chan->buffer_offset[index]));
+                csi_write(chan, index,
+                        TEGRA_VI_CSI_SURFACE0_STRIDE, bytes_per_line);
+
+                /* Program syncpoints */
+                thresh[index] = nvhost_syncpt_incr_max_ext(chan->vi->ndev,
+                                        chan->syncpt[index][0], 1);
+                mw_ack_done = VI_CSI_MW_ACK_DONE(chan->port[index]);
+                val = VI_CFG_VI_INCR_SYNCPT_COND(mw_ack_done) |
+                                chan->syncpt[index][0];
+                tegra_channel_write(chan, TEGRA_VI_CFG_VI_INCR_SYNCPT, val);
+                csi_write(chan, index,
+                        TEGRA_VI_CSI_SINGLE_SHOT, SINGLE_SHOT_CAPTURE);
+        }
+
+        for (index = 0; index < chan->valid_ports; index++) {
+                err = nvhost_syncpt_wait_timeout_ext(chan->vi->ndev,
+                        chan->syncpt[index][0], thresh[index],
+                        chan->timeout, NULL, &ts);
+                if (err) {
+                        dev_err(&chan->video.dev,
+                                "MW_ACK_DONE syncpoint time out!%d\n", index);
+                        state = VB2_BUF_STATE_ERROR;
+                        /* perform error recovery for timeout */
+                        tegra_channel_ec_recover(chan);
+                        chan->capture_state = CAPTURE_TIMEOUT;
+                        break;
+                }
+        }
+
+        /* Mark capture state to IDLE as capture is finished */
+        chan->capture_state = CAPTURE_IDLE;
+
+        tegra_channel_ring_buffer(chan, &buf->buf, &ts, state);
+}
+
+static int tegra_channel_kthread_capture_start(void *data)
+{
+        struct tegra_channel *chan = data;
+        struct tegra_channel_buffer *buf;
+        int err = 0;
+
+        set_freezable();
+
+        while (1) {
+
+                try_to_freeze();
+
+                wait_event_interruptible(chan->start_wait,
+                                         !list_empty(&chan->capture) ||
+                                         kthread_should_stop());
+
+                if (kthread_should_stop())
+                        break;
+
+                /* source is not streaming if error is non-zero */
+                /* wait till kthread stop and dont DeQ buffers */
+                if (err)
+                        continue;
+
+                buf = dequeue_buffer(chan);
+                if (!buf)
+                        continue;
+
+                err = tegra_channel_capture_frame(chan, buf);
+        }
+
+        return 0;
+}
+
+static void tegra_channel_stop_kthreads(struct tegra_channel *chan)
+{
+        mutex_lock(&chan->stop_kthread_lock);
+        /* Stop the kthread for capture */
+        if (chan->kthread_capture_start) {
+                kthread_stop(chan->kthread_capture_start);
+                chan->kthread_capture_start = NULL;
+        }
+        mutex_unlock(&chan->stop_kthread_lock);
+}
+
+static int tegra_channel_update_clknbw(
+        struct tegra_channel *chan, u8 on) __maybe_unused;
+static int tegra_channel_update_clknbw(struct tegra_channel *chan, u8 on)
+{
+        int ret = 0;
+        unsigned long request_pixelrate;
+        struct v4l2_subdev_frame_interval fie;
+        unsigned long csi_freq = 0;
+
+        fie.interval.denominator = DEFAULT_FRAMERATE;
+        fie.interval.numerator = 1;
+
+        if (v4l2_subdev_has_op(chan->subdev_on_csi,
+                                video, g_frame_interval))
+                v4l2_subdev_call(chan->subdev_on_csi, video,
+                                g_frame_interval, &fie);
+        if (on) {
+                /**
+                 * TODO: use real sensor pixelrate
+                 * See PowerService code
+                 */
+                request_pixelrate = (long long)(chan->format.width
+                                * chan->format.height
+                                * fie.interval.denominator / 100)
+                                * VI_CSI_CLK_SCALE;
+                /* for PG, get csi frequency from nvhost */
+                if (chan->pg_mode) {
+                        ret = nvhost_module_get_rate(
+                                        chan->vi->csi->pdev, &csi_freq, 0);
+                        csi_freq = ret ? DEFAULT_CSI_FREQ : csi_freq;
+                } else
+                        /* Use default csi4 frequency for t186 for now
+                         * We can't get the frequency from nvhost because
+                         * vi4 does not has access to csi4
+                         */
+                        csi_freq = DEFAULT_CSI_FREQ;
+
+                /* VI clk should be slightly faster than CSI clk*/
+                ret = nvhost_module_set_rate(chan->vi->ndev, &chan->video,
+                                max(request_pixelrate,
+                                csi_freq * VI_CSI_CLK_SCALE * NUM_PPC / 100),
+                                0, NVHOST_PIXELRATE);
+                if (ret) {
+                        dev_err(chan->vi->dev, "Fail to update vi clk\n");
+                        return ret;
+                }
+        } else {
+                ret = nvhost_module_set_rate(chan->vi->ndev, &chan->video, 0, 0,
+                                NVHOST_PIXELRATE);
+                if (ret) {
+                        dev_err(chan->vi->dev, "Fail to update vi clk\n");
+                        return ret;
+                }
+        }
+
+        chan->requested_kbyteps = (on > 0 ? 1 : -1) *
+                ((long long)(chan->format.width * chan->format.height
+                * fie.interval.denominator * BPP_MEM) * 115 / 100) / 1000;
+
+        mutex_lock(&chan->vi->bw_update_lock);
+        chan->vi->aggregated_kbyteps += chan->requested_kbyteps;
+        ret = vi_v4l2_update_isobw(chan->vi->aggregated_kbyteps, 0);
+        mutex_unlock(&chan->vi->bw_update_lock);
+        if (ret)
+                dev_info(chan->vi->dev,
+                "WAR:Calculation not precise.Ignore BW request failure\n");
+#if 0
+        ret = vi4_v4l2_set_la(chan->vi->ndev, 0, 0);
+        if (ret)
+                dev_info(chan->vi->dev,
+                "WAR:Calculation not precise.Ignore LA failure\n");
+#endif
+        return 0;
+}
+
+int vi2_channel_start_streaming(struct vb2_queue *vq, u32 count)
+{
+        struct tegra_channel *chan = vb2_get_drv_priv(vq);
+        struct media_pipeline *pipe = chan->video.entity.pipe;
+        int ret = 0, i;
+        struct tegra_csi_channel *csi_chan = NULL;
+        struct tegra_csi_device *csi = chan->vi->csi;
+        struct v4l2_ctrl *override_ctrl;
+
+        vi_channel_syncpt_init(chan);
+
+        tegra_channel_ec_init(chan);
+
+        /* Start the pipeline. */
+        ret = media_entity_pipeline_start(&chan->video.entity, pipe);
+        if (ret < 0)
+                goto error_pipeline_start;
+
+        if (chan->bypass) {
+                ret = tegra_channel_set_stream(chan, true);
+                if (ret < 0)
+                        goto error_set_stream;
+                return ret;
+        }
+        chan->capture_state = CAPTURE_IDLE;
+        /* Find connected csi_channel */
+        csi_chan = find_linked_csi_channel(chan, csi);
+
+        if (!csi_chan)
+                goto error_set_stream;
+        for (i = 0; i < chan->valid_ports; i++) {
+                /* csi2_start_streaming(csi_chan, i); */
+                /* ensure sync point state is clean */
+                nvhost_syncpt_set_min_eq_max_ext(chan->vi->ndev,
+                                                        chan->syncpt[i][0]);
+        }
+
+        /* Note: Program VI registers after TPG, sensors and CSI streaming */
+        ret = tegra_channel_capture_setup(chan);
+        if (ret < 0)
+                goto error_capture_setup;
+
+        chan->sequence = 0;
+        tegra_channel_init_ring_buffer(chan);
+
+        /* disable override for vi mode */
+        override_ctrl = v4l2_ctrl_find(
+                &chan->ctrl_handler, V4L2_CID_OVERRIDE_ENABLE);
+        if (!chan->pg_mode) {
+                if (override_ctrl) {
+                        ret = v4l2_ctrl_s_ctrl(override_ctrl, false);
+                        if (ret < 0)
+                                dev_err(&chan->video.dev,
+                                        "failed to disable override control\n");
+                } else
+                        dev_err(&chan->video.dev,
+                                "No override control\n");
+        }
+        /* Update clock and bandwidth based on the format */
+        tegra_channel_update_clknbw(chan, 1);
+
+        /* Start kthread to capture data to buffer */
+        chan->kthread_capture_start = kthread_run(
+                                        tegra_channel_kthread_capture_start,
+                                        chan, chan->video.name);
+        if (IS_ERR(chan->kthread_capture_start)) {
+                dev_err(&chan->video.dev,
+                        "failed to run kthread for capture start\n");
+                ret = PTR_ERR(chan->kthread_capture_start);
+                goto error_capture_setup;
+        }
+
+        return 0;
+
+error_capture_setup:
+        if (!chan->pg_mode)
+                tegra_channel_set_stream(chan, false);
+error_set_stream:
+        if (!chan->pg_mode)
+                media_entity_pipeline_stop(&chan->video.entity);
+error_pipeline_start:
+        vq->start_streaming_called = 0;
+        tegra_channel_queued_buf_done(chan, VB2_BUF_STATE_QUEUED);
+
+        return ret;
+}
+
+void vi2_channel_stop_streaming(struct vb2_queue *vq)
+{
+        struct tegra_channel *chan = vb2_get_drv_priv(vq);
+        int index;
+        bool is_streaming = atomic_read(&chan->is_streaming);
+        struct tegra_csi_channel *csi_chan = NULL;
+        struct tegra_csi_device *csi = chan->vi->csi;
+
+        if (!chan->bypass) {
+                tegra_channel_stop_kthreads(chan);
+                /* wait for last frame memory write ack */
+                if (is_streaming && chan->capture_state == CAPTURE_GOOD)
+                        tegra_channel_capture_done(chan);
+                /* free all the ring buffers */
+                free_ring_buffers(chan, chan->num_buffers);
+                /* dequeue buffers back to app which are in capture queue */
+                tegra_channel_queued_buf_done(chan, VB2_BUF_STATE_ERROR);
+
+                /* Disable clock gating to enable continuous clock */
+                tegra_channel_write(chan, TEGRA_VI_CFG_CG_CTRL, DISABLE);
+                /* Find connected csi_channel */
+                csi_chan = find_linked_csi_channel(chan, csi);
+                if (!csi_chan)
+                        pr_err("%s, no csi_chan found\n", __func__);
+                for (index = 0; index < chan->valid_ports; index++) {
+                        /* csi2_stop_streaming(csi_chan, index); */
+                        /* Always clear single shot if armed at close */
+                        if (csi_read(chan, index, TEGRA_VI_CSI_SINGLE_SHOT))
+                                tegra_channel_clear_singleshot(chan, index);
+                }
+                /* Enable clock gating so VI can be clock gated if necessary */
+                tegra_channel_write(chan, TEGRA_VI_CFG_CG_CTRL, ENABLE);
+        }
+
+        tegra_channel_set_stream(chan, false);
+        media_entity_pipeline_stop(&chan->video.entity);
+
+        if (!chan->bypass)
+                tegra_channel_update_clknbw(chan, 0);
+
+        vi_channel_syncpt_free(chan);
+}
+
+int tegra_vi2_power_on(struct tegra_mc_vi *vi)
+{
+        int ret;
+
+        ret = nvhost_module_busy(vi->ndev);
+        if (ret) {
+                dev_err(vi->dev, "%s:nvhost module is busy\n", __func__);
+                return ret;
+        }
+
+        vi_write(vi, TEGRA_VI_CFG_CG_CTRL, 1);
+
+        ret = tegra_camera_emc_clk_enable();
+        if (ret)
+                goto err_emc_enable;
+
+        return 0;
+
+err_emc_enable:
+        nvhost_module_idle(vi->ndev);
+
+        return ret;
+}
+
+void tegra_vi2_power_off(struct tegra_mc_vi *vi)
+{
+        tegra_channel_ec_close(vi);
+        tegra_camera_emc_clk_disable();
+        nvhost_module_idle(vi->ndev);
+}
+
+int vi2_power_on(struct tegra_channel *chan)
+{
+        int ret = 0;
+        struct tegra_mc_vi *vi;
+        struct vi *tegra_vi;
+        struct tegra_csi_device *csi;
+
+        vi = chan->vi;
+        tegra_vi = vi->vi;
+        csi = vi->csi;
+
+        ret = nvhost_module_add_client(vi->ndev, &chan->video);
+        if (ret)
+                return ret;
+
+        if (atomic_add_return(1, &vi->power_on_refcnt) == 1) {
+                tegra_vi2_power_on(vi);
+                if (chan->pg_mode)
+                        tegra_vi->tpg_opened = true;
+                else
+                        tegra_vi->sensor_opened = true;
+        }
+
+        if ((atomic_add_return(1, &chan->power_on_refcnt) == 1))
+                ret = tegra_channel_set_power(chan, 1);
+
+        return ret;
+}
+
+void vi2_power_off(struct tegra_channel *chan)
+{
+        int ret = 0;
+        struct tegra_mc_vi *vi;
+        struct vi *tegra_vi;
+        struct tegra_csi_device *csi;
+
+        vi = chan->vi;
+        tegra_vi = vi->vi;
+        csi = vi->csi;
+
+        if (atomic_dec_and_test(&chan->power_on_refcnt)) {
+                ret = tegra_channel_set_power(chan, 0);
+                if (ret < 0)
+                        dev_err(vi->dev, "Failed to power off subdevices\n");
+        }
+
+        /* The last release then turn off power */
+        if (atomic_dec_and_test(&vi->power_on_refcnt)) {
+                tegra_vi2_power_off(vi);
+                if (vi->pg_mode)
+                        tegra_vi->tpg_opened = false;
+                else
+                        tegra_vi->sensor_opened = false;
+        }
+        nvhost_module_remove_client(vi->ndev, &chan->video);
+}
diff --git a/drivers/media/platform/tegra/camera/vi/vi2_fops.h b/drivers/media/platform/tegra/camera/vi/vi2_fops.h
new file mode 100644
index 000000000..66c59879c
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/vi2_fops.h
@@ -0,0 +1,34 @@
+/*
+ * Tegra Video Input 2 device common APIs
+ *
+ * Tegra Graphics Host VI
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bryan Wu <pengw@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __T210_VI_H__
+#define __T210_VI_H__
+
+int vi2_power_on(struct tegra_channel *chan);
+void vi2_power_off(struct tegra_channel *chan);
+int vi2_channel_start_streaming(struct vb2_queue *vq, u32 count);
+int vi2_channel_stop_streaming(struct vb2_queue *vq);
+int vi2_add_ctrls(struct tegra_channel *chan);
+void vi2_init_video_formats(struct tegra_channel *chan);
+
+struct tegra_vi_fops vi2_fops = {
+        .vi_power_on = vi2_power_on,
+        .vi_power_off = vi2_power_off,
+        .vi_start_streaming = vi2_channel_start_streaming,
+        .vi_stop_streaming = vi2_channel_stop_streaming,
+        .vi_add_ctrls = vi2_add_ctrls,
+        .vi_init_video_formats = vi2_init_video_formats,
+};
+
+#endif
diff --git a/drivers/media/platform/tegra/camera/vi/vi2_formats.h b/drivers/media/platform/tegra/camera/vi/vi2_formats.h
new file mode 100644
index 000000000..c272eac3e
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/vi2_formats.h
@@ -0,0 +1,131 @@
+/*
+ * NVIDIA Tegra Video Input Device Driver VI2 formats
+ *
+ * Copyright (c) 2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bhanu Murthy V <bmurthyv@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __VI2_FORMATS_H_
+#define __VI2_FORMATS_H_
+
+#include "camera/vi/core.h"
+
+/*
+ * These go into the TEGRA_VI_CSI_n_IMAGE_DEF registers bits 23:16
+ * Output pixel memory format for the VI channel.
+ */
+enum tegra_image_format {
+        TEGRA_IMAGE_FORMAT_T_L8 = 16,
+
+        TEGRA_IMAGE_FORMAT_T_R16_I = 32,
+        TEGRA_IMAGE_FORMAT_T_B5G6R5,
+        TEGRA_IMAGE_FORMAT_T_R5G6B5,
+        TEGRA_IMAGE_FORMAT_T_A1B5G5R5,
+        TEGRA_IMAGE_FORMAT_T_A1R5G5B5,
+        TEGRA_IMAGE_FORMAT_T_B5G5R5A1,
+        TEGRA_IMAGE_FORMAT_T_R5G5B5A1,
+        TEGRA_IMAGE_FORMAT_T_A4B4G4R4,
+        TEGRA_IMAGE_FORMAT_T_A4R4G4B4,
+        TEGRA_IMAGE_FORMAT_T_B4G4R4A4,
+        TEGRA_IMAGE_FORMAT_T_R4G4B4A4,
+
+        TEGRA_IMAGE_FORMAT_T_A8B8G8R8 = 64,
+        TEGRA_IMAGE_FORMAT_T_A8R8G8B8,
+        TEGRA_IMAGE_FORMAT_T_B8G8R8A8,
+        TEGRA_IMAGE_FORMAT_T_R8G8B8A8,
+        TEGRA_IMAGE_FORMAT_T_A2B10G10R10,
+        TEGRA_IMAGE_FORMAT_T_A2R10G10B10,
+        TEGRA_IMAGE_FORMAT_T_B10G10R10A2,
+        TEGRA_IMAGE_FORMAT_T_R10G10B10A2,
+
+        TEGRA_IMAGE_FORMAT_T_A8Y8U8V8 = 193,
+        TEGRA_IMAGE_FORMAT_T_V8U8Y8A8,
+
+        TEGRA_IMAGE_FORMAT_T_A2Y10U10V10 = 197,
+        TEGRA_IMAGE_FORMAT_T_V10U10Y10A2,
+
+        TEGRA_IMAGE_FORMAT_T_Y8_U8__Y8_V8 = 200,
+        TEGRA_IMAGE_FORMAT_T_Y8_V8__Y8_U8,
+        TEGRA_IMAGE_FORMAT_T_U8_Y8__V8_Y8,
+        TEGRA_IMAGE_FORMAT_T_T_V8_Y8__U8_Y8,
+
+        TEGRA_IMAGE_FORMAT_T_T_Y8__U8__V8_N444 = 224,
+        TEGRA_IMAGE_FORMAT_T_Y8__U8V8_N444,
+        TEGRA_IMAGE_FORMAT_T_Y8__V8U8_N444,
+        TEGRA_IMAGE_FORMAT_T_Y8__U8__V8_N422,
+        TEGRA_IMAGE_FORMAT_T_Y8__U8V8_N422,
+        TEGRA_IMAGE_FORMAT_T_Y8__V8U8_N422,
+        TEGRA_IMAGE_FORMAT_T_Y8__U8__V8_N420,
+        TEGRA_IMAGE_FORMAT_T_Y8__U8V8_N420,
+        TEGRA_IMAGE_FORMAT_T_Y8__V8U8_N420,
+        TEGRA_IMAGE_FORMAT_T_X2Lc10Lb10La10,
+        TEGRA_IMAGE_FORMAT_T_A2R6R6R6R6R6,
+};
+
+static const struct tegra_video_format vi2_video_formats[] = {
+        /* RAW 6: TODO */
+
+        /* RAW 7: TODO */
+
+        /* RAW 8 */
+        TEGRA_VIDEO_FORMAT(RAW8, 8, SRGGB8_1X8, 1, 1, T_L8,
+                                RAW8, SRGGB8, "RGRG.. GBGB.."),
+        TEGRA_VIDEO_FORMAT(RAW8, 8, SGRBG8_1X8, 1, 1, T_L8,
+                                RAW8, SGRBG8, "GRGR.. BGBG.."),
+        TEGRA_VIDEO_FORMAT(RAW8, 8, SGBRG8_1X8, 1, 1, T_L8,
+                                RAW8, SGBRG8, "GBGB.. RGRG.."),
+        TEGRA_VIDEO_FORMAT(RAW8, 8, SBGGR8_1X8, 1, 1, T_L8,
+                                RAW8, SBGGR8, "BGBG.. GRGR.."),
+
+        /* RAW 10 */
+        TEGRA_VIDEO_FORMAT(RAW10, 10, SRGGB10_1X10, 2, 1, T_R16_I,
+                                RAW10, SRGGB10, "RGRG.. GBGB.."),
+        TEGRA_VIDEO_FORMAT(RAW10, 10, SGRBG10_1X10, 2, 1, T_R16_I,
+                                RAW10, SGRBG10, "GRGR.. BGBG.."),
+        TEGRA_VIDEO_FORMAT(RAW10, 10, SGBRG10_1X10, 2, 1, T_R16_I,
+                                RAW10, SGBRG10, "GBGB.. RGRG.."),
+        TEGRA_VIDEO_FORMAT(RAW10, 10, SBGGR10_1X10, 2, 1, T_R16_I,
+                                RAW10, SBGGR10, "BGBG.. GRGR.."),
+
+        /* RAW 10 Packed format */
+        TEGRA_VIDEO_FORMAT(RAW10, 10, XBGGR10P_3X10, 4, 3, T_X2Lc10Lb10La10,
+                                RAW10, XBGGR10P, "BGBG.. GRGR.."),
+        TEGRA_VIDEO_FORMAT(RAW10, 10, XRGGB10P_3X10, 4, 3, T_X2Lc10Lb10La10,
+                                RAW10, XRGGB10P, "RGRG.. GBGB.."),
+
+        /* RAW 12 */
+        TEGRA_VIDEO_FORMAT(RAW12, 12, SRGGB12_1X12, 2, 1, T_R16_I,
+                                RAW12, SRGGB12, "RGRG.. GBGB.."),
+        TEGRA_VIDEO_FORMAT(RAW12, 12, SGRBG12_1X12, 2, 1, T_R16_I,
+                                RAW12, SGRBG12, "GRGR.. BGBG.."),
+        TEGRA_VIDEO_FORMAT(RAW12, 12, SGBRG12_1X12, 2, 1, T_R16_I,
+                                RAW12, SGBRG12, "GBGB.. RGRG.."),
+        TEGRA_VIDEO_FORMAT(RAW12, 12, SBGGR12_1X12, 2, 1, T_R16_I,
+                                RAW12, SBGGR12, "BGBG.. GRGR.."),
+
+        /* RGB888 */
+        TEGRA_VIDEO_FORMAT(RGB888, 24, RGB888_1X24, 4, 1, T_A8R8G8B8,
+                                RGB888, ABGR32, "BGRA-8-8-8-8"),
+        TEGRA_VIDEO_FORMAT(RGB888, 24, RGB888_1X32_PADHI, 4, 1, T_A8B8G8R8,
+                                RGB888, RGB32, "RGB-8-8-8-8"),
+
+        /* YUV422 */
+        TEGRA_VIDEO_FORMAT(YUV422, 16, UYVY8_1X16, 2, 1, T_U8_Y8__V8_Y8,
+                                YUV422_8, UYVY, "YUV 4:2:2"),
+        TEGRA_VIDEO_FORMAT(YUV422, 16, UYVY8_1X16, 1, 1, T_Y8__V8U8_N422,
+                                YUV422_8, NV16, "NV16"),
+        TEGRA_VIDEO_FORMAT(YUV422, 16, UYVY8_2X8, 2, 1, T_U8_Y8__V8_Y8,
+                                YUV422_8, UYVY, "YUV 4:2:2 UYVY"),
+        TEGRA_VIDEO_FORMAT(YUV422, 16, VYUY8_2X8, 2, 1, T_T_V8_Y8__U8_Y8,
+                                YUV422_8, VYUY, "YUV 4:2:2 VYUY"),
+        TEGRA_VIDEO_FORMAT(YUV422, 16, YUYV8_2X8, 2, 1, T_Y8_U8__Y8_V8,
+                                YUV422_8, YUYV, "YUV 4:2:2 YUYV"),
+        TEGRA_VIDEO_FORMAT(YUV422, 16, YVYU8_2X8, 2, 1, T_Y8_V8__Y8_U8,
+                                YUV422_8, YVYU, "YUV 4:2:2 YVYU"),
+};
+#endif
diff --git a/drivers/media/platform/tegra/camera/vi/vi4_fops.c b/drivers/media/platform/tegra/camera/vi/vi4_fops.c
new file mode 100644
index 000000000..d78ed3421
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/vi4_fops.c
@@ -0,0 +1,1056 @@
+/*
+ * Tegra Video Input 4 device common APIs
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Frank Chen <frank@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+#include <linux/nvhost.h>
+#include <linux/tegra-powergate.h>
+#include <media/capture.h>
+#include <media/tegra_camera_platform.h>
+#include "linux/nvhost_ioctl.h"
+#include "mc_common.h"
+#include "nvhost_acm.h"
+#include "vi4_formats.h"
+#include "vi4_registers.h"
+#include "vi/vi4.h"
+#include "vi/vi_notify.h"
+
+#define DEFAULT_FRAMERATE       30
+#define DEFAULT_CSI_FREQ        204000000
+#define BPP_MEM         2
+#define MAX_VI_CHANNEL 12
+#define NUM_PPC         8
+#define VI_CSI_CLK_SCALE        110
+#define SOF_SYNCPT_IDX  0
+#define FE_SYNCPT_IDX   1
+
+void tegra_channel_queued_buf_done(struct tegra_channel *chan,
+                                          enum vb2_buffer_state state);
+int tegra_channel_set_stream(struct tegra_channel *chan, bool on);
+void tegra_channel_ring_buffer(struct tegra_channel *chan,
+                struct vb2_v4l2_buffer *vb,
+                struct timespec *ts, int state);
+struct tegra_channel_buffer *dequeue_buffer(struct tegra_channel *chan);
+void tegra_channel_init_ring_buffer(struct tegra_channel *chan);
+void free_ring_buffers(struct tegra_channel *chan, int frames);
+int tegra_channel_set_power(struct tegra_channel *chan, bool on);
+static void tegra_channel_stop_kthreads(struct tegra_channel *chan);
+static int tegra_channel_stop_increments(struct tegra_channel *chan);
+static void tegra_channel_notify_status_callback(
+                                struct vi_notify_channel *,
+                                const struct vi_capture_status *,
+                                void *);
+static void tegra_channel_error_worker(struct work_struct *status_work);
+static void tegra_channel_notify_error_callback(void *);
+
+u32 csimux_config_stream[] = {
+        CSIMUX_CONFIG_STREAM_0,
+        CSIMUX_CONFIG_STREAM_1,
+        CSIMUX_CONFIG_STREAM_2,
+        CSIMUX_CONFIG_STREAM_3,
+        CSIMUX_CONFIG_STREAM_4,
+        CSIMUX_CONFIG_STREAM_5
+};
+
+static void vi4_write(struct tegra_channel *chan, unsigned int addr, u32 val)
+{
+        writel(val, chan->vi->iomem + addr);
+}
+
+static u32 vi4_read(struct tegra_channel *chan, unsigned int addr)
+{
+        return readl(chan->vi->iomem + addr);
+}
+
+static void vi4_channel_write(struct tegra_channel *chan,
+                unsigned int index, unsigned int addr, u32 val)
+{
+        writel(val,
+                chan->vi->iomem + VI4_CHANNEL_OFFSET * (index + 1) + addr);
+}
+
+void vi4_init_video_formats(struct tegra_channel *chan)
+{
+        int i;
+
+        chan->num_video_formats = ARRAY_SIZE(vi4_video_formats);
+        for (i = 0; i < chan->num_video_formats; i++)
+                chan->video_formats[i] = &vi4_video_formats[i];
+}
+
+long vi4_default_ioctl(struct file *file, void *fh,
+                        bool use_prio, unsigned int cmd, void *arg)
+{
+        struct v4l2_fh *vfh = file->private_data;
+        struct tegra_channel *chan = to_tegra_channel(vfh->vdev);
+        long err = 0;
+
+        switch (_IOC_NR(cmd)) {
+#if defined(CONFIG_TEGRA_CAMERA_RTCPU)
+        case _IOC_NR(VIDIOC_CAPTURE_SETUP):
+                if (chan->bypass)
+                        err = vi_capture_setup(chan,
+                                        (struct vi_capture_setup *)arg);
+                else {
+                        dev_err(&chan->video.dev, "not in bypass mode\n");
+                        err = -ENODEV;
+                }
+                if (err)
+                        dev_err(&chan->video.dev, "capture setup failed\n");
+                break;
+        case _IOC_NR(VIDIOC_CAPTURE_RESET):
+                if (chan->bypass)
+                        err = vi_capture_reset(chan, *(uint32_t *)arg);
+                else {
+                        dev_err(&chan->video.dev, "not in bypass mode\n");
+                        err = -ENODEV;
+                }
+                if (err)
+                        dev_err(&chan->video.dev, "capture reset failed\n");
+                break;
+        case _IOC_NR(VIDIOC_CAPTURE_RELEASE):
+                if (chan->bypass)
+                        err = vi_capture_release(chan, *(uint32_t *)arg);
+                else {
+                        dev_err(&chan->video.dev, "not in bypass mode\n");
+                        err = -ENODEV;
+                }
+                if (err)
+                        dev_err(&chan->video.dev, "capture release failed\n");
+                break;
+        case _IOC_NR(VIDIOC_CAPTURE_GET_INFO):
+                if (chan->bypass)
+                        err = vi_capture_get_info(chan,
+                                        (struct vi_capture_info *)arg);
+                else {
+                        dev_err(&chan->video.dev, "not in bypass mode\n");
+                        err = -ENODEV;
+                }
+                if (err)
+                        dev_err(&chan->video.dev, "capture get info failed\n");
+                break;
+        case _IOC_NR(VIDIOC_CAPTURE_SET_CONFIG):
+                if (chan->bypass)
+                        err = vi_capture_control_message(chan,
+                                        (struct vi_capture_control_msg *)arg);
+                else {
+                        dev_err(&chan->video.dev, "not in bypass mode\n");
+                        err = -ENODEV;
+                }
+                if (err)
+                        dev_err(&chan->video.dev, "capture config failed\n");
+                break;
+        case _IOC_NR(VIDIOC_CAPTURE_REQUEST):
+                if (chan->bypass)
+                        err = vi_capture_request(chan,
+                                        (struct vi_capture_req *)arg);
+                else {
+                        dev_err(&chan->video.dev, "not in bypass mode\n");
+                        err = -ENODEV;
+                }
+                if (err)
+                        dev_err(&chan->video.dev,
+                                "capture request submit failed\n");
+                break;
+        case _IOC_NR(VIDIOC_CAPTURE_STATUS):
+                if (chan->bypass)
+                        err = vi_capture_status(chan, *(uint32_t *)arg);
+                else {
+                        dev_err(&chan->video.dev, "not in bypass mode\n");
+                        err = -ENODEV;
+                }
+                if (err)
+                        dev_err(&chan->video.dev,
+                                "capture get status failed\n");
+                break;
+#endif
+        default:
+                dev_err(&chan->video.dev, "%s:Unknown ioctl\n", __func__);
+                return -ENOIOCTLCMD;
+        }
+
+        return err;
+}
+
+
+int tegra_vi4_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+        struct tegra_channel *chan = container_of(ctrl->handler,
+                                struct tegra_channel, ctrl_handler);
+        int err = 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_WRITE_ISPFORMAT:
+                chan->write_ispformat = ctrl->val;
+                break;
+        default:
+                dev_err(&chan->video.dev, "%s:Not valid ctrl\n", __func__);
+                return -EINVAL;
+        }
+
+        return err;
+}
+
+static const struct v4l2_ctrl_ops vi4_ctrl_ops = {
+        .s_ctrl = tegra_vi4_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vi4_custom_ctrls[] = {
+        {
+                .ops = &vi4_ctrl_ops,
+                .id = V4L2_CID_WRITE_ISPFORMAT,
+                .name = "Write ISP format",
+                .type = V4L2_CTRL_TYPE_INTEGER,
+                .def = 1,
+                .min = 1,
+                .max = 1,
+                .step = 1,
+        },
+};
+
+int vi4_add_ctrls(struct tegra_channel *chan)
+{
+        int i;
+
+        /* Add vi4 custom controls */
+        for (i = 0; i < ARRAY_SIZE(vi4_custom_ctrls); i++) {
+                v4l2_ctrl_new_custom(&chan->ctrl_handler,
+                        &vi4_custom_ctrls[i], NULL);
+                if (chan->ctrl_handler.error) {
+                        dev_err(chan->vi->dev,
+                                "Failed to add %s ctrl\n",
+                                vi4_custom_ctrls[i].name);
+                        return chan->ctrl_handler.error;
+                }
+        }
+
+        return 0;
+}
+
+static bool vi4_init(struct tegra_channel *chan)
+{
+        vi4_write(chan, CFG_INTERRUPT_MASK, 0x3f0000f9);
+        vi4_write(chan, CFG_INTERRUPT_STATUS, 0x3f000001);
+        vi4_write(chan, NOTIFY_ERROR, 0x1);
+        vi4_write(chan, NOTIFY_TAG_CLASSIFY_0, 0xe39c08e3);
+        return true;
+}
+
+static bool vi4_check_status(struct tegra_channel *chan)
+{
+        int status;
+
+        /* check interrupt status error */
+        status = vi4_read(chan, CFG_INTERRUPT_STATUS);
+        if (status & 0x1)
+                dev_err(chan->vi->dev,
+                        "VI_CFG_INTERRUPT_STATUS_0: MASTER_ERR_STATUS error!\n");
+
+        /* Check VI NOTIFY input FIFO error */
+        status = vi4_read(chan, NOTIFY_ERROR);
+        if (status & 0x1)
+                dev_err(chan->vi->dev,
+                        "VI_NOTIFY_ERROR_0: NOTIFY_FIFO_OVERFLOW error!\n");
+
+        return true;
+}
+
+static bool vi_notify_wait(struct tegra_channel *chan,
+                struct timespec *ts)
+{
+        int i, err;
+        u32 thresh[TEGRA_CSI_BLOCKS], temp;
+
+        /*
+         * Increment syncpt for ATOMP_FE
+         *
+         * This is needed in order to keep the syncpt max up to date,
+         * even if we are not waiting for ATOMP_FE here
+         */
+        for (i = 0; i < chan->valid_ports; i++)
+                temp = nvhost_syncpt_incr_max_ext(chan->vi->ndev,
+                                        chan->syncpt[i][FE_SYNCPT_IDX], 1);
+
+        /*
+         * Increment syncpt for PXL_SOF
+         *
+         * Increment and retrieve PXL_SOF syncpt max value.
+         * This value will be used to wait for next syncpt
+         */
+        for (i = 0; i < chan->valid_ports; i++)
+                thresh[i] = nvhost_syncpt_incr_max_ext(chan->vi->ndev,
+                                        chan->syncpt[i][SOF_SYNCPT_IDX], 1);
+
+        /*
+         * Wait for PXL_SOF syncpt
+         *
+         * Use the syncpt max value we just set as threshold
+         */
+        for (i = 0; i < chan->valid_ports; i++) {
+                err = nvhost_syncpt_wait_timeout_ext(chan->vi->ndev,
+                                chan->syncpt[i][SOF_SYNCPT_IDX], thresh[i],
+                                250, NULL, NULL);
+                if (unlikely(err))
+                        dev_err(chan->vi->dev,
+                                "PXL_SOF syncpt timeout! err = %d\n", err);
+                else {
+                        struct vi_capture_status status;
+
+                        err = vi_notify_get_capture_status(chan->vnc[i],
+                                        chan->vnc_id[i],
+                                        thresh[i], &status);
+                        if (unlikely(err))
+                                dev_err(chan->vi->dev,
+                                        "no capture status! err = %d\n", err);
+                        else
+                                *ts = ns_to_timespec((s64)status.sof_ts);
+                }
+        }
+        return true;
+}
+
+static void tegra_channel_surface_setup(
+        struct tegra_channel *chan, struct tegra_channel_buffer *buf, int index)
+{
+        int vnc_id = chan->vnc_id[index];
+        unsigned int offset = chan->buffer_offset[index];
+
+        vi4_channel_write(chan, vnc_id, ATOMP_EMB_SURFACE_OFFSET0, 0x0);
+        vi4_channel_write(chan, vnc_id, ATOMP_EMB_SURFACE_OFFSET0_H, 0x0);
+        vi4_channel_write(chan, vnc_id, ATOMP_EMB_SURFACE_STRIDE0, 0x0);
+        vi4_channel_write(chan, vnc_id,
+                ATOMP_SURFACE_OFFSET0, buf->addr + offset);
+        vi4_channel_write(chan, vnc_id,
+                ATOMP_SURFACE_STRIDE0, chan->format.bytesperline);
+        vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_OFFSET0_H, 0x0);
+
+        if (chan->fmtinfo->fourcc == V4L2_PIX_FMT_NV16) {
+                vi4_channel_write(chan, vnc_id,
+                        ATOMP_SURFACE_OFFSET1, buf->addr + offset +
+                        chan->format.sizeimage / 2);
+                vi4_channel_write(chan, vnc_id,
+                        ATOMP_SURFACE_OFFSET1_H, 0x0);
+                vi4_channel_write(chan, vnc_id,
+                        ATOMP_SURFACE_STRIDE1, chan->format.bytesperline);
+
+        } else {
+                vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_OFFSET1, 0x0);
+                vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_OFFSET1_H, 0x0);
+                vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_STRIDE1, 0x0);
+        }
+
+        vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_OFFSET2, 0x0);
+        vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_OFFSET2_H, 0x0);
+        vi4_channel_write(chan, vnc_id, ATOMP_SURFACE_STRIDE2, 0x0);
+}
+
+static void tegra_channel_handle_error(struct tegra_channel *chan)
+{
+        struct v4l2_subdev *sd_on_csi = chan->subdev_on_csi;
+        static const struct v4l2_event source_ev_fmt = {
+                .type = V4L2_EVENT_SOURCE_CHANGE,
+                .u.src_change.changes = V4L2_EVENT_SRC_ERROR,
+        };
+
+        tegra_channel_stop_increments(chan);
+        vb2_queue_error(&chan->queue);
+
+        /* Application gets notified after CSI Tx's are reset */
+        if (sd_on_csi->devnode)
+                v4l2_subdev_notify_event(sd_on_csi, &source_ev_fmt);
+}
+
+static void tegra_channel_status_worker(struct work_struct *status_work)
+{
+        struct tegra_channel *chan;
+
+        chan = container_of(status_work, struct tegra_channel, status_work);
+
+        tegra_channel_handle_error(chan);
+}
+
+static void tegra_channel_notify_status_callback(
+                                struct vi_notify_channel *vnc,
+                                const struct vi_capture_status *status,
+                                void *client_data)
+{
+        struct tegra_channel *chan = (struct tegra_channel *)client_data;
+        int i;
+
+        spin_lock(&chan->capture_state_lock);
+        if (chan->capture_state == CAPTURE_GOOD)
+                chan->capture_state = CAPTURE_ERROR;
+        else {
+                spin_unlock(&chan->capture_state_lock);
+                return;
+        }
+        spin_unlock(&chan->capture_state_lock);
+
+        for (i = 0; i < chan->valid_ports; i++)
+                dev_err(chan->vi->dev, "Status: %2u channel:%02X frame:%04X\n",
+                        status->status, chan->vnc_id[i], status->frame);
+        dev_err(chan->vi->dev, "     timestamp sof %llu eof %llu data 0x%08x\n",
+                status->sof_ts, status->eof_ts, status->data);
+        dev_err(chan->vi->dev, "     capture_id %u stream %2u vchan %2u\n",
+                status->capture_id, status->st, status->vc);
+
+        schedule_work(&chan->status_work);
+}
+
+static int tegra_channel_notify_enable(
+        struct tegra_channel *chan, unsigned int index)
+{
+        struct tegra_vi4_syncpts_req req;
+        int i, err;
+
+        chan->vnc_id[index] = -1;
+        for (i = 0; i < MAX_VI_CHANNEL; i++) {
+                chan->vnc[index] = vi_notify_channel_open(i);
+                if (!IS_ERR(chan->vnc[index])) {
+                        chan->vnc_id[index] = i;
+                        break;
+                }
+        }
+        if (chan->vnc_id[index] < 0) {
+                dev_err(chan->vi->dev, "No VI channel available!\n");
+                return -EFAULT;
+        }
+
+        vi_notify_channel_set_notify_funcs(chan->vnc[index],
+                        &tegra_channel_notify_status_callback,
+                        &tegra_channel_notify_error_callback,
+                        (void *)chan);
+
+        /* get PXL_SOF syncpt id */
+        chan->syncpt[index][SOF_SYNCPT_IDX] =
+                nvhost_get_syncpt_client_managed(chan->vi->ndev, "tegra-vi4");
+        if (chan->syncpt[index][SOF_SYNCPT_IDX] == 0) {
+                dev_err(chan->vi->dev, "Failed to get PXL_SOF syncpt!\n");
+                return -EFAULT;
+        }
+
+        /* get ATOMP_FE syncpt id */
+        chan->syncpt[index][FE_SYNCPT_IDX] =
+                nvhost_get_syncpt_client_managed(chan->vi->ndev, "tegra-vi4");
+        if (chan->syncpt[index][FE_SYNCPT_IDX] == 0) {
+                dev_err(chan->vi->dev, "Failed to get ATOMP_FE syncpt!\n");
+                nvhost_syncpt_put_ref_ext(
+                        chan->vi->ndev, chan->syncpt[index][SOF_SYNCPT_IDX]);
+                return -EFAULT;
+        }
+
+        nvhost_syncpt_set_min_eq_max_ext(
+                chan->vi->ndev, chan->syncpt[index][SOF_SYNCPT_IDX]);
+        nvhost_syncpt_set_min_eq_max_ext(
+                chan->vi->ndev, chan->syncpt[index][FE_SYNCPT_IDX]);
+
+        /* enable VI Notify report */
+        req.syncpt_ids[0] = chan->syncpt[index][SOF_SYNCPT_IDX]; /* PXL_SOF */
+        req.syncpt_ids[1] = chan->syncpt[index][FE_SYNCPT_IDX]; /* ATOMP_FE */
+        req.syncpt_ids[2] = 0xffffffff;
+        req.stream = chan->port[index];
+        req.vc = 0;
+        req.pad = 0;
+
+        err = vi_notify_channel_enable_reports(
+                chan->vnc_id[index], chan->vnc[index], &req);
+        if (err < 0)
+                dev_err(chan->vi->dev,
+                        "Failed to enable report for VI Notify, err = %d\n",
+                        err);
+
+        return err;
+}
+
+static int tegra_channel_notify_disable(
+        struct tegra_channel *chan, unsigned int index)
+{
+        int err;
+        int ret = 0;
+        struct tegra_vi4_syncpts_req req;
+
+        /* free syncpts */
+        nvhost_syncpt_put_ref_ext(
+                chan->vi->ndev, chan->syncpt[index][SOF_SYNCPT_IDX]);
+        nvhost_syncpt_put_ref_ext(
+                chan->vi->ndev, chan->syncpt[index][FE_SYNCPT_IDX]);
+
+        /* close vi-notifier */
+        req.syncpt_ids[0] = 0xffffffff;
+        req.syncpt_ids[1] = 0xffffffff;
+        req.syncpt_ids[2] = 0xffffffff;
+        req.stream = chan->port[index];
+        req.vc = 0;
+        req.pad = 0;
+
+        err = vi_notify_channel_reset(
+                chan->vnc_id[index], chan->vnc[index], &req);
+        if (err < 0) {
+                dev_err(chan->vi->dev,
+                        "VI Notify channel reset failed, err = %d\n", err);
+                if (!ret)
+                        ret = err;
+        }
+
+        err = vi_notify_channel_close(chan->vnc_id[index], chan->vnc[index]);
+        if (err < 0) {
+                dev_err(chan->vi->dev,
+                        "VI Notify channel close failed, err = %d\n", err);
+                if (!ret)
+                        ret = err;
+        }
+
+        return ret;
+}
+
+static int tegra_channel_capture_setup(struct tegra_channel *chan,
+                unsigned int index)
+{
+        u32 height = chan->format.height;
+        u32 width = chan->format.width;
+        u32 format = chan->fmtinfo->img_fmt;
+        u32 data_type = chan->fmtinfo->img_dt;
+        u32 csi_port = chan->port[index];
+        u32 stream = 1U << csi_port;
+        u32 virtual_ch = 1U << 0;
+        u32 vnc_id;
+        int err;
+
+        if (chan->valid_ports > 1) {
+                height = chan->gang_height;
+                width = chan->gang_width;
+        }
+
+        err = tegra_channel_notify_enable(chan, index);
+        if (err < 0) {
+                dev_err(chan->vi->dev,
+                        "Failed to setup VI Notifier, err = %d\n", err);
+                return err;
+        }
+
+        vnc_id = chan->vnc_id[index];
+
+        vi4_write(chan, csimux_config_stream[csi_port], 0x1);
+
+        vi4_channel_write(chan, vnc_id, MATCH,
+                        ((stream << STREAM_SHIFT) & STREAM) |
+                        STREAM_MASK |
+                        ((virtual_ch << VIRTUAL_CHANNEL_SHIFT) &
+                        VIRTUAL_CHANNEL)  |
+                        VIRTUAL_CHANNEL_MASK);
+
+        vi4_channel_write(chan, vnc_id, MATCH_DATATYPE,
+                        ((data_type << DATATYPE_SHIFT) & DATATYPE) |
+                        DATATYPE_MASK);
+
+        vi4_channel_write(chan, vnc_id, DT_OVERRIDE, 0x0);
+
+        vi4_channel_write(chan, vnc_id, MATCH_FRAMEID,
+                        ((0 << FRAMEID_SHIFT) & FRAMEID) | 0);
+
+        vi4_channel_write(chan, vnc_id, FRAME_X, width);
+        vi4_channel_write(chan, vnc_id, FRAME_Y, height);
+        vi4_channel_write(chan, vnc_id, SKIP_X, 0x0);
+        vi4_channel_write(chan, vnc_id, CROP_X, width);
+        vi4_channel_write(chan, vnc_id, OUT_X, width);
+        vi4_channel_write(chan, vnc_id, SKIP_Y, 0x0);
+        vi4_channel_write(chan, vnc_id, CROP_Y, height);
+        vi4_channel_write(chan, vnc_id, OUT_Y, height);
+        vi4_channel_write(chan, vnc_id, PIXFMT_ENABLE, PIXFMT_EN);
+        vi4_channel_write(chan, vnc_id, PIXFMT_WIDE, 0x0);
+        vi4_channel_write(chan, vnc_id, PIXFMT_FORMAT, format);
+        vi4_channel_write(chan, vnc_id, DPCM_STRIP, 0x0);
+        vi4_channel_write(chan, vnc_id, ATOMP_DPCM_CHUNK, 0x0);
+        vi4_channel_write(chan, vnc_id, ISPBUFA, 0x0);
+        vi4_channel_write(chan, vnc_id, LINE_TIMER, 0x1000000);
+        vi4_channel_write(chan, vnc_id, EMBED_X, 0x0);
+        vi4_channel_write(chan, vnc_id, EMBED_Y, 0x0);
+        /*
+         * Set ATOMP_RESERVE to 0 so rctpu won't increment syncpt
+         * for captureInfo. This is copied from nvvi driver.
+         *
+         * If we don't set this register to 0, ATOMP_FE syncpt
+         * will be increment by 2 for each frame
+         */
+        vi4_channel_write(chan, vnc_id, ATOMP_RESERVE, 0x0);
+        dev_dbg(chan->vi->dev,
+                "Create Surface with imgW=%d, imgH=%d, memFmt=%d\n",
+                width, height, format);
+
+        return 0;
+}
+
+static int tegra_channel_capture_frame(struct tegra_channel *chan,
+                                       struct tegra_channel_buffer *buf)
+{
+        struct vb2_v4l2_buffer *vb = &buf->buf;
+        struct timespec ts;
+        int state = VB2_BUF_STATE_DONE;
+        unsigned long flags;
+        int err = false;
+        int i;
+
+        for (i = 0; i < chan->valid_ports; i++)
+                tegra_channel_surface_setup(chan, buf, i);
+
+        if (!chan->bfirst_fstart) {
+                err = tegra_channel_set_stream(chan, true);
+                if (err < 0)
+                        return err;
+        }
+
+        for (i = 0; i < chan->valid_ports; i++) {
+                vi4_channel_write(chan, chan->vnc_id[i], CHANNEL_COMMAND, LOAD);
+                vi4_channel_write(chan, chan->vnc_id[i],
+                        CONTROL, SINGLESHOT | MATCH_STATE_EN);
+        }
+
+        /* wait for vi notifier events */
+        vi_notify_wait(chan, &ts);
+
+        vi4_check_status(chan);
+
+        spin_lock_irqsave(&chan->capture_state_lock, flags);
+        if (chan->capture_state != CAPTURE_ERROR)
+                chan->capture_state = CAPTURE_GOOD;
+        spin_unlock_irqrestore(&chan->capture_state_lock, flags);
+
+        tegra_channel_ring_buffer(chan, vb, &ts, state);
+
+        return 0;
+}
+
+static int tegra_channel_stop_increments(struct tegra_channel *chan)
+{
+        int i;
+        struct tegra_vi4_syncpts_req req = {
+                .syncpt_ids = {
+                        0xffffffff,
+                        0xffffffff,
+                        0xffffffff,
+                },
+                .stream = chan->port[0],
+                .vc = 0,
+        };
+
+        /* No need to check errors. There's nothing we could do. */
+        for (i = 0; i < chan->valid_ports; i++)
+                vi_notify_channel_reset(chan->vnc_id[i], chan->vnc[i], &req);
+
+        return 0;
+}
+
+static void tegra_channel_capture_done(struct tegra_channel *chan)
+{
+        struct timespec ts;
+        struct tegra_channel_buffer *buf;
+        int state = VB2_BUF_STATE_DONE;
+        u32 thresh[TEGRA_CSI_BLOCKS];
+        int i, err;
+
+        /* dequeue buffer and return if no buffer exists */
+        buf = dequeue_buffer(chan);
+        if (!buf)
+                return;
+
+        /* make sure to read the last frame out before exit */
+        for (i = 0; i < chan->valid_ports; i++) {
+                tegra_channel_surface_setup(chan, buf, i);
+                vi4_channel_write(chan, chan->vnc_id[i], CHANNEL_COMMAND, LOAD);
+                vi4_channel_write(chan, chan->vnc_id[i],
+                        CONTROL, SINGLESHOT | MATCH_STATE_EN);
+        }
+
+        for (i = 0; i < chan->valid_ports; i++) {
+                err = nvhost_syncpt_read_ext_check(chan->vi->ndev,
+                                chan->syncpt[i][FE_SYNCPT_IDX], &thresh[i]);
+                /* Get current ATOMP_FE syncpt min value */
+                if (!err) {
+                        struct vi_capture_status status;
+                        u32 index = thresh[i] + 1;
+                        /* Wait for ATOMP_FE syncpt
+                         *
+                         * This is to make sure we don't exit the capture thread
+                         * before the last frame is done writing to memory
+                         */
+                        err = nvhost_syncpt_wait_timeout_ext(chan->vi->ndev,
+                                        chan->syncpt[i][FE_SYNCPT_IDX],
+                                        index,
+                                        250, NULL, NULL);
+                        if (unlikely(err))
+                                dev_err(chan->vi->dev,
+                                        "ATOMP_FE syncpt timeout!\n");
+                        else {
+                                err = vi_notify_get_capture_status(chan->vnc[i],
+                                                chan->vnc_id[i],
+                                                index, &status);
+                                if (unlikely(err))
+                                        dev_err(chan->vi->dev,
+                                                "no capture status! err = %d\n",
+                                                err);
+                                else
+                                        ts = ns_to_timespec((s64)status.eof_ts);
+                        }
+                }
+        }
+
+        /* Mark capture state to IDLE as capture is finished */
+        chan->capture_state = CAPTURE_IDLE;
+
+        tegra_channel_ring_buffer(chan, &buf->buf, &ts, state);
+}
+
+static int tegra_channel_kthread_capture_start(void *data)
+{
+        struct tegra_channel *chan = data;
+        struct tegra_channel_buffer *buf;
+        int err = 0;
+
+        set_freezable();
+
+        while (1) {
+
+                try_to_freeze();
+
+                wait_event_interruptible(chan->start_wait,
+                                         !list_empty(&chan->capture) ||
+                                         kthread_should_stop());
+
+                if (kthread_should_stop())
+                        break;
+
+                /* source is not streaming if error is non-zero */
+                /* wait till kthread stop and dont DeQ buffers */
+                if (err)
+                        continue;
+
+                buf = dequeue_buffer(chan);
+                if (!buf)
+                        continue;
+
+                err = tegra_channel_capture_frame(chan, buf);
+        }
+
+        return 0;
+}
+
+static void tegra_channel_stop_kthreads(struct tegra_channel *chan)
+{
+        mutex_lock(&chan->stop_kthread_lock);
+        /* Stop the kthread for capture */
+        if (chan->kthread_capture_start) {
+                kthread_stop(chan->kthread_capture_start);
+                chan->kthread_capture_start = NULL;
+        }
+        mutex_unlock(&chan->stop_kthread_lock);
+}
+
+static int tegra_channel_update_clknbw(struct tegra_channel *chan, u8 on)
+{
+        int ret = 0;
+        unsigned long request_pixelrate;
+        struct v4l2_subdev_frame_interval fie;
+        unsigned long csi_freq = 0;
+
+        fie.interval.denominator = DEFAULT_FRAMERATE;
+        fie.interval.numerator = 1;
+
+        if (v4l2_subdev_has_op(chan->subdev_on_csi,
+                                video, g_frame_interval))
+                v4l2_subdev_call(chan->subdev_on_csi, video,
+                                g_frame_interval, &fie);
+        if (on) {
+                /* for PG, using default frequence */
+                if (chan->pg_mode) {
+                        csi_freq = DEFAULT_CSI_FREQ;
+                        request_pixelrate = csi_freq * NUM_PPC;
+                } else {
+                        /**
+                         * TODO: use real sensor pixelrate
+                         * See PowerService code
+                         */
+                        request_pixelrate = (long long)(chan->format.width
+                                * chan->format.height
+                                * fie.interval.denominator / 100)
+                                * VI_CSI_CLK_SCALE;
+                        csi_freq = ((long long)chan->format.width
+                                * chan->format.height
+                                * fie.interval.denominator) / NUM_PPC;
+                }
+
+                /* VI clk should be slightly faster than CSI clk*/
+                ret = nvhost_module_set_rate(chan->vi->ndev, &chan->video,
+                                request_pixelrate, 0, NVHOST_PIXELRATE);
+                if (ret) {
+                        dev_err(chan->vi->dev, "Fail to update vi clk\n");
+                        return ret;
+                }
+        } else {
+                csi_freq = DEFAULT_CSI_FREQ;
+                ret = nvhost_module_set_rate(chan->vi->ndev, &chan->video, 0, 0,
+                                NVHOST_PIXELRATE);
+                if (ret) {
+                        dev_err(chan->vi->dev, "Fail to update vi clk\n");
+                        return ret;
+                }
+        }
+        if (chan->pg_mode)
+                chan->requested_kbyteps = (on > 0 ? 1 : -1) *
+                        ((long long)csi_freq * BPP_MEM * 110 / 100) / 1000;
+        else
+                chan->requested_kbyteps = (on > 0 ? 1 : -1) *
+                (((long long) chan->format.width * chan->format.height
+                * fie.interval.denominator * BPP_MEM) * 115 / 100) / 1000;
+
+        mutex_lock(&chan->vi->bw_update_lock);
+        chan->vi->aggregated_kbyteps += chan->requested_kbyteps;
+        ret = vi_v4l2_update_isobw(chan->vi->aggregated_kbyteps, 0);
+        mutex_unlock(&chan->vi->bw_update_lock);
+        if (ret)
+                dev_info(chan->vi->dev,
+                "WAR:Calculation not precise.Ignore BW request failure\n");
+        ret = vi4_v4l2_set_la(chan->vi->ndev, 0, 0);
+        if (ret)
+                dev_info(chan->vi->dev,
+                "WAR:Calculation not precise.Ignore LA failure\n");
+        return 0;
+}
+
+int vi4_channel_start_streaming(struct vb2_queue *vq, u32 count)
+{
+        struct tegra_channel *chan = vb2_get_drv_priv(vq);
+        struct media_pipeline *pipe = chan->video.entity.pipe;
+        int ret = 0, i;
+        unsigned long flags;
+        struct v4l2_ctrl *override_ctrl;
+
+        vi4_init(chan);
+        ret = media_entity_pipeline_start(&chan->video.entity, pipe);
+        if (ret < 0)
+                goto error_pipeline_start;
+
+        if (chan->bypass) {
+                ret = tegra_channel_set_stream(chan, true);
+                if (ret < 0)
+                        goto error_set_stream;
+                return ret;
+        }
+
+        spin_lock_irqsave(&chan->capture_state_lock, flags);
+        chan->capture_state = CAPTURE_IDLE;
+        spin_unlock_irqrestore(&chan->capture_state_lock, flags);
+
+        for (i = 0; i < chan->valid_ports; i++) {
+                ret = tegra_channel_capture_setup(chan, i);
+                if (ret < 0)
+                        goto error_capture_setup;
+        }
+
+        chan->sequence = 0;
+        tegra_channel_init_ring_buffer(chan);
+
+        /* disable override for vi mode */
+        override_ctrl = v4l2_ctrl_find(
+                &chan->ctrl_handler, V4L2_CID_OVERRIDE_ENABLE);
+        if (!chan->pg_mode) {
+                if (override_ctrl) {
+                        ret = v4l2_ctrl_s_ctrl(override_ctrl, false);
+                        if (ret < 0)
+                                dev_err(&chan->video.dev,
+                                        "failed to disable override control\n");
+                } else
+                        dev_err(&chan->video.dev,
+                                "No override control\n");
+        }
+
+        /* Update clock and bandwidth based on the format */
+        ret = tegra_channel_update_clknbw(chan, 1);
+        if (ret)
+                goto error_capture_setup;
+
+        INIT_WORK(&chan->error_work, tegra_channel_error_worker);
+        INIT_WORK(&chan->status_work, tegra_channel_status_worker);
+
+        /* Start kthread to capture data to buffer */
+        chan->kthread_capture_start = kthread_run(
+                                        tegra_channel_kthread_capture_start,
+                                        chan, chan->video.name);
+        if (IS_ERR(chan->kthread_capture_start)) {
+                dev_err(&chan->video.dev,
+                        "failed to run kthread for capture start\n");
+                ret = PTR_ERR(chan->kthread_capture_start);
+                goto error_capture_setup;
+        }
+
+        return 0;
+
+error_capture_setup:
+        if (!chan->pg_mode)
+                tegra_channel_set_stream(chan, false);
+error_set_stream:
+        media_entity_pipeline_stop(&chan->video.entity);
+error_pipeline_start:
+        vq->start_streaming_called = 0;
+        tegra_channel_queued_buf_done(chan, VB2_BUF_STATE_QUEUED);
+
+        return ret;
+}
+
+int vi4_channel_stop_streaming(struct vb2_queue *vq)
+{
+        struct tegra_channel *chan = vb2_get_drv_priv(vq);
+        bool is_streaming = atomic_read(&chan->is_streaming);
+        int i;
+
+        for (i = 0; i < chan->valid_ports; i++) {
+                if (chan->vnc_id[i] == -1)
+                        return 0;
+        }
+
+        cancel_work_sync(&chan->status_work);
+        cancel_work_sync(&chan->error_work);
+
+        if (!chan->bypass) {
+                tegra_channel_stop_kthreads(chan);
+                /* wait for last frame memory write ack */
+                if (is_streaming)
+                        tegra_channel_capture_done(chan);
+                for (i = 0; i < chan->valid_ports; i++)
+                        tegra_channel_notify_disable(chan, i);
+                /* free all the ring buffers */
+                free_ring_buffers(chan, chan->num_buffers);
+                /* dequeue buffers back to app which are in capture queue */
+                tegra_channel_queued_buf_done(chan, VB2_BUF_STATE_ERROR);
+        }
+
+        tegra_channel_set_stream(chan, false);
+        media_entity_pipeline_stop(&chan->video.entity);
+
+        if (!chan->bypass)
+                tegra_channel_update_clknbw(chan, 0);
+
+        return 0;
+}
+
+int tegra_vi4_power_on(struct tegra_mc_vi *vi)
+{
+        int ret;
+
+        ret = nvhost_module_busy(vi->ndev);
+        if (ret) {
+                dev_err(vi->dev, "%s:nvhost module is busy\n", __func__);
+                return ret;
+        }
+
+        ret = tegra_camera_emc_clk_enable();
+        if (ret)
+                goto err_emc_enable;
+
+        return 0;
+
+err_emc_enable:
+        nvhost_module_idle(vi->ndev);
+
+        return ret;
+}
+
+void tegra_vi4_power_off(struct tegra_mc_vi *vi)
+{
+        tegra_channel_ec_close(vi);
+        tegra_camera_emc_clk_disable();
+        nvhost_module_idle(vi->ndev);
+}
+
+int vi4_power_on(struct tegra_channel *chan)
+{
+        int ret = 0;
+        struct tegra_mc_vi *vi;
+        struct tegra_csi_device *csi;
+
+        vi = chan->vi;
+        csi = vi->csi;
+
+        /* Use chan->video as identifier of vi4 nvhost_module client
+         * since they are unique per channel
+         */
+        ret = nvhost_module_add_client(vi->ndev, &chan->video);
+        if (ret < 0)
+                return ret;
+
+        ret = tegra_vi4_power_on(vi);
+        if (ret < 0)
+                return ret;
+
+        if (atomic_add_return(1, &chan->power_on_refcnt) == 1) {
+                ret = tegra_channel_set_power(chan, 1);
+                if (ret < 0) {
+                        dev_err(vi->dev, "Failed to power on subdevices\n");
+                        return ret;
+                }
+        }
+
+#if defined(CONFIG_TEGRA_CAMERA_RTCPU)
+        ret = vi_capture_init(chan);
+        if (ret < 0)
+                return ret;
+#endif
+
+        return 0;
+}
+
+void vi4_power_off(struct tegra_channel *chan)
+{
+        int ret = 0;
+        struct tegra_mc_vi *vi;
+        struct tegra_csi_device *csi;
+
+        vi = chan->vi;
+        csi = vi->csi;
+
+#if defined(CONFIG_TEGRA_CAMERA_RTCPU)
+        vi_capture_shutdown(chan);
+#endif
+
+        if (atomic_dec_and_test(&chan->power_on_refcnt)) {
+                ret = tegra_channel_set_power(chan, 0);
+                if (ret < 0)
+                        dev_err(vi->dev, "Failed to power off subdevices\n");
+        }
+
+        tegra_vi4_power_off(vi);
+        nvhost_module_remove_client(vi->ndev, &chan->video);
+}
+
+static void tegra_channel_error_worker(struct work_struct *error_work)
+{
+        struct tegra_channel *chan;
+
+        chan = container_of(error_work, struct tegra_channel, error_work);
+
+        vi4_power_off(chan);
+        tegra_channel_handle_error(chan);
+}
+
+static void tegra_channel_notify_error_callback(void *client_data)
+{
+        struct tegra_channel *chan = (struct tegra_channel *)client_data;
+
+        spin_lock(&chan->capture_state_lock);
+        if (chan->capture_state == CAPTURE_GOOD)
+                chan->capture_state = CAPTURE_ERROR;
+        else {
+                spin_unlock(&chan->capture_state_lock);
+                return;
+        }
+        spin_unlock(&chan->capture_state_lock);
+
+        schedule_work(&chan->error_work);
+}
diff --git a/drivers/media/platform/tegra/camera/vi/vi4_fops.h b/drivers/media/platform/tegra/camera/vi/vi4_fops.h
new file mode 100644
index 000000000..4f48aca8d
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/vi4_fops.h
@@ -0,0 +1,37 @@
+/*
+ * Tegra Video Input 4 device common APIs
+ *
+ * Tegra Graphics Host VI
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Frank Chen <frankc@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __T186_VI_H__
+#define __T186_VI_H__
+
+int vi4_power_on(struct tegra_channel *chan);
+void vi4_power_off(struct tegra_channel *chan);
+int vi4_channel_start_streaming(struct vb2_queue *vq, u32 count);
+int vi4_channel_stop_streaming(struct vb2_queue *vq);
+int vi4_add_ctrls(struct tegra_channel *chan);
+void vi4_init_video_formats(struct tegra_channel *chan);
+long vi4_default_ioctl(struct file *file, void *fh,
+                        bool use_prio, unsigned int cmd, void *arg);
+
+struct tegra_vi_fops vi4_fops = {
+        .vi_power_on = vi4_power_on,
+        .vi_power_off = vi4_power_off,
+        .vi_start_streaming = vi4_channel_start_streaming,
+        .vi_stop_streaming = vi4_channel_stop_streaming,
+        .vi_add_ctrls = vi4_add_ctrls,
+        .vi_init_video_formats = vi4_init_video_formats,
+        .vi_default_ioctl = vi4_default_ioctl,
+};
+
+#endif
diff --git a/drivers/media/platform/tegra/camera/vi/vi4_formats.h b/drivers/media/platform/tegra/camera/vi/vi4_formats.h
new file mode 100644
index 000000000..bbdbbab71
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/vi4_formats.h
@@ -0,0 +1,140 @@
+/*
+ * NVIDIA Tegra Video Input Device Driver VI4 formats
+ *
+ * Copyright (c) 2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author: Bhanu Murthy V <bmurthyv@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __VI4_FORMATS_H_
+#define __VI4_FORMATS_H_
+
+#include "camera/vi/core.h"
+
+/*
+ * These go into the VI_CHn_PIXFMT_FORMAT register bits 7:0
+ * Output pixel memory format for the VI channel.
+ */
+enum tegra_image_format {
+        TEGRA_IMAGE_FORMAT_T_L8 = 16,
+
+        TEGRA_IMAGE_FORMAT_T_R16_I = 32,
+        TEGRA_IMAGE_FORMAT_T_B5G6R5,
+        TEGRA_IMAGE_FORMAT_T_R5G6B5,
+        TEGRA_IMAGE_FORMAT_T_A1B5G5R5,
+        TEGRA_IMAGE_FORMAT_T_A1R5G5B5,
+        TEGRA_IMAGE_FORMAT_T_B5G5R5A1,
+        TEGRA_IMAGE_FORMAT_T_R5G5B5A1,
+        TEGRA_IMAGE_FORMAT_T_A4B4G4R4,
+        TEGRA_IMAGE_FORMAT_T_A4R4G4B4,
+        TEGRA_IMAGE_FORMAT_T_B4G4R4A4,
+        TEGRA_IMAGE_FORMAT_T_R4G4B4A4,
+
+        TEGRA_IMAGE_FORMAT_T_A8B8G8R8 = 64,
+        TEGRA_IMAGE_FORMAT_T_A8R8G8B8,
+        TEGRA_IMAGE_FORMAT_T_B8G8R8A8,
+        TEGRA_IMAGE_FORMAT_T_R8G8B8A8,
+        TEGRA_IMAGE_FORMAT_T_A2B10G10R10,
+        TEGRA_IMAGE_FORMAT_T_A2R10G10B10,
+        TEGRA_IMAGE_FORMAT_T_B10G10R10A2,
+        TEGRA_IMAGE_FORMAT_T_R10G10B10A2,
+
+        TEGRA_IMAGE_FORMAT_T_A8Y8U8V8 = 193,
+        TEGRA_IMAGE_FORMAT_T_V8U8Y8A8,
+
+        TEGRA_IMAGE_FORMAT_T_A2Y10U10V10 = 197,
+        TEGRA_IMAGE_FORMAT_T_V10U10Y10A2,
+
+        TEGRA_IMAGE_FORMAT_T_Y8_U8__Y8_V8 = 200,
+        TEGRA_IMAGE_FORMAT_T_Y8_V8__Y8_U8,
+
+        TEGRA_IMAGE_FORMAT_T_U8_Y8__V8_Y8 = 203,
+
+        TEGRA_IMAGE_FORMAT_T_T_V8_Y8__U8_Y8 = 223,
+        TEGRA_IMAGE_FORMAT_T_T_Y8__U8__V8_N444,
+        TEGRA_IMAGE_FORMAT_T_Y8__U8V8_N444,
+        TEGRA_IMAGE_FORMAT_T_Y8__V8U8_N444,
+        TEGRA_IMAGE_FORMAT_T_Y8__U8__V8_N422,
+        TEGRA_IMAGE_FORMAT_T_Y8__U8V8_N422,
+        TEGRA_IMAGE_FORMAT_T_Y8__V8U8_N422,
+        TEGRA_IMAGE_FORMAT_T_Y8__U8__V8_N420,
+        TEGRA_IMAGE_FORMAT_T_Y8__U8V8_N420,
+        TEGRA_IMAGE_FORMAT_T_Y8__V8U8_N420,
+
+        TEGRA_IMAGE_FORMAT_T_Y10__U10__V10_N422 = 240,
+        TEGRA_IMAGE_FORMAT_T_Y10__U10V10_N422,
+        TEGRA_IMAGE_FORMAT_T_Y10__V10U10_N422,
+        TEGRA_IMAGE_FORMAT_T_Y10__U10__V10_N420,
+        TEGRA_IMAGE_FORMAT_T_Y10__U10V10_N420,
+        TEGRA_IMAGE_FORMAT_T_Y10__V10U10_N420,
+
+        TEGRA_IMAGE_FORMAT_T_R16 = 248,
+        TEGRA_IMAGE_FORMAT_T_R32,
+        TEGRA_IMAGE_FORMAT_T_L16_F,
+        TEGRA_IMAGE_FORMAT_T_L32_F,
+
+        TEGRA_IMAGE_FORMAT_T_DPCM_RAW10 = 254,
+        TEGRA_IMAGE_FORMAT_T_DPCM_RAW12,
+};
+
+static const struct tegra_video_format vi4_video_formats[] = {
+        /* RAW 6: TODO */
+
+        /* RAW 7: TODO */
+
+        /* RAW 8 */
+        TEGRA_VIDEO_FORMAT(RAW8, 8, SRGGB8_1X8, 1, 1, T_L8,
+                                RAW8, SRGGB8, "RGRG.. GBGB.."),
+        TEGRA_VIDEO_FORMAT(RAW8, 8, SGRBG8_1X8, 1, 1, T_L8,
+                                RAW8, SGRBG8, "GRGR.. BGBG.."),
+        TEGRA_VIDEO_FORMAT(RAW8, 8, SGBRG8_1X8, 1, 1, T_L8,
+                                RAW8, SGBRG8, "GBGB.. RGRG.."),
+        TEGRA_VIDEO_FORMAT(RAW8, 8, SBGGR8_1X8, 1, 1, T_L8,
+                                RAW8, SBGGR8, "BGBG.. GRGR.."),
+
+        /* RAW 10 */
+        TEGRA_VIDEO_FORMAT(RAW10, 10, SRGGB10_1X10, 2, 1, T_R16_I,
+                                RAW10, SRGGB10, "RGRG.. GBGB.."),
+        TEGRA_VIDEO_FORMAT(RAW10, 10, SGRBG10_1X10, 2, 1, T_R16_I,
+                                RAW10, SGRBG10, "GRGR.. BGBG.."),
+        TEGRA_VIDEO_FORMAT(RAW10, 10, SGBRG10_1X10, 2, 1, T_R16_I,
+                                RAW10, SGBRG10, "GBGB.. RGRG.."),
+        TEGRA_VIDEO_FORMAT(RAW10, 10, SBGGR10_1X10, 2, 1, T_R16_I,
+                                RAW10, SBGGR10, "BGBG.. GRGR.."),
+
+        /* RAW 12 */
+        TEGRA_VIDEO_FORMAT(RAW12, 12, SRGGB12_1X12, 2, 1, T_R16_I,
+                                RAW12, SRGGB12, "RGRG.. GBGB.."),
+        TEGRA_VIDEO_FORMAT(RAW12, 12, SGRBG12_1X12, 2, 1, T_R16_I,
+                                RAW12, SGRBG12, "GRGR.. BGBG.."),
+        TEGRA_VIDEO_FORMAT(RAW12, 12, SGBRG12_1X12, 2, 1, T_R16_I,
+                                RAW12, SGBRG12, "GBGB.. RGRG.."),
+        TEGRA_VIDEO_FORMAT(RAW12, 12, SBGGR12_1X12, 2, 1, T_R16_I,
+                                RAW12, SBGGR12, "BGBG.. GRGR.."),
+
+        /* RGB888 */
+        TEGRA_VIDEO_FORMAT(RGB888, 24, RGB888_1X24, 4, 1, T_A8R8G8B8,
+                                RGB888, ABGR32, "BGRA-8-8-8-8"),
+        TEGRA_VIDEO_FORMAT(RGB888, 24, RGB888_1X32_PADHI, 4, 1, T_A8B8G8R8,
+                                RGB888, RGB32, "RGB-8-8-8-8"),
+
+        /* YUV422 */
+        TEGRA_VIDEO_FORMAT(YUV422, 16, UYVY8_1X16, 2, 1, T_U8_Y8__V8_Y8,
+                                YUV422_8, UYVY, "YUV 4:2:2"),
+        TEGRA_VIDEO_FORMAT(YUV422, 16, UYVY8_1X16, 1, 1, T_Y8__V8U8_N422,
+                                YUV422_8, NV16, "NV16"),
+        TEGRA_VIDEO_FORMAT(YUV422, 16, UYVY8_2X8, 2, 1, T_U8_Y8__V8_Y8,
+                                YUV422_8, UYVY, "YUV 4:2:2 UYVY"),
+        TEGRA_VIDEO_FORMAT(YUV422, 16, VYUY8_2X8, 2, 1, T_T_V8_Y8__U8_Y8,
+                                YUV422_8, VYUY, "YUV 4:2:2 VYUY"),
+        TEGRA_VIDEO_FORMAT(YUV422, 16, YUYV8_2X8, 2, 1, T_Y8_U8__Y8_V8,
+                                YUV422_8, YUYV, "YUV 4:2:2 YUYV"),
+        TEGRA_VIDEO_FORMAT(YUV422, 16, YVYU8_2X8, 2, 1, T_Y8_V8__Y8_U8,
+                                YUV422_8, YVYU, "YUV 4:2:2 YVYU"),
+};
+
+#endif
diff --git a/drivers/media/platform/tegra/camera/vi/vi4_registers.h b/drivers/media/platform/tegra/camera/vi/vi4_registers.h
new file mode 100644
index 000000000..588ebb324
--- /dev/null
+++ b/drivers/media/platform/tegra/camera/vi/vi4_registers.h
@@ -0,0 +1,272 @@
+/*
+ * drivers/media/platform/tegra/camera/vi/vi4_registers.h
+ *
+ * Tegra 18x VI register offsets
+ *
+ * Copyright (c) 2016, NVIDIA CORPORATION. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __VI4_REGISTERS_H__
+#define __VI4_REGISTERS_H__
+
+/* VI registers. Start from 0x0 */
+#define VI_STREAMS (6)
+#define VIRTUAL_CHANNELS (4)
+#define VI4_CHANNEL_OFFSET         0x10000
+
+#define CFG_INTERRUPT_STATUS            0x44
+#define CFG_INTERRUPT_MASK                      0x48
+#define VGP6_INT_MASK                                   (0x1 << 29)
+#define VGP5_INT_MASK                                   (0x1 << 28)
+#define VGP4_INT_MASK                                   (0x1 << 27)
+#define VGP3_INT_MASK                                   (0x1 << 26)
+#define VGP2_INT_MASK                                   (0x1 << 25)
+#define VGP1_INT_MASK                                   (0x1 << 24)
+#define HOST_PKTINJECT_STALL_ERR_MASK   (0x1 << 7)
+#define CSIMUX_FIFO_OVFL_ERR_MASK               (0x1 << 6)
+#define ATOMP_PACKER_OVFL_ERR_MASK              (0x1 << 5)
+#define FMLITE_BUF_OVFL_ERR_MASK                (0x1 << 4)
+#define NOTIFY_FIFO_OVFL_ERR_MASK               (0x1 << 3)
+#define ISPBUFA_ERR_MASK                                (0x1 << 0)
+
+#define CFG_PWM_HIGH_PULSE                      0x50
+#define PWM_HIGH_PULSE                          (0xffffffff << 0)
+
+#define CSIMUX_CONFIG_STREAM_0          0x424
+#define CSIMUX_CONFIG_STREAM_1          0x428
+#define CSIMUX_CONFIG_STREAM_2          0x42C
+#define CSIMUX_CONFIG_STREAM_3          0x430
+#define CSIMUX_CONFIG_STREAM_4          0x434
+#define CSIMUX_CONFIG_STREAM_5          0x438
+#define FRAMEIDGEN                                      (0xf << 26)
+#define STICKYFAULT                                     (0x1 << 25)
+#define VPR                                                     (0x1 << 24)
+#define SRESET                                          (0x1 << 23)
+#define QBLOCK                                          (0x1 << 22)
+#define FEINJECT                                        (0x1 << 21)
+#define FESHORTTIMER                            (0x1 << 20)
+#define FEMAXTIME                                       (0xffff << 4)
+#define WT                                                      (0xf << 0)
+
+#define NOTIFY_FIFO_TAG_0                       0x4000
+#define NOTIFY_FRAME_ID                         (0xffff << 16)
+#define NOTIFY_CHANNEL                          (0xff << 8)
+#define NOTIFY_CHANNEL_SHIFT            (8)
+#define NOTIFY_TAG                                      (0x1f << 1)
+#define NOTIFY_TAG_SHIFT                        (1)
+#define NOTIFY_VALID                            (0x1 << 0)
+
+#define TAG_FS                                          0
+#define TAG_FE                                          1
+#define TAG_CSIMUX_FRAME                        2
+#define TAG_CSIMUX_STREAM                       3
+#define TAG_CHANSEL_PXL_SOF                     4
+#define TAG_CHANSEL_PXL_EOF                     5
+#define TAG_CHANSEL_EMBED_SOF           6
+#define TAG_CHANSEL_EMBED_EOF           7
+#define TAG_CHANSEL_NLINES                      8
+#define TAG_CHANSEL_FAULT                       9
+#define TAG_CHANSEL_FAULT_FE            10
+#define TAG_CHANSEL_NOMATCH                     11
+#define TAG_CHANSEL_COLLISION           12
+#define TAG_CHANSEL_SHORT_FRAME         13
+#define TAG_CHANSEL_LOAD_FRAMED         14
+#define TAG_ATOMP_PACKER_OVERFLOW       15
+#define TAG_ATOMP_FS                            16
+#define TAG_ATOMP_FE                            17
+#define TAG_ATOMP_FRAME_DONE            18
+#define TAG_ATOMP_EMB_DATA_DONE         19
+#define TAG_ATOMP_FRAME_NLINES_DONE     20
+#define TAG_ATOMP_FRAME_TRUNCATED       21
+#define TAG_ATOMP_FRAME_TOSSED          22
+#define TAG_ATOMP_PDAF_DATA_DONE        23
+#define TAG_ISPBUF_FIFO_OVERFLOW        26
+#define TAG_ISPBUF_FS                           27
+#define TAG_ISPBUF_FE                           28
+#define TAG_VGP0_DONE                           29
+#define TAG_VGP1_DONE                           30
+#define TAG_FMLITE_DONE                         31
+
+#define NOTIFY_FIFO_TIMESTAMP_0         0x4004
+#define NOTIFY_TIMESTAMP                        (0xffffffff << 0)
+
+#define NOTIFY_FIFO_DATA_0                      0x4008
+#define NOTIFY_DATA                                     (0xffffffff << 0)
+
+#define NOTIFY_TAG_CLASSIFY_0           0x6000
+#define NOTIFY_TAG_CLASSIFY_1           0x6004
+#define NOTIFY_TAG_CLASSIFY_2           0x6008
+#define NOTIFY_TAG_CLASSIFY_3           0x600c
+#define NOTIFY_TAG_CLASSIFY_4           0x6010
+#define STREAM5_FEINJECT_VC                     (0xf << 20)
+#define STREAM4_FEINJECT_VC                     (0xf << 16)
+#define STREAM3_FEINJECT_VC                     (0xf << 12)
+#define STREAM2_FEINJECT_VC                     (0xf << 8)
+#define STREAM1_FEINJECT_VC                     (0xf << 4)
+#define STREAM0_FEINJECT_VC                     (0xf << 0)
+
+#define NOTIFY_FIFO_OCCUPANCY_0         0x6014
+#define NOTIFY_MAX                                      (0x3ff << 20)
+#define NOTIFY_CURRENT                          (0x3ff << 10)
+#define NOTIFY_CURRENT_SHIFT            10
+#define NOTIFY_SIZE                                     (0x3ff << 0)
+
+/* VI_CH registers. Start from 0x10000, offset 0x10000 */
+#define CHANNEL_COMMAND                         0x004
+#define WR_ACT_SEL                                      (0x1 << 5)
+#define RD_MUX_SEL                                      (0x1 << 4)
+#define AUTOLOAD                                        (0x1 << 1)
+#define LOAD                                            (0x1 << 0)
+
+#define CONTROL                                         0x01c
+#define SPARE                                           (0xffff << 16)
+#define POST_RUNAWAY_EMBED                      (0x1 << 4)
+#define POST_RUNAWAY_PIXEL                      (0x1 << 3)
+#define EARLY_ABORT                                     (0x1 << 2)
+#define SINGLESHOT                                      (0x1 << 1)
+#define MATCH_STATE_EN                          (0x1 << 0)
+
+#define MATCH                                           0x020
+#define STREAM                                          (0x3f << 14)
+#define STREAM_SHIFT                            (14)
+#define STREAM_MASK                                     (0x3f << 8)
+#define VIRTUAL_CHANNEL                         (0xf << 4)
+#define VIRTUAL_CHANNEL_SHIFT           (4)
+#define VIRTUAL_CHANNEL_MASK            (0xf << 0)
+
+#define MATCH_DATATYPE                          0x024
+#define DATATYPE                                        (0x3f << 6)
+#define DATATYPE_SHIFT                          (6)
+#define DATATYPE_MASK                           (0x3f << 0)
+#define DATATYPE_MASK_SHIFT                     (0)
+
+#define MATCH_FRAMEID                           0x028
+#define FRAMEID                                         (0xffff << 16)
+#define FRAMEID_SHIFT                           (16)
+#define FRAMEID_MASK                            (0xffff << 0)
+
+#define DT_OVERRIDE                                     0x02c
+#define OVRD_DT                                         (0x3f << 1)
+#define DT_OVRD_EN                                      (0x1 << 0)
+
+#define FRAME_X                                         0x030
+#define CROP_X                                          0x04c
+#define OUT_X                                           0x058
+#define WIDTH                                           (0xffff < 0)
+
+#define FRAME_Y                                         0x034
+#define CROP_Y                                          0x054
+#define OUT_Y                                           0x05c
+#define HEIGHT                                          (0xffff < 0)
+
+#define EMBED_X                                         0x038
+#define MAX_BYTES                                       (0x3ffff < 0)
+
+#define EMBED_Y                                         0x03c
+#define SKIP_Y                                          0x050
+#define LINES                                           (0xffff < 0)
+/* for EMBED_Y only */
+#define EXPECT                                          (0x1 << 24)
+
+#define LINE_TIMER                                      0x044
+#define LINE_TIMER_EN                           (0x1 << 25)
+#define PERIODIC                                        (0x1 << 24)
+#define TRIPLINE                                        (0xffff << 0)
+
+#define SKIP_X                                          0x048
+#define PACKETS                                         (0x1fff << 0)
+
+#define NOTIFY_MASK                                     0x060
+#define MASK_DTYPE_MISMATCH                     (0x1 << 31)
+#define MASK_EMBED_INFRINGE                     (0x1 << 22)
+#define MASK_EMBED_LONG_LINE            (0x1 << 21)
+#define MASK_EMBED_SPURIOUS                     (0x1 << 20)
+#define MASK_EMBED_RUNAWAY                      (0x1 << 19)
+#define MASK_EMBED_MISSING_LE           (0x1 << 18)
+#define MASK_EMBED_EOF                          (0x1 << 17)
+#define MASK_EMBED_SOF                          (0x1 << 16)
+#define MASK_PIXEL_LINE_TIMER           (0x1 << 7)
+#define MASK_PIXEL_SHORT_LINE           (0x1 << 6)
+#define MASK_PIXEL_LONG_LINE            (0x1 << 5)
+#define MASK_PIXEL_SPURIOUS                     (0x1 << 4)
+#define MASK_PIXEL_RUNAWAY                      (0x1 << 3)
+#define MASK_PIXEL_MISSING_LE           (0x1 << 2)
+#define MASK_PIXEL_EOF                          (0x1 << 1)
+#define MASK_PIXEL_SOF                          (0x1 << 0)
+
+#define NOTIFY_MASK_XCPT                        0x064
+#define MASK_NOMATCH                            (0x1 << 9)
+#define MASK_EMBED_OPEN_LINE            (0x1 << 8)
+#define MASK_PIXEL_OPEN_LINE            (0x1 << 7)
+#define MASK_FORCE_FE                           (0x1 << 6)
+#define MASK_STALE_FRAME                        (0x1 << 5)
+#define MASK_COLLISION                          (0x1 << 4)
+#define MASK_EMPTY_FRAME                        (0x1 << 3)
+#define MASK_EMBED_SHORT_FRAME          (0x1 << 2)
+#define MASK_PIXEL_SHORT_FRAME          (0x1 << 1)
+#define MASK_LOAD_FRAMED                        (0x1 << 0)
+
+#define FRAME_COUNT                                     0x06c
+
+#define PIXFMT_ENABLE                           0x080
+#define PDAF_EN                                         (0x1 << 2)
+#define COMPAND_EN                                      (0x1 << 1)
+#define PIXFMT_EN                                       (0x1 << 0)
+
+#define PIXFMT_FORMAT                           0x084
+#define FORMAT                                          (0xff << 0)
+/* refer to enum tegra_image_format in core.h */
+
+#define PIXFMT_WIDE                                     0x088
+#define ENDIAN_BIG                                      (0x0 << 1)
+#define ENDIAN_LITTLE                           (0x1 << 1)
+#define PIXFMT_WIDE_EN                          (0x1 << 0)
+
+#define DPCM_STRIP                                      0x0b8
+#define OVERFETCH                                       (0x1fff < 16)
+#define STRIP_WIDTH                                     (0x1fff < 0)
+
+#define ATOMP_DPCM_CHUNK                        0x0ec
+#define CHUNK_OFFSET                            (0x3ffff << 0)
+
+#define ATOMP_SURFACE_OFFSET0           0x0e0
+#define ATOMP_SURFACE_OFFSET1           0x0f0
+#define ATOMP_SURFACE_OFFSET2           0x0fc
+#define ATOMP_EMB_SURFACE_OFFSET0       0x108
+#define SURFACE_OFFSET                          (0xffffffff << 0)
+
+#define ATOMP_SURFACE_OFFSET0_H         0x0e4
+#define ATOMP_SURFACE_OFFSET1_H         0x0f4
+#define ATOMP_SURFACE_OFFSET2_H         0x100
+#define ATOMP_EMB_SURFACE_OFFSET0_H             0x10c
+#define SURFACE_OFFSET_HI                       (0xff << 0)
+
+#define ATOMP_SURFACE_STRIDE0           0x0e8
+#define ATOMP_SURFACE_STRIDE1           0x0f8
+#define ATOMP_SURFACE_STRIDE2           0x104
+#define ATOMP_EMB_SURFACE_STRIDE0       0x110
+#define SURFACE_STRIDE                          (0x3ffff << 0)
+#define ATOMP_RESERVE                           0x120
+
+#define ISPBUFA                                         0x134
+#define ISPBUFA_EN                                      (0x1 << 0)
+
+#define ISPBUFA_ERROR                           0x1000
+#define FIFO_OVERFLOW                           (0x1 << 0)
+
+#define FMLITE_ERROR                            0x313c
+#define NOTIFY_ERROR                            0x6020
+
+#endif /* __VI4_REGISTERS_H__ */
diff --git a/drivers/media/platform/tegra/mipical/Kconfig b/drivers/media/platform/tegra/mipical/Kconfig
new file mode 100644
index 000000000..e00659b2b
--- /dev/null
+++ b/drivers/media/platform/tegra/mipical/Kconfig
@@ -0,0 +1,11 @@
+config TEGRA_MIPI_CAL
+        depends on TEGRA_DC || TEGRA_CAMERA_PLATFORM
+        bool "Enable MIPI Cal driver"
+        default y
+        select REGMAP
+        select REGMAP_MMIO
+        help
+          Provides an interfaces to do MIPI calibration for both DSI and CSI blocks.
+          You will always want this enabled if you are using either Camera or Display.
+          Disabling this can cause issues for drivers that depend on it being present.
+          Leaving this on is harmless even if no drivers that need it are enabled.
diff --git a/drivers/media/platform/tegra/mipical/Makefile b/drivers/media/platform/tegra/mipical/Makefile
new file mode 100644
index 000000000..ffd996e09
--- /dev/null
+++ b/drivers/media/platform/tegra/mipical/Makefile
@@ -0,0 +1,6 @@
+GCOV_PROFILE := y
+#
+# Makefile for Mipical driver
+#
+obj-$(CONFIG_TEGRA_MIPI_CAL) += mipi_cal.o
+obj-$(CONFIG_TEGRA_MIPI_CAL) += vmipi/vmipi.o
diff --git a/drivers/media/platform/tegra/mipical/mipi_cal.c b/drivers/media/platform/tegra/mipical/mipi_cal.c
new file mode 100644
index 000000000..d53af8ae3
--- /dev/null
+++ b/drivers/media/platform/tegra/mipical/mipi_cal.c
@@ -0,0 +1,1096 @@
+/*
+ * mipi_cal.c
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/device.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/of_platform.h>
+#include <linux/regmap.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/export.h>
+#include <linux/reset.h>
+#include <linux/string.h>
+#include <linux/debugfs.h>
+#include <linux/clk/tegra.h>
+#include <linux/pm.h>
+#include <linux/miscdevice.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+
+#include <soc/tegra/fuse.h>
+#include <soc/tegra/tegra_powergate.h>
+#include <linux/tegra_prod.h>
+#include <uapi/misc/tegra_mipi_ioctl.h>
+
+#include "registers.h"
+#include "mipi_cal.h"
+#include "vmipi/vmipi.h"
+
+#define DRV_NAME "tegra_mipi_cal"
+#define MIPI_CAL_TIMEOUT_MSEC 500
+
+struct tegra_mipi_bias {
+        /* BIAS_PAD_CFG0 */
+        u8 pad_pdvclamp;
+        u8 e_vclamp_ref;
+        /* BIAS_PAD_CFG1 */
+        u8 pad_driv_up_ref;
+        u8 pad_driv_dn_ref;
+        /* BIAS_PAD_CFG2 */
+        u8 pad_vclamp_level;
+        u8 pad_vauxp_level;
+};
+
+struct tegra_mipi_prod_csi {
+        struct tegra_mipi_bias bias_csi;
+        u8 overide_x;
+        u8 termos_x;
+        u8 termos_x_clk;
+};
+
+struct tegra_mipi_prod_dsi {
+        struct tegra_mipi_bias bias_dsi;
+        u8 overide_x;
+        u8 hspdos_x;
+        u8 hspuos_x;
+        u8 termos_x;
+        u8 clk_overide_x;
+        u8 clk_hspdos_x;
+        u8 clk_hspuos_x;
+        u8 clk_hstermos_x;
+};
+
+struct tegra_mipi;
+struct tegra_mipi {
+        struct device *dev;
+        struct clk *mipi_cal_clk;
+        struct clk *mipi_cal_fixed;
+        struct reset_control *rst;
+        struct regmap *regmap;
+        struct mutex lock;
+        /* Legacy way of storing mipical reg config */
+        struct tegra_mipi_prod_csi *prod_csi;
+        struct tegra_mipi_prod_dsi *prod_dsi;
+        /* If use tegra_prod framework */
+        struct tegra_prod *prod_gr_csi;
+        struct tegra_prod *prod_gr_dsi;
+        const struct tegra_mipi_soc *soc;
+        void __iomem *io;
+        atomic_t refcount;
+        struct miscdevice misc_dev;
+};
+
+static struct tegra_mipi *mipi;
+static const struct regmap_config t210_mipi_cal_regmap_config = {
+        .reg_bits = 32,
+        .reg_stride = 4,
+        .val_bits = 32,
+        .cache_type = REGCACHE_NONE,
+        .fast_io = 1,
+};
+
+#define ADDR(x) (x + mipi->soc->addr_offset)
+#define dump_register(nm)       \
+{                               \
+        .name = #nm,            \
+        .offset = nm,           \
+}
+struct tegra_mipi_soc {
+        int powergate_id;
+        unsigned int total_dsilanes;
+        unsigned int total_cillanes;
+        char addr_offset;
+        char ppsb_war;
+        int (*pad_enable)(struct tegra_mipi *mipi);
+        int (*pad_disable)(struct tegra_mipi *mipi);
+        int (*calibrate)(struct tegra_mipi *mipi, int lanes);
+        int (*parse_cfg)(struct platform_device *pdev, struct tegra_mipi *mipi);
+        u8 virtual_dev;
+};
+
+static int mipical_write(struct regmap *map,
+                unsigned int reg, unsigned int val)
+{
+        int ret, rb_val;
+
+        ret = regmap_write(map, reg, val);
+        if (mipi->soc->ppsb_war)
+        /* Read back register to make sure that register writes completed */
+                regmap_read(map, reg, &rb_val);
+
+        return ret;
+}
+
+static int mipical_update_bits(struct regmap *map, unsigned int reg,
+                unsigned int mask, unsigned int val)
+{
+        int ret, rb_val;
+
+        ret = regmap_update_bits(map, reg, mask, val);
+        if (mipi->soc->ppsb_war)
+        /* Read back register to make sure that register writes completed */
+                regmap_read(map, reg, &rb_val);
+
+        return ret;
+}
+static int tegra_mipi_clk_enable(struct tegra_mipi *mipi)
+{
+        int err;
+
+        err = tegra_unpowergate_partition(mipi->soc->powergate_id);
+        if (err) {
+                dev_err(mipi->dev, "Fail to unpowergate SOR\n");
+                return err;
+        }
+
+        err = clk_prepare_enable(mipi->mipi_cal_fixed);
+        if (err) {
+                dev_err(mipi->dev, "Fail to enable uart_mipi_cal clk\n");
+                goto err_fixed_clk;
+        }
+        mdelay(1);
+        err = clk_prepare_enable(mipi->mipi_cal_clk);
+        if (err) {
+                dev_err(mipi->dev, "Fail to enable mipi_cal clk\n");
+                goto err_mipi_cal_clk;
+        }
+        return 0;
+
+err_mipi_cal_clk:
+        clk_disable_unprepare(mipi->mipi_cal_fixed);
+err_fixed_clk:
+        tegra_powergate_partition(mipi->soc->powergate_id);
+
+        return err;
+}
+
+static void tegra_mipi_clk_disable(struct tegra_mipi *mipi)
+{
+        clk_disable_unprepare(mipi->mipi_cal_clk);
+        clk_disable_unprepare(mipi->mipi_cal_fixed);
+        tegra_powergate_partition(mipi->soc->powergate_id);
+}
+
+static void tegra_mipi_print(struct tegra_mipi *mipi) __maybe_unused;
+static void tegra_mipi_print(struct tegra_mipi *mipi)
+{
+        int val;
+        unsigned long rate;
+#define pr_reg(a)                                               \
+        do {                                                    \
+                regmap_read(mipi->regmap, ADDR(a), &val);               \
+                dev_info(mipi->dev, "%-30s %#04x %#010x\n",             \
+                        #a, a + mipi->soc->addr_offset, val);           \
+        } while (0)
+
+        rate = clk_get_rate(mipi->mipi_cal_fixed);
+        dev_dbg(mipi->dev, "Fixed clk %luMHz\n", rate/1000000);
+
+        pr_reg(MIPI_CAL_CTRL);
+        pr_reg(CIL_MIPI_CAL_STATUS);
+        pr_reg(CIL_MIPI_CAL_STATUS_2);
+        pr_reg(CILA_MIPI_CAL_CONFIG);
+        pr_reg(CILB_MIPI_CAL_CONFIG);
+        pr_reg(CILC_MIPI_CAL_CONFIG);
+        pr_reg(CILD_MIPI_CAL_CONFIG);
+        pr_reg(CILE_MIPI_CAL_CONFIG);
+        pr_reg(CILF_MIPI_CAL_CONFIG);
+        pr_reg(DSIA_MIPI_CAL_CONFIG);
+        pr_reg(DSIB_MIPI_CAL_CONFIG);
+        pr_reg(DSIC_MIPI_CAL_CONFIG);
+        pr_reg(DSID_MIPI_CAL_CONFIG);
+        pr_reg(MIPI_BIAS_PAD_CFG0);
+        pr_reg(MIPI_BIAS_PAD_CFG1);
+        pr_reg(MIPI_BIAS_PAD_CFG2);
+        pr_reg(DSIA_MIPI_CAL_CONFIG_2);
+        pr_reg(DSIB_MIPI_CAL_CONFIG_2);
+        pr_reg(DSIC_MIPI_CAL_CONFIG_2);
+        pr_reg(DSID_MIPI_CAL_CONFIG_2);
+#undef pr_reg
+}
+static int tegra_mipi_wait(struct tegra_mipi *mipi, int lanes)
+{
+        unsigned long timeout;
+        int val;
+
+        mipical_write(mipi->regmap, ADDR(CIL_MIPI_CAL_STATUS), 0xffffffff);
+        mipical_write(mipi->regmap, ADDR(CIL_MIPI_CAL_STATUS_2), 0xffffffff);
+        mipical_update_bits(mipi->regmap, ADDR(MIPI_CAL_CTRL), STARTCAL, 0x1);
+
+        timeout = jiffies + msecs_to_jiffies(MIPI_CAL_TIMEOUT_MSEC);
+        while (time_before(jiffies, timeout)) {
+                regmap_read(mipi->regmap, ADDR(CIL_MIPI_CAL_STATUS), &val);
+                if (((val & lanes) == lanes) && ((val & CAL_ACTIVE) == 0))
+                        return 0;
+                usleep_range(10, 50);
+        }
+        /* Sometimes there is false timeout. Sleep past the timeout and did
+         * not check the status again.
+         * Later status register dump shows no timeout.
+         * Add another check here in case sleep past the timeout.
+         */
+        regmap_read(mipi->regmap, ADDR(CIL_MIPI_CAL_STATUS), &val);
+        if (((val & lanes) == lanes) && ((val & CAL_ACTIVE) == 0))
+                return 0;
+        dev_err(mipi->dev, "Mipi cal timeout,val:%x, lanes:%x\n", val, lanes);
+        tegra_mipi_print(mipi);
+        return -ETIMEDOUT;
+
+}
+
+static int tegra_mipi_apply_bias_prod(struct regmap *reg,
+                                struct tegra_mipi_bias *bias)
+{
+        int ret;
+        unsigned int val;
+
+        val = (bias->pad_pdvclamp << PDVCLAMP_SHIFT) |
+                (bias->e_vclamp_ref << E_VCLAMP_REF_SHIFT);
+        ret = mipical_write(reg, ADDR(MIPI_BIAS_PAD_CFG0), val);
+        if (ret)
+                return ret;
+        val = (bias->pad_driv_up_ref << PAD_DRIV_UP_REF_SHIFT) |
+                (bias->pad_driv_dn_ref << PAD_DRIV_DN_REF_SHIFT);
+        ret = mipical_write(reg, ADDR(MIPI_BIAS_PAD_CFG1), val);
+        if (ret)
+                return ret;
+        val = (bias->pad_vclamp_level << PAD_VCLAMP_LEVEL_SHIFT) |
+                (bias->pad_vauxp_level << PAD_VAUXP_LEVEL_SHIFT);
+        ret = mipical_write(reg, ADDR(MIPI_BIAS_PAD_CFG2), val);
+
+        return ret;
+}
+static void tegra_mipi_apply_csi_prod(struct regmap *reg,
+                                struct tegra_mipi_prod_csi *prod_csi,
+                                int lanes)
+{
+        int val;
+
+        tegra_mipi_apply_bias_prod(reg, &prod_csi->bias_csi);
+        val = (prod_csi->termos_x << TERMOSA_SHIFT) |
+                (prod_csi->termos_x_clk << TERMOSA_CLK_SHIFT);
+
+        if (lanes & CSIA)
+                mipical_write(reg, ADDR(CILA_MIPI_CAL_CONFIG), val);
+        if (lanes & CSIB)
+                mipical_write(reg, ADDR(CILB_MIPI_CAL_CONFIG), val);
+        if (lanes & CSIC)
+                mipical_write(reg, ADDR(CILC_MIPI_CAL_CONFIG), val);
+        if (lanes & CSID)
+                mipical_write(reg, ADDR(CILD_MIPI_CAL_CONFIG), val);
+        if (lanes & CSIE)
+                mipical_write(reg, ADDR(CILE_MIPI_CAL_CONFIG), val);
+        if (lanes & CSIF)
+                mipical_write(reg, ADDR(CILF_MIPI_CAL_CONFIG), val);
+}
+
+static void tegra_mipi_apply_dsi_prod(struct regmap *reg,
+                                struct tegra_mipi_prod_dsi *prod_dsi,
+                                int lanes)
+{
+        int val, clk_val;
+
+        tegra_mipi_apply_bias_prod(reg, &prod_dsi->bias_dsi);
+        val = (prod_dsi->hspuos_x << HSPUOSDSIA_SHIFT) |
+                (prod_dsi->termos_x << TERMOSDSIA_SHIFT);
+        clk_val = (prod_dsi->clk_hspuos_x << HSCLKPUOSDSIA_SHIFT) |
+                (prod_dsi->clk_hstermos_x << HSCLKTERMOSDSIA_SHIFT);
+        if (lanes & DSIA) {
+                mipical_write(reg, ADDR(DSIA_MIPI_CAL_CONFIG), val);
+                mipical_write(reg, ADDR(DSIA_MIPI_CAL_CONFIG_2), clk_val);
+        }
+        if (lanes & DSIB) {
+                mipical_write(reg, ADDR(DSIB_MIPI_CAL_CONFIG), val);
+                mipical_write(reg, ADDR(DSIB_MIPI_CAL_CONFIG_2), clk_val);
+        }
+        if (lanes & DSIC) {
+                mipical_write(reg, ADDR(DSIC_MIPI_CAL_CONFIG), val);
+                mipical_write(reg, ADDR(DSIC_MIPI_CAL_CONFIG_2), clk_val);
+        }
+        if (lanes & DSID) {
+                mipical_write(reg, ADDR(DSID_MIPI_CAL_CONFIG), val);
+                mipical_write(reg, ADDR(DSID_MIPI_CAL_CONFIG_2), clk_val);
+        }
+
+}
+
+static int _t18x_tegra_mipi_bias_pad_enable(struct tegra_mipi *mipi)
+{
+        if (atomic_read(&mipi->refcount) < 0) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        if (atomic_inc_return(&mipi->refcount) == 1) {
+                tegra_mipi_clk_enable(mipi);
+                mipical_update_bits(mipi->regmap, ADDR(MIPI_BIAS_PAD_CFG0),
+                                PDVCLAMP, 0 << PDVCLAMP_SHIFT);
+                mipical_update_bits(mipi->regmap, ADDR(MIPI_BIAS_PAD_CFG2),
+                                PDVREG, 0 << PDVREG_SHIFT);
+        }
+        return 0;
+}
+
+static int _t18x_tegra_mipi_bias_pad_disable(struct tegra_mipi *mipi)
+{
+        if (atomic_read(&mipi->refcount) < 1) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        if (atomic_dec_return(&mipi->refcount) == 0) {
+                mipical_update_bits(mipi->regmap, ADDR(MIPI_BIAS_PAD_CFG0),
+                                PDVCLAMP, 1 << PDVCLAMP_SHIFT);
+                mipical_update_bits(mipi->regmap, ADDR(MIPI_BIAS_PAD_CFG2),
+                                PDVREG, 1 << PDVREG_SHIFT);
+                tegra_mipi_clk_disable(mipi);
+        }
+        return 0;
+}
+
+static int _t21x_tegra_mipi_bias_pad_enable(struct tegra_mipi *mipi)
+{
+        if (atomic_read(&mipi->refcount) < 0) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        if (atomic_inc_return(&mipi->refcount) == 1) {
+                tegra_mipi_clk_enable(mipi);
+                return mipical_update_bits(mipi->regmap,
+                                ADDR(MIPI_BIAS_PAD_CFG2), PDVREG, 0);
+        }
+        return 0;
+}
+
+int tegra_mipi_bias_pad_enable(void)
+{
+        if (!mipi)
+                return -EPROBE_DEFER;
+        dev_dbg(mipi->dev, "%s", __func__);
+
+        if (mipi->soc->pad_enable)
+                return mipi->soc->pad_enable(mipi);
+        else
+                return 0;
+}
+EXPORT_SYMBOL(tegra_mipi_bias_pad_enable);
+
+static int _t21x_tegra_mipi_bias_pad_disable(struct tegra_mipi *mipi)
+{
+        if (atomic_read(&mipi->refcount) < 1) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        if (atomic_dec_return(&mipi->refcount) == 0) {
+                mipical_update_bits(mipi->regmap,
+                                ADDR(MIPI_BIAS_PAD_CFG2), PDVREG, PDVREG);
+                tegra_mipi_clk_disable(mipi);
+        }
+        return 0;
+}
+
+int tegra_mipi_bias_pad_disable(void)
+{
+        if (!mipi)
+                return -ENODEV;
+        dev_dbg(mipi->dev, "%s", __func__);
+
+        if (mipi->soc->pad_disable)
+                return mipi->soc->pad_disable(mipi);
+        else
+                return 0;
+}
+EXPORT_SYMBOL(tegra_mipi_bias_pad_disable);
+
+static void select_lanes(struct tegra_mipi *mipi, int lanes)
+{
+        mipical_update_bits(mipi->regmap, ADDR(CILA_MIPI_CAL_CONFIG), SELA,
+                        ((lanes & CSIA) != 0 ? SELA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(CILB_MIPI_CAL_CONFIG), SELA,
+                        ((lanes & CSIB) != 0 ? SELA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(CILC_MIPI_CAL_CONFIG), SELA,
+                        ((lanes & CSIC) != 0 ? SELA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(CILD_MIPI_CAL_CONFIG), SELA,
+                        ((lanes & CSID) != 0 ? SELA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(CILE_MIPI_CAL_CONFIG), SELA,
+                        ((lanes & CSIE) != 0 ? SELA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(CILF_MIPI_CAL_CONFIG), SELA,
+                        ((lanes & CSIF) != 0 ? SELA : 0));
+
+        mipical_update_bits(mipi->regmap, ADDR(DSIA_MIPI_CAL_CONFIG), SELDSIA,
+                        ((lanes & DSIA) != 0 ? SELDSIA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(DSIB_MIPI_CAL_CONFIG), SELDSIA,
+                        ((lanes & DSIB) != 0 ? SELDSIA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(DSIC_MIPI_CAL_CONFIG), SELDSIA,
+                        ((lanes & DSIC) != 0 ? SELDSIA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(DSID_MIPI_CAL_CONFIG), SELDSIA,
+                        ((lanes & DSID) != 0 ? SELDSIA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(DSIA_MIPI_CAL_CONFIG_2),
+                        CLKSELDSIA, ((lanes & DSIA) != 0 ? CLKSELDSIA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(DSIB_MIPI_CAL_CONFIG_2),
+                        CLKSELDSIA, ((lanes & DSIB) != 0 ? CLKSELDSIA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(DSIC_MIPI_CAL_CONFIG_2),
+                        CLKSELDSIA, ((lanes & DSIC) != 0 ? CLKSELDSIA : 0));
+        mipical_update_bits(mipi->regmap, ADDR(DSID_MIPI_CAL_CONFIG_2),
+                        CLKSELDSIA, ((lanes & DSID) != 0 ? CLKSELDSIA : 0));
+}
+
+static void clear_all(struct tegra_mipi *mipi)
+{
+        mipical_write(mipi->regmap, ADDR(CILA_MIPI_CAL_CONFIG), 0);
+        mipical_write(mipi->regmap, ADDR(CILB_MIPI_CAL_CONFIG), 0);
+        mipical_write(mipi->regmap, ADDR(CILC_MIPI_CAL_CONFIG), 0);
+        mipical_write(mipi->regmap, ADDR(CILD_MIPI_CAL_CONFIG), 0);
+        mipical_write(mipi->regmap, ADDR(CILE_MIPI_CAL_CONFIG), 0);
+        mipical_write(mipi->regmap, ADDR(CILF_MIPI_CAL_CONFIG), 0);
+
+        mipical_write(mipi->regmap, ADDR(DSIA_MIPI_CAL_CONFIG), 0);
+        mipical_write(mipi->regmap, ADDR(DSIB_MIPI_CAL_CONFIG), 0);
+        mipical_write(mipi->regmap, ADDR(DSIC_MIPI_CAL_CONFIG), 0);
+        mipical_write(mipi->regmap, ADDR(DSID_MIPI_CAL_CONFIG), 0);
+        mipical_write(mipi->regmap, ADDR(DSIA_MIPI_CAL_CONFIG_2), 0);
+        mipical_write(mipi->regmap, ADDR(DSIB_MIPI_CAL_CONFIG_2), 0);
+        mipical_write(mipi->regmap, ADDR(DSIC_MIPI_CAL_CONFIG_2), 0);
+        mipical_write(mipi->regmap, ADDR(DSID_MIPI_CAL_CONFIG_2), 0);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static struct debugfs_reg32 mipical_regs[] = {
+        dump_register(MIPI_CAL_CTRL),
+        dump_register(MIPI_CAL_AUTOCAL_CTRL0),
+        dump_register(CIL_MIPI_CAL_STATUS),
+        dump_register(CIL_MIPI_CAL_STATUS_2),
+        dump_register(CILA_MIPI_CAL_CONFIG),
+        dump_register(CILB_MIPI_CAL_CONFIG),
+        dump_register(CILC_MIPI_CAL_CONFIG),
+        dump_register(CILD_MIPI_CAL_CONFIG),
+        dump_register(CILE_MIPI_CAL_CONFIG),
+        dump_register(CILF_MIPI_CAL_CONFIG),
+        dump_register(DSIA_MIPI_CAL_CONFIG),
+        dump_register(DSIB_MIPI_CAL_CONFIG),
+        dump_register(DSIC_MIPI_CAL_CONFIG),
+        dump_register(DSID_MIPI_CAL_CONFIG),
+        dump_register(MIPI_BIAS_PAD_CFG0),
+        dump_register(MIPI_BIAS_PAD_CFG1),
+        dump_register(MIPI_BIAS_PAD_CFG2),
+        dump_register(DSIA_MIPI_CAL_CONFIG_2),
+        dump_register(DSIB_MIPI_CAL_CONFIG_2),
+        dump_register(DSIC_MIPI_CAL_CONFIG_2),
+        dump_register(DSID_MIPI_CAL_CONFIG_2),
+};
+
+static u32 mipical_status;
+static u32 timeout_ct;
+static u32 counts;
+
+static int dbgfs_show_regs(struct seq_file *s, void *data)
+{
+        struct tegra_mipi *mipi = s->private;
+        int err;
+
+        err = tegra_unpowergate_partition(mipi->soc->powergate_id);
+        if (err) {
+                dev_err(mipi->dev, "Fail to unpowergate SOR\n");
+                return err;
+        }
+
+        err = tegra_mipi_clk_enable(mipi);
+        if (err) {
+                dev_err(mipi->dev, "Fail to enable mipi clk\n");
+                goto clk_err;
+        }
+        debugfs_print_regs32(s, mipical_regs, ARRAY_SIZE(mipical_regs),
+                                mipi->io, "");
+        tegra_mipi_clk_disable(mipi);
+        err = 0;
+
+clk_err:
+        tegra_powergate_partition(mipi->soc->powergate_id);
+        return err;
+}
+
+static int dbgfs_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, dbgfs_show_regs, inode->i_private);
+}
+
+static const struct file_operations dbgfs_ops = {
+        .open           = dbgfs_open,
+        .read           = seq_read,
+        .llseek         = seq_lseek,
+        .release        = single_release
+};
+
+static int dbgfs_mipi_init(struct tegra_mipi *mipi)
+{
+        struct dentry *dir;
+        struct dentry *val;
+        int i;
+
+        dir = debugfs_create_dir(DRV_NAME, NULL);
+        if (!dir)
+                return -ENOMEM;
+
+        val = debugfs_create_x32("LAST_STATUS", S_IRUGO, dir, &mipical_status);
+        if (!val)
+                goto err;
+        val = debugfs_create_u32("COUNT", S_IRUGO | S_IWUGO, dir, &counts);
+        if (!val)
+                goto err;
+        val = debugfs_create_u32("TIMEOUTS", S_IRUGO | S_IWUGO, dir,
+                                &timeout_ct);
+        if (!val)
+                goto err;
+
+        val = debugfs_create_file("regs", S_IRUGO, dir, mipi, &dbgfs_ops);
+        if (!val)
+                goto err;
+
+        /* Assign register address with adjusted offset */
+        for (i = 0; i < ARRAY_SIZE(mipical_regs); ++i)
+                mipical_regs[i].offset += mipi->soc->addr_offset;
+        return 0;
+err:
+        dev_err(mipi->dev, "%s:Fail to create debugfs\n", __func__);
+        debugfs_remove_recursive(dir);
+        return -ENODEV;
+}
+#endif
+static int _tegra_mipi_calibration(struct tegra_mipi *mipi, int lanes)
+{
+        int err;
+
+        mutex_lock(&mipi->lock);
+        /* clean up lanes */
+        clear_all(mipi);
+
+        /* Apply MIPI_CAL PROD_Set */
+        if (lanes & (CSIA|CSIB|CSIC|CSID|CSIE|CSIF))
+                tegra_mipi_apply_csi_prod(mipi->regmap, mipi->prod_csi,
+                                                lanes);
+        else
+                tegra_mipi_apply_dsi_prod(mipi->regmap, mipi->prod_dsi,
+                                                lanes);
+
+        /*Select lanes */
+        select_lanes(mipi, lanes);
+        /* Start calibration */
+        err = tegra_mipi_wait(mipi, lanes);
+
+#ifdef CONFIG_DEBUG_FS
+        regmap_read(mipi->regmap, ADDR(CIL_MIPI_CAL_STATUS), &mipical_status);
+        counts++;
+        if (err)
+                timeout_ct++;
+#endif
+        mutex_unlock(&mipi->lock);
+        return err;
+}
+static int tegra_mipical_using_prod(struct tegra_mipi *mipi, int lanes)
+{
+        int err = 0;
+
+        mutex_lock(&mipi->lock);
+
+        /* clean up lanes */
+        clear_all(mipi);
+
+        /* Apply MIPI_CAL PROD_Set */
+        if (lanes & (CSIA|CSIB|CSIC|CSID|CSIE|CSIF)) {
+                if (!IS_ERR(mipi->prod_gr_csi))
+                        err = tegra_prod_set_by_name(&mipi->io, "prod",
+                                        mipi->prod_gr_csi);
+        } else {
+                if (!IS_ERR(mipi->prod_gr_dsi))
+                        err = tegra_prod_set_by_name(&mipi->io,
+                                        "prod", mipi->prod_gr_dsi);
+        }
+        if (err) {
+                dev_err(mipi->dev, "tegra_prod set failed\n");
+                goto err_unlock;
+        }
+        /*Select lanes */
+        select_lanes(mipi, lanes);
+        /* Start calibration */
+        err = tegra_mipi_wait(mipi, lanes);
+
+#ifdef CONFIG_DEBUG_FS
+        regmap_read(mipi->regmap, ADDR(CIL_MIPI_CAL_STATUS), &mipical_status);
+        counts++;
+        if (err)
+                timeout_ct++;
+#endif
+err_unlock:
+        mutex_unlock(&mipi->lock);
+        return err;
+
+}
+int tegra_mipi_calibration(int lanes)
+{
+        if (!mipi)
+                return -ENODEV;
+        dev_dbg(mipi->dev, "%s", __func__);
+        if (mipi->soc->calibrate)
+                return mipi->soc->calibrate(mipi, lanes);
+        else
+                return 0;
+
+}
+EXPORT_SYMBOL(tegra_mipi_calibration);
+
+static void parse_bias_prod(struct device_node *np,
+                            struct tegra_mipi_bias *bias)
+{
+        int ret;
+        unsigned int v;
+
+        ret = of_property_read_u32(np, "bias-pad-cfg0", &v);
+        if (!ret) {
+                bias->pad_pdvclamp = (v & PDVCLAMP) >> PDVCLAMP_SHIFT;
+                bias->e_vclamp_ref = (v & E_VCLAMP_REF) >> E_VCLAMP_REF_SHIFT;
+        }
+
+        ret = of_property_read_u32(np, "bias-pad-cfg1", &v);
+        if (!ret) {
+                bias->pad_driv_up_ref =
+                                (v & PAD_DRIV_UP_REF) >> PAD_DRIV_UP_REF_SHIFT;
+                bias->pad_driv_dn_ref =
+                                (v & PAD_DRIV_DN_REF) >> PAD_DRIV_DN_REF_SHIFT;
+        }
+
+        ret = of_property_read_u32(np, "bias-pad-cfg2", &v);
+        if (!ret) {
+                bias->pad_vclamp_level =
+                        (v & PAD_VCLAMP_LEVEL) >> PAD_VCLAMP_LEVEL_SHIFT;
+                bias->pad_vauxp_level =
+                        (v & PAD_VAUXP_LEVEL) >> PAD_VAUXP_LEVEL_SHIFT;
+        }
+}
+
+static void parse_dsi_prod(struct device_node *np,
+                           struct tegra_mipi_prod_dsi *dsi)
+{
+        int ret;
+        unsigned int v;
+
+        if (!np)
+                return;
+        ret = of_property_read_u32(np, "dsix-cfg", &v);
+        if (!ret) {
+                dsi->overide_x = (v & OVERIDEDSIA) >> OVERIDEDSIA_SHIFT;
+                dsi->hspdos_x = (v & HSPDOSDSIA) >> HSPDOSDSIA_SHIFT;
+                dsi->hspuos_x = (v & HSPUOSDSIA) >> HSPUOSDSIA_SHIFT;
+                dsi->termos_x = (v & TERMOSDSIA) >> TERMOSDSIA_SHIFT;
+        }
+        ret = of_property_read_u32(np, "dsix-cfg2", &v);
+        if (!ret) {
+                dsi->clk_overide_x =
+                                (v & CLKOVERIDEDSIA) >> CLKOVERIDEDSIA_SHIFT;
+                dsi->clk_hspdos_x = (v & HSCLKPDOSDSIA) >> HSCLKPDOSDSIA_SHIFT;
+                dsi->clk_hspuos_x = (v & HSCLKPUOSDSIA) >> HSCLKPUOSDSIA_SHIFT;
+                dsi->clk_hstermos_x =
+                                (v & HSCLKTERMOSDSIA) >> HSCLKTERMOSDSIA_SHIFT;
+        }
+        parse_bias_prod(np, &dsi->bias_dsi);
+}
+static void parse_csi_prod(struct device_node *np,
+                           struct tegra_mipi_prod_csi *csi)
+{
+        int ret;
+        unsigned int v;
+
+        if (!np)
+                return;
+        ret = of_property_read_u32(np, "cilx-cfg", &v);
+        if (!ret) {
+                csi->overide_x = (v & OVERIDEA) >> OVERIDEA_SHIFT;
+                csi->termos_x = (v & TERMOSA) >> TERMOSA_SHIFT;
+                csi->termos_x_clk = (v & TERMOSA_CLK) >> TERMOSA_CLK_SHIFT;
+        }
+        parse_bias_prod(np, &csi->bias_csi);
+
+}
+static void parse_prod(struct device_node *np, struct tegra_mipi *mipi)
+{
+        struct device_node *next;
+
+        if (!np)
+                return;
+        next = of_get_child_by_name(np, "dsi");
+        parse_dsi_prod(next, mipi->prod_dsi);
+        next = of_get_child_by_name(np, "csi");
+        parse_csi_prod(next, mipi->prod_csi);
+}
+
+static void print_config(struct tegra_mipi *mipi)
+{
+        struct tegra_mipi_prod_csi *csi = mipi->prod_csi;
+        struct tegra_mipi_prod_dsi *dsi = mipi->prod_dsi;
+#define pr_val(x)       dev_dbg(mipi->dev, "%s:%x", #x, x)
+
+        pr_val(csi->overide_x);
+        pr_val(csi->termos_x);
+        pr_val(csi->termos_x_clk);
+        pr_val(dsi->overide_x);
+        pr_val(dsi->hspdos_x);
+        pr_val(dsi->hspuos_x);
+        pr_val(dsi->termos_x);
+        pr_val(dsi->clk_overide_x);
+        pr_val(dsi->clk_hspdos_x);
+        pr_val(dsi->clk_hspuos_x);
+        pr_val(dsi->clk_hstermos_x);
+#undef pr_val
+}
+
+static int tegra_prod_get_config(struct platform_device *pdev,
+                                struct tegra_mipi *mipi)
+{
+        struct device_node *np;
+
+        if (mipi->prod_gr_dsi != NULL && mipi->prod_gr_csi != NULL)
+                return 0;
+
+        np = of_find_node_by_name(NULL, "mipical");
+        if (!np) {
+                pr_err("%s: Can not find dsi prod node\n", __func__);
+        } else {
+                mipi->prod_gr_dsi =
+                                devm_tegra_prod_get_from_node(mipi->dev, np);
+                if (IS_ERR(mipi->prod_gr_dsi))
+                        dev_err(mipi->dev, "Fail to get DSI PROD settings\n");
+        }
+
+        np = of_find_node_by_name(NULL, "csi_mipical");
+        if (!np) {
+                pr_err("%s: Can not find csi_mipical node\n", __func__);
+        } else {
+                mipi->prod_gr_csi =
+                                devm_tegra_prod_get_from_node(mipi->dev, np);
+                if (IS_ERR(mipi->prod_gr_csi))
+                        dev_err(mipi->dev, "Fail to get CSI PROD settings\n");
+        }
+
+        return 0;
+}
+
+static int tegra_mipi_parse_config(struct platform_device *pdev,
+                struct tegra_mipi *mipi)
+{
+        struct device_node *np = pdev->dev.of_node;
+        struct device_node *next;
+
+        if (!np)
+                return -ENODEV;
+
+        if (of_device_is_compatible(np, "nvidia,tegra210-mipical"))
+                parse_prod(np, mipi);
+
+        if (of_device_is_compatible(np, "nvidia, tegra186-mipical")) {
+                if (tegra_chip_get_revision() == TEGRA186_REVISION_A01) {
+                        dev_dbg(mipi->dev, "T186-A01\n");
+                        next = of_get_child_by_name(np, "a01");
+                        parse_prod(next, mipi);
+                } else {
+                        dev_dbg(mipi->dev, "T186-A02\n");
+                        next = of_get_child_by_name(np, "a02");
+                        parse_prod(next, mipi);
+                }
+        }
+        print_config(mipi);
+        return 0;
+}
+static const struct tegra_mipi_soc tegra21x_mipi_soc = {
+        .total_dsilanes = 4,
+        .total_cillanes = 6,
+        .addr_offset = 0,
+        .ppsb_war = 1,
+        .pad_enable = &_t21x_tegra_mipi_bias_pad_enable,
+        .pad_disable = &_t21x_tegra_mipi_bias_pad_disable,
+        .calibrate = &_tegra_mipi_calibration,
+        .parse_cfg = &tegra_mipi_parse_config,
+        .powergate_id = TEGRA210_POWER_DOMAIN_SOR,
+};
+
+static const struct tegra_mipi_soc tegra18x_mipi_soc = {
+        .total_dsilanes = 4,
+        .total_cillanes = 6,
+        .addr_offset = 4,
+        .ppsb_war = 0,
+        .pad_enable = &_t18x_tegra_mipi_bias_pad_enable,
+        .pad_disable = &_t18x_tegra_mipi_bias_pad_disable,
+        .calibrate = &tegra_mipical_using_prod,
+        .parse_cfg = &tegra_prod_get_config,
+        .powergate_id = TEGRA186_POWER_DOMAIN_DISP,
+};
+
+static const struct tegra_mipi_soc tegra_vmipi_soc = {
+        .pad_enable = &tegra_vmipi_bias_pad_enable,
+        .pad_disable = &tegra_vmipi_bias_pad_disable,
+        .calibrate = &tegra_vmipi_calibration,
+        .virtual_dev = 1,
+};
+
+static const struct of_device_id tegra_mipi_of_match[] = {
+        {
+                .compatible = "nvidia,tegra210-mipical",
+                .data = &tegra21x_mipi_soc,
+        }, {
+                .compatible = "nvidia, tegra186-mipical",
+                .data = &tegra18x_mipi_soc,
+        }, {
+                .compatible = "nvidia, tegra186-mipical-shared-multi-os",
+                .data = &tegra18x_mipi_soc,
+        }, {
+                .compatible = "nvidia,tegra-mipical-hv",
+                .data = &tegra_vmipi_soc,
+        }, {
+        }
+};
+MODULE_DEVICE_TABLE(of, tegra_mipi_of_match);
+
+static int tegra_mipi_open(struct inode *inode, struct file *file)
+{
+        return 0;
+}
+static int tegra_mipi_release(struct inode *inode, struct file *file)
+{
+        return 0;
+}
+static long mipi_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+        struct tegra_mipi *mipi = container_of(file->private_data,
+                                        struct tegra_mipi, misc_dev);
+        if (_IOC_TYPE(cmd) != TEGRA_MIPI_IOCTL_MAGIC)
+                return -EINVAL;
+
+        switch (_IOC_NR(cmd)) {
+        case _IOC_NR(TEGRA_MIPI_IOCTL_BIAS_PAD_CTRL): {
+                unsigned int enable = 0;
+                int err;
+
+                if (copy_from_user(&enable, (const void __user *)arg,
+                                        sizeof(unsigned int))) {
+                        dev_err(mipi->dev, "Fail to get user data\n");
+                        return -EFAULT;
+                }
+                if (enable)
+                        err = tegra_mipi_bias_pad_enable();
+                else
+                        err = tegra_mipi_bias_pad_disable();
+                return  err;
+        }
+        case _IOC_NR(TEGRA_MIPI_IOCTL_CAL): {
+                int lanes = 0;
+
+                if (copy_from_user(&lanes, (const void __user *)arg,
+                                        sizeof(int))) {
+                        dev_err(mipi->dev, "Fail to get user data\n");
+                        return -EFAULT;
+                }
+                if (lanes)
+                        return tegra_mipi_calibration(lanes);
+
+                dev_err(mipi->dev, "Selected lane %x, skip mipical\n", lanes);
+                return 0;
+        }
+        default:
+                dev_err(mipi->dev, "Unknown ioctl\n");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static const struct file_operations tegra_mipi_miscdev_fops = {
+        .owner = THIS_MODULE,
+        .open = tegra_mipi_open,
+        .release = tegra_mipi_release,
+        .unlocked_ioctl = mipi_ioctl,
+#ifdef CONFIG_COMPAT
+        .compat_ioctl = mipi_ioctl,
+#endif
+};
+
+static int tegra_mipi_misc_register(struct tegra_mipi *mipi)
+{
+        int err;
+
+        if (!mipi)
+                return -EINVAL;
+
+        mipi->misc_dev.minor = MISC_DYNAMIC_MINOR;
+        mipi->misc_dev.name = DRV_NAME;
+        mipi->misc_dev.fops = &tegra_mipi_miscdev_fops;
+        err = misc_register(&mipi->misc_dev);
+        if (err)
+                dev_err(mipi->dev, "Fail to register misc dev\n");
+
+        return err;
+}
+
+int tegra_vmipi_probe(struct platform_device *pdev)
+{
+        int err;
+
+        err = tegra_vmipi_init(pdev);
+        if (err) {
+                dev_err(&pdev->dev, "Mipi cal virtual dev probe failed\n");
+                return err;
+        }
+
+        err = tegra_mipi_misc_register(mipi);
+        if (err)
+                tegra_vmipi_deinit();
+
+        return err;
+}
+
+static int tegra_mipi_probe(struct platform_device *pdev)
+{
+        const struct of_device_id *match;
+        struct resource *mem, *memregion;
+        void __iomem *regs;
+        int err = 0;
+        struct device_node *np;
+        const struct tegra_mipi_soc *cdata = NULL;
+
+        np = pdev->dev.of_node;
+        match = of_match_device(tegra_mipi_of_match, &pdev->dev);
+        if (!match) {
+                dev_err(&pdev->dev, "No device match found\n");
+                return -ENODEV;
+        }
+        cdata = match->data;
+        mipi = devm_kzalloc(&pdev->dev, sizeof(*mipi), GFP_KERNEL);
+        if (!mipi)
+                return -ENOMEM;
+
+        mipi->dev = &pdev->dev;
+        mipi->soc = cdata;
+        platform_set_drvdata(pdev, mipi);
+
+        if (mipi->soc->virtual_dev)
+                return tegra_vmipi_probe(pdev);
+
+        mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!mem) {
+                dev_err(&pdev->dev, "No memory resource\n");
+                return -EINVAL;
+        }
+        memregion = devm_request_mem_region(&pdev->dev, mem->start,
+                        resource_size(mem), pdev->name);
+        if (!memregion) {
+                dev_err(&pdev->dev, "Cannot request mem region\n");
+                return -EBUSY;
+        }
+        regs = devm_ioremap_nocache(&pdev->dev, mem->start, resource_size(mem));
+        if (!regs) {
+                dev_err(&pdev->dev, "ioremap failed\n");
+                return -ENOMEM;
+        }
+
+        mipi->io = regs;
+        mipi->regmap = devm_regmap_init_mmio(&pdev->dev, regs,
+                        &t210_mipi_cal_regmap_config);
+        if (IS_ERR(mipi->regmap)) {
+                dev_err(&pdev->dev, "Fai to initialize regmap\n");
+                return PTR_ERR(mipi->regmap);
+        }
+
+        mipi->mipi_cal_clk = devm_clk_get(&pdev->dev, "mipi_cal");
+        if (IS_ERR(mipi->mipi_cal_clk))
+                return PTR_ERR(mipi->mipi_cal_clk);
+
+        if (of_device_is_compatible(np, "nvidia,tegra210-mipical")) {
+                mipi->mipi_cal_fixed = devm_clk_get(&pdev->dev,
+                                "uart_mipi_cal");
+                if (IS_ERR(mipi->mipi_cal_fixed))
+                        return PTR_ERR(mipi->mipi_cal_fixed);
+                mipi->prod_csi = devm_kzalloc(&pdev->dev,
+                                sizeof(*mipi->prod_csi), GFP_KERNEL);
+                mipi->prod_dsi = devm_kzalloc(&pdev->dev,
+                                sizeof(*mipi->prod_dsi), GFP_KERNEL);
+        } else if (of_device_is_compatible(np, "nvidia, tegra186-mipical")) {
+                mipi->mipi_cal_fixed = devm_clk_get(&pdev->dev,
+                                "uart_fs_mipi_cal");
+                if (IS_ERR(mipi->mipi_cal_fixed))
+                        return PTR_ERR(mipi->mipi_cal_fixed);
+                mipi->rst = devm_reset_control_get(mipi->dev, "mipi_cal");
+                reset_control_deassert(mipi->rst);
+                /* Bug 200224083 requires both register fields set to 1
+                 * after de-asserted
+                 */
+                tegra_mipi_clk_enable(mipi);
+                mipical_update_bits(mipi->regmap, ADDR(MIPI_BIAS_PAD_CFG0),
+                                PDVCLAMP, 1 << PDVCLAMP_SHIFT);
+                mipical_update_bits(mipi->regmap, ADDR(MIPI_BIAS_PAD_CFG2),
+                                PDVREG, 1 << PDVREG_SHIFT);
+                tegra_mipi_clk_disable(mipi);
+        } else if (of_device_is_compatible(np, "nvidia, tegra186-mipical-shared-multi-os")) {
+                mipi->mipi_cal_fixed = devm_clk_get(&pdev->dev,
+                                "uart_fs_mipi_cal");
+                if (IS_ERR(mipi->mipi_cal_fixed))
+                        return PTR_ERR(mipi->mipi_cal_fixed);
+                mipi->rst = devm_reset_control_get(mipi->dev, "mipi_cal");
+                reset_control_deassert(mipi->rst);
+        }
+
+        if (mipi->soc->parse_cfg)
+                err = mipi->soc->parse_cfg(pdev, mipi);
+        if (err)
+                return err;
+
+        mutex_init(&mipi->lock);
+        atomic_set(&mipi->refcount, 0);
+
+        err = tegra_mipi_misc_register(mipi);
+        if (err)
+                return err;
+
+#ifdef CONFIG_DEBUG_FS
+        err = dbgfs_mipi_init(mipi);
+        if (err)
+                dev_err(&pdev->dev, "Fail to create debugfs\n");
+#endif
+
+        dev_dbg(&pdev->dev, "Mipi cal done probing...\n");
+        return err;
+}
+
+static struct platform_driver tegra_mipi_cal_platform_driver = {
+        .driver = {
+                .name = DRV_NAME,
+                .owner = THIS_MODULE,
+                .of_match_table = tegra_mipi_of_match,
+        },
+        .probe = tegra_mipi_probe,
+};
+
+static int __init tegra_mipi_module_init(void)
+{
+        return platform_driver_register(&tegra_mipi_cal_platform_driver);
+}
+
+static void __exit tegra_mipi_module_exit(void)
+{
+        platform_driver_unregister(&tegra_mipi_cal_platform_driver);
+}
+subsys_initcall(tegra_mipi_module_init);
+module_exit(tegra_mipi_module_exit);
+
+MODULE_AUTHOR("Wenjia Zhou <wenjiaz@nvidia.com>");
+MODULE_DESCRIPTION("Common MIPI calibration driver for CSI and DSI");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/media/platform/tegra/mipical/mipi_cal.h b/drivers/media/platform/tegra/mipical/mipi_cal.h
new file mode 100644
index 000000000..0fd5171fb
--- /dev/null
+++ b/drivers/media/platform/tegra/mipical/mipi_cal.h
@@ -0,0 +1,50 @@
+/*
+ * mipi_cal.h
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef MIPI_CAL_H
+#define MIPI_CAL_H
+
+#define DSID    (1 << 31)
+#define DSIC    (1 << 30)
+#define DSIB    (1 << 29)
+#define DSIA    (1 << 28)
+#define CSIF    (1 << 25)
+#define CSIE    (1 << 24)
+#define CSID    (1 << 23)
+#define CSIC    (1 << 22)
+#define CSIB    (1 << 21)
+#define CSIA    (1 << 20)
+#define MIPI_CPHY       1
+
+#ifdef CONFIG_TEGRA_MIPI_CAL
+int tegra_mipi_bias_pad_enable(void);
+int tegra_mipi_bias_pad_disable(void);
+int tegra_mipi_calibration(int lanes);
+#else
+static inline int tegra_mipi_bias_pad_enable(void)
+{
+        return 0;
+}
+static inline int tegra_mipi_bias_pad_disable(void)
+{
+        return 0;
+}
+static inline int tegra_mipi_calibration(int lanes)
+{
+        return 0;
+}
+#endif
+#endif
diff --git a/drivers/media/platform/tegra/mipical/registers.h b/drivers/media/platform/tegra/mipical/registers.h
new file mode 100644
index 000000000..5f209790b
--- /dev/null
+++ b/drivers/media/platform/tegra/mipical/registers.h
@@ -0,0 +1,99 @@
+ /*
+ * registers.h
+ *
+ * Copyright (c) 2016-2017, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/* New register on T186 that starts
+ * at offset 0
+ */
+#define MIPI_CAL_MODE           0x00
+
+/* T210 register offset*/
+#define MIPI_CAL_CTRL           0x00
+#define         NOISE_FLT       (0xf << 26)
+#define         PRESCALE        (0x3 << 24)
+#define         CLKEN_OVR       (1 << 4)
+#define         AUTOCAL_EN      (1 << 1)
+#define         STARTCAL        (1 << 0)
+#define MIPI_CAL_AUTOCAL_CTRL0  0x04
+#define CIL_MIPI_CAL_STATUS     0x08
+#define         CAL_DONE_DSID   (1 << 31)
+#define         CAL_DONE_DSIC   (1 << 30)
+#define         CAL_DONE_DSIB   (1 << 29)
+#define         CAL_DONE_DSIA   (1 << 28)
+#define         CAL_DONE_CSIF   (1 << 25)
+#define         CAL_DONE_CSIE   (1 << 24)
+#define         CAL_DONE_CSID   (1 << 23)
+#define         CAL_DONE_CSIC   (1 << 22)
+#define         CAL_DONE_CSIB   (1 << 21)
+#define         CAL_DONE_CSIA   (1 << 20)
+#define         CAL_DONE        (1 << 16)
+#define         CAL_ACTIVE      (1 << 0)
+#define CIL_MIPI_CAL_STATUS_2   0x0c
+#define CILA_MIPI_CAL_CONFIG    0x14
+#define         OVERIDEA        (1 << 30)
+#define         OVERIDEA_SHIFT  30
+#define         SELA            (1 << 21)
+#define         TERMOSA_CLK     (0x1f << 11)
+#define         TERMOSA_CLK_SHIFT       11
+#define         TERMOSA         0x1f
+#define         TERMOSA_SHIFT   0
+#define CILB_MIPI_CAL_CONFIG    0x18
+#define CILC_MIPI_CAL_CONFIG    0x1c
+#define CILD_MIPI_CAL_CONFIG    0x20
+#define CILE_MIPI_CAL_CONFIG    0x24
+#define CILF_MIPI_CAL_CONFIG    0x28
+#define DSIA_MIPI_CAL_CONFIG    0x38
+#define         OVERIDEDSIA     (1 << 30)
+#define         OVERIDEDSIA_SHIFT 30
+#define         SELDSIA         (1 << 21)
+#define         HSPDOSDSIA      (0x1f << 16)
+#define         HSPDOSDSIA_SHIFT 16
+#define         HSPUOSDSIA      (0x1f << 8)
+#define         HSPUOSDSIA_SHIFT 8
+#define         TERMOSDSIA      0x1f
+#define         TERMOSDSIA_SHIFT 0
+#define DSIB_MIPI_CAL_CONFIG    0x3c
+#define DSIC_MIPI_CAL_CONFIG    0x40
+#define DSID_MIPI_CAL_CONFIG    0x44
+#define MIPI_BIAS_PAD_CFG0      0x58
+#define         E_VCLAMP_REF    (1 << 0)
+#define         E_VCLAMP_REF_SHIFT      0
+#define         PDVCLAMP        (1 << 1)
+#define         PDVCLAMP_SHIFT  1
+#define MIPI_BIAS_PAD_CFG1      0x5c
+#define         PAD_DRIV_UP_REF (0x7 << 8)
+#define         PAD_DRIV_UP_REF_SHIFT 8
+#define         PAD_DRIV_DN_REF (0x7 << 16)
+#define         PAD_DRIV_DN_REF_SHIFT 16
+#define MIPI_BIAS_PAD_CFG2      0x60
+#define         PDVREG          (1 << 1)
+#define         PDVREG_SHIFT    1
+#define         PAD_VAUXP_LEVEL (0x7 << 4)
+#define         PAD_VAUXP_LEVEL_SHIFT 4
+#define         PAD_VCLAMP_LEVEL (0x7 << 16)
+#define         PAD_VCLAMP_LEVEL_SHIFT 16
+#define DSIA_MIPI_CAL_CONFIG_2  0x64
+#define         CLKOVERIDEDSIA  (1 << 30)
+#define         CLKOVERIDEDSIA_SHIFT 30
+#define         CLKSELDSIA      (1 << 21)
+#define         HSCLKTERMOSDSIA (0x1f << 16)
+#define         HSCLKTERMOSDSIA_SHIFT   16
+#define         HSCLKPDOSDSIA   (0x1f << 8)
+#define         HSCLKPDOSDSIA_SHIFT 8
+#define         HSCLKPUOSDSIA   0x1f
+#define         HSCLKPUOSDSIA_SHIFT 0
+#define DSIB_MIPI_CAL_CONFIG_2  0x68
+#define DSIC_MIPI_CAL_CONFIG_2  0x70
+#define DSID_MIPI_CAL_CONFIG_2  0x74
diff --git a/drivers/media/platform/tegra/mipical/vmipi/vmipi.c b/drivers/media/platform/tegra/mipical/vmipi/vmipi.c
new file mode 100644
index 000000000..ea441c53e
--- /dev/null
+++ b/drivers/media/platform/tegra/mipical/vmipi/vmipi.c
@@ -0,0 +1,246 @@
+/*
+ * vmipi.c
+ *
+ * Copyright (c) 2017, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/device.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/semaphore.h>
+#include <linux/wait.h>
+#include <linux/tegra-ivc.h>
+
+#include "mipi_cal.h"
+#include "vmipi.h"
+
+const char *tegra_vmipi_cmd[] = {
+        "TEGRA_VMIPI_CMD_CALIBRATE",
+        "TEGRA_VMIPI_CMD_BIAS_PAD_ENABLE",
+        "TEGRA_VMIPI_CMD_BIAS_PAD_DISABLE",
+};
+
+struct tegra_vmipi_ivc_context {
+        struct device *dev;
+        struct tegra_hv_ivc_cookie *cookie;
+        wait_queue_head_t tx_wq;
+        wait_queue_head_t rx_wq;
+        struct mutex lock;
+};
+static struct tegra_vmipi_ivc_context *ctx;
+
+static int tegra_vmipi_ivc_send(struct tegra_vmipi_cmd_msg *msg, int size)
+{
+        int ret = 0;
+
+        if (!tegra_hv_ivc_can_write(ctx->cookie)) {
+                ret = wait_event_timeout(ctx->tx_wq,
+                        tegra_hv_ivc_can_write(ctx->cookie),
+                        msecs_to_jiffies(500));
+                if (!ret) {
+                        dev_err(ctx->dev,
+                                "%s timeout waiting for tx buffer for cmd %s\n",
+                                __func__, tegra_vmipi_cmd[msg->cmd]);
+                        return -ETIMEDOUT;
+                }
+        }
+
+        ret = tegra_hv_ivc_write(ctx->cookie, msg, size);
+        if (ret != size) {
+                dev_err(ctx->dev,
+                        "%s: ivc send incorrect msg size %d for cmd %s\n",
+                        __func__, ret, tegra_vmipi_cmd[msg->cmd]);
+                return -EIO;
+        }
+
+        return 0;
+}
+
+static int tegra_vmipi_ivc_rcv(struct tegra_vmipi_cmd_msg *msg, int size)
+{
+        int ret = 0, len;
+
+        if (!tegra_hv_ivc_can_read(ctx->cookie)) {
+                ret = wait_event_timeout(ctx->rx_wq,
+                        tegra_hv_ivc_can_read(ctx->cookie),
+                        msecs_to_jiffies(500));
+                if (!ret)  {
+                        dev_err(ctx->dev,
+                                "%s timeout waiting for rx buffer for cmd %s\n",
+                                __func__, tegra_vmipi_cmd[msg->cmd]);
+                        ret = -ETIMEDOUT;
+                        goto out;
+                }
+                ret = 0;
+        }
+
+        len = tegra_hv_ivc_read(ctx->cookie, msg, size);
+        if (len != size) {
+                dev_err(ctx->dev,
+                        "%s: ivc read incorrect msg size %d for cmd %s\n",
+                        __func__, len, tegra_vmipi_cmd[msg->cmd]);
+                ret = -EIO;
+        }
+
+out:
+        if (tegra_hv_ivc_can_write(ctx->cookie))
+                wake_up(&ctx->tx_wq);
+
+        return ret ? ret : msg->ret;
+}
+
+static irqreturn_t tegra_vmipi_ivc_isr(int irq, void *data)
+{
+        struct tegra_vmipi_ivc_context *ictx =
+                (struct tegra_vmipi_ivc_context *)data;
+
+        if (tegra_hv_ivc_channel_notified(ictx->cookie))
+                return IRQ_HANDLED;
+
+        wake_up(&ictx->rx_wq);
+        return IRQ_HANDLED;
+}
+
+static int tegra_vmipi_ivc_send_rcv(struct tegra_vmipi_cmd_msg *msg, int size)
+{
+        int err;
+
+        if (!msg || !size)
+                return -EINVAL;
+
+        mutex_lock(&ctx->lock);
+
+        err = tegra_vmipi_ivc_send(msg, size);
+        if (err)
+                goto out;
+
+        err = tegra_vmipi_ivc_rcv(msg, size);
+
+out:
+        mutex_unlock(&ctx->lock);
+
+        return err;
+}
+
+int tegra_vmipi_calibration(struct tegra_mipi *mipi, int lanes)
+{
+        struct tegra_vmipi_cmd_msg msg;
+        int size = sizeof(msg);
+        struct tegra_vmipi_calibrate_params *p;
+
+        if (!ctx)
+                return -EINVAL;
+
+        msg.cmd = TEGRA_VMIPI_CMD_CALIBRATE;
+        p = &msg.params.calibrate;
+        p->lanes = lanes;
+        return tegra_vmipi_ivc_send_rcv(&msg, size);
+}
+
+int tegra_vmipi_bias_pad_enable(struct tegra_mipi *mipi)
+{
+        struct tegra_vmipi_cmd_msg msg;
+        int size = sizeof(msg);
+
+        if (!ctx)
+                return -EINVAL;
+
+        msg.cmd = TEGRA_VMIPI_CMD_BIAS_PAD_ENABLE;
+        return tegra_vmipi_ivc_send_rcv(&msg, size);
+}
+
+int tegra_vmipi_bias_pad_disable(struct tegra_mipi *mipi)
+{
+        struct tegra_vmipi_cmd_msg msg;
+        int size = sizeof(msg);
+
+        if (!ctx)
+                return -EINVAL;
+
+        msg.cmd = TEGRA_VMIPI_CMD_BIAS_PAD_DISABLE;
+        return tegra_vmipi_ivc_send_rcv(&msg, size);
+}
+
+int tegra_vmipi_init(struct platform_device *pdev)
+{
+        static struct tegra_vmipi_ivc_context *context;
+        struct device_node *np, *ivcq_np;
+        struct tegra_hv_ivc_cookie *cookie;
+        int err, ivcq;
+
+        if (!pdev)
+                return -ENODEV;
+
+        context = devm_kzalloc(&pdev->dev, sizeof(*context), GFP_KERNEL);
+        if (!context)
+                return -ENOMEM;
+
+        context->dev = &pdev->dev;
+
+        np = pdev->dev.of_node;
+        ivcq_np = of_parse_phandle(np, "ivc_queue", 0);
+        if (!ivcq_np) {
+                dev_err(&pdev->dev, "Fail to find vmipi ivc queue\n");
+                return -ENOMEM;
+        }
+
+        err = of_property_read_u32_index(np, "ivc_queue", 1, &ivcq);
+        if (err) {
+                dev_err(&pdev->dev, "Fail to read vmipi ivc queue value\n");
+                of_node_put(ivcq_np);
+                return -EINVAL;
+        }
+        of_node_put(ivcq_np);
+
+        cookie = tegra_hv_ivc_reserve(ivcq_np, ivcq, NULL);
+        if (IS_ERR_OR_NULL(cookie)) {
+                if (cookie == ERR_PTR(-EPROBE_DEFER))
+                        return -EPROBE_DEFER;
+
+                dev_err(&pdev->dev,
+                        "Fail to reserve vmipi ivc queue %d\n", ivcq);
+                return PTR_ERR(cookie);
+        }
+        context->cookie = cookie;
+
+        init_waitqueue_head(&context->tx_wq);
+        init_waitqueue_head(&context->rx_wq);
+
+        mutex_init(&context->lock);
+
+        tegra_hv_ivc_channel_reset(context->cookie);
+
+        err = devm_request_irq(&pdev->dev, cookie->irq, tegra_vmipi_ivc_isr, 0,
+                "mipi-virt", context);
+        if (err) {
+                dev_err(&pdev->dev, "Fail to request vmipi ivc irq %d\n",
+                        cookie->irq);
+                goto fail;
+        }
+
+        ctx = context;
+        return 0;
+
+fail:
+        tegra_hv_ivc_unreserve(context->cookie);
+        return err;
+}
+
+void tegra_vmipi_deinit(void)
+{
+        if (ctx)
+                tegra_hv_ivc_unreserve(ctx->cookie);
+}
diff --git a/drivers/media/platform/tegra/mipical/vmipi/vmipi.h b/drivers/media/platform/tegra/mipical/vmipi/vmipi.h
new file mode 100644
index 000000000..694f1ac21
--- /dev/null
+++ b/drivers/media/platform/tegra/mipical/vmipi/vmipi.h
@@ -0,0 +1,66 @@
+/*
+ * vmipi.h
+ *
+ * Copyright (c) 2017, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef VMIPI_H
+#define VMIPI_H
+
+struct tegra_mipi;
+
+enum {
+        TEGRA_VMIPI_CMD_CALIBRATE = 0,
+        TEGRA_VMIPI_CMD_BIAS_PAD_ENABLE = 1,
+        TEGRA_VMIPI_CMD_BIAS_PAD_DISABLE = 2,
+};
+
+struct tegra_vmipi_calibrate_params {
+        u32 lanes;
+};
+
+struct tegra_vmipi_cmd_msg {
+        u32 cmd;
+        int ret;
+        union {
+                struct tegra_vmipi_calibrate_params calibrate;
+        } params;
+};
+
+#ifdef CONFIG_TEGRA_MIPI_CAL
+int tegra_vmipi_init(struct platform_device *pdev);
+void tegra_vmipi_deinit(void);
+int tegra_vmipi_bias_pad_enable(struct tegra_mipi *mipi);
+int tegra_vmipi_bias_pad_disable(struct tegra_mipi *mipi);
+int tegra_vmipi_calibration(struct tegra_mipi *mipi, int lanes);
+#else
+static inline int tegra_vmipi_init(struct platform_device *pdev)
+{
+        return 0;
+}
+static inline void tegra_vmipi_init(void) {}
+static inline int tegra_vmipi_bias_pad_enable(struct tegra_mipi *mipi)
+{
+        return 0;
+}
+static inline int tegra_vmipi_bias_pad_disable(struct tegra_mipi *mipi)
+{
+        return 0;
+}
+static inline int tegra_vmipi_calibration(struct tegra_mipi *mipi,
+        int lanes)
+{
+        return 0;
+}
+#endif
+#endif
diff --git a/drivers/media/platform/tegra/regmap_util.c b/drivers/media/platform/tegra/regmap_util.c
new file mode 100644
index 000000000..c21dcac78
--- /dev/null
+++ b/drivers/media/platform/tegra/regmap_util.c
@@ -0,0 +1,173 @@
+/*
+ * regmap_util.c - utilities for writing regmap tables
+ *
+ * Copyright (c) 2013-2016, NVIDIA Corporation. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/regmap.h>
+#include <media/camera_common.h>
+
+int
+regmap_util_write_table_8(struct regmap *regmap,
+                          const struct reg_8 table[],
+                          const struct reg_8 override_list[],
+                          int num_override_regs, u16 wait_ms_addr, u16 end_addr)
+{
+        int err;
+        const struct reg_8 *next;
+        int i;
+        u8 val;
+
+        int range_start = -1;
+        int range_count = 0;
+        /* bug 200048392 -
+         * the vi i2c cannot take a FIFO buffer bigger than 16 bytes
+         */
+        u8 range_vals[16];
+        int max_range_vals = ARRAY_SIZE(range_vals);
+
+        for (next = table;; next++) {
+                /* If we have a range open and */
+                /* either the address doesn't match */
+                /* or the temporary storage is full, flush */
+                if  ((next->addr != range_start + range_count) ||
+                     (next->addr == end_addr) ||
+                     (next->addr == wait_ms_addr) ||
+                     (range_count == max_range_vals)) {
+
+                        if (range_count == 1) {
+                                err =
+                                    regmap_write(regmap, range_start,
+                                                 range_vals[0]);
+                        } else if (range_count > 1) {
+                                err =
+                                    regmap_bulk_write(regmap, range_start,
+                                                      &range_vals[0],
+                                                      range_count);
+                        }
+
+                        if (err) {
+                                pr_err("%s:regmap_util_write_table:%d",
+                                       __func__, err);
+                                return err;
+                        }
+
+                        range_start = -1;
+                        range_count = 0;
+
+                        /* Handle special address values */
+                        if (next->addr == end_addr)
+                                break;
+
+                        if (next->addr == wait_ms_addr) {
+                                msleep_range(next->val);
+                                continue;
+                        }
+                }
+
+                val = next->val;
+
+                /* When an override list is passed in, replace the reg */
+                /* value to write if the reg is in the list            */
+                if (override_list) {
+                        for (i = 0; i < num_override_regs; i++) {
+                                if (next->addr == override_list[i].addr) {
+                                        val = override_list[i].val;
+                                        break;
+                                }
+                        }
+                }
+
+                if (range_start == -1)
+                        range_start = next->addr;
+
+                range_vals[range_count++] = val;
+        }
+        return 0;
+}
+
+int
+regmap_util_write_table_16_as_8(struct regmap *regmap,
+                                const struct reg_16 table[],
+                                const struct reg_16 override_list[],
+                                int num_override_regs,
+                                u16 wait_ms_addr, u16 end_addr)
+{
+        int err;
+        const struct reg_16 *next;
+        int i;
+        u16 val;
+
+        int range_start = -1;
+        int range_count = 0;
+        u8 range_vals[256];
+        int max_range_vals = ARRAY_SIZE(range_vals);
+
+        for (next = table;; next++) {
+                /* If we have a range open and */
+                /* either the address doesn't match */
+                /* or the temporary storage is full, flush*/
+                if  ((next->addr != range_start + range_count) ||
+                     (next->addr == end_addr) ||
+                     (next->addr == wait_ms_addr) ||
+                     (range_count == max_range_vals)) {
+
+                        if (range_count > 1) {
+                                err =
+                                    regmap_bulk_write(regmap, range_start,
+                                                      &range_vals[0],
+                                                      range_count);
+                        }
+
+                        if (err) {
+                                pr_err("%s:regmap_util_write_table:%d",
+                                       __func__, err);
+                                return err;
+                        }
+
+                        range_start = -1;
+                        range_count = 0;
+
+                        /* Handle special address values */
+                        if (next->addr == end_addr)
+                                break;
+
+                        if (next->addr == wait_ms_addr) {
+                                msleep_range(next->val);
+                                continue;
+                        }
+                }
+
+                val = next->val;
+
+                /* When an override list is passed in, replace the reg */
+                /* value to write if the reg is in the list            */
+                if (override_list) {
+                        for (i = 0; i < num_override_regs; i++) {
+                                if (next->addr == override_list[i].addr) {
+                                        val = override_list[i].val;
+                                        break;
+                                }
+                        }
+                }
+
+                if (range_start == -1)
+                        range_start = next->addr;
+
+                range_vals[range_count++] = (u8) (val >> 8);
+                range_vals[range_count++] = (u8) (val & 0xFF);
+        }
+        return 0;
+}
diff --git a/drivers/media/platform/tegra/tpg/Makefile b/drivers/media/platform/tegra/tpg/Makefile
new file mode 100644
index 000000000..135473166
--- /dev/null
+++ b/drivers/media/platform/tegra/tpg/Makefile
@@ -0,0 +1,14 @@
+GCOV_PROFILE := y
+ccflags-y += -I../nvhost/drivers/video/tegra/host
+ccflags-y += -Idrivers/media/platform/tegra
+ccflags-y += -Werror
+
+ifeq ($(CONFIG_ARCH_TEGRA_18x_SOC),y)
+ccflags-y += -I../t18x/drivers/video/tegra/host
+nvhost-vi-tpg-objs += tpg_t18x.o
+else
+nvhost-vi-tpg-objs += tpg_t21x.o
+endif
+
+
+obj-$(CONFIG_VIDEO_TEGRA_VI_TPG) += nvhost-vi-tpg.o
diff --git a/drivers/media/platform/tegra/tpg/tpg_t18x.c b/drivers/media/platform/tegra/tpg/tpg_t18x.c
new file mode 100644
index 000000000..539d9b4de
--- /dev/null
+++ b/drivers/media/platform/tegra/tpg/tpg_t18x.c
@@ -0,0 +1,199 @@
+/*
+ * drivers/video/tegra/host/tpg/tpg.c
+ *
+ * Tegra VI test pattern generator driver
+ *
+ * Copyright (c) 2016, NVIDIA CORPORATION. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/init.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/debugfs.h>
+
+#include "camera/vi/mc_common.h"
+#include "camera/csi/csi.h"
+#include "vi/vi4.h"
+#include "nvcsi/nvcsi.h"
+#include "host1x/host1x.h"
+
+/* PG generate 1 pixel per nvcsi_clk:
+ * ((width + hblank) * height + vblank) * fps = nvcsi_clk_freq
+ *
+ */
+const struct tpg_frmfmt tegra18x_csi_tpg_frmfmt[] = {
+        {{1280, 720}, V4L2_PIX_FMT_SRGGB10, 120, 750, 0xffff},
+        {{1920, 1080}, V4L2_PIX_FMT_SRGGB10, 60, 930, 0xffff},
+        {{3840, 2160}, V4L2_PIX_FMT_SRGGB10, 20, 900, 0xffff},
+        {{1280, 720}, V4L2_PIX_FMT_RGB32, 60, 3351, 0xffff},
+        {{1920, 1080}, V4L2_PIX_FMT_RGB32, 30, 4315, 0xffff},
+        {{3840, 2160}, V4L2_PIX_FMT_RGB32, 20, 851, 0xffff},
+};
+
+#define TPG_PORT_IDX    0
+
+static int tpg_debugfs_height_show(void *data, u64 *val)
+{
+        struct tegra_csi_channel *chan = data;
+        struct tegra_csi_port *port = &chan->ports[TPG_PORT_IDX];
+
+        mutex_lock(&chan->format_lock);
+        *val = port->format.height;
+        mutex_unlock(&chan->format_lock);
+
+        return 0;
+}
+
+static int tpg_debugfs_height_write(void *data, u64 val)
+{
+        struct tegra_csi_channel *chan = data;
+        struct tegra_csi_device *csi = chan->csi;
+        struct tegra_csi_port *port = &chan->ports[TPG_PORT_IDX];
+
+        mutex_lock(&chan->format_lock);
+        port->format.height = val;
+        mutex_unlock(&chan->format_lock);
+
+        if (csi->fops->csi_override_format)
+                csi->fops->csi_override_format(chan, TPG_PORT_IDX);
+
+        return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(tpg_debugfs_height_fops,
+                        tpg_debugfs_height_show,
+                        tpg_debugfs_height_write,
+                        "%lld\n");
+
+static int tpg_debugfs_width_show(void *data, u64 *val)
+{
+        struct tegra_csi_channel *chan = data;
+        struct tegra_csi_port *port = &chan->ports[TPG_PORT_IDX];
+
+        mutex_lock(&chan->format_lock);
+        *val = port->format.width;
+        mutex_unlock(&chan->format_lock);
+
+        return 0;
+}
+
+static int tpg_debugfs_width_write(void *data, u64 val)
+{
+        struct tegra_csi_channel *chan = data;
+        struct tegra_csi_device *csi = chan->csi;
+        struct tegra_csi_port *port = &chan->ports[TPG_PORT_IDX];
+
+        mutex_lock(&chan->format_lock);
+        port->format.width = val;
+        mutex_unlock(&chan->format_lock);
+
+        if (csi->fops->csi_override_format)
+                csi->fops->csi_override_format(chan, TPG_PORT_IDX);
+
+        return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(tpg_debugfs_width_fops,
+                        tpg_debugfs_width_show,
+                        tpg_debugfs_width_write,
+                        "%lld\n");
+
+
+static void tpg_remove_debugfs(struct tegra_csi_device *csi)
+{
+        debugfs_remove_recursive(csi->debugdir);
+        csi->debugdir = NULL;
+}
+
+static int tpg_create_debugfs(struct tegra_csi_device *csi)
+{
+        struct dentry *dir;
+        struct tegra_csi_channel *chan;
+
+        csi->debugdir = dir = debugfs_create_dir("tpg", NULL);
+        if (dir == NULL)
+                return -ENOMEM;
+
+        list_for_each_entry(chan, &csi->csi_chans, list) {
+                const struct tegra_channel *vi_chan =
+                                v4l2_get_subdev_hostdata(&chan->subdev);
+                if (vi_chan->pg_mode) {
+                        const char *name = vi_chan->video.name;
+
+                        dev_dbg(csi->dev, "debugfs node installed %s\n", name);
+                        dir = debugfs_create_dir(name, csi->debugdir);
+                        if (!dir)
+                                goto error;
+
+                        if (!debugfs_create_file("height", S_IRUGO | S_IWUSR,
+                                                dir, chan,
+                                                &tpg_debugfs_height_fops))
+                                goto error;
+                        if (!debugfs_create_file("width", S_IRUGO | S_IWUSR,
+                                                dir, chan,
+                                                &tpg_debugfs_width_fops))
+                                goto error;
+                }
+        }
+
+        return 0;
+error:
+        tpg_remove_debugfs(csi);
+        return -ENOMEM;
+}
+
+static int __init tpg_probe_t18x(void)
+{
+        struct tegra_csi_device *mc_csi = tegra_get_mc_csi();
+        struct tegra_mc_vi *mc_vi = tegra_get_mc_vi();
+        int err;
+
+        dev_info(mc_csi->dev, "%s\n", __func__);
+        mc_vi->csi = mc_csi;
+        /* Init CSI related media controller interface */
+        mc_csi->tpg_frmfmt_table = tegra18x_csi_tpg_frmfmt;
+        mc_csi->tpg_frmfmt_table_size = ARRAY_SIZE(tegra18x_csi_tpg_frmfmt);
+        err = tpg_csi_media_controller_init(mc_csi, TEGRA_VI_PG_PATCH);
+        if (err)
+                return -EINVAL;
+        err = tpg_vi_media_controller_init(mc_vi, TEGRA_VI_PG_PATCH);
+        if (err)
+                goto vi_init_err;
+
+        err = tpg_create_debugfs(mc_csi);
+        if (err)
+                goto debugfs_init_err;
+
+        return err;
+debugfs_init_err:
+        tpg_remove_debugfs(mc_csi);
+vi_init_err:
+        tpg_csi_media_controller_cleanup(mc_csi);
+        dev_err(mc_csi->dev, "%s error\n", __func__);
+        return err;
+}
+static void __exit tpg_remove_t18x(void)
+{
+        struct tegra_csi_device *mc_csi = tegra_get_mc_csi();
+        struct tegra_mc_vi *mc_vi = tegra_get_mc_vi();
+
+        dev_info(mc_csi->dev, "%s\n", __func__);
+        tpg_remove_debugfs(mc_csi);
+        tpg_csi_media_controller_cleanup(mc_csi);
+        tpg_vi_media_controller_cleanup(mc_vi);
+}
+
+module_init(tpg_probe_t18x);
+module_exit(tpg_remove_t18x);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/tegra/tpg/tpg_t21x.c b/drivers/media/platform/tegra/tpg/tpg_t21x.c
new file mode 100644
index 000000000..6291d0209
--- /dev/null
+++ b/drivers/media/platform/tegra/tpg/tpg_t21x.c
@@ -0,0 +1,210 @@
+/*
+ * drivers/video/tegra/host/tpg/tpg.c
+ *
+ * Tegra VI test pattern generator driver
+ *
+ * Copyright (c) 2016, NVIDIA CORPORATION. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/init.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/debugfs.h>
+
+#include "camera/vi/mc_common.h"
+#include "camera/csi/csi.h"
+#include "vi/vi.h"
+#include "host1x/host1x.h"
+
+/*
+ * total_cycles_per_lane for one second = pll_d freq / 8
+ * width_in_bytes = ((width * bpp) / 8)
+ * cycles_per_line = width_in_bytes + hblank
+ * cycles_per_image = (cycles_per_line * height) + vblank
+ * image_cycles_per_lane = cycles_per_image / numlanes
+ * framerate = total_cycles_per_lane / image_cycles_per_lane
+ * As per IAS maximum overhead of ~15% can occur
+ * hblank and vblank are tuned to consider overhead during capture
+ * e.g. for 1920x1080, RAW 10 and two lane TPG
+ * cycles_per_lane = (((((1920 * 10)/8) + 512) * 1080) + 8) / 2 ~ 1572480
+ * framerate = ((927000000 / 8) / 1572480) ~ 73fps
+ * Max overhead of 15% results in minimum of 62fps (max can be 73fps)
+ * Note: with changing resolution, bpp and hblank overhead % varies.
+ */
+
+static struct tpg_frmfmt tegra21x_csi_tpg_frmfmt[] = {
+        {{1280, 720}, V4L2_PIX_FMT_SRGGB10, 120, 512, 8},
+        {{1920, 1080}, V4L2_PIX_FMT_SRGGB10, 60, 512, 8},
+        {{3840, 2160}, V4L2_PIX_FMT_SRGGB10, 20, 8, 8},
+        {{1280, 720}, V4L2_PIX_FMT_RGB32, 60, 512, 8},
+        {{1920, 1080}, V4L2_PIX_FMT_RGB32, 30, 512, 8},
+        {{3840, 2160}, V4L2_PIX_FMT_RGB32, 8, 8, 8},
+};
+
+#define TPG_PORT_IDX    0
+
+static int tpg_debugfs_height_show(void *data, u64 *val)
+{
+        struct tegra_csi_channel *chan = data;
+        struct tegra_csi_port *port = &chan->ports[TPG_PORT_IDX];
+
+        mutex_lock(&chan->format_lock);
+        *val = port->format.height;
+        mutex_unlock(&chan->format_lock);
+
+        return 0;
+}
+
+static int tpg_debugfs_height_write(void *data, u64 val)
+{
+        struct tegra_csi_channel *chan = data;
+        struct tegra_csi_device *csi = chan->csi;
+        struct tegra_csi_port *port = &chan->ports[TPG_PORT_IDX];
+
+        mutex_lock(&chan->format_lock);
+        port->format.height = val;
+        mutex_unlock(&chan->format_lock);
+
+        if (csi->fops->csi_override_format)
+                csi->fops->csi_override_format(chan, TPG_PORT_IDX);
+
+        return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(tpg_debugfs_height_fops,
+                        tpg_debugfs_height_show,
+                        tpg_debugfs_height_write,
+                        "%lld\n");
+
+static int tpg_debugfs_width_show(void *data, u64 *val)
+{
+        struct tegra_csi_channel *chan = data;
+        struct tegra_csi_port *port = &chan->ports[TPG_PORT_IDX];
+
+        mutex_lock(&chan->format_lock);
+        *val = port->format.width;
+        mutex_unlock(&chan->format_lock);
+
+        return 0;
+}
+
+static int tpg_debugfs_width_write(void *data, u64 val)
+{
+        struct tegra_csi_channel *chan = data;
+        struct tegra_csi_device *csi = chan->csi;
+        struct tegra_csi_port *port = &chan->ports[TPG_PORT_IDX];
+
+        mutex_lock(&chan->format_lock);
+        port->format.width = val;
+        mutex_unlock(&chan->format_lock);
+
+        if (csi->fops->csi_override_format)
+                csi->fops->csi_override_format(chan, TPG_PORT_IDX);
+
+        return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(tpg_debugfs_width_fops,
+                        tpg_debugfs_width_show,
+                        tpg_debugfs_width_write,
+                        "%lld\n");
+
+
+static void tpg_remove_debugfs(struct tegra_csi_device *csi)
+{
+        debugfs_remove_recursive(csi->debugdir);
+        csi->debugdir = NULL;
+}
+
+static int tpg_create_debugfs(struct tegra_csi_device *csi)
+{
+        struct dentry *dir;
+        struct tegra_csi_channel *chan;
+
+        csi->debugdir = dir = debugfs_create_dir("tpg", NULL);
+        if (dir == NULL)
+                return -ENOMEM;
+
+        list_for_each_entry(chan, &csi->csi_chans, list) {
+                const struct tegra_channel *vi_chan =
+                                v4l2_get_subdev_hostdata(&chan->subdev);
+                if (vi_chan->pg_mode) {
+                        const char *name = vi_chan->video.name;
+
+                        dev_dbg(csi->dev, "debugfs node installed %s\n", name);
+                        dir = debugfs_create_dir(name, csi->debugdir);
+                        if (!dir)
+                                goto error;
+
+                        if (!debugfs_create_file("height", S_IRUGO | S_IWUSR,
+                                                dir, chan,
+                                                &tpg_debugfs_height_fops))
+                                goto error;
+                        if (!debugfs_create_file("width", S_IRUGO | S_IWUSR,
+                                                dir, chan,
+                                                &tpg_debugfs_width_fops))
+                                goto error;
+                }
+        }
+
+        return 0;
+error:
+        tpg_remove_debugfs(csi);
+        return -ENOMEM;
+}
+
+static int __init tpg_probe_t21x(void)
+{
+        struct tegra_csi_device *mc_csi = &(tegra_vi_get()->csi);
+        struct tegra_mc_vi *mc_vi = &(tegra_vi_get()->mc_vi);
+        int err;
+
+        dev_info(mc_csi->dev, "%s\n", __func__);
+        mc_vi->csi = mc_csi;
+        /* Init CSI related media controller interface */
+        mc_csi->tpg_frmfmt_table = tegra21x_csi_tpg_frmfmt;
+        mc_csi->tpg_frmfmt_table_size = ARRAY_SIZE(tegra21x_csi_tpg_frmfmt);
+        err = tpg_csi_media_controller_init(mc_csi, TEGRA_VI_PG_PATCH);
+        if (err)
+                return -EINVAL;
+        err = tpg_vi_media_controller_init(mc_vi, TEGRA_VI_PG_PATCH);
+        if (err)
+                goto vi_init_err;
+
+        err = tpg_create_debugfs(mc_csi);
+        if (err)
+                goto debugfs_init_err;
+
+        return err;
+debugfs_init_err:
+        tpg_remove_debugfs(mc_csi);
+vi_init_err:
+        tpg_csi_media_controller_cleanup(mc_csi);
+        dev_err(mc_csi->dev, "%s error\n", __func__);
+        return err;
+}
+static void  __exit tpg_remove_t21x(void)
+{
+        struct tegra_csi_device *mc_csi = &(tegra_vi_get()->csi);
+        struct tegra_mc_vi *mc_vi = &(tegra_vi_get()->mc_vi);
+
+        dev_info(mc_csi->dev, "%s\n", __func__);
+        tpg_remove_debugfs(mc_csi);
+        tpg_csi_media_controller_cleanup(mc_csi);
+        tpg_vi_media_controller_cleanup(mc_vi);
+}
+
+module_init(tpg_probe_t21x);
+module_exit(tpg_remove_t21x);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/tegra/vi/Makefile b/drivers/media/platform/tegra/vi/Makefile
new file mode 100644
index 000000000..6cd9767ea
--- /dev/null
+++ b/drivers/media/platform/tegra/vi/Makefile
@@ -0,0 +1,17 @@
+GCOV_PROFILE := y
+ccflags-y += -I../nvhost/drivers/video/tegra/host
+ccflags-y += -Idrivers/video/tegra/camera
+ccflags-y += -Idrivers/media/platform/tegra/
+ccflags-y += -Werror
+
+ifeq ($(CONFIG_ARCH_TEGRA_18x_SOC),y)
+ccflags-y += -I../t18x/drivers/video/tegra/host/
+endif
+
+ifneq (,$(filter $(CONFIG_VIDEO_TEGRA_VI),y m))
+obj-y += vi_irq.o
+obj-y += tegra_vi.o
+endif
+
+nvhost-vi-objs += vi.o
+obj-$(CONFIG_VIDEO_TEGRA_VI) += nvhost-vi.o
diff --git a/drivers/media/platform/tegra/vi/tegra_vi.c b/drivers/media/platform/tegra/vi/tegra_vi.c
new file mode 100644
index 000000000..4d2e3b714
--- /dev/null
+++ b/drivers/media/platform/tegra/vi/tegra_vi.c
@@ -0,0 +1,515 @@
+/*
+ * Copyright (c) 2013-2016, NVIDIA CORPORATION. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/nvhost.h>
+#include <linux/nvhost_vi_ioctl.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/uaccess.h>
+#include <linux/clk.h>
+
+#include <linux/platform/tegra/latency_allowance.h>
+
+#include "bus_client.h"
+#include "nvhost_acm.h"
+#include "chip_support.h"
+#include "host1x/host1x.h"
+#include "vi/vi.h"
+#include "vi/vi_irq.h"
+
+#define T12_VI_CFG_CG_CTRL      0xb8
+#define T12_CG_2ND_LEVEL_EN     1
+#define T12_VI_CSI_0_SW_RESET   0x100
+#define T12_CSI_CSI_SW_SENSOR_A_RESET 0x858
+#define T12_CSI_CSICIL_SW_SENSOR_A_RESET 0x94c
+#define T12_VI_CSI_0_CSI_IMAGE_DT 0x120
+
+#define T12_VI_CSI_1_SW_RESET   0x200
+#define T12_CSI_CSI_SW_SENSOR_B_RESET 0x88c
+#define T12_CSI_CSICIL_SW_SENSOR_B_RESET 0x980
+#define T12_VI_CSI_1_CSI_IMAGE_DT 0x220
+
+#define T21_CSI_CILA_PAD_CONFIG0 0x92c
+#define T21_CSI1_CILA_PAD_CONFIG0 0x112c
+#define T21_CSI2_CILA_PAD_CONFIG0 0x192c
+
+#define VI_MAX_BPP 2
+
+int nvhost_vi_finalize_poweron(struct platform_device *dev)
+{
+        struct vi *tegra_vi = nvhost_get_private_data(dev);
+        int ret = 0;
+
+        if (!tegra_vi)
+                return -EINVAL;
+
+        if (tegra_vi->reg) {
+                ret = regulator_enable(tegra_vi->reg);
+                if (ret) {
+                        dev_err(&dev->dev,
+                                        "%s: enable csi regulator failed.\n",
+                                        __func__);
+                        return ret;
+                }
+        }
+
+#ifdef CONFIG_ARCH_TEGRA_12x_SOC
+        /* Only do this for vi.0 not for slave device vi.1 */
+        if (dev->id == 0)
+                host1x_writel(dev, T12_VI_CFG_CG_CTRL, T12_CG_2ND_LEVEL_EN);
+#endif
+
+        ret = vi_enable_irq(tegra_vi);
+        if (ret)
+                dev_err(&tegra_vi->ndev->dev, "%s: vi_enable_irq failed\n",
+                        __func__);
+
+        return ret;
+}
+
+int nvhost_vi_prepare_poweroff(struct platform_device *dev)
+{
+        int ret = 0;
+        struct vi *tegra_vi = nvhost_get_private_data(dev);
+
+        if (!tegra_vi)
+                return -EINVAL;
+
+        ret = vi_disable_irq(tegra_vi);
+        if (ret) {
+                dev_err(&tegra_vi->ndev->dev, "%s: vi_disable_irq failed\n",
+                        __func__);
+                return ret;
+        }
+
+        if (tegra_vi->reg) {
+                ret = regulator_disable(tegra_vi->reg);
+                if (ret)
+                        dev_err(&dev->dev,
+                                "%s: disable csi regulator failed.\n",
+                                __func__);
+        }
+
+        return ret;
+}
+
+#if defined(CONFIG_TEGRA_ISOMGR)
+static int vi_isomgr_register(struct vi *tegra_vi)
+{
+        int iso_client_id = TEGRA_ISO_CLIENT_VI_0;
+        struct clk *vi_clk;
+        struct nvhost_device_data *pdata =
+                                platform_get_drvdata(tegra_vi->ndev);
+
+        dev_dbg(&tegra_vi->ndev->dev, "%s++\n", __func__);
+
+        if (WARN_ONCE(pdata == NULL, "pdata not found, %s failed\n", __func__))
+                return -ENODEV;
+
+        /* Get max VI BW */
+        vi_clk = pdata->clk[0];
+        tegra_vi->max_bw =
+                        (clk_round_rate(vi_clk, UINT_MAX) / 1000) * VI_MAX_BPP;
+
+        /* Register with max possible BW in VI usecases.*/
+        tegra_vi->isomgr_handle = tegra_isomgr_register(iso_client_id,
+                                        tegra_vi->max_bw,
+                                        NULL,   /* tegra_isomgr_renegotiate */
+                                        NULL);  /* *priv */
+
+        if (!tegra_vi->isomgr_handle) {
+                dev_err(&tegra_vi->ndev->dev, "%s: unable to register isomgr\n",
+                                        __func__);
+                return -ENOMEM;
+        }
+
+        return 0;
+}
+
+static int vi_set_isomgr_request(struct vi *tegra_vi, uint vi_bw, uint lt)
+{
+        int ret = 0;
+
+        dev_dbg(&tegra_vi->ndev->dev,
+                "%s++ bw=%u, lt=%u\n", __func__, vi_bw, lt);
+
+        /* return value of tegra_isomgr_reserve is dvfs latency in usec */
+        ret = tegra_isomgr_reserve(tegra_vi->isomgr_handle,
+                                vi_bw,  /* KB/sec */
+                                lt);    /* usec */
+        if (!ret) {
+                dev_err(&tegra_vi->ndev->dev,
+                "%s: failed to reserve %u KBps\n", __func__, vi_bw);
+                return -ENOMEM;
+        }
+
+        /* return value of tegra_isomgr_realize is dvfs latency in usec */
+        ret = tegra_isomgr_realize(tegra_vi->isomgr_handle);
+        if (ret)
+                dev_dbg(&tegra_vi->ndev->dev,
+                "%s: tegra_vi isomgr latency is %d usec",
+                __func__, ret);
+        else {
+                dev_err(&tegra_vi->ndev->dev,
+                "%s: failed to realize %u KBps\n", __func__, vi_bw);
+                        return -ENOMEM;
+        }
+        return 0;
+}
+
+static int vi_isomgr_release(struct vi *tegra_vi)
+{
+        int ret = 0;
+
+        dev_dbg(&tegra_vi->ndev->dev, "%s++\n", __func__);
+
+        /* deallocate isomgr bw */
+        ret = vi_set_isomgr_request(tegra_vi, 0, 0);
+        if (ret) {
+                dev_err(&tegra_vi->ndev->dev,
+                "%s: failed to deallocate memory in isomgr\n",
+                __func__);
+                return -ENOMEM;
+        }
+
+        return 0;
+}
+#endif
+
+static int vi_set_la(u32 vi_bw)
+{
+        int ret = 0;
+
+#ifdef CONFIG_TEGRA_MC
+        ret = tegra_set_camera_ptsa(TEGRA_LA_VI_W, vi_bw, 1);
+
+        if (!ret) {
+                ret = tegra_set_latency_allowance(TEGRA_LA_VI_W,
+                        vi_bw);
+
+                if (ret)
+                        pr_err("%s: set latency failed: %d\n",
+                                __func__, ret);
+        } else {
+                pr_err("%s: set ptsa failed: %d\n", __func__, ret);
+        }
+#endif
+
+        return ret;
+}
+
+int vi_v4l2_set_la(struct vi *tegra_vi, u32 vi_bypass_bw, bool is_ioctl)
+{
+        int ret = 0;
+        unsigned long total_bw;
+
+        mutex_lock(&tegra_vi->update_la_lock);
+        if (is_ioctl)
+                tegra_vi->vi_bypass_bw = vi_bypass_bw;
+        total_bw = tegra_vi->mc_vi.aggregated_kbyteps + tegra_vi->vi_bypass_bw;
+        ret = vi_set_la(total_bw);
+        mutex_unlock(&tegra_vi->update_la_lock);
+        return ret;
+}
+EXPORT_SYMBOL(vi_v4l2_set_la);
+
+static long vi_ioctl(struct file *file,
+                unsigned int cmd, unsigned long arg)
+{
+        struct vi *tegra_vi = file->private_data;
+
+        if (_IOC_TYPE(cmd) != NVHOST_VI_IOCTL_MAGIC)
+                return -EFAULT;
+
+        switch (_IOC_NR(cmd)) {
+        case _IOC_NR(NVHOST_VI_IOCTL_ENABLE_TPG): {
+                uint enable;
+                int ret;
+                struct clk *clk;
+
+                if (copy_from_user(&enable,
+                        (const void __user *)arg, sizeof(uint))) {
+                        dev_err(&tegra_vi->ndev->dev,
+                                "%s: Failed to copy arg from user\n", __func__);
+                        return -EFAULT;
+                }
+
+                clk = clk_get(NULL, "pll_d");
+                if (IS_ERR(clk))
+                        return -EINVAL;
+
+                if (enable)
+                        ret = tegra_clk_cfg_ex(clk,
+                                TEGRA_CLK_PLLD_CSI_OUT_ENB, 1);
+                else
+                        ret = tegra_clk_cfg_ex(clk,
+                                TEGRA_CLK_MIPI_CSI_OUT_ENB, 1);
+                clk_put(clk);
+
+                return ret;
+        }
+        case _IOC_NR(NVHOST_VI_IOCTL_GET_VI_CLK): {
+                int ret;
+                u64 vi_clk_rate = 0;
+
+                ret = nvhost_module_get_rate(tegra_vi->ndev,
+                        (unsigned long *)&vi_clk_rate, 0);
+                if (ret) {
+                        dev_err(&tegra_vi->ndev->dev,
+                        "%s: failed to get vi clk\n",
+                        __func__);
+                        return ret;
+                }
+
+                if (copy_to_user((void __user *)arg,
+                        &vi_clk_rate, sizeof(vi_clk_rate))) {
+                        dev_err(&tegra_vi->ndev->dev,
+                        "%s:Failed to copy vi clk rate to user\n",
+                        __func__);
+                        return -EFAULT;
+                }
+
+                return 0;
+        }
+        case _IOC_NR(NVHOST_VI_IOCTL_SET_VI_LA_BW): {
+                u32 ret = 0;
+                u32 vi_la_bw;
+
+                if (copy_from_user(&vi_la_bw,
+                        (const void __user *)arg,
+                                sizeof(vi_la_bw))) {
+                        dev_err(&tegra_vi->ndev->dev,
+                                "%s: Failed to copy arg from user\n", __func__);
+                        return -EFAULT;
+                        }
+
+                /* Set latency allowance for VI, BW is in MBps */
+                ret = vi_v4l2_set_la(tegra_vi, vi_la_bw, 1);
+                if (ret) {
+                        dev_err(&tegra_vi->ndev->dev,
+                        "%s: failed to set la vi_bw %u MBps\n",
+                        __func__, vi_la_bw);
+                        return -ENOMEM;
+                }
+
+                return 0;
+        }
+        case _IOC_NR(NVHOST_VI_IOCTL_SET_EMC_INFO): {
+                uint vi_bw;
+                int ret;
+
+                if (copy_from_user(&vi_bw,
+                        (const void __user *)arg, sizeof(uint))) {
+                        dev_err(&tegra_vi->ndev->dev,
+                                "%s: Failed to copy arg from user\n", __func__);
+                        return -EFAULT;
+                }
+
+                ret = vi_set_la(vi_bw/1000);
+                if (ret) {
+                        dev_err(&tegra_vi->ndev->dev,
+                        "%s: failed to set la for vi_bw %u MBps\n",
+                        __func__, vi_bw/1000);
+                        return -ENOMEM;
+                }
+
+#if defined(CONFIG_TEGRA_ISOMGR)
+                /*
+                 * Register VI as isomgr client.
+                 */
+                if (!tegra_vi->isomgr_handle) {
+                        ret = vi_isomgr_register(tegra_vi);
+                        if (ret) {
+                                dev_err(&tegra_vi->ndev->dev,
+                                "%s: failed to register VI as isomgr client\n",
+                                __func__);
+                                return -ENOMEM;
+                        }
+                }
+
+                if (vi_bw > tegra_vi->max_bw) {
+                        dev_err(&tegra_vi->ndev->dev,
+                        "%s: Requested ISO BW %u is more than "
+                        "VI's max BW %u possible\n",
+                        __func__, vi_bw, tegra_vi->max_bw);
+                        return -EINVAL;
+                }
+
+                /*
+                 * Set VI ISO BW requirements.
+                 * There is no way to figure out what latency
+                 * can be tolerated in VI without reading VI
+                 * registers for now. 3 usec is minimum time
+                 * to switch PLL source. Let's put 4 usec as
+                 * latency for now.
+                 */
+                ret = vi_set_isomgr_request(tegra_vi, vi_bw, 4);
+                if (ret) {
+                        dev_err(&tegra_vi->ndev->dev,
+                        "%s: failed to reserve %u KBps\n",
+                        __func__, vi_bw);
+                        return -ENOMEM;
+                }
+#endif
+                return ret;
+        }
+        case _IOC_NR(NVHOST_VI_IOCTL_SET_VI_CLK): {
+                long vi_clk_rate = 0;
+
+                if (copy_from_user(&vi_clk_rate,
+                        (const void __user *)arg, sizeof(long))) {
+                        dev_err(&tegra_vi->ndev->dev,
+                                "%s: Failed to copy arg from user\n", __func__);
+                        return -EFAULT;
+                }
+
+                return nvhost_module_set_rate(tegra_vi->ndev,
+                                tegra_vi, vi_clk_rate, 0, NVHOST_CLOCK);
+        }
+        default:
+                dev_err(&tegra_vi->ndev->dev,
+                        "%s: Unknown vi ioctl.\n", __func__);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int vi_open(struct inode *inode, struct file *file)
+{
+        struct nvhost_device_data *pdata;
+        struct vi *vi;
+        int err = 0;
+
+        pdata = container_of(inode->i_cdev,
+                struct nvhost_device_data, ctrl_cdev);
+        if (WARN_ONCE(pdata == NULL, "pdata not found, %s failed\n", __func__))
+                return -ENODEV;
+
+        vi = (struct vi *)pdata->private_data;
+        if (WARN_ONCE(vi == NULL, "vi not found, %s failed\n", __func__))
+                return -ENODEV;
+
+        file->private_data = vi;
+
+        /* add vi client to acm */
+        if (nvhost_module_add_client(vi->ndev, vi)) {
+                dev_err(&vi->ndev->dev,
+                        "%s: failed add vi client\n",
+                        __func__);
+                return -ENOMEM;
+        }
+
+        return err;
+}
+
+static int vi_release(struct inode *inode, struct file *file)
+{
+        int ret = 0;
+        struct vi *tegra_vi = file->private_data;
+
+#if defined(CONFIG_TEGRA_ISOMGR)
+        /* nullify isomgr request */
+        if (tegra_vi->isomgr_handle) {
+                ret = vi_isomgr_release(tegra_vi);
+                if (ret) {
+                        dev_err(&tegra_vi->ndev->dev,
+                        "%s: failed to deallocate memory in isomgr\n",
+                        __func__);
+                        return -ENOMEM;
+                }
+        }
+#endif
+
+        /* remove vi client from acm */
+        nvhost_module_remove_client(tegra_vi->ndev, tegra_vi);
+
+        return ret;
+}
+
+const struct file_operations tegra_vi_ctrl_ops = {
+        .owner = THIS_MODULE,
+        .open = vi_open,
+        .unlocked_ioctl = vi_ioctl,
+#ifdef CONFIG_COMPAT
+        .compat_ioctl = vi_ioctl,
+#endif
+        .release = vi_release,
+};
+
+/* Reset sensor data if respective clk is ON */
+void nvhost_vi_reset_all(struct platform_device *pdev)
+{
+        void __iomem *reset_reg[4];
+        int err;
+        bool enabled = false;
+        struct nvhost_device_data *pdata = pdev->dev.platform_data;
+        struct clk *clk;
+
+        err = nvhost_clk_get(pdev, "cilab", &clk);
+        if (!err) {
+                reset_reg[0] = pdata->aperture[0] +
+                        T12_VI_CSI_0_SW_RESET;
+                reset_reg[1] = pdata->aperture[0] +
+                        T12_CSI_CSI_SW_SENSOR_A_RESET;
+                reset_reg[2] = pdata->aperture[0] +
+                        T12_CSI_CSICIL_SW_SENSOR_A_RESET;
+                reset_reg[3] = pdata->aperture[0] +
+                        T12_VI_CSI_0_CSI_IMAGE_DT;
+
+                writel(0, reset_reg[3]);
+                writel(0x1, reset_reg[2]);
+                writel(0x1, reset_reg[1]);
+                writel(0x1f, reset_reg[0]);
+
+                udelay(10);
+
+                writel(0, reset_reg[2]);
+                writel(0, reset_reg[1]);
+        }
+
+        err = nvhost_clk_get(pdev, "cilcd", &clk);
+        if (!err)
+                enabled = true;
+
+        err = nvhost_clk_get(pdev, "cile", &clk);
+        if (!err)
+                enabled = true;
+
+        if (enabled) {
+                reset_reg[0] = pdata->aperture[0] +
+                        T12_VI_CSI_1_SW_RESET;
+                reset_reg[1] = pdata->aperture[0] +
+                        T12_CSI_CSI_SW_SENSOR_B_RESET;
+                reset_reg[2] = pdata->aperture[0] +
+                        T12_CSI_CSICIL_SW_SENSOR_B_RESET;
+                reset_reg[3] = pdata->aperture[0] +
+                        T12_VI_CSI_1_CSI_IMAGE_DT;
+
+                writel(0, reset_reg[3]);
+                writel(0x1, reset_reg[2]);
+                writel(0x1, reset_reg[1]);
+                writel(0x1f, reset_reg[0]);
+
+                udelay(10);
+
+                writel(0, reset_reg[2]);
+                writel(0, reset_reg[1]);
+        }
+}
diff --git a/drivers/media/platform/tegra/vi/vi.c b/drivers/media/platform/tegra/vi/vi.c
new file mode 100644
index 000000000..61f47fd40
--- /dev/null
+++ b/drivers/media/platform/tegra/vi/vi.c
@@ -0,0 +1,611 @@
+/*
+ * drivers/video/tegra/host/vi/vi.c
+ *
+ * Tegra Graphics Host VI
+ *
+ * Copyright (c) 2012-2017, NVIDIA CORPORATION. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/init.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/resource.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+#include <linux/clk/tegra.h>
+#include <linux/tegra-soc.h>
+#include <linux/tegra_pm_domains.h>
+#include <linux/debugfs.h>
+#include <linux/slab.h>
+
+#include <media/tegra_v4l2_camera.h>
+
+#include "dev.h"
+#include "bus_client.h"
+#include "nvhost_acm.h"
+#include "t210/t210.h"
+#include "vi/vi.h"
+#include "vi/vi_irq.h"
+#include "camera/vi/vi2_fops.h"
+#include "camera/csi/csi2_fops.h"
+
+#include "tegra_camera_dev_mfi.h"
+
+#define MAX_DEVID_LENGTH        16
+#define TEGRA_VI_NAME           "tegra_vi"
+
+struct vi *tegra_vi;
+
+struct vi *tegra_vi_get(void)
+{
+        return tegra_vi;
+}
+EXPORT_SYMBOL(tegra_vi_get);
+
+static struct tegra_t210_vi_data t21_vi_data = {
+        .info = (struct nvhost_device_data *)&t21_vi_info,
+        .vi_fops = &vi2_fops,
+        .csi_fops = &csi2_fops,
+};
+
+static struct of_device_id tegra_vi_of_match[] = {
+        { .compatible = "nvidia,tegra210-vi",
+                .data = (struct tegra_t210_vi_data *)&t21_vi_data },
+        { },
+};
+
+static struct i2c_camera_ctrl *i2c_ctrl;
+
+static void (*mfi_callback)(void *);
+static struct mfi_cb_arg *mfi_callback_arg;
+static DEFINE_MUTEX(vi_isr_lock);
+
+int tegra_vi_register_mfi_cb(callback cb, void *cb_arg)
+{
+        mutex_lock(&vi_isr_lock);
+        if (mfi_callback || mfi_callback_arg) {
+                pr_err("cb already registered\n");
+                mutex_unlock(&vi_isr_lock);
+                return -1;
+        }
+
+        mfi_callback = cb;
+        mfi_callback_arg = (struct  mfi_cb_arg *)cb_arg;
+        mutex_unlock(&vi_isr_lock);
+
+        return 0;
+}
+EXPORT_SYMBOL(tegra_vi_register_mfi_cb);
+
+int tegra_vi_unregister_mfi_cb(void)
+{
+        mutex_lock(&vi_isr_lock);
+        mfi_callback = NULL;
+        mfi_callback_arg = NULL;
+        mutex_unlock(&vi_isr_lock);
+
+        return 0;
+}
+EXPORT_SYMBOL(tegra_vi_unregister_mfi_cb);
+
+struct tegra_mfi_chan {
+        struct work_struct mfi_cb_work;
+        struct rcu_head rcu;
+        u8 channel;
+};
+
+struct tegra_vi_mfi_ctx {
+        u8 num_channels;
+        struct workqueue_struct *mfi_workqueue;
+        struct tegra_mfi_chan __rcu *mfi_chans;
+};
+
+static void vi_mfi_worker(struct work_struct *vi_work)
+{
+        struct tegra_mfi_chan *mfi_chan =
+                container_of(vi_work, struct tegra_mfi_chan, mfi_cb_work);
+
+        mutex_lock(&vi_isr_lock);
+        if (mfi_callback == NULL) {
+                pr_debug("NULL callback\n");
+                mutex_unlock(&vi_isr_lock);
+                return;
+        }
+        if (mfi_callback_arg)
+                mfi_callback_arg->vi_chan = mfi_chan->channel;
+        mfi_callback(mfi_callback_arg);
+        mutex_unlock(&vi_isr_lock);
+}
+
+int tegra_vi_mfi_event_notify(struct tegra_vi_mfi_ctx *mfi_ctx, u8 channel)
+{
+        struct tegra_mfi_chan *chan;
+
+        if (!mfi_ctx) {
+                pr_err("Invalid mfi_ctx\n");
+                return -EINVAL;
+        }
+
+        if (channel > mfi_ctx->num_channels-1) {
+                pr_err("Invalid mfi channel\n");
+                return -EINVAL;
+        }
+
+        rcu_read_lock();
+        chan = rcu_dereference(mfi_ctx->mfi_chans);
+        queue_work(mfi_ctx->mfi_workqueue, &chan[channel].mfi_cb_work);
+        rcu_read_unlock();
+
+        return 0;
+}
+EXPORT_SYMBOL(tegra_vi_mfi_event_notify);
+
+bool tegra_vi_has_mfi_callback(void)
+{
+        bool ret;
+
+        mutex_lock(&vi_isr_lock);
+        ret = mfi_callback ? true : false;
+        mutex_unlock(&vi_isr_lock);
+
+        return ret;
+}
+EXPORT_SYMBOL(tegra_vi_has_mfi_callback);
+
+int tegra_vi_init_mfi(struct tegra_vi_mfi_ctx **pmfi_ctx, u8 num_channels)
+{
+        u8 i;
+        struct tegra_mfi_chan *chan;
+        struct tegra_vi_mfi_ctx *mfi_ctx;
+
+        if (!pmfi_ctx) {
+                pr_err("mfi_ctx is invalid\n");
+                return -EINVAL;
+        }
+
+        mfi_ctx = kzalloc(sizeof(*mfi_ctx), GFP_KERNEL);
+        if (unlikely(mfi_ctx == NULL))
+                return -ENOMEM;
+
+        /* create workqueue for mfi callback */
+        mfi_ctx->mfi_workqueue = alloc_workqueue("mfi_workqueue",
+                                        WQ_HIGHPRI | WQ_UNBOUND, 1);
+        if (!mfi_ctx->mfi_workqueue) {
+                pr_err("Failed to allocated mfi_workqueue");
+                tegra_vi_deinit_mfi(&mfi_ctx);
+                return -ENOMEM;
+        }
+
+        chan = kcalloc(num_channels, sizeof(*chan), GFP_KERNEL);
+        if (unlikely(chan == NULL)) {
+                tegra_vi_deinit_mfi(&mfi_ctx);
+                return -ENOMEM;
+        }
+
+        mfi_ctx->num_channels = num_channels;
+
+        /* Init mfi callback work */
+        for (i = 0; i < num_channels; i++) {
+                INIT_WORK(&chan[i].mfi_cb_work, vi_mfi_worker);
+                chan[i].channel = i;
+        }
+
+        rcu_assign_pointer(mfi_ctx->mfi_chans, chan);
+
+        *pmfi_ctx = mfi_ctx;
+
+        return 0;
+}
+EXPORT_SYMBOL(tegra_vi_init_mfi);
+
+void tegra_vi_deinit_mfi(struct tegra_vi_mfi_ctx **pmfi_ctx)
+{
+        struct tegra_vi_mfi_ctx *mfi_ctx;
+
+        if (!pmfi_ctx || !*pmfi_ctx)
+                return;
+
+        mfi_ctx = *pmfi_ctx;
+
+        flush_workqueue(mfi_ctx->mfi_workqueue);
+        destroy_workqueue(mfi_ctx->mfi_workqueue);
+
+        kfree_rcu(mfi_ctx->mfi_chans, rcu);
+
+        kfree(mfi_ctx);
+        mfi_ctx = NULL;
+}
+EXPORT_SYMBOL(tegra_vi_deinit_mfi);
+
+#if defined(CONFIG_TEGRA_ISOMGR)
+static int vi_isomgr_unregister(struct vi *tegra_vi)
+{
+        tegra_isomgr_unregister(tegra_vi->isomgr_handle);
+        tegra_vi->isomgr_handle = NULL;
+
+        return 0;
+}
+#endif
+
+static int vi_out_show(struct seq_file *s, void *unused)
+{
+        struct vi *vi = s->private;
+
+        seq_printf(s, "vi overflow: %u\n",
+                atomic_read(&(vi->vi_out.overflow)));
+
+        return 0;
+}
+
+static int vi_out_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, vi_out_show, inode->i_private);
+}
+
+static const struct file_operations vi_out_fops = {
+        .open           = vi_out_open,
+        .read           = seq_read,
+        .llseek         = seq_lseek,
+        .release        = single_release,
+};
+
+static void vi_remove_debugfs(struct vi *vi)
+{
+        debugfs_remove_recursive(vi->debugdir);
+        vi->debugdir = NULL;
+}
+
+static void vi_create_debugfs(struct vi *vi)
+{
+        struct dentry *ret;
+        char tegra_vi_name[20];
+        char debugfs_file_name[20];
+
+
+        snprintf(tegra_vi_name, sizeof(tegra_vi_name), "%s", TEGRA_VI_NAME);
+
+        vi->debugdir = debugfs_create_dir(tegra_vi_name, NULL);
+        if (!vi->debugdir) {
+                dev_err(&vi->ndev->dev,
+                        "%s: failed to create %s directory",
+                        __func__, tegra_vi_name);
+                goto create_debugfs_fail;
+        }
+
+        snprintf(debugfs_file_name, sizeof(debugfs_file_name), "%s", "vi_out");
+
+        ret = debugfs_create_file(debugfs_file_name, S_IRUGO,
+                        vi->debugdir, vi, &vi_out_fops);
+        if (!ret) {
+                dev_err(&vi->ndev->dev,
+                "%s: failed to create %s", __func__, debugfs_file_name);
+                goto create_debugfs_fail;
+        }
+
+        return;
+
+create_debugfs_fail:
+        dev_err(&vi->ndev->dev, "%s: could not create debugfs", __func__);
+        vi_remove_debugfs(vi);
+}
+
+static int nvhost_vi_slcg_handler(struct notifier_block *nb,
+                unsigned long action, void *data)
+{
+        struct clk *clk;
+        int ret = 0;
+
+        struct nvhost_device_data *pdata =
+                container_of(nb, struct nvhost_device_data,
+                        toggle_slcg_notifier);
+        struct vi *tegra_vi = (struct vi *)pdata->private_data;
+
+        if (tegra_vi->mc_vi.pg_mode)
+                return NOTIFY_OK;
+
+        clk = clk_get(NULL, "pll_d");
+        if (IS_ERR(clk))
+                return -EINVAL;
+
+        /* Make CSI sourced from PLL_D */
+        ret = tegra_clk_cfg_ex(clk, TEGRA_CLK_PLLD_CSI_OUT_ENB, 1);
+        if (ret) {
+                dev_err(&pdata->pdev->dev,
+                "%s: failed to select CSI source pll_d: %d\n",
+                __func__, ret);
+                return ret;
+        }
+
+        /* Enable PLL_D */
+        ret = clk_prepare_enable(clk);
+        if (ret) {
+                dev_err(&pdata->pdev->dev, "Can't enable pll_d: %d\n", ret);
+                return ret;
+        }
+
+        udelay(1);
+
+        /* Disable PLL_D */
+        clk_disable_unprepare(clk);
+
+        /* Restore CSI source */
+        ret = tegra_clk_cfg_ex(clk, TEGRA_CLK_MIPI_CSI_OUT_ENB, 1);
+        if (ret) {
+                dev_err(&pdata->pdev->dev,
+                "%s: failed to restore csi source: %d\n",
+                __func__, ret);
+                return ret;
+        }
+
+        clk_put(clk);
+
+        return NOTIFY_OK;
+}
+
+static int vi_probe(struct platform_device *dev)
+{
+        int err = 0;
+        struct nvhost_device_data *pdata = NULL;
+        struct tegra_t210_vi_data *data = NULL;
+        u8 num_channels;
+
+        if (dev->dev.of_node) {
+                const struct of_device_id *match;
+
+                match = of_match_device(tegra_vi_of_match, &dev->dev);
+                if (match) {
+                        data = (struct tegra_t210_vi_data *)match->data;
+                        pdata = data->info;
+                        dev->dev.platform_data = pdata;
+                }
+                /* DT initializes it to -1, use below WAR to set correct value.
+                 * TODO: Once proper fix for dev-id goes in, remove it.
+                 */
+                dev->id = dev->dev.id;
+        } else
+                pdata = (struct nvhost_device_data *)dev->dev.platform_data;
+
+        WARN_ON(!pdata);
+        if (!pdata) {
+                dev_info(&dev->dev, "no platform data\n");
+                return -ENODATA;
+        }
+
+        err = nvhost_check_bondout(pdata->bond_out_id);
+        if (err) {
+                dev_warn(&dev->dev, "No VI unit present. err:%d", err);
+                return err;
+        }
+
+        pdata->pdev = dev;
+        mutex_init(&pdata->lock);
+        platform_set_drvdata(dev, pdata);
+
+        dev_info(&dev->dev, "%s: ++\n", __func__);
+
+        tegra_vi = devm_kzalloc(&dev->dev, sizeof(struct vi), GFP_KERNEL);
+        if (!tegra_vi)
+                return -ENOMEM;
+
+        tegra_vi->ndev = dev;
+        tegra_vi->dev = &dev->dev;
+        err = nvhost_client_device_get_resources(dev);
+        if (err)
+                goto vi_probe_fail;
+
+        num_channels = 6;
+
+        err = tegra_vi_init_mfi(&tegra_vi->mfi_ctx, num_channels);
+        if (err)
+                goto vi_probe_fail;
+
+        if (!pdata->aperture[0]) {
+                dev_err(&dev->dev, "%s: failed to map register base\n",
+                                __func__);
+                return -ENXIO;
+        }
+
+        /* call vi_intr_init and stats_work */
+        INIT_WORK(&tegra_vi->stats_work, vi_stats_worker);
+
+        err = vi_intr_init(tegra_vi);
+        if (err)
+                goto vi_mfi_init_fail;
+
+        vi_create_debugfs(tegra_vi);
+
+        i2c_ctrl = pdata->private_data;
+        pdata->private_data = tegra_vi;
+        mutex_init(&tegra_vi->update_la_lock);
+
+        /* Create I2C Devices according to settings from board file */
+        if (i2c_ctrl && i2c_ctrl->new_devices)
+                i2c_ctrl->new_devices(dev);
+
+        tegra_vi->reg = regulator_get(&tegra_vi->ndev->dev, "avdd_dsi_csi");
+        if (IS_ERR(tegra_vi->reg)) {
+                err = PTR_ERR(tegra_vi->reg);
+                if (err == -ENODEV)
+                        dev_info(&tegra_vi->ndev->dev,
+                                "%s: no regulator device\n", __func__);
+                else
+                        dev_err(&tegra_vi->ndev->dev,
+                                "%s: couldn't get regulator\n", __func__);
+                tegra_vi->reg = NULL;
+                if (tegra_platform_is_silicon())
+                        goto camera_i2c_unregister;
+        }
+
+#ifdef CONFIG_TEGRA_CAMERA
+        tegra_vi->camera = tegra_camera_register(dev);
+        if (!tegra_vi->camera) {
+                dev_err(&dev->dev, "%s: can't register tegra_camera\n",
+                                __func__);
+                goto vi_regulator_put;
+        }
+#endif
+
+        if (pdata->slcg_notifier_enable &&
+                        (pdata->powergate_id != -1)) {
+                pdata->toggle_slcg_notifier.notifier_call =
+                &nvhost_vi_slcg_handler;
+
+                slcg_register_notifier(pdata->powergate_id,
+                        &pdata->toggle_slcg_notifier);
+        }
+
+        nvhost_module_init(dev);
+
+        err = nvhost_client_device_init(dev);
+        if (err)
+                goto camera_unregister;
+
+        tegra_vi->mc_vi.vi = tegra_vi;
+        tegra_vi->mc_vi.csi = &tegra_vi->csi;
+        tegra_vi->mc_vi.reg = tegra_vi->reg;
+        tegra_vi->mc_vi.fops = data->vi_fops;
+        tegra_vi->csi.fops = data->csi_fops;
+        err = tegra_csi_media_controller_init(&tegra_vi->csi, dev);
+        if (err)
+                goto vi_mc_init_error;
+
+        err = tegra_vi_media_controller_init(&tegra_vi->mc_vi, dev);
+        if (err)
+                goto vi_mc_init_error;
+
+        return 0;
+
+vi_mc_init_error:
+        nvhost_client_device_release(dev);
+camera_unregister:
+#ifdef CONFIG_TEGRA_CAMERA
+        tegra_camera_unregister(tegra_vi->camera);
+vi_regulator_put:
+#endif
+
+        regulator_put(tegra_vi->reg);
+        tegra_vi->reg = NULL;
+
+camera_i2c_unregister:
+        if (i2c_ctrl && i2c_ctrl->remove_devices)
+                i2c_ctrl->remove_devices(dev);
+        pdata->private_data = i2c_ctrl;
+vi_mfi_init_fail:
+        tegra_vi_deinit_mfi(&tegra_vi->mfi_ctx);
+vi_probe_fail:
+        dev_err(&dev->dev, "%s: failed\n", __func__);
+        return err;
+}
+
+static int __exit vi_remove(struct platform_device *dev)
+{
+#ifdef CONFIG_TEGRA_CAMERA
+        int err = 0;
+#endif
+        struct nvhost_device_data *pdata = platform_get_drvdata(dev);
+        struct vi *tegra_vi = (struct vi *)pdata->private_data;
+
+#ifdef CONFIG_PM
+        if (atomic_read(&dev->dev.power.usage_count) > 0)
+                return -EBUSY;
+#endif
+
+        dev_info(&dev->dev, "%s: ++\n", __func__);
+
+#if defined(CONFIG_TEGRA_ISOMGR)
+        if (tegra_vi->isomgr_handle)
+                vi_isomgr_unregister(tegra_vi);
+#endif
+
+        tegra_vi_deinit_mfi(&tegra_vi->mfi_ctx);
+
+        vi_remove_debugfs(tegra_vi);
+
+        tegra_vi_media_controller_cleanup(&tegra_vi->mc_vi);
+
+        nvhost_client_device_release(dev);
+
+        if (pdata->slcg_notifier_enable &&
+            (pdata->powergate_id != -1))
+                slcg_unregister_notifier(pdata->powergate_id,
+                                         &pdata->toggle_slcg_notifier);
+
+        vi_intr_free(tegra_vi);
+
+        pdata->aperture[0] = NULL;
+#ifdef CONFIG_TEGRA_CAMERA
+        err = tegra_camera_unregister(tegra_vi->camera);
+        if (err)
+                return err;
+#endif
+
+#ifdef CONFIG_PM_GENERIC_DOMAINS
+        tegra_pd_remove_device(&dev->dev);
+#endif
+
+        regulator_put(tegra_vi->reg);
+        tegra_vi->reg = NULL;
+
+        /* Remove I2C Devices according to settings from board file */
+        if (i2c_ctrl && i2c_ctrl->remove_devices)
+                i2c_ctrl->remove_devices(dev);
+
+        pdata->private_data = i2c_ctrl;
+
+        return 0;
+}
+
+static struct platform_driver vi_driver = {
+        .probe = vi_probe,
+        .remove = __exit_p(vi_remove),
+        .driver = {
+                .owner = THIS_MODULE,
+                .name = "vi",
+#ifdef CONFIG_PM
+                .pm = &nvhost_module_pm_ops,
+#endif
+#ifdef CONFIG_OF
+                .of_match_table = tegra_vi_of_match,
+#endif
+        }
+};
+
+static struct of_device_id tegra_vi_domain_match[] = {
+        {.compatible = "nvidia,tegra210-ve-pd",
+         .data = (struct nvhost_device_data *)&t21_vi_info},
+        {},
+};
+
+static int __init vi_init(void)
+{
+        int ret;
+
+        ret = nvhost_domain_init(tegra_vi_domain_match);
+        if (ret)
+                return ret;
+
+        return platform_driver_register(&vi_driver);
+}
+
+static void __exit vi_exit(void)
+{
+        platform_driver_unregister(&vi_driver);
+}
+
+late_initcall(vi_init);
+module_exit(vi_exit);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/tegra/vi/vi.h b/drivers/media/platform/tegra/vi/vi.h
new file mode 100644
index 000000000..a03057265
--- /dev/null
+++ b/drivers/media/platform/tegra/vi/vi.h
@@ -0,0 +1,128 @@
+/*
+ * drivers/video/tegra/host/vi/vi.h
+ *
+ * Tegra Graphics Host VI
+ *
+ * Copyright (c) 2012-2016, NVIDIA CORPORATION. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_VI_H__
+#define __NVHOST_VI_H__
+
+#include <linux/platform/tegra/isomgr.h>
+#include <linux/tegra-powergate.h>
+#include <linux/clk/tegra.h>
+
+#include "camera/vi/mc_common.h"
+#include "chip_support.h"
+
+#define VI_CFG_INTERRUPT_MASK_0                         0x8c
+#define VI_CFG_INTERRUPT_STATUS_0                       0x98
+
+#define CSI_CSI_PIXEL_PARSER_A_INTERRUPT_MASK_0         0x850
+#define CSI_CSI_PIXEL_PARSER_A_STATUS_0                 0x854
+#define PPA_FIFO_OVRF                                   (1 << 5)
+
+#define CSI_CSI_PIXEL_PARSER_B_INTERRUPT_MASK_0         0x884
+#define CSI_CSI_PIXEL_PARSER_B_STATUS_0                 0x888
+#define PPB_FIFO_OVRF                                   (1 << 5)
+
+#define VI_CSI_0_ERROR_STATUS                           0x184
+#define VI_CSI_1_ERROR_STATUS                           0x284
+#define VI_CSI_0_WD_CTRL                                0x18c
+#define VI_CSI_1_WD_CTRL                                0x28c
+#define VI_CSI_0_ERROR_INT_MASK_0                       0x188
+#define VI_CSI_1_ERROR_INT_MASK_0                       0x288
+
+#ifdef TEGRA_21X_OR_HIGHER_CONFIG
+#define VI_CSI_2_ERROR_STATUS                           0x384
+#define VI_CSI_3_ERROR_STATUS                           0x484
+#define VI_CSI_4_ERROR_STATUS                           0x584
+#define VI_CSI_5_ERROR_STATUS                           0x684
+
+#define VI_CSI_2_WD_CTRL                                0x38c
+#define VI_CSI_3_WD_CTRL                                0x48c
+#define VI_CSI_4_WD_CTRL                                0x58c
+#define VI_CSI_5_WD_CTRL                                0x68c
+
+#define VI_CSI_2_ERROR_INT_MASK_0                       0x388
+#define VI_CSI_3_ERROR_INT_MASK_0                       0x488
+#define VI_CSI_4_ERROR_INT_MASK_0                       0x588
+#define VI_CSI_5_ERROR_INT_MASK_0                       0x688
+
+#define CSI1_CSI_PIXEL_PARSER_A_INTERRUPT_MASK_0        0x1050
+#define CSI1_CSI_PIXEL_PARSER_A_STATUS_0                0x1054
+#define CSI1_CSI_PIXEL_PARSER_B_INTERRUPT_MASK_0        0x1084
+#define CSI1_CSI_PIXEL_PARSER_B_STATUS_0                0x1088
+#define CSI2_CSI_PIXEL_PARSER_A_INTERRUPT_MASK_0        0x1850
+#define CSI2_CSI_PIXEL_PARSER_A_STATUS_0                0x1854
+#define CSI2_CSI_PIXEL_PARSER_B_INTERRUPT_MASK_0        0x1884
+#define CSI2_CSI_PIXEL_PARSER_B_STATUS_0                0x1888
+
+#define NUM_VI_WATCHDOG                                 6
+#else
+#define NUM_VI_WATCHDOG                                 2
+#endif
+
+typedef void (*callback)(void *);
+
+struct tegra_vi_stats {
+        atomic_t overflow;
+};
+
+struct tegra_vi_mfi_ctx;
+
+struct vi {
+        struct tegra_camera *camera;
+        struct platform_device *ndev;
+        struct device *dev;
+        struct tegra_vi_data *data;
+        struct tegra_mc_vi mc_vi;
+        struct tegra_csi_device csi;
+
+        struct regulator *reg;
+        struct dentry *debugdir;
+        struct tegra_vi_stats vi_out;
+        struct work_struct stats_work;
+        struct tegra_vi_mfi_ctx *mfi_ctx;
+#if defined(CONFIG_TEGRA_ISOMGR)
+        tegra_isomgr_handle isomgr_handle;
+#endif
+        int vi_irq;
+        uint vi_bypass_bw;
+        uint max_bw;
+        struct mutex update_la_lock;
+        bool master_deinitialized;
+        bool tpg_opened;
+        bool sensor_opened;
+        bool bypass;
+};
+
+extern const struct file_operations tegra_vi_ctrl_ops;
+int nvhost_vi_prepare_poweroff(struct platform_device *);
+int nvhost_vi_finalize_poweron(struct platform_device *);
+
+void nvhost_vi_reset_all(struct platform_device *);
+struct vi *tegra_vi_get(void);
+int vi_v4l2_set_la(struct vi *tegra_vi, u32 vi_bypass_bw, bool is_ioctl);
+
+int tegra_vi_register_mfi_cb(callback cb, void *cb_arg);
+int tegra_vi_unregister_mfi_cb(void);
+
+bool tegra_vi_has_mfi_callback(void);
+int tegra_vi_mfi_event_notify(struct tegra_vi_mfi_ctx *mfi_ctx, u8 channel);
+int tegra_vi_init_mfi(struct tegra_vi_mfi_ctx **mfi_ctx, u8 num_channels);
+void tegra_vi_deinit_mfi(struct tegra_vi_mfi_ctx **mfi_ctx);
+#endif
diff --git a/drivers/media/platform/tegra/vi/vi_irq.c b/drivers/media/platform/tegra/vi/vi_irq.c
new file mode 100644
index 000000000..9f3cc3f3c
--- /dev/null
+++ b/drivers/media/platform/tegra/vi/vi_irq.c
@@ -0,0 +1,244 @@
+/*
+ * drivers/video/tegra/host/vi/vi_irq.c
+ *
+ * Copyright (c) 2014-2016, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/nvhost.h>
+
+#include "nvhost_acm.h"
+#include "vi/vi.h"
+#include "vi/vi_irq.h"
+
+static const int status_reg_table[] = {
+        VI_CFG_INTERRUPT_STATUS_0,
+        CSI_CSI_PIXEL_PARSER_A_STATUS_0,
+        CSI_CSI_PIXEL_PARSER_B_STATUS_0,
+        VI_CSI_0_ERROR_STATUS,
+        VI_CSI_1_ERROR_STATUS,
+#ifdef TEGRA_21X_OR_HIGHER_CONFIG
+        VI_CSI_2_ERROR_STATUS,
+        VI_CSI_3_ERROR_STATUS,
+        VI_CSI_4_ERROR_STATUS,
+        VI_CSI_5_ERROR_STATUS,
+        CSI1_CSI_PIXEL_PARSER_A_STATUS_0,
+        CSI1_CSI_PIXEL_PARSER_B_STATUS_0,
+        CSI2_CSI_PIXEL_PARSER_A_STATUS_0,
+        CSI2_CSI_PIXEL_PARSER_B_STATUS_0,
+#endif
+};
+
+static const int mask_reg_table[] = {
+        VI_CFG_INTERRUPT_MASK_0,
+        CSI_CSI_PIXEL_PARSER_A_INTERRUPT_MASK_0,
+        CSI_CSI_PIXEL_PARSER_B_INTERRUPT_MASK_0,
+        VI_CSI_0_ERROR_INT_MASK_0,
+        VI_CSI_1_ERROR_INT_MASK_0,
+        VI_CSI_0_WD_CTRL,
+        VI_CSI_1_WD_CTRL,
+#ifdef TEGRA_21X_OR_HIGHER_CONFIG
+        VI_CSI_2_WD_CTRL,
+        VI_CSI_3_WD_CTRL,
+        VI_CSI_4_WD_CTRL,
+        VI_CSI_5_WD_CTRL,
+        VI_CSI_2_ERROR_INT_MASK_0,
+        VI_CSI_3_ERROR_INT_MASK_0,
+        VI_CSI_4_ERROR_INT_MASK_0,
+        VI_CSI_5_ERROR_INT_MASK_0,
+        CSI1_CSI_PIXEL_PARSER_A_INTERRUPT_MASK_0,
+        CSI1_CSI_PIXEL_PARSER_B_INTERRUPT_MASK_0,
+        CSI2_CSI_PIXEL_PARSER_A_INTERRUPT_MASK_0,
+        CSI2_CSI_PIXEL_PARSER_B_INTERRUPT_MASK_0,
+#endif
+};
+
+static inline void clear_state(struct vi *tegra_vi, int addr)
+{
+        int val;
+
+        val = host1x_readl(tegra_vi->ndev, addr);
+        host1x_writel(tegra_vi->ndev, addr, val);
+}
+
+int vi_enable_irq(struct vi *tegra_vi)
+{
+        int i;
+
+        /* Mask all VI interrupt */
+        for (i = 0; i < ARRAY_SIZE(mask_reg_table); i++)
+                host1x_writel(tegra_vi->ndev, mask_reg_table[i], 0);
+
+        /* Clear all VI interrupt state registers */
+        for (i = 0; i < ARRAY_SIZE(status_reg_table); i++)
+                clear_state(tegra_vi, status_reg_table[i]);
+
+        enable_irq(tegra_vi->vi_irq);
+
+        return 0;
+
+}
+EXPORT_SYMBOL(vi_enable_irq);
+
+int vi_disable_irq(struct vi *tegra_vi)
+{
+        int i;
+
+        disable_irq(tegra_vi->vi_irq);
+
+        /* Mask all VI interrupt */
+        for (i = 0; i < ARRAY_SIZE(mask_reg_table); i++)
+                host1x_writel(tegra_vi->ndev, mask_reg_table[i], 0);
+
+        /* Clear all VI interrupt state registers */
+        for (i = 0; i < ARRAY_SIZE(status_reg_table); i++)
+                clear_state(tegra_vi, status_reg_table[i]);
+
+        return 0;
+}
+EXPORT_SYMBOL(vi_disable_irq);
+
+static irqreturn_t vi_checkwd(struct vi *tegra_vi, int stream)
+{
+        int err_addr, wd_addr, val;
+
+        switch (stream) {
+        case 0:
+                err_addr = VI_CSI_0_ERROR_STATUS;
+                wd_addr = VI_CSI_0_WD_CTRL;
+                break;
+        case 1:
+                err_addr = VI_CSI_1_ERROR_STATUS;
+                wd_addr = VI_CSI_1_WD_CTRL;
+                break;
+#ifdef TEGRA_21X_OR_HIGHER_CONFIG
+        case 2:
+                err_addr = VI_CSI_2_ERROR_STATUS;
+                wd_addr = VI_CSI_2_WD_CTRL;
+                break;
+        case 3:
+                err_addr = VI_CSI_3_ERROR_STATUS;
+                wd_addr = VI_CSI_3_WD_CTRL;
+                break;
+        case 4:
+                err_addr = VI_CSI_4_ERROR_STATUS;
+                wd_addr = VI_CSI_4_WD_CTRL;
+                break;
+        case 5:
+                err_addr = VI_CSI_5_ERROR_STATUS;
+                wd_addr = VI_CSI_5_WD_CTRL;
+                break;
+#endif
+        default:
+                return IRQ_NONE;
+        }
+
+        val = host1x_readl(tegra_vi->ndev, err_addr);
+        if (val & 0x20) {
+                host1x_writel(tegra_vi->ndev, wd_addr, 0);
+                host1x_writel(tegra_vi->ndev, err_addr, 0x20);
+                tegra_vi_mfi_event_notify(tegra_vi->mfi_ctx, stream);
+                return IRQ_HANDLED;
+        }
+
+        return IRQ_NONE;
+}
+
+static irqreturn_t vi_isr(int irq, void *dev_id)
+{
+        struct vi *tegra_vi = (struct vi *)dev_id;
+        int i, val;
+        irqreturn_t result;
+
+        for (i = 0; i < NUM_VI_WATCHDOG; i++) {
+                result = vi_checkwd(tegra_vi, i);
+                if (result == IRQ_HANDLED)
+                        goto handled;
+        }
+
+        dev_dbg(&tegra_vi->ndev->dev, "%s: ++", __func__);
+
+        val = host1x_readl(tegra_vi->ndev, CSI_CSI_PIXEL_PARSER_A_STATUS_0);
+
+        /* changes required as per t124 register spec */
+        if (val & PPA_FIFO_OVRF)
+                atomic_inc(&(tegra_vi->vi_out.overflow));
+
+        /* Disable IRQ */
+        vi_disable_irq(tegra_vi);
+
+        schedule_work(&tegra_vi->stats_work);
+
+handled:
+        return IRQ_HANDLED;
+}
+EXPORT_SYMBOL(vi_isr);
+
+void vi_stats_worker(struct work_struct *work)
+{
+        struct vi *tegra_vi = container_of(work, struct vi, stats_work);
+
+        dev_dbg(&tegra_vi->ndev->dev,
+                "%s: vi_out dropped data %u times", __func__,
+                atomic_read(&(tegra_vi->vi_out.overflow)));
+
+        /* Enable IRQ's */
+        vi_enable_irq(tegra_vi);
+}
+EXPORT_SYMBOL(vi_stats_worker);
+
+int vi_intr_init(struct vi *tegra_vi)
+{
+        struct platform_device *ndev = tegra_vi->ndev;
+
+        /* Interrupt resources are only associated with
+         * master dev vi.0 so irq must be programmed
+         * with it only.
+         */
+        int ret;
+
+        dev_dbg(&tegra_vi->ndev->dev, "%s: ++", __func__);
+
+        tegra_vi->vi_irq = platform_get_irq(ndev, 0);
+        if (IS_ERR_VALUE(tegra_vi->vi_irq)) {
+                dev_err(&tegra_vi->ndev->dev, "missing camera irq\n");
+                return -ENXIO;
+        }
+
+        ret = request_irq(tegra_vi->vi_irq,
+                        vi_isr,
+                        IRQF_SHARED,
+                        dev_name(&tegra_vi->ndev->dev),
+                        tegra_vi);
+        if (ret) {
+                dev_err(&tegra_vi->ndev->dev, "failed to get irq\n");
+                return -EBUSY;
+        }
+
+        disable_irq(tegra_vi->vi_irq);
+
+        return 0;
+}
+EXPORT_SYMBOL(vi_intr_init);
+
+int vi_intr_free(struct vi *tegra_vi)
+{
+        dev_dbg(&tegra_vi->ndev->dev, "%s: ++", __func__);
+
+        free_irq(tegra_vi->vi_irq, tegra_vi);
+
+        return 0;
+}
+EXPORT_SYMBOL(vi_intr_free);
+
diff --git a/drivers/media/platform/tegra/vi/vi_irq.h b/drivers/media/platform/tegra/vi/vi_irq.h
new file mode 100644
index 000000000..26fed8a00
--- /dev/null
+++ b/drivers/media/platform/tegra/vi/vi_irq.h
@@ -0,0 +1,26 @@
+/*
+ * drivers/video/tegra/host/vi/vi_irq.h
+ *
+ * Copyright (c) 2014-2016, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef __DRIVERS_VIDEO_VI_IRQ_H
+#define __DRIVERS_VIDEO_VI_IRQ_H
+
+int vi_intr_init(struct vi *vi);
+int vi_intr_free(struct vi *vi);
+void vi_stats_worker(struct work_struct *work);
+int vi_enable_irq(struct vi *vi);
+int vi_disable_irq(struct vi *vi);
+#endif
+
diff --git a/drivers/video/tegra/camera/Makefile b/drivers/video/tegra/camera/Makefile
new file mode 100644
index 000000000..87d2e3a72
--- /dev/null
+++ b/drivers/video/tegra/camera/Makefile
@@ -0,0 +1,10 @@
+GCOV_PROFILE := y
+ccflags-y += -Idrivers/media/platform/tegra/vi
+ccflags-y += -I../nvhost/drivers/video/tegra/host
+ccflags-y += -Idrivers/video/tegra/host
+ccflags-y += -Idrivers/media/platform/tegra/
+
+ifdef CONFIG_TEGRA_CAMERA_PLATFORM
+obj-y += tegra_camera_platform.o
+obj-y += tegra_camera_dev_mfi.o
+endif
diff --git a/drivers/video/tegra/camera/camera_priv.h b/drivers/video/tegra/camera/camera_priv.h
new file mode 100644
index 000000000..a854013dd
--- /dev/null
+++ b/drivers/video/tegra/camera/camera_priv.h
@@ -0,0 +1,52 @@
+/*
+ * drivers/video/tegra/camera/camera_priv.h
+ *
+ * Copyright (c) 2013, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef __DRIVERS_VIDEO_TEGRA_CAMERA_CAMERA_PRIV_H
+#define __DRIVERS_VIDEO_TEGRA_CAMERA_CAMERA_PRIV_H
+
+#include <linux/nvhost.h>
+#include <linux/io.h>
+#include "camera_priv_defs.h"
+
+static inline unsigned long tegra_camera_readl(struct tegra_camera *camera,
+                                                unsigned long reg)
+{
+        unsigned long ret;
+        struct platform_device *ndev = to_platform_device(camera->dev);
+        struct nvhost_device_data *pdata = platform_get_drvdata(ndev);
+
+        WARN(!tegra_is_clk_enabled(camera->clock[CAMERA_VI_CLK].clk),
+                "VI is clock-gated");
+
+        ret = readl(pdata->aperture[0] + reg);
+        return ret;
+}
+
+static inline void tegra_camera_writel(struct tegra_camera *camera,
+                                        unsigned long val,
+                                        unsigned long reg)
+{
+        struct platform_device *ndev = to_platform_device(camera->dev);
+        struct nvhost_device_data *pdata = platform_get_drvdata(ndev);
+
+        WARN(!tegra_is_clk_enabled(camera->clock[CAMERA_VI_CLK].clk),
+                "VI is clock-gated");
+
+        writel(val, pdata->aperture[0] + reg);
+}
+
+
+#endif
diff --git a/drivers/video/tegra/camera/camera_priv_defs.h b/drivers/video/tegra/camera/camera_priv_defs.h
new file mode 100644
index 000000000..7a2c8a6d3
--- /dev/null
+++ b/drivers/video/tegra/camera/camera_priv_defs.h
@@ -0,0 +1,114 @@
+/*
+ * drivers/video/tegra/camera/camera_priv_defs.h
+ *
+ * Copyright (c) 2013, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef __DRIVERS_VIDEO_TEGRA_CAMERA_CAMERA_PRIV_DEFS_H
+#define __DRIVERS_VIDEO_TEGRA_CAMERA_CAMERA_PRIV_DEFS_H
+
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/ioctl.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/export.h>
+#include <linux/slab.h>
+#include <linux/clk/tegra.h>
+#include <linux/tegra-powergate.h>
+
+#if defined(CONFIG_TEGRA_ISOMGR)
+#include <linux/platform/tegra/isomgr.h>
+#endif
+
+#include <video/tegra_camera.h>
+
+
+/*
+ * CAMERA_*_CLK is only for internal driver use.
+ * TEGRA_CAMERA_*_CLK is enum used between driver and user space.
+ * TEGRA_CAMERA_*_CLK is defined in tegra_camera.h
+ */
+enum {
+        CAMERA_VI_CLK,
+        CAMERA_EMC_CLK,
+        CAMERA_ISP_CLK,
+        CAMERA_CSI_CLK,
+#if defined(CONFIG_ARCH_TEGRA_11x_SOC) || defined(CONFIG_ARCH_TEGRA_14x_SOC)
+        CAMERA_CILAB_CLK,
+        CAMERA_CILE_CLK,
+        CAMERA_PLL_D2_CLK,
+#endif
+
+#ifdef CONFIG_ARCH_TEGRA_11x_SOC
+        CAMERA_CILCD_CLK,
+#endif
+        CAMERA_SCLK,
+        CAMERA_CLK_MAX,
+};
+
+struct clock {
+        struct clk *clk;
+        bool on;
+};
+
+struct vi_stats {
+        atomic_t overflow;
+};
+
+struct tegra_camera {
+        struct device *dev;
+        struct miscdevice misc_dev;
+        struct clock clock[CAMERA_CLK_MAX];
+        struct regulator *reg;
+        struct tegra_camera_clk_info info;
+        struct mutex tegra_camera_lock;
+        atomic_t in_use;
+        int power_on;
+        int irq;
+        struct dentry *debugdir;
+        struct vi_stats vi_out0;
+        struct vi_stats vi_out1;
+        struct work_struct stats_work;
+#if defined(CONFIG_TEGRA_ISOMGR)
+        tegra_isomgr_handle isomgr_handle;
+#endif
+};
+
+/*
+ * index: clock enum value
+ * name:  clock name
+ * init:  default clock state when camera is opened.
+ * freq:  initial clock frequency to set when camera is opened. If it is 0,
+ *        then no need to set clock freq.
+ */
+struct clock_data {
+        int index;
+        char *name;
+        bool init;
+        unsigned long freq;
+};
+
+struct tegra_camera *tegra_camera_register(struct platform_device *ndev);
+int tegra_camera_unregister(struct tegra_camera *camera);
+#ifdef CONFIG_PM
+int tegra_camera_suspend(struct tegra_camera *camera);
+int tegra_camera_resume(struct tegra_camera *camera);
+#endif
+
+#endif
diff --git a/drivers/video/tegra/camera/tegra_camera_dev_mfi.c b/drivers/video/tegra/camera/tegra_camera_dev_mfi.c
new file mode 100644
index 000000000..67e6eaaa1
--- /dev/null
+++ b/drivers/video/tegra/camera/tegra_camera_dev_mfi.c
@@ -0,0 +1,271 @@
+/*
+ * drivers/video/tegra/camera/tegra_camera_dev_mfi.c
+ *
+ * Copyright (c) 2015-2016, NVIDIA CORPORATION.  All rights reserved.
+
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+
+#include "tegra_camera_dev_mfi.h"
+
+static LIST_HEAD(cmfidev_list);
+static DEFINE_MUTEX(cmfidev_mutex);
+
+int tegra_camera_dev_mfi_init(void)
+{
+        INIT_LIST_HEAD(&cmfidev_list);
+
+        return 0;
+}
+
+void tegra_camera_dev_mfi_cb(void *stub)
+{
+        u32 idx = 0;
+        struct camera_mfi_dev *itr = NULL;
+        int err = 0;
+        mutex_lock(&cmfidev_mutex);
+        list_for_each_entry(itr, &cmfidev_list, list) {
+                if (itr->regmap) {
+                        /* MFI driver has to delay the focuser writes by one
+                         * frame, which is required to get sync in focus
+                         * position and sharpness.
+                         * So write previous frame focuser settings in current
+                         * frame's callback, and then save current frame focuser
+                         * writes for next callback.
+                         */
+                        for (idx = 0; idx < itr->prev_num_used; idx++) {
+                                err = regmap_write(itr->regmap,
+                                                itr->prev_reg[idx].addr,
+                                                itr->prev_reg[idx].val);
+                                if (err)
+                                        pr_err("%s: [%s] regmap_write failed\n",
+                                                __func__, itr->name);
+                        }
+                        /* Consume current settings, which would be programmed
+                         * in next frame callback.
+                         */
+                        for (idx = 0; idx < itr->num_used; idx++) {
+                                itr->prev_reg[idx].addr = itr->reg[idx].addr;
+                                itr->prev_reg[idx].val = itr->reg[idx].val;
+                        }
+                        itr->prev_num_used = itr->num_used;
+                } else if (itr->i2c_client) {
+                        for (idx = 0; idx < itr->num_used; idx++) {
+                                err = i2c_transfer(itr->i2c_client->adapter,
+                                                &itr->msg[idx].msg, 1);
+                                if (err != 1)
+                                        pr_err("%s: [%s] i2c_transfer failed\n",
+                                                __func__, itr->name);
+                        }
+                } else {
+                        pr_err("%s [%s] Unknown device mechanism\n",
+                                __func__, itr->name);
+                }
+                itr->num_used = 0;
+        }
+        mutex_unlock(&cmfidev_mutex);
+}
+
+int tegra_camera_dev_mfi_wr_add_i2c(
+        struct camera_mfi_dev *cmfidev,
+        struct i2c_msg *msg, int num)
+{
+        int err = -ENODEV;
+        int i = 0;
+        struct camera_mfi_dev *itr = NULL;
+
+        if (!strcmp(cmfidev->name, "")) {
+                err = -EINVAL;
+                goto cmfi_wr_add_i2c_end;
+        }
+
+        mutex_lock(&cmfidev_mutex);
+        list_for_each_entry(itr, &cmfidev_list, list) {
+                if (!strcmp(itr->name, cmfidev->name)) {
+                        if (itr->num_used == CAMERA_REGCACHE_MAX)
+                                err = -ENOSPC;
+                        else {
+                                for (i = 0; i < num; i++) {
+                                        itr->msg[itr->num_used].msg = msg[i];
+                                        memcpy(itr->msg[itr->num_used].buf,
+                                                                msg[i].buf,
+                                                                msg[i].len);
+                                        itr->msg[itr->num_used].msg.buf =
+                                                itr->msg[itr->num_used].buf;
+                                        itr->num_used++;
+                                }
+                                err = 0;
+                        }
+                }
+        }
+        mutex_unlock(&cmfidev_mutex);
+
+cmfi_wr_add_i2c_end:
+        return err;
+}
+EXPORT_SYMBOL(tegra_camera_dev_mfi_wr_add_i2c);
+
+int tegra_camera_dev_mfi_wr_add(
+        struct camera_mfi_dev *cmfidev,
+        u32 offset, u32 val)
+{
+        int err = -ENODEV;
+        struct camera_mfi_dev *itr = NULL;
+
+        if (cmfidev->name == NULL || !strcmp(cmfidev->name, "")) {
+                err = -EINVAL;
+                goto cmfi_wr_add_end;
+        }
+
+        mutex_lock(&cmfidev_mutex);
+        list_for_each_entry(itr, &cmfidev_list, list) {
+                if (!strcmp(itr->name, cmfidev->name)) {
+                        if (itr->num_used == CAMERA_REGCACHE_MAX) {
+                                err = -ENOSPC;
+                        } else {
+                                itr->reg[itr->num_used].addr = offset;
+                                itr->reg[itr->num_used].val = val;
+                                itr->num_used++;
+                                err = 0;
+                        }
+                }
+        }
+        mutex_unlock(&cmfidev_mutex);
+
+cmfi_wr_add_end:
+        return err;
+}
+EXPORT_SYMBOL(tegra_camera_dev_mfi_wr_add);
+
+int tegra_camera_dev_mfi_clear(struct camera_mfi_dev *cmfidev)
+{
+        int err = -ENODEV;
+        struct camera_mfi_dev *itr = NULL;
+
+        if (cmfidev == NULL) {
+                err = -EINVAL;
+                goto cmfidev_clear_end;
+        }
+
+        if (cmfidev->name == NULL || !strcmp(cmfidev->name, "")) {
+                err = -EINVAL;
+                goto cmfidev_clear_end;
+        }
+
+        mutex_lock(&cmfidev_mutex);
+        list_for_each_entry(itr, &cmfidev_list, list) {
+                if (!strcmp(itr->name, cmfidev->name)) {
+                        if (itr->num_used > 0)
+                                pr_info("%s [%s] force clear Q pending writes\n",
+                                                __func__, itr->name);
+                        itr->num_used = 0;
+                        err = 0;
+                }
+        }
+        mutex_unlock(&cmfidev_mutex);
+
+cmfidev_clear_end:
+        return err;
+}
+EXPORT_SYMBOL(tegra_camera_dev_mfi_clear);
+
+int tegra_camera_dev_mfi_add_i2cclient(
+        struct camera_mfi_dev **cmfidev,
+        u8 *name,
+        struct i2c_client *i2c_client)
+{
+        int err = 0;
+        struct camera_mfi_dev *itr = NULL;
+        struct camera_mfi_dev *new_cmfidev = NULL;
+
+        if (name == NULL || !strcmp(name, ""))
+                return -EINVAL;
+
+        mutex_lock(&cmfidev_mutex);
+        list_for_each_entry(itr, &cmfidev_list, list) {
+                if (!strcmp(itr->name, name)) {
+                        err = -EEXIST;
+                        goto cmfidev_add_i2c_unlock;
+                }
+        }
+        if (!err) {
+                new_cmfidev =
+                        kzalloc(sizeof(struct camera_mfi_dev), GFP_KERNEL);
+                if (!new_cmfidev) {
+                        pr_err("%s memory low!\n", __func__);
+                        err = -ENOMEM;
+                        goto cmfidev_add_i2c_unlock;
+                }
+                memset(new_cmfidev, 0, sizeof(struct camera_mfi_dev));
+                strncpy(new_cmfidev->name, name, sizeof(new_cmfidev->name)-1);
+                INIT_LIST_HEAD(&new_cmfidev->list);
+                new_cmfidev->i2c_client = i2c_client;
+                new_cmfidev->num_used = 0;
+                list_add(&new_cmfidev->list, &cmfidev_list);
+        }
+
+        *cmfidev = new_cmfidev;
+
+cmfidev_add_i2c_unlock:
+        mutex_unlock(&cmfidev_mutex);
+
+        return err;
+}
+EXPORT_SYMBOL(tegra_camera_dev_mfi_add_i2cclient);
+
+int tegra_camera_dev_mfi_add_regmap(
+        struct camera_mfi_dev **cmfidev,
+        u8 *name,
+        struct regmap *regmap)
+{
+        int err = 0;
+        struct camera_mfi_dev *itr = NULL;
+        struct camera_mfi_dev *new_cmfidev = NULL;
+
+        if (name == NULL || !strcmp(name, ""))
+                return -EINVAL;
+
+        mutex_lock(&cmfidev_mutex);
+        list_for_each_entry(itr, &cmfidev_list, list) {
+                if (!strcmp(itr->name, name)) {
+                        err = -EEXIST;
+                        goto cmfidev_add_regmap_unlock;
+                }
+        }
+        if (!err) {
+                new_cmfidev =
+                        kzalloc(sizeof(struct camera_mfi_dev), GFP_KERNEL);
+                if (!new_cmfidev) {
+                        pr_err("%s memory low!\n", __func__);
+                        err = -ENOMEM;
+                        goto cmfidev_add_regmap_unlock;
+                }
+                memset(new_cmfidev, 0, sizeof(struct camera_mfi_dev));
+                strncpy(new_cmfidev->name, name, sizeof(new_cmfidev->name)-1);
+                INIT_LIST_HEAD(&new_cmfidev->list);
+                new_cmfidev->regmap = regmap;
+                new_cmfidev->num_used = 0;
+                new_cmfidev->prev_num_used = 0;
+                list_add(&new_cmfidev->list, &cmfidev_list);
+        }
+
+        *cmfidev = new_cmfidev;
+
+cmfidev_add_regmap_unlock:
+        mutex_unlock(&cmfidev_mutex);
+
+        return err;
+}
+EXPORT_SYMBOL(tegra_camera_dev_mfi_add_regmap);
diff --git a/drivers/video/tegra/camera/tegra_camera_dev_mfi.h b/drivers/video/tegra/camera/tegra_camera_dev_mfi.h
new file mode 100644
index 000000000..675212502
--- /dev/null
+++ b/drivers/video/tegra/camera/tegra_camera_dev_mfi.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2015-2016, NVIDIA CORPORATION.  All rights reserved.
+
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __CAMERA_DEV_MFI_H__
+#define __CAMERA_DEV_MFI_H__
+
+#include <linux/list.h>
+#include <linux/i2c.h>
+#include <linux/regmap.h>
+
+#define CAMERA_MAX_NAME_LENGTH  32
+#define CAMERA_REGCACHE_MAX             (128)
+
+struct cam_reg {
+        u32 addr;
+        u32 val;
+};
+
+struct cam_i2c_msg {
+        struct i2c_msg msg;
+        u8 buf[8];
+};
+
+struct camera_mfi_dev {
+        char name[CAMERA_MAX_NAME_LENGTH];
+        struct regmap *regmap;
+        struct cam_reg reg[CAMERA_REGCACHE_MAX];
+        struct cam_reg prev_reg[CAMERA_REGCACHE_MAX];
+        struct i2c_client *i2c_client;
+        struct cam_i2c_msg msg[CAMERA_REGCACHE_MAX];
+        u32 num_used;
+        u32 prev_num_used;
+        struct list_head list;
+};
+
+struct mfi_cb_arg {
+        u8 vi_chan;
+};
+
+int tegra_camera_dev_mfi_init(void);
+void tegra_camera_dev_mfi_cb(void *stub);
+int tegra_camera_dev_mfi_clear(struct camera_mfi_dev *cmfidev);
+int tegra_camera_dev_mfi_wr_add(
+        struct camera_mfi_dev *cmfidev, u32 offset, u32 val);
+int tegra_camera_dev_mfi_wr_add_i2c(
+        struct camera_mfi_dev *cmfidev, struct i2c_msg *msg, int num);
+int tegra_camera_dev_mfi_add_regmap(
+        struct camera_mfi_dev **cmfidev, u8 *name, struct regmap *regmap);
+int tegra_camera_dev_mfi_add_i2cclient(
+        struct camera_mfi_dev **cmfidev, u8 *name,
+        struct i2c_client *i2c_client);
+
+#endif
+/* __CAMERA_DEV_MFI_H__ */
diff --git a/drivers/video/tegra/camera/tegra_camera_platform.c b/drivers/video/tegra/camera/tegra_camera_platform.c
new file mode 100644
index 000000000..206294c5b
--- /dev/null
+++ b/drivers/video/tegra/camera/tegra_camera_platform.c
@@ -0,0 +1,582 @@
+/*
+ * drivers/video/tegra/camera/tegra_camera_platform.c
+ *
+ * Copyright (c) 2015-2017, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/fs.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/module.h>
+#include <linux/miscdevice.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <soc/tegra/tegra_emc.h>
+
+#include "vi/vi.h"
+#include "tegra_camera_dev_mfi.h"
+#include <media/tegra_camera_platform.h>
+#include "camera_priv_defs.h"
+
+#define CAMDEV_NAME "tegra_camera_ctrl"
+
+/* Peak BPP for any of the YUV/Bayer formats */
+#define CAMERA_PEAK_BPP 2
+
+#define LANE_SPEED_1_GBPS 1000000000
+#define LANE_SPEED_1_5_GBPS 1500000000
+
+#if defined(CONFIG_TEGRA_BWMGR)
+#include <linux/platform/tegra/emc_bwmgr.h>
+#endif
+
+struct tegra_camera_info {
+        char devname[64];
+        atomic_t in_use;
+        struct device *dev;
+#if defined(CONFIG_TEGRA_BWMGR)
+        /* bandwidth manager handle */
+        struct tegra_bwmgr_client *bwmgr_handle;
+#endif
+        struct clk *emc;
+        struct clk *iso_emc;
+#if defined(CONFIG_TEGRA_ISOMGR)
+        tegra_isomgr_handle isomgr_handle;
+        u64 max_bw;
+#endif
+        struct mutex update_bw_lock;
+        u32 vi_mode_isobw;
+        u64 bypass_mode_isobw;
+        /* set max bw by default */
+        bool en_max_bw;
+};
+
+static const struct of_device_id tegra_camera_of_ids[] = {
+        { .compatible = "nvidia, tegra-camera-platform" },
+        { },
+};
+
+static struct miscdevice tegra_camera_misc;
+
+static int tegra_camera_isomgr_register(struct tegra_camera_info *info,
+                                        struct device *dev)
+{
+#if defined(CONFIG_TEGRA_ISOMGR)
+        int ret = 0;
+        u32 num_csi_lanes = 0;
+        u32 max_lane_speed = 0;
+        u32 bits_per_pixel = 0;
+        u32 vi_bpp = 0;
+        u64 vi_iso_bw = 0;
+        u32 vi_margin_pct = 0;
+        u32 max_pixel_rate = 0;
+        u32 isp_bpp = 0;
+        u64 isp_iso_bw = 0;
+        u32 isp_margin_pct = 0;
+        u32 tpg_max_iso = 0;
+        struct device_node *np = dev->of_node;
+
+        dev_dbg(info->dev, "%s++\n", __func__);
+
+        ret |= of_property_read_u32(np, "num_csi_lanes", &num_csi_lanes);
+        ret |= of_property_read_u32(np, "max_lane_speed", &max_lane_speed);
+        ret |= of_property_read_u32(np, "min_bits_per_pixel", &bits_per_pixel);
+        ret |= of_property_read_u32(np, "vi_peak_byte_per_pixel", &vi_bpp);
+        ret |= of_property_read_u32(np, "vi_bw_margin_pct", &vi_margin_pct);
+        ret |= of_property_read_u32(np, "max_pixel_rate", &max_pixel_rate);
+        ret |= of_property_read_u32(np, "isp_peak_byte_per_pixel", &isp_bpp);
+        ret |= of_property_read_u32(np, "isp_bw_margin_pct", &isp_margin_pct);
+
+        if (ret)
+                dev_info(info->dev, "%s: some fields not in DT.\n", __func__);
+
+        /*
+         * Use per-camera specifics to calculate ISO BW needed,
+         * which is smaller than the per-asic max.
+         *
+         * The formula for VI ISO BW is based on total number
+         * of active csi lanes when all cameras on the camera
+         * board are active.
+         *
+         * The formula for ISP ISO BW is based on max number
+         * of ISP's used in ISO mode given number of camera(s)
+         * on the camera board and the number of ISP's on the ASIC.
+         *
+         * The final ISO BW is based on the max of the two.
+         */
+        if (!bits_per_pixel) {
+                dev_err(info->dev, "bits_per_pixel is invalid\n");
+                return -EINVAL;
+        }
+        vi_iso_bw = ((num_csi_lanes * max_lane_speed) / bits_per_pixel)
+                                * vi_bpp * (100 + vi_margin_pct) / 100;
+        isp_iso_bw = max_pixel_rate * isp_bpp * (100 + isp_margin_pct) / 100;
+        if (vi_iso_bw > isp_iso_bw)
+                info->max_bw = vi_iso_bw;
+        else
+                info->max_bw = isp_iso_bw;
+
+        if (!info->max_bw) {
+                dev_err(info->dev, "%s: BW must be non-zero\n", __func__);
+                return -EINVAL;
+        }
+
+        ret = of_property_read_u32(np, "tpg_max_iso", &tpg_max_iso);
+        if (ret)
+                tpg_max_iso = 0;
+        else {
+                dev_info(info->dev, "%s tpg_max_iso = %uKBs\n", __func__,
+                                tpg_max_iso);
+                info->max_bw = max_t(u64, info->max_bw, tpg_max_iso);
+        }
+
+        dev_info(info->dev, "%s isp_iso_bw=%llu, vi_iso_bw=%llu, max_bw=%llu\n",
+                                __func__, isp_iso_bw, vi_iso_bw, info->max_bw);
+
+        /* Register with max possible BW for CAMERA usecases.*/
+        info->isomgr_handle = tegra_isomgr_register(
+                                        TEGRA_ISO_CLIENT_TEGRA_CAMERA,
+                                        info->max_bw,
+                                        NULL,   /* tegra_isomgr_renegotiate */
+                                        NULL);  /* *priv */
+
+        if (IS_ERR(info->isomgr_handle)) {
+                dev_err(info->dev,
+                        "%s: unable to register to isomgr\n",
+                                __func__);
+                return -ENOMEM;
+        }
+#endif
+
+        return 0;
+}
+
+static int tegra_camera_isomgr_unregister(struct tegra_camera_info *info)
+{
+#if defined(CONFIG_TEGRA_ISOMGR)
+        tegra_isomgr_unregister(info->isomgr_handle);
+        info->isomgr_handle = NULL;
+#endif
+
+        return 0;
+}
+
+static int tegra_camera_isomgr_request(
+                struct tegra_camera_info *info, uint iso_bw, uint lt)
+{
+#if defined(CONFIG_TEGRA_ISOMGR)
+        int ret = 0;
+
+        dev_dbg(info->dev,
+                "%s++ bw=%u, lt=%u\n", __func__, iso_bw, lt);
+
+        if (!info->isomgr_handle) {
+                dev_err(info->dev,
+                "%s: isomgr_handle is NULL\n",
+                __func__);
+                return -EINVAL;
+        }
+
+        /* return value of tegra_isomgr_reserve is dvfs latency in usec */
+        ret = tegra_isomgr_reserve(info->isomgr_handle,
+                                iso_bw, /* KB/sec */
+                                lt);    /* usec */
+        if (!ret) {
+                dev_err(info->dev,
+                "%s: failed to reserve %u KBps\n", __func__, iso_bw);
+                return -ENOMEM;
+        }
+
+        /* return value of tegra_isomgr_realize is dvfs latency in usec */
+        ret = tegra_isomgr_realize(info->isomgr_handle);
+        if (ret)
+                dev_dbg(info->dev,
+                "%s: tegra_camera isomgr latency is %d usec",
+                __func__, ret);
+        else {
+                dev_err(info->dev,
+                "%s: failed to realize %u KBps\n", __func__, iso_bw);
+                        return -ENOMEM;
+        }
+#endif
+
+        return 0;
+}
+
+static int tegra_camera_isomgr_release(struct tegra_camera_info *info)
+{
+#if defined(CONFIG_TEGRA_ISOMGR)
+        int ret = 0;
+
+        dev_dbg(info->dev, "%s++\n", __func__);
+
+        /* deallocate bypass mode isomgr bw */
+        info->bypass_mode_isobw = 0;
+        return vi_v4l2_update_isobw(0, 1);
+#endif
+
+        return 0;
+}
+int tegra_camera_emc_clk_enable(void)
+{
+#if defined(CONFIG_TEGRA_BWMGR)
+        return 0;
+#else
+        struct tegra_camera_info *info;
+        int ret = 0;
+
+        info = dev_get_drvdata(tegra_camera_misc.parent);
+        if (!info)
+                return -EINVAL;
+        ret = clk_prepare_enable(info->emc);
+        if (ret) {
+                dev_err(info->dev, "Cannot enable camera.emc\n");
+                return ret;
+        }
+
+        ret = clk_prepare_enable(info->iso_emc);
+        if (ret) {
+                dev_err(info->dev, "Cannot enable camera_iso.emc\n");
+                goto err_iso_emc;
+        }
+
+        return 0;
+err_iso_emc:
+        clk_disable_unprepare(info->emc);
+        return ret;
+#endif
+}
+EXPORT_SYMBOL(tegra_camera_emc_clk_enable);
+
+int tegra_camera_emc_clk_disable(void)
+{
+#if defined(CONFIG_TEGRA_BWMGR)
+        return 0;
+#else
+        struct tegra_camera_info *info;
+
+        info = dev_get_drvdata(tegra_camera_misc.parent);
+        if (!info)
+                return -EINVAL;
+        clk_disable_unprepare(info->emc);
+        clk_disable_unprepare(info->iso_emc);
+        return 0;
+#endif
+}
+EXPORT_SYMBOL(tegra_camera_emc_clk_disable);
+
+static int tegra_camera_open(struct inode *inode, struct file *file)
+{
+        struct tegra_camera_info *info;
+        struct miscdevice *mdev;
+
+        mdev = file->private_data;
+        info = dev_get_drvdata(mdev->parent);
+        file->private_data = info;
+#if defined(CONFIG_TEGRA_BWMGR)
+        /* get bandwidth manager handle if needed */
+        info->bwmgr_handle =
+                tegra_bwmgr_register(TEGRA_BWMGR_CLIENT_CAMERA);
+
+        /* set the initial rate */
+        if (IS_ERR_OR_NULL(info->bwmgr_handle)) {
+                info->bwmgr_handle = NULL;
+                return -ENODEV;
+        }
+        tegra_bwmgr_set_emc(info->bwmgr_handle, 0,
+                        TEGRA_BWMGR_SET_EMC_SHARED_BW);
+        return 0;
+#else
+        return tegra_camera_emc_clk_enable();
+#endif
+}
+
+static int tegra_camera_release(struct inode *inode, struct file *file)
+{
+
+        struct tegra_camera_info *info;
+        int ret;
+
+        info = file->private_data;
+#if defined(CONFIG_TEGRA_BWMGR)
+        tegra_bwmgr_unregister(info->bwmgr_handle);
+#else
+        tegra_camera_emc_clk_disable();
+#endif
+        /* nullify isomgr request */
+        ret = tegra_camera_isomgr_release(info);
+        if (ret) {
+                dev_err(info->dev,
+                "%s: failed to deallocate memory in isomgr\n",
+                __func__);
+                return -ENOMEM;
+        }
+        return ret;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static u64 vi_mode_d;
+static u64 bypass_mode_d;
+
+static int dbgfs_tegra_camera_init(void)
+{
+        struct dentry *dir;
+        struct dentry *val;
+
+        dir = debugfs_create_dir("tegra_camera_platform", NULL);
+        if (!dir)
+                return -ENOMEM;
+
+        val = debugfs_create_u64("vi", S_IRUGO, dir, &vi_mode_d);
+        if (!val)
+                return -ENOMEM;
+        val = debugfs_create_u64("scf", S_IRUGO, dir, &bypass_mode_d);
+        if (!val)
+                return -ENOMEM;
+        return 0;
+}
+#endif
+
+/*
+ * vi_kbyteps: Iso bw request from V4L2 vi driver.
+ * is_ioctl: Whether this function is called by userspace or kernel.
+ * This function aggregates V4L2 vi driver's request with userspace (SCF)
+ * iso bw request and submit total to isomgr.
+ */
+int vi_v4l2_update_isobw(u32 vi_kbyteps, u32 is_ioctl)
+{
+        struct tegra_camera_info *info;
+        unsigned long total_khz;
+        unsigned long bw;
+        int ret = 0;
+
+        if (tegra_camera_misc.parent == NULL) {
+                pr_info("driver not enabled, cannot update bw\n");
+                return -ENODEV;
+        }
+
+        info = dev_get_drvdata(tegra_camera_misc.parent);
+        if (!info)
+                return -ENODEV;
+        mutex_lock(&info->update_bw_lock);
+        if (!is_ioctl)
+                info->vi_mode_isobw = vi_kbyteps;
+        bw = info->bypass_mode_isobw + info->vi_mode_isobw;
+
+        /* Use Khz to prevent overflow */
+        total_khz = tegra_emc_bw_to_freq_req(bw);
+        total_khz = min(ULONG_MAX / 1000, total_khz);
+
+        dev_dbg(info->dev, "%s:Set iso bw %lu kbyteps at %lu KHz\n",
+                __func__, bw, total_khz);
+#if !defined(CONFIG_TEGRA_BWMGR)
+        ret = clk_set_rate(info->iso_emc, total_khz * 1000);
+        if (ret)
+                dev_err(info->dev, "%s:Failed to set iso bw\n",
+                        __func__);
+#endif
+        /*
+         * Request to ISOMGR.
+         * 3 usec is minimum time to switch PLL source.
+         * Let's put 4 usec as latency for now.
+         */
+        ret = tegra_camera_isomgr_request(info, bw, 4);
+        if (ret) {
+                dev_err(info->dev,
+                "%s: failed to reserve %lu KBps with isomgr\n",
+                __func__, bw);
+                mutex_unlock(&info->update_bw_lock);
+                return -ENOMEM;
+        }
+
+#ifdef CONFIG_DEBUG_FS
+        vi_mode_d = info->vi_mode_isobw;
+        bypass_mode_d = info->bypass_mode_isobw;
+#endif
+        mutex_unlock(&info->update_bw_lock);
+        return ret;
+}
+EXPORT_SYMBOL(vi_v4l2_update_isobw);
+
+static long tegra_camera_ioctl(struct file *file,
+        unsigned int cmd, unsigned long arg)
+{
+        int ret = 0;
+        struct tegra_camera_info *info;
+
+        info = file->private_data;
+
+        switch (_IOC_NR(cmd)) {
+        case _IOC_NR(TEGRA_CAMERA_IOCTL_SET_BW):
+        {
+                struct bw_info kcopy;
+                unsigned long mc_khz = 0;
+
+                memset(&kcopy, 0, sizeof(kcopy));
+
+                if (copy_from_user(&kcopy, (const void __user *)arg,
+                        sizeof(struct bw_info))) {
+                        dev_err(info->dev, "%s:Failed to get data from user\n",
+                                __func__);
+                        return -EFAULT;
+                }
+                /* Use Khz to prevent overflow */
+                mc_khz = tegra_emc_bw_to_freq_req(kcopy.bw);
+                mc_khz = min(ULONG_MAX / 1000, mc_khz);
+
+                if (kcopy.is_iso) {
+                        info->bypass_mode_isobw = kcopy.bw;
+                        ret = vi_v4l2_update_isobw(0, 1);
+                } else {
+                        dev_dbg(info->dev, "%s:Set bw %llu at %lu KHz\n",
+                                __func__, kcopy.bw, mc_khz);
+#if defined(CONFIG_TEGRA_BWMGR)
+                        ret = tegra_bwmgr_set_emc(info->bwmgr_handle,
+                                mc_khz*1000, TEGRA_BWMGR_SET_EMC_SHARED_BW);
+#else
+                        ret = clk_set_rate(info->emc, mc_khz * 1000);
+#endif
+                }
+                break;
+        }
+        default:
+                break;
+        }
+        return ret;
+}
+
+static const struct file_operations tegra_camera_ops = {
+        .owner = THIS_MODULE,
+        .open = tegra_camera_open,
+        .unlocked_ioctl = tegra_camera_ioctl,
+#ifdef CONFIG_COMPAT
+        .compat_ioctl = tegra_camera_ioctl,
+#endif
+        .release = tegra_camera_release,
+};
+
+
+
+static int tegra_camera_probe(struct platform_device *pdev)
+{
+        int ret;
+        struct tegra_camera_info *info;
+
+        dev_info(&pdev->dev, "%s:camera_platform_driver probe\n", __func__);
+
+        tegra_camera_misc.minor = MISC_DYNAMIC_MINOR;
+        tegra_camera_misc.name = CAMDEV_NAME;
+        tegra_camera_misc.fops = &tegra_camera_ops;
+        tegra_camera_misc.parent = &pdev->dev;
+
+        ret = misc_register(&tegra_camera_misc);
+        if (ret) {
+                dev_err(tegra_camera_misc.this_device,
+                        "register failed for %s\n",
+                        tegra_camera_misc.name);
+                return ret;
+        }
+        info = devm_kzalloc(tegra_camera_misc.this_device,
+                sizeof(struct tegra_camera_info), GFP_KERNEL);
+        if (!info)
+                return -ENOMEM;
+
+        strcpy(info->devname, tegra_camera_misc.name);
+        info->dev = tegra_camera_misc.this_device;
+
+#if !defined(CONFIG_TEGRA_BWMGR)
+        info->emc = devm_clk_get(info->dev, "emc");
+        if (IS_ERR(info->emc)) {
+                dev_err(info->dev, "Failed to get camera.emc\n");
+                return -EINVAL;
+        }
+        clk_set_rate(info->emc, 0);
+        info->iso_emc = devm_clk_get(info->dev, "iso.emc");
+        if (IS_ERR(info->iso_emc)) {
+                dev_err(info->dev, "Failed to get camera_iso.emc\n");
+                return -EINVAL;
+        }
+        clk_set_rate(info->iso_emc, 0);
+#endif
+        mutex_init(&info->update_bw_lock);
+        /* Register Camera as isomgr client. */
+        ret = tegra_camera_isomgr_register(info, &pdev->dev);
+        if (ret) {
+                dev_err(info->dev,
+                "%s: failed to register CAMERA as isomgr client\n",
+                __func__);
+                return -ENOMEM;
+        }
+
+        info->en_max_bw = of_property_read_bool(pdev->dev.of_node,
+                "default-max-bw");
+        if (info->en_max_bw == true) {
+                ret = tegra_camera_isomgr_request(info, info->max_bw, 4);
+                if (ret) {
+                        dev_err(info->dev,
+                        "%s: failed to request max bw\n", __func__);
+                        tegra_camera_isomgr_unregister(info);
+                        return -EFAULT;
+                }
+        }
+
+        tegra_camera_dev_mfi_init();
+        tegra_vi_register_mfi_cb(tegra_camera_dev_mfi_cb, NULL);
+
+        platform_set_drvdata(pdev, info);
+#ifdef CONFIG_DEBUG_FS
+        ret = dbgfs_tegra_camera_init();
+        if (ret)
+                dev_err(info->dev, "Fail to create debugfs");
+#endif
+        return 0;
+}
+
+static int tegra_camera_remove(struct platform_device *pdev)
+{
+        struct tegra_camera_info *info = platform_get_drvdata(pdev);
+
+        dev_info(&pdev->dev, "%s:camera_platform_driver remove\n", __func__);
+
+        /* deallocate isomgr bw */
+        if (info->en_max_bw)
+                tegra_camera_isomgr_request(info, 0, 4);
+
+        tegra_vi_unregister_mfi_cb();
+        tegra_camera_isomgr_unregister(info);
+        misc_deregister(&tegra_camera_misc);
+
+        return 0;
+}
+
+static struct platform_driver tegra_camera_driver = {
+        .probe = tegra_camera_probe,
+        .remove = tegra_camera_remove,
+        .driver = {
+                .owner = THIS_MODULE,
+                .name = "tegra_camera_platform",
+                .of_match_table = tegra_camera_of_ids
+        }
+};
+static int __init tegra_camera_init(void)
+{
+        return platform_driver_register(&tegra_camera_driver);
+}
+static void __exit tegra_camera_exit(void)
+{
+        platform_driver_unregister(&tegra_camera_driver);
+}
+
+module_init(tegra_camera_init);
+module_exit(tegra_camera_exit);
+