1 files changed, 48 insertions, 43 deletions

diff --git a/drivers/media/video/omap3isp/ispcsi2.c b/drivers/media/video/omap3isp/ispcsi2.c
index 69161a682b3d..0c5f1cb9d99d 100644
--- a/drivers/media/video/omap3isp/ispcsi2.c
+++ b/drivers/media/video/omap3isp/ispcsi2.c
@@ -1187,6 +1187,37 @@ static const struct media_entity_operations csi2_media_ops = {
         .link_setup = csi2_link_setup,
 };
 
+void omap3isp_csi2_unregister_entities(struct isp_csi2_device *csi2)
+{
+        v4l2_device_unregister_subdev(&csi2->subdev);
+        omap3isp_video_unregister(&csi2->video_out);
+}
+
+int omap3isp_csi2_register_entities(struct isp_csi2_device *csi2,
+                                    struct v4l2_device *vdev)
+{
+        int ret;
+
+        /* Register the subdev and video nodes. */
+        ret = v4l2_device_register_subdev(vdev, &csi2->subdev);
+        if (ret < 0)
+                goto error;
+
+        ret = omap3isp_video_register(&csi2->video_out, vdev);
+        if (ret < 0)
+                goto error;
+
+        return 0;
+
+error:
+        omap3isp_csi2_unregister_entities(csi2);
+        return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP CSI2 initialisation and cleanup
+ */
+
 /*
  * csi2_init_entities - Initialize subdev and media entity.
  * @csi2: Pointer to csi2 structure.
@@ -1228,57 +1259,23 @@ static int csi2_init_entities(struct isp_csi2_device *csi2)
 
         ret = omap3isp_video_init(&csi2->video_out, "CSI2a");
         if (ret < 0)
-                return ret;
+                goto error_video;
 
         /* Connect the CSI2 subdev to the video node. */
         ret = media_entity_create_link(&csi2->subdev.entity, CSI2_PAD_SOURCE,
                                        &csi2->video_out.video.entity, 0, 0);
         if (ret < 0)
-                return ret;
+                goto error_link;
 
         return 0;
-}
 
-void omap3isp_csi2_unregister_entities(struct isp_csi2_device *csi2)
-{
+error_link:
+        omap3isp_video_cleanup(&csi2->video_out);
+error_video:
         media_entity_cleanup(&csi2->subdev.entity);
-
-        v4l2_device_unregister_subdev(&csi2->subdev);
-        omap3isp_video_unregister(&csi2->video_out);
-}
-
-int omap3isp_csi2_register_entities(struct isp_csi2_device *csi2,
-                                    struct v4l2_device *vdev)
-{
-        int ret;
-
-        /* Register the subdev and video nodes. */
-        ret = v4l2_device_register_subdev(vdev, &csi2->subdev);
-        if (ret < 0)
-                goto error;
-
-        ret = omap3isp_video_register(&csi2->video_out, vdev);
-        if (ret < 0)
-                goto error;
-
-        return 0;
-
-error:
-        omap3isp_csi2_unregister_entities(csi2);
         return ret;
 }
 
-/* -----------------------------------------------------------------------------
- * ISP CSI2 initialisation and cleanup
- */
-
-/*
- * omap3isp_csi2_cleanup - Routine for module driver cleanup
- */
-void omap3isp_csi2_cleanup(struct isp_device *isp)
-{
-}
-
 /*
  * omap3isp_csi2_init - Routine for module driver init
  */
@@ -1298,7 +1295,7 @@ int omap3isp_csi2_init(struct isp_device *isp)
 
         ret = csi2_init_entities(csi2a);
         if (ret < 0)
-                goto fail;
+                return ret;
 
         if (isp->revision == ISP_REVISION_15_0) {
                 csi2c->isp = isp;
@@ -1311,7 +1308,15 @@ int omap3isp_csi2_init(struct isp_device *isp)
         }
 
         return 0;
-fail:
-        omap3isp_csi2_cleanup(isp);
-        return ret;
+}
+
+/*
+ * omap3isp_csi2_cleanup - Routine for module driver cleanup
+ */
+void omap3isp_csi2_cleanup(struct isp_device *isp)
+{
+        struct isp_csi2_device *csi2a = &isp->isp_csi2a;
+
+        omap3isp_video_cleanup(&csi2a->video_out);
+        media_entity_cleanup(&csi2a->subdev.entity);
 }