Commit 046ee0e2 authored by Alain Volmat's avatar Alain Volmat Committed by Hans Verkuil
Browse files

media: i2c: st-mipid02: add usage of v4l2_get_link_freq 



Use the helper v4l2_get_link_freq instead of performing manually
check of the LINK_FREQ or PIXELRATE ctrls.

Signed-off-by: default avatarAlain Volmat <alain.volmat@foss.st.com>
Signed-off-by: default avatarSakari Ailus <sakari.ailus@linux.intel.com>
Signed-off-by: default avatarHans Verkuil <hverkuil-cisco@xs4all.nl>
parent b33cb0cb

drivers/media/i2c/st-mipid02.c

+18 −48
Original line number Diff line number Diff line
@@ -367,64 +367,34 @@ static int mipid02_detect(struct mipid02_dev *bridge) 
	return mipid02_read_reg(bridge, MIPID02_CLK_LANE_WR_REG1, &reg);

}



static u32 mipid02_get_link_freq_from_cid_link_freq(struct mipid02_dev *bridge,

						    struct v4l2_subdev *subdev)

{

	struct v4l2_querymenu qm = {.id = V4L2_CID_LINK_FREQ, };

	struct v4l2_ctrl *ctrl;

	int ret;



	ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_LINK_FREQ);

	if (!ctrl)

		return 0;

	qm.index = v4l2_ctrl_g_ctrl(ctrl);



	ret = v4l2_querymenu(subdev->ctrl_handler, &qm);

	if (ret)

		return 0;



	return qm.value;

}



static u32 mipid02_get_link_freq_from_cid_pixel_rate(struct mipid02_dev *bridge,

						     struct v4l2_subdev *subdev)

{

	struct v4l2_fwnode_endpoint *ep = &bridge->rx;

	struct v4l2_ctrl *ctrl;

	u32 pixel_clock;

	u32 bpp = bpp_from_code(bridge->fmt.code);



	ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);

	if (!ctrl)

		return 0;

	pixel_clock = v4l2_ctrl_g_ctrl_int64(ctrl);



	return pixel_clock * bpp / (2 * ep->bus.mipi_csi2.num_data_lanes);

}



/*

 * We need to know link frequency to setup clk_lane_reg1 timings. Link frequency

 * will be computed using connected device V4L2_CID_PIXEL_RATE, bit per pixel

 * will be retrieve from connected device via v4l2_get_link_freq, bit per pixel

 * and number of lanes.

 */

static int mipid02_configure_from_rx_speed(struct mipid02_dev *bridge)

{

	struct i2c_client *client = bridge->i2c_client;

	struct v4l2_subdev *subdev = bridge->s_subdev;

	u32 link_freq;

	struct v4l2_fwnode_endpoint *ep = &bridge->rx;

	u32 bpp = bpp_from_code(bridge->fmt.code);

	/*

	 * clk_lane_reg1 requires 4 times the unit interval time, and bitrate

	 * is twice the link frequency, hence ui_4 = 1000000000 * 4 / 2

	 */

	u64 ui_4 = 2000000000;

	s64 link_freq;



	link_freq = mipid02_get_link_freq_from_cid_link_freq(bridge, subdev);

	if (!link_freq) {

		link_freq = mipid02_get_link_freq_from_cid_pixel_rate(bridge,

								      subdev);

		if (!link_freq) {

	link_freq = v4l2_get_link_freq(subdev->ctrl_handler, bpp,

				       2 * ep->bus.mipi_csi2.num_data_lanes);

	if (link_freq < 0) {

		dev_err(&client->dev, "Failed to get link frequency");

		return -EINVAL;

	}

	}



	dev_dbg(&client->dev, "detect link_freq = %d Hz", link_freq);

	bridge->r.clk_lane_reg1 |= (2000000000 / link_freq) << 2;

	dev_dbg(&client->dev, "detect link_freq = %lld Hz", link_freq);

	do_div(ui_4, link_freq);

	bridge->r.clk_lane_reg1 |= ui_4 << 2;



	return 0;

}