41 files changed, 2200 insertions, 2365 deletions
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index 338baa4c23ef..1778d320272e 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -27,19 +27,21 @@
 * REF_03 - Analog devices, ADV7604, Hardware Manual, Rev. F, August 2010
 */
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/i2c.h>
+#include <linux/v4l2-dv-timings.h>
-#include <linux/delay.h>
 #include <linux/videodev2.h>
 #include <linux/workqueue.h>
-#include <linux/v4l2-dv-timings.h>
-#include <media/v4l2-device.h>
+#include <media/adv7604.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
 #include <media/v4l2-dv-timings.h>
-#include <media/adv7604.h>
+#include <media/v4l2-of.h>
 static int debug;
 module_param(debug, int, 0644);
@@ -53,6 +55,76 @@ MODULE_LICENSE("GPL");
 /* ADV7604 system clock frequency */
 #define ADV7604_fsc (28636360)
+#define ADV7604_RGB_OUT                                 (1 << 1)
+#define ADV7604_OP_FORMAT_SEL_8BIT                      (0 << 0)
+#define ADV7604_OP_FORMAT_SEL_10BIT                     (1 << 0)
+#define ADV7604_OP_FORMAT_SEL_12BIT                     (2 << 0)
+#define ADV7604_OP_MODE_SEL_SDR_422                     (0 << 5)
+#define ADV7604_OP_MODE_SEL_DDR_422                     (1 << 5)
+#define ADV7604_OP_MODE_SEL_SDR_444                     (2 << 5)
+#define ADV7604_OP_MODE_SEL_DDR_444                     (3 << 5)
+#define ADV7604_OP_MODE_SEL_SDR_422_2X                  (4 << 5)
+#define ADV7604_OP_MODE_SEL_ADI_CM                      (5 << 5)
+#define ADV7604_OP_CH_SEL_GBR                           (0 << 5)
+#define ADV7604_OP_CH_SEL_GRB                           (1 << 5)
+#define ADV7604_OP_CH_SEL_BGR                           (2 << 5)
+#define ADV7604_OP_CH_SEL_RGB                           (3 << 5)
+#define ADV7604_OP_CH_SEL_BRG                           (4 << 5)
+#define ADV7604_OP_CH_SEL_RBG                           (5 << 5)
+#define ADV7604_OP_SWAP_CB_CR                           (1 << 0)
+enum adv7604_type {
+        ADV7604,
+        ADV7611,
+};
+struct adv7604_reg_seq {
+        unsigned int reg;
+        u8 val;
+};
+struct adv7604_format_info {
+        enum v4l2_mbus_pixelcode code;
+        u8 op_ch_sel;
+        bool rgb_out;
+        bool swap_cb_cr;
+        u8 op_format_sel;
+};
+struct adv7604_chip_info {
+        enum adv7604_type type;
+        bool has_afe;
+        unsigned int max_port;
+        unsigned int num_dv_ports;
+        unsigned int edid_enable_reg;
+        unsigned int edid_status_reg;
+        unsigned int lcf_reg;
+        unsigned int cable_det_mask;
+        unsigned int tdms_lock_mask;
+        unsigned int fmt_change_digital_mask;
+        const struct adv7604_format_info *formats;
+        unsigned int nformats;
+        void (*set_termination)(struct v4l2_subdev *sd, bool enable);
+        void (*setup_irqs)(struct v4l2_subdev *sd);
+        unsigned int (*read_hdmi_pixelclock)(struct v4l2_subdev *sd);
+        unsigned int (*read_cable_det)(struct v4l2_subdev *sd);
+        /* 0 = AFE, 1 = HDMI */
+        const struct adv7604_reg_seq *recommended_settings[2];
+        unsigned int num_recommended_settings[2];
+        unsigned long page_mask;
+};
 /*
 **********************************************************************
 *
@@ -60,13 +132,24 @@ MODULE_LICENSE("GPL");
 *
 **********************************************************************
 */
 struct adv7604_state {
+        const struct adv7604_chip_info *info;
        struct adv7604_platform_data pdata;
+        struct gpio_desc *hpd_gpio[4];
        struct v4l2_subdev sd;
-        struct media_pad pad;
+        struct media_pad pads[ADV7604_PAD_MAX];
+        unsigned int source_pad;
        struct v4l2_ctrl_handler hdl;
-        enum adv7604_input_port selected_input;
+        enum adv7604_pad selected_input;
        struct v4l2_dv_timings timings;
+        const struct adv7604_format_info *format;
        struct {
                u8 edid[256];
                u32 present;
@@ -80,18 +163,7 @@ struct adv7604_state {
        bool restart_stdi_once;
        /* i2c clients */
-        struct i2c_client *i2c_avlink;
+        struct i2c_client *i2c_clients[ADV7604_PAGE_MAX];
-        struct i2c_client *i2c_cec;
-        struct i2c_client *i2c_infoframe;
-        struct i2c_client *i2c_esdp;
-        struct i2c_client *i2c_dpp;
-        struct i2c_client *i2c_afe;
-        struct i2c_client *i2c_repeater;
-        struct i2c_client *i2c_edid;
-        struct i2c_client *i2c_hdmi;
-        struct i2c_client *i2c_test;
-        struct i2c_client *i2c_cp;
-        struct i2c_client *i2c_vdp;
        /* controls */
        struct v4l2_ctrl *detect_tx_5v_ctrl;
@@ -101,6 +173,11 @@ struct adv7604_state {
        struct v4l2_ctrl *rgb_quantization_range_ctrl;
 };
+static bool adv7604_has_afe(struct adv7604_state *state)
+{
+        return state->info->has_afe;
+}
 /* Supported CEA and DMT timings */
 static const struct v4l2_dv_timings adv7604_timings[] = {
        V4L2_DV_BT_CEA_720X480P59_94,
@@ -256,11 +333,6 @@ static inline struct adv7604_state *to_state(struct v4l2_subdev *sd)
        return container_of(sd, struct adv7604_state, sd);
 }
-static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
-{
-        return &container_of(ctrl->handler, struct adv7604_state, hdl)->sd;
-}
 static inline unsigned hblanking(const struct v4l2_bt_timings *t)
 {
        return V4L2_DV_BT_BLANKING_WIDTH(t);
@@ -298,14 +370,18 @@ static s32 adv_smbus_read_byte_data_check(struct i2c_client *client,
        return -EIO;
 }
-static s32 adv_smbus_read_byte_data(struct i2c_client *client, u8 command)
+static s32 adv_smbus_read_byte_data(struct adv7604_state *state,
+                                    enum adv7604_page page, u8 command)
 {
-        return adv_smbus_read_byte_data_check(client, command, true);
+        return adv_smbus_read_byte_data_check(state->i2c_clients[page],
+                                              command, true);
 }
-static s32 adv_smbus_write_byte_data(struct i2c_client *client,
+static s32 adv_smbus_write_byte_data(struct adv7604_state *state,
-                                        u8 command, u8 value)
+                                     enum adv7604_page page, u8 command,
+                                     u8 value)
 {
+        struct i2c_client *client = state->i2c_clients[page];
        union i2c_smbus_data data;
        int err;
        int i;
@@ -325,9 +401,11 @@ static s32 adv_smbus_write_byte_data(struct i2c_client *client,
        return err;
 }
-static s32 adv_smbus_write_i2c_block_data(struct i2c_client *client,
+static s32 adv_smbus_write_i2c_block_data(struct adv7604_state *state,
-               u8 command, unsigned length, const u8 *values)
+                                          enum adv7604_page page, u8 command,
+                                          unsigned length, const u8 *values)
 {
+        struct i2c_client *client = state->i2c_clients[page];
        union i2c_smbus_data data;
        if (length > I2C_SMBUS_BLOCK_MAX)
@@ -343,149 +421,150 @@ static s32 adv_smbus_write_i2c_block_data(struct i2c_client *client,
 static inline int io_read(struct v4l2_subdev *sd, u8 reg)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(client, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_IO, reg);
 }
 static inline int io_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(client, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_IO, reg, val);
 }
-static inline int io_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+static inline int io_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
 {
-        return io_write(sd, reg, (io_read(sd, reg) & mask) | val);
+        return io_write(sd, reg, (io_read(sd, reg) & ~mask) | val);
 }
 static inline int avlink_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_avlink, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_AVLINK, reg);
 }
 static inline int avlink_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_avlink, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_AVLINK, reg, val);
 }
 static inline int cec_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_cec, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_CEC, reg);
 }
 static inline int cec_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_cec, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_CEC, reg, val);
 }
-static inline int cec_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+static inline int cec_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
 {
-        return cec_write(sd, reg, (cec_read(sd, reg) & mask) | val);
+        return cec_write(sd, reg, (cec_read(sd, reg) & ~mask) | val);
 }
 static inline int infoframe_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_infoframe, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_INFOFRAME, reg);
 }
 static inline int infoframe_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_infoframe, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_INFOFRAME,
+                                         reg, val);
 }
 static inline int esdp_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_esdp, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_ESDP, reg);
 }
 static inline int esdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_esdp, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_ESDP, reg, val);
 }
 static inline int dpp_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_dpp, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_DPP, reg);
 }
 static inline int dpp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_dpp, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_DPP, reg, val);
 }
 static inline int afe_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_afe, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_AFE, reg);
 }
 static inline int afe_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_afe, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_AFE, reg, val);
 }
 static inline int rep_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_repeater, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_REP, reg);
 }
 static inline int rep_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_repeater, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_REP, reg, val);
 }
-static inline int rep_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+static inline int rep_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
 {
-        return rep_write(sd, reg, (rep_read(sd, reg) & mask) | val);
+        return rep_write(sd, reg, (rep_read(sd, reg) & ~mask) | val);
 }
 static inline int edid_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_edid, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_EDID, reg);
 }
 static inline int edid_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_edid, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_EDID, reg, val);
 }
 static inline int edid_read_block(struct v4l2_subdev *sd, unsigned len, u8 *val)
 {
        struct adv7604_state *state = to_state(sd);
-        struct i2c_client *client = state->i2c_edid;
+        struct i2c_client *client = state->i2c_clients[ADV7604_PAGE_EDID];
        u8 msgbuf0[1] = { 0 };
        u8 msgbuf1[256];
        struct i2c_msg msg[2] = {
@@ -518,11 +597,25 @@ static inline int edid_write_block(struct v4l2_subdev *sd,
        v4l2_dbg(2, debug, sd, "%s: write EDID block (%d byte)\n", __func__, len);
        for (i = 0; !err && i < len; i += I2C_SMBUS_BLOCK_MAX)
-                err = adv_smbus_write_i2c_block_data(state->i2c_edid, i,
+                err = adv_smbus_write_i2c_block_data(state, ADV7604_PAGE_EDID,
-                                I2C_SMBUS_BLOCK_MAX, val + i);
+                                i, I2C_SMBUS_BLOCK_MAX, val + i);
        return err;
 }
+static void adv7604_set_hpd(struct adv7604_state *state, unsigned int hpd)
+{
+        unsigned int i;
+        for (i = 0; i < state->info->num_dv_ports; ++i) {
+                if (IS_ERR(state->hpd_gpio[i]))
+                        continue;
+                gpiod_set_value_cansleep(state->hpd_gpio[i], hpd & BIT(i));
+        }
+        v4l2_subdev_notify(&state->sd, ADV7604_HOTPLUG, &hpd);
+}
 static void adv7604_delayed_work_enable_hotplug(struct work_struct *work)
 {
        struct delayed_work *dwork = to_delayed_work(work);
@@ -532,73 +625,210 @@ static void adv7604_delayed_work_enable_hotplug(struct work_struct *work)
        v4l2_dbg(2, debug, sd, "%s: enable hotplug\n", __func__);
-        v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)&state->edid.present);
+        adv7604_set_hpd(state, state->edid.present);
 }
 static inline int hdmi_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_hdmi, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_HDMI, reg);
+}
+static u16 hdmi_read16(struct v4l2_subdev *sd, u8 reg, u16 mask)
+{
+        return ((hdmi_read(sd, reg) << 8) | hdmi_read(sd, reg + 1)) & mask;
 }
 static inline int hdmi_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_hdmi, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_HDMI, reg, val);
 }
-static inline int hdmi_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+static inline int hdmi_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
 {
-        return hdmi_write(sd, reg, (hdmi_read(sd, reg) & mask) | val);
+        return hdmi_write(sd, reg, (hdmi_read(sd, reg) & ~mask) | val);
 }
 static inline int test_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_test, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_TEST, reg);
 }
 static inline int test_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_test, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_TEST, reg, val);
 }
 static inline int cp_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_cp, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_CP, reg);
+}
+static u16 cp_read16(struct v4l2_subdev *sd, u8 reg, u16 mask)
+{
+        return ((cp_read(sd, reg) << 8) | cp_read(sd, reg + 1)) & mask;
 }
 static inline int cp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_cp, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_CP, reg, val);
 }
-static inline int cp_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+static inline int cp_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
 {
-        return cp_write(sd, reg, (cp_read(sd, reg) & mask) | val);
+        return cp_write(sd, reg, (cp_read(sd, reg) & ~mask) | val);
 }
 static inline int vdp_read(struct v4l2_subdev *sd, u8 reg)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_read_byte_data(state->i2c_vdp, reg);
+        return adv_smbus_read_byte_data(state, ADV7604_PAGE_VDP, reg);
 }
 static inline int vdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
        struct adv7604_state *state = to_state(sd);
