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: Alain Volmat <alain.volmat@foss.st.com>
Signed-off-by: Sakari Ailus <sakari.ailus@linux.intel.com>
Signed-off-by: Hans Verkuil <hverkuil-cisco@xs4all.nl>

diff --git a/drivers/media/i2c/st-mipid02.c b/drivers/media/i2c/st-mipid02.c
index 914f915749a80faf10e1884f74324abb4d85b0f3..b699f0b4efe7d4b6b50c7330413c8c2ff45dfdb7 100644 (file)
--- a/drivers/media/i2c/st-mipid02.c
+++ b/drivers/media/i2c/st-mipid02.c
@@ -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;
-
-       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) {
-                       dev_err(&client->dev, "Failed to get link frequency");
-                       return -EINVAL;
-               }
+       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 = 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;
 }