struct v4l2_async_notifier notifier;
struct v4l2_async_subdev asd;
struct v4l2_subdev *remote;
+ unsigned int remote_pad;
struct v4l2_mbus_framefmt mf;
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)
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;
* 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;
fld |= FLD_FLD_NUM(1);
}
+ /*
+ * Get the number of active data lanes inspecting the remote mbus
+ * configuration.
+ */
+ ret = rcsi2_get_active_lanes(priv, &lanes);
+ if (ret)
+ return ret;
+
phycnt = PHYCNT_ENABLECLK;
- phycnt |= (1 << priv->lanes) - 1;
+ phycnt |= (1 << lanes) - 1;
- mbps = rcsi2_calc_mbps(priv, format->bpp);
+ mbps = rcsi2_calc_mbps(priv, format->bpp, lanes);
if (mbps < 0)
return mbps;
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;
}
priv->remote = subdev;
+ priv->remote_pad = pad;
dev_dbg(priv->dev, "Bound %s pad: %d\n", subdev->name, pad);