Use the D-PHY configuration reported by the remote subdevice in its
frame descriptor to configure the interface.
Store the number of lanes reported through the 'data-lanes' DT property
as the number of phyisically available lanes, which might not correspond
to the number of lanes actually in use.
Signed-off-by: Jacopo Mondi <jacopo+renesas@xxxxxxxxxx>
---
drivers/media/platform/rcar-vin/rcar-csi2.c | 71 +++++++++++++--------
1 file changed, 43 insertions(+), 28 deletions(-)
diff --git a/drivers/media/platform/rcar-vin/rcar-csi2.c b/drivers/media/platform/rcar-vin/rcar-csi2.c
index 6c46bcc0ee83..70b9a8165a6e 100644
--- a/drivers/media/platform/rcar-vin/rcar-csi2.c
+++ b/drivers/media/platform/rcar-vin/rcar-csi2.c
@@ -375,8 +375,8 @@ struct rcar_csi2 {
struct mutex lock;
int stream_count;
- unsigned short lanes;
- unsigned char lane_swap[4];
+ unsigned short available_data_lanes;
+ unsigned short num_data_lanes;
};
static inline struct rcar_csi2 *sd_to_csi2(struct v4l2_subdev *sd)
@@ -424,7 +424,7 @@ static int rcsi2_get_remote_frame_desc(struct rcar_csi2 *priv,
if (ret)
return -ENODEV;
- if (fd->type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
+ if (fd->type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2_DPHY) {
dev_err(priv->dev, "Frame desc do not describe CSI-2 link");
return -EINVAL;
}
@@ -438,7 +438,7 @@ static int rcsi2_wait_phy_start(struct rcar_csi2 *priv)
/* 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 << priv->num_data_lanes) - 1;
if ((rcsi2_read(priv, PHCLM_REG) & PHCLM_STOPSTATECKL) &&
(rcsi2_read(priv, PHDLM_REG) & lane_mask) == lane_mask)
@@ -511,14 +511,15 @@ static int rcsi2_calc_mbps(struct rcar_csi2 *priv,
* bps = link_freq * 2
*/
mbps = v4l2_ctrl_g_ctrl_int64(ctrl) * bpp;
- do_div(mbps, priv->lanes * 1000000);
+ do_div(mbps, priv->num_data_lanes * 1000000);
return mbps;
}
static int rcsi2_start(struct rcar_csi2 *priv)
{
- u32 phycnt, vcdt = 0, vcdt2 = 0;
+ struct v4l2_mbus_frame_desc_entry_csi2_dphy *dphy;
+ u32 phycnt, vcdt = 0, vcdt2 = 0, lswap = 0;
struct v4l2_mbus_frame_desc fd;
unsigned int i;
int mbps, ret;
@@ -548,8 +549,26 @@ static int rcsi2_start(struct rcar_csi2 *priv)
entry->bus.csi2.channel, entry->bus.csi2.data_type);
}
+ /* Get description of the D-PHY media bus configuration. */
+ dphy = &fd.phy.csi2_dphy;
+ if (dphy->clock_lane != 0) {
+ dev_err(priv->dev,
+ "CSI-2 configuration not supported: clock at index %u",
+ dphy->clock_lane);
+ return -EINVAL;
+ }
+
+ if (dphy->num_data_lanes > priv->available_data_lanes ||
+ dphy->num_data_lanes == 3) {
+ dev_err(priv->dev,
+ "Number of CSI-2 data lanes not supported: %u",
+ dphy->num_data_lanes);
+ return -EINVAL;
+ }
+ priv->num_data_lanes = dphy->num_data_lanes;
+
phycnt = PHYCNT_ENABLECLK;
- phycnt |= (1 << priv->lanes) - 1;
+ phycnt |= (1 << priv->num_data_lanes) - 1;
mbps = rcsi2_calc_mbps(priv, &fd);
if (mbps < 0)
@@ -566,12 +585,11 @@ static int rcsi2_start(struct rcar_csi2 *priv)
rcsi2_write(priv, VCDT_REG, vcdt);
if (vcdt2)
rcsi2_write(priv, VCDT2_REG, vcdt2);
+
/* Lanes are zero indexed. */
- rcsi2_write(priv, LSWAP_REG,
- LSWAP_L0SEL(priv->lane_swap[0] - 1) |
- LSWAP_L1SEL(priv->lane_swap[1] - 1) |
- LSWAP_L2SEL(priv->lane_swap[2] - 1) |
- LSWAP_L3SEL(priv->lane_swap[3] - 1));
+ for (i = 0; i < priv->num_data_lanes; ++i)
+ lswap |= (dphy->data_lanes[i] - 1) << (i * 2);
+ rcsi2_write(priv, LSWAP_REG, lswap);
/* Start */
if (priv->info->init_phtw) {
@@ -822,7 +840,7 @@ static const struct v4l2_async_notifier_operations rcar_csi2_notify_ops = {
static int rcsi2_parse_v4l2(struct rcar_csi2 *priv,
struct v4l2_fwnode_endpoint *vep)
{
- unsigned int i;
+ unsigned int num_data_lanes;
/* Only port 0 endpoint 0 is valid. */
if (vep->base.port || vep->base.id)
@@ -833,24 +851,21 @@ static int rcsi2_parse_v4l2(struct rcar_csi2 *priv,
return -EINVAL;
}
- priv->lanes = vep->bus.mipi_csi2.num_data_lanes;
- if (priv->lanes != 1 && priv->lanes != 2 && priv->lanes != 4) {
+ num_data_lanes = vep->bus.mipi_csi2.num_data_lanes;
+ switch (num_data_lanes) {
+ case 1:
+ /* fallthrough */
+ case 2:
+ /* fallthrough */
+ case 4:
+ priv->available_data_lanes = num_data_lanes;
+ break;
+ default:
dev_err(priv->dev, "Unsupported number of data-lanes: %u\n",
- priv->lanes);
+ num_data_lanes);
return -EINVAL;
}
- for (i = 0; i < ARRAY_SIZE(priv->lane_swap); i++) {
- priv->lane_swap[i] = i < priv->lanes ?
- vep->bus.mipi_csi2.data_lanes[i] : i;
-
- /* Check for valid lane number. */
- if (priv->lane_swap[i] < 1 || priv->lane_swap[i] > 4) {
- dev_err(priv->dev, "data-lanes must be in 1-4 range\n");
- return -EINVAL;
- }
- }
-
return 0;
}
@@ -1235,7 +1250,7 @@ static int rcsi2_probe(struct platform_device *pdev)
if (ret < 0)
goto error;
- dev_info(priv->dev, "%d lanes found\n", priv->lanes);
+ dev_info(priv->dev, "%d lanes found\n", priv->available_data_lanes);
return 0;
--
2.21.0
