media: rp1: cfe: Actually use the number of lanes configured

The driver was calling get_mbus_config to ask the sensor subdev
how many CSI2 data lanes it wished to use and with what other
properties, but then failed to pass that to the DPHY configuration.

Signed-off-by: Dave Stevenson <[email protected]>

Author: 6by9

drivers/media/platform/raspberrypi/rp1_cfe/cfe.c

@@ -1086,7 +1086,7 @@ static u64 sensor_link_rate(struct cfe_device *cfe)
 	}
 
 	link_freq = v4l2_get_link_freq(subdev->ctrl_handler, fmt->depth,
-				       cfe->csi2.active_data_lanes * 2);
+				       cfe->csi2.dphy.active_lanes * 2);
 	if (link_freq < 0)
 		goto err;
 
@@ -1149,27 +1149,24 @@ static int cfe_start_streaming(struct vb2_queue *vq, unsigned int count)
 	cfg_reg_write(cfe, MIPICFG_CFG, MIPICFG_CFG_SEL_CSI);
 	cfg_reg_write(cfe, MIPICFG_INTE, MIPICFG_INT_CSI_DMA | MIPICFG_INT_PISP_FE);
 
-	cfe->csi2.active_data_lanes = cfe->csi2.dphy.num_lanes;
-	cfe_dbg("Running with %u data lanes\n", cfe->csi2.active_data_lanes);
-
 	ret = v4l2_subdev_call(cfe->sensor, pad, get_mbus_config, 0,
 			       &mbus_config);
 	if (ret < 0 && ret != -ENOIOCTLCMD) {
 		cfe_err("g_mbus_config failed\n");
 		goto err_pm_put;
 	}
 
-	cfe->csi2.active_data_lanes = mbus_config.bus.mipi_csi2.num_data_lanes;
-	if (!cfe->csi2.active_data_lanes)
-		cfe->csi2.active_data_lanes = cfe->csi2.dphy.num_lanes;
-	if (cfe->csi2.active_data_lanes > cfe->csi2.dphy.num_lanes) {
+	cfe->csi2.dphy.active_lanes = mbus_config.bus.mipi_csi2.num_data_lanes;
+	if (!cfe->csi2.dphy.active_lanes)
+		cfe->csi2.dphy.active_lanes = cfe->csi2.dphy.max_lanes;
+	if (cfe->csi2.dphy.active_lanes > cfe->csi2.dphy.max_lanes) {
 		cfe_err("Device has requested %u data lanes, which is >%u configured in DT\n",
-			cfe->csi2.active_data_lanes, cfe->csi2.dphy.num_lanes);
+			cfe->csi2.dphy.active_lanes, cfe->csi2.dphy.max_lanes);
 		ret = -EINVAL;
 		goto err_disable_cfe;
 	}
 
-	cfe_dbg("Configuring CSI-2 block\n");
+	cfe_dbg("Configuring CSI-2 block - %u data lanes\n", cfe->csi2.dphy.active_lanes);
 	cfe->csi2.dphy.dphy_rate = sensor_link_rate(cfe) / 1000000UL;
 	csi2_open_rx(&cfe->csi2);
 
@@ -2167,11 +2164,11 @@ static int of_cfe_connect_subdevs(struct cfe_device *cfe)
 		}
 	}
 
-	cfe->csi2.dphy.num_lanes = ep.bus.mipi_csi2.num_data_lanes;
+	cfe->csi2.dphy.max_lanes = ep.bus.mipi_csi2.num_data_lanes;
 	cfe->csi2.bus_flags = ep.bus.mipi_csi2.flags;
 
 	cfe_dbg("subdevice %pOF: %u data lanes, flags=0x%08x, multipacket_line=%u\n",
-		sensor_node, cfe->csi2.dphy.num_lanes, cfe->csi2.bus_flags,
+		sensor_node, cfe->csi2.dphy.max_lanes, cfe->csi2.bus_flags,
 		cfe->csi2.multipacket_line);
 
 	/* Initialize and register the async notifier. */

drivers/media/platform/raspberrypi/rp1_cfe/csi2.h

@@ -57,7 +57,6 @@ struct csi2_device {
 
 	enum v4l2_mbus_type bus_type;
 	unsigned int bus_flags;
-	u32 active_data_lanes;
 	bool multipacket_line;
 	unsigned int num_lines[CSI2_NUM_CHANNELS];
 

drivers/media/platform/raspberrypi/rp1_cfe/dphy.c

@@ -149,7 +149,7 @@ static void dphy_init(struct dphy_data *dphy)
 
 void dphy_start(struct dphy_data *dphy)
 {
-	dw_csi2_host_write(dphy, N_LANES, (dphy->num_lanes - 1));
+	dw_csi2_host_write(dphy, N_LANES, (dphy->active_lanes - 1));
 	dphy_init(dphy);
 	dw_csi2_host_write(dphy, RESETN, 0xffffffff);
 	usleep_range(10, 50);

drivers/media/platform/raspberrypi/rp1_cfe/dphy.h

@@ -16,7 +16,8 @@ struct dphy_data {
 	void __iomem *base;
 
 	u32 dphy_rate;
-	u32 num_lanes;
+	u32 max_lanes;
+	u32 active_lanes;
 };
 
 void dphy_probe(struct dphy_data *dphy);