23 files changed, 1893 insertions, 594 deletions

diff --git a/Documentation/devicetree/bindings/media/i2c/adv7604.txt b/Documentation/devicetree/bindings/media/i2c/adv7604.txt
new file mode 100644
index 000000000000..c27cede3bd68
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/i2c/adv7604.txt
@@ -0,0 +1,70 @@
+* Analog Devices ADV7604/11 video decoder with HDMI receiver
+
+The ADV7604 and ADV7611 are multiformat video decoders with an integrated HDMI
+receiver. The ADV7604 has four multiplexed HDMI inputs and one analog input,
+and the ADV7611 has one HDMI input and no analog input.
+
+These device tree bindings support the ADV7611 only at the moment.
+
+Required Properties:
+
+  - compatible: Must contain one of the following
+    - "adi,adv7611" for the ADV7611
+
+  - reg: I2C slave address
+
+  - hpd-gpios: References to the GPIOs that control the HDMI hot-plug
+    detection pins, one per HDMI input. The active flag indicates the GPIO
+    level that enables hot-plug detection.
+
+The device node must contain one 'port' child node per device input and output
+port, in accordance with the video interface bindings defined in
+Documentation/devicetree/bindings/media/video-interfaces.txt. The port nodes
+are numbered as follows.
+
+  Port                  ADV7611
+------------------------------------------------------------
+  HDMI                  0
+  Digital output        1
+
+The digital output port node must contain at least one endpoint.
+
+Optional Properties:
+
+  - reset-gpios: Reference to the GPIO connected to the device's reset pin.
+
+Optional Endpoint Properties:
+
+  The following three properties are defined in video-interfaces.txt and are
+  valid for source endpoints only.
+
+  - hsync-active: Horizontal synchronization polarity. Defaults to active low.
+  - vsync-active: Vertical synchronization polarity. Defaults to active low.
+  - pclk-sample: Pixel clock polarity. Defaults to output on the falling edge.
+
+  If none of hsync-active, vsync-active and pclk-sample is specified the
+  endpoint will use embedded BT.656 synchronization.
+
+
+Example:
+
+        hdmi_receiver@4c {
+                compatible = "adi,adv7611";
+                reg = <0x4c>;
+
+                reset-gpios = <&ioexp 0 GPIO_ACTIVE_LOW>;
+                hpd-gpios = <&ioexp 2 GPIO_ACTIVE_HIGH>;
+
+                #address-cells = <1>;
+                #size-cells = <0>;
+
+                port@0 {
+                        reg = <0>;
+                };
+                port@1 {
+                        reg = <1>;
+                        hdmi_in: endpoint {
+                                remote-endpoint = <&ccdc_in>;
+                        };
+                };
+        };
diff --git a/Documentation/devicetree/bindings/media/renesas,vsp1.txt b/Documentation/devicetree/bindings/media/renesas,vsp1.txt
new file mode 100644
index 000000000000..87fe08abf36d
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/renesas,vsp1.txt
@@ -0,0 +1,43 @@
+* Renesas VSP1 Video Processing Engine
+
+The VSP1 is a video processing engine that supports up-/down-scaling, alpha
+blending, color space conversion and various other image processing features.
+It can be found in the Renesas R-Car second generation SoCs.
+
+Required properties:
+
+  - compatible: Must contain "renesas,vsp1"
+
+  - reg: Base address and length of the registers block for the VSP1.
+  - interrupts: VSP1 interrupt specifier.
+  - clocks: A phandle + clock-specifier pair for the VSP1 functional clock.
+
+  - renesas,#rpf: Number of Read Pixel Formatter (RPF) modules in the VSP1.
+  - renesas,#uds: Number of Up Down Scaler (UDS) modules in the VSP1.
+  - renesas,#wpf: Number of Write Pixel Formatter (WPF) modules in the VSP1.
+
+
+Optional properties:
+
+  - renesas,has-lif: Boolean, indicates that the LCD Interface (LIF) module is
+    available.
+  - renesas,has-lut: Boolean, indicates that the Look Up Table (LUT) module is
+    available.
+  - renesas,has-sru: Boolean, indicates that the Super Resolution Unit (SRU)
+    module is available.
+
+
+Example: R8A7790 (R-Car H2) VSP1-S node
+
+        vsp1@fe928000 {
+                compatible = "renesas,vsp1";
+                reg = <0 0xfe928000 0 0x8000>;
+                interrupts = <0 267 IRQ_TYPE_LEVEL_HIGH>;
+                clocks = <&mstp1_clks R8A7790_CLK_VSP1_S>;
+
+                renesas,has-lut;
+                renesas,has-sru;
+                renesas,#rpf = <5>;
+                renesas,#uds = <3>;
+                renesas,#wpf = <4>;
+        };
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/delay.h>
+#include <linux/v4l2-dv-timings.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_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;
+        struct i2c_client *i2c_clients[ADV7604_PAGE_MAX];
 
         /* 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,
-                                        u8 command, u8 value)
+static s32 adv_smbus_write_byte_data(struct adv7604_state *state,
+                                     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,
-               u8 command, unsigned length, const u8 *values)
+static s32 adv_smbus_write_i2c_block_data(struct adv7604_state *state,
+                                          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,
-                                I2C_SMBUS_BLOCK_MAX, val + i);
+                err = adv_smbus_write_i2c_block_data(state, ADV7604_PAGE_EDID,
+                                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 ||
-               state->selected_input == ADV7604_INPUT_VGA_COMP;
+        return state->selected_input == ADV7604_PAD_VGA_RGB ||
+               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 ||
-               state->selected_input == ADV7604_INPUT_HDMI_PORT_B ||
-               state->selected_input == ADV7604_INPUT_HDMI_PORT_C ||
-               state->selected_input == ADV7604_INPUT_HDMI_PORT_D;
+        return state->selected_input == ADV7604_PAD_HDMI_PORT_A ||
+               state->selected_input == ADV7604_PAD_HDMI_PORT_B ||
+               state->selected_input == ADV7604_PAD_HDMI_PORT_C ||
+               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;
-        switch (reg->reg >> 8) {
-        case 0:
-                reg->val = io_read(sd, reg->reg & 0xff);
-                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:
+        int ret;
+
+        ret = adv7604_read_reg(sd, reg->reg);
+        if (ret < 0) {
                 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) {
-        case 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:
+        ret = adv7604_write_reg(sd, reg->reg, reg->val);
+        if (ret < 0) {
                 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) |
-                        ((reg_io_6f & 0x08) >> 2) |
-                        (reg_io_6f & 0x04) |
-                        ((reg_io_6f & 0x02) << 2));
+                                info->read_cable_det(sd));
 }
 
 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 */
