adv7842.c

/*
 * adv7842 - Analog Devices ADV7842 video decoder driver
 *
 * Copyright 2013 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
 *
 * This program is free software; you may redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; version 2 of the License.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

/*
 * References (c = chapter, p = page):
 * REF_01 - Analog devices, ADV7842,
 *		Register Settings Recommendations, Rev. 1.9, April 2011
 * REF_02 - Analog devices, Software User Guide, UG-206,
 *		ADV7842 I2C Register Maps, Rev. 0, November 2010
 * REF_03 - Analog devices, Hardware User Guide, UG-214,
 *		ADV7842 Fast Switching 2:1 HDMI 1.4 Receiver with 3D-Comb
 *		Decoder and Digitizer , Rev. 0, January 2011
 */


#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/i2c.h>
#include <linux/delay.h>
#include <linux/videodev2.h>
#include <linux/workqueue.h>
#include <linux/v4l2-dv-timings.h>
#include <media/v4l2-device.h>
#include <media/v4l2-ctrls.h>
#include <media/v4l2-dv-timings.h>
#include <media/adv7842.h>

static int debug;
module_param(debug, int, 0644);
MODULE_PARM_DESC(debug, "debug level (0-2)");

MODULE_DESCRIPTION("Analog Devices ADV7842 video decoder driver");
MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
MODULE_AUTHOR("Martin Bugge <marbugge@cisco.com>");
MODULE_LICENSE("GPL");

/* ADV7842 system clock frequency */
#define ADV7842_fsc (28636360)

/*
**********************************************************************
*
*  Arrays with configuration parameters for the ADV7842
*
**********************************************************************
*/

struct adv7842_state {
	struct adv7842_platform_data pdata;
	struct v4l2_subdev sd;
	struct media_pad pad;
	struct v4l2_ctrl_handler hdl;
	enum adv7842_mode mode;
	struct v4l2_dv_timings timings;
	enum adv7842_vid_std_select vid_std_select;
	v4l2_std_id norm;
	struct {
		u8 edid[256];
		u32 present;
	} hdmi_edid;
	struct {
		u8 edid[256];
		u32 present;
	} vga_edid;
	struct v4l2_fract aspect_ratio;
	u32 rgb_quantization_range;
	bool is_cea_format;
	struct workqueue_struct *work_queues;
	struct delayed_work delayed_work_enable_hotplug;
	bool restart_stdi_once;
	bool hdmi_port_a;

	/* i2c clients */
	struct i2c_client *i2c_sdp_io;
	struct i2c_client *i2c_sdp;
	struct i2c_client *i2c_cp;
	struct i2c_client *i2c_vdp;
	struct i2c_client *i2c_afe;
	struct i2c_client *i2c_hdmi;
	struct i2c_client *i2c_repeater;
	struct i2c_client *i2c_edid;
	struct i2c_client *i2c_infoframe;
	struct i2c_client *i2c_cec;
	struct i2c_client *i2c_avlink;

	/* controls */
	struct v4l2_ctrl *detect_tx_5v_ctrl;
	struct v4l2_ctrl *analog_sampling_phase_ctrl;
	struct v4l2_ctrl *free_run_color_ctrl_manual;
	struct v4l2_ctrl *free_run_color_ctrl;
	struct v4l2_ctrl *rgb_quantization_range_ctrl;
};

/* Unsupported timings. This device cannot support 720p30. */
static const struct v4l2_dv_timings adv7842_timings_exceptions[] = {
	V4L2_DV_BT_CEA_1280X720P30,
	{ }
};

static bool adv7842_check_dv_timings(const struct v4l2_dv_timings *t, void *hdl)
{
	int i;

	for (i = 0; adv7842_timings_exceptions[i].bt.width; i++)
		if (v4l2_match_dv_timings(t, adv7842_timings_exceptions + i, 0))
			return false;
	return true;
}

struct adv7842_video_standards {
	struct v4l2_dv_timings timings;
	u8 vid_std;
	u8 v_freq;
};

