The current code did not extract the CSI2 link frequency from the menu items correctly. Fix this.
Signed-off-by: Marek Vasut <[email protected]> Cc: Fabio Estevam <[email protected]> Cc: Hans Verkuil <[email protected]> Cc: Mauro Carvalho Chehab <[email protected]> Cc: Philipp Zabel <[email protected]> Cc: Steve Longerbeam <[email protected]> To: [email protected] --- 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
