[media] V4L: sh_mobile_csi2: switch away from using the soc-camera bus notifier

This moves us one more step closer to eliminating the soc-camera bus
and devices on it. Besides, as a side effect, CSI-2 runtime PM on
sh-mobile secomes finer grained now: we only have to power on the
interface, when the device nodes are open.

Signed-off-by: Guennadi Liakhovetski <g.liakhovetski@gmx.de>
Acked-by: Paul Mundt <lethal@linux-sh.org>
Signed-off-by: Mauro Carvalho Chehab <mchehab@redhat.com>

2 files changed, 162 insertions, 90 deletions

diff --git a/drivers/media/video/sh_mobile_ceu_camera.c b/drivers/media/video/sh_mobile_ceu_camera.c
index 33e126e43b1f..152a65993286 100644
--- a/drivers/media/video/sh_mobile_ceu_camera.c
+++ b/drivers/media/video/sh_mobile_ceu_camera.c
@@ -38,6 +38,7 @@
 #include <media/v4l2-dev.h>
 #include <media/soc_camera.h>
 #include <media/sh_mobile_ceu.h>
+#include <media/sh_mobile_csi2.h>
 #include <media/videobuf2-dma-contig.h>
 #include <media/v4l2-mediabus.h>
 #include <media/soc_mediabus.h>