/* sorted by number of lines */
static const struct adv7842_video_standards adv7842_prim_mode_comp[] = {
	/* { V4L2_DV_BT_CEA_720X480P59_94, 0x0a, 0x00 }, TODO flickering */
	{ V4L2_DV_BT_CEA_720X576P50, 0x0b, 0x00 },
	{ V4L2_DV_BT_CEA_1280X720P50, 0x19, 0x01 },
	{ V4L2_DV_BT_CEA_1280X720P60, 0x19, 0x00 },
	{ V4L2_DV_BT_CEA_1920X1080P24, 0x1e, 0x04 },
	{ V4L2_DV_BT_CEA_1920X1080P25, 0x1e, 0x03 },
	{ V4L2_DV_BT_CEA_1920X1080P30, 0x1e, 0x02 },
	{ V4L2_DV_BT_CEA_1920X1080P50, 0x1e, 0x01 },
	{ V4L2_DV_BT_CEA_1920X1080P60, 0x1e, 0x00 },
	/* TODO add 1920x1080P60_RB (CVT timing) */
	{ },
};

/* sorted by number of lines */
static const struct adv7842_video_standards adv7842_prim_mode_gr[] = {
	{ V4L2_DV_BT_DMT_640X480P60, 0x08, 0x00 },
	{ V4L2_DV_BT_DMT_640X480P72, 0x09, 0x00 },
	{ V4L2_DV_BT_DMT_640X480P75, 0x0a, 0x00 },
	{ V4L2_DV_BT_DMT_640X480P85, 0x0b, 0x00 },
	{ V4L2_DV_BT_DMT_800X600P56, 0x00, 0x00 },
	{ V4L2_DV_BT_DMT_800X600P60, 0x01, 0x00 },
	{ V4L2_DV_BT_DMT_800X600P72, 0x02, 0x00 },
	{ V4L2_DV_BT_DMT_800X600P75, 0x03, 0x00 },
	{ V4L2_DV_BT_DMT_800X600P85, 0x04, 0x00 },
	{ V4L2_DV_BT_DMT_1024X768P60, 0x0c, 0x00 },
	{ V4L2_DV_BT_DMT_1024X768P70, 0x0d, 0x00 },
	{ V4L2_DV_BT_DMT_1024X768P75, 0x0e, 0x00 },
	{ V4L2_DV_BT_DMT_1024X768P85, 0x0f, 0x00 },
	{ V4L2_DV_BT_DMT_1280X1024P60, 0x05, 0x00 },
	{ V4L2_DV_BT_DMT_1280X1024P75, 0x06, 0x00 },
	{ V4L2_DV_BT_DMT_1360X768P60, 0x12, 0x00 },
	{ V4L2_DV_BT_DMT_1366X768P60, 0x13, 0x00 },
	{ V4L2_DV_BT_DMT_1400X1050P60, 0x14, 0x00 },
	{ V4L2_DV_BT_DMT_1400X1050P75, 0x15, 0x00 },
	{ V4L2_DV_BT_DMT_1600X1200P60, 0x16, 0x00 }, /* TODO not tested */
	/* TODO add 1600X1200P60_RB (not a DMT timing) */
	{ V4L2_DV_BT_DMT_1680X1050P60, 0x18, 0x00 },
	{ V4L2_DV_BT_DMT_1920X1200P60_RB, 0x19, 0x00 }, /* TODO not tested */
	{ },
};

/* sorted by number of lines */
static const struct adv7842_video_standards adv7842_prim_mode_hdmi_comp[] = {
	{ V4L2_DV_BT_CEA_720X480P59_94, 0x0a, 0x00 },
	{ V4L2_DV_BT_CEA_720X576P50, 0x0b, 0x00 },
	{ V4L2_DV_BT_CEA_1280X720P50, 0x13, 0x01 },
	{ V4L2_DV_BT_CEA_1280X720P60, 0x13, 0x00 },
	{ V4L2_DV_BT_CEA_1920X1080P24, 0x1e, 0x04 },
	{ V4L2_DV_BT_CEA_1920X1080P25, 0x1e, 0x03 },
	{ V4L2_DV_BT_CEA_1920X1080P30, 0x1e, 0x02 },
	{ V4L2_DV_BT_CEA_1920X1080P50, 0x1e, 0x01 },
	{ V4L2_DV_BT_CEA_1920X1080P60, 0x1e, 0x00 },
	{ },
};

