6 files changed, 138 insertions, 131 deletions

diff --git a/drivers/media/video/s5p-fimc/fimc-capture.c b/drivers/media/video/s5p-fimc/fimc-capture.c
index 0051d8161c6c..b45da2780213 100644
--- a/drivers/media/video/s5p-fimc/fimc-capture.c
+++ b/drivers/media/video/s5p-fimc/fimc-capture.c
@@ -993,7 +993,7 @@ static int fimc_pipeline_validate(struct fimc_dev *fimc)
                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
                         break;
                 /* Don't call FIMC subdev operation to avoid nested locking */
-                if (sd == fimc->vid_cap.subdev) {
+                if (sd == &fimc->vid_cap.subdev) {
                         struct fimc_frame *ff = &vid_cap->ctx->s_frame;
                         sink_fmt.format.width = ff->f_width;
                         sink_fmt.format.height = ff->f_height;
@@ -1489,53 +1489,6 @@ static struct v4l2_subdev_ops fimc_subdev_ops = {
         .pad = &fimc_subdev_pad_ops,
 };
 
-static int fimc_create_capture_subdev(struct fimc_dev *fimc,
-                                      struct v4l2_device *v4l2_dev)
-{
-        struct v4l2_subdev *sd;
-        int ret;
-
-        sd = kzalloc(sizeof(*sd), GFP_KERNEL);
-        if (!sd)
-                return -ENOMEM;
-
-        v4l2_subdev_init(sd, &fimc_subdev_ops);
-        sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
-        snprintf(sd->name, sizeof(sd->name), "FIMC.%d", fimc->pdev->id);
-
-        fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
-        fimc->vid_cap.sd_pads[FIMC_SD_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
-        ret = media_entity_init(&sd->entity, FIMC_SD_PADS_NUM,
-                                fimc->vid_cap.sd_pads, 0);
-        if (ret)
-                goto me_err;
-        ret = v4l2_device_register_subdev(v4l2_dev, sd);
-        if (ret)
-                goto sd_err;
-
-        fimc->vid_cap.subdev = sd;
-        v4l2_set_subdevdata(sd, fimc);
-        sd->entity.ops = &fimc_sd_media_ops;
-        return 0;
-sd_err:
-        media_entity_cleanup(&sd->entity);
-me_err:
-        kfree(sd);
-        return ret;
-}
-
-static void fimc_destroy_capture_subdev(struct fimc_dev *fimc)
-{
-        struct v4l2_subdev *sd = fimc->vid_cap.subdev;
-
-        if (!sd)
-                return;
-        media_entity_cleanup(&sd->entity);
-        v4l2_device_unregister_subdev(sd);
-        kfree(sd);
-        fimc->vid_cap.subdev = NULL;
-}
-
 /* Set default format at the sensor and host interface */
 static int fimc_capture_set_default_format(struct fimc_dev *fimc)
 {
@@ -1554,7 +1507,7 @@ static int fimc_capture_set_default_format(struct fimc_dev *fimc)
 }
 
 /* fimc->lock must be already initialized */
-int fimc_register_capture_device(struct fimc_dev *fimc,
+static int fimc_register_capture_device(struct fimc_dev *fimc,
                                  struct v4l2_device *v4l2_dev)
 {
         struct video_device *vfd;
@@ -1572,7 +1525,7 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
         ctx->out_path    = FIMC_DMA;
         ctx->state       = FIMC_CTX_CAP;
         ctx->s_frame.fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_CAM, 0);
-        ctx->d_frame.fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_CAM, 0);
+        ctx->d_frame.fmt = ctx->s_frame.fmt;
 
         vfd = video_device_alloc();
         if (!vfd) {
@@ -1580,8 +1533,7 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
                 goto err_vd_alloc;
         }
 
-        snprintf(vfd->name, sizeof(vfd->name), "%s.capture",
-                 dev_name(&fimc->pdev->dev));
+        snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.capture", fimc->id);
 
         vfd->fops       = &fimc_capture_fops;
         vfd->ioctl_ops  = &fimc_capture_ioctl_ops;
@@ -1616,18 +1568,22 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 
         vb2_queue_init(q);
 
-        fimc->vid_cap.vd_pad.flags = MEDIA_PAD_FL_SINK;
-        ret = media_entity_init(&vfd->entity, 1, &fimc->vid_cap.vd_pad, 0);
+        vid_cap->vd_pad.flags = MEDIA_PAD_FL_SINK;
+        ret = media_entity_init(&vfd->entity, 1, &vid_cap->vd_pad, 0);
         if (ret)
                 goto err_ent;
-        ret = fimc_create_capture_subdev(fimc, v4l2_dev);
+
+        ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
         if (ret)
-                goto err_sd_reg;
+                goto err_vd;
+
+        v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
+                  vfd->name, video_device_node_name(vfd));
 
         vfd->ctrl_handler = &ctx->ctrl_handler;
         return 0;
 
-err_sd_reg:
+err_vd:
         media_entity_cleanup(&vfd->entity);
 err_ent:
         video_device_release(vfd);
@@ -1636,17 +1592,73 @@ err_vd_alloc:
         return ret;
 }
 
-void fimc_unregister_capture_device(struct fimc_dev *fimc)
+static int fimc_capture_subdev_registered(struct v4l2_subdev *sd)
 {
-        struct video_device *vfd = fimc->vid_cap.vfd;
+        struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+        int ret;
 
-        if (vfd) {
-                media_entity_cleanup(&vfd->entity);
-                /* Can also be called if video device was
-                   not registered */
-                video_unregister_device(vfd);
+        ret = fimc_register_m2m_device(fimc, sd->v4l2_dev);
+        if (ret)
+                return ret;
+
+        ret = fimc_register_capture_device(fimc, sd->v4l2_dev);
+        if (ret)
+                fimc_unregister_m2m_device(fimc);
+
+        return ret;
+}
+
+static void fimc_capture_subdev_unregistered(struct v4l2_subdev *sd)
+{
+        struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+
+        if (fimc == NULL)
+                return;
+
+        fimc_unregister_m2m_device(fimc);
+
+        if (fimc->vid_cap.vfd) {
+                media_entity_cleanup(&fimc->vid_cap.vfd->entity);
+                video_unregister_device(fimc->vid_cap.vfd);
+                fimc->vid_cap.vfd = NULL;
         }
-        fimc_destroy_capture_subdev(fimc);
+
         kfree(fimc->vid_cap.ctx);
         fimc->vid_cap.ctx = NULL;
 }
+
+static const struct v4l2_subdev_internal_ops fimc_capture_sd_internal_ops = {
+        .registered = fimc_capture_subdev_registered,
+        .unregistered = fimc_capture_subdev_unregistered,
+};
+
+int fimc_initialize_capture_subdev(struct fimc_dev *fimc)
+{
+        struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
+        int ret;
+
+        v4l2_subdev_init(sd, &fimc_subdev_ops);
+        sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+        snprintf(sd->name, sizeof(sd->name), "FIMC.%d", fimc->pdev->id);
+
+        fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+        fimc->vid_cap.sd_pads[FIMC_SD_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+        ret = media_entity_init(&sd->entity, FIMC_SD_PADS_NUM,
+                                fimc->vid_cap.sd_pads, 0);
+        if (ret)
+                return ret;
+
+        sd->entity.ops = &fimc_sd_media_ops;
+        sd->internal_ops = &fimc_capture_sd_internal_ops;
+        v4l2_set_subdevdata(sd, fimc);
+        return 0;
+}
+
+void fimc_unregister_capture_subdev(struct fimc_dev *fimc)
+{
+        struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
+
+        v4l2_device_unregister_subdev(sd);
+        media_entity_cleanup(&sd->entity);
+        v4l2_set_subdevdata(sd, NULL);
+}
diff --git a/drivers/media/video/s5p-fimc/fimc-core.c b/drivers/media/video/s5p-fimc/fimc-core.c
index 749db4deefcb..add24cd373a5 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.c
+++ b/drivers/media/video/s5p-fimc/fimc-core.c
@@ -842,8 +842,6 @@ static int fimc_probe(struct platform_device *pdev)
         clk_set_rate(fimc->clock[CLK_BUS], drv_data->lclk_frequency);
         clk_enable(fimc->clock[CLK_BUS]);
 
-        platform_set_drvdata(pdev, fimc);
-
         ret = devm_request_irq(&pdev->dev, res->start, fimc_irq_handler,
                                0, pdev->name, fimc);
         if (ret) {
@@ -851,10 +849,15 @@ static int fimc_probe(struct platform_device *pdev)
                 goto err_clk;
         }
 
+        ret = fimc_initialize_capture_subdev(fimc);
+        if (ret)
+                goto err_clk;
+
+        platform_set_drvdata(pdev, fimc);
         pm_runtime_enable(&pdev->dev);
         ret = pm_runtime_get_sync(&pdev->dev);
         if (ret < 0)
-                goto err_clk;
+                goto err_sd;
         /* Initialize contiguous memory allocator */
         fimc->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
         if (IS_ERR(fimc->alloc_ctx)) {
@@ -866,9 +869,10 @@ static int fimc_probe(struct platform_device *pdev)
 
         pm_runtime_put(&pdev->dev);
         return 0;
-
 err_pm:
         pm_runtime_put(&pdev->dev);
+err_sd:
+        fimc_unregister_capture_subdev(fimc);
 err_clk:
         fimc_clk_put(fimc);
         return ret;
@@ -953,6 +957,7 @@ static int __devexit fimc_remove(struct platform_device *pdev)
         pm_runtime_disable(&pdev->dev);
         pm_runtime_set_suspended(&pdev->dev);
 
+        fimc_unregister_capture_subdev(fimc);
         vb2_dma_contig_cleanup_ctx(fimc->alloc_ctx);
 
         clk_disable(fimc->clock[CLK_BUS]);
diff --git a/drivers/media/video/s5p-fimc/fimc-core.h b/drivers/media/video/s5p-fimc/fimc-core.h
index 288631a86e53..ef7c6a23ca2a 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.h
+++ b/drivers/media/video/s5p-fimc/fimc-core.h
@@ -331,7 +331,7 @@ struct fimc_vid_cap {
         struct fimc_ctx                 *ctx;
         struct vb2_alloc_ctx            *alloc_ctx;
         struct video_device             *vfd;
-        struct v4l2_subdev              *subdev;
+        struct v4l2_subdev              subdev;
         struct media_pad                vd_pad;
         struct v4l2_mbus_framefmt       mf;
         struct media_pad                sd_pads[FIMC_SD_PADS_NUM];
@@ -737,9 +737,8 @@ void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state);
 
 /* -----------------------------------------------------*/
 /* fimc-capture.c                                       */
-int fimc_register_capture_device(struct fimc_dev *fimc,
-                                 struct v4l2_device *v4l2_dev);
-void fimc_unregister_capture_device(struct fimc_dev *fimc);
+int fimc_initialize_capture_subdev(struct fimc_dev *fimc);
+void fimc_unregister_capture_subdev(struct fimc_dev *fimc);
 int fimc_capture_ctrls_create(struct fimc_dev *fimc);
 void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
                         void *arg);
diff --git a/drivers/media/video/s5p-fimc/fimc-m2m.c b/drivers/media/video/s5p-fimc/fimc-m2m.c
index 9935cc4bd0cb..92b6059b25bf 100644
--- a/drivers/media/video/s5p-fimc/fimc-m2m.c
+++ b/drivers/media/video/s5p-fimc/fimc-m2m.c
@@ -776,7 +776,7 @@ int fimc_register_m2m_device(struct fimc_dev *fimc,
            This driver needs auditing so that this flag can be removed. */
         set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 
-        snprintf(vfd->name, sizeof(vfd->name), "%s.m2m", dev_name(&pdev->dev));
+        snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.m2m", fimc->id);
         video_set_drvdata(vfd, fimc);
 
         fimc->m2m.vfd = vfd;
@@ -788,9 +788,20 @@ int fimc_register_m2m_device(struct fimc_dev *fimc,
         }
 
         ret = media_entity_init(&vfd->entity, 0, NULL, 0);
-        if (!ret)
-                return 0;
+        if (ret)
+                goto err_me;
+
+        ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
+        if (ret)
+                goto err_vd;
+
+        v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
+                  vfd->name, video_device_node_name(vfd));
+        return 0;
 
+err_vd:
+        media_entity_cleanup(&vfd->entity);
+err_me:
         v4l2_m2m_release(fimc->m2m.m2m_dev);
 err_init:
         video_device_release(fimc->m2m.vfd);
diff --git a/drivers/media/video/s5p-fimc/fimc-mdevice.c b/drivers/media/video/s5p-fimc/fimc-mdevice.c
index f97ac02b8677..c319842c762d 100644
--- a/drivers/media/video/s5p-fimc/fimc-mdevice.c
+++ b/drivers/media/video/s5p-fimc/fimc-mdevice.c
@@ -304,8 +304,9 @@ static int fimc_md_register_sensor_entities(struct fimc_md *fmd)
 static int fimc_register_callback(struct device *dev, void *p)
 {
         struct fimc_dev *fimc = dev_get_drvdata(dev);
+        struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
         struct fimc_md *fmd = p;
-        int ret;
+        int ret = 0;
 
         if (!fimc || !fimc->pdev)
                 return 0;
@@ -313,12 +314,14 @@ static int fimc_register_callback(struct device *dev, void *p)
                 return 0;
 
         fmd->fimc[fimc->pdev->id] = fimc;
-        ret = fimc_register_m2m_device(fimc, &fmd->v4l2_dev);
-        if (ret)
-                return ret;
-        ret = fimc_register_capture_device(fimc, &fmd->v4l2_dev);
-        if (!ret)
-                fimc->vid_cap.user_subdev_api = fmd->user_subdev_api;
+        sd->grp_id = FIMC_GROUP_ID;
+
+        ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+        if (ret) {
+                v4l2_err(&fmd->v4l2_dev, "Failed to register FIMC.%d (%d)\n",
+                         fimc->id, ret);
+        }
+
         return ret;
 }
 
@@ -401,8 +404,7 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
         for (i = 0; i < FIMC_MAX_DEVS; i++) {
                 if (fmd->fimc[i] == NULL)
                         continue;
-                fimc_unregister_m2m_device(fmd->fimc[i]);
-                fimc_unregister_capture_device(fmd->fimc[i]);
+                v4l2_device_unregister_subdev(&fmd->fimc[i]->vid_cap.subdev);
                 fmd->fimc[i] = NULL;
         }
         for (i = 0; i < CSIS_MAX_ENTITIES; i++) {
@@ -420,35 +422,6 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
         }
 }
 
-static int fimc_md_register_video_nodes(struct fimc_md *fmd)
-{
-        struct video_device *vdev;
-        int i, ret = 0;
-
-        for (i = 0; i < FIMC_MAX_DEVS && !ret; i++) {
-                if (!fmd->fimc[i])
-                        continue;
-
-                vdev = fmd->fimc[i]->m2m.vfd;
-                if (vdev) {
-                        ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
-                        if (ret)
-                                break;
-                        v4l2_info(&fmd->v4l2_dev, "Registered %s as /dev/%s\n",
-                                  vdev->name, video_device_node_name(vdev));
-                }
-
-                vdev = fmd->fimc[i]->vid_cap.vfd;
-                if (vdev == NULL)
-                        continue;
-                ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
-                v4l2_info(&fmd->v4l2_dev, "Registered %s as /dev/%s\n",
-                          vdev->name, video_device_node_name(vdev));
-        }
-
-        return ret;
-}
-
 /**
  * __fimc_md_create_fimc_links - create links to all FIMC entities
  * @fmd: fimc media device
@@ -479,7 +452,7 @@ static int __fimc_md_create_fimc_links(struct fimc_md *fmd,
                         continue;
 
                 flags = (i == fimc_id) ? MEDIA_LNK_FL_ENABLED : 0;
-                sink = &fmd->fimc[i]->vid_cap.subdev->entity;
+                sink = &fmd->fimc[i]->vid_cap.subdev.entity;
                 ret = media_entity_create_link(source, pad, sink,
                                               FIMC_SD_PAD_SINK, flags);
                 if (ret)
@@ -588,7 +561,7 @@ static int fimc_md_create_links(struct fimc_md *fmd)
         for (i = 0; i < FIMC_MAX_DEVS; i++) {
                 if (!fmd->fimc[i])
                         continue;
-                source = &fmd->fimc[i]->vid_cap.subdev->entity;
+                source = &fmd->fimc[i]->vid_cap.subdev.entity;
                 sink = &fmd->fimc[i]->vid_cap.vfd->entity;
                 ret = media_entity_create_link(source, FIMC_SD_PAD_SOURCE,
                                               sink, 0, flags);
@@ -817,42 +790,48 @@ static int fimc_md_probe(struct platform_device *pdev)
         ret = media_device_register(&fmd->media_dev);
         if (ret < 0) {
                 v4l2_err(v4l2_dev, "Failed to register media device: %d\n", ret);
-                goto err2;
+                goto err_md;
         }
         ret = fimc_md_get_clocks(fmd);
         if (ret)
-                goto err3;
+                goto err_clk;
 
         fmd->user_subdev_api = false;
+
+        /* Protect the media graph while we're registering entities */
+        mutex_lock(&fmd->media_dev.graph_mutex);
+
         ret = fimc_md_register_platform_entities(fmd);
         if (ret)
-                goto err3;
+                goto err_unlock;
 
         if (pdev->dev.platform_data) {
                 ret = fimc_md_register_sensor_entities(fmd);
                 if (ret)
-                        goto err3;
+                        goto err_unlock;
         }
         ret = fimc_md_create_links(fmd);
         if (ret)
-                goto err3;
+                goto err_unlock;
         ret = v4l2_device_register_subdev_nodes(&fmd->v4l2_dev);
         if (ret)
-                goto err3;
-        ret = fimc_md_register_video_nodes(fmd);
-        if (ret)
-                goto err3;
+                goto err_unlock;
 
         ret = device_create_file(&pdev->dev, &dev_attr_subdev_conf_mode);
-        if (!ret) {
-                platform_set_drvdata(pdev, fmd);
-                return 0;
-        }
-err3:
+        if (ret)
+                goto err_unlock;
+
+        platform_set_drvdata(pdev, fmd);
+        mutex_unlock(&fmd->media_dev.graph_mutex);
+        return 0;
+
+err_unlock:
+        mutex_unlock(&fmd->media_dev.graph_mutex);
+err_clk:
         media_device_unregister(&fmd->media_dev);
         fimc_md_put_clocks(fmd);
         fimc_md_unregister_entities(fmd);
-err2:
+err_md:
         v4l2_device_unregister(&fmd->v4l2_dev);
         return ret;
 }
diff --git a/drivers/media/video/s5p-fimc/fimc-mdevice.h b/drivers/media/video/s5p-fimc/fimc-mdevice.h
index da3780823e7d..4f3b69c682cb 100644
--- a/drivers/media/video/s5p-fimc/fimc-mdevice.h
+++ b/drivers/media/video/s5p-fimc/fimc-mdevice.h
@@ -24,6 +24,7 @@
 #define SENSOR_GROUP_ID         (1 << 8)
 #define CSIS_GROUP_ID           (1 << 9)
 #define WRITEBACK_GROUP_ID      (1 << 10)
+#define FIMC_GROUP_ID           (1 << 11)
 
 #define FIMC_MAX_SENSORS        8
 #define FIMC_MAX_CAMCLKS        2