@@ -95,6 +96,7 @@ struct sh_mobile_ceu_buffer {
 struct sh_mobile_ceu_dev {
         struct soc_camera_host ici;
         struct soc_camera_device *icd;
+        struct platform_device *csi2_pdev;
 
         unsigned int irq;
         void __iomem *base;
@@ -498,11 +500,26 @@ out:
         return IRQ_HANDLED;
 }
 
+static struct v4l2_subdev *find_csi2(struct sh_mobile_ceu_dev *pcdev)
+{
+        struct v4l2_subdev *sd;
+
+        if (!pcdev->csi2_pdev)
+                return NULL;
+
+        v4l2_device_for_each_subdev(sd, &pcdev->ici.v4l2_dev)
+                if (&pcdev->csi2_pdev->dev == v4l2_get_subdevdata(sd))
+                        return sd;
+
+        return NULL;
+}
+
 /* Called with .video_lock held */
 static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
 {
         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
+        struct v4l2_subdev *csi2_sd;
         int ret;
 
         if (pcdev->icd)
@@ -515,8 +532,16 @@ static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
         pm_runtime_get_sync(ici->v4l2_dev.dev);
 
         ret = sh_mobile_ceu_soft_reset(pcdev);
-        if (!ret)
+
+        csi2_sd = find_csi2(pcdev);
+
+        ret = v4l2_subdev_call(csi2_sd, core, s_power, 1);
+        if (ret != -ENODEV && ret != -ENOIOCTLCMD && ret < 0) {
+                pm_runtime_put_sync(ici->v4l2_dev.dev);
+        } else {
                 pcdev->icd = icd;
+                ret = 0;
+        }
 
         return ret;
 }
@@ -526,9 +551,11 @@ static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
 {
         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
+        struct v4l2_subdev *csi2_sd = find_csi2(pcdev);
 
         BUG_ON(icd != pcdev->icd);
 
+        v4l2_subdev_call(csi2_sd, core, s_power, 0);
         /* disable capture, disable interrupts */
         ceu_write(pcdev, CEIER, 0);
         sh_mobile_ceu_soft_reset(pcdev);
@@ -640,7 +667,7 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
         }
 
         /* CSI2 special configuration */
-        if (pcdev->pdata->csi2_dev) {
+        if (pcdev->pdata->csi2) {
                 in_width = ((in_width - 2) * 2);
                 left_offset *= 2;
         }
@@ -782,7 +809,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
         value |= pcdev->is_16bit ? 1 << 12 : 0;
 
         /* CSI2 mode */
-        if (pcdev->pdata->csi2_dev)
+        if (pcdev->pdata->csi2)
                 value |= 3 << 12;
 
         ceu_write(pcdev, CAMCR, value);
@@ -920,7 +947,7 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
                 return 0;
         }
 
-        if (!pcdev->pdata->csi2_dev) {
+        if (!pcdev->pdata->csi2) {
                 ret = sh_mobile_ceu_try_bus_param(icd, fmt->bits_per_sample);
                 if (ret < 0)
                         return 0;
@@ -1943,7 +1970,7 @@ static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
                 .completion = COMPLETION_INITIALIZER_ONSTACK(wait.completion),
                 .notifier.notifier_call = bus_notify,
         };
-        struct device *csi2;
+        struct sh_mobile_ceu_companion *csi2;
 
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         irq = platform_get_irq(pdev, 0);
@@ -2016,26 +2043,61 @@ static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
         pcdev->ici.drv_name = dev_name(&pdev->dev);
         pcdev->ici.ops = &sh_mobile_ceu_host_ops;
 
+        pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+        if (IS_ERR(pcdev->alloc_ctx)) {
+                err = PTR_ERR(pcdev->alloc_ctx);
+                goto exit_free_clk;
+        }
+
+        err = soc_camera_host_register(&pcdev->ici);
+        if (err)
+                goto exit_free_ctx;
+
         /* CSI2 interfacing */
-        csi2 = pcdev->pdata->csi2_dev;
+        csi2 = pcdev->pdata->csi2;
         if (csi2) {
-                wait.dev = csi2;
+                struct platform_device *csi2_pdev =
+                        platform_device_alloc("sh-mobile-csi2", csi2->id);
+                struct sh_csi2_pdata *csi2_pdata = csi2->platform_data;
+
+                if (!csi2_pdev) {
+                        err = -ENOMEM;
+                        goto exit_host_unregister;
+                }
+
+                pcdev->csi2_pdev                = csi2_pdev;
+
+                err = platform_device_add_data(csi2_pdev, csi2_pdata, sizeof(*csi2_pdata));
+                if (err < 0)
+                        goto exit_pdev_put;
+
+                csi2_pdata                      = csi2_pdev->dev.platform_data;
+                csi2_pdata->v4l2_dev            = &pcdev->ici.v4l2_dev;
+
+                csi2_pdev->resource             = csi2->resource;
+                csi2_pdev->num_resources        = csi2->num_resources;
+
+                err = platform_device_add(csi2_pdev);
+                if (err < 0)
+                        goto exit_pdev_put;
+
+                wait.dev = &csi2_pdev->dev;
 
                 err = bus_register_notifier(&platform_bus_type, &wait.notifier);
                 if (err < 0)
-                        goto exit_free_clk;
+                        goto exit_pdev_unregister;
 
                 /*
                  * From this point the driver module will not unload, until
                  * we complete the completion.
                  */
 
-                if (!csi2->driver) {
+                if (!csi2_pdev->dev.driver) {
                         complete(&wait.completion);
                         /* Either too late, or probing failed */
                         bus_unregister_notifier(&platform_bus_type, &wait.notifier);
                         err = -ENXIO;
-                        goto exit_free_clk;
+                        goto exit_pdev_unregister;
                 }
 
                 /*
@@ -2044,34 +2106,28 @@ static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
                  * the "owner" is safe!
                  */
 
-                err = try_module_get(csi2->driver->owner);
+                err = try_module_get(csi2_pdev->dev.driver->owner);
 
                 /* Let notifier complete, if it has been locked */
                 complete(&wait.completion);
                 bus_unregister_notifier(&platform_bus_type, &wait.notifier);
                 if (!err) {
                         err = -ENODEV;
-                        goto exit_free_clk;
+                        goto exit_pdev_unregister;
                 }
         }
 
-        pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
-        if (IS_ERR(pcdev->alloc_ctx)) {
-                err = PTR_ERR(pcdev->alloc_ctx);
-                goto exit_module_put;
-        }
-
-        err = soc_camera_host_register(&pcdev->ici);
-        if (err)
-                goto exit_free_ctx;
-
         return 0;
 
+exit_pdev_unregister:
+        platform_device_del(pcdev->csi2_pdev);
+exit_pdev_put:
+        pcdev->csi2_pdev->resource = NULL;
+        platform_device_put(pcdev->csi2_pdev);
+exit_host_unregister:
+        soc_camera_host_unregister(&pcdev->ici);
 exit_free_ctx:
         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
-exit_module_put:
-        if (csi2 && csi2->driver)
-                module_put(csi2->driver->owner);
 exit_free_clk:
         pm_runtime_disable(&pdev->dev);
         free_irq(pcdev->irq, pcdev);
@@ -2091,7 +2147,7 @@ static int __devexit sh_mobile_ceu_remove(struct platform_device *pdev)
         struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
         struct sh_mobile_ceu_dev *pcdev = container_of(soc_host,
                                         struct sh_mobile_ceu_dev, ici);
-        struct device *csi2 = pcdev->pdata->csi2_dev;
+        struct platform_device *csi2_pdev = pcdev->csi2_pdev;
 
         soc_camera_host_unregister(soc_host);
         pm_runtime_disable(&pdev->dev);
@@ -2100,8 +2156,13 @@ static int __devexit sh_mobile_ceu_remove(struct platform_device *pdev)
                 dma_release_declared_memory(&pdev->dev);
         iounmap(pcdev->base);
         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
-        if (csi2 && csi2->driver)
-                module_put(csi2->driver->owner);
+        if (csi2_pdev && csi2_pdev->dev.driver) {
+                struct module *csi2_drv = csi2_pdev->dev.driver->owner;
+                platform_device_del(csi2_pdev);
+                csi2_pdev->resource = NULL;
+                platform_device_put(csi2_pdev);
+                module_put(csi2_drv);
+        }
         kfree(pcdev);
 
         return 0;
diff --git a/drivers/media/video/sh_mobile_csi2.c b/drivers/media/video/sh_mobile_csi2.c
index 98b87481fa94..2893a0134c7e 100644
--- a/drivers/media/video/sh_mobile_csi2.c
+++ b/drivers/media/video/sh_mobile_csi2.c
@@ -16,6 +16,7 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 
+#include <media/sh_mobile_ceu.h>
 #include <media/sh_mobile_csi2.h>
 #include <media/soc_camera.h>
 #include <media/v4l2-common.h>
@@ -33,7 +34,6 @@
 struct sh_csi2 {
         struct v4l2_subdev              subdev;
         struct list_head                list;
-        struct notifier_block           notifier;
         unsigned int                    irq;
         void __iomem                    *base;
         struct platform_device          *pdev;
@@ -132,13 +132,6 @@ static struct v4l2_subdev_video_ops sh_csi2_subdev_video_ops = {
         .try_mbus_fmt   = sh_csi2_try_fmt,
 };
 
-static struct v4l2_subdev_core_ops sh_csi2_subdev_core_ops;
-
-static struct v4l2_subdev_ops sh_csi2_subdev_ops = {
-        .core   = &sh_csi2_subdev_core_ops,
-        .video  = &sh_csi2_subdev_video_ops,
-};
-
 static void sh_csi2_hwinit(struct sh_csi2 *priv)
 {
         struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
@@ -186,65 +179,84 @@ static unsigned long sh_csi2_query_bus_param(struct soc_camera_device *icd)
         return soc_camera_apply_sensor_flags(icl, flags);
 }
 
-static int sh_csi2_notify(struct notifier_block *nb,
-                          unsigned long action, void *data)
+static int sh_csi2_client_connect(struct sh_csi2 *priv)
 {
-        struct device *dev = data;
-        struct soc_camera_device *icd = to_soc_camera_dev(dev);
-        struct v4l2_device *v4l2_dev = dev_get_drvdata(dev->parent);
-        struct sh_csi2 *priv =
-                container_of(nb, struct sh_csi2, notifier);
         struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
-        int ret, i;
+        struct v4l2_subdev *sd, *csi2_sd = &priv->subdev;
+        struct soc_camera_device *icd = NULL;
+        struct device *dev = v4l2_get_subdevdata(&priv->subdev);
+        int i;
+
+        v4l2_device_for_each_subdev(sd, csi2_sd->v4l2_dev)
+                if (sd->grp_id) {
+                        icd = (struct soc_camera_device *)sd->grp_id;
+                        break;
+                }
+
+        if (!icd)
+                return -EINVAL;
 
         for (i = 0; i < pdata->num_clients; i++)
                 if (&pdata->clients[i].pdev->dev == icd->pdev)
                         break;
 
-        dev_dbg(dev, "%s(%p): action = %lu, found #%d\n", __func__, dev, action, i);
+        dev_dbg(dev, "%s(%p): found #%d\n", __func__, dev, i);
 
         if (i == pdata->num_clients)
-                return NOTIFY_DONE;
+                return -ENODEV;
 
-        switch (action) {
-        case BUS_NOTIFY_BOUND_DRIVER:
-                snprintf(priv->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s%s",
-                         dev_name(v4l2_dev->dev), ".mipi-csi");
-                priv->subdev.grp_id = (long)icd;
-                ret = v4l2_device_register_subdev(v4l2_dev, &priv->subdev);
-                dev_dbg(dev, "%s(%p): ret(register_subdev) = %d\n", __func__, priv, ret);
-                if (ret < 0)
-                        return NOTIFY_DONE;
+        priv->client = pdata->clients + i;
 
-                priv->client = pdata->clients + i;
+        priv->set_bus_param             = icd->ops->set_bus_param;
+        priv->query_bus_param           = icd->ops->query_bus_param;
+        icd->ops->set_bus_param         = sh_csi2_set_bus_param;
+        icd->ops->query_bus_param       = sh_csi2_query_bus_param;
 
-                priv->set_bus_param             = icd->ops->set_bus_param;
-                priv->query_bus_param           = icd->ops->query_bus_param;
-                icd->ops->set_bus_param         = sh_csi2_set_bus_param;
-                icd->ops->query_bus_param       = sh_csi2_query_bus_param;
+        csi2_sd->grp_id = (long)icd;
 
-                pm_runtime_get_sync(v4l2_get_subdevdata(&priv->subdev));
+        pm_runtime_get_sync(dev);
 
-                sh_csi2_hwinit(priv);
-                break;
-        case BUS_NOTIFY_UNBIND_DRIVER:
-                priv->client = NULL;
+        sh_csi2_hwinit(priv);
 
-                /* Driver is about to be unbound */
-                icd->ops->set_bus_param         = priv->set_bus_param;
-                icd->ops->query_bus_param       = priv->query_bus_param;
-                priv->set_bus_param             = NULL;
-                priv->query_bus_param           = NULL;
+        return 0;
+}
 
-                v4l2_device_unregister_subdev(&priv->subdev);
+static void sh_csi2_client_disconnect(struct sh_csi2 *priv)
+{
+        struct soc_camera_device *icd = (struct soc_camera_device *)priv->subdev.grp_id;
 
-                pm_runtime_put(v4l2_get_subdevdata(&priv->subdev));
-                break;
-        }
+        priv->client = NULL;
+        priv->subdev.grp_id = 0;
 
-        return NOTIFY_OK;
+        /* Driver is about to be unbound */
+        icd->ops->set_bus_param         = priv->set_bus_param;
+        icd->ops->query_bus_param       = priv->query_bus_param;
+        priv->set_bus_param             = NULL;
+        priv->query_bus_param           = NULL;
+
+        pm_runtime_put(v4l2_get_subdevdata(&priv->subdev));
 }
 
+static int sh_csi2_s_power(struct v4l2_subdev *sd, int on)
+{
+        struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
+
+        if (on)
+                return sh_csi2_client_connect(priv);
+
+        sh_csi2_client_disconnect(priv);
+        return 0;
+}
+
+static struct v4l2_subdev_core_ops sh_csi2_subdev_core_ops = {
+        .s_power        = sh_csi2_s_power,
+};
+
+static struct v4l2_subdev_ops sh_csi2_subdev_ops = {
+        .core   = &sh_csi2_subdev_core_ops,
+        .video  = &sh_csi2_subdev_video_ops,
+};
+
 static __devinit int sh_csi2_probe(struct platform_device *pdev)
 {
         struct resource *res;
@@ -274,14 +286,6 @@ static __devinit int sh_csi2_probe(struct platform_device *pdev)
                 return -ENOMEM;
 
         priv->irq = irq;
-        priv->notifier.notifier_call = sh_csi2_notify;
-
-        /* We MUST attach after the MIPI sensor */
-        ret = bus_register_notifier(&soc_camera_bus_type, &priv->notifier);
-        if (ret < 0) {
-                dev_err(&pdev->dev, "CSI2 cannot register notifier\n");
-                goto ernotify;
-        }
 
         if (!request_mem_region(res->start, resource_size(res), pdev->name)) {
                 dev_err(&pdev->dev, "CSI2 register region already claimed\n");
@@ -297,11 +301,17 @@ static __devinit int sh_csi2_probe(struct platform_device *pdev)
         }
 
         priv->pdev = pdev;
+        platform_set_drvdata(pdev, priv);
 
         v4l2_subdev_init(&priv->subdev, &sh_csi2_subdev_ops);
         v4l2_set_subdevdata(&priv->subdev, &pdev->dev);
 
-        platform_set_drvdata(pdev, priv);
+        snprintf(priv->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s.mipi-csi",
+                 dev_name(pdata->v4l2_dev->dev));
+        ret = v4l2_device_register_subdev(pdata->v4l2_dev, &priv->subdev);
+        dev_dbg(&pdev->dev, "%s(%p): ret(register_subdev) = %d\n", __func__, priv, ret);
+        if (ret < 0)
+                goto esdreg;
 
         pm_runtime_enable(&pdev->dev);
 
@@ -309,11 +319,11 @@ static __devinit int sh_csi2_probe(struct platform_device *pdev)
 
         return 0;
 
+esdreg:
+        iounmap(priv->base);
 eremap:
         release_mem_region(res->start, resource_size(res));
 ereqreg:
-        bus_unregister_notifier(&soc_camera_bus_type, &priv->notifier);
-ernotify:
         kfree(priv);
 
         return ret;
@@ -324,7 +334,7 @@ static __devexit int sh_csi2_remove(struct platform_device *pdev)
         struct sh_csi2 *priv = platform_get_drvdata(pdev);
         struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
-        bus_unregister_notifier(&soc_camera_bus_type, &priv->notifier);
+        v4l2_device_unregister_subdev(&priv->subdev);
         pm_runtime_disable(&pdev->dev);
         iounmap(priv->base);
         release_mem_region(res->start, resource_size(res));
@@ -335,8 +345,9 @@ static __devexit int sh_csi2_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver __refdata sh_csi2_pdrv = {
-        .remove  = __devexit_p(sh_csi2_remove),
-        .driver  = {
+        .remove = __devexit_p(sh_csi2_remove),
+        .probe  = sh_csi2_probe,
+        .driver = {
                 .name   = "sh-mobile-csi2",
                 .owner  = THIS_MODULE,
         },
@@ -344,7 +355,7 @@ static struct platform_driver __refdata sh_csi2_pdrv = {
 
 static int __init sh_csi2_init(void)
 {
-        return platform_driver_probe(&sh_csi2_pdrv, sh_csi2_probe);
+        return platform_driver_register(&sh_csi2_pdrv);
 }
 
 static void __exit sh_csi2_exit(void)