/* sorted by number of lines */
static const struct adv7842_video_standards adv7842_prim_mode_hdmi_gr[] = {
	{ V4L2_DV_BT_DMT_640X480P60, 0x08, 0x00 },
	{ V4L2_DV_BT_DMT_640X480P72, 0x09, 0x00 },
	{ V4L2_DV_BT_DMT_640X480P75, 0x0a, 0x00 },
	{ V4L2_DV_BT_DMT_640X480P85, 0x0b, 0x00 },
	{ V4L2_DV_BT_DMT_800X600P56, 0x00, 0x00 },
	{ V4L2_DV_BT_DMT_800X600P60, 0x01, 0x00 },
	{ V4L2_DV_BT_DMT_800X600P72, 0x02, 0x00 },
	{ V4L2_DV_BT_DMT_800X600P75, 0x03, 0x00 },
	{ V4L2_DV_BT_DMT_800X600P85, 0x04, 0x00 },
	{ V4L2_DV_BT_DMT_1024X768P60, 0x0c, 0x00 },
	{ V4L2_DV_BT_DMT_1024X768P70, 0x0d, 0x00 },
	{ V4L2_DV_BT_DMT_1024X768P75, 0x0e, 0x00 },
	{ V4L2_DV_BT_DMT_1024X768P85, 0x0f, 0x00 },
	{ V4L2_DV_BT_DMT_1280X1024P60, 0x05, 0x00 },
	{ V4L2_DV_BT_DMT_1280X1024P75, 0x06, 0x00 },
	{ },
};

/* ----------------------------------------------------------------------- */

static inline struct adv7842_state *to_state(struct v4l2_subdev *sd)
{
	return container_of(sd, struct adv7842_state, sd);
}

static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
{
	return &container_of(ctrl->handler, struct adv7842_state, hdl)->sd;
}

static inline unsigned hblanking(const struct v4l2_bt_timings *t)
{
	return V4L2_DV_BT_BLANKING_WIDTH(t);
}

static inline unsigned htotal(const struct v4l2_bt_timings *t)
{
	return V4L2_DV_BT_FRAME_WIDTH(t);
}

static inline unsigned vblanking(const struct v4l2_bt_timings *t)
{
	return V4L2_DV_BT_BLANKING_HEIGHT(t);
}

static inline unsigned vtotal(const struct v4l2_bt_timings *t)
{
	return V4L2_DV_BT_FRAME_HEIGHT(t);
}


/* ----------------------------------------------------------------------- */

static s32 adv_smbus_read_byte_data_check(struct i2c_client *client,
					  u8 command, bool check)
{
	union i2c_smbus_data data;

	if (!i2c_smbus_xfer(client->adapter, client->addr, client->flags,
			    I2C_SMBUS_READ, command,
			    I2C_SMBUS_BYTE_DATA, &data))
		return data.byte;
	if (check)
		v4l_err(client, "error reading %02x, %02x\n",
			client->addr, command);
	return -EIO;
}

static s32 adv_smbus_read_byte_data(struct i2c_client *client, u8 command)
{
	int i;

	for (i = 0; i < 3; i++) {
		int ret = adv_smbus_read_byte_data_check(client, command, true);

		if (ret >= 0) {
			if (i)
				v4l_err(client, "read ok after %d retries\n", i);
			return ret;
		}
	}
	v4l_err(client, "read failed\n");
	return -EIO;
}

static s32 adv_smbus_write_byte_data(struct i2c_client *client,
				     u8 command, u8 value)
{
	union i2c_smbus_data data;
	int err;
	int i;

	data.byte = value;
	for (i = 0; i < 3; i++) {
		err = i2c_smbus_xfer(client->adapter, client->addr,
				     client->flags,
				     I2C_SMBUS_WRITE, command,
				     I2C_SMBUS_BYTE_DATA, &data);
		if (!err)
			break;
	}
	if (err < 0)
		v4l_err(client, "error writing %02x, %02x, %02x\n",
			client->addr, command, value);
	return err;
}

static void adv_smbus_write_byte_no_check(struct i2c_client *client,
					  u8 command, u8 value)
{
	union i2c_smbus_data data;
	data.byte = value;

	i2c_smbus_xfer(client->adapter, client->addr,
		       client->flags,
		       I2C_SMBUS_WRITE, command,
		       I2C_SMBUS_BYTE_DATA, &data);
}

