MLK-20529-1: camera: remove V4L2 limitation when the number of

sensors connected is less than the required in dtb

1. create links between entities when the number of sensors
connected is less than the required in dtb
2. unregister the video device which sensors are not connected

Signed-off-by: Guoniu.Zhou <guoniu.zhou@nxp.com>
(cherry picked from commit 33234cc31362b5ee23be34ce2a00947e0dc96acb)

4 files changed, 145 insertions, 2 deletions

diff --git a/drivers/media/platform/imx8/mxc-isi-cap.c b/drivers/media/platform/imx8/mxc-isi-cap.c
index da90c91cf880..1112cd3cb543 100644
--- a/drivers/media/platform/imx8/mxc-isi-cap.c
+++ b/drivers/media/platform/imx8/mxc-isi-cap.c
@@ -1723,7 +1723,7 @@ static void mxc_isi_subdev_unregistered(struct v4l2_subdev *sd)
 	dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__);
 	mutex_lock(&mxc_isi->lock);
 
-	if (mxc_isi->id == 0)
+	if (mxc_isi->id == 0 && mxc_isi->skip_m2m == 0)
 		mxc_isi_unregister_m2m_device(mxc_isi);
 
 	vdev = &mxc_isi->isi_cap.vdev;
diff --git a/drivers/media/platform/imx8/mxc-isi-core.h b/drivers/media/platform/imx8/mxc-isi-core.h
index 3369ae892ccb..3a1d70994292 100644
--- a/drivers/media/platform/imx8/mxc-isi-core.h
+++ b/drivers/media/platform/imx8/mxc-isi-core.h
@@ -290,6 +290,7 @@ struct mxc_isi_dev {
 
 	u32 interface[MAX_PORTS];
 	u32 flags;
+	u32 skip_m2m;
 	u8 chain_buf;
 
 	atomic_t open_count;
diff --git a/drivers/media/platform/imx8/mxc-media-dev.c b/drivers/media/platform/imx8/mxc-media-dev.c
index 52d3804b927b..f7c10b05a949 100644
--- a/drivers/media/platform/imx8/mxc-media-dev.c
+++ b/drivers/media/platform/imx8/mxc-media-dev.c
@@ -45,6 +45,7 @@ static int mxc_md_create_links(struct mxc_md *mxc_md)
 	struct mxc_sensor_info *sensor;
 	struct mxc_mipi_csi2_dev *mipi_csi2;
 	struct mxc_parallel_csi_dev *pcsidev;
+	int num_sensors = mxc_md->subdev_notifier.num_subdevs;
 	int i, j, ret = 0;
 	u16  source_pad, sink_pad;
 	u32 flags;
@@ -201,7 +202,7 @@ static int mxc_md_create_links(struct mxc_md *mxc_md)
 	}
 
 	/* Connect MIPI Sensor to MIPI CSI2 */
