[PATCH] media: imx: Parse MIPI CSI2 link frequency correctly

[Marek Vasut <marex@xxxxxxx>]

The current code did not extract the CSI2 link frequency from the menu
items correctly. Fix this.

Signed-off-by: Marek Vasut <marex@xxxxxxx>
Cc: Fabio Estevam <festevam@xxxxxxxxx>
Cc: Hans Verkuil <hans.verkuil@xxxxxxxxx>
Cc: Mauro Carvalho Chehab <mchehab+samsung@xxxxxxxxxx>
Cc: Philipp Zabel <p.zabel@xxxxxxxxxxxxxx>
Cc: Steve Longerbeam <steve_longerbeam@xxxxxxxxxx>
To: linux-media@xxxxxxxxxxxxxxx
---
 drivers/staging/media/imx/imx6-mipi-csi2.c | 28 ++++++++++++++++------
 1 file changed, 21 insertions(+), 7 deletions(-)

diff --git a/drivers/staging/media/imx/imx6-mipi-csi2.c b/drivers/staging/media/imx/imx6-mipi-csi2.c
index f29e28df36ed..64d8229336dd 100644
--- a/drivers/staging/media/imx/imx6-mipi-csi2.c
+++ b/drivers/staging/media/imx/imx6-mipi-csi2.c
@@ -193,18 +193,32 @@ static int max_mbps_to_hsfreqrange_sel(u32 max_mbps)
 
 static int csi2_dphy_init(struct csi2_dev *csi2)
 {
+	struct v4l2_querymenu qm = { .id = V4L2_CID_LINK_FREQ };
 	struct v4l2_ctrl *ctrl;
-	u32 mbps_per_lane;
-	int sel;
+	u32 mbps_per_lane = CSI2_DEFAULT_MAX_MBPS;
+	int ret, sel;
 
 	ctrl = v4l2_ctrl_find(csi2->src_sd->ctrl_handler,
 			      V4L2_CID_LINK_FREQ);
-	if (!ctrl)
-		mbps_per_lane = CSI2_DEFAULT_MAX_MBPS;
-	else
-		mbps_per_lane = DIV_ROUND_UP_ULL(2 * ctrl->qmenu_int[ctrl->val],
-						 USEC_PER_SEC);
+	if (ctrl) {
+		qm.index = v4l2_ctrl_g_ctrl(ctrl);
+		ret = v4l2_querymenu(csi2->src_sd->ctrl_handler, &qm);
+		if (ret) {
+			v4l2_err(&csi2->sd,
+				 "failed to get V4L2_CID_LINK_FREQ menu item, using default.\n");
+			goto exit;
+		}
+
+		if (!qm.value) {
+			v4l2_err(&csi2->sd,
+				 "invalid V4L2_CID_LINK_FREQ, using default.\n");
+			goto exit;
+		}
+
+		mbps_per_lane = DIV_ROUND_UP_ULL(qm.value, USEC_PER_SEC);
+	}
 
+exit:
 	sel = max_mbps_to_hsfreqrange_sel(mbps_per_lane);
 	if (sel < 0)
 		return sel;
-- 
2.20.1