static s32 adv_smbus_write_i2c_block_data(struct i2c_client *client,
				  u8 command, unsigned length, const u8 *values)
{
	union i2c_smbus_data data;

	if (length > I2C_SMBUS_BLOCK_MAX)
		length = I2C_SMBUS_BLOCK_MAX;
	data.block[0] = length;
	memcpy(data.block + 1, values, length);
	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
			      I2C_SMBUS_WRITE, command,
			      I2C_SMBUS_I2C_BLOCK_DATA, &data);
}

/* ----------------------------------------------------------------------- */

static inline int io_read(struct v4l2_subdev *sd, u8 reg)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);

	return adv_smbus_read_byte_data(client, reg);
}

static inline int io_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);

	return adv_smbus_write_byte_data(client, reg, val);
}

static inline int io_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
{
	return io_write(sd, reg, (io_read(sd, reg) & mask) | val);
}

static inline int avlink_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_avlink, reg);
}

static inline int avlink_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_avlink, reg, val);
}

static inline int cec_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_cec, reg);
}

static inline int cec_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_cec, reg, val);
}

static inline int cec_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
{
	return cec_write(sd, reg, (cec_read(sd, reg) & mask) | val);
}

static inline int infoframe_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_infoframe, reg);
}

static inline int infoframe_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_infoframe, reg, val);
}

static inline int sdp_io_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_sdp_io, reg);
}

static inline int sdp_io_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_sdp_io, reg, val);
}

static inline int sdp_io_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
{
	return sdp_io_write(sd, reg, (sdp_io_read(sd, reg) & mask) | val);
}

static inline int sdp_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_sdp, reg);
}

static inline int sdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_sdp, reg, val);
}

static inline int sdp_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
{
	return sdp_write(sd, reg, (sdp_read(sd, reg) & mask) | val);
}

static inline int afe_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_afe, reg);
}

static inline int afe_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_afe, reg, val);
}

static inline int afe_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
{
	return afe_write(sd, reg, (afe_read(sd, reg) & mask) | val);
}

static inline int rep_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_repeater, reg);
}

static inline int rep_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_repeater, reg, val);
}

static inline int rep_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
{
	return rep_write(sd, reg, (rep_read(sd, reg) & mask) | val);
}

static inline int edid_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_edid, reg);
}

static inline int edid_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_edid, reg, val);
}

static inline int hdmi_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_hdmi, reg);
}

static inline int hdmi_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_hdmi, reg, val);
}

static inline int hdmi_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
{
	return hdmi_write(sd, reg, (hdmi_read(sd, reg) & mask) | val);
}

static inline int cp_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_cp, reg);
}

static inline int cp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_cp, reg, val);
}

static inline int cp_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
{
	return cp_write(sd, reg, (cp_read(sd, reg) & mask) | val);
}

static inline int vdp_read(struct v4l2_subdev *sd, u8 reg)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_read_byte_data(state->i2c_vdp, reg);
}

static inline int vdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
{
	struct adv7842_state *state = to_state(sd);

	return adv_smbus_write_byte_data(state->i2c_vdp, reg, val);
}

static void main_reset(struct v4l2_subdev *sd)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);

	v4l2_dbg(1, debug, sd, "%s:\n", __func__);

	adv_smbus_write_byte_no_check(client, 0xff, 0x80);

	mdelay(5);
}

/* ----------------------------------------------------------------------- */

static inline bool is_analog_input(struct v4l2_subdev *sd)
{
	struct adv7842_state *state = to_state(sd);

	return ((state->mode == ADV7842_MODE_RGB) ||
		(state->mode == ADV7842_MODE_COMP));
}

static inline bool is_digital_input(struct v4l2_subdev *sd)
{
	struct adv7842_state *state = to_state(sd);

	return state->mode == ADV7842_MODE_HDMI;
}

static const struct v4l2_dv_timings_cap adv7842_timings_cap_analog = {
	.type = V4L2_DV_BT_656_1120,
	/* keep this initialization for compatibility with GCC < 4.4.6 */
	.reserved = { 0 },
	V4L2_INIT_BT_TIMINGS(0, 1920, 0, 1200, 25000000, 170000000,
		V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
		V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_REDUCED_BLANKING |
			V4L2_DV_BT_CAP_CUSTOM)
};