-        return adv_smbus_write_byte_data(state->i2c_vdp, reg, val);
+        return adv_smbus_write_byte_data(state, ADV7604_PAGE_VDP, reg, val);
+}
+#define ADV7604_REG(page, offset)       (((page) << 8) | (offset))
+#define ADV7604_REG_SEQ_TERM            0xffff
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int adv7604_read_reg(struct v4l2_subdev *sd, unsigned int reg)
+{
+        struct adv7604_state *state = to_state(sd);
+        unsigned int page = reg >> 8;
+        if (!(BIT(page) & state->info->page_mask))
+                return -EINVAL;
+        reg &= 0xff;
+        return adv_smbus_read_byte_data(state, page, reg);
+}
+#endif
+static int adv7604_write_reg(struct v4l2_subdev *sd, unsigned int reg, u8 val)
+{
+        struct adv7604_state *state = to_state(sd);
+        unsigned int page = reg >> 8;
+        if (!(BIT(page) & state->info->page_mask))
+                return -EINVAL;
+        reg &= 0xff;
+        return adv_smbus_write_byte_data(state, page, reg, val);
+}
+static void adv7604_write_reg_seq(struct v4l2_subdev *sd,
+                                  const struct adv7604_reg_seq *reg_seq)
+{
+        unsigned int i;
+        for (i = 0; reg_seq[i].reg != ADV7604_REG_SEQ_TERM; i++)
+                adv7604_write_reg(sd, reg_seq[i].reg, reg_seq[i].val);
+}
+/* -----------------------------------------------------------------------------
+ * Format helpers
+ */
+static const struct adv7604_format_info adv7604_formats[] = {
+        { V4L2_MBUS_FMT_RGB888_1X24, ADV7604_OP_CH_SEL_RGB, true, false,
+          ADV7604_OP_MODE_SEL_SDR_444 | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_YUYV8_2X8, ADV7604_OP_CH_SEL_RGB, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_YVYU8_2X8, ADV7604_OP_CH_SEL_RGB, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_YUYV10_2X10, ADV7604_OP_CH_SEL_RGB, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_10BIT },
+        { V4L2_MBUS_FMT_YVYU10_2X10, ADV7604_OP_CH_SEL_RGB, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_10BIT },
+        { V4L2_MBUS_FMT_YUYV12_2X12, ADV7604_OP_CH_SEL_RGB, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_12BIT },
+        { V4L2_MBUS_FMT_YVYU12_2X12, ADV7604_OP_CH_SEL_RGB, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_12BIT },
+        { V4L2_MBUS_FMT_UYVY8_1X16, ADV7604_OP_CH_SEL_RBG, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_VYUY8_1X16, ADV7604_OP_CH_SEL_RBG, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_YUYV8_1X16, ADV7604_OP_CH_SEL_RGB, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_YVYU8_1X16, ADV7604_OP_CH_SEL_RGB, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_UYVY10_1X20, ADV7604_OP_CH_SEL_RBG, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+        { V4L2_MBUS_FMT_VYUY10_1X20, ADV7604_OP_CH_SEL_RBG, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+        { V4L2_MBUS_FMT_YUYV10_1X20, ADV7604_OP_CH_SEL_RGB, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+        { V4L2_MBUS_FMT_YVYU10_1X20, ADV7604_OP_CH_SEL_RGB, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+        { V4L2_MBUS_FMT_UYVY12_1X24, ADV7604_OP_CH_SEL_RBG, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+        { V4L2_MBUS_FMT_VYUY12_1X24, ADV7604_OP_CH_SEL_RBG, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+        { V4L2_MBUS_FMT_YUYV12_1X24, ADV7604_OP_CH_SEL_RGB, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+        { V4L2_MBUS_FMT_YVYU12_1X24, ADV7604_OP_CH_SEL_RGB, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+};
+static const struct adv7604_format_info adv7611_formats[] = {
+        { V4L2_MBUS_FMT_RGB888_1X24, ADV7604_OP_CH_SEL_RGB, true, false,
+          ADV7604_OP_MODE_SEL_SDR_444 | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_YUYV8_2X8, ADV7604_OP_CH_SEL_RGB, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_YVYU8_2X8, ADV7604_OP_CH_SEL_RGB, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_YUYV12_2X12, ADV7604_OP_CH_SEL_RGB, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_12BIT },
+        { V4L2_MBUS_FMT_YVYU12_2X12, ADV7604_OP_CH_SEL_RGB, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_12BIT },
+        { V4L2_MBUS_FMT_UYVY8_1X16, ADV7604_OP_CH_SEL_RBG, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_VYUY8_1X16, ADV7604_OP_CH_SEL_RBG, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_YUYV8_1X16, ADV7604_OP_CH_SEL_RGB, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_YVYU8_1X16, ADV7604_OP_CH_SEL_RGB, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+        { V4L2_MBUS_FMT_UYVY12_1X24, ADV7604_OP_CH_SEL_RBG, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+        { V4L2_MBUS_FMT_VYUY12_1X24, ADV7604_OP_CH_SEL_RBG, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+        { V4L2_MBUS_FMT_YUYV12_1X24, ADV7604_OP_CH_SEL_RGB, false, false,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+        { V4L2_MBUS_FMT_YVYU12_1X24, ADV7604_OP_CH_SEL_RGB, false, true,
+          ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+};
+static const struct adv7604_format_info *
+adv7604_format_info(struct adv7604_state *state, enum v4l2_mbus_pixelcode code)
+{
+        unsigned int i;
+        for (i = 0; i < state->info->nformats; ++i) {
+                if (state->info->formats[i].code == code)
+                        return &state->info->formats[i];
+        }
+        return NULL;
 }
 /* ----------------------------------------------------------------------- */
@@ -607,18 +837,18 @@ static inline bool is_analog_input(struct v4l2_subdev *sd)
 {
        struct adv7604_state *state = to_state(sd);
-        return state->selected_input == ADV7604_INPUT_VGA_RGB ||
+        return state->selected_input == ADV7604_PAD_VGA_RGB ||
-               state->selected_input == ADV7604_INPUT_VGA_COMP;
+               state->selected_input == ADV7604_PAD_VGA_COMP;
 }
 static inline bool is_digital_input(struct v4l2_subdev *sd)
 {
        struct adv7604_state *state = to_state(sd);
-        return state->selected_input == ADV7604_INPUT_HDMI_PORT_A ||
+        return state->selected_input == ADV7604_PAD_HDMI_PORT_A ||
-               state->selected_input == ADV7604_INPUT_HDMI_PORT_B ||
+               state->selected_input == ADV7604_PAD_HDMI_PORT_B ||
-               state->selected_input == ADV7604_INPUT_HDMI_PORT_C ||
+               state->selected_input == ADV7604_PAD_HDMI_PORT_C ||
-               state->selected_input == ADV7604_INPUT_HDMI_PORT_D;
+               state->selected_input == ADV7604_PAD_HDMI_PORT_D;
 }
 /* ----------------------------------------------------------------------- */
@@ -644,119 +874,61 @@ static void adv7604_inv_register(struct v4l2_subdev *sd)
 static int adv7604_g_register(struct v4l2_subdev *sd,
                                        struct v4l2_dbg_register *reg)
 {
-        reg->size = 1;
+        int ret;
-        switch (reg->reg >> 8) {
-        case 0:
+        ret = adv7604_read_reg(sd, reg->reg);
-                reg->val = io_read(sd, reg->reg & 0xff);
+        if (ret < 0) {
-                break;
-        case 1:
-                reg->val = avlink_read(sd, reg->reg & 0xff);
-                break;
-        case 2:
-                reg->val = cec_read(sd, reg->reg & 0xff);
-                break;
-        case 3:
-                reg->val = infoframe_read(sd, reg->reg & 0xff);
-                break;
-        case 4:
-                reg->val = esdp_read(sd, reg->reg & 0xff);
-                break;
-        case 5:
-                reg->val = dpp_read(sd, reg->reg & 0xff);
-                break;
-        case 6:
-                reg->val = afe_read(sd, reg->reg & 0xff);
-                break;
-        case 7:
-                reg->val = rep_read(sd, reg->reg & 0xff);
-                break;
-        case 8:
-                reg->val = edid_read(sd, reg->reg & 0xff);
-                break;
-        case 9:
-                reg->val = hdmi_read(sd, reg->reg & 0xff);
-                break;
-        case 0xa:
-                reg->val = test_read(sd, reg->reg & 0xff);
-                break;
-        case 0xb:
-                reg->val = cp_read(sd, reg->reg & 0xff);
-                break;
-        case 0xc:
-                reg->val = vdp_read(sd, reg->reg & 0xff);
-                break;
-        default:
                v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
                adv7604_inv_register(sd);
-                break;
+                return ret;
        }
+        reg->size = 1;
+        reg->val = ret;
        return 0;
 }
 static int adv7604_s_register(struct v4l2_subdev *sd,
                                        const struct v4l2_dbg_register *reg)
 {
-        u8 val = reg->val & 0xff;
+        int ret;
-        switch (reg->reg >> 8) {
+        ret = adv7604_write_reg(sd, reg->reg, reg->val);
-        case 0:
+        if (ret < 0) {
-                io_write(sd, reg->reg & 0xff, val);
-                break;
-        case 1:
-                avlink_write(sd, reg->reg & 0xff, val);
-                break;
-        case 2:
-                cec_write(sd, reg->reg & 0xff, val);
-                break;
-        case 3:
-                infoframe_write(sd, reg->reg & 0xff, val);
-                break;
-        case 4:
-                esdp_write(sd, reg->reg & 0xff, val);
-                break;
-        case 5:
-                dpp_write(sd, reg->reg & 0xff, val);
-                break;
-        case 6:
-                afe_write(sd, reg->reg & 0xff, val);
-                break;
-        case 7:
-                rep_write(sd, reg->reg & 0xff, val);
-                break;
-        case 8:
-                edid_write(sd, reg->reg & 0xff, val);
-                break;
-        case 9:
-                hdmi_write(sd, reg->reg & 0xff, val);
-                break;
-        case 0xa:
-                test_write(sd, reg->reg & 0xff, val);
-                break;
-        case 0xb:
-                cp_write(sd, reg->reg & 0xff, val);
-                break;
-        case 0xc:
-                vdp_write(sd, reg->reg & 0xff, val);
-                break;
-        default:
                v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
                adv7604_inv_register(sd);
-                break;
+                return ret;
        }
        return 0;
 }
 #endif
+static unsigned int adv7604_read_cable_det(struct v4l2_subdev *sd)
+{
+        u8 value = io_read(sd, 0x6f);
+        return ((value & 0x10) >> 4)
+             | ((value & 0x08) >> 2)
+             | ((value & 0x04) << 0)
+             | ((value & 0x02) << 2);
+}
+static unsigned int adv7611_read_cable_det(struct v4l2_subdev *sd)
+{
+        u8 value = io_read(sd, 0x6f);
+        return value & 1;
+}
 static int adv7604_s_detect_tx_5v_ctrl(struct v4l2_subdev *sd)
 {
        struct adv7604_state *state = to_state(sd);
-        u8 reg_io_6f = io_read(sd, 0x6f);
+        const struct adv7604_chip_info *info = state->info;
        return v4l2_ctrl_s_ctrl(state->detect_tx_5v_ctrl,
-                        ((reg_io_6f & 0x10) >> 4) |
+                                info->read_cable_det(sd));
-                        ((reg_io_6f & 0x08) >> 2) |
-                        (reg_io_6f & 0x04) |
-                        ((reg_io_6f & 0x02) << 2));
 }
 static int find_and_set_predefined_video_timings(struct v4l2_subdev *sd,
@@ -787,11 +959,13 @@ static int configure_predefined_video_timings(struct v4l2_subdev *sd,
        v4l2_dbg(1, debug, sd, "%s", __func__);
-        /* reset to default values */
+        if (adv7604_has_afe(state)) {
-        io_write(sd, 0x16, 0x43);
+                /* reset to default values */
-        io_write(sd, 0x17, 0x5a);
+                io_write(sd, 0x16, 0x43);
+                io_write(sd, 0x17, 0x5a);
+        }
        /* disable embedded syncs for auto graphics mode */
-        cp_write_and_or(sd, 0x81, 0xef, 0x00);
+        cp_write_clr_set(sd, 0x81, 0x10, 0x00);
        cp_write(sd, 0x8f, 0x00);
        cp_write(sd, 0x90, 0x00);
        cp_write(sd, 0xa2, 0x00);
@@ -829,7 +1003,6 @@ static void configure_custom_video_timings(struct v4l2_subdev *sd,
                const struct v4l2_bt_timings *bt)
 {
        struct adv7604_state *state = to_state(sd);
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
        u32 width = htotal(bt);
        u32 height = vtotal(bt);
        u16 cp_start_sav = bt->hsync + bt->hbackporch - 4;
@@ -850,12 +1023,13 @@ static void configure_custom_video_timings(struct v4l2_subdev *sd,
                io_write(sd, 0x00, 0x07); /* video std */
                io_write(sd, 0x01, 0x02); /* prim mode */
                /* enable embedded syncs for auto graphics mode */
-                cp_write_and_or(sd, 0x81, 0xef, 0x10);
+                cp_write_clr_set(sd, 0x81, 0x10, 0x10);
                /* Should only be set in auto-graphics mode [REF_02, p. 91-92] */
                /* setup PLL_DIV_MAN_EN and PLL_DIV_RATIO */
                /* IO-map reg. 0x16 and 0x17 should be written in sequence */
-                if (adv_smbus_write_i2c_block_data(client, 0x16, 2, pll))
+                if (adv_smbus_write_i2c_block_data(state, ADV7604_PAGE_IO,
+                                                   0x16, 2, pll))
                        v4l2_err(sd, "writing to reg 0x16 and 0x17 failed\n");
                /* active video - horizontal timing */
@@ -906,7 +1080,8 @@ static void adv7604_set_offset(struct v4l2_subdev *sd, bool auto_offset, u16 off
        offset_buf[3] = offset_c & 0x0ff;
        /* Registers must be written in this order with no i2c access in between */
-        if (adv_smbus_write_i2c_block_data(state->i2c_cp, 0x77, 4, offset_buf))
+        if (adv_smbus_write_i2c_block_data(state, ADV7604_PAGE_CP,
+                                           0x77, 4, offset_buf))
                v4l2_err(sd, "%s: i2c error writing to CP reg 0x77, 0x78, 0x79, 0x7a\n", __func__);
 }
@@ -935,7 +1110,8 @@ static void adv7604_set_gain(struct v4l2_subdev *sd, bool auto_gain, u16 gain_a,
        gain_buf[3] = ((gain_c & 0x0ff));
        /* Registers must be written in this order with no i2c access in between */
-        if (adv_smbus_write_i2c_block_data(state->i2c_cp, 0x73, 4, gain_buf))
+        if (adv_smbus_write_i2c_block_data(state, ADV7604_PAGE_CP,
+                                           0x73, 4, gain_buf))
                v4l2_err(sd, "%s: i2c error writing to CP reg 0x73, 0x74, 0x75, 0x76\n", __func__);
 }
@@ -954,24 +1130,24 @@ static void set_rgb_quantization_range(struct v4l2_subdev *sd)
        switch (state->rgb_quantization_range) {
        case V4L2_DV_RGB_RANGE_AUTO:
-                if (state->selected_input == ADV7604_INPUT_VGA_RGB) {
+                if (state->selected_input == ADV7604_PAD_VGA_RGB) {
                        /* Receiving analog RGB signal
                         * Set RGB full range (0-255) */
-                        io_write_and_or(sd, 0x02, 0x0f, 0x10);
+                        io_write_clr_set(sd, 0x02, 0xf0, 0x10);
                        break;
                }
-                if (state->selected_input == ADV7604_INPUT_VGA_COMP) {
+                if (state->selected_input == ADV7604_PAD_VGA_COMP) {
                        /* Receiving analog YPbPr signal
                         * Set automode */
-                        io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+                        io_write_clr_set(sd, 0x02, 0xf0, 0xf0);
                        break;
                }
                if (hdmi_signal) {
                        /* Receiving HDMI signal
                         * Set automode */
-                        io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+                        io_write_clr_set(sd, 0x02, 0xf0, 0xf0);
                        break;
                }
@@ -980,10 +1156,10 @@ static void set_rgb_quantization_range(struct v4l2_subdev *sd)
                 * input format (CE/IT) in automatic mode */
                if (state->timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
                        /* RGB limited range (16-235) */
-                        io_write_and_or(sd, 0x02, 0x0f, 0x00);
+                        io_write_clr_set(sd, 0x02, 0xf0, 0x00);
                } else {
                        /* RGB full range (0-255) */
-                        io_write_and_or(sd, 0x02, 0x0f, 0x10);
+                        io_write_clr_set(sd, 0x02, 0xf0, 0x10);
                        if (is_digital_input(sd) && rgb_output) {
                                adv7604_set_offset(sd, false, 0x40, 0x40, 0x40);
@@ -994,25 +1170,25 @@ static void set_rgb_quantization_range(struct v4l2_subdev *sd)
                }
                break;
        case V4L2_DV_RGB_RANGE_LIMITED:
-                if (state->selected_input == ADV7604_INPUT_VGA_COMP) {
+                if (state->selected_input == ADV7604_PAD_VGA_COMP) {
                        /* YCrCb limited range (16-235) */
-                        io_write_and_or(sd, 0x02, 0x0f, 0x20);
+                        io_write_clr_set(sd, 0x02, 0xf0, 0x20);
                        break;
                }
                /* RGB limited range (16-235) */
-                io_write_and_or(sd, 0x02, 0x0f, 0x00);
+                io_write_clr_set(sd, 0x02, 0xf0, 0x00);
                break;
        case V4L2_DV_RGB_RANGE_FULL:
-                if (state->selected_input == ADV7604_INPUT_VGA_COMP) {
+                if (state->selected_input == ADV7604_PAD_VGA_COMP) {
                        /* YCrCb full range (0-255) */
-                        io_write_and_or(sd, 0x02, 0x0f, 0x60);
+                        io_write_clr_set(sd, 0x02, 0xf0, 0x60);
                        break;
                }
                /* RGB full range (0-255) */
-                io_write_and_or(sd, 0x02, 0x0f, 0x10);
+                io_write_clr_set(sd, 0x02, 0xf0, 0x10);
                if (is_analog_input(sd) || hdmi_signal)
                        break;
@@ -1030,7 +1206,9 @@ static void set_rgb_quantization_range(struct v4l2_subdev *sd)
 static int adv7604_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-        struct v4l2_subdev *sd = to_sd(ctrl);
+        struct v4l2_subdev *sd =
+                &container_of(ctrl->handler, struct adv7604_state, hdl)->sd;
        struct adv7604_state *state = to_state(sd);
        switch (ctrl->id) {
@@ -1051,6 +1229,8 @@ static int adv7604_s_ctrl(struct v4l2_ctrl *ctrl)
                set_rgb_quantization_range(sd);
                return 0;
        case V4L2_CID_ADV_RX_ANALOG_SAMPLING_PHASE:
+                if (!adv7604_has_afe(state))
+                        return -EINVAL;
                /* Set the analog sampling phase. This is needed to find the
                   best sampling phase for analog video: an application or
                   driver has to try a number of phases and analyze the picture
@@ -1060,7 +1240,7 @@ static int adv7604_s_ctrl(struct v4l2_ctrl *ctrl)
        case V4L2_CID_ADV_RX_FREE_RUN_COLOR_MANUAL:
                /* Use the default blue color for free running mode,
                   or supply your own. */
-                cp_write_and_or(sd, 0xbf, ~0x04, (ctrl->val << 2));
+                cp_write_clr_set(sd, 0xbf, 0x04, ctrl->val << 2);
                return 0;
        case V4L2_CID_ADV_RX_FREE_RUN_COLOR:
                cp_write(sd, 0xc0, (ctrl->val & 0xff0000) >> 16);
@@ -1088,7 +1268,10 @@ static inline bool no_signal_tmds(struct v4l2_subdev *sd)
 static inline bool no_lock_tmds(struct v4l2_subdev *sd)
 {
-        return (io_read(sd, 0x6a) & 0xe0) != 0xe0;
+        struct adv7604_state *state = to_state(sd);
+        const struct adv7604_chip_info *info = state->info;
+        return (io_read(sd, 0x6a) & info->tdms_lock_mask) != info->tdms_lock_mask;
 }
 static inline bool is_hdmi(struct v4l2_subdev *sd)
@@ -1098,6 +1281,15 @@ static inline bool is_hdmi(struct v4l2_subdev *sd)
 static inline bool no_lock_sspd(struct v4l2_subdev *sd)
 {
+        struct adv7604_state *state = to_state(sd);
+        /*
+         * Chips without a AFE don't expose registers for the SSPD, so just assume
+         * that we have a lock.
+         */
+        if (adv7604_has_afe(state))
+                return false;
        /* TODO channel 2 */
        return ((cp_read(sd, 0xb5) & 0xd0) != 0xd0);
 }
@@ -1127,6 +1319,11 @@ static inline bool no_signal(struct v4l2_subdev *sd)
 static inline bool no_lock_cp(struct v4l2_subdev *sd)
 {
+        struct adv7604_state *state = to_state(sd);
+        if (!adv7604_has_afe(state))
+                return false;
        /* CP has detected a non standard number of lines on the incoming
           video compared to what it is configured to receive by s_dv_timings */
        return io_read(sd, 0x12) & 0x01;
@@ -1195,28 +1392,40 @@ static int stdi2dv_timings(struct v4l2_subdev *sd,
        return -1;
 }
 static int read_stdi(struct v4l2_subdev *sd, struct stdi_readback *stdi)
 {
+        struct adv7604_state *state = to_state(sd);
+        const struct adv7604_chip_info *info = state->info;
+        u8 polarity;
        if (no_lock_stdi(sd) || no_lock_sspd(sd)) {
                v4l2_dbg(2, debug, sd, "%s: STDI and/or SSPD not locked\n", __func__);
                return -1;
        }
        /* read STDI */
-        stdi->bl = ((cp_read(sd, 0xb1) & 0x3f) << 8) | cp_read(sd, 0xb2);
+        stdi->bl = cp_read16(sd, 0xb1, 0x3fff);
-        stdi->lcf = ((cp_read(sd, 0xb3) & 0x7) << 8) | cp_read(sd, 0xb4);
+        stdi->lcf = cp_read16(sd, info->lcf_reg, 0x7ff);
        stdi->lcvs = cp_read(sd, 0xb3) >> 3;
        stdi->interlaced = io_read(sd, 0x12) & 0x10;
-        /* read SSPD */
+        if (adv7604_has_afe(state)) {
-        if ((cp_read(sd, 0xb5) & 0x03) == 0x01) {
+                /* read SSPD */
-                stdi->hs_pol = ((cp_read(sd, 0xb5) & 0x10) ?
+                polarity = cp_read(sd, 0xb5);
-                                ((cp_read(sd, 0xb5) & 0x08) ? '+' : '-') : 'x');
+                if ((polarity & 0x03) == 0x01) {
-                stdi->vs_pol = ((cp_read(sd, 0xb5) & 0x40) ?
+                        stdi->hs_pol = polarity & 0x10
-                                ((cp_read(sd, 0xb5) & 0x20) ? '+' : '-') : 'x');
+                                     ? (polarity & 0x08 ? '+' : '-') : 'x';
+                        stdi->vs_pol = polarity & 0x40
+                                     ? (polarity & 0x20 ? '+' : '-') : 'x';
+                } else {
+                        stdi->hs_pol = 'x';
+                        stdi->vs_pol = 'x';
+                }
        } else {
-                stdi->hs_pol = 'x';
+                polarity = hdmi_read(sd, 0x05);
-                stdi->vs_pol = 'x';
+                stdi->hs_pol = polarity & 0x20 ? '+' : '-';
+                stdi->vs_pol = polarity & 0x10 ? '+' : '-';
        }
        if (no_lock_stdi(sd) || no_lock_sspd(sd)) {
@@ -1243,8 +1452,14 @@ static int read_stdi(struct v4l2_subdev *sd, struct stdi_readback *stdi)
 static int adv7604_enum_dv_timings(struct v4l2_subdev *sd,
                        struct v4l2_enum_dv_timings *timings)
 {
+        struct adv7604_state *state = to_state(sd);
        if (timings->index >= ARRAY_SIZE(adv7604_timings) - 1)
                return -EINVAL;
+        if (timings->pad >= state->source_pad)
+                return -EINVAL;
        memset(timings->reserved, 0, sizeof(timings->reserved));
        timings->timings = adv7604_timings[timings->index];
        return 0;
@@ -1253,14 +1468,30 @@ static int adv7604_enum_dv_timings(struct v4l2_subdev *sd,
 static int adv7604_dv_timings_cap(struct v4l2_subdev *sd,
                        struct v4l2_dv_timings_cap *cap)
 {
+        struct adv7604_state *state = to_state(sd);
+        if (cap->pad >= state->source_pad)
+                return -EINVAL;
        cap->type = V4L2_DV_BT_656_1120;
        cap->bt.max_width = 1920;
        cap->bt.max_height = 1200;
        cap->bt.min_pixelclock = 25000000;
-        if (is_digital_input(sd))
+        switch (cap->pad) {
+        case ADV7604_PAD_HDMI_PORT_A:
+        case ADV7604_PAD_HDMI_PORT_B:
+        case ADV7604_PAD_HDMI_PORT_C:
+        case ADV7604_PAD_HDMI_PORT_D:
                cap->bt.max_pixelclock = 225000000;
-        else
+                break;
+        case ADV7604_PAD_VGA_RGB:
+        case ADV7604_PAD_VGA_COMP:
+        default:
                cap->bt.max_pixelclock = 170000000;
+                break;
+        }
        cap->bt.standards = V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
                         V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT;
        cap->bt.capabilities = V4L2_DV_BT_CAP_PROGRESSIVE |
@@ -1284,10 +1515,43 @@ static void adv7604_fill_optional_dv_timings_fields(struct v4l2_subdev *sd,
        }
 }
+static unsigned int adv7604_read_hdmi_pixelclock(struct v4l2_subdev *sd)
+{
+        unsigned int freq;
+        int a, b;
+        a = hdmi_read(sd, 0x06);
+        b = hdmi_read(sd, 0x3b);
+        if (a < 0 || b < 0)
+                return 0;
+        freq =  a * 1000000 + ((b & 0x30) >> 4) * 250000;
+        if (is_hdmi(sd)) {
+                /* adjust for deep color mode */
+                unsigned bits_per_channel = ((hdmi_read(sd, 0x0b) & 0x60) >> 4) + 8;
+                freq = freq * 8 / bits_per_channel;
+        }
+        return freq;
+}
+static unsigned int adv7611_read_hdmi_pixelclock(struct v4l2_subdev *sd)
+{
+        int a, b;
+        a = hdmi_read(sd, 0x51);
+        b = hdmi_read(sd, 0x52);
+        if (a < 0 || b < 0)
+                return 0;
+        return ((a << 1) | (b >> 7)) * 1000000 + (b & 0x7f) * 1000000 / 128;
+}
 static int adv7604_query_dv_timings(struct v4l2_subdev *sd,
                        struct v4l2_dv_timings *timings)
 {
        struct adv7604_state *state = to_state(sd);
+        const struct adv7604_chip_info *info = state->info;
        struct v4l2_bt_timings *bt = &timings->bt;
        struct stdi_readback stdi;
@@ -1311,44 +1575,25 @@ static int adv7604_query_dv_timings(struct v4l2_subdev *sd,
                V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
        if (is_digital_input(sd)) {
-                uint32_t freq;
                timings->type = V4L2_DV_BT_656_1120;
-                bt->width = (hdmi_read(sd, 0x07) & 0x0f) * 256 + hdmi_read(sd, 0x08);
+                /* FIXME: All masks are incorrect for ADV7611 */
-                bt->height = (hdmi_read(sd, 0x09) & 0x0f) * 256 + hdmi_read(sd, 0x0a);
+                bt->width = hdmi_read16(sd, 0x07, 0xfff);
-                freq = (hdmi_read(sd, 0x06) * 1000000) +
+                bt->height = hdmi_read16(sd, 0x09, 0xfff);
-                        ((hdmi_read(sd, 0x3b) & 0x30) >> 4) * 250000;
+                bt->pixelclock = info->read_hdmi_pixelclock(sd);
-                if (is_hdmi(sd)) {
+                bt->hfrontporch = hdmi_read16(sd, 0x20, 0x3ff);
-                        /* adjust for deep color mode */
+                bt->hsync = hdmi_read16(sd, 0x22, 0x3ff);
-                        unsigned bits_per_channel = ((hdmi_read(sd, 0x0b) & 0x60) >> 4) + 8;
+                bt->hbackporch = hdmi_read16(sd, 0x24, 0x3ff);
+                bt->vfrontporch = hdmi_read16(sd, 0x2a, 0x1fff) / 2;
-                        freq = freq * 8 / bits_per_channel;
+                bt->vsync = hdmi_read16(sd, 0x2e, 0x1fff) / 2;
-                }
+                bt->vbackporch = hdmi_read16(sd, 0x32, 0x1fff) / 2;
-                bt->pixelclock = freq;
-                bt->hfrontporch = (hdmi_read(sd, 0x20) & 0x03) * 256 +
-                        hdmi_read(sd, 0x21);
-                bt->hsync = (hdmi_read(sd, 0x22) & 0x03) * 256 +
-                        hdmi_read(sd, 0x23);
-                bt->hbackporch = (hdmi_read(sd, 0x24) & 0x03) * 256 +
-                        hdmi_read(sd, 0x25);
-                bt->vfrontporch = ((hdmi_read(sd, 0x2a) & 0x1f) * 256 +
-                        hdmi_read(sd, 0x2b)) / 2;
-                bt->vsync = ((hdmi_read(sd, 0x2e) & 0x1f) * 256 +
-                        hdmi_read(sd, 0x2f)) / 2;
-                bt->vbackporch = ((hdmi_read(sd, 0x32) & 0x1f) * 256 +
-                        hdmi_read(sd, 0x33)) / 2;
                bt->polarities = ((hdmi_read(sd, 0x05) & 0x10) ? V4L2_DV_VSYNC_POS_POL : 0) |
                        ((hdmi_read(sd, 0x05) & 0x20) ? V4L2_DV_HSYNC_POS_POL : 0);
                if (bt->interlaced == V4L2_DV_INTERLACED) {
-                        bt->height += (hdmi_read(sd, 0x0b) & 0x0f) * 256 +
+                        bt->height += hdmi_read16(sd, 0x0b, 0xfff);
-                                        hdmi_read(sd, 0x0c);
+                        bt->il_vfrontporch = hdmi_read16(sd, 0x2c, 0x1fff) / 2;
-                        bt->il_vfrontporch = ((hdmi_read(sd, 0x2c) & 0x1f) * 256 +
+                        bt->il_vsync = hdmi_read16(sd, 0x30, 0x1fff) / 2;
-                                        hdmi_read(sd, 0x2d)) / 2;
+                        bt->vbackporch = hdmi_read16(sd, 0x34, 0x1fff) / 2;
-                        bt->il_vsync = ((hdmi_read(sd, 0x30) & 0x1f) * 256 +
-                                        hdmi_read(sd, 0x31)) / 2;
-                        bt->vbackporch = ((hdmi_read(sd, 0x34) & 0x1f) * 256 +
-                                        hdmi_read(sd, 0x35)) / 2;
                }
                adv7604_fill_optional_dv_timings_fields(sd, timings);
        } else {
@@ -1378,11 +1623,11 @@ static int adv7604_query_dv_timings(struct v4l2_subdev *sd,
                                v4l2_dbg(1, debug, sd, "%s: restart STDI\n", __func__);
                                /* TODO restart STDI for Sync Channel 2 */
                                /* enter one-shot mode */
-                                cp_write_and_or(sd, 0x86, 0xf9, 0x00);
+                                cp_write_clr_set(sd, 0x86, 0x06, 0x00);
                                /* trigger STDI restart */
-                                cp_write_and_or(sd, 0x86, 0xf9, 0x04);
+                                cp_write_clr_set(sd, 0x86, 0x06, 0x04);
                                /* reset to continuous mode */
-                                cp_write_and_or(sd, 0x86, 0xf9, 0x02);
+                                cp_write_clr_set(sd, 0x86, 0x06, 0x02);
                                state->restart_stdi_once = false;
                                return -ENOLINK;
                        }
@@ -1441,7 +1686,7 @@ static int adv7604_s_dv_timings(struct v4l2_subdev *sd,
        state->timings = *timings;
-        cp_write(sd, 0x91, bt->interlaced ? 0x50 : 0x10);
+        cp_write_clr_set(sd, 0x91, 0x40, bt->interlaced ? 0x40 : 0x00);
        /* Use prim_mode and vid_std when available */
        err = configure_predefined_video_timings(sd, timings);
@@ -1468,6 +1713,16 @@ static int adv7604_g_dv_timings(struct v4l2_subdev *sd,
        return 0;
 }
+static void adv7604_set_termination(struct v4l2_subdev *sd, bool enable)
+{
+        hdmi_write(sd, 0x01, enable ? 0x00 : 0x78);
+}
+static void adv7611_set_termination(struct v4l2_subdev *sd, bool enable)
+{
+        hdmi_write(sd, 0x83, enable ? 0xfe : 0xff);
+}
 static void enable_input(struct v4l2_subdev *sd)
 {
        struct adv7604_state *state = to_state(sd);
@@ -1475,10 +1730,10 @@ static void enable_input(struct v4l2_subdev *sd)
        if (is_analog_input(sd)) {
                io_write(sd, 0x15, 0xb0);   /* Disable Tristate of Pins (no audio) */
        } else if (is_digital_input(sd)) {
-                hdmi_write_and_or(sd, 0x00, 0xfc, state->selected_input);
+                hdmi_write_clr_set(sd, 0x00, 0x03, state->selected_input);
-                hdmi_write(sd, 0x01, 0x00); /* Enable HDMI clock terminators */
+                state->info->set_termination(sd, true);
                io_write(sd, 0x15, 0xa0);   /* Disable Tristate of Pins */
-                hdmi_write_and_or(sd, 0x1a, 0xef, 0x00); /* Unmute audio */
+                hdmi_write_clr_set(sd, 0x1a, 0x10, 0x00); /* Unmute audio */
        } else {
                v4l2_dbg(2, debug, sd, "%s: Unknown port %d selected\n",
                                __func__, state->selected_input);
@@ -1487,67 +1742,36 @@ static void enable_input(struct v4l2_subdev *sd)
 static void disable_input(struct v4l2_subdev *sd)
 {
-        hdmi_write_and_or(sd, 0x1a, 0xef, 0x10); /* Mute audio */
+        struct adv7604_state *state = to_state(sd);
+        hdmi_write_clr_set(sd, 0x1a, 0x10, 0x10); /* Mute audio */
        msleep(16); /* 512 samples with >= 32 kHz sample rate [REF_03, c. 7.16.10] */
        io_write(sd, 0x15, 0xbe);   /* Tristate all outputs from video core */
-        hdmi_write(sd, 0x01, 0x78); /* Disable HDMI clock terminators */
+        state->info->set_termination(sd, false);
 }
 static void select_input(struct v4l2_subdev *sd)
 {
        struct adv7604_state *state = to_state(sd);
+        const struct adv7604_chip_info *info = state->info;
        if (is_analog_input(sd)) {
-                /* reset ADI recommended settings for HDMI: */
+                adv7604_write_reg_seq(sd, info->recommended_settings[0]);
-                /* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
-                hdmi_write(sd, 0x0d, 0x04); /* HDMI filter optimization */
-                hdmi_write(sd, 0x3d, 0x00); /* DDC bus active pull-up control */
-                hdmi_write(sd, 0x3e, 0x74); /* TMDS PLL optimization */
-                hdmi_write(sd, 0x4e, 0x3b); /* TMDS PLL optimization */
-                hdmi_write(sd, 0x57, 0x74); /* TMDS PLL optimization */
-                hdmi_write(sd, 0x58, 0x63); /* TMDS PLL optimization */
-                hdmi_write(sd, 0x8d, 0x18); /* equaliser */
-                hdmi_write(sd, 0x8e, 0x34); /* equaliser */
-                hdmi_write(sd, 0x93, 0x88); /* equaliser */
-                hdmi_write(sd, 0x94, 0x2e); /* equaliser */
-                hdmi_write(sd, 0x96, 0x00); /* enable automatic EQ changing */
                afe_write(sd, 0x00, 0x08); /* power up ADC */
                afe_write(sd, 0x01, 0x06); /* power up Analog Front End */
                afe_write(sd, 0xc8, 0x00); /* phase control */
-                /* set ADI recommended settings for digitizer */
-                /* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
-                afe_write(sd, 0x12, 0x7b); /* ADC noise shaping filter controls */
-                afe_write(sd, 0x0c, 0x1f); /* CP core gain controls */
-                cp_write(sd, 0x3e, 0x04); /* CP core pre-gain control */
-                cp_write(sd, 0xc3, 0x39); /* CP coast control. Graphics mode */
-                cp_write(sd, 0x40, 0x5c); /* CP core pre-gain control. Graphics mode */
        } else if (is_digital_input(sd)) {
                hdmi_write(sd, 0x00, state->selected_input & 0x03);
-                /* set ADI recommended settings for HDMI: */
+                adv7604_write_reg_seq(sd, info->recommended_settings[1]);
-                /* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
-                hdmi_write(sd, 0x0d, 0x84); /* HDMI filter optimization */
+                if (adv7604_has_afe(state)) {
-                hdmi_write(sd, 0x3d, 0x10); /* DDC bus active pull-up control */
+                        afe_write(sd, 0x00, 0xff); /* power down ADC */
-                hdmi_write(sd, 0x3e, 0x39); /* TMDS PLL optimization */
+                        afe_write(sd, 0x01, 0xfe); /* power down Analog Front End */
-                hdmi_write(sd, 0x4e, 0x3b); /* TMDS PLL optimization */
+                        afe_write(sd, 0xc8, 0x40); /* phase control */
-                hdmi_write(sd, 0x57, 0xb6); /* TMDS PLL optimization */
+                }
-                hdmi_write(sd, 0x58, 0x03); /* TMDS PLL optimization */
-                hdmi_write(sd, 0x8d, 0x18); /* equaliser */
-                hdmi_write(sd, 0x8e, 0x34); /* equaliser */
-                hdmi_write(sd, 0x93, 0x8b); /* equaliser */
-                hdmi_write(sd, 0x94, 0x2d); /* equaliser */
-                hdmi_write(sd, 0x96, 0x01); /* enable automatic EQ changing */
-                afe_write(sd, 0x00, 0xff); /* power down ADC */
-                afe_write(sd, 0x01, 0xfe); /* power down Analog Front End */
-                afe_write(sd, 0xc8, 0x40); /* phase control */
-                /* reset ADI recommended settings for digitizer */
-                /* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
-                afe_write(sd, 0x12, 0xfb); /* ADC noise shaping filter controls */
-                afe_write(sd, 0x0c, 0x0d); /* CP core gain controls */
                cp_write(sd, 0x3e, 0x00); /* CP core pre-gain control */
                cp_write(sd, 0xc3, 0x39); /* CP coast control. Graphics mode */
                cp_write(sd, 0x40, 0x80); /* CP core pre-gain control. Graphics mode */
@@ -1568,6 +1792,9 @@ static int adv7604_s_routing(struct v4l2_subdev *sd,
        if (input == state->selected_input)
                return 0;
+        if (input > state->info->max_port)
+                return -EINVAL;
        state->selected_input = input;
        disable_input(sd);
@@ -1579,34 +1806,139 @@ static int adv7604_s_routing(struct v4l2_subdev *sd,
        return 0;
 }
-static int adv7604_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index,
+static int adv7604_enum_mbus_code(struct v4l2_subdev *sd,
-                             enum v4l2_mbus_pixelcode *code)
+                                  struct v4l2_subdev_fh *fh,
+                                  struct v4l2_subdev_mbus_code_enum *code)
+{
+        struct adv7604_state *state = to_state(sd);
+        if (code->index >= state->info->nformats)
+                return -EINVAL;
+        code->code = state->info->formats[code->index].code;
+        return 0;
+}
+static void adv7604_fill_format(struct adv7604_state *state,
+                                struct v4l2_mbus_framefmt *format)
+{
+        memset(format, 0, sizeof(*format));
+        format->width = state->timings.bt.width;
+        format->height = state->timings.bt.height;
+        format->field = V4L2_FIELD_NONE;
+        if (state->timings.bt.standards & V4L2_DV_BT_STD_CEA861)
+                format->colorspace = (state->timings.bt.height <= 576) ?
+                        V4L2_COLORSPACE_SMPTE170M : V4L2_COLORSPACE_REC709;
+}
+/*
+ * Compute the op_ch_sel value required to obtain on the bus the component order
+ * corresponding to the selected format taking into account bus reordering
+ * applied by the board at the output of the device.
+ *
+ * The following table gives the op_ch_value from the format component order
+ * (expressed as op_ch_sel value in column) and the bus reordering (expressed as
+ * adv7604_bus_order value in row).
+ *
+ *           |  GBR(0)  GRB(1)  BGR(2)  RGB(3)  BRG(4)  RBG(5)
+ * ----------+-------------------------------------------------
+ * RGB (NOP) |  GBR     GRB     BGR     RGB     BRG     RBG
+ * GRB (1-2) |  BGR     RGB     GBR     GRB     RBG     BRG
+ * RBG (2-3) |  GRB     GBR     BRG     RBG     BGR     RGB
+ * BGR (1-3) |  RBG     BRG     RGB     BGR     GRB     GBR
+ * BRG (ROR) |  BRG     RBG     GRB     GBR     RGB     BGR
+ * GBR (ROL) |  RGB     BGR     RBG     BRG     GBR     GRB
+ */
+static unsigned int adv7604_op_ch_sel(struct adv7604_state *state)
+{
+#define _SEL(a,b,c,d,e,f)       { \
+        ADV7604_OP_CH_SEL_##a, ADV7604_OP_CH_SEL_##b, ADV7604_OP_CH_SEL_##c, \
+        ADV7604_OP_CH_SEL_##d, ADV7604_OP_CH_SEL_##e, ADV7604_OP_CH_SEL_##f }
+#define _BUS(x)                 [ADV7604_BUS_ORDER_##x]
+        static const unsigned int op_ch_sel[6][6] = {
+                _BUS(RGB) /* NOP */ = _SEL(GBR, GRB, BGR, RGB, BRG, RBG),
+                _BUS(GRB) /* 1-2 */ = _SEL(BGR, RGB, GBR, GRB, RBG, BRG),
+                _BUS(RBG) /* 2-3 */ = _SEL(GRB, GBR, BRG, RBG, BGR, RGB),
+                _BUS(BGR) /* 1-3 */ = _SEL(RBG, BRG, RGB, BGR, GRB, GBR),
+                _BUS(BRG) /* ROR */ = _SEL(BRG, RBG, GRB, GBR, RGB, BGR),
+                _BUS(GBR) /* ROL */ = _SEL(RGB, BGR, RBG, BRG, GBR, GRB),
+        };
+        return op_ch_sel[state->pdata.bus_order][state->format->op_ch_sel >> 5];
+}
+static void adv7604_setup_format(struct adv7604_state *state)
+{
+        struct v4l2_subdev *sd = &state->sd;
+        io_write_clr_set(sd, 0x02, 0x02,
+                        state->format->rgb_out ? ADV7604_RGB_OUT : 0);
+        io_write(sd, 0x03, state->format->op_format_sel |
+                 state->pdata.op_format_mode_sel);
+        io_write_clr_set(sd, 0x04, 0xe0, adv7604_op_ch_sel(state));
+        io_write_clr_set(sd, 0x05, 0x01,
+                        state->format->swap_cb_cr ? ADV7604_OP_SWAP_CB_CR : 0);
+}
+static int adv7604_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+                              struct v4l2_subdev_format *format)
 {
-        if (index)
+        struct adv7604_state *state = to_state(sd);
+        if (format->pad != state->source_pad)
                return -EINVAL;
-        /* Good enough for now */
-        *code = V4L2_MBUS_FMT_FIXED;
+        adv7604_fill_format(state, &format->format);
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+                struct v4l2_mbus_framefmt *fmt;
+                fmt = v4l2_subdev_get_try_format(fh, format->pad);
+                format->format.code = fmt->code;
+        } else {
+                format->format.code = state->format->code;
+        }
        return 0;
 }
-static int adv7604_g_mbus_fmt(struct v4l2_subdev *sd,
+static int adv7604_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-                struct v4l2_mbus_framefmt *fmt)
+                              struct v4l2_subdev_format *format)
 {
        struct adv7604_state *state = to_state(sd);
+        const struct adv7604_format_info *info;
-        fmt->width = state->timings.bt.width;
+        if (format->pad != state->source_pad)
-        fmt->height = state->timings.bt.height;
+                return -EINVAL;
-        fmt->code = V4L2_MBUS_FMT_FIXED;
-        fmt->field = V4L2_FIELD_NONE;
+        info = adv7604_format_info(state, format->format.code);
-        if (state->timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
+        if (info == NULL)
-                fmt->colorspace = (state->timings.bt.height <= 576) ?
+                info = adv7604_format_info(state, V4L2_MBUS_FMT_YUYV8_2X8);
-                        V4L2_COLORSPACE_SMPTE170M : V4L2_COLORSPACE_REC709;
+        adv7604_fill_format(state, &format->format);
+        format->format.code = info->code;
+        if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+                struct v4l2_mbus_framefmt *fmt;
+                fmt = v4l2_subdev_get_try_format(fh, format->pad);
+                fmt->code = format->format.code;
+        } else {
+                state->format = info;
+                adv7604_setup_format(state);
        }
        return 0;
 }
 static int adv7604_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 {
+        struct adv7604_state *state = to_state(sd);
+        const struct adv7604_chip_info *info = state->info;
        const u8 irq_reg_0x43 = io_read(sd, 0x43);
        const u8 irq_reg_0x6b = io_read(sd, 0x6b);
        const u8 irq_reg_0x70 = io_read(sd, 0x70);
@@ -1625,7 +1957,9 @@ static int adv7604_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
        /* format change */
        fmt_change = irq_reg_0x43 & 0x98;
-        fmt_change_digital = is_digital_input(sd) ? (irq_reg_0x6b & 0xc0) : 0;
+        fmt_change_digital = is_digital_input(sd)
+                           ? irq_reg_0x6b & info->fmt_change_digital_mask
+                           : 0;
        if (fmt_change || fmt_change_digital) {
                v4l2_dbg(1, debug, sd,
@@ -1647,7 +1981,7 @@ static int adv7604_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
        }
        /* tx 5v detect */
-        tx_5v = io_read(sd, 0x70) & 0x1e;
+        tx_5v = io_read(sd, 0x70) & info->cable_det_mask;
        if (tx_5v) {
                v4l2_dbg(1, debug, sd, "%s: tx_5v: 0x%x\n", __func__, tx_5v);
                io_write(sd, 0x71, tx_5v);
@@ -1663,7 +1997,7 @@ static int adv7604_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
        struct adv7604_state *state = to_state(sd);
        u8 *data = NULL;
-        if (edid->pad > ADV7604_EDID_PORT_D)
+        if (edid->pad > ADV7604_PAD_HDMI_PORT_D)
                return -EINVAL;
        if (edid->blocks == 0)
                return -EINVAL;
@@ -1678,10 +2012,10 @@ static int adv7604_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
                edid->blocks = state->edid.blocks;
        switch (edid->pad) {
-        case ADV7604_EDID_PORT_A:
+        case ADV7604_PAD_HDMI_PORT_A:
-        case ADV7604_EDID_PORT_B:
+        case ADV7604_PAD_HDMI_PORT_B:
-        case ADV7604_EDID_PORT_C:
+        case ADV7604_PAD_HDMI_PORT_C:
-        case ADV7604_EDID_PORT_D:
+        case ADV7604_PAD_HDMI_PORT_D:
                if (state->edid.present & (1 << edid->pad))
                        data = state->edid.edid;
                break;
@@ -1729,20 +2063,20 @@ static int get_edid_spa_location(const u8 *edid)
 static int adv7604_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
 {
        struct adv7604_state *state = to_state(sd);
+        const struct adv7604_chip_info *info = state->info;
        int spa_loc;
-        int tmp = 0;
        int err;
        int i;
-        if (edid->pad > ADV7604_EDID_PORT_D)
+        if (edid->pad > ADV7604_PAD_HDMI_PORT_D)
                return -EINVAL;
        if (edid->start_block != 0)
                return -EINVAL;
        if (edid->blocks == 0) {
                /* Disable hotplug and I2C access to EDID RAM from DDC port */
                state->edid.present &= ~(1 << edid->pad);
-                v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)&state->edid.present);
+                adv7604_set_hpd(state, state->edid.present);
-                rep_write_and_or(sd, 0x77, 0xf0, state->edid.present);
+                rep_write_clr_set(sd, info->edid_enable_reg, 0x0f, state->edid.present);
                /* Fall back to a 16:9 aspect ratio */
                state->aspect_ratio.numerator = 16;
@@ -1765,35 +2099,41 @@ static int adv7604_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
        /* Disable hotplug and I2C access to EDID RAM from DDC port */
        cancel_delayed_work_sync(&state->delayed_work_enable_hotplug);
-        v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)&tmp);
+        adv7604_set_hpd(state, 0);
-        rep_write_and_or(sd, 0x77, 0xf0, 0x00);
+        rep_write_clr_set(sd, info->edid_enable_reg, 0x0f, 0x00);
        spa_loc = get_edid_spa_location(edid->edid);
        if (spa_loc < 0)
                spa_loc = 0xc0; /* Default value [REF_02, p. 116] */
        switch (edid->pad) {
-        case ADV7604_EDID_PORT_A:
+        case ADV7604_PAD_HDMI_PORT_A:
                state->spa_port_a[0] = edid->edid[spa_loc];
                state->spa_port_a[1] = edid->edid[spa_loc + 1];
                break;
-        case ADV7604_EDID_PORT_B:
+        case ADV7604_PAD_HDMI_PORT_B:
                rep_write(sd, 0x70, edid->edid[spa_loc]);
                rep_write(sd, 0x71, edid->edid[spa_loc + 1]);
                break;
-        case ADV7604_EDID_PORT_C:
+        case ADV7604_PAD_HDMI_PORT_C:
                rep_write(sd, 0x72, edid->edid[spa_loc]);
                rep_write(sd, 0x73, edid->edid[spa_loc + 1]);
                break;
-        case ADV7604_EDID_PORT_D:
+        case ADV7604_PAD_HDMI_PORT_D:
                rep_write(sd, 0x74, edid->edid[spa_loc]);
                rep_write(sd, 0x75, edid->edid[spa_loc + 1]);
                break;
        default:
                return -EINVAL;
        }
-        rep_write(sd, 0x76, spa_loc & 0xff);
-        rep_write_and_or(sd, 0x77, 0xbf, (spa_loc >> 2) & 0x40);
+        if (info->type == ADV7604) {
+                rep_write(sd, 0x76, spa_loc & 0xff);
+                rep_write_clr_set(sd, 0x77, 0x40, (spa_loc & 0x100) >> 2);
+        } else {
+                /* FIXME: Where is the SPA location LSB register ? */
+                rep_write_clr_set(sd, 0x71, 0x01, (spa_loc & 0x100) >> 8);
+        }
        edid->edid[spa_loc] = state->spa_port_a[0];
        edid->edid[spa_loc + 1] = state->spa_port_a[1];
@@ -1812,10 +2152,10 @@ static int adv7604_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
        /* adv7604 calculates the checksums and enables I2C access to internal
           EDID RAM from DDC port. */
-        rep_write_and_or(sd, 0x77, 0xf0, state->edid.present);
+        rep_write_clr_set(sd, info->edid_enable_reg, 0x0f, state->edid.present);
        for (i = 0; i < 1000; i++) {
-                if (rep_read(sd, 0x7d) & state->edid.present)
+                if (rep_read(sd, info->edid_status_reg) & state->edid.present)
                        break;
                mdelay(1);
        }
@@ -1878,17 +2218,20 @@ static void print_avi_infoframe(struct v4l2_subdev *sd)
 static int adv7604_log_status(struct v4l2_subdev *sd)
 {
        struct adv7604_state *state = to_state(sd);
+        const struct adv7604_chip_info *info = state->info;
        struct v4l2_dv_timings timings;
        struct stdi_readback stdi;
        u8 reg_io_0x02 = io_read(sd, 0x02);
+        u8 edid_enabled;
+        u8 cable_det;
-        char *csc_coeff_sel_rb[16] = {
+        static const char * const csc_coeff_sel_rb[16] = {
                "bypassed", "YPbPr601 -> RGB", "reserved", "YPbPr709 -> RGB",
                "reserved", "RGB -> YPbPr601", "reserved", "RGB -> YPbPr709",
                "reserved", "YPbPr709 -> YPbPr601", "YPbPr601 -> YPbPr709",
                "reserved", "reserved", "reserved", "reserved", "manual"
        };
-        char *input_color_space_txt[16] = {
+        static const char * const input_color_space_txt[16] = {
                "RGB limited range (16-235)", "RGB full range (0-255)",
                "YCbCr Bt.601 (16-235)", "YCbCr Bt.709 (16-235)",
                "xvYCC Bt.601", "xvYCC Bt.709",
@@ -1896,12 +2239,12 @@ static int adv7604_log_status(struct v4l2_subdev *sd)
                "invalid", "invalid", "invalid", "invalid", "invalid",
                "invalid", "invalid", "automatic"
        };
-        char *rgb_quantization_range_txt[] = {
+        static const char * const rgb_quantization_range_txt[] = {
                "Automatic",
                "RGB limited range (16-235)",
                "RGB full range (0-255)",
        };
-        char *deep_color_mode_txt[4] = {
+        static const char * const deep_color_mode_txt[4] = {
                "8-bits per channel",
                "10-bits per channel",
                "12-bits per channel",
@@ -1910,20 +2253,22 @@ static int adv7604_log_status(struct v4l2_subdev *sd)
        v4l2_info(sd, "-----Chip status-----\n");
        v4l2_info(sd, "Chip power: %s\n", no_power(sd) ? "off" : "on");
+        edid_enabled = rep_read(sd, info->edid_status_reg);
        v4l2_info(sd, "EDID enabled port A: %s, B: %s, C: %s, D: %s\n",
-                        ((rep_read(sd, 0x7d) & 0x01) ? "Yes" : "No"),
+                        ((edid_enabled & 0x01) ? "Yes" : "No"),
-                        ((rep_read(sd, 0x7d) & 0x02) ? "Yes" : "No"),
+                        ((edid_enabled & 0x02) ? "Yes" : "No"),
-                        ((rep_read(sd, 0x7d) & 0x04) ? "Yes" : "No"),
+                        ((edid_enabled & 0x04) ? "Yes" : "No"),
-                        ((rep_read(sd, 0x7d) & 0x08) ? "Yes" : "No"));
+                        ((edid_enabled & 0x08) ? "Yes" : "No"));
        v4l2_info(sd, "CEC: %s\n", !!(cec_read(sd, 0x2a) & 0x01) ?
                        "enabled" : "disabled");
        v4l2_info(sd, "-----Signal status-----\n");
+        cable_det = info->read_cable_det(sd);
        v4l2_info(sd, "Cable detected (+5V power) port A: %s, B: %s, C: %s, D: %s\n",
-                        ((io_read(sd, 0x6f) & 0x10) ? "Yes" : "No"),
+                        ((cable_det & 0x01) ? "Yes" : "No"),
-                        ((io_read(sd, 0x6f) & 0x08) ? "Yes" : "No"),
+                        ((cable_det & 0x02) ? "Yes" : "No"),
-                        ((io_read(sd, 0x6f) & 0x04) ? "Yes" : "No"),
+                        ((cable_det & 0x04) ? "Yes" : "No"),
-                        ((io_read(sd, 0x6f) & 0x02) ? "Yes" : "No"));
+                        ((cable_det & 0x08) ? "Yes" : "No"));
        v4l2_info(sd, "TMDS signal detected: %s\n",
                        no_signal_tmds(sd) ? "false" : "true");
        v4l2_info(sd, "TMDS signal locked: %s\n",
@@ -2017,13 +2362,6 @@ static const struct v4l2_ctrl_ops adv7604_ctrl_ops = {
 static const struct v4l2_subdev_core_ops adv7604_core_ops = {
        .log_status = adv7604_log_status,
-        .g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
-        .try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
-        .s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
-        .g_ctrl = v4l2_subdev_g_ctrl,
-        .s_ctrl = v4l2_subdev_s_ctrl,
-        .queryctrl = v4l2_subdev_queryctrl,
-        .querymenu = v4l2_subdev_querymenu,
        .interrupt_service_routine = adv7604_isr,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
        .g_register = adv7604_g_register,
@@ -2037,17 +2375,16 @@ static const struct v4l2_subdev_video_ops adv7604_video_ops = {
        .s_dv_timings = adv7604_s_dv_timings,
        .g_dv_timings = adv7604_g_dv_timings,
        .query_dv_timings = adv7604_query_dv_timings,
-        .enum_dv_timings = adv7604_enum_dv_timings,
-        .dv_timings_cap = adv7604_dv_timings_cap,
-        .enum_mbus_fmt = adv7604_enum_mbus_fmt,
-        .g_mbus_fmt = adv7604_g_mbus_fmt,
-        .try_mbus_fmt = adv7604_g_mbus_fmt,
-        .s_mbus_fmt = adv7604_g_mbus_fmt,
 };
 static const struct v4l2_subdev_pad_ops adv7604_pad_ops = {
+        .enum_mbus_code = adv7604_enum_mbus_code,
+        .get_fmt = adv7604_get_format,
+        .set_fmt = adv7604_set_format,
        .get_edid = adv7604_get_edid,
        .set_edid = adv7604_set_edid,
+        .dv_timings_cap = adv7604_dv_timings_cap,
+        .enum_dv_timings = adv7604_enum_dv_timings,
 };
 static const struct v4l2_subdev_ops adv7604_ops = {
@@ -2096,6 +2433,7 @@ static const struct v4l2_ctrl_config adv7604_ctrl_free_run_color = {
 static int adv7604_core_init(struct v4l2_subdev *sd)
 {
        struct adv7604_state *state = to_state(sd);
+        const struct adv7604_chip_info *info = state->info;
        struct adv7604_platform_data *pdata = &state->pdata;
        hdmi_write(sd, 0x48,
@@ -2104,28 +2442,33 @@ static int adv7604_core_init(struct v4l2_subdev *sd)
        disable_input(sd);
+        if (pdata->default_input >= 0 &&
+            pdata->default_input < state->source_pad) {
+                state->selected_input = pdata->default_input;
+                select_input(sd);
+                enable_input(sd);
+        }
        /* power */
        io_write(sd, 0x0c, 0x42);   /* Power up part and power down VDP */
        io_write(sd, 0x0b, 0x44);   /* Power down ESDP block */
        cp_write(sd, 0xcf, 0x01);   /* Power down macrovision */
        /* video format */
-        io_write_and_or(sd, 0x02, 0xf0,
+        io_write_clr_set(sd, 0x02, 0x0f,
                        pdata->alt_gamma << 3 |
                        pdata->op_656_range << 2 |
-                        pdata->rgb_out << 1 |
                        pdata->alt_data_sat << 0);
-        io_write(sd, 0x03, pdata->op_format_sel);
+        io_write_clr_set(sd, 0x05, 0x0e, pdata->blank_data << 3 |
-        io_write_and_or(sd, 0x04, 0x1f, pdata->op_ch_sel << 5);
+                        pdata->insert_av_codes << 2 |
-        io_write_and_or(sd, 0x05, 0xf0, pdata->blank_data << 3 |
+                        pdata->replicate_av_codes << 1);
-                                        pdata->insert_av_codes << 2 |
+        adv7604_setup_format(state);
-                                        pdata->replicate_av_codes << 1 |
-                                        pdata->invert_cbcr << 0);
        cp_write(sd, 0x69, 0x30);   /* Enable CP CSC */
        /* VS, HS polarities */
-        io_write(sd, 0x06, 0xa0 | pdata->inv_vs_pol << 2 | pdata->inv_hs_pol << 1);
+        io_write(sd, 0x06, 0xa0 | pdata->inv_vs_pol << 2 |
+                 pdata->inv_hs_pol << 1 | pdata->inv_llc_pol);
        /* Adjust drive strength */
        io_write(sd, 0x14, 0x40 | pdata->dr_str_data << 4 |
@@ -2142,52 +2485,46 @@ static int adv7604_core_init(struct v4l2_subdev *sd)
                                     for digital formats */
        /* HDMI audio */
-        hdmi_write_and_or(sd, 0x15, 0xfc, 0x03); /* Mute on FIFO over-/underflow [REF_01, c. 1.2.18] */
+        hdmi_write_clr_set(sd, 0x15, 0x03, 0x03); /* Mute on FIFO over-/underflow [REF_01, c. 1.2.18] */
-        hdmi_write_and_or(sd, 0x1a, 0xf1, 0x08); /* Wait 1 s before unmute */
+        hdmi_write_clr_set(sd, 0x1a, 0x0e, 0x08); /* Wait 1 s before unmute */
-        hdmi_write_and_or(sd, 0x68, 0xf9, 0x06); /* FIFO reset on over-/underflow [REF_01, c. 1.2.19] */
+        hdmi_write_clr_set(sd, 0x68, 0x06, 0x06); /* FIFO reset on over-/underflow [REF_01, c. 1.2.19] */
        /* TODO from platform data */
        afe_write(sd, 0xb5, 0x01);  /* Setting MCLK to 256Fs */
-        afe_write(sd, 0x02, pdata->ain_sel); /* Select analog input muxing mode */
+        if (adv7604_has_afe(state)) {
-        io_write_and_or(sd, 0x30, ~(1 << 4), pdata->output_bus_lsb_to_msb << 4);
+                afe_write(sd, 0x02, pdata->ain_sel); /* Select analog input muxing mode */
+                io_write_clr_set(sd, 0x30, 1 << 4, pdata->output_bus_lsb_to_msb << 4);
+        }
        /* interrupts */
-        io_write(sd, 0x40, 0xc2); /* Configure INT1 */
+        io_write(sd, 0x40, 0xc0 | pdata->int1_config); /* Configure INT1 */
-        io_write(sd, 0x41, 0xd7); /* STDI irq for any change, disable INT2 */
        io_write(sd, 0x46, 0x98); /* Enable SSPD, STDI and CP unlocked interrupts */
-        io_write(sd, 0x6e, 0xc1); /* Enable V_LOCKED, DE_REGEN_LCK, HDMI_MODE interrupts */
+        io_write(sd, 0x6e, info->fmt_change_digital_mask); /* Enable V_LOCKED and DE_REGEN_LCK interrupts */
-        io_write(sd, 0x73, 0x1e); /* Enable CABLE_DET_A_ST (+5v) interrupts */
+        io_write(sd, 0x73, info->cable_det_mask); /* Enable cable detection (+5v) interrupts */
+        info->setup_irqs(sd);
        return v4l2_ctrl_handler_setup(sd->ctrl_handler);
 }
+static void adv7604_setup_irqs(struct v4l2_subdev *sd)
+{
+        io_write(sd, 0x41, 0xd7); /* STDI irq for any change, disable INT2 */
+}
+static void adv7611_setup_irqs(struct v4l2_subdev *sd)
+{
+        io_write(sd, 0x41, 0xd0); /* STDI irq for any change, disable INT2 */
+}
 static void adv7604_unregister_clients(struct adv7604_state *state)
 {
-        if (state->i2c_avlink)
+        unsigned int i;
-                i2c_unregister_device(state->i2c_avlink);
-        if (state->i2c_cec)
+        for (i = 1; i < ARRAY_SIZE(state->i2c_clients); ++i) {
-                i2c_unregister_device(state->i2c_cec);
+                if (state->i2c_clients[i])
-        if (state->i2c_infoframe)
+                        i2c_unregister_device(state->i2c_clients[i]);
-                i2c_unregister_device(state->i2c_infoframe);
+        }
-        if (state->i2c_esdp)
-                i2c_unregister_device(state->i2c_esdp);
-        if (state->i2c_dpp)
-                i2c_unregister_device(state->i2c_dpp);
-        if (state->i2c_afe)
-                i2c_unregister_device(state->i2c_afe);
-        if (state->i2c_repeater)
-                i2c_unregister_device(state->i2c_repeater);
-        if (state->i2c_edid)
-                i2c_unregister_device(state->i2c_edid);
-        if (state->i2c_hdmi)
-                i2c_unregister_device(state->i2c_hdmi);
-        if (state->i2c_test)
-                i2c_unregister_device(state->i2c_test);
-        if (state->i2c_cp)
-                i2c_unregister_device(state->i2c_cp);
-        if (state->i2c_vdp)
-                i2c_unregister_device(state->i2c_vdp);
 }
 static struct i2c_client *adv7604_dummy_client(struct v4l2_subdev *sd,
@@ -2200,15 +2537,219 @@ static struct i2c_client *adv7604_dummy_client(struct v4l2_subdev *sd,
        return i2c_new_dummy(client->adapter, io_read(sd, io_reg) >> 1);
 }
+static const struct adv7604_reg_seq adv7604_recommended_settings_afe[] = {
+        /* reset ADI recommended settings for HDMI: */
+        /* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x0d), 0x04 }, /* HDMI filter optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x0d), 0x04 }, /* HDMI filter optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x3d), 0x00 }, /* DDC bus active pull-up control */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x3e), 0x74 }, /* TMDS PLL optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x4e), 0x3b }, /* TMDS PLL optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x57), 0x74 }, /* TMDS PLL optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x58), 0x63 }, /* TMDS PLL optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x8d), 0x18 }, /* equaliser */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x8e), 0x34 }, /* equaliser */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x93), 0x88 }, /* equaliser */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x94), 0x2e }, /* equaliser */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x96), 0x00 }, /* enable automatic EQ changing */
+        /* set ADI recommended settings for digitizer */
+        /* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
+        { ADV7604_REG(ADV7604_PAGE_AFE, 0x12), 0x7b }, /* ADC noise shaping filter controls */
+        { ADV7604_REG(ADV7604_PAGE_AFE, 0x0c), 0x1f }, /* CP core gain controls */
+        { ADV7604_REG(ADV7604_PAGE_CP, 0x3e), 0x04 }, /* CP core pre-gain control */
+        { ADV7604_REG(ADV7604_PAGE_CP, 0xc3), 0x39 }, /* CP coast control. Graphics mode */
+        { ADV7604_REG(ADV7604_PAGE_CP, 0x40), 0x5c }, /* CP core pre-gain control. Graphics mode */
+        { ADV7604_REG_SEQ_TERM, 0 },
+};
+static const struct adv7604_reg_seq adv7604_recommended_settings_hdmi[] = {
+        /* set ADI recommended settings for HDMI: */
+        /* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x0d), 0x84 }, /* HDMI filter optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x3d), 0x10 }, /* DDC bus active pull-up control */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x3e), 0x39 }, /* TMDS PLL optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x4e), 0x3b }, /* TMDS PLL optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x57), 0xb6 }, /* TMDS PLL optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x58), 0x03 }, /* TMDS PLL optimization */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x8d), 0x18 }, /* equaliser */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x8e), 0x34 }, /* equaliser */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x93), 0x8b }, /* equaliser */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x94), 0x2d }, /* equaliser */
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x96), 0x01 }, /* enable automatic EQ changing */
+        /* reset ADI recommended settings for digitizer */
+        /* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
+        { ADV7604_REG(ADV7604_PAGE_AFE, 0x12), 0xfb }, /* ADC noise shaping filter controls */
+        { ADV7604_REG(ADV7604_PAGE_AFE, 0x0c), 0x0d }, /* CP core gain controls */
+        { ADV7604_REG_SEQ_TERM, 0 },
+};
+static const struct adv7604_reg_seq adv7611_recommended_settings_hdmi[] = {
+        { ADV7604_REG(ADV7604_PAGE_CP, 0x6c), 0x00 },
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x6f), 0x0c },
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x87), 0x70 },
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x57), 0xda },
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x58), 0x01 },
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x03), 0x98 },
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x4c), 0x44 },
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x8d), 0x04 },
+        { ADV7604_REG(ADV7604_PAGE_HDMI, 0x8e), 0x1e },
+        { ADV7604_REG_SEQ_TERM, 0 },
+};
+static const struct adv7604_chip_info adv7604_chip_info[] = {
+        [ADV7604] = {
+                .type = ADV7604,
+                .has_afe = true,
+                .max_port = ADV7604_PAD_VGA_COMP,
+                .num_dv_ports = 4,
+                .edid_enable_reg = 0x77,
+                .edid_status_reg = 0x7d,
+                .lcf_reg = 0xb3,
+                .tdms_lock_mask = 0xe0,
+                .cable_det_mask = 0x1e,
+                .fmt_change_digital_mask = 0xc1,
+                .formats = adv7604_formats,
+                .nformats = ARRAY_SIZE(adv7604_formats),
+                .set_termination = adv7604_set_termination,
+                .setup_irqs = adv7604_setup_irqs,
+                .read_hdmi_pixelclock = adv7604_read_hdmi_pixelclock,
+                .read_cable_det = adv7604_read_cable_det,
+                .recommended_settings = {
+                    [0] = adv7604_recommended_settings_afe,
+                    [1] = adv7604_recommended_settings_hdmi,
+                },
+                .num_recommended_settings = {
+                    [0] = ARRAY_SIZE(adv7604_recommended_settings_afe),
+                    [1] = ARRAY_SIZE(adv7604_recommended_settings_hdmi),
+                },
+                .page_mask = BIT(ADV7604_PAGE_IO) | BIT(ADV7604_PAGE_AVLINK) |
+                        BIT(ADV7604_PAGE_CEC) | BIT(ADV7604_PAGE_INFOFRAME) |
+                        BIT(ADV7604_PAGE_ESDP) | BIT(ADV7604_PAGE_DPP) |
+                        BIT(ADV7604_PAGE_AFE) | BIT(ADV7604_PAGE_REP) |
+                        BIT(ADV7604_PAGE_EDID) | BIT(ADV7604_PAGE_HDMI) |
+                        BIT(ADV7604_PAGE_TEST) | BIT(ADV7604_PAGE_CP) |
+                        BIT(ADV7604_PAGE_VDP),
+        },
+        [ADV7611] = {
+                .type = ADV7611,
+                .has_afe = false,
+                .max_port = ADV7604_PAD_HDMI_PORT_A,
+                .num_dv_ports = 1,
+                .edid_enable_reg = 0x74,
+                .edid_status_reg = 0x76,
+                .lcf_reg = 0xa3,
+                .tdms_lock_mask = 0x43,
+                .cable_det_mask = 0x01,
+                .fmt_change_digital_mask = 0x03,
+                .formats = adv7611_formats,
+                .nformats = ARRAY_SIZE(adv7611_formats),
+                .set_termination = adv7611_set_termination,
+                .setup_irqs = adv7611_setup_irqs,
+                .read_hdmi_pixelclock = adv7611_read_hdmi_pixelclock,
+                .read_cable_det = adv7611_read_cable_det,
+                .recommended_settings = {
+                    [1] = adv7611_recommended_settings_hdmi,
+                },
+                .num_recommended_settings = {
+                    [1] = ARRAY_SIZE(adv7611_recommended_settings_hdmi),
+                },
+                .page_mask = BIT(ADV7604_PAGE_IO) | BIT(ADV7604_PAGE_CEC) |
+                        BIT(ADV7604_PAGE_INFOFRAME) | BIT(ADV7604_PAGE_AFE) |
+                        BIT(ADV7604_PAGE_REP) |  BIT(ADV7604_PAGE_EDID) |
+                        BIT(ADV7604_PAGE_HDMI) | BIT(ADV7604_PAGE_CP),
+        },
+};
+static struct i2c_device_id adv7604_i2c_id[] = {
+        { "adv7604", (kernel_ulong_t)&adv7604_chip_info[ADV7604] },
+        { "adv7611", (kernel_ulong_t)&adv7604_chip_info[ADV7611] },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, adv7604_i2c_id);
+static struct of_device_id adv7604_of_id[] __maybe_unused = {
+        { .compatible = "adi,adv7611", .data = &adv7604_chip_info[ADV7611] },
+        { }
+};
+MODULE_DEVICE_TABLE(of, adv7604_of_id);
+static int adv7604_parse_dt(struct adv7604_state *state)
+{
+        struct v4l2_of_endpoint bus_cfg;
+        struct device_node *endpoint;
+        struct device_node *np;
+        unsigned int flags;
+        np = state->i2c_clients[ADV7604_PAGE_IO]->dev.of_node;
+        /* Parse the endpoint. */
+        endpoint = of_graph_get_next_endpoint(np, NULL);
+        if (!endpoint)
+                return -EINVAL;
+        v4l2_of_parse_endpoint(endpoint, &bus_cfg);
+        of_node_put(endpoint);
+        flags = bus_cfg.bus.parallel.flags;
+        if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+                state->pdata.inv_hs_pol = 1;
+        if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+                state->pdata.inv_vs_pol = 1;
+        if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+                state->pdata.inv_llc_pol = 1;
+        if (bus_cfg.bus_type == V4L2_MBUS_BT656) {
+                state->pdata.insert_av_codes = 1;
+                state->pdata.op_656_range = 1;
+        }
+        /* Disable the interrupt for now as no DT-based board uses it. */
+        state->pdata.int1_config = ADV7604_INT1_CONFIG_DISABLED;
+        /* Use the default I2C addresses. */
+        state->pdata.i2c_addresses[ADV7604_PAGE_AVLINK] = 0x42;
+        state->pdata.i2c_addresses[ADV7604_PAGE_CEC] = 0x40;
+        state->pdata.i2c_addresses[ADV7604_PAGE_INFOFRAME] = 0x3e;
+        state->pdata.i2c_addresses[ADV7604_PAGE_ESDP] = 0x38;
+        state->pdata.i2c_addresses[ADV7604_PAGE_DPP] = 0x3c;
+        state->pdata.i2c_addresses[ADV7604_PAGE_AFE] = 0x26;
+        state->pdata.i2c_addresses[ADV7604_PAGE_REP] = 0x32;
+        state->pdata.i2c_addresses[ADV7604_PAGE_EDID] = 0x36;
+        state->pdata.i2c_addresses[ADV7604_PAGE_HDMI] = 0x34;
+        state->pdata.i2c_addresses[ADV7604_PAGE_TEST] = 0x30;
+        state->pdata.i2c_addresses[ADV7604_PAGE_CP] = 0x22;
+        state->pdata.i2c_addresses[ADV7604_PAGE_VDP] = 0x24;
+        /* Hardcode the remaining platform data fields. */
+        state->pdata.disable_pwrdnb = 0;
+        state->pdata.disable_cable_det_rst = 0;
+        state->pdata.default_input = -1;
+        state->pdata.blank_data = 1;
+        state->pdata.alt_data_sat = 1;
+        state->pdata.op_format_mode_sel = ADV7604_OP_FORMAT_MODE0;
+        state->pdata.bus_order = ADV7604_BUS_ORDER_RGB;
+        return 0;
+}
 static int adv7604_probe(struct i2c_client *client,
                         const struct i2c_device_id *id)
 {
        static const struct v4l2_dv_timings cea640x480 =
                V4L2_DV_BT_CEA_640X480P59_94;
        struct adv7604_state *state;
-        struct adv7604_platform_data *pdata = client->dev.platform_data;
        struct v4l2_ctrl_handler *hdl;
        struct v4l2_subdev *sd;
+        unsigned int i;
+        u16 val;
        int err;
        /* Check if the adapter supports the needed features */
@@ -2223,32 +2764,80 @@ static int adv7604_probe(struct i2c_client *client,
                return -ENOMEM;
        }
+        state->i2c_clients[ADV7604_PAGE_IO] = client;
        /* initialize variables */
        state->restart_stdi_once = true;
        state->selected_input = ~0;
-        /* platform data */
+        if (IS_ENABLED(CONFIG_OF) && client->dev.of_node) {
-        if (!pdata) {
+                const struct of_device_id *oid;
+                oid = of_match_node(adv7604_of_id, client->dev.of_node);
+                state->info = oid->data;
+                err = adv7604_parse_dt(state);
+                if (err < 0) {
+                        v4l_err(client, "DT parsing error\n");
+                        return err;
+                }
+        } else if (client->dev.platform_data) {
+                struct adv7604_platform_data *pdata = client->dev.platform_data;
+                state->info = (const struct adv7604_chip_info *)id->driver_data;
+                state->pdata = *pdata;
+        } else {
                v4l_err(client, "No platform data!\n");
                return -ENODEV;
        }
-        state->pdata = *pdata;
+        /* Request GPIOs. */
+        for (i = 0; i < state->info->num_dv_ports; ++i) {
+                state->hpd_gpio[i] =
+                        devm_gpiod_get_index(&client->dev, "hpd", i);
+                if (IS_ERR(state->hpd_gpio[i]))
+                        continue;
+                gpiod_direction_output(state->hpd_gpio[i], 0);
+                v4l_info(client, "Handling HPD %u GPIO\n", i);
+        }
        state->timings = cea640x480;
+        state->format = adv7604_format_info(state, V4L2_MBUS_FMT_YUYV8_2X8);
        sd = &state->sd;
        v4l2_i2c_subdev_init(sd, client, &adv7604_ops);
+        snprintf(sd->name, sizeof(sd->name), "%s %d-%04x",
+                id->name, i2c_adapter_id(client->adapter),
+                client->addr);
        sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
-        /* i2c access to adv7604? */
+        /*
-        if (adv_smbus_read_byte_data_check(client, 0xfb, false) != 0x68) {
+         * Verify that the chip is present. On ADV7604 the RD_INFO register only
-                v4l2_info(sd, "not an adv7604 on address 0x%x\n",
+         * identifies the revision, while on ADV7611 it identifies the model as
-                                client->addr << 1);
+         * well. Use the HDMI slave address on ADV7604 and RD_INFO on ADV7611.
-                return -ENODEV;
+         */
+        if (state->info->type == ADV7604) {
+                val = adv_smbus_read_byte_data_check(client, 0xfb, false);
+                if (val != 0x68) {
+                        v4l2_info(sd, "not an adv7604 on address 0x%x\n",
+                                        client->addr << 1);
+                        return -ENODEV;
+                }
+        } else {
+                val = (adv_smbus_read_byte_data_check(client, 0xea, false) << 8)
+                    | (adv_smbus_read_byte_data_check(client, 0xeb, false) << 0);
+                if (val != 0x2051) {
+                        v4l2_info(sd, "not an adv7611 on address 0x%x\n",
+                                        client->addr << 1);
+                        return -ENODEV;
+                }
        }
        /* control handlers */
        hdl = &state->hdl;
-        v4l2_ctrl_handler_init(hdl, 9);
+        v4l2_ctrl_handler_init(hdl, adv7604_has_afe(state) ? 9 : 8);
        v4l2_ctrl_new_std(hdl, &adv7604_ctrl_ops,
                        V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
@@ -2261,15 +2850,17 @@ static int adv7604_probe(struct i2c_client *client,
        /* private controls */
        state->detect_tx_5v_ctrl = v4l2_ctrl_new_std(hdl, NULL,
-                        V4L2_CID_DV_RX_POWER_PRESENT, 0, 0x0f, 0, 0);
+                        V4L2_CID_DV_RX_POWER_PRESENT, 0,
+                        (1 << state->info->num_dv_ports) - 1, 0, 0);
        state->rgb_quantization_range_ctrl =
                v4l2_ctrl_new_std_menu(hdl, &adv7604_ctrl_ops,
                        V4L2_CID_DV_RX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
                        0, V4L2_DV_RGB_RANGE_AUTO);
        /* custom controls */
-        state->analog_sampling_phase_ctrl =
+        if (adv7604_has_afe(state))
-                v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_analog_sampling_phase, NULL);
+                state->analog_sampling_phase_ctrl =
+                        v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_analog_sampling_phase, NULL);
        state->free_run_color_manual_ctrl =
                v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_free_run_color_manual, NULL);
        state->free_run_color_ctrl =
@@ -2282,7 +2873,8 @@ static int adv7604_probe(struct i2c_client *client,
        }
        state->detect_tx_5v_ctrl->is_private = true;
        state->rgb_quantization_range_ctrl->is_private = true;
-        state->analog_sampling_phase_ctrl->is_private = true;
+        if (adv7604_has_afe(state))
+                state->analog_sampling_phase_ctrl->is_private = true;
        state->free_run_color_manual_ctrl->is_private = true;
        state->free_run_color_ctrl->is_private = true;
@@ -2291,25 +2883,18 @@ static int adv7604_probe(struct i2c_client *client,
                goto err_hdl;
        }
-        state->i2c_avlink = adv7604_dummy_client(sd, pdata->i2c_avlink, 0xf3);
+        for (i = 1; i < ADV7604_PAGE_MAX; ++i) {
-        state->i2c_cec = adv7604_dummy_client(sd, pdata->i2c_cec, 0xf4);
+                if (!(BIT(i) & state->info->page_mask))
-        state->i2c_infoframe = adv7604_dummy_client(sd, pdata->i2c_infoframe, 0xf5);
+                        continue;
-        state->i2c_esdp = adv7604_dummy_client(sd, pdata->i2c_esdp, 0xf6);
-        state->i2c_dpp = adv7604_dummy_client(sd, pdata->i2c_dpp, 0xf7);
+                state->i2c_clients[i] =
-        state->i2c_afe = adv7604_dummy_client(sd, pdata->i2c_afe, 0xf8);
+                        adv7604_dummy_client(sd, state->pdata.i2c_addresses[i],
-        state->i2c_repeater = adv7604_dummy_client(sd, pdata->i2c_repeater, 0xf9);
+                                             0xf2 + i);
-        state->i2c_edid = adv7604_dummy_client(sd, pdata->i2c_edid, 0xfa);
+                if (state->i2c_clients[i] == NULL) {
-        state->i2c_hdmi = adv7604_dummy_client(sd, pdata->i2c_hdmi, 0xfb);
+                        err = -ENOMEM;
-        state->i2c_test = adv7604_dummy_client(sd, pdata->i2c_test, 0xfc);
+                        v4l2_err(sd, "failed to create i2c client %u\n", i);
-        state->i2c_cp = adv7604_dummy_client(sd, pdata->i2c_cp, 0xfd);
+                        goto err_i2c;
-        state->i2c_vdp = adv7604_dummy_client(sd, pdata->i2c_vdp, 0xfe);
+                }
-        if (!state->i2c_avlink || !state->i2c_cec || !state->i2c_infoframe ||
-            !state->i2c_esdp || !state->i2c_dpp || !state->i2c_afe ||
-            !state->i2c_repeater || !state->i2c_edid || !state->i2c_hdmi ||
-            !state->i2c_test || !state->i2c_cp || !state->i2c_vdp) {
-                err = -ENOMEM;
-                v4l2_err(sd, "failed to create all i2c clients\n");
-                goto err_i2c;
        }
        /* work queues */
@@ -2323,8 +2908,14 @@ static int adv7604_probe(struct i2c_client *client,
        INIT_DELAYED_WORK(&state->delayed_work_enable_hotplug,
                        adv7604_delayed_work_enable_hotplug);
-        state->pad.flags = MEDIA_PAD_FL_SOURCE;
+        state->source_pad = state->info->num_dv_ports
-        err = media_entity_init(&sd->entity, 1, &state->pad, 0);
+                          + (state->info->has_afe ? 2 : 0);
+        for (i = 0; i < state->source_pad; ++i)
+                state->pads[i].flags = MEDIA_PAD_FL_SINK;
+        state->pads[state->source_pad].flags = MEDIA_PAD_FL_SOURCE;
+        err = media_entity_init(&sd->entity, state->source_pad + 1,
+                                state->pads, 0);
        if (err)
                goto err_work_queues;
@@ -2333,6 +2924,11 @@ static int adv7604_probe(struct i2c_client *client,
                goto err_entity;
        v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
                        client->addr << 1, client->adapter->name);
+        err = v4l2_async_register_subdev(sd);
+        if (err)
+                goto err_entity;
        return 0;
 err_entity:
@@ -2356,6 +2952,7 @@ static int adv7604_remove(struct i2c_client *client)
        cancel_delayed_work(&state->delayed_work_enable_hotplug);
        destroy_workqueue(state->work_queues);
+        v4l2_async_unregister_subdev(sd);
        v4l2_device_unregister_subdev(sd);
        media_entity_cleanup(&sd->entity);
        adv7604_unregister_clients(to_state(sd));
@@ -2365,20 +2962,15 @@ static int adv7604_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
-static struct i2c_device_id adv7604_id[] = {
-        { "adv7604", 0 },
-        { }
-};
-MODULE_DEVICE_TABLE(i2c, adv7604_id);
 static struct i2c_driver adv7604_driver = {
        .driver = {
                .owner = THIS_MODULE,
                .name = "adv7604",
+                .of_match_table = of_match_ptr(adv7604_of_id),
        },
        .probe = adv7604_probe,
        .remove = adv7604_remove,
-        .id_table = adv7604_id,
+        .id_table = adv7604_i2c_id,
 };
 module_i2c_driver(adv7604_driver);
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 20f1655e6d75..8108c698b548 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -93,7 +93,9 @@ config VIDEO_M32R_AR_M64278
 config VIDEO_OMAP3
        tristate "OMAP 3 Camera support"
-        depends on OMAP_IOVMM && VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API && ARCH_OMAP3
+        depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API && ARCH_OMAP3
+        select ARM_DMA_USE_IOMMU
+        select OMAP_IOMMU
        ---help---
          Driver for an OMAP 3 camera controller.
diff --git a/drivers/media/platform/omap3isp/Makefile b/drivers/media/platform/omap3isp/Makefile
index e8847e79e31a..254975a9174e 100644
--- a/drivers/media/platform/omap3isp/Makefile
+++ b/drivers/media/platform/omap3isp/Makefile
@@ -3,7 +3,7 @@
 ccflags-$(CONFIG_VIDEO_OMAP3_DEBUG) += -DDEBUG
 omap3-isp-objs += \
-        isp.o ispqueue.o ispvideo.o \
+        isp.o ispvideo.o \
        ispcsiphy.o ispccp2.o ispcsi2.o \
        ispccdc.o isppreview.o ispresizer.o \
        ispstat.o isph3a_aewb.o isph3a_af.o isphist.o
diff --git a/drivers/media/platform/omap3isp/isp.c b/drivers/media/platform/omap3isp/isp.c
index 06a0df434249..2c7aa6720569 100644
--- a/drivers/media/platform/omap3isp/isp.c
+++ b/drivers/media/platform/omap3isp/isp.c
@@ -69,6 +69,8 @@
 #include <linux/sched.h>
 #include <linux/vmalloc.h>
+#include <asm/dma-iommu.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
@@ -1397,14 +1399,14 @@ int omap3isp_module_sync_idle(struct media_entity *me, wait_queue_head_t *wait,
        if (isp_pipeline_is_last(me)) {
                struct isp_video *video = pipe->output;
                unsigned long flags;
-                spin_lock_irqsave(&video->queue->irqlock, flags);
+                spin_lock_irqsave(&video->irqlock, flags);
                if (video->dmaqueue_flags & ISP_VIDEO_DMAQUEUE_UNDERRUN) {
-                        spin_unlock_irqrestore(&video->queue->irqlock, flags);
+                        spin_unlock_irqrestore(&video->irqlock, flags);
                        atomic_set(stopping, 0);
                        smp_mb();
                        return 0;
                }
-                spin_unlock_irqrestore(&video->queue->irqlock, flags);
+                spin_unlock_irqrestore(&video->irqlock, flags);
                if (!wait_event_timeout(*wait, !atomic_read(stopping),
                                        msecs_to_jiffies(1000))) {
                        atomic_set(stopping, 0);
@@ -1625,7 +1627,7 @@ struct isp_device *omap3isp_get(struct isp_device *isp)
 * Decrement the reference count on the ISP. If the last reference is released,
 * power-down all submodules, disable clocks and free temporary buffers.
 */
-void omap3isp_put(struct isp_device *isp)
+static void __omap3isp_put(struct isp_device *isp, bool save_ctx)
 {
        if (isp == NULL)
                return;
@@ -1634,7 +1636,7 @@ void omap3isp_put(struct isp_device *isp)
        BUG_ON(isp->ref_count == 0);
        if (--isp->ref_count == 0) {
                isp_disable_interrupts(isp);
-                if (isp->domain) {
+                if (save_ctx) {
                        isp_save_ctx(isp);
                        isp->has_context = 1;
                }
@@ -1648,6 +1650,11 @@ void omap3isp_put(struct isp_device *isp)
        mutex_unlock(&isp->isp_mutex);
 }
+void omap3isp_put(struct isp_device *isp)
+{
+        __omap3isp_put(isp, true);
+}
 /* --------------------------------------------------------------------------
 * Platform device driver
 */
@@ -2120,6 +2127,61 @@ error_csiphy:
        return ret;
 }
+static void isp_detach_iommu(struct isp_device *isp)
+{
+        arm_iommu_release_mapping(isp->mapping);
+        isp->mapping = NULL;
+        iommu_group_remove_device(isp->dev);
+}
+static int isp_attach_iommu(struct isp_device *isp)
+{
+        struct dma_iommu_mapping *mapping;
+        struct iommu_group *group;
+        int ret;
+        /* Create a device group and add the device to it. */
+        group = iommu_group_alloc();
+        if (IS_ERR(group)) {
+                dev_err(isp->dev, "failed to allocate IOMMU group\n");
+                return PTR_ERR(group);
+        }
+        ret = iommu_group_add_device(group, isp->dev);
+        iommu_group_put(group);
+        if (ret < 0) {
+                dev_err(isp->dev, "failed to add device to IPMMU group\n");
+                return ret;
+        }
+        /*
+         * Create the ARM mapping, used by the ARM DMA mapping core to allocate
+         * VAs. This will allocate a corresponding IOMMU domain.
+         */
+        mapping = arm_iommu_create_mapping(&platform_bus_type, SZ_1G, SZ_2G);
+        if (IS_ERR(mapping)) {
+                dev_err(isp->dev, "failed to create ARM IOMMU mapping\n");
+                ret = PTR_ERR(mapping);
+                goto error;
+        }
+        isp->mapping = mapping;
+        /* Attach the ARM VA mapping to the device. */
+        ret = arm_iommu_attach_device(isp->dev, mapping);
+        if (ret < 0) {
+                dev_err(isp->dev, "failed to attach device to VA mapping\n");
+                goto error;
+        }
+        return 0;
+error:
+        isp_detach_iommu(isp);
+        return ret;
+}
 /*
 * isp_remove - Remove ISP platform device
 * @pdev: Pointer to ISP platform device
@@ -2135,10 +2197,8 @@ static int isp_remove(struct platform_device *pdev)
        isp_xclk_cleanup(isp);
        __omap3isp_get(isp, false);
-        iommu_detach_device(isp->domain, &pdev->dev);
+        isp_detach_iommu(isp);
-        iommu_domain_free(isp->domain);
+        __omap3isp_put(isp, false);
-        isp->domain = NULL;
-        omap3isp_put(isp);
        return 0;
 }
@@ -2265,39 +2325,32 @@ static int isp_probe(struct platform_device *pdev)
                }
        }
-        isp->domain = iommu_domain_alloc(pdev->dev.bus);
+        /* IOMMU */
-        if (!isp->domain) {
+        ret = isp_attach_iommu(isp);
-                dev_err(isp->dev, "can't alloc iommu domain\n");
+        if (ret < 0) {
-                ret = -ENOMEM;
+                dev_err(&pdev->dev, "unable to attach to IOMMU\n");
                goto error_isp;
        }
-        ret = iommu_attach_device(isp->domain, &pdev->dev);
-        if (ret) {
-                dev_err(&pdev->dev, "can't attach iommu device: %d\n", ret);
-                ret = -EPROBE_DEFER;
-                goto free_domain;
-        }
        /* Interrupt */
        isp->irq_num = platform_get_irq(pdev, 0);
        if (isp->irq_num <= 0) {
                dev_err(isp->dev, "No IRQ resource\n");
                ret = -ENODEV;
-                goto detach_dev;
+                goto error_iommu;
        }
        if (devm_request_irq(isp->dev, isp->irq_num, isp_isr, IRQF_SHARED,
                             "OMAP3 ISP", isp)) {
                dev_err(isp->dev, "Unable to request IRQ\n");
                ret = -EINVAL;
-                goto detach_dev;
+                goto error_iommu;
        }
        /* Entities */
        ret = isp_initialize_modules(isp);
        if (ret < 0)
-                goto detach_dev;
+                goto error_iommu;
        ret = isp_register_entities(isp);
        if (ret < 0)
@@ -2310,14 +2363,11 @@ static int isp_probe(struct platform_device *pdev)
 error_modules:
        isp_cleanup_modules(isp);
-detach_dev:
+error_iommu:
-        iommu_detach_device(isp->domain, &pdev->dev);
+        isp_detach_iommu(isp);
-free_domain:
-        iommu_domain_free(isp->domain);
-        isp->domain = NULL;
 error_isp:
        isp_xclk_cleanup(isp);
-        omap3isp_put(isp);
+        __omap3isp_put(isp, false);
 error:
        mutex_destroy(&isp->isp_mutex);
diff --git a/drivers/media/platform/omap3isp/isp.h b/drivers/media/platform/omap3isp/isp.h
index 6d5e69711907..2c314eea1252 100644
--- a/drivers/media/platform/omap3isp/isp.h
+++ b/drivers/media/platform/omap3isp/isp.h
@@ -45,8 +45,6 @@
 #include "ispcsi2.h"
 #include "ispccp2.h"
-#define IOMMU_FLAG (IOVMF_ENDIAN_LITTLE | IOVMF_ELSZ_8)
 #define ISP_TOK_TERM            0xFFFFFFFF      /*
                                                 * terminating token for ISP
                                                 * modules reg list
@@ -152,6 +150,7 @@ struct isp_xclk {
 *             regions.
 * @mmio_base_phys: Array with physical L4 bus addresses for ISP register
 *                  regions.
+ * @mapping: IOMMU mapping
 * @stat_lock: Spinlock for handling statistics
 * @isp_mutex: Mutex for serializing requests to ISP.
 * @stop_failure: Indicates that an entity failed to stop.
@@ -171,7 +170,6 @@ struct isp_xclk {
 * @isp_res: Pointer to current settings for ISP Resizer.
 * @isp_prev: Pointer to current settings for ISP Preview.
 * @isp_ccdc: Pointer to current settings for ISP CCDC.
- * @iommu: Pointer to requested IOMMU instance for ISP.
 * @platform_cb: ISP driver callback function pointers for platform code
 *
 * This structure is used to store the OMAP ISP Information.
@@ -189,6 +187,8 @@ struct isp_device {
        void __iomem *mmio_base[OMAP3_ISP_IOMEM_LAST];
        unsigned long mmio_base_phys[OMAP3_ISP_IOMEM_LAST];
+        struct dma_iommu_mapping *mapping;
        /* ISP Obj */
        spinlock_t stat_lock;   /* common lock for statistic drivers */
        struct mutex isp_mutex; /* For handling ref_count field */
@@ -219,8 +219,6 @@ struct isp_device {
        unsigned int sbl_resources;
        unsigned int subclk_resources;
-        struct iommu_domain *domain;
 };
 #define v4l2_dev_to_isp_device(dev) \
diff --git a/drivers/media/platform/omap3isp/ispccdc.c b/drivers/media/platform/omap3isp/ispccdc.c
index 4d920c800ff5..9f727d20f06d 100644
--- a/drivers/media/platform/omap3isp/ispccdc.c
+++ b/drivers/media/platform/omap3isp/ispccdc.c
@@ -30,7 +30,6 @@
 #include <linux/device.h>
 #include <linux/dma-mapping.h>
 #include <linux/mm.h>
-#include <linux/omap-iommu.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <media/v4l2-event.h>
@@ -206,7 +205,8 @@ static int ccdc_lsc_validate_config(struct isp_ccdc_device *ccdc,
 * ccdc_lsc_program_table - Program Lens Shading Compensation table address.
 * @ccdc: Pointer to ISP CCDC device.
 */
-static void ccdc_lsc_program_table(struct isp_ccdc_device *ccdc, u32 addr)
+static void ccdc_lsc_program_table(struct isp_ccdc_device *ccdc,
+                                   dma_addr_t addr)
 {
        isp_reg_writel(to_isp_device(ccdc), addr,
                       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_LSC_TABLE_BASE);
@@ -333,7 +333,7 @@ static int __ccdc_lsc_configure(struct isp_ccdc_device *ccdc,
                return -EBUSY;
        ccdc_lsc_setup_regs(ccdc, &req->config);
-        ccdc_lsc_program_table(ccdc, req->table);
+        ccdc_lsc_program_table(ccdc, req->table.dma);
        return 0;
 }
@@ -368,11 +368,12 @@ static void ccdc_lsc_free_request(struct isp_ccdc_device *ccdc,
        if (req == NULL)
                return;
-        if (req->iovm)
+        if (req->table.addr) {
-                dma_unmap_sg(isp->dev, req->iovm->sgt->sgl,
+                sg_free_table(&req->table.sgt);
-                             req->iovm->sgt->nents, DMA_TO_DEVICE);
+                dma_free_coherent(isp->dev, req->config.size, req->table.addr,
-        if (req->table)
+                                  req->table.dma);
-                omap_iommu_vfree(isp->domain, isp->dev, req->table);
+        }
        kfree(req);
 }
@@ -416,7 +417,6 @@ static int ccdc_lsc_config(struct isp_ccdc_device *ccdc,
        struct isp_device *isp = to_isp_device(ccdc);
        struct ispccdc_lsc_config_req *req;
        unsigned long flags;
-        void *table;
        u16 update;
        int ret;
@@ -444,38 +444,31 @@ static int ccdc_lsc_config(struct isp_ccdc_device *ccdc,
                req->enable = 1;
-                req->table = omap_iommu_vmalloc(isp->domain, isp->dev, 0,
+                req->table.addr = dma_alloc_coherent(isp->dev, req->config.size,
-                                        req->config.size, IOMMU_FLAG);
+                                                     &req->table.dma,
-                if (IS_ERR_VALUE(req->table)) {
+                                                     GFP_KERNEL);
-                        req->table = 0;
+                if (req->table.addr == NULL) {
-                        ret = -ENOMEM;
-                        goto done;
-                }
-                req->iovm = omap_find_iovm_area(isp->dev, req->table);
-                if (req->iovm == NULL) {
                        ret = -ENOMEM;
                        goto done;
                }
-                if (!dma_map_sg(isp->dev, req->iovm->sgt->sgl,
+                ret = dma_get_sgtable(isp->dev, &req->table.sgt,
-                                req->iovm->sgt->nents, DMA_TO_DEVICE)) {
+                                      req->table.addr, req->table.dma,
-                        ret = -ENOMEM;
+                                      req->config.size);
-                        req->iovm = NULL;
+                if (ret < 0)
                        goto done;
-                }
-                dma_sync_sg_for_cpu(isp->dev, req->iovm->sgt->sgl,
+                dma_sync_sg_for_cpu(isp->dev, req->table.sgt.sgl,
-                                    req->iovm->sgt->nents, DMA_TO_DEVICE);
+                                    req->table.sgt.nents, DMA_TO_DEVICE);
-                table = omap_da_to_va(isp->dev, req->table);
+                if (copy_from_user(req->table.addr, config->lsc,
-                if (copy_from_user(table, config->lsc, req->config.size)) {
+                                   req->config.size)) {
                        ret = -EFAULT;
                        goto done;
                }
-                dma_sync_sg_for_device(isp->dev, req->iovm->sgt->sgl,
+                dma_sync_sg_for_device(isp->dev, req->table.sgt.sgl,
-                                       req->iovm->sgt->nents, DMA_TO_DEVICE);
+                                       req->table.sgt.nents, DMA_TO_DEVICE);
        }
        spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
@@ -584,7 +577,7 @@ static void ccdc_configure_fpc(struct isp_ccdc_device *ccdc)
        if (!ccdc->fpc_en)
                return;
-        isp_reg_writel(isp, ccdc->fpc.fpcaddr, OMAP3_ISP_IOMEM_CCDC,
+        isp_reg_writel(isp, ccdc->fpc.dma, OMAP3_ISP_IOMEM_CCDC,
                       ISPCCDC_FPC_ADDR);
        /* The FPNUM field must be set before enabling FPC. */
        isp_reg_writel(isp, (ccdc->fpc.fpnum << ISPCCDC_FPC_FPNUM_SHIFT),
@@ -724,8 +717,9 @@ static int ccdc_config(struct isp_ccdc_device *ccdc,
        ccdc->shadow_update = 0;
        if (OMAP3ISP_CCDC_FPC & ccdc_struct->update) {
-                u32 table_old = 0;
+                struct omap3isp_ccdc_fpc fpc;
-                u32 table_new;
+                struct ispccdc_fpc fpc_old = { .addr = NULL, };
+                struct ispccdc_fpc fpc_new;
                u32 size;
                if (ccdc->state != ISP_PIPELINE_STREAM_STOPPED)
@@ -734,35 +728,39 @@ static int ccdc_config(struct isp_ccdc_device *ccdc,
                ccdc->fpc_en = !!(OMAP3ISP_CCDC_FPC & ccdc_struct->flag);
                if (ccdc->fpc_en) {
-                        if (copy_from_user(&ccdc->fpc, ccdc_struct->fpc,
+                        if (copy_from_user(&fpc, ccdc_struct->fpc, sizeof(fpc)))
-                                           sizeof(ccdc->fpc)))
                                return -EFAULT;
+                        size = fpc.fpnum * 4;
                        /*
-                         * table_new must be 64-bytes aligned, but it's
+                         * The table address must be 64-bytes aligned, which is
-                         * already done by omap_iommu_vmalloc().
+                         * guaranteed by dma_alloc_coherent().
                         */
-                        size = ccdc->fpc.fpnum * 4;
+                        fpc_new.fpnum = fpc.fpnum;
-                        table_new = omap_iommu_vmalloc(isp->domain, isp->dev,
+                        fpc_new.addr = dma_alloc_coherent(isp->dev, size,
-                                                        0, size, IOMMU_FLAG);
+                                                          &fpc_new.dma,
-                        if (IS_ERR_VALUE(table_new))
+                                                          GFP_KERNEL);
+                        if (fpc_new.addr == NULL)
                                return -ENOMEM;
-                        if (copy_from_user(omap_da_to_va(isp->dev, table_new),
+                        if (copy_from_user(fpc_new.addr,
-                                           (__force void __user *)
+                                           (__force void __user *)fpc.fpcaddr,
-                                           ccdc->fpc.fpcaddr, size)) {
+                                           size)) {
-                                omap_iommu_vfree(isp->domain, isp->dev,
+                                dma_free_coherent(isp->dev, size, fpc_new.addr,
-                                                                table_new);
+                                                  fpc_new.dma);
                                return -EFAULT;
                        }
-                        table_old = ccdc->fpc.fpcaddr;
+                        fpc_old = ccdc->fpc;
-                        ccdc->fpc.fpcaddr = table_new;
+                        ccdc->fpc = fpc_new;
                }
                ccdc_configure_fpc(ccdc);
-                if (table_old != 0)
-                        omap_iommu_vfree(isp->domain, isp->dev, table_old);
+                if (fpc_old.addr != NULL)
+                        dma_free_coherent(isp->dev, fpc_old.fpnum * 4,
+                                          fpc_old.addr, fpc_old.dma);
        }
        return ccdc_lsc_config(ccdc, ccdc_struct);
@@ -1523,7 +1521,7 @@ static int ccdc_isr_buffer(struct isp_ccdc_device *ccdc)
        buffer = omap3isp_video_buffer_next(&ccdc->video_out);
        if (buffer != NULL) {
-                ccdc_set_outaddr(ccdc, buffer->isp_addr);
+                ccdc_set_outaddr(ccdc, buffer->dma);
                restart = 1;
        }
@@ -1662,7 +1660,7 @@ static int ccdc_video_queue(struct isp_video *video, struct isp_buffer *buffer)
        if (!(ccdc->output & CCDC_OUTPUT_MEMORY))
                return -ENODEV;
-        ccdc_set_outaddr(ccdc, buffer->isp_addr);
+        ccdc_set_outaddr(ccdc, buffer->dma);
        /* We now have a buffer queued on the output, restart the pipeline
         * on the next CCDC interrupt if running in continuous mode (or when
@@ -2580,8 +2578,9 @@ void omap3isp_ccdc_cleanup(struct isp_device *isp)
        cancel_work_sync(&ccdc->lsc.table_work);
        ccdc_lsc_free_queue(ccdc, &ccdc->lsc.free_queue);
-        if (ccdc->fpc.fpcaddr != 0)
+        if (ccdc->fpc.addr != NULL)
-                omap_iommu_vfree(isp->domain, isp->dev, ccdc->fpc.fpcaddr);
+                dma_free_coherent(isp->dev, ccdc->fpc.fpnum * 4, ccdc->fpc.addr,
+                                  ccdc->fpc.dma);
        mutex_destroy(&ccdc->ioctl_lock);
 }
diff --git a/drivers/media/platform/omap3isp/ispccdc.h b/drivers/media/platform/omap3isp/ispccdc.h
index 9d24e4107864..f65061602c71 100644
--- a/drivers/media/platform/omap3isp/ispccdc.h
+++ b/drivers/media/platform/omap3isp/ispccdc.h
@@ -46,6 +46,12 @@ enum ccdc_input_entity {
 #define OMAP3ISP_CCDC_NEVENTS   16
+struct ispccdc_fpc {
+        void *addr;
+        dma_addr_t dma;
+        unsigned int fpnum;
+};
 enum ispccdc_lsc_state {
        LSC_STATE_STOPPED = 0,
        LSC_STATE_STOPPING = 1,
@@ -57,8 +63,12 @@ struct ispccdc_lsc_config_req {
        struct list_head list;
        struct omap3isp_ccdc_lsc_config config;
        unsigned char enable;
-        u32 table;
-        struct iovm_struct *iovm;
+        struct {
+                void *addr;
+                dma_addr_t dma;
+                struct sg_table sgt;
+        } table;
 };
 /*
@@ -136,7 +146,7 @@ struct isp_ccdc_device {
                     fpc_en:1;
        struct omap3isp_ccdc_blcomp blcomp;
        struct omap3isp_ccdc_bclamp clamp;
-        struct omap3isp_ccdc_fpc fpc;
+        struct ispccdc_fpc fpc;
        struct ispccdc_lsc lsc;
        unsigned int update;
        unsigned int shadow_update;
diff --git a/drivers/media/platform/omap3isp/ispccp2.c b/drivers/media/platform/omap3isp/ispccp2.c
index b30b67d22a58..f3801db9095c 100644
--- a/drivers/media/platform/omap3isp/ispccp2.c
+++ b/drivers/media/platform/omap3isp/ispccp2.c
@@ -549,7 +549,7 @@ static void ccp2_isr_buffer(struct isp_ccp2_device *ccp2)
        buffer = omap3isp_video_buffer_next(&ccp2->video_in);
        if (buffer != NULL)
-                ccp2_set_inaddr(ccp2, buffer->isp_addr);
+                ccp2_set_inaddr(ccp2, buffer->dma);
        pipe->state |= ISP_PIPELINE_IDLE_INPUT;
@@ -940,7 +940,7 @@ static int ccp2_video_queue(struct isp_video *video, struct isp_buffer *buffer)
 {
        struct isp_ccp2_device *ccp2 = &video->isp->isp_ccp2;
-        ccp2_set_inaddr(ccp2, buffer->isp_addr);
+        ccp2_set_inaddr(ccp2, buffer->dma);
        return 0;
 }
diff --git a/drivers/media/platform/omap3isp/ispcsi2.c b/drivers/media/platform/omap3isp/ispcsi2.c
index 620560828a48..5a2e47e58b84 100644
--- a/drivers/media/platform/omap3isp/ispcsi2.c
+++ b/drivers/media/platform/omap3isp/ispcsi2.c
@@ -695,7 +695,7 @@ static void csi2_isr_buffer(struct isp_csi2_device *csi2)
        if (buffer == NULL)
                return;
-        csi2_set_outaddr(csi2, buffer->isp_addr);
+        csi2_set_outaddr(csi2, buffer->dma);
        csi2_ctx_enable(isp, csi2, 0, 1);
 }
@@ -812,7 +812,7 @@ static int csi2_queue(struct isp_video *video, struct isp_buffer *buffer)
        struct isp_device *isp = video->isp;
        struct isp_csi2_device *csi2 = &isp->isp_csi2a;
-        csi2_set_outaddr(csi2, buffer->isp_addr);
+        csi2_set_outaddr(csi2, buffer->dma);
        /*
         * If streaming was enabled before there was a buffer queued
diff --git a/drivers/media/platform/omap3isp/isph3a_aewb.c b/drivers/media/platform/omap3isp/isph3a_aewb.c
index 75fd82b152ba..d6811ce263eb 100644
--- a/drivers/media/platform/omap3isp/isph3a_aewb.c
+++ b/drivers/media/platform/omap3isp/isph3a_aewb.c
@@ -47,7 +47,7 @@ static void h3a_aewb_setup_regs(struct ispstat *aewb, void *priv)
        if (aewb->state == ISPSTAT_DISABLED)
                return;
-        isp_reg_writel(aewb->isp, aewb->active_buf->iommu_addr,
+        isp_reg_writel(aewb->isp, aewb->active_buf->dma_addr,
                       OMAP3_ISP_IOMEM_H3A, ISPH3A_AEWBUFST);
        if (!aewb->update)
diff --git a/drivers/media/platform/omap3isp/isph3a_af.c b/drivers/media/platform/omap3isp/isph3a_af.c
index a0bf5af32438..6fc960cd30f5 100644
--- a/drivers/media/platform/omap3isp/isph3a_af.c
+++ b/drivers/media/platform/omap3isp/isph3a_af.c
@@ -51,7 +51,7 @@ static void h3a_af_setup_regs(struct ispstat *af, void *priv)
        if (af->state == ISPSTAT_DISABLED)
                return;
-        isp_reg_writel(af->isp, af->active_buf->iommu_addr, OMAP3_ISP_IOMEM_H3A,
+        isp_reg_writel(af->isp, af->active_buf->dma_addr, OMAP3_ISP_IOMEM_H3A,
                       ISPH3A_AFBUFST);
        if (!af->update)
diff --git a/drivers/media/platform/omap3isp/isppreview.c b/drivers/media/platform/omap3isp/isppreview.c
index 395b2b068c75..720809b07e75 100644
--- a/drivers/media/platform/omap3isp/isppreview.c
+++ b/drivers/media/platform/omap3isp/isppreview.c
@@ -1499,14 +1499,14 @@ static void preview_isr_buffer(struct isp_prev_device *prev)
        if (prev->input == PREVIEW_INPUT_MEMORY) {
                buffer = omap3isp_video_buffer_next(&prev->video_in);
                if (buffer != NULL)
-                        preview_set_inaddr(prev, buffer->isp_addr);
+                        preview_set_inaddr(prev, buffer->dma);
                pipe->state |= ISP_PIPELINE_IDLE_INPUT;
        }
        if (prev->output & PREVIEW_OUTPUT_MEMORY) {
                buffer = omap3isp_video_buffer_next(&prev->video_out);
                if (buffer != NULL) {
-                        preview_set_outaddr(prev, buffer->isp_addr);
+                        preview_set_outaddr(prev, buffer->dma);
                        restart = 1;
                }
                pipe->state |= ISP_PIPELINE_IDLE_OUTPUT;
@@ -1577,10 +1577,10 @@ static int preview_video_queue(struct isp_video *video,
        struct isp_prev_device *prev = &video->isp->isp_prev;
        if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
-                preview_set_inaddr(prev, buffer->isp_addr);
+                preview_set_inaddr(prev, buffer->dma);
        if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                preview_set_outaddr(prev, buffer->isp_addr);
+                preview_set_outaddr(prev, buffer->dma);
        return 0;
 }
diff --git a/drivers/media/platform/omap3isp/ispqueue.c b/drivers/media/platform/omap3isp/ispqueue.c
deleted file mode 100644
index a5e65858e799..000000000000
--- a/drivers/media/platform/omap3isp/ispqueue.c
+++ /dev/null
@@ -1,1161 +0,0 @@
-/*
- * ispqueue.c
- *
- * TI OMAP3 ISP - Video buffers queue handling
- *
- * Copyright (C) 2010 Nokia Corporation
- *
- * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
- *           Sakari Ailus <sakari.ailus@iki.fi>
- *
- * 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.
- *
- * 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.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- */
-#include <asm/cacheflush.h>
-#include <linux/dma-mapping.h>
-#include <linux/mm.h>
-#include <linux/pagemap.h>
-#include <linux/poll.h>
-#include <linux/scatterlist.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/vmalloc.h>
-#include "ispqueue.h"
-/* -----------------------------------------------------------------------------
- * Video buffers management
- */
-/*
- * isp_video_buffer_cache_sync - Keep the buffers coherent between CPU and ISP
- *
- * The typical operation required here is Cache Invalidation across
- * the (user space) buffer address range. And this _must_ be done
- * at QBUF stage (and *only* at QBUF).
- *
- * We try to use optimal cache invalidation function:
- * - dmac_map_area:
- *    - used when the number of pages are _low_.
- *    - it becomes quite slow as the number of pages increase.
- *       - for 648x492 viewfinder (150 pages) it takes 1.3 ms.
- *       - for 5 Mpix buffer (2491 pages) it takes between 25-50 ms.
- *
- * - flush_cache_all:
- *    - used when the number of pages are _high_.
- *    - time taken in the range of 500-900 us.
- *    - has a higher penalty but, as whole dcache + icache is invalidated
- */
-/*
- * FIXME: dmac_inv_range crashes randomly on the user space buffer
- *        address. Fall back to flush_cache_all for now.
- */
-#define ISP_CACHE_FLUSH_PAGES_MAX       0
-static void isp_video_buffer_cache_sync(struct isp_video_buffer *buf)
-{
-        if (buf->skip_cache)
-                return;
-        if (buf->vbuf.m.userptr == 0 || buf->npages == 0 ||
-            buf->npages > ISP_CACHE_FLUSH_PAGES_MAX)
-                flush_cache_all();
-        else {
-                dmac_map_area((void *)buf->vbuf.m.userptr, buf->vbuf.length,
-                              DMA_FROM_DEVICE);
-                outer_inv_range(buf->vbuf.m.userptr,
-                                buf->vbuf.m.userptr + buf->vbuf.length);
-        }
-}
-/*
- * isp_video_buffer_lock_vma - Prevent VMAs from being unmapped
- *
- * Lock the VMAs underlying the given buffer into memory. This avoids the
- * userspace buffer mapping from being swapped out, making VIPT cache handling
- * easier.
- *
- * Note that the pages will not be freed as the buffers have been locked to
- * memory using by a call to get_user_pages(), but the userspace mapping could
- * still disappear if the VMAs are not locked. This is caused by the memory
- * management code trying to be as lock-less as possible, which results in the
- * userspace mapping manager not finding out that the pages are locked under
- * some conditions.
- */
-static int isp_video_buffer_lock_vma(struct isp_video_buffer *buf, int lock)
-{
-        struct vm_area_struct *vma;
-        unsigned long start;
-        unsigned long end;
-        int ret = 0;
-        if (buf->vbuf.memory == V4L2_MEMORY_MMAP)
-                return 0;
-        /* We can be called from workqueue context if the current task dies to
-         * unlock the VMAs. In that case there's no current memory management
-         * context so unlocking can't be performed, but the VMAs have been or
-         * are getting destroyed anyway so it doesn't really matter.
-         */
-        if (!current || !current->mm)
-                return lock ? -EINVAL : 0;
-        start = buf->vbuf.m.userptr;
-        end = buf->vbuf.m.userptr + buf->vbuf.length - 1;
-        down_write(&current->mm->mmap_sem);
-        spin_lock(&current->mm->page_table_lock);
-        do {
-                vma = find_vma(current->mm, start);
-                if (vma == NULL) {
-                        ret = -EFAULT;
-                        goto out;
-                }
-                if (lock)
-                        vma->vm_flags |= VM_LOCKED;
-                else
-                        vma->vm_flags &= ~VM_LOCKED;
-                start = vma->vm_end + 1;
-        } while (vma->vm_end < end);
-        if (lock)
-                buf->vm_flags |= VM_LOCKED;
-        else
-                buf->vm_flags &= ~VM_LOCKED;
-out:
-        spin_unlock(&current->mm->page_table_lock);
-        up_write(&current->mm->mmap_sem);
-        return ret;
-}
-/*
- * isp_video_buffer_sglist_kernel - Build a scatter list for a vmalloc'ed buffer
- *
- * Iterate over the vmalloc'ed area and create a scatter list entry for every
- * page.
- */
-static int isp_video_buffer_sglist_kernel(struct isp_video_buffer *buf)
-{
-        struct scatterlist *sglist;
-        unsigned int npages;
-        unsigned int i;
-        void *addr;
-        addr = buf->vaddr;
-        npages = PAGE_ALIGN(buf->vbuf.length) >> PAGE_SHIFT;
-        sglist = vmalloc(npages * sizeof(*sglist));
-        if (sglist == NULL)
-                return -ENOMEM;
-        sg_init_table(sglist, npages);
-        for (i = 0; i < npages; ++i, addr += PAGE_SIZE) {
-                struct page *page = vmalloc_to_page(addr);
-                if (page == NULL || PageHighMem(page)) {
-                        vfree(sglist);
-                        return -EINVAL;
-                }
-                sg_set_page(&sglist[i], page, PAGE_SIZE, 0);
-        }
-        buf->sglen = npages;
-        buf->sglist = sglist;
-        return 0;
-}
-/*
- * isp_video_buffer_sglist_user - Build a scatter list for a userspace buffer
- *
- * Walk the buffer pages list and create a 1:1 mapping to a scatter list.
- */
-static int isp_video_buffer_sglist_user(struct isp_video_buffer *buf)
-{
-        struct scatterlist *sglist;
-        unsigned int offset = buf->offset;
-        unsigned int i;
-        sglist = vmalloc(buf->npages * sizeof(*sglist));
-        if (sglist == NULL)
-                return -ENOMEM;
-        sg_init_table(sglist, buf->npages);
-        for (i = 0; i < buf->npages; ++i) {
-                if (PageHighMem(buf->pages[i])) {
-                        vfree(sglist);
-                        return -EINVAL;
-                }
-                sg_set_page(&sglist[i], buf->pages[i], PAGE_SIZE - offset,
-                            offset);
-                offset = 0;
-        }
-        buf->sglen = buf->npages;
-        buf->sglist = sglist;
-        return 0;
-}
-/*
- * isp_video_buffer_sglist_pfnmap - Build a scatter list for a VM_PFNMAP buffer
- *
- * Create a scatter list of physically contiguous pages starting at the buffer
- * memory physical address.
- */
-static int isp_video_buffer_sglist_pfnmap(struct isp_video_buffer *buf)
-{
-        struct scatterlist *sglist;
-        unsigned int offset = buf->offset;
-        unsigned long pfn = buf->paddr >> PAGE_SHIFT;
-        unsigned int i;
-        sglist = vmalloc(buf->npages * sizeof(*sglist));
-        if (sglist == NULL)
-                return -ENOMEM;
-        sg_init_table(sglist, buf->npages);
-        for (i = 0; i < buf->npages; ++i, ++pfn) {
-                sg_set_page(&sglist[i], pfn_to_page(pfn), PAGE_SIZE - offset,
-                            offset);
-                /* PFNMAP buffers will not get DMA-mapped, set the DMA address
-                 * manually.
-                 */
-                sg_dma_address(&sglist[i]) = (pfn << PAGE_SHIFT) + offset;
-                offset = 0;
-        }
-        buf->sglen = buf->npages;
-        buf->sglist = sglist;
-        return 0;
-}
-/*
- * isp_video_buffer_cleanup - Release pages for a userspace VMA.
- *
- * Release pages locked by a call isp_video_buffer_prepare_user and free the
- * pages table.
- */
-static void isp_video_buffer_cleanup(struct isp_video_buffer *buf)
-{
-        enum dma_data_direction direction;
-        unsigned int i;
-        if (buf->queue->ops->buffer_cleanup)
-                buf->queue->ops->buffer_cleanup(buf);
-        if (!(buf->vm_flags & VM_PFNMAP)) {
-                direction = buf->vbuf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE
-                          ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-                dma_unmap_sg(buf->queue->dev, buf->sglist, buf->sglen,
-                             direction);
-        }
-        vfree(buf->sglist);
-        buf->sglist = NULL;
-        buf->sglen = 0;
-        if (buf->pages != NULL) {
-                isp_video_buffer_lock_vma(buf, 0);
-                for (i = 0; i < buf->npages; ++i)
-                        page_cache_release(buf->pages[i]);
-                vfree(buf->pages);
-                buf->pages = NULL;
-        }
-        buf->npages = 0;
-        buf->skip_cache = false;
-}
-/*
- * isp_video_buffer_prepare_user - Pin userspace VMA pages to memory.
- *
- * This function creates a list of pages for a userspace VMA. The number of
- * pages is first computed based on the buffer size, and pages are then
- * retrieved by a call to get_user_pages.
- *
- * Pages are pinned to memory by get_user_pages, making them available for DMA
- * transfers. However, due to memory management optimization, it seems the
- * get_user_pages doesn't guarantee that the pinned pages will not be written
- * to swap and removed from the userspace mapping(s). When this happens, a page
- * fault can be generated when accessing those unmapped pages.
- *
- * If the fault is triggered by a page table walk caused by VIPT cache
- * management operations, the page fault handler might oops if the MM semaphore
- * is held, as it can't handle kernel page faults in that case. To fix that, a
- * fixup entry needs to be added to the cache management code, or the userspace
- * VMA must be locked to avoid removing pages from the userspace mapping in the
- * first place.
- *
- * If the number of pages retrieved is smaller than the number required by the
- * buffer size, the function returns -EFAULT.
- */
-static int isp_video_buffer_prepare_user(struct isp_video_buffer *buf)
-{
-        unsigned long data;
-        unsigned int first;
-        unsigned int last;
-        int ret;
-        data = buf->vbuf.m.userptr;
-        first = (data & PAGE_MASK) >> PAGE_SHIFT;
-        last = ((data + buf->vbuf.length - 1) & PAGE_MASK) >> PAGE_SHIFT;
-        buf->offset = data & ~PAGE_MASK;
-        buf->npages = last - first + 1;
-        buf->pages = vmalloc(buf->npages * sizeof(buf->pages[0]));
-        if (buf->pages == NULL)
-                return -ENOMEM;
-        down_read(&current->mm->mmap_sem);
-        ret = get_user_pages(current, current->mm, data & PAGE_MASK,
-                             buf->npages,
-                             buf->vbuf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE, 0,
-                             buf->pages, NULL);
-        up_read(&current->mm->mmap_sem);
-        if (ret != buf->npages) {
-                buf->npages = ret < 0 ? 0 : ret;
-                isp_video_buffer_cleanup(buf);
-                return -EFAULT;
-        }
-        ret = isp_video_buffer_lock_vma(buf, 1);
-        if (ret < 0)
-                isp_video_buffer_cleanup(buf);
-        return ret;
-}
-/*
- * isp_video_buffer_prepare_pfnmap - Validate a VM_PFNMAP userspace buffer
- *
- * Userspace VM_PFNMAP buffers are supported only if they are contiguous in
- * memory and if they span a single VMA.
- *
- * Return 0 if the buffer is valid, or -EFAULT otherwise.
- */
-static int isp_video_buffer_prepare_pfnmap(struct isp_video_buffer *buf)
-{
-        struct vm_area_struct *vma;
-        unsigned long prev_pfn;
-        unsigned long this_pfn;
-        unsigned long start;
-        unsigned long end;
-        dma_addr_t pa = 0;
-        int ret = -EFAULT;
-        start = buf->vbuf.m.userptr;
-        end = buf->vbuf.m.userptr + buf->vbuf.length - 1;
-        buf->offset = start & ~PAGE_MASK;
-        buf->npages = (end >> PAGE_SHIFT) - (start >> PAGE_SHIFT) + 1;
-        buf->pages = NULL;
-        down_read(&current->mm->mmap_sem);
-        vma = find_vma(current->mm, start);
-        if (vma == NULL || vma->vm_end < end)
-                goto done;
-        for (prev_pfn = 0; start <= end; start += PAGE_SIZE) {
-                ret = follow_pfn(vma, start, &this_pfn);
-                if (ret)
-                        goto done;
-                if (prev_pfn == 0)
-                        pa = this_pfn << PAGE_SHIFT;
-                else if (this_pfn != prev_pfn + 1) {
-                        ret = -EFAULT;
-                        goto done;
-                }
-                prev_pfn = this_pfn;
-        }
-        buf->paddr = pa + buf->offset;
-        ret = 0;
-done:
-        up_read(&current->mm->mmap_sem);
-        return ret;
-}
-/*
- * isp_video_buffer_prepare_vm_flags - Get VMA flags for a userspace address
- *
- * This function locates the VMAs for the buffer's userspace address and checks
- * that their flags match. The only flag that we need to care for at the moment
- * is VM_PFNMAP.
- *
- * The buffer vm_flags field is set to the first VMA flags.
- *
- * Return -EFAULT if no VMA can be found for part of the buffer, or if the VMAs
- * have incompatible flags.
- */
-static int isp_video_buffer_prepare_vm_flags(struct isp_video_buffer *buf)
-{
-        struct vm_area_struct *vma;
-        pgprot_t uninitialized_var(vm_page_prot);
-        unsigned long start;
-        unsigned long end;
-        int ret = -EFAULT;
-        start = buf->vbuf.m.userptr;
-        end = buf->vbuf.m.userptr + buf->vbuf.length - 1;
-        down_read(&current->mm->mmap_sem);
-        do {
-                vma = find_vma(current->mm, start);
-                if (vma == NULL)
-                        goto done;
-                if (start == buf->vbuf.m.userptr) {
-                        buf->vm_flags = vma->vm_flags;
-                        vm_page_prot = vma->vm_page_prot;
-                }
-                if ((buf->vm_flags ^ vma->vm_flags) & VM_PFNMAP)
-                        goto done;
-                if (vm_page_prot != vma->vm_page_prot)
-                        goto done;
-                start = vma->vm_end + 1;
-        } while (vma->vm_end < end);
-        /* Skip cache management to enhance performances for non-cached or
-         * write-combining buffers.
-         */
-        if (vm_page_prot == pgprot_noncached(vm_page_prot) ||
-            vm_page_prot == pgprot_writecombine(vm_page_prot))
-                buf->skip_cache = true;
-        ret = 0;
-done:
-        up_read(&current->mm->mmap_sem);
-        return ret;
-}
-/*
- * isp_video_buffer_prepare - Make a buffer ready for operation
- *
- * Preparing a buffer involves:
- *
- * - validating VMAs (userspace buffers only)
- * - locking pages and VMAs into memory (userspace buffers only)
- * - building page and scatter-gather lists
- * - mapping buffers for DMA operation
- * - performing driver-specific preparation
- *
- * The function must be called in userspace context with a valid mm context
- * (this excludes cleanup paths such as sys_close when the userspace process
- * segfaults).
- */
-static int isp_video_buffer_prepare(struct isp_video_buffer *buf)
-{
-        enum dma_data_direction direction;
-        int ret;
-        switch (buf->vbuf.memory) {
-        case V4L2_MEMORY_MMAP:
-                ret = isp_video_buffer_sglist_kernel(buf);
-                break;
-        case V4L2_MEMORY_USERPTR:
-                ret = isp_video_buffer_prepare_vm_flags(buf);
-                if (ret < 0)
-                        return ret;
-                if (buf->vm_flags & VM_PFNMAP) {
-                        ret = isp_video_buffer_prepare_pfnmap(buf);
-                        if (ret < 0)
-                                return ret;
-                        ret = isp_video_buffer_sglist_pfnmap(buf);
-                } else {
-                        ret = isp_video_buffer_prepare_user(buf);
-                        if (ret < 0)
-                                return ret;
-                        ret = isp_video_buffer_sglist_user(buf);
-                }
-                break;
-        default:
-                return -EINVAL;
-        }
-        if (ret < 0)
-                goto done;
-        if (!(buf->vm_flags & VM_PFNMAP)) {
-                direction = buf->vbuf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE
-                          ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-                ret = dma_map_sg(buf->queue->dev, buf->sglist, buf->sglen,
-                                 direction);
-                if (ret != buf->sglen) {
-                        ret = -EFAULT;
-                        goto done;
-                }
-        }
-        if (buf->queue->ops->buffer_prepare)
-                ret = buf->queue->ops->buffer_prepare(buf);
-done:
-        if (ret < 0) {
-                isp_video_buffer_cleanup(buf);
-                return ret;
-        }
-        return ret;
-}
-/*
- * isp_video_queue_query - Query the status of a given buffer
- *
- * Locking: must be called with the queue lock held.
- */
-static void isp_video_buffer_query(struct isp_video_buffer *buf,
-                                   struct v4l2_buffer *vbuf)
-{
-        memcpy(vbuf, &buf->vbuf, sizeof(*vbuf));
-        if (buf->vma_use_count)
-                vbuf->flags |= V4L2_BUF_FLAG_MAPPED;
-        switch (buf->state) {
-        case ISP_BUF_STATE_ERROR:
-                vbuf->flags |= V4L2_BUF_FLAG_ERROR;
-                /* Fallthrough */
-        case ISP_BUF_STATE_DONE:
-                vbuf->flags |= V4L2_BUF_FLAG_DONE;
-                break;
-        case ISP_BUF_STATE_QUEUED:
-        case ISP_BUF_STATE_ACTIVE:
-                vbuf->flags |= V4L2_BUF_FLAG_QUEUED;
-                break;
-        case ISP_BUF_STATE_IDLE:
-        default:
-                break;
-        }
-}
-/*
- * isp_video_buffer_wait - Wait for a buffer to be ready
- *
- * In non-blocking mode, return immediately with 0 if the buffer is ready or
- * -EAGAIN if the buffer is in the QUEUED or ACTIVE state.
- *
- * In blocking mode, wait (interruptibly but with no timeout) on the buffer wait
- * queue using the same condition.
- */
-static int isp_video_buffer_wait(struct isp_video_buffer *buf, int nonblocking)
-{
-        if (nonblocking) {
-                return (buf->state != ISP_BUF_STATE_QUEUED &&
-                        buf->state != ISP_BUF_STATE_ACTIVE)
-                        ? 0 : -EAGAIN;
-        }
-        return wait_event_interruptible(buf->wait,
-                buf->state != ISP_BUF_STATE_QUEUED &&
-                buf->state != ISP_BUF_STATE_ACTIVE);
-}
-/* -----------------------------------------------------------------------------
- * Queue management
- */
-/*
- * isp_video_queue_free - Free video buffers memory
- *
- * Buffers can only be freed if the queue isn't streaming and if no buffer is
- * mapped to userspace. Return -EBUSY if those conditions aren't satisfied.
- *
- * This function must be called with the queue lock held.
- */
-static int isp_video_queue_free(struct isp_video_queue *queue)
-{
-        unsigned int i;
-        if (queue->streaming)
-                return -EBUSY;
-        for (i = 0; i < queue->count; ++i) {
-                if (queue->buffers[i]->vma_use_count != 0)
-                        return -EBUSY;
-        }
-        for (i = 0; i < queue->count; ++i) {
-                struct isp_video_buffer *buf = queue->buffers[i];
-                isp_video_buffer_cleanup(buf);
-                vfree(buf->vaddr);
-                buf->vaddr = NULL;
-                kfree(buf);
-                queue->buffers[i] = NULL;
-        }
-        INIT_LIST_HEAD(&queue->queue);
-        queue->count = 0;
-        return 0;
-}
-/*
- * isp_video_queue_alloc - Allocate video buffers memory
- *
- * This function must be called with the queue lock held.
- */
-static int isp_video_queue_alloc(struct isp_video_queue *queue,
-                                 unsigned int nbuffers,
-                                 unsigned int size, enum v4l2_memory memory)
-{
-        struct isp_video_buffer *buf;
-        unsigned int i;
-        void *mem;
-        int ret;
-        /* Start by freeing the buffers. */
-        ret = isp_video_queue_free(queue);
-        if (ret < 0)
-                return ret;
-        /* Bail out if no buffers should be allocated. */
-        if (nbuffers == 0)
-                return 0;
-        /* Initialize the allocated buffers. */
-        for (i = 0; i < nbuffers; ++i) {
-                buf = kzalloc(queue->bufsize, GFP_KERNEL);
-                if (buf == NULL)
-                        break;
-                if (memory == V4L2_MEMORY_MMAP) {
-                        /* Allocate video buffers memory for mmap mode. Align
-                         * the size to the page size.
-                         */
-                        mem = vmalloc_32_user(PAGE_ALIGN(size));
-                        if (mem == NULL) {
-                                kfree(buf);
-                                break;
-                        }
-                        buf->vbuf.m.offset = i * PAGE_ALIGN(size);
-                        buf->vaddr = mem;
-                }
-                buf->vbuf.index = i;
-                buf->vbuf.length = size;
-                buf->vbuf.type = queue->type;
-                buf->vbuf.flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-                buf->vbuf.field = V4L2_FIELD_NONE;
-                buf->vbuf.memory = memory;
-                buf->queue = queue;
-                init_waitqueue_head(&buf->wait);
-                queue->buffers[i] = buf;
-        }
-        if (i == 0)
-                return -ENOMEM;
-        queue->count = i;
-        return nbuffers;
-}
-/**
- * omap3isp_video_queue_cleanup - Clean up the video buffers queue
- * @queue: Video buffers queue
- *
- * Free all allocated resources and clean up the video buffers queue. The queue
- * must not be busy (no ongoing video stream) and buffers must have been
- * unmapped.
- *
- * Return 0 on success or -EBUSY if the queue is busy or buffers haven't been
- * unmapped.
- */
-int omap3isp_video_queue_cleanup(struct isp_video_queue *queue)
-{
-        return isp_video_queue_free(queue);
-}
-/**
- * omap3isp_video_queue_init - Initialize the video buffers queue
- * @queue: Video buffers queue
- * @type: V4L2 buffer type (capture or output)
- * @ops: Driver-specific queue operations
- * @dev: Device used for DMA operations
- * @bufsize: Size of the driver-specific buffer structure
- *
- * Initialize the video buffers queue with the supplied parameters.
- *
- * The queue type must be one of V4L2_BUF_TYPE_VIDEO_CAPTURE or
- * V4L2_BUF_TYPE_VIDEO_OUTPUT. Other buffer types are not supported yet.
- *
- * Buffer objects will be allocated using the given buffer size to allow room
- * for driver-specific fields. Driver-specific buffer structures must start
- * with a struct isp_video_buffer field. Drivers with no driver-specific buffer
- * structure must pass the size of the isp_video_buffer structure in the bufsize
- * parameter.
- *
- * Return 0 on success.
- */
-int omap3isp_video_queue_init(struct isp_video_queue *queue,
-                              enum v4l2_buf_type type,
-                              const struct isp_video_queue_operations *ops,
-                              struct device *dev, unsigned int bufsize)
-{
-        INIT_LIST_HEAD(&queue->queue);
-        mutex_init(&queue->lock);
-        spin_lock_init(&queue->irqlock);
-        queue->type = type;
-        queue->ops = ops;
-        queue->dev = dev;
-        queue->bufsize = bufsize;
-        return 0;
-}
-/* -----------------------------------------------------------------------------
- * V4L2 operations
- */
-/**
- * omap3isp_video_queue_reqbufs - Allocate video buffers memory
- *
- * This function is intended to be used as a VIDIOC_REQBUFS ioctl handler. It
- * allocated video buffer objects and, for MMAP buffers, buffer memory.
- *
- * If the number of buffers is 0, all buffers are freed and the function returns
- * without performing any allocation.
- *
- * If the number of buffers is not 0, currently allocated buffers (if any) are
- * freed and the requested number of buffers are allocated. Depending on
- * driver-specific requirements and on memory availability, a number of buffer
- * smaller or bigger than requested can be allocated. This isn't considered as
- * an error.
- *
- * Return 0 on success or one of the following error codes:
- *
- * -EINVAL if the buffer type or index are invalid
- * -EBUSY if the queue is busy (streaming or buffers mapped)
- * -ENOMEM if the buffers can't be allocated due to an out-of-memory condition
- */
-int omap3isp_video_queue_reqbufs(struct isp_video_queue *queue,
-                                 struct v4l2_requestbuffers *rb)
-{
-        unsigned int nbuffers = rb->count;
-        unsigned int size;
-        int ret;
-        if (rb->type != queue->type)
-                return -EINVAL;
-        queue->ops->queue_prepare(queue, &nbuffers, &size);
-        if (size == 0)
-                return -EINVAL;
-        nbuffers = min_t(unsigned int, nbuffers, ISP_VIDEO_MAX_BUFFERS);
-        mutex_lock(&queue->lock);
-        ret = isp_video_queue_alloc(queue, nbuffers, size, rb->memory);
-        if (ret < 0)
-                goto done;
-        rb->count = ret;
-        ret = 0;
-done:
-        mutex_unlock(&queue->lock);
-        return ret;
-}
-/**
- * omap3isp_video_queue_querybuf - Query the status of a buffer in a queue
- *
- * This function is intended to be used as a VIDIOC_QUERYBUF ioctl handler. It
- * returns the status of a given video buffer.
- *
- * Return 0 on success or -EINVAL if the buffer type or index are invalid.
- */
-int omap3isp_video_queue_querybuf(struct isp_video_queue *queue,
-                                  struct v4l2_buffer *vbuf)
-{
-        struct isp_video_buffer *buf;
-        int ret = 0;
-        if (vbuf->type != queue->type)
-                return -EINVAL;
-        mutex_lock(&queue->lock);
-        if (vbuf->index >= queue->count) {
-                ret = -EINVAL;
-                goto done;
-        }
-        buf = queue->buffers[vbuf->index];
-        isp_video_buffer_query(buf, vbuf);
-done:
-        mutex_unlock(&queue->lock);
-        return ret;
-}
-/**
- * omap3isp_video_queue_qbuf - Queue a buffer
- *
- * This function is intended to be used as a VIDIOC_QBUF ioctl handler.
- *
- * The v4l2_buffer structure passed from userspace is first sanity tested. If
- * sane, the buffer is then processed and added to the main queue and, if the
- * queue is streaming, to the IRQ queue.
- *
- * Before being enqueued, USERPTR buffers are checked for address changes. If
- * the buffer has a different userspace address, the old memory area is unlocked
- * and the new memory area is locked.
- */
-int omap3isp_video_queue_qbuf(struct isp_video_queue *queue,
-                              struct v4l2_buffer *vbuf)
-{
-        struct isp_video_buffer *buf;
-        unsigned long flags;
-        int ret = -EINVAL;
-        if (vbuf->type != queue->type)
-                goto done;
-        mutex_lock(&queue->lock);
-        if (vbuf->index >= queue->count)
-                goto done;
-        buf = queue->buffers[vbuf->index];
-        if (vbuf->memory != buf->vbuf.memory)
-                goto done;
-        if (buf->state != ISP_BUF_STATE_IDLE)
-                goto done;
-        if (vbuf->memory == V4L2_MEMORY_USERPTR &&
-            vbuf->length < buf->vbuf.length)
-                goto done;
-        if (vbuf->memory == V4L2_MEMORY_USERPTR &&
-            vbuf->m.userptr != buf->vbuf.m.userptr) {
-                isp_video_buffer_cleanup(buf);
-                buf->vbuf.m.userptr = vbuf->m.userptr;
-                buf->prepared = 0;
-        }
-        if (!buf->prepared) {
-                ret = isp_video_buffer_prepare(buf);
-                if (ret < 0)
-                        goto done;
-                buf->prepared = 1;
-        }
-        isp_video_buffer_cache_sync(buf);
-        buf->state = ISP_BUF_STATE_QUEUED;
-        list_add_tail(&buf->stream, &queue->queue);
-        if (queue->streaming) {
-                spin_lock_irqsave(&queue->irqlock, flags);
-                queue->ops->buffer_queue(buf);
-                spin_unlock_irqrestore(&queue->irqlock, flags);
-        }
-        ret = 0;
-done:
-        mutex_unlock(&queue->lock);
-        return ret;
-}
-/**
- * omap3isp_video_queue_dqbuf - Dequeue a buffer
- *
- * This function is intended to be used as a VIDIOC_DQBUF ioctl handler.
- *
- * Wait until a buffer is ready to be dequeued, remove it from the queue and
- * copy its information to the v4l2_buffer structure.
- *
- * If the nonblocking argument is not zero and no buffer is ready, return
- * -EAGAIN immediately instead of waiting.
- *
- * If no buffer has been enqueued, or if the requested buffer type doesn't match
- * the queue type, return -EINVAL.
- */
-int omap3isp_video_queue_dqbuf(struct isp_video_queue *queue,
-                               struct v4l2_buffer *vbuf, int nonblocking)
-{
-        struct isp_video_buffer *buf;
-        int ret;
-        if (vbuf->type != queue->type)
-                return -EINVAL;
-        mutex_lock(&queue->lock);
-        if (list_empty(&queue->queue)) {
-                ret = -EINVAL;
-                goto done;
-        }
-        buf = list_first_entry(&queue->queue, struct isp_video_buffer, stream);
-        ret = isp_video_buffer_wait(buf, nonblocking);
-        if (ret < 0)
-                goto done;
-        list_del(&buf->stream);
-        isp_video_buffer_query(buf, vbuf);
-        buf->state = ISP_BUF_STATE_IDLE;
-        vbuf->flags &= ~V4L2_BUF_FLAG_QUEUED;
-done:
-        mutex_unlock(&queue->lock);
-        return ret;
-}
-/**
- * omap3isp_video_queue_streamon - Start streaming
- *
- * This function is intended to be used as a VIDIOC_STREAMON ioctl handler. It
- * starts streaming on the queue and calls the buffer_queue operation for all
- * queued buffers.
- *
- * Return 0 on success.
- */
-int omap3isp_video_queue_streamon(struct isp_video_queue *queue)
-{
-        struct isp_video_buffer *buf;
-        unsigned long flags;
-        mutex_lock(&queue->lock);
-        if (queue->streaming)
-                goto done;
-        queue->streaming = 1;
-        spin_lock_irqsave(&queue->irqlock, flags);
-        list_for_each_entry(buf, &queue->queue, stream)
-                queue->ops->buffer_queue(buf);
-        spin_unlock_irqrestore(&queue->irqlock, flags);
-done:
-        mutex_unlock(&queue->lock);
-        return 0;
-}
-/**
- * omap3isp_video_queue_streamoff - Stop streaming
- *
- * This function is intended to be used as a VIDIOC_STREAMOFF ioctl handler. It
- * stops streaming on the queue and wakes up all the buffers.
- *
- * Drivers must stop the hardware and synchronize with interrupt handlers and/or
- * delayed works before calling this function to make sure no buffer will be
- * touched by the driver and/or hardware.
- */
-void omap3isp_video_queue_streamoff(struct isp_video_queue *queue)
-{
-        struct isp_video_buffer *buf;
-        unsigned long flags;
-        unsigned int i;
-        mutex_lock(&queue->lock);
-        if (!queue->streaming)
-                goto done;
-        queue->streaming = 0;
-        spin_lock_irqsave(&queue->irqlock, flags);
-        for (i = 0; i < queue->count; ++i) {
-                buf = queue->buffers[i];
-                if (buf->state == ISP_BUF_STATE_ACTIVE)
-                        wake_up(&buf->wait);
-                buf->state = ISP_BUF_STATE_IDLE;
-        }
-        spin_unlock_irqrestore(&queue->irqlock, flags);
-        INIT_LIST_HEAD(&queue->queue);
-done:
-        mutex_unlock(&queue->lock);
-}
-/**
- * omap3isp_video_queue_discard_done - Discard all buffers marked as DONE
- *
- * This function is intended to be used with suspend/resume operations. It
- * discards all 'done' buffers as they would be too old to be requested after
- * resume.
- *
- * Drivers must stop the hardware and synchronize with interrupt handlers and/or
- * delayed works before calling this function to make sure no buffer will be
- * touched by the driver and/or hardware.
- */
-void omap3isp_video_queue_discard_done(struct isp_video_queue *queue)
-{
-        struct isp_video_buffer *buf;
-        unsigned int i;
-        mutex_lock(&queue->lock);
-        if (!queue->streaming)
-                goto done;
-        for (i = 0; i < queue->count; ++i) {
-                buf = queue->buffers[i];
-                if (buf->state == ISP_BUF_STATE_DONE)
-                        buf->state = ISP_BUF_STATE_ERROR;
-        }
-done:
-        mutex_unlock(&queue->lock);
-}
-static void isp_video_queue_vm_open(struct vm_area_struct *vma)
-{
-        struct isp_video_buffer *buf = vma->vm_private_data;
-        buf->vma_use_count++;
-}
-static void isp_video_queue_vm_close(struct vm_area_struct *vma)
-{
-        struct isp_video_buffer *buf = vma->vm_private_data;
-        buf->vma_use_count--;
-}
-static const struct vm_operations_struct isp_video_queue_vm_ops = {
-        .open = isp_video_queue_vm_open,
-        .close = isp_video_queue_vm_close,
-};
-/**
- * omap3isp_video_queue_mmap - Map buffers to userspace
- *
- * This function is intended to be used as an mmap() file operation handler. It
- * maps a buffer to userspace based on the VMA offset.
- *
- * Only buffers of memory type MMAP are supported.
- */
-int omap3isp_video_queue_mmap(struct isp_video_queue *queue,
-                         struct vm_area_struct *vma)
-{
-        struct isp_video_buffer *uninitialized_var(buf);
-        unsigned long size;
-        unsigned int i;
-        int ret = 0;
-        mutex_lock(&queue->lock);
-        for (i = 0; i < queue->count; ++i) {
-                buf = queue->buffers[i];
-                if ((buf->vbuf.m.offset >> PAGE_SHIFT) == vma->vm_pgoff)
-                        break;
-        }
-        if (i == queue->count) {
-                ret = -EINVAL;
-                goto done;
-        }
-        size = vma->vm_end - vma->vm_start;
-        if (buf->vbuf.memory != V4L2_MEMORY_MMAP ||
-            size != PAGE_ALIGN(buf->vbuf.length)) {
-                ret = -EINVAL;
-                goto done;
-        }
-        ret = remap_vmalloc_range(vma, buf->vaddr, 0);
-        if (ret < 0)
-                goto done;
-        vma->vm_ops = &isp_video_queue_vm_ops;
-        vma->vm_private_data = buf;
-        isp_video_queue_vm_open(vma);
-done:
-        mutex_unlock(&queue->lock);
-        return ret;
-}
-/**
- * omap3isp_video_queue_poll - Poll video queue state
- *
- * This function is intended to be used as a poll() file operation handler. It
- * polls the state of the video buffer at the front of the queue and returns an
- * events mask.
- *
- * If no buffer is present at the front of the queue, POLLERR is returned.
- */
-unsigned int omap3isp_video_queue_poll(struct isp_video_queue *queue,
-                                       struct file *file, poll_table *wait)
-{
-        struct isp_video_buffer *buf;
-        unsigned int mask = 0;
-        mutex_lock(&queue->lock);
-        if (list_empty(&queue->queue)) {
-                mask |= POLLERR;
-                goto done;
-        }
-        buf = list_first_entry(&queue->queue, struct isp_video_buffer, stream);
-        poll_wait(file, &buf->wait, wait);
-        if (buf->state == ISP_BUF_STATE_DONE ||
-            buf->state == ISP_BUF_STATE_ERROR) {
-                if (queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                        mask |= POLLIN | POLLRDNORM;
-                else
-                        mask |= POLLOUT | POLLWRNORM;
-        }
-done:
-        mutex_unlock(&queue->lock);
-        return mask;
-}
diff --git a/drivers/media/platform/omap3isp/ispqueue.h b/drivers/media/platform/omap3isp/ispqueue.h
deleted file mode 100644
index 3e048ad65647..000000000000
--- a/drivers/media/platform/omap3isp/ispqueue.h
+++ /dev/null
@@ -1,188 +0,0 @@
-/*
- * ispqueue.h
- *
- * TI OMAP3 ISP - Video buffers queue handling
- *
- * Copyright (C) 2010 Nokia Corporation
- *
- * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
- *           Sakari Ailus <sakari.ailus@iki.fi>
- *
- * 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.
- *
- * 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.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- */
-#ifndef OMAP3_ISP_QUEUE_H
-#define OMAP3_ISP_QUEUE_H
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/mm_types.h>
-#include <linux/mutex.h>
-#include <linux/videodev2.h>
-#include <linux/wait.h>
-struct isp_video_queue;
-struct page;
-struct scatterlist;
-#define ISP_VIDEO_MAX_BUFFERS           16
-/**
- * enum isp_video_buffer_state - ISP video buffer state
- * @ISP_BUF_STATE_IDLE: The buffer is under userspace control (dequeued
- *      or not queued yet).
- * @ISP_BUF_STATE_QUEUED: The buffer has been queued but isn't used by the
- *      device yet.
- * @ISP_BUF_STATE_ACTIVE: The buffer is in use for an active video transfer.
- * @ISP_BUF_STATE_ERROR: The device is done with the buffer and an error
- *      occurred. For capture device the buffer likely contains corrupted data or
- *      no data at all.
- * @ISP_BUF_STATE_DONE: The device is done with the buffer and no error occurred.
- *      For capture devices the buffer contains valid data.
- */
-enum isp_video_buffer_state {
-        ISP_BUF_STATE_IDLE,
-        ISP_BUF_STATE_QUEUED,
-        ISP_BUF_STATE_ACTIVE,
-        ISP_BUF_STATE_ERROR,
-        ISP_BUF_STATE_DONE,
-};
-/**
- * struct isp_video_buffer - ISP video buffer
- * @vma_use_count: Number of times the buffer is mmap'ed to userspace
- * @stream: List head for insertion into main queue
- * @queue: ISP buffers queue this buffer belongs to
- * @prepared: Whether the buffer has been prepared
- * @skip_cache: Whether to skip cache management operations for this buffer
- * @vaddr: Memory virtual address (for kernel buffers)
- * @vm_flags: Buffer VMA flags (for userspace buffers)
- * @offset: Offset inside the first page (for userspace buffers)
- * @npages: Number of pages (for userspace buffers)
- * @pages: Pages table (for userspace non-VM_PFNMAP buffers)
- * @paddr: Memory physical address (for userspace VM_PFNMAP buffers)
- * @sglen: Number of elements in the scatter list (for non-VM_PFNMAP buffers)
- * @sglist: Scatter list (for non-VM_PFNMAP buffers)
- * @vbuf: V4L2 buffer
- * @irqlist: List head for insertion into IRQ queue
- * @state: Current buffer state
- * @wait: Wait queue to signal buffer completion
- */
-struct isp_video_buffer {
-        unsigned long vma_use_count;
-        struct list_head stream;
-        struct isp_video_queue *queue;
-        unsigned int prepared:1;
-        bool skip_cache;
-        /* For kernel buffers. */
-        void *vaddr;
-        /* For userspace buffers. */
-        vm_flags_t vm_flags;
-        unsigned long offset;
-        unsigned int npages;
-        struct page **pages;
-        dma_addr_t paddr;
-        /* For all buffers except VM_PFNMAP. */
-        unsigned int sglen;
-        struct scatterlist *sglist;
-        /* Touched by the interrupt handler. */
-        struct v4l2_buffer vbuf;
-        struct list_head irqlist;
-        enum isp_video_buffer_state state;
-        wait_queue_head_t wait;
-};
-#define to_isp_video_buffer(vb) container_of(vb, struct isp_video_buffer, vb)
-/**
- * struct isp_video_queue_operations - Driver-specific operations
- * @queue_prepare: Called before allocating buffers. Drivers should clamp the
- *      number of buffers according to their requirements, and must return the
- *      buffer size in bytes.
- * @buffer_prepare: Called the first time a buffer is queued, or after changing
- *      the userspace memory address for a USERPTR buffer, with the queue lock
- *      held. Drivers should perform device-specific buffer preparation (such as
- *      mapping the buffer memory in an IOMMU). This operation is optional.
- * @buffer_queue: Called when a buffer is being added to the queue with the
- *      queue irqlock spinlock held.
- * @buffer_cleanup: Called before freeing buffers, or before changing the
- *      userspace memory address for a USERPTR buffer, with the queue lock held.
- *      Drivers must perform cleanup operations required to undo the
- *      buffer_prepare call. This operation is optional.
- */
-struct isp_video_queue_operations {
-        void (*queue_prepare)(struct isp_video_queue *queue,
-                              unsigned int *nbuffers, unsigned int *size);
-        int  (*buffer_prepare)(struct isp_video_buffer *buf);
-        void (*buffer_queue)(struct isp_video_buffer *buf);
-        void (*buffer_cleanup)(struct isp_video_buffer *buf);
-};
-/**
- * struct isp_video_queue - ISP video buffers queue
- * @type: Type of video buffers handled by this queue
- * @ops: Queue operations
- * @dev: Device used for DMA operations
- * @bufsize: Size of a driver-specific buffer object
- * @count: Number of currently allocated buffers
- * @buffers: ISP video buffers
- * @lock: Mutex to protect access to the buffers, main queue and state
- * @irqlock: Spinlock to protect access to the IRQ queue
- * @streaming: Queue state, indicates whether the queue is streaming
- * @queue: List of all queued buffers
- */
-struct isp_video_queue {
-        enum v4l2_buf_type type;
-        const struct isp_video_queue_operations *ops;
-        struct device *dev;
-        unsigned int bufsize;
-        unsigned int count;
-        struct isp_video_buffer *buffers[ISP_VIDEO_MAX_BUFFERS];
-        struct mutex lock;
-        spinlock_t irqlock;
-        unsigned int streaming:1;
-        struct list_head queue;
-};
-int omap3isp_video_queue_cleanup(struct isp_video_queue *queue);
-int omap3isp_video_queue_init(struct isp_video_queue *queue,
-                              enum v4l2_buf_type type,
-                              const struct isp_video_queue_operations *ops,
-                              struct device *dev, unsigned int bufsize);
-int omap3isp_video_queue_reqbufs(struct isp_video_queue *queue,
-                                 struct v4l2_requestbuffers *rb);
-int omap3isp_video_queue_querybuf(struct isp_video_queue *queue,
-                                  struct v4l2_buffer *vbuf);
-int omap3isp_video_queue_qbuf(struct isp_video_queue *queue,
-                              struct v4l2_buffer *vbuf);
-int omap3isp_video_queue_dqbuf(struct isp_video_queue *queue,
-                               struct v4l2_buffer *vbuf, int nonblocking);
-int omap3isp_video_queue_streamon(struct isp_video_queue *queue);
-void omap3isp_video_queue_streamoff(struct isp_video_queue *queue);
-void omap3isp_video_queue_discard_done(struct isp_video_queue *queue);
-int omap3isp_video_queue_mmap(struct isp_video_queue *queue,
-                              struct vm_area_struct *vma);
-unsigned int omap3isp_video_queue_poll(struct isp_video_queue *queue,
-                                       struct file *file, poll_table *wait);
-#endif /* OMAP3_ISP_QUEUE_H */
diff --git a/drivers/media/platform/omap3isp/ispresizer.c b/drivers/media/platform/omap3isp/ispresizer.c
index 86369df81d74..6f077c2377db 100644
--- a/drivers/media/platform/omap3isp/ispresizer.c
+++ b/drivers/media/platform/omap3isp/ispresizer.c
@@ -1040,7 +1040,7 @@ static void resizer_isr_buffer(struct isp_res_device *res)
         */
        buffer = omap3isp_video_buffer_next(&res->video_out);
        if (buffer != NULL) {
-                resizer_set_outaddr(res, buffer->isp_addr);
+                resizer_set_outaddr(res, buffer->dma);
                restart = 1;
        }
@@ -1049,7 +1049,7 @@ static void resizer_isr_buffer(struct isp_res_device *res)
        if (res->input == RESIZER_INPUT_MEMORY) {
                buffer = omap3isp_video_buffer_next(&res->video_in);
                if (buffer != NULL)
-                        resizer_set_inaddr(res, buffer->isp_addr);
+                        resizer_set_inaddr(res, buffer->dma);
                pipe->state |= ISP_PIPELINE_IDLE_INPUT;
        }
@@ -1101,7 +1101,7 @@ static int resizer_video_queue(struct isp_video *video,
        struct isp_res_device *res = &video->isp->isp_res;
        if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
-                resizer_set_inaddr(res, buffer->isp_addr);
+                resizer_set_inaddr(res, buffer->dma);
        /*
         * We now have a buffer queued on the output. Despite what the
@@ -1116,7 +1116,7 @@ static int resizer_video_queue(struct isp_video *video,
         * continuous mode or when starting the stream.
         */
        if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                resizer_set_outaddr(res, buffer->isp_addr);
+                resizer_set_outaddr(res, buffer->dma);
        return 0;
 }
diff --git a/drivers/media/platform/omap3isp/ispstat.c b/drivers/media/platform/omap3isp/ispstat.c
index 5707f85c4cc4..e6cbc1eaf4ca 100644
--- a/drivers/media/platform/omap3isp/ispstat.c
+++ b/drivers/media/platform/omap3isp/ispstat.c
@@ -26,13 +26,12 @@
 */
 #include <linux/dma-mapping.h>
-#include <linux/omap-iommu.h>
 #include <linux/slab.h>
 #include <linux/uaccess.h>
 #include "isp.h"
-#define IS_COHERENT_BUF(stat)   ((stat)->dma_ch >= 0)
+#define ISP_STAT_USES_DMAENGINE(stat)   ((stat)->dma_ch >= 0)
 /*
 * MAGIC_SIZE must always be the greatest common divisor of
@@ -77,21 +76,10 @@ static void __isp_stat_buf_sync_magic(struct ispstat *stat,
                                        dma_addr_t, unsigned long, size_t,
                                        enum dma_data_direction))
 {
-        struct device *dev = stat->isp->dev;
+        /* Sync the initial and final magic words. */
-        struct page *pg;
+        dma_sync(stat->isp->dev, buf->dma_addr, 0, MAGIC_SIZE, dir);
-        dma_addr_t dma_addr;
+        dma_sync(stat->isp->dev, buf->dma_addr + (buf_size & PAGE_MASK),
-        u32 offset;
+                 buf_size & ~PAGE_MASK, MAGIC_SIZE, dir);
-        /* Initial magic words */
-        pg = vmalloc_to_page(buf->virt_addr);
-        dma_addr = pfn_to_dma(dev, page_to_pfn(pg));
-        dma_sync(dev, dma_addr, 0, MAGIC_SIZE, dir);
-        /* Final magic words */
-        pg = vmalloc_to_page(buf->virt_addr + buf_size);
-        dma_addr = pfn_to_dma(dev, page_to_pfn(pg));
-        offset = ((u32)buf->virt_addr + buf_size) & ~PAGE_MASK;
-        dma_sync(dev, dma_addr, offset, MAGIC_SIZE, dir);
 }
 static void isp_stat_buf_sync_magic_for_device(struct ispstat *stat,
@@ -99,7 +87,7 @@ static void isp_stat_buf_sync_magic_for_device(struct ispstat *stat,
                                               u32 buf_size,
                                               enum dma_data_direction dir)
 {
-        if (IS_COHERENT_BUF(stat))
+        if (ISP_STAT_USES_DMAENGINE(stat))
                return;
        __isp_stat_buf_sync_magic(stat, buf, buf_size, dir,
@@ -111,7 +99,7 @@ static void isp_stat_buf_sync_magic_for_cpu(struct ispstat *stat,
                                            u32 buf_size,
                                            enum dma_data_direction dir)
 {
-        if (IS_COHERENT_BUF(stat))
+        if (ISP_STAT_USES_DMAENGINE(stat))
                return;
        __isp_stat_buf_sync_magic(stat, buf, buf_size, dir,
@@ -180,21 +168,21 @@ static void isp_stat_buf_insert_magic(struct ispstat *stat,
 static void isp_stat_buf_sync_for_device(struct ispstat *stat,
                                         struct ispstat_buffer *buf)
 {
-        if (IS_COHERENT_BUF(stat))
+        if (ISP_STAT_USES_DMAENGINE(stat))
                return;
-        dma_sync_sg_for_device(stat->isp->dev, buf->iovm->sgt->sgl,
+        dma_sync_sg_for_device(stat->isp->dev, buf->sgt.sgl,
-                               buf->iovm->sgt->nents, DMA_FROM_DEVICE);
+                               buf->sgt.nents, DMA_FROM_DEVICE);
 }
 static void isp_stat_buf_sync_for_cpu(struct ispstat *stat,
                                      struct ispstat_buffer *buf)
 {
-        if (IS_COHERENT_BUF(stat))
+        if (ISP_STAT_USES_DMAENGINE(stat))
                return;
-        dma_sync_sg_for_cpu(stat->isp->dev, buf->iovm->sgt->sgl,
+        dma_sync_sg_for_cpu(stat->isp->dev, buf->sgt.sgl,
-                            buf->iovm->sgt->nents, DMA_FROM_DEVICE);
+                            buf->sgt.nents, DMA_FROM_DEVICE);
 }
 static void isp_stat_buf_clear(struct ispstat *stat)
@@ -354,29 +342,21 @@ static struct ispstat_buffer *isp_stat_buf_get(struct ispstat *stat,
 static void isp_stat_bufs_free(struct ispstat *stat)
 {
-        struct isp_device *isp = stat->isp;
+        struct device *dev = ISP_STAT_USES_DMAENGINE(stat)
-        int i;
+                           ? NULL : stat->isp->dev;
+        unsigned int i;
        for (i = 0; i < STAT_MAX_BUFS; i++) {
                struct ispstat_buffer *buf = &stat->buf[i];
-                if (!IS_COHERENT_BUF(stat)) {
+                if (!buf->virt_addr)
-                        if (IS_ERR_OR_NULL((void *)buf->iommu_addr))
+                        continue;
-                                continue;
-                        if (buf->iovm)
+                sg_free_table(&buf->sgt);
-                                dma_unmap_sg(isp->dev, buf->iovm->sgt->sgl,
-                                             buf->iovm->sgt->nents,
+                dma_free_coherent(dev, stat->buf_alloc_size, buf->virt_addr,
-                                             DMA_FROM_DEVICE);
+                                  buf->dma_addr);
-                        omap_iommu_vfree(isp->domain, isp->dev,
-                                                        buf->iommu_addr);
-                } else {
-                        if (!buf->virt_addr)
-                                continue;
-                        dma_free_coherent(stat->isp->dev, stat->buf_alloc_size,
-                                          buf->virt_addr, buf->dma_addr);
-                }
-                buf->iommu_addr = 0;
-                buf->iovm = NULL;
                buf->dma_addr = 0;
                buf->virt_addr = NULL;
                buf->empty = 1;
@@ -389,83 +369,51 @@ static void isp_stat_bufs_free(struct ispstat *stat)
        stat->active_buf = NULL;
 }
-static int isp_stat_bufs_alloc_iommu(struct ispstat *stat, unsigned int size)
+static int isp_stat_bufs_alloc_one(struct device *dev,
-{
+                                   struct ispstat_buffer *buf,
-        struct isp_device *isp = stat->isp;
+                                   unsigned int size)
-        int i;
-        stat->buf_alloc_size = size;
-        for (i = 0; i < STAT_MAX_BUFS; i++) {
-                struct ispstat_buffer *buf = &stat->buf[i];
-                struct iovm_struct *iovm;
-                WARN_ON(buf->dma_addr);
-                buf->iommu_addr = omap_iommu_vmalloc(isp->domain, isp->dev, 0,
-                                                        size, IOMMU_FLAG);
-                if (IS_ERR((void *)buf->iommu_addr)) {
-                        dev_err(stat->isp->dev,
-                                 "%s: Can't acquire memory for "
-                                 "buffer %d\n", stat->subdev.name, i);
-                        isp_stat_bufs_free(stat);
-                        return -ENOMEM;
-                }
-                iovm = omap_find_iovm_area(isp->dev, buf->iommu_addr);
-                if (!iovm ||
-                    !dma_map_sg(isp->dev, iovm->sgt->sgl, iovm->sgt->nents,
-                                DMA_FROM_DEVICE)) {
-                        isp_stat_bufs_free(stat);
-                        return -ENOMEM;
-                }
-                buf->iovm = iovm;
-                buf->virt_addr = omap_da_to_va(stat->isp->dev,
-                                          (u32)buf->iommu_addr);
-                buf->empty = 1;
-                dev_dbg(stat->isp->dev, "%s: buffer[%d] allocated."
-                        "iommu_addr=0x%08lx virt_addr=0x%08lx",
-                        stat->subdev.name, i, buf->iommu_addr,
-                        (unsigned long)buf->virt_addr);
-        }
-        return 0;
-}
-static int isp_stat_bufs_alloc_dma(struct ispstat *stat, unsigned int size)
 {
-        int i;
+        int ret;
-        stat->buf_alloc_size = size;
-        for (i = 0; i < STAT_MAX_BUFS; i++) {
-                struct ispstat_buffer *buf = &stat->buf[i];
-                WARN_ON(buf->iommu_addr);
-                buf->virt_addr = dma_alloc_coherent(stat->isp->dev, size,
-                                        &buf->dma_addr, GFP_KERNEL | GFP_DMA);
-                if (!buf->virt_addr || !buf->dma_addr) {
+        buf->virt_addr = dma_alloc_coherent(dev, size, &buf->dma_addr,
-                        dev_info(stat->isp->dev,
+                                            GFP_KERNEL | GFP_DMA);
-                                 "%s: Can't acquire memory for "
+        if (!buf->virt_addr)
-                                 "DMA buffer %d\n", stat->subdev.name, i);
+                return -ENOMEM;
-                        isp_stat_bufs_free(stat);
-                        return -ENOMEM;
-                }
-                buf->empty = 1;
-                dev_dbg(stat->isp->dev, "%s: buffer[%d] allocated."
+        ret = dma_get_sgtable(dev, &buf->sgt, buf->virt_addr, buf->dma_addr,
-                        "dma_addr=0x%08lx virt_addr=0x%08lx\n",
+                              size);
-                        stat->subdev.name, i, (unsigned long)buf->dma_addr,
+        if (ret < 0) {
-                        (unsigned long)buf->virt_addr);
+                dma_free_coherent(dev, size, buf->virt_addr, buf->dma_addr);
+                buf->virt_addr = NULL;
+                buf->dma_addr = 0;
+                return ret;
        }
        return 0;
 }
+/*
+ * The device passed to the DMA API depends on whether the statistics block uses
+ * ISP DMA, external DMA or PIO to transfer data.
+ *
+ * The first case (for the AEWB and AF engines) passes the ISP device, resulting
+ * in the DMA buffers being mapped through the ISP IOMMU.
+ *
+ * The second case (for the histogram engine) should pass the DMA engine device.
+ * As that device isn't accessible through the OMAP DMA engine API the driver
+ * passes NULL instead, resulting in the buffers being mapped directly as
+ * physical pages.
+ *
+ * The third case (for the histogram engine) doesn't require any mapping. The
+ * buffers could be allocated with kmalloc/vmalloc, but we still use
+ * dma_alloc_coherent() for consistency purpose.
+ */
 static int isp_stat_bufs_alloc(struct ispstat *stat, u32 size)
 {
+        struct device *dev = ISP_STAT_USES_DMAENGINE(stat)
+                           ? NULL : stat->isp->dev;
        unsigned long flags;
+        unsigned int i;
        spin_lock_irqsave(&stat->isp->stat_lock, flags);
@@ -489,10 +437,31 @@ static int isp_stat_bufs_alloc(struct ispstat *stat, u32 size)
        isp_stat_bufs_free(stat);
-        if (IS_COHERENT_BUF(stat))
+        stat->buf_alloc_size = size;
-                return isp_stat_bufs_alloc_dma(stat, size);
-        else
+        for (i = 0; i < STAT_MAX_BUFS; i++) {
-                return isp_stat_bufs_alloc_iommu(stat, size);
+                struct ispstat_buffer *buf = &stat->buf[i];
+                int ret;
+                ret = isp_stat_bufs_alloc_one(dev, buf, size);
+                if (ret < 0) {
+                        dev_err(stat->isp->dev,
+                                "%s: Failed to allocate DMA buffer %u\n",
+                                stat->subdev.name, i);
+                        isp_stat_bufs_free(stat);
+                        return ret;
+                }
+                buf->empty = 1;
+                dev_dbg(stat->isp->dev,
+                        "%s: buffer[%u] allocated. dma=0x%08lx virt=0x%08lx",
+                        stat->subdev.name, i,
+                        (unsigned long)buf->dma_addr,
+                        (unsigned long)buf->virt_addr);
+        }
+        return 0;
 }
 static void isp_stat_queue_event(struct ispstat *stat, int err)
diff --git a/drivers/media/platform/omap3isp/ispstat.h b/drivers/media/platform/omap3isp/ispstat.h
index 9a047c929b9f..58d6ac7cb664 100644
--- a/drivers/media/platform/omap3isp/ispstat.h
+++ b/drivers/media/platform/omap3isp/ispstat.h
@@ -46,8 +46,7 @@
 struct ispstat;
 struct ispstat_buffer {
-        unsigned long iommu_addr;
+        struct sg_table sgt;
-        struct iovm_struct *iovm;
        void *virt_addr;
        dma_addr_t dma_addr;
        struct timespec ts;
diff --git a/drivers/media/platform/omap3isp/ispvideo.c b/drivers/media/platform/omap3isp/ispvideo.c
index 85b4036ba5e4..e36bac26476c 100644
--- a/drivers/media/platform/omap3isp/ispvideo.c
+++ b/drivers/media/platform/omap3isp/ispvideo.c
@@ -27,7 +27,6 @@
 #include <linux/clk.h>
 #include <linux/mm.h>
 #include <linux/module.h>
-#include <linux/omap-iommu.h>
 #include <linux/pagemap.h>
 #include <linux/scatterlist.h>
 #include <linux/sched.h>
@@ -35,6 +34,7 @@
 #include <linux/vmalloc.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
 #include "ispvideo.h"
 #include "isp.h"
@@ -326,90 +326,36 @@ isp_video_check_format(struct isp_video *video, struct isp_video_fh *vfh)
 }
 /* -----------------------------------------------------------------------------
- * IOMMU management
- */
-#define IOMMU_FLAG      (IOVMF_ENDIAN_LITTLE | IOVMF_ELSZ_8)
-/*
- * ispmmu_vmap - Wrapper for Virtual memory mapping of a scatter gather list
- * @isp: Device pointer specific to the OMAP3 ISP.
- * @sglist: Pointer to source Scatter gather list to allocate.
- * @sglen: Number of elements of the scatter-gatter list.
- *
- * Returns a resulting mapped device address by the ISP MMU, or -ENOMEM if
- * we ran out of memory.
- */
-static dma_addr_t
-ispmmu_vmap(struct isp_device *isp, const struct scatterlist *sglist, int sglen)
-{
-        struct sg_table *sgt;
-        u32 da;
-        sgt = kmalloc(sizeof(*sgt), GFP_KERNEL);
-        if (sgt == NULL)
-                return -ENOMEM;
-        sgt->sgl = (struct scatterlist *)sglist;
-        sgt->nents = sglen;
-        sgt->orig_nents = sglen;
-        da = omap_iommu_vmap(isp->domain, isp->dev, 0, sgt, IOMMU_FLAG);
-        if (IS_ERR_VALUE(da))
-                kfree(sgt);
-        return da;
-}
-/*
- * ispmmu_vunmap - Unmap a device address from the ISP MMU
- * @isp: Device pointer specific to the OMAP3 ISP.
- * @da: Device address generated from a ispmmu_vmap call.
- */
-static void ispmmu_vunmap(struct isp_device *isp, dma_addr_t da)
-{
-        struct sg_table *sgt;
-        sgt = omap_iommu_vunmap(isp->domain, isp->dev, (u32)da);
-        kfree(sgt);
-}
-/* -----------------------------------------------------------------------------
 * Video queue operations
 */
-static void isp_video_queue_prepare(struct isp_video_queue *queue,
+static int isp_video_queue_setup(struct vb2_queue *queue,
-                                    unsigned int *nbuffers, unsigned int *size)
+                                 const struct v4l2_format *fmt,
+                                 unsigned int *count, unsigned int *num_planes,
+                                 unsigned int sizes[], void *alloc_ctxs[])
 {
-        struct isp_video_fh *vfh =
+        struct isp_video_fh *vfh = vb2_get_drv_priv(queue);
-                container_of(queue, struct isp_video_fh, queue);
        struct isp_video *video = vfh->video;
-        *size = vfh->format.fmt.pix.sizeimage;
+        *num_planes = 1;
-        if (*size == 0)
-                return;
-        *nbuffers = min(*nbuffers, video->capture_mem / PAGE_ALIGN(*size));
+        sizes[0] = vfh->format.fmt.pix.sizeimage;
-}
+        if (sizes[0] == 0)
+                return -EINVAL;
-static void isp_video_buffer_cleanup(struct isp_video_buffer *buf)
+        alloc_ctxs[0] = video->alloc_ctx;
-{
-        struct isp_video_fh *vfh = isp_video_queue_to_isp_video_fh(buf->queue);
-        struct isp_buffer *buffer = to_isp_buffer(buf);
-        struct isp_video *video = vfh->video;
-        if (buffer->isp_addr) {
+        *count = min(*count, video->capture_mem / PAGE_ALIGN(sizes[0]));
-                ispmmu_vunmap(video->isp, buffer->isp_addr);
-                buffer->isp_addr = 0;
+        return 0;
-        }
 }
-static int isp_video_buffer_prepare(struct isp_video_buffer *buf)
+static int isp_video_buffer_prepare(struct vb2_buffer *buf)
 {
-        struct isp_video_fh *vfh = isp_video_queue_to_isp_video_fh(buf->queue);
+        struct isp_video_fh *vfh = vb2_get_drv_priv(buf->vb2_queue);
        struct isp_buffer *buffer = to_isp_buffer(buf);
        struct isp_video *video = vfh->video;
-        unsigned long addr;
+        dma_addr_t addr;
        /* Refuse to prepare the buffer is the video node has registered an
         * error. We don't need to take any lock here as the operation is
@@ -420,19 +366,16 @@ static int isp_video_buffer_prepare(struct isp_video_buffer *buf)
        if (unlikely(video->error))
                return -EIO;
-        addr = ispmmu_vmap(video->isp, buf->sglist, buf->sglen);
+        addr = vb2_dma_contig_plane_dma_addr(buf, 0);
-        if (IS_ERR_VALUE(addr))
-                return -EIO;
        if (!IS_ALIGNED(addr, 32)) {
-                dev_dbg(video->isp->dev, "Buffer address must be "
+                dev_dbg(video->isp->dev,
-                        "aligned to 32 bytes boundary.\n");
+                        "Buffer address must be aligned to 32 bytes boundary.\n");
-                ispmmu_vunmap(video->isp, buffer->isp_addr);
                return -EINVAL;
        }
-        buf->vbuf.bytesused = vfh->format.fmt.pix.sizeimage;
+        vb2_set_plane_payload(&buffer->vb, 0, vfh->format.fmt.pix.sizeimage);
-        buffer->isp_addr = addr;
+        buffer->dma = addr;
        return 0;
 }
@@ -445,9 +388,9 @@ static int isp_video_buffer_prepare(struct isp_video_buffer *buf)
 * If the pipeline is busy, it will be restarted in the output module interrupt
 * handler.
 */
-static void isp_video_buffer_queue(struct isp_video_buffer *buf)
+static void isp_video_buffer_queue(struct vb2_buffer *buf)
 {
-        struct isp_video_fh *vfh = isp_video_queue_to_isp_video_fh(buf->queue);
+        struct isp_video_fh *vfh = vb2_get_drv_priv(buf->vb2_queue);
        struct isp_buffer *buffer = to_isp_buffer(buf);
        struct isp_video *video = vfh->video;
        struct isp_pipeline *pipe = to_isp_pipeline(&video->video.entity);
@@ -456,14 +399,18 @@ static void isp_video_buffer_queue(struct isp_video_buffer *buf)
        unsigned int empty;
        unsigned int start;
+        spin_lock_irqsave(&video->irqlock, flags);
        if (unlikely(video->error)) {
-                buf->state = ISP_BUF_STATE_ERROR;
+                vb2_buffer_done(&buffer->vb, VB2_BUF_STATE_ERROR);
-                wake_up(&buf->wait);
+                spin_unlock_irqrestore(&video->irqlock, flags);
                return;
        }
        empty = list_empty(&video->dmaqueue);
-        list_add_tail(&buffer->buffer.irqlist, &video->dmaqueue);
+        list_add_tail(&buffer->irqlist, &video->dmaqueue);
+        spin_unlock_irqrestore(&video->irqlock, flags);
        if (empty) {
                if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
@@ -487,23 +434,22 @@ static void isp_video_buffer_queue(struct isp_video_buffer *buf)
        }
 }
-static const struct isp_video_queue_operations isp_video_queue_ops = {
+static const struct vb2_ops isp_video_queue_ops = {
-        .queue_prepare = &isp_video_queue_prepare,
+        .queue_setup = isp_video_queue_setup,
-        .buffer_prepare = &isp_video_buffer_prepare,
+        .buf_prepare = isp_video_buffer_prepare,
-        .buffer_queue = &isp_video_buffer_queue,
+        .buf_queue = isp_video_buffer_queue,
-        .buffer_cleanup = &isp_video_buffer_cleanup,
 };
 /*
 * omap3isp_video_buffer_next - Complete the current buffer and return the next
 * @video: ISP video object
 *
- * Remove the current video buffer from the DMA queue and fill its timestamp,
+ * Remove the current video buffer from the DMA queue and fill its timestamp and
- * field count and state fields before waking up its completion handler.
+ * field count before handing it back to videobuf2.
 *
- * For capture video nodes the buffer state is set to ISP_BUF_STATE_DONE if no
+ * For capture video nodes the buffer state is set to VB2_BUF_STATE_DONE if no
- * error has been flagged in the pipeline, or to ISP_BUF_STATE_ERROR otherwise.
+ * error has been flagged in the pipeline, or to VB2_BUF_STATE_ERROR otherwise.
- * For video output nodes the buffer state is always set to ISP_BUF_STATE_DONE.
+ * For video output nodes the buffer state is always set to VB2_BUF_STATE_DONE.
 *
 * The DMA queue is expected to contain at least one buffer.
 *
@@ -513,26 +459,25 @@ static const struct isp_video_queue_operations isp_video_queue_ops = {
 struct isp_buffer *omap3isp_video_buffer_next(struct isp_video *video)
 {
        struct isp_pipeline *pipe = to_isp_pipeline(&video->video.entity);
-        struct isp_video_queue *queue = video->queue;
        enum isp_pipeline_state state;
-        struct isp_video_buffer *buf;
+        struct isp_buffer *buf;
        unsigned long flags;
        struct timespec ts;
-        spin_lock_irqsave(&queue->irqlock, flags);
+        spin_lock_irqsave(&video->irqlock, flags);
        if (WARN_ON(list_empty(&video->dmaqueue))) {
-                spin_unlock_irqrestore(&queue->irqlock, flags);
+                spin_unlock_irqrestore(&video->irqlock, flags);
                return NULL;
        }
-        buf = list_first_entry(&video->dmaqueue, struct isp_video_buffer,
+        buf = list_first_entry(&video->dmaqueue, struct isp_buffer,
                               irqlist);
        list_del(&buf->irqlist);
-        spin_unlock_irqrestore(&queue->irqlock, flags);
+        spin_unlock_irqrestore(&video->irqlock, flags);
        ktime_get_ts(&ts);
-        buf->vbuf.timestamp.tv_sec = ts.tv_sec;
+        buf->vb.v4l2_buf.timestamp.tv_sec = ts.tv_sec;
-        buf->vbuf.timestamp.tv_usec = ts.tv_nsec / NSEC_PER_USEC;
+        buf->vb.v4l2_buf.timestamp.tv_usec = ts.tv_nsec / NSEC_PER_USEC;
        /* Do frame number propagation only if this is the output video node.
         * Frame number either comes from the CSI receivers or it gets
@@ -541,22 +486,27 @@ struct isp_buffer *omap3isp_video_buffer_next(struct isp_video *video)
         * first, so the input number might lag behind by 1 in some cases.
         */
        if (video == pipe->output && !pipe->do_propagation)
-                buf->vbuf.sequence = atomic_inc_return(&pipe->frame_number);
+                buf->vb.v4l2_buf.sequence =
+                        atomic_inc_return(&pipe->frame_number);
        else
-                buf->vbuf.sequence = atomic_read(&pipe->frame_number);
+                buf->vb.v4l2_buf.sequence = atomic_read(&pipe->frame_number);
        /* Report pipeline errors to userspace on the capture device side. */
-        if (queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && pipe->error) {
+        if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && pipe->error) {
-                buf->state = ISP_BUF_STATE_ERROR;
+                state = VB2_BUF_STATE_ERROR;
                pipe->error = false;
        } else {
-                buf->state = ISP_BUF_STATE_DONE;
+                state = VB2_BUF_STATE_DONE;
        }
-        wake_up(&buf->wait);
+        vb2_buffer_done(&buf->vb, state);
+        spin_lock_irqsave(&video->irqlock, flags);
        if (list_empty(&video->dmaqueue)) {
-                if (queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                spin_unlock_irqrestore(&video->irqlock, flags);
+                if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
                        state = ISP_PIPELINE_QUEUE_OUTPUT
                              | ISP_PIPELINE_STREAM;
                else
@@ -571,16 +521,19 @@ struct isp_buffer *omap3isp_video_buffer_next(struct isp_video *video)
                return NULL;
        }
-        if (queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && pipe->input != NULL) {
+        if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && pipe->input != NULL) {
-                spin_lock_irqsave(&pipe->lock, flags);
+                spin_lock(&pipe->lock);
                pipe->state &= ~ISP_PIPELINE_STREAM;
-                spin_unlock_irqrestore(&pipe->lock, flags);
+                spin_unlock(&pipe->lock);
        }
-        buf = list_first_entry(&video->dmaqueue, struct isp_video_buffer,
+        buf = list_first_entry(&video->dmaqueue, struct isp_buffer,
                               irqlist);
-        buf->state = ISP_BUF_STATE_ACTIVE;
+        buf->vb.state = VB2_BUF_STATE_ACTIVE;
-        return to_isp_buffer(buf);
+        spin_unlock_irqrestore(&video->irqlock, flags);
+        return buf;
 }
 /*
@@ -592,25 +545,22 @@ struct isp_buffer *omap3isp_video_buffer_next(struct isp_video *video)
 */
 void omap3isp_video_cancel_stream(struct isp_video *video)
 {
-        struct isp_video_queue *queue = video->queue;
        unsigned long flags;
-        spin_lock_irqsave(&queue->irqlock, flags);
+        spin_lock_irqsave(&video->irqlock, flags);
        while (!list_empty(&video->dmaqueue)) {
-                struct isp_video_buffer *buf;
+                struct isp_buffer *buf;
                buf = list_first_entry(&video->dmaqueue,
-                                       struct isp_video_buffer, irqlist);
+                                       struct isp_buffer, irqlist);
                list_del(&buf->irqlist);
+                vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
-                buf->state = ISP_BUF_STATE_ERROR;
-                wake_up(&buf->wait);
        }
        video->error = true;
-        spin_unlock_irqrestore(&queue->irqlock, flags);
+        spin_unlock_irqrestore(&video->irqlock, flags);
 }
 /*
@@ -627,12 +577,15 @@ void omap3isp_video_resume(struct isp_video *video, int continuous)
 {
        struct isp_buffer *buf = NULL;
-        if (continuous && video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+        if (continuous && video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-                omap3isp_video_queue_discard_done(video->queue);
+                mutex_lock(&video->queue_lock);
+                vb2_discard_done(video->queue);
+                mutex_unlock(&video->queue_lock);
+        }
        if (!list_empty(&video->dmaqueue)) {
                buf = list_first_entry(&video->dmaqueue,
-                                       struct isp_buffer, buffer.irqlist);
+                                       struct isp_buffer, irqlist);
                video->ops->queue(video, buf);
                video->dmaqueue_flags |= ISP_VIDEO_DMAQUEUE_QUEUED;
        } else {
@@ -840,33 +793,56 @@ static int
 isp_video_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *rb)
 {
        struct isp_video_fh *vfh = to_isp_video_fh(fh);
+        struct isp_video *video = video_drvdata(file);
+        int ret;
-        return omap3isp_video_queue_reqbufs(&vfh->queue, rb);
+        mutex_lock(&video->queue_lock);
+        ret = vb2_reqbufs(&vfh->queue, rb);
+        mutex_unlock(&video->queue_lock);
+        return ret;
 }
 static int
 isp_video_querybuf(struct file *file, void *fh, struct v4l2_buffer *b)
 {
        struct isp_video_fh *vfh = to_isp_video_fh(fh);
+        struct isp_video *video = video_drvdata(file);
+        int ret;
+        mutex_lock(&video->queue_lock);
+        ret = vb2_querybuf(&vfh->queue, b);
+        mutex_unlock(&video->queue_lock);
-        return omap3isp_video_queue_querybuf(&vfh->queue, b);
+        return ret;
 }
 static int
 isp_video_qbuf(struct file *file, void *fh, struct v4l2_buffer *b)
 {
        struct isp_video_fh *vfh = to_isp_video_fh(fh);
+        struct isp_video *video = video_drvdata(file);
+        int ret;
-        return omap3isp_video_queue_qbuf(&vfh->queue, b);
+        mutex_lock(&video->queue_lock);
+        ret = vb2_qbuf(&vfh->queue, b);
+        mutex_unlock(&video->queue_lock);
+        return ret;
 }
 static int
 isp_video_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
 {
        struct isp_video_fh *vfh = to_isp_video_fh(fh);
+        struct isp_video *video = video_drvdata(file);
+        int ret;
+        mutex_lock(&video->queue_lock);
+        ret = vb2_dqbuf(&vfh->queue, b, file->f_flags & O_NONBLOCK);
+        mutex_unlock(&video->queue_lock);
-        return omap3isp_video_queue_dqbuf(&vfh->queue, b,
+        return ret;
-                                          file->f_flags & O_NONBLOCK);
 }
 static int isp_video_check_external_subdevs(struct isp_video *video,
@@ -1006,11 +982,6 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
        mutex_lock(&video->stream_lock);
-        if (video->streaming) {
-                mutex_unlock(&video->stream_lock);
-                return -EBUSY;
-        }
        /* Start streaming on the pipeline. No link touching an entity in the
         * pipeline can be activated or deactivated once streaming is started.
         */
@@ -1069,7 +1040,9 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
        INIT_LIST_HEAD(&video->dmaqueue);
        atomic_set(&pipe->frame_number, -1);
-        ret = omap3isp_video_queue_streamon(&vfh->queue);
+        mutex_lock(&video->queue_lock);
+        ret = vb2_streamon(&vfh->queue, type);
+        mutex_unlock(&video->queue_lock);
        if (ret < 0)
                goto err_check_format;
@@ -1082,19 +1055,19 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
                                              ISP_PIPELINE_STREAM_CONTINUOUS);
                if (ret < 0)
                        goto err_set_stream;
-                spin_lock_irqsave(&video->queue->irqlock, flags);
+                spin_lock_irqsave(&video->irqlock, flags);
                if (list_empty(&video->dmaqueue))
                        video->dmaqueue_flags |= ISP_VIDEO_DMAQUEUE_UNDERRUN;
-                spin_unlock_irqrestore(&video->queue->irqlock, flags);
+                spin_unlock_irqrestore(&video->irqlock, flags);
        }
-        video->streaming = 1;
        mutex_unlock(&video->stream_lock);
        return 0;
 err_set_stream:
-        omap3isp_video_queue_streamoff(&vfh->queue);
+        mutex_lock(&video->queue_lock);
+        vb2_streamoff(&vfh->queue, type);
+        mutex_unlock(&video->queue_lock);
 err_check_format:
        media_entity_pipeline_stop(&video->video.entity);
 err_pipeline_start:
@@ -1130,9 +1103,9 @@ isp_video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
        mutex_lock(&video->stream_lock);
        /* Make sure we're not streaming yet. */
-        mutex_lock(&vfh->queue.lock);
+        mutex_lock(&video->queue_lock);
-        streaming = vfh->queue.streaming;
+        streaming = vb2_is_streaming(&vfh->queue);
-        mutex_unlock(&vfh->queue.lock);
+        mutex_unlock(&video->queue_lock);
        if (!streaming)
                goto done;
@@ -1151,9 +1124,12 @@ isp_video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
        /* Stop the stream. */
        omap3isp_pipeline_set_stream(pipe, ISP_PIPELINE_STREAM_STOPPED);
-        omap3isp_video_queue_streamoff(&vfh->queue);
+        omap3isp_video_cancel_stream(video);
+        mutex_lock(&video->queue_lock);
+        vb2_streamoff(&vfh->queue, type);
+        mutex_unlock(&video->queue_lock);
        video->queue = NULL;
-        video->streaming = 0;
        video->error = false;
        if (video->isp->pdata->set_constraints)
@@ -1223,6 +1199,7 @@ static int isp_video_open(struct file *file)
 {
        struct isp_video *video = video_drvdata(file);
        struct isp_video_fh *handle;
+        struct vb2_queue *queue;
        int ret = 0;
        handle = kzalloc(sizeof(*handle), GFP_KERNEL);
@@ -1244,9 +1221,20 @@ static int isp_video_open(struct file *file)
                goto done;
        }
-        omap3isp_video_queue_init(&handle->queue, video->type,
+        queue = &handle->queue;
-                                  &isp_video_queue_ops, video->isp->dev,
+        queue->type = video->type;
-                                  sizeof(struct isp_buffer));
+        queue->io_modes = VB2_MMAP | VB2_USERPTR;
+        queue->drv_priv = handle;
+        queue->ops = &isp_video_queue_ops;
+        queue->mem_ops = &vb2_dma_contig_memops;
+        queue->buf_struct_size = sizeof(struct isp_buffer);
+        queue->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+        ret = vb2_queue_init(&handle->queue);
+        if (ret < 0) {
+                omap3isp_put(video->isp);
+                goto done;
+        }
        memset(&handle->format, 0, sizeof(handle->format));
        handle->format.type = video->type;
@@ -1273,9 +1261,9 @@ static int isp_video_release(struct file *file)
        /* Disable streaming and free the buffers queue resources. */
        isp_video_streamoff(file, vfh, video->type);
-        mutex_lock(&handle->queue.lock);
+        mutex_lock(&video->queue_lock);
-        omap3isp_video_queue_cleanup(&handle->queue);
+        vb2_queue_release(&handle->queue);
-        mutex_unlock(&handle->queue.lock);
+        mutex_unlock(&video->queue_lock);
        omap3isp_pipeline_pm_use(&video->video.entity, 0);
@@ -1292,16 +1280,27 @@ static int isp_video_release(struct file *file)
 static unsigned int isp_video_poll(struct file *file, poll_table *wait)
 {
        struct isp_video_fh *vfh = to_isp_video_fh(file->private_data);
-        struct isp_video_queue *queue = &vfh->queue;
+        struct isp_video *video = video_drvdata(file);
+        int ret;
-        return omap3isp_video_queue_poll(queue, file, wait);
+        mutex_lock(&video->queue_lock);
+        ret = vb2_poll(&vfh->queue, file, wait);
+        mutex_unlock(&video->queue_lock);
+        return ret;
 }
 static int isp_video_mmap(struct file *file, struct vm_area_struct *vma)
 {
        struct isp_video_fh *vfh = to_isp_video_fh(file->private_data);
+        struct isp_video *video = video_drvdata(file);
+        int ret;
+        mutex_lock(&video->queue_lock);
+        ret = vb2_mmap(&vfh->queue, vma);
+        mutex_unlock(&video->queue_lock);
-        return omap3isp_video_queue_mmap(&vfh->queue, vma);
+        return ret;
 }
 static struct v4l2_file_operations isp_video_fops = {
@@ -1342,15 +1341,23 @@ int omap3isp_video_init(struct isp_video *video, const char *name)
                return -EINVAL;
        }
+        video->alloc_ctx = vb2_dma_contig_init_ctx(video->isp->dev);
+        if (IS_ERR(video->alloc_ctx))
+                return PTR_ERR(video->alloc_ctx);
        ret = media_entity_init(&video->video.entity, 1, &video->pad, 0);
-        if (ret < 0)
+        if (ret < 0) {
+                vb2_dma_contig_cleanup_ctx(video->alloc_ctx);
                return ret;
+        }
        mutex_init(&video->mutex);
        atomic_set(&video->active, 0);
        spin_lock_init(&video->pipe.lock);
        mutex_init(&video->stream_lock);
+        mutex_init(&video->queue_lock);
+        spin_lock_init(&video->irqlock);
        /* Initialize the video device. */
        if (video->ops == NULL)
@@ -1371,7 +1378,9 @@ int omap3isp_video_init(struct isp_video *video, const char *name)
 void omap3isp_video_cleanup(struct isp_video *video)
 {
+        vb2_dma_contig_cleanup_ctx(video->alloc_ctx);
        media_entity_cleanup(&video->video.entity);
+        mutex_destroy(&video->queue_lock);
        mutex_destroy(&video->stream_lock);
        mutex_destroy(&video->mutex);
 }
diff --git a/drivers/media/platform/omap3isp/ispvideo.h b/drivers/media/platform/omap3isp/ispvideo.h
index 4e194076cc60..7d2e82122ecd 100644
--- a/drivers/media/platform/omap3isp/ispvideo.h
+++ b/drivers/media/platform/omap3isp/ispvideo.h
@@ -30,8 +30,7 @@
 #include <media/media-entity.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-fh.h>
+#include <media/videobuf2-core.h>
-#include "ispqueue.h"
 #define ISP_VIDEO_DRIVER_NAME           "ispvideo"
 #define ISP_VIDEO_DRIVER_VERSION        "0.0.2"
@@ -124,17 +123,19 @@ static inline int isp_pipeline_ready(struct isp_pipeline *pipe)
                               ISP_PIPELINE_IDLE_OUTPUT);
 }
-/*
+/**
- * struct isp_buffer - ISP buffer
+ * struct isp_buffer - ISP video buffer
- * @buffer: ISP video buffer
+ * @vb: videobuf2 buffer
- * @isp_addr: MMU mapped address (a.k.a. device address) of the buffer.
+ * @irqlist: List head for insertion into IRQ queue
+ * @dma: DMA address
 */
 struct isp_buffer {
-        struct isp_video_buffer buffer;
+        struct vb2_buffer vb;
-        dma_addr_t isp_addr;
+        struct list_head irqlist;
+        dma_addr_t dma;
 };
-#define to_isp_buffer(buf)      container_of(buf, struct isp_buffer, buffer)
+#define to_isp_buffer(buf)      container_of(buf, struct isp_buffer, vb)
 enum isp_video_dmaqueue_flags {
        /* Set if DMA queue becomes empty when ISP_PIPELINE_STREAM_CONTINUOUS */
@@ -172,16 +173,16 @@ struct isp_video {
        unsigned int bpl_value;         /* bytes per line value */
        unsigned int bpl_padding;       /* padding at end of line */
-        /* Entity video node streaming */
-        unsigned int streaming:1;
        /* Pipeline state */
        struct isp_pipeline pipe;
        struct mutex stream_lock;       /* pipeline and stream states */
        bool error;
        /* Video buffers queue */
-        struct isp_video_queue *queue;
+        void *alloc_ctx;
+        struct vb2_queue *queue;
+        struct mutex queue_lock;        /* protects the queue */
+        spinlock_t irqlock;             /* protects dmaqueue */
        struct list_head dmaqueue;
        enum isp_video_dmaqueue_flags dmaqueue_flags;
@@ -193,7 +194,7 @@ struct isp_video {
 struct isp_video_fh {
        struct v4l2_fh vfh;
        struct isp_video *video;
-        struct isp_video_queue queue;
+        struct vb2_queue queue;
        struct v4l2_format format;
        struct v4l2_fract timeperframe;
 };
diff --git a/drivers/media/platform/vsp1/Makefile b/drivers/media/platform/vsp1/Makefile
index 151cecd0ea25..6a93f928dfde 100644
--- a/drivers/media/platform/vsp1/Makefile
+++ b/drivers/media/platform/vsp1/Makefile
@@ -1,6 +1,6 @@
 vsp1-y                                  := vsp1_drv.o vsp1_entity.o vsp1_video.o
 vsp1-y                                  += vsp1_rpf.o vsp1_rwpf.o vsp1_wpf.o
 vsp1-y                                  += vsp1_hsit.o vsp1_lif.o vsp1_lut.o
-vsp1-y                                  += vsp1_sru.o vsp1_uds.o
+vsp1-y                                  += vsp1_bru.o vsp1_sru.o vsp1_uds.o
 obj-$(CONFIG_VIDEO_RENESAS_VSP1)        += vsp1.o
diff --git a/drivers/media/platform/vsp1/vsp1.h b/drivers/media/platform/vsp1/vsp1.h
index 0313210c6e9e..6ca2cf20d545 100644
--- a/drivers/media/platform/vsp1/vsp1.h
+++ b/drivers/media/platform/vsp1/vsp1.h
@@ -28,6 +28,7 @@ struct clk;
 struct device;
 struct vsp1_platform_data;
+struct vsp1_bru;
 struct vsp1_hsit;
 struct vsp1_lif;
 struct vsp1_lut;
@@ -45,11 +46,11 @@ struct vsp1_device {
        void __iomem *mmio;
        struct clk *clock;
-        struct clk *rt_clock;
        struct mutex lock;
        int ref_count;
+        struct vsp1_bru *bru;
        struct vsp1_hsit *hsi;
        struct vsp1_hsit *hst;
        struct vsp1_lif *lif;
diff --git a/drivers/media/platform/vsp1/vsp1_bru.c b/drivers/media/platform/vsp1/vsp1_bru.c
new file mode 100644
index 000000000000..f80695480060
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_bru.c
@@ -0,0 +1,395 @@
+/*
+ * vsp1_bru.c  --  R-Car VSP1 Blend ROP Unit
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/device.h>
+#include <linux/gfp.h>
+#include <media/v4l2-subdev.h>
+#include "vsp1.h"
+#include "vsp1_bru.h"
+#define BRU_MIN_SIZE                            4U
+#define BRU_MAX_SIZE                            8190U
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+static inline u32 vsp1_bru_read(struct vsp1_bru *bru, u32 reg)
+{
+        return vsp1_read(bru->entity.vsp1, reg);
+}
+static inline void vsp1_bru_write(struct vsp1_bru *bru, u32 reg, u32 data)
+{
+        vsp1_write(bru->entity.vsp1, reg, data);
+}
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+static bool bru_is_input_enabled(struct vsp1_bru *bru, unsigned int input)
+{
+        return media_entity_remote_pad(&bru->entity.pads[input]) != NULL;
+}
+static int bru_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+        struct vsp1_bru *bru = to_bru(subdev);
+        struct v4l2_mbus_framefmt *format;
+        unsigned int i;
+        if (!enable)
+                return 0;
+        format = &bru->entity.formats[BRU_PAD_SOURCE];
+        /* The hardware is extremely flexible but we have no userspace API to
+         * expose all the parameters, nor is it clear whether we would have use
+         * cases for all the supported modes. Let's just harcode the parameters
+         * to sane default values for now.
+         */
+        /* Disable both color data normalization and dithering. */
+        vsp1_bru_write(bru, VI6_BRU_INCTRL, 0);
+        /* Set the background position to cover the whole output image and
+         * set its color to opaque black.
+         */
+        vsp1_bru_write(bru, VI6_BRU_VIRRPF_SIZE,
+                       (format->width << VI6_BRU_VIRRPF_SIZE_HSIZE_SHIFT) |
+                       (format->height << VI6_BRU_VIRRPF_SIZE_VSIZE_SHIFT));
+        vsp1_bru_write(bru, VI6_BRU_VIRRPF_LOC, 0);
+        vsp1_bru_write(bru, VI6_BRU_VIRRPF_COL,
+                       0xff << VI6_BRU_VIRRPF_COL_A_SHIFT);
+        /* Route BRU input 1 as SRC input to the ROP unit and configure the ROP
+         * unit with a NOP operation to make BRU input 1 available as the
+         * Blend/ROP unit B SRC input.
+         */
+        vsp1_bru_write(bru, VI6_BRU_ROP, VI6_BRU_ROP_DSTSEL_BRUIN(1) |
+                       VI6_BRU_ROP_CROP(VI6_ROP_NOP) |
+                       VI6_BRU_ROP_AROP(VI6_ROP_NOP));
+        for (i = 0; i < 4; ++i) {
+                u32 ctrl = 0;
+                /* Configure all Blend/ROP units corresponding to an enabled BRU
+                 * input for alpha blending. Blend/ROP units corresponding to
+                 * disabled BRU inputs are used in ROP NOP mode to ignore the
+                 * SRC input.
+                 */
+                if (bru_is_input_enabled(bru, i))
+                        ctrl |= VI6_BRU_CTRL_RBC;
+                else
+                        ctrl |= VI6_BRU_CTRL_CROP(VI6_ROP_NOP)
+                             |  VI6_BRU_CTRL_AROP(VI6_ROP_NOP);
+                /* Select the virtual RPF as the Blend/ROP unit A DST input to
+                 * serve as a background color.
+                 */
+                if (i == 0)
+                        ctrl |= VI6_BRU_CTRL_DSTSEL_VRPF;
+                /* Route BRU inputs 0 to 3 as SRC inputs to Blend/ROP units A to
+                 * D in that order. The Blend/ROP unit B SRC is hardwired to the
+                 * ROP unit output, the corresponding register bits must be set
+                 * to 0.
+                 */
+                if (i != 1)
+                        ctrl |= VI6_BRU_CTRL_SRCSEL_BRUIN(i);
+                vsp1_bru_write(bru, VI6_BRU_CTRL(i), ctrl);
+                /* Harcode the blending formula to
+                 *
+                 *      DSTc = DSTc * (1 - SRCa) + SRCc * SRCa
+                 *      DSTa = DSTa * (1 - SRCa) + SRCa
+                 */
+                vsp1_bru_write(bru, VI6_BRU_BLD(i),
+                               VI6_BRU_BLD_CCMDX_255_SRC_A |
+                               VI6_BRU_BLD_CCMDY_SRC_A |
+                               VI6_BRU_BLD_ACMDX_255_SRC_A |
+                               VI6_BRU_BLD_ACMDY_COEFY |
+                               (0xff << VI6_BRU_BLD_COEFY_SHIFT));
+        }
+        return 0;
+}
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+/*
+ * The BRU can't perform format conversion, all sink and source formats must be
+ * identical. We pick the format on the first sink pad (pad 0) and propagate it
+ * to all other pads.
+ */
+static int bru_enum_mbus_code(struct v4l2_subdev *subdev,
+                              struct v4l2_subdev_fh *fh,
+                              struct v4l2_subdev_mbus_code_enum *code)
+{
+        static const unsigned int codes[] = {
+                V4L2_MBUS_FMT_ARGB8888_1X32,
+                V4L2_MBUS_FMT_AYUV8_1X32,
+        };
+        struct v4l2_mbus_framefmt *format;
+        if (code->pad == BRU_PAD_SINK(0)) {
+                if (code->index >= ARRAY_SIZE(codes))
+                        return -EINVAL;
+                code->code = codes[code->index];
+        } else {
+                if (code->index)
+                        return -EINVAL;
+                format = v4l2_subdev_get_try_format(fh, BRU_PAD_SINK(0));
+                code->code = format->code;
+        }
+        return 0;
+}
+static int bru_enum_frame_size(struct v4l2_subdev *subdev,
+                               struct v4l2_subdev_fh *fh,
+                               struct v4l2_subdev_frame_size_enum *fse)
+{
+        if (fse->index)
+                return -EINVAL;
+        if (fse->code != V4L2_MBUS_FMT_ARGB8888_1X32 &&
+            fse->code != V4L2_MBUS_FMT_AYUV8_1X32)
+                return -EINVAL;
+        fse->min_width = BRU_MIN_SIZE;
+        fse->max_width = BRU_MAX_SIZE;
+        fse->min_height = BRU_MIN_SIZE;
+        fse->max_height = BRU_MAX_SIZE;
+        return 0;
+}
+static struct v4l2_rect *bru_get_compose(struct vsp1_bru *bru,
+                                         struct v4l2_subdev_fh *fh,
+                                         unsigned int pad, u32 which)
+{
+        switch (which) {
+        case V4L2_SUBDEV_FORMAT_TRY:
+                return v4l2_subdev_get_try_crop(fh, pad);
+        case V4L2_SUBDEV_FORMAT_ACTIVE:
+                return &bru->compose[pad];
+        default:
+                return NULL;
+        }
+}
+static int bru_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
+                          struct v4l2_subdev_format *fmt)
+{
+        struct vsp1_bru *bru = to_bru(subdev);
+        fmt->format = *vsp1_entity_get_pad_format(&bru->entity, fh, fmt->pad,
+                                                  fmt->which);
+        return 0;
+}
+static void bru_try_format(struct vsp1_bru *bru, struct v4l2_subdev_fh *fh,
+                           unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+                           enum v4l2_subdev_format_whence which)
+{
+        struct v4l2_mbus_framefmt *format;
+        switch (pad) {
+        case BRU_PAD_SINK(0):
+                /* Default to YUV if the requested format is not supported. */
+                if (fmt->code != V4L2_MBUS_FMT_ARGB8888_1X32 &&
+                    fmt->code != V4L2_MBUS_FMT_AYUV8_1X32)
+                        fmt->code = V4L2_MBUS_FMT_AYUV8_1X32;
+                break;
+        default:
+                /* The BRU can't perform format conversion. */
+                format = vsp1_entity_get_pad_format(&bru->entity, fh,
+                                                    BRU_PAD_SINK(0), which);
+                fmt->code = format->code;
+                break;
+        }
+        fmt->width = clamp(fmt->width, BRU_MIN_SIZE, BRU_MAX_SIZE);
+        fmt->height = clamp(fmt->height, BRU_MIN_SIZE, BRU_MAX_SIZE);
+        fmt->field = V4L2_FIELD_NONE;
+        fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+static int bru_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
+                          struct v4l2_subdev_format *fmt)
+{
+        struct vsp1_bru *bru = to_bru(subdev);
+        struct v4l2_mbus_framefmt *format;
+        bru_try_format(bru, fh, fmt->pad, &fmt->format, fmt->which);
+        format = vsp1_entity_get_pad_format(&bru->entity, fh, fmt->pad,
+                                            fmt->which);
+        *format = fmt->format;
+        /* Reset the compose rectangle */
+        if (fmt->pad != BRU_PAD_SOURCE) {
+                struct v4l2_rect *compose;
+                compose = bru_get_compose(bru, fh, fmt->pad, fmt->which);
+                compose->left = 0;
+                compose->top = 0;
+                compose->width = format->width;
+                compose->height = format->height;
+        }
+        /* Propagate the format code to all pads */
+        if (fmt->pad == BRU_PAD_SINK(0)) {
+                unsigned int i;
+                for (i = 0; i <= BRU_PAD_SOURCE; ++i) {
+                        format = vsp1_entity_get_pad_format(&bru->entity, fh,
+                                                            i, fmt->which);
+                        format->code = fmt->format.code;
+                }
+        }
+        return 0;
+}
+static int bru_get_selection(struct v4l2_subdev *subdev,
+                             struct v4l2_subdev_fh *fh,
+                             struct v4l2_subdev_selection *sel)
+{
+        struct vsp1_bru *bru = to_bru(subdev);
+        if (sel->pad == BRU_PAD_SOURCE)
+                return -EINVAL;
+        switch (sel->target) {
+        case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+                sel->r.left = 0;
+                sel->r.top = 0;
+                sel->r.width = BRU_MAX_SIZE;
+                sel->r.height = BRU_MAX_SIZE;
+                return 0;
+        case V4L2_SEL_TGT_COMPOSE:
+                sel->r = *bru_get_compose(bru, fh, sel->pad, sel->which);
+                return 0;
+        default:
+                return -EINVAL;
+        }
+}
+static int bru_set_selection(struct v4l2_subdev *subdev,
+                             struct v4l2_subdev_fh *fh,
+                             struct v4l2_subdev_selection *sel)
+{
+        struct vsp1_bru *bru = to_bru(subdev);
+        struct v4l2_mbus_framefmt *format;
+        struct v4l2_rect *compose;
+        if (sel->pad == BRU_PAD_SOURCE)
+                return -EINVAL;
+        if (sel->target != V4L2_SEL_TGT_COMPOSE)
+                return -EINVAL;
+        /* The compose rectangle top left corner must be inside the output
+         * frame.
+         */
+        format = vsp1_entity_get_pad_format(&bru->entity, fh, BRU_PAD_SOURCE,
+                                            sel->which);
+        sel->r.left = clamp_t(unsigned int, sel->r.left, 0, format->width - 1);
+        sel->r.top = clamp_t(unsigned int, sel->r.top, 0, format->height - 1);
+        /* Scaling isn't supported, the compose rectangle size must be identical
+         * to the sink format size.
+         */
+        format = vsp1_entity_get_pad_format(&bru->entity, fh, sel->pad,
+                                            sel->which);
+        sel->r.width = format->width;
+        sel->r.height = format->height;
+        compose = bru_get_compose(bru, fh, sel->pad, sel->which);
+        *compose = sel->r;
+        return 0;
+}
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+static struct v4l2_subdev_video_ops bru_video_ops = {
+        .s_stream = bru_s_stream,
+};
+static struct v4l2_subdev_pad_ops bru_pad_ops = {
+        .enum_mbus_code = bru_enum_mbus_code,
+        .enum_frame_size = bru_enum_frame_size,
+        .get_fmt = bru_get_format,
+        .set_fmt = bru_set_format,
+        .get_selection = bru_get_selection,
+        .set_selection = bru_set_selection,
+};
+static struct v4l2_subdev_ops bru_ops = {
+        .video  = &bru_video_ops,
+        .pad    = &bru_pad_ops,
+};
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+struct vsp1_bru *vsp1_bru_create(struct vsp1_device *vsp1)
+{
+        struct v4l2_subdev *subdev;
+        struct vsp1_bru *bru;
+        int ret;
+        bru = devm_kzalloc(vsp1->dev, sizeof(*bru), GFP_KERNEL);
+        if (bru == NULL)
+                return ERR_PTR(-ENOMEM);
+        bru->entity.type = VSP1_ENTITY_BRU;
+        ret = vsp1_entity_init(vsp1, &bru->entity, 5);
+        if (ret < 0)
+                return ERR_PTR(ret);
+        /* Initialize the V4L2 subdev. */
+        subdev = &bru->entity.subdev;
+        v4l2_subdev_init(subdev, &bru_ops);
+        subdev->entity.ops = &vsp1_media_ops;
+        subdev->internal_ops = &vsp1_subdev_internal_ops;
+        snprintf(subdev->name, sizeof(subdev->name), "%s bru",
+                 dev_name(vsp1->dev));
+        v4l2_set_subdevdata(subdev, bru);
+        subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+        vsp1_entity_init_formats(subdev, NULL);
+        return bru;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_bru.h b/drivers/media/platform/vsp1/vsp1_bru.h
new file mode 100644
index 000000000000..37062704dbf6
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_bru.h
@@ -0,0 +1,39 @@
+/*
+ * vsp1_bru.h  --  R-Car VSP1 Blend ROP Unit
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_BRU_H__
+#define __VSP1_BRU_H__
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+#include "vsp1_entity.h"
+struct vsp1_device;
+#define BRU_PAD_SINK(n)                         (n)
+#define BRU_PAD_SOURCE                          4
+struct vsp1_bru {
+        struct vsp1_entity entity;
+        struct v4l2_rect compose[4];
+};
+static inline struct vsp1_bru *to_bru(struct v4l2_subdev *subdev)
+{
+        return container_of(subdev, struct vsp1_bru, entity.subdev);
+}
+struct vsp1_bru *vsp1_bru_create(struct vsp1_device *vsp1);
+#endif /* __VSP1_BRU_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_drv.c b/drivers/media/platform/vsp1/vsp1_drv.c
index 2f74f0e0ddf5..c69ee0657f75 100644
--- a/drivers/media/platform/vsp1/vsp1_drv.c
+++ b/drivers/media/platform/vsp1/vsp1_drv.c
@@ -16,10 +16,12 @@
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/videodev2.h>
 #include "vsp1.h"
+#include "vsp1_bru.h"
 #include "vsp1_hsit.h"
 #include "vsp1_lif.h"
 #include "vsp1_lut.h"
@@ -155,6 +157,14 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
        }
        /* Instantiate all the entities. */
+        vsp1->bru = vsp1_bru_create(vsp1);
+        if (IS_ERR(vsp1->bru)) {
+                ret = PTR_ERR(vsp1->bru);
+                goto done;
+        }
+        list_add_tail(&vsp1->bru->entity.list_dev, &vsp1->entities);
        vsp1->hsi = vsp1_hsit_create(vsp1, true);
        if (IS_ERR(vsp1->hsi)) {
                ret = PTR_ERR(vsp1->hsi);
@@ -329,33 +339,6 @@ static int vsp1_device_init(struct vsp1_device *vsp1)
        return 0;
 }
-static int vsp1_clocks_enable(struct vsp1_device *vsp1)
-{
-        int ret;
-        ret = clk_prepare_enable(vsp1->clock);
-        if (ret < 0)
-                return ret;
-        if (IS_ERR(vsp1->rt_clock))
-                return 0;
-        ret = clk_prepare_enable(vsp1->rt_clock);
-        if (ret < 0) {
-                clk_disable_unprepare(vsp1->clock);
-                return ret;
-        }
-        return 0;
-}
-static void vsp1_clocks_disable(struct vsp1_device *vsp1)
-{
-        if (!IS_ERR(vsp1->rt_clock))
-                clk_disable_unprepare(vsp1->rt_clock);
-        clk_disable_unprepare(vsp1->clock);
-}
 /*
 * vsp1_device_get - Acquire the VSP1 device
 *
@@ -373,7 +356,7 @@ struct vsp1_device *vsp1_device_get(struct vsp1_device *vsp1)
        if (vsp1->ref_count > 0)
                goto done;
-        ret = vsp1_clocks_enable(vsp1);
+        ret = clk_prepare_enable(vsp1->clock);
        if (ret < 0) {
                __vsp1 = NULL;
                goto done;
@@ -381,7 +364,7 @@ struct vsp1_device *vsp1_device_get(struct vsp1_device *vsp1)
        ret = vsp1_device_init(vsp1);
        if (ret < 0) {
-                vsp1_clocks_disable(vsp1);
+                clk_disable_unprepare(vsp1->clock);
                __vsp1 = NULL;
                goto done;
        }
@@ -405,7 +388,7 @@ void vsp1_device_put(struct vsp1_device *vsp1)
        mutex_lock(&vsp1->lock);
        if (--vsp1->ref_count == 0)
-                vsp1_clocks_disable(vsp1);
+                clk_disable_unprepare(vsp1->clock);
        mutex_unlock(&vsp1->lock);
 }
@@ -424,7 +407,7 @@ static int vsp1_pm_suspend(struct device *dev)
        if (vsp1->ref_count == 0)
                return 0;
-        vsp1_clocks_disable(vsp1);
+        clk_disable_unprepare(vsp1->clock);
        return 0;
 }
@@ -437,7 +420,7 @@ static int vsp1_pm_resume(struct device *dev)
        if (vsp1->ref_count)
                return 0;
-        return vsp1_clocks_enable(vsp1);
+        return clk_prepare_enable(vsp1->clock);
 }
 #endif
@@ -449,34 +432,59 @@ static const struct dev_pm_ops vsp1_pm_ops = {
 * Platform Driver
 */
-static struct vsp1_platform_data *
+static int vsp1_validate_platform_data(struct platform_device *pdev,
-vsp1_get_platform_data(struct platform_device *pdev)
+                                       struct vsp1_platform_data *pdata)
 {
-        struct vsp1_platform_data *pdata = pdev->dev.platform_data;
        if (pdata == NULL) {
                dev_err(&pdev->dev, "missing platform data\n");
-                return NULL;
+                return -EINVAL;
        }
        if (pdata->rpf_count <= 0 || pdata->rpf_count > VPS1_MAX_RPF) {
                dev_err(&pdev->dev, "invalid number of RPF (%u)\n",
                        pdata->rpf_count);
-                return NULL;
+                return -EINVAL;
        }
        if (pdata->uds_count <= 0 || pdata->uds_count > VPS1_MAX_UDS) {
                dev_err(&pdev->dev, "invalid number of UDS (%u)\n",
                        pdata->uds_count);
-                return NULL;
+                return -EINVAL;
        }
        if (pdata->wpf_count <= 0 || pdata->wpf_count > VPS1_MAX_WPF) {
                dev_err(&pdev->dev, "invalid number of WPF (%u)\n",
                        pdata->wpf_count);
-                return NULL;
+                return -EINVAL;
        }
+        return 0;
+}
+static struct vsp1_platform_data *
+vsp1_get_platform_data(struct platform_device *pdev)
+{
+        struct device_node *np = pdev->dev.of_node;
+        struct vsp1_platform_data *pdata;
+        if (!IS_ENABLED(CONFIG_OF) || np == NULL)
+                return pdev->dev.platform_data;
+        pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+        if (pdata == NULL)
+                return NULL;
+        if (of_property_read_bool(np, "renesas,has-lif"))
+                pdata->features |= VSP1_HAS_LIF;
+        if (of_property_read_bool(np, "renesas,has-lut"))
+                pdata->features |= VSP1_HAS_LUT;
+        if (of_property_read_bool(np, "renesas,has-sru"))
+                pdata->features |= VSP1_HAS_SRU;
+        of_property_read_u32(np, "renesas,#rpf", &pdata->rpf_count);
+        of_property_read_u32(np, "renesas,#uds", &pdata->uds_count);
+        of_property_read_u32(np, "renesas,#wpf", &pdata->wpf_count);
        return pdata;
 }
@@ -499,6 +507,10 @@ static int vsp1_probe(struct platform_device *pdev)
        if (vsp1->pdata == NULL)
                return -ENODEV;
+        ret = vsp1_validate_platform_data(pdev, vsp1->pdata);
+        if (ret < 0)
+                return ret;
        /* I/O, IRQ and clock resources */
        io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
        vsp1->mmio = devm_ioremap_resource(&pdev->dev, io);
@@ -511,9 +523,6 @@ static int vsp1_probe(struct platform_device *pdev)
                return PTR_ERR(vsp1->clock);
        }
-        /* The RT clock is optional */
-        vsp1->rt_clock = devm_clk_get(&pdev->dev, "rt");
        irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
        if (!irq) {
                dev_err(&pdev->dev, "missing IRQ\n");
@@ -548,6 +557,11 @@ static int vsp1_remove(struct platform_device *pdev)
        return 0;
 }
+static const struct of_device_id vsp1_of_match[] = {
+        { .compatible = "renesas,vsp1" },
+        { },
+};
 static struct platform_driver vsp1_platform_driver = {
        .probe          = vsp1_probe,
        .remove         = vsp1_remove,
@@ -555,6 +569,7 @@ static struct platform_driver vsp1_platform_driver = {
                .owner  = THIS_MODULE,
                .name   = "vsp1",
                .pm     = &vsp1_pm_ops,
+                .of_match_table = vsp1_of_match,
        },
 };
diff --git a/drivers/media/platform/vsp1/vsp1_entity.c b/drivers/media/platform/vsp1/vsp1_entity.c
index 3fc9e4266caf..44167834285d 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.c
+++ b/drivers/media/platform/vsp1/vsp1_entity.c
@@ -100,8 +100,10 @@ static int vsp1_entity_link_setup(struct media_entity *entity,
                if (source->sink)
                        return -EBUSY;
                source->sink = remote->entity;
+                source->sink_pad = remote->index;
        } else {
                source->sink = NULL;
+                source->sink_pad = 0;
        }
        return 0;
@@ -116,42 +118,43 @@ const struct media_entity_operations vsp1_media_ops = {
 * Initialization
 */
+static const struct vsp1_route vsp1_routes[] = {
+        { VSP1_ENTITY_BRU, 0, VI6_DPR_BRU_ROUTE,
+          { VI6_DPR_NODE_BRU_IN(0), VI6_DPR_NODE_BRU_IN(1),
+            VI6_DPR_NODE_BRU_IN(2), VI6_DPR_NODE_BRU_IN(3), } },
+        { VSP1_ENTITY_HSI, 0, VI6_DPR_HSI_ROUTE, { VI6_DPR_NODE_HSI, } },
+        { VSP1_ENTITY_HST, 0, VI6_DPR_HST_ROUTE, { VI6_DPR_NODE_HST, } },
+        { VSP1_ENTITY_LIF, 0, 0, { VI6_DPR_NODE_LIF, } },
+        { VSP1_ENTITY_LUT, 0, VI6_DPR_LUT_ROUTE, { VI6_DPR_NODE_LUT, } },
+        { VSP1_ENTITY_RPF, 0, VI6_DPR_RPF_ROUTE(0), { VI6_DPR_NODE_RPF(0), } },
+        { VSP1_ENTITY_RPF, 1, VI6_DPR_RPF_ROUTE(1), { VI6_DPR_NODE_RPF(1), } },
+        { VSP1_ENTITY_RPF, 2, VI6_DPR_RPF_ROUTE(2), { VI6_DPR_NODE_RPF(2), } },
+        { VSP1_ENTITY_RPF, 3, VI6_DPR_RPF_ROUTE(3), { VI6_DPR_NODE_RPF(3), } },
+        { VSP1_ENTITY_RPF, 4, VI6_DPR_RPF_ROUTE(4), { VI6_DPR_NODE_RPF(4), } },
+        { VSP1_ENTITY_SRU, 0, VI6_DPR_SRU_ROUTE, { VI6_DPR_NODE_SRU, } },
+        { VSP1_ENTITY_UDS, 0, VI6_DPR_UDS_ROUTE(0), { VI6_DPR_NODE_UDS(0), } },
+        { VSP1_ENTITY_UDS, 1, VI6_DPR_UDS_ROUTE(1), { VI6_DPR_NODE_UDS(1), } },
+        { VSP1_ENTITY_UDS, 2, VI6_DPR_UDS_ROUTE(2), { VI6_DPR_NODE_UDS(2), } },
+        { VSP1_ENTITY_WPF, 0, 0, { VI6_DPR_NODE_WPF(0), } },
+        { VSP1_ENTITY_WPF, 1, 0, { VI6_DPR_NODE_WPF(1), } },
+        { VSP1_ENTITY_WPF, 2, 0, { VI6_DPR_NODE_WPF(2), } },
+        { VSP1_ENTITY_WPF, 3, 0, { VI6_DPR_NODE_WPF(3), } },
+};
 int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
                     unsigned int num_pads)
 {
-        static const struct {
-                unsigned int id;
-                unsigned int reg;
-        } routes[] = {
-                { VI6_DPR_NODE_HSI, VI6_DPR_HSI_ROUTE },
-                { VI6_DPR_NODE_HST, VI6_DPR_HST_ROUTE },
-                { VI6_DPR_NODE_LIF, 0 },
-                { VI6_DPR_NODE_LUT, VI6_DPR_LUT_ROUTE },
-                { VI6_DPR_NODE_RPF(0), VI6_DPR_RPF_ROUTE(0) },
-                { VI6_DPR_NODE_RPF(1), VI6_DPR_RPF_ROUTE(1) },
-                { VI6_DPR_NODE_RPF(2), VI6_DPR_RPF_ROUTE(2) },
-                { VI6_DPR_NODE_RPF(3), VI6_DPR_RPF_ROUTE(3) },
-                { VI6_DPR_NODE_RPF(4), VI6_DPR_RPF_ROUTE(4) },
-                { VI6_DPR_NODE_SRU, VI6_DPR_SRU_ROUTE },
-                { VI6_DPR_NODE_UDS(0), VI6_DPR_UDS_ROUTE(0) },
-                { VI6_DPR_NODE_UDS(1), VI6_DPR_UDS_ROUTE(1) },
-                { VI6_DPR_NODE_UDS(2), VI6_DPR_UDS_ROUTE(2) },
-                { VI6_DPR_NODE_WPF(0), 0 },
-                { VI6_DPR_NODE_WPF(1), 0 },
-                { VI6_DPR_NODE_WPF(2), 0 },
-                { VI6_DPR_NODE_WPF(3), 0 },
-        };
        unsigned int i;
-        for (i = 0; i < ARRAY_SIZE(routes); ++i) {
+        for (i = 0; i < ARRAY_SIZE(vsp1_routes); ++i) {
-                if (routes[i].id == entity->id) {
+                if (vsp1_routes[i].type == entity->type &&
-                        entity->route = routes[i].reg;
+                    vsp1_routes[i].index == entity->index) {
+                        entity->route = &vsp1_routes[i];
                        break;
                }
        }
-        if (i == ARRAY_SIZE(routes))
+        if (i == ARRAY_SIZE(vsp1_routes))
                return -EINVAL;
        entity->vsp1 = vsp1;
diff --git a/drivers/media/platform/vsp1/vsp1_entity.h b/drivers/media/platform/vsp1/vsp1_entity.h
index f6fd6988aeb0..7afbd8a7ba66 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.h
+++ b/drivers/media/platform/vsp1/vsp1_entity.h
@@ -20,6 +20,7 @@
 struct vsp1_device;
 enum vsp1_entity_type {
+        VSP1_ENTITY_BRU,
        VSP1_ENTITY_HSI,
        VSP1_ENTITY_HST,
        VSP1_ENTITY_LIF,
@@ -30,13 +31,31 @@ enum vsp1_entity_type {
        VSP1_ENTITY_WPF,
 };
+/*
+ * struct vsp1_route - Entity routing configuration
+ * @type: Entity type this routing entry is associated with
+ * @index: Entity index this routing entry is associated with
+ * @reg: Output routing configuration register
+ * @inputs: Target node value for each input
+ *
+ * Each $vsp1_route entry describes routing configuration for the entity
+ * specified by the entry's @type and @index. @reg indicates the register that
+ * holds output routing configuration for the entity, and the @inputs array
+ * store the target node value for each input of the entity.
+ */
+struct vsp1_route {
+        enum vsp1_entity_type type;
+        unsigned int index;
+        unsigned int reg;
+        unsigned int inputs[4];
+};
 struct vsp1_entity {
        struct vsp1_device *vsp1;
        enum vsp1_entity_type type;
        unsigned int index;
-        unsigned int id;
+        const struct vsp1_route *route;
-        unsigned int route;
        struct list_head list_dev;
        struct list_head list_pipe;
@@ -45,6 +64,7 @@ struct vsp1_entity {
        unsigned int source_pad;
        struct media_entity *sink;
+        unsigned int sink_pad;
        struct v4l2_subdev subdev;
        struct v4l2_mbus_framefmt *formats;
diff --git a/drivers/media/platform/vsp1/vsp1_hsit.c b/drivers/media/platform/vsp1/vsp1_hsit.c
index 285485350d82..db2950a73c60 100644
--- a/drivers/media/platform/vsp1/vsp1_hsit.c
+++ b/drivers/media/platform/vsp1/vsp1_hsit.c
@@ -193,13 +193,10 @@ struct vsp1_hsit *vsp1_hsit_create(struct vsp1_device *vsp1, bool inverse)
        hsit->inverse = inverse;
-        if (inverse) {
+        if (inverse)
                hsit->entity.type = VSP1_ENTITY_HSI;
-                hsit->entity.id = VI6_DPR_NODE_HSI;
+        else
-        } else {
                hsit->entity.type = VSP1_ENTITY_HST;
-                hsit->entity.id = VI6_DPR_NODE_HST;
-        }
        ret = vsp1_entity_init(vsp1, &hsit->entity, 2);
        if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_lif.c b/drivers/media/platform/vsp1/vsp1_lif.c
index 135a78957014..d4fb23e9c4a8 100644
--- a/drivers/media/platform/vsp1/vsp1_lif.c
+++ b/drivers/media/platform/vsp1/vsp1_lif.c
@@ -215,7 +215,6 @@ struct vsp1_lif *vsp1_lif_create(struct vsp1_device *vsp1)
                return ERR_PTR(-ENOMEM);
        lif->entity.type = VSP1_ENTITY_LIF;
-        lif->entity.id = VI6_DPR_NODE_LIF;
        ret = vsp1_entity_init(vsp1, &lif->entity, 2);
        if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_lut.c b/drivers/media/platform/vsp1/vsp1_lut.c
index 4e9dc7c86ef8..fea36ebe2565 100644
--- a/drivers/media/platform/vsp1/vsp1_lut.c
+++ b/drivers/media/platform/vsp1/vsp1_lut.c
@@ -229,7 +229,6 @@ struct vsp1_lut *vsp1_lut_create(struct vsp1_device *vsp1)
                return ERR_PTR(-ENOMEM);
        lut->entity.type = VSP1_ENTITY_LUT;
-        lut->entity.id = VI6_DPR_NODE_LUT;
        ret = vsp1_entity_init(vsp1, &lut->entity, 2);
        if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_regs.h b/drivers/media/platform/vsp1/vsp1_regs.h
index 28650806c20f..3e74b44286f6 100644
--- a/drivers/media/platform/vsp1/vsp1_regs.h
+++ b/drivers/media/platform/vsp1/vsp1_regs.h
@@ -451,13 +451,111 @@
 * BRU Control Registers
 */
+#define VI6_ROP_NOP                     0
+#define VI6_ROP_AND                     1
+#define VI6_ROP_AND_REV                 2
+#define VI6_ROP_COPY                    3
+#define VI6_ROP_AND_INV                 4
+#define VI6_ROP_CLEAR                   5
+#define VI6_ROP_XOR                     6
+#define VI6_ROP_OR                      7
+#define VI6_ROP_NOR                     8
+#define VI6_ROP_EQUIV                   9
+#define VI6_ROP_INVERT                  10
+#define VI6_ROP_OR_REV                  11
+#define VI6_ROP_COPY_INV                12
+#define VI6_ROP_OR_INV                  13
+#define VI6_ROP_NAND                    14
+#define VI6_ROP_SET                     15
 #define VI6_BRU_INCTRL                  0x2c00
+#define VI6_BRU_INCTRL_NRM              (1 << 28)
+#define VI6_BRU_INCTRL_DnON             (1 << (16 + (n)))
+#define VI6_BRU_INCTRL_DITHn_OFF        (0 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_18BPP      (1 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_16BPP      (2 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_15BPP      (3 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_12BPP      (4 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_8BPP       (5 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_MASK       (7 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_SHIFT      ((n) * 4)
 #define VI6_BRU_VIRRPF_SIZE             0x2c04
+#define VI6_BRU_VIRRPF_SIZE_HSIZE_MASK  (0x1fff << 16)
+#define VI6_BRU_VIRRPF_SIZE_HSIZE_SHIFT 16
+#define VI6_BRU_VIRRPF_SIZE_VSIZE_MASK  (0x1fff << 0)
+#define VI6_BRU_VIRRPF_SIZE_VSIZE_SHIFT 0
 #define VI6_BRU_VIRRPF_LOC              0x2c08
+#define VI6_BRU_VIRRPF_LOC_HCOORD_MASK  (0x1fff << 16)
+#define VI6_BRU_VIRRPF_LOC_HCOORD_SHIFT 16
+#define VI6_BRU_VIRRPF_LOC_VCOORD_MASK  (0x1fff << 0)
+#define VI6_BRU_VIRRPF_LOC_VCOORD_SHIFT 0
 #define VI6_BRU_VIRRPF_COL              0x2c0c
+#define VI6_BRU_VIRRPF_COL_A_MASK       (0xff << 24)
+#define VI6_BRU_VIRRPF_COL_A_SHIFT      24
+#define VI6_BRU_VIRRPF_COL_RCR_MASK     (0xff << 16)
+#define VI6_BRU_VIRRPF_COL_RCR_SHIFT    16
+#define VI6_BRU_VIRRPF_COL_GY_MASK      (0xff << 8)
+#define VI6_BRU_VIRRPF_COL_GY_SHIFT     8
+#define VI6_BRU_VIRRPF_COL_BCB_MASK     (0xff << 0)
+#define VI6_BRU_VIRRPF_COL_BCB_SHIFT    0
 #define VI6_BRU_CTRL(n)                 (0x2c10 + (n) * 8)
+#define VI6_BRU_CTRL_RBC                (1 << 31)
+#define VI6_BRU_CTRL_DSTSEL_BRUIN(n)    ((n) << 20)
+#define VI6_BRU_CTRL_DSTSEL_VRPF        (4 << 20)
+#define VI6_BRU_CTRL_DSTSEL_MASK        (7 << 20)
+#define VI6_BRU_CTRL_SRCSEL_BRUIN(n)    ((n) << 16)
+#define VI6_BRU_CTRL_SRCSEL_VRPF        (4 << 16)
+#define VI6_BRU_CTRL_SRCSEL_MASK        (7 << 16)
+#define VI6_BRU_CTRL_CROP(rop)          ((rop) << 4)
+#define VI6_BRU_CTRL_CROP_MASK          (0xf << 4)
+#define VI6_BRU_CTRL_AROP(rop)          ((rop) << 0)
+#define VI6_BRU_CTRL_AROP_MASK          (0xf << 0)
 #define VI6_BRU_BLD(n)                  (0x2c14 + (n) * 8)
+#define VI6_BRU_BLD_CBES                (1 << 31)
+#define VI6_BRU_BLD_CCMDX_DST_A         (0 << 28)
+#define VI6_BRU_BLD_CCMDX_255_DST_A     (1 << 28)
+#define VI6_BRU_BLD_CCMDX_SRC_A         (2 << 28)
+#define VI6_BRU_BLD_CCMDX_255_SRC_A     (3 << 28)
+#define VI6_BRU_BLD_CCMDX_COEFX         (4 << 28)
+#define VI6_BRU_BLD_CCMDX_MASK          (7 << 28)
+#define VI6_BRU_BLD_CCMDY_DST_A         (0 << 24)
+#define VI6_BRU_BLD_CCMDY_255_DST_A     (1 << 24)
+#define VI6_BRU_BLD_CCMDY_SRC_A         (2 << 24)
+#define VI6_BRU_BLD_CCMDY_255_SRC_A     (3 << 24)
+#define VI6_BRU_BLD_CCMDY_COEFY         (4 << 24)
+#define VI6_BRU_BLD_CCMDY_MASK          (7 << 24)
+#define VI6_BRU_BLD_CCMDY_SHIFT         24
+#define VI6_BRU_BLD_ABES                (1 << 23)
+#define VI6_BRU_BLD_ACMDX_DST_A         (0 << 20)
+#define VI6_BRU_BLD_ACMDX_255_DST_A     (1 << 20)
+#define VI6_BRU_BLD_ACMDX_SRC_A         (2 << 20)
+#define VI6_BRU_BLD_ACMDX_255_SRC_A     (3 << 20)
+#define VI6_BRU_BLD_ACMDX_COEFX         (4 << 20)
+#define VI6_BRU_BLD_ACMDX_MASK          (7 << 20)
+#define VI6_BRU_BLD_ACMDY_DST_A         (0 << 16)
+#define VI6_BRU_BLD_ACMDY_255_DST_A     (1 << 16)
+#define VI6_BRU_BLD_ACMDY_SRC_A         (2 << 16)
+#define VI6_BRU_BLD_ACMDY_255_SRC_A     (3 << 16)
+#define VI6_BRU_BLD_ACMDY_COEFY         (4 << 16)
+#define VI6_BRU_BLD_ACMDY_MASK          (7 << 16)
+#define VI6_BRU_BLD_COEFX_MASK          (0xff << 8)
+#define VI6_BRU_BLD_COEFX_SHIFT         8
+#define VI6_BRU_BLD_COEFY_MASK          (0xff << 0)
+#define VI6_BRU_BLD_COEFY_SHIFT         0
 #define VI6_BRU_ROP                     0x2c30
+#define VI6_BRU_ROP_DSTSEL_BRUIN(n)     ((n) << 20)
+#define VI6_BRU_ROP_DSTSEL_VRPF         (4 << 20)
+#define VI6_BRU_ROP_DSTSEL_MASK         (7 << 20)
+#define VI6_BRU_ROP_CROP(rop)           ((rop) << 4)
+#define VI6_BRU_ROP_CROP_MASK           (0xf << 4)
+#define VI6_BRU_ROP_AROP(rop)           ((rop) << 0)
+#define VI6_BRU_ROP_AROP_MASK           (0xf << 0)
 /* -----------------------------------------------------------------------------
 * HGO Control Registers
diff --git a/drivers/media/platform/vsp1/vsp1_rpf.c b/drivers/media/platform/vsp1/vsp1_rpf.c
index 2b04d0f95c62..c3d98642a4aa 100644
--- a/drivers/media/platform/vsp1/vsp1_rpf.c
+++ b/drivers/media/platform/vsp1/vsp1_rpf.c
@@ -96,8 +96,10 @@ static int rpf_s_stream(struct v4l2_subdev *subdev, int enable)
        vsp1_rpf_write(rpf, VI6_RPF_INFMT, infmt);
        vsp1_rpf_write(rpf, VI6_RPF_DSWAP, fmtinfo->swap);
-        /* Output location. Composing isn't supported yet. */
+        /* Output location */
-        vsp1_rpf_write(rpf, VI6_RPF_LOC, 0);
+        vsp1_rpf_write(rpf, VI6_RPF_LOC,
+                       (rpf->location.left << VI6_RPF_LOC_HCOORD_SHIFT) |
+                       (rpf->location.top << VI6_RPF_LOC_VCOORD_SHIFT));
        /* Disable alpha, mask and color key. Set the alpha channel to a fixed
         * value of 255.
@@ -176,7 +178,6 @@ struct vsp1_rwpf *vsp1_rpf_create(struct vsp1_device *vsp1, unsigned int index)
        rpf->entity.type = VSP1_ENTITY_RPF;
        rpf->entity.index = index;
-        rpf->entity.id = VI6_DPR_NODE_RPF(index);
        ret = vsp1_entity_init(vsp1, &rpf->entity, 2);
        if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.h b/drivers/media/platform/vsp1/vsp1_rwpf.h
index 5c5ee81bbeae..b4fb65e58770 100644
--- a/drivers/media/platform/vsp1/vsp1_rwpf.h
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.h
@@ -30,6 +30,10 @@ struct vsp1_rwpf {
        unsigned int max_width;
        unsigned int max_height;
+        struct {
+                unsigned int left;
+                unsigned int top;
+        } location;
        struct v4l2_rect crop;
        unsigned int offsets[2];
diff --git a/drivers/media/platform/vsp1/vsp1_sru.c b/drivers/media/platform/vsp1/vsp1_sru.c
index 7ab1a0b2d656..aa0e04c56f3f 100644
--- a/drivers/media/platform/vsp1/vsp1_sru.c
+++ b/drivers/media/platform/vsp1/vsp1_sru.c
@@ -327,7 +327,6 @@ struct vsp1_sru *vsp1_sru_create(struct vsp1_device *vsp1)
                return ERR_PTR(-ENOMEM);
        sru->entity.type = VSP1_ENTITY_SRU;
-        sru->entity.id = VI6_DPR_NODE_SRU;
        ret = vsp1_entity_init(vsp1, &sru->entity, 2);
        if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_uds.c b/drivers/media/platform/vsp1/vsp1_uds.c
index 622342ac7770..0293bdbb4401 100644
--- a/drivers/media/platform/vsp1/vsp1_uds.c
+++ b/drivers/media/platform/vsp1/vsp1_uds.c
@@ -131,7 +131,7 @@ static int uds_s_stream(struct v4l2_subdev *subdev, int enable)
                return 0;
        /* Enable multi-tap scaling. */
-        vsp1_uds_write(uds, VI6_UDS_CTRL, VI6_UDS_CTRL_BC);
+        vsp1_uds_write(uds, VI6_UDS_CTRL, VI6_UDS_CTRL_AON | VI6_UDS_CTRL_BC);
        vsp1_uds_write(uds, VI6_UDS_PASS_BWIDTH,
                       (uds_passband_width(uds->hscale)
@@ -139,7 +139,6 @@ static int uds_s_stream(struct v4l2_subdev *subdev, int enable)
                       (uds_passband_width(uds->vscale)
                                << VI6_UDS_PASS_BWIDTH_V_SHIFT));
        /* Set the scaling ratios and the output size. */
        format = &uds->entity.formats[UDS_PAD_SOURCE];
@@ -323,7 +322,6 @@ struct vsp1_uds *vsp1_uds_create(struct vsp1_device *vsp1, unsigned int index)
        uds->entity.type = VSP1_ENTITY_UDS;
        uds->entity.index = index;
-        uds->entity.id = VI6_DPR_NODE_UDS(index);
        ret = vsp1_entity_init(vsp1, &uds->entity, 2);
        if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_video.c b/drivers/media/platform/vsp1/vsp1_video.c
index a0595c17700f..8a1253e51f04 100644
--- a/drivers/media/platform/vsp1/vsp1_video.c
+++ b/drivers/media/platform/vsp1/vsp1_video.c
@@ -28,6 +28,7 @@
 #include <media/videobuf2-dma-contig.h>
 #include "vsp1.h"
+#include "vsp1_bru.h"
 #include "vsp1_entity.h"
 #include "vsp1_rwpf.h"
 #include "vsp1_video.h"
@@ -280,6 +281,9 @@ static int vsp1_pipeline_validate_branch(struct vsp1_rwpf *input,
        struct media_pad *pad;
        bool uds_found = false;
+        input->location.left = 0;
+        input->location.top = 0;
        pad = media_entity_remote_pad(&input->entity.pads[RWPF_PAD_SOURCE]);
        while (1) {
@@ -292,6 +296,17 @@ static int vsp1_pipeline_validate_branch(struct vsp1_rwpf *input,
                entity = to_vsp1_entity(media_entity_to_v4l2_subdev(pad->entity));
+                /* A BRU is present in the pipeline, store the compose rectangle
+                 * location in the input RPF for use when configuring the RPF.
+                 */
+                if (entity->type == VSP1_ENTITY_BRU) {
+                        struct vsp1_bru *bru = to_bru(&entity->subdev);
+                        struct v4l2_rect *rect = &bru->compose[pad->index];
+                        input->location.left = rect->left;
+                        input->location.top = rect->top;
+                }
                /* We've reached the WPF, we're done. */
                if (entity->type == VSP1_ENTITY_WPF)
                        break;
@@ -363,6 +378,8 @@ static int vsp1_pipeline_validate(struct vsp1_pipeline *pipe,
                        rwpf->video.pipe_index = 0;
                } else if (e->type == VSP1_ENTITY_LIF) {
                        pipe->lif = e;
+                } else if (e->type == VSP1_ENTITY_BRU) {
+                        pipe->bru = e;
                }
        }
@@ -392,6 +409,7 @@ error:
        pipe->num_video = 0;
        pipe->num_inputs = 0;
        pipe->output = NULL;
+        pipe->bru = NULL;
        pipe->lif = NULL;
        return ret;
 }
@@ -430,6 +448,7 @@ static void vsp1_pipeline_cleanup(struct vsp1_pipeline *pipe)
                pipe->num_video = 0;
                pipe->num_inputs = 0;
                pipe->output = NULL;
+                pipe->bru = NULL;
                pipe->lif = NULL;
        }
@@ -461,7 +480,7 @@ static int vsp1_pipeline_stop(struct vsp1_pipeline *pipe)
        list_for_each_entry(entity, &pipe->entities, list_pipe) {
                if (entity->route)
-                        vsp1_write(entity->vsp1, entity->route,
+                        vsp1_write(entity->vsp1, entity->route->reg,
                                   VI6_DPR_NODE_UNUSED);
                v4l2_subdev_call(&entity->subdev, video, s_stream, 0);
@@ -680,11 +699,12 @@ static void vsp1_entity_route_setup(struct vsp1_entity *source)
 {
        struct vsp1_entity *sink;
-        if (source->route == 0)
+        if (source->route->reg == 0)
                return;
        sink = container_of(source->sink, struct vsp1_entity, subdev.entity);
-        vsp1_write(source->vsp1, source->route, sink->id);
+        vsp1_write(source->vsp1, source->route->reg,
+                   sink->route->inputs[source->sink_pad]);
 }
 static int vsp1_video_start_streaming(struct vb2_queue *vq, unsigned int count)
diff --git a/drivers/media/platform/vsp1/vsp1_video.h b/drivers/media/platform/vsp1/vsp1_video.h
index 53e4b3745940..c04d48fa2999 100644
--- a/drivers/media/platform/vsp1/vsp1_video.h
+++ b/drivers/media/platform/vsp1/vsp1_video.h
@@ -75,6 +75,7 @@ struct vsp1_pipeline {
        unsigned int num_inputs;
        struct vsp1_rwpf *inputs[VPS1_MAX_RPF];
        struct vsp1_rwpf *output;
+        struct vsp1_entity *bru;
        struct vsp1_entity *lif;
        struct list_head entities;
diff --git a/drivers/media/platform/vsp1/vsp1_wpf.c b/drivers/media/platform/vsp1/vsp1_wpf.c
index 11a61c601da0..1294340dcb36 100644
--- a/drivers/media/platform/vsp1/vsp1_wpf.c
+++ b/drivers/media/platform/vsp1/vsp1_wpf.c
@@ -58,13 +58,21 @@ static int wpf_s_stream(struct v4l2_subdev *subdev, int enable)
                return 0;
        }
-        /* Sources */
+        /* Sources. If the pipeline has a single input configure it as the
+         * master layer. Otherwise configure all inputs as sub-layers and
+         * select the virtual RPF as the master layer.
+         */
        for (i = 0; i < pipe->num_inputs; ++i) {
                struct vsp1_rwpf *input = pipe->inputs[i];
-                srcrpf |= VI6_WPF_SRCRPF_RPF_ACT_MST(input->entity.index);
+                srcrpf |= pipe->num_inputs == 1
+                        ? VI6_WPF_SRCRPF_RPF_ACT_MST(input->entity.index)
+                        : VI6_WPF_SRCRPF_RPF_ACT_SUB(input->entity.index);
        }
+        if (pipe->num_inputs > 1)
+                srcrpf |= VI6_WPF_SRCRPF_VIRACT_MST;
        vsp1_wpf_write(wpf, VI6_WPF_SRCRPF, srcrpf);
        /* Destination stride. */
@@ -181,7 +189,6 @@ struct vsp1_rwpf *vsp1_wpf_create(struct vsp1_device *vsp1, unsigned int index)
        wpf->entity.type = VSP1_ENTITY_WPF;
        wpf->entity.index = index;
-        wpf->entity.id = VI6_DPR_NODE_WPF(index);
        ret = vsp1_entity_init(vsp1, &wpf->entity, 2);
        if (ret < 0)
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index 47cd373e2295..79abbc8d9600 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -60,28 +60,6 @@
 #define DRIVER_DESC "RedRat3 USB IR Transceiver Driver"
 #define DRIVER_NAME "redrat3"
-/* module parameters */
-#ifdef CONFIG_USB_DEBUG
-static int debug = 1;
-#else
-static int debug;
-#endif
-#define RR3_DEBUG_STANDARD              0x1
-#define RR3_DEBUG_FUNCTION_TRACE        0x2
-#define rr3_dbg(dev, fmt, ...)                                  \
-        do {                                                    \
-                if (debug & RR3_DEBUG_STANDARD)                 \
-                        dev_info(dev, fmt, ## __VA_ARGS__);     \
-        } while (0)
-#define rr3_ftr(dev, fmt, ...)                                  \
-        do {                                                    \
-                if (debug & RR3_DEBUG_FUNCTION_TRACE)           \
-                        dev_info(dev, fmt, ## __VA_ARGS__);     \
-        } while (0)
 /* bulk data transfer types */
 #define RR3_ERROR               0x01
 #define RR3_MOD_SIGNAL_IN       0x20
@@ -237,13 +215,11 @@ static void redrat3_issue_async(struct redrat3_dev *rr3)
 {
        int res;
-        rr3_ftr(rr3->dev, "Entering %s\n", __func__);
        res = usb_submit_urb(rr3->read_urb, GFP_ATOMIC);
        if (res)
-                rr3_dbg(rr3->dev, "%s: receive request FAILED! "
+                dev_dbg(rr3->dev,
-                        "(res %d, len %d)\n", __func__, res,
+                        "%s: receive request FAILED! (res %d, len %d)\n",
-                        rr3->read_urb->transfer_buffer_length);
+                        __func__, res, rr3->read_urb->transfer_buffer_length);
 }
 static void redrat3_dump_fw_error(struct redrat3_dev *rr3, int code)
@@ -359,7 +335,7 @@ static void redrat3_rx_timeout(unsigned long data)
 {
        struct redrat3_dev *rr3 = (struct redrat3_dev *)data;
-        rr3_dbg(rr3->dev, "calling ir_raw_event_reset\n");
+        dev_dbg(rr3->dev, "calling ir_raw_event_reset\n");
        ir_raw_event_reset(rr3->rc);
 }
@@ -377,8 +353,6 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
                return;
        }
-        rr3_ftr(rr3->dev, "Entered %s\n", __func__);
        dev = rr3->dev;
        /* Make sure we reset the IR kfifo after a bit of inactivity */
@@ -386,7 +360,7 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
        mod_timer(&rr3->rx_timeout, jiffies + delay);
        mod_freq = redrat3_val_to_mod_freq(&rr3->irdata);
-        rr3_dbg(dev, "Got mod_freq of %u\n", mod_freq);
+        dev_dbg(dev, "Got mod_freq of %u\n", mod_freq);
        /* process each rr3 encoded byte into an int */
        sig_size = be16_to_cpu(rr3->irdata.sig_size);
@@ -408,7 +382,7 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
                /* cap the value to IR_MAX_DURATION */
                rawir.duration &= IR_MAX_DURATION;
-                rr3_dbg(dev, "storing %s with duration %d (i: %d)\n",
+                dev_dbg(dev, "storing %s with duration %d (i: %d)\n",
                        rawir.pulse ? "pulse" : "space", rawir.duration, i);
                ir_raw_event_store_with_filter(rr3->rc, &rawir);
        }
@@ -421,12 +395,12 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
                        rawir.duration = US_TO_NS(2800);
                else
                        rawir.duration = trailer;
-                rr3_dbg(dev, "storing trailing space with duration %d\n",
+                dev_dbg(dev, "storing trailing space with duration %d\n",
                        rawir.duration);
                ir_raw_event_store_with_filter(rr3->rc, &rawir);
        }
-        rr3_dbg(dev, "calling ir_raw_event_handle\n");
+        dev_dbg(dev, "calling ir_raw_event_handle\n");
        ir_raw_event_handle(rr3->rc);
 }
@@ -464,8 +438,6 @@ static int redrat3_enable_detector(struct redrat3_dev *rr3)
        struct device *dev = rr3->dev;
        u8 ret;
-        rr3_ftr(dev, "Entering %s\n", __func__);
        ret = redrat3_send_cmd(RR3_RC_DET_ENABLE, rr3);
        if (ret != 0)
                dev_dbg(dev, "%s: unexpected ret of %d\n",
@@ -486,7 +458,6 @@ static int redrat3_enable_detector(struct redrat3_dev *rr3)
 static inline void redrat3_delete(struct redrat3_dev *rr3,
                                  struct usb_device *udev)
 {
-        rr3_ftr(rr3->dev, "%s cleaning up\n", __func__);
        usb_kill_urb(rr3->read_urb);
        usb_kill_urb(rr3->flash_urb);
        usb_free_urb(rr3->read_urb);
@@ -519,7 +490,7 @@ static u32 redrat3_get_timeout(struct redrat3_dev *rr3)
        else {
                timeout = redrat3_len_to_us(be32_to_cpup(tmp));
-                rr3_dbg(rr3->dev, "Got timeout of %d ms\n", timeout / 1000);
+                dev_dbg(rr3->dev, "Got timeout of %d ms\n", timeout / 1000);
        }
        kfree(tmp);
@@ -535,8 +506,6 @@ static void redrat3_reset(struct redrat3_dev *rr3)
        u8 *val;
        int len = sizeof(u8);
-        rr3_ftr(dev, "Entering %s\n", __func__);
        rxpipe = usb_rcvctrlpipe(udev, 0);
        txpipe = usb_sndctrlpipe(udev, 0);
@@ -550,19 +519,19 @@ static void redrat3_reset(struct redrat3_dev *rr3)
        rc = usb_control_msg(udev, rxpipe, RR3_RESET,
                             USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
                             RR3_CPUCS_REG_ADDR, 0, val, len, HZ * 25);
-        rr3_dbg(dev, "reset returned 0x%02x\n", rc);
+        dev_dbg(dev, "reset returned 0x%02x\n", rc);
        *val = 5;
        rc = usb_control_msg(udev, txpipe, RR3_SET_IR_PARAM,
                             USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
                             RR3_IR_IO_LENGTH_FUZZ, 0, val, len, HZ * 25);
-        rr3_dbg(dev, "set ir parm len fuzz %d rc 0x%02x\n", *val, rc);
+        dev_dbg(dev, "set ir parm len fuzz %d rc 0x%02x\n", *val, rc);
        *val = RR3_DRIVER_MAXLENS;
        rc = usb_control_msg(udev, txpipe, RR3_SET_IR_PARAM,
                             USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
                             RR3_IR_IO_MAX_LENGTHS, 0, val, len, HZ * 25);
-        rr3_dbg(dev, "set ir parm max lens %d rc 0x%02x\n", *val, rc);
+        dev_dbg(dev, "set ir parm max lens %d rc 0x%02x\n", *val, rc);
        kfree(val);
 }
@@ -572,8 +541,6 @@ static void redrat3_get_firmware_rev(struct redrat3_dev *rr3)
        int rc = 0;
        char *buffer;
-        rr3_ftr(rr3->dev, "Entering %s\n", __func__);
        buffer = kzalloc(sizeof(char) * (RR3_FW_VERSION_LEN + 1), GFP_KERNEL);
        if (!buffer) {
                dev_err(rr3->dev, "Memory allocation failure\n");
@@ -591,7 +558,6 @@ static void redrat3_get_firmware_rev(struct redrat3_dev *rr3)
                dev_err(rr3->dev, "Problem fetching firmware ID\n");
        kfree(buffer);
-        rr3_ftr(rr3->dev, "Exiting %s\n", __func__);
 }
 static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
@@ -599,8 +565,6 @@ static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
        struct redrat3_header *header = rr3->bulk_in_buf;
        unsigned pktlen, pkttype;
-        rr3_ftr(rr3->dev, "Entering %s\n", __func__);
        /* grab the Length and type of transfer */
        pktlen = be16_to_cpu(header->length);
        pkttype = be16_to_cpu(header->transfer_type);
@@ -622,12 +586,12 @@ static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
        case RR3_MOD_SIGNAL_IN:
                memcpy(&rr3->irdata, rr3->bulk_in_buf, len);
                rr3->bytes_read = len;
-                rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n",
+                dev_dbg(rr3->dev, "bytes_read %d, pktlen %d\n",
                        rr3->bytes_read, pktlen);
                break;
        default:
-                rr3_dbg(rr3->dev, "ignoring packet with type 0x%02x, len of %d, 0x%02x\n",
+                dev_dbg(rr3->dev, "ignoring packet with type 0x%02x, len of %d, 0x%02x\n",
                                                pkttype, len, pktlen);
                break;
        }
@@ -637,8 +601,6 @@ static void redrat3_read_packet_continue(struct redrat3_dev *rr3, unsigned len)
 {
        void *irdata = &rr3->irdata;
-        rr3_ftr(rr3->dev, "Entering %s\n", __func__);
        if (len + rr3->bytes_read > sizeof(rr3->irdata)) {
                dev_warn(rr3->dev, "too much data for packet\n");
                rr3->bytes_read = 0;
@@ -648,7 +610,7 @@ static void redrat3_read_packet_continue(struct redrat3_dev *rr3, unsigned len)
        memcpy(irdata + rr3->bytes_read, rr3->bulk_in_buf, len);
        rr3->bytes_read += len;
-        rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n", rr3->bytes_read,
+        dev_dbg(rr3->dev, "bytes_read %d, pktlen %d\n", rr3->bytes_read,
                                 be16_to_cpu(rr3->irdata.header.length));
 }
@@ -659,8 +621,6 @@ static int redrat3_get_ir_data(struct redrat3_dev *rr3, unsigned len)
        unsigned pkttype;
        int ret = 0;
-        rr3_ftr(dev, "Entering %s\n", __func__);
        if (rr3->bytes_read == 0 && len >= sizeof(struct redrat3_header)) {
                redrat3_read_packet_start(rr3, len);
        } else if (rr3->bytes_read != 0) {
@@ -681,7 +641,7 @@ static int redrat3_get_ir_data(struct redrat3_dev *rr3, unsigned len)
        if (pkttype == RR3_MOD_SIGNAL_IN)
                redrat3_process_ir_data(rr3);
        else
-                rr3_dbg(dev, "discarding non-signal data packet (type 0x%02x)\n",
+                dev_dbg(dev, "discarding non-signal data packet (type 0x%02x)\n",
                                                                pkttype);
 out:
@@ -705,8 +665,6 @@ static void redrat3_handle_async(struct urb *urb)
                return;
        }
-        rr3_ftr(rr3->dev, "Entering %s\n", __func__);
        switch (urb->status) {
        case 0:
                ret = redrat3_get_ir_data(rr3, urb->actual_length);
@@ -743,7 +701,7 @@ static int redrat3_set_tx_carrier(struct rc_dev *rcdev, u32 carrier)
        struct redrat3_dev *rr3 = rcdev->priv;
        struct device *dev = rr3->dev;
-        rr3_dbg(dev, "Setting modulation frequency to %u", carrier);
+        dev_dbg(dev, "Setting modulation frequency to %u", carrier);
        if (carrier == 0)
                return -EINVAL;
@@ -764,8 +722,6 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
        u8 curlencheck = 0;
        unsigned i, sendbuf_len;
-        rr3_ftr(dev, "Entering %s\n", __func__);
        if (rr3->transmitting) {
                dev_warn(dev, "%s: transmitter already in use\n", __func__);
                return -EAGAIN;
@@ -801,7 +757,7 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
                                break;
                }
                if (lencheck == curlencheck) {
-                        rr3_dbg(dev, "txbuf[%d]=%u, pos %d, enc %u\n",
+                        dev_dbg(dev, "txbuf[%d]=%u, pos %d, enc %u\n",
                                i, txbuf[i], curlencheck, cur_sample_len);
                        if (curlencheck < RR3_DRIVER_MAXLENS) {
                                /* now convert the value to a proper
@@ -835,7 +791,7 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
        pipe = usb_sndbulkpipe(rr3->udev, rr3->ep_out->bEndpointAddress);
        ret = usb_bulk_msg(rr3->udev, pipe, irdata,
                            sendbuf_len, &ret_len, 10 * HZ);
-        rr3_dbg(dev, "sent %d bytes, (ret %d)\n", ret_len, ret);
+        dev_dbg(dev, "sent %d bytes, (ret %d)\n", ret_len, ret);
        /* now tell the hardware to transmit what we sent it */
        pipe = usb_rcvctrlpipe(rr3->udev, 0);
@@ -957,8 +913,6 @@ static int redrat3_dev_probe(struct usb_interface *intf,
        int pipe, i;
        int retval = -ENOMEM;
-        rr3_ftr(dev, "%s called\n", __func__);
        uhi = intf->cur_altsetting;
        /* find our bulk-in and bulk-out endpoints */
@@ -971,7 +925,7 @@ static int redrat3_dev_probe(struct usb_interface *intf,
                    ((addr & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN) &&
                    ((attrs & USB_ENDPOINT_XFERTYPE_MASK) ==
                     USB_ENDPOINT_XFER_BULK)) {
-                        rr3_dbg(dev, "found bulk-in endpoint at 0x%02x\n",
+                        dev_dbg(dev, "found bulk-in endpoint at 0x%02x\n",
                                ep->bEndpointAddress);
                        /* data comes in on 0x82, 0x81 is for other data... */
                        if (ep->bEndpointAddress == RR3_BULK_IN_EP_ADDR)
@@ -982,7 +936,7 @@ static int redrat3_dev_probe(struct usb_interface *intf,
                    ((addr & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT) &&
                    ((attrs & USB_ENDPOINT_XFERTYPE_MASK) ==
                     USB_ENDPOINT_XFER_BULK)) {
-                        rr3_dbg(dev, "found bulk-out endpoint at 0x%02x\n",
+                        dev_dbg(dev, "found bulk-out endpoint at 0x%02x\n",
                                ep->bEndpointAddress);
                        ep_out = ep;
                }
@@ -1074,7 +1028,6 @@ static int redrat3_dev_probe(struct usb_interface *intf,
        /* we can register the device now, as it is ready */
        usb_set_intfdata(intf, rr3);
-        rr3_ftr(dev, "Exiting %s\n", __func__);
        return 0;
 led_free_error:
@@ -1093,8 +1046,6 @@ static void redrat3_dev_disconnect(struct usb_interface *intf)
        struct usb_device *udev = interface_to_usbdev(intf);
        struct redrat3_dev *rr3 = usb_get_intfdata(intf);
-        rr3_ftr(&intf->dev, "Entering %s\n", __func__);
        if (!rr3)
                return;
@@ -1103,14 +1054,12 @@ static void redrat3_dev_disconnect(struct usb_interface *intf)
        led_classdev_unregister(&rr3->led);
        del_timer_sync(&rr3->rx_timeout);
        redrat3_delete(rr3, udev);
-        rr3_ftr(&intf->dev, "RedRat3 IR Transceiver now disconnected\n");
 }
 static int redrat3_dev_suspend(struct usb_interface *intf, pm_message_t message)
 {
        struct redrat3_dev *rr3 = usb_get_intfdata(intf);
-        rr3_ftr(rr3->dev, "suspend\n");
        led_classdev_suspend(&rr3->led);
        usb_kill_urb(rr3->read_urb);
        usb_kill_urb(rr3->flash_urb);
@@ -1120,7 +1069,7 @@ static int redrat3_dev_suspend(struct usb_interface *intf, pm_message_t message)
 static int redrat3_dev_resume(struct usb_interface *intf)
 {
        struct redrat3_dev *rr3 = usb_get_intfdata(intf);
-        rr3_ftr(rr3->dev, "resume\n");
        if (usb_submit_urb(rr3->read_urb, GFP_ATOMIC))
                return -EIO;
        led_classdev_resume(&rr3->led);
@@ -1144,8 +1093,3 @@ MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_AUTHOR(DRIVER_AUTHOR2);
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(usb, redrat3_dev_table);
-module_param(debug, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Enable module debug spew. 0 = no debugging (default) "
-                 "0x1 = standard debug messages, 0x2 = function tracing debug. "
-                 "Flag bits are addative (i.e., 0x3 for both debug types).");
diff --git a/drivers/media/rc/streamzap.c b/drivers/media/rc/streamzap.c
index f4e0bc3d382c..bd5e4ff9e0ba 100644
--- a/drivers/media/rc/streamzap.c
+++ b/drivers/media/rc/streamzap.c
@@ -42,12 +42,6 @@
 #define DRIVER_NAME     "streamzap"
 #define DRIVER_DESC     "Streamzap Remote Control driver"
-#ifdef CONFIG_USB_DEBUG
-static bool debug = 1;
-#else
-static bool debug;
-#endif
 #define USB_STREAMZAP_VENDOR_ID         0x0e9c
 #define USB_STREAMZAP_PRODUCT_ID        0x0000
@@ -528,6 +522,3 @@ module_usb_driver(streamzap_driver);
 MODULE_AUTHOR("Jarod Wilson <jarod@wilsonet.com>");
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
-module_param(debug, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Enable debugging messages");
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
index de02db802ace..e35580618936 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
@@ -399,7 +399,7 @@ static int dvb_usb_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
        /* clear 'streaming' status bit */
        clear_bit(ADAP_STREAMING, &adap->state_bits);
-        smp_mb__after_clear_bit();
+        smp_mb__after_atomic();
        wake_up_bit(&adap->state_bits, ADAP_STREAMING);
 skip_feed_stop:
@@ -550,7 +550,7 @@ static int dvb_usb_fe_init(struct dvb_frontend *fe)
 err:
        if (!adap->suspend_resume_active) {
                clear_bit(ADAP_INIT, &adap->state_bits);
-                smp_mb__after_clear_bit();
+                smp_mb__after_atomic();
                wake_up_bit(&adap->state_bits, ADAP_INIT);
        }
@@ -591,7 +591,7 @@ err:
        if (!adap->suspend_resume_active) {
                adap->active_fe = -1;
                clear_bit(ADAP_SLEEP, &adap->state_bits);
-                smp_mb__after_clear_bit();
+                smp_mb__after_atomic();
                wake_up_bit(&adap->state_bits, ADAP_SLEEP);
        }
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index 349e659d75fb..7c4489c42365 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -1200,6 +1200,30 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 EXPORT_SYMBOL_GPL(vb2_buffer_done);
 /**
+ * vb2_discard_done() - discard all buffers marked as DONE
+ * @q:          videobuf2 queue
+ *
+ * This function is intended to be used with suspend/resume operations. It
+ * discards all 'done' buffers as they would be too old to be requested after
+ * resume.
+ *
+ * Drivers must stop the hardware and synchronize with interrupt handlers and/or
+ * delayed works before calling this function to make sure no buffer will be
+ * touched by the driver and/or hardware.
+ */
+void vb2_discard_done(struct vb2_queue *q)
+{
+        struct vb2_buffer *vb;
+        unsigned long flags;
+        spin_lock_irqsave(&q->done_lock, flags);
+        list_for_each_entry(vb, &q->done_list, done_entry)
+                vb->state = VB2_BUF_STATE_ERROR;
+        spin_unlock_irqrestore(&q->done_lock, flags);
+}
+EXPORT_SYMBOL_GPL(vb2_discard_done);
+/**
 * __fill_vb2_buffer() - fill a vb2_buffer with information provided in a
 * v4l2_buffer by the userspace. The caller has already verified that struct
 * v4l2_buffer has a valid number of planes.