1 files changed, 133 insertions, 74 deletions

diff --git a/drivers/media/video/sh_mobile_ceu_camera.c b/drivers/media/video/sh_mobile_ceu_camera.c
index 3ae5c9c58cb..e54089802b6 100644
--- a/drivers/media/video/sh_mobile_ceu_camera.c
+++ b/drivers/media/video/sh_mobile_ceu_camera.c
@@ -27,7 +27,6 @@
 #include <linux/mm.h>
 #include <linux/moduleparam.h>
 #include <linux/time.h>
-#include <linux/version.h>
 #include <linux/slab.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
@@ -39,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>
@@ -96,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;
@@ -205,7 +206,7 @@ static int sh_mobile_ceu_soft_reset(struct sh_mobile_ceu_dev *pcdev)
 
 
         if (2 != success) {
-                dev_warn(&icd->dev, "soft reset time out\n");
+                dev_warn(icd->pdev, "soft reset time out\n");
                 return -EIO;
         }
 
@@ -220,7 +221,7 @@ static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
                         unsigned long sizes[], void *alloc_ctxs[])
 {
         struct soc_camera_device *icd = container_of(vq, struct soc_camera_device, vb2_vidq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
                                                 icd->current_fmt->host_fmt);
@@ -242,7 +243,7 @@ static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
                         *count = pcdev->video_limit / PAGE_ALIGN(sizes[0]);
         }
 
-        dev_dbg(icd->dev.parent, "count=%d, size=%lu\n", *count, sizes[0]);
+        dev_dbg(icd->parent, "count=%d, size=%lu\n", *count, sizes[0]);
 
         return 0;
 }
@@ -351,7 +352,7 @@ static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
 
         buf = to_ceu_vb(vb);
 
-        dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
                 vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 
         /* Added list head initialization on alloc */