static const struct v4l2_dv_timings_cap adv7842_timings_cap_digital = {
	.type = V4L2_DV_BT_656_1120,
	/* keep this initialization for compatibility with GCC < 4.4.6 */
	.reserved = { 0 },
	V4L2_INIT_BT_TIMINGS(0, 1920, 0, 1200, 25000000, 225000000,
		V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT,
		V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_REDUCED_BLANKING |
			V4L2_DV_BT_CAP_CUSTOM)
};

static inline const struct v4l2_dv_timings_cap *
adv7842_get_dv_timings_cap(struct v4l2_subdev *sd)
{
	return is_digital_input(sd) ? &adv7842_timings_cap_digital :
				      &adv7842_timings_cap_analog;
}

/* ----------------------------------------------------------------------- */

static void adv7842_delayed_work_enable_hotplug(struct work_struct *work)
{
	struct delayed_work *dwork = to_delayed_work(work);
	struct adv7842_state *state = container_of(dwork,
			struct adv7842_state, delayed_work_enable_hotplug);
	struct v4l2_subdev *sd = &state->sd;
	int present = state->hdmi_edid.present;
	u8 mask = 0;

	v4l2_dbg(2, debug, sd, "%s: enable hotplug on ports: 0x%x\n",
			__func__, present);

	if (present & (0x04 << ADV7842_EDID_PORT_A))
		mask |= 0x20;
	if (present & (0x04 << ADV7842_EDID_PORT_B))
		mask |= 0x10;
	io_write_and_or(sd, 0x20, 0xcf, mask);
}

static int edid_write_vga_segment(struct v4l2_subdev *sd)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);
	struct adv7842_state *state = to_state(sd);
	const u8 *val = state->vga_edid.edid;
	int err = 0;
	int i;

	v4l2_dbg(2, debug, sd, "%s: write EDID on VGA port\n", __func__);

	/* HPA disable on port A and B */
	io_write_and_or(sd, 0x20, 0xcf, 0x00);

	/* Disable I2C access to internal EDID ram from VGA DDC port */
	rep_write_and_or(sd, 0x7f, 0x7f, 0x00);

	/* edid segment pointer '1' for VGA port */
	rep_write_and_or(sd, 0x77, 0xef, 0x10);

	for (i = 0; !err && i < 256; i += I2C_SMBUS_BLOCK_MAX)
		err = adv_smbus_write_i2c_block_data(state->i2c_edid, i,
					     I2C_SMBUS_BLOCK_MAX, val + i);
	if (err)
		return err;

	/* Calculates the checksums and enables I2C access
	 * to internal EDID ram from VGA DDC port.
	 */
	rep_write_and_or(sd, 0x7f, 0x7f, 0x80);

	for (i = 0; i < 1000; i++) {
		if (rep_read(sd, 0x79) & 0x20)
			break;
		mdelay(1);
	}
	if (i == 1000) {
		v4l_err(client, "error enabling edid on VGA port\n");
		return -EIO;
	}

	/* enable hotplug after 200 ms */
	queue_delayed_work(state->work_queues,
			&state->delayed_work_enable_hotplug, HZ / 5);

	return 0;
}

static int edid_spa_location(const u8 *edid)
{
	u8 d;

	/*
	 * TODO, improve and update for other CEA extensions
	 * currently only for 1 segment (256 bytes),
	 * i.e. 1 extension block and CEA revision 3.
	 */
	if ((edid[0x7e] != 1) ||
	    (edid[0x80] != 0x02) ||
	    (edid[0x81] != 0x03)) {
		return -EINVAL;
	}
	/*
	 * search Vendor Specific Data Block (tag 3)
	 */
	d = edid[0x82] & 0x7f;
	if (d > 4) {
		int i = 0x84;
		int end = 0x80 + d;
		do {
			u8 tag = edid[i]>>5;
			u8 len = edid[i] & 0x1f;

			if ((tag == 3) && (len >= 5))
				return i + 4;
			i += len + 1;
		} while (i < end);
	}
	return -EINVAL;
}