-        io_write(sd, 0x16, 0x43);
-        io_write(sd, 0x17, 0x5a);
+        if (adv7604_has_afe(state)) {
+                /* reset to default values */
+                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->lcf = ((cp_read(sd, 0xb3) & 0x7) << 8) | cp_read(sd, 0xb4);
+        stdi->bl = cp_read16(sd, 0xb1, 0x3fff);
+        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 ((cp_read(sd, 0xb5) & 0x03) == 0x01) {
-                stdi->hs_pol = ((cp_read(sd, 0xb5) & 0x10) ?
-                                ((cp_read(sd, 0xb5) & 0x08) ? '+' : '-') : 'x');
-                stdi->vs_pol = ((cp_read(sd, 0xb5) & 0x40) ?
-                                ((cp_read(sd, 0xb5) & 0x20) ? '+' : '-') : 'x');
+        if (adv7604_has_afe(state)) {
+                /* read SSPD */
+                polarity = cp_read(sd, 0xb5);
+                if ((polarity & 0x03) == 0x01) {
+                        stdi->hs_pol = polarity & 0x10
+                                     ? (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';
-                stdi->vs_pol = 'x';
+                polarity = hdmi_read(sd, 0x05);
+                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);
-                bt->height = (hdmi_read(sd, 0x09) & 0x0f) * 256 + hdmi_read(sd, 0x0a);
-                freq = (hdmi_read(sd, 0x06) * 1000000) +
-                        ((hdmi_read(sd, 0x3b) & 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;
-                }
-                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;
+                /* FIXME: All masks are incorrect for ADV7611 */
+                bt->width = hdmi_read16(sd, 0x07, 0xfff);
+                bt->height = hdmi_read16(sd, 0x09, 0xfff);
+                bt->pixelclock = info->read_hdmi_pixelclock(sd);
+                bt->hfrontporch = hdmi_read16(sd, 0x20, 0x3ff);
+                bt->hsync = hdmi_read16(sd, 0x22, 0x3ff);
+                bt->hbackporch = hdmi_read16(sd, 0x24, 0x3ff);
+                bt->vfrontporch = hdmi_read16(sd, 0x2a, 0x1fff) / 2;
+                bt->vsync = hdmi_read16(sd, 0x2e, 0x1fff) / 2;
+                bt->vbackporch = hdmi_read16(sd, 0x32, 0x1fff) / 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 +
-                                        hdmi_read(sd, 0x0c);
-                        bt->il_vfrontporch = ((hdmi_read(sd, 0x2c) & 0x1f) * 256 +
-                                        hdmi_read(sd, 0x2d)) / 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;
+                        bt->height += hdmi_read16(sd, 0x0b, 0xfff);
+                        bt->il_vfrontporch = hdmi_read16(sd, 0x2c, 0x1fff) / 2;
+                        bt->il_vsync = hdmi_read16(sd, 0x30, 0x1fff) / 2;
+                        bt->vbackporch = hdmi_read16(sd, 0x34, 0x1fff) / 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(sd, 0x01, 0x00); /* Enable HDMI clock terminators */
+                hdmi_write_clr_set(sd, 0x00, 0x03, state->selected_input);
+                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 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 */
+                adv7604_write_reg_seq(sd, info->recommended_settings[0]);
 
                 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 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
-                hdmi_write(sd, 0x0d, 0x84); /* HDMI filter optimization */
-                hdmi_write(sd, 0x3d, 0x10); /* DDC bus active pull-up control */
-                hdmi_write(sd, 0x3e, 0x39); /* TMDS PLL optimization */
-                hdmi_write(sd, 0x4e, 0x3b); /* TMDS PLL optimization */
-                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 */
+                adv7604_write_reg_seq(sd, info->recommended_settings[1]);
+
+                if (adv7604_has_afe(state)) {
+                        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 */
+                }
+
                 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,
-                             enum v4l2_mbus_pixelcode *code)
+static int adv7604_enum_mbus_code(struct v4l2_subdev *sd,
+                                  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,
-                struct v4l2_mbus_framefmt *fmt)
+static int adv7604_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+                              struct v4l2_subdev_format *format)
 {
         struct adv7604_state *state = to_state(sd);
+        const struct adv7604_format_info *info;
 
-        fmt->width = state->timings.bt.width;
-        fmt->height = state->timings.bt.height;
-        fmt->code = V4L2_MBUS_FMT_FIXED;
-        fmt->field = V4L2_FIELD_NONE;
-        if (state->timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
-                fmt->colorspace = (state->timings.bt.height <= 576) ?
-                        V4L2_COLORSPACE_SMPTE170M : V4L2_COLORSPACE_REC709;
+        if (format->pad != state->source_pad)
+                return -EINVAL;
+
+        info = adv7604_format_info(state, format->format.code);
+        if (info == NULL)
+                info = adv7604_format_info(state, V4L2_MBUS_FMT_YUYV8_2X8);
+
+        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_EDID_PORT_B:
-        case ADV7604_EDID_PORT_C:
-        case ADV7604_EDID_PORT_D:
+        case ADV7604_PAD_HDMI_PORT_A:
+        case ADV7604_PAD_HDMI_PORT_B:
+        case ADV7604_PAD_HDMI_PORT_C:
+        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);
-                rep_write_and_or(sd, 0x77, 0xf0, state->edid.present);
+                adv7604_set_hpd(state, 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);
-        rep_write_and_or(sd, 0x77, 0xf0, 0x00);
+        adv7604_set_hpd(state, 0);
+        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"),
-                        ((rep_read(sd, 0x7d) & 0x02) ? "Yes" : "No"),
-                        ((rep_read(sd, 0x7d) & 0x04) ? "Yes" : "No"),
-                        ((rep_read(sd, 0x7d) & 0x08) ? "Yes" : "No"));
+                        ((edid_enabled & 0x01) ? "Yes" : "No"),
+                        ((edid_enabled & 0x02) ? "Yes" : "No"),
+                        ((edid_enabled & 0x04) ? "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"),
-                        ((io_read(sd, 0x6f) & 0x08) ? "Yes" : "No"),
-                        ((io_read(sd, 0x6f) & 0x04) ? "Yes" : "No"),
-                        ((io_read(sd, 0x6f) & 0x02) ? "Yes" : "No"));
+                        ((cable_det & 0x01) ? "Yes" : "No"),
+                        ((cable_det & 0x02) ? "Yes" : "No"),
+                        ((cable_det & 0x04) ? "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_and_or(sd, 0x04, 0x1f, pdata->op_ch_sel << 5);
-        io_write_and_or(sd, 0x05, 0xf0, pdata->blank_data << 3 |
-                                        pdata->insert_av_codes << 2 |
-                                        pdata->replicate_av_codes << 1 |
-                                        pdata->invert_cbcr << 0);
+        io_write_clr_set(sd, 0x05, 0x0e, pdata->blank_data << 3 |
+                        pdata->insert_av_codes << 2 |
+                        pdata->replicate_av_codes << 1);
+        adv7604_setup_format(state);
 
         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_and_or(sd, 0x1a, 0xf1, 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, 0x15, 0x03, 0x03); /* Mute on FIFO over-/underflow [REF_01, c. 1.2.18] */
+        hdmi_write_clr_set(sd, 0x1a, 0x0e, 0x08); /* Wait 1 s before unmute */
+        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 */
-        io_write_and_or(sd, 0x30, ~(1 << 4), pdata->output_bus_lsb_to_msb << 4);
+        if (adv7604_has_afe(state)) {
+                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, 0x41, 0xd7); /* STDI irq for any change, disable INT2 */
+        io_write(sd, 0x40, 0xc0 | pdata->int1_config); /* Configure INT1 */
         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, 0x73, 0x1e); /* Enable CABLE_DET_A_ST (+5v) interrupts */
+        io_write(sd, 0x6e, info->fmt_change_digital_mask); /* Enable V_LOCKED and DE_REGEN_LCK 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)
-                i2c_unregister_device(state->i2c_avlink);
-        if (state->i2c_cec)
-                i2c_unregister_device(state->i2c_cec);
-        if (state->i2c_infoframe)
-                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);
+        unsigned int i;
+
+        for (i = 1; i < ARRAY_SIZE(state->i2c_clients); ++i) {
+                if (state->i2c_clients[i])
+                        i2c_unregister_device(state->i2c_clients[i]);
+        }
 }
 
 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 (!pdata) {
+        if (IS_ENABLED(CONFIG_OF) && client->dev.of_node) {
+                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) {
-                v4l2_info(sd, "not an adv7604 on address 0x%x\n",
-                                client->addr << 1);
-                return -ENODEV;
+        /*
+         * Verify that the chip is present. On ADV7604 the RD_INFO register only
+         * identifies the revision, while on ADV7611 it identifies the model as
+         * well. Use the HDMI slave address on ADV7604 and RD_INFO on ADV7611.
+         */
+        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 =
-                v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_analog_sampling_phase, NULL);
+        if (adv7604_has_afe(state))
+                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);
-        state->i2c_cec = adv7604_dummy_client(sd, pdata->i2c_cec, 0xf4);
-        state->i2c_infoframe = adv7604_dummy_client(sd, pdata->i2c_infoframe, 0xf5);
-        state->i2c_esdp = adv7604_dummy_client(sd, pdata->i2c_esdp, 0xf6);
-        state->i2c_dpp = adv7604_dummy_client(sd, pdata->i2c_dpp, 0xf7);
-        state->i2c_afe = adv7604_dummy_client(sd, pdata->i2c_afe, 0xf8);
-        state->i2c_repeater = adv7604_dummy_client(sd, pdata->i2c_repeater, 0xf9);
-        state->i2c_edid = adv7604_dummy_client(sd, pdata->i2c_edid, 0xfa);
-        state->i2c_hdmi = adv7604_dummy_client(sd, pdata->i2c_hdmi, 0xfb);
-        state->i2c_test = adv7604_dummy_client(sd, pdata->i2c_test, 0xfc);
-        state->i2c_cp = adv7604_dummy_client(sd, pdata->i2c_cp, 0xfd);
-        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;
+        for (i = 1; i < ADV7604_PAGE_MAX; ++i) {
+                if (!(BIT(i) & state->info->page_mask))
+                        continue;
+
+                state->i2c_clients[i] =
+                        adv7604_dummy_client(sd, state->pdata.i2c_addresses[i],
+                                             0xf2 + i);
+                if (state->i2c_clients[i] == NULL) {
+                        err = -ENOMEM;
+                        v4l2_err(sd, "failed to create i2c client %u\n", i);
+                        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;
-        err = media_entity_init(&sd->entity, 1, &state->pad, 0);
+        state->source_pad = state->info->num_dv_ports
+                          + (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/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 *
-vsp1_get_platform_data(struct platform_device *pdev)
+static int vsp1_validate_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) {
-                if (routes[i].id == entity->id) {
-                        entity->route = routes[i].reg;
+        for (i = 0; i < ARRAY_SIZE(vsp1_routes); ++i) {
+                if (vsp1_routes[i].type == entity->type &&
+                    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;
-        unsigned int route;
+        const struct vsp1_route *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. */
-        vsp1_rpf_write(rpf, VI6_RPF_LOC, 0);
+        /* Output location */
+        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/include/media/adv7604.h b/include/media/adv7604.h
index c6b39372eed7..aa1c4477722d 100644
--- a/include/media/adv7604.h
+++ b/include/media/adv7604.h
@@ -32,14 +32,18 @@ enum adv7604_ain_sel {
         ADV7604_AIN9_4_5_6_SYNC_2_1 = 4,
 };
 
-/* Bus rotation and reordering (IO register 0x04, [7:5]) */
-enum adv7604_op_ch_sel {
-        ADV7604_OP_CH_SEL_GBR = 0,
-        ADV7604_OP_CH_SEL_GRB = 1,
-        ADV7604_OP_CH_SEL_BGR = 2,
-        ADV7604_OP_CH_SEL_RGB = 3,
-        ADV7604_OP_CH_SEL_BRG = 4,
-        ADV7604_OP_CH_SEL_RBG = 5,
+/*
+ * Bus rotation and reordering. This is used to specify component reordering on
+ * the board and describes the components order on the bus when the ADV7604
+ * outputs RGB.
+ */
+enum adv7604_bus_order {
+        ADV7604_BUS_ORDER_RGB,          /* No operation */
+        ADV7604_BUS_ORDER_GRB,          /* Swap 1-2     */
+        ADV7604_BUS_ORDER_RBG,          /* Swap 2-3     */
+        ADV7604_BUS_ORDER_BGR,          /* Swap 1-3     */
+        ADV7604_BUS_ORDER_BRG,          /* Rotate right */
+        ADV7604_BUS_ORDER_GBR,          /* Rotate left  */
 };
 
 /* Input Color Space (IO register 0x02, [7:4]) */
@@ -55,29 +59,11 @@ enum adv7604_inp_color_space {
         ADV7604_INP_COLOR_SPACE_AUTO = 0xf,
 };
 
-/* Select output format (IO register 0x03, [7:0]) */
-enum adv7604_op_format_sel {
-        ADV7604_OP_FORMAT_SEL_SDR_ITU656_8 = 0x00,
-        ADV7604_OP_FORMAT_SEL_SDR_ITU656_10 = 0x01,
-        ADV7604_OP_FORMAT_SEL_SDR_ITU656_12_MODE0 = 0x02,
-        ADV7604_OP_FORMAT_SEL_SDR_ITU656_12_MODE1 = 0x06,
-        ADV7604_OP_FORMAT_SEL_SDR_ITU656_12_MODE2 = 0x0a,
-        ADV7604_OP_FORMAT_SEL_DDR_422_8 = 0x20,
-        ADV7604_OP_FORMAT_SEL_DDR_422_10 = 0x21,
-        ADV7604_OP_FORMAT_SEL_DDR_422_12_MODE0 = 0x22,
-        ADV7604_OP_FORMAT_SEL_DDR_422_12_MODE1 = 0x23,
-        ADV7604_OP_FORMAT_SEL_DDR_422_12_MODE2 = 0x24,
-        ADV7604_OP_FORMAT_SEL_SDR_444_24 = 0x40,
-        ADV7604_OP_FORMAT_SEL_SDR_444_30 = 0x41,
-        ADV7604_OP_FORMAT_SEL_SDR_444_36_MODE0 = 0x42,
-        ADV7604_OP_FORMAT_SEL_DDR_444_24 = 0x60,
-        ADV7604_OP_FORMAT_SEL_DDR_444_30 = 0x61,
-        ADV7604_OP_FORMAT_SEL_DDR_444_36 = 0x62,
-        ADV7604_OP_FORMAT_SEL_SDR_ITU656_16 = 0x80,
-        ADV7604_OP_FORMAT_SEL_SDR_ITU656_20 = 0x81,
-        ADV7604_OP_FORMAT_SEL_SDR_ITU656_24_MODE0 = 0x82,
-        ADV7604_OP_FORMAT_SEL_SDR_ITU656_24_MODE1 = 0x86,
-        ADV7604_OP_FORMAT_SEL_SDR_ITU656_24_MODE2 = 0x8a,
+/* Select output format (IO register 0x03, [4:2]) */
+enum adv7604_op_format_mode_sel {
+        ADV7604_OP_FORMAT_MODE0 = 0x00,
+        ADV7604_OP_FORMAT_MODE1 = 0x04,
+        ADV7604_OP_FORMAT_MODE2 = 0x08,
 };
 
 enum adv7604_drive_strength {
@@ -86,6 +72,30 @@ enum adv7604_drive_strength {
         ADV7604_DR_STR_HIGH = 3,
 };
 
+enum adv7604_int1_config {
+        ADV7604_INT1_CONFIG_OPEN_DRAIN,
+        ADV7604_INT1_CONFIG_ACTIVE_LOW,
+        ADV7604_INT1_CONFIG_ACTIVE_HIGH,
+        ADV7604_INT1_CONFIG_DISABLED,
+};
+
+enum adv7604_page {
+        ADV7604_PAGE_IO,
+        ADV7604_PAGE_AVLINK,
+        ADV7604_PAGE_CEC,
+        ADV7604_PAGE_INFOFRAME,
+        ADV7604_PAGE_ESDP,
+        ADV7604_PAGE_DPP,
+        ADV7604_PAGE_AFE,
+        ADV7604_PAGE_REP,
+        ADV7604_PAGE_EDID,
+        ADV7604_PAGE_HDMI,
+        ADV7604_PAGE_TEST,
+        ADV7604_PAGE_CP,
+        ADV7604_PAGE_VDP,
+        ADV7604_PAGE_MAX,
+};
+
 /* Platform dependent definition */
 struct adv7604_platform_data {
         /* DIS_PWRDNB: 1 if the PWRDNB pin is unused and unconnected */
@@ -94,30 +104,34 @@ struct adv7604_platform_data {
         /* DIS_CABLE_DET_RST: 1 if the 5V pins are unused and unconnected */
         unsigned disable_cable_det_rst:1;
 
+        int default_input;
+
         /* Analog input muxing mode */
         enum adv7604_ain_sel ain_sel;
 
         /* Bus rotation and reordering */
-        enum adv7604_op_ch_sel op_ch_sel;
+        enum adv7604_bus_order bus_order;
 
-        /* Select output format */
-        enum adv7604_op_format_sel op_format_sel;
+        /* Select output format mode */
+        enum adv7604_op_format_mode_sel op_format_mode_sel;
+
+        /* Configuration of the INT1 pin */
+        enum adv7604_int1_config int1_config;
 
         /* IO register 0x02 */
         unsigned alt_gamma:1;
         unsigned op_656_range:1;
-        unsigned rgb_out:1;
         unsigned alt_data_sat:1;
 
         /* IO register 0x05 */
         unsigned blank_data:1;
         unsigned insert_av_codes:1;
         unsigned replicate_av_codes:1;
-        unsigned invert_cbcr:1;
 
         /* IO register 0x06 */
         unsigned inv_vs_pol:1;
         unsigned inv_hs_pol:1;
+        unsigned inv_llc_pol:1;
 
         /* IO register 0x14 */
         enum adv7604_drive_strength dr_str_data;
@@ -131,34 +145,22 @@ struct adv7604_platform_data {
         unsigned hdmi_free_run_mode;
 
         /* i2c addresses: 0 == use default */
-        u8 i2c_avlink;
-        u8 i2c_cec;
-        u8 i2c_infoframe;
-        u8 i2c_esdp;
-        u8 i2c_dpp;
-        u8 i2c_afe;
-        u8 i2c_repeater;
-        u8 i2c_edid;
-        u8 i2c_hdmi;
-        u8 i2c_test;
-        u8 i2c_cp;
-        u8 i2c_vdp;
+        u8 i2c_addresses[ADV7604_PAGE_MAX];
 };
 
-enum adv7604_input_port {
-        ADV7604_INPUT_HDMI_PORT_A,
-        ADV7604_INPUT_HDMI_PORT_B,
-        ADV7604_INPUT_HDMI_PORT_C,
-        ADV7604_INPUT_HDMI_PORT_D,
-        ADV7604_INPUT_VGA_RGB,
-        ADV7604_INPUT_VGA_COMP,
+enum adv7604_pad {
+        ADV7604_PAD_HDMI_PORT_A = 0,
+        ADV7604_PAD_HDMI_PORT_B = 1,
+        ADV7604_PAD_HDMI_PORT_C = 2,
+        ADV7604_PAD_HDMI_PORT_D = 3,
+        ADV7604_PAD_VGA_RGB = 4,
+        ADV7604_PAD_VGA_COMP = 5,
+        /* The source pad is either 1 (ADV7611) or 6 (ADV7604) */
+        ADV7604_PAD_SOURCE = 6,
+        ADV7611_PAD_SOURCE = 1,
+        ADV7604_PAD_MAX = 7,
 };
 
-#define ADV7604_EDID_PORT_A 0
-#define ADV7604_EDID_PORT_B 1
-#define ADV7604_EDID_PORT_C 2
-#define ADV7604_EDID_PORT_D 3
-
 #define V4L2_CID_ADV_RX_ANALOG_SAMPLING_PHASE   (V4L2_CID_DV_CLASS_BASE + 0x1000)
 #define V4L2_CID_ADV_RX_FREE_RUN_COLOR_MANUAL   (V4L2_CID_DV_CLASS_BASE + 0x1001)
 #define V4L2_CID_ADV_RX_FREE_RUN_COLOR          (V4L2_CID_DV_CLASS_BASE + 0x1002)
diff --git a/include/media/v4l2-subdev.h b/include/media/v4l2-subdev.h
index 9fab013eea86..d7465725773d 100644
--- a/include/media/v4l2-subdev.h
+++ b/include/media/v4l2-subdev.h
@@ -338,12 +338,8 @@ struct v4l2_subdev_video_ops {
                         struct v4l2_dv_timings *timings);
         int (*g_dv_timings)(struct v4l2_subdev *sd,
                         struct v4l2_dv_timings *timings);
-        int (*enum_dv_timings)(struct v4l2_subdev *sd,
-                        struct v4l2_enum_dv_timings *timings);
         int (*query_dv_timings)(struct v4l2_subdev *sd,
                         struct v4l2_dv_timings *timings);
-        int (*dv_timings_cap)(struct v4l2_subdev *sd,
-                        struct v4l2_dv_timings_cap *cap);
         int (*enum_mbus_fmt)(struct v4l2_subdev *sd, unsigned int index,
                              enum v4l2_mbus_pixelcode *code);
         int (*enum_mbus_fsizes)(struct v4l2_subdev *sd,