From 9dc4e48fbea5412127ce2eb30d688c4fc55f5565 Mon Sep 17 00:00:00 2001
From: Guennadi Liakhovetski <g.liakhovetski@pengutronix.de>
Date: Tue, 22 Apr 2008 14:45:32 -0300
Subject: [PATCH] V4L/DVB (7276): soc-camera: deactivate cameras when not used

Only attach cameras to the host interface for probing, then detach until
open. This allows platforms with several cameras on an interface,
physically supporting only one camera, to handle multiple cameras and
activate them selectively after initial probing. The first attach during
probe is needed to activate the host interface to be able to physically
communicate with cameras.

Signed-off-by: Guennadi Liakhovetski <g.liakhovetski@pengutronix.de>
Signed-off-by: Mauro Carvalho Chehab <mchehab@infradead.org>
---
 drivers/media/video/soc_camera.c | 52 ++++++++++++++++++++++++--------
 include/media/soc_camera.h       |  2 ++
 2 files changed, 41 insertions(+), 13 deletions(-)

diff --git a/drivers/media/video/soc_camera.c b/drivers/media/video/soc_camera.c
index c947525c3f..322f837bcf 100644
--- a/drivers/media/video/soc_camera.c
+++ b/drivers/media/video/soc_camera.c
@@ -179,11 +179,9 @@ static int soc_camera_dqbuf(struct file *file, void *priv,
 
 static int soc_camera_open(struct inode *inode, struct file *file)
 {
-	struct video_device *vdev = video_devdata(file);
-	struct soc_camera_device *icd = container_of(vdev->dev,
-					     struct soc_camera_device, dev);
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
+	struct video_device *vdev;
+	struct soc_camera_device *icd;
+	struct soc_camera_host *ici;
 	struct soc_camera_file *icf;
 	int ret;
 
@@ -191,7 +189,12 @@ static int soc_camera_open(struct inode *inode, struct file *file)
 	if (!icf)
 		return -ENOMEM;
 
-	icf->icd = icd;
+	/* Protect against icd->remove() until we module_get() both drivers. */
+	mutex_lock(&video_lock);
+
+	vdev = video_devdata(file);
+	icd = container_of(vdev->dev, struct soc_camera_device, dev);
+	ici = to_soc_camera_host(icd->dev.parent);
 
 	if (!try_module_get(icd->ops->owner)) {
 		dev_err(&icd->dev, "Couldn't lock sensor driver.\n");
@@ -205,6 +208,22 @@ static int soc_camera_open(struct inode *inode, struct file *file)
 		goto emgi;
 	}
 
+	icd->use_count++;
+
+	icf->icd = icd;
+
+	/* Now we really have to activate the camera */
+	if (icd->use_count == 1) {
+		ret = ici->add(icd);
+		if (ret < 0) {
+			dev_err(&icd->dev, "Couldn't activate the camera: %d\n", ret);
+			icd->use_count--;
+			goto eiciadd;
+		}
+	}
+
+	mutex_unlock(&video_lock);
+
 	file->private_data = icf;
 	dev_dbg(&icd->dev, "camera device open\n");
 
@@ -216,9 +235,13 @@ static int soc_camera_open(struct inode *inode, struct file *file)
 
 	return 0;
 
+	/* All errors are entered with the video_lock held */
+eiciadd:
+	module_put(ici->owner);
 emgi:
 	module_put(icd->ops->owner);
 emgd:
+	mutex_unlock(&video_lock);
 	vfree(icf);
 	return ret;
 }
@@ -230,8 +253,14 @@ static int soc_camera_close(struct inode *inode, struct file *file)
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct video_device *vdev = icd->vdev;
 
+	mutex_lock(&video_lock);
+	icd->use_count--;
+	if (!icd->use_count)
+		ici->remove(icd);
 	module_put(icd->ops->owner);
 	module_put(ici->owner);
+	mutex_unlock(&video_lock);
+
 	vfree(file->private_data);
 
 	dev_dbg(vdev->dev, "camera device close\n");
@@ -673,14 +702,14 @@ static int soc_camera_probe(struct device *dev)
 	if (!icd->probe)
 		return -ENODEV;
 
+	/* We only call ->add() here to activate and probe the camera.
+	 * We shall ->remove() and deactivate it immediately afterwards. */
 	ret = ici->add(icd);
 	if (ret < 0)
 		return ret;
 
 	ret = icd->probe(icd);
-	if (ret < 0)
-		ici->remove(icd);
-	else {
+	if (ret >= 0) {
 		const struct v4l2_queryctrl *qctrl;
 
 		qctrl = soc_camera_find_qctrl(icd->ops, V4L2_CID_GAIN);
@@ -689,6 +718,7 @@ static int soc_camera_probe(struct device *dev)
 		icd->exposure = qctrl ? qctrl->default_value :
 			(unsigned short)~0;
 	}
+	ici->remove(icd);
 
 	return ret;
 }
@@ -698,14 +728,10 @@ static int soc_camera_probe(struct device *dev)
 static int soc_camera_remove(struct device *dev)
 {
 	struct soc_camera_device *icd = to_soc_camera_dev(dev);
-	struct soc_camera_host *ici =
-		to_soc_camera_host(icd->dev.parent);
 
 	if (icd->remove)
 		icd->remove(icd);
 
-	ici->remove(icd);
-
 	return 0;
 }
 
diff --git a/include/media/soc_camera.h b/include/media/soc_camera.h
index 69aba7188e..c886b1e648 100644
--- a/include/media/soc_camera.h
+++ b/include/media/soc_camera.h
@@ -41,6 +41,8 @@ struct soc_camera_device {
 	int (*probe)(struct soc_camera_device *icd);
 	void (*remove)(struct soc_camera_device *icd);
 	struct module *owner;
+	/* soc_camera.c private count. Only accessed with video_lock held */
+	int use_count;
 };
 
 struct soc_camera_file {
-- 
2.39.5