static int edid_write_hdmi_segment(struct v4l2_subdev *sd, u8 port)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);
	struct adv7842_state *state = to_state(sd);
	const u8 *val = state->hdmi_edid.edid;
	int spa_loc = edid_spa_location(val);
	int err = 0;
	int i;

	v4l2_dbg(2, debug, sd, "%s: write EDID on port %c (spa at 0x%x)\n",
			__func__, (port == ADV7842_EDID_PORT_A) ? 'A' : 'B', spa_loc);

	/* HPA disable on port A and B */
	io_write_and_or(sd, 0x20, 0xcf, 0x00);

	/* Disable I2C access to internal EDID ram from HDMI DDC ports */
	rep_write_and_or(sd, 0x77, 0xf3, 0x00);

	if (!state->hdmi_edid.present)
		return 0;

	/* edid segment pointer '0' for HDMI ports */
	rep_write_and_or(sd, 0x77, 0xef, 0x00);

	for (i = 0; !err && i < 256; i += I2C_SMBUS_BLOCK_MAX)
		err = adv_smbus_write_i2c_block_data(state->i2c_edid, i,
						     I2C_SMBUS_BLOCK_MAX, val + i);
	if (err)
		return err;

	if (spa_loc < 0)
		spa_loc = 0xc0; /* Default value [REF_02, p. 199] */

	if (port == ADV7842_EDID_PORT_A) {
		rep_write(sd, 0x72, val[spa_loc]);
		rep_write(sd, 0x73, val[spa_loc + 1]);
	} else {
		rep_write(sd, 0x74, val[spa_loc]);
		rep_write(sd, 0x75, val[spa_loc + 1]);
	}
	rep_write(sd, 0x76, spa_loc & 0xff);
	rep_write_and_or(sd, 0x77, 0xbf, (spa_loc >> 2) & 0x40);

	/* Calculates the checksums and enables I2C access to internal
	 * EDID ram from HDMI DDC ports
	 */
	rep_write_and_or(sd, 0x77, 0xf3, state->hdmi_edid.present);

	for (i = 0; i < 1000; i++) {
		if (rep_read(sd, 0x7d) & state->hdmi_edid.present)
			break;
		mdelay(1);
	}
	if (i == 1000) {
		v4l_err(client, "error enabling edid on port %c\n",
				(port == ADV7842_EDID_PORT_A) ? 'A' : 'B');
		return -EIO;
	}

	/* enable hotplug after 200 ms */
	queue_delayed_work(state->work_queues,
			&state->delayed_work_enable_hotplug, HZ / 5);

	return 0;
}

/* ----------------------------------------------------------------------- */

#ifdef CONFIG_VIDEO_ADV_DEBUG
static void adv7842_inv_register(struct v4l2_subdev *sd)
{
	v4l2_info(sd, "0x000-0x0ff: IO Map\n");
	v4l2_info(sd, "0x100-0x1ff: AVLink Map\n");
	v4l2_info(sd, "0x200-0x2ff: CEC Map\n");
	v4l2_info(sd, "0x300-0x3ff: InfoFrame Map\n");
	v4l2_info(sd, "0x400-0x4ff: SDP_IO Map\n");
	v4l2_info(sd, "0x500-0x5ff: SDP Map\n");
	v4l2_info(sd, "0x600-0x6ff: AFE Map\n");
	v4l2_info(sd, "0x700-0x7ff: Repeater Map\n");
	v4l2_info(sd, "0x800-0x8ff: EDID Map\n");
	v4l2_info(sd, "0x900-0x9ff: HDMI Map\n");
	v4l2_info(sd, "0xa00-0xaff: CP Map\n");
	v4l2_info(sd, "0xb00-0xbff: VDP Map\n");
}

static int adv7842_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 = sdp_io_read(sd, reg->reg & 0xff);
		break;
	case 5:
		reg->val = sdp_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 = cp_read(sd, reg->reg & 0xff);
		break;
	case 0xb:
		reg->val = vdp_read(sd, reg->reg & 0xff);
		break;
	default:
		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
		adv7842_inv_register(sd);
		break;
	}
	return 0;
}

static int adv7842_s_register(struct v4l2_subdev *sd,
		const struct v4l2_dbg_register *reg)
{
	u8 val = reg->val & 0xff;

	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:
		sdp_io_write(sd, reg->reg & 0xff, val);
		break;
	case 5:
		sdp_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:
		cp_write(sd, reg->reg & 0xff, val);
		break;
	case 0xb:
		vdp_write(sd, reg->reg & 0xff, val);
		break;
	default:
		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
		adv7842_inv_register(sd);
		break;
	}
	return 0;
}
#endif

