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 <dave.stevenson@raspberrypi.com>

diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
index f865375..45bd8ab 100644 (file)
--- a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
+++ b/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,9 +1149,6 @@ 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) {
@@ -1159,17 +1156,17 @@ static int cfe_start_streaming(struct vb2_queue *vq, unsigned int count)
                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. */

diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h
index 19d4fc3..4fff16e 100644 (file)
--- a/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h
+++ b/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];
 

diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/dphy.c b/drivers/media/platform/raspberrypi/rp1_cfe/dphy.c
index f87d9f6..28ea321 100644 (file)
--- a/drivers/media/platform/raspberrypi/rp1_cfe/dphy.c
+++ b/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);

diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/dphy.h b/drivers/media/platform/raspberrypi/rp1_cfe/dphy.h
index d9dffcf..9d7a80b 100644 (file)
--- a/drivers/media/platform/raspberrypi/rp1_cfe/dphy.h
+++ b/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);