diff --git a/drivers/media/platform/rcar-vin/rcar-csi2.c b/drivers/media/platform/rcar-vin/rcar-csi2.c index c6cc4f473a07..cbb3e8a2887d 100644 --- a/drivers/media/platform/rcar-vin/rcar-csi2.c +++ b/drivers/media/platform/rcar-vin/rcar-csi2.c @@ -364,6 +364,7 @@ struct rcar_csi2 { struct v4l2_async_notifier notifier; struct v4l2_async_subdev asd; struct v4l2_subdev *remote; + unsigned int remote_pad; struct v4l2_mbus_framefmt mf; @@ -409,13 +410,14 @@ static void rcsi2_exit_standby(struct rcar_csi2 *priv) reset_control_deassert(priv->rstc); } -static int rcsi2_wait_phy_start(struct rcar_csi2 *priv) +static int rcsi2_wait_phy_start(struct rcar_csi2 *priv, + unsigned int lanes) { unsigned int timeout; /* Wait for the clock and data lanes to enter LP-11 state. */ for (timeout = 0; timeout <= 20; timeout++) { - const u32 lane_mask = (1 << priv->lanes) - 1; + const u32 lane_mask = (1 << lanes) - 1; if ((rcsi2_read(priv, PHCLM_REG) & PHCLM_STOPSTATECKL) && (rcsi2_read(priv, PHDLM_REG) & lane_mask) == lane_mask) @@ -447,7 +449,8 @@ static int rcsi2_set_phypll(struct rcar_csi2 *priv, unsigned int mbps) return 0; } -static int rcsi2_calc_mbps(struct rcar_csi2 *priv, unsigned int bpp) +static int rcsi2_calc_mbps(struct rcar_csi2 *priv, unsigned int bpp, + unsigned int lanes) { struct v4l2_subdev *source; struct v4l2_ctrl *ctrl; @@ -472,15 +475,64 @@ static int rcsi2_calc_mbps(struct rcar_csi2 *priv, unsigned int bpp) * bps = link_freq * 2 */ mbps = v4l2_ctrl_g_ctrl_int64(ctrl) * bpp; - do_div(mbps, priv->lanes * 1000000); + do_div(mbps, lanes * 1000000); return mbps; } +static int rcsi2_get_active_lanes(struct rcar_csi2 *priv, + unsigned int *lanes) +{ + struct v4l2_mbus_config mbus_config = { 0 }; + unsigned int num_lanes = UINT_MAX; + int ret; + + *lanes = priv->lanes; + + ret = v4l2_subdev_call(priv->remote, pad, get_mbus_config, + priv->remote_pad, &mbus_config); + if (ret == -ENOIOCTLCMD) { + dev_dbg(priv->dev, "No remote mbus configuration available\n"); + return 0; + } + + if (ret) { + dev_err(priv->dev, "Failed to get remote mbus configuration\n"); + return ret; + } + + if (mbus_config.type != V4L2_MBUS_CSI2_DPHY) { + dev_err(priv->dev, "Unsupported media bus type %u\n", + mbus_config.type); + return -EINVAL; + } + + if (mbus_config.flags & V4L2_MBUS_CSI2_1_LANE) + num_lanes = 1; + else if (mbus_config.flags & V4L2_MBUS_CSI2_2_LANE) + num_lanes = 2; + else if (mbus_config.flags & V4L2_MBUS_CSI2_3_LANE) + num_lanes = 3; + else if (mbus_config.flags & V4L2_MBUS_CSI2_4_LANE) + num_lanes = 4; + + if (num_lanes > priv->lanes) { + dev_err(priv->dev, + "Unsupported mbus config: too many data lanes %u\n", + num_lanes); + return -EINVAL; + } + + *lanes = num_lanes; + + return 0; +} + static int rcsi2_start_receiver(struct rcar_csi2 *priv) { const struct rcar_csi2_format *format; u32 phycnt, vcdt = 0, vcdt2 = 0, fld = 0; + unsigned int lanes; unsigned int i; int mbps, ret; @@ -522,10 +574,18 @@ static int rcsi2_start_receiver(struct rcar_csi2 *priv) fld |= FLD_FLD_NUM(1); } - phycnt = PHYCNT_ENABLECLK; - phycnt |= (1 << priv->lanes) - 1; + /* + * Get the number of active data lanes inspecting the remote mbus + * configuration. + */ + ret = rcsi2_get_active_lanes(priv, &lanes); + if (ret) + return ret; - mbps = rcsi2_calc_mbps(priv, format->bpp); + phycnt = PHYCNT_ENABLECLK; + phycnt |= (1 << lanes) - 1; + + mbps = rcsi2_calc_mbps(priv, format->bpp, lanes); if (mbps < 0) return mbps; @@ -572,7 +632,7 @@ static int rcsi2_start_receiver(struct rcar_csi2 *priv) rcsi2_write(priv, PHYCNT_REG, phycnt | PHYCNT_SHUTDOWNZ); rcsi2_write(priv, PHYCNT_REG, phycnt | PHYCNT_SHUTDOWNZ | PHYCNT_RSTZ); - ret = rcsi2_wait_phy_start(priv); + ret = rcsi2_wait_phy_start(priv, lanes); if (ret) return ret; @@ -749,6 +809,7 @@ static int rcsi2_notify_bound(struct v4l2_async_notifier *notifier, } priv->remote = subdev; + priv->remote_pad = pad; dev_dbg(priv->dev, "Bound %s pad: %d\n", subdev->name, pad);