static int adv7842_s_detect_tx_5v_ctrl(struct v4l2_subdev *sd)
{
	struct adv7842_state *state = to_state(sd);
	int prev = v4l2_ctrl_g_ctrl(state->detect_tx_5v_ctrl);
	u8 reg_io_6f = io_read(sd, 0x6f);
	int val = 0;

	if (reg_io_6f & 0x02)
		val |= 1; /* port A */
	if (reg_io_6f & 0x01)
		val |= 2; /* port B */

	v4l2_dbg(1, debug, sd, "%s: 0x%x -> 0x%x\n", __func__, prev, val);

	if (val != prev)
		return v4l2_ctrl_s_ctrl(state->detect_tx_5v_ctrl, val);
	return 0;
}

static int find_and_set_predefined_video_timings(struct v4l2_subdev *sd,
		u8 prim_mode,
		const struct adv7842_video_standards *predef_vid_timings,
		const struct v4l2_dv_timings *timings)
{
	int i;

	for (i = 0; predef_vid_timings[i].timings.bt.width; i++) {
		if (!v4l2_match_dv_timings(timings, &predef_vid_timings[i].timings,
					  is_digital_input(sd) ? 250000 : 1000000))
			continue;
		/* video std */
		io_write(sd, 0x00, predef_vid_timings[i].vid_std);
		/* v_freq and prim mode */
		io_write(sd, 0x01, (predef_vid_timings[i].v_freq << 4) + prim_mode);
		return 0;
	}

	return -1;
}

static int configure_predefined_video_timings(struct v4l2_subdev *sd,
		struct v4l2_dv_timings *timings)
{
	struct adv7842_state *state = to_state(sd);
	int err;

	v4l2_dbg(1, debug, sd, "%s\n", __func__);

	/* 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(sd, 0x26, 0x00);
	cp_write(sd, 0x27, 0x00);
	cp_write(sd, 0x28, 0x00);
	cp_write(sd, 0x29, 0x00);
	cp_write(sd, 0x8f, 0x40);
	cp_write(sd, 0x90, 0x00);
	cp_write(sd, 0xa5, 0x00);
	cp_write(sd, 0xa6, 0x00);
	cp_write(sd, 0xa7, 0x00);
	cp_write(sd, 0xab, 0x00);
	cp_write(sd, 0xac, 0x00);

	switch (state->mode) {
	case ADV7842_MODE_COMP:
	case ADV7842_MODE_RGB:
		err = find_and_set_predefined_video_timings(sd,
				0x01, adv7842_prim_mode_comp, timings);
		if (err)
			err = find_and_set_predefined_video_timings(sd,
					0x02, adv7842_prim_mode_gr, timings);
		break;
	case ADV7842_MODE_HDMI:
		err = find_and_set_predefined_video_timings(sd,
				0x05, adv7842_prim_mode_hdmi_comp, timings);
		if (err)
			err = find_and_set_predefined_video_timings(sd,
					0x06, adv7842_prim_mode_hdmi_gr, timings);
		break;
	default:
		v4l2_dbg(2, debug, sd, "%s: Unknown mode %d\n",
				__func__, state->mode);
		err = -1;
		break;
	}


	return err;
}

static void configure_custom_video_timings(struct v4l2_subdev *sd,
		const struct v4l2_bt_timings *bt)
{
	struct adv7842_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;
	u16 cp_start_eav = width - bt->hfrontporch;
	u16 cp_start_vbi = height - bt->vfrontporch + 1;
	u16 cp_end_vbi = bt->vsync + bt->vbackporch + 1;
	u16 ch1_fr_ll = (((u32)bt->pixelclock / 100) > 0) ?
		((width * (ADV7842_fsc / 100)) / ((u32)bt->pixelclock / 100)) : 0;
	const u8 pll[2] = {
		0xc0 | ((width >> 8) & 0x1f),
		width & 0xff
	};

	v4l2_dbg(2, debug, sd, "%s\n", __func__);

	switch (state->mode) {
	case ADV7842_MODE_COMP:
	case ADV7842_MODE_RGB:
		/* auto graphics */
		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);

		/* Should only be set in auto-graphics mode [REF_02, p. 91-92] */