-	for (i = 0; i < mxc_md->num_sensors; i++) {
+	for (i = 0; i < num_sensors; i++) {
 		sensor = &mxc_md->sensor[i];
 		if (sensor == NULL || sensor->sd == NULL)
 			continue;
@@ -318,6 +319,8 @@ static int subdev_notifier_complete(struct v4l2_async_notifier *notifier)
 	if (ret < 0)
 		goto unlock;
 
+	mxc_md->link_status = 1;
+
 	ret = v4l2_device_register_subdev_nodes(&mxc_md->v4l2_dev);
 unlock:
 	mutex_unlock(&mxc_md->media_dev.graph_mutex);
@@ -581,6 +584,7 @@ static void mxc_md_prepare_for_m2m(struct mxc_md *mxc_md,
 				if (pdev && pdev->dev.driver) {
 					device_lock(&pdev->dev);
 					mxc_md->mxc_isi[0] = dev_get_drvdata(&pdev->dev);
+					mxc_md->mxc_isi[0]->skip_m2m = 0;
 					device_unlock(&pdev->dev);
 				}
 				put_device(&pdev->dev);
@@ -617,6 +621,129 @@ static void mxc_md_unregister_entities(struct mxc_md *mxc_md)
 	v4l2_info(&mxc_md->v4l2_dev, "Unregistered all entities\n");
 }
 
+static int mxc_md_do_clean(struct mxc_md *mxc_md, struct media_pad *pad)
+{
+	struct device *dev = &mxc_md->pdev->dev;
+	struct media_pad *remote_pad;
+	struct v4l2_subdev	*subdev;
+	struct mxc_isi_dev *mxc_isi;
+
+	remote_pad = media_entity_remote_pad(pad);
+	if (remote_pad == NULL) {
+		dev_err(dev, "%s get remote pad fail\n", __func__);
+		return -ENODEV;
+	}
+
+	subdev = media_entity_to_v4l2_subdev(remote_pad->entity);
+	if (subdev == NULL) {
+		dev_err(dev, "%s media entity to v4l2 subdev fail\n", __func__);
+		return -ENODEV;
+	}
+
+	mxc_isi = v4l2_get_subdevdata(subdev);
+	if (mxc_isi == NULL) {
+		dev_err(dev, "%s Can't get subdev %s data\n", __func__, subdev->name);
+		return -ENODEV;
+	}
+
+	if (mxc_isi->id == 0)
+		mxc_isi->skip_m2m = 1;
+
+	v4l2_device_unregister_subdev(subdev);
+	media_entity_cleanup(&subdev->entity);
+
+	dev_info(dev, "clean ISI channel[%d]\n", mxc_isi->id);
+
+	return 0;
+}
+
+static int mxc_md_clean_channel(struct mxc_md *mxc_md, int index)
+{
+	struct mxc_sensor_info *sensor = &mxc_md->sensor[index];
+	struct mxc_mipi_csi2_dev *mipi_csi2;
+	struct mxc_parallel_csi_dev *pcsidev;
+	struct media_pad *local_pad;
+	struct media_entity *local_en;
+	u32 i, ret, mipi_vc = 0;
+
+	if (mxc_md->mipi_csi2[index]) {
+		mipi_csi2 = mxc_md->mipi_csi2[index];
+
+		if (mipi_csi2->vchannel == true)
+			mipi_vc = 4;
+		else
+			mipi_vc = 1;
+
+		local_en = &mipi_csi2->sd.entity;
+		if (local_en == NULL)
+			return -ENODEV;
+
+		for (i = 0; i < mipi_vc; i++) {
+			local_pad = &local_en->pads[MXC_MIPI_CSI2_VC0_PAD_SOURCE + i];
+			ret = mxc_md_do_clean(mxc_md, local_pad);
+			if (ret < 0)
+				return -ENODEV;
+		}
+	} else if (mxc_md->parallel_csi && !sensor->mipi_mode) {
+		pcsidev = mxc_md->pcsidev;
+		if (pcsidev == NULL)
+			return -ENODEV;
+
+		local_en = &pcsidev->sd.entity;
+		if (local_en == NULL)
+			return -ENODEV;
+
+		local_pad = &local_en->pads[MXC_PARALLEL_CSI_PAD_SOURCE];
+		ret = mxc_md_do_clean(mxc_md, local_pad);
+		if (ret < 0)
+			return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int mxc_md_clean_unlink_channels(struct mxc_md *mxc_md)
+{
+	struct mxc_sensor_info *sensor;
+	int num_subdevs = mxc_md->subdev_notifier.num_subdevs;
+	int i, ret;
+
+	for (i = 0; i < num_subdevs; i++) {
+		sensor = &mxc_md->sensor[i];
+		if (sensor->sd != NULL)
+			continue;
+
+		ret = mxc_md_clean_channel(mxc_md, i);
+		if (ret < 0) {
+			pr_err("%s: clean channel fail(%d)\n", __func__, i);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static void mxc_md_unregister_all(struct mxc_md *mxc_md)
+{
+	struct mxc_isi_dev *mxc_isi;
+	int i;
+
+	for (i = 0; i < MXC_ISI_MAX_DEVS; i++) {
+		mxc_isi = mxc_md->mxc_isi[i];
+		if (!mxc_isi)
+			continue;
+
+		if (mxc_isi->id == 0)
+			mxc_isi->skip_m2m = 1;
+
+		v4l2_device_unregister_subdev(&mxc_isi->isi_cap.sd);
+		media_entity_cleanup(&mxc_isi->isi_cap.sd.entity);
+
+		dev_info(&mxc_isi->pdev->dev, "%s unregister ISI channel[%d]\n",
+					__func__, mxc_isi->id);
+	}
+}
+
 static int mxc_md_link_notify(struct media_link *link, unsigned int flags,
 				unsigned int notification)
 {
@@ -681,6 +808,7 @@ static int mxc_md_probe(struct platform_device *pdev)
 		mxc_md->subdev_notifier.bound = subdev_notifier_bound;
 		mxc_md->subdev_notifier.complete = subdev_notifier_complete;
 		mxc_md->num_sensors = 0;
+		mxc_md->link_status = 0;
 
 		ret = v4l2_async_notifier_register(&mxc_md->v4l2_dev,
 						&mxc_md->subdev_notifier);
@@ -688,6 +816,19 @@ static int mxc_md_probe(struct platform_device *pdev)
 			dev_warn(&mxc_md->pdev->dev, "Sensor register failed\n");
 			goto err_m_ent;
 		}
+
+		if (!mxc_md->link_status) {
+			if (mxc_md->num_sensors > 0) {
+				ret = subdev_notifier_complete(&mxc_md->subdev_notifier);
+				if (ret < 0)
+					goto err_m_ent;
+
+				mxc_md_clean_unlink_channels(mxc_md);
+			} else {
+				/* no sensors connected */
+				mxc_md_unregister_all(mxc_md);
+			}
+		}
 	}
 
 	return 0;
diff --git a/drivers/media/platform/imx8/mxc-media-dev.h b/drivers/media/platform/imx8/mxc-media-dev.h
index 128d41b5192b..d5ee92317519 100644
--- a/drivers/media/platform/imx8/mxc-media-dev.h
+++ b/drivers/media/platform/imx8/mxc-media-dev.h
@@ -96,6 +96,7 @@ struct mxc_md {
 	struct mxc_mjpeg_dec  *mjpeg_dec;
 	struct mxc_mjpeg_enc  *mjpeg_enc;
 
+	int link_status;
 	int num_sensors;
 	unsigned int nr_isi;
 	bool parallel_csi;