@@ -371,7 +372,7 @@ static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
         size = icd->user_height * bytes_per_line;
 
         if (vb2_plane_size(vb, 0) < size) {
-                dev_err(icd->dev.parent, "Buffer too small (%lu < %lu)\n",
+                dev_err(icd->parent, "Buffer too small (%lu < %lu)\n",
                         vb2_plane_size(vb, 0), size);
                 return -ENOBUFS;
         }
@@ -384,11 +385,11 @@ static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
 static void sh_mobile_ceu_videobuf_queue(struct vb2_buffer *vb)
 {
         struct soc_camera_device *icd = container_of(vb->vb2_queue, struct soc_camera_device, vb2_vidq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
 
-        dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
                 vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 
         spin_lock_irq(&pcdev->lock);
@@ -409,7 +410,7 @@ static void sh_mobile_ceu_videobuf_queue(struct vb2_buffer *vb)
 static void sh_mobile_ceu_videobuf_release(struct vb2_buffer *vb)
 {
         struct soc_camera_device *icd = container_of(vb->vb2_queue, struct soc_camera_device, vb2_vidq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
@@ -421,8 +422,12 @@ static void sh_mobile_ceu_videobuf_release(struct vb2_buffer *vb)
                 pcdev->active = NULL;
         }
 
-        /* Doesn't hurt also if the list is empty */
-        list_del_init(&buf->queue);
+        /*
+         * Doesn't hurt also if the list is empty, but it hurts, if queuing the
+         * buffer failed, and .buf_init() hasn't been called
+         */
+        if (buf->queue.next)
+                list_del_init(&buf->queue);
 
         spin_unlock_irq(&pcdev->lock);
 }
@@ -437,7 +442,7 @@ static int sh_mobile_ceu_videobuf_init(struct vb2_buffer *vb)
 static int sh_mobile_ceu_stop_streaming(struct vb2_queue *q)
 {
         struct soc_camera_device *icd = container_of(q, struct soc_camera_device, vb2_vidq);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         struct list_head *buf_head, *tmp;
 
@@ -499,25 +504,48 @@ 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 soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
+        struct v4l2_subdev *csi2_sd;
         int ret;
 
         if (pcdev->icd)
                 return -EBUSY;
 
-        dev_info(icd->dev.parent,
+        dev_info(icd->parent,
                  "SuperH Mobile CEU driver attached to camera %d\n",
                  icd->devnum);
 
         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;
 }
@@ -525,11 +553,13 @@ static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
 /* Called with .video_lock held */
 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 soc_camera_host *ici = to_soc_camera_host(icd->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);
@@ -545,7 +575,7 @@ static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
 
         pm_runtime_put_sync(ici->v4l2_dev.dev);
 
-        dev_info(icd->dev.parent,
+        dev_info(icd->parent,
                  "SuperH Mobile CEU driver detached from camera %d\n",
                  icd->devnum);
 
@@ -585,14 +615,14 @@ static u16 calc_scale(unsigned int src, unsigned int *dst)
 /* rect is guaranteed to not exceed the scaled camera rectangle */
 static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         unsigned int height, width, cdwdr_width, in_width, in_height;
         unsigned int left_offset, top_offset;
         u32 camor;
 
-        dev_geo(icd->dev.parent, "Crop %ux%u@%u:%u\n",
+        dev_geo(icd->parent, "Crop %ux%u@%u:%u\n",
                 icd->user_width, icd->user_height, cam->ceu_left, cam->ceu_top);
 
         left_offset     = cam->ceu_left;
@@ -641,7 +671,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;
         }
@@ -649,7 +679,7 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
         /* Set CAMOR, CAPWR, CFSZR, take care of CDWDR */
         camor = left_offset | (top_offset << 16);
 
-        dev_geo(icd->dev.parent,
+        dev_geo(icd->parent,
                 "CAMOR 0x%x, CAPWR 0x%x, CFSZR 0x%x, CDWDR 0x%x\n", camor,
                 (in_height << 16) | in_width, (height << 16) | width,
                 cdwdr_width);
@@ -697,7 +727,7 @@ static void capture_restore(struct sh_mobile_ceu_dev *pcdev, u32 capsr)
 static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
                                        __u32 pixfmt)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         int ret;
         unsigned long camera_flags, common_flags, value;
@@ -783,7 +813,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);
@@ -806,7 +836,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
         sh_mobile_ceu_set_rect(icd);
         mdelay(1);
 
-        dev_geo(icd->dev.parent, "CFLCR 0x%x\n", pcdev->cflcr);
+        dev_geo(icd->parent, "CFLCR 0x%x\n", pcdev->cflcr);
         ceu_write(pcdev, CFLCR, pcdev->cflcr);
 
         /*
@@ -829,7 +859,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
         ceu_write(pcdev, CDOCR, value);
         ceu_write(pcdev, CFWCR, 0); /* keep "datafetch firewall" disabled */
 
-        dev_dbg(icd->dev.parent, "S_FMT successful for %c%c%c%c %ux%u\n",
+        dev_dbg(icd->parent, "S_FMT successful for %c%c%c%c %ux%u\n",
                 pixfmt & 0xff, (pixfmt >> 8) & 0xff,
                 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
                 icd->user_width, icd->user_height);
@@ -843,7 +873,7 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
 static int sh_mobile_ceu_try_bus_param(struct soc_camera_device *icd,
                                        unsigned char buswidth)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         unsigned long camera_flags, common_flags;
 
@@ -901,7 +931,7 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
                                      struct soc_camera_format_xlate *xlate)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         int ret, k, n;
@@ -921,7 +951,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;
@@ -1244,7 +1274,7 @@ static int client_s_fmt(struct soc_camera_device *icd,
 {
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         unsigned int width = mf->width, height = mf->height, tmp_w, tmp_h;
         unsigned int max_width, max_height;
         struct v4l2_cropcap cap;
@@ -1313,7 +1343,7 @@ static int client_scale(struct soc_camera_device *icd,
                         bool ceu_can_scale)
 {
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         struct v4l2_mbus_framefmt mf_tmp = *mf;
         unsigned int scale_h, scale_v;
         int ret;
@@ -1363,13 +1393,13 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
                                   struct v4l2_crop *a)
 {
         struct v4l2_rect *rect = &a->c;
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct device *dev = icd->parent;
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         struct v4l2_crop cam_crop;
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
         struct v4l2_rect *cam_rect = &cam_crop.c;
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct device *dev = icd->dev.parent;
         struct v4l2_mbus_framefmt mf;
         unsigned int scale_cam_h, scale_cam_v, scale_ceu_h, scale_ceu_v,
                 out_width, out_height;
@@ -1511,7 +1541,7 @@ static void calculate_client_output(struct soc_camera_device *icd,
                 struct v4l2_pix_format *pix, struct v4l2_mbus_framefmt *mf)
 {
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
-        struct device *dev = icd->dev.parent;
+        struct device *dev = icd->parent;
         struct v4l2_rect *cam_subrect = &cam->subrect;
         unsigned int scale_v, scale_h;
 
@@ -1555,12 +1585,12 @@ static void calculate_client_output(struct soc_camera_device *icd,
 static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
                                  struct v4l2_format *f)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct device *dev = icd->parent;
+        struct soc_camera_host *ici = to_soc_camera_host(dev);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         struct sh_mobile_ceu_cam *cam = icd->host_priv;
         struct v4l2_pix_format *pix = &f->fmt.pix;
         struct v4l2_mbus_framefmt mf;
-        struct device *dev = icd->dev.parent;
         __u32 pixfmt = pix->pixelformat;
         const struct soc_camera_format_xlate *xlate;
         /* Keep Compiler Happy */
@@ -1684,12 +1714,12 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
         int width, height;
         int ret;
 
-        dev_geo(icd->dev.parent, "TRY_FMT(pix=0x%x, %ux%u)\n",
+        dev_geo(icd->parent, "TRY_FMT(pix=0x%x, %ux%u)\n",
                  pixfmt, pix->width, pix->height);
 
         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
         if (!xlate) {
-                dev_warn(icd->dev.parent, "Format %x not found\n", pixfmt);
+                dev_warn(icd->parent, "Format %x not found\n", pixfmt);
                 return -EINVAL;
         }
 
@@ -1701,11 +1731,6 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
         width = pix->width;
         height = pix->height;
 
-        pix->bytesperline = soc_mbus_bytes_per_line(width, xlate->host_fmt);
-        if ((int)pix->bytesperline < 0)
-                return pix->bytesperline;
-        pix->sizeimage = height * pix->bytesperline;
-
         /* limit to sensor capabilities */
         mf.width        = pix->width;
         mf.height       = pix->height;
@@ -1741,7 +1766,7 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
                                                          try_mbus_fmt, &mf);
                         if (ret < 0) {
                                 /* Shouldn't actually happen... */
-                                dev_err(icd->dev.parent,
+                                dev_err(icd->parent,
                                         "FIXME: client try_fmt() = %d\n", ret);
                                 return ret;
                         }
@@ -1753,7 +1778,7 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
                         pix->height = height;
         }
 
-        dev_geo(icd->dev.parent, "%s(): return %d, fmt 0x%x, %ux%u\n",
+        dev_geo(icd->parent, "%s(): return %d, fmt 0x%x, %ux%u\n",
                 __func__, ret, pix->pixelformat, pix->width, pix->height);
 
         return ret;
@@ -1763,7 +1788,7 @@ static int sh_mobile_ceu_set_livecrop(struct soc_camera_device *icd,
                                       struct v4l2_crop *a)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         u32 out_width = icd->user_width, out_height = icd->user_height;
         int ret;
@@ -1775,13 +1800,13 @@ static int sh_mobile_ceu_set_livecrop(struct soc_camera_device *icd,
         /* Stop the client */
         ret = v4l2_subdev_call(sd, video, s_stream, 0);
         if (ret < 0)
-                dev_warn(icd->dev.parent,
+                dev_warn(icd->parent,
                          "Client failed to stop the stream: %d\n", ret);
         else
                 /* Do the crop, if it fails, there's nothing more we can do */
                 sh_mobile_ceu_set_crop(icd, a);
 
-        dev_geo(icd->dev.parent, "Output after crop: %ux%u\n", icd->user_width, icd->user_height);
+        dev_geo(icd->parent, "Output after crop: %ux%u\n", icd->user_width, icd->user_height);
 
         if (icd->user_width != out_width || icd->user_height != out_height) {
                 struct v4l2_format f = {
@@ -1827,7 +1852,6 @@ static int sh_mobile_ceu_querycap(struct soc_camera_host *ici,
                                   struct v4l2_capability *cap)
 {
         strlcpy(cap->card, "SuperH_Mobile_CEU", sizeof(cap->card));
-        cap->version = KERNEL_VERSION(0, 0, 5);
         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
         return 0;
 }
@@ -1848,7 +1872,7 @@ static int sh_mobile_ceu_init_videobuf(struct vb2_queue *q,
 static int sh_mobile_ceu_get_ctrl(struct soc_camera_device *icd,
                                   struct v4l2_control *ctrl)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
         u32 val;
 
@@ -1864,7 +1888,7 @@ static int sh_mobile_ceu_get_ctrl(struct soc_camera_device *icd,
 static int sh_mobile_ceu_set_ctrl(struct soc_camera_device *icd,
                                   struct v4l2_control *ctrl)
 {
-        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
         switch (ctrl->id) {
@@ -1950,7 +1974,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);
@@ -2023,26 +2047,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;
                 }
 
                 /*
@@ -2051,34 +2110,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);
@@ -2098,7 +2151,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);
@@ -2107,8 +2160,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;
@@ -2158,4 +2216,5 @@ module_exit(sh_mobile_ceu_exit);
 MODULE_DESCRIPTION("SuperH Mobile CEU driver");
 MODULE_AUTHOR("Magnus Damm");
 MODULE_LICENSE("GPL");
+MODULE_VERSION("0.0.6");
 MODULE_ALIAS("platform:sh_mobile_ceu");